void imu_impl_init( void )
{
/////////////////////////////////////////////////////////////////////
// MPU-60X0
mpu60x0_i2c_init(&imu_krooz.mpu, &(IMU_KROOZ_I2C_DEV), MPU60X0_ADDR);
// change the default configuration
imu_krooz.mpu.config.smplrt_div = KROOZ_SMPLRT_DIV;
imu_krooz.mpu.config.dlpf_cfg = KROOZ_LOWPASS_FILTER;
imu_krooz.mpu.config.gyro_range = KROOZ_GYRO_RANGE;
imu_krooz.mpu.config.accel_range = KROOZ_ACCEL_RANGE;
imu_krooz.mpu.config.drdy_int_enable = TRUE;
hmc58xx_init(&imu_krooz.hmc, &(IMU_KROOZ_I2C_DEV), HMC58XX_ADDR);
// Init median filters
#if IMU_KROOZ_USE_GYRO_MEDIAN_FILTER
InitMedianFilterRatesInt(median_gyro);
#endif
#if IMU_KROOZ_USE_ACCEL_MEDIAN_FILTER
InitMedianFilterVect3Int(median_accel);
#endif
InitMedianFilterVect3Int(median_mag);
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 = FALSE;
imu_krooz.acc_valid = FALSE;
imu_krooz.mag_valid = FALSE;
imu_krooz_sd_arch_init();
}
void imu_impl_init(void)
{
/////////////////////////////////////////////////////////////////////
// ITG3200
itg3200_init(&imu_navgo.itg, &(IMU_NAVGO_I2C_DEV), ITG3200_ADDR_ALT);
// change the default configuration
imu_navgo.itg.config.smplrt_div = NAVGO_GYRO_SMPLRT_DIV; // 500Hz sample rate since internal is 1kHz
imu_navgo.itg.config.dlpf_cfg = NAVGO_GYRO_LOWPASS;
/////////////////////////////////////////////////////////////////////
// ADXL345
adxl345_i2c_init(&imu_navgo.adxl, &(IMU_NAVGO_I2C_DEV), ADXL345_ADDR_ALT);
// change the default data rate
imu_navgo.adxl.config.rate = NAVGO_ACCEL_RATE;
/////////////////////////////////////////////////////////////////////
// HMC58XX
hmc58xx_init(&imu_navgo.hmc, &(IMU_NAVGO_I2C_DEV), HMC58XX_ADDR);
#if NAVGO_USE_MEDIAN_FILTER
// Init median filters
InitMedianFilterRatesInt(median_gyro);
InitMedianFilterVect3Int(median_accel);
InitMedianFilterVect3Int(median_mag);
#endif
imu_navgo.gyr_valid = FALSE;
imu_navgo.acc_valid = FALSE;
imu_navgo.mag_valid = FALSE;
}
void imu_impl_init(void)
{
/////////////////////////////////////////////////////////////////////
// MPU-60X0
mpu60x0_i2c_init(&imu_krooz.mpu, &(IMU_KROOZ_I2C_DEV), MPU60X0_ADDR);
// change the default configuration
imu_krooz.mpu.config.smplrt_div = KROOZ_SMPLRT_DIV;
imu_krooz.mpu.config.dlpf_cfg = KROOZ_LOWPASS_FILTER;
imu_krooz.mpu.config.gyro_range = KROOZ_GYRO_RANGE;
imu_krooz.mpu.config.accel_range = KROOZ_ACCEL_RANGE;
imu_krooz.mpu.config.drdy_int_enable = true;
hmc58xx_init(&imu_krooz.hmc, &(IMU_KROOZ_I2C_DEV), HMC58XX_ADDR);
// Init median filters
#if IMU_KROOZ_USE_ACCEL_MEDIAN_FILTER
InitMedianFilterVect3Int(median_accel);
#endif
InitMedianFilterVect3Int(median_mag);
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.hmc_eoc = false;
imu_krooz.mpu_eoc = false;
imu_krooz.ad7689_trans.slave_idx = IMU_KROOZ_SPI_SLAVE_IDX;
imu_krooz.ad7689_trans.select = SPISelectUnselect;
imu_krooz.ad7689_trans.cpol = SPICpolIdleLow;
imu_krooz.ad7689_trans.cpha = SPICphaEdge1;
imu_krooz.ad7689_trans.dss = SPIDss8bit;
imu_krooz.ad7689_trans.bitorder = SPIMSBFirst;
imu_krooz.ad7689_trans.cdiv = SPIDiv16;
imu_krooz.ad7689_trans.output_length = sizeof(imu_krooz.ad7689_spi_tx_buffer);
imu_krooz.ad7689_trans.output_buf = (uint8_t *) imu_krooz.ad7689_spi_tx_buffer;
imu_krooz.ad7689_trans.input_length = sizeof(imu_krooz.ad7689_spi_rx_buffer);
imu_krooz.ad7689_trans.input_buf = (uint8_t *) imu_krooz.ad7689_spi_rx_buffer;
imu_krooz.ad7689_trans.before_cb = NULL;
imu_krooz.ad7689_trans.after_cb = NULL;
axis_cnt = 0;
axis_nb = 2;
imu_krooz_sd_arch_init();
}