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.

monitor_garmin.c 8.0KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306
  1. /*
  2. * Garmin binary object for the GPS packet monitor.
  3. *
  4. * This file is Copyright (c) 2011-2018 by the GPSD project
  5. * SPDX-License-Identifier: BSD-2-clause
  6. */
  7. #include "gpsd_config.h" /* must be before all includes */
  8. #include <assert.h>
  9. #include <math.h>
  10. #include "gpsd.h"
  11. #include "bits.h"
  12. #include "gpsmon.h"
  13. #if defined(GARMIN_ENABLE) && defined(BINARY_ENABLE)
  14. extern const struct gps_type_t driver_garmin_ser_binary;
  15. static WINDOW *miscwin, *mid51win, *mid114win;
  16. #define display (void)mvwprintw
  17. #define GARMIN_CHANNELS 12
  18. #define GPSD_LE16TOH(x) getles16((char *)(&(x)), 0)
  19. #define GPSD_LE32TOH(x) getles32((char *)(&(x)), 0)
  20. #pragma pack(1)
  21. /* Satellite Data Record */
  22. typedef struct __attribute__((__packed__))
  23. {
  24. uint8_t svid;
  25. uint16_t snr;
  26. uint8_t elev;
  27. uint16_t azmth;
  28. uint8_t status;
  29. } cpo_sat_data;
  30. /* Position Record */
  31. typedef struct __attribute__((__packed__))
  32. {
  33. float alt;
  34. float epe;
  35. float eph;
  36. float epv;
  37. int16_t fix;
  38. double gps_tow;
  39. double lat;
  40. double lon;
  41. float lon_vel;
  42. float lat_vel;
  43. float alt_vel;
  44. float msl_hght;
  45. int16_t leap_sec;
  46. int32_t grmn_days;
  47. } cpo_pvt_data;
  48. /* Receiver Measurement Record */
  49. typedef struct __attribute__((__packed__))
  50. {
  51. // cppcheck-suppress unusedStructMember
  52. uint32_t cycles;
  53. // cppcheck-suppress unusedStructMember
  54. double pr;
  55. // cppcheck-suppress unusedStructMember
  56. uint16_t phase;
  57. // cppcheck-suppress unusedStructMember
  58. int8_t slp_dtct;
  59. // cppcheck-suppress unusedStructMember
  60. uint8_t snr_dbhz;
  61. uint8_t svid;
  62. // cppcheck-suppress unusedStructMember
  63. int8_t valid;
  64. } cpo_rcv_sv_data;
  65. typedef struct __attribute__((__packed__))
  66. {
  67. // cppcheck-suppress unusedStructMember
  68. double rcvr_tow;
  69. // cppcheck-suppress unusedStructMember
  70. int16_t rcvr_wn;
  71. // cppcheck-suppress unusedStructMember
  72. cpo_rcv_sv_data sv[GARMIN_CHANNELS];
  73. } cpo_rcv_data;
  74. /* description of the status */
  75. static char *fixdesc[] = {
  76. "no fix",
  77. "no fix",
  78. "2D",
  79. "3D",
  80. "2D dif",
  81. "3D dif",
  82. };
  83. /* check range of an unsigned quantity */
  84. #define CHECK_RANGE(vec, i) ((i) < sizeof(vec)/sizeof(vec[0]))
  85. static bool garmin_bin_initialize(void)
  86. {
  87. unsigned int i;
  88. #ifndef CONTROLSEND_ENABLE
  89. if(serial) {
  90. monitor_complain("Direct mode doesn't supported.");
  91. return false;
  92. }
  93. #endif
  94. miscwin = subwin(devicewin, 1, 80, 1, 0);
  95. mid51win = subwin(devicewin, 12, 18, 2, 0);
  96. mid114win = subwin(devicewin, GARMIN_CHANNELS + 3, 23, 2, 18);
  97. if (miscwin == NULL || mid51win == NULL || mid114win == NULL)
  98. return false;
  99. (void)syncok(miscwin, true);
  100. (void)syncok(mid51win, true);
  101. (void)syncok(mid114win, true);
  102. (void)wattrset(miscwin, A_BOLD);
  103. display(miscwin, 0, 0, "Time:");
  104. (void)wattrset(miscwin, A_NORMAL);
  105. (void)wborder(mid51win, 0, 0, 0, 0, 0, 0, 0, 0),
  106. (void)wattrset(mid51win, A_BOLD);
  107. display(mid51win, 0, 4, " Position ");
  108. display(mid51win, 1, 2, "Fix:");
  109. display(mid51win, 2, 2, "Lat:");
  110. (void)mvwaddch(mid51win, 2, 16, ACS_DEGREE);
  111. display(mid51win, 3, 2, "Lon:");
  112. (void)mvwaddch(mid51win, 3, 16, ACS_DEGREE);
  113. display(mid51win, 4, 2, "Alt: m");
  114. display(mid51win, 5, 2, "Speed: m/s");
  115. display(mid51win, 6, 2, "Climb: m/s");
  116. display(mid51win, 7, 2, "Leap: sec");
  117. display(mid51win, 8, 2, "epe: m");
  118. display(mid51win, 9, 2, "eph: m");
  119. display(mid51win, 10, 2, "epv: m");
  120. display(mid51win, 11, 3, " ID 51 (0x33) ");
  121. (void)wattrset(mid51win, A_NORMAL);
  122. (void)wborder(mid114win, 0, 0, 0, 0, 0, 0, 0, 0),
  123. (void)wattrset(mid114win, A_BOLD);
  124. display(mid114win, 1, 1, "Ch PRN Az El SNR ST");
  125. for (i = 0; i < GARMIN_CHANNELS; i++) {
  126. display(mid114win, (int)i + 2, 1, "%2d", i);
  127. }
  128. display(mid114win, 0, 5, " Satellite ");
  129. display(mid114win, 14, 4, " ID 114 (0x72) ");
  130. (void)wattrset(mid114win, A_NORMAL);
  131. /* initialize the GPS context's time fields */
  132. gpsd_time_init(session.context, time(NULL));
  133. return true;
  134. }
  135. static void garmin_bin_update(uint16_t pkt_id, uint32_t pkt_size UNUSED, unsigned char *pkt_data)
  136. {
  137. int i;
  138. cpo_sat_data *sats = NULL;
  139. cpo_pvt_data *pvt = NULL;
  140. //cpo_rcv_data *rmd = NULL;
  141. char tbuf[JSON_DATE_MAX+1];
  142. switch (pkt_id) {
  143. case 0x29: /* Receiver Measurement Record */
  144. case 0x34:
  145. /* for future use */
  146. //rmd = (cpo_rcv_data *)pkt_data;
  147. monitor_log("RMD 0x%02x=", pkt_id);
  148. break;
  149. case 0x33: /* Position Record */
  150. /* coverity_submit[tainted_data] */
  151. display(miscwin, 0, 6, "%-24s",
  152. timespec_to_iso8601(session.gpsdata.fix.time, tbuf, sizeof(tbuf)));
  153. pvt = (cpo_pvt_data *)pkt_data;
  154. display(mid51win, 1, 7, "%s",
  155. (CHECK_RANGE(fixdesc, (uint8_t)GPSD_LE16TOH(pvt->fix)) ? \
  156. fixdesc[GPSD_LE16TOH(pvt->fix)] : "unknown"));
  157. display(mid51win, 2, 8, "%3.5f", pvt->lat * RAD_2_DEG);
  158. display(mid51win, 3, 8, "%3.5f", pvt->lon * RAD_2_DEG);
  159. display(mid51win, 4, 8, "%8.2f", pvt->alt + pvt->msl_hght);
  160. display(mid51win, 5, 9, "%5.1f", hypot(pvt->lon_vel, pvt->lat_vel));
  161. display(mid51win, 6, 9, "%5.1f", pvt->alt_vel);
  162. display(mid51win, 7, 8, "%d", (int)GPSD_LE16TOH(pvt->leap_sec));
  163. if (GPSD_LE16TOH(pvt->fix) < 2) /* error value is very large when status no fix */
  164. pvt->epe = pvt->eph = pvt->epv = NAN;
  165. display(mid51win, 8, 7, "%6.2f", pvt->epe);
  166. display(mid51win, 9, 7, "%6.2f", pvt->eph);
  167. display(mid51win, 10, 7, "%6.2f", pvt->epv);
  168. monitor_log("PVT 0x%02x=", pkt_id);
  169. break;
  170. case 0x72: /* Satellite Data Record */
  171. sats = (cpo_sat_data *)pkt_data;
  172. for (i = 0; i < GARMIN_CHANNELS; i++, sats++) {
  173. display(mid114win, i + 2, 3, " %3u %3u %2u %4.1f %2x",
  174. sats->svid,
  175. GPSD_LE16TOH(sats->azmth),
  176. sats->elev,
  177. (float)GPSD_LE16TOH(sats->snr) / 100.0,
  178. sats->status);
  179. }
  180. monitor_log("SAT 0x%02x=", pkt_id);
  181. break;
  182. case 0xff: /* Product Data Record */
  183. monitor_log("PDR 0x%02x=", pkt_id);
  184. break;
  185. default:
  186. monitor_log("UNK 0x%02x=", pkt_id);
  187. break;
  188. }
  189. }
  190. static void garmin_bin_ser_update(void)
  191. {
  192. unsigned char *buf;
  193. size_t len;
  194. uint16_t pkt_id;
  195. uint32_t pkt_size;
  196. unsigned char pkt_data[255];
  197. int i, j;
  198. unsigned char c;
  199. unsigned char chksum;
  200. bool pkt_good = false, got_dle = false;
  201. buf = session.lexer.outbuffer;
  202. len = session.lexer.outbuflen;
  203. if (!(buf[0] == (unsigned char)0x10 && /* DLE */
  204. buf[len-2] == (unsigned char)0x10 && /* DLE */
  205. buf[len-1] == (unsigned char)0x03)) /* ETX */
  206. goto end; /* bad pkt */
  207. if (buf[1] == (unsigned char)0x10 && buf[2] != (unsigned char)0x10)
  208. goto end; /* bad pkt */
  209. pkt_id = (uint16_t)buf[1];
  210. if (buf[2] == (unsigned char)0x10 && buf[3] != (unsigned char)0x10)
  211. goto end; /* bad pkt */
  212. pkt_size = (uint32_t)buf[2];
  213. chksum = buf[1] + buf[2]; /* pkt_id + pkt_size */
  214. j = 0;
  215. for (i = 0; i <= 255; i++) {
  216. if (pkt_size == (uint32_t)j)
  217. break;
  218. c = buf[i+3];
  219. if (got_dle) {
  220. got_dle = false;
  221. if (c != (unsigned char)0x10)
  222. goto end; /* bad pkt */
  223. } else {
  224. pkt_data[j++] = c;
  225. chksum += c;
  226. if (c == (unsigned char)0x10)
  227. got_dle = true;
  228. }
  229. }
  230. /* check pkt chksum */
  231. if ((unsigned char)(-chksum) == buf[len-3])
  232. pkt_good = true;
  233. end:
  234. if (pkt_good) {
  235. #ifdef CONTROLSEND_ENABLE
  236. if(serial)
  237. /* good packet, send ACK */
  238. (void)monitor_control_send((unsigned char *)"\x10\x06\x00\xfa\x10\x03", 6);
  239. #endif
  240. garmin_bin_update(pkt_id, pkt_size, pkt_data);
  241. } else {
  242. #ifdef CONTROLSEND_ENABLE
  243. if(serial)
  244. /* bad packet, send NAK */
  245. (void)monitor_control_send((unsigned char *)"\x10\x15\x00\xeb\x10\x03", 6);
  246. #endif
  247. monitor_log("BAD 0x%02x=", buf[1]);
  248. }
  249. }
  250. static void garmin_bin_wrap(void)
  251. {
  252. (void)delwin(miscwin);
  253. (void)delwin(mid51win);
  254. (void)delwin(mid114win);
  255. }
  256. const struct monitor_object_t garmin_bin_ser_mmt = {
  257. .initialize = garmin_bin_initialize,
  258. .update = garmin_bin_ser_update,
  259. .command = NULL,
  260. .wrap = garmin_bin_wrap,
  261. .min_y = 16,.min_x = 80,
  262. .driver = &driver_garmin_ser_binary,
  263. };
  264. #endif /* defined(GARMIN_ENABLE) && defined(BINARY_ENABLE) */