/// <summary>
/// Sets wp total count
/// </summary>
/// <param name="wp_total"></param>
public void setWPTotal(ushort wp_total)
{
giveComport = true;
mavlink_mission_count_t req = new mavlink_mission_count_t();
req.target_system = MAV.sysid;
req.target_component = MAV.compid; // MSG_NAMES.MISSION_COUNT
req.count = wp_total;
generatePacket((byte) MAVLINK_MSG_ID.MISSION_COUNT, req);
DateTime start = DateTime.Now;
int retrys = 3;
while (true)
{
if (!(start.AddMilliseconds(700) > DateTime.Now))
{
if (retrys > 0)
{
log.Info("setWPTotal Retry " + retrys);
generatePacket((byte) MAVLINK_MSG_ID.MISSION_COUNT, req);
start = DateTime.Now;
retrys--;
continue;
}
giveComport = false;
throw new Exception("Timeout on read - setWPTotal");
}
MAVLinkMessage buffer = readPacket();
if (buffer.Length > 9)
{
if (buffer.msgid == (byte) MAVLINK_MSG_ID.MISSION_REQUEST)
{
var request = buffer.ToStructure<mavlink_mission_request_t>();
if (request.seq == 0)
{
if (MAV.param["WP_TOTAL"] != null)
MAV.param["WP_TOTAL"].Value = wp_total - 1;
if (MAV.param["CMD_TOTAL"] != null)
MAV.param["CMD_TOTAL"].Value = wp_total - 1;
if (MAV.param["MIS_TOTAL"] != null)
MAV.param["MIS_TOTAL"].Value = wp_total - 1;
MAV.wps.Clear();
giveComport = false;
return;
}
}
else
{
//Console.WriteLine(DateTime.Now + " PC getwp " + buffer.msgid);
}
}
}
}
/// <summary>
/// Sets wp total count
/// </summary>
/// <param name="wp_total"></param>
public void setWPTotal(ushort wp_total)
{
giveComport = true;
mavlink_mission_count_t req = new mavlink_mission_count_t();
req.target_system = sysid;
req.target_component = compid; // MAVLINK_MSG_ID_MISSION_COUNT
req.count = wp_total;
generatePacket(MAVLINK_MSG_ID_MISSION_COUNT, req);
DateTime start = DateTime.Now;
int retrys = 3;
while (true)
{
if (!(start.AddMilliseconds(700) > DateTime.Now))
{
if (retrys > 0)
{
log.Info("setWPTotal Retry " + retrys);
generatePacket(MAVLINK_MSG_ID_MISSION_COUNT, req);
start = DateTime.Now;
retrys--;
continue;
}
giveComport = false;
throw new Exception("Timeout on read - setWPTotal");
}
byte[] buffer = readPacket();
if (buffer.Length > 9)
{
if (buffer[5] == MAVLINK_MSG_ID_MISSION_REQUEST)
{
var request = buffer.ByteArrayToStructure<mavlink_mission_request_t>(6);
if (request.seq == 0)
{
if (param["WP_TOTAL"] != null)
param["WP_TOTAL"] = (float)wp_total - 1;
if (param["CMD_TOTAL"] != null)
param["CMD_TOTAL"] = (float)wp_total - 1;
wps.Clear();
giveComport = false;
return;
}
}
else
{
//Console.WriteLine(DateTime.Now + " PC getwp " + buffer[5]);
}
}
}
}
