From 956997eb1e5f5b47480b4a6fdfb2c5bf131f4a5d Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 19 Oct 2021 15:54:49 +0200 Subject: [PATCH] Replace arm_request and mode_request with combined action_request Which saves flash space, log size and is extensible to handle e.g. the VTOL transition and future actions. --- msg/CMakeLists.txt | 3 +- msg/{arm_request.msg => action_request.msg} | 4 ++ msg/mode_request.msg | 7 --- src/modules/commander/Commander.cpp | 38 ++++++------- src/modules/commander/Commander.hpp | 6 +- src/modules/logger/logged_topics.cpp | 3 +- src/modules/manual_control/ManualControl.cpp | 59 +++++++++----------- src/modules/manual_control/ManualControl.hpp | 9 +-- 8 files changed, 55 insertions(+), 74 deletions(-) rename msg/{arm_request.msg => action_request.msg} (68%) delete mode 100644 msg/mode_request.msg diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 239e1f7489..4314fb82ef 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -37,6 +37,7 @@ cmake_policy(SET CMP0057 NEW) include(px4_list_make_absolute) set(msg_files + action_request.msg actuator_armed.msg actuator_controls.msg actuator_controls_status.msg @@ -47,7 +48,6 @@ 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 @@ -109,7 +109,6 @@ set(msg_files mavlink_log.msg mission.msg mission_result.msg - mode_request.msg mount_orientation.msg multirotor_motor_limits.msg navigator_mission_item.msg diff --git a/msg/arm_request.msg b/msg/action_request.msg similarity index 68% rename from msg/arm_request.msg rename to msg/action_request.msg index f97abf319f..b31e39f670 100644 --- a/msg/arm_request.msg +++ b/msg/action_request.msg @@ -6,8 +6,12 @@ uint8 ACTION_ARM = 1 uint8 ACTION_TOGGLE_ARMING = 2 uint8 ACTION_UNKILL = 3 uint8 ACTION_KILL = 4 +uint8 ACTION_SWITCH_MODE = 5 uint8 source # how the request was triggered uint8 SOURCE_RC_STICK_GESTURE = 0 uint8 SOURCE_RC_SWITCH = 1 uint8 SOURCE_RC_BUTTON = 2 +uint8 SOURCE_RC_MODE_SLOT = 3 + +uint8 mode # for ACTION_SWITCH_MODE what mode is requested according to commander_state.MAIN_STATE_ diff --git a/msg/mode_request.msg b/msg/mode_request.msg deleted file mode 100644 index 5050d1936b..0000000000 --- a/msg/mode_request.msg +++ /dev/null @@ -1,7 +0,0 @@ -uint64 timestamp # time since system start (microseconds) - -uint8 mode # what mode is requested according to commander_state.MAIN_STATE_ - -uint8 source # how the request was triggered -uint8 SOURCE_RC_MODE_SLOT = 0 -uint8 SOURCE_RC_SWITCH = 1 diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 32739ba214..dda5f5353f 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -2449,26 +2449,26 @@ Commander::run() } } - while (_arm_request_sub.updated()) { - arm_request_s arm_request; + while (_action_request_sub.updated()) { + action_request_s action_request; - if (_arm_request_sub.copy(&arm_request)) { + if (_action_request_sub.copy(&action_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; + switch (action_request.source) { + case action_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 action_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; + case action_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; + switch (action_request.action) { + case action_request_s::ACTION_DISARM: disarm(arm_disarm_reason); break; - case arm_request_s::ACTION_ARM: arm(arm_disarm_reason); break; + case action_request_s::ACTION_ARM: arm(arm_disarm_reason); break; - case arm_request_s::ACTION_TOGGLE_ARMING: + case action_request_s::ACTION_TOGGLE_ARMING: if (_armed.armed) { disarm(arm_disarm_reason); @@ -2478,7 +2478,7 @@ Commander::run() break; - case arm_request_s::ACTION_UNKILL: + case action_request_s::ACTION_UNKILL: if (arm_disarm_reason == arm_disarm_reason_t::rc_switch && _armed.manual_lockdown) { mavlink_log_info(&_mavlink_log_pub, "Kill-switch disengaged\t"); events::send(events::ID("commander_kill_sw_disengaged"), events::Log::Info, "Kill-switch disengaged"); @@ -2488,7 +2488,7 @@ Commander::run() break; - case arm_request_s::ACTION_KILL: + case action_request_s::ACTION_KILL: if (arm_disarm_reason == arm_disarm_reason_t::rc_switch && !_armed.manual_lockdown) { const char kill_switch_string[] = "Kill-switch engaged\t"; events::LogLevels log_levels{events::Log::Info}; @@ -2508,18 +2508,14 @@ Commander::run() } break; + + case action_request_s::ACTION_SWITCH_MODE: + main_state_transition(_status, action_request.mode, _status_flags, _internal_state); + break; } } } - while (_mode_request_sub.updated()) { - mode_request_s mode_request; - - if (_mode_request_sub.copy(&mode_request)) { - main_state_transition(_status, mode_request.mode, _status_flags, _internal_state); - } - } - /* 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 c7318bf476..9e46558683 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -59,9 +59,9 @@ // subscriptions #include #include +#include #include #include -#include #include #include #include @@ -73,7 +73,6 @@ #include #include #include -#include #include #include #include @@ -390,8 +389,8 @@ private: WorkerThread _worker_thread; // Subscriptions + uORB::Subscription _action_request_sub {ORB_ID(action_request)}; 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)}; @@ -400,7 +399,6 @@ private: uORB::Subscription _iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)}; uORB::Subscription _land_detector_sub{ORB_ID(vehicle_land_detected)}; uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; - uORB::Subscription _mode_request_sub {ORB_ID(mode_request)}; uORB::Subscription _safety_sub{ORB_ID(safety)}; uORB::Subscription _system_power_sub{ORB_ID(system_power)}; uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index b5e602b935..424f7671c5 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -45,6 +45,7 @@ using namespace px4::logger; void LoggedTopics::add_default_topics() { + add_topic("action_request"); add_topic("actuator_armed"); add_topic("actuator_controls_0", 50); add_topic("actuator_controls_1", 100); @@ -53,7 +54,6 @@ void LoggedTopics::add_default_topics() add_topic("actuator_controls_status_0", 300); add_topic("airspeed", 1000); add_topic("airspeed_validated", 200); - add_topic("arm_request"); add_topic("autotune_attitude_control_status", 100); add_topic("camera_capture"); add_topic("camera_trigger"); @@ -73,7 +73,6 @@ void LoggedTopics::add_default_topics() add_topic("manual_control_setpoint", 200); add_topic("manual_control_switches"); add_topic("mission_result"); - add_topic("mode_request"); add_topic("navigator_mission_item"); add_topic("offboard_control_mode", 100); add_topic("onboard_computer_status", 10); diff --git a/src/modules/manual_control/ManualControl.cpp b/src/modules/manual_control/ManualControl.cpp index 28b3f8542e..80cedac354 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()) { - sendArmRequest(arm_request_s::ACTION_ARM, arm_request_s::SOURCE_RC_STICK_GESTURE); + sendActionRequest(action_request_s::ACTION_ARM, action_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()) { - sendArmRequest(arm_request_s::ACTION_DISARM, arm_request_s::SOURCE_RC_STICK_GESTURE); + sendActionRequest(action_request_s::ACTION_DISARM, action_request_s::SOURCE_RC_STICK_GESTURE); } // User override by stick @@ -165,24 +165,25 @@ 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()) { - sendArmRequest(arm_request_s::ACTION_TOGGLE_ARMING, arm_request_s::SOURCE_RC_BUTTON); + sendActionRequest(action_request_s::ACTION_TOGGLE_ARMING, action_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) { - sendArmRequest(arm_request_s::ACTION_ARM, arm_request_s::SOURCE_RC_SWITCH); + sendActionRequest(action_request_s::ACTION_ARM, action_request_s::SOURCE_RC_SWITCH); } else if (switches.arm_switch == manual_control_switches_s::SWITCH_POS_OFF) { - sendArmRequest(arm_request_s::ACTION_DISARM, arm_request_s::SOURCE_RC_SWITCH); + sendActionRequest(action_request_s::ACTION_DISARM, action_request_s::SOURCE_RC_SWITCH); } } } if (switches.return_switch != _previous_switches.return_switch) { if (switches.return_switch == manual_control_switches_s::SWITCH_POS_ON) { - sendModeRequest(commander_state_s::MAIN_STATE_AUTO_RTL, mode_request_s::SOURCE_RC_SWITCH); + sendActionRequest(action_request_s::ACTION_SWITCH_MODE, action_request_s::SOURCE_RC_MODE_SLOT, + commander_state_s::MAIN_STATE_AUTO_RTL); } else if (switches.return_switch == manual_control_switches_s::SWITCH_POS_OFF) { evaluateModeSlot(switches.mode_slot); @@ -191,7 +192,8 @@ void ManualControl::Run() if (switches.loiter_switch != _previous_switches.loiter_switch) { if (switches.loiter_switch == manual_control_switches_s::SWITCH_POS_ON) { - sendModeRequest(commander_state_s::MAIN_STATE_AUTO_LOITER, mode_request_s::SOURCE_RC_SWITCH); + sendActionRequest(action_request_s::ACTION_SWITCH_MODE, action_request_s::SOURCE_RC_MODE_SLOT, + commander_state_s::MAIN_STATE_AUTO_LOITER); } else if (switches.loiter_switch == manual_control_switches_s::SWITCH_POS_OFF) { evaluateModeSlot(switches.mode_slot); @@ -200,7 +202,8 @@ void ManualControl::Run() if (switches.offboard_switch != _previous_switches.offboard_switch) { if (switches.offboard_switch == manual_control_switches_s::SWITCH_POS_ON) { - sendModeRequest(commander_state_s::MAIN_STATE_OFFBOARD, mode_request_s::SOURCE_RC_SWITCH); + sendActionRequest(action_request_s::ACTION_SWITCH_MODE, action_request_s::SOURCE_RC_MODE_SLOT, + commander_state_s::MAIN_STATE_OFFBOARD); } else if (switches.offboard_switch == manual_control_switches_s::SWITCH_POS_OFF) { evaluateModeSlot(switches.mode_slot); @@ -209,10 +212,10 @@ void ManualControl::Run() if (switches.kill_switch != _previous_switches.kill_switch) { if (switches.kill_switch == manual_control_switches_s::SWITCH_POS_ON) { - sendArmRequest(arm_request_s::ACTION_KILL, arm_request_s::SOURCE_RC_SWITCH); + sendActionRequest(action_request_s::ACTION_KILL, action_request_s::SOURCE_RC_SWITCH); } else if (switches.kill_switch == manual_control_switches_s::SWITCH_POS_OFF) { - sendArmRequest(arm_request_s::ACTION_UNKILL, arm_request_s::SOURCE_RC_SWITCH); + sendActionRequest(action_request_s::ACTION_UNKILL, action_request_s::SOURCE_RC_SWITCH); } } @@ -237,7 +240,7 @@ void ManualControl::Run() } } else { - // Send an initial command to switch to the mode requested by RC + // Send an initial request to switch to the mode requested by RC evaluateModeSlot(switches.mode_slot); } @@ -291,27 +294,27 @@ void ManualControl::evaluateModeSlot(uint8_t mode_slot) break; case manual_control_switches_s::MODE_SLOT_1: - sendModeRequest(_param_fltmode_1.get(), mode_request_s::SOURCE_RC_MODE_SLOT); + sendActionRequest(action_request_s::ACTION_SWITCH_MODE, action_request_s::SOURCE_RC_MODE_SLOT, _param_fltmode_1.get()); break; case manual_control_switches_s::MODE_SLOT_2: - sendModeRequest(_param_fltmode_2.get(), mode_request_s::SOURCE_RC_MODE_SLOT); + sendActionRequest(action_request_s::ACTION_SWITCH_MODE, action_request_s::SOURCE_RC_MODE_SLOT, _param_fltmode_2.get()); break; case manual_control_switches_s::MODE_SLOT_3: - sendModeRequest(_param_fltmode_3.get(), mode_request_s::SOURCE_RC_MODE_SLOT); + sendActionRequest(action_request_s::ACTION_SWITCH_MODE, action_request_s::SOURCE_RC_MODE_SLOT, _param_fltmode_3.get()); break; case manual_control_switches_s::MODE_SLOT_4: - sendModeRequest(_param_fltmode_4.get(), mode_request_s::SOURCE_RC_MODE_SLOT); + sendActionRequest(action_request_s::ACTION_SWITCH_MODE, action_request_s::SOURCE_RC_MODE_SLOT, _param_fltmode_4.get()); break; case manual_control_switches_s::MODE_SLOT_5: - sendModeRequest(_param_fltmode_5.get(), mode_request_s::SOURCE_RC_MODE_SLOT); + sendActionRequest(action_request_s::ACTION_SWITCH_MODE, action_request_s::SOURCE_RC_MODE_SLOT, _param_fltmode_5.get()); break; case manual_control_switches_s::MODE_SLOT_6: - sendModeRequest(_param_fltmode_6.get(), mode_request_s::SOURCE_RC_MODE_SLOT); + sendActionRequest(action_request_s::ACTION_SWITCH_MODE, action_request_s::SOURCE_RC_MODE_SLOT, _param_fltmode_6.get()); break; default: @@ -320,22 +323,14 @@ void ManualControl::evaluateModeSlot(uint8_t mode_slot) } } -void ManualControl::sendModeRequest(uint8_t mode, uint8_t source) +void ManualControl::sendActionRequest(int8_t action, int8_t source, int8_t mode) { - mode_request_s mode_request{}; - mode_request.mode = mode; - mode_request.source = source; - mode_request.timestamp = hrt_absolute_time(); - _mode_request_pub.publish(mode_request); -} - -void ManualControl::sendArmRequest(int8_t action, int8_t source) -{ - 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); + action_request_s action_request{}; + action_request.action = action; + action_request.source = source; + action_request.mode = mode; + action_request.timestamp = hrt_absolute_time(); + _action_request_pub.publish(action_request); } void ManualControl::publish_landing_gear(int8_t action) diff --git a/src/modules/manual_control/ManualControl.hpp b/src/modules/manual_control/ManualControl.hpp index d4a329d944..1e7e4474a7 100644 --- a/src/modules/manual_control/ManualControl.hpp +++ b/src/modules/manual_control/ManualControl.hpp @@ -40,11 +40,10 @@ #include #include #include -#include +#include #include #include #include -#include #include #include #include @@ -121,14 +120,12 @@ private: void Run() override; void evaluateModeSlot(uint8_t mode_slot); - void sendModeRequest(uint8_t mode, uint8_t source); - void sendArmRequest(int8_t action, int8_t source); + 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 _arm_request_pub{ORB_ID(arm_request)}; + uORB::Publication _action_request_pub{ORB_ID(action_request)}; uORB::Publication _manual_control_setpoint_pub{ORB_ID(manual_control_setpoint)}; - uORB::Publication _mode_request_pub{ORB_ID(mode_request)}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::SubscriptionCallbackWorkItem _manual_control_input_subs[MAX_MANUAL_INPUT_COUNT] {