[rotorcraft] v_adapt: config improvements and refactor

- GUIDANCE_V_ADAPT_INITIAL_HOVER_THROTTLE settable in % with default of 0.3
- GUIDANCE_V_ADAPT_THROTTLE_ENABLED:
  - TRUE by default if GUIDANCE_V_NOMINAL_HOVER_THROTTLE was not defined
  - FALSE by default if only GUIDANCE_V_NOMINAL_HOVER_THROTTLE was set in airframe file
    (for backwards compatibily)
- switching between nominal_trottle and the adaptive estimate during operation via settings

conf/firmwares/rotorcraft.makefile

@@ -275,6 +275,7 @@ ap.srcs += $(SRC_FIRMWARE)/guidance/guidance_h.c
 ap.srcs += $(SRC_FIRMWARE)/guidance/guidance_h_ref.c
 ap.srcs += $(SRC_FIRMWARE)/guidance/guidance_v.c
 ap.srcs += $(SRC_FIRMWARE)/guidance/guidance_v_ref.c
+ap.srcs += $(SRC_FIRMWARE)/guidance/guidance_v_adapt.c
 
 #
 # INS choice

conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile

@@ -119,6 +119,7 @@ nps.srcs += $(SRC_FIRMWARE)/guidance/guidance_h.c
 nps.srcs += $(SRC_FIRMWARE)/guidance/guidance_h_ref.c
 nps.srcs += $(SRC_FIRMWARE)/guidance/guidance_v.c
 nps.srcs += $(SRC_FIRMWARE)/guidance/guidance_v_ref.c
+nps.srcs += $(SRC_FIRMWARE)/guidance/guidance_v_adapt.c
 
 #
 # INS choice

conf/settings/control/rotorcraft_guidance.xml

@@ -7,7 +7,8 @@
       <dl_setting var="guidance_v_kp" min="0" step="1" max="600"   module="guidance/guidance_v" shortname="kp" param="GUIDANCE_V_HOVER_KP"/>
       <dl_setting var="guidance_v_kd" min="0" step="1" max="600"   module="guidance/guidance_v" shortname="kd" param="GUIDANCE_V_HOVER_KD"/>
       <dl_setting var="guidance_v_ki" min="0" step="1" max="300"   module="guidance/guidance_v" shortname="ki" handler="SetKi" param="GUIDANCE_V_HOVER_KI"/>
-      <dl_setting var="guidance_v_nominal_throttle" min="0.1" step="0.01" max="0.9" module="guidance/guidance_v" shortname="nominal_throttle" param="GUIDANCE_V_NOMINAL_HOVER_THROTTLE"/>
+      <dl_setting var="guidance_v_nominal_throttle" min="0.2" step="0.01" max="0.8" module="guidance/guidance_v" shortname="nominal_throttle" param="GUIDANCE_V_NOMINAL_HOVER_THROTTLE"/>
+      <dl_setting var="guidance_v_adapt_throttle_enabled" min="0" step="1" max="1" module="guidance/guidance_v" shortname="adapt_throttle" param="GUIDANCE_V_ADAPT_THROTTLE_ENABLED" values="FALSE|TRUE"/>
       <dl_setting var="guidance_v_z_sp" min="-5" step="0.5" max="3" module="guidance/guidance_v" shortname="sp" unit="2e-8m" alt_unit="m" alt_unit_coef="0.00390625"/>
       <dl_setting var="ins.vf_realign" min="0" step="1" max="1" module="subsystems/ins" shortname="vf_realign" values="OFF|ON"/>
     </dl_settings>

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

@@ -24,32 +24,43 @@
  *
  */
 
-#define GUIDANCE_V_C
+#include "generated/airframe.h"
 #include "firmwares/rotorcraft/guidance/guidance_v.h"
 
-
 #include "subsystems/radio_control.h"
 #include "firmwares/rotorcraft/stabilization.h"
-// #include "booz_fms.h" FIXME
 #include "firmwares/rotorcraft/navigation.h"
 
 #include "state.h"
 
 #include "math/pprz_algebra_int.h"
 
-#include "generated/airframe.h"
 
