/// <summary>
/// Sets wp total count
/// </summary>
/// <param name="wp_total"></param>
public void setWPTotal(ushort wp_total)
{
#if MAVLINK10
MainV2.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)
{
Console.WriteLine("setWPTotal Retry " + retrys);
generatePacket(MAVLINK_MSG_ID_MISSION_COUNT, req);
start = DateTime.Now;
retrys--;
continue;
}
MainV2.givecomport = false;
throw new Exception("Timeout on read - setWPTotal");
}
byte[] buffer = readPacket();
if (buffer.Length > 9)
{
if (buffer[5] == MAVLINK_MSG_ID_MISSION_REQUEST)
{
__mavlink_mission_request_t request = new __mavlink_mission_request_t();
object temp = (object)request;
ByteArrayToStructure(buffer, ref temp, 6);
request = (__mavlink_mission_request_t)(temp);
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;
MainV2.givecomport = false;
return;
}
}
else
{
//Console.WriteLine(DateTime.Now + " PC getwp " + buffer[5]);
}
}
}
#else
MainV2.givecomport = true;
__mavlink_waypoint_count_t req = new __mavlink_waypoint_count_t();
req.target_system = sysid;
req.target_component = compid; // MAVLINK_MSG_ID_WAYPOINT_COUNT
req.count = wp_total;
generatePacket(MAVLINK_MSG_ID_WAYPOINT_COUNT, req);
DateTime start = DateTime.Now;
int retrys = 3;
while (true)
{
if (!(start.AddMilliseconds(700) > DateTime.Now))
{
if (retrys > 0)
{
Console.WriteLine("setWPTotal Retry " + retrys);
generatePacket(MAVLINK_MSG_ID_WAYPOINT_COUNT, req);
start = DateTime.Now;
retrys--;
continue;
}
MainV2.givecomport = false;
throw new Exception("Timeout on read - setWPTotal");
}
byte[] buffer = readPacket();
if (buffer.Length > 9)
{
if (buffer[5] == MAVLINK_MSG_ID_WAYPOINT_REQUEST)
{
__mavlink_waypoint_request_t request = new __mavlink_waypoint_request_t();
object temp = (object)request;
ByteArrayToStructure(buffer, ref temp, 6);
request = (__mavlink_waypoint_request_t)(temp);
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;
MainV2.givecomport = false;
return;
}
}
else
{
//Console.WriteLine(DateTime.Now + " PC getwp " + buffer[5]);
}
}
}
#endif
}
/// <summary>
/// Returns WP count
/// </summary>
/// <returns></returns>
public byte getWPCount()
{
MainV2.givecomport = true;
byte[] buffer;
#if MAVLINK10
__mavlink_mission_request_list_t req = new __mavlink_mission_request_list_t();
req.target_system = sysid;
req.target_component = compid;
// request list
generatePacket(MAVLINK_MSG_ID_MISSION_REQUEST_LIST, req);
DateTime start = DateTime.Now;
int retrys = 6;
while (true)
{
if (!(start.AddMilliseconds(500) > DateTime.Now))
{
if (retrys > 0)
{
Console.WriteLine("getWPCount Retry " + retrys + " - giv com " + MainV2.givecomport);
generatePacket(MAVLINK_MSG_ID_MISSION_REQUEST_LIST, req);
start = DateTime.Now;
retrys--;
continue;
}
MainV2.givecomport = false;
//return (byte)int.Parse(param["WP_TOTAL"].ToString());
throw new Exception("Timeout on read - getWPCount");
}
buffer = readPacket();
if (buffer.Length > 5)
{
if (buffer[5] == MAVLINK_MSG_ID_MISSION_COUNT)
{
__mavlink_mission_count_t count = new __mavlink_mission_count_t();
object temp = (object)count;
ByteArrayToStructure(buffer, ref temp, 6);
count = (__mavlink_mission_count_t)(temp);
Console.WriteLine("wpcount: " + count.count);
MainV2.givecomport = false;
return (byte)count.count; // should be ushort, but apm has limited wp count < byte
}
else
{
Console.WriteLine(DateTime.Now + " PC wpcount " + buffer[5] + " need " + MAVLINK_MSG_ID_MISSION_COUNT + " " + this.BaseStream.BytesToRead);
}
}
}
#else
__mavlink_waypoint_request_list_t req = new __mavlink_waypoint_request_list_t();
req.target_system = sysid;
req.target_component = compid;
// request list
generatePacket(MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST, req);
DateTime start = DateTime.Now;
int retrys = 6;
while (true)
{
if (!(start.AddMilliseconds(500) > DateTime.Now))
{
if (retrys > 0)
{
Console.WriteLine("getWPCount Retry " + retrys + " - giv com " + MainV2.givecomport);
generatePacket(MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST, req);
start = DateTime.Now;
retrys--;
continue;
}
MainV2.givecomport = false;
//return (byte)int.Parse(param["WP_TOTAL"].ToString());
throw new Exception("Timeout on read - getWPCount");
}
buffer = readPacket();
if (buffer.Length > 5)
{
if (buffer[5] == MAVLINK_MSG_ID_WAYPOINT_COUNT)
{
Console.WriteLine("wpcount: " + buffer[9]);
MainV2.givecomport = false;
return buffer[9]; // should be ushort, but apm has limited wp count < byte
}
else
{
Console.WriteLine(DateTime.Now + " PC wpcount " + buffer[5] + " need " + MAVLINK_MSG_ID_WAYPOINT_COUNT + " " + this.BaseStream.BytesToRead);
}
}
}
#endif
}
