driver_skytraq.c 20 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705
  1. /*
  2. * This is the gpsd driver for Skytraq GPSes operating in binary mode.
  3. *
  4. * SkyTraq is Big Endian
  5. *
  6. * This file is Copyright (c) 2016-2018 by the GPSD project
  7. * SPDX-License-Identifier: BSD-2-clause
  8. */
  9. #include "gpsd_config.h" /* must be before all includes */
  10. #include <ctype.h>
  11. #include <math.h>
  12. #include <stdbool.h>
  13. #include <stdio.h>
  14. #include <stdlib.h>
  15. #include <string.h> /* for strlcpy() */
  16. #include <strings.h>
  17. #include <unistd.h>
  18. #include "gpsd.h"
  19. #include "bits.h"
  20. #include "strfuncs.h"
  21. #if defined(SKYTRAQ_ENABLE)
  22. #include "timespec.h"
  23. #define HI(n) ((n) >> 8)
  24. #define LO(n) ((n) & 0xff)
  25. /*
  26. * No ACK/NAK? Just rety after 6 seconds
  27. */
  28. #define SKY_RETRY_TIME 6
  29. #define SKY_CHANNELS 48 /* max channels allowed in format */
  30. static gps_mask_t sky_parse(struct gps_device_t *, unsigned char *, size_t);
  31. static gps_mask_t sky_msg_80(struct gps_device_t *, unsigned char *, size_t);
  32. static gps_mask_t sky_msg_DC(struct gps_device_t *, unsigned char *, size_t);
  33. static gps_mask_t sky_msg_DD(struct gps_device_t *, unsigned char *, size_t);
  34. static gps_mask_t sky_msg_DE(struct gps_device_t *, unsigned char *, size_t);
  35. static gps_mask_t sky_msg_DF(struct gps_device_t *, unsigned char *, size_t);
  36. static gps_mask_t sky_msg_E0(struct gps_device_t *, unsigned char *, size_t);
  37. static gps_mask_t sky_msg_E2(struct gps_device_t *, unsigned char *, size_t);
  38. static gps_mask_t sky_msg_E3(struct gps_device_t *, unsigned char *, size_t);
  39. #ifdef __UNUSED
  40. /* Poll Software Version MID 2 */
  41. static unsigned char versionprobe[] = {
  42. 0xa0, 0xa1, 0x00, 0x02,
  43. 0x02, /* MID 2 */
  44. 0x01, /* System */
  45. 0x00, 0x0d, 0x0a
  46. };
  47. // cppcheck-suppress unusedFunction
  48. static bool sky_write(struct gps_device_t *session, unsigned char *msg)
  49. {
  50. unsigned int crc;
  51. size_t i, len;
  52. bool ok;
  53. unsigned int type = (unsigned int)msg[4];
  54. len = (size_t) ((msg[2] << 8) | msg[3]);
  55. /* calculate Checksum */
  56. crc = 0;
  57. /* coverity_submit[tainted_data] */
  58. for (i = 0; i < len; i++)
  59. crc ^= (int)msg[4 + i];
  60. /* enter CRC after payload */
  61. msg[len + 4] = (unsigned char)(crc & 0x00ff);
  62. GPSD_LOG(LOG_PROG, &session->context->errout,
  63. "Skytraq: Writing control type %02x:\n", type);
  64. ok = (gpsd_write(session, (const char *)msg, len+7) == (ssize_t) (len+7));
  65. return (ok);
  66. }
  67. #endif /* __UNUSED */
  68. /*
  69. * Convert PRN to gnssid and svid
  70. */
  71. static void PRN2_gnssId_svId(short PRN, uint8_t *gnssId, uint8_t *svId)
  72. {
  73. /* fit into gnssid:svid */
  74. if (0 == PRN) {
  75. /* skip 0 PRN */
  76. *gnssId = 0;
  77. *svId = 0;
  78. } else if ((1 <= PRN) && (32 >= PRN)) {
  79. /* GPS */
  80. *gnssId = 0;
  81. *svId = PRN;
  82. } else if ((65 <= PRN) && (96 >= PRN)) {
  83. /* GLONASS */
  84. *gnssId = 6;
  85. *svId = PRN - 64;
  86. } else if ((120 <= PRN) && (158 >= PRN)) {
  87. /* SBAS */
  88. *gnssId = 1;
  89. *svId = PRN;
  90. } else if ((201 <= PRN) && (239 >= PRN)) {
  91. /* BeiDou */
  92. *gnssId = 3;
  93. *svId = PRN - 200;
  94. } else if ((240 <= PRN) && (254 >= PRN)) {
  95. /* IRNSS */
  96. *gnssId = 20;
  97. *svId = PRN - 240;
  98. } else {
  99. /* huh? */
  100. *gnssId = 0;
  101. *svId = 0;
  102. }
  103. return;
  104. }
  105. /*
  106. * decode MID 0x80, Software Version
  107. *
  108. * 10 bytes
  109. */
  110. static gps_mask_t sky_msg_80(struct gps_device_t *session,
  111. unsigned char *buf, size_t len)
  112. {
  113. unsigned int kver_x; /* kernel version */
  114. unsigned int kver_y; /* kernel version */
  115. unsigned int kver_z; /* kernel version */
  116. unsigned int over_x; /* ODM version */
  117. unsigned int over_y; /* ODM version */
  118. unsigned int over_z; /* ODM version */
  119. unsigned int rev_yy; /* revision */
  120. unsigned int rev_mm; /* revision */
  121. unsigned int rev_dd; /* revision */
  122. if ( 14 != len)
  123. return 0;
  124. kver_x = getbeu16(buf, 2);
  125. kver_y = getub(buf, 4);
  126. kver_z = getub(buf, 5);
  127. over_x = getbeu16(buf, 6);
  128. over_y = getub(buf, 8);
  129. over_z = getub(buf, 9);
  130. rev_yy = getbeu16(buf, 10);
  131. rev_mm = getub(buf, 12);
  132. rev_dd = getub(buf, 13);
  133. (void)snprintf(session->subtype, sizeof(session->subtype) - 1,
  134. "kver=%3u.%2u,%2u, over=%3u.%2u,%2u, rev=%02u.%02u.%02u",
  135. kver_x, kver_y, kver_z,
  136. over_x, over_y, over_z,
  137. rev_yy, rev_mm, rev_dd);
  138. GPSD_LOG(LOG_DATA, &session->context->errout,
  139. "Skytraq: MID 0x80: kver=%u.%u,%u, over=%u.%u,%u, rev=%u.%u.%u\n",
  140. kver_x, kver_y, kver_z,
  141. over_x, over_y, over_z,
  142. rev_yy, rev_mm, rev_dd);
  143. return 0;
  144. }
  145. /*
  146. * decode MID 0xDC, Measurement Time
  147. *
  148. * 10 bytes
  149. */
  150. static gps_mask_t sky_msg_DC(struct gps_device_t *session,
  151. unsigned char *buf, size_t len)
  152. {
  153. unsigned int iod; /* Issue of data 0 - 255 */
  154. unsigned int wn; /* week number 0 - 65535 */
  155. unsigned int tow; /* receiver tow 0 - 604799999 in mS */
  156. unsigned int mp; /* measurement period 1 - 1000 ms */
  157. char ts_buf[TIMESPEC_LEN];
  158. timespec_t ts_tow;
  159. if ( 10 != len)
  160. return 0;
  161. iod = (unsigned int)getub(buf, 1);
  162. wn = getbeu16(buf, 2);
  163. tow = getbeu32(buf, 4);
  164. mp = getbeu16(buf, 8);
  165. MSTOTS(&ts_tow, tow);
  166. /* should this be newdata.skyview_time? */
  167. session->gpsdata.skyview_time = gpsd_gpstime_resolv(session, wn, ts_tow);
  168. GPSD_LOG(LOG_DATA, &session->context->errout,
  169. "Skytraq: MID 0xDC: iod %u wn %u tow %u mp %u t%s\n",
  170. iod, wn, tow, mp,
  171. timespec_str(&session->gpsdata.skyview_time, ts_buf,
  172. sizeof(ts_buf)));
  173. return 0;
  174. }
  175. /*
  176. * decode MID 0xDD, Raw Measurements
  177. *
  178. */
  179. static gps_mask_t sky_msg_DD(struct gps_device_t *session,
  180. unsigned char *buf, size_t len UNUSED)
  181. {
  182. unsigned int iod; /* Issue of data 0 - 255 */
  183. unsigned int nmeas; /* number of measurements */
  184. unsigned int i; /* generic loop variable */
  185. iod = (unsigned int)getub(buf, 1);
  186. nmeas = (unsigned int)getub(buf, 2);
  187. GPSD_LOG(LOG_DATA, &session->context->errout,
  188. "Skytraq: MID 0xDD: iod=%u, nmeas=%u\n",
  189. iod, nmeas);
  190. /* check IOD? */
  191. session->gpsdata.raw.mtime = session->gpsdata.skyview_time;
  192. /* zero the measurement data */
  193. /* so we can tell which meas never got set */
  194. memset(session->gpsdata.raw.meas, 0, sizeof(session->gpsdata.raw.meas));
  195. for (i = 0; i < nmeas; i++) {
  196. const char *obs_code;
  197. int off = 3 + (23 * i);
  198. uint8_t PRN = getub(buf, off + 0);
  199. /* carrier-to-noise density ratio dB-Hz */
  200. uint8_t cno = getub(buf, off + 1);
  201. /* psuedorange in meters */
  202. double prMes = getbed64((const char *)buf, off + 2);
  203. /* carrier phase in cycles */
  204. double cpMes = getbed64((const char *)buf, off + 10);
  205. /* doppler in Hz, positive towards sat */
  206. double doMes = getbef32((const char *)buf, off + 18);
  207. /* tracking stat
  208. * bit 0 - prMes valid
  209. * bit 1 - doppler valid
  210. * bit 2 - cpMes valid
  211. * bit 3 - cp slip
  212. * bit 4 - Coherent integration time?
  213. */
  214. uint8_t trkStat = getub(buf, off + 22);
  215. uint8_t gnssId = 0;
  216. uint8_t svId = 0;
  217. PRN2_gnssId_svId(PRN, &gnssId, &svId);
  218. session->gpsdata.raw.meas[i].gnssid = gnssId;
  219. switch (gnssId) {
  220. case 0: /* GPS */
  221. case 5: /* QZSS */
  222. case 20: /* IRNSS, just guessing here */
  223. obs_code = "L1C"; /* u-blox calls this L1C/A */
  224. break;
  225. case 1: /* SBAS */
  226. svId -= 100; /* adjust for RINEX 3 svid */
  227. obs_code = "L1C"; /* u-blox calls this L1C/A */
  228. break;
  229. case 2: /* GALILEO */
  230. obs_code = "L1B"; /* u-blox calls this E1OS */
  231. break;
  232. case 3: /* BeiDou */
  233. obs_code = "L2I"; /* u-blox calls this B1I */
  234. break;
  235. default: /* huh? */
  236. case 4: /* IMES. really? */
  237. obs_code = ""; /* u-blox calls this L1 */
  238. break;
  239. case 6: /* GLONASS */
  240. obs_code = "L1C"; /* u-blox calls this L1OF */
  241. break;
  242. }
  243. (void)strlcpy(session->gpsdata.raw.meas[i].obs_code, obs_code,
  244. sizeof(session->gpsdata.raw.meas[i].obs_code));
  245. session->gpsdata.raw.meas[i].svid = svId;
  246. session->gpsdata.raw.meas[i].snr = cno;
  247. session->gpsdata.raw.meas[i].satstat = trkStat;
  248. if (trkStat & 1) {
  249. /* prMes valid */
  250. session->gpsdata.raw.meas[i].pseudorange = prMes;
  251. } else {
  252. session->gpsdata.raw.meas[i].pseudorange = NAN;
  253. }
  254. if (trkStat & 2) {
  255. /* doppler valid */
  256. session->gpsdata.raw.meas[i].doppler = doMes;
  257. } else {
  258. session->gpsdata.raw.meas[i].doppler = NAN;
  259. }
  260. if (trkStat & 4) {
  261. /* cpMes valid */
  262. session->gpsdata.raw.meas[i].carrierphase = cpMes;
  263. } else {
  264. session->gpsdata.raw.meas[i].carrierphase = NAN;
  265. }
  266. session->gpsdata.raw.meas[i].codephase = NAN;
  267. session->gpsdata.raw.meas[i].deltarange = NAN;
  268. /* skytraq does not report locktime, so assume max */
  269. session->gpsdata.raw.meas[i].locktime = LOCKMAX;
  270. if (trkStat & 8) {
  271. /* possible slip */
  272. session->gpsdata.raw.meas[i].lli = 2;
  273. }
  274. GPSD_LOG(LOG_DATA, &session->context->errout,
  275. "PRN %u (%u:%u) prMes %f cpMes %f doMes %f\n"
  276. "cno %u rtkStat %u\n", PRN,
  277. gnssId, svId, prMes, cpMes, doMes, cno, trkStat);
  278. }
  279. // return RAW_IS; /* WIP */
  280. return 0;
  281. }
  282. /*
  283. * decode MID 0xDE, SV and channel status
  284. *
  285. * max payload: 3 + (Num_sats * 10) = 483 bytes
  286. */
  287. static gps_mask_t sky_msg_DE(struct gps_device_t *session,
  288. unsigned char *buf, size_t len UNUSED)
  289. {
  290. int st, nsv;
  291. unsigned int i;
  292. unsigned int iod; /* Issue of data 0 - 255 */
  293. unsigned int nsvs; /* number of SVs in this packet */
  294. iod = (unsigned int)getub(buf, 1);
  295. nsvs = (unsigned int)getub(buf, 2);
  296. /* too many sats? */
  297. if ( SKY_CHANNELS < nsvs )
  298. return 0;
  299. gpsd_zero_satellites(&session->gpsdata);
  300. for (i = st = nsv = 0; i < nsvs; i++) {
  301. int off = 3 + (10 * i); /* offset into buffer of start of this sat */
  302. bool good; /* do we have a good record ? */
  303. unsigned short sv_stat;
  304. unsigned short chan_stat;
  305. unsigned short ura;
  306. short PRN;
  307. uint8_t gnssId = 0;
  308. uint8_t svId = 0;
  309. PRN = (short)getub(buf, off + 1);
  310. /* fit into gnssid:svid */
  311. if (0 == PRN) {
  312. /* skip 0 PRN */
  313. continue;
  314. }
  315. PRN2_gnssId_svId(PRN, &gnssId, &svId);
  316. session->gpsdata.skyview[st].gnssid = gnssId;
  317. session->gpsdata.skyview[st].svid = svId;
  318. session->gpsdata.skyview[st].PRN = PRN;
  319. sv_stat = (unsigned short)getub(buf, off + 2);
  320. ura = (unsigned short)getub(buf, off + 3);
  321. session->gpsdata.skyview[st].ss = (double)getub(buf, off + 4);
  322. session->gpsdata.skyview[st].elevation =
  323. (double)getbes16(buf, off + 5);
  324. session->gpsdata.skyview[st].azimuth =
  325. (double)getbes16(buf, off + 7);
  326. chan_stat = (unsigned short)getub(buf, off + 9);
  327. session->gpsdata.skyview[st].used = (bool)(chan_stat & 0x30);
  328. good = session->gpsdata.skyview[st].PRN != 0 &&
  329. session->gpsdata.skyview[st].azimuth != 0 &&
  330. session->gpsdata.skyview[st].elevation != 0;
  331. GPSD_LOG(LOG_DATA, &session->context->errout,
  332. "Skytraq: PRN=%2d El=%4.0f Az=%5.0f ss=%3.2f stat=%02x,%02x "
  333. "ura=%d %c\n",
  334. session->gpsdata.skyview[st].PRN,
  335. session->gpsdata.skyview[st].elevation,
  336. session->gpsdata.skyview[st].azimuth,
  337. session->gpsdata.skyview[st].ss,
  338. chan_stat, sv_stat, ura,
  339. good ? '*' : ' ');
  340. if ( good ) {
  341. st += 1;
  342. if (session->gpsdata.skyview[st].used)
  343. nsv++;
  344. }
  345. }
  346. session->gpsdata.satellites_visible = st;
  347. session->gpsdata.satellites_used = nsv;
  348. GPSD_LOG(LOG_DATA, &session->context->errout,
  349. "Skytraq: MID 0xDE: nsvs=%u visible=%u iod=%u\n", nsvs,
  350. session->gpsdata.satellites_visible, iod);
  351. return SATELLITE_SET | USED_IS;
  352. }
  353. /*
  354. * decode MID 0xDF, Nav status (PVT)
  355. *
  356. * 81 bytes
  357. */
  358. static gps_mask_t sky_msg_DF(struct gps_device_t *session,
  359. unsigned char *buf, size_t len)
  360. {
  361. unsigned int iod; /* Issue of data 0 - 255 */
  362. unsigned short navstat;
  363. unsigned int wn; /* week number 0 - 65535 */
  364. double f_tow; /* receiver tow Sec */
  365. double clock_bias;
  366. double clock_drift;
  367. gps_mask_t mask = 0;
  368. timespec_t ts_tow;
  369. char ts_buf[TIMESPEC_LEN];
  370. if ( 81 != len)
  371. return 0;
  372. iod = (unsigned int)getub(buf, 1);
  373. /* fix status is byte 2 */
  374. navstat = (unsigned short)getub(buf, 2);
  375. session->gpsdata.status = STATUS_NO_FIX;
  376. session->newdata.mode = MODE_NO_FIX;
  377. switch ( navstat ) {
  378. case 1:
  379. /* fix prediction, ignore */
  380. break;
  381. case 2:
  382. session->gpsdata.status = STATUS_FIX;
  383. session->newdata.mode = MODE_2D;
  384. break;
  385. case 3:
  386. session->gpsdata.status = STATUS_FIX;
  387. session->newdata.mode = MODE_3D;
  388. break;
  389. case 4:
  390. session->gpsdata.status = STATUS_DGPS_FIX;
  391. session->newdata.mode = MODE_3D;
  392. break;
  393. default:
  394. break;
  395. }
  396. wn = getbeu16(buf, 3);
  397. f_tow = getbed64((const char *)buf, 5);
  398. DTOTS(&ts_tow, f_tow);
  399. /* position/velocity is bytes 13-48, meters and m/s */
  400. session->newdata.ecef.x = (double)getbed64((const char *)buf, 13),
  401. session->newdata.ecef.y = (double)getbed64((const char *)buf, 21),
  402. session->newdata.ecef.z = (double)getbed64((const char *)buf, 29),
  403. session->newdata.ecef.vx = (double)getbef32((const char *)buf, 37),
  404. session->newdata.ecef.vy = (double)getbef32((const char *)buf, 41),
  405. session->newdata.ecef.vz = (double)getbef32((const char *)buf, 45);
  406. mask |= ECEF_SET | VECEF_SET;
  407. clock_bias = getbed64((const char *)buf, 49);
  408. clock_drift = getbes32(buf, 57);
  409. session->gpsdata.dop.gdop = getbef32((const char *)buf, 61);
  410. session->gpsdata.dop.pdop = getbef32((const char *)buf, 65);
  411. session->gpsdata.dop.hdop = getbef32((const char *)buf, 69);
  412. session->gpsdata.dop.vdop = getbef32((const char *)buf, 73);
  413. session->gpsdata.dop.tdop = getbef32((const char *)buf, 77);
  414. session->newdata.time = gpsd_gpstime_resolv(session, wn, ts_tow );
  415. GPSD_LOG(LOG_DATA, &session->context->errout,
  416. "Skytraq: MID 0xDF: iod=%u, stat=%u, wn=%u, tow=%f, t=%s "
  417. "cb: %f, cd: %f "
  418. "gdop: %.2f, pdop: %.2f, hdop: %.2f, vdop: %.2f, tdop: %.2f\n",
  419. iod, navstat, wn, f_tow,
  420. timespec_str(&session->newdata.time, ts_buf, sizeof(ts_buf)),
  421. clock_bias, clock_drift,
  422. session->gpsdata.dop.gdop,
  423. session->gpsdata.dop.pdop,
  424. session->gpsdata.dop.hdop,
  425. session->gpsdata.dop.vdop,
  426. session->gpsdata.dop.tdop);
  427. mask |= TIME_SET | STATUS_SET | MODE_SET | DOP_SET | CLEAR_IS | REPORT_IS;
  428. return mask;
  429. }
  430. /*
  431. * decode MID 0xE0, GPS Subframe data
  432. *
  433. * len 33 bytes
  434. *
  435. */
  436. static gps_mask_t sky_msg_E0(struct gps_device_t *session,
  437. unsigned char *buf, size_t len UNUSED)
  438. {
  439. int i;
  440. unsigned int prn; /* GPS sat PRN */
  441. unsigned int subf; /* subframe 1-5 */
  442. /* the words are preprocessed, not raw, just the 24bits of data */
  443. uint32_t words[10]; /* subframe 1-5 */
  444. if ( 33 != len)
  445. return 0;
  446. prn = (unsigned int)getub(buf, 1);
  447. subf = (unsigned int)getub(buf, 2);
  448. for ( i = 0; i < 10; i++ ) {
  449. words[i] = (uint32_t)getbeu24(buf, 3 + (i * 3));
  450. }
  451. GPSD_LOG(LOG_DATA, &session->context->errout,
  452. "Skytraq: 50B MID 0xE0: prn=%u, subf=%u\n",
  453. prn, subf);
  454. return gpsd_interpret_subframe(session, prn, words);
  455. }
  456. /*
  457. * pretend to decode MID 0xE2, Beiduo D1 Subframe data
  458. *
  459. * from Beidou Standard BDS-SIS-ICD-2.0
  460. * D1, with the data rate of 50 bps, is broadcasted by the MEO/IGSO satellites
  461. *
  462. * len 31 bytes
  463. *
  464. */
  465. static gps_mask_t sky_msg_E2(struct gps_device_t *session,
  466. unsigned char *buf, size_t len)
  467. {
  468. int i;
  469. unsigned int prn; /* BeidouPS sat PRN 206-214 */
  470. unsigned int subf; /* subframe 1-5 */
  471. /* the words are preprocessed, not raw, just the 28 bytes of data */
  472. uint8_t bytes[28]; /* raw data */
  473. if ( 31 != len)
  474. return 0;
  475. prn = (unsigned int)getub(buf, 1);
  476. subf = (unsigned int)getub(buf, 2);
  477. for ( i = 0; i < 28; i++ ) {
  478. bytes[i] = getub(buf, 3 + i);
  479. }
  480. /* extra guard prevents expensive hexdump calls */
  481. if (session->context->errout.debug >= LOG_PROG) {
  482. GPSD_LOG(LOG_PROG, &session->context->errout,
  483. "Skytraq: Beidou D1 subframe PRN %d Subframe %d "
  484. "length %zd byte:%s\n",
  485. prn, subf,
  486. len,
  487. gpsd_hexdump(session->msgbuf, sizeof(session->msgbuf),
  488. (char *)bytes, 28));
  489. }
  490. return ONLINE_SET;
  491. }
  492. /*
  493. * pretend to decode MID 0xE3, Beiduo D2 Subframe data
  494. *
  495. * from Beidou Standard BDS-SIS-ICD-2.0
  496. * D2, with the data rate of 500 bps, is broadcasted by the GEO satellites.
  497. *
  498. * len 31 bytes
  499. *
  500. */
  501. static gps_mask_t sky_msg_E3(struct gps_device_t *session,
  502. unsigned char *buf, size_t len)
  503. {
  504. int i;
  505. unsigned int prn; /* BeidouPS sat PRN 201-205 */
  506. unsigned int subf; /* subframe 1-5 */
  507. /* the words are preprocessed, not raw, just the 28 bytes of data */
  508. uint8_t bytes[28]; /* raw data */
  509. if ( 31 != len)
  510. return 0;
  511. prn = (unsigned int)getub(buf, 1);
  512. subf = (unsigned int)getub(buf, 2);
  513. for ( i = 0; i < 28; i++ ) {
  514. bytes[i] = getub(buf, 3 + i);
  515. }
  516. /* extra guard prevents expensive hexdump calls */
  517. if (session->context->errout.debug >= LOG_PROG) {
  518. GPSD_LOG(LOG_PROG, &session->context->errout,
  519. "Skytraq: Beidou D2 subframe PRN %d Subframe %d "
  520. "length %zd byte:%s\n",
  521. prn, subf,
  522. len,
  523. gpsd_hexdump(session->msgbuf, sizeof(session->msgbuf),
  524. (char *)bytes, 28));
  525. }
  526. return ONLINE_SET;
  527. }
  528. static gps_mask_t sky_parse(struct gps_device_t * session, unsigned char *buf,
  529. size_t len)
  530. {
  531. gps_mask_t mask = 0;
  532. if (len == 0)
  533. return mask;
  534. buf += 4; /* skip the leaders and length */
  535. len -= 7; /* don't count the leaders, length, csum and terminators */
  536. // session->driver.sirf.lastid = buf[0];
  537. /* check the checksum?? */
  538. /* could change if the set of messages we enable does */
  539. /* session->cycle_end_reliable = true; */
  540. switch (buf[0]) {
  541. case 0x80:
  542. /* 128 */
  543. return sky_msg_80(session, buf, len);
  544. case 0x83:
  545. /* 131 - ACK */
  546. GPSD_LOG(LOG_PROG, &session->context->errout,
  547. "Skytraq: ACK to MID %#02x\n", buf[1]);
  548. break;
  549. case 0x84:
  550. /* 132 - NACK */
  551. GPSD_LOG(LOG_INF, &session->context->errout,
  552. "Skytraq: NACK to MID %#02x\n", buf[1]);
  553. break;
  554. case 0xDC:
  555. /* 220 */
  556. return sky_msg_DC(session, buf, len);
  557. case 0xDD:
  558. /* 221 */
  559. return sky_msg_DD(session, buf, len);
  560. case 0xDE:
  561. /* 222 */
  562. return sky_msg_DE(session, buf, len);
  563. case 0xDF:
  564. /* 223 - Nave status (PVT) */
  565. return sky_msg_DF(session, buf, len);
  566. case 0xE0:
  567. /* 224 */
  568. return sky_msg_E0(session, buf, len);
  569. case 0xE2:
  570. /* 226 - Beidou2 D1 Subframe data */
  571. return sky_msg_E2(session, buf, len);
  572. case 0xE3:
  573. /* 227 - Beidou2 D2 Subframe data */
  574. return sky_msg_E3(session, buf, len);
  575. default:
  576. GPSD_LOG(LOG_PROG, &session->context->errout,
  577. "Skytraq: Unknown packet id %#02x length %zd\n",
  578. buf[0], len);
  579. }
  580. return mask;
  581. }
  582. static gps_mask_t skybin_parse_input(struct gps_device_t *session)
  583. {
  584. if (session->lexer.type == SKY_PACKET) {
  585. return sky_parse(session, session->lexer.outbuffer,
  586. session->lexer.outbuflen);
  587. #ifdef NMEA0183_ENABLE
  588. } else if (session->lexer.type == NMEA_PACKET) {
  589. return nmea_parse((char *)session->lexer.outbuffer, session);
  590. #endif /* NMEA0183_ENABLE */
  591. } else
  592. return 0;
  593. }
  594. /* this is everything we export */
  595. /* *INDENT-OFF* */
  596. const struct gps_type_t driver_skytraq =
  597. {
  598. .type_name = "Skytraq", /* full name of type */
  599. .packet_type = SKY_PACKET, /* associated lexer packet type */
  600. .flags = DRIVER_STICKY, /* remember this */
  601. .trigger = NULL, /* no trigger */
  602. .channels = SKY_CHANNELS, /* consumer-grade GPS */
  603. .probe_detect = NULL, /* no probe */
  604. .get_packet = generic_get, /* be prepared for Skytraq or NMEA */
  605. .parse_packet = skybin_parse_input,/* parse message packets */
  606. .rtcm_writer = gpsd_write, /* send RTCM data straight */
  607. .init_query = NULL, /* non-perturbing initial qury */
  608. .event_hook = NULL, /* lifetime event handler */
  609. };
  610. /* *INDENT-ON* */
  611. #endif /* defined( SKYTRAQ_ENABLE) && defined(BINARY_ENABLE) */