-
+/* error if some gains are negative */
-/* warn if some gains are still negative */
 #if (GUIDANCE_V_HOVER_KP < 0) ||                   \
   (GUIDANCE_V_HOVER_KD < 0)   ||                   \
   (GUIDANCE_V_HOVER_KI < 0)
-#error "ALL control gains are now positive!!!"
+#error "ALL control gains must be positive!!!"
 #endif
 
+
+/* If only GUIDANCE_V_NOMINAL_HOVER_THROTTLE is defined,
+ * disable the adaptive throttle estimation by default.
+ * Otherwise enable adaptive estimation by default.
+ */
 #ifdef GUIDANCE_V_NOMINAL_HOVER_THROTTLE
-PRINT_CONFIG_VAR(GUIDANCE_V_NOMINAL_HOVER_THROTTLE)
+#  ifndef GUIDANCE_V_ADAPT_THROTTLE_ENABLED
+#    define GUIDANCE_V_ADAPT_THROTTLE_ENABLED FALSE
+#  endif
+#else
+#  define GUIDANCE_V_NOMINAL_HOVER_THROTTLE 0.4
+#  ifndef GUIDANCE_V_ADAPT_THROTTLE_ENABLED
+#    define GUIDANCE_V_ADAPT_THROTTLE_ENABLED TRUE
+#  endif
 #endif
+PRINT_CONFIG_VAR(GUIDANCE_V_NOMINAL_HOVER_THROTTLE)
+PRINT_CONFIG_VAR(GUIDANCE_V_ADAPT_THROTTLE_ENABLED)
+
 
 uint8_t guidance_v_mode;
 int32_t guidance_v_ff_cmd;
@@ -57,6 +68,7 @@ int32_t guidance_v_fb_cmd;
 int32_t guidance_v_delta_t;
 
 float guidance_v_nominal_throttle;
+bool_t guidance_v_adapt_throttle_enabled;
 
 
 /** Direct throttle from radio control.
@@ -91,7 +103,7 @@ int32_t guidance_v_z_sum_err;
   }
 
 
-__attribute__ ((always_inline)) static inline void run_hover_loop(bool_t in_flight);
+void run_hover_loop(bool_t in_flight);
 
 
 void guidance_v_init(void) {
@@ -104,9 +116,8 @@ void guidance_v_init(void) {
 
   guidance_v_z_sum_err = 0;
 
-#ifdef GUIDANCE_V_NOMINAL_HOVER_THROTTLE
   guidance_v_nominal_throttle = GUIDANCE_V_NOMINAL_HOVER_THROTTLE;
-#endif
+  guidance_v_adapt_throttle_enabled = GUIDANCE_V_ADAPT_THROTTLE_ENABLED;
 
   gv_adapt_init();
 }
@@ -254,7 +265,7 @@ void guidance_v_run(bool_t in_flight) {
 
 #define MAX_BANK_COEF (BFP_OF_REAL(RadOfDeg(30.),INT32_TRIG_FRAC))
 
-__attribute__ ((always_inline)) static inline void run_hover_loop(bool_t in_flight) {
+void run_hover_loop(bool_t in_flight) {
 
   /* convert our reference to generic representation */
   int64_t tmp  = gv_z_ref>>(GV_Z_REF_FRAC - INT32_POS_FRAC);
