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;

}