From fcc3b270ed164cb37b1db9e3cb3d8c2a084cfd82 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Tue, 13 Aug 2013 14:14:41 +0200 Subject: [PATCH] [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 | 1 + .../subsystems/rotorcraft/fdm_jsbsim.makefile | 1 + conf/settings/control/rotorcraft_guidance.xml | 3 +- .../rotorcraft/guidance/guidance_v.c | 51 +++++--- .../rotorcraft/guidance/guidance_v.h | 14 ++- .../{guidance_v_adpt.h => guidance_v_adapt.c} | 116 ++++++++---------- .../rotorcraft/guidance/guidance_v_adapt.h | 57 +++++++++ 7 files changed, 150 insertions(+), 93 deletions(-) rename sw/airborne/firmwares/rotorcraft/guidance/{guidance_v_adpt.h => guidance_v_adapt.c} (79%) create mode 100644 sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adapt.h 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 */