/// <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); }