Rita Yang
Example test suite for my ADIS16467 driver.
Embed:
(wiki syntax)
Show/hide line numbers
ADIS16467TestSuite.cpp
00001 /* 00002 USC RPL ADIS16467 Test Suite 00003 Contributors: Rita Yang 00004 */ 00005 00006 #include "ADIS16467TestSuite.h" 00007 00008 #include <cinttypes> 00009 00010 // These pin assignments are arbitrary, you will need to change them accordingly 00011 #define PIN_SPI_MOSI p20 00012 #define PIN_SPI_MISO p21 00013 #define PIN_SPI_SCK p22 00014 #define PIN_ADIS_CS p23 00015 #define PIN_ADIS_RST p24 00016 00017 Serial pc(USBTX, USBRX); 00018 00019 const char* statusToString(bool status) 00020 { 00021 if(status) 00022 { 00023 return "Yes"; 00024 } 00025 else 00026 { 00027 return "No"; 00028 } 00029 } 00030 00031 void ADIS16467TestSuite::test_readData() { 00032 while(1) { 00033 wait_ms(600); 00034 00035 float gyroX = (float)(imu->readGyroX()) * imu->GYRO_CONV; 00036 float gyroY = (float)(imu->readGyroY()) * imu->GYRO_CONV; 00037 float gyroZ = (float)(imu->readGyroZ()) * imu->GYRO_CONV; 00038 float accelX = (float)(imu->readAccelX()) * imu->ACCEL_CONV; 00039 float accelY = (float)(imu->readAccelY()) * imu->ACCEL_CONV; 00040 float accelZ = (float)(imu->readAccelZ()) * imu->ACCEL_CONV; 00041 float intTemp = (float)(imu->readInternalTemp()) * imu->TEMP_CONV; 00042 uint16_t dataCount = imu->readDataCounter(); 00043 00044 pc.printf("Gyro X : %.2f degree/sec\r\n", gyroX); 00045 pc.printf("Gyro Y : %.2f degree/sec\r\n", gyroY); 00046 pc.printf("Gyro Z : %.2f degree/sec\r\n", gyroZ); 00047 pc.printf("Accel X : %.2f mg\r\n", accelX); 00048 pc.printf("Accel Y : %.2f mg\r\n", accelY); 00049 pc.printf("Accel Z : %.2f mg\r\n", accelZ); 00050 pc.printf("Internal Temperature : %.2f C\r\n", intTemp); 00051 pc.printf("Data Counter : %5d\r\n", dataCount); 00052 pc.printf("\r\n"); 00053 } 00054 } 00055 00056 void ADIS16467TestSuite::test_printFlags() { 00057 pc.printf("-----Error Flags-----\r\n"); 00058 pc.printf("Clock Error : %s\r\n", statusToString(imu->getClockError())); 00059 pc.printf("Memory Failure : %s\r\n", statusToString(imu->getMemFailure())); 00060 pc.printf("Sensor Failure : %s\r\n", statusToString(imu->getSensorFailure())); 00061 pc.printf("Standby Mode : %s\r\n", statusToString(imu->getStandbyMode())); 00062 pc.printf("SPI Communication Error : %s\r\n", statusToString(imu->getSPIError())); 00063 pc.printf("Flash Memory Update Failure : %s\r\n", statusToString(imu->getFlashUpFail())); 00064 pc.printf("Datapath Overrun : %s\r\n", statusToString(imu->getDatapathOverrun())); 00065 } 00066 00067 void ADIS16467TestSuite::test_burstRead() { 00068 struct ADIS16467::BurstReadResult result; 00069 imu->burstRead(result); 00070 00071 pc.printf("Gyro X : %.2f degree/sec\r\n", ((float)result.gyroX * imu->GYRO_CONV)); 00072 pc.printf("Gyro Y : %.2f degree/sec\r\n", ((float)result.gyroY * imu->GYRO_CONV)); 00073 pc.printf("Gyro Z : %.2f degree/sec\r\n", ((float)result.gyroZ * imu->GYRO_CONV)); 00074 pc.printf("Accel X : %.2f mg\r\n", ((float)result.accelX * imu->ACCEL_CONV)); 00075 pc.printf("Accel Y : %.2f mg\r\n", ((float)result.accelY * imu->ACCEL_CONV)); 00076 pc.printf("Accel Z : %.2f mg\r\n", ((float)result.accelZ * imu->ACCEL_CONV)); 00077 pc.printf("Internal Temperature : %.2f C\r\n", ((float)result.internalTemp * imu->TEMP_CONV)); 00078 pc.printf("Data Counter : %5d\r\n", result.dataCounter); 00079 pc.printf("Checksum Value : %5d\r\n", result.checkSum); 00080 00081 imu->setErrorFlags(result.statusDiag); 00082 test_printFlags(); 00083 imu->resetAllFlags(); 00084 } 00085 00086 void ADIS16467TestSuite::test_readFlags() { 00087 imu->setErrorFlags(imu->readStatDiag()); 00088 test_printFlags(); 00089 imu->resetAllFlags(); 00090 } 00091 00092 void ADIS16467TestSuite::test_sensorSelfTest() { 00093 imu->sensorSelfTest(); 00094 test_printFlags(); 00095 imu->resetAllFlags(); 00096 } 00097 00098 void ADIS16467TestSuite::test_setPoll(uint16_t pollRate) 00099 { 00100 const float print_time = 1.0f; 00101 00102 imu->setPollRate(pollRate); 00103 imu->resetDeltaAngle(); 00104 00105 Timer printTimer; 00106 printTimer.start(); 00107 00108 while(1) 00109 { 00110 00111 wait(1/ static_cast<float>(pollRate * 2)); 00112 00113 imu->updateDeltaAngles(); 00114 00115 if(printTimer.read() > print_time) 00116 { 00117 printTimer.reset(); 00118 00119 float gyroX = (float)(imu->readGyroX()) * imu->GYRO_CONV; 00120 float gyroY = (float)(imu->readGyroY()) * imu->GYRO_CONV; 00121 float gyroZ = (float)(imu->readGyroZ()) * imu->GYRO_CONV; 00122 float accelX = (float)(imu->readAccelX()) * imu->ACCEL_CONV; 00123 float accelY = (float)(imu->readAccelY()) * imu->ACCEL_CONV; 00124 float accelZ = (float)(imu->readAccelZ()) * imu->ACCEL_CONV; 00125 float intTemp = (float)(imu->readInternalTemp()) * imu->TEMP_CONV; 00126 float timeStamp = (float)(imu->readTimeStamp()) * imu->TIME_CONV; 00127 uint16_t dataCount = imu->readDataCounter(); 00128 float deltaAngXSum = (float)(imu->getDeltaAngleXSum()) * imu->DELTA_ANGLE_CONV; 00129 float deltaAngYSum = (float)(imu->getDeltaAngleYSum()) * imu->DELTA_ANGLE_CONV; 00130 float deltaAngZSum = (float)(imu->getDeltaAngleZSum()) * imu->DELTA_ANGLE_CONV; 00131 00132 pc.printf("Gyro X : %.2f degree/sec\r\n", gyroX); 00133 pc.printf("Gyro Y : %.2f degree/sec\r\n", gyroY); 00134 pc.printf("Gyro Z : %.2f degree/sec\r\n", gyroZ); 00135 pc.printf("Accel X : %.2f mg\r\n", accelX); 00136 pc.printf("Accel Y : %.2f mg\r\n", accelY); 00137 pc.printf("Accel Z : %.2f mg\r\n", accelZ); 00138 pc.printf("Delta Angle X Sum: %.2f degree\r\n", deltaAngXSum); 00139 pc.printf("Delta Angle Y Sum: %.2f degree\r\n", deltaAngYSum); 00140 pc.printf("Delta Angle Z Sum: %.2f degree\r\n", deltaAngZSum); 00141 pc.printf("Internal Temperature : %.2f C\r\n", intTemp); 00142 pc.printf("Time Stamp : %.2f us\r\n", timeStamp); 00143 pc.printf("Data Counter : %5d\r\n", dataCount); 00144 pc.printf("\r\n"); 00145 } 00146 00147 } 00148 } 00149 00150 void ADIS16467TestSuite::test_calibrateBias() 00151 { 00152 const uint16_t pollRate = 10; // Hz 00153 const float testDuration = 60; // s 00154 00155 imu->setPollRate(pollRate); 00156 imu->resetDeltaAngle(); 00157 00158 pc.printf("Place the IMU on a flat surface and keep it still. Orientation is not important.\r\n"); 00159 00160 wait(5); 00161 00162 00163 Timer testTimer; 00164 testTimer.start(); 00165 00166 while(testTimer.read() < testDuration) 00167 { 00168 00169 wait(1/ static_cast<float>(pollRate * 2)); 00170 imu->updateDeltaAngles(); 00171 00172 float deltaAngXSum = (float)(imu->getDeltaAngleXSum()) * imu->DELTA_ANGLE_CONV; 00173 float deltaAngYSum = (float)(imu->getDeltaAngleYSum()) * imu->DELTA_ANGLE_CONV; 00174 float deltaAngZSum = (float)(imu->getDeltaAngleZSum()) * imu->DELTA_ANGLE_CONV; 00175 00176 pc.printf("X Drift: %.2f degree\r\n", deltaAngXSum); 00177 pc.printf("Y Drift: %.2f degree\r\n", deltaAngYSum); 00178 pc.printf("Z Drift: %.2f degree\r\n", deltaAngZSum); 00179 } 00180 00181 float timeElapsed = testTimer.read(); 00182 00183 float driftRateX = (float)(imu->getDeltaAngleXSum()) * imu->DELTA_ANGLE_CONV / timeElapsed; 00184 float driftRateY = (float)(imu->getDeltaAngleYSum()) * imu->DELTA_ANGLE_CONV / timeElapsed; 00185 float driftRateZ = (float)(imu->getDeltaAngleZSum()) * imu->DELTA_ANGLE_CONV / timeElapsed; 00186 00187 pc.printf("To zero X gyro, set bias to: %f degrees/sec\r\n", -1 * driftRateX); 00188 pc.printf("To zero Y gyro, set bias to: %f degrees/sec\r\n", -1 * driftRateY); 00189 pc.printf("To zero Z gyro, set bias to: %f degrees/sec\r\n", -1 * driftRateZ); 00190 00191 } 00192 00193 void ADIS16467TestSuite::test_existence() 00194 { 00195 pc.printf("The IMU is connected and working: %s\r\n", imu->checkExistence() ? "True" : "False"); 00196 } 00197 00198 void ADIS16467TestSuite::test_firmInfo() { 00199 struct ADIS16467::FirmwareInfo data; 00200 imu->getFirmwareInformation(data); 00201 00202 pc.printf("Firmware Revision: %" PRIu8 ".%" PRIu8"\r\n", data.firmRevMajor, data.firmRevMinor); 00203 pc.printf("Firmware Revision Date: %" PRIu8 "/%" PRIu8 "/%" PRIu16 "\r\n", data.firmwareMonth, data.firmwareDay, data.firmwareYear); 00204 pc.printf("Serial Number: 0x%" PRIx16 "\r\n", data.serialNum); 00205 } 00206 00207 00208 int main() { 00209 while(true) 00210 { 00211 pc.printf("\r\nADIS16467 IMU Test Suite:\r\n"); 00212 00213 ADIS16467::ADIS16467 imuModule = ADIS16467::ADIS16467(&pc, PIN_SPI_MOSI, PIN_SPI_MISO, PIN_SPI_SCK, 00214 PIN_ADIS_CS, PIN_ADIS_RST); 00215 00216 ADIS16467TestSuite harness; 00217 harness.imu = &imuModule; 00218 00219 harness.imu->initADIS(); 00220 00221 int test = -1; 00222 00223 //Menu 00224 pc.printf("Select a test: \n\r"); 00225 pc.printf("1. Read Data\r\n"); 00226 pc.printf("2. Read Error Flags\r\n"); 00227 pc.printf("3. Burst Read\r\n"); 00228 pc.printf("4. Sensor Self Test\r\n"); 00229 pc.printf("5. Set Poll Rate\r\n"); 00230 pc.printf("6. Calibrate Bias\r\n"); 00231 pc.printf("7. Existence Test\r\n"); 00232 pc.printf("8. Get Firmware Information\r\n"); 00233 pc.printf("9. Exit Test Suite\r\n"); 00234 00235 pc.scanf("%d", &test); 00236 pc.printf("Running test %d:\r\n\n", test); 00237 00238 harness.imu->setGyroBiases(-0.11594f, 0.258192f, 0.272063f); 00239 00240 //Run Tests 00241 switch(test) { 00242 case 1 : harness.test_readData(); break; 00243 case 2 : harness.test_readFlags(); break; 00244 case 3 : harness.test_burstRead(); break; 00245 case 4 : harness.test_sensorSelfTest(); break; 00246 case 5 : 00247 { 00248 int rate = -1; 00249 pc.printf("Input poll rate: "); 00250 pc.scanf("%d", &rate); 00251 harness.test_setPoll(rate); 00252 } 00253 case 6 : harness.test_calibrateBias(); break; 00254 case 7 : harness.test_existence(); break; 00255 case 8 : harness.test_firmInfo(); break; 00256 case 9 : printf("Exiting Test Suite.\r\n"); return 0; 00257 default : printf("Invalid Test Number Selection. Please Run Again.\r\n"); return 1; 00258 } 00259 00260 pc.printf("Done.\r\n"); 00261 00262 } 00263 00264 } 00265 00266 00267 00268
Generated on Sat Jul 16 2022 18:37:46 by
1.7.2