[paparazzi-commits] [4541] Add ability to change booz attitude stabilization reference omega/ zeta via settings variables, also a few message updates

[Allen Ibara]

Revision: 4541
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4541
Author:   aibara
Date:     2010-02-05 18:11:51 +0000 (Fri, 05 Feb 2010)
Log Message:
-----------
Add ability to change booz attitude stabilization reference omega/zeta via 
settings variables, also a few message updates

Modified Paths:
--------------
    paparazzi3/trunk/conf/messages.xml
    
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude.h
    
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_quat_float.c
    
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_float.h
    
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_quat_float.c

Modified: paparazzi3/trunk/conf/messages.xml
===================================================================
--- paparazzi3/trunk/conf/messages.xml  2010-02-05 18:08:04 UTC (rev 4540)
+++ paparazzi3/trunk/conf/messages.xml  2010-02-05 18:11:51 UTC (rev 4541)
@@ -1290,14 +1290,12 @@
       <field name="motor_command" type="float"/>
     </message>
 
-    <message name="ASTRO_MOTOR" id="183">
-      <field name="fault"      type="uint16"/>
-      <field name="raw_speed"  type="uint16"/>
-      <field name="raw_dcbus"  type="uint16"/>
+    <message name="BLMC_FAULT_STATUS" id="183">
+      <field name="raw_fault"  type="uint16[]"/>
     </message>
 
     <message name="BLMC_SPEEDS" id="184">
-      <field name="speeds"     type="uint16[]"/>
+      <field name="speeds"     type="int16[]"/>
     </message>
 
     <message name="AHRS_DEBUG_QUAT" id="185">
@@ -1458,6 +1456,28 @@
       <field name="ins_ydd"   type="int32" alt_unit="m/s2" 
alt_unit_coef="0.0009766"/>
       <field name="ins_zdd"   type="int32" alt_unit="m/s2" 
alt_unit_coef="0.0009766"/>
        </message>
+
+  <message name="AFL_COEFFS" id="250">
+      <field name="roll_a"   type="float"/>
+      <field name="roll_b"  type="float"/>
+      <field name="roll_c"  type="float"/>
+      <field name="pitch_a"  type="float"/>
+      <field name="pitch_b"   type="float"/>
+      <field name="pitch_c"   type="float"/>
+      <field name="yaw_a"   type="float"/>
+      <field name="yaw_b"  type="float"/>
+      <field name="yaw_c"  type="float"/>
+  </message>
+
+  <message name="BOOZ_ATT_REF_MODEL" id="251">
+    <field name="omega_p" type="float" />
+    <field name="zeta_p" type="float" />
+    <field name="omega_q" type="float" />
+    <field name="zeta_q" type="float" />
+    <field name="omega_r" type="float" />
+    <field name="zeta_r" type="float" />
+  </message>
+
 </class>
 
 

Modified: 
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude.h
===================================================================
--- 
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude.h   
    2010-02-05 18:08:04 UTC (rev 4540)
+++ 
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude.h   
    2010-02-05 18:11:51 UTC (rev 4541)
@@ -41,6 +41,8 @@
 extern float booz_stabilization_attitude_beta_vane_gain;
 extern float booz_stabilization_attitude_alpha_vane_gain;
 
+extern float booz_stabilization_attitude_alpha_alt_dgain;
+extern float booz_stabilization_attitude_alpha_alt_zeta;
 
 #define booz_stabilization_attitude_SetKiPhi(_val) {   \
     booz_stabilization_igain.x = _val;                 \

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-02-05 18:08:04 UTC (rev 4540)
+++ 
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_quat_float.c
    2010-02-05 18:11:51 UTC (rev 4541)
@@ -27,7 +27,6 @@
 #include "math/pprz_algebra_int.h"
 #include "booz_ahrs.h"
 #include "booz_radio_control.h"
-
 #include "airframe.h"
 
 
@@ -44,7 +43,12 @@
 float booz_stabilization_attitude_beta_vane_gain;
 float booz_stabilization_attitude_alpha_vane_gain;
 
+float booz_stabilization_attitude_alpha_alt_dgain;
+float booz_stabilization_attitude_alpha_alt_zeta;
 
