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@0:82ee5f7adbb0, 2018-12-29 (annotated)
- Committer:
- Jamie Smith
- Date:
- Sat Dec 29 04:14:43 2018 -0800
- Revision:
- 0:82ee5f7adbb0
- Child:
- 1:14c135ea7134
Initial commit
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 |
0:82ee5f7adbb0 | 312 | void BNOTestSuite::test_disable() |
Jamie Smith |
0:82ee5f7adbb0 | 313 | { |
Jamie Smith |
0:82ee5f7adbb0 | 314 | const size_t update_rate_ms = 200; |
Jamie Smith |
0:82ee5f7adbb0 | 315 | imu.enableReport(BNO080::TOTAL_ACCELERATION, update_rate_ms); |
Jamie Smith |
0:82ee5f7adbb0 | 316 | imu.enableReport(BNO080::ROTATION, update_rate_ms); |
Jamie Smith |
0:82ee5f7adbb0 | 317 | |
Jamie Smith |
0:82ee5f7adbb0 | 318 | Timer runningTime; |
Jamie Smith |
0:82ee5f7adbb0 | 319 | runningTime.start(); |
Jamie Smith |
0:82ee5f7adbb0 | 320 | |
Jamie Smith |
0:82ee5f7adbb0 | 321 | bool disabledRotation = false; |
Jamie Smith |
0:82ee5f7adbb0 | 322 | |
Jamie Smith |
0:82ee5f7adbb0 | 323 | while (true) |
Jamie Smith |
0:82ee5f7adbb0 | 324 | { |
Jamie Smith |
0:82ee5f7adbb0 | 325 | // note: for some reason it seems like wait() here waits 10x the time you tell it to |
Jamie Smith |
0:82ee5f7adbb0 | 326 | // not sure what's causing this behavior, and I don't see it with mbed os 2 |
Jamie Smith |
0:82ee5f7adbb0 | 327 | wait(.0001f * update_rate_ms); |
Jamie Smith |
0:82ee5f7adbb0 | 328 | |
Jamie Smith |
0:82ee5f7adbb0 | 329 | if(imu.updateData()) |
Jamie Smith |
0:82ee5f7adbb0 | 330 | { |
Jamie Smith |
0:82ee5f7adbb0 | 331 | if (imu.hasNewData(BNO080::ROTATION)) |
Jamie Smith |
0:82ee5f7adbb0 | 332 | { |
Jamie Smith |
0:82ee5f7adbb0 | 333 | pc.printf("IMU Rotation Euler: "); |
Jamie Smith |
0:82ee5f7adbb0 | 334 | TVector3 eulerRadians = imu.rotationVector.euler(); |
Jamie Smith |
0:82ee5f7adbb0 | 335 | TVector3 eulerDegrees = eulerRadians * (180.0 / M_PI); |
Jamie Smith |
0:82ee5f7adbb0 | 336 | eulerDegrees.print(pc, true); |
Jamie Smith |
0:82ee5f7adbb0 | 337 | pc.printf("\n"); |
Jamie Smith |
0:82ee5f7adbb0 | 338 | } |
Jamie Smith |
0:82ee5f7adbb0 | 339 | if (imu.hasNewData(BNO080::TOTAL_ACCELERATION)) |
Jamie Smith |
0:82ee5f7adbb0 | 340 | { |
Jamie Smith |
0:82ee5f7adbb0 | 341 | pc.printf("IMU Total Acceleration: "); |
Jamie Smith |
0:82ee5f7adbb0 | 342 | imu.totalAcceleration.print(pc, true); |
Jamie Smith |
0:82ee5f7adbb0 | 343 | pc.printf("\n"); |
Jamie Smith |
0:82ee5f7adbb0 | 344 | } |
Jamie Smith |
0:82ee5f7adbb0 | 345 | } |
Jamie Smith |
0:82ee5f7adbb0 | 346 | |
Jamie Smith |
0:82ee5f7adbb0 | 347 | if(runningTime.read() > 2 && !disabledRotation) |
Jamie Smith |
0:82ee5f7adbb0 | 348 | { |
Jamie Smith |
0:82ee5f7adbb0 | 349 | pc.printf("2 seconds have passed, disabling rotation report...\n"); |
Jamie Smith |
0:82ee5f7adbb0 | 350 | disabledRotation = true; |
Jamie Smith |
0:82ee5f7adbb0 | 351 | |
Jamie Smith |
0:82ee5f7adbb0 | 352 | imu.disableReport(BNO080::ROTATION); |
Jamie Smith |
0:82ee5f7adbb0 | 353 | } |
Jamie Smith |
0:82ee5f7adbb0 | 354 | } |
Jamie Smith |
0:82ee5f7adbb0 | 355 | } |
Jamie Smith |
0:82ee5f7adbb0 | 356 | |
Jamie Smith |
0:82ee5f7adbb0 | 357 | int main() |
Jamie Smith |
0:82ee5f7adbb0 | 358 | { |
Jamie Smith |
0:82ee5f7adbb0 | 359 | pc.baud(115200); |
Jamie Smith |
0:82ee5f7adbb0 | 360 | |
Jamie Smith |
0:82ee5f7adbb0 | 361 | pc.printf("============================================================\n"); |
Jamie Smith |
0:82ee5f7adbb0 | 362 | |
Jamie Smith |
0:82ee5f7adbb0 | 363 | |
Jamie Smith |
0:82ee5f7adbb0 | 364 | // reset and connect to IMU |
Jamie Smith |
0:82ee5f7adbb0 | 365 | while(!imu.begin()) |
Jamie Smith |
0:82ee5f7adbb0 | 366 | { |
Jamie Smith |
0:82ee5f7adbb0 | 367 | pc.printf("Failed to connect to IMU!\r\n"); |
Jamie Smith |
0:82ee5f7adbb0 | 368 | wait(.5); |
Jamie Smith |
0:82ee5f7adbb0 | 369 | } |
Jamie Smith |
0:82ee5f7adbb0 | 370 | |
Jamie Smith |
0:82ee5f7adbb0 | 371 | //Declare test harness |
Jamie Smith |
0:82ee5f7adbb0 | 372 | BNOTestSuite harness; |
Jamie Smith |
0:82ee5f7adbb0 | 373 | |
Jamie Smith |
0:82ee5f7adbb0 | 374 | int test = -1; |
Jamie Smith |
0:82ee5f7adbb0 | 375 | pc.printf("\r\n\nHamster BNO Test Suite:\r\n"); |
Jamie Smith |
0:82ee5f7adbb0 | 376 | |
Jamie Smith |
0:82ee5f7adbb0 | 377 | //MENU. ADD OPTION PER EACH TEST |
Jamie Smith |
0:82ee5f7adbb0 | 378 | pc.printf("Select a test: \n\r"); |
Jamie Smith |
0:82ee5f7adbb0 | 379 | pc.printf("1. Print chip info\r\n"); |
Jamie Smith |
0:82ee5f7adbb0 | 380 | pc.printf("2. Display rotation vector\r\n"); |
Jamie Smith |
0:82ee5f7adbb0 | 381 | pc.printf("3. Display rotation and acceleration\r\n"); |
Jamie Smith |
0:82ee5f7adbb0 | 382 | pc.printf("4. Run tap detector\r\n"); |
Jamie Smith |
0:82ee5f7adbb0 | 383 | pc.printf("5. Display game rotation vector\r\n"); |
Jamie Smith |
0:82ee5f7adbb0 | 384 | pc.printf("6. Test tare function\r\n"); |
Jamie Smith |
0:82ee5f7adbb0 | 385 | pc.printf("7. Run magnetometer calibration\r\n"); |
Jamie Smith |
0:82ee5f7adbb0 | 386 | pc.printf("8. Test stability classifier\r\n"); |
Jamie Smith |
0:82ee5f7adbb0 | 387 | pc.printf("9. Test metadata reading (requires BNO_DEBUG = 1)\r\n"); |
Jamie Smith |
0:82ee5f7adbb0 | 388 | pc.printf("10. Test set orientation\r\n"); |
Jamie Smith |
0:82ee5f7adbb0 | 389 | pc.printf("11. Test disabling reports\r\n"); |
Jamie Smith |
0:82ee5f7adbb0 | 390 | pc.printf("12. Exit test suite\r\n"); |
Jamie Smith |
0:82ee5f7adbb0 | 391 | |
Jamie Smith |
0:82ee5f7adbb0 | 392 | pc.scanf("%d", &test); |
Jamie Smith |
0:82ee5f7adbb0 | 393 | pc.printf("Running test %d:\r\n\n", test); |
Jamie Smith |
0:82ee5f7adbb0 | 394 | |
Jamie Smith |
0:82ee5f7adbb0 | 395 | |
Jamie Smith |
0:82ee5f7adbb0 | 396 | //SWITCH. ADD CASE PER EACH TEST |
Jamie Smith |
0:82ee5f7adbb0 | 397 | switch(test){ |
Jamie Smith |
0:82ee5f7adbb0 | 398 | case 1: |
Jamie Smith |
0:82ee5f7adbb0 | 399 | harness.test_printInfo(); |
Jamie Smith |
0:82ee5f7adbb0 | 400 | break; |
Jamie Smith |
0:82ee5f7adbb0 | 401 | case 2: |
Jamie Smith |
0:82ee5f7adbb0 | 402 | harness.test_readRotationVector(); |
Jamie Smith |
0:82ee5f7adbb0 | 403 | break; |
Jamie Smith |
0:82ee5f7adbb0 | 404 | case 3: |
Jamie Smith |
0:82ee5f7adbb0 | 405 | harness.test_readRotationAcceleration(); |
Jamie Smith |
0:82ee5f7adbb0 | 406 | break; |
Jamie Smith |
0:82ee5f7adbb0 | 407 | case 4: |
Jamie Smith |
0:82ee5f7adbb0 | 408 | harness.test_tapDetector(); |
Jamie Smith |
0:82ee5f7adbb0 | 409 | break; |
Jamie Smith |
0:82ee5f7adbb0 | 410 | case 5: |
Jamie Smith |
0:82ee5f7adbb0 | 411 | harness.test_gameRotationVector(); |
Jamie Smith |
0:82ee5f7adbb0 | 412 | break; |
Jamie Smith |
0:82ee5f7adbb0 | 413 | case 6: |
Jamie Smith |
0:82ee5f7adbb0 | 414 | harness.test_tare(); |
Jamie Smith |
0:82ee5f7adbb0 | 415 | break; |
Jamie Smith |
0:82ee5f7adbb0 | 416 | case 7: |
Jamie Smith |
0:82ee5f7adbb0 | 417 | harness.test_magCalibration(); |
Jamie Smith |
0:82ee5f7adbb0 | 418 | break; |
Jamie Smith |
0:82ee5f7adbb0 | 419 | case 8: |
Jamie Smith |
0:82ee5f7adbb0 | 420 | harness.test_stabilityClassifier(); |
Jamie Smith |
0:82ee5f7adbb0 | 421 | break; |
Jamie Smith |
0:82ee5f7adbb0 | 422 | case 9: |
Jamie Smith |
0:82ee5f7adbb0 | 423 | harness.test_metadata(); |
Jamie Smith |
0:82ee5f7adbb0 | 424 | break; |
Jamie Smith |
0:82ee5f7adbb0 | 425 | case 10: |
Jamie Smith |
0:82ee5f7adbb0 | 426 | harness.test_orientation(); |
Jamie Smith |
0:82ee5f7adbb0 | 427 | break; |
Jamie Smith |
0:82ee5f7adbb0 | 428 | case 11: |
Jamie Smith |
0:82ee5f7adbb0 | 429 | harness.test_disable(); |
Jamie Smith |
0:82ee5f7adbb0 | 430 | break; |
Jamie Smith |
0:82ee5f7adbb0 | 431 | case 12: |
Jamie Smith |
0:82ee5f7adbb0 | 432 | pc.printf("Exiting test suite.\r\n"); |
Jamie Smith |
0:82ee5f7adbb0 | 433 | return 0; |
Jamie Smith |
0:82ee5f7adbb0 | 434 | default: |
Jamie Smith |
0:82ee5f7adbb0 | 435 | printf("Invalid test number. Please run again.\r\n"); |
Jamie Smith |
0:82ee5f7adbb0 | 436 | return 1; |
Jamie Smith |
0:82ee5f7adbb0 | 437 | } |
Jamie Smith |
0:82ee5f7adbb0 | 438 | |
Jamie Smith |
0:82ee5f7adbb0 | 439 | pc.printf("Done!\r\n"); |
Jamie Smith |
0:82ee5f7adbb0 | 440 | return 0; |
Jamie Smith |
0:82ee5f7adbb0 | 441 | |
Jamie Smith |
0:82ee5f7adbb0 | 442 | } |
Jamie Smith |
0:82ee5f7adbb0 | 443 | |
Jamie Smith |
0:82ee5f7adbb0 | 444 | |
Jamie Smith |
0:82ee5f7adbb0 | 445 | |
Jamie Smith |
0:82ee5f7adbb0 | 446 | |
Jamie Smith |
0:82ee5f7adbb0 | 447 | |
Jamie Smith |
0:82ee5f7adbb0 | 448 | |
Jamie Smith |
0:82ee5f7adbb0 | 449 | |
Jamie Smith |
0:82ee5f7adbb0 | 450 | |
Jamie Smith |
0:82ee5f7adbb0 | 451 |