[airborne] also publish imu data via ABI

paparazzi · Dec 17, 2014 · 6b979d3 · 6b979d3
1 parent 603b40a
commit 6b979d3
Show file tree

Hide file tree

Showing 3 changed files with 48 additions and 13 deletions.
diff --git a/conf/abi.xml b/conf/abi.xml
@@ -20,6 +20,21 @@
       <field name="temp" type="float" unit="deg Celcius"/>
     </message>
 
+    <message name="IMU_GYRO_INT32" id="4">
+      <field name="stamp" type="uint32_t" unit="us"/>
+      <field name="gyro" type="struct Int32Rates"/>
+    </message>
+
+    <message name="IMU_ACCEL_INT32" id="5">
+      <field name="stamp" type="uint32_t" unit="us"/>
+      <field name="accel" type="struct Int32Vect3"/>
+    </message>
+
+    <message name="IMU_MAG_INT32" id="6">
+      <field name="stamp" type="uint32_t" unit="us"/>
+      <field name="mag" type="struct Int32Vect3"/>
+    </message>
+
   </msg_class>
 
 

diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c
@@ -750,12 +750,13 @@ static inline void on_gps_solution(void)
 #if USE_IMU
 static inline void on_accel_event(void)
 {
+  // current timestamp
+  uint32_t now_ts = get_sys_time_usec();
+
 #if USE_AUTO_AHRS_FREQ || !defined(AHRS_CORRECT_FREQUENCY)
   PRINT_CONFIG_MSG("Calculating dt for AHRS accel update.")
   // timestamp in usec when last callback was received
   static uint32_t last_ts = 0;
-  // current timestamp
-  uint32_t now_ts = get_sys_time_usec();
   // dt between this and last callback in seconds
   float dt = (float)(now_ts - last_ts) / 1e6;
   last_ts = now_ts;
@@ -766,19 +767,23 @@ static inline void on_accel_event(void)
 #endif
 
   imu_scale_accel(&imu);
+
+  AbiSendMsgIMU_ACCEL_INT32(1, &now_ts, &imu.accel);
+
   if (ahrs.status != AHRS_UNINIT) {
     ahrs_update_accel(dt);
   }
 }
 
 static inline void on_gyro_event(void)
 {
+  // current timestamp
+  uint32_t now_ts = get_sys_time_usec();
+
 #if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
   PRINT_CONFIG_MSG("Calculating dt for AHRS/INS propagation.")
   // timestamp in usec when last callback was received
   static uint32_t last_ts = 0;
-  // current timestamp
-  uint32_t now_ts = get_sys_time_usec();
   // dt between this and last callback in seconds
   float dt = (float)(now_ts - last_ts) / 1e6;
   last_ts = now_ts;
@@ -792,6 +797,8 @@ static inline void on_gyro_event(void)
 
   imu_scale_gyro(&imu);
 
+  AbiSendMsgIMU_GYRO_INT32(1, &now_ts, &imu.gyro);
+
 #if USE_AHRS_ALIGNER
   // Run aligner on raw data as it also makes averages.
   if (ahrs.status == AHRS_UNINIT) {
@@ -817,13 +824,14 @@ static inline void on_gyro_event(void)
 
 static inline void on_mag_event(void)
 {
+  // current timestamp
+  uint32_t now_ts = get_sys_time_usec();
+
 #if USE_MAGNETOMETER
 #if USE_AUTO_AHRS_FREQ || !defined(AHRS_MAG_CORRECT_FREQUENCY)
   PRINT_CONFIG_MSG("Calculating dt for AHRS mag update.")
   // timestamp in usec when last callback was received
   static uint32_t last_ts = 0;
-  // current timestamp
-  uint32_t now_ts = get_sys_time_usec();
   // dt between this and last callback in seconds
   float dt = (float)(now_ts - last_ts) / 1e6;
   last_ts = now_ts;
@@ -834,6 +842,9 @@ static inline void on_mag_event(void)
 #endif
 
   imu_scale_mag(&imu);
+
+  AbiSendMsgIMU_MAG_INT32(1, &now_ts, &imu.mag);
+
   if (ahrs.status == AHRS_RUNNING) {
     ahrs_update_mag(dt);
   }

diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c
@@ -335,12 +335,13 @@ STATIC_INLINE void main_event(void)
 
 static inline void on_accel_event(void)
 {
+  // current timestamp
+  uint32_t now_ts = get_sys_time_usec();
+
 #if USE_AUTO_AHRS_FREQ || !defined(AHRS_CORRECT_FREQUENCY)
   PRINT_CONFIG_MSG("Calculating dt for AHRS accel update.")
   // timestamp in usec when last callback was received
   static uint32_t last_ts = 0;
-  // current timestamp
-  uint32_t now_ts = get_sys_time_usec();
   // dt between this and last callback
   float dt = (float)(now_ts - last_ts) / 1e6;
   last_ts = now_ts;
@@ -352,19 +353,22 @@ static inline void on_accel_event(void)
 
   imu_scale_accel(&imu);
 
+  AbiSendMsgIMU_ACCEL_INT32(1, &now_ts, &imu.accel);
+
   if (ahrs.status != AHRS_UNINIT) {
     ahrs_update_accel(dt);
   }
 }
 
 static inline void on_gyro_event(void)
 {
+  // current timestamp
+  uint32_t now_ts = get_sys_time_usec();
+
 #if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
   PRINT_CONFIG_MSG("Calculating dt for AHRS/INS propagation.")
   // timestamp in usec when last callback was received
   static uint32_t last_ts = 0;
-  // current timestamp
-  uint32_t now_ts = get_sys_time_usec();
   // dt between this and last callback in seconds
   float dt = (float)(now_ts - last_ts) / 1e6;
   last_ts = now_ts;
@@ -376,6 +380,8 @@ static inline void on_gyro_event(void)
 
   imu_scale_gyro(&imu);
 
+  AbiSendMsgIMU_GYRO_INT32(1, &now_ts, &imu.gyro);
+
   if (ahrs.status == AHRS_UNINIT) {
     ahrs_aligner_run();
     if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED) {
@@ -406,15 +412,14 @@ static inline void on_gps_event(void)
 
 static inline void on_mag_event(void)
 {
-  imu_scale_mag(&imu);
+  // current timestamp
+  uint32_t now_ts = get_sys_time_usec();
 
 #if USE_MAGNETOMETER
 #if USE_AUTO_AHRS_FREQ || !defined(AHRS_MAG_CORRECT_FREQUENCY)
   PRINT_CONFIG_MSG("Calculating dt for AHRS mag update.")
   // timestamp in usec when last callback was received
   static uint32_t last_ts = 0;
-  // current timestamp
-  uint32_t now_ts = get_sys_time_usec();
   // dt between this and last callback in seconds
   float dt = (float)(now_ts - last_ts) / 1e6;
   last_ts = now_ts;
@@ -424,6 +429,10 @@ static inline void on_mag_event(void)
   const float dt = 1. / (AHRS_MAG_CORRECT_FREQUENCY);
 #endif
 
+ imu_scale_mag(&imu);
+
+ AbiSendMsgIMU_MAG_INT32(1, &now_ts, &imu.mag);
+
   if (ahrs.status == AHRS_RUNNING) {
     ahrs_update_mag(dt);
   }