navigator: fix incorrect takeoff altitude

This fixes a problem where we do not properly go to the set takeoff
altitude but end up lower.

The problem was that the setpoint triplet is reset when the navigation
mode changes. So in this case, the triplet is reset when we switch from
takeoff to loiter which can happen before reaching the actual takeoff
altitude.

The fix is an ugly hack to prevent the reset in the case of takeoff to
loiter. A better solution would be to remove the general reset and have
all navigation modes do the proper resets themselves.

This hotfix should however be lower risk.

src/modules/navigator/navigator_main.cpp

@@ -722,7 +722,16 @@ Navigator::task_main()
 
 		/* we have a new navigation mode: reset triplet */
 		if (_navigation_mode != navigation_mode_new) {
-			reset_triplets();
+			// We don't reset the triplet if we just did an auto-takeoff and are now
+			// going to loiter. Otherwise, we lose the takeoff altitude and end up lower
+			// than where we wanted to go.
+			//
+			// FIXME: a better solution would be to add reset where they are needed and remove
+			//        this general reset here.
+			if (!(_navigation_mode == &_takeoff &&
+			      navigation_mode_new == &_loiter)) {
+				reset_triplets();
+			}
 		}
 
 		_navigation_mode = navigation_mode_new;