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

VelocitySmoothing - Protect against division by zero and sqrt of a negative number

Browse Source
This commit is contained in:
bresch 2019-03-05 17:38:19 +01:00 committed by Beat Küng
parent e4967a9502
commit ddab0ccdf1
1 changed files with 14 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp
View File

@ -112,13 +112,18 @@ float VelocitySmoothing::computeT1(float T123, float accel_prev, float vel_prev,
float delta = T123 * T123 * max_jerk * max_jerk + 2.f * T123 * accel_prev * max_jerk - accel_prev * accel_prev
+ 4.f * max_jerk * (vel_prev - vel_setpoint);
if (delta < 0.f) {
// Solution is not real
return 0.f;
}
float sqrt_delta = sqrtf(delta);
float denominator_inv = 1.f / (2.f * a);
float T1_plus = math::max((-b + sqrt_delta) * denominator_inv, 0.f);
float T1_minus = math::max((-b - sqrt_delta) * denominator_inv, 0.f);
float T3_plus = computeT3(T1_plus, accel_prev, max_jerk);
float T3_minus = computeT3(T1_minus, accel_prev, max_jerk);
float T3_plus = accel_prev / max_jerk + T1_plus;
float T3_minus = accel_prev / max_jerk + T1_minus;
float T13_plus = T1_plus + T3_plus;
float T13_minus = T1_minus + T3_minus;
@ -147,7 +152,13 @@ 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);
float T2 = 0.f;
float den = accel_prev + max_jerk * T1;
if (math::abs_t(den) > FLT_EPSILON) {
T2 = (vel_setpoint - f) / den;
}
if (T2 < _dt) {
T2 = 0.f;