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