Rev Author Line No. Line
1213 mija 1 //************************************************************************
2 // NMEA LOAD FROM rx GPS
3  
4 #include <stdint.h>
5 #include <math.h>
6 #include <stdlib.h>
7 #include "nmea_scan.h"
8 #include "gps.h"
9  
10 enum {ID_RX_GGA,ID_RX_GSA,ID_RX_GSV,ID_RX_RMC,ID_RX_VTG,ID_RX_LOAD,ID_RX_FIND};
11  
12 uint8_t nmea_start_load(char data)
13 {
14 enum {NM_G,NM_P,NM_GP,NM_GPG,NM_GPR,NM_GPV,NM_GPGG,NM_GPGS,NM_GPRM,NM_GPVT};
15 static uint8_t ptr = NM_G;
16  
17 switch (ptr)
18 {
19 case NM_G: if (data == 'G') ptr = NM_P; //G
20 else { ptr = NM_G; return ID_RX_FIND;}
21 break;
22 case NM_P: if (data == 'P') ptr = NM_GP; //P
23 else { ptr = NM_G; return ID_RX_FIND;}
24 break;
25 case NM_GP: switch (data) //GPG || GPR || GPV
26 {
27 case 'G': ptr=NM_GPG;break;
28 case 'R': ptr=NM_GPR;break;
29 case 'V': ptr=NM_GPV;break;
30 default : ptr = NM_G; return ID_RX_FIND;
31 }
32 break;
33 case NM_GPG: switch(data) //GPGG || GPGS
34 {
35 case 'G': ptr=NM_GPGG;break;
36 case 'S': ptr=NM_GPGS;break;
37 default : ptr = NM_G; return ID_RX_FIND;
38 }
39 break;
40 case NM_GPR: if (data == 'M') ptr = NM_GPRM; //GPRM
41 else { ptr = NM_G; return ID_RX_FIND;}
42 break;
43 case NM_GPV: if (data == 'T') ptr = NM_GPVT; //GPVT
44 else { ptr = NM_G; return ID_RX_FIND;}
45 break;
46 case NM_GPGG: ptr = NM_G; //GPGGA
47 if (data == 'A') return ID_RX_GGA;
48 return ID_RX_FIND;
49 case NM_GPGS: ptr = NM_G;
50 switch(data) //GPGG || GPGS
51 {
52 case 'A': return ID_RX_GSA;
53 case 'V': return ID_RX_GSV;
54 }
55 return ID_RX_FIND;
56 case NM_GPRM: ptr = NM_G; //GPRMC
57 if (data == 'C') return ID_RX_RMC;
58 return ID_RX_FIND;
59 case NM_GPVT: ptr = NM_G; //GPVTG
60 if (data == 'G') return ID_RX_VTG;
61 return ID_RX_FIND;
62 default: ptr = NM_G; return ID_RX_FIND;
63  
64 }
65 return ID_RX_LOAD;
66 }
67  
68 uint8_t load_nmea(uint8_t rx_shift, char *rx_buf,char *scan_buf)
69 {
70 static uint8_t id_rx_msg = ID_RX_FIND;
71 static uint8_t rx_now = 0;
72 static uint8_t ptr = 0;
73 char data;
74  
75 while (1)
76 {
77 if (rx_shift == rx_now) return RETURN_RX;
78 if(++rx_now >= MAX_RX_BUF) rx_now = 0;
79 data = *(rx_buf + rx_now);
80  
81 switch (id_rx_msg)
82 {
83 case ID_RX_GGA: *(scan_buf+ptr++) = data;
84 if (data == '*') {id_rx_msg = ID_RX_FIND; return RETURN_GGA;}
85 if (ptr >= MAX_NMEA_LOAD) id_rx_msg = ID_RX_FIND;
86 break;
87 case ID_RX_GSA: *(scan_buf+ptr++) = data;
88 if (data == '*') {id_rx_msg = ID_RX_FIND; return RETURN_GSA;}
89 if (ptr >= MAX_NMEA_LOAD) id_rx_msg = ID_RX_FIND;
90 break;
91 case ID_RX_GSV: *(scan_buf+ptr++) = data;
92 if (data == '*') {id_rx_msg = ID_RX_FIND; return RETURN_GSV;}
93 if (ptr >= MAX_NMEA_LOAD) id_rx_msg = ID_RX_FIND;
94 break;
95 case ID_RX_RMC: *(scan_buf+ptr++) = data;
96 if (data == '*') {id_rx_msg = ID_RX_FIND; return RETURN_RMC;}
97 if (ptr >= MAX_NMEA_LOAD) id_rx_msg = ID_RX_FIND;
98 break;
99 case ID_RX_VTG: *(scan_buf+ptr++) = data;
100 if (data == '*') {id_rx_msg = ID_RX_FIND;return RETURN_VTG;}
101 if (ptr >= MAX_NMEA_LOAD) id_rx_msg = ID_RX_FIND;
102 break;
103 case ID_RX_LOAD: id_rx_msg = nmea_start_load(data); break;
104 case ID_RX_FIND:
105 default: ptr=0;if (data == '$') id_rx_msg = ID_RX_LOAD;
106  
107 }
108 }
109 return 0 ;
110 }
111  
112 void nmea_gga(char *buf,DATA_GPS *pgps)
113 {
114 uint8_t a;
115  
116 if (*(buf++) != ',') return; // ,
117 for (a=0;a<5;a++) while (*(buf++) != ','); // UTC,lat,N,lot,W,
118 pgps->fix_position = *(buf++) - 0x30; // fix_position
119 if (*(buf++) != ',') return; // ,
120 pgps->satelites_used = 10 * (*(buf++) - 0x30) ; // satelites
121 pgps->satelites_used += *(buf++)-0x30;
122 if (*(buf++) != ',') return; // ,
123 while (*(buf++) != ','); // HDOP,
124 pgps->altitude = atof(buf++); // atlitude,
125 for (a=0;a<2;a++) while (*(buf++) != ','); // ,M,
126 pgps->geoid = atof(buf++); // geoid
127 for (a=0;a<2;a++) while (*(buf++) != ','); //,M,
128 if (*buf != ',')
129 {
130 pgps->age_diff_corr = atol(buf); // age_dif_corr,
131 while (*(buf++) != ',');
132 }
133 else buf++;
134 if (*buf != ',') pgps->diff_id = atol(buf); // ID_diff_station
135 }
136  
137 void nmea_gsa(char *buf,DATA_GPS *pgps)
138 {
139 uint8_t a;
140  
141 if (*(buf++) != ',') return; // ,
142 pgps->mode1 = *(buf++); // mode1
143 if (*(buf++) != ',') return; // ,
144 pgps->mode2 = *(buf++); // mode2
145 if (*(buf++) != ',') return; // ,
146 for (a=0;a<12;a++)
147 {
148 if (*buf != ',')
149 {
150 //pgps->satelite_id[a] = 10*(*(buf++) - 0x30);
151 //pgps->satelite_id[a] += *(buf++)-0x30;
152 //buf++;
153 pgps->satelite_id[a] = atol(buf);
154 while (*(buf++) != ',');
155 }
156 else
157 {
158 pgps->satelite_id[a] = 0;
159 buf++;
160 }
161 }
162 pgps->PDOP = atof(buf++);
163 while (*(buf++) != ',');
164 pgps->HDOP = atof(buf++);
165 while (*(buf++) != ',');
166 pgps->VDOP = atof(buf++);
167 while (*(buf++) != ',');
168 }
169  
170 void nmea_gsv(char *buf,DATA_GPS *pgps)
171 {
172 uint8_t a,i,b;
173  
174 if (*(buf++) != ',') return; // ,
175 pgps->gsv_num_msg = (*(buf++) - 0x30); // num of msg
176 if (*(buf++) != ',') return; //,
177 pgps->gsv_msg = (*(buf++) - 0x30); // num msg
178 if (*(buf++) != ',') return; //,
179 pgps->gsv_satelites_view = 10*(*(buf++) - 0x30); // satelites view
180 pgps->gsv_satelites_view +=(*(buf++) - 0x30);
181 if (*(buf++) != ',') return; // ,
182 a = 4 * (pgps->gsv_msg - 1);
183 for (i= 0; i<4; i++)
184 {
185 /*pgps->satelit_detail[a].id = 10*(*(buf++) - 0x30);
186 pgps->satelit_detail[a].id += *(buf++) - 0x30;
187 buf++;
188 pgps->satelit_detail[a].elevation = 10*(*(buf++) - 0x30);
189 pgps->satelit_detail[a].elevation += *(buf++) - 0x30;
190 buf++;
191 pgps->satelit_detail[a].azimut = 100*(*(buf++) - 0x30);
192 pgps->satelit_detail[a].azimut += 10*(*(buf++) - 0x30);
193 pgps->satelit_detail[a].azimut += *(buf++) - 0x30;
194 buf++;*/
195 pgps->satelit_detail[a].id = atol(buf);
196 while (*(buf++) != ',');
197 pgps->satelit_detail[a].elevation = atol(buf);
198 while (*(buf++) != ',');
199 pgps->satelit_detail[a].azimut = atol(buf);
200 while (*(buf++) != ',');
201  
202 if ((*buf) != ',')
203 {
204 if (*(buf) == '*')
205 {
206 pgps->satelit_detail[a].SNR =0;
207 return;
208 }
209 //pgps->satelit_detail[a].SNR = 10*(*(buf++) - 0x30);
210 //pgps->satelit_detail[a].SNR += *(buf++) - 0x30;
211 pgps->satelit_detail[a].SNR = atol(buf);
212 b= *(buf++);
213 while ((b != ',') && (b != '*') ) b=*(buf++);
214 if (b == '*') return;
215 }
216 else
217 {
218 pgps->satelit_detail[a].SNR =0;
219 buf++;
220 }
221 if ( ++a >= pgps->gsv_satelites_view ) return;
222  
223 }
224 }
225  
226 //$GPRMC,161229.487,A,3723.2475,N,12158.3416,W,0.13,309.62,120598, ,*10
227 void nmea_rmc(char *buf,DATA_GPS *pgps)
228 {
229 uint8_t a;
230  
231 if (*(buf++) != ',') return; // ,
232 pgps->hour = (10* (*(buf++) - 0x30)); //hour
233 pgps->hour += (*(buf++) - 0x30);
234 pgps->minute = 10* (*(buf++) - 0x30); //minute
235 pgps->minute += (*(buf++) - 0x30);
236 pgps->second = 10* (*(buf++) - 0x30); //second
237 pgps->second += (*(buf++) - 0x30);
238 while (*(buf++) != ','); //.xxx
239 pgps->status = *(buf++);
240 if (*(buf++) != ',') return; //A,
241 pgps->latitude =10.0 * (*(buf++) - 0x30);
242 pgps->latitude += 1.0 * (*(buf++) - 0x30);
243 pgps->latitude += atof(buf)/60.0;
244 while (*(buf++) != ','); //latitude,
245 pgps->ns_indicator = *(buf++); // N/S
246 if (*(buf++) != ',') return; //,
247 pgps->longitude = 100.0 * (*(buf++) - 0x30);
248 pgps->longitude += 10.0 * (*(buf++) - 0x30);
249 pgps->longitude += 1.0 * (*(buf++) - 0x30);
250 pgps->longitude += atof(buf)/60.0;
251 while (*(buf++) != ','); //longitude,
252 pgps->we_indicator = *(buf++); // E/W
253 for (a=0;a<3;a++) while (*(buf++) != ','); // ,speed,course,
254 pgps->day = 10* (*(buf++) - 0x30);
255 pgps->day += *(buf++) - 0x30; // day
256 pgps->month = 10* (*(buf++) - 0x30);
257 pgps->month += *(buf++) - 0x30; // month
258 pgps->year = 10* (*(buf++) - 0x30);
259 pgps->year += *(buf) - 0x30; // year
260  
261 }
262  
263 void nmea_vtg(char *buf,DATA_GPS *pgps)
264 {
265 uint8_t a;
266  
267 if (*(buf++) != ',') return; // ,
268 pgps->course = atof(buf++); // course
269 for (a=0;a<6;a++) while (*(buf++) != ','); // ,T,course,M,speed,N,
270 pgps->speed = atof(buf++);
271 }
272