You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

nmea.cc 39KB

12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970717273747576777879808182838485868788899091929394959697989910010110210310410510610710810911011111211311411511611711811912012112212312412512612712812913013113213313413513613713813914014114214314414514614714814915015115215315415515615715815916016116216316416516616716816917017117217317417517617717817918018118218318418518618718818919019119219319419519619719819920020120220320420520620720820921021121221321421521621721821922022122222322422522622722822923023123223323423523623723823924024124224324424524624724824925025125225325425525625725825926026126226326426526626726826927027127227327427527627727827928028128228328428528628728828929029129229329429529629729829930030130230330430530630730830931031131231331431531631731831932032132232332432532632732832933033133233333433533633733833934034134234334434534634734834935035135235335435535635735835936036136236336436536636736836937037137237337437537637737837938038138238338438538638738838939039139239339439539639739839940040140240340440540640740840941041141241341441541641741841942042142242342442542642742842943043143243343443543643743843944044144244344444544644744844945045145245345445545645745845946046146246346446546646746846947047147247347447547647747847948048148248348448548648748848949049149249349449549649749849950050150250350450550650750850951051151251351451551651751851952052152252352452552652752852953053153253353453553653753853954054154254354454554654754854955055155255355455555655755855956056156256356456556656756856957057157257357457557657757857958058158258358458558658758858959059159259359459559659759859960060160260360460560660760860961061161261361461561661761861962062162262362462562662762862963063163263363463563663763863964064164264364464564664764864965065165265365465565665765865966066166266366466566666766866967067167267367467567667767867968068168268368468568668768868969069169269369469569669769869970070170270370470570670770870971071171271371471571671771871972072172272372472572672772872973073173273373473573673773873974074174274374474574674774874975075175275375475575675775875976076176276376476576676776876977077177277377477577677777877978078178278378478578678778878979079179279379479579679779879980080180280380480580680780880981081181281381481581681781881982082182282382482582682782882983083183283383483583683783883984084184284384484584684784884985085185285385485585685785885986086186286386486586686786886987087187287387487587687787887988088188288388488588688788888989089189289389489589689789889990090190290390490590690790890991091191291391491591691791891992092192292392492592692792892993093193293393493593693793893994094194294394494594694794894995095195295395495595695795895996096196296396496596696796896997097197297397497597697797897998098198298398498598698798898999099199299399499599699799899910001001100210031004100510061007100810091010101110121013101410151016101710181019102010211022102310241025102610271028102910301031103210331034103510361037103810391040104110421043104410451046104710481049105010511052105310541055105610571058105910601061106210631064106510661067106810691070107110721073107410751076107710781079108010811082108310841085108610871088108910901091109210931094109510961097109810991100110111021103110411051106110711081109111011111112111311141115111611171118111911201121112211231124112511261127112811291130113111321133113411351136113711381139114011411142114311441145114611471148114911501151115211531154115511561157115811591160116111621163116411651166116711681169117011711172117311741175117611771178117911801181118211831184118511861187118811891190119111921193119411951196119711981199120012011202120312041205120612071208120912101211121212131214121512161217121812191220122112221223122412251226122712281229123012311232123312341235123612371238123912401241124212431244124512461247124812491250125112521253125412551256125712581259126012611262126312641265126612671268126912701271127212731274127512761277127812791280128112821283128412851286128712881289129012911292129312941295129612971298129913001301130213031304130513061307130813091310131113121313131413151316131713181319132013211322132313241325132613271328132913301331133213331334133513361337133813391340134113421343134413451346134713481349135013511352135313541355135613571358135913601361136213631364136513661367136813691370137113721373137413751376137713781379138013811382138313841385138613871388138913901391139213931394139513961397139813991400140114021403140414051406140714081409141014111412141314141415141614171418141914201421142214231424142514261427142814291430143114321433143414351436143714381439144014411442144314441445144614471448144914501451145214531454145514561457145814591460146114621463146414651466146714681469147014711472147314741475147614771478147914801481
  1. /*
  2. Read files containing selected NMEA 0183 sentences.
  3. Based on information by Eino Uikkanenj
  4. Copyright (C) 2004-2015 Robert Lipe, robertlipe+source@gpsbabel.org
  5. This program is free software; you can redistribute it and/or modify
  6. it under the terms of the GNU General Public License as published by
  7. the Free Software Foundation; either version 2 of the License, or
  8. (at your option) any later version.
  9. This program is distributed in the hope that it will be useful,
  10. but WITHOUT ANY WARRANTY; without even the implied warranty of
  11. MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  12. GNU General Public License for more details.
  13. You should have received a copy of the GNU General Public License
  14. along with this program; if not, write to the Free Software
  15. Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111 USA
  16. */
  17. #include "defs.h"
  18. #include "cet_util.h"
  19. #include "gbser.h"
  20. #include "strptime.h"
  21. #include "jeeps/gpsmath.h"
  22. #include <ctype.h>
  23. #include <math.h>
  24. #include <time.h>
  25. #include <stdlib.h>
  26. #include <stdio.h>
  27. #include <QtCore/QStringList>
  28. /**********************************************************
  29. ' 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15
  30. ' $GPGGA - Global Positioning System Fix Data
  31. ' $GPGGA,155537,6006.718,N,02426.290,E,1,05,2.4,50.5,M,19.7,M,,*79
  32. ' 2 123519 Fix taken at 12:35:19 UTC
  33. ' 3,4 4807.038,N Latitude 48 deg 07.038' N
  34. ' 5,6 01131.324,E Longitude 11 deg 31.324' E
  35. ' 7 1 Fix quality: 0 = invalid
  36. ' 1 = GPS fix
  37. ' 2 = DGPS fix
  38. ' 8 08 Number of satellites being tracked
  39. ' 9 0.9 Horizontal dilution of position
  40. ' 10,11 545.4,M Altitude, Metres, above mean sea level
  41. ' 12,13 46.9,M Height of geoid (mean sea level) above WGS84 ellipsoid
  42. ' 14 (empty field) time in seconds since last DGPS update
  43. ' 15 (empty field) DGPS station ID number
  44. ' $GPWPL - waypoint location
  45. ' $GPWPL,4917.16,N,12310.64,W,003*65
  46. ' 2,3 4917.16,N Latitude of waypoint
  47. ' 4,5 12310.64,W Longitude of waypoint
  48. ' 6 003 Waypoint ID
  49. ' $GPGLL - Geographic position, Latitude and Longitude
  50. ' $GPGLL,4916.45,N,12311.12,W,225444,A
  51. ' 2,3 4916.46,N Latitude 49 deg. 16.45 min. North
  52. ' 4,5 12311.12,W Longitude 123 deg. 11.12 min. West
  53. ' 6 225444 Fix taken at 22:54:44 UTC
  54. ' 7 A Data valid
  55. ' $GPRMC - Recommended minimum specific GNSS Data
  56. ' $GPRMC,085721.194,A,5917.7210,N,01103.9227,E,21.42,50.33,300504,,*07
  57. ' 2 085721 Fix taken at 08:57:21 UTC
  58. ' 3 A Fix valid (this field reads V if fix is not valid)
  59. ' 4,5 5917.7210,N Latitude 59 deg 17.7210' N
  60. ' 6,7 01103.9227,E Longitude 11 deg 03.9227' E
  61. ' 8 21.42 Speed over ground (knots)
  62. ' 9 50.33 Course over ground (true)
  63. ' 10 300504 Date 30/05-2004
  64. ' 11 Empty field Magnetic variation
  65. GSA - GPS DOP and active satellites
  66. $GPGSA,A,3,04,05,,09,12,,,24,,,,,2.5,1.3,2.1*39
  67. A Auto selection of 2D or 3D fix (M = manual)
  68. 3 3D fix
  69. 04,05... PRNs of satellites used for fix (space for 12)
  70. 2.5 PDOP (dilution of precision)
  71. 1.3 Horizontal dilution of precision (HDOP)
  72. 2.1 Vertical dilution of precision (VDOP)
  73. DOP is an indication of the effect of satellite geometry on
  74. the accuracy of the fix.
  75. VTG - Track made good and ground speed
  76. $GPVTG,054.7,T,034.4,M,005.5,N,010.2,K
  77. 054.7,T True track made good
  78. 034.4,M Magnetic track made good
  79. 005.5,N Ground speed, knots
  80. 010.2,K Ground speed, Kilometers per hour
  81. WPL - waypoint location
  82. $GPWPL,4917.16,N,12310.64,W,003*65
  83. 4917.16,N Latitude of waypoint
  84. 12310.64,W Longitude of waypoint
  85. 003 Waypoint ID
  86. When a route is active, this sentence is sent once for each
  87. waypoint in the route, in sequence. When all waypoints have
  88. been reported, GPR00 is sent in the next data set. In any
  89. group of sentences, only one WPL sentence, or an R00
  90. sentence, will be sent.
  91. ' The optional checksum field consists of a "*" and two hex digits repre-
  92. ' senting the exclusive OR of all characters between, but not including,
  93. ' the "$" and "*". A checksum is required on some sentences.
  94. ****************************************/
  95. /*
  96. * An input file may have both GGA and GLL and RMC sentences for the exact
  97. * same position fix. If we see a single GGA, start ignoring GLL's and RMC's.
  98. * GLL's will also be ignored if RMC's are found and GGA's not found.
  99. */
  100. /*
  101. Zmarties notes:
  102. In practice, all fields of the NMEA sentences should be treated as optional -
  103. if the data is not available, then the field can be omitted (hence leading
  104. to the output of two consecutive commas).
  105. An NMEA recording can start anywhere in the stream of data. It is therefore
  106. necessary to discard sentences until sufficient data has been processed to
  107. have all the necessary data to construct a waypoint. In practice, this means
  108. discarding data until we have had the first sentence that provides the date.
  109. (We could scan forwards in the stream of data to find the first date, and
  110. then back apply it to all previous sentences, but that is probably more
  111. complexity that is necessary - the lost of one waypoint at the start of the
  112. stream can normally be tolerated.)
  113. If a sentence is received without a checksum, but previous sentences have
  114. had checksums, it is best to discard that sentence. In practice, the only
  115. time I have seen this is when the recording stops suddenly, where the last
  116. sentence is truncated - and missing part of the line, including the checksum.
  117. */
  118. typedef enum {
  119. gp_unknown = 0,
  120. gpgga,
  121. gplgll,
  122. gprmc
  123. } preferred_posn_type;
  124. static enum {
  125. rm_unknown = 0,
  126. rm_serial,
  127. rm_file
  128. } read_mode;
  129. static gbfile* file_in, *file_out;
  130. static route_head* trk_head;
  131. static short_handle mkshort_handle;
  132. static preferred_posn_type posn_type;
  133. static struct tm tm;
  134. static Waypoint* curr_waypt;
  135. static Waypoint* last_waypt;
  136. static void* gbser_handle;
  137. static QString posn_fname;
  138. static queue pcmpt_head;
  139. static int without_date; /* number of created trackpoints without a valid date */
  140. static struct tm opt_tm; /* converted "date" parameter */
  141. #define MYNAME "nmea"
  142. static char* opt_gprmc;
  143. static char* opt_gpgga;
  144. static char* opt_gpvtg;
  145. static char* opt_gpgsa;
  146. static char* snlenopt;
  147. static char* optdate;
  148. static char* getposnarg;
  149. static char* opt_sleep;
  150. static char* opt_baud;
  151. static char* opt_append;
  152. static char* opt_gisteq;
  153. static char* opt_ignorefix;
  154. static long sleepus;
  155. static int getposn;
  156. static int append_output;
  157. static int amod_waypoint;
  158. static time_t last_time;
  159. static double last_read_time; /* Last timestamp of GGA or PRMC */
  160. static int datum;
  161. static int had_checksum;
  162. static Waypoint* nmea_rd_posn(posn_status*);
  163. static void nmea_rd_posn_init(const QString& fname);
  164. arglist_t nmea_args[] = {
  165. {"snlen", &snlenopt, "Max length of waypoint name to write", "6", ARGTYPE_INT, "1", "64" },
  166. {"gprmc", &opt_gprmc, "Read/write GPRMC sentences", "1", ARGTYPE_BOOL, ARG_NOMINMAX },
  167. {"gpgga", &opt_gpgga, "Read/write GPGGA sentences", "1", ARGTYPE_BOOL, ARG_NOMINMAX },
  168. {"gpvtg", &opt_gpvtg, "Read/write GPVTG sentences", "1", ARGTYPE_BOOL, ARG_NOMINMAX },
  169. {"gpgsa", &opt_gpgsa, "Read/write GPGSA sentences", "1", ARGTYPE_BOOL, ARG_NOMINMAX },
  170. {"date", &optdate, "Complete date-free tracks with given date (YYYYMMDD).", NULL, ARGTYPE_INT, ARG_NOMINMAX },
  171. {
  172. "get_posn", &getposnarg, "Return current position as a waypoint",
  173. NULL, ARGTYPE_BOOL, ARG_NOMINMAX
  174. },
  175. {"pause", &opt_sleep, "Decimal seconds to pause between groups of strings", NULL, ARGTYPE_INT, ARG_NOMINMAX },
  176. {"append_positioning", &opt_append, "Append realtime positioning data to the output file instead of truncating", "0", ARGTYPE_BOOL, ARG_NOMINMAX },
  177. {"baud", &opt_baud, "Speed in bits per second of serial port (baud=4800)", NULL, ARGTYPE_INT, ARG_NOMINMAX },
  178. {"gisteq", &opt_gisteq, "Write tracks for Gisteq Phototracker", "0", ARGTYPE_BOOL, ARG_NOMINMAX },
  179. {"ignore_fix", &opt_ignorefix, "Accept position fixes in gpgga marked invalid", "0", ARGTYPE_BOOL, ARG_NOMINMAX },
  180. ARG_TERMINATOR
  181. };
  182. #define CHECK_BOOL(a) if (a && (*a == '0')) a = NULL
  183. /*
  184. * Slightly different than the Magellan checksum fn.
  185. */
  186. int
  187. nmea_cksum(const char* const buf)
  188. {
  189. int x = 0 ;
  190. const char* p;
  191. for (p = buf; *p; p++) {
  192. x ^= *p;
  193. }
  194. return x;
  195. }
  196. static void
  197. nmea_add_wpt(Waypoint* wpt, route_head* trk)
  198. {
  199. if (datum != DATUM_WGS84) {
  200. double lat, lon, alt;
  201. GPS_Math_Known_Datum_To_WGS84_M(
  202. wpt->latitude, wpt->longitude, 0,
  203. &lat, &lon, &alt, datum);
  204. wpt->latitude = lat;
  205. wpt->longitude = lon;
  206. }
  207. if (trk != NULL) {
  208. track_add_wpt(trk, wpt);
  209. } else {
  210. waypt_add(wpt);
  211. }
  212. }
  213. static void
  214. nmea_release_wpt(Waypoint* wpt)
  215. {
  216. if (wpt && ((wpt->Q.next == NULL) || (wpt->Q.next == &wpt->Q))) {
  217. /* This waypoint isn't queued.
  218. Release it, because we don't have any reference to this
  219. waypoint (! memory leak !) */
  220. delete wpt;
  221. }
  222. }
  223. static void
  224. nmea_rd_init(const QString& fname)
  225. {
  226. curr_waypt = NULL;
  227. last_waypt = NULL;
  228. last_time = -1;
  229. datum = DATUM_WGS84;
  230. had_checksum = 0;
  231. CHECK_BOOL(opt_gprmc);
  232. CHECK_BOOL(opt_gpgga);
  233. CHECK_BOOL(opt_gpvtg);
  234. CHECK_BOOL(opt_gpgsa);
  235. CHECK_BOOL(opt_gisteq);
  236. QUEUE_INIT(&pcmpt_head);
  237. if (getposnarg) {
  238. getposn = 1;
  239. }
  240. /* A special case hack that gets our current position and returns
  241. * it as one waypoint.
  242. */
  243. if (getposn) {
  244. Waypoint* wpt;
  245. posn_status st;
  246. nmea_rd_posn_init(fname);
  247. wpt = nmea_rd_posn(&st);
  248. if (!wpt) {
  249. return;
  250. }
  251. wpt->shortname = "Position";
  252. nmea_add_wpt(wpt, NULL);
  253. return;
  254. }
  255. read_mode = rm_file;
  256. file_in = gbfopen(fname, "rb", MYNAME);
  257. }
  258. static void
  259. nmea_rd_deinit(void)
  260. {
  261. switch (read_mode) {
  262. case rm_serial:
  263. gbser_deinit(gbser_handle);
  264. break;
  265. case rm_file:
  266. gbfclose(file_in);
  267. file_in = NULL;
  268. break;
  269. default:
  270. fatal("nmea_rd_deinit: illegal read_mode.\n");
  271. break;
  272. }
  273. posn_fname.clear();
  274. }
  275. static void
  276. nmea_wr_init(const QString& portname)
  277. {
  278. CHECK_BOOL(opt_gprmc);
  279. CHECK_BOOL(opt_gpgga);
  280. CHECK_BOOL(opt_gpvtg);
  281. CHECK_BOOL(opt_gpgsa);
  282. CHECK_BOOL(opt_gisteq);
  283. append_output = atoi(opt_append);
  284. file_out = gbfopen(portname, append_output ? "a+" : "w+", MYNAME);
  285. sleepus = -1;
  286. if (opt_sleep) {
  287. if (*opt_sleep) {
  288. sleepus = 1e6 * atof(opt_sleep);
  289. } else {
  290. sleepus = -1;
  291. }
  292. }
  293. mkshort_handle = mkshort_new_handle();
  294. setshort_length(mkshort_handle, atoi(snlenopt));
  295. if (opt_gisteq) {
  296. opt_gpgga = NULL;
  297. opt_gpvtg = NULL;
  298. opt_gpgsa = NULL;
  299. }
  300. }
  301. static void
  302. nmea_wr_deinit(void)
  303. {
  304. gbfclose(file_out);
  305. mkshort_del_handle(&mkshort_handle);
  306. }
  307. static void
  308. nmea_set_waypoint_time(Waypoint* wpt, struct tm* time, double fsec)
  309. {
  310. if (time->tm_year == 0) {
  311. wpt->SetCreationTime(((((time_t)time->tm_hour * 60) + time->tm_min) * 60) + time->tm_sec, lround(1000.0 * fsec));
  312. if (wpt->wpt_flags.fmt_use == 0) {
  313. wpt->wpt_flags.fmt_use = 1;
  314. without_date++;
  315. }
  316. } else {
  317. wpt->SetCreationTime(mkgmtime(time), lround(1000.0 * fsec));
  318. if (wpt->wpt_flags.fmt_use != 0) {
  319. wpt->wpt_flags.fmt_use = 0;
  320. without_date--;
  321. }
  322. }
  323. }
  324. static void
  325. gpgll_parse(char* ibuf)
  326. {
  327. if (trk_head == NULL) {
  328. trk_head = route_head_alloc();
  329. track_add_head(trk_head);
  330. }
  331. QStringList fields = QString(ibuf).split(",", QString::KeepEmptyParts);
  332. double latdeg = 0;
  333. if (fields.size() > 1) latdeg = fields[1].toDouble();
  334. QChar latdir = 'N';
  335. if (fields.size() > 2) latdir = fields[2][0];
  336. double lngdeg = 0;
  337. if (fields.size() > 3) lngdeg = fields[3].toDouble();
  338. QChar lngdir = 'E';
  339. if (fields.size() > 4) lngdir = fields[4][0];
  340. double hmsd = 0;
  341. if (fields.size() > 5) hmsd = fields[5].toDouble();
  342. bool valid = false;
  343. if (fields.size() > 6) valid = fields[6].startsWith('A');
  344. if (!valid) {
  345. return;
  346. }
  347. int hms = (int) hmsd;
  348. last_read_time = hms;
  349. double fsec = hmsd - hms;
  350. tm.tm_sec = hms % 100;
  351. hms = hms / 100;
  352. tm.tm_min = hms % 100;
  353. hms = hms / 100;
  354. tm.tm_hour = hms % 100;
  355. Waypoint* waypt = new Waypoint;
  356. nmea_set_waypoint_time(waypt, &tm, fsec);
  357. if (latdir == 'S') {
  358. latdeg = -latdeg;
  359. }
  360. waypt->latitude = ddmm2degrees(latdeg);
  361. if (lngdir == 'W') {
  362. lngdeg = -lngdeg;
  363. }
  364. waypt->longitude = ddmm2degrees(lngdeg);
  365. nmea_release_wpt(curr_waypt);
  366. curr_waypt = waypt;
  367. }
  368. static void
  369. gpgga_parse(char* ibuf)
  370. {
  371. if (trk_head == NULL) {
  372. trk_head = route_head_alloc();
  373. track_add_head(trk_head);
  374. }
  375. QStringList fields = QString(ibuf).split(",", QString::KeepEmptyParts);
  376. double hms = 0;
  377. if (fields.size() > 1) hms = fields[1].toDouble();
  378. double latdeg = 0;
  379. if (fields.size() > 2) latdeg = fields[2].toDouble();
  380. QChar latdir = 'N';
  381. if (fields.size() > 3) latdir = fields[3][0];
  382. double lngdeg = 0;
  383. if (fields.size() > 4) lngdeg = fields[4].toDouble();
  384. QChar lngdir = 'W';
  385. if (fields.size() > 5) lngdir = fields[5][0];
  386. int fix = fix_unknown;
  387. if (fields.size() > 6) fix = fields[6].toInt();
  388. int nsats = 0;
  389. if (fields.size() > 7) nsats = fields[7].toInt();
  390. double hdop = 0;
  391. if (fields.size() > 8) hdop = fields[8].toDouble();
  392. double alt = unknown_alt;
  393. if (fields.size() > 9) alt = fields[9].toDouble();
  394. QChar altunits;
  395. if (fields.size() > 10) altunits = fields[10][0];
  396. double geoidheight = unknown_alt;
  397. if (fields.size() > 11) geoidheight = fields[11].toDouble();
  398. QChar geoidheightunits = 'M';
  399. if (fields.size() > 12) geoidheightunits = fields[12][0];
  400. /*
  401. * In serial mode, allow the fix with an invalid position through
  402. * as serial units will often spit a remembered position up and
  403. * that is more comfortable than nothing at all...
  404. */
  405. CHECK_BOOL(opt_ignorefix);
  406. if ((fix <= 0) && (read_mode != rm_serial) && (!opt_ignorefix)) {
  407. return;
  408. }
  409. last_read_time = hms;
  410. double fsec = hms - (int)hms;
  411. tm.tm_sec = (long) hms % 100;
  412. hms = hms / 100;
  413. tm.tm_min = (long) hms % 100;
  414. hms = hms / 100;
  415. tm.tm_hour = (long) hms % 100;
  416. Waypoint* waypt = new Waypoint;
  417. nmea_set_waypoint_time(waypt, &tm, fsec);
  418. if (latdir == 'S') {
  419. latdeg = -latdeg;
  420. }
  421. waypt->latitude = ddmm2degrees(latdeg);
  422. if (lngdir == 'W') {
  423. lngdeg = -lngdeg;
  424. }
  425. waypt->longitude = ddmm2degrees(lngdeg);
  426. waypt->altitude = alt;
  427. WAYPT_SET(waypt, geoidheight, geoidheight);
  428. waypt->sat = nsats;
  429. waypt->hdop = hdop;
  430. switch (fix) {
  431. case 0:
  432. waypt->fix = fix_none;
  433. break;
  434. case 1:
  435. waypt->fix = (nsats>3)?(fix_3d):(fix_2d);
  436. break;
  437. case 2:
  438. waypt->fix = fix_dgps;
  439. break;
  440. case 3:
  441. waypt->fix = fix_pps;
  442. break;
  443. }
  444. nmea_release_wpt(curr_waypt);
  445. curr_waypt = waypt;
  446. }
  447. static void
  448. gprmc_parse(char* ibuf)
  449. {
  450. if (trk_head == NULL) {
  451. trk_head = route_head_alloc();
  452. track_add_head(trk_head);
  453. }
  454. QStringList fields = QString(ibuf).split(",", QString::KeepEmptyParts);
  455. double hms = 0;
  456. if (fields.size() > 1) hms = fields[1].toDouble();
  457. QChar fix = 'V'; // V == "Invalid"
  458. if (fields.size() > 2) fix = fields[2][0];
  459. double latdeg = 0;
  460. if (fields.size() > 3) latdeg = fields[3].toDouble();
  461. QChar latdir = 'N';
  462. if (fields.size() > 4) latdir = fields[4][0];
  463. double lngdeg = 0;
  464. if (fields.size() > 5) lngdeg = fields[5].toDouble();
  465. QChar lngdir = 'W';
  466. if (fields.size() > 6) lngdir = fields[6][0];
  467. double speed = 0;
  468. if (fields.size() > 7) speed = fields[7].toDouble();
  469. double course = 0;
  470. if (fields.size() > 8) course = fields[8].toDouble();
  471. int dmy = 0;
  472. if (fields.size() > 9) dmy = fields[9].toDouble();
  473. if (fix != 'A') {
  474. /* ignore this fix - it is invalid */
  475. return;
  476. }
  477. last_read_time = hms;
  478. double fsec = hms - (int)hms;
  479. tm.tm_sec = (long) hms % 100;
  480. hms = hms / 100;
  481. tm.tm_min = (long) hms % 100;
  482. hms = hms / 100;
  483. tm.tm_hour = (long) hms % 100;
  484. tm.tm_year = dmy % 100 + 100;
  485. dmy = dmy / 100;
  486. tm.tm_mon = dmy % 100 - 1;
  487. dmy = dmy / 100;
  488. tm.tm_mday = dmy;
  489. if (posn_type == gpgga) {
  490. /* capture useful data update and exit */
  491. if (curr_waypt) {
  492. if (! WAYPT_HAS(curr_waypt, speed)) {
  493. WAYPT_SET(curr_waypt, speed, KNOTS_TO_MPS(speed));
  494. }
  495. if (! WAYPT_HAS(curr_waypt, course)) {
  496. WAYPT_SET(curr_waypt, course, course);
  497. }
  498. /* The change of date wasn't recorded when
  499. * going from 235959 to 000000. */
  500. nmea_set_waypoint_time(curr_waypt, &tm, fsec);
  501. }
  502. /* This point is both a waypoint and a trackpoint. */
  503. if (amod_waypoint) {
  504. waypt_add(new Waypoint(*curr_waypt));
  505. amod_waypoint = 0;
  506. }
  507. return;
  508. }
  509. Waypoint* waypt = new Waypoint;
  510. WAYPT_SET(waypt, speed, KNOTS_TO_MPS(speed));
  511. WAYPT_SET(waypt, course, course);
  512. nmea_set_waypoint_time(waypt, &tm, fsec);
  513. if (latdir == 'S') {
  514. latdeg = -latdeg;
  515. }
  516. waypt->latitude = ddmm2degrees(latdeg);
  517. if (lngdir == 'W') {
  518. lngdeg = -lngdeg;
  519. }
  520. waypt->longitude = ddmm2degrees(lngdeg);
  521. nmea_release_wpt(curr_waypt);
  522. curr_waypt = waypt;
  523. /* This point is both a waypoint and a trackpoint. */
  524. if (amod_waypoint) {
  525. waypt_add(new Waypoint(*waypt));
  526. amod_waypoint = 0;
  527. }
  528. }
  529. static void
  530. gpwpl_parse(char* ibuf)
  531. {
  532. // The last field isn't actually separated by a field separator and
  533. // is a string, so we brutally whack the checksum (trailing *NN).
  534. QString qibuf = QString(ibuf);
  535. qibuf.truncate(qibuf.lastIndexOf('*'));
  536. QStringList fields = qibuf.split(",", QString::KeepEmptyParts);
  537. double latdeg = 0;
  538. if (fields.size() > 1) latdeg = fields[1].toDouble();
  539. QChar latdir = 'N';
  540. if (fields.size() > 2) latdir = fields[2][0];
  541. double lngdeg = 0;
  542. if (fields.size() > 3) lngdeg = fields[3].toDouble();
  543. QChar lngdir = 'E';
  544. if (fields.size() > 4) lngdir = fields[4][0];
  545. QString sname;
  546. if (fields.size() > 5) sname = fields[5];
  547. if (latdir == 'S') {
  548. latdeg = -latdeg;
  549. }
  550. if (lngdir == 'W') {
  551. lngdeg = -lngdeg;
  552. }
  553. Waypoint* waypt = new Waypoint;
  554. waypt->latitude = ddmm2degrees(latdeg);
  555. waypt->longitude = ddmm2degrees(lngdeg);
  556. waypt->shortname = sname;
  557. curr_waypt = NULL; /* waypoints won't be updated with GPS fixes */
  558. nmea_add_wpt(waypt, NULL);
  559. }
  560. static void
  561. gpzda_parse(char* ibuf)
  562. {
  563. double hms;
  564. int dd, mm, yy, lclhrs, lclmins;
  565. sscanf(ibuf,"$%*2cZDA,%lf,%d,%d,%d,%d,%d",
  566. &hms, &dd, &mm, &yy, &lclhrs, &lclmins);
  567. tm.tm_sec = (int) hms % 100;
  568. tm.tm_min = (((int) hms - tm.tm_sec) / 100) % 100;
  569. tm.tm_hour = (int) hms / 10000;
  570. tm.tm_mday = dd;
  571. tm.tm_mon = mm - 1;
  572. tm.tm_year = yy - 1900;
  573. // FIXME: why do we do all this and then do nothing with the result?
  574. // This can't have worked.
  575. }
  576. static void
  577. gpgsa_parse(char* ibuf)
  578. {
  579. char fixauto;
  580. char fix;
  581. int prn[12] = {0};
  582. int scn,cnt;
  583. float pdop=0,hdop=0,vdop=0;
  584. char* tok=0;
  585. memset(prn,0xff,sizeof(prn));
  586. scn = sscanf(ibuf,"$%*2cGSA,%c,%c,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d",
  587. &fixauto, &fix,
  588. &prn[0],&prn[1],&prn[2],&prn[3],&prn[4],&prn[5],
  589. &prn[6],&prn[7],&prn[8],&prn[9],&prn[10],&prn[11]);
  590. if (scn < 2) {
  591. warning(MYNAME ": Short GSA sentence.\n");
  592. }
  593. /*
  594. sscanf has scanned all the leftmost elements
  595. we'll rescan by skipping 15 commas to the dops
  596. */
  597. tok = ibuf;
  598. for (cnt=0; (tok)&&(cnt<15); cnt++) {
  599. tok = strchr(tok,',');
  600. if (!tok) {
  601. break;
  602. }
  603. tok++;
  604. }
  605. if (tok) {
  606. sscanf(tok,"%f,%f,%f",&pdop,&hdop,&vdop);
  607. }
  608. if (curr_waypt) {
  609. if (curr_waypt->fix!=fix_dgps) {
  610. if (fix=='3') {
  611. curr_waypt->fix=fix_3d;
  612. } else if (fix=='2') {
  613. curr_waypt->fix=fix_2d;
  614. }
  615. }
  616. curr_waypt->pdop = pdop;
  617. curr_waypt->hdop = hdop;
  618. curr_waypt->vdop = vdop;
  619. if (curr_waypt->sat <= 0) {
  620. for (cnt=0; cnt<12; cnt++) {
  621. curr_waypt->sat += (prn[cnt]>0)?(1):(0);
  622. }
  623. }
  624. }
  625. }
  626. static void
  627. gpvtg_parse(char* ibuf)
  628. {
  629. QStringList fields = QString(ibuf).split(",", QString::KeepEmptyParts);
  630. double course = 0;
  631. if (fields.size() > 1) course = fields[1].toDouble();
  632. double speed_n = 0;
  633. if (fields.size() > 5) speed_n = fields[5].toDouble();
  634. double speed_k = 0;
  635. if (fields.size() > 7) speed_k = fields[7].toDouble();
  636. if (curr_waypt) {
  637. WAYPT_SET(curr_waypt, course, course);
  638. if (speed_k > 0) {
  639. WAYPT_SET(curr_waypt, speed, KPH_TO_MPS(speed_k))
  640. } else {
  641. WAYPT_SET(curr_waypt, speed, KNOTS_TO_MPS(speed_n));
  642. }
  643. }
  644. }
  645. /*
  646. * AVMAP EKP-IV Tracks - a proprietary (and very weird) extended NMEA.
  647. * https://sourceforge.net/tracker/?func=detail&atid=489478&aid=1640814&group_id=58972
  648. */
  649. static
  650. double pcmpt_deg(int d)
  651. {
  652. int deg = d / 100000;
  653. double minutes = (((d / 100000.0) - deg) * 100) / 60.0;
  654. return (double) deg + minutes;
  655. }
  656. void
  657. pcmpt_parse(char* ibuf)
  658. {
  659. int i, j1, j2, j3, j4, j5, j6;
  660. int lat, lon;
  661. char altflag, u1, u2;
  662. float alt, f1, f2;
  663. char coords[20] = {0};
  664. int dmy, hms;
  665. dmy = hms = 0;
  666. sscanf(ibuf,"$PCMPT,%d,%d,%d,%c,%f,%d,%19[^,],%d,%f,%d,%f,%c,%d,%c,%d",
  667. &j1, &j2, &j3, &altflag, &alt, &j4, (char*) &coords,
  668. &j5, &f1, &j6, &f2, &u1, &dmy, &u2, &hms);
  669. if (altflag == 'D' && curr_waypt && alt > 0) {
  670. curr_waypt->altitude = alt /*+ 500*/;
  671. return;
  672. }
  673. /*
  674. * There are a couple of different second line records, but we
  675. * don't care about them.
  676. */
  677. if (j2 != 1) {
  678. return;
  679. }
  680. sscanf(coords, "%d%n", &lat, &i);
  681. if (coords[i] == 'S') {
  682. lat = -lat;
  683. }
  684. sscanf(coords + i + 1, "%d%n", &lon, &i);
  685. if (coords[i] == 'W') {
  686. lon= -lon;
  687. }
  688. if (lat || lon) {
  689. curr_waypt = new Waypoint;
  690. curr_waypt->longitude = pcmpt_deg(lon);
  691. curr_waypt->latitude = pcmpt_deg(lat);
  692. tm.tm_sec = (long) hms % 100;
  693. hms = hms / 100;
  694. tm.tm_min = (long) hms % 100;
  695. hms = hms / 100;
  696. tm.tm_hour = (long) hms % 100;
  697. tm.tm_year = dmy % 10000 - 1900;
  698. dmy = dmy / 10000;
  699. tm.tm_mon = dmy % 100 - 1;
  700. dmy = dmy / 100;
  701. tm.tm_mday = dmy;
  702. nmea_set_waypoint_time(curr_waypt, &tm, 0);
  703. ENQUEUE_HEAD(&pcmpt_head, &curr_waypt->Q);
  704. } else {
  705. queue* elem, *tmp;
  706. route_head* trk_head;
  707. if (QUEUE_EMPTY(&pcmpt_head)) {
  708. return;
  709. }
  710. /*
  711. * Since we oh-so-cleverly inserted points at the head,
  712. * we can rip through the queue forward now to get our
  713. ` * handy-dandy reversing effect.
  714. */
  715. trk_head = route_head_alloc();
  716. track_add_head(trk_head);
  717. QUEUE_FOR_EACH(&pcmpt_head, elem, tmp) {
  718. Waypoint* wpt = (Waypoint*) dequeue(elem);
  719. nmea_add_wpt(wpt, trk_head);
  720. }
  721. }
  722. }
  723. static void
  724. nmea_fix_timestamps(route_head* track)
  725. {
  726. if ((trk_head == NULL) || (without_date == 0)) {
  727. return;
  728. }
  729. if (tm.tm_year == 0) {
  730. queue* elem, *temp;
  731. Waypoint* prev = NULL;
  732. time_t delta_tm;
  733. if (optdate == NULL) {
  734. warning(MYNAME ": No date found within track (all points dropped)!\n");
  735. warning(MYNAME ": Please use option \"date\" to preset a valid date for thoose tracks.\n");
  736. track_del_head(track);
  737. return;
  738. }
  739. delta_tm = mkgmtime(&opt_tm);
  740. QUEUE_FOR_EACH(&track->waypoint_list, elem, temp) {
  741. Waypoint* wpt = (Waypoint*)elem;
  742. wpt->creation_time += delta_tm;
  743. if ((prev != NULL) && (prev->creation_time > wpt->creation_time)) { /* go over midnight ? */
  744. delta_tm += SECONDS_PER_DAY;
  745. wpt->creation_time += SECONDS_PER_DAY;
  746. }
  747. prev = wpt;
  748. }
  749. } else {
  750. time_t prev;
  751. queue* elem;
  752. tm.tm_hour = 23; /* last date found */
  753. tm.tm_min = 59;
  754. tm.tm_sec = 59;
  755. prev = mkgmtime(&tm);
  756. /* go backward through the track and complete timestamps */
  757. for (elem = QUEUE_LAST(&track->waypoint_list); elem != &track->waypoint_list; elem=elem->prev) {
  758. Waypoint* wpt = (Waypoint*)elem;
  759. if (wpt->wpt_flags.fmt_use != 0) {
  760. time_t dt;
  761. wpt->wpt_flags.fmt_use = 0; /* reset flag */
  762. dt = (prev / SECONDS_PER_DAY) * SECONDS_PER_DAY;
  763. wpt->creation_time += dt;
  764. if (wpt->creation_time.toTime_t() > prev) {
  765. wpt->creation_time+=SECONDS_PER_DAY;
  766. }
  767. }
  768. prev = wpt->GetCreationTime().toTime_t();
  769. }
  770. }
  771. }
  772. static int
  773. notalkerid_strmatch(const char * s1, const char *sentenceFormatterMnemonicCode)
  774. {
  775. /*
  776. * compare leading start of parametric sentence character ('$'), sentence address field, and trailing comma
  777. * to the desired sentence formatter mneumonic code (the 3rd-5th characters of the sentence address field).
  778. * The talker identifier mneumonic (the 1st-2nd characters of the sentence address field)
  779. * is likely "GP" for Global Posilioning System (GPS)
  780. * but other talkers like "IN" for Integrated Navigation can emit relevant sentences,
  781. * so we ignore the talker identifier mneumonic.
  782. */
  783. return strncmp(s1,"$",1) || strncmp(s1+3,sentenceFormatterMnemonicCode,3) || strncmp(s1+6,",",1);
  784. }
  785. void
  786. nmea_parse_one_line(char* ibuf)
  787. {
  788. char* ck;
  789. int ckval, ckcmp;
  790. char* tbuf = lrtrim(ibuf);
  791. /*
  792. * GISTEQ PhotoTracker (stupidly) puts a bogus field in front
  793. * of the line. Look for it and toss it.
  794. */
  795. if (0 == strncmp(tbuf, "---,", 4)) {
  796. tbuf += 4;
  797. }
  798. if (*tbuf != '$') {
  799. return;
  800. }
  801. ck = strrchr(tbuf, '*');
  802. if (ck != NULL) {
  803. *ck = '\0';
  804. ckval = nmea_cksum(&tbuf[1]);
  805. *ck = '*';
  806. ck++;
  807. sscanf(ck, "%2X", &ckcmp);
  808. if (ckval != ckcmp) {
  809. #if 0
  810. printf("ckval %X, %X, %s\n", ckval, ckcmp, ck);
  811. printf("NMEA %s\n", tbuf);
  812. #endif
  813. return;
  814. }
  815. had_checksum = 1;
  816. } else if (had_checksum) {
  817. /* we have had a checksum on all previous sentences, but not on this
  818. one, which probably indicates this line is truncated */
  819. had_checksum = 0;
  820. return;
  821. }
  822. if (strstr(tbuf+1,"$")!=NULL) {
  823. /* If line has more than one $, there is probably an error in it. */
  824. return;
  825. }
  826. /* @@@ zmarties: The parse routines all assume all fields are present, but
  827. the NMEA format allows any field to be missed out if there is no data
  828. for that field. Rather than change all the parse routines, we first
  829. substitute a default value of zero for any missing field.
  830. */
  831. if (strstr(tbuf, ",,")) {
  832. tbuf = gstrsub(tbuf, ",,", ",0,");
  833. }
  834. if (0 == notalkerid_strmatch(tbuf, "WPL")) {
  835. gpwpl_parse(tbuf);
  836. } else if (opt_gpgga && (0 == notalkerid_strmatch(tbuf, "GGA"))) {
  837. posn_type = gpgga;
  838. gpgga_parse(tbuf);
  839. } else if (opt_gprmc && (0 == notalkerid_strmatch(tbuf, "RMC"))) {
  840. if (posn_type != gpgga) {
  841. posn_type = gprmc;
  842. }
  843. /*
  844. * Always call gprmc_parse() because like GPZDA
  845. * it contains the full date.
  846. */
  847. gprmc_parse(tbuf);
  848. } else if (0 == notalkerid_strmatch(tbuf, "GLL")) {
  849. if ((posn_type != gpgga) && (posn_type != gprmc)) {
  850. gpgll_parse(tbuf);
  851. }
  852. } else if (0 == notalkerid_strmatch(tbuf, "ZDA")) {
  853. gpzda_parse(tbuf);
  854. } else if (0 == strncmp(tbuf, "$PCMPT,", 7)) {
  855. pcmpt_parse(tbuf);
  856. } else if (opt_gpvtg && (0 == notalkerid_strmatch(tbuf, "VTG"))) {
  857. gpvtg_parse(tbuf); /* speed and course */
  858. } else if (opt_gpgsa && (0 == notalkerid_strmatch(tbuf, "GSA"))) {
  859. gpgsa_parse(tbuf); /* GPS fix */
  860. } else if (0 == strncmp(tbuf, "$ADPMB,5,0", 10)) {
  861. amod_waypoint = 1;
  862. }
  863. if (tbuf != ibuf) {
  864. /* clear up the dynamic buffer we used because substition was required */
  865. xfree(tbuf);
  866. }
  867. }
  868. static void
  869. nmea_read(void)
  870. {
  871. char* ibuf;
  872. char* ck;
  873. double lt = -1;
  874. int line = -1;
  875. posn_type = gp_unknown;
  876. trk_head = NULL;
  877. without_date = 0;
  878. memset(&tm, 0, sizeof(tm));
  879. opt_tm = tm;
  880. /* This was done in rd_init() */
  881. if (getposn) {
  882. return;
  883. }
  884. if (optdate) {
  885. memset(&opt_tm, 0, sizeof(opt_tm));
  886. ck = (char*)strptime(optdate, "%Y%m%d", &opt_tm);
  887. if ((ck == NULL) || (*ck != '\0') || (strlen(optdate) != 8)) {
  888. fatal(MYNAME ": Invalid date \"%s\"!\n", optdate);
  889. } else if (opt_tm.tm_year < 70) {
  890. fatal(MYNAME ": Date \"%s\" is out of range (have to be 19700101 or later)!\n", optdate);
  891. }
  892. }
  893. curr_waypt = NULL;
  894. while ((ibuf = gbfgetstr(file_in))) {
  895. char* sdatum, *cx;
  896. line++;
  897. if ((line == 0) & file_in->unicode) {
  898. cet_convert_init(CET_CHARSET_UTF8, 1);
  899. }
  900. if ((line == 0) && (case_ignore_strncmp(ibuf, "@SonyGPS/ver", 12) == 0)) {
  901. /* special hack for Sony GPS-CS1 files:
  902. they are fully (?) nmea compatible, but come with a header line like
  903. "@Sonygps/ver1.0/wgs-84". */
  904. /* The Sony GPS-CS3KA extends that line even further
  905. so we now look for the second field to be /
  906. delimited.
  907. @Sonygps/ver1.0/wgs-84/gps-cs3.0
  908. */
  909. /* Check the GPS datum */
  910. cx = strchr(&ibuf[12], '/');
  911. if (cx != NULL) {
  912. char* edatum;
  913. sdatum = cx + 1;
  914. edatum = strchr(sdatum, '/');
  915. if (edatum) {
  916. *edatum = 0;
  917. }
  918. datum = GPS_Lookup_Datum_Index(sdatum);
  919. if (datum < 0) {
  920. fatal(MYNAME "/SonyGPS: Unsupported datum \"%s\" in source data!\n", sdatum);
  921. }
  922. }
  923. continue;
  924. }
  925. nmea_parse_one_line(ibuf);
  926. if (lt != last_read_time && curr_waypt && trk_head) {
  927. if (curr_waypt != last_waypt) {
  928. nmea_add_wpt(curr_waypt, trk_head);
  929. last_waypt = curr_waypt;
  930. }
  931. lt = last_read_time;
  932. }
  933. }
  934. /* try to complete date-less trackpoints */
  935. nmea_fix_timestamps(trk_head);
  936. }
  937. void
  938. nmea_rd_posn_init(const QString& fname)
  939. {
  940. if ((gbser_handle = gbser_init(qPrintable(fname))) != NULL) {
  941. read_mode = rm_serial;
  942. gbser_set_speed(gbser_handle, 4800);
  943. } else {
  944. fatal(MYNAME ": Could not open '%s' for position tracking.\n", qPrintable(fname));
  945. }
  946. gbser_flush(gbser_handle);
  947. if (opt_baud) {
  948. if (!gbser_set_speed(gbser_handle, atoi(opt_baud))) {
  949. fatal(MYNAME ": Unable to set baud rate %s\n", opt_baud);
  950. }
  951. }
  952. posn_fname = fname;
  953. }
  954. static void
  955. safe_print(int cnt, const char* b)
  956. {
  957. int i;
  958. for (i = 0; i < cnt; i++) {
  959. char c = isprint(b[i]) ? b[i] : '.';
  960. fputc(c, stderr);
  961. }
  962. }
  963. static void reset_sirf_to_nmea(int br);
  964. static
  965. int hunt_sirf(void)
  966. {
  967. /* Try to place the common BR's first to speed searching */
  968. static int br[] = {38400, 9600, 57600, 115200, 19200, 4800, -1};
  969. static int* brp = &br[0];
  970. char ibuf[1024];
  971. for (brp = br; *brp > 0; brp++) {
  972. int rv;
  973. if (global_opts.debug_level > 1) {
  974. fprintf(stderr, "Trying %d\n", *brp);
  975. }
  976. /*
  977. * Cycle our port's data speed and spray the "change to NMEA
  978. * mode to the device.
  979. */
  980. gbser_set_speed(gbser_handle, *brp);
  981. reset_sirf_to_nmea(*brp);
  982. rv = gbser_read_line(gbser_handle, ibuf, sizeof(ibuf),
  983. 1000, 0x0a, 0x0d);
  984. /*
  985. * If we didn't get a read error but did get a string that
  986. * started with a dollar sign, we're probably in NMEA mode
  987. * now.
  988. */
  989. if ((rv > -1) && (strlen(ibuf) > 0) && ibuf[0] == '$') {
  990. return 1;
  991. }
  992. /*
  993. * If nothing was received, it's not a sirf part. Fast exit.
  994. */
  995. if (rv < 0) {
  996. return 0;
  997. }
  998. }
  999. return 0;
  1000. }
  1001. static Waypoint*
  1002. nmea_rd_posn(posn_status* posn_status)
  1003. {
  1004. char ibuf[1024];
  1005. static double lt = -1;
  1006. int i;
  1007. int am_sirf = 0;
  1008. /*
  1009. * Read a handful of sentences, collecting the best info we
  1010. * can. If the timestamp changes (indicating the sequence is
  1011. * about to restart and thus the one we're collecting isn't going
  1012. * to get any better than we now have) hand that back to the caller.
  1013. */
  1014. for (i = 0; i < 10; i++) {
  1015. int rv;
  1016. ibuf[0] = 0;
  1017. rv = gbser_read_line(gbser_handle, ibuf, sizeof(ibuf), 2000, 0x0a, 0x0d);
  1018. if (global_opts.debug_level > 1) {
  1019. safe_print(strlen(ibuf), ibuf);
  1020. }
  1021. if (rv < 0) {
  1022. if (am_sirf == 0) {
  1023. if (global_opts.debug_level > 1) {
  1024. warning(MYNAME ": Attempting sirf mode.\n");
  1025. }
  1026. /* This is tacky, we have to change speed
  1027. * to 9600bps to tell it to speak NMEA at
  1028. * 4800.
  1029. */
  1030. am_sirf = hunt_sirf();
  1031. if (am_sirf) {
  1032. i = 0;
  1033. continue;
  1034. }
  1035. }
  1036. fatal(MYNAME ": No data received on %s.\n", qPrintable(posn_fname));
  1037. }
  1038. nmea_parse_one_line(ibuf);
  1039. if (lt != last_read_time) {
  1040. if (last_read_time) {
  1041. Waypoint* w = curr_waypt;
  1042. lt = last_read_time;
  1043. curr_waypt = NULL;
  1044. return w;
  1045. }
  1046. }
  1047. }
  1048. return NULL;
  1049. }
  1050. static void
  1051. nmea_wayptpr(const Waypoint* wpt)
  1052. {
  1053. char obuf[200];
  1054. double lat,lon;
  1055. QString s;
  1056. int cksum;
  1057. lat = degrees2ddmm(wpt->latitude);
  1058. lon = degrees2ddmm(wpt->longitude);
  1059. if (global_opts.synthesize_shortnames) {
  1060. s = mkshort_from_wpt(mkshort_handle, wpt);
  1061. } else {
  1062. s = mkshort(mkshort_handle, wpt->shortname);
  1063. }
  1064. snprintf(obuf, sizeof(obuf), "GPWPL,%08.3f,%c,%09.3f,%c,%s",
  1065. fabs(lat), lat < 0 ? 'S' : 'N',
  1066. fabs(lon), lon < 0 ? 'W' : 'E', CSTRc(s)
  1067. );
  1068. cksum = nmea_cksum(obuf);
  1069. gbfprintf(file_out, "$%s*%02X\n", obuf, cksum);
  1070. if (sleepus >= 0) {
  1071. gbfflush(file_out);
  1072. gb_sleep(sleepus);
  1073. }
  1074. }
  1075. void
  1076. nmea_track_init(const route_head*)
  1077. {
  1078. last_time = -1;
  1079. }
  1080. void
  1081. nmea_trackpt_pr(const Waypoint* wpt)
  1082. {
  1083. char obuf[200];
  1084. char fix='0';
  1085. double lat,lon;
  1086. int cksum;
  1087. struct tm* tm;
  1088. time_t hms;
  1089. time_t ymd;
  1090. if (opt_sleep) {
  1091. gbfflush(file_out);
  1092. if (last_time > 0) {
  1093. if (sleepus >= 0) {
  1094. gb_sleep(sleepus);
  1095. } else {
  1096. long wait_time = wpt->GetCreationTime().toTime_t() - last_time;
  1097. if (wait_time > 0) {
  1098. gb_sleep(wait_time * 1000000);
  1099. }
  1100. }
  1101. }
  1102. last_time = wpt->GetCreationTime().toTime_t();
  1103. }
  1104. lat = degrees2ddmm(wpt->latitude);
  1105. lon = degrees2ddmm(wpt->longitude);
  1106. time_t ct = wpt->GetCreationTime().toTime_t();
  1107. tm = gmtime(&ct);
  1108. if (tm) {
  1109. hms = tm->tm_hour * 10000 + tm->tm_min * 100 + tm->tm_sec;
  1110. ymd = tm->tm_mday * 10000 + tm->tm_mon * 100 + tm->tm_year;
  1111. } else {
  1112. hms = 0;
  1113. ymd = 0;
  1114. }
  1115. switch (wpt->fix) {
  1116. case fix_dgps:
  1117. fix='2';
  1118. break;
  1119. case fix_3d:
  1120. case fix_2d:
  1121. fix='1';
  1122. break;
  1123. case fix_pps:
  1124. fix='3';
  1125. break;
  1126. default:
  1127. fix='0';
  1128. }
  1129. if (opt_gprmc) {
  1130. snprintf(obuf, sizeof(obuf), "GPRMC,%010.3f,%c,%08.3f,%c,%09.3f,%c,%.2f,%.2f,%06d,,",
  1131. (double) hms + (wpt->GetCreationTime().time().msec() / 1000.0),
  1132. fix=='0' ? 'V' : 'A',
  1133. fabs(lat), lat < 0 ? 'S' : 'N',
  1134. fabs(lon), lon < 0 ? 'W' : 'E',
  1135. WAYPT_HAS(wpt, speed) ? MPS_TO_KNOTS(wpt->speed):(0),
  1136. WAYPT_HAS(wpt, course) ? (wpt->course):(0),
  1137. (int) ymd);
  1138. cksum = nmea_cksum(obuf);
  1139. /* GISTeq doesn't care about the checksum, but wants this prefixed, so
  1140. * we can write it with abandon.
  1141. */
  1142. if (opt_gisteq) {
  1143. gbfprintf(file_out, "---,");
  1144. }
  1145. gbfprintf(file_out, "$%s*%02X\n", obuf, cksum);
  1146. }
  1147. if (opt_gpgga) {
  1148. snprintf(obuf, sizeof(obuf), "GPGGA,%010.3f,%08.3f,%c,%09.3f,%c,%c,%02d,%.1f,%.3f,M,%.1f,M,,",
  1149. (double) hms + (wpt->GetCreationTime().time().msec() / 1000.0),
  1150. fabs(lat), lat < 0 ? 'S' : 'N',
  1151. fabs(lon), lon < 0 ? 'W' : 'E',
  1152. fix,
  1153. (wpt->sat>0)?(wpt->sat):(0),
  1154. (wpt->hdop>0)?(wpt->hdop):(0.0),
  1155. wpt->altitude == unknown_alt ? 0 : wpt->altitude,
  1156. WAYPT_HAS(wpt, geoidheight)? (wpt->geoidheight) : (0)); /* TODO: we could look up the geoidheight if needed */
  1157. cksum = nmea_cksum(obuf);
  1158. gbfprintf(file_out, "$%s*%02X\n", obuf, cksum);
  1159. }
  1160. if ((opt_gpvtg) && (WAYPT_HAS(wpt, course) || WAYPT_HAS(wpt, speed))) {
  1161. snprintf(obuf,sizeof(obuf),"GPVTG,%.3f,T,0,M,%.3f,N,%.3f,K",
  1162. WAYPT_HAS(wpt, course) ? (wpt->course):(0),
  1163. WAYPT_HAS(wpt, speed) ? MPS_TO_KNOTS(wpt->speed):(0),
  1164. WAYPT_HAS(wpt, speed) ? MPS_TO_KPH(wpt->speed):(0));
  1165. cksum = nmea_cksum(obuf);
  1166. gbfprintf(file_out, "$%s*%02X\n", obuf, cksum);
  1167. }
  1168. if ((opt_gpgsa) && (wpt->fix!=fix_unknown)) {
  1169. switch (wpt->fix) {
  1170. case fix_dgps:
  1171. /* or */
  1172. case fix_3d:
  1173. fix='3';
  1174. break;
  1175. case fix_2d:
  1176. fix='2';
  1177. break;
  1178. default:
  1179. fix=0;
  1180. }
  1181. snprintf(obuf,sizeof(obuf),"GPGSA,A,%c,,,,,,,,,,,,,%.1f,%.1f,%.1f",
  1182. fix,
  1183. (wpt->pdop>0)?(wpt->pdop):(0),
  1184. (wpt->hdop>0)?(wpt->hdop):(0),
  1185. (wpt->vdop>0)?(wpt->vdop):(0));
  1186. cksum = nmea_cksum(obuf);
  1187. gbfprintf(file_out, "$%s*%02X\n", obuf, cksum);
  1188. }
  1189. gbfflush(file_out);
  1190. }
  1191. static void
  1192. nmea_write(void)
  1193. {
  1194. waypt_disp_all(nmea_wayptpr);
  1195. track_disp_all(nmea_track_init, NULL, nmea_trackpt_pr);
  1196. }
  1197. static void
  1198. nmea_wr_posn_init(const QString& fname)
  1199. {
  1200. nmea_wr_init(fname);
  1201. }
  1202. static void
  1203. nmea_wr_posn(Waypoint* wpt)
  1204. {
  1205. nmea_trackpt_pr(wpt);
  1206. }
  1207. static void
  1208. nmea_wr_posn_deinit(void)
  1209. {
  1210. // nmea_wr_deinit();
  1211. }
  1212. ff_vecs_t nmea_vecs = {
  1213. ff_type_file,
  1214. {
  1215. (ff_cap)(ff_cap_read | ff_cap_write),
  1216. (ff_cap)(ff_cap_read | ff_cap_write),
  1217. ff_cap_none
  1218. },
  1219. nmea_rd_init,
  1220. nmea_wr_init,
  1221. nmea_rd_deinit,
  1222. nmea_wr_deinit,
  1223. nmea_read,
  1224. nmea_write,
  1225. NULL,
  1226. nmea_args,
  1227. CET_CHARSET_ASCII, 0, /* CET-REVIEW */
  1228. {
  1229. nmea_rd_posn_init, nmea_rd_posn, nmea_rd_deinit,
  1230. nmea_wr_posn_init, nmea_wr_posn, nmea_wr_posn_deinit
  1231. }
  1232. };
  1233. /*
  1234. * If we later decide to implement a "real" Sirf module, this code should
  1235. * go there. For now, we try a kind of heavy handed thing - if we don't
  1236. * see NMEA-isms from the device, we'll go on the premise that it MAY be
  1237. * a SiRF Star device and send it the "speak NMEA, please" command.
  1238. */
  1239. static void
  1240. sirf_write(unsigned char* buf)
  1241. {
  1242. int i, chksum = 0;
  1243. int len = buf[2] << 8 | buf[3];
  1244. for (i = 0; i < len; i++) {
  1245. chksum += buf[4 + i];
  1246. }
  1247. chksum &= 0x7fff;
  1248. buf[len + 4] = chksum >> 8;
  1249. buf[len + 5] = chksum & 0xff;
  1250. gbser_write(gbser_handle, buf, len + 8); /* 4 at front, 4 at back */
  1251. }
  1252. static
  1253. void reset_sirf_to_nmea(int br)
  1254. {
  1255. static unsigned char pkt[] = {0xa0, 0xa2, 0x00, 0x18,
  1256. 0x81, 0x02,
  1257. 0x01, 0x01, /* GGA */
  1258. 0x00, 0x00, /* suppress GLL */
  1259. 0x01, 0x00, /* suppress GSA */
  1260. 0x05, 0x00, /* suppress GSV */
  1261. 0x01, 0x01, /* use RMC for date*/
  1262. 0x00, 0x00, /* suppress VTG */
  1263. 0x00, 0x01, /* output rate */
  1264. 0x00, 0x01, /* unused recommended values */
  1265. 0x00, 0x01,
  1266. 0x00, 0x01, /* ZDA */
  1267. 0x12, 0xc0, /* 4800 bps */
  1268. 0x00, 0x00, /* checksum */
  1269. 0xb0, 0xb3
  1270. }; /* packet end */
  1271. /* repopulate bit rate */
  1272. pkt[26] = br >> 8;
  1273. pkt[27] = br & 0xff;
  1274. sirf_write(pkt);
  1275. gb_sleep(250 * 1000);
  1276. gbser_flush(gbser_handle);
  1277. }