private void BUT_Updatepos_Click(object sender, EventArgs e)
{
foreach (var port in MainV2.Comports)
{
port.MAV.cs.UpdateCurrentSettings(null, true, port);
if (port == SwarmInterface.Leader)
{
continue;
}
HIL.Vector3 offset = getOffsetFromLeader(((Formation)SwarmInterface).getLeader(), port);
if (Math.Abs(offset.x) < 200 && Math.Abs(offset.y) < 200)
{
grid1.UpdateIcon(port, (float)offset.x, (float)offset.y, (float)offset.z, true);
//((Formation)SwarmInterface).setOffsets(port, offset.x, offset.y, offset.z);
}
}
}
private void BUT_compassorient_Click(object sender, EventArgs e)
{
BUT_compassorient.Text = "Continue";
MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, 10);
switch (step)
{
case 0:
label5.Text = "Please face the autopilot north";
break;
case 1:
north = new HIL.Vector3(MainV2.comPort.MAV.cs.mx, MainV2.comPort.MAV.cs.my, MainV2.comPort.MAV.cs.mz);
label5.Text = "Please face the autopilot east";
break;
case 2:
east = new HIL.Vector3(MainV2.comPort.MAV.cs.mx, MainV2.comPort.MAV.cs.my, MainV2.comPort.MAV.cs.mz);
label5.Text = "Please face the autopilot south";
break;
case 3:
south = new HIL.Vector3(MainV2.comPort.MAV.cs.mx, MainV2.comPort.MAV.cs.my, MainV2.comPort.MAV.cs.mz);
label5.Text = "Please face the autopilot west";
break;
case 4:
west = new HIL.Vector3(MainV2.comPort.MAV.cs.mx, MainV2.comPort.MAV.cs.my, MainV2.comPort.MAV.cs.mz);
label5.Text = "Calculating";
if (docalc())
{
}
else
{
label5.Text = "Error, please try again, verify where north is.";
}
BUT_compassorient.Text = "Start";
step = 0;
return;
}
step++;
}
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));
}
public void setOffsets(MAVState mav, double x, double y, double z)
{
offsets[mav] = new HIL.Vector3(x, y, z);
log.Info(mav.ToString() + " " + offsets[mav].ToString());
}
public void setOffsets(MAVLink mav, double x, double y, double z)
{
offsets[mav] = new HIL.Vector3(x,y,z);
//log.Info(mav.ToString() + " " + offsets[mav].ToString());
}
bool docalc()
{
try
{
// HIL.Vector3 magoff = new HIL.Vector3((float)MainV2.comPort.MAV.param["COMPASS_OFS_X"], (float)MainV2.comPort.MAV.param["COMPASS_OFS_Y"], (float)MainV2.comPort.MAV.param["COMPASS_OFS_Z"]);
//north -= magoff;
//east -= magoff;
//south -= magoff;
//west -= magoff;
}
catch { }
foreach (Common.Rotation item in Enum.GetValues(typeof(Common.Rotation)))
{
// copy them, as we dont want to change the originals
HIL.Vector3 northc = new HIL.Vector3(north);
HIL.Vector3 eastc = new HIL.Vector3(east);
HIL.Vector3 southc = new HIL.Vector3(south);
HIL.Vector3 westc = new HIL.Vector3(west);
northc.rotate(item);
eastc.rotate(item);
southc.rotate(item);
westc.rotate(item);
// test the copies
if (withinMargin(calcheading(northc), 35, 0))
{
if (withinMargin(calcheading(eastc), 35, 90))
{
if (withinMargin(calcheading(southc), 35, 180))
{
if (withinMargin(calcheading(westc), 35, 270))
{
Console.WriteLine("Rotation " + item.ToString());
label5.Text = "Done Rotation: " + item.ToString();
return true;
}
}
}
}
}
return false;
}
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());
}
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;
((ProgressReporterSphere)sender).sphere1.Clear();
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 = 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 = 2; // 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)
{
e.ErrorMessage = "Data missing for some directions";
ans = null;
return;
}
}
}
}
public void setOffsets(MAVLink mav, float x, float y, float z)
{
offsets[mav] = new HIL.Vector3(x,y,z);
//log.Info(mav.ToString() + " " + offsets[mav].ToString());
}
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;
((ProgressReporterSphere)sender).sphere1.Clear();
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 = 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;
}
}
}
}
