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@1:14c135ea7134, 2019-06-14 (annotated)

Committer:
Jamie Smith
Date:
Fri Jun 14 20:33:19 2019 -0700
Revision:
1:14c135ea7134
Parent:
0:82ee5f7adbb0
Child:
3:f72d98d0095e
Add example for new permanent orientation feature

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
Jamie Smith 0:82ee5f7adbb0 9 #define RAD_TO_DEG (180.0/M_PI)
Jamie Smith 0:82ee5f7adbb0 10
Jamie Smith 0:82ee5f7adbb0 11 void BNOTestSuite::test_printInfo()
Jamie Smith 0:82ee5f7adbb0 12 {
Jamie Smith 0:82ee5f7adbb0 13 pc.printf("BNO080 reports as SW version %hhu.%hhu.%hu, build %lu, part no. %lu\n",
Jamie Smith 0:82ee5f7adbb0 14 imu.majorSoftwareVersion, imu.minorSoftwareVersion, imu.patchSoftwareVersion,
Jamie Smith 0:82ee5f7adbb0 15 imu.buildNumber, imu.partNumber);
Jamie Smith 0:82ee5f7adbb0 16 }
Jamie Smith 0:82ee5f7adbb0 17
Jamie Smith 0:82ee5f7adbb0 18
Jamie Smith 0:82ee5f7adbb0 19 void BNOTestSuite::test_readRotationVector()
Jamie Smith 0:82ee5f7adbb0 20 {
Jamie Smith 0:82ee5f7adbb0 21 const size_t update_rate_ms = 200;
Jamie Smith 0:82ee5f7adbb0 22
Jamie Smith 0:82ee5f7adbb0 23 imu.enableReport(BNO080::ROTATION, update_rate_ms);
Jamie Smith 0:82ee5f7adbb0 24
Jamie Smith 0:82ee5f7adbb0 25 while (true)
Jamie Smith 0:82ee5f7adbb0 26 {
Jamie Smith 0:82ee5f7adbb0 27 // note: for some reason it seems like wait() here waits 10x the time you tell it to
Jamie Smith 0:82ee5f7adbb0 28 // not sure what's causing this behavior, and I don't see it with mbed os 2
Jamie Smith 0:82ee5f7adbb0 29 wait(.0001f * 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 0:82ee5f7adbb0 57 wait(.001f);
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 0:82ee5f7adbb0 105 // note: for some reason it seems like wait() here waits 10x the time you tell it to
Jamie Smith 0:82ee5f7adbb0 106 // not sure what's causing this behavior, and I don't see it with mbed os 2
Jamie Smith 0:82ee5f7adbb0 107 wait(.0001f * update_rate_ms);
Jamie Smith 0:82ee5f7adbb0 108
Jamie Smith 0:82ee5f7adbb0 109 if(imu.updateData())
Jamie Smith 0:82ee5f7adbb0 110 {
Jamie Smith 0:82ee5f7adbb0 111 pc.printf("IMU Game Rotation Euler: [");
Jamie Smith 0:82ee5f7adbb0 112 TVector3 eulerRadians = imu.gameRotationVector.euler();
Jamie Smith 0:82ee5f7adbb0 113 TVector3 eulerDegrees = eulerRadians * RAD_TO_DEG;
Jamie Smith 0:82ee5f7adbb0 114 eulerDegrees.print(pc, true);
Jamie Smith 0:82ee5f7adbb0 115
Jamie Smith 0:82ee5f7adbb0 116 pc.printf("], Status: %s\n", imu.getReportStatusString(BNO080::GAME_ROTATION));
Jamie Smith 0:82ee5f7adbb0 117 }
Jamie Smith 0:82ee5f7adbb0 118 else
Jamie Smith 0:82ee5f7adbb0 119 {
Jamie Smith 0:82ee5f7adbb0 120 pc.printf("IMU was not ready with data packet!\r\n");
Jamie Smith 0:82ee5f7adbb0 121 }
Jamie Smith 0:82ee5f7adbb0 122 }
Jamie Smith 0:82ee5f7adbb0 123 }
Jamie Smith 0:82ee5f7adbb0 124
Jamie Smith 0:82ee5f7adbb0 125 void BNOTestSuite::test_tare()
Jamie Smith 0:82ee5f7adbb0 126 {
Jamie Smith 0:82ee5f7adbb0 127 const size_t update_rate_ms = 100;
Jamie Smith 0:82ee5f7adbb0 128 imu.enableReport(BNO080::ROTATION, update_rate_ms);
Jamie Smith 0:82ee5f7adbb0 129
Jamie Smith 0:82ee5f7adbb0 130 Timer runningTime;
Jamie Smith 0:82ee5f7adbb0 131 runningTime.start();
Jamie Smith 0:82ee5f7adbb0 132
Jamie Smith 0:82ee5f7adbb0 133 bool sentTareCmd = false;
Jamie Smith 0:82ee5f7adbb0 134
Jamie Smith 0:82ee5f7adbb0 135 while (true)
Jamie Smith 0:82ee5f7adbb0 136 {
Jamie Smith 0:82ee5f7adbb0 137 // note: for some reason it seems like wait() here waits 10x the time you tell it to
Jamie Smith 0:82ee5f7adbb0 138 // not sure what's causing this behavior, and I don't see it with mbed os 2
Jamie Smith 0:82ee5f7adbb0 139 wait(.0001f * update_rate_ms);
Jamie Smith 0:82ee5f7adbb0 140
Jamie Smith 0:82ee5f7adbb0 141 if(imu.updateData())
Jamie Smith 0:82ee5f7adbb0 142 {
Jamie Smith 0:82ee5f7adbb0 143 pc.printf("IMU Rotation Euler: [");
Jamie Smith 0:82ee5f7adbb0 144 TVector3 eulerRadians = imu.rotationVector.euler();
Jamie Smith 0:82ee5f7adbb0 145 TVector3 eulerDegrees = eulerRadians * RAD_TO_DEG;
Jamie Smith 0:82ee5f7adbb0 146 eulerDegrees.print(pc, true);
Jamie Smith 0:82ee5f7adbb0 147
Jamie Smith 0:82ee5f7adbb0 148 pc.printf("], Status: %s\n", imu.getReportStatusString(BNO080::ROTATION));
Jamie Smith 0:82ee5f7adbb0 149 }
Jamie Smith 0:82ee5f7adbb0 150 else
Jamie Smith 0:82ee5f7adbb0 151 {
Jamie Smith 0:82ee5f7adbb0 152 pc.printf("IMU was not ready with data packet!\r\n");
Jamie Smith 0:82ee5f7adbb0 153 }
Jamie Smith 0:82ee5f7adbb0 154
Jamie Smith 0:82ee5f7adbb0 155 if(runningTime.read() > 2 && !sentTareCmd)
Jamie Smith 0:82ee5f7adbb0 156 {
Jamie Smith 0:82ee5f7adbb0 157 pc.printf("2 seconds have passed, sending tare command...\n");
Jamie Smith 0:82ee5f7adbb0 158 imu.tare();
Jamie Smith 0:82ee5f7adbb0 159 sentTareCmd = true;
Jamie Smith 0:82ee5f7adbb0 160 }
Jamie Smith 0:82ee5f7adbb0 161 }
Jamie Smith 0:82ee5f7adbb0 162 }
Jamie Smith 0:82ee5f7adbb0 163
Jamie Smith 0:82ee5f7adbb0 164 void BNOTestSuite::test_magCalibration()
Jamie Smith 0:82ee5f7adbb0 165 {
Jamie Smith 0:82ee5f7adbb0 166 // enable calibration for the magnetometer
Jamie Smith 0:82ee5f7adbb0 167 bool success = imu.enableCalibration(false, false, true);
Jamie Smith 0:82ee5f7adbb0 168
Jamie Smith 0:82ee5f7adbb0 169 const size_t update_rate_ms = 200;
Jamie Smith 0:82ee5f7adbb0 170 imu.enableReport(BNO080::GAME_ROTATION, update_rate_ms);
Jamie Smith 0:82ee5f7adbb0 171 imu.enableReport(BNO080::MAG_FIELD, update_rate_ms);
Jamie Smith 0:82ee5f7adbb0 172 imu.enableReport(BNO080::MAG_FIELD_UNCALIBRATED, update_rate_ms);
Jamie Smith 0:82ee5f7adbb0 173
Jamie Smith 0:82ee5f7adbb0 174 if(success)
Jamie Smith 0:82ee5f7adbb0 175 {
Jamie Smith 0:82ee5f7adbb0 176 pc.printf("Calibration mode was successfully enabled!\n");
Jamie Smith 0:82ee5f7adbb0 177 }
Jamie Smith 0:82ee5f7adbb0 178 else
Jamie Smith 0:82ee5f7adbb0 179 {
Jamie Smith 0:82ee5f7adbb0 180 pc.printf("Calibration mode failed to enable!\n");
Jamie Smith 0:82ee5f7adbb0 181 while(true){}
Jamie Smith 0:82ee5f7adbb0 182 }
Jamie Smith 0:82ee5f7adbb0 183
Jamie Smith 0:82ee5f7adbb0 184 while (true)
Jamie Smith 0:82ee5f7adbb0 185 {
Jamie Smith 0:82ee5f7adbb0 186 // note: for some reason it seems like wait() here waits 10x the time you tell it to
Jamie Smith 0:82ee5f7adbb0 187 // not sure what's causing this behavior, and I don't see it with mbed os 2
Jamie Smith 0:82ee5f7adbb0 188 wait(.0001f * update_rate_ms);
Jamie Smith 0:82ee5f7adbb0 189
Jamie Smith 0:82ee5f7adbb0 190 if(imu.updateData())
Jamie Smith 0:82ee5f7adbb0 191 {
Jamie Smith 0:82ee5f7adbb0 192 pc.printf("IMU Magnetic Field: [");
Jamie Smith 0:82ee5f7adbb0 193 imu.magFieldUncalibrated.print(pc, true);
Jamie Smith 0:82ee5f7adbb0 194
Jamie Smith 0:82ee5f7adbb0 195 pc.printf("] uTesla, Status: %hhu (should be 2-3 when calibration is complete), ", imu.getReportStatus(BNO080::MAG_FIELD));
Jamie Smith 0:82ee5f7adbb0 196
Jamie Smith 0:82ee5f7adbb0 197 pc.printf("Hard iron offsets: [");
Jamie Smith 0:82ee5f7adbb0 198 imu.hardIronOffset.print(pc, true);
Jamie Smith 0:82ee5f7adbb0 199
Jamie Smith 0:82ee5f7adbb0 200 pc.printf("]\n");
Jamie Smith 0:82ee5f7adbb0 201
Jamie Smith 0:82ee5f7adbb0 202 }
Jamie Smith 0:82ee5f7adbb0 203 else
Jamie Smith 0:82ee5f7adbb0 204 {
Jamie Smith 0:82ee5f7adbb0 205 pc.printf("IMU was not ready with data packet!\r\n");
Jamie Smith 0:82ee5f7adbb0 206 }
Jamie Smith 0:82ee5f7adbb0 207 }
Jamie Smith 0:82ee5f7adbb0 208
Jamie Smith 0:82ee5f7adbb0 209 }
Jamie Smith 0:82ee5f7adbb0 210
Jamie Smith 0:82ee5f7adbb0 211 void BNOTestSuite::test_stabilityClassifier()
Jamie Smith 0:82ee5f7adbb0 212 {
Jamie Smith 0:82ee5f7adbb0 213 imu.enableReport(BNO080::STABILITY_CLASSIFIER, 200);
Jamie Smith 0:82ee5f7adbb0 214
Jamie Smith 0:82ee5f7adbb0 215 while (true)
Jamie Smith 0:82ee5f7adbb0 216 {
Jamie Smith 0:82ee5f7adbb0 217 wait(.001f);
Jamie Smith 0:82ee5f7adbb0 218
Jamie Smith 0:82ee5f7adbb0 219 if(imu.updateData())
Jamie Smith 0:82ee5f7adbb0 220 {
Jamie Smith 0:82ee5f7adbb0 221 if (imu.hasNewData(BNO080::STABILITY_CLASSIFIER))
Jamie Smith 0:82ee5f7adbb0 222 {
Jamie Smith 0:82ee5f7adbb0 223 pc.printf("Stability: ");
Jamie Smith 0:82ee5f7adbb0 224
Jamie Smith 0:82ee5f7adbb0 225 switch(imu.stability)
Jamie Smith 0:82ee5f7adbb0 226 {
Jamie Smith 0:82ee5f7adbb0 227 case BNO080::UNKNOWN:
Jamie Smith 0:82ee5f7adbb0 228 pc.printf("Unknown\n");
Jamie Smith 0:82ee5f7adbb0 229 break;
Jamie Smith 0:82ee5f7adbb0 230 case BNO080::ON_TABLE:
Jamie Smith 0:82ee5f7adbb0 231 pc.printf("On Table\n");
Jamie Smith 0:82ee5f7adbb0 232 break;
Jamie Smith 0:82ee5f7adbb0 233 case BNO080::STATIONARY:
Jamie Smith 0:82ee5f7adbb0 234 pc.printf("Stationary\n");
Jamie Smith 0:82ee5f7adbb0 235 break;
Jamie Smith 0:82ee5f7adbb0 236 case BNO080::STABLE:
Jamie Smith 0:82ee5f7adbb0 237 pc.printf("Stable\n");
Jamie Smith 0:82ee5f7adbb0 238 break;
Jamie Smith 0:82ee5f7adbb0 239 case BNO080::MOTION:
Jamie Smith 0:82ee5f7adbb0 240 pc.printf("Motion\n");
Jamie Smith 0:82ee5f7adbb0 241 break;
Jamie Smith 0:82ee5f7adbb0 242 }
Jamie Smith 0:82ee5f7adbb0 243 }
Jamie Smith 0:82ee5f7adbb0 244 }
Jamie Smith 0:82ee5f7adbb0 245 }
Jamie Smith 0:82ee5f7adbb0 246
Jamie Smith 0:82ee5f7adbb0 247 }
Jamie Smith 0:82ee5f7adbb0 248
Jamie Smith 0:82ee5f7adbb0 249 void BNOTestSuite::test_metadata()
Jamie Smith 0:82ee5f7adbb0 250 {
Jamie Smith 0:82ee5f7adbb0 251 pc.printf("Printing metadata for Linear Acceleration:\r\n");
Jamie Smith 0:82ee5f7adbb0 252 imu.printMetadataSummary(BNO080::LINEAR_ACCELERATION);
Jamie Smith 0:82ee5f7adbb0 253
Jamie Smith 0:82ee5f7adbb0 254 pc.printf("Printing metadata for Rotation Vector:\r\n");
Jamie Smith 0:82ee5f7adbb0 255 imu.printMetadataSummary(BNO080::ROTATION);
Jamie Smith 0:82ee5f7adbb0 256
Jamie Smith 0:82ee5f7adbb0 257 pc.printf("Printing metadata for Magnetic Field:\r\n");
Jamie Smith 0:82ee5f7adbb0 258 imu.printMetadataSummary(BNO080::MAG_FIELD);
Jamie Smith 0:82ee5f7adbb0 259
Jamie Smith 0:82ee5f7adbb0 260 pc.printf("Printing metadata for Tap Detector:\r\n");
Jamie Smith 0:82ee5f7adbb0 261 imu.printMetadataSummary(BNO080::TAP_DETECTOR);
Jamie Smith 0:82ee5f7adbb0 262
Jamie Smith 0:82ee5f7adbb0 263 }
Jamie Smith 0:82ee5f7adbb0 264
Jamie Smith 0:82ee5f7adbb0 265 void BNOTestSuite::test_orientation()
Jamie Smith 0:82ee5f7adbb0 266 {
Jamie Smith 0:82ee5f7adbb0 267 const size_t update_rate_ms = 200;
Jamie Smith 0:82ee5f7adbb0 268 imu.enableReport(BNO080::TOTAL_ACCELERATION, update_rate_ms);
Jamie Smith 0:82ee5f7adbb0 269 imu.enableReport(BNO080::ROTATION, update_rate_ms);
Jamie Smith 0:82ee5f7adbb0 270
Jamie Smith 0:82ee5f7adbb0 271 Timer runningTime;
Jamie Smith 0:82ee5f7adbb0 272 runningTime.start();
Jamie Smith 0:82ee5f7adbb0 273
Jamie Smith 0:82ee5f7adbb0 274 bool setOrientation = false;
Jamie Smith 0:82ee5f7adbb0 275
Jamie Smith 0:82ee5f7adbb0 276 while (true)
Jamie Smith 0:82ee5f7adbb0 277 {
Jamie Smith 0:82ee5f7adbb0 278 // note: for some reason it seems like wait() here waits 10x the time you tell it to
Jamie Smith 0:82ee5f7adbb0 279 // not sure what's causing this behavior, and I don't see it with mbed os 2
Jamie Smith 0:82ee5f7adbb0 280 wait(.0001f * update_rate_ms);
Jamie Smith 0:82ee5f7adbb0 281
Jamie Smith 0:82ee5f7adbb0 282 if(imu.updateData())
Jamie Smith 0:82ee5f7adbb0 283 {
Jamie Smith 0:82ee5f7adbb0 284 if (imu.hasNewData(BNO080::ROTATION))
Jamie Smith 0:82ee5f7adbb0 285 {
Jamie Smith 0:82ee5f7adbb0 286 pc.printf("IMU Rotation Euler: ");
Jamie Smith 0:82ee5f7adbb0 287 TVector3 eulerRadians = imu.rotationVector.euler();
Jamie Smith 0:82ee5f7adbb0 288 TVector3 eulerDegrees = eulerRadians * (180.0 / M_PI);
Jamie Smith 0:82ee5f7adbb0 289 eulerDegrees.print(pc, true);
Jamie Smith 0:82ee5f7adbb0 290 pc.printf("\n");
Jamie Smith 0:82ee5f7adbb0 291 }
Jamie Smith 0:82ee5f7adbb0 292 if (imu.hasNewData(BNO080::TOTAL_ACCELERATION))
Jamie Smith 0:82ee5f7adbb0 293 {
Jamie Smith 0:82ee5f7adbb0 294 pc.printf("IMU Total Acceleration: ");
Jamie Smith 0:82ee5f7adbb0 295 imu.totalAcceleration.print(pc, true);
Jamie Smith 0:82ee5f7adbb0 296 pc.printf("\n");
Jamie Smith 0:82ee5f7adbb0 297 }
Jamie Smith 0:82ee5f7adbb0 298 }
Jamie Smith 0:82ee5f7adbb0 299
Jamie Smith 0:82ee5f7adbb0 300 if(runningTime.read() > 2 && !setOrientation)
Jamie Smith 0:82ee5f7adbb0 301 {
Jamie Smith 0:82ee5f7adbb0 302 pc.printf("2 seconds have passed, sending set orientation command...\n");
Jamie Smith 0:82ee5f7adbb0 303 setOrientation = true;
Jamie Smith 0:82ee5f7adbb0 304
Jamie Smith 0:82ee5f7adbb0 305 // rotate sensor about the Y axis, so Z points down and X points west.
Jamie Smith 0:82ee5f7adbb0 306 // (values from BNO datasheet page 41)
Jamie Smith 0:82ee5f7adbb0 307 imu.setSensorOrientation(Quaternion(0, -1, 0, 0));
Jamie Smith 0:82ee5f7adbb0 308 }
Jamie Smith 0:82ee5f7adbb0 309 }
Jamie Smith 0:82ee5f7adbb0 310 }
Jamie Smith 0:82ee5f7adbb0 311
Jamie Smith 1:14c135ea7134 312 void BNOTestSuite::test_permanentOrientation()
Jamie Smith 1:14c135ea7134 313 {
Jamie Smith 1:14c135ea7134 314 pc.printf("Setting permanent sensor orientation...\n");
Jamie Smith 1:14c135ea7134 315 Timer orientationTimer;
Jamie Smith 1:14c135ea7134 316 orientationTimer.start();
Jamie Smith 1:14c135ea7134 317
Jamie Smith 1:14c135ea7134 318 // rotate sensor 90 degrees CCW about the Y axis, so X points up, Y points back, and Z points out
Jamie Smith 1:14c135ea7134 319 // (values from BNO datasheet page 41)
Jamie Smith 1:14c135ea7134 320 // imu.setPermanentOrientation(Quaternion(1.0f/SQRT_2, -1.0f/SQRT_2, 0, 0));
Jamie Smith 1:14c135ea7134 321
Jamie Smith 1:14c135ea7134 322 // rotate sensor 180 degrees about Y axis
Jamie Smith 1:14c135ea7134 323 //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 324 // see page 5 and 11 of hillcrestlabs FSM30X datasheet
Jamie Smith 1:14c135ea7134 325 imu.setPermanentOrientation(Quaternion(0,-1, 0, 0));
Jamie Smith 1:14c135ea7134 326
Jamie Smith 1:14c135ea7134 327 // reset to default orientation
Jamie Smith 1:14c135ea7134 328 //imu.setPermanentOrientation(Quaternion(0, 0, 0, 0));
Jamie Smith 1:14c135ea7134 329
Jamie Smith 1:14c135ea7134 330 orientationTimer.stop();
Jamie Smith 1:14c135ea7134 331 pc.printf("Done setting orientation (took %.03f seconds)\r\n", orientationTimer.read());
Jamie Smith 1:14c135ea7134 332
Jamie Smith 1:14c135ea7134 333 }
Jamie Smith 1:14c135ea7134 334
Jamie Smith 0:82ee5f7adbb0 335 void BNOTestSuite::test_disable()
Jamie Smith 0:82ee5f7adbb0 336 {
Jamie Smith 0:82ee5f7adbb0 337 const size_t update_rate_ms = 200;
Jamie Smith 0:82ee5f7adbb0 338 imu.enableReport(BNO080::TOTAL_ACCELERATION, update_rate_ms);
Jamie Smith 0:82ee5f7adbb0 339 imu.enableReport(BNO080::ROTATION, update_rate_ms);
Jamie Smith 0:82ee5f7adbb0 340
Jamie Smith 0:82ee5f7adbb0 341 Timer runningTime;
Jamie Smith 0:82ee5f7adbb0 342 runningTime.start();
Jamie Smith 0:82ee5f7adbb0 343
Jamie Smith 0:82ee5f7adbb0 344 bool disabledRotation = false;
Jamie Smith 0:82ee5f7adbb0 345
Jamie Smith 0:82ee5f7adbb0 346 while (true)
Jamie Smith 0:82ee5f7adbb0 347 {
Jamie Smith 0:82ee5f7adbb0 348 // note: for some reason it seems like wait() here waits 10x the time you tell it to
Jamie Smith 0:82ee5f7adbb0 349 // not sure what's causing this behavior, and I don't see it with mbed os 2
Jamie Smith 0:82ee5f7adbb0 350 wait(.0001f * update_rate_ms);
Jamie Smith 0:82ee5f7adbb0 351
Jamie Smith 0:82ee5f7adbb0 352 if(imu.updateData())
Jamie Smith 0:82ee5f7adbb0 353 {
Jamie Smith 0:82ee5f7adbb0 354 if (imu.hasNewData(BNO080::ROTATION))
Jamie Smith 0:82ee5f7adbb0 355 {
Jamie Smith 0:82ee5f7adbb0 356 pc.printf("IMU Rotation Euler: ");
Jamie Smith 0:82ee5f7adbb0 357 TVector3 eulerRadians = imu.rotationVector.euler();
Jamie Smith 0:82ee5f7adbb0 358 TVector3 eulerDegrees = eulerRadians * (180.0 / M_PI);
Jamie Smith 0:82ee5f7adbb0 359 eulerDegrees.print(pc, true);
Jamie Smith 0:82ee5f7adbb0 360 pc.printf("\n");
Jamie Smith 0:82ee5f7adbb0 361 }
Jamie Smith 0:82ee5f7adbb0 362 if (imu.hasNewData(BNO080::TOTAL_ACCELERATION))
Jamie Smith 0:82ee5f7adbb0 363 {
Jamie Smith 0:82ee5f7adbb0 364 pc.printf("IMU Total Acceleration: ");
Jamie Smith 0:82ee5f7adbb0 365 imu.totalAcceleration.print(pc, true);
Jamie Smith 0:82ee5f7adbb0 366 pc.printf("\n");
Jamie Smith 0:82ee5f7adbb0 367 }
Jamie Smith 0:82ee5f7adbb0 368 }
Jamie Smith 0:82ee5f7adbb0 369
Jamie Smith 0:82ee5f7adbb0 370 if(runningTime.read() > 2 && !disabledRotation)
Jamie Smith 0:82ee5f7adbb0 371 {
Jamie Smith 0:82ee5f7adbb0 372 pc.printf("2 seconds have passed, disabling rotation report...\n");
Jamie Smith 0:82ee5f7adbb0 373 disabledRotation = true;
Jamie Smith 0:82ee5f7adbb0 374
Jamie Smith 0:82ee5f7adbb0 375 imu.disableReport(BNO080::ROTATION);
Jamie Smith 0:82ee5f7adbb0 376 }
Jamie Smith 0:82ee5f7adbb0 377 }
Jamie Smith 0:82ee5f7adbb0 378 }
Jamie Smith 0:82ee5f7adbb0 379
Jamie Smith 0:82ee5f7adbb0 380 int main()
Jamie Smith 0:82ee5f7adbb0 381 {
Jamie Smith 0:82ee5f7adbb0 382 pc.baud(115200);
Jamie Smith 0:82ee5f7adbb0 383
Jamie Smith 0:82ee5f7adbb0 384 pc.printf("============================================================\n");
Jamie Smith 0:82ee5f7adbb0 385
Jamie Smith 0:82ee5f7adbb0 386
Jamie Smith 0:82ee5f7adbb0 387 // reset and connect to IMU
Jamie Smith 0:82ee5f7adbb0 388 while(!imu.begin())
Jamie Smith 0:82ee5f7adbb0 389 {
Jamie Smith 0:82ee5f7adbb0 390 pc.printf("Failed to connect to IMU!\r\n");
Jamie Smith 0:82ee5f7adbb0 391 wait(.5);
Jamie Smith 0:82ee5f7adbb0 392 }
Jamie Smith 0:82ee5f7adbb0 393
Jamie Smith 0:82ee5f7adbb0 394 //Declare test harness
Jamie Smith 0:82ee5f7adbb0 395 BNOTestSuite harness;
Jamie Smith 0:82ee5f7adbb0 396
Jamie Smith 0:82ee5f7adbb0 397 int test = -1;
Jamie Smith 0:82ee5f7adbb0 398 pc.printf("\r\n\nHamster BNO Test Suite:\r\n");
Jamie Smith 0:82ee5f7adbb0 399
Jamie Smith 0:82ee5f7adbb0 400 //MENU. ADD OPTION PER EACH TEST
Jamie Smith 0:82ee5f7adbb0 401 pc.printf("Select a test: \n\r");
Jamie Smith 0:82ee5f7adbb0 402 pc.printf("1. Print chip info\r\n");
Jamie Smith 0:82ee5f7adbb0 403 pc.printf("2. Display rotation vector\r\n");
Jamie Smith 0:82ee5f7adbb0 404 pc.printf("3. Display rotation and acceleration\r\n");
Jamie Smith 0:82ee5f7adbb0 405 pc.printf("4. Run tap detector\r\n");
Jamie Smith 0:82ee5f7adbb0 406 pc.printf("5. Display game rotation vector\r\n");
Jamie Smith 0:82ee5f7adbb0 407 pc.printf("6. Test tare function\r\n");
Jamie Smith 0:82ee5f7adbb0 408 pc.printf("7. Run magnetometer calibration\r\n");
Jamie Smith 0:82ee5f7adbb0 409 pc.printf("8. Test stability classifier\r\n");
Jamie Smith 0:82ee5f7adbb0 410 pc.printf("9. Test metadata reading (requires BNO_DEBUG = 1)\r\n");
Jamie Smith 0:82ee5f7adbb0 411 pc.printf("10. Test set orientation\r\n");
Jamie Smith 1:14c135ea7134 412 pc.printf("11. Test set permanent orientation\r\n");
Jamie Smith 1:14c135ea7134 413 pc.printf("12. Test disabling reports\r\n");
Jamie Smith 1:14c135ea7134 414 pc.printf("13. Exit test suite\r\n");
Jamie Smith 0:82ee5f7adbb0 415
Jamie Smith 0:82ee5f7adbb0 416 pc.scanf("%d", &test);
Jamie Smith 0:82ee5f7adbb0 417 pc.printf("Running test %d:\r\n\n", test);
Jamie Smith 0:82ee5f7adbb0 418
Jamie Smith 0:82ee5f7adbb0 419
Jamie Smith 0:82ee5f7adbb0 420 //SWITCH. ADD CASE PER EACH TEST
Jamie Smith 0:82ee5f7adbb0 421 switch(test){
Jamie Smith 0:82ee5f7adbb0 422 case 1:
Jamie Smith 0:82ee5f7adbb0 423 harness.test_printInfo();
Jamie Smith 0:82ee5f7adbb0 424 break;
Jamie Smith 0:82ee5f7adbb0 425 case 2:
Jamie Smith 0:82ee5f7adbb0 426 harness.test_readRotationVector();
Jamie Smith 0:82ee5f7adbb0 427 break;
Jamie Smith 0:82ee5f7adbb0 428 case 3:
Jamie Smith 0:82ee5f7adbb0 429 harness.test_readRotationAcceleration();
Jamie Smith 0:82ee5f7adbb0 430 break;
Jamie Smith 0:82ee5f7adbb0 431 case 4:
Jamie Smith 0:82ee5f7adbb0 432 harness.test_tapDetector();
Jamie Smith 0:82ee5f7adbb0 433 break;
Jamie Smith 0:82ee5f7adbb0 434 case 5:
Jamie Smith 0:82ee5f7adbb0 435 harness.test_gameRotationVector();
Jamie Smith 0:82ee5f7adbb0 436 break;
Jamie Smith 0:82ee5f7adbb0 437 case 6:
Jamie Smith 0:82ee5f7adbb0 438 harness.test_tare();
Jamie Smith 0:82ee5f7adbb0 439 break;
Jamie Smith 0:82ee5f7adbb0 440 case 7:
Jamie Smith 0:82ee5f7adbb0 441 harness.test_magCalibration();
Jamie Smith 0:82ee5f7adbb0 442 break;
Jamie Smith 0:82ee5f7adbb0 443 case 8:
Jamie Smith 0:82ee5f7adbb0 444 harness.test_stabilityClassifier();
Jamie Smith 0:82ee5f7adbb0 445 break;
Jamie Smith 0:82ee5f7adbb0 446 case 9:
Jamie Smith 0:82ee5f7adbb0 447 harness.test_metadata();
Jamie Smith 0:82ee5f7adbb0 448 break;
Jamie Smith 0:82ee5f7adbb0 449 case 10:
Jamie Smith 0:82ee5f7adbb0 450 harness.test_orientation();
Jamie Smith 0:82ee5f7adbb0 451 break;
Jamie Smith 0:82ee5f7adbb0 452 case 11:
Jamie Smith 1:14c135ea7134 453 harness.test_permanentOrientation();
Jamie Smith 1:14c135ea7134 454 break;
Jamie Smith 1:14c135ea7134 455 case 12:
Jamie Smith 0:82ee5f7adbb0 456 harness.test_disable();
Jamie Smith 0:82ee5f7adbb0 457 break;
Jamie Smith 1:14c135ea7134 458 case 13:
Jamie Smith 0:82ee5f7adbb0 459 pc.printf("Exiting test suite.\r\n");
Jamie Smith 0:82ee5f7adbb0 460 return 0;
Jamie Smith 0:82ee5f7adbb0 461 default:
Jamie Smith 0:82ee5f7adbb0 462 printf("Invalid test number. Please run again.\r\n");
Jamie Smith 0:82ee5f7adbb0 463 return 1;
Jamie Smith 0:82ee5f7adbb0 464 }
Jamie Smith 0:82ee5f7adbb0 465
Jamie Smith 0:82ee5f7adbb0 466 pc.printf("Done!\r\n");
Jamie Smith 0:82ee5f7adbb0 467 return 0;
Jamie Smith 0:82ee5f7adbb0 468
Jamie Smith 0:82ee5f7adbb0 469 }
Jamie Smith 0:82ee5f7adbb0 470
Jamie Smith 0:82ee5f7adbb0 471
Jamie Smith 0:82ee5f7adbb0 472
Jamie Smith 0:82ee5f7adbb0 473
Jamie Smith 0:82ee5f7adbb0 474
Jamie Smith 0:82ee5f7adbb0 475
Jamie Smith 0:82ee5f7adbb0 476
Jamie Smith 0:82ee5f7adbb0 477
Jamie Smith 0:82ee5f7adbb0 478