File content as of revision 3:488b7ef208be:

/* 
    USC RPL ADIS16467 Test Suite
    Contributors: Rita Yang
*/

#include "ADIS16467TestSuite.h"

#include <cinttypes>

// These pin assignments are arbitrary, you will need to change them accordingly
#define PIN_SPI_MOSI p20
#define PIN_SPI_MISO p21
#define PIN_SPI_SCK p22
#define PIN_ADIS_CS p23
#define PIN_ADIS_RST p24

Serial pc(USBTX, USBRX);

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");

	}

}