void imu_mpu9250_event(void) { uint32_t now_ts = get_sys_time_usec(); // If the MPU9250 I2C transaction has succeeded: convert the data mpu9250_i2c_event(&imu_mpu9250.mpu); if (imu_mpu9250.mpu.data_available) { // set channel order struct Int32Vect3 accel = { IMU_MPU9250_X_SIGN *(int32_t)(imu_mpu9250.mpu.data_accel.value[IMU_MPU9250_CHAN_X]), IMU_MPU9250_Y_SIGN *(int32_t)(imu_mpu9250.mpu.data_accel.value[IMU_MPU9250_CHAN_Y]), IMU_MPU9250_Z_SIGN *(int32_t)(imu_mpu9250.mpu.data_accel.value[IMU_MPU9250_CHAN_Z]) }; struct Int32Rates rates = { IMU_MPU9250_X_SIGN *(int32_t)(imu_mpu9250.mpu.data_rates.value[IMU_MPU9250_CHAN_X]), IMU_MPU9250_Y_SIGN *(int32_t)(imu_mpu9250.mpu.data_rates.value[IMU_MPU9250_CHAN_Y]), IMU_MPU9250_Z_SIGN *(int32_t)(imu_mpu9250.mpu.data_rates.value[IMU_MPU9250_CHAN_Z]) }; // unscaled vector VECT3_COPY(imu.accel_unscaled, accel); RATES_COPY(imu.gyro_unscaled, rates); imu_mpu9250.mpu.data_available = false; imu_scale_gyro(&imu); imu_scale_accel(&imu); AbiSendMsgIMU_GYRO_INT32(IMU_MPU9250_ID, now_ts, &imu.gyro); AbiSendMsgIMU_ACCEL_INT32(IMU_MPU9250_ID, now_ts, &imu.accel); } #if IMU_MPU9250_READ_MAG // Test if mag data are updated if (imu_mpu9250.mpu.akm.data_available) { struct Int32Vect3 mag = { IMU_MPU9250_X_SIGN *(int32_t)(imu_mpu9250.mpu.akm.data.value[IMU_MPU9250_CHAN_Y]), IMU_MPU9250_Y_SIGN *(int32_t)(imu_mpu9250.mpu.akm.data.value[IMU_MPU9250_CHAN_X]), -IMU_MPU9250_Z_SIGN *(int32_t)(imu_mpu9250.mpu.akm.data.value[IMU_MPU9250_CHAN_Z]) }; VECT3_COPY(imu.mag_unscaled, mag); imu_mpu9250.mpu.akm.data_available = false; imu_scale_mag(&imu); AbiSendMsgIMU_MAG_INT32(IMU_MPU9250_ID, now_ts, &imu.mag); } #endif }
void imu_mpu9250_event(void) { // If the MPU9250 I2C transaction has succeeded: convert the data mpu9250_i2c_event(&imu_mpu9250.mpu); if (imu_mpu9250.mpu.data_available) { // set channel order struct Int32Vect3 accel = { (int32_t)(imu_mpu9250.mpu.data_accel.value[IMU_MPU9250_CHAN_X]), (int32_t)(imu_mpu9250.mpu.data_accel.value[IMU_MPU9250_CHAN_Y]), (int32_t)(imu_mpu9250.mpu.data_accel.value[IMU_MPU9250_CHAN_Z]) }; struct Int32Rates rates = { (int32_t)(imu_mpu9250.mpu.data_rates.value[IMU_MPU9250_CHAN_X]), (int32_t)(imu_mpu9250.mpu.data_rates.value[IMU_MPU9250_CHAN_Y]), (int32_t)(imu_mpu9250.mpu.data_rates.value[IMU_MPU9250_CHAN_Z]) }; // unscaled vector VECT3_COPY(imu.accel_unscaled, accel); RATES_COPY(imu.gyro_unscaled, rates); imu_mpu9250.mpu.data_available = FALSE; imu_mpu9250.gyro_valid = TRUE; imu_mpu9250.accel_valid = TRUE; } // Test if mag data are updated if (imu_mpu9250.mpu.akm.data_available) { struct Int32Vect3 mag = { (int32_t)(imu_mpu9250.mpu.akm.data.value[IMU_MPU9250_CHAN_Y]), (int32_t)(imu_mpu9250.mpu.akm.data.value[IMU_MPU9250_CHAN_X]), -(int32_t)(imu_mpu9250.mpu.akm.data.value[IMU_MPU9250_CHAN_Z]) }; VECT3_COPY(imu.mag_unscaled, mag); imu_mpu9250.mag_valid = TRUE; imu_mpu9250.mpu.akm.data_available = FALSE; } }