Skip to content

Commit 6239252

Browse files
bparks13bparks13
committed
Refactor PolledBno to use Thread class, increase sample rate to 100 Hz
1 parent 19f0984 commit 6239252

File tree

2 files changed

+88
-38
lines changed

2 files changed

+88
-38
lines changed

‎Source/Devices/PolledBno055.cpp‎

Lines changed: 76 additions & 32 deletions
Original file line numberDiff line numberDiff line change
@@ -26,7 +26,8 @@ using namespace OnixSourcePlugin;
2626

2727
PolledBno055::PolledBno055(std::string name, std::string hubName, const oni_dev_idx_t deviceIdx_, std::shared_ptr<Onix1> ctx)
2828
: OnixDevice(name, hubName, PolledBno055::getDeviceType(), deviceIdx_, ctx, true),
29-
I2CRegisterContext(Bno055Address, deviceIdx_, ctx)
29+
I2CRegisterContext(Bno055Address, deviceIdx_, ctx),
30+
Thread("Polled BNO: " + name)
3031
{
3132
auto streamIdentifier = getStreamIdentifier();
3233

@@ -132,7 +133,7 @@ PolledBno055::PolledBno055(std::string name, std::string hubName, const oni_dev_
132133

133134
PolledBno055::~PolledBno055()
134135
{
135-
stopTimer();
136+
stopThread(500);
136137
}
137138

138139
OnixDeviceType PolledBno055::getDeviceType()
@@ -189,16 +190,36 @@ void PolledBno055::startAcquisition()
189190
sampleNumber = 0;
190191
currentFrame = 0;
191192

193+
previousTime = std::chrono::steady_clock::now();
194+
192195
if (isEnabled())
193-
startTimer(timerIntervalInMilliseconds);
196+
startThread();
194197
}
195198

196-
void PolledBno055::stopAcquisition()
199+
void PolledBno055::run()
197200
{
198-
stopTimer();
201+
while (!threadShouldExit())
202+
{
203+
time_point now = std::chrono::steady_clock::now();
204+
205+
std::chrono::milliseconds dur = std::chrono::duration_cast<std::chrono::milliseconds>(now - previousTime);
206+
207+
// NB: If the interval has not passed yet, wait for the remaining duration
208+
if (dur < TimerIntervalInMilliseconds)
209+
{
210+
std::this_thread::sleep_for(TimerIntervalInMilliseconds - dur);
211+
}
199212

200-
while (isTimerRunning())
201-
std::this_thread::sleep_for(std::chrono::milliseconds(10));
213+
pollFrame();
214+
215+
previousTime += TimerIntervalInMilliseconds;
216+
}
217+
}
218+
219+
void PolledBno055::stopAcquisition()
220+
{
221+
if (isThreadRunning())
222+
stopThread(500);
202223
}
203224

204225
void PolledBno055::addFrame(oni_frame_t* frame)
@@ -212,53 +233,65 @@ void PolledBno055::addSourceBuffers(OwnedArray<DataBuffer>& sourceBuffers)
212233
bnoBuffer = sourceBuffers.getLast();
213234
}
214235

215-
int16_t PolledBno055::readInt16(uint32_t startAddress)
236+
void PolledBno055::processFrames()
216237
{
217-
uint32_t value = 0;
218-
int rc = ReadWord(startAddress, 2, &value);
219-
220-
if (rc != ONI_ESUCCESS)
221-
return 0;
238+
}
222239

223-
return static_cast<int16_t>(value);
240+
int16_t PolledBno055::getInt16FromUint32(uint32_t value, bool getLowerValue)
241+
{
242+
return getLowerValue ?
243+
static_cast<int16_t>(value & 0xFFFFu) :
244+
static_cast<int16_t>((value & 0xFFFF0000u) >> 16);
224245
}
225246

