improve vertical adaptation filter

- ADAPT_NOISE_FACTOR allows to control convergence speed
- ref_zd is used to lower confidence when asking to climb/descend
- ADAPT_HOVER flag is not needed (default and only behavior
- large values of commands or measurements are discarded
- handle correctly SUPERVISION_MIN/MAX

sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c

@@ -37,14 +37,6 @@
 
 #include "generated/airframe.h"
 
-/* In case Asctec controllers are used without supervision */
-#ifndef SUPERVISION_MIN_MOTOR
-#define SUPERVISION_MIN_MOTOR 1
-#endif
-#ifndef SUPERVISION_MAX_MOTOR
-#define SUPERVISION_MAX_MOTOR 200
-#endif
-
 uint8_t guidance_v_mode;
 int32_t guidance_v_ff_cmd;
 int32_t guidance_v_fb_cmd;
@@ -150,13 +142,7 @@ void guidance_v_run(bool_t in_flight) {
   // FIXME... SATURATIONS NOT TAKEN INTO ACCOUNT
   // AKA SUPERVISION and co
   if (in_flight) {
-    // FIXME: we should use something after the supervision!!! fuck!!!
+    gv_adapt_run(ins_ltp_accel.z, stabilization_cmd[COMMAND_THRUST], guidance_v_zd_ref);
-    int32_t cmd_hack = Chop(stabilization_cmd[COMMAND_THRUST], SUPERVISION_MIN_MOTOR, SUPERVISION_MAX_MOTOR);
-    gv_adapt_run(ins_ltp_accel.z, cmd_hack);
-  }
-  else {
-    // reset vertical filter until takeoff
-    //ins_vf_realign = TRUE;
   }
 
   switch (guidance_v_mode) {

sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adpt.h

@@ -37,6 +37,16 @@ extern int32_t gv_adapt_Xmeas;
 
 #ifdef GUIDANCE_V_C
 
+/** Supervision default bounds
+ *  In case Asctec controllers are used without supervision
+ * */
+#ifndef SUPERVISION_MIN_MOTOR
+#define SUPERVISION_MIN_MOTOR 1
+#endif
+#ifndef SUPERVISION_MAX_MOTOR
+#define SUPERVISION_MAX_MOTOR 200
+#endif
+
 /** State of the estimator
  *  fixed point representation with #GV_ADAPT_X_FRAC
  *  Q13.18
@@ -61,56 +71,73 @@ int32_t gv_adapt_Xmeas;
 #define GV_ADAPT_P0_F 0.5
 #define GV_ADAPT_P0 BFP_OF_REAL(GV_ADAPT_P0_F, GV_ADAPT_P_FRAC)
 
-/* System and Measuremement noises */
+/* System  noises */
 #define GV_ADAPT_SYS_NOISE_F 0.00005
 #define GV_ADAPT_SYS_NOISE  BFP_OF_REAL(GV_ADAPT_SYS_NOISE_F, GV_ADAPT_P_FRAC)
 
-#if !USE_ADAPT_HOVER
+/** Adapt noise factor
+ * Smaller values will make the filter to adapter faster
+ * Bigger values (slower adaptation) make the filter more robust to external perturbations
+ * Factor should always be >0
+ */
+#ifndef GUIDANCE_V_ADAPT_NOISE_FACTOR
+#define GUIDANCE_V_ADAPT_NOISE_FACTOR 1.0
+#endif
 
-#define GV_ADAPT_MEAS_NOISE_F 2.0
+/* Measuremement noises */
-#define GV_ADAPT_MEAS_NOISE BFP_OF_REAL(GV_ADAPT_MEAS_NOISE_F, GV_ADAPT_P_FRAC)
+#define GV_ADAPT_MEAS_NOISE_HOVER_F (8.0*GUIDANCE_V_ADAPT_NOISE_FACTOR)
-
-#else /* USE_ADAPT_HOVER */
-
-#define GV_ADAPT_MEAS_NOISE_HOVER_F 10.0
 #define GV_ADAPT_MEAS_NOISE_HOVER BFP_OF_REAL(GV_ADAPT_MEAS_NOISE_HOVER_F, GV_ADAPT_P_FRAC)
-#define GV_ADAPT_MEAS_NOISE_F 50.0
+#define GV_ADAPT_MEAS_NOISE_OF_ZD (20.0*GUIDANCE_V_ADAPT_NOISE_FACTOR)
-#define GV_ADAPT_MEAS_NOISE BFP_OF_REAL(GV_ADAPT_MEAS_NOISE_F, GV_ADAPT_P_FRAC)
 
-#define GV_ADAPT_MAX_ACCEL ACCEL_BFP_OF_REAL(4.0)
+/** Filter is not fed if accel values are more than +/- MAX_ACCEL
-#define GV_ADAPT_HOVER_ACCEL ACCEL_BFP_OF_REAL(1.0)
+ * MAX_ACCEL is a positive value in m/s^2
-#define GV_ADAPT_MAX_CMD 180
+ */
-#define GV_ADAPT_MIN_CMD 20
+#ifndef GUIDANCE_V_ADAPT_MAX_ACCEL
-#define GV_ADAPT_HOVER_MAX_CMD 120
+#define GUIDANCE_V_ADAPT_MAX_ACCEL 4.0
-#define GV_ADAPT_HOVER_MIN_CMD 60
+#endif
+#define GV_ADAPT_MAX_ACCEL ACCEL_BFP_OF_REAL(GUIDANCE_V_ADAPT_MAX_ACCEL)
+
+/** Filter is not fed if command values are out of a % of MIN/MAX_SUPERVISION
+ * MAX_CMD and MIN_CMD must be between 0 and 1 with MIN_CMD < MAX_CMD
+ */
+#ifndef GUIDANCE_V_ADAPT_MAX_CMD
+#define GUIDANCE_V_ADAPT_MAX_CMD 0.9
+#endif
+#ifndef GUIDANCE_V_ADAPT_MIN_CMD
+#define GUIDANCE_V_ADAPT_MIN_CMD 0.1
+#endif
+
+#define GV_ADAPT_MAX_CMD ((int32_t)(SUPERVISION_MIN_MOTOR + (GUIDANCE_V_ADAPT_MAX_CMD * (SUPERVISION_MAX_MOTOR - SUPERVISION_MIN_MOTOR))))
+#define GV_ADAPT_MIN_CMD ((int32_t)(SUPERVISION_MIN_MOTOR + (GUIDANCE_V_ADAPT_MIN_CMD * (SUPERVISION_MAX_MOTOR - SUPERVISION_MIN_MOTOR))))
 
-#endif /* USE_ADAPT_HOVER */
 
 static inline void gv_adapt_init(void) {
   gv_adapt_X = GV_ADAPT_X0;
   gv_adapt_P = GV_ADAPT_P0;
 }
 
-/*
+/** Adaptation function
-  zdd_meas : INT32_ACCEL_FRAC
+ * zdd_meas : INT32_ACCEL_FRAC
-  thrust_applied : controller input [2-200]
+ * thrust_applied : controller input [SUPERVISION_MIN_MOTOR, SUPERVISION_MAX_MOTOR]
-*/
+ * zd_ref: INT32_SPEED_FRAC
+ */
 #define K_FRAC 12
-static inline void gv_adapt_run(int32_t zdd_meas, int32_t thrust_applied) {
+static inline void gv_adapt_run(int32_t zdd_meas, int32_t thrust_applied, int32_t zd_ref) {
 
-  /* Do you really think we want to divide by zero ? */
+  /* Do you really think we want to divide by zero ?
-  if (thrust_applied == 0) return;
+   * Negative commands are also prohibited here
+   */
+  if (thrust_applied <= 0) return;
 
   /* We don't propagate state, it's constant !       */
   /* We propagate our covariance                     */
   gv_adapt_P =  gv_adapt_P + GV_ADAPT_SYS_NOISE;
 
-#if USE_ADAPT_HOVER
   /* Update only if accel and commands are in a valid range */
   if (thrust_applied < GV_ADAPT_MIN_CMD || thrust_applied > GV_ADAPT_MAX_CMD
-      || zdd_meas < -GV_ADAPT_MAX_ACCEL || zdd_meas > GV_ADAPT_MAX_ACCEL)
+      || zdd_meas < -GV_ADAPT_MAX_ACCEL || zdd_meas > GV_ADAPT_MAX_ACCEL) {
     return;
-#endif
+  }
 
   /* 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)<<(GV_ADAPT_X_FRAC - INT32_ACCEL_FRAC);
@@ -124,14 +151,9 @@ static inline void gv_adapt_run(int32_t zdd_meas, int32_t thrust_applied) {
   int32_t residual = gv_adapt_Xmeas - gv_adapt_X;
 
   /* Covariance Error   */
-  int32_t E = 0;
+  int32_t ref = zd_ref >> (INT32_SPEED_FRAC - GV_ADAPT_P_FRAC);
-#if USE_ADAPT_HOVER
+  if (zd_ref < 0) ref = -ref;
-  if ((thrust_applied > GV_ADAPT_HOVER_MIN_CMD && thrust_applied < GV_ADAPT_HOVER_MAX_CMD) ||
+  int32_t E = gv_adapt_P + GV_ADAPT_MEAS_NOISE_HOVER + ref * GV_ADAPT_MEAS_NOISE_OF_ZD;
-      (zdd_meas > -GV_ADAPT_HOVER_ACCEL && zdd_meas < GV_ADAPT_HOVER_ACCEL))
-    E = gv_adapt_P + GV_ADAPT_MEAS_NOISE_HOVER;
-  else
-#endif
-    E = gv_adapt_P + GV_ADAPT_MEAS_NOISE;
 
   /* Kalman gain        */
   int32_t K = (gv_adapt_P<<K_FRAC) / E;

sw/airborne/firmwares/rotorcraft/stabilization/stabilization_none.c

@@ -32,6 +32,9 @@
 #define REF_DOT_FRAC 11
 #define REF_FRAC  16
 
+#ifndef SUPERVISION_SCALE
+#define SUPERVISION_SCALE MAX_PPRZ
+#endif
 
 #define OFFSET_AND_ROUND(_a, _b) (((_a)+(1<<((_b)-1)))>>(_b))
 #define OFFSET_AND_ROUND2(_a, _b) (((_a)+(1<<((_b)-1))-((_a)<0?1:0))>>(_b))