bool SetMagCTRL3()
{
if (!RTI2C.Write(mMag, LSM9DS1_MAG_CTRL3, 0x00))
{
ErrorMessage = "Failed to set LSM9DS1 compass CTRL3";
return(false);
}
return(true);
}
public async void PressureInit()
{
try {
string aqsFilter = I2cDevice.GetDeviceSelector("I2C1");
DeviceInformationCollection collection = await DeviceInformation.FindAllAsync(aqsFilter);
if (collection.Count == 0)
{
return;
}
I2cConnectionSettings settings0 = new I2cConnectionSettings(LPS25H_ADDRESS0);
settings0.BusSpeed = I2cBusSpeed.FastMode;
mPress = await I2cDevice.FromIdAsync(collection[0].Id, settings0);
} catch (Exception) {
ErrorMessage = "Failed to connect to LPS25H";
if (Debugger.IsAttached)
{
Debugger.Break();
}
return;
}
if (!RTI2C.Write(mPress, LPS25H_CTRL_REG_1, 0xc4))
{
ErrorMessage = "Failed to set LPS25H CTRL_REG_1";
return;
}
if (!RTI2C.Write(mPress, LPS25H_RES_CONF, 0x05))
{
ErrorMessage = "Failed to set LPS25H RES_CONF";
return;
}
if (!RTI2C.Write(mPress, LPS25H_FIFO_CTRL, 0xc0))
{
ErrorMessage = "Failed to set LPS25H FIFO_CTRL";
return;
}
if (!RTI2C.Write(mPress, LPS25H_CTRL_REG_2, 0x40))
{
ErrorMessage = "Failed to set LPS25H CTRL_REG_2";
return;
}
mInitComplete = true;
ErrorMessage = "LPS25H init complete";
return;
}
bool SetAccelCTRL6()
{
byte ctrl6;
if ((mLSM9DS1AccelSampleRate < 0) || (mLSM9DS1AccelSampleRate > 6))
{
ErrorMessage = string.Format("Illegal LSM9DS1 accel sample rate code {0}", mLSM9DS1AccelSampleRate);
return(false);
}
ctrl6 = (byte)(mLSM9DS1AccelSampleRate << 5);
if ((mLSM9DS1AccelLpf < 0) || (mLSM9DS1AccelLpf > 3))
{
ErrorMessage = string.Format("Illegal LSM9DS1 accel low pass fiter code {0}", mLSM9DS1AccelLpf);
return(false);
}
switch (mLSM9DS1AccelFsr)
{
case LSM9DS1_ACCEL_FSR_2:
mAccelScale = (double)0.000061;
break;
case LSM9DS1_ACCEL_FSR_4:
mAccelScale = (double)0.000122;
break;
case LSM9DS1_ACCEL_FSR_8:
mAccelScale = (double)0.000244;
break;
case LSM9DS1_ACCEL_FSR_16:
mAccelScale = (double)0.000732;
break;
default:
ErrorMessage = string.Format("Illegal LSM9DS1 accel FSR code {0}", mLSM9DS1AccelFsr);
return(false);
}
ctrl6 |= (byte)((mLSM9DS1AccelLpf) | (mLSM9DS1AccelFsr << 3));
if (!RTI2C.Write(mAccelGyro, LSM9DS1_CTRL6, ctrl6))
{
ErrorMessage = "Failed to set LSM9DS1 accel CTRL6";
return(false);
}
return(true);
}
bool SetAccelCTRL7()
{
byte ctrl7;
ctrl7 = 0x00;
//Bug: Bad things happen.
//ctrl7 = 0x05;
if (!RTI2C.Write(mAccelGyro, LSM9DS1_CTRL7, ctrl7))
{
ErrorMessage = "Failed to set LSM9DS1 accel CTRL7";
return(false);
}
return(true);
}
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);
}
bool SetMagCTRL1()
{
byte ctrl1;
if ((mLSM9DS1CompassSampleRate < 0) || (mLSM9DS1CompassSampleRate > 5))
{
ErrorMessage = string.Format("Illegal LSM9DS1 compass sample rate code {0}", mLSM9DS1CompassSampleRate);
return(false);
}
ctrl1 = (byte)(mLSM9DS1CompassSampleRate << 2);
if (!RTI2C.Write(mMag, LSM9DS1_MAG_CTRL1, ctrl1))
{
ErrorMessage = "Failed to set LSM9DS1 compass CTRL5";
return(false);
}
return(true);
}
bool SetGyroCTRL3()
{
byte ctrl3;
if ((mLSM9DS1GyroHpf < LSM9DS1_GYRO_HPF_0) || (mLSM9DS1GyroHpf > LSM9DS1_GYRO_HPF_9))
{
ErrorMessage = string.Format("Illegal LSM9DS1 gyro high pass filter code {0}", mLSM9DS1GyroHpf);
return(false);
}
ctrl3 = mLSM9DS1GyroHpf;
// Turn on hpf
ctrl3 |= 0x40;
if (!RTI2C.Write(mAccelGyro, LSM9DS1_CTRL3, ctrl3))
{
ErrorMessage = "Failed to set LSM9DS1 gyro CTRL3";
return(false);
}
return(true);
}
bool SetMagCTRL2()
{
byte ctrl2;
// convert FSR to uT
switch (mLSM9DS1CompassFsr)
{
case LSM9DS1_COMPASS_FSR_4:
ctrl2 = 0;
mMagScale = (double)0.014;
break;
case LSM9DS1_COMPASS_FSR_8:
ctrl2 = 0x20;
mMagScale = (double)0.029;
break;
case LSM9DS1_COMPASS_FSR_12:
ctrl2 = 0x40;
mMagScale = (double)0.043;
break;
case LSM9DS1_COMPASS_FSR_16:
ctrl2 = 0x60;
mMagScale = (double)0.058;
break;
default:
ErrorMessage = string.Format("Illegal LSM9DS1 compass FSR code {0}", mLSM9DS1CompassFsr);
return(false);
}
if (!RTI2C.Write(mMag, LSM9DS1_MAG_CTRL2, ctrl2))
{
ErrorMessage = "Failed to set LSM9DS1 compass CTRL6";
return(false);
}
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);
}
bool SetGyroSampleRate()
{
byte ctrl1;
switch (mLSM9DS1GyroSampleRate)
{
case LSM9DS1_GYRO_SAMPLERATE_14_9:
ctrl1 = 0x20;
mSampleRate = 15;
break;
case LSM9DS1_GYRO_SAMPLERATE_59_5:
ctrl1 = 0x40;
mSampleRate = 60;
break;
case LSM9DS1_GYRO_SAMPLERATE_119:
ctrl1 = 0x60;
mSampleRate = 119;
break;
case LSM9DS1_GYRO_SAMPLERATE_238:
ctrl1 = 0x80;
mSampleRate = 238;
break;
case LSM9DS1_GYRO_SAMPLERATE_476:
ctrl1 = 0xa0;
mSampleRate = 476;
break;
case LSM9DS1_GYRO_SAMPLERATE_952:
ctrl1 = 0xc0;
mSampleRate = 952;
break;
default:
ErrorMessage = string.Format("Illegal LSM9DS1 gyro sample rate code {0}", mLSM9DS1GyroSampleRate);
return(false);
}
mSampleInterval = (long)1000000 / mSampleRate;
switch (mLSM9DS1GyroBW)
{
case LSM9DS1_GYRO_BANDWIDTH_0:
ctrl1 |= 0x00;
break;
case LSM9DS1_GYRO_BANDWIDTH_1:
ctrl1 |= 0x01;
break;
case LSM9DS1_GYRO_BANDWIDTH_2:
ctrl1 |= 0x02;
break;
case LSM9DS1_GYRO_BANDWIDTH_3:
ctrl1 |= 0x03;
break;
}
switch (mLSM9DS1GyroFsr)
{
case LSM9DS1_GYRO_FSR_250:
ctrl1 |= 0x00;
mGyroScale = (double)0.00875 * RTMath.RTMATH_DEGREE_TO_RAD;
break;
case LSM9DS1_GYRO_FSR_500:
ctrl1 |= 0x08;
mGyroScale = (double)0.0175 * RTMath.RTMATH_DEGREE_TO_RAD;
break;
case LSM9DS1_GYRO_FSR_2000:
ctrl1 |= 0x18;
mGyroScale = (double)0.07 * RTMath.RTMATH_DEGREE_TO_RAD;
break;
default:
ErrorMessage = string.Format("Illegal LSM9DS1 gyro FSR code {0}", mLSM9DS1GyroFsr);
return(false);
}
if (!RTI2C.Write(mAccelGyro, LSM9DS1_CTRL1, ctrl1))
{
ErrorMessage = "Failed to set LSM9DS1 gyro CTRL1";
return(false);
}
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;
}
