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;