ExtLibs: ArduPilot: Mavlink: try and pull AP firmware type from versi…

…on string
RotorsAndCams · Feb 1, 2023 · f3aed96 · f3aed96
1 parent 6233bb0
commit f3aed96
Showing 1 changed file with 22 additions and 0 deletions.
diff --git a/ExtLibs/ArduPilot/Mavlink/MAVLinkInterface.cs b/ExtLibs/ArduPilot/Mavlink/MAVLinkInterface.cs
@@ -1715,6 +1715,8 @@ public async Task<MAVLinkParamList> getParamListMavftpAsync(byte sysid, byte com
                         logdata.ToLower().Contains("plane"))
                     {
                         MAVlist[sysid, compid].VersionString = logdata;
+                        // Version string may allow to better identify type
+                        setAPType(sysid, compid);
                     }
                     else if (logdata.ToLower().Contains("nuttx") || logdata.ToLower().Contains("chibios"))
                     {
@@ -6548,6 +6550,26 @@ public void setAPType(byte sysid, byte compid)
             switch (MAVlist[sysid, compid].apname)
             {
                 case MAV_AUTOPILOT.ARDUPILOTMEGA:
+                    // try and pull vehicle type from version string
+                    if (MAVlist[sysid, compid].VersionString != null && MAVlist[sysid, compid].VersionString.Length > 1)
+                    {
+                        var firmware_lookup = new Dictionary<string, Firmwares>()
+                                {
+                                    {"ArduPlane V", Firmwares.ArduPlane},
+                                    {"ArduCopter V", Firmwares.ArduCopter2},
+                                    {"Blimp V", Firmwares.ArduCopter2},
+                                    {"ArduRover V",Firmwares.ArduRover},
+                                    {"ArduSub V",Firmwares.ArduSub},
+                                    {"AntennaTracker V", Firmwares.ArduTracker}
+                                };
+                        var match = firmware_lookup.Where(d => MAVlist[sysid, compid].VersionString.StartsWith(d.Key)).ToArray();
+                        if (match.Count() == 1)
+                        {
+                            MAVlist[sysid, compid].cs.firmware = match[0].Value;
+                            break;
+                        }
+                    }
+
                     switch (MAVlist[sysid, compid].aptype)
                     {
                         case MAV_TYPE.FIXED_WING: