Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-04 22:17:01 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

[rotorcraft] v_adapt: config improvements and refactor

Browse Source 
- 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
This commit is contained in:
Felix Ruess
2013-08-13 14:14:41 +02:00
parent 95871c8d2b
commit fcc3b270ed
7 changed files with 150 additions and 93 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/firmwares/rotorcraft.makefile
+1
View File
@@ -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
+1
View File
@@ -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
+2 -1
View File
@@ -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
+33 -18
View File
@@ -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;
sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h
+8 -6
View File
@@ -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 */
sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adpt.h → sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adapt.c
+48 -68
View File
@@ -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 */
sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adapt.h
+57
View File
@@ -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 */