ardupilot/Tools/ArdupilotMegaPlanner/Log.cs

1015 lines
38 KiB
C#
Raw Normal View History

2011-09-08 22:31:32 -03:00
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;
2011-09-08 22:31:32 -03:00
}
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();
2011-09-08 22:31:32 -03:00
//comPort.Open();
}
catch (Exception)
{
2011-09-08 22:31:32 -03:00
MessageBox.Show("Error opening comport");
}
System.Threading.Thread t11 = new System.Threading.Thread(delegate()
{
DateTime start = DateTime.Now;
threadrun = true;
System.Threading.Thread.Sleep(4000);
2011-12-19 10:41:23 -04:00
try
{
comPort.Write("\n\n\n\n");
}
catch { }
2011-09-08 22:31:32 -03:00
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
2011-09-08 22:31:32 -03:00
}
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++)
if (!CHK_logs.Items.Contains(logcount))
2011-09-08 22:31:32 -03:00
{
CHK_logs.Items.Add(logcount);
2011-09-08 22:31:32 -03:00
}
};
try
{
BeginInvoke(m);
}
catch
{
}
}
void updateDisplay()
{
if (start.Second != DateTime.Now.Second)
{
this.BeginInvoke((System.Windows.Forms.MethodInvoker)delegate()
{
2011-12-19 10:41:23 -04:00
try
{
TXT_status.Text = status.ToString() + " " + receivedbytes + " " + comPort.BytesToRead;
}
catch { }
});
2011-09-08 22:31:32 -03:00
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
{
2011-09-08 22:31:32 -03:00
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");
status = serialstatus.Done;
}
2011-09-08 22:31:32 -03:00
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);
});
2011-09-08 22:31:32 -03:00
while (tr.Peek() != -1)
{
processLine(tr.ReadLine());
}
tr.Close();
try
{
writeKML(logfile + ".kml");
}
catch { } // usualy invalid lat long error
2011-09-08 22:31:32 -03:00
status = serialstatus.Done;
break;
case serialstatus.Createfile:
receivedbytes = 0;
Directory.CreateDirectory(Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + @"logs");
2011-09-08 22:31:32 -03:00
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();
});
2011-09-08 22:31:32 -03:00
}
//if (line.Contains("Dumping Log"))
{
status = serialstatus.Reading;
}
break;
case serialstatus.Done:
//
if (line.Contains("start") && line.Contains("end"))
{
Regex regex2 = new Regex(@"^Log ([0-9]+),", RegexOptions.IgnoreCase);
if (regex2.IsMatch(line))
{
MatchCollection matchs = regex2.Matches(line);
logcount = int.Parse(matchs[0].Groups[1].Value);
genchkcombo(logcount);
//status = serialstatus.Done;
}
}
if (line.Contains("No logs"))
{
status = serialstatus.Done;
}
2011-09-08 22:31:32 -03:00
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()
{
2011-09-08 22:31:32 -03:00
Console.Write(line);
2011-09-08 22:31:32 -03:00
TXT_seriallog.AppendText(line);
2011-09-08 22:31:32 -03:00
// auto scroll
if (TXT_seriallog.TextLength >= 10000)
{
TXT_seriallog.Text = TXT_seriallog.Text.Substring(TXT_seriallog.TextLength / 2);
}
2011-09-08 22:31:32 -03:00
TXT_seriallog.SelectionStart = TXT_seriallog.Text.Length;
2011-09-08 22:31:32 -03:00
TXT_seriallog.ScrollToCaret();
2011-09-08 22:31:32 -03:00
TXT_seriallog.Refresh();
2011-09-08 22:31:32 -03:00
});
2011-09-08 22:31:32 -03:00
}
2011-09-08 22:31:32 -03:00
}
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();
2011-09-08 22:31:32 -03:00
line = line.Replace(", ", ",");
line = line.Replace(": ", ":");
string[] items = line.Split(',', ':');
2011-09-08 22:31:32 -03:00
if (items[0].Contains("CMD"))
{
2011-09-08 22:31:32 -03:00
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());
2011-09-08 22:31:32 -03:00
cmd.Add(temp);
}
}
}
if (items[0].Contains("MOD"))
{
positionindex++;
modelist.Add(""); // i cant be bothered doing this properly
modelist.Add("");
2011-09-08 22:31:32 -03:00
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"));
if (items.Length == 11 && items[6] == "0")
alt = double.Parse(items[7], new System.Globalization.CultureInfo("en-US"));
2011-09-08 22:31:32 -03:00
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));
2011-09-08 22:31:32 -03:00
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;
}
2011-09-08 22:31:32 -03:00
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)
2011-09-08 22:31:32 -03:00
{
Data dat = new Data();
2011-09-08 22:31:32 -03:00
try
{
dat.datetime = int.Parse(lastline.Split(',', ':')[1]);
}
catch { }
runmodel = new Model();
2011-09-08 22:31:32 -03:00
runmodel.Location.longitude = lastpos.X;
runmodel.Location.latitude = lastpos.Y;
runmodel.Location.altitude = lastpos.Z;
2011-09-08 22:31:32 -03:00
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;
2011-09-08 22:31:32 -03:00
dat.model = runmodel;
dat.ctun = ctunlast;
dat.ntun = ntunlast;
2011-09-08 22:31:32 -03:00
flightdata.Add(dat);
2011-09-08 22:31:32 -03:00
}
}
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(new System.Globalization.CultureInfo("en-US")));
xw.WriteAttributeString("lon", mod.model.Location.longitude.ToString(new System.Globalization.CultureInfo("en-US")));
xw.WriteElementString("ele", mod.model.Location.altitude.ToString(new System.Globalization.CultureInfo("en-US")));
xw.WriteElementString("time", start.AddMilliseconds(mod.datetime).ToString("yyyy-MM-ddTHH:mm:sszzzzzz"));
xw.WriteElementString("course", (mod.model.Orientation.heading).ToString(new System.Globalization.CultureInfo("en-US")));
xw.WriteElementString("roll", mod.model.Orientation.roll.ToString(new System.Globalization.CultureInfo("en-US")));
xw.WriteElementString("pitch", mod.model.Orientation.tilt.ToString(new System.Globalization.CultureInfo("en-US")));
//xw.WriteElementString("speed", mod.model.Orientation.);
//xw.WriteElementString("fix", mod.model.Location.altitude);
xw.WriteEndElement();
}
xw.WriteEndElement();
xw.WriteEndElement();
xw.WriteEndElement();
xw.Close();
}
2011-09-08 22:31:32 -03:00
private void writeKML(string filename)
{
try
{
writeGPX(filename);
}
catch { }
2011-09-08 22:31:32 -03:00
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.
}
2011-09-08 22:31:32 -03:00
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;
2011-09-08 22:31:32 -03:00
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());
2011-09-08 22:31:32 -03:00
style2.Add(new LineStyle(color, 4));
2011-09-08 22:31:32 -03:00
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;
2011-09-08 22:31:32 -03:00
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
{
2011-09-08 22:31:32 -03:00
pmplane.Point = new KmlPoint((float)model.Location.longitude, (float)model.Location.latitude, (float)model.Location.altitude);
pmplane.Point.AltitudeMode = altmode;
2011-09-08 22:31:32 -03:00
Link link = new Link();
link.href = "block_plane_0.dae";
2011-09-08 22:31:32 -03:00
model.Link = link;
2011-09-08 22:31:32 -03:00
pmplane.Model = model;
planes.Add(pmplane);
}
catch { } // bad lat long value
2011-09-08 22:31:32 -03:00
lastmodel = mod.model;
a++;
}
2011-09-08 22:31:32 -03:00
kml.Document.Add(fldr);
kml.Save(filename);
// create kmz - aka zip file
FileStream fs = File.Open(filename.Replace(".log.kml", ".kmz"), FileMode.Create);
2011-09-08 22:31:32 -03:00
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)
2011-09-08 22:31:32 -03:00
{
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)
{
2011-09-08 22:31:32 -03:00
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");
2011-09-08 22:31:32 -03:00
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");
2011-09-08 22:31:32 -03:00
status = serialstatus.Done;
CHK_logs.Items.Clear();
2011-09-08 22:31:32 -03:00
}
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;
2011-09-08 22:31:32 -03:00
}
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;
2011-09-08 22:31:32 -03:00
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");
2011-09-08 22:31:32 -03:00
this.Refresh();
try
{
TextReader tr = new StreamReader(logfile);
MainV2.cs.firmware = MainV2.Firmwares.ArduPlane;
2011-09-08 22:31:32 -03:00
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);
2011-09-08 22:31:32 -03:00
}
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();
}
}
}