[paparazzi-commits] [4649] hff: rotate accel from IMU to BODY before storing it

[Felix Ruess]

Revision: 4649
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4649
Author:   flixr
Date:     2010-03-09 18:19:53 +0000 (Tue, 09 Mar 2010)
Log Message:
-----------
hff: rotate accel from IMU to BODY before storing it

Modified Paths:
--------------
    paparazzi3/trunk/sw/airborne/booz/booz2_ins.c
    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-03-09 17:35:48 UTC 
(rev 4648)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_ins.c       2010-03-09 18:19:53 UTC 
(rev 4649)
@@ -148,7 +148,7 @@
 #endif /* USE_VFF */
 
 #ifdef USE_HFF
-  b2_hff_store_accel();
+  b2_hff_store_accel_body();
   /* propagate horizontal filter */
   b2_hff_propagate();
   if ( booz_ins_ltp_initialised ) {

Modified: paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float.c      2010-03-09 
17:35:48 UTC (rev 4648)
+++ paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float.c      2010-03-09 
18:19:53 UTC (rev 4649)
@@ -108,10 +108,10 @@
   uint8_t size;
 };
 struct AccBuf acc_body;
-struct Int32Vect3 acc_mean;
+struct Int32Vect3 acc_body_mean;
 
-void b2_hff_store_accel(void) {
-  VECT3_COPY(acc_body.buf[acc_body.w], booz_imu.accel);
+void b2_hff_store_accel_body(void) {
+  INT32_RMAT_TRANSP_VMULT(acc_body.buf[acc_body.w], booz_imu.body_to_imu_rmat, 
 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 */
@@ -123,23 +123,23 @@
 }
 
 /* compute the mean of the last n accel measurements */
-static inline void b2_hff_compute_accel_mean(uint8_t n) {
+static inline void b2_hff_compute_accel_body_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);
+    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]);
+    }
+       VECT3_SDIV(acc_body_mean, sum, n);
   } else {
-       VECT3_COPY(acc_mean, sum);
+       VECT3_COPY(acc_body_mean, acc_body.buf[acc_body.w - 1]);
   }
 }
 
@@ -434,11 +434,9 @@
 
     if (b2_hff_lost_counter < b2_hff_lost_limit) {
       /* compute float ltp mean acceleration */
-      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, 
acc_mean);
+      b2_hff_compute_accel_body_mean(HFF_PRESCALER);
       struct Int32Vect3 mean_accel_ltp;
-      INT32_RMAT_TRANSP_VMULT(mean_accel_ltp, booz_ahrs.ltp_to_body_rmat, 
mean_accel_body);
+      INT32_RMAT_TRANSP_VMULT(mean_accel_ltp, booz_ahrs.ltp_to_body_rmat, 
acc_body_mean);
       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

Modified: paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float.h      2010-03-09 
17:35:48 UTC (rev 4648)
+++ paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float.h      2010-03-09 
18:19:53 UTC (rev 4649)
@@ -74,7 +74,7 @@
 extern uint16_t b2_hff_lost_limit;
 extern uint16_t b2_hff_lost_counter;
 
-extern void b2_hff_store_accel(void);
+extern void b2_hff_store_accel_body(void);
 
 extern struct HfilterFloat *b2_hff_rb_last;
 extern int lag_counter_err;