Diff: Ublox6.cpp

Revision:

19:0d1728091519

Parent:

18:3f8a8f6e3cc1

Child:

20:043987d06f8d

--- a/Ublox6.cpp	Fri Dec 14 00:36:06 2018 +0000
+++ b/Ublox6.cpp	Sat Dec 15 21:38:11 2018 +0000
@@ -8,6 +8,8 @@
 #define SOL_BIT     0x04
 #define VELNED_BIT  0x08
 
+const int READY_BITS=POSLLH_BIT|DOP_BIT|SOL_BIT|VELNED_BIT;
+
 #define SYNC1       0xB5
 #define SYNC2       0x62
 
@@ -44,26 +46,32 @@
     latest = tmp;
 }
 
-void Ublox6::parse(unsigned char cc)
+int Ublox6::parse(unsigned char cc)
 {
     //unsigned char cc = buf[out++];
     //out &= (MAX_LENGTH-1);
     static unsigned char ck1, ck2, state, code, id, idx, length, chk1, chk2;
     static bool checkOk;
     static unsigned char data[MAX_LENGTH];
+    int status = 0;
 
     switch (state) {
         case 0:    // wait for sync 1 (0xB5)
             ck1 = ck2 = 0;
             checkOk = false;
-            if (cc == SYNC1)
+            if (cc == SYNC1) {
                 state++;
+                puts("SYNC1");
+            }
             break;
         case 1:    // wait for sync 2 (0x62)
-            if (cc == SYNC2)
+            if (cc == SYNC2) {
                 state++;
-            else
+                puts("SYNC2");
+            } else {
                 state = 0;
+                puts("NO SYNC2");
+            }
             break;
         case 2:    // wait for class code
             code = cc;
@@ -85,6 +93,7 @@
             break;
         case 5:    // wait for length uint8_t 2
             length |= (unsigned int) cc << 8;
+            printf("len=%d\n", length);
             ck1 += cc;
             ck2 += ck1;
             idx = 0;
@@ -108,51 +117,58 @@
             chk2 = cc;
             checkOk = ck1 == chk1  &&  ck2 == chk2;
             if (!checkOk) {
-                // do something...?
+                puts("BAD CHKSUM\n");
             } else {
                 switch (code) {
-                    case 0x01:      // NAV-
-                        switch (id) {
-                            case POSLLH_MSG:  // NAV-POSLLH
-                                tmp.lon = ((float)LONG(4))/10000000.0;
-                                tmp.lat = ((float)LONG(8))/10000000.0;
-                                // vAcc = ULONG(24); // mm
-                                // hAcc = ULONG(20); // mm
-                                _ready |= POSLLH_BIT;
-                                break;
-                            case DOP_MSG:  // NAV-DOP
-                                //gDOP = ((float) UINT(4))/100.0;
-                                //tDOP = ((float) UINT(8))/100.0;
-                                //vDOP = ((float) UINT(10))/100.0;
-                                tmp.hdop = ((float) UINT(12))/100.0;
-                                _ready |= DOP_BIT;
-                                break;
-                            case SOL_MSG:  // NAV-SOL
-                                //week = UINT(8);
-                                //pDOP = ((float) UINT(44))/ 100.0;
-                                //pAcc = ULONG(24);
-                                tmp.svcount = data[47];
-                                _ready |= SOL_BIT;
-                                break;
-                            case VELNED_MSG:  // NAV-VELNED
-                                tmp.speed = ULONG(20)/100.0;
-                                //sAcc = ULONG(28)/100.0;
-                                tmp.course = ((float) LONG(24))/100000.0;
-                                //cAcc = ((float) LONG(32))/100000.0;
-                                _ready |= VELNED_BIT;                                
-                                break;
-                            default:
-                                break;
-                        }
+                case 0x01:      // NAV-
+                    switch (id) {
+                    case POSLLH_MSG:  // NAV-POSLLH
+                        tmp.lon = ((float)LONG(4))/10000000.0;
+                        tmp.lat = ((float)LONG(8))/10000000.0;
+                        // vAcc = ULONG(24); // mm
+                        // hAcc = ULONG(20); // mm
+                        _ready |= POSLLH_BIT;
+                        puts("LAT/LON");
+                        break;
+                    case DOP_MSG:  // NAV-DOP
+                        //gDOP = ((float) UINT(4))/100.0;
+                        //tDOP = ((float) UINT(8))/100.0;
+                        //vDOP = ((float) UINT(10))/100.0;
+                        tmp.hdop = ((float) UINT(12))/100.0;
+                        puts("HDOP");
+                        _ready |= DOP_BIT;
                         break;
-                    case 0x05:      // ACK-
-                        switch (id) {
-                            case 0x00:  // ACK-NAK
-                                break;
-                            case 0x01:  // ACK-ACK
-                                break;
-                        }
+                    case SOL_MSG:  // NAV-SOL
+                        //week = UINT(8);
+                        //pDOP = ((float) UINT(44))/ 100.0;
+                        //pAcc = ULONG(24);
+                        tmp.svcount = data[47];
+                        _ready |= SOL_BIT;
+                        puts("SVCOUNT");
+                        break;
+                    case VELNED_MSG:  // NAV-VELNED
+                        tmp.speed = ULONG(20)/100.0;
+                        //sAcc = ULONG(28)/100.0;
+                        tmp.course = ((float) LONG(24))/100000.0;
+                        //cAcc = ((float) LONG(32))/100000.0;
+                        _ready |= VELNED_BIT;                                
+                        puts("SPD/COURSE");
                         break;
+                    default:
+                        printf("OTHER=0x%04x\n", id);
+                        break;
+                    }
+                    if (_ready)
+                        printf("_ready = 0x%04x 0x%04x\n", _ready, READY_BITS);
+                    break;
+                case 0x05:      // ACK-
+                    switch (id) {
+                    case 0x00:  // ACK-NAK
+                        break;
+                    case 0x01:  // ACK-ACK
+                        break;
+                    }
+                    break;
                 }
             }
             state = 0;
@@ -161,14 +177,21 @@
         default:
             break;
     }
-    
-    if (_ready == POSLLH_BIT|DOP_BIT|SOL_BIT|VELNED_BIT) {
+
+    if ((_ready & READY_BITS) == READY_BITS) {
         latest = tmp;
+        tmp.lat = 0;
+        tmp.lon = 0;
+        tmp.speed = 0;
+        tmp.course = 0;
+        tmp.hdop = 0;
+        tmp.svcount = 0;
         _ready = 0;
+        status = 1;
         _available = true;
     }
 
-    return;
+    return status;
 }
 
 void Ublox6::read(double& lat, double& lon, float& course, float& speed, float& hdop, int& svcount) {
@@ -182,14 +205,12 @@
     return;
 }
 
-
-
+/*
 bool Ublox6::available(void)
 {
     return _available;
 }
 
-/*
 double Ublox6::latitude(void)
 {
     return _latitude;