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

run hff at a lower frequency (default 32Hz). define HFF_PRESCALER=<prescaler> in airframe file to change

Browse Source
This commit is contained in:
Felix Ruess
2009-08-21 08:25:25 +00:00
parent 47398d6a9c
commit c3b69389d1
5 changed files with 49 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/booz2_flixr.xml
+1 -1
View File
@@ -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
</makefile>
conf/autopilot/booz2_autopilot.makefile
-1
View File
@@ -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
sw/airborne/booz/booz2_ins.c
+29 -17
View File
@@ -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) {
sw/airborne/booz/ins/booz2_hf_float.c
+15 -8
View File
@@ -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;
sw/airborne/booz/ins/booz2_hf_float.h
+4
View File
@@ -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