Bmm150.cs

// 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.Device.I2c;
using System.Device.Model;
using System.Diagnostics;
using System.IO;
using System.Numerics;
using System.Threading;

namespace Iot.Device.Magnetometer
{
    /// <summary>
    /// Bmm150 class implementing a magnetometer.
    /// </summary>
    [Interface("Bmm150 class implementing a magnetometer")]
    public sealed class Bmm150 : IDisposable
    {
        /// <summary>
        /// I2c device comm channel.
        /// </summary>
        private I2cDevice _i2cDevice;

        /// <summary>
        /// Bmm150 device interface.
        /// </summary>
        private Bmm150I2cBase _bmm150Interface;

        /// <summary>
        /// Bmm150 Trim extended register data.
        /// </summary>
        private Bmm150TrimRegisterData _trimData;

        /// <summary>
        /// Magnetometer (R-HALL) temperature compensation value, used in axis compensation calculation functions.
        /// </summary>
        private uint _rHall;

        /// <summary>
        /// Flag to evaluate disposal of resources.
        /// </summary>
        private bool _shouldDispose = true;

        /// <summary>
        /// Gets or sets Magnetometer calibration compensation vector.
        /// </summary>
        public Vector3 CalibrationCompensation { get; set; } = new Vector3();

        /// <summary>
        /// Primary I2C address for the Bmm150
        /// In the official sheet (P36) states that address is 0x13.
        /// Visit https://github.com/m5stack/M5_BMM150/blob/master/src/M5_BMM150_DEFS.h#L16.3 for more information.
        /// </summary>
        public const byte PrimaryI2cAddress = 0x13;

        /// <summary>
        /// Secondary I2C address for the Bmm150
        /// In the official sheet (P36) states that address is 0x13, alhtough for m5stack is 0x10.
        /// </summary>
        public const byte SecondaryI2cAddress = 0x10;

        /// <summary>
        /// Gets or sets default timeout to use when timeout is not provided in the reading methods.
        /// </summary>
        [Property]
        public TimeSpan DefaultTimeout { get; set; } = TimeSpan.FromSeconds(1);

        /// <summary>
        /// Initializes a new instance of the <see cref="Bmm150" /> class.
        /// </summary>
        /// <param name="i2CDevice">The I2C device.</param>
        public Bmm150(I2cDevice i2CDevice)
            : this(i2CDevice, new Bmm150I2c())
        {
        }

        /// <summary>
        /// Initializes a new instance of the <see cref="Bmm150" /> class.
        /// </summary>
        /// <param name="i2cDevice">The I2C device.</param>
        /// <param name="bmm150Interface">The specific interface to communicate with the Bmm150.</param>
        /// <param name="shouldDispose">True to dispose the I2C device when class is disposed.</param>
        public Bmm150(I2cDevice i2cDevice, Bmm150I2cBase bmm150Interface, bool shouldDispose = true)
        {
            _i2cDevice = i2cDevice ?? throw new ArgumentNullException(nameof(i2cDevice));
            _bmm150Interface = bmm150Interface;
            _shouldDispose = shouldDispose;

            Initialize();

            // After initializing the device we read the 
            _trimData = ReadTrimRegisters();
        }

        /// <summary>
        /// Reads the trim registers of the sensor, used in compensation (x,y,z) calculation
        /// Visit https://github.com/BoschSensortec/BMM150-Sensor-API/blob/a20641f216057f0c54de115fe81b57368e119c01/bmm150.c#L1199 for more information.
        /// </summary>
        /// <returns>Trim registers value.</returns>
        private Bmm150TrimRegisterData ReadTrimRegisters()
        {
            SpanByte trimX1y1Data = new byte[2];
            SpanByte trimXyzData = new byte[4];
            SpanByte trimXy1xy2Data = new byte[10];

            // Read trim extended registers
            ReadBytes(Register.BMM150_DIG_X1, trimX1y1Data);
            ReadBytes(Register.BMM150_DIG_Z4_LSB, trimXyzData);
            ReadBytes(Register.BMM150_DIG_Z2_LSB, trimXy1xy2Data);

            return new Bmm150TrimRegisterData(trimX1y1Data, trimXyzData, trimXy1xy2Data);
        }

