[paparazzi-commits] [4801] fixes for new telemetry and new imu callbacks

[antoine drouin]

Revision: 4801
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4801
Author:   poine
Date:     2010-04-16 18:15:24 +0000 (Fri, 16 Apr 2010)
Log Message:
-----------
fixes for new telemetry and new imu callbacks

Modified Paths:
--------------
    paparazzi3/trunk/sw/airborne/booz/test/booz2_test_imu_b2.c

Modified: paparazzi3/trunk/sw/airborne/booz/test/booz2_test_imu_b2.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/test/booz2_test_imu_b2.c  2010-04-16 
18:14:39 UTC (rev 4800)
+++ paparazzi3/trunk/sw/airborne/booz/test/booz2_test_imu_b2.c  2010-04-16 
18:15:24 UTC (rev 4801)
@@ -31,8 +31,7 @@
 #include "messages.h"
 #include "downlink.h"
 
-#include "booz2_imu.h"
-#include "booz2_imu_b2.h"
+#include "booz_imu.h"
 
 #include "interrupt_hw.h"
 
@@ -41,7 +40,8 @@
 static inline void main_periodic_task( void );
 static inline void main_event_task( void );
 
-static inline void on_imu_event(void);
+static inline void on_gyro_accel_event(void);
+static inline void on_mag_event(void);
 
 int main( void ) {
   main_init();
@@ -54,35 +54,32 @@
 }
 
 static inline void main_init( void ) {
+
   hw_init();
   sys_time_init();
-  led_init();
-  uart1_init_tx();
-  booz2_imu_impl_init();
-  booz2_imu_init();
+  booz_imu_init();
 
   int_enable();
 }
 
 static inline void main_periodic_task( void ) {
-  //#if 0
   RunOnceEvery(100, {
-    LED_TOGGLE(3);
-    DOWNLINK_SEND_ALIVE(16, MD5SUM);
-  });
-  booz2_imu_periodic();
-
+      LED_TOGGLE(3);
+      DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);
+    });
+  booz_imu_periodic();
+  RunOnceEvery(10, { LED_PERIODIC();});
 }
 
 static inline void main_event_task( void ) {
 
-  Booz2ImuEvent(on_imu_event);
+  BoozImuEvent(on_gyro_accel_event, on_mag_event);
 
 }
 
-static inline void on_imu_event(void) {
-  Booz2ImuScaleGyro();
-  Booz2ImuScaleAccel();
+static inline void on_gyro_accel_event(void) {
+  BoozImuScaleGyro();
+  BoozImuScaleAccel();
 
   LED_TOGGLE(2);
   static uint8_t cnt;
@@ -90,21 +87,46 @@
   if (cnt > 15) cnt = 0;
 
   if (cnt == 0) {
-    DOWNLINK_SEND_IMU_GYRO_RAW(&booz_imu.gyro_unscaled.p,
+    DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel,
+                              &booz_imu.gyro_unscaled.p,
                               &booz_imu.gyro_unscaled.q,
                               &booz_imu.gyro_unscaled.r);
     
-    DOWNLINK_SEND_IMU_ACCEL_RAW(&booz_imu.accel_unscaled.x,
+    DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel,
+                               &booz_imu.accel_unscaled.x,
                                &booz_imu.accel_unscaled.y,
                                &booz_imu.accel_unscaled.z);
   }
   else if (cnt == 7) {
-    DOWNLINK_SEND_BOOZ2_GYRO(&booz_imu.gyro.p,
+    DOWNLINK_SEND_BOOZ2_GYRO(DefaultChannel,
+                            &booz_imu.gyro.p,
                             &booz_imu.gyro.q,
                             &booz_imu.gyro.r);
     
-    DOWNLINK_SEND_BOOZ2_ACCEL(&booz_imu.accel.x,
+    DOWNLINK_SEND_BOOZ2_ACCEL(DefaultChannel,
+                             &booz_imu.accel.x,
                              &booz_imu.accel.y,
                              &booz_imu.accel.z);
-  }  
+  }
 }
+
+
+static inline void on_mag_event(void) {
+  BoozImuScaleMag();
+  static uint8_t cnt;
+  cnt++;
+  if (cnt > 1) cnt = 0;
+
+  if (cnt%2) {
+    DOWNLINK_SEND_BOOZ2_MAG(DefaultChannel,
+                           &booz_imu.mag.x,
+                           &booz_imu.mag.y,
+                           &booz_imu.mag.z);
+  }
+  else {
+    DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel,
+                             &booz_imu.mag_unscaled.x,
+                             &booz_imu.mag_unscaled.y,
+                             &booz_imu.mag_unscaled.z);
+  }
+}