@@ -275,13 +286,17 @@ __attribute__ ((always_inline)) static inline void run_hover_loop(bool_t in_flig
     guidance_v_z_sum_err = 0;
 
   /* our nominal command : (g + zdd)*m   */
-#ifdef GUIDANCE_V_NOMINAL_HOVER_THROTTLE
+  int32_t inv_m;
-  const int32_t inv_m = BFP_OF_REAL(9.81/(guidance_v_nominal_throttle*MAX_PPRZ), FF_CMD_FRAC);
+  if (guidance_v_adapt_throttle_enabled) {
-#else
+    inv_m =  gv_adapt_X >> (GV_ADAPT_X_FRAC - FF_CMD_FRAC);
-  const int32_t inv_m =  gv_adapt_X>>(GV_ADAPT_X_FRAC - FF_CMD_FRAC);
+  }
-#endif
+  else {
+    /* use the fixed nominal throttle */
+    inv_m = BFP_OF_REAL(9.81 / (guidance_v_nominal_throttle * MAX_PPRZ), FF_CMD_FRAC);
+  }
+
   const int32_t g_m_zdd = (int32_t)BFP_OF_REAL(9.81, FF_CMD_FRAC) -
-                          (guidance_v_zdd_ref<<(FF_CMD_FRAC - INT32_ACCEL_FRAC));
+                          (guidance_v_zdd_ref << (FF_CMD_FRAC - INT32_ACCEL_FRAC));
 
   guidance_v_ff_cmd = g_m_zdd / inv_m;
   int32_t cphi,ctheta,cphitheta;

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

@@ -24,15 +24,13 @@
  *
  */
 
-#ifndef GUIDANCE_V
+#ifndef GUIDANCE_V_H
-#define GUIDANCE_V
+#define GUIDANCE_V_H
 
 #include "std.h"
 
-#include "generated/airframe.h"
 #include "firmwares/rotorcraft/guidance/guidance_v_ref.h"
-
+#include "firmwares/rotorcraft/guidance/guidance_v_adapt.h"
-#include "firmwares/rotorcraft/guidance/guidance_v_adpt.h"
 
 #define GUIDANCE_V_MODE_KILL      0
 #define GUIDANCE_V_MODE_RC_DIRECT 1
@@ -89,6 +87,10 @@ extern int32_t guidance_v_delta_t;
  */
 extern float guidance_v_nominal_throttle;
 
+/** Use adaptive throttle command estimation.
+ */
+extern bool_t guidance_v_adapt_throttle_enabled;
+
 extern int32_t guidance_v_kp; ///< vertical control P-gain
 extern int32_t guidance_v_kd; ///< vertical control D-gain
 extern int32_t guidance_v_ki; ///< vertical control I-gain
@@ -104,4 +106,4 @@ extern void guidance_v_run(bool_t in_flight);
     guidance_v_z_sum_err = 0;			\
   }
 
-#endif /* GUIDANCE_V */
+#endif /* GUIDANCE_V_H */

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

@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2009-2010 The Paparazzi Team
+ * Copyright (C) 2009-2013 The Paparazzi Team
  *
  * This file is part of paparazzi.
  *
@@ -19,18 +19,47 @@
  * Boston, MA 02111-1307, USA.
  */
 
-/** @file firmwares/rotorcraft/guidance/guidance_v_adpt.h
+/** @file firmwares/rotorcraft/guidance/guidance_v_adapt.c
- *  Adaptation bloc of the vertical guidance.
+ *  Adaptation block of the vertical guidance.
  *
  *  This is a dimension one kalman filter estimating
- *  the ratio of vertical acceleration over thrust command ( ~ invert of the mass )
+ *  the ratio of vertical acceleration over thrust command ( ~ inverse of the mass )
- *  needed by the invert dynamic model to produce a nominal command
+ *  needed by the invert dynamic model to produce a nominal command.
  */
 
-#ifndef GUIDANCE_V_ADPT
+#include "firmwares/rotorcraft/guidance/guidance_v_adapt.h"
-#define GUIDANCE_V_ADPT
-
 #include "paparazzi.h"
