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); 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); #if KROOZ_USE_MEDIAN_FILTER UpdateMedianFilterRatesInt(median_gyro, imu.gyro_unscaled); #endif VECT3_ASSIGN(imu.accel_unscaled, imu_krooz.accel_sum.y / imu_krooz.meas_nb, imu_krooz.accel_sum.x / imu_krooz.meas_nb, imu_krooz.accel_sum.z / imu_krooz.meas_nb); #if KROOZ_USE_MEDIAN_FILTER UpdateMedianFilterVect3Int(median_accel, imu.accel_unscaled); #endif RATES_ASSIGN(imu_krooz.rates_sum, 0, 0, 0); VECT3_ASSIGN(imu_krooz.accel_sum, 0, 0, 0); imu_krooz.meas_nb = 0; imu_krooz.gyr_valid = TRUE; imu_krooz.acc_valid = TRUE; } //RunOnceEvery(10,imu_krooz_downlink_raw()); }
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; } }