Diff: gps.cpp

Revision:

0:9f11e7a30865

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/gps.cpp	Thu May 19 09:03:44 2016 +0000
@@ -0,0 +1,120 @@
+#include "mbed.h"
+#include "drone.h"
+
+RawSerial gps(p28, p27);
+
+static char buf_gps[80];
+static int count_gps = 0;
+
+extern struct Status stat_gnd;
+
+float gpsminsec2degree (float ms) { // dddmm.mmmm -> deg
+    float deg;
+
+    deg = (int)ms / 100;
+    deg += (ms - (deg * 100)) / 60.0;
+    return deg;
+}
+
+int dmy2ymd (int d) {
+    if (! d) return 0;
+    return ((2000 + (d % 100)) * 10000) + (((d / 100) % 100) * 100) + ((d / 10000) % 100);
+}
+
+void parseGpsNmea () {
+    int i = 0;
+    float f;
+    char *buf = buf_gps;
+
+    if (strncmp(buf, "$GPRMC", 6) == 0) {
+        for (;;) {
+            buf = strstr(buf, ",");
+            if (buf == NULL) break;
+            buf ++;
+            f = atof(buf);
+            i ++;
+            switch (i) {
+            case 1:
+                stat_gnd.gps_time = f;
+                break;
+            case 9:
+                stat_gnd.gps_date = dmy2ymd(f);
+                break;
+            }
+        }
+    } else
+    if (strncmp(buf, "$GPGGA", 6) == 0) {
+        for (;;) {
+            buf = strstr(buf, ",");
+            if (buf == NULL) break;
+            buf ++;
+            f = atof(buf);
+            i ++;
+            switch (i) {
+            case 2:
+                stat_gnd.gps_lat = gpsminsec2degree(f) * 10000000;
+                break;
+            case 3:
+                if (buf[0] == 'S') stat_gnd.gps_lat = - stat_gnd.gps_lat;
+                break;
+            case 4:
+                stat_gnd.gps_lng = gpsminsec2degree(f) * 10000000;
+                break;
+            case 5:
+                if (buf[0] == 'W') stat_gnd.gps_lng = - stat_gnd.gps_lng;
+                break;
+            case 6:
+                if (buf[0] != '0') {
+                    stat_gnd.gps_flg = 1;
+                } else {
+                    stat_gnd.gps_lat = 0;
+                    stat_gnd.gps_lng = 0;
+                }
+                break;
+            case 7:
+//                gps_cnt = f;
+                break;
+            case 9:
+                stat_gnd.gps_h = f;
+                break;
+            }
+        }
+    } else
+    if (strncmp(buf, "$GPGSV", 6) == 0) {
+        for (;;) {
+            buf = strstr(buf, ",");
+            if (buf == NULL) break;
+            buf ++;
+            f = atof(buf);
+            i ++;
+            switch (i) {
+            case 3:
+                stat_gnd.gps_sat = f;
+                break;
+            }
+        }
+    } else
+    if (strncmp(buf, "$GPGLL", 6) == 0) {
+    }
+}
+
+void isrGps () {
+    char c;
+    c = gps.getc();
+    if (c == '\r' || c == '\n') {
+        buf_gps[count_gps] = 0;
+        if (count_gps) {
+            parseGpsNmea();
+            count_gps = 0;
+        }
+    } else
+    if (count_gps < sizeof(buf_gps) - 1) {
+        buf_gps[count_gps] = c;
+        count_gps ++;
+    }
+}
+
+void initGps () {
+    gps.baud(4800);
+    gps.attach(isrGps);
+}