/// <summary>
/// Get param list from apm
/// </summary>
/// <returns></returns>
private Hashtable getParamListBG()
{
giveComport = true;
List<int> indexsreceived = new List<int>();
// create new list so if canceled we use the old list
MAVLinkParamList newparamlist = new MAVLinkParamList();
int param_total = 1;
mavlink_param_request_list_t req = new mavlink_param_request_list_t();
req.target_system = MAV.sysid;
req.target_component = MAV.compid;
generatePacket((byte) MAVLINK_MSG_ID.PARAM_REQUEST_LIST, req);
DateTime start = DateTime.Now;
DateTime restart = DateTime.Now;
DateTime lastmessage = DateTime.MinValue;
//hires.Stopwatch stopwatch = new hires.Stopwatch();
int packets = 0;
bool onebyone = false;
DateTime lastonebyone = DateTime.MinValue;
do
{
if (frmProgressReporter.doWorkArgs.CancelRequested)
{
frmProgressReporter.doWorkArgs.CancelAcknowledged = true;
giveComport = false;
frmProgressReporter.doWorkArgs.ErrorMessage = "User Canceled";
return MAV.param;
}
// 4 seconds between valid packets
if (!(start.AddMilliseconds(4000) > DateTime.Now) && !logreadmode)
{
onebyone = true;
if (lastonebyone.AddMilliseconds(600) < DateTime.Now)
{
log.Info("Get param 1 by 1 - got " + indexsreceived.Count + " of " + param_total);
int queued = 0;
// try getting individual params
for (short i = 0; i <= (param_total - 1); i++)
{
if (!indexsreceived.Contains(i))
{
if (frmProgressReporter.doWorkArgs.CancelRequested)
{
frmProgressReporter.doWorkArgs.CancelAcknowledged = true;
giveComport = false;
frmProgressReporter.doWorkArgs.ErrorMessage = "User Canceled";
return MAV.param;
}
// prevent dropping out of this get params loop
try
{
queued++;
mavlink_param_request_read_t req2 = new mavlink_param_request_read_t();
req2.target_system = MAV.sysid;
req2.target_component = MAV.compid;
req2.param_index = i;
req2.param_id = new byte[] {0x0};
Array.Resize(ref req2.param_id, 16);
generatePacket((byte) MAVLINK_MSG_ID.PARAM_REQUEST_READ, req2);
if (queued >= 10)
{
lastonebyone = DateTime.Now;
break;
}
}
catch (Exception excp)
{
log.Info("GetParam Failed index: " + i + " " + excp);
throw excp;
}
}
}
}
}
//Console.WriteLine(DateTime.Now.Millisecond + " gp0 ");
MAVLinkMessage buffer = readPacket();
//Console.WriteLine(DateTime.Now.Millisecond + " gp1 ");
if (buffer.Length > 5)
{
packets++;
// stopwatch.Start();
if (buffer.msgid == (byte)MAVLINK_MSG_ID.PARAM_VALUE && buffer.sysid == req.target_system && buffer.compid == req.target_component)
{
restart = DateTime.Now;
// if we are doing one by one dont update start time
if (!onebyone)
start = DateTime.Now;
mavlink_param_value_t par = buffer.ToStructure<mavlink_param_value_t>();
// set new target
param_total = (par.param_count);
string paramID = System.Text.ASCIIEncoding.ASCII.GetString(par.param_id);
int pos = paramID.IndexOf('\0');
if (pos != -1)
{
paramID = paramID.Substring(0, pos);
}
// check if we already have it
if (indexsreceived.Contains(par.param_index))
{
log.Info("Already got " + (par.param_index) + " '" + paramID + "'");
this.frmProgressReporter.UpdateProgressAndStatus((indexsreceived.Count*100)/param_total,
"Already Got param " + paramID);
continue;
}
//Console.WriteLine(DateTime.Now.Millisecond + " gp2 ");
if (!MainV2.MONO)
log.Info(DateTime.Now.Millisecond + " got param " + (par.param_index) + " of " +
(par.param_count) + " name: " + paramID);
//Console.WriteLine(DateTime.Now.Millisecond + " gp2a ");
if (MAV.apname == MAV_AUTOPILOT.ARDUPILOTMEGA)
{
newparamlist[paramID] = new MAVLinkParam(paramID, par.param_value, MAV_PARAM_TYPE.REAL32, (MAV_PARAM_TYPE)par.param_type);
}
else
{
newparamlist[paramID] = new MAVLinkParam(paramID, par.param_value,
(MAV_PARAM_TYPE) par.param_type);
}
//Console.WriteLine(DateTime.Now.Millisecond + " gp2b ");
// exclude index of 65535
if (par.param_index != 65535)
indexsreceived.Add(par.param_index);
MAV.param_types[paramID] = (MAV_PARAM_TYPE) par.param_type;
//Console.WriteLine(DateTime.Now.Millisecond + " gp3 ");
this.frmProgressReporter.UpdateProgressAndStatus((indexsreceived.Count*100)/param_total,
Strings.Gotparam + paramID);
// we hit the last param - lets escape eq total = 176 index = 0-175
if (par.param_index == (param_total - 1))
start = DateTime.MinValue;
}
if (buffer.msgid == (byte) MAVLINK_MSG_ID.STATUSTEXT)
{
var msg = buffer.ToStructure<MAVLink.mavlink_statustext_t>();
string logdata = Encoding.ASCII.GetString(msg.text);
int ind = logdata.IndexOf('\0');
if (ind != -1)
logdata = logdata.Substring(0, ind);
if (logdata.ToLower().Contains("copter") || logdata.ToLower().Contains("rover") ||
logdata.ToLower().Contains("plane"))
{
MAV.VersionString = logdata;
}
else if (logdata.ToLower().Contains("nuttx"))
{
MAV.SoftwareVersions = logdata;
}
else if (logdata.ToLower().Contains("px4v2"))
{
MAV.SerialString = logdata;
}
else if (logdata.ToLower().Contains("frame"))
{
MAV.FrameString = logdata;
}
}
//stopwatch.Stop();
// Console.WriteLine("Time elapsed: {0}", stopwatch.Elapsed);
// Console.WriteLine(DateTime.Now.Millisecond + " gp4 " + BaseStream.BytesToRead);
}
if (logreadmode && logplaybackfile.BaseStream.Position >= logplaybackfile.BaseStream.Length)
{
break;
}
if (!logreadmode && !BaseStream.IsOpen)
{
var exp = new Exception("Not Connected");
frmProgressReporter.doWorkArgs.ErrorMessage = exp.Message;
throw exp;
}
} while (indexsreceived.Count < param_total);
if (indexsreceived.Count != param_total)
{
var exp = new Exception("Missing Params " + indexsreceived.Count + " vs " + param_total);
frmProgressReporter.doWorkArgs.ErrorMessage = exp.Message;
throw exp;
}
giveComport = false;
MAV.param.Clear();
MAV.param.AddRange(newparamlist);
return MAV.param;
}
/// <summary>
/// Get param list from apm
/// </summary>
/// <returns></returns>
private Hashtable getParamListBG()
{
giveComport = true;
List<int> indexsreceived = new List<int>();
// clear old
MAV.param = new Hashtable();
int retrys = 6;
int param_count = 0;
int param_total = 1;
mavlink_param_request_list_t req = new mavlink_param_request_list_t();
req.target_system = MAV.sysid;
req.target_component = MAV.compid;
generatePacket((byte)MAVLINK_MSG_ID.PARAM_REQUEST_LIST, req);
DateTime start = DateTime.Now;
DateTime restart = DateTime.Now;
DateTime lastmessage = DateTime.MinValue;
//hires.Stopwatch stopwatch = new hires.Stopwatch();
int packets = 0;
do
{
if (frmProgressReporter.doWorkArgs.CancelRequested)
{
frmProgressReporter.doWorkArgs.CancelAcknowledged = true;
giveComport = false;
frmProgressReporter.doWorkArgs.ErrorMessage = "User Canceled";
return MAV.param;
}
// 4 seconds between valid packets
if (!(start.AddMilliseconds(4000) > DateTime.Now) && !logreadmode)
{
log.Info("Get param 1 by 1 - got " + indexsreceived.Count + " of " + param_total);
// try getting individual params
for (short i = 0; i <= (param_total - 1); i++)
{
if (!indexsreceived.Contains(i))
{
if (frmProgressReporter.doWorkArgs.CancelRequested)
{
frmProgressReporter.doWorkArgs.CancelAcknowledged = true;
giveComport = false;
frmProgressReporter.doWorkArgs.ErrorMessage = "User Canceled";
return MAV.param;
}
// prevent dropping out of this get params loop
try
{
GetParam(i);
param_count++;
indexsreceived.Add(i);
this.frmProgressReporter.UpdateProgressAndStatus((indexsreceived.Count * 100) / param_total, "Got param index " + i);
}
catch (Exception excp)
{
log.Info("GetParam Failed index: " + i + " " + excp);
try
{
// GetParam(i);
// param_count++;
// indexsreceived.Add(i);
}
catch { }
// fail over to full list
//break;
}
}
}
if (retrys == 4)
{
requestDatastream(MAVLink.MAV_DATA_STREAM.ALL, 1);
}
if (retrys > 0)
{
log.InfoFormat("getParamList Retry {0} sys {1} comp {2}", retrys, MAV.sysid, MAV.compid);
generatePacket((byte)MAVLINK_MSG_ID.PARAM_REQUEST_LIST, req);
start = DateTime.Now;
retrys--;
continue;
}
giveComport = false;
if (packets > 0 && param_total == 1)
{
throw new Exception("Timeout on read - getParamList\n" + packets + " Packets where received, but no paramater packets where received\n");
}
if (packets == 0)
{
throw new Exception("Timeout on read - getParamList\nNo Packets where received\n");
}
throw new Exception("Timeout on read - getParamList\nReceived: " + indexsreceived.Count + " of " + param_total + " after 6 retrys\n\nPlease Check\n1. Link Speed\n2. Link Quality\n3. Hardware hasn't hung");
}
//Console.WriteLine(DateTime.Now.Millisecond + " gp0 ");
byte[] buffer = readPacket();
//Console.WriteLine(DateTime.Now.Millisecond + " gp1 ");
if (buffer.Length > 5)
{
packets++;
// stopwatch.Start();
if (buffer[5] == (byte)MAVLINK_MSG_ID.PARAM_VALUE)
{
restart = DateTime.Now;
start = DateTime.Now;
mavlink_param_value_t par = buffer.ByteArrayToStructure<mavlink_param_value_t>(6);
// set new target
param_total = (par.param_count);
string paramID = System.Text.ASCIIEncoding.ASCII.GetString(par.param_id);
int pos = paramID.IndexOf('\0');
if (pos != -1)
{
paramID = paramID.Substring(0, pos);
}
// check if we already have it
if (indexsreceived.Contains(par.param_index))
{
log.Info("Already got " + (par.param_index) + " '" + paramID + "'");
this.frmProgressReporter.UpdateProgressAndStatus((indexsreceived.Count * 100) / param_total, "Already Got param " + paramID);
continue;
}
//Console.WriteLine(DateTime.Now.Millisecond + " gp2 ");
if (!MainV2.MONO)
log.Info(DateTime.Now.Millisecond + " got param " + (par.param_index) + " of " + (par.param_count) + " name: " + paramID);
//Console.WriteLine(DateTime.Now.Millisecond + " gp2a ");
MAV.param[paramID] = (par.param_value);
//Console.WriteLine(DateTime.Now.Millisecond + " gp2b ");
param_count++;
indexsreceived.Add(par.param_index);
param_types[paramID] = (MAV_PARAM_TYPE)par.param_type;
//Console.WriteLine(DateTime.Now.Millisecond + " gp3 ");
this.frmProgressReporter.UpdateProgressAndStatus((indexsreceived.Count * 100) / param_total, "Got param " + paramID);
// we hit the last param - lets escape eq total = 176 index = 0-175
if (par.param_index == (param_total - 1))
start = DateTime.MinValue;
}
if (buffer[5] == (byte)MAVLINK_MSG_ID.STATUSTEXT)
{
var msg = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.STATUSTEXT].ByteArrayToStructure<MAVLink.mavlink_statustext_t>(6);
string logdata = Encoding.ASCII.GetString(msg.text);
int ind = logdata.IndexOf('\0');
if (ind != -1)
logdata = logdata.Substring(0, ind);
if (logdata.ToLower().Contains("copter")||logdata.ToLower().Contains("rover")||logdata.ToLower().Contains("plane"))
{
MAV.VersionString = logdata;
}
else if (logdata.ToLower().Contains("nuttx"))
{
MAV.SoftwareVersions = logdata;
}
else if (logdata.ToLower().Contains("px4v2"))
{
MAV.SerialString = logdata;
}
}
//stopwatch.Stop();
// Console.WriteLine("Time elapsed: {0}", stopwatch.Elapsed);
// Console.WriteLine(DateTime.Now.Millisecond + " gp4 " + BaseStream.BytesToRead);
}
if (logreadmode && logplaybackfile.BaseStream.Position >= logplaybackfile.BaseStream.Length)
{
break;
}
} while (indexsreceived.Count < param_total);
if (indexsreceived.Count != param_total)
{
throw new Exception("Missing Params");
}
giveComport = false;
return MAV.param;
}
/// <summary>
/// Get param by either index or name
/// </summary>
/// <param name="index"></param>
/// <param name="name"></param>
/// <returns></returns>
internal float GetParam(string name = "", int index = -1)
{
if (name == "" && index == -1)
return 0;
MainV2.giveComport = true;
byte[] buffer;
mavlink_param_request_list_t req = new mavlink_param_request_list_t();
req.target_system = sysid;
req.target_component = compid;
generatePacket(MAVLINK_MSG_ID_PARAM_REQUEST_READ, req);
DateTime start = DateTime.Now;
int retrys = 3;
while (true)
{
if (!(start.AddMilliseconds(200) > DateTime.Now))
{
if (retrys > 0)
{
log.Info("GetParam Retry " + retrys);
generatePacket(MAVLINK_MSG_ID_PARAM_REQUEST_READ, req);
start = DateTime.Now;
retrys--;
continue;
}
MainV2.giveComport = false;
throw new Exception("Timeout on read - GetParam");
}
buffer = readPacket();
if (buffer.Length > 5)
{
if (buffer[5] == MAVLINK_MSG_ID_PARAM_VALUE)
{
MainV2.giveComport = false;
mavlink_param_value_t par = buffer.ByteArrayToStructure<mavlink_param_value_t>(6);
return par.param_value;
}
}
}
}
/// <summary>
/// Get param list from apm
/// </summary>
/// <returns></returns>
private Hashtable getParamListBG()
{
giveComport = true;
List<int> indexsreceived = new List<int>();
// clear old
param = new Hashtable();
int retrys = 6;
int param_count = 0;
int param_total = 1;
goagain:
mavlink_param_request_list_t req = new mavlink_param_request_list_t();
req.target_system = sysid;
req.target_component = compid;
generatePacket(MAVLINK_MSG_ID_PARAM_REQUEST_LIST, req);
DateTime start = DateTime.Now;
DateTime restart = DateTime.Now;
DateTime lastmessage = DateTime.MinValue;
//hires.Stopwatch stopwatch = new hires.Stopwatch();
int packets = 0;
do
{
if (frmProgressReporter.doWorkArgs.CancelRequested)
{
frmProgressReporter.doWorkArgs.CancelAcknowledged = true;
giveComport = false;
frmProgressReporter.doWorkArgs.ErrorMessage = "User Canceled";
return param;
}
// 4 seconds between valid packets
if (!(start.AddMilliseconds(4000) > DateTime.Now))
{
// try getting individual params
for (short i = 0; i <= (param_total - 1); i++)
{
if (!indexsreceived.Contains(i))
{
// prevent dropping out of this get params loop
try
{
GetParam(i);
param_count++;
indexsreceived.Add(i);
}
catch {
// fail over to full list
break;
}
}
}
if (retrys == 4)
{
requestDatastream(ArdupilotMega.MAVLink.MAV_DATA_STREAM.ALL, 1);
}
if (retrys > 0)
{
log.InfoFormat("getParamList Retry {0} sys {1} comp {2}", retrys, sysid, compid);
generatePacket(MAVLINK_MSG_ID_PARAM_REQUEST_LIST, req);
start = DateTime.Now;
retrys--;
continue;
}
giveComport = false;
if (packets > 0 && param_total == 1)
{
throw new Exception("Timeout on read - getParamList\n" + packets + " Packets where received, but no paramater packets where received\n");
}
if (packets == 0)
{
throw new Exception("Timeout on read - getParamList\nNo Packets where received\n");
}
throw new Exception("Timeout on read - getParamList\nReceived: " + indexsreceived.Count + " of " + param_total + " after 6 retrys\n\nPlease Check\n1. Link Speed\n2. Link Quality\n3. Hardware hasn't hung");
}
//Console.WriteLine(DateTime.Now.Millisecond + " gp0 ");
byte[] buffer = readPacket();
//Console.WriteLine(DateTime.Now.Millisecond + " gp1 ");
if (buffer.Length > 5)
{
packets++;
// stopwatch.Start();
if (buffer[5] == MAVLINK_MSG_ID_PARAM_VALUE)
{
restart = DateTime.Now;
start = DateTime.Now;
mavlink_param_value_t par = buffer.ByteArrayToStructure<mavlink_param_value_t>(6);
// set new target
param_total = (par.param_count);
string paramID = System.Text.ASCIIEncoding.ASCII.GetString(par.param_id);
int pos = paramID.IndexOf('\0');
if (pos != -1)
{
paramID = paramID.Substring(0, pos);
}
// check if we already have it
if (indexsreceived.Contains(par.param_index))
{
log.Info("Sudah terdapat " + (par.param_index) + " '" + paramID + "'");
this.frmProgressReporter.UpdateProgressAndStatus((indexsreceived.Count * 100) / param_total, "Sudah terdapat " + paramID);
continue;
}
//Console.WriteLine(DateTime.Now.Millisecond + " gp2 ");
if (!MainV2.MONO)
log.Info(DateTime.Now.Millisecond + " ambil parameter " + (par.param_index) + " pada " + (par.param_count) + " nama: " + paramID);
//Console.WriteLine(DateTime.Now.Millisecond + " gp2a ");
param[paramID] = (par.param_value);
//Console.WriteLine(DateTime.Now.Millisecond + " gp2b ");
param_count++;
indexsreceived.Add(par.param_index);
param_types[paramID] = (MAV_PARAM_TYPE)par.param_type;
//Console.WriteLine(DateTime.Now.Millisecond + " gp3 ");
this.frmProgressReporter.UpdateProgressAndStatus((indexsreceived.Count * 100) / param_total, "Ambil parameter " + paramID);
// we have them all - lets escape eq total = 176 index = 0-175
if (par.param_index == (param_total - 1))
break;
}
else
{
//Console.WriteLine(DateTime.Now + " PC paramlist " + buffer[5] + " want " + MAVLINK_MSG_ID_PARAM_VALUE + " btr " + BaseStream.BytesToRead);
}
//stopwatch.Stop();
// Console.WriteLine("Time elapsed: {0}", stopwatch.Elapsed);
// Console.WriteLine(DateTime.Now.Millisecond + " gp4 " + BaseStream.BytesToRead);
}
} while (indexsreceived.Count < param_total);
if (indexsreceived.Count != param_total)
{
if (retrys > 0)
{
this.frmProgressReporter.UpdateProgressAndStatus((indexsreceived.Count * 100) / param_total, "Getting missed params");
retrys--;
goto goagain;
}
throw new Exception("Missing Params");
}
giveComport = false;
return param;
}
/// <summary>
/// Get param list from apm
/// </summary>
/// <returns></returns>
private Hashtable getParamListBG()
{
MainV2.giveComport = true;
List<int> got = new List<int>();
// clear old
param = new Hashtable();
int retrys = 6;
int param_count = 0;
int param_total = 5;
goagain:
mavlink_param_request_list_t req = new mavlink_param_request_list_t();
req.target_system = sysid;
req.target_component = compid;
generatePacket(MAVLINK_MSG_ID_PARAM_REQUEST_LIST, req);
DateTime start = DateTime.Now;
DateTime restart = DateTime.Now;
do
{
if (frmProgressReporter.doWorkArgs.CancelRequested)
{
frmProgressReporter.doWorkArgs.CancelAcknowledged = true;
MainV2.giveComport = false;
frmProgressReporter.doWorkArgs.ErrorMessage = "User Canceled";
return param;
}
// 4 seconds between valid packets
if (!(start.AddMilliseconds(4000) > DateTime.Now))
{
if (retrys > 0)
{
log.InfoFormat("getParamList Retry {0} sys {1} comp {2}", retrys, sysid, compid);
generatePacket(MAVLINK_MSG_ID_PARAM_REQUEST_LIST, req);
start = DateTime.Now;
retrys--;
continue;
}
MainV2.giveComport = false;
throw new Exception("Timeout on read - getParamList " + got.Count + " " + param_total + "\n\nYour serial link isn't fast enough\n");
}
byte[] buffer = readPacket();
if (buffer.Length > 5)
{
//stopwatch.Reset();
//stopwatch.Start();
if (buffer[5] == MAVLINK_MSG_ID_PARAM_VALUE)
{
restart = DateTime.Now;
start = DateTime.Now;
mavlink_param_value_t par = buffer.ByteArrayToStructure<mavlink_param_value_t>(6);
// set new target
param_total = (par.param_count - 1);
string paramID = System.Text.ASCIIEncoding.ASCII.GetString(par.param_id);
int pos = paramID.IndexOf('\0');
if (pos != -1)
{
paramID = paramID.Substring(0, pos);
}
// check if we already have it
if (got.Contains(par.param_index))
{
log.Info("Already got "+(par.param_index) + " '" + paramID + "'");
this.frmProgressReporter.UpdateProgressAndStatus((got.Count * 100) / param_total, "Already Got param " + paramID);
continue;
}
log.Info(DateTime.Now.Millisecond + " got param " + (par.param_index) + " of " + (par.param_count - 2) + " name: " + paramID);
modifyParamForDisplay(true, paramID, ref par.param_value);
param[paramID] = (par.param_value);
param_count++;
got.Add(par.param_index);
// if (Progress != null)
// Progress((param.Count * 100) / param_total, "Got param " + paramID);
this.frmProgressReporter.UpdateProgressAndStatus((got.Count * 100) / param_total, "Got param " + paramID);
// we have them all - lets escape eq total = 176 index = 0-175
if (par.param_index == (param_total - 1))
break;
}
else
{
//Console.WriteLine(DateTime.Now + " PC paramlist " + buffer[5] + " want " + MAVLINK_MSG_ID_PARAM_VALUE + " btr " + BaseStream.BytesToRead);
}
//stopwatch.Stop();
//Console.WriteLine("Time elapsed: {0}", stopwatch.Elapsed);
}
} while (got.Count < param_total);
if (got.Count != param_total)
{
if (retrys > 0)
{
this.frmProgressReporter.UpdateProgressAndStatus((got.Count * 100) / param_total, "Getting missed params");
retrys--;
goto goagain;
}
throw new Exception("Missing Params");
}
MainV2.giveComport = false;
return param;
}
