[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [4021] ahrs accel buffer fix
From: |
Felix Ruess |
Subject: |
[paparazzi-commits] [4021] ahrs accel buffer fix |
Date: |
Fri, 28 Aug 2009 20:12:14 +0000 |
Revision: 4021
http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4021
Author: flixr
Date: 2009-08-28 20:12:13 +0000 (Fri, 28 Aug 2009)
Log Message:
-----------
ahrs accel buffer fix
Modified Paths:
--------------
paparazzi3/trunk/sw/airborne/booz/booz_ahrs.c
Modified: paparazzi3/trunk/sw/airborne/booz/booz_ahrs.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz_ahrs.c 2009-08-28 19:12:16 UTC
(rev 4020)
+++ paparazzi3/trunk/sw/airborne/booz/booz_ahrs.c 2009-08-28 20:12:13 UTC
(rev 4021)
@@ -28,7 +28,6 @@
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_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);
}
[Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [4021] ahrs accel buffer fix,
Felix Ruess <=