diff --git a/ExtLibs/ArduPilot/Mavlink/MAVLinkInterface.cs b/ExtLibs/ArduPilot/Mavlink/MAVLinkInterface.cs
index e3ac888075..4ceb202f1e 100644
--- a/ExtLibs/ArduPilot/Mavlink/MAVLinkInterface.cs
+++ b/ExtLibs/ArduPilot/Mavlink/MAVLinkInterface.cs
@@ -733,7 +733,7 @@ private void OpenBg(IProgressReporterDialogue PRsender, bool getparams)
var countDown = new Timer {Interval = 1000, AutoReset = false};
countDown.Elapsed += (sender, e) =>
{
- int secondsRemaining = (deadline - e.SignalTime).Seconds;
+ int secondsRemaining = Convert.ToInt32((deadline - e.SignalTime).TotalSeconds);
frmProgressReporter.UpdateProgressAndStatus(-1, string.Format(Strings.Trying, secondsRemaining));
if (secondsRemaining > 0) countDown.Start();
};
diff --git a/ExtLibs/Mavlink/Mavlink.cs b/ExtLibs/Mavlink/Mavlink.cs
index 53788fc901..c692bf9f79 100644
--- a/ExtLibs/Mavlink/Mavlink.cs
+++ b/ExtLibs/Mavlink/Mavlink.cs
@@ -5,7 +5,7 @@
public partial class MAVLink
{
- public const string MAVLINK_BUILD_DATE = "Wed Dec 18 2024";
+ public const string MAVLINK_BUILD_DATE = "Thu Jan 09 2025";
public const string MAVLINK_WIRE_PROTOCOL_VERSION = "2.0";
public const int MAVLINK_MAX_PAYLOAD_LEN = 255;
@@ -2429,6 +2429,9 @@ public enum PLANE_MODE: int /*default*/
/// LOITER2QLAND |
[Description("LOITER2QLAND")]
LOITER_ALT_QLAND=25,
+ /// AUTOLAND |
+ [Description("AUTOLAND")]
+ AUTOLAND=26,
};
diff --git a/ExtLibs/Mavlink/mavlink.lua b/ExtLibs/Mavlink/mavlink.lua
index 5d8ffc3d1c..6e86a199be 100644
--- a/ExtLibs/Mavlink/mavlink.lua
+++ b/ExtLibs/Mavlink/mavlink.lua
@@ -901,6 +901,7 @@ local enumEntryName = {
[23] = "PLANE_MODE_QACRO",
[24] = "PLANE_MODE_THERMAL",
[25] = "PLANE_MODE_LOITER_ALT_QLAND",
+ [26] = "PLANE_MODE_AUTOLAND",
},
["COPTER_MODE"] = {
[0] = "COPTER_MODE_STABILIZE",
diff --git a/ExtLibs/Mavlink/message_definitions/ardupilotmega.xml b/ExtLibs/Mavlink/message_definitions/ardupilotmega.xml
index 63e1cfedfc..99604d5083 100644
--- a/ExtLibs/Mavlink/message_definitions/ardupilotmega.xml
+++ b/ExtLibs/Mavlink/message_definitions/ardupilotmega.xml
@@ -1146,6 +1146,9 @@
LOITER2QLAND
+
+ AUTOLAND
+
A mapping of copter flight modes for custom_mode field of heartbeat.
diff --git a/ExtLibs/Xamarin/Xamarin/Resources/mavcmd.xml b/ExtLibs/Xamarin/Xamarin/Resources/mavcmd.xml
index d2b9db9e45..cb429fe607 100644
--- a/ExtLibs/Xamarin/Xamarin/Resources/mavcmd.xml
+++ b/ExtLibs/Xamarin/Xamarin/Resources/mavcmd.xml
@@ -10,8 +10,116 @@
Lat
Long
- Alt
+ Alt
+
+
+
+
+
+
+
+ Alt
+
+
+
+
+
+
+
+
+
+
+
+ Time (sec)
+ Roll
+ Pitch
+ Yaw
+ ClimbRate
+
+
+
+
+ Seconds (or -1)
+ Hour UTC (or -1)
+ Minute UTC (or -1)
+ Second UTC
+
+
+
+
+
+ on=1/off=0
+
+
+
+
+
+
+
+
+
+
+
+
+ Lat
+ Long
+ Alt
+
+
+ Time s
+
+
+
+ Lat
+ Long
+ Alt
+
+
+ Turns
+
+ Radius
+
+ Lat
+ Long
+ Alt
+
+
+
+
+
+
+ Lat
+ Long
+ Alt
+
+
+ max descent
+
+
+
+ Lat
+ Long
+ Alt
+
+
+ command
+ timeout
+ arg1
+ arg2
+ arg3
+ arg4
+
+
+
+ ID
+ P1
+ P2
+ P3
+
+
+
+
Delay
@@ -19,35 +127,353 @@
Lat
Long
- Alt
-
+ Alt
+
+
+ Id
+ Interval
+ Total Images
+
+
+
+
+
+
+ Id
+
+
+
+
+
+
+
+
+ Type
+ Value
+
+
+
+
+
+
+
+ Type
+ Value
+
+
+
+
+
+
+
+ Id
+ Primary(1:RGB,2:IR,3:NDVI,4:Wide)
+ Secondary(1:RGB,2:IR,3:NDVI,4:Wide)
+
+
+
+
+
+
+ StreamId
+
+
+
+
+
+
+
+
+ StreamId
+
+
+
+
+
+
+
+
+ AuxFunction
+ SwitchPosition
+
+
+
+
+
+
+
+ Type (0:horiz,2:up,3:down)
+ Speed m/s
+
+
+
+
+
+
+
+ Mode
+ Shutter Speed
+ Aperture
+ ISO
+ ExposureMode
+ CommandID
+
+
+
+ Session Control
+ Zoom Position
+ Zoom Step
+ Focus Lock
+ Shutter Cmd
+ CommandID
+
+
+
+ Start Engine
+ Cold Start
+ Height Delay
+
+
+
+
+
+
+ Pitch Angle
+ Yaw Angle
+ Pitch Rate
+ Yaw Rate
+ Flags
+
+ Gimbal Id
+
+
+ Gripper No
+ drop(0)/grab(1)
+
+
+
+
+
+
+
+ timeout S
+ min alt
+ max alt
+ max dist
+
+
+
+
+
+ WP #
+ Repeat#
+
+
+
+
+
+
+
+ Tag
+
+
+
+
+
+
+
+
+ Tag
+ Repeat
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Pitch
+ Roll
+ Yaw
+
+
+
+
+
+
+ Enable(1)/Release(2)
+
+
+
+
+
+
+
+
+ Relay No
+ Repeat#
+ Delay (s)
+
+
+
+
+
+
+ Ser No
+ PWM
+ Repeat#
+ Delay (s)
+
+
+
+
+
+ Dist (m)
+
+
+
+
+
+
+
+
+ Relay No
+ off(0)/on(1)
+
+
+
+
+
+
+
+ Distance(m)
+
+
+
+
+
+
+
+
+
+
+
+
+ Lat
+ Long
+ Alt
+
+
+ Ser No
+ PWM
+
+
+
+
+
+
+
+ Sprayer Enable
+
+
+
+
+
+
+
+
+ winch no
+ action
+ length
+ rate
+
+
+
+
+
+ Time (sec)
+
+
+
+
+
+
+
+
+ Dist (m)
+
+
+
+
+
+
+
+
+ Deg
+ Speed deg/s
+ Dir 1=CW
+ 0=Abs,1=Rel
+
+
+
+
+
+
+
+
+
+ Acc radius
+ Pass by dist
+
+ Lat
+ Long
+ Alt
+
+
+
+
+ Dir 1=CW
+
+ Lat
+ Long
+ Alt
+
Turns
Radius
-
+ 1=Exit Tangent
Lat
Long
- Alt
+ Alt
Time s
-
-
+ Dir 1=CW
+ 1=Exit Tangent
Lat
Long
- Alt
+ Alt
-
+
-
+ Radius
-
+ 1=Exit Tangent
Lat
Long
- Alt
-
+ Alt
+
@@ -58,22 +484,22 @@
-
+ Abort Alt
Lat
Long
- Alt
+ Alt
-
+ Pitch Angle
- Alt
+ Alt
Seconds (or -1)
@@ -84,15 +510,24 @@
-
- on=1/off=0
+
+
-
-
+ Alt
+
+
+
+
+
+
+ Lat
+ Long
+ Alt
+
max descent
@@ -100,26 +535,170 @@
Lat
Long
- Alt
+ Alt
-
- timeout S
- min alt
- max alt
- max dist
+
+ commandId
+ timeout
+ arg1
+ arg2
+ arg3
+ arg4
+
+
+
+ ID
+ P1
+ P2
+ P3
-
-
- winch no
- action
- length
- rate
+
+
+ mode (3=mc/4=plane)
+
+
+
+
+
+
+
+
+ Id
+ Interval
+ Total Images
+
+
+
+
+
+
+ Id
+
+
+
+
+
+
+
+
+ Type
+ Value
+
+
+
+
+
+
+
+ Type
+ Value
+
+
+
+
+
+
+
+ Id
+ Primary(1:RGB,2:IR,3:NDVI,4:Wide)
+ Secondary(1:RGB,2:IR,3:NDVI,4:Wide)
+
+
+
+
+
+
+ StreamId
+
+
+
+
+
+
+
+
+ StreamId
+
+
+
-
+
+
+ AuxFunction
+ SwitchPosition
+
+
+
+
+
+
+
+ Alt
+ Descent rate
+ Wiggle time
+
+
+
+
+
+
+
+
+
+
+
+
+ Alt
+
+
+ Time (sec)
+
+
+
+
+
+
+
+
+ Dist (m)
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gripper No
+ drop(0)/grab(1)
+
+
+
+
+
+
+
+
+
+
+
+ Lat
+ Long
+
+
@@ -127,27 +706,18 @@
Lat
Long
- Alt
+ Alt
-
- Time (sec)
+
+ 0=normal, 1=inverted
-
-
- Rate (cm/sec)
-
-
-
-
-
- Alt
-
-
+
+
Dist (m)
@@ -155,16 +725,7 @@
-
-
- Deg
- Sec
- Dir 1=CW
- rel/abs
-
-
-
-
+
WP #
Repeat#
@@ -174,42 +735,33 @@
-
- Type
- Speed m/s
-
-
-
-
-
-
-
- Gripper No
- drop(0)/grab(1)
+
+ Tag
+
-
-
- Enable(1)/Release(2)
-
+
+
+ Tag
+ Repeat
-
-
- Dist (m)
-
-
+
+
+ Type (0=as 1=gs)
+ Speed (m/s)
+ Throttle(%)
-
+
Current(1)/Spec(0)
@@ -217,7 +769,7 @@
Lat
Long
- Alt
+ Alt
Relay No
@@ -265,7 +817,7 @@
- On/Off
+ Session Control
Zoom Position
Zoom Step
Focus Lock
@@ -282,280 +834,243 @@
-
-
-
-
-
-
- Acc radius
- Pass by dist
-
- Lat
- Long
- Alt
-
-
-
-
- Dir 1=CW
-
- Lat
- Long
- Alt
-
-
- Turns
-
- Radius
- 1=Exit Tangent
- Lat
- Long
- Alt
-
-
- Time s
-
- Dir 1=CW
- 1=Exit Tangent
- Lat
- Long
- Alt
-
-
-
- Radius
-
- 1=Exit Tangent
- Lat
- Long
- Alt
-
-
-
+
+ Pitch Angle
+ Yaw Angle
+ Pitch Rate
+ Yaw Rate
+ Flags
+
+ Gimbal Id
+
+
+ Enable(1)/Release(2)
-
-
- Abort Alt
-
-
-
- Lat
- Long
- Alt
-
-
- Pitch Angle
+
+
+ Disable(0)/Enable(1)/Disable Floor Only(2)
- Alt
-
-
-
+
+
+
+ Sprayer Enable
- Alt
-
-
-
+
+
+
+ enable
- Lat
- Long
- Alt
-
-
- commandId
- timeout
- arg1
- arg2
-
-
- ID
- P1
- P2
- P3
+
+
+ Start Engine
+ Cold Start
+ Height Delay
+
-
-
- mode (3=mc/4=plane)
+
+
+ Distance(m)
-
-
- Alt
- Descent rate
- Wiggle time
-
-
-
-
-
-
-
+
+
+
+
+
+ Delay
-
-
- Alt
-
-
- Time (sec)
+ Lat
+ Long
+ Alt
+
+
+
-
-
- Dist (m)
-
-
-
+
+
+ Seconds (or -1)
+ Hour UTC (or -1)
+ Minute UTC (or -1)
+ Second UTC
-
-
-
+
+
+ on=1/off=0
-
-
+
+
Lat
Long
-
-
-
-
+ Alt
+
+
+ Turns
-
+ Radius
Lat
Long
- Alt
-
-
- 0=normal, 1=inverted
+ Alt
+
+
+ Time s
+ Lat
+ Long
+ Alt
+
+
+ command
+ timeout
+ arg1
+ arg2
+ arg3
+ arg4
+
+
+
+ ID
+ P1
+ P2
+ P3
+
+
+
+
+
+ angle(CD)
+ Speed(0..1)
+ 0=Abs,1=Rel
+
-
-
- mode #
-
-
+
+
+ Id
+ Interval
+ Total Images
-
-
- Dist (m)
+
+
+ Id
-
-
- WP #
- Repeat#
+
+
+ Type
+ Value
+
+
+
+
+
+
+
+ Type
+ Value
-
-
- Type (0=as 1=gs)
- Speed (m/s)
- Throttle(%)
+
+
+ Id
+ Primary(1:RGB,2:IR,3:NDVI,4:Wide)
+ Secondary(1:RGB,2:IR,3:NDVI,4:Wide)
-
-
- Current(1)/Spec(0)
+
+
+ StreamId
- Lat
- Long
- Alt
-
-
- Relay No
- off(0)/on(1)
-
-
-
-
- Relay No
- Repeat#
- Delay (s)
+
+
+ StreamId
+
+
-
-
- Ser No
- PWM
+
+
+ AuxFunction
+ SwitchPosition
-
-
- Ser No
- PWM
- Repeat#
- Delay (s)
+
+
+
+ Speed (m/s)
+
+
-
+
Mode
Shutter Speed
@@ -566,7 +1081,7 @@
- On/Off
+ Session Control
Zoom Position
Zoom Step
Focus Lock
@@ -574,126 +1089,87 @@
CommandID
-
-
-
-
-
-
+
+ Pitch Angle
+ Yaw Angle
+ Pitch Rate
+ Yaw Rate
+ Flags
-
-
-
- Pitch
- Roll
- Yaw
+ Gimbal Id
+
+
+ Gripper No
+ drop(0)/grab(1)
+
-
-
- Enable(1)/Release(2)
-
+
+
+ WP #
+ Repeat#
-
-
- Disable(0)/Enable(1)/Disable Floor Only(2)
+
+
+ Tag
-
-
-
-
-
- Delay
-
-
-
- Lat
- Long
- Alt
-
-
-
-
- Dir 1=CW
-
- Lat
- Long
- Alt
-
-
- Time s
-
- Dir 1=CW
-
- Lat
- Long
- Alt
-
-
-
-
-
-
- Lat
- Long
- Alt
-
-
- Time (sec)
-
+
+
+ Tag
+ Repeat
-
-
- Dist (m)
+
+
+ timeout S
-
+ max dist
-
-
- angle(CD)
- Speed(0..1)
- 0=Abs,1=Rel
+
+
+ Pitch
+ Roll
+ Yaw
-
-
- Seconds (or -1)
- Hour UTC (or -1)
- Minute UTC (or -1)
- Second UTC
+
+
+ Relay No
+ Repeat#
+ Delay (s)
+
-
-
- Gripper No
- drop(0)/grab(1)
-
-
+
+
+ Ser No
+ PWM
+ Repeat#
+ Delay (s)
-
+
Current(1)/Spec(0)
@@ -703,87 +1179,51 @@
Long
Alt
-
-
-
-
-
- Lat
- Long
- Alt
-
-
- Direction (0=F,1=R)
+
+ Dist (m)
-
-
- mode#
-
+
+
+ Relay No
+ off(0)/on(1)
-
-
- Dist (m)
+
+
+ Distance(m)
-
-
- WP #
- Repeat#
+
+
+ Direction (0=F,1=R)
+
-
-
- Type (0=as 1=gs)
- Speed (m/s)
- Throttle(%)
-
-
-
-
-
-
- Current(1)/Spec(0)
+
+
+
Lat
Long
- Alt
-
-
- Relay No
- off(0)/on(1)
-
-
-
-
-
-
-
- Relay No
- Repeat#
- Delay (s)
-
-
-
-
-
+ Alt
+
Ser No
PWM
@@ -793,50 +1233,32 @@
-
- Ser No
- PWM
- Repeat#
- Delay (s)
-
-
-
-
-
-
+
+ Sprayer Enable
-
-
-
+
+
+ Time (sec)
-
-
-
+
+
+ Dist (m)
-
-
- Pitch
- Roll
- Yaw
-
-
-
-
-
+
diff --git a/GCSViews/FlightPlanner.cs b/GCSViews/FlightPlanner.cs
index 7b75cb6816..24dca8eab6 100644
--- a/GCSViews/FlightPlanner.cs
+++ b/GCSViews/FlightPlanner.cs
@@ -97,6 +97,7 @@ private void but_mincommands_Click(object sender, System.EventArgs e)
private readonly Random rnd = new Random();
public GMapMarker center = new GMarkerGoogle(new PointLatLng(0.0, 0.0), GMarkerGoogleType.none);
private Dictionary cmdParamNames = new Dictionary();
+ private Dictionary cmdParamMultipliers = new Dictionary();
private GMapMarkerRect CurentRectMarker;
private altmode currentaltmode = (altmode) Settings.Instance.GetInt32("FPaltmode", (int)altmode.Relative);
private GMapMarker CurrentGMapMarker;
@@ -661,7 +662,7 @@ public void BUT_write_Click(object sender, EventArgs e)
home.id = (ushort) MAVLink.MAV_CMD.WAYPOINT;
home.lat = (double.Parse(TXT_homelat.Text));
home.lng = (double.Parse(TXT_homelng.Text));
- home.alt = (float.Parse(TXT_homealt.Text) / CurrentState.multiplierdist); // use saved home
+ home.alt = (float.Parse(TXT_homealt.Text) / CurrentState.multiplieralt); // use saved home
}
catch
{
@@ -1112,7 +1113,7 @@ public void setfromMap(double lat, double lng, int alt, double p1 = -1)
DataGridViewTextBoxCell cell;
- if (alt == -2 && Commands.Columns[Alt.Index].HeaderText.Equals("Alt"))
+ if (alt == -2 && Commands.Columns[Alt.Index].HeaderText.ToString().StartsWith("Alt"))
{
if (CHK_verifyheight.Checked &&
(altmode) CMB_altmode.SelectedValue != altmode.Terrain) //Drag with verifyheight // use srtm data
@@ -1155,7 +1156,7 @@ public void setfromMap(double lat, double lng, int alt, double p1 = -1)
cell.DataGridView.EndEdit();
}
- if (alt != -1 && alt != -2 && Commands.Columns[Alt.Index].HeaderText.Equals("Alt"))
+ if (alt != -1 && alt != -2 && Commands.Columns[Alt.Index].HeaderText.ToString().StartsWith("Alt"))
{
cell = Commands.Rows[selectedrow].Cells[Alt.Index] as DataGridViewTextBoxCell;
@@ -1424,8 +1425,8 @@ public void writeKML()
wpOverlay.CreateOverlay(home,
commandlist,
- double.Parse(TXT_WPRad.Text) / CurrentState.multiplieralt,
- double.Parse(TXT_loiterrad.Text) / CurrentState.multiplieralt, CurrentState.multiplieralt);
+ double.Parse(TXT_WPRad.Text) / CurrentState.multiplierdist,
+ double.Parse(TXT_loiterrad.Text) / CurrentState.multiplierdist, CurrentState.multiplieralt);
}
catch (FormatException)
{
@@ -3061,8 +3062,10 @@ private Locationwp DataViewtoLocationwp(int a)
temp.p1 = float.Parse(Commands.Rows[a].Cells[Param1.Index].Value.ToString());
- if (Commands.Rows[a].Cells[Command.Index].Value.Equals("WAYPOINT"))
+ var command = Commands.Rows[a].Cells[Command.Index].Value.ToString();
+ if (command == "WAYPOINT")
{
+ // Clamp the delay value to 0
if (temp.p1 < 0)
{
temp.p1 = 0;
@@ -3071,15 +3074,18 @@ private Locationwp DataViewtoLocationwp(int a)
}
}
+
+ var multipliers = cmdParamMultipliers[command];
temp.alt =
(float)
- (double.Parse(Commands.Rows[a].Cells[Alt.Index].Value.ToString()) / CurrentState.multiplieralt);
- temp.lat = (double.Parse(Commands.Rows[a].Cells[Lat.Index].Value.ToString()));
- temp.lng = (double.Parse(Commands.Rows[a].Cells[Lon.Index].Value.ToString()));
+ (double.Parse(Commands.Rows[a].Cells[Alt.Index].Value.ToString()) / multipliers[6]);
+ temp.lat = (double.Parse(Commands.Rows[a].Cells[Lat.Index].Value.ToString()) / multipliers[4]);
+ temp.lng = (double.Parse(Commands.Rows[a].Cells[Lon.Index].Value.ToString()) / multipliers[5]);
- temp.p2 = (float) (double.Parse(Commands.Rows[a].Cells[Param2.Index].Value.ToString()));
- temp.p3 = (float) (double.Parse(Commands.Rows[a].Cells[Param3.Index].Value.ToString()));
- temp.p4 = (float) (double.Parse(Commands.Rows[a].Cells[Param4.Index].Value.ToString()));
+ temp.p1 = (float) (double.Parse(Commands.Rows[a].Cells[Param1.Index].Value.ToString()) / multipliers[0]);
+ temp.p2 = (float) (double.Parse(Commands.Rows[a].Cells[Param2.Index].Value.ToString()) / multipliers[1]);
+ temp.p3 = (float) (double.Parse(Commands.Rows[a].Cells[Param3.Index].Value.ToString()) / multipliers[2]);
+ temp.p4 = (float) (double.Parse(Commands.Rows[a].Cells[Param4.Index].Value.ToString()) / multipliers[3]);
temp.Tag = Commands.Rows[a].Cells[TagData.Index].Value;
@@ -5352,22 +5358,23 @@ private void processToScreen(List cmds, bool append = false)
}
DataGridViewComboBoxCell cellframe = Commands.Rows[i].Cells[Frame.Index] as DataGridViewComboBoxCell;
+ var multipliers = cmdParamMultipliers[cellcmd.Value.ToString()];
cellframe.Value = (int) temp.frame;
cell = Commands.Rows[i].Cells[Alt.Index] as DataGridViewTextBoxCell;
- cell.Value = temp.alt * CurrentState.multiplieralt;
+ cell.Value = temp.alt * multipliers[6];
cell = Commands.Rows[i].Cells[Lat.Index] as DataGridViewTextBoxCell;
- cell.Value = temp.lat;
+ cell.Value = temp.lat * multipliers[4];
cell = Commands.Rows[i].Cells[Lon.Index] as DataGridViewTextBoxCell;
- cell.Value = temp.lng;
+ cell.Value = temp.lng * multipliers[5];
cell = Commands.Rows[i].Cells[Param1.Index] as DataGridViewTextBoxCell;
- cell.Value = temp.p1;
+ cell.Value = temp.p1 * multipliers[0];
cell = Commands.Rows[i].Cells[Param2.Index] as DataGridViewTextBoxCell;
- cell.Value = temp.p2;
+ cell.Value = temp.p2 * multipliers[1];
cell = Commands.Rows[i].Cells[Param3.Index] as DataGridViewTextBoxCell;
- cell.Value = temp.p3;
+ cell.Value = temp.p3 * multipliers[2];
cell = Commands.Rows[i].Cells[Param4.Index] as DataGridViewTextBoxCell;
- cell.Value = temp.p4;
+ cell.Value = temp.p4 * multipliers[3];
// convert to utm/other
convertFromGeographic(temp.lat, temp.lng);
@@ -5426,9 +5433,10 @@ private void processToScreen(List cmds, bool append = false)
MainMap_OnMapZoomChanged();
}
- private Dictionary readCMDXML()
+ private void readCMDXML()
{
- Dictionary cmd = new Dictionary();
+ cmdParamNames.Clear();
+ cmdParamMultipliers.Clear();
// do lang stuff here
@@ -5479,6 +5487,7 @@ private Dictionary readCMDXML()
{
string cmdname = inner.Name;
string[] cmdarray = new string[7];
+ double[] cmdmultarray = new double[7] {1, 1, 1, 1, 1, 1, 1};
int b = 0;
XmlReader inner2 = inner.ReadSubtree();
@@ -5490,12 +5499,38 @@ private Dictionary readCMDXML()
inner2.MoveToElement();
if (inner2.IsStartElement())
{
+ var unitType = inner2.GetAttribute("unitType");
cmdarray[b] = inner2.ReadString();
+ if (unitType != null)
+ {
+ switch (unitType)
+ {
+ case "alt":
+ cmdmultarray[b] = CurrentState.multiplieralt;
+ cmdarray[b] += $" ({CurrentState.AltUnit})";
+ break;
+ case "speed":
+ cmdmultarray[b] = CurrentState.multiplierspeed;
+ cmdarray[b] += $" ({CurrentState.SpeedUnit})";
+ break;
+ case "dist":
+ cmdmultarray[b] = CurrentState.multiplierdist;
+ cmdarray[b] += $" ({CurrentState.DistanceUnit})";
+ break;
+ }
+ }
+ // Handle a legacy mavcmd.xml from before the unitType attribute was added
+ if (cmdarray[b] == "Alt" && inner2.Name == "Z")
+ {
+ cmdmultarray[b] = CurrentState.multiplieralt;
+ cmdarray[b] += $" ({CurrentState.AltUnit})";
+ }
b++;
}
}
- cmd[cmdname] = cmdarray;
+ cmdParamNames[cmdname] = cmdarray;
+ cmdParamMultipliers[cmdname] = cmdmultarray;
}
}
}
@@ -5506,15 +5541,11 @@ private Dictionary readCMDXML()
{
Dictionary configCommands = new Dictionary();
configCommands = JsonConvert.DeserializeObject>(Settings.Instance["PlannerExtraCommand"]);
- var newCmdList = cmd.Concat(configCommands.Where(kvp => !cmd.ContainsKey(kvp.Key))).ToDictionary(kvp => kvp.Key, kvp => kvp.Value);
- return newCmdList;
+ cmdParamNames = cmdParamNames.Concat(configCommands.Where(kvp => !cmdParamNames.ContainsKey(kvp.Key))).ToDictionary(kvp => kvp.Key, kvp => kvp.Value);
}
-
- return cmd;
}
catch
{
- return cmd;
}
}
@@ -6806,11 +6837,11 @@ public void TXT_WPRad_Leave(object sender, EventArgs e)
public void updateCMDParams()
{
- cmdParamNames = readCMDXML();
+ readCMDXML();
if ((MAVLink.MAV_MISSION_TYPE) cmb_missiontype.SelectedValue == MAVLink.MAV_MISSION_TYPE.FENCE)
{
- var fence = new[]
+ var fenceNames = new[]
{
"",
"",
@@ -6820,20 +6851,26 @@ public void updateCMDParams()
"Long",
""
};
+ var fenceMult = new[] {1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0};
cmdParamNames.Clear();
- cmdParamNames.Add(MAVLink.MAV_CMD.FENCE_RETURN_POINT.ToString(), fence.ToArray());
- fence[0] = "Points";
- cmdParamNames.Add(MAVLink.MAV_CMD.FENCE_POLYGON_VERTEX_INCLUSION.ToString(), fence.ToArray());
- cmdParamNames.Add(MAVLink.MAV_CMD.FENCE_POLYGON_VERTEX_EXCLUSION.ToString(), fence.ToArray());
- fence[0] = "Radius";
- cmdParamNames.Add(MAVLink.MAV_CMD.FENCE_CIRCLE_EXCLUSION.ToString(), fence.ToArray());
- cmdParamNames.Add(MAVLink.MAV_CMD.FENCE_CIRCLE_INCLUSION.ToString(), fence.ToArray());
-
+ cmdParamMultipliers.Clear();
+ cmdParamNames.Add(MAVLink.MAV_CMD.FENCE_RETURN_POINT.ToString(), fenceNames.ToArray());
+ cmdParamMultipliers.Add(MAVLink.MAV_CMD.FENCE_RETURN_POINT.ToString(), fenceMult);
+ fenceNames[0] = "Points";
+ cmdParamNames.Add(MAVLink.MAV_CMD.FENCE_POLYGON_VERTEX_INCLUSION.ToString(), fenceNames.ToArray());
+ cmdParamNames.Add(MAVLink.MAV_CMD.FENCE_POLYGON_VERTEX_EXCLUSION.ToString(), fenceNames.ToArray());
+ cmdParamMultipliers.Add(MAVLink.MAV_CMD.FENCE_POLYGON_VERTEX_INCLUSION.ToString(), fenceMult);
+ cmdParamMultipliers.Add(MAVLink.MAV_CMD.FENCE_POLYGON_VERTEX_EXCLUSION.ToString(), fenceMult);
+ fenceNames[0] = "Radius (m)"; // Don't actually convert, but make it clear it's meters
+ cmdParamNames.Add(MAVLink.MAV_CMD.FENCE_CIRCLE_EXCLUSION.ToString(), fenceNames.ToArray());
+ cmdParamNames.Add(MAVLink.MAV_CMD.FENCE_CIRCLE_INCLUSION.ToString(), fenceNames.ToArray());
+ cmdParamMultipliers.Add(MAVLink.MAV_CMD.FENCE_CIRCLE_EXCLUSION.ToString(), fenceMult);
+ cmdParamMultipliers.Add(MAVLink.MAV_CMD.FENCE_CIRCLE_INCLUSION.ToString(), fenceMult);
}
if ((MAVLink.MAV_MISSION_TYPE) cmb_missiontype.SelectedValue == MAVLink.MAV_MISSION_TYPE.RALLY)
{
- var rally = new[]
+ var rallyNames = new[]
{
"",
"",
@@ -6841,10 +6878,13 @@ public void updateCMDParams()
"",
"Lat",
"Long",
- "Alt"
+ $"Alt ({CurrentState.AltUnit})"
};
+ var rallyMult = new[] {1.0, 1.0, 1.0, 1.0, 1.0, 1.0, CurrentState.multiplieralt};
cmdParamNames.Clear();
- cmdParamNames.Add(MAVLink.MAV_CMD.RALLY_POINT.ToString(), rally);
+ cmdParamMultipliers.Clear();
+ cmdParamNames.Add(MAVLink.MAV_CMD.RALLY_POINT.ToString(), rallyNames);
+ cmdParamMultipliers.Add(MAVLink.MAV_CMD.RALLY_POINT.ToString(), rallyMult);
}
List cmds = new List();
diff --git a/mavcmd.xml b/mavcmd.xml
index bcc18adbe4..ac3815d159 100644
--- a/mavcmd.xml
+++ b/mavcmd.xml
@@ -10,7 +10,7 @@
Lat
Long
- Alt
+ Alt
@@ -19,7 +19,7 @@
- Alt
+ Alt
@@ -64,7 +64,7 @@
Lat
Long
- Alt
+ Alt
Time s
@@ -73,7 +73,7 @@
Lat
Long
- Alt
+ Alt
Turns
@@ -82,7 +82,7 @@
Lat
Long
- Alt
+ Alt
@@ -91,7 +91,7 @@
Lat
Long
- Alt
+ Alt
max descent
@@ -100,7 +100,7 @@
Lat
Long
- Alt
+ Alt
command
@@ -127,7 +127,7 @@
Lat
Long
- Alt
+ Alt
Id
@@ -379,7 +379,7 @@
Lat
Long
- Alt
+ Alt
Ser No
@@ -445,7 +445,7 @@
Lat
Long
- Alt
+ Alt
@@ -454,7 +454,7 @@
Lat
Long
- Alt
+ Alt
Turns
@@ -463,7 +463,7 @@
1=Exit Tangent
Lat
Long
- Alt
+ Alt
Time s
@@ -472,7 +472,7 @@
1=Exit Tangent
Lat
Long
- Alt
+ Alt
@@ -481,7 +481,7 @@
1=Exit Tangent
Lat
Long
- Alt
+ Alt
@@ -499,7 +499,7 @@
Lat
Long
- Alt
+ Alt
Pitch Angle
@@ -508,7 +508,7 @@
- Alt
+ Alt
Seconds (or -1)
@@ -526,7 +526,7 @@
- Alt
+ Alt
@@ -535,7 +535,7 @@
Lat
Long
- Alt
+ Alt
max descent
@@ -544,7 +544,7 @@
Lat
Long
- Alt
+ Alt
commandId
@@ -661,7 +661,7 @@
- Alt
+ Alt
Time (sec)
@@ -715,7 +715,7 @@
Lat
Long
- Alt
+ Alt
0=normal, 1=inverted
@@ -778,7 +778,7 @@
Lat
Long
- Alt
+ Alt
Relay No
@@ -916,7 +916,7 @@
Lat
Long
- Alt
+ Alt
@@ -952,7 +952,7 @@
Lat
Long
- Alt
+ Alt
Turns
@@ -961,7 +961,7 @@
Lat
Long
- Alt
+ Alt
Time s
@@ -970,7 +970,7 @@
Lat
Long
- Alt
+ Alt
command
@@ -1231,7 +1231,7 @@
Lat
Long
- Alt
+ Alt
Ser No