Revision 3:c7cd4887560d, committed 2013-10-25

Comitter:

mazgch

Date:

Fri Oct 25 09:45:55 2013 +0000

Parent:

2:b6012cd91657

Child:

4:c959dd4c5fe8

Commit message:

added sending of msgs

Changed in this revision

--- 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;

--- a/GPS.h	Fri Oct 25 08:47:22 2013 +0000
+++ b/GPS.h	Fri Oct 25 09:45:55 2013 +0000
@@ -17,8 +17,8 @@
     #define PROTOCOL(x) (x & 0xFF0000)
 
     virtual int getMessage(char* buf, int len) = 0; 
-    virtual int putNmea(const char* buf, int len) = 0;
-    virtual int putUbx(const unsigned char cls, unsigned char id, unsigned char* buf, int len) = 0;
+    virtual int sendNmea(const char* buf, int len) = 0;
+    virtual int sendUbx(unsigned char cls, unsigned char id, const void* buf, int len) = 0;
     
     static const char* findNmeaItemPos(int ix, const char* start, const char* end);
     static bool getNmeaItem(int ix, char* buf, int len, double& val);
@@ -28,8 +28,7 @@
     static int _getMessage(Pipe<char>* pipe, char* buf, int len);
     static int _parseNmea(Pipe<char>* pipe, int len);
     static int _parseUbx(Pipe<char>* pipe, int len);
-    static int _putNmea(Stream* stream, const char* buf, int len);
-    static int _putUbx(Stream* stream, const unsigned char cls, unsigned char id, unsigned char* buf, int len);
+    virtual int send(const void* buf, int len) = 0;
     static const char toHex[16];
 };
 
@@ -40,13 +39,10 @@
     virtual ~GPSSerial(void);
     
     virtual int getMessage(char* buf, int len);
-    virtual int putNmea(const char* buf, int len)
-        { return _putNmea(this, buf, len); }
-    virtual int putUbx(const unsigned char cls, unsigned char id, unsigned char* buf, int len)
-        { return _putUbx(this, cls, id, buf, len); }
 protected:
     void serialRxIrq(void);
-    virtual char next(void) { return _pipe.next(); }
+    virtual char next(void);
+    virtual int send(const void* buf, int len);
     Pipe<char> _pipe;
 };
 
@@ -57,16 +53,13 @@
     bool detect(void);
     
     virtual int getMessage(char* buf, int len);
-    // 
-    virtual int putNmea(const char* buf, int len)
-        { return 0/*_putNmea(NULL, buf, len)*/; }
-    virtual int putUbx(const unsigned char cls, unsigned char id, unsigned char* buf, int len)
-        { return 0/*_putUbx(NULL, cls, id, buf, len)*/; }
+    virtual int sendNmea(const char* buf, int len);
+    virtual int sendUbx(unsigned char cls, unsigned char id, const void* buf, int len);
 protected:
-    virtual char next(void) { return _pipe.next(); }
-    int _get(char* buf, int len);          // read the NMEA or UBX stream
-    int _put(const char* buf, int len);    // Write the NMEA or UBX stream
-
+    virtual char next(void);
+    virtual int send(const void* buf, int len);
+    int _get(char* buf, int len);                 // read the NMEA or UBX stream
+    
     Pipe<char> _pipe; 
     bool found;
     static const char REGLEN;