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

Velocity Smoothing - Various improvements, cleanup and corner cases fixes

Browse Source
This commit is contained in:
bresch
2019-01-31 11:54:55 +01:00
parent f1262cbe84
commit 1730cd3a69
2 changed files with 31 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp
+31 -52
View File
@@ -69,15 +69,6 @@ float VelocitySmoothing::saturateT1ForAccel(float accel_prev, float max_jerk, fl
return T1_new;
}
float VelocitySmoothing::recomputeMaxJerk(float accel_prev, float max_jerk, float T1)
{
/* If T1 is smaller than dt, it means that the jerk is too large to reach the
* desired acceleration with a bang-bang signal => recompute the maximum jerk
*/
float accel_T1 = accel_prev + max_jerk * T1;
return (accel_T1 - accel_prev) / T1;
}
float VelocitySmoothing::computeT1(float accel_prev, float vel_prev, float vel_setpoint, float max_jerk)
{
float b = 2.f * accel_prev / max_jerk;
@@ -93,14 +84,22 @@ float VelocitySmoothing::computeT1(float accel_prev, float vel_prev, float vel_s
float T1_plus = (-b + sqrt_delta) * 0.5f;
float T1_minus = (-b - sqrt_delta) * 0.5f;
float T1 = math::max(math::max(T1_plus, T1_minus), 0.f);
float T3_plus = accel_prev / max_jerk + T1_plus;
float T3_minus = accel_prev / max_jerk + T1_minus;
float T1 = 0.f;
if (T1_plus >= 0.f && T3_plus >= 0.f) {
T1 = T1_plus;
} else if ( T1_minus >= 0.f && T3_minus >= 0.f) {
T1 = T1_minus;
}
T1 = saturateT1ForAccel(accel_prev, max_jerk, T1);
if ((T1 > FLT_EPSILON) &&
(T1 < _dt)) {
_max_jerk_T1 = recomputeMaxJerk(accel_prev, max_jerk, T1);
T1 = _dt;
if (T1 < _dt) {
T1 = 0.f;
}
return math::max(T1, 0.f);
@@ -135,10 +134,8 @@ float VelocitySmoothing::computeT1(float T123, float accel_prev, float vel_prev,
T1 = saturateT1ForAccel(accel_prev, max_jerk, T1);
if ((T1 > FLT_EPSILON) &&
(T1 < _dt)) {
_max_jerk_T1 = recomputeMaxJerk(accel_prev, max_jerk, T1);
T1 = _dt;
if (T1 < _dt) {
T1 = 0.f;
}
return T1;
@@ -151,6 +148,11 @@ float VelocitySmoothing::computeT2(float T1, float T3, float accel_prev, float v
float f = accel_prev * T1 + max_jerk * T1 * T1 * 0.5f + vel_prev + accel_prev * T3 + max_jerk * T1 * T3
- max_jerk * T3 * T3 * 0.5f;
float T2 = (vel_setpoint - f) / (accel_prev + max_jerk * T1);
if (T2 < _dt) {
T2 = 0.f;
}
return math::max(T2, 0.f);
}
@@ -163,6 +165,12 @@ float VelocitySmoothing::computeT2(float T123, float T1, float T3)
float VelocitySmoothing::computeT3(float T1, float accel_prev, float max_jerk)
{
float T3 = accel_prev / max_jerk + T1;
if (T1 < FLT_EPSILON && T3 < _dt && T3 > 0.f) {
T3 = _dt;
_max_jerk_T1 = accel_prev / T3;
}
return math::max(T3, 0.f);
}
@@ -171,31 +179,15 @@ void VelocitySmoothing::integrateT(float dt, float jerk, float accel_prev, float
{
accel_out = jerk * dt + accel_prev;
// Paranoid check, should never be outside the saturations
if (accel_out > _max_accel) {
accel_out = _max_accel;
} else if (accel_out < -_max_accel) {
accel_out = -_max_accel;
}
vel_out = dt * 0.5f * (accel_out + accel_prev) + vel_prev;
// Paranoid check, should never be outside the saturations
if (vel_out > _max_vel) {
vel_out = _max_vel;
} else if (vel_out < -_max_vel) {
vel_out = -_max_vel;
}
pos_out = dt / 3.f * (vel_out + accel_prev * dt * 0.5f + 2.f * vel_prev) + _pos;
}
void VelocitySmoothing::updateDurations(float dt, float vel_setpoint)
{
_vel_sp = vel_setpoint;
_dt = dt;
_vel_sp = math::constrain(vel_setpoint, -_max_vel, _max_vel);
_dt = math::max(dt, FLT_EPSILON);
updateDurations();
}
@@ -214,18 +206,9 @@ void VelocitySmoothing::updateDurations(float T123)
T1 = computeT1(_accel, _vel, _vel_sp, _max_jerk_T1);
}
/* Force T1/2/3 to zero if smaller than an epoch to avoid chattering */
if (T1 < _dt) {
T1 = 0.f;
}
// compute decreasing acceleration time
T3 = computeT3(T1, _accel, _max_jerk_T1);
if (T3 < _dt) {
T3 = 0.f;
}
// compute constant acceleration time
if (PX4_ISFINITE(T123)) {
T2 = computeT2(T123, T1, T3);
@@ -234,10 +217,6 @@ void VelocitySmoothing::updateDurations(float T123)
T2 = computeT2(T1, T3, _accel, _vel, _vel_sp, _max_jerk_T1);
}
if (T2 < _dt) {
T2 = 0.f;
}
_T1 = T1;
_T2 = T2;
_T3 = T3;
@@ -257,8 +236,8 @@ void VelocitySmoothing::integrate(float dt, float integration_scale_factor, floa
if (_T1 > FLT_EPSILON) {
_jerk = _max_jerk_T1;
if (_T1 < dt) {
// _T1 was supposed to be _dt, however, now, dt is bogger than _dt. We have to reduce the jerk to avoid an acceleration overshoot.
if (_T1 < dt && dt > _dt) {
// _T1 was supposed to be _dt, however, now, dt is bigger than _dt. We have to reduce the jerk to avoid an acceleration overshoot.
_jerk *= _dt / dt; // Keep the same area _dt * _jerk = dt * jerk_new
}
@@ -268,7 +247,7 @@ void VelocitySmoothing::integrate(float dt, float integration_scale_factor, floa
} else if (_T3 > FLT_EPSILON) {
_jerk = -_max_jerk_T1;
if (_T3 < dt) {
if (_T3 < dt && dt > _dt) {
// Same as for _T1 < dt above
_jerk *= _dt / dt;
}
src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.hpp
-1
View File
@@ -134,7 +134,6 @@ private:
*/
inline float computeT1(float T123, float accel_prev, float vel_prev, float vel_setpoint, float max_jerk);
inline float saturateT1ForAccel(float accel_prev, float max_jerk, float T1);
inline float recomputeMaxJerk(float accel_prev, float max_jerk, float T1);
/**
* Compute constant acceleration time
*/