Skip to content

Commit

Permalink
MagCalib: update dflog magcal
Browse files Browse the repository at this point in the history
  • Loading branch information
@meee1
meee1 committed Aug 27, 2016
1 parent 4cb2191 commit c6e033c
Showing 1 changed file with 55 additions and 13 deletions.
68 changes: 55 additions & 13 deletions MagCalib.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@
using log4net;
using MissionPlanner.HIL;
using MissionPlanner.Controls;
using MissionPlanner.Log;
using MissionPlanner.Utilities;

namespace MissionPlanner
Expand Down Expand Up @@ -101,7 +102,7 @@ public static void ProcessLog(int throttleThreshold = 0)
{
using (OpenFileDialog openFileDialog1 = new OpenFileDialog())
{
openFileDialog1.Filter = "Log Files|*.tlog;*.log";
openFileDialog1.Filter = "Log Files|*.tlog;*.log;*.bin";
openFileDialog1.FilterIndex = 2;
openFileDialog1.RestoreDirectory = true;
openFileDialog1.Multiselect = true;
Expand Down Expand Up @@ -712,13 +713,19 @@ public static double[] getOffsetsLog(string fn)

List<Tuple<float, float, float>> data2 = new List<Tuple<float, float, float>>();

Log.DFLog dflog = new Log.DFLog();
List<Tuple<float, float, float>> data3 = new List<Tuple<float, float, float>>();

var logfile = dflog.ReadLog(fn);
double[] ofsDoubles = new double[3];
double[] ofsDoubles2 = new double[3];
double[] ofsDoubles3 = new double[3];

foreach (var line in logfile)
CollectionBuffer logdata = new CollectionBuffer(File.OpenRead(fn));

var dflog = logdata.dflog;

foreach (var line in logdata.GetEnumeratorType(new[]{"MAG","MAG2","MAG3"}))
{
if (line.msgtype == "MAG" || line.msgtype == "MAG2")
if (line.msgtype == "MAG" || line.msgtype == "MAG2" || line.msgtype == "MAG3")
{
int indexmagx = dflog.FindMessageOffset(line.msgtype, "MagX");
int indexmagy = dflog.FindMessageOffset(line.msgtype, "MagY");
Expand Down Expand Up @@ -747,6 +754,10 @@ public static double[] getOffsetsLog(string fn)
magy - offsety,
magz - offsetz));

ofsDoubles[0] = offsetx;
ofsDoubles[1] = offsety;
ofsDoubles[2] = offsetz;

// fox dxf
vertex = new Polyline3dVertex(new Vector3f(magx - offsetx,
magy - offsety,
Expand All @@ -760,18 +771,51 @@ public static double[] getOffsetsLog(string fn)
magx - offsetx,
magy - offsety,
magz - offsetz));

ofsDoubles2[0] = offsetx;
ofsDoubles2[1] = offsety;
ofsDoubles2[2] = offsetz;
}
else if (line.msgtype == "MAG3")
{
data3.Add(new Tuple<float, float, float>(
magx - offsetx,
magy - offsety,
magz - offsetz));

ofsDoubles3[0] = offsetx;
ofsDoubles3[1] = offsety;
ofsDoubles3[2] = offsetz;
}
}
}
}

double[] x = LeastSq(data);
double[] x = LeastSq(data, false);

log.InfoFormat("magcal 1 ofs {0},{1},{2} strength {3} old ofs {4},{5},{6}", x[0], x[1], x[2], x[3], ofsDoubles[0], ofsDoubles[1], ofsDoubles[2]);

x = LeastSq(data,true);

log.InfoFormat("magcalel 1 ofs {0},{1},{2} strength {3} old ofs {4},{5},{6}", x[0], x[1], x[2], x[3], ofsDoubles[0], ofsDoubles[1], ofsDoubles[2]);

if (data2.Count > 0)
{
double[] x2 = LeastSq(data2);
double[] x2 = LeastSq(data2, false);
log.InfoFormat("magcal 2 ofs {0},{1},{2} strength {3} old ofs {4},{5},{6}", x2[0], x2[1], x2[2], x2[3], ofsDoubles2[0], ofsDoubles2[1], ofsDoubles2[2]);
x2 = LeastSq(data2, true);
log.InfoFormat("magcalel 2 ofs {0},{1},{2} strength {3} old ofs {4},{5},{6}", x2[0], x2[1], x2[2], x2[3], ofsDoubles2[0], ofsDoubles2[1], ofsDoubles2[2]);
}

if (data3.Count > 0)
{
double[] x3 = LeastSq(data3, false);
log.InfoFormat("magcal 3 ofs {0},{1},{2} strength {3} old ofs {4},{5},{6}", x3[0], x3[1], x3[2], x3[3], ofsDoubles3[0], ofsDoubles3[1], ofsDoubles3[2]);
x3 = LeastSq(data3, true);
log.InfoFormat("magcalel 3 ofs {0},{1},{2} strength {3} old ofs {4},{5},{6}", x3[0], x3[1], x3[2], x3[3], ofsDoubles3[0], ofsDoubles3[1], ofsDoubles3[2]);
}


log.Info("Least Sq Done " + DateTime.Now);

doDXF(vertexes, x);
Expand Down Expand Up @@ -993,11 +1037,11 @@ public static double[] LeastSq(List<Tuple<float, float, float>> data, bool ellip
double[] x;

//
x = new double[] {0, 0, 0, 0};
x = new double[] {0, 0, 0, avg_samples};

x = doLSQ(data, sphere_error, x);

rad = x[3];
rad = avg_samples;//x[3];

log.Info("lsq rad " + rad);

Expand All @@ -1020,10 +1064,10 @@ public static double[] LeastSq(List<Tuple<float, float, float>> data, bool ellip
static double[] doLSQ(List<Tuple<float, float, float>> data, Action<double[], double[], object> fitalgo,
double[] x)
{
double epsg = 0.00000001;
double epsg = 0.001;
double epsf = 0;
double epsx = 0;
int maxits = 20;
int maxits = 100;

alglib.minlmstate state;
alglib.minlmreport rep;
Expand Down Expand Up @@ -1248,8 +1292,6 @@ static void sphere_ellipsoid_error(double[] p1, double[] fi, object obj)
if (p1.Length >= 8)
offdiagonals = new Vector3(p1[6], p1[7], p1[8]);

diagonals.x = 1.0;

int a = 0;
foreach (var d in (List<Tuple<float, float, float>>) obj)
{
Expand Down

0 comments on commit c6e033c

Please sign in to comment.