UAV-develop/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-07-24 15:27:41 +08:00
Code Issues Packages Projects Releases Wiki Activity

fix(flight_mode_manager): keep auto-takeoff vertical on a moving deck

Browse Source 
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 <gennaroguido2002@gmail.com>
This commit is contained in:
gguidone
2026-06-18 13:29:21 +02:00
committed by Ramon Roche
parent 76fdc0f134
commit 671f60e6da
2 changed files with 17 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

29
src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
View File

@@ -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

2
src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp
View File

@@ -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