/// <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;
if (throttleThreshold <= 0)
{
useData = true;
}
log.Info("Start log: " + DateTime.Now);
MAVLinkInterface mine = new MAVLinkInterface();
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);
System.Console.WriteLine("Old Method {0} {1} {2}", -(maxx + minx) / 2, -(maxy + miny) / 2, -(maxz + minz) / 2);
double[] x = LeastSq(data);
log.Info("Least Sq Done " + DateTime.Now);
doDXF(vertexes, x);
Array.Resize <double>(ref x, 3);
return(x);
}
private static void WriteDxfFile()
{
DxfDocument dxf = new DxfDocument();
//arc
Arc arc = new Arc(new Vector3(10, 10, 0), 10, 45, 135);
arc.Layer = new Layer("arc");
arc.Layer.Color.Index = 1;
dxf.AddEntity(arc);
//xData sample
XData xdata = new XData(new ApplicationRegistry("netDxf"));
xdata.XDataRecord.Add(new XDataRecord(XDataCode.String, "extended data with netDxf"));
xdata.XDataRecord.Add(XDataRecord.OpenControlString);
xdata.XDataRecord.Add(new XDataRecord(XDataCode.WorldSpacePositionX, 0));
xdata.XDataRecord.Add(new XDataRecord(XDataCode.WorldSpacePositionY, 0));
xdata.XDataRecord.Add(new XDataRecord(XDataCode.WorldSpacePositionZ, 0));
xdata.XDataRecord.Add(XDataRecord.CloseControlString);
XData xdata2 = new XData(new ApplicationRegistry("other application"));
xdata2.XDataRecord.Add(new XDataRecord(XDataCode.String, "extended data with netDxf"));
xdata2.XDataRecord.Add(XDataRecord.OpenControlString);
xdata2.XDataRecord.Add(new XDataRecord(XDataCode.String, "string record"));
xdata2.XDataRecord.Add(new XDataRecord(XDataCode.Real, 15.5));
xdata2.XDataRecord.Add(new XDataRecord(XDataCode.Long, 350));
xdata2.XDataRecord.Add(XDataRecord.CloseControlString);
//circle
Vector3 extrusion = new Vector3(1, 1, 1);
Vector3 centerWCS = new Vector3(1, 1, 1);
Vector3 centerOCS = MathHelper.Transform(centerWCS,
extrusion,
MathHelper.CoordinateSystem.World,
MathHelper.CoordinateSystem.Object);
Circle circle = new Circle(centerOCS, 5);
circle.Layer = new Layer("circle with spaces");
circle.Layer.Color = AciColor.Yellow;
circle.LineType = LineType.Dashed;
circle.Normal = extrusion;
circle.XData = new Dictionary <ApplicationRegistry, XData>
{
{ xdata.ApplicationRegistry, xdata },
{ xdata2.ApplicationRegistry, xdata2 }
};
dxf.AddEntity(circle);
//points
Point point1 = new Point(new Vector3(-3, -3, 0));
point1.Layer = new Layer("point");
point1.Color = new AciColor(30);
Point point2 = new Point(new Vector3(1, 1, 1));
point2.Layer = point1.Layer;
point2.Layer.Color.Index = 9;
point2.Normal = new Vector3(1, 1, 1);
dxf.AddEntity(point1);
dxf.AddEntity(point2);
//3dface
Face3d face3D = new Face3d(new Vector3(-5, -5, 5),
new Vector3(5, -5, 5),
new Vector3(5, 5, 5),
new Vector3(-5, 5, 5));
face3D.Layer = new Layer("3dface");
face3D.Layer.Color.Index = 3;
dxf.AddEntity(face3D);
//polyline
PolylineVertex polyVertex;
List <PolylineVertex> polyVertexes = new List <PolylineVertex>();
polyVertex = new PolylineVertex(new Vector2(-50, -50));
polyVertex.BeginWidth = 2;
polyVertexes.Add(polyVertex);
polyVertex = new PolylineVertex(new Vector2(50, -50));
polyVertex.BeginWidth = 1;
polyVertexes.Add(polyVertex);
polyVertex = new PolylineVertex(new Vector2(50, 50));
polyVertex.Bulge = 1;
polyVertexes.Add(polyVertex);
polyVertex = new PolylineVertex(new Vector2(-50, 50));
polyVertexes.Add(polyVertex);
Polyline polyline2d = new Polyline(polyVertexes, true);
polyline2d.Layer = new Layer("polyline2d");
polyline2d.Layer.Color.Index = 5;
polyline2d.Normal = new Vector3(1, 1, 1);
polyline2d.Elevation = 100.0f;
dxf.AddEntity(polyline2d);
//lightweight polyline
LightWeightPolylineVertex lwVertex;
List <LightWeightPolylineVertex> lwVertexes = new List <LightWeightPolylineVertex>();
lwVertex = new LightWeightPolylineVertex(new Vector2(-25, -25));
lwVertex.BeginWidth = 2;
lwVertexes.Add(lwVertex);
lwVertex = new LightWeightPolylineVertex(new Vector2(25, -25));
lwVertex.BeginWidth = 1;
lwVertexes.Add(lwVertex);
lwVertex = new LightWeightPolylineVertex(new Vector2(25, 25));
lwVertex.Bulge = 1;
lwVertexes.Add(lwVertex);
lwVertex = new LightWeightPolylineVertex(new Vector2(-25, 25));
lwVertexes.Add(lwVertex);
LightWeightPolyline lwPolyline = new LightWeightPolyline(lwVertexes, true);
lwPolyline.Layer = new Layer("lwpolyline");
lwPolyline.Layer.Color.Index = 5;
lwPolyline.Normal = new Vector3(1, 1, 1);
lwPolyline.Elevation = 100.0f;
dxf.AddEntity(lwPolyline);
//line
Line line = new Line(new Vector3(0, 0, 0), new Vector3(10, 10, 10));
line.Layer = new Layer("line");
line.Layer.Color.Index = 6;
dxf.AddEntity(line);
//3d polyline
Polyline3dVertex vertex;
List <Polyline3dVertex> vertexes = new List <Polyline3dVertex>();
vertex = new Polyline3dVertex(new Vector3(-50, -50, 0));
vertexes.Add(vertex);
vertex = new Polyline3dVertex(new Vector3(50, -50, 10));
vertexes.Add(vertex);
vertex = new Polyline3dVertex(new Vector3(50, 50, 25));
vertexes.Add(vertex);
vertex = new Polyline3dVertex(new Vector3(-50, 50, 50));
vertexes.Add(vertex);
Polyline3d polyline = new Polyline3d(vertexes, true);
polyline.Layer = new Layer("polyline3d");
polyline.Layer.Color.Index = 24;
dxf.AddEntity(polyline);
//block definition
Block block = new Block("TestBlock");
block.Entities.Add(new Line(new Vector3(-5, -5, 5), new Vector3(5, 5, 5)));
block.Entities.Add(new Line(new Vector3(5, -5, 5), new Vector3(-5, 5, 5)));
//insert
Insert insert = new Insert(block, new Vector3(5, 5, 5));
insert.Layer = new Layer("insert");
insert.Layer.Color.Index = 4;
dxf.AddEntity(insert);
//text
TextStyle style = new TextStyle("True type font", "Arial.ttf");
Text text = new Text("Hello world!", Vector3.Zero, 10.0f, style);
text.Layer = new Layer("text");
text.Layer.Color.Index = 8;
text.Alignment = TextAlignment.TopRight;
dxf.AddEntity(text);
dxf.Save("AutoCad2010.dxf", DxfVersion.AutoCad2010);
dxf.Save("AutoCad2007.dxf", DxfVersion.AutoCad2007);
dxf.Save("AutoCad2004.dxf", DxfVersion.AutoCad2004);
dxf.Save("AutoCad2000.dxf", DxfVersion.AutoCad2000);
}
public static double[] getOffsetsLog(string fn)
{
// this is for a dxf
Polyline3dVertex vertex;
List <Polyline3dVertex> vertexes = new List <Polyline3dVertex>();
List <Tuple <float, float, float> > data = new List <Tuple <float, float, float> >();
List <Tuple <float, float, float> > data2 = new List <Tuple <float, float, float> >();
var logfile = Log.DFLog.ReadLog(fn);
foreach (var line in logfile)
{
if (line.msgtype == "MAG" || line.msgtype == "MAG2")
{
int indexmagx = Log.DFLog.FindMessageOffset(line.msgtype, "MagX");
int indexmagy = Log.DFLog.FindMessageOffset(line.msgtype, "MagY");
int indexmagz = Log.DFLog.FindMessageOffset(line.msgtype, "MagZ");
int indexoffsetx = Log.DFLog.FindMessageOffset(line.msgtype, "OfsX");
int indexoffsety = Log.DFLog.FindMessageOffset(line.msgtype, "OfsY");
int indexoffsetz = Log.DFLog.FindMessageOffset(line.msgtype, "OfsZ");
if (indexmagx != -1 && indexoffsetx != -1)
{
float magx = float.Parse(line.items[indexmagx]);
float magy = float.Parse(line.items[indexmagy]);
float magz = float.Parse(line.items[indexmagz]);
float offsetx = float.Parse(line.items[indexoffsetx]);
float offsety = float.Parse(line.items[indexoffsety]);
float offsetz = float.Parse(line.items[indexoffsetz]);
//offsetx = offsety = offsetz = 0;
if (line.msgtype == "MAG")
{
data.Add(new Tuple <float, float, float>(
magx - offsetx,
magy - offsety,
magz - offsetz));
// fox dxf
vertex = new Polyline3dVertex(new Vector3f(magx - offsetx,
magy - offsety,
magz - offsetz)
);
vertexes.Add(vertex);
}
else if (line.msgtype == "MAG2")
{
data2.Add(new Tuple <float, float, float>(
magx - offsetx,
magy - offsety,
magz - offsetz));
}
}
}
}
double[] x = LeastSq(data);
double[] x2 = LeastSq(data2);
log.Info("Least Sq Done " + DateTime.Now);
doDXF(vertexes, x);
Array.Resize <double>(ref x, 3);
return(x);
}
/// <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);
}
