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"); } }