static void prd_DoWork(object sender, ProgressWorkerEventArgs e, object passdata = null)
{
// turn learning off
MainV2.comPort.setParam("COMPASS_LEARN", 0);
bool havecompass2 = false;
//compass2 get mag2 offsets
if (MainV2.comPort.MAV.param.ContainsKey("COMPASS_OFS2_X"))
{
//com2ofsx = MainV2.comPort.GetParam("COMPASS_OFS2_X");
//com2ofsy = MainV2.comPort.GetParam("COMPASS_OFS2_Y");
//com2ofsz = MainV2.comPort.GetParam("COMPASS_OFS2_Z");
MainV2.comPort.setParam("COMPASS_OFS2_X", 0);
MainV2.comPort.setParam("COMPASS_OFS2_Y", 0);
MainV2.comPort.setParam("COMPASS_OFS2_Z", 0);
havecompass2 = true;
}
// old method
float minx = 0;
float maxx = 0;
float miny = 0;
float maxy = 0;
float minz = 0;
float maxz = 0;
// backup current rate and set to 10 hz
byte backupratesens = MainV2.comPort.MAV.cs.ratesensors;
MainV2.comPort.MAV.cs.ratesensors = 10;
MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.comPort.MAV.cs.ratesensors); // mag captures at 10 hz
var sub = MainV2.comPort.SubscribeToPacketType(MAVLink.MAVLINK_MSG_ID.RAW_IMU, ReceviedPacket);
var sub2 = MainV2.comPort.SubscribeToPacketType(MAVLink.MAVLINK_MSG_ID.SCALED_IMU2, ReceviedPacket);
string extramsg = "";
((ProgressReporterSphere)sender).sphere1.Clear();
((ProgressReporterSphere)sender).sphere2.Clear();
int lastcount = 0;
DateTime lastlsq = DateTime.MinValue;
DateTime lastlsq2 = DateTime.MinValue;
HIL.Vector3 centre = new HIL.Vector3();
while (true)
{
// slow down execution
System.Threading.Thread.Sleep(20);
((ProgressReporterDialogue)sender).UpdateProgressAndStatus(-1, "Got " + datacompass1.Count + " Samples " + extramsg);
if (e.CancelRequested)
{
// restore old sensor rate
MainV2.comPort.MAV.cs.ratesensors = backupratesens;
MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.comPort.MAV.cs.ratesensors);
e.CancelAcknowledged = false;
e.CancelRequested = false;
break;
}
if (datacompass1.Count == 0)
{
continue;
}
// dont use dup data
if (lastcount == datacompass1.Count)
{
continue;
}
lastcount = datacompass1.Count;
float rawmx = datacompass1[datacompass1.Count - 1].Item1;
float rawmy = datacompass1[datacompass1.Count - 1].Item2;
float rawmz = datacompass1[datacompass1.Count - 1].Item3;
// for old method
setMinorMax(rawmx, ref minx, ref maxx);
setMinorMax(rawmy, ref miny, ref maxy);
setMinorMax(rawmz, ref minz, ref maxz);
// get the current estimated centerpoint
//new HIL.Vector3((float)-((maxx + minx) / 2), (float)-((maxy + miny) / 2), (float)-((maxz + minz) / 2));
// run lsq every second when more than 100 datapoints
if (datacompass1.Count > 100 && lastlsq.Second != DateTime.Now.Second)
{
lastlsq = DateTime.Now;
lock (datacompass1)
{
var lsq = MagCalib.LeastSq(datacompass1, false);
// simple validation
if (Math.Abs(lsq[0]) < 999)
{
centre = new HIL.Vector3(lsq[0], lsq[1], lsq[2]);
log.Info("new centre " + centre.ToString());
((ProgressReporterSphere)sender).sphere1.CenterPoint = new OpenTK.Vector3((float)centre.x, (float)centre.y, (float)centre.z);
}
}
}
// run lsq every second when more than 100 datapoints
if (datacompass2.Count > 100 && lastlsq2.Second != DateTime.Now.Second)
{
lastlsq2 = DateTime.Now;
lock (datacompass2)
{
var lsq = MagCalib.LeastSq(datacompass2, false);
// simple validation
if (Math.Abs(lsq[0]) < 999)
{
HIL.Vector3 centre2 = new HIL.Vector3(lsq[0], lsq[1], lsq[2]);
log.Info("new centre2 " + centre2.ToString());
((ProgressReporterSphere)sender).sphere2.CenterPoint = new OpenTK.Vector3((float)centre2.x, (float)centre2.y, (float)centre2.z);
}
}
}
// add to sphere with center correction
((ProgressReporterSphere)sender).sphere1.AddPoint(new OpenTK.Vector3(rawmx, rawmy, rawmz));
((ProgressReporterSphere)sender).sphere1.AimClear();
if (havecompass2 && datacompass2.Count > 0)
{
float raw2mx = datacompass2[datacompass2.Count - 1].Item1;
float raw2my = datacompass2[datacompass2.Count - 1].Item2;
float raw2mz = datacompass2[datacompass2.Count - 1].Item3;
((ProgressReporterSphere)sender).sphere2.AddPoint(new OpenTK.Vector3(raw2mx, raw2my, raw2mz));
((ProgressReporterSphere)sender).sphere2.AimClear();
}
HIL.Vector3 point;
point = new HIL.Vector3(rawmx, rawmy, rawmz) + centre;
//find the mean radius
float radius = 0;
for (int i = 0; i < datacompass1.Count; i++)
{
point = new HIL.Vector3(datacompass1[i].Item1, datacompass1[i].Item2, datacompass1[i].Item3);
radius += (float)(point + centre).length();
}
radius /= datacompass1.Count;
//test that we can find one point near a set of points all around the sphere surface
string displayresult = "";
int factor = 3; // 4 point check 16 points
float max_distance = radius / 3; //pretty generouse
for (int j = 0; j <= factor; j++)
{
double theta = (Math.PI * (j + 0)) / factor;
for (int i = 0; i <= factor; i++)
{
double phi = (2 * Math.PI * i) / factor;
HIL.Vector3 point_sphere = new HIL.Vector3(
(float)(Math.Sin(theta) * Math.Cos(phi) * radius),
(float)(Math.Sin(theta) * Math.Sin(phi) * radius),
(float)(Math.Cos(theta) * radius)) - centre;
//log.DebugFormat("magcalib check - {0} {1} dist {2}", theta * rad2deg, phi * rad2deg, max_distance);
bool found = false;
for (int k = 0; k < datacompass1.Count; k++)
{
point = new HIL.Vector3(datacompass1[k].Item1, datacompass1[k].Item2, datacompass1[k].Item3);
double d = (point_sphere - point).length();
if (d < max_distance)
{
found = true;
break;
}
}
// draw them all
//((ProgressReporterSphere)sender).sphere1.AimFor(new OpenTK.Vector3((float)point_sphere.x, (float)point_sphere.y, (float)point_sphere.z));
if (!found)
{
displayresult = "more data needed " + (theta * rad2deg).ToString("0") + " " + (phi * rad2deg).ToString("0");
((ProgressReporterSphere)sender).sphere1.AimFor(new OpenTK.Vector3((float)point_sphere.x, (float)point_sphere.y, (float)point_sphere.z));
//j = factor;
//break;
}
}
}
extramsg = displayresult;
}
MainV2.comPort.UnSubscribeToPacketType(sub);
MainV2.comPort.UnSubscribeToPacketType(sub2);
if (minx > 0 && maxx > 0 || minx < 0 && maxx < 0 || miny > 0 && maxy > 0 || miny < 0 && maxy < 0 || minz > 0 && maxz > 0 || minz < 0 && maxz < 0)
{
e.ErrorMessage = "Bad compass raw values. Check for magnetic interferance.";
ans = null;
ans2 = null;
return;
}
// restore old sensor rate
MainV2.comPort.MAV.cs.ratesensors = backupratesens;
MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.comPort.MAV.cs.ratesensors);
if (extramsg != "")
{
if (CustomMessageBox.Show("You are missing data points. do you want to run the calibration anyway?", "run anyway", MessageBoxButtons.YesNo) == DialogResult.No)
{
e.CancelAcknowledged = true;
e.CancelRequested = true;
ans = null;
ans2 = null;
return;
}
}
// remove outlyers
RemoveOutliers(ref datacompass1);
if (havecompass2 && datacompass2.Count > 0)
{
RemoveOutliers(ref datacompass2);
}
if (datacompass1.Count < 10)
{
e.ErrorMessage = "Log does not contain enough data";
ans = null;
ans2 = null;
return;
}
bool ellipsoid = false;
if (MainV2.comPort.MAV.param.ContainsKey("MAG_DIA"))
{
ellipsoid = true;
}
log.Info("Compass 1");
ans = MagCalib.LeastSq(datacompass1, ellipsoid);
if (havecompass2 && datacompass2.Count > 0)
{
log.Info("Compass 2");
ans2 = MagCalib.LeastSq(datacompass2, ellipsoid);
}
}
static void prd_DoWork(object sender, ProgressWorkerEventArgs e, object passdata = null)
{
// list of x,y,z 's
List <Tuple <float, float, float> > data = new List <Tuple <float, float, float> >();
// old method
float minx = 0;
float maxx = 0;
float miny = 0;
float maxy = 0;
float minz = 0;
float maxz = 0;
// backup current rate and set to 10 hz
byte backupratesens = MainV2.comPort.MAV.cs.ratesensors;
MainV2.comPort.MAV.cs.ratesensors = 10;
MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.comPort.MAV.cs.ratesensors); // mag captures at 10 hz
float oldmx = 0;
float oldmy = 0;
float oldmz = 0;
// filter data points to only x number per quadrant
int div = 20;
Hashtable filter = new Hashtable();
string extramsg = "";
((ProgressReporterSphere)sender).sphere1.Clear();
while (true)
{
((ProgressReporterDialogue)sender).UpdateProgressAndStatus(-1, "Got " + data.Count + " Samples " + extramsg);
if (e.CancelRequested)
{
// restore old sensor rate
MainV2.comPort.MAV.cs.ratesensors = backupratesens;
MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.comPort.MAV.cs.ratesensors);
e.CancelAcknowledged = false;
e.CancelRequested = false;
break;
}
if (oldmx != MainV2.comPort.MAV.cs.mx &&
oldmy != MainV2.comPort.MAV.cs.my &&
oldmz != MainV2.comPort.MAV.cs.mz)
{
// for new lease sq
string item = (int)(MainV2.comPort.MAV.cs.mx / div) + "," +
(int)(MainV2.comPort.MAV.cs.my / div) + "," +
(int)(MainV2.comPort.MAV.cs.mz / div);
if (filter.ContainsKey(item))
{
filter[item] = (int)filter[item] + 1;
if ((int)filter[item] > 3)
{
continue;
}
}
else
{
filter[item] = 1;
}
// add data
data.Add(new Tuple <float, float, float>(
MainV2.comPort.MAV.cs.mx - (float)MainV2.comPort.MAV.cs.mag_ofs_x,
MainV2.comPort.MAV.cs.my - (float)MainV2.comPort.MAV.cs.mag_ofs_y,
MainV2.comPort.MAV.cs.mz - (float)MainV2.comPort.MAV.cs.mag_ofs_z));
oldmx = MainV2.comPort.MAV.cs.mx;
oldmy = MainV2.comPort.MAV.cs.my;
oldmz = MainV2.comPort.MAV.cs.mz;
((ProgressReporterSphere)sender).sphere1.AddPoint(new OpenTK.Vector3(oldmx, oldmy, oldmz));
// for old method
setMinorMax(MainV2.comPort.MAV.cs.mx - (float)MainV2.comPort.MAV.cs.mag_ofs_x, ref minx, ref maxx);
setMinorMax(MainV2.comPort.MAV.cs.my - (float)MainV2.comPort.MAV.cs.mag_ofs_y, ref miny, ref maxy);
setMinorMax(MainV2.comPort.MAV.cs.mz - (float)MainV2.comPort.MAV.cs.mag_ofs_z, ref minz, ref maxz);
}
//find the mean radius
HIL.Vector3 centre = new HIL.Vector3(); //new HIL.Vector3((float)-((maxx + minx) / 2), (float)-((maxy + miny) / 2), (float)-((maxz + minz) / 2));
HIL.Vector3 point;
float radius = 0;
for (int i = 0; i < data.Count; i++)
{
point = new HIL.Vector3(data[i].Item1, data[i].Item2, data[i].Item3);
radius += (float)(point - centre).length();
}
radius /= data.Count;
//test that we can find one point near a set of points all around the sphere surface
int factor = 4; // 4 point check 2x2
float max_distance = radius / 3; //pretty generouse
for (int j = 0; j < factor; j++)
{
double theta = (Math.PI * (j + 0.5)) / factor;
for (int i = 0; i < factor; i++)
{
double phi = (2 * Math.PI * i) / factor;
HIL.Vector3 point_sphere = new HIL.Vector3(
(float)(Math.Sin(theta) * Math.Cos(phi) * radius),
(float)(Math.Sin(theta) * Math.Sin(phi) * radius),
(float)(Math.Cos(theta) * radius)) + centre;
//Console.WriteLine("{0} {1}", theta * rad2deg, phi * rad2deg);
bool found = false;
for (int k = 0; k < data.Count; k++)
{
point = new HIL.Vector3(data[k].Item1, data[k].Item2, data[k].Item3);
double d = (point_sphere - point).length();
if (d < max_distance)
{
found = true;
break;
}
}
if (!found)
{
extramsg = "more data needed";
//e.ErrorMessage = "Data missing for some directions";
//ans = null;
//return;
j = factor;
break;
}
else
{
extramsg = "";
}
}
}
}
if (minx > 0 && maxx > 0 || minx < 0 && maxx < 0 || miny > 0 && maxy > 0 || miny < 0 && maxy < 0 || minz > 0 && maxz > 0 || minz < 0 && maxz < 0)
{
e.ErrorMessage = "Bad compass raw values. Check for magnetic interferance.";
ans = null;
return;
}
// restore old sensor rate
MainV2.comPort.MAV.cs.ratesensors = backupratesens;
MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.comPort.MAV.cs.ratesensors);
if (data.Count < 10 || extramsg != "")
{
e.ErrorMessage = "Log does not contain enough data";
ans = null;
return;
}
bool ellipsoid = false;
if (MainV2.comPort.MAV.param.ContainsKey("MAG_DIA"))
{
ellipsoid = true;
}
ans = MagCalib.LeastSq(data, ellipsoid);
}
static void prd_DoWork(object sender, ProgressWorkerEventArgs e, object passdata = null)
{
// turn learning off
MainV2.comPort.setParam("COMPASS_LEARN", 0);
bool havecompass2 = false;
bool havecompass3 = false;
//compass2 get mag2 offsets
if (MainV2.comPort.MAV.param.ContainsKey("COMPASS_OFS2_X"))
{
MainV2.comPort.setParam("COMPASS_OFS2_X", 0, true);
MainV2.comPort.setParam("COMPASS_OFS2_Y", 0, true);
MainV2.comPort.setParam("COMPASS_OFS2_Z", 0, true);
havecompass2 = true;
}
//compass3
if (MainV2.comPort.MAV.param.ContainsKey("COMPASS_OFS3_X"))
{
MainV2.comPort.setParam("COMPASS_OFS3_X", 0, true);
MainV2.comPort.setParam("COMPASS_OFS3_Y", 0, true);
MainV2.comPort.setParam("COMPASS_OFS3_Z", 0, true);
havecompass3 = true;
}
int hittarget = 14; // int.Parse(File.ReadAllText("magtarget.txt"));
// old method
float minx = 0;
float maxx = 0;
float miny = 0;
float maxy = 0;
float minz = 0;
float maxz = 0;
// backup current rate and set
byte backupratesens = MainV2.comPort.MAV.cs.ratesensors;
byte backuprateatt = MainV2.comPort.MAV.cs.rateattitude;
byte backupratepos = MainV2.comPort.MAV.cs.rateposition;
MainV2.comPort.MAV.cs.ratesensors = 2;
MainV2.comPort.MAV.cs.rateattitude = 0;
MainV2.comPort.MAV.cs.rateposition = 0;
MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.ALL, 0);
MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, 50);
// subscribe to data packets
var sub = MainV2.comPort.SubscribeToPacketType(MAVLink.MAVLINK_MSG_ID.RAW_IMU, ReceviedPacket);
var sub2 = MainV2.comPort.SubscribeToPacketType(MAVLink.MAVLINK_MSG_ID.SCALED_IMU2, ReceviedPacket);
var sub3 = MainV2.comPort.SubscribeToPacketType(MAVLink.MAVLINK_MSG_ID.SCALED_IMU3, ReceviedPacket);
string extramsg = "";
// clear any old data
((ProgressReporterSphere) sender).sphere1.Clear();
((ProgressReporterSphere) sender).sphere2.Clear();
((ProgressReporterSphere) sender).sphere3.Clear();
// keep track of data count and last lsq run
int lastcount = 0;
DateTime lastlsq = DateTime.MinValue;
DateTime lastlsq2 = DateTime.MinValue;
DateTime lastlsq3 = DateTime.MinValue;
HIL.Vector3 centre = new HIL.Vector3();
while (true)
{
// slow down execution
System.Threading.Thread.Sleep(10);
string str = "Got + " + datacompass1.Count + " samples\n" +
"Compass 1 error: " + error;
if (MainV2.comPort.MAV.param.ContainsKey("COMPASS_OFS2_X"))
str += "\nCompass 2 error: " + error2;
if (MainV2.comPort.MAV.param.ContainsKey("COMPASS_OFS3_X"))
str += "\nCompass 3 error: " + error3;
str += "\n" + extramsg;
((ProgressReporterDialogue) sender).UpdateProgressAndStatus(-1, str);
if (e.CancelRequested)
{
e.CancelAcknowledged = false;
e.CancelRequested = false;
break;
}
if (datacompass1.Count == 0)
continue;
float rawmx = datacompass1[datacompass1.Count - 1].Item1;
float rawmy = datacompass1[datacompass1.Count - 1].Item2;
float rawmz = datacompass1[datacompass1.Count - 1].Item3;
// for old method
setMinorMax(rawmx, ref minx, ref maxx);
setMinorMax(rawmy, ref miny, ref maxy);
setMinorMax(rawmz, ref minz, ref maxz);
// get the current estimated centerpoint
//new HIL.Vector3((float)-((maxx + minx) / 2), (float)-((maxy + miny) / 2), (float)-((maxz + minz) / 2));
//Console.WriteLine("1 " + DateTime.Now.Millisecond);
// run lsq every second when more than 100 datapoints
if (datacompass1.Count > 100 && lastlsq.Second != DateTime.Now.Second)
{
MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.ALL, 0);
MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, 50);
lastlsq = DateTime.Now;
lock (datacompass1)
{
var lsq = MagCalib.LeastSq(datacompass1, false);
// simple validation
if (Math.Abs(lsq[0]) < 999)
{
centre = new HIL.Vector3(lsq[0], lsq[1], lsq[2]);
log.Info("new centre " + centre.ToString());
((ProgressReporterSphere) sender).sphere1.CenterPoint = new OpenTK.Vector3(
(float) centre.x, (float) centre.y, (float) centre.z);
}
}
}
// run lsq every second when more than 100 datapoints
if (datacompass2.Count > 100 && lastlsq2.Second != DateTime.Now.Second)
{
lastlsq2 = DateTime.Now;
lock (datacompass2)
{
var lsq = MagCalib.LeastSq(datacompass2, false);
// simple validation
if (Math.Abs(lsq[0]) < 999)
{
HIL.Vector3 centre2 = new HIL.Vector3(lsq[0], lsq[1], lsq[2]);
log.Info("new centre2 " + centre2.ToString());
((ProgressReporterSphere) sender).sphere2.CenterPoint = new OpenTK.Vector3(
(float) centre2.x, (float) centre2.y, (float) centre2.z);
}
}
}
// run lsq every second when more than 100 datapoints
if (datacompass3.Count > 100 && lastlsq3.Second != DateTime.Now.Second)
{
lastlsq3 = DateTime.Now;
lock (datacompass3)
{
var lsq = MagCalib.LeastSq(datacompass3, false);
// simple validation
if (Math.Abs(lsq[0]) < 999)
{
HIL.Vector3 centre3 = new HIL.Vector3(lsq[0], lsq[1], lsq[2]);
log.Info("new centre2 " + centre3.ToString());
((ProgressReporterSphere) sender).sphere3.CenterPoint = new OpenTK.Vector3(
(float) centre3.x, (float) centre3.y, (float) centre3.z);
}
}
}
//Console.WriteLine("1a " + DateTime.Now.Millisecond);
// dont use dup data
if (lastcount == datacompass1.Count)
continue;
lastcount = datacompass1.Count;
// add to sphere with center correction
((ProgressReporterSphere) sender).sphere1.AddPoint(new OpenTK.Vector3(rawmx, rawmy, rawmz));
((ProgressReporterSphere) sender).sphere1.AimClear();
if (datacompass2.Count > 30)
{
float raw2mx = datacompass2[datacompass2.Count - 1].Item1;
float raw2my = datacompass2[datacompass2.Count - 1].Item2;
float raw2mz = datacompass2[datacompass2.Count - 1].Item3;
((ProgressReporterSphere) sender).sphere2.AddPoint(new OpenTK.Vector3(raw2mx, raw2my, raw2mz));
((ProgressReporterSphere) sender).sphere2.AimClear();
}
if (datacompass3.Count > 30)
{
float raw3mx = datacompass3[datacompass3.Count - 1].Item1;
float raw3my = datacompass3[datacompass3.Count - 1].Item2;
float raw3mz = datacompass3[datacompass3.Count - 1].Item3;
((ProgressReporterSphere) sender).sphere3.AddPoint(new OpenTK.Vector3(raw3mx, raw3my, raw3mz));
((ProgressReporterSphere) sender).sphere3.AimClear();
}
//Console.WriteLine("2 " + DateTime.Now.Millisecond);
HIL.Vector3 point;
point = new HIL.Vector3(rawmx, rawmy, rawmz) + centre;
//find the mean radius
float radius = 0;
for (int i = 0; i < datacompass1.Count; i++)
{
point = new HIL.Vector3(datacompass1[i].Item1, datacompass1[i].Item2, datacompass1[i].Item3);
radius += (float) (point + centre).length();
}
radius /= datacompass1.Count;
//test that we can find one point near a set of points all around the sphere surface
int pointshit = 0;
string displayresult = "";
int factor = 3; // pitch
int factor2 = 4; // yaw
float max_distance = radius/3; //pretty generouse
for (int j = 0; j <= factor; j++)
{
double theta = (Math.PI*(j + 0.5))/factor;
for (int i = 0; i <= factor2; i++)
{
double phi = (2*Math.PI*i)/factor2;
HIL.Vector3 point_sphere = new HIL.Vector3(
(float) (Math.Sin(theta)*Math.Cos(phi)*radius),
(float) (Math.Sin(theta)*Math.Sin(phi)*radius),
(float) (Math.Cos(theta)*radius)) - centre;
//log.InfoFormat("magcalib check - {0} {1} dist {2}", theta * rad2deg, phi * rad2deg, max_distance);
bool found = false;
for (int k = 0; k < datacompass1.Count; k++)
{
point = new HIL.Vector3(datacompass1[k].Item1, datacompass1[k].Item2, datacompass1[k].Item3);
double d = (point_sphere - point).length();
if (d < max_distance)
{
pointshit++;
found = true;
break;
}
}
// draw them all
//((ProgressReporterSphere)sender).sphere1.AimFor(new OpenTK.Vector3((float)point_sphere.x, (float)point_sphere.y, (float)point_sphere.z));
if (!found)
{
displayresult = "more data needed Aim For " +
GetColour((int) (theta*rad2deg), (int) (phi*rad2deg));
((ProgressReporterSphere) sender).sphere1.AimFor(new OpenTK.Vector3((float) point_sphere.x,
(float) point_sphere.y, (float) point_sphere.z));
//j = factor;
//break;
}
}
}
extramsg = displayresult;
//Console.WriteLine("3 "+ DateTime.Now.Millisecond);
// check primary compass error
if (error < 0.2 && pointshit > hittarget && ((ProgressReporterSphere) sender).autoaccept)
{
extramsg = "";
break;
}
}
MainV2.comPort.UnSubscribeToPacketType(sub);
MainV2.comPort.UnSubscribeToPacketType(sub2);
MainV2.comPort.UnSubscribeToPacketType(sub3);
// restore old sensor rate
MainV2.comPort.MAV.cs.ratesensors = backupratesens;
MainV2.comPort.MAV.cs.rateattitude = backuprateatt;
MainV2.comPort.MAV.cs.rateposition = backupratepos;
MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.comPort.MAV.cs.ratesensors);
MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.POSITION, MainV2.comPort.MAV.cs.rateposition);
// request gps
MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.EXTRA1, MainV2.comPort.MAV.cs.rateattitude);
// request attitude
MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.EXTRA2, MainV2.comPort.MAV.cs.rateattitude);
// request vfr
MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.EXTRA3, MainV2.comPort.MAV.cs.ratesensors);
// request extra stuff - tridge
MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.comPort.MAV.cs.ratesensors);
// request raw sensor
MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RC_CHANNELS, MainV2.comPort.MAV.cs.raterc);
// request rc info
if (MainV2.speechEnable)
{
MainV2.speechEngine.SpeakAsync("Compass Calibration Complete");
}
else
{
Console.Beep();
}
if (minx > 0 && maxx > 0 || minx < 0 && maxx < 0 || miny > 0 && maxy > 0 || miny < 0 && maxy < 0 ||
minz > 0 && maxz > 0 || minz < 0 && maxz < 0)
{
e.ErrorMessage = "Bad compass raw values. Check for magnetic interferance.";
ans = null;
ans2 = null;
return;
}
if (extramsg != "")
{
if (CustomMessageBox.Show(Strings.MissingDataPoints, Strings.RunAnyway, MessageBoxButtons.YesNo) ==
DialogResult.No)
{
e.CancelAcknowledged = true;
e.CancelRequested = true;
ans = null;
ans2 = null;
ans3 = null;
return;
}
}
// remove outlyers
RemoveOutliers(ref datacompass1);
if (havecompass2 && datacompass2.Count > 0)
{
RemoveOutliers(ref datacompass2);
}
if (havecompass3 && datacompass3.Count > 0)
{
RemoveOutliers(ref datacompass3);
}
if (datacompass1.Count < 10)
{
e.ErrorMessage = "Log does not contain enough data";
ans = null;
ans2 = null;
return;
}
bool ellipsoid = false;
if (MainV2.comPort.MAV.param.ContainsKey("MAG_DIA"))
{
ellipsoid = true;
}
log.Info("Compass 1");
ans = MagCalib.LeastSq(datacompass1, ellipsoid);
if (havecompass2 && datacompass2.Count > 0)
{
log.Info("Compass 2");
ans2 = MagCalib.LeastSq(datacompass2, ellipsoid);
}
if (havecompass3 && datacompass3.Count > 0)
{
log.Info("Compass 3");
ans3 = MagCalib.LeastSq(datacompass3, ellipsoid);
}
}
public void setOffsets(MAVLinkInterface mav, double x, double y, double z)
{
offsets[mav] = new HIL.Vector3(x, y, z);
//log.Info(mav.ToString() + " " + offsets[mav].ToString());
}
static void prd_DoWork(object sender, ProgressWorkerEventArgs e, object passdata = null)
{
// turn learning off
MainV2.comPort.setParam("COMPASS_LEARN", 0);
bool havecompass2 = false;
//compass2 get mag2 offsets
if (MainV2.comPort.MAV.param.ContainsKey("COMPASS_OFS2_X"))
{
//com2ofsx = MainV2.comPort.GetParam("COMPASS_OFS2_X");
//com2ofsy = MainV2.comPort.GetParam("COMPASS_OFS2_Y");
//com2ofsz = MainV2.comPort.GetParam("COMPASS_OFS2_Z");
MainV2.comPort.setParam("COMPASS_OFS2_X", 0, true);
MainV2.comPort.setParam("COMPASS_OFS2_Y", 0, true);
MainV2.comPort.setParam("COMPASS_OFS2_Z", 0, true);
havecompass2 = true;
}
int hittarget = 14;// int.Parse(File.ReadAllText("magtarget.txt"));
// old method
float minx = 0;
float maxx = 0;
float miny = 0;
float maxy = 0;
float minz = 0;
float maxz = 0;
// backup current rate and set
byte backupratesens = MainV2.comPort.MAV.cs.ratesensors;
byte backuprateatt = MainV2.comPort.MAV.cs.rateattitude;
byte backupratepos = MainV2.comPort.MAV.cs.rateposition;
MainV2.comPort.MAV.cs.ratesensors = 2;
MainV2.comPort.MAV.cs.rateattitude = 0;
MainV2.comPort.MAV.cs.rateposition = 0;
MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.ALL, 0);
MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, 50);
// subscribe to data packets
var sub = MainV2.comPort.SubscribeToPacketType(MAVLink.MAVLINK_MSG_ID.RAW_IMU, ReceviedPacket);
var sub2 = MainV2.comPort.SubscribeToPacketType(MAVLink.MAVLINK_MSG_ID.SCALED_IMU2, ReceviedPacket);
string extramsg = "";
// clear any old data
((ProgressReporterSphere)sender).sphere1.Clear();
((ProgressReporterSphere)sender).sphere2.Clear();
// keep track of data count and last lsq run
int lastcount = 0;
DateTime lastlsq = DateTime.MinValue;
DateTime lastlsq2 = DateTime.MinValue;
HIL.Vector3 centre = new HIL.Vector3();
while (true)
{
// slow down execution
System.Threading.Thread.Sleep(10);
((ProgressReporterDialogue)sender).UpdateProgressAndStatus(-1, "Got " + datacompass1.Count + " Samples\ncompass 1 error:" + error + "\ncompass 2 error:" + error2 + "\n" + extramsg);
if (e.CancelRequested)
{
e.CancelAcknowledged = false;
e.CancelRequested = false;
break;
}
if (datacompass1.Count == 0)
{
continue;
}
float rawmx = datacompass1[datacompass1.Count - 1].Item1;
float rawmy = datacompass1[datacompass1.Count - 1].Item2;
float rawmz = datacompass1[datacompass1.Count - 1].Item3;
// for old method
setMinorMax(rawmx, ref minx, ref maxx);
setMinorMax(rawmy, ref miny, ref maxy);
setMinorMax(rawmz, ref minz, ref maxz);
// get the current estimated centerpoint
//new HIL.Vector3((float)-((maxx + minx) / 2), (float)-((maxy + miny) / 2), (float)-((maxz + minz) / 2));
//Console.WriteLine("1 " + DateTime.Now.Millisecond);
// run lsq every second when more than 100 datapoints
if (datacompass1.Count > 100 && lastlsq.Second != DateTime.Now.Second)
{
MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.ALL, 0);
MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, 50);
lastlsq = DateTime.Now;
lock (datacompass1)
{
var lsq = MagCalib.LeastSq(datacompass1, false);
// simple validation
if (Math.Abs(lsq[0]) < 999)
{
centre = new HIL.Vector3(lsq[0], lsq[1], lsq[2]);
log.Info("new centre " + centre.ToString());
((ProgressReporterSphere)sender).sphere1.CenterPoint = new OpenTK.Vector3((float)centre.x, (float)centre.y, (float)centre.z);
}
}
}
// run lsq every second when more than 100 datapoints
if (datacompass2.Count > 100 && lastlsq2.Second != DateTime.Now.Second)
{
lastlsq2 = DateTime.Now;
lock (datacompass2)
{
var lsq = MagCalib.LeastSq(datacompass2, false);
// simple validation
if (Math.Abs(lsq[0]) < 999)
{
HIL.Vector3 centre2 = new HIL.Vector3(lsq[0], lsq[1], lsq[2]);
log.Info("new centre2 " + centre2.ToString());
((ProgressReporterSphere)sender).sphere2.CenterPoint = new OpenTK.Vector3((float)centre2.x, (float)centre2.y, (float)centre2.z);
}
}
}
//Console.WriteLine("1a " + DateTime.Now.Millisecond);
// dont use dup data
if (lastcount == datacompass1.Count)
{
continue;
}
lastcount = datacompass1.Count;
// add to sphere with center correction
((ProgressReporterSphere)sender).sphere1.AddPoint(new OpenTK.Vector3(rawmx, rawmy, rawmz));
((ProgressReporterSphere)sender).sphere1.AimClear();
if (datacompass2.Count > 30)
{
float raw2mx = datacompass2[datacompass2.Count - 1].Item1;
float raw2my = datacompass2[datacompass2.Count - 1].Item2;
float raw2mz = datacompass2[datacompass2.Count - 1].Item3;
((ProgressReporterSphere)sender).sphere2.AddPoint(new OpenTK.Vector3(raw2mx, raw2my, raw2mz));
((ProgressReporterSphere)sender).sphere2.AimClear();
}
//Console.WriteLine("2 " + DateTime.Now.Millisecond);
HIL.Vector3 point;
point = new HIL.Vector3(rawmx, rawmy, rawmz) + centre;
//find the mean radius
float radius = 0;
for (int i = 0; i < datacompass1.Count; i++)
{
point = new HIL.Vector3(datacompass1[i].Item1, datacompass1[i].Item2, datacompass1[i].Item3);
radius += (float)(point + centre).length();
}
radius /= datacompass1.Count;
//test that we can find one point near a set of points all around the sphere surface
int pointshit = 0;
string displayresult = "";
int factor = 3; // pitch
int factor2 = 4; // yaw
float max_distance = radius / 3; //pretty generouse
for (int j = 0; j <= factor; j++)
{
double theta = (Math.PI * (j + 0.5)) / factor;
for (int i = 0; i <= factor2; i++)
{
double phi = (2 * Math.PI * i) / factor2;
HIL.Vector3 point_sphere = new HIL.Vector3(
(float)(Math.Sin(theta) * Math.Cos(phi) * radius),
(float)(Math.Sin(theta) * Math.Sin(phi) * radius),
(float)(Math.Cos(theta) * radius)) - centre;
//log.InfoFormat("magcalib check - {0} {1} dist {2}", theta * rad2deg, phi * rad2deg, max_distance);
bool found = false;
for (int k = 0; k < datacompass1.Count; k++)
{
point = new HIL.Vector3(datacompass1[k].Item1, datacompass1[k].Item2, datacompass1[k].Item3);
double d = (point_sphere - point).length();
if (d < max_distance)
{
pointshit++;
found = true;
break;
}
}
// draw them all
//((ProgressReporterSphere)sender).sphere1.AimFor(new OpenTK.Vector3((float)point_sphere.x, (float)point_sphere.y, (float)point_sphere.z));
if (!found)
{
displayresult = "more data needed Aim For " + GetColour((int)(theta * rad2deg), (int)(phi * rad2deg));
((ProgressReporterSphere)sender).sphere1.AimFor(new OpenTK.Vector3((float)point_sphere.x, (float)point_sphere.y, (float)point_sphere.z));
//j = factor;
//break;
}
}
}
extramsg = displayresult;
//Console.WriteLine("3 "+ DateTime.Now.Millisecond);
// check primary compass error
if (error < 0.2 && pointshit > hittarget && ((ProgressReporterSphere)sender).autoaccept)
{
extramsg = "";
break;
}
}
MainV2.comPort.UnSubscribeToPacketType(sub);
MainV2.comPort.UnSubscribeToPacketType(sub2);
// restore old sensor rate
MainV2.comPort.MAV.cs.ratesensors = backupratesens;
MainV2.comPort.MAV.cs.rateattitude = backuprateatt;
MainV2.comPort.MAV.cs.rateposition = backupratepos;
MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.comPort.MAV.cs.ratesensors);
MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.POSITION, MainV2.comPort.MAV.cs.rateposition); // request gps
MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.EXTRA1, MainV2.comPort.MAV.cs.rateattitude); // request attitude
MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.EXTRA2, MainV2.comPort.MAV.cs.rateattitude); // request vfr
MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.EXTRA3, MainV2.comPort.MAV.cs.ratesensors); // request extra stuff - tridge
MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.comPort.MAV.cs.ratesensors); // request raw sensor
MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RC_CHANNELS, MainV2.comPort.MAV.cs.raterc); // request rc info
if (MainV2.speechEnable)
{
MainV2.speechEngine.SpeakAsync("Compass Calibration Complete");
}
else
{
Console.Beep();
}
if (minx > 0 && maxx > 0 || minx < 0 && maxx < 0 || miny > 0 && maxy > 0 || miny < 0 && maxy < 0 || minz > 0 && maxz > 0 || minz < 0 && maxz < 0)
{
e.ErrorMessage = "Bad compass raw values. Check for magnetic interferance.";
ans = null;
ans2 = null;
return;
}
if (extramsg != "")
{
if (CustomMessageBox.Show(Strings.MissingDataPoints, Strings.RunAnyway, MessageBoxButtons.YesNo) == DialogResult.No)
{
e.CancelAcknowledged = true;
e.CancelRequested = true;
ans = null;
ans2 = null;
return;
}
}
// remove outlyers
RemoveOutliers(ref datacompass1);
if (havecompass2 && datacompass2.Count > 0)
{
RemoveOutliers(ref datacompass2);
}
if (datacompass1.Count < 10)
{
e.ErrorMessage = "Log does not contain enough data";
ans = null;
ans2 = null;
return;
}
bool ellipsoid = false;
if (MainV2.comPort.MAV.param.ContainsKey("MAG_DIA"))
{
ellipsoid = true;
}
log.Info("Compass 1");
ans = MagCalib.LeastSq(datacompass1, ellipsoid);
if (havecompass2 && datacompass2.Count > 0)
{
log.Info("Compass 2");
ans2 = MagCalib.LeastSq(datacompass2, ellipsoid);
}
}
static void prd_DoWork(object sender, ProgressWorkerEventArgs e, object passdata = null)
{
// turn learning off
MainV2.comPort.setParam("COMPASS_LEARN", 0);
bool havecompass2 = false;
//compass2 get mag2 offsets
float com2ofsx = 0;
float com2ofsy = 0;
float com2ofsz = 0;
if (MainV2.comPort.MAV.param.ContainsKey("COMPASS_OFS2_X"))
{
com2ofsx = MainV2.comPort.GetParam("COMPASS_OFS2_X");
com2ofsy = MainV2.comPort.GetParam("COMPASS_OFS2_Y");
com2ofsz = MainV2.comPort.GetParam("COMPASS_OFS2_Z");
havecompass2 = true;
}
// old method
float minx = 0;
float maxx = 0;
float miny = 0;
float maxy = 0;
float minz = 0;
float maxz = 0;
// backup current rate and set to 10 hz
byte backupratesens = MainV2.comPort.MAV.cs.ratesensors;
MainV2.comPort.MAV.cs.ratesensors = 10;
MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.comPort.MAV.cs.ratesensors); // mag captures at 10 hz
var sub = MainV2.comPort.SubscribeToPacketType(MAVLink.MAVLINK_MSG_ID.RAW_IMU, ReceviedPacket);
var sub2 = MainV2.comPort.SubscribeToPacketType(MAVLink.MAVLINK_MSG_ID.SCALED_IMU2, ReceviedPacket);
string extramsg = "";
((ProgressReporterSphere)sender).sphere1.Clear();
((ProgressReporterSphere)sender).sphere2.Clear();
int lastcount = 0;
DateTime lastlsq = DateTime.MinValue;
DateTime lastlsq2 = DateTime.MinValue;
HIL.Vector3 centre = new HIL.Vector3();
while (true)
{
// slow down execution
System.Threading.Thread.Sleep(20);
((ProgressReporterDialogue)sender).UpdateProgressAndStatus(-1, "Got " + datacompass1.Count + " Samples " + extramsg);
if (e.CancelRequested)
{
// restore old sensor rate
MainV2.comPort.MAV.cs.ratesensors = backupratesens;
MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.comPort.MAV.cs.ratesensors);
e.CancelAcknowledged = false;
e.CancelRequested = false;
break;
}
if (datacompass1.Count == 0)
continue;
// dont use dup data
if (lastcount == datacompass1.Count)
continue;
lastcount = datacompass1.Count;
float rawmx = datacompass1[datacompass1.Count - 1].Item1;
float rawmy = datacompass1[datacompass1.Count - 1].Item2;
float rawmz = datacompass1[datacompass1.Count - 1].Item3;
// for old method
setMinorMax(rawmx, ref minx, ref maxx);
setMinorMax(rawmy, ref miny, ref maxy);
setMinorMax(rawmz, ref minz, ref maxz);
// get the current estimated centerpoint
//new HIL.Vector3((float)-((maxx + minx) / 2), (float)-((maxy + miny) / 2), (float)-((maxz + minz) / 2));
// run lsq every seconds when more than 100 datapoints
if (datacompass1.Count > 100 && lastlsq.Second != DateTime.Now.Second)
{
lastlsq = DateTime.Now;
lock (datacompass1)
{
var lsq = MagCalib.LeastSq(datacompass1, false);
// simple validation
if (Math.Abs(lsq[0]) < 999)
{
centre = new HIL.Vector3(lsq[0], lsq[1], lsq[2]);
log.Info("new centre " + centre.ToString());
((ProgressReporterSphere)sender).sphere1.CenterPoint = new OpenTK.Vector3((float)centre.x, (float)centre.y, (float)centre.z);
}
}
}
// run lsq every seconds when more than 100 datapoints
if (datacompass2.Count > 100 && lastlsq2.Second != DateTime.Now.Second)
{
lastlsq2 = DateTime.Now;
lock (datacompass2)
{
var lsq = MagCalib.LeastSq(datacompass2, false);
// simple validation
if (Math.Abs(lsq[0]) < 999)
{
HIL.Vector3 centre2 = new HIL.Vector3(lsq[0], lsq[1], lsq[2]);
log.Info("new centre2 " + centre2.ToString());
((ProgressReporterSphere)sender).sphere2.CenterPoint = new OpenTK.Vector3((float)centre2.x, (float)centre2.y, (float)centre2.z);
}
}
}
HIL.Vector3 point;
// add to sphere with center correction
point = new HIL.Vector3(rawmx, rawmy, rawmz) + centre;
((ProgressReporterSphere)sender).sphere1.AddPoint(new OpenTK.Vector3(rawmx, rawmy, rawmz));
((ProgressReporterSphere)sender).sphere1.AimClear();
if (havecompass2 && datacompass2.Count > 0)
{
float raw2mx = datacompass2[datacompass2.Count - 1].Item1;
float raw2my = datacompass2[datacompass2.Count - 1].Item2;
float raw2mz = datacompass2[datacompass2.Count - 1].Item3;
((ProgressReporterSphere)sender).sphere2.AddPoint(new OpenTK.Vector3(raw2mx, raw2my, raw2mz));
((ProgressReporterSphere)sender).sphere2.AimClear();
}
//find the mean radius
float radius = 0;
for (int i = 0; i < datacompass1.Count; i++)
{
point = new HIL.Vector3(datacompass1[i].Item1, datacompass1[i].Item2, datacompass1[i].Item3);
radius += (float)(point + centre).length();
}
radius /= datacompass1.Count;
//test that we can find one point near a set of points all around the sphere surface
string displayresult = "";
int factor = 4; // 4 point check 16 points
float max_distance = radius / 3; //pretty generouse
for (int j = 0; j < factor; j++)
{
double theta = (Math.PI * (j + 0.5)) / factor;
for (int i = 0; i < factor; i++)
{
double phi = (2 * Math.PI * i) / factor;
HIL.Vector3 point_sphere = new HIL.Vector3(
(float)(Math.Sin(theta) * Math.Cos(phi) * radius),
(float)(Math.Sin(theta) * Math.Sin(phi) * radius),
(float)(Math.Cos(theta) * radius)) - centre;
//log.DebugFormat("magcalib check - {0} {1} dist {2}", theta * rad2deg, phi * rad2deg, max_distance);
bool found = false;
for (int k = 0; k < datacompass1.Count; k++)
{
point = new HIL.Vector3(datacompass1[k].Item1, datacompass1[k].Item2, datacompass1[k].Item3);
double d = (point_sphere - point).length();
if (d < max_distance)
{
found = true;
break;
}
}
if (!found)
{
displayresult = "more data needed " + (theta * rad2deg).ToString("0") + " " + (phi * rad2deg).ToString("0");
((ProgressReporterSphere)sender).sphere1.AimFor(new OpenTK.Vector3((float)point_sphere.x, (float)point_sphere.y, (float)point_sphere.z));
//j = factor;
break;
}
}
}
extramsg = displayresult;
}
MainV2.comPort.UnSubscribeToPacketType(sub);
MainV2.comPort.UnSubscribeToPacketType(sub2);
if (minx > 0 && maxx > 0 || minx < 0 && maxx < 0 || miny > 0 && maxy > 0 || miny < 0 && maxy < 0 || minz > 0 && maxz > 0 || minz < 0 && maxz < 0)
{
e.ErrorMessage = "Bad compass raw values. Check for magnetic interferance.";
ans = null;
ans2 = null;
return;
}
// restore old sensor rate
MainV2.comPort.MAV.cs.ratesensors = backupratesens;
MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.comPort.MAV.cs.ratesensors);
if (extramsg != "")
{
if (CustomMessageBox.Show("You are missing data points. do you want to run the calibration anyway?", "run anyway", MessageBoxButtons.YesNo) == DialogResult.No)
{
e.CancelAcknowledged = true;
e.CancelRequested = true;
ans = null;
ans2 = null;
return;
}
}
// remove outlyers
RemoveOutliers(ref datacompass1);
if (havecompass2 && datacompass2.Count > 0)
{
RemoveOutliers(ref datacompass2);
}
if (datacompass1.Count < 10)
{
e.ErrorMessage = "Log does not contain enough data";
ans = null;
ans2 = null;
return;
}
bool ellipsoid = false;
if (MainV2.comPort.MAV.param.ContainsKey("MAG_DIA"))
{
ellipsoid = true;
}
log.Info("Compass 1");
ans = MagCalib.LeastSq(datacompass1, ellipsoid);
if (havecompass2 && datacompass2.Count > 0)
{
log.Info("Compass 2");
ans2 = MagCalib.LeastSq(datacompass2, ellipsoid);
}
}
static void prd_DoWork(object sender, ProgressWorkerEventArgs e, object passdata = null)
{
// old method
float minx = 0;
float maxx = 0;
float miny = 0;
float maxy = 0;
float minz = 0;
float maxz = 0;
// backup current rate and set to 10 hz
byte backupratesens = MainV2.comPort.MAV.cs.ratesensors;
MainV2.comPort.MAV.cs.ratesensors = 10;
MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.comPort.MAV.cs.ratesensors); // mag captures at 10 hz
var sub = MainV2.comPort.SubscribeToPacketType(MAVLink.MAVLINK_MSG_ID.RAW_IMU, ReceviedPacket);
string extramsg = "";
((ProgressReporterSphere)sender).sphere1.Clear();
int lastcount = 0;
while (true)
{
// slow down execution
System.Threading.Thread.Sleep(20);
((ProgressReporterDialogue)sender).UpdateProgressAndStatus(-1, "Got " + data.Count + " Samples " + extramsg);
if (e.CancelRequested)
{
// restore old sensor rate
MainV2.comPort.MAV.cs.ratesensors = backupratesens;
MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.comPort.MAV.cs.ratesensors);
e.CancelAcknowledged = false;
e.CancelRequested = false;
break;
}
if (data.Count == 0)
{
continue;
}
// dont use dup data
if (lastcount == data.Count)
{
continue;
}
lastcount = data.Count;
float rawmx = data[data.Count - 1].Item1;
float rawmy = data[data.Count - 1].Item2;
float rawmz = data[data.Count - 1].Item3;
// for old method
setMinorMax(rawmx, ref minx, ref maxx);
setMinorMax(rawmy, ref miny, ref maxy);
setMinorMax(rawmz, ref minz, ref maxz);
// get the current estimated centerpoint
HIL.Vector3 centre = new HIL.Vector3((float)-((maxx + minx) / 2), (float)-((maxy + miny) / 2), (float)-((maxz + minz) / 2));
HIL.Vector3 point;
// add to sphere with center correction
point = new HIL.Vector3(rawmx, rawmy, rawmz) + centre;
((ProgressReporterSphere)sender).sphere1.AddPoint(new OpenTK.Vector3((float)point.x, (float)point.y, (float)point.z));
//find the mean radius
float radius = 0;
for (int i = 0; i < data.Count; i++)
{
point = new HIL.Vector3(data[i].Item1, data[i].Item2, data[i].Item3);
radius += (float)(point + centre).length();
}
radius /= data.Count;
//test that we can find one point near a set of points all around the sphere surface
int factor = 4; // 4 point check 16 points
float max_distance = radius / 3; //pretty generouse
for (int j = 0; j < factor; j++)
{
double theta = (Math.PI * (j + 0.5)) / factor;
for (int i = 0; i < factor; i++)
{
double phi = (2 * Math.PI * i) / factor;
HIL.Vector3 point_sphere = new HIL.Vector3(
(float)(Math.Sin(theta) * Math.Cos(phi) * radius),
(float)(Math.Sin(theta) * Math.Sin(phi) * radius),
(float)(Math.Cos(theta) * radius)) - centre;
//log.DebugFormat("magcalib check - {0} {1} dist {2}", theta * rad2deg, phi * rad2deg, max_distance);
bool found = false;
for (int k = 0; k < data.Count; k++)
{
point = new HIL.Vector3(data[k].Item1, data[k].Item2, data[k].Item3);
double d = (point_sphere - point).length();
if (d < max_distance)
{
found = true;
break;
}
}
if (!found)
{
extramsg = "more data needed";
//e.ErrorMessage = "Data missing for some directions";
//ans = null;
//return;
j = factor;
break;
}
else
{
extramsg = "";
}
}
}
}
MainV2.comPort.UnSubscribeToPacketType(sub);
if (minx > 0 && maxx > 0 || minx < 0 && maxx < 0 || miny > 0 && maxy > 0 || miny < 0 && maxy < 0 || minz > 0 && maxz > 0 || minz < 0 && maxz < 0)
{
e.ErrorMessage = "Bad compass raw values. Check for magnetic interferance.";
ans = null;
return;
}
// restore old sensor rate
MainV2.comPort.MAV.cs.ratesensors = backupratesens;
MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.comPort.MAV.cs.ratesensors);
if (extramsg != "")
{
if (CustomMessageBox.Show("You are missing data points. do you want to run the calibration anyway?", "run anyway", MessageBoxButtons.YesNo) == DialogResult.No)
{
e.CancelAcknowledged = true;
e.CancelRequested = true;
ans = null;
return;
}
}
// remove outlyers
RemoveOutliers(ref data);
if (data.Count < 10)
{
e.ErrorMessage = "Log does not contain enough data";
ans = null;
return;
}
bool ellipsoid = false;
if (MainV2.comPort.MAV.param.ContainsKey("MAG_DIA"))
{
ellipsoid = true;
}
ans = MagCalib.LeastSq(data, ellipsoid);
}
static void prd_DoWork(object sender, ProgressWorkerEventArgs e, object passdata = null)
{
// old method
float minx = 0;
float maxx = 0;
float miny = 0;
float maxy = 0;
float minz = 0;
float maxz = 0;
// backup current rate and set to 10 hz
byte backupratesens = MainV2.comPort.MAV.cs.ratesensors;
MainV2.comPort.MAV.cs.ratesensors = 10;
MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.comPort.MAV.cs.ratesensors); // mag captures at 10 hz
var sub = MainV2.comPort.SubscribeToPacketType(MAVLink.MAVLINK_MSG_ID.RAW_IMU, ReceviedPacket);
string extramsg = "";
((ProgressReporterSphere)sender).sphere1.Clear();
int lastcount = 0;
while (true)
{
// slow down execution
System.Threading.Thread.Sleep(20);
((ProgressReporterDialogue)sender).UpdateProgressAndStatus(-1, "Got " + data.Count + " Samples " + extramsg);
if (e.CancelRequested)
{
// restore old sensor rate
MainV2.comPort.MAV.cs.ratesensors = backupratesens;
MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.comPort.MAV.cs.ratesensors);
e.CancelAcknowledged = false;
e.CancelRequested = false;
break;
}
if (data.Count == 0)
continue;
// dont use dup data
if (lastcount == data.Count)
continue;
lastcount = data.Count;
float rawmx = data[data.Count - 1].Item1;
float rawmy = data[data.Count - 1].Item2;
float rawmz = data[data.Count - 1].Item3;
// for old method
setMinorMax(rawmx, ref minx, ref maxx);
setMinorMax(rawmy, ref miny, ref maxy);
setMinorMax(rawmz, ref minz, ref maxz);
// get the current estimated centerpoint
HIL.Vector3 centre = new HIL.Vector3((float)-((maxx + minx) / 2), (float)-((maxy + miny) / 2), (float)-((maxz + minz) / 2));
HIL.Vector3 point;
// add to sphere with center correction
point = new HIL.Vector3(rawmx, rawmy, rawmz) + centre;
((ProgressReporterSphere)sender).sphere1.AddPoint(new OpenTK.Vector3((float)point.x, (float)point.y, (float)point.z));
//find the mean radius
float radius = 0;
for (int i = 0; i < data.Count; i++)
{
point = new HIL.Vector3(data[i].Item1, data[i].Item2, data[i].Item3);
radius += (float)(point + centre).length();
}
radius /= data.Count;
//test that we can find one point near a set of points all around the sphere surface
int factor = 4; // 4 point check 16 points
float max_distance = radius / 3; //pretty generouse
for (int j = 0; j < factor; j++)
{
double theta = (Math.PI * (j + 0.5)) / factor;
for (int i = 0; i < factor; i++)
{
double phi = (2 * Math.PI * i) / factor;
HIL.Vector3 point_sphere = new HIL.Vector3(
(float)(Math.Sin(theta) * Math.Cos(phi) * radius),
(float)(Math.Sin(theta) * Math.Sin(phi) * radius),
(float)(Math.Cos(theta) * radius)) - centre;
//log.DebugFormat("magcalib check - {0} {1} dist {2}", theta * rad2deg, phi * rad2deg, max_distance);
bool found = false;
for (int k = 0; k < data.Count; k++)
{
point = new HIL.Vector3(data[k].Item1, data[k].Item2, data[k].Item3);
double d = (point_sphere - point).length();
if (d < max_distance)
{
found = true;
break;
}
}
if (!found)
{
extramsg = "more data needed";
//e.ErrorMessage = "Data missing for some directions";
//ans = null;
//return;
j = factor;
break;
}
else
{
extramsg = "";
}
}
}
}
MainV2.comPort.UnSubscribeToPacketType(sub);
if (minx > 0 && maxx > 0 || minx < 0 && maxx < 0 || miny > 0 && maxy > 0 || miny < 0 && maxy < 0 || minz > 0 && maxz > 0 || minz < 0 && maxz < 0)
{
e.ErrorMessage = "Bad compass raw values. Check for magnetic interferance.";
ans = null;
return;
}
// restore old sensor rate
MainV2.comPort.MAV.cs.ratesensors = backupratesens;
MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.comPort.MAV.cs.ratesensors);
if (extramsg != "")
{
if (CustomMessageBox.Show("You are missing data points. do you want to run the calibration anyway?", "run anyway", MessageBoxButtons.YesNo) == DialogResult.No)
{
e.CancelAcknowledged = true;
e.CancelRequested = true;
ans = null;
return;
}
}
// remove outlyers
RemoveOutliers(ref data);
if (data.Count < 10)
{
e.ErrorMessage = "Log does not contain enough data";
ans = null;
return;
}
bool ellipsoid = false;
if (MainV2.comPort.MAV.param.ContainsKey("MAG_DIA"))
{
ellipsoid = true;
}
ans = MagCalib.LeastSq(data, ellipsoid);
}
static void prd_DoWork(object sender, ProgressWorkerEventArgs e, object passdata = null)
{
// list of x,y,z 's
List<Tuple<float, float, float>> data = new List<Tuple<float, float, float>>();
// old method
float minx = 0;
float maxx = 0;
float miny = 0;
float maxy = 0;
float minz = 0;
float maxz = 0;
// backup current rate and set to 10 hz
byte backupratesens = MainV2.comPort.MAV.cs.ratesensors;
MainV2.comPort.MAV.cs.ratesensors = 10;
MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.comPort.MAV.cs.ratesensors); // mag captures at 10 hz
float oldmx = 0;
float oldmy = 0;
float oldmz = 0;
// filter data points to only x number per quadrant
int div = 20;
Hashtable filter = new Hashtable();
string extramsg = "";
((ProgressReporterSphere)sender).sphere1.Clear();
while (true)
{
// slow down execution
System.Threading.Thread.Sleep(1);
((ProgressReporterDialogue)sender).UpdateProgressAndStatus(-1, "Got " + data.Count + " Samples " + extramsg);
if (e.CancelRequested)
{
// restore old sensor rate
MainV2.comPort.MAV.cs.ratesensors = backupratesens;
MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.comPort.MAV.cs.ratesensors);
e.CancelAcknowledged = false;
e.CancelRequested = false;
break;
}
if (oldmx != MainV2.comPort.MAV.cs.mx &&
oldmy != MainV2.comPort.MAV.cs.my &&
oldmz != MainV2.comPort.MAV.cs.mz)
{
// for new lease sq
string item = (int)(MainV2.comPort.MAV.cs.mx / div) + "," +
(int)(MainV2.comPort.MAV.cs.my / div) + "," +
(int)(MainV2.comPort.MAV.cs.mz / div);
if (filter.ContainsKey(item))
{
filter[item] = (int)filter[item] + 1;
if ((int)filter[item] > 3)
continue;
}
else
{
filter[item] = 1;
}
// add data
data.Add(new Tuple<float, float, float>(
MainV2.comPort.MAV.cs.mx - (float)MainV2.comPort.MAV.cs.mag_ofs_x,
MainV2.comPort.MAV.cs.my - (float)MainV2.comPort.MAV.cs.mag_ofs_y,
MainV2.comPort.MAV.cs.mz - (float)MainV2.comPort.MAV.cs.mag_ofs_z));
oldmx = MainV2.comPort.MAV.cs.mx;
oldmy = MainV2.comPort.MAV.cs.my;
oldmz = MainV2.comPort.MAV.cs.mz;
// for old method
setMinorMax(MainV2.comPort.MAV.cs.mx - (float)MainV2.comPort.MAV.cs.mag_ofs_x, ref minx, ref maxx);
setMinorMax(MainV2.comPort.MAV.cs.my - (float)MainV2.comPort.MAV.cs.mag_ofs_y, ref miny, ref maxy);
setMinorMax(MainV2.comPort.MAV.cs.mz - (float)MainV2.comPort.MAV.cs.mag_ofs_z, ref minz, ref maxz);
// get the current estimated centerpoint
HIL.Vector3 centre = new HIL.Vector3((float)-((maxx + minx) / 2), (float)-((maxy + miny) / 2), (float)-((maxz + minz) / 2));
HIL.Vector3 point;
// add to sphere after trnslating the centre point
point = new HIL.Vector3(oldmx, oldmy, oldmz) + centre;
((ProgressReporterSphere)sender).sphere1.AddPoint(new OpenTK.Vector3((float)point.x, (float)point.y, (float)point.z));
//find the mean radius
float radius = 0;
for (int i = 0; i < data.Count; i++)
{
point = new HIL.Vector3(data[i].Item1, data[i].Item2, data[i].Item3);
radius += (float)(point - centre).length();
}
radius /= data.Count;
//test that we can find one point near a set of points all around the sphere surface
int factor = 4; // 4 point check 16 points
float max_distance = radius / 3; //pretty generouse
for (int j = 0; j < factor; j++)
{
double theta = (Math.PI * (j + 0.5)) / factor;
for (int i = 0; i < factor; i++)
{
double phi = (2 * Math.PI * i) / factor;
HIL.Vector3 point_sphere = new HIL.Vector3(
(float)(Math.Sin(theta) * Math.Cos(phi) * radius),
(float)(Math.Sin(theta) * Math.Sin(phi) * radius),
(float)(Math.Cos(theta) * radius)) - centre;
//log.DebugFormat("magcalib check - {0} {1} dist {2}", theta * rad2deg, phi * rad2deg, max_distance);
bool found = false;
for (int k = 0; k < data.Count; k++)
{
point = new HIL.Vector3(data[k].Item1, data[k].Item2, data[k].Item3);
double d = (point_sphere - point).length();
if (d < max_distance)
{
found = true;
break;
}
}
if (!found)
{
extramsg = "more data needed";
//e.ErrorMessage = "Data missing for some directions";
//ans = null;
//return;
j = factor;
break;
}
else
{
extramsg = "";
}
}
}
}
}
if (minx > 0 && maxx > 0 || minx < 0 && maxx < 0 ||miny > 0 && maxy > 0 || miny < 0 && maxy < 0 ||minz > 0 && maxz > 0 || minz < 0 && maxz < 0)
{
e.ErrorMessage = "Bad compass raw values. Check for magnetic interferance.";
ans = null;
return;
}
// restore old sensor rate
MainV2.comPort.MAV.cs.ratesensors = backupratesens;
MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.comPort.MAV.cs.ratesensors);
if (extramsg != "")
{
if (CustomMessageBox.Show("You are missing data points. do you want to run the calibration anyway?", "run anyway", MessageBoxButtons.YesNo) == DialogResult.No)
{
e.CancelAcknowledged = true;
e.CancelRequested = true;
ans = null;
return;
}
}
if (data.Count < 10)
{
e.ErrorMessage = "Log does not contain enough data";
ans = null;
return;
}
bool ellipsoid = false;
if (MainV2.comPort.MAV.param.ContainsKey("MAG_DIA"))
{
ellipsoid = true;
}
ans = MagCalib.LeastSq(data, ellipsoid);
}
public HIL.Vector3 getOffsetFromLeader(MAVLinkInterface leader, MAVLinkInterface mav)
{
//convert Wgs84ConversionInfo to utm
CoordinateTransformationFactory ctfac = new CoordinateTransformationFactory();
GeographicCoordinateSystem wgs84 = GeographicCoordinateSystem.WGS84;
int utmzone = (int) ((leader.MAV.cs.lng - -186.0)/6.0);
IProjectedCoordinateSystem utm = ProjectedCoordinateSystem.WGS84_UTM(utmzone,
leader.MAV.cs.lat < 0 ? false : true);
ICoordinateTransformation trans = ctfac.CreateFromCoordinateSystems(wgs84, utm);
double[] masterpll = {leader.MAV.cs.lng, leader.MAV.cs.lat};
// get leader utm coords
double[] masterutm = trans.MathTransform.Transform(masterpll);
double[] mavpll = {mav.MAV.cs.lng, mav.MAV.cs.lat};
//getLeader follower utm coords
double[] mavutm = trans.MathTransform.Transform(mavpll);
var heading = -leader.MAV.cs.yaw;
var norotation = new HIL.Vector3(masterutm[1] - mavutm[1], masterutm[0] - mavutm[0], 0);
float rad2deg = (float)(180 / Math.PI);
float deg2rad = (float)(1.0 / rad2deg);
norotation.x *= -1;
norotation.y *= -1;
return new HIL.Vector3( norotation.x * Math.Cos(heading * deg2rad) - norotation.y * Math.Sin(heading * deg2rad), norotation.x * Math.Sin(heading * deg2rad) + norotation.y * Math.Cos(heading * deg2rad), 0);
}