Basic implementation update

Calculates height and vertical speed. Next I would like to get the
basics of gps working.