/// <summary>
/// Reads position registers and calculates euler angles from them
/// </summary>
/// <returns>Euler angles representing current position - yaw, roll, pitch</returns>
public float[] Read()
{
byte[] data = bno.ReadRegisters(0x1a, 6);
float[] result = new float[]
{
(short)((data[1] << 8) | data[0]) / representationInLSB, //heading
(short)((data[3] << 8) | data[2]) / representationInLSB, //roll
(short)((data[5] << 8) | data[4]) / representationInLSB //pitch
};
return(result);
}
public static SensorReading CreateFromDevice(I2cPeripheral device, SensorSettings sensorSettings)
{
// Read the current control register
var status = device.ReadRegister(RegisterAddresses.ControlTemperatureAndPressure.Address);
// Force a sample
status = BitHelpers.SetBit(status, 0x00, true);
device.WriteRegister(RegisterAddresses.ControlTemperatureAndPressure.Address, status);
// Wait for the sample to be taken.
do
{
status = device.ReadRegister(RegisterAddresses.ControlTemperatureAndPressure.Address);
} while (BitHelpers.GetBitValue(status, 0x00));
var sensorData = device.ReadRegisters(
RegisterAddresses.AllSensors.Address,
RegisterAddresses.AllSensors.Length)
.AsSpan();
var rawPressure = GetRawValue(sensorData.Slice(0, 3));
var rawTemperature = GetRawValue(sensorData.Slice(3, 3));
var rawHumidity = GetRawValue(sensorData.Slice(6, 2));
//var rawVoc = GetRawValue(sensorData.Slice(8, 2));
var compensationData1 = device.ReadRegisters(RegisterAddresses.CompensationData1.Address,
RegisterAddresses.CompensationData1.Length);
var compensationData2 = device.ReadRegisters(RegisterAddresses.CompensationData2.Address,
RegisterAddresses.CompensationData2.Length);
var compensationData = ArrayPool <byte> .Shared.Rent(64);
try {
Array.Copy(compensationData1, 0, compensationData, 0, compensationData1.Length);
Array.Copy(compensationData2, 0, compensationData, 25, compensationData2.Length);
var temp = RawToTemp(rawTemperature,
new TemperatureCompensation(compensationData));
var pressure = RawToPressure(temp, rawPressure,
new PressureCompensation(compensationData));
var humidity = RawToHumidity(temp, rawHumidity,
new HumidityCompensation(compensationData));
return(new SensorReading(pressure, temp, humidity, 0, sensorSettings));
} finally {
ArrayPool <byte> .Shared.Return(compensationData, true);
}
}
