Diff: GPS.cpp

Revision:

3:c7cd4887560d

Parent:

2:b6012cd91657

Child:

4:c959dd4c5fe8

diff -r b6012cd91657 -r c7cd4887560d GPS.cpp
--- a/GPS.cpp	Fri Oct 25 08:47:22 2013 +0000
+++ b/GPS.cpp	Fri Oct 25 09:45:55 2013 +0000
@@ -75,47 +75,45 @@
     return o;
 }
 
-int GPSParser::_putNmea(Stream* stream, const char* buf, int len)
+int GPSParser::sendNmea(const char* buf, int len)
 {
-    stream->putc('$');
-    int c = 0;
-    for (int i = 0; i < len; i ++)
-    {
-        int t = *buf++;
-        stream->putc(t);
-        c ^= t;
-    }
-    stream->putc('*');
-    stream->putc(toHex[(c >> 4) & 0xF]);
-    stream->putc(toHex[(c >> 0) & 0xF]);
-    stream->putc('\r');
-    stream->putc('\n');
-    return len + 6;
+    char head[1] = { '$' };
+    char tail[5] = { '*', 0x00/*crc_high*/, 0x00/*crc_low*/, '\r', '\n' };
+    int i;
+    int crc = 0;
+    for (i = 0; i < len; i ++)
+        crc ^= *buf++;
+    i  = send(head, sizeof(head));
+    i += send(buf, len);
+    tail[1] = toHex[(crc > 4) & 0xF0];
+    tail[2] = toHex[(crc > 0) & 0x0F];
+    i += send(tail, sizeof(tail));
+    return i;
 }
 
-int GPSParser::_putUbx(Stream* stream, const unsigned char cls, unsigned char id, unsigned char* buf, int len)
+int GPSParser::sendUbx(unsigned char cls, unsigned char id, const void* buf, int len)
 {
-    stream->putc('\xB5'); // 'µ'
-    stream->putc('\x62'); // 'b'
-    int ca = cls, cb = cls;
-    stream->putc(cls);
-    ca += id; cb += ca;
-    stream->putc(id);
-    int t = (len >> 0) & 0xFF;
-    ca += t; cb += ca;
-    stream->putc(t);
-    t = (len >> 8) & 0xFF;
-    ca += t; cb += ca;
-    stream->putc(t);
-    for (int i = 0; i < len; i ++)
+    char head[6] = { 0xB5, 0x62, cls, id, len >> 0, len >> 8 };
+    char crc[2];
+    int i;
+    int ca = 0;
+    int cb = 0;
+    for (i = 2; i < 6; i ++)
     {
-        t = *buf++;
-        ca += t; cb += ca;
-        stream->putc(t);
+        ca += head[i];
+        cb += ca;
     }
-    stream->putc(ca & 0xFF);
-    stream->putc(cb & 0xFF);
-    return len + 8;
+    for (i = 0; i < len; i ++)
+    {
+        ca += ((char*)buf)[i];
+        cb += ca;
+    }
+    i  = send(head, sizeof(head));
+    i += send(buf, len);
+    crc[0] = ca & 0xFF;
+    crc[1] = cb & 0xFF;
+    i += send(crc,  sizeof(crc));
+    return i;
 }
 
 const char* GPSParser::findNmeaItemPos(int ix, const char* start, const char* end)
@@ -206,6 +204,18 @@
     return _getMessage(&_pipe, buf, len);   
 }
 
+char GPSSerial::next(void)
+{ 
+    return _pipe.next(); 
+}
+
+int GPSSerial::send(const void* buf, int len)   
+{ 
+    for (int i = 0; i < len; i ++)
+        putc(((char*)buf)[i]); 
+    return len; 
+}
+
 // ----------------------------------------------------------------
 // I2C Implementation 
 // ----------------------------------------------------------------
@@ -236,8 +246,35 @@
     return _getMessage(&_pipe, buf, len);   
 }
 
+int GPSI2C::sendNmea(const char* buf, int len)
+{ 
+    int sent = 0;
+    if (len) 
+    {
+        if (!I2C::write(GPSADR,&REGSTREAM,sizeof(REGSTREAM),true))
+            sent = GPSParser::sendNmea(buf, len);
+        found = len == sent;
+        stop();
+    }
+    return sent;
+}
+
+int GPSI2C::sendUbx(unsigned char cls, unsigned char id, const void* buf, int len)
+{ 
+    int sent = 0;
+    if (len) 
+    {
+        if (!I2C::write(GPSADR,&REGSTREAM,sizeof(REGSTREAM),true))
+            sent = GPSParser::sendUbx(cls, id, buf, len);
+        found = (len == sent);
+        stop();
+    }
+    return sent;
+}
+
 int GPSI2C::_get(char* buf, int len)
 {
+    int read = 0;
     unsigned char sz[2];
     if (!I2C::write(GPSADR,&REGLEN,sizeof(REGLEN),true) && 
         !I2C::read(GPSADR,(char*)sz,sizeof(sz),true))
@@ -246,43 +283,24 @@
         if (size > len)
             size = len;
         if (size > 0)
-        {
-            if (!I2C::read(GPSADR,buf,size))
-            {
-                found = true;
-                return size;
-            }
-            // error reading data
-            found = false;
-            return 0;
-        }
-        else
-        {
-            found = true;
-            // no data -> ok 
-        }
+            read = !I2C::read(GPSADR,buf,size, true) ? size : 0;
+        stop();
+        found = read = size;
     }
     else 
-    {
-        // error setting address and reading length
         found = false;
-    }
     I2C::stop();
-    return 0;
+    return read;
 }
 
-int GPSI2C::_put(const char* buf, int len)
-{
-    if (len == 0)
-        return 0;
-    if (!I2C::write(GPSADR,&REGSTREAM,sizeof(REGSTREAM),true) && 
-        !I2C::write(GPSADR,buf,len,false))
-    {
-        found = true;
-        return len;
-    }
-    found = false;
-    return 0;
+char GPSI2C::next(void)                       
+{ 
+    return _pipe.next(); 
+}
+
+int GPSI2C::send(const void* buf, int len)
+{ 
+    return !I2C::write(GPSADR,(const char*)buf,len,true) ? len : 0; 
 }
 
 const char GPSI2C::REGLEN    = 0xFD;