public MPU6050Device()
        {
            int exceptionCount = 0;

            while (true)
            {
                try
                {
                    Log.WriteLine("Initialising the MPU-6050 accelerometer and gyro package...");

                    // Check connectivity
                    byte[] data = new byte[] { new byte() };
                    _I2CBus.ReadRegister(_i2cConfig, MPU6050Registers.WHO_AM_I, data, _timeout);
                    Debug.Assert((data[0] & 0x68) == 0x68);

                    // PWR_MGMT_1 = Power Management 1
                    // 0xF9 = 11111001: Device Reset=true, Sleep=true, Cycle=true, Temp Sensor=On, Clock Select=PLL with X axis gyroscope reference
                    // TODO: Given the reset, half of these values are probably ignored, and the defaults are used instead.
                    _I2CBus.WriteRegister(_i2cConfig, MPU6050Registers.PWR_MGMT_1, 0xF9, _timeout);
                    Thread.Sleep(100); // allow to reset

                    // Exit sleep mode (CARE! Not sure why, but this resets the device too! - only set config after this line)
                    // TODO: Figure out what's going on here. Perhaps the reset at the line above was taking some time, hence why the Scale settings (below) were ignored
                    _I2CBus.WriteRegister(_i2cConfig, MPU6050Registers.PWR_MGMT_1, 0x01, _timeout);
                    Thread.Sleep(100); // allow to reset

                    // Gyro Scale
                    _gyroRange = GyroConfig.Range.plusMinus0500dps;
                    _I2CBus.WriteRegister(_i2cConfig, MPU6050Registers.GYRO_CONFIG, GyroConfig.Build(_gyroRange), _timeout);

                    // Accelerometer Scale
                    _accelRange = AccelConfig.Range.plusMinus08G;
                    _I2CBus.WriteRegister(_i2cConfig, MPU6050Registers.ACCEL_CONFIG, AccelConfig.Build(_accelRange), _timeout);

                    // Confirm I2C_MST_EN is disabled (required to connect to the HMC5883L)
                    data = new byte[] { new byte() };
                    _I2CBus.ReadRegister(_i2cConfig, 0x6A, data, 1000);
                    Debug.Assert((data[0] & 0x10) == 0);

                    // Turn on I2C Bypass (required to connect to the HMC5883L)
                    _I2CBus.WriteRegister(_i2cConfig, 0x37, 0x02, 1000);
                    data = new byte[] { new byte() };
                    _I2CBus.ReadRegister(_i2cConfig, 0x37, data, 1000);
                    Debug.Assert((data[0] & 0x02) == 0x02);

                    Log.WriteLine("MPU-6050 Ready\n");

                    break;
                }
                catch (System.Exception)
                {
                    exceptionCount++;
                    if (exceptionCount != 3)
                    {
                        Log.WriteLine("Exception during initialisation! Retrying...\n");
                        //I2CBus.DestroySingleton();
                        //_I2CBus = I2CBus.GetInstance();
                        continue;
                    }

                    Log.WriteLine("Exception during initialisation! Throwing...");
                    throw;
                }
            }
        }
Esempio n. 2
0
        public static AccelerationAndGyroData Build(byte[] results, GyroConfig.Range gyroRange, AccelConfig.Range accelRange)
        {
            // Result for the acceleration sensor, merged together by bit shifting
            var RawAccelerationX = ToShort(results[0], results[1]);
            var RawAccelerationY = ToShort(results[2], results[3]);
            var RawAccelerationZ = ToShort(results[4], results[5]);

            // Results for temperature (not tested)
            var RawTemperature = ToShort(results[6], results[7]);

            // Result for the gyro sensor, merged together by bit shifting
            var RawGyroX = ToShort(results[8], results[9]);
            var RawGyroY = ToShort(results[10], results[11]);
            var RawGyroZ = ToShort(results[12], results[13]);

            // Now convert to sensible numbers
            var Temp = ParseTemp(RawTemperature);
            var Ax   = ConvertRawValueToUnits(RawAccelerationX, (short)accelRange);
            var Ay   = ConvertRawValueToUnits(RawAccelerationY, (short)accelRange);
            var Az   = ConvertRawValueToUnits(RawAccelerationZ, (short)accelRange);
            var Rx   = ConvertRawValueToUnits(RawGyroX, (short)gyroRange);
            var Ry   = ConvertRawValueToUnits(RawGyroY, (short)gyroRange);
            var Rz   = ConvertRawValueToUnits(RawGyroZ, (short)gyroRange);

            return(new AccelerationAndGyroData(Ax, Ay, Az, Temp, Rx, Ry, Rz));
        }