summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--gpsd.h-tail2
-rw-r--r--pseudoais.c95
-rw-r--r--pseudonmea.c103
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;l<len;l++) {
+ unsigned int a, b;
+
+ a = data[l];
+ if (a == 0) {
+ flag = 1;
+ }
+ if (flag == 0) {
+ b = contab1[a & 0x7f];
+ } else {
+ b = 0;
+ }
+ ais_addbits(bits, start+6*l, 6, b);
+ }
+ return 0;
+}
+
+
static unsigned int ais_binary_to_ascii(unsigned char *bits, unsigned int len)
{
unsigned int l;
@@ -69,7 +105,8 @@ static unsigned int ais_binary_to_ascii(unsigned char *bits, unsigned int len)
unsigned int ais_binary_encode(struct ais_t *ais,
- unsigned char *bits)
+ unsigned char *bits,
+ int flag)
{
unsigned int len;
@@ -115,8 +152,41 @@ unsigned int ais_binary_encode(struct ais_t *ais,
len = 149 + 19;
break;
case 5: /* Ship static and voyage related data */
- break;
+ ais_addbits(bits, 38, 2, ais->type5.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 */
View Lorry Controller Status