List<string[]> readLog(string fn)
{
if (logcache.Count > 0)
return logcache;
List<string[]> list = new List<string[]>();
if (fn.ToLower().EndsWith("tlog"))
{
MAVLink mine = new MAVLink();
mine.logplaybackfile = new BinaryReader(File.Open(fn, FileMode.Open, FileAccess.Read, FileShare.Read));
mine.logreadmode = true;
mine.MAV.packets.Initialize(); // clear
CurrentState cs = new CurrentState();
string[] oldvalues = {""};
while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length)
{
byte[] packet = mine.readPacket();
cs.datetime = mine.lastlogread;
cs.UpdateCurrentSettings(null, true, mine);
// old
// line "GPS: 82686250, 1, 8, -34.1406480, 118.5441900, 0.0000, 309.1900, 315.9500, 0.0000, 279.1200" string
//Status,Time,NSats,HDop,Lat,Lng,RelAlt,Alt,Spd,GCrs
//GPS, 3, 122732, 10, 0.00, -35.3628880, 149.1621961, 808.90, 810.30, 23.30, 94.04
string[] vals = new string[] { "GPS", "3", (cs.datetime.ToUniversalTime() - new DateTime(cs.datetime.Year,cs.datetime.Month,cs.datetime.Day,0,0,0,DateTimeKind.Utc)).TotalMilliseconds.ToString(),
cs.satcount.ToString(),cs.gpshdop.ToString(),cs.lat.ToString(),cs.lng.ToString(),cs.alt.ToString(),cs.alt.ToString(),cs.groundspeed.ToString(),cs.yaw.ToString()};
if (oldvalues.Length > 2 && oldvalues[latpos] == vals[latpos]
&& oldvalues[lngpos] == vals[lngpos]
&& oldvalues[altpos] == vals[altpos])
continue;
oldvalues = vals;
list.Add(vals);
// 4 5 7
Console.Write((mine.logplaybackfile.BaseStream.Position * 100 / mine.logplaybackfile.BaseStream.Length) + " \r");
}
mine.logplaybackfile.Close();
logcache = list;
return list;
}
StreamReader sr = new StreamReader(fn);
string lasttime = "0";
while (!sr.EndOfStream)
{
string line = sr.ReadLine();
if (line.ToLower().StartsWith("gps"))
{
if (!sr.EndOfStream)
{
string line2 = sr.ReadLine();
if (line2.ToLower().StartsWith("att"))
{
line = string.Concat(line, ",", line2);
}
}
string[] vals = line.Split(new char[] {',',':'});
if (lasttime == vals[timepos])
continue;
lasttime = vals[timepos];
list.Add(vals);
}
}
sr.Close();
sr.Dispose();
logcache = list;
return list;
}
/// <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();
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 new double[] {0}; }
mine.logreadmode = true;
mine.MAV.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");
}
data.Sort(
delegate(Tuple<float, float, float> d1, Tuple<float, float, float> d2)
{
// get distance from 0,0,0
double ans1 = Math.Sqrt(d1.Item1 * d1.Item1 + d1.Item2 * d1.Item2+ d1.Item3 * d1.Item3);
double ans2 = Math.Sqrt(d2.Item1 * d2.Item1 + d2.Item2 * d2.Item2+ d2.Item3 * d2.Item3);
if (ans1 > ans2)
return 1;
if (ans1 < ans2)
return -1;
return 0;
}
);
data.RemoveRange(data.Count - (data.Count / 16), data.Count / 16);
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;
}
void dolog()
{
flightdata.Clear();
MAVLink mine = new MAVLink();
try
{
mine.logplaybackfile = new BinaryReader(File.Open(txt_tlog.Text, FileMode.Open, FileAccess.Read, FileShare.Read));
}
catch { CustomMessageBox.Show("Log Can not be opened. Are you still connected?"); return; }
mine.logreadmode = true;
mine.MAV.packets.Initialize(); // clear
mine.readPacket();
startlogtime = mine.lastlogread;
double oldlatlngsum = 0;
int appui = 0;
while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length)
{
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 + cs.alt) != oldlatlngsum
// && cs.lat != 0 && cs.lng != 0)
DateTime nexttime = mine.lastlogread.AddMilliseconds(-(mine.lastlogread.Millisecond % 100));
if (!flightdata.ContainsKey(nexttime))
{
Console.WriteLine(cs.lat + " " + cs.lng + " " + cs.alt + " lah " + (float)(cs.lat + cs.lng + cs.alt) + "!=" + oldlatlngsum);
CurrentState cs2 = (CurrentState)cs.Clone();
try
{
flightdata.Add(nexttime, cs2);
}
catch { }
oldlatlngsum = (cs.lat + cs.lng + cs.alt);
}
}
mine.logreadmode = false;
mine.logplaybackfile.Close();
mine.logplaybackfile = null;
}
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 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();
mine.logplaybackfile = new BinaryReader(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.Read));
mine.logreadmode = true;
mine.packets.Initialize(); // clear
CurrentState cs = new CurrentState();
float oldlatlngalt = 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.Refresh();
byte[] packet = mine.readPacket();
cs.datetime = mine.lastlogread;
cs.UpdateCurrentSettings(null, true, mine);
try
{
if (MainV2.talk != null)
MainV2.talk.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) != oldlatlngalt
&& cs.lat != 0 && cs.lng != 0)
{
Console.WriteLine(cs.lat + " " + cs.lng + " " + cs.alt + " lah " + (float)(cs.lat + cs.lng + cs.alt) + "!=" + oldlatlngalt);
CurrentState cs2 = (CurrentState)cs.Clone();
flightdata.Add(cs2);
oldlatlngalt = (cs.lat + cs.lng);
}
}
mine.logreadmode = false;
mine.logplaybackfile.Close();
mine.logplaybackfile = null;
writeKML(logfile + ".kml");
progressBar1.Value = 100;
}
}
}
List<string[]> readLog(string fn)
{
List<string[]> list = new List<string[]>();
if (fn.ToLower().EndsWith("tlog"))
{
MAVLink mine = new MAVLink();
mine.logplaybackfile = new BinaryReader(File.Open(fn, FileMode.Open, FileAccess.Read, FileShare.Read));
mine.logreadmode = true;
mine.packets.Initialize(); // clear
CurrentState cs = new CurrentState();
string[] oldvalues = {""};
while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length)
{
byte[] packet = mine.readPacket();
cs.datetime = mine.lastlogread;
cs.UpdateCurrentSettings(null, true, mine);
// line "GPS: 82686250, 1, 8, -34.1406480, 118.5441900, 0.0000, 309.1900, 315.9500, 0.0000, 279.1200" string
string[] vals = new string[] { "GPS", (cs.datetime - new DateTime(cs.datetime.Year,cs.datetime.Month,cs.datetime.Day,0,0,0,DateTimeKind.Local)).TotalMilliseconds.ToString(), "1",
cs.satcount.ToString(),cs.lat.ToString(),cs.lng.ToString(),"0.0",cs.alt.ToString(),cs.alt.ToString(),"0.0",cs.groundcourse.ToString()};
if (oldvalues.Length > 2 && oldvalues[latpos] == vals[latpos]
&& oldvalues[lngpos] == vals[lngpos]
&& oldvalues[altpos] == vals[altpos])
continue;
oldvalues = vals;
list.Add(vals);
// 4 5 7
Console.Write((mine.logplaybackfile.BaseStream.Position * 100 / mine.logplaybackfile.BaseStream.Length) + " \r");
}
mine.logplaybackfile.Close();
return list;
}
StreamReader sr = new StreamReader(fn);
string lasttime = "0";
while (!sr.EndOfStream)
{
string line = sr.ReadLine();
if (line.ToLower().StartsWith("gps"))
{
string[] vals = line.Split(new char[] {',',':'});
if (lasttime == vals[1])
continue;
lasttime = vals[1];
list.Add(vals);
}
}
sr.Close();
sr.Dispose();
return list;
}
