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

FlightTasks: initialize smoothing with current acceleration

Browse Source
This commit is contained in:
Wang Jiayue
2026-06-12 18:04:57 +08:00
committed by Ramon Roche
parent 04e656edb9
commit bfb48ad17e
3 changed files with 11 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp
View File

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

4
src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp
View File

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

6
src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp
View File

@@ -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));
}