You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
1011 lines
38 KiB
1011 lines
38 KiB
using System; |
|
using System.Collections.Generic; |
|
using System.ComponentModel; |
|
using System.Data; |
|
using System.Drawing; |
|
using System.Text; |
|
using System.Windows.Forms; |
|
using System.IO.Ports; |
|
using System.IO; |
|
using System.Text.RegularExpressions; |
|
using KMLib; |
|
using KMLib.Feature; |
|
using KMLib.Geometry; |
|
using Core.Geometry; |
|
using ICSharpCode.SharpZipLib.Zip; |
|
using ICSharpCode.SharpZipLib.Checksums; |
|
using ICSharpCode.SharpZipLib.Core; |
|
|
|
|
|
namespace ArdupilotMega |
|
{ |
|
public partial class Log : Form |
|
{ |
|
ICommsSerial comPort; |
|
int logcount = 0; |
|
serialstatus status = serialstatus.Connecting; |
|
byte[] buffer = new byte[4000]; |
|
StreamWriter sw; |
|
int currentlog = 0; |
|
bool threadrun = true; |
|
string logfile = ""; |
|
int receivedbytes = 0; |
|
List<Data> flightdata = new List<Data>(); |
|
//List<Model> orientation = new List<Model>(); |
|
Model runmodel = new Model(); |
|
Object thisLock = new Object(); |
|
DateTime start = DateTime.Now; |
|
|
|
public struct Data |
|
{ |
|
public Model model; |
|
public string[] ntun; |
|
public string[] ctun; |
|
public int datetime; |
|
} |
|
|
|
enum serialstatus |
|
{ |
|
Connecting, |
|
Createfile, |
|
Closefile, |
|
Reading, |
|
Waiting, |
|
Done |
|
} |
|
|
|
public Log() |
|
{ |
|
InitializeComponent(); |
|
} |
|
|
|
private void Log_Load(object sender, EventArgs e) |
|
{ |
|
status = serialstatus.Connecting; |
|
|
|
comPort = MainV2.comPort.BaseStream; |
|
|
|
//comPort.ReceivedBytesThreshold = 50; |
|
//comPort.ReadBufferSize = 1024 * 1024; |
|
try |
|
{ |
|
comPort.toggleDTR(); |
|
//comPort.Open(); |
|
} |
|
catch (Exception) |
|
{ |
|
MessageBox.Show("Error opening comport"); |
|
} |
|
|
|
System.Threading.Thread t11 = new System.Threading.Thread(delegate() |
|
{ |
|
DateTime start = DateTime.Now; |
|
|
|
threadrun = true; |
|
|
|
System.Threading.Thread.Sleep(2000); |
|
|
|
try |
|
{ |
|
comPort.Write("\n\n\n\n"); |
|
} |
|
catch { } |
|
|
|
while (threadrun) |
|
{ |
|
try |
|
{ |
|
updateDisplay(); |
|
|
|
System.Threading.Thread.Sleep(10); |
|
if (!comPort.IsOpen) |
|
break; |
|
while (comPort.BytesToRead >= 4) |
|
{ |
|
comPort_DataReceived((object)null, (SerialDataReceivedEventArgs)null); |
|
} |
|
} |
|
catch (Exception ex) { Console.WriteLine("crash in comport reader " + ex.ToString()); } // cant exit unless told to |
|
} |
|
Console.WriteLine("Comport thread close"); |
|
}); |
|
t11.Name = "comport reader"; |
|
t11.Start(); |
|
MainV2.threads.Add(t11); |
|
|
|
// doesnt seem to work on mac |
|
//comPort.DataReceived += new SerialDataReceivedEventHandler(comPort_DataReceived); |
|
} |
|
|
|
void genchkcombo(int logcount) |
|
{ |
|
MethodInvoker m = delegate() |
|
{ |
|
CHK_logs.Items.Clear(); |
|
for (int a = 1; a <= logcount; a++) |
|
{ |
|
CHK_logs.Items.Add(a); |
|
} |
|
}; |
|
try |
|
{ |
|
BeginInvoke(m); |
|
} |
|
catch |
|
{ |
|
} |
|
} |
|
|
|
void updateDisplay() |
|
{ |
|
if (start.Second != DateTime.Now.Second) |
|
{ |
|
this.BeginInvoke((System.Windows.Forms.MethodInvoker)delegate() |
|
{ |
|
try |
|
{ |
|
TXT_status.Text = status.ToString() + " " + receivedbytes + " " + comPort.BytesToRead; |
|
} |
|
catch { } |
|
}); |
|
start = DateTime.Now; |
|
} |
|
} |
|
|
|
void comPort_DataReceived(object sender, SerialDataReceivedEventArgs e) |
|
{ |
|
try |
|
{ |
|
|
|
|
|
while (comPort.BytesToRead > 0 && threadrun) |
|
{ |
|
updateDisplay(); |
|
|
|
string line = ""; |
|
|
|
comPort.ReadTimeout = 500; |
|
try |
|
{ |
|
line = comPort.ReadLine(); //readline(comPort); |
|
if (!line.Contains("\n")) |
|
line = line + "\n"; |
|
} |
|
catch |
|
{ |
|
line = comPort.ReadExisting(); |
|
//byte[] data = readline(comPort); |
|
//line = Encoding.ASCII.GetString(data, 0, data.Length); |
|
} |
|
|
|
receivedbytes += line.Length; |
|
|
|
//string line = Encoding.ASCII.GetString(data, 0, data.Length); |
|
|
|
switch (status) |
|
{ |
|
case serialstatus.Connecting: |
|
|
|
if (line.Contains("reset to FLY") || line.Contains("interactive setup") || line.Contains("CLI:") || line.Contains("Ardu")) |
|
{ |
|
comPort.Write("logs\r"); |
|
} |
|
if (line.Contains("logs")) |
|
{ |
|
Regex regex2 = new Regex(@"^([0-9]+)", RegexOptions.IgnoreCase); |
|
if (regex2.IsMatch(line)) |
|
{ |
|
MatchCollection matchs = regex2.Matches(line); |
|
logcount = int.Parse(matchs[0].Groups[0].Value); |
|
genchkcombo(logcount); |
|
status = serialstatus.Done; |
|
} |
|
} |
|
if (line.Contains("No logs")) |
|
{ |
|
status = serialstatus.Done; |
|
} |
|
|
|
break; |
|
case serialstatus.Closefile: |
|
sw.Close(); |
|
TextReader tr = new StreamReader(logfile); |
|
|
|
MainV2.cs.firmware = MainV2.Firmwares.ArduPlane; |
|
|
|
this.Invoke((System.Windows.Forms.MethodInvoker)delegate() |
|
{ |
|
TXT_seriallog.AppendText("Createing KML for " + logfile); |
|
}); |
|
|
|
while (tr.Peek() != -1) |
|
{ |
|
processLine(tr.ReadLine()); |
|
} |
|
|
|
tr.Close(); |
|
|
|
try |
|
{ |
|
writeKML(logfile + ".kml"); |
|
} |
|
catch { } // usualy invalid lat long error |
|
status = serialstatus.Done; |
|
break; |
|
case serialstatus.Createfile: |
|
receivedbytes = 0; |
|
Directory.CreateDirectory(Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + @"logs"); |
|
logfile = Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + @"logs" + Path.DirectorySeparatorChar + DateTime.Now.ToString("yyyy-MM-dd hh-mm") + " " + currentlog + ".log"; |
|
sw = new StreamWriter(logfile); |
|
status = serialstatus.Waiting; |
|
lock (thisLock) |
|
{ |
|
this.Invoke((System.Windows.Forms.MethodInvoker)delegate() |
|
{ |
|
TXT_seriallog.Clear(); |
|
}); |
|
} |
|
//if (line.Contains("Dumping Log")) |
|
{ |
|
status = serialstatus.Reading; |
|
} |
|
break; |
|
case serialstatus.Done: |
|
// |
|
break; |
|
case serialstatus.Reading: |
|
if (line.Contains("packets read") || line.Contains("Done") || line.Contains("logs enabled")) |
|
{ |
|
status = serialstatus.Closefile; |
|
break; |
|
} |
|
sw.Write(line); |
|
continue; |
|
case serialstatus.Waiting: |
|
if (line.Contains("Dumping Log") || line.Contains("GPS:") || line.Contains("NTUN:") || line.Contains("CTUN:") || line.Contains("PM:")) |
|
{ |
|
status = serialstatus.Reading; |
|
} |
|
break; |
|
} |
|
lock (thisLock) |
|
{ |
|
this.BeginInvoke((MethodInvoker)delegate() |
|
{ |
|
|
|
Console.Write(line); |
|
|
|
TXT_seriallog.AppendText(line); |
|
|
|
// auto scroll |
|
if (TXT_seriallog.TextLength >= 10000) |
|
{ |
|
TXT_seriallog.Text = TXT_seriallog.Text.Substring(TXT_seriallog.TextLength / 2); |
|
} |
|
|
|
TXT_seriallog.SelectionStart = TXT_seriallog.Text.Length; |
|
|
|
TXT_seriallog.ScrollToCaret(); |
|
|
|
TXT_seriallog.Refresh(); |
|
|
|
}); |
|
|
|
|
|
|
|
} |
|
} |
|
|
|
Console.WriteLine("exit while"); |
|
} |
|
catch (Exception ex) { MessageBox.Show("Error reading data" + ex.ToString()); } |
|
} |
|
|
|
string lastline = ""; |
|
string[] ctunlast = new string[] { "", "", "", "", "", "", "", "", "", "", "", "", "", "" }; |
|
string[] ntunlast = new string[] { "", "", "", "", "", "", "", "", "", "", "", "", "", "" }; |
|
List<PointLatLngAlt> cmd = new List<PointLatLngAlt>(); |
|
Point3D oldlastpos = new Point3D(); |
|
Point3D lastpos = new Point3D(); |
|
|
|
private void processLine(string line) |
|
{ |
|
try |
|
{ |
|
Application.DoEvents(); |
|
|
|
line = line.Replace(", ", ","); |
|
line = line.Replace(": ", ":"); |
|
|
|
string[] items = line.Split(',', ':'); |
|
|
|
if (items[0].Contains("CMD")) |
|
{ |
|
if (flightdata.Count == 0) |
|
{ |
|
if (int.Parse(items[2]) <= (int)MAVLink.MAV_CMD.LAST) // wps |
|
{ |
|
PointLatLngAlt temp = new PointLatLngAlt(double.Parse(items[5], new System.Globalization.CultureInfo("en-US")) / 10000000, double.Parse(items[6], new System.Globalization.CultureInfo("en-US")) / 10000000, double.Parse(items[4], new System.Globalization.CultureInfo("en-US")) / 100, items[1].ToString()); |
|
cmd.Add(temp); |
|
} |
|
} |
|
} |
|
if (items[0].Contains("MOD")) |
|
{ |
|
positionindex++; |
|
modelist.Add(""); // i cant be bothered doing this properly |
|
modelist.Add(""); |
|
modelist[positionindex] = (items[1]); |
|
} |
|
if (items[0].Contains("GPS") && items[2] == "1" && items[4] != "0" && items[4] != "-1" && lastline != line) // check gps line and fixed status |
|
{ |
|
if (position[positionindex] == null) |
|
position[positionindex] = new List<Point3D>(); |
|
|
|
double alt = double.Parse(items[6], new System.Globalization.CultureInfo("en-US")); |
|
|
|
if (items.Length == 11 && items[6] == "0.0000") |
|
alt = double.Parse(items[7], new System.Globalization.CultureInfo("en-US")); |
|
|
|
|
|
position[positionindex].Add(new Point3D(double.Parse(items[5], new System.Globalization.CultureInfo("en-US")), double.Parse(items[4], new System.Globalization.CultureInfo("en-US")), alt)); |
|
oldlastpos = lastpos; |
|
lastpos = (position[positionindex][position[positionindex].Count - 1]); |
|
lastline = line; |
|
} |
|
if (items[0].Contains("GPS") && items[4] != "0" && items[4] != "-1" && items.Length <= 9) |
|
{ |
|
if (position[positionindex] == null) |
|
position[positionindex] = new List<Point3D>(); |
|
|
|
MainV2.cs.firmware = MainV2.Firmwares.ArduCopter2; |
|
|
|
double alt = double.Parse(items[5], new System.Globalization.CultureInfo("en-US")); |
|
|
|
position[positionindex].Add(new Point3D(double.Parse(items[4], new System.Globalization.CultureInfo("en-US")), double.Parse(items[3], new System.Globalization.CultureInfo("en-US")), alt)); |
|
oldlastpos = lastpos; |
|
lastpos = (position[positionindex][position[positionindex].Count - 1]); |
|
lastline = line; |
|
|
|
} |
|
if (items[0].Contains("CTUN")) |
|
{ |
|
ctunlast = items; |
|
} |
|
if (items[0].Contains("NTUN")) |
|
{ |
|
ntunlast = items; |
|
line = "ATT:" + double.Parse(ctunlast[3], new System.Globalization.CultureInfo("en-US")) * 100 + "," + double.Parse(ctunlast[6], new System.Globalization.CultureInfo("en-US")) * 100 + "," + double.Parse(items[1], new System.Globalization.CultureInfo("en-US")) * 100; |
|
items = line.Split(',', ':'); |
|
} |
|
if (items[0].Contains("ATT")) |
|
{ |
|
try |
|
{ |
|
if (lastpos.X != 0 && oldlastpos.X != lastpos.X && oldlastpos.Y != lastpos.Y) |
|
{ |
|
Data dat = new Data(); |
|
|
|
try |
|
{ |
|
dat.datetime = int.Parse(lastline.Split(',', ':')[1]); |
|
} |
|
catch { } |
|
|
|
runmodel = new Model(); |
|
|
|
runmodel.Location.longitude = lastpos.X; |
|
runmodel.Location.latitude = lastpos.Y; |
|
runmodel.Location.altitude = lastpos.Z; |
|
|
|
oldlastpos = lastpos; |
|
|
|
runmodel.Orientation.roll = double.Parse(items[1], new System.Globalization.CultureInfo("en-US")) / -100; |
|
runmodel.Orientation.tilt = double.Parse(items[2], new System.Globalization.CultureInfo("en-US")) / -100; |
|
runmodel.Orientation.heading = double.Parse(items[3], new System.Globalization.CultureInfo("en-US")) / 100; |
|
|
|
dat.model = runmodel; |
|
dat.ctun = ctunlast; |
|
dat.ntun = ntunlast; |
|
|
|
flightdata.Add(dat); |
|
} |
|
} |
|
catch { } |
|
} |
|
} |
|
catch (Exception) |
|
{ |
|
// if items is to short or parse fails.. ignore |
|
} |
|
} |
|
|
|
List<string> modelist = new List<string>(); |
|
List<Core.Geometry.Point3D>[] position = new List<Core.Geometry.Point3D>[200]; |
|
int positionindex = 0; |
|
|
|
private void writeGPX(string filename) |
|
{ |
|
System.Xml.XmlTextWriter xw = new System.Xml.XmlTextWriter(Path.GetDirectoryName(filename) + Path.DirectorySeparatorChar + Path.GetFileNameWithoutExtension(filename) + ".gpx",Encoding.ASCII); |
|
|
|
xw.WriteStartElement("gpx"); |
|
|
|
xw.WriteStartElement("trk"); |
|
|
|
xw.WriteStartElement("trkseg"); |
|
|
|
DateTime start = new DateTime(DateTime.Now.Year,DateTime.Now.Month,DateTime.Now.Day,0,0,0); |
|
|
|
foreach (Data mod in flightdata) |
|
{ |
|
xw.WriteStartElement("trkpt"); |
|
xw.WriteAttributeString("lat",mod.model.Location.latitude.ToString()); |
|
xw.WriteAttributeString("lon", mod.model.Location.longitude.ToString()); |
|
|
|
xw.WriteElementString("ele", mod.model.Location.altitude.ToString()); |
|
xw.WriteElementString("time", start.AddMilliseconds(mod.datetime).ToString("yyyy-MM-ddTHH:mm:sszzzzzz")); |
|
xw.WriteElementString("course", (mod.model.Orientation.heading).ToString()); |
|
|
|
xw.WriteElementString("roll", mod.model.Orientation.roll.ToString()); |
|
xw.WriteElementString("pitch", mod.model.Orientation.tilt.ToString()); |
|
//xw.WriteElementString("speed", mod.model.Orientation.); |
|
//xw.WriteElementString("fix", mod.model.Location.altitude); |
|
|
|
xw.WriteEndElement(); |
|
} |
|
|
|
xw.WriteEndElement(); |
|
xw.WriteEndElement(); |
|
xw.WriteEndElement(); |
|
|
|
xw.Close(); |
|
} |
|
|
|
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); |
|
Console.WriteLine("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(); |
|
} |
|
|
|
private void Log_FormClosing(object sender, FormClosingEventArgs e) |
|
{ |
|
threadrun = false; |
|
System.Threading.Thread.Sleep(500); |
|
if (comPort.IsOpen) |
|
{ |
|
//comPort.Close(); |
|
} |
|
System.Threading.Thread.Sleep(500); |
|
} |
|
|
|
private void CHK_logs_Click(object sender, EventArgs e) |
|
{ |
|
ListBox lb = sender as ListBox; |
|
|
|
} |
|
|
|
private void BUT_DLall_Click(object sender, EventArgs e) |
|
{ |
|
if (status == serialstatus.Done) |
|
{ |
|
System.Threading.Thread t11 = new System.Threading.Thread(delegate() { downloadthread(1, logcount); }); |
|
t11.Name = "Log Download All thread"; |
|
t11.Start(); |
|
MainV2.threads.Add(t11); |
|
} |
|
} |
|
|
|
private void downloadthread(int startlognum, int endlognum) |
|
{ |
|
for (int a = startlognum; a <= endlognum; a++) |
|
{ |
|
currentlog = a; |
|
System.Threading.Thread.Sleep(500); |
|
comPort.Write("dump "); |
|
System.Threading.Thread.Sleep(100); |
|
comPort.Write(a.ToString() + "\r"); |
|
status = serialstatus.Createfile; |
|
|
|
while (status != serialstatus.Done) |
|
{ |
|
System.Threading.Thread.Sleep(100); |
|
} |
|
|
|
} |
|
} |
|
|
|
private void downloadsinglethread() |
|
{ |
|
for (int i = 0; i < CHK_logs.CheckedItems.Count; ++i) |
|
{ |
|
int a = (int)CHK_logs.CheckedItems[i]; |
|
{ |
|
currentlog = a; |
|
System.Threading.Thread.Sleep(500); |
|
comPort.Write("dump "); |
|
System.Threading.Thread.Sleep(100); |
|
comPort.Write(a.ToString() + "\r"); |
|
status = serialstatus.Createfile; |
|
|
|
while (status != serialstatus.Done) |
|
{ |
|
System.Threading.Thread.Sleep(100); |
|
} |
|
} |
|
} |
|
} |
|
|
|
private void BUT_DLthese_Click(object sender, EventArgs e) |
|
{ |
|
if (status == serialstatus.Done) |
|
{ |
|
System.Threading.Thread t11 = new System.Threading.Thread(delegate() { downloadsinglethread(); }); |
|
t11.Name = "Log download single thread"; |
|
t11.Start(); |
|
MainV2.threads.Add(t11); |
|
} |
|
} |
|
|
|
private void BUT_clearlogs_Click(object sender, EventArgs e) |
|
{ |
|
System.Threading.Thread.Sleep(500); |
|
comPort.Write("erase\r"); |
|
System.Threading.Thread.Sleep(100); |
|
TXT_seriallog.AppendText("!!Allow 30 seconds for erase\n"); |
|
status = serialstatus.Done; |
|
} |
|
|
|
private void BUT_redokml_Click(object sender, EventArgs e) |
|
{ |
|
OpenFileDialog openFileDialog1 = new OpenFileDialog(); |
|
openFileDialog1.Filter = "*.log|*.log"; |
|
openFileDialog1.FilterIndex = 2; |
|
openFileDialog1.RestoreDirectory = true; |
|
openFileDialog1.Multiselect = true; |
|
try |
|
{ |
|
openFileDialog1.InitialDirectory = Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + @"logs" + Path.DirectorySeparatorChar; |
|
} |
|
catch { } // incase dir doesnt exist |
|
|
|
if (openFileDialog1.ShowDialog() == DialogResult.OK) |
|
{ |
|
foreach (string logfile in openFileDialog1.FileNames) |
|
{ |
|
TXT_seriallog.AppendText("\n\nProcessing " + logfile + "\n"); |
|
this.Refresh(); |
|
|
|
TextReader tr = new StreamReader(logfile); |
|
|
|
MainV2.cs.firmware = MainV2.Firmwares.ArduPlane; |
|
|
|
while (tr.Peek() != -1) |
|
{ |
|
processLine(tr.ReadLine()); |
|
} |
|
|
|
tr.Close(); |
|
|
|
writeKML(logfile + ".kml"); |
|
|
|
TXT_seriallog.AppendText("Done\n"); |
|
} |
|
} |
|
} |
|
|
|
public static Color HexStringToColor(string hexColor) |
|
{ |
|
string hc = (hexColor); |
|
if (hc.Length != 8) |
|
{ |
|
// you can choose whether to throw an exception |
|
//throw new ArgumentException("hexColor is not exactly 6 digits."); |
|
return Color.Empty; |
|
} |
|
string a = hc.Substring(0, 2); |
|
string r = hc.Substring(6, 2); |
|
string g = hc.Substring(4, 2); |
|
string b = hc.Substring(2, 2); |
|
Color color = Color.Empty; |
|
try |
|
{ |
|
int ai |
|
= Int32.Parse(a, System.Globalization.NumberStyles.HexNumber); |
|
int ri |
|
= Int32.Parse(r, System.Globalization.NumberStyles.HexNumber); |
|
int gi |
|
= Int32.Parse(g, System.Globalization.NumberStyles.HexNumber); |
|
int bi |
|
= Int32.Parse(b, System.Globalization.NumberStyles.HexNumber); |
|
color = Color.FromArgb(ai, ri, gi, bi); |
|
} |
|
catch |
|
{ |
|
// you can choose whether to throw an exception |
|
//throw new ArgumentException("Conversion failed."); |
|
return Color.Empty; |
|
} |
|
return color; |
|
} |
|
|
|
private void BUT_firstperson_Click(object sender, EventArgs e) |
|
{ |
|
OpenFileDialog openFileDialog1 = new OpenFileDialog(); |
|
openFileDialog1.Filter = "*.log|*.log"; |
|
openFileDialog1.FilterIndex = 2; |
|
openFileDialog1.RestoreDirectory = true; |
|
openFileDialog1.Multiselect = true; |
|
try |
|
{ |
|
openFileDialog1.InitialDirectory = Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + @"logs" + Path.DirectorySeparatorChar; |
|
} |
|
catch { } // incase dir doesnt exist |
|
|
|
if (openFileDialog1.ShowDialog() == DialogResult.OK) |
|
{ |
|
foreach (string logfile in openFileDialog1.FileNames) |
|
{ |
|
TXT_seriallog.AppendText("\n\nProcessing " + logfile + "\n"); |
|
this.Refresh(); |
|
|
|
try |
|
{ |
|
TextReader tr = new StreamReader(logfile); |
|
|
|
MainV2.cs.firmware = MainV2.Firmwares.ArduPlane; |
|
|
|
while (tr.Peek() != -1) |
|
{ |
|
processLine(tr.ReadLine()); |
|
} |
|
|
|
tr.Close(); |
|
} |
|
catch (Exception ex) { MessageBox.Show("Error processing log. Is it still downloading? " + ex.Message); continue; } |
|
|
|
writeKMLFirstPerson(logfile + ".kml"); |
|
|
|
TXT_seriallog.AppendText("Done\n"); |
|
} |
|
} |
|
} |
|
|
|
private void writeKMLFirstPerson(string filename) |
|
{ |
|
StreamWriter stream = new StreamWriter(File.Open(filename, FileMode.Create)); |
|
System.Text.ASCIIEncoding encoding = new System.Text.ASCIIEncoding(); |
|
string header = "<?xml version=\"1.0\" encoding=\"UTF-8\"?><kml xmlns=\"http://www.opengis.net/kml/2.2\" xmlns:gx=\"http://www.google.com/kml/ext/2.2\" xmlns:kml=\"http://www.opengis.net/kml/2.2\" xmlns:atom=\"http://www.w3.org/2005/Atom\">\n <Document> <name>Paths</name> <description>Path</description>\n <Style id=\"yellowLineGreenPoly\"> <LineStyle> <color>7f00ffff</color> <width>4</width> </LineStyle> <PolyStyle> <color>7f00ff00</color> </PolyStyle> </Style>\n "; |
|
stream.Write(header); |
|
|
|
StringBuilder kml = new StringBuilder(); |
|
StringBuilder data = new StringBuilder(); |
|
|
|
double lastlat = 0; |
|
double lastlong = 0; |
|
int gpspackets = 0; |
|
int lastgpspacket = 0; |
|
|
|
foreach (Data mod in flightdata) |
|
{ |
|
if (mod.model.Location.latitude == 0) |
|
continue; |
|
|
|
gpspackets++; |
|
if (lastlat == mod.model.Location.latitude && lastlong == mod.model.Location.longitude) |
|
continue; |
|
// double speed 0.05 - assumeing 10hz in log file |
|
// 1 speed = 0.1 10 / 1 = 0.1 |
|
data.Append(@" |
|
<gx:FlyTo> |
|
<gx:duration>" + ((gpspackets - lastgpspacket) * 0.1) + @"</gx:duration> |
|
<gx:flyToMode>smooth</gx:flyToMode> |
|
<Camera> |
|
<longitude>" + mod.model.Location.longitude.ToString(new System.Globalization.CultureInfo("en-US")) + @"</longitude> |
|
<latitude>" + mod.model.Location.latitude.ToString(new System.Globalization.CultureInfo("en-US")) + @"</latitude> |
|
<altitude>" + mod.model.Location.altitude.ToString(new System.Globalization.CultureInfo("en-US")) + @"</altitude> |
|
<roll>" + mod.model.Orientation.roll.ToString(new System.Globalization.CultureInfo("en-US")) + @"</roll> |
|
<tilt>" + (90 - mod.model.Orientation.tilt).ToString(new System.Globalization.CultureInfo("en-US")) + @"</tilt> |
|
<heading>" + mod.model.Orientation.heading.ToString(new System.Globalization.CultureInfo("en-US")) + @"</heading> |
|
<altitudeMode>absolute</altitudeMode> |
|
</Camera> |
|
</gx:FlyTo> |
|
"); |
|
lastlat = mod.model.Location.latitude; |
|
lastlong = mod.model.Location.longitude; |
|
lastgpspacket = gpspackets; |
|
} |
|
|
|
kml.Append(@" |
|
<Folder> |
|
<name>Flight</name> |
|
<gx:Tour> |
|
<name>Flight Do</name> |
|
<gx:Playlist> |
|
" + data + |
|
@"</gx:Playlist> |
|
</gx:Tour> |
|
</Folder> |
|
</Document> |
|
</kml> |
|
"); |
|
|
|
stream.Write(kml.ToString()); |
|
stream.Close(); |
|
|
|
// 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(); |
|
} |
|
|
|
} |
|
} |