/// <summary>
/// Tries to update the readings.
/// Returns true if new readings are available, otherwise false.
/// An exception is thrown if something goes wrong.
/// </summary>
public override bool Update()
{
var readings = new SensorReadings
{
Timestamp = DateTime.Now
};
byte status = I2CSupport.Read8Bits(_i2CDevice, HTS221Defines.STATUS, "Failed to read HTS221 status");
if ((status & 0x02) == 0x02)
{
Int16 rawHumidity = (Int16)I2CSupport.Read16Bits(_i2CDevice, HTS221Defines.HUMIDITY_OUT_L + 0x80, ByteOrder.LittleEndian, "Failed to read HTS221 humidity");
readings.Humidity = _humidityConversionFunc(rawHumidity);
readings.HumidityValid = true;
}
if ((status & 0x01) == 0x01)
{
Int16 rawTemperature = (Int16)I2CSupport.Read16Bits(_i2CDevice, HTS221Defines.TEMP_OUT_L + 0x80, ByteOrder.LittleEndian, "Failed to read HTS221 temperature");
readings.Temperature = _temperatureConversionFunc(rawTemperature);
readings.TemperatureValid = true;
}
if (readings.HumidityValid || readings.TemperatureValid)
{
AssignNewReadings(readings);
return(true);
}
return(false);
}
private Func <Int16, double> GetTemperatureConversionFunc()
{
byte tempRawMsb = I2CSupport.Read8Bits(_i2CDevice, HTS221Defines.T1_T0 + 0x80, "Failed to read HTS221 T1_T0");
byte temp0Lsb = I2CSupport.Read8Bits(_i2CDevice, HTS221Defines.T0_C_8 + 0x80, "Failed to read HTS221 T0_C_8");
UInt16 T0_C_8 = (UInt16)((((UInt16)tempRawMsb & 0x03) << 8) | (UInt16)temp0Lsb);
double T0 = T0_C_8 / 8.0;
byte temp1Lsb = I2CSupport.Read8Bits(_i2CDevice, HTS221Defines.T1_C_8 + 0x80, "Failed to read HTS221 T1_C_8");
UInt16 T1_C_8 = (UInt16)(((UInt16)(tempRawMsb & 0x0C) << 6) | (UInt16)temp1Lsb);
double T1 = T1_C_8 / 8.0;
Int16 T0_OUT = (Int16)I2CSupport.Read16Bits(_i2CDevice, HTS221Defines.T0_OUT + 0x80, ByteOrder.LittleEndian, "Failed to read HTS221 T0_OUT");
Int16 T1_OUT = (Int16)I2CSupport.Read16Bits(_i2CDevice, HTS221Defines.T1_OUT + 0x80, ByteOrder.LittleEndian, "Failed to read HTS221 T1_OUT");
// Temperature calibration slope
double m = (T1 - T0) / (T1_OUT - T0_OUT);
// Temperature calibration y intercept
double b = T0 - (m * T0_OUT);
return(rawTemperature => rawTemperature * m + b);
}
/// <summary>
/// Tries to update the readings.
/// Returns true if new readings are available, otherwise false.
/// An exception is thrown if something goes wrong.
/// </summary>
public override bool Update()
{
byte status = I2CSupport.Read8Bits(_i2CDevice, LPS25HDefines.STATUS_REG, "Failed to read LPS25H status");
var readings = new SensorReadings
{
Timestamp = DateTime.Now
};
if ((status & 0x02) == 0x02)
{
Int32 rawPressure = (Int32)I2CSupport.Read24Bits(_i2CDevice, LPS25HDefines.PRESS_OUT_XL + 0x80, ByteOrder.LittleEndian, "Failed to read LPS25H pressure");
readings.Pressure = rawPressure / 4096.0;
readings.PressureValid = true;
}
if ((status & 0x01) == 0x01)
{
Int16 rawTemperature = (Int16)I2CSupport.Read16Bits(_i2CDevice, LPS25HDefines.TEMP_OUT_L + 0x80, ByteOrder.LittleEndian, "Failed to read LPS25H temperature");
readings.Temperature = rawTemperature / 480.0 + 42.5;
readings.TemperatureValid = true;
}
if (readings.PressureValid || readings.TemperatureValid)
{
AssignNewReadings(readings);
return(true);
}
return(false);
}
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 VerifyDeviceMagId()
{
byte id = I2CSupport.Read8Bits(_magI2CDevice, LSM9DS1Defines.MAG_WHO_AM_I, "Failed to read LSM9DS1 mag id");
if (id != LSM9DS1Defines.MAG_ID)
{
throw new SensorException($"Incorrect LSM9DS1 mag id {id}");
}
}
private void VerifyDeviceAccelGyroId()
{
byte id = I2CSupport.Read8Bits(_accelGyroI2CDevice, LSM9DS1Defines.WHO_AM_I, "Failed to read LSM9DS1 accel/gyro id");
if (id != LSM9DS1Defines.ID)
{
throw new SensorException($"Incorrect LSM9DS1 gyro id {id}");
}
}
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");
}
/// <summary>
/// Tries to update the readings.
/// Returns true if new readings are available, otherwise false.
/// An exception is thrown if something goes wrong.
/// </summary>
public override bool Update()
{
byte status = I2CSupport.Read8Bits(_accelGyroI2CDevice, LSM9DS1Defines.STATUS, "Failed to read LSM9DS1 status");
if ((status & 0x03) != 0x03)
{
// Data not yet available
return(false);
}
byte[] gyroData = I2CSupport.ReadBytes(_accelGyroI2CDevice, 0x80 + LSM9DS1Defines.OUT_X_L_G, 6, "Failed to read LSM9DS1 gyro data");
byte[] accelData = I2CSupport.ReadBytes(_accelGyroI2CDevice, 0x80 + LSM9DS1Defines.OUT_X_L_XL, 6, "Failed to read LSM9DS1 accel data");
byte[] magData = I2CSupport.ReadBytes(_magI2CDevice, 0x80 + LSM9DS1Defines.MAG_OUT_X_L, 6, "Failed to read LSM9DS1 compass data");
var readings = new SensorReadings
{
Timestamp = DateTime.Now,
Gyro = MathSupport.ConvertToVector(gyroData, _gyroScale, ByteOrder.LittleEndian),
GyroValid = true,
Acceleration = MathSupport.ConvertToVector(accelData, _accelerationScale, ByteOrder.LittleEndian),
AccelerationValid = true,
MagneticField = MathSupport.ConvertToVector(magData, _magneticFieldScale, ByteOrder.LittleEndian),
MagneticFieldValid = true
};
// Sort out gyro axes and correct for bias
readings.Gyro.Z = -readings.Gyro.Z;
// Sort out accel data;
readings.Acceleration.X = -readings.Acceleration.X;
readings.Acceleration.Y = -readings.Acceleration.Y;
// Sort out mag axes
readings.MagneticField.X = -readings.MagneticField.X;
readings.MagneticField.Z = -readings.MagneticField.Z;
AssignNewReadings(readings);
return(true);
}
private Func <Int16, double> GetHumidityConversionFunc()
{
byte H0_H_2 = I2CSupport.Read8Bits(_i2CDevice, HTS221Defines.H0_H_2 + 0x80, "Failed to read HTS221 H0_H_2");
double H0 = H0_H_2 / 2.0;
byte H1_H_2 = I2CSupport.Read8Bits(_i2CDevice, HTS221Defines.H1_H_2 + 0x80, "Failed to read HTS221 H1_H_2");
double H1 = H1_H_2 / 2.0;
Int16 H0_T0_OUT = (Int16)I2CSupport.Read16Bits(_i2CDevice, HTS221Defines.H0_T0_OUT + 0x80, ByteOrder.LittleEndian, "Failed to read HTS221 H0_T_OUT");
Int16 H1_T0_OUT = (Int16)I2CSupport.Read16Bits(_i2CDevice, HTS221Defines.H1_T0_OUT + 0x80, ByteOrder.LittleEndian, "Failed to read HTS221 H1_T_OUT");
// Humidity calibration slope
double m = (H1 - H0) / (H1_T0_OUT - H0_T0_OUT);
// Humidity calibration y intercept
double b = H0 - (m * H0_T0_OUT);
return(rawHumidity => rawHumidity * m + b);
}
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);
}
