private void SetRange(MPU6050Range range)
{
switch (range)
{
case MPU6050Range.MPU6050_RANGE_2G:
rangePerDigit = .000061;
break;
case MPU6050Range.MPU6050_RANGE_4G:
rangePerDigit = .000122;
break;
case MPU6050Range.MPU6050_RANGE_8G:
rangePerDigit = .000244;
break;
case MPU6050Range.MPU6050_RANGE_16G:
rangePerDigit = .0004882;
break;
default:
break;
}
int value = _device.ReadAddressByte(MPU6050_REG_ACCEL_CONFIG);
value &= 0b11100111;
value |= ((int)range << 3);
_device.WriteAddressByte(MPU6050_REG_ACCEL_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);
}
