From 97aa06cc1974449039dc1e66e0ca8e70a27d6063 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 16 Apr 2021 16:45:50 +0200 Subject: [PATCH] commander: move switch handling to manual_control --- src/modules/commander/Commander.cpp | 134 ---------- src/modules/commander/Commander.hpp | 11 - src/modules/manual_control/ManualControl.cpp | 266 ++++++++++++++++++- src/modules/manual_control/ManualControl.hpp | 24 +- 4 files changed, 274 insertions(+), 161 deletions(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index d5ff74ca88..2a94f6de24 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1874,14 +1874,6 @@ Commander::run() _arm_requirements.mission = _param_arm_mission_required.get(); _arm_requirements.geofence = _param_geofence_action.get() > geofence_result_s::GF_ACTION_NONE; - /* flight mode slots */ - _flight_mode_slots[0] = _param_fltmode_1.get(); - _flight_mode_slots[1] = _param_fltmode_2.get(); - _flight_mode_slots[2] = _param_fltmode_3.get(); - _flight_mode_slots[3] = _param_fltmode_4.get(); - _flight_mode_slots[4] = _param_fltmode_5.get(); - _flight_mode_slots[5] = _param_fltmode_6.get(); - _auto_disarm_killed.set_hysteresis_time_from(false, _param_com_kill_disarm.get() * 1_s); /* check for unsafe Airmode settings: yaw airmode requires the use of an arming switch */ @@ -2466,13 +2458,6 @@ Commander::run() _landing_gear_pub.publish(landing_gear); } } - - // evaluate the main state machine according to mode switches - if (set_main_state() == TRANSITION_CHANGED) { - // play tune on mode change only if armed, blink LED always - tune_positive(_armed.armed); - _status_changed = true; - } } /* check throttle kill switch */ @@ -3103,125 +3088,6 @@ Commander::control_status_leds(bool changed, const uint8_t battery_warning) _leds_counter++; } -transition_result_t Commander::set_main_state() -{ - if ((_manual_control_switches.timestamp == 0) - || (_manual_control_switches.timestamp == _last_manual_control_switches.timestamp)) { - - // no manual control or no update -> nothing changed - return TRANSITION_NOT_CHANGED; - } - - // Note: even if _status_flags.offboard_control_set_by_command is set - // we want to allow rc mode change to take precedence. This is a safety - // feature, just in case offboard control goes crazy. - - // only switch mode based on RC switch if necessary to also allow mode switching via MAVLink - bool should_evaluate_rc_mode_switch = - (_last_manual_control_switches.offboard_switch != _manual_control_switches.offboard_switch) - || (_last_manual_control_switches.return_switch != _manual_control_switches.return_switch) - || (_last_manual_control_switches.loiter_switch != _manual_control_switches.loiter_switch) - || (_last_manual_control_switches.mode_slot != _manual_control_switches.mode_slot); - - if (_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { - // if already armed don't evaluate first time RC - if (_last_manual_control_switches.timestamp == 0) { - should_evaluate_rc_mode_switch = false; - _last_manual_control_switches = _manual_control_switches; - } - - } else { - // not armed - if (!should_evaluate_rc_mode_switch) { - // to respect initial switch position (eg POSCTL) force RC switch re-evaluation if estimates become valid - const bool altitude_got_valid = (!_last_condition_local_altitude_valid && _status_flags.condition_local_altitude_valid); - const bool lpos_got_valid = (!_last_condition_local_position_valid && _status_flags.condition_local_position_valid); - const bool gpos_got_valid = (!_last_condition_global_position_valid && _status_flags.condition_global_position_valid); - - if (altitude_got_valid || lpos_got_valid || gpos_got_valid) { - should_evaluate_rc_mode_switch = true; - } - } - } - - if (!should_evaluate_rc_mode_switch) { - /* no timestamp change or no switch change -> nothing changed */ - return TRANSITION_NOT_CHANGED; - } - - _last_manual_control_switches = _manual_control_switches; - - // reset the position and velocity validity calculation to give the best change of being able to select - // the desired mode - reset_posvel_validity(); - - /* set main state according to RC switches */ - transition_result_t res = TRANSITION_NOT_CHANGED; - - /* offboard switch overrides main switch */ - if (_manual_control_switches.offboard_switch == manual_control_switches_s::SWITCH_POS_ON) { - res = main_state_transition(_status, commander_state_s::MAIN_STATE_OFFBOARD, _status_flags, _internal_state); - - if (res == TRANSITION_DENIED) { - print_reject_mode(commander_state_s::MAIN_STATE_OFFBOARD); - /* mode rejected, continue to evaluate the main system mode */ - - } else { - /* changed successfully or already in this state */ - return res; - } - } - - /* RTL switch overrides main switch */ - if (_manual_control_switches.return_switch == manual_control_switches_s::SWITCH_POS_ON) { - res = try_mode_change(commander_state_s::MAIN_STATE_AUTO_RTL); - - if (res != TRANSITION_DENIED) { - /* changed successfully or already in this state */ - return res; - } - - /* if we get here mode was rejected, continue to evaluate the main system mode */ - } - - /* Loiter switch overrides main switch */ - if (_manual_control_switches.loiter_switch == manual_control_switches_s::SWITCH_POS_ON) { - res = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LOITER, _status_flags, _internal_state); - - if (res == TRANSITION_DENIED) { - print_reject_mode(commander_state_s::MAIN_STATE_AUTO_LOITER); - /* mode rejected, continue to evaluate the main system mode */ - - } else { - /* changed successfully or already in this state */ - return res; - } - } - - /* we know something has changed - check if we are in mode slot operation */ - if (_manual_control_switches.mode_slot != manual_control_switches_s::MODE_SLOT_NONE) { - - if (_manual_control_switches.mode_slot > manual_control_switches_s::MODE_SLOT_NUM) { - PX4_WARN("m slot overflow"); - return TRANSITION_DENIED; - } - - int new_mode = _flight_mode_slots[_manual_control_switches.mode_slot - 1]; - - if (new_mode < 0) { - /* slot is unused */ - res = TRANSITION_NOT_CHANGED; - - } else { - res = try_mode_change(new_mode); - } - - return res; - } - - return res; -} - void Commander::reset_posvel_validity() { diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 41fe75aaa0..03fd46627b 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -175,9 +175,6 @@ private: void UpdateEstimateValidity(); - // Set the system main state based on the current RC inputs - transition_result_t set_main_state(); - bool shutdown_if_allowed(); bool stabilization_required(); @@ -251,13 +248,6 @@ private: (ParamInt) _param_rc_in_off, - (ParamInt) _param_fltmode_1, - (ParamInt) _param_fltmode_2, - (ParamInt) _param_fltmode_3, - (ParamInt) _param_fltmode_4, - (ParamInt) _param_fltmode_5, - (ParamInt) _param_fltmode_6, - // Circuit breakers (ParamInt) _param_cbrk_supply_chk, (ParamInt) _param_cbrk_usb_chk, @@ -361,7 +351,6 @@ private: manual_control_switches_s _last_manual_control_switches{}; ManualControl _manual_control{this}; hrt_abstime _rc_signal_lost_timestamp{0}; ///< Time at which the RC reception was lost - int32_t _flight_mode_slots[manual_control_switches_s::MODE_SLOT_NUM] {}; hrt_abstime _boot_timestamp{0}; hrt_abstime _last_disarmed_timestamp{0}; diff --git a/src/modules/manual_control/ManualControl.cpp b/src/modules/manual_control/ManualControl.cpp index ee0afdeab8..761f25a6a9 100644 --- a/src/modules/manual_control/ManualControl.cpp +++ b/src/modules/manual_control/ManualControl.cpp @@ -36,6 +36,9 @@ #include #include #include +#include +#include +#include namespace manual_control { @@ -123,7 +126,7 @@ void ManualControl::Run() if (_selector.setpoint().arm_gesture && !_previous_arm_gesture) { _previous_arm_gesture = true; - send_arm_command(); + send_arm_command(true, ArmingOrigin::GESTURE); } else if (!_selector.setpoint().arm_gesture) { _previous_arm_gesture = false; @@ -131,7 +134,7 @@ void ManualControl::Run() if (_selector.setpoint().disarm_gesture && !_previous_disarm_gesture) { _previous_disarm_gesture = true; - send_disarm_command(); + send_arm_command(false, ArmingOrigin::GESTURE); } else if (!_selector.setpoint().disarm_gesture) { _previous_disarm_gesture = false; @@ -160,22 +163,111 @@ void ManualControl::Run() // Only use switches if current source is RC as well. if (_selector.setpoint().data_source == manual_control_input_s::SOURCE_RC) { if (_previous_switches_initialized) { + if (switches.mode_slot != _previous_switches.mode_slot) { + + switch (switches.mode_slot) { + case manual_control_switches_s::MODE_SLOT_NONE: + _last_mode_slot_flt = -1; + break; + + case manual_control_switches_s::MODE_SLOT_1: + _last_mode_slot_flt = _param_fltmode_1.get(); + break; + + case manual_control_switches_s::MODE_SLOT_2: + _last_mode_slot_flt = _param_fltmode_2.get(); + break; + + case manual_control_switches_s::MODE_SLOT_3: + _last_mode_slot_flt = _param_fltmode_3.get(); + break; + + case manual_control_switches_s::MODE_SLOT_4: + _last_mode_slot_flt = _param_fltmode_4.get(); + break; + + case manual_control_switches_s::MODE_SLOT_5: + _last_mode_slot_flt = _param_fltmode_5.get(); + break; + + case manual_control_switches_s::MODE_SLOT_6: + _last_mode_slot_flt = _param_fltmode_6.get(); + break; + + default: + _last_mode_slot_flt = -1; + PX4_WARN("mode slot overflow"); + break; + + } + + send_mode_command(_last_mode_slot_flt); + } + if (switches.arm_switch != _previous_switches.arm_switch) { if (switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON) { - send_arm_command(); + send_arm_command(true, ArmingOrigin::SWITCH); } else if (switches.arm_switch == manual_control_switches_s::SWITCH_POS_OFF) { - send_disarm_command(); + send_arm_command(false, ArmingOrigin::SWITCH); } } + // TODO: handle case with arming switch as button + if (switches.return_switch != _previous_switches.return_switch) { if (switches.return_switch == manual_control_switches_s::SWITCH_POS_ON) { send_rtl_command(); + + } else if (switches.return_switch == manual_control_switches_s::SWITCH_POS_OFF) { + send_mode_command(_last_mode_slot_flt); } } - // TODO: handle the rest of the buttons + if (switches.loiter_switch != _previous_switches.loiter_switch) { + if (switches.loiter_switch == manual_control_switches_s::SWITCH_POS_ON) { + send_loiter_command(); + + } else if (switches.loiter_switch == manual_control_switches_s::SWITCH_POS_OFF) { + send_mode_command(_last_mode_slot_flt); + } + } + + if (switches.offboard_switch != _previous_switches.offboard_switch) { + if (switches.offboard_switch == manual_control_switches_s::SWITCH_POS_ON) { + send_offboard_command(); + + } else if (switches.offboard_switch == manual_control_switches_s::SWITCH_POS_OFF) { + send_mode_command(_last_mode_slot_flt); + } + } + + if (switches.kill_switch != _previous_switches.kill_switch) { + if (switches.kill_switch == manual_control_switches_s::SWITCH_POS_ON) { + send_termination_command(true); + + } else if (switches.kill_switch == manual_control_switches_s::SWITCH_POS_OFF) { + send_termination_command(false); + } + } + + if (switches.gear_switch != _previous_switches.gear_switch) { + if (switches.gear_switch == manual_control_switches_s::SWITCH_POS_ON) { + publish_landing_gear(landing_gear_s::GEAR_UP); + + } else if (switches.gear_switch == manual_control_switches_s::SWITCH_POS_OFF) { + publish_landing_gear(landing_gear_s::GEAR_DOWN); + } + } + + if (switches.transition_switch != _previous_switches.transition_switch) { + if (switches.transition_switch == manual_control_switches_s::SWITCH_POS_ON) { + send_vtol_transition_command(vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW); + + } else if (switches.transition_switch == manual_control_switches_s::SWITCH_POS_OFF) { + send_vtol_transition_command(vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC); + } + } } _previous_switches = switches; @@ -184,6 +276,7 @@ void ManualControl::Run() } else { _previous_switches = {}; _previous_switches_initialized = false; + _last_mode_slot_flt = -1; } } @@ -219,12 +312,92 @@ void ManualControl::Run() perf_end(_loop_perf); } -void ManualControl::send_arm_command() +void ManualControl::send_mode_command(int32_t commander_main_state) { + if (commander_main_state == -1) { + // Not assigned. + return; + } + 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.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE; + command.param1 = 1.0f; + + switch (commander_main_state) { + case commander_state_s::MAIN_STATE_MANUAL: + command.param2 = PX4_CUSTOM_MAIN_MODE_MANUAL; + break; + + case commander_state_s::MAIN_STATE_ALTCTL: + command.param2 = PX4_CUSTOM_MAIN_MODE_ALTCTL; + break; + + case commander_state_s::MAIN_STATE_POSCTL: + command.param2 = PX4_CUSTOM_MAIN_MODE_POSCTL; + command.param3 = PX4_CUSTOM_SUB_MODE_POSCTL_POSCTL; + break; + + case commander_state_s::MAIN_STATE_AUTO_MISSION: + command.param2 = PX4_CUSTOM_MAIN_MODE_AUTO; + command.param3 = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; + break; + + case commander_state_s::MAIN_STATE_AUTO_LOITER: + command.param2 = PX4_CUSTOM_MAIN_MODE_AUTO; + command.param3 = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; + break; + + case commander_state_s::MAIN_STATE_AUTO_RTL: + command.param2 = PX4_CUSTOM_MAIN_MODE_AUTO; + command.param3 = PX4_CUSTOM_SUB_MODE_AUTO_RTL; + break; + + case commander_state_s::MAIN_STATE_ACRO: + command.param2 = PX4_CUSTOM_MAIN_MODE_ACRO; + break; + + case commander_state_s::MAIN_STATE_OFFBOARD: + command.param2 = PX4_CUSTOM_MAIN_MODE_OFFBOARD; + break; + + case commander_state_s::MAIN_STATE_STAB: + command.param2 = PX4_CUSTOM_MAIN_MODE_STABILIZED; + break; + + case commander_state_s::MAIN_STATE_AUTO_TAKEOFF: + command.param2 = PX4_CUSTOM_MAIN_MODE_AUTO; + command.param3 = PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF; + break; + + case commander_state_s::MAIN_STATE_AUTO_LAND: + command.param2 = PX4_CUSTOM_MAIN_MODE_AUTO; + command.param3 = PX4_CUSTOM_SUB_MODE_AUTO_LAND; + break; + + case commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET: + command.param2 = PX4_CUSTOM_MAIN_MODE_AUTO; + command.param3 = PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET; + break; + + case commander_state_s::MAIN_STATE_AUTO_PRECLAND: + command.param2 = PX4_CUSTOM_MAIN_MODE_AUTO; + command.param3 = PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND; + break; + + case commander_state_s::MAIN_STATE_ORBIT: + // TODO: check if this works without the DO_ORBIT command + command.param2 = PX4_CUSTOM_MAIN_MODE_POSCTL; + command.param3 = PX4_CUSTOM_SUB_MODE_POSCTL_ORBIT; + break; + + case commander_state_s::MAIN_STATE_MAX: + + // FALLTHROUGH + default: + PX4_WARN("Unknown main_state"); + return; + } + command.target_system = 1; command.target_component = 1; @@ -233,12 +406,14 @@ void ManualControl::send_arm_command() command_pub.publish(command); } -void ManualControl::send_disarm_command() +void ManualControl::send_arm_command(bool should_arm, ArmingOrigin origin) { 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.param1 = should_arm ? 1.0f : 0.0f; + + command.param3 = static_cast(origin); // We use param3 to signal the origin. + command.target_system = 1; command.target_component = 1; @@ -251,7 +426,7 @@ void ManualControl::send_rtl_command() { vehicle_command_s command{}; command.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE; - command.param1 = 1.0; + command.param1 = 1.0f; command.param2 = PX4_CUSTOM_MAIN_MODE_AUTO; command.param3 = PX4_CUSTOM_SUB_MODE_AUTO_RTL; command.target_system = 1; @@ -262,6 +437,71 @@ void ManualControl::send_rtl_command() command_pub.publish(command); } +void ManualControl::send_loiter_command() +{ + vehicle_command_s command{}; + command.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE; + command.param1 = 1.0f; + command.param2 = PX4_CUSTOM_MAIN_MODE_AUTO; + command.param3 = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; + 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_offboard_command() +{ + vehicle_command_s command{}; + command.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE; + command.param1 = 1.0f; + command.param2 = PX4_CUSTOM_MAIN_MODE_OFFBOARD; + command.target_system = 1; + 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_termination_command(bool should_terminate) +{ + vehicle_command_s command{}; + command.command = vehicle_command_s::VEHICLE_CMD_DO_FLIGHTTERMINATION; + command.param1 = should_terminate ? 1.0f : 0.0f; + 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::publish_landing_gear(int8_t action) +{ + landing_gear_s landing_gear{}; + landing_gear.landing_gear = action; + landing_gear.timestamp = hrt_absolute_time(); + uORB::Publication landing_gear_pub{ORB_ID(landing_gear)}; + landing_gear_pub.publish(landing_gear); +} + +void ManualControl::send_vtol_transition_command(uint8_t action) +{ + vehicle_command_s command{}; + command.command = vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION; + command.param1 = action; + 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 ab5937fc01..7a7c87ecf6 100644 --- a/src/modules/manual_control/ManualControl.hpp +++ b/src/modules/manual_control/ManualControl.hpp @@ -76,11 +76,22 @@ public: private: static constexpr int MAX_MANUAL_INPUT_COUNT = 3; + enum class ArmingOrigin { + GESTURE = 1, + SWITCH = 2, + BUTTON = 3, + }; + void Run() override; - void send_arm_command(); - void send_disarm_command(); + void send_mode_command(int32_t commander_main_state); + void send_arm_command(bool should_arm, ArmingOrigin origin); void send_rtl_command(); + void send_loiter_command(); + void send_offboard_command(); + void send_termination_command(bool should_terminate); + void publish_landing_gear(int8_t action); + void send_vtol_transition_command(uint8_t action); uORB::Publication _manual_control_setpoint_pub{ORB_ID(manual_control_setpoint)}; @@ -109,6 +120,7 @@ private: manual_control_switches_s _previous_switches{}; bool _previous_switches_initialized{false}; + int32_t _last_mode_slot_flt{-1}; 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")}; @@ -117,7 +129,13 @@ private: (ParamInt) _param_com_rc_in_mode, (ParamFloat) _param_com_rc_loss_t, (ParamFloat) _param_com_rc_stick_ov, - (ParamInt) _param_rc_arm_hyst + (ParamInt) _param_rc_arm_hyst, + (ParamInt) _param_fltmode_1, + (ParamInt) _param_fltmode_2, + (ParamInt) _param_fltmode_3, + (ParamInt) _param_fltmode_4, + (ParamInt) _param_fltmode_5, + (ParamInt) _param_fltmode_6 ) }; } // namespace manual_control