Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-07 00:53:41 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

set saturations in the conf file and possibility to adapt slower

Browse Source
This commit is contained in:
Gautier Hattenberger
2009-09-08 15:15:11 +00:00
parent 85c130e991
commit 40c35fdddf
2 changed files with 60 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/booz/guidance/booz2_guidance_v_adpt.h
+32 -2
View File
@@ -64,9 +64,26 @@ int32_t b2_gv_adapt_Xmeas;
/* System and Measuremement noises */
#define B2_GV_ADAPT_SYS_NOISE_F 0.00005
#define B2_GV_ADAPT_SYS_NOISE BFP_OF_REAL(B2_GV_ADAPT_SYS_NOISE_F, B2_GV_ADAPT_P_FRAC)
#define B2_GV_ADAPT_MEAS_NOISE_F 2.0
#ifndef USE_ADAPT_HOVER
#define B2_GV_ADAPT_MEAS_NOISE_F 2.0
#define B2_GV_ADAPT_MEAS_NOISE_HOVER BFP_OF_REAL(B2_GV_ADAPT_MEAS_NOISE_HOVER_F, B2_GV_ADAPT_P_FRAC)
#else /* USE_ADAPT_HOVER */
#define B2_GV_ADAPT_MEAS_NOISE_HOVER_F 10.0
#define B2_GV_ADAPT_MEAS_NOISE_HOVER BFP_OF_REAL(B2_GV_ADAPT_MEAS_NOISE_HOVER_F, B2_GV_ADAPT_P_FRAC)
#define B2_GV_ADAPT_MEAS_NOISE_F 50.0
#define B2_GV_ADAPT_MEAS_NOISE BFP_OF_REAL(B2_GV_ADAPT_MEAS_NOISE_F, B2_GV_ADAPT_P_FRAC)
#define B2_GV_ADAPT_MAX_ACCEL ACCEL_BFP_OF_REAL(4.0)
#define B2_GV_ADAPT_HOVER_ACCEL ACCEL_BFP_OF_REAL(1.0)
#define B2_GV_ADAPT_MAX_CMD 180
#define B2_GV_ADAPT_MIN_CMD 20
#define B2_GV_ADAPT_HOVER_MAX_CMD 120
#define B2_GV_ADAPT_HOVER_MIN_CMD 60
#endif
static inline void b2_gv_adapt_init(void) {
b2_gv_adapt_X = B2_GV_ADAPT_X0;
@@ -87,6 +104,12 @@ static inline void b2_gv_adapt_run(int32_t zdd_meas, int32_t thrust_applied) {
b2_gv_adapt_P = b2_gv_adapt_P + B2_GV_ADAPT_SYS_NOISE;
/* Compute our measurement. If zdd_meas is in the range +/-5g, meas is less than 24 bits */
const int32_t g_m_zdd = ((int32_t)BFP_OF_REAL(9.81, INT32_ACCEL_FRAC) - zdd_meas)<<(B2_GV_ADAPT_X_FRAC - INT32_ACCEL_FRAC);
#ifdef USE_ADAPT_HOVER
/* Update only if accel and commands are in a valid range */
if (thrust_applied < B2_GV_ADAPT_MIN_CMD || thrust_applied > B2_GV_ADAPT_MAX_CMD
|| zdd_meas < -B2_GV_ADAPT_MAX_ACCEL || zdd_meas > B2_GV_ADAPT_MAX_ACCEL)
return;
#endif
if ( g_m_zdd > 0)
b2_gv_adapt_Xmeas = (g_m_zdd + (thrust_applied>>1)) / thrust_applied;
else
@@ -94,7 +117,14 @@ static inline void b2_gv_adapt_run(int32_t zdd_meas, int32_t thrust_applied) {
/* Compute a residual */
int32_t residual = b2_gv_adapt_Xmeas - b2_gv_adapt_X;
/* Covariance Error */
int32_t E = b2_gv_adapt_P + B2_GV_ADAPT_MEAS_NOISE;
int32_t E = 0;
#ifdef USE_ADAPT_HOVER
if ((thrust_applied > B2_GV_ADAPT_HOVER_MIN_CMD && thrust_applied < B2_GV_ADAPT_HOVER_MAX_CMD) ||
(zdd_meas > -B2_GV_ADAPT_HOVER_ACCEL && zdd_meas < B2_GV_ADAPT_HOVER_ACCEL))
E = b2_gv_adapt_P + B2_GV_ADAPT_MEAS_NOISE_HOVER;
else
#endif
E = b2_gv_adapt_P + B2_GV_ADAPT_MEAS_NOISE;
/* Kalman gain */
int32_t K = (b2_gv_adapt_P<<K_FRAC) / E;
/* Update Covariance */
sw/airborne/booz/guidance/booz2_guidance_v_ref.h
+28 -12
View File
@@ -24,6 +24,7 @@
#ifndef BOOZ2_GUIDANCE_V_REF_H
#define BOOZ2_GUIDANCE_V_REF_H
#include "airframe.h"
#include "inttypes.h"
#include "math/pprz_algebra.h"
#include "math/pprz_algebra_int.h"
@@ -49,22 +50,37 @@ extern int64_t b2_gv_z_ref;
#define B2_GV_Z_REF_FRAC (B2_GV_ZD_REF_FRAC + B2_GV_FREQ_FRAC)
/* Saturations definition */
#define B2_GV_MIN_ZDD_F (-2.0*9.81)
#define B2_GV_MIN_ZDD BFP_OF_REAL(B2_GV_MIN_ZDD_F, B2_GV_ZDD_REF_FRAC)
#define B2_GV_MAX_ZDD_F ( 0.8*9.81)
#define B2_GV_MAX_ZDD BFP_OF_REAL(B2_GV_MAX_ZDD_F, B2_GV_ZDD_REF_FRAC)
#define B2_GV_MIN_ZD_F (-3.)
#define B2_GV_MIN_ZD BFP_OF_REAL(B2_GV_MIN_ZD_F , B2_GV_ZD_REF_FRAC)
#define B2_GV_MAX_ZD_F ( 3.)
#define B2_GV_MAX_ZD BFP_OF_REAL(B2_GV_MAX_ZD_F , B2_GV_ZD_REF_FRAC)
#ifndef BOOZ2_GUIDANCE_V_REF_MIN_ZDD
#define BOOZ2_GUIDANCE_V_REF_MIN_ZDD (-2.0*9.81)
#endif
#define B2_GV_MIN_ZDD BFP_OF_REAL(BOOZ2_GUIDANCE_V_REF_MIN_ZDD, B2_GV_ZDD_REF_FRAC)
#ifndef BOOZ2_GUIDANCE_V_REF_MAX_ZDD
#define BOOZ2_GUIDANCE_V_REF_MAX_ZDD ( 0.8*9.81)
#endif
#define B2_GV_MAX_ZDD BFP_OF_REAL(BOOZ2_GUIDANCE_V_REF_MAX_ZDD, B2_GV_ZDD_REF_FRAC)
#ifndef BOOZ2_GUIDANCE_V_REF_MIN_ZD
#define BOOZ2_GUIDANCE_V_REF_MIN_ZD (-3.)
#endif
#define B2_GV_MIN_ZD BFP_OF_REAL(BOOZ2_GUIDANCE_V_REF_MIN_ZD , B2_GV_ZD_REF_FRAC)
#ifndef BOOZ2_GUIDANCE_V_REF_MAX_ZD
#define BOOZ2_GUIDANCE_V_REF_MAX_ZD ( 3.)
#endif
#define B2_GV_MAX_ZD BFP_OF_REAL(BOOZ2_GUIDANCE_V_REF_MAX_ZD , B2_GV_ZD_REF_FRAC)
/* second order model natural frequency and damping */
#define B2_GV_OMEGA RadOfDeg(100.)
#define B2_GV_ZETA 0.85
#ifndef BOOZ2_GUIDANCE_V_REF_OMEGA
#define BOOZ2_GUIDANCE_V_REF_OMEGA RadOfDeg(100.)
#endif
#ifndef BOOZ2_GUIDANCE_V_REF_ZETA
#define BOOZ2_GUIDANCE_V_REF_ZETA 0.85
#endif
#define B2_GV_ZETA_OMEGA_FRAC 10
#define B2_GV_ZETA_OMEGA BFP_OF_REAL((B2_GV_ZETA*B2_GV_OMEGA), B2_GV_ZETA_OMEGA_FRAC)
#define B2_GV_ZETA_OMEGA BFP_OF_REAL((BOOZ2_GUIDANCE_V_REF_ZETA*BOOZ2_GUIDANCE_V_REF_OMEGA), B2_GV_ZETA_OMEGA_FRAC)
#define B2_GV_OMEGA_2_FRAC 7
#define B2_GV_OMEGA_2 BFP_OF_REAL((B2_GV_OMEGA*B2_GV_OMEGA), B2_GV_OMEGA_2_FRAC)
#define B2_GV_OMEGA_2 BFP_OF_REAL((BOOZ2_GUIDANCE_V_REF_OMEGA*BOOZ2_GUIDANCE_V_REF_OMEGA), B2_GV_OMEGA_2_FRAC)
/* first order time constant */
#define B2_GV_REF_THAU_F 0.25