Fix waypoint altitude. Adjusts for home position altitude.
[ ] @PeruDayani is there a way to query for the official home altitude? The waypoints uploaded uses altitude above ground level rather than altitude above elliposid / sea level which is what the GPS uses.
Flight tests successful.
Fixed a few bugs: