ReadRegisterSingle() private méthode

private ReadRegisterSingle ( byte register ) : byte
register byte
Résultat byte
Exemple #1
0
        //This works
        // Accelerometer and gyroscope self test; check calibration wrt factory settings
        // Should return percent deviation from factory trim values, +/- 14 or less deviation is a pass
        internal bool Mpu9150SelfTest(out double[] destination)
        {
            destination = new double[7];
            byte[]   rawData     = { 0, 0, 0, 0 };
            byte[]   selfTest    = { 0, 0, 0, 0, 0, 0 };
            double[] factoryTrim = { 0, 0, 0, 0, 0, 0 };

            // Configure the accelerometer for self-test
            _mpu9150.Write(ACCEL_CONFIG, 0xF0);
            // Enable self test on all three axes and set accelerometer range to +/- 8 g
            _mpu9150.Write(GYRO_CONFIG, 0xE0);
            // Enable self test on all three axes and set gyro range to +/- 250 degrees/s
            Task.Delay(250);                                       // Delay a while to let the device execute the self-test
            rawData[0] = _mpu9150.ReadRegisterSingle(SELF_TEST_X); // X-axis self-test results
            rawData[1] = _mpu9150.ReadRegisterSingle(SELF_TEST_Y); // Y-axis self-test results
            rawData[2] = _mpu9150.ReadRegisterSingle(SELF_TEST_Z); // Z-axis self-test results
            rawData[3] = _mpu9150.ReadRegisterSingle(SELF_TEST_A); // Mixed-axis self-test results

            // Extract the acceleration test results first
            selfTest[0] = (byte)((rawData[0] >> 3) | (rawData[3] & 0x30) >> 4);
            // XA_TEST result is a five-bit unsigned integer
            selfTest[1] = (byte)((rawData[1] >> 3) | (rawData[3] & 0x0C) >> 4);
            // YA_TEST result is a five-bit unsigned integer
            selfTest[2] = (byte)((rawData[2] >> 3) | (rawData[3] & 0x03) >> 4);
            // ZA_TEST result is a five-bit unsigned integer
            // Extract the gyration test results first
            selfTest[3] = (byte)(rawData[0] & 0x1F);  // XG_TEST result is a five-bit unsigned integer
            selfTest[4] = (byte)(rawData[1] & 0x1F);  // YG_TEST result is a five-bit unsigned integer
            selfTest[5] = (byte)(rawData[2] & 0x1F);  // ZG_TEST result is a five-bit unsigned integer
            // Process results to allow final comparison with factory set values
            factoryTrim[0] = (4096.0f * 0.34f) * (Math.Pow((0.92f / 0.34f), ((selfTest[0] - 1.0f) / 30.0f)));
            // FT[Xa] factory trim calculation
            factoryTrim[1] = (4096.0f * 0.34f) * (Math.Pow((0.92f / 0.34f), ((selfTest[1] - 1.0f) / 30.0f)));
            // FT[Ya] factory trim calculation
            factoryTrim[2] = (4096.0f * 0.34f) * (Math.Pow((0.92f / 0.34f), ((selfTest[2] - 1.0f) / 30.0f)));
            // FT[Za] factory trim calculation
            factoryTrim[3] = (25.0f * 131.0f) * (Math.Pow(1.046f, (selfTest[3] - 1.0f))); // FT[Xg] factory trim calculation
            factoryTrim[4] = (-25.0f * 131.0f) * (Math.Pow(1.046f, (selfTest[4] - 1.0f)));
            // FT[Yg] factory trim calculation
            factoryTrim[5] = (25.0f * 131.0f) * (Math.Pow(1.046f, (selfTest[5] - 1.0f))); // FT[Zg] factory trim calculation

            //  Output self-test results and factory trim calculation if desired
            //  Serial.println(selfTest[0]); Serial.println(selfTest[1]); Serial.println(selfTest[2]);
            //  Serial.println(selfTest[3]); Serial.println(selfTest[4]); Serial.println(selfTest[5]);
            //  Serial.println(factoryTrim[0]); Serial.println(factoryTrim[1]); Serial.println(factoryTrim[2]);
            //  Serial.println(factoryTrim[3]); Serial.println(factoryTrim[4]); Serial.println(factoryTrim[5]);

            // Report results as a ratio of (STR - FT)/FT; the change from Factory Trim of the Self-Test Response
            // To get to percent, must multiply by 100 and subtract result from 100
            for (var i = 0; i < 6; i++)
            {
                destination[i] = (100.0f + 100.0f * (selfTest[i] - factoryTrim[i]) / factoryTrim[i]);
                // Report percent differences
            }

            if (destination[0] < 1.0f && destination[1] < 1.0f && destination[2] < 1.0f && destination[3] < 1.0f && destination[4] < 1.0f && destination[5] < 1.0f)
            {
                Debug.WriteLine("MPU Self test passed.");
                return(true);
            }

            return(false);
        }
