From a349dae76098a32f0aaf9b3bbb3c3950747f5e26 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 19 Oct 2021 18:26:18 +0200 Subject: [PATCH] Use action_request to command RC VTOL transitions --- msg/action_request.msg | 2 + src/modules/manual_control/ManualControl.cpp | 17 +---- src/modules/manual_control/ManualControl.hpp | 1 - .../vtol_att_control_main.cpp | 62 +++++++------------ .../vtol_att_control/vtol_att_control_main.h | 8 +-- 5 files changed, 30 insertions(+), 60 deletions(-) diff --git a/msg/action_request.msg b/msg/action_request.msg index b31e39f670..a9c2f7258f 100644 --- a/msg/action_request.msg +++ b/msg/action_request.msg @@ -7,6 +7,8 @@ uint8 ACTION_TOGGLE_ARMING = 2 uint8 ACTION_UNKILL = 3 uint8 ACTION_KILL = 4 uint8 ACTION_SWITCH_MODE = 5 +uint8 ACTION_VTOL_TRANSITION_TO_MULTICOPTER = 6 +uint8 ACTION_VTOL_TRANSITION_TO_FIXEDWING = 7 uint8 source # how the request was triggered uint8 SOURCE_RC_STICK_GESTURE = 0 diff --git a/src/modules/manual_control/ManualControl.cpp b/src/modules/manual_control/ManualControl.cpp index 80cedac354..d2d9642306 100644 --- a/src/modules/manual_control/ManualControl.cpp +++ b/src/modules/manual_control/ManualControl.cpp @@ -232,10 +232,10 @@ void ManualControl::Run() 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); + sendActionRequest(action_request_s::ACTION_VTOL_TRANSITION_TO_FIXEDWING, action_request_s::SOURCE_RC_SWITCH); } else if (switches.transition_switch == manual_control_switches_s::SWITCH_POS_OFF) { - send_vtol_transition_command(vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC); + sendActionRequest(action_request_s::ACTION_VTOL_TRANSITION_TO_MULTICOPTER, action_request_s::SOURCE_RC_SWITCH); } } @@ -342,19 +342,6 @@ void ManualControl::publish_landing_gear(int8_t action) 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 = _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); -} - 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 1e7e4474a7..493c41756e 100644 --- a/src/modules/manual_control/ManualControl.hpp +++ b/src/modules/manual_control/ManualControl.hpp @@ -122,7 +122,6 @@ private: void evaluateModeSlot(uint8_t mode_slot); void sendActionRequest(int8_t action, int8_t source, int8_t mode = 0); void publish_landing_gear(int8_t action); - void send_vtol_transition_command(uint8_t action); uORB::Publication _action_request_pub{ORB_ID(action_request)}; uORB::Publication _manual_control_setpoint_pub{ORB_ID(manual_control_setpoint)}; diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index ce03d7e15e..8e592a1ca3 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -139,6 +139,27 @@ VtolAttitudeControl::init() return true; } +void VtolAttitudeControl::action_request_poll() +{ + while (_action_request_sub.updated()) { + action_request_s action_request; + + if (_action_request_sub.copy(&action_request)) { + switch (action_request.action) { + case action_request_s::ACTION_VTOL_TRANSITION_TO_MULTICOPTER: + _transition_command = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC; + _immediate_transition = false; + break; + + case action_request_s::ACTION_VTOL_TRANSITION_TO_FIXEDWING: + _transition_command = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW; + _immediate_transition = false; + break; + } + } + } +} + void VtolAttitudeControl::vehicle_cmd_poll() { vehicle_command_s vehicle_command; @@ -181,27 +202,6 @@ void VtolAttitudeControl::vehicle_cmd_poll() } } -/* - * Returns true if fixed-wing mode is requested. - * Changed either via switch or via command. - */ -bool -VtolAttitudeControl::is_fixed_wing_requested() -{ - bool to_fw = false; - - if (_manual_control_switches.transition_switch != manual_control_switches_s::SWITCH_POS_NONE && - _v_control_mode.flag_control_manual_enabled) { - to_fw = (_manual_control_switches.transition_switch == manual_control_switches_s::SWITCH_POS_ON); - - } else { - // listen to transition commands if not in manual or mode switch is not mapped - to_fw = (_transition_command == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW); - } - - return to_fw; -} - void VtolAttitudeControl::quadchute(QuadchuteReason reason) { @@ -434,7 +434,6 @@ VtolAttitudeControl::Run() } _v_control_mode_sub.update(&_v_control_mode); - _manual_control_switches_sub.update(&_manual_control_switches); _v_att_sub.update(&_v_att); _local_pos_sub.update(&_local_pos); _local_pos_sp_sub.update(&_local_pos_sp); @@ -442,6 +441,7 @@ VtolAttitudeControl::Run() _airspeed_validated_sub.update(&_airspeed_validated); _tecs_status_sub.update(&_tecs_status); _land_detected_sub.update(&_land_detected); + action_request_poll(); vehicle_cmd_poll(); // check if mc and fw sp were updated @@ -451,24 +451,6 @@ VtolAttitudeControl::Run() // update the vtol state machine which decides which mode we are in _vtol_type->update_vtol_state(); - // reset transition command if not auto control - if (_v_control_mode.flag_control_manual_enabled) { - if (_vtol_type->get_mode() == mode::ROTARY_WING) { - _transition_command = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC; - - } else if (_vtol_type->get_mode() == mode::FIXED_WING) { - _transition_command = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW; - - } else if (_vtol_type->get_mode() == mode::TRANSITION_TO_MC) { - /* We want to make sure that a mode change (manual>auto) during the back transition - * doesn't result in an unsafe state. This prevents the instant fall back to - * fixed-wing on the switch from manual to auto */ - _transition_command = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC; - } - } - - - // check in which mode we are in and call mode specific functions switch (_vtol_type->get_mode()) { case mode::TRANSITION_TO_FW: diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index 0f93fdc72a..88c42e7b05 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -64,9 +64,9 @@ #include #include #include +#include #include #include -#include #include #include #include @@ -116,7 +116,7 @@ public: bool init(); - bool is_fixed_wing_requested(); + bool is_fixed_wing_requested() { return _transition_command == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW; }; void quadchute(QuadchuteReason reason); int get_transition_command() {return _transition_command;} bool get_immediate_transition() {return _immediate_transition;} @@ -150,12 +150,12 @@ private: uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + uORB::Subscription _action_request_sub{ORB_ID(action_request)}; uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)}; // airspeed subscription uORB::Subscription _fw_virtual_att_sp_sub{ORB_ID(fw_virtual_attitude_setpoint)}; uORB::Subscription _land_detected_sub{ORB_ID(vehicle_land_detected)}; uORB::Subscription _local_pos_sp_sub{ORB_ID(vehicle_local_position_setpoint)}; // setpoint subscription uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)}; // sensor subscription - uORB::Subscription _manual_control_switches_sub{ORB_ID(manual_control_switches)}; //manual control setpoint subscription uORB::Subscription _mc_virtual_att_sp_sub{ORB_ID(mc_virtual_attitude_setpoint)}; uORB::Subscription _pos_sp_triplet_sub{ORB_ID(position_setpoint_triplet)}; // local position setpoint subscription uORB::Subscription _tecs_status_sub{ORB_ID(tecs_status)}; @@ -181,7 +181,6 @@ private: actuator_controls_s _actuators_out_1{}; //actuator controls going to the fw mixer (used for elevons) airspeed_validated_s _airspeed_validated{}; // airspeed - manual_control_switches_s _manual_control_switches{}; //manual control setpoint position_setpoint_triplet_s _pos_sp_triplet{}; tecs_status_s _tecs_status{}; vehicle_attitude_s _v_att{}; //vehicle attitude @@ -243,6 +242,7 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ + void action_request_poll(); void vehicle_cmd_poll(); int parameters_update(); //Update local parameter cache