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.

driver_garmin.c 41KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919920921922923924925926927928929930931932933934935936937938939940941942943944945946947948949950951952953954955956957958959960961962963964965966967968969970971972973974975976977978979980981982983984985986987988989990991992993994995996997998999100010011002100310041005100610071008100910101011101210131014101510161017101810191020102110221023102410251026102710281029103010311032103310341035103610371038103910401041104210431044104510461047104810491050105110521053105410551056105710581059106010611062106310641065106610671068106910701071107210731074107510761077107810791080108110821083108410851086108710881089109010911092109310941095109610971098109911001101110211031104110511061107110811091110111111121113111411151116111711181119112011211122112311241125112611271128112911301131113211331134113511361137113811391140114111421143114411451146114711481149115011511152115311541155115611571158115911601161116211631164116511661167116811691170117111721173117411751176117711781179118011811182118311841185118611871188118911901191119211931194119511961197119811991200120112021203120412051206120712081209121012111212121312141215121612171218121912201221122212231224122512261227122812291230123112321233123412351236123712381239124012411242124312441245124612471248124912501251125212531254125512561257125812591260126112621263126412651266126712681269127012711272127312741275127612771278127912801281128212831284128512861287128812891290129112921293129412951296129712981299130013011302130313041305130613071308130913101311131213131314131513161317131813191320132113221323
  1. /*
  2. * This file contains two drivers for Garmin receivers and some code
  3. * shared by both drivers.
  4. *
  5. * One driver "garmin_usb_binary" handles the Garmin binary packet
  6. * format supported by the USB Garmins tested with the Garmin 18 and
  7. * other models. (There is also "garmin_usb_binary_old".) These are ONLY
  8. * for USB devices reporting as: 091e:0003.
  9. *
  10. * The other driver "garmin_ser_binary" is for Garmin receivers via a
  11. * serial port, whether or not one uses a USB/serial adaptor or a real
  12. * serial port. These receivers provide adequate NMEA support, so it
  13. * often makes sense to just put them into NMEA mode.
  14. *
  15. * On Linux, USB Garmins (091e:0003) need the Linux garmin_gps driver and
  16. * will not function without it. On other operating systems, it is clear
  17. * garmin_usb_binary_old does not work since it requires the Linux
  18. * garmin_gps module.
  19. *
  20. * This code has been tested and at least at one time is known to work on
  21. * big- and little-endian CPUs and 32 and 64 bit cpu modes.
  22. *
  23. *
  24. * Documentation for the Garmin protocols can be found via
  25. * http://www.garmin.com/support/commProtocol.html
  26. * The file IOSDK.zip contains IntfSpec.pdf, which describes the
  27. * protocol in terms of Application, Link, and Physical. This
  28. * identical file is also available at:
  29. * http://www.garmin.com/support/pdf/iop_spec.pdf
  30. * An older version of iop_spec.pdf that describes only Serial Binary
  31. * is available at:
  32. * http://vancouver-webpages.com/pub/peter/iop_spec.pdf
  33. * Information about the GPS 18
  34. * http://www.garmin.com/manuals/425_TechnicalSpecification.pdf
  35. *
  36. * There is one physical link protocol for serial which uses DLE/ETX
  37. * framing. There is another physical protocol for USB which relies
  38. * on the packetization intrinsic to USB bulk pipes.
  39. *
  40. * There are several link protocols; all devices implement L000.
  41. * There are then product-specific protocols; most devices implement
  42. * L001. Link protocols are the same and carried over either Physical
  43. * protocol.
  44. *
  45. * Application protocols are named A000 and then with different
  46. * 3-digit numbers. They are carried over Link protocols.
  47. *
  48. * Thus, much of the higher-level code dealing the data formats is
  49. * shared between USB Binary and Serial Binary.
  50. *
  51. * This code is partly from the Garmin IOSDK and partly from the
  52. * sample code in the Linux garmin_gps driver.
  53. *
  54. * bad code by: Gary E. Miller <gem@rellim.com>
  55. *
  56. * -D 3 = packet trace
  57. * -D 4 = packet details
  58. * -D 5 = more packet details
  59. * -D 6 = very excessive details
  60. *
  61. * limitations:
  62. *
  63. * do not have from garmin:
  64. * pdop
  65. * hdop
  66. * vdop
  67. * magnetic variation
  68. *
  69. * known bugs:
  70. * hangs in the fread loop instead of keeping state and returning.
  71. *
  72. * TODO:
  73. *
  74. * ?? Add probe function for Serial Binary to start PVT output.
  75. *
  76. * This file is Copyright (c) 2010-2019 by the GPSD project
  77. * SPDX-License-Identifier: BSD-2-clause
  78. */
  79. #include "gpsd_config.h" /* must be before all includes */
  80. #ifdef GARMIN_ENABLE
  81. #include <errno.h>
  82. #include <math.h>
  83. #include <stdbool.h>
  84. #include <stdio.h>
  85. #include <string.h>
  86. #include <time.h>
  87. #include <unistd.h>
  88. #if defined(HAVE_LIBUSB)
  89. #include <libusb.h>
  90. #endif
  91. #include "gpsd.h"
  92. #include "bits.h"
  93. #include "timespec.h"
  94. #define GPSD_LE16TOH(x) getles16((char *)(&(x)), 0)
  95. #define GPSD_LE32TOH(x) getles32((char *)(&(x)), 0)
  96. #define GPSD_LEF32(x) getlef32((const char *)(&(x)), 0)
  97. #define GPSD_LED64(x) getled64((const char *)(&(x)), 0)
  98. #define USE_RMD 0
  99. /* Used in Serial Physical Layer */
  100. #define ETX 0x03
  101. #define ACK 0x06
  102. #define DLE 0x10
  103. #define NAK 0x15
  104. #define GARMIN_LAYERID_TRANSPORT (uint8_t) 0
  105. #define GARMIN_LAYERID_APPL (uint32_t) 20
  106. // Linux Garmin USB driver layer-id to use for some control mechanisms
  107. #define GARMIN_LAYERID_PRIVATE 0x01106E4B
  108. // packet ids used in private layer
  109. #define PRIV_PKTID_SET_DEBUG 1
  110. #define PRIV_PKTID_SET_MODE 2
  111. #define PRIV_PKTID_INFO_REQ 3
  112. #define PRIV_PKTID_INFO_RESP 4
  113. #define PRIV_PKTID_RESET_REQ 5
  114. #define PRIV_PKTID_SET_DEF_MODE 6
  115. #define MODE_NATIVE 0
  116. #define MODE_GARMIN_SERIAL 1
  117. #define GARMIN_PKTID_TRANSPORT_START_SESSION_REQ 5
  118. #define GARMIN_PKTID_TRANSPORT_START_SESSION_RESP 6
  119. #define GARMIN_PKTID_PROTOCOL_ARRAY 253
  120. #define GARMIN_PKTID_PRODUCT_RQST 254
  121. #define GARMIN_PKTID_PRODUCT_DATA 255
  122. /* 0x29 ')' */
  123. #define GARMIN_PKTID_RMD41_DATA 41
  124. /* 0x33 '3' */
  125. #define GARMIN_PKTID_PVT_DATA 51
  126. /* 0x33 '4' */
  127. #define GARMIN_PKTID_RMD_DATA 52
  128. /* 0x72 'r' */
  129. #define GARMIN_PKTID_SAT_DATA 114
  130. #define GARMIN_PKTID_L001_XFER_CMPLT 12
  131. #define GARMIN_PKTID_L001_COMMAND_DATA 10
  132. #define GARMIN_PKTID_L001_DATE_TIME_DATA 14
  133. #define GARMIN_PKTID_L001_RECORDS 27
  134. #define GARMIN_PKTID_L001_WPT_DATA 35
  135. #define CMND_ABORT 0
  136. #define CMND_START_PVT_DATA 49
  137. #define CMND_STOP_PVT_DATA 50
  138. #define CMND_START_RM_DATA 110
  139. #define MAX_BUFFER_SIZE 4096
  140. #define GARMIN_CHANNELS 12
  141. // something magic about 64, garmin driver will not return more than
  142. // 64 at a time. If you read less than 64 bytes the next read will
  143. // just get the last of the 64 byte buffer.
  144. #define ASYNC_DATA_SIZE 64
  145. #pragma pack(1)
  146. // This is the data format of the satellite data from the garmin USB
  147. typedef struct __attribute__((__packed__))
  148. {
  149. uint8_t svid;
  150. uint16_t snr; // 0 - 0xffff
  151. uint8_t elev;
  152. uint16_t azmth;
  153. uint8_t status; // bit 0, has ephemeris, 1, has diff correction
  154. // bit 2 used in solution
  155. // bit 3??
  156. } cpo_sat_data;
  157. /* Garmin D800_Pvt_Datetype_Type */
  158. /* packet type: GARMIN_PKTID_PVT_DATA 52 */
  159. /* This is the data format of the position data from the garmin USB */
  160. typedef struct __attribute__((__packed__))
  161. {
  162. float alt; /* altitude above WGS 84 (meters) */
  163. float epe; /* estimated position error, 2 sigma (meters) */
  164. float eph; /* epe, but horizontal only (meters) */
  165. float epv; /* epe but vertical only (meters ) */
  166. int16_t fix; /* 0 - failed integrity check
  167. * 1 - invalid or unavailable fix
  168. * 2 - 2D
  169. * 3 - 3D
  170. * 4 - 2D Diff
  171. * 5 - 3D Diff
  172. */
  173. double gps_tow; /* gps time of week (seconds) */
  174. double lat; /* ->latitude (radians) */
  175. double lon; /* ->longitude (radians) */
  176. float lon_vel; /* velocity east (meters/second) */
  177. float lat_vel; /* velocity north (meters/second) */
  178. float alt_vel; /* velocity up (meters/sec) */
  179. // Garmin GPS25 uses pkt_id 0x28 and does not output the
  180. // next 3 items
  181. float msl_hght; /* height of WGS 84 above MSL (meters) */
  182. int16_t leap_sec; /* diff between GPS and UTC (seconds) */
  183. int32_t grmn_days; /* days from UTC December 31st, 1989 to the
  184. * beginning of the current week */
  185. } cpo_pvt_data;
  186. typedef struct __attribute__((__packed__))
  187. {
  188. uint32_t cycles;
  189. double pr; // psuedorange in meters
  190. uint16_t phase;
  191. int8_t slp_dtct;
  192. uint8_t snr_dbhz;
  193. uint8_t svid;
  194. int8_t valid;
  195. } cpo_rcv_sv_data;
  196. /* packet type: GARMIN_PKTID_RMD_DATA 53 */
  197. /* seems identical to the packet id 0x29 from the Garmin GPS 25 */
  198. typedef struct __attribute__((__packed__))
  199. {
  200. double rcvr_tow;
  201. int16_t rcvr_wn;
  202. cpo_rcv_sv_data sv[GARMIN_CHANNELS];
  203. } cpo_rcv_data;
  204. // This is the packet format to/from the Garmin USB
  205. typedef struct __attribute__((__packed__))
  206. {
  207. uint8_t mPacketType;
  208. uint8_t mReserved1;
  209. uint16_t mReserved2;
  210. uint16_t mPacketId;
  211. uint16_t mReserved3;
  212. uint32_t mDataSize;
  213. union
  214. {
  215. //int8_t chars[MAX_BUFFER_SIZE];
  216. // cppcheck-suppress unusedStructMember
  217. uint8_t uchars[MAX_BUFFER_SIZE];
  218. cpo_pvt_data pvt;
  219. cpo_sat_data sats;
  220. } mData;
  221. } Packet_t;
  222. // useful funcs to read/write ints
  223. // floats and doubles are Intel (little-endian) order only...
  224. static inline void set_int16(uint8_t * buf, uint32_t value)
  225. {
  226. buf[0] = (uint8_t) (0x0FF & value);
  227. buf[1] = (uint8_t) (0x0FF & (value >> 8));
  228. }
  229. static inline void set_int32(uint8_t * buf, uint32_t value)
  230. {
  231. buf[0] = (uint8_t) (0x0FF & value);
  232. buf[1] = (uint8_t) (0x0FF & (value >> 8));
  233. buf[2] = (uint8_t) (0x0FF & (value >> 16));
  234. buf[3] = (uint8_t) (0x0FF & (value >> 24));
  235. }
  236. static inline uint16_t get_uint16(const uint8_t * buf)
  237. {
  238. return (uint16_t) (0xFF & buf[0])
  239. | ((uint16_t) (0xFF & buf[1]) << 8);
  240. }
  241. #if defined(HAVE_LIBUSB) && defined(__linux__)
  242. static inline uint32_t get_int32(const uint8_t * buf)
  243. {
  244. return (uint32_t) (0xFF & buf[0])
  245. | ((uint32_t) (0xFF & buf[1]) << 8)
  246. | ((uint32_t) (0xFF & buf[2]) << 16)
  247. | ((uint32_t) (0xFF & buf[3]) << 24);
  248. }
  249. #endif /* HAVE_LIBUSB */
  250. // convert radians to degrees
  251. static inline double radtodeg(double rad)
  252. {
  253. return (double)(rad * RAD_2_DEG);
  254. }
  255. static gps_mask_t PrintSERPacket(struct gps_device_t *session,
  256. unsigned char pkt_id, int pkt_len,
  257. unsigned char *buf);
  258. #if defined(HAVE_LIBUSB) && defined(__linux__)
  259. static gps_mask_t PrintUSBPacket(struct gps_device_t *session,
  260. Packet_t * pkt);
  261. #endif /* HAVE_LIBUSB */
  262. gps_mask_t PrintSERPacket(struct gps_device_t *session, unsigned char pkt_id,
  263. int pkt_len, unsigned char *buf)
  264. {
  265. gps_mask_t mask = ONLINE_SET;
  266. int i = 0, j = 0;
  267. uint16_t prod_id = 0;
  268. uint16_t ver = 0;
  269. int maj_ver;
  270. int min_ver;
  271. time_t time_l = 0;
  272. char msg_buf[512] = "";
  273. char *msg = NULL;
  274. cpo_sat_data *sats = NULL;
  275. cpo_pvt_data *pvt = NULL;
  276. cpo_rcv_data *rmd = NULL;
  277. double gps_tow = 0;
  278. GPSD_LOG(LOG_DATA, &session->context->errout,
  279. "Garmin: PrintSERPacket(, %#02x, %#02x, )\n", pkt_id, pkt_len);
  280. session->cycle_end_reliable = true;
  281. switch (pkt_id) {
  282. case ACK:
  283. GPSD_LOG(LOG_PROG, &session->context->errout, "Garmin: ACK\n");
  284. break;
  285. case NAK:
  286. GPSD_LOG(LOG_PROG, &session->context->errout, "Garmin: NAK\n");
  287. break;
  288. case GARMIN_PKTID_L001_COMMAND_DATA:
  289. prod_id = get_uint16((uint8_t *) buf);
  290. switch (prod_id) {
  291. case CMND_ABORT:
  292. msg = "Abort current xfer";
  293. break;
  294. case CMND_START_PVT_DATA:
  295. msg = "Start Xmit PVT data";
  296. break;
  297. case CMND_STOP_PVT_DATA:
  298. msg = "Stop Xmit PVT data";
  299. break;
  300. case CMND_START_RM_DATA:
  301. msg = "Start RMD data";
  302. break;
  303. default:
  304. (void)snprintf(msg_buf, sizeof(msg_buf), "Unknown: %u",
  305. (unsigned int)prod_id);
  306. msg = msg_buf;
  307. break;
  308. }
  309. GPSD_LOG(LOG_PROG, &session->context->errout,
  310. "Garmin: Appl, Command Data: %s\n", msg);
  311. break;
  312. case GARMIN_PKTID_PRODUCT_RQST:
  313. GPSD_LOG(LOG_PROG, &session->context->errout,
  314. "Garmin: Appl, Product Data req\n");
  315. break;
  316. case GARMIN_PKTID_PRODUCT_DATA:
  317. prod_id = get_uint16((uint8_t *) buf);
  318. ver = get_uint16((uint8_t *) & buf[2]);
  319. maj_ver = (int)(ver / 100);
  320. min_ver = (int)(ver - (maj_ver * 100));
  321. GPSD_LOG(LOG_PROG, &session->context->errout,
  322. "Garmin: Appl, Product Data, sz: %d\n",
  323. pkt_len);
  324. (void)snprintf(session->subtype, sizeof(session->subtype),
  325. "%d: %d.%02d", (int)prod_id, maj_ver, min_ver);
  326. GPSD_LOG(LOG_INF, &session->context->errout,
  327. "Garmin: Product ID: %d, SoftVer: %d.%02d\n",
  328. prod_id, maj_ver, min_ver);
  329. GPSD_LOG(LOG_INF, &session->context->errout,
  330. "Garmin: Product Desc: %s\n", &buf[4]);
  331. mask |= DEVICEID_SET;
  332. GPSD_LOG(LOG_DATA, &session->context->errout,
  333. "Garmin: PRODUCT_DATA: subtype=%s\n",
  334. session->subtype);
  335. break;
  336. case GARMIN_PKTID_PVT_DATA:
  337. GPSD_LOG(LOG_PROG, &session->context->errout,
  338. "Garmin: PVT Data (51) Sz: %d\n", pkt_len);
  339. pvt = (cpo_pvt_data *) buf;
  340. session->context->leap_seconds = (int)GPSD_LE16TOH(pvt->leap_sec);
  341. session->context->valid = LEAP_SECOND_VALID;
  342. // 631065600, unix seconds for 31 Dec 1989 Zulu
  343. time_l = (time_t) (631065600 + (GPSD_LE32TOH(pvt->grmn_days) * 86400));
  344. // TODO, convert grmn_days to context->gps_week
  345. time_l -= session->context->leap_seconds;
  346. // gps_tow is always like x.999 or x.998 just round it to nearest sec
  347. // FIXME! this will break 5Hz garmins...
  348. gps_tow = GPSD_LED64(pvt->gps_tow);
  349. time_l += (time_t)round(gps_tow);
  350. /* sanity check unix time against leap second.
  351. * Leap second 18 at 1 Jan 2017: 1483228800 */
  352. if (17 < session->context->leap_seconds &&
  353. 1483228800L > time_l) {
  354. time_l += 619315200; // fast forward 1024 weeks
  355. }
  356. DTOTS(&session->context->gps_tow, gps_tow);
  357. session->newdata.time.tv_sec = time_l;
  358. session->newdata.time.tv_nsec = 0;
  359. // (long long) for 32-bit systems
  360. GPSD_LOG(LOG_PROG, &session->context->errout,
  361. "Garmin: time_l: %lld\n", (long long)time_l);
  362. session->newdata.latitude = radtodeg(GPSD_LED64(pvt->lat));
  363. session->newdata.longitude = radtodeg(GPSD_LED64(pvt->lon));
  364. // altitude is WGS84
  365. session->newdata.altHAE = GPSD_LEF32(pvt->alt);
  366. // geoid separation from WGS 84
  367. // gpsd sign is opposite of garmin sign
  368. session->newdata.geoid_sep = -GPSD_LEF32(pvt->msl_hght);
  369. /* Estimated position error in meters. Confidence (sigma) not
  370. * specified by Garmin.
  371. * We follow the advice at <http://gpsinformation.net/main/errors.htm>.
  372. * Since GPS data is not gaussian, this is marginal advice...
  373. * If this assumption changes here, it should also change in
  374. * nmea_parse.c where we analyze PGRME.
  375. */
  376. session->newdata.sep = GPSD_LEF32(pvt->epe) *
  377. (GPSD_CONFIDENCE / CEP50_SIGMA);
  378. /* eph, horizaontal error, 2 sigma */
  379. session->newdata.eph = GPSD_LEF32(pvt->eph) *
  380. (GPSD_CONFIDENCE / CEP50_SIGMA);
  381. /* eph, horizaontal error, 2 sigma */
  382. session->newdata.epv = GPSD_LEF32(pvt->epv) *
  383. (GPSD_CONFIDENCE / CEP50_SIGMA);
  384. /* meters/sec */
  385. session->newdata.NED.velN = GPSD_LEF32(pvt->lat_vel);
  386. session->newdata.NED.velE = GPSD_LEF32(pvt->lon_vel);
  387. session->newdata.NED.velD = -GPSD_LEF32(pvt->alt_vel);
  388. switch (GPSD_LE16TOH(pvt->fix)) {
  389. case 0:
  390. case 1:
  391. default:
  392. // no fix
  393. session->gpsdata.status = STATUS_NO_FIX;
  394. session->newdata.mode = MODE_NO_FIX;
  395. break;
  396. case 2:
  397. // 2D fix
  398. session->gpsdata.status = STATUS_FIX;
  399. session->newdata.mode = MODE_2D;
  400. break;
  401. case 3:
  402. // 3D fix
  403. session->gpsdata.status = STATUS_FIX;
  404. session->newdata.mode = MODE_3D;
  405. break;
  406. case 4:
  407. // 2D Differential fix
  408. session->gpsdata.status = STATUS_DGPS_FIX;
  409. session->newdata.mode = MODE_2D;
  410. break;
  411. case 5:
  412. // 3D differential fix
  413. session->gpsdata.status = STATUS_DGPS_FIX;
  414. session->newdata.mode = MODE_3D;
  415. break;
  416. }
  417. GPSD_LOG(LOG_PROG, &session->context->errout,
  418. "Garmin: Appl, mode %d, status %d\n",
  419. session->newdata.mode, session->gpsdata.status);
  420. /* save some expensive calculations if not needed */
  421. if (session->context->errout.debug >= LOG_INF) {
  422. GPSD_LOG(LOG_INF, &session->context->errout,
  423. "Garmin: UTC Time: %lld\n",
  424. (long long)session->newdata.time.tv_sec);
  425. GPSD_LOG(LOG_INF, &session->context->errout,
  426. "Garmin: Geoid Separation (MSL-WGS84): from garmin %lf, "
  427. "calculated %lf\n",
  428. session->newdata.geoid_sep,
  429. wgs84_separation(session->newdata.latitude,
  430. session->newdata.longitude));
  431. GPSD_LOG(LOG_INF, &session->context->errout,
  432. "Garmin: Alt: %.3f, sep: %.3f, eph: %.3f, Epv: %.3f, "
  433. "Fix: %d, Gps_tow: %f, Lat: %.3f, Lon: %.3f, "
  434. "velN: %.3f, velE: %.3f, velD: %.3f, geoidsep: %.3f, "
  435. "Leap: %d, GarminDays: %d\n",
  436. session->newdata.altHAE,
  437. session->newdata.sep,
  438. session->newdata.eph,
  439. session->newdata.epv,
  440. GPSD_LE16TOH(pvt->fix),
  441. gps_tow,
  442. session->newdata.latitude,
  443. session->newdata.longitude,
  444. session->newdata.NED.velN,
  445. session->newdata.NED.velE,
  446. session->newdata.NED.velD,
  447. session->newdata.geoid_sep,
  448. session->context->leap_seconds,
  449. GPSD_LE32TOH(pvt->grmn_days));
  450. }
  451. if (session->newdata.mode > MODE_NO_FIX) {
  452. /* data only valid with a fix */
  453. mask |=
  454. TIME_SET | LATLON_SET | ALTITUDE_SET | STATUS_SET | MODE_SET |
  455. HERR_SET | PERR_IS | CLEAR_IS | REPORT_IS | VNED_SET;
  456. /*
  457. * Garmin documentation says we should wait until four good fixes
  458. * have been seen before trying to use the device for precision
  459. * time service.
  460. */
  461. if (session->fixcnt > 3)
  462. mask |= NTPTIME_IS;
  463. }
  464. GPSD_LOG(LOG_DATA, &session->context->errout,
  465. "Garmin: PVT_DATA: time=%lld, lat=%.2f lon=%.2f "
  466. "eph=%.2f sep=%.2f epv=%.2f mode=%d status=%d\n",
  467. (long long)session->newdata.time.tv_sec,
  468. session->newdata.latitude,
  469. session->newdata.longitude,
  470. session->newdata.eph,
  471. session->newdata.sep,
  472. session->newdata.epv,
  473. session->newdata.mode,
  474. session->gpsdata.status);
  475. break;
  476. case GARMIN_PKTID_RMD_DATA:
  477. case GARMIN_PKTID_RMD41_DATA:
  478. rmd = (cpo_rcv_data *) buf;
  479. GPSD_LOG(LOG_DATA, &session->context->errout,
  480. "Garmin: PVT RMD Data Sz: %d\n", pkt_len);
  481. GPSD_LOG(LOG_PROG, &session->context->errout,
  482. "Garmin: PVT RMD rcvr_tow: %f, rcvr_wn: %d\n",
  483. GPSD_LED64(rmd->rcvr_tow), GPSD_LE16TOH(rmd->rcvr_wn));
  484. for (i = 0; i < GARMIN_CHANNELS; i++) {
  485. GPSD_LOG(LOG_INF, &session->context->errout,
  486. "Garmin: PVT RMD Sat: %3u, cycles: %9u, pr: %16.6f, "
  487. "phase: %7.3f, slp_dtct: %3s, snr: %3u, Valid: %3s\n",
  488. (int)rmd->sv[i].svid + 1,
  489. GPSD_LE32TOH(rmd->sv[i].cycles),
  490. GPSD_LED64(rmd->sv[i].pr),
  491. (GPSD_LE16TOH(rmd->sv[i].phase) * 360.0) / 2048.0,
  492. rmd->sv[i].slp_dtct != 0 ? "Yes" : "No",
  493. rmd->sv[i].snr_dbhz,
  494. rmd->sv[i].valid != 0 ? "Yes" : "No");
  495. }
  496. break;
  497. case GARMIN_PKTID_SAT_DATA:
  498. // record ID 0x72 (114)
  499. GPSD_LOG(LOG_PROG, &session->context->errout,
  500. "Garmin: SAT Data Sz: %d\n", pkt_len);
  501. sats = (cpo_sat_data *) buf;
  502. session->gpsdata.satellites_used = 0;
  503. gpsd_zero_satellites(&session->gpsdata);
  504. for (i = 0, j = 0; i < GARMIN_CHANNELS; i++, sats++) {
  505. GPSD_LOG(LOG_INF, &session->context->errout,
  506. "Garmin: Sat %2d, snr: %5u, elev: %2d, Azmth: %3d, "
  507. "Stat: x%x\n",
  508. sats->svid, GPSD_LE16TOH(sats->snr), sats->elev,
  509. GPSD_LE16TOH(sats->azmth),
  510. sats->status);
  511. if (255 == (int)sats->svid) {
  512. // Garmin uses 255 for empty
  513. // gpsd uses 0 for empty
  514. continue;
  515. }
  516. if ((int)sats->svid <= 32) {
  517. /* GPS 1-32 */
  518. session->gpsdata.skyview[j].PRN = (short)sats->svid;
  519. session->gpsdata.skyview[j].svid = (short)sats->svid;
  520. session->gpsdata.skyview[j].gnssid = GNSSID_GPS;
  521. } else {
  522. /* SBAS 33-64 */
  523. session->gpsdata.skyview[j].PRN = (short)sats->svid;
  524. session->gpsdata.skyview[j].svid = (short)sats->svid + 87;
  525. session->gpsdata.skyview[j].gnssid = GNSSID_SBAS;
  526. }
  527. session->gpsdata.skyview[j].azimuth =
  528. (short)GPSD_LE16TOH(sats->azmth);
  529. session->gpsdata.skyview[j].elevation = (short)sats->elev;
  530. if (0xffff == sats->snr) {
  531. session->gpsdata.skyview[j].ss = NAN;
  532. } else {
  533. // Garmin does not document this. snr is in dB*100
  534. // Known, but not seen satellites have a dB value of -1*100
  535. session->gpsdata.skyview[j].ss =
  536. (float)(GPSD_LE16TOH(sats->snr) / 100.0);
  537. }
  538. // FIX-ME: Garmin documents this, but Daniel Dorau
  539. // <daniel.dorau@gmx.de> says the behavior on his GPSMap60CSX
  540. // doesn't match it.
  541. if ((uint8_t) 0 != (sats->status & 4)) {
  542. // used in solution?
  543. session->gpsdata.skyview[j].used = true;
  544. session->gpsdata.satellites_used++;
  545. }
  546. session->gpsdata.satellites_visible++;
  547. j++;
  548. }
  549. session->gpsdata.skyview_time.tv_sec = 0;
  550. session->gpsdata.skyview_time.tv_nsec = 0;
  551. mask |= USED_IS | SATELLITE_SET;
  552. GPSD_LOG(LOG_DATA, &session->context->errout,
  553. "Garmin: SAT_DATA: visible=%d used=%d\n",
  554. session->gpsdata.satellites_visible,
  555. session->gpsdata.satellites_used);
  556. break;
  557. case GARMIN_PKTID_PROTOCOL_ARRAY:
  558. // Pid_Protocol_Array, ID 253
  559. // this packet is never requested, it just comes, in some case
  560. // after a GARMIN_PKTID_PRODUCT_RQST
  561. GPSD_LOG(LOG_INF, &session->context->errout,
  562. "Garmin: Appl, Product Capability, sz: %d\n",
  563. pkt_len);
  564. for (i = 0; i < pkt_len; i += 3) {
  565. GPSD_LOG(LOG_INF, &session->context->errout,
  566. "Garmin: %c%03d\n",
  567. buf[i], get_uint16((uint8_t *) & buf[i + 1]));
  568. }
  569. break;
  570. default:
  571. GPSD_LOG(LOG_WARN, &session->context->errout,
  572. "Garmin: Unknown packet id: %#02x, Sz: %#02x\n",
  573. pkt_id, pkt_len);
  574. break;
  575. }
  576. GPSD_LOG(LOG_DATA, &session->context->errout,
  577. "Garmin: PrintSERPacket(, %#02x, %#02x, ) mask=(%s)\n",
  578. pkt_id, pkt_len, gps_maskdump(mask));
  579. return mask;
  580. }
  581. #if defined(HAVE_LIBUSB) && defined(__linux__)
  582. // This works around cppcheck not looking into enough config branches
  583. // cppcheck-suppress unusedFunction
  584. static gps_mask_t PrintUSBPacket(struct gps_device_t *session, Packet_t * pkt)
  585. /* For debugging, decodes and prints some known packets */
  586. {
  587. gps_mask_t mask = 0;
  588. int maj_ver;
  589. int min_ver;
  590. uint32_t mode = 0;
  591. uint16_t prod_id = 0;
  592. uint32_t veri = 0;
  593. uint32_t serial;
  594. uint32_t mDataSize = get_int32((uint8_t *) & pkt->mDataSize);
  595. uint8_t *buffer = (uint8_t *) pkt;
  596. GPSD_LOG(LOG_PROG, &session->context->errout, "Garmin: PrintUSBPacket()\n");
  597. if (DLE == pkt->mPacketType) {
  598. GPSD_LOG(LOG_PROG, &session->context->errout,
  599. "Garmin: really a SER packet!\n");
  600. return PrintSERPacket(session,
  601. (unsigned char)buffer[1],
  602. (int)buffer[2], (unsigned char *)(buffer + 3));
  603. }
  604. if (4096 < mDataSize) {
  605. GPSD_LOG(LOG_WARN, &session->context->errout,
  606. "Garmin: bogus packet, size too large=%d\n",
  607. mDataSize);
  608. return 0;
  609. }
  610. switch (pkt->mPacketType) {
  611. case GARMIN_LAYERID_TRANSPORT:
  612. /* Garmin USB layer specific */
  613. switch (pkt->mPacketId) {
  614. case GARMIN_PKTID_TRANSPORT_START_SESSION_REQ:
  615. // Pid_Start_Session, ID 5
  616. GPSD_LOG(LOG_PROG, &session->context->errout,
  617. "Garmin: Transport, Start Session req\n");
  618. break;
  619. case GARMIN_PKTID_TRANSPORT_START_SESSION_RESP:
  620. // Pid_Session_Started, ID 6
  621. mode = get_int32(&pkt->mData.uchars[0]);
  622. GPSD_LOG(LOG_PROG, &session->context->errout,
  623. "Garmin: Transport, Start Session resp, unit: 0x%x\n",
  624. mode);
  625. break;
  626. default:
  627. GPSD_LOG(LOG_PROG, &session->context->errout,
  628. "Garmin: Transport, Packet: Type %d %d %d, ID: %d,"
  629. "Sz: %d\n",
  630. pkt->mPacketType, pkt->mReserved1, pkt->mReserved2,
  631. pkt->mPacketId, mDataSize);
  632. break;
  633. }
  634. break;
  635. case GARMIN_LAYERID_APPL:
  636. /* raw data transport, shared with Garmin Serial Driver */
  637. mask = PrintSERPacket(session,
  638. (unsigned char)pkt->mPacketId,
  639. (int)mDataSize,
  640. (unsigned char *)pkt->mData.uchars);
  641. break;
  642. case 75:
  643. // private, garmin USB kernel driver specific
  644. switch (pkt->mPacketId) {
  645. case PRIV_PKTID_SET_MODE:
  646. prod_id = get_uint16(&pkt->mData.uchars[0]);
  647. GPSD_LOG(LOG_PROG, &session->context->errout,
  648. "Garmin: Private, Set Mode: %d\n", prod_id);
  649. break;
  650. case PRIV_PKTID_INFO_REQ:
  651. GPSD_LOG(LOG_PROG, &session->context->errout,
  652. "Garmin: Private, ID: Info Req\n");
  653. break;
  654. case PRIV_PKTID_INFO_RESP:
  655. veri = get_int32(pkt->mData.uchars);
  656. maj_ver = (int)(veri >> 16);
  657. min_ver = (int)(veri & 0xffff);
  658. mode = get_int32(&pkt->mData.uchars[4]);
  659. serial = get_int32(&pkt->mData.uchars[8]);
  660. GPSD_LOG(LOG_PROG, &session->context->errout,
  661. "Garmin: Private, ID: Info Resp\n");
  662. GPSD_LOG(LOG_INF, &session->context->errout,
  663. "Garmin: USB Driver found, Version %d.%d, Mode: %d, GPS Serial# %u\n",
  664. maj_ver, min_ver, mode, serial);
  665. break;
  666. default:
  667. GPSD_LOG(LOG_PROG, &session->context->errout,
  668. "Garmin: Private, Packet: ID: %d, Sz: %d\n",
  669. pkt->mPacketId, mDataSize);
  670. break;
  671. }
  672. break;
  673. default:
  674. GPSD_LOG(LOG_PROG, &session->context->errout,
  675. "Garmin: Packet: Type %d %d %d, ID: %d, Sz: %d\n",
  676. pkt->mPacketType, pkt->mReserved1, pkt->mReserved2,
  677. pkt->mPacketId, mDataSize);
  678. break;
  679. }
  680. return mask;
  681. }
  682. #endif /* HAVE_LIBUSB */
  683. #if defined(HAVE_LIBUSB) && defined(__linux__)
  684. /* build and send a packet w/ USB protocol */
  685. static void Build_Send_USB_Packet(struct gps_device_t *session,
  686. uint32_t layer_id, uint32_t pkt_id,
  687. uint32_t length, uint32_t data)
  688. {
  689. uint8_t *buffer = (uint8_t *) session->driver.garmin.Buffer;
  690. Packet_t *thePacket = (Packet_t *) buffer;
  691. ssize_t theBytesReturned = 0;
  692. ssize_t theBytesToWrite = 12 + (ssize_t) length;
  693. set_int32(buffer, layer_id);
  694. set_int32(buffer + 4, pkt_id);
  695. set_int32(buffer + 8, length);
  696. if (2 == length) {
  697. set_int16(buffer + 12, data);
  698. } else if (4 == length) {
  699. set_int32(buffer + 12, data);
  700. }
  701. (void)PrintUSBPacket(session, thePacket);
  702. theBytesReturned = gpsd_write(session, (const char *)thePacket,
  703. (size_t) theBytesToWrite);
  704. GPSD_LOG(LOG_PROG, &session->context->errout,
  705. "Garmin: SendPacket(), wrote %zd bytes\n",
  706. theBytesReturned);
  707. // Garmin says:
  708. // If the packet size was an exact multiple of the USB packet
  709. // size, we must make a final write call with no data
  710. // as a practical matter no known packets are 64 bytes long so
  711. // this is untested
  712. // So here goes just in case
  713. if (0 == (theBytesToWrite % ASYNC_DATA_SIZE)) {
  714. char *n = "";
  715. (void)gpsd_write(session, n, 0);
  716. }
  717. }
  718. #endif /* HAVE_LIBUSB && __linux__ */
  719. /* build and send a packet in serial protocol */
  720. /* layer_id unused */
  721. // FIX-ME: This should go through the common message buffer someday
  722. static void Build_Send_SER_Packet(struct gps_device_t *session,
  723. uint32_t layer_id UNUSED, uint32_t pkt_id,
  724. uint32_t length, uint32_t data)
  725. {
  726. uint8_t *buffer = (uint8_t *) session->driver.garmin.Buffer;
  727. uint8_t *buffer0 = buffer;
  728. Packet_t *thePacket = (Packet_t *) buffer;
  729. ssize_t theBytesReturned = 0;
  730. ssize_t theBytesToWrite = 6 + (ssize_t) length;
  731. uint8_t chksum = 0;
  732. *buffer++ = (uint8_t) DLE;
  733. *buffer++ = (uint8_t) pkt_id;
  734. chksum = pkt_id;
  735. *buffer++ = (uint8_t) length;
  736. chksum += length;
  737. /* ??? What is this doing? */
  738. if (2 == length) {
  739. /* carefull! no DLE stuffing here! */
  740. set_int16(buffer, data);
  741. chksum += buffer[0];
  742. chksum += buffer[1];
  743. } else if (4 == length) {
  744. /* carefull! no DLE stuffing here! */
  745. set_int32(buffer, data);
  746. chksum += buffer[0];
  747. chksum += buffer[1];
  748. chksum += buffer[2];
  749. chksum += buffer[3];
  750. }
  751. /* ??? How is data copied to the buffer? */
  752. buffer += length;
  753. // Add checksum
  754. *buffer++ = -chksum;
  755. if (DLE == -chksum) {
  756. /* stuff another DLE */
  757. *buffer++ = (uint8_t) DLE;
  758. theBytesToWrite++;
  759. }
  760. // Add DLE, ETX
  761. *buffer++ = (uint8_t) DLE;
  762. /* we used to say n++ here, but scan-build complains */
  763. *buffer = (uint8_t) ETX;
  764. (void)PrintSERPacket(session,
  765. (unsigned char)buffer0[1],
  766. (int)buffer0[2], (unsigned char *)(buffer0 + 3));
  767. theBytesReturned = gpsd_write(session, (const char *)thePacket,
  768. (size_t) theBytesToWrite);
  769. GPSD_LOG(LOG_PROG, &session->context->errout,
  770. "Garmin: SendPacket(), wrote %zd bytes\n",
  771. theBytesReturned);
  772. }
  773. #if defined(HAVE_LIBUSB) && defined(__linux__)
  774. /*
  775. * is_usb_device() - is a specified device USB matching given vendor/product?
  776. *
  777. * BUG: Doesn't actually match against path yet. Must finish this function
  778. * by querying /sys/dev/char, either directly or using libudev. Greg KH
  779. * assures this is possible, though he is vague about how.
  780. *
  781. * libudev: http://www.kernel.org/pub/linux/utils/kernel/hotplug/libudev/
  782. */
  783. // This works around cppcheck not looking into enough config branches
  784. // cppcheck-suppress unusedFunction
  785. static bool is_usb_device(const char *path UNUSED, int vendor, int product,
  786. struct gpsd_errout_t *errout)
  787. {
  788. // discover devices
  789. libusb_device **list;
  790. ssize_t cnt;
  791. ssize_t i = 0;
  792. bool found = false;
  793. GPSD_LOG(LOG_INF, errout, "attempting USB device enumeration.\n");
  794. (void)libusb_init(NULL);
  795. if ((cnt = libusb_get_device_list(NULL, &list)) < 0) {
  796. GPSD_LOG(LOG_ERROR, errout, "USB device list call failed.\n");
  797. libusb_exit(NULL);
  798. return false;
  799. }
  800. for (i = 0; i < cnt; i++) {
  801. struct libusb_device_descriptor desc;
  802. libusb_device *dev = list[i];
  803. int r = libusb_get_device_descriptor(dev, &desc);
  804. if (r < 0) {
  805. GPSD_LOG(LOG_ERROR, errout,
  806. "USB descriptor fetch failed on device %zd.\n", i);
  807. continue;
  808. }
  809. /* we can extract device descriptor data */
  810. GPSD_LOG(LOG_INF, errout,
  811. "%04x:%04x (bus %d, device %d)\n",
  812. desc.idVendor, desc.idProduct,
  813. libusb_get_bus_number(dev),
  814. libusb_get_device_address(dev));
  815. /* we match if vendor and product ID are right */
  816. if (desc.idVendor == (uint16_t)vendor && desc.idProduct == (uint16_t)product) {
  817. found = true;
  818. break;
  819. }
  820. }
  821. GPSD_LOG(LOG_INF, errout,
  822. "vendor/product match with %04x:%04x %sfound\n",
  823. vendor, product, found ? "" : "not ");
  824. libusb_free_device_list(list, 1);
  825. libusb_exit(NULL);
  826. return found;
  827. }
  828. #endif /* HAVE_LIBUSB */
  829. /*
  830. * garmin_usb_detect() - detect a Garmin USB device connected to session fd.
  831. *
  832. * This is ONLY for USB devices reporting as: 091e:0003.
  833. *
  834. * This driver ONLY works in Linux and ONLY when the garmin_gps kernel
  835. * module is installed.
  836. *
  837. * This is only necessary because under Linux Garmin USB devices need a
  838. * kernel module rather than being normal USB-serial devices.
  839. *
  840. * The actual wire protocol from the Garmin device is very strange. There
  841. * are no delimiters. End of packet is signaled by a zero-length read
  842. * on the USB device, and start of packet is the next read. You can't just
  843. * ignore the zero reads and pass the data through - you'd never be able
  844. * to tell where the packet boundaries are.
  845. *
  846. * The garmin_usb module's job is to grab the packet and frame it in
  847. * DLEs (with DLE stuffing). This makes the USB packets look as
  848. * though they came from a regular Garmin *serial* device, which is how
  849. * most of the processing for both types can be unified here.
  850. *
  851. * return 1 is device found
  852. * return 0 if not
  853. */
  854. static bool garmin_usb_detect(struct gps_device_t *session UNUSED)
  855. {
  856. #if defined(__linux__)
  857. /*
  858. * Only perform this check if we're looking at a USB-serial
  859. * device. This prevents drivers for attached serial GPSes
  860. * fronm being rudely elbowed aside by this one if they happen
  861. * to be trying to coexist with the Garmin.
  862. */
  863. if (session->sourcetype != source_usb)
  864. return false;
  865. else {
  866. #ifdef HAVE_LIBUSB
  867. if (!is_usb_device(session->gpsdata.dev.path, 0x091e, 0x0003,
  868. &session->context->errout))
  869. return false;
  870. if (!gpsd_set_raw(session)) {
  871. GPSD_LOG(LOG_ERROR, &session->context->errout,
  872. "Garmin: garmin_usb_detect: error changing port attributes: %s\n",
  873. strerror(errno));
  874. return false;
  875. }
  876. if (sizeof(session->driver.garmin.Buffer) < sizeof(Packet_t)) {
  877. /* dunno how this happens, but it does on some compilers */
  878. GPSD_LOG(LOG_ERROR, &session->context->errout,
  879. "Garmin: garmin_usb_detect: Compile error, garmin.Buffer too small.\n");
  880. return false;
  881. }
  882. // FIXME!!! needs to use libusb totally and move garmin_gps aside */
  883. // set Mode 1, mode 0 is broken somewhere past 2.6.14
  884. // but how?
  885. GPSD_LOG(LOG_PROG, &session->context->errout,
  886. "Garmin: Set garmin_gps driver mode = 0\n");
  887. Build_Send_USB_Packet(session, GARMIN_LAYERID_PRIVATE,
  888. PRIV_PKTID_SET_MODE, 4, MODE_GARMIN_SERIAL);
  889. // expect no return packet !?
  890. return true;
  891. #else
  892. return false;
  893. #endif /* HAVE_LIBUSB */
  894. }
  895. #else
  896. return false;
  897. #endif /* __linux__ */
  898. }
  899. static void garmin_event_hook(struct gps_device_t *session, event_t event)
  900. {
  901. if (session->context->readonly)
  902. return;
  903. /*
  904. * FIX-ME: It might not be necessary to call this on reactivate.
  905. * Experiment to see if the holds its settings through a close.
  906. */
  907. if (event == event_identified || event == event_reactivate) {
  908. // Tell the device to send product data
  909. GPSD_LOG(LOG_PROG, &session->context->errout,
  910. "Garmin: Get Product Data\n");
  911. Build_Send_SER_Packet(session, GARMIN_LAYERID_APPL,
  912. GARMIN_PKTID_PRODUCT_RQST, 0, 0);
  913. // turn on PVT data 49
  914. GPSD_LOG(LOG_PROG, &session->context->errout,
  915. "Garmin: Set to send reports every 1 second\n");
  916. Build_Send_SER_Packet(session, GARMIN_LAYERID_APPL,
  917. GARMIN_PKTID_L001_COMMAND_DATA, 2,
  918. CMND_START_PVT_DATA);
  919. #if USE_RMD
  920. // turn on RMD data 110
  921. GPSD_LOG(LOG_PROG, &session->context->errout,
  922. "Garmin: Set to send Raw sat data\n");
  923. Build_Send_SER_Packet(session, GARMIN_LAYERID_APPL,
  924. GARMIN_PKTID_L001_COMMAND_DATA, 2,
  925. CMND_START_RM_DATA);
  926. #endif
  927. }
  928. if (event == event_deactivate)
  929. /* FIX-ME: is any action needed, or is closing the port sufficient? */
  930. GPSD_LOG(LOG_PROG, &session->context->errout,
  931. "Garmin: garmin_close()\n");
  932. }
  933. #define Send_ACK() Build_Send_SER_Packet(session, 0, ACK, 0, 0)
  934. #define Send_NAK() Build_Send_SER_Packet(session, 0, NAK, 0, 0)
  935. gps_mask_t garmin_ser_parse(struct gps_device_t *session)
  936. {
  937. unsigned char *buf = session->lexer.outbuffer;
  938. size_t len = session->lexer.outbuflen;
  939. unsigned char data_buf[MAX_BUFFER_SIZE];
  940. unsigned char c;
  941. int i = 0;
  942. size_t n = 0;
  943. int data_index = 0;
  944. int got_dle = 0;
  945. unsigned char pkt_id = 0;
  946. unsigned char pkt_len = 0;
  947. unsigned char chksum = 0;
  948. gps_mask_t mask = 0;
  949. struct timespec delay;
  950. GPSD_LOG(LOG_RAW, &session->context->errout,
  951. "Garmin: garmin_ser_parse()\n");
  952. if (6 > len) {
  953. /* WTF? */
  954. /* minimum packet; <DLE> [pkt id] [length=0] [chksum] <DLE> <STX> */
  955. Send_NAK();
  956. GPSD_LOG(LOG_RAW, &session->context->errout,
  957. "Garmin: serial too short: %zd\n", len);
  958. return 0;
  959. }
  960. /* debug */
  961. for (i = 0; i < (int)len; i++) {
  962. GPSD_LOG(LOG_RAW, &session->context->errout,
  963. "Garmin: Char: %#02x\n", buf[i]);
  964. }
  965. if ('\x10' != buf[0]) {
  966. Send_NAK();
  967. GPSD_LOG(LOG_RAW, &session->context->errout,
  968. "Garmin: buf[0] not DLE\n");
  969. return 0;
  970. }
  971. n = 1;
  972. pkt_id = buf[n++];
  973. chksum = pkt_id;
  974. if ('\x10' == pkt_id) {
  975. if ('\x10' != buf[n++]) {
  976. Send_NAK();
  977. GPSD_LOG(LOG_RAW, &session->context->errout,
  978. "Garmin: Bad pkt_id %#02x\n", pkt_id);
  979. return 0;
  980. }
  981. }
  982. pkt_len = buf[n++];
  983. chksum += pkt_len;
  984. if ('\x10' == pkt_len) {
  985. if ('\x10' != buf[n++]) {
  986. GPSD_LOG(LOG_RAW, &session->context->errout,
  987. "Garmin: Bad pkt_len %#02x\n", pkt_len);
  988. Send_NAK();
  989. return 0;
  990. }
  991. }
  992. data_index = 0;
  993. for (i = 0; i < 256; i++) {
  994. if ((int)pkt_len == data_index) {
  995. // got it all
  996. break;
  997. }
  998. if (len < n + i) {
  999. GPSD_LOG(LOG_RAW, &session->context->errout,
  1000. "Garmin: Packet too short %zd < %zd\n",
  1001. len, n + i);
  1002. Send_NAK();
  1003. return 0;
  1004. }
  1005. c = buf[n + i];
  1006. if (got_dle) {
  1007. got_dle = 0;
  1008. if ('\x10' != c) {
  1009. Send_NAK();
  1010. GPSD_LOG(LOG_RAW, &session->context->errout,
  1011. "Garmin: Bad DLE %#02x\n", c);
  1012. return 0;
  1013. }
  1014. } else {
  1015. chksum += c;
  1016. data_buf[data_index++] = c;
  1017. if ('\x10' == c) {
  1018. got_dle = 1;
  1019. }
  1020. }
  1021. }
  1022. /* get checksum */
  1023. if (len < n + i) {
  1024. Send_NAK();
  1025. GPSD_LOG(LOG_RAW, &session->context->errout,
  1026. "Garmin: No checksum, Packet too short %zd < %zd\n", len,
  1027. n + i);
  1028. return 0;
  1029. }
  1030. c = buf[n + i++];
  1031. chksum += c;
  1032. /* get final DLE */
  1033. if (len < n + i) {
  1034. Send_NAK();
  1035. GPSD_LOG(LOG_RAW, &session->context->errout,
  1036. "Garmin: No final DLE, Packet too short %zd < %zd\n", len,
  1037. n + i);
  1038. return 0;
  1039. }
  1040. c = buf[n + i++];
  1041. if ('\x10' != c) {
  1042. Send_NAK();
  1043. GPSD_LOG(LOG_RAW, &session->context->errout,
  1044. "Garmin: Final DLE not DLE\n");
  1045. return 0;
  1046. }
  1047. /* get final ETX */
  1048. if (len < n + i) {
  1049. Send_NAK();
  1050. GPSD_LOG(LOG_RAW, &session->context->errout,
  1051. "Garmin: No final ETX, Packet too short %zd < %zd\n", len,
  1052. n + i);
  1053. return 0;
  1054. }
  1055. /* we used to say n++ here, but scan-build complains */
  1056. c = buf[n + i];
  1057. if ('\x03' != c) {
  1058. Send_NAK();
  1059. GPSD_LOG(LOG_RAW, &session->context->errout,
  1060. "Garmin: Final ETX not ETX\n");
  1061. return 0;
  1062. }
  1063. /* debug */
  1064. for (i = 0; i < data_index; i++) {
  1065. GPSD_LOG(LOG_RAW, &session->context->errout,
  1066. "Garmin: Char: %#02x\n", data_buf[i]);
  1067. }
  1068. GPSD_LOG(LOG_DATA, &session->context->errout,
  1069. "Garmin: garmin_ser_parse() Type: %#02x, Len: %#02x, chksum: %#02x\n",
  1070. pkt_id, pkt_len, chksum);
  1071. mask = PrintSERPacket(session, pkt_id, pkt_len, data_buf);
  1072. // sending ACK too soon might hang the session
  1073. // so send ACK last, after a pause
  1074. /* wait 300 uSec */
  1075. delay.tv_sec = 0;
  1076. delay.tv_nsec = 300000L;
  1077. nanosleep(&delay, NULL);
  1078. Send_ACK();
  1079. GPSD_LOG(LOG_DATA, &session->context->errout,
  1080. "Garmin: garmin_ser_parse( )\n");
  1081. return mask;
  1082. }
  1083. #ifdef RECONFIGURE_ENABLE
  1084. static void settle(void)
  1085. {
  1086. struct timespec delay, rem;
  1087. memset(&delay, 0, sizeof(delay));
  1088. delay.tv_sec = 0;
  1089. delay.tv_nsec = 333000000L;
  1090. nanosleep(&delay, &rem);
  1091. }
  1092. static void garmin_switcher(struct gps_device_t *session, int mode)
  1093. {
  1094. if (mode == MODE_NMEA) {
  1095. const unsigned char switcher[] =
  1096. { 0x10, 0x0A, 0x02, 0x26, 0x00, 0xCE, 0x10, 0x03 };
  1097. // Note hard-coded string length in the next line...
  1098. ssize_t status = gpsd_write(session, (char *)switcher,
  1099. sizeof(switcher));
  1100. if (status == (ssize_t)sizeof(switcher)) {
  1101. GPSD_LOG(LOG_PROG, &session->context->errout,
  1102. "Garmin: => GPS: turn off binary %02x %02x %02x... \n",
  1103. switcher[0], switcher[1], switcher[2]);
  1104. } else {
  1105. GPSD_LOG(LOG_ERROR, &session->context->errout,
  1106. "Garmin: => GPS: FAILED\n");
  1107. }
  1108. settle(); // wait 333mS, essential!
  1109. /* once a sec, no binary, no averaging, NMEA 2.3, WAAS */
  1110. (void)nmea_send(session, "$PGRMC1,1,1");
  1111. //(void)nmea_send(fd, "$PGRMC1,1,1,1,,,,2,W,N");
  1112. (void)nmea_send(session, "$PGRMI,,,,,,,R");
  1113. settle(); // wait 333mS, essential!
  1114. } else {
  1115. (void)nmea_send(session, "$PGRMC1,1,2,1,,,,2,W,N");
  1116. (void)nmea_send(session, "$PGRMI,,,,,,,R");
  1117. settle(); // wait 333mS, essential!
  1118. }
  1119. }
  1120. #endif /* RECONFIGURE_ENABLE */
  1121. #ifdef CONTROLSEND_ENABLE
  1122. static ssize_t garmin_control_send(struct gps_device_t *session,
  1123. char *buf, size_t buflen)
  1124. /* not used by the daemon, it's for gpsctl and friends */
  1125. {
  1126. session->msgbuflen = buflen;
  1127. (void)memcpy(session->msgbuf, buf, buflen);
  1128. return gpsd_write(session, session->msgbuf, session->msgbuflen);
  1129. }
  1130. #endif /* CONTROLSEND_ENABLE */
  1131. static double garmin_time_offset(struct gps_device_t *session)
  1132. {
  1133. if (session->sourcetype == source_usb) {
  1134. return 0.035; /* Garmin USB, expect +/- 40mS jitter */
  1135. }
  1136. /* only two sentences ships time */
  1137. /* but the PVT data is always first */
  1138. switch (session->gpsdata.dev.baudrate) {
  1139. case 4800:
  1140. return 0.430; /* TBD */
  1141. case 9600:
  1142. return 0.430; /* tested 12Arp10 */
  1143. case 19200:
  1144. return 0.430; /* TBD */
  1145. case 38400:
  1146. return 0.430; /* TBD */
  1147. }
  1148. return 0.430; /* WTF? WAG */
  1149. }
  1150. /* this is everything we export */
  1151. /* *INDENT-OFF* */
  1152. const struct gps_type_t driver_garmin_usb_binary =
  1153. {
  1154. .type_name = "Garmin USB binary", /* full name of type */
  1155. .packet_type = GARMIN_PACKET, /* associated lexer packet type */
  1156. .flags = DRIVER_STICKY, /* remember this */
  1157. .trigger = NULL, /* no trigger, it has a probe */
  1158. .channels = GARMIN_CHANNELS, /* consumer-grade GPS */
  1159. .probe_detect = garmin_usb_detect,/* how to detect at startup time */
  1160. .get_packet = generic_get, /* how to grab a packet */
  1161. .parse_packet = garmin_ser_parse, /* parse message packets */
  1162. .rtcm_writer = NULL, /* don't send DGPS corrections */
  1163. .init_query = NULL, /* non-perturbing initial query */
  1164. .event_hook = garmin_event_hook,/* lifetime ebent handler */
  1165. #ifdef RECONFIGURE_ENABLE
  1166. .speed_switcher = NULL, /* no speed switcher */
  1167. .mode_switcher = NULL, /* Garmin USB Binary has no NMEA */
  1168. .rate_switcher = NULL, /* no sample-rate switcher */
  1169. .min_cycle.tv_sec = 0,
  1170. .min_cycle.tv_nsec = 10000000, /* 10Hz */
  1171. #endif /* RECONFIGURE_ENABLE */
  1172. #ifdef CONTROLSEND_ENABLE
  1173. .control_send = garmin_control_send, /* send raw bytes */
  1174. #endif /* CONTROLSEND_ENABLE */
  1175. .time_offset = garmin_time_offset,
  1176. };
  1177. /* *INDENT-ON* */
  1178. /* *INDENT-OFF* */
  1179. const struct gps_type_t driver_garmin_ser_binary =
  1180. {
  1181. .type_name = "Garmin Serial binary", /* full name of type */
  1182. .packet_type = GARMIN_PACKET, /* associated lexer packet type */
  1183. .flags = DRIVER_STICKY, /* remember this */
  1184. .trigger = NULL, /* no trigger, it has a probe */
  1185. .channels = GARMIN_CHANNELS, /* consumer-grade GPS */
  1186. .probe_detect = NULL, /* how to detect at startup time */
  1187. .get_packet = generic_get, /* how to grab a packet */
  1188. .parse_packet = garmin_ser_parse, /* parse message packets */
  1189. .rtcm_writer = NULL, /* don't send DGPS corrections */
  1190. .init_query = NULL, /* non-perturbing initial query */
  1191. .event_hook = NULL, /* lifetime event handler */
  1192. #ifdef RECONFIGURE_ENABLE
  1193. .speed_switcher = NULL, /* no speed switcher */
  1194. .mode_switcher = garmin_switcher, /* how to change modes */
  1195. .rate_switcher = NULL, /* no sample-rate switcher */
  1196. .min_cycle.tv_sec = 0,
  1197. .min_cycle.tv_nsec = 10000000, /* 10Hz */
  1198. #endif /* RECONFIGURE_ENABLE */
  1199. #ifdef CONTROLSEND_ENABLE
  1200. .control_send = garmin_control_send, /* send raw bytes */
  1201. #endif /* CONTROLSEND_ENABLE */
  1202. .time_offset = garmin_time_offset,
  1203. };
  1204. /* *INDENT-ON* */
  1205. #endif /* GARMIN_ENABLE */
  1206. // vim: set expandtab shiftwidth=4