/** * Handle all the events of the Navstik IMU components. * When there is data available convert it to the correct axis and save it in the imu structure. */ void imu_bebop_event(void) { /* MPU-60x0 event taks */ mpu60x0_i2c_event(&imu_bebop.mpu); if (imu_bebop.mpu.data_available) { /* default orientation of the MPU is upside down sor corrigate this here */ RATES_ASSIGN(imu.gyro_unscaled, imu_bebop.mpu.data_rates.rates.p, -imu_bebop.mpu.data_rates.rates.q, -imu_bebop.mpu.data_rates.rates.r); VECT3_ASSIGN(imu.accel_unscaled, imu_bebop.mpu.data_accel.vect.x, -imu_bebop.mpu.data_accel.vect.y, -imu_bebop.mpu.data_accel.vect.z); imu_bebop.mpu.data_available = FALSE; imu_bebop.gyro_valid = TRUE; imu_bebop.accel_valid = TRUE; } /* AKM8963 event task */ ak8963_event(&imu_bebop.ak); if (imu_bebop.ak.data_available) { //32760 to -32760 VECT3_ASSIGN(imu.mag_unscaled, -imu_bebop.ak.data.vect.y, imu_bebop.ak.data.vect.x, imu_bebop.ak.data.vect.z); imu_bebop.ak.data_available = FALSE; imu_bebop.mag_valid = TRUE; } }
void mpu9250_i2c_event(struct Mpu9250_I2c *mpu) { if (mpu->config.initialized) { if (mpu->i2c_trans.status == I2CTransFailed) { mpu->i2c_trans.status = I2CTransDone; } else if (mpu->i2c_trans.status == I2CTransSuccess) { // Successfull reading if (bit_is_set(mpu->i2c_trans.buf[0], 0)) { // new data mpu->data_accel.vect.x = Int16FromBuf(mpu->i2c_trans.buf, 1); mpu->data_accel.vect.y = Int16FromBuf(mpu->i2c_trans.buf, 3); mpu->data_accel.vect.z = Int16FromBuf(mpu->i2c_trans.buf, 5); mpu->data_rates.rates.p = Int16FromBuf(mpu->i2c_trans.buf, 9); mpu->data_rates.rates.q = Int16FromBuf(mpu->i2c_trans.buf, 11); mpu->data_rates.rates.r = Int16FromBuf(mpu->i2c_trans.buf, 13); // if we are reading slaves through the mpu, copy the ext_sens_data if ((mpu->config.i2c_bypass == FALSE) && (mpu->config.nb_slaves > 0)) { /* the buffer is volatile, since filled from ISR * but we know it's ok to use it here so we silence the warning */ #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wcast-qual" memcpy(mpu->data_ext, (uint8_t *) &(mpu->i2c_trans.buf[15]), mpu->config.nb_bytes - 15); #pragma GCC diagnostic pop } mpu->data_available = TRUE; } mpu->i2c_trans.status = I2CTransDone; } } else if (mpu->config.init_status != MPU9250_CONF_UNINIT) { // Configuring but not yet initialized switch (mpu->i2c_trans.status) { case I2CTransFailed: mpu->config.init_status--; // Retry config (TODO max retry) case I2CTransSuccess: case I2CTransDone: mpu9250_send_config(mpu9250_i2c_write_to_reg, (void*)mpu, &(mpu->config)); if (mpu->config.initialized) mpu->i2c_trans.status = I2CTransDone; break; default: break; } } // Ak8963 event function ak8963_event(&mpu->akm); }
/** * Handle all the events of the Navstik IMU components. * When there is data available convert it to the correct axis and save it in the imu structure. */ void imu_bebop_event(void) { uint32_t now_ts = get_sys_time_usec(); /* MPU-60x0 event taks */ mpu60x0_i2c_event(&imu_bebop.mpu); if (imu_bebop.mpu.data_available) { /* default orientation of the MPU is upside down sor corrigate this here */ RATES_ASSIGN(imu.gyro_unscaled, imu_bebop.mpu.data_rates.rates.p, -imu_bebop.mpu.data_rates.rates.q, -imu_bebop.mpu.data_rates.rates.r); VECT3_ASSIGN(imu.accel_unscaled, imu_bebop.mpu.data_accel.vect.x, -imu_bebop.mpu.data_accel.vect.y, -imu_bebop.mpu.data_accel.vect.z); imu_bebop.mpu.data_available = false; imu_scale_gyro(&imu); imu_scale_accel(&imu); AbiSendMsgIMU_GYRO_INT32(IMU_BOARD_ID, now_ts, &imu.gyro); AbiSendMsgIMU_ACCEL_INT32(IMU_BOARD_ID, now_ts, &imu.accel); } /* AKM8963 event task */ ak8963_event(&imu_bebop.ak); if (imu_bebop.ak.data_available) { #if BEBOP_VERSION2 struct Int32Vect3 mag_temp; // In the second bebop version the magneto is turned 90 degrees VECT3_ASSIGN(mag_temp, -imu_bebop.ak.data.vect.x, -imu_bebop.ak.data.vect.y, imu_bebop.ak.data.vect.z); // Rotate the magneto struct Int32RMat *imu_to_mag_rmat = orientationGetRMat_i(&imu_to_mag_bebop); int32_rmat_vmult(&imu.mag_unscaled, imu_to_mag_rmat, &mag_temp); #else //BEBOP regular first verion VECT3_ASSIGN(imu.mag_unscaled, -imu_bebop.ak.data.vect.y, imu_bebop.ak.data.vect.x, imu_bebop.ak.data.vect.z); #endif imu_bebop.ak.data_available = false; imu_scale_mag(&imu); AbiSendMsgIMU_MAG_INT32(IMU_BOARD_ID, now_ts, &imu.mag); } }