![](/media/cache/profiles/TAG-avatar.png.50x50_q85.png)
Example/test programs for my BNO080 driver.
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?
User | Revision | Line number | New 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 |