[paparazzi-commits] [4300] Implement rate-mode pitch/roll sticks for ref_quat_float ( instead of postition setpoints).

[Allen Ibara]

Revision: 4300
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4300
Author:   aibara
Date:     2009-10-29 21:25:18 +0000 (Thu, 29 Oct 2009)
Log Message:
-----------
Implement rate-mode pitch/roll sticks for ref_quat_float (instead of postition 
setpoints). Requires BOOZ_STABILIZATION_ATTITUDE_DEADBAND_A/E and conditionally 
enabled by STICKS_PITCH_RATE / STICKS_ROLL_RATE #defines. Also make 
booz_stabilization_attitude_ref_update take in_flight as an argument and skip 
updating yaw reference acceleration when not in_flight.

Modified Paths:
--------------
    
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude.h
    
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_euler_float.c
    
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_euler_int.c
    
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_quat_float.c
    
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
    
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.h

Modified: 
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude.h
===================================================================
--- 
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude.h   
    2009-10-27 18:02:24 UTC (rev 4299)
+++ 
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude.h   
    2009-10-29 21:25:18 UTC (rev 4300)
@@ -35,7 +35,7 @@
 #include "stabilization/booz_stabilization_attitude_ref.h"
 #include STABILISATION_ATTITUDE_REF_H
 extern void booz_stabilization_attitude_ref_init(void);
-extern void booz_stabilization_attitude_ref_update(void);
+extern void booz_stabilization_attitude_ref_update(bool_t in_flight);
 
 
 #define booz_stabilization_attitude_SetKiPhi(_val) {   \

Modified: 
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_euler_float.c
===================================================================
--- 
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_euler_float.c
   2009-10-27 18:02:24 UTC (rev 4299)
+++ 
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_euler_float.c
   2009-10-29 21:25:18 UTC (rev 4300)
@@ -88,7 +88,7 @@
 
 void booz_stabilization_attitude_run(bool_t  in_flight) {
 
-  booz_stabilization_attitude_ref_update();
+  booz_stabilization_attitude_ref_update(in_flight);
 
   /* Compute feedforward */
   booz_stabilization_att_ff_cmd[COMMAND_ROLL] = 

Modified: 
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_euler_int.c
===================================================================
--- 
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_euler_int.c
     2009-10-27 18:02:24 UTC (rev 4299)
+++ 
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_euler_int.c
     2009-10-29 21:25:18 UTC (rev 4300)
@@ -93,7 +93,7 @@
 
 
   /* update reference */
-  booz_stabilization_attitude_ref_update();
+  booz_stabilization_attitude_ref_update(in_flight);
 
   /* compute feedforward command */
   booz_stabilization_att_ff_cmd[COMMAND_ROLL] = 

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-27 18:02:24 UTC (rev 4299)
+++ 
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_quat_float.c
    2009-10-29 21:25:18 UTC (rev 4300)
@@ -74,22 +74,28 @@
 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;
 }
 
 static void update_sp_quat_from_eulers(void) {
     struct FloatRMat sp_rmat;
 
+#ifdef STICKS_RMAT312
     FLOAT_RMAT_OF_EULERS_312(sp_rmat, booz_stab_att_sp_euler);
-    /*    FLOAT_RMAT_OF_EULERS_321(sp_rmat, _sp);*/
+#else
+    FLOAT_RMAT_OF_EULERS_321(sp_rmat, booz_stab_att_sp_euler);
+#endif
     FLOAT_QUAT_OF_RMAT(booz_stab_att_sp_quat, sp_rmat);
 }
 
 static void update_ref_quat_from_eulers(void) {
     struct FloatRMat ref_rmat;
 
+#ifdef STICKS_RMAT312
     FLOAT_RMAT_OF_EULERS_312(ref_rmat, booz_stab_att_ref_euler);
+#else
+    FLOAT_RMAT_OF_EULERS_321(ref_rmat, booz_stab_att_ref_euler);
+#endif
     FLOAT_QUAT_OF_RMAT(booz_stab_att_ref_quat, ref_rmat);
 }
 
@@ -101,20 +107,36 @@
 
 void booz_stabilization_attitude_read_rc(bool_t in_flight) {
 
-    booz_stab_att_sp_euler.phi   = radio_control.values[RADIO_CONTROL_ROLL]  * 
ROLL_COEF;
-    booz_stab_att_sp_euler.theta = radio_control.values[RADIO_CONTROL_PITCH] * 
PITCH_COEF;
 
     if (in_flight) {
+#ifdef STICKS_ROLL_RATE
+      if (ROLL_DEADBAND_EXCEEDED()) {
+        booz_stab_att_sp_euler.phi += radio_control.values[RADIO_CONTROL_ROLL] 
* ROLL_COEF / RC_UPDATE_FREQ;
+        //FLOAT_ANGLE_NORMALIZE(booz_stab_att_sp_euler.phi);
+      }
+#else
+      booz_stab_att_sp_euler.phi   = radio_control.values[RADIO_CONTROL_ROLL]  
* ROLL_COEF;
+#endif
+#ifdef STICKS_PITCH_RATE
+      if (PITCH_DEADBAND_EXCEEDED()) {
+        booz_stab_att_sp_euler.theta += 
radio_control.values[RADIO_CONTROL_PITCH] * PITCH_COEF / RC_UPDATE_FREQ;
+        //FLOAT_ANGLE_NORMALIZE(booz_stab_att_sp_euler.theta);
+      }
+#else
+      booz_stab_att_sp_euler.theta = radio_control.values[RADIO_CONTROL_PITCH] 
* PITCH_COEF;
+#endif
       if (YAW_DEADBAND_EXCEEDED()) {
-        booz_stab_att_sp_euler.psi += radio_control.values[RADIO_CONTROL_YAW] 
* YAW_COEF;
+        booz_stab_att_sp_euler.psi += radio_control.values[RADIO_CONTROL_YAW] 
* YAW_COEF / RC_UPDATE_FREQ;
         FLOAT_ANGLE_NORMALIZE(booz_stab_att_sp_euler.psi);
       }
       update_sp_quat_from_eulers();
     } else { /* if not flying, use current yaw as setpoint */
+      booz_stab_att_sp_euler.phi   = radio_control.values[RADIO_CONTROL_ROLL]  
* ROLL_COEF;
+      booz_stab_att_sp_euler.theta = radio_control.values[RADIO_CONTROL_PITCH] 
* PITCH_COEF;
       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;
+      booz_stab_att_ref_rate.r = 8 * radio_control.values[RADIO_CONTROL_YAW] * 
YAW_COEF;
     }
 
 }
