From c8a15152e74788002ec0129fd36d3f9845ebd707 Mon Sep 17 00:00:00 2001 From: Reinhard Arlt Date: Tue, 23 Jul 2013 10:56:57 +0200 Subject: More work on AIVDM messages in nmea mode. --- gpsd.h-tail | 2 +- pseudoais.c | 95 ++++++++++++++++++++++++++++++++++++++++++++++++++++-- pseudonmea.c | 103 ++++++++++++++++++++++++++++++++++++++++++++++++----------- 3 files changed, 178 insertions(+), 22 deletions(-) diff --git a/gpsd.h-tail b/gpsd.h-tail index abe36a06..f127467f 100644 --- a/gpsd.h-tail +++ b/gpsd.h-tail @@ -761,7 +761,7 @@ extern void nmea_tpv_dump(struct gps_device_t *, /*@out@*/char[], size_t); extern void nmea_sky_dump(struct gps_device_t *, /*@out@*/char[], size_t); extern void nmea_subframe_dump(struct gps_device_t *, /*@out@*/char[], size_t); extern void nmea_ais_dump(struct gps_device_t *, /*@out@*/char[], size_t); -extern unsigned int ais_binary_encode(struct ais_t *ais, /*@out@*/unsigned char *bits); +extern unsigned int ais_binary_encode(struct ais_t *ais, /*@out@*/unsigned char *bits, int flag); extern void ntpshm_init(struct gps_context_t *, bool); extern int ntpshm_put(struct gps_device_t *, double, double); diff --git a/pseudoais.c b/pseudoais.c index 93a6658c..b3acae23 100644 --- a/pseudoais.c +++ b/pseudoais.c @@ -18,6 +18,16 @@ static unsigned char convtab[] = {"0123456789:;<=>?@ABCDEFGHIJKLMNOPQRSTUVW`abcdefghijklmnopqrstuvw"}; +static unsigned char contab1[] = {0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff, + 0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff, + 0x20,0x21,0x22,0x23,0x24,0x25,0x26,0x27,0x28,0x29,0x2a,0x2b,0x2c,0x2d,0x2e,0x2f, + 0x30,0x31,0x32,0x33,0x34,0x35,0x36,0x37,0x38,0x39,0x3a,0x3b,0x3c,0x3d,0x3e,0x3f, + 0x00,0x01,0x02,0x03,0x04,0x05,0x06,0x07,0x08,0x09,0x0a,0x0b,0x0c,0x0d,0x0e,0x0f, + 0x10,0x11,0x12,0x13,0x14,0x15,0x16,0x17,0x18,0x19,0x1a,0x1b,0x1c,0x1d,0x1e,0x1f, + 0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff, + 0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff}; + + static unsigned int ais_addbits(unsigned char *bits, unsigned int start, unsigned int len, @@ -52,6 +62,32 @@ static unsigned int ais_addbits(unsigned char *bits, } +static unsigned int ais_addchar(unsigned char *bits, + unsigned int start, + unsigned int len, + char *data) +{ + unsigned int l; + unsigned int flag; + + for(l=0,flag=0;ltype5.ais_version); + ais_addbits(bits, 40, 30, ais->type5.imo); + ais_addchar(bits, 70, 7, ais->type5.callsign); + ais_addchar(bits, 112, 20, ais->type5.shipname); + ais_addbits(bits, 232, 8, ais->type5.shiptype); + ais_addbits(bits, 240, 9, ais->type5.to_bow); + ais_addbits(bits, 249, 9, ais->type5.to_stern); + ais_addbits(bits, 258, 6, ais->type5.to_port); + ais_addbits(bits, 264, 6, ais->type5.to_starboard); + ais_addbits(bits, 270, 4, ais->type5.epfd); + ais_addbits(bits, 274, 4, ais->type5.month); + ais_addbits(bits, 278, 5, ais->type5.day); + ais_addbits(bits, 283, 5, ais->type5.hour); + ais_addbits(bits, 288, 6, ais->type5.minute); + ais_addbits(bits, 294, 8, ais->type5.draught); + ais_addchar(bits, 302, 20, ais->type5.destination); + ais_addbits(bits, 422, 1, ais->type5.dte); +/* ais_addbits(bits, 423, 1, ais->type5.spare); */ + len = 423 + 1; + break; case 9: /* Standard SAR Aircraft Position Report */ + ais_addbits(bits, 38, 12, ais->type9.alt); + ais_addbits(bits, 50, 10, ais->type9.speed); + ais_addbits(bits, 60, 1, ais->type9.accuracy); + ais_addbits(bits, 61, 28, ais->type9.lon); + ais_addbits(bits, 89, 27, ais->type9.lat); + ais_addbits(bits, 116, 12, ais->type9.course); + ais_addbits(bits, 128, 6, ais->type9.second); + ais_addbits(bits, 134, 8, ais->type9.regional); + ais_addbits(bits, 142, 1, ais->type9.dte); +/* ais_addbits(bits, 143, 3, ais->type9.spare); */ + ais_addbits(bits, 146, 1, ais->type9.assigned); + ais_addbits(bits, 147, 1, ais->type9.raim); + ais_addbits(bits, 148, 19, ais->type9.radio); + len = 148 + 19; break; case 18: /* Standard Class B CS Position Report */ ais_addbits(bits, 38, 8, ais->type18.reserved); @@ -139,6 +209,27 @@ unsigned int ais_binary_encode(struct ais_t *ais, len = 148 + 20; break; case 19: /* Extended Class B CS Position Report */ + ais_addbits(bits, 38, 8, ais->type19.reserved); + ais_addbits(bits, 46, 10, ais->type19.speed); + ais_addbits(bits, 56, 1, ais->type19.accuracy); + ais_addbits(bits, 57, 28, ais->type19.lon); + ais_addbits(bits, 85, 27, ais->type19.lat); + ais_addbits(bits, 112, 12, ais->type19.course); + ais_addbits(bits, 124, 9, ais->type19.heading); + ais_addbits(bits, 133, 6, ais->type19.second); + ais_addbits(bits, 139, 4, ais->type19.regional); + ais_addchar(bits, 143, 20, ais->type19.shipname); + ais_addbits(bits, 263, 8, ais->type19.shiptype); + ais_addbits(bits, 271, 9, ais->type19.to_bow); + ais_addbits(bits, 280, 9, ais->type19.to_stern); + ais_addbits(bits, 289, 6, ais->type19.to_port); + ais_addbits(bits, 295, 6, ais->type19.to_starboard); + ais_addbits(bits, 299, 4, ais->type19.epfd); + ais_addbits(bits, 302, 1, ais->type19.raim); + ais_addbits(bits, 305, 1, ais->type19.dte); + ais_addbits(bits, 306, 1, ais->type19.assigned); +/* ais_addbits(bits, 307, 5, ais->type19.spare); */ + len = 307 + 5; break; case 21: /* Aid-to-Navigation Report */ break; diff --git a/pseudonmea.c b/pseudonmea.c index 01111086..82f25d2c 100644 --- a/pseudonmea.c +++ b/pseudonmea.c @@ -277,41 +277,106 @@ static void gpsd_binary_almanac_dump(struct gps_device_t *session, } #ifdef AIVDM_ENABLE + +#define GETLEFT(a) (((a%6) == 0) ? 0 : (6 - (a%6))) + static void gpsd_binary_ais_dump(struct gps_device_t *session, char bufp[], size_t len) { - static unsigned int number = 0; char type[8] = "!AIVDM"; unsigned char data[256]; unsigned int msg1, msg2; - unsigned char number1; + static unsigned char number1; char numc[4]; char channel; unsigned int left; unsigned int datalen; + unsigned int offset; - msg1 = 1; - msg2 = 1; - number = -1; - numc[0] = 0; - data[0] = 0; - left = 0; channel = 'A'; memset(data, 0, sizeof(data)); - datalen = ais_binary_encode(&session->gpsdata.ais, &data[0]); + datalen = ais_binary_encode(&session->gpsdata.ais, &data[0], 0); + if (datalen > 6*60) { + msg1 = datalen / (6*60); + if ((datalen % (6*60)) != 0) { + msg1 += 1; + } + numc[0] = '0' + number1; + numc[1] = 0; + number1 += 1; + if (number1 > 9) { + number1 = 0; + } + offset = 0; + for (msg2=1;msg2<=msg1;msg2++) { + unsigned char old; - (void)snprintf(bufp, len, - "%s,%d,%d,%s,%c,%s,%d", - type, - msg1, - msg2, - numc, - channel, - data, - left); + old = 0; + if (strlen((char *)&data[(msg2-1)*60]) > 60) { + old = data[(msg2-0)*60]; + data[(msg2-0)*60] = 0; + } + if (datalen >= (6*60)) { + left = 0; + datalen -= 6*60; + } else { + left = GETLEFT(datalen); + } + (void)snprintf(&bufp[offset], len-offset, + "%s,%d,%d,%s,%c,%s,%d", + type, + msg1, + msg2, + numc, + channel, + &data[(msg2-1)*60], + left); - nmea_add_checksum(bufp); + nmea_add_checksum(&bufp[offset]); + if (old != 0) { + data[(msg2-0)*60] = old; + } + offset = strlen(bufp); + } + } else { + msg1 = 1; + msg2 = 1; + numc[0] = 0; + left = GETLEFT(datalen); + (void)snprintf(bufp, len, + "%s,%d,%d,%s,%c,%s,%d", + type, + msg1, + msg2, + numc, + channel, + data, + left); + + nmea_add_checksum(bufp); + } + + if (session->gpsdata.ais.type == 24) { + msg1 = 1; + msg2 = 1; + numc[0] = 0; + + memset(data, 0, sizeof(data)); + datalen = ais_binary_encode(&session->gpsdata.ais, &data[0], 1); + left = GETLEFT(datalen); + offset = strlen(bufp); + (void)snprintf(&bufp[offset], len-offset, + "%s,%d,%d,%s,%c,%s,%d", + type, + msg1, + msg2, + numc, + channel, + data, + left); + nmea_add_checksum(bufp+offset); + } } #endif /* AIVDM_ENABLE */ -- cgit v1.2.1