[paparazzi-commits] [4628] zero attitude ref if not used

[Felix Ruess]

Revision: 4628
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4628
Author:   flixr
Date:     2010-03-05 00:27:11 +0000 (Fri, 05 Mar 2010)
Log Message:
-----------
zero attitude ref if not used

Modified Paths:
--------------
    
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_euler_float.c
    
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_euler_int.c

Modified: 
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_euler_float.c
===================================================================
--- 
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_euler_float.c
       2010-03-04 14:49:49 UTC (rev 4627)
+++ 
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_euler_float.c
       2010-03-05 00:27:11 UTC (rev 4628)
@@ -43,15 +43,15 @@
 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 @@
     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 */
 
-
 }
-
-

Modified: 
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_euler_int.c
===================================================================
--- 
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_euler_int.c
 2010-03-04 14:49:49 UTC (rev 4627)
+++ 
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_euler_int.c
 2010-03-05 00:27:11 UTC (rev 4628)
@@ -1,6 +1,6 @@
 /*
  * $Id: booz_stabilization_attitude_ref_euler_int.h -1   $
- *  
+ *
  * Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
  *
  * 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 @@
 
 #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   */
-  
-}
 
+}