1 | /**
|
2 | * @brief Gibt den String nach dem n-ten Komma zurück
|
3 | * @param sentence Enthält NMEA Satz
|
4 | * @param n Nummer des Teilstrings
|
5 | * @return String
|
6 | */
|
7 |
|
8 | char *readFieldByNumber (char *sentence, uint8_t n)
|
9 | {
|
10 | static char result[100];
|
11 | char *p = sentence;
|
12 |
|
13 | while (n-- > 0)
|
14 | while (*p++ != ',') ;
|
15 | // if (strlen(sentence)>=99) {
|
16 | // printf_P(PSTR("sentence zu lang: %d\n%s\n"),strlen(sentence),sentence);
|
17 | // }
|
18 | strncpy (result, p,100);
|
19 | result[99]=0;
|
20 | p = result;
|
21 | while (*p && *p != ',' && *p != '*' && *p != '\r')
|
22 | p++;
|
23 |
|
24 | *p = '\0';
|
25 | return result;
|
26 | }
|
27 |
|
28 | /**
|
29 | * @brief Parst den GGA Satz
|
30 | * @param data Enthält NMEA Satz GGA
|
31 | *
|
32 | * $GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47
|
33 | * @return none Daten sind in der Struktur gpspos
|
34 | */
|
35 |
|
36 | void parseGGA(char *data) {
|
37 | char chksum[3];
|
38 | char value=0,csnum=0;
|
39 | char *csPointer;
|
40 | char tmp[5];
|
41 | float m;
|
42 | int16_t d;
|
43 | static uint32_t oldAge=0;
|
44 | float oldLat,oldLon;
|
45 |
|
46 | csPointer=(data+1);
|
47 | while(*csPointer != '*' ){
|
48 | value ^= *(csPointer++);
|
49 | }
|
50 | chksum[0]=*(++csPointer);
|
51 | chksum[1]=*(++csPointer);
|
52 | chksum[2]=0;
|
53 | csnum=strtol(chksum, NULL, 16);
|
54 | if (csnum!=value) {
|
55 | printf_P(PSTR("NMEA CS ! %s %02x\n"),chksum,value);
|
56 | return;
|
57 | }
|
58 |
|
59 | oldLat = gpspos.lat;
|
60 | oldLon = gpspos.lon;
|
61 |
|
62 | gpspos.lat=atof(readFieldByNumber(data, 2));
|
63 | d=gpspos.lat/100;
|
64 | m=(gpspos.lat-100*d);
|
65 | gpspos.lat = d + m / 60.0;
|
66 |
|
67 | strncpy(tmp, readFieldByNumber(data, 3), 2);
|
68 | if (tmp[0]=='S') {
|
69 | gpspos.lat=-gpspos.lat;
|
70 | }
|
71 | gpspos.lon=atof(readFieldByNumber(data, 4));
|
72 | d=gpspos.lon/100;
|
73 | m=(gpspos.lon-100*d);
|
74 | gpspos.lon = d + m / 60.0;
|
75 |
|
76 | gpspos.alt=atof(readFieldByNumber(data, 9));
|
77 |
|
78 | strncpy(tmp, readFieldByNumber(data, 5), 2);
|
79 | if (tmp[0]=='W') {
|
80 | gpspos.lon=-gpspos.lon;
|
81 | }
|
82 | gpspos.fix=strtol(readFieldByNumber(data, 6), NULL, 10);
|
83 | gpspos.sats=strtol(readFieldByNumber(data, 7), NULL, 10);
|
84 | // Horizontal dilution
|
85 | gpspos.hdop=atof(readFieldByNumber(data, 8));
|
86 |
|
87 | // printf_P(PSTR("lat: %f lon: %f fix:%d sats: %d Zeit: %s %d\n"),
|
88 | // gpspos.lat,gpspos.lon,gpspos.fix,gpspos.sats,gpspos.utc,strlen(data));
|
89 | if (gpspos.fix>0) {
|
90 | oldAge = uptimeCounter;
|
91 | } else {
|
92 | // Nicht 0.0 anzeigen, weil letzte Position angezeigt werden soll bei Fixverlust
|
93 | gpspos.lon = oldLon;
|
94 | gpspos.lat = oldLat;
|
95 | }
|
96 | gpspos.age = uptimeCounter - oldAge; // Zeit in Sekunden seit letzter Position
|
97 | return;
|
98 | }
|
99 |
|
100 |
|
101 | /**
|
102 | * @brief Parst den GZA Satz
|
103 | * @param data Enthält NMEA Satz GZA (Datum und Zeit)
|
104 | *
|
105 | * $GPZDA,194916.00,09,06,2015,00,00*6D
|
106 | * @return none Daten sind in der Struktur gpspos
|
107 | */
|
108 |
|
109 | void parseZDA(char *data) {
|
110 | char chksum[3];
|
111 | char value=0,csnum=0;
|
112 | char *csPointer;
|
113 | char *p;
|
114 | static uint8_t needDateCounter=0;
|
115 |
|
116 | csPointer=(data+1);
|
117 | while(*csPointer != '*' ){
|
118 | value ^= *(csPointer++);
|
119 | }
|
120 | chksum[0]=*(++csPointer);
|
121 | chksum[1]=*(++csPointer);
|
122 | chksum[2]=0;
|
123 | csnum=strtol(chksum, NULL, 16);
|
124 | if (csnum!=value) {
|
125 | printf_P(PSTR("NMEA CS ! %s %02x\n"),chksum,value);
|
126 | return;
|
127 | }
|
128 |
|
129 | strncpy(gpspos.date, readFieldByNumber(data, 4),4); // Jahr
|
130 | strncpy((gpspos.date+4), readFieldByNumber(data, 3),2); // Monat
|
131 | strncpy((gpspos.date+6), readFieldByNumber(data, 2),2); // Tag
|
132 | gpspos.date[8]=0;
|
133 |
|
134 | p=strncpy(gpspos.utc, readFieldByNumber(data, 1),sizeof(gpspos.utc));
|
135 | if (debugOut) {
|
136 | printf_P(PSTR("ZDA Zeit: %s(%d bytes)"),gpspos.utc,strlen(p));
|
137 | }
|
138 | if (strlen(p)==0) {
|
139 | strncpy(gpspos.utc,"---------",sizeof(gpspos.utc));
|
140 | if (gpsBattGood!=0) {
|
141 | gpsBattGood--;
|
142 | }
|
143 | } else {
|
144 | if (need_setDate!=0) {
|
145 | needDateCounter++;
|
146 | if (needDateCounter>30) {
|
147 | setGPSDate();
|
148 | needDateCounter = 30;
|
149 | }
|
150 | }
|
151 | }
|
152 |
|
153 | // GPS in Powersave schicken
|
154 | if (uBattAvg>POWERSAVELIMIT) {
|
155 | if (gpsIsPowerSave) {
|
156 | switchGpsPowersave(0);
|
157 | }
|
158 | } else {
|
159 | if (!gpsIsPowerSave) {
|
160 | switchGpsPowersave(1);
|
161 | }
|
162 | }
|
163 |
|
164 | return;
|
165 | }
|
