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> >();
// 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
DateTime deadline = DateTime.Now.AddSeconds(60);
float oldmx = 0;
float oldmy = 0;
float oldmz = 0;
while (deadline > DateTime.Now)
{
double timeremaining = (deadline - DateTime.Now).TotalSeconds;
((ProgressReporterDialogue)sender).UpdateProgressAndStatus((int)(((60 - timeremaining) / 60) * 100), timeremaining.ToString("0") + " Seconds - got " + data.Count + " Samples");
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 = true;
return;
}
if (oldmx != MainV2.comPort.MAV.cs.mx &&
oldmy != MainV2.comPort.MAV.cs.my &&
oldmz != MainV2.comPort.MAV.cs.mz)
{
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;
}
}
// 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)
{
e.ErrorMessage = "Log does not contain enough data";
ans = null;
return;
}
ans = MagCalib.LeastSq(data);
}
private void BUT_MagCalibration_Click(object sender, EventArgs e)
{
// list of x,y,z 's
List <Tuple <float, float, float> > data = new List <Tuple <float, float, float> >();
// backup current rate and set to 10 hz
byte backupratesens = MainV2.cs.ratesensors;
MainV2.cs.ratesensors = 10;
MainV2.comPort.requestDatastream((byte)MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.cs.ratesensors); // mag captures at 10 hz
CustomMessageBox.Show("Data will be collected for 30 seconds, Please click ok and move the apm around all axises");
DateTime deadline = DateTime.Now.AddSeconds(30);
float oldmx = 0;
float oldmy = 0;
float oldmz = 0;
while (deadline > DateTime.Now)
{
// dont let the gui hang
Application.DoEvents();
if (oldmx != MainV2.cs.mx &&
oldmy != MainV2.cs.my &&
oldmz != MainV2.cs.mz)
{
data.Add(new Tuple <float, float, float>(
MainV2.cs.mx - (float)MainV2.cs.mag_ofs_x,
MainV2.cs.my - (float)MainV2.cs.mag_ofs_y,
MainV2.cs.mz - (float)MainV2.cs.mag_ofs_z));
oldmx = MainV2.cs.mx;
oldmy = MainV2.cs.my;
oldmz = MainV2.cs.mz;
}
}
// restore old sensor rate
MainV2.cs.ratesensors = backupratesens;
MainV2.comPort.requestDatastream((byte)MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.cs.ratesensors);
if (data.Count < 10)
{
CustomMessageBox.Show("Log does not contain enough data");
return;
}
double[] ans = MagCalib.LeastSq(data);
MagCalib.SaveOffsets(ans);
}
private void BUT_MagCalibration_Click(object sender, EventArgs e)
{
if (DialogResult.Yes == CustomMessageBox.Show("Use live data, or a log\n\nYes for Live data", "Mag Calibration", MessageBoxButtons.YesNo))
{
List <Tuple <float, float, float> > data = new List <Tuple <float, float, float> >();
byte backupratesens = MainV2.cs.ratesensors;
MainV2.cs.ratesensors = 10;
MainV2.comPort.requestDatastream((byte)MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.cs.ratesensors); // mag captures at 10 hz
CustomMessageBox.Show("Data will be collected for 30 seconds, Please click ok and move the apm around all axises");
DateTime deadline = DateTime.Now.AddSeconds(30);
float oldmx = 0;
float oldmy = 0;
float oldmz = 0;
while (deadline > DateTime.Now)
{
Application.DoEvents();
if (oldmx != MainV2.cs.mx &&
oldmy != MainV2.cs.my &&
oldmz != MainV2.cs.mz)
{
data.Add(new Tuple <float, float, float>(
MainV2.cs.mx - (float)MainV2.cs.mag_ofs_x,
MainV2.cs.my - (float)MainV2.cs.mag_ofs_y,
MainV2.cs.mz - (float)MainV2.cs.mag_ofs_z));
oldmx = MainV2.cs.mx;
oldmy = MainV2.cs.my;
oldmz = MainV2.cs.mz;
}
}
MainV2.cs.ratesensors = backupratesens;
if (data.Count < 10)
{
CustomMessageBox.Show("Log does not contain enough data");
return;
}
double[] ans = MagCalib.LeastSq(data);
MagCalib.SaveOffsets(ans);
}
else
{
string minthro = "30";
Common.InputBox("Min Throttle", "Use only data above this throttle percent.", ref minthro);
int ans = 0;
int.TryParse(minthro, out ans);
MagCalib.ProcessLog(ans);
}
}
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> >();
// 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
DateTime deadline = DateTime.Now.AddSeconds(60);
float oldmx = 0;
float oldmy = 0;
float oldmz = 0;
while (deadline > DateTime.Now)
{
double timeremaining = (deadline - DateTime.Now).TotalSeconds;
((ProgressReporterDialogue)sender).UpdateProgressAndStatus((int)(((60 - timeremaining) / 60) * 100), timeremaining.ToString("0") + " Seconds - got " + data.Count + " Samples");
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 = true;
return;
}
if (oldmx != MainV2.comPort.MAV.cs.mx &&
oldmy != MainV2.comPort.MAV.cs.my &&
oldmz != MainV2.comPort.MAV.cs.mz)
{
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;
}
}
// 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)
{
e.ErrorMessage = "Log does not contain enough data";
ans = null;
return;
}
ans = MagCalib.LeastSq(data);
//find the mean radius
Vector3f centre = new Vector3f((float)-ans[0], (float)-ans[1], (float)-ans[2]);
Vector3f point;
float radius = 0;
for (int i = 0; i < data.Count; i++)
{
point = new Vector3f(data[i].Item1, data[i].Item2, data[i].Item3);
radius += Vector3f.Distance(point, centre);
}
radius /= data.Count;
//test that we can find one point near a set of points all around the sphere surface
int factor = 10;
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;
Vector3f point_sphere = new Vector3f(
(float)(Math.Sin(theta) * Math.Cos(phi) * radius),
(float)(Math.Sin(theta) * Math.Sin(phi) * radius),
(float)(Math.Cos(theta) * radius)) + centre;
bool found = false;
for (int k = 0; k < data.Count; k++)
{
point = new Vector3f(data[i].Item1, data[i].Item2, data[i].Item3);
double d = Vector3f.Distance(point_sphere, point);
if (d < max_distance)
{
found = true;
break;
}
}
if (!found)
{
e.ErrorMessage = "Data missing for some directions";
ans = null;
return;
}
}
}
}
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> >();
// 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;
((ProgressReporterSphere)sender).sphere1.Clear();
while (true)
{
((ProgressReporterDialogue)sender).UpdateProgressAndStatus(-1, "Got " + data.Count + " Samples");
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)
{
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));
}
}
// 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)
{
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);
//find the mean radius
HIL.Vector3 centre = new HIL.Vector3((float)-ans[0], (float)-ans[1], (float)-ans[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 = 3; // 9 point check 3x3
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)
{
e.ErrorMessage = "Data missing for some directions";
ans = null;
return;
}
}
}
}