/*
* extrapolate latitude/longitude given bearing and distance
* Note that this function is accurate to about 1mm at a distance of
* 100m. This function has the advantage that it works in relative
* positions, so it keeps the accuracy even when dealing with small
* distances and floating point numbers
*/
private static void location_update(Locationwp loc, float bearing, float distance)
{
float ofs_north = cosf(radians(bearing)) * distance;
float ofs_east = sinf(radians(bearing)) * distance;
location_offset(loc, ofs_north, ofs_east);
}
static object Convert(Locationwp cmd, bool isint = false)
{
if (isint)
{
var temp = new MAVLink.mavlink_mission_item_int_t()
{
command = cmd.id,
param1 = cmd.p1,
param2 = cmd.p2,
param3 = cmd.p3,
param4 = cmd.p4,
x = (int)(cmd.lat * 1.0e7),
y = (int)(cmd.lng * 1.0e7),
z = (float) cmd.alt
};
return temp;
}
else
{
var temp = new MAVLink.mavlink_mission_item_t()
{
command = cmd.id,
param1 = cmd.p1,
param2 = cmd.p2,
param3 = cmd.p3,
param4 = cmd.p4,
x = (float) cmd.lat,
y = (float) cmd.lng,
z = (float) cmd.alt
};
return temp;
}
}
static object Convert(Locationwp cmd, bool isint = false)
{
if (isint)
{
var temp = new MAVLink.mavlink_mission_item_int_t()
{
command = cmd.id,
param1 = cmd.p1,
param2 = cmd.p2,
param3 = cmd.p3,
param4 = cmd.p4,
x = (int)(cmd.lat * 1.0e7),
y = (int)(cmd.lng * 1.0e7),
z = (float)cmd.alt
};
return(temp);
}
else
{
var temp = new MAVLink.mavlink_mission_item_t()
{
command = cmd.id,
param1 = cmd.p1,
param2 = cmd.p2,
param3 = cmd.p3,
param4 = cmd.p4,
x = (float)cmd.lat,
y = (float)cmd.lng,
z = (float)cmd.alt
};
return(temp);
}
}
/*
* extrapolate latitude/longitude given distances north and east
* This function costs about 80 usec on an AVR2560
*/
private static void location_offset(Locationwp loc, float ofs_north, float ofs_east)
{
if (!is_zero(ofs_north) || !is_zero(ofs_east))
{
int32_t dlat = (int32_t)(ofs_north * LOCATION_SCALING_FACTOR_INV);
int32_t dlng = (int32_t)((ofs_east * LOCATION_SCALING_FACTOR_INV) / longitude_scale(loc));
loc.lat += dlat;
loc.lng += dlng;
}
}
private void calculate_grid_info(Locationwp loc, ref grid_info info)
{
// grids start on integer degrees. This makes storing terrain data
// on the SD card a bit easier
info.lat_degrees = (int8_t)((loc.lat < 0 ? (loc.lat - 9999999L) : loc.lat) / (10 * 1000 * 1000L));
info.lon_degrees = (int16_t)((loc.lng < 0 ? (loc.lng - 9999999L) : loc.lng) / (10 * 1000 * 1000L));
// create reference position for this rounded degree position
Locationwp ref1 = new Locationwp();
ref1.lat = info.lat_degrees * 10 * 1000 * 1000L;
ref1.lng = info.lon_degrees * 10 * 1000 * 1000L;
// find offset from reference
Vector3 offset = location_diff(ref1, loc);
// get indices in terms of grid_spacing elements
uint32_t idx_x = (uint32_t)(offset.x / grid_spacing);
uint32_t idx_y = (uint32_t)(offset.y / grid_spacing);
// find indexes into 32*28 grids for this degree reference. Note
// the use of TERRAIN_GRID_BLOCK_SPACING_{X,Y} which gives a one square
// overlap between grids
info.grid_idx_x = (ushort)(idx_x / TERRAIN_GRID_BLOCK_SPACING_X);
info.grid_idx_y = (ushort)(idx_y / TERRAIN_GRID_BLOCK_SPACING_Y);
// find the indices within the 32*28 grid
info.idx_x = (byte)(idx_x % TERRAIN_GRID_BLOCK_SPACING_X);
info.idx_y = (byte)(idx_y % TERRAIN_GRID_BLOCK_SPACING_Y);
// find the fraction (0..1) within the square
info.frac_x = (float)((offset.x - idx_x * grid_spacing) / grid_spacing);
info.frac_y = (float)((offset.y - idx_y * grid_spacing) / grid_spacing);
// calculate lat/lon of SW corner of 32*28 grid_block
location_offset(ref1,
info.grid_idx_x * TERRAIN_GRID_BLOCK_SPACING_X * (float)grid_spacing,
info.grid_idx_y * TERRAIN_GRID_BLOCK_SPACING_Y * (float)grid_spacing);
info.grid_lat = (int32_t)ref1.lat;
info.grid_lon = (int32_t)ref1.lng;
//ASSERT_RANGE(info.idx_x,0, TERRAIN_GRID_BLOCK_SPACING_X-1);
//ASSERT_RANGE(info.idx_y,0, TERRAIN_GRID_BLOCK_SPACING_Y-1);
//ASSERT_RANGE(info.frac_x,0,1);
//ASSERT_RANGE(info.frac_y,0,1);
}
float longitude_scale(Locationwp loc)
{
int32_t last_lat = -999999999;
float scale = 1.0f;
if (labs(last_lat - loc.lat) < 100000)
{
// we are within 0.01 degrees (about 1km) of the
// same latitude. We can avoid the cos() and return
// the same scale factor.
return(scale);
}
scale = cosf(loc.lat * 1.0e-7f * DEG_TO_RAD);
scale = constrain_float(scale, 0.01f, 1.0f);
last_lat = (int32_t)loc.lat;
return(scale);
}
/*
* calculate lookahead rise in terrain. This returns extra altitude
* needed to clear upcoming terrain in meters
*/
private float lookahead(Locationwp loc, float bearing, float distance, float climb_ratio)
{
if (!enable || grid_spacing <= 0)
{
return(0);
}
//Locationwp loc;
//if (!ahrs.get_position(loc))
{
// we don't know where we are
//return 0;
}
float base_height = 0;
if (!height_amsl(loc, ref base_height))
{
// we don't know our current terrain height
return(0);
}
float climb = 0;
float lookahead_estimate = 0;
// check for terrain at grid spacing intervals
while (distance > 0)
{
location_update(loc, bearing, grid_spacing);
climb += climb_ratio * grid_spacing;
distance -= grid_spacing;
float height = 0;
if (height_amsl(loc, ref height))
{
float rise = (height - base_height) - climb;
if (rise > lookahead_estimate)
{
lookahead_estimate = rise;
}
}
}
return(lookahead_estimate);
}
public void test()
{
var fs = File.Open(@"C:\Users\michael\Documents\Mission Planner\sitl\d0\terrain\S36E149.DAT", FileMode.Open);
var br = new BinaryReader(fs);
var buffer = new byte[2048];
while (br.BaseStream.Position < br.BaseStream.Length)
{
grid_info info = new grid_info();
var loc = new Locationwp()
{
lat = -35.363261 * 1.0e7, lng = 149.165230 * 1.0e7
};
calculate_grid_info(loc, ref info);
disk_block.block.lat = info.grid_lat;
disk_block.block.lon = info.grid_lon;
disk_block.block.spacing = grid_spacing;
disk_block.block.grid_idx_x = info.grid_idx_x;
disk_block.block.grid_idx_y = info.grid_idx_y;
disk_block.block.lat_degrees = info.lat_degrees;
disk_block.block.lon_degrees = info.lon_degrees;
disk_block.block.version = TERRAIN_GRID_FORMAT_VERSION;
// 29,5 = 1792000
//read_block(fs);
//continue;
var off = br.BaseStream.Position;
br.Read(buffer, 0, buffer.Length);
disk_block = buffer.ByteArrayToStructure <grid_io_block>(0);
if (disk_block.block.bitmap > 0 && disk_block.block.lat != 0 && disk_block.block.lon != 0)
{
Console.WriteLine("Got {0} - {1} at idx {2},{3} off {4}", disk_block.block.lat, disk_block.block.lon, disk_block.block.grid_idx_x, disk_block.block.grid_idx_y, off);
}
}
}
/*
calculate lookahead rise in terrain. This returns extra altitude
needed to clear upcoming terrain in meters
*/
float lookahead(Locationwp loc, float bearing, float distance, float climb_ratio)
{
if (!enable || grid_spacing <= 0)
{
return 0;
}
//Locationwp loc;
//if (!ahrs.get_position(loc))
{
// we don't know where we are
//return 0;
}
float base_height = 0;
if (!height_amsl(loc, ref base_height))
{
// we don't know our current terrain height
return 0;
}
float climb = 0;
float lookahead_estimate = 0;
// check for terrain at grid spacing intervals
while (distance > 0)
{
location_update(loc, bearing, grid_spacing);
climb += climb_ratio*grid_spacing;
distance -= grid_spacing;
float height = 0;
if (height_amsl(loc, ref height))
{
float rise = (height - base_height) - climb;
if (rise > lookahead_estimate)
{
lookahead_estimate = rise;
}
}
}
return lookahead_estimate;
}
private void seek_offset(Stream st)
{
grid_block block = disk_block.block;
// work out how many longitude blocks there are at this latitude
Locationwp loc1 = new Locationwp();
Locationwp loc2 = new Locationwp();
loc1.lat = block.lat_degrees * 10 * 1000 * 1000L;
loc1.lng = block.lon_degrees * 10 * 1000 * 1000L;
loc2.lat = block.lat_degrees * 10 * 1000 * 1000L;
loc2.lng = (block.lon_degrees + 1) * 10 * 1000 * 1000L;
// shift another two blocks east to ensure room is available
location_offset(loc2, 0, 2 * grid_spacing * TERRAIN_GRID_BLOCK_SIZE_Y);
Vector3 offset = location_diff(loc1, loc2);
uint16_t east_blocks = (uint16_t)(offset.y / (grid_spacing * TERRAIN_GRID_BLOCK_SIZE_Y));
uint32_t file_offset = (uint32_t)((east_blocks * block.grid_idx_x +
block.grid_idx_y) * 2048);
st.Seek(file_offset, SeekOrigin.Begin);
}
private void button16_Click(object sender, EventArgs e)
{
// define the home point
Locationwp home = new Locationwp();
try
{
home.id = (byte)MAVLink.MAV_CMD.WAYPOINT;
home.lat = (double.Parse(TXT_homelat.Text));
home.lng = (double.Parse(TXT_homelng.Text));
home.alt = (float.Parse(TXT_homealt.Text) / CurrentState.multiplierdist); // use saved home
}
catch { throw new Exception("Your home location is invalid"); }
gMapControl1.Position = new PointLatLng(home.lat, home.lng);
gMapControl1.Zoom = 18;
}
private void updateLocationLabel(Locationwp plla)
{
this.BeginInvoke((MethodInvoker)delegate
{
LBL_location.Text = gotolocation.Lat + " " + gotolocation.Lng + " " + gotolocation.Alt +" "+ gotolocation.Tag;
}
);
}
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;
// fix for followme change
if (MAV.cs.mode.ToUpper() != "GUIDED")
setMode("GUIDED");
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;
}
/// <summary>
/// Gets specfied WP
/// </summary>
/// <param name="index"></param>
/// <returns>WP</returns>
public Locationwp getWP(ushort index)
{
while (giveComport)
System.Threading.Thread.Sleep(100);
bool use_int = (MAV.cs.capabilities & MAV_PROTOCOL_CAPABILITY.MISSION_INT) > 0;
object req;
if (use_int)
{
mavlink_mission_request_int_t reqi = new mavlink_mission_request_int_t();
reqi.target_system = MAV.sysid;
reqi.target_component = MAV.compid;
reqi.seq = index;
// request
generatePacket((byte)MAVLINK_MSG_ID.MISSION_REQUEST_INT, reqi);
req = reqi;
}
else
{
mavlink_mission_request_t reqf = new mavlink_mission_request_t();
reqf.target_system = MAV.sysid;
reqf.target_component = MAV.compid;
reqf.seq = index;
// request
generatePacket((byte)MAVLINK_MSG_ID.MISSION_REQUEST, reqf);
req = reqf;
}
giveComport = true;
Locationwp loc = new Locationwp();
DateTime start = DateTime.Now;
int retrys = 5;
while (true)
{
if (!(start.AddMilliseconds(3500) > DateTime.Now)) // apm times out after 5000ms
{
if (retrys > 0)
{
log.Info("getWP Retry " + retrys);
if (use_int)
generatePacket((byte)MAVLINK_MSG_ID.MISSION_REQUEST_INT, req);
else
generatePacket((byte)MAVLINK_MSG_ID.MISSION_REQUEST, req);
start = DateTime.Now;
retrys--;
continue;
}
giveComport = false;
throw new TimeoutException("Timeout on read - getWP");
}
//Console.WriteLine("getwp read " + DateTime.Now.Millisecond);
MAVLinkMessage buffer = readPacket();
//Console.WriteLine("getwp readend " + DateTime.Now.Millisecond);
if (buffer.Length > 5)
{
if (buffer.msgid == (byte) MAVLINK_MSG_ID.MISSION_ITEM)
{
//Console.WriteLine("getwp ans " + DateTime.Now.Millisecond);
var wp = buffer.ToStructure<mavlink_mission_item_t>();
// received a packet, but not what we requested
if (index != wp.seq)
{
generatePacket((byte) MAVLINK_MSG_ID.MISSION_REQUEST, req);
continue;
}
loc.options = (byte) (wp.frame);
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));
log.InfoFormat("getWP {0} {1} {2} {3} {4} opt {5}", loc.id, loc.p1, loc.alt, loc.lat, loc.lng,
loc.options);
break;
}
else if (buffer.msgid == (byte) MAVLINK_MSG_ID.MISSION_ITEM_INT)
{
//Console.WriteLine("getwp ans " + DateTime.Now.Millisecond);
var wp = buffer.ToStructure<mavlink_mission_item_int_t>();
// received a packet, but not what we requested
if (index != wp.seq)
{
generatePacket((byte)MAVLINK_MSG_ID.MISSION_REQUEST_INT, req);
continue;
}
loc.options = (byte)(wp.frame);
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 / 1.0e7));
loc.lng = ((wp.y / 1.0e7));
log.InfoFormat("getWPint {0} {1} {2} {3} {4} opt {5}", loc.id, loc.p1, loc.alt, loc.lat, loc.lng,
loc.options);
break;
}
else
{
//log.Info(DateTime.Now + " PC getwp " + buffer.msgid);
}
}
}
giveComport = false;
return loc;
}
private void goHereToolStripMenuItem_Click(object sender, EventArgs e)
{
if (!MainV2.comPort.BaseStream.IsOpen)
{
CustomMessageBox.Show(Strings.PleaseConnect, Strings.ERROR);
return;
}
if (MainV2.comPort.MAV.GuidedMode.z == 0)
{
flyToHereAltToolStripMenuItem_Click(null, null);
if (MainV2.comPort.MAV.GuidedMode.z == 0)
return;
}
if (MouseDownStart.Lat == 0 || MouseDownStart.Lng == 0)
{
CustomMessageBox.Show(Strings.BadCoords, Strings.ERROR);
return;
}
Locationwp gotohere = new Locationwp();
gotohere.id = (byte) MAVLink.MAV_CMD.WAYPOINT;
gotohere.alt = MainV2.comPort.MAV.GuidedMode.z; // back to m
gotohere.lat = (MouseDownStart.Lat);
gotohere.lng = (MouseDownStart.Lng);
try
{
MainV2.comPort.setGuidedModeWP(gotohere);
}
catch (Exception ex)
{
MainV2.comPort.giveComport = false;
CustomMessageBox.Show(Strings.CommandFailed + ex.Message, Strings.ERROR);
}
}
private void goHereToolStripMenuItem_Click(object sender, EventArgs e)
{
if (!MainV2.comPort.BaseStream.IsOpen)
{
CustomMessageBox.Show("Please Connect First");
return;
}
if (MainV2.comPort.MAV.GuidedMode.z == 0)
{
flyToHereAltToolStripMenuItem_Click(null, null);
if (MainV2.comPort.MAV.GuidedMode.z == 0)
return;
}
if (gotolocation.Lat == 0 || gotolocation.Lng == 0)
{
CustomMessageBox.Show("Bad Lat/Long");
return;
}
Locationwp gotohere = new Locationwp();
gotohere.id = (byte)MAVLink.MAV_CMD.WAYPOINT;
gotohere.alt = (float)(MainV2.comPort.MAV.GuidedMode.z); // back to m
gotohere.lat = (gotolocation.Lat);
gotohere.lng = (gotolocation.Lng);
try
{
MainV2.comPort.setGuidedModeWP(gotohere);
}
catch (Exception ex) { MainV2.comPort.giveComport = false; CustomMessageBox.Show("Error sending command : " + ex.Message); }
}
public static List <Locationwp> ReadWaypointFile(string file)
{
int wp_count = 0;
bool error = false;
List <Locationwp> cmds = new List <Locationwp>();
StreamReader sr = new StreamReader(file);
string header = sr.ReadLine();
if (header == null || !header.Contains("QGC WPL"))
{
CustomMessageBox.Show("Invalid Waypoint file");
return(cmds);
}
while (!error && !sr.EndOfStream)
{
string line = sr.ReadLine();
// waypoints
if (line.StartsWith("#"))
{
continue;
}
//seq/cur/frame/mode
string[] items = line.Split(new[] { '\t', ' ', ',' }, StringSplitOptions.RemoveEmptyEntries);
if (items.Length <= 9)
{
continue;
}
try
{
// check to see if the first wp is index 0/home.
// if it is not index 0, add a blank home point
if (wp_count == 0 && items[0] != "0")
{
cmds.Add(new Locationwp());
}
Locationwp temp = new Locationwp();
temp.frame = (byte)int.Parse(items[2], CultureInfo.InvariantCulture);
if (items[2] == "3")
{
// abs MAV_FRAME_GLOBAL_RELATIVE_ALT=3
temp.options = 1;
}
else if (items[2] == "10")
{
temp.options = 8;
}
else
{
temp.options = 0;
}
temp.id = (ushort)Enum.Parse(typeof(MAVLink.MAV_CMD), items[3], false);
temp.p1 = float.Parse(items[4], CultureInfo.InvariantCulture);
if (temp.id == 99)
{
temp.id = 0;
}
temp.alt = (float)(double.Parse(items[10], CultureInfo.InvariantCulture));
temp.lat = (double.Parse(items[8], CultureInfo.InvariantCulture));
temp.lng = (double.Parse(items[9], CultureInfo.InvariantCulture));
temp.p2 = (float)(double.Parse(items[5], CultureInfo.InvariantCulture));
temp.p3 = (float)(double.Parse(items[6], CultureInfo.InvariantCulture));
temp.p4 = (float)(double.Parse(items[7], CultureInfo.InvariantCulture));
cmds.Add(temp);
wp_count++;
}
catch (Exception ex)
{
log.Error(ex);
CustomMessageBox.Show("Line invalid\n" + line);
}
}
sr.Close();
return(cmds);
}
void mainloop()
{
DateTime nextsend = DateTime.Now;
DateTime nextrallypntupdate = DateTime.Now;
StreamWriter sw = new StreamWriter(File.OpenWrite("MovingBase.txt"));
threadrun = true;
while (threadrun)
{
try
{
string line = comPort.ReadLine();
sw.WriteLine(line);
//string line = string.Format("$GP{0},{1:HHmmss},{2},{3},{4},{5},{6},{7},{8},{9},{10},{11},{12},{13},", "GGA", DateTime.Now.ToUniversalTime(), Math.Abs(lat * 100), MainV2.comPort.MAV.cs.lat < 0 ? "S" : "N", Math.Abs(lng * 100), MainV2.comPort.MAV.cs.lng < 0 ? "W" : "E", MainV2.comPort.MAV.cs.gpsstatus, MainV2.comPort.MAV.cs.satcount, MainV2.comPort.MAV.cs.gpshdop, MainV2.comPort.MAV.cs.alt, "M", 0, "M", "");
if (line.StartsWith("$GPGGA")) //
{
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 = double.Parse(items[9], CultureInfo.InvariantCulture);
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("new home wp " +DateTime.Now.ToString("h:MM:ss")+" "+ gotolocation.Lat + " " + gotolocation.Lng + " " +gotolocation.Alt);
lastgotolocation = new PointLatLngAlt(gotolocation);
Locationwp gotohere = new Locationwp();
gotohere.id = (byte)MAVLink.MAV_CMD.WAYPOINT;
gotohere.alt = (float)(gotolocation.Alt);
gotohere.lat = (gotolocation.Lat);
gotohere.lng = (gotolocation.Lng);
try
{
updateLocationLabel(gotohere);
}
catch { }
MainV2.comPort.MAV.cs.MovingBase = gotolocation;
// plane only
if (updaterallypnt && DateTime.Now > nextrallypntupdate)
{
nextrallypntupdate = DateTime.Now.AddSeconds(5);
try
{
MainV2.comPort.setParam("RALLY_TOTAL", 1);
MainV2.comPort.setRallyPoint(0, new PointLatLngAlt(gotolocation) { Alt = gotolocation.Alt + double.Parse(MainV2.config["TXT_DefaultAlt"].ToString()) }, 0, 0, 0, (byte)(float)MainV2.comPort.MAV.param["RALLY_TOTAL"]);
MainV2.comPort.setParam("RALLY_TOTAL", 1);
}
catch (Exception ex) { Console.WriteLine(ex.ToString()); }
}
}
}
catch { System.Threading.Thread.Sleep((int)(1000 / updaterate)); }
}
sw.Close();
}
/// <summary>
/// Gets specfied WP
/// </summary>
/// <param name="index"></param>
/// <returns>WP</returns>
public Locationwp getWP(ushort index)
{
while (giveComport)
System.Threading.Thread.Sleep(100);
giveComport = true;
Locationwp loc = new Locationwp();
mavlink_mission_request_t req = new mavlink_mission_request_t();
req.target_system = MAV.sysid;
req.target_component = MAV.compid;
req.seq = index;
//Console.WriteLine("getwp req "+ DateTime.Now.Millisecond);
// request
generatePacket((byte)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)
{
log.Info("getWP Retry " + retrys);
generatePacket((byte)MAVLINK_MSG_ID.MISSION_REQUEST, req);
start = DateTime.Now;
retrys--;
continue;
}
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] == (byte)MAVLINK_MSG_ID.MISSION_ITEM)
{
//Console.WriteLine("getwp ans " + DateTime.Now.Millisecond);
var wp = buffer.ByteArrayToStructure<mavlink_mission_item_t>(6);
// received a packet, but not what we requested
if (req.seq != wp.seq)
{
generatePacket((byte)MAVLINK_MSG_ID.MISSION_REQUEST, req);
continue;
}
loc.options = (byte)(wp.frame);
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));
log.InfoFormat("getWP {0} {1} {2} {3} {4} opt {5}", loc.id, loc.p1, loc.alt, loc.lat, loc.lng, loc.options);
break;
}
else
{
log.Info(DateTime.Now + " PC getwp " + buffer[5]);
}
}
}
giveComport = false;
return loc;
}
/// <summary>
/// Gets specfied WP
/// </summary>
/// <param name="index"></param>
/// <returns>WP</returns>
public Locationwp getWP(ushort index)
{
giveComport = true;
Locationwp loc = new Locationwp();
mavlink_mission_request_t req = new mavlink_mission_request_t();
req.target_system = MAV.sysid;
req.target_component = MAV.compid;
req.seq = index;
//Console.WriteLine("getwp req "+ DateTime.Now.Millisecond);
// request
generatePacket((byte)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)
{
log.Info("getWP Retry " + retrys);
generatePacket((byte)MAVLINK_MSG_ID.MISSION_REQUEST, req);
start = DateTime.Now;
retrys--;
continue;
}
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] == (byte)MAVLINK_MSG_ID.MISSION_ITEM)
{
//Console.WriteLine("getwp ans " + DateTime.Now.Millisecond);
//Array.Copy(buffer, 6, buffer, 0, buffer.Length - 6);
var wp = buffer.ByteArrayToStructure<mavlink_mission_item_t>(6);
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 (MAV.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;
}
}
*/
log.InfoFormat("getWP {0} {1} {2} {3} {4} opt {5}", loc.id, loc.p1, loc.alt, loc.lat, loc.lng, loc.options);
break;
}
else
{
log.Info(DateTime.Now + " PC getwp " + buffer[5]);
}
}
}
giveComport = false;
return loc;
}
private static Vector3 location_diff(Locationwp loc1, Locationwp loc2)
{
return(new Vector3((loc2.lat - loc1.lat) * LOCATION_SCALING_FACTOR,
(loc2.lng - loc1.lng) * LOCATION_SCALING_FACTOR * longitude_scale(loc1)));
}
/*
* return terrain height in meters above average sea level (WGS84) for
* a given position
*
* This is the base function that other height calculations are derived
* from. The functions below are more convenient for most uses
*
* This function costs about 20 microseconds on Pixhawk
*/
private bool height_amsl(Locationwp loc, ref float height)
{
height = (float)srtm.getAltitude(loc.lat, loc.lng).alt;
return(true);
}
public void readQGC110wpfile(string file, bool append = false)
{
int wp_count = 0;
bool error = false;
List<Locationwp> cmds = new List<Locationwp>();
try
{
StreamReader sr = new StreamReader(file); //"defines.h"
string header = sr.ReadLine();
if (header == null || !header.Contains("QGC WPL"))
{
CustomMessageBox.Show("无效航线文件");
return;
}
while (!error && !sr.EndOfStream)
{
string line = sr.ReadLine();
// waypoints
if (line.StartsWith("#"))
continue;
string[] items = line.Split(new[] { '\t', ' ', ',' }, StringSplitOptions.RemoveEmptyEntries);
if (items.Length <= 9)
continue;
try
{
Locationwp temp = new Locationwp();
if (items[2] == "3")
{
// abs MAV_FRAME_GLOBAL_RELATIVE_ALT=3
temp.options = 1;
}
else
{
temp.options = 0;
}
temp.id = (byte)(int)Enum.Parse(typeof(MAVLink.MAV_CMD), items[3], false);
temp.p1 = float.Parse(items[4], new CultureInfo("en-US"));
if (temp.id == 99)
temp.id = 0;
temp.alt = (float)(double.Parse(items[10], new CultureInfo("en-US")));
temp.lat = (double.Parse(items[8], new CultureInfo("en-US")));
temp.lng = (double.Parse(items[9], new CultureInfo("en-US")));
temp.p2 = (float)(double.Parse(items[5], new CultureInfo("en-US")));
temp.p3 = (float)(double.Parse(items[6], new CultureInfo("en-US")));
temp.p4 = (float)(double.Parse(items[7], new CultureInfo("en-US")));
cmds.Add(temp);
wp_count++;
}
catch
{
CustomMessageBox.Show("Line invalid\n" + line);
}
}
sr.Close();
processToScreen(cmds, append);
writeKML();
gMapControl1.ZoomAndCenterMarkers("objects");
}
catch (Exception ex)
{
CustomMessageBox.Show("Can't open file! " + ex);
}
}
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();
// 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", "");
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: 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 " + 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=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"))
{
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--APMPLANNER\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\nContent-Type: image/jpg\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.packets[(byte)MAVLink.MAVLINK_MSG_ID.ATTITUDE] != null)
{
message.ATTITUDE = new Message2()
{
index = MainV2.comPort.MAV.packetseencount[(byte)MAVLink.MAVLINK_MSG_ID.ATTITUDE], msg = MainV2.comPort.MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.ATTITUDE].ByteArrayToStructure <MAVLink.mavlink_attitude_t>(6)
}
}
;
if (MainV2.comPort.MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.VFR_HUD] != null)
{
message.VFR_HUD = new Message2()
{
index = MainV2.comPort.MAV.packetseencount[(byte)MAVLink.MAVLINK_MSG_ID.VFR_HUD], msg = MainV2.comPort.MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.VFR_HUD].ByteArrayToStructure <MAVLink.mavlink_vfr_hud_t>(6)
}
}
;
if (MainV2.comPort.MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.NAV_CONTROLLER_OUTPUT] != null)
{
message.NAV_CONTROLLER_OUTPUT = new Message2()
{
index = MainV2.comPort.MAV.packetseencount[(byte)MAVLink.MAVLINK_MSG_ID.NAV_CONTROLLER_OUTPUT], msg = MainV2.comPort.MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.NAV_CONTROLLER_OUTPUT].ByteArrayToStructure <MAVLink.mavlink_nav_controller_output_t>(6)
}
}
;
if (MainV2.comPort.MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.GPS_RAW_INT] != null)
{
message.GPS_RAW_INT = new Message2()
{
index = MainV2.comPort.MAV.packetseencount[(byte)MAVLink.MAVLINK_MSG_ID.GPS_RAW_INT], msg = MainV2.comPort.MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.GPS_RAW_INT].ByteArrayToStructure <MAVLink.mavlink_gps_raw_int_t>(6)
}
}
;
if (MainV2.comPort.MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.HEARTBEAT] != null)
{
message.HEARTBEAT = new Message2()
{
index = MainV2.comPort.MAV.packetseencount[(byte)MAVLink.MAVLINK_MSG_ID.HEARTBEAT], msg = MainV2.comPort.MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.HEARTBEAT].ByteArrayToStructure <MAVLink.mavlink_heartbeat_t>(6)
}
}
;
if (MainV2.comPort.MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.GPS_STATUS] != null)
{
message.GPS_STATUS = new Message2()
{
index = MainV2.comPort.MAV.packetseencount[(byte)MAVLink.MAVLINK_MSG_ID.GPS_STATUS], msg = MainV2.comPort.MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.GPS_STATUS].ByteArrayToStructure <MAVLink.mavlink_gps_status_t>(6)
}
}
;
if (MainV2.comPort.MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.STATUSTEXT] != null)
{
message.STATUSTEXT = new Message2()
{
index = MainV2.comPort.MAV.packetseencount[(byte)MAVLink.MAVLINK_MSG_ID.STATUSTEXT], msg = MainV2.comPort.MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.STATUSTEXT].ByteArrayToStructure <MAVLink.mavlink_statustext_t>(6)
}
}
;
if (MainV2.comPort.MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.SYS_STATUS] != null)
{
message.SYS_STATUS = new Message2()
{
index = MainV2.comPort.MAV.packetseencount[(byte)MAVLink.MAVLINK_MSG_ID.SYS_STATUS], msg = MainV2.comPort.MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.SYS_STATUS].ByteArrayToStructure <MAVLink.mavlink_sys_status_t>(6)
}
}
;
message.META_LINKQUALITY = message.SYS_STATUS = new Message2()
{
index = packetindex, time_usec = 0, 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, 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>
";
temp = asciiEncoding.GetBytes(content);
stream.Write(temp, 0, temp.Length);
}
stream.Close();
}
catch (Exception ee)
{
log.Error("Failed http ", ee);
}
}
}
float longitude_scale(Locationwp loc)
{
int32_t last_lat = -999999999;
float scale = 1.0f;
if (labs(last_lat - loc.lat) < 100000)
{
// we are within 0.01 degrees (about 1km) of the
// same latitude. We can avoid the cos() and return
// the same scale factor.
return scale;
}
scale = cosf(loc.lat*1.0e-7f*DEG_TO_RAD);
scale = constrain_float(scale, 0.01f, 1.0f);
last_lat = (int32_t) loc.lat;
return scale;
}
private static float longitude_scale(Locationwp loc)
{
float scale = cosf(loc.lat * 1.0e-7f * DEG_TO_RAD);
return(constrain_float(scale, 0.01f, 1.0f));
}
private void updateLocationLabel(Locationwp plla)
{
if (!Instance.IsDisposed)
{
Instance.BeginInvoke((MethodInvoker)delegate
{
Instance.LBL_location.Text = gotolocation.Lat + " " + gotolocation.Lng + " " + gotolocation.Alt + " " + gotolocation.Tag;
}
);
}
}
/*
return terrain height in meters above average sea level (WGS84) for
a given position
This is the base function that other height calculations are derived
from. The functions below are more convenient for most uses
This function costs about 20 microseconds on Pixhawk
*/
bool height_amsl(Locationwp loc, ref float height)
{
height = (float) srtm.getAltitude(loc.lat, loc.lng).alt;
return true;
}
/// <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, byte autocontinue = 1)
{
giveComport = true;
mavlink_mission_item_t req = new mavlink_mission_item_t();
req.target_system = MAV.sysid;
req.target_component = MAV.compid; // MSG_NAMES.MISSION_ITEM
req.command = loc.id;
req.current = current;
req.autocontinue = autocontinue;
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((byte)MAVLINK_MSG_ID.MISSION_ITEM, req);
DateTime start = DateTime.Now;
int retrys = 10;
while (true)
{
if (!(start.AddMilliseconds(400) > DateTime.Now))
{
if (retrys > 0)
{
log.Info("setWP Retry " + retrys);
generatePacket((byte)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] == (byte)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)
{
MAV.GuidedMode = req;
}
else if (req.current == 3)
{
}
else
{
MAV.wps[req.seq] = req;
}
return (MAV_MISSION_RESULT)ans.type;
}
else if (buffer[5] == (byte)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)
{
MAV.GuidedMode = req;
}
else if (req.current == 3)
{
}
else
{
MAV.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);
// resend point now
start = DateTime.MinValue;
}
}
else
{
//Console.WriteLine(DateTime.Now + " PC setwp " + buffer[5]);
}
}
}
// return MAV_MISSION_RESULT.MAV_MISSION_INVALID;
}
/*
* extrapolate latitude/longitude given bearing and distance
* Note that this function is accurate to about 1mm at a distance of
* 100m. This function has the advantage that it works in relative
* positions, so it keeps the accuracy even when dealing with small
* distances and floating point numbers
*/
void location_update(Locationwp loc, float bearing, float distance)
{
float ofs_north = cosf(radians(bearing))*distance;
float ofs_east = sinf(radians(bearing))*distance;
location_offset(loc, ofs_north, ofs_east);
}
public static Item ConvertFromLocationwp(Locationwp locationwp)
{
return(locationwp);
}
/*
* extrapolate latitude/longitude given distances north and east
* This function costs about 80 usec on an AVR2560
*/
void location_offset(Locationwp loc, float ofs_north, float ofs_east)
{
if (!is_zero(ofs_north) || !is_zero(ofs_east))
{
int32_t dlat = (int32_t) (ofs_north*LOCATION_SCALING_FACTOR_INV);
int32_t dlng = (int32_t) ((ofs_east*LOCATION_SCALING_FACTOR_INV)/longitude_scale(loc));
loc.lat += dlat;
loc.lng += dlng;
}
}
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 = 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\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 = 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.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, time_usec = 0, 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, 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>
";
temp = asciiEncoding.GetBytes(content);
stream.Write(temp, 0, temp.Length);
stream.Close();
}
stream.Close();
}
catch (Exception ee)
{
log.Error("Failed mjpg ", ee);
try
{
client.Close();
}
catch { }
}
}
}
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 PointLatLngAlt(Locationwp locwp)
{
this.Lat = locwp.lat;
this.Lng = locwp.lng;
this.Alt = locwp.alt;
}
void mainloop()
{
DateTime nextsend = DateTime.Now;
StreamWriter sw = new StreamWriter(File.OpenWrite("followmeraw.txt"));
threadrun = true;
while (threadrun)
{
try
{
string line = comPort.ReadLine();
sw.WriteLine(line);
//string line = string.Format("$GP{0},{1:HHmmss},{2},{3},{4},{5},{6},{7},{8},{9},{10},{11},{12},{13},", "GGA", DateTime.Now.ToUniversalTime(), Math.Abs(lat * 100), MainV2.comPort.MAV.cs.lat < 0 ? "S" : "N", Math.Abs(lng * 100), MainV2.comPort.MAV.cs.lng < 0 ? "W" : "E", MainV2.comPort.MAV.cs.gpsstatus, MainV2.comPort.MAV.cs.satcount, MainV2.comPort.MAV.cs.gpshdop, MainV2.comPort.MAV.cs.alt, "M", 0, "M", "");
if (line.StartsWith("$GPGGA")) //
{
string[] items = line.Trim().Split(',', '*');
if (items[15] != GetChecksum(line.Trim()))
{
Console.WriteLine("Bad Nmea line " + items[15] + " vs " + GetChecksum(line.Trim()));
continue;
}
if (items[6] == "0")
{
Console.WriteLine("No Fix");
continue;
}
gotolocation.Lat = double.Parse(items[2], CultureInfo.InvariantCulture)/100.0;
gotolocation.Lat = (int) gotolocation.Lat + ((gotolocation.Lat - (int) gotolocation.Lat)/0.60);
if (items[3] == "S")
gotolocation.Lat *= -1;
gotolocation.Lng = double.Parse(items[4], CultureInfo.InvariantCulture)/100.0;
gotolocation.Lng = (int) gotolocation.Lng + ((gotolocation.Lng - (int) gotolocation.Lng)/0.60);
if (items[5] == "W")
gotolocation.Lng *= -1;
gotolocation.Alt = intalt;
gotolocation.Tag = "Sats " + items[7] + " hdop " + items[8];
}
if (DateTime.Now > nextsend && gotolocation.Lat != 0 && gotolocation.Lng != 0 &&
gotolocation.Alt != 0) // 200 * 10 = 2 sec /// lastgotolocation != gotolocation &&
{
nextsend = DateTime.Now.AddMilliseconds(1000/updaterate);
Console.WriteLine("Sending follow wp " + DateTime.Now.ToString("h:MM:ss") + " " +
gotolocation.Lat + " " + gotolocation.Lng + " " + gotolocation.Alt);
lastgotolocation = new PointLatLngAlt(gotolocation);
Locationwp gotohere = new Locationwp();
gotohere.id = (byte) MAVLink.MAV_CMD.WAYPOINT;
gotohere.alt = (float) (gotolocation.Alt);
gotohere.lat = (gotolocation.Lat);
gotohere.lng = (gotolocation.Lng);
try
{
updateLocationLabel(gotohere);
}
catch
{
}
if (MainV2.comPort.BaseStream.IsOpen && MainV2.comPort.giveComport == false)
{
try
{
MainV2.comPort.giveComport = true;
MainV2.comPort.setGuidedModeWP(gotohere);
MainV2.comPort.giveComport = false;
}
catch
{
MainV2.comPort.giveComport = false;
}
}
}
}
catch
{
System.Threading.Thread.Sleep((int) (1000/updaterate));
}
}
sw.Close();
}
public PointLatLngAlt(Locationwp locwp)
{
this.Lat = locwp.lat;
this.Lng = locwp.lng;
this.Alt = locwp.alt;
}
void saveWPs(object sender, ProgressWorkerEventArgs e, object passdata = null)
{
try
{
MAVLinkInterface port = comPort;
if (!port.BaseStream.IsOpen)
{
//throw new Exception("Please connect first!");
throw new Exception("请先连接无人机!");
}
comPort.giveComport = true;
int a = 0;
// define the home point
Locationwp home = new Locationwp();
try
{
home.id = (byte)MAVLink.MAV_CMD.WAYPOINT;
home.lat = (double.Parse(TXT_homelat.Text));
home.lng = (double.Parse(TXT_homelng.Text));
home.alt = (float.Parse(TXT_homealt.Text) / CurrentState.multiplierdist); // use saved home
}
catch { throw new Exception("Your home location is invalid"); }
// log
//log.Info("wps values " + comPort.MAV.wps.Values.Count);
//log.Info("cmd rows " + (Commands.Rows.Count + 1)); // + home
// check for changes / future mod to send just changed wp's
if (comPort.MAV.wps.Values.Count == (Commands.Rows.Count + 1))
{
Hashtable wpstoupload = new Hashtable();
a = -1;
foreach (var item in comPort.MAV.wps.Values)
{
// skip home
if (a == -1)
{
a++;
continue;
}
MAVLink.mavlink_mission_item_t temp = DataViewtoLocationwp(a);
if (temp.command == item.command &&
temp.x == item.x &&
temp.y == item.y &&
temp.z == item.z &&
temp.param1 == item.param1 &&
temp.param2 == item.param2 &&
temp.param3 == item.param3 &&
temp.param4 == item.param4
)
{
//log.Info("wp match " + (a + 1));
}
else
{
//log.Info("wp no match" + (a + 1));
wpstoupload[a] = "";
}
a++;
}
}
// set wp total
((ProgressReporterDialogue)sender).UpdateProgressAndStatus(0, "上传航点数量");
ushort totalwpcountforupload = (ushort)(Commands.Rows.Count + 1);
port.setWPTotal(totalwpcountforupload); // + home
// set home location - overwritten/ignored depending on firmware.
((ProgressReporterDialogue)sender).UpdateProgressAndStatus(0, "上传回家点");
var homeans = port.setWP(home, 0, MAVLink.MAV_FRAME.GLOBAL, 0);
if (homeans != MAVLink.MAV_MISSION_RESULT.MAV_MISSION_ACCEPTED)
{
CustomMessageBox.Show(Strings.ErrorRejectedByMAV, Strings.ERROR);
return;
}
// define the default frame.
MAVLink.MAV_FRAME frame = MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT;
// get the command list from the datagrid
var commandlist = GetCommandList();
// upload from wp1, as home is alreadey sent
a = 1;
// process commandlist to the mav
foreach (var temp in commandlist)
{
// this code below fails
//a = commandlist.IndexOf(temp) + 1;
((ProgressReporterDialogue)sender).UpdateProgressAndStatus(a * 100 / Commands.Rows.Count, "上传航点 " + a);
// make sure we are using the correct frame for these commands
if (temp.id < (byte)MAVLink.MAV_CMD.LAST || temp.id == (byte)MAVLink.MAV_CMD.DO_SET_HOME)
{
var mode = currentaltmode;
if (mode == altmode.Terrain)
{
frame = MAVLink.MAV_FRAME.GLOBAL_TERRAIN_ALT;
}
else if (mode == altmode.Absolute)
{
frame = MAVLink.MAV_FRAME.GLOBAL;
}
else
{
frame = MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT;
}
}
// try send the wp
MAVLink.MAV_MISSION_RESULT ans = port.setWP(temp, (ushort)(a), frame, 0);
// we timed out while uploading wps/ command wasnt replaced/ command wasnt added
if (ans == MAVLink.MAV_MISSION_RESULT.MAV_MISSION_ERROR)
{
// resend for partial upload
port.setWPPartialUpdate((ushort)(a), totalwpcountforupload);
// reupload this point.
ans = port.setWP(temp, (ushort)(a), frame, 0);
}
if (ans == MAVLink.MAV_MISSION_RESULT.MAV_MISSION_NO_SPACE)
{
//e.ErrorMessage = "Upload failed, please reduce the number of wp's";
e.ErrorMessage = "上传航点失败,航点数量太多,请减少航点数量.";
return;
}
if (ans == MAVLink.MAV_MISSION_RESULT.MAV_MISSION_INVALID)
{
e.ErrorMessage = "Upload failed, mission was rejected byt the Mav,\n item had a bad option wp# " + a + " " + ans;
return;
}
if (ans == MAVLink.MAV_MISSION_RESULT.MAV_MISSION_INVALID_SEQUENCE)
{
// invalid sequence can only occur if we failed to see a response from the apm when we sent the request.
// therefore it did see the request and has moved on that step, and so do we.
a++;
continue;
}
if (ans != MAVLink.MAV_MISSION_RESULT.MAV_MISSION_ACCEPTED)
{
//e.ErrorMessage = "Upload wps failed " + Enum.Parse(typeof(MAVLink.MAV_CMD), temp.id.ToString()) + " " + Enum.Parse(typeof(MAVLink.MAV_MISSION_RESULT), ans.ToString());
e.ErrorMessage = "上传失败 " + Enum.Parse(typeof(MAVLink.MAV_CMD), temp.id.ToString()) + " " + Enum.Parse(typeof(MAVLink.MAV_MISSION_RESULT), ans.ToString());
return;
}
a++;
}
port.setWPACK();
((ProgressReporterDialogue)sender).UpdateProgressAndStatus(95, "Setting params");
// m
port.setParam("WP_RADIUS", (byte)int.Parse(TXT_WPRad.Text) / CurrentState.multiplierdist);
// cm's
port.setParam("WPNAV_RADIUS", (byte)int.Parse(TXT_WPRad.Text) / CurrentState.multiplierdist * 100);
try
{
port.setParam(new[] { "LOITER_RAD", "WP_LOITER_RAD" }, int.Parse(TXT_loiterrad.Text) / CurrentState.multiplierdist);
}
catch
{
}
((ProgressReporterDialogue)sender).UpdateProgressAndStatus(100, "完成.");
}
catch (Exception ex)
{
log.Error(ex); comPort.giveComport = false; throw;
}
comPort.giveComport = false;
}
public Locationwp getHomePosition()
{
doCommand(MAV_CMD.GET_HOME_POSITION, 0, 0, 0, 0, 0, 0, 0);
giveComport = true;
MAVLinkMessage buffer;
DateTime start = DateTime.Now;
int retrys = 3;
while (true)
{
if (!(start.AddMilliseconds(700) > DateTime.Now))
{
if (retrys > 0)
{
log.Info("getHomePosition Retry " + retrys + " - giv com " + giveComport);
doCommand(MAV_CMD.GET_HOME_POSITION, 0, 0, 0, 0, 0, 0, 0);
giveComport = true;
start = DateTime.Now;
retrys--;
continue;
}
giveComport = false;
//return (byte)int.Parse(param["WP_TOTAL"].ToString());
throw new TimeoutException("Timeout on read - getHomePosition");
}
buffer = readPacket();
if (buffer.Length > 5)
{
if (buffer.msgid == (byte)MAVLINK_MSG_ID.HOME_POSITION)
{
var home = buffer.ToStructure<mavlink_home_position_t>();
var loc = new Locationwp().Set(home.latitude / 1.0e7, home.longitude / 1.0e7, home.altitude / 1000.0, (byte)MAV_CMD.WAYPOINT);
giveComport = false;
return loc; // should be ushort, but apm has limited wp count < byte
}
}
else
{
log.Info(DateTime.Now + " PC getHomePosition ");
}
}
}
/// <summary>
/// used to write a KML, update the Map view polygon, and update the row headers
/// </summary>
public void writeKML()
{
// quickadd is for when loading wps from eeprom or file, to prevent slow, loading times
if (quickadd)
return;
// this is to share the current mission with the data tab
pointlist = new List<PointLatLngAlt>();
fullpointlist.Clear();
Debug.WriteLine(DateTime.Now);
try
{
if (objectsoverlay != null) // hasnt been created yet
{
objectsoverlay.Markers.Clear();
}
// process and add home to the list
string home;
if (TXT_homealt.Text != "" && TXT_homelat.Text != "" && TXT_homelng.Text != "")
{
home = string.Format("{0},{1},{2}\r\n", TXT_homelng.Text, TXT_homelat.Text, TXT_DefaultAlt.Text);
if (objectsoverlay != null) // during startup
{
pointlist.Add(new PointLatLngAlt(double.Parse(TXT_homelat.Text), double.Parse(TXT_homelng.Text), (int)double.Parse(TXT_homealt.Text), "H"));
fullpointlist.Add(pointlist[pointlist.Count - 1]);
addpolygonmarker("H", double.Parse(TXT_homelng.Text), double.Parse(TXT_homelat.Text), 0, null);
}
}
else
{
home = "";
pointlist.Add(null);
fullpointlist.Add(pointlist[pointlist.Count - 1]);
}
// setup for centerpoint calc etc.
double avglat = 0;
double avglong = 0;
double maxlat = -180;
double maxlong = -180;
double minlat = 180;
double minlong = 180;
double homealt = 0;
try
{
if (!String.IsNullOrEmpty(TXT_homealt.Text))
homealt = (int)double.Parse(TXT_homealt.Text);
}
catch { }
//if ((altmode)CMB_altmode.SelectedValue == altmode.Absolute)
//{
// homealt = 0; // for absolute we dont need to add homealt
//}
int usable = 0;
updateRowNumbers();
long temp = Stopwatch.GetTimestamp();
string lookat = "";
for (int a = 0; a < Commands.Rows.Count - 0; a++)
{
try
{
if (Commands.Rows[a].Cells[Command.Index].Value.ToString().Contains("UNKNOWN"))
continue;
int command = (byte)(int)Enum.Parse(typeof(MAVLink.MAV_CMD), Commands.Rows[a].Cells[Command.Index].Value.ToString(), false);
if (command < (byte)MAVLink.MAV_CMD.LAST &&
command != (byte)MAVLink.MAV_CMD.TAKEOFF &&
command != (byte)MAVLink.MAV_CMD.RETURN_TO_LAUNCH &&
command != (byte)MAVLink.MAV_CMD.CONTINUE_AND_CHANGE_ALT &&
command != (byte)MAVLink.MAV_CMD.GUIDED_ENABLE
|| command == (byte)MAVLink.MAV_CMD.DO_SET_ROI)
{
string cell2 = Commands.Rows[a].Cells[Alt.Index].Value.ToString(); // alt
string cell3 = Commands.Rows[a].Cells[Lat.Index].Value.ToString(); // lat
string cell4 = Commands.Rows[a].Cells[Lon.Index].Value.ToString(); // lng
// land can be 0,0 or a lat,lng
if (command == (byte)MAVLink.MAV_CMD.LAND && cell3 == "0" && cell4 == "0")
continue;
if (cell4 == "?" || cell3 == "?")
continue;
if (command == (byte)MAVLink.MAV_CMD.DO_SET_ROI)
{
pointlist.Add(new PointLatLngAlt(double.Parse(cell3), double.Parse(cell4), (int)double.Parse(cell2) + homealt, "ROI" + (a + 1)) { color = Color.Red });
// do set roi is not a nav command. so we dont route through it
//fullpointlist.Add(pointlist[pointlist.Count - 1]);
GMarkerGoogle m = new GMarkerGoogle(new PointLatLng(double.Parse(cell3), double.Parse(cell4)), GMarkerGoogleType.red);
m.ToolTipMode = MarkerTooltipMode.Always;
m.ToolTipText = (a + 1).ToString();
m.Tag = (a + 1).ToString();
GMapMarkerRect mBorders = new GMapMarkerRect(m.Position);
{
mBorders.InnerMarker = m;
mBorders.Tag = "Dont draw line";
}
// check for clear roi, and hide it
if (m.Position.Lat != 0 && m.Position.Lng != 0)
{
// order matters
objectsoverlay.Markers.Add(m);
objectsoverlay.Markers.Add(mBorders);
}
}
else if (command == (byte)MAVLink.MAV_CMD.LOITER_TIME || command == (byte)MAVLink.MAV_CMD.LOITER_TURNS || command == (byte)MAVLink.MAV_CMD.LOITER_UNLIM)
{
pointlist.Add(new PointLatLngAlt(double.Parse(cell3), double.Parse(cell4), (int)double.Parse(cell2) + homealt, (a + 1).ToString()) { color = Color.LightBlue });
fullpointlist.Add(pointlist[pointlist.Count - 1]);
addpolygonmarker((a + 1).ToString(), double.Parse(cell4), double.Parse(cell3), (int)double.Parse(cell2), Color.LightBlue);
}
else if (command == (byte)MAVLink.MAV_CMD.SPLINE_WAYPOINT)
{
pointlist.Add(new PointLatLngAlt(double.Parse(cell3), double.Parse(cell4), (int)double.Parse(cell2) + homealt, (a + 1).ToString()) { Tag2 = "spline" });
fullpointlist.Add(pointlist[pointlist.Count - 1]);
addpolygonmarker((a + 1).ToString(), double.Parse(cell4), double.Parse(cell3), (int)double.Parse(cell2), Color.Green);
}
else
{
pointlist.Add(new PointLatLngAlt(double.Parse(cell3), double.Parse(cell4), (int)double.Parse(cell2) + homealt, (a + 1).ToString()));
fullpointlist.Add(pointlist[pointlist.Count - 1]);
addpolygonmarker((a + 1).ToString(), double.Parse(cell4), double.Parse(cell3), (int)double.Parse(cell2), null);
}
avglong += double.Parse(Commands.Rows[a].Cells[Lon.Index].Value.ToString());
avglat += double.Parse(Commands.Rows[a].Cells[Lat.Index].Value.ToString());
usable++;
maxlong = Math.Max(double.Parse(Commands.Rows[a].Cells[Lon.Index].Value.ToString()), maxlong);
maxlat = Math.Max(double.Parse(Commands.Rows[a].Cells[Lat.Index].Value.ToString()), maxlat);
minlong = Math.Min(double.Parse(Commands.Rows[a].Cells[Lon.Index].Value.ToString()), minlong);
minlat = Math.Min(double.Parse(Commands.Rows[a].Cells[Lat.Index].Value.ToString()), minlat);
Debug.WriteLine(temp - Stopwatch.GetTimestamp());
}
else if (command == (byte)MAVLink.MAV_CMD.DO_JUMP) // fix do jumps into the future
{
pointlist.Add(null);
int wpno = int.Parse(Commands.Rows[a].Cells[Param1.Index].Value.ToString());
int repeat = int.Parse(Commands.Rows[a].Cells[Param2.Index].Value.ToString());
List<PointLatLngAlt> list = new List<PointLatLngAlt>();
// cycle through reps
for (int repno = repeat; repno > 0; repno--)
{
// cycle through wps
for (int no = wpno; no <= a; no++)
{
if (pointlist[no] != null)
list.Add(pointlist[no]);
}
}
fullpointlist.AddRange(list);
}
else
{
pointlist.Add(null);
}
}
catch (Exception e)
{
log.Info("writekml - bad wp data " + e);
}
}
if (usable > 0)
{
avglat = avglat / usable;
avglong = avglong / usable;
double latdiff = maxlat - minlat;
double longdiff = maxlong - minlong;
float range = 4000;
Locationwp loc1 = new Locationwp();
loc1.lat = (minlat);
loc1.lng = (minlong);
Locationwp loc2 = new Locationwp();
loc2.lat = (maxlat);
loc2.lng = (maxlong);
//double distance = getDistance(loc1, loc2); // same code as ardupilot
double distance = 2000;
if (usable > 1)
{
range = (float)(distance * 2);
}
else
{
range = 4000;
}
if (avglong != 0 && usable < 3)
{
// no autozoom
lookat = "<LookAt> <longitude>" + (minlong + longdiff / 2).ToString(new CultureInfo("en-US")) + "</longitude> <latitude>" + (minlat + latdiff / 2).ToString(new CultureInfo("en-US")) + "</latitude> <range>" + range + "</range> </LookAt>";
//MainMap.ZoomAndCenterMarkers("objects");
//MainMap.Zoom -= 1;
//MainMap_OnMapZoomChanged();
}
}
else if (home.Length > 5 && usable == 0)
{
lookat = "<LookAt> <longitude>" + TXT_homelng.Text.ToString(new CultureInfo("en-US")) + "</longitude> <latitude>" + TXT_homelat.Text.ToString(new CultureInfo("en-US")) + "</latitude> <range>4000</range> </LookAt>";
RectLatLng? rect = gMapControl1.GetRectOfAllMarkers("objects");
if (rect.HasValue)
{
gMapControl1.Position = rect.Value.LocationMiddle;
}
//MainMap.Zoom = 17;
//MainMap_OnMapZoomChanged();
}
//RegeneratePolygon();
RegenerateWPRoute(fullpointlist);
if (fullpointlist.Count > 0)
{
double homedist = 0;
if (home.Length > 5)
{
homedist = gMapControl1.MapProvider.Projection.GetDistance(fullpointlist[fullpointlist.Count - 1], fullpointlist[0]);
}
double dist = 0;
for (int a = 1; a < fullpointlist.Count; a++)
{
if (fullpointlist[a - 1] == null)
continue;
if (fullpointlist[a] == null)
continue;
dist += gMapControl1.MapProvider.Projection.GetDistance(fullpointlist[a - 1], fullpointlist[a]);
}
//lbl_distance.Text = rm.GetString("lbl_distance.Text") + ": " + FormatDistance(dist + homedist, false);
}
setgradanddistandaz();
}
catch (Exception ex)
{
log.Info(ex.ToString());
}
Debug.WriteLine(DateTime.Now);
}
/// <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,
byte autocontinue = 1, bool use_int = false)
{
if (use_int)
{
mavlink_mission_item_int_t req = new mavlink_mission_item_int_t();
req.target_system = MAV.sysid;
req.target_component = MAV.compid;
req.command = loc.id;
req.current = current;
req.autocontinue = autocontinue;
req.frame = (byte) frame;
req.y = (int) (loc.lng*1.0e7);
req.x = (int) (loc.lat*1.0e7);
req.z = (float) (loc.alt);
req.param1 = loc.p1;
req.param2 = loc.p2;
req.param3 = loc.p3;
req.param4 = loc.p4;
req.seq = index;
return setWP(req);
}
else
{
mavlink_mission_item_t req = new mavlink_mission_item_t();
req.target_system = MAV.sysid;
req.target_component = MAV.compid;
req.command = loc.id;
req.current = current;
req.autocontinue = autocontinue;
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;
return setWP(req);
}
}
Locationwp DataViewtoLocationwp(int a)
{
Locationwp temp = new Locationwp();
if (Commands.Rows[a].Cells[Command.Index].Value.ToString().Contains("UNKNOWN"))
{
temp.id = (byte)Commands.Rows[a].Cells[Command.Index].Tag;
}
else
{
temp.id = (byte)(int)Enum.Parse(typeof(MAVLink.MAV_CMD), Commands.Rows[a].Cells[Command.Index].Value.ToString(), false);
}
temp.p1 = float.Parse(Commands.Rows[a].Cells[Param1.Index].Value.ToString());
temp.alt = (float)(double.Parse(Commands.Rows[a].Cells[Alt.Index].Value.ToString()) / CurrentState.multiplierdist);
temp.lat = (double.Parse(Commands.Rows[a].Cells[Lat.Index].Value.ToString()));
temp.lng = (double.Parse(Commands.Rows[a].Cells[Lon.Index].Value.ToString()));
temp.p2 = (float)(double.Parse(Commands.Rows[a].Cells[Param2.Index].Value.ToString()));
temp.p3 = (float)(double.Parse(Commands.Rows[a].Cells[Param3.Index].Value.ToString()));
temp.p4 = (float)(double.Parse(Commands.Rows[a].Cells[Param4.Index].Value.ToString()));
return temp;
}
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 static MissionItem ConvertFromLocationwp(Locationwp locationwp)
{
return locationwp;
}
