private void writeKML(string filename)
{
try
{
writeGPX(filename);
}
catch { }
Color[] colours = { Color.Red, Color.Orange, Color.Yellow, Color.Green, Color.Blue, Color.Indigo, Color.Violet, Color.Pink };
AltitudeMode altmode = AltitudeMode.absolute;
if (MainV2.cs.firmware == MainV2.Firmwares.ArduCopter2)
{
altmode = AltitudeMode.relativeToGround; // because of sonar, this is both right and wrong. right for sonar, wrong in terms of gps as the land slopes off.
}
KMLRoot kml = new KMLRoot();
Folder fldr = new Folder("Log");
Style style = new Style();
style.Id = "yellowLineGreenPoly";
style.Add(new LineStyle(HexStringToColor("7f00ffff"), 4));
PolyStyle pstyle = new PolyStyle();
pstyle.Color = HexStringToColor("7f00ff00");
style.Add(pstyle);
kml.Document.AddStyle(style);
int stylecode = 0xff;
int g = -1;
foreach (List<Point3D> poslist in position)
{
g++;
if (poslist == null)
continue;
LineString ls = new LineString();
ls.AltitudeMode = altmode;
ls.Extrude = true;
//ls.Tessellate = true;
Coordinates coords = new Coordinates();
coords.AddRange(poslist);
ls.coordinates = coords;
Placemark pm = new Placemark();
string mode = "";
if (g < modelist.Count)
mode = modelist[g];
pm.name = g + " Flight Path " + mode;
pm.styleUrl = "#yellowLineGreenPoly";
pm.LineString = ls;
stylecode = colours[g % (colours.Length - 1)].ToArgb();
Style style2 = new Style();
Color color = Color.FromArgb(0xff, (stylecode >> 16) & 0xff, (stylecode >> 8) & 0xff, (stylecode >> 0) & 0xff);
log.Info("colour " + color.ToArgb().ToString("X") + " " + color.ToKnownColor().ToString());
style2.Add(new LineStyle(color, 4));
pm.AddStyle(style2);
fldr.Add(pm);
}
Folder planes = new Folder();
planes.name = "Planes";
fldr.Add(planes);
Folder waypoints = new Folder();
waypoints.name = "Waypoints";
fldr.Add(waypoints);
LineString lswp = new LineString();
lswp.AltitudeMode = AltitudeMode.relativeToGround;
lswp.Extrude = true;
Coordinates coordswp = new Coordinates();
foreach (PointLatLngAlt p1 in cmd)
{
coordswp.Add(new Point3D(p1.Lng, p1.Lat, p1.Alt));
}
lswp.coordinates = coordswp;
Placemark pmwp = new Placemark();
pmwp.name = "Waypoints";
//pm.styleUrl = "#yellowLineGreenPoly";
pmwp.LineString = lswp;
waypoints.Add(pmwp);
int a = 0;
int l = -1;
Model lastmodel = null;
foreach (Data mod in flightdata)
{
l++;
if (mod.model.Location.latitude == 0)
continue;
if (lastmodel != null)
{
if (lastmodel.Location.Equals(mod.model.Location))
{
continue;
}
}
Placemark pmplane = new Placemark();
pmplane.name = "Plane " + a;
pmplane.visibility = false;
Model model = mod.model;
model.AltitudeMode = altmode;
model.Scale.x = 2;
model.Scale.y = 2;
model.Scale.z = 2;
try
{
pmplane.description = @"<![CDATA[
<table>
<tr><td>Roll: " + model.Orientation.roll + @" </td></tr>
<tr><td>Pitch: " + model.Orientation.tilt + @" </td></tr>
<tr><td>Yaw: " + model.Orientation.heading + @" </td></tr>
<tr><td>WP dist " + mod.ntun[2] + @" </td></tr>
<tr><td>tar bear " + mod.ntun[3] + @" </td></tr>
<tr><td>nav bear " + mod.ntun[4] + @" </td></tr>
<tr><td>alt error " + mod.ntun[5] + @" </td></tr>
</table>
]]>";
}
catch { }
try
{
pmplane.Point = new KmlPoint((float)model.Location.longitude, (float)model.Location.latitude, (float)model.Location.altitude);
pmplane.Point.AltitudeMode = altmode;
Link link = new Link();
link.href = "block_plane_0.dae";
model.Link = link;
pmplane.Model = model;
planes.Add(pmplane);
}
catch { } // bad lat long value
lastmodel = mod.model;
a++;
}
kml.Document.Add(fldr);
kml.Save(filename);
// create kmz - aka zip file
FileStream fs = File.Open(filename.Replace(".log.kml", ".kmz"), FileMode.Create);
ZipOutputStream zipStream = new ZipOutputStream(fs);
zipStream.SetLevel(9); //0-9, 9 being the highest level of compression
zipStream.UseZip64 = UseZip64.Off; // older zipfile
// entry 1
string entryName = ZipEntry.CleanName(Path.GetFileName(filename)); // Removes drive from name and fixes slash direction
ZipEntry newEntry = new ZipEntry(entryName);
newEntry.DateTime = DateTime.Now;
zipStream.PutNextEntry(newEntry);
// Zip the file in buffered chunks
// the "using" will close the stream even if an exception occurs
byte[] buffer = new byte[4096];
using (FileStream streamReader = File.OpenRead(filename))
{
StreamUtils.Copy(streamReader, zipStream, buffer);
}
zipStream.CloseEntry();
File.Delete(filename);
filename = Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + "block_plane_0.dae";
// entry 2
entryName = ZipEntry.CleanName(Path.GetFileName(filename)); // Removes drive from name and fixes slash direction
newEntry = new ZipEntry(entryName);
newEntry.DateTime = DateTime.Now;
zipStream.PutNextEntry(newEntry);
// Zip the file in buffered chunks
// the "using" will close the stream even if an exception occurs
buffer = new byte[4096];
using (FileStream streamReader = File.OpenRead(filename))
{
StreamUtils.Copy(streamReader, zipStream, buffer);
}
zipStream.CloseEntry();
zipStream.IsStreamOwner = true; // Makes the Close also Close the underlying stream
zipStream.Close();
positionindex = 0;
modelist.Clear();
flightdata.Clear();
position = new List<Core.Geometry.Point3D>[200];
cmd.Clear();
}
public void PlatformBehaviorToKML()
{
var jaxOpsArea = new GeoArray(new Geo(29.3590, -79.2195),
new Geo(31.1627, -79.2195),
new Geo(31.1627, -81.2789),
new Geo(30.1627, -81.2789),
new Geo(29.3590, -80.8789),
new Geo(29.3590, -79.2195));
var platform = new Platform
{
PlatformName = "Test Platform",
Perimeter = jaxOpsArea,
Depth = 0,
IsRandom = true,
TrackType = TrackType.PerimeterBounce,
Sources = new ObservableList<Source>(),
Speed = 20,
};
var source = new Source
{
SourceName = "Test Source",
Modes = new ObservableList<Mode>(),
Platform = platform,
};
platform.Sources.Add(source);
var mode = new Mode
{
ModeName = "Test Mode",
PulseInterval = new TimeSpan(0, 0, 0, 10),
PulseLength = new TimeSpan(0, 0, 0, 0, 500),
Depth = 5,
HighFrequency = 1000,
LowFrequency = 1000,
DepressionElevationAngle = 10,
VerticalBeamWidth = 90,
SourceLevel = 200,
Source = source,
};
source.Modes.Add(mode);
var behavior = new PlatformBehavior(platform, new TimeSpan(0, 0, 0, 1), 86400);
#if true
var kml = new KMLRoot();
var folder = new Folder("Jacksonville");
jaxOpsArea.Placemark.name = "Jacksonville OpArea";
jaxOpsArea.Placemark.Snippet = "The operational area";
jaxOpsArea.Placemark.Snippet.maxLines = 1;
folder.Add(jaxOpsArea.Placemark);
#if true
var timeStep = 0;
foreach (var state in behavior.PlatformStates)
{
if (timeStep % 100 == 0)
{
state.PlatformLocation.Location.Placemark.name = string.Format("TimeStep {0}", timeStep);
folder.Add(state.PlatformLocation.Location.Placemark);
}
timeStep++;
}
#else
result.Placemark.name = "Platform track";
result.Placemark.Snippet = "The track of the platform";
result.Placemark.Snippet.maxLines = 1;
result.Placemark.Geometry.AltitudeMode = AltitudeMode.clampedToGround;
folder.Add(result.Placemark);
#endif
kml.Document.Add(folder);
var savePath = Path.Combine(System.Environment.GetFolderPath(System.Environment.SpecialFolder.MyDocuments), "Platform Behavior Tests", "PlatformBehavior.kml");
Debug.WriteLine("Saving KML...");
kml.Save(savePath);
#endif
}
public void writeKML(string filename)
{
try
{
writeGPX(filename);
writeRinex(filename);
}
catch
{
}
Color[] colours =
{
Color.Red, Color.Orange, Color.Yellow, Color.Green, Color.Blue, Color.Indigo,
Color.Violet, Color.Pink
};
AltitudeMode altmode = AltitudeMode.absolute;
KMLRoot kml = new KMLRoot();
Folder fldr = new Folder("Log");
Style style = new Style();
style.Id = "yellowLineGreenPoly";
style.Add(new LineStyle(HexStringToColor("7f00ffff"), 4));
Style style1 = new Style();
style1.Id = "spray";
style1.Add(new LineStyle(HexStringToColor("4c0000ff"), 0));
style1.Add(new PolyStyle() {Color = HexStringToColor("4c0000ff")});
PolyStyle pstyle = new PolyStyle();
pstyle.Color = HexStringToColor("7f00ff00");
style.Add(pstyle);
kml.Document.AddStyle(style);
kml.Document.AddStyle(style1);
int stylecode = 0xff;
int g = -1;
foreach (List<Point3D> poslist in position)
{
g++;
if (poslist == null)
continue;
/*
List<PointLatLngAlt> pllalist = new List<PointLatLngAlt>();
foreach (var point in poslist)
{
pllalist.Add(new PointLatLngAlt(point.Y, point.X, point.Z, ""));
}
var ans = Utilities.LineOffset.GetPolygon(pllalist, 2);
while (ans.Count > 0)
{
var first = ans[0];
var second = ans[1];
var secondlast = ans[ans.Count - 2];
var last = ans[ans.Count-1];
ans.Remove(first);
ans.Remove(last);
var polycoords = new BoundaryIs();
polycoords.LinearRing = new LinearRing();
polycoords.LinearRing.Coordinates.Add(new Point3D(first.Lng, first.Lat, 1));
polycoords.LinearRing.Coordinates.Add(new Point3D(second.Lng, second.Lat, 1));
polycoords.LinearRing.Coordinates.Add(new Point3D(secondlast.Lng, secondlast.Lat, 1));
polycoords.LinearRing.Coordinates.Add(new Point3D(last.Lng, last.Lat, 1));
polycoords.LinearRing.Coordinates.Add(new Point3D(first.Lng, first.Lat, 1));
//if (!IsClockwise(polycoords.LinearRing.Coordinates))
// polycoords.LinearRing.Coordinates.Reverse();
Polygon kmlpoly = new Polygon() { AltitudeMode = AltitudeMode.relativeToGround, Extrude = false, OuterBoundaryIs = polycoords };
Placemark pmpoly = new Placemark();
pmpoly.Polygon = kmlpoly;
pmpoly.name = g + " test";
pmpoly.styleUrl = "#spray";
fldr.Add(pmpoly);
}
*/
LineString ls = new LineString();
ls.AltitudeMode = altmode;
ls.Extrude = true;
//ls.Tessellate = true;
Coordinates coords = new Coordinates();
coords.AddRange(poslist);
ls.coordinates = coords;
Placemark pm = new Placemark();
string mode = "";
if (g < modelist.Count)
mode = modelist[g];
pm.name = g + " Flight Path " + mode;
pm.styleUrl = "#yellowLineGreenPoly";
pm.LineString = ls;
stylecode = colours[g%(colours.Length - 1)].ToArgb();
Style style2 = new Style();
Color color = Color.FromArgb(0xff, (stylecode >> 16) & 0xff, (stylecode >> 8) & 0xff,
(stylecode >> 0) & 0xff);
log.Info("colour " + color.ToArgb().ToString("X") + " " + color.ToKnownColor().ToString());
style2.Add(new LineStyle(color, 4));
pm.AddStyle(style2);
fldr.Add(pm);
}
Placemark pmPOS = new Placemark();
pmPOS.name = "POS Message";
pmPOS.LineString = new LineString();
pmPOS.LineString.coordinates = new Coordinates();
Point3D lastPoint3D = new Point3D();
PointLatLngAlt lastplla = PointLatLngAlt.Zero;
foreach (var item in PosLatLngAlts)
{
var newpoint = new Point3D(item.Lng, item.Lat, item.Alt);
if (item.GetDistance(lastplla) < 0.1 &&
lastPoint3D.Z >= (newpoint.Z - 0.3) &&
lastPoint3D.Z <= (newpoint.Z + 0.3))
continue;
pmPOS.LineString.coordinates.Add(newpoint);
lastPoint3D = newpoint;
lastplla = item;
if (pmPOS.LineString.coordinates.Count > 20000)
{
//add current
pmPOS.AddStyle(style);
fldr.Add(pmPOS);
// create new
pmPOS = new Placemark();
pmPOS.name = "POS Message - extra";
pmPOS.LineString = new LineString();
pmPOS.LineString.coordinates = new Coordinates();
lastPoint3D = new Point3D();
lastplla = PointLatLngAlt.Zero;
}
}
pmPOS.AddStyle(style);
fldr.Add(pmPOS);
Folder planes = new Folder();
planes.name = "Planes";
fldr.Add(planes);
Folder waypoints = new Folder();
waypoints.name = "Waypoints";
fldr.Add(waypoints);
LineString lswp = new LineString();
lswp.AltitudeMode = AltitudeMode.relativeToGround;
lswp.Extrude = true;
Coordinates coordswp = new Coordinates();
foreach (PointLatLngAlt p1 in cmd)
{
if (p1.Lng == 0 && p1.Lat == 0)
continue;
coordswp.Add(new Point3D(p1.Lng, p1.Lat, p1.Alt));
}
lswp.coordinates = coordswp;
Placemark pmwp = new Placemark();
pmwp.name = "Waypoints";
//pm.styleUrl = "#yellowLineGreenPoly";
pmwp.LineString = lswp;
if (coordswp.Count > 0)
waypoints.Add(pmwp);
int a = 0;
int l = -1;
Model lastmodel = null;
foreach (Data mod in flightdata)
{
l++;
if (mod.model.Location.latitude == 0)
continue;
if (lastmodel != null)
{
if (lastmodel.Location.Equals(mod.model.Location))
{
continue;
}
}
Placemark pmplane = new Placemark();
pmplane.name = "Plane " + a;
pmplane.visibility = false;
Model model = mod.model;
model.AltitudeMode = altmode;
model.Scale.x = 2;
model.Scale.y = 2;
model.Scale.z = 2;
try
{
pmplane.description = @"<![CDATA[
<table>
<tr><td>Roll: " + model.Orientation.roll + @" </td></tr>
<tr><td>Pitch: " + model.Orientation.tilt + @" </td></tr>
<tr><td>Yaw: " + model.Orientation.heading + @" </td></tr>
<tr><td>WP dist " + mod.ntun[2] + @" </td></tr>
<tr><td>tar bear " + mod.ntun[3] + @" </td></tr>
<tr><td>nav bear " + mod.ntun[4] + @" </td></tr>
<tr><td>alt error " + mod.ntun[5] + @" </td></tr>
</table>
]]>";
}
catch
{
}
try
{
pmplane.Point = new KmlPoint((float) model.Location.longitude, (float) model.Location.latitude,
(float) model.Location.altitude);
pmplane.Point.AltitudeMode = altmode;
Link link = new Link();
link.href = "block_plane_0.dae";
model.Link = link;
pmplane.Model = model;
planes.Add(pmplane);
}
catch
{
} // bad lat long value
lastmodel = mod.model;
a++;
}
kml.Document.Add(fldr);
kml.Save(filename);
// create kmz - aka zip file
FileStream fs = File.Open(filename.ToLower().Replace(".log.kml", ".kmz").Replace(".bin.kml", ".kmz"),
FileMode.Create);
ZipOutputStream zipStream = new ZipOutputStream(fs);
zipStream.SetLevel(9); //0-9, 9 being the highest level of compression
zipStream.UseZip64 = UseZip64.Off; // older zipfile
// entry 1
string entryName = ZipEntry.CleanName(Path.GetFileName(filename));
// Removes drive from name and fixes slash direction
ZipEntry newEntry = new ZipEntry(entryName);
newEntry.DateTime = DateTime.Now;
zipStream.PutNextEntry(newEntry);
// Zip the file in buffered chunks
// the "using" will close the stream even if an exception occurs
byte[] buffer = new byte[4096];
using (FileStream streamReader = File.OpenRead(filename))
{
StreamUtils.Copy(streamReader, zipStream, buffer);
}
zipStream.CloseEntry();
File.Delete(filename);
filename = Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar +
"block_plane_0.dae";
// entry 2
entryName = ZipEntry.CleanName(Path.GetFileName(filename));
// Removes drive from name and fixes slash direction
newEntry = new ZipEntry(entryName);
newEntry.DateTime = DateTime.Now;
zipStream.PutNextEntry(newEntry);
// Zip the file in buffered chunks
// the "using" will close the stream even if an exception occurs
buffer = new byte[4096];
using (FileStream streamReader = File.OpenRead(filename))
{
StreamUtils.Copy(streamReader, zipStream, buffer);
}
zipStream.CloseEntry();
zipStream.IsStreamOwner = true; // Makes the Close also Close the underlying stream
zipStream.Close();
positionindex = 0;
modelist.Clear();
flightdata.Clear();
position = new List<Core.Geometry.Point3D>[200];
cmd.Clear();
}
public void PerimeterBounceToKML()
{
var jaxOpsArea = new GeoArray(
new Geo(29.3590, -79.2195),
new Geo(31.1627, -79.2195),
new Geo(31.1627, -81.2789),
new Geo(30.1627, -81.2789),
new Geo(29.3590, -80.8789),
new Geo(29.3590, -79.2195));
var kml = new KMLRoot();
var folder = new Folder("Jacksonville");
jaxOpsArea.Placemark.name = "Jacksonville OpArea";
jaxOpsArea.Placemark.Snippet = "The operational area";
jaxOpsArea.Placemark.Snippet.maxLines = 1;
folder.Add(jaxOpsArea.Placemark);
GeoArray result = null;
while (result == null)
{
try
{
result = jaxOpsArea.PerimeterBounce(null, double.NaN, 1e8);
}
catch (PerimeterBounceException ex)
{
Debug.WriteLine(ex.Message);
Debug.WriteLine("PerimeterBounce failed, retrying...");
}
}
var startLocation = result[0];
startLocation.Placemark.name = "Start location";
startLocation.Placemark.Snippet = "The start of the track";
startLocation.Placemark.Snippet.maxLines = 1;
startLocation.Placemark.AddStyle(new IconStyle(System.Drawing.Color.Green) { Icon = new Icon() { href = @"http://www.clker.com/cliparts/q/y/S/n/A/V/green-pin-th.png" } });
folder.Add(startLocation.Placemark);
result.Placemark.name = "Platform track";
result.Placemark.Snippet = "The track of the platform";
result.Placemark.Snippet.maxLines = 1;
result.Placemark.Geometry.AltitudeMode = AltitudeMode.clampedToGround;
folder.Add(result.Placemark);
#if false
var endPoint = result.Segments.Last()[1];
if (!jaxOpsArea.Contains(endPoint))
{
var goodBounceSegment = result.Segments.ToArray()[result.Segments.Count() - 2];
var goodBouncePoint = goodBounceSegment[0];
var goodBounce = goodBouncePoint.Placemark;
Debug.WriteLine("Test failed");
goodBounce.name = string.Format("Last Good Bounce {0:000}", result.Segments.Count() - 1);
Debug.WriteLine(goodBounce.name);
goodBounce.Snippet = string.Format("Lat: {0:0.#####} Lon: {1:0.#####}", goodBouncePoint.Latitude, goodBouncePoint.Longitude);
goodBounce.Snippet.maxLines = 1;
folder.Add(goodBounce);
goodBounceSegment.Placemark.name = "Last good bounce segment";
goodBounceSegment.Placemark.AddStyle(new LineStyle(System.Drawing.Color.Red, 5));
folder.Add(goodBounceSegment.Placemark);
var badBouncePoint = goodBounceSegment[1];
var badBounce = badBouncePoint.Placemark;
badBounce.name = string.Format("Bad Bounce {0:000}", result.Segments.Count());
badBounce.Snippet = string.Format("Lat: {0:0.#####} Lon: {1:0.#####}", badBouncePoint.Latitude, badBouncePoint.Longitude);
badBounce.Snippet.maxLines = 1;
folder.Add(badBounce);
}
else Debug.WriteLine("Test passed");
var segments = result.Segments.ToArray();
for (var segmentIndex = 0; segmentIndex < segments.Length; segmentIndex++)
{
var bounce = segments[segmentIndex][1].Placemark;
bounce.name = string.Format("Bounce {0:000}", segmentIndex + 1);
bounce.Snippet = string.Format("Lat: {0:0.#####} Lon: {1:0.#####}", segments[segmentIndex][1].Latitude, segments[segmentIndex][1].Longitude);
bounce.Snippet.maxLines = 1;
folder.Add(bounce);
}
#endif
var endLocation = result[result.Length - 1];
endLocation.Placemark.name = "End location";
endLocation.Placemark.Snippet = "The end of the track";
endLocation.Placemark.Snippet.maxLines = 1;
endLocation.Placemark.AddStyle(new IconStyle(System.Drawing.Color.Red) { Icon = new Icon { href = @"http://www.clker.com/cliparts/Z/x/U/0/B/3/red-pin-th.png" } });
folder.Add(endLocation.Placemark);
kml.Document.Add(folder);
var savePath = Path.Combine(Environment.GetFolderPath(Environment.SpecialFolder.MyDocuments), "Perimeter Bounce Tests", "PerimeterBounce.kml");
Debug.WriteLine("Saving KML...");
kml.Save(savePath);
}