void imu_impl_init(void)
{
imu_aspirin.accel_valid = FALSE;
imu_aspirin.gyro_valid = FALSE;
imu_aspirin.mag_valid = FALSE;
/* Set accel configuration */
adxl345_i2c_init(&imu_aspirin.acc_adxl, &(ASPIRIN_I2C_DEV), ADXL345_ADDR);
// set the data rate
imu_aspirin.acc_adxl.config.rate = ASPIRIN_ACCEL_RATE;
/// @todo drdy int handling for adxl345
//imu_aspirin.acc_adxl.config.drdy_int_enable = TRUE;
// With CS tied high to VDD I/O, the ADXL345 is in I2C mode
GPIO_ARCH_SET_SPI_CS_HIGH();
/* Gyro configuration and initalization */
itg3200_init(&imu_aspirin.gyro_itg, &(ASPIRIN_I2C_DEV), ITG3200_ADDR);
/* change the default config */
// Aspirin sample rate divider
imu_aspirin.gyro_itg.config.smplrt_div = ASPIRIN_GYRO_SMPLRT_DIV;
// digital low pass filter
imu_aspirin.gyro_itg.config.dlpf_cfg = ASPIRIN_GYRO_LOWPASS;
/// @todo eoc interrupt for itg3200, polling for now (including status reg)
/* interrupt on data ready, idle high, latch until read any register */
//itg_conf.int_cfg = (0x01 | (0x1<<4) | (0x1<<5) | 0x01<<7);
/* initialize mag and set default options */
hmc58xx_init(&imu_aspirin.mag_hmc, &(ASPIRIN_I2C_DEV), HMC58XX_ADDR);
#ifdef IMU_ASPIRIN_VERSION_1_0
imu_aspirin.mag_hmc.type = HMC_TYPE_5843;
#endif
}
void imu_impl_init(void)
{
GPIO_ARCH_SET_SPI_CS_HIGH();
/////////////////////////////////////////////////////////////////////
// ITG3200
ppzuavimu_itg3200.type = I2CTransTx;
ppzuavimu_itg3200.slave_addr = ITG3200_ADDR;
ppzuavimu_itg3200.buf[0] = ITG3200_REG_DLPF_FS;
#if PERIODIC_FREQUENCY == 60
/* set gyro range to 2000deg/s and low pass at 20Hz (< 60Hz/2) internal sampling at 1kHz */
ppzuavimu_itg3200.buf[1] = (0x03<<3) | (0x04<<0);
# warning Info: ITG3200 read at 50Hz
#else
# if PERIODIC_FREQUENCY == 120
# warning Info: ITG3200 read at 100Hz
/* set gyro range to 2000deg/s and low pass at 42Hz (< 120Hz/2) internal sampling at 1kHz */
ppzuavimu_itg3200.buf[1] = (0x03<<3) | (0x03<<0);
# else
# error PERIODIC_FREQUENCY should be either 60Hz or 120Hz. Otherwise manually fix the sensor rates
# endif
#endif
ppzuavimu_itg3200.len_w = 2;
i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_itg3200);
while(ppzuavimu_itg3200.status == I2CTransPending);
/* set sample rate to 66Hz: so at 60Hz there is always a new sample ready and you loose little */
ppzuavimu_itg3200.buf[0] = ITG3200_REG_SMPLRT_DIV;
#if PERIODIC_FREQUENCY == 60
ppzuavimu_itg3200.buf[1] = 19; // 50Hz
#else
ppzuavimu_itg3200.buf[1] = 9; // 100Hz
#endif
i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_itg3200);
while(ppzuavimu_itg3200.status == I2CTransPending);
/* switch to gyroX clock */
ppzuavimu_itg3200.buf[0] = ITG3200_REG_PWR_MGM;
ppzuavimu_itg3200.buf[1] = 0x01;
i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_itg3200);
while(ppzuavimu_itg3200.status == I2CTransPending);
/* no interrupts for now, but set data ready interrupt to enable reading status bits */
ppzuavimu_itg3200.buf[0] = ITG3200_REG_INT_CFG;
ppzuavimu_itg3200.buf[1] = 0x01;
i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_itg3200);
while(ppzuavimu_itg3200.status == I2CTransPending);
/////////////////////////////////////////////////////////////////////
// ADXL345
/* set data rate to 100Hz */
ppzuavimu_adxl345.slave_addr = ADXL345_ADDR;
ppzuavimu_adxl345.type = I2CTransTx;
ppzuavimu_adxl345.buf[0] = ADXL345_REG_BW_RATE;
#if PERIODIC_FREQUENCY == 60
ppzuavimu_adxl345.buf[1] = 0x09; // normal power and 50Hz sampling, 50Hz BW
#else
ppzuavimu_adxl345.buf[1] = 0x0a; // normal power and 100Hz sampling, 50Hz BW
#endif
ppzuavimu_adxl345.len_w = 2;
i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_adxl345);
while(ppzuavimu_adxl345.status == I2CTransPending);
/* switch to measurement mode */
ppzuavimu_adxl345.type = I2CTransTx;
ppzuavimu_adxl345.buf[0] = ADXL345_REG_POWER_CTL;
ppzuavimu_adxl345.buf[1] = 1<<3;
ppzuavimu_adxl345.len_w = 2;
i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_adxl345);
while(ppzuavimu_adxl345.status == I2CTransPending);
/* Set range to 16g but keeping full resolution of 3.9 mV/g */
ppzuavimu_adxl345.type = I2CTransTx;
ppzuavimu_adxl345.buf[0] = ADXL345_REG_DATA_FORMAT;
ppzuavimu_adxl345.buf[1] = 1<<3 | 0<<2 | 0x03; // bit 3 is full resolution bit, bit 2 is left justify bit 0,1 are range: 00=2g 01=4g 10=8g 11=16g
ppzuavimu_adxl345.len_w = 2;
i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_adxl345);
while(ppzuavimu_adxl345.status == I2CTransPending);
/////////////////////////////////////////////////////////////////////
// HMC5843
ppzuavimu_hmc5843.slave_addr = HMC5843_ADDR;
ppzuavimu_hmc5843.type = I2CTransTx;
ppzuavimu_hmc5843.buf[0] = HMC5843_REG_CFGA; // set to rate to max speed: 50Hz no bias
ppzuavimu_hmc5843.buf[1] = 0x00 | (0x06 << 2);
ppzuavimu_hmc5843.len_w = 2;
i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_hmc5843);
while(ppzuavimu_hmc5843.status == I2CTransPending);
ppzuavimu_hmc5843.type = I2CTransTx;
ppzuavimu_hmc5843.buf[0] = HMC5843_REG_CFGB; // set to gain to 1 Gauss
ppzuavimu_hmc5843.buf[1] = 0x01<<5;
ppzuavimu_hmc5843.len_w = 2;
i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_hmc5843);
while(ppzuavimu_hmc5843.status == I2CTransPending);
ppzuavimu_hmc5843.type = I2CTransTx;
ppzuavimu_hmc5843.buf[0] = HMC5843_REG_MODE; // set to continuous mode
ppzuavimu_hmc5843.buf[1] = 0x00;
ppzuavimu_hmc5843.len_w = 2;
i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_hmc5843);
while(ppzuavimu_hmc5843.status == I2CTransPending);
}
