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 #include <string.h>
00028 #include <stdlib.h>
00029 #endif
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 #define BUFFERSIZE 80
00043 u08 NmeaPacket[BUFFERSIZE];
00044 u08 debug;
00045
00046 void nmeaInit(void)
00047 {
00048 debug = 0;
00049 }
00050
00051 u08 nmeaProcess(cBuffer* rxBuffer)
00052 {
00053 u08 foundpacket = FALSE;
00054 u08 startFlag = FALSE;
00055
00056 u08 i,j;
00057
00058
00059
00060 while(rxBuffer->datalength)
00061 {
00062
00063 if(bufferGetAtIndex(rxBuffer,0) == '$')
00064 {
00065
00066 startFlag = TRUE;
00067
00068 break;
00069 }
00070 else
00071 bufferGetFromFront(rxBuffer);
00072 }
00073
00074
00075 if(startFlag)
00076 {
00077 for(i=1; i<(rxBuffer->datalength)-1; i++)
00078 {
00079
00080 if((bufferGetAtIndex(rxBuffer,i) == '\r') && (bufferGetAtIndex(rxBuffer,i+1) == '\n'))
00081 {
00082
00083
00084 bufferGetFromFront(rxBuffer);
00085
00086 for(j=0; j<(i-1); j++)
00087 NmeaPacket[j] = bufferGetFromFront(rxBuffer);
00088
00089 NmeaPacket[j] = 0;
00090
00091 bufferGetFromFront(rxBuffer);
00092 bufferGetFromFront(rxBuffer);
00093
00094 if(debug)
00095 {
00096 rprintf("Rx NMEA packet type: ");
00097 rprintfStrLen(NmeaPacket, 0, 5);
00098 rprintfStrLen(NmeaPacket, 5, (i-1)-5);
00099 rprintfCRLF();
00100 }
00101
00102
00103 foundpacket = TRUE;
00104 break;
00105 }
00106 }
00107 }
00108
00109 if(foundpacket)
00110 {
00111
00112 if(!strncmp(NmeaPacket, "GPGGA", 5))
00113 nmeaProcessGPGGA(NmeaPacket);
00114 else if(!strncmp(NmeaPacket, "GPVTG", 5))
00115 nmeaProcessGPVTG(NmeaPacket);
00116 }
00117 return foundpacket;
00118 }
00119
00120 void nmeaProcessGPGGA(u08* packet)
00121 {
00122 u08 i;
00123 char* endptr;
00124
00125 if(debug)
00126 {
00127 rprintf("NMEA: ");
00128 rprintfStr(packet);
00129 rprintfCRLF();
00130 }
00131
00132
00133 i = 6;
00134
00135
00136 GpsInfo.PosLLA.TimeOfFix.f = strtod(&packet[i], &endptr);
00137 while(packet[i++] != ',');
00138
00139
00140 GpsInfo.PosLLA.lat.f = strtod(&packet[i], &endptr);
00141 while(packet[i++] != ',');
00142
00143
00144 if(packet[i] == 'S') GpsInfo.PosLLA.lat.f = -GpsInfo.PosLLA.lat.f;
00145 while(packet[i++] != ',');
00146
00147
00148 GpsInfo.PosLLA.lon.f = strtod(&packet[i], &endptr);
00149 while(packet[i++] != ',');
00150
00151
00152 if(packet[i] == 'W') GpsInfo.PosLLA.lon.f = -GpsInfo.PosLLA.lon.f;
00153 while(packet[i++] != ',');
00154
00155
00156
00157
00158 if( (packet[i] != '0') && (packet[i] != ',') )
00159 GpsInfo.PosLLA.updates++;
00160 while(packet[i++] != ',');
00161
00162
00163 GpsInfo.numSVs = atoi(&packet[i]);
00164 while(packet[i++] != ',');
00165 while(packet[i++] != ',');
00166
00167
00168 GpsInfo.PosLLA.alt.f = strtod(&packet[i], &endptr);
00169
00170 while(packet[i++] != ',');
00171 while(packet[i++] != ',');
00172 while(packet[i++] != ',');
00173 while(packet[i++] != ',');
00174 while(packet[i++] != ',');
00175 while(packet[i++] != '*');
00176 }
00177
00178 void nmeaProcessGPVTG(u08* packet)
00179 {
00180 u08 i;
00181 char* endptr;
00182
00183 if(debug)
00184 {
00185 rprintf("NMEA: ");
00186 rprintfStr(packet);
00187 rprintfCRLF();
00188 }
00189
00190
00191 i = 6;
00192
00193 GpsInfo.VelHS.heading.f = strtod(&packet[i], &endptr);
00194 while(packet[i++] != ',');
00195 while(packet[i++] != ',');
00196
00197
00198
00199 while(packet[i++] != ',');
00200 while(packet[i++] != ',');
00201
00202
00203
00204 while(packet[i++] != ',');
00205 while(packet[i++] != ',');
00206
00207
00208 GpsInfo.VelHS.speed.f = strtod(&packet[i], &endptr);
00209 while(packet[i++] != ',');
00210 while(packet[i++] != '*');
00211
00212 GpsInfo.VelHS.updates++;
00213 }
00214