static void mpu6500GyroRead(int16_t *gyroADC)
{
    uint8_t buf[6];

    mpu6500ReadRegister(MPU6500_RA_GYRO_XOUT_H, buf, 6);

    gyroADC[X] = (int16_t)((buf[0] << 8) | buf[1]);
    gyroADC[Y] = (int16_t)((buf[2] << 8) | buf[3]);
    gyroADC[Z] = (int16_t)((buf[4] << 8) | buf[5]);
}
static void mpu6500AccRead(int16_t *accData)
{
    uint8_t buf[6];

    mpu6500ReadRegister(MPU6500_RA_ACCEL_XOUT_H, buf, 6);

    accData[X] = (int16_t)((buf[0] << 8) | buf[1]);
    accData[Y] = (int16_t)((buf[2] << 8) | buf[3]);
    accData[Z] = (int16_t)((buf[4] << 8) | buf[5]);
}
static bool mpu6500Detect(void)
{
    uint8_t tmp;

    mpu6500ReadRegister(MPU6500_RA_WHOAMI, &tmp, 1);
    if (tmp != MPU6500_WHO_AM_I_CONST)
        return false;

    return true;
}
bool mpu6500SpiDetect(void)
{
    uint8_t tmp;

    mpu6500SpiInit();

    mpu6500ReadRegister(MPU_RA_WHO_AM_I, 1, &tmp);

    if (tmp != MPU6500_WHO_AM_I_CONST)
        return false;

    return true;
}
bool mpu6500SpiDetect(void)
{
    uint8_t tmp;

    mpu6500SpiInit();

    mpu6500ReadRegister(MPU_RA_WHO_AM_I, 1, &tmp);

    if (tmp == MPU6500_WHO_AM_I_CONST || tmp == MPU9250_WHO_AM_I_CONST || tmp == ICM20608G_WHO_AM_I_CONST) {
        return true;
    }

    return false;
}