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

Commander: dont accept reposition commands without the mode switch bit 

avoids erroneous (unexpected) position setpoints when switching into hold from another mode
This commit is contained in:
Thomas Stastny 2023-09-13 19:19:44 +02:00 committed by Daniel Agar
parent 2d80291b43
commit bec0d83cf4
1 changed files with 8 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
src/modules/commander/Commander.cpp
View File

@ -719,8 +719,14 @@ Commander::handle_command(const vehicle_command_s &cmd)
// to not require navigator and command to receive / process
// the data at the exact same time.
// Check if a mode switch had been requested
if ((((uint32_t)cmd.param2) & 1) > 0) {
const uint32_t change_mode_flags = uint32_t(cmd.param2);
const bool mode_switch_not_requested = (change_mode_flags & 1) == 0;
const bool unsupported_bits_set = (change_mode_flags & ~1) != 0;
if (mode_switch_not_requested || unsupported_bits_set) {
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
} else {
if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER)) {
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
@ -728,9 +734,6 @@ Commander::handle_command(const vehicle_command_s &cmd)
printRejectMode(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER);
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
}
} else {
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
}
}
break;