allsensors
Fork of C027_Support by
Embed:
(wiki syntax)
Show/hide line numbers
GPS.cpp
00001 #include "mbed.h" 00002 #include <ctype.h> 00003 #include "GPS.h" 00004 #ifdef TARGET_UBLOX_C027 00005 #include "C027_api.h" 00006 #endif 00007 00008 void GPSParser::powerOff(void) 00009 { 00010 // set the gps into backup mode using the command RMX-LPREQ 00011 struct { unsigned long dur; unsigned long flags; } msg = {0/*endless*/,0/*backup*/}; 00012 sendUbx(0x02, 0x41, &msg, sizeof(msg)); 00013 } 00014 00015 int GPSParser::_getMessage(Pipe<char> * pipe, char* buf, int len) 00016 { 00017 int unkn = 0; 00018 int sz = pipe->size(); 00019 int fr = pipe->free(); 00020 if (len > sz) 00021 len = sz; 00022 while (len > 0) 00023 { 00024 // NMEA protocol 00025 pipe->set(unkn); 00026 int nmea = _parseNmea(pipe,len); 00027 if ((nmea != NOT_FOUND) && (unkn > 0)) 00028 return UNKNOWN | pipe->get (buf,unkn); 00029 if (nmea == WAIT && fr) 00030 return WAIT; 00031 if (nmea > 0) 00032 return NMEA | pipe->get (buf,nmea); 00033 // UBX protocol 00034 00035 pipe->set(unkn); 00036 int ubx = _parseUbx(pipe,len); 00037 if ((ubx != NOT_FOUND) && (unkn > 0)) 00038 return UNKNOWN | pipe->get (buf,unkn); 00039 if (ubx == WAIT && fr) 00040 return WAIT; 00041 if (ubx > 0) 00042 return UBX | pipe->get (buf,ubx); 00043 00044 // UNKNOWN 00045 unkn ++; 00046 len--; 00047 } 00048 if (unkn > 0) 00049 return UNKNOWN | pipe->get (buf,unkn); 00050 return WAIT; 00051 } 00052 00053 int GPSParser::_parseNmea(Pipe<char> * pipe, int len) 00054 { 00055 int o = 0; 00056 int c = 0; 00057 char ch; 00058 if (++o > len) return WAIT; 00059 if ('$' != pipe->next()) return NOT_FOUND; 00060 // this needs to be extended by crc checking 00061 for (;;) 00062 { 00063 if (++o > len) return WAIT; 00064 ch = pipe->next(); 00065 if ('*' == ch) break; // crc delimiter 00066 if (!isprint(ch)) return NOT_FOUND; 00067 c ^= ch; 00068 } 00069 if (++o > len) return WAIT; 00070 ch = toHex[(c >> 4) & 0xF]; // high nibble 00071 if (ch != pipe->next()) return NOT_FOUND; 00072 if (++o > len) return WAIT; 00073 ch = toHex[(c >> 0) & 0xF]; // low nibble 00074 if (ch != pipe->next()) return NOT_FOUND; 00075 if (++o > len) return WAIT; 00076 if ('\r' != pipe->next()) return NOT_FOUND; 00077 if (++o > len) return WAIT; 00078 if ('\n' != pipe->next()) return NOT_FOUND; 00079 return o; 00080 } 00081 00082 int GPSParser::_parseUbx(Pipe<char> * pipe, int l) 00083 { 00084 int o = 0; 00085 if (++o > l) return WAIT; 00086 if ('\xB5' != pipe->next()) return NOT_FOUND; 00087 if (++o > l) return WAIT; 00088 if ('\x62' != pipe->next()) return NOT_FOUND; 00089 o += 4; 00090 if (o > l) return WAIT; 00091 int i,j,ca,cb; 00092 i = pipe->next(); ca = i; cb = ca; // cls 00093 i = pipe->next(); ca += i; cb += ca; // id 00094 i = pipe->next(); ca += i; cb += ca; // len_lsb 00095 j = pipe->next(); ca += j; cb += ca; // len_msb 00096 j = i + (j << 8); 00097 while (j--) 00098 { 00099 if (++o > l) return WAIT; 00100 i = pipe->next(); 00101 ca += i; 00102 cb += ca; 00103 } 00104 ca &= 0xFF; cb &= 0xFF; 00105 if (++o > l) return WAIT; 00106 if (ca != pipe->next()) return NOT_FOUND; 00107 if (++o > l) return WAIT; 00108 if (cb != pipe->next()) return NOT_FOUND; 00109 return o; 00110 } 00111 00112 int GPSParser::send(const char* buf, int len) 00113 { 00114 return _send(buf, len); 00115 } 00116 00117 int GPSParser::sendNmea(const char* buf, int len) 00118 { 00119 char head[1] = { '$' }; 00120 char tail[5] = { '*', 0x00/*crc_high*/, 0x00/*crc_low*/, '\r', '\n' }; 00121 int i; 00122 int crc = 0; 00123 for (i = 0; i < len; i ++) 00124 crc ^= *buf++; 00125 i = _send(head, sizeof(head)); 00126 i += _send(buf, len); 00127 tail[1] = toHex[(crc > 4) & 0xF0]; 00128 tail[2] = toHex[(crc > 0) & 0x0F]; 00129 i += _send(tail, sizeof(tail)); 00130 return i; 00131 } 00132 00133 int GPSParser::sendUbx(unsigned char cls, unsigned char id, const void* buf /*= NULL*/, int len /*= 0*/) 00134 { 00135 char head[6] = { 0xB5, 0x62, cls, id, len >> 0, len >> 8 }; 00136 char crc[2]; 00137 int i; 00138 int ca = 0; 00139 int cb = 0; 00140 for (i = 2; i < 6; i ++) 00141 { 00142 ca += head[i]; 00143 cb += ca; 00144 } 00145 for (i = 0; i < len; i ++) 00146 { 00147 ca += ((char*)buf)[i]; 00148 cb += ca; 00149 } 00150 i = _send(head, sizeof(head)); 00151 i += _send(buf, len); 00152 crc[0] = ca & 0xFF; 00153 crc[1] = cb & 0xFF; 00154 i += _send(crc, sizeof(crc)); 00155 return i; 00156 } 00157 00158 const char* GPSParser::findNmeaItemPos(int ix, const char* start, const char* end) 00159 { 00160 // find the start 00161 for (; (start < end) && (ix > 0); start ++) 00162 { 00163 if (*start == ',') 00164 ix --; 00165 } 00166 // found and check bounds 00167 if ((ix == 0) && (start < end) && 00168 (*start != ',') && (*start != '*') && (*start != '\r') && (*start != '\n')) 00169 return start; 00170 else 00171 return NULL; 00172 } 00173 00174 bool GPSParser::getNmeaItem(int ix, char* buf, int len, double& val) 00175 { 00176 char* end = &buf[len]; 00177 const char* pos = findNmeaItemPos(ix, buf, end); 00178 // find the start 00179 if (!pos) 00180 return false; 00181 val = strtod(pos, &end); 00182 // restore the last character 00183 return (end > pos); 00184 } 00185 00186 bool GPSParser::getNmeaItem(int ix, char* buf, int len, int& val, int base /*=10*/) 00187 { 00188 char* end = &buf[len]; 00189 const char* pos = findNmeaItemPos(ix, buf, end); 00190 // find the start 00191 if (!pos) 00192 return false; 00193 val = (int)strtol(pos, &end, base); 00194 return (end > pos); 00195 } 00196 00197 bool GPSParser::getNmeaItem(int ix, char* buf, int len, char& val) 00198 { 00199 const char* end = &buf[len]; 00200 const char* pos = findNmeaItemPos(ix, buf, end); 00201 // find the start 00202 if (!pos) 00203 return false; 00204 // skip leading spaces 00205 while ((pos < end) && isspace(*pos)) 00206 pos++; 00207 // check bound 00208 if ((pos < end) && 00209 (*pos != ',') && (*pos != '*') && (*pos != '\r') && (*pos != '\n')) 00210 { 00211 val = *pos; 00212 return true; 00213 } 00214 return false; 00215 } 00216 00217 bool GPSParser::getNmeaAngle(int ix, char* buf, int len, double& val) 00218 { 00219 char ch; 00220 if (getNmeaItem(ix,buf,len,val) && getNmeaItem(ix+1,buf,len,ch) && 00221 ((ch == 'S') || (ch == 'N') || (ch == 'E') || (ch == 'W'))) 00222 { 00223 val *= 0.01; 00224 int i = (int)val; 00225 val = (val - i) / 0.6 + i; 00226 if (ch == 'S' || ch == 'W') 00227 val = -val; 00228 return true; 00229 } 00230 return false; 00231 } 00232 00233 const char GPSParser::toHex[] = { '0','1','2','3','4','5','6','7','8','9','A','B','C','D','E','F' }; 00234 00235 // ---------------------------------------------------------------- 00236 // Serial Implementation 00237 // ---------------------------------------------------------------- 00238 00239 GPSSerial::GPSSerial(PinName tx /*= GPSTXD*/, PinName rx /*= GPSRXD*/, int baudrate /*= GPSBAUD*/, 00240 int rxSize /*= 256*/, int txSize /*= 128*/) : 00241 SerialPipe(tx, rx, rxSize, txSize) 00242 { 00243 baud(baudrate); 00244 #ifdef TARGET_UBLOX_C027 00245 _onboard = (tx == GPSTXD) || (rx == GPSRXD); 00246 if (_onboard) 00247 c027_gps_powerOn(); 00248 #endif 00249 } 00250 00251 GPSSerial::~GPSSerial(void) 00252 { 00253 powerOff(); 00254 #ifdef TARGET_UBLOX_C027 00255 if (_onboard) 00256 c027_gps_powerOff(); 00257 #endif 00258 } 00259 00260 bool GPSSerial::init(PinName pn) 00261 { 00262 // send a byte to wakup the device again 00263 putc(0xFF); 00264 // wait until we get some bytes 00265 int size = _pipeRx.size(); 00266 Timer timer; 00267 timer.start(); 00268 while ((100 > timer.read_ms()) && (size == _pipeRx.size())) 00269 /* nothing / just wait */; 00270 return (size != _pipeRx.size()); 00271 } 00272 00273 int GPSSerial::getMessage(char* buf, int len) 00274 { 00275 return _getMessage(&_pipeRx, buf, len); 00276 } 00277 00278 int GPSSerial::_send(const void* buf, int len) 00279 { 00280 return put((const char*)buf, len, true/*=blocking*/); 00281 } 00282 00283 // ---------------------------------------------------------------- 00284 // I2C Implementation 00285 // ---------------------------------------------------------------- 00286 00287 GPSI2C::GPSI2C(PinName sda /*= GPSSDA*/, PinName scl /*= GPSSCL*/, 00288 unsigned char i2cAdr /*=GPSADR*/, int rxSize /*= 256*/) : 00289 I2C(sda,scl), 00290 _pipe(rxSize), 00291 _i2cAdr(i2cAdr) 00292 { 00293 frequency(100000); 00294 #ifdef TARGET_UBLOX_C027 00295 _onboard = (sda == GPSSDA) && (scl == GPSSCL); 00296 if (_onboard) 00297 c027_gps_powerOn(); 00298 #endif 00299 } 00300 00301 GPSI2C::~GPSI2C(void) 00302 { 00303 powerOff(); 00304 #ifdef TARGET_UBLOX_C027 00305 if (_onboard) 00306 c027_gps_powerOff(); 00307 #endif 00308 } 00309 00310 bool GPSI2C::init(PinName pn) 00311 { 00312 if (pn != NC) { 00313 DigitalOut pin(pn, 0); 00314 ::wait_us(1); 00315 pin = 1; 00316 ::wait_ms(100); 00317 } 00318 return !I2C::write(_i2cAdr,®STREAM,sizeof(REGSTREAM)); 00319 } 00320 00321 int GPSI2C::getMessage(char* buf, int len) 00322 { 00323 // fill the pipe 00324 int sz = _pipe.free(); 00325 if (sz) 00326 sz = _get(buf, sz); 00327 if (sz) 00328 _pipe.put(buf, sz); 00329 // now parse it 00330 return _getMessage(&_pipe, buf, len); 00331 } 00332 00333 int GPSI2C::send(const char* buf, int len) 00334 { 00335 int sent = 0; 00336 if (len) 00337 { 00338 if (!I2C::write(_i2cAdr,®STREAM,sizeof(REGSTREAM),true)) 00339 sent = send(buf, len); 00340 stop(); 00341 } 00342 return sent; 00343 } 00344 00345 int GPSI2C::sendNmea(const char* buf, int len) 00346 { 00347 int sent = 0; 00348 if (!I2C::write(_i2cAdr,®STREAM,sizeof(REGSTREAM),true)) 00349 sent = GPSParser::sendNmea(buf, len); 00350 stop(); 00351 return sent; 00352 } 00353 00354 int GPSI2C::sendUbx(unsigned char cls, unsigned char id, const void* buf, int len) 00355 { 00356 int sent = 0; 00357 if (!I2C::write(_i2cAdr,®STREAM,sizeof(REGSTREAM),true)) 00358 sent = GPSParser::sendUbx(cls, id, buf, len); 00359 I2C::stop(); 00360 return sent; 00361 } 00362 00363 int GPSI2C::_get(char* buf, int len) 00364 { 00365 int read = 0; 00366 unsigned char sz[2] = {0,0}; 00367 if (!I2C::write(_i2cAdr,®LEN,sizeof(REGLEN),true) && 00368 !I2C::read(_i2cAdr,(char*)sz,sizeof(sz))) 00369 { 00370 int size = 256 * (int)sz[0] + sz[1]; 00371 if (size > len) 00372 size = len; 00373 if (size > 0) 00374 { 00375 if (!I2C::write(_i2cAdr,®STREAM,sizeof(REGSTREAM),true) && 00376 !I2C::read(_i2cAdr,buf,size)) { 00377 read = size; 00378 } 00379 } 00380 } 00381 return read; 00382 } 00383 00384 int GPSI2C::_send(const void* buf, int len) 00385 { 00386 return !I2C::write(_i2cAdr,(const char*)buf,len,true) ? len : 0; 00387 } 00388 00389 const char GPSI2C::REGLEN = 0xFD; 00390 const char GPSI2C::REGSTREAM = 0xFF;
Generated on Wed Jul 13 2022 00:19:01 by 1.7.2