Jamie Smith / Mbed OS BNO080-Examples

Example/test programs for my BNO080 driver.

Dependencies:   BNO080

BNO080 Driver Examples

These examples show how to use some of the functionality on my BNO080 driver. To get started with MBed CLI:

Build Instructions

$ hg clone https://MultipleMonomials@os.mbed.com/users/MultipleMonomials/code/BNO080-Examples/
$ cd BNO080-Examples
$ mbed deploy
$ mbed compile

BNOTestSuite.cpp@4:85b98cc04a0a, 2020-07-21 (annotated)

Committer:
Jamie Smith
Date:
Tue Jul 21 22:00:26 2020 -0700
Revision:
4:85b98cc04a0a
Parent:
3:f72d98d0095e
Child:
5:ee64252765de
Update to MBed OS 6, add accelerometer calibration test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Jamie Smith 0:82ee5f7adbb0 1 /*
Jamie Smith 0:82ee5f7adbb0 2 USC RPL HAMSTER v2.3
Jamie Smith 0:82ee5f7adbb0 3 Contributors: Lauren Potterat
Jamie Smith 0:82ee5f7adbb0 4 */
Jamie Smith 0:82ee5f7adbb0 5
Jamie Smith 0:82ee5f7adbb0 6
Jamie Smith 0:82ee5f7adbb0 7 #include "BNOTestSuite.h"
Jamie Smith 0:82ee5f7adbb0 8
MultipleMonomials 3:f72d98d0095e 9 #include <cinttypes>
MultipleMonomials 3:f72d98d0095e 10
Jamie Smith 0:82ee5f7adbb0 11 #define RAD_TO_DEG (180.0/M_PI)
Jamie Smith 0:82ee5f7adbb0 12
Jamie Smith 0:82ee5f7adbb0 13 void BNOTestSuite::test_printInfo()
Jamie Smith 0:82ee5f7adbb0 14 {
MultipleMonomials 3:f72d98d0095e 15 pc.printf("BNO080 reports as SW version %" PRIu8 ".%" PRIu8 ".%" PRIu16", build %" PRIu32 ", part no. %" PRIu32"\n",
Jamie Smith 0:82ee5f7adbb0 16 imu.majorSoftwareVersion, imu.minorSoftwareVersion, imu.patchSoftwareVersion,
Jamie Smith 0:82ee5f7adbb0 17 imu.buildNumber, imu.partNumber);
Jamie Smith 0:82ee5f7adbb0 18 }
Jamie Smith 0:82ee5f7adbb0 19
Jamie Smith 0:82ee5f7adbb0 20
Jamie Smith 0:82ee5f7adbb0 21 void BNOTestSuite::test_readRotationVector()
Jamie Smith 0:82ee5f7adbb0 22 {
Jamie Smith 0:82ee5f7adbb0 23 const size_t update_rate_ms = 200;
Jamie Smith 0:82ee5f7adbb0 24
Jamie Smith 0:82ee5f7adbb0 25 imu.enableReport(BNO080::ROTATION, update_rate_ms);
Jamie Smith 0:82ee5f7adbb0 26
Jamie Smith 0:82ee5f7adbb0 27 while (true)
Jamie Smith 0:82ee5f7adbb0 28 {
Jamie Smith 4:85b98cc04a0a 29 ThisThread::sleep_for(1ms * update_rate_ms);
Jamie Smith 0:82ee5f7adbb0 30
Jamie Smith 0:82ee5f7adbb0 31 if(imu.updateData())
Jamie Smith 0:82ee5f7adbb0 32 {
Jamie Smith 0:82ee5f7adbb0 33 TVector3 eulerRadians = imu.rotationVector.euler();
Jamie Smith 0:82ee5f7adbb0 34 TVector3 eulerDegrees = eulerRadians * RAD_TO_DEG;
Jamie Smith 0:82ee5f7adbb0 35
Jamie Smith 0:82ee5f7adbb0 36 pc.printf("IMU Rotation Euler: Roll: %f deg, Pitch: %f deg, Yaw: %f deg \r\n",
Jamie Smith 0:82ee5f7adbb0 37 eulerDegrees[0], eulerDegrees[1], eulerDegrees[2]);
Jamie Smith 0:82ee5f7adbb0 38
Jamie Smith 0:82ee5f7adbb0 39 pc.printf(", Accuracy: %.02f", (imu.rotationAccuracy * RAD_TO_DEG));
Jamie Smith 0:82ee5f7adbb0 40
Jamie Smith 0:82ee5f7adbb0 41 pc.printf(", Status: %s\n", imu.getReportStatusString(BNO080::ROTATION));
Jamie Smith 0:82ee5f7adbb0 42 }
Jamie Smith 0:82ee5f7adbb0 43 else
Jamie Smith 0:82ee5f7adbb0 44 {
Jamie Smith 0:82ee5f7adbb0 45 pc.printf("IMU was not ready with data packet!\r\n");
Jamie Smith 0:82ee5f7adbb0 46 }
Jamie Smith 0:82ee5f7adbb0 47 }
Jamie Smith 0:82ee5f7adbb0 48 }
Jamie Smith 0:82ee5f7adbb0 49
Jamie Smith 0:82ee5f7adbb0 50 void BNOTestSuite::test_readRotationAcceleration()
Jamie Smith 0:82ee5f7adbb0 51 {
Jamie Smith 0:82ee5f7adbb0 52 imu.enableReport(BNO080::ROTATION, 1000);
Jamie Smith 0:82ee5f7adbb0 53 imu.enableReport(BNO080::TOTAL_ACCELERATION, 500);
Jamie Smith 0:82ee5f7adbb0 54
Jamie Smith 0:82ee5f7adbb0 55 while (true)
Jamie Smith 0:82ee5f7adbb0 56 {
Jamie Smith 4:85b98cc04a0a 57 ThisThread::sleep_for(1ms);
Jamie Smith 0:82ee5f7adbb0 58
Jamie Smith 0:82ee5f7adbb0 59 if(imu.updateData())
Jamie Smith 0:82ee5f7adbb0 60 {
Jamie Smith 0:82ee5f7adbb0 61 if (imu.hasNewData(BNO080::ROTATION))
Jamie Smith 0:82ee5f7adbb0 62 {
Jamie Smith 0:82ee5f7adbb0 63 pc.printf("IMU Rotation Euler: ");
Jamie Smith 0:82ee5f7adbb0 64 TVector3 eulerRadians = imu.rotationVector.euler();
Jamie Smith 0:82ee5f7adbb0 65 TVector3 eulerDegrees = eulerRadians * (180.0 / M_PI);
Jamie Smith 0:82ee5f7adbb0 66 eulerDegrees.print(pc, true);
Jamie Smith 0:82ee5f7adbb0 67 pc.printf("\n");
Jamie Smith 0:82ee5f7adbb0 68 }
Jamie Smith 0:82ee5f7adbb0 69 if (imu.hasNewData(BNO080::TOTAL_ACCELERATION))
Jamie Smith 0:82ee5f7adbb0 70 {
Jamie Smith 0:82ee5f7adbb0 71 pc.printf("IMU Total Acceleration: ");
Jamie Smith 0:82ee5f7adbb0 72 imu.totalAcceleration.print(pc, true);
Jamie Smith 0:82ee5f7adbb0 73 pc.printf("\n");
Jamie Smith 0:82ee5f7adbb0 74 }
Jamie Smith 0:82ee5f7adbb0 75 }
Jamie Smith 0:82ee5f7adbb0 76 }
Jamie Smith 0:82ee5f7adbb0 77 }
Jamie Smith 0:82ee5f7adbb0 78
Jamie Smith 0:82ee5f7adbb0 79 void BNOTestSuite::test_tapDetector()
Jamie Smith 0:82ee5f7adbb0 80 {
Jamie Smith 0:82ee5f7adbb0 81 imu.enableReport(BNO080::TAP_DETECTOR, 10);
Jamie Smith 0:82ee5f7adbb0 82 pc.printf("Listening for taps...\n");
Jamie Smith 0:82ee5f7adbb0 83
Jamie Smith 0:82ee5f7adbb0 84 while(true)
Jamie Smith 0:82ee5f7adbb0 85 {
Jamie Smith 0:82ee5f7adbb0 86 if(imu.updateData())
Jamie Smith 0:82ee5f7adbb0 87 {
Jamie Smith 0:82ee5f7adbb0 88 if(imu.tapDetected)
Jamie Smith 0:82ee5f7adbb0 89 {
Jamie Smith 0:82ee5f7adbb0 90 pc.printf("Tap detected!\n");
Jamie Smith 0:82ee5f7adbb0 91 imu.tapDetected = false;
Jamie Smith 0:82ee5f7adbb0 92 }
Jamie Smith 0:82ee5f7adbb0 93 }
Jamie Smith 0:82ee5f7adbb0 94 }
Jamie Smith 0:82ee5f7adbb0 95 }
Jamie Smith 0:82ee5f7adbb0 96
Jamie Smith 0:82ee5f7adbb0 97 void BNOTestSuite::test_gameRotationVector()
Jamie Smith 0:82ee5f7adbb0 98 {
Jamie Smith 0:82ee5f7adbb0 99 const size_t update_rate_ms = 200;
Jamie Smith 0:82ee5f7adbb0 100
Jamie Smith 0:82ee5f7adbb0 101 imu.enableReport(BNO080::GAME_ROTATION, update_rate_ms);
Jamie Smith 0:82ee5f7adbb0 102
Jamie Smith 0:82ee5f7adbb0 103 while (true)
Jamie Smith 0:82ee5f7adbb0 104 {
Jamie Smith 4:85b98cc04a0a 105 ThisThread::sleep_for(1ms * update_rate_ms);
Jamie Smith 0:82ee5f7adbb0 106
Jamie Smith 0:82ee5f7adbb0 107 if(imu.updateData())
Jamie Smith 0:82ee5f7adbb0 108 {
Jamie Smith 0:82ee5f7adbb0 109 pc.printf("IMU Game Rotation Euler: [");
Jamie Smith 0:82ee5f7adbb0 110 TVector3 eulerRadians = imu.gameRotationVector.euler();
Jamie Smith 0:82ee5f7adbb0 111 TVector3 eulerDegrees = eulerRadians * RAD_TO_DEG;
Jamie Smith 0:82ee5f7adbb0 112 eulerDegrees.print(pc, true);
Jamie Smith 0:82ee5f7adbb0 113
Jamie Smith 0:82ee5f7adbb0 114 pc.printf("], Status: %s\n", imu.getReportStatusString(BNO080::GAME_ROTATION));
Jamie Smith 0:82ee5f7adbb0 115 }
Jamie Smith 0:82ee5f7adbb0 116 else
Jamie Smith 0:82ee5f7adbb0 117 {
Jamie Smith 0:82ee5f7adbb0 118 pc.printf("IMU was not ready with data packet!\r\n");
Jamie Smith 0:82ee5f7adbb0 119 }
Jamie Smith 0:82ee5f7adbb0 120 }
Jamie Smith 0:82ee5f7adbb0 121 }
Jamie Smith 0:82ee5f7adbb0 122
Jamie Smith 0:82ee5f7adbb0 123 void BNOTestSuite::test_tare()
Jamie Smith 0:82ee5f7adbb0 124 {
Jamie Smith 0:82ee5f7adbb0 125 const size_t update_rate_ms = 100;
Jamie Smith 0:82ee5f7adbb0 126 imu.enableReport(BNO080::ROTATION, update_rate_ms);
Jamie Smith 0:82ee5f7adbb0 127
Jamie Smith 0:82ee5f7adbb0 128 Timer runningTime;
Jamie Smith 0:82ee5f7adbb0 129 runningTime.start();
Jamie Smith 0:82ee5f7adbb0 130
Jamie Smith 0:82ee5f7adbb0 131 bool sentTareCmd = false;
Jamie Smith 0:82ee5f7adbb0 132
Jamie Smith 0:82ee5f7adbb0 133 while (true)
Jamie Smith 0:82ee5f7adbb0 134 {
Jamie Smith 4:85b98cc04a0a 135 ThisThread::sleep_for(1ms * update_rate_ms);
Jamie Smith 0:82ee5f7adbb0 136
Jamie Smith 0:82ee5f7adbb0 137 if(imu.updateData())
Jamie Smith 0:82ee5f7adbb0 138 {
Jamie Smith 0:82ee5f7adbb0 139 pc.printf("IMU Rotation Euler: [");
Jamie Smith 0:82ee5f7adbb0 140 TVector3 eulerRadians = imu.rotationVector.euler();
Jamie Smith 0:82ee5f7adbb0 141 TVector3 eulerDegrees = eulerRadians * RAD_TO_DEG;
Jamie Smith 0:82ee5f7adbb0 142 eulerDegrees.print(pc, true);
Jamie Smith 0:82ee5f7adbb0 143
Jamie Smith 0:82ee5f7adbb0 144 pc.printf("], Status: %s\n", imu.getReportStatusString(BNO080::ROTATION));
Jamie Smith 0:82ee5f7adbb0 145 }
Jamie Smith 0:82ee5f7adbb0 146 else
Jamie Smith 0:82ee5f7adbb0 147 {
Jamie Smith 0:82ee5f7adbb0 148 pc.printf("IMU was not ready with data packet!\r\n");
Jamie Smith 0:82ee5f7adbb0 149 }
Jamie Smith 0:82ee5f7adbb0 150
Jamie Smith 0:82ee5f7adbb0 151 if(runningTime.read() > 2 && !sentTareCmd)
Jamie Smith 0:82ee5f7adbb0 152 {
Jamie Smith 0:82ee5f7adbb0 153 pc.printf("2 seconds have passed, sending tare command...\n");
Jamie Smith 0:82ee5f7adbb0 154 imu.tare();
Jamie Smith 0:82ee5f7adbb0 155 sentTareCmd = true;
Jamie Smith 0:82ee5f7adbb0 156 }
Jamie Smith 0:82ee5f7adbb0 157 }
Jamie Smith 0:82ee5f7adbb0 158 }
Jamie Smith 0:82ee5f7adbb0 159
Jamie Smith 0:82ee5f7adbb0 160 void BNOTestSuite::test_magCalibration()
Jamie Smith 0:82ee5f7adbb0 161 {
Jamie Smith 0:82ee5f7adbb0 162 // enable calibration for the magnetometer
Jamie Smith 0:82ee5f7adbb0 163 bool success = imu.enableCalibration(false, false, true);
Jamie Smith 0:82ee5f7adbb0 164
Jamie Smith 0:82ee5f7adbb0 165 const size_t update_rate_ms = 200;
Jamie Smith 0:82ee5f7adbb0 166 imu.enableReport(BNO080::GAME_ROTATION, update_rate_ms);
Jamie Smith 0:82ee5f7adbb0 167 imu.enableReport(BNO080::MAG_FIELD, update_rate_ms);
Jamie Smith 0:82ee5f7adbb0 168 imu.enableReport(BNO080::MAG_FIELD_UNCALIBRATED, update_rate_ms);
Jamie Smith 0:82ee5f7adbb0 169
Jamie Smith 0:82ee5f7adbb0 170 if(success)
Jamie Smith 0:82ee5f7adbb0 171 {
Jamie Smith 0:82ee5f7adbb0 172 pc.printf("Calibration mode was successfully enabled!\n");
Jamie Smith 0:82ee5f7adbb0 173 }
Jamie Smith 0:82ee5f7adbb0 174 else
Jamie Smith 0:82ee5f7adbb0 175 {
Jamie Smith 0:82ee5f7adbb0 176 pc.printf("Calibration mode failed to enable!\n");
Jamie Smith 0:82ee5f7adbb0 177 while(true){}
Jamie Smith 0:82ee5f7adbb0 178 }
Jamie Smith 0:82ee5f7adbb0 179
Jamie Smith 0:82ee5f7adbb0 180 while (true)
Jamie Smith 0:82ee5f7adbb0 181 {
Jamie Smith 4:85b98cc04a0a 182 ThisThread::sleep_for(1ms * update_rate_ms);
Jamie Smith 0:82ee5f7adbb0 183
Jamie Smith 0:82ee5f7adbb0 184 if(imu.updateData())
Jamie Smith 0:82ee5f7adbb0 185 {
Jamie Smith 0:82ee5f7adbb0 186 pc.printf("IMU Magnetic Field: [");
Jamie Smith 0:82ee5f7adbb0 187 imu.magFieldUncalibrated.print(pc, true);
Jamie Smith 0:82ee5f7adbb0 188
Jamie Smith 0:82ee5f7adbb0 189 pc.printf("] uTesla, Status: %hhu (should be 2-3 when calibration is complete), ", imu.getReportStatus(BNO080::MAG_FIELD));
Jamie Smith 0:82ee5f7adbb0 190
Jamie Smith 0:82ee5f7adbb0 191 pc.printf("Hard iron offsets: [");
Jamie Smith 0:82ee5f7adbb0 192 imu.hardIronOffset.print(pc, true);
Jamie Smith 0:82ee5f7adbb0 193
Jamie Smith 0:82ee5f7adbb0 194 pc.printf("]\n");
Jamie Smith 0:82ee5f7adbb0 195
Jamie Smith 0:82ee5f7adbb0 196 }
Jamie Smith 0:82ee5f7adbb0 197 else
Jamie Smith 0:82ee5f7adbb0 198 {
Jamie Smith 0:82ee5f7adbb0 199 pc.printf("IMU was not ready with data packet!\r\n");
Jamie Smith 0:82ee5f7adbb0 200 }
Jamie Smith 0:82ee5f7adbb0 201 }
Jamie Smith 0:82ee5f7adbb0 202
Jamie Smith 0:82ee5f7adbb0 203 }
Jamie Smith 0:82ee5f7adbb0 204
Jamie Smith 0:82ee5f7adbb0 205 void BNOTestSuite::test_stabilityClassifier()
Jamie Smith 0:82ee5f7adbb0 206 {
Jamie Smith 0:82ee5f7adbb0 207 imu.enableReport(BNO080::STABILITY_CLASSIFIER, 200);
Jamie Smith 0:82ee5f7adbb0 208
Jamie Smith 0:82ee5f7adbb0 209 while (true)
Jamie Smith 0:82ee5f7adbb0 210 {
Jamie Smith 4:85b98cc04a0a 211 ThisThread::sleep_for(1ms);
Jamie Smith 0:82ee5f7adbb0 212
Jamie Smith 0:82ee5f7adbb0 213 if(imu.updateData())
Jamie Smith 0:82ee5f7adbb0 214 {
Jamie Smith 0:82ee5f7adbb0 215 if (imu.hasNewData(BNO080::STABILITY_CLASSIFIER))
Jamie Smith 0:82ee5f7adbb0 216 {
Jamie Smith 0:82ee5f7adbb0 217 pc.printf("Stability: ");
Jamie Smith 0:82ee5f7adbb0 218
Jamie Smith 0:82ee5f7adbb0 219 switch(imu.stability)
Jamie Smith 0:82ee5f7adbb0 220 {
Jamie Smith 0:82ee5f7adbb0 221 case BNO080::UNKNOWN:
Jamie Smith 0:82ee5f7adbb0 222 pc.printf("Unknown\n");
Jamie Smith 0:82ee5f7adbb0 223 break;
Jamie Smith 0:82ee5f7adbb0 224 case BNO080::ON_TABLE:
Jamie Smith 0:82ee5f7adbb0 225 pc.printf("On Table\n");
Jamie Smith 0:82ee5f7adbb0 226 break;
Jamie Smith 0:82ee5f7adbb0 227 case BNO080::STATIONARY:
Jamie Smith 0:82ee5f7adbb0 228 pc.printf("Stationary\n");
Jamie Smith 0:82ee5f7adbb0 229 break;
Jamie Smith 0:82ee5f7adbb0 230 case BNO080::STABLE:
Jamie Smith 0:82ee5f7adbb0 231 pc.printf("Stable\n");
Jamie Smith 0:82ee5f7adbb0 232 break;
Jamie Smith 0:82ee5f7adbb0 233 case BNO080::MOTION:
Jamie Smith 0:82ee5f7adbb0 234 pc.printf("Motion\n");
Jamie Smith 0:82ee5f7adbb0 235 break;
Jamie Smith 0:82ee5f7adbb0 236 }
Jamie Smith 0:82ee5f7adbb0 237 }
Jamie Smith 0:82ee5f7adbb0 238 }
Jamie Smith 0:82ee5f7adbb0 239 }
Jamie Smith 0:82ee5f7adbb0 240
Jamie Smith 0:82ee5f7adbb0 241 }
Jamie Smith 0:82ee5f7adbb0 242
Jamie Smith 0:82ee5f7adbb0 243 void BNOTestSuite::test_metadata()
Jamie Smith 0:82ee5f7adbb0 244 {
Jamie Smith 0:82ee5f7adbb0 245 pc.printf("Printing metadata for Linear Acceleration:\r\n");
Jamie Smith 0:82ee5f7adbb0 246 imu.printMetadataSummary(BNO080::LINEAR_ACCELERATION);
Jamie Smith 0:82ee5f7adbb0 247
Jamie Smith 0:82ee5f7adbb0 248 pc.printf("Printing metadata for Rotation Vector:\r\n");
Jamie Smith 0:82ee5f7adbb0 249 imu.printMetadataSummary(BNO080::ROTATION);
Jamie Smith 0:82ee5f7adbb0 250
Jamie Smith 0:82ee5f7adbb0 251 pc.printf("Printing metadata for Magnetic Field:\r\n");
Jamie Smith 0:82ee5f7adbb0 252 imu.printMetadataSummary(BNO080::MAG_FIELD);
Jamie Smith 0:82ee5f7adbb0 253
Jamie Smith 0:82ee5f7adbb0 254 pc.printf("Printing metadata for Tap Detector:\r\n");
Jamie Smith 0:82ee5f7adbb0 255 imu.printMetadataSummary(BNO080::TAP_DETECTOR);
Jamie Smith 0:82ee5f7adbb0 256
Jamie Smith 0:82ee5f7adbb0 257 }
Jamie Smith 0:82ee5f7adbb0 258
Jamie Smith 0:82ee5f7adbb0 259 void BNOTestSuite::test_orientation()
Jamie Smith 0:82ee5f7adbb0 260 {
Jamie Smith 0:82ee5f7adbb0 261 const size_t update_rate_ms = 200;
Jamie Smith 0:82ee5f7adbb0 262 imu.enableReport(BNO080::TOTAL_ACCELERATION, update_rate_ms);
Jamie Smith 0:82ee5f7adbb0 263 imu.enableReport(BNO080::ROTATION, update_rate_ms);
Jamie Smith 0:82ee5f7adbb0 264
Jamie Smith 0:82ee5f7adbb0 265 Timer runningTime;
Jamie Smith 0:82ee5f7adbb0 266 runningTime.start();
Jamie Smith 0:82ee5f7adbb0 267
Jamie Smith 0:82ee5f7adbb0 268 bool setOrientation = false;
Jamie Smith 0:82ee5f7adbb0 269
Jamie Smith 0:82ee5f7adbb0 270 while (true)
Jamie Smith 0:82ee5f7adbb0 271 {
Jamie Smith 4:85b98cc04a0a 272 ThisThread::sleep_for(1ms * update_rate_ms);
Jamie Smith 0:82ee5f7adbb0 273
Jamie Smith 0:82ee5f7adbb0 274 if(imu.updateData())
Jamie Smith 0:82ee5f7adbb0 275 {
Jamie Smith 0:82ee5f7adbb0 276 if (imu.hasNewData(BNO080::ROTATION))
Jamie Smith 0:82ee5f7adbb0 277 {
Jamie Smith 0:82ee5f7adbb0 278 pc.printf("IMU Rotation Euler: ");
Jamie Smith 0:82ee5f7adbb0 279 TVector3 eulerRadians = imu.rotationVector.euler();
Jamie Smith 0:82ee5f7adbb0 280 TVector3 eulerDegrees = eulerRadians * (180.0 / M_PI);
Jamie Smith 0:82ee5f7adbb0 281 eulerDegrees.print(pc, true);
Jamie Smith 0:82ee5f7adbb0 282 pc.printf("\n");
Jamie Smith 0:82ee5f7adbb0 283 }
Jamie Smith 0:82ee5f7adbb0 284 if (imu.hasNewData(BNO080::TOTAL_ACCELERATION))
Jamie Smith 0:82ee5f7adbb0 285 {
Jamie Smith 0:82ee5f7adbb0 286 pc.printf("IMU Total Acceleration: ");
Jamie Smith 0:82ee5f7adbb0 287 imu.totalAcceleration.print(pc, true);
Jamie Smith 0:82ee5f7adbb0 288 pc.printf("\n");
Jamie Smith 0:82ee5f7adbb0 289 }
Jamie Smith 0:82ee5f7adbb0 290 }
Jamie Smith 0:82ee5f7adbb0 291
Jamie Smith 0:82ee5f7adbb0 292 if(runningTime.read() > 2 && !setOrientation)
Jamie Smith 0:82ee5f7adbb0 293 {
Jamie Smith 0:82ee5f7adbb0 294 pc.printf("2 seconds have passed, sending set orientation command...\n");
Jamie Smith 0:82ee5f7adbb0 295 setOrientation = true;
Jamie Smith 0:82ee5f7adbb0 296
Jamie Smith 0:82ee5f7adbb0 297 // rotate sensor about the Y axis, so Z points down and X points west.
Jamie Smith 0:82ee5f7adbb0 298 // (values from BNO datasheet page 41)
Jamie Smith 0:82ee5f7adbb0 299 imu.setSensorOrientation(Quaternion(0, -1, 0, 0));
Jamie Smith 0:82ee5f7adbb0 300 }
Jamie Smith 0:82ee5f7adbb0 301 }
Jamie Smith 0:82ee5f7adbb0 302 }
Jamie Smith 0:82ee5f7adbb0 303
Jamie Smith 1:14c135ea7134 304 void BNOTestSuite::test_permanentOrientation()
Jamie Smith 1:14c135ea7134 305 {
Jamie Smith 1:14c135ea7134 306 pc.printf("Setting permanent sensor orientation...\n");
Jamie Smith 1:14c135ea7134 307 Timer orientationTimer;
Jamie Smith 1:14c135ea7134 308 orientationTimer.start();
Jamie Smith 1:14c135ea7134 309
Jamie Smith 1:14c135ea7134 310 // rotate sensor 90 degrees CCW about the Y axis, so X points up, Y points back, and Z points out
Jamie Smith 1:14c135ea7134 311 // (values from BNO datasheet page 41)
Jamie Smith 1:14c135ea7134 312 // imu.setPermanentOrientation(Quaternion(1.0f/SQRT_2, -1.0f/SQRT_2, 0, 0));
Jamie Smith 1:14c135ea7134 313
Jamie Smith 1:14c135ea7134 314 // rotate sensor 180 degrees about Y axis
Jamie Smith 1:14c135ea7134 315 //with these quaternion values x axis is oriented to point toward BRB, Z points up and Y points out towards you when you face the unit
Jamie Smith 1:14c135ea7134 316 // see page 5 and 11 of hillcrestlabs FSM30X datasheet
Jamie Smith 1:14c135ea7134 317 imu.setPermanentOrientation(Quaternion(0,-1, 0, 0));
Jamie Smith 1:14c135ea7134 318
Jamie Smith 1:14c135ea7134 319 // reset to default orientation
Jamie Smith 1:14c135ea7134 320 //imu.setPermanentOrientation(Quaternion(0, 0, 0, 0));
Jamie Smith 1:14c135ea7134 321
Jamie Smith 1:14c135ea7134 322 orientationTimer.stop();
Jamie Smith 1:14c135ea7134 323 pc.printf("Done setting orientation (took %.03f seconds)\r\n", orientationTimer.read());
Jamie Smith 1:14c135ea7134 324
Jamie Smith 1:14c135ea7134 325 }
Jamie Smith 1:14c135ea7134 326
Jamie Smith 0:82ee5f7adbb0 327 void BNOTestSuite::test_disable()
Jamie Smith 0:82ee5f7adbb0 328 {
Jamie Smith 0:82ee5f7adbb0 329 const size_t update_rate_ms = 200;
Jamie Smith 0:82ee5f7adbb0 330 imu.enableReport(BNO080::TOTAL_ACCELERATION, update_rate_ms);
Jamie Smith 0:82ee5f7adbb0 331 imu.enableReport(BNO080::ROTATION, update_rate_ms);
Jamie Smith 0:82ee5f7adbb0 332
Jamie Smith 0:82ee5f7adbb0 333 Timer runningTime;
Jamie Smith 0:82ee5f7adbb0 334 runningTime.start();
Jamie Smith 0:82ee5f7adbb0 335
Jamie Smith 0:82ee5f7adbb0 336 bool disabledRotation = false;
Jamie Smith 0:82ee5f7adbb0 337
Jamie Smith 0:82ee5f7adbb0 338 while (true)
Jamie Smith 0:82ee5f7adbb0 339 {
Jamie Smith 4:85b98cc04a0a 340 ThisThread::sleep_for(1ms * update_rate_ms);
Jamie Smith 0:82ee5f7adbb0 341
Jamie Smith 0:82ee5f7adbb0 342 if(imu.updateData())
Jamie Smith 0:82ee5f7adbb0 343 {
Jamie Smith 0:82ee5f7adbb0 344 if (imu.hasNewData(BNO080::ROTATION))
Jamie Smith 0:82ee5f7adbb0 345 {
Jamie Smith 0:82ee5f7adbb0 346 pc.printf("IMU Rotation Euler: ");
Jamie Smith 0:82ee5f7adbb0 347 TVector3 eulerRadians = imu.rotationVector.euler();
Jamie Smith 0:82ee5f7adbb0 348 TVector3 eulerDegrees = eulerRadians * (180.0 / M_PI);
Jamie Smith 0:82ee5f7adbb0 349 eulerDegrees.print(pc, true);
Jamie Smith 0:82ee5f7adbb0 350 pc.printf("\n");
Jamie Smith 0:82ee5f7adbb0 351 }
Jamie Smith 0:82ee5f7adbb0 352 if (imu.hasNewData(BNO080::TOTAL_ACCELERATION))
Jamie Smith 0:82ee5f7adbb0 353 {
Jamie Smith 0:82ee5f7adbb0 354 pc.printf("IMU Total Acceleration: ");
Jamie Smith 0:82ee5f7adbb0 355 imu.totalAcceleration.print(pc, true);
Jamie Smith 0:82ee5f7adbb0 356 pc.printf("\n");
Jamie Smith 0:82ee5f7adbb0 357 }
Jamie Smith 0:82ee5f7adbb0 358 }
Jamie Smith 0:82ee5f7adbb0 359
Jamie Smith 0:82ee5f7adbb0 360 if(runningTime.read() > 2 && !disabledRotation)
Jamie Smith 0:82ee5f7adbb0 361 {
Jamie Smith 0:82ee5f7adbb0 362 pc.printf("2 seconds have passed, disabling rotation report...\n");
Jamie Smith 0:82ee5f7adbb0 363 disabledRotation = true;
Jamie Smith 0:82ee5f7adbb0 364
Jamie Smith 0:82ee5f7adbb0 365 imu.disableReport(BNO080::ROTATION);
Jamie Smith 0:82ee5f7adbb0 366 }
Jamie Smith 0:82ee5f7adbb0 367 }
Jamie Smith 0:82ee5f7adbb0 368 }
Jamie Smith 0:82ee5f7adbb0 369
Jamie Smith 4:85b98cc04a0a 370 void BNOTestSuite::test_accelCalibration()
Jamie Smith 4:85b98cc04a0a 371 {
Jamie Smith 4:85b98cc04a0a 372 // enable calibration for the magnetometer
Jamie Smith 4:85b98cc04a0a 373 bool success = imu.enableCalibration(true, false, false);
Jamie Smith 4:85b98cc04a0a 374
Jamie Smith 4:85b98cc04a0a 375 const size_t update_rate_ms = 200;
Jamie Smith 4:85b98cc04a0a 376 imu.enableReport(BNO080::Report::TOTAL_ACCELERATION, update_rate_ms);
Jamie Smith 4:85b98cc04a0a 377
Jamie Smith 4:85b98cc04a0a 378 if(success)
Jamie Smith 4:85b98cc04a0a 379 {
Jamie Smith 4:85b98cc04a0a 380 pc.printf("Calibration mode was successfully enabled!\r\n");
Jamie Smith 4:85b98cc04a0a 381 }
Jamie Smith 4:85b98cc04a0a 382 else
Jamie Smith 4:85b98cc04a0a 383 {
Jamie Smith 4:85b98cc04a0a 384 pc.printf("Calibration mode failed to enable!\r\n");
Jamie Smith 4:85b98cc04a0a 385 while(true){}
Jamie Smith 4:85b98cc04a0a 386 }
Jamie Smith 4:85b98cc04a0a 387
Jamie Smith 4:85b98cc04a0a 388 while (true)
Jamie Smith 4:85b98cc04a0a 389 {
Jamie Smith 4:85b98cc04a0a 390 ThisThread::sleep_for(1ms * update_rate_ms);
Jamie Smith 4:85b98cc04a0a 391
Jamie Smith 4:85b98cc04a0a 392 if(imu.updateData())
Jamie Smith 4:85b98cc04a0a 393 {
Jamie Smith 4:85b98cc04a0a 394 pc.printf("IMU Total Acceleration: [");
Jamie Smith 4:85b98cc04a0a 395 imu.totalAcceleration.print(pc, true);
Jamie Smith 4:85b98cc04a0a 396 pc.printf("] m/s^2, Status: %hhu (should be 2-3 when calibration is complete), ", imu.getReportStatus(BNO080::Report::TOTAL_ACCELERATION));
Jamie Smith 4:85b98cc04a0a 397 pc.printf("]\r\n");
Jamie Smith 4:85b98cc04a0a 398
Jamie Smith 4:85b98cc04a0a 399 }
Jamie Smith 4:85b98cc04a0a 400 else
Jamie Smith 4:85b98cc04a0a 401 {
Jamie Smith 4:85b98cc04a0a 402 pc.printf("IMU was not ready with data packet!\r\n");
Jamie Smith 4:85b98cc04a0a 403 }
Jamie Smith 4:85b98cc04a0a 404
Jamie Smith 4:85b98cc04a0a 405 if(serial.readable())
Jamie Smith 4:85b98cc04a0a 406 {
Jamie Smith 4:85b98cc04a0a 407 // char typed over serial
Jamie Smith 4:85b98cc04a0a 408 pc.printf("Saving current calibration...\r\n");
Jamie Smith 4:85b98cc04a0a 409 imu.saveCalibration();
Jamie Smith 4:85b98cc04a0a 410
Jamie Smith 4:85b98cc04a0a 411 // throw out rest of chars in serial buffer
Jamie Smith 4:85b98cc04a0a 412 while(serial.readable())
Jamie Smith 4:85b98cc04a0a 413 {
Jamie Smith 4:85b98cc04a0a 414 pc.getc();
Jamie Smith 4:85b98cc04a0a 415 }
Jamie Smith 4:85b98cc04a0a 416 }
Jamie Smith 4:85b98cc04a0a 417 }
Jamie Smith 4:85b98cc04a0a 418
Jamie Smith 4:85b98cc04a0a 419 }
Jamie Smith 4:85b98cc04a0a 420
Jamie Smith 0:82ee5f7adbb0 421 int main()
Jamie Smith 0:82ee5f7adbb0 422 {
Jamie Smith 0:82ee5f7adbb0 423 pc.printf("============================================================\n");
Jamie Smith 0:82ee5f7adbb0 424
Jamie Smith 0:82ee5f7adbb0 425 // reset and connect to IMU
Jamie Smith 0:82ee5f7adbb0 426 while(!imu.begin())
Jamie Smith 0:82ee5f7adbb0 427 {
Jamie Smith 0:82ee5f7adbb0 428 pc.printf("Failed to connect to IMU!\r\n");
MultipleMonomials 3:f72d98d0095e 429 ThisThread::sleep_for(500);
Jamie Smith 0:82ee5f7adbb0 430 }
Jamie Smith 0:82ee5f7adbb0 431
Jamie Smith 0:82ee5f7adbb0 432 //Declare test harness
Jamie Smith 0:82ee5f7adbb0 433 BNOTestSuite harness;
Jamie Smith 0:82ee5f7adbb0 434
Jamie Smith 0:82ee5f7adbb0 435 int test = -1;
Jamie Smith 0:82ee5f7adbb0 436 pc.printf("\r\n\nHamster BNO Test Suite:\r\n");
Jamie Smith 0:82ee5f7adbb0 437
Jamie Smith 0:82ee5f7adbb0 438 //MENU. ADD OPTION PER EACH TEST
Jamie Smith 0:82ee5f7adbb0 439 pc.printf("Select a test: \n\r");
Jamie Smith 0:82ee5f7adbb0 440 pc.printf("1. Print chip info\r\n");
Jamie Smith 0:82ee5f7adbb0 441 pc.printf("2. Display rotation vector\r\n");
Jamie Smith 0:82ee5f7adbb0 442 pc.printf("3. Display rotation and acceleration\r\n");
Jamie Smith 0:82ee5f7adbb0 443 pc.printf("4. Run tap detector\r\n");
Jamie Smith 0:82ee5f7adbb0 444 pc.printf("5. Display game rotation vector\r\n");
Jamie Smith 0:82ee5f7adbb0 445 pc.printf("6. Test tare function\r\n");
Jamie Smith 0:82ee5f7adbb0 446 pc.printf("7. Run magnetometer calibration\r\n");
Jamie Smith 0:82ee5f7adbb0 447 pc.printf("8. Test stability classifier\r\n");
Jamie Smith 0:82ee5f7adbb0 448 pc.printf("9. Test metadata reading (requires BNO_DEBUG = 1)\r\n");
Jamie Smith 0:82ee5f7adbb0 449 pc.printf("10. Test set orientation\r\n");
Jamie Smith 1:14c135ea7134 450 pc.printf("11. Test set permanent orientation\r\n");
Jamie Smith 1:14c135ea7134 451 pc.printf("12. Test disabling reports\r\n");
Jamie Smith 4:85b98cc04a0a 452 pc.printf("13. Test accelerometer calibration\r\n");
Jamie Smith 0:82ee5f7adbb0 453
Jamie Smith 0:82ee5f7adbb0 454 pc.scanf("%d", &test);
Jamie Smith 0:82ee5f7adbb0 455 pc.printf("Running test %d:\r\n\n", test);
Jamie Smith 0:82ee5f7adbb0 456
Jamie Smith 0:82ee5f7adbb0 457
Jamie Smith 0:82ee5f7adbb0 458 //SWITCH. ADD CASE PER EACH TEST
Jamie Smith 0:82ee5f7adbb0 459 switch(test){
Jamie Smith 0:82ee5f7adbb0 460 case 1:
Jamie Smith 0:82ee5f7adbb0 461 harness.test_printInfo();
Jamie Smith 0:82ee5f7adbb0 462 break;
Jamie Smith 0:82ee5f7adbb0 463 case 2:
Jamie Smith 0:82ee5f7adbb0 464 harness.test_readRotationVector();
Jamie Smith 0:82ee5f7adbb0 465 break;
Jamie Smith 0:82ee5f7adbb0 466 case 3:
Jamie Smith 0:82ee5f7adbb0 467 harness.test_readRotationAcceleration();
Jamie Smith 0:82ee5f7adbb0 468 break;
Jamie Smith 0:82ee5f7adbb0 469 case 4:
Jamie Smith 0:82ee5f7adbb0 470 harness.test_tapDetector();
Jamie Smith 0:82ee5f7adbb0 471 break;
Jamie Smith 0:82ee5f7adbb0 472 case 5:
Jamie Smith 0:82ee5f7adbb0 473 harness.test_gameRotationVector();
Jamie Smith 0:82ee5f7adbb0 474 break;
Jamie Smith 0:82ee5f7adbb0 475 case 6:
Jamie Smith 0:82ee5f7adbb0 476 harness.test_tare();
Jamie Smith 0:82ee5f7adbb0 477 break;
Jamie Smith 0:82ee5f7adbb0 478 case 7:
Jamie Smith 0:82ee5f7adbb0 479 harness.test_magCalibration();
Jamie Smith 0:82ee5f7adbb0 480 break;
Jamie Smith 0:82ee5f7adbb0 481 case 8:
Jamie Smith 0:82ee5f7adbb0 482 harness.test_stabilityClassifier();
Jamie Smith 0:82ee5f7adbb0 483 break;
Jamie Smith 0:82ee5f7adbb0 484 case 9:
Jamie Smith 0:82ee5f7adbb0 485 harness.test_metadata();
Jamie Smith 0:82ee5f7adbb0 486 break;
Jamie Smith 0:82ee5f7adbb0 487 case 10:
Jamie Smith 0:82ee5f7adbb0 488 harness.test_orientation();
Jamie Smith 0:82ee5f7adbb0 489 break;
Jamie Smith 0:82ee5f7adbb0 490 case 11:
Jamie Smith 1:14c135ea7134 491 harness.test_permanentOrientation();
Jamie Smith 1:14c135ea7134 492 break;
Jamie Smith 1:14c135ea7134 493 case 12:
Jamie Smith 0:82ee5f7adbb0 494 harness.test_disable();
Jamie Smith 0:82ee5f7adbb0 495 break;
Jamie Smith 0:82ee5f7adbb0 496 default:
Jamie Smith 0:82ee5f7adbb0 497 printf("Invalid test number. Please run again.\r\n");
Jamie Smith 0:82ee5f7adbb0 498 return 1;
Jamie Smith 0:82ee5f7adbb0 499 }
Jamie Smith 0:82ee5f7adbb0 500
Jamie Smith 0:82ee5f7adbb0 501 pc.printf("Done!\r\n");
Jamie Smith 0:82ee5f7adbb0 502 return 0;
Jamie Smith 0:82ee5f7adbb0 503
Jamie Smith 0:82ee5f7adbb0 504 }
Jamie Smith 0:82ee5f7adbb0 505
Jamie Smith 0:82ee5f7adbb0 506
Jamie Smith 0:82ee5f7adbb0 507
Jamie Smith 0:82ee5f7adbb0 508
Jamie Smith 0:82ee5f7adbb0 509
Jamie Smith 0:82ee5f7adbb0 510
Jamie Smith 0:82ee5f7adbb0 511
Jamie Smith 0:82ee5f7adbb0 512
Jamie Smith 0:82ee5f7adbb0 513