UAVCan: add toggle checking

avrem · Apr 28, 2020 · ed5ed76 · ed5ed76
1 parent 5df2f2c
commit ed5ed76
Showing 1 changed file with 17 additions and 2 deletions.
diff --git a/ExtLibs/UAVCAN/UAVCan.cs b/ExtLibs/UAVCAN/UAVCan.cs
@@ -1256,7 +1256,22 @@ public void ReadMessage(string line)
 
             transfer[(packet_id, payload.TransferID)].AddRange(payload.Payload);
 
-            //todo check toggle
+            {
+                var totalbytes = transfer[(packet_id, payload.TransferID)].Count;
+
+                var current = (totalbytes / 7) % 2;
+
+                if((current == 1) == payload.Toggle)
+                {
+                    if (!payload.EOT)
+                    {
+                        transfer.Remove((packet_id, payload.TransferID));
+                        Console.WriteLine("Bad Toggle {0}", frame.MsgTypeID);
+                        return;
+                        //error here
+                    }
+                }
+            }
 
             if (payload.SOT && !payload.EOT)
             {
@@ -1331,7 +1346,7 @@ public void ReadMessage(string line)
 
                     if (crc != payload_crc)
                     {
-                        Console.WriteLine("Bad Message " + frame.MsgTypeID);
+                        Console.WriteLine("Bad Message CRC Fail " + frame.MsgTypeID);
                         return;
                     }
                 }