Skip to content

Commit fd037c1

Browse files
aacuevasjonnew
aacuevas
authored and
jonnew
committed
Add support for i2c multibyte access and make polledbno use it
1 parent ddfb86b commit fd037c1

File tree

2 files changed

+64
-34
lines changed

2 files changed

+64
-34
lines changed

‎OpenEphys.Onix1/I2CRegisterContext.cs‎

Lines changed: 23 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -18,21 +18,41 @@ public I2CRegisterContext(DeviceContext deviceContext, uint i2cAddress)
1818
device = deviceContext ?? throw new ArgumentNullException(nameof(deviceContext));
1919
address = i2cAddress;
2020
}
21-
2221
public void WriteByte(uint address, uint value, bool sixteenBitAddress = false)
2322
{
23+
WriteWord(address, value, 1, sixteenBitAddress);
24+
}
25+
26+
public void WriteWord(uint address, uint value, uint numBytes, bool sixteenBitAddress = false)
27+
{
28+
if (numBytes < 1 || numBytes > 4) throw new ArgumentOutOfRangeException(nameof(numBytes));
2429
uint registerAddress = (address << 7) | (this.address & 0x7F);
2530
registerAddress |= sixteenBitAddress ? 0x80000000 : 0;
26-
device.WriteRegister(registerAddress, (byte)value);
31+
registerAddress |= (numBytes - 1) << 28;
32+
device.WriteRegister(registerAddress, value);
2733
}
2834

2935
public byte ReadByte(uint address, bool sixteenBitAddress = false)
3036
{
37+
return (byte)ReadWord(address, 1, sixteenBitAddress);
38+
}
39+
public uint ReadWord(uint address, uint numBytes, bool sixteenBitAddress = false)
40+
{
41+
if (numBytes < 1 || numBytes > 4) throw new ArgumentOutOfRangeException(nameof(numBytes));
3142
uint registerAddress = (address << 7) | (this.address & 0x7F);
3243
registerAddress |= sixteenBitAddress ? 0x80000000 : 0;
33-
return (byte)device.ReadRegister(registerAddress);
44+
registerAddress |= (numBytes - 1) << 28;
45+
return device.ReadRegister(registerAddress);
3446
}
3547

