Diff: Ublox6.cpp

Revision:

16:eb28d0f64a9b

Child:

18:3f8a8f6e3cc1

diff -r 35c40765f7c3 -r eb28d0f64a9b Ublox6.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Ublox6.cpp	Thu Dec 13 17:35:29 2018 +0000
@@ -0,0 +1,188 @@
+#include "mbed.h"
+#include "Ublox6.h"
+
+#define MAX_LENGTH 512
+
+#define DOP_BIT     0x01
+#define POSLLH_BIT  0x02
+#define SOL_BIT     0x04
+#define VELNED_BIT  0x08
+#define SYNC1       0xB5
+#define SYNC2       0x62
+#define POSLLH_MSG  0x02
+#define SBAS_MSG    0x32
+#define VELNED_MSG  0x12
+#define STATUS_MSG  0x03
+#define SOL_MSG     0x06
+#define DOP_MSG     0x04
+#define DGPS_MSG    0x31
+#define SVINFO_MSG  0x30
+#define CFG         0x06
+#define NAV         0x01
+#define MSG         0x01
+
+#define LONG(X)    *(int32_t *)(&data[X])
+#define ULONG(X)   *(uint32_t *)(&data[X])
+#define INT(X)     *(int16_t *)(&data[X])
+#define UINT(X)    *(uint16_t *)(&data[X])
+
+// TODO 2: abstract LED/status to some kind of pub/sub thing
+
+void 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];
+
+    switch (state) {
+        case 0:    // wait for sync 1 (0xB5)
+            ck1 = ck2 = 0;
+            checkOk = false;
+            if (cc == SYNC1)
+                state++;
+            break;
+        case 1:    // wait for sync 2 (0x62)
+            if (cc == SYNC2)
+                state++;
+            else
+                state = 0;
+            break;
+        case 2:    // wait for class code
+            code = cc;
+            ck1 += cc;
+            ck2 += ck1;
+            state++;
+            break;
+        case 3:    // wait for Id
+            id = cc;
+            ck1 += cc;
+            ck2 += ck1;
+            state++;
+            break;
+        case 4:    // wait for length uint8_t 1
+            length = cc;
+            ck1 += cc;
+            ck2 += ck1;
+            state++;
+            break;
+        case 5:    // wait for length uint8_t 2
+            length |= (unsigned int) cc << 8;
+            ck1 += cc;
+            ck2 += ck1;
+            idx = 0;
+            state++;
+            if (length > MAX_LENGTH)
+                state= 0;
+            break;
+        case 6:    // wait for <length> payload uint8_ts
+            data[idx++] = cc;
+            ck1 += cc;
+            ck2 += ck1;
+            if (idx >= length) {
+                state++;
+            }
+            break;
+        case 7:    // wait for checksum 1
+            chk1 = cc;
+            state++;
+            break;
+        case 8: {  // wait for checksum 2
+            chk2 = cc;
+            checkOk = ck1 == chk1  &&  ck2 == chk2;
+            if (!checkOk) {
+                // do something...?
+            } else {
+                switch (code) {
+                    case 0x01:      // NAV-
+                        switch (id) {
+                            case POSLLH_MSG:  // NAV-POSLLH
+                                _longitude = ((float)LONG(4))/10000000.0;
+                                _latitude = ((float)LONG(8))/10000000.0;
+                                // vAcc = ULONG(24); // mm
+                                // hAcc = ULONG(20); // mm
+                                _available |= 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;
+                                _hdop = ((float) UINT(12))/100.0;
+                                _available |= DOP_BIT;
+                                break;
+                            case SOL_MSG:  // NAV-SOL
+                                //week = UINT(8);
+                                //pDOP = ((float) UINT(44))/ 100.0;
+                                //pAcc = ULONG(24);
+                                _sat_count = data[47];
+                                _available |= SOL_BIT;
+                                break;
+                            case VELNED_MSG:  // NAV-VELNED
+                                _speed_mps = ULONG(20)/100.0;
+                                //sAcc = ULONG(28)/100.0;
+                                _course_deg = ((float) LONG(24))/100000.0;
+                                //cAcc = ((float) LONG(32))/100000.0;
+                                _available |= VELNED_BIT;                                
+                                break;
+                            default:
+                                break;
+                        }
+                        break;
+                    case 0x05:      // ACK-
+                        switch (id) {
+                            case 0x00:  // ACK-NAK
+                                break;
+                            case 0x01:  // ACK-ACK
+                                break;
+                        }
+                        break;
+                }
+            }
+            state = 0;
+            break;
+        }
+        default:
+            break;
+    }
+}
+
+bool Ublox6::available(void)
+{
+    return (_available & (DOP_BIT|POSLLH_BIT|SOL_BIT|VELNED_BIT));
+}
+
+void Ublox6::reset_available(void)
+{
+    _available = 0;
+}
+
+double Ublox6::latitude(void)
+{
+    return _latitude;
+}
+
+double Ublox6::longitude(void)
+{
+    return _longitude;
+}
+
+float Ublox6::speed_mps(void)
+{
+    return _speed_mps;
+}
+
+float Ublox6::heading_deg(void)
+{
+    return _course_deg;
+}
+
+float Ublox6::hdop(void)
+{
+    return _hdop;
+}
+
+int Ublox6::sat_count(void)
+{
+    return _sat_count;
+}