public ProgressReporterDialogue()
{
InitializeComponent();
doWorkArgs = new ProgressWorkerEventArgs();
this.AutoScaleMode = System.Windows.Forms.AutoScaleMode.None;
this.btnClose.Visible = false;
}
public ProgressReporterDialogue()
{
InitializeComponent();
doWorkArgs = new ProgressWorkerEventArgs();
this.btnClose.Visible = false;
}
public ProgressReporterDialogue()
{
InitializeComponent();
doWorkArgs = new ProgressWorkerEventArgs();
this.AutoScaleMode = System.Windows.Forms.AutoScaleMode.None;
this.btnClose.Visible = false;
}
private void pdr_DoWork(object sender, ProgressWorkerEventArgs e, object passdata = null)
{
var fw = new Firmware();
fw.Progress -= fw_Progress1;
fw.Progress += fw_Progress;
softwares = fw.getFWList(firmwareurl);
foreach (var soft in softwares)
{
updateDisplayNameInvoke(soft);
}
}
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);
}
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);
}
private void OpenBg(object PRsender, bool getparams, ProgressWorkerEventArgs progressWorkerEventArgs)
{
frmProgressReporter.UpdateProgressAndStatus(-1, Strings.MavlinkConnecting);
giveComport = true;
if (BaseStream is SerialPort)
{
// allow settings to settle - previous dtr
System.Threading.Thread.Sleep(1000);
}
Terrain = new TerrainFollow(this);
bool hbseen = false;
try
{
BaseStream.ReadBufferSize = 16*1024;
lock (objlock) // so we dont have random traffic
{
log.Info("Open port with " + BaseStream.PortName + " " + BaseStream.BaudRate);
BaseStream.Open();
BaseStream.DiscardInBuffer();
// other boards seem to have issues if there is no delay? posible bootloader timeout issue
Thread.Sleep(1000);
}
MAVLinkMessage buffer = new MAVLinkMessage();
MAVLinkMessage buffer1 = new MAVLinkMessage();
DateTime start = DateTime.Now;
DateTime deadline = start.AddSeconds(CONNECT_TIMEOUT_SECONDS);
var countDown = new System.Timers.Timer {Interval = 1000, AutoReset = false};
countDown.Elapsed += (sender, e) =>
{
int secondsRemaining = (deadline - e.SignalTime).Seconds;
frmProgressReporter.UpdateProgressAndStatus(-1, string.Format(Strings.Trying, secondsRemaining));
if (secondsRemaining > 0) countDown.Start();
};
countDown.Start();
// px4 native
BaseStream.WriteLine("sh /etc/init.d/rc.usb");
int count = 0;
while (true)
{
if (progressWorkerEventArgs.CancelRequested)
{
progressWorkerEventArgs.CancelAcknowledged = true;
countDown.Stop();
if (BaseStream.IsOpen)
BaseStream.Close();
giveComport = false;
return;
}
log.Info(DateTime.Now.Millisecond + " Start connect loop ");
if (DateTime.Now > deadline)
{
//if (Progress != null)
// Progress(-1, "No Heartbeat Packets");
countDown.Stop();
this.Close();
if (hbseen)
{
progressWorkerEventArgs.ErrorMessage = Strings.Only1Hb;
throw new Exception(Strings.Only1HbD);
}
else
{
progressWorkerEventArgs.ErrorMessage = "No Heartbeat Packets Received";
throw new Exception(@"Can not establish a connection\n
Please check the following
1. You have firmware loaded
2. You have the correct serial port selected
3. PX4 - You have the microsd card installed
4. Try a diffrent usb port\n\n" +
"No Mavlink Heartbeat Packets where read from this port - Verify Baud Rate and setup\nMission Planner waits for 2 valid heartbeat packets before connecting");
}
}
System.Threading.Thread.Sleep(1);
// can see 2 heartbeat packets at any time, and will connect - was one after the other
if (buffer.Length == 0)
buffer = getHeartBeat();
System.Threading.Thread.Sleep(1);
if (buffer1.Length == 0)
buffer1 = getHeartBeat();
if (buffer.Length > 0 || buffer1.Length > 0)
hbseen = true;
count++;
// 2 hbs that match
if (buffer.Length > 5 && buffer1.Length > 5 && buffer.sysid == buffer1.sysid && buffer.compid == buffer1.compid)
{
mavlink_heartbeat_t hb = buffer.ToStructure<mavlink_heartbeat_t>();
if (hb.type != (byte) MAVLink.MAV_TYPE.GCS)
{
SetupMavConnect(buffer, hb);
break;
}
}
// 2 hb's that dont match. more than one sysid here
if (buffer.Length > 5 && buffer1.Length > 5 && (buffer.sysid == buffer1.sysid || buffer.compid == buffer1.compid))
{
mavlink_heartbeat_t hb = buffer.ToStructure<mavlink_heartbeat_t>();
if (hb.type != (byte) MAVLink.MAV_TYPE.ANTENNA_TRACKER && hb.type != (byte) MAVLink.MAV_TYPE.GCS)
{
SetupMavConnect(buffer, hb);
break;
}
hb = buffer1.ToStructure<mavlink_heartbeat_t>();
if (hb.type != (byte) MAVLink.MAV_TYPE.ANTENNA_TRACKER && hb.type != (byte) MAVLink.MAV_TYPE.GCS)
{
SetupMavConnect(buffer1, hb);
break;
}
}
}
countDown.Stop();
getVersion();
frmProgressReporter.UpdateProgressAndStatus(0,
"Getting Params.. (sysid " + MAV.sysid + " compid " + MAV.compid + ") ");
if (getparams)
{
getParamListBG();
}
byte[] temp = ASCIIEncoding.ASCII.GetBytes("Mission Planner " + Application.ProductVersion + "\0");
Array.Resize(ref temp, 50);
generatePacket((byte) MAVLINK_MSG_ID.STATUSTEXT,
new mavlink_statustext_t() {severity = (byte) MAV_SEVERITY.INFO, text = temp});
if (frmProgressReporter.doWorkArgs.CancelAcknowledged == true)
{
giveComport = false;
if (BaseStream.IsOpen)
BaseStream.Close();
return;
}
}
catch (Exception e)
{
try
{
BaseStream.Close();
}
catch
{
}
giveComport = false;
if (string.IsNullOrEmpty(progressWorkerEventArgs.ErrorMessage))
progressWorkerEventArgs.ErrorMessage = Strings.ConnectFailed;
log.Error(e);
throw;
}
//frmProgressReporter.Close();
giveComport = false;
frmProgressReporter.UpdateProgressAndStatus(100, Strings.Done);
log.Info("Done open " + MAV.sysid + " " + MAV.compid);
MAV.packetslost = 0;
MAV.synclost = 0;
}
void FrmProgressReporterGetParams(object sender, ProgressWorkerEventArgs e, object passdata = null)
{
getParamListBG();
}
void saveWPs(object sender, ProgressWorkerEventArgs e, object passdata = null)
{
try
{
MAVLinkInterface port = comPort;
if (!port.BaseStream.IsOpen)
{
//throw new Exception("Please connect first!");
throw new Exception("请先连接无人机!");
}
comPort.giveComport = true;
int a = 0;
// define the home point
Locationwp home = new Locationwp();
try
{
home.id = (byte)MAVLink.MAV_CMD.WAYPOINT;
home.lat = (double.Parse(TXT_homelat.Text));
home.lng = (double.Parse(TXT_homelng.Text));
home.alt = (float.Parse(TXT_homealt.Text) / CurrentState.multiplierdist); // use saved home
}
catch { throw new Exception("Your home location is invalid"); }
// log
//log.Info("wps values " + comPort.MAV.wps.Values.Count);
//log.Info("cmd rows " + (Commands.Rows.Count + 1)); // + home
// check for changes / future mod to send just changed wp's
if (comPort.MAV.wps.Values.Count == (Commands.Rows.Count + 1))
{
Hashtable wpstoupload = new Hashtable();
a = -1;
foreach (var item in comPort.MAV.wps.Values)
{
// skip home
if (a == -1)
{
a++;
continue;
}
MAVLink.mavlink_mission_item_t temp = DataViewtoLocationwp(a);
if (temp.command == item.command &&
temp.x == item.x &&
temp.y == item.y &&
temp.z == item.z &&
temp.param1 == item.param1 &&
temp.param2 == item.param2 &&
temp.param3 == item.param3 &&
temp.param4 == item.param4
)
{
//log.Info("wp match " + (a + 1));
}
else
{
//log.Info("wp no match" + (a + 1));
wpstoupload[a] = "";
}
a++;
}
}
// set wp total
((ProgressReporterDialogue)sender).UpdateProgressAndStatus(0, "上传航点数量");
ushort totalwpcountforupload = (ushort)(Commands.Rows.Count + 1);
port.setWPTotal(totalwpcountforupload); // + home
// set home location - overwritten/ignored depending on firmware.
((ProgressReporterDialogue)sender).UpdateProgressAndStatus(0, "上传回家点");
var homeans = port.setWP(home, 0, MAVLink.MAV_FRAME.GLOBAL, 0);
if (homeans != MAVLink.MAV_MISSION_RESULT.MAV_MISSION_ACCEPTED)
{
CustomMessageBox.Show(Strings.ErrorRejectedByMAV, Strings.ERROR);
return;
}
// define the default frame.
MAVLink.MAV_FRAME frame = MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT;
// get the command list from the datagrid
var commandlist = GetCommandList();
// upload from wp1, as home is alreadey sent
a = 1;
// process commandlist to the mav
foreach (var temp in commandlist)
{
// this code below fails
//a = commandlist.IndexOf(temp) + 1;
((ProgressReporterDialogue)sender).UpdateProgressAndStatus(a * 100 / Commands.Rows.Count, "上传航点 " + a);
// make sure we are using the correct frame for these commands
if (temp.id < (byte)MAVLink.MAV_CMD.LAST || temp.id == (byte)MAVLink.MAV_CMD.DO_SET_HOME)
{
var mode = currentaltmode;
if (mode == altmode.Terrain)
{
frame = MAVLink.MAV_FRAME.GLOBAL_TERRAIN_ALT;
}
else if (mode == altmode.Absolute)
{
frame = MAVLink.MAV_FRAME.GLOBAL;
}
else
{
frame = MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT;
}
}
// try send the wp
MAVLink.MAV_MISSION_RESULT ans = port.setWP(temp, (ushort)(a), frame, 0);
// we timed out while uploading wps/ command wasnt replaced/ command wasnt added
if (ans == MAVLink.MAV_MISSION_RESULT.MAV_MISSION_ERROR)
{
// resend for partial upload
port.setWPPartialUpdate((ushort)(a), totalwpcountforupload);
// reupload this point.
ans = port.setWP(temp, (ushort)(a), frame, 0);
}
if (ans == MAVLink.MAV_MISSION_RESULT.MAV_MISSION_NO_SPACE)
{
//e.ErrorMessage = "Upload failed, please reduce the number of wp's";
e.ErrorMessage = "上传航点失败,航点数量太多,请减少航点数量.";
return;
}
if (ans == MAVLink.MAV_MISSION_RESULT.MAV_MISSION_INVALID)
{
e.ErrorMessage = "Upload failed, mission was rejected byt the Mav,\n item had a bad option wp# " + a + " " + ans;
return;
}
if (ans == MAVLink.MAV_MISSION_RESULT.MAV_MISSION_INVALID_SEQUENCE)
{
// invalid sequence can only occur if we failed to see a response from the apm when we sent the request.
// therefore it did see the request and has moved on that step, and so do we.
a++;
continue;
}
if (ans != MAVLink.MAV_MISSION_RESULT.MAV_MISSION_ACCEPTED)
{
//e.ErrorMessage = "Upload wps failed " + Enum.Parse(typeof(MAVLink.MAV_CMD), temp.id.ToString()) + " " + Enum.Parse(typeof(MAVLink.MAV_MISSION_RESULT), ans.ToString());
e.ErrorMessage = "上传失败 " + Enum.Parse(typeof(MAVLink.MAV_CMD), temp.id.ToString()) + " " + Enum.Parse(typeof(MAVLink.MAV_MISSION_RESULT), ans.ToString());
return;
}
a++;
}
port.setWPACK();
((ProgressReporterDialogue)sender).UpdateProgressAndStatus(95, "Setting params");
// m
port.setParam("WP_RADIUS", (byte)int.Parse(TXT_WPRad.Text) / CurrentState.multiplierdist);
// cm's
port.setParam("WPNAV_RADIUS", (byte)int.Parse(TXT_WPRad.Text) / CurrentState.multiplierdist * 100);
try
{
port.setParam(new[] { "LOITER_RAD", "WP_LOITER_RAD" }, int.Parse(TXT_loiterrad.Text) / CurrentState.multiplierdist);
}
catch
{
}
((ProgressReporterDialogue)sender).UpdateProgressAndStatus(100, "完成.");
}
catch (Exception ex)
{
log.Error(ex); comPort.giveComport = false; throw;
}
comPort.giveComport = false;
}
void prd_DoWork(object sender, ProgressWorkerEventArgs e, object passdata = null)
{
this.ConvertBini(inputfn, outputfn, true);
}
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;
}
}
}
}
static void DoUpdateWorker_DoWork(object sender, ProgressWorkerEventArgs e, object passdata = null)
{
// TODO: Is this the right place?
#region Fetch Parameter Meta Data
var progressReporterDialogue = ((ProgressReporterDialogue) sender);
progressReporterDialogue.UpdateProgressAndStatus(-1, "Getting Updated Parameters");
try
{
ParameterMetaDataParser.GetParameterInformation();
}
catch (Exception ex)
{
log.Error(ex.ToString());
CustomMessageBox.Show("Error getting Parameter Information");
}
#endregion Fetch Parameter Meta Data
progressReporterDialogue.UpdateProgressAndStatus(-1, "Getting Base URL");
try
{
File.WriteAllText(Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + "writetest.txt", "this is a test");
}
catch(Exception ex)
{
log.Info("Write test failed");
throw new Exception("Unable to write to the install directory", ex);
}
finally
{
try
{
File.Delete(Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + "writetest.txt");
}
catch
{
log.Info("Write test cleanup failed");
}
}
// check for updates
// if (Debugger.IsAttached)
{
// log.Info("Skipping update test as it appears we are debugging");
}
// else
{
updateCheckMain(progressReporterDialogue);
}
}
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);
}
}
void DoCalibration(object sender, ProgressWorkerEventArgs e, object passdata = null)
{
var prd = ((ProgressReporterDialogue)sender);
prd.UpdateProgressAndStatus(-1, "Starting Compass Mot");
int8_t comp_type; // throttle or current based compensation
Vector3f compass_base = new Vector3f(); // compass vector when throttle is zero
Vector3f motor_impact = new Vector3f(); // impact of motors on compass vector
Vector3f motor_impact_scaled; // impact of motors on compass vector scaled with throttle
Vector3f motor_compensation; // final compensation to be stored to eeprom
float throttle_pct; // throttle as a percentage 0.0 ~ 1.0
float throttle_pct_max = 0.0f; // maximum throttle reached (as a percentage 0~1.0)
float current_amps_max = 0.0f; // maximum current reached
float interference_pct; // interference as a percentage of total mag field (for reporting purposes only)
//uint32_t last_run_time;
uint8_t print_counter = 49;
bool updated = false; // have we updated the compensation vector at least once
if ((float)MainV2.comPort.MAV.param["BATT_MONITOR"] == 4f) // volt and current
{
comp_type = (sbyte)comptype.Current;
prd.UpdateProgressAndStatus(-1, "Compass Mot using current");
}
else
{
comp_type = (sbyte)comptype.Throttle;
prd.UpdateProgressAndStatus(-1, "Compass Mot using throttle");
}
if ((float)MainV2.comPort.MAV.param["COMPASS_USE"] != 1)
{
e.ErrorMessage = "Compass is disabled";
return;
}
// request streams
MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RC_CHANNELS, 10); // rc out
MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, 50); // mag out
// reset compass mot
MainV2.comPort.setParam("COMPASS_MOTCT ", 0.0f);
MainV2.comPort.setParam("COMPASS_MOT_X", 0.000000f);
MainV2.comPort.setParam("COMPASS_MOT_Y", 0.000000f);
MainV2.comPort.setParam("COMPASS_MOT_Z", 0.000000f);
// store initial x,y,z compass values
compass_base.x = MainV2.comPort.MAV.cs.mx;
compass_base.y = MainV2.comPort.MAV.cs.my;
compass_base.z = MainV2.comPort.MAV.cs.mz;
// initialise motor compensation
motor_compensation = new Vector3f(0, 0, 0);
int magseen = MainV2.comPort.MAV.packetseencount[(byte)MAVLink.MAVLINK_MSG_ID.RAW_IMU];
int rcseen = MainV2.comPort.MAV.packetseencount[(byte)MAVLink.MAVLINK_MSG_ID.HIL_RC_INPUTS_RAW];
DateTime deadline = DateTime.Now.AddSeconds(10);
prd.UpdateProgressAndStatus(-1, "Waiting for Mag and RC data");
while (true)
{
if (magseen > (magseen + 100) && rcseen > (rcseen + 20))
{
break;
}
if (e.CancelRequested)
{
e.CancelAcknowledged = true;
return;
}
if (DateTime.Now > deadline)
{
e.ErrorMessage = "Not enough packets where received\n" + magseen + " mag " + rcseen + " rc";
return;
}
}
while (true)
{
if (prd.doWorkArgs.CancelRequested)
{
prd.doWorkArgs.CancelAcknowledged = true;
break;
}
// radio
// passthorugh - cant do.
// compass read
// battery read
// calculate scaling for throttle
int checkme;
throttle_pct = (float)MainV2.comPort.MAV.cs.ch3percent / 100.0f;
throttle_pct = constrain_float(throttle_pct, 0.0f, 1.0f);
// if throttle is zero, update base x,y,z values
if (throttle_pct == 0.0f)
{
compass_base.x = compass_base.x * 0.99f + (float)MainV2.comPort.MAV.cs.mx * 0.01f;
compass_base.y = compass_base.y * 0.99f + (float)MainV2.comPort.MAV.cs.my * 0.01f;
compass_base.z = compass_base.z * 0.99f + (float)MainV2.comPort.MAV.cs.mz * 0.01f;
// causing printing to happen as soon as throttle is lifted
print_counter = 49;
}
else
{
// calculate diff from compass base and scale with throttle
motor_impact.x = MainV2.comPort.MAV.cs.mx - compass_base.x;
motor_impact.y = MainV2.comPort.MAV.cs.my - compass_base.y;
motor_impact.z = MainV2.comPort.MAV.cs.mz - compass_base.z;
// throttle based compensation
if (comp_type == (byte)comptype.Throttle)
{
// scale by throttle
motor_impact_scaled = motor_impact / throttle_pct;
// adjust the motor compensation to negate the impact
motor_compensation = motor_compensation * 0.99f - motor_impact_scaled * 0.01f;
updated = true;
}
else
{
// current based compensation if more than 3amps being drawn
motor_impact_scaled = motor_impact / MainV2.comPort.MAV.cs.current;
// adjust the motor compensation to negate the impact if drawing over 3amps
if (MainV2.comPort.MAV.cs.current >= 3.0f)
{
motor_compensation = motor_compensation * 0.99f - motor_impact_scaled * 0.01f;
updated = true;
}
}
// record maximum throttle and current
throttle_pct_max = max(throttle_pct_max, throttle_pct);
current_amps_max = max(current_amps_max, MainV2.comPort.MAV.cs.current);
// display output at 1hz if throttle is above zero
print_counter++;
if (print_counter >= 50)
{
print_counter = 0;
var line = String.Format("thr:%d cur:%4.2f mot x:%4.1f y:%4.1f z:%4.1f comp x:%4.2f y:%4.2f z:%4.2f\n", (int)MainV2.comPort.MAV.cs.ch3percent, (float)MainV2.comPort.MAV.cs.current, (float)motor_impact.x, (float)motor_impact.y, (float)motor_impact.z, (float)motor_compensation.x, (float)motor_compensation.y, (float)motor_compensation.z);
Console.Write(line);
prd.UpdateProgressAndStatus(-1, line);
}
}
}
MainV2.comPort.doARM(false);
// set and save motor compensation
if (updated)
{
MainV2.comPort.setParam("COMPASS_MOTCT ", comp_type);
MainV2.comPort.setParam("COMPASS_MOT_X", (float)motor_compensation.x);
MainV2.comPort.setParam("COMPASS_MOT_Y", (float)motor_compensation.y);
MainV2.comPort.setParam("COMPASS_MOT_Z", (float)motor_compensation.z);
// calculate and display interference compensation at full throttle as % of total mag field
if (comp_type == (byte)comptype.Throttle)
{
// interference is impact@fullthrottle / mag field * 100
interference_pct = (float)motor_compensation.length() / (float)COMPASS_MAGFIELD_EXPECTED * 100.0f;
}
else
{
// interference is impact/amp * (max current seen / max throttle seen) / mag field * 100
interference_pct = (float)motor_compensation.length() * (current_amps_max / throttle_pct_max) / (float)COMPASS_MAGFIELD_EXPECTED * 100.0f;
}
string line = String.Format("\nInterference at full throttle is {0}% of mag field\n\n", (int)interference_pct);
Console.Write(line);
prd.UpdateProgressAndStatus(100, line);
}
else
{
prd.UpdateProgressAndStatus(100, "Failed");
}
MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.ALL, 2);
}
public ProgressReporterDialogue()
{
doWorkArgs = new ProgressWorkerEventArgs();
this.AutoScaleMode = System.Windows.Forms.AutoScaleMode.None;
}
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);
}
}
static void DoUpdateWorker_DoWork(object sender, ProgressWorkerEventArgs e, object passdata = null)
{
// TODO: Is this the right place?
#region Fetch Parameter Meta Data
var progressReporterDialogue = ((ProgressReporterDialogue)sender);
progressReporterDialogue.UpdateProgressAndStatus(-1, "Getting Updated Parameters");
try
{
ParameterMetaDataParser.GetParameterInformation();
}
catch (Exception ex) { log.Error(ex.ToString()); CustomMessageBox.Show("Error getting Parameter Information"); }
#endregion Fetch Parameter Meta Data
progressReporterDialogue.UpdateProgressAndStatus(-1, "Getting Base URL");
// check for updates
// if (Debugger.IsAttached)
{
// log.Info("Skipping update test as it appears we are debugging");
}
// else
{
updateCheckMain(progressReporterDialogue);
}
}
void getWPs(object sender, ProgressWorkerEventArgs e, object passdata = null)
{
List<Locationwp> cmds = new List<Locationwp>();
try
{
MAVLinkInterface port = comPort;
if (!port.BaseStream.IsOpen)
{
throw new Exception("Please Connect First!");
}
comPort.giveComport = true;
param = port.MAV.param;
//log.Info("Getting WP #");
((ProgressReporterDialogue)sender).UpdateProgressAndStatus(0, "Getting WP count");
int cmdcount = port.getWPCount();
for (ushort a = 0; a < cmdcount; a++)
{
if (((ProgressReporterDialogue)sender).doWorkArgs.CancelRequested)
{
((ProgressReporterDialogue)sender).doWorkArgs.CancelAcknowledged = true;
throw new Exception("Cancel Requested");
}
//log.Info("Getting WP" + a);
((ProgressReporterDialogue)sender).UpdateProgressAndStatus(a * 100 / cmdcount, "下载航点 " + a);
cmds.Add(port.getWP(a));
}
port.setWPACK();
((ProgressReporterDialogue)sender).UpdateProgressAndStatus(100, "完成");
//log.Info("Done");
}
catch { throw; }
WPtoScreen(cmds);
}
void FrmProgressReporterDoWorkNOParams(object sender, ProgressWorkerEventArgs e, object passdata = null)
{
OpenBg(sender, false, e);
}
private void OpenBg(object PRsender, bool getparams, ProgressWorkerEventArgs progressWorkerEventArgs)
{
frmProgressReporter.UpdateProgressAndStatus(-1, "Mavlink Connecting...");
giveComport = true;
// allow settings to settle - previous dtr
System.Threading.Thread.Sleep(500);
// reset
MAV.sysid = 0;
MAV.compid = 0;
MAV.param = new Hashtable();
MAV.packets.Initialize();
MAV.VersionString = "";
MAV.SoftwareVersions = "";
MAV.SerialString = "";
bool hbseen = false;
try
{
BaseStream.ReadBufferSize = 16 * 1024;
lock (objlock) // so we dont have random traffic
{
log.Info("Open port with " + BaseStream.PortName + " " + BaseStream.BaudRate);
BaseStream.Open();
BaseStream.DiscardInBuffer();
// other boards seem to have issues if there is no delay? posible bootloader timeout issue
Thread.Sleep(1000);
}
byte[] buffer = new byte[0];
byte[] buffer1 = new byte[0];
DateTime start = DateTime.Now;
DateTime deadline = start.AddSeconds(CONNECT_TIMEOUT_SECONDS);
var countDown = new System.Timers.Timer { Interval = 1000, AutoReset = false };
countDown.Elapsed += (sender, e) =>
{
int secondsRemaining = (deadline - e.SignalTime).Seconds;
//if (Progress != null)
// Progress(-1, string.Format("Trying to connect.\nTimeout in {0}", secondsRemaining));
frmProgressReporter.UpdateProgressAndStatus(-1, string.Format("Trying to connect.\nTimeout in {0}", secondsRemaining));
if (secondsRemaining > 0) countDown.Start();
};
countDown.Start();
int count = 0;
while (true)
{
if (progressWorkerEventArgs.CancelRequested)
{
progressWorkerEventArgs.CancelAcknowledged = true;
countDown.Stop();
if (BaseStream.IsOpen)
BaseStream.Close();
giveComport = false;
return;
}
// incase we are in setup mode
//BaseStream.WriteLine("planner\rgcs\r");
log.Info(DateTime.Now.Millisecond + " Start connect loop ");
if (DateTime.Now > deadline)
{
//if (Progress != null)
// Progress(-1, "No Heatbeat Packets");
countDown.Stop();
this.Close();
if (hbseen)
{
progressWorkerEventArgs.ErrorMessage = "Only 1 Heatbeat Received";
throw new Exception("Only 1 Mavlink Heartbeat Packets was read from this port - Verify your hardware is setup correctly\nMission Planner waits for 2 valid heartbeat packets before connecting");
}
else
{
progressWorkerEventArgs.ErrorMessage = "No Heatbeat Packets Received";
throw new Exception(@"Can not establish a connection\n
Please check the following
1. You have firmware loaded
2. You have the correct serial port selected
3. PX4 - You have the microsd card installed
4. Try a diffrent usb port\n\n"+"No Mavlink Heartbeat Packets where read from this port - Verify Baud Rate and setup\nMission Planner waits for 2 valid heartbeat packets before connecting");
}
}
System.Threading.Thread.Sleep(1);
// incase we are in setup mode
//BaseStream.WriteLine("planner\rgcs\r");
// can see 2 heartbeat packets at any time, and will connect - was one after the other
if (buffer.Length == 0)
buffer = getHeartBeat();
// incase we are in setup mode
//BaseStream.WriteLine("planner\rgcs\r");
System.Threading.Thread.Sleep(1);
if (buffer1.Length == 0)
buffer1 = getHeartBeat();
if (buffer.Length > 0 || buffer1.Length > 0)
hbseen = true;
count++;
if (buffer.Length > 5 && buffer1.Length > 5 && buffer[3] == buffer1[3] && buffer[4] == buffer1[4])
{
mavlink_heartbeat_t hb = buffer.ByteArrayToStructure<mavlink_heartbeat_t>(6);
if (hb.type != (byte)MAVLink.MAV_TYPE.GCS)
{
mavlinkversion = hb.mavlink_version;
MAV.aptype = (MAV_TYPE)hb.type;
MAV.apname = (MAV_AUTOPILOT)hb.autopilot;
setAPType();
MAV.sysid = buffer[3];
MAV.compid = buffer[4];
MAV.recvpacketcount = buffer[2];
log.InfoFormat("ID sys {0} comp {1} ver{2}", MAV.sysid, MAV.compid, mavlinkversion);
break;
}
}
}
countDown.Stop();
frmProgressReporter.UpdateProgressAndStatus(0, "Getting Params.. (sysid " + MAV.sysid + " compid " + MAV.compid + ") ");
if (getparams)
{
getParamListBG();
}
if (frmProgressReporter.doWorkArgs.CancelAcknowledged == true)
{
giveComport = false;
if (BaseStream.IsOpen)
BaseStream.Close();
return;
}
}
catch (Exception e)
{
try
{
BaseStream.Close();
}
catch { }
giveComport = false;
if (string.IsNullOrEmpty(progressWorkerEventArgs.ErrorMessage))
progressWorkerEventArgs.ErrorMessage = "Connect Failed";
log.Error(e);
throw;
}
//frmProgressReporter.Close();
giveComport = false;
frmProgressReporter.UpdateProgressAndStatus(100, "Done.");
log.Info("Done open " + MAV.sysid + " " + MAV.compid);
packetslost = 0;
synclost = 0;
}
void FrmProgressReporterGetParams(object sender, ProgressWorkerEventArgs e, object passdata = null)
{
Hashtable old = new Hashtable(MAV.param);
getParamListBG();
if (frmProgressReporter.doWorkArgs.CancelRequested)
{
MAV.param = old;
}
}
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);
}
