?lang_form? ?lang_select? ?lang_submit? ?lang_endform?
{HEADER END}
{BLAME START}

library

?curdirlinks? -

Blame information for rev 6

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, &degrees);
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, &degrees);
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  
{BLAME END}
{FOOTER START}

Powered by WebSVN v2.8.3