public static PointLatLngAlt ProjectPoint()
{
PointLatLngAlt currentlocation = new PointLatLngAlt(MainV2.comPort.MAV.cs.lat, MainV2.comPort.MAV.cs.lng);
double yawangle = MainV2.comPort.MAV.cs.campointc;
double rollangle = MainV2.comPort.MAV.cs.campointb;
double pitchangle = MainV2.comPort.MAV.cs.campointa;
if ((float)MainV2.comPort.MAV.param["MNT_TYPE"] == 4)
{
yawangle = MainV2.comPort.MAVlist[71].cs.yaw;
rollangle = MainV2.comPort.MAVlist[71].cs.roll;
pitchangle = MainV2.comPort.MAVlist[71].cs.pitch;
}
int distout = 10;
PointLatLngAlt newpos = PointLatLngAlt.Zero;
//dist = Math.Tan((90 + pitchangle) * deg2rad) * (MainV2.comPort.MAV.cs.alt);
while (distout < 1000)
{
// get a projected point to test intersection against - not using slope distance
PointLatLngAlt newposdist = currentlocation.newpos(yawangle + MainV2.comPort.MAV.cs.yaw, distout);
newposdist.Alt = srtm.getAltitude(newposdist.Lat, newposdist.Lng).alt;
// get another point 50 infront
PointLatLngAlt newposdist2 = currentlocation.newpos(yawangle + MainV2.comPort.MAV.cs.yaw, distout + 50);
newposdist2.Alt = srtm.getAltitude(newposdist2.Lat, newposdist2.Lng).alt;
// get the flat terrain distance out - at 0 alt
double distflat = Math.Tan((90 + pitchangle) * deg2rad) * (MainV2.comPort.MAV.cs.altasl);
// x is dist from plane, y is alt
var newpoint = FindLineIntersection(new PointF(0, MainV2.comPort.MAV.cs.altasl), new PointF((float)distflat, 0),
new PointF((float)distout, (float)newposdist.Alt), new PointF((float)distout + 50, (float)newposdist2.Alt));
if (newpoint.X != 0)
{
newpos = newposdist2;
break;
}
distout += 50;
}
//Console.WriteLine("pitch " + pitchangle.ToString("0.000") + " yaw " + yawangle.ToString("0.000") + " dist" + dist.ToString("0.000"));
//PointLatLngAlt newpos = currentlocation.newpos( yawangle + MainV2.comPort.MAV.cs.yaw, dist);
//Console.WriteLine(newpos);
return(newpos);
}
public static PointLatLngAlt ProjectPoint()
{
PointLatLngAlt currentlocation = new PointLatLngAlt(MainV2.comPort.MAV.cs.lat, MainV2.comPort.MAV.cs.lng);
double yawangle = MainV2.comPort.MAV.cs.campointc;
double rollangle = MainV2.comPort.MAV.cs.campointb;
double pitchangle = MainV2.comPort.MAV.cs.campointa;
if ((float) MainV2.comPort.MAV.param["MNT_TYPE"] == 4)
{
yawangle = MainV2.comPort.MAVlist[71].cs.yaw;
rollangle = MainV2.comPort.MAVlist[71].cs.roll;
pitchangle = MainV2.comPort.MAVlist[71].cs.pitch;
}
int distout = 10;
PointLatLngAlt newpos = PointLatLngAlt.Zero;
//dist = Math.Tan((90 + pitchangle) * deg2rad) * (MainV2.comPort.MAV.cs.alt);
while (distout < 1000)
{
// get a projected point to test intersection against - not using slope distance
PointLatLngAlt newposdist = currentlocation.newpos(yawangle + MainV2.comPort.MAV.cs.yaw, distout);
newposdist.Alt = srtm.getAltitude(newposdist.Lat, newposdist.Lng).alt;
// get another point 50 infront
PointLatLngAlt newposdist2 = currentlocation.newpos(yawangle + MainV2.comPort.MAV.cs.yaw, distout + 50);
newposdist2.Alt = srtm.getAltitude(newposdist2.Lat, newposdist2.Lng).alt;
// get the flat terrain distance out - at 0 alt
double distflat = Math.Tan((90 + pitchangle) * deg2rad) * (MainV2.comPort.MAV.cs.altasl);
// x is dist from plane, y is alt
var newpoint = FindLineIntersection(new PointF(0,MainV2.comPort.MAV.cs.altasl), new PointF((float)distflat, 0),
new PointF((float)distout, (float)newposdist.Alt), new PointF((float)distout + 50, (float)newposdist2.Alt));
if (newpoint.X != 0)
{
newpos = newposdist2;
break;
}
distout += 50;
}
//Console.WriteLine("pitch " + pitchangle.ToString("0.000") + " yaw " + yawangle.ToString("0.000") + " dist" + dist.ToString("0.000"));
//PointLatLngAlt newpos = currentlocation.newpos( yawangle + MainV2.comPort.MAV.cs.yaw, dist);
//Console.WriteLine(newpos);
return newpos;
}
public static PointLatLngAlt ProjectPoint()
{
//MainV2.comPort.GetMountStatus();
PointLatLngAlt currentlocation = new PointLatLngAlt(MainV2.comPort.MAV.cs.lat, MainV2.comPort.MAV.cs.lng);
double yawangle = MainV2.comPort.MAV.cs.campointc * 0.01f;
double rollangle = MainV2.comPort.MAV.cs.campointb * 0.01f;
double pitchangle = MainV2.comPort.MAV.cs.campointa * 0.01f;
yawangle = ConvertPwmtoAngle(axis.yaw);
//rollangle = ConvertPwmtoAngle(axis.roll);
pitchangle = ConvertPwmtoAngle(axis.pitch) + MainV2.comPort.MAV.cs.pitch;
pitchangle -= Math.Sin(yawangle * deg2rad) * MainV2.comPort.MAV.cs.roll;
// tan (o/a)
double dist = Math.Tan((90 + pitchangle) * deg2rad) * (MainV2.comPort.MAV.cs.alt);
if (dist > 9999)
{
return(PointLatLngAlt.Zero);
}
Console.WriteLine("pitch " + pitchangle.ToString("0.000") + " yaw " + yawangle.ToString("0.000") + " dist" + dist.ToString("0.000"));
PointLatLngAlt newpos = currentlocation.newpos(yawangle + MainV2.comPort.MAV.cs.yaw, dist);
//Console.WriteLine(newpos);
return(newpos);
}
public static PointLatLngAlt Project(PointLatLngAlt location, Vector3 velocity, DateTime locationtime, DateTime now)
{
var deltatime = now - locationtime;
var bearing = Math.Atan2(velocity.y, velocity.x) * (180 / Math.PI);
var dist = velocity * deltatime.TotalSeconds;
var newpos = location.newpos(bearing, dist.length());
newpos.Alt += dist.z;
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 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> 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;
}
public static PointLatLngAlt ProjectPoint()
{
MainV2.comPort.GetMountStatus();
// this should be looking at rc_channel function
yawchannel = (int)(float)MainV2.comPort.MAV.param["MNT_RC_IN_PAN"];
pitchchannel = (int)(float)MainV2.comPort.MAV.param["MNT_RC_IN_TILT"];
rollchannel = (int)(float)MainV2.comPort.MAV.param["MNT_RC_IN_ROLL"];
if (!MainV2.comPort.BaseStream.IsOpen)
{
return(PointLatLngAlt.Zero);
}
PointLatLngAlt currentlocation = new PointLatLngAlt(MainV2.comPort.MAV.cs.lat, MainV2.comPort.MAV.cs.lng);
double yawangle = MainV2.comPort.MAV.cs.campointc;
double rollangle = MainV2.comPort.MAV.cs.campointb;
double pitchangle = MainV2.comPort.MAV.cs.campointa;
if (Math.Abs(rollangle) > 180 || yawangle == 0 && pitchangle == 0)
{
yawangle = ConvertPwmtoAngle(axis.yaw);
//rollangle = ConvertPwmtoAngle(axis.roll);
pitchangle = ConvertPwmtoAngle(axis.pitch) + MainV2.comPort.MAV.cs.pitch;
pitchangle -= Math.Sin(yawangle * deg2rad) * MainV2.comPort.MAV.cs.roll;
}
// tan (o/a)
// todo account for ground elevation change.
double dist = Math.Tan((90 + pitchangle) * deg2rad) * (MainV2.comPort.MAV.cs.alt);
if (dist > 9999)
{
return(PointLatLngAlt.Zero);
}
//Console.WriteLine("pitch " + pitchangle.ToString("0.000") + " yaw " + yawangle.ToString("0.000") + " dist" + dist.ToString("0.000"));
PointLatLngAlt newpos = currentlocation.newpos(yawangle + MainV2.comPort.MAV.cs.yaw, dist);
//Console.WriteLine(newpos);
return(newpos);
}
public static void createGrid(PointLatLngAlt centerPoint)
{
int startalt = 10;
int endalt = 20;
int seperation = 2;
int radius = 5;
int photos = 50;
int startheading = 0;
InputBox.Show("", "startalt", ref startalt);
InputBox.Show("", "endalt", ref endalt);
InputBox.Show("", "seperation", ref seperation);
InputBox.Show("", "radius", ref radius);
InputBox.Show("", "photos", ref photos);
InputBox.Show("", "start heading", ref startheading);
MainV2.instance.FlightPlanner.FlightPlannerBase.quickadd = true;
// set roi centerpoint
FlightPlannerBase.instance.AddCommand(MAVLink.MAV_CMD.DO_SET_ROI, 0, 0, 0, 0, centerPoint.Lng, centerPoint.Lat,
centerPoint.Alt);
// alts
for (int alt = startalt; alt <= endalt; alt += seperation)
{
// headings
for (int heading = startheading; heading <= startheading + 360; heading += 360 / photos)
{
MainV2.instance.FlightPlanner.FlightPlannerBase.quickadd = true;
var newpoint = centerPoint.newpos(heading, radius);
// add wp
FlightPlannerBase.instance.AddCommand(MAVLink.MAV_CMD.WAYPOINT, 2, 0, 0, 0, newpoint.Lng,
newpoint.Lat, alt);
// trigger camera
FlightPlannerBase.instance.AddCommand(MAVLink.MAV_CMD.DO_DIGICAM_CONTROL, 0, 0, 0, 0, 0, 1, 0);
}
}
MainV2.instance.FlightPlanner.FlightPlannerBase.quickadd = false;
MainV2.instance.FlightPlanner.FlightPlannerBase.writeKML();
}
//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);
}
//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;
}
//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 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++;
}
}
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));
}
//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 static PointLatLngAlt ProjectPoint()
{
MainV2.comPort.GetMountStatus();
// this should be looking at rc_channel function
yawchannel = (int)(float)MainV2.comPort.MAV.param["MNT_RC_IN_PAN"];
pitchchannel = (int)(float)MainV2.comPort.MAV.param["MNT_RC_IN_TILT"];
rollchannel = (int)(float)MainV2.comPort.MAV.param["MNT_RC_IN_ROLL"];
if (!MainV2.comPort.BaseStream.IsOpen)
return PointLatLngAlt.Zero;
PointLatLngAlt currentlocation = new PointLatLngAlt(MainV2.comPort.MAV.cs.lat, MainV2.comPort.MAV.cs.lng);
double yawangle = MainV2.comPort.MAV.cs.campointc;
double rollangle = MainV2.comPort.MAV.cs.campointb;
double pitchangle = MainV2.comPort.MAV.cs.campointa;
if (Math.Abs(rollangle) > 180 || yawangle == 0 && pitchangle == 0)
{
yawangle = ConvertPwmtoAngle(axis.yaw);
//rollangle = ConvertPwmtoAngle(axis.roll);
pitchangle = ConvertPwmtoAngle(axis.pitch) + MainV2.comPort.MAV.cs.pitch;
pitchangle -= Math.Sin(yawangle * deg2rad) * MainV2.comPort.MAV.cs.roll;
}
// tan (o/a)
// todo account for ground elevation change.
double dist = Math.Tan((90 +pitchangle)* deg2rad) * (MainV2.comPort.MAV.cs.alt);
if (dist > 9999)
return PointLatLngAlt.Zero;
//Console.WriteLine("pitch " + pitchangle.ToString("0.000") + " yaw " + yawangle.ToString("0.000") + " dist" + dist.ToString("0.000"));
PointLatLngAlt newpos = currentlocation.newpos( yawangle + MainV2.comPort.MAV.cs.yaw, dist);
//Console.WriteLine(newpos);
return newpos;
}
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));
}
public static PointLatLngAlt ProjectPoint()
{
MainV2.comPort.GetMountStatus();
// this should be looking at rc_channel function
yawchannel = (int)(float)MainV2.comPort.MAV.param["MNT_RC_IN_PAN"];
pitchchannel = (int)(float)MainV2.comPort.MAV.param["MNT_RC_IN_TILT"];
rollchannel = (int)(float)MainV2.comPort.MAV.param["MNT_RC_IN_ROLL"];
//if (!MainV2.comPort.BaseStream.IsOpen)
// return PointLatLngAlt.Zero;
PointLatLngAlt currentlocation = new PointLatLngAlt(MainV2.comPort.MAV.cs.lat, MainV2.comPort.MAV.cs.lng);
double yawangle = MainV2.comPort.MAV.cs.campointc;
double rollangle = MainV2.comPort.MAV.cs.campointb;
double pitchangle = MainV2.comPort.MAV.cs.campointa;
//
if ((float)MainV2.comPort.MAV.param["MNT_TYPE"] == 4)
{
yawangle = MainV2.comPort.MAVlist[71].cs.yaw;
rollangle = MainV2.comPort.MAVlist[71].cs.roll;
pitchangle = MainV2.comPort.MAVlist[71].cs.pitch;
}
if (Math.Abs(rollangle) > 180 || yawangle == 0 && pitchangle == 0)
{
yawangle = ConvertPwmtoAngle(axis.yaw);
//rollangle = ConvertPwmtoAngle(axis.roll);
pitchangle = ConvertPwmtoAngle(axis.pitch) + MainV2.comPort.MAV.cs.pitch;
pitchangle -= Math.Sin(yawangle * deg2rad) * MainV2.comPort.MAV.cs.roll;
}
// tan (o/a)
// todo account for ground elevation change.
int distout = 10;
PointLatLngAlt newpos = PointLatLngAlt.Zero;
//dist = Math.Tan((90 + pitchangle) * deg2rad) * (MainV2.comPort.MAV.cs.alt);
while (distout < 1000)
{
// get a projected point to test intersection against - not using slope distance
PointLatLngAlt newposdist = currentlocation.newpos(yawangle + MainV2.comPort.MAV.cs.yaw, distout);
newposdist.Alt = srtm.getAltitude(newposdist.Lat, newposdist.Lng).alt;
// get another point 50 infront
PointLatLngAlt newposdist2 = currentlocation.newpos(yawangle + MainV2.comPort.MAV.cs.yaw, distout + 50);
newposdist2.Alt = srtm.getAltitude(newposdist2.Lat, newposdist2.Lng).alt;
// get the flat terrain distance out - at 0 alt
double distflat = Math.Tan((90 + pitchangle) * deg2rad) * (MainV2.comPort.MAV.cs.altasl);
// x is dist from plane, y is alt
var newpoint = FindLineIntersection(new PointF(0, MainV2.comPort.MAV.cs.altasl), new PointF((float)distflat, 0),
new PointF((float)distout, (float)newposdist.Alt), new PointF((float)distout + 50, (float)newposdist2.Alt));
if (newpoint.X != 0)
{
newpos = newposdist2;
break;
}
distout += 50;
}
//Console.WriteLine("pitch " + pitchangle.ToString("0.000") + " yaw " + yawangle.ToString("0.000") + " dist" + dist.ToString("0.000"));
//PointLatLngAlt newpos = currentlocation.newpos( yawangle + MainV2.comPort.MAV.cs.yaw, dist);
//Console.WriteLine(newpos);
return(newpos);
}
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;
}
public static PointLatLngAlt ProjectPoint()
{
//MainV2.comPort.GetMountStatus();
int fixme;
if (!MainV2.comPort.BaseStream.IsOpen)
return PointLatLngAlt.Zero;
PointLatLngAlt currentlocation = new PointLatLngAlt(MainV2.comPort.MAV.cs.lat, MainV2.comPort.MAV.cs.lng);
double yawangle = MainV2.comPort.MAV.cs.campointc * 0.01f;
double rollangle = MainV2.comPort.MAV.cs.campointb * 0.01f;
double pitchangle = MainV2.comPort.MAV.cs.campointa * 0.01f;
yawangle = ConvertPwmtoAngle(axis.yaw);
//rollangle = ConvertPwmtoAngle(axis.roll);
pitchangle = ConvertPwmtoAngle(axis.pitch) + MainV2.comPort.MAV.cs.pitch;
pitchangle -= Math.Sin(yawangle * deg2rad) * MainV2.comPort.MAV.cs.roll;
// tan (o/a)
double dist = Math.Tan((90 +pitchangle)* deg2rad) * (MainV2.comPort.MAV.cs.alt);
if (dist > 9999)
return PointLatLngAlt.Zero;
//Console.WriteLine("pitch " + pitchangle.ToString("0.000") + " yaw " + yawangle.ToString("0.000") + " dist" + dist.ToString("0.000"));
PointLatLngAlt newpos = currentlocation.newpos( yawangle + MainV2.comPort.MAV.cs.yaw, dist);
//Console.WriteLine(newpos);
return newpos;
}
public static PointLatLngAlt ProjectPoint()
{
MainV2.comPort.GetMountStatus();
// this should be looking at rc_channel function
yawchannel = (int)(float)MainV2.comPort.MAV.param["MNT_RC_IN_PAN"];
pitchchannel = (int)(float)MainV2.comPort.MAV.param["MNT_RC_IN_TILT"];
rollchannel = (int)(float)MainV2.comPort.MAV.param["MNT_RC_IN_ROLL"];
//if (!MainV2.comPort.BaseStream.IsOpen)
// return PointLatLngAlt.Zero;
PointLatLngAlt currentlocation = new PointLatLngAlt(MainV2.comPort.MAV.cs.lat, MainV2.comPort.MAV.cs.lng);
double yawangle = MainV2.comPort.MAV.cs.campointc;
double rollangle = MainV2.comPort.MAV.cs.campointb;
double pitchangle = MainV2.comPort.MAV.cs.campointa;
//
if ((float) MainV2.comPort.MAV.param["MNT_TYPE"] == 4)
{
yawangle = MainV2.comPort.MAVlist[71].cs.yaw;
rollangle = MainV2.comPort.MAVlist[71].cs.roll;
pitchangle = MainV2.comPort.MAVlist[71].cs.pitch;
}
if (Math.Abs(rollangle) > 180 || yawangle == 0 && pitchangle == 0)
{
yawangle = ConvertPwmtoAngle(axis.yaw);
//rollangle = ConvertPwmtoAngle(axis.roll);
pitchangle = ConvertPwmtoAngle(axis.pitch) + MainV2.comPort.MAV.cs.pitch;
pitchangle -= Math.Sin(yawangle * deg2rad) * MainV2.comPort.MAV.cs.roll;
}
// tan (o/a)
// todo account for ground elevation change.
int distout = 10;
PointLatLngAlt newpos = PointLatLngAlt.Zero;
//dist = Math.Tan((90 + pitchangle) * deg2rad) * (MainV2.comPort.MAV.cs.alt);
while (distout < 1000)
{
// get a projected point to test intersection against - not using slope distance
PointLatLngAlt newposdist = currentlocation.newpos(yawangle + MainV2.comPort.MAV.cs.yaw, distout);
newposdist.Alt = srtm.getAltitude(newposdist.Lat, newposdist.Lng).alt;
// get another point 50 infront
PointLatLngAlt newposdist2 = currentlocation.newpos(yawangle + MainV2.comPort.MAV.cs.yaw, distout + 50);
newposdist2.Alt = srtm.getAltitude(newposdist2.Lat, newposdist2.Lng).alt;
// get the flat terrain distance out - at 0 alt
double distflat = Math.Tan((90 + pitchangle) * deg2rad) * (MainV2.comPort.MAV.cs.altasl);
// x is dist from plane, y is alt
var newpoint = FindLineIntersection(new PointF(0,MainV2.comPort.MAV.cs.altasl), new PointF((float)distflat, 0),
new PointF((float)distout, (float)newposdist.Alt), new PointF((float)distout + 50, (float)newposdist2.Alt));
if (newpoint.X != 0)
{
newpos = newposdist2;
break;
}
distout += 50;
}
//Console.WriteLine("pitch " + pitchangle.ToString("0.000") + " yaw " + yawangle.ToString("0.000") + " dist" + dist.ToString("0.000"));
//PointLatLngAlt newpos = currentlocation.newpos( yawangle + MainV2.comPort.MAV.cs.yaw, dist);
//Console.WriteLine(newpos);
return newpos;
}
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);
}
