From 9c6b1a0f04c5f00f87e3b370b5a7c27dde2a8bd3 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 6 Mar 2018 16:02:23 +0100 Subject: [PATCH] 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 | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 9651f48c03..77f8d2482b 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/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;