From a19fac6ef81aef7fbe130c87b8717f53b9ab40c4 Mon Sep 17 00:00:00 2001 From: Mario Goebbels Date: Thu, 19 Apr 2018 23:56:51 +0200 Subject: [PATCH] actuator: Avoid zero-crossing throttle. Properly this time. --- flight/Modules/Actuator/actuator.c | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) 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; }