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