From 6de6abfb64961abc590787213dfce888e3a367cd Mon Sep 17 00:00:00 2001 From: mahima-yoga Date: Fri, 9 Jan 2026 16:55:57 +0100 Subject: [PATCH] RunwayTakeoff: add the RunwayTakeoffState to the FixedWingRunwayControl.msg --- msg/FixedWingRunwayControl.msg | 9 ++++++++- src/modules/fw_mode_manager/FixedWingModeManager.cpp | 4 ++++ .../fw_mode_manager/runway_takeoff/RunwayTakeoff.cpp | 8 ++++---- .../fw_mode_manager/runway_takeoff/RunwayTakeoff.h | 4 ++-- 4 files changed, 18 insertions(+), 7 deletions(-) diff --git a/msg/FixedWingRunwayControl.msg b/msg/FixedWingRunwayControl.msg index 62d09a5ebf..f83e109547 100644 --- a/msg/FixedWingRunwayControl.msg +++ b/msg/FixedWingRunwayControl.msg @@ -1,8 +1,15 @@ # Auxiliary control fields for fixed-wing runway takeoff/landing -# Passes information from the FixedWingModeManager to the FixedWingAttitudeController +# Passes information from the FixedWingModeManager to the FixedWingAttitudeController (wheel control) and FixedWingLandDetector (takeoff state) uint64 timestamp # [us] time since system start +uint8 STATE_THROTTLE_RAMP = 0 # ramping up throttle +uint8 STATE_CLAMPED_TO_RUNWAY = 1 # clamped to runway, controlling yaw directly (wheel or rudder) +uint8 STATE_CLIMBOUT = 2 # climbout to safe height before navigation +uint8 STATE_FLYING = 3 # navigate freely + +uint8 runway_takeoff_state # Current state of runway takeoff state machine + bool wheel_steering_enabled # Flag that enables the wheel steering. float32 wheel_steering_nudging_rate # [norm] [@range -1, 1] [FRD] Manual wheel nudging, added to controller output. NAN is interpreted as 0. diff --git a/src/modules/fw_mode_manager/FixedWingModeManager.cpp b/src/modules/fw_mode_manager/FixedWingModeManager.cpp index c41496e350..ce11ef7f9c 100644 --- a/src/modules/fw_mode_manager/FixedWingModeManager.cpp +++ b/src/modules/fw_mode_manager/FixedWingModeManager.cpp @@ -1170,6 +1170,7 @@ FixedWingModeManager::control_auto_takeoff(const hrt_abstime &now, const float c fixed_wing_runway_control_s fw_runway_control{}; fw_runway_control.timestamp = now; + fw_runway_control.runway_takeoff_state = _runway_takeoff.getState(); fw_runway_control.wheel_steering_enabled = true; fw_runway_control.wheel_steering_nudging_rate = _param_rwto_nudge.get() ? _sticks.getYaw() : 0.f; @@ -1343,6 +1344,7 @@ FixedWingModeManager::control_auto_takeoff_no_nav(const hrt_abstime &now, const fixed_wing_runway_control_s fw_runway_control{}; fw_runway_control.timestamp = now; + fw_runway_control.runway_takeoff_state = _runway_takeoff.getState(); fw_runway_control.wheel_steering_enabled = true; fw_runway_control.wheel_steering_nudging_rate = _param_rwto_nudge.get() ? _sticks.getYaw() : 0.f; @@ -1591,6 +1593,7 @@ FixedWingModeManager::control_auto_landing_straight(const hrt_abstime &now, cons fixed_wing_runway_control_s fw_runway_control{}; fw_runway_control.timestamp = now; + fw_runway_control.runway_takeoff_state = fixed_wing_runway_control_s::STATE_FLYING; // not in takeoff, use FLYING as default fw_runway_control.wheel_steering_enabled = true; fw_runway_control.wheel_steering_nudging_rate = _param_fw_lnd_nudge.get() > LandingNudgingOption::kNudgingDisabled ? _sticks.getYaw() : 0.f; @@ -1761,6 +1764,7 @@ FixedWingModeManager::control_auto_landing_circular(const hrt_abstime &now, cons fixed_wing_runway_control_s fw_runway_control{}; fw_runway_control.timestamp = now; + fw_runway_control.runway_takeoff_state = fixed_wing_runway_control_s::STATE_FLYING; // not in takeoff, use FLYING as default fw_runway_control.wheel_steering_enabled = true; fw_runway_control.wheel_steering_nudging_rate = _param_fw_lnd_nudge.get() > LandingNudgingOption::kNudgingDisabled ? _sticks.getYaw() : 0.f; diff --git a/src/modules/fw_mode_manager/runway_takeoff/RunwayTakeoff.cpp b/src/modules/fw_mode_manager/runway_takeoff/RunwayTakeoff.cpp index 498a5332b1..96cffebb52 100644 --- a/src/modules/fw_mode_manager/runway_takeoff/RunwayTakeoff.cpp +++ b/src/modules/fw_mode_manager/runway_takeoff/RunwayTakeoff.cpp @@ -86,7 +86,7 @@ void RunwayTakeoff::update(const hrt_abstime &time_now, const float takeoff_airs case RunwayTakeoffState::CLIMBOUT: if (vehicle_altitude > clearance_altitude) { - takeoff_state_ = RunwayTakeoffState::FLY; + takeoff_state_ = RunwayTakeoffState::FLYING; events::send(events::ID("runway_takeoff_reached_clearance_altitude"), events::Log::Info, "Reached clearance altitude"); } @@ -134,7 +134,7 @@ float RunwayTakeoff::getThrottle(const float idle_throttle) const break; - case RunwayTakeoffState::FLY: + case RunwayTakeoffState::FLYING: throttle = NAN; } @@ -147,7 +147,7 @@ float RunwayTakeoff::getMinPitch(float min_pitch_in_climbout, float min_pitch) c // constrain to the taxi pitch setpoint return math::radians(param_rwto_psp_.get() - 0.01f); - } else if (takeoff_state_ < RunwayTakeoffState::FLY) { + } else if (takeoff_state_ < RunwayTakeoffState::FLYING) { // ramp in the climbout pitch constraint over the rotation transition time const float taxi_pitch_min = math::radians(param_rwto_psp_.get() - 0.01f); return interpolateValuesOverAbsoluteTime(taxi_pitch_min, min_pitch_in_climbout, takeoff_time_, @@ -164,7 +164,7 @@ float RunwayTakeoff::getMaxPitch(const float max_pitch) const // constrain to the taxi pitch setpoint return math::radians(param_rwto_psp_.get() + 0.01f); - } else if (takeoff_state_ < RunwayTakeoffState::FLY) { + } else if (takeoff_state_ < RunwayTakeoffState::FLYING) { // ramp in the climbout pitch constraint over the rotation transition time const float taxi_pitch_max = math::radians(param_rwto_psp_.get() + 0.01f); return interpolateValuesOverAbsoluteTime(taxi_pitch_max, max_pitch, takeoff_time_, param_rwto_rot_time_.get()); diff --git a/src/modules/fw_mode_manager/runway_takeoff/RunwayTakeoff.h b/src/modules/fw_mode_manager/runway_takeoff/RunwayTakeoff.h index 22fa8a51ec..5b77426bca 100644 --- a/src/modules/fw_mode_manager/runway_takeoff/RunwayTakeoff.h +++ b/src/modules/fw_mode_manager/runway_takeoff/RunwayTakeoff.h @@ -57,7 +57,7 @@ enum RunwayTakeoffState { THROTTLE_RAMP = 0, // ramping up throttle CLAMPED_TO_RUNWAY, // clamped to runway, controlling yaw directly (wheel or rudder) CLIMBOUT, // climbout to safe height before navigation - FLY // navigate freely + FLYING // navigate freely }; class __EXPORT RunwayTakeoff : public ModuleParams @@ -135,7 +135,7 @@ public: float getMaxPitch(const float max_pitch) const; // NOTE: this is only to be used for mistaken mode transitions to takeoff while already in air - void forceSetFlyState() { takeoff_state_ = RunwayTakeoffState::FLY; } + void forceSetFlyState() { takeoff_state_ = RunwayTakeoffState::FLYING; } /** * @brief Reset the state machine.