driver_garmin.c 46 KB

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