public PointLatLngAlt(PointLatLngAlt plla)
{
this.Lat = plla.Lat;
this.Lng = plla.Lng;
this.Alt = plla.Alt;
this.color = plla.color;
this.Tag = plla.Tag;
}
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 - -183.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);
// add offsets to utm
p1[0] += ((HIL.Vector3)offsets[port]).x;
p1[1] += ((HIL.Vector3)offsets[port]).y;
// 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;
port.setGuidedModeWP(new Locationwp() { alt = (float)target.Alt, lat = target.Lat, lng = target.Lng, id = (byte)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 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 SendPositionVelocity(PointLatLngAlt pos, Vector3 vel)
{
MavState.parent.setPositionTargetGlobalInt(MavState.sysid, MavState.compid, true, true, false,
MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT_INT, pos.Lat, pos.Lng, pos.Alt, vel.x, vel.y, -vel.z);
}
public void CreateOverlay(MAVLink.MAV_FRAME altmode, PointLatLngAlt home, List <Locationwp> missionitems, double wpradius, double loiterradius)
{
overlay.Clear();
double maxlat = -180;
double maxlong = -180;
double minlat = 180;
double minlong = 180;
Func <double, double, double> gethomealt = (lat, lng) => GetHomeAlt(altmode, home.Alt, lat, lng);
home.Tag = "H";
home.Tag2 = altmode.ToString();
pointlist.Add(home);
fullpointlist.Add(pointlist[pointlist.Count - 1]);
addpolygonmarker("H", home.Lng, home.Lat, home.Alt, null, 0);
int a = 0;
foreach (var itemtuple in missionitems.PrevNowNext())
{
var itemprev = itemtuple.Item1;
var item = itemtuple.Item2;
var itemnext = itemtuple.Item3;
ushort command = item.id;
// invalid locationwp
if (command == 0)
{
continue;
}
if (command < (ushort)MAVLink.MAV_CMD.LAST &&
command != (ushort)MAVLink.MAV_CMD.TAKEOFF && // doesnt have a position
command != (ushort)MAVLink.MAV_CMD.VTOL_TAKEOFF && // doesnt have a position
command != (ushort)MAVLink.MAV_CMD.RETURN_TO_LAUNCH &&
command != (ushort)MAVLink.MAV_CMD.CONTINUE_AND_CHANGE_ALT &&
command != (ushort)MAVLink.MAV_CMD.DELAY &&
command != (ushort)MAVLink.MAV_CMD.GUIDED_ENABLE ||
command == (ushort)MAVLink.MAV_CMD.DO_SET_ROI)
{
// land can be 0,0 or a lat,lng
if (command == (ushort)MAVLink.MAV_CMD.LAND && item.lat == 0 && item.lng == 0)
{
continue;
}
if (command == (ushort)MAVLink.MAV_CMD.DO_SET_ROI)
{
pointlist.Add(new PointLatLngAlt(item.lat, item.lng,
item.alt + gethomealt(item.lat, item.lng), "ROI" + (a + 1))
{
color = Color.Red
});
// do set roi is not a nav command. so we dont route through it
//fullpointlist.Add(pointlist[pointlist.Count - 1]);
GMarkerGoogle m =
new GMarkerGoogle(new PointLatLng(item.lat, item.lng),
GMarkerGoogleType.red);
m.ToolTipMode = MarkerTooltipMode.Always;
m.ToolTipText = (a + 1).ToString();
m.Tag = (a + 1).ToString();
GMapMarkerRect mBorders = new GMapMarkerRect(m.Position);
{
mBorders.InnerMarker = m;
mBorders.Tag = "Dont draw line";
}
// check for clear roi, and hide it
if (m.Position.Lat != 0 && m.Position.Lng != 0)
{
// order matters
overlay.Markers.Add(m);
overlay.Markers.Add(mBorders);
}
}
else if (command == (ushort)MAVLink.MAV_CMD.LOITER_TIME ||
command == (ushort)MAVLink.MAV_CMD.LOITER_TURNS ||
command == (ushort)MAVLink.MAV_CMD.LOITER_UNLIM)
{
if (item.lat == 0 && item.lng == 0)
{
// loiter at current location.
if (fullpointlist.Count >= 1)
{
var lastpnt = fullpointlist[fullpointlist.Count - 1];
//addpolygonmarker((a + 1).ToString(), lastpnt.Lng, lastpnt.Lat,item.alt, Color.LightBlue, loiterradius);
}
}
else
{
pointlist.Add(new PointLatLngAlt(item.lat, item.lng,
item.alt + gethomealt(item.lat, item.lng), (a + 1).ToString())
{
color = Color.LightBlue
});
// exit at tangent
if (item.p4 == 1)
{
var from = pointlist.Last();
var to = itemnext.lat != 0 && itemnext.lng != 0
? new PointLatLngAlt(itemnext)
{
Alt = itemnext.alt + gethomealt(item.lat, item.lng)
}
: from;
var bearing = from.GetBearing(to);
var dist = from.GetDistance(to);
if (dist > loiterradius)
{
fullpointlist.Add(pointlist[pointlist.Count - 1]);
var offset = from.newpos(bearing + 90, loiterradius);
fullpointlist.Add(offset);
}
else
{
fullpointlist.Add(pointlist[pointlist.Count - 1]);
}
}
else
{
fullpointlist.Add(pointlist[pointlist.Count - 1]);
}
addpolygonmarker((a + 1).ToString(), item.lng, item.lat,
item.alt, Color.LightBlue, loiterradius);
}
}
else if (command == (ushort)MAVLink.MAV_CMD.SPLINE_WAYPOINT)
{
pointlist.Add(new PointLatLngAlt(item.lat, item.lng,
item.alt + gethomealt(item.lat, item.lng), (a + 1).ToString())
{
Tag2 = "spline"
});
fullpointlist.Add(pointlist[pointlist.Count - 1]);
addpolygonmarker((a + 1).ToString(), item.lng, item.lat,
item.alt, Color.Green, wpradius);
}
else
{
pointlist.Add(new PointLatLngAlt(item.lat, item.lng,
item.alt + gethomealt(item.lat, item.lng), (a + 1).ToString()));
fullpointlist.Add(pointlist[pointlist.Count - 1]);
addpolygonmarker((a + 1).ToString(), item.lng, item.lat,
item.alt, null, wpradius);
}
maxlong = Math.Max(item.lng, maxlong);
maxlat = Math.Max(item.lat, maxlat);
minlong = Math.Min(item.lng, minlong);
minlat = Math.Min(item.lat, minlat);
}
else if (command == (ushort)MAVLink.MAV_CMD.DO_JUMP) // fix do jumps into the future
{
pointlist.Add(null);
int wpno = (int)item.p1;
int repeat = (int)item.p2;
List <PointLatLngAlt> list = new List <PointLatLngAlt>();
// cycle through reps
for (int repno = repeat; repno > 0; repno--)
{
// cycle through wps
for (int no = wpno; no <= a; no++)
{
if (pointlist[no] != null)
{
list.Add(pointlist[no]);
}
}
}
fullpointlist.AddRange(list);
}
else
{
pointlist.Add(null);
}
a++;
}
RegenerateWPRoute(fullpointlist, home);
}
void mainloop()
{
DateTime nextsend = DateTime.Now;
DateTime nextrallypntupdate = DateTime.Now;
StreamWriter sw = new StreamWriter(File.OpenWrite("MovingBase.txt"));
threadrun = true;
while (threadrun)
{
try
{
string line = comPort.ReadLine();
sw.WriteLine(line);
//string line = string.Format("$GP{0},{1:HHmmss},{2},{3},{4},{5},{6},{7},{8},{9},{10},{11},{12},{13},", "GGA", DateTime.Now.ToUniversalTime(), Math.Abs(lat * 100), MainV2.comPort.MAV.cs.lat < 0 ? "S" : "N", Math.Abs(lng * 100), MainV2.comPort.MAV.cs.lng < 0 ? "W" : "E", MainV2.comPort.MAV.cs.gpsstatus, MainV2.comPort.MAV.cs.satcount, MainV2.comPort.MAV.cs.gpshdop, MainV2.comPort.MAV.cs.alt, "M", 0, "M", "");
if (line.StartsWith("$GPGGA") || line.StartsWith("$GNGGA")) //
{
string[] items = line.Trim().Split(',', '*');
if (items[items.Length - 1] != GetChecksum(line.Trim()))
{
Console.WriteLine("Bad Nmea line " + items[15] + " vs " + GetChecksum(line.Trim()));
continue;
}
if (items[6] == "0")
{
Console.WriteLine("No Fix");
continue;
}
gotolocation.Lat = double.Parse(items[2], CultureInfo.InvariantCulture) / 100.0;
gotolocation.Lat = (int)gotolocation.Lat + ((gotolocation.Lat - (int)gotolocation.Lat) / 0.60);
if (items[3] == "S")
{
gotolocation.Lat *= -1;
}
gotolocation.Lng = double.Parse(items[4], CultureInfo.InvariantCulture) / 100.0;
gotolocation.Lng = (int)gotolocation.Lng + ((gotolocation.Lng - (int)gotolocation.Lng) / 0.60);
if (items[5] == "W")
{
gotolocation.Lng *= -1;
}
gotolocation.Alt = double.Parse(items[9], CultureInfo.InvariantCulture);
gotolocation.Tag = "Sats " + items[7] + " hdop " + items[8];
}
if (DateTime.Now > nextsend && gotolocation.Lat != 0 && gotolocation.Lng != 0) // 200 * 10 = 2 sec /// lastgotolocation != gotolocation &&
{
nextsend = DateTime.Now.AddMilliseconds(1000 / updaterate);
Console.WriteLine("new home wp " + DateTime.Now.ToString("h:MM:ss") + " " + gotolocation.Lat +
" " + gotolocation.Lng + " " + gotolocation.Alt);
lastgotolocation = new PointLatLngAlt(gotolocation);
Locationwp gotohere = new Locationwp();
gotohere.id = (ushort)MAVLink.MAV_CMD.WAYPOINT;
gotohere.alt = (float)(gotolocation.Alt);
gotohere.lat = (gotolocation.Lat);
gotohere.lng = (gotolocation.Lng);
try
{
updateLocationLabel(gotohere);
}
catch
{
}
MainV2.comPort.MAV.cs.MovingBase = gotolocation;
// plane only
if (updaterallypnt && DateTime.Now > nextrallypntupdate)
{
nextrallypntupdate = DateTime.Now.AddSeconds(5);
try
{
MainV2.comPort.setParam("RALLY_TOTAL", 1);
MainV2.comPort.setRallyPoint(0,
new PointLatLngAlt(gotolocation)
{
Alt =
gotolocation.Alt + double.Parse(Settings.Instance["TXT_DefaultAlt"].ToString())
},
0, 0, 0, (byte)(float)MainV2.comPort.MAV.param["RALLY_TOTAL"]);
MainV2.comPort.setParam("RALLY_TOTAL", 1);
}
catch (Exception ex)
{
Console.WriteLine(ex.ToString());
}
}
}
}
catch
{
System.Threading.Thread.Sleep((int)(1000 / updaterate));
}
}
sw.Close();
}
public async Task StartSwarmSeperate(Firmwares firmware)
{
var max = 10;
if (InputBox.Show("how many?", "how many?", ref max) != DialogResult.OK)
{
return;
}
// kill old session
try
{
simulator.ForEach(a =>
{
try
{
a.Kill();
}
catch { }
});
}
catch
{
}
Task <string> exepath;
string model = "";
if (firmware == Firmwares.ArduPlane)
{
exepath = CheckandGetSITLImage("ArduPlane.elf");
model = "plane";
}
else
if (firmware == Firmwares.ArduRover)
{
exepath = CheckandGetSITLImage("ArduRover.elf");
model = "rover";
}
else // (firmware == Firmwares.ArduCopter2)
{
exepath = CheckandGetSITLImage("ArduCopter.elf");
model = "+";
}
var config = await GetDefaultConfig(model);
max--;
for (int a = (int)max; a >= 0; a--)
{
var extra = " --disable-fgview -r50 ";
if (!string.IsNullOrEmpty(config))
{
extra += @" --defaults """ + config + @",identity.parm"" -P SERIAL0_PROTOCOL=2 -P SERIAL1_PROTOCOL=2 ";
}
var home = new PointLatLngAlt(markeroverlay.Markers[0].Position).newpos((double)NUM_heading.Value, a * 4);
if (max == a)
{
extra += String.Format(
" -M{4} -s1 --home {3} --instance {0} --uartA tcp:0 {1} -P SYSID_THISMAV={2} ",
a, "", a + 1, BuildHomeLocation(home, (int)NUM_heading.Value), model);
}
else
{
extra += String.Format(
" -M{4} -s1 --home {3} --instance {0} --uartA tcp:0 {1} -P SYSID_THISMAV={2} ",
a, "" /*"--uartD tcpclient:127.0.0.1:" + (5770 + 10 * a)*/, a + 1,
BuildHomeLocation(home, (int)NUM_heading.Value), model);
}
string simdir = sitldirectory + model + (a + 1) + Path.DirectorySeparatorChar;
Directory.CreateDirectory(simdir);
File.WriteAllText(simdir + "identity.parm", String.Format(@"SERIAL0_PROTOCOL=2
SERIAL1_PROTOCOL=2
SYSID_THISMAV={0}
SIM_TERRAIN=0
TERRAIN_ENABLE=0
SCHED_LOOP_RATE=50
SIM_RATE_HZ=400
SIM_DRIFT_SPEED=0
SIM_DRIFT_TIME=0
", a + 1));
string path = Environment.GetEnvironmentVariable("PATH");
Environment.SetEnvironmentVariable("PATH", sitldirectory + ";" + simdir + ";" + path,
EnvironmentVariableTarget.Process);
Environment.SetEnvironmentVariable("HOME", simdir, EnvironmentVariableTarget.Process);
ProcessStartInfo exestart = new ProcessStartInfo();
exestart.FileName = await exepath;
exestart.Arguments = extra;
exestart.WorkingDirectory = simdir;
exestart.WindowStyle = ProcessWindowStyle.Minimized;
exestart.UseShellExecute = true;
simulator.Add(System.Diagnostics.Process.Start(exestart));
await Task.Delay(100);
}
await Task.Delay(2000);
MainV2.View.ShowScreen(MainV2.View.screens[0].Name);
try
{
Parallel.For(0, max + 1, (a) =>
//for (int a = (int)max; a >= 0; a--)
{
var mav = new MAVLinkInterface();
var client = new Comms.TcpSerial();
client.client = new TcpClient("127.0.0.1", 5760 + (10 * (a)));
mav.BaseStream = client;
//SITLSEND = new UdpClient("127.0.0.1", 5501);
Thread.Sleep(200);
this.InvokeIfRequired(() => { MainV2.instance.doConnect(mav, "preset", "5760", false); });
lock (this)
MainV2.Comports.Add(mav);
try
{
_ = mav.getParamListMavftpAsync((byte)mav.sysidcurrent, (byte)mav.compidcurrent);
}
catch
{
}
}
);
return;
}
catch (Exception ex)
{
log.Error(ex);
CustomMessageBox.Show(Strings.Failed_to_connect_to_SITL_instance +
ex.InnerException?.Message, Strings.ERROR);
return;
}
}
public bool setFencePoint(byte index, PointLatLngAlt plla, byte fencepointcount)
{
mavlink_fence_point_t fp = new mavlink_fence_point_t();
fp.idx = index;
fp.count = fencepointcount;
fp.lat = (float)plla.Lat;
fp.lng = (float)plla.Lng;
fp.target_component = compid;
fp.target_system = sysid;
int retry = 3;
while (retry > 0)
{
generatePacket(MAVLINK_MSG_ID_FENCE_POINT, fp);
int counttemp = 0;
PointLatLngAlt newfp = getFencePoint(fp.idx, ref counttemp);
if (newfp.Lat == plla.Lat && newfp.Lng == fp.lng)
return true;
retry--;
}
return false;
}
public double GetDistance2(PointLatLngAlt p2)
{
//http://www.movable-type.co.uk/scripts/latlong.html
var R = 6371.0; // 6371 km
var dLat = (p2.Lat - Lat) * deg2rad;
var dLon = (p2.Lng - Lng) * deg2rad;
var lat1 = Lat * deg2rad;
var lat2 = p2.Lat * deg2rad;
var a = Math.Sin(dLat / 2.0) * Math.Sin(dLat / 2.0) +
Math.Sin(dLon / 2.0) * Math.Sin(dLon / 2.0) * Math.Cos(lat1) * Math.Cos(lat2);
var c = 2.0 * Math.Atan2(Math.Sqrt(a), Math.Sqrt(1.0 - a));
var d = R * c * 1000.0; // M
return d;
}
private void processLine(string line)
{
try
{
Application.DoEvents();
line = line.Replace(", ", ",");
line = line.Replace(": ", ":");
string[] items = line.Split(',', ':');
if (items[0].Contains("CMD"))
{
if (flightdata.Count == 0)
{
if (int.Parse(items[2]) <= (int)MAVLink.MAV_CMD.LAST) // wps
{
PointLatLngAlt temp = new PointLatLngAlt(double.Parse(items[5], new System.Globalization.CultureInfo("en-US")) / 10000000, double.Parse(items[6], new System.Globalization.CultureInfo("en-US")) / 10000000, double.Parse(items[4], new System.Globalization.CultureInfo("en-US")) / 100, items[1].ToString());
cmd.Add(temp);
}
}
}
if (items[0].Contains("MOD"))
{
positionindex++;
modelist.Add(""); // i cant be bothered doing this properly
modelist.Add("");
modelist[positionindex] = (items[1]);
}
if (items[0].Contains("GPS") && items[2] == "1" && items[4] != "0" && items[4] != "-1" && lastline != line) // check gps line and fixed status
{
MainV2.comPort.MAV.cs.firmware = MainV2.Firmwares.ArduPlane;
if (position[positionindex] == null)
position[positionindex] = new List<Point3D>();
if (double.Parse(items[4], new System.Globalization.CultureInfo("en-US")) == 0)
return;
double alt = double.Parse(items[6], new System.Globalization.CultureInfo("en-US"));
if (items.Length == 11 && items[6] == "0.0000")
alt = double.Parse(items[7], new System.Globalization.CultureInfo("en-US"));
if (items.Length == 11 && items[6] == "0")
alt = double.Parse(items[7], new System.Globalization.CultureInfo("en-US"));
position[positionindex].Add(new Point3D(double.Parse(items[5], new System.Globalization.CultureInfo("en-US")), double.Parse(items[4], new System.Globalization.CultureInfo("en-US")), alt));
oldlastpos = lastpos;
lastpos = (position[positionindex][position[positionindex].Count - 1]);
lastline = line;
}
if (items[0].Contains("GPS") && items[4] != "0" && items[4] != "-1" && items.Length <= 9) // AC
{
MainV2.comPort.MAV.cs.firmware = MainV2.Firmwares.ArduCopter2;
if (position[positionindex] == null)
position[positionindex] = new List<Point3D>();
if (double.Parse(items[4], new System.Globalization.CultureInfo("en-US")) == 0)
return;
double alt = double.Parse(items[5], new System.Globalization.CultureInfo("en-US"));
position[positionindex].Add(new Point3D(double.Parse(items[4], new System.Globalization.CultureInfo("en-US")), double.Parse(items[3], new System.Globalization.CultureInfo("en-US")), alt));
oldlastpos = lastpos;
lastpos = (position[positionindex][position[positionindex].Count - 1]);
lastline = line;
}
if (items[0].Contains("CTUN"))
{
ctunlast = items;
}
if (items[0].Contains("NTUN"))
{
ntunlast = items;
line = "ATT:" + double.Parse(ctunlast[3], new System.Globalization.CultureInfo("en-US")) * 100 + "," + double.Parse(ctunlast[6], new System.Globalization.CultureInfo("en-US")) * 100 + "," + double.Parse(items[1], new System.Globalization.CultureInfo("en-US")) * 100;
items = line.Split(',', ':');
}
if (items[0].Contains("ATT"))
{
try
{
if (lastpos.X != 0 && oldlastpos.X != lastpos.X && oldlastpos.Y != lastpos.Y)
{
Data dat = new Data();
try
{
dat.datetime = int.Parse(lastline.Split(',', ':')[1]);
}
catch { }
runmodel = new Model();
runmodel.Location.longitude = lastpos.X;
runmodel.Location.latitude = lastpos.Y;
runmodel.Location.altitude = lastpos.Z;
oldlastpos = lastpos;
runmodel.Orientation.roll = double.Parse(items[1], new System.Globalization.CultureInfo("en-US")) / -100;
runmodel.Orientation.tilt = double.Parse(items[2], new System.Globalization.CultureInfo("en-US")) / -100;
runmodel.Orientation.heading = double.Parse(items[3], new System.Globalization.CultureInfo("en-US")) / 100;
dat.model = runmodel;
dat.ctun = ctunlast;
dat.ntun = ntunlast;
flightdata.Add(dat);
}
}
catch { }
}
}
catch (Exception)
{
// if items is to short or parse fails.. ignore
}
}
void mainloop()
{
DateTime nextsend = DateTime.Now;
StreamWriter sw = new StreamWriter(File.OpenWrite("followmeraw.txt"));
threadrun = true;
while (threadrun)
{
try
{
string line = comPort.ReadLine();
sw.WriteLine(line);
//string line = string.Format("$GP{0},{1:HHmmss},{2},{3},{4},{5},{6},{7},{8},{9},{10},{11},{12},{13},", "GGA", DateTime.Now.ToUniversalTime(), Math.Abs(lat * 100), MainV2.comPort.MAV.cs.lat < 0 ? "S" : "N", Math.Abs(lng * 100), MainV2.comPort.MAV.cs.lng < 0 ? "W" : "E", MainV2.comPort.MAV.cs.gpsstatus, MainV2.comPort.MAV.cs.satcount, MainV2.comPort.MAV.cs.gpshdop, MainV2.comPort.MAV.cs.alt, "M", 0, "M", "");
if (line.StartsWith("$GPGGA")) //
{
string[] items = line.Trim().Split(',', '*');
if (items[15] != GetChecksum(line.Trim()))
{
Console.WriteLine("Bad Nmea line " + items[15] + " vs " + GetChecksum(line.Trim()));
continue;
}
if (items[6] == "0")
{
Console.WriteLine("No Fix");
continue;
}
gotolocation.Lat = double.Parse(items[2], CultureInfo.InvariantCulture) / 100.0;
gotolocation.Lat = (int)gotolocation.Lat + ((gotolocation.Lat - (int)gotolocation.Lat) / 0.60);
if (items[3] == "S")
{
gotolocation.Lat *= -1;
}
gotolocation.Lng = double.Parse(items[4], CultureInfo.InvariantCulture) / 100.0;
gotolocation.Lng = (int)gotolocation.Lng + ((gotolocation.Lng - (int)gotolocation.Lng) / 0.60);
if (items[5] == "W")
{
gotolocation.Lng *= -1;
}
gotolocation.Alt = intalt; // double.Parse(line.Substring(c9, c10 - c9 - 1)) +
gotolocation.Tag = "Sats " + items[7] + " hdop " + items[8];
}
if (DateTime.Now > nextsend && gotolocation.Lat != 0 && gotolocation.Lng != 0 && gotolocation.Alt != 0) // 200 * 10 = 2 sec /// lastgotolocation != gotolocation &&
{
nextsend = DateTime.Now.AddMilliseconds(1000 / updaterate);
Console.WriteLine("Sending follow wp " + DateTime.Now.ToString("h:MM:ss") + " " + gotolocation.Lat + " " + gotolocation.Lng + " " + gotolocation.Alt);
lastgotolocation = new PointLatLngAlt(gotolocation);
Locationwp gotohere = new Locationwp();
gotohere.id = (byte)MAVLink.MAV_CMD.WAYPOINT;
gotohere.alt = (float)(gotolocation.Alt);
gotohere.lat = (gotolocation.Lat);
gotohere.lng = (gotolocation.Lng);
try
{
updateLocationLabel(gotohere);
}
catch { }
if (MainV2.comPort.BaseStream.IsOpen && MainV2.comPort.giveComport == false)
{
try
{
MainV2.comPort.giveComport = true;
MainV2.comPort.setGuidedModeWP(gotohere);
MainV2.comPort.giveComport = false;
}
catch { MainV2.comPort.giveComport = false; }
}
}
}
catch { System.Threading.Thread.Sleep((int)(1000 / updaterate)); }
}
sw.Close();
}
//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 override void Update()
{
if (MainV2.comPort.MAV.cs.lat == 0 || MainV2.comPort.MAV.cs.lng == 0)
return;
if (Leader == null)
Leader = MainV2.comPort;
masterpos = new PointLatLngAlt(MainV2.comPort.MAV.cs.lat, MainV2.comPort.MAV.cs.lng, MainV2.comPort.MAV.cs.alt, "");
}
void mainloop()
{
DateTime nextsend = DateTime.Now;
StreamWriter sw = new StreamWriter(File.OpenWrite("followmeraw.txt"));
threadrun = true;
while (threadrun)
{
try
{
string line = comPort.ReadLine();
sw.WriteLine(line);
//string line = string.Format("$GP{0},{1:HHmmss},{2},{3},{4},{5},{6},{7},{8},{9},{10},{11},{12},{13},", "GGA", DateTime.Now.ToUniversalTime(), Math.Abs(lat * 100), MainV2.comPort.MAV.cs.lat < 0 ? "S" : "N", Math.Abs(lng * 100), MainV2.comPort.MAV.cs.lng < 0 ? "W" : "E", MainV2.comPort.MAV.cs.gpsstatus, MainV2.comPort.MAV.cs.satcount, MainV2.comPort.MAV.cs.gpshdop, MainV2.comPort.MAV.cs.alt, "M", 0, "M", "");
if (line.StartsWith("$GPGGA")) //
{
string[] items = line.Trim().Split(',','*');
if (items[15] != GetChecksum(line.Trim()))
{
Console.WriteLine("Bad Nmea line " + items[15] + " vs " + GetChecksum(line.Trim()));
continue;
}
if (items[6] == "0")
{
Console.WriteLine("No Fix");
continue;
}
gotolocation.Lat = double.Parse(items[2], CultureInfo.InvariantCulture) / 100.0;
gotolocation.Lat = (int)gotolocation.Lat + ((gotolocation.Lat - (int)gotolocation.Lat) / 0.60);
if (items[3] == "S")
gotolocation.Lat *= -1;
gotolocation.Lng = double.Parse(items[4], CultureInfo.InvariantCulture) / 100.0;
gotolocation.Lng = (int)gotolocation.Lng + ((gotolocation.Lng - (int)gotolocation.Lng) / 0.60);
if (items[5] == "W")
gotolocation.Lng *= -1;
gotolocation.Alt = intalt; // double.Parse(line.Substring(c9, c10 - c9 - 1)) +
gotolocation.Tag = "Sats "+ items[7] + " hdop " + items[8] ;
}
if (DateTime.Now > nextsend && gotolocation.Lat != 0 && gotolocation.Lng != 0 && gotolocation.Alt != 0) // 200 * 10 = 2 sec /// lastgotolocation != gotolocation &&
{
nextsend = DateTime.Now.AddMilliseconds(1000/updaterate);
Console.WriteLine("Sending follow wp " +DateTime.Now.ToString("h:MM:ss")+" "+ gotolocation.Lat + " " + gotolocation.Lng + " " +gotolocation.Alt);
lastgotolocation = new PointLatLngAlt(gotolocation);
Locationwp gotohere = new Locationwp();
gotohere.id = (byte)MAVLink.MAV_CMD.WAYPOINT;
gotohere.alt = (float)(gotolocation.Alt);
gotohere.lat = (gotolocation.Lat);
gotohere.lng = (gotolocation.Lng);
try
{
updateLocationLabel(gotohere);
}
catch { }
if (MainV2.comPort.BaseStream.IsOpen && MainV2.comPort.giveComport == false)
{
try
{
MainV2.comPort.giveComport = true;
MainV2.comPort.setGuidedModeWP(gotohere);
MainV2.comPort.giveComport = false;
}
catch { MainV2.comPort.giveComport = false; }
}
}
}
catch { System.Threading.Thread.Sleep((int)(1000 / updaterate)); }
}
sw.Close();
}
public PointLatLngAlt getFencePoint(int no, ref int total)
{
byte[] buffer;
giveComport = true;
PointLatLngAlt plla = new PointLatLngAlt();
mavlink_fence_fetch_point_t req = new mavlink_fence_fetch_point_t();
req.idx = (byte)no;
req.target_component = compid;
req.target_system = sysid;
// request point
generatePacket(MAVLINK_MSG_ID_FENCE_FETCH_POINT, req);
DateTime start = DateTime.Now;
int retrys = 3;
while (true)
{
if (!(start.AddMilliseconds(500) > DateTime.Now))
{
if (retrys > 0)
{
log.Info("getFencePoint Retry " + retrys + " - giv com " + giveComport);
generatePacket(MAVLINK_MSG_ID_FENCE_FETCH_POINT, req);
start = DateTime.Now;
retrys--;
continue;
}
giveComport = false;
throw new Exception("Timeout on read - getFencePoint");
}
buffer = readPacket();
if (buffer.Length > 5)
{
if (buffer[5] == MAVLINK_MSG_ID_FENCE_POINT)
{
giveComport = false;
mavlink_fence_point_t fp = buffer.ByteArrayToStructure<mavlink_fence_point_t>(6);
plla.Lat = fp.lat;
plla.Lng = fp.lng;
plla.Tag = fp.idx.ToString();
total = fp.count;
return plla;
}
}
}
}
List <PointLatLngAlt> getGEAltPath(List <PointLatLngAlt> list)
{
double alt = 0;
double lat = 0;
double lng = 0;
int pos = 0;
List <PointLatLngAlt> answer = new List <PointLatLngAlt>();
//http://code.google.com/apis/maps/documentation/elevation/
//http://maps.google.com/maps/api/elevation/xml
string coords = "";
foreach (PointLatLngAlt loc in list)
{
if (loc == null)
{
continue;
}
coords = coords + loc.Lat.ToString(new System.Globalization.CultureInfo("en-US")) + "," +
loc.Lng.ToString(new System.Globalization.CultureInfo("en-US")) + "|";
}
coords = coords.Remove(coords.Length - 1);
if (list.Count < 2 || coords.Length > (2048 - 256))
{
CustomMessageBox.Show("Too many/few WP's or to Big a Distance " + (distance / 1000) + "km", Strings.ERROR);
return(answer);
}
try
{
using (
XmlTextReader xmlreader =
new XmlTextReader("https://maps.google.com/maps/api/elevation/xml?path=" + coords + "&samples=" +
(distance / 100).ToString(new System.Globalization.CultureInfo("en-US")) +
"&sensor=false&key=" + GoogleMapProvider.APIKey))
{
while (xmlreader.Read())
{
xmlreader.MoveToElement();
switch (xmlreader.Name)
{
case "elevation":
alt = double.Parse(xmlreader.ReadString(), new System.Globalization.CultureInfo("en-US"));
Console.WriteLine("DO it " + lat + " " + lng + " " + alt);
PointLatLngAlt loc = new PointLatLngAlt(lat, lng, alt, "");
answer.Add(loc);
pos++;
break;
case "lat":
lat = double.Parse(xmlreader.ReadString(), new System.Globalization.CultureInfo("en-US"));
break;
case "lng":
lng = double.Parse(xmlreader.ReadString(), new System.Globalization.CultureInfo("en-US"));
break;
default:
break;
}
}
}
}
catch
{
CustomMessageBox.Show("Error getting GE data", Strings.ERROR);
}
return(answer);
}
private void domainUpDown1_ValueChanged(object sender, EventArgs e)
{
if (CMB_camera.Text != "")
{
doCalc();
}
// new grid system test
List <PointLatLngAlt> list = new List <PointLatLngAlt>();
plugin.Host.FPDrawnPolygon.Points.ForEach(x => { list.Add(x); });
grid = Grid.CreateGrid(list, (double)NUM_altitude.Value, (double)NUM_Distance.Value, (double)NUM_spacing.Value, (double)NUM_angle.Value, (double)NUM_overshoot.Value, (double)NUM_overshoot2.Value, (Grid.StartPosition)Enum.Parse(typeof(Grid.StartPosition), CMB_startfrom.Text), false);
List <PointLatLng> list2 = new List <PointLatLng>();
grid.ForEach(x => { list2.Add(x); });
map.HoldInvalidation = true;
layerpolygons.Polygons.Clear();
layerpolygons.Markers.Clear();
if (grid.Count == 0)
{
return;
}
if (chk_boundary.Checked)
{
AddDrawPolygon();
}
int strips = 0;
int images = 0;
int a = 1;
PointLatLngAlt prevpoint = grid[0];
foreach (var item in grid)
{
if (item.Tag == "M")
{
images++;
if (chk_internals.Checked)
{
layerpolygons.Markers.Add(new GMarkerGoogle(item, GMarkerGoogleType.green)
{
ToolTipText = a.ToString(), ToolTipMode = MarkerTooltipMode.OnMouseOver
});
a++;
}
try
{
if (TXT_fovH.Text != "")
{
double fovh = double.Parse(TXT_fovH.Text);
double fovv = double.Parse(TXT_fovV.Text);
double startangle = 0;
if (!CHK_camdirection.Checked)
{
startangle = 90;
}
double angle1 = startangle - (Math.Tan((fovv / 2.0) / (fovh / 2.0)) * rad2deg);
double dist1 = Math.Sqrt(Math.Pow(fovh / 2.0, 2) + Math.Pow(fovv / 2.0, 2));
double bearing = (double)NUM_angle.Value;// (prevpoint.GetBearing(item) + 360.0) % 360;
List <PointLatLng> footprint = new List <PointLatLng>();
footprint.Add(item.newpos(bearing + angle1, dist1));
footprint.Add(item.newpos(bearing + 180 - angle1, dist1));
footprint.Add(item.newpos(bearing + 180 + angle1, dist1));
footprint.Add(item.newpos(bearing - angle1, dist1));
GMapPolygon poly = new GMapPolygon(footprint, a.ToString());
poly.Stroke.Color = Color.FromArgb(250 - ((a * 5) % 240), 250 - ((a * 3) % 240), 250 - ((a * 9) % 240));
poly.Stroke.Width = 1;
poly.Fill = new SolidBrush(Color.FromArgb(40, Color.Purple));
if (chk_footprints.Checked)
{
layerpolygons.Polygons.Add(poly);
}
}
}
catch { }
}
else
{
strips++;
if (chk_markers.Checked)
{
layerpolygons.Markers.Add(new GMarkerGoogle(item, GMarkerGoogleType.green)
{
ToolTipText = a.ToString(), ToolTipMode = MarkerTooltipMode.Always
});
}
a++;
}
prevpoint = item;
}
// add wp polygon
wppoly = new GMapPolygon(list2, "Grid");
wppoly.Stroke.Color = Color.Yellow;
wppoly.Fill = Brushes.Transparent;
wppoly.Stroke.Width = 4;
if (chk_grid.Checked)
{
layerpolygons.Polygons.Add(wppoly);
}
Console.WriteLine("Poly Dist " + wppoly.Distance);
lbl_area.Text = calcpolygonarea(plugin.Host.FPDrawnPolygon.Points).ToString("#") + " m^2";
lbl_distance.Text = wppoly.Distance.ToString("0.##") + " km";
lbl_spacing.Text = NUM_spacing.Value.ToString("#") + " m";
lbl_grndres.Text = TXT_cmpixel.Text;
lbl_pictures.Text = images.ToString();
lbl_strips.Text = ((int)(strips / 2)).ToString();
lbl_distbetweenlines.Text = NUM_Distance.Value.ToString("0.##") + " m";
lbl_footprint.Text = TXT_fovH.Text + " x " + TXT_fovV.Text + " m";
map.HoldInvalidation = false;
map.ZoomAndCenterMarkers("polygons");
}
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);
}
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);
// 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;
port.setGuidedModeWP(new Locationwp()
{
alt = (float)target.Alt,
lat = target.Lat,
lng = target.Lng,
id = (byte)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++;
}
}
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)
{
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;
}
/*
public void UpdateCurrentSettings(System.Windows.Forms.BindingSource bs, bool updatenow)
{
UpdateCurrentSettings(bs, false, MainV2.comPort);
}
*/
public void UpdateCurrentSettings(System.Windows.Forms.BindingSource bs, bool updatenow, MAVLink mavinterface)
{
lock (locker)
{
if (DateTime.Now > lastupdate.AddMilliseconds(50) || updatenow) // 20 hz
{
lastupdate = DateTime.Now;
//check if valid mavinterface
if (mavinterface != null && mavinterface.packetsnotlost != 0)
linkqualitygcs = (ushort)((mavinterface.packetsnotlost / (mavinterface.packetsnotlost + mavinterface.packetslost)) * 100.0);
if (DateTime.Now.Second != lastsecondcounter.Second)
{
lastsecondcounter = DateTime.Now;
if (lastpos.Lat != 0 && lastpos.Lng != 0)
{
if (!MainV2.comPort.BaseStream.IsOpen && !MainV2.comPort.logreadmode)
distTraveled = 0;
distTraveled += (float)lastpos.GetDistance(new PointLatLngAlt(lat, lng, 0, "")) * multiplierdist;
lastpos = new PointLatLngAlt(lat, lng, 0, "");
}
else
{
lastpos = new PointLatLngAlt(lat, lng, 0, "");
}
// throttle is up, or groundspeed is > 3 m/s
if (ch3percent > 12 || _groundspeed > 3.0)
timeInAir++;
if (!gotwind)
dowindcalc();
}
if (mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_STATUSTEXT] != null) // status text
{
var msg = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_STATUSTEXT].ByteArrayToStructure<MAVLink.mavlink_statustext_t>(6);
/*
enum gcs_severity {
SEVERITY_LOW=1,
SEVERITY_MEDIUM,
SEVERITY_HIGH,
SEVERITY_CRITICAL
};
*/
byte sev = msg.severity;
string logdata = Encoding.ASCII.GetString(mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_STATUSTEXT], 6, mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_STATUSTEXT].Length - 6);
int ind = logdata.IndexOf('\0');
if (ind != -1)
logdata = logdata.Substring(0, ind);
if (sev >= 3)
{
messageHigh = logdata;
}
try
{
while (messages.Count > 50)
{
messages.RemoveAt(0);
}
messages.Add(logdata + "\n");
}
catch { }
mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_STATUSTEXT] = null;
}
byte[] bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_SCALED];
if (bytearray != null) // hil mavlink 0.9
{
var hil = bytearray.ByteArrayToStructure<MAVLink.mavlink_rc_channels_scaled_t>(6);
hilch1 = hil.chan1_scaled;
hilch2 = hil.chan2_scaled;
hilch3 = hil.chan3_scaled;
hilch4 = hil.chan4_scaled;
hilch5 = hil.chan5_scaled;
hilch6 = hil.chan6_scaled;
hilch7 = hil.chan7_scaled;
hilch8 = hil.chan8_scaled;
// Console.WriteLine("MAVLINK_MSG_ID_RC_CHANNELS_SCALED Packet");
mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_SCALED] = null;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_FENCE_STATUS];
if (bytearray != null)
{
var fence = bytearray.ByteArrayToStructure<MAVLink.mavlink_fence_status_t>(6);
if (fence.breach_status != (byte)MAVLink.FENCE_BREACH.NONE)
{
// fence breached
messageHigh = "Fence Breach";
}
mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_FENCE_STATUS] = null;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_HIL_CONTROLS];
if (bytearray != null) // hil mavlink 0.9 and 1.0
{
var hil = bytearray.ByteArrayToStructure<MAVLink.mavlink_hil_controls_t>(6);
hilch1 = (int)(hil.roll_ailerons * 10000);
hilch2 = (int)(hil.pitch_elevator * 10000);
hilch3 = (int)(hil.throttle * 10000);
hilch4 = (int)(hil.yaw_rudder * 10000);
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_HIL_CONTROLS] = null;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_HWSTATUS];
if (bytearray != null)
{
var hwstatus = bytearray.ByteArrayToStructure<MAVLink.mavlink_hwstatus_t>(6);
hwvoltage = hwstatus.Vcc / 1000.0f;
i2cerrors = hwstatus.I2Cerr;
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_HWSTATUS] = null;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_RANGEFINDER];
if (bytearray != null)
{
var sonar = bytearray.ByteArrayToStructure<MAVLink.mavlink_rangefinder_t>(6);
sonarrange = sonar.distance;
sonarvoltage = sonar.voltage;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_WIND];
if (bytearray != null)
{
var wind = bytearray.ByteArrayToStructure<MAVLink.mavlink_wind_t>(6);
gotwind = true;
wind_dir = (wind.direction + 360) % 360;
wind_vel = wind.speed * multiplierspeed;
//MAVLink.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS] = null;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_HEARTBEAT];
if (bytearray != null)
{
var hb = bytearray.ByteArrayToStructure<MAVLink.mavlink_heartbeat_t>(6);
armed = (hb.base_mode & (byte)MAVLink.MAV_MODE_FLAG.SAFETY_ARMED) == (byte)MAVLink.MAV_MODE_FLAG.SAFETY_ARMED;
failsafe = hb.system_status == (byte)MAVLink.MAV_STATE.CRITICAL;
string oldmode = mode;
if ((hb.base_mode & (byte)MAVLink.MAV_MODE_FLAG.CUSTOM_MODE_ENABLED) != 0)
{
// prevent running thsi unless we have to
if (_mode != hb.custom_mode)
{
List<KeyValuePair<int, string>> modelist = Common.getModesList(this);
bool found = false;
foreach (KeyValuePair<int, string> pair in modelist)
{
if (pair.Key == hb.custom_mode)
{
mode = pair.Value.ToString();
_mode = hb.custom_mode;
found = true;
break;
}
}
if (!found)
{
log.Warn("Mode not found bm:" + hb.base_mode + " cm:" + hb.custom_mode);
_mode = hb.custom_mode;
}
}
}
if (oldmode != mode && MainV2.speechEnable && MainV2.getConfig("speechmodeenabled") == "True")
{
MainV2.speechEngine.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechmode")));
}
}
bytearray = mavinterface.MAV.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS];
if (bytearray != null)
{
var sysstatus = bytearray.ByteArrayToStructure<MAVLink.mavlink_sys_status_t>(6);
battery_voltage = (float)sysstatus.voltage_battery / 1000.0f;
battery_remaining = (float)sysstatus.battery_remaining;
current = (float)sysstatus.current_battery / 100.0f;
packetdropremote = sysstatus.drop_rate_comm;
mavinterface.MAV.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS] = null;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_SCALED_PRESSURE];
if (bytearray != null)
{
var pres = bytearray.ByteArrayToStructure<MAVLink.mavlink_scaled_pressure_t>(6);
press_abs = pres.press_abs;
press_temp = pres.temperature;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_SENSOR_OFFSETS];
if (bytearray != null)
{
var sensofs = bytearray.ByteArrayToStructure<MAVLink.mavlink_sensor_offsets_t>(6);
mag_ofs_x = sensofs.mag_ofs_x;
mag_ofs_y = sensofs.mag_ofs_y;
mag_ofs_z = sensofs.mag_ofs_z;
mag_declination = sensofs.mag_declination;
raw_press = sensofs.raw_press;
raw_temp = sensofs.raw_temp;
gyro_cal_x = sensofs.gyro_cal_x;
gyro_cal_y = sensofs.gyro_cal_y;
gyro_cal_z = sensofs.gyro_cal_z;
accel_cal_x = sensofs.accel_cal_x;
accel_cal_y = sensofs.accel_cal_y;
accel_cal_z = sensofs.accel_cal_z;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_ATTITUDE];
if (bytearray != null)
{
var att = bytearray.ByteArrayToStructure<MAVLink.mavlink_attitude_t>(6);
roll = att.roll * rad2deg;
pitch = att.pitch * rad2deg;
yaw = att.yaw * rad2deg;
// Console.WriteLine(roll + " " + pitch + " " + yaw);
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_ATTITUDE] = null;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW_INT];
if (bytearray != null)
{
var gps = bytearray.ByteArrayToStructure<MAVLink.mavlink_gps_raw_int_t>(6);
if (!useLocation)
{
lat = gps.lat * 1.0e-7f;
lng = gps.lon * 1.0e-7f;
// alt = gps.alt; // using vfr as includes baro calc
}
altasl = gps.alt / 1000.0f;
gpsstatus = gps.fix_type;
// Console.WriteLine("gpsfix {0}",gpsstatus);
gpshdop = (float)Math.Round((double)gps.eph / 100.0,2);
satcount = gps.satellites_visible;
groundspeed = gps.vel * 1.0e-2f;
groundcourse = gps.cog * 1.0e-2f;
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW] = null;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_GPS_STATUS];
if (bytearray != null)
{
var gps = bytearray.ByteArrayToStructure<MAVLink.mavlink_gps_status_t>(6);
satcount = gps.satellites_visible;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_RADIO];
if (bytearray != null)
{
var radio = bytearray.ByteArrayToStructure<MAVLink.mavlink_radio_t>(6);
rssi = radio.rssi;
remrssi = radio.remrssi;
txbuffer = radio.txbuf;
rxerrors = radio.rxerrors;
noise = radio.noise;
remnoise = radio.remnoise;
fixedp = radio.fixedp;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_RADIO_STATUS];
if (bytearray != null)
{
var radio = bytearray.ByteArrayToStructure<MAVLink.mavlink_radio_status_t>(6);
rssi = radio.rssi;
remrssi = radio.remrssi;
txbuffer = radio.txbuf;
rxerrors = radio.rxerrors;
noise = radio.noise;
remnoise = radio.remnoise;
fixedp = radio.fixedp;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_GLOBAL_POSITION_INT];
if (bytearray != null)
{
var loc = bytearray.ByteArrayToStructure<MAVLink.mavlink_global_position_int_t>(6);
// the new arhs deadreckoning may send 0 alt and 0 long. check for and undo
alt = loc.relative_alt / 1000.0f;
useLocation = true;
if (loc.lat == 0 && loc.lon == 0)
{
useLocation = false;
}
else
{
lat = loc.lat / 10000000.0f;
lng = loc.lon / 10000000.0f;
}
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_MISSION_CURRENT];
if (bytearray != null)
{
var wpcur = bytearray.ByteArrayToStructure<MAVLink.mavlink_mission_current_t>(6);
int oldwp = (int)wpno;
wpno = wpcur.seq;
if (oldwp != wpno && MainV2.speechEnable && MainV2.getConfig("speechwaypointenabled") == "True")
{
MainV2.speechEngine.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechwaypoint")));
}
//MAVLink.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_WAYPOINT_CURRENT] = null;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT];
if (bytearray != null)
{
var nav = bytearray.ByteArrayToStructure<MAVLink.mavlink_nav_controller_output_t>(6);
nav_roll = nav.nav_roll;
nav_pitch = nav.nav_pitch;
nav_bearing = nav.nav_bearing;
target_bearing = nav.target_bearing;
wp_dist = nav.wp_dist;
alt_error = nav.alt_error;
aspd_error = nav.aspd_error / 100.0f;
xtrack_error = nav.xtrack_error;
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT] = null;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_RAW];
if (bytearray != null)
{
var rcin = bytearray.ByteArrayToStructure<MAVLink.mavlink_rc_channels_raw_t>(6);
ch1in = rcin.chan1_raw;
ch2in = rcin.chan2_raw;
ch3in = rcin.chan3_raw;
ch4in = rcin.chan4_raw;
ch5in = rcin.chan5_raw;
ch6in = rcin.chan6_raw;
ch7in = rcin.chan7_raw;
ch8in = rcin.chan8_raw;
//percent
rxrssi = (float)((rcin.rssi / 255.0) * 100.0);
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_RAW] = null;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_SERVO_OUTPUT_RAW];
if (bytearray != null)
{
var servoout = bytearray.ByteArrayToStructure<MAVLink.mavlink_servo_output_raw_t>(6);
ch1out = servoout.servo1_raw;
ch2out = servoout.servo2_raw;
ch3out = servoout.servo3_raw;
ch4out = servoout.servo4_raw;
ch5out = servoout.servo5_raw;
ch6out = servoout.servo6_raw;
ch7out = servoout.servo7_raw;
ch8out = servoout.servo8_raw;
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_SERVO_OUTPUT_RAW] = null;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_RAW_IMU];
if (bytearray != null)
{
var imu = bytearray.ByteArrayToStructure<MAVLink.mavlink_raw_imu_t>(6);
gx = imu.xgyro;
gy = imu.ygyro;
gz = imu.zgyro;
ax = imu.xacc;
ay = imu.yacc;
az = imu.zacc;
mx = imu.xmag;
my = imu.ymag;
mz = imu.zmag;
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_RAW_IMU] = null;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_SCALED_IMU];
if (bytearray != null)
{
var imu = bytearray.ByteArrayToStructure<MAVLink.mavlink_scaled_imu_t>(6);
gx = imu.xgyro;
gy = imu.ygyro;
gz = imu.zgyro;
ax = imu.xacc;
ay = imu.yacc;
az = imu.zacc;
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_RAW_IMU] = null;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_VFR_HUD];
if (bytearray != null)
{
var vfr = bytearray.ByteArrayToStructure<MAVLink.mavlink_vfr_hud_t>(6);
//groundspeed = vfr.groundspeed;
airspeed = vfr.airspeed;
//alt = vfr.alt; // this might include baro
ch3percent = vfr.throttle;
//Console.WriteLine(alt);
//climbrate = vfr.climb;
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_VFR_HUD] = null;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_MEMINFO];
if (bytearray != null)
{
var mem = bytearray.ByteArrayToStructure<MAVLink.mavlink_meminfo_t>(6);
freemem = mem.freemem;
brklevel = mem.brkval;
}
}
//Console.Write(DateTime.Now.Millisecond + " start ");
// update form
try
{
if (bs != null)
{
//System.Diagnostics.Debug.WriteLine(DateTime.Now.Millisecond);
// Console.Write(" "+DateTime.Now.Millisecond);
bs.DataSource = this;
// Console.Write(" " + DateTime.Now.Millisecond + " 1 " + updatenow + " " + System.Threading.Thread.CurrentThread.Name);
bs.ResetBindings(false);
//bs.ResetCurrentItem();
// mono workaround - this is alot faster
//bs.Clear();
//bs.Add(this);
// Console.WriteLine(" " + DateTime.Now.Millisecond + " done ");
}
}
catch { log.InfoFormat("CurrentState Binding error"); }
}
}
void processbg(string file)
{
Loading.ShowLoading(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.img = new Bitmap(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 (CollectionBuffer colbuf = new CollectionBuffer(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();
}
}
logs.Add(loginfo);
}
public async void StartSwarmChain()
{
var max = 10;
if (InputBox.Show("how many?", "how many?", ref max) != DialogResult.OK)
{
return;
}
// kill old session
try
{
simulator.ForEach(a =>
{
try
{
a.Kill();
}
catch { }
});
}
catch
{
}
var exepath = CheckandGetSITLImage("ArduCopter.elf");
var model = "+";
var config = await GetDefaultConfig(model);
max--;
for (int a = (int)max; a >= 0; a--)
{
var extra = " --disable-fgview -r50";
if (!string.IsNullOrEmpty(config))
{
extra += @" --defaults """ + config + @",identity.parm"" -P SERIAL0_PROTOCOL=2 -P SERIAL1_PROTOCOL=2 ";
}
var home = new PointLatLngAlt(markeroverlay.Markers[0].Position).newpos((double)NUM_heading.Value, a * 4);
if (max == a)
{
extra += String.Format(
" -M{4} -s1 --home {3} --instance {0} --uartA tcp:0 {1} -P SYSID_THISMAV={2} ",
a, "", a + 1, BuildHomeLocation(home, (int)NUM_heading.Value), model);
}
else
{
extra += String.Format(
" -M{4} -s1 --home {3} --instance {0} --uartA tcp:0 {1} -P SYSID_THISMAV={2} ",
a, "--uartD tcpclient:127.0.0.1:" + (5772 + 10 * a), a + 1,
BuildHomeLocation(home, (int)NUM_heading.Value), model);
}
string simdir = sitldirectory + model + (a + 1) + Path.DirectorySeparatorChar;
Directory.CreateDirectory(simdir);
File.WriteAllText(simdir + "identity.parm", String.Format(@"SERIAL0_PROTOCOL=2
SERIAL1_PROTOCOL=2
SYSID_THISMAV={0}
SIM_TERRAIN=0
TERRAIN_ENABLE=0
SCHED_LOOP_RATE=50
SIM_RATE_HZ=400
SIM_DRIFT_SPEED=0
SIM_DRIFT_TIME=0
", a + 1));
string path = Environment.GetEnvironmentVariable("PATH");
Environment.SetEnvironmentVariable("PATH", sitldirectory + ";" + simdir + ";" + path,
EnvironmentVariableTarget.Process);
Environment.SetEnvironmentVariable("HOME", simdir, EnvironmentVariableTarget.Process);
ProcessStartInfo exestart = new ProcessStartInfo();
exestart.FileName = await exepath;
exestart.Arguments = extra;
exestart.WorkingDirectory = simdir;
exestart.WindowStyle = ProcessWindowStyle.Minimized;
exestart.UseShellExecute = true;
File.AppendAllText(Settings.GetUserDataDirectory() + "sitl.bat",
"mkdir " + (a + 1) + "\ncd " + (a + 1) + "\n" + @"""" + await exepath + @"""" + " " + extra + " &\n");
File.AppendAllText(Settings.GetUserDataDirectory() + "sitl1.sh",
"mkdir " + (a + 1) + "\ncd " + (a + 1) + "\n" + @"""../" +
Path.GetFileName(await exepath).Replace("C:", "/mnt/c").Replace("\\", "/").Replace(".exe", ".elf") + @"""" + " " +
extra.Replace("C:", "/mnt/c").Replace("\\", "/") + " &\nsleep .3\ncd ..\n");
simulator.Add(System.Diagnostics.Process.Start(exestart));
}
System.Threading.Thread.Sleep(2000);
MainV2.View.ShowScreen(MainV2.View.screens[0].Name);
try
{
var client = new Comms.TcpSerial();
client.client = new TcpClient("127.0.0.1", 5760);
MainV2.comPort.BaseStream = client;
SITLSEND = new UdpClient("127.0.0.1", 5501);
Thread.Sleep(200);
this.InvokeIfRequired(() => { MainV2.instance.doConnect(MainV2.comPort, "preset", "5760", false); });
try
{
_ = MainV2.comPort.getParamListMavftpAsync((byte)MainV2.comPort.sysidcurrent, (byte)MainV2.comPort.compidcurrent);
}
catch
{
}
return;
}
catch
{
CustomMessageBox.Show(Strings.Failed_to_connect_to_SITL_instance, Strings.ERROR);
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.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++;
}
}
}
public static List <PointLatLngAlt> CreateGrid(List <PointLatLngAlt> polygon, double altitude, double distance, double spacing, double angle, double overshoot1, double overshoot2, StartPosition startpos, bool shutter, float minLaneSeparation, float leadin = 0)
{
//DoDebug();
if (spacing < 4 && spacing != 0)
{
spacing = 4;
}
if (distance < 0.1)
{
distance = 0.1;
}
if (polygon.Count == 0)
{
return(new List <PointLatLngAlt>());
}
// Make a non round number in case of corner cases
if (minLaneSeparation != 0)
{
minLaneSeparation += 0.5F;
}
// Lane Separation in meters
double minLaneSeparationINMeters = minLaneSeparation * distance;
List <PointLatLngAlt> ans = new List <PointLatLngAlt>();
// utm zone distance calcs will be done in
int utmzone = polygon[0].GetUTMZone();
// utm position list
List <utmpos> utmpositions = utmpos.ToList(PointLatLngAlt.ToUTM(utmzone, polygon), utmzone);
// close the loop if its not already
if (utmpositions[0] != utmpositions[utmpositions.Count - 1])
{
utmpositions.Add(utmpositions[0]); // make a full loop
}
// get mins/maxs of coverage area
Rect area = getPolyMinMax(utmpositions);
// get initial grid
// used to determine the size of the outer grid area
double diagdist = area.DiagDistance();
// somewhere to store out generated lines
List <linelatlng> grid = new List <linelatlng>();
// number of lines we need
int lines = 0;
// get start point middle
double x = area.MidWidth;
double y = area.MidHeight;
addtomap(new utmpos(x, y, utmzone), "Base");
// get left extent
double xb1 = x;
double yb1 = y;
// to the left
newpos(ref xb1, ref yb1, angle - 90, diagdist / 2 + distance);
// backwards
newpos(ref xb1, ref yb1, angle + 180, diagdist / 2 + distance);
utmpos left = new utmpos(xb1, yb1, utmzone);
addtomap(left, "left");
// get right extent
double xb2 = x;
double yb2 = y;
// to the right
newpos(ref xb2, ref yb2, angle + 90, diagdist / 2 + distance);
// backwards
newpos(ref xb2, ref yb2, angle + 180, diagdist / 2 + distance);
utmpos right = new utmpos(xb2, yb2, utmzone);
addtomap(right, "right");
// set start point to left hand side
x = xb1;
y = yb1;
// draw the outergrid, this is a grid that cover the entire area of the rectangle plus more.
while (lines < ((diagdist + distance * 2) / distance))
{
// copy the start point to generate the end point
double nx = x;
double ny = y;
newpos(ref nx, ref ny, angle, diagdist + distance * 2);
linelatlng line = new linelatlng();
line.p1 = new utmpos(x, y, utmzone);
line.p2 = new utmpos(nx, ny, utmzone);
line.basepnt = new utmpos(x, y, utmzone);
grid.Add(line);
// addtomap(line);
newpos(ref x, ref y, angle + 90, distance);
lines++;
}
// find intersections with our polygon
// store lines that dont have any intersections
List <linelatlng> remove = new List <linelatlng>();
int gridno = grid.Count;
// cycle through our grid
for (int a = 0; a < gridno; a++)
{
double closestdistance = double.MaxValue;
double farestdistance = double.MinValue;
utmpos closestpoint = utmpos.Zero;
utmpos farestpoint = utmpos.Zero;
// somewhere to store our intersections
List <utmpos> matchs = new List <utmpos>();
int b = -1;
int crosses = 0;
utmpos newutmpos = utmpos.Zero;
foreach (utmpos pnt in utmpositions)
{
b++;
if (b == 0)
{
continue;
}
newutmpos = FindLineIntersection(utmpositions[b - 1], utmpositions[b], grid[a].p1, grid[a].p2);
if (!newutmpos.IsZero)
{
crosses++;
matchs.Add(newutmpos);
if (closestdistance > grid[a].p1.GetDistance(newutmpos))
{
closestpoint.y = newutmpos.y;
closestpoint.x = newutmpos.x;
closestpoint.zone = newutmpos.zone;
closestdistance = grid[a].p1.GetDistance(newutmpos);
}
if (farestdistance < grid[a].p1.GetDistance(newutmpos))
{
farestpoint.y = newutmpos.y;
farestpoint.x = newutmpos.x;
farestpoint.zone = newutmpos.zone;
farestdistance = grid[a].p1.GetDistance(newutmpos);
}
}
}
if (crosses == 0) // outside our polygon
{
if (!PointInPolygon(grid[a].p1, utmpositions) && !PointInPolygon(grid[a].p2, utmpositions))
{
remove.Add(grid[a]);
}
}
else if (crosses == 1) // bad - shouldnt happen
{
}
else if (crosses == 2) // simple start and finish
{
linelatlng line = grid[a];
line.p1 = closestpoint;
line.p2 = farestpoint;
grid[a] = line;
}
else // multiple intersections
{
linelatlng line = grid[a];
remove.Add(line);
while (matchs.Count > 1)
{
linelatlng newline = new linelatlng();
closestpoint = findClosestPoint(closestpoint, matchs);
newline.p1 = closestpoint;
matchs.Remove(closestpoint);
closestpoint = findClosestPoint(closestpoint, matchs);
newline.p2 = closestpoint;
matchs.Remove(closestpoint);
newline.basepnt = line.basepnt;
grid.Add(newline);
}
}
}
// cleanup and keep only lines that pass though our polygon
foreach (linelatlng line in remove)
{
grid.Remove(line);
}
// debug
foreach (linelatlng line in grid)
{
addtomap(line);
}
if (grid.Count == 0)
{
return(ans);
}
// pick start positon based on initial point rectangle
utmpos startposutm;
switch (startpos)
{
default:
case StartPosition.Home:
startposutm = new utmpos(MainV2.comPort.MAV.cs.HomeLocation);
break;
case StartPosition.BottomLeft:
startposutm = new utmpos(area.Left, area.Bottom, utmzone);
break;
case StartPosition.BottomRight:
startposutm = new utmpos(area.Right, area.Bottom, utmzone);
break;
case StartPosition.TopLeft:
startposutm = new utmpos(area.Left, area.Top, utmzone);
break;
case StartPosition.TopRight:
startposutm = new utmpos(area.Right, area.Top, utmzone);
break;
case StartPosition.Point:
startposutm = new utmpos(StartPointLatLngAlt);
break;
}
// find the closes polygon point based from our startpos selection
startposutm = findClosestPoint(startposutm, utmpositions);
// find closest line point to startpos
linelatlng closest = findClosestLine(startposutm, grid, 0 /*Lane separation does not apply to starting point*/, angle);
utmpos lastpnt;
// get the closes point from the line we picked
if (closest.p1.GetDistance(startposutm) < closest.p2.GetDistance(startposutm))
{
lastpnt = closest.p1;
}
else
{
lastpnt = closest.p2;
}
// S = start
// E = end
// ME = middle end
// SM = start middle
while (grid.Count > 0)
{
// for each line, check which end of the line is the next closest
if (closest.p1.GetDistance(lastpnt) < closest.p2.GetDistance(lastpnt))
{
utmpos newstart = newpos(closest.p1, angle, -leadin);
newstart.Tag = "S";
addtomap(newstart, "S");
ans.Add(newstart);
closest.p1.Tag = "SM";
addtomap(closest.p1, "SM");
ans.Add(closest.p1);
if (spacing > 0)
{
for (int d = (int)(spacing - ((closest.basepnt.GetDistance(closest.p1)) % spacing));
d < (closest.p1.GetDistance(closest.p2));
d += (int)spacing)
{
double ax = closest.p1.x;
double ay = closest.p1.y;
newpos(ref ax, ref ay, angle, d);
var utmpos1 = new utmpos(ax, ay, utmzone)
{
Tag = "M"
};
addtomap(utmpos1, "M");
ans.Add(utmpos1);
}
}
closest.p2.Tag = "ME";
addtomap(closest.p2, "ME");
ans.Add(closest.p2);
utmpos newend = newpos(closest.p2, angle, overshoot1);
newend.Tag = "E";
addtomap(newend, "E");
ans.Add(newend);
lastpnt = closest.p2;
grid.Remove(closest);
if (grid.Count == 0)
{
break;
}
closest = findClosestLine(newend, grid, minLaneSeparationINMeters, angle);
}
else
{
utmpos newstart = newpos(closest.p2, angle, leadin);
newstart.Tag = "S";
addtomap(newstart, "S");
ans.Add(newstart);
closest.p2.Tag = "SM";
addtomap(closest.p2, "SM");
ans.Add(closest.p2);
if (spacing > 0)
{
for (int d = (int)((closest.basepnt.GetDistance(closest.p2)) % spacing);
d < (closest.p1.GetDistance(closest.p2));
d += (int)spacing)
{
double ax = closest.p2.x;
double ay = closest.p2.y;
newpos(ref ax, ref ay, angle, -d);
var utmpos2 = new utmpos(ax, ay, utmzone)
{
Tag = "M"
};
addtomap(utmpos2, "M");
ans.Add(utmpos2);
}
}
closest.p1.Tag = "ME";
addtomap(closest.p1, "ME");
ans.Add(closest.p1);
utmpos newend = newpos(closest.p1, angle, -overshoot2);
newend.Tag = "E";
addtomap(newend, "E");
ans.Add(newend);
lastpnt = closest.p1;
grid.Remove(closest);
if (grid.Count == 0)
{
break;
}
closest = findClosestLine(newend, grid, minLaneSeparationINMeters, angle);
}
}
// update zoom after 2 seconds
timer.Start();
// set the altitude on all points
ans.ForEach(plla => { plla.Alt = altitude; });
return(ans);
}
List<PointLatLngAlt> getGEAltPath(List<PointLatLngAlt> list)
{
double alt = 0;
double lat = 0;
double lng = 0;
int pos = 0;
List<PointLatLngAlt> answer = new List<PointLatLngAlt>();
//http://code.google.com/apis/maps/documentation/elevation/
//http://maps.google.com/maps/api/elevation/xml
string coords = "";
foreach (PointLatLngAlt loc in list)
{
coords = coords + loc.Lat.ToString(new System.Globalization.CultureInfo("en-US")) + "," + loc.Lng.ToString(new System.Globalization.CultureInfo("en-US")) + "|";
}
coords = coords.Remove(coords.Length - 1);
if (list.Count <= 2 || coords.Length > (2048 - 256) || distance > 50000)
{
CustomMessageBox.Show("Too many/few WP's or to Big a Distance " + (distance / 1000) + "km", "Error");
return answer;
}
try
{
using (XmlTextReader xmlreader = new XmlTextReader("http://maps.google.com/maps/api/elevation/xml?path=" + coords + "&samples=" + (distance / 100).ToString(new System.Globalization.CultureInfo("en-US")) + "&sensor=false"))
{
while (xmlreader.Read())
{
xmlreader.MoveToElement();
switch (xmlreader.Name)
{
case "elevation":
alt = double.Parse(xmlreader.ReadString(), new System.Globalization.CultureInfo("en-US"));
Console.WriteLine("DO it " + lat + " " + lng + " " + alt);
PointLatLngAlt loc = new PointLatLngAlt(lat,lng,alt,"");
answer.Add(loc);
pos++;
break;
case "lat":
lat = double.Parse(xmlreader.ReadString(), new System.Globalization.CultureInfo("en-US"));
break;
case "lng":
lng = double.Parse(xmlreader.ReadString(), new System.Globalization.CultureInfo("en-US"));
break;
default:
break;
}
}
}
}
catch { CustomMessageBox.Show("Error getting GE data", "Error"); }
return answer;
}
private void domainUpDown1_ValueChanged(object sender, EventArgs e)
{
// new grid system test
List <PointLatLngAlt> list = new List <PointLatLngAlt>();
plugin.Host.FPDrawnPolygon.Points.ForEach(x => { list.Add(x); });
Grid.Host2 = plugin.Host;
grid = Grid.CreateGrid(list, (double)NUM_altitude.Value, (double)NUM_Distance.Value,
(double)NUM_spacing.Value, (double)NUM_angle.Value, (double)NUM_overshoot.Value,
(double)NUM_overshoot2.Value,
(Grid.StartPosition)Enum.Parse(typeof(Grid.StartPosition), CMB_startfrom.Text), false, 0);
List <PointLatLng> list2 = new List <PointLatLng>();
grid.ForEach(x => { list2.Add(x); });
map.HoldInvalidation = true;
layerpolygons.Polygons.Clear();
layerpolygons.Markers.Clear();
if (grid.Count == 0)
{
return;
}
if (chk_boundary.Checked)
{
AddDrawPolygon();
}
int strips = 0;
int a = 1;
PointLatLngAlt prevpoint = grid[0];
foreach (var item in grid)
{
if (item.Tag == "M")
{
if (CHK_internals.Checked)
{
layerpolygons.Markers.Add(new GMarkerGoogle(item, GMarkerGoogleType.green)
{
ToolTipText = a.ToString(), ToolTipMode = MarkerTooltipMode.Always
});
a++;
}
}
else
{
if (item.Tag == "S" || item.Tag == "E")
{
strips++;
if (chk_markers.Checked)
{
layerpolygons.Markers.Add(new GMarkerGoogle(item, GMarkerGoogleType.green)
{
ToolTipText = a.ToString(),
ToolTipMode = MarkerTooltipMode.Always
});
}
a++;
}
}
prevpoint = item;
}
// add wp polygon
wppoly = new GMapPolygon(list2, "Grid");
wppoly.Stroke.Color = Color.Yellow;
wppoly.Fill = Brushes.Transparent;
wppoly.Stroke.Width = 4;
if (chk_grid.Checked)
{
layerpolygons.Polygons.Add(wppoly);
}
Console.WriteLine("Poly Dist " + wppoly.Distance);
lbl_area.Text = calcpolygonarea(plugin.Host.FPDrawnPolygon.Points).ToString("#") + " m^2";
lbl_distance.Text = wppoly.Distance.ToString("0.##") + " km";
lbl_strips.Text = ((int)(strips / 2)).ToString();
lbl_distbetweenlines.Text = NUM_Distance.Value.ToString("0.##") + " m";
map.HoldInvalidation = false;
map.ZoomAndCenterMarkers("polygons");
}
protected override void OnPaint(System.Windows.Forms.PaintEventArgs e)
{
DateTime start = DateTime.Now;
if (this.DesignMode)
{
return;
}
if (area.LocationMiddle.Lat == 0 && area.LocationMiddle.Lng == 0)
{
return;
}
try
{
base.OnPaint(e);
}
catch
{
return;
}
utmzone = center.GetUTMZone();
double heightscale = 1;//(step/90.0)*5;
var campos = convertCoords(center);
cameraX = campos[0];
cameraY = campos[1];
cameraZ = (campos[2] < srtm.getAltitude(center.Lat, center.Lng).alt)
? (srtm.getAltitude(center.Lat, center.Lng).alt + 1) * heightscale
: center.Alt * heightscale; // (srtm.getAltitude(lookZ, lookX, 20) + 100) * heighscale;
lookX = campos[0] + Math.Sin(MathHelper.Radians(rpy.Z)) * 100;
lookY = campos[1] + Math.Cos(MathHelper.Radians(rpy.Z)) * 100;
lookZ = cameraZ;
var size = 20000;
// in front
PointLatLngAlt front = center.newpos(rpy.Z, size);
// behind
PointLatLngAlt behind = center.newpos(rpy.Z, -50);
// left : 90 allows for 180 degree viewing angle
PointLatLngAlt left = center.newpos(rpy.Z - 45, size);
// right
PointLatLngAlt right = center.newpos(rpy.Z + 45, size);
double maxlat = Math.Max(left.Lat, Math.Max(right.Lat, Math.Max(front.Lat, behind.Lat)));
double minlat = Math.Min(left.Lat, Math.Min(right.Lat, Math.Min(front.Lat, behind.Lat)));
double maxlng = Math.Max(left.Lng, Math.Max(right.Lng, Math.Max(front.Lng, behind.Lng)));
double minlng = Math.Min(left.Lng, Math.Min(right.Lng, Math.Min(front.Lng, behind.Lng)));
area = RectLatLng.FromLTRB(minlng, maxlat, maxlng, minlat);
zoom = 20;
float screenscale = 1;//this.Width/(float) this.Height*1f;
if (!Context.IsCurrent)
{
MakeCurrent();
}
GL.MatrixMode(MatrixMode.Projection);
OpenTK.Matrix4 projection = OpenTK.Matrix4.CreatePerspectiveFieldOfView((float)(90 * MathHelper.deg2rad), screenscale, 0.00000001f,
(float)20000);
GL.LoadMatrix(ref projection);
Console.WriteLine("cam: {0} {1} {2} lookat: {3} {4} {5}", (float)cameraX, (float)cameraY, (float)cameraZ,
(float)lookX,
(float)lookY, (float)lookZ);
Matrix4 modelview = Matrix4.LookAt((float)cameraX, (float)cameraY, (float)cameraZ + 100f * 0, (float)lookX,
(float)lookY, (float)lookZ, 0, 0, 1);
GL.MatrixMode(MatrixMode.Modelview);
// roll
modelview = Matrix4.Mult(modelview, Matrix4.CreateRotationZ((float)(rpy.X * MathHelper.deg2rad)));
// pitch
modelview = Matrix4.Mult(modelview, Matrix4.CreateRotationX((float)(rpy.Y * -MathHelper.deg2rad)));
GL.LoadMatrix(ref modelview);
GL.ClearColor(Color.CornflowerBlue);
GL.Clear(ClearBufferMask.ColorBufferBit | ClearBufferMask.DepthBufferBit | ClearBufferMask.AccumBufferBit);
GL.LightModel(LightModelParameter.LightModelAmbient, new float[] { 1f, 1f, 1f, 1f });
GL.Disable(EnableCap.Fog);
GL.Enable(EnableCap.Fog);
//GL.Enable(EnableCap.Lighting);
//GL.Enable(EnableCap.Light0);
GL.Fog(FogParameter.FogColor, new float[] { 100 / 255.0f, 149 / 255.0f, 237 / 255.0f, 1f });
//GL.Fog(FogParameter.FogDensity,0.1f);
GL.Fog(FogParameter.FogMode, (int)FogMode.Linear);
GL.Fog(FogParameter.FogStart, (float)4000);
GL.Fog(FogParameter.FogEnd, (float)size);
GL.Disable(EnableCap.DepthTest);
GL.DepthFunc(DepthFunction.Always);
/*
* GL.Begin(BeginMode.LineStrip);
*
* GL.Color3(Color.White);
* GL.Vertex3(0, 0, 0);
*
* GL.Color3(Color.Red);
* GL.Vertex3(area.Bottom, 0, area.Left);
*
* GL.Color3(Color.Yellow);
* GL.Vertex3(lookX, lookY, lookZ);
*
* GL.Color3(Color.Green);
* GL.Vertex3(cameraX, cameraY, cameraZ);
*
* GL.End();
*/
/*
* GL.PointSize(10);
* GL.Color4(Color.Yellow);
* GL.LineWidth(5);
*
*
* GL.Begin(PrimitiveType.LineStrip);
*
* GL.Vertex3(area.LocationTopLeft.Lng, area.LocationTopLeft.Lat, (float)cameraZ);
* GL.Vertex3(area.LocationTopLeft.Lng, area.LocationRightBottom.Lat, (float)cameraZ);
* GL.Vertex3(area.LocationRightBottom.Lng, area.LocationRightBottom.Lat, (float)cameraZ);
* GL.Vertex3(area.LocationRightBottom.Lng, area.LocationTopLeft.Lat, (float)cameraZ);
* GL.Vertex3(area.LocationTopLeft.Lng, area.LocationTopLeft.Lat, (float)cameraZ);
*
* GL.End();
*
* GL.PointSize((float) (step*1));
* GL.Color3(Color.Blue);
* GL.Begin(PrimitiveType.Points);
* GL.Vertex3(new Vector3((float) center.Lng, (float) center.Lat, (float) cameraZ));
* GL.End();
*/
//GL.ClampColor(ClampColorTarget.ClampReadColor, ClampColorMode.True);
/*
* GL.Enable(EnableCap.Blend);
* GL.DepthMask(false);
* GL.BlendFunc(BlendingFactorSrc.Zero, BlendingFactorDest.Src1Color);
* GL.DepthMask(true);
* GL.Disable(EnableCap.Blend);
*/
// textureid.Clear();
core.fillEmptyTiles = true;
core.LevelsKeepInMemmory = 20;
core.Provider = type;
core.Position = center;
//core.ReloadMap();
List <tileZoomArea> tileArea = new List <tileZoomArea>();
for (int a = 10; a <= zoom; a++)
{
core.Zoom = a;
var area2 = new RectLatLng(center.Lat, center.Lng, 0, 0);
// 200m at max zoom
// step at 0 zoom
var distm = MathHelper.map(a, 0, zoom, size, 50);
var offset = center.newpos(rpy.Z, distm);
area2.Inflate(Math.Abs(center.Lat - offset.Lat), Math.Abs(center.Lng - offset.Lng));
var extratile = 0;
if (a == zoom)
{
extratile = 1;
}
var tiles = new tileZoomArea()
{
zoom = a, points = prj.GetAreaTileList(area2, a, extratile), area = area2
};
tileArea.Add(tiles);
}
//tileArea.Reverse();
while (textureid.Count > 250)
{
var first = textureid.Keys.First();
GL.DeleteTexture(textureid[first]);
textureid.Remove(first);
}
// get tiles & combine into one
foreach (var tilearea in tileArea)
{
foreach (var p in tilearea.points)
{
core.tileDrawingListLock.AcquireReaderLock();
core.Matrix.EnterReadLock();
try
{
GMap.NET.Internals.Tile t = core.Matrix.GetTileWithNoLock(tilearea.zoom, p);
if (t.NotEmpty)
{
foreach (GMapImage img in t.Overlays)
{
if (img.IsParent)
{
}
if (!textureid.ContainsKey(p))
{
try
{
generateTexture(p, (Bitmap)img.Img);
}
catch
{
continue;
}
}
}
}
else
{
}
}
finally
{
core.Matrix.LeaveReadLock();
core.tileDrawingListLock.ReleaseReaderLock();
}
if (textureid.ContainsKey(p))
{
int texture = textureid[p];
GL.Enable(EnableCap.Texture2D);
GL.BindTexture(TextureTarget.Texture2D, texture);
}
else
{
//Console.WriteLine("Missing tile");
GL.Disable(EnableCap.Texture2D);
continue;
}
long xr = p.X * prj.TileSize.Width;
long yr = p.Y * prj.TileSize.Width;
long x2 = (p.X + 1) * prj.TileSize.Width;
long y2 = (p.Y + 1) * prj.TileSize.Width;
GL.LineWidth(4);
GL.Color3(Color.White);
GL.Clear(ClearBufferMask.DepthBufferBit);
GL.Enable(EnableCap.DepthTest);
// generate terrain
GL.Begin(PrimitiveType.Points);
GL.PointSize((float)(20));
//GL.Begin(PrimitiveType.Points);
GL.Color3(Color.Blue);
var latlng = prj.FromPixelToLatLng(xr, yr, tilearea.zoom);
var utm = convertCoords(latlng);
utm[2] = srtm.getAltitude(latlng.Lat, latlng.Lng).alt;
GL.TexCoord2(0, 0);
GL.Vertex3(utm[0], utm[1], utm[2]);
// next down
latlng = prj.FromPixelToLatLng(xr, y2, tilearea.zoom);
utm = convertCoords(latlng);
utm[2] = srtm.getAltitude(latlng.Lat, latlng.Lng).alt;
GL.TexCoord2(0, 1);
GL.Vertex3(utm[0], utm[1], utm[2]);
// next right
latlng = prj.FromPixelToLatLng(x2, yr, tilearea.zoom);
utm = convertCoords(latlng);
utm[2] = srtm.getAltitude(latlng.Lat, latlng.Lng).alt;
GL.TexCoord2(1, 0);
GL.Vertex3(utm[0], utm[1], utm[2]);
// next right down
latlng = prj.FromPixelToLatLng(x2, y2, tilearea.zoom);
utm = convertCoords(latlng);
utm[2] = srtm.getAltitude(latlng.Lat, latlng.Lng).alt;
GL.TexCoord2(1, 1);
GL.Vertex3(utm[0], utm[1], utm[2]);
GL.End();
var dist = LocationCenter.GetDistance(latlng);
var pxstep = 128;
if (dist < 500)
{
pxstep = 32;
}
double[] oldutm = null;
GL.Begin(PrimitiveType.TriangleStrip);
for (long x = xr; x < x2; x += pxstep)
{
long xnext = x + pxstep;
//GL.Begin(PrimitiveType.LineStrip);
for (long y = yr; y < y2; y += pxstep)
{
long ynext = y + pxstep;
//GL.Begin(PrimitiveType.Lines);
var latlng1 = prj.FromPixelToLatLng(x, y, tilearea.zoom);
var utm1 = convertCoords(latlng1);
utm1[2] = srtm.getAltitude(latlng1.Lat, latlng1.Lng).alt;
var imgx = MathHelper.map(x, xr, x2, 0, 1);
var imgy = MathHelper.map(y, yr, y2, 0, 1);
GL.TexCoord2(imgx, imgy);
GL.Vertex3(utm1[0], utm1[1], utm1[2]);
//
var latlng2 = prj.FromPixelToLatLng(x, ynext, tilearea.zoom);
var utm2 = convertCoords(latlng2);
utm2[2] = srtm.getAltitude(latlng2.Lat, latlng2.Lng).alt;
imgx = MathHelper.map(x, xr, x2, 0, 1);
imgy = MathHelper.map(ynext, yr, y2, 0, 1);
GL.TexCoord2(imgx, imgy);
GL.Vertex3(utm2[0], utm2[1], utm2[2]);
//
latlng2 = prj.FromPixelToLatLng(xnext, y, tilearea.zoom);
utm2 = convertCoords(latlng2);
utm2[2] = srtm.getAltitude(latlng2.Lat, latlng2.Lng).alt;
imgx = MathHelper.map(xnext, xr, x2, 0, 1);
imgy = MathHelper.map(y, yr, y2, 0, 1);
GL.TexCoord2(imgx, imgy);
GL.Vertex3(utm2[0], utm2[1], utm2[2]);
//
latlng2 = prj.FromPixelToLatLng(xnext, ynext, tilearea.zoom);
utm2 = convertCoords(latlng2);
utm2[2] = srtm.getAltitude(latlng2.Lat, latlng2.Lng).alt;
imgx = MathHelper.map(xnext, xr, x2, 0, 1);
imgy = MathHelper.map(ynext, yr, y2, 0, 1);
GL.TexCoord2(imgx, imgy);
GL.Vertex3(utm2[0], utm2[1], utm2[2]);
}
}
GL.End();
GL.Disable(EnableCap.Texture2D);
}
}
GL.Flush();
try
{
this.SwapBuffers();
//Context.MakeCurrent(null);
}
catch
{
}
//this.Invalidate();
var delta = DateTime.Now - start;
Console.WriteLine("OpenGLTest2 {0}", delta.TotalMilliseconds);
}
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);
}
}
public void writeKML(string filename)
{
try
{
// writeGPX(filename);
writeRinex(filename);
//writeWPFile(filename);
}
catch
{
}
Color[] colours =
{
Color.Red, Color.Orange, Color.Yellow, Color.Green, Color.Blue, Color.Indigo,
Color.Violet, Color.Pink
};
AltitudeMode altmode = AltitudeMode.absolute;
KMLRoot kml = new KMLRoot();
Folder fldr = new Folder("Log");
Style style = new Style();
style.Id = "yellowLineGreenPoly";
style.Add(new LineStyle(HexStringToColor("7f00ffff"), 4));
Style style1 = new Style();
style1.Id = "spray";
style1.Add(new LineStyle(HexStringToColor("4c0000ff"), 0));
style1.Add(new PolyStyle()
{
Color = HexStringToColor("4c0000ff")
});
PolyStyle pstyle = new PolyStyle();
pstyle.Color = HexStringToColor("7f00ff00");
style.Add(pstyle);
kml.Document.AddStyle(style);
kml.Document.AddStyle(style1);
int stylecode = 0xff;
int g = -1;
foreach (List <Point3D> poslist in position)
{
g++;
if (poslist == null)
{
continue;
}
/*
* List<PointLatLngAlt> pllalist = new List<PointLatLngAlt>();
*
* foreach (var point in poslist)
* {
* pllalist.Add(new PointLatLngAlt(point.Y, point.X, point.Z, ""));
* }
*
* var ans = Utilities.LineOffset.GetPolygon(pllalist, 2);
*
*
*
* while (ans.Count > 0)
* {
* var first = ans[0];
* var second = ans[1];
* var secondlast = ans[ans.Count - 2];
* var last = ans[ans.Count-1];
*
* ans.Remove(first);
* ans.Remove(last);
*
* var polycoords = new BoundaryIs();
*
* polycoords.LinearRing = new LinearRing();
*
* polycoords.LinearRing.Coordinates.Add(new Point3D(first.Lng, first.Lat, 1));
* polycoords.LinearRing.Coordinates.Add(new Point3D(second.Lng, second.Lat, 1));
* polycoords.LinearRing.Coordinates.Add(new Point3D(secondlast.Lng, secondlast.Lat, 1));
* polycoords.LinearRing.Coordinates.Add(new Point3D(last.Lng, last.Lat, 1));
* polycoords.LinearRing.Coordinates.Add(new Point3D(first.Lng, first.Lat, 1));
*
* //if (!IsClockwise(polycoords.LinearRing.Coordinates))
* // polycoords.LinearRing.Coordinates.Reverse();
*
* Polygon kmlpoly = new Polygon() { AltitudeMode = AltitudeMode.relativeToGround, Extrude = false, OuterBoundaryIs = polycoords };
*
* Placemark pmpoly = new Placemark();
* pmpoly.Polygon = kmlpoly;
* pmpoly.name = g + " test";
* pmpoly.styleUrl = "#spray";
*
* fldr.Add(pmpoly);
* }
*/
LineString ls = new LineString();
ls.AltitudeMode = altmode;
ls.Extrude = true;
//ls.Tessellate = true;
Coordinates coords = new Coordinates();
coords.AddRange(poslist);
ls.coordinates = coords;
Placemark pm = new Placemark();
string mode = "";
if (g < modelist.Count)
{
mode = modelist[g];
}
pm.name = g + " Flight Path " + mode;
pm.styleUrl = "#yellowLineGreenPoly";
pm.LineString = ls;
stylecode = colours[g % (colours.Length - 1)].ToArgb();
Style style2 = new Style();
Color color = Color.FromArgb(0xff, (stylecode >> 16) & 0xff, (stylecode >> 8) & 0xff,
(stylecode >> 0) & 0xff);
//log.Info("colour " + color.ToArgb().ToString("X") + " " + color.ToKnownColor().ToString());
style2.Add(new LineStyle(color, 4));
pm.AddStyle(style2);
fldr.Add(pm);
}
Placemark pmPOS = new Placemark();
pmPOS.name = "GPOS Message";
pmPOS.LineString = new LineString();
pmPOS.LineString.coordinates = new Coordinates();
Point3D lastPoint3D = new Point3D();
PointLatLngAlt lastplla = PointLatLngAlt.Zero;
foreach (var item in PosLatLngAlts)
{
var newpoint = new Point3D(item.Lng, item.Lat, item.Alt);
if (item.GetDistance(lastplla) < 0.1 &&
lastPoint3D.Z >= (newpoint.Z - 0.3) &&
lastPoint3D.Z <= (newpoint.Z + 0.3))
{
continue;
}
pmPOS.LineString.coordinates.Add(newpoint);
lastPoint3D = newpoint;
lastplla = item;
if (pmPOS.LineString.coordinates.Count > 20000)
{
//add current
pmPOS.AddStyle(style);
fldr.Add(pmPOS);
// create new
pmPOS = new Placemark();
pmPOS.name = "GPOS Message - extra";
pmPOS.LineString = new LineString();
pmPOS.LineString.coordinates = new Coordinates();
lastPoint3D = new Point3D();
lastplla = PointLatLngAlt.Zero;
}
}
pmPOS.AddStyle(style);
fldr.Add(pmPOS);
Folder planes = new Folder();
planes.name = "Planes";
fldr.Add(planes);
Folder waypoints = new Folder();
waypoints.name = "Waypoints";
fldr.Add(waypoints);
LineString lswp = new LineString();
lswp.AltitudeMode = AltitudeMode.relativeToGround;
lswp.Extrude = true;
Coordinates coordswp = new Coordinates();
foreach (PointLatLngAlt p1 in cmd)
{
if (p1.Lng == 0 && p1.Lat == 0)
{
continue;
}
coordswp.Add(new Point3D(p1.Lng, p1.Lat, p1.Alt));
}
lswp.coordinates = coordswp;
Placemark pmwp = new Placemark();
pmwp.name = "Waypoints";
//pm.styleUrl = "#yellowLineGreenPoly";
pmwp.LineString = lswp;
if (coordswp.Count > 0)
{
waypoints.Add(pmwp);
}
int a = 0;
int l = -1;
Model lastmodel = null;
foreach (Data mod in flightdata)
{
l++;
if (mod.model.Location.latitude == 0)
{
continue;
}
if (lastmodel != null)
{
if (lastmodel.Location.Equals(mod.model.Location))
{
continue;
}
}
Placemark pmplane = new Placemark();
pmplane.name = "Plane " + a;
pmplane.visibility = false;
Model model = mod.model;
model.AltitudeMode = altmode;
model.Scale.x = 2;
model.Scale.y = 2;
model.Scale.z = 2;
try
{
pmplane.description = @"<![CDATA[
<table>
<tr><td>Roll: " + model.Orientation.roll + @" </td></tr>
<tr><td>Pitch: " + model.Orientation.tilt + @" </td></tr>
<tr><td>Yaw: " + model.Orientation.heading + @" </td></tr>
<tr><td>WP dist " + mod.ntun[2] + @" </td></tr>
<tr><td>tar bear " + mod.ntun[3] + @" </td></tr>
<tr><td>nav bear " + mod.ntun[4] + @" </td></tr>
<tr><td>alt error " + mod.ntun[5] + @" </td></tr>
</table>
]]>";
}
catch
{
}
try
{
pmplane.Point = new KmlPoint((float)model.Location.longitude, (float)model.Location.latitude,
(float)model.Location.altitude);
pmplane.Point.AltitudeMode = altmode;
Link link = new Link();
link.href = "block_plane_0.dae";
model.Link = link;
pmplane.Model = model;
planes.Add(pmplane);
}
catch
{
} // bad lat long value
lastmodel = mod.model;
a++;
}
kml.Document.Add(fldr);
kml.Save(filename);
// create kmz - aka zip file
FileStream fs = File.Open(filename.ToLower().Replace(".kmz.kml", ".kmz"), FileMode.Create);
ZipOutputStream zipStream = new ZipOutputStream(fs);
zipStream.SetLevel(9); //0-9, 9 being the highest level of compression
zipStream.UseZip64 = UseZip64.Off; // older zipfile
// entry 1
string entryName = ZipEntry.CleanName(Path.GetFileName(filename));
// Removes drive from name and fixes slash direction
ZipEntry newEntry = new ZipEntry(entryName);
newEntry.DateTime = DateTime.Now;
zipStream.PutNextEntry(newEntry);
// Zip the file in buffered chunks
// the "using" will close the stream even if an exception occurs
byte[] buffer = new byte[4096];
using (FileStream streamReader = File.OpenRead(filename))
{
StreamUtils.Copy(streamReader, zipStream, buffer);
}
zipStream.CloseEntry();
File.Delete(filename);
filename = Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar +
"block_plane_0.dae";
// entry 2
entryName = ZipEntry.CleanName(Path.GetFileName(filename));
// Removes drive from name and fixes slash direction
newEntry = new ZipEntry(entryName);
newEntry.DateTime = DateTime.Now;
zipStream.PutNextEntry(newEntry);
// Zip the file in buffered chunks
// the "using" will close the stream even if an exception occurs
buffer = new byte[4096];
using (FileStream streamReader = File.OpenRead(filename))
{
StreamUtils.Copy(streamReader, zipStream, buffer);
}
zipStream.CloseEntry();
zipStream.IsStreamOwner = true; // Makes the Close also Close the underlying stream
zipStream.Close();
positionindex = 0;
modelist.Clear();
flightdata.Clear();
position = new List <Core.Geometry.Point3D> [200];
cmd.Clear();
cmdraw.Clear();
}
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)
{
loc.Alt -= srtm.getAltitude(loc.Lat, loc.Lng).alt;
}
continue;
}
double dist = last.GetDistance(loc);
if (altmode == FlightPlanner.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, newpoint.Alt / CurrentState.multiplieralt);
// terrain alt
list4terrain.Add(disttotal, (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 void processLine(string line)
{
try
{
if (doevent.Second != DateTime.Now.Second)
{
Application.DoEvents();
doevent = DateTime.Now;
}
if (line.Length == 0)
{
return;
}
DateTime start = DateTime.Now;
if (line.ToLower().Contains("ArduCopter"))
{
MainV2.comPort.MAV.cs.firmware = MainV2.Firmwares.ArduCopter2;
}
if (line.ToLower().Contains("ArduPlane"))
{
MainV2.comPort.MAV.cs.firmware = MainV2.Firmwares.ArduPlane;
}
if (line.ToLower().Contains("ArduRover"))
{
MainV2.comPort.MAV.cs.firmware = MainV2.Firmwares.ArduRover;
}
line = line.Replace(", ", ",");
line = line.Replace(": ", ":");
string[] items = line.Split(',', ':');
if (items[0].Contains("FMT"))
{
try
{
DFLog.FMTLine(line);
}
catch { }
}
else if (items[0].Contains("CMD"))
{
if (flightdata.Count == 0)
{
if (int.Parse(items[3]) <= (int)MAVLink.MAV_CMD.LAST) // wps
{
PointLatLngAlt temp = new PointLatLngAlt(double.Parse(items[7], new System.Globalization.CultureInfo("en-US")), double.Parse(items[8], new System.Globalization.CultureInfo("en-US")), double.Parse(items[6], new System.Globalization.CultureInfo("en-US")), items[2].ToString());
cmd.Add(temp);
}
}
}
else if (items[0].Contains("MOD"))
{
positionindex++;
while (modelist.Count < positionindex + 1)
{
modelist.Add("");
}
if (items.Length == 4)
{
modelist[positionindex] = (items[2]);
}
else
{
modelist[positionindex] = (items[1]);
}
}
else if (items[0].Contains("GPS") && DFLog.logformat.ContainsKey("GPS"))
{
if (items[0].Contains("GPS2"))
{
return;
}
if (items[DFLog.FindMessageOffset("GPS", "Status")] != "3")
{
return;
}
if (position[positionindex] == null)
{
position[positionindex] = new List <Point3D>();
}
// if (double.Parse(items[4], new System.Globalization.CultureInfo("en-US")) == 0)
// return;
// 7 agl
// 8 asl...
double alt = double.Parse(items[DFLog.FindMessageOffset("GPS", "Alt")], new System.Globalization.CultureInfo("en-US"));
position[positionindex].Add(new Point3D(double.Parse(items[DFLog.FindMessageOffset("GPS", "Lng")],
new System.Globalization.CultureInfo("en-US")),
double.Parse(items[DFLog.FindMessageOffset("GPS", "Lat")],
new System.Globalization.CultureInfo("en-US")), alt));
oldlastpos = lastpos;
lastpos = (position[positionindex][position[positionindex].Count - 1]);
lastline = line;
}
else if (items[0].Contains("GPS") && items[2] == "1" && items[4] != "0" && items[4] != "-1" && lastline != line) // check gps line and fixed status
{
MainV2.comPort.MAV.cs.firmware = MainV2.Firmwares.ArduPlane;
if (position[positionindex] == null)
{
position[positionindex] = new List <Point3D>();
}
if (double.Parse(items[4], new System.Globalization.CultureInfo("en-US")) == 0)
{
return;
}
double alt = double.Parse(items[6], new System.Globalization.CultureInfo("en-US"));
if (items.Length == 11 && items[6] == "0.0000")
{
alt = double.Parse(items[7], new System.Globalization.CultureInfo("en-US"));
}
if (items.Length == 11 && items[6] == "0")
{
alt = double.Parse(items[7], new System.Globalization.CultureInfo("en-US"));
}
position[positionindex].Add(new Point3D(double.Parse(items[5], new System.Globalization.CultureInfo("en-US")), double.Parse(items[4], new System.Globalization.CultureInfo("en-US")), alt));
oldlastpos = lastpos;
lastpos = (position[positionindex][position[positionindex].Count - 1]);
lastline = line;
}
else if (items[0].Contains("GPS") && items[4] != "0" && items[4] != "-1" && items.Length <= 9) // AC
{
MainV2.comPort.MAV.cs.firmware = MainV2.Firmwares.ArduCopter2;
if (position[positionindex] == null)
{
position[positionindex] = new List <Point3D>();
}
if (double.Parse(items[4], new System.Globalization.CultureInfo("en-US")) == 0)
{
return;
}
double alt = double.Parse(items[5], new System.Globalization.CultureInfo("en-US"));
position[positionindex].Add(new Point3D(double.Parse(items[4], new System.Globalization.CultureInfo("en-US")), double.Parse(items[3], new System.Globalization.CultureInfo("en-US")), alt));
oldlastpos = lastpos;
lastpos = (position[positionindex][position[positionindex].Count - 1]);
lastline = line;
}
else if ((items[0].Contains("GPS") && items[1] == "3" && items[6] != "0" && items[6] != "-1" && lastline != line && items.Length == 12) ||
(items[0].Contains("GPS") && items[1] == "3" && items[6] != "0" && items[6] != "-1" && lastline != line && items.Length == 14))
{
if (position[positionindex] == null)
{
position[positionindex] = new List <Point3D>();
}
// if (double.Parse(items[4], new System.Globalization.CultureInfo("en-US")) == 0)
// return;
// 8 agl
// 9 asl...
double alt = double.Parse(items[9], new System.Globalization.CultureInfo("en-US"));
position[positionindex].Add(new Point3D(double.Parse(items[7], new System.Globalization.CultureInfo("en-US")), double.Parse(items[6], new System.Globalization.CultureInfo("en-US")), alt));
oldlastpos = lastpos;
lastpos = (position[positionindex][position[positionindex].Count - 1]);
lastline = line;
}
else if (items[0].Contains("GPS") && items[1] == "3" && items[4] != "0" && items[4] != "-1" && lastline != line && items.Length == 11) // check gps line and fixed status
{
if (position[positionindex] == null)
{
position[positionindex] = new List <Point3D>();
}
// if (double.Parse(items[4], new System.Globalization.CultureInfo("en-US")) == 0)
// return;
// 7 agl
// 8 asl...
double alt = double.Parse(items[8], new System.Globalization.CultureInfo("en-US"));
position[positionindex].Add(new Point3D(double.Parse(items[6], new System.Globalization.CultureInfo("en-US")), double.Parse(items[5], new System.Globalization.CultureInfo("en-US")), alt));
oldlastpos = lastpos;
lastpos = (position[positionindex][position[positionindex].Count - 1]);
lastline = line;
}
else if (items[0].Contains("GRAW"))
{
gpsrawdata.Add(line);
}
else if (items[0].Contains("CTUN"))
{
ctunlast = items;
}
else if (items[0].Contains("NTUN"))
{
ntunlast = items;
try
{
// line = "ATT:" + double.Parse(ctunlast[3], new System.Globalization.CultureInfo("en-US")) * 100 + "," + double.Parse(ctunlast[6], new System.Globalization.CultureInfo("en-US")) * 100 + "," + double.Parse(items[1], new System.Globalization.CultureInfo("en-US")) * 100;
// items = line.Split(',', ':');
}
catch { }
}
else if (items[0].Contains("ATT"))
{
try
{
if (lastpos.X != 0 && oldlastpos.X != lastpos.X && oldlastpos.Y != lastpos.Y)
{
Data dat = new Data();
try
{
dat.datetime = DFLog.GetTimeGPS(lastline);
}
catch { }
runmodel = new Model();
runmodel.Location.longitude = lastpos.X;
runmodel.Location.latitude = lastpos.Y;
runmodel.Location.altitude = lastpos.Z;
oldlastpos = lastpos;
runmodel.Orientation.roll = double.Parse(items[DFLog.FindMessageOffset("ATT", "Roll")], new System.Globalization.CultureInfo("en-US")) / -1;
runmodel.Orientation.tilt = double.Parse(items[DFLog.FindMessageOffset("ATT", "Pitch")], new System.Globalization.CultureInfo("en-US")) / -1;
runmodel.Orientation.heading = double.Parse(items[DFLog.FindMessageOffset("ATT", "Yaw")], new System.Globalization.CultureInfo("en-US")) / 1;
dat.model = runmodel;
dat.ctun = ctunlast;
dat.ntun = ntunlast;
flightdata.Add(dat);
}
}
catch (Exception ex) { log.Error(ex); }
}
if ((DateTime.Now - start).TotalMilliseconds > 5)
{
Console.WriteLine(line);
}
}
catch (Exception)
{
// if items is to short or parse fails.. ignore
}
}
private void ElevationProfile_Load(object sender, EventArgs e)
{
if (planlocs.Count <= 1)
{
this.Close();
return;
}
// GE plot
double a = 0;
double increment = (distance / (float)(gelocs.Count - 1));
foreach (PointLatLngAlt geloc in gelocs)
{
if (geloc == null)
{
continue;
}
list2.Add(a, geloc.Alt);
Console.WriteLine("GE " + geloc.Lng + "," + geloc.Lat + "," + geloc.Alt);
a += increment;
}
// Planner Plot
a = 0;
int count = 0;
PointLatLngAlt lastloc = null;
foreach (PointLatLngAlt planloc in planlocs)
{
if (planloc == null)
{
continue;
}
if (lastloc != null)
{
a += planloc.GetDistance(lastloc);
}
// deal with at mode
if (altmode == GCSViews.FlightPlanner.altmode.Terrain)
{
list1 = list4terrain;
break;
}
else if (altmode == GCSViews.FlightPlanner.altmode.Relative)
{
// already includes the home alt
list1.Add(a, (planloc.Alt / CurrentState.multiplieralt), 0, planloc.Tag);
}
else
{
// abs
// already absolute
list1.Add(a, (planloc.Alt / CurrentState.multiplieralt), 0, planloc.Tag);
}
lastloc = planloc;
count++;
}
// draw graph
CreateChart(zg1);
}
private void mainloop()
{
run = true;
DateTime tracklast = DateTime.Now.AddSeconds(0);
List <PointLatLng> trackPoints = new List <PointLatLng>();
DateTime waypoints = DateTime.Now.AddSeconds(0);
DateTime mapupdate = DateTime.Now.AddSeconds(0);
while (run)
{
try
{
Thread.Sleep(50);
Forms.Device.BeginInvokeOnMainThread(() =>
{
var start = DateTime.Now;
Hud.airspeed = MainV2.comPort.MAV.cs.airspeed;
Hud.alt = MainV2.comPort.MAV.cs.alt;
Hud.batterylevel = (float)MainV2.comPort.MAV.cs.battery_voltage;
Hud.batteryremaining = MainV2.comPort.MAV.cs.battery_remaining;
Hud.connected = MainV2.comPort.MAV.cs.connected;
Hud.current = (float)MainV2.comPort.MAV.cs.current;
Hud.datetime = MainV2.comPort.MAV.cs.datetime;
Hud.disttowp = MainV2.comPort.MAV.cs.wp_dist;
Hud.ekfstatus = MainV2.comPort.MAV.cs.ekfstatus;
Hud.failsafe = MainV2.comPort.MAV.cs.failsafe;
Hud.gpsfix = MainV2.comPort.MAV.cs.gpsstatus;
Hud.gpsfix2 = MainV2.comPort.MAV.cs.gpsstatus2;
Hud.gpshdop = MainV2.comPort.MAV.cs.gpshdop;
Hud.gpshdop2 = MainV2.comPort.MAV.cs.gpshdop2;
Hud.groundalt = (float)MainV2.comPort.MAV.cs.HomeAlt;
Hud.groundcourse = MainV2.comPort.MAV.cs.groundcourse;
Hud.groundspeed = MainV2.comPort.MAV.cs.groundspeed;
Hud.heading = MainV2.comPort.MAV.cs.yaw;
Hud.linkqualitygcs = MainV2.comPort.MAV.cs.linkqualitygcs;
Hud.message = MainV2.comPort.MAV.cs.messageHigh;
Hud.messagetime = MainV2.comPort.MAV.cs.messageHighTime;
Hud.mode = MainV2.comPort.MAV.cs.mode;
Hud.navpitch = MainV2.comPort.MAV.cs.nav_pitch;
Hud.navroll = MainV2.comPort.MAV.cs.nav_roll;
Hud.pitch = MainV2.comPort.MAV.cs.pitch;
Hud.roll = MainV2.comPort.MAV.cs.roll;
Hud.status = MainV2.comPort.MAV.cs.armed;
Hud.targetalt = MainV2.comPort.MAV.cs.targetalt;
Hud.targetheading = MainV2.comPort.MAV.cs.nav_bearing;
Hud.targetspeed = MainV2.comPort.MAV.cs.targetairspeed;
Hud.turnrate = MainV2.comPort.MAV.cs.turnrate;
Hud.verticalspeed = MainV2.comPort.MAV.cs.verticalspeed;
Hud.vibex = MainV2.comPort.MAV.cs.vibex;
Hud.vibey = MainV2.comPort.MAV.cs.vibey;
Hud.vibez = MainV2.comPort.MAV.cs.vibez;
Hud.wpno = (int)MainV2.comPort.MAV.cs.wpno;
Hud.xtrack_error = MainV2.comPort.MAV.cs.xtrack_error;
Hud.AOA = MainV2.comPort.MAV.cs.AOA;
Hud.SSA = MainV2.comPort.MAV.cs.SSA;
Hud.critAOA = MainV2.comPort.MAV.cs.crit_AOA;
// update map
if (tracklast.AddSeconds(Settings.Instance.GetDouble("FD_MapUpdateDelay", 1.2)) < DateTime.Now)
{
adsb.CurrentPosition = MainV2.comPort.MAV.cs.HomeLocation;
// show proximity screen
if (MainV2.comPort.MAV?.Proximity != null && MainV2.comPort.MAV.Proximity.DataAvailable)
{
//this.BeginInvoke((MethodInvoker)delegate { new ProximityControl(MainV2.comPort.MAV).Show(); });
}
if (Settings.Instance.GetBoolean("CHK_maprotation"))
{
// dont holdinvalidation here
setMapBearing();
}
if (route == null)
{
route = new GMapRoute(trackPoints, "track");
routes.Routes.Add(route);
}
PointLatLng currentloc = new PointLatLng(MainV2.comPort.MAV.cs.lat, MainV2.comPort.MAV.cs.lng);
gMapControl1.HoldInvalidation = true;
int numTrackLength = Settings.Instance.GetInt32("NUM_tracklength", 200);
// maintain route history length
if (route.Points.Count > numTrackLength)
{
route.Points.RemoveRange(0,
route.Points.Count - numTrackLength);
}
// add new route point
if (MainV2.comPort.MAV.cs.lat != 0 && MainV2.comPort.MAV.cs.lng != 0)
{
route.Points.Add(currentloc);
}
updateRoutePosition();
// update programed wp course
if (waypoints.AddSeconds(5) < DateTime.Now)
{
//Console.WriteLine("Doing FD WP's");
updateClearMissionRouteMarkers();
var wps = MainV2.comPort.MAV.wps.Values.ToList();
if (wps.Count >= 1)
{
var homeplla = new PointLatLngAlt(MainV2.comPort.MAV.cs.HomeLocation.Lat,
MainV2.comPort.MAV.cs.HomeLocation.Lng,
MainV2.comPort.MAV.cs.HomeLocation.Alt / CurrentState.multiplieralt, "H");
var overlay = new WPOverlay();
{
List <Locationwp> mission_items;
mission_items = MainV2.comPort.MAV.wps.Values.Select(a => (Locationwp)a).ToList();
mission_items.RemoveAt(0);
if (wps.Count == 1)
{
overlay.CreateOverlay((MAVLink.MAV_FRAME)wps[0].frame, homeplla,
mission_items,
0 / CurrentState.multiplieralt, 0 / CurrentState.multiplieralt);
}
else
{
overlay.CreateOverlay((MAVLink.MAV_FRAME)wps[1].frame, homeplla,
mission_items,
0 / CurrentState.multiplieralt, 0 / CurrentState.multiplieralt);
}
}
var existing = gMapControl1.Overlays.Where(a => a.Id == overlay.overlay.Id).ToList();
foreach (var b in existing)
{
gMapControl1.Overlays.Remove(b);
}
gMapControl1.Overlays.Insert(1, overlay.overlay);
overlay.overlay.ForceUpdate();
//distanceBar1.ClearWPDist();
var i = -1;
var travdist = 0.0;
var lastplla = overlay.pointlist.First();
foreach (var plla in overlay.pointlist)
{
i++;
if (plla == null)
{
continue;
}
var dist = lastplla.GetDistance(plla);
//distanceBar1.AddWPDist((float)dist);
if (i <= MainV2.comPort.MAV.cs.wpno)
{
travdist += dist;
}
}
travdist -= MainV2.comPort.MAV.cs.wp_dist;
//if (MainV2.comPort.MAV.cs.mode.ToUpper() == "AUTO")
//distanceBar1.traveleddist = (float)travdist;
}
RegeneratePolygon();
// update rally points
rallypointoverlay.Markers.Clear();
foreach (var mark in MainV2.comPort.MAV.rallypoints.Values)
{
rallypointoverlay.Markers.Add(new GMapMarkerRallyPt(mark));
}
// optional on Flight data
if (MainV2.ShowAirports)
{
// airports
foreach (var item in Airports.getAirports(gMapControl1.Position).ToArray())
{
try
{
rallypointoverlay.Markers.Add(new GMapMarkerAirport(item)
{
ToolTipText = item.Tag,
ToolTipMode = MarkerTooltipMode.OnMouseOver
});
}
catch (Exception e)
{
log.Error(e);
}
}
}
waypoints = DateTime.Now;
}
updateClearRoutesMarkers();
// add this after the mav icons are drawn
if (MainV2.comPort.MAV.cs.MovingBase != null && MainV2.comPort.MAV.cs.MovingBase == PointLatLngAlt.Zero)
{
addMissionRouteMarker(new GMarkerGoogle(currentloc, GMarkerGoogleType.blue_dot)
{
Position = MainV2.comPort.MAV.cs.MovingBase,
ToolTipText = "Moving Base",
ToolTipMode = MarkerTooltipMode.OnMouseOver
});
}
// add gimbal point center
try
{
if (MainV2.comPort.MAV.param.ContainsKey("MNT_STAB_TILT") &&
MainV2.comPort.MAV.param.ContainsKey("MNT_STAB_ROLL") &&
MainV2.comPort.MAV.param.ContainsKey("MNT_TYPE"))
{
float temp1 = (float)MainV2.comPort.MAV.param["MNT_STAB_TILT"];
float temp2 = (float)MainV2.comPort.MAV.param["MNT_STAB_ROLL"];
float temp3 = (float)MainV2.comPort.MAV.param["MNT_TYPE"];
if (MainV2.comPort.MAV.param.ContainsKey("MNT_STAB_PAN") &&
// (float)MainV2.comPort.MAV.param["MNT_STAB_PAN"] == 1 &&
((float)MainV2.comPort.MAV.param["MNT_STAB_TILT"] == 1 &&
(float)MainV2.comPort.MAV.param["MNT_STAB_ROLL"] == 0) ||
(float)MainV2.comPort.MAV.param["MNT_TYPE"] == 4) // storm driver
{
/*
* var marker = GimbalPoint.ProjectPoint();
*
* if (marker != PointLatLngAlt.Zero)
* {
* MainV2.comPort.MAV.cs.GimbalPoint = marker;
*
* addMissionRouteMarker(new GMarkerGoogle(marker, GMarkerGoogleType.blue_dot)
* {
* ToolTipText = "Camera Target\n" + marker,
* ToolTipMode = MarkerTooltipMode.OnMouseOver
* });
* }
*/
}
}
// cleanup old - no markers where added, so remove all old
if (MainV2.comPort.MAV.camerapoints.Count < photosoverlay.Markers.Count)
{
photosoverlay.Markers.Clear();
}
var min_interval = 0.0;
if (MainV2.comPort.MAV.param.ContainsKey("CAM_MIN_INTERVAL"))
{
min_interval = MainV2.comPort.MAV.param["CAM_MIN_INTERVAL"].Value / 1000.0;
}
// set fov's based on last grid calc
if (Settings.Instance["camera_fovh"] != null)
{
GMapMarkerPhoto.hfov = Settings.Instance.GetDouble("camera_fovh");
GMapMarkerPhoto.vfov = Settings.Instance.GetDouble("camera_fovv");
}
// add new - populate camera_feedback to map
double oldtime = double.MinValue;
foreach (var mark in MainV2.comPort.MAV.camerapoints.ToArray())
{
var timesincelastshot = (mark.time_usec / 1000.0) / 1000.0 - oldtime;
MainV2.comPort.MAV.cs.timesincelastshot = timesincelastshot;
bool contains = photosoverlay.Markers.Any(p => p.Tag.Equals(mark.time_usec));
if (!contains)
{
if (timesincelastshot < min_interval)
{
addMissionPhotoMarker(new GMapMarkerPhoto(mark, true));
}
else
{
addMissionPhotoMarker(new GMapMarkerPhoto(mark, false));
}
}
oldtime = (mark.time_usec / 1000.0) / 1000.0;
}
var GMapMarkerOverlapCount = new GMapMarkerOverlapCount(PointLatLng.Empty);
// age current
int camcount = MainV2.comPort.MAV.camerapoints.Count;
int a = 0;
foreach (var mark in photosoverlay.Markers)
{
if (mark is GMapMarkerPhoto)
{
if (CameraOverlap)
{
var marker = ((GMapMarkerPhoto)mark);
// abandon roll higher than 25 degrees
if (Math.Abs(marker.Roll) < 25)
{
GMapMarkerOverlapCount.Add(
((GMapMarkerPhoto)mark).footprintpoly);
}
}
if (a < (camcount - 4))
{
((GMapMarkerPhoto)mark).drawfootprint = false;
}
}
a++;
}
if (CameraOverlap)
{
if (!kmlpolygons.Markers.Contains(GMapMarkerOverlapCount) &&
camcount > 0)
{
kmlpolygons.Markers.Clear();
kmlpolygons.Markers.Add(GMapMarkerOverlapCount);
}
}
else if (kmlpolygons.Markers.Contains(GMapMarkerOverlapCount))
{
kmlpolygons.Markers.Clear();
}
}
catch (Exception ex)
{
log.Error(ex);
}
lock (MainV2.instance.adsblock)
{
foreach (adsb.PointLatLngAltHdg plla in MainV2.instance.adsbPlanes.Values)
{
// 30 seconds history
if (((DateTime)plla.Time) > DateTime.Now.AddSeconds(-30))
{
var adsbplane = new GMapMarkerADSBPlane(plla, plla.Heading)
{
ToolTipText = "ICAO: " + plla.Tag + "\n" +
"Alt: " + plla.Alt.ToString("0") + "\n" +
"Speed: " + plla.Speed.ToString("0") + "\n" +
"Heading: " + plla.Heading.ToString("0")
,
ToolTipMode = MarkerTooltipMode.OnMouseOver,
Tag = plla
};
if (plla.DisplayICAO)
{
adsbplane.ToolTipMode = MarkerTooltipMode.Always;
}
switch (plla.ThreatLevel)
{
case MAVLink.MAV_COLLISION_THREAT_LEVEL.NONE:
adsbplane.AlertLevel = GMapMarkerADSBPlane.AlertLevelOptions.Green;
break;
case MAVLink.MAV_COLLISION_THREAT_LEVEL.LOW:
adsbplane.AlertLevel = GMapMarkerADSBPlane.AlertLevelOptions.Orange;
break;
case MAVLink.MAV_COLLISION_THREAT_LEVEL.HIGH:
adsbplane.AlertLevel = GMapMarkerADSBPlane.AlertLevelOptions.Red;
break;
}
addMissionRouteMarker(adsbplane);
}
}
}
if (route.Points.Count > 0)
{
// add primary route icon
// draw guide mode point for only main mav
if (MainV2.comPort.MAV.cs.mode.ToLower() == "guided" && MainV2.comPort.MAV.GuidedMode.x != 0)
{
addpolygonmarker("Guided Mode", MainV2.comPort.MAV.GuidedMode.y / 1e7,
MainV2.comPort.MAV.GuidedMode.x / 1e7, (int)MainV2.comPort.MAV.GuidedMode.z,
Color.Blue,
routes);
}
// draw all icons for all connected mavs
foreach (var port in MainV2.Comports.ToArray())
{
// draw the mavs seen on this port
foreach (var MAV in port.MAVlist)
{
var marker = Common.getMAVMarker(MAV);
if (marker.Position.Lat == 0 && marker.Position.Lng == 0)
{
continue;
}
addMissionRouteMarker(marker);
}
}
if (route.Points.Count == 0 || route.Points[route.Points.Count - 1].Lat != 0 &&
(mapupdate.AddSeconds(3) < DateTime.Now) && CHK_autopan.IsToggled)
{
updateMapPosition(currentloc);
mapupdate = DateTime.Now;
}
if (route.Points.Count == 1 && gMapControl1.Zoom == 3) // 3 is the default load zoom
{
updateMapPosition(currentloc);
updateMapZoom(17);
}
}
prop.Update(MainV2.comPort.MAV.cs.HomeLocation, MainV2.comPort.MAV.cs.Location,
MainV2.comPort.MAV.cs.battery_kmleft);
prop.alt = MainV2.comPort.MAV.cs.alt;
prop.altasl = MainV2.comPort.MAV.cs.altasl;
prop.center = gMapControl1.Position;
gMapControl1.HoldInvalidation = false;
if (gMapControl1.Visible)
{
gMapControl1.Invalidate();
}
tracklast = DateTime.Now;
}
var ts = (DateTime.Now - start);
//Console.WriteLine("Hud update {0}", ts.TotalSeconds);
});
}
catch
{
}
}
}
public static List <PointLatLngAlt> CreateCorridor(List <PointLatLngAlt> polygon, double height, double camViewHeight, double camVertSpacing, double distance, double angle,
double camPitch, bool flipDirection, double bermDepth, int numBenches, double toeHeight, double toepoint, double toepoint_runs, bool pathHome, double homeAlt, FlightPlanner.altmode altmode)
{
int direction = (flipDirection == true ? -1 : 1);
if (camVertSpacing < 0.1)
{
camVertSpacing = 0.1;
}
if (polygon.Count == 0 || numBenches < 1)
{
return(new List <PointLatLngAlt>());
}
List <PointLatLngAlt> ans = new List <PointLatLngAlt>();
// utm zone distance calcs will be done in
int utmzone = polygon[0].GetUTMZone();
// utm position list
List <utmpos> utmpositions = utmpos.ToList(PointLatLngAlt.ToUTM(utmzone, polygon), utmzone);
double vertOffset = 0;
double horizOffset = 0;
double toepoint_runs_count = 0;
//calculate number of lanes with a starting altitude of half the calculated cam view height
// double initialAltitude = camViewHeight * Math.Sin(angle * deg2rad) / 3;
double initialAltitude = toepoint;
var vertIncrement = camVertSpacing * Math.Sin(angle * deg2rad);
var lanes = Math.Round((height - initialAltitude) / vertIncrement) + toepoint_runs + 1;
//repeat for each bench, applying height/berm depth offsets
for (int bench = 0; bench < numBenches; bench++)
{
//repeat for each increment up face
for (int lane = 0; lane < lanes; lane++)
{
if (toepoint_runs_count < toepoint_runs)
{
//calculate offset from the base of the face based on toe angle, camera pitch, camera overlap % and bench offset
vertOffset = distance * Math.Sin(camPitch * deg2rad) + (initialAltitude + (bench * height) + toeHeight);
horizOffset = distance * Math.Cos(camPitch * deg2rad) - ((initialAltitude) / Math.Tan(angle * deg2rad)) - bench * (bermDepth + height / Math.Tan(angle * deg2rad));
toepoint_runs_count++;
}
else
{
//calculate offset from the base of the face based on toe angle, camera pitch, camera overlap % and bench offset
vertOffset = distance * Math.Sin(camPitch * deg2rad) + (initialAltitude + ((lane - toepoint_runs) * vertIncrement) + (bench * height) + toeHeight);
horizOffset = distance * Math.Cos(camPitch * deg2rad) - ((initialAltitude + ((lane - toepoint_runs) * vertIncrement)) / Math.Tan(angle * deg2rad)) - bench * (bermDepth + height / Math.Tan(angle * deg2rad));
}
// THIS IS COMMENTED OUT BECAUSE IT'S INSANE NONSENSE THAT SHOULD NEVER HAVE BEEN IN HERE.
//
// //convert to absolute if flight planner is set to absolute mode (shift up by home alt)
// if (altmode == FlightPlanner.altmode.Absolute)
// {
// vertOffset += homeAlt;
// }
//if this is the first lane of a bench, climb to the altitude of the first waypoint of the lane before moving to the waypoint
if (lane == 0 && ans.Count > 0)
{
PointLatLngAlt intermediateWP = new PointLatLngAlt(ans.Last().Lat, ans.Last().Lng, vertOffset)
{
Tag = "S"
};
ans.Add(intermediateWP);
}
GenerateOffsetPath(utmpositions, horizOffset * direction, utmzone)
.ForEach(pnt => { ans.Add(pnt); ans.Last().Alt = vertOffset; });
//reverse the order of waypoints and direction of offset on the way back
utmpositions.Reverse();
direction = -direction;
}
}
//if an odd number of lanes were specified create one last run along the path back to home
if (pathHome && ((lanes * numBenches) % 2) == 1)
{
GenerateOffsetPath(utmpositions, horizOffset * direction, utmzone)
.ForEach(pnt => { ans.Add(pnt); ans.Last().Alt = vertOffset; ans.Last().Tag = "R"; });
}
return(ans);
}
private void domainUpDown1_ValueChanged(object sender, EventArgs e)
{
//if (CMB_camera.Text != "")
//{
// doCalc();
//}
Host2 = plugin.Host;
//grid = Utilities.Grid.CreateGrid(list, (double) NUM_altitude.Value, (double) NUM_Distance.Value,
// (double) NUM_spacing.Value, (double) NUM_angle.Value, (double) NUM_overshoot.Value,
// (double) NUM_overshoot2.Value,
// (Utilities.Grid.StartPosition) Enum.Parse(typeof(Utilities.Grid.StartPosition), CMB_startfrom.Text),
// false, 0, 0, plugin.Host.cs.HomeLocation);
grid = Utilities.Grid.CreateGrid1(list, (double)NUM_altitude.Value, (double)NUM_Distance.Value,
(double)NUM_spacing.Value, (double)num_offset.Value, (double)NUM_angle.Value, (double)NUM_overshoot.Value,
(double)NUM_overshoot2.Value,
(Utilities.Grid.StartPosition)Enum.Parse(typeof(Utilities.Grid.StartPosition), CMB_startfrom.Text),
false, 0, 0, plugin.Host.cs.HomeLocation);
List <PointLatLng> list2 = new List <PointLatLng>();
grid.ForEach(x => { list2.Add(x); });
map.HoldInvalidation = true;
layerpolygons.Polygons.Clear();
layerpolygons.Markers.Clear();
if (chk_boundary.Checked)
{
AddDrawPolygon();
}
if (grid.Count == 0)
{
map.ZoomAndCenterMarkers("polygons");
return;
}
int strips = 0;
int a = 1;
int m_n = 0;//tag = M 航点数量
PointLatLngAlt prevpoint = grid[0];
foreach (var item in grid)
{
if (item.Tag == "M")
{
if (CHK_internals.Checked)
{
layerpolygons.Markers.Add(new GMarkerGoogle(item, GMarkerGoogleType.green)
{
ToolTipText = a.ToString(), ToolTipMode = MarkerTooltipMode.OnMouseOver
});
a++;
m_n++;
}
}
else
{
if (item.Tag == "S" || item.Tag == "E")
{
strips++;
if (chk_markers.Checked)
{
layerpolygons.Markers.Add(new GMarkerGoogle(item, GMarkerGoogleType.green)
{
ToolTipText = a.ToString(),
ToolTipMode = MarkerTooltipMode.OnMouseOver
});
}
a++;
}
}
prevpoint = item;
}
// add wp polygon
wppoly = new GMapPolygon(list2, "Grid");
wppoly.Stroke.Color = Color.Yellow;
wppoly.Fill = Brushes.Transparent;
wppoly.Stroke.Width = 4;
if (chk_grid.Checked)
{
layerpolygons.Polygons.Add(wppoly);
}
Console.WriteLine("Poly Dist " + wppoly.Distance);
lbl_area.Text = (calcpolygonarea(list) / 1000000).ToString("f6") + " km^2";
if (chk_markers.Checked == true)
{
lbl_distance.Text = (wppoly.Distance + ((double)loiter_r.Value * Math.PI * 2 * a) / 1000).ToString("0.##") + " km";
}
else
{
lbl_distance.Text = (wppoly.Distance + ((double)loiter_r.Value * Math.PI * 2 * m_n) / 1000).ToString("0.##") + " km";
}
lbl_strips.Text = ((int)(strips / 2)).ToString();
lbl_distbetweenlines.Text = NUM_Distance.Value.ToString("0.##") + " m";
map.HoldInvalidation = false;
//map.ZoomAndCenterMarkers("polygons");
}
private void processLine(string line)
{
if (CHK_arducopter.Checked)
{
MainV2.comPort.MAV.cs.firmware = MainV2.Firmwares.ArduCopter2;
}
else
{
MainV2.comPort.MAV.cs.firmware = MainV2.Firmwares.ArduPlane;
}
try
{
Application.DoEvents();
line = line.Replace(", ", ",");
line = line.Replace(": ", ":");
string[] items = line.Split(',', ':');
if (items[0].Contains("CMD"))
{
if (flightdata.Count == 0)
{
if (int.Parse(items[2]) <= (int)MAVLink.MAV_CMD.LAST) // wps
{
PointLatLngAlt temp = new PointLatLngAlt(double.Parse(items[7], new System.Globalization.CultureInfo("en-US")) / 10000000, double.Parse(items[8], new System.Globalization.CultureInfo("en-US")) / 10000000, double.Parse(items[6], new System.Globalization.CultureInfo("en-US")) / 100, items[1].ToString());
cmd.Add(temp);
}
}
}
if (items[0].Contains("MOD"))
{
positionindex++;
modelist.Add(""); // i cant be bothered doing this properly
modelist.Add("");
modelist[positionindex] = (items[1]);
}
//GPS, 1, 15691, 10, 0.00, -35.3629379, 149.1650850, -0.08, 585.41, 0.00, 126.89
if (items[0].Contains("GPS") && items[2] == "1" && items[4] != "0" && items[4] != "-1" && lastline != line) // check gps line and fixed status
{
MainV2.comPort.MAV.cs.firmware = MainV2.Firmwares.ArduPlane;
if (position[positionindex] == null)
{
position[positionindex] = new List <Point3D>();
}
if (double.Parse(items[4], new System.Globalization.CultureInfo("en-US")) == 0)
{
return;
}
double alt = double.Parse(items[6], new System.Globalization.CultureInfo("en-US"));
if (items.Length == 11 && items[6] == "0.0000")
{
alt = double.Parse(items[7], new System.Globalization.CultureInfo("en-US"));
}
if (items.Length == 11 && items[6] == "0")
{
alt = double.Parse(items[7], new System.Globalization.CultureInfo("en-US"));
}
position[positionindex].Add(new Point3D(double.Parse(items[5], new System.Globalization.CultureInfo("en-US")), double.Parse(items[4], new System.Globalization.CultureInfo("en-US")), alt));
oldlastpos = lastpos;
lastpos = (position[positionindex][position[positionindex].Count - 1]);
lastline = line;
}
if (items[0].Contains("GPS") && items[4] != "0" && items[4] != "-1" && items.Length <= 9) // AC
{
MainV2.comPort.MAV.cs.firmware = MainV2.Firmwares.ArduCopter2;
if (position[positionindex] == null)
{
position[positionindex] = new List <Point3D>();
}
if (double.Parse(items[4], new System.Globalization.CultureInfo("en-US")) == 0)
{
return;
}
double alt = double.Parse(items[5], new System.Globalization.CultureInfo("en-US"));
position[positionindex].Add(new Point3D(double.Parse(items[4], new System.Globalization.CultureInfo("en-US")), double.Parse(items[3], new System.Globalization.CultureInfo("en-US")), alt));
oldlastpos = lastpos;
lastpos = (position[positionindex][position[positionindex].Count - 1]);
lastline = line;
}
//GPS, 1, 15691, 10, 0.00, -35.3629379, 149.1650850, -0.08, 585.41, 0.00, 126.89
if (items[0].Contains("GPS") && items[1] == "3" && items[4] != "0" && items[4] != "-1" && lastline != line) // check gps line and fixed status
{
if (position[positionindex] == null)
{
position[positionindex] = new List <Point3D>();
}
// if (double.Parse(items[4], new System.Globalization.CultureInfo("en-US")) == 0)
// return;
// 7 agl
// 8 asl...
double alt = double.Parse(items[8], new System.Globalization.CultureInfo("en-US"));
position[positionindex].Add(new Point3D(double.Parse(items[6], new System.Globalization.CultureInfo("en-US")), double.Parse(items[5], new System.Globalization.CultureInfo("en-US")), alt));
oldlastpos = lastpos;
lastpos = (position[positionindex][position[positionindex].Count - 1]);
lastline = line;
}
if (items[0].Contains("CTUN"))
{
ctunlast = items;
}
if (items[0].Contains("NTUN"))
{
ntunlast = items;
line = "ATT:" + double.Parse(ctunlast[3], new System.Globalization.CultureInfo("en-US")) * 100 + "," + double.Parse(ctunlast[6], new System.Globalization.CultureInfo("en-US")) * 100 + "," + double.Parse(items[1], new System.Globalization.CultureInfo("en-US")) * 100;
items = line.Split(',', ':');
}
if (items[0].Contains("ATT"))
{
try
{
if (lastpos.X != 0 && oldlastpos.X != lastpos.X && oldlastpos.Y != lastpos.Y)
{
Data dat = new Data();
try
{
dat.datetime = int.Parse(lastline.Split(',', ':')[2]);
}
catch { }
runmodel = new Model();
runmodel.Location.longitude = lastpos.X;
runmodel.Location.latitude = lastpos.Y;
runmodel.Location.altitude = lastpos.Z;
oldlastpos = lastpos;
runmodel.Orientation.roll = double.Parse(items[1], new System.Globalization.CultureInfo("en-US")) / -100;
runmodel.Orientation.tilt = double.Parse(items[2], new System.Globalization.CultureInfo("en-US")) / -100;
runmodel.Orientation.heading = double.Parse(items[3], new System.Globalization.CultureInfo("en-US")) / 100;
dat.model = runmodel;
dat.ctun = ctunlast;
dat.ntun = ntunlast;
flightdata.Add(dat);
}
}
catch { }
}
}
catch (Exception)
{
// if items is to short or parse fails.. ignore
}
}
private void BUT_Accept_Click(object sender, EventArgs e)
{
if (grid != null && grid.Count > 0)
{
MainV2.instance.FlightPlanner.quickadd = true;
PointLatLngAlt lastpnt = PointLatLngAlt.Zero;
if (MainV2.comPort.BaseStream.IsOpen)
{
homelat = MainV2.comPort.MAV.cs.lat;
homelong = MainV2.comPort.MAV.cs.lng;
}
else
{
homelat = double.Parse(MainV2.instance.FlightPlanner.TXT_homelat.Text);
homelong = double.Parse(MainV2.instance.FlightPlanner.TXT_homelng.Text);
}
//plugin.Host.AddWPtoList(MAVLink.MAV_CMD.DO_CHANGE_SPEED, 1,
// (int)((float)NUM_UpDownFlySpeed.Value / CurrentState.multiplierspeed), 0, 0, 0, 0, 0,
// null);
//double X, Y;
int n = 2;//计算第一个wp
if (chk_markers.Checked)
{
n = 1;
}
//var firstwp = grid[n];
//var firstwpxyz = GaussProjection.GaussProjCal(new BLHCoordinate(firstwp.Lat,firstwp.Lng,firstwp.Alt),CoordConsts.cgcs2000atum,0);
//var homexyz = GaussProjection.GaussProjCal(new BLHCoordinate(homelat,homelong,0), CoordConsts.cgcs2000atum, 0);
//double dist_hometofirstwp = squar(firstwpxyz.X - homexyz.X, firstwpxyz.Y - homexyz.Y);
//double dist_rat = (double)loiter_r.Value / dist_hometofirstwp;
////目标XY
// X = firstwpxyz.X - dist_rat * (firstwpxyz.X - homexyz.X);
// Y = firstwpxyz.Y - dist_rat * (firstwpxyz.Y - homexyz.Y);
//var targetwp = GaussProjection.GaussProjInvCal(new XYZCoordinate(X, Y, 0), CoordConsts.cgcs2000atum, (int)((firstwp.Lng - 1.5) / 3 + 1)*3);//根据第一个航点计算带号
//plugin.Host.AddWPtoList(MAVLink.MAV_CMD.WAYPOINT, 0, 0, 0, 0, targetwp.L,targetwp.B, grid[n].Alt);
//plugin.Host.AddWPtoList(MAVLink.MAV_CMD.DO_SET_CAM_TRIGG_DIST, (double)cam_dist.Value, 0, 0, 0, 0, 0, 0);
grid.ForEach(plla =>
{
if (plla.Tag == "M")
{
if (CHK_internals.Checked)
{
plugin.Host.AddWPtoList(MAVLink.MAV_CMD.WAYPOINT, 0, 0, 0, 0, plla.Lng, plla.Lat, 0);
}
}
else
{
if (!(plla.Lat == lastpnt.Lat && plla.Lng == lastpnt.Lng && plla.Alt == lastpnt.Alt) && chk_markers.Checked == true)
{
plugin.Host.AddWPtoList(MAVLink.MAV_CMD.WAYPOINT, 0, 0, 0, 0, plla.Lng, plla.Lat, 0);
}
lastpnt = plla;
}
});
//plugin.Host.AddWPtoList(MAVLink.MAV_CMD.DO_SET_CAM_TRIGG_DIST, 0, 0, 0, 0, 0, 0, 0);
MainV2.instance.FlightPlanner.quickadd = false;
MainV2.instance.FlightPlanner.writeKML();
savesettings();
this.Close();
Process.Start("http://127.0.0.1:56781/controlpoint.kml");
}
else
{
CustomMessageBox.Show("Bad Grid", "Error");
}
}
/// <summary>
/// Calc Distance in M
/// </summary>
/// <param name="p2"></param>
/// <returns>Distance in M</returns>
public double GetDistance(PointLatLngAlt p2)
{
double d = Lat * 0.017453292519943295;
double num2 = Lng * 0.017453292519943295;
double num3 = p2.Lat * 0.017453292519943295;
double num4 = p2.Lng * 0.017453292519943295;
double num5 = num4 - num2;
double num6 = num3 - d;
double num7 = Math.Pow(Math.Sin(num6 / 2.0), 2.0) + ((Math.Cos(d) * Math.Cos(num3)) * Math.Pow(Math.Sin(num5 / 2.0), 2.0));
double num8 = 2.0 * Math.Atan2(Math.Sqrt(num7), Math.Sqrt(1.0 - num7));
return (6371 * num8) * 1000.0; // M
}
private void domainUpDown1_ValueChanged(object sender, EventArgs e)
{
if (CMB_camera.Text != "")
{
doCalc();
}
// new grid system test
if (boxpoly == null || boxpoly.Points == null || boxpoly.Points.Count == 0)
{
return;
}
var newlist = new List <PointLatLngAlt>();
boxpoly.Points.ForEach(x => { newlist.Add(x); });
grid = Utilities.Grid.CreateGrid(newlist, (double)NUM_altitude.Value, (double)NUM_Distance,
(double)NUM_spacing, (double)NUM_angle.Value, 0, 0, Utilities.Grid.StartPosition.Home, false, 0, 0,
plugin.Host.cs.HomeLocation);
List <PointLatLng> list2 = new List <PointLatLng>();
grid.ForEach(x => { list2.Add(x); });
map.HoldInvalidation = true;
layerpolygons.Polygons.Clear();
layerpolygons.Markers.Clear();
layerpolygons.Polygons.Add(boxpoly);
if (grid.Count == 0)
{
return;
}
// if (chk_boundary.Checked)
// AddDrawPolygon();
int strips = 0;
int images = 0;
int a = 1;
PointLatLngAlt prevpoint = grid[0];
foreach (var item in grid)
{
if (item.Tag == "M")
{
images++;
if (chk_internals.Checked)
{
layerpolygons.Markers.Add(new GMarkerGoogle(item, GMarkerGoogleType.green)
{
ToolTipText = a.ToString(), ToolTipMode = MarkerTooltipMode.OnMouseOver
});
a++;
}
try
{
if (chk_footprints.Checked)
{
if (TXT_fovH != "")
{
double fovh = double.Parse(TXT_fovH);
double fovv = double.Parse(TXT_fovV);
double startangle = 0;
if (!RAD_camdirectionland.Checked)
{
startangle = 90;
}
double angle1 = startangle - (Math.Tan((fovv / 2.0) / (fovh / 2.0)) * rad2deg);
double dist1 = Math.Sqrt(Math.Pow(fovh / 2.0, 2) + Math.Pow(fovv / 2.0, 2));
double bearing = (double)NUM_angle.Value;
// (prevpoint.GetBearing(item) + 360.0) % 360;
List <PointLatLng> footprint = new List <PointLatLng>();
footprint.Add(item.newpos(bearing + angle1, dist1));
footprint.Add(item.newpos(bearing + 180 - angle1, dist1));
footprint.Add(item.newpos(bearing + 180 + angle1, dist1));
footprint.Add(item.newpos(bearing - angle1, dist1));
GMapPolygon poly = new GMapPolygon(footprint, a.ToString());
poly.Stroke.Color = Color.FromArgb(250 - ((a * 5) % 240), 250 - ((a * 3) % 240),
250 - ((a * 9) % 240));
poly.Stroke.Width = 1;
poly.Fill = new SolidBrush(Color.FromArgb(40, Color.Purple));
layerpolygons.Polygons.Add(poly);
}
}
}
catch { }
}
else
{
strips++;
if (chk_markers.Checked)
{
layerpolygons.Markers.Add(new GMarkerGoogle(item, GMarkerGoogleType.green)
{
ToolTipText = a.ToString(), ToolTipMode = MarkerTooltipMode.Always
});
}
a++;
}
prevpoint = item;
}
// add wp polygon
wppoly = new GMapPolygon(list2, "Grid");
wppoly.Stroke.Color = Color.Yellow;
wppoly.Fill = Brushes.Transparent;
wppoly.Stroke.Width = 4;
if (chk_grid.Checked)
{
layerpolygons.Polygons.Add(wppoly);
}
Console.WriteLine("Poly Dist " + wppoly.Distance);
quickViewarea.number = calcpolygonarea(list) / (1000.0 * 1000.0);
lbl_distance.Text = wppoly.Distance.ToString("0.##") + " km";
lbl_spacing.Text = NUM_spacing.ToString("#") + " m";
quickViewgroundres.number = TXT_cmpixel;
quickViewimagecount.number = images;
lbl_strips.Text = ((int)(strips / 2)).ToString();
lbl_distbetweenlines.Text = NUM_Distance.ToString("0.##") + " m";
lbl_footprint.Text = TXT_fovH + " x " + TXT_fovV + " m";
double seconds = ((wppoly.Distance * 1000.0) / ((double)numericUpDownFlySpeed.Value * 0.8));
// reduce flying speed by 20 %
label28.Text = secondsToNice(seconds);
quickViewflighttime.number = seconds / 60.0;
seconds = ((wppoly.Distance * 1000.0) / ((double)numericUpDownFlySpeed.Value));
label32.Text = secondsToNice(((double)NUM_spacing / (double)numericUpDownFlySpeed.Value));
map.HoldInvalidation = false;
map.ZoomAndCenterMarkers("polygons");
}
private void map_MouseMove(object sender, MouseEventArgs e)
{
var mousecurrent = map.FromLocalToLatLng(e.X, e.Y);
if (mousedown)
{
mousedragging = true;
if (currentmode == mode.panmode)
{
if (e.Button == MouseButtons.Left)
{
double latdif = mousestart.Lat - mousecurrent.Lat;
double lngdif = mousestart.Lng - mousecurrent.Lng;
try
{
map.Position = new PointLatLng(map.Position.Lat + latdif, map.Position.Lng + lngdif);
}
catch { }
}
}
else if (currentmode == mode.drawbox)
{
if (e.Button == MouseButtons.Left)
{
var rect = RectangleF.FromLTRB((float)mousestart.Lng, (float)mousestart.Lat, (float)mousecurrent.Lng, (float)mousecurrent.Lat);
list.Clear();
// tl
list.Add(mousestart);
// tr
list.Add(new PointLatLng(rect.Top, rect.Right));
// br
list.Add(mousecurrent);
// bl
list.Add(new PointLatLng(rect.Bottom, rect.Left));
if (boxpoly != null)
{
layerpolygons.Polygons.Remove(boxpoly);
}
boxpoly = null;
boxpoly = new GMapPolygon(list, "boxpoly");
boxpoly.IsHitTestVisible = true;
boxpoly.Stroke = new Pen(Color.Red, 2);
boxpoly.Fill = Brushes.Transparent;
layerpolygons.Polygons.Add(boxpoly);
map.Invalidate();
}
}
else if (currentmode == mode.movebox)
{
//if (mouseinsidepoly)
{
double latdif = mousestart.Lat - mousecurrent.Lat;
double lngdif = mousestart.Lng - mousecurrent.Lng;
for (int a = 0; a < boxpoly.Points.Count; a++)
{
boxpoly.Points[a] = new PointLatLng(boxpoly.Points[a].Lat - latdif, boxpoly.Points[a].Lng - lngdif);
}
UpdateListFromBox();
map.UpdatePolygonLocalPosition(boxpoly);
map.Invalidate();
mousestart = mousecurrent;
}
}
else if (currentmode == mode.editbox)
{
double latdif = mousestart.Lat - mousecurrent.Lat;
double lngdif = mousestart.Lng - mousecurrent.Lng;
// 2 point the create the lowest crosstrack distance
// extend at 90 degrees to the bearing of the points based on mouse position
PointLatLngAlt p0;
PointLatLngAlt p1;
PointLatLngAlt bestp0 = PointLatLngAlt.Zero;
PointLatLngAlt bestp1 = PointLatLngAlt.Zero;
double bestcrosstrack = 9999999;
double R = 6371000;
for (int a = 0; a < boxpoly.Points.Count; a++)
{
p0 = boxpoly.Points[a];
p1 = boxpoly.Points[(a + 1) % (boxpoly.Points.Count)];
var distp0p1 = p0.GetDistance(mousecurrent);
var bearingp0curr = p0.GetBearing(mousecurrent);
var bearringp0p1 = p0.GetBearing(p1);
var ct = Math.Asin(Math.Sin(distp0p1 / R) * Math.Sin((bearingp0curr - bearringp0p1) * deg2rad)) * R;
if (Math.Abs(ct) < Math.Abs(bestcrosstrack))
{
bestp0 = p0;
bestp1 = p1;
bestcrosstrack = ct;
}
}
var bearing = bestp0.GetBearing(bestp1);
layerpolygons.Markers.Clear();
layerpolygons.Markers.Add(new GMarkerGoogle(bestp0, GMarkerGoogleType.blue));
layerpolygons.Markers.Add(new GMarkerGoogle(bestp1, GMarkerGoogleType.blue));
bearing = ((PointLatLngAlt)mousestart).GetBearing(mousecurrent);
var newposp0 = bestp0.newpos(bearing, Math.Abs(bestcrosstrack));
var newposp1 = bestp1.newpos(bearing, Math.Abs(bestcrosstrack));
boxpoly.Points[boxpoly.Points.IndexOf(bestp0)] = newposp0;
boxpoly.Points[boxpoly.Points.IndexOf(bestp1)] = newposp1;
UpdateListFromBox();
map.UpdatePolygonLocalPosition(boxpoly);
map.Invalidate();
mousestart = mousecurrent;
}
}
mousedragging = false;
}
public void CreateOverlay(PointLatLngAlt home, List <Locationwp> missionitems, double wpradius, double loiterradius, double altunitmultiplier)
{
overlay.Clear();
GMapPolygon fencepoly = null;
double maxlat = -180;
double maxlong = -180;
double minlat = 180;
double minlong = 180;
int dolandstart = -1;
Func <MAVLink.MAV_FRAME, double, double, double> gethomealt = (altmode, lat, lng) =>
GetHomeAlt(altmode, home.Alt, lat, lng);
if (home != PointLatLngAlt.Zero)
{
home.Tag = "H";
pointlist.Add(home);
route.Add(pointlist[pointlist.Count - 1]);
addpolygonmarker("H", home.Lng, home.Lat, home.Alt * altunitmultiplier, null, 0);
}
for (int a = 0; a < missionitems.Count; a++)
{
var item = missionitems[a];
var itemnext = a + 1 < missionitems.Count ? missionitems[a + 1] : default(Locationwp);
ushort command = item.id;
// invalid locationwp
if (command == 0)
{
pointlist.Add(null);
continue;
}
// navigatable points
if (command < (ushort)MAVLink.MAV_CMD.LAST &&
command != (ushort)MAVLink.MAV_CMD.RETURN_TO_LAUNCH &&
command != (ushort)MAVLink.MAV_CMD.CONTINUE_AND_CHANGE_ALT &&
command != (ushort)MAVLink.MAV_CMD.DELAY &&
command != (ushort)MAVLink.MAV_CMD.GUIDED_ENABLE ||
command == (ushort)MAVLink.MAV_CMD.DO_SET_ROI || command == (ushort)MAVLink.MAV_CMD.DO_LAND_START)
{
// land can be 0,0 or a lat,lng
if (command == (ushort)MAVLink.MAV_CMD.LAND && item.lat == 0 && item.lng == 0)
{
continue;
}
if (command == (ushort)MAVLink.MAV_CMD.DO_LAND_START && item.lat != 0 && item.lng != 0)
{
pointlist.Add(new PointLatLngAlt(item.lat, item.lng,
item.alt + gethomealt((MAVLink.MAV_FRAME)item.frame, item.lat, item.lng),
(a + 1).ToString()));
route.Add(pointlist[pointlist.Count - 1]);
dolandstart = a;
// draw everything before
if (route.Count > 0)
{
RegenerateWPRoute(route, home, false);
route.Clear();
}
route.Add(pointlist[pointlist.Count - 1]);
addpolygonmarker((a + 1).ToString(), item.lng, item.lat,
item.alt * altunitmultiplier, null, wpradius);
}
else if (command == (ushort)MAVLink.MAV_CMD.LAND && item.lat != 0 && item.lng != 0)
{
pointlist.Add(new PointLatLngAlt(item.lat, item.lng,
item.alt + gethomealt((MAVLink.MAV_FRAME)item.frame, item.lat, item.lng),
(a + 1).ToString()));
route.Add(pointlist[pointlist.Count - 1]);
addpolygonmarker((a + 1).ToString(), item.lng, item.lat,
item.alt * altunitmultiplier, null, wpradius);
RegenerateWPRoute(route, home, false);
route.Clear();
}
else if (command == (ushort)MAVLink.MAV_CMD.DO_SET_ROI)
{
pointlist.Add(new PointLatLngAlt(item.lat, item.lng,
item.alt + gethomealt((MAVLink.MAV_FRAME)item.frame, item.lat, item.lng),
"ROI" + (a + 1))
{
color = Color.Red
});
// do set roi is not a nav command. so we dont route through it
//fullpointlist.Add(pointlist[pointlist.Count - 1]);
GMarkerGoogle m =
new GMarkerGoogle(new PointLatLng(item.lat, item.lng),
GMarkerGoogleType.red);
m.ToolTipMode = MarkerTooltipMode.Always;
m.ToolTipText = (a + 1).ToString();
m.Tag = (a + 1).ToString();
GMapMarkerRect mBorders = new GMapMarkerRect(m.Position);
{
mBorders.InnerMarker = m;
mBorders.Tag = "Dont draw line";
}
// check for clear roi, and hide it
if (m.Position.Lat != 0 && m.Position.Lng != 0)
{
// order matters
overlay.Markers.Add(m);
overlay.Markers.Add(mBorders);
}
}
else if (command == (ushort)MAVLink.MAV_CMD.LOITER_TIME ||
command == (ushort)MAVLink.MAV_CMD.LOITER_TURNS ||
command == (ushort)MAVLink.MAV_CMD.LOITER_UNLIM)
{
if (item.lat == 0 && item.lng == 0)
{
pointlist.Add(null);
// loiter at current location.
if (route.Count >= 1)
{
var lastpnt = route[route.Count - 1];
//addpolygonmarker((a + 1).ToString(), lastpnt.Lng, lastpnt.Lat,item.alt, Color.LightBlue, loiterradius);
}
}
else
{
pointlist.Add(new PointLatLngAlt(item.lat, item.lng,
item.alt + gethomealt((MAVLink.MAV_FRAME)item.frame, item.lat, item.lng),
(a + 1).ToString())
{
color = Color.LightBlue
});
// exit at tangent
if (item.p4 == 1)
{
var from = pointlist.Last();
var to = itemnext.lat != 0 && itemnext.lng != 0
? new PointLatLngAlt(itemnext)
{
Alt = itemnext.alt + gethomealt((MAVLink.MAV_FRAME)item.frame, item.lat,
item.lng)
}
: from;
var bearing = from.GetBearing(to);
var dist = from.GetDistance(to);
if (dist > loiterradius)
{
route.Add(pointlist[pointlist.Count - 1]);
var offset = from.newpos(bearing + 90, loiterradius);
route.Add(offset);
}
else
{
route.Add(pointlist[pointlist.Count - 1]);
}
}
else
{
route.Add(pointlist[pointlist.Count - 1]);
}
addpolygonmarker((a + 1).ToString(), item.lng, item.lat,
item.alt * altunitmultiplier, Color.LightBlue, loiterradius);
}
}
else if (command == (ushort)MAVLink.MAV_CMD.SPLINE_WAYPOINT)
{
pointlist.Add(new PointLatLngAlt(item.lat, item.lng,
item.alt + gethomealt((MAVLink.MAV_FRAME)item.frame, item.lat, item.lng),
(a + 1).ToString())
{
Tag2 = "spline"
});
route.Add(pointlist[pointlist.Count - 1]);
addpolygonmarker((a + 1).ToString(), item.lng, item.lat,
item.alt * altunitmultiplier, Color.Green, wpradius);
}
else if (command == (ushort)MAVLink.MAV_CMD.WAYPOINT && item.lat == 0 && item.lng == 0)
{
if (pointlist.Count > 0)
{
route.Add(pointlist[pointlist.Count - 1]);
}
pointlist.Add(null);
}
else
{
if (item.lat != 0 && item.lng != 0)
{
pointlist.Add(new PointLatLngAlt(item.lat, item.lng,
item.alt + gethomealt((MAVLink.MAV_FRAME)item.frame, item.lat, item.lng),
(a + 1).ToString()));
route.Add(pointlist[pointlist.Count - 1]);
addpolygonmarker((a + 1).ToString(), item.lng, item.lat,
item.alt * altunitmultiplier, null, wpradius);
}
else
{
pointlist.Add(null);
}
}
maxlong = Math.Max(item.lng, maxlong);
maxlat = Math.Max(item.lat, maxlat);
minlong = Math.Min(item.lng, minlong);
minlat = Math.Min(item.lat, minlat);
}
else if (command == (ushort)MAVLink.MAV_CMD.DO_JUMP) // fix do jumps into the future
{
pointlist.Add(null);
int wpno = (int)Math.Max(item.p1, 0);
int repeat = (int)item.p2;
List <PointLatLngAlt> list = new List <PointLatLngAlt>();
// cycle through reps
for (int repno = repeat; repno > 0; repno--)
{
// cycle through wps
for (int no = wpno; no <= a; no++)
{
if (pointlist[no] != null)
{
list.Add(pointlist[no]);
}
}
}
/*
* if (repeat == -1)
* {
* for (int wps = wpno; wps < missionitems.Count; wps++)
* {
* var newitem = missionitems[wps-1];
* if (newitem.lat == 0 && newitem.lng == 0 && newitem.id < (ushort)MAVLink.MAV_CMD.LAST)
* continue;
* list.Add((PointLatLngAlt) newitem);
* if (newitem.id == (ushort) MAVLink.MAV_CMD.LAND)
* {
* route.AddRange(list);
* RegenerateWPRoute(route, home, false);
* route.Clear();
* list.Clear();
* break;
* }
* }
* }
*/
route.AddRange(list);
}
else if (command == (ushort)MAVLink.MAV_CMD.FENCE_POLYGON_VERTEX_INCLUSION) // fence
{
if (fencepoly == null)
{
fencepoly = new GMapPolygon(new List <PointLatLng>(), a.ToString());
}
pointlist.Add(new PointLatLngAlt(item.lat, item.lng, 0, (a + 1).ToString()));
fencepoly.Points.Add(new PointLatLngAlt(item.lat, item.lng, 0, (a + 1).ToString()));
addpolygonmarker((a + 1).ToString(), item.lng, item.lat,
null, Color.Blue, 0, MAVLink.MAV_MISSION_TYPE.FENCE);
if (fencepoly.Points.Count == item.p1)
{
fencepoly.Fill = Brushes.Transparent;
fencepoly.Stroke = Pens.Pink;
overlay.Polygons.Add(fencepoly);
fencepoly = null;
}
}
else if (command == (ushort)MAVLink.MAV_CMD.FENCE_POLYGON_VERTEX_EXCLUSION) // fence
{
if (fencepoly == null)
{
fencepoly = new GMapPolygon(new List <PointLatLng>(), a.ToString());
}
pointlist.Add(new PointLatLngAlt(item.lat, item.lng, 0, (a + 1).ToString()));
fencepoly.Points.Add(new PointLatLngAlt(item.lat, item.lng, 0, (a + 1).ToString()));
addpolygonmarker((a + 1).ToString(), item.lng, item.lat, null, Color.Red, 0, MAVLink.MAV_MISSION_TYPE.FENCE);
if (fencepoly.Points.Count == item.p1)
{
fencepoly.Fill = new SolidBrush(Color.FromArgb(30, 255, 0, 0));
fencepoly.Stroke = Pens.Red;
overlay.Polygons.Add(fencepoly);
fencepoly = null;
}
}
else if (command == (ushort)MAVLink.MAV_CMD.FENCE_CIRCLE_EXCLUSION) // fence
{
pointlist.Add(new PointLatLngAlt(item.lat, item.lng, 0, (a + 1).ToString()));
addpolygonmarker((a + 1).ToString(), item.lng, item.lat,
null, Color.Red, item.p1, MAVLink.MAV_MISSION_TYPE.FENCE, Color.FromArgb(30, 255, 0, 0));
}
else if (command == (ushort)MAVLink.MAV_CMD.FENCE_CIRCLE_INCLUSION) // fence
{
pointlist.Add(new PointLatLngAlt(item.lat, item.lng, 0, (a + 1).ToString()));
addpolygonmarker((a + 1).ToString(), item.lng, item.lat,
null, Color.Blue, item.p1, MAVLink.MAV_MISSION_TYPE.FENCE);
}
else if (command == (ushort)MAVLink.MAV_CMD.FENCE_RETURN_POINT) // fence
{
pointlist.Add(new PointLatLngAlt(item.lat, item.lng, 0, (a + 1).ToString()));
addpolygonmarker((a + 1).ToString(), item.lng, item.lat,
null, Color.Orange, 0, MAVLink.MAV_MISSION_TYPE.FENCE);
}
else if (command >= (ushort)MAVLink.MAV_CMD.RALLY_POINT) // rally
{
pointlist.Add(new PointLatLngAlt(item.lat, item.lng, 0, (a + 1).ToString()));
addpolygonmarker((a + 1).ToString(), item.lng, item.lat,
null, Color.Orange, 0, MAVLink.MAV_MISSION_TYPE.RALLY);
}
else
{
pointlist.Add(null);
}
//a++;
}
RegenerateWPRoute(route, home);
}
private void goHereToolStripMenuItem_Click(object sender, EventArgs e)
{
if (!MainV2.comPort.BaseStream.IsOpen)
{
CustomMessageBox.Show("Please Connect First");
return;
}
string alt = "100";
if (MainV2.cs.firmware == MainV2.Firmwares.ArduCopter2)
{
alt = (10 * MainV2.cs.multiplierdist).ToString("0");
}
else
{
alt = (100 * MainV2.cs.multiplierdist).ToString("0");
}
if (DialogResult.Cancel == Common.InputBox("Enter Alt", "Enter Guided Mode Alt", ref alt))
return;
int intalt = (int)(100 * MainV2.cs.multiplierdist);
if (!int.TryParse(alt, out intalt))
{
CustomMessageBox.Show("Bad Alt");
return;
}
if (gotolocation.Lat == 0 || gotolocation.Lng == 0)
{
CustomMessageBox.Show("Bad Lat/Long");
return;
}
Locationwp gotohere = new Locationwp();
gotohere.id = (byte)MAVLink.MAV_CMD.WAYPOINT;
gotohere.alt = (float)(intalt / MainV2.cs.multiplierdist); // back to m
gotohere.lat = (float)(gotolocation.Lat);
gotohere.lng = (float)(gotolocation.Lng);
try
{
MainV2.giveComport = true;
MainV2.comPort.setWP(gotohere, 0, MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT, (byte)2);
GuidedModeWP = new PointLatLngAlt(gotohere.lat, gotohere.lng, gotohere.alt,"Guided Mode");
MainV2.giveComport = false;
}
catch (Exception ex) { MainV2.giveComport = false; CustomMessageBox.Show("Error sending command : " + ex.Message); }
}
public double GetBearing(PointLatLngAlt p2)
{
//http://www.movable-type.co.uk/scripts/latlong.html
double dLon = this.Lng - p2.Lng;
var y = Math.Sin(dLon) * Math.Cos(p2.Lat);
var x = Math.Cos(this.Lat) * Math.Sin(p2.Lat) -
Math.Sin(this.Lat) * Math.Cos(p2.Lat) * Math.Cos(dLon);
var brng = Math.Atan2(y, x) * rad2deg;
return brng;
}