Exemple #2
0
        private async void Interrupt(GpioPin sender, GpioPinValueChangedEventArgs args)
        {
            await Task.Delay(10);

            if (_mpu9150 == null)
            {
                return;
            }

            int interruptStatus = _mpu9150.ReadRegisterSingle((byte)Mpu9150Setup.InterruptStatus);

            if ((interruptStatus & 0x10) != 0)
            {
                _mpu9150.Write((byte)Mpu9150Setup.UserCtrl, 0x44);// reset - enable fifo
            }
            if ((interruptStatus & 0x1) == 0)
            {
                return;
            }

            var ea = new MpuSensorEventArgs();

            ea.Status       = (byte)interruptStatus;
            ea.SamplePeriod = 0.02f;
            var l = new List <MpuSensorValue>();

            int count = _mpu9150.ReadUshort((byte)Mpu9150Setup.FifoCount);

            while (count >= SensorBytes)
            {
                _mpu9150.Write((byte)Mpu9150Setup.FifoReadWrite);

                byte[] buffer;

                _mpu9150.Read(SensorBytes, out buffer);
                count -= SensorBytes;

                var xa = (short)(buffer[0] << 8 | buffer[1]);
                var ya = (short)(buffer[2] << 8 | buffer[3]);
                var za = (short)(buffer[4] << 8 | buffer[5]);

                var xg = (short)(buffer[6] << 8 | buffer[7]);
                var yg = (short)(buffer[8] << 8 | buffer[9]);
                var zg = (short)(buffer[10] << 8 | buffer[11]);

                var sv = new MpuSensorValue
                {
                    AccelerationX = xa / 16384d,
                    AccelerationY = ya / 16384d,
                    AccelerationZ = za / 16384d,
                    GyroX         = xg / 131d,
                    GyroY         = yg / 131d,
                    GyroZ         = zg / 131d
                };
                l.Add(sv);

                var gain = 0.00875;

                var xRotationPerSecond = sv.GyroX * gain; //xRotationPerSecond is the rate of rotation per second.

                var loopPeriod = 0.015;                   //loop period - 0.02

                _gyroXangle += xRotationPerSecond * loopPeriod;

                var radToDeg = 57.29578;

                var accXangle = (Math.Atan2(sv.AccelerationY, sv.AccelerationZ) + Math.PI) * radToDeg;

                var complementaryFilterConstant = 0.98;

                _cFangleX = complementaryFilterConstant * (_cFangleX + xRotationPerSecond * loopPeriod) + (1 - complementaryFilterConstant) * accXangle;

                //Debug.WriteLine("X: " + sv.GyroX + ", Y: " + sv.GyroY + ", Z: " + sv.GyroZ);
                //Debug.WriteLine("CFangleX: " + _cFangleX);
                //Debug.WriteLine("AccelX: " + sv.AccelerationX + ", AccelY: " + sv.AccelerationY + ", AccelZ: " + sv.AccelerationZ);
            }
            ea.Values = l.ToArray();

            if (SensorInterruptEvent == null)
            {
                return;
            }

            if (ea.Values.Length > 0)
            {
                SensorInterruptEvent(this, ea);
            }
        }