[paparazzi-commits] [4104] set saturations in the conf file and possibility to adapt slower

[Gautier Hattenberger]

Revision: 4104
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4104
Author:   gautier
Date:     2009-09-08 15:15:11 +0000 (Tue, 08 Sep 2009)
Log Message:
-----------
set saturations in the conf file and possibility to adapt slower

Modified Paths:
--------------
    paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_v_adpt.h
    paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_v_ref.h

Modified: paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_v_adpt.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_v_adpt.h  
2009-09-08 12:32:08 UTC (rev 4103)
+++ paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_v_adpt.h  
2009-09-08 15:15:11 UTC (rev 4104)
@@ -64,9 +64,26 @@
 /* System and Measuremement noises */
 #define B2_GV_ADAPT_SYS_NOISE_F 0.00005
 #define B2_GV_ADAPT_SYS_NOISE  BFP_OF_REAL(B2_GV_ADAPT_SYS_NOISE_F, 
B2_GV_ADAPT_P_FRAC)
-#define B2_GV_ADAPT_MEAS_NOISE_F 2.0
+
+#ifndef USE_ADAPT_HOVER
+
+#define B2_GV_ADAPT_MEAS_NOISE_F 2.0                                           
        
+#define B2_GV_ADAPT_MEAS_NOISE_HOVER 
BFP_OF_REAL(B2_GV_ADAPT_MEAS_NOISE_HOVER_F, B2_GV_ADAPT_P_FRAC)
+
+#else /* USE_ADAPT_HOVER */
+
+#define B2_GV_ADAPT_MEAS_NOISE_HOVER_F 10.0
+#define B2_GV_ADAPT_MEAS_NOISE_HOVER 
BFP_OF_REAL(B2_GV_ADAPT_MEAS_NOISE_HOVER_F, B2_GV_ADAPT_P_FRAC)
+#define B2_GV_ADAPT_MEAS_NOISE_F 50.0
 #define B2_GV_ADAPT_MEAS_NOISE BFP_OF_REAL(B2_GV_ADAPT_MEAS_NOISE_F, 
B2_GV_ADAPT_P_FRAC)
 
+#define B2_GV_ADAPT_MAX_ACCEL ACCEL_BFP_OF_REAL(4.0)
+#define B2_GV_ADAPT_HOVER_ACCEL ACCEL_BFP_OF_REAL(1.0)
+#define B2_GV_ADAPT_MAX_CMD 180
+#define B2_GV_ADAPT_MIN_CMD 20
+#define B2_GV_ADAPT_HOVER_MAX_CMD 120
+#define B2_GV_ADAPT_HOVER_MIN_CMD 60
+#endif
 
 static inline void b2_gv_adapt_init(void) {
   b2_gv_adapt_X = B2_GV_ADAPT_X0;
@@ -87,6 +104,12 @@
   b2_gv_adapt_P =  b2_gv_adapt_P + B2_GV_ADAPT_SYS_NOISE;
   /* Compute our measurement. If zdd_meas is in the range +/-5g, meas is less 
than 24 bits */
   const int32_t g_m_zdd = ((int32_t)BFP_OF_REAL(9.81, INT32_ACCEL_FRAC) - 
zdd_meas)<<(B2_GV_ADAPT_X_FRAC - INT32_ACCEL_FRAC);
+#ifdef USE_ADAPT_HOVER
+  /* Update only if accel and commands are in a valid range */
+  if (thrust_applied < B2_GV_ADAPT_MIN_CMD || thrust_applied > 
B2_GV_ADAPT_MAX_CMD
+      || zdd_meas < -B2_GV_ADAPT_MAX_ACCEL || zdd_meas > B2_GV_ADAPT_MAX_ACCEL)
+    return;
+#endif
   if ( g_m_zdd > 0)
     b2_gv_adapt_Xmeas = (g_m_zdd + (thrust_applied>>1)) / thrust_applied;
   else
@@ -94,7 +117,14 @@
   /* Compute a residual */
   int32_t residual = b2_gv_adapt_Xmeas - b2_gv_adapt_X;
   /* Covariance Error   */
-  int32_t E = b2_gv_adapt_P + B2_GV_ADAPT_MEAS_NOISE;
+  int32_t E = 0;
+#ifdef USE_ADAPT_HOVER
+  if ((thrust_applied > B2_GV_ADAPT_HOVER_MIN_CMD && thrust_applied < 
B2_GV_ADAPT_HOVER_MAX_CMD) ||
+      (zdd_meas > -B2_GV_ADAPT_HOVER_ACCEL && zdd_meas < 
B2_GV_ADAPT_HOVER_ACCEL))
+    E = b2_gv_adapt_P + B2_GV_ADAPT_MEAS_NOISE_HOVER;
+  else
+#endif
+    E = b2_gv_adapt_P + B2_GV_ADAPT_MEAS_NOISE;
   /* Kalman gain        */
   int32_t K = (b2_gv_adapt_P<<K_FRAC) / E;
   /* Update Covariance */

Modified: paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_v_ref.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_v_ref.h   
2009-09-08 12:32:08 UTC (rev 4103)
+++ paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_v_ref.h   
2009-09-08 15:15:11 UTC (rev 4104)
@@ -24,6 +24,7 @@
 #ifndef BOOZ2_GUIDANCE_V_REF_H
 #define BOOZ2_GUIDANCE_V_REF_H
 
+#include "airframe.h"
 #include "inttypes.h"
 #include "math/pprz_algebra.h"
 #include "math/pprz_algebra_int.h"
@@ -49,22 +50,37 @@
 #define B2_GV_Z_REF_FRAC (B2_GV_ZD_REF_FRAC + B2_GV_FREQ_FRAC)
 
 /* Saturations definition */
-#define B2_GV_MIN_ZDD_F (-2.0*9.81)
-#define B2_GV_MIN_ZDD BFP_OF_REAL(B2_GV_MIN_ZDD_F, B2_GV_ZDD_REF_FRAC)
-#define B2_GV_MAX_ZDD_F ( 0.8*9.81)
-#define B2_GV_MAX_ZDD BFP_OF_REAL(B2_GV_MAX_ZDD_F, B2_GV_ZDD_REF_FRAC)
-#define B2_GV_MIN_ZD_F  (-3.)
-#define B2_GV_MIN_ZD  BFP_OF_REAL(B2_GV_MIN_ZD_F , B2_GV_ZD_REF_FRAC)
-#define B2_GV_MAX_ZD_F  ( 3.)
-#define B2_GV_MAX_ZD  BFP_OF_REAL(B2_GV_MAX_ZD_F , B2_GV_ZD_REF_FRAC)
+#ifndef BOOZ2_GUIDANCE_V_REF_MIN_ZDD
+#define BOOZ2_GUIDANCE_V_REF_MIN_ZDD (-2.0*9.81)
+#endif
+#define B2_GV_MIN_ZDD BFP_OF_REAL(BOOZ2_GUIDANCE_V_REF_MIN_ZDD, 
B2_GV_ZDD_REF_FRAC)
 
