Revision 1:2d8275968aaf, committed 2014-02-05

Comitter:

va009039

Date:

Wed Feb 05 13:40:35 2014 +0000

Parent:

0:4231f156a567

Commit message:

add NMEA decode. update KL46Z-USBHost library.

Changed in this revision

--- a/KL46Z-USBHost.lib	Sat Jan 18 13:43:06 2014 +0000
+++ b/KL46Z-USBHost.lib	Wed Feb 05 13:40:35 2014 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/va009039/code/KL46Z-USBHost/#5160ee0c522d
+https://mbed.org/users/va009039/code/KL46Z-USBHost/#40c7f6788902

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SLCD.lib	Wed Feb 05 13:40:35 2014 +0000
@@ -0,0 +1,1 @@
+https://mbed.org/users/Sissors/code/SLCD/#f70873bc6121

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/USBHostGPS/USBHostGPS.cpp	Wed Feb 05 13:40:35 2014 +0000
@@ -0,0 +1,98 @@
+#include "USBHostGPS.h"
+
+USBHostGPS::USBHostGPS(int baud_)
+{
+    host = USBHost::getHostInst();
+    init();
+    baud = baud_;
+}
+
+void USBHostGPS::init() {
+    dev = NULL;
+    bulk_in = NULL;
+    onUpdateRaw = NULL;
+    dev_connected = false;
+    gps_device_found = false;
+    gps_intf = -1;
+}
+
+bool USBHostGPS::connected() {
+    return dev_connected;
+}
+
+bool USBHostGPS::connect() {
+
+    if (dev_connected) {
+        return true;
+    }
+    
+    for (uint8_t i = 0; i < MAX_DEVICE_CONNECTED; i++) {
+        if ((dev = host->getDevice(i)) != NULL) {
+            if(host->enumerate(dev, this)) {
+                break;
+            }
+            if (gps_device_found) {
+                bulk_in = dev->getEndpoint(gps_intf, BULK_ENDPOINT, IN);
+                USB_TEST_ASSERT(bulk_in);
+                // stop bit = 1, parity = none, 8bit
+                uint8_t data[] = {baud&0xff, baud>>8, baud>>16, baud>>24, 0x00, 0x00, 0x08};
+                USB_TYPE rc = host->controlWrite(dev, 0x21, PL2303_SET_LINE_CODING, 0, 0, data, sizeof(data));
+                USB_TEST_ASSERT(rc == USB_TYPE_OK);
+                USB_INFO("New GPS device: VID:%04x PID:%04x [dev: %p - intf: %d]", dev->getVid(), dev->getPid(), dev, gps_intf);
+                bulk_in->attach(this, &USBHostGPS::rxHandler);
+                host->bulkRead(dev, bulk_in, bulk_buf, bulk_in->getSize(), false);
+                
+                dev_connected = true;
+                return true;
+            }
+        }
+    }
+    init();
+    return false;
+}
+
+void USBHostGPS::rxHandler() {
+    int len = bulk_in->getLengthTransferred();
+    if (onUpdateRaw) {
+        (*onUpdateRaw)((char*)bulk_buf, len);
+    }
+    nmea.inputNMEA((char*)bulk_buf, len);
+
+    if (dev) {
+        host->bulkRead(dev, bulk_in, bulk_buf, bulk_in->getSize(), false);
+    }
+}
+
+/*virtual*/ void USBHostGPS::setVidPid(uint16_t vid, uint16_t pid)
+{
+    USB_DBG("vid:%04x pid:%04x", vid, pid);
+    if (pid == 0x2303) {
+        gps_device_found = true;
+    }
+}
+
+/*virtual*/ bool USBHostGPS::parseInterface(uint8_t intf_nb, uint8_t intf_class, uint8_t intf_subclass, uint8_t intf_protocol) //Must return true if the interface should be parsed
+{
+    USB_DBG("intf: %d class: %02x %02x %02x", intf_nb, intf_class, intf_subclass, intf_protocol);
+    if (gps_device_found) {
+        if (gps_intf == -1) {
+            gps_intf = intf_nb;
+            return true;
+        }
+    }    
+    return false;
+}
+
+/*virtual*/ bool USBHostGPS::useEndpoint(uint8_t intf_nb, ENDPOINT_TYPE type, ENDPOINT_DIRECTION dir) //Must return true if the endpoint will be used
+{
+    USB_DBG("intf_nb=%d type=%d dir=%d", intf_nb, type, dir);
+    if (gps_device_found) {
+        if (intf_nb == gps_intf) {
+            if (type == BULK_ENDPOINT && dir == IN) {
+                return true;
+            }
+        }
+    }
+    return false;
+}
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/USBHostGPS/USBHostGPS.h	Wed Feb 05 13:40:35 2014 +0000
@@ -0,0 +1,60 @@
+// Simple USBHost GPS Dongle for FRDM-KL46Z
+#include "USBHost.h"
+#include "decodeNMEA.h"
+
+#define PL2303_SET_LINE_CODING 0x20
+
+class USBHostGPS : public IUSBEnumerator {
+public:
+
+    /**
+    * Constructor
+    */
+    USBHostGPS(int baud = 38400);
+
+    /**
+     * Try to connect a USB GPS device
+     *
+     * @return true if connection was successful
+     */
+    bool connect();
+
+    /**
+    * Check if a USB GPS is connected
+    *
+    * @returns true if a mouse is connected
+    */
+    bool connected();
+
+    int readNMEA(char* data, int size, int timeout_ms) {
+        host->bulkRead(dev, bulk_in, (uint8_t*)data, size);
+        return bulk_in->getLengthTransferred();
+    }
+    void attachEventRaw(void (*ptr)(char* data, int size)) {
+        if (ptr != NULL) {
+            onUpdateRaw = ptr;
+        }
+    }
+
+    decodeNMEA nmea;
+
+protected:
+    //From IUSBEnumerator
+    virtual void setVidPid(uint16_t vid, uint16_t pid);
+    virtual bool parseInterface(uint8_t intf_nb, uint8_t intf_class, uint8_t intf_subclass, uint8_t intf_protocol); //Must return true if the interface should be parsed
+    virtual bool useEndpoint(uint8_t intf_nb, ENDPOINT_TYPE type, ENDPOINT_DIRECTION dir); //Must return true if the endpoint will be used
+
+private:
+    USBHost * host;
+    USBDeviceConnected* dev;
+    USBEndpoint* bulk_in;
+    bool dev_connected;
+    bool gps_device_found;
+    int gps_intf;
+
+    void rxHandler();
+    void (*onUpdateRaw)(char* data, int size);
+    uint8_t bulk_buf[64];
+    int baud;
+    void init();
+};

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/USBHostGPS/decodeNMEA.cpp	Wed Feb 05 13:40:35 2014 +0000
@@ -0,0 +1,159 @@
+// decodeNMEA.cpp 2012/12/13
+#include "mbed.h"
+#include "decodeNMEA.h"
+
+#define SEQ_INIT 0
+#define SEQ_DATA 1
+#define SEQ_SUM0 2
+#define SEQ_SUM1 3
+
+#define GPGGA 1
+#define GPGLL 2
+#define GPGSV 3
+#define GPRMC 4
+#define GPVTG 5
+#define GPZDA 6
+
+decodeNMEA::decodeNMEA():m_seq(0) {
+    gprmc_t = 0;
+    update_t = 0;
+    m_status = false;
+}
+
+void decodeNMEA::inputNMEA(char* buf, int len) {
+    for(int i = 0; i < len; i++) {
+        inputNMEA(buf[i]);
+    }
+}
+
+static int ctoh(char c) {
+    if (c >= '0' && c <= '9') {
+        return c-'0';
+    }
+    return c-'A'+10;
+}
+     
+void decodeNMEA::inputNMEA(char c) {
+    switch(m_seq) {
+        case SEQ_INIT:
+            if (c == '$') {
+                m_type = 0;
+                m_row = 0;
+                m_buf_pos = 0;
+                m_sum = 0x00;
+                m_seq = SEQ_DATA;
+            }
+            break;
+        case SEQ_DATA:
+            m_sum ^= c;
+            if (c == ',' || c == '*') {
+                m_buf[m_buf_pos] = '\0';
+                parse(m_type, m_row, m_buf);
+                m_row++;
+                m_buf_pos =0;
+                if (c == '*') { // check sum ?
+                    m_sum ^= c;
+                    m_seq = SEQ_SUM0;
+                }
+            } else {
+                if (m_buf_pos < sizeof(m_buf)-1) {
+                    m_buf[m_buf_pos++] = c;
+                }
+            }
+            break;
+        case SEQ_SUM0:
+            if (ctoh(c) == (m_sum>>4)) {
+                m_seq = SEQ_SUM1;
+            } else {
+                m_seq = SEQ_INIT;
+            }
+            break;
+        case SEQ_SUM1:
+            if (ctoh(c) == (m_sum&0x0f)) {
+                update(m_type, m_row);
+            } else {
+                
+                m_seq = SEQ_INIT;
+            }
+            break;
+        default:
+            m_seq = SEQ_INIT;
+            break;
+    }
+}
+
+float DMMtoDegree(const char *s)
+{
+    char *p = strchr(const_cast<char*>(s), '.');
+    if (p == NULL) {
+        return 0.0;
+    }
+    const uint32_t k[] = {10000,1000,100,10,1};
+    uint32_t i3 = atoi(p+1) * k[strlen(p+1)];
+    uint32_t i2 = atoi(p-2);
+    uint32_t i1 = atoi(s) / 100;
+
+    uint32_t i = i1*10000*60 + (i2*10000 + i3);
+    return i / 10000.0 / 60.0;
+}
+
+void decodeNMEA::parse(int type, int row, char* buf) {
+    if (row == 0) {
+        if (strcmp(buf, "GPRMC") == 0) {
+            m_type = GPRMC;
+            m_status = false;
+        } else {
+            m_type = 0;
+        }
+        return;
+    }
+    if (type == GPRMC) {
+        switch(row) {
+            case 1:
+                tmp_timeinfo.tm_sec = atoi(buf+4);
+                buf[4] = '\0';
+                tmp_timeinfo.tm_min = atoi(buf+2);
+                buf[2] = '\0';
+                tmp_timeinfo.tm_hour = atoi(buf);
+                break;
+            case 2:
+                if (buf[0] == 'A') {
+                    m_status = true;
+                }
+                break;
+            case 3:
+                tmp_lat = DMMtoDegree(buf);
+                break;
+            case 4:
+                if (buf[0] == 'S') {
+                    tmp_lat *= -1;
+                }
+                break;
+            case 5:
+                tmp_lon = DMMtoDegree(buf);
+                break;
+            case 6:
+                if (buf[0] == 'W') {
+                    tmp_lon *= -1;
+                }
+                break;
+            case 9:
+                tmp_timeinfo.tm_year = 2000 - 1900 + atoi(buf+4);
+                buf[4] = '\0';
+                tmp_timeinfo.tm_mon = atoi(buf+2) - 1;
+                buf[2] = '\0';
+                tmp_timeinfo.tm_mday = atoi(buf);
+                break;
+        }
+    }
+}
+
+void decodeNMEA::update(int type, int row) {
+    if (type == GPRMC && m_status) {
+        lat = tmp_lat;
+        lon = tmp_lon;
+        gprmc_t = mktime(&tmp_timeinfo);
+        update_t = gprmc_t;
+    }
+}
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/USBHostGPS/decodeNMEA.h	Wed Feb 05 13:40:35 2014 +0000
@@ -0,0 +1,28 @@
+// decodeNMEA.h 2012/12/15
+#ifndef DECODE_NMEA_H
+#define DECODE_NMEA_H
+
+class decodeNMEA {
+public:
+    decodeNMEA();
+    void inputNMEA(char* buf, int len);
+    void inputNMEA(char c);
+    float lat,lon;
+    time_t gprmc_t;
+    time_t update_t;
+
+private:
+    void parse(int type, int row, char* buf);
+    void update(int type, int row);
+    int m_seq;
+    int m_type;
+    int m_row;
+    uint8_t m_sum;
+    char m_buf[12];
+    int m_buf_pos;
+    bool m_status;
+    float tmp_lat,tmp_lon;
+    struct tm tmp_timeinfo;
+};
+
+#endif // DECODE_NMEA_H

