Few more cleanups for quat int

paparazzi · May 25, 2011 · cafd7b8 · cafd7b8
1 parent 6c48298
commit cafd7b8
Showing 1 changed file with 4 additions and 7 deletions.
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c
@@ -155,9 +155,6 @@ void stabilization_attitude_ref_update() {
   QUAT_ADD(stab_att_ref_quat, qdot);
   INT32_QUAT_NORMALIZE(stab_att_ref_quat);
 
-  /* integrate reference rotational speeds   */
-  //struct Int32Rates delta_rate;
-
   /* integrate reference rotational speeds   */
   const struct Int32Rates delta_rate = {
          stab_att_ref_accel.p >> ( F_UPDATE_RES + REF_ACCEL_FRAC - REF_RATE_FRAC),
@@ -181,10 +178,10 @@ void stabilization_attitude_ref_update() {
     ((int32_t)(-2.*ZETA_OMEGA_Q) * (stab_att_ref_rate.q >> (REF_RATE_FRAC - REF_ACCEL_FRAC))) >> (ZETA_OMEGA_Q_RES),
     ((int32_t)(-2.*ZETA_OMEGA_R) * (stab_att_ref_rate.r >> (REF_RATE_FRAC - REF_ACCEL_FRAC))) >> (ZETA_OMEGA_R_RES) };
 
-   const struct Int32Rates accel_angle = {
-     ((int32_t)(-OMEGA_2_P)* (err.qx   >> (REF_ANGLE_FRAC - REF_ACCEL_FRAC))) >> (OMEGA_2_P_RES),
-     ((int32_t)(-OMEGA_2_Q)* (err.qy   >> (REF_ANGLE_FRAC - REF_ACCEL_FRAC))) >> (OMEGA_2_Q_RES),
-     ((int32_t)(-OMEGA_2_R)* (err.qz   >> (REF_ANGLE_FRAC - REF_ACCEL_FRAC))) >> (OMEGA_2_R_RES) };
+  const struct Int32Rates accel_angle = {
+    ((int32_t)(-OMEGA_2_P)* (err.qx   >> (REF_ANGLE_FRAC - REF_ACCEL_FRAC))) >> (OMEGA_2_P_RES),
+    ((int32_t)(-OMEGA_2_Q)* (err.qy   >> (REF_ANGLE_FRAC - REF_ACCEL_FRAC))) >> (OMEGA_2_Q_RES),
+    ((int32_t)(-OMEGA_2_R)* (err.qz   >> (REF_ANGLE_FRAC - REF_ACCEL_FRAC))) >> (OMEGA_2_R_RES) };
 
   RATES_SUM(stab_att_ref_accel, accel_rate, accel_angle);