Line No. | Rev | Author | Line |
---|---|---|---|
1 | 6 | kaklik | /*! \file nmea.c \brief NMEA protocol function library. */ |
2 | //***************************************************************************** |
||
3 | // |
||
4 | // File Name : 'nmea.c' |
||
5 | // Title : NMEA protocol function library |
||
6 | // Author : Pascal Stang - Copyright (C) 2002 |
||
7 | // Created : 2002.08.27 |
||
8 | // Revised : 2002.08.27 |
||
9 | // Version : 0.1 |
||
10 | // Target MCU : Atmel AVR Series |
||
11 | // Editor Tabs : 4 |
||
12 | // |
||
13 | // NOTE: This code is currently below version 1.0, and therefore is considered |
||
14 | // to be lacking in some functionality or documentation, or may not be fully |
||
15 | // tested. Nonetheless, you can expect most functions to work. |
||
16 | // |
||
17 | // This code is distributed under the GNU Public License |
||
18 | // which can be found at http://www.gnu.org/licenses/gpl.txt |
||
19 | // |
||
20 | //***************************************************************************** |
||
21 | |||
22 | #ifndef WIN32 |
||
23 | #include <avr/io.h> |
||
24 | #include <avr/interrupt.h> |
||
25 | #include <avr/pgmspace.h> |
||
26 | #endif |
||
27 | #include <string.h> |
||
28 | #include <stdlib.h> |
||
29 | #include <math.h> |
||
30 | |||
31 | #include "global.h" |
||
32 | #include "buffer.h" |
||
33 | #include "rprintf.h" |
||
34 | #include "gps.h" |
||
35 | |||
36 | #include "nmea.h" |
||
37 | |||
38 | // Program ROM constants |
||
39 | |||
40 | // Global variables |
||
41 | extern GpsInfoType GpsInfo; |
||
42 | u08 NmeaPacket[NMEA_BUFFERSIZE]; |
||
43 | |||
44 | void nmeaInit(void) |
||
45 | { |
||
46 | } |
||
47 | |||
48 | u08* nmeaGetPacketBuffer(void) |
||
49 | { |
||
50 | return NmeaPacket; |
||
51 | } |
||
52 | |||
53 | u08 nmeaProcess(cBuffer* rxBuffer) |
||
54 | { |
||
55 | u08 foundpacket = NMEA_NODATA; |
||
56 | u08 startFlag = FALSE; |
||
57 | //u08 data; |
||
58 | u16 i,j; |
||
59 | |||
60 | // process the receive buffer |
||
61 | // go through buffer looking for packets |
||
62 | while(rxBuffer->datalength) |
||
63 | { |
||
64 | // look for a start of NMEA packet |
||
65 | if(bufferGetAtIndex(rxBuffer,0) == '$') |
||
66 | { |
||
67 | // found start |
||
68 | startFlag = TRUE; |
||
69 | // when start is found, we leave it intact in the receive buffer |
||
70 | // in case the full NMEA string is not completely received. The |
||
71 | // start will be detected in the next nmeaProcess iteration. |
||
72 | |||
73 | // done looking for start |
||
74 | break; |
||
75 | } |
||
76 | else |
||
77 | bufferGetFromFront(rxBuffer); |
||
78 | } |
||
79 | |||
80 | // if we detected a start, look for end of packet |
||
81 | if(startFlag) |
||
82 | { |
||
83 | for(i=1; i<(rxBuffer->datalength)-1; i++) |
||
84 | { |
||
85 | // check for end of NMEA packet <CR><LF> |
||
86 | if((bufferGetAtIndex(rxBuffer,i) == '\r') && (bufferGetAtIndex(rxBuffer,i+1) == '\n')) |
||
87 | { |
||
88 | // have a packet end |
||
89 | // dump initial '$' |
||
90 | bufferGetFromFront(rxBuffer); |
||
91 | // copy packet to NmeaPacket |
||
92 | for(j=0; j<(i-1); j++) |
||
93 | { |
||
94 | // although NMEA strings should be 80 characters or less, |
||
95 | // receive buffer errors can generate erroneous packets. |
||
96 | // Protect against packet buffer overflow |
||
97 | if(j<(NMEA_BUFFERSIZE-1)) |
||
98 | NmeaPacket[j] = bufferGetFromFront(rxBuffer); |
||
99 | else |
||
100 | bufferGetFromFront(rxBuffer); |
||
101 | } |
||
102 | // null terminate it |
||
103 | NmeaPacket[j] = 0; |
||
104 | // dump <CR><LF> from rxBuffer |
||
105 | bufferGetFromFront(rxBuffer); |
||
106 | bufferGetFromFront(rxBuffer); |
||
107 | |||
108 | #ifdef NMEA_DEBUG_PKT |
||
109 | rprintf("Rx NMEA packet type: "); |
||
110 | rprintfStrLen(NmeaPacket, 0, 5); |
||
111 | rprintfStrLen(NmeaPacket, 5, (i-1)-5); |
||
112 | rprintfCRLF(); |
||
113 | #endif |
||
114 | // found a packet |
||
115 | // done with this processing session |
||
116 | foundpacket = NMEA_UNKNOWN; |
||
117 | break; |
||
118 | } |
||
119 | } |
||
120 | } |
||
121 | |||
122 | if(foundpacket) |
||
123 | { |
||
124 | // check message type and process appropriately |
||
125 | if(!strncmp(NmeaPacket, "GPGGA", 5)) |
||
126 | { |
||
127 | // process packet of this type |
||
128 | nmeaProcessGPGGA(NmeaPacket); |
||
129 | // report packet type |
||
130 | foundpacket = NMEA_GPGGA; |
||
131 | } |
||
132 | else if(!strncmp(NmeaPacket, "GPVTG", 5)) |
||
133 | { |
||
134 | // process packet of this type |
||
135 | nmeaProcessGPVTG(NmeaPacket); |
||
136 | // report packet type |
||
137 | foundpacket = NMEA_GPVTG; |
||
138 | } |
||
139 | } |
||
140 | else if(rxBuffer->datalength >= rxBuffer->size) |
||
141 | { |
||
142 | // if we found no packet, and the buffer is full |
||
143 | // we're logjammed, flush entire buffer |
||
144 | bufferFlush(rxBuffer); |
||
145 | } |
||
146 | return foundpacket; |
||
147 | } |
||
148 | |||
149 | void nmeaProcessGPGGA(u08* packet) |
||
150 | { |
||
151 | u08 i; |
||
152 | char* endptr; |
||
153 | double degrees, minutesfrac; |
||
154 | |||
155 | #ifdef NMEA_DEBUG_GGA |
||
156 | rprintf("NMEA: "); |
||
157 | rprintfStr(packet); |
||
158 | rprintfCRLF(); |
||
159 | #endif |
||
160 | |||
161 | // start parsing just after "GPGGA," |
||
162 | i = 6; |
||
163 | // attempt to reject empty packets right away |
||
164 | if(packet[i]==',' && packet[i+1]==',') |
||
165 | return; |
||
166 | |||
167 | // get UTC time [hhmmss.sss] |
||
168 | GpsInfo.PosLLA.TimeOfFix.f = strtod(&packet[i], &endptr); |
||
169 | while(packet[i++] != ','); // next field: latitude |
||
170 | |||
171 | // get latitude [ddmm.mmmmm] |
||
172 | GpsInfo.PosLLA.lat.f = strtod(&packet[i], &endptr); |
||
173 | // convert to pure degrees [dd.dddd] format |
||
174 | minutesfrac = modf(GpsInfo.PosLLA.lat.f/100, °rees); |
||
175 | GpsInfo.PosLLA.lat.f = degrees + (minutesfrac*100)/60; |
||
176 | // convert to radians |
||
177 | GpsInfo.PosLLA.lat.f *= (M_PI/180); |
||
178 | while(packet[i++] != ','); // next field: N/S indicator |
||
179 | |||
180 | // correct latitute for N/S |
||
181 | if(packet[i] == 'S') GpsInfo.PosLLA.lat.f = -GpsInfo.PosLLA.lat.f; |
||
182 | while(packet[i++] != ','); // next field: longitude |
||
183 | |||
184 | // get longitude [ddmm.mmmmm] |
||
185 | GpsInfo.PosLLA.lon.f = strtod(&packet[i], &endptr); |
||
186 | // convert to pure degrees [dd.dddd] format |
||
187 | minutesfrac = modf(GpsInfo.PosLLA.lon.f/100, °rees); |
||
188 | GpsInfo.PosLLA.lon.f = degrees + (minutesfrac*100)/60; |
||
189 | // convert to radians |
||
190 | GpsInfo.PosLLA.lon.f *= (M_PI/180); |
||
191 | while(packet[i++] != ','); // next field: E/W indicator |
||
192 | |||
193 | // correct latitute for E/W |
||
194 | if(packet[i] == 'W') GpsInfo.PosLLA.lon.f = -GpsInfo.PosLLA.lon.f; |
||
195 | while(packet[i++] != ','); // next field: position fix status |
||
196 | |||
197 | // position fix status |
||
198 | // 0 = Invalid, 1 = Valid SPS, 2 = Valid DGPS, 3 = Valid PPS |
||
199 | // check for good position fix |
||
200 | if( (packet[i] != '0') && (packet[i] != ',') ) |
||
201 | GpsInfo.PosLLA.updates++; |
||
202 | while(packet[i++] != ','); // next field: satellites used |
||
203 | |||
204 | // get number of satellites used in GPS solution |
||
205 | GpsInfo.numSVs = atoi(&packet[i]); |
||
206 | while(packet[i++] != ','); // next field: HDOP (horizontal dilution of precision) |
||
207 | while(packet[i++] != ','); // next field: altitude |
||
208 | |||
209 | // get altitude (in meters) |
||
210 | GpsInfo.PosLLA.alt.f = strtod(&packet[i], &endptr); |
||
211 | |||
212 | while(packet[i++] != ','); // next field: altitude units, always 'M' |
||
213 | while(packet[i++] != ','); // next field: geoid seperation |
||
214 | while(packet[i++] != ','); // next field: seperation units |
||
215 | while(packet[i++] != ','); // next field: DGPS age |
||
216 | while(packet[i++] != ','); // next field: DGPS station ID |
||
217 | while(packet[i++] != '*'); // next field: checksum |
||
218 | } |
||
219 | |||
220 | void nmeaProcessGPVTG(u08* packet) |
||
221 | { |
||
222 | u08 i; |
||
223 | char* endptr; |
||
224 | |||
225 | #ifdef NMEA_DEBUG_VTG |
||
226 | rprintf("NMEA: "); |
||
227 | rprintfStr(packet); |
||
228 | rprintfCRLF(); |
||
229 | #endif |
||
230 | |||
231 | // start parsing just after "GPVTG," |
||
232 | i = 6; |
||
233 | // attempt to reject empty packets right away |
||
234 | if(packet[i]==',' && packet[i+1]==',') |
||
235 | return; |
||
236 | |||
237 | // get course (true north ref) in degrees [ddd.dd] |
||
238 | GpsInfo.VelHS.heading.f = strtod(&packet[i], &endptr); |
||
239 | while(packet[i++] != ','); // next field: 'T' |
||
240 | while(packet[i++] != ','); // next field: course (magnetic north) |
||
241 | |||
242 | // get course (magnetic north ref) in degrees [ddd.dd] |
||
243 | //GpsInfo.VelHS.heading.f = strtod(&packet[i], &endptr); |
||
244 | while(packet[i++] != ','); // next field: 'M' |
||
245 | while(packet[i++] != ','); // next field: speed (knots) |
||
246 | |||
247 | // get speed in knots |
||
248 | //GpsInfo.VelHS.speed.f = strtod(&packet[i], &endptr); |
||
249 | while(packet[i++] != ','); // next field: 'N' |
||
250 | while(packet[i++] != ','); // next field: speed (km/h) |
||
251 | |||
252 | // get speed in km/h |
||
253 | GpsInfo.VelHS.speed.f = strtod(&packet[i], &endptr); |
||
254 | while(packet[i++] != ','); // next field: 'K' |
||
255 | while(packet[i++] != '*'); // next field: checksum |
||
256 | |||
257 | GpsInfo.VelHS.updates++; |
||
258 | } |
||
259 |
Powered by WebSVN v2.8.3