Skip to content

Commit

Permalink
FlightPlanner: add unitType attr to mavcmd.xml
Browse files Browse the repository at this point in the history 
This fixes an issue where commands that used the <Z> parameter for
anything other than altitude were getting unit conversions applied
when they shouldn't have been.

This also opens up the possibility of converting units for other
columns.
  • Loading branch information
@robertlong13
robertlong13 committed Jan 8, 2025
1 parent 88c9e89 commit 782aa00
Show file tree
Hide file tree
Showing 2 changed files with 104 additions and 64 deletions.
114 changes: 77 additions & 37 deletions GCSViews/FlightPlanner.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<string, string[]> cmdParamNames = new Dictionary<string, string[]>();
private Dictionary<string, double[]> cmdParamMultipliers = new Dictionary<string, double[]>();
private GMapMarkerRect CurentRectMarker;
private altmode currentaltmode = (altmode) Settings.Instance.GetInt32("FPaltmode", (int)altmode.Relative);
private GMapMarker CurrentGMapMarker;
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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;

Expand Down Expand Up @@ -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;
Expand All @@ -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;

Expand Down Expand Up @@ -5352,22 +5358,23 @@ private void processToScreen(List<Locationwp> 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);
Expand Down Expand Up @@ -5426,9 +5433,10 @@ private void processToScreen(List<Locationwp> cmds, bool append = false)
MainMap_OnMapZoomChanged();
}

private Dictionary<string, string[]> readCMDXML()
private void readCMDXML()
{
Dictionary<string, string[]> cmd = new Dictionary<string, string[]>();
cmdParamNames.Clear();
cmdParamMultipliers.Clear();

// do lang stuff here

Expand Down Expand Up @@ -5479,6 +5487,7 @@ private Dictionary<string, string[]> 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();
Expand All @@ -5490,12 +5499,38 @@ private Dictionary<string, string[]> 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;
}
}
}
Expand All @@ -5506,15 +5541,11 @@ private Dictionary<string, string[]> readCMDXML()
{
Dictionary<string, string[]> configCommands = new Dictionary<string, string[]>();
configCommands = JsonConvert.DeserializeObject<Dictionary<string, string[]>>(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;
}
}

Expand Down Expand Up @@ -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[]
{
"",
"",
Expand All @@ -6820,31 +6851,40 @@ 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[]
{
"",
"",
"",
"",
"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<string> cmds = new List<string>();
Expand Down
Loading

0 comments on commit 782aa00

Please sign in to comment.