![](/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
Diff: BNOTestSuite.cpp
- Revision:
- 5:ee64252765de
- Parent:
- 4:85b98cc04a0a
--- a/BNOTestSuite.cpp Tue Jul 21 22:00:26 2020 -0700 +++ b/BNOTestSuite.cpp Tue Nov 24 15:27:32 2020 -0800 @@ -22,13 +22,20 @@ { const size_t update_rate_ms = 200; - imu.enableReport(BNO080::ROTATION, update_rate_ms); + imu.lockMutex(); + imu.enableReport(BNO080Base::ROTATION, update_rate_ms); + imu.unlockMutex(); while (true) { - ThisThread::sleep_for(1ms * update_rate_ms); + // note: this sleep here is important! + // It prevents the mutex from being locked 100% of the time, which + // would keep the IMU's thread from running. + ThisThread::sleep_for((1ms * update_rate_ms) / 2); - if(imu.updateData()) + imu.lockMutex(); + imu.updateData(); + if(imu.hasNewData(BNO080::ROTATION)) { TVector3 eulerRadians = imu.rotationVector.euler(); TVector3 eulerDegrees = eulerRadians * RAD_TO_DEG; @@ -38,27 +45,27 @@ pc.printf(", Accuracy: %.02f", (imu.rotationAccuracy * RAD_TO_DEG)); - pc.printf(", Status: %s\n", imu.getReportStatusString(BNO080::ROTATION)); + pc.printf(", Status: %s\n", imu.getReportStatusString(BNO080Base::ROTATION)); } - else - { - pc.printf("IMU was not ready with data packet!\r\n"); - } - } + imu.unlockMutex(); + } } void BNOTestSuite::test_readRotationAcceleration() { - imu.enableReport(BNO080::ROTATION, 1000); - imu.enableReport(BNO080::TOTAL_ACCELERATION, 500); + imu.lockMutex(); + imu.enableReport(BNO080Base::ROTATION, 1000); + imu.enableReport(BNO080Base::TOTAL_ACCELERATION, 500); + imu.unlockMutex(); while (true) { ThisThread::sleep_for(1ms); + imu.lockMutex(); if(imu.updateData()) { - if (imu.hasNewData(BNO080::ROTATION)) + if (imu.hasNewData(BNO080Base::ROTATION)) { pc.printf("IMU Rotation Euler: "); TVector3 eulerRadians = imu.rotationVector.euler(); @@ -66,23 +73,29 @@ eulerDegrees.print(pc, true); pc.printf("\n"); } - if (imu.hasNewData(BNO080::TOTAL_ACCELERATION)) + if (imu.hasNewData(BNO080Base::TOTAL_ACCELERATION)) { pc.printf("IMU Total Acceleration: "); imu.totalAcceleration.print(pc, true); pc.printf("\n"); } } + imu.unlockMutex(); } } void BNOTestSuite::test_tapDetector() { - imu.enableReport(BNO080::TAP_DETECTOR, 10); + imu.lockMutex(); + imu.enableReport(BNO080Base::TAP_DETECTOR, 10); + imu.unlockMutex(); + pc.printf("Listening for taps...\n"); while(true) { + ThisThread::sleep_for(1ms); + imu.lockMutex(); if(imu.updateData()) { if(imu.tapDetected) @@ -91,6 +104,7 @@ imu.tapDetected = false; } } + imu.unlockMutex(); } } @@ -98,12 +112,15 @@ { const size_t update_rate_ms = 200; - imu.enableReport(BNO080::GAME_ROTATION, update_rate_ms); + imu.lockMutex(); + imu.enableReport(BNO080Base::GAME_ROTATION, update_rate_ms); + imu.unlockMutex(); while (true) { - ThisThread::sleep_for(1ms * update_rate_ms); + ThisThread::sleep_for(1ms); + imu.lockMutex(); if(imu.updateData()) { pc.printf("IMU Game Rotation Euler: ["); @@ -111,19 +128,18 @@ TVector3 eulerDegrees = eulerRadians * RAD_TO_DEG; eulerDegrees.print(pc, true); - pc.printf("], Status: %s\n", imu.getReportStatusString(BNO080::GAME_ROTATION)); + pc.printf("], Status: %s\n", imu.getReportStatusString(BNO080Base::GAME_ROTATION)); } - else - { - pc.printf("IMU was not ready with data packet!\r\n"); - } + imu.unlockMutex(); } } void BNOTestSuite::test_tare() { const size_t update_rate_ms = 100; - imu.enableReport(BNO080::ROTATION, update_rate_ms); + imu.lockMutex(); + imu.enableReport(BNO080Base::ROTATION, update_rate_ms); + imu.unlockMutex(); Timer runningTime; runningTime.start(); @@ -132,8 +148,9 @@ while (true) { - ThisThread::sleep_for(1ms * update_rate_ms); + ThisThread::sleep_for(1ms); + imu.lockMutex(); if(imu.updateData()) { pc.printf("IMU Rotation Euler: ["); @@ -141,31 +158,31 @@ TVector3 eulerDegrees = eulerRadians * RAD_TO_DEG; eulerDegrees.print(pc, true); - pc.printf("], Status: %s\n", imu.getReportStatusString(BNO080::ROTATION)); - } - else - { - pc.printf("IMU was not ready with data packet!\r\n"); + pc.printf("], Status: %s\n", imu.getReportStatusString(BNO080Base::ROTATION)); } - if(runningTime.read() > 2 && !sentTareCmd) + if(runningTime.elapsed_time() > 2s && !sentTareCmd) { pc.printf("2 seconds have passed, sending tare command...\n"); imu.tare(); sentTareCmd = true; } + imu.unlockMutex(); } } void BNOTestSuite::test_magCalibration() { // enable calibration for the magnetometer + imu.lockMutex(); bool success = imu.enableCalibration(false, false, true); const size_t update_rate_ms = 200; - imu.enableReport(BNO080::GAME_ROTATION, update_rate_ms); - imu.enableReport(BNO080::MAG_FIELD, update_rate_ms); - imu.enableReport(BNO080::MAG_FIELD_UNCALIBRATED, update_rate_ms); + imu.enableReport(BNO080Base::GAME_ROTATION, update_rate_ms); + imu.enableReport(BNO080Base::MAG_FIELD, update_rate_ms); + imu.enableReport(BNO080Base::MAG_FIELD_UNCALIBRATED, update_rate_ms); + + imu.unlockMutex(); if(success) { @@ -179,14 +196,15 @@ while (true) { - ThisThread::sleep_for(1ms * update_rate_ms); + ThisThread::sleep_for(1ms); + imu.lockMutex(); if(imu.updateData()) { pc.printf("IMU Magnetic Field: ["); imu.magFieldUncalibrated.print(pc, true); - pc.printf("] uTesla, Status: %hhu (should be 2-3 when calibration is complete), ", imu.getReportStatus(BNO080::MAG_FIELD)); + pc.printf("] uTesla, Status: %hhu (should be 2-3 when calibration is complete), ", imu.getReportStatus(BNO080Base::MAG_FIELD)); pc.printf("Hard iron offsets: ["); imu.hardIronOffset.print(pc, true); @@ -194,73 +212,80 @@ pc.printf("]\n"); } - else - { - pc.printf("IMU was not ready with data packet!\r\n"); - } + imu.unlockMutex(); + } } void BNOTestSuite::test_stabilityClassifier() { - imu.enableReport(BNO080::STABILITY_CLASSIFIER, 200); + imu.lockMutex(); + imu.enableReport(BNO080Base::STABILITY_CLASSIFIER, 200); + imu.unlockMutex(); while (true) { ThisThread::sleep_for(1ms); + imu.lockMutex(); if(imu.updateData()) { - if (imu.hasNewData(BNO080::STABILITY_CLASSIFIER)) + if (imu.hasNewData(BNO080Base::STABILITY_CLASSIFIER)) { pc.printf("Stability: "); switch(imu.stability) { - case BNO080::UNKNOWN: + case BNO080Base::UNKNOWN: pc.printf("Unknown\n"); break; - case BNO080::ON_TABLE: + case BNO080Base::ON_TABLE: pc.printf("On Table\n"); break; - case BNO080::STATIONARY: + case BNO080Base::STATIONARY: pc.printf("Stationary\n"); break; - case BNO080::STABLE: + case BNO080Base::STABLE: pc.printf("Stable\n"); break; - case BNO080::MOTION: + case BNO080Base::MOTION: pc.printf("Motion\n"); break; } } } + imu.unlockMutex(); + } } void BNOTestSuite::test_metadata() { + imu.lockMutex(); pc.printf("Printing metadata for Linear Acceleration:\r\n"); - imu.printMetadataSummary(BNO080::LINEAR_ACCELERATION); + imu.printMetadataSummary(BNO080Base::LINEAR_ACCELERATION); pc.printf("Printing metadata for Rotation Vector:\r\n"); - imu.printMetadataSummary(BNO080::ROTATION); + imu.printMetadataSummary(BNO080Base::ROTATION); pc.printf("Printing metadata for Magnetic Field:\r\n"); - imu.printMetadataSummary(BNO080::MAG_FIELD); + imu.printMetadataSummary(BNO080Base::MAG_FIELD); pc.printf("Printing metadata for Tap Detector:\r\n"); - imu.printMetadataSummary(BNO080::TAP_DETECTOR); + imu.printMetadataSummary(BNO080Base::TAP_DETECTOR); + imu.unlockMutex(); } void BNOTestSuite::test_orientation() { const size_t update_rate_ms = 200; - imu.enableReport(BNO080::TOTAL_ACCELERATION, update_rate_ms); - imu.enableReport(BNO080::ROTATION, update_rate_ms); + imu.lockMutex(); + imu.enableReport(BNO080Base::TOTAL_ACCELERATION, update_rate_ms); + imu.enableReport(BNO080Base::ROTATION, update_rate_ms); + imu.unlockMutex(); Timer runningTime; runningTime.start(); @@ -269,11 +294,12 @@ while (true) { - ThisThread::sleep_for(1ms * update_rate_ms); + ThisThread::sleep_for(1ms); + imu.lockMutex(); if(imu.updateData()) { - if (imu.hasNewData(BNO080::ROTATION)) + if (imu.hasNewData(BNO080Base::ROTATION)) { pc.printf("IMU Rotation Euler: "); TVector3 eulerRadians = imu.rotationVector.euler(); @@ -281,7 +307,7 @@ eulerDegrees.print(pc, true); pc.printf("\n"); } - if (imu.hasNewData(BNO080::TOTAL_ACCELERATION)) + if (imu.hasNewData(BNO080Base::TOTAL_ACCELERATION)) { pc.printf("IMU Total Acceleration: "); imu.totalAcceleration.print(pc, true); @@ -289,7 +315,7 @@ } } - if(runningTime.read() > 2 && !setOrientation) + if(runningTime.elapsed_time() > 2s && !setOrientation) { pc.printf("2 seconds have passed, sending set orientation command...\n"); setOrientation = true; @@ -298,6 +324,7 @@ // (values from BNO datasheet page 41) imu.setSensorOrientation(Quaternion(0, -1, 0, 0)); } + imu.unlockMutex(); } } @@ -314,21 +341,25 @@ // rotate sensor 180 degrees about Y axis //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 // see page 5 and 11 of hillcrestlabs FSM30X datasheet - imu.setPermanentOrientation(Quaternion(0,-1, 0, 0)); - + imu.lockMutex(); + imu.setPermanentOrientation(Quaternion(0,-1, 0, 0)); + imu.unlockMutex(); + // reset to default orientation //imu.setPermanentOrientation(Quaternion(0, 0, 0, 0)); orientationTimer.stop(); - pc.printf("Done setting orientation (took %.03f seconds)\r\n", orientationTimer.read()); + pc.printf("Done setting orientation (took %.03f seconds)\r\n", std::chrono::duration<float>{orientationTimer.elapsed_time()}.count()); } void BNOTestSuite::test_disable() { const size_t update_rate_ms = 200; - imu.enableReport(BNO080::TOTAL_ACCELERATION, update_rate_ms); - imu.enableReport(BNO080::ROTATION, update_rate_ms); + imu.lockMutex(); + imu.enableReport(BNO080Base::TOTAL_ACCELERATION, update_rate_ms); + imu.enableReport(BNO080Base::ROTATION, update_rate_ms); + imu.unlockMutex(); Timer runningTime; runningTime.start(); @@ -339,9 +370,10 @@ { ThisThread::sleep_for(1ms * update_rate_ms); + imu.lockMutex(); if(imu.updateData()) { - if (imu.hasNewData(BNO080::ROTATION)) + if (imu.hasNewData(BNO080Base::ROTATION)) { pc.printf("IMU Rotation Euler: "); TVector3 eulerRadians = imu.rotationVector.euler(); @@ -349,7 +381,7 @@ eulerDegrees.print(pc, true); pc.printf("\n"); } - if (imu.hasNewData(BNO080::TOTAL_ACCELERATION)) + if (imu.hasNewData(BNO080Base::TOTAL_ACCELERATION)) { pc.printf("IMU Total Acceleration: "); imu.totalAcceleration.print(pc, true); @@ -357,23 +389,26 @@ } } - if(runningTime.read() > 2 && !disabledRotation) + if(runningTime.elapsed_time() > 2s && !disabledRotation) { pc.printf("2 seconds have passed, disabling rotation report...\n"); disabledRotation = true; - imu.disableReport(BNO080::ROTATION); + imu.disableReport(BNO080Base::ROTATION); } + imu.unlockMutex(); } } void BNOTestSuite::test_accelCalibration() { // enable calibration for the magnetometer + imu.lockMutex(); bool success = imu.enableCalibration(true, false, false); const size_t update_rate_ms = 200; - imu.enableReport(BNO080::Report::TOTAL_ACCELERATION, update_rate_ms); + imu.enableReport(BNO080Base::Report::TOTAL_ACCELERATION, update_rate_ms); + imu.unlockMutex(); if(success) { @@ -387,20 +422,17 @@ while (true) { - ThisThread::sleep_for(1ms * update_rate_ms); + ThisThread::sleep_for(1ms); + imu.lockMutex(); if(imu.updateData()) { pc.printf("IMU Total Acceleration: ["); imu.totalAcceleration.print(pc, true); - pc.printf("] m/s^2, Status: %hhu (should be 2-3 when calibration is complete), ", imu.getReportStatus(BNO080::Report::TOTAL_ACCELERATION)); + pc.printf("] m/s^2, Status: %hhu (should be 2-3 when calibration is complete), ", imu.getReportStatus(BNO080Base::Report::TOTAL_ACCELERATION)); pc.printf("]\r\n"); } - else - { - pc.printf("IMU was not ready with data packet!\r\n"); - } if(serial.readable()) { @@ -414,26 +446,15 @@ pc.getc(); } } + imu.unlockMutex(); } } int main() { - pc.printf("============================================================\n"); - - // reset and connect to IMU - while(!imu.begin()) - { - pc.printf("Failed to connect to IMU!\r\n"); - ThisThread::sleep_for(500); - } - - //Declare test harness - BNOTestSuite harness; - int test = -1; - pc.printf("\r\n\nHamster BNO Test Suite:\r\n"); + pc.printf("\r\n\nBNO080 Test Suite:\r\n"); //MENU. ADD OPTION PER EACH TEST pc.printf("Select a test: \n\r"); @@ -454,6 +475,17 @@ pc.scanf("%d", &test); pc.printf("Running test %d:\r\n\n", test); + pc.printf("============================================================\n"); + + // reset and connect to IMU + while(!imu.begin()) + { + pc.printf("Failed to connect to IMU!\r\n"); + ThisThread::sleep_for(500ms); + } + + //Declare test harness + BNOTestSuite harness; //SWITCH. ADD CASE PER EACH TEST switch(test){ @@ -493,6 +525,9 @@ case 12: harness.test_disable(); break; + case 13: + harness.test_accelCalibration(); + break; default: printf("Invalid test number. Please run again.\r\n"); return 1;