--- a/main.cpp	Sat Jan 18 13:43:06 2014 +0000
+++ b/main.cpp	Wed Feb 05 13:40:35 2014 +0000
@@ -1,23 +1,40 @@
-// Simple USBHost GPS Dongle for FRDM-KL46Z test program
 #include "USBHostGPS.h"
+#include "SLCD.h"
 
-DigitalOut led1(PTD5); // green
-DigitalOut led2(PTE29); // red
+DigitalOut led1(LED_GREEN);
+DigitalOut led2(LED_RED);
 #define LED_OFF 1
 #define LED_ON  0
 
+SLCD slcd;
+
+void callback_gps(char* buf, int size) {
+    for(int i = 0; i < size; i++) {
+        char c = buf[i];
+        printf("%c", c);
+    }
+    led2 = !led2;
+}
+
 int main() {
     // GT-730F/L 38400bps
     // Gosget SD-200 GPS DONGLE 4800bps
-    USBHostGPS gps(4800);
-    led2 = LED_OFF;
+     USBHostGPS gps(4800);
+    if (!gps.connect()) {
+        error("GPS not found.\n");
+    }
+    gps.attachEventRaw(callback_gps);
+
+    time_t prev = gps.nmea.update_t;
     while(1) {
-        char buf[64];
-        int result = gps.readNMEA(buf, sizeof(buf));
-        if (result > 0) {
-            for(int i = 0; i < result; i++) {
-                printf("%c", buf[i]);
-            }
+        USBHost::poll();
+        if (prev != gps.nmea.update_t) {
+            struct tm * timeinfo;
+            timeinfo = localtime(&gps.nmea.update_t);
+            char buf[5];
+            strftime(buf, sizeof(buf), "%M%S", timeinfo);
+            slcd.printf("%.4s", buf);
+            prev = gps.nmea.update_t;
             led1 = !led1;
         }
     }