Diff: ADIS16467TestSuite.cpp

Revision:

0:38da6269ed5c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ADIS16467TestSuite.cpp	Fri Feb 21 00:36:41 2020 -0800
@@ -0,0 +1,260 @@
+/* 
+    USC RPL ADIS16467 Test Suite
+    Contributors: Rita Yang
+*/
+
+#include "ADIS16467TestSuite.h"
+
+#include <cinttypes>
+
+const char* statusToString(bool status)
+{
+    if(status)
+    {
+        return "Yes";
+    }
+    else
+    {
+        return "No";
+    }
+}
+
+void ADIS16467TestSuite::test_readData() {
+	while(1) {
+		wait_ms(600);
+
+		float gyroX = (float)(imu->readGyroX()) * imu->GYRO_CONV;
+		float gyroY = (float)(imu->readGyroY()) * imu->GYRO_CONV;
+		float gyroZ = (float)(imu->readGyroZ()) * imu->GYRO_CONV;
+		float accelX = (float)(imu->readAccelX()) * imu->ACCEL_CONV;
+		float accelY = (float)(imu->readAccelY()) * imu->ACCEL_CONV;
+		float accelZ = (float)(imu->readAccelZ()) * imu->ACCEL_CONV;
+		float intTemp = (float)(imu->readInternalTemp()) * imu->TEMP_CONV;
+		uint16_t dataCount = imu->readDataCounter();
+
+		pc.printf("Gyro X : %.2f degree/sec\r\n", gyroX);
+		pc.printf("Gyro Y : %.2f degree/sec\r\n", gyroY);
+		pc.printf("Gyro Z : %.2f degree/sec\r\n", gyroZ);
+		pc.printf("Accel X : %.2f mg\r\n", accelX);
+		pc.printf("Accel Y : %.2f mg\r\n", accelY);
+		pc.printf("Accel Z : %.2f mg\r\n", accelZ);
+		pc.printf("Internal Temperature : %.2f C\r\n", intTemp);
+		pc.printf("Data Counter : %5d\r\n", dataCount);
+		pc.printf("\r\n");
+	}
+}
+
+void ADIS16467TestSuite::test_printFlags() {
+	pc.printf("-----Error Flags-----\r\n");
+	pc.printf("Clock Error : %s\r\n", statusToString(imu->getClockError()));
+	pc.printf("Memory Failure : %s\r\n", statusToString(imu->getMemFailure()));
+	pc.printf("Sensor Failure : %s\r\n", statusToString(imu->getSensorFailure()));
+	pc.printf("Standby Mode : %s\r\n", statusToString(imu->getStandbyMode()));
+	pc.printf("SPI Communication Error : %s\r\n", statusToString(imu->getSPIError()));
+	pc.printf("Flash Memory Update Failure : %s\r\n", statusToString(imu->getFlashUpFail()));
+	pc.printf("Datapath Overrun : %s\r\n", statusToString(imu->getDatapathOverrun()));
+}
+
+void ADIS16467TestSuite::test_burstRead() {
+	struct ADIS16467::BurstReadResult result;
+	imu->burstRead(result);
+
+	pc.printf("Gyro X : %.2f degree/sec\r\n", ((float)result.gyroX * imu->GYRO_CONV));
+	pc.printf("Gyro Y : %.2f degree/sec\r\n", ((float)result.gyroY * imu->GYRO_CONV));
+	pc.printf("Gyro Z : %.2f degree/sec\r\n", ((float)result.gyroZ * imu->GYRO_CONV));
+	pc.printf("Accel X : %.2f mg\r\n", ((float)result.accelX * imu->ACCEL_CONV));
+	pc.printf("Accel Y : %.2f mg\r\n", ((float)result.accelY * imu->ACCEL_CONV));
+	pc.printf("Accel Z : %.2f mg\r\n", ((float)result.accelZ * imu->ACCEL_CONV));
+	pc.printf("Internal Temperature : %.2f C\r\n", ((float)result.internalTemp * imu->TEMP_CONV));
+	pc.printf("Data Counter : %5d\r\n", result.dataCounter);
+	pc.printf("Checksum Value : %5d\r\n", result.checkSum);
+
+	imu->setErrorFlags(result.statusDiag);
+	test_printFlags();
+	imu->resetAllFlags();
+}
+
+void ADIS16467TestSuite::test_readFlags() {
+	imu->setErrorFlags(imu->readStatDiag());
+	test_printFlags();
+	imu->resetAllFlags();
+}
+
+void ADIS16467TestSuite::test_sensorSelfTest() {
+	imu->sensorSelfTest();
+	test_printFlags();
+	imu->resetAllFlags();
+}
+
+void ADIS16467TestSuite::test_setPoll(uint16_t pollRate)
+{
+	const float print_time = 1.0f;
+
+	imu->setPollRate(pollRate);
+	imu->resetDeltaAngle();
+
+	Timer printTimer;
+	printTimer.start();
+
+	while(1)
+	{
+
+		wait(1/ static_cast<float>(pollRate * 2));
+
+		imu->updateDeltaAngles();
+
+		if(printTimer.read() > print_time)
+		{
+			printTimer.reset();
+
+			float gyroX = (float)(imu->readGyroX()) * imu->GYRO_CONV;
+			float gyroY = (float)(imu->readGyroY()) * imu->GYRO_CONV;
+			float gyroZ = (float)(imu->readGyroZ()) * imu->GYRO_CONV;
+			float accelX = (float)(imu->readAccelX()) * imu->ACCEL_CONV;
+			float accelY = (float)(imu->readAccelY()) * imu->ACCEL_CONV;
+			float accelZ = (float)(imu->readAccelZ()) * imu->ACCEL_CONV;
+			float intTemp = (float)(imu->readInternalTemp()) * imu->TEMP_CONV;
+			float timeStamp = (float)(imu->readTimeStamp()) * imu->TIME_CONV;
+			uint16_t dataCount = imu->readDataCounter();
+			float deltaAngXSum = (float)(imu->getDeltaAngleXSum()) * imu->DELTA_ANGLE_CONV;
+			float deltaAngYSum = (float)(imu->getDeltaAngleYSum()) * imu->DELTA_ANGLE_CONV;
+			float deltaAngZSum = (float)(imu->getDeltaAngleZSum()) * imu->DELTA_ANGLE_CONV;
+
+			pc.printf("Gyro X : %.2f degree/sec\r\n", gyroX);
+			pc.printf("Gyro Y : %.2f degree/sec\r\n", gyroY);
+			pc.printf("Gyro Z : %.2f degree/sec\r\n", gyroZ);
+			pc.printf("Accel X : %.2f mg\r\n", accelX);
+			pc.printf("Accel Y : %.2f mg\r\n", accelY);
+			pc.printf("Accel Z : %.2f mg\r\n", accelZ);
+			pc.printf("Delta Angle X Sum: %.2f degree\r\n", deltaAngXSum);
+			pc.printf("Delta Angle Y Sum: %.2f degree\r\n", deltaAngYSum);
+			pc.printf("Delta Angle Z Sum: %.2f degree\r\n", deltaAngZSum);
+			pc.printf("Internal Temperature : %.2f C\r\n", intTemp);
+			pc.printf("Time Stamp : %.2f us\r\n", timeStamp);
+			pc.printf("Data Counter : %5d\r\n", dataCount);
+			pc.printf("\r\n");
+		}
+
+	}
+}
+
+void ADIS16467TestSuite::test_calibrateBias()
+{
+	const uint16_t pollRate = 10; // Hz
+	const float testDuration = 60; // s
+
+	imu->setPollRate(pollRate);
+	imu->resetDeltaAngle();
+
+	pc.printf("Place the IMU on a flat surface and keep it still.  Orientation is not important.\r\n");
+
+	wait(5);
+
+
+	Timer testTimer;
+	testTimer.start();
+
+	while(testTimer.read() < testDuration)
+	{
+
+		wait(1/ static_cast<float>(pollRate * 2));
+		imu->updateDeltaAngles();
+
+		float deltaAngXSum = (float)(imu->getDeltaAngleXSum()) * imu->DELTA_ANGLE_CONV;
+		float deltaAngYSum = (float)(imu->getDeltaAngleYSum()) * imu->DELTA_ANGLE_CONV;
+		float deltaAngZSum = (float)(imu->getDeltaAngleZSum()) * imu->DELTA_ANGLE_CONV;
+
+		pc.printf("X Drift: %.2f degree\r\n", deltaAngXSum);
+		pc.printf("Y Drift: %.2f degree\r\n", deltaAngYSum);
+		pc.printf("Z Drift: %.2f degree\r\n", deltaAngZSum);
+	}
+
+	float timeElapsed = testTimer.read();
+
+	float driftRateX = (float)(imu->getDeltaAngleXSum()) * imu->DELTA_ANGLE_CONV / timeElapsed;
+	float driftRateY = (float)(imu->getDeltaAngleYSum()) * imu->DELTA_ANGLE_CONV / timeElapsed;
+	float driftRateZ = (float)(imu->getDeltaAngleZSum()) * imu->DELTA_ANGLE_CONV / timeElapsed;
+
+	pc.printf("To zero X gyro, set bias to: %f degrees/sec\r\n", -1 * driftRateX);
+	pc.printf("To zero Y gyro, set bias to: %f degrees/sec\r\n", -1 * driftRateY);
+	pc.printf("To zero Z gyro, set bias to: %f degrees/sec\r\n", -1 * driftRateZ);
+
+}
+
+void ADIS16467TestSuite::test_existence()
+{
+	pc.printf("The IMU is connected and working: %s\r\n", imu->checkExistence() ? "True" : "False");
+}
+
+void ADIS16467TestSuite::test_firmInfo() {
+	struct ADIS16467::FirmwareInfo data;
+	imu->getFirmwareInformation(data);
+
+	pc.printf("Firmware Revision: %" PRIu8 ".%" PRIu8"\r\n", data.firmRevMajor, data.firmRevMinor);
+	pc.printf("Firmware Revision Date: %" PRIu8 "/%" PRIu8 "/%" PRIu16 "\r\n", data.firmwareMonth, data.firmwareDay, data.firmwareYear);
+	pc.printf("Serial Number: 0x%" PRIx16 "\r\n", data.serialNum);
+}
+
+
+int main() {
+
+	while(true)
+	{
+		pc.printf("\r\nADIS16467 IMU Test Suite:\r\n");
+
+		ADIS16467::ADIS16467 imuModule = ADIS16467::ADIS16467(&pc, PIN_SPI_MOSI, PIN_SPI_MISO, PIN_SPI_SCK,
+															  PIN_ADIS_CS, PIN_ADIS_RST);
+    
+		ADIS16467TestSuite harness;
+		harness.imu = &imuModule;
+
+		harness.imu->initADIS();
+
+		int test = -1;
+
+		//Menu
+		pc.printf("Select a test: \n\r");
+		pc.printf("1.  Read Data\r\n");
+		pc.printf("2.  Read Error Flags\r\n");
+		pc.printf("3.  Burst Read\r\n");
+		pc.printf("4.  Sensor Self Test\r\n");
+		pc.printf("5.  Set Poll Rate\r\n");
+		pc.printf("6.  Calibrate Bias\r\n");
+		pc.printf("7.  Existence Test\r\n");
+		pc.printf("8.  Get Firmware Information\r\n");
+		pc.printf("9.  Exit Test Suite\r\n");
+
+		pc.scanf("%d", &test);
+		pc.printf("Running test %d:\r\n\n", test);
+
+		harness.imu->setGyroBiases(-0.11594f, 0.258192f, 0.272063f);
+
+		//Run Tests
+		switch(test) {
+			case 1 : harness.test_readData(); break;
+			case 2 : harness.test_readFlags(); break;
+			case 3 : harness.test_burstRead(); break;
+			case 4 : harness.test_sensorSelfTest(); break;
+			case 5 :
+			{
+				int rate = -1;
+				pc.printf("Input poll rate: ");
+				pc.scanf("%d", &rate);
+				harness.test_setPoll(rate);
+			}
+			case 6 : harness.test_calibrateBias(); break;
+			case 7 : harness.test_existence(); break;
+			case 8 : harness.test_firmInfo(); break;
+			case 9 : printf("Exiting Test Suite.\r\n"); return 0;
+			default : printf("Invalid Test Number Selection. Please Run Again.\r\n"); return 1;
+		}
+
+		pc.printf("Done.\r\n");
+
+	}
+
+}
+
+
+
+