From c3b69389d18ca247cabb4aa09bb8084966560428 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Fri, 21 Aug 2009 08:25:25 +0000 Subject: [PATCH] run hff at a lower frequency (default 32Hz). define HFF_PRESCALER= in airframe file to change --- conf/airframes/booz2_flixr.xml | 2 +- conf/autopilot/booz2_autopilot.makefile | 1 - sw/airborne/booz/booz2_ins.c | 46 ++++++++++++++++--------- sw/airborne/booz/ins/booz2_hf_float.c | 23 ++++++++----- sw/airborne/booz/ins/booz2_hf_float.h | 4 +++ 5 files changed, 49 insertions(+), 27 deletions(-) diff --git a/conf/airframes/booz2_flixr.xml b/conf/airframes/booz2_flixr.xml index ffa72b5e44..08373d292b 100644 --- a/conf/airframes/booz2_flixr.xml +++ b/conf/airframes/booz2_flixr.xml @@ -205,7 +205,7 @@ include $(CFG_BOOZ)/subsystems/booz2_ahrs_cmpl.makefile #enable horizontal filter - ap.CFLAGS += -DUSE_HFF + ap.CFLAGS += -DUSE_HFF -DHFF_PRESCALER=16 diff --git a/conf/autopilot/booz2_autopilot.makefile b/conf/autopilot/booz2_autopilot.makefile index ae0b2ada0e..5a0140773d 100644 --- a/conf/autopilot/booz2_autopilot.makefile +++ b/conf/autopilot/booz2_autopilot.makefile @@ -132,7 +132,6 @@ ap.srcs += $(SRC_BOOZ)/booz2_ins.c ap.srcs += math/pprz_geodetic_int.c math/pprz_geodetic_float.c # horizontal filter float version ap.srcs += $(SRC_BOOZ)/ins/booz2_hf_float.c -ap.CFLAGS += -DDT_HFILTER="(1./512)" # vertical filter float version ap.srcs += $(SRC_BOOZ)/ins/booz2_vf_float.c ap.CFLAGS += -DUSE_VFF -DDT_VFILTER="(1./512.)" -DFLOAT_T=float diff --git a/sw/airborne/booz/booz2_ins.c b/sw/airborne/booz/booz2_ins.c index b6c3964c4c..27b663b7a0 100644 --- a/sw/airborne/booz/booz2_ins.c +++ b/sw/airborne/booz/booz2_ins.c @@ -68,6 +68,11 @@ struct EnuCoor_i booz_ins_enu_pos; struct EnuCoor_i booz_ins_enu_speed; struct EnuCoor_i booz_ins_enu_accel; +#ifdef USE_HFF +/* counter for hff propagation*/ +uint8_t b2_hff_ps_counter; +#endif + void booz_ins_init() { #ifdef USE_NAV_INS_INIT @@ -109,19 +114,12 @@ void booz_ins_propagate() { INT32_RMAT_TRANSP_VMULT(accel_body, booz_imu.body_to_imu_rmat, booz_imu.accel); struct Int32Vect3 accel_ltp; INT32_RMAT_TRANSP_VMULT(accel_ltp, booz_ahrs.ltp_to_body_rmat, accel_body); -#ifdef USE_HFF - float x_accel_float = ACCEL_FLOAT_OF_BFP(accel_ltp.x); - float y_accel_float = ACCEL_FLOAT_OF_BFP(accel_ltp.y); -#else +#ifndef USE_HFF booz_ins_ltp_accel.x = accel_ltp.x; booz_ins_ltp_accel.y = accel_ltp.y; #endif float z_accel_float = ACCEL_FLOAT_OF_BFP(accel_ltp.z); #else /* BOOZ_INS_UNTILT_ACCELS */ -#ifdef USE_HFF - float x_accel_float = ACCEL_FLOAT_OF_BFP(booz_imu.accel.x); - float y_accel_float = ACCEL_FLOAT_OF_BFP(booz_imu.accel.y); -#endif float z_accel_float = ACCEL_FLOAT_OF_BFP(booz_imu.accel.z); #endif /* BOOZ_INS_UNTILT_ACCELS */ @@ -139,14 +137,29 @@ void booz_ins_propagate() { #endif /* USE_VFF */ #ifdef USE_HFF - if (booz_ahrs.status == BOOZ_AHRS_RUNNING && booz_gps_state.fix == BOOZ2_GPS_FIX_3D && booz_ins_ltp_initialised ) { - b2_hff_propagate(x_accel_float, y_accel_float); - booz_ins_ltp_accel.x = ACCEL_BFP_OF_REAL(b2_hff_xdotdot); - booz_ins_ltp_accel.y = ACCEL_BFP_OF_REAL(b2_hff_ydotdot); - booz_ins_ltp_speed.x = SPEED_BFP_OF_REAL(b2_hff_xdot); - booz_ins_ltp_speed.y = SPEED_BFP_OF_REAL(b2_hff_ydot); - booz_ins_ltp_pos.x = POS_BFP_OF_REAL(b2_hff_x); - booz_ins_ltp_pos.y = POS_BFP_OF_REAL(b2_hff_y); + if (b2_hff_ps_counter == HFF_PRESCALER) { + b2_hff_ps_counter = 1; + if (booz_ahrs.status == BOOZ_AHRS_RUNNING && booz_ins_ltp_initialised ) { + /* compute float ltp mean acceleration */ + booz_ahrs_compute_accel_mean(HFF_PRESCALER); + struct Int32Vect3 mean_accel_body; + INT32_RMAT_TRANSP_VMULT(mean_accel_body, booz_imu.body_to_imu_rmat, booz_ahrs_accel_mean); + struct Int32Vect3 mean_accel_ltp; + float x_accel_mean_f = ACCEL_FLOAT_OF_BFP(mean_accel_ltp.x); + float y_accel_mean_f = ACCEL_FLOAT_OF_BFP(mean_accel_ltp.y); + INT32_RMAT_TRANSP_VMULT(mean_accel_ltp, booz_ahrs.ltp_to_body_rmat, mean_accel_body); + /* propagate horizontal filter */ + b2_hff_propagate(x_accel_mean_f, y_accel_mean_f); + /* update ins state from horizontal filter */ + booz_ins_ltp_accel.x = ACCEL_BFP_OF_REAL(b2_hff_xdotdot); + booz_ins_ltp_accel.y = ACCEL_BFP_OF_REAL(b2_hff_ydotdot); + booz_ins_ltp_speed.x = SPEED_BFP_OF_REAL(b2_hff_xdot); + booz_ins_ltp_speed.y = SPEED_BFP_OF_REAL(b2_hff_ydot); + booz_ins_ltp_pos.x = POS_BFP_OF_REAL(b2_hff_x); + booz_ins_ltp_pos.y = POS_BFP_OF_REAL(b2_hff_y); + } + } else { + b2_hff_ps_counter++; } #endif /* USE_HFF */ INT32_VECT3_ENU_OF_NED(booz_ins_enu_pos, booz_ins_ltp_pos); @@ -155,7 +168,6 @@ void booz_ins_propagate() { } void booz_ins_update_baro() { - #ifdef USE_VFF if (booz2_analog_baro_status == BOOZ2_ANALOG_BARO_RUNNING) { if (!booz_ins_baro_initialised) { diff --git a/sw/airborne/booz/ins/booz2_hf_float.c b/sw/airborne/booz/ins/booz2_hf_float.c index 756f330002..fd12ab1fe1 100644 --- a/sw/airborne/booz/ins/booz2_hf_float.c +++ b/sw/airborne/booz/ins/booz2_hf_float.c @@ -31,14 +31,21 @@ X_y = [ y ydot ybias ] */ + +/* horizontal filter propagation frequency */ +#define HFF_FREQ 512./HFF_PRESCALER +#define DT_HFILTER 1./HFF_FREQ + /* initial covariance diagonal */ #define INIT_PXX 1. /* process noise (is the same for x and y)*/ #define ACCEL_NOISE 0.5 -#define Q ACCEL_NOISE/512./512./2. -#define Qdotdot ACCEL_NOISE/512. -#define Qbiasbias 1e-7 -#define R 1. +#define Q ACCEL_NOISE*DT_HFILTER*DT_HFILTER/2. +#define Qdotdot ACCEL_NOISE*DT_HFILTER +#define Qbiasbias 1e-7*HFF_PRESCALER +//TODO: proper measurement noise +#define Rpos 1. +#define Rspeed 1. float b2_hff_x; float b2_hff_xbias; @@ -206,7 +213,7 @@ static inline void b2_hff_update_x(float x_meas) { b2_hff_x_meas = x_meas; const float y = x_meas - b2_hff_x; - const float S = b2_hff_xP[0][0] + R; + const float S = b2_hff_xP[0][0] + Rpos; const float K1 = b2_hff_xP[0][0] * 1/S; const float K2 = b2_hff_xP[1][0] * 1/S; const float K3 = b2_hff_xP[2][0] * 1/S; @@ -241,7 +248,7 @@ static inline void b2_hff_update_y(float y_meas) { b2_hff_y_meas = y_meas; const float y = y_meas - b2_hff_y; - const float S = b2_hff_yP[0][0] + R; + const float S = b2_hff_yP[0][0] + Rpos; const float K1 = b2_hff_yP[0][0] * 1/S; const float K2 = b2_hff_yP[1][0] * 1/S; const float K3 = b2_hff_yP[2][0] * 1/S; @@ -296,7 +303,7 @@ void b2_hff_update_v(float xspeed, float yspeed) { static inline void b2_hff_update_xdot(float v) { const float yd = v - b2_hff_xdot; - const float S = b2_hff_xP[1][1] + R; + const float S = b2_hff_xP[1][1] + Rspeed; const float K1 = b2_hff_xP[0][1] * 1/S; const float K2 = b2_hff_xP[1][1] * 1/S; const float K3 = b2_hff_xP[2][1] * 1/S; @@ -329,7 +336,7 @@ static inline void b2_hff_update_xdot(float v) { static inline void b2_hff_update_ydot(float v) { const float yd = v - b2_hff_ydot; - const float S = b2_hff_yP[1][1] + R; + const float S = b2_hff_yP[1][1] + Rspeed; const float K1 = b2_hff_yP[0][1] * 1/S; const float K2 = b2_hff_yP[1][1] * 1/S; const float K3 = b2_hff_yP[2][1] * 1/S; diff --git a/sw/airborne/booz/ins/booz2_hf_float.h b/sw/airborne/booz/ins/booz2_hf_float.h index bb3f9979b4..76b3c21b15 100644 --- a/sw/airborne/booz/ins/booz2_hf_float.h +++ b/sw/airborne/booz/ins/booz2_hf_float.h @@ -26,6 +26,10 @@ #define B2_HFF_STATE_SIZE 3 +#ifndef HFF_PRESCALER +#define HFF_PRESCALER 16 +#endif + #define B2_HFF_UPDATE_SPEED #define B2_HFF_UPDATE_POS