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.
 
 
 
 
 
 

1781 lines
56 KiB

  1. /*
  2. * NMEA2000 over CAN.
  3. *
  4. * This file is Copyright (c) 2012-2018 by the GPSD project
  5. * SPDX-License-Identifier: BSD-2-clause
  6. */
  7. #include "gpsd_config.h" /* must be before all includes */
  8. #if defined(NMEA2000_ENABLE)
  9. #include <ctype.h>
  10. #include <fcntl.h>
  11. #include <linux/can.h>
  12. #include <linux/can/raw.h>
  13. #include <math.h>
  14. #include <net/if.h>
  15. #include <stdarg.h>
  16. #include <stdbool.h>
  17. #include <stdio.h>
  18. #include <stdlib.h>
  19. #include <string.h>
  20. #include <sys/ioctl.h>
  21. #include <sys/socket.h>
  22. #include <time.h>
  23. #include <unistd.h>
  24. #include "gpsd.h"
  25. #include "libgps.h"
  26. #include "driver_nmea2000.h"
  27. #include "bits.h"
  28. #include "timespec.h"
  29. #define LOG_FILE 1
  30. #define NMEA2000_NETS 4
  31. #define NMEA2000_UNITS 256
  32. #define CAN_NAMELEN 32
  33. #define MIN(a,b) ((a < b) ? a : b)
  34. #define NMEA2000_DEBUG_AIS 0
  35. #define NMEA2000_FAST_DEBUG 0
  36. static struct gps_device_t *nmea2000_units[NMEA2000_NETS][NMEA2000_UNITS];
  37. static char can_interface_name[NMEA2000_NETS][CAN_NAMELEN+1];
  38. typedef struct PGN
  39. {
  40. unsigned int pgn;
  41. unsigned int fast;
  42. unsigned int type;
  43. gps_mask_t (* func)(unsigned char *bu, int len, struct PGN *pgn, struct gps_device_t *session);
  44. const char *name;
  45. } PGN;
  46. #if LOG_FILE
  47. FILE *logFile = NULL;
  48. #endif /* of if LOG_FILE */
  49. extern bool __attribute__ ((weak)) gpsd_add_device(const char *device_name, bool flag_nowait);
  50. #define SHIFT32 0x100000000l
  51. static int scale_int(int32_t var, const int64_t factor)
  52. {
  53. int64_t ret;
  54. ret = var;
  55. ret *= factor;
  56. ret >>= 32;
  57. return((int)ret);
  58. }
  59. static void print_data(struct gps_context_t *context,
  60. unsigned char *buffer, int len, PGN *pgn)
  61. {
  62. #ifdef LIBGPS_DEBUG
  63. if ((libgps_debuglevel >= LOG_IO) != 0) {
  64. int l1, l2, ptr;
  65. char bu[128];
  66. ptr = 0;
  67. l2 = sprintf(&bu[ptr], "got data:%6u:%3d: ", pgn->pgn, len);
  68. ptr += l2;
  69. for (l1=0;l1<len;l1++) {
  70. if (((l1 % 20) == 0) && (l1 != 0)) {
  71. GPSD_LOG(LOG_IO, &context->errout, "%s\n", bu);
  72. ptr = 0;
  73. l2 = sprintf(&bu[ptr], " : ");
  74. ptr += l2;
  75. }
  76. l2 = sprintf(&bu[ptr], "%02ux ", (unsigned int)buffer[l1]);
  77. ptr += l2;
  78. }
  79. GPSD_LOG(LOG_IO, &context->errout, "%s\n", bu);
  80. }
  81. #else
  82. (void)context;
  83. (void)buffer;
  84. (void)len;
  85. (void)pgn;
  86. #endif
  87. }
  88. static gps_mask_t get_mode(struct gps_device_t *session)
  89. {
  90. if (session->driver.nmea2000.mode_valid & 1) {
  91. session->newdata.mode = session->driver.nmea2000.mode;
  92. } else {
  93. session->newdata.mode = MODE_NOT_SEEN;
  94. }
  95. if (session->driver.nmea2000.mode_valid & 2) {
  96. return MODE_SET | USED_IS;
  97. } else {
  98. return MODE_SET;
  99. }
  100. }
  101. static int decode_ais_header(struct gps_context_t *context,
  102. unsigned char *bu, int len, struct ais_t *ais, unsigned int mask)
  103. {
  104. if (len > 4) {
  105. ais->type = (unsigned int) ( bu[0] & 0x3f);
  106. ais->repeat = (unsigned int) ((bu[0] >> 6) & 0x03);
  107. ais->mmsi = (unsigned int) getleu32(bu, 1);
  108. ais->mmsi &= mask;
  109. GPSD_LOG(LOG_INF, &context->errout,
  110. "NMEA2000 AIS message type %u, MMSI %09d:\n",
  111. ais->type, ais->mmsi);
  112. return(1);
  113. } else {
  114. ais->type = 0;
  115. ais->repeat = 0;
  116. ais->mmsi = 0;
  117. GPSD_LOG(LOG_ERROR, &context->errout,
  118. "NMEA2000 AIS message type %u, too short message.\n",
  119. ais->type);
  120. }
  121. return(0);
  122. }
  123. static void decode_ais_channel_info(unsigned char *bu,
  124. int len,
  125. unsigned int offset,
  126. struct gps_device_t *session)
  127. {
  128. unsigned int pos, bpos;
  129. uint16_t x;
  130. pos = offset / 8;
  131. bpos = offset % 8;
  132. if (pos >= (unsigned int)len) {
  133. session->driver.aivdm.ais_channel = 'A';
  134. return;
  135. }
  136. x = getleu16(bu, pos);
  137. x = (uint16_t)((x >> bpos) & 0x1f);
  138. switch (x) {
  139. case 1:
  140. case 3:
  141. session->driver.aivdm.ais_channel = 'B';
  142. break;
  143. default:
  144. session->driver.aivdm.ais_channel = 'A';
  145. break;
  146. }
  147. return;
  148. }
  149. static int ais_turn_rate(int rate)
  150. {
  151. if (rate < 0) {
  152. return(-ais_turn_rate(-rate));
  153. }
  154. return((int)(4.733 * sqrt(rate * RAD_2_DEG * .0001 * 60.0)));
  155. }
  156. static double ais_direction(unsigned int val, double scale)
  157. {
  158. if ((val == 0xffff) && (scale == 1.0)) {
  159. return(511.0);
  160. }
  161. return(val * RAD_2_DEG * 0.0001 * scale);
  162. }
  163. /*
  164. * PGN 59392: ISO Acknowledgment
  165. */
  166. static gps_mask_t hnd_059392(unsigned char *bu, int len, PGN *pgn, struct gps_device_t *session)
  167. {
  168. print_data(session->context, bu, len, pgn);
  169. GPSD_LOG(LOG_DATA, &session->context->errout,
  170. "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);
  171. return(0);
  172. }
  173. /*
  174. * PGN 60928: ISO Address Claim
  175. */
  176. static gps_mask_t hnd_060928(unsigned char *bu, int len, PGN *pgn, struct gps_device_t *session)
  177. {
  178. print_data(session->context, bu, len, pgn);
  179. GPSD_LOG(LOG_DATA, &session->context->errout,
  180. "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);
  181. return(0);
  182. }
  183. /*
  184. * PGN 126208: NMEA Command/Request/Acknowledge
  185. */
  186. static gps_mask_t hnd_126208(unsigned char *bu, int len, PGN *pgn, struct gps_device_t *session)
  187. {
  188. print_data(session->context, bu, len, pgn);
  189. GPSD_LOG(LOG_DATA, &session->context->errout,
  190. "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);
  191. return(0);
  192. }
  193. /*
  194. * PGN 126464: ISO Transmit/Receive PGN List
  195. */
  196. static gps_mask_t hnd_126464(unsigned char *bu, int len, PGN *pgn, struct gps_device_t *session)
  197. {
  198. print_data(session->context, bu, len, pgn);
  199. GPSD_LOG(LOG_DATA, &session->context->errout,
  200. "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);
  201. return(0);
  202. }
  203. /*
  204. * PGN 126996: ISO Product Information
  205. */
  206. static gps_mask_t hnd_126996(unsigned char *bu, int len, PGN *pgn, struct gps_device_t *session)
  207. {
  208. print_data(session->context, bu, len, pgn);
  209. GPSD_LOG(LOG_DATA, &session->context->errout,
  210. "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);
  211. return(0);
  212. }
  213. /*
  214. * PGN 127258: GNSS Magnetic Variation
  215. *
  216. * 1 Sequence ID
  217. * 2 Variation Source
  218. * 3 Reserved Bits
  219. * 4 Age of Service (Date)
  220. * 5 Variation
  221. * 6 Reserved B
  222. */
  223. static gps_mask_t hnd_127258(unsigned char *bu, int len, PGN *pgn, struct gps_device_t *session)
  224. {
  225. print_data(session->context, bu, len, pgn);
  226. /* FIXME? Get magnetic variation */
  227. GPSD_LOG(LOG_DATA, &session->context->errout,
  228. "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);
  229. return(0);
  230. }
  231. /*
  232. * PGN 129025: GNSS Position Rapid Update
  233. */
  234. static gps_mask_t hnd_129025(unsigned char *bu, int len, PGN *pgn, struct gps_device_t *session)
  235. {
  236. print_data(session->context, bu, len, pgn);
  237. GPSD_LOG(LOG_DATA, &session->context->errout,
  238. "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);
  239. session->newdata.latitude = getles32(bu, 0) * 1e-7;
  240. session->newdata.longitude = getles32(bu, 4) * 1e-7;
  241. return LATLON_SET | get_mode(session);
  242. }
  243. /*
  244. * PGN 129026: GNSS COG and SOG Rapid Update
  245. */
  246. static gps_mask_t hnd_129026(unsigned char *bu, int len, PGN *pgn, struct gps_device_t *session)
  247. {
  248. print_data(session->context, bu, len, pgn);
  249. GPSD_LOG(LOG_DATA, &session->context->errout,
  250. "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);
  251. session->driver.nmea2000.sid[0] = bu[0];
  252. session->newdata.track = getleu16(bu, 2) * 1e-4 * RAD_2_DEG;
  253. session->newdata.speed = getleu16(bu, 4) * 1e-2;
  254. return SPEED_SET | TRACK_SET | get_mode(session);
  255. }
  256. /*
  257. * PGN 126992: GNSS System Time
  258. */
  259. static gps_mask_t hnd_126992(unsigned char *bu, int len, PGN *pgn, struct gps_device_t *session)
  260. {
  261. // uint8_t sid;
  262. // uint8_t source;
  263. uint32_t msecs; /* time in ms */
  264. print_data(session->context, bu, len, pgn);
  265. GPSD_LOG(LOG_DATA, &session->context->errout,
  266. "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);
  267. // sid = bu[0];
  268. // source = bu[1] & 0x0f;
  269. msecs = getleu32(bu, 4);
  270. MSTOTS(&session->newdata.time, msecs);
  271. session->newdata.time.tv_sec += (time_t)(getleu16(bu, 2) * 24 * 60 * 60);
  272. return TIME_SET | get_mode(session);
  273. }
  274. static const int mode_tab[] = {MODE_NO_FIX, MODE_2D, MODE_3D, MODE_NO_FIX,
  275. MODE_NO_FIX, MODE_NO_FIX, MODE_NO_FIX, MODE_NO_FIX};
  276. /*
  277. * PGN 129539: GNSS DOPs
  278. */
  279. static gps_mask_t hnd_129539(unsigned char *bu, int len, PGN *pgn, struct gps_device_t *session)
  280. {
  281. gps_mask_t mask;
  282. unsigned int req_mode;
  283. unsigned int act_mode;
  284. print_data(session->context, bu, len, pgn);
  285. GPSD_LOG(LOG_DATA, &session->context->errout,
  286. "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);
  287. mask = 0;
  288. session->driver.nmea2000.sid[1] = bu[0];
  289. session->driver.nmea2000.mode_valid |= 1;
  290. req_mode = (unsigned int)((bu[1] >> 0) & 0x07);
  291. act_mode = (unsigned int)((bu[1] >> 3) & 0x07);
  292. /* This is a workaround for some GARMIN plotter, actual mode auto makes no sense for me! */
  293. if ((act_mode == 3) && (req_mode != 3)) {
  294. act_mode = req_mode;
  295. }
  296. session->driver.nmea2000.mode = mode_tab[act_mode];
  297. session->gpsdata.dop.hdop = getleu16(bu, 2) * 1e-2;
  298. session->gpsdata.dop.vdop = getleu16(bu, 4) * 1e-2;
  299. session->gpsdata.dop.tdop = getleu16(bu, 6) * 1e-2;
  300. mask |= DOP_SET;
  301. GPSD_LOG(LOG_DATA, &session->context->errout,
  302. "pgn %6d(%3d): sid:%02x hdop:%5.2f vdop:%5.2f tdop:%5.2f\n",
  303. pgn->pgn,
  304. session->driver.nmea2000.unit,
  305. session->driver.nmea2000.sid[1],
  306. session->gpsdata.dop.hdop,
  307. session->gpsdata.dop.vdop,
  308. session->gpsdata.dop.tdop);
  309. return mask | get_mode(session);
  310. }
  311. /*
  312. * PGN 129540: GNSS Satellites in View
  313. */
  314. static gps_mask_t hnd_129540(unsigned char *bu, int len, PGN *pgn, struct gps_device_t *session)
  315. {
  316. int l1;
  317. print_data(session->context, bu, len, pgn);
  318. GPSD_LOG(LOG_DATA, &session->context->errout,
  319. "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);
  320. session->driver.nmea2000.sid[2] = bu[0];
  321. session->gpsdata.satellites_visible = (int)bu[2];
  322. memset(session->gpsdata.skyview, '\0', sizeof(session->gpsdata.skyview));
  323. for (l1=0;l1<session->gpsdata.satellites_visible;l1++) {
  324. int svt;
  325. double azi, elev, snr;
  326. elev = getles16(bu, 3+12*l1+1) * 1e-4 * RAD_2_DEG;
  327. azi = getleu16(bu, 3+12*l1+3) * 1e-4 * RAD_2_DEG;
  328. snr = getles16(bu, 3+12*l1+5) * 1e-2;
  329. svt = (int)(bu[3+12*l1+11] & 0x0f);
  330. session->gpsdata.skyview[l1].elevation = (short) (round(elev));
  331. session->gpsdata.skyview[l1].azimuth = (short) (round(azi));
  332. session->gpsdata.skyview[l1].ss = snr;
  333. session->gpsdata.skyview[l1].PRN = (short)bu[3+12*l1+0];
  334. session->gpsdata.skyview[l1].used = false;
  335. if ((svt == 2) || (svt == 5)) {
  336. session->gpsdata.skyview[l1].used = true;
  337. }
  338. }
  339. session->driver.nmea2000.mode_valid |= 2;
  340. return SATELLITE_SET | USED_IS;
  341. }
  342. /*
  343. * PGN 129029: GNSS Position Data
  344. */
  345. static gps_mask_t hnd_129029(unsigned char *bu, int len, PGN *pgn, struct gps_device_t *session)
  346. {
  347. /* field description
  348. * 1 SID
  349. * 2 Position Date
  350. * 3 Position time
  351. * 4 Latitude
  352. * 5 Longitude
  353. * 6 Altitude (what kind? probably WGS84?)
  354. * 7 Type of System
  355. * 8 Method, GNSS
  356. * 9 Integrity
  357. * 10 reserved
  358. * 11 Number of SVs
  359. * 12 HDOP
  360. * 13 PDOP
  361. * 14 Geoidal Separation
  362. * 15 # of reference stations
  363. * 16 REf station 1 type
  364. * 17 REf station 1 ID
  365. * 18 REf station 1 DGNSS correction age
  366. * [ last 3 repeated]
  367. */
  368. gps_mask_t mask;
  369. uint32_t msecs; /* time in ms */
  370. print_data(session->context, bu, len, pgn);
  371. GPSD_LOG(LOG_DATA, &session->context->errout,
  372. "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);
  373. mask = 0;
  374. session->driver.nmea2000.sid[3] = bu[0];
  375. msecs = getleu32(bu, 3);
  376. MSTOTS(&session->newdata.time, msecs);
  377. session->newdata.time.tv_sec += (time_t)(getleu16(bu,1) * 24 * 60 * 60);
  378. mask |= TIME_SET;
  379. session->newdata.latitude = getles64(bu, 7) * 1e-16;
  380. session->newdata.longitude = getles64(bu, 15) * 1e-16;
  381. mask |= LATLON_SET;
  382. session->newdata.altHAE = getles64(bu, 23) * 1e-6;
  383. mask |= ALTITUDE_SET;
  384. // printf("mode %x %x\n", (bu[31] >> 4) & 0x0f, bu[31]);
  385. switch ((bu[31] >> 4) & 0x0f) {
  386. case 0:
  387. session->gpsdata.status = STATUS_NO_FIX;
  388. break;
  389. case 1:
  390. session->gpsdata.status = STATUS_FIX;
  391. break;
  392. case 2:
  393. session->gpsdata.status = STATUS_DGPS_FIX;
  394. break;
  395. case 3:
  396. case 4:
  397. case 5:
  398. session->gpsdata.status = STATUS_FIX; /* Is this correct ? */
  399. break;
  400. default:
  401. session->gpsdata.status = STATUS_NO_FIX;
  402. break;
  403. }
  404. mask |= STATUS_SET;
  405. session->newdata.geoid_sep = getles32(bu, 38) / 100.0;
  406. session->gpsdata.satellites_used = (int)bu[33];
  407. session->gpsdata.dop.hdop = getleu16(bu, 34) * 0.01;
  408. session->gpsdata.dop.pdop = getleu16(bu, 36) * 0.01;
  409. mask |= DOP_SET;
  410. return mask | get_mode(session);
  411. }
  412. /*
  413. * PGN 129038: AIS Class A Position Report
  414. */
  415. static gps_mask_t hnd_129038(unsigned char *bu, int len, PGN *pgn, struct gps_device_t *session)
  416. {
  417. struct ais_t *ais;
  418. ais = &session->gpsdata.ais;
  419. print_data(session->context, bu, len, pgn);
  420. GPSD_LOG(LOG_DATA, &session->context->errout,
  421. "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);
  422. if (decode_ais_header(session->context, bu, len, ais, 0xffffffffU) != 0) {
  423. ais->type1.lon = (int) scale_int(getles32(bu, 5), (int64_t)(SHIFT32 *.06L));
  424. ais->type1.lat = (int) scale_int(getles32(bu, 9), (int64_t)(SHIFT32 *.06L));
  425. ais->type1.accuracy = (bool) ((bu[13] >> 0) & 0x01);
  426. ais->type1.raim = (bool) ((bu[13] >> 1) & 0x01);
  427. ais->type1.second = (unsigned int) ((bu[13] >> 2) & 0x3f);
  428. ais->type1.course = (unsigned int) ais_direction((unsigned int)getleu16(bu, 14), 10.0);
  429. ais->type1.speed = (unsigned int) (getleu16(bu, 16) * MPS_TO_KNOTS * 0.01 / 0.1);
  430. ais->type1.radio = (unsigned int) (getleu32(bu, 18) & 0x7ffff);
  431. ais->type1.heading = (unsigned int) ais_direction((unsigned int)getleu16(bu, 21), 1.0);
  432. ais->type1.turn = ais_turn_rate((int)getles16(bu, 23));
  433. ais->type1.status = (unsigned int) ((bu[25] >> 0) & 0x0f);
  434. ais->type1.maneuver = 0; /* Not transmitted ???? */
  435. decode_ais_channel_info(bu, len, 163, session);
  436. return(ONLINE_SET | AIS_SET);
  437. }
  438. return(0);
  439. }
  440. /*
  441. * PGN 129039: AIS Class B Position Report
  442. */
  443. static gps_mask_t hnd_129039(unsigned char *bu, int len, PGN *pgn, struct gps_device_t *session)
  444. {
  445. struct ais_t *ais;
  446. ais = &session->gpsdata.ais;
  447. print_data(session->context, bu, len, pgn);
  448. GPSD_LOG(LOG_DATA, &session->context->errout,
  449. "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);
  450. if (decode_ais_header(session->context, bu, len, ais, 0xffffffffU) != 0) {
  451. ais->type18.lon = (int) scale_int(getles32(bu, 5), (int64_t)(SHIFT32 *.06L));
  452. ais->type18.lat = (int) scale_int(getles32(bu, 9), (int64_t)(SHIFT32 *.06L));
  453. ais->type18.accuracy = (bool) ((bu[13] >> 0) & 0x01);
  454. ais->type18.raim = (bool) ((bu[13] >> 1) & 0x01);
  455. ais->type18.second = (unsigned int) ((bu[13] >> 2) & 0x3f);
  456. ais->type18.course = (unsigned int) ais_direction((unsigned int) getleu16(bu, 14), 10.0);
  457. ais->type18.speed = (unsigned int) (getleu16(bu, 16) * MPS_TO_KNOTS * 0.01 / 0.1);
  458. ais->type18.radio = (unsigned int) (getleu32(bu, 18) & 0x7ffff);
  459. ais->type18.heading = (unsigned int) ais_direction((unsigned int) getleu16(bu, 21), 1.0);
  460. ais->type18.reserved = 0;
  461. ais->type18.regional = (unsigned int) ((bu[24] >> 0) & 0x03);
  462. ais->type18.cs = (bool) ((bu[24] >> 2) & 0x01);
  463. ais->type18.display = (bool) ((bu[24] >> 3) & 0x01);
  464. ais->type18.dsc = (bool) ((bu[24] >> 4) & 0x01);
  465. ais->type18.band = (bool) ((bu[24] >> 5) & 0x01);
  466. ais->type18.msg22 = (bool) ((bu[24] >> 6) & 0x01);
  467. ais->type18.assigned = (bool) ((bu[24] >> 7) & 0x01);
  468. decode_ais_channel_info(bu, len, 163, session);
  469. return(ONLINE_SET | AIS_SET);
  470. }
  471. return(0);
  472. }
  473. /*
  474. * PGN 129040: AIS Class B Extended Position Report
  475. */
  476. /* No test case for this message at the moment */
  477. static gps_mask_t hnd_129040(unsigned char *bu, int len, PGN *pgn, struct gps_device_t *session)
  478. {
  479. struct ais_t *ais;
  480. ais = &session->gpsdata.ais;
  481. print_data(session->context, bu, len, pgn);
  482. GPSD_LOG(LOG_DATA, &session->context->errout,
  483. "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);
  484. if (decode_ais_header(session->context, bu, len, ais, 0xffffffffU) != 0) {
  485. uint16_t length, beam, to_bow, to_starboard;
  486. int l;
  487. ais->type19.lon = (int) scale_int(getles32(bu, 5), (int64_t)(SHIFT32 *.06L));
  488. ais->type19.lat = (int) scale_int(getles32(bu, 9), (int64_t)(SHIFT32 *.06L));
  489. ais->type19.accuracy = (bool) ((bu[13] >> 0) & 0x01);
  490. ais->type19.raim = (bool) ((bu[13] >> 1) & 0x01);
  491. ais->type19.second = (unsigned int) ((bu[13] >> 2) & 0x3f);
  492. ais->type19.course = (unsigned int) ais_direction((unsigned int) getleu16(bu, 14), 10.0);
  493. ais->type19.speed = (unsigned int) (getleu16(bu, 16) * MPS_TO_KNOTS * 0.01 / 0.1);
  494. ais->type19.reserved = (unsigned int) ((bu[18] >> 0) & 0xff);
  495. ais->type19.regional = (unsigned int) ((bu[19] >> 0) & 0x0f);
  496. ais->type19.shiptype = (unsigned int) ((bu[20] >> 0) & 0xff);
  497. ais->type19.heading = (unsigned int) ais_direction((unsigned int) getleu16(bu, 21), 1.0);
  498. length = getleu16(bu, 24);
  499. beam = getleu16(bu, 26);
  500. to_starboard = getleu16(bu, 28);
  501. to_bow = getleu16(bu, 30);
  502. if ((length == 0xffff) || (to_bow == 0xffff)) {
  503. length = 0;
  504. to_bow = 0;
  505. }
  506. if ((beam == 0xffff) || (to_starboard == 0xffff)) {
  507. beam = 0;
  508. to_starboard = 0;
  509. }
  510. ais->type19.to_bow = (unsigned int) (to_bow/10);
  511. ais->type19.to_stern = (unsigned int) ((length-to_bow)/10);
  512. ais->type19.to_port = (unsigned int) ((beam-to_starboard)/10);
  513. ais->type19.to_starboard = (unsigned int) (to_starboard/10);
  514. ais->type19.epfd = (unsigned int) ((bu[23] >> 4) & 0x0f);
  515. ais->type19.dte = (unsigned int) ((bu[52] >> 0) & 0x01);
  516. ais->type19.assigned = (bool) ((bu[52] >> 1) & 0x01);
  517. for (l=0;l<AIS_SHIPNAME_MAXLEN;l++) {
  518. ais->type19.shipname[l] = (char) bu[32+l];
  519. }
  520. ais->type19.shipname[AIS_SHIPNAME_MAXLEN] = (char) 0;
  521. decode_ais_channel_info(bu, len, 422, session);
  522. return(ONLINE_SET | AIS_SET);
  523. }
  524. return(0);
  525. }
  526. /*
  527. * PGN 129793: AIS UTC and Date Report
  528. */
  529. static gps_mask_t hnd_129793(unsigned char *bu, int len, PGN *pgn, struct gps_device_t *session)
  530. {
  531. struct ais_t *ais;
  532. ais = &session->gpsdata.ais;
  533. print_data(session->context, bu, len, pgn);
  534. GPSD_LOG(LOG_DATA, &session->context->errout,
  535. "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);
  536. if (decode_ais_header(session->context, bu, len, ais, 0xffffffffU) != 0) {
  537. uint32_t time;
  538. uint32_t date;
  539. time_t date1;
  540. struct tm date2;
  541. ais->type4.lon = (int) scale_int(getles32(bu, 5), (int64_t)(SHIFT32 *.06L));
  542. ais->type4.lat = (int) scale_int(getles32(bu, 9), (int64_t)(SHIFT32 *.06L));
  543. ais->type4.accuracy = (bool) ((bu[13] >> 0) & 0x01);
  544. ais->type4.raim = (bool) ((bu[13] >> 1) & 0x01);
  545. time = getleu32(bu, 14);
  546. if (time != 0xffffffff) {
  547. time = time / 10000;
  548. ais->type4.second = time % 60; time = time / 60;
  549. ais->type4.minute = time % 60; time = time / 60;
  550. ais->type4.hour = time % 24;
  551. } else {
  552. ais->type4.second = AIS_SECOND_NOT_AVAILABLE;
  553. ais->type4.minute = AIS_MINUTE_NOT_AVAILABLE;
  554. ais->type4.hour = AIS_HOUR_NOT_AVAILABLE;
  555. }
  556. ais->type4.radio = (unsigned int) (getleu32(bu, 18) & 0x7ffff);
  557. date = getleu16(bu, 21);
  558. if (date != 0xffff) {
  559. date1 = (time_t)date * (24L *60L *60L);
  560. (void) gmtime_r(&date1, &date2);
  561. ais->type4.year = (unsigned int) (date2.tm_year+1900);
  562. ais->type4.month = (unsigned int) (date2.tm_mon+1);
  563. ais->type4.day = (unsigned int) (date2.tm_mday);
  564. } else {
  565. ais->type4.day = AIS_DAY_NOT_AVAILABLE;
  566. ais->type4.month = AIS_MONTH_NOT_AVAILABLE;
  567. ais->type4.year = AIS_YEAR_NOT_AVAILABLE;
  568. }
  569. ais->type4.epfd = (unsigned int) ((bu[23] >> 4) & 0x0f);
  570. decode_ais_channel_info(bu, len, 163, session);
  571. return(ONLINE_SET | AIS_SET);
  572. }
  573. return(0);
  574. }
  575. /*
  576. * PGN 129794: AIS Class A Static and Voyage Related Data
  577. */
  578. static gps_mask_t hnd_129794(unsigned char *bu, int len, PGN *pgn, struct gps_device_t *session)
  579. {
  580. struct ais_t *ais;
  581. ais = &session->gpsdata.ais;
  582. print_data(session->context, bu, len, pgn);
  583. GPSD_LOG(LOG_DATA, &session->context->errout,
  584. "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);
  585. if (decode_ais_header(session->context, bu, len, ais, 0xffffffffU) != 0) {
  586. uint16_t length, beam, to_bow, to_starboard, date;
  587. int l;
  588. uint32_t time;
  589. time_t date1;
  590. struct tm date2;
  591. int cpy_stop;
  592. ais->type5.ais_version = (unsigned int) ((bu[73] >> 0) & 0x03);
  593. ais->type5.imo = (unsigned int) getleu32(bu, 5);
  594. if (ais->type5.imo == 0xffffffffU) {
  595. ais->type5.imo = 0;
  596. }
  597. ais->type5.shiptype = (unsigned int) ((bu[36] >> 0) & 0xff);
  598. length = getleu16(bu, 37);
  599. beam = getleu16(bu, 39);
  600. to_starboard = getleu16(bu, 41);
  601. to_bow = getleu16(bu, 43);
  602. if ((length == 0xffff) || (to_bow == 0xffff)) {
  603. length = 0;
  604. to_bow = 0;
  605. }
  606. if ((beam == 0xffff) || (to_starboard == 0xffff)) {
  607. beam = 0;
  608. to_starboard = 0;
  609. }
  610. ais->type5.to_bow = (unsigned int) (to_bow/10);
  611. ais->type5.to_stern = (unsigned int) ((length-to_bow)/10);
  612. ais->type5.to_port = (unsigned int) ((beam-to_starboard)/10);
  613. ais->type5.to_starboard = (unsigned int) (to_starboard/10);
  614. ais->type5.epfd = (unsigned int) ((bu[73] >> 2) & 0x0f);
  615. date = getleu16(bu, 45);
  616. time = getleu32(bu, 47);
  617. date1 = (time_t) (date*24*60*60);
  618. (void) gmtime_r(&date1, &date2);
  619. ais->type5.month = (unsigned int) (date2.tm_mon+1);
  620. ais->type5.day = (unsigned int) (date2.tm_mday);
  621. ais->type5.minute = (unsigned int) (time/(10000*60));
  622. ais->type5.hour = (unsigned int) (ais->type5.minute/60);
  623. ais->type5.minute = (unsigned int) (ais->type5.minute-(ais->type5.hour*60));
  624. ais->type5.draught = (unsigned int) (getleu16(bu, 51)/10);
  625. ais->type5.dte = (unsigned int) ((bu[73] >> 6) & 0x01);
  626. for (l=0,cpy_stop=0;l<7;l++) {
  627. char next;
  628. next = (char) bu[9+l];
  629. if ((next < ' ') || (next > 0x7e)) {
  630. cpy_stop = 1;
  631. }
  632. if (cpy_stop == 0) {
  633. ais->type5.callsign[l] = next;
  634. } else {
  635. ais->type5.callsign[l] = 0;
  636. }
  637. }
  638. ais->type5.callsign[7] = (char) 0;
  639. for (l=0,cpy_stop=0;l<AIS_SHIPNAME_MAXLEN;l++) {
  640. char next;
  641. next = (char) bu[16+l];
  642. if ((next < ' ') || (next > 0x7e)) {
  643. cpy_stop = 1;
  644. }
  645. if (cpy_stop == 0) {
  646. ais->type5.shipname[l] = next;
  647. } else {
  648. ais->type5.shipname[l] = 0;
  649. }
  650. }
  651. ais->type5.shipname[AIS_SHIPNAME_MAXLEN] = (char) 0;
  652. for (l=0,cpy_stop=0;l<20;l++) {
  653. char next;
  654. next = (char) bu[53+l];
  655. if ((next < ' ') || (next > 0x7e)) {
  656. cpy_stop = 1;
  657. }
  658. if (cpy_stop == 0) {
  659. ais->type5.destination[l] = next;
  660. } else {
  661. ais->type5.destination[l] = 0;
  662. }
  663. }
  664. ais->type5.destination[20] = (char) 0;
  665. #if NMEA2000_DEBUG_AIS
  666. printf("AIS: MMSI: %09u\n",
  667. ais->mmsi);
  668. printf("AIS: name: %-20.20s i:%8u c:%-8.8s b:%6u s:%6u p:%6u s:%6u dr:%4.1f\n",
  669. ais->type5.shipname,
  670. ais->type5.imo,
  671. ais->type5.callsign,
  672. ais->type5.to_bow,
  673. ais->type5.to_stern,
  674. ais->type5.to_port,
  675. ais->type5.to_starboard,
  676. ais->type5.draught/10.0);
  677. printf("AIS: arival:%-20.20s at %02u-%02u-%04d %02u:%0u\n",
  678. ais->type5.destination,
  679. ais->type5.day,
  680. ais->type5.month,
  681. date2.tm_year+1900,
  682. ais->type5.hour,
  683. ais->type5.minute);
  684. #endif /* of #if NMEA2000_DEBUG_AIS */
  685. decode_ais_channel_info(bu, len, 592, session);
  686. return(ONLINE_SET | AIS_SET);
  687. }
  688. return(0);
  689. }
  690. /*
  691. * PGN 129798: AIS SAR Aircraft Position Report
  692. */
  693. /* No test case for this message at the moment */
  694. static gps_mask_t hnd_129798(unsigned char *bu, int len, PGN *pgn, struct gps_device_t *session)
  695. {
  696. struct ais_t *ais;
  697. ais = &session->gpsdata.ais;
  698. print_data(session->context, bu, len, pgn);
  699. GPSD_LOG(LOG_DATA, &session->context->errout,
  700. "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);
  701. if (decode_ais_header(session->context, bu, len, ais, 0xffffffffU) != 0) {
  702. ais->type9.lon = (int) scale_int(getles32(bu, 5), (int64_t)(SHIFT32 *.06L));
  703. ais->type9.lat = (int) scale_int(getles32(bu, 9), (int64_t)(SHIFT32 *.06L));
  704. ais->type9.accuracy = (bool) ((bu[13] >> 0) & 0x01);
  705. ais->type9.raim = (bool) ((bu[13] >> 1) & 0x01);
  706. ais->type9.second = (unsigned int) ((bu[13] >> 2) & 0x3f);
  707. ais->type9.course = (unsigned int) ais_direction((unsigned int) getleu16(bu, 14), 10.0);
  708. ais->type9.speed = (unsigned int) (getleu16(bu, 16) * MPS_TO_KNOTS * 0.01 / 0.1);
  709. ais->type9.radio = (unsigned int) (getleu32(bu, 18) & 0x7ffff);
  710. ais->type9.alt = (unsigned int) (getleu64(bu, 21)/1000000);
  711. ais->type9.regional = (unsigned int) ((bu[29] >> 0) & 0xff);
  712. ais->type9.dte = (unsigned int) ((bu[30] >> 0) & 0x01);
  713. /* ais->type9.spare = (bu[30] >> 1) & 0x7f; */
  714. ais->type9.assigned = 0; /* Not transmitted ???? */
  715. decode_ais_channel_info(bu, len, 163, session);
  716. return(ONLINE_SET | AIS_SET);
  717. }
  718. return(0);
  719. }
  720. /*
  721. * PGN 129802: AIS Safty Related Broadcast Message
  722. */
  723. /* No test case for this message at the moment */
  724. static gps_mask_t hnd_129802(unsigned char *bu, int len, PGN *pgn, struct gps_device_t *session)
  725. {
  726. struct ais_t *ais;
  727. ais = &session->gpsdata.ais;
  728. print_data(session->context, bu, len, pgn);
  729. GPSD_LOG(LOG_DATA, &session->context->errout,
  730. "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);
  731. if (decode_ais_header(session->context, bu, len, ais, 0x3fffffff) != 0) {
  732. int l;
  733. /* ais->type14.channel = (bu[ 5] >> 0) & 0x1f; */
  734. for (l=0;l<36;l++) {
  735. ais->type14.text[l] = (char) bu[6+l];
  736. }
  737. ais->type14.text[36] = (char) 0;
  738. decode_ais_channel_info(bu, len, 40, session);
  739. return(ONLINE_SET | AIS_SET);
  740. }
  741. return(0);
  742. }
  743. /*
  744. * PGN 129809: AIS Class B CS Static Data Report, Part A
  745. */
  746. static gps_mask_t hnd_129809(unsigned char *bu, int len, PGN *pgn, struct gps_device_t *session)
  747. {
  748. struct ais_t *ais;
  749. ais = &session->gpsdata.ais;
  750. print_data(session->context, bu, len, pgn);
  751. GPSD_LOG(LOG_DATA, &session->context->errout,
  752. "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);
  753. if (decode_ais_header(session->context, bu, len, ais, 0xffffffffU) != 0) {
  754. int l;
  755. int index = session->driver.aivdm.context[0].type24_queue.index;
  756. struct ais_type24a_t *saveptr = &session->driver.aivdm.context[0].type24_queue.ships[index];
  757. GPSD_LOG(LOG_PROG, &session->context->errout,
  758. "NMEA2000: AIS message 24A from %09u stashed.\n",
  759. ais->mmsi);
  760. for (l=0;l<AIS_SHIPNAME_MAXLEN;l++) {
  761. ais->type24.shipname[l] = (char) bu[ 5+l];
  762. saveptr->shipname[l] = (char) bu[ 5+l];
  763. }
  764. ais->type24.shipname[AIS_SHIPNAME_MAXLEN] = (char) 0;
  765. saveptr->shipname[AIS_SHIPNAME_MAXLEN] = (char) 0;
  766. saveptr->mmsi = ais->mmsi;
  767. index += 1;
  768. index %= MAX_TYPE24_INTERLEAVE;
  769. session->driver.aivdm.context[0].type24_queue.index = index;
  770. decode_ais_channel_info(bu, len, 200, session);
  771. ais->type24.part = part_a;
  772. return(ONLINE_SET | AIS_SET);
  773. }
  774. return(0);
  775. }
  776. /*
  777. * PGN 129810: AIS Class B CS Static Data Report, Part B
  778. */
  779. static gps_mask_t hnd_129810(unsigned char *bu, int len, PGN *pgn, struct gps_device_t *session)
  780. {
  781. struct ais_t *ais;
  782. ais = &session->gpsdata.ais;
  783. print_data(session->context, bu, len, pgn);
  784. GPSD_LOG(LOG_DATA, &session->context->errout,
  785. "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);
  786. if (decode_ais_header(session->context, bu, len, ais, 0xffffffffU) != 0) {
  787. int l, i;
  788. ais->type24.shiptype = (unsigned int) ((bu[ 5] >> 0) & 0xff);
  789. for (l=0;l<7;l++) {
  790. ais->type24.vendorid[l] = (char) bu[ 6+l];
  791. }
  792. ais->type24.vendorid[7] = (char) 0;
  793. for (l=0;l<7;l++) {
  794. ais->type24.callsign[l] = (char) bu[13+l];
  795. }
  796. ais->type24.callsign[7] = (char )0;
  797. ais->type24.model = 0;
  798. ais->type24.serial = 0;
  799. if (AIS_AUXILIARY_MMSI(ais->mmsi)) {
  800. ais->type24.mothership_mmsi = (unsigned int) (getleu32(bu, 28));
  801. } else {
  802. uint16_t length, beam, to_bow, to_starboard;
  803. length = getleu16(bu, 20);
  804. beam = getleu16(bu, 22);
  805. to_starboard = getleu16(bu, 24);
  806. to_bow = getleu16(bu, 26);
  807. if ((length == 0xffff) || (to_bow == 0xffff)) {
  808. length = 0;
  809. to_bow = 0;
  810. }
  811. if ((beam == 0xffff) || (to_starboard == 0xffff)) {
  812. beam = 0;
  813. to_starboard = 0;
  814. }
  815. ais->type24.dim.to_bow = (unsigned int) (to_bow/10);
  816. ais->type24.dim.to_stern = (unsigned int) ((length-to_bow)/10);
  817. ais->type24.dim.to_port = (unsigned int) ((beam-to_starboard)/10);
  818. ais->type24.dim.to_starboard = (unsigned int) (to_starboard/10);
  819. }
  820. for (i = 0; i < MAX_TYPE24_INTERLEAVE; i++) {
  821. if (session->driver.aivdm.context[0].type24_queue.ships[i].mmsi == ais->mmsi) {
  822. for (l=0;l<AIS_SHIPNAME_MAXLEN;l++) {
  823. ais->type24.shipname[l] = (char)(session->driver.aivdm.context[0].type24_queue.ships[i].shipname[l]);
  824. }
  825. ais->type24.shipname[AIS_SHIPNAME_MAXLEN] = (char) 0;
  826. GPSD_LOG(LOG_PROG, &session->context->errout,
  827. "NMEA2000: AIS 24B from %09u matches a 24A.\n",
  828. ais->mmsi);
  829. /* prevent false match if a 24B is repeated */
  830. session->driver.aivdm.context[0].type24_queue.ships[i].mmsi = 0;
  831. #if NMEA2000_DEBUG_AIS
  832. printf("AIS: MMSI: %09u\n", ais->mmsi);
  833. printf("AIS: name: %-20.20s v:%-8.8s c:%-8.8s b:%6u s:%6u p:%6u s:%6u\n",
  834. ais->type24.shipname,
  835. ais->type24.vendorid,
  836. ais->type24.callsign,
  837. ais->type24.dim.to_bow,
  838. ais->type24.dim.to_stern,
  839. ais->type24.dim.to_port,
  840. ais->type24.dim.to_starboard);
  841. #endif /* of #if NMEA2000_DEBUG_AIS */
  842. decode_ais_channel_info(bu, len, 264, session);
  843. ais->type24.part = both;
  844. return(ONLINE_SET | AIS_SET);
  845. }
  846. }
  847. #if NMEA2000_DEBUG_AIS
  848. printf("AIS: MMSI : %09u\n", ais->mmsi);
  849. printf("AIS: vendor: %-8.8s c:%-8.8s b:%6u s:%6u p:%6u s:%6u\n",
  850. ais->type24.vendorid,
  851. ais->type24.callsign,
  852. ais->type24.dim.to_bow,
  853. ais->type24.dim.to_stern,
  854. ais->type24.dim.to_port,
  855. ais->type24.dim.to_starboard);
  856. #endif /* of #if NMEA2000_DEBUG_AIS */
  857. decode_ais_channel_info(bu, len, 264, session);
  858. ais->type24.part = part_b;
  859. return(ONLINE_SET | AIS_SET);
  860. }
  861. return(0);
  862. }
  863. /*
  864. * PGN 127506: PWR DC Detailed Status
  865. */
  866. static gps_mask_t hnd_127506(unsigned char *bu, int len, PGN *pgn, struct gps_device_t *session)
  867. {
  868. print_data(session->context, bu, len, pgn);
  869. GPSD_LOG(LOG_DATA, &session->context->errout,
  870. "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);
  871. return(0);
  872. }
  873. /*
  874. * PGN 127508: PWR Battery Status
  875. */
  876. static gps_mask_t hnd_127508(unsigned char *bu, int len, PGN *pgn, struct gps_device_t *session)
  877. {
  878. print_data(session->context, bu, len, pgn);
  879. GPSD_LOG(LOG_DATA, &session->context->errout,
  880. "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);
  881. return(0);
  882. }
  883. /*
  884. * PGN 127513: PWR Battery Configuration Status
  885. */
  886. static gps_mask_t hnd_127513(unsigned char *bu, int len, PGN *pgn, struct gps_device_t *session)
  887. {
  888. print_data(session->context, bu, len, pgn);
  889. GPSD_LOG(LOG_DATA, &session->context->errout,
  890. "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);
  891. return(0);
  892. }
  893. /*
  894. * PGN 127245: NAV Rudder
  895. */
  896. static gps_mask_t hnd_127245(unsigned char *bu, int len, PGN *pgn, struct gps_device_t *session)
  897. {
  898. print_data(session->context, bu, len, pgn);
  899. GPSD_LOG(LOG_DATA, &session->context->errout,
  900. "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);
  901. return(0);
  902. }
  903. /*
  904. * PGN 127250: NAV Vessel Heading
  905. */
  906. static gps_mask_t hnd_127250(unsigned char *bu, int len, PGN *pgn, struct gps_device_t *session)
  907. {
  908. int aux;
  909. print_data(session->context, bu, len, pgn);
  910. session->gpsdata.attitude.heading = getleu16(bu, 1) * RAD_2_DEG * 0.0001;
  911. // printf("ATT 0:%8.3f\n",session->gpsdata.attitude.heading);
  912. aux = getles16(bu, 3);
  913. if (aux != 0x07fff) {
  914. session->gpsdata.attitude.heading += aux * RAD_2_DEG * 0.0001;
  915. }
  916. // printf("ATT 1:%8.3f %6x\n",session->gpsdata.attitude.heading, aux);
  917. aux = getles16(bu, 5);
  918. if (aux != 0x07fff) {
  919. session->gpsdata.attitude.heading += aux * RAD_2_DEG * 0.0001;
  920. }
  921. // printf("ATT 2:%8.3f %6x\n",session->gpsdata.attitude.heading, aux);
  922. GPSD_LOG(LOG_DATA, &session->context->errout,
  923. "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);
  924. return(ONLINE_SET | ATTITUDE_SET);
  925. }
  926. /*
  927. * PGN 128259: NAV Speed
  928. */
  929. static gps_mask_t hnd_128259(unsigned char *bu, int len, PGN *pgn, struct gps_device_t *session)
  930. {
  931. print_data(session->context, bu, len, pgn);
  932. GPSD_LOG(LOG_DATA, &session->context->errout,
  933. "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);
  934. return(0);
  935. }
  936. /*
  937. * PGN 128267: NAV Water Depth
  938. */
  939. static gps_mask_t hnd_128267(unsigned char *bu, int len, PGN *pgn, struct gps_device_t *session)
  940. {
  941. print_data(session->context, bu, len, pgn);
  942. session->gpsdata.attitude.depth = getleu32(bu, 1) *.01;
  943. GPSD_LOG(LOG_DATA, &session->context->errout,
  944. "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);
  945. return(ONLINE_SET | ATTITUDE_SET);
  946. }
  947. /*
  948. * PGN 128275: NAV Distance Log
  949. */
  950. static gps_mask_t hnd_128275(unsigned char *bu, int len, PGN *pgn, struct gps_device_t *session)
  951. {
  952. print_data(session->context, bu, len, pgn);
  953. GPSD_LOG(LOG_DATA, &session->context->errout,
  954. "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);
  955. return(0);
  956. }
  957. /*
  958. * PGN 129283: NAV Cross Track Error
  959. */
  960. static gps_mask_t hnd_129283(unsigned char *bu, int len, PGN *pgn, struct gps_device_t *session)
  961. {
  962. print_data(session->context, bu, len, pgn);
  963. GPSD_LOG(LOG_DATA, &session->context->errout,
  964. "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);
  965. return(0);
  966. }
  967. /*
  968. * PGN 129284: NAV Navigation Data
  969. */
  970. static gps_mask_t hnd_129284(unsigned char *bu, int len, PGN *pgn, struct gps_device_t *session)
  971. {
  972. print_data(session->context, bu, len, pgn);
  973. GPSD_LOG(LOG_DATA, &session->context->errout,
  974. "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);
  975. return(0);
  976. }
  977. /*
  978. * PGN 129285: NAV Navigation - Route/WP Information
  979. */
  980. static gps_mask_t hnd_129285(unsigned char *bu, int len, PGN *pgn, struct gps_device_t *session)
  981. {
  982. print_data(session->context, bu, len, pgn);
  983. GPSD_LOG(LOG_DATA, &session->context->errout,
  984. "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);
  985. return(0);
  986. }
  987. /*
  988. * PGN 130306: NAV Wind Data
  989. */
  990. static gps_mask_t hnd_130306(unsigned char *bu, int len, PGN *pgn, struct gps_device_t *session)
  991. {
  992. print_data(session->context, bu, len, pgn);
  993. GPSD_LOG(LOG_DATA, &session->context->errout,
  994. "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);
  995. return(0);
  996. }
  997. /*
  998. * PGN 130310: NAV Water Temp., Outside Air Temp., Atmospheric Pressure
  999. */
  1000. static gps_mask_t hnd_130310(unsigned char *bu, int len, PGN *pgn, struct gps_device_t *session)
  1001. {
  1002. print_data(session->context, bu, len, pgn);
  1003. GPSD_LOG(LOG_DATA, &session->context->errout,
  1004. "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);
  1005. return(0);
  1006. }
  1007. /*
  1008. * PGN 130311: NAV Environmental Parameters
  1009. */
  1010. static gps_mask_t hnd_130311(unsigned char *bu, int len, PGN *pgn, struct gps_device_t *session)
  1011. {
  1012. print_data(session->context, bu, len, pgn);
  1013. GPSD_LOG(LOG_DATA, &session->context->errout,
  1014. "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);
  1015. return(0);
  1016. }
  1017. static const char msg_059392[] = {"ISO Acknowledgment"};
  1018. static const char msg_060928[] = {"ISO Address Claim"};
  1019. static const char msg_126208[] = {"NMEA Command/Request/Acknowledge"};
  1020. static const char msg_126464[] = {"ISO Transmit/Receive PGN List"};
  1021. static const char msg_126992[] = {"GNSS System Time"};
  1022. static const char msg_126996[] = {"ISO Product Information"};
  1023. static const char msg_127506[] = {"PWR DC Detailed Status"};
  1024. static const char msg_127508[] = {"PWR Battery Status"};
  1025. static const char msg_127513[] = {"PWR Battery Configuration Status"};
  1026. static const char msg_127258[] = {"GNSS Magnetic Variation"};
  1027. static const char msg_129025[] = {"GNSS Position Rapid Update"};
  1028. static const char msg_129026[] = {"GNSS COG and SOG Rapid Update"};
  1029. static const char msg_129029[] = {"GNSS Positition Data"};
  1030. static const char msg_129539[] = {"GNSS DOPs"};
  1031. static const char msg_129540[] = {"GNSS Satellites in View"};
  1032. static const char msg_129038[] = {"AIS Class A Position Report"};
  1033. static const char msg_129039[] = {"AIS Class B Position Report"};
  1034. static const char msg_129040[] = {"AIS Class B Extended Position Report"};
  1035. static const char msg_129793[] = {"AIS UTC and Date report"};
  1036. static const char msg_129794[] = {"AIS Class A Static and Voyage Related Data"};
  1037. static const char msg_129798[] = {"AIS SAR Aircraft Position Report"};
  1038. static const char msg_129802[] = {"AIS Safty Related Broadcast Message"};
  1039. static const char msg_129809[] = {"AIS Class B CS Static Data Report, Part A"};
  1040. static const char msg_129810[] = {"AIS Class B CS Static Data Report, Part B"};
  1041. static const char msg_127245[] = {"NAV Rudder"};
  1042. static const char msg_127250[] = {"NAV Vessel Heading"};
  1043. static const char msg_128259[] = {"NAV Speed"};
  1044. static const char msg_128267[] = {"NAV Water Depth"};
  1045. static const char msg_128275[] = {"NAV Distance Log"};
  1046. static const char msg_129283[] = {"NAV Cross Track Error"};
  1047. static const char msg_129284[] = {"NAV Navigation Data"};
  1048. static const char msg_129285[] = {"NAV Navigation - Route/WP Information"};
  1049. static const char msg_130306[] = {"NAV Wind Data"};
  1050. static const char msg_130310[] = {"NAV Water Temp., Outside Air Temp., Atmospheric Pressure"};
  1051. static const char msg_130311[] = {"NAV Environmental Parameters"};
  1052. static const char msg_error [] = {"**error**"};
  1053. static PGN gpspgn[] = {{ 59392, 0, 0, hnd_059392, &msg_059392[0]},
  1054. { 60928, 0, 0, hnd_060928, &msg_060928[0]},
  1055. {126208, 0, 0, hnd_126208, &msg_126208[0]},
  1056. {126464, 1, 0, hnd_126464, &msg_126464[0]},
  1057. {126992, 0, 0, hnd_126992, &msg_126992[0]},
  1058. {126996, 1, 0, hnd_126996, &msg_126996[0]},
  1059. {127258, 0, 0, hnd_127258, &msg_127258[0]},
  1060. {129025, 0, 1, hnd_129025, &msg_129025[0]},
  1061. {129026, 0, 1, hnd_129026, &msg_129026[0]},
  1062. {129029, 1, 1, hnd_129029, &msg_129029[0]},
  1063. {129283, 0, 0, hnd_129283, &msg_129283[0]},
  1064. {129284, 1, 0, hnd_129284, &msg_129284[0]},
  1065. {129285, 1, 0, hnd_129285, &msg_129285[0]},
  1066. {129539, 0, 1, hnd_129539, &msg_129539[0]},
  1067. {129540, 1, 1, hnd_129540, &msg_129540[0]},
  1068. {0 , 0, 0, NULL, &msg_error [0]}};
  1069. static PGN aispgn[] = {{ 59392, 0, 0, hnd_059392, &msg_059392[0]},
  1070. { 60928, 0, 0, hnd_060928, &msg_060928[0]},
  1071. {126208, 0, 0, hnd_126208, &msg_126208[0]},
  1072. {126464, 1, 0, hnd_126464, &msg_126464[0]},
  1073. {126992, 0, 0, hnd_126992, &msg_126992[0]},
  1074. {126996, 1, 0, hnd_126996, &msg_126996[0]},
  1075. {129038, 1, 2, hnd_129038, &msg_129038[0]},
  1076. {129039, 1, 2, hnd_129039, &msg_129039[0]},
  1077. {129040, 1, 2, hnd_129040, &msg_129040[0]},
  1078. {129793, 1, 2, hnd_129793, &msg_129793[0]},
  1079. {129794, 1, 2, hnd_129794, &msg_129794[0]},
  1080. {129798, 1, 2, hnd_129798, &msg_129798[0]},
  1081. {129802, 1, 2, hnd_129802, &msg_129802[0]},
  1082. {129809, 1, 2, hnd_129809, &msg_129809[0]},
  1083. {129810, 1, 2, hnd_129810, &msg_129810[0]},
  1084. {0 , 0, 0, NULL, &msg_error [0]}};
  1085. static PGN pwrpgn[] = {{ 59392, 0, 0, hnd_059392, &msg_059392[0]},
  1086. { 60928, 0, 0, hnd_060928, &msg_060928[0]},
  1087. {126208, 0, 0, hnd_126208, &msg_126208[0]},
  1088. {126464, 1, 0, hnd_126464, &msg_126464[0]},
  1089. {126992, 0, 0, hnd_126992, &msg_126992[0]},
  1090. {126996, 1, 0, hnd_126996, &msg_126996[0]},
  1091. {127506, 1, 3, hnd_127506, &msg_127506[0]},
  1092. {127508, 1, 3, hnd_127508, &msg_127508[0]},
  1093. {127513, 1, 3, hnd_127513, &msg_127513[0]},
  1094. {0 , 0, 0, NULL, &msg_error [0]}};
  1095. static PGN navpgn[] = {{ 59392, 0, 0, hnd_059392, &msg_059392[0]},
  1096. { 60928, 0, 0, hnd_060928, &msg_060928[0]},
  1097. {126208, 0, 0, hnd_126208, &msg_126208[0]},
  1098. {126464, 1, 0, hnd_126464, &msg_126464[0]},
  1099. {126992, 0, 0, hnd_126992, &msg_126992[0]},
  1100. {126996, 1, 0, hnd_126996, &msg_126996[0]},
  1101. {127245, 0, 4, hnd_127245, &msg_127245[0]},
  1102. {127250, 0, 4, hnd_127250, &msg_127250[0]},
  1103. {127258, 0, 0, hnd_127258, &msg_127258[0]},
  1104. {128259, 0, 4, hnd_128259, &msg_128259[0]},
  1105. {128267, 0, 4, hnd_128267, &msg_128267[0]},
  1106. {128275, 1, 4, hnd_128275, &msg_128275[0]},
  1107. {129283, 0, 0, hnd_129283, &msg_129283[0]},
  1108. {129284, 1, 0, hnd_129284, &msg_129284[0]},
  1109. {129285, 1, 0, hnd_129285, &msg_129285[0]},
  1110. {130306, 0, 4, hnd_130306, &msg_130306[0]},
  1111. {130310, 0, 4, hnd_130310, &msg_130310[0]},
  1112. {130311, 0, 4, hnd_130311, &msg_130311[0]},
  1113. {0 , 0, 0, NULL, &msg_error [0]}};
  1114. static PGN *search_pgnlist(unsigned int pgn, PGN *pgnlist)
  1115. {
  1116. int l1;
  1117. PGN *work;
  1118. l1 = 0;
  1119. work = NULL;
  1120. while (pgnlist[l1].pgn != 0) {
  1121. if (pgnlist[l1].pgn == pgn) {
  1122. work = &pgnlist[l1];
  1123. break;
  1124. } else {
  1125. l1 = l1 + 1;
  1126. }
  1127. }
  1128. return work;
  1129. }
  1130. static void find_pgn(struct can_frame *frame, struct gps_device_t *session)
  1131. {
  1132. unsigned int can_net;
  1133. session->driver.nmea2000.workpgn = NULL;
  1134. can_net = session->driver.nmea2000.can_net;
  1135. if (can_net > (NMEA2000_NETS-1)) {
  1136. GPSD_LOG(LOG_ERROR, &session->context->errout,
  1137. "NMEA2000 find_pgn: Invalid can network %d.\n", can_net);
  1138. return;
  1139. }
  1140. if (frame->can_id & 0x80000000) {
  1141. // cppcheck-suppress unreadVariable
  1142. #ifdef __UNUSED__
  1143. unsigned int source_prio;
  1144. unsigned int daddr;
  1145. #endif
  1146. // cppcheck-suppress unreadVariable
  1147. unsigned int source_pgn;
  1148. unsigned int source_unit;
  1149. #if LOG_FILE
  1150. if (logFile != NULL) {
  1151. struct timespec msgTime;
  1152. clock_gettime(CLOCK_REALTIME, &msgTime);
  1153. (void)fprintf(logFile,
  1154. "(%010lld.%06ld) can0 %08x#",
  1155. (long long)msgTime.tv_sec,
  1156. msgTime.tv_nsec / 1000,
  1157. frame->can_id & 0x1ffffff);
  1158. if ((frame->can_dlc & 0x0f) > 0) {
  1159. int l1;
  1160. for(l1=0;l1<(frame->can_dlc & 0x0f);l1++) {
  1161. (void)fprintf(logFile, "%02x", frame->data[l1]);
  1162. }
  1163. }
  1164. (void)fprintf(logFile, "\n");
  1165. }
  1166. #endif /* of if LOG_FILE */
  1167. session->driver.nmea2000.can_msgcnt += 1;
  1168. source_pgn = (frame->can_id >> 8) & 0x1ffff;
  1169. #ifdef __UNUSED__
  1170. source_prio = (frame->can_id >> 26) & 0x7;
  1171. #endif
  1172. source_unit = frame->can_id & 0x0ff;
  1173. if (((source_pgn & 0x0ff00) >> 8) < 240) {
  1174. #ifdef __UNUSED__
  1175. daddr = source_pgn & 0x000ff;
  1176. #endif
  1177. source_pgn = source_pgn & 0x1ff00;
  1178. } else {
  1179. #ifdef __UNUSED__
  1180. daddr = 0xff;
  1181. #endif
  1182. }
  1183. if (!session->driver.nmea2000.unit_valid) {
  1184. unsigned int l1, l2;
  1185. for (l1=0;l1<NMEA2000_NETS;l1++) {
  1186. for (l2=0;l2<NMEA2000_UNITS;l2++) {
  1187. if (session == nmea2000_units[l1][l2]) {
  1188. session->driver.nmea2000.unit = l2;
  1189. session->driver.nmea2000.unit_valid = true;
  1190. session->driver.nmea2000.can_net = l1;
  1191. can_net = l1;
  1192. }
  1193. }
  1194. }
  1195. }
  1196. if (!session->driver.nmea2000.unit_valid) {
  1197. session->driver.nmea2000.unit = source_unit;
  1198. session->driver.nmea2000.unit_valid = true;
  1199. nmea2000_units[can_net][source_unit] = session;
  1200. }
  1201. if (source_unit == session->driver.nmea2000.unit) {
  1202. PGN *work;
  1203. if (session->driver.nmea2000.pgnlist != NULL) {
  1204. work = search_pgnlist(source_pgn, session->driver.nmea2000.pgnlist);
  1205. } else {
  1206. PGN *pgnlist;
  1207. pgnlist = &gpspgn[0];
  1208. work = search_pgnlist(source_pgn, pgnlist);
  1209. if (work == NULL) {
  1210. pgnlist = &aispgn[0];
  1211. work = search_pgnlist(source_pgn, pgnlist);
  1212. }
  1213. if (work == NULL) {
  1214. pgnlist = &pwrpgn[0];
  1215. work = search_pgnlist(source_pgn, pgnlist);
  1216. }
  1217. if (work == NULL) {
  1218. pgnlist = &navpgn[0];
  1219. work = search_pgnlist(source_pgn, pgnlist);
  1220. }
  1221. if ((work != NULL) && (work->type > 0)) {
  1222. session->driver.nmea2000.pgnlist = pgnlist;
  1223. }
  1224. }
  1225. if (work != NULL) {
  1226. if (work->fast == 0) {
  1227. size_t l2;
  1228. GPSD_LOG(LOG_DATA, &session->context->errout,
  1229. "pgn %6d:%s \n", work->pgn, work->name);
  1230. session->driver.nmea2000.workpgn = (void *) work;
  1231. session->lexer.outbuflen = frame->can_dlc & 0x0f;
  1232. for (l2=0;l2<session->lexer.outbuflen;l2++) {
  1233. session->lexer.outbuffer[l2]= frame->data[l2];
  1234. }
  1235. } else if ((frame->data[0] & 0x1f) == 0) {
  1236. unsigned int l2;
  1237. session->driver.nmea2000.fast_packet_len = frame->data[1];
  1238. session->driver.nmea2000.idx = frame->data[0];
  1239. #if NMEA2000_FAST_DEBUG
  1240. GPSD_LOG(LOG_ERROR, &session->context->errout,
  1241. "Set idx %2x %2x %2x %6d\n",
  1242. frame->data[0],
  1243. session->driver.nmea2000.unit,
  1244. frame->data[1],
  1245. source_pgn);
  1246. #endif /* of #if NMEA2000_FAST_DEBUG */
  1247. session->lexer.inbuflen = 0;
  1248. session->driver.nmea2000.idx += 1;
  1249. for (l2=2;l2<8;l2++) {
  1250. session->lexer.inbuffer[session->lexer.inbuflen++] = frame->data[l2];
  1251. }
  1252. GPSD_LOG(LOG_DATA, &session->context->errout,
  1253. "pgn %6d:%s \n", work->pgn, work->name);
  1254. } else if (frame->data[0] == session->driver.nmea2000.idx) {
  1255. unsigned int l2;
  1256. for (l2=1;l2<8;l2++) {
  1257. if (session->driver.nmea2000.fast_packet_len > session->lexer.inbuflen) {
  1258. session->lexer.inbuffer[session->lexer.inbuflen++] = frame->data[l2];
  1259. }
  1260. }
  1261. if (session->lexer.inbuflen == session->driver.nmea2000.fast_packet_len) {
  1262. #if NMEA2000_FAST_DEBUG
  1263. GPSD_LOG(LOG_ERROR, &session->context->errout,
  1264. "Fast done %2x %2x %2x %2x %6d\n",
  1265. session->driver.nmea2000.idx,
  1266. frame->data[0],
  1267. session->driver.nmea2000.unit,
  1268. (unsigned int) session->driver.nmea2000.fast_packet_len,
  1269. source_pgn);
  1270. #endif /* of #if NMEA2000_FAST_DEBUG */
  1271. session->driver.nmea2000.workpgn = (void *) work;
  1272. session->lexer.outbuflen = session->driver.nmea2000.fast_packet_len;
  1273. for(l2=0;l2 < (unsigned int)session->lexer.outbuflen; l2++) {
  1274. session->lexer.outbuffer[l2] = session->lexer.inbuffer[l2];
  1275. }
  1276. session->driver.nmea2000.fast_packet_len = 0;
  1277. } else {
  1278. session->driver.nmea2000.idx += 1;
  1279. }
  1280. } else {
  1281. GPSD_LOG(LOG_ERROR, &session->context->errout,
  1282. "Fast error %2x %2x %2x %2x %6d\n",
  1283. session->driver.nmea2000.idx,
  1284. frame->data[0],
  1285. session->driver.nmea2000.unit,
  1286. (unsigned int) session->driver.nmea2000.fast_packet_len,
  1287. source_pgn);
  1288. }
  1289. } else {
  1290. GPSD_LOG(LOG_WARN, &session->context->errout,
  1291. "PGN not found %08d %08x \n",
  1292. source_pgn, source_pgn);
  1293. }
  1294. } else {
  1295. // we got a unknown unit number
  1296. if (nmea2000_units[can_net][source_unit] == NULL) {
  1297. char buffer[55];
  1298. (void) snprintf(buffer,
  1299. sizeof(buffer),
  1300. "nmea2000://%s:%u",
  1301. can_interface_name[can_net],
  1302. source_unit);
  1303. if (gpsd_add_device != NULL) {
  1304. (void) gpsd_add_device(buffer, true);
  1305. }
  1306. }
  1307. }
  1308. } else {
  1309. // we got RTR or 2.0A CAN frame, not used
  1310. }
  1311. }
  1312. static ssize_t nmea2000_get(struct gps_device_t *session)
  1313. {
  1314. struct can_frame frame;
  1315. ssize_t status;
  1316. session->lexer.outbuflen = 0;
  1317. status = read(session->gpsdata.gps_fd, &frame, sizeof(frame));
  1318. if (status == (ssize_t)sizeof(frame)) {
  1319. session->lexer.type = NMEA2000_PACKET;
  1320. find_pgn(&frame, session);
  1321. return frame.can_dlc & 0x0f;
  1322. }
  1323. return 0;
  1324. }
  1325. static gps_mask_t nmea2000_parse_input(struct gps_device_t *session)
  1326. {
  1327. gps_mask_t mask;
  1328. PGN *work;
  1329. // printf("NMEA2000 parse_input called\n");
  1330. mask = 0;
  1331. work = (PGN *) session->driver.nmea2000.workpgn;
  1332. if (work != NULL) {
  1333. mask = (work->func)(&session->lexer.outbuffer[0], (int)session->lexer.outbuflen, work, session);
  1334. session->driver.nmea2000.workpgn = NULL;
  1335. }
  1336. session->lexer.outbuflen = 0;
  1337. return mask;
  1338. }
  1339. int nmea2000_open(struct gps_device_t *session)
  1340. {
  1341. char interface_name[GPS_PATH_MAX];
  1342. socket_t sock;
  1343. int status;
  1344. int unit_number;
  1345. int can_net;
  1346. unsigned int l;
  1347. struct ifreq ifr;
  1348. struct sockaddr_can addr;
  1349. char *unit_ptr;
  1350. INVALIDATE_SOCKET(session->gpsdata.gps_fd);
  1351. session->driver.nmea2000.can_net = 0;
  1352. can_net = -1;
  1353. unit_number = -1;
  1354. (void)strlcpy(interface_name, session->gpsdata.dev.path + 11, sizeof(interface_name));
  1355. unit_ptr = NULL;
  1356. for (l=0;l<strnlen(interface_name,sizeof(interface_name));l++) {
  1357. if (interface_name[l] == ':') {
  1358. unit_ptr = &interface_name[l+1];
  1359. interface_name[l] = 0;
  1360. continue;
  1361. }
  1362. if (unit_ptr != NULL) {
  1363. if (isdigit(interface_name[l]) == 0) {
  1364. GPSD_LOG(LOG_ERROR, &session->context->errout,
  1365. "NMEA2000 open: Invalid character in unit number.\n");
  1366. return -1;
  1367. }
  1368. }
  1369. }
  1370. if (unit_ptr != NULL) {
  1371. unit_number = atoi(unit_ptr);
  1372. if ((unit_number < 0) || (unit_number > (NMEA2000_UNITS-1))) {
  1373. GPSD_LOG(LOG_ERROR, &session->context->errout,
  1374. "NMEA2000 open: Unit number out of range.\n");
  1375. return -1;
  1376. }
  1377. for (l = 0; l < NMEA2000_NETS; l++) {
  1378. if (strncmp(can_interface_name[l],
  1379. interface_name,
  1380. MIN(sizeof(interface_name), sizeof(can_interface_name[l]))) == 0) {
  1381. can_net = l;
  1382. break;
  1383. }
  1384. }
  1385. if (can_net < 0) {
  1386. GPSD_LOG(LOG_ERROR, &session->context->errout,
  1387. "NMEA2000 open: CAN device not open: %s .\n", interface_name);
  1388. return -1;
  1389. }
  1390. } else {
  1391. for (l = 0; l < NMEA2000_NETS; l++) {
  1392. if (strncmp(can_interface_name[l],
  1393. interface_name,
  1394. MIN(sizeof(interface_name), sizeof(can_interface_name[l]))) == 0) {
  1395. GPSD_LOG(LOG_ERROR, &session->context->errout,
  1396. "NMEA2000 open: CAN device duplicate open: %s .\n",
  1397. interface_name);
  1398. return -1;
  1399. }
  1400. }
  1401. for (l = 0; l < NMEA2000_NETS; l++) {
  1402. if (can_interface_name[l][0] == 0) {
  1403. can_net = l;
  1404. break;
  1405. }
  1406. }
  1407. if (can_net < 0) {
  1408. GPSD_LOG(LOG_ERROR, &session->context->errout,
  1409. "NMEA2000 open: Too many CAN networks open.\n");
  1410. return -1;
  1411. }
  1412. }
  1413. /* Create the socket */
  1414. sock = socket(PF_CAN, SOCK_RAW, CAN_RAW);
  1415. if (BAD_SOCKET(sock)) {
  1416. GPSD_LOG(LOG_ERROR, &session->context->errout,
  1417. "NMEA2000 open: can not get socket.\n");
  1418. return -1;
  1419. }
  1420. status = fcntl(sock, F_SETFL, O_NONBLOCK);
  1421. if (status != 0) {
  1422. GPSD_LOG(LOG_ERROR, &session->context->errout,
  1423. "NMEA2000 open: can not set socket to O_NONBLOCK.\n");
  1424. close(sock);
  1425. return -1;
  1426. }
  1427. /* Locate the interface you wish to use */
  1428. strlcpy(ifr.ifr_name, interface_name, sizeof(ifr.ifr_name));
  1429. status = ioctl(sock, SIOCGIFINDEX, &ifr); /* ifr.ifr_ifindex gets filled
  1430. * with that device's index */
  1431. if (status != 0) {
  1432. GPSD_LOG(LOG_ERROR, &session->context->errout,
  1433. "NMEA2000 open: can not find CAN device.\n");
  1434. close(sock);
  1435. return -1;
  1436. }
  1437. /* Select that CAN interface, and bind the socket to it. */
  1438. addr.can_family = AF_CAN;
  1439. addr.can_ifindex = ifr.ifr_ifindex;
  1440. status = bind(sock, (struct sockaddr*)&addr, sizeof(addr) );
  1441. if (status != 0) {
  1442. GPSD_LOG(LOG_ERROR, &session->context->errout,
  1443. "NMEA2000 open: bind failed.\n");
  1444. close(sock);
  1445. return -1;
  1446. }
  1447. gpsd_switch_driver(session, "NMEA2000");
  1448. session->gpsdata.gps_fd = sock;
  1449. session->sourcetype = source_can;
  1450. session->servicetype = service_sensor;
  1451. session->driver.nmea2000.can_net = can_net;
  1452. if (unit_ptr != NULL) {
  1453. nmea2000_units[can_net][unit_number] = session;
  1454. session->driver.nmea2000.unit = unit_number;
  1455. session->driver.nmea2000.unit_valid = true;
  1456. } else {
  1457. strlcpy(can_interface_name[can_net],
  1458. interface_name,
  1459. MIN(sizeof(can_interface_name[0]), sizeof(interface_name)));
  1460. session->driver.nmea2000.unit_valid = false;
  1461. for (l=0;l<NMEA2000_UNITS;l++) {
  1462. nmea2000_units[can_net][l] = NULL;
  1463. }
  1464. }
  1465. session->gpsdata.dev.parity = 'N';
  1466. session->gpsdata.dev.baudrate = 250000;
  1467. session->gpsdata.dev.stopbits = 0;
  1468. return session->gpsdata.gps_fd;
  1469. }
  1470. void nmea2000_close(struct gps_device_t *session)
  1471. {
  1472. if (!BAD_SOCKET(session->gpsdata.gps_fd)) {
  1473. GPSD_LOG(LOG_SPIN, &session->context->errout,
  1474. "close(%d) in nmea2000_close(%s)\n",
  1475. session->gpsdata.gps_fd, session->gpsdata.dev.path);
  1476. (void)close(session->gpsdata.gps_fd);
  1477. INVALIDATE_SOCKET(session->gpsdata.gps_fd);
  1478. if (session->driver.nmea2000.unit_valid) {
  1479. unsigned int l1, l2;
  1480. for (l1=0;l1<NMEA2000_NETS;l1++) {
  1481. for (l2=0;l2<NMEA2000_UNITS;l2++) {
  1482. if (session == nmea2000_units[l1][l2]) {
  1483. session->driver.nmea2000.unit_valid = false;
  1484. session->driver.nmea2000.unit = 0;
  1485. session->driver.nmea2000.can_net = 0;
  1486. nmea2000_units[l1][l2] = NULL;
  1487. }
  1488. }
  1489. }
  1490. }
  1491. }
  1492. }
  1493. /* *INDENT-OFF* */
  1494. const struct gps_type_t driver_nmea2000 = {
  1495. .type_name = "NMEA2000", /* full name of type */
  1496. .packet_type = NMEA2000_PACKET, /* associated lexer packet type */
  1497. .flags = DRIVER_STICKY, /* remember this */
  1498. .trigger = NULL, /* detect their main sentence */
  1499. .channels = 12, /* not an actual GPS at all */
  1500. .probe_detect = NULL,
  1501. .get_packet = nmea2000_get, /* how to get a packet */
  1502. .parse_packet = nmea2000_parse_input, /* how to interpret a packet */
  1503. .rtcm_writer = NULL, /* Don't send RTCM to this */
  1504. .init_query = NULL, /* non-perturbing query */
  1505. .event_hook = NULL,
  1506. #ifdef RECONFIGURE_ENABLE
  1507. .speed_switcher = NULL, /* no speed switcher */
  1508. .mode_switcher = NULL, /* no mode switcher */
  1509. .rate_switcher = NULL, /* no rate switcher */
  1510. .min_cycle.tv_sec = 1, /* not relevant, no rate switch */
  1511. .min_cycle.tv_nsec = 0, /* not relevant, no rate switch */
  1512. #endif /* RECONFIGURE_ENABLE */
  1513. #ifdef CONTROLSEND_ENABLE
  1514. .control_send = NULL, /* how to send control strings */
  1515. #endif /* CONTROLSEND_ENABLE */
  1516. .time_offset = NULL,
  1517. };
  1518. /* *INDENT-ON* */
  1519. /* end */
  1520. #else /* of defined(NMEA2000_ENABLE) */
  1521. /* dummy variable to some old linkers do not commplain about empty
  1522. * object file */
  1523. int nmea2000_dummy = 1;
  1524. #endif /* of defined(NMEA2000_ENABLE) */