sv-publisher: fix invalid calculation of the total packet length in case the payload is larger than 127 bytes

src/sampled_values/sv_publisher.c

@@ -367,7 +367,15 @@ SVPublisher_ASDU_getEncodedSize(SVPublisher_ASDU self)
         encodedSize += 4;
 
     /* sample */
+    if (self->dataSize < 128)
         encodedSize += 2;
+    else if (self->dataSize < 256)
+        encodedSize += 3;
+    else if (self->dataSize < 65535)
+        encodedSize += 4;
+    else
+        encodedSize += 5;
+
     encodedSize += self->dataSize;
 
     /* smpMod */