48+
public void ReadWord(uint address, uint numBytes, byte[] arr, int offset, bool sixteenBitAddress = false)
49+
{
50+
uint data = ReadWord(address, numBytes, sixteenBitAddress);
51+
for (int i = 0; i < numBytes; i++)
52+
{
53+
arr[offset + i] = (byte)(data >> (8 * i));
54+
}
55+
}
3656
public byte[] ReadBytes(uint address, int count, bool sixteenBitAddress = false)
3757
{
3858
var data = new byte[count];

‎OpenEphys.Onix1/PolledBno055Data.cs‎

Lines changed: 41 additions & 31 deletions
Original file line numberDiff line numberDiff line change
@@ -115,46 +115,56 @@ public unsafe IObservable<Bno055DataFrame> Generate<TSource>(IObservable<TSource
115115
registeredValues.Add(deviceName);
116116
}
117117

118+
// Preallocate the array to avoid unnecesary allocation each frame
119+
byte[] data = new byte[28];
118120
var s = source.SubscribeSafe(observer, _ =>
119121
{
120122
Bno055DataFrame frame = default;
121123
device.Context.EnsureContext(() =>
122124
{
123-
byte[] data = {
124-
polled.HasFlag(PolledBno055Registers.EulerAngle) ? i2c.ReadByte(PolledBno055.EulerHeadingLsbAddress + 0) : (byte)0,
125-
polled.HasFlag(PolledBno055Registers.EulerAngle) ? i2c.ReadByte(PolledBno055.EulerHeadingLsbAddress + 1) : (byte)0,
126-
polled.HasFlag(PolledBno055Registers.EulerAngle) ? i2c.ReadByte(PolledBno055.EulerHeadingLsbAddress + 2) : (byte)0,
127-
polled.HasFlag(PolledBno055Registers.EulerAngle) ? i2c.ReadByte(PolledBno055.EulerHeadingLsbAddress + 3) : (byte)0,
128-
polled.HasFlag(PolledBno055Registers.EulerAngle) ? i2c.ReadByte(PolledBno055.EulerHeadingLsbAddress + 4) : (byte)0,
129-
polled.HasFlag(PolledBno055Registers.EulerAngle) ? i2c.ReadByte(PolledBno055.EulerHeadingLsbAddress + 5) : (byte)0,
125+
if (polled.HasFlag(PolledBno055Registers.EulerAngle))
126+
{
127+
i2c.ReadWord(PolledBno055.EulerHeadingLsbAddress + 0, 4, data, 0);
128+
i2c.ReadWord(PolledBno055.EulerHeadingLsbAddress + 4, 2, data, 4);
129+
}
130+
else
131+
{
132+
Array.Clear(data, 0, 6);
133+
}
130134

131-
polled.HasFlag(PolledBno055Registers.Quaternion) ? i2c.ReadByte(PolledBno055.EulerHeadingLsbAddress + 6) : (byte)0,
132-
polled.HasFlag(PolledBno055Registers.Quaternion) ? i2c.ReadByte(PolledBno055.EulerHeadingLsbAddress + 7) : (byte)0,
133-
polled.HasFlag(PolledBno055Registers.Quaternion) ? i2c.ReadByte(PolledBno055.EulerHeadingLsbAddress + 8) : (byte)0,
134-
polled.HasFlag(PolledBno055Registers.Quaternion) ? i2c.ReadByte(PolledBno055.EulerHeadingLsbAddress + 9) : (byte)0,
135-
polled.HasFlag(PolledBno055Registers.Quaternion) ? i2c.ReadByte(PolledBno055.EulerHeadingLsbAddress + 10) : (byte)0,
136-
polled.HasFlag(PolledBno055Registers.Quaternion) ? i2c.ReadByte(PolledBno055.EulerHeadingLsbAddress + 11) : (byte)0,
137-
polled.HasFlag(PolledBno055Registers.Quaternion) ? i2c.ReadByte(PolledBno055.EulerHeadingLsbAddress + 12) : (byte)0,
138-
polled.HasFlag(PolledBno055Registers.Quaternion) ? i2c.ReadByte(PolledBno055.EulerHeadingLsbAddress + 13) : (byte)0,
135+
if (polled.HasFlag(PolledBno055Registers.Quaternion))
136+
{
137+
i2c.ReadWord(PolledBno055.EulerHeadingLsbAddress + 6, 4, data, 6);
138+
i2c.ReadWord(PolledBno055.EulerHeadingLsbAddress + 10, 4, data, 10);
139+
}
140+
else
141+
{
142+
Array.Clear(data, 6, 8);
143+
}
139144

140-
polled.HasFlag(PolledBno055Registers.Acceleration) ? i2c.ReadByte(PolledBno055.EulerHeadingLsbAddress + 14) : (byte)0,
141-
polled.HasFlag(PolledBno055Registers.Acceleration) ? i2c.ReadByte(PolledBno055.EulerHeadingLsbAddress + 15) : (byte)0,
142-
polled.HasFlag(PolledBno055Registers.Acceleration) ? i2c.ReadByte(PolledBno055.EulerHeadingLsbAddress + 16) : (byte)0,
143-
polled.HasFlag(PolledBno055Registers.Acceleration) ? i2c.ReadByte(PolledBno055.EulerHeadingLsbAddress + 17) : (byte)0,
144-
polled.HasFlag(PolledBno055Registers.Acceleration) ? i2c.ReadByte(PolledBno055.EulerHeadingLsbAddress + 18) : (byte)0,
145-
polled.HasFlag(PolledBno055Registers.Acceleration) ? i2c.ReadByte(PolledBno055.EulerHeadingLsbAddress + 19) : (byte)0,
145+
if (polled.HasFlag(PolledBno055Registers.Acceleration))
146+
{
147+
i2c.ReadWord(PolledBno055.EulerHeadingLsbAddress + 14, 4, data, 14);
148+
i2c.ReadWord(PolledBno055.EulerHeadingLsbAddress + 18, 2, data, 18);
149+
}
150+
else
151+
{
152+
Array.Clear(data, 0, 6);
153+
}
146154

147-
polled.HasFlag(PolledBno055Registers.Gravity) ? i2c.ReadByte(PolledBno055.EulerHeadingLsbAddress + 20) : (byte)0,
148-
polled.HasFlag(PolledBno055Registers.Gravity) ? i2c.ReadByte(PolledBno055.EulerHeadingLsbAddress + 21) : (byte)0,
149-
polled.HasFlag(PolledBno055Registers.Gravity) ? i2c.ReadByte(PolledBno055.EulerHeadingLsbAddress + 22) : (byte)0,
150-
polled.HasFlag(PolledBno055Registers.Gravity) ? i2c.ReadByte(PolledBno055.EulerHeadingLsbAddress + 23) : (byte)0,
151-
polled.HasFlag(PolledBno055Registers.Gravity) ? i2c.ReadByte(PolledBno055.EulerHeadingLsbAddress + 24) : (byte)0,
152-
polled.HasFlag(PolledBno055Registers.Gravity) ? i2c.ReadByte(PolledBno055.EulerHeadingLsbAddress + 25) : (byte)0,
155+
if (polled.HasFlag(PolledBno055Registers.Gravity))
156+
{
157+
i2c.ReadWord(PolledBno055.EulerHeadingLsbAddress + 20, 4, data, 20);
158+
i2c.ReadWord(PolledBno055.EulerHeadingLsbAddress + 24, 2, data, 24);
159+
}
160+
else
161+
{
162+
Array.Clear(data, 0, 6);
163+
}
153164

154-
polled.HasFlag(PolledBno055Registers.Temperature) ? i2c.ReadByte(PolledBno055.EulerHeadingLsbAddress + 26) : (byte)0,
155-
156-
polled.HasFlag(PolledBno055Registers.Calibration) ? i2c.ReadByte(PolledBno055.EulerHeadingLsbAddress + 27) : (byte)0,
157-
};
165+
//NOTE: actual performance would be a little better if we merged these two so they could be read together
166+
data[26] = polled.HasFlag(PolledBno055Registers.Temperature) ? i2c.ReadByte(PolledBno055.EulerHeadingLsbAddress + 26) : (byte)0;
167+
data[27] = polled.HasFlag(PolledBno055Registers.Calibration) ? i2c.ReadByte(PolledBno055.EulerHeadingLsbAddress + 27) : (byte)0;
158168

159169
ulong clock = passthrough.ReadRegister(DS90UB9x.LASTI2CL);
160170
clock += (ulong)passthrough.ReadRegister(DS90UB9x.LASTI2CH) << 32;

0 commit comments

Comments
 (0)