226-
void PolledBno055::hiResTimerCallback()
247+
void PolledBno055::pollFrame()
227248
{
228249
size_t offset = 0;
250+
uint32_t value;
251+
229252

230253
// Euler
231-
bnoSamples[offset++ * NumFrames + currentFrame] = readInt16(EulerHeadingLsbAddress) * EulerAngleScale;
232-
bnoSamples[offset++ * NumFrames + currentFrame] = readInt16(EulerHeadingLsbAddress + 2) * EulerAngleScale;
233-
bnoSamples[offset++ * NumFrames + currentFrame] = readInt16(EulerHeadingLsbAddress + 4) * EulerAngleScale;
254+
ReadWord(EulerHeadingLsbAddress, 4, &value);
255+
bnoSamples[offset++ * NumFrames + currentFrame] = getInt16FromUint32(value, true) * EulerAngleScale;
256+
bnoSamples[offset++ * NumFrames + currentFrame] = getInt16FromUint32(value, false) * EulerAngleScale;
257+
258+
ReadWord(EulerHeadingLsbAddress + 4, 4, &value);
259+
bnoSamples[offset++ * NumFrames + currentFrame] = getInt16FromUint32(value, true) * EulerAngleScale;
234260

235261
// Quaternion
236-
bnoSamples[offset++ * NumFrames + currentFrame] = readInt16(EulerHeadingLsbAddress + 6) * QuaternionScale;
237-
bnoSamples[offset++ * NumFrames + currentFrame] = readInt16(EulerHeadingLsbAddress + 8) * QuaternionScale;
238-
bnoSamples[offset++ * NumFrames + currentFrame] = readInt16(EulerHeadingLsbAddress + 10) * QuaternionScale;
239-
bnoSamples[offset++ * NumFrames + currentFrame] = readInt16(EulerHeadingLsbAddress + 12) * QuaternionScale;
262+
bnoSamples[offset++ * NumFrames + currentFrame] = getInt16FromUint32(value, false) * QuaternionScale;
263+
264+
ReadWord(EulerHeadingLsbAddress + 8, 4, &value);
265+
bnoSamples[offset++ * NumFrames + currentFrame] = getInt16FromUint32(value, true) * QuaternionScale;
266+
bnoSamples[offset++ * NumFrames + currentFrame] = getInt16FromUint32(value, false) * QuaternionScale;
267+
268+
ReadWord(EulerHeadingLsbAddress + 12, 4, &value);
269+
bnoSamples[offset++ * NumFrames + currentFrame] = getInt16FromUint32(value, true) * QuaternionScale;
240270

241271
// Acceleration
242272

243-
bnoSamples[offset++ * NumFrames + currentFrame] = readInt16(EulerHeadingLsbAddress + 14) * AccelerationScale;
244-
bnoSamples[offset++ * NumFrames + currentFrame] = readInt16(EulerHeadingLsbAddress + 16) * AccelerationScale;
245-
bnoSamples[offset++ * NumFrames + currentFrame] = readInt16(EulerHeadingLsbAddress + 18) * AccelerationScale;
273+
bnoSamples[offset++ * NumFrames + currentFrame] = getInt16FromUint32(value, false) * AccelerationScale;
274+
275+
ReadWord(EulerHeadingLsbAddress + 16, 4, &value);
276+
bnoSamples[offset++ * NumFrames + currentFrame] = getInt16FromUint32(value, true) * AccelerationScale;
277+
bnoSamples[offset++ * NumFrames + currentFrame] = getInt16FromUint32(value, false) * AccelerationScale;
246278

247279
// Gravity
248280

249-
bnoSamples[offset++ * NumFrames + currentFrame] = readInt16(EulerHeadingLsbAddress + 20) * AccelerationScale;
250-
bnoSamples[offset++ * NumFrames + currentFrame] = readInt16(EulerHeadingLsbAddress + 22) * AccelerationScale;
251-
bnoSamples[offset++ * NumFrames + currentFrame] = readInt16(EulerHeadingLsbAddress + 24) * AccelerationScale;
281+
ReadWord(EulerHeadingLsbAddress + 20, 4, &value);
282+
bnoSamples[offset++ * NumFrames + currentFrame] = getInt16FromUint32(value, true) * AccelerationScale;
283+
bnoSamples[offset++ * NumFrames + currentFrame] = getInt16FromUint32(value, false) * AccelerationScale;
284+
285+
ReadWord(EulerHeadingLsbAddress + 24, 4, &value);
286+
bnoSamples[offset++ * NumFrames + currentFrame] = getInt16FromUint32(value, true) * AccelerationScale;
252287

253288
// Temperature
254289

255-
oni_reg_val_t byte;
256-
ReadByte(EulerHeadingLsbAddress + 26, &byte);
257-
bnoSamples[offset++ * NumFrames + currentFrame] = static_cast<uint8_t>(byte);
290+
bnoSamples[offset++ * NumFrames + currentFrame] = static_cast<uint8_t>((value & 0xFF0000u) >> 16);
258291

259292
// Calibration Status
260293

261-
ReadByte(EulerHeadingLsbAddress + 27, &byte);
294+
oni_reg_val_t byte = static_cast<uint8_t>((value & 0xFF000000u) >> 24);
262295

263296
constexpr uint8_t statusMask = 0b11;
264297

@@ -286,6 +319,17 @@ void PolledBno055::hiResTimerCallback()
286319
}
287320
}
288321

