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