bool verifympu6500WriteRegister(uint8_t reg, uint8_t data) {
uint8_t in;
uint8_t attemptsRemaining = 20;
mpu6500WriteRegister(reg, data);
delayMicroseconds(100);
do {
mpu6500SlowReadRegister(reg, 1, &in);
if (in == data) {
return true;
} else {
mpu6500WriteRegister(reg, data);
delayMicroseconds(100);
}
} while (attemptsRemaining--);
return true; //returning false here causes a usage fault
}
static void mpu6500GyroInit(void)
{
#ifdef NAZE
gpio_config_t gpio;
// MPU_INT output on rev5 hardware (PC13). rev4 was on PB13, conflicts with SPI devices
if (hse_value == 12000000) {
gpio.pin = Pin_13;
gpio.speed = Speed_2MHz;
gpio.mode = Mode_IN_FLOATING;
gpioInit(GPIOC, &gpio);
}
#endif
mpu6500WriteRegister(MPU6500_RA_PWR_MGMT_1, MPU6500_BIT_RESET);
delay(100);
mpu6500WriteRegister(MPU6500_RA_PWR_MGMT_1, 0);
delay(100);
mpu6500WriteRegister(MPU6500_RA_PWR_MGMT_1, INV_CLK_PLL);
mpu6500WriteRegister(MPU6500_RA_GYRO_CFG, INV_FSR_2000DPS << 3);
mpu6500WriteRegister(MPU6500_RA_ACCEL_CFG, INV_FSR_8G << 3);
mpu6500WriteRegister(MPU6500_RA_LPF, mpuLowPassFilter);
mpu6500WriteRegister(MPU6500_RA_RATE_DIV, 0); // 1kHz S/R
}
