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); }