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
- Committer:
- Jamie Smith
- Date:
- 2020-11-24
- Revision:
- 5:ee64252765de
- Parent:
- 4:85b98cc04a0a
File content as of revision 5:ee64252765de:
/* USC RPL HAMSTER v2.3 Contributors: Lauren Potterat */ #include "BNOTestSuite.h" #include <cinttypes> #define RAD_TO_DEG (180.0/M_PI) void BNOTestSuite::test_printInfo() { pc.printf("BNO080 reports as SW version %" PRIu8 ".%" PRIu8 ".%" PRIu16", build %" PRIu32 ", part no. %" PRIu32"\n", imu.majorSoftwareVersion, imu.minorSoftwareVersion, imu.patchSoftwareVersion, imu.buildNumber, imu.partNumber); } void BNOTestSuite::test_readRotationVector() { const size_t update_rate_ms = 200; imu.lockMutex(); imu.enableReport(BNO080Base::ROTATION, update_rate_ms); imu.unlockMutex(); while (true) { // 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); imu.lockMutex(); imu.updateData(); if(imu.hasNewData(BNO080::ROTATION)) { TVector3 eulerRadians = imu.rotationVector.euler(); TVector3 eulerDegrees = eulerRadians * RAD_TO_DEG; pc.printf("IMU Rotation Euler: Roll: %f deg, Pitch: %f deg, Yaw: %f deg \r\n", eulerDegrees[0], eulerDegrees[1], eulerDegrees[2]); pc.printf(", Accuracy: %.02f", (imu.rotationAccuracy * RAD_TO_DEG)); pc.printf(", Status: %s\n", imu.getReportStatusString(BNO080Base::ROTATION)); } imu.unlockMutex(); } } void BNOTestSuite::test_readRotationAcceleration() { 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(BNO080Base::ROTATION)) { pc.printf("IMU Rotation Euler: "); TVector3 eulerRadians = imu.rotationVector.euler(); TVector3 eulerDegrees = eulerRadians * (180.0 / M_PI); eulerDegrees.print(pc, true); pc.printf("\n"); } 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.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) { pc.printf("Tap detected!\n"); imu.tapDetected = false; } } imu.unlockMutex(); } } void BNOTestSuite::test_gameRotationVector() { const size_t update_rate_ms = 200; imu.lockMutex(); imu.enableReport(BNO080Base::GAME_ROTATION, update_rate_ms); imu.unlockMutex(); while (true) { ThisThread::sleep_for(1ms); imu.lockMutex(); if(imu.updateData()) { pc.printf("IMU Game Rotation Euler: ["); TVector3 eulerRadians = imu.gameRotationVector.euler(); TVector3 eulerDegrees = eulerRadians * RAD_TO_DEG; eulerDegrees.print(pc, true); pc.printf("], Status: %s\n", imu.getReportStatusString(BNO080Base::GAME_ROTATION)); } imu.unlockMutex(); } } void BNOTestSuite::test_tare() { const size_t update_rate_ms = 100; imu.lockMutex(); imu.enableReport(BNO080Base::ROTATION, update_rate_ms); imu.unlockMutex(); Timer runningTime; runningTime.start(); bool sentTareCmd = false; while (true) { ThisThread::sleep_for(1ms); imu.lockMutex(); if(imu.updateData()) { pc.printf("IMU Rotation Euler: ["); TVector3 eulerRadians = imu.rotationVector.euler(); TVector3 eulerDegrees = eulerRadians * RAD_TO_DEG; eulerDegrees.print(pc, true); pc.printf("], Status: %s\n", imu.getReportStatusString(BNO080Base::ROTATION)); } 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(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) { pc.printf("Calibration mode was successfully enabled!\n"); } else { pc.printf("Calibration mode failed to enable!\n"); while(true){} } while (true) { 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(BNO080Base::MAG_FIELD)); pc.printf("Hard iron offsets: ["); imu.hardIronOffset.print(pc, true); pc.printf("]\n"); } imu.unlockMutex(); } } void BNOTestSuite::test_stabilityClassifier() { imu.lockMutex(); imu.enableReport(BNO080Base::STABILITY_CLASSIFIER, 200); imu.unlockMutex(); while (true) { ThisThread::sleep_for(1ms); imu.lockMutex(); if(imu.updateData()) { if (imu.hasNewData(BNO080Base::STABILITY_CLASSIFIER)) { pc.printf("Stability: "); switch(imu.stability) { case BNO080Base::UNKNOWN: pc.printf("Unknown\n"); break; case BNO080Base::ON_TABLE: pc.printf("On Table\n"); break; case BNO080Base::STATIONARY: pc.printf("Stationary\n"); break; case BNO080Base::STABLE: pc.printf("Stable\n"); break; 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(BNO080Base::LINEAR_ACCELERATION); pc.printf("Printing metadata for Rotation Vector:\r\n"); imu.printMetadataSummary(BNO080Base::ROTATION); pc.printf("Printing metadata for Magnetic Field:\r\n"); imu.printMetadataSummary(BNO080Base::MAG_FIELD); pc.printf("Printing metadata for Tap Detector:\r\n"); imu.printMetadataSummary(BNO080Base::TAP_DETECTOR); imu.unlockMutex(); } void BNOTestSuite::test_orientation() { const size_t update_rate_ms = 200; imu.lockMutex(); imu.enableReport(BNO080Base::TOTAL_ACCELERATION, update_rate_ms); imu.enableReport(BNO080Base::ROTATION, update_rate_ms); imu.unlockMutex(); Timer runningTime; runningTime.start(); bool setOrientation = false; while (true) { ThisThread::sleep_for(1ms); imu.lockMutex(); if(imu.updateData()) { if (imu.hasNewData(BNO080Base::ROTATION)) { pc.printf("IMU Rotation Euler: "); TVector3 eulerRadians = imu.rotationVector.euler(); TVector3 eulerDegrees = eulerRadians * (180.0 / M_PI); eulerDegrees.print(pc, true); pc.printf("\n"); } if (imu.hasNewData(BNO080Base::TOTAL_ACCELERATION)) { pc.printf("IMU Total Acceleration: "); imu.totalAcceleration.print(pc, true); pc.printf("\n"); } } if(runningTime.elapsed_time() > 2s && !setOrientation) { pc.printf("2 seconds have passed, sending set orientation command...\n"); setOrientation = true; // rotate sensor about the Y axis, so Z points down and X points west. // (values from BNO datasheet page 41) imu.setSensorOrientation(Quaternion(0, -1, 0, 0)); } imu.unlockMutex(); } } void BNOTestSuite::test_permanentOrientation() { pc.printf("Setting permanent sensor orientation...\n"); Timer orientationTimer; orientationTimer.start(); // rotate sensor 90 degrees CCW about the Y axis, so X points up, Y points back, and Z points out // (values from BNO datasheet page 41) // imu.setPermanentOrientation(Quaternion(1.0f/SQRT_2, -1.0f/SQRT_2, 0, 0)); // 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.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", std::chrono::duration<float>{orientationTimer.elapsed_time()}.count()); } void BNOTestSuite::test_disable() { const size_t update_rate_ms = 200; imu.lockMutex(); imu.enableReport(BNO080Base::TOTAL_ACCELERATION, update_rate_ms); imu.enableReport(BNO080Base::ROTATION, update_rate_ms); imu.unlockMutex(); Timer runningTime; runningTime.start(); bool disabledRotation = false; while (true) { ThisThread::sleep_for(1ms * update_rate_ms); imu.lockMutex(); if(imu.updateData()) { if (imu.hasNewData(BNO080Base::ROTATION)) { pc.printf("IMU Rotation Euler: "); TVector3 eulerRadians = imu.rotationVector.euler(); TVector3 eulerDegrees = eulerRadians * (180.0 / M_PI); eulerDegrees.print(pc, true); pc.printf("\n"); } if (imu.hasNewData(BNO080Base::TOTAL_ACCELERATION)) { pc.printf("IMU Total Acceleration: "); imu.totalAcceleration.print(pc, true); pc.printf("\n"); } } if(runningTime.elapsed_time() > 2s && !disabledRotation) { pc.printf("2 seconds have passed, disabling rotation report...\n"); disabledRotation = true; 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(BNO080Base::Report::TOTAL_ACCELERATION, update_rate_ms); imu.unlockMutex(); if(success) { pc.printf("Calibration mode was successfully enabled!\r\n"); } else { pc.printf("Calibration mode failed to enable!\r\n"); while(true){} } while (true) { 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(BNO080Base::Report::TOTAL_ACCELERATION)); pc.printf("]\r\n"); } if(serial.readable()) { // char typed over serial pc.printf("Saving current calibration...\r\n"); imu.saveCalibration(); // throw out rest of chars in serial buffer while(serial.readable()) { pc.getc(); } } imu.unlockMutex(); } } int main() { int test = -1; pc.printf("\r\n\nBNO080 Test Suite:\r\n"); //MENU. ADD OPTION PER EACH TEST pc.printf("Select a test: \n\r"); pc.printf("1. Print chip info\r\n"); pc.printf("2. Display rotation vector\r\n"); pc.printf("3. Display rotation and acceleration\r\n"); pc.printf("4. Run tap detector\r\n"); pc.printf("5. Display game rotation vector\r\n"); pc.printf("6. Test tare function\r\n"); pc.printf("7. Run magnetometer calibration\r\n"); pc.printf("8. Test stability classifier\r\n"); pc.printf("9. Test metadata reading (requires BNO_DEBUG = 1)\r\n"); pc.printf("10. Test set orientation\r\n"); pc.printf("11. Test set permanent orientation\r\n"); pc.printf("12. Test disabling reports\r\n"); pc.printf("13. Test accelerometer calibration\r\n"); 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){ case 1: harness.test_printInfo(); break; case 2: harness.test_readRotationVector(); break; case 3: harness.test_readRotationAcceleration(); break; case 4: harness.test_tapDetector(); break; case 5: harness.test_gameRotationVector(); break; case 6: harness.test_tare(); break; case 7: harness.test_magCalibration(); break; case 8: harness.test_stabilityClassifier(); break; case 9: harness.test_metadata(); break; case 10: harness.test_orientation(); break; case 11: harness.test_permanentOrientation(); break; case 12: harness.test_disable(); break; case 13: harness.test_accelCalibration(); break; default: printf("Invalid test number. Please run again.\r\n"); return 1; } pc.printf("Done!\r\n"); return 0; }