/**
* 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);
}
}
