UbxGpsNavSol.cpp

00001 #include "UbxGpsNavSol.hh"
00002 #include "mbed.h"
00003 
00004 UbxGpsNavSol::UbxGpsNavSol(PinName tx, PinName rx, int baud):UARTSerial(tx, rx, baud){
00005     this->carriagePosition = 0;
00006     // this->setLength(52);
00007     this->offsetClassProperties = 8;
00008     this->offsetHeaders = 4;
00009 }
00010 
00011 UbxGpsNavSol::~UbxGpsNavSol(){}
00012 
00013 bool UbxGpsNavSol::ready(){
00014     //unsigned char buffer[60];
00015 
00016     if(this->readable()){
00017       for(int k = 0; k < 60; k++)
00018         this->buffer[k] = 0;
00019 
00020       this->read(this->buffer, 60);
00021     }
00022 
00023     if(this->buffer[0] != UBX_NAV_SOL_HEADER_1 || this->buffer[1] != UBX_NAV_SOL_HEADER_2)
00024       return false;
00025 
00026     if(this->buffer[2] != UBX_NAV_SOL_CLASS || this->buffer[3] != UBX_NAV_SOL_ID)
00027       return false;
00028 
00029     payload_length = this->buffer[5];
00030     payload_length = payload_length << 8;
00031     payload_length = this->buffer[4];
00032 
00033     if(payload_length != UBX_NAV_SOL_PAYLOAD_LENGTH)
00034       return false;
00035 
00036      if(calculateChecksum(buffer) == false)
00037        return false;
00038 
00039     iTOW = this->buffer[9] << 8;
00040     iTOW |= this->buffer[8];
00041     iTOW <<= 8;
00042     iTOW |= this->buffer[7];
00043     iTOW <<= 8;
00044     iTOW |= this->buffer[6];
00045 
00046     fTOW = this->buffer[13] << 8;
00047     fTOW |= this->buffer[12];
00048     fTOW <<= 8;
00049     fTOW |= this->buffer[11];
00050     fTOW <<= 8;
00051     fTOW |= this->buffer[10];
00052 
00053     week = this->buffer[15] << 8;
00054     week |= this->buffer[14];
00055 
00056     gpsFix = this->buffer[16];
00057 
00058     flags = this->buffer[17];
00059 
00060     ecefX = this->buffer[21] << 8;
00061     ecefX |= this->buffer[20];
00062     ecefX <<= 8;
00063     ecefX |= this->buffer[19];
00064     ecefX <<= 8;
00065     ecefX |= this->buffer[18];
00066 
00067     ecefY = this->buffer[25] << 8;
00068     ecefY |= this->buffer[24];
00069     ecefY <<= 8;
00070     ecefY |= this->buffer[23];
00071     ecefY <<= 8;
00072     ecefY |= this->buffer[22];
00073 
00074     ecefZ = this->buffer[29] << 8;
00075     ecefZ |= this->buffer[28];
00076     ecefZ <<= 8;
00077     ecefZ |= this->buffer[27];
00078     ecefZ <<= 8;
00079     ecefZ |= this->buffer[26];
00080 
00081     pAcc = this->buffer[33] << 8;
00082     pAcc |= this->buffer[32];
00083     pAcc <<= 8;
00084     pAcc |= this->buffer[31];
00085     pAcc <<= 8;
00086     pAcc |= this->buffer[30];
00087 
00088     ecefVX = this->buffer[37] << 8;
00089     ecefVX |= this->buffer[36];
00090     ecefVX <<= 8;
00091     ecefVX |= this->buffer[35];
00092     ecefVX <<= 8;
00093     ecefVX |= this->buffer[34];
00094 
00095     ecefVY = this->buffer[41] << 8;
00096     ecefVY |= this->buffer[40];
00097     ecefVY <<= 8;
00098     ecefVY |= this->buffer[39];
00099     ecefVY <<= 8;
00100     ecefVY |= this->buffer[38];
00101 
00102     ecefVZ = this->buffer[45] << 8;
00103     ecefVZ |= this->buffer[44];
00104     ecefVZ <<= 8;
00105     ecefVZ |= this->buffer[43];
00106     ecefVZ <<= 8;
00107     ecefVZ |= this->buffer[42];
00108 
00109     sAcc = this->buffer[49] << 8;
00110     sAcc |= this->buffer[48];
00111     sAcc <<= 8;
00112     sAcc |= this->buffer[47];
00113     sAcc <<= 8;
00114     sAcc |= this->buffer[46];
00115 
00116     pDOP = this->buffer[51] << 8;
00117     pDOP |= this->buffer[50];
00118 
00119     reserved1 = this->buffer[52];
00120 
00121     numSV = this->buffer[53];
00122 
00123     reserved2 = this->buffer[57] << 8;
00124     reserved2 |= this->buffer[56];
00125     reserved2 <<= 8;
00126     reserved2 |= this->buffer[55];
00127     reserved2 <<= 8;
00128     reserved2 |= this->buffer[54];
00129 
00130     return true;
00131 }
00132 
00133 bool UbxGpsNavSol::calculateChecksum(unsigned char *buffer){
00134   uint8_t check_a, check_b;
00135   check_a = check_b = 0;
00136 
00137   for(int i = 2; i < 58; i++){
00138     check_a += buffer[i];
00139     check_b += check_a;
00140   }
00141 
00142   if(check_a == buffer[59] && check_b == buffer[58])
00143     return true;
00144 
00145   else
00146     return false;
00147 }
00148 
00149 bool UbxGpsNavSol::enableNAVSOL(){
00150   const uint8_t buffer[] = {
00151     0xB5, // sync char 1 1
00152     0x62, // sync char 2 2
00153     0x06, // class 3
00154     0x01, // id 4
00155     0x02, // length 5
00156     0x00, // length 6
00157     0x01, // payload 7
00158     0x06, // payload 8
00159     0x10,//CKA
00160     0x39,//CKB
00161 
00162   };
00163   if(this->writable()){
00164     this->write(buffer, 10);
00165     return true;
00166   }
00167 
00168   return false;
00169 }
00170 
00171 bool UbxGpsNavSol::restoreDefaults(){
00172   const uint8_t packet[] = {
00173     0xB5, // sync char 1
00174     0x62, // sync char 2
00175     0x06, // class
00176     0x09, // id
00177     0x0D, // length
00178     0x00, // length
00179     0xFF, // payload
00180     0xFF, // payload
00181     0x00, // payload
00182     0x00, // payload
00183     0x00, // payload
00184     0x00, // payload
00185     0x00, // payload
00186     0x00, // payload
00187     0xFF, // payload
00188     0xFF, // payload
00189     0x00, // payload
00190     0x00, // payload
00191     0x17, // payload
00192     0x2F, // CK_A
00193     0xAE, // CK_B
00194   };
00195 
00196   if(this->writable()){
00197     this->write(packet, 21);
00198     return true;
00199   }
00200 
00201   return false;
00202 }
00203 
00204 void UbxGpsNavSol::disableNmea(){
00205   const uint8_t messages[][2] = {
00206       {0xF0, 0x0A},
00207       {0xF0, 0x09},
00208       {0xF0, 0x00},
00209       {0xF0, 0x01},
00210       {0xF0, 0x0D},
00211       {0xF0, 0x06},
00212       {0xF0, 0x02},
00213       {0xF0, 0x07},
00214       {0xF0, 0x03},
00215       {0xF0, 0x04},
00216       {0xF0, 0x0E},
00217       {0xF0, 0x0F},
00218       {0xF0, 0x05},
00219       {0xF0, 0x08},
00220       {0xF1, 0x00},
00221       {0xF1, 0x01},
00222       {0xF1, 0x03},
00223       {0xF1, 0x04},
00224       {0xF1, 0x05},
00225       {0xF1, 0x06},
00226   };
00227 
00228   // CFG-MSG packet buffer
00229   uint8_t packet[] = {
00230       0xB5, // sync char 1
00231       0x62, // sync char 2
00232       0x06, // class
00233       0x01, // id
00234       0x03, // length
00235       0x00, // length
00236       0x00, // payload (first byte from messages array element)
00237       0x00, // payload (second byte from messages array element)
00238       0x00, // payload (not changed in the case)
00239       0x00, // CK_A
00240       0x00, // CK_B
00241   };
00242 
00243   uint8_t packetSize = sizeof(packet);
00244 
00245   // Offset to the place where payload starts
00246   uint8_t payloadOffset = 6;
00247 
00248   // Iterate over the messages array
00249   for (uint8_t i = 0; i < sizeof(messages) / sizeof(*messages); i++)
00250   {
00251       // Copy two bytes of payload to the packet buffer
00252       for (uint8_t j = 0; j < sizeof(*messages); j++)
00253       {
00254           packet[payloadOffset + j] = messages[i][j];
00255       }
00256 
00257       // Set checksum bytes to the null
00258       packet[packetSize - 2] = 0x00;
00259       packet[packetSize - 1] = 0x00;
00260 
00261       // Calculate checksum over the packet buffer excluding sync (first two) and checksum chars (last two)
00262       for (uint8_t j = 0; j < packetSize - 4; j++)
00263       {
00264           packet[packetSize - 2] += packet[2 + j];
00265           packet[packetSize - 1] += packet[packetSize - 2];
00266       }
00267 
00268       if(this->writable()){
00269         this->write(packet, packetSize);
00270       }
00271     }
00272 }
00273 
00274 bool UbxGpsNavSol::changeBaudrate(){
00275   const uint8_t packet[] = {
00276     0xB5, // sync char 1
00277     0x62, // sync char 2
00278     0x06, // class
00279     0x00, // id
00280     0x20, // length
00281     0x00, // length
00282     0x02, // payload - portID
00283     0x00, // payload - reserved0
00284     0x00, // payload - txReady
00285     0x00, // payload - txReady
00286     0xD0, // payload - mode
00287     0x08, // payload - mode
00288     0x00, // payload - mode
00289     0x00, // payload - mode
00290     0x00, // payload - reserved3
00291     0xC2, // payload - reserved3
00292     0x01, // payload - reserved3
00293     0x00, // payload - reserved3
00294     0x07, // payload - inProtoMask
00295     0x00, // payload - inProtoMask
00296     0x03, // payload - outProtoMask --> activate NMEA???
00297     0x00, // payload - outProtoMask
00298     0x00, // payload - flags
00299     0x00, // payload - flags
00300     0x00, // payload - reserved5
00301     0x00, // payload - reserved5
00302     0xCC, // CK_A
00303     0xBA, // CK_B
00304   };
00305 
00306   if(this->writable()){
00307     this->write(packet, sizeof(packet));
00308     return true;
00309   }
00310 
00311   return false;
00312 }
00313 
00314 bool UbxGpsNavSol::disableUnnecessaryChannels(){
00315   const uint8_t packet[] = {
00316     0xB5, // sync char 1
00317     0x62, // sync char 2
00318     0x06, // class
00319     0x3E, // id
00320     0x24, // length
00321     0x00, // length
00322 
00323     0x00, 0x00, 0x16, 0x04, 0x00, 0x04, 0xFF, 0x00, // payload
00324     0x01, 0x00, 0x00, 0x01, 0x01, 0x01, 0x03, 0x00, // payload
00325     0x00, 0x00, 0x00, 0x01, 0x05, 0x00, 0x03, 0x00, // payload
00326     0x00, 0x00, 0x00, 0x01, 0x06, 0x08, 0xFF, 0x00, // payload
00327     0x00, 0x00, 0x00, 0x01,                         // payload
00328 
00329     0xA4, // CK_A
00330     0x25, // CK_B
00331   };
00332 
00333   if(this->writable()){
00334     this->write(packet, sizeof(packet));
00335     return true;
00336   }
00337 
00338   return false;
00339 }
00340 
00341 bool UbxGpsNavSol::changeFrequency(){
00342   const uint8_t packet[] = {
00343         0xB5, // sync char 1
00344         0x62, // sync char 2
00345         0x06, // class
00346         0x08, // id
00347         0x06, // length
00348         0x00, // length
00349         0x64, // payload
00350         0x00, // payload
00351         0x01, // payload
00352         0x00, // payload
00353         0x01, // payload
00354         0x00, // payload
00355         0x7A, // CK_A
00356         0x12, // CK_B
00357     };
00358 
00359     if(this->writable()){
00360       this->write(packet, sizeof(packet));
00361       return true;
00362     }
00363 
00364     return false;
00365 }
00366 
00367 bool UbxGpsNavSol::disableGNSS(){
00368     const uint8_t packet[] = {
00369       0xB5, 0x62, 0x06, 0x3E, 0x3C, 0x00,
00370       0x00, 0x00, 0x20, 0x07, 0x00, 0x08,
00371       0x10, 0x00, 0x01, 0x00, 0x01, 0x01,
00372       0x01, 0x01, 0x03, 0x00, 0x00, 0x00,
00373       0x01, 0x01, 0x02, 0x04, 0x08, 0x00,
00374       0x00, 0x00, 0x01, 0x01, 0x03, 0x08,
00375       0x10, 0x00, 0x00, 0x00, 0x01, 0x01,
00376       0x04, 0x00, 0x08, 0x00, 0x00, 0x00,
00377       0x01, 0x01, 0x05, 0x00, 0x03, 0x00,
00378       0x00, 0x00, 0x01, 0x01, 0x06, 0x08,
00379       0x0E, 0x00, 0x00, 0x00, 0x01, 0x01,
00380       0x2C, 0x4D
00381     };
00382 
00383     if(this->writable()){
00384       this->write(packet, sizeof(packet));
00385       return true;
00386     }
00387 
00388     return false;
00389 }
00390 
00391 void UbxGpsNavSol::baud(int baud){
00392   this->set_baud(baud);
00393 }
00394 
00395 bool UbxGpsNavSol::saveConfiguration() {
00396     const uint8_t packet[] = {
00397     0xB5, // sync char 1
00398     0x62, // sync char 2
00399     0x06, // class
00400     0x09, // id
00401     0x0D, // length
00402     0x00, // length
00403     0x00, // payload
00404     0x00, // payload
00405     0x00, // payload
00406     0x00, // payload
00407     0xFF, // payload
00408     0xFF, // payload
00409     0x00, // payload
00410     0x00, // payload
00411     0x00, // payload
00412     0x00, // payload
00413     0x00, // payload
00414     0x00, // payload
00415     0x17, // payload
00416     0x31, // CK_A
00417     0xBF, // CK_B
00418   };
00419 
00420     if(this->writable()){
00421       this->write(packet, sizeof(packet));
00422       return true;
00423     }
00424 
00425     return false;
00426 }
00427