Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix calculations of BMM150 #2376

Merged
merged 10 commits into from
Jan 28, 2025
Merged

Fix calculations of BMM150 #2376

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
10 commits
Select commit Hold shift + click to select a range
6611042
Solution layout fixed
pgrawehr Dec 31, 2024
8be0e89
Fix various incorrect sign usages
pgrawehr Dec 31, 2024
0f59972
Compensation calculation fixed
pgrawehr Jan 1, 2025
0e618e1
Fix heading calculation
pgrawehr Jan 1, 2025
9aa6429
Documentation update
pgrawehr Jan 18, 2025
2e4c2be
Fix for malfunctioning serial port drivers
pgrawehr Jan 18, 2025
6d013e2
Documentation fix
pgrawehr Jan 18, 2025
0c5c8f4
Update suppressions
pgrawehr Jan 19, 2025
c777f98
Review findings
pgrawehr Jan 27, 2025
78be65b
Format fix
pgrawehr Jan 28, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
70 changes: 70 additions & 0 deletions src/Iot.Device.Bindings/CompatibilitySuppressions.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -50,6 +50,41 @@
<Right>lib/net6.0/Iot.Device.Bindings.dll</Right>
<IsBaselineSuppression>true</IsBaselineSuppression>
</Suppression>
<Suppression>
<DiagnosticId>CP0002</DiagnosticId>
<Target>M:Iot.Device.Bmp180.Bmm150.CalibrateMagnetometer(System.Int32)</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>M:Iot.Device.Bmp180.Bmm150.GetDeviceInfo</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>M:Iot.Device.Bmp180.Bmm150Compensation.CompensateX(System.Double,System.UInt32,Iot.Device.Bmp180.Bmm150TrimRegisterData)</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>M:Iot.Device.Bmp180.Bmm150Compensation.CompensateY(System.Double,System.UInt32,Iot.Device.Bmp180.Bmm150TrimRegisterData)</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>M:Iot.Device.Bmp180.Bmm150Compensation.CompensateZ(System.Double,System.UInt32,Iot.Device.Bmp180.Bmm150TrimRegisterData)</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>M:Iot.Device.Board.Board.CreatePwmChannel(System.Int32,System.Int32,System.Int32,System.Double,System.Int32,System.Device.Gpio.PinNumberingScheme)</Target>
Expand Down Expand Up @@ -183,6 +218,41 @@
<Right>lib/netstandard2.0/Iot.Device.Bindings.dll</Right>
<IsBaselineSuppression>true</IsBaselineSuppression>
</Suppression>
<Suppression>
<DiagnosticId>CP0002</DiagnosticId>
<Target>M:Iot.Device.Bmp180.Bmm150.CalibrateMagnetometer(System.Int32)</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>M:Iot.Device.Bmp180.Bmm150.GetDeviceInfo</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>M:Iot.Device.Bmp180.Bmm150Compensation.CompensateX(System.Double,System.UInt32,Iot.Device.Bmp180.Bmm150TrimRegisterData)</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>M:Iot.Device.Bmp180.Bmm150Compensation.CompensateY(System.Double,System.UInt32,Iot.Device.Bmp180.Bmm150TrimRegisterData)</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>M:Iot.Device.Bmp180.Bmm150Compensation.CompensateZ(System.Double,System.UInt32,Iot.Device.Bmp180.Bmm150TrimRegisterData)</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>M:Iot.Device.Board.Board.CreatePwmChannel(System.Int32,System.Int32,System.Int32,System.Double,System.Int32,System.Device.Gpio.PinNumberingScheme)</Target>
Expand Down
7 changes: 7 additions & 0 deletions src/devices/Arduino/ArduinoBoard.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -79,6 +79,11 @@ public ArduinoBoard(Stream serialPortStream, bool usesHardwareFlowControl)
/// <remarks>
/// The device is initialized when the first command is sent. The constructor always succeeds.
/// </remarks>
/// <remarks>
/// The stream must have a blocking read operation, or the connection might fail. Some serial port drivers incorrectly
/// return immediately when no data is available and the <code>ReadTimeout</code> is set to infinite (the default). In such a case, set the
/// ReadTimeout to a large value (such as <code>Int.Max - 10</code>), which will simulate a blocking call.
/// </remarks>
/// <param name="serialPortStream">A stream to an Arduino/Firmata device</param>
public ArduinoBoard(Stream serialPortStream)
: this(serialPortStream, false)
Expand All @@ -96,6 +101,8 @@ public ArduinoBoard(string portName, int baudRate)
{
_dataStream = null;
_serialPort = new SerialPort(portName, baudRate);
// Set the timeout to a long time, but not infinite. See the note for the constructor above.
_serialPort.ReadTimeout = int.MaxValue - 10;
pgrawehr marked this conversation as resolved.
Show resolved Hide resolved
StreamUsesHardwareFlowControl = false; // Would need to configure the serial port externally for this to work
_logger = this.GetCurrentClassLogger();
}
Expand Down
15 changes: 11 additions & 4 deletions src/devices/Arduino/FirmataDevice.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -741,12 +741,19 @@ private bool FillQueue()
throw new ObjectDisposedException(nameof(FirmataDevice));
}

Span<byte> rawData = stackalloc byte[512];
try
{
Span<byte> rawData = stackalloc byte[512];

int bytesRead = _firmataStream.Read(rawData);
for (int i = 0; i < bytesRead; i++)
int bytesRead = _firmataStream.Read(rawData);
for (int i = 0; i < bytesRead; i++)
{
_dataQueue.Enqueue(rawData[i]);
}
}
catch (TimeoutException x)
{
_dataQueue.Enqueue(rawData[i]);
_logger.LogWarning(x, "Input stream reported timeout - likely and incorrectly configured driver and thus ignoring.");
}

return _dataQueue.Count > 0;
Expand Down
126 changes: 40 additions & 86 deletions src/devices/Bmm150/Bmm150.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,7 @@
using System.IO;
using System.Numerics;
using System.Threading;
using UnitsNet;

namespace Iot.Device.Bmp180
{
Expand Down Expand Up @@ -132,67 +133,6 @@ private void Initialize()
WriteRegister(Bmp180Register.OP_MODE_ADDR, 0x00);
}

/// <summary>
/// Get the device information
/// </summary>
/// <returns>The device information</returns>
public byte GetDeviceInfo() => ReadByte(Bmp180Register.INFO);

/// <summary>
/// Calibrate the magnetometer.
/// Please make sure you are not close to any magnetic field like magnet or phone
/// Please make sure you are moving the magnetometer all over space, rotating it.
/// </summary>
/// <param name="numberOfMeasurements">Number of measurement for the calibration, default is 100</param>
// https://platformio.org/lib/show/12697/M5_BMM150
public void CalibrateMagnetometer(int numberOfMeasurements = 100)
{
Vector3 mag_min = new Vector3() { X = 9000, Y = 9000, Z = 30000 };
Vector3 mag_max = new Vector3() { X = -9000, Y = -9000, Z = -30000 };
Vector3 rawMagnetometerData;

for (int i = 0; i < numberOfMeasurements; i++)
{
try
{
rawMagnetometerData = ReadMagnetometerWithoutCorrection();

if (rawMagnetometerData.X != 0)
{
mag_min.X = (rawMagnetometerData.X < mag_min.X) ? rawMagnetometerData.X : mag_min.X;
mag_max.X = (rawMagnetometerData.X > mag_max.X) ? rawMagnetometerData.X : mag_max.X;
}

if (rawMagnetometerData.Y != 0)
{
mag_max.Y = (rawMagnetometerData.Y > mag_max.Y) ? rawMagnetometerData.Y : mag_max.Y;
mag_min.Y = (rawMagnetometerData.Y < mag_min.Y) ? rawMagnetometerData.Y : mag_min.Y;
}

if (rawMagnetometerData.Z != 0)
{
mag_min.Z = (rawMagnetometerData.Z < mag_min.Z) ? rawMagnetometerData.Z : mag_min.Z;
mag_max.Z = (rawMagnetometerData.Z > mag_max.Z) ? rawMagnetometerData.Z : mag_max.Z;
}

// Wait for 100ms until next reading
Wait(100);
}
catch
{
// skip this reading
}
}

// Refresh CalibrationCompensation vector
CalibrationCompensation = new Vector3()
{
X = (mag_max.X + mag_min.X) / 2,
Y = (mag_max.Y + mag_min.Y) / 2,
Z = (mag_max.Z + mag_min.Z) / 2
};
}

