예제 #1
0
 bool SetMagCTRL3()
 {
     if (!RTI2C.Write(mMag, LSM9DS1_MAG_CTRL3, 0x00))
     {
         ErrorMessage = "Failed to set LSM9DS1 compass CTRL3";
         return(false);
     }
     return(true);
 }
예제 #2
0
        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;
        }
예제 #3
0
        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);
        }
예제 #4
0
        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);
        }
예제 #5
0
        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);
        }
예제 #6
0
        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);
        }
예제 #7
0
        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);
        }
예제 #8
0
        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);
        }
예제 #9
0
        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);
        }
예제 #10
0
        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;
        }
예제 #11
0
        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);
        }
예제 #12
0
        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);
        }
예제 #13
0
        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;
        }