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