00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022 #ifndef WIN32
00023 #include <avr/io.h>
00024 #include <avr/signal.h>
00025 #include <avr/interrupt.h>
00026 #include <avr/pgmspace.h>
00027 #endif
00028 #include <string.h>
00029 #include <stdlib.h>
00030 #include <math.h>
00031
00032 #include "global.h"
00033 #include "buffer.h"
00034 #include "rprintf.h"
00035 #include "gps.h"
00036
00037 #include "nmea.h"
00038
00039
00040
00041
00042 extern GpsInfoType GpsInfo;
00043 u08 NmeaPacket[NMEA_BUFFERSIZE];
00044
00045 void nmeaInit(void)
00046 {
00047 }
00048
00049 u08* nmeaGetPacketBuffer(void)
00050 {
00051 return NmeaPacket;
00052 }
00053
00054 u08 nmeaProcess(cBuffer* rxBuffer)
00055 {
00056 u08 foundpacket = NMEA_NODATA;
00057 u08 startFlag = FALSE;
00058
00059 u16 i,j;
00060
00061
00062
00063 while(rxBuffer->datalength)
00064 {
00065
00066 if(bufferGetAtIndex(rxBuffer,0) == '$')
00067 {
00068
00069 startFlag = TRUE;
00070
00071
00072
00073
00074
00075 break;
00076 }
00077 else
00078 bufferGetFromFront(rxBuffer);
00079 }
00080
00081
00082 if(startFlag)
00083 {
00084 for(i=1; i<(rxBuffer->datalength)-1; i++)
00085 {
00086
00087 if((bufferGetAtIndex(rxBuffer,i) == '\r') && (bufferGetAtIndex(rxBuffer,i+1) == '\n'))
00088 {
00089
00090
00091 bufferGetFromFront(rxBuffer);
00092
00093 for(j=0; j<(i-1); j++)
00094 {
00095
00096
00097
00098 if(j<(NMEA_BUFFERSIZE-1))
00099 NmeaPacket[j] = bufferGetFromFront(rxBuffer);
00100 else
00101 bufferGetFromFront(rxBuffer);
00102 }
00103
00104 NmeaPacket[j] = 0;
00105
00106 bufferGetFromFront(rxBuffer);
00107 bufferGetFromFront(rxBuffer);
00108
00109 #ifdef NMEA_DEBUG_PKT
00110 rprintf("Rx NMEA packet type: ");
00111 rprintfStrLen(NmeaPacket, 0, 5);
00112 rprintfStrLen(NmeaPacket, 5, (i-1)-5);
00113 rprintfCRLF();
00114 #endif
00115
00116
00117 foundpacket = NMEA_UNKNOWN;
00118 break;
00119 }
00120 }
00121 }
00122
00123 if(foundpacket)
00124 {
00125
00126 if(!strncmp(NmeaPacket, "GPGGA", 5))
00127 {
00128
00129 nmeaProcessGPGGA(NmeaPacket);
00130
00131 foundpacket = NMEA_GPGGA;
00132 }
00133 else if(!strncmp(NmeaPacket, "GPVTG", 5))
00134 {
00135
00136 nmeaProcessGPVTG(NmeaPacket);
00137
00138 foundpacket = NMEA_GPVTG;
00139 }
00140 }
00141 else if(rxBuffer->datalength >= rxBuffer->size)
00142 {
00143
00144
00145 bufferFlush(rxBuffer);
00146 }
00147 return foundpacket;
00148 }
00149
00150 void nmeaProcessGPGGA(u08* packet)
00151 {
00152 u08 i;
00153 char* endptr;
00154 double degrees, minutesfrac;
00155
00156 #ifdef NMEA_DEBUG_GGA
00157 rprintf("NMEA: ");
00158 rprintfStr(packet);
00159 rprintfCRLF();
00160 #endif
00161
00162
00163 i = 6;
00164
00165 if(packet[i]==',' && packet[i+1]==',')
00166 return;
00167
00168
00169 GpsInfo.PosLLA.TimeOfFix.f = strtod(&packet[i], &endptr);
00170 while(packet[i++] != ',');
00171
00172
00173 GpsInfo.PosLLA.lat.f = strtod(&packet[i], &endptr);
00174
00175 minutesfrac = modf(GpsInfo.PosLLA.lat.f/100, °rees);
00176 GpsInfo.PosLLA.lat.f = degrees + (minutesfrac*100)/60;
00177
00178 GpsInfo.PosLLA.lat.f *= (M_PI/180);
00179 while(packet[i++] != ',');
00180
00181
00182 if(packet[i] == 'S') GpsInfo.PosLLA.lat.f = -GpsInfo.PosLLA.lat.f;
00183 while(packet[i++] != ',');
00184
00185
00186 GpsInfo.PosLLA.lon.f = strtod(&packet[i], &endptr);
00187
00188 minutesfrac = modf(GpsInfo.PosLLA.lon.f/100, °rees);
00189 GpsInfo.PosLLA.lon.f = degrees + (minutesfrac*100)/60;
00190
00191 GpsInfo.PosLLA.lon.f *= (M_PI/180);
00192 while(packet[i++] != ',');
00193
00194
00195 if(packet[i] == 'W') GpsInfo.PosLLA.lon.f = -GpsInfo.PosLLA.lon.f;
00196 while(packet[i++] != ',');
00197
00198
00199
00200
00201 if( (packet[i] != '0') && (packet[i] != ',') )
00202 GpsInfo.PosLLA.updates++;
00203 while(packet[i++] != ',');
00204
00205
00206 GpsInfo.numSVs = atoi(&packet[i]);
00207 while(packet[i++] != ',');
00208 while(packet[i++] != ',');
00209
00210
00211 GpsInfo.PosLLA.alt.f = strtod(&packet[i], &endptr);
00212
00213 while(packet[i++] != ',');
00214 while(packet[i++] != ',');
00215 while(packet[i++] != ',');
00216 while(packet[i++] != ',');
00217 while(packet[i++] != ',');
00218 while(packet[i++] != '*');
00219 }
00220
00221 void nmeaProcessGPVTG(u08* packet)
00222 {
00223 u08 i;
00224 char* endptr;
00225
00226 #ifdef NMEA_DEBUG_VTG
00227 rprintf("NMEA: ");
00228 rprintfStr(packet);
00229 rprintfCRLF();
00230 #endif
00231
00232
00233 i = 6;
00234
00235 if(packet[i]==',' && packet[i+1]==',')
00236 return;
00237
00238
00239 GpsInfo.VelHS.heading.f = strtod(&packet[i], &endptr);
00240 while(packet[i++] != ',');
00241 while(packet[i++] != ',');
00242
00243
00244
00245 while(packet[i++] != ',');
00246 while(packet[i++] != ',');
00247
00248
00249
00250 while(packet[i++] != ',');
00251 while(packet[i++] != ',');
00252
00253
00254 GpsInfo.VelHS.speed.f = strtod(&packet[i], &endptr);
00255 while(packet[i++] != ',');
00256 while(packet[i++] != '*');
00257
00258 GpsInfo.VelHS.updates++;
00259 }
00260