Skip to content

Commit

Permalink
Fix calculations of BMM150 (#2376)
Browse files Browse the repository at this point in the history 
* Solution layout fixed

* Fix various incorrect sign usages

The reported sensor output was completely wrong

* Compensation calculation fixed

* Fix heading calculation

Include it in API, to simplify usage and avoid errors

* Documentation update

* Fix for malfunctioning serial port drivers

* Update suppressions

* Review findings
  • Loading branch information
@pgrawehr
pgrawehr authored Jan 28, 2025
1 parent dd8e964 commit 9b7972d
Show file tree
Hide file tree
Showing 18 changed files with 671 additions and 272 deletions.
70 changes: 70 additions & 0 deletions src/Iot.Device.Bindings/CompatibilitySuppressions.xml
View file
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
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;
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
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
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

0 comments on commit 9b7972d

Please sign in to comment.