public List<PointLatLngAlt> calcspline(PointLatLngAlt currentpos, PointLatLngAlt p1, PointLatLngAlt p2) { List<PointLatLngAlt> answer = new List<PointLatLngAlt>(); spline_t = spline_t_sq = 0.0f; spline_p0 = currentpos; spline_p1 = p1; spline_p2 = p2; spline_p0_prime = new Vector3(); double yaw = (currentpos.GetBearing(p1) + 360) % 360; spline_p0_prime.x = .000001 * Math.Sin(yaw * deg2rad); spline_p0_prime.y = .000001 * Math.Cos(yaw * deg2rad); spline_p0_prime.z = spline_p1.z - spline_p0.z; initialize_spline_segment(); int steps = 30; foreach (var step in range(steps +1)) { spline_t = (1f / steps) * step; spline_t_sq = spline_t * spline_t; evaluate_spline(); answer.Add(new PointLatLngAlt(spline_target.x, spline_target.y, spline_target.z, "")); } return answer; }
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; }
// Method checks if the landing direction not crosses the forbiden area near landing public MissionCheckerResult doCheckLandingDirection() { int last_wp = getLastMissionWP(); // check direction between last mission waypoint and landing(takeoff) point including forbidden area sector. if (last_wp > 0 && m_land_id >= 0) { PointLatLngAlt lp_coords = defined_mission[m_land_id].getCoords(); double lp_bearing = lp_coords.GetBearing(defined_mission[last_wp].getCoords()); //perform check! double unsafeAngleStart = defined_mission[m_land_id].P2; while (unsafeAngleStart > 360) { unsafeAngleStart -= 360; } double unsafeAngleEnd = unsafeAngleStart + defined_mission[m_land_id].P3; while (unsafeAngleEnd > 360) { unsafeAngleEnd -= 360; } double unsafeAngleOffset = defined_mission[m_land_id].P3; if (unsafeAngleStart > unsafeAngleEnd) { if (lp_bearing >= unsafeAngleStart && lp_bearing <= unsafeAngleStart + unsafeAngleOffset) { return(MissionCheckerResult.LANDING_CROSING_UNSAFE_AREA); } } else { if (lp_bearing >= unsafeAngleStart && lp_bearing <= unsafeAngleEnd) { return(MissionCheckerResult.LANDING_CROSING_UNSAFE_AREA); } } } return(MissionCheckerResult.OK); }
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); }
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 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); }
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 void UpdatePositions() { // add new point to trail trail.Add(new PointLatLngAlt(GroundMasterDrone.MavState.cs.lat, GroundMasterDrone.MavState.cs.lng, GroundMasterDrone.MavState.cs.alt,"")); while (trail.Count > 1000) trail.RemoveAt(0); // get current positions and velocitys foreach (var drone in Drones) { if (drone.Location == null) drone.Location = new PointLatLngAlt(); drone.Location.Lat = drone.MavState.cs.lat; drone.Location.Lng = drone.MavState.cs.lng; drone.Location.Alt = drone.MavState.cs.alt; if (drone.Velocity == null) drone.Velocity = new Vector3(); drone.Velocity.x = Math.Cos(drone.MavState.cs.groundcourse*deg2rad)*drone.MavState.cs.groundspeed; drone.Velocity.y = Math.Sin(drone.MavState.cs.groundcourse*deg2rad)*drone.MavState.cs.groundspeed; drone.Velocity.z = drone.MavState.cs.verticalspeed; drone.TargetVelocity = GroundMasterDrone.Velocity; } var targetbearing = GroundMasterDrone.Heading; // if (GroundMasterDrone.MavState.cs.wp_dist < Seperation*1.5) { var headingtowp = (int) GroundMasterDrone.MavState.cs.wpno; var nextwp = headingtowp + 1; try { PointLatLngAlt targetwp = new PointLatLngAlt(GroundMasterDrone.MavState.wps[headingtowp]); //PointLatLngAlt targetwp = GroundMasterDrone.Location; PointLatLngAlt nexttargetwp = new PointLatLngAlt(GroundMasterDrone.MavState.wps[nextwp]); var bearing = targetwp.GetBearing(nexttargetwp); // point on the wp line for target var targetpnt = targetwp.newpos(bearing, Seperation); targetbearing = GroundMasterDrone.Location.GetBearing(targetpnt); if (Math.Abs(targetbearing - bearing) > 20) { //targetbearing = bearing; } AirMasterDrone.TargetVelocity.x = Math.Cos(targetbearing*deg2rad)* GroundMasterDrone.MavState.cs.groundspeed; AirMasterDrone.TargetVelocity.y = Math.Sin(targetbearing*deg2rad)* GroundMasterDrone.MavState.cs.groundspeed; } catch { } } else { } // calc airmaster position AirMasterDrone.TargetLocation = GroundMasterDrone.Location.newpos(targetbearing, Seperation); AirMasterDrone.TargetLocation.Alt = Altitude; // send new position to airmaster AirMasterDrone.SendPositionVelocity(AirMasterDrone.TargetLocation, AirMasterDrone.TargetVelocity * 0.6); AirMasterDrone.MavState.GuidedMode.x = (float)AirMasterDrone.TargetLocation.Lat; AirMasterDrone.MavState.GuidedMode.y = (float)AirMasterDrone.TargetLocation.Lng; AirMasterDrone.MavState.GuidedMode.z = (float)AirMasterDrone.TargetLocation.Alt; // get the path List<PointLatLngAlt> newpositions = PlanMove(); List<PointLatLngAlt> newlist = new List<PointLatLngAlt>(); newlist.Add(GroundMasterDrone.Location); newlist.AddRange(newpositions); newpositions = newlist; int a = 0; // send position and velocity foreach (var drone in Drones) { if(drone.MavState == airmaster) continue; if (drone.MavState == groundmaster) continue; if (a > (newpositions.Count - 1)) break; newpositions[a].Alt += Altitude; // spline control drone.SendPositionVelocity(newpositions[a], drone.TargetVelocity/2); drone.MavState.GuidedMode.x = (float)newpositions[a].Lat; drone.MavState.GuidedMode.y = (float)newpositions[a].Lng; drone.MavState.GuidedMode.z = (float) newpositions[a].Alt; // vel only //drone.SendVelocity(drone.TargetVelocity); a++; } }
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); }
private PointLatLngAlt get_last_nav_coords(PointLatLngAlt lta_point, PointLatLngAlt land_point) { double bearing = land_point.GetBearing(lta_point); return(land_point.newpos(bearing, m_hwp_radius * 0.4f)); }
private PointLatLngAlt get_loiter_coords(PointLatLngAlt wp_point, PointLatLngAlt land_point) { double bearing = land_point.GetBearing(wp_point); return(land_point.newpos(bearing, m_hwp_radius)); }