Eduardo Vergara
/
BNO080-Examples
Example/test programs for my BNO080 driver.
Diff: BNOTestSuite.cpp
- Revision:
- 0:82ee5f7adbb0
- Child:
- 1:14c135ea7134
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/BNOTestSuite.cpp Sat Dec 29 04:14:43 2018 -0800 @@ -0,0 +1,451 @@ +/* + USC RPL HAMSTER v2.3 + Contributors: Lauren Potterat +*/ + + +#include "BNOTestSuite.h" + +#define RAD_TO_DEG (180.0/M_PI) + +void BNOTestSuite::test_printInfo() +{ + pc.printf("BNO080 reports as SW version %hhu.%hhu.%hu, build %lu, part no. %lu\n", + imu.majorSoftwareVersion, imu.minorSoftwareVersion, imu.patchSoftwareVersion, + imu.buildNumber, imu.partNumber); +} + + +void BNOTestSuite::test_readRotationVector() +{ + const size_t update_rate_ms = 200; + + imu.enableReport(BNO080::ROTATION, update_rate_ms); + + while (true) + { + // note: for some reason it seems like wait() here waits 10x the time you tell it to + // not sure what's causing this behavior, and I don't see it with mbed os 2 + wait(.0001f * update_rate_ms); + + if(imu.updateData()) + { + 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(BNO080::ROTATION)); + } + else + { + pc.printf("IMU was not ready with data packet!\r\n"); + } + } +} + +void BNOTestSuite::test_readRotationAcceleration() +{ + imu.enableReport(BNO080::ROTATION, 1000); + imu.enableReport(BNO080::TOTAL_ACCELERATION, 500); + + while (true) + { + wait(.001f); + + if(imu.updateData()) + { + if (imu.hasNewData(BNO080::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(BNO080::TOTAL_ACCELERATION)) + { + pc.printf("IMU Total Acceleration: "); + imu.totalAcceleration.print(pc, true); + pc.printf("\n"); + } + } + } +} + +void BNOTestSuite::test_tapDetector() +{ + imu.enableReport(BNO080::TAP_DETECTOR, 10); + pc.printf("Listening for taps...\n"); + + while(true) + { + if(imu.updateData()) + { + if(imu.tapDetected) + { + pc.printf("Tap detected!\n"); + imu.tapDetected = false; + } + } + } +} + +void BNOTestSuite::test_gameRotationVector() +{ + const size_t update_rate_ms = 200; + + imu.enableReport(BNO080::GAME_ROTATION, update_rate_ms); + + while (true) + { + // note: for some reason it seems like wait() here waits 10x the time you tell it to + // not sure what's causing this behavior, and I don't see it with mbed os 2 + wait(.0001f * update_rate_ms); + + 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(BNO080::GAME_ROTATION)); + } + else + { + pc.printf("IMU was not ready with data packet!\r\n"); + } + } +} + +void BNOTestSuite::test_tare() +{ + const size_t update_rate_ms = 100; + imu.enableReport(BNO080::ROTATION, update_rate_ms); + + Timer runningTime; + runningTime.start(); + + bool sentTareCmd = false; + + while (true) + { + // note: for some reason it seems like wait() here waits 10x the time you tell it to + // not sure what's causing this behavior, and I don't see it with mbed os 2 + wait(.0001f * update_rate_ms); + + 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(BNO080::ROTATION)); + } + else + { + pc.printf("IMU was not ready with data packet!\r\n"); + } + + if(runningTime.read() > 2 && !sentTareCmd) + { + pc.printf("2 seconds have passed, sending tare command...\n"); + imu.tare(); + sentTareCmd = true; + } + } +} + +void BNOTestSuite::test_magCalibration() +{ + // enable calibration for the magnetometer + 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); + + if(success) + { + pc.printf("Calibration mode was successfully enabled!\n"); + } + else + { + pc.printf("Calibration mode failed to enable!\n"); + while(true){} + } + + while (true) + { + // note: for some reason it seems like wait() here waits 10x the time you tell it to + // not sure what's causing this behavior, and I don't see it with mbed os 2 + wait(.0001f * update_rate_ms); + + 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("Hard iron offsets: ["); + imu.hardIronOffset.print(pc, true); + + pc.printf("]\n"); + + } + else + { + pc.printf("IMU was not ready with data packet!\r\n"); + } + } + +} + +void BNOTestSuite::test_stabilityClassifier() +{ + imu.enableReport(BNO080::STABILITY_CLASSIFIER, 200); + + while (true) + { + wait(.001f); + + if(imu.updateData()) + { + if (imu.hasNewData(BNO080::STABILITY_CLASSIFIER)) + { + pc.printf("Stability: "); + + switch(imu.stability) + { + case BNO080::UNKNOWN: + pc.printf("Unknown\n"); + break; + case BNO080::ON_TABLE: + pc.printf("On Table\n"); + break; + case BNO080::STATIONARY: + pc.printf("Stationary\n"); + break; + case BNO080::STABLE: + pc.printf("Stable\n"); + break; + case BNO080::MOTION: + pc.printf("Motion\n"); + break; + } + } + } + } + +} + +void BNOTestSuite::test_metadata() +{ + pc.printf("Printing metadata for Linear Acceleration:\r\n"); + imu.printMetadataSummary(BNO080::LINEAR_ACCELERATION); + + pc.printf("Printing metadata for Rotation Vector:\r\n"); + imu.printMetadataSummary(BNO080::ROTATION); + + pc.printf("Printing metadata for Magnetic Field:\r\n"); + imu.printMetadataSummary(BNO080::MAG_FIELD); + + pc.printf("Printing metadata for Tap Detector:\r\n"); + imu.printMetadataSummary(BNO080::TAP_DETECTOR); + +} + +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); + + Timer runningTime; + runningTime.start(); + + bool setOrientation = false; + + while (true) + { + // note: for some reason it seems like wait() here waits 10x the time you tell it to + // not sure what's causing this behavior, and I don't see it with mbed os 2 + wait(.0001f * update_rate_ms); + + if(imu.updateData()) + { + if (imu.hasNewData(BNO080::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(BNO080::TOTAL_ACCELERATION)) + { + pc.printf("IMU Total Acceleration: "); + imu.totalAcceleration.print(pc, true); + pc.printf("\n"); + } + } + + if(runningTime.read() > 2 && !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)); + } + } +} + +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); + + Timer runningTime; + runningTime.start(); + + bool disabledRotation = false; + + while (true) + { + // note: for some reason it seems like wait() here waits 10x the time you tell it to + // not sure what's causing this behavior, and I don't see it with mbed os 2 + wait(.0001f * update_rate_ms); + + if(imu.updateData()) + { + if (imu.hasNewData(BNO080::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(BNO080::TOTAL_ACCELERATION)) + { + pc.printf("IMU Total Acceleration: "); + imu.totalAcceleration.print(pc, true); + pc.printf("\n"); + } + } + + if(runningTime.read() > 2 && !disabledRotation) + { + pc.printf("2 seconds have passed, disabling rotation report...\n"); + disabledRotation = true; + + imu.disableReport(BNO080::ROTATION); + } + } +} + +int main() +{ + pc.baud(115200); + + pc.printf("============================================================\n"); + + + // reset and connect to IMU + while(!imu.begin()) + { + pc.printf("Failed to connect to IMU!\r\n"); + wait(.5); + } + + //Declare test harness + BNOTestSuite harness; + + int test = -1; + pc.printf("\r\n\nHamster BNO 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 disabling reports\r\n"); + pc.printf("12. Exit test suite\r\n"); + + pc.scanf("%d", &test); + pc.printf("Running test %d:\r\n\n", test); + + + //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_disable(); + break; + case 12: + pc.printf("Exiting test suite.\r\n"); + return 0; + default: + printf("Invalid test number. Please run again.\r\n"); + return 1; + } + + pc.printf("Done!\r\n"); + return 0; + +} + + + + + + + + +