[paparazzi-commits] [4669] Updates to quat_float from Matt

[Allen Ibara]

Revision: 4669
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4669
Author:   aibara
Date:     2010-03-11 06:52:30 +0000 (Thu, 11 Mar 2010)
Log Message:
-----------
Updates to quat_float from Matt

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

Modified: 
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_quat_float.c
===================================================================
--- 
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_quat_float.c
    2010-03-11 04:13:32 UTC (rev 4668)
+++ 
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_quat_float.c
    2010-03-11 06:52:30 UTC (rev 4669)
@@ -52,8 +52,6 @@
 static float beta_vane_stick_cmd = 0;
 static struct FloatQuat alpha_setpoint_quat = { 1., 0., 0., 0. };
 static float alpha_error = 0;
-
-static struct FloatQuat pr_setpoint_quat = { 1., 0., 0., 0. };
 #endif // USE_VANE
 
 void booz_stabilization_attitude_init(void) {
@@ -237,7 +235,7 @@
       if (vane_transition == 0) {
 
        // new setpoint
-       FLOAT_QUAT_COPY(pr_setpoint_quat, booz_ahrs_float.ltp_to_body_quat);
+       FLOAT_QUAT_COPY(booz_stab_att_sp_quat, 
booz_ahrs_float.ltp_to_body_quat);
 
        // store old gains
        d_gain_y = booz_stabilization_dgain.y;
@@ -250,11 +248,10 @@
       booz_stabilization_pgain.y = booz_stabilization_attitude_alpha_alt_pgain;
 
       // integrate stick commands in phi and psi
-      FLOAT_QUAT_COPY(setpoint_quat_old, pr_setpoint_quat);
-      FLOAT_QUAT_COMP(pr_setpoint_quat, setpoint_quat_old, sticks_quat);
+      FLOAT_QUAT_COPY(setpoint_quat_old, booz_stab_att_sp_quat);
+      FLOAT_QUAT_COMP(booz_stab_att_sp_quat, setpoint_quat_old, sticks_quat);
       
       // make new trajectory setpoint
-      FLOAT_QUAT_COPY(booz_stab_att_sp_quat, pr_setpoint_quat);
     } else {
       if (vane_transition == 1) {
        // just switched out of vane mode
@@ -393,9 +390,10 @@
   /*  override qy in alpha mode  */
   if (rate_stick_mode) {
     if (radio_control.values[RADIO_CONTROL_AUX4] < 0) {
-      att_err.qy = alpha_error;
+      att_err.qy += alpha_error;
     }
   }
+  FLOAT_QUAT_NORMALISE(att_err);
 #endif // USE_VANE
 
   /*  PID                  */