Skip to content

Commit 84749a5

Browse files
sfe-SparkFrosfe-SparkFro
committed
Add standard deviation example
1 parent 49503bc commit 84749a5

File tree

3 files changed

+175
-3
lines changed

3 files changed

+175
-3
lines changed

‎examples/Example6_StandardDeviation/Example6_StandardDeviation.ino‎

Lines changed: 110 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,110 @@
1+
#include "SparkFun_Qwiic_OTOS_Arduino_Library.h"
2+
#include "Wire.h"
3+
4+
// Create an Optical Tracking Odometry Sensor object
5+
QwiicOTOS myOtos;
6+
7+
void setup()
8+
{
9+
// Start serial
10+
Serial.begin(115200);
11+
Serial.println("Qwiic OTOS Example 5 - Velocity and Acceleration");
12+
13+
Wire.begin();
14+
15+
// Attempt to begin the sensor
16+
while (myOtos.begin() == false)
17+
{
18+
Serial.println("OTOS not connected, check your wiring and I2C address!");
19+
delay(1000);
20+
}
21+
22+
Serial.println("OTOS connected!");
23+
24+
// Reset the tracking algorithm, making the sensor report it's at the origin
25+
myOtos.resetTracking();
26+
}
27+
28+
void loop()
29+
{
30+
// Create structs for position and standard deviations. This example can be
31+
// extended to include velocity and acceleration, which have been omitted
32+
// for simplicity, but can be added by uncommenting the code below
33+
otos_pose2d_t pos;
34+
// otos_pose2d_t vel;
35+
// otos_pose2d_t acc;
36+
otos_pose2d_t posStdDev;
37+
// otos_pose2d_t velStdDev;
38+
// otos_pose2d_t accStdDev;
39+
40+
// Read the position like normal (and velocity and acceleration if desired)
41+
myOtos.getPosition(pos);
42+
// myOtos.getVelocity(vel);
43+
// myOtos.getAccerlation(acc);
44+
45+
// Read the standard deviation of the tracking. Note that these values are
46+
// just the square root of the diagonal elements of the covariance matrices
47+
// of the Kalman filters used in the firmware of the OTOS, and THEY DO NOT
48+
// REPRESENT THE ACTUAL TRACKING ERROR! These values are provided primarily
49+
// for anyone wanting to do sensor fusion with additional sensors, but they
50+
// can be used to at least get an idea of the quality of the tracking.
51+
myOtos.getPositionStdDev(posStdDev);
52+
// myOtos.getVelocityStdDev(velStdDev);
53+
// myOtos.getAccerlationStdDev(accStdDev);
54+
55+
// These values can instead be read out in chunks:
56+
// myOtos.getPosVelAcc(pos, vel, acc);
57+
// myOtos.getPosVelAccStdDev(posStdDev, velStdDev, accStdDev);
58+
59+
// Or all at once:
60+
// myOtos.getPosVelAccAndStdDev(pos, vel, acc, posStdDev, velStdDev, accStdDev);
61+
62+
// Print measurements
63+
Serial.println();
64+
Serial.println("Sensor pose:");
65+
Serial.print("X (Inches): ");
66+
Serial.print(pos.x);
67+
Serial.print(" +/- ");
68+
Serial.println(posStdDev.x);
69+
Serial.print("Y (Inches): ");
70+
Serial.print(pos.y);
71+
Serial.print(" +/- ");
72+
Serial.println(posStdDev.y);
73+
Serial.print("Heading (Degrees): ");
74+
Serial.print(pos.h);
75+
Serial.print(" +/- ");
76+
Serial.println(posStdDev.h);
77+
78+
// Serial.println();
79+
// Serial.println("Sensor velocity:");
80+
// Serial.print("X (Inches/sec): ");
81+
// Serial.print(vel.x);
82+
// Serial.print(" +/- ");
83+
// Serial.println(velStdDev.x);
84+
// Serial.print("Y (Inches/sec): ");
85+
// Serial.print(vel.y);
86+
// Serial.print(" +/- ");
87+
// Serial.println(velStdDev.y);
88+
// Serial.print("Heading (Degrees/sec): ");
89+
// Serial.print(vel.h);
90+
// Serial.print(" +/- ");
91+
// Serial.println(velStdDev.h);
92+
93+
// Serial.println();
94+
// Serial.println("Sensor acceleration:");
95+
// Serial.print("X (Inches/sec^2): ");
96+
// Serial.print(acc.x);
97+
// Serial.print(" +/- ");
98+
// Serial.println(accStdDev.x);
99+
// Serial.print("Y (Inches/sec^2): ");
100+
// Serial.print(acc.y);
101+
// Serial.print(" +/- ");
102+
// Serial.println(accStdDev.y);
103+
// Serial.print("Heading (Degrees/sec^2): ");
104+
// Serial.print(acc.h);
105+
// Serial.print(" +/- ");
106+
// Serial.println(accStdDev.h);
107+
108+
// Wait a bit so we don't spam the serial port
109+
delay(500);
110+
}

‎src/sfeQwiicOtos.cpp‎

Lines changed: 63 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -230,9 +230,24 @@ sfeTkError_t sfeQwiicOtos::getAccerlation(otos_pose2d_t &pose)
230230
return readPoseRegs(kOtosRegAccXL, pose, kInt16ToMeter, kInt16ToRpss);
231231
}
232232

