private void updateLocationLabel(Locationwp plla)
{
this.BeginInvoke((MethodInvoker) delegate
{
LBL_location.Text = gotolocation.Lat + " " + gotolocation.Lng + " " + gotolocation.Alt + " " + gotolocation.Tag;
}
);
}
public PointLatLngAlt(Locationwp locwp)
{
this.Lat = locwp.lat;
this.Lng = locwp.lng;
this.Alt = locwp.alt;
}
private void updateLocationLabel(Locationwp plla)
{
this.BeginInvoke((MethodInvoker)delegate
{
LBL_location.Text = gotolocation.Lat + " " + gotolocation.Lng + " " + gotolocation.Alt +" "+ gotolocation.Tag;
}
);
}
void mainloop()
{
DateTime nextsend = DateTime.Now;
threadrun = true;
while (threadrun)
{
try
{
string line = comPort.ReadLine();
//string line = string.Format("$GP{0},{1:HHmmss},{2},{3},{4},{5},{6},{7},{8},{9},{10},{11},{12},{13},", "GGA", DateTime.Now.ToUniversalTime(), Math.Abs(lat * 100), MainV2.comPort.MAV.cs.lat < 0 ? "S" : "N", Math.Abs(lng * 100), MainV2.comPort.MAV.cs.lng < 0 ? "W" : "E", MainV2.comPort.MAV.cs.gpsstatus, MainV2.comPort.MAV.cs.satcount, MainV2.comPort.MAV.cs.gpshdop, MainV2.comPort.MAV.cs.alt, "M", 0, "M", "");
if (line.StartsWith("$GPGGA")) //
{
string[] items = line.Trim().Split(',','*');
if (items[15] != GetChecksum(line.Trim()))
{
Console.WriteLine("Bad Nmea line " + items[15] + " vs " + GetChecksum(line.Trim()));
continue;
}
if (items[6] == "0")
{
Console.WriteLine("No Fix");
continue;
}
gotolocation.Lat = double.Parse(items[2], CultureInfo.InvariantCulture) / 100.0;
gotolocation.Lat = (int)gotolocation.Lat + ((gotolocation.Lat - (int)gotolocation.Lat) / 0.60);
if (items[3] == "S")
gotolocation.Lat *= -1;
gotolocation.Lng = double.Parse(items[4], CultureInfo.InvariantCulture) / 100.0;
gotolocation.Lng = (int)gotolocation.Lng + ((gotolocation.Lng - (int)gotolocation.Lng) / 0.60);
if (items[5] == "W")
gotolocation.Lng *= -1;
gotolocation.Alt = intalt; // double.Parse(line.Substring(c9, c10 - c9 - 1)) +
gotolocation.Tag = "Sats "+ items[7] + " hdop " + items[8] ;
}
if (DateTime.Now > nextsend && gotolocation.Lat != 0 && gotolocation.Lng != 0 && gotolocation.Alt != 0) // 200 * 10 = 2 sec /// lastgotolocation != gotolocation &&
{
nextsend = DateTime.Now.AddSeconds(2);
Console.WriteLine("Sending follow wp " +DateTime.Now.ToString("h:MM:ss")+" "+ gotolocation.Lat + " " + gotolocation.Lng + " " +gotolocation.Alt);
lastgotolocation = new PointLatLngAlt(gotolocation);
Locationwp gotohere = new Locationwp();
gotohere.id = (byte)MAVLink.MAV_CMD.WAYPOINT;
gotohere.alt = (float)(gotolocation.Alt);
gotohere.lat = (gotolocation.Lat);
gotohere.lng = (gotolocation.Lng);
try
{
updateLocationLabel(gotohere);
}
catch { }
if (MainV2.comPort.BaseStream.IsOpen && MainV2.comPort.giveComport == false)
{
try
{
MainV2.comPort.giveComport = true;
MainV2.comPort.setGuidedModeWP(gotohere);
MainV2.comPort.giveComport = false;
}
catch { MainV2.comPort.giveComport = false; }
}
}
}
catch { System.Threading.Thread.Sleep(2000); }
}
}
/// <summary>
/// Save wp to eeprom
/// </summary>
/// <param name="loc">location struct</param>
/// <param name="index">wp no</param>
/// <param name="frame">global or relative</param>
/// <param name="current">0 = no , 2 = guided mode</param>
public void setWP(Locationwp loc, ushort index, MAV_FRAME frame, byte current)
{
MainV2.givecomport = true;
#if MAVLINK10
__mavlink_mission_item_t req = new __mavlink_mission_item_t();
#else
__mavlink_waypoint_t req = new __mavlink_waypoint_t();
#endif
req.target_system = sysid;
req.target_component = compid; // MAVLINK_MSG_ID_MISSION_ITEM
req.command = loc.id;
req.param1 = loc.p1;
req.current = current;
req.frame = (byte)frame;
req.y = (float)(loc.lng);
req.x = (float)(loc.lat);
req.z = (float)(loc.alt);
req.param1 = loc.p1;
req.param2 = loc.p2;
req.param3 = loc.p3;
req.param4 = loc.p4;
/*
if (MainV2.cs.firmware == MainV2.Firmwares.ArduPlane)
{
switch (loc.id)
{ // Switch to map APM command fields inot MAVLink command fields
case (byte)MAV_CMD.LOITER_TURNS:
case (byte)MAV_CMD.TAKEOFF:
req.param1 = loc.p1;
break;
case (byte)MAV_CMD.DO_SET_HOME:
req.param1 = loc.p1;
break;
case (byte)MAV_CMD.CONDITION_CHANGE_ALT:
req.param1 = loc.lat;
req.x = 0;
req.y = 0;
break;
case (byte)MAV_CMD.LOITER_TIME:
req.param1 = loc.p1 * 10; // APM loiter time is in ten second increments
break;
case (byte)MAV_CMD.CONDITION_DELAY:
case (byte)MAV_CMD.CONDITION_DISTANCE:
req.param1 = loc.lat;
break;
case (byte)MAV_CMD.DO_JUMP:
req.param2 = loc.lat;
req.param1 = loc.p1;
break;
case (byte)MAV_CMD.DO_REPEAT_SERVO:
req.param4 = loc.lng;
goto case (byte)MAV_CMD.DO_CHANGE_SPEED;
case (byte)MAV_CMD.DO_REPEAT_RELAY:
case (byte)MAV_CMD.DO_CHANGE_SPEED:
req.param3 = loc.lat;
req.param2 = loc.alt;
req.param1 = loc.p1;
break;
case (byte)MAV_CMD.DO_SET_PARAMETER:
case (byte)MAV_CMD.DO_SET_RELAY:
case (byte)MAV_CMD.DO_SET_SERVO:
req.param2 = loc.alt;
req.param1 = loc.p1;
break;
}
}
*/
req.seq = index;
Console.WriteLine("setWP {6} frame {0} cmd {1} p1 {2} x {3} y {4} z {5}", req.frame, req.command, req.param1, req.x, req.y, req.z, index);
// request
#if MAVLINK10
generatePacket(MAVLINK_MSG_ID_MISSION_ITEM, req);
#else
generatePacket(MAVLINK_MSG_ID_WAYPOINT, req);
#endif
DateTime start = DateTime.Now;
int retrys = 6;
while (true)
{
if (!(start.AddMilliseconds(500) > DateTime.Now))
{
if (retrys > 0)
{
Console.WriteLine("setWP Retry " + retrys);
#if MAVLINK10
generatePacket(MAVLINK_MSG_ID_MISSION_ITEM, req);
#else
generatePacket(MAVLINK_MSG_ID_WAYPOINT, req);
#endif
start = DateTime.Now;
retrys--;
continue;
}
MainV2.givecomport = false;
throw new Exception("Timeout on read - setWP");
}
byte[] buffer = readPacket();
if (buffer.Length > 5)
{
#if MAVLINK10
if (buffer[5] == MAVLINK_MSG_ID_MISSION_ACK)
{
__mavlink_mission_ack_t ans = new __mavlink_mission_ack_t();
object temp = (object)ans;
ByteArrayToStructure(buffer, ref temp, 6);
ans = (__mavlink_mission_ack_t)(temp);
Console.WriteLine("set wp " + index + " ACK 47 : " + buffer[5] + " ans " + Enum.Parse(typeof(MAV_MISSION_RESULT), ans.type.ToString()));
break;
}
else if (buffer[5] == MAVLINK_MSG_ID_MISSION_REQUEST)
{
__mavlink_mission_request_t ans = new __mavlink_mission_request_t();
object temp = (object)ans;
ByteArrayToStructure(buffer, ref temp, 6);
ans = (__mavlink_mission_request_t)(temp);
if (ans.seq == (index + 1))
{
Console.WriteLine("set wp doing " + index + " req " + ans.seq + " REQ 40 : " + buffer[5]);
MainV2.givecomport = false;
break;
}
else
{
Console.WriteLine("set wp fail doing " + index + " req " + ans.seq + " ACK 47 or REQ 40 : " + buffer[5] + " seq {0} ts {1} tc {2}", req.seq, req.target_system, req.target_component);
//break;
}
}
else
{
//Console.WriteLine(DateTime.Now + " PC setwp " + buffer[5]);
}
#else
if (buffer[5] == MAVLINK_MSG_ID_WAYPOINT_ACK)
{ //__mavlink_waypoint_request_t
Console.WriteLine("set wp " + index + " ACK 47 : " + buffer[5]);
break;
}
else if (buffer[5] == MAVLINK_MSG_ID_WAYPOINT_REQUEST)
{
__mavlink_waypoint_request_t ans = new __mavlink_waypoint_request_t();
object temp = (object)ans;
//Array.Copy(buffer, 6, buffer, 0, buffer.Length - 6);
ByteArrayToStructure(buffer, ref temp, 6);
ans = (__mavlink_waypoint_request_t)(temp);
if (ans.seq == (index + 1))
{
Console.WriteLine("set wp doing " + index + " req " + ans.seq + " REQ 40 : " + buffer[5]);
MainV2.givecomport = false;
break;
}
else
{
Console.WriteLine("set wp fail doing " + index + " req " + ans.seq + " ACK 47 or REQ 40 : " + buffer[5] + " seq {0} ts {1} tc {2}", req.seq, req.target_system, req.target_component);
//break;
}
}
else
{
//Console.WriteLine(DateTime.Now + " PC setwp " + buffer[5]);
}
#endif
}
}
}
/// <summary>
/// Gets specfied WP
/// </summary>
/// <param name="index"></param>
/// <returns>WP</returns>
public Locationwp getWP(ushort index)
{
MainV2.givecomport = true;
Locationwp loc = new Locationwp();
#if MAVLINK10
__mavlink_mission_request_t req = new __mavlink_mission_request_t();
req.target_system = sysid;
req.target_component = compid;
req.seq = index;
//Console.WriteLine("getwp req "+ DateTime.Now.Millisecond);
// request
generatePacket(MAVLINK_MSG_ID_MISSION_REQUEST, req);
DateTime start = DateTime.Now;
int retrys = 5;
while (true)
{
if (!(start.AddMilliseconds(800) > DateTime.Now)) // apm times out after 1000ms
{
if (retrys > 0)
{
Console.WriteLine("getWP Retry " + retrys);
generatePacket(MAVLINK_MSG_ID_MISSION_REQUEST, req);
start = DateTime.Now;
retrys--;
continue;
}
MainV2.givecomport = false;
throw new Exception("Timeout on read - getWP");
}
//Console.WriteLine("getwp read " + DateTime.Now.Millisecond);
byte[] buffer = readPacket();
//Console.WriteLine("getwp readend " + DateTime.Now.Millisecond);
if (buffer.Length > 5)
{
if (buffer[5] == MAVLINK_MSG_ID_MISSION_ITEM)
{
//Console.WriteLine("getwp ans " + DateTime.Now.Millisecond);
__mavlink_mission_item_t wp = new __mavlink_mission_item_t();
object temp = (object)wp;
//Array.Copy(buffer, 6, buffer, 0, buffer.Length - 6);
ByteArrayToStructure(buffer, ref temp, 6);
wp = (__mavlink_mission_item_t)(temp);
#else
__mavlink_waypoint_request_t req = new __mavlink_waypoint_request_t();
req.target_system = sysid;
req.target_component = compid;
req.seq = index;
//Console.WriteLine("getwp req "+ DateTime.Now.Millisecond);
// request
generatePacket(MAVLINK_MSG_ID_WAYPOINT_REQUEST, req);
DateTime start = DateTime.Now;
int retrys = 5;
while (true)
{
if (!(start.AddMilliseconds(800) > DateTime.Now)) // apm times out after 1000ms
{
if (retrys > 0)
{
Console.WriteLine("getWP Retry " + retrys);
generatePacket(MAVLINK_MSG_ID_WAYPOINT_REQUEST, req);
start = DateTime.Now;
retrys--;
continue;
}
MainV2.givecomport = false;
throw new Exception("Timeout on read - getWP");
}
//Console.WriteLine("getwp read " + DateTime.Now.Millisecond);
byte[] buffer = readPacket();
//Console.WriteLine("getwp readend " + DateTime.Now.Millisecond);
if (buffer.Length > 5)
{
if (buffer[5] == MAVLINK_MSG_ID_WAYPOINT)
{
//Console.WriteLine("getwp ans " + DateTime.Now.Millisecond);
__mavlink_waypoint_t wp = new __mavlink_waypoint_t();
object temp = (object)wp;
//Array.Copy(buffer, 6, buffer, 0, buffer.Length - 6);
ByteArrayToStructure(buffer, ref temp, 6);
wp = (__mavlink_waypoint_t)(temp);
#endif
loc.options = (byte)(wp.frame & 0x1);
loc.id = (byte)(wp.command);
loc.p1 = (wp.param1);
loc.p2 = (wp.param2);
loc.p3 = (wp.param3);
loc.p4 = (wp.param4);
loc.alt = ((wp.z));
loc.lat = ((wp.x));
loc.lng = ((wp.y));
/*
if (MainV2.cs.firmware == MainV2.Firmwares.ArduPlane)
{
switch (loc.id)
{ // Switch to map APM command fields inot MAVLink command fields
case (byte)MAV_CMD.LOITER_TURNS:
case (byte)MAV_CMD.TAKEOFF:
case (byte)MAV_CMD.DO_SET_HOME:
//case (byte)MAV_CMD.DO_SET_ROI:
loc.alt = (float)((wp.z));
loc.lat = (float)((wp.x));
loc.lng = (float)((wp.y));
loc.p1 = (float)wp.param1;
break;
case (byte)MAV_CMD.CONDITION_CHANGE_ALT:
loc.lat = (int)wp.param1;
loc.p1 = 0;
break;
case (byte)MAV_CMD.LOITER_TIME:
if (MainV2.APMFirmware == MainV2.Firmwares.ArduPlane)
{
loc.p1 = (byte)(wp.param1 / 10); // APM loiter time is in ten second increments
}
else
{
loc.p1 = (byte)wp.param1;
}
break;
case (byte)MAV_CMD.CONDITION_DELAY:
case (byte)MAV_CMD.CONDITION_DISTANCE:
loc.lat = (int)wp.param1;
break;
case (byte)MAV_CMD.DO_JUMP:
loc.lat = (int)wp.param2;
loc.p1 = (byte)wp.param1;
break;
case (byte)MAV_CMD.DO_REPEAT_SERVO:
loc.lng = (int)wp.param4;
goto case (byte)MAV_CMD.DO_CHANGE_SPEED;
case (byte)MAV_CMD.DO_REPEAT_RELAY:
case (byte)MAV_CMD.DO_CHANGE_SPEED:
loc.lat = (int)wp.param3;
loc.alt = (int)wp.param2;
loc.p1 = (byte)wp.param1;
break;
case (byte)MAV_CMD.DO_SET_PARAMETER:
case (byte)MAV_CMD.DO_SET_RELAY:
case (byte)MAV_CMD.DO_SET_SERVO:
loc.alt = (int)wp.param2;
loc.p1 = (byte)wp.param1;
break;
case (byte)MAV_CMD.WAYPOINT:
loc.p1 = (byte)wp.param1;
break;
}
}
*/
Console.WriteLine("getWP {0} {1} {2} {3} {4} opt {5}", loc.id, loc.p1, loc.alt, loc.lat, loc.lng, loc.options);
break;
}
else
{
Console.WriteLine(DateTime.Now + " PC getwp " + buffer[5]);
}
}
}
MainV2.givecomport = false;
return loc;
}
public object DebugPacket(byte[] datin)
{
string text = "";
return DebugPacket(datin, ref text);
}
public PointLatLngAlt(Locationwp locwp)
{
this.Lat = locwp.lat;
this.Lng = locwp.lng;
this.Alt = locwp.alt;
}
/// <summary>
/// Save wp to eeprom
/// </summary>
/// <param name="loc">location struct</param>
/// <param name="index">wp no</param>
/// <param name="frame">global or relative</param>
/// <param name="current">0 = no , 2 = guided mode</param>
public MAV_MISSION_RESULT setWP(Locationwp loc, ushort index, MAV_FRAME frame, byte current = 0)
{
giveComport = true;
mavlink_mission_item_t req = new mavlink_mission_item_t();
req.target_system = sysid;
req.target_component = compid; // MAVLINK_MSG_ID_MISSION_ITEM
req.command = loc.id;
req.param1 = loc.p1;
req.current = current;
req.frame = (byte)frame;
req.y = (float)(loc.lng);
req.x = (float)(loc.lat);
req.z = (float)(loc.alt);
req.param1 = loc.p1;
req.param2 = loc.p2;
req.param3 = loc.p3;
req.param4 = loc.p4;
req.seq = index;
log.InfoFormat("setWP {6} frame {0} cmd {1} p1 {2} x {3} y {4} z {5}", req.frame, req.command, req.param1, req.x, req.y, req.z, index);
// request
generatePacket(MAVLINK_MSG_ID_MISSION_ITEM, req);
DateTime start = DateTime.Now;
int retrys = 10;
while (true)
{
if (!(start.AddMilliseconds(150) > DateTime.Now))
{
if (retrys > 0)
{
log.Info("setWP Retry " + retrys);
generatePacket(MAVLINK_MSG_ID_MISSION_ITEM, req);
start = DateTime.Now;
retrys--;
continue;
}
giveComport = false;
throw new Exception("Timeout on read - setWP");
}
byte[] buffer = readPacket();
if (buffer.Length > 5)
{
if (buffer[5] == MAVLINK_MSG_ID_MISSION_ACK)
{
var ans = buffer.ByteArrayToStructure<mavlink_mission_ack_t>(6);
log.Info("set wp " + index + " ACK 47 : " + buffer[5] + " ans " + Enum.Parse(typeof(MAV_MISSION_RESULT), ans.type.ToString()));
if (req.current == 2)
{
GuidedMode = req;
}
else if (req.current == 3)
{
}
else
{
wps[req.seq] = req;
}
return (MAV_MISSION_RESULT)ans.type;
}
else if (buffer[5] == MAVLINK_MSG_ID_MISSION_REQUEST)
{
var ans = buffer.ByteArrayToStructure<mavlink_mission_request_t>(6);
if (ans.seq == (index + 1))
{
log.Info("set wp doing " + index + " req " + ans.seq + " REQ 40 : " + buffer[5]);
giveComport = false;
if (req.current == 2)
{
GuidedMode = req;
}
else if (req.current == 3)
{
}
else
{
wps[req.seq] = req;
}
return MAV_MISSION_RESULT.MAV_MISSION_ACCEPTED;
}
else
{
log.InfoFormat("set wp fail doing " + index + " req " + ans.seq + " ACK 47 or REQ 40 : " + buffer[5] + " seq {0} ts {1} tc {2}", req.seq, req.target_system, req.target_component);
//break;
}
}
else
{
//Console.WriteLine(DateTime.Now + " PC setwp " + buffer[5]);
}
}
}
// return MAV_MISSION_RESULT.MAV_MISSION_INVALID;
}
public void setNewWPAlt(Locationwp gotohere)
{
giveComport = true;
try
{
gotohere.id = (byte)MAV_CMD.WAYPOINT;
MAV_MISSION_RESULT ans = setWP(gotohere, 0, MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT, (byte)3);
if (ans != MAV_MISSION_RESULT.MAV_MISSION_ACCEPTED)
throw new Exception("Alt Change Failed");
}
catch (Exception ex) { giveComport = false; log.Error(ex); throw; }
giveComport = false;
}
public void setGuidedModeWP(Locationwp gotohere)
{
if (gotohere.alt == 0 || gotohere.lat == 0 || gotohere.lng == 0)
return;
giveComport = true;
try
{
gotohere.id = (byte)MAV_CMD.WAYPOINT;
MAV_MISSION_RESULT ans = setWP(gotohere, 0, MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT, (byte)2);
if (ans != MAV_MISSION_RESULT.MAV_MISSION_ACCEPTED)
throw new Exception("Guided Mode Failed");
}
catch (Exception ex) { log.Error(ex); }
giveComport = false;
}
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.mymap.streamjpgenable = true;
GCSViews.FlightData.myhud.streamjpgenable = true;
NetworkStream stream = client.GetStream();
// 3 seconds
stream.ReadTimeout = 3000;
again:
var asciiEncoding = new ASCIIEncoding();
var request = new byte[1024];
int len = stream.Read(request, 0, request.Length);
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", "");
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: APM 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[256];
string sendme = MainV2.comPort.MAV.cs.roll + "," + MainV2.comPort.MAV.cs.pitch + "," + MainV2.comPort.MAV.cs.yaw;
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"))
{
string header = "HTTP/1.1 200 OK\r\nContent-Type: application/vnd.google-earth.kml+xml\r\nContent-Length: " + georefkml.Length + "\r\n\r\n";
byte[] temp = asciiEncoding.GetBytes(header);
stream.Write(temp, 0, temp.Length);
byte[] buffer = Encoding.ASCII.GetBytes(georefkml);
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 ";
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.alt;
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
};
kml.Viewpoint = la;
kml.AddFeature(pmplane);
SharpKml.Dom.CoordinateCollection coords = new SharpKml.Dom.CoordinateCollection();
foreach (var point in MainV2.comPort.MAV.wps.Values)
{
coords.Add(new SharpKml.Base.Vector(point.x, point.y, point.z));
}
SharpKml.Dom.LineString ls = new SharpKml.Dom.LineString();
ls.AltitudeMode = SharpKml.Dom.AltitudeMode.RelativeToGround;
ls.Coordinates = coords;
SharpKml.Dom.Placemark pm = new SharpKml.Dom.Placemark() { Geometry = ls };
kml.AddFeature(pm);
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=APMPLANNER\r\n\r\n--APMPLANNER\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"))
{
GCSViews.FlightData.mymap.streamjpgenable = true;
data = GCSViews.FlightData.mymap.streamjpg.ToArray();
}
else
{
GCSViews.FlightData.mymap.streamjpgenable = true;
GCSViews.FlightData.myhud.streamjpgenable = true;
Image img1 = Image.FromStream(GCSViews.FlightData.myhud.streamjpg);
Image img2 = Image.FromStream(GCSViews.FlightData.mymap.streamjpg);
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--APMPLANNER\r\n";
temp = asciiEncoding.GetBytes(header);
stream.Write(temp, 0, temp.Length);
}
GCSViews.FlightData.mymap.streamjpgenable = false;
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 = float.Parse(match.Groups[1].Value),
lng = float.Parse(match.Groups[2].Value),
alt = float.Parse(match.Groups[3].Value)
};
try
{
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\nContent-Type: image/jpg\r\nContent-Length: " + memstream.Length + "\r\n\r\n";
byte[] temp = asciiEncoding.GetBytes(header);
stream.Write(temp, 0, temp.Length);
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);
}
file.Close();
resi.Dispose();
orig.Dispose();
}
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 = float.Parse(match.Groups[1].Value),
lng = float.Parse(match.Groups[2].Value),
alt = float.Parse(match.Groups[3].Value)
};
try
{
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.packets[MAVLink.MAVLINK_MSG_ID_ATTITUDE] != null)
message.ATTITUDE = new Message2() { index = MainV2.comPort.MAV.packetseencount[MAVLink.MAVLINK_MSG_ID_ATTITUDE], msg = MainV2.comPort.MAV.packets[MAVLink.MAVLINK_MSG_ID_ATTITUDE].ByteArrayToStructure<MAVLink.mavlink_attitude_t>(6) };
if (MainV2.comPort.MAV.packets[MAVLink.MAVLINK_MSG_ID_VFR_HUD] != null)
message.VFR_HUD = new Message2() { index = MainV2.comPort.MAV.packetseencount[MAVLink.MAVLINK_MSG_ID_VFR_HUD], msg = MainV2.comPort.MAV.packets[MAVLink.MAVLINK_MSG_ID_VFR_HUD].ByteArrayToStructure<MAVLink.mavlink_vfr_hud_t>(6) };
if (MainV2.comPort.MAV.packets[MAVLink.MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT] != null)
message.NAV_CONTROLLER_OUTPUT = new Message2() { index = MainV2.comPort.MAV.packetseencount[MAVLink.MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT], msg = MainV2.comPort.MAV.packets[MAVLink.MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT].ByteArrayToStructure<MAVLink.mavlink_nav_controller_output_t>(6) };
if (MainV2.comPort.MAV.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW_INT] != null)
message.GPS_RAW_INT = new Message2() { index = MainV2.comPort.MAV.packetseencount[MAVLink.MAVLINK_MSG_ID_GPS_RAW_INT], msg = MainV2.comPort.MAV.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW_INT].ByteArrayToStructure<MAVLink.mavlink_gps_raw_int_t>(6) };
if (MainV2.comPort.MAV.packets[MAVLink.MAVLINK_MSG_ID_HEARTBEAT] != null)
message.HEARTBEAT = new Message2() { index = MainV2.comPort.MAV.packetseencount[MAVLink.MAVLINK_MSG_ID_HEARTBEAT], msg = MainV2.comPort.MAV.packets[MAVLink.MAVLINK_MSG_ID_HEARTBEAT].ByteArrayToStructure<MAVLink.mavlink_heartbeat_t>(6) };
if (MainV2.comPort.MAV.packets[MAVLink.MAVLINK_MSG_ID_GPS_STATUS] != null)
message.GPS_STATUS = new Message2() { index = MainV2.comPort.MAV.packetseencount[MAVLink.MAVLINK_MSG_ID_GPS_STATUS], msg = MainV2.comPort.MAV.packets[MAVLink.MAVLINK_MSG_ID_GPS_STATUS].ByteArrayToStructure<MAVLink.mavlink_gps_status_t>(6) };
if (MainV2.comPort.MAV.packets[MAVLink.MAVLINK_MSG_ID_STATUSTEXT] != null)
message.STATUSTEXT = new Message2() { index = MainV2.comPort.MAV.packetseencount[MAVLink.MAVLINK_MSG_ID_STATUSTEXT], msg = MainV2.comPort.MAV.packets[MAVLink.MAVLINK_MSG_ID_STATUSTEXT].ByteArrayToStructure<MAVLink.mavlink_statustext_t>(6) };
if (MainV2.comPort.MAV.packets[MAVLink.MAVLINK_MSG_ID_SYS_STATUS] != null)
message.SYS_STATUS = new Message2() { index = MainV2.comPort.MAV.packetseencount[MAVLink.MAVLINK_MSG_ID_SYS_STATUS], msg = MainV2.comPort.MAV.packets[MAVLink.MAVLINK_MSG_ID_SYS_STATUS].ByteArrayToStructure<MAVLink.mavlink_sys_status_t>(6) };
message.META_LINKQUALITY = message.SYS_STATUS = new Message2() { index = packetindex, msg = new META_LINKQUALITY() { master_in = (int)MainV2.comPort.packetsnotlost, mavpackettype = "META_LINKQUALITY", master_out = MainV2.comPort.packetcount, packet_loss = 100 - MainV2.comPort.MAV.cs.linkqualitygcs } };
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 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();
}
stream.Close();
}
catch (Exception ee)
{
log.Error("Failed mjpg ", ee);
try
{
client.Close();
}
catch { }
}
}
}
void mainloop()
{
DateTime nextsend = DateTime.Now;
StreamWriter sw = new StreamWriter(File.OpenWrite("followmeraw.txt"));
threadrun = true;
while (threadrun)
{
try
{
string line = comPort.ReadLine();
sw.WriteLine(line);
//string line = string.Format("$GP{0},{1:HHmmss},{2},{3},{4},{5},{6},{7},{8},{9},{10},{11},{12},{13},", "GGA", DateTime.Now.ToUniversalTime(), Math.Abs(lat * 100), MainV2.comPort.MAV.cs.lat < 0 ? "S" : "N", Math.Abs(lng * 100), MainV2.comPort.MAV.cs.lng < 0 ? "W" : "E", MainV2.comPort.MAV.cs.gpsstatus, MainV2.comPort.MAV.cs.satcount, MainV2.comPort.MAV.cs.gpshdop, MainV2.comPort.MAV.cs.alt, "M", 0, "M", "");
if (line.StartsWith("$GPGGA")) //
{
string[] items = line.Trim().Split(',', '*');
if (items[15] != GetChecksum(line.Trim()))
{
Console.WriteLine("Bad Nmea line " + items[15] + " vs " + GetChecksum(line.Trim()));
continue;
}
if (items[6] == "0")
{
Console.WriteLine("No Fix");
continue;
}
gotolocation.Lat = double.Parse(items[2], CultureInfo.InvariantCulture) / 100.0;
gotolocation.Lat = (int)gotolocation.Lat + ((gotolocation.Lat - (int)gotolocation.Lat) / 0.60);
if (items[3] == "S")
{
gotolocation.Lat *= -1;
}
gotolocation.Lng = double.Parse(items[4], CultureInfo.InvariantCulture) / 100.0;
gotolocation.Lng = (int)gotolocation.Lng + ((gotolocation.Lng - (int)gotolocation.Lng) / 0.60);
if (items[5] == "W")
{
gotolocation.Lng *= -1;
}
gotolocation.Alt = intalt; // double.Parse(line.Substring(c9, c10 - c9 - 1)) +
gotolocation.Tag = "Sats " + items[7] + " hdop " + items[8];
}
if (DateTime.Now > nextsend && gotolocation.Lat != 0 && gotolocation.Lng != 0 && gotolocation.Alt != 0) // 200 * 10 = 2 sec /// lastgotolocation != gotolocation &&
{
nextsend = DateTime.Now.AddMilliseconds(1000 / updaterate);
Console.WriteLine("Sending follow wp " + DateTime.Now.ToString("h:MM:ss") + " " + gotolocation.Lat + " " + gotolocation.Lng + " " + gotolocation.Alt);
lastgotolocation = new PointLatLngAlt(gotolocation);
Locationwp gotohere = new Locationwp();
gotohere.id = (byte)MAVLink.MAV_CMD.WAYPOINT;
gotohere.alt = (float)(gotolocation.Alt);
gotohere.lat = (gotolocation.Lat);
gotohere.lng = (gotolocation.Lng);
try
{
updateLocationLabel(gotohere);
}
catch { }
if (MainV2.comPort.BaseStream.IsOpen && MainV2.comPort.giveComport == false)
{
try
{
MainV2.comPort.giveComport = true;
MainV2.comPort.setGuidedModeWP(gotohere);
MainV2.comPort.giveComport = false;
}
catch { MainV2.comPort.giveComport = false; }
}
}
}
catch { System.Threading.Thread.Sleep((int)(1000 / updaterate)); }
}
sw.Close();
}
/// <summary>
/// little web server for sending network link kml's
/// </summary>
void listernforclients()
{
try
{
listener.Start();
}
catch (Exception e)
{
log.Error("Exception starting lister. Possible multiple instances of planner?", e);
return;
} // in use
// Enter the listening loop.
while (true)
{
// Perform a blocking call to accept requests.
// You could also user server.AcceptSocket() here.
try
{
log.Info("Listening for client - 1 client at a time");
TcpClient client = listener.AcceptTcpClient();
// 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.mymap.streamjpgenable = true;
GCSViews.FlightData.myhud.streamjpgenable = true;
MethodInvoker m = delegate()
{
GCSViews.FlightData.mymap.Refresh();
};
this.Invoke(m);
NetworkStream stream = client.GetStream();
var asciiEncoding = new ASCIIEncoding();
var request = new byte[1024];
int len = stream.Read(request, 0, request.Length);
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", "");
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: APM 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[256];
string sendme = MainV2.comPort.MAV.cs.roll + "," + MainV2.comPort.MAV.cs.pitch + "," + MainV2.comPort.MAV.cs.yaw;
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"))
{
string header = "HTTP/1.1 200 OK\r\nContent-Type: application/vnd.google-earth.kml+xml\r\n\r\n";
byte[] temp = asciiEncoding.GetBytes(header);
stream.Write(temp, 0, temp.Length);
byte[] buffer = Encoding.ASCII.GetBytes(georefkml);
stream.Write(buffer, 0, buffer.Length);
stream.Close();
}
else if (url.Contains("network.kml"))
{
string header = "HTTP/1.1 200 OK\r\nContent-Type: application/vnd.google-earth.kml+xml\r\n\r\n";
byte[] temp = asciiEncoding.GetBytes(header);
stream.Write(temp, 0, temp.Length);
SharpKml.Dom.Document kml = new SharpKml.Dom.Document();
SharpKml.Dom.Placemark pmplane = new SharpKml.Dom.Placemark();
pmplane.Name = "P/Q ";
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.alt;
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
};
kml.Viewpoint = la;
kml.AddFeature(pmplane);
SharpKml.Dom.CoordinateCollection coords = new SharpKml.Dom.CoordinateCollection();
foreach (var point in MainV2.comPort.MAV.wps.Values)
{
coords.Add(new SharpKml.Base.Vector(point.x, point.y, point.z));
}
SharpKml.Dom.LineString ls = new SharpKml.Dom.LineString();
ls.AltitudeMode = SharpKml.Dom.AltitudeMode.RelativeToGround;
ls.Coordinates = coords;
SharpKml.Dom.Placemark pm = new SharpKml.Dom.Placemark() { Geometry = ls };
kml.AddFeature(pm);
SharpKml.Base.Serializer serializer = new SharpKml.Base.Serializer();
serializer.Serialize(kml);
byte[] buffer = Encoding.ASCII.GetBytes(serializer.Xml);
stream.Write(buffer, 0, buffer.Length);
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"))
{
string header = "HTTP/1.1 200 OK\r\nContent-Type: multipart/x-mixed-replace;boundary=APMPLANNER\r\n\r\n--APMPLANNER\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"))
{
GCSViews.FlightData.mymap.streamjpgenable = true;
data = GCSViews.FlightData.mymap.streamjpg.ToArray();
}
else
{
GCSViews.FlightData.mymap.streamjpgenable = true;
GCSViews.FlightData.myhud.streamjpgenable = true;
Image img1 = Image.FromStream(GCSViews.FlightData.myhud.streamjpg);
Image img2 = Image.FromStream(GCSViews.FlightData.mymap.streamjpg);
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--APMPLANNER\r\n";
temp = asciiEncoding.GetBytes(header);
stream.Write(temp, 0, temp.Length);
}
GCSViews.FlightData.mymap.streamjpgenable = false;
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 = float.Parse(match.Groups[1].Value),
lng = float.Parse(match.Groups[2].Value),
alt = float.Parse(match.Groups[3].Value)
};
try
{
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();
}
catch (Exception ee)
{
log.Error("Failed mjpg ", ee);
}
}
}
