Add altitude calculation
Make sure GPS data doesn't get intermingled with baro data.