From ed5ed7645fad2521c8360163118df6f09493ce98 Mon Sep 17 00:00:00 2001
From: Michael Oborne <mich146@hotmail.com>
Date: Tue, 28 Apr 2020 18:51:16 +0800
Subject: [PATCH] UAVCan: add toggle checking

---
 ExtLibs/UAVCAN/UAVCan.cs | 19 +++++++++++++++++--
 1 file changed, 17 insertions(+), 2 deletions(-)

diff --git a/ExtLibs/UAVCAN/UAVCan.cs b/ExtLibs/UAVCAN/UAVCan.cs
index c333aa932c..a5c132f60a 100644
--- 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;
                     }
                 }