void imu_mpu_hmc_event(void)
{
uint32_t now_ts = get_sys_time_usec();
mpu60x0_spi_event(&imu_mpu_hmc.mpu);
if (imu_mpu_hmc.mpu.data_available) {
RATES_COPY(imu.gyro_unscaled, imu_mpu_hmc.mpu.data_rates.rates);
VECT3_COPY(imu.accel_unscaled, imu_mpu_hmc.mpu.data_accel.vect);
imu_mpu_hmc.mpu.data_available = false;
imu_scale_gyro(&imu);
imu_scale_accel(&imu);
AbiSendMsgIMU_GYRO_INT32(IMU_MPU6000_HMC_ID, now_ts, &imu.gyro);
AbiSendMsgIMU_ACCEL_INT32(IMU_MPU6000_HMC_ID, now_ts, &imu.accel);
}
/* HMC58XX event task */
hmc58xx_event(&imu_mpu_hmc.hmc);
if (imu_mpu_hmc.hmc.data_available) {
/* mag rotated by 90deg around z axis relative to MPU */
imu.mag_unscaled.x = imu_mpu_hmc.hmc.data.vect.y;
imu.mag_unscaled.y = -imu_mpu_hmc.hmc.data.vect.x;
imu.mag_unscaled.z = imu_mpu_hmc.hmc.data.vect.z;
imu_mpu_hmc.hmc.data_available = false;
imu_scale_mag(&imu);
AbiSendMsgIMU_MAG_INT32(IMU_MPU6000_HMC_ID, now_ts, &imu.mag);
}
}
void imu_hbmini_event(void)
{
uint32_t now_ts = get_sys_time_usec();
max1168_event();
if (max1168_status == MAX1168_DATA_AVAILABLE) {
imu.gyro_unscaled.p = max1168_values[IMU_GYRO_P_CHAN];
imu.gyro_unscaled.q = max1168_values[IMU_GYRO_Q_CHAN];
imu.gyro_unscaled.r = max1168_values[IMU_GYRO_R_CHAN];
imu.accel_unscaled.x = max1168_values[IMU_ACCEL_X_CHAN];
imu.accel_unscaled.y = max1168_values[IMU_ACCEL_Y_CHAN];
imu.accel_unscaled.z = max1168_values[IMU_ACCEL_Z_CHAN];
max1168_status = MAX1168_IDLE;
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);
}
// HMC58XX event task
hmc58xx_event(&imu_hbmini.hmc);
if (imu_hbmini.hmc.data_available) {
imu.mag_unscaled.z = imu_hbmini.hmc.data.value[IMU_MAG_X_CHAN];
imu.mag_unscaled.y = imu_hbmini.hmc.data.value[IMU_MAG_Y_CHAN];
imu.mag_unscaled.x = imu_hbmini.hmc.data.value[IMU_MAG_Z_CHAN];
imu_hbmini.hmc.data_available = FALSE;
imu_scale_mag(&imu);
AbiSendMsgIMU_MAG_INT32(IMU_BOARD_ID, now_ts, &imu.mag);
}
}
void imu_px4fmu_event(void)
{
uint32_t now_ts = get_sys_time_usec();
mpu60x0_spi_event(&imu_px4fmu.mpu);
if (imu_px4fmu.mpu.data_available) {
RATES_ASSIGN(imu.gyro_unscaled,
imu_px4fmu.mpu.data_rates.rates.q,
imu_px4fmu.mpu.data_rates.rates.p,
-imu_px4fmu.mpu.data_rates.rates.r);
VECT3_ASSIGN(imu.accel_unscaled,
imu_px4fmu.mpu.data_accel.vect.y,
imu_px4fmu.mpu.data_accel.vect.x,
-imu_px4fmu.mpu.data_accel.vect.z);
imu_px4fmu.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);
}
/* HMC58XX event task */
hmc58xx_event(&imu_px4fmu.hmc);
if (imu_px4fmu.hmc.data_available) {
imu.mag_unscaled.x = imu_px4fmu.hmc.data.vect.y;
imu.mag_unscaled.y = imu_px4fmu.hmc.data.vect.x;
imu.mag_unscaled.z = -imu_px4fmu.hmc.data.vect.z;
imu_px4fmu.hmc.data_available = FALSE;
imu_scale_mag(&imu);
AbiSendMsgIMU_MAG_INT32(IMU_BOARD_ID, now_ts, &imu.mag);
}
}
/**
* 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_navstik_event(void)
{
uint32_t now_ts = get_sys_time_usec();
/* MPU-60x0 event taks */
mpu60x0_i2c_event(&imu_navstik.mpu);
if (imu_navstik.mpu.data_available) {
/* default orientation as should be printed on the pcb, z-down, ICs down */
RATES_COPY(imu.gyro_unscaled, imu_navstik.mpu.data_rates.rates);
VECT3_COPY(imu.accel_unscaled, imu_navstik.mpu.data_accel.vect);
imu_navstik.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);
}
/* HMC58XX event task */
hmc58xx_event(&imu_navstik.hmc);
if (imu_navstik.hmc.data_available) {
imu.mag_unscaled.x = imu_navstik.hmc.data.vect.y;
imu.mag_unscaled.y = -imu_navstik.hmc.data.vect.x;
imu.mag_unscaled.z = imu_navstik.hmc.data.vect.z;
imu_navstik.hmc.data_available = false;
imu_scale_mag(&imu);
AbiSendMsgIMU_MAG_INT32(IMU_BOARD_ID, now_ts, &imu.mag);
}
}
uint8_t
imu_event_task(void)
{
uint8_t valid = 0;
if ( max1168_event() )
{
valid = IMU_ACC | IMU_GYR;
booz_imu.gyro_unscaled.p = max1168_values[IMU_GYRO_P_CHAN];
booz_imu.gyro_unscaled.q = max1168_values[IMU_GYRO_Q_CHAN];
booz_imu.gyro_unscaled.r = max1168_values[IMU_GYRO_R_CHAN];
booz_imu.accel_unscaled.x = max1168_values[IMU_ACCEL_X_CHAN];
booz_imu.accel_unscaled.y = max1168_values[IMU_ACCEL_Y_CHAN];
booz_imu.accel_unscaled.z = max1168_values[IMU_ACCEL_Z_CHAN];
imu_scale_gyro();
imu_scale_accel();
max1168_reset();
}
if (do_max1168_read && imu_spi_selected == SPI_NONE)
{
Booz2ImuSetSpi16bits();
imu_spi_selected = SPI_SLAVE_MAX1168;
do_max1168_read = FALSE;
max1168_read();
}
if (do_micromag_read && imu_spi_selected == SPI_NONE)
{
Booz2ImuSetSpi8bits();
imu_spi_selected = SPI_SLAVE_MM;
do_micromag_read = FALSE;
}
if (imu_spi_selected == SPI_SLAVE_MM)
micromag_read();
if ( micromag_event() )
{
valid |= IMU_MAG;
booz_imu.mag_unscaled.x = micromag_values[IMU_MAG_X_CHAN];
booz_imu.mag_unscaled.y = micromag_values[IMU_MAG_Y_CHAN];
booz_imu.mag_unscaled.z = micromag_values[IMU_MAG_Z_CHAN];
imu_scale_mag();
micromag_reset();
}
return valid;
}
void imu_mpu_spi_event(void)
{
mpu60x0_spi_event(&imu_mpu_spi.mpu);
if (imu_mpu_spi.mpu.data_available) {
uint32_t now_ts = get_sys_time_usec();
RATES_COPY(imu.gyro_unscaled, imu_mpu_spi.mpu.data_rates.rates);
VECT3_COPY(imu.accel_unscaled, imu_mpu_spi.mpu.data_accel.vect);
imu_mpu_spi.mpu.data_available = false;
imu_scale_gyro(&imu);
imu_scale_accel(&imu);
AbiSendMsgIMU_GYRO_INT32(IMU_MPU6000_ID, now_ts, &imu.gyro);
AbiSendMsgIMU_ACCEL_INT32(IMU_MPU6000_ID, now_ts, &imu.accel);
}
}
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)
{
uint32_t now_ts = get_sys_time_usec();
// If the MPU9250 SPI transaction has succeeded: convert the data
mpu9250_spi_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);
#if IMU_MPU9250_READ_MAG
if (!bit_is_set(imu_mpu9250.mpu.data_ext[6], 3)) { //mag valid just HOFL == 0
/** FIXME: assumes that we get new mag data each time instead of reading drdy bit */
struct Int32Vect3 mag;
mag.x = (IMU_MPU9250_X_SIGN) * Int16FromBuf(imu_mpu9250.mpu.data_ext, 2 * IMU_MPU9250_CHAN_Y);
mag.y = (IMU_MPU9250_Y_SIGN) * Int16FromBuf(imu_mpu9250.mpu.data_ext, 2 * IMU_MPU9250_CHAN_X);
mag.z = -(IMU_MPU9250_Z_SIGN) * Int16FromBuf(imu_mpu9250.mpu.data_ext, 2 * IMU_MPU9250_CHAN_Z);
VECT3_COPY(imu.mag_unscaled, mag);
imu_scale_mag(&imu);
AbiSendMsgIMU_MAG_INT32(IMU_MPU9250_ID, now_ts, &imu.mag);
}
#endif
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);
}
}
void imu_periodic(void)
{
// Start reading the latest gyroscope data
if (!imu_krooz.mpu.config.initialized) {
mpu60x0_i2c_start_configure(&imu_krooz.mpu);
}
if (!imu_krooz.hmc.initialized) {
hmc58xx_start_configure(&imu_krooz.hmc);
}
uint32_t now_ts = get_sys_time_usec();
if (imu_krooz.meas_nb) {
RATES_ASSIGN(imu.gyro_unscaled, -imu_krooz.rates_sum.q / imu_krooz.meas_nb,
imu_krooz.rates_sum.p / imu_krooz.meas_nb,
imu_krooz.rates_sum.r / imu_krooz.meas_nb);
RATES_ASSIGN(imu_krooz.rates_sum, 0, 0, 0);
imu_krooz.meas_nb = 0;
imu_scale_gyro(&imu);
AbiSendMsgIMU_GYRO_INT32(IMU_BOARD_ID, now_ts, &imu.gyro);
}
if (imu_krooz.meas_nb_acc.x && imu_krooz.meas_nb_acc.y && imu_krooz.meas_nb_acc.z) {
imu.accel_unscaled.x = 65536 - imu_krooz.accel_sum.x / imu_krooz.meas_nb_acc.x;
imu.accel_unscaled.y = 65536 - imu_krooz.accel_sum.y / imu_krooz.meas_nb_acc.y;
imu.accel_unscaled.z = imu_krooz.accel_sum.z / imu_krooz.meas_nb_acc.z;
#if IMU_KROOZ_USE_ACCEL_MEDIAN_FILTER
UpdateMedianFilterVect3Int(median_accel, imu.accel_unscaled);
#endif
VECT3_SMUL(imu_krooz.accel_filtered, imu_krooz.accel_filtered, IMU_KROOZ_ACCEL_AVG_FILTER);
VECT3_ADD(imu_krooz.accel_filtered, imu.accel_unscaled);
VECT3_SDIV(imu_krooz.accel_filtered, imu_krooz.accel_filtered, (IMU_KROOZ_ACCEL_AVG_FILTER + 1));
VECT3_COPY(imu.accel_unscaled, imu_krooz.accel_filtered);
INT_VECT3_ZERO(imu_krooz.accel_sum);
INT_VECT3_ZERO(imu_krooz.meas_nb_acc);
imu_scale_accel(&imu);
AbiSendMsgIMU_ACCEL_INT32(IMU_BOARD_ID, now_ts, &imu.accel);
}
RunOnceEvery(128, {axis_nb = 5;});
void imu_drotek2_event(void)
{
uint32_t now_ts = get_sys_time_usec();
// If the MPU6050 I2C transaction has succeeded: convert the data
mpu60x0_i2c_event(&imu_drotek2.mpu);
if (imu_drotek2.mpu.data_available) {
#if IMU_DROTEK_2_ORIENTATION_IC_UP
/* change orientation, so if ICs face up, z-axis is down */
imu.gyro_unscaled.p = imu_drotek2.mpu.data_rates.rates.p;
imu.gyro_unscaled.q = -imu_drotek2.mpu.data_rates.rates.q;
imu.gyro_unscaled.r = -imu_drotek2.mpu.data_rates.rates.r;
imu.accel_unscaled.x = imu_drotek2.mpu.data_accel.vect.x;
imu.accel_unscaled.y = -imu_drotek2.mpu.data_accel.vect.y;
imu.accel_unscaled.z = -imu_drotek2.mpu.data_accel.vect.z;
#else
/* default orientation as should be printed on the pcb, z-down, ICs down */
RATES_COPY(imu.gyro_unscaled, imu_drotek2.mpu.data_rates.rates);
VECT3_COPY(imu.accel_unscaled, imu_drotek2.mpu.data_accel.vect);
#endif
imu_drotek2.mpu.data_available = false;
imu_scale_gyro(&imu);
imu_scale_accel(&imu);
AbiSendMsgIMU_GYRO_INT32(IMU_DROTEK_ID, now_ts, &imu.gyro);
AbiSendMsgIMU_ACCEL_INT32(IMU_DROTEK_ID, now_ts, &imu.accel);
}
/* HMC58XX event task */
hmc58xx_event(&imu_drotek2.hmc);
if (imu_drotek2.hmc.data_available) {
#if IMU_DROTEK_2_ORIENTATION_IC_UP
imu.mag_unscaled.x = imu_drotek2.hmc.data.vect.x;
imu.mag_unscaled.y = -imu_drotek2.hmc.data.vect.y;
imu.mag_unscaled.z = -imu_drotek2.hmc.data.vect.z;
#else
VECT3_COPY(imu.mag_unscaled, imu_drotek2.hmc.data.vect);
#endif
imu_drotek2.hmc.data_available = false;
imu_scale_mag(&imu);
AbiSendMsgIMU_MAG_INT32(IMU_DROTEK_ID, now_ts, &imu.mag);
}
}
void imu_nps_event(void)
{
uint32_t now_ts = get_sys_time_usec();
if (imu_nps.gyro_available) {
imu_nps.gyro_available = FALSE;
imu_scale_gyro(&imu);
AbiSendMsgIMU_GYRO_INT32(IMU_BOARD_ID, now_ts, &imu.gyro);
}
if (imu_nps.accel_available) {
imu_nps.accel_available = FALSE;
imu_scale_accel(&imu);
AbiSendMsgIMU_ACCEL_INT32(IMU_BOARD_ID, now_ts, &imu.accel);
}
if (imu_nps.mag_available) {
imu_nps.mag_available = FALSE;
imu_scale_mag(&imu);
AbiSendMsgIMU_MAG_INT32(IMU_BOARD_ID, now_ts, &imu.mag);
}
}
/**
* 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);
}
}
void imu_periodic(void)
{
// Actual Nr of ADC measurements per channel per periodic loop
static int last_head = 0;
uint32_t now_ts = get_sys_time_usec();
imu_overrun = analog_imu_adc_buf[0].head - last_head;
if (imu_overrun < 0) {
imu_overrun += ADC_CHANNEL_GYRO_NB_SAMPLES;
}
last_head = analog_imu_adc_buf[0].head;
// Read All Measurements
#ifdef ADC_CHANNEL_GYRO_P
imu.gyro_unscaled.p = analog_imu_adc_buf[0].sum / ADC_CHANNEL_GYRO_NB_SAMPLES;
#endif
#ifdef ADC_CHANNEL_GYRO_Q
imu.gyro_unscaled.q = analog_imu_adc_buf[1].sum / ADC_CHANNEL_GYRO_NB_SAMPLES;
#endif
#ifdef ADC_CHANNEL_GYRO_R
imu.gyro_unscaled.r = analog_imu_adc_buf[2].sum / ADC_CHANNEL_GYRO_NB_SAMPLES;
#endif
#ifdef ADC_CHANNEL_ACCEL_X
imu.accel_unscaled.x = analog_imu_adc_buf[3].sum / ADC_CHANNEL_ACCEL_NB_SAMPLES;
#endif
#ifdef ADC_CHANNEL_ACCEL_Y
imu.accel_unscaled.y = analog_imu_adc_buf[4].sum / ADC_CHANNEL_ACCEL_NB_SAMPLES;
#endif
#ifdef ADC_CHANNEL_ACCEL_Z
imu.accel_unscaled.z = analog_imu_adc_buf[5].sum / ADC_CHANNEL_ACCEL_NB_SAMPLES;
#endif
imu_scale_gyro(&imu);
imu_scale_accel(&imu);
AbiSendMsgIMU_GYRO_INT32(IMU_ANALOG_ID, now_ts, &imu.gyro);
AbiSendMsgIMU_ACCEL_INT32(IMU_ANALOG_ID, now_ts, &imu.accel);
}
void imu_umarim_event(void)
{
uint32_t now_ts = get_sys_time_usec();
// If the itg3200 I2C transaction has succeeded: convert the data
itg3200_event(&imu_umarim.itg);
if (imu_umarim.itg.data_available) {
RATES_COPY(imu.gyro_unscaled, imu_umarim.itg.data.rates);
imu_umarim.itg.data_available = FALSE;
imu_scale_gyro(&imu);
AbiSendMsgIMU_GYRO_INT32(IMU_BOARD_ID, now_ts, &imu.gyro);
}
// If the adxl345 I2C transaction has succeeded: convert the data
adxl345_i2c_event(&imu_umarim.adxl);
if (imu_umarim.adxl.data_available) {
VECT3_ASSIGN(imu.accel_unscaled, imu_umarim.adxl.data.vect.y, -imu_umarim.adxl.data.vect.x,
imu_umarim.adxl.data.vect.z);
imu_umarim.adxl.data_available = FALSE;
imu_scale_accel(&imu);
AbiSendMsgIMU_ACCEL_INT32(IMU_BOARD_ID, now_ts, &imu.accel);
}
}
void imu_aspirin_i2c_event(void)
{
uint32_t now_ts = get_sys_time_usec();
adxl345_i2c_event(&imu_aspirin.acc_adxl);
if (imu_aspirin.acc_adxl.data_available) {
VECT3_COPY(imu.accel_unscaled, imu_aspirin.acc_adxl.data.vect);
imu_aspirin.acc_adxl.data_available = FALSE;
imu_scale_accel(&imu);
AbiSendMsgIMU_ACCEL_INT32(IMU_ASPIRIN_ID, now_ts, &imu.accel);
}
/* If the itg3200 I2C transaction has succeeded: convert the data */
itg3200_event(&imu_aspirin.gyro_itg);
if (imu_aspirin.gyro_itg.data_available) {
RATES_COPY(imu.gyro_unscaled, imu_aspirin.gyro_itg.data.rates);
imu_aspirin.gyro_itg.data_available = FALSE;
imu_scale_gyro(&imu);
AbiSendMsgIMU_GYRO_INT32(IMU_ASPIRIN_ID, now_ts, &imu.gyro);
}
/* HMC58XX event task */
hmc58xx_event(&imu_aspirin.mag_hmc);
if (imu_aspirin.mag_hmc.data_available) {
#ifdef IMU_ASPIRIN_VERSION_1_0
VECT3_COPY(imu.mag_unscaled, imu_aspirin.mag_hmc.data.vect);
#else // aspirin 1.5 with hmc5883
imu.mag_unscaled.x = imu_aspirin.mag_hmc.data.vect.y;
imu.mag_unscaled.y = -imu_aspirin.mag_hmc.data.vect.x;
imu.mag_unscaled.z = imu_aspirin.mag_hmc.data.vect.z;
#endif
imu_aspirin.mag_hmc.data_available = FALSE;
imu_scale_mag(&imu);
AbiSendMsgIMU_MAG_INT32(IMU_ASPIRIN_ID, now_ts, &imu.mag);
}
}
void imu_ppzuav_event(void)
{
uint32_t now_ts = get_sys_time_usec();
adxl345_i2c_event(&imu_ppzuav.acc_adxl);
if (imu_ppzuav.acc_adxl.data_available) {
imu.accel_unscaled.x = -imu_ppzuav.acc_adxl.data.vect.x;
imu.accel_unscaled.y = imu_ppzuav.acc_adxl.data.vect.y;
imu.accel_unscaled.z = -imu_ppzuav.acc_adxl.data.vect.z;
imu_ppzuav.acc_adxl.data_available = false;
imu_scale_accel(&imu);
AbiSendMsgIMU_ACCEL_INT32(IMU_PPZUAV_ID, now_ts, &imu.accel);
}
/* If the itg3200 I2C transaction has succeeded: convert the data */
itg3200_event(&imu_ppzuav.gyro_itg);
if (imu_ppzuav.gyro_itg.data_available) {
imu.gyro_unscaled.p = -imu_ppzuav.gyro_itg.data.rates.p;
imu.gyro_unscaled.q = imu_ppzuav.gyro_itg.data.rates.q;
imu.gyro_unscaled.r = -imu_ppzuav.gyro_itg.data.rates.r;
imu_ppzuav.gyro_itg.data_available = false;
imu_scale_gyro(&imu);
AbiSendMsgIMU_GYRO_INT32(IMU_PPZUAV_ID, now_ts, &imu.gyro);
}
/* HMC58XX event task */
hmc58xx_event(&imu_ppzuav.mag_hmc);
if (imu_ppzuav.mag_hmc.data_available) {
imu.mag_unscaled.x = -imu_ppzuav.mag_hmc.data.vect.y;
imu.mag_unscaled.y = -imu_ppzuav.mag_hmc.data.vect.x;
imu.mag_unscaled.z = -imu_ppzuav.mag_hmc.data.vect.z;
imu_ppzuav.mag_hmc.data_available = false;
imu_scale_mag(&imu);
AbiSendMsgIMU_MAG_INT32(IMU_PPZUAV_ID, now_ts, &imu.mag);
}
}
void imu_aspirin2_event(void)
{
uint32_t now_ts = get_sys_time_usec();
mpu60x0_spi_event(&imu_aspirin2.mpu);
if (imu_aspirin2.mpu.data_available) {
#if !ASPIRIN_2_DISABLE_MAG
/* HMC5883 has xzy order of axes in returned data */
struct Int32Vect3 mag;
mag.x = Int16FromBuf(imu_aspirin2.mpu.data_ext, 0);
mag.z = Int16FromBuf(imu_aspirin2.mpu.data_ext, 2);
mag.y = Int16FromBuf(imu_aspirin2.mpu.data_ext, 4);
#endif
/* Handle axis assignement for Lisa/S integrated Aspirin like IMU. */
#ifdef LISA_S
#ifdef LISA_S_UPSIDE_DOWN
RATES_ASSIGN(imu.gyro_unscaled,
imu_aspirin2.mpu.data_rates.rates.p,
-imu_aspirin2.mpu.data_rates.rates.q,
-imu_aspirin2.mpu.data_rates.rates.r);
VECT3_ASSIGN(imu.accel_unscaled,
imu_aspirin2.mpu.data_accel.vect.x,
-imu_aspirin2.mpu.data_accel.vect.y,
-imu_aspirin2.mpu.data_accel.vect.z);
VECT3_ASSIGN(imu.mag_unscaled, mag.x, -mag.y, -mag.z);
#else
RATES_COPY(imu.gyro_unscaled, imu_aspirin2.mpu.data_rates.rates);
VECT3_COPY(imu.accel_unscaled, imu_aspirin2.mpu.data_accel.vect);
#if !ASPIRIN_2_DISABLE_MAG
VECT3_COPY(imu.mag_unscaled, mag);
#endif
#endif
#else
/* Handle axis assignement for Lisa/M or Lisa/MX V2.1 integrated Aspirin like
* IMU.
*/
#ifdef LISA_M_OR_MX_21
RATES_ASSIGN(imu.gyro_unscaled,
-imu_aspirin2.mpu.data_rates.rates.q,
imu_aspirin2.mpu.data_rates.rates.p,
imu_aspirin2.mpu.data_rates.rates.r);
VECT3_ASSIGN(imu.accel_unscaled,
-imu_aspirin2.mpu.data_accel.vect.y,
imu_aspirin2.mpu.data_accel.vect.x,
imu_aspirin2.mpu.data_accel.vect.z);
#if !ASPIRIN_2_DISABLE_MAG
VECT3_ASSIGN(imu.mag_unscaled, -mag.y, mag.x, mag.z);
#endif
#else
/* Handle real Aspirin IMU axis assignement. */
#ifdef LISA_M_LONGITUDINAL_X
RATES_ASSIGN(imu.gyro_unscaled,
imu_aspirin2.mpu.data_rates.rates.q,
-imu_aspirin2.mpu.data_rates.rates.p,
imu_aspirin2.mpu.data_rates.rates.r);
VECT3_ASSIGN(imu.accel_unscaled,
imu_aspirin2.mpu.data_accel.vect.y,
-imu_aspirin2.mpu.data_accel.vect.x,
imu_aspirin2.mpu.data_accel.vect.z);
#if !ASPIRIN_2_DISABLE_MAG
VECT3_ASSIGN(imu.mag_unscaled, -mag.x, -mag.y, mag.z);
#endif
#else
RATES_COPY(imu.gyro_unscaled, imu_aspirin2.mpu.data_rates.rates);
VECT3_COPY(imu.accel_unscaled, imu_aspirin2.mpu.data_accel.vect);
#if !ASPIRIN_2_DISABLE_MAG
VECT3_ASSIGN(imu.mag_unscaled, mag.y, -mag.x, mag.z)
#endif
#endif
#endif
#endif
imu_aspirin2.mpu.data_available = false;
imu_scale_gyro(&imu);
imu_scale_accel(&imu);
AbiSendMsgIMU_GYRO_INT32(IMU_ASPIRIN2_ID, now_ts, &imu.gyro);
AbiSendMsgIMU_ACCEL_INT32(IMU_ASPIRIN2_ID, now_ts, &imu.accel);
#if !ASPIRIN_2_DISABLE_MAG
imu_scale_mag(&imu);
AbiSendMsgIMU_MAG_INT32(IMU_ASPIRIN2_ID, now_ts, &imu.mag);
#endif
}
}
void aspirin2_subsystem_event(void)
{
int32_t x, y, z;
uint32_t now_ts = get_sys_time_usec();
// If the itg3200 I2C transaction has succeeded: convert the data
if (aspirin2_mpu60x0.status == I2CTransSuccess) {
#define MPU_OFFSET_GYRO 9
x = (int16_t)((aspirin2_mpu60x0.buf[0 + MPU_OFFSET_GYRO] << 8) | aspirin2_mpu60x0.buf[1 + MPU_OFFSET_GYRO]);
y = (int16_t)((aspirin2_mpu60x0.buf[2 + MPU_OFFSET_GYRO] << 8) | aspirin2_mpu60x0.buf[3 + MPU_OFFSET_GYRO]);
z = (int16_t)((aspirin2_mpu60x0.buf[4 + MPU_OFFSET_GYRO] << 8) | aspirin2_mpu60x0.buf[5 + MPU_OFFSET_GYRO]);
RATES_ASSIGN(imu.gyro_unscaled, x, y, z);
#define MPU_OFFSET_ACC 1
x = (int16_t)((aspirin2_mpu60x0.buf[0 + MPU_OFFSET_ACC] << 8) | aspirin2_mpu60x0.buf[1 + MPU_OFFSET_ACC]);
y = (int16_t)((aspirin2_mpu60x0.buf[2 + MPU_OFFSET_ACC] << 8) | aspirin2_mpu60x0.buf[3 + MPU_OFFSET_ACC]);
z = (int16_t)((aspirin2_mpu60x0.buf[4 + MPU_OFFSET_ACC] << 8) | aspirin2_mpu60x0.buf[5 + MPU_OFFSET_ACC]);
VECT3_ASSIGN(imu.accel_unscaled, x, y, z);
// Is this is new data
if (aspirin2_mpu60x0.buf[0] & 0x01) {
imu_scale_gyro(&imu);
imu_scale_accel(&imu);
AbiSendMsgIMU_GYRO_INT32(IMU_PPZUAV_ID, now_ts, &imu.gyro);
AbiSendMsgIMU_ACCEL_INT32(IMU_PPZUAV_ID, now_ts, &imu.accel);
}
aspirin2_mpu60x0.status =
I2CTransDone; // remove the I2CTransSuccess status, otherwise data ready will be triggered again without new data
}
/*
// If the adxl345 I2C transaction has succeeded: convert the data
if (ppzuavimu_adxl345.status == I2CTransSuccess)
{
x = (int16_t) ((ppzuavimu_adxl345.buf[1] << 8) | ppzuavimu_adxl345.buf[0]);
y = (int16_t) ((ppzuavimu_adxl345.buf[3] << 8) | ppzuavimu_adxl345.buf[2]);
z = (int16_t) ((ppzuavimu_adxl345.buf[5] << 8) | ppzuavimu_adxl345.buf[4]);
#ifdef ASPIRIN_IMU
VECT3_ASSIGN(imu.accel_unscaled, x, -y, -z);
#else // PPZIMU
VECT3_ASSIGN(imu.accel_unscaled, -x, y, -z);
#endif
acc_valid = TRUE;
ppzuavimu_adxl345.status = I2CTransDone;
}
// If the hmc5843 I2C transaction has succeeded: convert the data
if (ppzuavimu_hmc5843.status == I2CTransSuccess)
{
x = (int16_t) ((ppzuavimu_hmc5843.buf[0] << 8) | ppzuavimu_hmc5843.buf[1]);
y = (int16_t) ((ppzuavimu_hmc5843.buf[2] << 8) | ppzuavimu_hmc5843.buf[3]);
z = (int16_t) ((ppzuavimu_hmc5843.buf[4] << 8) | ppzuavimu_hmc5843.buf[5]);
#ifdef ASPIRIN_IMU
VECT3_ASSIGN(imu.mag_unscaled, x, -y, -z);
#else // PPZIMU
VECT3_ASSIGN(imu.mag_unscaled, -y, -x, -z);
#endif
mag_valid = TRUE;
ppzuavimu_hmc5843.status = I2CTransDone;
}
*/
}
