public static List<PointLatLngAlt> getAirports(PointLatLngAlt centerpoint) { lock (locker) { log.Info("getAirports " + centerpoint); // check if we have moved 66% from our last cache center point if (currentcenter.GetDistance(centerpoint) < ((proximity / 3) * 2)) { return cache; } log.Info("getAirports - regen list"); // generate a new list currentcenter = centerpoint; cache.Clear(); foreach (PointLatLngAlt item in airports) { if (item.GetDistance(centerpoint) < proximity) { cache.Add(item); } } return cache; } }
public void LoadImageTiff(string filename) { try { using (Tiff tiff = Tiff.Open(filename, "r")) { Width_px = tiff.GetField(TiffTag.IMAGEWIDTH)[0].ToDouble(); Height_px = tiff.GetField(TiffTag.IMAGELENGTH)[0].ToDouble(); bits = tiff.GetField(TiffTag.BITSPERSAMPLE)[0].ToInt(); var modelscale = tiff.GetField(TiffTag.GEOTIFF_MODELPIXELSCALETAG); var tiepoint = tiff.GetField(TiffTag.GEOTIFF_MODELTIEPOINTTAG); i = BitConverter.ToDouble(tiepoint[1].ToByteArray(), 0); j = BitConverter.ToDouble(tiepoint[1].ToByteArray(), 0 + 8); k = BitConverter.ToDouble(tiepoint[1].ToByteArray(), 0 + 16); x = BitConverter.ToDouble(tiepoint[1].ToByteArray(), 0 + 24); y = BitConverter.ToDouble(tiepoint[1].ToByteArray(), 0 + 32); z = BitConverter.ToDouble(tiepoint[1].ToByteArray(), 0 + 40); xscale = BitConverter.ToDouble(modelscale[1].ToByteArray(), 0); yscale = BitConverter.ToDouble(modelscale[1].ToByteArray(), 0 + 8); zscale = BitConverter.ToDouble(modelscale[1].ToByteArray(), 0 + 16); Longitude = x; Latitude = y; PixelSizeXdeg = xscale; PixelSizeYdeg = yscale; //CenterLatitude = Latitude - ((PixelSizeYdeg * Height_px) / 2); //CenterLongitude = Longitude + ((PixelSizeXdeg * Width_px) / 2); PointLatLngAlt p = new PointLatLngAlt(Latitude, Longitude); Width_m = p.GetDistance(new PointLatLngAlt(Latitude, Longitude + (PixelSizeXdeg * Width_px))); Height_m = p.GetDistance(new PointLatLngAlt(Latitude - (PixelSizeYdeg * Height_px), Longitude)); } } catch (Exception ex) { CustomMessageBox.Show("Geotiff Error. " + ex.Message, "Load Fail"); } }
public static List <PointLatLngAlt> GetPolygon(List <PointLatLngAlt> polyline, int distm) { if (polyline.Count <= 3) { return(new List <PointLatLngAlt>()); } List <PointLatLngAlt> leftoffsetpoints = new List <PointLatLngAlt>(); List <PointLatLngAlt> rightoffsetpoints = new List <PointLatLngAlt>(); PointLatLngAlt prevpoint = polyline[0]; // generate a point list for all points foreach (var point in polyline) { if (point == prevpoint) { continue; } double dist = prevpoint.GetDistance(point); if (dist < (distm * 1.1)) { continue; } double bearing = prevpoint.GetBearing(point); leftoffsetpoints.Add(point.newpos(bearing - 90, distm)); rightoffsetpoints.Add(point.newpos(bearing + 90, distm)); prevpoint = point; } if (leftoffsetpoints.Count <= 1) { return(new List <PointLatLngAlt>()); } // List <PointLatLngAlt> polygonPoints = new List <PointLatLngAlt>(); polygonPoints.AddRange(leftoffsetpoints); rightoffsetpoints.Reverse(); polygonPoints.AddRange(rightoffsetpoints); return(polygonPoints); }
public static List<PointLatLngAlt> GeneratePath(MAVState MAV) { List<PointLatLngAlt> result = new List<PointLatLngAlt>(); MAVLink.mavlink_mission_item_t? prevwp = null; int a = -1; foreach (var wp in MAV.wps.Values) { a++; if (!prevwp.HasValue) { // change firstwp/aka home to valid alt prevwp = new MAVLink.mavlink_mission_item_t?(new MAVLink.mavlink_mission_item_t() { x=wp.x,y = wp.y, z = 0}); continue; } if (wp.command != (byte)MAVLink.MAV_CMD.WAYPOINT && wp.command != (byte)MAVLink.MAV_CMD.SPLINE_WAYPOINT) continue; var wp1 = new PointLatLngAlt(prevwp.Value); var wp2 = new PointLatLngAlt(wp); var bearing = wp1.GetBearing(wp2); var distwps = wp1.GetDistance(wp2); var startalt = wp1.Alt; var endalt = wp2.Alt; if (startalt == 0) { startalt = endalt; } if(distwps > 5000) continue; for (double d = 0.1; d < distwps; d += 0.1) { var pnt = wp1.newpos(bearing, d); pnt.Alt = startalt + (d/distwps) * (endalt-startalt); result.Add(pnt); } prevwp = wp; } return result; }
PointLatLngAlt FindTrailPnt(PointLatLngAlt from) { // get the start point for the distance int start = trail.IndexOf(from); for (int i = start+1; i < trail.Count; i++) { double dist = from.GetDistance(trail[i]); // 2d distance if (dist > FollowDistance) { return trail[i]; } } return null; }
static PointLatLngAlt calcIntersection(PointLatLngAlt plla, PointLatLngAlt dest, int step = 100) { int distout = 10; PointLatLngAlt newpos = PointLatLngAlt.Zero; var dist = plla.GetDistance(dest); var Y = plla.GetBearing(dest); // 20 km while (distout < (dist + 100)) { // get a projected point to test intersection against - not using slope distance PointLatLngAlt newposdist = plla.newpos(Y, distout); newposdist.Alt = srtm.getAltitude(newposdist.Lat, newposdist.Lng).alt; // get another point 'step' infront PointLatLngAlt newposdist2 = plla.newpos(Y, distout + step); newposdist2.Alt = srtm.getAltitude(newposdist2.Lat, newposdist2.Lng).alt; // x is dist from plane, y is alt var newpoint = FindLineIntersection(new PointF(0, (float)plla.Alt), new PointF((float)dist, (float)dest.Alt), new PointF((float)distout, (float)newposdist.Alt), new PointF((float)distout + step, (float)newposdist2.Alt)); if (newpoint.X != 0) { newpos = plla.newpos(Y, newpoint.X); newpos.Alt = newpoint.Y; return(newpos); } distout += step; } //addtomap(newpos, distout.ToString()); dest.Alt = 0; return(dest); }
public static PointLatLngAlt getIntersectionWithTerrain(PointLatLngAlt start, PointLatLngAlt end) { int distout = 0; int stepsize = 50; var maxdist = start.GetDistance(end); var bearing = start.GetBearing(end); var altdiff = end.Alt - start.Alt; PointLatLngAlt newpos = PointLatLngAlt.Zero; while (distout < maxdist) { // get a projected point to test intersection against - not using slope distance PointLatLngAlt terrainstart = start.newpos(bearing, distout); terrainstart.Alt = srtm.getAltitude(terrainstart.Lat, terrainstart.Lng).alt; // get another point stepsize infront PointLatLngAlt terrainend = start.newpos(bearing, distout + stepsize); terrainend.Alt = srtm.getAltitude(terrainend.Lat, terrainend.Lng).alt; // x is dist from start, y is alt var newpoint = FindLineIntersection(new PointF(0, (float)start.Alt), new PointF((float)maxdist, (float)end.Alt), new PointF((float)distout, (float)terrainstart.Alt), new PointF((float)distout + stepsize, (float)terrainend.Alt)); if (newpoint.X != 0) { newpos = start.newpos(bearing, newpoint.X); newpos.Alt = newpoint.Y; break; } distout += stepsize; } if (newpos == PointLatLngAlt.Zero) { newpos = end; } return(newpos); }
public static List <PointLatLngAlt> getAirports(PointLatLngAlt centerpoint) { lock (locker) { DateTime start = DateTime.Now; //log.Info("getAirports " + centerpoint); // check if we have moved 66% from our last cache center point if (currentcenter.GetDistance(centerpoint) < ((proximity / 3) * 2)) { if (!newairports) { return(cache); } } newairports = false; log.Info("getAirports - regen list"); // generate a new list currentcenter = centerpoint; cache.Clear(); foreach (PointLatLngAlt item in airports) { if (item.GetDistance(centerpoint) < proximity) { cache.Add(item); } } log.Info("getAirports done " + (DateTime.Now - start).TotalSeconds + " sec"); return(cache); } }
//http://www.chrobotics.com/library/understanding-euler-angles // x = north / roll // y = pitch / east // z = yaw / down public static List<PointLatLngAlt> calc(PointLatLngAlt plla, double R, double P, double Y, double hfov, double vfov) { // alt should be asl // P pitch where the center os pointing ie -80 // R roll // if roll and pitch is 0, use the quick method. if (R == 0 && P == 0) { // calc fov in m on the ground at 0 alt var fovh = Math.Tan(hfov / 2.0 * deg2rad) * plla.Alt; var fovv = Math.Tan(vfov / 2.0 * deg2rad) * plla.Alt; var fovd = Math.Sqrt(fovh * fovh + fovv * fovv); // where we put our footprint var ans2 = new List<PointLatLngAlt>(); // calc bearing from center to corner var bearing1 = Math.Atan2(fovh, fovv) * rad2deg; // calc first corner point var newpos1 = plla.newpos(bearing1 + Y, Math.Sqrt(fovh * fovh + fovv * fovv)); // set alt to 0, as we used the hypot for distance and fov is based on 0 alt newpos1.Alt = 0; // calc intersection from center to new point and return ground hit point var cen1 = calcIntersection(plla, newpos1); // add to our footprint poly ans2.Add(cen1); addtomap(cen1, "cen1"); //repeat newpos1 = plla.newpos(Y - bearing1, Math.Sqrt(fovh * fovh + fovv * fovv)); newpos1.Alt = 0; cen1 = calcIntersection(plla, newpos1); ans2.Add(cen1); addtomap(cen1, "cen2"); newpos1 = plla.newpos(bearing1 + Y - 180, Math.Sqrt(fovh * fovh + fovv * fovv)); newpos1.Alt = 0; cen1 = calcIntersection(plla, newpos1); ans2.Add(cen1); addtomap(cen1, "cen3"); newpos1 = plla.newpos(Y - bearing1 - 180, Math.Sqrt(fovh * fovh + fovv * fovv)); newpos1.Alt = 0; cen1 = calcIntersection(plla, newpos1); ans2.Add(cen1); addtomap(cen1, "cen4"); addtomap(plla, "plane"); return ans2; } GMapPolygon poly = new GMapPolygon(new List<PointLatLng>(), "rect"); double frontangle = (P*0) + vfov/2; double backangle = (P*0) - vfov/2; double leftangle = (R*0) + hfov/2; double rightangle = (R*0) - hfov/2; addtomap(plla, "plane"); Matrix3 dcm = new Matrix3(); dcm.from_euler(R * deg2rad, P * deg2rad, Y * deg2rad); dcm.normalize(); Vector3 center1 = new Vector3(0, 0, 10000); var test = dcm * center1; var bearing2 = Math.Atan2(test.y, test.x) * rad2deg; var newpos2 = plla.newpos(bearing2, Math.Sqrt(test.x * test.x + test.y * test.y)); newpos2.Alt -= test.z; var cen = calcIntersection(plla, newpos2); var dist = plla.GetDistance(cen); addtomap(cen, "center "+ dist.ToString("0")); // dcm.from_euler(R * deg2rad, P * deg2rad, Y * deg2rad); dcm.rotate(new Vector3(rightangle * deg2rad, 0, 0)); dcm.normalize(); dcm.rotate(new Vector3(0, frontangle * deg2rad, 0)); dcm.normalize(); /* var mx = new Matrix3(); var my = new Matrix3(); var mz = new Matrix3(); mx.from_euler((rightangle + R) * deg2rad, 0, 0); my.from_euler(0, (frontangle + P) * deg2rad, 0); mz.from_euler(0, 0, Y * deg2rad); printdcm(mx); printdcm(my); printdcm(mz); printdcm(my * mx); printdcm(mz * my * mx); test = mz * my * mx * center1; */ test = dcm * center1; bearing2 = (Math.Atan2(test.y, test.x) * rad2deg); newpos2 = plla.newpos(bearing2, Math.Sqrt(test.x * test.x + test.y * test.y)); newpos2.Alt -= test.z; //addtomap(newpos2, "tr2"); var groundpointtr = calcIntersection(plla, newpos2); poly.Points.Add(groundpointtr); addtomap(groundpointtr, "tr"); // dcm.from_euler(R * deg2rad, P * deg2rad, Y * deg2rad); dcm.rotate(new Vector3(leftangle * deg2rad, 0, 0)); dcm.normalize(); dcm.rotate(new Vector3(0, frontangle * deg2rad, 0)); dcm.normalize(); test = dcm * center1; bearing2 = Math.Atan2(test.y, test.x)*rad2deg; newpos2 = plla.newpos(bearing2, Math.Sqrt(test.x * test.x + test.y * test.y)); newpos2.Alt -= test.z; var groundpointtl = calcIntersection(plla, newpos2); poly.Points.Add(groundpointtl); addtomap(groundpointtl, "tl"); // dcm.from_euler(R * deg2rad, P * deg2rad, Y * deg2rad); dcm.rotate(new Vector3(leftangle * deg2rad, 0, 0)); dcm.normalize(); dcm.rotate(new Vector3(0, backangle * deg2rad, 0)); dcm.normalize(); test = dcm * center1; bearing2 = Math.Atan2(test.y, test.x) * rad2deg; newpos2 = plla.newpos(bearing2, Math.Sqrt(test.x * test.x + test.y * test.y)); newpos2.Alt -= test.z; var groundpointbl = calcIntersection(plla, newpos2); poly.Points.Add(groundpointbl); addtomap(groundpointbl, "bl"); // dcm.from_euler(R * deg2rad, P * deg2rad, Y * deg2rad); dcm.rotate(new Vector3(rightangle * deg2rad, 0, 0)); dcm.normalize(); dcm.rotate(new Vector3(0, backangle * deg2rad, 0)); dcm.normalize(); test = dcm * center1; bearing2 = Math.Atan2(test.y, test.x) * rad2deg; newpos2 = plla.newpos(bearing2, Math.Sqrt(test.x * test.x + test.y * test.y)); newpos2.Alt -= test.z; var groundpointbr = calcIntersection(plla, newpos2); poly.Points.Add(groundpointbr); addtomap(groundpointbr, "br"); // polygons.Polygons.Clear(); polygons.Polygons.Add(poly); var ans = new List<PointLatLngAlt>(); ans.Add(groundpointtl); ans.Add(groundpointtr); ans.Add(groundpointbr); ans.Add(groundpointbl); return ans; }
public void doConnect(MAVLinkInterface comPort, string portname, string baud) { bool skipconnectcheck = false; log.Info("We are connecting"); switch (portname) { case "preset": skipconnectcheck = true; break; case "TCP": comPort.BaseStream = new TcpSerial(); _connectionControl.CMB_serialport.Text = "TCP"; break; case "UDP": comPort.BaseStream = new UdpSerial(); _connectionControl.CMB_serialport.Text = "UDP"; break; case "UDPCl": comPort.BaseStream = new UdpSerialConnect(); _connectionControl.CMB_serialport.Text = "UDPCl"; break; case "AUTO": default: comPort.BaseStream = new SerialPort(); break; } // Tell the connection UI that we are now connected. _connectionControl.IsConnected(true); // Here we want to reset the connection stats counter etc. this.ResetConnectionStats(); comPort.MAV.cs.ResetInternals(); //cleanup any log being played comPort.logreadmode = false; if (comPort.logplaybackfile != null) comPort.logplaybackfile.Close(); comPort.logplaybackfile = null; try { // do autoscan if (portname == "AUTO") { Comms.CommsSerialScan.Scan(false); DateTime deadline = DateTime.Now.AddSeconds(50); while (Comms.CommsSerialScan.foundport == false) { System.Threading.Thread.Sleep(100); if (DateTime.Now > deadline) { CustomMessageBox.Show(Strings.Timeout); _connectionControl.IsConnected(false); return; } } _connectionControl.CMB_serialport.Text = portname = Comms.CommsSerialScan.portinterface.PortName; _connectionControl.CMB_baudrate.Text = baud = Comms.CommsSerialScan.portinterface.BaudRate.ToString(); } log.Info("Set Portname"); // set port, then options comPort.BaseStream.PortName = portname; log.Info("Set Baudrate"); try { comPort.BaseStream.BaudRate = int.Parse(baud); } catch (Exception exp) { log.Error(exp); } // prevent serialreader from doing anything comPort.giveComport = true; log.Info("About to do dtr if needed"); // reset on connect logic. if (config["CHK_resetapmonconnect"] == null || bool.Parse(config["CHK_resetapmonconnect"].ToString()) == true) { log.Info("set dtr rts to false"); comPort.BaseStream.DtrEnable = false; comPort.BaseStream.RtsEnable = false; comPort.BaseStream.toggleDTR(); } comPort.giveComport = false; // setup to record new logs try { Directory.CreateDirectory(MainV2.LogDir); comPort.logfile = new BufferedStream(File.Open(MainV2.LogDir + Path.DirectorySeparatorChar + DateTime.Now.ToString("yyyy-MM-dd HH-mm-ss") + ".tlog", FileMode.CreateNew, FileAccess.ReadWrite, FileShare.None)); comPort.rawlogfile = new BufferedStream(File.Open(MainV2.LogDir + Path.DirectorySeparatorChar + DateTime.Now.ToString("yyyy-MM-dd HH-mm-ss") + ".rlog", FileMode.CreateNew, FileAccess.ReadWrite, FileShare.None)); log.Info("creating logfile " + DateTime.Now.ToString("yyyy-MM-dd HH-mm-ss") + ".tlog"); } catch (Exception exp2) { log.Error(exp2); CustomMessageBox.Show(Strings.Failclog); } // soft fail // reset connect time - for timeout functions connecttime = DateTime.Now; // do the connect comPort.Open(false, skipconnectcheck); if (!comPort.BaseStream.IsOpen) { log.Info("comport is closed. existing connect"); try { _connectionControl.IsConnected(false); UpdateConnectIcon(); comPort.Close(); } catch { } return; } // 3dr radio is hidden as no hb packet is ever emitted if (comPort.sysidseen.Count > 1) { // we have more than one mav // user selection of sysid MissionPlanner.Controls.SysidSelector id = new SysidSelector(); id.Show(); } // create a copy int[] list = comPort.sysidseen.ToArray(); // get all the params foreach (var sysid in list) { comPort.sysidcurrent = sysid; comPort.getParamList(); } // set to first seen comPort.sysidcurrent = list[0]; // detect firmware we are conected to. if (comPort.MAV.cs.firmware == Firmwares.ArduCopter2) { _connectionControl.TOOL_APMFirmware.SelectedIndex = _connectionControl.TOOL_APMFirmware.Items.IndexOf(Firmwares.ArduCopter2); } else if (comPort.MAV.cs.firmware == Firmwares.Ateryx) { _connectionControl.TOOL_APMFirmware.SelectedIndex = _connectionControl.TOOL_APMFirmware.Items.IndexOf(Firmwares.Ateryx); } else if (comPort.MAV.cs.firmware == Firmwares.ArduRover) { _connectionControl.TOOL_APMFirmware.SelectedIndex = _connectionControl.TOOL_APMFirmware.Items.IndexOf(Firmwares.ArduRover); } else if (comPort.MAV.cs.firmware == Firmwares.ArduPlane) { _connectionControl.TOOL_APMFirmware.SelectedIndex = _connectionControl.TOOL_APMFirmware.Items.IndexOf(Firmwares.ArduPlane); } // check for newer firmware var softwares = Firmware.LoadSoftwares(); if (softwares.Count > 0) { try { string[] fields1 = comPort.MAV.VersionString.Split(' '); foreach (Firmware.software item in softwares) { string[] fields2 = item.name.Split(' '); // check primare firmware type. ie arudplane, arducopter if (fields1[0] == fields2[0]) { Version ver1 = VersionDetection.GetVersion(comPort.MAV.VersionString); Version ver2 = VersionDetection.GetVersion(item.name); if (ver2 > ver1) { Common.MessageShowAgain(Strings.NewFirmware, Strings.NewFirmwareA + item.name + Strings.Pleaseup); break; } // check the first hit only break; } } } catch (Exception ex) { log.Error(ex); } } FlightData.CheckBatteryShow(); MissionPlanner.Utilities.Tracking.AddEvent("Connect", "Connect", comPort.MAV.cs.firmware.ToString(), comPort.MAV.param.Count.ToString()); MissionPlanner.Utilities.Tracking.AddTiming("Connect", "Connect Time", (DateTime.Now - connecttime).TotalMilliseconds, ""); MissionPlanner.Utilities.Tracking.AddEvent("Connect", "Baud", comPort.BaseStream.BaudRate.ToString(), ""); // save the baudrate for this port config[_connectionControl.CMB_serialport.Text + "_BAUD"] = _connectionControl.CMB_baudrate.Text; this.Text = titlebar + " " + comPort.MAV.VersionString; // refresh config window if needed if (MyView.current != null) { if (MyView.current.Name == "HWConfig") MyView.ShowScreen("HWConfig"); if (MyView.current.Name == "SWConfig") MyView.ShowScreen("SWConfig"); } // load wps on connect option. if (config["loadwpsonconnect"] != null && bool.Parse(config["loadwpsonconnect"].ToString()) == true) { // only do it if we are connected. if (comPort.BaseStream.IsOpen) { MenuFlightPlanner_Click(null, null); FlightPlanner.BUT_read_Click(null, null); } } // get any rallypoints if (MainV2.comPort.MAV.param.ContainsKey("RALLY_TOTAL") && int.Parse(MainV2.comPort.MAV.param["RALLY_TOTAL"].ToString()) > 0) { FlightPlanner.getRallyPointsToolStripMenuItem_Click(null, null); double maxdist = 0; foreach (var rally in comPort.MAV.rallypoints) { foreach (var rally1 in comPort.MAV.rallypoints) { var pnt1 = new PointLatLngAlt(rally.Value.lat / 10000000.0f, rally.Value.lng / 10000000.0f); var pnt2 = new PointLatLngAlt(rally1.Value.lat / 10000000.0f, rally1.Value.lng / 10000000.0f); var dist = pnt1.GetDistance(pnt2); maxdist = Math.Max(maxdist, dist); } } if (comPort.MAV.param.ContainsKey("RALLY_LIMIT_KM") && (maxdist / 1000.0) > (float)comPort.MAV.param["RALLY_LIMIT_KM"]) { CustomMessageBox.Show(Strings.Warningrallypointdistance + " " + (maxdist / 1000.0).ToString("0.00") + " > " + (float)comPort.MAV.param["RALLY_LIMIT_KM"]); } } // set connected icon this.MenuConnect.Image = displayicons.disconnect; } catch (Exception ex) { log.Warn(ex); try { _connectionControl.IsConnected(false); UpdateConnectIcon(); comPort.Close(); } catch (Exception ex2) { log.Warn(ex2); } CustomMessageBox.Show("Can not establish a connection\n\n" + ex.Message); return; } }
private List<PointLatLngAlt> GetLocations(List<PointLatLngAlt> path, PointLatLngAlt location, double lead, double seperation) { List<PointLatLngAlt> result = new List<PointLatLngAlt>(); // get the current location closest point PointLatLngAlt closestPointLatLngAlt = PointLatLngAlt.Zero; double mindist = 99999999; foreach (var pointLatLngAlt in path) { var distloc = location.GetDistance(pointLatLngAlt); if (distloc < mindist) { mindist = distloc; closestPointLatLngAlt = pointLatLngAlt; } } var start = path.IndexOf(closestPointLatLngAlt); var a = 0; for (int i = start; i < (path.Count - 1); i++) { var targetdistance = lead - a * seperation; if (targetdistance < 0) i-=2; if (i < 0) break; double dist = closestPointLatLngAlt.GetDistance(path[i]); // 2d distance if (dist >= Math.Abs(targetdistance)) { result.Add(path[i]); i = start; if (result.Count > 20) break; if (seperation == 0) break; a++; } } return result; }
//http://www.chrobotics.com/library/understanding-euler-angles // x = north / roll // y = pitch / east // z = yaw / down public static List <PointLatLngAlt> calc(PointLatLngAlt plla, double R, double P, double Y, double hfov, double vfov) { // alt should be asl // P pitch where the center os pointing ie -80 // R roll // if roll and pitch is 0, use the quick method. if (R == 0 && P == 0) { // calc fov in m on the ground at 0 alt var fovh = Math.Tan(hfov / 2.0 * MathHelper.deg2rad) * plla.Alt; var fovv = Math.Tan(vfov / 2.0 * MathHelper.deg2rad) * plla.Alt; var fovd = Math.Sqrt(fovh * fovh + fovv * fovv); // where we put our footprint var ans2 = new List <PointLatLngAlt>(); // calc bearing from center to corner var bearing1 = Math.Atan2(fovh, fovv) * MathHelper.rad2deg; // calc first corner point var newpos1 = plla.newpos(bearing1 + Y, Math.Sqrt(fovh * fovh + fovv * fovv)); // set alt to 0, as we used the hypot for distance and fov is based on 0 alt newpos1.Alt = 0; // calc intersection from center to new point and return ground hit point var cen1 = calcIntersection(plla, newpos1); // add to our footprint poly ans2.Add(cen1); addtomap(cen1, "cen1"); //repeat newpos1 = plla.newpos(Y - bearing1, Math.Sqrt(fovh * fovh + fovv * fovv)); newpos1.Alt = 0; cen1 = calcIntersection(plla, newpos1); ans2.Add(cen1); addtomap(cen1, "cen2"); newpos1 = plla.newpos(bearing1 + Y - 180, Math.Sqrt(fovh * fovh + fovv * fovv)); newpos1.Alt = 0; cen1 = calcIntersection(plla, newpos1); ans2.Add(cen1); addtomap(cen1, "cen3"); newpos1 = plla.newpos(Y - bearing1 - 180, Math.Sqrt(fovh * fovh + fovv * fovv)); newpos1.Alt = 0; cen1 = calcIntersection(plla, newpos1); ans2.Add(cen1); addtomap(cen1, "cen4"); addtomap(plla, "plane"); return(ans2); } GMapPolygon poly = new GMapPolygon(new List <PointLatLng>(), "rect"); double frontangle = (P * 0) + vfov / 2; double backangle = (P * 0) - vfov / 2; double leftangle = (R * 0) + hfov / 2; double rightangle = (R * 0) - hfov / 2; addtomap(plla, "plane"); Matrix3 dcm = new Matrix3(); dcm.from_euler(R * MathHelper.deg2rad, P * MathHelper.deg2rad, Y * MathHelper.deg2rad); dcm.normalize(); Vector3 center1 = new Vector3(0, 0, 10000); var test = dcm * center1; var bearing2 = Math.Atan2(test.y, test.x) * MathHelper.rad2deg; var newpos2 = plla.newpos(bearing2, Math.Sqrt(test.x * test.x + test.y * test.y)); newpos2.Alt -= test.z; var cen = calcIntersection(plla, newpos2); var dist = plla.GetDistance(cen); addtomap(cen, "center " + dist.ToString("0")); // dcm.from_euler(R * MathHelper.deg2rad, P * MathHelper.deg2rad, Y * MathHelper.deg2rad); dcm.rotate(new Vector3(rightangle * MathHelper.deg2rad, 0, 0)); dcm.normalize(); dcm.rotate(new Vector3(0, frontangle * MathHelper.deg2rad, 0)); dcm.normalize(); /* * var mx = new Matrix3(); * var my = new Matrix3(); * var mz = new Matrix3(); * * mx.from_euler((rightangle + R) * MathHelper.deg2rad, 0, 0); * my.from_euler(0, (frontangle + P) * MathHelper.deg2rad, 0); * mz.from_euler(0, 0, Y * MathHelper.deg2rad); * * printdcm(mx); * printdcm(my); * printdcm(mz); * printdcm(my * mx); * printdcm(mz * my * mx); * * test = mz * my * mx * center1; */ test = dcm * center1; bearing2 = (Math.Atan2(test.y, test.x) * MathHelper.rad2deg); newpos2 = plla.newpos(bearing2, Math.Sqrt(test.x * test.x + test.y * test.y)); newpos2.Alt -= test.z; //addtomap(newpos2, "tr2"); var groundpointtr = calcIntersection(plla, newpos2); poly.Points.Add(groundpointtr); addtomap(groundpointtr, "tr"); // dcm.from_euler(R * MathHelper.deg2rad, P * MathHelper.deg2rad, Y * MathHelper.deg2rad); dcm.rotate(new Vector3(leftangle * MathHelper.deg2rad, 0, 0)); dcm.normalize(); dcm.rotate(new Vector3(0, frontangle * MathHelper.deg2rad, 0)); dcm.normalize(); test = dcm * center1; bearing2 = Math.Atan2(test.y, test.x) * MathHelper.rad2deg; newpos2 = plla.newpos(bearing2, Math.Sqrt(test.x * test.x + test.y * test.y)); newpos2.Alt -= test.z; var groundpointtl = calcIntersection(plla, newpos2); poly.Points.Add(groundpointtl); addtomap(groundpointtl, "tl"); // dcm.from_euler(R * MathHelper.deg2rad, P * MathHelper.deg2rad, Y * MathHelper.deg2rad); dcm.rotate(new Vector3(leftangle * MathHelper.deg2rad, 0, 0)); dcm.normalize(); dcm.rotate(new Vector3(0, backangle * MathHelper.deg2rad, 0)); dcm.normalize(); test = dcm * center1; bearing2 = Math.Atan2(test.y, test.x) * MathHelper.rad2deg; newpos2 = plla.newpos(bearing2, Math.Sqrt(test.x * test.x + test.y * test.y)); newpos2.Alt -= test.z; var groundpointbl = calcIntersection(plla, newpos2); poly.Points.Add(groundpointbl); addtomap(groundpointbl, "bl"); // dcm.from_euler(R * MathHelper.deg2rad, P * MathHelper.deg2rad, Y * MathHelper.deg2rad); dcm.rotate(new Vector3(rightangle * MathHelper.deg2rad, 0, 0)); dcm.normalize(); dcm.rotate(new Vector3(0, backangle * MathHelper.deg2rad, 0)); dcm.normalize(); test = dcm * center1; bearing2 = Math.Atan2(test.y, test.x) * MathHelper.rad2deg; newpos2 = plla.newpos(bearing2, Math.Sqrt(test.x * test.x + test.y * test.y)); newpos2.Alt -= test.z; var groundpointbr = calcIntersection(plla, newpos2); poly.Points.Add(groundpointbr); addtomap(groundpointbr, "br"); // polygons.Polygons.Clear(); polygons.Polygons.Add(poly); var ans = new List <PointLatLngAlt>(); ans.Add(groundpointtl); ans.Add(groundpointtr); ans.Add(groundpointbr); ans.Add(groundpointbl); return(ans); }
public static List <PointLatLngAlt> GetPolygon(List <PointLatLngAlt> polyline, int distm) { if (polyline.Count <= 3) { return(new List <PointLatLngAlt>()); } List <PointLatLngAlt> leftoffsetpoints = new List <PointLatLngAlt>(); List <PointLatLngAlt> rightoffsetpoints = new List <PointLatLngAlt>(); PointLatLngAlt prevpoint = polyline[0]; // generate a point list for all points foreach (var point in polyline) { if (point == prevpoint) { continue; } double dist = prevpoint.GetDistance(point); if (dist < (distm * 1.1)) { continue; } double bearing = prevpoint.GetBearing(point); leftoffsetpoints.Add(point.newpos(bearing - 90, distm)); rightoffsetpoints.Add(point.newpos(bearing + 90, distm)); prevpoint = point; } if (leftoffsetpoints.Count <= 1) { return(new List <PointLatLngAlt>()); } // List <PointLatLngAlt> polygonPoints = new List <PointLatLngAlt>(); polygonPoints.AddRange(leftoffsetpoints); rightoffsetpoints.Reverse(); polygonPoints.AddRange(rightoffsetpoints); return(polygonPoints); prevpoint = leftoffsetpoints[0]; while (leftoffsetpoints.Count > 0) { PointLatLngAlt closest = PointLatLngAlt.Zero; double closestdist = double.MaxValue; foreach (var pointLatLngAlt in leftoffsetpoints) { double ans; if ((ans = prevpoint.GetDistance(pointLatLngAlt)) < closestdist) { closestdist = ans; closest = pointLatLngAlt; if (ans == 0) { break; } } } leftoffsetpoints.Remove(closest); if (closestdist != 0) { polygonPoints.Add(closest); } prevpoint = closest; } prevpoint = rightoffsetpoints[rightoffsetpoints.Count - 1]; rightoffsetpoints.Add(prevpoint); polygonPoints.Add(prevpoint); while (rightoffsetpoints.Count > 0) { PointLatLngAlt closest = PointLatLngAlt.Zero; double closestdist = double.MaxValue; foreach (var pointLatLngAlt in rightoffsetpoints) { double ans; if ((ans = prevpoint.GetDistance(pointLatLngAlt)) < closestdist) { closestdist = ans; closest = pointLatLngAlt; if (ans == 0) { break; } } } rightoffsetpoints.Remove(closest); if (closestdist != 0) { polygonPoints.Add(closest); } prevpoint = closest; } return(polygonPoints); }
//http://www.chrobotics.com/library/understanding-euler-angles // x = north / roll // y = pitch / east // z = yaw / down public static List<PointLatLngAlt> calc(PointLatLngAlt plla, double R, double P, double Y, double hfov, double vfov) { // alt should be asl // P pitch where the center os pointing ie -80 // R roll GMapPolygon poly = new GMapPolygon(new List<PointLatLng>(), "rect"); double frontangle = (P*0) + vfov/2; double backangle = (P*0) - vfov/2; double leftangle = (R*0) + hfov/2; double rightangle = (R*0) - hfov/2; var fovh = Math.Tan(hfov/2.0 * deg2rad)*2.0; var fovv = Math.Tan(vfov/2.0 * deg2rad)*2.0; //DoDebug(); addtomap(plla, "plane"); Matrix3 dcm = new Matrix3(); dcm.from_euler(R * deg2rad, P * deg2rad, Y * deg2rad); dcm.normalize(); Vector3 center1 = new Vector3(0, 0, 10000); var test = dcm * center1; var bearing2 = Math.Atan2(test.y, test.x) * rad2deg; var newpos2 = plla.newpos(bearing2, Math.Sqrt(test.x * test.x + test.y * test.y)); newpos2.Alt -= test.z; var cen = calcIntersection(plla, newpos2); var dist = plla.GetDistance(cen); addtomap(cen, "center "+ dist.ToString("0")); // dcm.from_euler(R * deg2rad, P * deg2rad, Y * deg2rad); dcm.rotate(new Vector3(rightangle * deg2rad, 0, 0)); dcm.normalize(); dcm.rotate(new Vector3(0, frontangle * deg2rad, 0)); dcm.normalize(); test = dcm * center1; bearing2 = (Math.Atan2(test.y, test.x) * rad2deg); newpos2 = plla.newpos(bearing2, Math.Sqrt(test.x * test.x + test.y * test.y)); newpos2.Alt -= test.z; //addtomap(newpos2, "tr2"); var groundpointtr = calcIntersection(plla, newpos2); poly.Points.Add(groundpointtr); addtomap(groundpointtr, "tr"); // dcm.from_euler(R * deg2rad, P * deg2rad, Y * deg2rad); dcm.rotate(new Vector3(leftangle * deg2rad, 0, 0)); dcm.normalize(); dcm.rotate(new Vector3(0, frontangle * deg2rad, 0)); dcm.normalize(); test = dcm * center1; bearing2 = Math.Atan2(test.y, test.x)*rad2deg; newpos2 = plla.newpos(bearing2, Math.Sqrt(test.x * test.x + test.y * test.y)); newpos2.Alt -= test.z; var groundpointtl = calcIntersection(plla, newpos2); poly.Points.Add(groundpointtl); addtomap(groundpointtl, "tl"); // dcm.from_euler(R * deg2rad, P * deg2rad, Y * deg2rad); dcm.rotate(new Vector3(leftangle * deg2rad, 0, 0)); dcm.normalize(); dcm.rotate(new Vector3(0, backangle * deg2rad, 0)); dcm.normalize(); test = dcm * center1; bearing2 = Math.Atan2(test.y, test.x) * rad2deg; newpos2 = plla.newpos(bearing2, Math.Sqrt(test.x * test.x + test.y * test.y)); newpos2.Alt -= test.z; var groundpointbl = calcIntersection(plla, newpos2); poly.Points.Add(groundpointbl); addtomap(groundpointbl, "bl"); // dcm.from_euler(R * deg2rad, P * deg2rad, Y * deg2rad); dcm.rotate(new Vector3(rightangle * deg2rad, 0, 0)); dcm.normalize(); dcm.rotate(new Vector3(0, backangle * deg2rad, 0)); dcm.normalize(); test = dcm * center1; bearing2 = Math.Atan2(test.y, test.x) * rad2deg; newpos2 = plla.newpos(bearing2, Math.Sqrt(test.x * test.x + test.y * test.y)); newpos2.Alt -= test.z; var groundpointbr = calcIntersection(plla, newpos2); poly.Points.Add(groundpointbr); addtomap(groundpointbr, "br"); // polygons.Polygons.Clear(); polygons.Polygons.Add(poly); var ans = new List<PointLatLngAlt>(); ans.Add(groundpointtl); ans.Add(groundpointtr); ans.Add(groundpointbr); ans.Add(groundpointbl); return ans; }
static PointLatLngAlt calcIntersection(PointLatLngAlt plla, PointLatLngAlt dest, int step = 100) { int distout = 10; PointLatLngAlt newpos = PointLatLngAlt.Zero; var dist = plla.GetDistance(dest); var Y = plla.GetBearing(dest); // 20 km while (distout < (dist+100)) { // get a projected point to test intersection against - not using slope distance PointLatLngAlt newposdist = plla.newpos(Y, distout); newposdist.Alt = srtm.getAltitude(newposdist.Lat, newposdist.Lng).alt; // get another point 'step' infront PointLatLngAlt newposdist2 = plla.newpos(Y, distout + step); newposdist2.Alt = srtm.getAltitude(newposdist2.Lat, newposdist2.Lng).alt; // x is dist from plane, y is alt var newpoint = FindLineIntersection(new PointF(0, (float)plla.Alt), new PointF((float)dist, (float)dest.Alt), new PointF((float)distout, (float)newposdist.Alt), new PointF((float)distout + step, (float)newposdist2.Alt)); if (newpoint.X != 0) { newpos = plla.newpos(Y, newpoint.X); newpos.Alt = newpoint.Y; return newpos; } distout += step; } //addtomap(newpos, distout.ToString()); dest.Alt = 0; return dest; }
private void RegenerateWPRoute(List<PointLatLngAlt> fullpointlist) { route.Clear(); homeroute.Clear(); polygonsoverlay.Routes.Clear(); PointLatLngAlt lastpnt = fullpointlist[0]; PointLatLngAlt lastpnt2 = fullpointlist[0]; PointLatLngAlt lastnonspline = fullpointlist[0]; List<PointLatLngAlt> splinepnts = new List<PointLatLngAlt>(); List<PointLatLngAlt> wproute = new List<PointLatLngAlt>(); // add home - this causeszx the spline to always have a straight finish fullpointlist.Add(fullpointlist[0]); for (int a = 0; a < fullpointlist.Count; a++) { if (fullpointlist[a] == null) continue; if (fullpointlist[a].Tag2 == "spline") { if (splinepnts.Count == 0) splinepnts.Add(lastpnt); splinepnts.Add(fullpointlist[a]); } else { if (splinepnts.Count > 0) { List<PointLatLng> list = new List<PointLatLng>(); splinepnts.Add(fullpointlist[a]); Spline2 sp = new Spline2(); //sp._flags.segment_type = MissionPlanner.Controls.Waypoints.Spline2.SegmentType.SEGMENT_STRAIGHT; //sp._flags.reached_destination = true; //sp._origin = sp.pv_location_to_vector(lastpnt); //sp._destination = sp.pv_location_to_vector(fullpointlist[0]); // sp._spline_origin_vel = sp.pv_location_to_vector(lastpnt) - sp.pv_location_to_vector(lastnonspline); sp.set_wp_origin_and_destination(sp.pv_location_to_vector(lastpnt2), sp.pv_location_to_vector(lastpnt)); sp._flags.reached_destination = true; for (int no = 1; no < (splinepnts.Count - 1); no++) { Spline2.spline_segment_end_type segtype = Spline2.spline_segment_end_type.SEGMENT_END_STRAIGHT; if (no < (splinepnts.Count - 2)) { segtype = Spline2.spline_segment_end_type.SEGMENT_END_SPLINE; } sp.set_spline_destination(sp.pv_location_to_vector(splinepnts[no]), false, segtype, sp.pv_location_to_vector(splinepnts[no + 1])); //sp.update_spline(); while (sp._flags.reached_destination == false) { float t = 1f; //sp.update_spline(); sp.advance_spline_target_along_track(t); // Console.WriteLine(sp.pv_vector_to_location(sp.target_pos).ToString()); list.Add(sp.pv_vector_to_location(sp.target_pos)); } list.Add(splinepnts[no]); } list.ForEach(x => { wproute.Add(x); }); splinepnts.Clear(); /* MissionPlanner.Controls.Waypoints.Spline sp = new Controls.Waypoints.Spline(); var spline = sp.doit(splinepnts, 20, lastlastpnt.GetBearing(splinepnts[0]),false); */ lastnonspline = fullpointlist[a]; } wproute.Add(fullpointlist[a]); lastpnt2 = lastpnt; lastpnt = fullpointlist[a]; } } /* List<PointLatLng> list = new List<PointLatLng>(); fullpointlist.ForEach(x => { list.Add(x); }); route.Points.AddRange(list); */ // route is full need to get 1, 2 and last point as "HOME" route int count = wproute.Count; int counter = 0; PointLatLngAlt homepoint = new PointLatLngAlt(); PointLatLngAlt firstpoint = new PointLatLngAlt(); PointLatLngAlt lastpoint = new PointLatLngAlt(); if (count > 2) { // homeroute = last, home, first wproute.ForEach(x => { counter++; if (counter == 1) { homepoint = x; return; } if (counter == 2) { firstpoint = x; } if (counter == count - 1) { lastpoint = x; } if (counter == count) { homeroute.Points.Add(lastpoint); homeroute.Points.Add(homepoint); homeroute.Points.Add(firstpoint); return; } route.Points.Add(x); }); homeroute.Stroke = new Pen(Color.Yellow, 2); // if we have a large distance between home and the first/last point, it hangs on the draw of a the dashed line. if (homepoint.GetDistance(lastpoint) < 5000 && homepoint.GetDistance(firstpoint) < 5000) homeroute.Stroke.DashStyle = DashStyle.Dash; polygonsoverlay.Routes.Add(homeroute); route.Stroke = new Pen(Color.Yellow, 4); route.Stroke.DashStyle = DashStyle.Custom; polygonsoverlay.Routes.Add(route); } }
PointLatLngAlt FindTrailPnt(PointLatLngAlt from) { // get the start point for the distance int start = trail.IndexOf(from); for (int i = start; i > 0; i--) { double dist = from.GetDistance(trail[i]); // 2d distance if (dist > Seperation) { return trail[i]; } } return null; }
//http://www.chrobotics.com/library/understanding-euler-angles // x = north / roll // y = pitch / east // z = yaw / down public static List <PointLatLngAlt> calc(PointLatLngAlt plla, double R, double P, double Y, double hfov, double vfov) { // alt should be asl // P pitch where the center os pointing ie -80 // R roll GMapPolygon poly = new GMapPolygon(new List <PointLatLng>(), "rect"); double frontangle = (P * 0) + vfov / 2; double backangle = (P * 0) - vfov / 2; double leftangle = (R * 0) + hfov / 2; double rightangle = (R * 0) - hfov / 2; var fovh = Math.Tan(hfov / 2.0 * deg2rad) * 2.0; var fovv = Math.Tan(vfov / 2.0 * deg2rad) * 2.0; //DoDebug(); addtomap(plla, "plane"); Matrix3 dcm = new Matrix3(); dcm.from_euler(R * deg2rad, P * deg2rad, Y * deg2rad); dcm.normalize(); Vector3 center1 = new Vector3(0, 0, 10000); var test = dcm * center1; var bearing2 = Math.Atan2(test.y, test.x) * rad2deg; var newpos2 = plla.newpos(bearing2, Math.Sqrt(test.x * test.x + test.y * test.y)); newpos2.Alt -= test.z; var cen = calcIntersection(plla, newpos2); var dist = plla.GetDistance(cen); addtomap(cen, "center " + dist.ToString("0")); // dcm.from_euler(R * deg2rad, P * deg2rad, Y * deg2rad); dcm.rotate(new Vector3(rightangle * deg2rad, 0, 0)); dcm.normalize(); dcm.rotate(new Vector3(0, frontangle * deg2rad, 0)); dcm.normalize(); test = dcm * center1; bearing2 = (Math.Atan2(test.y, test.x) * rad2deg); newpos2 = plla.newpos(bearing2, Math.Sqrt(test.x * test.x + test.y * test.y)); newpos2.Alt -= test.z; //addtomap(newpos2, "tr2"); var groundpointtr = calcIntersection(plla, newpos2); poly.Points.Add(groundpointtr); addtomap(groundpointtr, "tr"); // dcm.from_euler(R * deg2rad, P * deg2rad, Y * deg2rad); dcm.rotate(new Vector3(leftangle * deg2rad, 0, 0)); dcm.normalize(); dcm.rotate(new Vector3(0, frontangle * deg2rad, 0)); dcm.normalize(); test = dcm * center1; bearing2 = Math.Atan2(test.y, test.x) * rad2deg; newpos2 = plla.newpos(bearing2, Math.Sqrt(test.x * test.x + test.y * test.y)); newpos2.Alt -= test.z; var groundpointtl = calcIntersection(plla, newpos2); poly.Points.Add(groundpointtl); addtomap(groundpointtl, "tl"); // dcm.from_euler(R * deg2rad, P * deg2rad, Y * deg2rad); dcm.rotate(new Vector3(leftangle * deg2rad, 0, 0)); dcm.normalize(); dcm.rotate(new Vector3(0, backangle * deg2rad, 0)); dcm.normalize(); test = dcm * center1; bearing2 = Math.Atan2(test.y, test.x) * rad2deg; newpos2 = plla.newpos(bearing2, Math.Sqrt(test.x * test.x + test.y * test.y)); newpos2.Alt -= test.z; var groundpointbl = calcIntersection(plla, newpos2); poly.Points.Add(groundpointbl); addtomap(groundpointbl, "bl"); // dcm.from_euler(R * deg2rad, P * deg2rad, Y * deg2rad); dcm.rotate(new Vector3(rightangle * deg2rad, 0, 0)); dcm.normalize(); dcm.rotate(new Vector3(0, backangle * deg2rad, 0)); dcm.normalize(); test = dcm * center1; bearing2 = Math.Atan2(test.y, test.x) * rad2deg; newpos2 = plla.newpos(bearing2, Math.Sqrt(test.x * test.x + test.y * test.y)); newpos2.Alt -= test.z; var groundpointbr = calcIntersection(plla, newpos2); poly.Points.Add(groundpointbr); addtomap(groundpointbr, "br"); // polygons.Polygons.Clear(); polygons.Polygons.Add(poly); var ans = new List <PointLatLngAlt>(); ans.Add(groundpointtl); ans.Add(groundpointtr); ans.Add(groundpointbr); ans.Add(groundpointbl); return(ans); }
public override void SendCommand() { if (masterpos.Lat == 0 || masterpos.Lng == 0) return; Console.WriteLine(DateTime.Now); Console.WriteLine("Leader {0} {1} {2}", masterpos.Lat, masterpos.Lng, masterpos.Alt); int a = 0; foreach (var port in MainV2.Comports) { if (port == Leader) continue; PointLatLngAlt target = new PointLatLngAlt(masterpos); try { //convert Wgs84ConversionInfo to utm CoordinateTransformationFactory ctfac = new CoordinateTransformationFactory(); GeographicCoordinateSystem wgs84 = GeographicCoordinateSystem.WGS84; int utmzone = (int) ((masterpos.Lng - -186.0)/6.0); IProjectedCoordinateSystem utm = ProjectedCoordinateSystem.WGS84_UTM(utmzone, masterpos.Lat < 0 ? false : true); ICoordinateTransformation trans = ctfac.CreateFromCoordinateSystems(wgs84, utm); double[] pll1 = {target.Lng, target.Lat}; double[] p1 = trans.MathTransform.Transform(pll1); double heading = -Leader.MAV.cs.yaw; double length = offsets[port].length(); var x = ((HIL.Vector3)offsets[port]).x; var y = ((HIL.Vector3)offsets[port]).y; // add offsets to utm p1[0] += x*Math.Cos(heading*deg2rad) - y*Math.Sin(heading*deg2rad); p1[1] += x*Math.Sin(heading*deg2rad) + y*Math.Cos(heading*deg2rad); if (port.MAV.cs.firmware == MainV2.Firmwares.ArduPlane) { // project the point forwards gs*5 var gs = port.MAV.cs.groundspeed; p1[1] += gs*5*Math.Cos((-heading)*deg2rad); p1[0] += gs*5*Math.Sin((-heading)*deg2rad); } // convert back to wgs84 IMathTransform inversedTransform = trans.MathTransform.Inverse(); double[] point = inversedTransform.Transform(p1); target.Lat = point[1]; target.Lng = point[0]; target.Alt += ((HIL.Vector3) offsets[port]).z; if (port.MAV.cs.firmware == MainV2.Firmwares.ArduPlane) { var dist = target.GetDistance(new PointLatLngAlt(port.MAV.cs.lat, port.MAV.cs.lng, port.MAV.cs.alt)); dist -= port.MAV.cs.groundspeed*5; var leadergs = Leader.MAV.cs.groundspeed; var newspeed = (leadergs + (float) (dist/10)); if (newspeed < 5) newspeed = 5; port.setParam("TRIM_ARSPD_CM", newspeed*100.0f); } port.setGuidedModeWP(new Locationwp() { alt = (float) target.Alt, lat = target.Lat, lng = target.Lng, id = (ushort)MAVLink.MAV_CMD.WAYPOINT }); Console.WriteLine("{0} {1} {2} {3}", port.ToString(), target.Lat, target.Lng, target.Alt); } catch (Exception ex) { Console.WriteLine("Failed to send command " + port.ToString() + "\n" + ex.ToString()); } a++; } }
PointLatLngAlt getSRTMAltPath(List <PointLatLngAlt> list, float triangle, ref bool carryon, ref bool up, ref bool down) { PointLatLngAlt answer = new PointLatLngAlt(); PointLatLngAlt last = list[0]; PointLatLngAlt loc = list[1]; double disttotal = 0; int a = 0; double elev = 0; double height = 0; double dist = last.GetDistance(loc); int points = (int)(dist / 10) + 1; double deltalat = (last.Lat - loc.Lat); double deltalng = (last.Lng - loc.Lng); double steplat = deltalat / points; double steplng = deltalng / points; PointLatLngAlt lastpnt = last; while (a <= points && elev < homealt + base_height + height && elev < drone_alt - clearance) { double lat = last.Lat - steplat * a; double lng = last.Lng - steplng * a; var newpoint = new PointLatLngAlt(lat, lng, srtm.getAltitude(lat, lng).alt, ""); double subdist = lastpnt.GetDistance(newpoint); disttotal += subdist; height = disttotal * Math.Tan(triangle); //Height of traingle formed at point with projected line // srtm alts answer = newpoint; elev = newpoint.Alt; lastpnt = newpoint; a++; } if (elev < homealt + base_height + height + 0.1 && elev > homealt + base_height + height - 0.1 && elev < drone_alt - clearance + 0.1 && elev > drone_alt - clearance - 0.1) //Terrain intercept found { carryon = false; } else { if (elev > homealt + base_height + height && elev > drone_alt - clearance) { up = true; down = false; } else { down = true; up = false; } } return(answer); }
public string SUBMITHOST; // Submitter Host “1.2.3.4” or “enduser5.faa.gov” public List <List <PointLatLng> > GetPaths() { //RLN27.576944W97.108611LN27.468056W96.961111LN27.322222W97.050000LN27.345833W97.088889LN27.439167W97.186944RLN27.672778W97.212222LN27.576944W97.108611LN27.533333W97.133333LN27.638333W97.237222RCN27.686333W97.294667R007.00 List <List <PointLatLng> > list = new List <List <PointLatLng> >(); List <PointLatLng> pointlist = new List <PointLatLng>(); var matches = all.Matches(BOUND); Console.WriteLine(BOUND); bool isarcterminate = false; bool iscircleterminate = false; int arcdir = 0; PointLatLngAlt pointcent = null; PointLatLngAlt pointstart = null; foreach (Match item in matches) { try { if (item.Groups[2].Value == "L") { var point = new PointLatLngAlt(double.Parse(item.Groups[4].Value, CultureInfo.InvariantCulture), double.Parse(item.Groups[6].Value, CultureInfo.InvariantCulture)); if (item.Groups[3].Value == "S") { point.Lat *= -1; } if (item.Groups[5].Value == "W") { point.Lng *= -1; } if (isarcterminate) { double radius = pointcent.GetDistance(pointstart); double startbearing = pointcent.GetBearing(pointstart); double endbearing = pointcent.GetBearing(point); if (arcdir > 0 && endbearing < startbearing) { endbearing += 360; } if (arcdir < 0) { for (double a = startbearing; a > endbearing; a += (10 * arcdir)) { pointlist.Add(pointcent.newpos(a, radius)); } } else { for (double a = startbearing; a < endbearing; a += (10 * arcdir)) { pointlist.Add(pointcent.newpos(a, radius)); } } pointlist.Add(point); list.Add(pointlist); pointlist = new List <PointLatLng>(); isarcterminate = false; iscircleterminate = false; continue; } if (iscircleterminate) { iscircleterminate = false; continue; } pointlist.Add(point); continue; } else if (item.Groups[7].Value == "A") { pointcent = new PointLatLngAlt(double.Parse(item.Groups[10].Value, CultureInfo.InvariantCulture), double.Parse(item.Groups[12].Value, CultureInfo.InvariantCulture)); if (item.Groups[9].Value == "S") { pointcent.Lat *= -1; } if (item.Groups[11].Value == "W") { pointcent.Lng *= -1; } pointstart = new PointLatLngAlt(double.Parse(item.Groups[14].Value, CultureInfo.InvariantCulture), double.Parse(item.Groups[16].Value, CultureInfo.InvariantCulture)); if (item.Groups[13].Value == "S") { pointstart.Lat *= -1; } if (item.Groups[15].Value == "W") { pointstart.Lng *= -1; } arcdir = item.Groups[8].Value == "+" ? 1 : -1; isarcterminate = true; continue; } else if (item.Groups[17].Value == "C") { var point = new PointLatLngAlt(double.Parse(item.Groups[19].Value, CultureInfo.InvariantCulture), double.Parse(item.Groups[21].Value, CultureInfo.InvariantCulture)); if (item.Groups[18].Value == "S") { point.Lat *= -1; } if (item.Groups[20].Value == "W") { point.Lng *= -1; } // radius in m from nautical miles double radius = double.Parse(item.Groups[22].Value, CultureInfo.InvariantCulture) * 1852; for (int a = 0; a <= 360; a += 10) { pointlist.Add(point.newpos(a, radius)); } list.Add(pointlist); pointlist = new List <PointLatLng>(); iscircleterminate = true; continue; } } catch { } } return(list); }