Suga koubou / Mbed 2 deprecated Drone_ground

DJI NAZA-M controller (remote controller side) see: https://developer.mbed.org/users/okini3939/notebook/drone/

Dependencies:   NECnfc SpiOLED USBHost mbed

gps.cpp@1:d83f8332ebfe, 2016-05-19 (annotated)

Committer:
okini3939
Date:
Thu May 19 09:05:02 2016 +0000
Revision:
1:d83f8332ebfe
Parent:
0:9f11e7a30865 
1st build;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
okini3939 0:9f11e7a30865 1 #include "mbed.h"
okini3939 0:9f11e7a30865 2 #include "drone.h"
okini3939 0:9f11e7a30865 3
okini3939 0:9f11e7a30865 4 RawSerial gps(p28, p27);
okini3939 0:9f11e7a30865 5
okini3939 0:9f11e7a30865 6 static char buf_gps[80];
okini3939 0:9f11e7a30865 7 static int count_gps = 0;
okini3939 0:9f11e7a30865 8
okini3939 0:9f11e7a30865 9 extern struct Status stat_gnd;
okini3939 0:9f11e7a30865 10
okini3939 0:9f11e7a30865 11 float gpsminsec2degree (float ms) { // dddmm.mmmm -> deg
okini3939 0:9f11e7a30865 12 float deg;
okini3939 0:9f11e7a30865 13
okini3939 0:9f11e7a30865 14 deg = (int)ms / 100;
okini3939 0:9f11e7a30865 15 deg += (ms - (deg * 100)) / 60.0;
okini3939 0:9f11e7a30865 16 return deg;
okini3939 0:9f11e7a30865 17 }
okini3939 0:9f11e7a30865 18
okini3939 0:9f11e7a30865 19 int dmy2ymd (int d) {
okini3939 0:9f11e7a30865 20 if (! d) return 0;
okini3939 0:9f11e7a30865 21 return ((2000 + (d % 100)) * 10000) + (((d / 100) % 100) * 100) + ((d / 10000) % 100);
okini3939 0:9f11e7a30865 22 }
okini3939 0:9f11e7a30865 23
okini3939 0:9f11e7a30865 24 void parseGpsNmea () {
okini3939 0:9f11e7a30865 25 int i = 0;
okini3939 0:9f11e7a30865 26 float f;
okini3939 0:9f11e7a30865 27 char *buf = buf_gps;
okini3939 0:9f11e7a30865 28
okini3939 0:9f11e7a30865 29 if (strncmp(buf, "$GPRMC", 6) == 0) {
okini3939 0:9f11e7a30865 30 for (;;) {
okini3939 0:9f11e7a30865 31 buf = strstr(buf, ",");
okini3939 0:9f11e7a30865 32 if (buf == NULL) break;
okini3939 0:9f11e7a30865 33 buf ++;
okini3939 0:9f11e7a30865 34 f = atof(buf);
okini3939 0:9f11e7a30865 35 i ++;
okini3939 0:9f11e7a30865 36 switch (i) {
okini3939 0:9f11e7a30865 37 case 1:
okini3939 0:9f11e7a30865 38 stat_gnd.gps_time = f;
okini3939 0:9f11e7a30865 39 break;
okini3939 0:9f11e7a30865 40 case 9:
okini3939 0:9f11e7a30865 41 stat_gnd.gps_date = dmy2ymd(f);
okini3939 0:9f11e7a30865 42 break;
okini3939 0:9f11e7a30865 43 }
okini3939 0:9f11e7a30865 44 }
okini3939 0:9f11e7a30865 45 } else
okini3939 0:9f11e7a30865 46 if (strncmp(buf, "$GPGGA", 6) == 0) {
okini3939 0:9f11e7a30865 47 for (;;) {
okini3939 0:9f11e7a30865 48 buf = strstr(buf, ",");
okini3939 0:9f11e7a30865 49 if (buf == NULL) break;
okini3939 0:9f11e7a30865 50 buf ++;
okini3939 0:9f11e7a30865 51 f = atof(buf);
okini3939 0:9f11e7a30865 52 i ++;
okini3939 0:9f11e7a30865 53 switch (i) {
okini3939 0:9f11e7a30865 54 case 2:
okini3939 0:9f11e7a30865 55 stat_gnd.gps_lat = gpsminsec2degree(f) * 10000000;
okini3939 0:9f11e7a30865 56 break;
okini3939 0:9f11e7a30865 57 case 3:
okini3939 0:9f11e7a30865 58 if (buf[0] == 'S') stat_gnd.gps_lat = - stat_gnd.gps_lat;
okini3939 0:9f11e7a30865 59 break;
okini3939 0:9f11e7a30865 60 case 4:
okini3939 0:9f11e7a30865 61 stat_gnd.gps_lng = gpsminsec2degree(f) * 10000000;
okini3939 0:9f11e7a30865 62 break;
okini3939 0:9f11e7a30865 63 case 5:
okini3939 0:9f11e7a30865 64 if (buf[0] == 'W') stat_gnd.gps_lng = - stat_gnd.gps_lng;
okini3939 0:9f11e7a30865 65 break;
okini3939 0:9f11e7a30865 66 case 6:
okini3939 0:9f11e7a30865 67 if (buf[0] != '0') {
okini3939 0:9f11e7a30865 68 stat_gnd.gps_flg = 1;
okini3939 0:9f11e7a30865 69 } else {
okini3939 0:9f11e7a30865 70 stat_gnd.gps_lat = 0;
okini3939 0:9f11e7a30865 71 stat_gnd.gps_lng = 0;
okini3939 0:9f11e7a30865 72 }
okini3939 0:9f11e7a30865 73 break;
okini3939 0:9f11e7a30865 74 case 7:
okini3939 0:9f11e7a30865 75 // gps_cnt = f;
okini3939 0:9f11e7a30865 76 break;
okini3939 0:9f11e7a30865 77 case 9:
okini3939 0:9f11e7a30865 78 stat_gnd.gps_h = f;
okini3939 0:9f11e7a30865 79 break;
okini3939 0:9f11e7a30865 80 }
okini3939 0:9f11e7a30865 81 }
okini3939 0:9f11e7a30865 82 } else
okini3939 0:9f11e7a30865 83 if (strncmp(buf, "$GPGSV", 6) == 0) {
okini3939 0:9f11e7a30865 84 for (;;) {
okini3939 0:9f11e7a30865 85 buf = strstr(buf, ",");
okini3939 0:9f11e7a30865 86 if (buf == NULL) break;
okini3939 0:9f11e7a30865 87 buf ++;
okini3939 0:9f11e7a30865 88 f = atof(buf);
okini3939 0:9f11e7a30865 89 i ++;
okini3939 0:9f11e7a30865 90 switch (i) {
okini3939 0:9f11e7a30865 91 case 3:
okini3939 0:9f11e7a30865 92 stat_gnd.gps_sat = f;
okini3939 0:9f11e7a30865 93 break;
okini3939 0:9f11e7a30865 94 }
okini3939 0:9f11e7a30865 95 }
okini3939 0:9f11e7a30865 96 } else
okini3939 0:9f11e7a30865 97 if (strncmp(buf, "$GPGLL", 6) == 0) {
okini3939 0:9f11e7a30865 98 }
okini3939 0:9f11e7a30865 99 }
okini3939 0:9f11e7a30865 100
okini3939 0:9f11e7a30865 101 void isrGps () {
okini3939 0:9f11e7a30865 102 char c;
okini3939 0:9f11e7a30865 103 c = gps.getc();
okini3939 0:9f11e7a30865 104 if (c == '\r' || c == '\n') {
okini3939 0:9f11e7a30865 105 buf_gps[count_gps] = 0;
okini3939 0:9f11e7a30865 106 if (count_gps) {
okini3939 0:9f11e7a30865 107 parseGpsNmea();
okini3939 0:9f11e7a30865 108 count_gps = 0;
okini3939 0:9f11e7a30865 109 }
okini3939 0:9f11e7a30865 110 } else
okini3939 0:9f11e7a30865 111 if (count_gps < sizeof(buf_gps) - 1) {
okini3939 0:9f11e7a30865 112 buf_gps[count_gps] = c;
okini3939 0:9f11e7a30865 113 count_gps ++;
okini3939 0:9f11e7a30865 114 }
okini3939 0:9f11e7a30865 115 }
okini3939 0:9f11e7a30865 116
okini3939 0:9f11e7a30865 117 void initGps () {
okini3939 0:9f11e7a30865 118 gps.baud(4800);
okini3939 0:9f11e7a30865 119 gps.attach(isrGps);
okini3939 0:9f11e7a30865 120 }