diff --git a/flight/Modules/Actuator/actuator.c b/flight/Modules/Actuator/actuator.c index 3f4215800a..9341f92873 100644 --- a/flight/Modules/Actuator/actuator.c +++ b/flight/Modules/Actuator/actuator.c @@ -1217,12 +1217,11 @@ static void smithp_compensate(struct smith_predictor *m, float *desired_vect) region, that makes throttle eventually oscillate around the zero point under certain conditions. This leads to funny business with the motors, e.g. grinding. */ float t = desired_vect[i]; - if ((t >= THROTTLE_EPSILON && (t+v) < THROTTLE_EPSILON) || - (t <= -THROTTLE_EPSILON && (t+v) > -THROTTLE_EPSILON)) { - v = 0; - } - /* Also bound throttle. */ - desired_vect[i] = bound_sym(desired_vect[i] + v, 1.0f); + float r = t+v; + + /* Don't cross zero via prediction. Also bound throttle. */ + if (sign(t) == sign(r)) + desired_vect[i] = bound_sym(r, 1.0f); } else { desired_vect[i] += v; }