From 8641cabeecca5bdb4c2c6e204cf5a7479370e247 Mon Sep 17 00:00:00 2001 From: Thomas Stastny Date: Sat, 28 Aug 2021 19:14:48 +0200 Subject: [PATCH] npfg: fix incorrect extra multiplication of airspeed to acceleration reference --- src/lib/npfg/npfg.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/lib/npfg/npfg.cpp b/src/lib/npfg/npfg.cpp index a111e3b343..c6394f03b9 100644 --- a/src/lib/npfg/npfg.cpp +++ b/src/lib/npfg/npfg.cpp @@ -508,12 +508,12 @@ float NPFG::lateralAccel(const Vector2f &air_vel, const Vector2f &air_vel_ref, c if (dot_air_vel_err < 0.0f) { // hold max lateral acceleration command above 90 deg heading error - return p_gain_ * ((cross_air_vel_err < 0.0f) ? -airspeed *airspeed : airspeed * airspeed); + return p_gain_ * ((cross_air_vel_err < 0.0f) ? -airspeed : airspeed); } else { // airspeed/airspeed_ref is used to scale any incremented airspeed reference back to the current airspeed // for acceleration commands in a "feedback" sense (i.e. at the current vehicle airspeed) - return p_gain_ * cross_air_vel_err * airspeed / airspeed_ref_; + return p_gain_ * cross_air_vel_err / airspeed_ref_; } } // lateralAccel