|
|
|
@ -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(); |
|
|
|
|