diff --git a/sw/airborne/arch/sim/subsystems/imu/imu_aspirin_arch.c b/sw/airborne/arch/sim/subsystems/imu/imu_aspirin_arch.c index d14f1f9104..9d2192f553 100644 --- a/sw/airborne/arch/sim/subsystems/imu/imu_aspirin_arch.c +++ b/sw/airborne/arch/sim/subsystems/imu/imu_aspirin_arch.c @@ -26,7 +26,7 @@ #include "generated/airframe.h" void imu_aspirin_arch_init(void) { - + imu_aspirin.status = AspirinStatusIdle; } @@ -35,7 +35,7 @@ void imu_aspirin_arch_init(void) { void imu_feed_gyro_accel(void) { -#if 1 +#if 0 // the high byte is in buf[0], low byte in buf[1], etc. imu_aspirin.i2c_trans_gyro.buf[0] = ((int16_t)sensors.gyro.value.x) >> 8; imu_aspirin.i2c_trans_gyro.buf[1] = ((int16_t)sensors.gyro.value.x) & 0xff; diff --git a/sw/airborne/subsystems/imu/imu_aspirin.h b/sw/airborne/subsystems/imu/imu_aspirin.h index a00c60371b..275e7c691b 100644 --- a/sw/airborne/subsystems/imu/imu_aspirin.h +++ b/sw/airborne/subsystems/imu/imu_aspirin.h @@ -234,8 +234,22 @@ static inline void imu_aspirin_event(void (* _gyro_handler)(void), void (* _acce } +#ifndef SITL #define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) do { \ imu_aspirin_event(_gyro_handler, _accel_handler, _mag_handler); \ } while(0); +#else +#define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) { \ + if (imu_aspirin.accel_available) { \ + imu_aspirin.accel_available = FALSE; \ + _accel_handler(); \ + } \ + if (imu_aspirin.gyro_available_blaaa) { \ + imu_aspirin.gyro_available_blaaa = FALSE; \ + _gyro_handler(); \ + } \ + ImuMagEvent(_mag_handler); \ + } +#endif #endif /* IMU_ASPIRIN_H */