+static struct FloatQuat pitch_setpoint_quat = { 1., 0., 0., 0. };
+static struct FloatQuat pr_setpoint_quat = { 1., 0., 0., 0. };
+
 void booz_stabilization_attitude_init(void) {
 
   booz_stabilization_attitude_ref_init();
@@ -75,6 +79,8 @@
 #ifdef USE_VANE
   booz_stabilization_attitude_alpha_vane_gain = 
BOOZ_STABILIZATION_ATTITUDE_ALPHA_VANE_T;
   booz_stabilization_attitude_beta_vane_gain = 
BOOZ_STABILIZATION_ATTITUDE_BETA_VANE_T;
+  booz_stabilization_attitude_alpha_alt_dgain = booz_stabilization_dgain.y;
+  booz_stabilization_attitude_alpha_alt_zeta = booz_stab_att_ref_model.zeta_q;
 #endif
 
 }
@@ -95,6 +101,7 @@
   return -atan2f(dcm02, dcm22);
 }
 
+
 // complicated function to reset setpoint quaternion to pitch theta, roll phi 
using provided
 // quaternion initial rotating first about pitch, then roll (and not yaw)
 static void reset_sp_quat(float phi, float theta, struct FloatQuat *initial)
@@ -170,21 +177,26 @@
   FLOAT_QUAT_COMP(booz_stab_att_sp_quat, prev_sp_quat, sticks_quat);
 }
 
+static void booz_stabilization_attitude_new_setpoint(void)
+{
+      // add integrated stick commands to existing setpoint
+      FLOAT_QUAT_COMP(booz_stab_att_sp_quat, pr_setpoint_quat, 
pitch_setpoint_quat);
+}
+
+// when doing closed-loop angle of attack control, the pitch
+// setpoint is not based on a rate stick but on the AoA error.  
 void booz_stabilization_attitude_read_alpha_vane(float alpha)
 {
-  struct FloatEulers sticks_eulers;
-  struct FloatQuat sticks_quat, prev_sp_quat;
-  sticks_eulers.phi = 0;
-  sticks_eulers.theta = booz_stabilization_attitude_alpha_vane_gain * alpha / 
RC_UPDATE_FREQ;
-  sticks_eulers.psi = 0;
-
-  // convert eulers to quaternion
-  FLOAT_QUAT_OF_EULERS(sticks_quat, sticks_eulers);
-  FLOAT_QUAT_COPY(prev_sp_quat, booz_stab_att_sp_quat)
+  // argument alpha is error between measurement and setpoint, I believe
+  struct FloatVect3 y_axis = { 0, 1, 0 };
   
-  // rotate previous setpoint by commanded rotation
-  FLOAT_QUAT_COMP(booz_stab_att_sp_quat, prev_sp_quat, sticks_quat);
+  // make new quaternion of alpha deviation added to current theta angle
+  FLOAT_QUAT_OF_AXIS_ANGLE(pitch_setpoint_quat, y_axis, alpha + 
booz_ahrs_float.ltp_to_body_euler.theta); 
+  
+  // make new trajectory setpoint
+  booz_stabilization_attitude_new_setpoint();
 }