322+
int16_t PolledBno055::readInt16(uint32_t startAddress)
323+
{
324+
uint32_t value = 0;
325+
int rc = ReadWord(startAddress, 2, &value);
326+
327+
if (rc != ONI_ESUCCESS)
328+
return 0;
329+
330+
return static_cast<int16_t>(value);
331+
}
332+
289333
void PolledBno055::setBnoAxisMap(Bno055AxisMap map)
290334
{
291335
axisMap = map;

‎Source/Devices/PolledBno055.h‎

Lines changed: 12 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -33,7 +33,7 @@ namespace OnixSourcePlugin
3333
{
3434
class PolledBno055 : public OnixDevice,
3535
public I2CRegisterContext,
36-
public HighResolutionTimer
36+
public Thread
3737
{
3838
public:
3939

@@ -48,9 +48,12 @@ namespace OnixSourcePlugin
4848
void stopAcquisition() override;
4949
void addFrame(oni_frame_t*) override;
5050
void processFrames() override;
51+
void pollFrame();
5152
void addSourceBuffers(OwnedArray<DataBuffer>& sourceBuffers) override;
5253

53-
void hiResTimerCallback() override;
54+
void run() override;
55+
56+
static OnixDeviceType getDeviceType();
5457

5558
enum class Bno055AxisMap : uint32_t
5659
{
@@ -73,10 +76,10 @@ namespace OnixSourcePlugin
7376
void setBnoAxisMap(Bno055AxisMap map);
7477
void setBnoAxisSign(uint32_t sign);
7578

76-
static OnixDeviceType getDeviceType();
77-
7879
private:
7980

81+
using time_point = std::chrono::time_point<std::chrono::steady_clock>;
82+
8083
DataBuffer* bnoBuffer;
8184

8285
static constexpr int Bno055Address = 0x28;
@@ -96,9 +99,9 @@ namespace OnixSourcePlugin
9699
uint32_t axisSign = (uint32_t)Bno055AxisSign::Default; // NB: Holds the uint value of the flag. Allows for combinations of X/Y/Z to combined together
97100

98101
static constexpr int NumberOfChannels = 3 + 3 + 4 + 3 + 1 + 4;
99-
static constexpr double SampleRate = 30.0;
102+
static constexpr double SampleRate = 100.0;
100103

101-
static constexpr int TimerIntervalInMilliseconds = (int)(1e3 * (1 / SampleRate));
104+
static constexpr std::chrono::milliseconds TimerIntervalInMilliseconds = std::chrono::milliseconds((int)(1e3 * (1 / SampleRate)));
102105

103106
static constexpr int NumFrames = 2;
104107

@@ -111,7 +114,10 @@ namespace OnixSourcePlugin
111114
unsigned short currentFrame = 0;
112115
int sampleNumber = 0;
113116

117+
time_point previousTime;
118+
114119
int16_t readInt16(uint32_t);
120+
static int16_t getInt16FromUint32(uint32_t, bool);
115121

116122
JUCE_LEAK_DETECTOR(PolledBno055);
117123
};

0 commit comments

Comments
 (0)