diff --git a/conf/firmwares/rotorcraft.makefile b/conf/firmwares/rotorcraft.makefile
index ffbea72141..bb42616a4b 100644
--- a/conf/firmwares/rotorcraft.makefile
+++ b/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
diff --git a/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile b/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile
index f57c246e8c..4785cbc075 100644
--- a/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile
+++ b/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
diff --git a/conf/settings/control/rotorcraft_guidance.xml b/conf/settings/control/rotorcraft_guidance.xml
index 1362a96a38..45c73a958b 100644
--- a/conf/settings/control/rotorcraft_guidance.xml
+++ b/conf/settings/control/rotorcraft_guidance.xml
@@ -7,7 +7,8 @@
-
+
+
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
index 53f74082a7..432f655f1b 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
+++ b/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"
-
-/* warn if some gains are still negative */
+/* error if some gains are 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
- const int32_t inv_m = BFP_OF_REAL(9.81/(guidance_v_nominal_throttle*MAX_PPRZ), FF_CMD_FRAC);
-#else
- const int32_t inv_m = gv_adapt_X>>(GV_ADAPT_X_FRAC - FF_CMD_FRAC);
-#endif
+ int32_t inv_m;
+ if (guidance_v_adapt_throttle_enabled) {
+ inv_m = gv_adapt_X >> (GV_ADAPT_X_FRAC - FF_CMD_FRAC);
+ }
+ 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;
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h
index a9c2f86403..4a94b4cfa8 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h
@@ -24,15 +24,13 @@
*
*/
-#ifndef GUIDANCE_V
-#define GUIDANCE_V
+#ifndef GUIDANCE_V_H
+#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_adpt.h"
+#include "firmwares/rotorcraft/guidance/guidance_v_adapt.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 */
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adpt.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adapt.c
similarity index 79%
rename from sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adpt.h
rename to sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adapt.c
index ba743cb2cd..bcc58fbb15 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adpt.h
+++ b/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
- * Adaptation bloc of the vertical guidance.
+/** @file firmwares/rotorcraft/guidance/guidance_v_adapt.c
+ * 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 )
- * needed by the invert dynamic model to produce a nominal command
+ * 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_ADPT
-#define GUIDANCE_V_ADPT
-
+#include "firmwares/rotorcraft/guidance/guidance_v_adapt.h"
#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) {
- gv_adapt_X = GV_ADAPT_X0;
- gv_adapt_P = GV_ADAPT_P0;
+void gv_adapt_init(void) {
+ gv_adapt_X = gv_adapt_X0;
+ 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) {
- gv_adapt_P = GV_ADAPT_P0;
+ if (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 */
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adapt.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adapt.h
new file mode 100644
index 0000000000..8be2e35a22
--- /dev/null
+++ b/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 */