wp: fix mavftp for non-location commands

RotorsAndCams · Oct 25, 2022 · c1ab979 · c1ab979
1 parent 53e1c08
commit c1ab979
Showing 1 changed file with 30 additions and 4 deletions.
diff --git a/ExtLibs/Utilities/locationwp.cs b/ExtLibs/Utilities/locationwp.cs
@@ -32,6 +32,20 @@ public static implicit operator MAVLink.mavlink_mission_item_int_t(Locationwp in
             return (MAVLink.mavlink_mission_item_int_t)Convert(input, true);
         }
 
+        public static bool isLocationCommand(ushort id)
+        {
+            switch (id)
+            {
+            case (ushort)MAVLink.MAV_CMD.DO_DIGICAM_CONTROL:
+            case (ushort)MAVLink.MAV_CMD.ATTITUDE_TIME:
+            case (ushort)MAVLink.MAV_CMD.SCRIPT_TIME:
+            case (ushort)MAVLink.MAV_CMD.DO_GIMBAL_MANAGER_PITCHYAW:
+                return false;
+            default:
+                return true;
+            }
+        }
+
         public static implicit operator Locationwp(MAVLink.mavlink_set_position_target_global_int_t input)
         {
             Locationwp temp = new Locationwp()
@@ -72,15 +86,21 @@ public static implicit operator Locationwp(MAVLink.mavlink_mission_item_t input)
 
         public static implicit operator Locationwp(MAVLink.mavlink_mission_item_int_t input)
         {
+            double x = input.x;
+            double y = input.y;
+            if (isLocationCommand(input.command)) {
+                x *= 1.0e-7;
+                y *= 1.0e-7;
+            }
             Locationwp temp = new Locationwp()
             {
                 id = input.command,
                 p1 = input.param1,
                 p2 = input.param2,
                 p3 = input.param3,
                 p4 = input.param4,
-                lat = input.x / 1.0e7,
-                lng = input.y / 1.0e7,
+                lat = x,
+                lng = y,
                 alt = input.z,
                 _seq = input.seq,
                 frame = input.frame
@@ -125,15 +145,21 @@ static object Convert(Locationwp cmd, bool isint = false)
         {
             if (isint)
             {
+                double x = cmd.lat;
+                double y = cmd.lng;
+                if (isLocationCommand(cmd.id)) {
+                    x *= 1.0e7;
+                    y *= 1.0e7;
+                }
                 var temp = new MAVLink.mavlink_mission_item_int_t()
                 {
                     command = cmd.id,
                     param1 = cmd.p1,
                     param2 = cmd.p2,
                     param3 = cmd.p3,
                     param4 = cmd.p4,
-                    x = (int)(cmd.lat * 1.0e7),
-                    y = (int)(cmd.lng * 1.0e7),
+                    x = (int)(x),
+                    y = (int)(y),
                     z = (float) cmd.alt,
                     seq = cmd._seq,
                     frame = cmd.frame