void frmProgressReporter_DoWork(object sender, ArdupilotMega.Controls.ProgressWorkerEventArgs e)
{
string dest = Port;
if (ArdupilotMega.MainV2.config["UDP_port"] != null)
{
dest = ArdupilotMega.MainV2.config["UDP_port"].ToString();
}
ArdupilotMega.Common.InputBox("Listern Port", "Enter Local port (ensure remote end is already sending)", ref dest);
Port = dest;
ArdupilotMega.MainV2.config["UDP_port"] = Port;
client = new UdpClient(int.Parse(Port));
while (true)
{
((ProgressReporterDialogue)sender).UpdateProgressAndStatus(-1, "Waiting for UDP");
System.Threading.Thread.Sleep(500);
if (((ProgressReporterDialogue)sender).doWorkArgs.CancelRequested)
{
((ProgressReporterDialogue)sender).doWorkArgs.CancelAcknowledged = true;
try
{
client.Close();
}
catch { }
return;
}
if (BytesToRead > 0)
{
break;
}
}
if (BytesToRead == 0)
{
return;
}
try
{
client.Receive(ref RemoteIpEndPoint);
log.InfoFormat("NetSerial connecting to {0} : {1}", RemoteIpEndPoint.Address, RemoteIpEndPoint.Port);
client.Connect(RemoteIpEndPoint);
}
catch (Exception ex)
{
if (client != null && client.Client.Connected)
{
client.Close();
}
log.Info(ex.ToString());
System.Windows.Forms.CustomMessageBox.Show("Please check your Firewall settings\nPlease try running this command\n1. Run the following command in an elevated command prompt to disable Windows Firewall temporarily:\n \nNetsh advfirewall set allprofiles state off\n \nNote: This is just for test; please turn it back on with the command 'Netsh advfirewall set allprofiles state on'.\n");
throw new Exception("The socket/serialproxy is closed " + e);
}
}
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);
}
void FrmProgressReporterGetParams(object sender, ProgressWorkerEventArgs e, object passdata = null)
{
Hashtable old = new Hashtable(param);
getParamListBG();
if (frmProgressReporter.doWorkArgs.CancelRequested)
{
param = old;
}
}
private void OpenBg(bool getparams, ProgressWorkerEventArgs progressWorkerEventArgs)
{
frmProgressReporter.UpdateProgressAndStatus(-1, "Menghubungkan dengan satelit...");
giveComport = true;
// allow settings to settle - previous dtr
System.Threading.Thread.Sleep(500);
// reset
sysid = 0;
compid = 0;
param = new Hashtable();
packets.Initialize();
bool hbseen = false;
try
{
BaseStream.ReadBufferSize = 4 * 1024;
lock (objlock) // so we dont have random traffic
{
log.Info("Open port with " + BaseStream.PortName + " " + BaseStream.BaudRate);
BaseStream.Open();
BaseStream.DiscardInBuffer();
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();
CustomMessageBox.Show(@"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");
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\nAPM Planner waits for 2 valid heartbeat packets before connecting");
}
else
{
progressWorkerEventArgs.ErrorMessage = "No Heatbeat Packets Received";
throw new Exception("No Mavlink Heartbeat Packets where read from this port - Verify Baud Rate and setup\nAPM 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);
mavlinkversion = hb.mavlink_version;
aptype = (MAV_TYPE)hb.type;
apname = (MAV_AUTOPILOT)hb.autopilot;
setAPType();
sysid = buffer[3];
compid = buffer[4];
recvpacketcount = buffer[2];
log.InfoFormat("ID sys {0} comp {1} ver{2}", sysid, compid, mavlinkversion);
break;
}
}
countDown.Stop();
frmProgressReporter.UpdateProgressAndStatus(0, "Getting Params.. (sysid " + sysid + " compid " + 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";
throw e;
}
//frmProgressReporter.Close();
giveComport = false;
frmProgressReporter.UpdateProgressAndStatus(100, "Done.");
log.Info("Done open " + sysid + " " + compid);
packetslost = 0;
synclost = 0;
}
public ProgressReporterDialogue()
{
InitializeComponent();
doWorkArgs = new ProgressWorkerEventArgs();
this.btnClose.Visible = false;
}
void FrmProgressReporterDoWorkNOParams(object sender, ProgressWorkerEventArgs e, object passdata = null)
{
OpenBg(false, e);
}
void FrmProgressReporterDoWorkAndParams(object sender, ProgressWorkerEventArgs e)
{
OpenBg(true, e);
}
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);
}
}
private void OpenBg(bool getparams, ProgressWorkerEventArgs progressWorkerEventArgs)
{
frmProgressReporter.UpdateProgressAndStatus(-1, "Mavlink Connecting...");
// reset
sysid = 0;
compid = 0;
param = new Hashtable();
try
{
MainV2.giveComport = true;
BaseStream.ReadBufferSize = 4 * 1024;
lock (objlock) // so we dont have random traffic
{
log.Info("Open port with " + BaseStream.PortName + " " + BaseStream.BaudRate);
BaseStream.Open();
BaseStream.DiscardInBuffer();
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();
MainV2.giveComport = false;
return;
}
// incase we are in setup mode
BaseStream.WriteLine("planner\rgcs\r");
log.Info(DateTime.Now.Millisecond + " Start connect loop ");
if (lastbad[0] == '!' && lastbad[1] == 'G' || lastbad[0] == 'G' && lastbad[1] == '!') // waiting for gps lock
{
//if (Progress != null)
// Progress(-1, "Waiting for GPS detection..");
frmProgressReporter.UpdateProgressAndStatus(-1, "Waiting for GPS detection..");
deadline = deadline.AddSeconds(5); // each round is 1.1 seconds
}
if (DateTime.Now > deadline)
{
//if (Progress != null)
// Progress(-1, "No Heatbeat Packets");
this.Close();
progressWorkerEventArgs.ErrorMessage = "No Heatbeat Packets Received";
throw new Exception("No Mavlink Heartbeat Packets where read from this port - Verify Baud Rate and setup\nIt might also be waiting for GPS Lock\nAPM 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();
try
{
log.Debug("MAv Data: len " + buffer.Length + " btr " + BaseStream.BytesToRead);
}
catch { }
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);
mavlinkversion = hb.mavlink_version;
aptype = hb.type;
sysid = buffer[3];
compid = buffer[4];
recvpacketcount = buffer[2];
log.InfoFormat("ID sys {0} comp {1} ver{2}", sysid, compid, mavlinkversion);
break;
}
}
countDown.Stop();
// if (Progress != null)
// Progress(-1, "Getting Params.. (sysid " + sysid + " compid " + compid + ") ");
frmProgressReporter.UpdateProgressAndStatus(0, "Getting Params.. (sysid " + sysid + " compid " + compid + ") ");
if (getparams)
getParamListBG();
if (frmProgressReporter.doWorkArgs.CancelAcknowledged == true)
{
MainV2.giveComport = false;
if (BaseStream.IsOpen)
BaseStream.Close();
return;
}
}
catch (Exception e)
{
try
{
BaseStream.Close();
}
catch { }
MainV2.giveComport = false;
// if (Progress != null)
// Progress(-1, "Connect Failed\n" + e.Message);
if (string.IsNullOrEmpty(progressWorkerEventArgs.ErrorMessage))
progressWorkerEventArgs.ErrorMessage = "Connect Failed";
throw e;
}
//frmProgressReporter.Close();
MainV2.giveComport = false;
frmProgressReporter.UpdateProgressAndStatus(100, "Done.");
log.Info("Done open " + sysid + " " + compid);
packetslost = 0;
synclost = 0;
}
public ProgressReporterDialogue()
{
InitializeComponent();
doWorkArgs = new ProgressWorkerEventArgs();
this.btnClose.Visible = false;
}
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.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.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[MAVLink.MAVLINK_MSG_ID_RAW_IMU];
int rcseen = MainV2.comPort.MAV.packetseencount[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);
}
