Devuan fork of gpsd
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.
 
 
 
 
 
 

465 lines
14 KiB

  1. /*
  2. * Handle the Garmin simple text format supported by some Garmins.
  3. * Tested with the 'Garmin eTrex Legend' device working in 'Text Out' mode.
  4. *
  5. * Protocol info from:
  6. * http://www8.garmin.com/support/text_out.html
  7. * http://www.garmin.com/support/commProtocol.html
  8. *
  9. * Code by: Petr Slansky <slansky@usa.net>
  10. * all rights abandoned, a thank would be nice if you use this code.
  11. *
  12. * -D 3 = packet trace
  13. * -D 4 = packet details
  14. * -D 5 = more packet details
  15. * -D 6 = very excessive details
  16. *
  17. * limitations:
  18. * very simple protocol, only very basic information
  19. * TODO
  20. * do not have from garmin:
  21. * pdop
  22. * vdop
  23. * magnetic variation
  24. * satellite information
  25. *
  26. * This file is Copyright (c) 2010-2018 by the GPSD project
  27. * SPDX-License-Identifier: BSD-2-clause
  28. *
  29. */
  30. /***************************************************
  31. Garmin Simple Text Output Format:
  32. The simple text (ASCII) output contains time, position, and velocity data in
  33. the fixed width fields (not delimited) defined in the following table:
  34. FIELD DESCRIPTION: WIDTH: NOTES:
  35. ----------------------- ------- ------------------------
  36. Sentence start 1 Always '@'
  37. ----------------------- ------- ------------------------
  38. /Year 2 Last two digits of UTC year
  39. | ----------------------- ------- ------------------------
  40. | Month 2 UTC month, "01".."12"
  41. T | ----------------------- ------- ------------------------
  42. i | Day 2 UTC day of month, "01".."31"
  43. m | ----------------------- ------- ------------------------
  44. e | Hour 2 UTC hour, "00".."23"
  45. | ----------------------- ------- ------------------------
  46. | Minute 2 UTC minute, "00".."59"
  47. | ----------------------- ------- ------------------------
  48. \Second 2 UTC second, "00".."59"
  49. ----------------------- ------- ------------------------
  50. /Latitude hemisphere 1 'N' or 'S'
  51. | ----------------------- ------- ------------------------
  52. | Latitude position 7 WGS84 ddmmmmm, with an implied
  53. | decimal after the 4th digit
  54. | ----------------------- ------- ------------------------
  55. | Longitude hemishpere 1 'E' or 'W'
  56. | ----------------------- ------- ------------------------
  57. | Longitude position 8 WGS84 dddmmmmm with an implied
  58. P | decimal after the 5th digit
  59. o | ----------------------- ------- ------------------------
  60. s | Position status 1 'd' if current 2D differential GPS position
  61. i | 'D' if current 3D differential GPS position
  62. t | 'g' if current 2D GPS position
  63. i | 'G' if current 3D GPS position
  64. o | 'S' if simulated position
  65. n | '_' if invalid position
  66. | ----------------------- ------- ------------------------
  67. | Horizontal posn error 3 EPH in meters
  68. | ----------------------- ------- ------------------------
  69. | Altitude sign 1 '+' or '-'
  70. | ----------------------- ------- ------------------------
  71. | Altitude 5 Height above or below mean
  72. \ sea level in meters
  73. ----------------------- ------- ------------------------
  74. /East/West velocity 1 'E' or 'W'
  75. | direction
  76. | ----------------------- ------- ------------------------
  77. | East/West velocity 4 Meters per second in tenths,
  78. | magnitude ("1234" = 123.4 m/s)
  79. V | ----------------------- ------- ------------------------
  80. e | North/South velocity 1 'N' or 'S'
  81. l | direction
  82. o | ----------------------- ------- ------------------------
  83. c | North/South velocity 4 Meters per second in tenths,
  84. i | magnitude ("1234" = 123.4 m/s)
  85. t | ----------------------- ------- ------------------------
  86. y | Vertical velocity 1 'U' or 'D' (up/down)
  87. | direction
  88. | ----------------------- ------- ------------------------
  89. | Vertical velocity 4 Meters per second in hundredths,
  90. \ magnitude ("1234" = 12.34 m/s)
  91. ----------------------- ------- ------------------------
  92. Sentence end 2 Carriage return, '0x0D', and
  93. line feed, '0x0A'
  94. ----------------------- ------- ------------------------
  95. If a numeric value does not fill its entire field width, the field is padded
  96. with leading '0's (eg. an altitude of 50 meters above MSL will be output as
  97. "+00050").
  98. Any or all of the data in the text sentence (except for the sentence start
  99. and sentence end fields) may be replaced with underscores to indicate
  100. invalid data.
  101. ***************************************************/
  102. #include "gpsd_config.h" /* must be before all includes */
  103. #include <stdlib.h>
  104. #include <stdbool.h>
  105. #include <math.h>
  106. #include <string.h>
  107. #include <strings.h>
  108. #include "gpsd.h"
  109. #ifdef GARMINTXT_ENABLE
  110. /* Simple text message is fixed length, 55 chars text data + 2 characters EOL */
  111. /* buffer for text processing */
  112. #define TXT_BUFFER_SIZE 13
  113. /**************************************************************************
  114. * decode text string to double number, translate prefix to sign
  115. * return 0: OK
  116. * -1: data error
  117. * -2: data not valid
  118. *
  119. * examples with context->errout.debug == 0:
  120. * gar_decode(context, cbuf, 9, "EW", 100000.0, &result);
  121. * E01412345 -> +14.12345
  122. * gar_decode(context, cbuf, 9, "EW", 100000.0, &result);
  123. * W01412345 -> -14.12345
  124. * gar_decode(context, cbuf, 3, "", 10.0, &result);
  125. * 123 -> +12.3
  126. **************************************************************************/
  127. static int gar_decode(const struct gps_context_t *context,
  128. const char *data, const size_t length,
  129. const char *prefix, const double dividor,
  130. double *result)
  131. {
  132. char buf[10];
  133. float sign = 1.0;
  134. int preflen = (int)strlen(prefix);
  135. int offset = 1; /* assume one character prefix (E,W,S,N,U,D, etc) */
  136. long int intresult;
  137. if (length >= sizeof(buf)) {
  138. GPSD_LOG(LOG_ERROR, &context->errout, "internal buffer too small\n");
  139. return -1;
  140. }
  141. memset(buf, 0, (int)sizeof(buf));
  142. (void)strlcpy(buf, data, length);
  143. GPSD_LOG(LOG_RAW, &context->errout, "Decoded string: %s\n", buf);
  144. if (strchr(buf, '_') != NULL) {
  145. /* value is not valid, ignore it */
  146. return -2;
  147. }
  148. /* parse prefix */
  149. do {
  150. if (preflen == 0) {
  151. offset = 0; /* only number, no prefix */
  152. break;
  153. }
  154. /* second character in prefix is flag for negative number */
  155. if (preflen >= 2) {
  156. // cppcheck-suppress arrayIndexOutOfBounds
  157. if (buf[0] == prefix[1]) {
  158. sign = -1.0;
  159. break;
  160. }
  161. }
  162. /* first character in prefix is flag for positive number */
  163. if (preflen >= 1) {
  164. if (buf[0] == prefix[0]) {
  165. sign = 1.0;
  166. break;
  167. }
  168. }
  169. GPSD_LOG(LOG_WARN, &context->errout,
  170. "Unexpected char \"%c\" in data \"%s\"\n",
  171. buf[0], buf);
  172. return -1;
  173. } while (0);
  174. if (strspn(buf + offset, "0123456789") != length - offset) {
  175. GPSD_LOG(LOG_WARN, &context->errout, "Invalid value %s\n", buf);
  176. return -1;
  177. }
  178. intresult = atol(buf + offset);
  179. if (intresult == 0L)
  180. sign = 0.0; /* don't create negative zero */
  181. *result = (double)intresult / dividor * sign;
  182. return 0; /* SUCCESS */
  183. }
  184. /**************************************************************************
  185. * decode integer from string, check if the result is in expected range
  186. * return 0: OK
  187. * -1: data error
  188. * -2: data not valid
  189. **************************************************************************/
  190. static int gar_int_decode(const struct gps_context_t *context,
  191. const char *data, const size_t length,
  192. const unsigned int min, const unsigned int max,
  193. unsigned int *result)
  194. {
  195. char buf[6];
  196. unsigned int res;
  197. if (length >= sizeof(buf)) {
  198. GPSD_LOG(LOG_ERROR, &context->errout, "internal buffer too small\n");
  199. return -1;
  200. }
  201. memset(buf, 0, (int)sizeof(buf));
  202. (void)strlcpy(buf, data, length);
  203. GPSD_LOG(LOG_RAW, &context->errout, "Decoded string: %s\n", buf);
  204. if (strchr(buf, '_') != NULL) {
  205. /* value is not valid, ignore it */
  206. return -2;
  207. }
  208. if (strspn(buf, "0123456789") != length) {
  209. GPSD_LOG(LOG_WARN, &context->errout, "Invalid value %s\n", buf);
  210. return -1;
  211. }
  212. res = (unsigned)atoi(buf);
  213. if ((res >= min) && (res <= max)) {
  214. *result = res;
  215. return 0; /* SUCCESS */
  216. } else {
  217. GPSD_LOG(LOG_WARN, &context->errout,
  218. "Value %u out of range <%u, %u>\n", res, min,
  219. max);
  220. return -1;
  221. }
  222. }
  223. /**************************************************************************
  224. *
  225. * Entry points begin here
  226. *
  227. **************************************************************************/
  228. gps_mask_t garmintxt_parse(struct gps_device_t * session)
  229. {
  230. /* parse GARMIN Simple Text sentence, unpack it into a session structure */
  231. gps_mask_t mask = 0;
  232. GPSD_LOG(LOG_PROG, &session->context->errout,
  233. "Garmin Simple Text packet, len %zd: %s\n",
  234. session->lexer.outbuflen, (char*)session->lexer.outbuffer);
  235. if (session->lexer.outbuflen < 54) {
  236. /* trailing CR and LF can be ignored; ('@' + 54x 'DATA' + '\r\n')
  237. * has length 57 */
  238. GPSD_LOG(LOG_WARN, &session->context->errout,
  239. "Message is too short, rejected.\n");
  240. return ONLINE_SET;
  241. }
  242. session->lexer.type = GARMINTXT_PACKET;
  243. /* only one message, set cycle start */
  244. session->cycle_end_reliable = true;
  245. do {
  246. struct tm gdate; /* date part of last sentence time */
  247. unsigned int result;
  248. char *buf = (char *)session->lexer.outbuffer + 1;
  249. GPSD_LOG(LOG_PROG, &session->context->errout,
  250. "Timestamp: %.12s\n", buf);
  251. /* year */
  252. if (0 != gar_int_decode(session->context,
  253. buf + 0, 2, 0, 99, &result))
  254. break;
  255. gdate.tm_year = (session->context->century + (int)result) - 1900;
  256. /* month */
  257. if (0 != gar_int_decode(session->context,
  258. buf + 2, 2, 1, 12, &result))
  259. break;
  260. gdate.tm_mon = (int)result - 1;
  261. /* day */
  262. if (0 != gar_int_decode(session->context,
  263. buf + 4, 2, 1, 31, &result))
  264. break;
  265. gdate.tm_mday = (int)result;
  266. /* hour */
  267. if (0 != gar_int_decode(session->context,
  268. buf + 6, 2, 0, 23, &result))
  269. break;
  270. /* mday update?? */
  271. gdate.tm_hour = (int)result;
  272. /* minute */
  273. if (0 != gar_int_decode(session->context,
  274. buf + 8, 2, 0, 59, &result))
  275. break;
  276. gdate.tm_min = (int)result;
  277. /* second */
  278. /* second value can be even 60, occasional leap second */
  279. if (0 != gar_int_decode(session->context,
  280. buf + 10, 2, 0, 60, &result))
  281. break;
  282. gdate.tm_sec = (int)result;
  283. session->newdata.time.tv_sec = mkgmtime(&gdate);
  284. session->newdata.time.tv_nsec = 0;
  285. mask |= TIME_SET;
  286. } while (0);
  287. /* assume that position is unknown; if the position is known we
  288. * will fix status information later */
  289. session->newdata.mode = MODE_NO_FIX;
  290. session->gpsdata.status = STATUS_NO_FIX;
  291. mask |= MODE_SET | STATUS_SET | CLEAR_IS | REPORT_IS;
  292. /* process position */
  293. do {
  294. double lat, lon;
  295. unsigned int degfrag;
  296. char status;
  297. /* Latitude, [NS]ddmmmmm */
  298. /* decode degrees of Latitude */
  299. if (0 !=
  300. gar_decode(session->context,
  301. (char *)session->lexer.outbuffer + 13, 3, "NS", 1.0,
  302. &lat))
  303. break;
  304. /* decode minutes of Latitude */
  305. if (0 !=
  306. gar_int_decode(session->context,
  307. (char *)session->lexer.outbuffer + 16, 5, 0,
  308. 99999, &degfrag))
  309. break;
  310. lat += degfrag * 100.0 / 60.0 / 100000.0;
  311. session->newdata.latitude = lat;
  312. /* Longitude, [EW]dddmmmmm */
  313. /* decode degrees of Longitude */
  314. if (0 !=
  315. gar_decode(session->context,
  316. (char *)session->lexer.outbuffer + 21, 4, "EW", 1.0,
  317. &lon))
  318. break;
  319. /* decode minutes of Longitude */
  320. if (0 !=
  321. gar_int_decode(session->context,
  322. (char *)session->lexer.outbuffer + 25, 5, 0,
  323. 99999, &degfrag))
  324. break;
  325. lon += degfrag * 100.0 / 60.0 / 100000.0;
  326. session->newdata.longitude = lon;
  327. session->newdata.geoid_sep = wgs84_separation(lat, lon);
  328. /* fix mode, GPS status, [gGdDS_] */
  329. status = (char)session->lexer.outbuffer[30];
  330. switch (status) {
  331. case 'G':
  332. case 'S': /* 'S' is DEMO mode, assume 3D position */
  333. session->newdata.mode = MODE_3D;
  334. session->gpsdata.status = STATUS_FIX;
  335. break;
  336. case 'D':
  337. session->newdata.mode = MODE_3D;
  338. session->gpsdata.status = STATUS_DGPS_FIX;
  339. break;
  340. case 'g':
  341. session->newdata.mode = MODE_2D;
  342. session->gpsdata.status = STATUS_FIX;
  343. break;
  344. case 'd':
  345. session->newdata.mode = MODE_2D;
  346. session->gpsdata.status = STATUS_DGPS_FIX;
  347. break;
  348. default:
  349. session->newdata.mode = MODE_NO_FIX;
  350. session->gpsdata.status = STATUS_NO_FIX;
  351. }
  352. mask |= MODE_SET | STATUS_SET | LATLON_SET;
  353. } while (0);
  354. /* EPH */
  355. do {
  356. double eph;
  357. if (0 !=
  358. gar_decode(session->context,
  359. (char *)session->lexer.outbuffer + 31, 3, "", 1.0,
  360. &eph))
  361. break;
  362. /* this conversion looks dodgy... */
  363. session->newdata.eph = eph * (GPSD_CONFIDENCE / CEP50_SIGMA);
  364. mask |= HERR_SET;
  365. } while (0);
  366. /* Altitude */
  367. do {
  368. double alt;
  369. if (0 !=
  370. gar_decode(session->context,
  371. (char *)session->lexer.outbuffer + 34, 6, "+-", 1.0,
  372. &alt))
  373. break;
  374. /* alt is MSL */
  375. session->newdata.altMSL = alt;
  376. /* Let gpsd_error_model() deal with altHAE */
  377. mask |= ALTITUDE_SET;
  378. } while (0);
  379. /* Velocities, meters per second */
  380. do {
  381. double ewvel, nsvel;
  382. double climb;
  383. if (0 != gar_decode(session->context,
  384. (char *)session->lexer.outbuffer + 40, 5,
  385. "EW", 10.0, &ewvel))
  386. break;
  387. if (0 != gar_decode(session->context,
  388. (char *)session->lexer.outbuffer + 45, 5,
  389. "NS", 10.0, &nsvel))
  390. break;
  391. if (0 != gar_decode(session->context,
  392. (char *)session->lexer.outbuffer + 50, 5,
  393. "UD", 100.0, &climb))
  394. break;
  395. session->newdata.NED.velN = ewvel;
  396. session->newdata.NED.velE = nsvel;
  397. session->newdata.NED.velD = -climb;
  398. mask |= VNED_SET;
  399. } while (0);
  400. GPSD_LOG(LOG_DATA, &session->context->errout,
  401. "GTXT: time=%lld, lat=%.2f lon=%.2f altMSL=%.2f "
  402. "climb=%.2f eph=%.2f mode=%d status=%d\n",
  403. (long long)session->newdata.time.tv_sec, session->newdata.latitude,
  404. session->newdata.longitude, session->newdata.altMSL,
  405. session->newdata.climb, session->newdata.eph,
  406. session->newdata.mode,
  407. session->gpsdata.status);
  408. return mask;
  409. }
  410. #endif /* GARMINTXT_ENABLE */