private void BUT_MagCalibration_Click(object sender, EventArgs e)
{
if (!MainV2.comPort.BaseStream.IsOpen)
{
CustomMessageBox.Show("You are no longer connected to the board\n the wizard will now exit", "Error");
Wizard.instance.Close();
}
MainV2.comPort.MAV.cs.ratesensors = 2;
MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.EXTRA3, MainV2.comPort.MAV.cs.ratesensors);
MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.comPort.MAV.cs.ratesensors);
MainV2.comPort.setParam("MAG_ENABLE", 1);
CustomMessageBox.Show("Please click ok and move the apm around all axises");
ProgressReporterSphere prd = new ProgressReporterSphere();
Utilities.ThemeManager.ApplyThemeTo(prd);
prd.btnCancel.Text = "Done";
prd.DoWork += prd_DoWork;
prd.RunBackgroundOperationAsync();
if (ans != null)
{
MagCalib.SaveOffsets(ans);
}
}
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)
{
CustomMessageBox.Show("Data will be collected for 60 seconds, Please click ok and move the apm around all axises");
ProgressReporterDialogue prd = new ProgressReporterDialogue();
Utilities.ThemeManager.ApplyThemeTo(prd);
prd.DoWork += prd_DoWork;
prd.RunBackgroundOperationAsync();
if (ans != null)
{
MagCalib.SaveOffsets(ans);
}
}
private void BUT_MagCalibration_Click(object sender, EventArgs e)
{
CustomMessageBox.Show("Please click ok and move the apm around all axises");
ProgressReporterSphere prd = new ProgressReporterSphere();
prd.btnCancel.Text = "Done";
Utilities.ThemeManager.ApplyThemeTo(prd);
prd.DoWork += prd_DoWork;
prd.RunBackgroundOperationAsync();
if (ans != null)
{
MagCalib.SaveOffsets(ans);
}
}
private void BUT_MagCalibration_Click(object sender, EventArgs e)
{
MainV2.comPort.MAV.cs.ratesensors = 2;
MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.EXTRA3, MainV2.comPort.MAV.cs.ratesensors);
MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.comPort.MAV.cs.ratesensors);
MainV2.comPort.setParam("MAG_ENABLE", 1);
CustomMessageBox.Show("Data will be collected for 60 seconds, Please click ok and move the apm around all axises");
ProgressReporterDialogue prd = new ProgressReporterDialogue();
Utilities.ThemeManager.ApplyThemeTo(prd);
prd.DoWork += prd_DoWork;
prd.RunBackgroundOperationAsync();
if (ans != null)
{
MagCalib.SaveOffsets(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");
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)
{
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);
}
}