+#include "math/pprz_algebra_int.h"
+
+
+/** Initial hover throttle as factor of MAX_PPRZ.
+ *  Should be a value between #GUIDANCE_V_ADAPT_MIN_HOVER_THROTTLE and
+ *  #GUIDANCE_V_ADAPT_MAX_HOVER_THROTTLE.
+ *  It is better to start with low thrust and let it rise as the adaptive filter
+ *  finds the vehicle needs more thrust.
+ */
+#ifndef GUIDANCE_V_ADAPT_INITIAL_HOVER_THROTTLE
+#define GUIDANCE_V_ADAPT_INITIAL_HOVER_THROTTLE 0.3
+#endif
+PRINT_CONFIG_VAR(GUIDANCE_V_ADAPT_INITIAL_HOVER_THROTTLE)
+
+/** Minimum hover throttle as factor of MAX_PPRZ.
+ *  With the default of 0.2 the nominal hover throttle will
+ *  never go lower than 20%.
+ */
+#ifndef GUIDANCE_V_ADAPT_MIN_HOVER_THROTTLE
+#define GUIDANCE_V_ADAPT_MIN_HOVER_THROTTLE 0.2
+#endif
+PRINT_CONFIG_VAR(GUIDANCE_V_ADAPT_MIN_HOVER_THROTTLE)
+
+/** Maximum hover throttle as factor of MAX_PPRZ.
+ *  With the default of 0.75 the nominal hover throttle will
+ *  never go over 75% of max throttle.
+ */
+#ifndef GUIDANCE_V_ADAPT_MAX_HOVER_THROTTLE
+#define GUIDANCE_V_ADAPT_MAX_HOVER_THROTTLE 0.75
+#endif
+PRINT_CONFIG_VAR(GUIDANCE_V_ADAPT_MAX_HOVER_THROTTLE)
 
 /** Adapt noise factor.
  *  Smaller values will make the filter to adapt faster.
@@ -41,14 +70,6 @@
 #define GUIDANCE_V_ADAPT_NOISE_FACTOR 1.0
 #endif
 
-/** Initial estimation.
- *  The initial value can be adapted for faster converging time.
- *  It is usually recommended to start with a low value (overestimation of the mass),
- *  as it is helping for a smooth takeoff.
- */
-#ifndef GUIDANCE_V_ADAPT_X0
-#define GUIDANCE_V_ADAPT_X0 0.003
-#endif
 
 /** Filter is not fed if accel values are more than +/- MAX_ACCEL.
  *  MAX_ACCEL is a positive value in m/s^2
@@ -67,53 +88,12 @@
 #define GUIDANCE_V_ADAPT_MIN_CMD 0.1
 #endif
 
-/** Minimum hover throttle as factor of MAX_PPRZ.
- *  With the default of 0.2 the nominal hover throttle will
- *  never go lower than 20%.
- */
-#ifndef GUIDANCE_V_ADAPT_MIN_HOVER_THROTTLE
-#define GUIDANCE_V_ADAPT_MIN_HOVER_THROTTLE 0.2
-#endif
 
-/** Maximum hover throttle as factor of MAX_PPRZ.
- *  With the default of 0.75 the nominal hover throttle will
- *  never go over 75% of max throttle.
- */
-#ifndef GUIDANCE_V_ADAPT_MAX_HOVER_THROTTLE
-#define GUIDANCE_V_ADAPT_MAX_HOVER_THROTTLE 0.75
-#endif
-
-/** State of the estimator.
- *  fixed point representation with #GV_ADAPT_X_FRAC
- *  Q13.18
- */
-extern int32_t gv_adapt_X;
-#define GV_ADAPT_X_FRAC 24
-
-/** Covariance.
- *  fixed point representation with #GV_ADAPT_P_FRAC
- *  Q13.18
- */
-extern int32_t gv_adapt_P;
-#define GV_ADAPT_P_FRAC 18
-
-/** Measurement */
-extern int32_t gv_adapt_Xmeas;
-
-
-#ifdef GUIDANCE_V_C
 
 int32_t gv_adapt_X;
 int32_t gv_adapt_P;
 int32_t gv_adapt_Xmeas;
 
-
-/* Initial State and Covariance    */
-#define GV_ADAPT_X0_F GUIDANCE_V_ADAPT_X0
-#define GV_ADAPT_X0 BFP_OF_REAL(GV_ADAPT_X0_F, GV_ADAPT_X_FRAC)
-#define GV_ADAPT_P0_F 0.1
-#define GV_ADAPT_P0 BFP_OF_REAL(GV_ADAPT_P0_F, GV_ADAPT_P_FRAC)
-
 /* System  noises */
 #ifndef GV_ADAPT_SYS_NOISE_F
 #define GV_ADAPT_SYS_NOISE_F 0.00005
@@ -125,10 +105,15 @@ int32_t gv_adapt_Xmeas;
 #define GV_ADAPT_MEAS_NOISE_HOVER BFP_OF_REAL(GV_ADAPT_MEAS_NOISE_HOVER_F, GV_ADAPT_P_FRAC)
 #define GV_ADAPT_MEAS_NOISE_OF_ZD (100.0*GUIDANCE_V_ADAPT_NOISE_FACTOR)
 
