From bfb48ad17e1afbad2220207f51cf5fb7e3fc7cd0 Mon Sep 17 00:00:00 2001 From: Wang Jiayue Date: Fri, 12 Jun 2026 18:04:57 +0800 Subject: [PATCH] FlightTasks: initialize smoothing with current acceleration --- .../ManualAcceleration/FlightTaskManualAcceleration.cpp | 7 ++++++- .../FlightTaskManualAltitudeSmoothVel.cpp | 4 ++-- .../flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp | 6 +++--- 3 files changed, 11 insertions(+), 6 deletions(-) diff --git a/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp b/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp index f73fbe40231..4c243dfcf27 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp @@ -52,7 +52,12 @@ bool FlightTaskManualAcceleration::activate(const trajectory_setpoint_s &last_se _stick_acceleration_xy.resetVelocity(_velocity.xy()); } - _stick_acceleration_xy.resetAcceleration(Vector2f(last_setpoint.acceleration)); + if (Vector2f(last_setpoint.acceleration).isAllFinite()) { + _stick_acceleration_xy.resetAcceleration(Vector2f(last_setpoint.acceleration)); + + } else { + _stick_acceleration_xy.resetAcceleration(_acceleration.xy()); + } return ret; } diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp b/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp index 465d13b095c..45329acd3e8 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp @@ -53,8 +53,8 @@ bool FlightTaskManualAltitudeSmoothVel::activate(const trajectory_setpoint_s &la // If the velocity setpoint is unknown, set to the current velocity float vz_sp_last = PX4_ISFINITE(last_setpoint.velocity[2]) ? last_setpoint.velocity[2] : _velocity(2); - // No acceleration estimate available, set to zero if the setpoint is NAN - float az_sp_last = PX4_ISFINITE(last_setpoint.acceleration[2]) ? last_setpoint.acceleration[2] : 0.f; + // If accel setpoint unknown, set to the current accel + float az_sp_last = PX4_ISFINITE(last_setpoint.acceleration[2]) ? last_setpoint.acceleration[2] : _acceleration(2); _smoothing.reset(az_sp_last, vz_sp_last, z_sp_last); diff --git a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp index 7cacad1b910..e21a567e259 100644 --- a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp +++ b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp @@ -197,8 +197,8 @@ bool FlightTaskOrbit::activate(const trajectory_setpoint_s &last_setpoint) // If the velocity setpoint is unknown, set to the current velocity if (!PX4_ISFINITE(vel_prev(i))) { vel_prev(i) = _velocity(i); } - // No acceleration estimate available, set to zero if the setpoint is NAN - if (!PX4_ISFINITE(accel_prev(i))) { accel_prev(i) = 0.f; } + // If accel setpoint unknown, set to the current accel + if (!PX4_ISFINITE(accel_prev(i))) { accel_prev(i) = _acceleration(i); } } _position_smoothing.reset(accel_prev, vel_prev, pos_prev); @@ -219,7 +219,7 @@ bool FlightTaskOrbit::update() _in_circle_approach = false; _slew_rate_velocity.setForcedValue(0.f); // reset the slew rate when moving between orbits. FlightTaskManualAltitudeSmoothVel::_smoothing.reset( - PX4_ISFINITE(_acceleration_setpoint(2)) ? _acceleration_setpoint(2) : 0.f, + PX4_ISFINITE(_acceleration_setpoint(2)) ? _acceleration_setpoint(2) : _acceleration(2), PX4_ISFINITE(_velocity_setpoint(2)) ? _velocity_setpoint(2) : _velocity(2), PX4_ISFINITE(_position_setpoint(2)) ? _position_setpoint(2) : _position(2)); }