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
00041 #include "AP_GPS_NMEA.h"
00042
00043
00044 AP_GPS_NMEA::AP_GPS_NMEA(Stream *s) : GPS(s)
00045 {
00046 }
00047
00048
00049 void
00050 AP_GPS_NMEA::init(void)
00051 {
00052
00053
00054 _port->print(NMEA_OUTPUT_SENTENCES);
00055 _port->print(NMEA_OUTPUT_4HZ);
00056 _port->print(SBAS_ON);
00057 _port->print(DGPS_SBAS);
00058 _port->print(DATUM_GOOGLE);
00059 }
00060
00061
00062
00063
00064 bool
00065 AP_GPS_NMEA::read(void)
00066 {
00067 char c;
00068 int numc;
00069 int i;
00070 bool parsed = false;
00071
00072 numc = _port->available();
00073
00074 if (numc > 0){
00075 for (i = 0; i < numc; i++){
00076 c = _port->read();
00077 if (c == '$'){
00078 bufferidx = 0;
00079 buffer[bufferidx++] = c;
00080 GPS_checksum = 0;
00081 GPS_checksum_calc = true;
00082 continue;
00083 }
00084 if (c == '\r'){
00085 buffer[bufferidx++] = 0;
00086 parsed = parse_nmea_gps();
00087 } else {
00088 if (bufferidx < (GPS_BUFFERSIZE - 1)){
00089 if (c == '*')
00090 GPS_checksum_calc = false;
00091 buffer[bufferidx++] = c;
00092 if (GPS_checksum_calc){
00093 GPS_checksum ^= c;
00094 }
00095 } else {
00096 bufferidx = 0;
00097 }
00098 }
00099 }
00100 }
00101 }
00102
00103
00104
00105
00106
00107 bool
00108 AP_GPS_NMEA::parse_nmea_gps(void)
00109 {
00110 uint8_t NMEA_check;
00111 long aux_deg;
00112 long aux_min;
00113 char *parseptr;
00114
00115 if (strncmp(buffer,"$GPGGA",6)==0){
00116 if (buffer[bufferidx-4]=='*'){
00117 NMEA_check = parseHex(buffer[bufferidx - 3]) * 16 + parseHex(buffer[bufferidx - 2]);
00118 if (GPS_checksum == NMEA_check){
00119
00120 parseptr = strchr(buffer, ',')+1;
00121
00122 time = parsenumber(parseptr, 2);
00123 parseptr = strchr(parseptr, ',')+1;
00124 aux_deg = parsedecimal(parseptr, 2);
00125 aux_min = parsenumber(parseptr + 2, 4);
00126 latitude = aux_deg * 10000000 + (aux_min * 50) / 3;
00127 parseptr = strchr(parseptr, ',')+1;
00128 if ( * parseptr == 'S')
00129 latitude = -1 * latitude;
00130 parseptr = strchr(parseptr, ',')+1;
00131
00132 aux_deg = parsedecimal(parseptr, 3);
00133 aux_min = parsenumber(parseptr + 3, 4);
00134 longitude = aux_deg * 10000000 + (aux_min * 50) / 3;
00135
00136 parseptr = strchr(parseptr, ',')+1;
00137 if ( * parseptr == 'W')
00138 longitude = -1 * longitude;
00139 parseptr = strchr(parseptr, ',')+1;
00140 fix = parsedecimal(parseptr, 1);
00141 parseptr = strchr(parseptr, ',')+1;
00142 num_sats = parsedecimal(parseptr, 2);
00143 parseptr = strchr(parseptr, ',')+1;
00144 hdop = parsenumber(parseptr, 1);
00145 parseptr = strchr(parseptr, ',')+1;
00146 altitude = parsenumber(parseptr, 1) * 100;
00147 if (fix < 1)
00148 quality = 0;
00149 else if(num_sats < 5)
00150 quality = 1;
00151 else if(hdop > 30)
00152 quality = 2;
00153 else if(hdop > 25)
00154 quality = 3;
00155 else
00156 quality = 4;
00157 } else {
00158 _error("GPSERR: Checksum error!!\n");
00159 return false;
00160 }
00161 }
00162 } else if (strncmp(buffer,"$GPVTG",6)==0){
00163
00164 if (buffer[bufferidx-4]=='*'){
00165 NMEA_check = parseHex(buffer[bufferidx - 3]) * 16 + parseHex(buffer[bufferidx - 2]);
00166 if (GPS_checksum == NMEA_check){
00167 parseptr = strchr(buffer, ',')+1;
00168 ground_course = parsenumber(parseptr, 1) * 10;
00169 parseptr = strchr(parseptr, ',')+1;
00170 parseptr = strchr(parseptr, ',')+1;
00171 parseptr = strchr(parseptr, ',')+1;
00172 parseptr = strchr(parseptr, ',')+1;
00173 parseptr = strchr(parseptr, ',')+1;
00174 parseptr = strchr(parseptr, ',')+1;
00175 ground_speed = parsenumber(parseptr, 1) * 100 / 36;
00176
00177 } else {
00178 _error("GPSERR: Checksum error!!\n");
00179 return false;
00180 }
00181 }
00182 } else {
00183 bufferidx = 0;
00184 _error("GPSERR: Bad sentence!!\n");
00185 return false;
00186 }
00187 return true;
00188 }
00189
00190
00191
00192 uint8_t
00193 AP_GPS_NMEA::parseHex(char c) {
00194 if (c < '0')
00195 return (0);
00196 if (c <= '9')
00197 return (c - '0');
00198 if (c < 'A')
00199 return (0);
00200 if (c <= 'F')
00201 return ((c - 'A')+10);
00202 }
00203
00204
00205 long
00206 AP_GPS_NMEA::parsedecimal(char *str, uint8_t num_car) {
00207 long d = 0;
00208 uint8_t i;
00209
00210 i = num_car;
00211 while ((str[0] != 0) && (i > 0)) {
00212 if ((str[0] > '9') || (str[0] < '0'))
00213 return d;
00214 d *= 10;
00215 d += str[0] - '0';
00216 str++;
00217 i--;
00218 }
00219 return d;
00220 }
00221
00222
00223 long
00224 AP_GPS_NMEA::parsenumber(char *str, uint8_t numdec) {
00225 long d = 0;
00226 uint8_t ndec = 0;
00227
00228 while (str[0] != 0) {
00229 if (str[0] == '.'){
00230 ndec = 1;
00231 } else {
00232 if ((str[0] > '9') || (str[0] < '0'))
00233 return d;
00234 d *= 10;
00235 d += str[0] - '0';
00236 if (ndec > 0)
00237 ndec++;
00238 if (ndec > numdec)
00239 return d;
00240 }
00241 str++;
00242 }
00243 return d;
00244 }