+
 #endif
 
 void booz_stabilization_attitude_read_rc(bool_t in_flight) {
@@ -195,21 +207,79 @@
   pprz_t pitch = radio_control.values[RADIO_CONTROL_PITCH];
   pprz_t yaw = radio_control.values[RADIO_CONTROL_YAW];
   struct FloatEulers sticks_eulers;
-  struct FloatQuat sticks_quat, prev_sp_quat;
+  struct FloatQuat sticks_quat, prev_sp_quat, setpoint_quat_old;
 
+  // convert sticks to commanded rates
+  sticks_eulers.phi = APPLY_DEADBAND(roll, 
BOOZ_STABILIZATION_ATTITUDE_DEADBAND_A) * ROLL_COEF_RATE / RC_UPDATE_FREQ;
+  sticks_eulers.psi = APPLY_DEADBAND(yaw, 
BOOZ_STABILIZATION_ATTITUDE_DEADBAND_R) * YAW_COEF_H / RC_UPDATE_FREQ;
+  sticks_eulers.theta = APPLY_DEADBAND(pitch, 
BOOZ_STABILIZATION_ATTITUDE_DEADBAND_E) * PITCH_COEF_RATE / RC_UPDATE_FREQ;
+
   // RC stick commands rate or position?
   if (rate_stick_mode) {
-    // convert sticks to commanded rates
-    sticks_eulers.phi = APPLY_DEADBAND(roll, 
BOOZ_STABILIZATION_ATTITUDE_DEADBAND_A) * ROLL_COEF_RATE / RC_UPDATE_FREQ;
-    sticks_eulers.theta = APPLY_DEADBAND(pitch, 
BOOZ_STABILIZATION_ATTITUDE_DEADBAND_E) * PITCH_COEF_RATE / RC_UPDATE_FREQ;
-    sticks_eulers.psi = APPLY_DEADBAND(yaw, 
BOOZ_STABILIZATION_ATTITUDE_DEADBAND_R) * YAW_COEF_H / RC_UPDATE_FREQ;
+#ifdef USE_VANE
+    struct FloatRMat pitch_rmat, ltp_to_body_rmat, phi_psi_rmat;
 
+    static int vane_transition = 0;
+    static float d_gain_y = 0;
+    static float zeta_y = 0;
+
+    // struct FloatVect3 x_axis = { 1, 0, 0 };
+    struct FloatVect3 y_axis = { 0, 1, 0 };
+    // struct FloatVect3 z_axis = { 0, 0, 1 };
+    
+    // is vane engaged?        
+    if (radio_control.values[RADIO_CONTROL_AUX4] < 0) {
+      sticks_eulers.theta = 0;
+
+      // generate new rotation based on current stick commands
+      FLOAT_QUAT_OF_EULERS(sticks_quat, sticks_eulers);
+
+      // if first time on the vane, set setpoint to existing roll and yaw 
angles
+      if (vane_transition == 0) {
+       // convert current body quat and current pitch rotation to DCMs
+       FLOAT_RMAT_OF_QUAT(ltp_to_body_rmat, booz_ahrs_float.ltp_to_body_quat);
+       FLOAT_RMAT_OF_AXIS_ANGLE(pitch_rmat, y_axis, 
-get_pitch_rotation_angle(&ltp_to_body_rmat));
+
+       // subtract pitch rotation from current body DCM
+       FLOAT_RMAT_COMP(phi_psi_rmat, ltp_to_body_rmat, pitch_rmat);
+       
+       // new phi & psi setpoints come from body DCM minus pitch
+       FLOAT_QUAT_OF_RMAT(pr_setpoint_quat, phi_psi_rmat);
+       vane_transition = 1;
+
+       // swap in new D gain
+       d_gain_y = booz_stabilization_dgain.y;
+       booz_stabilization_dgain.y = 
booz_stabilization_attitude_alpha_alt_dgain;
+
+       // swap in new reference zeta (damping)
+       zeta_y = booz_stab_att_ref_model.zeta_q;
+       booz_stab_att_ref_model.zeta_q = 
booz_stabilization_attitude_alpha_alt_zeta;
+      }
+
+      // integrate stick commands in phi and psi
+      FLOAT_QUAT_COPY(setpoint_quat_old, pr_setpoint_quat);
+      FLOAT_QUAT_COMP(pr_setpoint_quat, sticks_quat, setpoint_quat_old);
+      
+      // make new trajectory setpoint
+      booz_stabilization_attitude_new_setpoint();
+    } else {
+      if (vane_transition == 1) {
+       // just switched out of vane mode
+       booz_stabilization_dgain.y = d_gain_y;
+       booz_stab_att_ref_model.zeta_q = zeta_y; 
+      }
+#endif // USE_VANE
     // convert eulers to quaternion
     FLOAT_QUAT_OF_EULERS(sticks_quat, sticks_eulers);
-    FLOAT_QUAT_COPY(prev_sp_quat, booz_stab_att_sp_quat)
+    FLOAT_QUAT_COPY(prev_sp_quat, booz_stab_att_sp_quat);
   
     // rotate previous setpoint by commanded rotation
     FLOAT_QUAT_COMP(booz_stab_att_sp_quat, prev_sp_quat, sticks_quat);
+#ifdef USE_VANE
+    vane_transition = 0;
+    }
+#endif // USE_VANE
+
   } else {
     // First time switching from rate to position reset the setpoint based on 
the body
     if (last_rate_stick_mode) {

Modified: 
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_float.h
===================================================================
--- 
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_float.h
     2010-02-05 18:08:04 UTC (rev 4540)
+++ 
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_float.h
     2010-02-05 18:11:51 UTC (rev 4541)
@@ -30,5 +30,16 @@
 extern struct FloatRates  booz_stab_att_ref_rate;
 extern struct FloatRates  booz_stab_att_ref_accel;
 
+struct FloatRefModel {
+  float omega_p;
+  float zeta_p;
+  float omega_q;
+  float zeta_q;
+  float omega_r;
+  float zeta_r;
+};
+
+extern struct FloatRefModel booz_stab_att_ref_model;
+
 #endif /* BOOZ_STABILISATION_ATTITUDE_REF_FLOAT_H */
 

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-02-05 18:08:04 UTC (rev 4540)
+++ 
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_quat_float.c
        2010-02-05 18:11:51 UTC (rev 4541)
@@ -23,6 +23,7 @@
 
 #include "booz_stabilization.h"
 
+#include "booz_stabilization_attitude_ref_float.h"
 
 struct FloatEulers booz_stab_att_sp_euler;
 struct FloatQuat   booz_stab_att_sp_quat;
@@ -31,6 +32,7 @@
 struct FloatRates  booz_stab_att_ref_rate;
 struct FloatRates  booz_stab_att_ref_accel;
 
+struct FloatRefModel booz_stab_att_ref_model;
 
 void booz_stabilization_attitude_ref_init(void) {
 
@@ -41,6 +43,13 @@
   FLOAT_RATES_ZERO( booz_stab_att_ref_rate);
   FLOAT_RATES_ZERO( booz_stab_att_ref_accel);
 
+  booz_stab_att_ref_model.omega_p = BOOZ_STABILIZATION_ATTITUDE_REF_OMEGA_P;
+  booz_stab_att_ref_model.zeta_p = BOOZ_STABILIZATION_ATTITUDE_REF_ZETA_P;
+  booz_stab_att_ref_model.omega_q = BOOZ_STABILIZATION_ATTITUDE_REF_OMEGA_Q;
+  booz_stab_att_ref_model.zeta_q = BOOZ_STABILIZATION_ATTITUDE_REF_ZETA_Q;
+  booz_stab_att_ref_model.omega_r = BOOZ_STABILIZATION_ATTITUDE_REF_OMEGA_R;
+  booz_stab_att_ref_model.zeta_r = BOOZ_STABILIZATION_ATTITUDE_REF_ZETA_R;
+
 }
 
 /*
@@ -52,13 +61,6 @@
 #define DT_UPDATE (1./512.)
 #endif
 
-#define OMEGA_P   BOOZ_STABILIZATION_ATTITUDE_REF_OMEGA_P
-#define ZETA_P    BOOZ_STABILIZATION_ATTITUDE_REF_ZETA_P
-#define OMEGA_Q   BOOZ_STABILIZATION_ATTITUDE_REF_OMEGA_Q
-#define ZETA_Q    BOOZ_STABILIZATION_ATTITUDE_REF_ZETA_Q
-#define OMEGA_R   BOOZ_STABILIZATION_ATTITUDE_REF_OMEGA_R
-#define ZETA_R    BOOZ_STABILIZATION_ATTITUDE_REF_ZETA_R
-
 void booz_stabilization_attitude_ref_update() {
 
   /* integrate reference attitude            */
@@ -80,9 +82,12 @@
   /* wrap it in the shortest direction       */
   FLOAT_QUAT_WRAP_SHORTEST(err);
   /* 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;
+  booz_stab_att_ref_accel.p = 
-2.*booz_stab_att_ref_model.zeta_p*booz_stab_att_ref_model.omega_p*booz_stab_att_ref_rate.p
 
+    - booz_stab_att_ref_model.omega_p*booz_stab_att_ref_model.omega_p*err.qx;
+  booz_stab_att_ref_accel.q = 
-2.*booz_stab_att_ref_model.zeta_q*booz_stab_att_ref_model.omega_q*booz_stab_att_ref_rate.q
 
+    - booz_stab_att_ref_model.omega_q*booz_stab_att_ref_model.omega_q*err.qy;
+  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;
 
   /* compute ref_euler */
   FLOAT_EULERS_OF_QUAT(booz_stab_att_ref_euler, booz_stab_att_ref_quat);