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 27 Packages Projects Releases Wiki Activity

cleanup ahrs a little, move accel mean computation to horizontal filter

Browse Source
This commit is contained in:
Felix Ruess
2010-02-18 23:29:54 +00:00
parent 6ec1362ec3
commit 19b9d727ed
6 changed files with 71 additions and 77 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/booz/booz2_ins.c
+11 -12
View File
@@ -148,18 +148,17 @@ void booz_ins_propagate() {
#endif /* USE_VFF */
#ifdef USE_HFF
if (booz_ahrs.status == BOOZ_AHRS_RUNNING ) {
/* propagate horizontal filter */
b2_hff_propagate();
if ( booz_ins_ltp_initialised ) {
/* update ins state from horizontal filter */
booz_ins_ltp_accel.x = ACCEL_BFP_OF_REAL(b2_hff_state.xdotdot);
booz_ins_ltp_accel.y = ACCEL_BFP_OF_REAL(b2_hff_state.ydotdot);
booz_ins_ltp_speed.x = SPEED_BFP_OF_REAL(b2_hff_state.xdot);
booz_ins_ltp_speed.y = SPEED_BFP_OF_REAL(b2_hff_state.ydot);
booz_ins_ltp_pos.x = POS_BFP_OF_REAL(b2_hff_state.x);
booz_ins_ltp_pos.y = POS_BFP_OF_REAL(b2_hff_state.y);
}
b2_hff_store_accel();
/* propagate horizontal filter */
b2_hff_propagate();
if ( booz_ins_ltp_initialised ) {
/* update ins state from horizontal filter */
booz_ins_ltp_accel.x = ACCEL_BFP_OF_REAL(b2_hff_state.xdotdot);
booz_ins_ltp_accel.y = ACCEL_BFP_OF_REAL(b2_hff_state.ydotdot);
booz_ins_ltp_speed.x = SPEED_BFP_OF_REAL(b2_hff_state.xdot);
booz_ins_ltp_speed.y = SPEED_BFP_OF_REAL(b2_hff_state.ydot);
booz_ins_ltp_pos.x = POS_BFP_OF_REAL(b2_hff_state.x);
booz_ins_ltp_pos.y = POS_BFP_OF_REAL(b2_hff_state.y);
}
#else
booz_ins_ltp_accel.x = accel_ltp.x;
sw/airborne/booz/booz2_main.c
-3
View File
@@ -123,7 +123,6 @@ STATIC_INLINE void booz2_main_init( void ) {
booz_ahrs_aligner_init();
booz_ahrs_init();
booz_ahrs_init_accel_rb();
booz_ins_init();
@@ -249,8 +248,6 @@ static inline void on_gyro_accel_event( void ) {
BoozImuScaleGyro();
BoozImuScaleAccel();
booz_ahrs_store_accel();
if (booz_ahrs.status == BOOZ_AHRS_UNINIT) {
booz_ahrs_aligner_run();
if (booz_ahrs_aligner.status == BOOZ_AHRS_ALIGNER_LOCKED)
sw/airborne/booz/booz_ahrs.c
-49
View File
@@ -26,52 +26,3 @@
struct BoozAhrs booz_ahrs;
struct BoozAhrsFloat booz_ahrs_float;
#define RB_MAXN 64
struct Int32Vect3 accel_buf[RB_MAXN];
struct Int32Vect3 booz_ahrs_accel_mean;
uint8_t rb_r; /* pos to read from, oldest measurement */
uint8_t rb_w; /* pos to write to */
uint8_t rb_n; /* number of elements in rb */
void booz_ahrs_init_accel_rb(void) {
rb_r = 0;
rb_w = 0;
rb_n = 0;
}
void booz_ahrs_store_accel(void) {
VECT3_COPY(accel_buf[rb_w], booz_imu.accel);
rb_w = (rb_w + 1) < RB_MAXN ? (rb_w + 1) : 0;
/* once the buffer is full it always has the last RB_MAXN accel measurements */
if (rb_n < RB_MAXN) {
rb_n++;
} else {
rb_r = (rb_r + 1) < RB_MAXN ? (rb_r + 1) : 0;
}
}
/* compute the mean of the last n accel measurements */
void booz_ahrs_compute_accel_mean(uint8_t n) {
struct Int32Vect3 sum;
int i, j;
INT_VECT3_ZERO(sum);
if (n > rb_n) {
n = rb_n;
}
for (i = 1; i <= n; i++) {
j = (rb_w - i) > 0 ? rb_w - i : rb_w - i + RB_MAXN;
VECT3_ADD(sum, accel_buf[j]);
}
if (n > 1) {
VECT3_SDIV(booz_ahrs_accel_mean, sum, n);
} else {
VECT3_COPY(booz_ahrs_accel_mean, sum);
}
}
sw/airborne/booz/booz_ahrs.h
-5
View File
@@ -57,7 +57,6 @@ struct BoozAhrsFloat {
extern struct BoozAhrs booz_ahrs;
extern struct BoozAhrsFloat booz_ahrs_float;
extern struct Int32Vect3 booz_ahrs_accel_mean;
#define BOOZ_AHRS_FLOAT_OF_INT32() { \
QUAT_FLOAT_OF_BFP(booz_ahrs_float.ltp_to_body_quat, booz_ahrs.ltp_to_body_quat); \
@@ -74,8 +73,4 @@ extern void booz_ahrs_update(void);
extern void booz_ahrs_update_accel(void);
extern void booz_ahrs_update_mag(void);
extern void booz_ahrs_init_accel_rb(void);
extern void booz_ahrs_store_accel(void);
extern void booz_ahrs_compute_accel_mean(uint8_t n);
#endif /* BOOZ_AHRS_H */
sw/airborne/booz/ins/booz2_hf_float.c
+58 -5
View File
@@ -82,6 +82,53 @@ float b2_hff_y_meas;
int b2_hff_ps_counter;
/*
* accel(in body frame) buffer
*/
#define ACC_RB_MAXN 64
struct AccBuf {
struct Int32Vect3 buf[ACC_RB_MAXN];
uint8_t r; /* pos to read from, oldest measurement */
uint8_t w; /* pos to write to */
uint8_t n; /* number of elements in rb */
uint8_t size;
};
struct AccBuf acc_body;
struct Int32Vect3 acc_mean;
void b2_hff_store_accel(void) {
VECT3_COPY(acc_body.buf[acc_body.w], booz_imu.accel);
acc_body.w = (acc_body.w + 1) < acc_body.size ? (acc_body.w + 1) : 0;
/* once the buffer is full it always has the last acc_body.size accel measurements */
if (acc_body.n < acc_body.size) {
acc_body.n++;
} else {
acc_body.r = (acc_body.r + 1) < acc_body.size ? (acc_body.r + 1) : 0;
}
}
/* compute the mean of the last n accel measurements */
static inline void b2_hff_compute_accel_mean(uint8_t n) {
struct Int32Vect3 sum;
int i, j;
INT_VECT3_ZERO(sum);
if (n > acc_body.n) {
n = acc_body.n;
}
for (i = 1; i <= n; i++) {
j = (acc_body.w - i) > 0 ? acc_body.w - i : acc_body.w - i + acc_body.size;
VECT3_ADD(sum, acc_body.buf[j]);
}
if (n > 1) {
VECT3_SDIV(acc_mean, sum, n);
} else {
VECT3_COPY(acc_mean, sum);
}
}
/*
* For GPS lag compensation
*
@@ -107,7 +154,7 @@ int b2_hff_ps_counter;
*/
#define MAX_PP_STEPS 6
/* variables for accel buffer */
/* variables for mean accel buffer */
#define ACC_BUF_MAXN (GPS_LAG_N+10)
#define INC_ACC_IDX(idx) { idx = (idx + 1) < ACC_BUF_MAXN ? (idx + 1) : 0; }
@@ -177,7 +224,13 @@ void b2_hff_init(float init_x, float init_xdot, float init_y, float init_ydot) {
Rspeed = R_SPEED;
b2_hff_init_x(init_x, init_xdot);
b2_hff_init_y(init_y, init_ydot);
/* init buffer for mean accel calculation */
acc_body.r = 0;
acc_body.w = 0;
acc_body.n = 0;
acc_body.size = ACC_RB_MAXN;
#ifdef GPS_LAG
/* init buffer for past mean accel values */
acc_buf_r = 0;
acc_buf_w = 0;
acc_buf_n = 0;
@@ -229,7 +282,7 @@ static inline void b2_hff_init_y(float init_y, float init_ydot) {
}
#ifdef GPS_LAG
void b2_hff_store_accel(float x, float y) {
static inline void b2_hff_store_accel_ltp(float x, float y) {
past_accel[acc_buf_w].x = x;
past_accel[acc_buf_w].y = y;
INC_ACC_IDX(acc_buf_w);
@@ -365,15 +418,15 @@ void b2_hff_propagate(void) {
if (gps_lost_counter < GPS_LOST_LIMIT) {
/* compute float ltp mean acceleration */
booz_ahrs_compute_accel_mean(HFF_PRESCALER);
b2_hff_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);
INT32_RMAT_TRANSP_VMULT(mean_accel_body, booz_imu.body_to_imu_rmat, acc_mean);
struct Int32Vect3 mean_accel_ltp;
INT32_RMAT_TRANSP_VMULT(mean_accel_ltp, booz_ahrs.ltp_to_body_rmat, mean_accel_body);
b2_hff_xdd_meas = ACCEL_FLOAT_OF_BFP(mean_accel_ltp.x);
b2_hff_ydd_meas = ACCEL_FLOAT_OF_BFP(mean_accel_ltp.y);
#ifdef GPS_LAG
b2_hff_store_accel(b2_hff_xdd_meas, b2_hff_ydd_meas);
b2_hff_store_accel_ltp(b2_hff_xdd_meas, b2_hff_ydd_meas);
#endif
/*
sw/airborne/booz/ins/booz2_hf_float.h
+2 -3
View File
@@ -70,9 +70,8 @@ extern void b2_hff_update_pos(float xpos, float ypos);
extern void b2_hff_update_v(float xspeed, float yspeed);
extern void b2_hff_realign(struct FloatVect2 pos, struct FloatVect2 speed);
#ifdef GPS_LAG
extern void b2_hff_store_accel(float x, float y);
#endif
extern void b2_hff_store_accel(void);
extern struct HfilterFloat *b2_hff_rb_last;
extern int lag_counter_err;
extern int save_counter;