public bool HumidityRead(ref RTIMUData data)
{
byte[] oneByte = new byte[1];
byte[] twoByte = new byte[2];
data.humidityValid = false;
data.temperatureValid = false;
data.temperature = 0;
data.humidity = 0;
if (!RTI2C.Read(mHum, HTS221_STATUS, oneByte))
{
ErrorMessage = "Failed to read HTS221 status";
return(false);
}
if ((oneByte[0] & 2) == 2)
{
if (!RTI2C.Read(mHum, HTS221_HUMIDITY_OUT_L + 0x80, twoByte))
{
ErrorMessage = "Failed to read HTS221 humidity";
return(false);
}
mHumidity = (Int16)((((UInt16)twoByte[1]) << 8) | (UInt16)twoByte[0]);
mHumidity = mHumidity * mHumidity_m + mHumidity_c;
mHumidityValid = true;
}
if ((oneByte[0] & 1) == 1)
{
if (!RTI2C.Read(mHum, HTS221_TEMP_OUT_L + 0x80, twoByte))
{
ErrorMessage = "Failed to read HTS221 temperature";
return(false);
}
mTemperature = (Int16)((((UInt16)twoByte[1]) << 8) | (UInt16)twoByte[0]);
mTemperature = mTemperature * mTemperature_m + mTemperature_c;
mTemperatureValid = true;
}
data.humidityValid = mHumidityValid;
data.humidity = mHumidity;
data.temperatureValid = mTemperatureValid;
data.temperature = mTemperature;
return(true);
}
public bool PressureRead(ref RTIMUData data)
{
byte[] oneByte = new byte[1];
byte[] twoByte = new byte[2];
byte[] threeByte = new byte[3];
data.pressureValid = false;
data.temperatureValid = false;
data.temperature = 0;
data.pressure = 0;
if (!RTI2C.Read(mPress, LPS25H_STATUS_REG, oneByte))
{
ErrorMessage = "Failed to read LPS25H status";
return(false);
}
if ((oneByte[0] & 2) == 2)
{
if (!RTI2C.Read(mPress, LPS25H_PRESS_OUT_XL + 0x80, threeByte))
{
ErrorMessage = "Failed to read LPS25H pressure";
return(false);
}
mPressure = (double)((((UInt32)threeByte[2]) << 16) | (((UInt32)threeByte[1]) << 8) | (UInt32)threeByte[0]) / (double)4096;
mPressureValid = true;
}
if ((oneByte[0] & 1) == 1)
{
if (!RTI2C.Read(mPress, LPS25H_TEMP_OUT_L + 0x80, twoByte))
{
ErrorMessage = "Failed to read LPS25H temperature";
return(false);
}
mTemperature = (Int16)((((UInt16)twoByte[1]) << 8) | (UInt16)twoByte[0]) / (double)480 + (double)42.5;
mTemperatureValid = true;
}
data.pressureValid = mPressureValid;
data.pressure = mPressure;
data.temperatureValid = mTemperatureValid;
data.temperature = mTemperature;
return(true);
}
public async void HumidityInit()
{
byte[] oneByte = new byte[1];
byte[] twoByte = new byte[2];
byte H0_H_2 = 0;
byte H1_H_2 = 0;
UInt16 T0_C_8 = 0;
UInt16 T1_C_8 = 0;
Int16 H0_T0_OUT = 0;
Int16 H1_T0_OUT = 0;
Int16 T0_OUT = 0;
Int16 T1_OUT = 0;
double H0, H1, T0, T1;
try {
string aqsFilter = I2cDevice.GetDeviceSelector("I2C1");
DeviceInformationCollection collection = await DeviceInformation.FindAllAsync(aqsFilter);
if (collection.Count == 0)
{
return;
}
I2cConnectionSettings settings0 = new I2cConnectionSettings(HTS221_ADDRESS);
settings0.BusSpeed = I2cBusSpeed.FastMode;
mHum = await I2cDevice.FromIdAsync(collection[0].Id, settings0);
} catch (Exception) {
ErrorMessage = "Failed to connect to HTS221";
if (Debugger.IsAttached)
{
Debugger.Break();
}
return;
}
if (!RTI2C.Write(mHum, HTS221_CTRL1, 0x87))
{
ErrorMessage = "Failed to set HTS221 CTRL_REG_1";
return;
}
if (!RTI2C.Write(mHum, HTS221_AV_CONF, 0x1b))
{
ErrorMessage = "Failed to set HTS221 AV_CONF";
return;
}
// Get calibration data
if (!RTI2C.Read(mHum, HTS221_T1_T0 + 0x80, oneByte))
{
ErrorMessage = "Failed to read HTS221 T1_T0";
return;
}
byte temp0 = oneByte[0];
if (!RTI2C.Read(mHum, HTS221_T0_C_8 + 0x80, oneByte))
{
ErrorMessage = "Failed to read HTS221 T0_C_8";
return;
}
byte temp1 = oneByte[0];
T0_C_8 = (UInt16)((((UInt16)temp1 & 0x3) << 8) | (UInt16)temp0);
T0 = (double)T0_C_8 / 8.0;
if (!RTI2C.Read(mHum, HTS221_T1_C_8 + 0x80, oneByte))
{
ErrorMessage = "Failed to read HTS221 T1_C_8";
return;
}
temp0 = oneByte[0];
T1_C_8 = (UInt16)(((UInt16)(temp1 & 0xC) << 6) | (UInt16)temp0);
T1 = (double)T1_C_8 / 8.0;
if (!RTI2C.Read(mHum, HTS221_T0_OUT + 0x80, twoByte))
{
ErrorMessage = "Failed to read HTS221 T0_OUT";
return;
}
T0_OUT = (Int16)((((UInt16)twoByte[1]) << 8) | (UInt16)twoByte[0]);
if (!RTI2C.Read(mHum, HTS221_T1_OUT + 0x80, twoByte))
{
ErrorMessage = "Failed to read HTS221 T1_OUT";
return;
}
T1_OUT = (Int16)((((UInt16)twoByte[1]) << 8) | (UInt16)twoByte[0]);
if (!RTI2C.Read(mHum, HTS221_H0_H_2 + 0x80, oneByte))
{
ErrorMessage = "Failed to read HTS221 H0_H_2";
return;
}
H0_H_2 = oneByte[0];
H0 = (double)H0_H_2 / 2.0;
if (!RTI2C.Read(mHum, HTS221_H1_H_2 + 0x80, oneByte))
{
ErrorMessage = "Failed to read HTS221 H1_H_2";
return;
}
H1_H_2 = oneByte[0];
H1 = (double)H1_H_2 / 2.0;
if (!RTI2C.Read(mHum, HTS221_H0_T0_OUT + 0x80, twoByte))
{
ErrorMessage = "Failed to read HTS221 H0_T_OUT";
return;
}
H0_T0_OUT = (Int16)((((UInt16)twoByte[1]) << 8) | (UInt16)twoByte[0]);
if (!RTI2C.Read(mHum, HTS221_H1_T0_OUT + 0x80, twoByte))
{
ErrorMessage = "Failed to read HTS221 H1_T_OUT";
return;
}
H1_T0_OUT = (Int16)((((UInt16)twoByte[1]) << 8) | (UInt16)twoByte[0]);
mTemperature_m = (T1 - T0) / (T1_OUT - T0_OUT);
mTemperature_c = T0 - (mTemperature_m * T0_OUT);
mHumidity_m = (H1 - H0) / (H1_T0_OUT - H0_T0_OUT);
mHumidity_c = (H0)-(mHumidity_m * H0_T0_OUT);
mInitComplete = true;
ErrorMessage = "HTS221 init complete";
return;
}
public bool IMURead(out RTIMUData data)
{
byte[] status = new byte[1];
byte[] gyroData = new byte[6];
byte[] accelData = new byte[6];
byte[] magData = new byte[6];
mImuData = new RTIMUData();
data = mImuData;
// set validity flags
mImuData.fusionPoseValid = false;
mImuData.fusionQPoseValid = false;
mImuData.gyroValid = true;
mImuData.accelValid = true;
mImuData.magValid = true;
mImuData.pressureValid = false;
mImuData.temperatureValid = false;
mImuData.humidityValid = false;
if (!RTI2C.Read(mAccelGyro, LSM9DS1_STATUS, status))
{
ErrorMessage = "Failed to read LSM9DS1 status";
return(false);
}
if ((status[0] & 0x3) != 3)
{
return(false);
}
if (!RTI2C.Read(mAccelGyro, 0x80 + LSM9DS1_OUT_X_L_G, gyroData))
{
ErrorMessage = "Failed to read LSM9DS1 gyro data";
return(false);
}
if (!RTI2C.Read(mAccelGyro, 0x80 + LSM9DS1_OUT_X_L_XL, accelData))
{
ErrorMessage = "Failed to read LSM9DS1 accel data";
return(false);
}
if (!RTI2C.Read(mMag, 0x80 + LSM9DS1_MAG_OUT_X_L, magData))
{
ErrorMessage = "Failed to read LSM9DS1 compass data";
return(false);
}
mImuData.timestamp = System.DateTime.Now.Ticks / (long)10;
RTMath.ConvertToVector(gyroData, out mImuData.gyro, mGyroScale, false);
RTMath.ConvertToVector(accelData, out mImuData.accel, mAccelScale, false);
RTMath.ConvertToVector(magData, out mImuData.mag, mMagScale, false);
// sort out gyro axes and correct for bias
mImuData.gyro.Z = -mImuData.gyro.Z;
// sort out accel data;
mImuData.accel.X = -mImuData.accel.X;
mImuData.accel.Y = -mImuData.accel.Y;
// sort out mag axes
mImuData.mag.X = -mImuData.mag.X;
mImuData.mag.Z = -mImuData.mag.Z;
// now do standard processing
HandleGyroBias();
CalibrateAverageCompass();
data = mImuData;
return(true);
}
public async void IMUInit()
{
byte[] oneByte = new byte[1];
// open the I2C devices
try {
string aqsFilter = I2cDevice.GetDeviceSelector("I2C1");
DeviceInformationCollection collection = await DeviceInformation.FindAllAsync(aqsFilter);
if (collection.Count == 0)
{
return;
}
I2cConnectionSettings settings0 = new I2cConnectionSettings(AccelGyroAddress);
settings0.BusSpeed = I2cBusSpeed.FastMode;
mAccelGyro = await I2cDevice.FromIdAsync(collection[0].Id, settings0);
I2cConnectionSettings settings1 = new I2cConnectionSettings(MagAddress);
settings1.BusSpeed = I2cBusSpeed.FastMode;
mMag = await I2cDevice.FromIdAsync(collection[0].Id, settings1);
} catch (Exception) {
ErrorMessage = "Failed to connect to IMU";
if (Debugger.IsAttached)
{
Debugger.Break();
}
return;
}
// Set up the gyro/accel
if (!RTI2C.Write(mAccelGyro, LSM9DS1_CTRL8, 0x81))
{
ErrorMessage = "Failed to boot LSM9DS1";
return;
}
await Task.Delay(100);
if (!RTI2C.Read(mAccelGyro, LSM9DS1_WHO_AM_I, oneByte))
{
ErrorMessage = "Failed to read LSM9DS1 accel/gyro id";
return;
}
if (oneByte[0] != LSM9DS1_ID)
{
ErrorMessage = string.Format("Incorrect LSM9DS1 gyro id {0}", oneByte[0]);
return;
}
if (!SetGyroSampleRate())
{
return;
}
if (!SetGyroCTRL3())
{
return;
}
// Set up the mag
if (!RTI2C.Read(mMag, LSM9DS1_MAG_WHO_AM_I, oneByte))
{
ErrorMessage = "Failed to read LSM9DS1 accel/mag id";
return;
}
if (oneByte[0] != LSM9DS1_MAG_ID)
{
ErrorMessage = string.Format("Incorrect LSM9DS1 accel/mag id {0}", oneByte[0]);
return;
}
if (!SetAccelCTRL6())
{
return;
}
if (!SetAccelCTRL7())
{
return;
}
if (!SetMagCTRL1())
{
return;
}
if (!SetMagCTRL2())
{
return;
}
if (!SetMagCTRL3())
{
return;
}
GyroBiasInit();
ErrorMessage = "IMU init completed";
InitComplete = true;
return;
}
