dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

FlightModeManager: refactor flight task switch result condition for vehicle commands

This commit is contained in:
Matthias Grob 2022-03-28 14:03:25 +02:00
parent ffaef906c4
commit 9bdde98a68
1 changed files with 3 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
src/modules/flight_mode_manager/FlightModeManager.cpp
View File

@ -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<int>(switch_result);
command_ack.target_system = command.source_system;
command_ack.target_component = command.source_component;
command_ack.timestamp = hrt_absolute_time();