Diff: GPS/Ublox6.cpp

Revision:

36:3095e00eef37

Parent:

24:a7f92dfc5310

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/GPS/Ublox6.cpp	Tue Jan 01 17:44:27 2019 +0000
@@ -0,0 +1,241 @@
+#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
+
+const int READY_BITS=POSLLH_BIT|DOP_BIT|SOL_BIT|VELNED_BIT;
+
+#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])
+
+Ublox6::Ublox6()
+{
+    // clear flags
+    _ready = 0;
+    _available = false;
+    // clear out structs
+    tmp.lat = 0;
+    tmp.lon = 0;
+    tmp.course = 0;
+    tmp.speed = 0;
+    tmp.hdop = 0;
+    tmp.svcount = 0;
+    latest = tmp;
+    _callback = NULL;
+}
+
+int Ublox6::parse(int 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) {
+                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) {
+                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;
+                    }
+                    break;
+                case 0x05:      // ACK-
+                    switch (id) {
+                    case 0x00:  // ACK-NAK
+                        break;
+                    case 0x01:  // ACK-ACK
+                        break;
+                    }
+                    break;
+                }
+            }
+            state = 0;
+            break;
+        }
+        default:
+            break;
+    }
+
+    if ((_ready & READY_BITS) == READY_BITS) {
+        latest = tmp;
+        _ready = 0;
+        status = 1;
+
+        // clear tmp
+        tmp.lat = 0;
+        tmp.lon = 0;
+        tmp.speed = 0;
+        tmp.course = 0;
+        tmp.hdop = 0;
+        tmp.svcount = 0;
+        if (_callback)
+            _callback();
+    }
+
+    return status;
+}
+
+
+void Ublox6::read(double& lat, double& lon, float& course, float& speed, float& hdop, int& svcount) {
+    lat = latest.lat;
+    lon = latest.lon;
+    course = latest.course;
+    speed = latest.speed;
+    hdop = latest.hdop;
+    svcount = latest.svcount;
+
+    return;
+}
+
+
+void Ublox6::subscribe(Callback<void()> cb) {
+    _callback = cb;
+
+    return;
+}
+
+/*
+bool Ublox6::available(void)
+{
+    return _available;
+}
+
+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 _svcount;
+}
+*/