From 40c35fdddfe106af7543c5788017e316e25e50cb Mon Sep 17 00:00:00 2001
From: Gautier Hattenberger <gautier.hattenberger@enac.fr>
Date: Tue, 8 Sep 2009 15:15:11 +0000
Subject: [PATCH] set saturations in the conf file and possibility to adapt
 slower

---
 .../booz/guidance/booz2_guidance_v_adpt.h     | 34 +++++++++++++++-
 .../booz/guidance/booz2_guidance_v_ref.h      | 40 +++++++++++++------
 2 files changed, 60 insertions(+), 14 deletions(-)

diff --git a/sw/airborne/booz/guidance/booz2_guidance_v_adpt.h b/sw/airborne/booz/guidance/booz2_guidance_v_adpt.h
index a68899eef9..d3c6e59567 100644
--- a/sw/airborne/booz/guidance/booz2_guidance_v_adpt.h
+++ b/sw/airborne/booz/guidance/booz2_guidance_v_adpt.h
@@ -64,9 +64,26 @@ int32_t b2_gv_adapt_Xmeas;
 /* 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 @@ static inline void b2_gv_adapt_run(int32_t zdd_meas, int32_t thrust_applied) {
   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 @@ static inline void b2_gv_adapt_run(int32_t zdd_meas, int32_t thrust_applied) {
   /* 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 */
diff --git a/sw/airborne/booz/guidance/booz2_guidance_v_ref.h b/sw/airborne/booz/guidance/booz2_guidance_v_ref.h
index 4e1a0498c7..e9de705c8f 100644
--- a/sw/airborne/booz/guidance/booz2_guidance_v_ref.h
+++ b/sw/airborne/booz/guidance/booz2_guidance_v_ref.h
@@ -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 @@ extern int64_t b2_gv_z_ref;
 #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