        /// <summary>
        /// Starts the Bmm150 init sequence.
        /// </summary>
        private void Initialize()
        {
            // Set Sleep mode
            WriteRegister(Register.POWER_CONTROL_ADDR, 0x01);
            Wait(6);

            // Check for a valid chip ID
            if (!IsVersionCorrect)
            {
                throw new IOException($"This device does not contain the correct signature (0x32) for a Bmm150");
            }

            // Set operation mode to: "Normal Mode"
            WriteRegister(Register.OP_MODE_ADDR, 0x00);
        }

        /// <summary>
        /// Get the device information.
        /// </summary>
        /// <returns>The device information.</returns>
        public byte GetDeviceInfo() => ReadByte(Register.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.
        /// Visit https://platformio.org/lib/show/12697/M5_BMM150 for more information.
        /// </summary>
        /// <param name="numberOfMeasurements">Number of measurement for the calibration, default is 100.</param>
        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>
        public bool HasDataToRead => (ReadByte(Register.DATA_READY_STATUS) & 0x01) == 0x01;

        /// <summary>
        /// Check if the version is the correct one (0x32). This is fixed for this device.
        /// </summary>
        /// <returns>Returns true if the version match.</returns>
        public bool IsVersionCorrect => ReadByte(Register.WIA) == 0x32;

        /// <summary>
        /// Read the magnetometer without Bias correction and can wait for new data to be present.
        /// </summary>
        /// <param name="waitForData"><see langword="true"/> to wait for new data.</param>
        /// <returns>The data from the magnetometer.</returns>
        public Vector3 ReadMagnetometerWithoutCorrection(bool waitForData = true) => ReadMagnetometerWithoutCorrection(waitForData, DefaultTimeout);

        /// <summary>
        /// Read the magnetometer without Bias correction and can wait for new data to be present
        /// Visit https://github.com/BoschSensortec/BMM150-Sensor-API/blob/a20641f216057f0c54de115fe81b57368e119c01/bmm150.c#L921 for more information.
        /// </summary>
        /// <param name="waitForData"><see langword="true"/> to wait for new data.</param>
        /// <param name="timeout">Timeout for waiting the data, ignored if <paramref name="waitForData"/> is <see langword="false"/>.</param>
        /// <returns>The data from the magnetometer.</returns>
        public Vector3 ReadMagnetometerWithoutCorrection(bool waitForData, TimeSpan timeout)
        {
            SpanByte rawData = new byte[8];

            // Wait for a data to be present
            if (waitForData)
            {
                DateTime dt = DateTime.UtcNow.Add(timeout);
                while (!HasDataToRead)
                {
                    if (DateTime.UtcNow > dt)
                    {
                        throw new TimeoutException($"{nameof(ReadMagnetometerWithoutCorrection)} timeout reading value");
                    }
                }
            }

            ReadBytes(Register.HXL, rawData);

            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)
            {
                magnetoRaw.X = -magnetoRaw.X;
            }

            // 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;
            }

            // 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;
            }

            _rHall = (uint)(rawData[7] << 6 | rawData[6] >> 2);

            return magnetoRaw;
        }

        /// <summary>
        /// Read the magnetometer with bias correction and can wait for new data to be present.
        /// </summary>
        /// <param name="waitForData"><see langword="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);

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

            magn.X = Bmm150Compensation.CompensateX(magn.X - CalibrationCompensation.X, _rHall, _trimData);
            magn.Y = Bmm150Compensation.CompensateY(magn.Y - CalibrationCompensation.Y, _rHall, _trimData);
            magn.Z = Bmm150Compensation.CompensateZ(magn.Z - CalibrationCompensation.Z, _rHall, _trimData);

            return magn;
        }

        private void WriteRegister(Register reg, byte data) => _bmm150Interface.WriteRegister(_i2cDevice, (byte)reg, data);

        private byte ReadByte(Register reg) => _bmm150Interface.ReadByte(_i2cDevice, (byte)reg);

        private void ReadBytes(Register reg, SpanByte readBytes) => _bmm150Interface.ReadBytes(_i2cDevice, (byte)reg, readBytes);

        private void Wait(int milisecondsTimeout)
        {
            Thread.Sleep(milisecondsTimeout);
        }

        /// <summary>
        /// <inheritdoc/>
        /// </summary>
        public void Dispose()
        {
            if (_shouldDispose)
            {
                _i2cDevice?.Dispose();
                _i2cDevice = null;
            }
        }
    }
}