From 45cd19bcff01facb12ebd97d0708a028de8496eb Mon Sep 17 00:00:00 2001 From: Mahima Yoga Date: Thu, 18 Jun 2026 18:37:23 +0200 Subject: [PATCH] feat(Commander): add COM_FLTMODE_BOOT parameter for configurable flight mode on boot (#27641) * feat(Commander): add COM_FLTMODE_BOOT parameter for configurable flight mode on boot This adds COM_FLTMODE_BOOT, an enum parameter (Altitude/Position/Hold/Stabilized, default Hold) that controls the nav_state set on boot, before any user mode command is received. The navStateFromParam() translation function (previously a ManualControl method) has been moved to mode_util::navStateFromFlightModeParam() in ModeUtil/conversions.hpp so COM_FLTMODE_BOOT can follow the same enumeration as COM_FLTMODE* params. * chore(commander): use nav_state for mode mapping * feat(commander): extend list of modes available * chore: fix style * refactor(commander): simplify COM_FLTMODE_BOOT boot mode initialization Apply the configured boot flight mode directly in the Commander constructor. Also removes the stick-presence-based mode logic (everHadModeChange check in manualControlCheck), and changes the default to Position (NAVIGATION_STATE_POSCTL). * chore: remove accidental whitespace * chore: set default COM_FLTMODE_BOOT to Hold (nav_state 4) * feat: add Takeoff state to COM_FLTMODE_BOOT --- src/modules/commander/Commander.cpp | 10 +++----- src/modules/commander/Commander.hpp | 1 + src/modules/commander/UserModeIntention.cpp | 2 -- src/modules/commander/UserModeIntention.hpp | 10 ++------ src/modules/commander/commander_params.yaml | 28 +++++++++++++++++++++ 5 files changed, 34 insertions(+), 17 deletions(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 39ffa526e77..2a255267b88 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -837,6 +837,9 @@ Commander::Commander() : updateParameters(); + // Apply the configured boot flight mode before the first run + _user_mode_intention.change((uint8_t)_param_com_fltmode_boot.get(), ModeChangeSource::User, false, true); + _failsafe.setOnNotifyUserCallback(&Commander::onFailsafeNotifyUserTrampoline, this); _auto_disarm_killed.set_hysteresis_time_from(false, 5_s); } @@ -3153,13 +3156,6 @@ void Commander::manualControlCheck() } } - } else { - const bool is_mavlink = (manual_control_setpoint.data_source > manual_control_setpoint_s::SOURCE_RC); - - // if there's never been a mode change force position control as initial state - if (!_user_mode_intention.everHadModeChange() && (is_mavlink || !_mode_switch_mapped)) { - _user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_POSCTL, ModeChangeSource::User, false, true); - } } } } diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 1eb3024d931..8ea172e458f 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -348,6 +348,7 @@ private: (ParamInt) _param_com_flight_uuid, (ParamFloat) _param_com_cpu_max, (ParamBool) _param_com_arm_on_boot, + (ParamInt) _param_com_fltmode_boot, (ParamInt) _param_com_arm_traff ) }; diff --git a/src/modules/commander/UserModeIntention.cpp b/src/modules/commander/UserModeIntention.cpp index 2085995d6c5..c1578b2fa03 100644 --- a/src/modules/commander/UserModeIntention.cpp +++ b/src/modules/commander/UserModeIntention.cpp @@ -44,8 +44,6 @@ UserModeIntention::UserModeIntention(const vehicle_status_s &vehicle_status, bool UserModeIntention::change(uint8_t user_intended_nav_state, ModeChangeSource source, bool allow_fallback, bool force) { - _ever_had_mode_change = true; - if (_handler) { // If a replacement mode is selected, select the internal one instead. The replacement will be selected after. user_intended_nav_state = _handler->getReplacedModeIfAny(user_intended_nav_state); diff --git a/src/modules/commander/UserModeIntention.hpp b/src/modules/commander/UserModeIntention.hpp index fd563e5d492..3f229b9d1a7 100644 --- a/src/modules/commander/UserModeIntention.hpp +++ b/src/modules/commander/UserModeIntention.hpp @@ -99,11 +99,6 @@ public: */ void onDisarm(); - /** - * Returns false if there has not been any mode change yet - */ - bool everHadModeChange() const { return _ever_had_mode_change; } - bool getHadModeChangeAndClear() { bool ret = _had_mode_change; _had_mode_change = false; return ret; } private: @@ -114,9 +109,8 @@ private: const HealthAndArmingChecks &_health_and_arming_checks; ModeChangeHandler *const _handler{nullptr}; - uint8_t _user_intented_nav_state{vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER}; ///< Current user intended mode - uint8_t _nav_state_after_disarming{vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER}; ///< Mode that is switched into after landing/disarming + uint8_t _user_intented_nav_state{vehicle_status_s::NAVIGATION_STATE_POSCTL}; ///< Current user intended mode + uint8_t _nav_state_after_disarming{vehicle_status_s::NAVIGATION_STATE_POSCTL}; ///< Mode that is switched into after landing/disarming - bool _ever_had_mode_change{false}; ///< true if there was ever a mode change call (also if the same mode as already set) bool _had_mode_change{false}; ///< true if there was a mode change call since the last getHadModeChangeAndClear() }; diff --git a/src/modules/commander/commander_params.yaml b/src/modules/commander/commander_params.yaml index 3fb943abf90..5788092a081 100644 --- a/src/modules/commander/commander_params.yaml +++ b/src/modules/commander/commander_params.yaml @@ -330,6 +330,34 @@ parameters: 0: one arm 1: two step arm default: 0 + COM_FLTMODE_BOOT: + description: + short: Flight mode on boot + long: |- + Flight mode set on boot and after a power cycle, before any RC input or + mode command is received. + type: enum + values: + 0: Manual + 1: Altitude + 2: Position + 3: Mission + 4: Hold + 6: Position Slow + 8: Altitude Cruise + 10: Acro + 14: Offboard + 15: Stabilized + 17: Takeoff + 23: External 1 + 24: External 2 + 25: External 3 + 26: External 4 + 27: External 5 + 28: External 6 + 29: External 7 + 30: External 8 + default: 4 COM_ARM_ON_BOOT: description: short: Arm automatically on boot