Browse Source

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.
sbg
Julian Oes 5 years ago committed by Daniel Agar
parent
commit
3233e0794d
  1. 12
      src/modules/navigator/navigator_main.cpp

12
src/modules/navigator/navigator_main.cpp

@ -662,18 +662,6 @@ Navigator::run()
_navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]); _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 nothing is running, set position setpoint triplet invalid once */
if (_navigation_mode == nullptr && !_pos_sp_triplet_published_invalid_once) { if (_navigation_mode == nullptr && !_pos_sp_triplet_published_invalid_once) {
_pos_sp_triplet_published_invalid_once = true; _pos_sp_triplet_published_invalid_once = true;

Loading…
Cancel
Save