From 3233e0794d0856ace2016133983bfc726409276a Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 10 Feb 2020 14:51:15 +0100 Subject: [PATCH] navigator: fix edge case with valid idle setpoint This is an attempt to fix an edge case in the triplet publication which can lead to crashes on autopilots with slow SD cards. The sequence of events before this patch is: 1. Switch to POSCTL when disarmed. At this point current valid with setpoint idle is published. 2. Arm, takeoff, and fly using joystick/RC. 3. Switch to RTL (or trigger RTL using RC loss). At this point the setpoint is valid but still idle and the motors will shut off. 4. Once navigator has published the new setpoint (which can take up to 1.5 seconds on slow SD cards) we will hopefully recover. With this patch we omit this edge case, so we never publish this idle setpoint when landed. The assumption is that this idle setpoint is no longer required with the current flight task code, however, that needs to be further verified. --- src/modules/navigator/navigator_main.cpp | 12 ------------ 1 file changed, 12 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 0427bfa41a..0ea7333174 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -662,18 +662,6 @@ Navigator::run() _navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]); } - /* if we landed and have not received takeoff setpoint then stay in idle */ - if (_land_detected.landed && - !((_vstatus.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF) - || (_vstatus.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION))) { - - reset_triplets(); - - _pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_IDLE; - _pos_sp_triplet.current.valid = true; - _pos_sp_triplet.current.timestamp = hrt_absolute_time(); - } - /* if nothing is running, set position setpoint triplet invalid once */ if (_navigation_mode == nullptr && !_pos_sp_triplet_published_invalid_once) { _pos_sp_triplet_published_invalid_once = true;