/// <summary>
/// Get param list from apm
/// </summary>
/// <returns></returns>
public Hashtable getParamList()
{
MainV2.givecomport = true;
List<int> missed = new List<int>();
// ryan - re start
__mavlink_param_request_read_t rereq = new __mavlink_param_request_read_t();
rereq.target_system = sysid;
rereq.target_component = compid;
__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;
int retrys = 3;
int nextid = 0;
int a = 0;
int z = 5;
while (a < z)
{
if (!(start.AddMilliseconds(5000) > DateTime.Now))
{
if (retrys > 0)
{
Console.WriteLine("getParamList Retry " + retrys + " sys " + sysid + " comp " + compid);
generatePacket(MAVLINK_MSG_ID_PARAM_REQUEST_LIST, req);
start = DateTime.Now;
retrys--;
continue;
}
MainV2.givecomport = false;
throw new Exception("Timeout on read - getParamList");
}
if (!(restart.AddMilliseconds(1000) > DateTime.Now))
{
rereq.param_id = new byte[] {0x0,0x0};
rereq.param_index = (short)nextid;
sendPacket(rereq);
restart = DateTime.Now;
}
System.Windows.Forms.Application.DoEvents();
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 = new __mavlink_param_value_t();
object temp = par;
ByteArrayToStructure(buffer, ref temp, 6);
par = (__mavlink_param_value_t)temp;
z = (par.param_count);
if (nextid == (par.param_index))
{
nextid++;
}
else
{
if (retrys > 0)
{
generatePacket(MAVLINK_MSG_ID_PARAM_REQUEST_LIST, req);
a = 0;
nextid = 0;
retrys--;
continue;
}
missed.Add(nextid); // for later devel
MainV2.givecomport = false;
throw new Exception("Missed ID expecting " + nextid + " got " + (par.param_index) + "\nPlease try loading again");
}
string st = System.Text.ASCIIEncoding.ASCII.GetString(par.param_id);
int pos = st.IndexOf('\0');
if (pos != -1)
{
st = st.Substring(0, pos);
}
Console.WriteLine(DateTime.Now.Millisecond + " got param " + (par.param_index) + " of " + (z - 1) + " name: " + st);
modifyParamForDisplay(true, st, ref par.param_value);
param[st] = (par.param_value);
a++;
}
else
{
//Console.WriteLine(DateTime.Now + " PC paramlist " + buffer[5] + " " + this.BytesToRead);
}
//stopwatch.Stop();
//Console.WriteLine("Time elapsed: {0}", stopwatch.Elapsed);
}
}
MainV2.givecomport = false;
return param;
}
/// <summary>
/// Set parameter on apm
/// </summary>
/// <param name="paramname">name as a string</param>
/// <param name="value"></param>
public bool setParam(string paramname, float value)
{
if (!param.ContainsKey(paramname))
{
Console.WriteLine("Param doesnt exist " + paramname);
return false;
}
MainV2.givecomport = true;
__mavlink_param_set_t req = new __mavlink_param_set_t();
req.target_system = sysid;
req.target_component = compid;
byte[] temp = ASCIIEncoding.ASCII.GetBytes(paramname);
modifyParamForDisplay(false, paramname, ref value);
#if MAVLINK10
Array.Resize(ref temp, 16);
#else
Array.Resize(ref temp, 15);
#endif
req.param_id = temp;
req.param_value = (value);
generatePacket(MAVLINK_MSG_ID_PARAM_SET, req);
Console.WriteLine("setParam '{0}' = '{1}' sysid {2} compid {3}", paramname, req.param_value, sysid, compid);
DateTime start = DateTime.Now;
int retrys = 3;
while (true)
{
if (!(start.AddMilliseconds(500) > DateTime.Now))
{
if (retrys > 0)
{
Console.WriteLine("setParam Retry " + retrys);
generatePacket(MAVLINK_MSG_ID_PARAM_SET, req);
start = DateTime.Now;
retrys--;
continue;
}
MainV2.givecomport = false;
throw new Exception("Timeout on read - setParam " + paramname);
}
byte[] buffer = readPacket();
if (buffer.Length > 5)
{
if (buffer[5] == MAVLINK_MSG_ID_PARAM_VALUE)
{
__mavlink_param_value_t par = new __mavlink_param_value_t();
object tempobj = par;
ByteArrayToStructure(buffer, ref tempobj, 6);
par = (__mavlink_param_value_t)tempobj;
string st = System.Text.ASCIIEncoding.ASCII.GetString(par.param_id);
int pos = st.IndexOf('\0');
if (pos != -1)
{
st = st.Substring(0, pos);
}
if (st != paramname)
{
Console.WriteLine("MAVLINK bad param responce - {0} vs {1}", paramname, st);
continue;
}
modifyParamForDisplay(true, st, ref par.param_value);
param[st] = (par.param_value);
MainV2.givecomport = false;
//System.Threading.Thread.Sleep(100);//(int)(8.5 * 5)); // 8.5ms per byte
return true;
}
}
}
}
