From 671f60e6dabe8a6d6ff1456d55cce8baca6a74dd Mon Sep 17 00:00:00 2001 From: gguidone Date: Thu, 18 Jun 2026 13:29:21 +0200 Subject: [PATCH] fix(flight_mode_manager): keep auto-takeoff vertical on a moving deck On a moving deck the lift-off XY lock alone still leaned the vehicle: the position smoother carried the inherited deck velocity into the takeoff and braked it with a backward tilt during the thrust-limited ramp, which sprang back at FLIGHT. Give altitude priority for the first metre after lift-off: hold the live horizontal position through the ramp (zero tracking error, no lean or spring at FLIGHT) and the live horizontal velocity through the ramp and the first metre of climb. Self-nulling on a static takeoff (no inherited velocity), so the path is identical for static and moving takeoffs - no new param, no branch. Signed-off-by: gguidone --- .../tasks/Auto/FlightTaskAuto.cpp | 29 ++++++++++--------- .../tasks/Auto/FlightTaskAuto.hpp | 2 +- 2 files changed, 17 insertions(+), 14 deletions(-) diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index 916f8151e9e..38ae6a835a2 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -40,6 +40,9 @@ using namespace matrix; +// First meter after lift-off prioritises altitude over horizontal tracking. +static constexpr float kTakeoffClimbPriorityHeight = 1.0f; // [m] + bool FlightTaskAuto::activate(const trajectory_setpoint_s &last_setpoint) { bool ret = FlightTask::activate(last_setpoint); @@ -75,6 +78,7 @@ bool FlightTaskAuto::activate(const trajectory_setpoint_s &last_setpoint) _is_emergency_braking_active = false; _time_last_cruise_speed_override = 0; _takeoff_locked_xy.setNaN(); + _takeoff_liftoff_z = NAN; return ret; } @@ -86,6 +90,7 @@ void FlightTaskAuto::reActivate() // On ground, reset acceleration and velocity to zero _position_smoothing.reset({0.f, 0.f, 0.f}, {0.f, 0.f, 0.7f}, _position); _takeoff_locked_xy.setNaN(); + _takeoff_liftoff_z = NAN; } bool FlightTaskAuto::updateInitialize() @@ -160,11 +165,8 @@ bool FlightTaskAuto::update() _position_setpoint = _triplet_current; _velocity_setpoint.setNaN(); - // During takeoff, lock the lift-off XY so the climb stays vertical and wind-robust: - // track the live position through the ramp (freezes at FLIGHT as the deck moves), then - // hold it as the target until takeoff completes. Z stays at the takeoff target altitude. if (_type == WaypointType::takeoff) { - if (_inTakeoffRamp()) { + if (_takeoff_status_sub.get().takeoff_state < takeoff_status_s::TAKEOFF_STATE_FLIGHT) { _takeoff_locked_xy = Vector2f(_position); } @@ -190,9 +192,16 @@ bool FlightTaskAuto::update() const bool force_zero_velocity_setpoint = should_wait_for_yaw_align || _is_emergency_braking_active; _updateTrajConstraints(); - if (_inTakeoffRamp()) { - // Hold the live horizontal position during the takeoff ramp so the climb isn't slowed and tracks a moving deck - _position_smoothing.forceSetPosition({_position(0), _position(1), NAN}); + if (_type == WaypointType::takeoff) { + if (_takeoff_status_sub.get().takeoff_state < takeoff_status_s::TAKEOFF_STATE_FLIGHT) { + _takeoff_liftoff_z = _position(2); + _position_smoothing.forceSetPosition({_position(0), _position(1), NAN}); + } + + if (!PX4_ISFINITE(_takeoff_liftoff_z) + || (_takeoff_liftoff_z - _position(2)) < kTakeoffClimbPriorityHeight) { + _position_smoothing.forceSetVelocity({_velocity(0), _velocity(1), NAN}); + } } PositionSmoothing::PositionSmoothingSetpoints smoothed_setpoints; @@ -730,12 +739,6 @@ bool FlightTaskAuto::isTargetModified() const return xy_modified || z_modified; } -bool FlightTaskAuto::_inTakeoffRamp() const -{ - return (_type == WaypointType::takeoff) - && (_takeoff_status_sub.get().takeoff_state < takeoff_status_s::TAKEOFF_STATE_FLIGHT); -} - void FlightTaskAuto::_updateTrajConstraints() { // update params of the position smoothing diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp index 2c2bf8e6785..15190b9dd9a 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp @@ -107,7 +107,6 @@ protected: bool _generateHeadingAlongTraj(); /**< Generates heading along trajectory. */ bool isTargetModified() const; void _updateTrajConstraints(); - bool _inTakeoffRamp() const; /**< taking off and not yet at TAKEOFF_STATE_FLIGHT */ void rcHelpModifyYaw(float &yaw_sp); @@ -183,6 +182,7 @@ protected: private: matrix::Vector2f _lock_position_xy{NAN, NAN}; /**< if no valid triplet is received, lock positition to current position */ matrix::Vector2f _takeoff_locked_xy{NAN, NAN}; /**< lift-off XY tracked during the takeoff ramp and frozen at FLIGHT to keep the climb vertical */ + float _takeoff_liftoff_z{NAN}; /**< lift-off altitude (NED z), frozen at FLIGHT */ bool _yaw_lock{false}; /**< if within acceptance radius, lock yaw to current yaw */ matrix::Vector3f _triplet_previous; ///< previous waypoint in triplet from navigator