diff --git a/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_euler_float.c b/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_euler_float.c index 0c90370c9b..02c82e7faf 100644 --- a/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_euler_float.c +++ b/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_euler_float.c @@ -43,15 +43,15 @@ void booz_stabilization_attitude_ref_init(void) { void booz_stabilization_attitude_ref_update() { #ifdef USE_REF - - /* dumb integrate reference attitude */ + + /* dumb integrate reference attitude */ struct FloatRates delta_rate; RATES_SMUL(delta_rate, booz_stab_att_ref_rate, DT_UPDATE); struct FloatEulers delta_angle; EULERS_ASSIGN(delta_angle, delta_rate.p, delta_rate.q, delta_rate.r); EULERS_ADD(booz_stab_att_ref_euler, delta_angle ); FLOAT_ANGLE_NORMALIZE(booz_stab_att_ref_euler.psi); - + /* integrate reference rotational speeds */ struct FloatRates delta_accel; RATES_SMUL(delta_accel, booz_stab_att_ref_accel, DT_UPDATE); @@ -68,21 +68,18 @@ void booz_stabilization_attitude_ref_update() { booz_stab_att_ref_accel.q = -2.*ZETA_Q*OMEGA_P*booz_stab_att_ref_rate.q - OMEGA_Q*OMEGA_Q*ref_err.theta; booz_stab_att_ref_accel.r = -2.*ZETA_R*OMEGA_P*booz_stab_att_ref_rate.r - OMEGA_R*OMEGA_R*ref_err.psi; - /* saturate acceleration */ + /* saturate acceleration */ const struct Int32Rates MIN_ACCEL = { -REF_ACCEL_MAX_P, -REF_ACCEL_MAX_Q, -REF_ACCEL_MAX_R }; const struct Int32Rates MAX_ACCEL = { REF_ACCEL_MAX_P, REF_ACCEL_MAX_Q, REF_ACCEL_MAX_R }; \ RATES_BOUND_BOX(booz_stab_att_ref_accel, MIN_ACCEL, MAX_ACCEL); /* saturate speed and trim accel accordingly */ SATURATE_SPEED_TRIM_ACCEL(); - + #else /* !USE_REF */ - EULERS_COPY(booz_stabilization_att_ref, booz_stabilization_att_sp); - FLOAT_RATES_ZERO(booz_stabilization_rate_ref); - FLOAT_RATES_ZERO(booz_stabilization_accel_ref); + EULERS_COPY(booz_stab_att_ref_euler, booz_stabilization_att_sp); + FLOAT_RATES_ZERO(booz_stab_att_ref_rate); + FLOAT_RATES_ZERO(booz_stab_att_ref_accel); #endif /* USE_REF */ - } - - diff --git a/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_euler_int.c b/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_euler_int.c index c4b0d9daa8..8f7abc18cf 100644 --- a/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_euler_int.c +++ b/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_euler_int.c @@ -1,6 +1,6 @@ /* * $Id: booz_stabilization_attitude_ref_euler_int.h -1 $ - * + * * Copyright (C) 2008-2009 Antoine Drouin * * This file is part of paparazzi. @@ -18,7 +18,7 @@ * You should have received a copy of the GNU General Public License * along with paparazzi; see the file COPYING. If not, write to * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. + * Boston, MA 02111-1307, USA. */ #include "booz_stabilization.h" @@ -75,56 +75,55 @@ void booz_stabilization_attitude_ref_update() { #ifdef USE_REF - /* dumb integrate reference attitude */ - const struct Int32Eulers d_angle = { + /* dumb integrate reference attitude */ + const struct Int32Eulers d_angle = { booz_stab_att_ref_rate.p >> ( F_UPDATE_RES + REF_RATE_FRAC - REF_ANGLE_FRAC), booz_stab_att_ref_rate.q >> ( F_UPDATE_RES + REF_RATE_FRAC - REF_ANGLE_FRAC), booz_stab_att_ref_rate.r >> ( F_UPDATE_RES + REF_RATE_FRAC - REF_ANGLE_FRAC)}; EULERS_ADD(booz_stab_att_ref_euler, d_angle ); ANGLE_REF_NORMALIZE(booz_stab_att_ref_euler.psi); - - /* integrate reference rotational speeds */ - const struct Int32Rates d_rate = { + + /* integrate reference rotational speeds */ + const struct Int32Rates d_rate = { booz_stab_att_ref_accel.p >> ( F_UPDATE_RES + REF_ACCEL_FRAC - REF_RATE_FRAC), booz_stab_att_ref_accel.q >> ( F_UPDATE_RES + REF_ACCEL_FRAC - REF_RATE_FRAC), booz_stab_att_ref_accel.r >> ( F_UPDATE_RES + REF_ACCEL_FRAC - REF_RATE_FRAC)}; RATES_ADD(booz_stab_att_ref_rate, d_rate); - - /* compute reference attitude error */ - struct Int32Eulers ref_err; + + /* compute reference attitude error */ + struct Int32Eulers ref_err; EULERS_DIFF(ref_err, booz_stab_att_ref_euler, booz_stab_att_sp_euler); - /* wrap it in the shortest direction */ - ANGLE_REF_NORMALIZE(ref_err.psi); - - /* compute reference angular accelerations */ - const struct Int32Rates accel_rate = { + /* wrap it in the shortest direction */ + ANGLE_REF_NORMALIZE(ref_err.psi); + + /* compute reference angular accelerations */ + const struct Int32Rates accel_rate = { ((int32_t)(-2.*ZETA_OMEGA_P) * (booz_stab_att_ref_rate.p >> (REF_RATE_FRAC - REF_ACCEL_FRAC))) - >> (ZETA_OMEGA_P_RES), + >> (ZETA_OMEGA_P_RES), ((int32_t)(-2.*ZETA_OMEGA_Q) * (booz_stab_att_ref_rate.q >> (REF_RATE_FRAC - REF_ACCEL_FRAC))) - >> (ZETA_OMEGA_Q_RES), + >> (ZETA_OMEGA_Q_RES), ((int32_t)(-2.*ZETA_OMEGA_R) * (booz_stab_att_ref_rate.r >> (REF_RATE_FRAC - REF_ACCEL_FRAC))) - >> (ZETA_OMEGA_R_RES) }; - - const struct Int32Rates accel_angle = { + >> (ZETA_OMEGA_R_RES) }; + + const struct Int32Rates accel_angle = { ((int32_t)(-OMEGA_2_P)* (ref_err.phi >> (REF_ANGLE_FRAC - REF_ACCEL_FRAC))) >> (OMEGA_2_P_RES), ((int32_t)(-OMEGA_2_Q)* (ref_err.theta >> (REF_ANGLE_FRAC - REF_ACCEL_FRAC))) >> (OMEGA_2_Q_RES), ((int32_t)(-OMEGA_2_R)* (ref_err.psi >> (REF_ANGLE_FRAC - REF_ACCEL_FRAC))) >> (OMEGA_2_R_RES) }; - + RATES_SUM(booz_stab_att_ref_accel, accel_rate, accel_angle); - - /* saturate acceleration */ + + /* saturate acceleration */ const struct Int32Rates MIN_ACCEL = { -REF_ACCEL_MAX_P, -REF_ACCEL_MAX_Q, -REF_ACCEL_MAX_R }; const struct Int32Rates MAX_ACCEL = { REF_ACCEL_MAX_P, REF_ACCEL_MAX_Q, REF_ACCEL_MAX_R }; RATES_BOUND_BOX(booz_stab_att_ref_accel, MIN_ACCEL, MAX_ACCEL); - + /* saturate speed and trim accel accordingly */ SATURATE_SPEED_TRIM_ACCEL(); #else /* !USE_REF */ - EULERS_COPY(booz_stabilization_att_ref, booz_stabilization_att_sp); - INT_RATES_ZERO(booz_stabilization_rate_ref); - INT_RATES_ZERO(booz_stabilization_accel_ref); + EULERS_COPY(booz_stab_att_ref_euler, booz_stabilization_att_sp); + INT_RATES_ZERO(booz_stab_att_ref_rate); + INT_RATES_ZERO(booz_stab_att_ref_accel); #endif /* USE_REF */ - -} +}