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