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

[rotorcraft] v_adapt: limit min/max estimated hover throttle

Browse Source 
The adaptive vertical filter estimates the ratio of vertical acceleration over thrust command.
In effect this will now limit the throttle applied in hover between 20% and 75% of max throttle by default.
This commit is contained in:
Felix Ruess
2013-08-07 22:58:12 +02:00
parent 8efc24099c
commit e9ca4d4f1f
1 changed files with 35 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adpt.h
+35 -28
View File
@@ -33,7 +33,7 @@
#include "paparazzi.h"
/** Adapt noise factor.
* Smaller values will make the filter to adapter faster.
* Smaller values will make the filter to adapt faster.
* Bigger values (slower adaptation) make the filter more robust to external perturbations.
* Factor should always be >0
*/
@@ -67,6 +67,22 @@
#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
@@ -109,27 +125,6 @@ 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)
/* Max accel */
#define GV_ADAPT_MAX_ACCEL ACCEL_BFP_OF_REAL(GUIDANCE_V_ADAPT_MAX_ACCEL)
/* Command bounds */
#define GV_ADAPT_MAX_CMD ((int32_t)(GUIDANCE_V_ADAPT_MAX_CMD*MAX_PPRZ))
#define GV_ADAPT_MIN_CMD ((int32_t)(GUIDANCE_V_ADAPT_MIN_CMD*MAX_PPRZ))
/* Output bounds.
* Don't let it climb over a value that would
* give less than zero throttle and don't let it down to zero.
* Worst cases:
* MIN_ACCEL / MAX_THROTTLE
* MAX_ACCEL / MIN_THROTTLE
* ex:
* 9.81*2^18/255 = 10084
* 9.81*2^18/1 = 2571632
*/
// TODO Check this properly
#define GV_ADAPT_MAX_OUT (BFP_OF_REAL(9.81, GV_ADAPT_X_FRAC))
#define GV_ADAPT_MIN_OUT (BFP_OF_REAL(9.81, GV_ADAPT_X_FRAC) / MAX_PPRZ)
static inline void gv_adapt_init(void) {
gv_adapt_X = GV_ADAPT_X0;
@@ -145,10 +140,14 @@ static inline void gv_adapt_init(void) {
*/
static inline 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;
static const int32_t gv_adapt_max_accel = ACCEL_BFP_OF_REAL(GUIDANCE_V_ADAPT_MAX_ACCEL);
/* Update only if accel and commands are in a valid range */
/* This also ensures we don't divide by zero */
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) {
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) {
return;
}
@@ -178,15 +177,23 @@ 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 */
gv_adapt_X = gv_adapt_X + (((int64_t)(K * residual))>>K_FRAC);
/* Again don't let it climb over a value that would
* give less than zero throttle and don't let it down to zero.
/* Output bounds.
* Don't let it climb over a value that would
* give less than #GUIDANCE_V_ADAPT_MIN_HOVER_THROTTLE % throttle
* or more than #GUIDANCE_V_ADAPT_MAX_HOVER_THROTTLE % throttle.
*/
Bound(gv_adapt_X, GV_ADAPT_MIN_OUT, GV_ADAPT_MAX_OUT);
static const int32_t max_out = BFP_OF_REAL(9.81, GV_ADAPT_X_FRAC) /
(GUIDANCE_V_ADAPT_MIN_HOVER_THROTTLE * MAX_PPRZ);
static const int32_t min_out = BFP_OF_REAL(9.81, GV_ADAPT_X_FRAC) /
(GUIDANCE_V_ADAPT_MAX_HOVER_THROTTLE * MAX_PPRZ);
Bound(gv_adapt_X, min_out, max_out);
}