From 05d40f40d42201100445a176862d3d5855dcecf1 Mon Sep 17 00:00:00 2001 From: Claudio Micheli Date: Mon, 16 Aug 2021 11:19:20 +0200 Subject: [PATCH] Handle rc toggle mode buttons to initialize flight modes Signed-off-by: Claudio Micheli --- src/modules/commander/Commander.cpp | 2 +- src/modules/commander/ManualControl.cpp | 8 ++++++++ src/modules/commander/ManualControl.hpp | 5 +++-- 3 files changed, 12 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index a6f0e6c885..b968a8097f 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -2428,7 +2428,7 @@ Commander::run() } } - if (!_armed.armed && _manual_control.isMavlink() && (_internal_state.main_state_changes == 0)) { + if (!_armed.armed && _manual_control.isModeInitializationRequired() && (_internal_state.main_state_changes == 0)) { // if there's never been a mode change force position control as initial state _internal_state.main_state = commander_state_s::MAIN_STATE_POSCTL; } diff --git a/src/modules/commander/ManualControl.cpp b/src/modules/commander/ManualControl.cpp index 6d9bc6bd11..4c2430cbe9 100644 --- a/src/modules/commander/ManualControl.cpp +++ b/src/modules/commander/ManualControl.cpp @@ -177,6 +177,14 @@ bool ManualControl::wantsArm(const vehicle_control_mode_s &vehicle_control_mode, return ret; } +bool ManualControl::isModeInitializationRequired() +{ + const bool isMavlink = _manual_control_setpoint.data_source > manual_control_setpoint_s::SOURCE_RC; + const bool rc_uses_toggle_buttons = !isMavlink && _param_rc_map_flightmode_buttons.get() > 0; + + return (isMavlink || rc_uses_toggle_buttons); +} + void ManualControl::updateParams() { ModuleParams::updateParams(); diff --git a/src/modules/commander/ManualControl.hpp b/src/modules/commander/ManualControl.hpp index 862b2afdb1..cce7c118d6 100644 --- a/src/modules/commander/ManualControl.hpp +++ b/src/modules/commander/ManualControl.hpp @@ -64,12 +64,12 @@ public: */ bool update(); bool isRCAvailable() const { return _rc_available; } - bool isMavlink() const { return _manual_control_setpoint.data_source > manual_control_setpoint_s::SOURCE_RC; } bool wantsOverride(const vehicle_control_mode_s &vehicle_control_mode, const vehicle_status_s &vehicle_status); bool wantsDisarm(const vehicle_control_mode_s &vehicle_control_mode, const vehicle_status_s &vehicle_status, manual_control_switches_s &manual_control_switches, const bool landed); bool wantsArm(const vehicle_control_mode_s &vehicle_control_mode, const vehicle_status_s &vehicle_status, const manual_control_switches_s &manual_control_switches, const bool landed); + bool isModeInitializationRequired(); bool isThrottleLow() const { return _last_manual_control_setpoint.z < 0.1f; } bool isThrottleAboveCenter() const { return _last_manual_control_setpoint.z > 0.6f; } hrt_abstime getLastRcTimestamp() const { return _last_manual_control_setpoint.timestamp; } @@ -96,6 +96,7 @@ private: (ParamBool) _param_com_arm_swisbtn, (ParamBool) _param_com_rearm_grace, (ParamInt) _param_rc_override, - (ParamFloat) _param_com_rc_stick_ov + (ParamFloat) _param_com_rc_stick_ov, + (ParamInt) _param_rc_map_flightmode_buttons ) };