// Licensed to the .NET Foundation under one or more agreements.
// The .NET Foundation licenses this file to you under the MIT license.
// See the LICENSE file in the project root for more information.
using System;
using System.Buffers.Binary;
using System.Device.I2c.Devices;
using System.Numerics;
namespace Iot.Device.Hmc5883l
{
///
/// 3-Axis Digital Compass HMC5883L
///
public class Hmc5883l : IDisposable
{
///
/// HMC5883L Default I2C Address
///
public const byte DefaultI2cAddress = 0x1E;
private I2cDevice _sensor;
private readonly byte _measuringMode;
private readonly byte _outputRate;
private readonly byte _gain;
private readonly byte _samplesAmount;
private readonly byte _measurementConfig;
///
/// HMC5883L Direction Vector
///
public Vector3 DirectionVector => ReadDirectionVector();
///
/// HMC5883L Heading (DEG)
///
public double Heading => VectorToHeading(ReadDirectionVector());
///
/// HMC5883L Status
///
public Status DeviceStatus => GetStatus();
///
/// Initialize a new HMC5883L device connected through I2C
///
/// I2C Device, like UnixI2cDevice or Windows10I2cDevice
/// Gain Setting
/// The Mode of Measuring
/// Typical Data Output Rate (Hz)
/// Number of samples averaged per measurement output
/// Measurement configuration
public Hmc5883l(
I2cDevice sensor,
Gain gain = Gain.Gain1090,
MeasuringMode measuringMode = MeasuringMode.Continuous,
OutputRate outputRate = OutputRate.Rate15,
SamplesAmount samplesAmount = SamplesAmount.One,
MeasurementConfiguration measurementConfig = MeasurementConfiguration.Normal)
{
_sensor = sensor;
_gain = (byte)gain;
_measuringMode = (byte)measuringMode;
_outputRate = (byte)outputRate;
_samplesAmount = (byte)samplesAmount;
_measurementConfig = (byte)measurementConfig;
Initialize();
}
///
/// Initialize the sensor
///
private void Initialize()
{
// Details in Datasheet P12
byte configA = (byte)(_samplesAmount | (_outputRate << 2) | _measurementConfig);
byte configB = (byte)(_gain << 5);
Span commandA = stackalloc byte[] { (byte)Register.HMC_CONFIG_REG_A_ADDR, configA };
Span commandB = stackalloc byte[] { (byte)Register.HMC_CONFIG_REG_B_ADDR, configB };
Span commandMode = stackalloc byte[] { (byte)Register.HMC_MODE_REG_ADDR, _measuringMode };
_sensor.Write(commandA);
_sensor.Write(commandB);
_sensor.Write(commandMode);
}
///
/// Read raw data from HMC5883L
///
/// Raw Data
private Vector3 ReadDirectionVector()
{
Span xRead = stackalloc byte[2];
Span yRead = stackalloc byte[2];
Span zRead = stackalloc byte[2];
_sensor.WriteByte((byte)Register.HMC_X_MSB_REG_ADDR);
_sensor.Read(xRead);
_sensor.WriteByte((byte)Register.HMC_Y_MSB_REG_ADDR);
_sensor.Read(yRead);
_sensor.WriteByte((byte)Register.HMC_Z_MSB_REG_ADDR);
_sensor.Read(zRead);
short x = BinaryPrimitives.ReadInt16BigEndian(xRead);
short y = BinaryPrimitives.ReadInt16BigEndian(yRead);
short z = BinaryPrimitives.ReadInt16BigEndian(zRead);
return new Vector3(x, y, z);
}
///
/// Calculate heading
///
/// HMC5883L Direction Vector
/// Heading (DEG)
private double VectorToHeading(Vector3 vector)
{
double deg = Math.Atan2(vector.Y, vector.X) * 180 / Math.PI;
if (deg < 0)
deg += 360;
return deg;
}
///
/// Cleanup
///
public void Dispose()
{
if (_sensor != null)
{
_sensor.Dispose();
_sensor = null;
}
}
///
/// Reads device statuses.
///
/// Device statuses
private Status GetStatus()
{
_sensor.WriteByte((byte)Register.HMC_STATUS_REG_ADDR);
byte status = _sensor.ReadByte();
return (Status)status;
}
}
}