[paparazzi-commits] [4558] cleanup ahrs a little, move accel mean computation to horizontal filter

[Felix Ruess]

Revision: 4558
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4558
Author:   flixr
Date:     2010-02-18 23:29:54 +0000 (Thu, 18 Feb 2010)
Log Message:
-----------
cleanup ahrs a little, move accel mean computation to horizontal filter

Modified Paths:
--------------
    paparazzi3/trunk/sw/airborne/booz/booz2_ins.c
    paparazzi3/trunk/sw/airborne/booz/booz2_main.c
    paparazzi3/trunk/sw/airborne/booz/booz_ahrs.c
    paparazzi3/trunk/sw/airborne/booz/booz_ahrs.h
    paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float.c
    paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float.h

Modified: paparazzi3/trunk/sw/airborne/booz/booz2_ins.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_ins.c       2010-02-18 21:20:00 UTC 
(rev 4557)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_ins.c       2010-02-18 23:29:54 UTC 
(rev 4558)
@@ -148,18 +148,17 @@
 #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;

Modified: paparazzi3/trunk/sw/airborne/booz/booz2_main.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_main.c      2010-02-18 21:20:00 UTC 
(rev 4557)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_main.c      2010-02-18 23:29:54 UTC 
(rev 4558)
@@ -123,7 +123,6 @@
 
   booz_ahrs_aligner_init();
   booz_ahrs_init();
-  booz_ahrs_init_accel_rb();
 
   booz_ins_init();
 
@@ -249,8 +248,6 @@
   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)

Modified: paparazzi3/trunk/sw/airborne/booz/booz_ahrs.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz_ahrs.c       2010-02-18 21:20:00 UTC 
(rev 4557)
+++ paparazzi3/trunk/sw/airborne/booz/booz_ahrs.c       2010-02-18 23:29:54 UTC 
(rev 4558)
@@ -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);
-  }
-}
-

Modified: paparazzi3/trunk/sw/airborne/booz/booz_ahrs.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz_ahrs.h       2010-02-18 21:20:00 UTC 
(rev 4557)
+++ paparazzi3/trunk/sw/airborne/booz/booz_ahrs.h       2010-02-18 23:29:54 UTC 
(rev 4558)
@@ -57,7 +57,6 @@
 
 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_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 */

Modified: paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float.c      2010-02-18 
21:20:00 UTC (rev 4557)
+++ paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float.c      2010-02-18 
23:29:54 UTC (rev 4558)
@@ -83,6 +83,53 @@
 
 
 /*
+ * 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 @@
  */
 #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 @@
   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 @@
 }
 
 #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 @@
 
     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
 
       /*

Modified: paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float.h      2010-02-18 
21:20:00 UTC (rev 4557)
+++ paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float.h      2010-02-18 
23:29:54 UTC (rev 4558)
@@ -70,9 +70,8 @@
 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;