private List <string> GetLogFileValidFields(string logfile)
{
Form selectform = SelectDataToGraphForm();
Hashtable seenIt = new Hashtable();
{
MAVLink mine = new MAVLink();
mine.logplaybackfile = new BinaryReader(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.Read));
mine.logreadmode = true;
mine.packets.Initialize(); // clear
while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length)
{
progressBar1.Value = (int)((float)mine.logplaybackfile.BaseStream.Position / (float)mine.logplaybackfile.BaseStream.Length * 100.0f);
this.Refresh();
byte[] packet = mine.readPacket();
object data = mine.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))
{
AddDataOption(selectform, field.Name + " " + field.DeclaringType.Name);
seenIt[field.DeclaringType.Name + "." + field.Name] = 1;
}
}
}
}
mine.logreadmode = false;
mine.logplaybackfile.Close();
mine.logplaybackfile = null;
selectform.ShowDialog();
progressBar1.Value = 100;
}
return(selection);
}
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
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 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();
MavlinkInterface.logplaybackfile = new BinaryReader(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.Read));
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);
}
/// <summary>
/// Processes a tlog to get the offsets - creates dxf of data
/// </summary>
/// <param name="fn">Filename</param>
/// <returns>Offsets</returns>
public static double[] getOffsets(string fn, int throttleThreshold = 0)
{
// based off tridge's work
string logfile = fn;
// old method
float minx = 0;
float maxx = 0;
float miny = 0;
float maxy = 0;
float minz = 0;
float maxz = 0;
// this is for a dxf
Polyline3dVertex vertex;
List <Polyline3dVertex> vertexes = new List <Polyline3dVertex>();
// data storage
Tuple <float, float, float> offset = new Tuple <float, float, float>(0, 0, 0);
List <Tuple <float, float, float> > data = new List <Tuple <float, float, float> >();
Hashtable filter = new Hashtable();
// track data to use
bool useData = false;
log.Info("Start log: " + DateTime.Now);
MAVLink mine = new MAVLink();
mine.logplaybackfile = new BinaryReader(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.Read));
mine.logreadmode = true;
mine.packets.Initialize(); // clear
// gather data
while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length)
{
byte[] packetraw = mine.readPacket();
var packet = mine.DebugPacket(packetraw, false);
// this is for packets we dont know about
if (packet == null)
{
continue;
}
if (packet.GetType() == typeof(MAVLink.mavlink_vfr_hud_t))
{
if (((MAVLink.mavlink_vfr_hud_t)packet).throttle >= throttleThreshold)
{
useData = true;
}
else
{
useData = false;
}
}
if (packet.GetType() == typeof(MAVLink.mavlink_sensor_offsets_t))
{
offset = new Tuple <float, float, float>(
((MAVLink.mavlink_sensor_offsets_t)packet).mag_ofs_x,
((MAVLink.mavlink_sensor_offsets_t)packet).mag_ofs_y,
((MAVLink.mavlink_sensor_offsets_t)packet).mag_ofs_z);
}
else if (packet.GetType() == typeof(MAVLink.mavlink_raw_imu_t) && useData)
{
int div = 20;
// fox dxf
vertex = new Polyline3dVertex(new Vector3f(
((MAVLink.mavlink_raw_imu_t)packet).xmag - offset.Item1,
((MAVLink.mavlink_raw_imu_t)packet).ymag - offset.Item2,
((MAVLink.mavlink_raw_imu_t)packet).zmag - offset.Item3)
);
vertexes.Add(vertex);
// for old method
setMinorMax(((MAVLink.mavlink_raw_imu_t)packet).xmag - offset.Item1, ref minx, ref maxx);
setMinorMax(((MAVLink.mavlink_raw_imu_t)packet).ymag - offset.Item2, ref miny, ref maxy);
setMinorMax(((MAVLink.mavlink_raw_imu_t)packet).zmag - offset.Item3, ref minz, ref maxz);
// for new lease sq
string item = (int)(((MAVLink.mavlink_raw_imu_t)packet).xmag / div) + "," +
(int)(((MAVLink.mavlink_raw_imu_t)packet).ymag / div) + "," +
(int)(((MAVLink.mavlink_raw_imu_t)packet).zmag / div);
if (filter.ContainsKey(item))
{
filter[item] = (int)filter[item] + 1;
if ((int)filter[item] > 3)
{
continue;
}
}
else
{
filter[item] = 1;
}
data.Add(new Tuple <float, float, float>(
((MAVLink.mavlink_raw_imu_t)packet).xmag - offset.Item1,
((MAVLink.mavlink_raw_imu_t)packet).ymag - offset.Item2,
((MAVLink.mavlink_raw_imu_t)packet).zmag - offset.Item3));
}
}
log.Info("Log Processed " + DateTime.Now);
Console.WriteLine("Extracted " + data.Count + " data points");
Console.WriteLine("Current offset: " + offset);
mine.logreadmode = false;
mine.logplaybackfile.Close();
mine.logplaybackfile = null;
if (data.Count < 10)
{
CustomMessageBox.Show("Log does not contain enough data");
throw new Exception("Not Enough Data");
}
double[] x = LeastSq(data);
System.Console.WriteLine("Old Method {0} {1} {2}", -(maxx + minx) / 2, -(maxy + miny) / 2, -(maxz + minz) / 2);
log.Info("Least Sq Done " + DateTime.Now);
// create a dxf for those who want to "see" the calibration
DxfDocument dxf = new DxfDocument();
Polyline3d polyline = new Polyline3d(vertexes, true);
polyline.Layer = new Layer("polyline3d");
polyline.Layer.Color.Index = 24;
dxf.AddEntity(polyline);
Point pnt = new Point(new Vector3f(-offset.Item1, -offset.Item2, -offset.Item3));
pnt.Layer = new Layer("old offset");
pnt.Layer.Color.Index = 22;
dxf.AddEntity(pnt);
pnt = new Point(new Vector3f(-(float)x[0], -(float)x[1], -(float)x[2]));
pnt.Layer = new Layer("new offset");
pnt.Layer.Color.Index = 21;
dxf.AddEntity(pnt);
dxf.Save("magoffset.dxf", DxfVersion.AutoCad2000);
log.Info("dxf Done " + DateTime.Now);
Array.Resize <double>(ref x, 3);
return(x);
}
private void GetLogFileData(ZedGraphControl zg1, string logfile, List <string> lookforfields)
{
if (zg1 == null)
{
return;
}
if (lookforfields != null && lookforfields.Count == 0)
{
return;
}
PointPairList[] lists = new PointPairList[lookforfields.Count];
Random rand = new Random();
int step = 0;
// setup display and arrays
for (int a = 0; a < lookforfields.Count; a++)
{
lists[a] = new PointPairList();
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);
}
{
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)
{
}
else
{
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++;
}
}
}
if (appui != DateTime.Now.Second)
{
// cant do entire app as mixes with flightdata timer
this.Refresh();
appui = DateTime.Now.Second;
}
}
MavlinkInterface.logreadmode = false;
MavlinkInterface.logplaybackfile.Close();
MavlinkInterface.logplaybackfile = null;
//writeKML(logfile + ".kml");
progressBar1.Value = 100;
}
}
