void imu_navgo_event( void )
{
// If the itg3200 I2C transaction has succeeded: convert the data
itg3200_event();
if (itg3200_data_available) {
RATES_COPY(imu.gyro_unscaled, itg3200_data);
itg3200_data_available = FALSE;
gyr_valid = TRUE;
}
// If the adxl345 I2C transaction has succeeded: convert the data
adxl345_event();
if (adxl345_data_available) {
VECT3_ASSIGN(imu.accel_unscaled, adxl345_data.y, -adxl345_data.x, adxl345_data.z);
adxl345_data_available = FALSE;
acc_valid = TRUE;
}
// HMC58XX event task
hmc58xx_event();
if (hmc58xx_data_available) {
VECT3_ASSIGN(imu.mag_unscaled, -hmc58xx_data.x, -hmc58xx_data.y, hmc58xx_data.z);
hmc58xx_data_available = FALSE;
mag_valid = TRUE;
}
}
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_aspirin_event(void)
{
adxl345_spi_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_aspirin.accel_valid = TRUE;
}
/* 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_aspirin.gyro_valid = TRUE;
}
/* 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_aspirin.mag_valid = TRUE;
}
}
void imu_krooz_event(void)
{
if (imu_krooz.mpu_eoc) {
mpu60x0_i2c_read(&imu_krooz.mpu);
imu_krooz.mpu_eoc = false;
}
// If the MPU6050 I2C transaction has succeeded: convert the data
mpu60x0_i2c_event(&imu_krooz.mpu);
if (imu_krooz.mpu.data_available) {
RATES_ADD(imu_krooz.rates_sum, imu_krooz.mpu.data_rates.rates);
VECT3_ADD(imu_krooz.accel_sum, imu_krooz.mpu.data_accel.vect);
imu_krooz.meas_nb++;
imu_krooz.mpu.data_available = false;
}
if (imu_krooz.hmc_eoc) {
hmc58xx_read(&imu_krooz.hmc);
imu_krooz.hmc_eoc = false;
}
// If the HMC5883 I2C transaction has succeeded: convert the data
hmc58xx_event(&imu_krooz.hmc);
if (imu_krooz.hmc.data_available) {
VECT3_ASSIGN(imu.mag_unscaled, imu_krooz.hmc.data.vect.y, -imu_krooz.hmc.data.vect.x, imu_krooz.hmc.data.vect.z);
UpdateMedianFilterVect3Int(median_mag, imu.mag_unscaled);
imu_krooz.hmc.data_available = false;
imu_scale_mag(&imu);
AbiSendMsgIMU_MAG_INT32(IMU_BOARD_ID, get_sys_time_usec(), &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);
}
}
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);
}
}
void imu_px4fmu_event(void)
{
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_px4fmu.gyro_valid = TRUE;
imu_px4fmu.accel_valid = TRUE;
}
/* 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_px4fmu.mag_valid = TRUE;
}
}
void imu_hbmini_event(void)
{
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_hbmini.gyr_valid = TRUE;
imu_hbmini.acc_valid = TRUE;
}
// 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_hbmini.mag_valid = TRUE;
}
}
void imu_krooz_event( void )
{
if (imu_krooz.mpu_eoc) {
mpu60x0_i2c_read(&imu_krooz.mpu);
imu_krooz.mpu_eoc = FALSE;
}
// If the MPU6050 I2C transaction has succeeded: convert the data
mpu60x0_i2c_event(&imu_krooz.mpu);
if (imu_krooz.mpu.data_available) {
RATES_ADD(imu_krooz.rates_sum, imu_krooz.mpu.data_rates.rates);
VECT3_ADD(imu_krooz.accel_sum, imu_krooz.mpu.data_accel.vect);
imu_krooz.meas_nb++;
imu_krooz.mpu.data_available = FALSE;
}
if (imu_krooz.hmc_eoc) {
hmc58xx_read(&imu_krooz.hmc);
imu_krooz.hmc_eoc = FALSE;
}
// If the HMC5883 I2C transaction has succeeded: convert the data
hmc58xx_event(&imu_krooz.hmc);
if (imu_krooz.hmc.data_available) {
VECT3_ASSIGN(imu.mag_unscaled, imu_krooz.hmc.data.vect.y, -imu_krooz.hmc.data.vect.x, imu_krooz.hmc.data.vect.z);
UpdateMedianFilterVect3Int(median_mag, imu.mag_unscaled);
imu_krooz.hmc.data_available = FALSE;
imu_krooz.mag_valid = TRUE;
}
}
void mag_hmc58xx_module_event(void)
{
hmc58xx_event(&mag_hmc58xx);
if (mag_hmc58xx.data_available) {
#if MODULE_HMC58XX_UPDATE_AHRS
// current timestamp
uint32_t now_ts = get_sys_time_usec();
// set channel order
struct Int32Vect3 mag = {
(int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_X]),
(int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_Y]),
(int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_Z])
};
// unscaled vector
VECT3_COPY(imu.mag_unscaled, mag);
// scale vector
imu_scale_mag(&imu);
AbiSendMsgIMU_MAG_INT32(MAG_HMC58XX_SENDER_ID, now_ts, &imu.mag);
#endif
#if MODULE_HMC58XX_SYNC_SEND
mag_hmc58xx_report();
#endif
#if MODULE_HMC58XX_UPDATE_AHRS || MODULE_HMC58XX_SYNC_SEND
mag_hmc58xx.data_available = FALSE;
#endif
}
}
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 mag_hmc58xx_module_event(void) {
hmc58xx_event(&mag_hmc58xx);
#if MODULE_HMC58XX_SYNC_SEND
if (mag_hmc58xx.data_available) {
mag_hmc58xx_report();
mag_hmc58xx.data_available = FALSE;
}
#endif
}
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_krooz_event( void )
{
// If the MPU6050 I2C transaction has succeeded: convert the data
mpu60x0_i2c_event(&imu_krooz.mpu);
if (imu_krooz.mpu.data_available) {
RATES_ADD(imu_krooz.rates_sum, imu_krooz.mpu.data_rates.rates);
VECT3_ADD(imu_krooz.accel_sum, imu_krooz.mpu.data_accel.vect);
imu_krooz.meas_nb++;
imu_krooz.mpu.data_available = FALSE;
}
// If the HMC5883 I2C transaction has succeeded: convert the data
hmc58xx_event(&imu_krooz.hmc);
if (imu_krooz.hmc.data_available) {
VECT3_COPY(imu.mag_unscaled, imu_krooz.hmc.data.vect);
#if KROOZ_USE_MEDIAN_FILTER
UpdateMedianFilterVect3Int(median_mag, imu.mag_unscaled);
#endif
imu_krooz.hmc.data_available = FALSE;
imu_krooz.mag_valid = TRUE;
}
}
void imu_navgo_event( void )
{
// If the itg3200 I2C transaction has succeeded: convert the data
itg3200_event(&imu_navgo.itg);
if (imu_navgo.itg.data_available) {
RATES_ASSIGN(imu.gyro_unscaled, -imu_navgo.itg.data.rates.q, imu_navgo.itg.data.rates.p, imu_navgo.itg.data.rates.r);
#if NAVGO_USE_MEDIAN_FILTER
UpdateMedianFilterRatesInt(median_gyro, imu.gyro_unscaled);
#endif
imu_navgo.itg.data_available = FALSE;
imu_navgo.gyr_valid = TRUE;
}
// If the adxl345 I2C transaction has succeeded: convert the data
adxl345_i2c_event(&imu_navgo.adxl);
if (imu_navgo.adxl.data_available) {
VECT3_ASSIGN(imu.accel_unscaled, imu_navgo.adxl.data.vect.y, -imu_navgo.adxl.data.vect.x, imu_navgo.adxl.data.vect.z);
#if NAVGO_USE_MEDIAN_FILTER
UpdateMedianFilterVect3Int(median_accel, imu.accel_unscaled);
#endif
imu_navgo.adxl.data_available = FALSE;
imu_navgo.acc_valid = TRUE;
}
// HMC58XX event task
hmc58xx_event(&imu_navgo.hmc);
if (imu_navgo.hmc.data_available) {
VECT3_COPY(imu.mag_unscaled, imu_navgo.hmc.data.vect);
#if NAVGO_USE_MEDIAN_FILTER
UpdateMedianFilterVect3Int(median_mag, imu.mag_unscaled);
#endif
imu_navgo.hmc.data_available = FALSE;
imu_navgo.mag_valid = TRUE;
}
}
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_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);
}
}
