From 1c15cc11d8b05b8cc78fd393ebbc0a7d61d9fb5d Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 15 Apr 2021 14:08:26 +0200 Subject: [PATCH] manual_control: implement (dis)arming via command --- src/modules/commander/Commander.cpp | 43 ++++++------- src/modules/manual_control/ManualControl.cpp | 65 ++++++++++++++++++-- src/modules/manual_control/ManualControl.hpp | 8 +++ 3 files changed, 88 insertions(+), 28 deletions(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index e1a275cfc4..af2b0c6729 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -921,6 +921,15 @@ Commander::handle_command(const vehicle_command_s &cmd) // Arm is forced (checks skipped) when param2 is set to a magic number. const bool forced = (static_cast(roundf(cmd.param2)) == 21196); + const bool cmd_from_manual_stick = (static_cast(roundf(cmd.param3)) == 1); + const bool cmd_from_io = (static_cast(roundf(cmd.param3)) == 1234); + + if (cmd_from_manual_stick && !_vehicle_control_mode.flag_control_manual_enabled) { + mavlink_log_critical(&_mavlink_log_pub, "Not arming! 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 (cmd_arms) { @@ -948,8 +957,6 @@ Commander::handle_command(const vehicle_command_s &cmd) break; } - const bool cmd_from_io = (static_cast(roundf(cmd.param3)) == 1234); - // Flick to in-air restore first if this comes from an onboard system and from IO if (cmd.source_system == _status.system_id && cmd.source_component == _status.component_id && cmd_from_io && cmd_arms) { @@ -964,7 +971,11 @@ Commander::handle_command(const vehicle_command_s &cmd) arming_res = arm(arm_disarm_reason_t::command_external); } else { - arming_res = arm(arm_disarm_reason_t::command_internal, !forced); + if (cmd_from_manual_stick) { + arming_res = arm(arm_disarm_reason_t::rc_stick, !forced); + } else { + arming_res = arm(arm_disarm_reason_t::command_internal, !forced); + } } } else { @@ -972,7 +983,11 @@ Commander::handle_command(const vehicle_command_s &cmd) arming_res = disarm(arm_disarm_reason_t::command_external); } else { - arming_res = disarm(arm_disarm_reason_t::command_internal); + if (cmd_from_manual_stick) { + arming_res = disarm(arm_disarm_reason_t::rc_stick); + } else { + arming_res = disarm(arm_disarm_reason_t::command_internal); + } } } @@ -2383,26 +2398,6 @@ Commander::run() _status.rc_signal_lost = false; - const bool rc_arming_enabled = (_status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF); - - if (rc_arming_enabled) { - if (_manual_control.wantsDisarm(_vehicle_control_mode, _status, _manual_control_switches, _land_detector.landed)) { - disarm(arm_disarm_reason_t::rc_stick); - } - - if (_manual_control.wantsArm(_vehicle_control_mode, _status, _manual_control_switches, _land_detector.landed)) { - if (_vehicle_control_mode.flag_control_manual_enabled) { - arm(arm_disarm_reason_t::rc_stick); - - } else { - mavlink_log_critical(&_mavlink_log_pub, "Not arming! Switch to a manual mode first\t"); - events::send(events::ID("commander_arming_denied_not_manual"), {events::Log::Error, events::LogInternal::Info}, - "Not arming! Switch to a manual mode first"); - tune_negative(true); - } - } - } - // abort autonomous mode and switch to position mode if sticks are moved significantly if ((_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) && !in_low_battery_failsafe && !_geofence_warning_action_on diff --git a/src/modules/manual_control/ManualControl.cpp b/src/modules/manual_control/ManualControl.cpp index 7b82ae5f2e..67730a6a95 100644 --- a/src/modules/manual_control/ManualControl.cpp +++ b/src/modules/manual_control/ManualControl.cpp @@ -34,6 +34,7 @@ #include "ManualControl.hpp" #include +#include namespace manual_control { @@ -91,12 +92,17 @@ void ManualControl::Run() if (_manual_control_input_subs[i].update(&manual_control_input)) { found_at_least_one = true; - _selector.update_manual_control_input(now, manual_control_input, i); - } } + bool switches_updated = false; + manual_control_switches_s manual_control_switches; + + if (_manual_control_switches_sub.update(&manual_control_switches)) { + switches_updated = true; + } + if (!found_at_least_one) { _selector.update_time_only(now); } @@ -114,6 +120,21 @@ void ManualControl::Run() _selector.setpoint().arm_gesture = _stick_arm_hysteresis.get_state(); _selector.setpoint().disarm_gesture = _stick_disarm_hysteresis.get_state(); + if (_selector.setpoint().arm_gesture && !_previous_arm_gesture) { + _previous_arm_gesture = true; + send_arm_command(); + + } else if (!_selector.setpoint().arm_gesture) { + _previous_arm_gesture = false; + } + + if (_selector.setpoint().disarm_gesture && !_previous_disarm_gesture) { + _previous_disarm_gesture = true; + send_disarm_command(); + + } else if (!_selector.setpoint().disarm_gesture) { + _previous_disarm_gesture = false; + } // user wants override //const float minimum_stick_change = 0.01f * _param_com_rc_stick_ov.get(); @@ -129,18 +150,26 @@ void ManualControl::Run() //_selector.setpoint().user_override = rpy_moved || (use_throttle && throttle_moved); + // TODO: at least 3 samples in a time + + if (switches_updated) { + // Only use switches if current source is RC as well. + if (_selector.setpoint().data_source == manual_control_input_s::SOURCE_RC) { + // TODO: handle buttons + } + } _selector.setpoint().timestamp = now; _manual_control_setpoint_pub.publish(_selector.setpoint()); - _manual_control_input_subs[_selector.instance()].registerCallback(); - if (_last_selected_input != _selector.instance()) { PX4_INFO("selected manual_control_input changed %d -> %d", _last_selected_input, _selector.instance()); _last_selected_input = _selector.instance(); } + _manual_control_input_subs[_selector.instance()].registerCallback(); + _manual_control_switches_sub.registerCallback(); } else { _last_selected_input = -1; @@ -156,6 +185,34 @@ void ManualControl::Run() perf_end(_loop_perf); } +void ManualControl::send_arm_command() +{ + vehicle_command_s command{}; + command.command = vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM; + command.param1 = 1.0; + command.param3 = 1.0; // We use param3 to signal that the origin is manual control stick. + command.target_system = 1; + command.target_component = 1; + + uORB::Publication command_pub{ORB_ID(vehicle_command)}; + command.timestamp = hrt_absolute_time(); + command_pub.publish(command); +} + +void ManualControl::send_disarm_command() +{ + vehicle_command_s command{}; + command.command = vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM; + command.param1 = 0.0; + command.param3 = 1.0; // We use param3 to signal that the origin is manual control stick. + command.target_system = 1; + command.target_component = 1; + + uORB::Publication command_pub{ORB_ID(vehicle_command)}; + command.timestamp = hrt_absolute_time(); + command_pub.publish(command); +} + int ManualControl::task_spawn(int argc, char *argv[]) { ManualControl *instance = new ManualControl(); diff --git a/src/modules/manual_control/ManualControl.hpp b/src/modules/manual_control/ManualControl.hpp index ba91fe2d7d..8302d6b0dd 100644 --- a/src/modules/manual_control/ManualControl.hpp +++ b/src/modules/manual_control/ManualControl.hpp @@ -41,6 +41,7 @@ #include #include #include +#include #include #include #include @@ -84,6 +85,9 @@ private: void Run() override; + void send_arm_command(); + void send_disarm_command(); + uORB::Publication _manual_control_setpoint_pub{ORB_ID(manual_control_setpoint)}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; @@ -92,6 +96,7 @@ private: {this, ORB_ID(manual_control_input), 1}, {this, ORB_ID(manual_control_input), 2}, }; + uORB::SubscriptionCallbackWorkItem _manual_control_switches_sub{this, ORB_ID(manual_control_switches)}; systemlib::Hysteresis _stick_arm_hysteresis{false}; systemlib::Hysteresis _stick_disarm_hysteresis{false}; @@ -100,6 +105,9 @@ private: bool _published_invalid_once{false}; int _last_selected_input{-1}; + bool _previous_arm_gesture{false}; + bool _previous_disarm_gesture{false}; + perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; perf_counter_t _loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")};