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;
+
+}
+
+
+
+
+
+
+
+
+