![](/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@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?
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 | |
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 |