More work on AIVDM messages in nmea mode.

1 files changed, 84 insertions, 19 deletions

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 */