public static void SaveOffsets2(double[] ofs)
{
MavLinkInterface mavLinkInterface = new MavLinkInterface();
MavLink.mavlink_command_long_t req = new MavLink.mavlink_command_long_t();
req.target_system = Global.sysID;
req.target_component = Global.compID;
req.command = (ushort)242;
req.param1 = 5;
req.param2 = (float)ofs[0];
req.param3 = (float)ofs[1];
req.param4 = (float)ofs[2];
req.param5 = 0;
req.param6 = 0;
req.param7 = 0;
// send each one twice.
mavLinkInterface.AssembleAndSendFrame((byte)MavLink.MAVLINK_MSG_ID.REQUEST_DATA_STREAM, req);
mavLinkInterface.AssembleAndSendFrame((byte)MavLink.MAVLINK_MSG_ID.REQUEST_DATA_STREAM, req);
MessageBox.Show(
"New offsets for compass #1 are " + ofs[0].ToString("0") + " " + ofs[1].ToString("0") + " " +
ofs[2].ToString("0") + "\nThese have been saved for you.", "New Mag Offsets");
}
static void prd_DoWork(object sender, ProgressWorkerEventArgs e, object passdata = null)
{
bool havecompass2 = false;
havecompass2 = true;
int hittarget = 14;
// old method
float minx = 0;
float maxx = 0;
float miny = 0;
float maxy = 0;
float minz = 0;
float maxz = 0;
MavLinkInterface mavLinkInterface = new MavLinkInterface();
MavLink.mavlink_request_data_stream_t req = new MavLink.mavlink_request_data_stream_t();
mavLinkInterface = new MavLinkInterface();
req = new MavLink.mavlink_request_data_stream_t();
req.target_system = Global.sysID;
req.target_component = Global.compID;
req.req_message_rate = 30; //50
req.start_stop = 1; // start
req.req_stream_id = (byte)1; // id 1
// send each one twice.
mavLinkInterface.AssembleAndSendFrame((byte)MavLink.MAVLINK_MSG_ID.REQUEST_DATA_STREAM, req);
mavLinkInterface.AssembleAndSendFrame((byte)MavLink.MAVLINK_MSG_ID.REQUEST_DATA_STREAM, req);
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;
DateTime lastlsq3 = DateTime.MinValue;
HI.Vector3 centre = new HI.Vector3();
int pointshitnum = 0;
while (true)
{
// slow down execution
System.Threading.Thread.Sleep(10);
string str = " 共有 " + datacompass1.Count + " 份数据\n\n" +
" 罗盘 1 误差：" + error + "\n" +
" 罗盘 2 误差：" + error2 + "\n" +
" 结束条件，罗盘 1 误差不大于0.2";
str += "\n " + extramsg;
((ProgressReporterDialogue)sender).UpdateProgressAndStatus(-1, str);
addcompass1list();
addcompass2list();
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);
if (datacompass1.Count > 100 && lastlsq.Second != DateTime.Now.Second) //w
{
//req = new MavLink.mavlink_request_data_stream_t();
//req.target_system = Global.sysID;
//req.target_component = Global.compID;
//req.req_message_rate = 30; //50
//req.start_stop = 1; // start
//req.req_stream_id = (byte)1; // id 1
//// send each one twice.
//mavLinkInterface.AssembleAndSendFrame((byte)MavLink.MAVLINK_MSG_ID.REQUEST_DATA_STREAM, req);
//mavLinkInterface.AssembleAndSendFrame((byte)MavLink.MAVLINK_MSG_ID.REQUEST_DATA_STREAM, req);
lastlsq = DateTime.Now;
lock (datacompass1)
{
var lsq = MagCalib.LeastSq(datacompass1, false);
// simple validation
if (Math.Abs(lsq[0]) < 999)
{
centre = new HI.Vector3(lsq[0], lsq[1], lsq[2]);
((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)//w
{
lastlsq2 = DateTime.Now;
lock (datacompass2)
{
var lsq = MagCalib.LeastSq(datacompass2, false);
// simple validation
if (Math.Abs(lsq[0]) < 999)
{
HI.Vector3 centre2 = new HI.Vector3(lsq[0], lsq[1], lsq[2]);
((ProgressReporterSphere)sender).sphere2.CenterPoint = new OpenTK.Vector3(
(float)centre2.x, (float)centre2.y, (float)centre2.z);
}
}
}
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)//w
{
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();
}
HI.Vector3 point;
point = new HI.Vector3(rawmx, rawmy, rawmz) + centre;
//find the mean radius
float radius = 0;
for (int i = 0; i < datacompass1.Count; i++)
{
point = new HI.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;
HI.Vector3 point_sphere = new HI.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 HI.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 = "需要更多的数据 " +
// 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;
if (error < 0.2 && pointshit > hittarget && ((ProgressReporterSphere)sender).autoaccept)
{
extramsg = "";
MessageBox.Show("旋转完成\r\n请自行关闭罗盘校准窗口");
break;
}
}
req = new MavLink.mavlink_request_data_stream_t();
req.target_system = Global.sysID;
req.target_component = Global.compID;
req.req_message_rate = 2; //50
req.start_stop = 1; // start
req.req_stream_id = (byte)1; // id 1
// send each one twice.
mavLinkInterface.AssembleAndSendFrame((byte)MavLink.MAVLINK_MSG_ID.REQUEST_DATA_STREAM, req);
mavLinkInterface.AssembleAndSendFrame((byte)MavLink.MAVLINK_MSG_ID.REQUEST_DATA_STREAM, req);
ans = MagCalib.LeastSq(datacompass1, false);
ans2 = MagCalib.LeastSq(datacompass2, false);
}
