mirror of https://github.com/ArduPilot/ardupilot
3383 lines
129 KiB
C#
3383 lines
129 KiB
C#
using System;
|
|
using System.Collections.Generic; // Lists
|
|
using System.Text; // stringbuilder
|
|
using System.Drawing; // pens etc
|
|
using System.IO; // file io
|
|
using System.IO.Ports; // serial
|
|
using System.Windows.Forms; // Forms
|
|
using System.Collections; // hashs
|
|
using System.Text.RegularExpressions; // regex
|
|
using System.Xml; // GE xml alt reader
|
|
using System.Net; // dns, ip address
|
|
using System.Net.Sockets; // tcplistner
|
|
using GMap.NET;
|
|
using GMap.NET.WindowsForms;
|
|
using System.Globalization; // language
|
|
using GMap.NET.WindowsForms.Markers;
|
|
using System.Resources;
|
|
using System.Reflection;
|
|
using System.ComponentModel;
|
|
using System.Threading;
|
|
using log4net;
|
|
using SharpKml.Base;
|
|
using SharpKml.Dom;
|
|
|
|
|
|
|
|
namespace ArdupilotMega.GCSViews
|
|
{
|
|
partial class FlightPlanner : MyUserControl
|
|
{
|
|
private static readonly ILog log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
|
|
int selectedrow = 0;
|
|
bool quickadd = false;
|
|
bool isonline = true;
|
|
bool sethome = false;
|
|
bool polygongridmode = false;
|
|
Hashtable param = new Hashtable();
|
|
|
|
public static List<PointLatLngAlt> pointlist = new List<PointLatLngAlt>(); // used to calc distance
|
|
static public Object thisLock = new Object();
|
|
private TextBox textBox1;
|
|
private ComponentResourceManager rm = new ComponentResourceManager(typeof(FlightPlanner));
|
|
|
|
private Dictionary<string, string[]> cmdParamNames = new Dictionary<string, string[]>();
|
|
|
|
/// <summary>
|
|
/// Reads defines.h for all valid commands and eeprom positions
|
|
/// </summary>
|
|
/// <param name="file">File Path</param>
|
|
/// <returns></returns>
|
|
public bool readdefines(string file)
|
|
{
|
|
if (!File.Exists(file))
|
|
{
|
|
return false;
|
|
}
|
|
try
|
|
{
|
|
StreamReader sr = new StreamReader(file); //"defines.h"
|
|
while (!sr.EndOfStream)
|
|
{
|
|
string line = sr.ReadLine();
|
|
Regex regex2 = new Regex(@"define\s+([^\s]+)\s+([^\s]+)", RegexOptions.IgnoreCase);
|
|
if (regex2.IsMatch(line))
|
|
{
|
|
MatchCollection matchs = regex2.Matches(line);
|
|
for (int i = 0; i < matchs.Count; i++)
|
|
{
|
|
int num = 0;
|
|
if (matchs[i].Groups[2].Value.ToString().ToLower().Contains("0x"))
|
|
{
|
|
try
|
|
{
|
|
num = Convert.ToInt32(matchs[i].Groups[2].Value.ToString(), 16);
|
|
}
|
|
catch (Exception) { System.Diagnostics.Debug.WriteLine("BAD hex " + matchs[i].Groups[1].Value.ToString()); }
|
|
}
|
|
else
|
|
{
|
|
try
|
|
{
|
|
num = Convert.ToInt32(matchs[i].Groups[2].Value.ToString(), 10);
|
|
}
|
|
catch (Exception) { System.Diagnostics.Debug.WriteLine("BAD dec " + matchs[i].Groups[1].Value.ToString()); }
|
|
}
|
|
System.Diagnostics.Debug.WriteLine(matchs[i].Groups[1].Value.ToString() + " = " + matchs[i].Groups[2].Value.ToString() + " = " + num.ToString());
|
|
try
|
|
{
|
|
// hashdefines.Add(matchs[i].Groups[1].Value.ToString(), num);
|
|
}
|
|
catch (Exception) { }
|
|
}
|
|
}
|
|
}
|
|
|
|
sr.Close();
|
|
|
|
|
|
// if (!hashdefines.ContainsKey("WP_START_BYTE"))
|
|
{
|
|
CustomMessageBox.Show("Your Ardupilot Mega project defines.h is Invalid");
|
|
//return false;
|
|
}
|
|
}
|
|
catch (Exception)
|
|
{
|
|
CustomMessageBox.Show("Can't open file!");
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Read from waypoint writter *.h file
|
|
/// </summary>
|
|
/// <param name="file">File Path</param>
|
|
/// <returns></returns>
|
|
bool readwaypointwritterfile(string file)
|
|
{
|
|
|
|
byte wp_rad = 30;
|
|
byte loit_rad = 45;
|
|
int alt_hold = 100;
|
|
byte wp_count = 0;
|
|
bool error = false;
|
|
List<Locationwp> cmds = new List<Locationwp>();
|
|
|
|
cmds.Add(new Locationwp());
|
|
|
|
try
|
|
{
|
|
StreamReader sr = new StreamReader(file); //"defines.h"
|
|
while (!error && !sr.EndOfStream)
|
|
{
|
|
string line = sr.ReadLine();
|
|
// defines
|
|
Regex regex2 = new Regex(@"define\s+([^\s]+)\s+([^\s]+)", RegexOptions.IgnoreCase);
|
|
if (regex2.IsMatch(line))
|
|
{
|
|
MatchCollection matchs = regex2.Matches(line);
|
|
for (int i = 0; i < matchs.Count; i++)
|
|
{
|
|
if (matchs[i].Groups[1].Value.ToString().Equals("WP_RADIUS"))
|
|
wp_rad = (byte)double.Parse(matchs[i].Groups[2].Value.ToString());
|
|
if (matchs[i].Groups[1].Value.ToString().Equals("LOITER_RADIUS"))
|
|
loit_rad = (byte)double.Parse(matchs[i].Groups[2].Value.ToString());
|
|
if (matchs[i].Groups[1].Value.ToString().Equals("ALT_TO_HOLD"))
|
|
alt_hold = (int)double.Parse(matchs[i].Groups[2].Value.ToString());
|
|
}
|
|
}
|
|
// waypoints
|
|
regex2 = new Regex(@"([^,{]+),([^,]+),([^,]+),([^,]+),([^,}]+)", RegexOptions.IgnoreCase);
|
|
if (regex2.IsMatch(line))
|
|
{
|
|
MatchCollection matchs = regex2.Matches(line);
|
|
for (int i = 0; i < matchs.Count; i++)
|
|
{
|
|
Locationwp temp = new Locationwp();
|
|
temp.options = 1;
|
|
temp.id = (byte)(int)Enum.Parse(typeof(MAVLink.MAV_CMD), matchs[i].Groups[1].Value.ToString().Replace("NAV_", ""), false);
|
|
temp.p1 = byte.Parse(matchs[i].Groups[2].Value.ToString());
|
|
|
|
if (temp.id < (byte)MAVLink.MAV_CMD.LAST)
|
|
{
|
|
temp.alt = (float)(double.Parse(matchs[i].Groups[3].Value.ToString(), new System.Globalization.CultureInfo("en-US")));
|
|
temp.lat = (float)(double.Parse(matchs[i].Groups[4].Value.ToString(), new System.Globalization.CultureInfo("en-US")));
|
|
temp.lng = (float)(double.Parse(matchs[i].Groups[5].Value.ToString(), new System.Globalization.CultureInfo("en-US")));
|
|
}
|
|
else
|
|
{
|
|
temp.alt = (float)(double.Parse(matchs[i].Groups[3].Value.ToString(), new System.Globalization.CultureInfo("en-US")));
|
|
temp.lat = (float)(double.Parse(matchs[i].Groups[4].Value.ToString(), new System.Globalization.CultureInfo("en-US")));
|
|
temp.lng = (float)(double.Parse(matchs[i].Groups[5].Value.ToString(), new System.Globalization.CultureInfo("en-US")));
|
|
}
|
|
cmds.Add(temp);
|
|
|
|
wp_count++;
|
|
if (wp_count == byte.MaxValue)
|
|
break;
|
|
}
|
|
if (wp_count == byte.MaxValue)
|
|
{
|
|
CustomMessageBox.Show("To many Waypoints!!!");
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
sr.Close();
|
|
|
|
TXT_DefaultAlt.Text = (alt_hold).ToString();
|
|
TXT_WPRad.Text = (wp_rad).ToString();
|
|
TXT_loiterrad.Text = (loit_rad).ToString();
|
|
|
|
processToScreen(cmds);
|
|
|
|
writeKML();
|
|
|
|
MainMap.ZoomAndCenterMarkers("objects");
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
CustomMessageBox.Show("Can't open file! " + ex.ToString());
|
|
return false;
|
|
}
|
|
return true;
|
|
|
|
}
|
|
|
|
/// <summary>
|
|
/// used to adjust existing point in the datagrid including "Home"
|
|
/// </summary>
|
|
/// <param name="pointno"></param>
|
|
/// <param name="lat"></param>
|
|
/// <param name="lng"></param>
|
|
/// <param name="alt"></param>
|
|
public void callMeDrag(string pointno, double lat, double lng, int alt)
|
|
{
|
|
if (pointno == "")
|
|
{
|
|
return;
|
|
}
|
|
|
|
// dragging a WP
|
|
if (pointno == "Home")
|
|
{
|
|
if (isonline && CHK_geheight.Checked)
|
|
{
|
|
TXT_homealt.Text = getGEAlt(lat, lng).ToString();
|
|
}
|
|
else
|
|
{
|
|
// no change
|
|
//TXT_homealt.Text = alt.ToString();
|
|
}
|
|
TXT_homelat.Text = lat.ToString();
|
|
TXT_homelng.Text = lng.ToString();
|
|
return;
|
|
}
|
|
|
|
try
|
|
{
|
|
selectedrow = int.Parse(pointno) - 1;
|
|
Commands.CurrentCell = Commands[1, selectedrow];
|
|
}
|
|
catch
|
|
{
|
|
return;
|
|
}
|
|
|
|
setfromGE(lat, lng, alt);
|
|
}
|
|
/// <summary>
|
|
/// Actualy Sets the values into the datagrid and verifys height if turned on
|
|
/// </summary>
|
|
/// <param name="lat"></param>
|
|
/// <param name="lng"></param>
|
|
/// <param name="alt"></param>
|
|
void setfromGE(double lat, double lng, int alt)
|
|
{
|
|
if (selectedrow > Commands.RowCount)
|
|
{
|
|
CustomMessageBox.Show("Invalid coord, How did you do this?");
|
|
return;
|
|
}
|
|
DataGridViewTextBoxCell cell;
|
|
if (Commands.Columns[Lat.Index].HeaderText.Equals(cmdParamNames["WAYPOINT"][4]/*"Lat"*/))
|
|
{
|
|
cell = Commands.Rows[selectedrow].Cells[Lat.Index] as DataGridViewTextBoxCell;
|
|
cell.Value = lat.ToString("0.0000000");
|
|
cell.DataGridView.EndEdit();
|
|
}
|
|
if (Commands.Columns[Lon.Index].HeaderText.Equals(cmdParamNames["WAYPOINT"][5]/*"Long"*/))
|
|
{
|
|
cell = Commands.Rows[selectedrow].Cells[Lon.Index] as DataGridViewTextBoxCell;
|
|
cell.Value = lng.ToString("0.0000000");
|
|
cell.DataGridView.EndEdit();
|
|
}
|
|
if (alt != -1 && Commands.Columns[Alt.Index].HeaderText.Equals(cmdParamNames["WAYPOINT"][6]/*"Alt"*/))
|
|
{
|
|
cell = Commands.Rows[selectedrow].Cells[Alt.Index] as DataGridViewTextBoxCell;
|
|
|
|
{
|
|
float result;
|
|
bool pass = float.TryParse(TXT_homealt.Text, out result);
|
|
|
|
if (result == 0 || pass == false)
|
|
{
|
|
CustomMessageBox.Show("You must have a home altitude");
|
|
return;
|
|
}
|
|
int results1;
|
|
if (!int.TryParse(TXT_DefaultAlt.Text, out results1))
|
|
{
|
|
CustomMessageBox.Show("Your default alt is not valid");
|
|
return;
|
|
}
|
|
}
|
|
|
|
cell.Value = TXT_DefaultAlt.Text;
|
|
|
|
float ans;
|
|
if (float.TryParse(cell.Value.ToString(), out ans))
|
|
{
|
|
ans = (int)ans;
|
|
if (alt != 0) // use passed in value;
|
|
cell.Value = alt.ToString();
|
|
if (ans == 0)
|
|
cell.Value = 50;
|
|
// online verify height
|
|
if (isonline && CHK_geheight.Checked)
|
|
{
|
|
if (CHK_altmode.Checked)
|
|
{
|
|
cell.Value = ((int)getGEAlt(lat, lng) + int.Parse(TXT_DefaultAlt.Text)).ToString();
|
|
}
|
|
else
|
|
{
|
|
cell.Value = ((int)getGEAlt(lat, lng) + int.Parse(TXT_DefaultAlt.Text) - float.Parse(TXT_homealt.Text)).ToString();
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// is absolute but no verify
|
|
if (CHK_altmode.Checked)
|
|
{
|
|
cell.Value = (float.Parse(TXT_homealt.Text) + int.Parse(TXT_DefaultAlt.Text)).ToString();
|
|
} // is relative and check height
|
|
else if (isonline && CHK_geheight.Checked)
|
|
{
|
|
alt = (int)getGEAlt(lat, lng);
|
|
|
|
if (float.Parse(TXT_homealt.Text) + int.Parse(TXT_DefaultAlt.Text) < alt) // calced height is less that GE ground height
|
|
{
|
|
CustomMessageBox.Show("Altitude appears to be low!! (you will fly into a hill)\nGoogle Ground height: " + alt + " Meters\nYour height: " + ((float.Parse(TXT_homealt.Text) + int.Parse(TXT_DefaultAlt.Text))) + " Meters");
|
|
cell.Style.BackColor = Color.Red;
|
|
}
|
|
else
|
|
{
|
|
cell.Style.BackColor = Color.LightGreen;
|
|
}
|
|
}
|
|
|
|
}
|
|
cell.DataGridView.EndEdit();
|
|
}
|
|
else
|
|
{
|
|
CustomMessageBox.Show("Invalid Home or wp Alt");
|
|
cell.Style.BackColor = Color.Red;
|
|
}
|
|
|
|
}
|
|
writeKML();
|
|
Commands.EndEdit();
|
|
}
|
|
/// <summary>
|
|
/// Used for current mouse position
|
|
/// </summary>
|
|
/// <param name="lat"></param>
|
|
/// <param name="lng"></param>
|
|
/// <param name="alt"></param>
|
|
public void callMeDisplay(double lat, double lng, int alt)
|
|
{
|
|
TXT_mouselat.Text = lat.ToString();
|
|
TXT_mouselong.Text = lng.ToString();
|
|
TXT_mousealt.Text = srtm.getAltitude(lat, lng, MainMap.Zoom).ToString("0");
|
|
|
|
try
|
|
{
|
|
double lastdist = MainMap.Manager.GetDistance(polygon.Points[polygon.Points.Count - 1], currentMarker.Position);
|
|
|
|
lbl_prevdist.Text = rm.GetString("lbl_prevdist.Text") + ": " + FormatDistance(lastdist, true);
|
|
|
|
double homedist = MainMap.Manager.GetDistance(currentMarker.Position, polygon.Points[0]);
|
|
|
|
lbl_homedist.Text = rm.GetString("lbl_homedist.Text") + ": " + FormatDistance(homedist, true);
|
|
}
|
|
catch { }
|
|
}
|
|
|
|
/// <summary>
|
|
/// Used to create a new WP
|
|
/// </summary>
|
|
/// <param name="lat"></param>
|
|
/// <param name="lng"></param>
|
|
/// <param name="alt"></param>
|
|
public void callMe(double lat, double lng, int alt)
|
|
{
|
|
if (polygongridmode)
|
|
{
|
|
addPolygonPointToolStripMenuItem_Click(null, null);
|
|
return;
|
|
}
|
|
|
|
if (sethome)
|
|
{
|
|
sethome = false;
|
|
callMeDrag("Home", lat, lng, alt);
|
|
return;
|
|
}
|
|
// creating a WP
|
|
|
|
Commands.Rows.Add();
|
|
selectedrow = Commands.RowCount - 1;
|
|
Commands.CurrentCell = Commands.Rows[selectedrow].Cells[Param1.Index];
|
|
|
|
setfromGE(lat, lng, alt);
|
|
}
|
|
|
|
public FlightPlanner()
|
|
{
|
|
InitializeComponent();
|
|
|
|
// config map
|
|
//MainMap.MapType = MapType.GoogleSatellite;
|
|
MainMap.MinZoom = 1;
|
|
MainMap.CacheLocation = Path.GetDirectoryName(Application.ExecutablePath) + "/gmapcache/";
|
|
|
|
//MainMap.Manager.ImageCacheLocal.PutImageToCache(,MapType.None,new GPoint(),17);
|
|
|
|
// map events
|
|
MainMap.OnCurrentPositionChanged += new CurrentPositionChanged(MainMap_OnCurrentPositionChanged);
|
|
MainMap.OnTileLoadStart += new TileLoadStart(MainMap_OnTileLoadStart);
|
|
MainMap.OnTileLoadComplete += new TileLoadComplete(MainMap_OnTileLoadComplete);
|
|
MainMap.OnMarkerClick += new MarkerClick(MainMap_OnMarkerClick);
|
|
MainMap.OnMapZoomChanged += new MapZoomChanged(MainMap_OnMapZoomChanged);
|
|
MainMap.OnMapTypeChanged += new MapTypeChanged(MainMap_OnMapTypeChanged);
|
|
MainMap.MouseMove += new MouseEventHandler(MainMap_MouseMove);
|
|
MainMap.MouseDown += new MouseEventHandler(MainMap_MouseDown);
|
|
MainMap.MouseUp += new MouseEventHandler(MainMap_MouseUp);
|
|
MainMap.OnMarkerEnter += new MarkerEnter(MainMap_OnMarkerEnter);
|
|
MainMap.OnMarkerLeave += new MarkerLeave(MainMap_OnMarkerLeave);
|
|
|
|
MainMap.MapScaleInfoEnabled = false;
|
|
MainMap.ScalePen = new Pen(Color.Red);
|
|
|
|
MainMap.ForceDoubleBuffer = false;
|
|
|
|
WebRequest.DefaultWebProxy.Credentials = System.Net.CredentialCache.DefaultCredentials;
|
|
|
|
// get map type
|
|
comboBoxMapType.DataSource = Enum.GetValues(typeof(MapType));
|
|
comboBoxMapType.SelectedItem = MainMap.MapType;
|
|
|
|
comboBoxMapType.SelectedValueChanged += new System.EventHandler(this.comboBoxMapType_SelectedValueChanged);
|
|
/*
|
|
// acccess mode
|
|
comboBoxMode.DataSource = Enum.GetValues(typeof(AccessMode));
|
|
comboBoxMode.SelectedItem = GMaps.Instance.Mode;
|
|
|
|
// get position
|
|
textBoxLat.Text = MainMap.Position.Lat.ToString(CultureInfo.InvariantCulture);
|
|
textBoxLng.Text = MainMap.Position.Lng.ToString(CultureInfo.InvariantCulture);
|
|
*/
|
|
|
|
MainMap.RoutesEnabled = true;
|
|
|
|
//MainMap.MaxZoom = 18;
|
|
|
|
// get zoom
|
|
trackBar1.Minimum = MainMap.MinZoom;
|
|
trackBar1.Maximum = MainMap.MaxZoom + 0.99;
|
|
|
|
// draw this layer first
|
|
kmlpolygons = new GMapOverlay(MainMap, "kmlpolygons");
|
|
MainMap.Overlays.Add(kmlpolygons);
|
|
|
|
geofence = new GMapOverlay(MainMap, "geofence");
|
|
MainMap.Overlays.Add(geofence);
|
|
|
|
routes = new GMapOverlay(MainMap, "routes");
|
|
MainMap.Overlays.Add(routes);
|
|
|
|
polygons = new GMapOverlay(MainMap, "polygons");
|
|
MainMap.Overlays.Add(polygons);
|
|
|
|
objects = new GMapOverlay(MainMap, "objects");
|
|
MainMap.Overlays.Add(objects);
|
|
|
|
drawnpolygons = new GMapOverlay(MainMap, "drawnpolygons");
|
|
MainMap.Overlays.Add(drawnpolygons);
|
|
|
|
|
|
|
|
top = new GMapOverlay(MainMap, "top");
|
|
//MainMap.Overlays.Add(top);
|
|
|
|
objects.Markers.Clear();
|
|
|
|
// set current marker
|
|
currentMarker = new GMapMarkerGoogleRed(MainMap.Position);
|
|
//top.Markers.Add(currentMarker);
|
|
|
|
// map center
|
|
center = new GMapMarkerCross(MainMap.Position);
|
|
top.Markers.Add(center);
|
|
|
|
MainMap.Zoom = 3;
|
|
|
|
//set home
|
|
try
|
|
{
|
|
MainMap.Position = new PointLatLng(double.Parse(TXT_homelat.Text), double.Parse(TXT_homelng.Text));
|
|
MainMap.Zoom = 13;
|
|
|
|
}
|
|
catch (Exception) { }
|
|
|
|
RegeneratePolygon();
|
|
|
|
if (MainV2.getConfig("MapType") != "")
|
|
{
|
|
try
|
|
{
|
|
comboBoxMapType.SelectedItem = Enum.Parse(typeof(MapType), MainV2.getConfig("MapType"));
|
|
}
|
|
catch { }
|
|
}
|
|
|
|
updateCMDParams();
|
|
|
|
Up.Image = global::ArdupilotMega.Properties.Resources.up;
|
|
Down.Image = global::ArdupilotMega.Properties.Resources.down;
|
|
}
|
|
|
|
void updateCMDParams()
|
|
{
|
|
cmdParamNames = readCMDXML();
|
|
|
|
List<string> cmds = new List<string>();
|
|
|
|
foreach (string item in cmdParamNames.Keys)
|
|
{
|
|
cmds.Add(item);
|
|
}
|
|
|
|
Command.DataSource = cmds;
|
|
}
|
|
|
|
Dictionary<string, string[]> readCMDXML()
|
|
{
|
|
Dictionary<string, string[]> cmd = new Dictionary<string, string[]>();
|
|
|
|
// do lang stuff here
|
|
|
|
string file = Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + "mavcmd.xml";
|
|
|
|
using (XmlReader reader = XmlReader.Create(file))
|
|
{
|
|
reader.Read();
|
|
reader.ReadStartElement("CMD");
|
|
if (MainV2.APMFirmware == MainV2.Firmwares.ArduPlane)
|
|
{
|
|
reader.ReadToFollowing("APM");
|
|
}
|
|
else
|
|
{
|
|
reader.ReadToFollowing("AC2");
|
|
}
|
|
|
|
XmlReader inner = reader.ReadSubtree();
|
|
|
|
inner.Read();
|
|
|
|
inner.MoveToElement();
|
|
|
|
inner.Read();
|
|
|
|
while (inner.Read())
|
|
{
|
|
inner.MoveToElement();
|
|
if (inner.IsStartElement())
|
|
{
|
|
string cmdname = inner.Name;
|
|
string[] cmdarray = new string[7];
|
|
int b = 0;
|
|
|
|
XmlReader inner2 = inner.ReadSubtree();
|
|
|
|
inner2.Read();
|
|
|
|
while (inner2.Read())
|
|
{
|
|
inner2.MoveToElement();
|
|
if (inner2.IsStartElement())
|
|
{
|
|
cmdarray[b] = inner2.ReadString();
|
|
b++;
|
|
}
|
|
}
|
|
|
|
cmd[cmdname] = cmdarray;
|
|
}
|
|
}
|
|
}
|
|
|
|
return cmd;
|
|
}
|
|
|
|
void Commands_DataError(object sender, DataGridViewDataErrorEventArgs e)
|
|
{
|
|
log.Info(e.Exception.ToString() + " " + e.Context + " col " + e.ColumnIndex);
|
|
e.Cancel = false;
|
|
e.ThrowException = false;
|
|
//throw new NotImplementedException();
|
|
}
|
|
/// <summary>
|
|
/// Adds a new row to the datagrid
|
|
/// </summary>
|
|
/// <param name="sender"></param>
|
|
/// <param name="e"></param>
|
|
private void BUT_Add_Click(object sender, EventArgs e)
|
|
{
|
|
if (Commands.CurrentRow == null)
|
|
{
|
|
selectedrow = 0;
|
|
}
|
|
else
|
|
{
|
|
selectedrow = Commands.CurrentRow.Index;
|
|
}
|
|
|
|
if (Commands.RowCount <= 1)
|
|
{
|
|
Commands.Rows.Add();
|
|
}
|
|
else
|
|
{
|
|
if (Commands.RowCount == selectedrow + 1)
|
|
{
|
|
DataGridViewRow temp = Commands.Rows[selectedrow];
|
|
Commands.Rows.Add();
|
|
}
|
|
else
|
|
{
|
|
Commands.Rows.Insert(selectedrow + 1, 1);
|
|
}
|
|
}
|
|
writeKML();
|
|
}
|
|
|
|
private void Planner_Load(object sender, EventArgs e)
|
|
{
|
|
quickadd = true;
|
|
|
|
config(false);
|
|
|
|
if (MainV2.cs.HomeLocation.Lat != 0 && MainV2.cs.HomeLocation.Lng != 0)
|
|
{
|
|
TXT_homelat.Text = MainV2.cs.HomeLocation.Lat.ToString();
|
|
|
|
TXT_homelng.Text = MainV2.cs.HomeLocation.Lng.ToString();
|
|
|
|
TXT_homealt.Text = MainV2.cs.HomeLocation.Alt.ToString();
|
|
}
|
|
|
|
|
|
quickadd = false;
|
|
|
|
if (MainV2.config["WMSserver"] != null)
|
|
MainMap.Manager.CustomWMSURL = MainV2.config["WMSserver"].ToString();
|
|
|
|
trackBar1.Value = (int)MainMap.Zoom;
|
|
|
|
// check for net and set offline if needed
|
|
try
|
|
{
|
|
IPAddress[] addresslist = Dns.GetHostAddresses("www.google.com");
|
|
}
|
|
catch (Exception)
|
|
{ // here if dns failed
|
|
isonline = false;
|
|
}
|
|
|
|
// setup geofence
|
|
List<PointLatLng> polygonPoints = new List<PointLatLng>();
|
|
gf = new GMapPolygon(polygonPoints, "geofence");
|
|
gf.Stroke = new Pen(Color.Pink, 5);
|
|
|
|
//setup drawnpolgon
|
|
List<PointLatLng> polygonPoints2 = new List<PointLatLng>();
|
|
drawnpolygon = new GMapPolygon(polygonPoints2, "drawnpoly");
|
|
drawnpolygon.Stroke = new Pen(Color.Red, 2);
|
|
|
|
updateCMDParams();
|
|
|
|
// mono
|
|
panelMap.Dock = DockStyle.None;
|
|
panelMap.Dock = DockStyle.Fill;
|
|
panelMap_Resize(null, null);
|
|
|
|
writeKML();
|
|
}
|
|
|
|
void parser_ElementAdded(object sender, SharpKml.Base.ElementEventArgs e)
|
|
{
|
|
processKML(e.Element);
|
|
}
|
|
|
|
private void processKML(SharpKml.Dom.Element Element)
|
|
{
|
|
try
|
|
{
|
|
log.Info(Element.ToString() + " " + Element.Parent);
|
|
}
|
|
catch { }
|
|
|
|
SharpKml.Dom.Document doc = Element as SharpKml.Dom.Document;
|
|
SharpKml.Dom.Placemark pm = Element as SharpKml.Dom.Placemark;
|
|
SharpKml.Dom.Folder folder = Element as SharpKml.Dom.Folder;
|
|
SharpKml.Dom.Polygon polygon = Element as SharpKml.Dom.Polygon;
|
|
SharpKml.Dom.LineString ls = Element as SharpKml.Dom.LineString;
|
|
|
|
if (doc != null)
|
|
{
|
|
foreach (var feat in doc.Features)
|
|
{
|
|
//Console.WriteLine("feat " + feat.GetType());
|
|
//processKML((Element)feat);
|
|
}
|
|
}
|
|
else
|
|
if (folder != null)
|
|
{
|
|
foreach (Feature feat in folder.Features)
|
|
{
|
|
//Console.WriteLine("feat "+feat.GetType());
|
|
//processKML(feat);
|
|
}
|
|
}
|
|
else if (pm != null)
|
|
{
|
|
|
|
}
|
|
else if (polygon != null)
|
|
{
|
|
GMapPolygon kmlpolygon = new GMapPolygon(new List<PointLatLng>(), "kmlpolygon");
|
|
|
|
kmlpolygon.Stroke.Color = Color.Purple;
|
|
|
|
foreach (var loc in polygon.OuterBoundary.LinearRing.Coordinates)
|
|
{
|
|
kmlpolygon.Points.Add(new PointLatLng(loc.Latitude, loc.Longitude));
|
|
}
|
|
|
|
kmlpolygons.Polygons.Add(kmlpolygon);
|
|
}
|
|
else if (ls != null)
|
|
{
|
|
GMapRoute kmlroute = new GMapRoute(new List<PointLatLng>(), "kmlroute");
|
|
|
|
kmlroute.Stroke.Color = Color.Purple;
|
|
|
|
foreach (var loc in ls.Coordinates)
|
|
{
|
|
kmlroute.Points.Add(new PointLatLng(loc.Latitude, loc.Longitude));
|
|
}
|
|
|
|
kmlpolygons.Routes.Add(kmlroute);
|
|
}
|
|
}
|
|
|
|
private void ChangeColumnHeader(string command)
|
|
{
|
|
try
|
|
{
|
|
if (cmdParamNames.ContainsKey(command))
|
|
for (int i = 1; i <= 7; i++)
|
|
Commands.Columns[i].HeaderText = cmdParamNames[command][i - 1];
|
|
else
|
|
for (int i = 1; i <= 7; i++)
|
|
Commands.Columns[i].HeaderText = "setme";
|
|
}
|
|
catch (Exception ex) { CustomMessageBox.Show(ex.ToString()); }
|
|
}
|
|
|
|
/// <summary>
|
|
/// Used to update column headers
|
|
/// </summary>
|
|
/// <param name="sender"></param>
|
|
/// <param name="e"></param>
|
|
private void Commands_RowEnter(object sender, DataGridViewCellEventArgs e)
|
|
{
|
|
if (quickadd)
|
|
return;
|
|
try
|
|
{
|
|
selectedrow = e.RowIndex;
|
|
string option = Commands[Command.Index, selectedrow].EditedFormattedValue.ToString();
|
|
string cmd;
|
|
try
|
|
{
|
|
cmd = Commands[Command.Index, selectedrow].Value.ToString();
|
|
}
|
|
catch { cmd = option; }
|
|
//Console.WriteLine("editformat " + option + " value " + cmd);
|
|
ChangeColumnHeader(cmd);
|
|
}
|
|
catch (Exception ex) { CustomMessageBox.Show(ex.ToString()); }
|
|
}
|
|
|
|
private void Commands_RowsAdded(object sender, DataGridViewRowsAddedEventArgs e)
|
|
{
|
|
for (int i = 0; i < Commands.ColumnCount; i++)
|
|
{
|
|
DataGridViewCell tcell = Commands.Rows[e.RowIndex].Cells[i];
|
|
if (tcell.GetType() == typeof(DataGridViewTextBoxCell))
|
|
{
|
|
if (tcell.Value == null)
|
|
tcell.Value = "0";
|
|
}
|
|
}
|
|
|
|
DataGridViewComboBoxCell cell = Commands.Rows[e.RowIndex].Cells[Command.Index] as DataGridViewComboBoxCell;
|
|
if (cell.Value == null)
|
|
{
|
|
cell.Value = "WAYPOINT";
|
|
Commands.Rows[e.RowIndex].Cells[Delete.Index].Value = "X";
|
|
if (!quickadd)
|
|
{
|
|
Commands_RowEnter(sender, new DataGridViewCellEventArgs(0, e.RowIndex - 0)); // do header labels
|
|
Commands_RowValidating(sender, new DataGridViewCellCancelEventArgs(0, e.RowIndex)); // do default values
|
|
}
|
|
}
|
|
|
|
if (quickadd)
|
|
return;
|
|
|
|
try
|
|
{
|
|
Commands.CurrentCell = Commands.Rows[e.RowIndex].Cells[0];
|
|
|
|
if (Commands.Rows[e.RowIndex - 1].Cells[Command.Index].Value.ToString() == "WAYPOINT")
|
|
{
|
|
Commands.Rows[e.RowIndex].Selected = true; // highlight row
|
|
}
|
|
else
|
|
{
|
|
Commands.CurrentCell = Commands[1, e.RowIndex - 1];
|
|
//Commands_RowEnter(sender, new DataGridViewCellEventArgs(0, e.RowIndex-1));
|
|
}
|
|
}
|
|
catch (Exception) { }
|
|
// Commands.EndEdit();
|
|
}
|
|
private void Commands_RowValidating(object sender, DataGridViewCellCancelEventArgs e)
|
|
{
|
|
selectedrow = e.RowIndex;
|
|
Commands_RowEnter(sender, new DataGridViewCellEventArgs(0, e.RowIndex - 0)); // do header labels - encure we dont 0 out valid colums
|
|
int cols = Commands.Columns.Count;
|
|
for (int a = 1; a < cols; a++)
|
|
{
|
|
DataGridViewTextBoxCell cell;
|
|
cell = Commands.Rows[selectedrow].Cells[a] as DataGridViewTextBoxCell;
|
|
|
|
if (Commands.Columns[a].HeaderText.Equals("") && cell != null && cell.Value == null)
|
|
{
|
|
cell.Value = "0";
|
|
}
|
|
else
|
|
{
|
|
if (cell != null && (cell.Value == null || cell.Value.ToString() == ""))
|
|
{
|
|
cell.Value = "?";
|
|
}
|
|
else
|
|
{
|
|
// not a text box
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// used to add a marker to the map display
|
|
/// </summary>
|
|
/// <param name="tag"></param>
|
|
/// <param name="lng"></param>
|
|
/// <param name="lat"></param>
|
|
/// <param name="alt"></param>
|
|
private void addpolygonmarker(string tag, double lng, double lat, int alt, Color? color)
|
|
{
|
|
try
|
|
{
|
|
PointLatLng point = new PointLatLng(lat, lng);
|
|
GMapMarkerGoogleGreen m = new GMapMarkerGoogleGreen(point);
|
|
m.ToolTipMode = MarkerTooltipMode.Always;
|
|
m.ToolTipText = tag;
|
|
m.Tag = tag;
|
|
|
|
//ArdupilotMega.GMapMarkerRectWPRad mBorders = new ArdupilotMega.GMapMarkerRectWPRad(point, (int)float.Parse(TXT_WPRad.Text), MainMap);
|
|
GMapMarkerRect mBorders = new GMapMarkerRect(point);
|
|
{
|
|
mBorders.InnerMarker = m;
|
|
mBorders.wprad = (int)float.Parse(TXT_WPRad.Text);
|
|
mBorders.MainMap = MainMap;
|
|
if (color.HasValue)
|
|
{
|
|
mBorders.Color = color.Value;
|
|
}
|
|
}
|
|
|
|
objects.Markers.Add(m);
|
|
objects.Markers.Add(mBorders);
|
|
}
|
|
catch (Exception) { }
|
|
}
|
|
|
|
private void addpolygonmarkergrid(string tag, double lng, double lat, int alt)
|
|
{
|
|
try
|
|
{
|
|
PointLatLng point = new PointLatLng(lat, lng);
|
|
GMapMarkerGoogleRed m = new GMapMarkerGoogleRed(point);
|
|
m.ToolTipMode = MarkerTooltipMode.Never;
|
|
m.ToolTipText = "grid" + tag;
|
|
m.Tag = "grid" + tag;
|
|
|
|
//ArdupilotMega.GMapMarkerRectWPRad mBorders = new ArdupilotMega.GMapMarkerRectWPRad(point, (int)float.Parse(TXT_WPRad.Text), MainMap);
|
|
GMapMarkerRect mBorders = new GMapMarkerRect(point);
|
|
{
|
|
mBorders.InnerMarker = m;
|
|
}
|
|
|
|
drawnpolygons.Markers.Add(m);
|
|
drawnpolygons.Markers.Add(mBorders);
|
|
}
|
|
catch (Exception ex) { log.Info(ex.ToString()); }
|
|
}
|
|
|
|
/// <summary>
|
|
/// used to write a KML, update the Map view polygon, and update the row headers
|
|
/// </summary>
|
|
private void writeKML()
|
|
{
|
|
// quickadd is for when loading wps from eeprom or file, to prevent slow, loading times
|
|
if (quickadd)
|
|
return;
|
|
|
|
// this is to share the current mission with the data tab
|
|
pointlist = new List<PointLatLngAlt>();
|
|
|
|
System.Diagnostics.Debug.WriteLine(DateTime.Now);
|
|
try
|
|
{
|
|
if (objects != null) // hasnt been created yet
|
|
{
|
|
objects.Markers.Clear();
|
|
}
|
|
|
|
// process and add home to the list
|
|
string home;
|
|
if (TXT_homealt.Text != "" && TXT_homelat.Text != "" && TXT_homelng.Text != "")
|
|
{
|
|
home = string.Format("{0},{1},{2}\r\n", TXT_homelng.Text, TXT_homelat.Text, TXT_DefaultAlt.Text);
|
|
if (objects != null) // during startup
|
|
{
|
|
pointlist.Add(new PointLatLngAlt(double.Parse(TXT_homelat.Text), double.Parse(TXT_homelng.Text), (int)double.Parse(TXT_homealt.Text), "Home"));
|
|
addpolygonmarker("Home", double.Parse(TXT_homelng.Text), double.Parse(TXT_homelat.Text), 0, null);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
home = "";
|
|
}
|
|
|
|
// setup for centerpoint calc etc.
|
|
double avglat = 0;
|
|
double avglong = 0;
|
|
double maxlat = -180;
|
|
double maxlong = -180;
|
|
double minlat = 180;
|
|
double minlong = 180;
|
|
double homealt = 0;
|
|
try
|
|
{
|
|
homealt = (int)double.Parse(TXT_homealt.Text);
|
|
}
|
|
catch { }
|
|
if (CHK_altmode.Checked)
|
|
{
|
|
homealt = 0; // for absolute we dont need to add homealt
|
|
}
|
|
|
|
int usable = 0;
|
|
|
|
// number rows
|
|
System.Threading.Thread t1 = new System.Threading.Thread(delegate()
|
|
{
|
|
// thread for updateing row numbers
|
|
for (int a = 0; a < Commands.Rows.Count - 0; a++)
|
|
{
|
|
try
|
|
{
|
|
if (Commands.Rows[a].HeaderCell.Value == null)
|
|
{
|
|
Commands.Rows[a].HeaderCell.Style.Alignment = DataGridViewContentAlignment.MiddleCenter;
|
|
Commands.Rows[a].HeaderCell.Value = (a + 1).ToString();
|
|
}
|
|
// skip rows with the correct number
|
|
string rowno = Commands.Rows[a].HeaderCell.Value.ToString();
|
|
if (!rowno.Equals((a + 1).ToString()))
|
|
{
|
|
// this code is where the delay is when deleting.
|
|
Commands.Rows[a].HeaderCell.Value = (a + 1).ToString();
|
|
}
|
|
}
|
|
catch (Exception) { }
|
|
}
|
|
});
|
|
t1.Name = "Row number updater";
|
|
t1.IsBackground = true;
|
|
t1.Start();
|
|
MainV2.threads.Add(t1);
|
|
|
|
long temp = System.Diagnostics.Stopwatch.GetTimestamp();
|
|
|
|
string lookat = "";
|
|
for (int a = 0; a < Commands.Rows.Count - 0; a++)
|
|
{
|
|
try
|
|
{
|
|
int command = (byte)(int)Enum.Parse(typeof(MAVLink.MAV_CMD), Commands.Rows[a].Cells[Command.Index].Value.ToString(), false);
|
|
if (command < (byte)MAVLink.MAV_CMD.LAST && command != (byte)MAVLink.MAV_CMD.TAKEOFF)
|
|
{
|
|
string cell2 = Commands.Rows[a].Cells[Alt.Index].Value.ToString(); // alt
|
|
string cell3 = Commands.Rows[a].Cells[Lat.Index].Value.ToString(); // lat
|
|
string cell4 = Commands.Rows[a].Cells[Lon.Index].Value.ToString(); // lng
|
|
|
|
if (cell4 == "0" || cell3 == "0")
|
|
continue;
|
|
if (cell4 == "?" || cell3 == "?")
|
|
continue;
|
|
|
|
|
|
if (command == (byte)MAVLink.MAV_CMD.LOITER_TIME || command == (byte)MAVLink.MAV_CMD.LOITER_TURNS || command == (byte)MAVLink.MAV_CMD.LOITER_UNLIM) {
|
|
pointlist.Add(new PointLatLngAlt(double.Parse(cell3), double.Parse(cell4), (int)double.Parse(cell2) + homealt, (a + 1).ToString()){ color = Color.LightBlue });
|
|
addpolygonmarker((a + 1).ToString(), double.Parse(cell4), double.Parse(cell3), (int)double.Parse(cell2) , Color.LightBlue);
|
|
} else {
|
|
pointlist.Add(new PointLatLngAlt(double.Parse(cell3), double.Parse(cell4), (int)double.Parse(cell2) + homealt, (a + 1).ToString()));
|
|
addpolygonmarker((a + 1).ToString(), double.Parse(cell4), double.Parse(cell3), (int)double.Parse(cell2) ,null);
|
|
}
|
|
|
|
avglong += double.Parse(Commands.Rows[a].Cells[Lon.Index].Value.ToString());
|
|
avglat += double.Parse(Commands.Rows[a].Cells[Lat.Index].Value.ToString());
|
|
usable++;
|
|
|
|
maxlong = Math.Max(double.Parse(Commands.Rows[a].Cells[Lon.Index].Value.ToString()), maxlong);
|
|
maxlat = Math.Max(double.Parse(Commands.Rows[a].Cells[Lat.Index].Value.ToString()), maxlat);
|
|
minlong = Math.Min(double.Parse(Commands.Rows[a].Cells[Lon.Index].Value.ToString()), minlong);
|
|
minlat = Math.Min(double.Parse(Commands.Rows[a].Cells[Lat.Index].Value.ToString()), minlat);
|
|
|
|
System.Diagnostics.Debug.WriteLine(temp - System.Diagnostics.Stopwatch.GetTimestamp());
|
|
}
|
|
}
|
|
catch (Exception e) { log.Info("writekml - bad wp data " + e.ToString()); }
|
|
}
|
|
|
|
if (usable > 0)
|
|
{
|
|
avglat = avglat / usable;
|
|
avglong = avglong / usable;
|
|
double latdiff = maxlat - minlat;
|
|
double longdiff = maxlong - minlong;
|
|
float range = 4000;
|
|
|
|
Locationwp loc1 = new Locationwp();
|
|
loc1.lat = (float)(minlat);
|
|
loc1.lng = (float)(minlong);
|
|
Locationwp loc2 = new Locationwp();
|
|
loc2.lat = (float)(maxlat);
|
|
loc2.lng = (float)(maxlong);
|
|
|
|
//double distance = getDistance(loc1, loc2); // same code as ardupilot
|
|
double distance = 2000;
|
|
|
|
if (usable > 1)
|
|
{
|
|
range = (float)(distance * 2);
|
|
}
|
|
else
|
|
{
|
|
range = 4000;
|
|
}
|
|
|
|
if (avglong != 0 && usable < 3)
|
|
{
|
|
// no autozoom
|
|
lookat = "<LookAt> <longitude>" + (minlong + longdiff / 2).ToString(new System.Globalization.CultureInfo("en-US")) + "</longitude> <latitude>" + (minlat + latdiff / 2).ToString(new System.Globalization.CultureInfo("en-US")) + "</latitude> <range>" + range + "</range> </LookAt>";
|
|
//MainMap.ZoomAndCenterMarkers("objects");
|
|
//MainMap.Zoom -= 1;
|
|
//MainMap_OnMapZoomChanged();
|
|
}
|
|
}
|
|
else if (home.Length > 5 && usable == 0)
|
|
{
|
|
lookat = "<LookAt> <longitude>" + TXT_homelng.Text.ToString(new System.Globalization.CultureInfo("en-US")) + "</longitude> <latitude>" + TXT_homelat.Text.ToString(new System.Globalization.CultureInfo("en-US")) + "</latitude> <range>4000</range> </LookAt>";
|
|
|
|
RectLatLng? rect = MainMap.GetRectOfAllMarkers("objects");
|
|
if (rect.HasValue)
|
|
{
|
|
MainMap.Position = rect.Value.LocationMiddle;
|
|
}
|
|
|
|
MainMap.Zoom = 17;
|
|
|
|
MainMap_OnMapZoomChanged();
|
|
}
|
|
|
|
RegeneratePolygon();
|
|
|
|
if (polygon != null && polygon.Points.Count > 0)
|
|
{
|
|
double homedist = 0;
|
|
|
|
if (home.Length > 5)
|
|
{
|
|
pointlist.Add(new PointLatLngAlt(double.Parse(TXT_homelat.Text), double.Parse(TXT_homelng.Text), (int)double.Parse(TXT_homealt.Text), "Home"));
|
|
|
|
homedist = MainMap.Manager.GetDistance(polygon.Points[polygon.Points.Count - 1], polygon.Points[0]);
|
|
|
|
lbl_homedist.Text = rm.GetString("lbl_homedist.Text") + ": " + FormatDistance(homedist, true);
|
|
}
|
|
lbl_distance.Text = rm.GetString("lbl_distance.Text") + ": " + FormatDistance(polygon.Distance + homedist, false);
|
|
}
|
|
|
|
|
|
config(true);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
log.Info(ex.ToString());
|
|
}
|
|
|
|
System.Diagnostics.Debug.WriteLine(DateTime.Now);
|
|
}
|
|
/// <summary>
|
|
/// Saves a waypoint writer file
|
|
/// </summary>
|
|
private void savewaypoints()
|
|
{
|
|
SaveFileDialog fd = new SaveFileDialog();
|
|
fd.Filter = "Ardupilot Mission (*.txt)|*.*";
|
|
fd.DefaultExt = ".txt";
|
|
DialogResult result = fd.ShowDialog();
|
|
string file = fd.FileName;
|
|
if (file != "")
|
|
{
|
|
try
|
|
{
|
|
StreamWriter sw = new StreamWriter(file);
|
|
sw.WriteLine("QGC WPL 110");
|
|
try
|
|
{
|
|
sw.WriteLine("0\t1\t0\t16\t0\t0\t0\t0\t" + double.Parse(TXT_homelat.Text).ToString("0.000000", new System.Globalization.CultureInfo("en-US")) + "\t" + double.Parse(TXT_homelng.Text).ToString("0.000000", new System.Globalization.CultureInfo("en-US")) + "\t" + double.Parse(TXT_homealt.Text).ToString("0.000000", new System.Globalization.CultureInfo("en-US")) + "\t1");
|
|
}
|
|
catch
|
|
{
|
|
sw.WriteLine("0\t1\t0\t0\t0\t0\t0\t0\t0\t0\t0\t1");
|
|
}
|
|
for (int a = 0; a < Commands.Rows.Count - 0; a++)
|
|
{
|
|
byte mode = (byte)(MAVLink.MAV_CMD)Enum.Parse(typeof(MAVLink.MAV_CMD), Commands.Rows[a].Cells[0].Value.ToString());
|
|
|
|
sw.Write((a + 1)); // seq
|
|
sw.Write("\t" + 0); // current
|
|
sw.Write("\t" + (CHK_altmode.Checked == true ? (byte)MAVLink.MAV_FRAME.MAV_FRAME_GLOBAL : (byte)MAVLink.MAV_FRAME.MAV_FRAME_GLOBAL_RELATIVE_ALT)); //frame
|
|
sw.Write("\t" + mode);
|
|
sw.Write("\t" + double.Parse(Commands.Rows[a].Cells[Param1.Index].Value.ToString()).ToString("0.000000", new System.Globalization.CultureInfo("en-US")));
|
|
sw.Write("\t" + double.Parse(Commands.Rows[a].Cells[Param2.Index].Value.ToString()).ToString("0.000000", new System.Globalization.CultureInfo("en-US")));
|
|
sw.Write("\t" + double.Parse(Commands.Rows[a].Cells[Param3.Index].Value.ToString()).ToString("0.000000", new System.Globalization.CultureInfo("en-US")));
|
|
sw.Write("\t" + double.Parse(Commands.Rows[a].Cells[Param4.Index].Value.ToString()).ToString("0.000000", new System.Globalization.CultureInfo("en-US")));
|
|
sw.Write("\t" + double.Parse(Commands.Rows[a].Cells[Lat.Index].Value.ToString()).ToString("0.000000", new System.Globalization.CultureInfo("en-US")));
|
|
sw.Write("\t" + double.Parse(Commands.Rows[a].Cells[Lon.Index].Value.ToString()).ToString("0.000000", new System.Globalization.CultureInfo("en-US")));
|
|
sw.Write("\t" + (double.Parse(Commands.Rows[a].Cells[Alt.Index].Value.ToString()) / MainV2.cs.multiplierdist).ToString("0.000000", new System.Globalization.CultureInfo("en-US")));
|
|
sw.Write("\t" + 1);
|
|
sw.WriteLine("");
|
|
}
|
|
sw.Close();
|
|
}
|
|
catch (Exception) { CustomMessageBox.Show("Error writing file"); }
|
|
}
|
|
}
|
|
|
|
private void SaveFile_Click(object sender, EventArgs e)
|
|
{
|
|
savewaypoints();
|
|
writeKML();
|
|
}
|
|
|
|
/// <summary>
|
|
/// Reads the EEPROM from a com port
|
|
/// </summary>
|
|
/// <param name="sender"></param>
|
|
/// <param name="e"></param>
|
|
internal void BUT_read_Click(object sender, EventArgs e)
|
|
{
|
|
Controls.ProgressReporterDialogue frmProgressReporter = new Controls.ProgressReporterDialogue
|
|
{
|
|
StartPosition = System.Windows.Forms.FormStartPosition.CenterScreen,
|
|
Text = "Receiving WP's"
|
|
};
|
|
|
|
frmProgressReporter.DoWork += getWPs;
|
|
frmProgressReporter.UpdateProgressAndStatus(-1, "Receiving WP's");
|
|
|
|
ThemeManager.ApplyThemeTo(frmProgressReporter);
|
|
|
|
frmProgressReporter.RunBackgroundOperationAsync();
|
|
}
|
|
|
|
void getWPs(object sender, Controls.ProgressWorkerEventArgs e)
|
|
{
|
|
|
|
List<Locationwp> cmds = new List<Locationwp>();
|
|
int error = 0;
|
|
|
|
try
|
|
{
|
|
MAVLink port = MainV2.comPort;
|
|
|
|
if (!port.BaseStream.IsOpen)
|
|
{
|
|
throw new Exception("Please Connect First!");
|
|
}
|
|
|
|
MainV2.givecomport = true;
|
|
|
|
param = port.param;
|
|
|
|
log.Info("Getting WP #");
|
|
|
|
((Controls.ProgressReporterDialogue)sender).UpdateProgressAndStatus(0, "Getting WP count");
|
|
|
|
int cmdcount = port.getWPCount();
|
|
|
|
for (ushort a = 0; a < cmdcount; a++)
|
|
{
|
|
log.Info("Getting WP" + a);
|
|
((Controls.ProgressReporterDialogue)sender).UpdateProgressAndStatus(a * 100 / cmdcount, "Getting WP " + a);
|
|
cmds.Add(port.getWP(a));
|
|
}
|
|
|
|
port.setWPACK();
|
|
|
|
((Controls.ProgressReporterDialogue)sender).UpdateProgressAndStatus(100, "Done");
|
|
|
|
log.Info("Done");
|
|
}
|
|
catch (Exception ex) { error = 1; CustomMessageBox.Show("Error : " + ex.ToString()); }
|
|
try
|
|
{
|
|
this.BeginInvoke((MethodInvoker)delegate()
|
|
{
|
|
if (error == 0)
|
|
{
|
|
try
|
|
{
|
|
processToScreen(cmds);
|
|
}
|
|
catch (Exception exx) { log.Info(exx.ToString()); }
|
|
}
|
|
|
|
MainV2.givecomport = false;
|
|
|
|
BUT_read.Enabled = true;
|
|
|
|
writeKML();
|
|
|
|
});
|
|
}
|
|
catch (Exception exx) { log.Info(exx.ToString()); }
|
|
}
|
|
|
|
/// <summary>
|
|
/// Writes the mission from the datagrid and values to the EEPROM
|
|
/// </summary>
|
|
/// <param name="sender"></param>
|
|
/// <param name="e"></param>
|
|
private void BUT_write_Click(object sender, EventArgs e)
|
|
{
|
|
if (CHK_altmode.Checked)
|
|
{
|
|
if (DialogResult.No == CustomMessageBox.Show("Absolute Alt is ticked are you sure?", "Alt Mode", MessageBoxButtons.YesNo))
|
|
{
|
|
CHK_altmode.Checked = false;
|
|
}
|
|
}
|
|
|
|
// check for invalid grid data
|
|
for (int a = 0; a < Commands.Rows.Count - 0; a++)
|
|
{
|
|
for (int b = 0; b < Commands.ColumnCount - 0; b++)
|
|
{
|
|
double answer;
|
|
if (b >= 1 && b <= 4)
|
|
{
|
|
if (!double.TryParse(Commands[b, a].Value.ToString(), out answer))
|
|
{
|
|
CustomMessageBox.Show("There are errors in your mission");
|
|
return;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
Controls.ProgressReporterDialogue frmProgressReporter = new Controls.ProgressReporterDialogue
|
|
{
|
|
StartPosition = System.Windows.Forms.FormStartPosition.CenterScreen,
|
|
Text = "Sending WP's"
|
|
};
|
|
|
|
frmProgressReporter.DoWork += saveWPs;
|
|
frmProgressReporter.UpdateProgressAndStatus(-1, "Sending WP's");
|
|
|
|
ThemeManager.ApplyThemeTo(frmProgressReporter);
|
|
|
|
frmProgressReporter.RunBackgroundOperationAsync();
|
|
|
|
|
|
MainMap.Focus();
|
|
|
|
}
|
|
|
|
void saveWPs(object sender, Controls.ProgressWorkerEventArgs e)
|
|
{
|
|
try
|
|
{
|
|
MAVLink port = MainV2.comPort;
|
|
|
|
if (!port.BaseStream.IsOpen)
|
|
{
|
|
throw new Exception("Please Connect First!");
|
|
}
|
|
|
|
MainV2.givecomport = true;
|
|
|
|
Locationwp home = new Locationwp();
|
|
|
|
try
|
|
{
|
|
home.id = (byte)MAVLink.MAV_CMD.WAYPOINT;
|
|
home.lat = (float.Parse(TXT_homelat.Text));
|
|
home.lng = (float.Parse(TXT_homelng.Text));
|
|
home.alt = (float.Parse(TXT_homealt.Text) / MainV2.cs.multiplierdist); // use saved home
|
|
}
|
|
catch { throw new Exception("Your home location is invalid"); }
|
|
|
|
((Controls.ProgressReporterDialogue)sender).UpdateProgressAndStatus(0, "Set Total WPs ");
|
|
|
|
port.setWPTotal((ushort)(Commands.Rows.Count + 1)); // + home
|
|
|
|
((Controls.ProgressReporterDialogue)sender).UpdateProgressAndStatus(0, "Set Home");
|
|
|
|
port.setWP(home, (ushort)0, MAVLink.MAV_FRAME.MAV_FRAME_GLOBAL, 0);
|
|
|
|
MAVLink.MAV_FRAME frame = MAVLink.MAV_FRAME.MAV_FRAME_GLOBAL_RELATIVE_ALT;
|
|
|
|
// process grid to memory eeprom
|
|
for (int a = 0; a < Commands.Rows.Count - 0; a++)
|
|
{
|
|
((Controls.ProgressReporterDialogue)sender).UpdateProgressAndStatus(a * 100 / Commands.Rows.Count, "Setting WP " + a);
|
|
|
|
Locationwp temp = new Locationwp();
|
|
temp.id = (byte)(int)Enum.Parse(typeof(MAVLink.MAV_CMD), Commands.Rows[a].Cells[Command.Index].Value.ToString(), false);
|
|
temp.p1 = float.Parse(Commands.Rows[a].Cells[Param1.Index].Value.ToString());
|
|
if (temp.id < (byte)MAVLink.MAV_CMD.LAST || temp.id == (byte)MAVLink.MAV_CMD.DO_SET_HOME)
|
|
{
|
|
if (CHK_altmode.Checked)
|
|
{
|
|
frame = MAVLink.MAV_FRAME.MAV_FRAME_GLOBAL;
|
|
}
|
|
else
|
|
{
|
|
frame = MAVLink.MAV_FRAME.MAV_FRAME_GLOBAL_RELATIVE_ALT;
|
|
}
|
|
}
|
|
|
|
temp.alt = (float)(double.Parse(Commands.Rows[a].Cells[Alt.Index].Value.ToString()) / MainV2.cs.multiplierdist);
|
|
temp.lat = (float)(double.Parse(Commands.Rows[a].Cells[Lat.Index].Value.ToString()));
|
|
temp.lng = (float)(double.Parse(Commands.Rows[a].Cells[Lon.Index].Value.ToString()));
|
|
|
|
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()));
|
|
|
|
port.setWP(temp, (ushort)(a + 1), frame, 0);
|
|
}
|
|
|
|
port.setWPACK();
|
|
|
|
((Controls.ProgressReporterDialogue)sender).UpdateProgressAndStatus(95, "Setting Params");
|
|
|
|
if (CHK_holdalt.Checked)
|
|
{
|
|
port.setParam("ALT_HOLD_RTL", int.Parse(TXT_DefaultAlt.Text) / MainV2.cs.multiplierdist);
|
|
}
|
|
else
|
|
{
|
|
port.setParam("ALT_HOLD_RTL", -1);
|
|
}
|
|
|
|
port.setParam("WP_RADIUS", (byte)int.Parse(TXT_WPRad.Text) / MainV2.cs.multiplierdist);
|
|
|
|
try
|
|
{
|
|
port.setParam("WP_LOITER_RAD", (byte)(int.Parse(TXT_loiterrad.Text) / MainV2.cs.multiplierdist));
|
|
}
|
|
catch
|
|
{
|
|
port.setParam("LOITER_RAD", (byte)int.Parse(TXT_loiterrad.Text) / MainV2.cs.multiplierdist);
|
|
}
|
|
|
|
((Controls.ProgressReporterDialogue)sender).UpdateProgressAndStatus(100, "Done.");
|
|
}
|
|
catch (Exception ex) { MainV2.givecomport = false; throw ex; }
|
|
|
|
MainV2.givecomport = false;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Processes a loaded EEPROM to the map and datagrid
|
|
/// </summary>
|
|
void processToScreen(List<Locationwp> cmds)
|
|
{
|
|
quickadd = true;
|
|
Commands.Rows.Clear();
|
|
|
|
if (cmds.Count == 0)
|
|
{
|
|
quickadd = false;
|
|
return;
|
|
}
|
|
|
|
int i = -1;
|
|
foreach (Locationwp temp in cmds)
|
|
{
|
|
i++;
|
|
//Console.WriteLine("FP processToScreen " + i);
|
|
if (temp.id == 0 && i != 0) // 0 and not home
|
|
break;
|
|
if (temp.id == 255 && i != 0) // bad record - never loaded any WP's - but have started the board up.
|
|
break;
|
|
if (i + 1 >= Commands.Rows.Count)
|
|
{
|
|
Commands.Rows.Add();
|
|
}
|
|
//if (i == 0 && temp.alt == 0) // skip 0 home
|
|
// continue;
|
|
DataGridViewTextBoxCell cell;
|
|
DataGridViewComboBoxCell cellcmd;
|
|
cellcmd = Commands.Rows[i].Cells[Command.Index] as DataGridViewComboBoxCell;
|
|
cellcmd.Value = "WAYPOINT";
|
|
|
|
foreach (object value in Enum.GetValues(typeof(MAVLink.MAV_CMD)))
|
|
{
|
|
if ((int)value == temp.id)
|
|
{
|
|
cellcmd.Value = value.ToString();
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (temp.id < (byte)MAVLink.MAV_CMD.LAST || temp.id == (byte)MAVLink.MAV_CMD.DO_SET_HOME)
|
|
{
|
|
if ((temp.options & 0x1) == 0 && i != 0) // home is always abs
|
|
{
|
|
CHK_altmode.Checked = true;
|
|
}
|
|
else
|
|
{
|
|
CHK_altmode.Checked = false;
|
|
}
|
|
|
|
|
|
|
|
}
|
|
|
|
cell = Commands.Rows[i].Cells[Alt.Index] as DataGridViewTextBoxCell;
|
|
cell.Value = Math.Round((temp.alt * MainV2.cs.multiplierdist), 0);
|
|
cell = Commands.Rows[i].Cells[Lat.Index] as DataGridViewTextBoxCell;
|
|
cell.Value = (double)temp.lat;
|
|
cell = Commands.Rows[i].Cells[Lon.Index] as DataGridViewTextBoxCell;
|
|
cell.Value = (double)temp.lng;
|
|
|
|
cell = Commands.Rows[i].Cells[Param1.Index] as DataGridViewTextBoxCell;
|
|
cell.Value = temp.p1;
|
|
cell = Commands.Rows[i].Cells[Param2.Index] as DataGridViewTextBoxCell;
|
|
cell.Value = temp.p2;
|
|
cell = Commands.Rows[i].Cells[Param3.Index] as DataGridViewTextBoxCell;
|
|
cell.Value = temp.p3;
|
|
cell = Commands.Rows[i].Cells[Param4.Index] as DataGridViewTextBoxCell;
|
|
cell.Value = temp.p4;
|
|
}
|
|
try
|
|
{
|
|
DataGridViewTextBoxCell cellhome;
|
|
cellhome = Commands.Rows[0].Cells[Lat.Index] as DataGridViewTextBoxCell;
|
|
if (cellhome.Value != null)
|
|
{
|
|
if (cellhome.Value.ToString() != TXT_homelat.Text && cellhome.Value.ToString() != "0")
|
|
{
|
|
DialogResult dr = CustomMessageBox.Show("Reset Home to loaded coords", "Reset Home Coords", MessageBoxButtons.YesNo);
|
|
|
|
if (dr == DialogResult.Yes)
|
|
{
|
|
TXT_homelat.Text = (double.Parse(cellhome.Value.ToString())).ToString();
|
|
cellhome = Commands.Rows[0].Cells[Lon.Index] as DataGridViewTextBoxCell;
|
|
TXT_homelng.Text = (double.Parse(cellhome.Value.ToString())).ToString();
|
|
cellhome = Commands.Rows[0].Cells[Alt.Index] as DataGridViewTextBoxCell;
|
|
TXT_homealt.Text = (double.Parse(cellhome.Value.ToString()) * MainV2.cs.multiplierdist).ToString();
|
|
}
|
|
}
|
|
}
|
|
|
|
log.Info("Setting wp params");
|
|
|
|
string hold_alt = ((int)((float)param["ALT_HOLD_RTL"] * MainV2.cs.multiplierdist)).ToString();
|
|
|
|
log.Info("param ALT_HOLD_RTL " + hold_alt);
|
|
|
|
if (!hold_alt.Equals("-1"))
|
|
{
|
|
TXT_DefaultAlt.Text = hold_alt;
|
|
}
|
|
|
|
TXT_WPRad.Text = ((int)((float)param["WP_RADIUS"] * MainV2.cs.multiplierdist)).ToString();
|
|
|
|
log.Info("param WP_RADIUS " + TXT_WPRad.Text);
|
|
|
|
try
|
|
{
|
|
if (param["LOITER_RADIUS"] != null)
|
|
TXT_loiterrad.Text = ((int)((float)param["LOITER_RADIUS"] * MainV2.cs.multiplierdist)).ToString();
|
|
|
|
if (param["WP_LOITER_RAD"] != null)
|
|
TXT_loiterrad.Text = ((int)((float)param["WP_LOITER_RAD"] * MainV2.cs.multiplierdist)).ToString();
|
|
|
|
log.Info("param LOITER_RADIUS " + TXT_loiterrad.Text);
|
|
}
|
|
catch
|
|
{
|
|
|
|
}
|
|
CHK_holdalt.Checked = Convert.ToBoolean((float)param["ALT_HOLD_RTL"] > 0);
|
|
log.Info("param ALT_HOLD_RTL " + CHK_holdalt.Checked.ToString());
|
|
|
|
}
|
|
catch (Exception ex) { log.Info(ex.ToString()); } // if there is no valid home
|
|
|
|
if (Commands.RowCount > 0)
|
|
{
|
|
log.Info("remove home from list");
|
|
Commands.Rows.Remove(Commands.Rows[0]); // remove home row
|
|
}
|
|
|
|
quickadd = false;
|
|
|
|
writeKML();
|
|
|
|
MainMap.ZoomAndCenterMarkers("objects");
|
|
|
|
MainMap_OnMapZoomChanged();
|
|
}
|
|
|
|
|
|
|
|
/// <summary>
|
|
/// Saves this forms config to MAIN, where it is written in a global config
|
|
/// </summary>
|
|
/// <param name="write">true/false</param>
|
|
private void config(bool write)
|
|
{
|
|
if (write)
|
|
{
|
|
ArdupilotMega.MainV2.config["TXT_homelat"] = TXT_homelat.Text;
|
|
ArdupilotMega.MainV2.config["TXT_homelng"] = TXT_homelng.Text;
|
|
ArdupilotMega.MainV2.config["TXT_homealt"] = TXT_homealt.Text;
|
|
|
|
|
|
ArdupilotMega.MainV2.config["TXT_WPRad"] = TXT_WPRad.Text;
|
|
|
|
ArdupilotMega.MainV2.config["TXT_loiterrad"] = TXT_loiterrad.Text;
|
|
|
|
ArdupilotMega.MainV2.config["TXT_DefaultAlt"] = TXT_DefaultAlt.Text;
|
|
|
|
ArdupilotMega.MainV2.config["CHK_altmode"] = CHK_altmode.Checked;
|
|
|
|
}
|
|
else
|
|
{
|
|
Hashtable temp = new Hashtable((Hashtable)ArdupilotMega.MainV2.config.Clone());
|
|
|
|
foreach (string key in temp.Keys)
|
|
{
|
|
switch (key)
|
|
{
|
|
case "TXT_WPRad":
|
|
TXT_WPRad.Text = ArdupilotMega.MainV2.config[key].ToString();
|
|
break;
|
|
case "TXT_loiterrad":
|
|
TXT_loiterrad.Text = ArdupilotMega.MainV2.config[key].ToString();
|
|
break;
|
|
case "TXT_DefaultAlt":
|
|
TXT_DefaultAlt.Text = ArdupilotMega.MainV2.config[key].ToString();
|
|
break;
|
|
case "CHK_altmode":
|
|
CHK_altmode.Checked = false;//bool.Parse(ArdupilotMega.MainV2.config[key].ToString());
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
}
|
|
}
|
|
|
|
private void TXT_WPRad_KeyPress(object sender, KeyPressEventArgs e)
|
|
{
|
|
int isNumber = 0;
|
|
if (e.KeyChar.ToString() == "\b")
|
|
return;
|
|
e.Handled = !int.TryParse(e.KeyChar.ToString(), out isNumber);
|
|
}
|
|
|
|
private void TXT_WPRad_Leave(object sender, EventArgs e)
|
|
{
|
|
int isNumber = 0;
|
|
if (!int.TryParse(TXT_WPRad.Text, out isNumber))
|
|
{
|
|
TXT_WPRad.Text = "30";
|
|
}
|
|
if (isNumber > 127)
|
|
{
|
|
CustomMessageBox.Show("The value can only be between 0 and 127");
|
|
TXT_WPRad.Text = "127";
|
|
}
|
|
writeKML();
|
|
}
|
|
|
|
private void TXT_loiterrad_KeyPress(object sender, KeyPressEventArgs e)
|
|
{
|
|
int isNumber = 0;
|
|
if (e.KeyChar.ToString() == "\b")
|
|
return;
|
|
e.Handled = !int.TryParse(e.KeyChar.ToString(), out isNumber);
|
|
}
|
|
|
|
private void TXT_loiterrad_Leave(object sender, EventArgs e)
|
|
{
|
|
int isNumber = 0;
|
|
if (!int.TryParse(TXT_loiterrad.Text, out isNumber))
|
|
{
|
|
TXT_loiterrad.Text = "45";
|
|
}
|
|
if (isNumber > 127)
|
|
{
|
|
CustomMessageBox.Show("The value can only be between 0 and 127");
|
|
TXT_loiterrad.Text = "127";
|
|
}
|
|
}
|
|
|
|
private void TXT_DefaultAlt_KeyPress(object sender, KeyPressEventArgs e)
|
|
{
|
|
int isNumber = 0;
|
|
if (e.KeyChar.ToString() == "\b")
|
|
return;
|
|
e.Handled = !int.TryParse(e.KeyChar.ToString(), out isNumber);
|
|
}
|
|
|
|
private void TXT_DefaultAlt_Leave(object sender, EventArgs e)
|
|
{
|
|
int isNumber = 0;
|
|
if (!int.TryParse(TXT_DefaultAlt.Text, out isNumber))
|
|
{
|
|
TXT_DefaultAlt.Text = "100";
|
|
}
|
|
}
|
|
|
|
|
|
/// <summary>
|
|
/// used to control buttons in the datagrid
|
|
/// </summary>
|
|
/// <param name="sender"></param>
|
|
/// <param name="e"></param>
|
|
private void Commands_CellContentClick(object sender, DataGridViewCellEventArgs e)
|
|
{
|
|
try
|
|
{
|
|
if (e.RowIndex < 0)
|
|
return;
|
|
if (e.ColumnIndex == Delete.Index && (e.RowIndex + 0) < Commands.RowCount) // delete
|
|
{
|
|
quickadd = true;
|
|
Commands.Rows.RemoveAt(e.RowIndex);
|
|
quickadd = false;
|
|
writeKML();
|
|
}
|
|
if (e.ColumnIndex == Up.Index && e.RowIndex != 0) // up
|
|
{
|
|
DataGridViewRow myrow = Commands.CurrentRow;
|
|
Commands.Rows.Remove(myrow);
|
|
Commands.Rows.Insert(e.RowIndex - 1, myrow);
|
|
writeKML();
|
|
}
|
|
if (e.ColumnIndex == Down.Index && e.RowIndex < Commands.RowCount - 1) // down
|
|
{
|
|
DataGridViewRow myrow = Commands.CurrentRow;
|
|
Commands.Rows.Remove(myrow);
|
|
Commands.Rows.Insert(e.RowIndex + 1, myrow);
|
|
writeKML();
|
|
}
|
|
}
|
|
catch (Exception) { CustomMessageBox.Show("Row error"); }
|
|
}
|
|
|
|
private void Commands_DefaultValuesNeeded(object sender, DataGridViewRowEventArgs e)
|
|
{
|
|
e.Row.Cells[Delete.Index].Value = "X";
|
|
e.Row.Cells[Up.Index].Value = global::ArdupilotMega.Properties.Resources.up;
|
|
e.Row.Cells[Down.Index].Value = global::ArdupilotMega.Properties.Resources.down;
|
|
}
|
|
|
|
private void TXT_homelat_TextChanged(object sender, EventArgs e)
|
|
{
|
|
sethome = false;
|
|
try
|
|
{
|
|
MainV2.cs.HomeLocation.Lat = double.Parse(TXT_homelat.Text);
|
|
}
|
|
catch { }
|
|
writeKML();
|
|
|
|
}
|
|
|
|
private void TXT_homelng_TextChanged(object sender, EventArgs e)
|
|
{
|
|
sethome = false;
|
|
try
|
|
{
|
|
MainV2.cs.HomeLocation.Lng = double.Parse(TXT_homelng.Text);
|
|
}
|
|
catch { }
|
|
writeKML();
|
|
}
|
|
|
|
private void TXT_homealt_TextChanged(object sender, EventArgs e)
|
|
{
|
|
sethome = false;
|
|
try
|
|
{
|
|
MainV2.cs.HomeLocation.Alt = double.Parse(TXT_homealt.Text);
|
|
}
|
|
catch { }
|
|
writeKML();
|
|
}
|
|
|
|
private void Planner_FormClosing(object sender, FormClosingEventArgs e)
|
|
{
|
|
}
|
|
|
|
private void BUT_loadwpfile_Click(object sender, EventArgs e)
|
|
{
|
|
OpenFileDialog fd = new OpenFileDialog();
|
|
fd.Filter = "Ardupilot Mission (*.txt)|*.*";
|
|
fd.DefaultExt = ".txt";
|
|
DialogResult result = fd.ShowDialog();
|
|
string file = fd.FileName;
|
|
if (file != "")
|
|
{
|
|
if (file.ToLower().EndsWith(".h"))
|
|
{
|
|
readwaypointwritterfile(file);
|
|
}
|
|
else
|
|
{
|
|
readQGC110wpfile(file);
|
|
}
|
|
}
|
|
}
|
|
|
|
void readQGC110wpfile(string file)
|
|
{
|
|
int wp_count = 0;
|
|
bool error = false;
|
|
List<Locationwp> cmds = new List<Locationwp>();
|
|
|
|
try
|
|
{
|
|
StreamReader sr = new StreamReader(file); //"defines.h"
|
|
string header = sr.ReadLine();
|
|
if (header == null || !header.Contains("QGC WPL 110"))
|
|
{
|
|
CustomMessageBox.Show("Invalid Waypoint file");
|
|
return;
|
|
}
|
|
while (!error && !sr.EndOfStream)
|
|
{
|
|
string line = sr.ReadLine();
|
|
// waypoints
|
|
|
|
if (line.StartsWith("#"))
|
|
continue;
|
|
|
|
string[] items = line.Split(new char[] { (char)'\t', ' ' }, StringSplitOptions.RemoveEmptyEntries);
|
|
|
|
if (items.Length <= 9)
|
|
continue;
|
|
|
|
try
|
|
{
|
|
|
|
Locationwp temp = new Locationwp();
|
|
if (items[2] == "3")
|
|
{ // abs MAV_FRAME_GLOBAL_RELATIVE_ALT=3
|
|
temp.options = 1;
|
|
}
|
|
else
|
|
{
|
|
temp.options = 0;
|
|
}
|
|
temp.id = (byte)(int)Enum.Parse(typeof(MAVLink.MAV_CMD), items[3], false);
|
|
temp.p1 = float.Parse(items[4], new System.Globalization.CultureInfo("en-US"));
|
|
|
|
if (temp.id == 99)
|
|
temp.id = 0;
|
|
|
|
temp.alt = (float)(double.Parse(items[10], new System.Globalization.CultureInfo("en-US")));
|
|
temp.lat = (float)(double.Parse(items[8], new System.Globalization.CultureInfo("en-US")));
|
|
temp.lng = (float)(double.Parse(items[9], new System.Globalization.CultureInfo("en-US")));
|
|
|
|
temp.p2 = (float)(double.Parse(items[5], new System.Globalization.CultureInfo("en-US")));
|
|
temp.p3 = (float)(double.Parse(items[6], new System.Globalization.CultureInfo("en-US")));
|
|
temp.p4 = (float)(double.Parse(items[7], new System.Globalization.CultureInfo("en-US")));
|
|
|
|
cmds.Add(temp);
|
|
|
|
wp_count++;
|
|
|
|
}
|
|
catch { CustomMessageBox.Show("Line invalid\n" + line); }
|
|
|
|
if (wp_count == byte.MaxValue)
|
|
{
|
|
CustomMessageBox.Show("To many Waypoints!!!");
|
|
break;
|
|
}
|
|
|
|
}
|
|
|
|
sr.Close();
|
|
|
|
processToScreen(cmds);
|
|
|
|
writeKML();
|
|
|
|
MainMap.ZoomAndCenterMarkers("objects");
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
CustomMessageBox.Show("Can't open file! " + ex.ToString());
|
|
}
|
|
}
|
|
|
|
private void trackBar1_ValueChanged(object sender, EventArgs e)
|
|
{
|
|
try
|
|
{
|
|
lock (thisLock)
|
|
{
|
|
MainMap.Zoom = trackBar1.Value;
|
|
}
|
|
}
|
|
catch { }
|
|
}
|
|
|
|
// marker
|
|
GMapMarker currentMarker;
|
|
GMapMarker center = new GMapMarkerCross(new PointLatLng(0.0, 0.0));
|
|
|
|
// polygons
|
|
GMapPolygon polygon;
|
|
GMapPolygon drawnpolygon;
|
|
GMapPolygon gf;
|
|
|
|
GMapOverlay kmlpolygons;
|
|
GMapOverlay geofence;
|
|
|
|
|
|
// layers
|
|
GMapOverlay top;
|
|
internal GMapOverlay objects;
|
|
static GMapOverlay routes;// static so can update from gcs
|
|
static GMapOverlay polygons;
|
|
GMapOverlay drawnpolygons;
|
|
|
|
// etc
|
|
readonly Random rnd = new Random();
|
|
string mobileGpsLog = string.Empty;
|
|
GMapMarkerRect CurentRectMarker = null;
|
|
bool isMouseDown = false;
|
|
bool isMouseDraging = false;
|
|
PointLatLng start;
|
|
PointLatLng end;
|
|
|
|
//public long ElapsedMilliseconds;
|
|
|
|
#region -- map events --
|
|
void MainMap_OnMarkerLeave(GMapMarker item)
|
|
{
|
|
if (!isMouseDown)
|
|
{
|
|
if (item is GMapMarkerRect)
|
|
{
|
|
CurentRectMarker = null;
|
|
|
|
GMapMarkerRect rc = item as GMapMarkerRect;
|
|
rc.Pen.Color = Color.Blue;
|
|
MainMap.Invalidate(false);
|
|
}
|
|
}
|
|
}
|
|
|
|
void MainMap_OnMarkerEnter(GMapMarker item)
|
|
{
|
|
if (!isMouseDown)
|
|
{
|
|
if (item is GMapMarkerRect)
|
|
{
|
|
GMapMarkerRect rc = item as GMapMarkerRect;
|
|
rc.Pen.Color = Color.Red;
|
|
MainMap.Invalidate(false);
|
|
|
|
CurentRectMarker = rc;
|
|
}
|
|
}
|
|
}
|
|
|
|
void MainMap_OnMapTypeChanged(MapType type)
|
|
{
|
|
comboBoxMapType.SelectedItem = MainMap.MapType;
|
|
|
|
trackBar1.Minimum = MainMap.MinZoom;
|
|
trackBar1.Maximum = MainMap.MaxZoom + 0.99;
|
|
|
|
MainMap.ZoomAndCenterMarkers("objects");
|
|
|
|
if (type == MapType.CustomWMS)
|
|
{
|
|
string url = "";
|
|
if (MainV2.config["WMSserver"] != null)
|
|
url = MainV2.config["WMSserver"].ToString();
|
|
Common.InputBox("WMS Server", "Enter the WMS server URL", ref url);
|
|
MainV2.config["WMSserver"] = url;
|
|
MainMap.Manager.CustomWMSURL = url;
|
|
}
|
|
}
|
|
|
|
void MainMap_MouseUp(object sender, MouseEventArgs e)
|
|
{
|
|
end = MainMap.FromLocalToLatLng(e.X, e.Y);
|
|
|
|
if (e.Button == MouseButtons.Right) // ignore right clicks
|
|
{
|
|
return;
|
|
}
|
|
|
|
if (isMouseDown) // mouse down on some other object and dragged to here.
|
|
{
|
|
if (e.Button == MouseButtons.Left)
|
|
{
|
|
isMouseDown = false;
|
|
}
|
|
if (!isMouseDraging)
|
|
{
|
|
if (CurentRectMarker != null)
|
|
{
|
|
// cant add WP in existing rect
|
|
}
|
|
else
|
|
{
|
|
callMe(currentMarker.Position.Lat, currentMarker.Position.Lng, 0);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (CurentRectMarker != null)
|
|
{
|
|
if (CurentRectMarker.InnerMarker.Tag.ToString().Contains("grid"))
|
|
{
|
|
try
|
|
{
|
|
drawnpolygon.Points[int.Parse(CurentRectMarker.InnerMarker.Tag.ToString().Replace("grid", "")) - 1] = new PointLatLng(end.Lat, end.Lng);
|
|
MainMap.UpdatePolygonLocalPosition(drawnpolygon);
|
|
MainMap.Invalidate();
|
|
}
|
|
catch { }
|
|
}
|
|
else
|
|
{
|
|
callMeDrag(CurentRectMarker.InnerMarker.Tag.ToString(), currentMarker.Position.Lat, currentMarker.Position.Lng, -1);
|
|
}
|
|
CurentRectMarker = null;
|
|
}
|
|
}
|
|
}
|
|
|
|
isMouseDraging = false;
|
|
}
|
|
|
|
void MainMap_MouseDown(object sender, MouseEventArgs e)
|
|
{
|
|
start = MainMap.FromLocalToLatLng(e.X, e.Y);
|
|
|
|
if (e.Button == MouseButtons.Left && Control.ModifierKeys != Keys.Alt)
|
|
{
|
|
isMouseDown = true;
|
|
isMouseDraging = false;
|
|
|
|
if (currentMarker.IsVisible)
|
|
{
|
|
currentMarker.Position = MainMap.FromLocalToLatLng(e.X, e.Y);
|
|
}
|
|
}
|
|
}
|
|
|
|
// move current marker with left holding
|
|
void MainMap_MouseMove(object sender, MouseEventArgs e)
|
|
{
|
|
PointLatLng point = MainMap.FromLocalToLatLng(e.X, e.Y);
|
|
|
|
currentMarker.Position = point;
|
|
|
|
if (!isMouseDown)
|
|
{
|
|
callMeDisplay(point.Lat, point.Lng, 0);
|
|
}
|
|
|
|
//draging
|
|
if (e.Button == MouseButtons.Left && isMouseDown)
|
|
{
|
|
isMouseDraging = true;
|
|
if (CurentRectMarker == null) // left click pan
|
|
{
|
|
double latdif = start.Lat - point.Lat;
|
|
double lngdif = start.Lng - point.Lng;
|
|
|
|
try
|
|
{
|
|
lock (thisLock)
|
|
{
|
|
MainMap.Position = new PointLatLng(center.Position.Lat + latdif, center.Position.Lng + lngdif);
|
|
}
|
|
}
|
|
catch { }
|
|
}
|
|
else // move rect marker
|
|
{
|
|
try
|
|
{
|
|
if (CurentRectMarker.InnerMarker.Tag.ToString().Contains("grid"))
|
|
{
|
|
drawnpolygon.Points[int.Parse(CurentRectMarker.InnerMarker.Tag.ToString().Replace("grid", "")) - 1] = new PointLatLng(point.Lat, point.Lng);
|
|
MainMap.UpdatePolygonLocalPosition(drawnpolygon);
|
|
MainMap.Invalidate();
|
|
}
|
|
}
|
|
catch { }
|
|
|
|
PointLatLng pnew = MainMap.FromLocalToLatLng(e.X, e.Y);
|
|
|
|
int? pIndex = (int?)CurentRectMarker.Tag;
|
|
if (pIndex.HasValue)
|
|
{
|
|
if (pIndex < polygon.Points.Count)
|
|
{
|
|
polygon.Points[pIndex.Value] = pnew;
|
|
lock (thisLock)
|
|
{
|
|
MainMap.UpdatePolygonLocalPosition(polygon);
|
|
}
|
|
}
|
|
}
|
|
|
|
if (currentMarker.IsVisible)
|
|
{
|
|
currentMarker.Position = pnew;
|
|
}
|
|
CurentRectMarker.Position = pnew;
|
|
|
|
if (CurentRectMarker.InnerMarker != null)
|
|
{
|
|
CurentRectMarker.InnerMarker.Position = pnew;
|
|
}
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
// MapZoomChanged
|
|
void MainMap_OnMapZoomChanged()
|
|
{
|
|
if (MainMap.Zoom > 0)
|
|
{
|
|
try
|
|
{
|
|
trackBar1.Value = (int)(MainMap.Zoom);
|
|
}
|
|
catch { }
|
|
//textBoxZoomCurrent.Text = MainMap.Zoom.ToString();
|
|
center.Position = MainMap.Position;
|
|
}
|
|
}
|
|
|
|
// click on some marker
|
|
void MainMap_OnMarkerClick(GMapMarker item, MouseEventArgs e)
|
|
{
|
|
int answer;
|
|
try // when dragging item can sometimes be null
|
|
{
|
|
if (int.TryParse(item.Tag.ToString(), out answer))
|
|
{
|
|
|
|
Commands.CurrentCell = Commands[0, answer - 1];
|
|
|
|
}
|
|
}
|
|
catch { }
|
|
return;
|
|
}
|
|
|
|
// loader start loading tiles
|
|
void MainMap_OnTileLoadStart()
|
|
{
|
|
MethodInvoker m = delegate()
|
|
{
|
|
lbl_status.Text = "Status: loading tiles...";
|
|
};
|
|
try
|
|
{
|
|
BeginInvoke(m);
|
|
}
|
|
catch
|
|
{
|
|
}
|
|
}
|
|
|
|
// loader end loading tiles
|
|
void MainMap_OnTileLoadComplete(long ElapsedMilliseconds)
|
|
{
|
|
|
|
//MainMap.ElapsedMilliseconds = ElapsedMilliseconds;
|
|
|
|
MethodInvoker m = delegate()
|
|
{
|
|
lbl_status.Text = "Status: loaded tiles";
|
|
|
|
//panelMenu.Text = "Menu, last load in " + MainMap.ElapsedMilliseconds + "ms";
|
|
|
|
//textBoxMemory.Text = string.Format(CultureInfo.InvariantCulture, "{0:0.00}MB of {1:0.00}MB", MainMap.Manager.MemoryCacheSize, MainMap.Manager.MemoryCacheCapacity);
|
|
};
|
|
try
|
|
{
|
|
BeginInvoke(m);
|
|
}
|
|
catch
|
|
{
|
|
}
|
|
|
|
}
|
|
|
|
// current point changed
|
|
void MainMap_OnCurrentPositionChanged(PointLatLng point)
|
|
{
|
|
if (point.Lat > 90) { point.Lat = 90; }
|
|
if (point.Lat < -90) { point.Lat = -90; }
|
|
if (point.Lng > 180) { point.Lng = 180; }
|
|
if (point.Lng < -180) { point.Lng = -180; }
|
|
center.Position = point;
|
|
TXT_mouselat.Text = point.Lat.ToString(CultureInfo.InvariantCulture);
|
|
TXT_mouselong.Text = point.Lng.ToString(CultureInfo.InvariantCulture);
|
|
}
|
|
|
|
// center markers on start
|
|
private void MainForm_Load(object sender, EventArgs e)
|
|
{
|
|
if (objects.Markers.Count > 0)
|
|
{
|
|
MainMap.ZoomAndCenterMarkers(null);
|
|
}
|
|
trackBar1.Value = (int)MainMap.Zoom;
|
|
}
|
|
|
|
// ensure focus on map, trackbar can have it too
|
|
private void MainMap_MouseEnter(object sender, EventArgs e)
|
|
{
|
|
// MainMap.Focus();
|
|
}
|
|
#endregion
|
|
|
|
/// <summary>
|
|
/// used to redraw the polygon
|
|
/// </summary>
|
|
void RegeneratePolygon()
|
|
{
|
|
List<PointLatLng> polygonPoints = new List<PointLatLng>();
|
|
|
|
if (objects == null)
|
|
return;
|
|
|
|
foreach (GMapMarker m in objects.Markers)
|
|
{
|
|
if (m is GMapMarkerRect)
|
|
{
|
|
m.Tag = polygonPoints.Count;
|
|
polygonPoints.Add(m.Position);
|
|
}
|
|
}
|
|
|
|
if (polygon == null)
|
|
{
|
|
polygon = new GMapPolygon(polygonPoints, "polygon test");
|
|
polygons.Polygons.Add(polygon);
|
|
}
|
|
else
|
|
{
|
|
polygon.Points.Clear();
|
|
polygon.Points.AddRange(polygonPoints);
|
|
|
|
polygon.Stroke = new Pen(Color.Yellow, 4);
|
|
|
|
if (polygons.Polygons.Count == 0)
|
|
{
|
|
polygons.Polygons.Add(polygon);
|
|
}
|
|
else
|
|
{
|
|
lock (thisLock)
|
|
{
|
|
MainMap.UpdatePolygonLocalPosition(polygon);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
private void comboBoxMapType_SelectedValueChanged(object sender, EventArgs e)
|
|
{
|
|
try
|
|
{
|
|
MainMap.MapType = (MapType)comboBoxMapType.SelectedItem;
|
|
FlightData.mymap.MapType = (MapType)comboBoxMapType.SelectedItem;
|
|
MainV2.config["MapType"] = comboBoxMapType.Text;
|
|
}
|
|
catch { CustomMessageBox.Show("Map change failed. try zomming out first."); }
|
|
}
|
|
|
|
private void Commands_EditingControlShowing(object sender, DataGridViewEditingControlShowingEventArgs e)
|
|
{
|
|
if (e.Control.GetType() == typeof(DataGridViewComboBoxEditingControl))
|
|
{
|
|
var temp = ((ComboBox)e.Control);
|
|
((ComboBox)e.Control).SelectionChangeCommitted -= new EventHandler(Commands_SelectionChangeCommitted);
|
|
((ComboBox)e.Control).SelectionChangeCommitted += new EventHandler(Commands_SelectionChangeCommitted);
|
|
((ComboBox)e.Control).ForeColor = Color.White;
|
|
((ComboBox)e.Control).BackColor = Color.FromArgb(0x43, 0x44, 0x45);
|
|
System.Diagnostics.Debug.WriteLine("Setting event handle");
|
|
}
|
|
}
|
|
|
|
void Commands_SelectionChangeCommitted(object sender, EventArgs e)
|
|
{
|
|
// update row headers
|
|
((ComboBox)sender).ForeColor = Color.White;
|
|
ChangeColumnHeader(((ComboBox)sender).Text);
|
|
try
|
|
{
|
|
for (int i = 0; i < Commands.ColumnCount; i++)
|
|
{
|
|
DataGridViewCell tcell = Commands.Rows[selectedrow].Cells[i];
|
|
if (tcell.GetType() == typeof(DataGridViewTextBoxCell))
|
|
{
|
|
if (tcell.Value.ToString() == "?")
|
|
tcell.Value = "0";
|
|
}
|
|
}
|
|
}
|
|
catch { }
|
|
}
|
|
/// <summary>
|
|
/// Get the Google earth ALT for a given coord
|
|
/// </summary>
|
|
/// <param name="lat"></param>
|
|
/// <param name="lng"></param>
|
|
/// <returns>Altitude</returns>
|
|
double getGEAlt(double lat, double lng)
|
|
{
|
|
double alt = 0;
|
|
//http://maps.google.com/maps/api/elevation/xml
|
|
|
|
try
|
|
{
|
|
using (XmlTextReader xmlreader = new XmlTextReader("http://maps.google.com/maps/api/elevation/xml?locations=" + lat.ToString(new System.Globalization.CultureInfo("en-US")) + "," + lng.ToString(new System.Globalization.CultureInfo("en-US")) + "&sensor=true"))
|
|
{
|
|
while (xmlreader.Read())
|
|
{
|
|
xmlreader.MoveToElement();
|
|
switch (xmlreader.Name)
|
|
{
|
|
case "elevation":
|
|
alt = double.Parse(xmlreader.ReadString(), new System.Globalization.CultureInfo("en-US"));
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
catch { }
|
|
|
|
return alt * MainV2.cs.multiplierdist;
|
|
}
|
|
|
|
private void TXT_homelat_Enter(object sender, EventArgs e)
|
|
{
|
|
sethome = true;
|
|
CustomMessageBox.Show("Click on the Map to set Home ");
|
|
}
|
|
|
|
private void Planner_Resize(object sender, EventArgs e)
|
|
{
|
|
MainMap.Zoom = trackBar1.Value;
|
|
}
|
|
|
|
private void button1_Click(object sender, EventArgs e)
|
|
{
|
|
writeKML();
|
|
double homealt;
|
|
double.TryParse(TXT_homealt.Text, out homealt);
|
|
Form temp = new ElevationProfile(pointlist, homealt);
|
|
ThemeManager.ApplyThemeTo(temp);
|
|
temp.ShowDialog();
|
|
}
|
|
|
|
private void CHK_altmode_CheckedChanged(object sender, EventArgs e)
|
|
{
|
|
if (Commands.RowCount > 0 && !quickadd)
|
|
CustomMessageBox.Show("You will need to change your altitudes");
|
|
}
|
|
|
|
protected override void OnPaint(PaintEventArgs pe)
|
|
{
|
|
try
|
|
{
|
|
base.OnPaint(pe);
|
|
}
|
|
catch (Exception)
|
|
{
|
|
}
|
|
}
|
|
|
|
private void Commands_CellEndEdit(object sender, DataGridViewCellEventArgs e)
|
|
{
|
|
Commands_RowEnter(null, new DataGridViewCellEventArgs(Commands.CurrentCell.ColumnIndex, Commands.CurrentCell.RowIndex));
|
|
}
|
|
|
|
private void MainMap_Resize(object sender, EventArgs e)
|
|
{
|
|
MainMap.Zoom = MainMap.Zoom + 0.01;
|
|
}
|
|
|
|
private void trackBar1_Scroll(object sender, EventArgs e)
|
|
{
|
|
try
|
|
{
|
|
lock (thisLock)
|
|
{
|
|
MainMap.Zoom = trackBar1.Value;
|
|
}
|
|
}
|
|
catch { }
|
|
}
|
|
|
|
private void panel4_PanelCollapsing(object sender, BSE.Windows.Forms.XPanderStateChangeEventArgs e)
|
|
{
|
|
|
|
}
|
|
|
|
private void BUT_Prefetch_Click(object sender, EventArgs e)
|
|
{
|
|
RectLatLng area = MainMap.SelectedArea;
|
|
if (area.IsEmpty)
|
|
{
|
|
DialogResult res = CustomMessageBox.Show("No ripp area defined, ripp displayed on screen?", "Rip", MessageBoxButtons.YesNo);
|
|
if (res == DialogResult.Yes)
|
|
{
|
|
area = MainMap.CurrentViewArea;
|
|
}
|
|
}
|
|
|
|
if (!area.IsEmpty)
|
|
{
|
|
DialogResult res = CustomMessageBox.Show("Ready ripp at Zoom = " + (int)MainMap.Zoom + " ?", "GMap.NET", MessageBoxButtons.YesNo);
|
|
|
|
for (int i = 1; i <= MainMap.MaxZoom; i++)
|
|
{
|
|
if (res == DialogResult.Yes)
|
|
{
|
|
TilePrefetcher obj = new TilePrefetcher();
|
|
obj.ShowCompleteMessage = false;
|
|
obj.Start(area, MainMap.Projection, i, MainMap.MapType, 100);
|
|
}
|
|
else if (res == DialogResult.No)
|
|
{
|
|
continue;
|
|
}
|
|
else if (res == DialogResult.Cancel)
|
|
{
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
CustomMessageBox.Show("Select map area holding ALT", "GMap.NET", MessageBoxButtons.OK, MessageBoxIcon.Exclamation);
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// from http://stackoverflow.com/questions/1119451/how-to-tell-if-a-line-intersects-a-polygon-in-c
|
|
/// </summary>
|
|
/// <param name="start1"></param>
|
|
/// <param name="end1"></param>
|
|
/// <param name="start2"></param>
|
|
/// <param name="end2"></param>
|
|
/// <returns></returns>
|
|
public PointLatLng FindLineIntersection(PointLatLng start1, PointLatLng end1, PointLatLng start2, PointLatLng end2)
|
|
{
|
|
double denom = ((end1.Lng - start1.Lng) * (end2.Lat - start2.Lat)) - ((end1.Lat - start1.Lat) * (end2.Lng - start2.Lng));
|
|
// AB & CD are parallel
|
|
if (denom == 0)
|
|
return PointLatLng.Zero;
|
|
double numer = ((start1.Lat - start2.Lat) * (end2.Lng - start2.Lng)) - ((start1.Lng - start2.Lng) * (end2.Lat - start2.Lat));
|
|
double r = numer / denom;
|
|
double numer2 = ((start1.Lat - start2.Lat) * (end1.Lng - start1.Lng)) - ((start1.Lng - start2.Lng) * (end1.Lat - start1.Lat));
|
|
double s = numer2 / denom;
|
|
if ((r < 0 || r > 1) || (s < 0 || s > 1))
|
|
return PointLatLng.Zero;
|
|
// Find intersection point
|
|
PointLatLng result = new PointLatLng();
|
|
result.Lng = start1.Lng + (r * (end1.Lng - start1.Lng));
|
|
result.Lat = start1.Lat + (r * (end1.Lat - start1.Lat));
|
|
return result;
|
|
}
|
|
|
|
RectLatLng getPolyMinMax(GMapPolygon poly)
|
|
{
|
|
if (poly.Points.Count == 0)
|
|
return new RectLatLng();
|
|
|
|
double minx, miny, maxx, maxy;
|
|
|
|
minx = maxx = poly.Points[0].Lng;
|
|
miny = maxy = poly.Points[0].Lat;
|
|
|
|
foreach (PointLatLng pnt in poly.Points)
|
|
{
|
|
//Console.WriteLine(pnt.ToString());
|
|
minx = Math.Min(minx, pnt.Lng);
|
|
maxx = Math.Max(maxx, pnt.Lng);
|
|
|
|
miny = Math.Min(miny, pnt.Lat);
|
|
maxy = Math.Max(maxy, pnt.Lat);
|
|
}
|
|
|
|
return new RectLatLng(maxy, minx, Math.Abs(maxx - minx), Math.Abs(miny - maxy));
|
|
}
|
|
|
|
const float rad2deg = (float)(180 / Math.PI);
|
|
const float deg2rad = (float)(1.0 / rad2deg);
|
|
|
|
private void BUT_grid_Click(object sender, EventArgs e)
|
|
{
|
|
polygongridmode = false;
|
|
|
|
if (drawnpolygon == null || drawnpolygon.Points.Count == 0)
|
|
{
|
|
CustomMessageBox.Show("Right click the map to draw a polygon", "Area", MessageBoxButtons.OK, MessageBoxIcon.Exclamation);
|
|
return;
|
|
}
|
|
GMapPolygon area = drawnpolygon;
|
|
area.Points.Add(area.Points[0]); // make a full loop
|
|
RectLatLng arearect = getPolyMinMax(area);
|
|
if (area.Distance > 0)
|
|
{
|
|
|
|
PointLatLng topright = new PointLatLng(arearect.LocationTopLeft.Lat, arearect.LocationRightBottom.Lng);
|
|
PointLatLng bottomleft = new PointLatLng(arearect.LocationRightBottom.Lat, arearect.LocationTopLeft.Lng);
|
|
|
|
double diagdist = MainMap.Manager.GetDistance(arearect.LocationTopLeft, arearect.LocationRightBottom) * 1000;
|
|
double heightdist = MainMap.Manager.GetDistance(arearect.LocationTopLeft, bottomleft) * 1000;
|
|
double widthdist = MainMap.Manager.GetDistance(arearect.LocationTopLeft, topright) * 1000;
|
|
|
|
|
|
|
|
string alt = (100 * MainV2.cs.multiplierdist).ToString("0");
|
|
Common.InputBox("Altitude", "Relative Altitude", ref alt);
|
|
|
|
|
|
string distance = (50 * MainV2.cs.multiplierdist).ToString("0");
|
|
Common.InputBox("Distance", "Distance between lines", ref distance);
|
|
|
|
//string overshoot = (30 * MainV2.cs.multiplierdist).ToString("0");
|
|
//Common.InputBox("Overshoot", "Enter of line overshoot amount", ref overshoot);
|
|
|
|
string angle = (90).ToString("0");
|
|
Common.InputBox("Angle", "Enter the line direction (0-180)", ref angle);
|
|
|
|
double tryme = 0;
|
|
|
|
if (!double.TryParse(angle, out tryme))
|
|
{
|
|
CustomMessageBox.Show("Invalid Angle");
|
|
return;
|
|
}
|
|
if (!double.TryParse(alt, out tryme))
|
|
{
|
|
CustomMessageBox.Show("Invalid Alt");
|
|
return;
|
|
}
|
|
if (!double.TryParse(distance, out tryme))
|
|
{
|
|
CustomMessageBox.Show("Invalid Distance");
|
|
return;
|
|
}
|
|
|
|
#if DEBUG
|
|
//Commands.Rows.Clear();
|
|
#endif
|
|
// get x y components
|
|
double x1 = Math.Cos((double.Parse(angle)) * deg2rad);
|
|
double y1 = Math.Sin((double.Parse(angle)) * deg2rad);
|
|
|
|
// get x y step amount in lat lng from m
|
|
double latdiff = arearect.HeightLat / ((heightdist / (double.Parse(distance) * (y1) / MainV2.cs.multiplierdist)));
|
|
double lngdiff = arearect.WidthLng / ((widthdist / (double.Parse(distance) * (x1) / MainV2.cs.multiplierdist)));
|
|
|
|
double latlngdiff = Math.Sqrt(latdiff * latdiff + lngdiff * lngdiff);
|
|
|
|
double fulllatdiff = arearect.HeightLat * x1 * 2;
|
|
double fulllngdiff = arearect.WidthLng * y1 * 2;
|
|
|
|
// lat - up down
|
|
// lng - left right
|
|
|
|
int overshootdist = 0;// (int)(double.Parse(overshoot) / MainV2.cs.multiplierdist);
|
|
|
|
int altitude = (int)(double.Parse(alt) / MainV2.cs.multiplierdist);
|
|
|
|
double overshootdistlng = arearect.WidthLng / widthdist * overshootdist;
|
|
|
|
bool dir = false;
|
|
|
|
int count = 0;
|
|
|
|
double x = bottomleft.Lat - Math.Abs(fulllatdiff);
|
|
double y = bottomleft.Lng - Math.Abs(fulllngdiff);
|
|
|
|
log.InfoFormat("{0} < {1} {2} < {3}", x, (topright.Lat), y, (topright.Lng));
|
|
|
|
while (x < (topright.Lat + Math.Abs(fulllatdiff)) && y < (topright.Lng + Math.Abs(fulllngdiff)))
|
|
{
|
|
if (double.Parse(angle) < 45)
|
|
{
|
|
x = bottomleft.Lat;
|
|
y += latlngdiff;
|
|
}
|
|
else if (double.Parse(angle) > 135)
|
|
{
|
|
x = arearect.LocationTopLeft.Lat; //arearect.LocationTopLeft.Lat;
|
|
y += latlngdiff;
|
|
}
|
|
else if (double.Parse(angle) > 90)
|
|
{
|
|
y = bottomleft.Lng; //arearect.LocationTopLeft.Lat;
|
|
x += latlngdiff;
|
|
}
|
|
else
|
|
{
|
|
y = bottomleft.Lng;
|
|
x += latlngdiff;
|
|
}
|
|
|
|
//callMe(x , y, 0);
|
|
//callMe(x + (fulllatdiff), y + (fulllngdiff), 0);
|
|
|
|
//continue;
|
|
|
|
PointLatLng closestlatlong = PointLatLng.Zero;
|
|
PointLatLng farestlatlong = PointLatLng.Zero;
|
|
|
|
double noc = double.MaxValue;
|
|
double nof = double.MinValue;
|
|
|
|
if (dir)
|
|
{
|
|
double ax = x;
|
|
double ay = y;
|
|
|
|
double bx = x + fulllatdiff;
|
|
double by = y + fulllngdiff;
|
|
int a = -1;
|
|
PointLatLng newlatlong = PointLatLng.Zero;
|
|
foreach (PointLatLng pnt in area.Points)
|
|
{
|
|
a++;
|
|
if (a == 0)
|
|
{
|
|
continue;
|
|
}
|
|
newlatlong = FindLineIntersection(area.Points[a - 1], area.Points[a], new PointLatLng(ax, ay), new PointLatLng(bx, by));
|
|
if (!newlatlong.IsZero)
|
|
{
|
|
if (noc > MainMap.Manager.GetDistance(new PointLatLng(ax, ay), newlatlong))
|
|
{
|
|
closestlatlong.Lat = newlatlong.Lat;
|
|
closestlatlong.Lng = newlatlong.Lng;
|
|
noc = MainMap.Manager.GetDistance(new PointLatLng(ax, ay), newlatlong);
|
|
}
|
|
if (nof < MainMap.Manager.GetDistance(new PointLatLng(ax, ay), newlatlong))
|
|
{
|
|
farestlatlong.Lat = newlatlong.Lat;
|
|
farestlatlong.Lng = newlatlong.Lng;
|
|
nof = MainMap.Manager.GetDistance(new PointLatLng(ax, ay), newlatlong);
|
|
}
|
|
}
|
|
}
|
|
|
|
if (!farestlatlong.IsZero)
|
|
callMe(farestlatlong.Lat, farestlatlong.Lng, altitude);
|
|
if (!closestlatlong.IsZero)
|
|
callMe(closestlatlong.Lat, closestlatlong.Lng - overshootdistlng, altitude);
|
|
|
|
//callMe(x, topright.Lng, altitude);
|
|
//callMe(x, bottomleft.Lng - overshootdistlng, altitude);
|
|
}
|
|
else
|
|
{
|
|
double ax = x;
|
|
double ay = y;
|
|
|
|
double bx = x + fulllatdiff;
|
|
double by = y + fulllngdiff;
|
|
int a = -1;
|
|
PointLatLng newlatlong = PointLatLng.Zero;
|
|
foreach (PointLatLng pnt in area.Points)
|
|
{
|
|
a++;
|
|
if (a == 0)
|
|
{
|
|
continue;
|
|
}
|
|
newlatlong = FindLineIntersection(area.Points[a - 1], area.Points[a], new PointLatLng(ax, ay), new PointLatLng(bx, by));
|
|
if (!newlatlong.IsZero)
|
|
{
|
|
if (noc > MainMap.Manager.GetDistance(new PointLatLng(ax, ay), newlatlong))
|
|
{
|
|
closestlatlong.Lat = newlatlong.Lat;
|
|
closestlatlong.Lng = newlatlong.Lng;
|
|
noc = MainMap.Manager.GetDistance(new PointLatLng(ax, ay), newlatlong);
|
|
}
|
|
if (nof < MainMap.Manager.GetDistance(new PointLatLng(ax, ay), newlatlong))
|
|
{
|
|
farestlatlong.Lat = newlatlong.Lat;
|
|
farestlatlong.Lng = newlatlong.Lng;
|
|
nof = MainMap.Manager.GetDistance(new PointLatLng(ax, ay), newlatlong);
|
|
}
|
|
}
|
|
}
|
|
if (!closestlatlong.IsZero)
|
|
callMe(closestlatlong.Lat, closestlatlong.Lng, altitude);
|
|
if (!farestlatlong.IsZero)
|
|
callMe(farestlatlong.Lat, farestlatlong.Lng + overshootdistlng, altitude);
|
|
//callMe(x, bottomleft.Lng, altitude);
|
|
//callMe(x, topright.Lng + overshootdistlng, altitude);
|
|
}
|
|
|
|
dir = !dir;
|
|
|
|
count++;
|
|
|
|
if (Commands.RowCount > 150)
|
|
{
|
|
CustomMessageBox.Show("Stopping at 150 WP's");
|
|
break;
|
|
}
|
|
}
|
|
|
|
//drawnpolygon.Points.Clear();
|
|
//drawnpolygons.Markers.Clear();
|
|
MainMap.Refresh();
|
|
|
|
}
|
|
}
|
|
|
|
private void label4_LinkClicked(object sender, LinkLabelLinkClickedEventArgs e)
|
|
{
|
|
if (MainV2.cs.lat != 0)
|
|
{
|
|
TXT_homealt.Text = (MainV2.cs.alt).ToString("0");
|
|
TXT_homelat.Text = MainV2.cs.lat.ToString();
|
|
TXT_homelng.Text = MainV2.cs.lng.ToString();
|
|
}
|
|
else
|
|
{
|
|
CustomMessageBox.Show("If you're at the field, connect to your APM and wait for GPS lock. Then click 'Home Location' link to set home to your location");
|
|
}
|
|
}
|
|
|
|
|
|
/// <summary>
|
|
/// Format distance according to prefer distance unit
|
|
/// </summary>
|
|
/// <param name="distInKM">distance in kilometers</param>
|
|
/// <param name="toMeterOrFeet">convert distance to meter or feet if true, covert to km or miles if false</param>
|
|
/// <returns>formatted distance with unit</returns>
|
|
private string FormatDistance(double distInKM, bool toMeterOrFeet)
|
|
{
|
|
string sunits = MainV2.getConfig("distunits");
|
|
Common.distances units = Common.distances.Meters;
|
|
|
|
if (sunits != "")
|
|
try
|
|
{
|
|
units = (Common.distances)Enum.Parse(typeof(Common.distances), sunits);
|
|
}
|
|
catch (Exception) { }
|
|
|
|
switch (units)
|
|
{
|
|
case Common.distances.Feet:
|
|
return toMeterOrFeet ? string.Format((distInKM * 3280.8399).ToString("0.00 ft")) :
|
|
string.Format((distInKM * 0.621371).ToString("0.0000 miles"));
|
|
case Common.distances.Meters:
|
|
default:
|
|
return toMeterOrFeet ? string.Format((distInKM * 1000).ToString("0.00 m")) :
|
|
string.Format(distInKM.ToString("0.0000 km"));
|
|
}
|
|
}
|
|
|
|
PointLatLng startmeasure = new PointLatLng();
|
|
|
|
private void ContextMeasure_Click(object sender, EventArgs e)
|
|
{
|
|
if (startmeasure.IsZero)
|
|
{
|
|
startmeasure = start;
|
|
polygons.Markers.Add(new GMapMarkerGoogleRed(start));
|
|
MainMap.Invalidate();
|
|
}
|
|
else
|
|
{
|
|
List<PointLatLng> polygonPoints = new List<PointLatLng>();
|
|
polygonPoints.Add(startmeasure);
|
|
polygonPoints.Add(start);
|
|
|
|
GMapPolygon line = new GMapPolygon(polygonPoints, "measure dist");
|
|
line.Stroke.Color = Color.Green;
|
|
|
|
polygons.Polygons.Add(line);
|
|
|
|
polygons.Markers.Add(new GMapMarkerGoogleRed(start));
|
|
MainMap.Invalidate();
|
|
CustomMessageBox.Show("Distance: " + FormatDistance(MainMap.Manager.GetDistance(startmeasure, start), true) + " AZ: " + (MainMap.Manager.GetBearing(startmeasure, start).ToString("0")));
|
|
polygons.Polygons.Remove(line);
|
|
polygons.Markers.Clear();
|
|
startmeasure = new PointLatLng();
|
|
}
|
|
}
|
|
|
|
private void rotateMapToolStripMenuItem_Click(object sender, EventArgs e)
|
|
{
|
|
string heading = "0";
|
|
Common.InputBox("Rotate map to heading", "Enter new UP heading", ref heading);
|
|
float ans = 0;
|
|
if (float.TryParse(heading, out ans))
|
|
{
|
|
MainMap.Bearing = ans;
|
|
FlightData.mymap.Bearing = ans;
|
|
}
|
|
}
|
|
|
|
private void addPolygonPointToolStripMenuItem_Click(object sender, EventArgs e)
|
|
{
|
|
if (polygongridmode == false)
|
|
{
|
|
CustomMessageBox.Show("You will remain in polygon mode until you clear the polygon or create a grid/upload a fence");
|
|
}
|
|
|
|
polygongridmode = true;
|
|
|
|
List<PointLatLng> polygonPoints = new List<PointLatLng>();
|
|
if (drawnpolygons.Polygons.Count == 0)
|
|
{
|
|
drawnpolygon.Points.Clear();
|
|
drawnpolygons.Polygons.Add(drawnpolygon);
|
|
}
|
|
drawnpolygon.Points.Add(new PointLatLng(start.Lat, start.Lng));
|
|
|
|
addpolygonmarkergrid(drawnpolygon.Points.Count.ToString(), start.Lng, start.Lat, 0);
|
|
|
|
MainMap.UpdatePolygonLocalPosition(drawnpolygon);
|
|
|
|
MainMap.Invalidate();
|
|
|
|
}
|
|
|
|
private void clearPolygonToolStripMenuItem_Click(object sender, EventArgs e)
|
|
{
|
|
polygongridmode = false;
|
|
if (drawnpolygon == null)
|
|
return;
|
|
drawnpolygon.Points.Clear();
|
|
drawnpolygons.Markers.Clear();
|
|
MainMap.Invalidate();
|
|
|
|
writeKML();
|
|
}
|
|
|
|
private void clearMissionToolStripMenuItem_Click(object sender, EventArgs e)
|
|
{
|
|
Commands.Rows.Clear();
|
|
selectedrow = 0;
|
|
writeKML();
|
|
}
|
|
|
|
private void loiterForeverToolStripMenuItem_Click(object sender, EventArgs e)
|
|
{
|
|
selectedrow = Commands.Rows.Add();
|
|
|
|
Commands.Rows[selectedrow].Cells[Command.Index].Value = MAVLink.MAV_CMD.LOITER_UNLIM.ToString();
|
|
|
|
ChangeColumnHeader(MAVLink.MAV_CMD.LOITER_UNLIM.ToString());
|
|
|
|
setfromGE(end.Lat, end.Lng, (int)float.Parse(TXT_DefaultAlt.Text));
|
|
|
|
writeKML();
|
|
}
|
|
|
|
private void jumpstartToolStripMenuItem_Click(object sender, EventArgs e)
|
|
{
|
|
string repeat = "5";
|
|
Common.InputBox("Jump repeat", "Number of times to Repeat", ref repeat);
|
|
|
|
int row = Commands.Rows.Add();
|
|
|
|
Commands.Rows[row].Cells[Command.Index].Value = MAVLink.MAV_CMD.DO_JUMP.ToString();
|
|
|
|
Commands.Rows[row].Cells[Param1.Index].Value = 1;
|
|
|
|
Commands.Rows[row].Cells[Param2.Index].Value = repeat;
|
|
|
|
writeKML();
|
|
}
|
|
|
|
private void jumpwPToolStripMenuItem_Click(object sender, EventArgs e)
|
|
{
|
|
string wp = "1";
|
|
Common.InputBox("WP No", "Jump to WP no?", ref wp);
|
|
string repeat = "5";
|
|
Common.InputBox("Jump repeat", "Number of times to Repeat", ref repeat);
|
|
|
|
int row = Commands.Rows.Add();
|
|
|
|
Commands.Rows[row].Cells[Command.Index].Value = MAVLink.MAV_CMD.DO_JUMP.ToString();
|
|
|
|
Commands.Rows[row].Cells[Param1.Index].Value = wp;
|
|
|
|
Commands.Rows[row].Cells[Param2.Index].Value = repeat;
|
|
|
|
writeKML();
|
|
}
|
|
|
|
private void deleteWPToolStripMenuItem_Click(object sender, EventArgs e)
|
|
{
|
|
int no = 0;
|
|
if (CurentRectMarker != null)
|
|
{
|
|
if (int.TryParse(CurentRectMarker.InnerMarker.Tag.ToString(), out no))
|
|
{
|
|
Commands.Rows.RemoveAt(no - 1); // home is 0
|
|
}
|
|
else if (int.TryParse(CurentRectMarker.InnerMarker.Tag.ToString().Replace("grid", ""), out no))
|
|
{
|
|
try
|
|
{
|
|
drawnpolygon.Points.RemoveAt(no - 1);
|
|
drawnpolygons.Markers.Clear();
|
|
|
|
int a = 1;
|
|
foreach (PointLatLng pnt in drawnpolygon.Points)
|
|
{
|
|
addpolygonmarkergrid(a.ToString(), pnt.Lng, pnt.Lat, 0);
|
|
a++;
|
|
}
|
|
|
|
MainMap.UpdatePolygonLocalPosition(drawnpolygon);
|
|
|
|
MainMap.Invalidate();
|
|
}
|
|
catch {
|
|
CustomMessageBox.Show("Remove point Failed. Please try again.");
|
|
}
|
|
}
|
|
}
|
|
|
|
CurentRectMarker = null;
|
|
|
|
writeKML();
|
|
}
|
|
|
|
private void loitertimeToolStripMenuItem_Click(object sender, EventArgs e)
|
|
{
|
|
string time = "5";
|
|
Common.InputBox("Loiter Time", "Loiter Time", ref time);
|
|
|
|
selectedrow = Commands.Rows.Add();
|
|
|
|
Commands.Rows[selectedrow].Cells[Command.Index].Value = MAVLink.MAV_CMD.LOITER_TIME.ToString();
|
|
|
|
Commands.Rows[selectedrow].Cells[Param1.Index].Value = time;
|
|
|
|
ChangeColumnHeader(MAVLink.MAV_CMD.LOITER_TIME.ToString());
|
|
|
|
setfromGE(end.Lat, end.Lng, (int)float.Parse(TXT_DefaultAlt.Text));
|
|
|
|
writeKML();
|
|
}
|
|
|
|
private void loitercirclesToolStripMenuItem_Click(object sender, EventArgs e)
|
|
{
|
|
string turns = "3";
|
|
Common.InputBox("Loiter Turns", "Loiter Turns", ref turns);
|
|
|
|
selectedrow = Commands.Rows.Add();
|
|
|
|
Commands.Rows[selectedrow].Cells[Command.Index].Value = MAVLink.MAV_CMD.LOITER_TURNS.ToString();
|
|
|
|
Commands.Rows[selectedrow].Cells[Param1.Index].Value = turns;
|
|
|
|
ChangeColumnHeader(MAVLink.MAV_CMD.LOITER_TURNS.ToString());
|
|
|
|
setfromGE(end.Lat, end.Lng, (int)float.Parse(TXT_DefaultAlt.Text));
|
|
|
|
writeKML();
|
|
}
|
|
|
|
private void BUT_Camera_Click(object sender, EventArgs e)
|
|
{
|
|
Camera form = new Camera();
|
|
ThemeManager.ApplyThemeTo(form);
|
|
form.Show();
|
|
}
|
|
|
|
private void panelMap_Resize(object sender, EventArgs e)
|
|
{
|
|
// this is a mono fix for the zoom bar
|
|
//Console.WriteLine("panelmap "+panelMap.Size.ToString());
|
|
MainMap.Size = new Size(panelMap.Size.Width - 50, panelMap.Size.Height);
|
|
trackBar1.Location = new System.Drawing.Point(panelMap.Size.Width - 50, trackBar1.Location.Y);
|
|
trackBar1.Size = new System.Drawing.Size(trackBar1.Size.Width, panelMap.Size.Height - trackBar1.Location.Y);
|
|
label11.Location = new System.Drawing.Point(panelMap.Size.Width - 50, label11.Location.Y);
|
|
}
|
|
|
|
private void BUT_zoomto_Click(object sender, EventArgs e)
|
|
{
|
|
string place = "Perth Airport, Australia";
|
|
if (DialogResult.OK == Common.InputBox("Location", "Enter your location", ref place))
|
|
{
|
|
|
|
GeoCoderStatusCode status = MainMap.SetCurrentPositionByKeywords(place);
|
|
if (status != GeoCoderStatusCode.G_GEO_SUCCESS)
|
|
{
|
|
CustomMessageBox.Show("Google Maps Geocoder can't find: '" + place + "', reason: " + status.ToString(), "GMap.NET", MessageBoxButtons.OK, MessageBoxIcon.Exclamation);
|
|
}
|
|
else
|
|
{
|
|
MainMap.Zoom = 15;
|
|
}
|
|
}
|
|
}
|
|
|
|
private void BUT_loadkml_Click(object sender, EventArgs e)
|
|
{
|
|
|
|
OpenFileDialog fd = new OpenFileDialog();
|
|
fd.Filter = "Google Earth KML (*.kml)|*.kml";
|
|
fd.DefaultExt = ".kml";
|
|
DialogResult result = fd.ShowDialog();
|
|
string file = fd.FileName;
|
|
if (file != "")
|
|
{
|
|
try
|
|
{
|
|
kmlpolygons.Polygons.Clear();
|
|
kmlpolygons.Routes.Clear();
|
|
|
|
FlightData.kmlpolygons.Routes.Clear();
|
|
FlightData.kmlpolygons.Polygons.Clear();
|
|
|
|
string kml = new StreamReader(File.OpenRead(file)).ReadToEnd();
|
|
|
|
kml = kml.Replace("<Snippet/>", "");
|
|
|
|
var parser = new SharpKml.Base.Parser();
|
|
|
|
parser.ElementAdded += parser_ElementAdded;
|
|
parser.ParseString(kml, true);
|
|
|
|
if (DialogResult.Yes == CustomMessageBox.Show("Do you want to load this into the flight data screen?", "Load data", MessageBoxButtons.YesNo))
|
|
{
|
|
foreach (var temp in kmlpolygons.Polygons)
|
|
{
|
|
FlightData.kmlpolygons.Polygons.Add(temp);
|
|
}
|
|
foreach (var temp in kmlpolygons.Routes)
|
|
{
|
|
FlightData.kmlpolygons.Routes.Add(temp);
|
|
}
|
|
}
|
|
|
|
}
|
|
catch (Exception ex) { CustomMessageBox.Show("Bad KML File :" + ex.ToString()); }
|
|
}
|
|
|
|
}
|
|
|
|
private void timer1_Tick(object sender, EventArgs e)
|
|
{
|
|
try
|
|
{
|
|
routes.Markers.Clear();
|
|
|
|
if (MainV2.cs.lat == 0 || MainV2.cs.lng == 0)
|
|
return;
|
|
|
|
PointLatLng currentloc = new PointLatLng(MainV2.cs.lat, MainV2.cs.lng);
|
|
|
|
if (MainV2.cs.firmware == MainV2.Firmwares.ArduPlane)
|
|
{
|
|
routes.Markers.Add(new GMapMarkerPlane(currentloc, MainV2.cs.yaw, MainV2.cs.groundcourse, MainV2.cs.nav_bearing, MainV2.cs.target_bearing) { ToolTipText = MainV2.cs.alt.ToString("0"), ToolTipMode = MarkerTooltipMode.Always });
|
|
}
|
|
else
|
|
{
|
|
routes.Markers.Add(new GMapMarkerQuad(currentloc, MainV2.cs.yaw, MainV2.cs.groundcourse, MainV2.cs.nav_bearing));
|
|
}
|
|
}
|
|
catch { }
|
|
}
|
|
|
|
private void GeoFenceuploadToolStripMenuItem_Click(object sender, EventArgs e)
|
|
{
|
|
polygongridmode = false;
|
|
//FENCE_TOTAL
|
|
if (MainV2.comPort.param["FENCE_ACTION"] == null)
|
|
{
|
|
CustomMessageBox.Show("Not Supported");
|
|
return;
|
|
}
|
|
|
|
if (drawnpolygon == null)
|
|
{
|
|
CustomMessageBox.Show("No polygon to upload");
|
|
return;
|
|
}
|
|
|
|
if (geofence.Markers.Count == 0)
|
|
{
|
|
CustomMessageBox.Show("No return location set");
|
|
return;
|
|
}
|
|
|
|
if (drawnpolygon.Points.Count == 0)
|
|
{
|
|
CustomMessageBox.Show("No polygon drawn");
|
|
return;
|
|
}
|
|
|
|
// check if return is inside polygon
|
|
List<PointLatLng> plll = new List<PointLatLng>(drawnpolygon.Points.ToArray());
|
|
// close it
|
|
plll.Add(plll[0]);
|
|
// check it
|
|
if (!pnpoly(plll.ToArray(), geofence.Markers[0].Position.Lat, geofence.Markers[0].Position.Lng))
|
|
{
|
|
CustomMessageBox.Show("Your return location is outside the polygon");
|
|
return;
|
|
}
|
|
|
|
string minalts = (int.Parse(MainV2.comPort.param["FENCE_MINALT"].ToString()) * MainV2.cs.multiplierdist).ToString("0");
|
|
Common.InputBox("Min Alt", "Box Minimum Altitude?", ref minalts);
|
|
|
|
string maxalts = (int.Parse(MainV2.comPort.param["FENCE_MAXALT"].ToString()) * MainV2.cs.multiplierdist).ToString("0");
|
|
Common.InputBox("Max Alt", "Box Maximum Altitude?", ref maxalts);
|
|
|
|
int minalt = 0;
|
|
int maxalt = 0;
|
|
|
|
if (!int.TryParse(minalts, out minalt))
|
|
{
|
|
CustomMessageBox.Show("Bad Min Alt");
|
|
return;
|
|
}
|
|
|
|
if (!int.TryParse(maxalts, out maxalt))
|
|
{
|
|
CustomMessageBox.Show("Bad Max Alt");
|
|
return;
|
|
}
|
|
|
|
try
|
|
{
|
|
MainV2.comPort.setParam("FENCE_MINALT", minalt);
|
|
MainV2.comPort.setParam("FENCE_MAXALT", maxalt);
|
|
}
|
|
catch
|
|
{
|
|
CustomMessageBox.Show("Failed to set min/max fence alt");
|
|
return;
|
|
}
|
|
|
|
try
|
|
{
|
|
if (MainV2.comPort.param["FENCE_ACTION"].ToString() != "0")
|
|
MainV2.comPort.setParam("FENCE_ACTION", 0);
|
|
}
|
|
catch
|
|
{
|
|
CustomMessageBox.Show("Failed to set FENCE_ACTION");
|
|
return;
|
|
}
|
|
|
|
// points + return + close
|
|
byte pointcount = (byte)(drawnpolygon.Points.Count + 2);
|
|
|
|
MainV2.comPort.setParam("FENCE_TOTAL", pointcount);
|
|
|
|
byte a = 0;
|
|
|
|
// add return loc
|
|
MainV2.comPort.setFencePoint(a, new PointLatLngAlt(geofence.Markers[0].Position), pointcount);
|
|
a++;
|
|
|
|
// add points
|
|
foreach (var pll in drawnpolygon.Points)
|
|
{
|
|
MainV2.comPort.setFencePoint(a, new PointLatLngAlt(pll), pointcount);
|
|
a++;
|
|
}
|
|
// add polygon close
|
|
MainV2.comPort.setFencePoint(a, new PointLatLngAlt(drawnpolygon.Points[0]), pointcount);
|
|
|
|
// clear everything
|
|
drawnpolygons.Polygons.Clear();
|
|
drawnpolygons.Markers.Clear();
|
|
geofence.Polygons.Clear();
|
|
gf.Points.Clear();
|
|
|
|
// add polygon
|
|
gf.Points.AddRange(drawnpolygon.Points.ToArray());
|
|
|
|
drawnpolygon.Points.Clear();
|
|
|
|
geofence.Polygons.Add(gf);
|
|
|
|
// update flightdata
|
|
FlightData.geofence.Markers.Clear();
|
|
FlightData.geofence.Polygons.Clear();
|
|
FlightData.geofence.Polygons.Add(new GMapPolygon(gf.Points, "gf fd") { Stroke = gf.Stroke });
|
|
FlightData.geofence.Markers.Add(new GMapMarkerGoogleRed(geofence.Markers[0].Position) { ToolTipText = geofence.Markers[0].ToolTipText, ToolTipMode = geofence.Markers[0].ToolTipMode });
|
|
|
|
MainMap.UpdatePolygonLocalPosition(gf);
|
|
MainMap.UpdateMarkerLocalPosition(geofence.Markers[0]);
|
|
|
|
MainMap.Invalidate();
|
|
}
|
|
|
|
private void GeoFencedownloadToolStripMenuItem_Click(object sender, EventArgs e)
|
|
{
|
|
polygongridmode = false;
|
|
int count = 1;
|
|
|
|
if (MainV2.comPort.param["FENCE_ACTION"] == null || MainV2.comPort.param["FENCE_TOTAL"] == null)
|
|
{
|
|
CustomMessageBox.Show("Not Supported");
|
|
return;
|
|
}
|
|
|
|
if (int.Parse(MainV2.comPort.param["FENCE_TOTAL"].ToString()) <= 1)
|
|
{
|
|
CustomMessageBox.Show("Nothing to download");
|
|
return;
|
|
}
|
|
|
|
geofence.Polygons.Clear();
|
|
geofence.Markers.Clear();
|
|
gf.Points.Clear();
|
|
|
|
|
|
for (int a = 0; a < count; a++)
|
|
{
|
|
PointLatLngAlt plla = MainV2.comPort.getFencePoint(a, ref count);
|
|
gf.Points.Add(new PointLatLng(plla.Lat, plla.Lng));
|
|
}
|
|
|
|
// do return location
|
|
geofence.Markers.Add(new GMapMarkerGoogleRed(new PointLatLng(gf.Points[0].Lat, gf.Points[0].Lng)) { ToolTipMode = MarkerTooltipMode.OnMouseOver, ToolTipText = "GeoFence Return" });
|
|
gf.Points.RemoveAt(0);
|
|
|
|
// add now - so local points are calced
|
|
geofence.Polygons.Add(gf);
|
|
|
|
// update flight data
|
|
FlightData.geofence.Markers.Clear();
|
|
FlightData.geofence.Polygons.Clear();
|
|
FlightData.geofence.Polygons.Add(new GMapPolygon(gf.Points, "gf fd") { Stroke = gf.Stroke });
|
|
FlightData.geofence.Markers.Add(new GMapMarkerGoogleRed(geofence.Markers[0].Position) { ToolTipText = geofence.Markers[0].ToolTipText, ToolTipMode = geofence.Markers[0].ToolTipMode });
|
|
|
|
MainMap.UpdatePolygonLocalPosition(gf);
|
|
MainMap.UpdateMarkerLocalPosition(geofence.Markers[0]);
|
|
|
|
MainMap.Invalidate();
|
|
}
|
|
|
|
private void setReturnLocationToolStripMenuItem_Click(object sender, EventArgs e)
|
|
{
|
|
geofence.Markers.Clear();
|
|
geofence.Markers.Add(new GMapMarkerGoogleRed(new PointLatLng(start.Lat, start.Lng)) { ToolTipMode = MarkerTooltipMode.OnMouseOver, ToolTipText = "GeoFence Return" });
|
|
|
|
MainMap.Invalidate();
|
|
}
|
|
|
|
/// <summary>
|
|
/// from http://www.ecse.rpi.edu/Homepages/wrf/Research/Short_Notes/pnpoly.html
|
|
/// </summary>
|
|
/// <param name="array"> a closed polygon</param>
|
|
/// <param name="testx"></param>
|
|
/// <param name="testy"></param>
|
|
/// <returns> true = outside</returns>
|
|
bool pnpoly(PointLatLng[] array, double testx, double testy)
|
|
{
|
|
int nvert = array.Length;
|
|
int i, j = 0;
|
|
bool c = false;
|
|
for (i = 0, j = nvert - 1; i < nvert; j = i++)
|
|
{
|
|
if (((array[i].Lng > testy) != (array[j].Lng > testy)) &&
|
|
(testx < (array[j].Lat - array[i].Lat) * (testy - array[i].Lng) / (array[j].Lng - array[i].Lng) + array[i].Lat))
|
|
c = !c;
|
|
}
|
|
return c;
|
|
}
|
|
|
|
private void loadFromFileToolStripMenuItem_Click(object sender, EventArgs e)
|
|
{
|
|
OpenFileDialog fd = new OpenFileDialog();
|
|
fd.Filter = "Fence (*.fen)|*.fen";
|
|
fd.ShowDialog();
|
|
if (File.Exists(fd.FileName))
|
|
{
|
|
StreamReader sr = new StreamReader(fd.OpenFile());
|
|
|
|
drawnpolygons.Markers.Clear();
|
|
drawnpolygons.Polygons.Clear();
|
|
drawnpolygon.Points.Clear();
|
|
|
|
int a = 0;
|
|
|
|
while (!sr.EndOfStream)
|
|
{
|
|
string line = sr.ReadLine();
|
|
if (line.StartsWith("#"))
|
|
{
|
|
continue;
|
|
}
|
|
else
|
|
{
|
|
string[] items = line.Split(new char[] { ' ', '\t' }, StringSplitOptions.RemoveEmptyEntries);
|
|
|
|
if (a == 0)
|
|
{
|
|
geofence.Markers.Clear();
|
|
geofence.Markers.Add(new GMapMarkerGoogleRed(new PointLatLng(double.Parse(items[0]), double.Parse(items[1]))) { ToolTipMode = MarkerTooltipMode.OnMouseOver, ToolTipText = "GeoFence Return" });
|
|
MainMap.UpdateMarkerLocalPosition(geofence.Markers[0]);
|
|
}
|
|
else
|
|
{
|
|
drawnpolygon.Points.Add(new PointLatLng(double.Parse(items[0]), double.Parse(items[1])));
|
|
addpolygonmarkergrid(drawnpolygon.Points.Count.ToString(), double.Parse(items[1]), double.Parse(items[0]), 0);
|
|
}
|
|
a++;
|
|
}
|
|
}
|
|
|
|
// remove loop close
|
|
if (drawnpolygon.Points[0] == drawnpolygon.Points[drawnpolygon.Points.Count - 1])
|
|
{
|
|
drawnpolygon.Points.RemoveAt(drawnpolygon.Points.Count - 1);
|
|
}
|
|
|
|
drawnpolygons.Polygons.Add(drawnpolygon);
|
|
|
|
MainMap.UpdatePolygonLocalPosition(drawnpolygon);
|
|
|
|
MainMap.Invalidate();
|
|
}
|
|
}
|
|
|
|
private void saveToFileToolStripMenuItem_Click(object sender, EventArgs e)
|
|
{
|
|
if (geofence.Markers.Count == 0)
|
|
{
|
|
CustomMessageBox.Show("Please set a return location");
|
|
return;
|
|
}
|
|
|
|
|
|
SaveFileDialog sf = new SaveFileDialog();
|
|
sf.Filter = "Fence (*.fen)|*.fen";
|
|
sf.ShowDialog();
|
|
if (sf.FileName != "")
|
|
{
|
|
try
|
|
{
|
|
StreamWriter sw = new StreamWriter(sf.OpenFile());
|
|
|
|
sw.WriteLine("#saved by APM Planner " + Application.ProductVersion);
|
|
|
|
sw.WriteLine(geofence.Markers[0].Position.Lat + " " + geofence.Markers[0].Position.Lng);
|
|
if (drawnpolygon.Points.Count > 0)
|
|
{
|
|
foreach (var pll in drawnpolygon.Points)
|
|
{
|
|
sw.WriteLine(pll.Lat + " " + pll.Lng);
|
|
}
|
|
|
|
PointLatLng pll2 = drawnpolygon.Points[0];
|
|
|
|
sw.WriteLine(pll2.Lat + " " + pll2.Lng);
|
|
}
|
|
else
|
|
{
|
|
foreach (var pll in gf.Points)
|
|
{
|
|
sw.WriteLine(pll.Lat + " " + pll.Lng);
|
|
}
|
|
|
|
PointLatLng pll2 = gf.Points[0];
|
|
|
|
sw.WriteLine(pll2.Lat + " " + pll2.Lng);
|
|
}
|
|
|
|
sw.Close();
|
|
}
|
|
catch { CustomMessageBox.Show("Failed to write fence file"); }
|
|
}
|
|
}
|
|
|
|
public T DeepClone<T>(T obj)
|
|
{
|
|
using (var ms = new System.IO.MemoryStream())
|
|
{
|
|
var formatter = new System.Runtime.Serialization.Formatters.Binary.BinaryFormatter();
|
|
|
|
formatter.Serialize(ms, obj);
|
|
|
|
ms.Position = 0;
|
|
|
|
return (T)formatter.Deserialize(ms);
|
|
}
|
|
}
|
|
}
|
|
} |