DJI NAZA-M controller (remote controller side) see: https://developer.mbed.org/users/okini3939/notebook/drone/
Dependencies: NECnfc SpiOLED USBHost mbed
gps.cpp
- Committer:
- okini3939
- Date:
- 2016-05-19
- Revision:
- 1:d83f8332ebfe
- Parent:
- 0:9f11e7a30865
File content as of revision 1:d83f8332ebfe:
#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); }