Skip to content

I
Iot

jobily / Iot

通知 1
Star 0
Fork 0
I
Iot

体验新版 GitCode，发现更多精彩内容 >>

未验证 提交 c742336e 编写于 作者: S Sergey Karmanov 提交者： GitHub
浏览文件
操作

Added MPU6050 support (#1762) 

* Added MPU6050

Based on MPU6886

* Remove MPU6050 folder

* Moved the Fritzing file to a new schema folder and added a schema for mpu6050

* Added MPU6050 sensor

* Adjusting MPU6500 to remove duplicate code

* Added null check, and updated README

* Rename Mpu folder and upload images

* Change device name in README
Co-authored-by: NLaurent Ellerbach <laurelle@microsoft.com>
Co-authored-by: NLaurent Ellerbach <laurelle@microsoft.com>
上级 3b51723a
隐藏空白更改
内联 并排
Showing with 385 addition and 199 deletion
src/Iot.Device.Bindings/CompatibilitySuppressions.xml
浏览文件 @ c742336e
<?xml version="1.0" encoding="utf-8"?>
<Suppressions xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns:xsd="http://www.w3.org/2001/XMLSchema">
<!-- All of the bellow suppresions are because we renamed the bindings that had "IoT" as a namespace to instead be "Iot" -->
<Suppression>
<DiagnosticId>CP0001</DiagnosticId>
<Target>T:IoT.Device.Tsl256x.Channel</Target>
......@@ -169,4 +168,46 @@
<Right>lib/netstandard2.0/Iot.Device.Bindings.dll</Right>
<IsBaselineSuppression>true</IsBaselineSuppression>
</Suppression>
<Suppression>
<DiagnosticId>CP0002</DiagnosticId>
<Target>F:Iot.Device.Imu.Mpu6500.DefaultI2cAddress</Target>
<Left>lib/net6.0/Iot.Device.Bindings.dll</Left>
<Right>lib/net6.0/Iot.Device.Bindings.dll</Right>
<IsBaselineSuppression>true</IsBaselineSuppression>
</Suppression>
<Suppression>
<DiagnosticId>CP0002</DiagnosticId>
<Target>F:Iot.Device.Imu.Mpu6500.SecondI2cAddress</Target>
<Left>lib/net6.0/Iot.Device.Bindings.dll</Left>
<Right>lib/net6.0/Iot.Device.Bindings.dll</Right>
<IsBaselineSuppression>true</IsBaselineSuppression>
</Suppression>
<Suppression>
<DiagnosticId>CP0002</DiagnosticId>
<Target>F:Iot.Device.Imu.Mpu6500.DefaultI2cAddress</Target>
<Left>lib/netcoreapp3.1/Iot.Device.Bindings.dll</Left>
<Right>lib/netcoreapp3.1/Iot.Device.Bindings.dll</Right>
<IsBaselineSuppression>true</IsBaselineSuppression>
</Suppression>
<Suppression>
<DiagnosticId>CP0002</DiagnosticId>
<Target>F:Iot.Device.Imu.Mpu6500.SecondI2cAddress</Target>
<Left>lib/netcoreapp3.1/Iot.Device.Bindings.dll</Left>
<Right>lib/netcoreapp3.1/Iot.Device.Bindings.dll</Right>
<IsBaselineSuppression>true</IsBaselineSuppression>
</Suppression>
<Suppression>
<DiagnosticId>CP0002</DiagnosticId>
<Target>F:Iot.Device.Imu.Mpu6500.DefaultI2cAddress</Target>
<Left>lib/netstandard2.0/Iot.Device.Bindings.dll</Left>
<Right>lib/netstandard2.0/Iot.Device.Bindings.dll</Right>
<IsBaselineSuppression>true</IsBaselineSuppression>
</Suppression>
<Suppression>
<DiagnosticId>CP0002</DiagnosticId>
<Target>F:Iot.Device.Imu.Mpu6500.SecondI2cAddress</Target>
<Left>lib/netstandard2.0/Iot.Device.Bindings.dll</Left>
<Right>lib/netstandard2.0/Iot.Device.Bindings.dll</Right>
<IsBaselineSuppression>true</IsBaselineSuppression>
</Suppression>
</Suppressions>
\ No newline at end of file
src/devices/Mpu9250/Mpu6500.cs → src/devices/Mpu6xxx9xxx/Mpu6050.cs
浏览文件 @ c742336e
......@@ -17,10 +17,10 @@ using UnitsNet;
namespace Iot.Device.Imu
{
/// <summary>
/// MPU6500 - gyroscope, accelerometer and temperature sensor
/// MPU6050 - gyroscope, accelerometer and temperature sensor
/// </summary>
[Interface("MPU6500 - gyroscope, accelerometer and temperature sensor")]
public class Mpu6500 : IDisposable
[Interface("MPU6050 - gyroscope, accelerometer and temperature sensor")]
public class Mpu6050 : IDisposable
{
/// <summary>
/// Default address for MPU9250
......@@ -44,17 +44,22 @@ namespace Iot.Device.Imu
internal bool _wakeOnMotion;
/// <summary>
/// Initialize the MPU6500
/// Initialize the MPU6050
/// </summary>
/// <param name="i2cDevice">The I2C device</param>
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
}
/// <summary>
/// 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
/// </summary>
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()
/// <summary>
/// Gets the raw accelerometer data
/// </summary>
/// <returns></returns>
internal Vector3 GetRawAccelerometer()
{
Span<byte> 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()
/// <summary>
/// Gets the raw gyroscope data
/// </summary>
/// <returns></returns>
internal Vector3 GetRawGyroscope()
{
Span<byte> 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<byte> 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
}
/// <summary>
/// Return true if the version of MPU6500 is the correct one
/// Return true if the version of MPU6050 is the correct one
/// </summary>
/// <returns>True if success</returns>
// 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;
/// <summary>
/// Get or set the sample diver mode
......@@ -444,7 +468,8 @@ namespace Iot.Device.Imu
{
Span<byte> 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);
}
/// <summary>
/// <![CDATA[
/// Run a self test and returns the gyroscope and accelerometer vectores
/// a. If factory Self-Test values ST_OTP≠0, compare the current Self-Test response (GXST, GYST, GZST, AXST, AYST and AZST)
/// to the factory Self-Test values (ST_OTP) and report Self-Test is passing if all the following criteria are fulfilled:
/// Axis | Pass criteria
/// X-gyro | (GXST / GXST_OTP) > 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
/// ]]>
/// </summary>
/// <returns>the gyroscope and accelerometer vectors</returns>
[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<byte> 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<byte> 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<byte> dataout = stackalloc byte[]
{
(byte)reg, data
(byte)reg,
data
};
_i2cDevice.Write(dataout);
}
......
src/devices/Mpu6xxx9xxx/Mpu6500.cs 0 → 100644
浏览文件 @ c742336e
// 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
{
/// <summary>
/// MPU6500 - gyroscope, accelerometer and temperature sensor
/// </summary>
[Interface("MPU6500 - gyroscope, accelerometer and temperature sensor")]
public class Mpu6500 : Mpu6050
{
/// <summary>
/// Initialize the MPU6500
/// </summary>
/// <param name="i2cDevice">The I2C device</param>
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;
}
/// <summary>
/// Used to create the class for the MPU9250. Initialization is a bit different than for the MPU6500
/// </summary>
internal Mpu6500(I2cDevice i2cDevice, bool isInternal)
: base(i2cDevice, isInternal)
{
}
#region Modes, constructor, Dispose
/// <summary>
/// Return true if the version of MPU6500 is the correct one
/// </summary>
/// <returns>True if success</returns>
// Check if the version is thee correct one
internal new bool CheckVersion() => ReadByte(Register.WHO_AM_I) == 0x70;
/// <summary>
/// <![CDATA[
/// Run a self test and returns the gyroscope and accelerometer vectores
/// a. If factory Self-Test values ST_OTP≠0, compare the current Self-Test response (GXST, GYST, GZST, AXST, AYST and AZST)
/// to the factory Self-Test values (ST_OTP) and report Self-Test is passing if all the following criteria are fulfilled:
/// Axis | Pass criteria
/// X-gyro | (GXST / GXST_OTP) > 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
/// ]]>
/// </summary>
/// <returns>the gyroscope and accelerometer vectors</returns>
[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
}
}
src/devices/Mpu9250/Mpu9250.sln → src/devices/Mpu6xxx9xxx/Mpu6xxx9xxx.sln
浏览文件 @ c742336e

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
src/devices/Mpu9250/README.md → src/devices/Mpu6xxx9xxx/README.md
浏览文件 @ c742336e
# 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
......
src/devices/Mpu6xxx9xxx/samples/Mpu6050/Mpu6050.sample.csproj 0 → 100644
浏览文件 @ c742336e
<Project Sdk="Microsoft.NET.Sdk">
<PropertyGroup>
<OutputType>Exe</OutputType>
<TargetFramework>$(DefaultSampleTfms)</TargetFramework>
</PropertyGroup>
<ItemGroup>
<ProjectReference Include="..\..\Mpu6xxx9xxx.csproj" />
</ItemGroup>
</Project>
\ No newline at end of file
src/devices/Mpu6xxx9xxx/samples/Mpu6050/Program.cs 0 → 100644
浏览文件 @ c742336e
// 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
src/devices/Mpu9250/samples/Mpu9250.sample.csproj → src/devices/Mpu6xxx9xxx/samples/Mpu9250/Mpu9250.sample.csproj
浏览文件 @ c742336e
......@@ -4,6 +4,6 @@
<TargetFramework>$(DefaultSampleTfms)</TargetFramework>
</PropertyGroup>
<ItemGroup>
<ProjectReference Include="..\Mpu9250.csproj" />
<ProjectReference Include="..\..\Mpu6xxx9xxx.csproj" />
</ItemGroup>
</Project>
\ No newline at end of file
Markdown is supported
0% 或 .
You are about to add 0 people to the discussion. Proceed with caution.
先完成此消息的编辑！
想要评论请 注册 或