UM6LT.cpp

00001 #include "UM6LT.h"
00002 #include "mbed.h"
00003 
00004 UM6LT::UM6LT(PinName tx, PinName rx):serial_(tx,rx) {serial_.baud(115200);stdDelayMs = 2;}
00005 
00006 //----------------------------------------------------------------------------------------------
00007 //-----------------------------------   Private Functions   ------------------------------------
00008 //----------------------------------------------------------------------------------------------
00009 
00010 int UM6LT::verifyChecksum(int byte1, int byte2){
00011 
00012     int checksumDATA = 0;
00013     div_t div_res1;
00014     div_t div_res2;
00015     int quot1 = byte1;
00016     int quot2 = byte2;
00017     
00018     int coef[16] = {1, 2, 4, 8, 16, 32, 64, 128, 256, 512, 1024, 2048, 4096, 8192, 16384, 32768};
00019     
00020     for(int i=0; i<8; ++i){
00021         div_res1 = div(quot1,2);
00022         div_res2 = div(quot2,2);
00023         checksumDATA = checksumDATA + div_res1.rem * coef[i+8] + div_res2.rem * coef[i];
00024         quot1 = div_res1.quot;
00025         quot2 = div_res2.quot;
00026     }
00027     
00028     return checksumDATA;
00029     
00030 }
00031 
00032 //----------------------------------------------------------------------------------------------
00033 
00034 void UM6LT::createChecksum(int checksum_dec, int& byte1, int& byte2){
00035 
00036     div_t div_res;
00037     int quot = checksum_dec;    
00038     int coef[24] = {0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 4, 8, 16, 32, 64, 128, 0, 0, 0, 0, 0, 0, 0, 0};
00039     
00040     byte1 = 0;
00041     byte2 = 0;
00042     
00043     for(int i=0; i<16; ++i){
00044         div_res = div(quot,2);
00045         byte1 = byte1 + div_res.rem * coef[i];
00046         byte2 = byte2 + div_res.rem * coef[i + 8];
00047         quot = div_res.quot;
00048     }
00049 
00050 }
00051 
00052 //----------------------------------------------------------------------------------------------
00053 
00054 void UM6LT::createByte(int* bitsList, int& byte) {
00055 
00056     int coef[] = {1, 2, 4, 8, 16, 32, 64, 128};
00057 
00058     for(int i=0; i<8; i++){
00059         byte = byte + bitsList[i]*coef[i];
00060     }
00061 
00062 }
00063 
00064 //----------------------------------------------------------------------------------------------
00065 
00066 void UM6LT::decomposeByte(int* bitsList, int byte){
00067 
00068     div_t div_res;
00069     int quot = byte;
00070     
00071     for(int i=0; i<8; i++){
00072         div_res = div(quot, 2);
00073         bitsList[i] = div_res.rem;
00074         quot = div_res.quot;
00075     }
00076 
00077 }
00078 
00079 //----------------------------------------------------------------------------------------------
00080 
00081 int UM6LT::twosComplement(int byte1, int byte2){
00082 
00083     int sumData = 0;
00084     div_t div_res1;
00085     div_t div_res2;
00086     int quot1 = byte1;
00087     int quot2 = byte2;
00088     
00089     int coef[16] = {1, 2, 4, 8, 16, 32, 64, 128, 256, 512, 1024, 2048, 4096, 8192, 16384, 32768};
00090     
00091     for(int i=0; i<7; ++i){
00092         div_res1 = div(quot1,2);
00093         div_res2 = div(quot2,2);
00094         sumData = sumData + div_res1.rem * coef[i+8] + div_res2.rem * coef[i];
00095         quot1 = div_res1.quot;
00096         quot2 = div_res2.quot;
00097     }
00098     
00099     div_res1 = div(quot1,2);
00100     div_res2 = div(quot2,2);
00101     
00102     if(div_res1.rem == 1){
00103         sumData = sumData + div_res2.rem * coef[7] - coef[15];
00104     }
00105     else{        
00106         sumData = sumData + div_res2.rem * coef[7];
00107     }
00108     
00109     return sumData;
00110 
00111 }
00112 
00113 //----------------------------------------------------------------------------------------------
00114 
00115 void UM6LT::oneWordWrite(int hasData, int isBatch, int BL, int address, int* data) {
00116 
00117     int N, PT, checksumDATA;
00118     
00119 
00120     if(hasData){        
00121         N = BL*4; //Number of data bytes
00122     }
00123     else{
00124         N = 0;
00125     }
00126     
00127     int word [N+7];      
00128     int coef [] = {4, 8, 16, 32};
00129     div_t div_res;
00130     int quot = BL;
00131     
00132     PT = 128*hasData + 64*isBatch;
00133     for(int i=0; i<4; ++i){
00134         div_res = div(quot,2);
00135         PT = PT + div_res.rem * coef[i];
00136         quot = div_res.quot;
00137     }
00138     
00139     checksumDATA = 115 + 110 + 112 + PT + address;
00140     
00141     if(hasData){
00142         for(int i=0; i<N; i++){
00143             checksumDATA = checksumDATA + data[i];
00144             word[i+5] = data[i];
00145         }        
00146     }    
00147     
00148     int byte1 = 0;
00149     int byte2 = 0;
00150     createChecksum(checksumDATA, byte1, byte2);
00151     
00152     word[0] = 115; //'s'
00153     word[1] = 110; //'n'
00154     word[2] = 112; //'p'
00155     word[3] = PT;
00156     word[4] = address;
00157     word[N+5] = byte1;
00158     word[N+6] = byte2;    
00159     
00160     if(serial_.writeable()) {
00161         for(int i=0; i<N+7; i++){
00162             serial_.putc(word[i]);
00163         }
00164     }
00165     else{
00166         printf("IMU not writeable.\r\n");
00167     }
00168     
00169 }
00170 
00171 //----------------------------------------------------------------------------------------------
00172 
00173 void UM6LT::oneWordRead(int& PT, int& N, int& address, int* data){
00174 
00175     int sumData = 0;
00176     int PT_byte [8];
00177     char snp [] = {'0', '0', '0'};
00178 
00179     if(serial_.readable()){
00180         
00181         while((snp[0] != 's' || snp[1] != 'n' || snp[2] != 'p') && serial_.readable()){
00182             snp[0] = snp[1];
00183             snp[1] = snp[2];
00184             snp[2] = serial_.getc();
00185         }
00186         
00187         if(snp[0] == 's' && snp[1]=='n' && snp[2] == 'p'){            
00188             PT = serial_.getc();
00189             address = serial_.getc();
00190             
00191             decomposeByte(PT_byte, PT);
00192             
00193             int hasData = PT_byte[7];
00194             int CF = PT_byte[0];
00195             
00196             if(CF){
00197                 // find something to do if command failed
00198                 printf("Last command sent to UM6 failed.\r\n");
00199             }
00200             else if(hasData){            
00201                 int BL = PT_byte[5]*8 + PT_byte[4]*4 + PT_byte[3]*2 + PT_byte[2];
00202                 N = 4*BL;       
00203                 for(int i=0; i<N; i++){
00204                     data[i] = serial_.getc();
00205                     sumData = sumData + data[i];
00206                 }            
00207             }            
00208             else{
00209                 N = 0;
00210             }
00211             
00212             int byte1 = serial_.getc();
00213             int byte2 = serial_.getc();
00214        
00215             int checksumUM6LT = verifyChecksum(byte1, byte2);
00216             int checksumDATA = 115 + 110 + 112 + PT + address + sumData;
00217             
00218             if(checksumDATA != checksumUM6LT){
00219                 // Find something to do if checksum not good
00220                 printf("Calculated checksum does not match the one from the UM6.\r\n");
00221                 data[0] = -1;
00222             }            
00223         }
00224         else{
00225             printf("Could not read an expected word from UM6. Its buffer is now empty\r\n");
00226         }               
00227     }
00228     else{
00229         // Find something to do if IMU not readable
00230         printf("Minor synchronization issue with UM6.\r\n");
00231         data[0] = -1;
00232     }
00233     
00234 }
00235 
00236 //----------------------------------------------------------------------------------------------
00237 
00238 void UM6LT::requestData(int address, int expectedBL){
00239 
00240     int hasData = 0;
00241     int isBatch = 1;
00242     int data[1];
00243     
00244     if(serial_.readable()){
00245         while(serial_.readable()){
00246             data[0] = serial_.getc();
00247         }
00248     }
00249     if(serial_.writeable()){
00250         oneWordWrite(hasData, isBatch, expectedBL, address, data);
00251     }
00252     else{
00253         printf("IMU not writeable. Data request not sent.\r\n");
00254     }
00255 
00256 }
00257 
00258 //----------------------------------------------------------------------------------------------
00259 
00260 int UM6LT::sendOneCommand(int address){
00261 
00262     int PT = -1;
00263     int N = -1;
00264     int addressRead = -1;
00265     int data [1] = {0};
00266     
00267     int okToMoveOn = 0;
00268     int count = 0;
00269     
00270     while(!okToMoveOn && count<5){
00271         requestData(address, 0);
00272         wait_ms(stdDelayMs);
00273         oneWordRead(PT, N, addressRead, data);
00274         okToMoveOn = noCommError(addressRead, address, PT);
00275         count++;
00276     }
00277     
00278     if(okToMoveOn){
00279         switch(address){
00280             case UM6_FLASH_COMMIT:
00281                 printf("All current configurations were written to flash.\r\n");
00282                 break;
00283             case UM6_RESET_EKF:
00284                 printf("EKF algorithm successfully reset.\r\n");
00285                 break;
00286             case UM6_SET_ACCEL_REF:
00287                 printf("Acceleration reference vector updated.\r\n");
00288                 break;
00289             case UM6_SET_MAG_REF:
00290                 printf("Magnetic reference vector updated.\r\n");
00291                 break;
00292             case UM6_RESET_TO_FACTORY:
00293                 printf("UM6 reset to factory settings.\r\n");
00294                 break;
00295             default:
00296                 printf("This command is not coded. Can't say what happened. address: 0x%x\r\n", address);
00297                 break;
00298         }
00299         return 1;
00300     }
00301     else{
00302         WhatCommError(addressRead, address, PT);
00303         switch(address){
00304             case UM6_FLASH_COMMIT:
00305                 printf("Current configurations could not be written to flash.\r\n");
00306                 break;
00307             case UM6_RESET_EKF:
00308                 printf("EKF algorithm could not be reset.\r\n");
00309                 break;
00310             case UM6_SET_ACCEL_REF:
00311                 printf("Acceleration reference vector failed to update.\r\n");
00312                 break;
00313             case UM6_SET_MAG_REF:
00314                 printf("Magnetic reference vector failed to update.\r\n");
00315                 break;
00316             case UM6_RESET_TO_FACTORY:
00317                 printf("UM6 could not be reset to factory settings.\r\n");
00318                 break;
00319             default:
00320                 printf("This command is not coded. Can't say what happened. address: 0x%x\r\n", address);
00321                 break;
00322         }
00323         return 0;
00324     }  
00325 
00326 }
00327 
00328 //----------------------------------------------------------------------------------------------
00329 
00330 int UM6LT::getTwoBatches(int address, int* dataToPost){
00331 
00332     dataToPost[0] = 0;
00333     dataToPost[1] = 0;
00334     dataToPost[2] = 0;
00335     dataToPost[3] = 0;
00336     
00337     int PT = -1;
00338     int N = -1;
00339     int addressRead = -1;
00340     int dataToRead [8] = {0,0,0,0,0,0,0,0};
00341     
00342     int okToMoveOn = 0;
00343     int count = 0;
00344     
00345     while(!okToMoveOn && count<5){
00346         requestData(address, 2);
00347         wait_ms(stdDelayMs);
00348         oneWordRead(PT, N, addressRead, dataToRead);
00349         okToMoveOn = noCommError(addressRead, address, PT);
00350         count++;
00351     }
00352     
00353     if(okToMoveOn){
00354         switch(address){
00355             case UM6_EULER_PHI_THETA:
00356                 dataToPost[0] = (int)(twosComplement(dataToRead[0], dataToRead[1])*UM6_ANGLE_FACTOR);
00357                 dataToPost[1] = (int)(twosComplement(dataToRead[2], dataToRead[3])*UM6_ANGLE_FACTOR);
00358                 dataToPost[2] = (int)(twosComplement(dataToRead[4], dataToRead[5])*UM6_ANGLE_FACTOR);
00359                 break;
00360             case UM6_ACCEL_PROC_XY:
00361                 dataToPost[0] = (int)(twosComplement(dataToRead[0], dataToRead[1])*UM6_ACCEL_FACTOR*1000);
00362                 dataToPost[1] = (int)(twosComplement(dataToRead[2], dataToRead[3])*UM6_ACCEL_FACTOR*1000);
00363                 dataToPost[2] = (int)(twosComplement(dataToRead[4], dataToRead[5])*UM6_ACCEL_FACTOR*1000);
00364                 break;
00365             case UM6_MAG_PROC_XY:
00366                 dataToPost[0] = (int)(twosComplement(dataToRead[0], dataToRead[1])*UM6_MAG_FACTOR*1000);
00367                 dataToPost[1] = (int)(twosComplement(dataToRead[2], dataToRead[3])*UM6_MAG_FACTOR*1000);
00368                 dataToPost[2] = (int)(twosComplement(dataToRead[4], dataToRead[5])*UM6_MAG_FACTOR*1000);
00369                 break;
00370             case UM6_GYRO_PROC_XY:
00371                 dataToPost[0] = (int)(twosComplement(dataToRead[0], dataToRead[1])*UM6_ANGLE_RATE_FACTOR);
00372                 dataToPost[1] = (int)(twosComplement(dataToRead[2], dataToRead[3])*UM6_ANGLE_RATE_FACTOR);
00373                 dataToPost[2] = (int)(twosComplement(dataToRead[4], dataToRead[5])*UM6_ANGLE_RATE_FACTOR);
00374                 break;
00375             case UM6_QUAT_AB:
00376                 dataToPost[0] = (int)(twosComplement(dataToRead[0], dataToRead[1])*UM6_QUATERNION_FACTOR*1000);
00377                 dataToPost[1] = (int)(twosComplement(dataToRead[2], dataToRead[3])*UM6_QUATERNION_FACTOR*1000);
00378                 dataToPost[2] = (int)(twosComplement(dataToRead[4], dataToRead[5])*UM6_QUATERNION_FACTOR*1000);
00379                 dataToPost[3] = (int)(twosComplement(dataToRead[6], dataToRead[7])*UM6_QUATERNION_FACTOR*1000);
00380                 break;
00381             default:
00382                 dataToPost[0] = -1;
00383                 dataToPost[1] = -1;
00384                 dataToPost[2] = -1;
00385                 dataToPost[3] = -1;
00386                 printf("Data acquisition not programmed for this address: %d\r\n", address);
00387                 break;
00388         }
00389         return 1;
00390     }
00391     else{
00392         WhatCommError(address, UM6_ACCEL_PROC_XY, PT);
00393         switch(address){
00394             case UM6_EULER_PHI_THETA:
00395                 printf("Could not acquire the values of the Euler angles.\r\n");
00396                 break;
00397             case UM6_ACCEL_PROC_XY:
00398                 printf("Could not acquire the components of the acceleration vector.\r\n");
00399                 break;
00400             case UM6_MAG_PROC_XY:
00401                 printf("Could not acquire the components of the magnetic vector.\r\n");
00402                 break;
00403             case UM6_GYRO_PROC_XY:
00404                 printf("Could not acquire the values of angle rates.\r\n");
00405                 break;
00406             case UM6_QUAT_AB:
00407                 printf("Could not acquire the components of the quaternion.\r\n");
00408                 break;
00409             default:
00410                 printf("Data acquisition not programmed for this address: %d\r\n", address);
00411                 break;
00412         }
00413         return 0;
00414     }   
00415 
00416 }
00417 
00418 //----------------------------------------------------------------------------------------------
00419 
00420 int UM6LT::noCommError(int addressRead, int addressExpected, int PT){
00421 
00422     if(addressRead == 0xFD){
00423         return 0;
00424     }
00425     else if(addressRead == 0xFE){
00426         return 0;
00427     }
00428     else if(addressRead == 0xFF){
00429         return 0;
00430     }
00431     else if(addressRead == addressExpected){
00432         return 1;
00433     }
00434     else{
00435         return 0;
00436     }
00437     
00438 }
00439 
00440 //----------------------------------------------------------------------------------------------
00441 
00442 void UM6LT::WhatCommError(int addressRead, int addressExpected, int PT){
00443 
00444     if(addressRead == 0xFD){
00445         printf("\r\n !!!!!! \r\nLast word sent to UM6 had a bad checksum.\r\n !!!!!! \r\n");
00446     }
00447     else if(addressRead == 0xFE){
00448         printf("\r\n !!!!!! \r\nLast word was sent to UM6 at an unknown address.\r\n !!!!!! \r\n");
00449         printf("address read: 0x%x\r\naddress expected: 0x%x\r\n", addressRead, addressExpected);
00450     }
00451     else if(addressRead == 0xFF){
00452         printf("\r\n !!!!!! \r\nLast word sent to UM6 had a bad batch size.\r\n !!!!!! \r\n");
00453         printf("PT: %d\r\n", PT);
00454     }
00455     else if(addressRead == addressExpected){
00456         printf("No communication error detected...\r\n");
00457         printf("PT: %d\r\n", PT);
00458         printf("address read: 0x%x\r\naddress expected: 0x%x\r\n", addressRead, addressExpected);
00459     }
00460     else{    
00461         printf("\r\n !!!!!! \r\nLast word read was not the one expected.\r\n !!!!!! \r\n");
00462         printf("PT: %d\r\n", PT);
00463         printf("address read: 0x%x\r\naddress expected: 0x%x\r\n", addressRead, addressExpected);
00464     }
00465     
00466 }
00467 
00468 //----------------------------------------------------------------------------------------------
00469 //----------------------------------   Public Functions   --------------------------------------
00470 //----------------------------------------------------------------------------------------------
00471 
00472 void UM6LT::baud(int baudrate){serial_.baud(baudrate);}
00473 
00474 //----------------------------------------------------------------------------------------------
00475 
00476 int UM6LT::readable(void){return serial_.readable();}
00477 
00478 //----------------------------------------------------------------------------------------------
00479 
00480 char UM6LT::getc(void){return serial_.getc();}
00481 
00482 //----------------------------------------------------------------------------------------------
00483 
00484 void UM6LT::putc(char byte){serial_.putc(byte);}
00485 
00486 //----------------------------------------------------------------------------------------------
00487 
00488 void UM6LT::setCommParams(int broadcastRate, int baudrate, int* dataToTransmit, int broadcastEnabled){
00489 
00490  // Broadcast rate should be an integer between 20 and 300 (in Hertz)
00491  // Baudrate should be either 9600, 14400, 19200, 38400, 57600 or 115200
00492  // DataToTransmit defines what data the UM6LT will provide. It's a list of 0 and 1 for a total length of N=9. See UM6LT documentation
00493  
00494     int data [4] = {0, 0, 0, 0};
00495     int bitsList2 [8];
00496     int bitsList3 [8];
00497     int bitsList4 [8];
00498     
00499     int broadcastRateByte = ((broadcastRate - 20)*255)/280;
00500     
00501     int baudrateByte [3];
00502     switch(baudrate){
00503         case 9600:
00504             baudrateByte[0] = 0;
00505             baudrateByte[1] = 0;
00506             baudrateByte[2] = 0;
00507             break;
00508         case 14400:
00509             baudrateByte[0] = 1;
00510             baudrateByte[1] = 0;
00511             baudrateByte[2] = 0;
00512             break;
00513         case 19200:
00514             baudrateByte[0] = 0;
00515             baudrateByte[1] = 1;
00516             baudrateByte[2] = 0;
00517             break;
00518         case 38400:
00519             baudrateByte[0] = 1;
00520             baudrateByte[1] = 1;
00521             baudrateByte[2] = 0;
00522             break;
00523         case 57600:
00524             baudrateByte[0] = 0;
00525             baudrateByte[1] = 0;
00526             baudrateByte[2] = 1;
00527             break;
00528         case 115200:
00529             baudrateByte[0] = 1;
00530             baudrateByte[1] = 0;
00531             baudrateByte[2] = 1;
00532             break;
00533         default:
00534             baudrateByte[0] = 1;
00535             baudrateByte[1] = 0;
00536             baudrateByte[2] = 1;
00537             printf("Baudrate not listed for UM6LT. Default = 115200.\r\n");
00538             break;
00539         }
00540     
00541     bitsList2[0] = baudrateByte[0];
00542     bitsList2[1] = baudrateByte[1];
00543     bitsList2[2] = baudrateByte[2];
00544     for(int i=0; i<5; i++){
00545         bitsList2[i+3] = 0;
00546         bitsList3[i] = 0;
00547     }
00548     
00549     for(int i=0; i<3; i++){
00550         bitsList3[i+5] = dataToTransmit[i];
00551     }
00552     
00553     for(int i=0; i<6; i++){
00554         bitsList4[i] = dataToTransmit[i+3];
00555     }
00556     
00557     bitsList4[6] = broadcastEnabled;
00558     bitsList4[7] = 0;
00559     
00560     data[3] = broadcastRateByte;
00561     createByte(bitsList2, data[2]);
00562     createByte(bitsList3, data[1]);
00563     createByte(bitsList4, data[0]);
00564         
00565     int hasData = 1;
00566     int isBatch = 0;
00567     int address = UM6_COMMUNICATION;
00568     int BL = 1;
00569     
00570     int PTread = -1;
00571     int Nread = -1;
00572     int addressRead = -1;
00573     int dataRead[1];
00574     
00575     while(addressRead != address){
00576         oneWordWrite(hasData, isBatch, BL, address, data);
00577         wait_ms(stdDelayMs);
00578         oneWordRead(PTread, Nread, addressRead, dataRead);
00579     }
00580     
00581     if(PTread%2 == 1){
00582         printf("Communication configuration failed\r\n");
00583     }
00584     else{
00585         printf("UM6 communication configuration completed\r\n");
00586     }
00587     
00588 }
00589 
00590 //----------------------------------------------------------------------------------------------
00591 
00592 void UM6LT::setConfigParams(int wantPPS, int wantQuatUpdate, int wantCal, int wantAccelUpdate, int wantMagUpdate){
00593     
00594     int data [4] = {0, 0, 0, 0};
00595     int bitsList [8] = {0, 0, 0, 0, 0, 0, 0, 0};
00596     
00597     bitsList[3] = wantPPS;
00598     bitsList[4] = wantQuatUpdate;
00599     bitsList[5] = wantCal;
00600     bitsList[6] = wantAccelUpdate;
00601     bitsList[7] = wantMagUpdate;
00602     
00603     createByte(bitsList, data[0]);
00604     
00605     int hasData = 1;
00606     int isBatch = 0;
00607     int address = UM6_MISC_CONFIG;
00608     int BL = 1;
00609     
00610     int PTread = -1;
00611     int Nread = -1;
00612     int addressRead = -1;
00613     int dataRead[1];
00614         
00615     while(addressRead != address){
00616         oneWordWrite(hasData, isBatch, BL, address, data);
00617         wait_ms(stdDelayMs);
00618         oneWordRead(PTread, Nread, addressRead, dataRead);
00619     }
00620     
00621     if(PTread%2 == 1){
00622         printf("Miscellaneous configuration failed\r\n");
00623     }
00624     else{
00625         printf("UM6 miscellaneous configuration completed\r\n");
00626     }
00627 
00628 }
00629 
00630 //----------------------------------------------------------------------------------------------
00631 
00632 int UM6LT::getStatus(void){
00633 
00634     int PT, N, address;
00635     PT = -1;
00636     N = -1;
00637     address = -1;
00638     
00639     int data[4] = {1, 1, 1, 1};
00640     int bitsList1[8];
00641     int bitsList2[8];
00642     int bitsList3[8];
00643     int bitsList4[8];
00644     
00645     int okToMoveOn = 0;
00646     int count = 0;
00647     
00648     while(!okToMoveOn && count<5){
00649         requestData(UM6_STATUS, 1);
00650         wait_ms(stdDelayMs);
00651         oneWordRead(PT, N, address, data);
00652         okToMoveOn = noCommError(address, UM6_STATUS, PT);
00653         count++;
00654     }
00655     
00656     if(okToMoveOn){
00657         decomposeByte(bitsList1, data[3]);
00658         decomposeByte(bitsList2, data[2]);
00659         decomposeByte(bitsList3, data[1]);
00660         decomposeByte(bitsList4, data[0]);
00661                 
00662         if(bitsList1[0] == 1){
00663             printf("Self-test completed\r\n");
00664             return 0;
00665         }
00666         else if(bitsList2[5] == 1){
00667             printf("Mag data timed out\r\n");
00668             return 0;
00669         }
00670         else if(bitsList2[6] == 1){
00671             printf("Accel data timed out\r\n");
00672             return 0;
00673         }
00674         else if(bitsList2[7] == 1){
00675             printf("Gyro data timed out\r\n");
00676             return 0;
00677         }
00678         else if(bitsList3[0] == 1){
00679             printf("EKF rebooted b/c of state estimate divergence\r\n");
00680             return 0;
00681         }
00682         else if(bitsList3[1] == 1){
00683             printf("Bus error with Mag sensor\r\n");
00684             return 0;
00685         }
00686         else if(bitsList3[2] == 1){
00687             printf("Bus error with Accel sensor\r\n");
00688             return 0;
00689         }
00690         else if(bitsList3[3] == 1){
00691             printf("Bus error with Gyro sensor\r\n");
00692             return 0;
00693         }
00694         else if(bitsList3[4] == 1){
00695             printf("Self-test operation failed on Mag z-axis\r\n");
00696             return 0;
00697         }
00698         else if(bitsList3[5] == 1){
00699             printf("Self-test operation failed on Mag y-axis\r\n");
00700             return 0;
00701         }
00702         else if(bitsList3[6] == 1){
00703             printf("Self-test operation failed on Mag x-axis\r\n");
00704             return 0;
00705         }
00706         else if(bitsList3[7] == 1){
00707             printf("Self-test operation failed on Accel z-axis\r\n");
00708             return 0;
00709         }
00710         else if(bitsList4[0] == 1){
00711             printf("Self-test operation failed on Accel y-axis\r\n");
00712             return 0;
00713         }
00714         else if(bitsList4[1] == 1){
00715             printf("Self-test operation failed on Accel x-axis\r\n");
00716             return 0;
00717         }
00718         else if(bitsList4[2] == 1){
00719             printf("Self-test operation failed on Gyro z-axis\r\n");
00720             return 0;
00721         }
00722         else if(bitsList4[3] == 1){
00723             printf("Self-test operation failed on Gyro y-axis\r\n");
00724             return 0;
00725         }
00726         else if(bitsList4[4] == 1){
00727             printf("Self-test operation failed on Gyro x-axis\r\n");
00728             return 0;
00729         }
00730         else if(bitsList4[5] == 1){
00731             printf("Gyro start-up initialization failed -> Gyro is dead\r\n");
00732             return 0;
00733         }
00734         else if(bitsList4[6] == 1){
00735             printf("Accel start-up initialization failed -> Accel is dead\r\n");
00736             return 0;
00737         }
00738         else if(bitsList4[7] == 1){
00739             printf("Mag start-up initialization failed -> Mag is dead\r\n");
00740             return 0;
00741         }
00742         else{
00743             printf("No problem to report.\r\n");
00744             return 1;
00745         }
00746     }
00747     else{
00748         WhatCommError(address, UM6_STATUS, PT);
00749         return 0;
00750     }
00751 
00752 }
00753 
00754 //----------------------------------------------------------------------------------------------
00755 
00756 int UM6LT::zeroGyros(int& gyroBiasX, int& gyroBiasY, int& gyroBiasZ){
00757 
00758     int PT = -1;
00759     int N = -1;
00760     int address = -1;
00761     int data [8] = {0, 0, 0, 0, 0, 0, 0, 0};
00762     
00763     int okToMoveOn = 0;
00764     int count = 0;
00765     
00766     printf("Do not move the imu: gyro zeroing in progress.\r\n");
00767     
00768     while(!okToMoveOn && count<5){
00769         requestData(UM6_ZERO_GYROS, 0);
00770         wait_ms(stdDelayMs);
00771         oneWordRead(PT, N, address, data);
00772         okToMoveOn = noCommError(address, UM6_ZERO_GYROS, PT);
00773         count++;
00774     }
00775     if(okToMoveOn){
00776         okToMoveOn = 0;
00777         count = 0;
00778         PT = -1;
00779         N = -1;
00780         address = -1;
00781         wait(1);
00782         while(!okToMoveOn && count<5){
00783             wait_ms(stdDelayMs);
00784             oneWordRead(PT, N, address, data);
00785             okToMoveOn = noCommError(address, UM6_GYRO_BIAS_XY, PT);
00786             count++;
00787         }
00788         
00789         if(okToMoveOn){
00790             gyroBiasX = twosComplement(data[0], data[1]);
00791             gyroBiasY = twosComplement(data[2], data[3]);
00792             gyroBiasZ = twosComplement(data[4], data[5]);
00793             printf("Gyro zeroing completed.\r\n");
00794             return 1;
00795         }
00796         else{
00797             WhatCommError(address, UM6_GYRO_BIAS_XY, PT);
00798             printf("Could not acquire the values of gyro biases.\r\n");
00799             return 0;
00800         }
00801     }
00802     else{
00803         WhatCommError(address, UM6_ZERO_GYROS, PT);
00804         printf("Could not trigger gyro zeroing script.\r\n");
00805         return 0;
00806     }
00807 
00808 }
00809 
00810 //----------------------------------------------------------------------------------------------
00811 
00812 int UM6LT::autoSetAccelRef(void){
00813 
00814     int address = UM6_SET_ACCEL_REF;
00815     
00816     if(sendOneCommand(address)){
00817         return 1;
00818     }
00819     else{
00820         return 0;
00821     }
00822     
00823 }
00824 
00825 //----------------------------------------------------------------------------------------------
00826 
00827 int UM6LT::autoSetMagRef(void){
00828 
00829     int address = UM6_SET_MAG_REF;
00830     
00831     if(sendOneCommand(address)){
00832         return 1;
00833     }
00834     else{
00835         return 0;
00836     }
00837 }
00838 
00839 //----------------------------------------------------------------------------------------------
00840 
00841 int UM6LT::resetEKF(void){
00842 
00843     int address = UM6_RESET_EKF;
00844     
00845     if(sendOneCommand(address)){
00846         return 1;
00847     }
00848     else{
00849         return 0;
00850     }
00851 }
00852 
00853 //----------------------------------------------------------------------------------------------
00854 
00855 int UM6LT::writeToFlash(void){
00856 
00857     int address = UM6_FLASH_COMMIT;
00858     
00859     if(sendOneCommand(address)){
00860         return 1;
00861     }
00862     else{
00863         return 0;
00864     }
00865 }
00866 
00867 //----------------------------------------------------------------------------------------------
00868 
00869 int UM6LT::resetToFactory(void){
00870 
00871     int address = UM6_RESET_TO_FACTORY;
00872     
00873     if(sendOneCommand(address)){
00874         return 1;
00875     }
00876     else{
00877         return 0;
00878     }
00879 
00880 }
00881 
00882 //----------------------------------------------------------------------------------------------
00883 
00884 int UM6LT::getAngles(int& roll, int& pitch, int& yaw){
00885 
00886     roll = 0;
00887     pitch= 0;
00888     yaw= 0;
00889     int data[4] = {0, 0, 0, 0};
00890     int address = UM6_EULER_PHI_THETA;
00891     
00892     if(getTwoBatches(address, data)){
00893         roll = data[0];
00894         pitch = data[1];
00895         yaw = data[2];
00896         return 1;
00897     }
00898     else{
00899         return 0;
00900     }
00901     
00902 }
00903 
00904 //----------------------------------------------------------------------------------------------
00905 
00906 
00907 int UM6LT::getAccel(int& accelX, int& accelY, int& accelZ){
00908 
00909     accelX = 0;
00910     accelY = 0;
00911     accelZ = 0;
00912     int data[4] = {0, 0, 0, 0};
00913     int address = UM6_ACCEL_PROC_XY;
00914     
00915     if(getTwoBatches(address, data)){
00916         accelX = data[0];
00917         accelY = data[1];
00918         accelZ = data[2];
00919         return 1;
00920     }
00921     else{
00922         return 0;
00923     }
00924     
00925 }
00926 
00927 //----------------------------------------------------------------------------------------------
00928 
00929 
00930 int UM6LT::getMag(int& magX, int& magY, int& magZ){
00931 
00932     magX = 0;
00933     magY = 0;
00934     magZ = 0;
00935     int data[4] = {0, 0, 0, 0};
00936     int address = UM6_MAG_PROC_XY;
00937     
00938     if(getTwoBatches(address, data)){
00939         magX = data[0];
00940         magY = data[1];
00941         magZ = data[2];
00942         return 1;
00943     }
00944     else{
00945         return 0;
00946     }
00947     
00948 }
00949 
00950 //----------------------------------------------------------------------------------------------
00951 
00952 
00953 int UM6LT::getAngleRates(int& rollRate, int& pitchRate, int& yawRate){
00954 
00955     rollRate = 0;
00956     pitchRate = 0;
00957     yawRate = 0;
00958     int data[4] = {0, 0, 0, 0};
00959     int address = UM6_GYRO_PROC_XY;
00960     
00961     if(getTwoBatches(address, data)){
00962         rollRate = data[0];
00963         pitchRate = data[1];
00964         yawRate = data[2];
00965         return 1;
00966     }
00967     else{
00968         return 0;
00969     }
00970     
00971 }
00972 
00973 //----------------------------------------------------------------------------------------------
00974 
00975 int UM6LT::getQuaternion(int& a, int& b, int& c, int& d){
00976 
00977     a = 0;
00978     b = 0;
00979     c = 0;
00980     d = 0;
00981     int data[4] = {0, 0, 0, 0};
00982     int address = UM6_QUAT_AB;
00983     
00984     if(getTwoBatches(address, data)){
00985         a = data[0];
00986         b = data[1];
00987         c = data[2];
00988         d = data[3];
00989         return 1;
00990     }
00991     else{
00992         return 0;
00993     }
00994     
00995 }
00996 
00997 //----------------------------------------------------------------------------------------------