From af607e30404e891030c09fdd1845ad6973c05887 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 18 Oct 2021 18:55:28 +0200 Subject: [PATCH] Use separate arm_request instead of vehicle_command for RC arming --- msg/CMakeLists.txt | 1 + msg/arm_request.msg | 11 ++ msg/vehicle_command.msg | 8 +- src/modules/commander/Commander.cpp | 114 ++++++++----------- src/modules/commander/Commander.hpp | 2 + src/modules/manual_control/ManualControl.cpp | 30 ++--- src/modules/manual_control/ManualControl.hpp | 4 +- 7 files changed, 78 insertions(+), 92 deletions(-) create mode 100644 msg/arm_request.msg diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index a343062b64..0ba9691742 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -47,6 +47,7 @@ set(msg_files airspeed.msg airspeed_validated.msg airspeed_wind.msg + arm_request.msg autotune_attitude_control_status.msg battery_status.msg camera_capture.msg diff --git a/msg/arm_request.msg b/msg/arm_request.msg new file mode 100644 index 0000000000..426f3ab18c --- /dev/null +++ b/msg/arm_request.msg @@ -0,0 +1,11 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 action # what action is requested +uint8 ACTION_DISARM = 0 +uint8 ACTION_ARM = 1 +uint8 ACTION_TOGGLE = 2 + +uint8 source # how the request was triggered +uint8 SOURCE_RC_STICK_GESTURE = 0 +uint8 SOURCE_RC_SWITCH = 1 +uint8 SOURCE_RC_BUTTON = 2 diff --git a/msg/vehicle_command.msg b/msg/vehicle_command.msg index d128060a47..e32e00922e 100644 --- a/msg/vehicle_command.msg +++ b/msg/vehicle_command.msg @@ -69,7 +69,7 @@ uint16 VEHICLE_CMD_OBLIQUE_SURVEY=260 # Mission command to set a Camera Auto M uint16 VEHICLE_CMD_GIMBAL_DEVICE_INFORMATION = 283 # Command to ask information about a low level gimbal uint16 VEHICLE_CMD_MISSION_START = 300 # start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| -uint16 VEHICLE_CMD_COMPONENT_ARM_DISARM = 400 # Arms / Disarms a component |1 to arm, 0 to disarm|-1 to toggle +uint16 VEHICLE_CMD_COMPONENT_ARM_DISARM = 400 # Arms / Disarms a component |1 to arm, 0 to disarm uint16 VEHICLE_CMD_INJECT_FAILURE = 420 # Inject artificial failure for testing purposes uint16 VEHICLE_CMD_START_RX_PAIR = 500 # Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| uint16 VEHICLE_CMD_REQUEST_MESSAGE = 512 # Request to send a single instance of the specified message @@ -151,15 +151,9 @@ uint8 FAILURE_TYPE_DELAYED = 6 uint8 FAILURE_TYPE_INTERMITTENT = 7 # used as param1 in ARM_DISARM command -int8 ARMING_ACTION_TOGGLE = -1 int8 ARMING_ACTION_DISARM = 0 int8 ARMING_ACTION_ARM = 1 -# used as param3 in ARM_DISARM command -int8 ARMING_ORIGIN_GESTURE = 1 -int8 ARMING_ORIGIN_SWITCH = 2 -int8 ARMING_ORIGIN_BUTTON = 3 - uint8 ORB_QUEUE_LENGTH = 8 float32 param1 # Parameter 1, as defined by MAVLink uint16 VEHICLE_CMD enum. diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 7a41fb8b8a..b40bd957d3 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -562,7 +562,7 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_ run_preflight_checks = false; } - if (run_preflight_checks) { + if (run_preflight_checks && !_armed.armed) { if (_vehicle_control_mode.flag_control_manual_enabled) { if (_vehicle_control_mode.flag_control_climb_rate_enabled && !_status.rc_signal_lost && _is_throttle_above_center) { @@ -584,11 +584,21 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_ tune_negative(true); return TRANSITION_DENIED; } + + } else if (calling_reason == arm_disarm_reason_t::rc_stick + || calling_reason == arm_disarm_reason_t::rc_switch + || calling_reason == arm_disarm_reason_t::rc_button) { + mavlink_log_critical(&_mavlink_log_pub, "Arming denied: switch to manual mode first\t"); + events::send(events::ID("commander_arm_denied_not_manual"), + {events::Log::Critical, events::LogInternal::Info}, + "Arming denied: switch to manual mode first"); + tune_negative(true); + return TRANSITION_DENIED; } if ((_param_geofence_action.get() == geofence_result_s::GF_ACTION_RTL) && !_status_flags.condition_home_position_valid) { - mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Geofence RTL requires valid home\t"); + mavlink_log_critical(&_mavlink_log_pub, "Arming denied: Geofence RTL requires valid home\t"); events::send(events::ID("commander_arm_denied_geofence_rtl"), {events::Log::Critical, events::LogInternal::Info}, "Arming denied: Geofence RTL requires valid home"); @@ -852,8 +862,7 @@ Commander::handle_command(const vehicle_command_s &cmd) const int8_t arming_action = static_cast(lroundf(cmd.param1)); if (arming_action != vehicle_command_s::ARMING_ACTION_ARM - && arming_action != vehicle_command_s::ARMING_ACTION_DISARM - && arming_action != vehicle_command_s::ARMING_ACTION_TOGGLE) { + && arming_action != vehicle_command_s::ARMING_ACTION_DISARM) { mavlink_log_critical(&_mavlink_log_pub, "Unsupported ARM_DISARM param: %.3f\t", (double)cmd.param1); events::send(events::ID("commander_unsupported_arm_disarm_param"), events::Log::Error, "Unsupported ARM_DISARM param: {1:.3}", cmd.param1); @@ -861,26 +870,8 @@ Commander::handle_command(const vehicle_command_s &cmd) } else { // Arm is forced (checks skipped) when param2 is set to a magic number. const bool forced = (static_cast(lroundf(cmd.param2)) == 21196); - - const int8_t arming_origin = static_cast(lroundf(cmd.param3)); - - const bool cmd_from_manual_stick = (arming_origin == vehicle_command_s::ARMING_ORIGIN_GESTURE); - const bool cmd_from_manual_switch = (arming_origin == vehicle_command_s::ARMING_ORIGIN_SWITCH); - const bool cmd_from_manual_button = (arming_origin == vehicle_command_s::ARMING_ORIGIN_BUTTON); const bool cmd_from_io = (static_cast(roundf(cmd.param3)) == 1234); - if (cmd_from_manual_stick && !_vehicle_control_mode.flag_control_manual_enabled) { - if (arming_action == vehicle_command_s::ARMING_ACTION_ARM) { - mavlink_log_critical(&_mavlink_log_pub, "Not arming! Switch to a manual mode first"); - - } else if (arming_action == vehicle_command_s::ARMING_ACTION_DISARM) { - mavlink_log_critical(&_mavlink_log_pub, "Not disarming! Switch to a manual mode first"); - } - - cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED; - break; - } - if (!forced) { if (!(_land_detector.landed || _land_detector.maybe_landed) && !is_ground_rover(_status)) { if (arming_action == vehicle_command_s::ARMING_ACTION_ARM) { @@ -916,53 +907,14 @@ Commander::handle_command(const vehicle_command_s &cmd) } transition_result_t arming_res = TRANSITION_DENIED; + arm_disarm_reason_t arm_disarm_reason = cmd.from_external ? arm_disarm_reason_t::command_external : + arm_disarm_reason_t::command_internal; if (arming_action == vehicle_command_s::ARMING_ACTION_ARM) { - if (cmd.from_external) { - arming_res = arm(arm_disarm_reason_t::command_external); - - } else { - if (cmd_from_manual_stick) { - arming_res = arm(arm_disarm_reason_t::rc_stick); - - } else if (cmd_from_manual_switch) { - arming_res = arm(arm_disarm_reason_t::rc_switch); - - } else { - arming_res = arm(arm_disarm_reason_t::command_internal, !forced); - } - } + arming_res = arm(arm_disarm_reason, cmd.from_external || !forced); } else if (arming_action == vehicle_command_s::ARMING_ACTION_DISARM) { - if (cmd.from_external) { - arming_res = disarm(arm_disarm_reason_t::command_external); - - } else { - if (cmd_from_manual_stick) { - arming_res = disarm(arm_disarm_reason_t::rc_stick); - - } else if (cmd_from_manual_switch) { - arming_res = disarm(arm_disarm_reason_t::rc_switch); - - } else { - arming_res = disarm(arm_disarm_reason_t::command_internal); - } - } - - } else if (arming_action == vehicle_command_s::ARMING_ACTION_TOGGLE) { - // -1 means toggle by a button - // This should come from an arming button internally, otherwise something is wrong. - if (!cmd.from_external && cmd_from_manual_button) { - if (_armed.armed) { - arming_res = disarm(arm_disarm_reason_t::rc_button); - - } else { - arming_res = arm(arm_disarm_reason_t::rc_button); - } - - } else { - PX4_WARN("ARM_DISARM toggle command ignored"); - } + arming_res = disarm(arm_disarm_reason); } @@ -2497,6 +2449,38 @@ Commander::run() } } + while (_arm_request_sub.updated()) { + arm_request_s arm_request; + + if (_arm_request_sub.copy(&arm_request)) { + arm_disarm_reason_t arm_disarm_reason{}; + + switch (arm_request.source) { + case arm_request_s::SOURCE_RC_STICK_GESTURE: arm_disarm_reason = arm_disarm_reason_t::rc_stick; break; + + case arm_request_s::SOURCE_RC_SWITCH: arm_disarm_reason = arm_disarm_reason_t::rc_switch; break; + + case arm_request_s::SOURCE_RC_BUTTON: arm_disarm_reason = arm_disarm_reason_t::rc_button; break; + } + + switch (arm_request.action) { + case arm_request_s::ACTION_DISARM: disarm(arm_disarm_reason); break; + + case arm_request_s::ACTION_ARM: arm(arm_disarm_reason); break; + + case arm_request_s::ACTION_TOGGLE: + if (_armed.armed) { + disarm(arm_disarm_reason); + + } else { + arm(arm_disarm_reason); + } + + break; + } + } + } + /* Check for failure detector status */ if (_failure_detector.update(_status, _vehicle_control_mode)) { _status.failure_detector_status = _failure_detector.getStatus().value; diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index fd25f6a86a..3805d35dca 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -61,6 +61,7 @@ #include #include #include +#include #include #include #include @@ -389,6 +390,7 @@ private: // Subscriptions uORB::Subscription _actuator_controls_sub{ORB_ID_VEHICLE_ATTITUDE_CONTROLS}; + uORB::Subscription _arm_request_sub {ORB_ID(arm_request)}; uORB::Subscription _cmd_sub {ORB_ID(vehicle_command)}; uORB::Subscription _cpuload_sub{ORB_ID(cpuload)}; uORB::Subscription _esc_status_sub{ORB_ID(esc_status)}; diff --git a/src/modules/manual_control/ManualControl.cpp b/src/modules/manual_control/ManualControl.cpp index 618f45fdea..d451446b91 100644 --- a/src/modules/manual_control/ManualControl.cpp +++ b/src/modules/manual_control/ManualControl.cpp @@ -119,7 +119,7 @@ void ManualControl::Run() _stick_arm_hysteresis.set_state_and_update(stick_lower_right && right_stick_centered, _selector.setpoint().timestamp); if (!previous_stick_arm_hysteresis && _stick_arm_hysteresis.get_state()) { - send_arm_command(vehicle_command_s::ARMING_ACTION_ARM, vehicle_command_s::ARMING_ORIGIN_GESTURE); + sendArmRequest(arm_request_s::ACTION_ARM, arm_request_s::SOURCE_RC_STICK_GESTURE); } // Disarm gesture @@ -130,7 +130,7 @@ void ManualControl::Run() _stick_disarm_hysteresis.set_state_and_update(stick_lower_left && right_stick_centered, _selector.setpoint().timestamp); if (!previous_stick_disarm_hysteresis && _stick_disarm_hysteresis.get_state()) { - send_arm_command(vehicle_command_s::ARMING_ACTION_DISARM, vehicle_command_s::ARMING_ORIGIN_GESTURE); + sendArmRequest(arm_request_s::ACTION_DISARM, arm_request_s::SOURCE_RC_STICK_GESTURE); } // User override by stick @@ -165,20 +165,17 @@ void ManualControl::Run() _button_hysteresis.set_state_and_update(switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON, now); if (!previous_button_hysteresis && _button_hysteresis.get_state()) { - send_arm_command(vehicle_command_s::ARMING_ACTION_TOGGLE, - vehicle_command_s::ARMING_ORIGIN_BUTTON); + sendArmRequest(arm_request_s::ACTION_TOGGLE, arm_request_s::SOURCE_RC_BUTTON); } } else { // Arming switch if (switches.arm_switch != _previous_switches.arm_switch) { if (switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON) { - send_arm_command(vehicle_command_s::ARMING_ACTION_ARM, - vehicle_command_s::ARMING_ORIGIN_SWITCH); + sendArmRequest(arm_request_s::ACTION_ARM, arm_request_s::SOURCE_RC_SWITCH); } else if (switches.arm_switch == manual_control_switches_s::SWITCH_POS_OFF) { - send_arm_command(vehicle_command_s::ARMING_ACTION_DISARM, - vehicle_command_s::ARMING_ORIGIN_SWITCH); + sendArmRequest(arm_request_s::ACTION_DISARM, arm_request_s::SOURCE_RC_SWITCH); } } } @@ -419,18 +416,13 @@ void ManualControl::send_mode_command(int32_t commander_main_state) command_pub.publish(command); } -void ManualControl::send_arm_command(int8_t action, int8_t origin) +void ManualControl::sendArmRequest(int8_t action, int8_t source) { - vehicle_command_s command{}; - command.command = vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM; - command.param1 = static_cast(action); - command.param3 = static_cast(origin); // We use param3 to signal the origin. - command.target_system = _param_mav_sys_id.get(); - command.target_component = _param_mav_comp_id.get(); - - uORB::Publication command_pub{ORB_ID(vehicle_command)}; - command.timestamp = hrt_absolute_time(); - command_pub.publish(command); + arm_request_s arm_request{}; + arm_request.action = action; + arm_request.source = source; + arm_request.timestamp = hrt_absolute_time(); + _arm_request_pub.publish(arm_request); } void ManualControl::send_rtl_command() diff --git a/src/modules/manual_control/ManualControl.hpp b/src/modules/manual_control/ManualControl.hpp index 796436f8a3..39d6f493b4 100644 --- a/src/modules/manual_control/ManualControl.hpp +++ b/src/modules/manual_control/ManualControl.hpp @@ -40,6 +40,7 @@ #include #include #include +#include #include #include #include @@ -120,7 +121,7 @@ private: void evaluate_mode_slot(uint8_t mode_slot); void send_mode_command(int32_t commander_main_state); - void send_arm_command(int8_t action, int8_t origin); + void sendArmRequest(int8_t action, int8_t source); void send_rtl_command(); void send_loiter_command(); void send_offboard_command(); @@ -128,6 +129,7 @@ private: void publish_landing_gear(int8_t action); void send_vtol_transition_command(uint8_t action); + uORB::Publication _arm_request_pub{ORB_ID(arm_request)}; uORB::Publication _manual_control_setpoint_pub{ORB_ID(manual_control_setpoint)}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};