233+
sfeTkError_t sfeQwiicOtos::getPositionStdDev(otos_pose2d_t &pose)
234+
{
235+
return readPoseRegs(kOtosRegPosStdXL, pose, kInt16ToMeter, kInt16ToRad);
236+
}
237+
238+
sfeTkError_t sfeQwiicOtos::getVelocityStdDev(otos_pose2d_t &pose)
239+
{
240+
return readPoseRegs(kOtosRegVelStdXL, pose, kInt16ToMeter, kInt16ToRps);
241+
}
242+
243+
sfeTkError_t sfeQwiicOtos::getAccerlationStdDev(otos_pose2d_t &pose)
244+
{
245+
return readPoseRegs(kOtosRegAccStdXL, pose, kInt16ToMeter, kInt16ToRpss);
246+
}
247+
233248
sfeTkError_t sfeQwiicOtos::getPosVelAcc(otos_pose2d_t &pos, otos_pose2d_t &vel, otos_pose2d_t &acc)
234249
{
235-
// Read all three pose registers
250+
// Read all pose registers
236251
uint8_t rawData[18];
237252
size_t bytesRead = 0;
238253
sfeTkError_t err = _commBus->readRegisterRegion(kOtosRegPosXL, rawData, 18, bytesRead);
@@ -252,6 +267,53 @@ sfeTkError_t sfeQwiicOtos::getPosVelAcc(otos_pose2d_t &pos, otos_pose2d_t &vel,
252267
return kSTkErrOk;
253268
}
254269

270+
sfeTkError_t sfeQwiicOtos::getPosVelAccStdDev(otos_pose2d_t &pos, otos_pose2d_t &vel, otos_pose2d_t &acc)
271+
{
272+
// Read all pose registers
273+
uint8_t rawData[18];
274+
size_t bytesRead = 0;
275+
sfeTkError_t err = _commBus->readRegisterRegion(kOtosRegPosStdXL, rawData, 18, bytesRead);
276+
if(err != kSTkErrOk)
277+
return err;
278+
279+
// Check if we read the correct number of bytes
280+
if(bytesRead != 18)
281+
return kSTkErrFail;
282+
283+
// Convert raw data to pose units
284+
regsToPose(rawData, pos, kInt16ToMeter, kInt16ToRad);
285+
regsToPose(rawData + 6, vel, kInt16ToMeter, kInt16ToRps);
286+
regsToPose(rawData + 12, acc, kInt16ToMeter, kInt16ToRpss);
287+
288+
// Done!
289+
return kSTkErrOk;
290+
}
291+
292+
sfeTkError_t sfeQwiicOtos::getPosVelAccAndStdDev(otos_pose2d_t &pos, otos_pose2d_t &vel, otos_pose2d_t &acc, otos_pose2d_t &posStdDev, otos_pose2d_t &velStdDev, otos_pose2d_t &accStdDev)
293+
{
294+
// Read all pose registers
295+
uint8_t rawData[36];
296+
size_t bytesRead = 0;
297+
sfeTkError_t err = _commBus->readRegisterRegion(kOtosRegPosXL, rawData, 36, bytesRead);
298+
if(err != kSTkErrOk)
299+
return err;
300+
301+
// Check if we read the correct number of bytes
302+
if(bytesRead != 36)
303+
return kSTkErrFail;
304+
305+
// Convert raw data to pose units
306+
regsToPose(rawData, pos, kInt16ToMeter, kInt16ToRad);
307+
regsToPose(rawData + 6, vel, kInt16ToMeter, kInt16ToRps);
308+
regsToPose(rawData + 12, acc, kInt16ToMeter, kInt16ToRpss);
309+
regsToPose(rawData + 18, posStdDev, kInt16ToMeter, kInt16ToRad);
310+
regsToPose(rawData + 24, velStdDev, kInt16ToMeter, kInt16ToRps);
311+
regsToPose(rawData + 30, accStdDev, kInt16ToMeter, kInt16ToRpss);
312+
313+
// Done!
314+
return kSTkErrOk;
315+
}
316+
255317
sfeTkError_t sfeQwiicOtos::readPoseRegs(uint8_t reg, otos_pose2d_t &pose, float rawToXY, float rawToH)
256318
{
257319
size_t bytesRead = 0;

‎src/sfeQwiicOtos.h‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -175,14 +175,14 @@ class sfeQwiicOtos
175175

176176
sfeTkError_t getAccerlation(otos_pose2d_t &pose);
177177

178-
sfeTkError_t getPosVelAcc(otos_pose2d_t &pos, otos_pose2d_t &vel, otos_pose2d_t &acc);
179-
180178
sfeTkError_t getPositionStdDev(otos_pose2d_t &pose);
181179

182180
sfeTkError_t getVelocityStdDev(otos_pose2d_t &pose);
183181

184182
sfeTkError_t getAccerlationStdDev(otos_pose2d_t &pose);
185183

184+
sfeTkError_t getPosVelAcc(otos_pose2d_t &pos, otos_pose2d_t &vel, otos_pose2d_t &acc);
185+
186186
sfeTkError_t getPosVelAccStdDev(otos_pose2d_t &pos, otos_pose2d_t &vel, otos_pose2d_t &acc);
187187

188188
sfeTkError_t getPosVelAccAndStdDev(otos_pose2d_t &pos, otos_pose2d_t &vel, otos_pose2d_t &acc, otos_pose2d_t &posStdDev, otos_pose2d_t &velStdDev, otos_pose2d_t &accStdDev);

0 commit comments

Comments
 (0)