+#ifndef BOOZ2_GUIDANCE_V_REF_MAX_ZDD
+#define BOOZ2_GUIDANCE_V_REF_MAX_ZDD ( 0.8*9.81)
+#endif
+#define B2_GV_MAX_ZDD BFP_OF_REAL(BOOZ2_GUIDANCE_V_REF_MAX_ZDD, 
B2_GV_ZDD_REF_FRAC)
+
+#ifndef BOOZ2_GUIDANCE_V_REF_MIN_ZD
+#define BOOZ2_GUIDANCE_V_REF_MIN_ZD (-3.)
+#endif
+#define B2_GV_MIN_ZD  BFP_OF_REAL(BOOZ2_GUIDANCE_V_REF_MIN_ZD , 
B2_GV_ZD_REF_FRAC)
+
+#ifndef BOOZ2_GUIDANCE_V_REF_MAX_ZD
+#define BOOZ2_GUIDANCE_V_REF_MAX_ZD ( 3.)
+#endif
+#define B2_GV_MAX_ZD  BFP_OF_REAL(BOOZ2_GUIDANCE_V_REF_MAX_ZD , 
B2_GV_ZD_REF_FRAC)
+
 /* second order model natural frequency and damping */
-#define B2_GV_OMEGA RadOfDeg(100.)
-#define B2_GV_ZETA  0.85
+#ifndef BOOZ2_GUIDANCE_V_REF_OMEGA
+#define BOOZ2_GUIDANCE_V_REF_OMEGA RadOfDeg(100.)
+#endif
+#ifndef BOOZ2_GUIDANCE_V_REF_ZETA
+#define BOOZ2_GUIDANCE_V_REF_ZETA  0.85
+#endif
 #define B2_GV_ZETA_OMEGA_FRAC 10
-#define B2_GV_ZETA_OMEGA BFP_OF_REAL((B2_GV_ZETA*B2_GV_OMEGA), 
B2_GV_ZETA_OMEGA_FRAC)
+#define B2_GV_ZETA_OMEGA 
BFP_OF_REAL((BOOZ2_GUIDANCE_V_REF_ZETA*BOOZ2_GUIDANCE_V_REF_OMEGA), 
B2_GV_ZETA_OMEGA_FRAC)
 #define B2_GV_OMEGA_2_FRAC 7
-#define B2_GV_OMEGA_2    BFP_OF_REAL((B2_GV_OMEGA*B2_GV_OMEGA), 
B2_GV_OMEGA_2_FRAC)
+#define B2_GV_OMEGA_2    
BFP_OF_REAL((BOOZ2_GUIDANCE_V_REF_OMEGA*BOOZ2_GUIDANCE_V_REF_OMEGA), 
B2_GV_OMEGA_2_FRAC)
 
 /* first order time constant */
 #define B2_GV_REF_THAU_F  0.25