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 void NewIMUData(ref RTIMUData data)
{
mSampleNumber++;
if (EnableGyro)
{
mGyro = data.gyro;
}
else
{
mGyro = new RTVector3();
}
mAccel = data.accel;
mMag = data.mag;
mMagValid = data.magValid;
if (mFirstTime)
{
mLastFusionTime = data.timestamp;
CalculatePose(mAccel, mMag, CompassAdjDeclination);
// initialize the poses
mStateQ.FromEuler(mMeasuredPose);
mFusionQPose = mStateQ;
mFusionPose = mMeasuredPose;
mFirstTime = false;
}
else
{
mTimeDelta = (double)(data.timestamp - mLastFusionTime) / (double)1000000;
if (mTimeDelta > 0)
{
CalculatePose(mAccel, mMag, CompassAdjDeclination);
Predict();
Update();
mStateQ.ToEuler(out mFusionPose);
mFusionQPose = mStateQ;
}
mLastFusionTime = data.timestamp;
}
data.fusionPoseValid = true;
data.fusionQPoseValid = true;
data.fusionPose = mFusionPose;
data.fusionQPose = mFusionQPose;
}
public RTIMUThread()
{
mImu.IMUInit();
mPressure.PressureInit();
mHumidity.HumidityInit();
mStartTime = System.DateTime.Now.Ticks;
Task.Run(() =>
{
while (true) {
Task.Delay(2);
if (mImu.InitComplete) {
RTIMUData data;
while (mImu.IMURead(out data)) {
mSampleCount++;
// collect all of the data
mFusion.NewIMUData(ref data);
mHumidity.HumidityRead(ref data);
mPressure.PressureRead(ref data);
// data is now ready for procesing - just pass to display in this case
lock (this) {
mImuData = data;
}
}
}
if ((System.DateTime.Now.Ticks - mStartTime) >= 10000000) {
mStartTime = System.DateTime.Now.Ticks;
mSampleRate = mSampleCount;
mSampleCount = 0;
}
}
});
}
public void NewIMUData(ref RTIMUData data)
{
mSampleNumber++;
if (EnableGyro)
mGyro = data.gyro;
else
mGyro = new RTVector3();
mAccel = data.accel;
mMag = data.mag;
mMagValid = data.magValid;
if (mFirstTime) {
mLastFusionTime = data.timestamp;
CalculatePose(mAccel, mMag, CompassAdjDeclination);
// initialize the poses
mStateQ.FromEuler(mMeasuredPose);
mFusionQPose = mStateQ;
mFusionPose = mMeasuredPose;
mFirstTime = false;
} else {
mTimeDelta = (double)(data.timestamp - mLastFusionTime) / (double)1000000;
if (mTimeDelta > 0) {
CalculatePose(mAccel, mMag, CompassAdjDeclination);
Predict();
Update();
mStateQ.ToEuler(out mFusionPose);
mFusionQPose = mStateQ;
}
mLastFusionTime = data.timestamp;
}
data.fusionPoseValid = true;
data.fusionQPoseValid = true;
data.fusionPose = mFusionPose;
data.fusionQPose = mFusionQPose;
}
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;
}
// Note - code assumes that this is the first thing called after axis swapping
// for each specific IMU chip has occurred.
protected void HandleGyroBias()
{
// do axis rotation
if ((mAxisRotation > 0) && (mAxisRotation < RTIMU_AXIS_ROTATION_COUNT))
{
// need to do an axis rotation
RTIMUData tempIMU = mImuData;
// do new x value
if (mAxisRotationArray[mAxisRotation, 0] != 0)
{
mImuData.gyro.X = tempIMU.gyro.X * mAxisRotationArray[mAxisRotation, 0];
mImuData.accel.X = tempIMU.accel.X * mAxisRotationArray[mAxisRotation, 0];
mImuData.mag.X = tempIMU.mag.X * mAxisRotationArray[mAxisRotation, 0];
}
else if (mAxisRotationArray[mAxisRotation, 1] != 0)
{
mImuData.gyro.X = tempIMU.gyro.Y * mAxisRotationArray[mAxisRotation, 1];
mImuData.accel.X = tempIMU.accel.Y * mAxisRotationArray[mAxisRotation, 1];
mImuData.mag.X = tempIMU.mag.Y * mAxisRotationArray[mAxisRotation, 1];
}
else if (mAxisRotationArray[mAxisRotation, 2] != 0)
{
mImuData.gyro.X = tempIMU.gyro.Z * mAxisRotationArray[mAxisRotation, 2];
mImuData.accel.X = tempIMU.accel.Z * mAxisRotationArray[mAxisRotation, 2];
mImuData.mag.X = tempIMU.mag.Z * mAxisRotationArray[mAxisRotation, 2];
}
// do new y value
if (mAxisRotationArray[mAxisRotation, 3] != 0)
{
mImuData.gyro.Y = tempIMU.gyro.X * mAxisRotationArray[mAxisRotation, 3];
mImuData.accel.Y = tempIMU.accel.X * mAxisRotationArray[mAxisRotation, 3];
mImuData.mag.Y = tempIMU.mag.X * mAxisRotationArray[mAxisRotation, 3];
}
else if (mAxisRotationArray[mAxisRotation, 4] != 0)
{
mImuData.gyro.Y = tempIMU.gyro.Y * mAxisRotationArray[mAxisRotation, 4];
mImuData.accel.Y = tempIMU.accel.Y * mAxisRotationArray[mAxisRotation, 4];
mImuData.mag.Y = tempIMU.mag.Y * mAxisRotationArray[mAxisRotation, 4];
}
else if (mAxisRotationArray[mAxisRotation, 5] != 0)
{
mImuData.gyro.Y = tempIMU.gyro.Z * mAxisRotationArray[mAxisRotation, 5];
mImuData.accel.Y = tempIMU.accel.Z * mAxisRotationArray[mAxisRotation, 5];
mImuData.mag.Y = tempIMU.mag.Z * mAxisRotationArray[mAxisRotation, 5];
}
// do new z value
if (mAxisRotationArray[mAxisRotation, 6] != 0)
{
mImuData.gyro.Z = tempIMU.gyro.X * mAxisRotationArray[mAxisRotation, 6];
mImuData.accel.Z = tempIMU.accel.X * mAxisRotationArray[mAxisRotation, 6];
mImuData.mag.Z = tempIMU.mag.X * mAxisRotationArray[mAxisRotation, 6];
}
else if (mAxisRotationArray[mAxisRotation, 7] != 0)
{
mImuData.gyro.Z = tempIMU.gyro.Y * mAxisRotationArray[mAxisRotation, 7];
mImuData.accel.Z = tempIMU.accel.Y * mAxisRotationArray[mAxisRotation, 7];
mImuData.mag.Z = tempIMU.mag.Y * mAxisRotationArray[mAxisRotation, 7];
}
else if (mAxisRotationArray[mAxisRotation, 8] != 0)
{
mImuData.gyro.Z = tempIMU.gyro.Z * mAxisRotationArray[mAxisRotation, 8];
mImuData.accel.Z = tempIMU.accel.Z * mAxisRotationArray[mAxisRotation, 8];
mImuData.mag.Z = tempIMU.mag.Z * mAxisRotationArray[mAxisRotation, 8];
}
}
RTVector3 deltaAccel = mPreviousAccel;
deltaAccel -= mImuData.accel; // compute difference
mPreviousAccel = mImuData.accel;
if ((deltaAccel.length() < RTIMU_FUZZY_ACCEL_ZERO) && (mImuData.gyro.length() < RTIMU_FUZZY_GYRO_ZERO))
{
// what we are seeing on the gyros should be bias only so learn from this
if (mGyroSampleCount < (5 * mSampleRate))
{
mGyroBias.X = (1.0 - mGyroLearningAlpha) * mGyroBias.X + mGyroLearningAlpha * mImuData.gyro.X;
mGyroBias.Y = (1.0 - mGyroLearningAlpha) * mGyroBias.Y + mGyroLearningAlpha * mImuData.gyro.Y;
mGyroBias.Z = (1.0 - mGyroLearningAlpha) * mGyroBias.Z + mGyroLearningAlpha * mImuData.gyro.Z;
mGyroSampleCount++;
if (mGyroSampleCount == (5 * mSampleRate))
{
// this could have been true already of course
mGyroBiasValid = true;
}
}
else
{
mGyroBias.X = (1.0 - mGyroContinuousAlpha) * mGyroBias.X + mGyroContinuousAlpha * mImuData.gyro.X;
mGyroBias.Y = (1.0 - mGyroContinuousAlpha) * mGyroBias.Y + mGyroContinuousAlpha * mImuData.gyro.Y;
mGyroBias.Z = (1.0 - mGyroContinuousAlpha) * mGyroBias.Z + mGyroContinuousAlpha * mImuData.gyro.Z;
}
}
mImuData.gyro -= mGyroBias;
}
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 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;
}
