From 116a094e3170b7c43de698c5a93755e13ff8770a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Thu, 20 Feb 2020 10:45:12 +0100 Subject: [PATCH] commander: fix broken 'commander arm/disarm' CLI Regression from https://github.com/PX4/Firmware/pull/13613. VEHICLE_CMD_COMPONENT_ARM_DISARM from CLI would enter the ARMING_STATE_IN_AIR_RESTORE logic. This was never intended for disarming (and leads to state machine transition failures), and IMO it is also not intended for commands from CLI. --- src/drivers/px4io/px4io.cpp | 1 + src/modules/commander/Commander.cpp | 7 +++++-- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index a7e58d8b9d52..97c8fd578866 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -785,6 +785,7 @@ PX4IO::init() /* send command to arm system via command API */ vcmd.timestamp = hrt_absolute_time(); vcmd.param1 = 1.0f; /* request arming */ + vcmd.param3 = 1234.f; /* mark the command coming from IO (for in-air restoring) */ vcmd.command = vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM; /* send command once */ diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 75e6162dd6c1..3116cbb06372 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -691,8 +691,11 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_ break; } - // Flick to inair restore first if this comes from an onboard system - if (cmd.source_system == status_local->system_id && cmd.source_component == status_local->component_id) { + const bool cmd_from_io = (static_cast(roundf(cmd.param3)) == 1234); + + // Flick to inair restore first if this comes from an onboard system and from IO + if (cmd.source_system == status_local->system_id && cmd.source_component == status_local->component_id + && cmd_from_io && cmd_arms) { status.arming_state = vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE; } else {