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