From bec0d83cf44cd02593b37688b064a31a98df0bd2 Mon Sep 17 00:00:00 2001 From: Thomas Stastny Date: Wed, 13 Sep 2023 19:19:44 +0200 Subject: [PATCH] Commander: dont accept reposition commands without the mode switch bit avoids erroneous (unexpected) position setpoints when switching into hold from another mode --- src/modules/commander/Commander.cpp | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index d4b8d539d4..7cb6cd6577 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -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;