diff --git a/sw/airborne/subsystems/nav.c b/sw/airborne/subsystems/nav.c index beb3279d29..53c5384702 100644 --- a/sw/airborne/subsystems/nav.c +++ b/sw/airborne/subsystems/nav.c @@ -208,8 +208,8 @@ static inline void nav_follow(uint8_t _ac_id, float _distance, float _height); static void nav_ground_speed_loop( void ) { if (MINIMUM_AIRSPEED < nav_ground_speed_setpoint && nav_ground_speed_setpoint < MAXIMUM_AIRSPEED) { - float err = estimator_hspeed_mod - nav_ground_speed_setpoint; - v_ctl_auto_throttle_cruise_throttle -= nav_ground_speed_pgain*err; + float err = nav_ground_speed_setpoint - estimator_hspeed_mod; + v_ctl_auto_throttle_cruise_throttle += nav_ground_speed_pgain*err; Bound(v_ctl_auto_throttle_cruise_throttle, V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE, V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE); } else { /* Reset cruise throttle to nominal value */