+/* Initial Covariance    */
+#define GV_ADAPT_P0_F 0.1
+static const int32_t gv_adapt_P0 = BFP_OF_REAL(GV_ADAPT_P0_F, GV_ADAPT_P_FRAC);
+static const int32_t gv_adapt_X0 = BFP_OF_REAL(9.81, GV_ADAPT_X_FRAC) /
+  (GUIDANCE_V_ADAPT_INITIAL_HOVER_THROTTLE * MAX_PPRZ);
 
-static inline void gv_adapt_init(void) {
+void gv_adapt_init(void) {
-  gv_adapt_X = GV_ADAPT_X0;
+  gv_adapt_X = gv_adapt_X0;
-  gv_adapt_P = GV_ADAPT_P0;
+  gv_adapt_P = gv_adapt_P0;
 }
 
 #define K_FRAC 12
@@ -138,7 +123,7 @@ static inline void gv_adapt_init(void) {
  * @param thrust_applied  controller input [0 : MAX_PPRZ]
  * @param zd_ref          vertical speed reference in m/s with #INT32_SPEED_FRAC
  */
-static inline void gv_adapt_run(int32_t zdd_meas, int32_t thrust_applied, int32_t zd_ref) {
+void gv_adapt_run(int32_t zdd_meas, int32_t thrust_applied, int32_t zd_ref) {
 
   static const int32_t gv_adapt_min_cmd = GUIDANCE_V_ADAPT_MIN_CMD * MAX_PPRZ;
   static const int32_t gv_adapt_max_cmd = GUIDANCE_V_ADAPT_MAX_CMD * MAX_PPRZ;
@@ -177,8 +162,8 @@ static inline void gv_adapt_run(int32_t zdd_meas, int32_t thrust_applied, int32_
   /* Update Covariance  Pnew = P - K * P   */
   gv_adapt_P = gv_adapt_P - ((K * gv_adapt_P)>>K_FRAC);
   /* Don't let covariance climb over initial value */
-  if (gv_adapt_P > GV_ADAPT_P0) {
+  if (gv_adapt_P > gv_adapt_P0) {
-    gv_adapt_P = GV_ADAPT_P0;
+    gv_adapt_P = gv_adapt_P0;
   }
 
   /* Update State */
@@ -195,8 +180,3 @@ static inline void gv_adapt_run(int32_t zdd_meas, int32_t thrust_applied, int32_
     (GUIDANCE_V_ADAPT_MAX_HOVER_THROTTLE * MAX_PPRZ);
   Bound(gv_adapt_X, min_out, max_out);
 }
-
-
-#endif /* GUIDANCE_V_C */
-
-#endif /* GUIDANCE_V_ADPT */

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

@@ -0,0 +1,57 @@
+/*
+ * Copyright (C) 2009-2013 The Paparazzi Team
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/** @file firmwares/rotorcraft/guidance/guidance_v_adapt.h
+ *  Adaptation block of the vertical guidance.
+ *
+ *  This is a dimension one kalman filter estimating
+ *  the ratio of vertical acceleration over thrust command ( ~ inverse of the mass )
+ *  needed by the invert dynamic model to produce a nominal command.
+ */
+
+#ifndef GUIDANCE_V_ADAPT_H
+#define GUIDANCE_V_ADAPT_H
+
+#include "std.h"
+
+/** State of the estimator.
+ *  fixed point representation with #GV_ADAPT_X_FRAC
+ *  Q13.18
+ */
+extern int32_t gv_adapt_X;
+#define GV_ADAPT_X_FRAC 24
+
+/** Covariance.
+ *  fixed point representation with #GV_ADAPT_P_FRAC
+ *  Q13.18
+ */
+extern int32_t gv_adapt_P;
+#define GV_ADAPT_P_FRAC 18
+
+/** Measurement */
+extern int32_t gv_adapt_Xmeas;
+
+
+extern void gv_adapt_init(void);
+extern void gv_adapt_run(int32_t zdd_meas, int32_t thrust_applied, int32_t zd_ref);
+
+
+#endif /* GUIDANCE_V_ADAPT_H */