CurrentState: correct unit handling for radius (ArduPilot#3262)

This fixes the turn radius display on the map when using other speed units
Autonomost · Jan 12, 2024 · 75c40eb · 75c40eb
1 parent fb6af04
commit 75c40eb
Show file tree

Hide file tree

Showing 3 changed files with 4 additions and 4 deletions.
diff --git a/Common.cs b/Common.cs
@@ -42,7 +42,7 @@ public static GMapMarker getMAVMarker(MAVState MAV, GMapOverlay overlay = null)
                         itemp.Cog = MAV.cs.groundcourse;
                         itemp.Target = MAV.cs.target_bearing;
                         itemp.Nav_bearing = MAV.cs.nav_bearing;
-                        itemp.Radius = MAV.cs.radius * CurrentState.multiplierdist;
+                        itemp.Radius = (float)CurrentState.fromDistDisplayUnit(MAV.cs.radius);
                         return null;
                     }
                     else if (item is GMapMarkerQuad)
@@ -76,7 +76,7 @@ public static GMapMarker getMAVMarker(MAVState MAV, GMapOverlay overlay = null)
             {
                 return (new GMapMarkerPlane(MAV.sysid - 1, portlocation, MAV.cs.yaw,
                     MAV.cs.groundcourse, MAV.cs.nav_bearing, MAV.cs.target_bearing,
-                    MAV.cs.radius * CurrentState.multiplierdist)
+                    (float)CurrentState.toDistDisplayUnit(MAV.cs.radius))
                 {
                     ToolTipText = ArduPilot.Common.speechConversion(MAV, "" + Settings.Instance["mapicondesc"]),
                     ToolTipMode = String.IsNullOrEmpty(Settings.Instance["mapicondesc"]) ? MarkerTooltipMode.Never : MarkerTooltipMode.Always,

diff --git a/ExtLibs/ArduPilot/CurrentState.cs b/ExtLibs/ArduPilot/CurrentState.cs
@@ -1054,7 +1054,7 @@ public float radius
             get
             {
                 if (_groundspeed <= 1) return 0;
-                return (float)(groundspeed * groundspeed / (9.80665 * Math.Tan(roll * MathHelper.deg2rad)));
+                return (float)toDistDisplayUnit(_groundspeed * _groundspeed / (9.80665 * Math.Tan(roll * MathHelper.deg2rad)));
             }
         }
         [GroupText("Position")]

diff --git a/Swarm/Formation.cs b/Swarm/Formation.cs
@@ -191,7 +191,7 @@ public override void SendCommand()
 
                             if (true)
                             {
-                                var leaderturnrad = Leader.cs.radius;
+                                var leaderturnrad = CurrentState.fromDistDisplayUnit(Leader.cs.radius);
                                 var mavturnradius = leaderturnrad - x;
 
                                 {