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