Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-01 04:46:51 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

ahrs accel buffer fix

Browse Source
This commit is contained in:
Felix Ruess
2009-08-28 20:12:13 +00:00
parent da4d417c16
commit 638193fad6
1 changed files with 22 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/booz/booz_ahrs.c
+22 -23
View File
@@ -28,7 +28,6 @@ struct BoozAhrs booz_ahrs;
struct BoozAhrsFloat booz_ahrs_float;
#define RB_MAXN 64
#define RB_TOP RB_MAXN - 1
struct Int32Vect3 accel_buf[RB_MAXN];
struct Int32Vect3 booz_ahrs_accel_mean;
@@ -38,37 +37,37 @@ 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;
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_TOP ? rb_w + 1 : 0;
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++;
}
/* 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;
struct Int32Vect3 sum;
int i, j;
INT_VECT3_ZERO(sum);
INT_VECT3_ZERO(sum);
if (n > rb_n) {
n = rb_n;
}
for (i = 0; i < n; i++) {
j = (rb_r + i) < RB_TOP ? rb_r + i : rb_r + i - RB_TOP;
VECT3_ADD(sum, accel_buf[j]);
}
VECT3_SDIV(booz_ahrs_accel_mean, sum, n);
if (n > rb_n) {
n = rb_n;
}
for (i = 0; i < n; i++) {
j = (rb_r + i) < RB_MAXN ? rb_r + i : rb_r + i - RB_MAXN;
VECT3_ADD(sum, accel_buf[j]);
}
VECT3_SDIV(booz_ahrs_accel_mean, sum, n);
}