private void btnCal_Click(object sender, EventArgs e)
{
try
{
List <BLHCoordinate> pSourceBLH = new List <BLHCoordinate>();
List <XYZCoordinate> pSource = new List <XYZCoordinate>();
List <XYZCoordinate> pTo = new List <XYZCoordinate>();
List <XYZCoordinate> pToNew = new List <XYZCoordinate>();
if (txtB1.Text != "" && txtL1.Text != "")
{
pSourceBLH.Add(new BLHCoordinate(double.Parse(txtB1.Text), double.Parse(txtL1.Text), 0));
}
if (txtB2.Text != "" && txtL2.Text != "")
{
pSourceBLH.Add(new BLHCoordinate(double.Parse(txtB2.Text), double.Parse(txtL2.Text), 0));
}
if (txtB3.Text != "" && txtL3.Text != "")
{
pSourceBLH.Add(new BLHCoordinate(double.Parse(txtB3.Text), double.Parse(txtL3.Text), 0));
}
if (txtB4.Text != "" && txtL4.Text != "")
{
pSourceBLH.Add(new BLHCoordinate(double.Parse(txtB4.Text), double.Parse(txtL4.Text), 0));
}
if (txtB5.Text != "" && txtL5.Text != "")
{
pSourceBLH.Add(new BLHCoordinate(double.Parse(txtB5.Text), double.Parse(txtL5.Text), 0));
}
if (txtB6.Text != "" && txtL6.Text != "")
{
pSourceBLH.Add(new BLHCoordinate(double.Parse(txtB6.Text), double.Parse(txtL6.Text), 0));
}
if (txtX1.Text != "" && txtY1.Text != "")
{
pTo.Add(new XYZCoordinate(double.Parse(txtX1.Text), double.Parse(txtY1.Text), 0));
}
if (txtX2.Text != "" && txtY2.Text != "")
{
pTo.Add(new XYZCoordinate(double.Parse(txtX2.Text), double.Parse(txtY2.Text), 0));
}
if (txtX3.Text != "" && txtY3.Text != "")
{
pTo.Add(new XYZCoordinate(double.Parse(txtX3.Text), double.Parse(txtY3.Text), 0));
}
if (txtX4.Text != "" && txtY4.Text != "")
{
pTo.Add(new XYZCoordinate(double.Parse(txtX4.Text), double.Parse(txtY4.Text), 0));
}
if (txtX5.Text != "" && txtY5.Text != "")
{
pTo.Add(new XYZCoordinate(double.Parse(txtX5.Text), double.Parse(txtY5.Text), 0));
}
if (txtX6.Text != "" && txtY6.Text != "")
{
pTo.Add(new XYZCoordinate(double.Parse(txtX6.Text), double.Parse(txtY6.Text), 0));
}
int ptNum = 0;
if (txtPtNum.Text != "")
{
ptNum = int.Parse(txtPtNum.Text);
}
double Lon = 0;
if (pSourceBLH.Count > 1)
{
Lon = (int)pSourceBLH[0].L;
}
else
{
MessageBox.Show("请输入控制点坐标!");
return;
}
for (int i = 0; i < ptNum; i++)
{
XYZCoordinate wgsxy = GaussProjection.GaussProjCal(pSourceBLH[i], WGS84Datum, Lon);
pSource.Add(wgsxy);
pToNew.Add(pTo[i]);
}
double rms;
FourPara forPara = Para4.Calc4Para(pSource, pTo, out rms);
txtLon.Text = Lon + "";
txtDeltaX.Text = forPara.deltaX + "";
txtDeltaY.Text = forPara.deltaY + "";
txtAx.Text = forPara.Ax + "";
txtS.Text = forPara.scale + "";
}
catch
{
MessageBox.Show("错误，请检查!");
}
}
private void BUT_Accept_Click(object sender, EventArgs e)
{
if (grid != null && grid.Count > 0)
{
MainV2.instance.FlightPlanner.quickadd = true;
PointLatLngAlt lastpnt = PointLatLngAlt.Zero;
if (MainV2.comPort.BaseStream.IsOpen)
{
homelat = MainV2.comPort.MAV.cs.lat;
homelong = MainV2.comPort.MAV.cs.lng;
}
else
{
homelat = double.Parse(MainV2.instance.FlightPlanner.TXT_homelat.Text);
homelong = double.Parse(MainV2.instance.FlightPlanner.TXT_homelng.Text);
}
//plugin.Host.AddWPtoList(MAVLink.MAV_CMD.DO_CHANGE_SPEED, 1,
// (int)((float)NUM_UpDownFlySpeed.Value / CurrentState.multiplierspeed), 0, 0, 0, 0, 0,
// null);
double X, Y;
int n = 2;//计算第一个wp
if (chk_markers.Checked)
{
n = 1;
}
var firstwp = grid[n];
var firstwpxyz = GaussProjection.GaussProjCal(new BLHCoordinate(firstwp.Lat, firstwp.Lng, firstwp.Alt), CoordConsts.cgcs2000atum, 0);
var homexyz = GaussProjection.GaussProjCal(new BLHCoordinate(homelat, homelong, 0), CoordConsts.cgcs2000atum, 0);
double dist_hometofirstwp = squar(firstwpxyz.X - homexyz.X, firstwpxyz.Y - homexyz.Y);
double dist_rat = (double)loiter_r.Value / dist_hometofirstwp;
//目标XY
X = firstwpxyz.X - dist_rat * (firstwpxyz.X - homexyz.X);
Y = firstwpxyz.Y - dist_rat * (firstwpxyz.Y - homexyz.Y);
var targetwp = GaussProjection.GaussProjInvCal(new XYZCoordinate(X, Y, 0), CoordConsts.cgcs2000atum, (int)((firstwp.Lng - 1.5) / 3 + 1) * 3);//根据第一个航点计算带号
plugin.Host.AddWPtoList(MAVLink.MAV_CMD.WAYPOINT, 0, 0, 0, 0, targetwp.L, targetwp.B, grid[n].Alt);
plugin.Host.AddWPtoList(MAVLink.MAV_CMD.DO_SET_CAM_TRIGG_DIST, (double)cam_dist.Value, 0, 0, 0, 0, 0, 0);
grid.ForEach(plla =>
{
if (plla.Tag == "M")
{
if (CHK_internals.Checked)
{
plugin.Host.AddWPtoList(MAVLink.MAV_CMD.LOITER_TURNS, (double)loiter_turn.Value, 0, (double)loiter_r.Value, 0, plla.Lng, plla.Lat, plla.Alt);
}
}
else
{
if (!(plla.Lat == lastpnt.Lat && plla.Lng == lastpnt.Lng && plla.Alt == lastpnt.Alt) && chk_markers.Checked == true)
{
plugin.Host.AddWPtoList(MAVLink.MAV_CMD.LOITER_TURNS, (double)loiter_turn.Value, 0, (double)loiter_r.Value, 0, plla.Lng, plla.Lat, plla.Alt);
}
lastpnt = plla;
}
});
plugin.Host.AddWPtoList(MAVLink.MAV_CMD.DO_SET_CAM_TRIGG_DIST, 0, 0, 0, 0, 0, 0, 0);
MainV2.instance.FlightPlanner.quickadd = false;
MainV2.instance.FlightPlanner.writeKML();
savesettings();
this.Close();
}
else
{
CustomMessageBox.Show("Bad Grid", "Error");
}
}