/// <summary>
/// Set parameter on apm
/// </summary>
/// <param name="paramname">name as a string</param>
/// <param name="value"></param>
public bool setParam(string paramname, double value, bool force = false)
{
if (!MAV.param.ContainsKey(paramname))
{
log.Warn("Trying to set Param that doesnt exist " + paramname + "=" + value);
return false;
}
if (MAV.param[paramname].Value == value && !force)
{
log.Warn("setParam " + paramname + " not modified as same");
return true;
}
giveComport = true;
// param type is set here, however it is always sent over the air as a float 100int = 100f.
var req = new mavlink_param_set_t
{
target_system = MAV.sysid,
target_component = MAV.compid,
param_type = (byte) MAV.param_types[paramname]
};
byte[] temp = Encoding.ASCII.GetBytes(paramname);
Array.Resize(ref temp, 16);
req.param_id = temp;
if (MAV.apname == MAV_AUTOPILOT.ARDUPILOTMEGA)
{
req.param_value = new MAVLinkParam(paramname, value, (MAV_PARAM_TYPE.REAL32)).float_value;
}
else
{
req.param_value =
new MAVLinkParam(paramname, value, (MAV_PARAM_TYPE) MAV.param_types[paramname]).float_value;
}
generatePacket((byte) MAVLINK_MSG_ID.PARAM_SET, req);
log.InfoFormat("setParam '{0}' = '{1}' sysid {2} compid {3}", paramname, req.param_value, MAV.sysid,
MAV.compid);
DateTime start = DateTime.Now;
int retrys = 3;
while (true)
{
if (!(start.AddMilliseconds(700) > DateTime.Now))
{
if (retrys > 0)
{
log.Info("setParam Retry " + retrys);
generatePacket((byte) MAVLINK_MSG_ID.PARAM_SET, req);
start = DateTime.Now;
retrys--;
continue;
}
giveComport = false;
throw new Exception("Timeout on read - setParam " + paramname);
}
MAVLinkMessage buffer = readPacket();
if (buffer.Length > 5)
{
if (buffer.msgid == (byte) MAVLINK_MSG_ID.PARAM_VALUE)
{
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);
}
if (st != paramname)
{
log.InfoFormat("MAVLINK bad param response - {0} vs {1}", paramname, st);
continue;
}
log.Info("setParam gotback " + st + " : " + par.param_value);
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);
}
lastparamset = DateTime.Now;
giveComport = false;
//System.Threading.Thread.Sleep(100);//(int)(8.5 * 5)); // 8.5ms per byte
return true;
}
}
}
}
/// <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))
{
log.Info("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);
log.InfoFormat("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)
{
log.Info("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 = buffer.ByteArrayToStructure<mavlink_param_value_t>(6);
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)
{
log.InfoFormat("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;
}
}
}
}
/// <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))
{
log.Warn("Trying to set Param that doesnt exist " + paramname + "=" + value);
return false;
}
if ((float)param[paramname] == value)
{
log.Debug("setParam " + paramname + " not modified");
return true;
}
giveComport = true;
// param type is set here, however it is always sent over the air as a float 100int = 100f.
var req = new mavlink_param_set_t { target_system = sysid, target_component = compid, param_type = (byte)param_types[paramname] };
byte[] temp = Encoding.ASCII.GetBytes(paramname);
Array.Resize(ref temp, 16);
req.param_id = temp;
req.param_value = (value);
generatePacket(MAVLINK_MSG_ID_PARAM_SET, req);
log.InfoFormat("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)
{
log.Info("setParam Retry " + retrys);
generatePacket(MAVLINK_MSG_ID_PARAM_SET, req);
start = DateTime.Now;
retrys--;
continue;
}
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 = buffer.ByteArrayToStructure<mavlink_param_value_t>(6);
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)
{
log.InfoFormat("MAVLINK bad param responce - {0} vs {1}", paramname, st);
continue;
}
param[st] = (par.param_value);
giveComport = false;
//System.Threading.Thread.Sleep(100);//(int)(8.5 * 5)); // 8.5ms per byte
return true;
}
}
}
}
/// <summary>
/// Set parameter on apm
/// </summary>
/// <param name="paramname">name as a string</param>
/// <param name="value"></param>
public bool setParam(byte sysid, byte compid, string paramname, double value, bool force = false)
{
if (!MAVlist[sysid,compid].param.ContainsKey(paramname))
{
log.Warn("Trying to set Param that doesnt exist " + paramname + "=" + value);
return false;
}
if (MAVlist[sysid, compid].param[paramname].Value == value && !force)
{
log.Warn("setParam " + paramname + " not modified as same");
return true;
}
giveComport = true;
// param type is set here, however it is always sent over the air as a float 100int = 100f.
var req = new mavlink_param_set_t
{
target_system = sysid,
target_component = compid,
param_type = (byte)MAVlist[sysid, compid].param_types[paramname]
};
byte[] temp = Encoding.ASCII.GetBytes(paramname);
Array.Resize(ref temp, 16);
req.param_id = temp;
if (MAVlist[sysid, compid].apname == MAV_AUTOPILOT.ARDUPILOTMEGA)
{
req.param_value = new MAVLinkParam(paramname, value, (MAV_PARAM_TYPE.REAL32)).float_value;
}
else
{
req.param_value = new MAVLinkParam(paramname, value, (MAV_PARAM_TYPE)MAVlist[sysid, compid].param_types[paramname]).float_value;
}
int currentparamcount = MAVlist[sysid, compid].param.Count;
generatePacket((byte) MAVLINK_MSG_ID.PARAM_SET, req, sysid, compid);
log.InfoFormat("setParam '{0}' = '{1}' sysid {2} compid {3}", paramname, value, sysid,
compid);
DateTime start = DateTime.Now;
int retrys = 3;
while (true)
{
if (!(start.AddMilliseconds(700) > DateTime.Now))
{
if (retrys > 0)
{
log.Info("setParam Retry " + retrys);
generatePacket((byte) MAVLINK_MSG_ID.PARAM_SET, req, sysid, compid);
start = DateTime.Now;
retrys--;
continue;
}
giveComport = false;
throw new TimeoutException("Timeout on read - setParam " + paramname);
}
MAVLinkMessage buffer = readPacket();
if (buffer.Length > 5)
{
if (buffer.msgid == (byte) MAVLINK_MSG_ID.PARAM_VALUE)
{
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);
}
if (st != paramname)
{
log.InfoFormat("MAVLINK bad param response - {0} vs {1}", paramname, st);
continue;
}
if (MAVlist[sysid, compid].apname == MAV_AUTOPILOT.ARDUPILOTMEGA)
{
var offset = Marshal.OffsetOf(typeof(mavlink_param_value_t), "param_value");
MAVlist[sysid, compid].param[st] = new MAVLinkParam(st, 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");
MAVlist[sysid, compid].param[st] = new MAVLinkParam(st, BitConverter.GetBytes(Marshal.ReadInt32(par, offset.ToInt32())), (MAV_PARAM_TYPE)par.param_type, (MAV_PARAM_TYPE)par.param_type);
}
log.Info("setParam gotback " + st + " : " + MAVlist[sysid, compid].param[st]);
lastparamset = DateTime.Now;
giveComport = false;
//System.Threading.Thread.Sleep(100);//(int)(8.5 * 5)); // 8.5ms per byte
// check if enabeling this param has added subparams, queue on gui thread
if (currentparamcount < par.param_count)
{
MainV2.instance.BeginInvoke((Action) delegate
{
Loading.ShowLoading(String.Format(Strings.ParamRefreshRequired, currentparamcount,
par.param_count));
});
}
return true;
}
}
}
}
