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);
newparamlist.TotalReported = param_total;
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)
{
var offset = Marshal.OffsetOf(typeof(mavlink_param_value_t), "param_value");
newparamlist[paramID] = new MAVLinkParam(paramID, BitConverter.GetBytes(Marshal.ReadInt32(par, offset.ToInt32())), MAV_PARAM_TYPE.REAL32, (MAV_PARAM_TYPE)par.param_type);
}
else
{
var offset = Marshal.OffsetOf(typeof(mavlink_param_value_t), "param_value");
newparamlist[paramID] = new MAVLinkParam(paramID, BitConverter.GetBytes(Marshal.ReadInt32(par, offset.ToInt32())),
(MAV_PARAM_TYPE)par.param_type, (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.TotalReported = param_total;
MAV.param.AddRange(newparamlist);
return MAV.param;
}
