From 3df0a39dc18102e799a5a8685492110ef068596e Mon Sep 17 00:00:00 2001 From: Silvan Date: Wed, 8 Jul 2026 19:07:06 +0200 Subject: [PATCH] feat(FWModeManager): runway takeoff set yaw setpoint to bearing to takeoff WP Signed-off-by: Silvan --- msg/FixedWingRunwayControl.msg | 1 + src/modules/fw_att_control/FixedwingAttitudeControl.cpp | 8 ++++++-- src/modules/fw_mode_manager/FixedWingModeManager.cpp | 8 ++++++++ 3 files changed, 15 insertions(+), 2 deletions(-) diff --git a/msg/FixedWingRunwayControl.msg b/msg/FixedWingRunwayControl.msg index f83e109547e9..162fae68e7fc 100644 --- a/msg/FixedWingRunwayControl.msg +++ b/msg/FixedWingRunwayControl.msg @@ -12,4 +12,5 @@ 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_yaw_setpoint # [rad] [@range -pi, pi] For the wheel controller to track. NAN means hold the heading captured when steering engaged. 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_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index 0a8300c7a823..52d40fc0aa88 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -359,8 +359,12 @@ void FixedwingAttitudeControl::Run() } - // set now yaw setpoint once we're entering the first time - if (!PX4_ISFINITE(_steering_wheel_yaw_setpoint)) { + // track the heading commanded by the mode manager (e.g. the runway bearing towards the + // takeoff waypoint); if none is provided, hold the heading captured when steering engaged + if (PX4_ISFINITE(runway_control.wheel_steering_yaw_setpoint)) { + _steering_wheel_yaw_setpoint = runway_control.wheel_steering_yaw_setpoint; + + } else if (!PX4_ISFINITE(_steering_wheel_yaw_setpoint)) { _steering_wheel_yaw_setpoint = euler_angles.psi(); } diff --git a/src/modules/fw_mode_manager/FixedWingModeManager.cpp b/src/modules/fw_mode_manager/FixedWingModeManager.cpp index af9ffad84549..c18b14868686 100644 --- a/src/modules/fw_mode_manager/FixedWingModeManager.cpp +++ b/src/modules/fw_mode_manager/FixedWingModeManager.cpp @@ -1220,6 +1220,8 @@ FixedWingModeManager::control_auto_takeoff(const hrt_abstime &now, const float c fw_runway_control.timestamp = now; fw_runway_control.runway_takeoff_state = _runway_takeoff.getState(); fw_runway_control.wheel_steering_enabled = true; + // keep the nose aligned with the runway bearing towards the takeoff waypoint during the ground roll + fw_runway_control.wheel_steering_yaw_setpoint = takeoff_bearing; fw_runway_control.wheel_steering_nudging_rate = _param_rwto_nudge.get() ? _sticks.getYaw() : 0.f; _fixed_wing_runway_control_pub.publish(fw_runway_control); @@ -1398,6 +1400,8 @@ FixedWingModeManager::control_auto_takeoff_no_nav(const hrt_abstime &now, const fw_runway_control.timestamp = now; fw_runway_control.runway_takeoff_state = _runway_takeoff.getState(); fw_runway_control.wheel_steering_enabled = true; + // no navigation reference available: hold the heading captured when steering engaged + fw_runway_control.wheel_steering_yaw_setpoint = NAN; fw_runway_control.wheel_steering_nudging_rate = _param_rwto_nudge.get() ? _sticks.getYaw() : 0.f; _fixed_wing_runway_control_pub.publish(fw_runway_control); @@ -1647,6 +1651,8 @@ FixedWingModeManager::control_auto_landing_straight(const hrt_abstime &now, cons 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; + // during landing rollout hold the heading captured at touchdown + fw_runway_control.wheel_steering_yaw_setpoint = NAN; fw_runway_control.wheel_steering_nudging_rate = _param_fw_lnd_nudge.get() > LandingNudgingOption::kNudgingDisabled ? _sticks.getYaw() : 0.f; @@ -1820,6 +1826,8 @@ FixedWingModeManager::control_auto_landing_circular(const hrt_abstime &now, cons 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; + // during landing rollout hold the heading captured at touchdown + fw_runway_control.wheel_steering_yaw_setpoint = NAN; fw_runway_control.wheel_steering_nudging_rate = _param_fw_lnd_nudge.get() > LandingNudgingOption::kNudgingDisabled ? _sticks.getYaw() : 0.f;