private void SetScale(MPU6050Scale scale)
{
switch (scale)
{
case MPU6050Scale.MPU6050_SCALE_250DPS:
dpsPerDigit = .007633;
break;
case MPU6050Scale.MPU6050_SCALE_500DPS:
dpsPerDigit = .015267;
break;
case MPU6050Scale.MPU6050_SCALE_1000DPS:
dpsPerDigit = .030487;
break;
case MPU6050Scale.MPU6050_SCALE_2000DPS:
dpsPerDigit = .060975;
break;
}
int value = _device.ReadAddressByte(MPU6050_REG_GYRO_CONFIG);
value &= 0b11100111;
value |= ((int)scale << 3);
_device.WriteAddressByte(MPU6050_REG_GYRO_CONFIG, (byte)value);
}
public IMUDriver(int address, MPU6050Scale scale, MPU6050Range range)
{
_device = Pi.I2C.AddDevice(address);
if (!IsCorrectModel())
{
throw new Exception("Wrong gyro model detected");
}
SetClockSource(MPU6050ClockSource.MPU6050_CLOCK_PLL_XGYRO);
SetScale(scale);
SetRange(range);
SetSleepEnable(false);
}
