Fix truncating 0s when encoding frame

lib/mavlink_frame.dart

@@ -79,7 +79,18 @@ class MavlinkFrame {
     int incompatibilityFlags = 0;
     int compatibilityFlags = 0;
     var payload = message.serialize();
-    var payloadLength = payload.lengthInBytes;
+    var payloadBytes = payload.buffer.asUint8List().sublist(0);
+
+    // Truncate any zero's off of the payload. Can't be shorter than 1 byte even if all zeros
+    while(payloadBytes.length > 1){
+      if(payloadBytes.last == 0x00){
+        payloadBytes = payloadBytes.sublist(0, payloadBytes.length - 1);
+      }
+      else {
+        break;
+      }
+    }
+    var payloadLength = payloadBytes.length;
     var messageIdBytes = [
       message.mavlinkMessageId & 0xff,
       (message.mavlinkMessageId >> 8) & 0xff,
@@ -97,6 +108,9 @@ class MavlinkFrame {
     bytes.setUint8(7, messageIdBytes[0]);
     bytes.setUint8(8, messageIdBytes[1]);
     bytes.setUint8(9, messageIdBytes[2]);
+    for(int i = 0; i < payloadLength; i++){
+      bytes.setUint8(10 + i, payloadBytes[i]);
+    }
 
     var crc = CrcX25();
     crc.accumulate(payloadLength);
@@ -108,11 +122,8 @@ class MavlinkFrame {
     crc.accumulate(messageIdBytes[0]);
     crc.accumulate(messageIdBytes[1]);
     crc.accumulate(messageIdBytes[2]);
-
-    var payloadBytes = payload.buffer.asUint8List();
-    for (var i = 0; i < payloadLength; i++) {
-      bytes.setUint8(10 + i, payloadBytes[i]);
-      crc.accumulate(payloadBytes[i]);
+    for(final byte in payloadBytes){
+      crc.accumulate(byte);
     }
     crc.accumulate(message.mavlinkCrcExtra);
 

test/mavlink_frame_serialize_test.dart

@@ -0,0 +1,43 @@
+import 'package:dart_mavlink/dialects/common.dart';
+import 'package:dart_mavlink/mavlink_frame.dart';
+import 'package:dart_mavlink/mavlink_version.dart';
+import 'package:test/test.dart';
+
+void main() {
+  setUp(() {});
+
+  test('Serialize full v2 frame with extension fields', () async {
+    // In this message mission type is an extension field
+    var msg = MissionRequestList(targetSystem: 1, targetComponent: 2, missionType: 3);
+    var frame = MavlinkFrame(MavlinkVersion.v2, 0, 255, 190, msg);
+    var frameSerialized = frame.serialize();
+    var expectedBytes = [253, 3, 0, 0, 0, 255, 190, 43, 0, 0, 1, 2, 3, 243, 192];
+    expect(expectedBytes, frameSerialized.buffer.asUint8List());
+  });
+
+  test('Serialize full v2 frame with extension fields, last value set to 0 to test truncation', () async {
+    // In this message mission type is an extension field, set to 0 to test truncation
+    var msg = MissionRequestList(targetSystem: 1, targetComponent: 2, missionType: 0);
+    var frame = MavlinkFrame(MavlinkVersion.v2, 0, 255, 190, msg);
+    var frameSerialized = frame.serialize();
+    var expectedBytes = [253, 2, 0, 0, 0, 255, 190, 43, 0, 0, 1, 2, 192, 186];
+    expect(expectedBytes, frameSerialized.buffer.asUint8List());
+  });
+
+  test('Serialize full v2 frame with extension fields, all values set to 0 to test over-truncation', () async {
+    // all values set to zero in message body, the first 0 byte should be retained
+    var msg = MissionRequestList(targetSystem: 0, targetComponent: 0, missionType: 0);
+    var frame = MavlinkFrame(MavlinkVersion.v2, 0, 255, 190, msg);
+    var frameSerialized = frame.serialize();
+    var expectedBytes = [253, 1, 0, 0, 0, 255, 190, 43, 0, 0, 0, 158, 53];
+    expect(expectedBytes, frameSerialized.buffer.asUint8List());
+  });
+
+  test("Serialize large message with lots of 0 truncated bytes", () async {
+    var msg = FileTransferProtocol(targetNetwork: 1, targetSystem: 2, targetComponent: 3, payload: [30, 30, 30, 30, 30, 30, 30, 30, 30, 30]);
+    var frame = MavlinkFrame(MavlinkVersion.v2, 0, 255, 190, msg);
+    var frameSerialized = frame.serialize();
+    var expectedBytes = [253, 13, 0, 0, 0, 255, 190, 110, 0, 0, 1, 2, 3, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 141, 124];
+    expect(expectedBytes, frameSerialized);
+  });
+}

test/mavlink_parser_v2_test.dart

@@ -1,8 +1,10 @@
 import 'package:dart_mavlink/dialects/common.dart';
 import 'package:dart_mavlink/mavlink_parser.dart';
 import 'package:dart_mavlink/mavlink_version.dart';
+import 'package:dart_mavlink/mavlink_frame.dart';
 import 'package:test/test.dart';
 import 'dart:typed_data';
+import 'dart:convert';
 
 void main() {
   MavlinkParser? parser;
@@ -95,4 +97,27 @@ void main() {
       expect(serialized[i], d[i]);
     }
   });
-}
+
+test("Testing multiple statustext messages back to back. Testing truncated zeros being handled correct", () async {
+  // The following bytes are 5 status text messages with ["lorem","ipsum","dolor","sit","amet"] as their contents 
+  // Sent from a sender with sysId:compId 255:190
+ var d = Uint8List.fromList([253, 6, 0, 0, 0, 255, 190, 253, 0, 0, 6, 108, 111, 114, 101, 109, 127, 220, 253, 6, 0, 0, 1, 255, 190, 253, 0, 0, 6, 105, 112, 115, 117, 109, 199, 138, 253, 6, 0, 0, 2, 255, 190, 253, 0, 0, 6, 100, 111, 108, 111, 114, 189, 254, 253, 4, 0, 0, 3, 255, 190, 253, 0, 0, 6, 115, 105, 116, 57, 191, 253, 5, 0, 0, 4, 255, 190, 253, 0, 0, 6, 97, 109, 101, 116, 7, 103]);
+
+  var parser = MavlinkParser(MavlinkDialectCommon());
+  String str = "";
+  var numParsed = 0;
+  parser.stream.listen((MavlinkFrame frame){
+    if (frame.message is Statustext){
+      var msg = frame.message as Statustext;
+      List<int> trimmed = List.from(msg.text);
+      trimmed.removeWhere((item) => item == 0x00);
+      str += utf8.decode(trimmed);
+      numParsed += 1;
+    }
+  });
+
+  parser.parse(d);
+  await Future.doWhile(() => Future(() async {return numParsed <5;}));
+  expect(str, "loremipsumdolorsitamet");
+});
+}