[paparazzi-commits] [4224] Split update of reference and setpoint quaternions into separate functions and only update the reference

[Allen Ibara]

Revision: 4224
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4224
Author:   aibara
Date:     2009-10-02 16:16:33 +0000 (Fri, 02 Oct 2009)
Log Message:
-----------
Split update of reference and setpoint quaternions into separate functions and 
only update the reference
when in_flight mode is off

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
    2009-10-02 16:15:42 UTC (rev 4223)
+++ 
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_quat_float.c
    2009-10-02 16:16:33 UTC (rev 4224)
@@ -78,21 +78,25 @@
     booz_stab_att_ref_accel.r = 0;
 }
 
-static void update_quats_from_euler(void) {
-    struct FloatRMat sp_rmat, ref_rmat;
+static void update_sp_quat_from_eulers(void) {
+    struct FloatRMat sp_rmat;
 
     FLOAT_RMAT_OF_EULERS_312(sp_rmat, booz_stab_att_sp_euler);
     /*    FLOAT_RMAT_OF_EULERS_321(sp_rmat, _sp);*/
     FLOAT_QUAT_OF_RMAT(booz_stab_att_sp_quat, sp_rmat);
+}
 
+static void update_ref_quat_from_eulers(void) {
+    struct FloatRMat ref_rmat;
+
     FLOAT_RMAT_OF_EULERS_312(ref_rmat, booz_stab_att_ref_euler);
     FLOAT_QUAT_OF_RMAT(booz_stab_att_ref_quat, ref_rmat);
 }
 
 void booz_stabilization_attitude_read_beta_vane(float beta)
 {
-  booz_stab_att_ref_rate.r += beta / 4;
-  update_quats_from_euler();
+  booz_stab_att_sp_euler.psi += beta / RC_UPDATE_FREQ;
+  update_sp_quat_from_eulers();
 }
 
 void booz_stabilization_attitude_read_rc(bool_t in_flight) {
@@ -105,12 +109,14 @@
         booz_stab_att_sp_euler.psi += radio_control.values[RADIO_CONTROL_YAW] 
* YAW_COEF;
         FLOAT_ANGLE_NORMALIZE(booz_stab_att_sp_euler.psi);
       }
+      update_sp_quat_from_eulers();
     } else { /* if not flying, use current yaw as setpoint */
       reset_psi_ref_from_body();
+      update_sp_quat_from_eulers();
+      update_ref_quat_from_eulers();
       booz_stab_att_ref_rate.r = 
RC_UPDATE_FREQ*radio_control.values[RADIO_CONTROL_YAW]*YAW_COEF;
     }
 
-    update_quats_from_euler();
 }
 
 
@@ -118,7 +124,8 @@
 void booz_stabilization_attitude_enter(void) {
 
   reset_psi_ref_from_body();
-  update_quats_from_euler();
+  update_sp_quat_from_eulers();
+  update_ref_quat_from_eulers();
   
   FLOAT_EULERS_ZERO( booz_stabilization_att_sum_err );
   FLOAT_QUAT_ZERO( booz_stabilization_sum_err );