public void sendWPS(List <PointLatLngAlt> Points)
{
MAVLink.MAV_FRAME frame = MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT;
Locationwp home = new Locationwp();
try
{
home.id = (byte)MAVLink.MAV_CMD.WAYPOINT;
home.lat = 23.1647061918178;
home.lng = 113.455638885498;
home.alt = 20; // use saved home
}
catch { throw new Exception("Your home location is invalid"); }
ushort count = (ushort)(Points.Count + 1);//含家地址
UpLoadForm upform = new UpLoadForm();
upform.StartPosition = FormStartPosition.Manual;
upform.Location = new System.Drawing.Point((this.form.Width - upform.Width) / 2, (this.form.Height - upform.Height) / 2);
// this.upform.Visible = true;
// Application.DoEvents();
upform.Show();
// Application.DoEvents();
upform.SetMessage(0, count);
// this.upform.ShowDialog();
// Thread.Sleep(100);
setWPTotal(count);
this.ListMessage("已发送航点数量...");
upform.SetMessage(1, 0);
bool mission_uploading = true;
Locationwp[] wp_location = new Locationwp[count - 1];
int i = 0;
foreach (PointLatLngAlt point in Points)
{
wp_location[i].id = (byte)MAVLink.MAV_CMD.WAYPOINT;
wp_location[i].lat = point.Lat;
wp_location[i].lng = point.Lng;
wp_location[i].alt = point.Alt;
i++;
}
int seq = 0;
bool isSendTotalSuccess = false;
int cnt = 0;
while (mission_uploading)
{
Program.mav_msg_handler.mission_request = false;
Program.mav_msg_handler.mission_ack = false;
Program.mav_msg_handler.SetTimeout(5);
while (true)
{
if (Program.mav_msg_handler.Wait())
{
if (isSendTotalSuccess)
{
ListMessage("发送航点超时出错");
mission_uploading = false;
break;
}
else
{
cnt++;
if (cnt >= 3)
{
ListMessage("发送航点数量超时出错");
mission_uploading = false;
break;
}
else
{
ListMessage("发送航点数量超时出错，准备重发...");
setWPTotal(count);
break;
}
}
}
if (upform.isCancel)
{
this.ListMessage("您已经取消航点发送...");
upform.Close();
return;
}
if (Program.mav_msg_handler.mission_ack)
{
// this.ListMessage("收到ACK...");
if (Program.mav_msg_handler.mavlink_mission_ack.type != 0)
{
ListMessage("发送航点出错");
}
else
{
ListMessage("航点上传完毕");
}
mission_uploading = false;
break;
}
if (Program.mav_msg_handler.mission_request == true)
{
isSendTotalSuccess = true;
// this.ListMessage("收到REQUEST...");
seq = Program.mav_msg_handler.mavlink_mission_request.seq;
if (seq == 0)
{
setWP(home, 0, MAVLink.MAV_FRAME.GLOBAL, 0);
ListMessage("正在发送HOME...");
}
else
{
setWP(wp_location[seq - 1], (ushort)(seq), frame, 0);
ListMessage("正在发送航点" + seq.ToString() + "...");
upform.SetMessage(2, seq);
upform.SetCount(seq, count);
}
break;
}
Application.DoEvents();
}
}
Thread.Sleep(500);
upform.SetMessage(3, 0);
Thread.Sleep(500);
setWPACK();
upform.Close();
}
public List <PointLatLngAlt> drawPoints = new List <PointLatLngAlt>(); //点集
public void getWPs()
{
bool succeed = false;
List <Locationwp> cmds = new List <Locationwp>();
UpLoadForm up = new UpLoadForm();
up.SetMessage(4, 0);
up.Show();
int cmdcount = getWPCount();
up.SetMessage(5, cmdcount);
Program.mav_msg_handler.mission_items.Clear();
ushort a = 0;
for (a = 0; a < cmdcount; a++)
{
up.SetCount1(a, cmdcount);
Locationwp wp = getWP(a, ref succeed);
if (succeed)
{
cmds.Add(wp);
}
else
{
break;
}
if (up.isCancel)
{
ListMessage("已取消下载航点...");
up.Close();
return;
}
Application.DoEvents();
}
if (succeed)
{
up.SetMessage(7, 0);
drawPoints.Clear();
foreach (Locationwp loc in cmds)
{
PointLatLngAlt PL = new PointLatLngAlt();
PL.Lat = loc.lat;
PL.Lng = loc.lng;
PL.Alt = loc.alt;
drawPoints.Add(PL);
}
}
else
{
up.SetMessage(8, 0);
}
Thread.Sleep(500);
up.Close();
}