@@ -146,7 +168,7 @@
   /* 
    * Update reference
    */
-  booz_stabilization_attitude_ref_update();
+  booz_stabilization_attitude_ref_update(in_flight);
 
   /* 
    * Compute feedforward

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
       2009-10-27 18:02:24 UTC (rev 4299)
+++ 
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_euler_float.c
       2009-10-29 21:25:18 UTC (rev 4300)
@@ -40,7 +40,7 @@
 
 #define USE_REF 1
 
-void booz_stabilization_attitude_ref_update(void) {
+void booz_stabilization_attitude_ref_update(bool_t in_flight) {
 
 #ifdef 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
 2009-10-27 18:02:24 UTC (rev 4299)
+++ 
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_euler_int.c
 2009-10-29 21:25:18 UTC (rev 4300)
@@ -71,7 +71,7 @@
 
 #define USE_REF 1
 
-void booz_stabilization_attitude_ref_update(void) {
+void booz_stabilization_attitude_ref_update(bool_t in_flight) {
 
 #ifdef USE_REF
 

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
        2009-10-27 18:02:24 UTC (rev 4299)
+++ 
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_quat_float.c
        2009-10-29 21:25:18 UTC (rev 4300)
@@ -59,7 +59,7 @@
 #define OMEGA_R   BOOZ_STABILIZATION_ATTITUDE_REF_OMEGA_R
 #define ZETA_R    BOOZ_STABILIZATION_ATTITUDE_REF_ZETA_R
 
-void booz_stabilization_attitude_ref_update(void) {
+void booz_stabilization_attitude_ref_update(bool_t in_flight) {
 
   /* integrate reference attitude            */
   struct FloatQuat qdot;
@@ -82,8 +82,11 @@
   /* propagate the 2nd order linear model    */
   booz_stab_att_ref_accel.p = -2.*ZETA_P*OMEGA_P*booz_stab_att_ref_rate.p - 
OMEGA_P*OMEGA_P*err.qx;
   booz_stab_att_ref_accel.q = -2.*ZETA_Q*OMEGA_Q*booz_stab_att_ref_rate.q - 
OMEGA_Q*OMEGA_Q*err.qy;
-  booz_stab_att_ref_accel.r = -2.*ZETA_R*OMEGA_R*booz_stab_att_ref_rate.r - 
OMEGA_R*OMEGA_R*err.qz;
 
+  if (in_flight) {
+    booz_stab_att_ref_accel.r = -2.*ZETA_R*OMEGA_R*booz_stab_att_ref_rate.r - 
OMEGA_R*OMEGA_R*err.qz;
+  }
+
   /* compute ref_euler */
   FLOAT_EULERS_OF_QUAT(booz_stab_att_ref_euler, booz_stab_att_ref_quat);
 

Modified: 
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_quat_float.h
===================================================================
--- 
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_quat_float.h
        2009-10-27 18:02:24 UTC (rev 4299)
+++ 
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_quat_float.h
        2009-10-29 21:25:18 UTC (rev 4300)
@@ -33,8 +33,14 @@
 #define RC_UPDATE_FREQ 40.
 #define ROLL_COEF  (-BOOZ_STABILIZATION_ATTITUDE_SP_MAX_PHI   / MAX_PPRZ)
 #define PITCH_COEF ( BOOZ_STABILIZATION_ATTITUDE_SP_MAX_THETA / MAX_PPRZ)
-#define YAW_COEF   (-BOOZ_STABILIZATION_ATTITUDE_SP_MAX_R     / MAX_PPRZ / 
RC_UPDATE_FREQ)
+#define YAW_COEF   (-BOOZ_STABILIZATION_ATTITUDE_SP_MAX_R     / MAX_PPRZ)
 
+#define ROLL_DEADBAND_EXCEEDED()                                               
\
+  (radio_control.values[RADIO_CONTROL_ROLL] >  
BOOZ_STABILIZATION_ATTITUDE_DEADBAND_A || \
+   radio_control.values[RADIO_CONTROL_ROLL] < 
-BOOZ_STABILIZATION_ATTITUDE_DEADBAND_A)
+#define PITCH_DEADBAND_EXCEEDED()                                              
\
+  (radio_control.values[RADIO_CONTROL_PITCH] >  
BOOZ_STABILIZATION_ATTITUDE_DEADBAND_E || \
+   radio_control.values[RADIO_CONTROL_PITCH] < 
-BOOZ_STABILIZATION_ATTITUDE_DEADBAND_E)
 #define YAW_DEADBAND_EXCEEDED()                                                
\
   (radio_control.values[RADIO_CONTROL_YAW] >  
BOOZ_STABILIZATION_ATTITUDE_DEADBAND_R || \
    radio_control.values[RADIO_CONTROL_YAW] < 
-BOOZ_STABILIZATION_ATTITUDE_DEADBAND_R)