Browse Source

FlightModeManager: refactor flight task switch result condition for vehicle commands

v1.13.0-BW
Matthias Grob 3 years ago
parent
commit
9bdde98a68
  1. 7
      src/modules/flight_mode_manager/FlightModeManager.cpp

7
src/modules/flight_mode_manager/FlightModeManager.cpp

@ -415,11 +415,11 @@ void FlightModeManager::handleCommand() @@ -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() @@ -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() @@ -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<int>(switch_result);
command_ack.target_system = command.source_system;
command_ack.target_component = command.source_component;
command_ack.timestamp = hrt_absolute_time();

Loading…
Cancel
Save