diff --git a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c index cff5f79434..aeb7ee4c97 100644 --- a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c +++ b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c @@ -65,7 +65,10 @@ float h_ctl_pitch_loop_setpoint; float h_ctl_pitch_pgain; float h_ctl_pitch_dgain; pprz_t h_ctl_elevator_setpoint; -uint8_t h_ctl_pitch_mode; +#ifdef USE_AOA + uint8_t h_ctl_pitch_mode; +#endif + /* inner loop pre-command */ float h_ctl_aileron_of_throttle; @@ -120,6 +123,10 @@ void h_ctl_init( void ) { h_ctl_course_dgain = H_CTL_COURSE_DGAIN; h_ctl_roll_max_setpoint = H_CTL_ROLL_MAX_SETPOINT; +#ifdef USE_AOA + h_ctl_pitch_mode = 0; +#endif + h_ctl_disabled = FALSE; h_ctl_roll_setpoint = 0.; @@ -419,6 +426,8 @@ inline static void h_ctl_pitch_loop( void ) { - h_ctl_elevator_of_roll / h_ctl_pitch_pgain * fabs(estimator_phi); float err = 0; + +#ifdef USE_AOA switch (h_ctl_pitch_mode){ case H_CTL_PITCH_MODE_THETA: err = estimator_theta - h_ctl_pitch_loop_setpoint; @@ -430,6 +439,11 @@ inline static void h_ctl_pitch_loop( void ) { err = estimator_theta - h_ctl_pitch_loop_setpoint; break; } +#else //NO_AOA + err = estimator_theta - h_ctl_pitch_loop_setpoint; +#endif + + float d_err = err - last_err; last_err = err; float cmd = h_ctl_pitch_pgain * (err + h_ctl_pitch_dgain * d_err);