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

278 lines
8.2 KiB

  1. /*
  2. * This file is Copyright (c) 2010-2018 by the GPSD project
  3. * SPDX-License-Identifier: BSD-2-clause
  4. */
  5. #include "gpsd_config.h" /* must be before all includes */
  6. #include <math.h>
  7. #include <stdlib.h> /* for labs() */
  8. #include <time.h>
  9. #include "gpsd.h"
  10. #include "bits.h"
  11. #include "gpsmon.h"
  12. #ifdef UBLOX_ENABLE
  13. #include "driver_ubx.h"
  14. extern const struct gps_type_t driver_ubx;
  15. static WINDOW *satwin, *navsolwin, *dopwin, *ppswin;
  16. #define display (void)mvwprintw
  17. static bool ubx_initialize(void)
  18. {
  19. int i;
  20. /* "heavily inspired" by monitor_nmea.c */
  21. if ((satwin = derwin(devicewin, 19, 28, 0, 0)) == NULL)
  22. return false;
  23. (void)wborder(satwin, 0, 0, 0, 0, 0, 0, 0, 0), (void)syncok(satwin, true);
  24. (void)wattrset(satwin, A_BOLD);
  25. display(satwin, 1, 1, "Ch PRN Az El S/N Flag U");
  26. for (i = 0; i < 16; i++)
  27. display(satwin, (int)(i + 2), 1, "%2d", i);
  28. display(satwin, 18, 7, " NAV_SVINFO ");
  29. (void)wattrset(satwin, A_NORMAL);
  30. /* "heavily inspired" by monitor_nmea.c */
  31. if ((navsolwin = derwin(devicewin, 13, 51, 0, 28)) == NULL)
  32. return false;
  33. (void)wborder(navsolwin, 0, 0, 0, 0, 0, 0, 0, 0),
  34. (void)wattrset(navsolwin, A_BOLD);
  35. (void)wmove(navsolwin, 1, 1);
  36. (void)wprintw(navsolwin, "ECEF Pos:");
  37. (void)wmove(navsolwin, 2, 1);
  38. (void)wprintw(navsolwin, "ECEF Vel:");
  39. (void)wmove(navsolwin, 4, 1);
  40. (void)wprintw(navsolwin, "LTP Pos:");
  41. (void)wmove(navsolwin, 5, 1);
  42. (void)wprintw(navsolwin, "LTP Vel:");
  43. (void)wmove(navsolwin, 7, 1);
  44. (void)wprintw(navsolwin, "Time:");
  45. (void)wmove(navsolwin, 8, 1);
  46. (void)wprintw(navsolwin, "Time GPS: Day:");
  47. (void)wmove(navsolwin, 10, 1);
  48. (void)wprintw(navsolwin, "Est Pos Err m Est Vel Err m/s");
  49. (void)wmove(navsolwin, 11, 1);
  50. (void)wprintw(navsolwin, "PRNs: ## PDOP: xx.x Fix 0x.. Flags 0x..");
  51. display(navsolwin, 12, 20, " NAV_SOL ");
  52. (void)wattrset(navsolwin, A_NORMAL);
  53. if ((dopwin = derwin(devicewin, 3, 51, 13, 28)) == NULL)
  54. return false;
  55. (void)wborder(dopwin, 0, 0, 0, 0, 0, 0, 0, 0);
  56. (void)wattrset(dopwin, A_BOLD);
  57. (void)wmove(dopwin, 1, 1);
  58. (void)wprintw(dopwin, "DOP [H] [V] [P] [T] [G]");
  59. display(dopwin, 2, 20, " NAV_DOP ");
  60. (void)wattrset(dopwin, A_NORMAL);
  61. if ((ppswin = derwin(devicewin, 3, 51, 16, 28)) == NULL)
  62. return false;
  63. (void)wborder(ppswin, 0, 0, 0, 0, 0, 0, 0, 0);
  64. (void)syncok(ppswin, true);
  65. (void)wattrset(ppswin, A_BOLD);
  66. #define TOFF_LINE 1
  67. #define TOFF_COLUMN 1
  68. (void)mvwaddstr(ppswin, TOFF_LINE, TOFF_COLUMN, "TOFF: ");
  69. (void)mvwaddstr(ppswin, TOFF_LINE, TOFF_COLUMN + 10, "N/A");
  70. #define PPS_LINE 1
  71. #define PPS_COLUMN 26
  72. (void)mvwaddstr(ppswin, PPS_LINE, PPS_COLUMN, "PPS: ");
  73. (void)mvwaddstr(ppswin, PPS_LINE, PPS_COLUMN + 10, "N/A");
  74. (void)wattrset(ppswin, A_NORMAL);
  75. return true;
  76. }
  77. static void display_nav_svinfo(unsigned char *buf, size_t data_len)
  78. {
  79. int i, nchan;
  80. if (data_len < 152)
  81. return;
  82. nchan = (int)getub(buf, 4);
  83. if (nchan > 16)
  84. nchan = 16;
  85. for (i = 0; i < nchan; i++) {
  86. int off = 8 + 12 * i;
  87. unsigned char ss, prn;
  88. char el;
  89. short az;
  90. unsigned short fl;
  91. prn = (unsigned char)getub(buf, off + 1);
  92. fl = (unsigned short)getleu16(buf, off + 2);
  93. ss = (unsigned char)getub(buf, off + 4);
  94. el = (char)getsb(buf, off + 5);
  95. az = (short)getles16(buf, off + 6);
  96. (void)wmove(satwin, (int)(i + 2), 4);
  97. (void)wprintw(satwin, "%3d %3d %3d %2d %04x %c",
  98. prn, az, el, ss, fl, (fl & UBX_SAT_USED) ? 'Y' : ' ');
  99. }
  100. (void)wnoutrefresh(satwin);
  101. return;
  102. }
  103. static void display_nav_sol(unsigned char *buf, size_t data_len)
  104. {
  105. unsigned short gw = 0;
  106. unsigned int tow = 0, flags;
  107. double epx, epy, epz, evx, evy, evz;
  108. unsigned char navmode;
  109. struct gps_data_t g;
  110. if (data_len != 52)
  111. return;
  112. navmode = (unsigned char)getub(buf, 10);
  113. flags = (unsigned int)getub(buf, 11);
  114. if ((flags & (UBX_SOL_VALID_WEEK | UBX_SOL_VALID_TIME)) != 0) {
  115. tow = (unsigned int)getleu32(buf, 0);
  116. gw = (unsigned short)getles16(buf, 8);
  117. }
  118. epx = (double)(getles32(buf, 12) / 100.0);
  119. epy = (double)(getles32(buf, 16) / 100.0);
  120. epz = (double)(getles32(buf, 20) / 100.0);
  121. evx = (double)(getles32(buf, 28) / 100.0);
  122. evy = (double)(getles32(buf, 32) / 100.0);
  123. evz = (double)(getles32(buf, 36) / 100.0);
  124. (void)ecef_to_wgs84fix(&g.fix, epx, epy, epz, evx, evy, evz);
  125. /* maybe should check the ecef_to_wgs84fix() return code? */
  126. g.fix.epx = g.fix.epy = (double)(getles32(buf, 24) / 100.0);
  127. g.fix.eps = (double)(getles32(buf, 40) / 100.0);
  128. g.dop.pdop = (double)(getleu16(buf, 44) / 100.0);
  129. g.satellites_used = (int)getub(buf, 47);
  130. (void)wmove(navsolwin, 1, 11);
  131. (void)wprintw(navsolwin, "%+10.2fm %+10.2fm %+10.2fm", epx, epy, epz);
  132. (void)wmove(navsolwin, 2, 11);
  133. (void)wprintw(navsolwin, "%+9.2fm/s %+9.2fm/s %+9.2fm/s", evx, evy, evz);
  134. (void)wmove(navsolwin, 4, 11);
  135. (void)wattrset(navsolwin, A_UNDERLINE);
  136. (void)wprintw(navsolwin, "%12.9f %13.9f %8.2fm",
  137. g.fix.latitude, g.fix.longitude, g.fix.altHAE);
  138. (void)mvwaddch(navsolwin, 4, 23, ACS_DEGREE);
  139. (void)mvwaddch(navsolwin, 4, 38, ACS_DEGREE);
  140. (void)wmove(navsolwin, 5, 11);
  141. (void)wprintw(navsolwin, "%6.2fm/s %5.1fo %6.2fm/s",
  142. g.fix.speed, g.fix.track, g.fix.climb);
  143. (void)mvwaddch(navsolwin, 5, 26, ACS_DEGREE);
  144. (void)wattrset(navsolwin, A_NORMAL);
  145. (void)wmove(navsolwin, 7, 7);
  146. {
  147. unsigned int day = tow / 86400000;
  148. unsigned int tod = tow % 86400000;
  149. unsigned int h = tod / 3600000;
  150. unsigned int m = tod % 3600000;
  151. unsigned int s = m % 60000;
  152. m = (m - s) / 60000;
  153. (void)wattrset(navsolwin, A_UNDERLINE);
  154. (void)wprintw(navsolwin, "%u %02u:%02u:%05.2f", day, h, m, (double)s / 1000);
  155. (void)wattrset(navsolwin, A_NORMAL);
  156. }
  157. (void)wmove(navsolwin, 8, 11);
  158. if ((flags & (UBX_SOL_VALID_WEEK | UBX_SOL_VALID_TIME)) != 0) {
  159. (void)wprintw(navsolwin, "%d+%10.3lf", gw, (double)(tow / 1000.0));
  160. (void)wmove(navsolwin, 8, 36);
  161. (void)wprintw(navsolwin, "%d", (tow / 86400000));
  162. }
  163. /* relies on the fact that epx and epy are set to same value */
  164. (void)wmove(navsolwin, 10, 12);
  165. (void)wprintw(navsolwin, "%7.2f", g.fix.epx);
  166. (void)wmove(navsolwin, 10, 33);
  167. (void)wprintw(navsolwin, "%6.2f", g.fix.epv);
  168. (void)wmove(navsolwin, 11, 7);
  169. (void)wprintw(navsolwin, "%2d", g.satellites_used);
  170. (void)wmove(navsolwin, 11, 15);
  171. (void)wprintw(navsolwin, "%5.1f", g.dop.pdop);
  172. (void)wmove(navsolwin, 11, 25);
  173. (void)wprintw(navsolwin, "0x%02x", navmode);
  174. (void)wmove(navsolwin, 11, 36);
  175. (void)wprintw(navsolwin, "0x%02x", flags);
  176. (void)wnoutrefresh(navsolwin);
  177. }
  178. static void display_nav_dop(unsigned char *buf, size_t data_len)
  179. {
  180. if (data_len != 18)
  181. return;
  182. (void)wmove(dopwin, 1, 9);
  183. (void)wprintw(dopwin, "%4.1f", getleu16(buf, 12) / 100.0);
  184. (void)wmove(dopwin, 1, 18);
  185. (void)wprintw(dopwin, "%4.1f", getleu16(buf, 10) / 100.0);
  186. (void)wmove(dopwin, 1, 27);
  187. (void)wprintw(dopwin, "%4.1f", getleu16(buf, 6) / 100.0);
  188. (void)wmove(dopwin, 1, 36);
  189. (void)wprintw(dopwin, "%4.1f", getleu16(buf, 8) / 100.0);
  190. (void)wmove(dopwin, 1, 45);
  191. (void)wprintw(dopwin, "%4.1f", getleu16(buf, 4) / 100.0);
  192. (void)wnoutrefresh(dopwin);
  193. }
  194. static void ubx_update(void)
  195. {
  196. unsigned char *buf;
  197. size_t data_len;
  198. unsigned short msgid;
  199. buf = session.lexer.outbuffer;
  200. msgid = (unsigned short)((buf[2] << 8) | buf[3]);
  201. data_len = (size_t) getles16(buf, 4);
  202. switch (msgid) {
  203. case UBX_NAV_SVINFO:
  204. /* coverity_submit[tainted_data] */
  205. display_nav_svinfo(&buf[6], data_len);
  206. break;
  207. case UBX_NAV_DOP:
  208. display_nav_dop(&buf[6], data_len);
  209. break;
  210. case UBX_NAV_SOL:
  211. display_nav_sol(&buf[6], data_len);
  212. break;
  213. default:
  214. break;
  215. }
  216. toff_update(ppswin, TOFF_LINE, TOFF_COLUMN + 6);
  217. pps_update(ppswin, PPS_LINE, PPS_COLUMN + 5);
  218. }
  219. static int ubx_command(char line[]UNUSED)
  220. {
  221. return COMMAND_UNKNOWN;
  222. }
  223. static void ubx_wrap(void)
  224. {
  225. (void)delwin(satwin);
  226. return;
  227. }
  228. const struct monitor_object_t ubx_mmt = {
  229. .initialize = ubx_initialize,
  230. .update = ubx_update,
  231. .command = ubx_command,
  232. .wrap = ubx_wrap,
  233. .min_y = 19,.min_x = 80, /* size of the device window */
  234. .driver = &driver_ubx,
  235. };
  236. #endif