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

StickAccelerationXY: make sure speeds below 1m/s are exactly reached

Browse Source 
by only applying the sqrt linear drag when brakeing.
It was originally not done this way to avoid discontinuities and
the exact speed bewlo 1m/s didn't matter. With the position slow mode
the exact slow speeds now matter. And the discontinuities are avoided by
reusing the brake boost filter.
This commit is contained in:
Matthias Grob 2023-08-10 15:49:59 +02:00
parent f703f07399
commit df41bc3d26
1 changed files with 7 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -157,7 +157,13 @@ Vector2f StickAccelerationXY::calculateDrag(Vector2f drag_coefficient, const flo
// increase drag with squareroot function when velocity is lower than 1m/s
const Vector2f velocity_with_sqrt_boost = vel_sp.unit_or_zero() * math::sqrt_linear(vel_sp.norm());
return drag_coefficient.emult(velocity_with_sqrt_boost);
// only apply the drag increase below 1m/s when actually brakeing such that speeds below 1m/s
// are exactly reached but do so by blending it with the filter to avoid any discontinuity when switching
const float brake_scale = math::interpolate(_brake_boost_filter.getState(), 1.f, 2.f, 0.f, 1.f);
const Vector2f mixed_velocity = brake_scale * velocity_with_sqrt_boost + (1.f - brake_scale) * vel_sp;
return drag_coefficient.emult(mixed_velocity);
}
void StickAccelerationXY::applyTiltLimit(Vector2f &acceleration)