[paparazzi-commits] [4877] Call setpoint enter sooner, and bound reference accelerations for booz attitude ref quat float

[Allen Ibara]

Revision: 4877
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4877
Author:   aibara
Date:     2010-04-24 03:58:15 +0000 (Sat, 24 Apr 2010)
Log Message:
-----------
Call setpoint enter sooner, and bound reference accelerations for booz attitude 
ref quat float

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

Modified: 
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_quat_float.c
===================================================================
--- 
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_quat_float.c
        2010-04-24 03:56:36 UTC (rev 4876)
+++ 
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_quat_float.c
        2010-04-24 03:58:15 UTC (rev 4877)
@@ -32,6 +32,10 @@
 #include "booz_stabilization_attitude_ref_float.h"
 #include "quat_setpoint.h"
 
+#define REF_ACCEL_MAX_P BOOZ_STABILIZATION_ATTITUDE_REF_MAX_PDOT
+#define REF_ACCEL_MAX_Q BOOZ_STABILIZATION_ATTITUDE_REF_MAX_QDOT
+#define REF_ACCEL_MAX_R BOOZ_STABILIZATION_ATTITUDE_REF_MAX_RDOT
+
 struct FloatEulers booz_stab_att_sp_euler;
 struct FloatQuat   booz_stab_att_sp_quat;
 struct FloatEulers booz_stab_att_ref_euler;
@@ -42,7 +46,6 @@
 struct FloatRefModel booz_stab_att_ref_model;
 
 static void reset_psi_ref_from_body(void) {
-    booz_stab_att_sp_euler.psi = booz_ahrs_float.ltp_to_body_euler.psi;
     booz_stab_att_ref_euler.psi = booz_ahrs_float.ltp_to_body_euler.psi;
     booz_stab_att_ref_rate.r = 0;
     booz_stab_att_ref_accel.r = 0;
@@ -57,6 +60,7 @@
     FLOAT_RMAT_OF_EULERS_321(ref_rmat, booz_stab_att_ref_euler);
 #endif
     FLOAT_QUAT_OF_RMAT(booz_stab_att_ref_quat, ref_rmat);
+    FLOAT_QUAT_WRAP_SHORTEST(booz_stab_att_ref_quat);
 }
 
 void booz_stabilization_attitude_ref_init(void) {
@@ -80,8 +84,8 @@
 void booz_stabilization_attitude_ref_enter()
 {
   reset_psi_ref_from_body();
+  booz_stabilization_attitude_sp_enter();
   update_ref_quat_from_eulers();
-  booz_stabilization_attitude_sp_enter();
 }
 
 /*
@@ -106,7 +110,7 @@
   struct FloatRates delta_rate;
   RATES_SMUL(delta_rate, booz_stab_att_ref_accel, DT_UPDATE);
   RATES_ADD(booz_stab_att_ref_rate, delta_rate);
-  
+
   /* compute reference angular accelerations */
   struct FloatQuat err; 
   /* compute reference attitude error        */
@@ -121,6 +125,11 @@
   booz_stab_att_ref_accel.r = 
-2.*booz_stab_att_ref_model.zeta_r*booz_stab_att_ref_model.omega_r*booz_stab_att_ref_rate.r
 
     - booz_stab_att_ref_model.omega_r*booz_stab_att_ref_model.omega_r*err.qz;
 
+  /*   saturate acceleration */
+  const struct FloatRates MIN_ACCEL = { -REF_ACCEL_MAX_P, -REF_ACCEL_MAX_Q, 
-REF_ACCEL_MAX_R };
+  const struct FloatRates 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);
+
   /* compute ref_euler */
   FLOAT_EULERS_OF_QUAT(booz_stab_att_ref_euler, booz_stab_att_ref_quat);
 }