GUIDANCE_V_GAIN_SCALER Caused overflow in vertical d and i gain

paparazzi · Apr 10, 2012 · f2a901b · f2a901b
1 parent bd88929
commit f2a901b
Showing 1 changed file with 4 additions and 4 deletions.
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
@@ -284,11 +284,11 @@ __attribute__ ((always_inline)) static inline void run_hover_loop(bool_t in_flig
 
   /* our error feed back command                   */
   /* z-axis pointing down -> positive error means we need less thrust */
-  guidance_v_fb_cmd = ((-guidance_v_kp * GUIDANCE_V_GAIN_SCALER * err_z)  >> 12) +
-                      ((-guidance_v_kd * GUIDANCE_V_GAIN_SCALER * err_zd) >> 21) +
-                      ((-guidance_v_ki * GUIDANCE_V_GAIN_SCALER * guidance_v_z_sum_err) >> 21);
+  guidance_v_fb_cmd = ((-guidance_v_kp * err_z)  >> 12) +
+                      ((-guidance_v_kd * err_zd) >> 21) +
+                      ((-guidance_v_ki * guidance_v_z_sum_err) >> 21);
 
-  guidance_v_delta_t = guidance_v_ff_cmd + guidance_v_fb_cmd;
+  guidance_v_delta_t = guidance_v_ff_cmd + (guidance_v_fb_cmd * GUIDANCE_V_GAIN_SCALER);
 
   /* bound the result */
   Bound(guidance_v_delta_t, 0, MAX_PPRZ);