public static List<PointLatLngAlt> getAirports(PointLatLngAlt centerpoint)
{
log.Info("getAirports " + centerpoint);
// check if we have moved 66% from our last cache center point
if (currentcenter.GetDistance(centerpoint) < ((proximity/3)*2))
{
return cache;
}
log.Info("getAirports - regen list");
// generate a new list
currentcenter = centerpoint;
cache.Clear();
foreach (PointLatLngAlt item in airports)
{
if (item.GetDistance(centerpoint) < proximity)
{
cache.Add(item);
}
}
return cache;
}
public List<PointLatLngAlt> calcspline(PointLatLngAlt currentpos, PointLatLngAlt p1, PointLatLngAlt p2)
{
List<PointLatLngAlt> answer = new List<PointLatLngAlt>();
spline_t = spline_t_sq = 0.0f;
spline_p0 = currentpos;
spline_p1 = p1;
spline_p2 = p2;
spline_p0_prime = new Vector3();
double yaw = (currentpos.GetBearing(p1) + 360) % 360;
spline_p0_prime.x = .000001 * Math.Sin(yaw * deg2rad);
spline_p0_prime.y = .000001 * Math.Cos(yaw * deg2rad);
spline_p0_prime.z = spline_p1.z - spline_p0.z;
initialize_spline_segment();
int steps = 30;
foreach (var step in range(steps +1))
{
spline_t = (1f / steps) * step;
spline_t_sq = spline_t * spline_t;
evaluate_spline();
answer.Add(new PointLatLngAlt(spline_target.x, spline_target.y, spline_target.z, ""));
}
return answer;
}
void QueueSendGrid(object nothing)
{
issending = true;
// 8 across - 7 down
// cycle though the bitmask to check what we need to send (8*7)
for (byte i = 0; i < 56; i++)
{
// check to see if the ap requested this box.
if ((lastrequest.mask & ((ulong)1 << i)) > 0)
{
// get the requested lat and lon
double lat = lastrequest.lat / 1e7;
double lon = lastrequest.lon / 1e7;
// get the distance between grids
int bitgridspacing = lastrequest.grid_spacing * 4;
// get the new point, based on our current bit.
var newplla = new PointLatLngAlt(lat, lon).location_offset(bitgridspacing * (i % 8), bitgridspacing * (int)Math.Floor(i / 8.0));
// send a 4*4 grid, based on the lat lon of the bitmask
SendGrid(newplla.Lat, newplla.Lng, lastrequest.grid_spacing, i);
// 12hz = (43+6) * 12 = 588 bps
System.Threading.Thread.Sleep(1000/12);
}
}
issending = false;
}
void SendGrid(double lat, double lon, ushort grid_spacing, byte bit)
{
log.Info("SendGrid "+ lat + " " + lon + " space " + grid_spacing + " bit "+ bit);
MAVLink.mavlink_terrain_data_t resp = new MAVLink.mavlink_terrain_data_t();
resp.grid_spacing = grid_spacing;
resp.lat = lastrequest.lat;
resp.lon = lastrequest.lon;
resp.gridbit = bit;
resp.data = new short[16];
for (int i = 0; i < (4 * 4); i++)
{
int y = i % 4;
int x = i / 4;
PointLatLngAlt plla = new PointLatLngAlt(lat, lon).location_offset(x * grid_spacing, y * grid_spacing);
double alt = srtm.getAltitude(plla.Lat, plla.Lng);
resp.data[i] = (short)alt;
}
MainV2.comPort.sendPacket(resp);
}
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 utmpos(PointLatLngAlt pos)
{
double[] dd = pos.ToUTM(pos.GetUTMZone());
this.x = dd[0];
this.y = dd[1];
this.zone = pos.GetUTMZone();
this.Tag = null;
}
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);
// 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++;
}
}
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(Leader.MAV.cs.lat, Leader.MAV.cs.lng, Leader.MAV.cs.alt, "");
}
// pv_latlon_to_vector - convert lat/lon coordinates to a position vector
public Vector3 pv_location_to_vector(PointLatLngAlt loc)
{
PointLatLngAlt home = MainV2.comPort.MAV.cs.HomeLocation;
scaleLongDown = longitude_scale(home);
scaleLongUp = 1.0f / scaleLongDown;
Vector3 tmp = new Vector3((loc.Lat * 1.0e7f - home.Lat * 1.0e7f) * LATLON_TO_CM, (loc.Lng * 1.0e7f - home.Lng * 1.0e7f) * LATLON_TO_CM * scaleLongDown, loc.Alt * 100);
return tmp;
}
public static PointLatLngAlt ProjectPoint()
{
PointLatLngAlt currentlocation = new PointLatLngAlt(MainV2.comPort.MAV.cs.lat, MainV2.comPort.MAV.cs.lng);
double yawangle = MainV2.comPort.MAV.cs.campointc;
double rollangle = MainV2.comPort.MAV.cs.campointb;
double pitchangle = MainV2.comPort.MAV.cs.campointa;
if ((float) MainV2.comPort.MAV.param["MNT_TYPE"] == 4)
{
yawangle = MainV2.comPort.MAVlist[71].cs.yaw;
rollangle = MainV2.comPort.MAVlist[71].cs.roll;
pitchangle = MainV2.comPort.MAVlist[71].cs.pitch;
}
int distout = 10;
PointLatLngAlt newpos = PointLatLngAlt.Zero;
//dist = Math.Tan((90 + pitchangle) * deg2rad) * (MainV2.comPort.MAV.cs.alt);
while (distout < 1000)
{
// get a projected point to test intersection against - not using slope distance
PointLatLngAlt newposdist = currentlocation.newpos(yawangle + MainV2.comPort.MAV.cs.yaw, distout);
newposdist.Alt = srtm.getAltitude(newposdist.Lat, newposdist.Lng).alt;
// get another point 50 infront
PointLatLngAlt newposdist2 = currentlocation.newpos(yawangle + MainV2.comPort.MAV.cs.yaw, distout + 50);
newposdist2.Alt = srtm.getAltitude(newposdist2.Lat, newposdist2.Lng).alt;
// get the flat terrain distance out - at 0 alt
double distflat = Math.Tan((90 + pitchangle) * deg2rad) * (MainV2.comPort.MAV.cs.altasl);
// x is dist from plane, y is alt
var newpoint = FindLineIntersection(new PointF(0,MainV2.comPort.MAV.cs.altasl), new PointF((float)distflat, 0),
new PointF((float)distout, (float)newposdist.Alt), new PointF((float)distout + 50, (float)newposdist2.Alt));
if (newpoint.X != 0)
{
newpos = newposdist2;
break;
}
distout += 50;
}
//Console.WriteLine("pitch " + pitchangle.ToString("0.000") + " yaw " + yawangle.ToString("0.000") + " dist" + dist.ToString("0.000"));
//PointLatLngAlt newpos = currentlocation.newpos( yawangle + MainV2.comPort.MAV.cs.yaw, dist);
//Console.WriteLine(newpos);
return newpos;
}
public static void POIAdd(PointLatLngAlt Point, string tag)
{
// local copy
PointLatLngAlt pnt = Point;
pnt.Tag = tag + "\n" + pnt.ToString();
POI.POIs.Add(pnt);
if (POIModified != null)
POIModified(null, null);
}
public static void AddAirport(PointLatLngAlt plla)
{
// foreach (PointLatLngAlt item in airports)
{
// if (item.GetDistance(plla) < 200)
{
// return;
}
}
airports.Add(plla);
}
public static void POIDelete(PointLatLngAlt Point)
{
for (int a = 0; a < POI.POIs.Count; a++)
{
if (POI.POIs[a].Point() == Point)
{
POI.POIs.RemoveAt(a);
if (POIModified != null)
POIModified(null, null);
return;
}
}
}
public static void POIAdd(PointLatLngAlt Point)
{
if (Point == null)
return;
PointLatLngAlt pnt = Point;
string output = "";
if (DialogResult.OK != InputBox.Show("POI", "Enter ID", ref output))
return;
POIAdd(Point, output);
}
public PointLatLngAlt ToLLA()
{
IProjectedCoordinateSystem utm = ProjectedCoordinateSystem.WGS84_UTM(Math.Abs(zone), zone < 0 ? false : true);
ICoordinateTransformation trans = ctfac.CreateFromCoordinateSystems(wgs84, utm);
// get leader utm coords
double[] pll = trans.MathTransform.Inverse().Transform(this);
PointLatLngAlt ans = new PointLatLngAlt(pll[1], pll[0]);
if (this.Tag != null)
ans.Tag = this.Tag.ToString();
return ans;
}
public static List<PointLatLngAlt> GeneratePath(MAVState MAV)
{
List<PointLatLngAlt> result = new List<PointLatLngAlt>();
MAVLink.mavlink_mission_item_t? prevwp = null;
int a = -1;
foreach (var wp in MAV.wps.Values)
{
a++;
if (!prevwp.HasValue)
{
// change firstwp/aka home to valid alt
prevwp = new MAVLink.mavlink_mission_item_t?(new MAVLink.mavlink_mission_item_t() { x=wp.x,y = wp.y, z = 0});
continue;
}
if (wp.command != (byte)MAVLink.MAV_CMD.WAYPOINT && wp.command != (byte)MAVLink.MAV_CMD.SPLINE_WAYPOINT)
continue;
var wp1 = new PointLatLngAlt(prevwp.Value);
var wp2 = new PointLatLngAlt(wp);
var bearing = wp1.GetBearing(wp2);
var distwps = wp1.GetDistance(wp2);
var startalt = wp1.Alt;
var endalt = wp2.Alt;
if (startalt == 0)
{
startalt = endalt;
}
if(distwps > 5000)
continue;
for (double d = 0.1; d < distwps; d += 0.1)
{
var pnt = wp1.newpos(bearing, d);
pnt.Alt = startalt + (d/distwps) * (endalt-startalt);
result.Add(pnt);
}
prevwp = wp;
}
return result;
}
PointLatLngAlt FindTrailPnt(PointLatLngAlt from)
{
// get the start point for the distance
int start = trail.IndexOf(from);
for (int i = start+1; i < trail.Count; i++)
{
double dist = from.GetDistance(trail[i]); // 2d distance
if (dist > FollowDistance)
{
return trail[i];
}
}
return null;
}
public static void POIAdd(PointLatLngAlt Point)
{
PointLatLngAlt pnt = Point;
string output = "";
if (DialogResult.OK != InputBox.Show("POI", "Enter ID", ref output))
return;
pnt.Tag = output + "\n" + pnt.ToString();
POI.POIs.Add(pnt);
if (POIModified != null)
POIModified(null, null);
}
public static void POIEdit(PointLatLngAlt Point)
{
string output = "";
if (DialogResult.OK != InputBox.Show("POI", "Enter ID", ref output))
return;
for (int a = 0; a < POI.POIs.Count; a++)
{
if (POI.POIs[a].Point() == Point)
{
POI.POIs[a].Tag = output + "\n" + Point.ToString();
if (POIModified != null)
POIModified(null, null);
return;
}
}
}
public static void AddAirport(PointLatLngAlt plla)
{
lock (locker)
{
if (checkdups)
{
foreach (PointLatLngAlt item in airports)
{
if (item.GetDistance(plla) < 1000) // 1000m
{
return;
}
}
}
airports.Add(plla);
newairports = true;
}
}
bool ReceviedPacket(byte[] rawpacket)
{
if (rawpacket[5] == (byte)MAVLink.MAVLINK_MSG_ID.TERRAIN_REQUEST)
{
MAVLink.mavlink_terrain_request_t packet = rawpacket.ByteArrayToStructure<MAVLink.mavlink_terrain_request_t>();
lastrequest = packet;
log.Info("received TERRAIN_REQUEST " + packet.lat / 1e7 + " " + packet.lon / 1e7 + " space " + packet.grid_spacing + " " + Convert.ToString((long)packet.mask, 2));
// 8 across - 7 down
// cycle though the bitmask to check what we need to send (8*7)
for (byte i = 0; i < 56; i++)
{
// check to see if the ap requested this box.
if ((lastrequest.mask & ((ulong)1 << i)) > 0)
{
// get the requested lat and lon
double lat = lastrequest.lat / 1e7;
double lon = lastrequest.lon / 1e7;
// get the distance between grids
int bitgridspacing = lastrequest.grid_spacing * 4;
// get the new point, based on our current bit.
var newplla = new PointLatLngAlt(lat, lon).location_offset(bitgridspacing * (i % 8),bitgridspacing * (int)Math.Floor(i / 8.0));
// send a 4*4 grid, based on the lat lon of the bitmask
SendGrid(newplla.Lat, newplla.Lng, lastrequest.grid_spacing, i);
}
}
}
else if (rawpacket[5] == (byte)MAVLink.MAVLINK_MSG_ID.TERRAIN_REPORT)
{
MAVLink.mavlink_terrain_report_t packet = rawpacket.ByteArrayToStructure<MAVLink.mavlink_terrain_report_t>();
log.Info("received TERRAIN_REPORT " + packet.lat / 1e7 + " " + packet.lon / 1e7 + " " + packet.loaded + " " + packet.pending);
}
return false;
}
public static void ReadOurairports(string fn)
{
string[] lines = File.ReadAllLines(fn);
foreach (var line in lines)
{
string[] items = line.Split(',');
if (items.Length == 0)
continue;
if (items[0] == "\"id\"")
continue;
try
{
string name = items[3];
int latOffset = 0;
while (name[0] == '"' && name[name.Length - 1] != '"')
{
latOffset += 1;
name = name + "," + items[3 + latOffset];
}
name.Trim('"');
double lat = double.Parse(items[4 + latOffset].Trim('"'), CultureInfo.InvariantCulture);
double lng = double.Parse(items[5 + latOffset].Trim('"'), CultureInfo.InvariantCulture);
double alt = 0;
//double alt = double.Parse(items[6 + latOffset].Trim('"')) * 0.3048;
var newap = new PointLatLngAlt(lat, lng, alt, name);
airports.Add(newap);
Console.WriteLine(newap);
}
catch { }
}
}
public static List<PointLatLngAlt> getAirports(PointLatLngAlt centerpoint)
{
lock (locker)
{
DateTime start = DateTime.Now;
//log.Info("getAirports " + centerpoint);
// check if we have moved 66% from our last cache center point
if (currentcenter.GetDistance(centerpoint) < ((proximity / 3) * 2))
{
if (!newairports)
return cache;
}
newairports = false;
log.Info("getAirports - regen list");
// generate a new list
currentcenter = centerpoint;
cache.Clear();
foreach (PointLatLngAlt item in airports)
{
if (item.GetDistance(centerpoint) < proximity)
{
cache.Add(item);
}
}
log.Info("getAirports done " + (DateTime.Now - start).TotalSeconds + " sec");
return cache;
}
}
public void ProcessClient(object clientobj)
{
var client = clientobj as TcpClient;
using (client)
{
try
{
// Get a stream object for reading and writing
log.Info("Accepted Client " + client.Client.RemoteEndPoint.ToString());
//client.SendBufferSize = 100 * 1024; // 100kb
//client.LingerState.Enabled = true;
//client.NoDelay = true;
// makesure we have valid image
GCSViews.FlightData.myhud.streamjpgenable = true;
NetworkStream stream = client.GetStream();
// 5 seconds - default for httpd 2.2+
stream.ReadTimeout = 5000;
goto skipagain;
again:
log.Info("doing Again");
skipagain:
var asciiEncoding = new ASCIIEncoding();
var request = new byte[1024 * 4];
int len = 0;
// handle header
try
{
len = stream.Read(request, 0, request.Length);
}
catch
{
return;
}
string head = System.Text.Encoding.ASCII.GetString(request, 0, len);
log.Info(head);
int index = head.IndexOf('\n');
if (index == -1)
{
return;
}
string url = head.Substring(0, index - 1);
//url = url.Replace("\r", "");
//url = url.Replace("GET ","");
//url = url.Replace(" HTTP/1.0", "");
//url = url.Replace(" HTTP/1.1", "");
//Tracking.AddEvent("HTTPServer", "Get", url, "");
/////////////////////////////////////////////////////////////////
if (url.Contains(" /websocket/server"))
{
using (var writer = new StreamWriter(stream, Encoding.Default))
{
writer.WriteLine("HTTP/1.1 101 WebSocket Protocol Handshake");
writer.WriteLine("Upgrade: WebSocket");
writer.WriteLine("Connection: Upgrade");
//writer.WriteLine("WebSocket-Location: ws://localhost:56781/websocket/server");
int start = head.IndexOf("Sec-WebSocket-Key:") + 19;
int end = head.IndexOf('\r', start);
if (end == -1)
{
end = head.IndexOf('\n', start);
}
string accept = ComputeWebSocketHandshakeSecurityHash09(head.Substring(start, end - start));
writer.WriteLine("Sec-WebSocket-Accept: " + accept);
writer.WriteLine("Server: Mission Planner");
writer.WriteLine("");
writer.Flush();
while (client.Connected)
{
while (client.Available > 0)
{
var bydata = stream.ReadByte();
Console.Write(bydata.ToString("X2"));
if (bydata == 0x88)
{
return;
}
}
byte[] packet = new byte[1024 * 32];
var cs = JsonConvert.SerializeObject(MainV2.comPort.MAV.cs);
var wps = JsonConvert.SerializeObject(MainV2.comPort.MAV.wps);
foreach (var sendme in new[] { cs, wps })
{
int i = 0;
var tosend = sendme.Length;
packet[i++] = 0x81; // fin - utf
if (tosend <= 125)
{
packet[i++] = (byte)(tosend);
}
else
{
packet[i++] = 126; // nomask - 2 byte length
packet[i++] = (byte)(tosend >> 8);
packet[i++] = (byte)(tosend & 0xff);
}
foreach (char ch in sendme)
{
packet[i++] = (byte)ch;
}
stream.Write(packet, 0, i);
stream.Flush();
}
Thread.Sleep(200);
}
}
}
/////////////////////////////////////////////////////////////////
else if (url.Contains(" /websocket/raw"))
{
using (var writer = new StreamWriter(stream, Encoding.Default))
{
writer.WriteLine("HTTP/1.1 101 WebSocket Protocol Handshake");
writer.WriteLine("Upgrade: WebSocket");
writer.WriteLine("Connection: Upgrade");
writer.WriteLine("WebSocket-Location: ws://localhost:56781/websocket/raw");
int start = head.IndexOf("Sec-WebSocket-Key:") + 19;
int end = head.IndexOf('\r', start);
if (end == -1)
{
end = head.IndexOf('\n', start);
}
string accept = ComputeWebSocketHandshakeSecurityHash09(head.Substring(start, end - start));
writer.WriteLine("Sec-WebSocket-Accept: " + accept);
if (head.Contains("Sec-WebSocket-Protocol:"))
{
writer.WriteLine("Sec-WebSocket-Protocol: binary");
}
writer.WriteLine("Server: Mission Planner");
writer.WriteLine("");
writer.Flush();
EventHandler <MAVLink.MAVLinkMessage> action = null;
action = (sender, message) =>
{
var sendme = message.buffer;
try
{
byte[] packet = new byte[1024 * 32];
int i = 0;
var tosend = sendme.Length;
packet[i++] = 0x82; // fin - data
if (tosend <= 125)
{
packet[i++] = (byte)(tosend);
}
else
{
packet[i++] = 126; // nomask - 2 byte length
packet[i++] = (byte)(tosend >> 8);
packet[i++] = (byte)(tosend & 0xff);
}
foreach (char ch in sendme)
{
packet[i++] = (byte)ch;
}
stream.WriteAsync(packet, 0, i);
stream.FlushAsync();
}
catch
{
((MAVLinkInterface)sender).OnPacketReceived -= action;
stream.Close();
client.Close();
}
};
MainV2.comPort.OnPacketReceived += action;
while (client.Connected)
{
while (client.Available > 0)
{
var bydata = stream.ReadByte();
Console.Write(bydata.ToString("X2"));
if (bydata == 0x88)
{
return;
}
}
Thread.Sleep(200);
}
MainV2.comPort.OnPacketReceived -= action;
}
}
/////////////////////////////////////////////////////////////////
else if (url.Contains(" /georefnetwork.kml"))
{
byte[] buffer = Encoding.ASCII.GetBytes(georefkml);
string header =
"HTTP/1.1 200 OK\r\nServer: here\r\nKeep-Alive: timeout=15, max=100\r\nConnection: Keep-Alive\r\nCache-Control: no-cache\r\nContent-Type: application/vnd.google-earth.kml+xml\r\nX-Pad: avoid browser bug\r\nContent-Length: " +
buffer.Length + "\r\n\r\n";
byte[] temp = asciiEncoding.GetBytes(header);
stream.Write(temp, 0, temp.Length);
stream.Write(buffer, 0, buffer.Length);
stream.Flush();
goto again;
//stream.Close();
}
/////////////////////////////////////////////////////////////////
else if (url.Contains(" /location.kml"))
{
SharpKml.Dom.Document kml = new SharpKml.Dom.Document();
foreach (var mavLinkInterface in MainV2.Comports)
{
foreach (var MAV in mavLinkInterface.MAVlist)
{
SharpKml.Dom.Placemark pmplane = new SharpKml.Dom.Placemark();
pmplane.Name = "P/Q " + MAV.cs.altasl;
pmplane.Visibility = true;
SharpKml.Dom.Location loc = new SharpKml.Dom.Location();
loc.Latitude = MAV.cs.lat;
loc.Longitude = MAV.cs.lng;
loc.Altitude = MAV.cs.altasl;
if (loc.Altitude < 0)
{
loc.Altitude = 0.01;
}
SharpKml.Dom.Orientation ori = new SharpKml.Dom.Orientation();
ori.Heading = MAV.cs.yaw;
ori.Roll = -MAV.cs.roll;
ori.Tilt = -MAV.cs.pitch;
SharpKml.Dom.Scale sca = new SharpKml.Dom.Scale();
sca.X = 2;
sca.Y = 2;
sca.Z = 2;
SharpKml.Dom.Model model = new SharpKml.Dom.Model();
model.Location = loc;
model.Orientation = ori;
model.AltitudeMode = SharpKml.Dom.AltitudeMode.Absolute;
model.Scale = sca;
SharpKml.Dom.Link link = new SharpKml.Dom.Link();
link.Href = new Uri("block_plane_0.dae", UriKind.Relative);
model.Link = link;
pmplane.Geometry = model;
kml.AddFeature(pmplane);
}
}
SharpKml.Dom.LookAt la = new SharpKml.Dom.LookAt()
{
Altitude = MainV2.comPort.MAV.cs.altasl,
Latitude = MainV2.comPort.MAV.cs.lat,
Longitude = MainV2.comPort.MAV.cs.lng,
Tilt = 80,
Heading = MainV2.comPort.MAV.cs.yaw,
AltitudeMode = SharpKml.Dom.AltitudeMode.Absolute,
Range = 50
};
if (la.Latitude.Value != 0 && la.Longitude.Value != 0)
{
kml.Viewpoint = la;
}
SharpKml.Base.Serializer serializer = new SharpKml.Base.Serializer();
serializer.Serialize(kml);
byte[] buffer = Encoding.ASCII.GetBytes(serializer.Xml);
string header =
"HTTP/1.1 200 OK\r\nContent-Type: application/vnd.google-earth.kml+xml\r\nContent-Length: " +
buffer.Length + "\r\n\r\n";
byte[] temp = asciiEncoding.GetBytes(header);
stream.Write(temp, 0, temp.Length);
stream.Write(buffer, 0, buffer.Length);
goto again;
}
else if (url.Contains(" /network.kml"))
{
byte[] buffer = Encoding.ASCII.GetBytes(@"<?xml version=""1.0"" encoding=""UTF-8""?>
<kml xmlns=""http://www.opengis.net/kml/2.2"" xmlns:gx=""http://www.google.com/kml/ext/2.2"" xmlns:kml=""http://www.opengis.net/kml/2.2"" xmlns:atom=""http://www.w3.org/2005/Atom"">
<Folder>
<name> Network Links </name>
<open> 1 </open>
<NetworkLink>
<name> View Centered Placemark</name>
<open> 1 </open>
<refreshVisibility> 0 </refreshVisibility>
<flyToView> 1 </flyToView>
<Link>
<href> http://127.0.0.1:56781/location.kml</href>
<refreshMode> onInterval </refreshMode>
<refreshInterval> 1 </refreshInterval>
<viewRefreshTime> 1 </viewRefreshTime>
</Link>
</NetworkLink>
<NetworkLink>
<name> View Centered Placemark</name>
<open> 1 </open>
<refreshVisibility> 0 </refreshVisibility>
<flyToView> 0 </flyToView>
<Link>
<href> http://127.0.0.1:56781/wps.kml</href>
</Link>
</NetworkLink>
</Folder>
</kml>");
string header =
"HTTP/1.1 200 OK\r\nContent-Type: application/vnd.google-earth.kml+xml\r\nContent-Length: " +
buffer.Length + "\r\n\r\n";
byte[] temp = asciiEncoding.GetBytes(header);
stream.Write(temp, 0, temp.Length);
stream.Write(buffer, 0, buffer.Length);
stream.Flush();
goto again;
}
else if (url.Contains(" /wps.kml"))
{
SharpKml.Dom.Document kml = new SharpKml.Dom.Document();
SharpKml.Dom.CoordinateCollection coords = new SharpKml.Dom.CoordinateCollection();
PointLatLngAlt home = null;
// draw track
try
{
foreach (var point in GCSViews.FlightPlanner.instance.pointlist)
{
if (point == null)
{
continue;
}
if (point.Tag.ToLower().Contains("home"))
{
home = point;
}
coords.Add(new SharpKml.Base.Vector(point.Lat, point.Lng, point.Alt));
}
}
catch
{
}
var altmode = SharpKml.Dom.AltitudeMode.Absolute;
foreach (var point in GCSViews.FlightPlanner.instance.pointlist)
{
if (point == null)
{
continue;
}
SharpKml.Dom.Placemark wp = new SharpKml.Dom.Placemark();
wp.Name = "WP " + point.Tag + " Alt: " + point.Alt;
SharpKml.Dom.Point wppoint = new SharpKml.Dom.Point();
wppoint.AltitudeMode = altmode;
wppoint.Coordinate = new Vector()
{
Latitude = point.Lat,
Longitude = point.Lng,
Altitude = point.Alt
};
wp.Geometry = wppoint;
kml.AddFeature(wp);
}
SharpKml.Dom.LineString ls = new SharpKml.Dom.LineString();
ls.AltitudeMode = altmode;
ls.Coordinates = coords;
ls.Extrude = false;
ls.Tessellate = true;
Style style = new Style();
style.Id = "yellowLineGreenPoly";
unchecked {
style.Line = new LineStyle(new Color32((int)0xff00ffff), 4);
}
Style style2 = new Style();
style2.Id = "yellowLineGreenPoly";
style2.Line = new LineStyle(new Color32((int)0x7f00ffff), 4);
// above ground
SharpKml.Dom.Placemark pm = new SharpKml.Dom.Placemark()
{
Geometry = ls,
Name = "WPs",
StyleSelector = style
};
kml.AddFeature(pm);
// on ground
SharpKml.Dom.LineString ls2 = new SharpKml.Dom.LineString();
ls2.Coordinates = coords;
ls2.Extrude = false;
ls2.Tessellate = true;
ls2.AltitudeMode = SharpKml.Dom.AltitudeMode.ClampToGround;
SharpKml.Dom.Placemark pm2 = new SharpKml.Dom.Placemark()
{
Geometry = ls2,
Name = "onground",
StyleSelector = style2
};
kml.AddFeature(pm2);
SharpKml.Base.Serializer serializer = new SharpKml.Base.Serializer();
serializer.Serialize(kml);
byte[] buffer = Encoding.ASCII.GetBytes(serializer.Xml);
string header =
"HTTP/1.1 200 OK\r\nContent-Type: application/vnd.google-earth.kml+xml\r\nContent-Length: " +
buffer.Length + "\r\n\r\n";
byte[] temp = asciiEncoding.GetBytes(header);
stream.Write(temp, 0, temp.Length);
stream.Write(buffer, 0, buffer.Length);
stream.Flush();
goto again;
//stream.Close();
}
/////////////////////////////////////////////////////////////////
else if (url.Contains(" /hud.html"))
{
var file = Xamarin.Properties.Resources.hud;
string header = "HTTP/1.1 200 OK\r\nConnection: close\r\nContent-Type: text/html\r\nContent-Length: " + file.Length + "\r\n\r\n";
byte[] temp = asciiEncoding.GetBytes(header);
stream.Write(temp, 0, temp.Length);
byte[] buffer = ASCIIEncoding.ASCII.GetBytes(file);
stream.Write(buffer, 0, buffer.Length);
stream.Close();
}
/////////////////////////////////////////////////////////////////
else if (url.Contains(" /guided?"))
{
//http://127.0.0.1:56781/guided?lat=-34&lng=117.8&alt=30
Regex rex = new Regex(@"lat=([\-\.0-9]+)&lng=([\-\.0-9]+)&alt=([\.0-9]+)",
RegexOptions.IgnoreCase);
Match match = rex.Match(url);
if (match.Success)
{
Locationwp gwp = new Locationwp()
{
lat = double.Parse(match.Groups[1].Value),
lng = double.Parse(match.Groups[2].Value),
alt = float.Parse(match.Groups[3].Value)
};
try
{
MainV2.comPort.setGuidedModeWP(gwp);
}
catch
{
}
string header = "HTTP/1.1 200 OK\r\n\r\nSent Guide Mode Wp";
byte[] temp = asciiEncoding.GetBytes(header);
stream.Write(temp, 0, temp.Length);
}
else
{
string header = "HTTP/1.1 200 OK\r\n\r\nFailed Guide Mode Wp";
byte[] temp = asciiEncoding.GetBytes(header);
stream.Write(temp, 0, temp.Length);
}
stream.Close();
}
/////////////////////////////////////////////////////////////////
else if (url.ToLower().Contains("post /guide"))
{
Regex rex = new Regex(@"lat"":([\-\.0-9]+),""lon"":([\-\.0-9]+),""alt"":([\.0-9]+)",
RegexOptions.IgnoreCase);
Match match = rex.Match(head);
if (match.Success)
{
Locationwp gwp = new Locationwp()
{
lat = double.Parse(match.Groups[1].Value),
lng = double.Parse(match.Groups[2].Value),
alt = float.Parse(match.Groups[3].Value)
};
try
{
MainV2.comPort.setGuidedModeWP(gwp);
}
catch
{
}
string header = "HTTP/1.1 200 OK\r\n\r\nSent Guide Mode Wp";
byte[] temp = asciiEncoding.GetBytes(header);
stream.Write(temp, 0, temp.Length);
}
else
{
string header = "HTTP/1.1 200 OK\r\n\r\nFailed Guide Mode Wp";
byte[] temp = asciiEncoding.GetBytes(header);
stream.Write(temp, 0, temp.Length);
}
stream.Close();
}
/////////////////////////////////////////////////////////////////
else if (url.ToLower().Contains(" /command_long"))
{
string header = "HTTP/1.1 404 not found\r\nContent-Type: image/jpg\r\n\r\n";
byte[] temp = asciiEncoding.GetBytes(header);
stream.Write(temp, 0, temp.Length);
stream.Close();
}
/////////////////////////////////////////////////////////////////
else if (url.ToLower().Contains(" /rcoverride"))
{
string header = "HTTP/1.1 404 not found\r\nContent-Type: image/jpg\r\n\r\n";
byte[] temp = asciiEncoding.GetBytes(header);
stream.Write(temp, 0, temp.Length);
stream.Close();
}
/////////////////////////////////////////////////////////////////
else if (url.ToLower().Contains(" /get_mission"))
{
string header = "HTTP/1.1 404 not found\r\nContent-Type: image/jpg\r\n\r\n";
byte[] temp = asciiEncoding.GetBytes(header);
stream.Write(temp, 0, temp.Length);
stream.Close();
}
/////////////////////////////////////////////////////////////////
else if (url.ToLower().Contains(" /mavlink/"))
{
/*
* GET /mavlink/ATTITUDE+VFR_HUD+NAV_CONTROLLER_OUTPUT+META_WAYPOINT+GPS_RAW_INT+HEARTBEAT+META_LINKQUALITY+GPS_STATUS+STATUSTEXT+SYS_STATUS?_=1355828718540 HTTP/1.1
* Host: ubuntu:9999
* Connection: keep-alive
* X-Requested-With: XMLHttpRequest
* User-Agent: Mozilla/5.0 (Windows NT 6.1; WOW64) AppleWebKit/537.11 (KHTML, like Gecko) Chrome/23.0.1271.97 Safari/537.11
* Accept:
* Referer: http://ubuntu:9999/index.html
* Accept-Encoding: gzip,deflate,sdch
* Accept-Language: en-GB,en-US;q=0.8,en;q=0.6
* Accept-Charset: ISO-8859-1,utf-8;q=0.7,*;q=0.3
*
* HTTP/1.1 200 OK
* Content-Type: application/json
* Content-Length: 2121
* Date: Thu, 29 Nov 2012 12:13:38 GMT
* Server: ubuntu
*
* {
* "VFR_HUD": {"msg": {"throttle": 0, "groundspeed": 0.0, "airspeed": 0.0, "climb": 0.0, "mavpackettype": "VFR_HUD", "alt": -0.47999998927116394, "heading": 108}, "index": 687, "time_usec": 0},
* "STATUSTEXT": {"msg": {"mavpackettype": "STATUSTEXT", "severity": 1, "text": "Initialising APM..."}, "index": 2, "time_usec": 0},
* "SYS_STATUS": {"msg": {"onboard_control_sensors_present": 4294966287, "load": 0, "battery_remaining": -1, "errors_count4": 0, "drop_rate_comm": 0, "errors_count2": 0, "errors_count3": 0, "errors_comm": 0, "current_battery": -1, "errors_count1": 0, "onboard_control_sensors_health": 4294966287, "mavpackettype": "SYS_STATUS", "onboard_control_sensors_enabled": 4294945807, "voltage_battery": 10080}, "index": 693, "time_usec": 0},
* "META_LINKQUALITY": {"msg": {"master_in": 11110, "mav_loss": 0, "mavpackettype": "META_LINKQUALITY", "master_out": 194, "packet_loss": 0.0}, "index": 194, "time_usec": 0},
* "ATTITUDE": {"msg": {"pitchspeed": -0.000976863200776279, "yaw": 1.8878594636917114, "rollspeed": -0.0030046366155147552, "time_boot_ms": 194676, "pitch": -0.09986469894647598, "mavpackettype": "ATTITUDE", "yawspeed": -0.0015030358918011189, "roll": -0.029391441494226456}, "index": 687, "time_usec": 0},
* "GPS_RAW_INT": {"msg": {"fix_type": 1, "cog": 0, "epv": 65535, "lon": 0, "time_usec": 0, "eph": 9999, "satellites_visible": 0, "lat": 0, "mavpackettype": "GPS_RAW_INT", "alt": 137000, "vel": 0}, "index": 687, "time_usec": 0},
* "HEARTBEAT": {"msg": {"custom_mode": 0, "system_status": 4, "base_mode": 81, "autopilot": 3, "mavpackettype": "HEARTBEAT", "type": 2, "mavlink_version": 3}, "index": 190, "time_usec": 0},
* "GPS_STATUS": {"msg": {"satellite_snr": "", "satellite_azimuth": "", "satellite_prn": "", "satellite_elevation": "", "satellites_visible": 0, "satellite_used": "", "mavpackettype": "GPS_STATUS"}, "index": 2, "time_usec": 0},
* "NAV_CONTROLLER_OUTPUT": {"msg": {"wp_dist": 0, "nav_pitch": 0.0, "target_bearing": 0, "nav_roll": 0.0, "aspd_error": 0.0, "alt_error": 0.0, "mavpackettype": "NAV_CONTROLLER_OUTPUT", "xtrack_error": 0.0, "nav_bearing": 0}, "index": 687, "time_usec": 0}}
*/
object[] data = new object[20];
if (MainV2.comPort.MAV.getPacket((byte)MAVLink.MAVLINK_MSG_ID.ATTITUDE) != null)
{
var tmsg = MainV2.comPort.MAV.getPacket((uint)MAVLink.MAVLINK_MSG_ID.ATTITUDE)
.ToStructure <MAVLink.mavlink_attitude_t>();
var json = JsonConvert.SerializeObject(tmsg);
var name = MAVLink.MAVLINK_MESSAGE_INFOS.GetMessageInfo((uint)MAVLink.MAVLINK_MSG_ID.ATTITUDE).name;
}
Messagejson message = new Messagejson();
if (MainV2.comPort.MAV.getPacket((byte)MAVLink.MAVLINK_MSG_ID.ATTITUDE) != null)
{
message.ATTITUDE = new Message2()
{
index = 1,
msg =
MainV2.comPort.MAV.getPacket((byte)MAVLink.MAVLINK_MSG_ID.ATTITUDE)
.ToStructure <MAVLink.mavlink_attitude_t>()
}
}
;
if (MainV2.comPort.MAV.getPacket((byte)MAVLink.MAVLINK_MSG_ID.VFR_HUD) != null)
{
message.VFR_HUD = new Message2()
{
index = 1,
msg =
MainV2.comPort.MAV.getPacket((byte)MAVLink.MAVLINK_MSG_ID.VFR_HUD)
.ToStructure <MAVLink.mavlink_vfr_hud_t>()
}
}
;
if (MainV2.comPort.MAV.getPacket((byte)MAVLink.MAVLINK_MSG_ID.NAV_CONTROLLER_OUTPUT) != null)
{
message.NAV_CONTROLLER_OUTPUT = new Message2()
{
index = 1,
msg =
MainV2.comPort.MAV.getPacket((byte)MAVLink.MAVLINK_MSG_ID.NAV_CONTROLLER_OUTPUT)
.ToStructure <MAVLink.mavlink_nav_controller_output_t>()
}
}
;
if (MainV2.comPort.MAV.getPacket((byte)MAVLink.MAVLINK_MSG_ID.GPS_RAW_INT) != null)
{
message.GPS_RAW_INT = new Message2()
{
index = 1,
msg =
MainV2.comPort.MAV.getPacket((byte)MAVLink.MAVLINK_MSG_ID.GPS_RAW_INT)
.ToStructure <MAVLink.mavlink_gps_raw_int_t>()
}
}
;
if (MainV2.comPort.MAV.getPacket((byte)MAVLink.MAVLINK_MSG_ID.HEARTBEAT) != null)
{
message.HEARTBEAT = new Message2()
{
index = 1,
msg =
MainV2.comPort.MAV.getPacket((byte)MAVLink.MAVLINK_MSG_ID.HEARTBEAT)
.ToStructure <MAVLink.mavlink_heartbeat_t>()
}
}
;
if (MainV2.comPort.MAV.getPacket((byte)MAVLink.MAVLINK_MSG_ID.GPS_STATUS) != null)
{
message.GPS_STATUS = new Message2()
{
index = 1,
msg =
MainV2.comPort.MAV.getPacket((byte)MAVLink.MAVLINK_MSG_ID.GPS_STATUS)
.ToStructure <MAVLink.mavlink_gps_status_t>()
}
}
;
if (MainV2.comPort.MAV.getPacket((byte)MAVLink.MAVLINK_MSG_ID.STATUSTEXT) != null)
{
message.STATUSTEXT = new Message2()
{
index = 1,
msg =
MainV2.comPort.MAV.getPacket((byte)MAVLink.MAVLINK_MSG_ID.STATUSTEXT)
.ToStructure <MAVLink.mavlink_statustext_t>()
}
}
;
if (MainV2.comPort.MAV.getPacket((byte)MAVLink.MAVLINK_MSG_ID.SYS_STATUS) != null)
{
message.SYS_STATUS = new Message2()
{
index = 1,
msg =
MainV2.comPort.MAV.getPacket((byte)MAVLink.MAVLINK_MSG_ID.SYS_STATUS)
.ToStructure <MAVLink.mavlink_sys_status_t>()
}
}
;
message.META_LINKQUALITY =
message.SYS_STATUS =
new Message2()
{
index = packetindex,
time_usec = 0,
msg =
new META_LINKQUALITY()
{
master_in = (int)MainV2.comPort.MAV.packetsnotlost,
mavpackettype = "META_LINKQUALITY",
master_out = MainV2.comPort.packetcount,
packet_loss = 100 - MainV2.comPort.MAV.cs.linkqualitygcs,
mav_loss = 0
}
};
packetindex++;
string output = JsonConvert.SerializeObject(message);
string header = "HTTP/1.1 200 OK\r\nContent-Type: application/json\r\nContent-Length: " +
output.Length + "\r\n\r\n";
byte[] temp = asciiEncoding.GetBytes(header);
stream.Write(temp, 0, temp.Length);
temp = asciiEncoding.GetBytes(output);
stream.Write(temp, 0, temp.Length);
stream.Flush();
goto again;
//stream.Close();
}
/////////////////////////////////////////////////////////////////
else if (url.ToLower().Contains(" /mav/"))
{
//C:\Users\hog\Desktop\DIYDrones\mavelous\modules\lib\mavelous_web
Regex rex = new Regex(@"([^\s]+)\s(.+)\sHTTP/1", RegexOptions.IgnoreCase);
Match match = rex.Match(url);
if (match.Success)
{
string fileurl = WebUtility.UrlDecode(match.Groups[2].Value);
fileurl = fileurl.Replace("/mav/", "");
if (fileurl == "" || fileurl == "/")
{
fileurl = "index.html";
}
if (File.Exists(mavelous_web + fileurl))
{
string header = "HTTP/1.1 200 OK\r\n";
if (fileurl.Contains(".htm"))
{
header += "Content-Type: text/html\r\n";
}
else if (fileurl.Contains(".js"))
{
header += "Content-Type: application/x-javascript\r\n";
}
else if (fileurl.Contains(".css"))
{
header += "Content-Type: text/css\r\n";
}
else
{
header += "Content-Type: text/plain\r\n";
}
var fileinfo = new FileInfo(mavelous_web + fileurl);
var filetime = fileinfo.LastWriteTimeUtc.ToString("ddd, dd MMM yyyy HH:mm:ss") + " GMT";
var modified = Regex.Match(head, "If-Modified-Since:(.*)", RegexOptions.IgnoreCase);
if (modified.Success && modified.Groups[1].Value.Trim().ToLower() == filetime.ToLower())
{
string header2 = "HTTP/1.1 304 not modified\r\nConnection: Keep-Alive\r\n\r\n";
byte[] temp2 = asciiEncoding.GetBytes(header2);
stream.Write(temp2, 0, temp2.Length);
stream.Flush();
goto again;
}
header += "Connection: keep-alive\r\nLast-Modified: " + filetime + "\r\nContent-Length: " + fileinfo.Length + "\r\n\r\n";
byte[] temp = asciiEncoding.GetBytes(header);
stream.Write(temp, 0, temp.Length);
BinaryReader file =
new BinaryReader(File.Open(mavelous_web + fileurl, FileMode.Open, FileAccess.Read,
FileShare.Read));
byte[] buffer = new byte[1024];
while (file.BaseStream.Position < file.BaseStream.Length)
{
int leng = file.Read(buffer, 0, buffer.Length);
stream.Write(buffer, 0, leng);
}
file.Close();
stream.Flush();
goto again;
}
}
/////////////////////////////////////////////////////////////////
{
string header = "HTTP/1.1 404 not found\r\nConnection: Keep-Alive\r\nContent-Length: 0\r\nContent -Type: text/plain\r\n\r\n";
byte[] temp = asciiEncoding.GetBytes(header);
stream.Write(temp, 0, temp.Length);
stream.Flush();
goto again;
}
}
/////////////////////////////////////////////////////////////////
else if (url.ToLower().Contains(" / "))
{
Console.WriteLine(url);
string header = "HTTP/1.1 200 OK\r\nConnection: close\r\nContent-Type: text/html\r\n\r\n";
byte[] temp = asciiEncoding.GetBytes(header);
stream.Write(temp, 0, temp.Length);
string content = @"
<a href=/mav/>Mavelous</a>
<a href=/mavlink/>Mavelous traffic</a>
<a href=/hud.jpg>Hud image</a>
<a href=/map.jpg>Map image </a>
<a href=/both.jpg>Map & hud image</a>
<a href=/hud.html>hud html5</a>
<a href=/network.kml>network kml</a>
<a href=/georefnetwork.kml>georef kml</a>
";
temp = asciiEncoding.GetBytes(content);
stream.Write(temp, 0, temp.Length);
}
/////////////////////////////////////////////////////////////////
else
{
string header = "HTTP/1.1 404 not found\r\nContent-Type: text/plain\r\n\r\n";
byte[] temp = asciiEncoding.GetBytes(header);
stream.Write(temp, 0, temp.Length);
}
stream.Close();
log.Info("Close http " + url);
client.Close();
}
catch (Exception ee)
{
log.Error("Failed http ", ee);
}
}
}
public bool LoadFile(string filename)
{
FileName = filename;
log.InfoFormat("GeoTiff {0}", filename);
using (Tiff tiff = Tiff.Open(filename, "r"))
{
width = tiff.GetField(TiffTag.IMAGEWIDTH)[0].ToInt();
height = tiff.GetField(TiffTag.IMAGELENGTH)[0].ToInt();
bits = tiff.GetField(TiffTag.BITSPERSAMPLE)[0].ToInt();
//https://www.awaresystems.be/imaging/tiff/tifftags/sampleformat.html
type = tiff.GetField(TiffTag.SAMPLEFORMAT)[0].ToInt();
modelscale = tiff.GetField(TiffTag.GEOTIFF_MODELPIXELSCALETAG);
tiepoint = tiff.GetField(TiffTag.GEOTIFF_MODELTIEPOINTTAG);
var GeoKeyDirectoryTag = tiff.GetField((TiffTag)34735);
var KeyDirectoryVersion = BitConverter.ToUInt16(GeoKeyDirectoryTag[1].ToByteArray(), 0);
var KeyRevision = BitConverter.ToUInt16(GeoKeyDirectoryTag[1].ToByteArray(), 2);
var MinorRevision = BitConverter.ToUInt16(GeoKeyDirectoryTag[1].ToByteArray(), 4);
var NumberOfKeys = BitConverter.ToUInt16(GeoKeyDirectoryTag[1].ToByteArray(), 6);
ProjectedCSTypeGeoKey = 0;
for (int i = 8; i < 8 + NumberOfKeys * 8; i += 8)
{
var KeyID = BitConverter.ToUInt16(GeoKeyDirectoryTag[1].ToByteArray(), i);
var TIFFTagLocation = BitConverter.ToUInt16(GeoKeyDirectoryTag[1].ToByteArray(), i + 2);
var Count = BitConverter.ToUInt16(GeoKeyDirectoryTag[1].ToByteArray(), i + 4);
var Value_Offset = BitConverter.ToUInt16(GeoKeyDirectoryTag[1].ToByteArray(), i + 6);
log.InfoFormat("GeoKeyDirectoryTag ID={0} TagLoc={1} Count={2} Value/offset={3}", (GKID)KeyID, TIFFTagLocation,
Count, Value_Offset);
if (KeyID == (int)GKID.ProjectedCSTypeGeoKey)
{
ProjectedCSTypeGeoKey = Value_Offset;
}
if (KeyID == (int)GKID.GTRasterTypeGeoKey)
{
GTRasterTypeGeoKey = Value_Offset;
}
if (TIFFTagLocation != 0)
{
if (TIFFTagLocation == 34737)
{
var value = tiff.GetField((TiffTag)TIFFTagLocation)[1].ToByteArray().Skip(Value_Offset)
.Take(Count);
log.InfoFormat("GeoKeyDirectoryTag ID={0} Value={1}", (GKID)KeyID,
Encoding.ASCII.GetString(value.ToArray()));
}
if (TIFFTagLocation == 34736)
{
/*
* var value = tiff.GetField((TiffTag)TIFFTagLocation)[1].ToByteArray().Skip(Value_Offset*8)
* .Take(Count*8);
* log.InfoFormat("GeoKeyDirectoryTag ID={0} Value={1}", (GKID) KeyID, value);
*/
}
}
}
GeoAsciiParamsTag = tiff.GetField((TiffTag)34737);
if (GeoAsciiParamsTag != null && GeoAsciiParamsTag.Length == 2)
{
log.InfoFormat("GeoAsciiParamsTag {0}", GeoAsciiParamsTag[1]);
}
i = BitConverter.ToDouble(tiepoint[1].ToByteArray(), 0);
j = BitConverter.ToDouble(tiepoint[1].ToByteArray(), 0 + 8);
k = BitConverter.ToDouble(tiepoint[1].ToByteArray(), 0 + 16);
x = BitConverter.ToDouble(tiepoint[1].ToByteArray(), 0 + 24);
y = BitConverter.ToDouble(tiepoint[1].ToByteArray(), 0 + 32);
z = BitConverter.ToDouble(tiepoint[1].ToByteArray(), 0 + 40);
log.InfoFormat("Tie Point ({0},{1},{2}) --> ({3},{4},{5})", i, j, k, x, y, z);
xscale = BitConverter.ToDouble(modelscale[1].ToByteArray(), 0);
yscale = BitConverter.ToDouble(modelscale[1].ToByteArray(), 0 + 8);
zscale = BitConverter.ToDouble(modelscale[1].ToByteArray(), 0 + 16);
log.InfoFormat("Scale ({0},{1},{2})", xscale, yscale, zscale);
// wgs84 utm
if (ProjectedCSTypeGeoKey >= 32601 && ProjectedCSTypeGeoKey <= 32760)
{
if (ProjectedCSTypeGeoKey > 32700)
{
UTMZone = (ProjectedCSTypeGeoKey - 32700) * -1;
var pnt = PointLatLngAlt.FromUTM(UTMZone, x, y);
var pnt2 = PointLatLngAlt.FromUTM(UTMZone, x + width * xscale,
y + height * yscale);
var pnt3 = PointLatLngAlt.FromUTM(UTMZone, x + width * xscale, y);
var pnt4 = PointLatLngAlt.FromUTM(UTMZone, x, y + height * yscale);
ymin = Math.Min(Math.Min(Math.Min(pnt.Lat, pnt2.Lat), pnt3.Lat), pnt4.Lat);
xmin = Math.Min(Math.Min(Math.Min(pnt.Lng, pnt2.Lng), pnt3.Lng), pnt4.Lng);
ymax = Math.Max(Math.Max(Math.Max(pnt.Lat, pnt2.Lat), pnt3.Lat), pnt4.Lat);
xmax = Math.Max(Math.Max(Math.Max(pnt.Lng, pnt2.Lng), pnt3.Lng), pnt4.Lng);
}
if (ProjectedCSTypeGeoKey < 32700)
{
UTMZone = ProjectedCSTypeGeoKey - 32600;
var pnt = PointLatLngAlt.FromUTM(UTMZone, x, y);
var pnt2 = PointLatLngAlt.FromUTM(UTMZone, x + width * xscale,
y - height * yscale);
var pnt3 = PointLatLngAlt.FromUTM(UTMZone, x + width * xscale, y);
var pnt4 = PointLatLngAlt.FromUTM(UTMZone, x, y - height * yscale);
ymin = Math.Min(Math.Min(Math.Min(pnt.Lat, pnt2.Lat), pnt3.Lat), pnt4.Lat);
xmin = Math.Min(Math.Min(Math.Min(pnt.Lng, pnt2.Lng), pnt3.Lng), pnt4.Lng);
ymax = Math.Max(Math.Max(Math.Max(pnt.Lat, pnt2.Lat), pnt3.Lat), pnt4.Lat);
xmax = Math.Max(Math.Max(Math.Max(pnt.Lng, pnt2.Lng), pnt3.Lng), pnt4.Lng);
}
}
// etrs89 utm
if (ProjectedCSTypeGeoKey >= 3038 && ProjectedCSTypeGeoKey <= 3051)
{
UTMZone = ProjectedCSTypeGeoKey - 3012;
// 3038 - 26
var pnt = PointLatLngAlt.FromUTM(UTMZone, x, y);
var pnt2 = PointLatLngAlt.FromUTM(UTMZone, x + width * xscale,
y - height * yscale);
var pnt3 = PointLatLngAlt.FromUTM(UTMZone, x + width * xscale, y);
var pnt4 = PointLatLngAlt.FromUTM(UTMZone, x, y - height * yscale);
ymin = Math.Min(Math.Min(Math.Min(pnt.Lat, pnt2.Lat), pnt3.Lat), pnt4.Lat);
xmin = Math.Min(Math.Min(Math.Min(pnt.Lng, pnt2.Lng), pnt3.Lng), pnt4.Lng);
ymax = Math.Max(Math.Max(Math.Max(pnt.Lat, pnt2.Lat), pnt3.Lat), pnt4.Lat);
xmax = Math.Max(Math.Max(Math.Max(pnt.Lng, pnt2.Lng), pnt3.Lng), pnt4.Lng);
}
if (ProjectedCSTypeGeoKey >= 25828 && ProjectedCSTypeGeoKey <= 25838)
{
UTMZone = ProjectedCSTypeGeoKey - 25800;
// 3038 - 26
var pnt = PointLatLngAlt.FromUTM(UTMZone, x, y);
var pnt2 = PointLatLngAlt.FromUTM(UTMZone, x + width * xscale,
y - height * yscale);
var pnt3 = PointLatLngAlt.FromUTM(UTMZone, x + width * xscale, y);
var pnt4 = PointLatLngAlt.FromUTM(UTMZone, x, y - height * yscale);
ymin = Math.Min(Math.Min(Math.Min(pnt.Lat, pnt2.Lat), pnt3.Lat), pnt4.Lat);
xmin = Math.Min(Math.Min(Math.Min(pnt.Lng, pnt2.Lng), pnt3.Lng), pnt4.Lng);
ymax = Math.Max(Math.Max(Math.Max(pnt.Lat, pnt2.Lat), pnt3.Lat), pnt4.Lat);
xmax = Math.Max(Math.Max(Math.Max(pnt.Lng, pnt2.Lng), pnt3.Lng), pnt4.Lng);
}
Area = new RectLatLng(ymax, xmin, xmax - xmin, ymax - ymin);
log.InfoFormat("Coverage {0}", Area.ToString());
if (GTRasterTypeGeoKey == 1)
{
// starts from top left so x + y -
x += xscale / 2.0;
y -= yscale / 2.0;
}
log.InfoFormat("Start Point ({0},{1},{2}) --> ({3},{4},{5})", i, j, k, x, y, z);
lock (index)
GeoTiff.index.Add(this);
/*
*
* short numberOfDirectories = tiff.NumberOfDirectories();
* for (short d = 0; d < numberOfDirectories; ++d)
* {
* tiff.SetDirectory((short)d);
*
* for (ushort t = ushort.MinValue; t < ushort.MaxValue; ++t)
* {
* TiffTag tag = (TiffTag)t;
* FieldValue[] value = tiff.GetField(tag);
* if (value != null)
* {
* for (int j2 = 0; j2 < value.Length; j2++)
* {
* Console.WriteLine("{0} : {1} : {2}", tag.ToString(), value[j2].Value.GetType().ToString(), value[j2].ToString());
* }
* }
* }
* }
*/
}
return(true);
}
public static srtm.altresponce getAltitude(double lat, double lng, double zoom = 16)
{
lock (index)
if (index.Count == 0)
{
return(srtm.altresponce.Invalid);
}
var answer = new srtm.altresponce();
foreach (var geotiffdata in index.ToArray())
{
if (geotiffdata.Area.Contains(lat, lng))
{
// get answer
var xf = map(lat, geotiffdata.Area.Top, geotiffdata.Area.Bottom, 0, geotiffdata.height - 1);
var yf = map(lng, geotiffdata.Area.Left, geotiffdata.Area.Right, 0, geotiffdata.width - 1);
//wgs84 && etrs89
if (geotiffdata.ProjectedCSTypeGeoKey >= 3038 && geotiffdata.ProjectedCSTypeGeoKey <= 3051 ||
geotiffdata.ProjectedCSTypeGeoKey >= 32601 && geotiffdata.ProjectedCSTypeGeoKey <= 32760 ||
geotiffdata.ProjectedCSTypeGeoKey >= 25828 && geotiffdata.ProjectedCSTypeGeoKey <= 25838)
{
var pnt = PointLatLngAlt.ToUTM((geotiffdata.UTMZone) * 1, lat, lng);
xf = map(pnt[1], geotiffdata.y, geotiffdata.y - geotiffdata.height * geotiffdata.yscale, 0,
geotiffdata.height - 1);
yf = map(pnt[0], geotiffdata.x, geotiffdata.x + geotiffdata.width * geotiffdata.xscale, 0,
geotiffdata.width - 1);
}
int x_int = (int)xf;
double x_frac = xf - x_int;
int y_int = (int)yf;
double y_frac = yf - y_int;
//could be on one of the other images
if (x_int < 0 || y_int < 0 || x_int >= geotiffdata.width - 1 || y_int >= geotiffdata.height - 1)
{
continue;
}
double alt00 = GetAlt(geotiffdata, x_int, y_int);
double alt10 = GetAlt(geotiffdata, x_int + 1, y_int);
double alt01 = GetAlt(geotiffdata, x_int, y_int + 1);
double alt11 = GetAlt(geotiffdata, x_int + 1, y_int + 1);
double v1 = avg(alt00, alt10, x_frac);
double v2 = avg(alt01, alt11, x_frac);
double v = avg(v1, v2, y_frac);
if (v > -1000)
{
answer.currenttype = srtm.tiletype.valid;
}
if (alt00 < -1000 || alt10 < -1000 || alt01 < -1000 || alt11 < -1000)
{
answer.currenttype = srtm.tiletype.invalid;
}
answer.alt = v;
answer.altsource = "GeoTiff";
return(answer);
}
}
return(srtm.altresponce.Invalid);
}
public List <object> LocationToFence(List <Locationwp> list)
{
FencePolygon pol = new FencePolygon();
foreach (var locationwp in list)
{
var plla = new PointLatLngAlt(locationwp);
switch (locationwp.id)
{
case (ushort)MAVLink.MAV_CMD.FENCE_POLYGON_VERTEX_EXCLUSION:
{
pol.Mode = FencePolygon.PolyType.Exclusive;
pol.Points.Add(plla);
if (pol.Points.Count == locationwp.p1)
{
Fences.Add(pol);
pol = new FencePolygon();
}
}
break;
case (ushort)MAVLink.MAV_CMD.FENCE_POLYGON_VERTEX_INCLUSION:
{
pol.Mode = FencePolygon.PolyType.Inclusive;
pol.Points.Add(plla);
if (pol.Points.Count == locationwp.p1)
{
Fences.Add(pol);
pol = new FencePolygon();
}
}
break;
case (ushort)MAVLink.MAV_CMD.FENCE_CIRCLE_EXCLUSION:
{
var cir = new FenceCircle()
{
Mode = FenceCircle.PolyType.Exclusive,
Center = plla,
Radius = locationwp.p1
};
Fences.Add(cir);
}
break;
case (ushort)MAVLink.MAV_CMD.FENCE_CIRCLE_INCLUSION:
{
var cir = new FenceCircle()
{
Mode = FenceCircle.PolyType.Inclusive,
Center = plla,
Radius = locationwp.p1
};
Fences.Add(cir);
}
break;
case (ushort)MAVLink.MAV_CMD.FENCE_RETURN_POINT:
Fences.Add(new FenceReturn()
{
Return = plla
});
break;
default:
break;
}
}
return(Fences);
}
public static void StartTimer()
{
if (tmr.Enabled || timerRenable.Enabled)
{
return;
}
stop = false;
timerRenable.Interval = 60000;
timerRenable.Tick += (snd, obj) =>
{ tmr.Enabled = true; timerRenable.Enabled = false; };
tmr.Interval = 3000;
tmr.Tick += (snd, obj) =>
{
//if (MainV2.comPort == null || !MainV2.comPort.BaseStream.IsOpen) return;
if (MainV2.comPort.MAV == null || MainV2.comPort.MAV.cs == null)
{
return;
}
if (MainV2.comPort.MAV.cs.Location == null)
{
return;
}
if (MainV2.comPort.MAV.cs.Location.Lat == 0 && MainV2.comPort.MAV.cs.Location.Lng == 0)
{
return;
}
//if (MainV2.comPort.MAV.cs.gpsstatus <= 3) return;
System.ComponentModel.BackgroundWorker bgWorker = new System.ComponentModel.BackgroundWorker();
bool error = false;
tmr.Enabled = timerRenable.Enabled = false;
bgWorker.DoWork += (sender, obj2) =>
{
try
{
//http://public-api.adsbexchange.com/VirtualRadar/AircraftList.json?lat=33.433638&lng=-112.008113&fDstL=0&fDstU=37.04
using (var client = new System.Net.WebClient())
{
string request = string.Format("http://public-api.adsbexchange.com/VirtualRadar/AircraftList.json?lat={0}&lng={1}&fDstL=0&fDstU=37.04", MainV2.comPort.MAV.cs.Location.Lat, MainV2.comPort.MAV.cs.Location.Lng);
if (!string.IsNullOrEmpty(ldv))
{
request = string.Format("http://public-api.adsbexchange.com/VirtualRadar/AircraftList.json?lat={0}&lng={1}&fDstL=0&fDstU=37.04&ldv={2}", MainV2.comPort.MAV.cs.Location.Lat, MainV2.comPort.MAV.cs.Location.Lng, ldv);
}
var json = client.DownloadString(request);
var serializer = new System.Web.Script.Serialization.JavaScriptSerializer();
RootObject adsbexchange = serializer.Deserialize <RootObject>(json);
// TODO: do something with the model
//System.Diagnostics.Debug.WriteLine("Found " + adsbexchange.acList.Count + " aircraft within 20nm");
if (adsbexchange == null)
{
return;
}
if (!string.IsNullOrEmpty(adsbexchange.lastDv))
{
ldv = adsbexchange.lastDv;
}
lock (MainV2.instance.adsbPlanes)
{
float multi = Settings.Instance["distunits"] == "Feet" ? 3.28084f : 1f;
foreach (AcList vehicle in adsbexchange.acList)
{
string id = vehicle.Id.ToString();
//vehicle alt is always feet, if set to meters change
//GAlt is altitude adjusted for local air pressure
PointLatLngAlt vehicleplla = new PointLatLngAlt(vehicle.Lat, vehicle.Long, vehicle.GAlt * 3.2808399f);
double distance = MainV2.comPort.MAV.cs.Location.GetDistance(vehicleplla) * 0.000539957;
double altsep = vehicleplla.Alt - (MainV2.comPort.MAV.cs.altasl * multi);
if (MainV2.instance.adsbPlanes.ContainsKey(id))
{
// update existing
MainV2.instance.adsbPlanes[id].Lat = vehicle.Lat;
MainV2.instance.adsbPlanes[id].Lng = vehicle.Long;
MainV2.instance.adsbPlanes[id].Alt = vehicleplla.Alt;
MainV2.instance.adsbPlanes[id].Heading = (float)vehicle.Trak;
MainV2.instance.adsbPlanes[id].Time = DateTime.Now;
MainV2.instance.adsbPlanes[id].CallSign = vehicle.Call;
MainV2.instance.adsbPlanes[id].Tag = vehicle.Icao; // + Environment.NewLine + species + "/" + vehicle.Call + Environment.NewLine + vehicle.Mdl;
MainV2.instance.adsbPlanes[id].Vsi = vehicle.Vsi;
MainV2.instance.adsbPlanes[id].Species = (adsb.ADSBSpecies)vehicle.Species;
MainV2.instance.adsbPlanes[id].type = vehicle.Type;
}
else
{
// create new plane
MainV2.instance.adsbPlanes[id] =
new adsb.PointLatLngAltHdg(vehicle.Lat, vehicle.Long,
vehicleplla.Alt, (float)vehicle.Trak, id,
DateTime.Now)
{
CallSign = vehicle.Call,
Tag = vehicle.Icao,
DisplayICAO = true,
Vsi = vehicle.Vsi,
Species = (adsb.ADSBSpecies)vehicle.Species,
type = vehicle.Type
};
}
if (distance <= 2 && altsep <= 152.4 && altsep >= -152.4) //closer than 2NM and verticle seperation <= 500 feet
{
MainV2.instance.adsbPlanes[id].ThreatLevel = MAVLink.MAV_COLLISION_THREAT_LEVEL.HIGH;
}
else if (distance <= 5 && altsep <= 457.2 && altsep >= -457.2) //greater than 2NM but less than 5NM and verticle seperation <= 1500 feet
{
MainV2.instance.adsbPlanes[id].ThreatLevel = MAVLink.MAV_COLLISION_THREAT_LEVEL.LOW;
}
else
{
MainV2.instance.adsbPlanes[id].ThreatLevel = MAVLink.MAV_COLLISION_THREAT_LEVEL.NONE;
}
try
{
//send adsb to mav
if (MainV2.comPort.BaseStream.IsOpen)
{
MAVLink.mavlink_adsb_vehicle_t packet = new MAVLink.mavlink_adsb_vehicle_t();
packet.altitude = (int)(MainV2.instance.adsbPlanes[id].Alt * 1000);
packet.altitude_type = (byte)MAVLink.ADSB_ALTITUDE_TYPE.GEOMETRIC;
packet.callsign = ASCIIEncoding.ASCII.GetBytes(MainV2.instance.adsbPlanes[id].CallSign == null ? MainV2.instance.adsbPlanes[id].Tag : MainV2.instance.adsbPlanes[id].CallSign);
packet.emitter_type = (byte)MAVLink.ADSB_EMITTER_TYPE.NO_INFO;
packet.heading = (ushort)(MainV2.instance.adsbPlanes[id].Heading * 100);
packet.lat = (int)(MainV2.instance.adsbPlanes[id].Lat * 1e7);
packet.lon = (int)(MainV2.instance.adsbPlanes[id].Lng * 1e7);
packet.ICAO_address = uint.Parse(id, System.Globalization.NumberStyles.HexNumber);
packet.flags = (ushort)(MAVLink.ADSB_FLAGS.VALID_ALTITUDE | MAVLink.ADSB_FLAGS.VALID_COORDS |
MAVLink.ADSB_FLAGS.VALID_HEADING | MAVLink.ADSB_FLAGS.VALID_CALLSIGN);
//send to current connected
MainV2.comPort.sendPacket(packet, MainV2.comPort.MAV.sysid, MainV2.comPort.MAV.compid);
}
}
catch
{
}
}
foreach (var key in MainV2.instance.adsbPlanes.Keys)
{
adsb.PointLatLngAltHdg tmp;
if (!adsbexchange.acList.Any(x => x.Id.ToString() == key && x.Gnd == false))
{
MainV2.instance.adsbPlanes.TryRemove(key, out tmp);
}
}
}
}
}
catch (System.Net.WebException webException)
{
error = true;
timerRenable.Enabled = true;
}
catch { }
};
if (!error && !stop)
{
tmr.Enabled = true;
}
bgWorker.RunWorkerAsync();
};
tmr.Enabled = true;
}
public static PointLatLngAlt getIntersectionWithTerrain(PointLatLngAlt start, PointLatLngAlt end)
{
int distout = 0;
int stepsize = 50;
var maxdist = start.GetDistance(end);
var bearing = start.GetBearing(end);
var altdiff = end.Alt - start.Alt;
PointLatLngAlt newpos = PointLatLngAlt.Zero;
while (distout < maxdist)
{
// get a projected point to test intersection against - not using slope distance
PointLatLngAlt terrainstart = start.newpos(bearing, distout);
terrainstart.Alt = srtm.getAltitude(terrainstart.Lat, terrainstart.Lng).alt;
// get another point stepsize infront
PointLatLngAlt terrainend = start.newpos(bearing, distout + stepsize);
terrainend.Alt = srtm.getAltitude(terrainend.Lat, terrainend.Lng).alt;
// x is dist from start, y is alt
var newpoint = FindLineIntersection(new PointF(0, (float)start.Alt),
new PointF((float)maxdist, (float)end.Alt),
new PointF((float)distout, (float)terrainstart.Alt),
new PointF((float)distout + stepsize, (float)terrainend.Alt));
if (newpoint.X != 0)
{
newpos = start.newpos(bearing, newpoint.X);
newpos.Alt = newpoint.Y;
break;
}
distout += stepsize;
}
if (newpos == PointLatLngAlt.Zero)
{
newpos = end;
}
return(newpos);
}
private PointLatLngAlt get_last_nav_coords(PointLatLngAlt lta_point, PointLatLngAlt land_point)
{
double bearing = land_point.GetBearing(lta_point);
return(land_point.newpos(bearing, m_hwp_radius * 0.4f));
}
public static PointLatLngAlt ProjectPoint()
{
MainV2.comPort.GetMountStatus();
// this should be looking at rc_channel function
yawchannel = (int)(float)MainV2.comPort.MAV.param["MNT_RC_IN_PAN"];
pitchchannel = (int)(float)MainV2.comPort.MAV.param["MNT_RC_IN_TILT"];
rollchannel = (int)(float)MainV2.comPort.MAV.param["MNT_RC_IN_ROLL"];
//if (!MainV2.comPort.BaseStream.IsOpen)
// return PointLatLngAlt.Zero;
PointLatLngAlt currentlocation = new PointLatLngAlt(MainV2.comPort.MAV.cs.lat, MainV2.comPort.MAV.cs.lng);
double yawangle = MainV2.comPort.MAV.cs.campointc;
double rollangle = MainV2.comPort.MAV.cs.campointb;
double pitchangle = MainV2.comPort.MAV.cs.campointa;
//
if ((float) MainV2.comPort.MAV.param["MNT_TYPE"] == 4)
{
yawangle = MainV2.comPort.MAVlist[71].cs.yaw;
rollangle = MainV2.comPort.MAVlist[71].cs.roll;
pitchangle = MainV2.comPort.MAVlist[71].cs.pitch;
}
if (Math.Abs(rollangle) > 180 || yawangle == 0 && pitchangle == 0)
{
yawangle = ConvertPwmtoAngle(axis.yaw);
//rollangle = ConvertPwmtoAngle(axis.roll);
pitchangle = ConvertPwmtoAngle(axis.pitch) + MainV2.comPort.MAV.cs.pitch;
pitchangle -= Math.Sin(yawangle * deg2rad) * MainV2.comPort.MAV.cs.roll;
}
// tan (o/a)
// todo account for ground elevation change.
int distout = 10;
PointLatLngAlt newpos = PointLatLngAlt.Zero;
//dist = Math.Tan((90 + pitchangle) * deg2rad) * (MainV2.comPort.MAV.cs.alt);
while (distout < 1000)
{
// get a projected point to test intersection against - not using slope distance
PointLatLngAlt newposdist = currentlocation.newpos(yawangle + MainV2.comPort.MAV.cs.yaw, distout);
newposdist.Alt = srtm.getAltitude(newposdist.Lat, newposdist.Lng).alt;
// get another point 50 infront
PointLatLngAlt newposdist2 = currentlocation.newpos(yawangle + MainV2.comPort.MAV.cs.yaw, distout + 50);
newposdist2.Alt = srtm.getAltitude(newposdist2.Lat, newposdist2.Lng).alt;
// get the flat terrain distance out - at 0 alt
double distflat = Math.Tan((90 + pitchangle) * deg2rad) * (MainV2.comPort.MAV.cs.altasl);
// x is dist from plane, y is alt
var newpoint = FindLineIntersection(new PointF(0,MainV2.comPort.MAV.cs.altasl), new PointF((float)distflat, 0),
new PointF((float)distout, (float)newposdist.Alt), new PointF((float)distout + 50, (float)newposdist2.Alt));
if (newpoint.X != 0)
{
newpos = newposdist2;
break;
}
distout += 50;
}
//Console.WriteLine("pitch " + pitchangle.ToString("0.000") + " yaw " + yawangle.ToString("0.000") + " dist" + dist.ToString("0.000"));
//PointLatLngAlt newpos = currentlocation.newpos( yawangle + MainV2.comPort.MAV.cs.yaw, dist);
//Console.WriteLine(newpos);
return newpos;
}
public void SetupM8P(ICommsSerial port, PointLatLngAlt basepos, int surveyindur = 0, double surveyinacc = 0, bool m8p_130plus = false)
{
port.BaseStream.Flush();
port.BaudRate = 9600;
System.Threading.Thread.Sleep(100);
// port config - 115200 - uart1
var packet = generate(0x6, 0x00, new byte[] { 0x01, 0x00, 0x00, 0x00, 0xD0, 0x08, 0x00, 0x00, 0x00, 0xC2,
0x01, 0x00, 0x23, 0x00, 0x23, 0x00, 0x00, 0x00, 0x00, 0x00 });
port.Write(packet, 0, packet.Length);
System.Threading.Thread.Sleep(300);
// port config - usb
packet = generate(0x6, 0x00, new byte[] { 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x23, 0x00, 0x23, 0x00, 0x00, 0x00, 0x00, 0x00 });
port.Write(packet, 0, packet.Length);
System.Threading.Thread.Sleep(300);
port.BaseStream.Flush();
port.BaudRate = 115200;
// port config - 115200 - uart1
packet = generate(0x6, 0x00, new byte[] { 0x01, 0x00, 0x00, 0x00, 0xD0, 0x08, 0x00, 0x00, 0x00, 0xC2,
0x01, 0x00, 0x23, 0x00, 0x23, 0x00, 0x00, 0x00, 0x00, 0x00 });
port.Write(packet, 0, packet.Length);
System.Threading.Thread.Sleep(300);
// port config - usb
packet = generate(0x6, 0x00, new byte[] { 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x23, 0x00, 0x23, 0x00, 0x00, 0x00, 0x00, 0x00 });
port.Write(packet, 0, packet.Length);
System.Threading.Thread.Sleep(300);
// set rate to 1hz
packet = generate(0x6, 0x8, new byte[] { 0xE8, 0x03, 0x01, 0x00, 0x01, 0x00 });
port.Write(packet, 0, packet.Length);
System.Threading.Thread.Sleep(200);
// set navmode to stationary
packet = generate(0x6, 0x24, new byte[] { 0xFF, 0xFF, 0x02, 0x03, 0x00, 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00
, 0xFA, 0x00, 0xFA, 0x00, 0x64, 0x00, 0x2C, 0x01, 0x00, 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
, 0x00, 0x00 });
port.Write(packet, 0, packet.Length);
System.Threading.Thread.Sleep(200);
// turn off all nmea
for (int a = 0; a <= 0xf; a++)
{
if (a == 0xb || a == 0xc || a == 0xe)
{
continue;
}
turnon_off(port, 0xf0, (byte)a, 0);
}
// mon-ver
poll_msg(port, 0xa, 0x4);
// surveyin msg - for feedback
turnon_off(port, 0x01, 0x3b, 1);
// pvt msg - for feedback
turnon_off(port, 0x01, 0x07, 1);
// 1005 - 5s
turnon_off(port, 0xf5, 0x05, 5);
byte rate1 = 1;
byte rate2 = 0;
if (m8p_130plus)
{
rate1 = 0;
rate2 = 1;
}
// 1074 - 1s
turnon_off(port, 0xf5, 0x4a, rate2);
// 1077 - 1s
turnon_off(port, 0xf5, 0x4d, rate1);
// 1084 - 1s
turnon_off(port, 0xf5, 0x54, rate2);
// 1087 - 1s
turnon_off(port, 0xf5, 0x57, rate1);
// 1124 - 1s
turnon_off(port, 0xf5, 0x7c, rate2);
// 1127 - 1s
turnon_off(port, 0xf5, 0x7f, rate1);
// 1230 - 5s
turnon_off(port, 0xf5, 0xE6, 5);
// rxm-raw/rawx - 1s
turnon_off(port, 0x02, 0x15, 1);
turnon_off(port, 0x02, 0x10, 1);
// rxm-sfrb/sfrb - 2s
turnon_off(port, 0x02, 0x13, 2);
turnon_off(port, 0x02, 0x11, 2);
System.Threading.Thread.Sleep(100);
System.Threading.Thread.Sleep(100);
if (basepos == PointLatLngAlt.Zero)
{
// survey in config - 60s and < 2m
packet = generate(0x6, 0x71, new ubx_cfg_tmode3((uint)surveyindur, surveyinacc));
port.Write(packet, 0, packet.Length);
}
else
{
byte[] data = new ubx_cfg_tmode3(basepos.Lat, basepos.Lng, basepos.Alt);
packet = generate(0x6, 0x71, data);
port.Write(packet, 0, packet.Length);
}
System.Threading.Thread.Sleep(100);
}
public void SetupM8P(ICommsSerial port, PointLatLngAlt basepos, int surveyindur = 0, double surveyinacc = 0)
{
// port config - 115200 - uart1
var packet = generate(0x6, 0x00, new byte[] { 0x01, 0x00, 0x00, 0x00, 0xD0, 0x08, 0x00, 0x00, 0x00, 0xC2,
0x01, 0x00, 0x23, 0x00, 0x23, 0x00, 0x00, 0x00, 0x00, 0x00 });
port.Write(packet, 0, packet.Length);
System.Threading.Thread.Sleep(100);
// port config - usb
packet = generate(0x6, 0x00, new byte[] { 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x23, 0x00, 0x23, 0x00, 0x00, 0x00, 0x00, 0x00 });
port.Write(packet, 0, packet.Length);
System.Threading.Thread.Sleep(100);
// set rate to 1hz
packet = generate(0x6, 0x8, new byte[] { 0xE8, 0x03, 0x01, 0x00, 0x01, 0x00 });
port.Write(packet, 0, packet.Length);
System.Threading.Thread.Sleep(100);
// turn off all nmea
for (int a = 0; a <= 0xf; a++)
{
turnon_off(port, 0xf0, (byte)a, 0);
}
// surveyin msg - for feedback
turnon_off(port, 0x01, 0x3b, 1);
// pvt msg - for feedback
turnon_off(port, 0x01, 0x07, 1);
// 1005 - 5s
turnon_off(port, 0xf5, 0x05, 5);
// 1077 - 1s
turnon_off(port, 0xf5, 0x4d, 1);
// 1087 - 1s
turnon_off(port, 0xf5, 0x57, 1);
// rxm-raw/rawx - 1s
turnon_off(port, 0x02, 0x15, 1);
turnon_off(port, 0x02, 0x10, 1);
// rxm-sfrb/sfrb - 5s
turnon_off(port, 0x02, 0x13, 5);
turnon_off(port, 0x02, 0x11, 5);
System.Threading.Thread.Sleep(100);
System.Threading.Thread.Sleep(100);
if (basepos == PointLatLngAlt.Zero)
{
// survey in config - 60s and < 2m
packet = generate(0x6, 0x71, new ubx_cfg_tmode3(60, 2));
port.Write(packet, 0, packet.Length);
}
else
{
byte[] data = new ubx_cfg_tmode3(basepos.Lat, basepos.Lng, basepos.Alt);
packet = generate(0x6, 0x71, data);
port.Write(packet, 0, packet.Length);
}
System.Threading.Thread.Sleep(100);
}
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 = MAV.compid;
fp.target_system = MAV.sysid;
int retry = 3;
PointLatLngAlt newfp;
while (retry > 0)
{
generatePacket((byte) MAVLINK_MSG_ID.FENCE_POINT, fp);
int counttemp = 0;
newfp = getFencePoint(fp.idx, ref counttemp);
if (newfp.GetDistance(plla) < 5)
return true;
retry--;
}
throw new Exception("Could not verify GeoFence Point");
}
private PointLatLngAlt get_loiter_coords(PointLatLngAlt wp_point, PointLatLngAlt land_point)
{
double bearing = land_point.GetBearing(wp_point);
return(land_point.newpos(bearing, m_hwp_radius));
}
//http://www.chrobotics.com/library/understanding-euler-angles
// x = north / roll
// y = pitch / east
// z = yaw / down
public static List <PointLatLngAlt> calc(PointLatLngAlt plla, double R, double P, double Y, double hfov, double vfov)
{
// alt should be asl
// P pitch where the center os pointing ie -80
// R roll
GMapPolygon poly = new GMapPolygon(new List <PointLatLng>(), "rect");
double frontangle = (P * 0) + vfov / 2;
double backangle = (P * 0) - vfov / 2;
double leftangle = (R * 0) + hfov / 2;
double rightangle = (R * 0) - hfov / 2;
var fovh = Math.Tan(hfov / 2.0 * deg2rad) * 2.0;
var fovv = Math.Tan(vfov / 2.0 * deg2rad) * 2.0;
//DoDebug();
addtomap(plla, "plane");
Matrix3 dcm = new Matrix3();
dcm.from_euler(R * deg2rad, P * deg2rad, Y * deg2rad);
dcm.normalize();
Vector3 center1 = new Vector3(0, 0, 10000);
var test = dcm * center1;
var bearing2 = Math.Atan2(test.y, test.x) * rad2deg;
var newpos2 = plla.newpos(bearing2, Math.Sqrt(test.x * test.x + test.y * test.y));
newpos2.Alt -= test.z;
var cen = calcIntersection(plla, newpos2);
var dist = plla.GetDistance(cen);
addtomap(cen, "center " + dist.ToString("0"));
//
dcm.from_euler(R * deg2rad, P * deg2rad, Y * deg2rad);
dcm.rotate(new Vector3(rightangle * deg2rad, 0, 0));
dcm.normalize();
dcm.rotate(new Vector3(0, frontangle * deg2rad, 0));
dcm.normalize();
test = dcm * center1;
bearing2 = (Math.Atan2(test.y, test.x) * rad2deg);
newpos2 = plla.newpos(bearing2, Math.Sqrt(test.x * test.x + test.y * test.y));
newpos2.Alt -= test.z;
//addtomap(newpos2, "tr2");
var groundpointtr = calcIntersection(plla, newpos2);
poly.Points.Add(groundpointtr);
addtomap(groundpointtr, "tr");
//
dcm.from_euler(R * deg2rad, P * deg2rad, Y * deg2rad);
dcm.rotate(new Vector3(leftangle * deg2rad, 0, 0));
dcm.normalize();
dcm.rotate(new Vector3(0, frontangle * deg2rad, 0));
dcm.normalize();
test = dcm * center1;
bearing2 = Math.Atan2(test.y, test.x) * rad2deg;
newpos2 = plla.newpos(bearing2, Math.Sqrt(test.x * test.x + test.y * test.y));
newpos2.Alt -= test.z;
var groundpointtl = calcIntersection(plla, newpos2);
poly.Points.Add(groundpointtl);
addtomap(groundpointtl, "tl");
//
dcm.from_euler(R * deg2rad, P * deg2rad, Y * deg2rad);
dcm.rotate(new Vector3(leftangle * deg2rad, 0, 0));
dcm.normalize();
dcm.rotate(new Vector3(0, backangle * deg2rad, 0));
dcm.normalize();
test = dcm * center1;
bearing2 = Math.Atan2(test.y, test.x) * rad2deg;
newpos2 = plla.newpos(bearing2, Math.Sqrt(test.x * test.x + test.y * test.y));
newpos2.Alt -= test.z;
var groundpointbl = calcIntersection(plla, newpos2);
poly.Points.Add(groundpointbl);
addtomap(groundpointbl, "bl");
//
dcm.from_euler(R * deg2rad, P * deg2rad, Y * deg2rad);
dcm.rotate(new Vector3(rightangle * deg2rad, 0, 0));
dcm.normalize();
dcm.rotate(new Vector3(0, backangle * deg2rad, 0));
dcm.normalize();
test = dcm * center1;
bearing2 = Math.Atan2(test.y, test.x) * rad2deg;
newpos2 = plla.newpos(bearing2, Math.Sqrt(test.x * test.x + test.y * test.y));
newpos2.Alt -= test.z;
var groundpointbr = calcIntersection(plla, newpos2);
poly.Points.Add(groundpointbr);
addtomap(groundpointbr, "br");
//
polygons.Polygons.Clear();
polygons.Polygons.Add(poly);
var ans = new List <PointLatLngAlt>();
ans.Add(groundpointtl);
ans.Add(groundpointtr);
ans.Add(groundpointbr);
ans.Add(groundpointbl);
return(ans);
}
public static void POIAdd(PointLatLngAlt Point, string tag)
{
POIAdd(Point, tag, false, false);
}
public void DownloadFence(MAVLinkInterface port, Action <int, string> progress = null)
{
var list = mav_mission.download(port, MAVLink.MAV_MISSION_TYPE.FENCE, progress);
FencePolygon pol = new FencePolygon();
foreach (var locationwp in list)
{
var plla = new PointLatLngAlt(locationwp);
switch (locationwp.id)
{
case (ushort)MAVLink.MAV_CMD.FENCE_POLYGON_VERTEX_EXCLUSION:
{
pol.Mode = FencePolygon.PolyType.Exclusive;
pol.Points.Add(plla);
if (pol.Points.Count == locationwp.p1)
{
Fences.Add(pol);
pol = new FencePolygon();
}
}
break;
case (ushort)MAVLink.MAV_CMD.FENCE_POLYGON_VERTEX_INCLUSION:
{
pol.Mode = FencePolygon.PolyType.Inclusive;
pol.Points.Add(plla);
if (pol.Points.Count == locationwp.p1)
{
Fences.Add(pol);
pol = new FencePolygon();
}
}
break;
case (ushort)MAVLink.MAV_CMD.FENCE_CIRCLE_EXCLUSION:
{
var cir = new FenceCircle()
{
Mode = FenceCircle.PolyType.Exclusive,
Center = plla,
Radius = locationwp.p1
};
Fences.Add(cir);
}
break;
case (ushort)MAVLink.MAV_CMD.FENCE_CIRCLE_INCLUSION:
{
var cir = new FenceCircle()
{
Mode = FenceCircle.PolyType.Inclusive,
Center = plla,
Radius = locationwp.p1
};
Fences.Add(cir);
}
break;
case (ushort)MAVLink.MAV_CMD.FENCE_RETURN_POINT:
Fences.Add(new FenceReturn()
{
Return = plla
});
break;
default:
break;
}
}
}
private void map_MouseMove(object sender, MouseEventArgs e)
{
PointLatLng point = map.FromLocalToLatLng(e.X, e.Y);
currentMousePosition = point;
if (MouseDownStart == point)
return;
if (!isMouseDown)
{
// update mouse pos display
//SetMouseDisplay(point.Lat, point.Lng, 0);
}
//draging
if (e.Button == MouseButtons.Left && isMouseDown)
{
isMouseDraging = true;
if (CurrentGMapMarker != null)
{
PointLatLng pnew = map.FromLocalToLatLng(e.X, e.Y);
CurrentGMapMarker.Position = pnew;
list[CurrentGMapMarkerIndex] = new PointLatLngAlt(pnew);
domainUpDown1_ValueChanged(sender, e);
}
else // left click pan
{
double latdif = MouseDownStart.Lat - point.Lat;
double lngdif = MouseDownStart.Lng - point.Lng;
try
{
lock (thisLock)
{
map.Position = new PointLatLng(map.Position.Lat + latdif, map.Position.Lng + lngdif);
}
}
catch { }
}
}
}
public static async Task <List <PointLatLngAlt> > CreateGridAsync(List <PointLatLngAlt> polygon, double altitude,
double distance, double spacing, double angle, double overshoot1, double overshoot2, StartPosition startpos,
bool shutter, float minLaneSeparation, float leadin1, float leadin2, PointLatLngAlt HomeLocation, bool useextendedendpoint = true)
{
return(await Task.Run((() => CreateGrid(polygon, altitude, distance, spacing, angle, overshoot1, overshoot2,
startpos, shutter, minLaneSeparation, leadin1, leadin2, HomeLocation, useextendedendpoint)))
.ConfigureAwait(false));
}
public static List <PointLatLngAlt> CreateRotary(List <PointLatLngAlt> polygon, double altitude, double distance, double spacing, double angle, double overshoot1, double overshoot2, StartPosition startpos, bool shutter, float minLaneSeparation, float leadin, PointLatLngAlt HomeLocation, int clockwise_laps, bool match_spiral_perimeter)
{
spacing = 0;
if (distance < 0.1)
{
distance = 0.1;
}
if (polygon.Count == 0)
{
return(new List <PointLatLngAlt>());
}
List <utmpos> ans = new List <utmpos>();
// 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
}
var maxlane = 200; // (Centroid(utmpositions).GetDistance(utmpositions[0]) / distance);
ClipperLib.ClipperOffset clipperOffset = new ClipperLib.ClipperOffset();
clipperOffset.AddPath(utmpositions.Select(a => { return(new ClipperLib.IntPoint(a.x * 1000.0, a.y * 1000.0)); }).ToList(), ClipperLib.JoinType.jtMiter, ClipperLib.EndType.etClosedPolygon);
for (int lane = 0; lane < maxlane; lane++)
{
List <utmpos> ans1 = new List <utmpos>();
ClipperLib.PolyTree tree = new ClipperLib.PolyTree();
clipperOffset.Execute(ref tree, (Int64)(distance * 1000.0 * -lane));
if (tree.ChildCount == 0)
{
break;
}
if (lane < clockwise_laps || clockwise_laps < 0)
{
ClipperLib.Clipper.ReversePaths(ClipperLib.Clipper.PolyTreeToPaths(tree));
}
foreach (var treeChild in tree.Childs)
{
ans1 = treeChild.Contour.Select(a => new utmpos(a.X / 1000.0, a.Y / 1000.0, utmzone))
.ToList();
if (lane == 0 && clockwise_laps != 1 && match_spiral_perimeter)
{
ans1.Insert(0, ans1.Last <utmpos>()); // start at the last point of the first calculated lap
// to make a closed polygon on the first trip around
}
if (lane == clockwise_laps - 1)
{
ans1.Add(ans1.First <utmpos>()); // revisit the first waypoint on this lap to cleanly exit the CW pattern
}
if (ans.Count() > 2)
{
var start1 = ans[ans.Count() - 1];
var end1 = ans[ans.Count() - 2];
var start2 = ans1[0];
var end2 = ans1[ans1.Count() - 1];
}
ans.AddRange(ans1);
}
}
// set the altitude on all points
return(ans.Select(plla => { var a = plla.ToLLA(); a.Alt = altitude; a.Tag = "S"; return a; }).ToList());
}
//http://www.chrobotics.com/library/understanding-euler-angles
// x = north / roll
// y = pitch / east
// z = yaw / down
public static List <PointLatLngAlt> calc(PointLatLngAlt plla, double R, double P, double Y, double hfov, double vfov)
{
// alt should be asl
// P pitch where the center os pointing ie -80
// R roll
// if roll and pitch is 0, use the quick method.
if (R == 0 && P == 0)
{
// calc fov in m on the ground at 0 alt
var fovh = Math.Tan(hfov / 2.0 * deg2rad) * plla.Alt;
var fovv = Math.Tan(vfov / 2.0 * deg2rad) * plla.Alt;
var fovd = Math.Sqrt(fovh * fovh + fovv * fovv);
// where we put our footprint
var ans2 = new List <PointLatLngAlt>();
// calc bearing from center to corner
var bearing1 = Math.Atan2(fovh, fovv) * rad2deg;
// calc first corner point
var newpos1 = plla.newpos(bearing1 + Y, Math.Sqrt(fovh * fovh + fovv * fovv));
// set alt to 0, as we used the hypot for distance and fov is based on 0 alt
newpos1.Alt = 0;
// calc intersection from center to new point and return ground hit point
var cen1 = calcIntersection(plla, newpos1);
// add to our footprint poly
ans2.Add(cen1);
addtomap(cen1, "cen1");
//repeat
newpos1 = plla.newpos(Y - bearing1, Math.Sqrt(fovh * fovh + fovv * fovv));
newpos1.Alt = 0;
cen1 = calcIntersection(plla, newpos1);
ans2.Add(cen1);
addtomap(cen1, "cen2");
newpos1 = plla.newpos(bearing1 + Y - 180, Math.Sqrt(fovh * fovh + fovv * fovv));
newpos1.Alt = 0;
cen1 = calcIntersection(plla, newpos1);
ans2.Add(cen1);
addtomap(cen1, "cen3");
newpos1 = plla.newpos(Y - bearing1 - 180, Math.Sqrt(fovh * fovh + fovv * fovv));
newpos1.Alt = 0;
cen1 = calcIntersection(plla, newpos1);
ans2.Add(cen1);
addtomap(cen1, "cen4");
addtomap(plla, "plane");
return(ans2);
}
GMapPolygon poly = new GMapPolygon(new List <PointLatLng>(), "rect");
double frontangle = (P * 0) + vfov / 2;
double backangle = (P * 0) - vfov / 2;
double leftangle = (R * 0) + hfov / 2;
double rightangle = (R * 0) - hfov / 2;
addtomap(plla, "plane");
Matrix3 dcm = new Matrix3();
dcm.from_euler(R * deg2rad, P * deg2rad, Y * deg2rad);
dcm.normalize();
Vector3 center1 = new Vector3(0, 0, 10000);
var test = dcm * center1;
var bearing2 = Math.Atan2(test.y, test.x) * rad2deg;
var newpos2 = plla.newpos(bearing2, Math.Sqrt(test.x * test.x + test.y * test.y));
newpos2.Alt -= test.z;
var cen = calcIntersection(plla, newpos2);
var dist = plla.GetDistance(cen);
addtomap(cen, "center " + dist.ToString("0"));
//
dcm.from_euler(R * deg2rad, P * deg2rad, Y * deg2rad);
dcm.rotate(new Vector3(rightangle * deg2rad, 0, 0));
dcm.normalize();
dcm.rotate(new Vector3(0, frontangle * deg2rad, 0));
dcm.normalize();
/*
* var mx = new Matrix3();
* var my = new Matrix3();
* var mz = new Matrix3();
*
* mx.from_euler((rightangle + R) * deg2rad, 0, 0);
* my.from_euler(0, (frontangle + P) * deg2rad, 0);
* mz.from_euler(0, 0, Y * deg2rad);
*
* printdcm(mx);
* printdcm(my);
* printdcm(mz);
* printdcm(my * mx);
* printdcm(mz * my * mx);
*
* test = mz * my * mx * center1;
*/
test = dcm * center1;
bearing2 = (Math.Atan2(test.y, test.x) * rad2deg);
newpos2 = plla.newpos(bearing2, Math.Sqrt(test.x * test.x + test.y * test.y));
newpos2.Alt -= test.z;
//addtomap(newpos2, "tr2");
var groundpointtr = calcIntersection(plla, newpos2);
poly.Points.Add(groundpointtr);
addtomap(groundpointtr, "tr");
//
dcm.from_euler(R * deg2rad, P * deg2rad, Y * deg2rad);
dcm.rotate(new Vector3(leftangle * deg2rad, 0, 0));
dcm.normalize();
dcm.rotate(new Vector3(0, frontangle * deg2rad, 0));
dcm.normalize();
test = dcm * center1;
bearing2 = Math.Atan2(test.y, test.x) * rad2deg;
newpos2 = plla.newpos(bearing2, Math.Sqrt(test.x * test.x + test.y * test.y));
newpos2.Alt -= test.z;
var groundpointtl = calcIntersection(plla, newpos2);
poly.Points.Add(groundpointtl);
addtomap(groundpointtl, "tl");
//
dcm.from_euler(R * deg2rad, P * deg2rad, Y * deg2rad);
dcm.rotate(new Vector3(leftangle * deg2rad, 0, 0));
dcm.normalize();
dcm.rotate(new Vector3(0, backangle * deg2rad, 0));
dcm.normalize();
test = dcm * center1;
bearing2 = Math.Atan2(test.y, test.x) * rad2deg;
newpos2 = plla.newpos(bearing2, Math.Sqrt(test.x * test.x + test.y * test.y));
newpos2.Alt -= test.z;
var groundpointbl = calcIntersection(plla, newpos2);
poly.Points.Add(groundpointbl);
addtomap(groundpointbl, "bl");
//
dcm.from_euler(R * deg2rad, P * deg2rad, Y * deg2rad);
dcm.rotate(new Vector3(rightangle * deg2rad, 0, 0));
dcm.normalize();
dcm.rotate(new Vector3(0, backangle * deg2rad, 0));
dcm.normalize();
test = dcm * center1;
bearing2 = Math.Atan2(test.y, test.x) * rad2deg;
newpos2 = plla.newpos(bearing2, Math.Sqrt(test.x * test.x + test.y * test.y));
newpos2.Alt -= test.z;
var groundpointbr = calcIntersection(plla, newpos2);
poly.Points.Add(groundpointbr);
addtomap(groundpointbr, "br");
//
polygons.Polygons.Clear();
polygons.Polygons.Add(poly);
var ans = new List <PointLatLngAlt>();
ans.Add(groundpointtl);
ans.Add(groundpointtr);
ans.Add(groundpointbr);
ans.Add(groundpointbl);
return(ans);
}
public SightGen(PointLatLng location, List <PointLatLng> pointslist, double alt, PointLatLng aircraftlocation, double aircraft_alt)
{
range = Settings.Instance.GetFloat("Propagation_Range");
clearance = Settings.Instance.GetFloat("Propagation_Clearance");
base_height = Settings.Instance.GetFloat("Propagation_Height");
var converge = Settings.Instance.GetInt32("Propagation_Converge");
var rotational = Settings.Instance.GetInt32("Propagation_Rotational");
if (rotational == 0)
{
rotational = 1;
}
drone_alt = aircraft_alt;
// enable rf when there is no drone connected
// use home loc + max(base height , clearance) + 0.1;
if (drone_alt == 0)
{
drone_alt = alt + Math.Max(base_height, clearance) + 0.1;
}
homealt = alt;
var startlocation = new PointLatLngAlt(location)
{
Alt = homealt + base_height
};
var dronelocation = new PointLatLngAlt(aircraftlocation)
{
Alt = drone_alt - clearance
};
SortedDictionary <double, PointLatLngAlt> sortedlist = new SortedDictionary <double, PointLatLngAlt>();
Parallel.ForEach(Extensions.SteppedRange(0, 2 * (float)Math.PI, Math.PI / 180 * rotational), angle => //Terrain intercept scan full 360
{
List <PointLatLngAlt> pointends = new List <PointLatLngAlt>();
var carryon = true;
var up = false;
var down = false;
float triangle = 45 * (float)Math.PI / 180; //radians
var Max = 90 * (float)Math.PI / 180; //radians
float Min = 0; //radians
// calc LOS vector and intersection with terrain
//point = srtm.getIntersectionWithTerrain(startlocation, endlocation);
PointLatLngAlt point = null;
var newpos = new PointLatLngAlt(location).newpos(angle * MathHelper.rad2deg, range * 1000);
pointends.Clear();
pointends.Add(location);
pointends.Add(newpos);
while (carryon && Min + converge * (float)Math.PI / 180 < Max) //Breaks if terrain intercept found or convergence occurs
{
point = getSRTMAltPath(pointends, triangle, ref carryon, ref up, ref down); //DEM data
if (up)
{
Min = triangle;
}
else if (down)
{
Max = triangle;
}
triangle = (Max + Min) / 2;
}
lock (sortedlist)
sortedlist.Add(angle, point);
});
sortedlist.Values.ForEach(a => pointslist.Add(a));
pointslist.Add(pointslist[0]);
pointslist.DeDupOrderedList();
}
public void DoAcceptTcpClientCallback(IAsyncResult ar)
{
// Get the listener that handles the client request.
TcpListener listener = (TcpListener)ar.AsyncState;
// End the operation and display the received data on
// the console.
using (
TcpClient client = listener.EndAcceptTcpClient(ar))
{
// Signal the calling thread to continue.
tcpClientConnected.Set();
try
{
// Get a stream object for reading and writing
log.Info("Accepted Client " + client.Client.RemoteEndPoint.ToString());
//client.SendBufferSize = 100 * 1024; // 100kb
//client.LingerState.Enabled = true;
//client.NoDelay = true;
// makesure we have valid image
GCSViews.FlightData.myhud.streamjpgenable = true;
NetworkStream stream = client.GetStream();
// 5 seconds - default for httpd 2.2+
stream.ReadTimeout = 5000;
goto skipagain;
again:
log.Info("doing Again");
skipagain:
var asciiEncoding = new ASCIIEncoding();
var request = new byte[1024 * 4];
int len = 0;
// handle header
try
{
len = stream.Read(request, 0, request.Length);
}
catch
{
return;
}
string head = System.Text.Encoding.ASCII.GetString(request, 0, len);
log.Info(head);
int index = head.IndexOf('\n');
string url = head.Substring(0, index - 1);
//url = url.Replace("\r", "");
//url = url.Replace("GET ","");
//url = url.Replace(" HTTP/1.0", "");
//url = url.Replace(" HTTP/1.1", "");
Tracking.AddEvent("HTTPServer", "Get", url, "");
/////////////////////////////////////////////////////////////////
if (url.Contains("websocket"))
{
using (var writer = new StreamWriter(stream, Encoding.Default))
{
writer.WriteLine("HTTP/1.1 101 WebSocket Protocol Handshake");
writer.WriteLine("Upgrade: WebSocket");
writer.WriteLine("Connection: Upgrade");
writer.WriteLine("WebSocket-Location: ws://localhost:56781/websocket/server");
int start = head.IndexOf("Sec-WebSocket-Key:") + 19;
int end = head.IndexOf('\r', start);
if (end == -1)
{
end = head.IndexOf('\n', start);
}
string accept = ComputeWebSocketHandshakeSecurityHash09(head.Substring(start, end - start));
writer.WriteLine("Sec-WebSocket-Accept: " + accept);
writer.WriteLine("Server: Mission Planner");
writer.WriteLine("");
writer.Flush();
while (client.Connected)
{
Thread.Sleep(200);
log.Debug(stream.DataAvailable + " " + client.Available);
while (client.Available > 0)
{
Console.Write(stream.ReadByte());
}
byte[] packet = new byte[1024];
string sendme = MainV2.comPort.MAV.cs.roll + "," + MainV2.comPort.MAV.cs.pitch + "," + MainV2.comPort.MAV.cs.yaw
+ "," + MainV2.comPort.MAV.cs.lat + "," + MainV2.comPort.MAV.cs.lng + "," + MainV2.comPort.MAV.cs.alt;
packet[0] = 0x81; // fin - binary
packet[1] = (byte)sendme.Length;
int i = 2;
foreach (char ch in sendme)
{
packet[i++] = (byte)ch;
}
stream.Write(packet, 0, i);
//break;
}
}
}
/////////////////////////////////////////////////////////////////
else if (url.Contains("georefnetwork.kml"))
{
byte[] buffer = Encoding.ASCII.GetBytes(georefkml);
string header =
"HTTP/1.1 200 OK\r\nServer: here\r\nKeep-Alive: timeout=15, max=100\r\nConnection: Keep-Alive\r\nCache-Control: no-cache\r\nContent-Type: application/vnd.google-earth.kml+xml\r\nX-Pad: avoid browser bug\r\nContent-Length: " +
buffer.Length + "\r\n\r\n";
byte[] temp = asciiEncoding.GetBytes(header);
stream.Write(temp, 0, temp.Length);
stream.Write(buffer, 0, buffer.Length);
goto again;
//stream.Close();
}
/////////////////////////////////////////////////////////////////
else if (url.Contains("network.kml"))
{
SharpKml.Dom.Document kml = new SharpKml.Dom.Document();
SharpKml.Dom.Placemark pmplane = new SharpKml.Dom.Placemark();
pmplane.Name = "P/Q " + MainV2.comPort.MAV.cs.altasl;
pmplane.Visibility = true;
SharpKml.Dom.Location loc = new SharpKml.Dom.Location();
loc.Latitude = MainV2.comPort.MAV.cs.lat;
loc.Longitude = MainV2.comPort.MAV.cs.lng;
loc.Altitude = MainV2.comPort.MAV.cs.altasl;
if (loc.Altitude < 0)
{
loc.Altitude = 0.01;
}
SharpKml.Dom.Orientation ori = new SharpKml.Dom.Orientation();
ori.Heading = MainV2.comPort.MAV.cs.yaw;
ori.Roll = -MainV2.comPort.MAV.cs.roll;
ori.Tilt = -MainV2.comPort.MAV.cs.pitch;
SharpKml.Dom.Scale sca = new SharpKml.Dom.Scale();
sca.X = 2;
sca.Y = 2;
sca.Z = 2;
SharpKml.Dom.Model model = new SharpKml.Dom.Model();
model.Location = loc;
model.Orientation = ori;
model.AltitudeMode = SharpKml.Dom.AltitudeMode.Absolute;
model.Scale = sca;
SharpKml.Dom.Link link = new SharpKml.Dom.Link();
link.Href = new Uri("block_plane_0.dae", UriKind.Relative);
model.Link = link;
pmplane.Geometry = model;
SharpKml.Dom.LookAt la = new SharpKml.Dom.LookAt()
{
Altitude = loc.Altitude.Value,
Latitude = loc.Latitude.Value,
Longitude = loc.Longitude.Value,
Tilt = 80,
Heading = MainV2.comPort.MAV.cs.yaw,
AltitudeMode = SharpKml.Dom.AltitudeMode.Absolute,
Range = 50
};
if (loc.Latitude.Value != 0 && loc.Longitude.Value != 0)
{
kml.Viewpoint = la;
kml.AddFeature(pmplane);
}
SharpKml.Dom.CoordinateCollection coords = new SharpKml.Dom.CoordinateCollection();
//if (loc.Latitude.Value != 0 && loc.Longitude.Value != 0)
{
//foreach (var point in MainV2.comPort.MAV.wps.Values)
{
// coords.Add(new SharpKml.Base.Vector(point.x, point.y, point.z));
}
}
//else
{
PointLatLngAlt home = null;
// draw track
try
{
foreach (var point in GCSViews.FlightPlanner.instance.fullpointlist)
{
if (point.Tag.ToLower().Contains("home"))
{
home = point;
}
if (point != null)
{
coords.Add(new SharpKml.Base.Vector(point.Lat, point.Lng, point.Alt));
}
}
}
catch
{
}
foreach (var point in GCSViews.FlightPlanner.instance.fullpointlist)
{
if (point == null)
{
continue;
}
SharpKml.Dom.Placemark wp = new SharpKml.Dom.Placemark();
wp.Name = "WP " + point.Tag + " Alt: " + point.Alt;
SharpKml.Dom.Point wppoint = new SharpKml.Dom.Point();
var altmode = SharpKml.Dom.AltitudeMode.RelativeToGround;
wppoint.AltitudeMode = altmode;
wppoint.Coordinate = new Vector()
{
Latitude = point.Lat,
Longitude = point.Lng,
Altitude = point.Alt
};
wp.Geometry = wppoint;
kml.AddFeature(wp);
}
}
SharpKml.Dom.LineString ls = new SharpKml.Dom.LineString();
ls.AltitudeMode = SharpKml.Dom.AltitudeMode.RelativeToGround;
ls.Coordinates = coords;
ls.Extrude = false;
ls.Tessellate = true;
Style style = new Style();
style.Id = "yellowLineGreenPoly";
style.Line = new LineStyle(new Color32(HexStringToColor("ff00ffff")), 4);
Style style2 = new Style();
style2.Id = "yellowLineGreenPoly";
style2.Line = new LineStyle(new Color32(HexStringToColor("7f00ffff")), 4);
// above ground
SharpKml.Dom.Placemark pm = new SharpKml.Dom.Placemark()
{
Geometry = ls,
Name = "WPs",
StyleSelector = style
};
kml.AddFeature(pm);
// on ground
SharpKml.Dom.LineString ls2 = new SharpKml.Dom.LineString();
ls2.Coordinates = coords;
ls2.Extrude = false;
ls2.Tessellate = true;
ls2.AltitudeMode = SharpKml.Dom.AltitudeMode.ClampToGround;
SharpKml.Dom.Placemark pm2 = new SharpKml.Dom.Placemark()
{
Geometry = ls2,
Name = "onground",
StyleSelector = style2
};
kml.AddFeature(pm2);
SharpKml.Base.Serializer serializer = new SharpKml.Base.Serializer();
serializer.Serialize(kml);
byte[] buffer = Encoding.ASCII.GetBytes(serializer.Xml);
string header =
"HTTP/1.1 200 OK\r\nContent-Type: application/vnd.google-earth.kml+xml\r\nContent-Length: " +
buffer.Length + "\r\n\r\n";
byte[] temp = asciiEncoding.GetBytes(header);
stream.Write(temp, 0, temp.Length);
stream.Write(buffer, 0, buffer.Length);
goto again;
//stream.Close();
}
/////////////////////////////////////////////////////////////////
else if (url.Contains("block_plane_0.dae"))
{
string header = "HTTP/1.1 200 OK\r\nContent-Type: text/plain\r\n\r\n";
byte[] temp = asciiEncoding.GetBytes(header);
stream.Write(temp, 0, temp.Length);
BinaryReader file =
new BinaryReader(File.Open("block_plane_0.dae", FileMode.Open, FileAccess.Read,
FileShare.Read));
byte[] buffer = new byte[1024];
while (file.PeekChar() != -1)
{
int leng = file.Read(buffer, 0, buffer.Length);
stream.Write(buffer, 0, leng);
}
file.Close();
stream.Close();
}
/////////////////////////////////////////////////////////////////
else if (url.Contains("hud.html"))
{
string header = "HTTP/1.1 200 OK\r\nContent-Type: text/html\r\n\r\n";
byte[] temp = asciiEncoding.GetBytes(header);
stream.Write(temp, 0, temp.Length);
BinaryReader file =
new BinaryReader(File.Open("hud.html", FileMode.Open, FileAccess.Read, FileShare.Read));
byte[] buffer = new byte[1024];
while (file.PeekChar() != -1)
{
int leng = file.Read(buffer, 0, buffer.Length);
stream.Write(buffer, 0, leng);
}
file.Close();
stream.Close();
}
/////////////////////////////////////////////////////////////////
else if (url.ToLower().Contains("hud.jpg") || url.ToLower().Contains("map.jpg") ||
url.ToLower().Contains("both.jpg"))
{
refreshmap();
string header =
"HTTP/1.1 200 OK\r\nContent-Type: multipart/x-mixed-replace;boundary=PLANNER\r\n\r\n--PLANNER\r\n";
byte[] temp = asciiEncoding.GetBytes(header);
stream.Write(temp, 0, temp.Length);
while (client.Connected)
{
System.Threading.Thread.Sleep(200); // 5hz
byte[] data = null;
if (url.ToLower().Contains("hud"))
{
GCSViews.FlightData.myhud.streamjpgenable = true;
data = GCSViews.FlightData.myhud.streamjpg.ToArray();
}
else if (url.ToLower().Contains("map"))
{
data = GetControlJpegRaw(GCSViews.FlightData.mymap);
}
else
{
GCSViews.FlightData.myhud.streamjpgenable = true;
Image img1 = Image.FromStream(GCSViews.FlightData.myhud.streamjpg);
Image img2 = GetControlJpeg(GCSViews.FlightData.mymap);
int bigger = img1.Height > img2.Height ? img1.Height : img2.Height;
Image imgout = new Bitmap(img1.Width + img2.Width, bigger);
Graphics grap = Graphics.FromImage(imgout);
grap.DrawImageUnscaled(img1, 0, 0);
grap.DrawImageUnscaled(img2, img1.Width, 0);
MemoryStream streamjpg = new MemoryStream();
imgout.Save(streamjpg, System.Drawing.Imaging.ImageFormat.Jpeg);
data = streamjpg.ToArray();
}
header = "Content-Type: image/jpeg\r\nContent-Length: " + data.Length + "\r\n\r\n";
temp = asciiEncoding.GetBytes(header);
stream.Write(temp, 0, temp.Length);
stream.Write(data, 0, data.Length);
header = "\r\n--PLANNER\r\n";
temp = asciiEncoding.GetBytes(header);
stream.Write(temp, 0, temp.Length);
}
GCSViews.FlightData.myhud.streamjpgenable = false;
stream.Close();
}
/////////////////////////////////////////////////////////////////
else if (url.Contains("/guided?"))
{
//http://127.0.0.1:56781/guided?lat=-34&lng=117.8&alt=30
Regex rex = new Regex(@"lat=([\-\.0-9]+)&lng=([\-\.0-9]+)&alt=([\.0-9]+)",
RegexOptions.IgnoreCase);
Match match = rex.Match(url);
if (match.Success)
{
Locationwp gwp = new Locationwp()
{
lat = double.Parse(match.Groups[1].Value),
lng = double.Parse(match.Groups[2].Value),
alt = float.Parse(match.Groups[3].Value)
};
try
{
MainV2.comPort.setGuidedModeWP(gwp);
}
catch
{
}
string header = "HTTP/1.1 200 OK\r\n\r\nSent Guide Mode Wp";
byte[] temp = asciiEncoding.GetBytes(header);
stream.Write(temp, 0, temp.Length);
}
else
{
string header = "HTTP/1.1 200 OK\r\n\r\nFailed Guide Mode Wp";
byte[] temp = asciiEncoding.GetBytes(header);
stream.Write(temp, 0, temp.Length);
}
stream.Close();
}
/////////////////////////////////////////////////////////////////
else if (url.ToLower().Contains(".jpg"))
{
Regex rex = new Regex(@"([^\s]+)\s(.+)\sHTTP/1", RegexOptions.IgnoreCase);
Match match = rex.Match(url);
if (match.Success)
{
string fileurl = match.Groups[2].Value;
using (Image orig = Image.FromFile(georefimagepath + fileurl))
using (Image resi = ResizeImage(orig, new Size(640, 480)))
using (MemoryStream memstream = new MemoryStream())
{
resi.Save(memstream, System.Drawing.Imaging.ImageFormat.Jpeg);
memstream.Position = 0;
string header =
"HTTP/1.1 200 OK\r\nServer: here\r\nKeep-Alive: timeout=15, max=100\r\nConnection: Keep-Alive\r\nContent-Type: image/jpg\r\nX-Pad: avoid browser bug\r\nContent-Length: " +
memstream.Length + "\r\n\r\n";
byte[] temp = asciiEncoding.GetBytes(header);
stream.Write(temp, 0, temp.Length);
using (BinaryReader file = new BinaryReader(memstream))
{
byte[] buffer = new byte[1024];
while (file.BaseStream.Position < file.BaseStream.Length)
{
int leng = file.Read(buffer, 0, buffer.Length);
stream.Write(buffer, 0, leng);
}
}
}
goto again;
//stream.Close();
}
/////////////////////////////////////////////////////////////////
else
{
string header = "HTTP/1.1 404 not found\r\nContent-Type: image/jpg\r\n\r\n";
byte[] temp = asciiEncoding.GetBytes(header);
stream.Write(temp, 0, temp.Length);
}
stream.Close();
}
/////////////////////////////////////////////////////////////////
else if (url.ToLower().Contains("post /guide"))
{
Regex rex = new Regex(@"lat"":([\-\.0-9]+),""lon"":([\-\.0-9]+),""alt"":([\.0-9]+)",
RegexOptions.IgnoreCase);
Match match = rex.Match(head);
if (match.Success)
{
Locationwp gwp = new Locationwp()
{
lat = double.Parse(match.Groups[1].Value),
lng = double.Parse(match.Groups[2].Value),
alt = float.Parse(match.Groups[3].Value)
};
try
{
MainV2.comPort.setGuidedModeWP(gwp);
}
catch
{
}
string header = "HTTP/1.1 200 OK\r\n\r\nSent Guide Mode Wp";
byte[] temp = asciiEncoding.GetBytes(header);
stream.Write(temp, 0, temp.Length);
}
else
{
string header = "HTTP/1.1 200 OK\r\n\r\nFailed Guide Mode Wp";
byte[] temp = asciiEncoding.GetBytes(header);
stream.Write(temp, 0, temp.Length);
}
stream.Close();
}
/////////////////////////////////////////////////////////////////
else if (url.ToLower().Contains("/command_long"))
{
string header = "HTTP/1.1 404 not found\r\nContent-Type: image/jpg\r\n\r\n";
byte[] temp = asciiEncoding.GetBytes(header);
stream.Write(temp, 0, temp.Length);
stream.Close();
}
/////////////////////////////////////////////////////////////////
else if (url.ToLower().Contains("/rcoverride"))
{
string header = "HTTP/1.1 404 not found\r\nContent-Type: image/jpg\r\n\r\n";
byte[] temp = asciiEncoding.GetBytes(header);
stream.Write(temp, 0, temp.Length);
stream.Close();
}
/////////////////////////////////////////////////////////////////
else if (url.ToLower().Contains("/get_mission"))
{
string header = "HTTP/1.1 404 not found\r\nContent-Type: image/jpg\r\n\r\n";
byte[] temp = asciiEncoding.GetBytes(header);
stream.Write(temp, 0, temp.Length);
stream.Close();
}
/////////////////////////////////////////////////////////////////
else if (url.ToLower().Contains("/mavlink/"))
{
/*
* GET /mavlink/ATTITUDE+VFR_HUD+NAV_CONTROLLER_OUTPUT+META_WAYPOINT+GPS_RAW_INT+HEARTBEAT+META_LINKQUALITY+GPS_STATUS+STATUSTEXT+SYS_STATUS?_=1355828718540 HTTP/1.1
* Host: ubuntu:9999
* Connection: keep-alive
* X-Requested-With: XMLHttpRequest
* User-Agent: Mozilla/5.0 (Windows NT 6.1; WOW64) AppleWebKit/537.11 (KHTML, like Gecko) Chrome/23.0.1271.97 Safari/537.11
* Accept:
* Referer: http://ubuntu:9999/index.html
* Accept-Encoding: gzip,deflate,sdch
* Accept-Language: en-GB,en-US;q=0.8,en;q=0.6
* Accept-Charset: ISO-8859-1,utf-8;q=0.7,*;q=0.3
*
* HTTP/1.1 200 OK
* Content-Type: application/json
* Content-Length: 2121
* Date: Thu, 29 Nov 2012 12:13:38 GMT
* Server: ubuntu
*
* {
* "VFR_HUD": {"msg": {"throttle": 0, "groundspeed": 0.0, "airspeed": 0.0, "climb": 0.0, "mavpackettype": "VFR_HUD", "alt": -0.47999998927116394, "heading": 108}, "index": 687, "time_usec": 0},
* "STATUSTEXT": {"msg": {"mavpackettype": "STATUSTEXT", "severity": 1, "text": "Initialising APM..."}, "index": 2, "time_usec": 0},
* "SYS_STATUS": {"msg": {"onboard_control_sensors_present": 4294966287, "load": 0, "battery_remaining": -1, "errors_count4": 0, "drop_rate_comm": 0, "errors_count2": 0, "errors_count3": 0, "errors_comm": 0, "current_battery": -1, "errors_count1": 0, "onboard_control_sensors_health": 4294966287, "mavpackettype": "SYS_STATUS", "onboard_control_sensors_enabled": 4294945807, "voltage_battery": 10080}, "index": 693, "time_usec": 0},
* "META_LINKQUALITY": {"msg": {"master_in": 11110, "mav_loss": 0, "mavpackettype": "META_LINKQUALITY", "master_out": 194, "packet_loss": 0.0}, "index": 194, "time_usec": 0},
* "ATTITUDE": {"msg": {"pitchspeed": -0.000976863200776279, "yaw": 1.8878594636917114, "rollspeed": -0.0030046366155147552, "time_boot_ms": 194676, "pitch": -0.09986469894647598, "mavpackettype": "ATTITUDE", "yawspeed": -0.0015030358918011189, "roll": -0.029391441494226456}, "index": 687, "time_usec": 0},
* "GPS_RAW_INT": {"msg": {"fix_type": 1, "cog": 0, "epv": 65535, "lon": 0, "time_usec": 0, "eph": 9999, "satellites_visible": 0, "lat": 0, "mavpackettype": "GPS_RAW_INT", "alt": 137000, "vel": 0}, "index": 687, "time_usec": 0},
* "HEARTBEAT": {"msg": {"custom_mode": 0, "system_status": 4, "base_mode": 81, "autopilot": 3, "mavpackettype": "HEARTBEAT", "type": 2, "mavlink_version": 3}, "index": 190, "time_usec": 0},
* "GPS_STATUS": {"msg": {"satellite_snr": "", "satellite_azimuth": "", "satellite_prn": "", "satellite_elevation": "", "satellites_visible": 0, "satellite_used": "", "mavpackettype": "GPS_STATUS"}, "index": 2, "time_usec": 0},
* "NAV_CONTROLLER_OUTPUT": {"msg": {"wp_dist": 0, "nav_pitch": 0.0, "target_bearing": 0, "nav_roll": 0.0, "aspd_error": 0.0, "alt_error": 0.0, "mavpackettype": "NAV_CONTROLLER_OUTPUT", "xtrack_error": 0.0, "nav_bearing": 0}, "index": 687, "time_usec": 0}}
*/
JavaScriptSerializer serializer = new JavaScriptSerializer();
object[] data = new object[20];
Messagejson message = new Messagejson();
if (MainV2.comPort.MAV.getPacket((byte)MAVLink.MAVLINK_MSG_ID.ATTITUDE) != null)
{
message.ATTITUDE = new Message2()
{
index = 1,
msg =
MainV2.comPort.MAV.getPacket((byte)MAVLink.MAVLINK_MSG_ID.ATTITUDE)
.ByteArrayToStructure <MAVLink.mavlink_attitude_t>()
}
}
;
if (MainV2.comPort.MAV.getPacket((byte)MAVLink.MAVLINK_MSG_ID.VFR_HUD) != null)
{
message.VFR_HUD = new Message2()
{
index = 1,
msg =
MainV2.comPort.MAV.getPacket((byte)MAVLink.MAVLINK_MSG_ID.VFR_HUD)
.ByteArrayToStructure <MAVLink.mavlink_vfr_hud_t>()
}
}
;
if (MainV2.comPort.MAV.getPacket((byte)MAVLink.MAVLINK_MSG_ID.NAV_CONTROLLER_OUTPUT) != null)
{
message.NAV_CONTROLLER_OUTPUT = new Message2()
{
index = 1,
msg =
MainV2.comPort.MAV.getPacket((byte)MAVLink.MAVLINK_MSG_ID.NAV_CONTROLLER_OUTPUT)
.ByteArrayToStructure <MAVLink.mavlink_nav_controller_output_t>()
}
}
;
if (MainV2.comPort.MAV.getPacket((byte)MAVLink.MAVLINK_MSG_ID.GPS_RAW_INT) != null)
{
message.GPS_RAW_INT = new Message2()
{
index = 1,
msg =
MainV2.comPort.MAV.getPacket((byte)MAVLink.MAVLINK_MSG_ID.GPS_RAW_INT)
.ByteArrayToStructure <MAVLink.mavlink_gps_raw_int_t>()
}
}
;
if (MainV2.comPort.MAV.getPacket((byte)MAVLink.MAVLINK_MSG_ID.HEARTBEAT) != null)
{
message.HEARTBEAT = new Message2()
{
index = 1,
msg =
MainV2.comPort.MAV.getPacket((byte)MAVLink.MAVLINK_MSG_ID.HEARTBEAT)
.ByteArrayToStructure <MAVLink.mavlink_heartbeat_t>()
}
}
;
if (MainV2.comPort.MAV.getPacket((byte)MAVLink.MAVLINK_MSG_ID.GPS_STATUS) != null)
{
message.GPS_STATUS = new Message2()
{
index = 1,
msg =
MainV2.comPort.MAV.getPacket((byte)MAVLink.MAVLINK_MSG_ID.GPS_STATUS)
.ByteArrayToStructure <MAVLink.mavlink_gps_status_t>()
}
}
;
if (MainV2.comPort.MAV.getPacket((byte)MAVLink.MAVLINK_MSG_ID.STATUSTEXT) != null)
{
message.STATUSTEXT = new Message2()
{
index = 1,
msg =
MainV2.comPort.MAV.getPacket((byte)MAVLink.MAVLINK_MSG_ID.STATUSTEXT)
.ByteArrayToStructure <MAVLink.mavlink_statustext_t>()
}
}
;
if (MainV2.comPort.MAV.getPacket((byte)MAVLink.MAVLINK_MSG_ID.SYS_STATUS) != null)
{
message.SYS_STATUS = new Message2()
{
index = 1,
msg =
MainV2.comPort.MAV.getPacket((byte)MAVLink.MAVLINK_MSG_ID.SYS_STATUS)
.ByteArrayToStructure <MAVLink.mavlink_sys_status_t>()
}
}
;
message.META_LINKQUALITY =
message.SYS_STATUS =
new Message2()
{
index = packetindex,
time_usec = 0,
msg =
new META_LINKQUALITY()
{
master_in = (int)MainV2.comPort.MAV.packetsnotlost,
mavpackettype = "META_LINKQUALITY",
master_out = MainV2.comPort.packetcount,
packet_loss = 100 - MainV2.comPort.MAV.cs.linkqualitygcs,
mav_loss = 0
}
};
packetindex++;
string output = serializer.Serialize(message);
string header = "HTTP/1.1 200 OK\r\nContent-Type: application/json\r\nContent-Length: " +
output.Length + "\r\n\r\n";
byte[] temp = asciiEncoding.GetBytes(header);
stream.Write(temp, 0, temp.Length);
temp = asciiEncoding.GetBytes(output);
stream.Write(temp, 0, temp.Length);
goto again;
//stream.Close();
}
/////////////////////////////////////////////////////////////////
else if (url.ToLower().Contains("/mav/"))
{
//C:\Users\hog\Desktop\DIYDrones\mavelous\modules\lib\mavelous_web
Regex rex = new Regex(@"([^\s]+)\s(.+)\sHTTP/1", RegexOptions.IgnoreCase);
Match match = rex.Match(url);
if (match.Success)
{
string fileurl = match.Groups[2].Value;
fileurl = fileurl.Replace("/mav/", "");
if (fileurl == "" || fileurl == "/")
{
fileurl = "index.html";
}
string header = "HTTP/1.1 200 OK\r\n";
if (fileurl.Contains(".html"))
{
header += "Content-Type: text/html\r\n\r\n";
}
else if (fileurl.Contains(".js"))
{
header += "Content-Type: application/x-javascript\r\n\r\n";
}
else if (fileurl.Contains(".css"))
{
header += "Content-Type: text/css\r\n\r\n";
}
else
{
header += "Content-Type: text/plain\r\n\r\n";
}
byte[] temp = asciiEncoding.GetBytes(header);
stream.Write(temp, 0, temp.Length);
BinaryReader file =
new BinaryReader(File.Open(mavelous_web + fileurl, FileMode.Open, FileAccess.Read,
FileShare.Read));
byte[] buffer = new byte[1024];
while (file.BaseStream.Position < file.BaseStream.Length)
{
int leng = file.Read(buffer, 0, buffer.Length);
stream.Write(buffer, 0, leng);
}
file.Close();
stream.Close();
}
/////////////////////////////////////////////////////////////////
else
{
string header = "HTTP/1.1 404 not found\r\nContent-Type: image/jpg\r\n\r\n";
byte[] temp = asciiEncoding.GetBytes(header);
stream.Write(temp, 0, temp.Length);
stream.Close();
}
}
/////////////////////////////////////////////////////////////////
else
{
Console.WriteLine(url);
string header = "HTTP/1.1 200 OK\r\nContent-Type: text/html\r\n\r\n";
byte[] temp = asciiEncoding.GetBytes(header);
stream.Write(temp, 0, temp.Length);
string content = @"
<a href=/mav/>Mavelous</a>
<a href=/mavlink/>Mavelous traffic</a>
<a href=/hud.jpg>Hud image</a>
<a href=/map.jpg>Map image </a>
<a href=/both.jpg>Map & hud image</a>
<a href=/hud.html>hud html5</a>
<a href=/network.kml>network kml</a>
<a href=/georefnetwork.kml>georef kml</a>
";
temp = asciiEncoding.GetBytes(content);
stream.Write(temp, 0, temp.Length);
}
stream.Close();
log.Info("Close http " + url);
}
catch (Exception ee)
{
log.Error("Failed http ", ee);
}
}
}
public static PointLatLngAlt ProjectPoint()
{
MainV2.comPort.GetMountStatus();
// this should be looking at rc_channel function
yawchannel = (int)(float)MainV2.comPort.MAV.param["MNT_RC_IN_PAN"];
pitchchannel = (int)(float)MainV2.comPort.MAV.param["MNT_RC_IN_TILT"];
rollchannel = (int)(float)MainV2.comPort.MAV.param["MNT_RC_IN_ROLL"];
//if (!MainV2.comPort.BaseStream.IsOpen)
// return PointLatLngAlt.Zero;
PointLatLngAlt currentlocation = new PointLatLngAlt(MainV2.comPort.MAV.cs.lat, MainV2.comPort.MAV.cs.lng);
double yawangle = MainV2.comPort.MAV.cs.campointc;
double rollangle = MainV2.comPort.MAV.cs.campointb;
double pitchangle = MainV2.comPort.MAV.cs.campointa;
//
if ((double)MainV2.comPort.MAV.param["MNT_TYPE"] == 4)
{
yawangle = MainV2.comPort.MAVlist[MainV2.comPort.sysidcurrent, 67].cs.yaw;
rollangle = MainV2.comPort.MAVlist[MainV2.comPort.sysidcurrent, 67].cs.roll;
pitchangle = MainV2.comPort.MAVlist[MainV2.comPort.sysidcurrent, 67].cs.pitch;
}
if (Math.Abs(rollangle) > 180 || yawangle == 0 && pitchangle == 0)
{
yawangle = ConvertPwmtoAngle(axis.yaw);
//rollangle = ConvertPwmtoAngle(axis.roll);
pitchangle = ConvertPwmtoAngle(axis.pitch) + MainV2.comPort.MAV.cs.pitch;
pitchangle -= Math.Sin(yawangle * MathHelper.deg2rad) * MainV2.comPort.MAV.cs.roll;
}
// tan (o/a)
// todo account for ground elevation change.
int distout = 10;
PointLatLngAlt newpos = PointLatLngAlt.Zero;
//dist = Math.Tan((90 + pitchangle) * MathHelper.deg2rad) * (MainV2.comPort.MAV.cs.alt);
while (distout < 1000)
{
// get a projected point to test intersection against - not using slope distance
PointLatLngAlt newposdist = currentlocation.newpos(yawangle + MainV2.comPort.MAV.cs.yaw, distout);
newposdist.Alt = srtm.getAltitude(newposdist.Lat, newposdist.Lng).alt;
// get another point 50 infront
PointLatLngAlt newposdist2 = currentlocation.newpos(yawangle + MainV2.comPort.MAV.cs.yaw, distout + 50);
newposdist2.Alt = srtm.getAltitude(newposdist2.Lat, newposdist2.Lng).alt;
// get the flat terrain distance out - at 0 alt
double distflat = Math.Tan((90 + pitchangle) * MathHelper.deg2rad) * (MainV2.comPort.MAV.cs.altasl);
// x is dist from plane, y is alt
var newpoint = FindLineIntersection(new PointF(0, MainV2.comPort.MAV.cs.altasl),
new PointF((float)distflat, 0),
new PointF((float)distout, (float)newposdist.Alt),
new PointF((float)distout + 50, (float)newposdist2.Alt));
if (newpoint.X != 0)
{
newpos = newposdist2;
break;
}
distout += 50;
}
//Console.WriteLine("pitch " + pitchangle.ToString("0.000") + " yaw " + yawangle.ToString("0.000") + " dist" + dist.ToString("0.000"));
//PointLatLngAlt newpos = currentlocation.newpos( yawangle + MainV2.comPort.MAV.cs.yaw, dist);
//Console.WriteLine(newpos);
return(newpos);
}
static void addtomap(PointLatLngAlt pos, string tag)
{
}
public static List <PointLatLngAlt> CreateRotary(List <PointLatLngAlt> polygon, double altitude, double distance, double spacing, double angle, double overshoot1, double overshoot2, StartPosition startpos, bool shutter, float minLaneSeparation, float leadin, PointLatLngAlt HomeLocation)
{
spacing = 0;
if (distance < 0.1)
{
distance = 0.1;
}
if (polygon.Count == 0)
{
return(new List <PointLatLngAlt>());
}
List <utmpos> ans = new List <utmpos>();
// 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
}
var maxlane = 200; // (Centroid(utmpositions).GetDistance(utmpositions[0]) / distance);
ClipperLib.ClipperOffset clipperOffset = new ClipperLib.ClipperOffset();
clipperOffset.AddPath(utmpositions.Select(a => { return(new ClipperLib.IntPoint(a.x * 1000.0, a.y * 1000.0)); }).ToList(), ClipperLib.JoinType.jtMiter, ClipperLib.EndType.etClosedPolygon);
for (int lane = 0; lane < maxlane; lane++)
{
List <utmpos> ans1 = new List <utmpos>();
ClipperLib.PolyTree tree = new ClipperLib.PolyTree();
clipperOffset.Execute(ref tree, (Int64)(distance * 1000.0 * -lane));
if (tree.ChildCount == 0)
{
break;
}
ans1 = tree.GetFirst().Contour.Select(a => new utmpos(a.X / 1000.0, a.Y / 1000.0, utmzone)).ToList();
if (ans.Count() > 2)
{
var start1 = ans[ans.Count() - 1];
var end1 = ans[ans.Count() - 2];
var start2 = ans1[0];
var end2 = ans1[ans1.Count() - 1];
var intersection = FindLineIntersectionExtension(start1, end1, start2, end2);
if (intersection != utmpos.Zero)
{
//ans.Add(intersection);
}
}
ans.AddRange(ans1);
}
// set the altitude on all points
return(ans.Select(plla => { var a = plla.ToLLA(); a.Alt = altitude; a.Tag = "S"; return a; }).ToList());
}
public bool LoadFile(string filename)
{
FileName = filename;
log.InfoFormat("GeoTiff {0}", filename);
using (Tiff tiff = Tiff.Open(filename, "r"))
{
width = tiff.GetField(TiffTag.IMAGEWIDTH)[0].ToInt();
height = tiff.GetField(TiffTag.IMAGELENGTH)[0].ToInt();
bits = tiff.GetField(TiffTag.BITSPERSAMPLE)[0].ToInt();
//https://www.awaresystems.be/imaging/tiff/tifftags/sampleformat.html
type = tiff.GetField(TiffTag.SAMPLEFORMAT)[0].ToInt();
modelscale = tiff.GetField(TiffTag.GEOTIFF_MODELPIXELSCALETAG);
tiepoint = tiff.GetField(TiffTag.GEOTIFF_MODELTIEPOINTTAG);
for (int i = 0; i < tiff.GetTagListCount(); i += 1)
{
var tagno = tiff.GetTagListEntry(i);
var tag = (TiffTag)tagno;
var info = tiff.GetField((TiffTag)tagno);
log.InfoFormat("tiff ID={0} ? {1} len={2}", tag, (GKID)tagno, info.Length);
}
var GeoKeyDirectoryTag = tiff.GetField((TiffTag)34735);
var KeyDirectoryVersion = BitConverter.ToUInt16(GeoKeyDirectoryTag[1].ToByteArray(), 0);
var KeyRevision = BitConverter.ToUInt16(GeoKeyDirectoryTag[1].ToByteArray(), 2);
var MinorRevision = BitConverter.ToUInt16(GeoKeyDirectoryTag[1].ToByteArray(), 4);
var NumberOfKeys = BitConverter.ToUInt16(GeoKeyDirectoryTag[1].ToByteArray(), 6);
ProjectedCSTypeGeoKey = 0;
for (int i = 8; i < 8 + NumberOfKeys * 8; i += 8)
{
var KeyID = BitConverter.ToUInt16(GeoKeyDirectoryTag[1].ToByteArray(), i);
var TIFFTagLocation = BitConverter.ToUInt16(GeoKeyDirectoryTag[1].ToByteArray(), i + 2);
var Count = BitConverter.ToUInt16(GeoKeyDirectoryTag[1].ToByteArray(), i + 4);
var Value_Offset = BitConverter.ToUInt16(GeoKeyDirectoryTag[1].ToByteArray(), i + 6);
log.InfoFormat("GeoKeyDirectoryTag ID={0} TagLoc={1} Count={2} Value/offset={3}", (GKID)KeyID, TIFFTagLocation,
Count, Value_Offset);
// save it
if (TIFFTagLocation == 0)
{
GeoKeys[(GKID)KeyID] = Value_Offset;
}
else if (TIFFTagLocation == 34737)
{
GeoKeys[(GKID)KeyID] = Encoding.ASCII.GetString(tiff.GetField((TiffTag)TIFFTagLocation)[1].ToByteArray().Skip(Value_Offset).Take(Count).ToArray());
}
else if (TIFFTagLocation == 34736)
{
GeoKeys[(GKID)KeyID] = BitConverter.ToDouble(tiff.GetField((TiffTag)TIFFTagLocation)[1].ToByteArray().Skip(Value_Offset * 8).Take(Count * 8).ToArray(), 0);
}
else
{
GeoKeys[(GKID)KeyID] = Value_Offset;
}
if (KeyID == (int)GKID.ProjectedCSTypeGeoKey)
{
ProjectedCSTypeGeoKey = Value_Offset;
}
if (KeyID == (int)GKID.GTRasterTypeGeoKey)
{
GTRasterTypeGeoKey = Value_Offset;
}
if (KeyID == (int)GKID.ProjCoordTransGeoKey)
{
ProjCoordTransGeoKey = Value_Offset;
}
if (TIFFTagLocation != 0)
{
if (TIFFTagLocation == 34737) //ascii
{
var value = tiff.GetField((TiffTag)TIFFTagLocation)[1].ToByteArray().Skip(Value_Offset)
.Take(Count);
log.InfoFormat("GeoKeyDirectoryTag ID={0} Value={1}", (GKID)KeyID,
Encoding.ASCII.GetString(value.ToArray()));
}
if (TIFFTagLocation == 34736) // double
{
var value = tiff.GetField((TiffTag)TIFFTagLocation)[1].ToByteArray().Skip(Value_Offset * 8).Take(Count * 8);
log.InfoFormat("GeoKeyDirectoryTag ID={0} Value={1}", (GKID)KeyID, BitConverter.ToDouble(value.ToArray(), 0));
}
if (KeyID == (int)GKID.PCSCitationGeoKey)
{
var value = tiff.GetField((TiffTag)TIFFTagLocation)[1].ToByteArray().Skip(Value_Offset).Take(Count);
PCSCitationGeoKey = Encoding.ASCII.GetString(value.ToArray());
log.InfoFormat("GeoKeyDirectoryTag ID={0} Value={1}", (GKID)KeyID, Encoding.ASCII.GetString(value.ToArray()));
}
}
}
GeoAsciiParamsTag = tiff.GetField((TiffTag)34737);
if (GeoAsciiParamsTag != null && GeoAsciiParamsTag.Length == 2)
{
log.InfoFormat("GeoAsciiParamsTag 34737 {0}", GeoAsciiParamsTag[1]);
}
i = BitConverter.ToDouble(tiepoint[1].ToByteArray(), 0);
j = BitConverter.ToDouble(tiepoint[1].ToByteArray(), 0 + 8);
k = BitConverter.ToDouble(tiepoint[1].ToByteArray(), 0 + 16);
x = BitConverter.ToDouble(tiepoint[1].ToByteArray(), 0 + 24);
y = BitConverter.ToDouble(tiepoint[1].ToByteArray(), 0 + 32);
z = BitConverter.ToDouble(tiepoint[1].ToByteArray(), 0 + 40);
log.InfoFormat("Tie Point ({0},{1},{2}) --> ({3},{4},{5})", i, j, k, x, y, z);
xscale = BitConverter.ToDouble(modelscale[1].ToByteArray(), 0);
yscale = BitConverter.ToDouble(modelscale[1].ToByteArray(), 0 + 8);
zscale = BitConverter.ToDouble(modelscale[1].ToByteArray(), 0 + 16);
log.InfoFormat("Scale ({0},{1},{2})", xscale, yscale, zscale);
if (GTRasterTypeGeoKey == 1)
{
// starts from top left so x + y -
x += xscale / 2.0;
y -= yscale / 2.0;
}
if (ProjectedCSTypeGeoKey == 32767 && ProjCoordTransGeoKey == 15)
{ // user-defined
ProjectionInfo pStart = ProjectionInfo.FromProj4String($"+proj=stere +lat_ts={GeoKeys[GKID.ProjOriginLatGeoKey].ToString()} +lat_0=90 +lon_0={GeoKeys[GKID.ProjStraightVertPoleLongGeoKey].ToString()} +x_0=0 +y_0=0 +ellps={GeoKeys[GKID.GeogCitationGeoKey].ToString().Replace(" ", "").Replace("|", "")} +datum={GeoKeys[GKID.GeogCitationGeoKey].ToString().Replace(" ", "").Replace("|", "")} +units=m +no_defs ");
ProjectionInfo pESRIEnd = KnownCoordinateSystems.Geographic.World.WGS1984;
srcProjection = pStart;
double[] xyarray = { x, y,
x + width * xscale, y - height * yscale,
x + width * xscale, y,
x, y - height * yscale };
Reproject.ReprojectPoints(xyarray, null, pStart, pESRIEnd, 0, xyarray.Length / 2);
ymin = Math.Min(Math.Min(Math.Min(xyarray[1], xyarray[3]), xyarray[5]), xyarray[7]);
xmin = Math.Min(Math.Min(Math.Min(xyarray[0], xyarray[2]), xyarray[4]), xyarray[6]);
ymax = Math.Max(Math.Max(Math.Max(xyarray[1], xyarray[3]), xyarray[5]), xyarray[7]);
xmax = Math.Max(Math.Max(Math.Max(xyarray[0], xyarray[2]), xyarray[4]), xyarray[6]);
}
else if (ProjectedCSTypeGeoKey != 32767 && ProjectedCSTypeGeoKey != 0)
{
try
{
srcProjection = ProjectionInfo.FromEpsgCode(ProjectedCSTypeGeoKey);
ProjectionInfo pESRIEnd = KnownCoordinateSystems.Geographic.World.WGS1984;
double[] xyarray = { x, y,
x + width * xscale, y - height * yscale,
x + width * xscale, y,
x, y - height * yscale };
Reproject.ReprojectPoints(xyarray, null, srcProjection, pESRIEnd, 0, xyarray.Length / 2);
ymin = Math.Min(Math.Min(Math.Min(xyarray[1], xyarray[3]), xyarray[5]), xyarray[7]);
xmin = Math.Min(Math.Min(Math.Min(xyarray[0], xyarray[2]), xyarray[4]), xyarray[6]);
ymax = Math.Max(Math.Max(Math.Max(xyarray[1], xyarray[3]), xyarray[5]), xyarray[7]);
xmax = Math.Max(Math.Max(Math.Max(xyarray[0], xyarray[2]), xyarray[4]), xyarray[6]);
}
catch (Exception ex) { log.Error(ex); srcProjection = null; }
}
else
{
try
{
srcProjection = ProjectionInfo.FromEsriString(GeoKeys[GKID.PCSCitationGeoKey].ToString());
ProjectionInfo pESRIEnd = KnownCoordinateSystems.Geographic.World.WGS1984;
double[] xyarray = { x, y,
x + width * xscale, y - height * yscale,
x + width * xscale, y,
x, y - height * yscale };
Reproject.ReprojectPoints(xyarray, null, srcProjection, pESRIEnd, 0, xyarray.Length / 2);
ymin = Math.Min(Math.Min(Math.Min(xyarray[1], xyarray[3]), xyarray[5]), xyarray[7]);
xmin = Math.Min(Math.Min(Math.Min(xyarray[0], xyarray[2]), xyarray[4]), xyarray[6]);
ymax = Math.Max(Math.Max(Math.Max(xyarray[1], xyarray[3]), xyarray[5]), xyarray[7]);
xmax = Math.Max(Math.Max(Math.Max(xyarray[0], xyarray[2]), xyarray[4]), xyarray[6]);
}
catch (Exception ex) { log.Error(ex); srcProjection = null; }
}
if (srcProjection != null)
{
}
else
// wgs84 utm
if (ProjectedCSTypeGeoKey >= 32601 && ProjectedCSTypeGeoKey <= 32760)
{
if (ProjectedCSTypeGeoKey > 32700)
{
UTMZone = (ProjectedCSTypeGeoKey - 32700) * -1;
srcProjection = ProjectionInfo.FromProj4String($"+proj=utm +zone={UTMZone} +ellps=WGS84 +datum=WGS84 +units=m +no_defs ");
//tl
var pnt = PointLatLngAlt.FromUTM(UTMZone, x, y);
//br
var pnt2 = PointLatLngAlt.FromUTM(UTMZone, x + width * xscale,
y - height * yscale);
//tr
var pnt3 = PointLatLngAlt.FromUTM(UTMZone, x + width * xscale, y);
//bl
var pnt4 = PointLatLngAlt.FromUTM(UTMZone, x, y - height * yscale);
ymin = Math.Min(Math.Min(Math.Min(pnt.Lat, pnt2.Lat), pnt3.Lat), pnt4.Lat);
xmin = Math.Min(Math.Min(Math.Min(pnt.Lng, pnt2.Lng), pnt3.Lng), pnt4.Lng);
ymax = Math.Max(Math.Max(Math.Max(pnt.Lat, pnt2.Lat), pnt3.Lat), pnt4.Lat);
xmax = Math.Max(Math.Max(Math.Max(pnt.Lng, pnt2.Lng), pnt3.Lng), pnt4.Lng);
}
if (ProjectedCSTypeGeoKey < 32700)
{
UTMZone = ProjectedCSTypeGeoKey - 32600;
srcProjection = ProjectionInfo.FromProj4String($"+proj=utm +zone={UTMZone} +ellps=WGS84 +datum=WGS84 +units=m +no_defs ");
var pnt = PointLatLngAlt.FromUTM(UTMZone, x, y);
var pnt2 = PointLatLngAlt.FromUTM(UTMZone, x + width * xscale,
y - height * yscale);
var pnt3 = PointLatLngAlt.FromUTM(UTMZone, x + width * xscale, y);
var pnt4 = PointLatLngAlt.FromUTM(UTMZone, x, y - height * yscale);
ymin = Math.Min(Math.Min(Math.Min(pnt.Lat, pnt2.Lat), pnt3.Lat), pnt4.Lat);
xmin = Math.Min(Math.Min(Math.Min(pnt.Lng, pnt2.Lng), pnt3.Lng), pnt4.Lng);
ymax = Math.Max(Math.Max(Math.Max(pnt.Lat, pnt2.Lat), pnt3.Lat), pnt4.Lat);
xmax = Math.Max(Math.Max(Math.Max(pnt.Lng, pnt2.Lng), pnt3.Lng), pnt4.Lng);
}
}
else
// etrs89 utm
if (ProjectedCSTypeGeoKey >= 3038 && ProjectedCSTypeGeoKey <= 3051)
{
UTMZone = ProjectedCSTypeGeoKey - 3012;
srcProjection = ProjectionInfo.FromProj4String($"+proj=utm +zone={UTMZone} +ellps=GRS80 +units=m +no_defs ");
// 3038 - 26
var pnt = PointLatLngAlt.FromUTM(UTMZone, x, y);
var pnt2 = PointLatLngAlt.FromUTM(UTMZone, x + width * xscale,
y - height * yscale);
var pnt3 = PointLatLngAlt.FromUTM(UTMZone, x + width * xscale, y);
var pnt4 = PointLatLngAlt.FromUTM(UTMZone, x, y - height * yscale);
ymin = Math.Min(Math.Min(Math.Min(pnt.Lat, pnt2.Lat), pnt3.Lat), pnt4.Lat);
xmin = Math.Min(Math.Min(Math.Min(pnt.Lng, pnt2.Lng), pnt3.Lng), pnt4.Lng);
ymax = Math.Max(Math.Max(Math.Max(pnt.Lat, pnt2.Lat), pnt3.Lat), pnt4.Lat);
xmax = Math.Max(Math.Max(Math.Max(pnt.Lng, pnt2.Lng), pnt3.Lng), pnt4.Lng);
}
else
if (ProjectedCSTypeGeoKey >= 25828 && ProjectedCSTypeGeoKey <= 25838)
{
UTMZone = ProjectedCSTypeGeoKey - 25800;
// 3038 - 26
var pnt = PointLatLngAlt.FromUTM(UTMZone, x, y);
var pnt2 = PointLatLngAlt.FromUTM(UTMZone, x + width * xscale,
y - height * yscale);
var pnt3 = PointLatLngAlt.FromUTM(UTMZone, x + width * xscale, y);
var pnt4 = PointLatLngAlt.FromUTM(UTMZone, x, y - height * yscale);
ymin = Math.Min(Math.Min(Math.Min(pnt.Lat, pnt2.Lat), pnt3.Lat), pnt4.Lat);
xmin = Math.Min(Math.Min(Math.Min(pnt.Lng, pnt2.Lng), pnt3.Lng), pnt4.Lng);
ymax = Math.Max(Math.Max(Math.Max(pnt.Lat, pnt2.Lat), pnt3.Lat), pnt4.Lat);
xmax = Math.Max(Math.Max(Math.Max(pnt.Lng, pnt2.Lng), pnt3.Lng), pnt4.Lng);
}
else
/// gda94
if (ProjectedCSTypeGeoKey >= 28348 && ProjectedCSTypeGeoKey <= 28358)
{
UTMZone = (ProjectedCSTypeGeoKey - 28300) * -1;
var pnt = PointLatLngAlt.FromUTM(UTMZone, x, y);
var pnt2 = PointLatLngAlt.FromUTM(UTMZone, x + width * xscale,
y - height * yscale);
var pnt3 = PointLatLngAlt.FromUTM(UTMZone, x + width * xscale, y);
var pnt4 = PointLatLngAlt.FromUTM(UTMZone, x, y - height * yscale);
ymin = Math.Min(Math.Min(Math.Min(pnt.Lat, pnt2.Lat), pnt3.Lat), pnt4.Lat);
xmin = Math.Min(Math.Min(Math.Min(pnt.Lng, pnt2.Lng), pnt3.Lng), pnt4.Lng);
ymax = Math.Max(Math.Max(Math.Max(pnt.Lat, pnt2.Lat), pnt3.Lat), pnt4.Lat);
xmax = Math.Max(Math.Max(Math.Max(pnt.Lng, pnt2.Lng), pnt3.Lng), pnt4.Lng);
}
else
// geo lat/lng
if (ProjectedCSTypeGeoKey == 0 || ProjectedCSTypeGeoKey == 4326)
{
var pnt = new PointLatLngAlt(y, x);
var pnt2 = new PointLatLngAlt(y - height * yscale, x + width * xscale);
var pnt3 = new PointLatLngAlt(y, x + width * xscale);
var pnt4 = new PointLatLngAlt(y - height * yscale, x);
ymin = Math.Min(Math.Min(Math.Min(pnt.Lat, pnt2.Lat), pnt3.Lat), pnt4.Lat);
xmin = Math.Min(Math.Min(Math.Min(pnt.Lng, pnt2.Lng), pnt3.Lng), pnt4.Lng);
ymax = Math.Max(Math.Max(Math.Max(pnt.Lat, pnt2.Lat), pnt3.Lat), pnt4.Lat);
xmax = Math.Max(Math.Max(Math.Max(pnt.Lng, pnt2.Lng), pnt3.Lng), pnt4.Lng);
}
Area = new RectLatLng(ymax, xmin, xmax - xmin, ymax - ymin);
log.InfoFormat("Coverage {0}", Area.ToString());
log.InfoFormat("Start Point ({0},{1},{2}) --> ({3},{4},{5})", i, j, k, x, y, z);
lock (index)
GeoTiff.index.Add(this);
/*
*
* short numberOfDirectories = tiff.NumberOfDirectories();
* for (short d = 0; d < numberOfDirectories; ++d)
* {
* tiff.SetDirectory((short)d);
*
* for (ushort t = ushort.MinValue; t < ushort.MaxValue; ++t)
* {
* TiffTag tag = (TiffTag)t;
* FieldValue[] value = tiff.GetField(tag);
* if (value != null)
* {
* for (int j2 = 0; j2 < value.Length; j2++)
* {
* Console.WriteLine("{0} : {1} : {2}", tag.ToString(), value[j2].Value.GetType().ToString(), value[j2].ToString());
* }
* }
* }
* }
*/
}
return(true);
}
public static async Task <List <PointLatLngAlt> > CreateGridAsync(List <PointLatLngAlt> polygon, double altitude,
double distance, double spacing, double angle, double overshoot1, double overshoot2, StartPosition startpos,
bool shutter, float minLaneSeparation, float leadin, PointLatLngAlt HomeLocation)
{
return(await Task.Run((() => CreateGrid(polygon, altitude, distance, spacing, angle, overshoot1, overshoot2,
startpos, shutter, minLaneSeparation, leadin, HomeLocation))));
}
public bool setRallyPoint(byte index, PointLatLngAlt plla, short break_alt, UInt16 land_dir_cd, byte flags,
byte rallypointcount)
{
mavlink_rally_point_t rp = new mavlink_rally_point_t();
rp.idx = index;
rp.count = rallypointcount;
rp.lat = (int) (plla.Lat*t7);
rp.lng = (int) (plla.Lng*t7);
rp.alt = (short) plla.Alt;
rp.break_alt = break_alt;
rp.land_dir = land_dir_cd;
rp.flags = (byte) flags;
rp.target_component = MAV.compid;
rp.target_system = MAV.sysid;
int retry = 3;
while (retry > 0)
{
generatePacket((byte) MAVLINK_MSG_ID.RALLY_POINT, rp);
int counttemp = 0;
PointLatLngAlt newfp = getRallyPoint(rp.idx, ref counttemp);
if (newfp.Lat == plla.Lat && newfp.Lng == rp.lng)
{
Console.WriteLine("Rally Set");
return true;
}
retry--;
}
return false;
}
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, PointLatLngAlt HomeLocation)
{
//DoDebug();
if (spacing < 0.1 && spacing != 0)
{
spacing = 0.1;
}
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(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);
if (leadin < 0)
{
var p2 = new utmpos(newstart)
{
Tag = "SM"
};
addtomap(p2, "SM");
ans.Add(p2);
}
else
{
closest.p1.Tag = "SM";
addtomap(closest.p1, "SM");
ans.Add(closest.p1);
}
if (spacing > 0)
{
for (double d = (spacing - ((closest.basepnt.GetDistance(closest.p1)) % spacing));
d < (closest.p1.GetDistance(closest.p2));
d += 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);
}
}
utmpos newend = newpos(closest.p2, angle, overshoot1);
if (overshoot1 < 0)
{
var p2 = new utmpos(newend)
{
Tag = "ME"
};
addtomap(p2, "ME");
ans.Add(p2);
}
else
{
closest.p2.Tag = "ME";
addtomap(closest.p2, "ME");
ans.Add(closest.p2);
}
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);
if (leadin < 0)
{
var p2 = new utmpos(newstart)
{
Tag = "SM"
};
addtomap(p2, "SM");
ans.Add(p2);
}
else
{
closest.p2.Tag = "SM";
addtomap(closest.p2, "SM");
ans.Add(closest.p2);
}
if (spacing > 0)
{
for (double d = ((closest.basepnt.GetDistance(closest.p2)) % spacing);
d < (closest.p1.GetDistance(closest.p2));
d += 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);
}
}
utmpos newend = newpos(closest.p1, angle, -overshoot2);
if (overshoot2 < 0)
{
var p2 = new utmpos(newend)
{
Tag = "ME"
};
addtomap(p2, "ME");
ans.Add(p2);
}
else
{
closest.p1.Tag = "ME";
addtomap(closest.p1, "ME");
ans.Add(closest.p1);
}
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);
}
}
// set the altitude on all points
ans.ForEach(plla => { plla.Alt = altitude; });
return(ans);
}
public void doConnect(MAVLinkInterface comPort, string portname, string baud)
{
bool skipconnectcheck = false;
log.Info("We are connecting");
switch (portname)
{
case "preset":
skipconnectcheck = true;
break;
case "TCP":
comPort.BaseStream = new TcpSerial();
_connectionControl.CMB_serialport.Text = "TCP";
break;
case "UDP":
comPort.BaseStream = new UdpSerial();
_connectionControl.CMB_serialport.Text = "UDP";
break;
case "UDPCl":
comPort.BaseStream = new UdpSerialConnect();
_connectionControl.CMB_serialport.Text = "UDPCl";
break;
case "AUTO":
default:
comPort.BaseStream = new SerialPort();
break;
}
// Tell the connection UI that we are now connected.
_connectionControl.IsConnected(true);
// Here we want to reset the connection stats counter etc.
this.ResetConnectionStats();
comPort.MAV.cs.ResetInternals();
//cleanup any log being played
comPort.logreadmode = false;
if (comPort.logplaybackfile != null)
comPort.logplaybackfile.Close();
comPort.logplaybackfile = null;
try
{
// do autoscan
if (portname == "AUTO")
{
Comms.CommsSerialScan.Scan(false);
DateTime deadline = DateTime.Now.AddSeconds(50);
while (Comms.CommsSerialScan.foundport == false)
{
System.Threading.Thread.Sleep(100);
if (DateTime.Now > deadline)
{
CustomMessageBox.Show(Strings.Timeout);
_connectionControl.IsConnected(false);
return;
}
}
_connectionControl.CMB_serialport.Text = portname = Comms.CommsSerialScan.portinterface.PortName;
_connectionControl.CMB_baudrate.Text = baud = Comms.CommsSerialScan.portinterface.BaudRate.ToString();
}
log.Info("Set Portname");
// set port, then options
comPort.BaseStream.PortName = portname;
log.Info("Set Baudrate");
try
{
comPort.BaseStream.BaudRate = int.Parse(baud);
}
catch (Exception exp)
{
log.Error(exp);
}
// prevent serialreader from doing anything
comPort.giveComport = true;
log.Info("About to do dtr if needed");
// reset on connect logic.
if (config["CHK_resetapmonconnect"] == null || bool.Parse(config["CHK_resetapmonconnect"].ToString()) == true)
{
log.Info("set dtr rts to false");
comPort.BaseStream.DtrEnable = false;
comPort.BaseStream.RtsEnable = false;
comPort.BaseStream.toggleDTR();
}
comPort.giveComport = false;
// setup to record new logs
try
{
Directory.CreateDirectory(MainV2.LogDir);
comPort.logfile = new BufferedStream(File.Open(MainV2.LogDir + Path.DirectorySeparatorChar + DateTime.Now.ToString("yyyy-MM-dd HH-mm-ss") + ".tlog", FileMode.CreateNew, FileAccess.ReadWrite, FileShare.None));
comPort.rawlogfile = new BufferedStream(File.Open(MainV2.LogDir + Path.DirectorySeparatorChar + DateTime.Now.ToString("yyyy-MM-dd HH-mm-ss") + ".rlog", FileMode.CreateNew, FileAccess.ReadWrite, FileShare.None));
log.Info("creating logfile " + DateTime.Now.ToString("yyyy-MM-dd HH-mm-ss") + ".tlog");
}
catch (Exception exp2) { log.Error(exp2); CustomMessageBox.Show(Strings.Failclog); } // soft fail
// reset connect time - for timeout functions
connecttime = DateTime.Now;
// do the connect
comPort.Open(false, skipconnectcheck);
if (!comPort.BaseStream.IsOpen)
{
log.Info("comport is closed. existing connect");
try
{
_connectionControl.IsConnected(false);
UpdateConnectIcon();
comPort.Close();
}
catch { }
return;
}
// 3dr radio is hidden as no hb packet is ever emitted
if (comPort.sysidseen.Count > 1)
{
// we have more than one mav
// user selection of sysid
MissionPlanner.Controls.SysidSelector id = new SysidSelector();
id.Show();
}
// create a copy
int[] list = comPort.sysidseen.ToArray();
// get all the params
foreach (var sysid in list)
{
comPort.sysidcurrent = sysid;
comPort.getParamList();
}
// set to first seen
comPort.sysidcurrent = list[0];
// detect firmware we are conected to.
if (comPort.MAV.cs.firmware == Firmwares.ArduCopter2)
{
_connectionControl.TOOL_APMFirmware.SelectedIndex = _connectionControl.TOOL_APMFirmware.Items.IndexOf(Firmwares.ArduCopter2);
}
else if (comPort.MAV.cs.firmware == Firmwares.Ateryx)
{
_connectionControl.TOOL_APMFirmware.SelectedIndex = _connectionControl.TOOL_APMFirmware.Items.IndexOf(Firmwares.Ateryx);
}
else if (comPort.MAV.cs.firmware == Firmwares.ArduRover)
{
_connectionControl.TOOL_APMFirmware.SelectedIndex = _connectionControl.TOOL_APMFirmware.Items.IndexOf(Firmwares.ArduRover);
}
else if (comPort.MAV.cs.firmware == Firmwares.ArduPlane)
{
_connectionControl.TOOL_APMFirmware.SelectedIndex = _connectionControl.TOOL_APMFirmware.Items.IndexOf(Firmwares.ArduPlane);
}
// check for newer firmware
var softwares = Firmware.LoadSoftwares();
if (softwares.Count > 0)
{
try
{
string[] fields1 = comPort.MAV.VersionString.Split(' ');
foreach (Firmware.software item in softwares)
{
string[] fields2 = item.name.Split(' ');
// check primare firmware type. ie arudplane, arducopter
if (fields1[0] == fields2[0])
{
Version ver1 = VersionDetection.GetVersion(comPort.MAV.VersionString);
Version ver2 = VersionDetection.GetVersion(item.name);
if (ver2 > ver1)
{
Common.MessageShowAgain(Strings.NewFirmware, Strings.NewFirmwareA + item.name + Strings.Pleaseup);
break;
}
// check the first hit only
break;
}
}
}
catch (Exception ex) { log.Error(ex); }
}
FlightData.CheckBatteryShow();
MissionPlanner.Utilities.Tracking.AddEvent("Connect", "Connect", comPort.MAV.cs.firmware.ToString(), comPort.MAV.param.Count.ToString());
MissionPlanner.Utilities.Tracking.AddTiming("Connect", "Connect Time", (DateTime.Now - connecttime).TotalMilliseconds, "");
MissionPlanner.Utilities.Tracking.AddEvent("Connect", "Baud", comPort.BaseStream.BaudRate.ToString(), "");
// save the baudrate for this port
config[_connectionControl.CMB_serialport.Text + "_BAUD"] = _connectionControl.CMB_baudrate.Text;
this.Text = titlebar + " " + comPort.MAV.VersionString;
// refresh config window if needed
if (MyView.current != null)
{
if (MyView.current.Name == "HWConfig")
MyView.ShowScreen("HWConfig");
if (MyView.current.Name == "SWConfig")
MyView.ShowScreen("SWConfig");
}
// load wps on connect option.
if (config["loadwpsonconnect"] != null && bool.Parse(config["loadwpsonconnect"].ToString()) == true)
{
// only do it if we are connected.
if (comPort.BaseStream.IsOpen)
{
MenuFlightPlanner_Click(null, null);
FlightPlanner.BUT_read_Click(null, null);
}
}
// get any rallypoints
if (MainV2.comPort.MAV.param.ContainsKey("RALLY_TOTAL") && int.Parse(MainV2.comPort.MAV.param["RALLY_TOTAL"].ToString()) > 0)
{
FlightPlanner.getRallyPointsToolStripMenuItem_Click(null, null);
double maxdist = 0;
foreach (var rally in comPort.MAV.rallypoints)
{
foreach (var rally1 in comPort.MAV.rallypoints)
{
var pnt1 = new PointLatLngAlt(rally.Value.lat / 10000000.0f, rally.Value.lng / 10000000.0f);
var pnt2 = new PointLatLngAlt(rally1.Value.lat / 10000000.0f, rally1.Value.lng / 10000000.0f);
var dist = pnt1.GetDistance(pnt2);
maxdist = Math.Max(maxdist, dist);
}
}
if (comPort.MAV.param.ContainsKey("RALLY_LIMIT_KM") && (maxdist / 1000.0) > (float)comPort.MAV.param["RALLY_LIMIT_KM"])
{
CustomMessageBox.Show(Strings.Warningrallypointdistance + " " + (maxdist / 1000.0).ToString("0.00") + " > " + (float)comPort.MAV.param["RALLY_LIMIT_KM"]);
}
}
// set connected icon
this.MenuConnect.Image = displayicons.disconnect;
}
catch (Exception ex)
{
log.Warn(ex);
try
{
_connectionControl.IsConnected(false);
UpdateConnectIcon();
comPort.Close();
}
catch (Exception ex2)
{
log.Warn(ex2);
}
CustomMessageBox.Show("Can not establish a connection\n\n" + ex.Message);
return;
}
}
public string SUBMITHOST; // Submitter Host “1.2.3.4” or “enduser5.faa.gov”
public List <List <PointLatLng> > GetPaths()
{
//RLN27.576944W97.108611LN27.468056W96.961111LN27.322222W97.050000LN27.345833W97.088889LN27.439167W97.186944RLN27.672778W97.212222LN27.576944W97.108611LN27.533333W97.133333LN27.638333W97.237222RCN27.686333W97.294667R007.00
List <List <PointLatLng> > list = new List <List <PointLatLng> >();
List <PointLatLng> pointlist = new List <PointLatLng>();
var matches = all.Matches(BOUND);
Console.WriteLine(BOUND);
bool isarcterminate = false;
bool iscircleterminate = false;
int arcdir = 0;
PointLatLngAlt pointcent = null;
PointLatLngAlt pointstart = null;
foreach (Match item in matches)
{
try
{
if (item.Groups[2].Value == "L")
{
var point = new PointLatLngAlt(double.Parse(item.Groups[4].Value, CultureInfo.InvariantCulture), double.Parse(item.Groups[6].Value, CultureInfo.InvariantCulture));
if (item.Groups[3].Value == "S")
{
point.Lat *= -1;
}
if (item.Groups[5].Value == "W")
{
point.Lng *= -1;
}
if (isarcterminate)
{
double radius = pointcent.GetDistance(pointstart);
double startbearing = pointcent.GetBearing(pointstart);
double endbearing = pointcent.GetBearing(point);
if (arcdir > 0 && endbearing < startbearing)
{
endbearing += 360;
}
if (arcdir < 0)
{
for (double a = startbearing; a > endbearing; a += (10 * arcdir))
{
pointlist.Add(pointcent.newpos(a, radius));
}
}
else
{
for (double a = startbearing; a < endbearing; a += (10 * arcdir))
{
pointlist.Add(pointcent.newpos(a, radius));
}
}
pointlist.Add(point);
list.Add(pointlist);
pointlist = new List <PointLatLng>();
isarcterminate = false;
iscircleterminate = false;
continue;
}
if (iscircleterminate)
{
iscircleterminate = false;
continue;
}
pointlist.Add(point);
continue;
}
else if (item.Groups[7].Value == "A")
{
pointcent = new PointLatLngAlt(double.Parse(item.Groups[10].Value, CultureInfo.InvariantCulture), double.Parse(item.Groups[12].Value, CultureInfo.InvariantCulture));
if (item.Groups[9].Value == "S")
{
pointcent.Lat *= -1;
}
if (item.Groups[11].Value == "W")
{
pointcent.Lng *= -1;
}
pointstart = new PointLatLngAlt(double.Parse(item.Groups[14].Value, CultureInfo.InvariantCulture), double.Parse(item.Groups[16].Value, CultureInfo.InvariantCulture));
if (item.Groups[13].Value == "S")
{
pointstart.Lat *= -1;
}
if (item.Groups[15].Value == "W")
{
pointstart.Lng *= -1;
}
arcdir = item.Groups[8].Value == "+" ? 1 : -1;
isarcterminate = true;
continue;
}
else if (item.Groups[17].Value == "C")
{
var point = new PointLatLngAlt(double.Parse(item.Groups[19].Value, CultureInfo.InvariantCulture), double.Parse(item.Groups[21].Value, CultureInfo.InvariantCulture));
if (item.Groups[18].Value == "S")
{
point.Lat *= -1;
}
if (item.Groups[20].Value == "W")
{
point.Lng *= -1;
}
// radius in m from nautical miles
double radius = double.Parse(item.Groups[22].Value, CultureInfo.InvariantCulture) * 1852;
for (int a = 0; a <= 360; a += 10)
{
pointlist.Add(point.newpos(a, radius));
}
list.Add(pointlist);
pointlist = new List <PointLatLng>();
iscircleterminate = true;
continue;
}
}
catch { }
}
return(list);
}
private void map_MouseUp(object sender, MouseEventArgs e)
{
MouseDownEnd = map.FromLocalToLatLng(e.X, e.Y);
// Console.WriteLine("MainMap MU");
if (e.Button == MouseButtons.Right) // ignore right clicks
{
return;
}
if (isMouseDown) // mouse down on some other object and dragged to here.
{
if (e.Button == MouseButtons.Left)
{
isMouseDown = false;
}
if (!isMouseDraging)
{
if (CurrentGMapMarker != null)
{
// Redraw polygon
//AddDrawPolygon();
}
}
}
isMouseDraging = false;
CurrentGMapMarker = null;
CurrentGMapMarkerIndex = 0;
CurrentGMapMarkerStartPos = null;
}
PointLatLngAlt getSRTMAltPath(List <PointLatLngAlt> list, float triangle, ref bool carryon, ref bool up, ref bool down)
{
PointLatLngAlt answer = new PointLatLngAlt();
PointLatLngAlt last = list[0];
PointLatLngAlt loc = list[1];
double disttotal = 0;
int a = 0;
double elev = 0;
double height = 0;
double dist = last.GetDistance(loc);
int points = (int)(dist / 10) + 1;
double deltalat = (last.Lat - loc.Lat);
double deltalng = (last.Lng - loc.Lng);
double steplat = deltalat / points;
double steplng = deltalng / points;
PointLatLngAlt lastpnt = last;
while (a <= points && elev < homealt + base_height + height && elev < drone_alt - clearance)
{
double lat = last.Lat - steplat * a;
double lng = last.Lng - steplng * a;
var newpoint = new PointLatLngAlt(lat, lng, srtm.getAltitude(lat, lng).alt, "");
double subdist = lastpnt.GetDistance(newpoint);
disttotal += subdist;
height = disttotal * Math.Tan(triangle); //Height of traingle formed at point with projected line
// srtm alts
answer = newpoint;
elev = newpoint.Alt;
lastpnt = newpoint;
a++;
}
if (elev < homealt + base_height + height + 0.1 && elev > homealt + base_height + height - 0.1 &&
elev < drone_alt - clearance + 0.1 && elev > drone_alt - clearance - 0.1) //Terrain intercept found
{
carryon = false;
}
else
{
if (elev > homealt + base_height + height && elev > drone_alt - clearance)
{
up = true;
down = false;
}
else
{
down = true;
up = false;
}
}
return(answer);
}
private void map_OnMarkerEnter(GMapMarker item)
{
if (!isMouseDown)
{
if (item is GMapMarker)
{
CurrentGMapMarker = item as GMapMarker;
CurrentGMapMarkerStartPos = CurrentGMapMarker.Position;
}
}
}
public static async Task <List <PointLatLngAlt> > CreateRotaryAsync(List <PointLatLngAlt> polygon, double altitude, double distance, double spacing, double angle, double overshoot1, double overshoot2, StartPosition startpos, bool shutter, float minLaneSeparation, float leadin, PointLatLngAlt HomeLocation, int clockwise_laps, bool match_spiral_perimeter)
{
return(await Task.Run((() => CreateRotary(polygon, altitude, distance, spacing, angle, overshoot1, overshoot2,
startpos, shutter, minLaneSeparation, leadin, HomeLocation, clockwise_laps, match_spiral_perimeter))).ConfigureAwait(false));
}
