mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 22:48:29 -04:00
09d8a8a2d1
remove some mavlink 0.9 code add arm/disarm button. Not yet implemented AC and AP Side. modify heli setup screen modify failsafe screen fix old firmware under mono workaround mono crash. modify planner wp storage tweak log dl screen add change alt button modify tlog wp extractor, multiple extractions speed up srtm reading fix lang edit.
1408 lines
52 KiB
C#
1408 lines
52 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Collections;
|
|
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;
|
|
|
|
using SharpKml.Base;
|
|
using SharpKml.Dom;
|
|
using SharpKml.Dom.GX;
|
|
|
|
using System.Reflection;
|
|
using System.Xml;
|
|
using log4net;
|
|
using ZedGraph; // Graphs
|
|
|
|
using ArdupilotMega.Utilities;
|
|
|
|
using System.CodeDom.Compiler;
|
|
|
|
namespace ArdupilotMega
|
|
{
|
|
public partial class MavlinkLog : Form
|
|
{
|
|
private static readonly ILog log = LogManager.GetLogger(System.Reflection.MethodBase.GetCurrentMethod().DeclaringType);
|
|
|
|
List<CurrentState> flightdata = new List<CurrentState>();
|
|
|
|
List<string> selection = new List<string>();
|
|
List<string> options = new List<string>();
|
|
|
|
Hashtable datappl = new Hashtable();
|
|
Hashtable packetdata = new Hashtable();
|
|
|
|
PointLatLngAlt homepos = new PointLatLngAlt();
|
|
|
|
public MavlinkLog()
|
|
{
|
|
InitializeComponent();
|
|
|
|
zg1.GraphPane.YAxis.Title.IsVisible = false;
|
|
zg1.GraphPane.Title.IsVisible = true;
|
|
zg1.GraphPane.Title.Text = "Mavlink Log Graph";
|
|
}
|
|
|
|
private void writeKML(string filename)
|
|
{
|
|
SharpKml.Dom.AltitudeMode altmode = SharpKml.Dom.AltitudeMode.Absolute;
|
|
|
|
if (MainV2.cs.firmware == MainV2.Firmwares.ArduPlane || MainV2.cs.firmware == MainV2.Firmwares.ArduRover)
|
|
{
|
|
altmode = SharpKml.Dom.AltitudeMode.Absolute;
|
|
}
|
|
else if (MainV2.cs.firmware == MainV2.Firmwares.ArduCopter2)
|
|
{
|
|
altmode = SharpKml.Dom.AltitudeMode.RelativeToGround;
|
|
}
|
|
|
|
Color[] colours = { Color.Red, Color.Orange, Color.Yellow, Color.Green, Color.Blue, Color.Indigo, Color.Violet, Color.Pink };
|
|
|
|
Document kml = new Document();
|
|
|
|
Tour tour = new Tour() { Name = "First Person View" };
|
|
Playlist tourplaylist = new Playlist();
|
|
|
|
AddNamespace(kml, "gx", "http://www.google.com/kml/ext/2.2");
|
|
|
|
Style style = new Style();
|
|
style.Id = "yellowLineGreenPoly";
|
|
style.Line = new LineStyle(new Color32(HexStringToColor("7f00ffff")), 4);
|
|
|
|
|
|
|
|
PolygonStyle pstyle = new PolygonStyle();
|
|
pstyle.Color = new Color32(HexStringToColor("7f00ff00"));
|
|
style.Polygon = pstyle;
|
|
|
|
kml.AddStyle(style);
|
|
|
|
Style stylet = new Style();
|
|
stylet.Id = "track";
|
|
SharpKml.Dom.IconStyle ico = new SharpKml.Dom.IconStyle();
|
|
LabelStyle lst = new LabelStyle();
|
|
lst.Scale = 0;
|
|
stylet.Icon = ico;
|
|
ico.Icon = new IconStyle.IconLink(new Uri("http://earth.google.com/images/kml-icons/track-directional/track-none.png"));
|
|
stylet.Icon.Scale = 0.5;
|
|
stylet.Label = lst;
|
|
|
|
kml.AddStyle(stylet);
|
|
|
|
// create sub folders
|
|
Folder planes = new Folder();
|
|
planes.Name = "Planes";
|
|
kml.AddFeature(planes);
|
|
|
|
Folder points = new Folder();
|
|
points.Name = "Points";
|
|
kml.AddFeature(points);
|
|
|
|
|
|
// coords for line string
|
|
CoordinateCollection coords = new CoordinateCollection();
|
|
|
|
int a = 1;
|
|
int c = -1;
|
|
DateTime lasttime = DateTime.MaxValue;
|
|
DateTime starttime = DateTime.MinValue;
|
|
Color stylecolor = Color.AliceBlue;
|
|
string mode = "";
|
|
if (flightdata.Count > 0)
|
|
{
|
|
mode = flightdata[0].mode;
|
|
}
|
|
foreach (CurrentState cs in flightdata)
|
|
{
|
|
progressBar1.Value = 50 + (int)((float)a / (float)flightdata.Count * 100.0f / 2.0f);
|
|
progressBar1.Refresh();
|
|
|
|
if (starttime == DateTime.MinValue)
|
|
{
|
|
starttime = cs.datetime;
|
|
lasttime = cs.datetime;
|
|
}
|
|
|
|
if (mode != cs.mode || flightdata.Count == a)
|
|
{
|
|
c++;
|
|
|
|
LineString ls = new LineString();
|
|
ls.AltitudeMode = altmode;
|
|
ls.Extrude = true;
|
|
|
|
ls.Coordinates = coords;
|
|
|
|
Placemark pm = new Placemark();
|
|
|
|
pm.Name = c + " Flight Path " + mode;
|
|
pm.StyleUrl = new Uri("#yellowLineGreenPoly", UriKind.Relative);
|
|
pm.Geometry = ls;
|
|
|
|
SharpKml.Dom.TimeSpan ts = new SharpKml.Dom.TimeSpan();
|
|
ts.Begin = starttime;
|
|
ts.End = cs.datetime;
|
|
|
|
pm.Time = ts;
|
|
|
|
// setup for next line
|
|
mode = cs.mode;
|
|
starttime = cs.datetime;
|
|
|
|
stylecolor = colours[c % (colours.Length - 1)];
|
|
|
|
Style style2 = new Style();
|
|
style2.Line = new LineStyle(new Color32(stylecolor), 4);
|
|
|
|
pm.StyleSelector = style2;
|
|
|
|
kml.AddFeature(pm);
|
|
|
|
coords = new CoordinateCollection();
|
|
}
|
|
|
|
coords.Add(new Vector(cs.lat, cs.lng, cs.alt));
|
|
|
|
SharpKml.Dom.Timestamp tstamp = new SharpKml.Dom.Timestamp();
|
|
tstamp.When = cs.datetime;
|
|
|
|
FlyTo flyto = new FlyTo();
|
|
|
|
flyto.Duration = (cs.datetime - lasttime).TotalMilliseconds / 1000.0;
|
|
|
|
flyto.Mode = FlyToMode.Smooth;
|
|
SharpKml.Dom.Camera cam = new SharpKml.Dom.Camera();
|
|
cam.AltitudeMode = altmode;
|
|
cam.Latitude = cs.lat;
|
|
cam.Longitude = cs.lng;
|
|
cam.Altitude = cs.alt;
|
|
cam.Heading = cs.yaw;
|
|
cam.Roll = -cs.roll;
|
|
cam.Tilt = (90 - (cs.pitch * -1));
|
|
|
|
cam.GXTimePrimitive = tstamp;
|
|
|
|
flyto.View = cam;
|
|
//if (Math.Abs(flyto.Duration.Value) > 0.1)
|
|
{
|
|
tourplaylist.AddTourPrimitive(flyto);
|
|
lasttime = cs.datetime;
|
|
}
|
|
|
|
|
|
Placemark pmplane = new Placemark();
|
|
pmplane.Name = "Point " + a;
|
|
|
|
|
|
|
|
pmplane.Time = tstamp;
|
|
|
|
pmplane.Visibility = false;
|
|
|
|
SharpKml.Dom.Location loc = new SharpKml.Dom.Location();
|
|
loc.Latitude = cs.lat;
|
|
loc.Longitude = cs.lng;
|
|
loc.Altitude = cs.alt;
|
|
|
|
if (loc.Altitude < 0)
|
|
loc.Altitude = 0.01;
|
|
|
|
SharpKml.Dom.Orientation ori = new SharpKml.Dom.Orientation();
|
|
ori.Heading = cs.yaw;
|
|
ori.Roll = -cs.roll;
|
|
ori.Tilt = -cs.pitch;
|
|
|
|
SharpKml.Dom.Scale sca = new SharpKml.Dom.Scale();
|
|
|
|
sca.X = 2;
|
|
sca.Y = 2;
|
|
sca.Z = 2;
|
|
|
|
Model model = new Model();
|
|
model.Location = loc;
|
|
model.Orientation = ori;
|
|
model.AltitudeMode = altmode;
|
|
model.Scale = sca;
|
|
|
|
try
|
|
{
|
|
Description desc = new Description();
|
|
desc.Text = @"<![CDATA[
|
|
<table>
|
|
<tr><td>Roll: " + model.Orientation.Roll.Value.ToString("0.00") + @" </td></tr>
|
|
<tr><td>Pitch: " + model.Orientation.Tilt.Value.ToString("0.00") + @" </td></tr>
|
|
<tr><td>Yaw: " + model.Orientation.Heading.Value.ToString("0.00") + @" </td></tr>
|
|
<tr><td>Time: " + cs.datetime.ToString("HH:mm:sszzzzzz") + @" </td></tr>
|
|
</table> ";
|
|
// ]]>";
|
|
|
|
pmplane.Description = desc;
|
|
}
|
|
catch { }
|
|
|
|
SharpKml.Dom.Link link = new SharpKml.Dom.Link();
|
|
link.Href = new Uri("block_plane_0.dae", UriKind.Relative);
|
|
|
|
model.Link = link;
|
|
|
|
pmplane.Geometry = model;
|
|
|
|
planes.AddFeature(pmplane);
|
|
|
|
///
|
|
|
|
Placemark pmt = new Placemark();
|
|
|
|
SharpKml.Dom.Point pnt = new SharpKml.Dom.Point();
|
|
pnt.AltitudeMode = altmode;
|
|
pnt.Coordinate = new Vector(cs.lat,cs.lng,cs.alt);
|
|
|
|
pmt.Name = "" + a;
|
|
|
|
pmt.Description = pmplane.Description;
|
|
|
|
pmt.Time = tstamp;
|
|
|
|
pmt.Geometry = pnt;
|
|
pmt.StyleUrl = new Uri("#track", UriKind.Relative);
|
|
|
|
points.AddFeature(pmt);
|
|
|
|
a++;
|
|
}
|
|
|
|
tour.Playlist = tourplaylist;
|
|
|
|
kml.AddFeature(tour);
|
|
|
|
Serializer serializer = new Serializer();
|
|
serializer.Serialize(kml);
|
|
|
|
|
|
//Console.WriteLine(serializer.Xml);
|
|
|
|
StreamWriter sw = new StreamWriter(filename);
|
|
sw.Write(serializer.Xml);
|
|
sw.Close();
|
|
|
|
// create kmz - aka zip file
|
|
|
|
FileStream fs = File.Open(filename.Replace(Path.GetExtension(filename), ".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.Open(filename,FileMode.Open,FileAccess.Read,FileShare.ReadWrite))
|
|
{
|
|
StreamUtils.Copy(streamReader, zipStream, buffer);
|
|
}
|
|
zipStream.CloseEntry();
|
|
|
|
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();
|
|
|
|
}
|
|
|
|
static void AddNamespace(Element element, string prefix, string uri)
|
|
{
|
|
// The Namespaces property is marked as internal.
|
|
PropertyInfo property = typeof(Element).GetProperty(
|
|
"Namespaces",
|
|
BindingFlags.Instance | BindingFlags.NonPublic);
|
|
|
|
var namespaces = (XmlNamespaceManager)property.GetValue(element, null);
|
|
namespaces.AddNamespace(prefix, uri);
|
|
}
|
|
|
|
private void Log_FormClosing(object sender, FormClosingEventArgs e)
|
|
{
|
|
|
|
}
|
|
|
|
private void BUT_redokml_Click(object sender, EventArgs e)
|
|
{
|
|
OpenFileDialog openFileDialog1 = new OpenFileDialog();
|
|
openFileDialog1.Filter = "*.tlog|*.tlog";
|
|
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)
|
|
{
|
|
if (MainV2.comPort.logplaybackfile != null)
|
|
{
|
|
MainV2.comPort.logreadmode = false;
|
|
MainV2.comPort.logplaybackfile.Close();
|
|
}
|
|
|
|
foreach (string logfile in openFileDialog1.FileNames)
|
|
{
|
|
|
|
MAVLink mine = new MAVLink();
|
|
try
|
|
{
|
|
mine.logplaybackfile = new BinaryReader(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.Read));
|
|
}
|
|
catch (Exception ex) { log.Debug(ex.ToString()); CustomMessageBox.Show("Log Can not be opened. Are you still connected?"); return; }
|
|
mine.logreadmode = true;
|
|
|
|
mine.packets.Initialize(); // clear
|
|
|
|
CurrentState cs = new CurrentState();
|
|
|
|
float oldlatlngsum = 0;
|
|
|
|
int appui = 0;
|
|
|
|
while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length)
|
|
{
|
|
// bar moves to 50 % in this step
|
|
progressBar1.Value = (int)((float)mine.logplaybackfile.BaseStream.Position / (float)mine.logplaybackfile.BaseStream.Length * 100.0f / 2.0f);
|
|
progressBar1.Invalidate();
|
|
progressBar1.Refresh();
|
|
|
|
byte[] packet = mine.readPacket();
|
|
|
|
cs.datetime = mine.lastlogread;
|
|
|
|
cs.UpdateCurrentSettings(null, true, mine);
|
|
|
|
if (appui != DateTime.Now.Second)
|
|
{
|
|
// cant do entire app as mixes with flightdata timer
|
|
this.Refresh();
|
|
appui = DateTime.Now.Second;
|
|
}
|
|
|
|
try
|
|
{
|
|
if (MainV2.speechEngine != null)
|
|
MainV2.speechEngine.SpeakAsyncCancelAll();
|
|
}
|
|
catch { } // ignore because of this Exception System.PlatformNotSupportedException: No voice installed on the system or none available with the current security setting.
|
|
|
|
if ((float)(cs.lat + cs.lng) != oldlatlngsum
|
|
&& cs.lat != 0 && cs.lng != 0)
|
|
{
|
|
Console.WriteLine(cs.lat + " " + cs.lng + " " + cs.alt + " lah " + (float)(cs.lat + cs.lng) + "!=" + oldlatlngsum);
|
|
CurrentState cs2 = (CurrentState)cs.Clone();
|
|
|
|
flightdata.Add(cs2);
|
|
|
|
oldlatlngsum = (cs.lat + cs.lng);
|
|
}
|
|
}
|
|
|
|
mine.logreadmode = false;
|
|
mine.logplaybackfile.Close();
|
|
mine.logplaybackfile = null;
|
|
|
|
Application.DoEvents();
|
|
|
|
mine.setAPType();
|
|
|
|
writeGPX(logfile);
|
|
writeKML(logfile + ".kml");
|
|
|
|
flightdata.Clear();
|
|
|
|
progressBar1.Value = 100;
|
|
|
|
}
|
|
}
|
|
}
|
|
|
|
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");
|
|
|
|
foreach (CurrentState cs in flightdata)
|
|
{
|
|
xw.WriteStartElement("trkpt");
|
|
xw.WriteAttributeString("lat", cs.lat.ToString(new System.Globalization.CultureInfo("en-US")));
|
|
xw.WriteAttributeString("lon", cs.lng.ToString(new System.Globalization.CultureInfo("en-US")));
|
|
|
|
xw.WriteElementString("ele", cs.alt.ToString(new System.Globalization.CultureInfo("en-US")));
|
|
xw.WriteElementString("time", cs.datetime.ToString("yyyy-MM-ddTHH:mm:sszzzzzz"));
|
|
xw.WriteElementString("course", (cs.yaw).ToString(new System.Globalization.CultureInfo("en-US")));
|
|
|
|
xw.WriteElementString("roll", cs.roll.ToString(new System.Globalization.CultureInfo("en-US")));
|
|
xw.WriteElementString("pitch", cs.pitch.ToString(new System.Globalization.CultureInfo("en-US")));
|
|
//xw.WriteElementString("speed", mod.model.Orientation.);
|
|
//xw.WriteElementString("fix", cs.altitude);
|
|
|
|
xw.WriteEndElement();
|
|
}
|
|
|
|
xw.WriteEndElement();
|
|
xw.WriteEndElement();
|
|
xw.WriteEndElement();
|
|
|
|
xw.Close();
|
|
}
|
|
|
|
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_humanreadable_Click(object sender, EventArgs e)
|
|
{
|
|
OpenFileDialog openFileDialog1 = new OpenFileDialog();
|
|
openFileDialog1.Filter = "*.tlog|*.tlog";
|
|
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)
|
|
{
|
|
if (MainV2.comPort.logplaybackfile != null)
|
|
{
|
|
MainV2.comPort.logreadmode = false;
|
|
MainV2.comPort.logplaybackfile.Close();
|
|
}
|
|
|
|
foreach (string logfile in openFileDialog1.FileNames)
|
|
{
|
|
|
|
MAVLink mine = new MAVLink();
|
|
try
|
|
{
|
|
mine.logplaybackfile = new BinaryReader(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.Read));
|
|
}
|
|
catch (Exception ex) { log.Debug(ex.ToString()); CustomMessageBox.Show("Log Can not be opened. Are you still connected?"); return; }
|
|
|
|
mine.logreadmode = true;
|
|
|
|
mine.packets.Initialize(); // clear
|
|
|
|
StreamWriter sw = new StreamWriter(Path.GetDirectoryName(logfile)+ Path.DirectorySeparatorChar + Path.GetFileNameWithoutExtension(logfile) + ".txt");
|
|
|
|
while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length)
|
|
{
|
|
// bar moves to 100 % in this step
|
|
progressBar1.Value = (int)((float)mine.logplaybackfile.BaseStream.Position / (float)mine.logplaybackfile.BaseStream.Length * 100.0f / 1.0f);
|
|
|
|
progressBar1.Refresh();
|
|
//Application.DoEvents();
|
|
|
|
byte[] packet = mine.readPacket();
|
|
string text = "";
|
|
mine.DebugPacket(packet, ref text);
|
|
|
|
sw.Write(mine.lastlogread +" "+text);
|
|
}
|
|
|
|
sw.Close();
|
|
|
|
progressBar1.Value = 100;
|
|
|
|
mine.logreadmode = false;
|
|
mine.logplaybackfile.Close();
|
|
mine.logplaybackfile = null;
|
|
|
|
}
|
|
}
|
|
}
|
|
|
|
private void BUT_graphmavlog_Click(object sender, EventArgs e)
|
|
{
|
|
|
|
//http://devreminder.wordpress.com/net/net-framework-fundamentals/c-dynamic-math-expression-evaluation/
|
|
//http://www.c-sharpcorner.com/UploadFile/mgold/CodeDomCalculator08082005003253AM/CodeDomCalculator.aspx
|
|
|
|
//string mathExpression = "(1+1)*3";
|
|
//Console.WriteLine(String.Format("{0}={1}",mathExpression, Evaluate(mathExpression)));
|
|
|
|
|
|
OpenFileDialog openFileDialog1 = new OpenFileDialog();
|
|
try
|
|
{
|
|
// openFileDialog1.InitialDirectory = Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + @"logs" + Path.DirectorySeparatorChar;
|
|
}
|
|
catch { } // incase dir doesnt exist
|
|
openFileDialog1.Filter = "*.tlog|*.tlog";
|
|
openFileDialog1.FilterIndex = 2;
|
|
openFileDialog1.RestoreDirectory = true;
|
|
openFileDialog1.Multiselect = false;
|
|
|
|
|
|
if (openFileDialog1.ShowDialog() == DialogResult.OK)
|
|
{
|
|
if (MainV2.comPort.logplaybackfile != null)
|
|
{
|
|
MainV2.comPort.logreadmode = false;
|
|
MainV2.comPort.logplaybackfile.Close();
|
|
}
|
|
|
|
List<string> fields = GetLogFileValidFields(openFileDialog1.FileName);
|
|
|
|
zg1.GraphPane.CurveList.Clear();
|
|
|
|
//GetLogFileData(zg1, openFileDialog1.FileName, fields);
|
|
|
|
try
|
|
{
|
|
// fix new line types
|
|
ThemeManager.ApplyThemeTo(this);
|
|
|
|
zg1.Invalidate();
|
|
zg1.AxisChange();
|
|
}
|
|
catch { }
|
|
}
|
|
}
|
|
|
|
static int[] ColourValues = new int[] {
|
|
0xFF0000,0x00FF00,0x0000FF,0xFFFF00,0xFF00FF,0x00FFFF,0x000000,
|
|
0x800000,0x008000,0x000080,0x808000,0x800080,0x008080,0x808080,
|
|
0xC00000,0x00C000,0x0000C0,0xC0C000,0xC000C0,0x00C0C0,0xC0C0C0,
|
|
0x400000,0x004000,0x000040,0x404000,0x400040,0x004040,0x404040,
|
|
0x200000,0x002000,0x000020,0x202000,0x200020,0x002020,0x202020,
|
|
0x600000,0x006000,0x000060,0x606000,0x600060,0x006060,0x606060,
|
|
0xA00000,0x00A000,0x0000A0,0xA0A000,0xA000A0,0x00A0A0,0xA0A0A0,
|
|
0xE00000,0x00E000,0x0000E0,0xE0E000,0xE000E0,0x00E0E0,0xE0E0E0,
|
|
};
|
|
|
|
private List<string> GetLogFileValidFields(string logfile)
|
|
{
|
|
Form selectform = SelectDataToGraphForm();
|
|
|
|
Hashtable seenIt = new Hashtable();
|
|
|
|
selection = new List<string>();
|
|
|
|
options = new List<string>();
|
|
|
|
this.datappl.Clear();
|
|
this.packetdata.Clear();
|
|
|
|
colorStep = 0;
|
|
|
|
{
|
|
|
|
MAVLink MavlinkInterface = new MAVLink();
|
|
try
|
|
{
|
|
MavlinkInterface.logplaybackfile = new BinaryReader(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.Read));
|
|
}
|
|
catch (Exception ex) { log.Debug(ex.ToString()); CustomMessageBox.Show("Log Can not be opened. Are you still connected?"); return options; }
|
|
MavlinkInterface.logreadmode = true;
|
|
|
|
MavlinkInterface.packets.Initialize(); // clear
|
|
|
|
CurrentState cs = new CurrentState();
|
|
|
|
// to get first packet time
|
|
MavlinkInterface.readPacket();
|
|
|
|
DateTime startlogtime = MavlinkInterface.lastlogread;
|
|
|
|
while (MavlinkInterface.logplaybackfile.BaseStream.Position < MavlinkInterface.logplaybackfile.BaseStream.Length)
|
|
{
|
|
progressBar1.Value = (int)((float)MavlinkInterface.logplaybackfile.BaseStream.Position / (float)MavlinkInterface.logplaybackfile.BaseStream.Length * 100.0f);
|
|
progressBar1.Refresh();
|
|
|
|
byte[] packet = MavlinkInterface.readPacket();
|
|
|
|
cs.datetime = MavlinkInterface.lastlogread;
|
|
|
|
cs.UpdateCurrentSettings(null, true, MavlinkInterface);
|
|
|
|
object data = MavlinkInterface.DebugPacket(packet, false);
|
|
|
|
if (data == null)
|
|
{
|
|
log.Info("No info on packet");
|
|
continue;
|
|
}
|
|
|
|
Type test = data.GetType();
|
|
|
|
|
|
if (true) {
|
|
string packetname = test.Name.Replace("mavlink_", "").Replace("_t", "").ToUpper();
|
|
|
|
if (!packetdata.ContainsKey(packetname))
|
|
{
|
|
packetdata[packetname] = new Dictionary<double,object>();
|
|
}
|
|
|
|
Dictionary<double, object> temp = (Dictionary<double, object>)packetdata[packetname];
|
|
|
|
double time = (MavlinkInterface.lastlogread - startlogtime).TotalMilliseconds / 1000.0;
|
|
|
|
temp[time] = data;
|
|
}
|
|
|
|
foreach (var field in test.GetFields())
|
|
{
|
|
// field.Name has the field's name.
|
|
|
|
object fieldValue = field.GetValue(data); // Get value
|
|
|
|
if (field.FieldType.IsArray)
|
|
{
|
|
|
|
}
|
|
else
|
|
{
|
|
if (!seenIt.ContainsKey(field.DeclaringType.Name + "." + field.Name))
|
|
{
|
|
seenIt[field.DeclaringType.Name + "." + field.Name] = 1;
|
|
//AddDataOption(selectform, field.Name + " " + field.DeclaringType.Name);
|
|
options.Add(field.DeclaringType.Name + "." + field.Name);
|
|
}
|
|
|
|
if (!this.datappl.ContainsKey(field.Name + " " + field.DeclaringType.Name))
|
|
this.datappl[field.Name + " " + field.DeclaringType.Name] = new PointPairList();
|
|
|
|
PointPairList list = ((PointPairList)this.datappl[field.Name + " " + field.DeclaringType.Name]);
|
|
|
|
object value = fieldValue;
|
|
// seconds scale
|
|
double time = (MavlinkInterface.lastlogread - startlogtime).TotalMilliseconds / 1000.0;
|
|
|
|
if (value.GetType() == typeof(Single))
|
|
{
|
|
list.Add(time, (Single)field.GetValue(data));
|
|
}
|
|
else if (value.GetType() == typeof(short))
|
|
{
|
|
list.Add(time, (short)field.GetValue(data));
|
|
}
|
|
else if (value.GetType() == typeof(ushort))
|
|
{
|
|
list.Add(time, (ushort)field.GetValue(data));
|
|
}
|
|
else if (value.GetType() == typeof(byte))
|
|
{
|
|
list.Add(time, (byte)field.GetValue(data));
|
|
}
|
|
else if (value.GetType() == typeof(Int32))
|
|
{
|
|
list.Add(time, (Int32)field.GetValue(data));
|
|
}
|
|
else if (value.GetType() == typeof(ulong))
|
|
{
|
|
list.Add(time, (ulong)field.GetValue(data));
|
|
}
|
|
else
|
|
{
|
|
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
MavlinkInterface.logreadmode = false;
|
|
MavlinkInterface.logplaybackfile.Close();
|
|
MavlinkInterface.logplaybackfile = null;
|
|
|
|
try
|
|
{
|
|
|
|
dospecial("GPS_RAW");
|
|
|
|
|
|
addMagField();
|
|
|
|
addDistHome();
|
|
|
|
}
|
|
catch (Exception ex) { log.Info(ex.ToString()); }
|
|
|
|
// custom sort based on packet name
|
|
//options.Sort(delegate(string c1, string c2) { return String.Compare(c1.Substring(0,c1.IndexOf('.')),c2.Substring(0,c2.IndexOf('.')));});
|
|
|
|
// this needs sorting
|
|
string lastitem = "";
|
|
foreach (string item in options)
|
|
{
|
|
var items = item.Split('.');
|
|
if (items[0] != lastitem)
|
|
AddHeader(selectform, items[0].Replace("mavlink_","").Replace("_t","").ToUpper());
|
|
AddDataOption(selectform, items[1] + " " + items[0]);
|
|
lastitem = items[0];
|
|
}
|
|
|
|
selectform.Show();
|
|
|
|
progressBar1.Value = 100;
|
|
|
|
}
|
|
|
|
return selection;
|
|
}
|
|
|
|
public static T Cast<T>(object o)
|
|
{
|
|
return (T)o;
|
|
}
|
|
|
|
void dospecial(string PacketName)
|
|
{
|
|
Dictionary<double, object> temp = null;
|
|
|
|
try
|
|
{
|
|
temp = (Dictionary<double, object>)packetdata[PacketName];
|
|
}
|
|
catch
|
|
{
|
|
CustomMessageBox.Show("Bad PacketName");
|
|
return;
|
|
}
|
|
|
|
string code = @"
|
|
|
|
public double stage(object inp) {
|
|
return getAltAboveHome((MAVLink.mavlink_gps_raw_int_t) inp);
|
|
}
|
|
|
|
public double getAltAboveHome(MAVLink.mavlink_gps_raw_int_t gps)
|
|
{
|
|
if (customforusenumber == -1 && gps.fix_type != 2)
|
|
customforusenumber = gps.alt;
|
|
|
|
return gps.alt - customforusenumber;
|
|
}
|
|
";
|
|
|
|
// build the class using codedom
|
|
CodeGen.BuildClass(code);
|
|
|
|
// compile the class into an in-memory assembly.
|
|
// if it doesn't compile, show errors in the window
|
|
CompilerResults results = CodeGen.CompileAssembly();
|
|
|
|
if (results != null && results.CompiledAssembly != null)
|
|
{
|
|
string field = "Custom Custom"; // reverse bellow
|
|
|
|
options.Add("Custom.Custom");
|
|
|
|
this.datappl[field] = new PointPairList();
|
|
|
|
|
|
|
|
MethodInfo mi = RunCode(results);
|
|
|
|
|
|
// from here
|
|
PointPairList result = (PointPairList)this.datappl[field];
|
|
|
|
try
|
|
{
|
|
|
|
object assemblyInstance = results.CompiledAssembly.CreateInstance("ExpressionEvaluator.Calculator");
|
|
|
|
foreach (double time in temp.Keys)
|
|
{
|
|
result.Add(time, (double)mi.Invoke(assemblyInstance, new object[] { temp[time] }));
|
|
}
|
|
}
|
|
catch { }
|
|
}
|
|
else
|
|
{
|
|
CustomMessageBox.Show("Compile Failed");
|
|
return;
|
|
}
|
|
|
|
object answer = CodeGen.runCode(code);
|
|
|
|
Console.WriteLine(answer);
|
|
}
|
|
|
|
public MethodInfo RunCode(CompilerResults results)
|
|
{
|
|
Assembly executingAssembly = results.CompiledAssembly;
|
|
try
|
|
{
|
|
//cant call the entry method if the assembly is null
|
|
if (executingAssembly != null)
|
|
{
|
|
object assemblyInstance = executingAssembly.CreateInstance("ExpressionEvaluator.Calculator");
|
|
//Use reflection to call the static Main function
|
|
|
|
Module[] modules = executingAssembly.GetModules(false);
|
|
Type[] types = modules[0].GetTypes();
|
|
|
|
//loop through each class that was defined and look for the first occurrance of the entry point method
|
|
foreach (Type type in types)
|
|
{
|
|
MethodInfo[] mis = type.GetMethods();
|
|
foreach (MethodInfo mi in mis)
|
|
{
|
|
if (mi.Name == "stage")
|
|
{
|
|
return mi;
|
|
//object result = mi.Invoke(assemblyInstance, null);
|
|
//return result.ToString();
|
|
}
|
|
}
|
|
}
|
|
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
Console.WriteLine("Error: An exception occurred while executing the script", ex);
|
|
}
|
|
return null;
|
|
}
|
|
|
|
PointPairList GetValuesForField(string name)
|
|
{
|
|
// eg RAW_IMU.xmag to "xmag mavlink_raw_imu_t"
|
|
|
|
string[] items = name.ToLower().Split(new char[] {'.',' '});
|
|
|
|
PointPairList list = ((PointPairList)this.datappl[items[1] + " mavlink_" + items[0] + "_t"]);
|
|
|
|
return list;
|
|
}
|
|
|
|
void addMagField()
|
|
{
|
|
string field = "mag_field Custom";
|
|
|
|
options.Add("Custom.mag_field");
|
|
|
|
this.datappl[field] = new PointPairList();
|
|
|
|
PointPairList list = ((PointPairList)this.datappl[field]);
|
|
|
|
PointPairList listx = ((PointPairList)this.datappl["xmag mavlink_raw_imu_t"]);
|
|
PointPairList listy = ((PointPairList)this.datappl["ymag mavlink_raw_imu_t"]);
|
|
PointPairList listz = ((PointPairList)this.datappl["zmag mavlink_raw_imu_t"]);
|
|
|
|
//(float)Math.Sqrt(Math.Pow(mx, 2) + Math.Pow(my, 2) + Math.Pow(mz, 2));
|
|
|
|
for (int a = 0; a < listx.Count; a++)
|
|
{
|
|
|
|
double ans = Math.Sqrt(Math.Pow(listx[a].Y, 2) + Math.Pow(listy[a].Y, 2) + Math.Pow(listz[a].Y, 2));
|
|
|
|
//Console.WriteLine("{0} {1} {2} {3}", ans, listx[a].Y, listy[a].Y, listz[a].Y);
|
|
|
|
list.Add(listx[a].X, ans);
|
|
}
|
|
}
|
|
|
|
void addDistHome()
|
|
{
|
|
string field = "dist_home Custom";
|
|
|
|
options.Add("Custom.dist_home");
|
|
|
|
this.datappl[field] = new PointPairList();
|
|
|
|
PointLatLngAlt home = new PointLatLngAlt();
|
|
|
|
PointPairList list = ((PointPairList)this.datappl[field]);
|
|
|
|
PointPairList listfix = ((PointPairList)this.datappl["fix_type mavlink_gps_raw_int_t"]);
|
|
PointPairList listx = ((PointPairList)this.datappl["lat mavlink_gps_raw_int_t"]);
|
|
PointPairList listy = ((PointPairList)this.datappl["lon mavlink_gps_raw_int_t"]);
|
|
PointPairList listz = ((PointPairList)this.datappl["alt mavlink_gps_raw_int_t"]);
|
|
|
|
for (int a = 0; a < listfix.Count; a++)
|
|
{
|
|
if (listfix[a].Y == 3)
|
|
{
|
|
home = new PointLatLngAlt(listx[a].Y / 10000000.0, listy[a].Y / 10000000.0, listz[a].Y / 1000.0, "Home");
|
|
break;
|
|
}
|
|
}
|
|
|
|
//(float)Math.Sqrt(Math.Pow(mx, 2) + Math.Pow(my, 2) + Math.Pow(mz, 2));
|
|
|
|
for (int a = 0; a < listx.Count; a++)
|
|
{
|
|
|
|
double ans = home.GetDistance(new PointLatLngAlt(listx[a].Y / 10000000.0, listy[a].Y / 10000000.0, listz[a].Y / 1000.0, "Point"));
|
|
|
|
//Console.WriteLine("{0} {1} {2} {3}", ans, listx[a].Y, listy[a].Y, listz[a].Y);
|
|
|
|
list.Add(listx[a].X, ans);
|
|
}
|
|
}
|
|
|
|
private void AddHeader(Form selectform, string Name)
|
|
{
|
|
System.Windows.Forms.Label lbl_head = new System.Windows.Forms.Label();
|
|
|
|
log.Info("Add Header " + Name);
|
|
|
|
lbl_head.Text = Name;
|
|
lbl_head.Name = Name;
|
|
lbl_head.Location = new System.Drawing.Point(x, y);
|
|
lbl_head.Size = new System.Drawing.Size(100, 20);
|
|
|
|
selectform.Controls.Add(lbl_head);
|
|
|
|
Application.DoEvents();
|
|
|
|
x += 0;
|
|
y += 20;
|
|
|
|
if (y > selectform.Height - 60)
|
|
{
|
|
x += 100;
|
|
y = 10;
|
|
|
|
selectform.Width = x + 100;
|
|
}
|
|
}
|
|
|
|
private void AddDataOption(Form selectform, string Name)
|
|
{
|
|
|
|
CheckBox chk_box = new CheckBox();
|
|
|
|
log.Info("Add Option " + Name);
|
|
|
|
chk_box.Text = Name;
|
|
chk_box.Name = Name;
|
|
chk_box.Location = new System.Drawing.Point(x, y);
|
|
chk_box.Size = new System.Drawing.Size(100, 20);
|
|
chk_box.CheckedChanged += new EventHandler(chk_box_CheckedChanged);
|
|
chk_box.MouseUp += new MouseEventHandler(chk_box_MouseUp);
|
|
|
|
selectform.Controls.Add(chk_box);
|
|
|
|
Application.DoEvents();
|
|
|
|
x += 0;
|
|
y += 20;
|
|
|
|
if (y > selectform.Height - 60)
|
|
{
|
|
x += 100;
|
|
y = 10;
|
|
|
|
selectform.Width = x + 100;
|
|
}
|
|
}
|
|
|
|
void chk_box_MouseUp(object sender, MouseEventArgs e)
|
|
{
|
|
if (e.Button == System.Windows.Forms.MouseButtons.Right)
|
|
{
|
|
// dont action a already draw item
|
|
if (!((CheckBox)sender).Checked)
|
|
{
|
|
rightclick = true;
|
|
((CheckBox)sender).Checked = true;
|
|
}
|
|
else
|
|
{
|
|
((CheckBox)sender).Checked = false;
|
|
}
|
|
rightclick = false;
|
|
}
|
|
}
|
|
|
|
int colorStep = 0;
|
|
bool rightclick = false;
|
|
|
|
void chk_box_CheckedChanged(object sender, EventArgs e)
|
|
{
|
|
if (((CheckBox)sender).Checked)
|
|
{
|
|
selection.Add(((CheckBox)sender).Name);
|
|
|
|
LineItem myCurve;
|
|
|
|
int colorvalue = ColourValues[colorStep % ColourValues.Length];
|
|
colorStep++;
|
|
|
|
myCurve = zg1.GraphPane.AddCurve(((CheckBox)sender).Name.Replace("mavlink_", ""), (PointPairList)datappl[((CheckBox)sender).Name], Color.FromArgb(unchecked(colorvalue + (int)0xff000000)), SymbolType.None);
|
|
|
|
myCurve.Tag = ((CheckBox)sender).Name;
|
|
|
|
if (myCurve.Tag.ToString() == "roll mavlink_attitude_t" ||
|
|
myCurve.Tag.ToString() == "pitch mavlink_attitude_t" ||
|
|
myCurve.Tag.ToString() == "yaw mavlink_attitude_t")
|
|
{
|
|
PointPairList ppl = new PointPairList((PointPairList)datappl[((CheckBox)sender).Name]);
|
|
for (int a = 0; a < ppl.Count; a++)
|
|
{
|
|
ppl[a].Y = ppl[a].Y * (180.0 / Math.PI);
|
|
}
|
|
|
|
myCurve.Points = ppl;
|
|
}
|
|
|
|
double xMin, xMax, yMin, yMax;
|
|
|
|
myCurve.GetRange(out xMin, out xMax, out yMin, out yMax, true, false, zg1.GraphPane);
|
|
|
|
if (rightclick || (yMin > 850 && yMax < 2100 && yMin < 2100))
|
|
{
|
|
myCurve.IsY2Axis = true;
|
|
myCurve.YAxisIndex = 0;
|
|
zg1.GraphPane.Y2Axis.IsVisible = true;
|
|
|
|
myCurve.Label.Text = myCurve.Label.Text + "-R";
|
|
}
|
|
}
|
|
else
|
|
{
|
|
selection.Remove(((CheckBox)sender).Name);
|
|
foreach (var item in zg1.GraphPane.CurveList)
|
|
{
|
|
if (item.Tag.ToString() == ((CheckBox)sender).Name)
|
|
{
|
|
zg1.GraphPane.CurveList.Remove(item);
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
try
|
|
{
|
|
// fix new line types
|
|
ThemeManager.ApplyThemeTo(this);
|
|
|
|
zg1.GraphPane.XAxis.AxisGap = 0;
|
|
|
|
zg1.Invalidate();
|
|
zg1.AxisChange();
|
|
}
|
|
catch { }
|
|
}
|
|
|
|
int x = 10;
|
|
int y = 10;
|
|
|
|
private Form SelectDataToGraphForm()
|
|
{
|
|
Form selectform = new Form()
|
|
{
|
|
Name = "select",
|
|
Width = 50,
|
|
Height = 500,
|
|
Text = "Graph This",
|
|
TopLevel = true
|
|
};
|
|
|
|
x = 10;
|
|
y = 10;
|
|
|
|
AddHeader(selectform, "Left Click");
|
|
AddHeader(selectform, "Left Axis");
|
|
AddHeader(selectform, "Right Click");
|
|
AddHeader(selectform, "Right Axis");
|
|
|
|
ThemeManager.ApplyThemeTo(selectform);
|
|
|
|
return selectform;
|
|
}
|
|
|
|
private void BUT_convertcsv_Click(object sender, EventArgs e)
|
|
{
|
|
OpenFileDialog openFileDialog1 = new OpenFileDialog();
|
|
openFileDialog1.Filter = "*.tlog|*.tlog";
|
|
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)
|
|
{
|
|
|
|
MAVLink mine = new MAVLink();
|
|
try
|
|
{
|
|
mine.logplaybackfile = new BinaryReader(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.Read));
|
|
}
|
|
catch (Exception ex) { log.Debug(ex.ToString()); CustomMessageBox.Show("Log Can not be opened. Are you still connected?"); return; }
|
|
mine.logreadmode = true;
|
|
|
|
mine.packets.Initialize(); // clear
|
|
|
|
StreamWriter sw = new StreamWriter(Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar + Path.GetFileNameWithoutExtension(logfile) + ".csv");
|
|
|
|
while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length)
|
|
{
|
|
// bar moves to 100 % in this step
|
|
progressBar1.Value = (int)((float)mine.logplaybackfile.BaseStream.Position / (float)mine.logplaybackfile.BaseStream.Length * 100.0f / 1.0f);
|
|
|
|
progressBar1.Refresh();
|
|
//Application.DoEvents();
|
|
|
|
byte[] packet = mine.readPacket();
|
|
string text = "";
|
|
mine.DebugPacket(packet, ref text,true,",");
|
|
|
|
sw.Write(mine.lastlogread + "," + text);
|
|
}
|
|
|
|
sw.Close();
|
|
|
|
progressBar1.Value = 100;
|
|
|
|
mine.logreadmode = false;
|
|
mine.logplaybackfile.Close();
|
|
mine.logplaybackfile = null;
|
|
|
|
}
|
|
}
|
|
}
|
|
|
|
private void BUT_paramsfromlog_Click(object sender, EventArgs e)
|
|
{
|
|
OpenFileDialog openFileDialog1 = new OpenFileDialog();
|
|
openFileDialog1.Filter = "*.tlog|*.tlog";
|
|
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)
|
|
{
|
|
if (MainV2.comPort.logplaybackfile != null)
|
|
{
|
|
MainV2.comPort.logreadmode = false;
|
|
MainV2.comPort.logplaybackfile.Close();
|
|
}
|
|
|
|
foreach (string logfile in openFileDialog1.FileNames)
|
|
{
|
|
|
|
MAVLink mine = new MAVLink();
|
|
try
|
|
{
|
|
mine.logplaybackfile = new BinaryReader(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.Read));
|
|
}
|
|
catch (Exception ex) { log.Debug(ex.ToString()); CustomMessageBox.Show("Log Can not be opened. Are you still connected?"); return; }
|
|
|
|
mine.logreadmode = true;
|
|
|
|
mine.packets.Initialize(); // clear
|
|
|
|
StreamWriter sw = new StreamWriter(Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar + Path.GetFileNameWithoutExtension(logfile) + ".param");
|
|
|
|
// bar moves to 100 % in this step
|
|
progressBar1.Value = (int)((float)mine.logplaybackfile.BaseStream.Position / (float)mine.logplaybackfile.BaseStream.Length * 100.0f / 1.0f);
|
|
|
|
progressBar1.Refresh();
|
|
//Application.DoEvents();
|
|
|
|
mine.getParamList();
|
|
|
|
foreach (string item in mine.param.Keys)
|
|
{
|
|
sw.WriteLine(item + "\t" + mine.param[item]);
|
|
}
|
|
|
|
sw.Close();
|
|
|
|
progressBar1.Value = 100;
|
|
|
|
mine.logreadmode = false;
|
|
mine.logplaybackfile.Close();
|
|
mine.logplaybackfile = null;
|
|
|
|
CustomMessageBox.Show("File Saved with log file");
|
|
}
|
|
}
|
|
}
|
|
|
|
private void BUT_getwpsfromlog_Click(object sender, EventArgs e)
|
|
{
|
|
OpenFileDialog openFileDialog1 = new OpenFileDialog();
|
|
openFileDialog1.Filter = "*.tlog|*.tlog";
|
|
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)
|
|
{
|
|
if (MainV2.comPort.logplaybackfile != null)
|
|
{
|
|
MainV2.comPort.logreadmode = false;
|
|
MainV2.comPort.logplaybackfile.Close();
|
|
}
|
|
|
|
foreach (string logfile in openFileDialog1.FileNames)
|
|
{
|
|
|
|
int wplists = 0;
|
|
|
|
MAVLink mine = new MAVLink();
|
|
try
|
|
{
|
|
mine.logplaybackfile = new BinaryReader(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.Read));
|
|
}
|
|
catch (Exception ex) { log.Debug(ex.ToString()); CustomMessageBox.Show("Log Can not be opened. Are you still connected?"); return; }
|
|
|
|
mine.logreadmode = true;
|
|
|
|
mine.packets.Initialize(); // clear
|
|
|
|
while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length)
|
|
{
|
|
// bar moves to 100 % in this step
|
|
progressBar1.Value = (int)((float)mine.logplaybackfile.BaseStream.Position / (float)mine.logplaybackfile.BaseStream.Length * 100.0f / 1.0f);
|
|
|
|
progressBar1.Refresh();
|
|
//Application.DoEvents();
|
|
byte count = 0;
|
|
try
|
|
{
|
|
count = mine.getWPCount();
|
|
}
|
|
catch { }
|
|
|
|
if (count == 0)
|
|
{
|
|
continue;
|
|
}
|
|
|
|
StreamWriter sw = new StreamWriter(Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar + Path.GetFileNameWithoutExtension(logfile) + "-" + wplists + ".txt");
|
|
|
|
sw.WriteLine("QGC WPL 110");
|
|
for (ushort a = 0; a < count; a++)
|
|
{
|
|
Locationwp wp = mine.getWP(a);
|
|
//sw.WriteLine(item + "\t" + mine.param[item]);
|
|
byte mode = (byte)wp.id;
|
|
|
|
sw.Write((a + 1)); // seq
|
|
sw.Write("\t" + 0); // current
|
|
sw.Write("\t" + (byte)MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT); //frame
|
|
sw.Write("\t" + mode);
|
|
sw.Write("\t" + wp.p1.ToString("0.000000", new System.Globalization.CultureInfo("en-US")));
|
|
sw.Write("\t" + wp.p2.ToString("0.000000", new System.Globalization.CultureInfo("en-US")));
|
|
sw.Write("\t" + wp.p3.ToString("0.000000", new System.Globalization.CultureInfo("en-US")));
|
|
sw.Write("\t" + wp.p4.ToString("0.000000", new System.Globalization.CultureInfo("en-US")));
|
|
sw.Write("\t" + wp.lat.ToString("0.000000", new System.Globalization.CultureInfo("en-US")));
|
|
sw.Write("\t" + wp.lng.ToString("0.000000", new System.Globalization.CultureInfo("en-US")));
|
|
sw.Write("\t" + (wp.alt / MainV2.cs.multiplierdist).ToString("0.000000", new System.Globalization.CultureInfo("en-US")));
|
|
sw.Write("\t" + 1);
|
|
sw.WriteLine("");
|
|
}
|
|
|
|
sw.Close();
|
|
wplists++;
|
|
}
|
|
|
|
progressBar1.Value = 100;
|
|
|
|
mine.logreadmode = false;
|
|
mine.logplaybackfile.Close();
|
|
mine.logplaybackfile = null;
|
|
|
|
CustomMessageBox.Show("File Saved with log file");
|
|
|
|
}
|
|
}
|
|
}
|
|
}
|
|
} |