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@5:ee64252765de, 2020-11-24 (annotated)

Committer:
Jamie Smith
Date:
Tue Nov 24 15:27:32 2020 -0800
Revision:
5:ee64252765de
Parent:
4:85b98cc04a0a 
Update for BNO080Async

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