BeRTOS
|
00001 00053 #include "nmea.h" 00054 00055 #include "cfg/cfg_nmea.h" 00056 00057 #include <cfg/debug.h> 00058 00059 #define LOG_LEVEL NMEA_LOG_LEVEL 00060 #define LOG_FORMAT NMEA_LOG_FORMAT 00061 #include <cfg/log.h> 00062 00063 #include <net/nmeap/inc/nmeap.h> 00064 00065 #include <ctype.h> 00066 #include <time.h> 00067 #include <string.h> 00068 #include <stdlib.h> 00069 00070 /* 00071 * Make conversion from one string to int. 00072 * 00073 * You can specify the precision if the string is a float 00074 * number. The result is an int multiplied to 10^precision. 00075 */ 00076 static uint32_t tokenToInt(const char *s, int precision) 00077 { 00078 uint32_t num = 0; 00079 bool sep_found = false; 00080 int i; 00081 00082 if (!s) 00083 return 0; 00084 00085 for(i = 0; i < NMEAP_MAX_SENTENCE_LENGTH; i++) 00086 { 00087 unsigned char c = *s++; 00088 00089 if (c == '.') 00090 { 00091 sep_found = true; 00092 continue; 00093 } 00094 00095 if (c == '\0' || !isdigit(c) || (precision == 0 && sep_found)) 00096 break; 00097 00098 if (sep_found) 00099 precision--; 00100 00101 num *= 10; 00102 num += c - '0'; 00103 } 00104 00105 while (precision--) 00106 num *= 10; 00107 00108 return num; 00109 } 00110 00111 /* 00112 * Convert a string to micro degree. 00113 */ 00114 static udegree_t convertToDegree(const char *str) 00115 { 00116 uint32_t dec; 00117 uint32_t deg; 00118 uint32_t min; 00119 00120 if (*str == 0) 00121 return 0; 00122 00123 dec = tokenToInt(str, 4); 00124 deg = dec / 1000000; 00125 min = dec - deg * 1000000; 00126 dec = deg * 1000000 + ((min * 5) + 1) / 3; 00127 00128 return dec; 00129 } 00130 00131 /* 00132 * Retun latitude in micro degree from a string. 00133 */ 00134 static udegree_t nmea_latitude(const char *plat, const char *phem) 00135 { 00136 int ns; 00137 00138 if (*phem == 0) 00139 return 0; 00140 00141 /* north lat is +, south lat is - */ 00142 ns = (*phem == 'N') ? 1 : -1; 00143 00144 00145 return ns * convertToDegree(plat); 00146 } 00147 00148 /* 00149 * Retun longitude in micro degree from a string. 00150 */ 00151 static udegree_t nmea_longitude(const char *plot, const char *phem) 00152 { 00153 int ew; 00154 00155 if (*phem == 0) 00156 return 0; 00157 00158 /* west long is negative, east long is positive */ 00159 ew = (*phem == 'E') ? 1 : -1; 00160 00161 return ew * convertToDegree(plot); 00162 } 00163 00164 /* 00165 * Return altitude in meter from a string. 00166 * 00167 */ 00168 static int32_t nmea_altitude(const char *palt, const char *punits) 00169 { 00170 int32_t alt; 00171 00172 if (*palt == 0) 00173 return 0; 00174 00175 alt = atoi(palt); 00176 00177 if (*punits == 'F') 00178 { 00179 /* convert to feet */ 00180 /* alt = alt * 3.2808399 */ 00181 alt = alt * 3 + /* 3.0 */ 00182 (alt >> 2) + /* 0.25 */ 00183 (alt >> 6) + /* 0.015625 */ 00184 (alt >> 7) + /* 0.0078125 */ 00185 (alt >> 8); /* 0,00390625 */ 00186 00187 } 00188 00189 return alt; 00190 } 00191 00192 /* 00193 * Convert time and date stamp string to unix time. 00194 */ 00195 static time_t timestampToSec(uint32_t time_stamp, uint32_t date_stamp) 00196 { 00197 struct tm t; 00198 uint16_t msec; 00199 uint16_t tmr[3]; 00200 uint16_t date[3]; 00201 00202 memset(&t, 0, sizeof(t)); 00203 memset(&tmr, 0, sizeof(tmr)); 00204 memset(&date, 0, sizeof(date)); 00205 00206 LOG_INFO("time_s[%lu],date[%lu]\n", (long)time_stamp, (long)date_stamp); 00207 uint32_t res = time_stamp / 1000; 00208 uint32_t all = time_stamp; 00209 msec = all - res * 1000; 00210 00211 for (int i = 0; i < 3; i++) 00212 { 00213 all = res; 00214 res = all / 100; 00215 tmr[i] = all - res * 100; 00216 LOG_INFO("t[%d]%d\n", tmr[i],i); 00217 } 00218 00219 t.tm_sec = tmr[0] + (ROUND_UP(msec, 1000) / 1000); 00220 t.tm_min = tmr[1]; 00221 t.tm_hour = tmr[2]; 00222 //If we do not have refence data, we set 1/1/1970 as default 00223 t.tm_mday = 1; 00224 t.tm_mon = 0; 00225 t.tm_year = 70; 00226 00227 if (date_stamp) 00228 { 00229 res = all = date_stamp; 00230 for (int i = 0; i < 3; i++) 00231 { 00232 all = res; 00233 res = all / 100; 00234 date[i] = all - res * 100; 00235 LOG_INFO("d[%d]%d\n", date[i],i); 00236 } 00237 t.tm_mday = date[2]; 00238 t.tm_mon = date[1] - 1; // time struct count month from 0 to 11; 00239 // we should specify the number of years from 1900, but the year field 00240 // is only two digits, so we add 100 (2000 - 1900).. 00241 t.tm_year = date[0] + 100; 00242 } 00243 LOG_INFO("times=%d,%d,%d,%d,%d,%d\n",t.tm_sec, t.tm_min, t.tm_hour, t.tm_year, t.tm_mon, t.tm_mday); 00244 00245 return mktime(&t); 00246 } 00247 00251 void gpgga_callout(nmeap_context_t *context, void *data, void *user_data) 00252 { 00253 (void)context; 00254 (void)user_data; 00255 (void)data; 00256 LOG_INFOB( 00257 NmeaGga *gga = (NmeaGga *)data; 00258 LOG_INFO("Found GPGGA message %ld %ld %d %lu %d %d %d %d\n", 00259 (long)gga->latitude, 00260 (long)gga->longitude, 00261 gga->altitude, 00262 gga->time, 00263 gga->satellites, 00264 gga->quality, 00265 gga->hdop, 00266 gga->geoid); 00267 ); 00268 } 00269 00273 void gprmc_callout(nmeap_context_t *context, void *data, void *user_data) 00274 { 00275 (void)context; 00276 (void)user_data; 00277 (void)data; 00278 LOG_INFOB( 00279 NmeaRmc *rmc = (NmeaRmc *)data; 00280 00281 LOG_INFO("Found GPRMC message %lu %c %ld %ld %d %d %d\n", 00282 rmc->time, 00283 rmc->warn, 00284 (long)rmc->latitude, 00285 (long)rmc->longitude, 00286 rmc->speed, 00287 rmc->course, 00288 rmc->mag_var); 00289 ); 00290 } 00291 00295 void gpgsv_callout(nmeap_context_t *context, void *data, void *user_data) 00296 { 00297 (void)context; 00298 (void)user_data; 00299 (void)data; 00300 LOG_INFOB( 00301 NmeaGsv *gsv = (NmeaGsv *)data; 00302 00303 LOG_INFO("Found GPGSV message %d %d %d\n", gsv->tot_message, gsv->message_num, gsv->tot_svv); 00304 00305 for (int i = 0; i < 4; i++) 00306 LOG_INFO("%d %d %d %d\n", gsv->info[i].sv_prn, gsv->info[i].elevation, gsv->info[i].azimut, gsv->info[i].snr); 00307 ); 00308 } 00309 00313 void gpvtg_callout(nmeap_context_t *context, void *data, void *user_data) 00314 { 00315 (void)context; 00316 (void)user_data; 00317 (void)data; 00318 LOG_INFOB( 00319 NmeaVtg *vtg = (NmeaVtg *)data; 00320 LOG_INFO("Found GPVTG message %d %d %d\n", vtg->track_good, vtg->knot_speed, vtg->km_speed); 00321 ); 00322 } 00323 00324 00325 00329 int nmea_gpgga(nmeap_context_t *context, nmeap_sentence_t *sentence) 00330 { 00331 /* 00332 * get pointer to sentence data 00333 */ 00334 NmeaGga *gga = (NmeaGga *)sentence->data; 00335 00336 ASSERT(gga); 00337 ASSERT(context->tokens >= 12); 00338 00339 gga->latitude = nmea_latitude(context->token[2],context->token[3]); 00340 gga->longitude = nmea_longitude(context->token[4],context->token[5]); 00341 gga->altitude = nmea_altitude(context->token[9],context->token[10]); 00342 gga->time = timestampToSec(tokenToInt(context->token[1], 3), 0); 00343 gga->satellites = atoi(context->token[7]); 00344 gga->quality = atoi(context->token[6]); 00345 gga->hdop = tokenToInt(context->token[8], 1); 00346 gga->geoid = nmea_altitude(context->token[11],context->token[12]); 00347 00348 /* 00349 * if the sentence has a callout, call it 00350 */ 00351 00352 if (sentence->callout != 0) 00353 (*sentence->callout)(context, gga, context->user_data); 00354 00355 return NMEA_GPGGA; 00356 } 00357 00361 int nmea_gprmc(nmeap_context_t *context, nmeap_sentence_t *sentence) 00362 { 00363 00364 /* 00365 * get pointer to sentence data 00366 */ 00367 NmeaRmc *rmc = (NmeaRmc *)sentence->data; 00368 00369 ASSERT(rmc); 00370 ASSERT(context->tokens >= 10); 00371 00372 /* 00373 * extract data from the tokens 00374 */ 00375 rmc->time = timestampToSec(tokenToInt(context->token[1], 3), tokenToInt(context->token[9], 0)); 00376 rmc->warn = *context->token[2]; 00377 rmc->latitude = nmea_latitude(context->token[3],context->token[4]); 00378 rmc->longitude = nmea_longitude(context->token[5],context->token[6]); 00379 rmc->speed = atoi(context->token[7]); 00380 rmc->course = atoi(context->token[8]); 00381 rmc->mag_var = atoi(context->token[10]); 00382 00383 if (sentence->callout != 0) 00384 (*sentence->callout)(context, rmc, context->user_data); 00385 00386 return NMEA_GPRMC; 00387 } 00388 00389 00393 int nmea_gpvtg(nmeap_context_t *context, nmeap_sentence_t *sentence) 00394 { 00395 00396 /* 00397 * get pointer to sentence data 00398 */ 00399 NmeaVtg *vtg = (NmeaVtg *)sentence->data; 00400 00401 ASSERT(vtg); 00402 ASSERT(context->tokens >= 7); 00403 00404 /* 00405 * extract data from the tokens 00406 */ 00407 vtg->track_good = atoi(context->token[1]); 00408 vtg->knot_speed = atoi(context->token[5]); 00409 vtg->km_speed = atoi(context->token[7]); 00410 00411 /* 00412 * if the sentence has a callout, call it 00413 */ 00414 if (sentence->callout != 0) 00415 (*sentence->callout)(context, vtg, context->user_data); 00416 00417 return NMEA_GPVTG; 00418 } 00419 00423 int nmea_gpgsv(nmeap_context_t *context, nmeap_sentence_t *sentence) 00424 { 00425 /* 00426 * get pointer to sentence data 00427 */ 00428 NmeaGsv *gsv = (NmeaGsv *)sentence->data; 00429 00430 00431 /* 00432 * extract data from the tokens 00433 */ 00434 gsv->tot_message = atoi(context->token[1]); 00435 gsv->message_num = atoi(context->token[2]); 00436 gsv->tot_svv = atoi(context->token[3]); 00437 00438 // Fill remaning member until we have token 00439 int j = 0; 00440 for (int i = 4; i < context->tokens - 3; i += 4, j++) 00441 { 00442 00443 gsv->info[j].sv_prn = atoi(context->token[i]); 00444 gsv->info[j].elevation = atoi(context->token[i + 1]); 00445 gsv->info[j].azimut = atoi(context->token[i + 2]); 00446 gsv->info[j].snr = atoi(context->token[i + 3]); 00447 } 00448 00449 /* 00450 * if the sentence has a callout, call it 00451 */ 00452 if (sentence->callout != 0) 00453 (*sentence->callout)(context, gsv, context->user_data); 00454 00455 return NMEA_GPGSV; 00456 } 00457 00458 00462 void nmea_poll(nmeap_context_t *context, KFile *channel) 00463 { 00464 int c, e; 00465 while ((c = kfile_getc(channel)) != EOF) 00466 nmeap_parse(context, c); 00467 00468 if ((e = kfile_error(channel))) 00469 { 00470 LOG_ERR("ch error [%0X]\n", e); 00471 kfile_clearerr(channel); 00472 } 00473 } 00474