MAX8U.cpp

00001 //MAX 8U GPS Driver
00002 
00003 //Author: Adhyyan Sekhsaria, Jamie Smith
00004 
00005 //Written with the help of the following data sheets:
00006 //https://www.u-blox.com/sites/default/files/MAX-8-M8-FW3_HardwareIntegrationManual_%28UBX-15030059%29.pdf
00007 //https://www.u-blox.com/en/docs/UBX-13003221
00008 
00009 #include "MAX8U.h"
00010 
00011 #include <cinttypes>
00012 #include <algorithm>
00013 
00014 #define MAX8U_DEBUG 1
00015 
00016 const char* GNSSNames[] = {"GPS", "SBAS", "Galileo", "BeiDou", "IMES", "QZSS", "GLONASS"};
00017 
00018 const char *MAX8U::SatelliteInfo::getGNSSName()
00019 {
00020     return GNSSNames[static_cast<uint8_t>(gnss)];
00021 }
00022 
00023 
00024 MAX8U::MAX8U(Serial *debugPort, PinName user_SDApin, PinName user_SCLpin, PinName user_RSTPin,
00025              uint8_t i2cAddress, int i2cPortSpeed) :
00026     _debugPort(debugPort),
00027     _i2cPort(user_SDApin, user_SCLpin),
00028     _i2cAddress(i2cAddress),
00029     _rst(user_RSTPin, 1)
00030 
00031 {
00032 
00033     //Get user settings
00034     _i2cPortSpeed = i2cPortSpeed;
00035 
00036     #define MAX8U_MAX_SPEED 4000000 // 400 kHz
00037     if(_i2cPortSpeed > MAX8U_MAX_SPEED)
00038     {
00039         _i2cPortSpeed = MAX8U_MAX_SPEED; 
00040     }
00041     _i2cPort.frequency(_i2cPortSpeed);
00042 }
00043 
00044 
00045 bool MAX8U::begin(){
00046     //Configure the MAX8U for I2C communication
00047 
00048     _rst = 0; // Reset BNO080
00049     wait(.002f); // Min length not specified in datasheet?
00050     _rst = 1; // Bring out of reset
00051 
00052     //TODO: wait for the GPS to boot. Couldnt find any pin in the data sheet which would be useful
00053     wait(0.1f); //for now, just wait for some time
00054 
00055     if(checkVersion())
00056     {
00057 #ifdef MAX8U_DEBUG
00058         _debugPort->printf("MAX8U booted up");
00059 #endif
00060         return true;
00061     }
00062     else
00063     {
00064         _debugPort->printf("MAX8U not detected!");
00065         return false;
00066     }
00067 }
00068 
00069 bool MAX8U::update()
00070 {
00071     bool gotAnyMessages = false;
00072 
00073     while(readNextMessage())
00074     {
00075         processMessage();
00076 
00077         gotAnyMessages = true;
00078     }
00079 
00080 
00081     return gotAnyMessages;
00082 }
00083 
00084 bool MAX8U::configure()
00085 {
00086     // switch to UBX mode
00087     if(!configureCommSettings())
00088     {
00089         return false;
00090     }
00091 
00092     // enable NAV messages
00093     // latitude, longitude, and height
00094     if(!setMessageEnabled(UBX_CLASS_NAV, UBX_NAV_POSLLH, true))
00095     {
00096         return false;
00097     }
00098 
00099     if(!setMessageEnabled(UBX_CLASS_NAV, UBX_NAV_SOL, true))
00100     {
00101         return false;
00102     }
00103 
00104     if(!setMessageEnabled(UBX_CLASS_NAV, UBX_NAV_TIMEUTC, true))
00105     {
00106         return false;
00107     }
00108 
00109 
00110     if(!setMessageEnabled(UBX_CLASS_NAV, UBX_NAV_VELNED, true))
00111     {
00112         return false;
00113     }
00114 
00115     return saveSettings();
00116 
00117 }
00118 
00119 bool MAX8U::setMessageEnabled(uint8_t messageClass, uint8_t messageID, bool enabled)
00120 {
00121     uint8_t data[3];
00122 
00123     data[0] = messageClass; // byte 0: class
00124     data[1] = messageID; // byte 1: ID
00125     data[2] = static_cast<uint8_t>(enabled ? 1 : 0); // byte 2: rate
00126 
00127     if(!sendCommand(UBX_CLASS_CFG, UBX_CFG_MSG, data, sizeof(data)))
00128     {
00129         return false;
00130     }
00131 
00132     return waitForACK(UBX_CLASS_CFG, UBX_CFG_MSG);
00133 }
00134 
00135 MAX8U::AntennaPowerStatus MAX8U::antennaPowerStatus() {
00136     sendCommand(UBX_CLASS_MON, UBX_MON_HW, nullptr, 0);
00137 
00138     if(!waitForMessage(UBX_CLASS_MON, UBX_MON_HW))
00139     {
00140         return MAX8U::AntennaPowerStatus::NO_MESSAGE_RCVD;
00141     }
00142 
00143     //else print out whether its on or off
00144     MAX8U::AntennaPowerStatus powerStatus = AntennaPowerStatus (static_cast<uint8_t >(buffer[UBX_DATA_OFFSET + 21]));
00145     return powerStatus;
00146 }
00147 
00148 ssize_t MAX8U::readSatelliteInfo(MAX8U::SatelliteInfo *satelliteInfos, size_t infoLen)
00149 {
00150     sendCommand(UBX_CLASS_NAV, UBX_NAV_SAT, nullptr, 0);
00151 
00152     if(!waitForMessage(UBX_CLASS_NAV, UBX_NAV_SAT))
00153     {
00154         return -1;
00155     }
00156 
00157     uint8_t satellitesReturned = static_cast<uint8_t>(buffer[UBX_DATA_OFFSET + 5]);
00158 
00159     for(size_t i = 0; i < std::min(static_cast<size_t>(satellitesReturned), infoLen); i++)
00160     {
00161         // detect a buffer overrun in the case where more satellites were returned than could
00162         // fit in the buffer
00163         size_t flagOffset = UBX_DATA_OFFSET + 16 + 12*i;
00164         if(flagOffset >= bufferMaxLen)
00165         {
00166             _debugPort->printf("Error: NAV-SAT message truncated by receive buffer size!\r\n");
00167 
00168             // keep the part that was valid
00169             return i;
00170         }
00171 
00172         satelliteInfos[i].gnss = static_cast<GNSSID>(static_cast<uint8_t>(buffer[UBX_DATA_OFFSET + 8 + 12 * i]));
00173         satelliteInfos[i].satelliteID = static_cast<uint8_t>(buffer[UBX_DATA_OFFSET + 9 + 12*i]);
00174         satelliteInfos[i].signalStrength = static_cast<uint8_t>(buffer[UBX_DATA_OFFSET + 10 + 12*i]);
00175 
00176         uint32_t flag = readUnalignedValue<uint32_t>(buffer, flagOffset);
00177 
00178         satelliteInfos[i].signalQuality = (flag & 0x0007);
00179         satelliteInfos[i].svUsed = (flag & (1<<3));
00180 
00181         //satelliteInfos[i].elevation = static_cast<int8_t >(buffer[UBX_DATA_OFFSET])
00182 
00183 #if MAX8U_DEBUG
00184         _debugPort->printf("NAV_SAT Strength for %s %" PRIu8 ":: %" PRIu8 " dBHz; Quality: %" PRIu8 "\r\n",
00185                            satelliteInfos[i].getGNSSName(), satelliteInfos[i].satelliteID,
00186                            satelliteInfos[i].signalStrength, satelliteInfos[i].signalQuality);
00187 #endif
00188     }
00189 
00190     return satellitesReturned;
00191 }
00192 
00193 
00194 void MAX8U::processMessage()
00195 {
00196     if(buffer[UBX_BYTE_CLASS] == UBX_CLASS_NAV)
00197     {
00198         switch(buffer[UBX_BYTE_ID])
00199         {
00200             case UBX_NAV_POSLLH:
00201                 processNAV_POSLLH();
00202                 break;
00203             case UBX_NAV_SOL:
00204                 processNAV_SOL();
00205                 break;
00206             case UBX_NAV_TIMEUTC:
00207                 processNAV_TIMEUTC();
00208                 break;
00209             case UBX_NAV_VELNED:
00210                 processNAV_VELNED();
00211                 break;
00212         }
00213 
00214     }
00215 }
00216 void MAX8U::processNAV_VELNED() {
00217     northVel = readUnalignedValue<int32_t>(buffer, UBX_DATA_OFFSET + 4);
00218     eastVel = readUnalignedValue<int32_t>(buffer, UBX_DATA_OFFSET + 8);
00219     downVel = readUnalignedValue<int32_t>(buffer, UBX_DATA_OFFSET + 12);
00220     speed3D = readUnalignedValue<uint32_t>(buffer, UBX_DATA_OFFSET + 16);
00221 
00222 #if MAX8U_DEBUG
00223     _debugPort->printf("Got NAV_VELNED message.  North Vel=%" PRIi32 ", East Vel=%" PRIi32 ", Down Vel=%" PRIi32 ", 3D Speed=%" PRIu32 "\r\n",
00224             northVel, eastVel, downVel, speed3D);
00225 #endif
00226 }
00227 
00228 
00229 void MAX8U::processNAV_POSLLH()
00230 {
00231     // read latitude and longitude
00232     int32_t longitudeInt = readUnalignedValue<int32_t>(buffer, UBX_DATA_OFFSET + 4);
00233     longitude = longitudeInt * 1e-7;
00234 
00235     int32_t latitudeInt = readUnalignedValue<int32_t>(buffer, UBX_DATA_OFFSET + 8);
00236     latitude = latitudeInt * 1e-7;
00237 
00238     height = readUnalignedValue<int32_t>(buffer, UBX_DATA_OFFSET + 12)/(1000.0f); //height above ellipsoid
00239 
00240 #if MAX8U_DEBUG
00241     _debugPort->printf("Got NAV_POSLLH message.  Longitude=%.06f deg, Latitude=%.06f deg, Height=%.02f m\r\n", longitude, latitude, height);
00242 #endif
00243 }
00244 
00245 
00246 void MAX8U::processNAV_SOL()
00247 {
00248     fixQuality = static_cast<GPSFix>(buffer[UBX_DATA_OFFSET + 10]);
00249 
00250     posAccuracy = readUnalignedValue<uint32_t>(buffer, UBX_DATA_OFFSET + 24) / 100.0f;
00251 
00252     numSatellites = static_cast<uint8_t>(buffer[UBX_DATA_OFFSET + 47]);
00253 
00254 #if MAX8U_DEBUG
00255     _debugPort->printf("Got NAV_SOL message.  Fix quality=%" PRIu8 ", Pos accuracy=%.02f m, Num satellites=%" PRIu8 "\r\n",
00256                        static_cast<uint8_t>(fixQuality), posAccuracy, numSatellites);
00257 #endif
00258 }
00259 
00260 void MAX8U::processNAV_TIMEUTC()
00261 {
00262     year = readUnalignedValue<uint16_t>(buffer, UBX_DATA_OFFSET + 12);
00263 
00264     month = static_cast<uint8_t>(buffer[UBX_DATA_OFFSET + 14]);
00265 
00266     day = static_cast<uint8_t>(buffer[UBX_DATA_OFFSET + 15]);
00267 
00268     hour = static_cast<uint8_t>(buffer[UBX_DATA_OFFSET + 16]);
00269 
00270     minute = static_cast<uint8_t>(buffer[UBX_DATA_OFFSET + 17]);
00271 
00272     second = static_cast<uint8_t>(buffer[UBX_DATA_OFFSET + 18]);
00273 
00274 #if MAX8U_DEBUG
00275     _debugPort->printf("Got NAV_TIMEUTC message.  year=%" PRIu16 ", month =%" PRIu8 ", day=%" PRIu8
00276             ", hour = %" PRIu8 ", min = %" PRIu8 ", sec = %" PRIu8 "\r\n",
00277                        year, month, day, hour, minute, second);
00278 #endif
00279 
00280 }
00281 
00282 bool MAX8U::waitForMessage(uint8_t messageClass, uint8_t messageID, float timeout)
00283 {
00284     Timer timeoutTimer;
00285     timeoutTimer.start();
00286 
00287     while(timeoutTimer.read() <= timeout)
00288     {
00289         bool newMessageReceived = readNextMessage();
00290 
00291         //We dont use TX Ready pin here, because it isn't configured when the GPS is first set up.
00292 
00293         if(!newMessageReceived){
00294             wait(0.001f);
00295             continue;
00296         }
00297 
00298         if(messageClass == buffer[UBX_BYTE_CLASS] && messageID == buffer[UBX_BYTE_ID])
00299         {
00300             // found correct packet!
00301             return true;
00302         }
00303         else
00304         {
00305             // other data message, send to proper channels
00306             processMessage();
00307         }
00308 
00309     }
00310 
00311     _debugPort->printf("Timeout waiting for message 0x%02" PRIx8 " 0x%02" PRIx8 ".\r\n", messageClass, messageID);
00312     return false;
00313 }
00314 
00315 bool MAX8U::waitForACK(uint8_t sentMessageClass, uint8_t sentMessageID, float timeout)
00316 {
00317     // NOTE: we assume that we wait for an ACK before sending another message, so
00318     // there will never be two ACKs in play at once
00319 
00320     if(!waitForMessage(UBX_CLASS_ACK, UBX_ACK_ACK))
00321     {
00322         _debugPort->printf("Timeout waiting for ACK for message 0x%02" PRIx8 " 0x%02" PRIx8 "\r\n",
00323             sentMessageClass, sentMessageID);
00324         return false;
00325     }
00326 
00327     // check the byte IDs
00328     if(buffer[UBX_DATA_OFFSET] != sentMessageClass || buffer[UBX_DATA_OFFSET+1] != sentMessageID)
00329     {
00330         _debugPort->printf("Ack rcvd for wrong message\r\n");
00331         return false;
00332     }
00333 
00334 
00335     return true;
00336 }
00337 
00338 bool MAX8U::configureCommSettings(){
00339     //Configure DDC(I2C) by writing
00340 
00341     //Configures the MAX8 to output in UBX format instead of NMEA format.
00342 
00343     /*
00344     UBX-CFG-PRT Payload
00345     1 PortId  = 0
00346     1 reserved1
00347     2 txReady 
00348     4 mode - 7 address and 0 for write
00349     4 reserved - all 0s
00350     2 inProtoMask - keep 0th bit on, rest off
00351     2 outProtoMask - keep 0th bit on, rest off
00352     2 flags - all 0
00353     2 reserved - all 0
00354     */
00355 
00356     uint16_t dataLen = 20;
00357     uint8_t data[20];
00358     data[0]  = 0; //Port Id
00359     data[1]  = 0; //Reserved
00360 
00361     // disable TX ready
00362     data[2]  = 0;
00363     data[3]  = 0;
00364 
00365     data[4]  = (_i2cAddress<<1);
00366     data[5]  = 0;
00367     data[6]  = 0;
00368     data[7]  = 0;
00369 
00370     data[8]  = 0;
00371     data[9]  = 0;
00372     data[10] = 0;
00373     data[11] = 0;
00374 
00375     data[12] = 0x01; //enabling UBX mode for input
00376     data[13] = 0;
00377 
00378     data[14] = 0x01; //enabling UBX mode for output
00379     data[15] = 0;
00380 
00381     data[16] = 0;
00382     data[17] = 0;
00383     data[18] = 0;
00384     data[19] = 0;
00385 
00386 
00387     if(!sendCommand(UBX_CLASS_CFG, UBX_CFG_PRT, data, dataLen))
00388     {
00389         return false;
00390     }
00391 
00392     return waitForACK(UBX_CLASS_CFG, UBX_CFG_PRT);
00393 }
00394 
00395 //send the header then the data to the Max8 on the default address 0x42
00396 //Returns false if sensor does not ACK
00397 bool MAX8U::sendCommand(uint8_t messageClass, uint8_t messageID, uint8_t *data, uint16_t dataLen)
00398 {
00399 
00400     //using UBX protocol
00401 
00402     // start the transaction and contact the IMU
00403     _i2cPort.start();
00404 
00405     // to indicate an i2c read, shift the 7 bit address up 1 bit and keep bit 0 as a 0
00406     int writeResult = _i2cPort.write(_i2cAddress << 1);
00407 
00408     if(writeResult != 1)
00409     {
00410         _debugPort->printf("MAX8U I2C write failed!\r\n");
00411         _i2cPort.stop();
00412         return false;
00413     }
00414     
00415     #if MAX8U_DEBUG
00416         _debugPort->printf("MAX8U I2C write acked!\r\n");
00417     #endif
00418 
00419     //compute checksum on header and data. Refer to datasheet
00420     uint8_t chka = 0, chkb = 0;
00421 
00422     // add header portions to checksum;
00423     chka += messageClass;
00424     chkb += chka;
00425 
00426     chka += messageID;
00427     chkb += chka;
00428 
00429     chka += dataLen & 0xFF;
00430     chkb += chka;
00431 
00432     chka += dataLen >> 8;
00433     chkb += chka;
00434 
00435     // add data to checksum
00436     for(int i = 0; i < dataLen; i++){
00437         chka = chka + data[i];
00438         chkb = chkb + chka;
00439     }
00440 
00441     //send the sync chars
00442     _i2cPort.write(UBX_SYNC_CHAR_1);
00443     _i2cPort.write(UBX_SYNC_CHAR_2);
00444 
00445     // send the header
00446     _i2cPort.write(messageClass);
00447     _i2cPort.write(messageID);
00448     _i2cPort.write(dataLen & 0xFF);
00449     _i2cPort.write(dataLen >> 8);
00450 
00451 
00452     for(uint8_t i = 0; i < dataLen; i++){
00453         _i2cPort.write(data[i]);
00454     }
00455 
00456     _i2cPort.write(chka);
00457     _i2cPort.write(chkb);
00458 
00459     _i2cPort.stop();
00460 
00461 #if  MAX8U_DEBUG
00462     _debugPort->printf("Sending: %02" PRIx8 " %02" PRIx8 " %02" PRIx8 " %02" PRIx8 " %02" PRIx8 " %02" PRIx8,
00463         UBX_SYNC_CHAR_1, UBX_SYNC_CHAR_2, messageClass, messageID, dataLen & 0xFF, dataLen >> 8);
00464 
00465     for(uint16_t i = 0; i < dataLen; i++){
00466         _debugPort->printf(" %02" PRIx8, data[i]);
00467     }
00468     _debugPort->printf(" %02" PRIx8 " %02" PRIx8 "\r\n", chka, chkb);
00469 #endif
00470 
00471     return true;
00472 }
00473 
00474 bool MAX8U::saveSettings()
00475 {
00476     const size_t dataLen = 12;
00477     uint8_t data[dataLen];
00478 
00479     // don't clear any settings
00480     memset(data, 0, 4);
00481 
00482     // save all settings
00483     data[4] = 0b00011111;
00484     data[5] = 0b00011111;
00485     data[6] = 0;
00486     data[7] = 0;
00487 
00488     memset(data + 8, 0, 4);
00489 
00490     if(!sendCommand(UBX_CLASS_CFG, UBX_CFG_CFG, data, dataLen))
00491     {
00492         return false;
00493     }
00494 
00495     return waitForACK(UBX_CLASS_CFG, UBX_CFG_CFG);
00496 
00497 }
00498 
00499 bool MAX8U::checkVersion()
00500 {
00501     sendCommand(UBX_CLASS_MON, UBX_MON_VER, nullptr, 0);
00502 
00503     if(!waitForMessage(UBX_CLASS_MON, UBX_MON_VER))
00504     {
00505         return false;
00506     }
00507 
00508 #if MAX8U_DEBUG
00509     _debugPort->printf("MAXU8 Software Version: %s\r\n", buffer + UBX_DATA_OFFSET);
00510     _debugPort->printf("MAXU8 Software Version: %s\r\n", buffer + UBX_DATA_OFFSET + 30);
00511 
00512     // print additional data
00513     size_t numAdditionalLines = (currMessageLength - UBX_HEADER_FOOTER_LENGTH - 40) / 30;
00514 
00515     for(size_t line = 0; line < numAdditionalLines; line++)
00516     {
00517         _debugPort->printf("Extra Info: %s\r\n", buffer + UBX_DATA_OFFSET + 40 + 30*line);
00518     }
00519 
00520     #endif
00521 
00522     return true;
00523 }
00524 
00525 
00526 void MAX8U::readGNSSConfig() {
00527     sendCommand(UBX_CLASS_CFG, UBX_CFG_GNSS, nullptr, 0);
00528 
00529     if(!waitForMessage(UBX_CLASS_CFG, UBX_CFG_GNSS)){
00530         _debugPort->printf("NO MESSAGE RCVD for GNSS CFG message");
00531         return;
00532     }
00533 
00534     uint8_t numTrkChHw = static_cast<uint8_t >(buffer[UBX_DATA_OFFSET + 1]);
00535     uint8_t usedTracks = static_cast<uint8_t >(buffer[UBX_DATA_OFFSET + 2]);
00536     uint8_t blocks = static_cast<uint8_t >(buffer[UBX_DATA_OFFSET + 3]);
00537     _debugPort->printf("CHANNELS: %" PRIx8 " , USED: %" PRIx8 " , LEN: %" PRIx8 "\r\n", numTrkChHw, usedTracks, blocks);
00538     for(int i = 0; i < blocks; i++){
00539         uint8_t gnssID = static_cast<uint8_t >(buffer[UBX_DATA_OFFSET + 4 + 8*i]);
00540         uint32_t flag = static_cast<uint32_t >(buffer[UBX_DATA_OFFSET + 8 + 8*i]);
00541         bool enabled = flag&1;
00542         _debugPort->printf("GNSS ID: %" PRIx8 ", NAME: %s, ENABLED: %d \r\n", gnssID, GNSSNames[gnssID], enabled);
00543     }
00544 
00545 }
00546 
00547 bool MAX8U::readNextMessage(){
00548     //keep reading the 0xff register until the buffer is ove
00549 
00550     int msgLen = readLen();
00551     if(msgLen == -1){
00552         _debugPort->printf("Didn't rcv ack from MAX8 when reading length\r\n");
00553         return false;
00554     }
00555 
00556     if(msgLen == 0)
00557     {
00558         //nothing to do
00559         return false;
00560     }
00561 
00562     _i2cPort.start();
00563     int readResult = _i2cPort.write((_i2cAddress << 1) | 0x01);
00564 
00565     if(readResult != 1){
00566         _debugPort->printf("Didn't receive ack from MAX8\r\n");
00567         return false;
00568     }
00569 
00570     // whether this is an NMEA or UBX sentence
00571     bool isLastByte = false;
00572     currMessageLength = 0;
00573     int ubxMsgLen = 100000; // large value to stop loop exit condition, will read real value later
00574     for(int i = 0; i < msgLen; i++) // for loop in case there's a data error and we don't detect the last byte
00575     {
00576         uint8_t incoming = static_cast<uint8_t >(_i2cPort.read(!isLastByte));
00577         if(i == 5 && !isNMEASentence){
00578             //read length and change that to msg length
00579             ubxMsgLen = (static_cast<uint16_t>(buffer[i] << 8) | buffer[i-1]) + 8;
00580             //non-payload body of a ubx message is 8
00581 
00582         }
00583 
00584         if(i == 0)
00585         {
00586             if(incoming == NMEA_MESSAGE_START_CHAR)
00587             {
00588                 // NMEA sentences start with a dollars sign
00589                 isNMEASentence = true;
00590             }
00591             else if(incoming == UBX_MESSAGE_START_CHAR)
00592             {
00593                 // UBX sentences start with a 0xB5
00594                 isNMEASentence = false;
00595             }
00596             else if(incoming == 0xFF)
00597             {
00598                 _debugPort->printf("Message Length > 0, although buffer is empty\r\n");
00599             }else{
00600                 _debugPort->printf("Unknown first character \r\n");
00601             }
00602 
00603         }
00604 
00605         if(i < bufferMaxLen){
00606             buffer[i] = incoming;
00607             ++currMessageLength;
00608         }
00609 
00610         if(isLastByte)
00611         {
00612             break;
00613         }
00614 
00615         // post-receive actions
00616 
00617         // if it's an NMEA sentence, there is a CRLF at the end
00618         // if it's an UBX  sentence, there is a length passed before the payload
00619         if((isNMEASentence && incoming == '\r') || (!isNMEASentence && i == ubxMsgLen-2))
00620         {
00621             isLastByte = true;
00622         }
00623     }
00624 
00625     // add null terminator
00626     buffer[currMessageLength] = 0;
00627 
00628     _i2cPort.stop();
00629 
00630 
00631 #if  MAX8U_DEBUG
00632     _debugPort->printf("Read stream of MAX8U: ");
00633     for(uint16_t i = 0; i < currMessageLength; i++){
00634         _debugPort->printf("%02" PRIx8, buffer[i]);
00635     }
00636     _debugPort->printf(";\r\n");
00637 #endif
00638 
00639     if(!isNMEASentence)
00640     {
00641         uint8_t chka = 0, chkb = 0;
00642         for(uint16_t i = 2; i < currMessageLength-2; i++)
00643         {
00644             chka += buffer[i];
00645             chkb += chka;
00646         }
00647 
00648         if((chka != buffer[currMessageLength-2]) || (chkb != buffer[currMessageLength-1]))
00649         {
00650             _debugPort->printf("Checksums for UBX message don't match!\r\n");
00651             return false;
00652         }
00653     }
00654 
00655     return true;
00656 
00657 }
00658 
00659 
00660 int32_t MAX8U::readLen(){
00661 
00662     _i2cPort.start();
00663     int i2cStatus = _i2cPort.write((_i2cAddress << 1) | 0x00);
00664     if(i2cStatus != 1)
00665         return -1;
00666     _i2cPort.write(0xFD);
00667 
00668 
00669     _i2cPort.start();
00670     i2cStatus = _i2cPort.write((_i2cAddress << 1) | 0x01);
00671     if(i2cStatus != 1)
00672         return -1;
00673 
00674     uint8_t highByte = static_cast<uint8_t>(_i2cPort.read(true));
00675     uint8_t lowByte = static_cast<uint8_t>(_i2cPort.read(false));
00676 
00677     _i2cPort.stop();
00678 
00679     return (static_cast<uint16_t>(highByte << 8) | lowByte);
00680 }
00681 
00682