/// <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 by either index or name
/// </summary>
/// <param name="index"></param>
/// <param name="name"></param>
/// <returns></returns>
internal float GetParam(string name = "", short index = -1)
{
if (name == "" && index == -1)
return 0;
log.Info("GetParam name: '" + name + "' or index: " + index);
giveComport = true;
MAVLinkMessage buffer;
mavlink_param_request_read_t req = new mavlink_param_request_read_t();
req.target_system = MAV.sysid;
req.target_component = MAV.compid;
req.param_index = index;
req.param_id = new byte[] {0x0};
if (index == -1)
{
req.param_id = System.Text.ASCIIEncoding.ASCII.GetBytes(name);
}
Array.Resize(ref req.param_id, 16);
generatePacket((byte) MAVLINK_MSG_ID.PARAM_REQUEST_READ, req);
DateTime start = DateTime.Now;
int retrys = 3;
while (true)
{
if (!(start.AddMilliseconds(700) > DateTime.Now))
{
if (retrys > 0)
{
log.Info("GetParam Retry " + retrys);
generatePacket((byte) MAVLINK_MSG_ID.PARAM_REQUEST_READ, req);
start = DateTime.Now;
retrys--;
continue;
}
giveComport = false;
throw new Exception("Timeout on read - GetParam");
}
buffer = readPacket();
if (buffer.Length > 5)
{
if (buffer.msgid == (byte)MAVLINK_MSG_ID.PARAM_VALUE && buffer.sysid == req.target_system && buffer.compid == req.target_component)
{
giveComport = false;
mavlink_param_value_t par = buffer.ToStructure<mavlink_param_value_t>();
string st = System.Text.ASCIIEncoding.ASCII.GetString(par.param_id);
int pos = st.IndexOf('\0');
if (pos != -1)
{
st = st.Substring(0, pos);
}
// not the correct id
if (!(par.param_index == index || st == name))
{
log.ErrorFormat("Wrong Answer {0} - {1} - {2} --- '{3}' vs '{4}'", par.param_index,
ASCIIEncoding.ASCII.GetString(par.param_id), par.param_value,
ASCIIEncoding.ASCII.GetString(req.param_id).TrimEnd(), st);
continue;
}
// update table
if (MAV.apname == MAV_AUTOPILOT.ARDUPILOTMEGA)
{
MAV.param[st] = new MAVLinkParam(st, par.param_value, MAV_PARAM_TYPE.REAL32, (MAV_PARAM_TYPE) par.param_type);
}
else
{
MAV.param[st] = new MAVLinkParam(st, par.param_value, (MAV_PARAM_TYPE) par.param_type);
}
MAV.param_types[st] = (MAV_PARAM_TYPE) par.param_type;
log.Info(DateTime.Now.Millisecond + " got param " + (par.param_index) + " of " +
(par.param_count) + " name: " + st);
return par.param_value;
}
}
}
}
/// <summary>
/// Get param by either index or name
/// </summary>
/// <param name="index"></param>
/// <param name="name"></param>
/// <returns></returns>
internal float GetParam(string name = "", short index = -1)
{
if (name == "" && index == -1)
return 0;
log.Info("GetParam name: " + name + " index: " + index);
giveComport = true;
byte[] buffer;
mavlink_param_request_read_t req = new mavlink_param_request_read_t();
req.target_system = MAV.sysid;
req.target_component = MAV.compid;
req.param_index = index;
if (index == -1)
{
req.param_id = System.Text.ASCIIEncoding.ASCII.GetBytes(name);
Array.Resize(ref req.param_id, 16);
}
generatePacket((byte)MAVLINK_MSG_ID.PARAM_REQUEST_READ, req);
DateTime start = DateTime.Now;
int retrys = 3;
while (true)
{
if (!(start.AddMilliseconds(700) > DateTime.Now))
{
if (retrys > 0)
{
log.Info("GetParam Retry " + retrys);
generatePacket((byte)MAVLINK_MSG_ID.PARAM_REQUEST_READ, req);
start = DateTime.Now;
retrys--;
continue;
}
giveComport = false;
throw new Exception("Timeout on read - GetParam");
}
buffer = readPacket();
if (buffer.Length > 5)
{
if (buffer[5] == (byte)MAVLINK_MSG_ID.PARAM_VALUE)
{
giveComport = false;
mavlink_param_value_t par = buffer.ByteArrayToStructure<mavlink_param_value_t>(6);
// not the correct id
if (!(par.param_index == index || ASCIIEncoding.ASCII.GetString(par.param_id) == ASCIIEncoding.ASCII.GetString(req.param_id)))
{
Console.WriteLine("Wrong Answer {0} - {1} - {2}", par.param_index, ASCIIEncoding.ASCII.GetString(par.param_id), par.param_value);
continue;
}
string st = System.Text.ASCIIEncoding.ASCII.GetString(par.param_id);
int pos = st.IndexOf('\0');
if (pos != -1)
{
st = st.Substring(0, pos);
}
// update table
MAV.param[st] = par.param_value;
param_types[st] = (MAV_PARAM_TYPE)par.param_type;
log.Info(DateTime.Now.Millisecond + " got param " + (par.param_index) + " of " + (par.param_count) + " name: " + st);
return par.param_value;
}
}
}
}
