MAVLinkInterface: logdownload mod

ArduPilot · Aug 12, 2024 · 0fdf550 · 0fdf550
1 parent 12a7456
commit 0fdf550
Showing 1 changed file with 16 additions and 3 deletions.
diff --git a/ExtLibs/ArduPilot/Mavlink/MAVLinkInterface.cs b/ExtLibs/ArduPilot/Mavlink/MAVLinkInterface.cs
@@ -7,6 +7,7 @@
 using Newtonsoft.Json;
 using System;
 using System.Collections;
+using System.Collections.Concurrent;
 using System.Collections.Generic;
 using System.Diagnostics;
 using System.IO;
@@ -5885,7 +5886,7 @@ public async Task<string> GetLog(byte sysid, byte compid, ushort no)
                 Hashtable set = new Hashtable();
 
                 giveComport = true;
-                MAVLinkMessage buffer;
+                MAVLinkMessage buffer = MAVLinkMessage.Invalid;
 
                 if (Progress != null)
                 {
@@ -5912,6 +5913,13 @@ public async Task<string> GetLog(byte sysid, byte compid, ushort no)
                 DateTime start = DateTime.Now;
                 int retrys = 3;
 
+                ConcurrentQueue<MAVLinkMessage> queue = new ConcurrentQueue<MAVLinkMessage>();
+                EventHandler<MAVLinkMessage> handler = (sender, msg) =>
+                                {
+                                    queue.Enqueue(msg);
+                                };
+                OnPacketReceived += handler;
+
                 while (true)
                 {
                     if (!(start.AddMilliseconds(3000) > DateTime.Now))
@@ -5926,11 +5934,13 @@ public async Task<string> GetLog(byte sysid, byte compid, ushort no)
                         }
 
                         giveComport = false;
+                        OnPacketReceived -= handler;
                         throw new TimeoutException("Timeout on read - GetLog");
                     }
 
                     var start1 = DateTime.Now;
-                    buffer = await readPacketAsync().ConfigureAwait(false);
+                    if (!queue.TryDequeue(out buffer))
+                        Thread.Sleep(10);
                     var end = DateTime.Now - start1;
                     var lapse = end.TotalMilliseconds;
                     //Console.WriteLine("readPacketAsync: " + lapse);
@@ -5994,6 +6004,7 @@ public async Task<string> GetLog(byte sysid, byte compid, ushort no)
                     if (totallength == ms.Length && set.Count >= ((totallength) / 90 + 1))
                     {
                         giveComport = false;
+                        OnPacketReceived -= handler;
                         return filename;
                     }
 
@@ -6023,7 +6034,8 @@ public async Task<string> GetLog(byte sysid, byte compid, ushort no)
                         }
                     }
 
-                    buffer = await readPacketAsync().ConfigureAwait(false);
+                    if (!queue.TryDequeue(out buffer))
+                        Thread.Sleep(10);
                     if (buffer.Length > 5)
                     {
                         if (buffer.msgid == (byte) MAVLINK_MSG_ID.LOG_DATA && buffer.sysid == req.target_system &&
@@ -6076,6 +6088,7 @@ public async Task<string> GetLog(byte sysid, byte compid, ushort no)
                     }
                 }
 
+                OnPacketReceived -= handler;
                 throw new Exception("Failed to get log");
             }
         }