BNOTestSuite.cpp

00001 /* 
00002     USC RPL HAMSTER v2.3
00003     Contributors: Lauren Potterat 
00004 */
00005 
00006 
00007 #include "BNOTestSuite.h"
00008 
00009 #define RAD_TO_DEG (180.0/M_PI)
00010 
00011 void BNOTestSuite::test_printInfo()
00012 {
00013     pc.printf("BNO080 reports as SW version %hhu.%hhu.%hu, build %lu, part no. %lu\n",
00014             imu.majorSoftwareVersion, imu.minorSoftwareVersion, imu.patchSoftwareVersion,
00015             imu.buildNumber, imu.partNumber);
00016 }
00017 
00018 
00019 void BNOTestSuite::test_readRotationVector()
00020 {
00021     const size_t update_rate_ms = 200;
00022 
00023     imu.enableReport(BNO080::ROTATION, update_rate_ms);
00024 
00025     while (true)
00026     {
00027         // note: for some reason it seems like wait() here waits 10x the time you tell it to
00028         // not sure what's causing this behavior, and I don't see it with mbed os 2
00029         wait(.0001f * update_rate_ms);
00030 
00031         if(imu.updateData())
00032         {
00033             TVector3 eulerRadians = imu.rotationVector.euler();
00034             TVector3 eulerDegrees = eulerRadians * RAD_TO_DEG;
00035 
00036             pc.printf("IMU Rotation Euler: Roll: %f deg, Pitch: %f deg, Yaw: %f deg \r\n",
00037                       eulerDegrees[0], eulerDegrees[1], eulerDegrees[2]);
00038 
00039             pc.printf(", Accuracy: %.02f", (imu.rotationAccuracy * RAD_TO_DEG));
00040 
00041             pc.printf(", Status: %s\n", imu.getReportStatusString(BNO080::ROTATION));
00042         }
00043         else
00044         {
00045             pc.printf("IMU was not ready with data packet!\r\n");
00046         }
00047     }
00048 }
00049 
00050 void BNOTestSuite::test_readRotationAcceleration()
00051 {
00052     imu.enableReport(BNO080::ROTATION, 1000);
00053     imu.enableReport(BNO080::TOTAL_ACCELERATION, 500);
00054 
00055     while (true)
00056     {
00057         wait(.001f);
00058 
00059         if(imu.updateData())
00060         {
00061             if (imu.hasNewData(BNO080::ROTATION))
00062             {
00063                 pc.printf("IMU Rotation Euler: ");
00064                 TVector3 eulerRadians = imu.rotationVector.euler();
00065                 TVector3 eulerDegrees = eulerRadians * (180.0 / M_PI);
00066                 eulerDegrees.print(pc, true);
00067                 pc.printf("\n");
00068             }
00069             if (imu.hasNewData(BNO080::TOTAL_ACCELERATION))
00070             {
00071                 pc.printf("IMU Total Acceleration: ");
00072                 imu.totalAcceleration.print(pc, true);
00073                 pc.printf("\n");
00074             }
00075         }
00076     }
00077 }
00078 
00079 void BNOTestSuite::test_tapDetector()
00080 {
00081     imu.enableReport(BNO080::TAP_DETECTOR, 10);
00082     pc.printf("Listening for taps...\n");
00083 
00084     while(true)
00085     {
00086         if(imu.updateData())
00087         {
00088             if(imu.tapDetected)
00089             {
00090                 pc.printf("Tap detected!\n");
00091                 imu.tapDetected = false;
00092             }
00093         }
00094     }
00095 }
00096 
00097 void BNOTestSuite::test_gameRotationVector()
00098 {
00099     const size_t update_rate_ms = 200;
00100 
00101     imu.enableReport(BNO080::GAME_ROTATION, update_rate_ms);
00102 
00103     while (true)
00104     {
00105         // note: for some reason it seems like wait() here waits 10x the time you tell it to
00106         // not sure what's causing this behavior, and I don't see it with mbed os 2
00107         wait(.0001f * update_rate_ms);
00108 
00109         if(imu.updateData())
00110         {
00111             pc.printf("IMU Game Rotation Euler: [");
00112             TVector3 eulerRadians = imu.gameRotationVector.euler();
00113             TVector3 eulerDegrees = eulerRadians * RAD_TO_DEG;
00114             eulerDegrees.print(pc, true);
00115 
00116             pc.printf("], Status: %s\n", imu.getReportStatusString(BNO080::GAME_ROTATION));
00117         }
00118         else
00119         {
00120             pc.printf("IMU was not ready with data packet!\r\n");
00121         }
00122     }
00123 }
00124 
00125 void BNOTestSuite::test_tare()
00126 {
00127     const size_t update_rate_ms = 100;
00128     imu.enableReport(BNO080::ROTATION, update_rate_ms);
00129 
00130     Timer runningTime;
00131     runningTime.start();
00132 
00133     bool sentTareCmd = false;
00134 
00135     while (true)
00136     {
00137         // note: for some reason it seems like wait() here waits 10x the time you tell it to
00138         // not sure what's causing this behavior, and I don't see it with mbed os 2
00139         wait(.0001f * update_rate_ms);
00140 
00141         if(imu.updateData())
00142         {
00143             pc.printf("IMU Rotation Euler: [");
00144             TVector3 eulerRadians = imu.rotationVector.euler();
00145             TVector3 eulerDegrees = eulerRadians * RAD_TO_DEG;
00146             eulerDegrees.print(pc, true);
00147 
00148             pc.printf("], Status: %s\n", imu.getReportStatusString(BNO080::ROTATION));
00149         }
00150         else
00151         {
00152             pc.printf("IMU was not ready with data packet!\r\n");
00153         }
00154 
00155         if(runningTime.read() > 2 && !sentTareCmd)
00156         {
00157             pc.printf("2 seconds have passed, sending tare command...\n");
00158             imu.tare();
00159             sentTareCmd = true;
00160         }
00161     }
00162 }
00163 
00164 void BNOTestSuite::test_magCalibration()
00165 {
00166     // enable calibration for the magnetometer
00167     bool success = imu.enableCalibration(false, false, true);
00168 
00169     const size_t update_rate_ms = 200;
00170     imu.enableReport(BNO080::GAME_ROTATION, update_rate_ms);
00171     imu.enableReport(BNO080::MAG_FIELD, update_rate_ms);
00172     imu.enableReport(BNO080::MAG_FIELD_UNCALIBRATED, update_rate_ms);
00173 
00174     if(success)
00175     {
00176         pc.printf("Calibration mode was successfully enabled!\n");
00177     }
00178     else
00179     {
00180         pc.printf("Calibration mode failed to enable!\n");
00181         while(true){}
00182     }
00183 
00184     while (true)
00185     {
00186         // note: for some reason it seems like wait() here waits 10x the time you tell it to
00187         // not sure what's causing this behavior, and I don't see it with mbed os 2
00188         wait(.0001f * update_rate_ms);
00189 
00190         if(imu.updateData())
00191         {
00192             pc.printf("IMU Magnetic Field: [");
00193             imu.magFieldUncalibrated.print(pc, true);
00194 
00195             pc.printf("] uTesla, Status: %hhu (should be 2-3 when calibration is complete), ", imu.getReportStatus(BNO080::MAG_FIELD));
00196 
00197             pc.printf("Hard iron offsets: [");
00198             imu.hardIronOffset.print(pc, true);
00199 
00200             pc.printf("]\n");
00201 
00202         }
00203         else
00204         {
00205             pc.printf("IMU was not ready with data packet!\r\n");
00206         }
00207     }
00208 
00209 }
00210 
00211 void BNOTestSuite::test_stabilityClassifier()
00212 {
00213     imu.enableReport(BNO080::STABILITY_CLASSIFIER, 200);
00214 
00215     while (true)
00216     {
00217         wait(.001f);
00218 
00219         if(imu.updateData())
00220         {
00221             if (imu.hasNewData(BNO080::STABILITY_CLASSIFIER))
00222             {
00223                 pc.printf("Stability: ");
00224 
00225                 switch(imu.stability)
00226                 {
00227                     case BNO080::UNKNOWN:
00228                         pc.printf("Unknown\n");
00229                         break;
00230                     case BNO080::ON_TABLE:
00231                         pc.printf("On Table\n");
00232                         break;
00233                     case BNO080::STATIONARY:
00234                         pc.printf("Stationary\n");
00235                         break;
00236                     case BNO080::STABLE:
00237                         pc.printf("Stable\n");
00238                         break;
00239                     case BNO080::MOTION:
00240                         pc.printf("Motion\n");
00241                         break;
00242                 }
00243             }
00244         }
00245     }
00246 
00247 }
00248 
00249 void BNOTestSuite::test_metadata()
00250 {
00251     pc.printf("Printing metadata for Linear Acceleration:\r\n");
00252     imu.printMetadataSummary(BNO080::LINEAR_ACCELERATION);
00253 
00254     pc.printf("Printing metadata for Rotation Vector:\r\n");
00255     imu.printMetadataSummary(BNO080::ROTATION);
00256 
00257     pc.printf("Printing metadata for Magnetic Field:\r\n");
00258     imu.printMetadataSummary(BNO080::MAG_FIELD);
00259 
00260     pc.printf("Printing metadata for Tap Detector:\r\n");
00261     imu.printMetadataSummary(BNO080::TAP_DETECTOR);
00262 
00263 }
00264 
00265 void BNOTestSuite::test_orientation()
00266 {
00267     const size_t update_rate_ms = 200;
00268     imu.enableReport(BNO080::TOTAL_ACCELERATION, update_rate_ms);
00269     imu.enableReport(BNO080::ROTATION, update_rate_ms);
00270 
00271     Timer runningTime;
00272     runningTime.start();
00273 
00274     bool setOrientation = false;
00275 
00276     while (true)
00277     {
00278         // note: for some reason it seems like wait() here waits 10x the time you tell it to
00279         // not sure what's causing this behavior, and I don't see it with mbed os 2
00280         wait(.0001f * update_rate_ms);
00281 
00282         if(imu.updateData())
00283         {
00284             if (imu.hasNewData(BNO080::ROTATION))
00285             {
00286                 pc.printf("IMU Rotation Euler: ");
00287                 TVector3 eulerRadians = imu.rotationVector.euler();
00288                 TVector3 eulerDegrees = eulerRadians * (180.0 / M_PI);
00289                 eulerDegrees.print(pc, true);
00290                 pc.printf("\n");
00291             }
00292             if (imu.hasNewData(BNO080::TOTAL_ACCELERATION))
00293             {
00294                 pc.printf("IMU Total Acceleration: ");
00295                 imu.totalAcceleration.print(pc, true);
00296                 pc.printf("\n");
00297             }
00298         }
00299 
00300         if(runningTime.read() > 2 && !setOrientation)
00301         {
00302             pc.printf("2 seconds have passed, sending set orientation command...\n");
00303             setOrientation = true;
00304 
00305             // rotate sensor about the Y axis, so Z points down and X points west.
00306             // (values from BNO datasheet page 41)
00307             imu.setSensorOrientation(Quaternion(0, -1, 0, 0));
00308         }
00309     }
00310 }
00311 
00312 void BNOTestSuite::test_permanentOrientation()
00313 {
00314     pc.printf("Setting permanent sensor orientation...\n");
00315     Timer orientationTimer;
00316     orientationTimer.start();
00317 
00318     // rotate sensor 90 degrees CCW about the Y axis, so X points up, Y points back, and Z points out
00319     // (values from BNO datasheet page 41)
00320     // imu.setPermanentOrientation(Quaternion(1.0f/SQRT_2, -1.0f/SQRT_2, 0, 0));
00321 
00322     // rotate sensor 180 degrees about Y axis
00323     //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
00324     // see page 5 and 11 of hillcrestlabs FSM30X datasheet
00325     imu.setPermanentOrientation(Quaternion(0,-1, 0, 0)); 
00326     
00327     // reset to default orientation
00328     //imu.setPermanentOrientation(Quaternion(0, 0, 0, 0));
00329 
00330     orientationTimer.stop();
00331     pc.printf("Done setting orientation (took %.03f seconds)\r\n", orientationTimer.read());
00332 
00333 }
00334 
00335 void BNOTestSuite::test_disable()
00336 {
00337     const size_t update_rate_ms = 200;
00338     imu.enableReport(BNO080::TOTAL_ACCELERATION, update_rate_ms);
00339     imu.enableReport(BNO080::ROTATION, update_rate_ms);
00340 
00341     Timer runningTime;
00342     runningTime.start();
00343 
00344     bool disabledRotation = false;
00345 
00346     while (true)
00347     {
00348         // note: for some reason it seems like wait() here waits 10x the time you tell it to
00349         // not sure what's causing this behavior, and I don't see it with mbed os 2
00350         wait(.0001f * update_rate_ms);
00351 
00352         if(imu.updateData())
00353         {
00354             if (imu.hasNewData(BNO080::ROTATION))
00355             {
00356                 pc.printf("IMU Rotation Euler: ");
00357                 TVector3 eulerRadians = imu.rotationVector.euler();
00358                 TVector3 eulerDegrees = eulerRadians * (180.0 / M_PI);
00359                 eulerDegrees.print(pc, true);
00360                 pc.printf("\n");
00361             }
00362             if (imu.hasNewData(BNO080::TOTAL_ACCELERATION))
00363             {
00364                 pc.printf("IMU Total Acceleration: ");
00365                 imu.totalAcceleration.print(pc, true);
00366                 pc.printf("\n");
00367             }
00368         }
00369 
00370         if(runningTime.read() > 2 && !disabledRotation)
00371         {
00372             pc.printf("2 seconds have passed, disabling rotation report...\n");
00373             disabledRotation = true;
00374 
00375             imu.disableReport(BNO080::ROTATION);
00376         }
00377     }
00378 }
00379 
00380 int main()
00381 {
00382     pc.baud(115200);
00383 
00384     pc.printf("============================================================\n");
00385 
00386 
00387     // reset and connect to IMU
00388     while(!imu.begin())
00389     {
00390         pc.printf("Failed to connect to IMU!\r\n");
00391         wait(.5);
00392     }
00393 
00394     //Declare test harness
00395     BNOTestSuite harness;
00396 
00397     int test = -1;
00398     pc.printf("\r\n\nHamster BNO Test Suite:\r\n");
00399 
00400     //MENU. ADD OPTION PER EACH TEST
00401     pc.printf("Select a test: \n\r");
00402     pc.printf("1. Print chip info\r\n");
00403     pc.printf("2. Display rotation vector\r\n");
00404     pc.printf("3. Display rotation and acceleration\r\n");
00405     pc.printf("4. Run tap detector\r\n");
00406     pc.printf("5. Display game rotation vector\r\n");
00407     pc.printf("6. Test tare function\r\n");
00408     pc.printf("7. Run magnetometer calibration\r\n");
00409     pc.printf("8. Test stability classifier\r\n");
00410     pc.printf("9. Test metadata reading (requires BNO_DEBUG = 1)\r\n");
00411     pc.printf("10. Test set orientation\r\n");
00412     pc.printf("11. Test set permanent orientation\r\n");
00413     pc.printf("12. Test disabling reports\r\n");
00414     pc.printf("13. Exit test suite\r\n");
00415 
00416     pc.scanf("%d", &test);
00417     pc.printf("Running test %d:\r\n\n", test);
00418 
00419 
00420     //SWITCH. ADD CASE PER EACH TEST
00421     switch(test){
00422         case 1:
00423             harness.test_printInfo();
00424             break;
00425         case 2:
00426             harness.test_readRotationVector();
00427             break;
00428         case 3:
00429             harness.test_readRotationAcceleration();
00430             break;
00431         case 4:
00432             harness.test_tapDetector();
00433             break;
00434         case 5:
00435             harness.test_gameRotationVector();
00436             break;
00437         case 6:
00438             harness.test_tare();
00439             break;
00440         case 7:
00441             harness.test_magCalibration();
00442             break;
00443         case 8:
00444             harness.test_stabilityClassifier();
00445             break;
00446         case 9:
00447             harness.test_metadata();
00448             break;
00449         case 10:
00450             harness.test_orientation();
00451             break;
00452         case 11:
00453             harness.test_permanentOrientation();
00454             break;
00455         case 12:
00456             harness.test_disable();
00457             break;
00458         case 13:
00459             pc.printf("Exiting test suite.\r\n");
00460             return 0;
00461         default:
00462             printf("Invalid test number. Please run again.\r\n");
00463             return 1;
00464     }
00465 
00466     pc.printf("Done!\r\n");
00467     return 0;
00468 
00469 }
00470 
00471 
00472 
00473 
00474 
00475 
00476 
00477 
00478