/// <summary>
/// True if there is a data to read
/// </summary>
Expand Down Expand Up @@ -239,28 +179,41 @@ public Vector3 ReadMagnetometerWithoutCorrection(bool waitForData, TimeSpan time

Vector3 magnetoRaw = new Vector3();

// Shift the MSB data to left by 5 bits
// Multiply by 32 to get the shift left by 5 value
magnetoRaw.X = (rawData[1] & 0x7F) << 5 | rawData[0] >> 3;
if ((rawData[1] & 0x80) == 0x80)
// Because we mix and match signed and unsigned below
unchecked
{
magnetoRaw.X = -magnetoRaw.X;
}
int temp;
// Shift the MSB data to left by 5 bits
// Multiply by 32 to get the shift left by 5 value
// X and Y have 13 significant bits each
temp = (rawData[1]) << 5 | rawData[0] >> 3;
if ((rawData[1] & 0x80) == 0x80)
{
temp = temp | (int)0xFFFFE000;
}

// Shift the MSB data to left by 5 bits
// Multiply by 32 to get the shift left by 5 value
magnetoRaw.Y = (rawData[3] & 0x07F) << 5 | rawData[2] >> 3;
if ((rawData[3] & 0x80) == 0x80)
{
magnetoRaw.Y = -magnetoRaw.Y;
}
magnetoRaw.X = temp;

// Shift the MSB data to left by 7 bits
// Multiply by 128 to get the shift left by 7 value
magnetoRaw.Z = (rawData[5] & 0x07F) << 7 | rawData[4] >> 1;
if ((rawData[5] & 0x80) == 0x80)
{
magnetoRaw.Z = -magnetoRaw.Z;
// Shift the MSB data to left by 5 bits
// Multiply by 32 to get the shift left by 5 value
temp = (rawData[3]) << 5 | rawData[2] >> 3;
if ((rawData[3] & 0x80) == 0x80)
{
temp = temp | (int)0xFFFFE000;
}

magnetoRaw.Y = temp;

// Shift the MSB data to left by 7 bits
// Multiply by 128 to get the shift left by 7 value
// The Z value has 15 significant bits
temp = (rawData[5]) << 7 | rawData[4] >> 1;
if ((rawData[5] & 0x80) == 0x80)
{
temp = temp | (int)0xFFFF8000;
}

magnetoRaw.Z = temp;
}

_rHall = (uint)(rawData[7] << 6 | rawData[6] >> 2);
Expand All @@ -274,23 +227,24 @@ public Vector3 ReadMagnetometerWithoutCorrection(bool waitForData, TimeSpan time
/// <param name="waitForData">true to wait for new data</param>
/// <returns>The data from the magnetometer</returns>
[Telemetry("Magnetometer")]
public Vector3 ReadMagnetometer(bool waitForData = true) => ReadMagnetometer(waitForData, DefaultTimeout);
public MagnetometerData ReadMagnetometer(bool waitForData = true) => ReadMagnetometer(waitForData, DefaultTimeout);

/// <summary>
/// Read the magnetometer with compensation calculation and can wait for new data to be present
/// </summary>
/// <param name="waitForData">true to wait for new data</param>
/// <param name="timeout">timeout for waiting the data, ignored if waitForData is false</param>
/// <returns>The data from the magnetometer</returns>
public Vector3 ReadMagnetometer(bool waitForData, TimeSpan timeout)
public MagnetometerData ReadMagnetometer(bool waitForData, TimeSpan timeout)
{
var magn = ReadMagnetometerWithoutCorrection(waitForData, timeout);

magn.X = (float)Bmm150Compensation.CompensateX(magn.X - CalibrationCompensation.X, _rHall, _trimData);
magn.Y = (float)Bmm150Compensation.CompensateY(magn.Y - CalibrationCompensation.Y, _rHall, _trimData);
magn.Z = (float)Bmm150Compensation.CompensateZ(magn.Z - CalibrationCompensation.Z, _rHall, _trimData);
MagnetometerData ret = new MagnetometerData(
MagneticField.FromMicroteslas(Bmm150Compensation.CompensateX((int)magn.X, _rHall, _trimData) - CalibrationCompensation.X),
MagneticField.FromMicroteslas(Bmm150Compensation.CompensateY((int)magn.Y, _rHall, _trimData) - CalibrationCompensation.Y),
MagneticField.FromMicroteslas(Bmm150Compensation.CompensateZ((int)magn.Z, _rHall, _trimData) - CalibrationCompensation.Z));

return magn;
return ret;
}

private void WriteRegister(Bmp180Register reg, byte data) => _bmm150Interface.WriteRegister(_i2cDevice, (byte)reg, data);
Expand Down
Loading