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.
 
 
 
 
 
 

495 lines
12 KiB

  1. /*
  2. * gps2udp
  3. *
  4. * Dump NMEA to UDP socket for AIShub
  5. * gps2udp -u data.aishub.net:1234
  6. *
  7. * Author: Fulup Ar Foll (directly inspired from gpspipe.c)
  8. * Date: 2013-03-01
  9. *
  10. * This file is Copyright (c) 2013-2018 by the GPSD project
  11. * SPDX-License-Identifier: BSD-2-clause
  12. *
  13. */
  14. #include "gpsd_config.h" /* must be before all includes */
  15. #include <arpa/inet.h>
  16. #include <assert.h>
  17. #include <errno.h>
  18. #include <fcntl.h>
  19. #include <netdb.h> /* for gethostbyname() */
  20. #include <netinet/in.h>
  21. #include <stdbool.h>
  22. #include <stdio.h>
  23. #include <stdlib.h>
  24. #include <string.h> /* for strlcpy(), strsep(), etc. */
  25. #include <strings.h>
  26. #include <sys/select.h>
  27. #include <sys/socket.h>
  28. #include <sys/stat.h>
  29. #include <sys/time.h>
  30. #include <sys/types.h>
  31. #include <termios.h>
  32. #include <time.h>
  33. #include <unistd.h>
  34. #include "gpsd.h"
  35. #include "gpsdclient.h"
  36. #include "revision.h"
  37. #include "strfuncs.h"
  38. #include "timespec.h"
  39. #define MAX_TIME_LEN 80
  40. #define MAX_GPSD_RETRY 10
  41. static struct gps_data_t gpsdata;
  42. /* UDP socket variables */
  43. #define MAX_UDP_DEST 5
  44. static struct sockaddr_in remote[MAX_UDP_DEST];
  45. static int sock[MAX_UDP_DEST];
  46. static int udpchannel;
  47. /* gpsclient source */
  48. static struct fixsource_t gpsd_source;
  49. static unsigned int flags;
  50. static int debug = 0;
  51. static bool aisonly = false;
  52. static char* time2string(void)
  53. /* return local time hh:mm:ss */
  54. {
  55. static char buffer[MAX_TIME_LEN];
  56. time_t curtime;
  57. struct tm *loctime;
  58. /* Get the current time. */
  59. curtime = time (NULL);
  60. /* Convert it to local time representation. */
  61. loctime = localtime (&curtime);
  62. /* Print it out in a nice format. */
  63. (void)strftime (buffer, sizeof(buffer), "%H:%M:%S", loctime);
  64. return (buffer);
  65. }
  66. static int send_udp (char *nmeastring, size_t ind)
  67. {
  68. char message [255];
  69. char *buffer;
  70. int channel;
  71. /* if string length is unknow make a copy and compute it */
  72. if (ind == 0) {
  73. /* compute message size and add 0x0a 0x0d */
  74. for (ind=0; nmeastring [ind] != '\0'; ind ++) {
  75. if (ind >= sizeof(message) - 3) {
  76. (void)fprintf(stderr, "gps2udp: too big [%s] \n", nmeastring);
  77. return -1;
  78. }
  79. message[ind] = nmeastring[ind];
  80. }
  81. buffer = message;
  82. } else {
  83. /* use directly nmeastring but change terminition */
  84. buffer = nmeastring;
  85. ind = ind-1;
  86. }
  87. /* Add termination to NMEA feed for AISHUB */
  88. buffer[ind] = '\r'; ind++;
  89. buffer[ind] = '\n'; ind++;
  90. buffer[ind] = '\0';
  91. if ((flags & WATCH_JSON)==0 && buffer[0] == '{') {
  92. /* do not send JSON when not configured to do so */
  93. return 0;
  94. }
  95. /* send message on udp channel */
  96. for (channel=0; channel < udpchannel; channel ++) {
  97. ssize_t status = sendto(sock[channel],
  98. buffer,
  99. ind,
  100. 0,
  101. (const struct sockaddr *)&remote[channel],
  102. (int)sizeof(remote));
  103. if (status < (ssize_t)ind) {
  104. (void)fprintf(stderr, "gps2udp: failed to send [%s] \n", nmeastring);
  105. return -1;
  106. }
  107. }
  108. return 0;
  109. }
  110. static int open_udp(char **hostport)
  111. /* Open and bind udp socket to host */
  112. {
  113. int channel;
  114. for (channel=0; channel <udpchannel; channel ++)
  115. {
  116. char *hostname = NULL;
  117. char *portname = NULL;
  118. char *endptr = NULL;
  119. int portnum;
  120. struct hostent *hp;
  121. /* parse argument */
  122. hostname = strsep(&hostport[channel], ":");
  123. portname = strsep(&hostport[channel], ":");
  124. if ((hostname == NULL) || (portname == NULL)) {
  125. (void)fprintf(stderr, "gps2udp: syntax is [-u hostname:port]\n");
  126. return (-1);
  127. }
  128. errno = 0;
  129. portnum = (int)strtol(portname, &endptr, 10);
  130. if (1 > portnum || 65535 < portnum || '\0' != *endptr || 0 != errno) {
  131. (void)fprintf(stderr, "gps2udp: syntax is [-u hostname:port] [%s] is not a valid port number\n",portname);
  132. return (-1);
  133. }
  134. sock[channel]= socket(AF_INET, SOCK_DGRAM, 0);
  135. if (sock[channel] < 0) {
  136. (void)fprintf(stderr, "gps2udp: error creating UDP socket\n");
  137. return (-1);
  138. }
  139. remote[channel].sin_family = (sa_family_t)AF_INET;
  140. hp = gethostbyname(hostname);
  141. if (hp==NULL) {
  142. (void)fprintf(stderr,
  143. "gps2udp: syntax is [-u hostname:port] [%s]"
  144. " is not a valid hostname\n",
  145. hostname);
  146. return (-1);
  147. }
  148. memcpy( &remote[channel].sin_addr, hp->h_addr_list[0], hp->h_length);
  149. remote[channel].sin_port = htons((in_port_t)portnum);
  150. }
  151. return (0);
  152. }
  153. static void usage(void)
  154. {
  155. (void)fprintf(stderr,
  156. "Usage: gps2udp [OPTIONS] [server[:port[:device]]]\n\n"
  157. "-h Show this help.\n"
  158. "-u Send UDP NMEA/JSON feed to host:port [multiple -u host:port accepted]\n"
  159. "-n Feed NMEA.\n"
  160. "-j Feed JSON.\n"
  161. "-a Select AIS message only.\n"
  162. "-c [count] exit after count packets.\n"
  163. "-b Run in background as a daemon.\n"
  164. "-d [0-2] 1 display sent packets, 2 display ignored packets.\n"
  165. "-v Print version and exit.\n\n"
  166. "example: gps2udp -a -n -c 2 -d 1 -u data.aishub.net:2222 fridu.net\n"
  167. );
  168. }
  169. static void connect2gpsd(bool restart)
  170. /* loop until we connect with gpsd */
  171. {
  172. unsigned int delay;
  173. if (restart) {
  174. (void)gps_close(&gpsdata);
  175. if (debug > 0)
  176. (void)fprintf(stdout,
  177. "gps2udp [%s] reset gpsd connection\n",
  178. time2string());
  179. }
  180. /* loop until we reach GPSd */
  181. for (delay = 10; ; delay = delay*2) {
  182. int status = gps_open(gpsd_source.server, gpsd_source.port, &gpsdata);
  183. if (status != 0) {
  184. (void)fprintf(stderr,
  185. "gps2udp [%s] connection failed at %s:%s\n",
  186. time2string(), gpsd_source.server, gpsd_source.port);
  187. (void)sleep(delay);
  188. } else {
  189. if (debug > 0)
  190. (void)fprintf(stdout, "gps2udp [%s] connect to gpsd %s:%s\n",
  191. time2string(), gpsd_source.server, gpsd_source.port);
  192. break;
  193. }
  194. }
  195. /* select the right set of gps data */
  196. (void)gps_stream(&gpsdata, flags, gpsd_source.device);
  197. }
  198. static ssize_t read_gpsd(char *message, size_t len)
  199. /* get data from gpsd */
  200. {
  201. int ind;
  202. char c;
  203. int retry=0;
  204. /* allow room for trailing NUL */
  205. len--;
  206. /* loop until we get some data or an error */
  207. for (ind = 0; ind < (int)len;) {
  208. /* prepare for a blocking read with a 10s timeout */
  209. int result = nanowait(gpsdata.gps_fd, 10 % NS_IN_SEC);
  210. switch (result)
  211. {
  212. case 1: /* we have data waiting, let's process them */
  213. result = (int)read(gpsdata.gps_fd, &c, 1);
  214. /* If we lost gpsd connection reset it */
  215. if (result != 1) {
  216. connect2gpsd (true);
  217. }
  218. if ((c == '\n') || (c == '\r')){
  219. message[ind]='\0';
  220. if (ind > 0) {
  221. if (retry > 0) {
  222. if (debug ==1)
  223. (void)fprintf (stdout,"\r");
  224. if (debug > 1)
  225. (void)fprintf(stdout,
  226. " [%s] No Data for: %ds\n",
  227. time2string(), retry*10);
  228. }
  229. if (aisonly && message[0] != '!') {
  230. if (debug >1)
  231. (void)fprintf(stdout,
  232. ".... [%s %d] %s\n", time2string(),
  233. ind, message);
  234. return(0);
  235. }
  236. }
  237. return ((ssize_t)ind+1);
  238. } else {
  239. message[ind]= c;
  240. ind++;
  241. }
  242. break;
  243. case 0: /* no data fail in timeout */
  244. retry++;
  245. /* if too many empty packets are received reset gpsd connection */
  246. if (retry > MAX_GPSD_RETRY)
  247. {
  248. connect2gpsd(true);
  249. retry = 0;
  250. }
  251. if (debug > 0)
  252. ignore_return(write (1, ".", 1));
  253. break;
  254. default: /* we lost connection with gpsd */
  255. connect2gpsd(true);
  256. break;
  257. }
  258. }
  259. message[ind] = '\0';
  260. (void)fprintf (stderr,"\n gps2udp: message too big [%s]\n", message);
  261. return(-1);
  262. }
  263. static unsigned char AISto6bit(unsigned char c)
  264. /* 6 bits decoding of AIS payload */
  265. {
  266. unsigned char cp = c;
  267. if(c < (unsigned char)0x30)
  268. return (unsigned char)-1;
  269. if(c > (unsigned char)0x77)
  270. return (unsigned char)-1;
  271. if(((unsigned char)0x57 < c) && (c < (unsigned char)0x60))
  272. return (unsigned char)-1;
  273. cp += (unsigned char)0x28;
  274. if(cp > (unsigned char)0x80)
  275. cp += (unsigned char)0x20;
  276. else
  277. cp += (unsigned char)0x28;
  278. return (unsigned char)(cp & (unsigned char)0x3f);
  279. }
  280. static unsigned int AISGetInt(unsigned char *bitbytes, unsigned int sp, unsigned int len)
  281. /* get MMSI from AIS bit string */
  282. {
  283. unsigned int acc = 0;
  284. unsigned int s0p = sp-1; // to zero base
  285. unsigned int i;
  286. for(i=0 ; i<len ; i++)
  287. {
  288. unsigned int cp, cx, c0;
  289. acc = acc << 1;
  290. cp = (s0p + i) / 6;
  291. cx = (unsigned int)bitbytes[cp]; // what if cp >= byte_length?
  292. c0 = (cx >> (5 - ((s0p + i) % 6))) & 1;
  293. acc |= c0;
  294. }
  295. return acc;
  296. }
  297. int main(int argc, char **argv)
  298. {
  299. bool daemonize = false;
  300. long count = -1;
  301. int option;
  302. char *udphostport[MAX_UDP_DEST];
  303. flags = WATCH_ENABLE;
  304. while ((option = getopt(argc, argv, "?habnjvc:l:u:d:")) != -1)
  305. {
  306. switch (option) {
  307. case 'd':
  308. debug = atoi(optarg);
  309. if ((debug <1) || (debug > 2)) {
  310. usage();
  311. exit(1);
  312. }
  313. break;
  314. case 'n':
  315. if (debug >0)
  316. (void)fprintf(stdout, "NMEA selected\n");
  317. flags |= WATCH_NMEA;
  318. break;
  319. case 'j':
  320. if (debug >0)
  321. (void)fprintf(stdout, "JSON selected\n");
  322. flags |= WATCH_JSON;
  323. break;
  324. case 'a':
  325. aisonly = true;
  326. break;
  327. case 'c':
  328. count = atol(optarg);
  329. break;
  330. case 'b':
  331. daemonize = true;
  332. break;
  333. case 'u':
  334. if (udpchannel >= MAX_UDP_DEST) {
  335. (void)fprintf(stderr,
  336. "gps2udp: too many UDP destinations (max=%d)\n",
  337. MAX_UDP_DEST);
  338. } else {
  339. udphostport[udpchannel++] = optarg;
  340. }
  341. break;
  342. case 'v':
  343. (void)fprintf(stderr, "%s: %s (revision %s)\n",
  344. argv[0], VERSION, REVISION);
  345. exit(0);
  346. case '?':
  347. case 'h':
  348. default:
  349. usage();
  350. exit(1);
  351. }
  352. }
  353. /* Grok the server, port, and device. */
  354. if (optind < argc)
  355. gpsd_source_spec(argv[optind], &gpsd_source);
  356. else
  357. gpsd_source_spec(NULL, &gpsd_source);
  358. if (gpsd_source.device != NULL)
  359. flags |= WATCH_DEVICE;
  360. /* check before going background if we can connect to gpsd */
  361. connect2gpsd(false);
  362. /* Open UDP port */
  363. if (udpchannel > 0) {
  364. int status = open_udp(udphostport);
  365. if (status !=0) exit (1);
  366. }
  367. /* Daemonize if the user requested it. */
  368. if (daemonize) {
  369. if (os_daemon(0, 0) != 0) {
  370. (void)fprintf(stderr,
  371. "gps2udp: daemonization failed: %s\n",
  372. strerror(errno));
  373. }
  374. }
  375. /* infinite loop to get data from gpsd and push them to aggregators */
  376. for (;;)
  377. {
  378. char buffer[512];
  379. ssize_t len;
  380. len = read_gpsd(buffer, sizeof(buffer));
  381. /* ignore empty message */
  382. if (len > 3)
  383. {
  384. if (debug > 0)
  385. {
  386. (void)fprintf (stdout,"---> [%s] -- %s",time2string(),buffer);
  387. // Try to extract MMSI from AIS payload
  388. if (str_starts_with(buffer, "!AIVDM"))
  389. {
  390. #define MAX_INFO 6
  391. int i,j;
  392. unsigned char packet[512];
  393. unsigned char *adrpkt = packet;
  394. unsigned char *info[MAX_INFO];
  395. unsigned int mmsi;
  396. unsigned char bitstrings [255];
  397. // strtok break original string
  398. (void)strlcpy((char *)packet, buffer, sizeof(packet));
  399. for (j=0; j<MAX_INFO; j++) {
  400. info[j] = (unsigned char *)strsep((char **)&adrpkt, ",");
  401. }
  402. for(i=0 ; i < (int)strlen((char *)info[5]); i++) {
  403. if (i > (int) sizeof (bitstrings)) break;
  404. bitstrings[i] = AISto6bit(info[5][i]);
  405. }
  406. mmsi = AISGetInt(bitstrings, 9, 30);
  407. (void)fprintf(stdout," MMSI=%9u", mmsi);
  408. }
  409. (void)fprintf(stdout,"\n");
  410. }
  411. // send to all UDP destinations
  412. if (udpchannel > 0)
  413. (void)send_udp(buffer, (size_t)len);
  414. // if we count messages check it now
  415. if (count >= 0) {
  416. if (count-- == 0) {
  417. /* completed count */
  418. (void)fprintf(stderr,
  419. "gpsd2udp: normal exit after counted packets\n");
  420. exit (0);
  421. }
  422. } // end count
  423. } // end len > 3
  424. } // end for (;;)
  425. // This is an infinite loop, should never be here
  426. (void)fprintf (stderr, "gpsd2udp ERROR abnormal exit\n");
  427. exit (-1);
  428. }
  429. /* end */