public PointLatLngAlt getFencePoint(int no, ref int total)
{
MAVLinkMessage buffer;
giveComport = true;
PointLatLngAlt plla = new PointLatLngAlt();
mavlink_fence_fetch_point_t req = new mavlink_fence_fetch_point_t();
req.idx = (byte) no;
req.target_component = MAV.compid;
req.target_system = MAV.sysid;
// request point
generatePacket((byte) MAVLINK_MSG_ID.FENCE_FETCH_POINT, req);
DateTime start = DateTime.Now;
int retrys = 3;
while (true)
{
if (!(start.AddMilliseconds(700) > DateTime.Now))
{
if (retrys > 0)
{
log.Info("getFencePoint Retry " + retrys + " - giv com " + giveComport);
generatePacket((byte) MAVLINK_MSG_ID.FENCE_FETCH_POINT, req);
start = DateTime.Now;
retrys--;
continue;
}
giveComport = false;
throw new Exception("Timeout on read - getFencePoint");
}
buffer = readPacket();
if (buffer.Length > 5)
{
if (buffer.msgid == (byte) MAVLINK_MSG_ID.FENCE_POINT)
{
giveComport = false;
mavlink_fence_point_t fp = buffer.ToStructure<mavlink_fence_point_t>();
plla.Lat = fp.lat;
plla.Lng = fp.lng;
plla.Tag = fp.idx.ToString();
total = fp.count;
return plla;
}
}
}
}
/// <summary>
/// Used to extract mission from log file
/// </summary>
/// <param name="buffer">packet</param>
void getWPsfromstream(ref byte[] buffer)
{
#if MAVLINK10
if (buffer[5] == MAVLINK_MSG_ID_MISSION_COUNT)
{
// clear old
wps = new PointLatLngAlt[wps.Length];
}
if (buffer[5] == MAVLink.MAVLINK_MSG_ID_MISSION_ITEM)
{
mavlink_mission_item_t wp = buffer.ByteArrayToStructure<mavlink_mission_item_t>(6);
#else
if (buffer[5] == MAVLINK_MSG_ID_WAYPOINT_COUNT)
{
// clear old
wps = new PointLatLngAlt[wps.Length];
}
if (buffer[5] == MAVLink.MAVLINK_MSG_ID_WAYPOINT)
{
mavlink_waypoint_t wp = buffer.ByteArrayToStructure<mavlink_waypoint_t>(6);
#endif
wps[wp.seq] = new PointLatLngAlt(wp.x, wp.y, wp.z, wp.seq.ToString());
Console.WriteLine("WP # {7} cmd {8} p1 {0} p2 {1} p3 {2} p4 {3} x {4} y {5} z {6}",wp.param1,wp.param2,wp.param3,wp.param4,wp.x,wp.y,wp.z,wp.seq,wp.command);
}
}
public PointLatLngAlt getFencePoint(int no, ref int total)
{
byte[] buffer;
MainV2.giveComport = true;
PointLatLngAlt plla = new PointLatLngAlt();
mavlink_fence_fetch_point_t req = new mavlink_fence_fetch_point_t();
req.idx = (byte)no;
req.target_component = compid;
req.target_system = sysid;
// request point
generatePacket(MAVLINK_MSG_ID_FENCE_FETCH_POINT, req);
DateTime start = DateTime.Now;
int retrys = 3;
while (true)
{
if (!(start.AddMilliseconds(500) > DateTime.Now))
{
if (retrys > 0)
{
log.Info("getFencePoint Retry " + retrys + " - giv com " + MainV2.giveComport);
generatePacket(MAVLINK_MSG_ID_FENCE_FETCH_POINT, req);
start = DateTime.Now;
retrys--;
continue;
}
MainV2.giveComport = false;
throw new Exception("Timeout on read - getFencePoint");
}
buffer = readPacket();
if (buffer.Length > 5)
{
if (buffer[5] == MAVLINK_MSG_ID_FENCE_POINT)
{
MainV2.giveComport = false;
mavlink_fence_point_t fp = buffer.ByteArrayToStructure<mavlink_fence_point_t>(6);
plla.Lat = fp.lat;
plla.Lng = fp.lng;
plla.Tag = fp.idx.ToString();
total = fp.count;
return plla;
}
}
}
}
