From 6b979d307b7e88786a0e6d544dd9c0ae359e6741 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Thu, 27 Nov 2014 18:01:58 +0100 Subject: [PATCH] [airborne] also publish imu data via ABI --- conf/abi.xml | 15 +++++++++++++++ sw/airborne/firmwares/fixedwing/main_ap.c | 23 +++++++++++++++++------ sw/airborne/firmwares/rotorcraft/main.c | 23 ++++++++++++++++------- 3 files changed, 48 insertions(+), 13 deletions(-) diff --git a/conf/abi.xml b/conf/abi.xml index 536fce48e85..6d29c3722d3 100644 --- a/conf/abi.xml +++ b/conf/abi.xml @@ -20,6 +20,21 @@ + + + + + + + + + + + + + + + diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c index a599236aff9..d9e061a8215 100644 --- 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,6 +767,9 @@ 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); } @@ -773,12 +777,13 @@ static inline void on_accel_event(void) 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 index 44dee0a798a..a9f5ba12a92 100644 --- 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,6 +353,8 @@ 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); } @@ -359,12 +362,13 @@ static inline void on_accel_event(void) 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); }