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