[paparazzi-commits] [4021] ahrs accel buffer fix

[Felix Ruess]

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);
 }