Skip to content

Commit

Permalink
FlightData: support alt frames for Guided
Browse files Browse the repository at this point in the history
  • Loading branch information
robertlong13 authored and meee1 committed Oct 16, 2024
1 parent 8aaf0b6 commit fb69492
Show file tree
Hide file tree
Showing 3 changed files with 163 additions and 3 deletions.
2 changes: 1 addition & 1 deletion ExtLibs/ArduPilot/Mavlink/MAVLinkInterface.cs
Original file line number Diff line number Diff line change
Expand Up @@ -4359,7 +4359,7 @@ public void setGuidedModeWP(byte sysid, byte compid, Locationwp gotohere, bool s

if (MAVlist[sysid, compid].cs.firmware == Firmwares.ArduPlane)
{
MAV_MISSION_RESULT ans = setWP(sysid, compid, gotohere, 0, MAV_FRAME.GLOBAL_RELATIVE_ALT, (byte) 2);
MAV_MISSION_RESULT ans = setWP(sysid, compid, gotohere, 0, (MAV_FRAME)gotohere.frame, (byte) 2);

if (ans != MAV_MISSION_RESULT.MAV_MISSION_ACCEPTED)
{
Expand Down
141 changes: 141 additions & 0 deletions ExtLibs/Controls/AltInputBox.cs
Original file line number Diff line number Diff line change
@@ -0,0 +1,141 @@
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using System.Windows.Forms;

namespace MissionPlanner.Controls
{
public class AltInputBox
{
public delegate void ThemeManager(Control ctl);
public static event ThemeManager ApplyTheme;

public static string alt = "";
public static MAVLink.MAV_FRAME frame = MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT; // Holds the altitude frame selection

public static DialogResult Show(string title, string promptText, ref string alt, ref MAVLink.MAV_FRAME frame)
{
DialogResult answer = DialogResult.Cancel;
AltInputBox.alt = alt;
AltInputBox.frame = frame;

// ensure we run this on the right thread - mono - mac
if (Application.OpenForms.Count > 0 && Application.OpenForms[0].InvokeRequired)
{
Application.OpenForms[0].Invoke((MethodInvoker)delegate
{
answer = ShowUI(title, promptText);
});
}
else
{
answer = ShowUI(title, promptText);
}

alt = AltInputBox.alt;
frame = AltInputBox.frame;

return answer;
}

static DialogResult ShowUI(string title, string promptText)
{
Form form = new Form();
Label label = new Label();
TextBox textBoxAlt = new TextBox();
ComboBox comboBoxFrame = new ComboBox();
MyButton buttonOk = new MyButton();
MyButton buttonCancel = new MyButton();

// Form layout setup
form.SuspendLayout();
const int yMargin = 10;
const int xMargin = 10;

form.TopMost = true;
form.TopLevel = true;
form.Text = title;
form.ClientSize = new Size(396, 150); // Keep the same form width
form.FormBorderStyle = FormBorderStyle.FixedSingle;
form.StartPosition = FormStartPosition.CenterScreen;
form.MinimizeBox = false;
form.MaximizeBox = false;

// Label for the prompt text
var y = 20;
label.AutoSize = true;
label.Location = new Point(9, y);
label.Size = new Size(372, 13);
label.Text = promptText;
label.MaximumSize = new Size(372, 0);

// ComboBox for altitude frame selection (reduce width, aligned next to TextBox)
comboBoxFrame.Size = new Size(70, 20);
comboBoxFrame.DropDownStyle = ComboBoxStyle.DropDownList;

// Add KeyValuePair items for each option
comboBoxFrame.Items.Add(new KeyValuePair<string, MAVLink.MAV_FRAME>("Absolute", MAVLink.MAV_FRAME.GLOBAL));
comboBoxFrame.Items.Add(new KeyValuePair<string, MAVLink.MAV_FRAME>("Relative", MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT));
comboBoxFrame.Items.Add(new KeyValuePair<string, MAVLink.MAV_FRAME>("Terrain", MAVLink.MAV_FRAME.GLOBAL_TERRAIN_ALT));
comboBoxFrame.DisplayMember = "Key";
comboBoxFrame.ValueMember = "Value";

// Set the initial selected value by matching the frameValue
comboBoxFrame.SelectedIndex = comboBoxFrame.Items.Cast<KeyValuePair<string, MAVLink.MAV_FRAME>>()
.ToList().FindIndex(item => item.Value == frame);

// TextBox for altitude input
textBoxAlt.Size = new Size(form.ClientSize.Width - comboBoxFrame.Width - 3 * xMargin, 20);
textBoxAlt.Text = alt;

// OK Button
buttonOk.Size = new Size(75, 23);
buttonOk.Text = "OK";
buttonOk.DialogResult = DialogResult.OK;

// Cancel Button
buttonCancel.Size = new Size(75, 23);
buttonCancel.Text = "Cancel";
buttonCancel.DialogResult = DialogResult.Cancel;

// Add controls to the form
form.Controls.Add(label);
form.Controls.Add(textBoxAlt);
form.Controls.Add(comboBoxFrame);
form.Controls.Add(buttonOk);
form.Controls.Add(buttonCancel);

form.AcceptButton = buttonOk;
form.CancelButton = buttonCancel;

// Resume layout
form.ResumeLayout(false);
form.PerformLayout();

// Adjust the location of textBox, buttonOk, buttonCancel based on the content of the label.
y = y + label.Height + yMargin;
comboBoxFrame.Location = new Point(xMargin, y);
textBoxAlt.Location = new Point(comboBoxFrame.Location.X + comboBoxFrame.Width + xMargin, y + 4);
y = y + textBoxAlt.Height + yMargin;
buttonOk.Location = new Point(228, y);
buttonCancel.Location = new Point(309, y);
// Increase the size of the form.
form.ClientSize = new Size(396, y + buttonOk.Height + yMargin);

// Apply any theme settings
ApplyTheme?.Invoke(form);

// Show the form and return the result
DialogResult dialogResult = form.ShowDialog();

if (dialogResult == DialogResult.OK)
{
alt = textBoxAlt.Text;
frame = (MAVLink.MAV_FRAME)((dynamic)comboBoxFrame.SelectedItem).Value;
}

form.Dispose();
return dialogResult;
}
}
}
23 changes: 21 additions & 2 deletions GCSViews/FlightData.cs
Original file line number Diff line number Diff line change
Expand Up @@ -2829,6 +2829,7 @@ private void flightPlannerToolStripMenuItem_Click(object sender, EventArgs e)
private void flyToHereAltToolStripMenuItem_Click(object sender, EventArgs e)
{
string alt = "100";
MAVLink.MAV_FRAME frame = MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT;

if (MainV2.comPort.MAV.cs.firmware == Firmwares.ArduCopter2)
{
Expand All @@ -2841,11 +2842,14 @@ private void flyToHereAltToolStripMenuItem_Click(object sender, EventArgs e)

if (Settings.Instance.ContainsKey("guided_alt"))
alt = Settings.Instance["guided_alt"];
if (Settings.Instance.ContainsKey("guided_alt_frame"))
frame = (MAVLink.MAV_FRAME)byte.Parse(Settings.Instance["guided_alt_frame"]);

if (DialogResult.Cancel == InputBox.Show("Enter Alt", "Enter Guided Mode Alt", ref alt))
if (DialogResult.Cancel == AltInputBox.Show("Enter Alt", "Enter Guided Mode Alt", ref alt, ref frame))
return;

Settings.Instance["guided_alt"] = alt;
Settings.Instance["guided_alt_frame"] = ((byte)frame).ToString();

int intalt = (int) (100 * CurrentState.multiplieralt);
if (!int.TryParse(alt, out intalt))
Expand All @@ -2855,14 +2859,16 @@ private void flyToHereAltToolStripMenuItem_Click(object sender, EventArgs e)
}

MainV2.comPort.MAV.GuidedMode.z = intalt / CurrentState.multiplieralt;
MainV2.comPort.MAV.GuidedMode.frame = (byte) frame;

if (MainV2.comPort.MAV.cs.mode == "Guided")
{
MainV2.comPort.setGuidedModeWP(new Locationwp
{
alt = MainV2.comPort.MAV.GuidedMode.z,
lat = MainV2.comPort.MAV.GuidedMode.x / 1e7,
lng = MainV2.comPort.MAV.GuidedMode.y / 1e7
lng = MainV2.comPort.MAV.GuidedMode.y / 1e7,
frame = (byte)frame
});
}
}
Expand Down Expand Up @@ -3017,6 +3023,7 @@ private void goHereToolStripMenuItem_Click(object sender, EventArgs e)
gotohere.alt = MainV2.comPort.MAV.GuidedMode.z; // back to m
gotohere.lat = (MouseDownStart.Lat);
gotohere.lng = (MouseDownStart.Lng);
gotohere.frame = MainV2.comPort.MAV.GuidedMode.frame;

try
{
Expand Down Expand Up @@ -5816,6 +5823,16 @@ private void flyToCoordsToolStripMenuItem_Click(object sender, EventArgs e)
var location = "";
InputBox.Show("Enter Fly To Coords", "Please enter the coords 'lat;long;alt' or 'lat;long'", ref location);

byte frame = (byte)MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT;
if (!MainV2.comPort.MAV.GuidedMode.Equals(new MAVLink.mavlink_mission_item_int_t()))
{
frame = MainV2.comPort.MAV.GuidedMode.frame;
}
else if (Settings.Instance.ContainsKey("guided_alt_frame"))
{
byte.TryParse(Settings.Instance["guided_alt_frame"], out frame);
}

var split = location.Split(';');

if (split.Length == 3)
Expand All @@ -5832,6 +5849,7 @@ private void flyToCoordsToolStripMenuItem_Click(object sender, EventArgs e)
gotohere.alt = (float) plla.Alt / CurrentState.multiplieralt; // back to m
gotohere.lat = (plla.Lat);
gotohere.lng = (plla.Lng);
gotohere.frame = frame;

try
{
Expand All @@ -5856,6 +5874,7 @@ private void flyToCoordsToolStripMenuItem_Click(object sender, EventArgs e)
gotohere.alt = MainV2.comPort.MAV.GuidedMode.z; // back to m
gotohere.lat = (plla.Lat);
gotohere.lng = (plla.Lng);
gotohere.frame = frame;

try
{
Expand Down

0 comments on commit fb69492

Please sign in to comment.