private void SetMagCtrl2()
{
byte ctrl2;
// convert FSR to uT
switch (_config.MagneticFullScaleRange)
{
case MagneticFullScaleRange.Range4Gauss:
ctrl2 = 0;
_magneticFieldScale = 0.014;
break;
case MagneticFullScaleRange.Range8Gauss:
ctrl2 = 0x20;
_magneticFieldScale = 0.029;
break;
case MagneticFullScaleRange.Range12Gauss:
ctrl2 = 0x40;
_magneticFieldScale = 0.043;
break;
case MagneticFullScaleRange.Range16Gauss:
ctrl2 = 0x60;
_magneticFieldScale = 0.058;
break;
default:
throw new SensorException($"Illegal LSM9DS1 compass FSR code {_config.MagneticFullScaleRange}");
}
I2CSupport.Write(_magI2CDevice, LSM9DS1Defines.MAG_CTRL2, ctrl2, "Failed to set LSM9DS1 compass CTRL6");
}
private void SetAccelCtrl7()
{
byte ctrl7 = 0x00;
//Bug: Bad things happen.
//byte ctrl7 = 0x05;
I2CSupport.Write(_accelGyroI2CDevice, LSM9DS1Defines.CTRL7, ctrl7, "Failed to set LSM9DS1 accel CTRL7");
}
private void SetMagCtrl1()
{
int compassSampleRateValue = (int)_config.CompassSampleRate;
if ((compassSampleRateValue < 0) || (compassSampleRateValue > 7))
{
throw new SensorException($"Illegal LSM9DS1 compass sample rate code {_config.CompassSampleRate}");
}
byte ctrl1 = (byte)(compassSampleRateValue << 2);
I2CSupport.Write(_magI2CDevice, LSM9DS1Defines.MAG_CTRL1, ctrl1, "Failed to set LSM9DS1 compass CTRL5");
}
protected override async Task <bool> InitDeviceAsync()
{
await ConnectToI2CDevices();
I2CSupport.Write(_i2CDevice, HTS221Defines.CTRL1, 0x87, "Failed to set HTS221 CTRL_REG_1");
I2CSupport.Write(_i2CDevice, HTS221Defines.AV_CONF, 0x1b, "Failed to set HTS221 AV_CONF");
_temperatureConversionFunc = GetTemperatureConversionFunc();
_humidityConversionFunc = GetHumidityConversionFunc();
return(true);
}
protected override async Task <bool> InitDeviceAsync()
{
await ConnectToI2CDevices();
I2CSupport.Write(_i2CDevice, LPS25HDefines.CTRL_REG_1, 0xc4, "Failed to set LPS25H CTRL_REG_1");
I2CSupport.Write(_i2CDevice, LPS25HDefines.RES_CONF, 0x05, "Failed to set LPS25H RES_CONF");
I2CSupport.Write(_i2CDevice, LPS25HDefines.FIFO_CTRL, 0xc0, "Failed to set LPS25H FIFO_CTRL");
I2CSupport.Write(_i2CDevice, LPS25HDefines.CTRL_REG_2, 0x40, "Failed to set LPS25H CTRL_REG_2");
return(true);
}
private void SetGyroCtrl3()
{
int gyroHighPassFilterCodeValue = (int)_config.GyroHighPassFilterCode;
if ((gyroHighPassFilterCodeValue < 0) || (gyroHighPassFilterCodeValue > 9))
{
throw new SensorException($"Illegal LSM9DS1 gyro high pass filter code {_config.GyroHighPassFilterCode}");
}
byte ctrl3 = (byte)gyroHighPassFilterCodeValue;
// Turn on hpf
ctrl3 |= 0x40;
I2CSupport.Write(_accelGyroI2CDevice, LSM9DS1Defines.CTRL3, ctrl3, "Failed to set LSM9DS1 gyro CTRL3");
}
private void SetAccelCtrl6()
{
int accelSampleRateValue = (int)_config.AccelSampleRate;
if ((accelSampleRateValue < 1) || (accelSampleRateValue > 6))
{
throw new SensorException($"Illegal LSM9DS1 accel sample rate code {_config.AccelSampleRate}");
}
int accelLowPassFilterValue = (int)_config.AccelLowPassFilter;
if ((accelLowPassFilterValue < 0) || (accelLowPassFilterValue > 3))
{
throw new SensorException($"Illegal LSM9DS1 accel low pass fiter code {_config.AccelLowPassFilter}");
}
byte ctrl6 = (byte)(accelSampleRateValue << 5);
switch (_config.AccelFullScaleRange)
{
case AccelFullScaleRange.Range2g:
_accelerationScale = 0.000061;
break;
case AccelFullScaleRange.Range4g:
_accelerationScale = 0.000122;
break;
case AccelFullScaleRange.Range8g:
_accelerationScale = 0.000244;
break;
case AccelFullScaleRange.Range16g:
_accelerationScale = 0.000732;
break;
default:
throw new SensorException($"Illegal LSM9DS1 accel FSR code {_config.AccelFullScaleRange}");
}
ctrl6 |= (byte)((accelLowPassFilterValue) | (accelSampleRateValue << 3));
I2CSupport.Write(_accelGyroI2CDevice, LSM9DS1Defines.CTRL6, ctrl6, "Failed to set LSM9DS1 accel CTRL6");
}
private void SetMagCtrl3()
{
I2CSupport.Write(_magI2CDevice, LSM9DS1Defines.MAG_CTRL3, 0x00, "Failed to set LSM9DS1 compass CTRL3");
}
private void SetGyroSampleRate()
{
byte ctrl1;
switch (_config.GyroSampleRate)
{
case GyroSampleRate.Freq14_9Hz:
ctrl1 = 0x20;
SampleRate = 15;
break;
case GyroSampleRate.Freq59_5Hz:
ctrl1 = 0x40;
SampleRate = 60;
break;
case GyroSampleRate.Freq119Hz:
ctrl1 = 0x60;
SampleRate = 119;
break;
case GyroSampleRate.Freq238Hz:
ctrl1 = 0x80;
SampleRate = 238;
break;
case GyroSampleRate.Freq476Hz:
ctrl1 = 0xa0;
SampleRate = 476;
break;
case GyroSampleRate.Freq952Hz:
ctrl1 = 0xc0;
SampleRate = 952;
break;
default:
throw new SensorException($"Illegal LSM9DS1 gyro sample rate code {_config.GyroSampleRate}");
}
SampleInterval = (long)1000000 / SampleRate;
switch (_config.GyroBandwidthCode)
{
case GyroBandwidthCode.BandwidthCode0:
ctrl1 |= 0x00;
break;
case GyroBandwidthCode.BandwidthCode1:
ctrl1 |= 0x01;
break;
case GyroBandwidthCode.BandwidthCode2:
ctrl1 |= 0x02;
break;
case GyroBandwidthCode.BandwidthCode3:
ctrl1 |= 0x03;
break;
default:
throw new SensorException($"Illegal LSM9DS1 gyro BW code {_config.GyroBandwidthCode}");
}
switch (_config.GyroFullScaleRange)
{
case GyroFullScaleRange.Range245:
ctrl1 |= 0x00;
_gyroScale = 0.00875 * MathSupport.DegreeToRad;
break;
case GyroFullScaleRange.Range500:
ctrl1 |= 0x08;
_gyroScale = 0.0175 * MathSupport.DegreeToRad;
break;
case GyroFullScaleRange.Range2000:
ctrl1 |= 0x18;
_gyroScale = 0.07 * MathSupport.DegreeToRad;
break;
default:
throw new SensorException($"Illegal LSM9DS1 gyro FSR code {_config.GyroFullScaleRange}");
}
I2CSupport.Write(_accelGyroI2CDevice, LSM9DS1Defines.CTRL1, ctrl1, "Failed to set LSM9DS1 gyro CTRL1");
}
private void BootDevice()
{
I2CSupport.Write(_accelGyroI2CDevice, LSM9DS1Defines.CTRL8, 0x81, "Failed to boot LSM9DS1");
Thread.Sleep(100);
}
private async Task BootDevice()
{
I2CSupport.Write(_accelGyroI2CDevice, LSM9DS1Defines.CTRL8, 0x81, "Failed to boot LSM9DS1");
await Task.Delay(100);
}
