spiClient_t *spiClientInit(SPI_TypeDef *spi, uint16_t baud, GPIO_TypeDef *csPort, uint16_t csPin, volatile uint32_t *flag, spiCallback_t *callback) {
spiClient_t *client;
client = (spiClient_t *)aqCalloc(1, sizeof(spiClient_t));
#ifdef SPI_SPI1_CLOCK
if (spi == SPI1) {
client->interface = 0;
spi1Init();
}
#endif
#ifdef SPI_SPI2_CLOCK
if (spi == SPI2) {
client->interface = 1;
spi2Init();
}
#endif
#ifdef SPI_SPI3_CLOCK
if (spi == SPI3) {
client->interface = 2;
spi3Init();
}
#endif
client->cs = digitalInit(csPort, csPin, 1);
spiDeselect(client);
spiChangeBaud(client, baud);
client->flag = flag;
client->callback = callback;
return client;
}
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);
}