StickAccelerationXY: apply jerk limit after drag

This commit is contained in:
Matthias Grob
2023-01-11 14:50:32 +01:00
committed by Mathieu Bresciani
parent 2a315f86ca
commit a7c03de434
@@ -14,7 +14,7 @@
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without spec{fic prior written permission.
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
@@ -92,7 +92,6 @@ void StickAccelerationXY::generateSetpoints(Vector2f stick_xy, const float yaw,
Sticks::rotateIntoHeadingFrameXY(stick_xy, yaw, yaw_sp);
_acceleration_setpoint = stick_xy.emult(acceleration_scale);
applyJerkLimit(dt);
// Add drag to limit speed and brake again
Vector2f drag = calculateDrag(acceleration_scale.edivide(velocity_scale), dt, stick_xy, _velocity_setpoint);
@@ -110,6 +109,7 @@ void StickAccelerationXY::generateSetpoints(Vector2f stick_xy, const float yaw,
_acceleration_setpoint -= drag;
applyJerkLimit(dt);
applyTiltLimit(_acceleration_setpoint);
// Generate velocity setpoint by forward integrating commanded acceleration