fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

fix StickAccelerationXY: avoid setpoint oscillations around 0

Browse Source 
With a higher responsiveness, after centering the stick, the velocity and
acceleration setpoints could oscillate around 0 and never reach 0, due to
discretization.
This also prevented position lock engagement.
This commit is contained in:
Beat Küng 2021-02-08 13:20:13 +01:00 committed by Matthias Grob
parent 77884bdd34
commit 76a447ed0f
1 changed files with 11 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp
View File

@ -81,12 +81,21 @@ void StickAccelerationXY::generateSetpoints(Vector2f stick_xy, const float yaw,
applyFeasibilityLimit(_acceleration, dt);
// Add drag to limit speed and brake again
_acceleration -= calculateDrag(acceleration_scale.edivide(velocity_scale), dt, stick_xy, _velocity);
Vector2f drag = calculateDrag(acceleration_scale.edivide(velocity_scale), dt, stick_xy, _velocity);
// Don't allow the drag to change the sign of the velocity, otherwise we might get into oscillations around 0, due
// to discretization
if (_acceleration.norm_squared() < FLT_EPSILON && _velocity.norm_squared() < drag.norm_squared() * dt * dt) {
drag.setZero();
_velocity.setZero();
}
_acceleration -= drag;
applyTiltLimit(_acceleration);
// Generate velocity setpoint by forward integrating commanded acceleration
_velocity += Vector2f(_acceleration) * dt;
_velocity += _acceleration * dt;
lockPosition(_velocity, pos, dt, _position);
}