diff --git a/src/modules/flight_mode_manager/FlightModeManager.cpp b/src/modules/flight_mode_manager/FlightModeManager.cpp index 62909dba76..1e3b5eb995 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.cpp +++ b/src/modules/flight_mode_manager/FlightModeManager.cpp @@ -415,11 +415,11 @@ void FlightModeManager::handleCommand() // ignore all unkown commands if (desired_task != FlightTaskIndex::None) { // switch to the commanded task - FlightTaskError switch_result = switchTask(desired_task); + bool switch_succeeded = (switchTask(desired_task) == FlightTaskError::NoError); uint8_t cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED; // if we are in/switched to the desired task - if (switch_result >= FlightTaskError::NoError) { + if (switch_succeeded) { cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED; // if the task is running apply parameters to it and see if it rejects @@ -427,7 +427,7 @@ void FlightModeManager::handleCommand() cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_DENIED; // if we just switched and parameters are not accepted, go to failsafe - if (switch_result >= FlightTaskError::NoError) { + if (switch_succeeded) { switchTask(FlightTaskIndex::Failsafe); } } @@ -437,7 +437,6 @@ void FlightModeManager::handleCommand() vehicle_command_ack_s command_ack{}; command_ack.command = command.command; command_ack.result = cmd_result; - command_ack.result_param1 = static_cast(switch_result); command_ack.target_system = command.source_system; command_ack.target_component = command.source_component; command_ack.timestamp = hrt_absolute_time();