diff --git a/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj b/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj
index f7586c56c9..98449d5ca8 100644
--- a/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj
+++ b/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj
@@ -1326,6 +1326,9 @@
true
+
+
+
"$(TargetDir)version.exe" "$(TargetPath)" > "$(TargetDir)version.txt"
diff --git a/Tools/ArdupilotMegaPlanner/ArdupilotMega.sln b/Tools/ArdupilotMegaPlanner/ArdupilotMega.sln
index 256d4bb24b..b74dfc2d2b 100644
--- a/Tools/ArdupilotMegaPlanner/ArdupilotMega.sln
+++ b/Tools/ArdupilotMegaPlanner/ArdupilotMega.sln
@@ -9,6 +9,8 @@ Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "wix", "wix\wix.csproj", "{7
EndProject
Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "3DRRadio", "3DRRadio\3DRRadio.csproj", "{B8943726-04B0-4477-BFDA-E156A0CD98A4}"
EndProject
+Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "px4uploader", "..\px4uploader\px4uploader.csproj", "{664FC484-2A94-4B0D-808F-A71F88E06B11}"
+EndProject
Global
GlobalSection(SolutionConfigurationPlatforms) = preSolution
Debug|Any CPU = Debug|Any CPU
@@ -69,6 +71,18 @@ Global
{B8943726-04B0-4477-BFDA-E156A0CD98A4}.Release|Win32.ActiveCfg = Release|x86
{B8943726-04B0-4477-BFDA-E156A0CD98A4}.Release|x86.ActiveCfg = Release|x86
{B8943726-04B0-4477-BFDA-E156A0CD98A4}.Release|x86.Build.0 = Release|x86
+ {664FC484-2A94-4B0D-808F-A71F88E06B11}.Debug|Any CPU.ActiveCfg = Debug|x86
+ {664FC484-2A94-4B0D-808F-A71F88E06B11}.Debug|Mixed Platforms.ActiveCfg = Debug|x86
+ {664FC484-2A94-4B0D-808F-A71F88E06B11}.Debug|Mixed Platforms.Build.0 = Debug|x86
+ {664FC484-2A94-4B0D-808F-A71F88E06B11}.Debug|Win32.ActiveCfg = Debug|x86
+ {664FC484-2A94-4B0D-808F-A71F88E06B11}.Debug|x86.ActiveCfg = Debug|x86
+ {664FC484-2A94-4B0D-808F-A71F88E06B11}.Debug|x86.Build.0 = Debug|x86
+ {664FC484-2A94-4B0D-808F-A71F88E06B11}.Release|Any CPU.ActiveCfg = Release|x86
+ {664FC484-2A94-4B0D-808F-A71F88E06B11}.Release|Mixed Platforms.ActiveCfg = Release|x86
+ {664FC484-2A94-4B0D-808F-A71F88E06B11}.Release|Mixed Platforms.Build.0 = Release|x86
+ {664FC484-2A94-4B0D-808F-A71F88E06B11}.Release|Win32.ActiveCfg = Release|x86
+ {664FC484-2A94-4B0D-808F-A71F88E06B11}.Release|x86.ActiveCfg = Release|x86
+ {664FC484-2A94-4B0D-808F-A71F88E06B11}.Release|x86.Build.0 = Release|x86
EndGlobalSection
GlobalSection(SolutionProperties) = preSolution
HideSolutionNode = FALSE
diff --git a/Tools/ArdupilotMegaPlanner/Controls/HUD.cs b/Tools/ArdupilotMegaPlanner/Controls/HUD.cs
index 311596edd0..66b8600655 100644
--- a/Tools/ArdupilotMegaPlanner/Controls/HUD.cs
+++ b/Tools/ArdupilotMegaPlanner/Controls/HUD.cs
@@ -286,7 +286,9 @@ namespace ArdupilotMega.Controls
//GL.Enable(EnableCap.AlphaTest)
// Console.WriteLine("hud paint");
-
+
+ // Console.WriteLine("ms " + (DateTime.Now.Millisecond));
+
if (!started)
return;
diff --git a/Tools/ArdupilotMegaPlanner/CurrentState.cs b/Tools/ArdupilotMegaPlanner/CurrentState.cs
index 58fddbfaca..81dc71ac85 100644
--- a/Tools/ArdupilotMegaPlanner/CurrentState.cs
+++ b/Tools/ArdupilotMegaPlanner/CurrentState.cs
@@ -751,7 +751,7 @@ namespace ArdupilotMega
gpsstatus = gps.fix_type;
// Console.WriteLine("gpsfix {0}",gpsstatus);
- gpshdop = gps.eph;
+ gpshdop = (float)Math.Round((double)gps.eph / 10.0,2);
satcount = gps.satellites_visible;
diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs b/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs
index 41e5ef651b..6ed5444441 100644
--- a/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs
+++ b/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs
@@ -2020,6 +2020,8 @@ print 'Roll complete'
MainV2.config["mjpeg_url"] = url;
+ Utilities.CaptureMJPEG.Stop();
+
Utilities.CaptureMJPEG.URL = url;
Utilities.CaptureMJPEG.OnNewImage += new EventHandler(CaptureMJPEG_OnNewImage);
diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.Designer.cs b/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.Designer.cs
index e4109f4283..ac33d0a964 100644
--- a/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.Designer.cs
+++ b/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.Designer.cs
@@ -122,6 +122,7 @@
this.autoWPToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem();
this.createWpCircleToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem();
this.gridToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem();
+ this.gridV2ToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem();
this.mapToolToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem();
this.ContextMeasure = new System.Windows.Forms.ToolStripMenuItem();
this.rotateMapToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem();
@@ -591,7 +592,6 @@
this.comboBoxMapType.FormattingEnabled = true;
this.comboBoxMapType.Name = "comboBoxMapType";
this.toolTip1.SetToolTip(this.comboBoxMapType, resources.GetString("comboBoxMapType.ToolTip"));
- this.comboBoxMapType.SelectedIndexChanged += new System.EventHandler(this.comboBoxMapType_SelectedValueChanged);
//
// panelMap
//
@@ -836,7 +836,8 @@
//
this.autoWPToolStripMenuItem.DropDownItems.AddRange(new System.Windows.Forms.ToolStripItem[] {
this.createWpCircleToolStripMenuItem,
- this.gridToolStripMenuItem});
+ this.gridToolStripMenuItem,
+ this.gridV2ToolStripMenuItem});
this.autoWPToolStripMenuItem.Name = "autoWPToolStripMenuItem";
resources.ApplyResources(this.autoWPToolStripMenuItem, "autoWPToolStripMenuItem");
//
@@ -852,6 +853,12 @@
resources.ApplyResources(this.gridToolStripMenuItem, "gridToolStripMenuItem");
this.gridToolStripMenuItem.Click += new System.EventHandler(this.gridToolStripMenuItem_Click);
//
+ // gridV2ToolStripMenuItem
+ //
+ this.gridV2ToolStripMenuItem.Name = "gridV2ToolStripMenuItem";
+ resources.ApplyResources(this.gridV2ToolStripMenuItem, "gridV2ToolStripMenuItem");
+ this.gridV2ToolStripMenuItem.Click += new System.EventHandler(this.gridV2ToolStripMenuItem_Click);
+ //
// mapToolToolStripMenuItem
//
this.mapToolToolStripMenuItem.DropDownItems.AddRange(new System.Windows.Forms.ToolStripItem[] {
@@ -1105,5 +1112,6 @@
private System.Windows.Forms.ToolStripMenuItem saveWPFileToolStripMenuItem;
private System.Windows.Forms.ToolStripMenuItem trackerHomeToolStripMenuItem;
private System.Windows.Forms.ToolStripMenuItem flyToHereToolStripMenuItem;
+ private System.Windows.Forms.ToolStripMenuItem gridV2ToolStripMenuItem;
}
}
\ No newline at end of file
diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs b/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs
index 8963af6487..e7774107da 100644
--- a/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs
+++ b/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs
@@ -343,9 +343,8 @@ namespace ArdupilotMega.GCSViews
}
// creating a WP
- Commands.Rows.Add();
- selectedrow = Commands.RowCount - 1;
- Commands.CurrentCell = Commands.Rows[selectedrow].Cells[Param1.Index];
+ selectedrow = Commands.Rows.Add();
+ // Commands.CurrentCell = Commands.Rows[selectedrow].Cells[Param1.Index];
setfromGE(lat, lng, alt);
}
@@ -568,14 +567,14 @@ namespace ArdupilotMega.GCSViews
if (Commands.RowCount <= 1)
{
- Commands.Rows.Add();
+ selectedrow = Commands.Rows.Add();
}
else
{
if (Commands.RowCount == selectedrow + 1)
{
DataGridViewRow temp = Commands.Rows[selectedrow];
- Commands.Rows.Add();
+ selectedrow = Commands.Rows.Add();
}
else
{
@@ -1425,7 +1424,7 @@ namespace ArdupilotMega.GCSViews
break;
if (i + 1 >= Commands.Rows.Count)
{
- Commands.Rows.Add();
+ selectedrow = Commands.Rows.Add();
}
//if (i == 0 && temp.alt == 0) // skip 0 home
// continue;
@@ -2236,7 +2235,13 @@ namespace ArdupilotMega.GCSViews
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)
@@ -2554,9 +2559,12 @@ namespace ArdupilotMega.GCSViews
private void clearMissionToolStripMenuItem_Click(object sender, EventArgs e)
{
+ quickadd = true;
+
while (Commands.Rows.Count > 0)
Commands.Rows.RemoveAt(0);
selectedrow = 0;
+ quickadd = false;
writeKML();
}
@@ -2578,13 +2586,13 @@ namespace ArdupilotMega.GCSViews
string repeat = "5";
Common.InputBox("Jump repeat", "Number of times to Repeat", ref repeat);
- int row = Commands.Rows.Add();
+ selectedrow = Commands.Rows.Add();
- Commands.Rows[row].Cells[Command.Index].Value = MAVLink.MAV_CMD.DO_JUMP.ToString();
+ Commands.Rows[selectedrow].Cells[Command.Index].Value = MAVLink.MAV_CMD.DO_JUMP.ToString();
- Commands.Rows[row].Cells[Param1.Index].Value = 1;
+ Commands.Rows[selectedrow].Cells[Param1.Index].Value = 1;
- Commands.Rows[row].Cells[Param2.Index].Value = repeat;
+ Commands.Rows[selectedrow].Cells[Param2.Index].Value = repeat;
writeKML();
}
@@ -2596,13 +2604,13 @@ namespace ArdupilotMega.GCSViews
string repeat = "5";
Common.InputBox("Jump repeat", "Number of times to Repeat", ref repeat);
- int row = Commands.Rows.Add();
+ selectedrow = Commands.Rows.Add();
- Commands.Rows[row].Cells[Command.Index].Value = MAVLink.MAV_CMD.DO_JUMP.ToString();
+ Commands.Rows[selectedrow].Cells[Command.Index].Value = MAVLink.MAV_CMD.DO_JUMP.ToString();
- Commands.Rows[row].Cells[Param1.Index].Value = wp;
+ Commands.Rows[selectedrow].Cells[Param1.Index].Value = wp;
- Commands.Rows[row].Cells[Param2.Index].Value = repeat;
+ Commands.Rows[selectedrow].Cells[Param2.Index].Value = repeat;
writeKML();
}
@@ -3184,6 +3192,382 @@ namespace ArdupilotMega.GCSViews
writeKML();
}
+ public struct linelatlng
+ {
+ public PointLatLng p1;
+ public PointLatLng p2;
+ // used as a base for grid along line
+ public PointLatLng basepnt;
+ }
+
+ private void gridv2()
+ {
+ 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;
+ }
+
+ // ensure points/latlong are current
+ MainMap.Zoom = (int)MainMap.Zoom;
+
+ MainMap.Refresh();
+
+ GMapPolygon area = drawnpolygon;
+ if (area.Points[0] != area.Points[area.Points.Count - 1])
+ 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 wpevery = (40 * MainV2.cs.multiplierdist).ToString("0");
+ Common.InputBox("Every", "Put a WP every x distance (-1 for none)", ref wpevery);
+
+ string angle = (90).ToString("0");
+ Common.InputBox("Angle", "Enter the line direction (0-90)", 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 (!double.TryParse(wpevery, out tryme))
+ {
+ CustomMessageBox.Show("Invalid Waypoint spacing");
+ return;
+ }
+
+ // get x y components
+ double y1 = Math.Cos((double.Parse(angle)) * deg2rad); // needs to mod for long scale
+ double x1 = Math.Sin((double.Parse(angle)) * deg2rad);
+
+ // get x y step amount in lat lng from m
+ double latdiff = arearect.HeightLat / ((heightdist / (double.Parse(distance) * (x1) / MainV2.cs.multiplierdist)));
+ double lngdiff = arearect.WidthLng / ((widthdist / (double.Parse(distance) * (y1) / MainV2.cs.multiplierdist)));
+
+ double latlngdiff = Math.Sqrt(latdiff * latdiff + lngdiff * lngdiff);
+
+ double latlngdiff2 = Math.Sqrt(arearect.HeightLat * arearect.HeightLat + arearect.WidthLng * arearect.WidthLng);
+
+ double fulllatdiff = arearect.HeightLat * x1 * 2;
+ double fulllngdiff = arearect.WidthLng * y1 * 2;
+
+ int altitude = (int)(double.Parse(alt) / MainV2.cs.multiplierdist);
+
+ // draw a grid
+ double x = bottomleft.Lng - 0.00001;
+ double y = bottomleft.Lat;
+
+ newpos(ref y, ref x, double.Parse(angle) - 90, diagdist);
+
+
+ List grid = new List();
+
+ int lines = 0;
+
+ y1 = Math.Cos((double.Parse(angle) + 90) * deg2rad); // needs to mod for long scale
+ x1 = Math.Sin((double.Parse(angle) + 90) * deg2rad);
+
+ // get x y step amount in lat lng from m
+ latdiff = arearect.HeightLat / ((heightdist / (double.Parse(distance) * (y1) / MainV2.cs.multiplierdist)));
+ lngdiff = arearect.WidthLng / ((widthdist / (double.Parse(distance) * (x1) / MainV2.cs.multiplierdist)));
+
+ quickadd = true;
+
+ while (lines * double.Parse(distance) < diagdist * 3) //x < topright.Lat && y < topright.Lng)
+ {
+ //callMe(y, x, 0);
+ double nx = x;
+ double ny = y;
+ newpos(ref ny, ref nx, double.Parse(angle), diagdist);
+
+ //callMe(ny, nx, 0);
+
+ linelatlng line = new linelatlng();
+ line.p1 = new PointLatLng(y, x);
+ line.p2 = new PointLatLng(ny, nx);
+ line.basepnt = new PointLatLng(y, x);
+ grid.Add(line);
+
+ x += lngdiff;
+ y += latdiff;
+ lines++;
+ }
+
+ // callMe(x, y, 0);
+
+ quickadd = false;
+
+ // return;
+
+ // find intersections
+ List remove = new List();
+
+ int gridno = grid.Count;
+
+ for (int a = 0; a < gridno; a++)
+ {
+ double noc = double.MaxValue;
+ double nof = double.MinValue;
+
+ PointLatLng closestlatlong = PointLatLng.Zero;
+ PointLatLng farestlatlong = PointLatLng.Zero;
+
+ List matchs = new List();
+
+ int b = -1;
+ int crosses = 0;
+ PointLatLng newlatlong = PointLatLng.Zero;
+ foreach (PointLatLng pnt in area.Points)
+ {
+ b++;
+ if (b == 0)
+ {
+ continue;
+ }
+ newlatlong = FindLineIntersection(area.Points[b - 1], area.Points[b], grid[a].p1, grid[a].p2);
+ if (!newlatlong.IsZero)
+ {
+ crosses++;
+ matchs.Add(newlatlong);
+ if (noc > MainMap.Manager.GetDistance(grid[a].p1, newlatlong))
+ {
+ closestlatlong.Lat = newlatlong.Lat;
+ closestlatlong.Lng = newlatlong.Lng;
+ noc = MainMap.Manager.GetDistance(grid[a].p1, newlatlong);
+ }
+ if (nof < MainMap.Manager.GetDistance(grid[a].p1, newlatlong))
+ {
+ farestlatlong.Lat = newlatlong.Lat;
+ farestlatlong.Lng = newlatlong.Lng;
+ nof = MainMap.Manager.GetDistance(grid[a].p1, newlatlong);
+ }
+ }
+ }
+ if (crosses == 0)
+ {
+ if (!PointInPolygon(grid[a].p1, area.Points) && !PointInPolygon(grid[a].p2, area.Points))
+ remove.Add(grid[a]);
+ }
+ else if (crosses == 1)
+ {
+
+ }
+ else if (crosses == 2)
+ {
+ linelatlng line = grid[a];
+ line.p1 = closestlatlong;
+ line.p2 = farestlatlong;
+ grid[a] = line;
+ }/*
+ else if (crosses == 4)
+ {
+ linelatlng line = grid[a];
+ line.p1 = closestlatlong;
+
+ PointLatLng far = farestlatlong;
+
+ matchs.Remove(closestlatlong);
+ matchs.Remove(farestlatlong);
+
+ farestlatlong = findClosestPoint(line.p1, matchs);
+
+ matchs.Remove(farestlatlong);
+
+ line.p2 = farestlatlong;
+ grid[a] = line;
+
+ grid.Add(new linelatlng() { p1 = matchs[0], p2 = far, basepnt = line.basepnt });
+
+ }*/
+ else
+ {
+ linelatlng line = grid[a];
+ remove.Add(line);
+ /*
+ // set new start point
+ line.p1 = findClosestPoint(line.basepnt, matchs); ;
+ matchs.Remove(line.p1);
+
+ line.p2 = findClosestPoint(line.basepnt, matchs);
+ matchs.Remove(line.p2);
+
+ grid[a] = line;
+
+ callMe(line.basepnt.Lat, line.basepnt.Lng, altitude);
+ callMe(line.p1.Lat, line.p1.Lng, altitude);
+ callMe(line.p2.Lat, line.p2.Lng, altitude);
+
+ continue;
+ */
+
+ while (matchs.Count > 1)
+ {
+ linelatlng newline = new linelatlng();
+
+ closestlatlong = findClosestPoint(closestlatlong, matchs);
+ newline.p1 = closestlatlong;
+ matchs.Remove(closestlatlong);
+
+ closestlatlong = findClosestPoint(closestlatlong, matchs);
+ newline.p2 = closestlatlong;
+ matchs.Remove(closestlatlong);
+
+ newline.basepnt = line.basepnt;
+
+ grid.Add(newline);
+ }
+ if (a > 150)
+ break;
+ }
+ }
+
+ // return;
+
+ foreach (linelatlng line in remove)
+ {
+ grid.Remove(line);
+ }
+
+ quickadd = true;
+
+ linelatlng closest = findClosestLine(MainV2.cs.HomeLocation.Point(),grid);
+
+ PointLatLng lastpnt = closest.p1;
+
+ while (grid.Count > 0)
+ {
+ if (MainMap.Manager.GetDistance(closest.p1, lastpnt) < MainMap.Manager.GetDistance(closest.p2, lastpnt))
+ {
+ callMe(closest.p1.Lat, closest.p1.Lng, altitude);
+
+ if (double.Parse(wpevery) > 0)
+ {
+ for (int d = (int)(double.Parse(wpevery) - ((MainMap.Manager.GetDistance(closest.basepnt, closest.p1) * 1000) % double.Parse(wpevery))); d < (MainMap.Manager.GetDistance(closest.p1, closest.p2) * 1000); d += (int)double.Parse(wpevery))
+ {
+ double ax = closest.p1.Lat;
+ double ay = closest.p1.Lng;
+
+ newpos(ref ax, ref ay, double.Parse(angle), d);
+ callMe(ax, ay, altitude);
+ }
+ }
+
+ callMe(closest.p2.Lat, closest.p2.Lng, altitude);
+
+ lastpnt = closest.p2;
+
+ grid.Remove(closest);
+ if (grid.Count == 0)
+ break;
+ closest = findClosestLine(closest.p2, grid);
+ }
+ else
+ {
+ callMe(closest.p2.Lat, closest.p2.Lng, altitude);
+
+ if (double.Parse(wpevery) > 0)
+ {
+ for (int d = (int)((MainMap.Manager.GetDistance(closest.basepnt, closest.p2) * 1000) % double.Parse(wpevery)); d < (MainMap.Manager.GetDistance(closest.p1, closest.p2) * 1000); d += (int)double.Parse(wpevery))
+ {
+ double ax = closest.p2.Lat;
+ double ay = closest.p2.Lng;
+
+ newpos(ref ax, ref ay, double.Parse(angle), -d);
+ callMe(ax, ay, altitude);
+ }
+ }
+
+ callMe(closest.p1.Lat, closest.p1.Lng, altitude);
+
+ lastpnt = closest.p1;
+
+ grid.Remove(closest);
+ if (grid.Count == 0)
+ break;
+ closest = findClosestLine(closest.p1, grid);
+ }
+ }
+
+ foreach (linelatlng line in grid)
+ {
+ // callMe(line.p1.Lat, line.p1.Lng, 0);
+ // callMe(line.p2.Lat, line.p2.Lng, 0);
+ }
+
+ quickadd = false;
+
+ writeKML();
+ }
+ }
+
+ PointLatLng findClosestPoint(PointLatLng start, List list)
+ {
+ PointLatLng answer = PointLatLng.Zero;
+ double currentbest = double.MaxValue;
+
+ foreach (PointLatLng pnt in list)
+ {
+ double dist1 = MainMap.Manager.GetDistance(start, pnt);
+
+ if (dist1 < currentbest)
+ {
+ answer = pnt;
+ currentbest = dist1;
+ }
+ }
+
+ return answer;
+ }
+
+ linelatlng findClosestLine(PointLatLng start, List list)
+ {
+ linelatlng answer = list[0];
+ double shortest = double.MaxValue;
+
+ foreach (linelatlng line in list)
+ {
+ double ans1 = MainMap.Manager.GetDistance(start, line.p1);
+ double ans2 = MainMap.Manager.GetDistance(start, line.p2);
+ PointLatLng shorterpnt = ans1 < ans2 ? line.p1 : line.p2;
+
+ if (shortest > MainMap.Manager.GetDistance(start, shorterpnt))
+ {
+ answer = line;
+ shortest = MainMap.Manager.GetDistance(start, shorterpnt);
+ }
+ }
+
+ return answer;
+ }
+
private void gridToolStripMenuItem_Click(object sender, EventArgs e)
{
polygongridmode = false;
@@ -3290,6 +3674,8 @@ namespace ArdupilotMega.GCSViews
log.InfoFormat("{0} < {1} {2} < {3}", x, (topright.Lat), y, (topright.Lng));
+ quickadd = true;
+
while (x < (topright.Lat + Math.Abs(latlngdiff)) && y < (topright.Lng + Math.Abs(latlngdiff)))
{
if (double.Parse(angle) < 45)
@@ -3450,11 +3836,55 @@ namespace ArdupilotMega.GCSViews
//drawnpolygon.Points.Clear();
//drawnpolygons.Markers.Clear();
+ quickadd = false;
+ writeKML();
MainMap.Refresh();
}
+
}
+ bool PointInPolygon(PointLatLng p, List poly)
+ {
+ PointLatLng p1, p2;
+ bool inside = false;
+
+ if (poly.Count < 3)
+ {
+ return inside;
+ }
+ PointLatLng oldPoint = new PointLatLng(
+
+ poly[poly.Count - 1].Lat, poly[poly.Count - 1].Lng);
+
+ for (int i = 0; i < poly.Count; i++)
+ {
+
+ PointLatLng newPoint = new PointLatLng(poly[i].Lat, poly[i].Lng);
+
+ if (newPoint.Lat > oldPoint.Lat)
+ {
+ p1 = oldPoint;
+ p2 = newPoint;
+ }
+ else
+ {
+ p1 = newPoint;
+ p2 = oldPoint;
+ }
+
+ if ((newPoint.Lat < p.Lat) == (p.Lat <= oldPoint.Lat)
+ && ((double)p.Lng - (double)p1.Lng) * (double)(p2.Lat - p1.Lat)
+ < ((double)p2.Lng - (double)p1.Lng) * (double)(p.Lat - p1.Lat))
+ {
+ inside = !inside;
+ }
+ oldPoint = newPoint;
+ }
+ return inside;
+ }
+
+
void newpos(ref double lat, ref double lon, double bearing, double distance)
{
// '''extrapolate latitude/longitude given a heading and distance
@@ -3692,5 +4122,10 @@ namespace ArdupilotMega.GCSViews
{
MainV2.cs.TrackerLocation = new PointLatLngAlt(end) { Alt = MainV2.cs.HomeAlt };
}
+
+ private void gridV2ToolStripMenuItem_Click(object sender, EventArgs e)
+ {
+ gridv2();
+ }
}
}
\ No newline at end of file
diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.resx b/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.resx
index d3be25c6fd..f9862a4eed 100644
--- a/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.resx
+++ b/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.resx
@@ -556,7 +556,7 @@
BUT_write
- ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner10, Version=1.1.4613.14163, Culture=neutral, PublicKeyToken=null
+ ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner10, Version=1.1.4615.38724, Culture=neutral, PublicKeyToken=null
panel5
@@ -583,7 +583,7 @@
BUT_read
- ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner10, Version=1.1.4613.14163, Culture=neutral, PublicKeyToken=null
+ ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner10, Version=1.1.4615.38724, Culture=neutral, PublicKeyToken=null
panel5
@@ -1147,7 +1147,7 @@
BUT_Add
- ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner10, Version=1.1.4613.14163, Culture=neutral, PublicKeyToken=null
+ ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner10, Version=1.1.4615.38724, Culture=neutral, PublicKeyToken=null
panelWaypoints
@@ -1488,6 +1488,12 @@
Grid
+
+ 162, 22
+
+
+ GridV2
+
152, 22
@@ -1742,7 +1748,7 @@
MainMap
- ArdupilotMega.Controls.myGMAP, ArdupilotMegaPlanner10, Version=1.1.4613.14163, Culture=neutral, PublicKeyToken=null
+ ArdupilotMega.Controls.myGMAP, ArdupilotMegaPlanner10, Version=1.1.4615.38724, Culture=neutral, PublicKeyToken=null
panelMap
@@ -1772,7 +1778,7 @@
trackBar1
- ArdupilotMega.Controls.MyTrackBar, ArdupilotMegaPlanner10, Version=1.1.4613.14163, Culture=neutral, PublicKeyToken=null
+ ArdupilotMega.Controls.MyTrackBar, ArdupilotMegaPlanner10, Version=1.1.4615.38724, Culture=neutral, PublicKeyToken=null
panelMap
@@ -2125,6 +2131,12 @@
System.Windows.Forms.ToolStripMenuItem, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089
+
+ gridV2ToolStripMenuItem
+
+
+ System.Windows.Forms.ToolStripMenuItem, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089
+
mapToolToolStripMenuItem
@@ -2219,6 +2231,6 @@
FlightPlanner
- System.Windows.Forms.MyUserControl, ArdupilotMegaPlanner10, Version=1.1.4613.14163, Culture=neutral, PublicKeyToken=null
+ System.Windows.Forms.MyUserControl, ArdupilotMegaPlanner10, Version=1.1.4615.38724, Culture=neutral, PublicKeyToken=null
\ No newline at end of file
diff --git a/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs b/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs
index 71af2c4a50..12c80e2c00 100644
--- a/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs
+++ b/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs
@@ -34,5 +34,5 @@ using System.Resources;
// by using the '*' as shown below:
// [assembly: AssemblyVersion("1.0.*")]
[assembly: AssemblyVersion("1.1.*")]
-[assembly: AssemblyFileVersion("1.2.7")]
+[assembly: AssemblyFileVersion("1.2.8")]
[assembly: NeutralResourcesLanguageAttribute("")]
diff --git a/Tools/ArdupilotMegaPlanner/Utilities/CaptureMJPEG.cs b/Tools/ArdupilotMegaPlanner/Utilities/CaptureMJPEG.cs
index c2e6d88950..915a4c9502 100644
--- a/Tools/ArdupilotMegaPlanner/Utilities/CaptureMJPEG.cs
+++ b/Tools/ArdupilotMegaPlanner/Utilities/CaptureMJPEG.cs
@@ -26,7 +26,7 @@ namespace ArdupilotMega.Utilities
public static void runAsync()
{
- while (running && asyncthread != null && asyncthread.IsAlive)
+ while (asyncthread != null && asyncthread.IsAlive)
{
running = false;
}
@@ -46,6 +46,22 @@ namespace ArdupilotMega.Utilities
running = false;
}
+ public static string ReadLine(BinaryReader br)
+ {
+ StringBuilder sb = new StringBuilder();
+
+ while (true) {
+ byte by = br.ReadByte();
+ sb.Append((char)by);
+ if (by == '\n')
+ break;
+ }
+
+ sb = sb.Replace("\r\n", "");
+
+ return sb.ToString();
+ }
+
static void getUrl()
{
@@ -69,21 +85,37 @@ namespace ArdupilotMega.Utilities
// Get the stream containing content returned by the server.
Stream dataStream = response.GetResponseStream();
- StreamReader sr = new StreamReader(dataStream);
+ BinaryReader br = new BinaryReader(dataStream);
// get boundary header
+
string mpheader = response.Headers["Content-Type"];
- int startboundary = mpheader.IndexOf("boundary=") + 9;
- int endboundary = mpheader.Length;
+ if (mpheader.IndexOf("boundary=") == -1) {
+ ReadLine(br); // this is a blank line
+ string line = "proxyline";
+ while (line.Length > 2)
+ {
+ line = ReadLine(br);
+ if (line.StartsWith("--")) {
+ mpheader = line;
+ break;
+ }
+ }
+ }
+ else
+ {
+ int startboundary = mpheader.IndexOf("boundary=") + 9;
+ int endboundary = mpheader.Length;
- mpheader = mpheader.Substring(startboundary, endboundary - startboundary);
+ mpheader = mpheader.Substring(startboundary, endboundary - startboundary);
+ }
while (running)
{
try
{
// get the multipart start header
- int length = int.Parse(getHeader(sr)["Content-Length"]);
+ int length = int.Parse(getHeader(br)["Content-Length"]);
// read boundary header
if (length > 0)
@@ -95,13 +127,26 @@ namespace ArdupilotMega.Utilities
int offset = 0;
int len = 0;
- while ((len = dataStream.Read(buf1, offset, length)) > 0)
+ while ((len = br.Read(buf1, offset, length)) > 0)
{
offset += len;
length -= len;
+
}
+ /*
+ BinaryWriter sw = new BinaryWriter(File.OpenWrite("test.jpg"));
- frame = new Bitmap(new MemoryStream(buf1));
+ sw.Write(buf1,0,buf1.Length);
+
+ sw.Close();
+ */
+ try
+ {
+
+ frame = new Bitmap(new MemoryStream(buf1));
+
+ }
+ catch { }
fps++;
@@ -117,7 +162,7 @@ namespace ArdupilotMega.Utilities
}
// blank line at end of data
- sr.ReadLine();
+ ReadLine(br);
}
catch (Exception ex) { log.Info(ex); break; }
}
@@ -138,7 +183,7 @@ namespace ArdupilotMega.Utilities
}
- static Dictionary getHeader(StreamReader stream)
+ static Dictionary getHeader(BinaryReader stream)
{
Dictionary answer = new Dictionary();
@@ -146,7 +191,7 @@ namespace ArdupilotMega.Utilities
do
{
- line = stream.ReadLine();
+ line = ReadLine(stream);
string[] items = line.Split(new char[] { ':' }, StringSplitOptions.RemoveEmptyEntries);