ardupilot/Tools/ArdupilotMegaPlanner/MavlinkLog.cs

1039 lines
38 KiB
C#
Raw Normal View History

2011-09-08 22:31:32 -03:00
using System;
using System.Collections.Generic;
using System.Collections;
2011-09-08 22:31:32 -03:00
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
2011-09-08 22:31:32 -03:00
namespace ArdupilotMega
{
public partial class MavlinkLog : Form
{
private static readonly ILog log = LogManager.GetLogger(System.Reflection.MethodBase.GetCurrentMethod().DeclaringType);
2011-09-08 22:31:32 -03:00
List<CurrentState> flightdata = new List<CurrentState>();
List<string> selection = new List<string>();
Hashtable data = new Hashtable();
2011-09-08 22:31:32 -03:00
public MavlinkLog()
{
InitializeComponent();
zg1.GraphPane.YAxis.Title.IsVisible = false;
zg1.GraphPane.Title.IsVisible = true;
zg1.GraphPane.Title.Text = "Mavlink Log Graph";
2011-09-08 22:31:32 -03:00
}
private void writeKML(string filename)
{
SharpKml.Dom.AltitudeMode altmode = SharpKml.Dom.AltitudeMode.Absolute;
if (MainV2.cs.firmware == MainV2.Firmwares.ArduPlane)
{
altmode = SharpKml.Dom.AltitudeMode.Absolute;
}
else if (MainV2.cs.firmware == MainV2.Firmwares.ArduCopter2)
{
altmode = SharpKml.Dom.AltitudeMode.RelativeToGround;
}
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 };
Document kml = new Document();
Tour tour = new Tour() { Name = "First Person View" };
2011-09-08 22:31:32 -03:00
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);
2011-09-08 22:31:32 -03:00
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);
2011-09-08 22:31:32 -03:00
// create sub folders
Folder planes = new Folder();
planes.Name = "Planes";
kml.AddFeature(planes);
Folder points = new Folder();
points.Name = "Points";
kml.AddFeature(points);
2011-09-08 22:31:32 -03:00
// 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;
2011-09-08 22:31:32 -03:00
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;
2011-09-08 22:31:32 -03:00
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;
2011-09-08 22:31:32 -03:00
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;
2011-09-08 22:31:32 -03:00
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;
2011-09-08 22:31:32 -03:00
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> ";
// ]]>";
2011-09-08 22:31:32 -03:00
pmplane.Description = desc;
}
catch { }
SharpKml.Dom.Link link = new SharpKml.Dom.Link();
2011-09-08 22:31:32 -03:00
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);
2011-09-08 22:31:32 -03:00
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);
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.Open(filename,FileMode.Open,FileAccess.Read,FileShare.ReadWrite))
2011-09-08 22:31:32 -03:00
{
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();
2011-09-08 22:31:32 -03:00
}
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)
{
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; }
2011-09-08 22:31:32 -03:00
mine.logreadmode = true;
mine.packets.Initialize(); // clear
2011-09-08 22:31:32 -03:00
CurrentState cs = new CurrentState();
float oldlatlngsum = 0;
2011-09-08 22:31:32 -03:00
int appui = 0;
2011-09-08 22:31:32 -03:00
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();
2011-09-08 22:31:32 -03:00
progressBar1.Refresh();
byte[] packet = mine.readPacket();
cs.datetime = mine.lastlogread;
cs.UpdateCurrentSettings(null, true, mine);
2011-09-08 22:31:32 -03:00
if (appui != DateTime.Now.Second)
{
// cant do entire app as mixes with flightdata timer
this.Refresh();
appui = DateTime.Now.Second;
}
2011-09-08 22:31:32 -03:00
try
{
if (MainV2.speechEngine != null)
MainV2.speechEngine.SpeakAsyncCancelAll();
2011-09-08 22:31:32 -03:00
}
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
2011-09-08 22:31:32 -03:00
&& cs.lat != 0 && cs.lng != 0)
{
Console.WriteLine(cs.lat + " " + cs.lng + " " + cs.alt + " lah " + (float)(cs.lat + cs.lng) + "!=" + oldlatlngsum);
2011-09-08 22:31:32 -03:00
CurrentState cs2 = (CurrentState)cs.Clone();
flightdata.Add(cs2);
oldlatlngsum = (cs.lat + cs.lng);
2011-09-08 22:31:32 -03:00
}
}
mine.logreadmode = false;
mine.logplaybackfile.Close();
mine.logplaybackfile = null;
Application.DoEvents();
2011-09-08 22:31:32 -03:00
writeGPX(logfile);
2011-09-08 22:31:32 -03:00
writeKML(logfile + ".kml");
flightdata.Clear();
progressBar1.Value = 100;
2011-09-08 22:31:32 -03:00
}
}
}
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();
}
2011-09-08 22:31:32 -03:00
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)
{
foreach (string logfile in openFileDialog1.FileNames)
{
MAVLink mine = new MAVLink();
mine.logplaybackfile = new BinaryReader(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.Read));
mine.logreadmode = true;
mine.packets.Initialize(); // clear
2011-09-08 22:31:32 -03:00
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();
2011-09-08 22:31:32 -03:00
byte[] packet = mine.readPacket();
string text = "";
mine.DebugPacket(packet, ref text);
sw.Write(mine.lastlogread +" "+text);
2011-09-08 22:31:32 -03:00
}
sw.Close();
progressBar1.Value = 100;
mine.logreadmode = false;
mine.logplaybackfile.Close();
mine.logplaybackfile = null;
}
}
}
2012-03-03 03:42:41 -04:00
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)));
2012-03-03 03:42:41 -04:00
OpenFileDialog openFileDialog1 = new OpenFileDialog();
openFileDialog1.Filter = "*.tlog|*.tlog";
openFileDialog1.FilterIndex = 2;
openFileDialog1.RestoreDirectory = true;
openFileDialog1.Multiselect = false;
2012-03-03 03:42:41 -04:00
try
{
openFileDialog1.InitialDirectory = Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + @"logs" + Path.DirectorySeparatorChar;
}
catch { } // incase dir doesnt exist
if (openFileDialog1.ShowDialog() == DialogResult.OK)
{
List<string> fields = GetLogFileValidFields(openFileDialog1.FileName);
zg1.GraphPane.CurveList.Clear();
//GetLogFileData(zg1, openFileDialog1.FileName, fields);
try
2012-03-03 03:42:41 -04:00
{
// fix new line types
ThemeManager.ApplyThemeTo(this);
2012-03-03 03:42:41 -04:00
zg1.Invalidate();
zg1.AxisChange();
}
catch { }
}
}
2012-03-03 03:42:41 -04:00
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,
};
2012-03-03 03:42:41 -04:00
private void GetLogFileData(ZedGraphControl zg1, string logfile, List<string> lookforfields)
{
if (zg1 == null)
return;
2012-03-03 03:42:41 -04:00
if (lookforfields != null && lookforfields.Count == 0)
return;
2012-03-03 03:42:41 -04:00
PointPairList[] lists = new PointPairList[lookforfields.Count];
2012-03-03 03:42:41 -04:00
Random rand = new Random();
2012-03-03 03:42:41 -04:00
// setup arrays
for (int a = 0; a < lookforfields.Count; a++)
{
lists[a] = new PointPairList();
}
{
MAVLink MavlinkInterface = new MAVLink();
MavlinkInterface.logplaybackfile = new BinaryReader(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.Read));
MavlinkInterface.logreadmode = true;
MavlinkInterface.packets.Initialize(); // clear
int appui = 0;
// 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();
object data = MavlinkInterface.DebugPacket(packet, false);
Type test = data.GetType();
foreach (var field in test.GetFields())
{
// field.Name has the field's name.
object fieldValue = field.GetValue(data); // Get value
if (field.FieldType.IsArray)
2012-03-03 03:42:41 -04:00
{
}
else
2012-03-03 03:42:41 -04:00
{
string currentitem = field.Name + " " + field.DeclaringType.Name;
int a = 0;
foreach (var lookforfield in lookforfields)
{
if (currentitem == lookforfield)
{
object value = field.GetValue(data);
// seconds scale
double time = (MavlinkInterface.lastlogread - startlogtime).TotalMilliseconds / 1000.0;
if (value.GetType() == typeof(Single))
{
lists[a].Add(time, (Single)field.GetValue(data));
}
else if (value.GetType() == typeof(short))
{
lists[a].Add(time, (short)field.GetValue(data));
}
else if (value.GetType() == typeof(ushort))
{
lists[a].Add(time, (ushort)field.GetValue(data));
}
else if (value.GetType() == typeof(byte))
{
lists[a].Add(time, (byte)field.GetValue(data));
}
else if (value.GetType() == typeof(Int32))
{
lists[a].Add(time, (Int32)field.GetValue(data));
}
}
a++;
}
2012-03-03 03:42:41 -04:00
}
}
2012-03-03 03:42:41 -04:00
if (appui != DateTime.Now.Second)
{
// cant do entire app as mixes with flightdata timer
this.Refresh();
appui = DateTime.Now.Second;
}
}
2012-03-03 03:42:41 -04:00
MavlinkInterface.logreadmode = false;
MavlinkInterface.logplaybackfile.Close();
MavlinkInterface.logplaybackfile = null;
2012-03-03 03:42:41 -04:00
//writeKML(logfile + ".kml");
progressBar1.Value = 100;
}
int step = 0;
zg1.GraphPane.AddY2Axis("PWM");
zg1.GraphPane.AddY2Axis("Angle");
//zg1.GraphPane.XAxis.Title.Text = "Seconds";
// setup display and arrays
for (int a = 0; a < lookforfields.Count; a++)
{
LineItem myCurve;
int colorvalue = ColourValues[step % ColourValues.Length];
step++;
myCurve = zg1.GraphPane.AddCurve(lookforfields[a].Replace("__mavlink_", ""), lists[a], Color.FromArgb(unchecked(colorvalue + (int)0xff000000)), SymbolType.None);
double xMin, xMax, yMin, yMax;
myCurve.GetRange(out xMin, out xMax, out yMin, out yMax, true, false, zg1.GraphPane);
if (yMin > 900 && yMax < 2100)
{
myCurve.IsY2Axis = true;
myCurve.YAxisIndex = 0;
zg1.GraphPane.Y2Axis.IsVisible = true;
}
}
}
private List<string> GetLogFileValidFields(string logfile)
{
Form selectform = SelectDataToGraphForm();
Hashtable seenIt = new Hashtable();
selection = new List<string>();
List<string> options = new List<string>();
this.data.Clear();
colorStep = 0;
{
MAVLink MavlinkInterface = new MAVLink();
MavlinkInterface.logplaybackfile = new BinaryReader(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.Read));
MavlinkInterface.logreadmode = true;
MavlinkInterface.packets.Initialize(); // clear
// 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();
object data = MavlinkInterface.DebugPacket(packet, false);
Type test = data.GetType();
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.data.ContainsKey(field.Name + " " + field.DeclaringType.Name))
this.data[field.Name + " " + field.DeclaringType.Name] = new PointPairList();
PointPairList list = ((PointPairList)this.data[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));
}
2012-03-03 03:42:41 -04:00
}
}
}
2012-03-03 03:42:41 -04:00
MavlinkInterface.logreadmode = false;
MavlinkInterface.logplaybackfile.Close();
MavlinkInterface.logplaybackfile = null;
try
{
addMagField(ref options);
}
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('.')));});
2012-03-03 03:42:41 -04:00
// this needs sorting
foreach (string item in options)
{
var items = item.Split('.');
AddDataOption(selectform, items[1] + " " + items[0]);
}
selectform.Show();
2012-03-03 03:42:41 -04:00
progressBar1.Value = 100;
2012-03-03 03:42:41 -04:00
}
2012-03-03 03:42:41 -04:00
return selection;
}
void addMagField(ref List<string> options)
{
string field = "mag_field Custom";
options.Add("Custom.mag_field");
this.data[field] = new PointPairList();
PointPairList list = ((PointPairList)this.data[field]);
PointPairList listx = ((PointPairList)this.data["xmag __mavlink_raw_imu_t"]);
PointPairList listy = ((PointPairList)this.data["ymag __mavlink_raw_imu_t"]);
PointPairList listz = ((PointPairList)this.data["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);
}
}
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);
selectform.Controls.Add(chk_box);
Application.DoEvents();
x += 0;
y += 20;
if (y > selectform.Height - 50)
{
x += 100;
y = 10;
selectform.Width = x + 100;
2012-03-03 03:42:41 -04:00
}
}
int colorStep = 0;
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)data[((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)data[((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 (yMin > 900 && yMax < 2100 && yMin < 2100)
{
myCurve.IsY2Axis = true;
myCurve.YAxisIndex = 0;
zg1.GraphPane.Y2Axis.IsVisible = true;
}
}
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.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;
{
CheckBox chk_box = new CheckBox();
chk_box.Text = "Logarithmic";
chk_box.Name = "Logarithmic";
chk_box.Location = new System.Drawing.Point(x, y);
chk_box.Size = new System.Drawing.Size(100, 20);
//chk_box.CheckedChanged += new EventHandler(chk_log_CheckedChanged);
selectform.Controls.Add(chk_box);
}
y += 20;
ThemeManager.ApplyThemeTo(selectform);
return selectform;
}
2011-09-08 22:31:32 -03:00
}
}