public bool setFencePoint(byte index, PointLatLngAlt plla, byte fencepointcount)
{
mavlink_fence_point_t fp = new mavlink_fence_point_t();
fp.idx = index;
fp.count = fencepointcount;
fp.lat = (float) plla.Lat;
fp.lng = (float) plla.Lng;
fp.target_component = MAV.compid;
fp.target_system = MAV.sysid;
int retry = 3;
PointLatLngAlt newfp;
while (retry > 0)
{
generatePacket((byte) MAVLINK_MSG_ID.FENCE_POINT, fp);
int counttemp = 0;
newfp = getFencePoint(fp.idx, ref counttemp);
if (newfp.GetDistance(plla) < 5)
return true;
retry--;
}
throw new Exception("Could not verify GeoFence Point");
}