feat: add arducopter name generation

asv-soft · Nov 13, 2023 · 4dcbcec · 4dcbcec
1 parent 11f9d5b
commit 4dcbcec
Show file tree

Hide file tree

Showing 3 changed files with 19 additions and 4 deletions.
diff --git a/src/Asv.Mavlink/Devices/Base/Client/ClientDevice.cs b/src/Asv.Mavlink/Devices/Base/Client/ClientDevice.cs
@@ -69,8 +69,8 @@ private async void TryReconnect()
         _onInit.OnNext(InitState.InProgress);
         try
         {
-            _name.OnNext(await GetCustomName(DisposeCancel).ConfigureAwait(false));
             await InternalInit().ConfigureAwait(false);
+            _name.OnNext(await GetCustomName(DisposeCancel).ConfigureAwait(false));
             _onInit.OnNext(InitState.Complete);
             _needToRequestAgain = false;
         }

diff --git a/src/Asv.Mavlink/Devices/Vehicles/Client/Ardupilot/ArducopterClient.cs b/src/Asv.Mavlink/Devices/Vehicles/Client/Ardupilot/ArducopterClient.cs
@@ -23,9 +23,24 @@ public ArduCopterClient(IMavlinkV2Connection connection, MavlinkClientIdentity i
 
     }
 
-    protected override Task<string> GetCustomName(CancellationToken cancel)
+    protected override async Task<string> GetCustomName(CancellationToken cancel)
     {
-        return Task.FromResult("ArduCopter");
+        try
+        {
+            if (Params.IsInit)
+            {
+                var frameClass = ArdupilotFrameTypeHelper.ParseFrameClass((sbyte)await Params.ReadOnce("FRAME_CLASS", cancel).ConfigureAwait(false));
+                var frameType = ArdupilotFrameTypeHelper.ParseFrameType((sbyte)await Params.ReadOnce("FRAME_TYPE", cancel).ConfigureAwait(false));
+                var serial  = (int)await Params.ReadOnce("BRD_SERIAL_NUM", cancel).ConfigureAwait(false);
+                return ArdupilotFrameTypeHelper.GenerateName(frameClass, frameType,serial);
+            }
+        }
+        catch (Exception e)
+        {
+            // ignored
+        }
+
+        return $"Arducopter [{Identity.TargetSystemId:00},{Identity.TargetComponentId:00}]";
     }
 
     public override DeviceClass Class => DeviceClass.Copter;

diff --git a/src/Asv.Mavlink/Devices/Vehicles/Client/Ardupilot/ArdupilotFrameTypeEnum.cs b/src/Asv.Mavlink/Devices/Vehicles/Client/Ardupilot/ArdupilotFrameTypeEnum.cs
@@ -57,7 +57,7 @@ public static ArdupilotFrameClassEnum ParseFrameClass(long? frameType)
             return (ArdupilotFrameClassEnum)frameType.Value;
         }
 
-        public static string GenerateName(ArdupilotFrameClassEnum frameClass, ArdupilotFrameTypeEnum frameType, ushort serialNumber)
+        public static string GenerateName(ArdupilotFrameClassEnum frameClass, ArdupilotFrameTypeEnum frameType, int serialNumber)
         {
             return $"{frameClass:G} {frameType:G} [{serialNumber:D5}]";
         }