From 81c5025ad87ede7e8cdf5772de098827fdd63db3 Mon Sep 17 00:00:00 2001 From: Dennis Mannhart Date: Wed, 22 Aug 2018 11:50:20 +0200 Subject: [PATCH] mc_pos_control: enter FlighttaskFailsafe if Flighttask wants to be started, but all of the Flighttask fail. A failure of the Flighttask can occur if the demanded Flighttask does not have the required data. --- .../mc_pos_control/mc_pos_control_main.cpp | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 678c3f7ae0..12896ef054 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -742,7 +742,6 @@ MulticopterPositionControl::task_main() publish_avoidance_desired_waypoint(); } else { - // no flighttask is active: set attitude setpoint to idle _att_sp.roll_body = _att_sp.pitch_body = 0.0f; _att_sp.yaw_body = _local_pos.yaw; @@ -864,7 +863,6 @@ MulticopterPositionControl::start_flight_task() } } - // manual stabilized control if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_MANUAL || _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_STAB || task_failure) { @@ -883,9 +881,17 @@ MulticopterPositionControl::start_flight_task() // check task failure if (task_failure) { - // No task was activated. - _flight_tasks.switchTask(FlightTaskIndex::None); - warn_rate_limited("No Flighttask is running"); + + // for some reason no flighttask was able to start. + // go into failsafe flighttask + int error = _flight_tasks.switchTask(FlightTaskIndex::Failsafe); + task_failure = false; + + if (error != 0) { + // No task was activated. + _flight_tasks.switchTask(FlightTaskIndex::None); + + } } }