00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040 #include "GPS_NMEA.h"
00041
00042 #include <avr/interrupt.h>
00043 #include "WProgram.h"
00044
00045
00046
00047 GPS_NMEA_Class::GPS_NMEA_Class()
00048 {
00049 }
00050
00051
00052 void GPS_NMEA_Class::Init(void)
00053 {
00054 Type = 2;
00055 GPS_checksum_calc = false;
00056 bufferidx = 0;
00057 NewData=0;
00058 Fix=0;
00059 Quality=0;
00060 PrintErrors=0;
00061
00062 #if defined(__AVR_ATmega1280__)|| defined(__AVR_ATmega2560__)
00063 Serial1.begin(38400);
00064 #else
00065 Serial.begin(38400);
00066 #endif
00067 }
00068
00069
00070
00071
00072 void GPS_NMEA_Class::Read(void)
00073 {
00074 char c;
00075 int numc;
00076 int i;
00077
00078
00079 #if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) // If AtMega1280/2560 then Serial port 1...
00080 numc = Serial1.available();
00081 #else
00082 numc = Serial.available();
00083 #endif
00084 if (numc > 0)
00085 for (i=0;i<numc;i++){
00086 #if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) // If AtMega1280/2560 then Serial port 1...
00087 c = Serial1.read();
00088 #else
00089 c = Serial.read();
00090 #endif
00091 if (c == '$'){
00092 bufferidx = 0;
00093 buffer[bufferidx++] = c;
00094 GPS_checksum = 0;
00095 GPS_checksum_calc = true;
00096 continue;
00097 }
00098 if (c == '\r'){
00099 buffer[bufferidx++] = 0;
00100 parse_nmea_gps();
00101 }
00102 else {
00103 if (bufferidx < (GPS_BUFFERSIZE-1)){
00104 if (c == '*')
00105 GPS_checksum_calc = false;
00106 buffer[bufferidx++] = c;
00107 if (GPS_checksum_calc)
00108 GPS_checksum ^= c;
00109 }
00110 else
00111 bufferidx=0;
00112 }
00113 }
00114 }
00115
00116
00117
00118
00119
00120 void GPS_NMEA_Class::parse_nmea_gps(void)
00121 {
00122 byte NMEA_check;
00123 long aux_deg;
00124 long aux_min;
00125 char *parseptr;
00126
00127
00128 if (strncmp(buffer,"$GPGGA",6)==0){
00129 if (buffer[bufferidx-4]=='*'){
00130 NMEA_check = parseHex(buffer[bufferidx-3])*16 + parseHex(buffer[bufferidx-2]);
00131 if (GPS_checksum == NMEA_check){
00132
00133 NewData = 1;
00134 parseptr = strchr(buffer, ',')+1;
00135
00136 Time = parsenumber(parseptr,2);
00137 parseptr = strchr(parseptr, ',')+1;
00138
00139 aux_deg = parsedecimal(parseptr,2);
00140 aux_min = parsenumber(parseptr+2,4);
00141 Lattitude = aux_deg*10000000 + (aux_min*50)/3;
00142 parseptr = strchr(parseptr, ',')+1;
00143
00144 if (*parseptr=='S')
00145 Lattitude = -1*Lattitude;
00146
00147 parseptr = strchr(parseptr, ',')+1;
00148
00149 aux_deg = parsedecimal(parseptr,3);
00150 aux_min = parsenumber(parseptr+3,4);
00151 Longitude = aux_deg*10000000 + (aux_min*50)/3;
00152
00153 parseptr = strchr(parseptr, ',')+1;
00154
00155 if (*parseptr=='W')
00156 Longitude = -1*Longitude;
00157
00158 parseptr = strchr(parseptr, ',')+1;
00159 Fix = parsedecimal(parseptr,1);
00160 parseptr = strchr(parseptr, ',')+1;
00161 NumSats = parsedecimal(parseptr,2);
00162 parseptr = strchr(parseptr, ',')+1;
00163 HDOP = parsenumber(parseptr,1);
00164 parseptr = strchr(parseptr, ',')+1;
00165 Altitude = parsenumber(parseptr,1)*100;
00166 if (Fix < 1)
00167 Quality = 0;
00168 else if(NumSats<5)
00169 Quality = 1;
00170 else if(HDOP>30)
00171 Quality = 2;
00172 else if(HDOP>25)
00173 Quality = 3;
00174 else
00175 Quality = 4;
00176 }
00177 else
00178 {
00179 if (PrintErrors)
00180 Serial.println("GPSERR: Checksum error!!");
00181 }
00182 }
00183 }
00184 else if (strncmp(buffer,"$GPVTG",6)==0){
00185
00186 if (buffer[bufferidx-4]=='*'){
00187 NMEA_check = parseHex(buffer[bufferidx-3])*16 + parseHex(buffer[bufferidx-2]);
00188 if (GPS_checksum == NMEA_check){
00189 parseptr = strchr(buffer, ',')+1;
00190 Ground_Course = parsenumber(parseptr,2);
00191 parseptr = strchr(parseptr, ',')+1;
00192 parseptr = strchr(parseptr, ',')+1;
00193 parseptr = strchr(parseptr, ',')+1;
00194 parseptr = strchr(parseptr, ',')+1;
00195 parseptr = strchr(parseptr, ',')+1;
00196 parseptr = strchr(parseptr, ',')+1;
00197 Ground_Speed = parsenumber(parseptr,2)*10/36;
00198
00199 }
00200 else
00201 {
00202 if (PrintErrors)
00203 Serial.println("GPSERR: Checksum error!!");
00204 }
00205 }
00206 }
00207 else
00208 {
00209 bufferidx = 0;
00210 if (PrintErrors)
00211 Serial.println("GPSERR: Bad sentence!!");
00212 }
00213 }
00214
00215
00216
00217
00218
00219
00220 byte GPS_NMEA_Class::parseHex(char c) {
00221 if (c < '0')
00222 return (0);
00223 if (c <= '9')
00224 return (c - '0');
00225 if (c < 'A')
00226 return (0);
00227 if (c <= 'F')
00228 return ((c - 'A')+10);
00229 }
00230
00231
00232 long GPS_NMEA_Class::parsedecimal(char *str,byte num_car) {
00233 long d = 0;
00234 byte i;
00235
00236 i = num_car;
00237 while ((str[0] != 0)&&(i>0)) {
00238 if ((str[0] > '9') || (str[0] < '0'))
00239 return d;
00240 d *= 10;
00241 d += str[0] - '0';
00242 str++;
00243 i--;
00244 }
00245 return d;
00246 }
00247
00248
00249 long GPS_NMEA_Class::parsenumber(char *str,byte numdec) {
00250 long d = 0;
00251 byte ndec = 0;
00252
00253 while (str[0] != 0) {
00254 if (str[0] == '.'){
00255 ndec = 1;
00256 }
00257 else {
00258 if ((str[0] > '9') || (str[0] < '0'))
00259 return d;
00260 d *= 10;
00261 d += str[0] - '0';
00262 if (ndec > 0)
00263 ndec++;
00264 if (ndec > numdec)
00265 return d;
00266 }
00267 str++;
00268 }
00269 return d;
00270 }
00271
00272 GPS_NMEA_Class GPS;