uint8_t hmc5983Init(void) {
int i = HMC5983_RETRIES;
switch ((int)p[IMU_FLIP]) {
case 1:
hmc5983Data.magSign[0] = 1.0f;
hmc5983Data.magSign[1] = -1.0f;
hmc5983Data.magSign[2] = -1.0f;
break;
case 2:
hmc5983Data.magSign[0] = -1.0f;
hmc5983Data.magSign[1] = 1.0f;
hmc5983Data.magSign[2] = -1.0f;
break;
case 0:
default:
hmc5983Data.magSign[0] = 1.0f;
hmc5983Data.magSign[1] = 1.0f;
hmc5983Data.magSign[2] = 1.0f;
break;
}
// wait for a valid response
while (--i && hmc5983GetReg(0x0a) != 'H')
delay(100);
if (i > 0) {
// 75Hz, 8x oversample
hmc5983ReliablySetReg(0x00, 0b11111000);
delay(10);
// // highest gain (+-0.88 Ga)
// hmc5983ReliablySetReg(0x01, 0b00000000);
// gain (+-2.5 Ga)
hmc5983ReliablySetReg(0x01, 0b01100000);
delay(10);
hmc5983ReliablySetReg(0x02, 0b00000000);
delay(10);
hmc5983Data.readCmd = HMC5983_READ_MULT_BIT | 0x03;
spiChangeCallback(hmc5983Data.spi, hmc5983TransferComplete);
// External Interrupt line for data ready
extRegisterCallback(DIMU_HMC5983_INT_PORT, DIMU_HMC5983_INT_PIN, EXTI_Trigger_Rising, 1, GPIO_PuPd_NOPULL, hmc5983IntHandler);
hmc5983Data.initialized = 1;
}
else {
hmc5983Data.initialized = 0;
}
return hmc5983Data.initialized;
}
void max21100Init(void) {
switch ((int)p[IMU_FLIP]) {
case 1:
max21100Data.accSign[0] = 1.0f;
max21100Data.accSign[1] = -1.0f;
max21100Data.accSign[2] = -1.0f;
max21100Data.gyoSign[0] = 1.0f;
max21100Data.gyoSign[1] = -1.0f;
max21100Data.gyoSign[2] = -1.0f;
break;
case 2:
max21100Data.accSign[0] = -1.0f;
max21100Data.accSign[1] = 1.0f;
max21100Data.accSign[2] = -1.0f;
max21100Data.gyoSign[0] = -1.0f;
max21100Data.gyoSign[1] = 1.0f;
max21100Data.gyoSign[2] = -1.0f;
break;
case 0:
default:
max21100Data.accSign[0] = 1.0f;
max21100Data.accSign[1] = 1.0f;
max21100Data.accSign[2] = 1.0f;
max21100Data.gyoSign[0] = 1.0f;
max21100Data.gyoSign[1] = 1.0f;
max21100Data.gyoSign[2] = 1.0f;
break;
}
utilFilterInit(&max21100Data.tempFilter, DIMU_OUTER_DT, DIMU_TEMP_TAU, IMU_ROOM_TEMP);
max21100ReliablySetReg(0x22, 0x00); // BNK 00
// turn off I2C
max21100ReliablySetReg(0x16, 0x01);
// check device ID
while (max21100GetReg(0x20) != 0xb2)
;
// power up
max21100ReliablySetReg(0x00, 0b01111111); // Normal Mode GYO+ACC
// GYO scale @ 2KHz
#if MAX21100_GYO_SCALE == 250
max21100ReliablySetReg(0x01, 0b00100011);
#endif
#if MAX21100_GYO_SCALE == 500
max21100ReliablySetReg(0x01, 0b00100010);
#endif
#if MAX21100_GYO_SCALE == 1000
max21100ReliablySetReg(0x01, 0b00100001);
#endif
#if MAX21100_GYO_SCALE == 2000
max21100ReliablySetReg(0x01, 0b00100000);
#endif
max21100ReliablySetReg(0x02, 0b00100000); // GYO 8KHz ODR
// ACC scale
#if MAX21100_ACC_SCALE == 2
max21100ReliablySetReg(0x04, 0b11000111);
#endif
#if MAX21100_ACC_SCALE == 4
max21100ReliablySetReg(0x04, 0b10000111);
#endif
#if MAX21100_ACC_SCALE == 8
max21100ReliablySetReg(0x04, 0b01000111);
#endif
#if MAX21100_ACC_SCALE == 16
max21100ReliablySetReg(0x04, 0b00000111);
#endif
max21100ReliablySetReg(0x05, 0b00110000); // ACC 2KHz ODR
max21100SetReg(0x22, 0x01); // BNK 01
max21100ReliablySetReg(0x0b, 0b00100000); // IRQ config
max21100ReliablySetReg(0x0c, 0b00000000); // IRQ config
max21100SetReg(0x0d, 0b10000000); // IRQ config
max21100Data.readReg = MAX21100_READ_BIT | 0x24; // start of sensor registers
// External Interrupt line for data ready
extRegisterCallback(DIMU_MAX21100_INT_PORT, DIMU_MAX21100_INT_PIN, EXTI_Trigger_Rising, 1, GPIO_PuPd_NOPULL, max21100IntHandler);
spiChangeCallback(max21100Data.spi, max21100TransferComplete);
}
void mpu6000Init(void) {
switch ((int)p[IMU_FLIP]) {
case 1:
mpu6000Data.accSign[0] = 1.0f;
mpu6000Data.accSign[1] = -1.0f;
mpu6000Data.accSign[2] = -1.0f;
mpu6000Data.gyoSign[0] = 1.0f;
mpu6000Data.gyoSign[1] = -1.0f;
mpu6000Data.gyoSign[2] = -1.0f;
break;
case 2:
mpu6000Data.accSign[0] = -1.0f;
mpu6000Data.accSign[1] = 1.0f;
mpu6000Data.accSign[2] = -1.0f;
mpu6000Data.gyoSign[0] = -1.0f;
mpu6000Data.gyoSign[1] = 1.0f;
mpu6000Data.gyoSign[2] = -1.0f;
break;
case 0:
default:
mpu6000Data.accSign[0] = 1.0f;
mpu6000Data.accSign[1] = 1.0f;
mpu6000Data.accSign[2] = 1.0f;
mpu6000Data.gyoSign[0] = 1.0f;
mpu6000Data.gyoSign[1] = 1.0f;
mpu6000Data.gyoSign[2] = 1.0f;
break;
}
utilFilterInit(&mpu6000Data.tempFilter, DIMU_OUTER_DT, DIMU_TEMP_TAU, IMU_ROOM_TEMP);
// reset
mpu6000SetReg(107, 0b10000000);
delay(100);
// wake up w/ Z axis clock reg
mpu6000ReliablySetReg(107, 0x03);
// disable I2C interface
mpu6000ReliablySetReg(106, 0b010000);
// wait for a valid response
while (mpu6000GetReg(117) != 0x68)
delay(10);
// GYO scale
#if MPU6000_GYO_SCALE == 250
mpu6000ReliablySetReg(27, 0b00000);
#endif
#if MPU6000_GYO_SCALE == 500
mpu6000ReliablySetReg(27, 0b01000);
#endif
#if MPU6000_GYO_SCALE == 1000
mpu6000ReliablySetReg(27, 0b10000);
#endif
#if MPU6000_GYO_SCALE == 2000
mpu6000ReliablySetReg(27, 0b11000);
#endif
// ACC scale
#if MPU6000_ACC_SCALE == 2
mpu6000ReliablySetReg(28, 0b00000);
#endif
#if MPU6000_ACC_SCALE == 4
mpu6000ReliablySetReg(28, 0b01000);
#endif
#if MPU6000_ACC_SCALE == 8
mpu6000ReliablySetReg(28, 0b10000);
#endif
#if MPU6000_ACC_SCALE == 16
mpu6000ReliablySetReg(28, 0b11000);
#endif
// Sample rate
mpu6000ReliablySetReg(25, 0x00);
// LPF
mpu6000ReliablySetReg(26, 0x00);
// Interrupt setup
mpu6000ReliablySetReg(55, 0x01<<4);
mpu6000ReliablySetReg(56, 0x01);
// bump clock rate up to 21MHz
spiChangeBaud(mpu6000Data.spi, MPU6000_SPI_RUN_BAUD);
mpu6000Data.readReg = MPU6000_READ_BIT | 0x3b; // start of sensor registers
spiChangeCallback(mpu6000Data.spi, mpu6000TransferComplete);
// External Interrupt line for data ready
extRegisterCallback(DIMU_MPU6000_INT_PORT, DIMU_MPU6000_INT_PIN, EXTI_Trigger_Rising, 1, GPIO_PuPd_NOPULL, mpu6000IntHandler);
}
