diff --git a/src/Iot.Device.Bindings/CompatibilitySuppressions.xml b/src/Iot.Device.Bindings/CompatibilitySuppressions.xml index 5c5a001be2169b99b8adab8414bd015253e09b64..66a254f5d0d467220b9214cc85cda49497d8fcb3 100644 --- a/src/Iot.Device.Bindings/CompatibilitySuppressions.xml +++ b/src/Iot.Device.Bindings/CompatibilitySuppressions.xml @@ -1,6 +1,5 @@  - CP0001 T:IoT.Device.Tsl256x.Channel @@ -169,4 +168,46 @@ lib/netstandard2.0/Iot.Device.Bindings.dll true + + CP0002 + F:Iot.Device.Imu.Mpu6500.DefaultI2cAddress + lib/net6.0/Iot.Device.Bindings.dll + lib/net6.0/Iot.Device.Bindings.dll + true + + + CP0002 + F:Iot.Device.Imu.Mpu6500.SecondI2cAddress + lib/net6.0/Iot.Device.Bindings.dll + lib/net6.0/Iot.Device.Bindings.dll + true + + + CP0002 + F:Iot.Device.Imu.Mpu6500.DefaultI2cAddress + lib/netcoreapp3.1/Iot.Device.Bindings.dll + lib/netcoreapp3.1/Iot.Device.Bindings.dll + true + + + CP0002 + F:Iot.Device.Imu.Mpu6500.SecondI2cAddress + lib/netcoreapp3.1/Iot.Device.Bindings.dll + lib/netcoreapp3.1/Iot.Device.Bindings.dll + true + + + CP0002 + F:Iot.Device.Imu.Mpu6500.DefaultI2cAddress + lib/netstandard2.0/Iot.Device.Bindings.dll + lib/netstandard2.0/Iot.Device.Bindings.dll + true + + + CP0002 + F:Iot.Device.Imu.Mpu6500.SecondI2cAddress + lib/netstandard2.0/Iot.Device.Bindings.dll + lib/netstandard2.0/Iot.Device.Bindings.dll + true + \ No newline at end of file diff --git a/src/devices/Mpu9250/AccelerometerBandwidth.cs b/src/devices/Mpu6xxx9xxx/AccelerometerBandwidth.cs similarity index 100% rename from src/devices/Mpu9250/AccelerometerBandwidth.cs rename to src/devices/Mpu6xxx9xxx/AccelerometerBandwidth.cs diff --git a/src/devices/Mpu9250/AccelerometerLowPowerFrequency.cs b/src/devices/Mpu6xxx9xxx/AccelerometerLowPowerFrequency.cs similarity index 100% rename from src/devices/Mpu9250/AccelerometerLowPowerFrequency.cs rename to src/devices/Mpu6xxx9xxx/AccelerometerLowPowerFrequency.cs diff --git a/src/devices/Mpu9250/AccelerometerRange.cs b/src/devices/Mpu6xxx9xxx/AccelerometerRange.cs similarity index 100% rename from src/devices/Mpu9250/AccelerometerRange.cs rename to src/devices/Mpu6xxx9xxx/AccelerometerRange.cs diff --git a/src/devices/Mpu9250/Ak8963Attached.cs b/src/devices/Mpu6xxx9xxx/Ak8963Attached.cs similarity index 100% rename from src/devices/Mpu9250/Ak8963Attached.cs rename to src/devices/Mpu6xxx9xxx/Ak8963Attached.cs diff --git a/src/devices/Mpu9250/ClockSource.cs b/src/devices/Mpu6xxx9xxx/ClockSource.cs similarity index 100% rename from src/devices/Mpu9250/ClockSource.cs rename to src/devices/Mpu6xxx9xxx/ClockSource.cs diff --git a/src/devices/Mpu9250/DisableModes.cs b/src/devices/Mpu6xxx9xxx/DisableModes.cs similarity index 100% rename from src/devices/Mpu9250/DisableModes.cs rename to src/devices/Mpu6xxx9xxx/DisableModes.cs diff --git a/src/devices/Mpu9250/FifoModes.cs b/src/devices/Mpu6xxx9xxx/FifoModes.cs similarity index 100% rename from src/devices/Mpu9250/FifoModes.cs rename to src/devices/Mpu6xxx9xxx/FifoModes.cs diff --git a/src/devices/Mpu6xxx9xxx/FritzingScheme/Mpu6050.fzz b/src/devices/Mpu6xxx9xxx/FritzingScheme/Mpu6050.fzz new file mode 100644 index 0000000000000000000000000000000000000000..cb0096063382864f954fa5fb7a497833860514ce Binary files /dev/null and b/src/devices/Mpu6xxx9xxx/FritzingScheme/Mpu6050.fzz differ diff --git a/src/devices/Mpu6xxx9xxx/FritzingScheme/Mpu9250.fzz b/src/devices/Mpu6xxx9xxx/FritzingScheme/Mpu9250.fzz new file mode 100644 index 0000000000000000000000000000000000000000..81d04b188b57e1157eea505e8a27ed5a3618cac3 Binary files /dev/null and b/src/devices/Mpu6xxx9xxx/FritzingScheme/Mpu9250.fzz differ diff --git a/src/devices/Mpu9250/GyroscopeBandwidth.cs b/src/devices/Mpu6xxx9xxx/GyroscopeBandwidth.cs similarity index 100% rename from src/devices/Mpu9250/GyroscopeBandwidth.cs rename to src/devices/Mpu6xxx9xxx/GyroscopeBandwidth.cs diff --git a/src/devices/Mpu9250/GyroscopeRange.cs b/src/devices/Mpu6xxx9xxx/GyroscopeRange.cs similarity index 100% rename from src/devices/Mpu9250/GyroscopeRange.cs rename to src/devices/Mpu6xxx9xxx/GyroscopeRange.cs diff --git a/src/devices/Mpu9250/I2cBusFrequency.cs b/src/devices/Mpu6xxx9xxx/I2cBusFrequency.cs similarity index 100% rename from src/devices/Mpu9250/I2cBusFrequency.cs rename to src/devices/Mpu6xxx9xxx/I2cBusFrequency.cs diff --git a/src/devices/Mpu9250/I2cChannel.cs b/src/devices/Mpu6xxx9xxx/I2cChannel.cs similarity index 100% rename from src/devices/Mpu9250/I2cChannel.cs rename to src/devices/Mpu6xxx9xxx/I2cChannel.cs diff --git a/src/devices/Mpu9250/Mpu6500.cs b/src/devices/Mpu6xxx9xxx/Mpu6050.cs similarity index 77% rename from src/devices/Mpu9250/Mpu6500.cs rename to src/devices/Mpu6xxx9xxx/Mpu6050.cs index d68a0b1ceb28b14f6f8ec3ff9bb48124e4d6ea9a..397371a1eed8cdd3cd3f507d347564cbb1bc0574 100644 --- a/src/devices/Mpu9250/Mpu6500.cs +++ b/src/devices/Mpu6xxx9xxx/Mpu6050.cs @@ -17,10 +17,10 @@ using UnitsNet; namespace Iot.Device.Imu { /// - /// MPU6500 - gyroscope, accelerometer and temperature sensor + /// MPU6050 - gyroscope, accelerometer and temperature sensor /// - [Interface("MPU6500 - gyroscope, accelerometer and temperature sensor")] - public class Mpu6500 : IDisposable + [Interface("MPU6050 - gyroscope, accelerometer and temperature sensor")] + public class Mpu6050 : IDisposable { /// /// Default address for MPU9250 @@ -44,17 +44,22 @@ namespace Iot.Device.Imu internal bool _wakeOnMotion; /// - /// Initialize the MPU6500 + /// Initialize the MPU6050 /// /// The I2C device - public Mpu6500(I2cDevice i2cDevice) + public Mpu6050(I2cDevice i2cDevice) { + if (i2cDevice == null) + { + throw new ArgumentNullException($"Varible i2cDevice is null"); + } + _i2cDevice = i2cDevice; Reset(); PowerOn(); if (!CheckVersion()) { - throw new IOException($"This device does not contain the correct signature 0x70 for a MPU6500"); + throw new IOException($"This device does not contain the correct signature 0x68 for a MPU6050"); } GyroscopeBandwidth = GyroscopeBandwidth.Bandwidth0250Hz; @@ -64,9 +69,9 @@ namespace Iot.Device.Imu } /// - /// Used to create the class for the MPU9250. Initialization is a bit different than for the MPU6500 + /// Used to create the class for the MPU9250. Initialization is a bit different than for the MPU6050 /// - internal Mpu6500(I2cDevice i2cDevice, bool isInternal) + internal Mpu6050(I2cDevice i2cDevice, bool isInternal) { _i2cDevice = i2cDevice ?? throw new ArgumentNullException(nameof(i2cDevice)); } @@ -157,11 +162,20 @@ namespace Iot.Device.Imu [Telemetry("Acceleration")] public Vector3 GetAccelerometer() => GetRawAccelerometer() * AccelerationScale; - private Vector3 GetRawAccelerometer() + /// + /// Gets the raw accelerometer data + /// + /// + internal Vector3 GetRawAccelerometer() { Span rawData = stackalloc byte[6] { - 0, 0, 0, 0, 0, 0 + 0, + 0, + 0, + 0, + 0, + 0 }; Vector3 ace = new Vector3(); ReadBytes(Register.ACCEL_XOUT_H, rawData); @@ -300,11 +314,20 @@ namespace Iot.Device.Imu [Telemetry("AngularRate")] public Vector3 GetGyroscopeReading() => GetRawGyroscope() * GyroscopeScale; - private Vector3 GetRawGyroscope() + /// + /// Gets the raw gyroscope data + /// + /// + internal Vector3 GetRawGyroscope() { Span rawData = stackalloc byte[6] { - 0, 0, 0, 0, 0, 0 + 0, + 0, + 0, + 0, + 0, + 0 }; Vector3 gyro = new Vector3(); ReadBytes(Register.GYRO_XOUT_H, rawData); @@ -326,7 +349,8 @@ namespace Iot.Device.Imu { Span rawData = stackalloc byte[2] { - 0, 0 + 0, + 0 }; ReadBytes(Register.TEMP_OUT_H, rawData); // formula from the documentation @@ -405,11 +429,11 @@ namespace Iot.Device.Imu } /// - /// Return true if the version of MPU6500 is the correct one + /// Return true if the version of MPU6050 is the correct one /// /// True if success // Check if the version is thee correct one - internal bool CheckVersion() => ReadByte(Register.WHO_AM_I) == 0x70; + internal bool CheckVersion() => ReadByte(Register.WHO_AM_I) == 0x68; /// /// Get or set the sample diver mode @@ -444,7 +468,8 @@ namespace Iot.Device.Imu { Span rawData = stackalloc byte[2] { - 0, 0 + 0, + 0 }; ReadBytes(Register.FIFO_COUNTH, rawData); return BinaryPrimitives.ReadUInt16BigEndian(rawData); @@ -669,177 +694,6 @@ namespace Iot.Device.Imu return (_gyroscopeBias, _accelerometerBias); } - /// - /// 0.5 - /// Y-gyro | (GYST / GYST_OTP) > 0.5 - /// Z-gyro | (GZST / GZST_OTP) > 0.5 - /// X-Accel | 0.5 < (AXST / AXST_OTP) < 1.5 - /// Y-Accel | 0.5 < (AYST / AYST_OTP) < 1.5 - /// Z-Accel | 0.5 < (AZST / AZST_OTP) < 1.5 - /// b. If factory Self-Test values ST_OTP=0, compare the current Self-Test response (GXST, GYST, GZST, AXST, AYST and AZST) - /// to the ST absolute limits (ST_AL) and report Self-Test is passing if all the following criteria are fulfilled. - /// Axis | Pass criteria - /// X-gyro | |GXST| ≥ 60dps - /// Y-gyro | |GYST| ≥ 60dps - /// Z-gyro | |GZST| ≥ 60dps - /// X-Accel| 225mgee ≤ |AXST| ≤ 675mgee - /// Y-Accel| 225mgee ≤ |AXST| ≤ 675mgee - /// Z-Accel| 225mgee ≤ |AXST| ≤ 675mgee - /// c. If the Self-Test passes criteria (a) and (b), it’s necessary to check gyro offset values. - /// Report passing Self-Test if the following criteria fulfilled. - /// Axis | Pass criteria - /// X-gyro | |GXOFFSET| ≤ 20dps - /// Y-gyro | |GYOFFSET| ≤ 20dps - /// Z-gyro | |GZOFFSET| ≤ 20dps - /// ]]> - /// - /// the gyroscope and accelerometer vectors - [Command] - public (Vector3 GyroscopeAverage, Vector3 AccelerometerAverage) RunGyroscopeAccelerometerSelfTest() - { - // Used for the number of cycles to run the test - // Value is 200 according to documentation AN-MPU-9250A-03 - const int numCycles = 200; - - Vector3 accAverage = new Vector3(); - Vector3 gyroAvegage = new Vector3(); - Vector3 accSelfTestAverage = new Vector3(); - Vector3 gyroSelfTestAverage = new Vector3(); - Vector3 gyroSelfTest = new Vector3(); - Vector3 accSelfTest = new Vector3(); - Vector3 gyroFactoryTrim = new Vector3(); - Vector3 accFactoryTrim = new Vector3(); - // Tests done with slower GyroScale and Accelerator so 2G so value is 0 in both cases - byte gyroAccScale = 0; - - // Setup the registers for Gyroscope as in documentation - // DLPF Config | LPF BW | Sampling Rate | Filter Delay - // 2 | 92Hz | 1kHz | 3.9ms - WriteRegister(Register.SMPLRT_DIV, 0x00); - WriteRegister(Register.CONFIG, 0x02); - GyroscopeRange = GyroscopeRange.Range0250Dps; - // Set full scale range for the gyro to 250 dps - // Setup the registers for accelerometer as in documentation - // DLPF Config | LPF BW | Sampling Rate | Filter Delay - // 2 | 92Hz | 1kHz | 7.8ms - WriteRegister(Register.ACCEL_CONFIG_2, 0x02); - AccelerometerRange = AccelerometerRange.Range02G; - - // Read the data 200 times as per the documentation page 5 - for (int reading = 0; reading < numCycles; reading++) - { - gyroAvegage = GetRawGyroscope(); - accAverage = GetRawAccelerometer(); - } - - accAverage /= numCycles; - gyroAvegage /= numCycles; - - // Set USR_Reg: (1Bh) Gyro_Config, gdrive_axisCTST [0-2] to b111 to enable Self-Test. - WriteRegister(Register.ACCEL_CONFIG, 0xE0); - // Set USR_Reg: (1Ch) Accel_Config, AX/Y/Z_ST_EN [0-2] to b111 to enable Self-Test. - WriteRegister(Register.GYRO_CONFIG, 0xE0); - // Wait 20ms for oscillations to stabilize - Thread.Sleep(20); - - // Read the gyroscope and accelerometer output at a 1kHz rate and average 200 readings. - // The averaged values will be the LSB of GX_ST_OS, GY_ST_OS, GZ_ST_OS, AX_ST_OS, AY_ST_OS and AZ_ST_OS in the software - for (int reading = 0; reading < numCycles; reading++) - { - gyroSelfTestAverage = GetRawGyroscope(); - accSelfTestAverage = GetRawAccelerometer(); - } - - accSelfTestAverage /= numCycles; - gyroSelfTestAverage /= numCycles; - - // To cleanup the configuration after the test - // Set USR_Reg: (1Bh) Gyro_Config, gdrive_axisCTST [0-2] to b000. - WriteRegister(Register.ACCEL_CONFIG, 0x00); - // Set USR_Reg: (1Ch) Accel_Config, AX/Y/Z_ST_EN [0-2] to b000. - WriteRegister(Register.GYRO_CONFIG, 0x00); - // Wait 20ms for oscillations to stabilize - Thread.Sleep(20); - - // Retrieve factory Self-Test code (ST_Code) from USR_Reg in the software - gyroSelfTest.X = ReadByte(Register.SELF_TEST_X_GYRO); - gyroSelfTest.Y = ReadByte(Register.SELF_TEST_Y_GYRO); - gyroSelfTest.Z = ReadByte(Register.SELF_TEST_Z_GYRO); - accSelfTest.X = ReadByte(Register.SELF_TEST_X_ACCEL); - accSelfTest.Y = ReadByte(Register.SELF_TEST_Y_ACCEL); - accSelfTest.Z = ReadByte(Register.SELF_TEST_Z_ACCEL); - - // Calculate all factory trim - accFactoryTrim.X = (float)((2620 / 1 << gyroAccScale) * (Math.Pow(1.01, accSelfTest.X - 1.0))); - accFactoryTrim.Y = (float)((2620 / 1 << gyroAccScale) * (Math.Pow(1.01, accSelfTest.Y - 1.0))); - accFactoryTrim.Z = (float)((2620 / 1 << gyroAccScale) * (Math.Pow(1.01, accSelfTest.Z - 1.0))); - gyroFactoryTrim.X = (float)((2620 / 1 << gyroAccScale) * (Math.Pow(1.01, gyroSelfTest.X - 1.0))); - gyroFactoryTrim.Y = (float)((2620 / 1 << gyroAccScale) * (Math.Pow(1.01, gyroSelfTest.Y - 1.0))); - gyroFactoryTrim.Z = (float)((2620 / 1 << gyroAccScale) * (Math.Pow(1.01, gyroSelfTest.Z - 1.0))); - - if (gyroFactoryTrim.X != 0) - { - gyroAvegage.X = (gyroSelfTestAverage.X - gyroAvegage.X) / gyroFactoryTrim.X; - } - else - { - gyroAvegage.X = Math.Abs(gyroSelfTestAverage.X - gyroAvegage.X); - } - - if (gyroFactoryTrim.Y != 0) - { - gyroAvegage.Y = (gyroSelfTestAverage.Y - gyroAvegage.Y) / gyroFactoryTrim.Y; - } - else - { - gyroAvegage.Y = Math.Abs(gyroSelfTestAverage.Y - gyroAvegage.Y); - } - - if (gyroFactoryTrim.Z != 0) - { - gyroAvegage.Z = (gyroSelfTestAverage.Z - gyroAvegage.Z) / gyroFactoryTrim.Z; - } - else - { - gyroAvegage.Z = Math.Abs(gyroSelfTestAverage.Z - gyroAvegage.Z); - } - - // Accelerator - if (accFactoryTrim.X != 0) - { - accAverage.X = (accSelfTestAverage.X - accAverage.X) / accFactoryTrim.X; - } - else - { - accAverage.X = Math.Abs(accSelfTestAverage.X - accSelfTestAverage.X); - } - - if (accFactoryTrim.Y != 0) - { - accAverage.Y = (accSelfTestAverage.Y - accAverage.Y) / accFactoryTrim.Y; - } - else - { - accAverage.Y = Math.Abs(accSelfTestAverage.Y - accSelfTestAverage.Y); - } - - if (accFactoryTrim.Z != 0) - { - accAverage.Z = (accSelfTestAverage.Z - accAverage.Z) / accFactoryTrim.Z; - } - else - { - accAverage.Z = Math.Abs(accSelfTestAverage.Z - accSelfTestAverage.Z); - } - - return (gyroAvegage, accAverage); - } - #endregion #region I2C @@ -857,7 +711,8 @@ namespace Iot.Device.Imu byte slvAddress = (byte)((byte)Register.I2C_SLV0_ADDR + 3 * (byte)i2cChannel); Span dataout = stackalloc byte[2] { - slvAddress, address + slvAddress, + address }; _i2cDevice.Write(dataout); // I2C_SLVx_REG = I2C_SLVx_ADDR + 1 @@ -893,7 +748,8 @@ namespace Iot.Device.Imu byte slvAddress = (byte)((byte)Register.I2C_SLV0_ADDR + 3 * (byte)i2cChannel); Span dataout = stackalloc byte[2] { - slvAddress, (byte)(address | 0x80) + slvAddress, + (byte)(address | 0x80) }; _i2cDevice.Write(dataout); dataout[0] = (byte)(slvAddress + 1); @@ -913,7 +769,8 @@ namespace Iot.Device.Imu { Span dataout = stackalloc byte[] { - (byte)reg, data + (byte)reg, + data }; _i2cDevice.Write(dataout); } diff --git a/src/devices/Mpu6xxx9xxx/Mpu6500.cs b/src/devices/Mpu6xxx9xxx/Mpu6500.cs new file mode 100644 index 0000000000000000000000000000000000000000..57a03576e41eecdcee40401afc96dbbdd43ffce0 --- /dev/null +++ b/src/devices/Mpu6xxx9xxx/Mpu6500.cs @@ -0,0 +1,236 @@ +// Licensed to the .NET Foundation under one or more agreements. +// The .NET Foundation licenses this file to you under the MIT license. + +using System; +using System.Buffers.Binary; +using System.Collections.Generic; +using System.Device; +using System.Device.I2c; +using System.Device.Model; +using System.IO; +using System.Numerics; +using System.Text; +using System.Threading; +using Iot.Device.Magnetometer; +using UnitsNet; + +namespace Iot.Device.Imu +{ + /// + /// MPU6500 - gyroscope, accelerometer and temperature sensor + /// + [Interface("MPU6500 - gyroscope, accelerometer and temperature sensor")] + public class Mpu6500 : Mpu6050 + { + /// + /// Initialize the MPU6500 + /// + /// The I2C device + public Mpu6500(I2cDevice i2cDevice) + : base(i2cDevice, true) + { + Reset(); + PowerOn(); + if (!CheckVersion()) + { + throw new IOException($"This device does not contain the correct signature 0x70 for a MPU6500"); + } + + GyroscopeBandwidth = GyroscopeBandwidth.Bandwidth0250Hz; + GyroscopeRange = GyroscopeRange.Range0250Dps; + AccelerometerBandwidth = AccelerometerBandwidth.Bandwidth1130Hz; + AccelerometerRange = AccelerometerRange.Range02G; + } + + /// + /// Used to create the class for the MPU9250. Initialization is a bit different than for the MPU6500 + /// + internal Mpu6500(I2cDevice i2cDevice, bool isInternal) + : base(i2cDevice, isInternal) + { + } + + #region Modes, constructor, Dispose + + /// + /// Return true if the version of MPU6500 is the correct one + /// + /// True if success + // Check if the version is thee correct one + internal new bool CheckVersion() => ReadByte(Register.WHO_AM_I) == 0x70; + + /// + /// 0.5 + /// Y-gyro | (GYST / GYST_OTP) > 0.5 + /// Z-gyro | (GZST / GZST_OTP) > 0.5 + /// X-Accel | 0.5 < (AXST / AXST_OTP) < 1.5 + /// Y-Accel | 0.5 < (AYST / AYST_OTP) < 1.5 + /// Z-Accel | 0.5 < (AZST / AZST_OTP) < 1.5 + /// b. If factory Self-Test values ST_OTP=0, compare the current Self-Test response (GXST, GYST, GZST, AXST, AYST and AZST) + /// to the ST absolute limits (ST_AL) and report Self-Test is passing if all the following criteria are fulfilled. + /// Axis | Pass criteria + /// X-gyro | |GXST| ≥ 60dps + /// Y-gyro | |GYST| ≥ 60dps + /// Z-gyro | |GZST| ≥ 60dps + /// X-Accel| 225mgee ≤ |AXST| ≤ 675mgee + /// Y-Accel| 225mgee ≤ |AXST| ≤ 675mgee + /// Z-Accel| 225mgee ≤ |AXST| ≤ 675mgee + /// c. If the Self-Test passes criteria (a) and (b), it’s necessary to check gyro offset values. + /// Report passing Self-Test if the following criteria fulfilled. + /// Axis | Pass criteria + /// X-gyro | |GXOFFSET| ≤ 20dps + /// Y-gyro | |GYOFFSET| ≤ 20dps + /// Z-gyro | |GZOFFSET| ≤ 20dps + /// ]]> + /// + /// the gyroscope and accelerometer vectors + [Command] + public (Vector3 GyroscopeAverage, Vector3 AccelerometerAverage) RunGyroscopeAccelerometerSelfTest() + { + // Used for the number of cycles to run the test + // Value is 200 according to documentation AN-MPU-9250A-03 + const int numCycles = 200; + + Vector3 accAverage = new Vector3(); + Vector3 gyroAvegage = new Vector3(); + Vector3 accSelfTestAverage = new Vector3(); + Vector3 gyroSelfTestAverage = new Vector3(); + Vector3 gyroSelfTest = new Vector3(); + Vector3 accSelfTest = new Vector3(); + Vector3 gyroFactoryTrim = new Vector3(); + Vector3 accFactoryTrim = new Vector3(); + // Tests done with slower GyroScale and Accelerator so 2G so value is 0 in both cases + byte gyroAccScale = 0; + + // Setup the registers for Gyroscope as in documentation + // DLPF Config | LPF BW | Sampling Rate | Filter Delay + // 2 | 92Hz | 1kHz | 3.9ms + WriteRegister(Register.SMPLRT_DIV, 0x00); + WriteRegister(Register.CONFIG, 0x02); + GyroscopeRange = GyroscopeRange.Range0250Dps; + // Set full scale range for the gyro to 250 dps + // Setup the registers for accelerometer as in documentation + // DLPF Config | LPF BW | Sampling Rate | Filter Delay + // 2 | 92Hz | 1kHz | 7.8ms + WriteRegister(Register.ACCEL_CONFIG_2, 0x02); + AccelerometerRange = AccelerometerRange.Range02G; + + // Read the data 200 times as per the documentation page 5 + for (int reading = 0; reading < numCycles; reading++) + { + gyroAvegage = GetRawGyroscope(); + accAverage = GetRawAccelerometer(); + } + + accAverage /= numCycles; + gyroAvegage /= numCycles; + + // Set USR_Reg: (1Bh) Gyro_Config, gdrive_axisCTST [0-2] to b111 to enable Self-Test. + WriteRegister(Register.ACCEL_CONFIG, 0xE0); + // Set USR_Reg: (1Ch) Accel_Config, AX/Y/Z_ST_EN [0-2] to b111 to enable Self-Test. + WriteRegister(Register.GYRO_CONFIG, 0xE0); + // Wait 20ms for oscillations to stabilize + Thread.Sleep(20); + + // Read the gyroscope and accelerometer output at a 1kHz rate and average 200 readings. + // The averaged values will be the LSB of GX_ST_OS, GY_ST_OS, GZ_ST_OS, AX_ST_OS, AY_ST_OS and AZ_ST_OS in the software + for (int reading = 0; reading < numCycles; reading++) + { + gyroSelfTestAverage = GetRawGyroscope(); + accSelfTestAverage = GetRawAccelerometer(); + } + + accSelfTestAverage /= numCycles; + gyroSelfTestAverage /= numCycles; + + // To cleanup the configuration after the test + // Set USR_Reg: (1Bh) Gyro_Config, gdrive_axisCTST [0-2] to b000. + WriteRegister(Register.ACCEL_CONFIG, 0x00); + // Set USR_Reg: (1Ch) Accel_Config, AX/Y/Z_ST_EN [0-2] to b000. + WriteRegister(Register.GYRO_CONFIG, 0x00); + // Wait 20ms for oscillations to stabilize + Thread.Sleep(20); + + // Retrieve factory Self-Test code (ST_Code) from USR_Reg in the software + gyroSelfTest.X = ReadByte(Register.SELF_TEST_X_GYRO); + gyroSelfTest.Y = ReadByte(Register.SELF_TEST_Y_GYRO); + gyroSelfTest.Z = ReadByte(Register.SELF_TEST_Z_GYRO); + accSelfTest.X = ReadByte(Register.SELF_TEST_X_ACCEL); + accSelfTest.Y = ReadByte(Register.SELF_TEST_Y_ACCEL); + accSelfTest.Z = ReadByte(Register.SELF_TEST_Z_ACCEL); + + // Calculate all factory trim + accFactoryTrim.X = (float)((2620 / 1 << gyroAccScale) * (Math.Pow(1.01, accSelfTest.X - 1.0))); + accFactoryTrim.Y = (float)((2620 / 1 << gyroAccScale) * (Math.Pow(1.01, accSelfTest.Y - 1.0))); + accFactoryTrim.Z = (float)((2620 / 1 << gyroAccScale) * (Math.Pow(1.01, accSelfTest.Z - 1.0))); + gyroFactoryTrim.X = (float)((2620 / 1 << gyroAccScale) * (Math.Pow(1.01, gyroSelfTest.X - 1.0))); + gyroFactoryTrim.Y = (float)((2620 / 1 << gyroAccScale) * (Math.Pow(1.01, gyroSelfTest.Y - 1.0))); + gyroFactoryTrim.Z = (float)((2620 / 1 << gyroAccScale) * (Math.Pow(1.01, gyroSelfTest.Z - 1.0))); + + if (gyroFactoryTrim.X != 0) + { + gyroAvegage.X = (gyroSelfTestAverage.X - gyroAvegage.X) / gyroFactoryTrim.X; + } + else + { + gyroAvegage.X = Math.Abs(gyroSelfTestAverage.X - gyroAvegage.X); + } + + if (gyroFactoryTrim.Y != 0) + { + gyroAvegage.Y = (gyroSelfTestAverage.Y - gyroAvegage.Y) / gyroFactoryTrim.Y; + } + else + { + gyroAvegage.Y = Math.Abs(gyroSelfTestAverage.Y - gyroAvegage.Y); + } + + if (gyroFactoryTrim.Z != 0) + { + gyroAvegage.Z = (gyroSelfTestAverage.Z - gyroAvegage.Z) / gyroFactoryTrim.Z; + } + else + { + gyroAvegage.Z = Math.Abs(gyroSelfTestAverage.Z - gyroAvegage.Z); + } + + // Accelerator + if (accFactoryTrim.X != 0) + { + accAverage.X = (accSelfTestAverage.X - accAverage.X) / accFactoryTrim.X; + } + else + { + accAverage.X = Math.Abs(accSelfTestAverage.X - accSelfTestAverage.X); + } + + if (accFactoryTrim.Y != 0) + { + accAverage.Y = (accSelfTestAverage.Y - accAverage.Y) / accFactoryTrim.Y; + } + else + { + accAverage.Y = Math.Abs(accSelfTestAverage.Y - accSelfTestAverage.Y); + } + + if (accFactoryTrim.Z != 0) + { + accAverage.Z = (accSelfTestAverage.Z - accAverage.Z) / accFactoryTrim.Z; + } + else + { + accAverage.Z = Math.Abs(accSelfTestAverage.Z - accSelfTestAverage.Z); + } + + return (gyroAvegage, accAverage); + } + + #endregion + + } +} diff --git a/src/devices/Mpu9250/Mpu9250.csproj b/src/devices/Mpu6xxx9xxx/Mpu6xxx9xxx.csproj similarity index 100% rename from src/devices/Mpu9250/Mpu9250.csproj rename to src/devices/Mpu6xxx9xxx/Mpu6xxx9xxx.csproj diff --git a/src/devices/Mpu9250/Mpu9250.sln b/src/devices/Mpu6xxx9xxx/Mpu6xxx9xxx.sln similarity index 62% rename from src/devices/Mpu9250/Mpu9250.sln rename to src/devices/Mpu6xxx9xxx/Mpu6xxx9xxx.sln index e262c8cbbf10ee16998291d14cb973251d2caafb..e536000dc82ef56b84fbf0ae99749ddb6a8f746d 100644 --- a/src/devices/Mpu9250/Mpu9250.sln +++ b/src/devices/Mpu6xxx9xxx/Mpu6xxx9xxx.sln @@ -1,13 +1,15 @@ - + Microsoft Visual Studio Solution File, Format Version 12.00 -# Visual Studio 15 -VisualStudioVersion = 15.0.26124.0 +# Visual Studio Version 16 +VisualStudioVersion = 16.0.32106.194 MinimumVisualStudioVersion = 15.0.26124.0 Project("{2150E333-8FDC-42A3-9474-1A3956D46DE8}") = "samples", "samples", "{59C8658D-6633-48D0-9E42-17F64BA63302}" EndProject -Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Mpu9250.sample", "samples\Mpu9250.sample.csproj", "{B8C988E9-A95A-4A8B-8D73-F821D64AEA9C}" +Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "Mpu9250.sample", "samples\Mpu9250\Mpu9250.sample.csproj", "{B8C988E9-A95A-4A8B-8D73-F821D64AEA9C}" EndProject -Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Mpu9250", "Mpu9250.csproj", "{F57412F0-ECDA-4268-99A0-0C8835A94395}" +Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "Mpu6050.sample", "samples\Mpu6050\Mpu6050.sample.csproj", "{7C19C242-8DA5-4C4C-9BBC-AC28C7D53D58}" +EndProject +Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "Mpu6xxx9xxx", "Mpu6xxx9xxx.csproj", "{F57412F0-ECDA-4268-99A0-0C8835A94395}" EndProject Global GlobalSection(SolutionConfigurationPlatforms) = preSolution @@ -34,6 +36,18 @@ Global {B8C988E9-A95A-4A8B-8D73-F821D64AEA9C}.Release|x64.Build.0 = Release|Any CPU {B8C988E9-A95A-4A8B-8D73-F821D64AEA9C}.Release|x86.ActiveCfg = Release|Any CPU {B8C988E9-A95A-4A8B-8D73-F821D64AEA9C}.Release|x86.Build.0 = Release|Any CPU + {7C19C242-8DA5-4C4C-9BBC-AC28C7D53D58}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {7C19C242-8DA5-4C4C-9BBC-AC28C7D53D58}.Debug|Any CPU.Build.0 = Debug|Any CPU + {7C19C242-8DA5-4C4C-9BBC-AC28C7D53D58}.Debug|x64.ActiveCfg = Debug|Any CPU + {7C19C242-8DA5-4C4C-9BBC-AC28C7D53D58}.Debug|x64.Build.0 = Debug|Any CPU + {7C19C242-8DA5-4C4C-9BBC-AC28C7D53D58}.Debug|x86.ActiveCfg = Debug|Any CPU + {7C19C242-8DA5-4C4C-9BBC-AC28C7D53D58}.Debug|x86.Build.0 = Debug|Any CPU + {7C19C242-8DA5-4C4C-9BBC-AC28C7D53D58}.Release|Any CPU.ActiveCfg = Release|Any CPU + {7C19C242-8DA5-4C4C-9BBC-AC28C7D53D58}.Release|Any CPU.Build.0 = Release|Any CPU + {7C19C242-8DA5-4C4C-9BBC-AC28C7D53D58}.Release|x64.ActiveCfg = Release|Any CPU + {7C19C242-8DA5-4C4C-9BBC-AC28C7D53D58}.Release|x64.Build.0 = Release|Any CPU + {7C19C242-8DA5-4C4C-9BBC-AC28C7D53D58}.Release|x86.ActiveCfg = Release|Any CPU + {7C19C242-8DA5-4C4C-9BBC-AC28C7D53D58}.Release|x86.Build.0 = Release|Any CPU {F57412F0-ECDA-4268-99A0-0C8835A94395}.Debug|Any CPU.ActiveCfg = Debug|Any CPU {F57412F0-ECDA-4268-99A0-0C8835A94395}.Debug|Any CPU.Build.0 = Debug|Any CPU {F57412F0-ECDA-4268-99A0-0C8835A94395}.Debug|x64.ActiveCfg = Debug|Any CPU @@ -49,5 +63,9 @@ Global EndGlobalSection GlobalSection(NestedProjects) = preSolution {B8C988E9-A95A-4A8B-8D73-F821D64AEA9C} = {59C8658D-6633-48D0-9E42-17F64BA63302} + {7C19C242-8DA5-4C4C-9BBC-AC28C7D53D58} = {59C8658D-6633-48D0-9E42-17F64BA63302} + EndGlobalSection + GlobalSection(ExtensibilityGlobals) = postSolution + SolutionGuid = {07264935-6DD3-485B-98CF-891BC8BA5671} EndGlobalSection EndGlobal diff --git a/src/devices/Mpu9250/Mpu9250.cs b/src/devices/Mpu6xxx9xxx/Mpu9250.cs similarity index 100% rename from src/devices/Mpu9250/Mpu9250.cs rename to src/devices/Mpu6xxx9xxx/Mpu9250.cs diff --git a/src/devices/Mpu9250/README.md b/src/devices/Mpu6xxx9xxx/README.md similarity index 93% rename from src/devices/Mpu9250/README.md rename to src/devices/Mpu6xxx9xxx/README.md index c12d495e2c2f3b03807b7ab7ee7294045b2f8c2b..312532639f70b51b2b386c4f93201d7f0debceef 100644 --- a/src/devices/Mpu9250/README.md +++ b/src/devices/Mpu6xxx9xxx/README.md @@ -1,6 +1,6 @@ -# MPU6500/MPU9250 - Gyroscope, Accelerometer, Temperature and Magnetometer (MPU9250 only) +# MPU6050/MPU6500/MPU9250 - Gyroscope, Accelerometer, Temperature and Magnetometer (MPU9250 only) -MPU6500 is a 3 axis Gyroscope, 3 axis Accelerometer and Temperature sensor that can be accessed either thru I2C or SPI. This implementation is only for I2C. The sensor can be found in various implementation but its main usage is in the MPU9250. +MPU6000, MPU6050 and PMU6500 are a 3 axis Gyroscope, 3 axis Accelerometer and Temperature sensor that can be accessed either thru I2C or SPI. This implementation is only for I2C. The MPU6000 and MPU6050 are identical. They have very little differences with MPU6500. The MPU6500 sensor can be found in various implementation but its main usage is in the MPU9250. MPU9250 is a 3 axis Gyroscope, 3 axis Accelerometer, 3 axis Magnetometer and Temperature sensor that can be accessed either thru I2C or SPI. This implementation is only for I2C. The sensor can be found in various implementation like [Grove](http://wiki.seeedstudio.com/Grove-IMU_9DOF_v2.0/) or [Sparkfun](https://www.sparkfun.com/products/13762). MPU9250 incorporate a MPU6500 and an AK8963. @@ -10,6 +10,8 @@ The Magnetometer used is an [AK8963](../Ak8963/README.md). In general, it is man * Registers [documentation](http://www.invensense.com/wp-content/uploads/2017/11/RM-MPU-9250A-00-v1.6.pdf) * [Product specifications](http://www.invensense.com/wp-content/uploads/2015/02/PS-MPU-9250A-01-v1.1.pdf) +* [MPU6000 and MPU6050 register map](https://invensense.tdk.com/wp-content/uploads/2015/02/MPU-6000-Register-Map1.pdf) +* [MPU6500 register map](https://invensense.tdk.com/wp-content/uploads/2015/02/MPU-6500-Register-Map2.pdf) ## Usage diff --git a/src/devices/Mpu9250/Register.cs b/src/devices/Mpu6xxx9xxx/Register.cs similarity index 100% rename from src/devices/Mpu9250/Register.cs rename to src/devices/Mpu6xxx9xxx/Register.cs diff --git a/src/devices/Mpu9250/SelfTestScale.cs b/src/devices/Mpu6xxx9xxx/SelfTestScale.cs similarity index 100% rename from src/devices/Mpu9250/SelfTestScale.cs rename to src/devices/Mpu6xxx9xxx/SelfTestScale.cs diff --git a/src/devices/Mpu9250/UserControls.cs b/src/devices/Mpu6xxx9xxx/UserControls.cs similarity index 100% rename from src/devices/Mpu9250/UserControls.cs rename to src/devices/Mpu6xxx9xxx/UserControls.cs diff --git a/src/devices/Mpu9250/category.txt b/src/devices/Mpu6xxx9xxx/category.txt similarity index 100% rename from src/devices/Mpu9250/category.txt rename to src/devices/Mpu6xxx9xxx/category.txt diff --git a/src/devices/Mpu6xxx9xxx/images/Mpu6050.png b/src/devices/Mpu6xxx9xxx/images/Mpu6050.png new file mode 100644 index 0000000000000000000000000000000000000000..c4d289d076befa48a6a6ebeb7527844aeebb25a4 Binary files /dev/null and b/src/devices/Mpu6xxx9xxx/images/Mpu6050.png differ diff --git a/src/devices/Mpu6xxx9xxx/images/Mpu9250.png b/src/devices/Mpu6xxx9xxx/images/Mpu9250.png new file mode 100644 index 0000000000000000000000000000000000000000..23fbbea06c31ca3b3c86c2e29ea5a6dfcca2c106 Binary files /dev/null and b/src/devices/Mpu6xxx9xxx/images/Mpu9250.png differ diff --git a/src/devices/Mpu6xxx9xxx/samples/Mpu6050/Mpu6050.sample.csproj b/src/devices/Mpu6xxx9xxx/samples/Mpu6050/Mpu6050.sample.csproj new file mode 100644 index 0000000000000000000000000000000000000000..657368377d0796224634351f534653fd97e96f81 --- /dev/null +++ b/src/devices/Mpu6xxx9xxx/samples/Mpu6050/Mpu6050.sample.csproj @@ -0,0 +1,9 @@ + + + Exe + $(DefaultSampleTfms) + + + + + \ No newline at end of file diff --git a/src/devices/Mpu6xxx9xxx/samples/Mpu6050/Program.cs b/src/devices/Mpu6xxx9xxx/samples/Mpu6050/Program.cs new file mode 100644 index 0000000000000000000000000000000000000000..8a9b4b52ba6748281ec496983b96ed60eab33e59 --- /dev/null +++ b/src/devices/Mpu6xxx9xxx/samples/Mpu6050/Program.cs @@ -0,0 +1,23 @@ +// Licensed to the .NET Foundation under one or more agreements. +// The .NET Foundation licenses this file to you under the MIT license. + +using System; +using System.Threading; +using System.Device.I2c; +using System.Numerics; +using Iot.Device.Imu; + +I2cConnectionSettings settings = new(busId: 1, deviceAddress: Mpu6050.DefaultI2cAddress); +using (Mpu6050 ag = new(I2cDevice.Create(settings))) +{ + Console.WriteLine($"Internal temperature: {ag.GetTemperature().DegreesCelsius} C"); + + while (!Console.KeyAvailable) + { + var acc = ag.GetAccelerometer(); + var gyr = ag.GetGyroscopeReading(); + Console.WriteLine($"Accelerometer data x:{acc.X} y:{acc.Y} z:{acc.Z}"); + Console.WriteLine($"Gyroscope data x:{gyr.X} y:{gyr.Y} z:{gyr.Z}\n"); + Thread.Sleep(100); + } +} \ No newline at end of file diff --git a/src/devices/Mpu9250/samples/Mpu9250.sample.csproj b/src/devices/Mpu6xxx9xxx/samples/Mpu9250/Mpu9250.sample.csproj similarity index 73% rename from src/devices/Mpu9250/samples/Mpu9250.sample.csproj rename to src/devices/Mpu6xxx9xxx/samples/Mpu9250/Mpu9250.sample.csproj index c29a7c43c60f8f05da435a7f979ddb0bbf4b58e7..657368377d0796224634351f534653fd97e96f81 100644 --- a/src/devices/Mpu9250/samples/Mpu9250.sample.csproj +++ b/src/devices/Mpu6xxx9xxx/samples/Mpu9250/Mpu9250.sample.csproj @@ -4,6 +4,6 @@ $(DefaultSampleTfms) - + \ No newline at end of file diff --git a/src/devices/Mpu9250/samples/Program.cs b/src/devices/Mpu6xxx9xxx/samples/Mpu9250/Program.cs similarity index 100% rename from src/devices/Mpu9250/samples/Program.cs rename to src/devices/Mpu6xxx9xxx/samples/Mpu9250/Program.cs diff --git a/src/devices/Mpu9250/Mpu9250.fzz b/src/devices/Mpu9250/Mpu9250.fzz deleted file mode 100644 index 2701d66ef3d54e3c5568e426bb429d98d5171b59..0000000000000000000000000000000000000000 Binary files a/src/devices/Mpu9250/Mpu9250.fzz and /dev/null differ diff --git a/src/devices/Mpu9250/Mpu9250_bb.png b/src/devices/Mpu9250/Mpu9250_bb.png deleted file mode 100644 index 21c1fc0c1d238cab09d190842d6881580c18ebb4..0000000000000000000000000000000000000000 Binary files a/src/devices/Mpu9250/Mpu9250_bb.png and /dev/null differ