public async Task SetTrack(PointLatLngAlt myposition, PointLatLngAlt target)
{
var bearing = myposition.GetBearing(target);
var distance = myposition.GetDistance(target);
// prevent div/0 and also point it flat
if (distance == 0)
{
distance = 1000;
}
var curve = getCurve(myposition.Alt, distance / 1000.0);
if (curve > 0)
{
Console.Write("Target below horizon");
}
var heightdelta = target.Alt - myposition.Alt;
// soh cah toa
// in degrees FOV
var fov = Math.Atan(40 / distance) * (180 / Math.PI);
// in zoom X
var z = (float)MathHelper.mapConstrained(fov, FOVMax, FOVMin, ZoomMin, ZoomMax);
await SetRPYAsync(0, Math.Atan(heightdelta / distance) *rad2deg, bearing, z);
}
List <PointLatLngAlt> getSRTMAltPath(List <PointLatLngAlt> list)
{
List <PointLatLngAlt> answer = new List <PointLatLngAlt>();
PointLatLngAlt last = null;
double disttotal = 0;
foreach (PointLatLngAlt loc in list)
{
if (loc == null)
{
continue;
}
if (last == null)
{
last = loc;
continue;
}
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;
for (int a = 0; a <= points; a++)
{
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;
// srtm alts
list3.Add(disttotal, newpoint.Alt / CurrentState.multiplierdist);
// terrain alt
list4terrain.Add(disttotal, (newpoint.Alt - homealt + loc.Alt) / CurrentState.multiplierdist);
lastpnt = newpoint;
}
answer.Add(new PointLatLngAlt(loc.Lat, loc.Lng, srtm.getAltitude(loc.Lat, loc.Lng).alt, ""));
last = loc;
}
return(answer);
}
public static List <PointLatLngAlt> GeneratePath(MAVState MAV)
{
List <PointLatLngAlt> result = new List <PointLatLngAlt>();
MAVLink.mavlink_mission_item_int_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_int_t?(new MAVLink.mavlink_mission_item_int_t()
{
x = wp.x, y = wp.y, z = 0
});
continue;
}
if (wp.command != (ushort)MAVLink.MAV_CMD.WAYPOINT && wp.command != (ushort)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);
}
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);
}
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;
}
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);
}
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);
}
public void SetTrack(PointLatLngAlt myposition, PointLatLngAlt target)
{
var bearing = myposition.GetBearing(target);
var distance = myposition.GetDistance(target);
var curve = getCurve(myposition.Alt, distance);
if (curve > 0)
{
Console.Write("Target below horizon");
}
var heightdelta = target.Alt - myposition.Alt;
SetRPYAsync(0, Math.Tan(heightdelta / distance) * rad2deg, bearing);
}
public int get_closest_pole()
{
double distance = Double.MaxValue;
int closest_pole = -1;
int index = 0;
foreach (PointLatLngAlt p in poles)
{
double d = copter_position.GetDistance(p);
if (d < distance)
{
distance = d;
closest_pole = index;
}
index++;
}
return(closest_pole);
}
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());
return(plla);
}
public override bool Loop()
{
// display to console
if (display.AddSeconds(5) < DateTime.Now)
{
Console.WriteLine(stats.ToString());
display = DateTime.Now;
}
// connects
if (connectedstate != Host.comPort.BaseStream.IsOpen)
{
if (Host.comPort.BaseStream.IsOpen == true)
{
stats.lastconnect = DateTime.Now;
stats.connects++;
connectedstate = true;
}
else
{
stats.lastdisconnect = DateTime.Now;
connectedstate = false;
}
}
// if we are not connected, dont do anything
if (!Host.comPort.BaseStream.IsOpen)
{
return(true);
}
// armed time
if (Host.cs.armed)
{
stats.armedtime++;
}
// distance traveled
if (Host.cs.armed && Host.cs.gpsstatus == 3 && (Host.cs.ch3percent > 12 || Host.cs.groundspeed > 3.0))
{
stats.timeInAir++;
if (lastpos != null && lastpos.Lat != 0 && lastpos.Lng != 0)
{
stats.distTraveledmeters += lastpos.GetDistance(new PointLatLngAlt(Host.cs.lat, Host.cs.lng, Host.cs.altasl, ""));
lastpos = new PointLatLngAlt(Host.cs.lat, Host.cs.lng, Host.cs.altasl, "");
}
else
{
lastpos = new PointLatLngAlt(Host.cs.lat, Host.cs.lng, Host.cs.altasl, "");
}
}
// altitude gained
if (Host.cs.armed && Host.cs.gpsstatus == 3)
{
stats.maxalt = Math.Max(Host.cs.altasl, stats.maxalt);
stats.minalt = Math.Min(Host.cs.altasl, stats.minalt);
stats.maxspeed = Math.Max(Host.cs.groundspeed, stats.maxspeed);
stats.avgspeed = Host.cs.groundspeed;
}
// gps lock time
if (Host.cs.gpsstatus == 3)
{
stats.gpslocktime++;
}
if (Host.cs.battery_usedmah > 0)
{
stats.mahused += Host.cs.battery_usedmah - lastmahused;
lastmahused = Host.cs.battery_usedmah;
}
else
{
lastmahused = 0;
}
// bytes received
//stats["bytesreceived"] += Host.comPort.BytesReceived.Buffer(TimeSpan.FromSeconds(1)).Select(bytes => bytes.Sum());
// bytes sent
//stats["bytessent"] += Host.comPort.BytesSent.Buffer(TimeSpan.FromSeconds(1)).Select(bytes => bytes.Sum());
// connect time
if (Host.comPort.BaseStream.IsOpen)
{
stats.connectedtime++;
}
return(true);
}
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.ToArray())
{
foreach (var mav in port.MAVlist)
{
if (mav == Leader)
{
continue;
}
PointLatLngAlt target = new PointLatLngAlt(masterpos);
try
{
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.cs.yaw;
double length = offsets[mav].length();
var x = ((HIL.Vector3)offsets[mav]).x;
var y = ((HIL.Vector3)offsets[mav]).y;
// add offsets to utm
p1[0] += x * Math.Cos(heading * MathHelper.deg2rad) - y * Math.Sin(heading * MathHelper.deg2rad);
p1[1] += x * Math.Sin(heading * MathHelper.deg2rad) + y * Math.Cos(heading * MathHelper.deg2rad);
if (mav.cs.firmware == MainV2.Firmwares.ArduPlane)
{
// project the point forwards gs*5
var gs = mav.cs.groundspeed * 5;
p1[1] += gs * Math.Cos((-heading) * MathHelper.deg2rad);
p1[0] += gs * Math.Sin((-heading) * MathHelper.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[mav]).z;
if (mav.cs.firmware == MainV2.Firmwares.ArduPlane)
{
var dist =
target.GetDistance(new PointLatLngAlt(mav.cs.lat, mav.cs.lng, mav.cs.alt));
dist -= mav.cs.groundspeed * 5;
var leadergs = Leader.cs.groundspeed;
var newspeed = (leadergs + (float)(dist / 10));
if (newspeed < 5)
{
newspeed = 5;
}
port.setParam(mav.sysid, mav.compid, "TRIM_ARSPD_CM", newspeed * 100.0f);
// send position
port.setGuidedModeWP(mav.sysid, mav.compid, new Locationwp()
{
alt = (float)target.Alt,
lat = target.Lat,
lng = target.Lng,
id = (ushort)MAVLink.MAV_CMD.WAYPOINT
});
}
else
{
Vector3 vel = new Vector3(Leader.cs.vx, Leader.cs.vy, Leader.cs.vz);
// do pos/vel
port.setPositionTargetGlobalInt(mav.sysid, mav.compid, true,
true, false, false,
MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT_INT, target.Lat, target.Lng, target.Alt, vel.x,
vel.y, -vel.z, 0, 0);
// do yaw
if (!gimbal)
{
// within 3 degrees dont send
if (Math.Abs(mav.cs.yaw - Leader.cs.yaw) > 3)
{
port.doCommand(mav.sysid, mav.compid, MAVLink.MAV_CMD.CONDITION_YAW, Leader.cs.yaw,
100.0f, 0, 0, 0, 0, 0, false);
}
}
else
{
// gimbal direction
if (Math.Abs(mav.cs.yaw - Leader.cs.yaw) > 3)
{
port.setMountControl(mav.sysid, mav.compid, 45, 0, Leader.cs.yaw, false);
}
}
}
//Console.WriteLine("{0} {1} {2} {3}", port.ToString(), target.Lat, target.Lng, target.Alt);
}
catch (Exception ex)
{
Console.WriteLine("Failed to send command " + mav.ToString() + "\n" + ex.ToString());
}
a++;
}
}
}
private void RegenerateWPRoute(List <PointLatLngAlt> fullpointlist, PointLatLngAlt HomeLocation)
{
route.Clear();
homeroute.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(HomeLocation);
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();
lastnonspline = fullpointlist[a];
}
wproute.Add(fullpointlist[a]);
lastpnt2 = lastpnt;
lastpnt = fullpointlist[a];
}
}
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;
}
overlay.Routes.Add(homeroute);
route.Stroke = new Pen(Color.Yellow, 4);
route.Stroke.DashStyle = DashStyle.Custom;
overlay.Routes.Add(route);
}
}
void processbg(string file)
{
a++;
Loading.ShowLoading(a + "/" + files.Count + " " + file, this);
if (!File.Exists(file + ".jpg"))
{
LogMap.MapLogs(new string[] { file });
}
var loginfo = new loginfo();
loginfo.fullname = file;
try
{
// file not found exception even though it passes the exists check above.
loginfo.Size = new FileInfo(file).Length;
}
catch
{
}
if (File.Exists(file + ".jpg"))
{
loginfo.imgfile = file + ".jpg";
}
if (file.ToLower().EndsWith(".tlog"))
{
using (MAVLinkInterface mine = new MAVLinkInterface())
{
try
{
mine.logplaybackfile =
new BinaryReader(File.Open(file, FileMode.Open, FileAccess.Read, FileShare.Read));
}
catch (Exception ex)
{
log.Debug(ex.ToString());
CustomMessageBox.Show("Log Can not be opened. Are you still connected?");
return;
}
mine.logreadmode = true;
mine.speechenabled = false;
// file is to small
if (mine.logplaybackfile.BaseStream.Length < 1024 * 4)
{
return;
}
mine.getHeartBeat();
loginfo.Date = mine.lastlogread;
loginfo.Aircraft = mine.sysidcurrent;
loginfo.Frame = mine.MAV.aptype.ToString();
var start = mine.lastlogread;
try
{
mine.logplaybackfile.BaseStream.Seek(0, SeekOrigin.Begin);
}
catch
{
}
var end = mine.lastlogread;
var length = mine.logplaybackfile.BaseStream.Length;
var a = 0;
// abandon last 100 bytes
while (mine.logplaybackfile.BaseStream.Position < (length - 100))
{
var packet = mine.readPacket();
// gcs
if (packet.sysid == 255)
{
continue;
}
if (packet.msgid == (uint)MAVLink.MAVLINK_MSG_ID.CAMERA_FEEDBACK)
{
loginfo.CamMSG++;
}
if (a % 10 == 0)
{
mine.MAV.cs.UpdateCurrentSettings(null, true, mine);
}
a++;
if (mine.lastlogread > end)
{
end = mine.lastlogread;
}
}
loginfo.Home = mine.MAV.cs.Location;
loginfo.TimeInAir = mine.MAV.cs.timeInAir;
loginfo.DistTraveled = mine.MAV.cs.distTraveled;
loginfo.Duration = (end - start).ToString();
}
}
else if (file.ToLower().EndsWith(".bin") || file.ToLower().EndsWith(".log"))
{
using (DFLogBuffer colbuf = new DFLogBuffer(File.OpenRead(file)))
{
PointLatLngAlt lastpos = null;
DateTime start = DateTime.MinValue;
DateTime end = DateTime.MinValue;
DateTime tia = DateTime.MinValue;
// set time in air/home/distancetraveled
foreach (var dfItem in colbuf.GetEnumeratorType("GPS"))
{
if (dfItem["Status"] != null)
{
var status = int.Parse(dfItem["Status"]);
if (status >= 3)
{
var pos = new PointLatLngAlt(
double.Parse(dfItem["Lat"], CultureInfo.InvariantCulture),
double.Parse(dfItem["Lng"], CultureInfo.InvariantCulture),
double.Parse(dfItem["Alt"], CultureInfo.InvariantCulture));
if (lastpos == null)
{
lastpos = pos;
}
if (start == DateTime.MinValue)
{
loginfo.Date = dfItem.time;
start = dfItem.time;
}
end = dfItem.time;
// add distance
loginfo.DistTraveled += (float)lastpos.GetDistance(pos);
// set home
if (loginfo.Home == null)
{
loginfo.Home = pos;
}
if (dfItem.time > tia.AddSeconds(1))
{
// ground speed > 0.2 or alt > homelat+2
if (double.Parse(dfItem["Spd"], CultureInfo.InvariantCulture) > 0.2 ||
pos.Alt > (loginfo.Home.Alt + 2))
{
loginfo.TimeInAir++;
}
tia = dfItem.time;
}
}
}
}
loginfo.Duration = (end - start).ToString();
loginfo.CamMSG = colbuf.GetEnumeratorType("CAM").Count();
loginfo.Aircraft = 0; //colbuf.dflog.param[""];
loginfo.Frame = "Unknown"; //mine.MAV.aptype.ToString();
}
}
lock (logs)
logs.Add(loginfo);
}
float testCode(MAVState MAV, PointLatLngAlt Location)
{
// this is GeoFenceDist from currentstate
try
{
float disttotal = 99999;
var R = 6371e3;
var currenthash = MAV.fencepoints.GetHashCode();
lock (this)
if (currenthash != listhash)
{
list = MAV.fencepoints
.Where(a => a.Value.command != (ushort)MAVLink.MAV_CMD.FENCE_RETURN_POINT)
.ChunkByField((a, b, count) =>
{
// these fields types stand alone
if (a.Value.command == (ushort)MAVLink.MAV_CMD.FENCE_CIRCLE_EXCLUSION ||
a.Value.command == (ushort)MAVLink.MAV_CMD.FENCE_CIRCLE_INCLUSION)
{
return(false);
}
if (count >= b.Value.param1)
{
return(false);
}
return(a.Value.command == b.Value.command);
}).ToList();
listhash = currenthash;
}
// check all sublists
foreach (var sublist in list)
{
// process circles
if (sublist.Count() == 1)
{
var item = sublist.First().Value;
if (item.command == (ushort)MAVLink.MAV_CMD.FENCE_CIRCLE_EXCLUSION)
{
var lla = new PointLatLngAlt(item.x / 1e7,
item.y / 1e7);
var dist = lla.GetDistance(Location);
if (dist < item.param1)
{
return(0);
}
disttotal = (float)Math.Min(dist - item.param1, disttotal);
}
else if (item.command == (ushort)MAVLink.MAV_CMD.FENCE_CIRCLE_INCLUSION)
{
var lla = new PointLatLngAlt(item.x / 1e7,
item.y / 1e7);
var dist = lla.GetDistance(Location);
if (dist > item.param1)
{
return(0);
}
disttotal = (float)Math.Min(item.param1 - dist, disttotal);
}
}
if (sublist == null || sublist.Count() < 3)
{
continue;
}
if (PolygonTools.isInside(
sublist.CloseLoop().Select(a => new PolygonTools.Point(a.Value.y / 1e7, a.Value.x / 1e7)).ToList(),
new PolygonTools.Point(Location.Lng, Location.Lat)))
{
if (sublist.First().Value.command == (ushort)MAVLink.MAV_CMD.FENCE_POLYGON_VERTEX_EXCLUSION)
{
return(0);
}
}
else
{
if (sublist.First().Value.command == (ushort)MAVLink.MAV_CMD.FENCE_POLYGON_VERTEX_INCLUSION)
{
return(0);
}
}
PointLatLngAlt lineStartLatLngAlt = null;
// check all segments
foreach (var mavlinkFencePointT in sublist.CloseLoop())
{
if (lineStartLatLngAlt == null)
{
lineStartLatLngAlt = new PointLatLngAlt(mavlinkFencePointT.Value.x / 1e7,
mavlinkFencePointT.Value.y / 1e7);
continue;
}
// crosstrack distance
var lineEndLatLngAlt = new PointLatLngAlt(mavlinkFencePointT.Value.x / 1e7,
mavlinkFencePointT.Value.y / 1e7);
var lineDist = lineStartLatLngAlt.GetDistance2(lineEndLatLngAlt);
var distToLocation = lineStartLatLngAlt.GetDistance2(Location);
var bearToLocation = lineStartLatLngAlt.GetBearing(Location);
var lineBear = lineStartLatLngAlt.GetBearing(lineEndLatLngAlt);
var angle = bearToLocation - lineBear;
if (angle < 0)
{
angle += 360;
}
var alongline = Math.Cos(angle * MathHelper.deg2rad) * distToLocation;
// check to see if our point is still within the line length
if (alongline < 0 || alongline > lineDist)
{
lineStartLatLngAlt = lineEndLatLngAlt;
continue;
}
var dXt2 = Math.Sin(angle * MathHelper.deg2rad) * distToLocation;
var dXt = Math.Asin(Math.Sin(distToLocation / R) * Math.Sin(angle * MathHelper.deg2rad)) * R;
disttotal = (float)Math.Min(disttotal, Math.Abs(dXt2));
lineStartLatLngAlt = lineEndLatLngAlt;
}
}
// check also distance from the points - because if we are outside the polygon, we may be on a corner segment
foreach (var sublist in list)
{
foreach (var mavlinkFencePointT in sublist)
{
if (mavlinkFencePointT.Value.command == (ushort)MAVLink.MAV_CMD.FENCE_CIRCLE_INCLUSION)
{
continue;
}
var pathpoint = new PointLatLngAlt(mavlinkFencePointT.Value.x / 1e7,
mavlinkFencePointT.Value.y / 1e7);
var dXt2 = pathpoint.GetDistance(Location);
disttotal = (float)Math.Min(disttotal, Math.Abs(dXt2));
}
}
return(disttotal);
}
catch
{
return(0);
}
}
void dostats(whattostat stats)
{
// connects
if (connectedstate != Host.comPort.BaseStream.IsOpen)
{
if (Host.comPort.BaseStream.IsOpen == true)
{
stats.lastconnect = DateTime.Now;
stats.connects++;
connectedstate = true;
}
else
{
stats.lastdisconnect = DateTime.Now;
connectedstate = false;
}
}
// if we are not connected, dont do anything
if (!Host.comPort.BaseStream.IsOpen)
{
return;
}
// armed time
if (Host.cs.armed)
{
stats.armedtime++;
}
// distance traveled
if (Host.cs.armed && Host.cs.gpsstatus >= 3 && (Host.cs.ch3percent > 12 || Host.cs.groundspeed > 3.0))
{
stats.timeInAir++;
if (lastpos != null && lastpos.Lat != 0 && lastpos.Lng != 0)
{
double dist = lastpos.GetDistance(new PointLatLngAlt(Host.cs.lat, Host.cs.lng, Host.cs.altasl, ""));
// max jump size is 400 m
if (dist < 400)
{
stats.distTraveledmeters += dist;
}
lastpos = new PointLatLngAlt(Host.cs.lat, Host.cs.lng, Host.cs.altasl, "");
}
else
{
lastpos = new PointLatLngAlt(Host.cs.lat, Host.cs.lng, Host.cs.altasl, "");
}
}
// altitude gained
if (Host.cs.armed && Host.cs.gpsstatus >= 3)
{
stats.maxalt = Math.Max(Host.cs.altasl, stats.maxalt);
stats.minalt = Math.Min(Host.cs.altasl, stats.minalt);
stats.maxspeed = Math.Max(Host.cs.groundspeed, stats.maxspeed);
stats.avgspeed = Host.cs.groundspeed;
}
// gps lock time
if (Host.cs.gpsstatus >= 3)
{
stats.gpslocktime++;
}
if (Host.cs.battery_usedmah > 0)
{
stats.mahused += Host.cs.battery_usedmah - lastmahused;
lastmahused = Host.cs.battery_usedmah;
}
else
{
lastmahused = 0;
}
// bytes received
//stats["bytesreceived"] += Host.comPort.BytesReceived.Buffer(TimeSpan.FromSeconds(1)).Select(bytes => bytes.Sum());
// bytes sent
//stats["bytessent"] += Host.comPort.BytesSent.Buffer(TimeSpan.FromSeconds(1)).Select(bytes => bytes.Sum());
// connect time
if (Host.comPort.BaseStream.IsOpen)
{
stats.connectedtime++;
}
return;
}
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);
// send position
port.setGuidedModeWP(new Locationwp()
{
alt = (float)target.Alt,
lat = target.Lat,
lng = target.Lng,
id = (ushort)MAVLink.MAV_CMD.WAYPOINT
});
}
else
{
Vector3 vel = new Vector3(Math.Cos(Leader.MAV.cs.groundcourse * deg2rad) * Leader.MAV.cs.groundspeed,
Math.Sin(Leader.MAV.cs.groundcourse * deg2rad) * Leader.MAV.cs.groundspeed, Leader.MAV.cs.verticalspeed);
port.setPositionTargetGlobalInt((byte)port.sysidcurrent, (byte)port.compidcurrent, true, true, false,
MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT_INT, target.Lat, target.Lng, target.Alt, vel.x, vel.y, -vel.z);
}
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++;
}
}
List <PointLatLngAlt> getSRTMAltPath(List <PointLatLngAlt> list)
{
List <PointLatLngAlt> answer = new List <PointLatLngAlt>();
PointLatLngAlt last = null;
double disttotal = 0;
foreach (PointLatLngAlt loc in list)
{
if (loc == null)
{
continue;
}
if (last == null)
{
last = loc;
if (altmode == FlightPlanner.altmode.Terrain || altmode2 == FlightPlanner2.altmode.Terrain)
{
loc.Alt -= srtm.getAltitude(loc.Lat, loc.Lng).alt;
}
continue;
}
double dist = last.GetDistance(loc);
if (altmode == FlightPlanner.altmode.Terrain || altmode2 == FlightPlanner2.altmode.Terrain)
{
loc.Alt -= srtm.getAltitude(loc.Lat, loc.Lng).alt;
}
int points = (int)(dist / 10) + 1;
double deltalat = (last.Lat - loc.Lat);
double deltalng = (last.Lng - loc.Lng);
double deltaalt = last.Alt - loc.Alt;
double steplat = deltalat / points;
double steplng = deltalng / points;
double stepalt = deltaalt / points;
PointLatLngAlt lastpnt = last;
for (int a = 0; a <= points; a++)
{
double lat = last.Lat - steplat * a;
double lng = last.Lng - steplng * a;
double alt = last.Alt - stepalt * a;
var newpoint = new PointLatLngAlt(lat, lng, srtm.getAltitude(lat, lng).alt, "");
double subdist = lastpnt.GetDistance(newpoint);
disttotal += subdist;
// srtm alts
list3.Add(disttotal * CurrentState.multiplierdist, Convert.ToInt32(newpoint.Alt * CurrentState.multiplieralt));
// terrain alt
list4terrain.Add(disttotal * CurrentState.multiplierdist, Convert.ToInt32((newpoint.Alt + alt) * CurrentState.multiplieralt));
lastpnt = newpoint;
}
answer.Add(new PointLatLngAlt(loc.Lat, loc.Lng, srtm.getAltitude(loc.Lat, loc.Lng).alt, ""));
last = loc;
}
return(answer);
}
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.ToArray())
{
foreach (var mav in port.MAVlist)
{
if (mav == Leader)
{
continue;
}
PointLatLngAlt target = new PointLatLngAlt(masterpos);
try
{
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.cs.yaw;
double length = offsets[mav].length();
var x = ((Vector3)offsets[mav]).x;
var y = ((Vector3)offsets[mav]).y;
// add offsets to utm
p1[0] += x * Math.Cos(heading * MathHelper.deg2rad) - y * Math.Sin(heading * MathHelper.deg2rad);
p1[1] += x * Math.Sin(heading * MathHelper.deg2rad) + y * Math.Cos(heading * MathHelper.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 += ((Vector3)offsets[mav]).z;
if (mav.cs.firmware == Firmwares.ArduPlane)
{
// get distance from target position
var dist = target.GetDistance(mav.cs.Location);
// get bearing to target
var targyaw = mav.cs.Location.GetBearing(target);
var targettrailer = target.newpos(Leader.cs.yaw, Math.Abs(dist) * -0.25);
var targetleader = target.newpos(Leader.cs.yaw, 10 + dist);
var yawerror = wrap_180(targyaw - mav.cs.yaw);
var mavleadererror = wrap_180(Leader.cs.yaw - mav.cs.yaw);
if (dist < 100)
{
targyaw = mav.cs.Location.GetBearing(targetleader);
yawerror = wrap_180(targyaw - mav.cs.yaw);
var targBearing = mav.cs.Location.GetBearing(target);
// check the bearing for the leader and target are within 45 degrees.
if (Math.Abs(wrap_180(targBearing - targyaw)) > 45)
{
dist *= -1;
}
}
else
{
targyaw = mav.cs.Location.GetBearing(targettrailer);
yawerror = wrap_180(targyaw - mav.cs.yaw);
}
// display update
mav.GuidedMode.x = (float)target.Lat;
mav.GuidedMode.y = (float)target.Lng;
mav.GuidedMode.z = (float)target.Alt;
MAVLink.mavlink_set_attitude_target_t att_target = new MAVLink.mavlink_set_attitude_target_t();
att_target.target_system = mav.sysid;
att_target.target_component = mav.compid;
att_target.type_mask = 0xff;
Tuple <PID, PID, PID, PID> pid;
if (pids.ContainsKey(mav))
{
pid = pids[mav];
}
else
{
pid = new Tuple <PID, PID, PID, PID>(
new PID(1f, .03f, 0.02f, 10, 20, 0.1f, 0),
new PID(1f, .03f, 0.02f, 10, 20, 0.1f, 0),
new PID(1, 0, 0.00f, 15, 20, 0.1f, 0),
new PID(0.01f, 0.001f, 0, 0.5f, 20, 0.1f, 0));
pids.Add(mav, pid);
}
var rollp = pid.Item1;
var pitchp = pid.Item2;
var yawp = pid.Item3;
var thrustp = pid.Item4;
var newroll = 0d;
var newpitch = 0d;
if (true)
{
var altdelta = target.Alt - mav.cs.alt;
newpitch = altdelta;
att_target.type_mask -= 0b00000010;
pitchp.set_input_filter_all((float)altdelta);
newpitch = pitchp.get_pid();
}
if (true)
{
var leaderturnrad = Leader.cs.radius;
var mavturnradius = leaderturnrad - x;
{
var distToTarget = mav.cs.Location.GetDistance(target);
var bearingToTarget = mav.cs.Location.GetBearing(target);
// bearing stability
if (distToTarget < 30)
{
bearingToTarget = mav.cs.Location.GetBearing(targetleader);
}
// fly in from behind
if (distToTarget > 100)
{
bearingToTarget = mav.cs.Location.GetBearing(targettrailer);
}
var bearingDelta = wrap_180(bearingToTarget - mav.cs.yaw);
var tangent90 = bearingDelta > 0 ? 90 : -90;
newroll = 0;
// if the delta is > 90 then we are facing the wrong direction
if (Math.Abs(bearingDelta) < 85)
{
var insideAngle = Math.Abs(tangent90 - bearingDelta);
var angleCenter = 180 - insideAngle * 2;
// sine rule
var sine1 = Math.Max(distToTarget, 40) /
Math.Sin(angleCenter * MathHelper.deg2rad);
var radius = sine1 * Math.Sin(insideAngle * MathHelper.deg2rad);
// average calced + leader offset turnradius - acts as a FF
radius = (Math.Abs(radius) + Math.Abs(mavturnradius)) / 2;
var angleBank = ((mav.cs.groundspeed * mav.cs.groundspeed) / radius) / 9.8;
angleBank *= MathHelper.rad2deg;
if (bearingDelta > 0)
{
newroll = Math.Abs(angleBank);
}
else
{
newroll = -Math.Abs(angleBank);
}
}
newroll += MathHelper.constrain(bearingDelta, -20, 20);
}
// tr = gs2 / (9.8 * x)
// (9.8 * x) * tr = gs2
// 9.8 * x = gs2 / tr
// (gs2/tr)/9.8 = x
var angle = ((mav.cs.groundspeed * mav.cs.groundspeed) / mavturnradius) / 9.8;
//newroll = angle * MathHelper.rad2deg;
// 1 degree of roll for ever 1 degree of yaw error
//newroll += MathHelper.constrain(yawerror, -20, 20);
//rollp.set_input_filter_all((float)yawdelta);
}
// do speed
if (true)
{
//att_target.thrust = (float) MathHelper.mapConstrained(dist, 0, 40, 0, 1);
att_target.type_mask -= 0b01000000;
// in m out 0-1
thrustp.set_input_filter_all((float)dist);
// prevent buildup prior to being close
if (dist > 40)
{
thrustp.reset_I();
}
// 0.1 demand + pid results
att_target.thrust = (float)MathHelper.constrain(thrustp.get_pid(), 0.1, 1);
}
Quaternion q = new Quaternion();
q.from_vector312(newroll * MathHelper.deg2rad, newpitch * MathHelper.deg2rad, yawerror * MathHelper.deg2rad);
att_target.q = new float[4];
att_target.q[0] = (float)q.q1;
att_target.q[1] = (float)q.q2;
att_target.q[2] = (float)q.q3;
att_target.q[3] = (float)q.q4;
//0b0= rpy
att_target.type_mask -= 0b10000101;
//att_target.type_mask -= 0b10000100;
Console.WriteLine("sysid {0} - {1} dist {2} r {3} p {4} y {5}", mav.sysid,
att_target.thrust, dist, newroll, newpitch, (targyaw - mav.cs.yaw));
/* Console.WriteLine("rpyt {0} {1} {2} {3} I {4} {5} {6} {7}",
* rollp.get_pid(), pitchp.get_pid(), yawp.get_pid(), thrustp.get_pid(),
* rollp.get_i(), pitchp.get_i(), yawp.get_i(), thrustp.get_i());
*/
port.sendPacket(att_target, mav.sysid, mav.compid);
}
else
{
Vector3 vel = new Vector3(Leader.cs.vx, Leader.cs.vy, Leader.cs.vz);
// do pos/vel
port.setPositionTargetGlobalInt(mav.sysid, mav.compid, true,
true, false, false,
MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT_INT, target.Lat, target.Lng, target.Alt, vel.x,
vel.y, vel.z, 0, 0);
// do yaw
if (!gimbal)
{
// within 3 degrees dont send
if (Math.Abs(mav.cs.yaw - Leader.cs.yaw) > 3)
{
port.doCommand(mav.sysid, mav.compid, MAVLink.MAV_CMD.CONDITION_YAW, Leader.cs.yaw,
100.0f, 0, 0, 0, 0, 0, false);
}
}
else
{
// gimbal direction
if (Math.Abs(mav.cs.yaw - Leader.cs.yaw) > 3)
{
port.setMountControl(mav.sysid, mav.compid, 45, 0, Leader.cs.yaw, false);
}
}
}
//Console.WriteLine("{0} {1} {2} {3}", port.ToString(), target.Lat, target.Lng, target.Alt);
}
catch (Exception ex)
{
Console.WriteLine("Failed to send command " + mav.ToString() + "\n" + ex.ToString());
}
a++;
}
}
}
float testCode(MAVState MAV, PointLatLngAlt Location)
{
// this is GeoFenceDist from currentstate
try
{
float disttotal = 99999;
var R = 6371e3;
var list = MAV.fencepoints
.Where(a => a.Value.command != (ushort)MAVLink.MAV_CMD.FENCE_RETURN_POINT)
.ChunkByField((a, b, count) =>
{
// these fields types stand alone
if (a.Value.command == (ushort)MAVLink.MAV_CMD.FENCE_CIRCLE_EXCLUSION ||
a.Value.command == (ushort)MAVLink.MAV_CMD.FENCE_CIRCLE_INCLUSION)
{
return(false);
}
if (count > b.Value.param1)
{
return(false);
}
return(a.Value.command == b.Value.command);
});
// check all sublists
foreach (var sublist in list)
{
PointLatLngAlt lineStartLatLngAlt = null;
// check all segments
foreach (var mavlinkFencePointT in sublist.CloseLoop())
{
if (lineStartLatLngAlt == null)
{
lineStartLatLngAlt = new PointLatLngAlt(mavlinkFencePointT.Value.x / 1e7,
mavlinkFencePointT.Value.y / 1e7);
continue;
}
// crosstrack distance
var lineEndLatLngAlt = new PointLatLngAlt(mavlinkFencePointT.Value.x / 1e7,
mavlinkFencePointT.Value.y / 1e7);
var lineDist = lineStartLatLngAlt.GetDistance2(lineEndLatLngAlt);
var distToLocation = lineStartLatLngAlt.GetDistance2(Location);
var bearToLocation = lineStartLatLngAlt.GetBearing(Location);
var lineBear = lineStartLatLngAlt.GetBearing(lineEndLatLngAlt);
var angle = bearToLocation - lineBear;
if (angle < 0)
{
angle += 360;
}
var alongline = Math.Cos(angle * MathHelper.deg2rad) * distToLocation;
// check to see if our point is still within the line length
if (alongline < 0 || alongline > lineDist)
{
lineStartLatLngAlt = lineEndLatLngAlt;
continue;
}
var dXt2 = Math.Sin(angle * MathHelper.deg2rad) * distToLocation;
var dXt = Math.Asin(Math.Sin(distToLocation / R) * Math.Sin(angle * MathHelper.deg2rad)) * R;
disttotal = (float)Math.Min(disttotal, Math.Abs(dXt2));
lineStartLatLngAlt = lineEndLatLngAlt;
}
}
// check also distance from the points - because if we are outside the polygon, we may be on a corner segment
foreach (var sublist in list)
{
foreach (var mavlinkFencePointT in sublist)
{
var pathpoint = new PointLatLngAlt(mavlinkFencePointT.Value.x / 1e7,
mavlinkFencePointT.Value.y / 1e7);
var dXt2 = pathpoint.GetDistance(Location);
disttotal = (float)Math.Min(disttotal, Math.Abs(dXt2));
}
}
return(disttotal);
}
catch
{
return(0);
}
}
public override bool Loop()
{
// connects
if (connectedstate != Host.comPort.BaseStream.IsOpen)
{
if (Host.comPort.BaseStream.IsOpen == true)
{
stats["connects"]++;
connectedstate = true;
}
}
// if we are not connected, dont do anything
if (!Host.comPort.BaseStream.IsOpen)
{
return(true);
}
// armed time
if (Host.cs.armed)
{
stats["armed"]++;
}
// distance traveled
if (Host.cs.armed && Host.cs.gpsstatus == 3)
{
if (lastpos.Lat != 0 && lastpos.Lng != 0 && Host.cs.armed)
{
stats["distTraveled"] += (ulong)lastpos.GetDistance(new PointLatLngAlt(Host.cs.lat, Host.cs.lng, Host.cs.altasl, ""));
lastpos = new PointLatLngAlt(Host.cs.lat, Host.cs.lng, Host.cs.altasl, "");
}
else
{
lastpos = new PointLatLngAlt(Host.cs.lat, Host.cs.lng, Host.cs.altasl, "");
}
}
// altitude gained
if (Host.cs.armed && Host.cs.gpsstatus == 3)
{
stats["maxalt"] = (ulong)Host.cs.altasl;
stats["minalt"] = (ulong)Host.cs.altasl;
}
// gps lock time
if (Host.cs.gpsstatus == 3)
{
stats["gpslock"]++;
}
// time in air
if (Host.cs.ch3percent > 12 || Host.cs.groundspeed > 3.0)
{
stats["timeInAir"]++;
}
// bytes received
//stats["bytesreceived"] += Host.comPort.BytesReceived.Buffer(TimeSpan.FromSeconds(1)).Select(bytes => bytes.Sum());
// bytes sent
//stats["bytessent"] += Host.comPort.BytesSent.Buffer(TimeSpan.FromSeconds(1)).Select(bytes => bytes.Sum());
// connect time
if (Host.comPort.BaseStream.IsOpen)
{
stats["connectedtime"]++;
}
return(true);
}
//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);
}
