private void BUT_load_Click(object sender, EventArgs e)
{
var load = Sequence.Load("test.txt");
if (load != null)
{
workingSequence = load;
}
bindingSource1.DataSource = workingSequence;
if (load.Layouts.Count == 0)
{
return;
}
num_drones.Value = load.Layouts.First().Offset.Keys.Count();
foreach (var sysid in load.Layouts.First().Offset.Keys)
{
mavs[sysid] = new MAVState(mavint, (byte)sysid, 0);
}
comboBox1_SelectedIndexChanged(null, null);
UpdateDisplay();
}
private void copter_select_SelectedValueClick(object sender, EventArgs e)
{
if (copter_select.SelectedItem != null)
{
Selectedid = byte.Parse((string)copter_select.SelectedItem);
try //request data stream instantly
{
MAVState MAV = MainV2.comPort.MAV;
MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.EXTENDED_STATUS, MAV.cs.ratestatus,
MAV.sysid, MAV.compid); // mode
MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.POSITION, MAV.cs.rateposition,
MAV.sysid, MAV.compid); // request gps
MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.EXTRA1, MAV.cs.rateattitude,
MAV.sysid, MAV.compid); // request attitude
MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.EXTRA2, MAV.cs.rateattitude,
MAV.sysid, MAV.compid); // request vfr
MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.EXTRA3, MAV.cs.ratesensors, MAV.sysid,
MAV.compid); // request extra stuff - tridge
MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MAV.cs.ratesensors,
MAV.sysid, MAV.compid); // request raw sensor
MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RC_CHANNELS, MAV.cs.raterc, MAV.sysid,
MAV.compid); // request rc info
}
catch
{
Console.WriteLine("Failed to request rates");
}
}
else
{
Selectedid = 0;
}
}
public void UpdateIcon(MAVState mav, float x, float y, float z, bool movable)
{
foreach (var icon in icons)
{
if (icon.interf == mav)
{
icon.Movable = movable;
if (!movable)
{
icon.x = 0;
icon.y = 0;
icon.z = 0;
icon.Color = Color.Blue;
}
else
{
icon.x = x;
icon.y = y;
icon.z = z;
icon.Color = Color.Red;
}
this.Invalidate();
return;
}
}
Console.WriteLine("ADD MAV {0} {1} {2}", x, y, z);
icons.Add(new icon()
{
interf = mav, y = y, z = z, x = x, Movable = movable, Name = mav.ToString()
});
this.Invalidate();
}
internal void Create(byte sysid, byte compid)
{
int id = (byte)sysid * 256 + (byte)compid;
if (!masterlist.ContainsKey(id))
masterlist[id] = new MAVState();
}
private void BUT_load_Click(object sender, EventArgs e)
{
OpenFileDialog ofd = new OpenFileDialog();
ofd.DefaultExt = ".txt";
if (ofd.ShowDialog(this) != DialogResult.OK)
{
return;
}
var load = Sequence.Load(ofd.FileName);
if (load != null)
{
workingSequence = load;
}
bindingSource1.DataSource = workingSequence;
if (load.Layouts.Count == 0)
{
return;
}
num_drones.Value = load.Layouts.First().Offset.Keys.Count();
foreach (var sysid in load.Layouts.First().Offset.Keys)
{
mavs[sysid] = new MAVState(mavint, (byte)sysid, 0);
}
comboBox1_SelectedIndexChanged(null, null);
UpdateDisplay();
}
public Vector3 getOffsetFromLeader(MAVState leader, MAVState mav)
{
//convert Wgs84ConversionInfo to utm
CoordinateTransformationFactory ctfac = new CoordinateTransformationFactory();
IGeographicCoordinateSystem wgs84 = GeographicCoordinateSystem.WGS84;
int utmzone = (int)((leader.cs.lng - -186.0) / 6.0);
IProjectedCoordinateSystem utm = ProjectedCoordinateSystem.WGS84_UTM(utmzone,
leader.cs.lat < 0 ? false : true);
ICoordinateTransformation trans = ctfac.CreateFromCoordinateSystems(wgs84, utm);
double[] masterpll = { leader.cs.lng, leader.cs.lat };
// get leader utm coords
double[] masterutm = trans.MathTransform.Transform(masterpll);
double[] mavpll = { mav.cs.lng, mav.cs.lat };
//getLeader follower utm coords
double[] mavutm = trans.MathTransform.Transform(mavpll);
var heading = -leader.cs.yaw;
var norotation = new Vector3(masterutm[1] - mavutm[1], masterutm[0] - mavutm[0], 0);
norotation.x *= -1;
norotation.y *= -1;
return(new Vector3(norotation.x * Math.Cos(heading * MathHelper.deg2rad) - norotation.y * Math.Sin(heading * MathHelper.deg2rad), norotation.x * Math.Sin(heading * MathHelper.deg2rad) + norotation.y * Math.Cos(heading * MathHelper.deg2rad), 0));
}
public ProximityControl(MAVState state)
{
_parent = state;
Paint += Temp_Paint;
KeyPress += Temp_KeyPress;
Resize += Temp_Resize;
}
public static List <PointLatLngAlt> GenerateOffsetPath(MAVState MAV, Vector3 offset)
{
List <PointLatLngAlt> result = GeneratePath(MAV);
return(result);
}
public static List<PointLatLngAlt> GenerateOffsetPath(MAVState MAV, Vector3 offset)
{
List<PointLatLngAlt> result = GeneratePath(MAV);
return result;
}
public Proximity(MAVState mavInt)
{
_parent = mavInt;
sub = mavInt.parent.SubscribeToPacketType(MAVLINK_MSG_ID.DISTANCE_SENSOR, messageReceived);
log.InfoFormat("created for {0} - {1}", mavInt.sysid, mavInt.compid);
temp.Paint += Temp_Paint;
}
public Proximity(MAVState mavInt)
{
_parent = mavInt;
temp.Paint += Temp_Paint;
temp.KeyPress += Temp_KeyPress;
temp.Resize += Temp_Resize;
}
internal void Create(byte sysid, byte compid)
{
int id = (byte)sysid * 256 + (byte)compid;
if (!masterlist.ContainsKey(id))
{
masterlist[id] = new MAVState();
}
}
/// <summary>
/// 设置飞行参数
/// </summary>
/// <param name="MAV"></param>
/// <param name="IsCome"></param>
/// <param name="Ip"></param>
private void Dv1_linkEvent(MAVState MAV, bool IsCome, string Ip)
{
if (IsCome)
{
if (Ip != dv1.ComPort)
{
return;
}
}
else
{
if (Ip != dv1.IP)
{
return;
}
}
pitchAndBank1.Bank = MAV.cs.roll;
pitchAndBank1.Pitch = -MAV.cs.pitch;
airSpeedIndicator1.AirSpeed = (int)MAV.cs.groundspeed;
altitudeMeter1.Altitude = MAV.cs.alt;
//System_modelabel.Text = MAV.cs.mode;
//Firmware_label.Text = MAV.cs.firmware.ToString();
//if (dv1.comPort.isconnect)
//{
// //Connect_button.BackgroundImage = global::Amov.Planner.Properties.Resources.链接;
//}
//LinkqualityGcsLab.Text = MAV.cs.linkqualitygcs.ToString() + "%";//显示数传连接质量
//Gps_nmulabel.Text = MAV.cs.satcount.ToString();//显示GPS卫星数量
//Hdop_label.Text = MAV.cs.gpshdop.ToString();//显示GPS水平定位因子
//if (MAV.cs.gpshdop <= 3)//如果定位因子小于3，载具才会在GPS上显示
//{
// gMap.Position = new PointLatLng(MAV.cs.lat, MAV.cs.lng);
// swpc.addvehiclemarker(MAV.cs.lat, MAV.cs.lng, MAV.cs.yaw);//在地图上显示载具
//}
//if (massag_error != MAV.cs.messageHigh)//如果这次的报错和上次的不一样，就更新本次的报错内容，防止textbox重复循环打印报错
//{
// Error_box.AppendText("错误/警告:" + dv1.comPort.MAV.cs.messageHigh + "\r\n");
//}
//massag_error = MAV.cs.messageHigh;
//if (MAV.cs.gpshdop < 3 && MAV.cs.satcount > 6)//GPS信号比较良好的情况下实时显示载具轨迹
//{
// swpc.DrawWaypoint(gMap.Position, swpc.realRouteOverlay);
//}
//if (MAV.cs.armed == true) Arm_button.BackgroundImage = global::Amov.Planner.Properties.Resources._7_1已解锁;
}
public HIL.Vector3 getOffsets(MAVState mav)
{
if (offsets.ContainsKey(mav))
{
return(offsets[mav]);
}
return(new HIL.Vector3(offsets.Count, 0, 0));
}
public void AddHiddenList(byte sysid, byte compid)
{
int id = (byte)sysid * 256 + (byte)compid;
hiddenlist[id] = new MAVState()
{
sysid = sysid, compid = compid
};
}
public Proximity(MAVState mavInt)
{
_parent = mavInt;
sub = mavInt.parent.SubscribeToPacketType(MAVLINK_MSG_ID.DISTANCE_SENSOR, messageReceived);
log.InfoFormat("created for {0} - {1}", mavInt.sysid, mavInt.compid);
temp.Paint += Temp_Paint;
temp.KeyPress += Temp_KeyPress;
temp.Resize += Temp_Resize;
}
public static List <PointLatLngAlt> GeneratePath(MAVState MAV)
{
List <PointLatLngAlt> result = new List <PointLatLngAlt>();
MAVLink.mavlink_mission_item_int_t?prevwp = null;
int a = -1;
foreach (var wp in MAV.wps.Values)
{
a++;
if (!prevwp.HasValue)
{
// change firstwp/aka home to valid alt
prevwp = new MAVLink.mavlink_mission_item_int_t?(new MAVLink.mavlink_mission_item_int_t()
{
x = wp.x, y = wp.y, z = 0
});
continue;
}
if (wp.command != (ushort)MAVLink.MAV_CMD.WAYPOINT && wp.command != (ushort)MAVLink.MAV_CMD.SPLINE_WAYPOINT)
{
continue;
}
var wp1 = new PointLatLngAlt(prevwp.Value);
var wp2 = new PointLatLngAlt(wp);
var bearing = wp1.GetBearing(wp2);
var distwps = wp1.GetDistance(wp2);
var startalt = wp1.Alt;
var endalt = wp2.Alt;
if (startalt == 0)
{
startalt = endalt;
}
if (distwps > 5000)
{
continue;
}
for (double d = 0.1; d < distwps; d += 0.1)
{
var pnt = wp1.newpos(bearing, d);
pnt.Alt = startalt + (d / distwps) * (endalt - startalt);
result.Add(pnt);
}
prevwp = wp;
}
return(result);
}
private void grid1_UpdateOffsets(MAVState mav, float x, float y, float z, Grid.icon ico)
{
if (mav == SwarmInterface.Leader)
{
CustomMessageBox.Show("Can not move Leader");
ico.z = 0;
}
else
{
((Formation)SwarmInterface).setOffsets(mav, x, y, z);
}
}
public void CameraInfo(MAVState mav, Action <MAVLink.mavlink_camera_information_t> act)
{
var sub = mav.parent.SubscribeToPacketType(MAVLink.MAVLINK_MSG_ID.CAMERA_INFORMATION,
message =>
{
act((MAVLink.mavlink_camera_information_t)message.data);
return(true);
});
mav.parent.doCommand(mav.sysid, mav.compid, MAVLink.MAV_CMD.REQUEST_CAMERA_INFORMATION, 0, 0, 0, 0, 0, 0, 0);
new Timer((a) => { mav.parent.UnSubscribeToPacketType(sub); }, sub, 10000, Timeout.Infinite);
}
public Proximity(MAVState mavInt, byte sysid, byte compid)
{
this.sysid = sysid;
this.compid = compid;
_parent = mavInt;
sub = mavInt.parent.SubscribeToPacketType(MAVLINK_MSG_ID.DISTANCE_SENSOR, messageReceived, sysid,
compid);
sub2 = mavInt.parent.SubscribeToPacketType(MAVLINK_MSG_ID.OBSTACLE_DISTANCE, messageReceived, sysid,
compid);
log.InfoFormat("created for {0} - {1}", mavInt.sysid, mavInt.compid);
}
public void test(MAVState mav)
{
mav.parent.doCommand(mav.sysid, mav.compid, MAVLink.MAV_CMD.REQUEST_CAMERA_INFORMATION, 0, 0, 0, 0, 0, 0, 0);
//mav.parent.doCommand(mav.sysid, mav.compid, MAVLink.MAV_CMD.REQUEST_VIDEO_STREAM_INFORMATION, 0, 0, 0, 0, 0, 0, 0);
mav.parent.doCommand(mav.sysid, mav.compid, MAVLink.MAV_CMD.REQUEST_CAMERA_SETTINGS, 0, 0, 0, 0, 0, 0, 0);
mav.parent.doCommand(mav.sysid, mav.compid, MAVLink.MAV_CMD.SET_CAMERA_MODE, 0, 0, 0, 0, 0, 0, 0);
mav.parent.doCommand(mav.sysid, mav.compid, MAVLink.MAV_CMD.REQUEST_STORAGE_INFORMATION, 0, 0, 0, 0, 0, 0, 0);
//mav.parent.doCommand(mav.sysid, mav.compid, MAVLink.MAV_CMD.VIDEO_START_STREAMING, 0, 0, 0, 0, 0, 0, 0);
//mav.parent.doCommand(mav.sysid, mav.compid, MAVLink.MAV_CMD.VIDEO_STOP_STREAMING, 0, 0, 0, 0, 0, 0, 0);
mav.parent.doCommand(mav.sysid, mav.compid, MAVLink.MAV_CMD.VIDEO_START_CAPTURE, 0, 0, 0, 0, 0, 0, 0);
mav.parent.doCommand(mav.sysid, mav.compid, MAVLink.MAV_CMD.VIDEO_STOP_CAPTURE, 0, 0, 0, 0, 0, 0, 0);
mav.parent.doCommand(mav.sysid, mav.compid, MAVLink.MAV_CMD.IMAGE_START_CAPTURE, 0, 0, 0, 0, 0, 0, 0);
mav.parent.doCommand(mav.sysid, mav.compid, MAVLink.MAV_CMD.REQUEST_CAMERA_CAPTURE_STATUS, 0, 0, 0, 0, 0, 0, 0);
}
public static List<PointLatLngAlt> GeneratePath(MAVState MAV)
{
List<PointLatLngAlt> result = new List<PointLatLngAlt>();
MAVLink.mavlink_mission_item_t? prevwp = null;
int a = -1;
foreach (var wp in MAV.wps.Values)
{
a++;
if (!prevwp.HasValue)
{
// change firstwp/aka home to valid alt
prevwp = new MAVLink.mavlink_mission_item_t?(new MAVLink.mavlink_mission_item_t() { x=wp.x,y = wp.y, z = 0});
continue;
}
if (wp.command != (byte)MAVLink.MAV_CMD.WAYPOINT && wp.command != (byte)MAVLink.MAV_CMD.SPLINE_WAYPOINT)
continue;
var wp1 = new PointLatLngAlt(prevwp.Value);
var wp2 = new PointLatLngAlt(wp);
var bearing = wp1.GetBearing(wp2);
var distwps = wp1.GetDistance(wp2);
var startalt = wp1.Alt;
var endalt = wp2.Alt;
if (startalt == 0)
{
startalt = endalt;
}
if(distwps > 5000)
continue;
for (double d = 0.1; d < distwps; d += 0.1)
{
var pnt = wp1.newpos(bearing, d);
pnt.Alt = startalt + (d/distwps) * (endalt-startalt);
result.Add(pnt);
}
prevwp = wp;
}
return result;
}
public ProximityControl(MAVState state)
{
InitializeComponent();
_parent = state;
Paint += Temp_Paint;
KeyPress += Temp_KeyPress;
Resize += Temp_Resize;
FormClosing += ProximityControl_FormClosing;;
timer1.Interval = 100;
timer1.Tick += (s, e) => { Invalidate(); };
timer1.Start();
}
internal void Create(byte sysid, byte compid)
{
int id = (byte)sysid * 256 + (byte)compid;
// move from hidden to visible
if (hiddenlist.ContainsKey(id))
{
masterlist[id] = hiddenlist[id];
hiddenlist.Remove(id);
}
if (!masterlist.ContainsKey(id))
{
masterlist[id] = new MAVState();
}
}
private void num_drones_ValueChanged(object sender, EventArgs e)
{
var count = workingSequence.Layouts.Count > 0 ? workingSequence.Layouts.First().Offset.Keys.Count() : 0;
if (num_drones.Value == count)
{
return;
}
if (num_drones.Value < count)
{
// add the drone to all layouts
foreach (var workingSequenceLayout in workingSequence.Layouts)
{
workingSequenceLayout.Offset.Remove(count);
}
bindingSource1.DataSource = workingSequence;
}
else
{
int sysid = 1;
try
{
sysid = workingSequence.Layouts.First().Offset.Keys.Max() + 1;
}
catch
{
}
// add the drone to all layouts
foreach (var workingSequenceLayout in workingSequence.Layouts)
{
workingSequenceLayout.AddOffset(sysid, new Vector3(sysid, 0, 0));
}
mavs[sysid] = new MAVState(mavint, (byte)sysid, 0);
bindingSource1.DataSource = workingSequence;
}
UpdateDisplay();
}
//connect读取的一个事件
void readcomdisplay_readmavlinkEvent(MAVState MAV, bool falg, string Ip)
{
timetick = Environment.TickCount;
Speed = MAV.cs.groundspeed;
lat = MAV.cs.lat;
lng = MAV.cs.lng;
PannelMode = MAV.cs.PanelMode;
if (linkEvent != null)
{
if (ConnectType == ConnectType.Com)
{
linkEvent.Invoke(MAV, true, ComPort);//事件触发更新UI界面
}
else
{
linkEvent.Invoke(MAV, false, Ip);//
}
}
}
private void BUT_newdrone_Click(object sender, EventArgs e)
{
int sysid = 1;
try
{
sysid = workingSequence.Layouts.First().Offset.Keys.Max() + 1;
} catch { }
// add the drone to all layouts
foreach (var workingSequenceLayout in workingSequence.Layouts)
{
workingSequenceLayout.AddOffset(sysid, new Vector3(sysid, 0, 0));
}
mavs[sysid] = new MAVState(mavint, (byte)sysid, 0);
bindingSource1.DataSource = workingSequence;
UpdateDisplay();
}
public static string speechConversion(MAVState MAV, string input)
{
if (MAV.cs.wpno == 0)
{
input = input.Replace("{wpn}", "Home");
}
else
{
input = input.Replace("{wpn}", MAV.cs.wpno.ToString());
}
input = input.Replace("{asp}", MAV.cs.airspeed.ToString("0"));
input = input.Replace("{alt}", MAV.cs.alt.ToString("0"));
input = input.Replace("{wpa}", MAV.cs.targetalt.ToString("0"));
input = input.Replace("{gsp}", MAV.cs.groundspeed.ToString("0"));
input = input.Replace("{mode}", MAV.cs.mode.ToString());
input = input.Replace("{batv}", MAV.cs.battery_voltage.ToString("0.00"));
input = input.Replace("{batp}", (MAV.cs.battery_remaining).ToString("0"));
input = input.Replace("{vsp}", (MAV.cs.verticalspeed).ToString("0.0"));
input = input.Replace("{curr}", (MAV.cs.current).ToString("0.0"));
input = input.Replace("{hdop}", (MAV.cs.gpshdop).ToString("0.00"));
input = input.Replace("{satcount}", (MAV.cs.satcount).ToString("0"));
input = input.Replace("{rssi}", (MAV.cs.rssi).ToString("0"));
input = input.Replace("{disthome}", (MAV.cs.DistToHome).ToString("0"));
input = input.Replace("{timeinair}",
(new TimeSpan(0, 0, 0, (int)MAV.cs.timeInAir)).ToString());
try
{
object thisBoxed = MAV.cs;
Type test = thisBoxed.GetType();
PropertyInfo[] props = test.GetProperties();
//props
foreach (var field in props)
{
// field.Name has the field's name.
object fieldValue;
TypeCode typeCode;
try
{
fieldValue = field.GetValue(thisBoxed, null); // Get value
if (fieldValue == null)
{
continue;
}
// Get the TypeCode enumeration. Multiple types get mapped to a common typecode.
typeCode = Type.GetTypeCode(fieldValue.GetType());
}
catch
{
continue;
}
var fname = String.Format("{{{0}}}", field.Name);
input = input.Replace(fname, fieldValue.ToString());
}
}
catch
{
}
return(input);
}
public void AddHiddenList(byte sysid, byte compid)
{
int id = (byte)sysid * 256 + (byte)compid;
hiddenlist[id] = new MAVState() { sysid = sysid, compid = compid };
}
public void setOffsets(MAVState mav, double x, double y, double z)
{
offsets[mav] = new HIL.Vector3(x, y, z);
log.Info(mav.ToString() + " " + offsets[mav].ToString());
}
internal void Create(byte sysid, byte compid)
{
int id = (byte) sysid*256 + (byte) compid;
// move from hidden to visible
if (hiddenlist.ContainsKey(id))
{
masterlist[id] = hiddenlist[id];
hiddenlist.Remove(id);
}
if (!masterlist.ContainsKey(id))
masterlist[id] = new MAVState();
}
private void grid_UpdateOffsets(MAVState mav, float x, float y, float z, Grid.icon ico)
{
workingLayout.AddOffset(mav.sysid, new Vector3(x, y, z));
}
float testCode(MAVState MAV, PointLatLngAlt Location)
{
// this is GeoFenceDist from currentstate
try
{
float disttotal = 99999;
var R = 6371e3;
var currenthash = MAV.fencepoints.GetHashCode();
lock (this)
if (currenthash != listhash)
{
list = MAV.fencepoints
.Where(a => a.Value.command != (ushort)MAVLink.MAV_CMD.FENCE_RETURN_POINT)
.ChunkByField((a, b, count) =>
{
// these fields types stand alone
if (a.Value.command == (ushort)MAVLink.MAV_CMD.FENCE_CIRCLE_EXCLUSION ||
a.Value.command == (ushort)MAVLink.MAV_CMD.FENCE_CIRCLE_INCLUSION)
{
return(false);
}
if (count >= b.Value.param1)
{
return(false);
}
return(a.Value.command == b.Value.command);
}).ToList();
listhash = currenthash;
}
// check all sublists
foreach (var sublist in list)
{
// process circles
if (sublist.Count() == 1)
{
var item = sublist.First().Value;
if (item.command == (ushort)MAVLink.MAV_CMD.FENCE_CIRCLE_EXCLUSION)
{
var lla = new PointLatLngAlt(item.x / 1e7,
item.y / 1e7);
var dist = lla.GetDistance(Location);
if (dist < item.param1)
{
return(0);
}
disttotal = (float)Math.Min(dist - item.param1, disttotal);
}
else if (item.command == (ushort)MAVLink.MAV_CMD.FENCE_CIRCLE_INCLUSION)
{
var lla = new PointLatLngAlt(item.x / 1e7,
item.y / 1e7);
var dist = lla.GetDistance(Location);
if (dist > item.param1)
{
return(0);
}
disttotal = (float)Math.Min(item.param1 - dist, disttotal);
}
}
if (sublist == null || sublist.Count() < 3)
{
continue;
}
if (PolygonTools.isInside(
sublist.CloseLoop().Select(a => new PolygonTools.Point(a.Value.y / 1e7, a.Value.x / 1e7)).ToList(),
new PolygonTools.Point(Location.Lng, Location.Lat)))
{
if (sublist.First().Value.command == (ushort)MAVLink.MAV_CMD.FENCE_POLYGON_VERTEX_EXCLUSION)
{
return(0);
}
}
else
{
if (sublist.First().Value.command == (ushort)MAVLink.MAV_CMD.FENCE_POLYGON_VERTEX_INCLUSION)
{
return(0);
}
}
PointLatLngAlt lineStartLatLngAlt = null;
// check all segments
foreach (var mavlinkFencePointT in sublist.CloseLoop())
{
if (lineStartLatLngAlt == null)
{
lineStartLatLngAlt = new PointLatLngAlt(mavlinkFencePointT.Value.x / 1e7,
mavlinkFencePointT.Value.y / 1e7);
continue;
}
// crosstrack distance
var lineEndLatLngAlt = new PointLatLngAlt(mavlinkFencePointT.Value.x / 1e7,
mavlinkFencePointT.Value.y / 1e7);
var lineDist = lineStartLatLngAlt.GetDistance2(lineEndLatLngAlt);
var distToLocation = lineStartLatLngAlt.GetDistance2(Location);
var bearToLocation = lineStartLatLngAlt.GetBearing(Location);
var lineBear = lineStartLatLngAlt.GetBearing(lineEndLatLngAlt);
var angle = bearToLocation - lineBear;
if (angle < 0)
{
angle += 360;
}
var alongline = Math.Cos(angle * MathHelper.deg2rad) * distToLocation;
// check to see if our point is still within the line length
if (alongline < 0 || alongline > lineDist)
{
lineStartLatLngAlt = lineEndLatLngAlt;
continue;
}
var dXt2 = Math.Sin(angle * MathHelper.deg2rad) * distToLocation;
var dXt = Math.Asin(Math.Sin(distToLocation / R) * Math.Sin(angle * MathHelper.deg2rad)) * R;
disttotal = (float)Math.Min(disttotal, Math.Abs(dXt2));
lineStartLatLngAlt = lineEndLatLngAlt;
}
}
// check also distance from the points - because if we are outside the polygon, we may be on a corner segment
foreach (var sublist in list)
{
foreach (var mavlinkFencePointT in sublist)
{
if (mavlinkFencePointT.Value.command == (ushort)MAVLink.MAV_CMD.FENCE_CIRCLE_INCLUSION)
{
continue;
}
var pathpoint = new PointLatLngAlt(mavlinkFencePointT.Value.x / 1e7,
mavlinkFencePointT.Value.y / 1e7);
var dXt2 = pathpoint.GetDistance(Location);
disttotal = (float)Math.Min(disttotal, Math.Abs(dXt2));
}
}
return(disttotal);
}
catch
{
return(0);
}
}
public static string speechConversion(MAVState MAV, string input)
{
if (MAV.cs.wpno == 0)
{
input = input.Replace("{wpn}", "Home");
}
else
{
input = input.Replace("{wpn}", MAV.cs.wpno.ToString());
}
input = input.Replace("{sysid}", MAV.sysid.ToString("0"));
input = input.Replace("{compid}", MAV.compid.ToString("0"));
input = input.Replace("{asp}", MAV.cs.airspeed.ToString("0"));
input = input.Replace("{alt}", MAV.cs.alt.ToString("0"));
input = input.Replace("{wpa}", MAV.cs.targetalt.ToString("0"));
input = input.Replace("{gsp}", MAV.cs.groundspeed.ToString("0"));
input = input.Replace("{mode}", MAV.cs.mode.ToString());
input = input.Replace("{batv}", MAV.cs.battery_voltage.ToString("0.00", CultureInfo.InstalledUICulture));
input = input.Replace("{batp}", (MAV.cs.battery_remaining).ToString("0"));
input = input.Replace("{vsp}", (MAV.cs.verticalspeed).ToString("0.0", CultureInfo.InstalledUICulture));
input = input.Replace("{curr}", (MAV.cs.current).ToString("0.0", CultureInfo.InstalledUICulture));
input = input.Replace("{hdop}", (MAV.cs.gpshdop).ToString("0.00"));
input = input.Replace("{satcount}", (MAV.cs.satcount).ToString("0"));
input = input.Replace("{rssi}", (MAV.cs.rssi).ToString("0"));
input = input.Replace("{disthome}", (MAV.cs.DistToHome).ToString("0"));
input = input.Replace("{timeinair}",
(new TimeSpan(0, 0, 0, (int)MAV.cs.timeInAir)).ToString());
input = input.Replace("{altunit}", CurrentState.AltUnit);
input = input.Replace("{speedunit}", CurrentState.SpeedUnit);
try
{
object thisBoxed = MAV.cs;
Type test = thisBoxed.GetType();
PropertyInfo[] props =
test.GetProperties(BindingFlags.Instance | BindingFlags.Static | BindingFlags.Public);
var posible = input.Split(new string[] { "{", "}" }, StringSplitOptions.RemoveEmptyEntries);
//props
foreach (var field in props)
{
if (!posible.Contains(field.Name))
{
continue;
}
// field.Name has the field's name.
object fieldValue;
TypeCode typeCode;
try
{
fieldValue = field.GetValue(thisBoxed, null); // Get value
if (fieldValue == null)
{
continue;
}
// Get the TypeCode enumeration. Multiple types get mapped to a common typecode.
typeCode = Type.GetTypeCode(fieldValue.GetType());
}
catch
{
continue;
}
var fname = String.Format("{{{0}}}", field.Name);
if (typeCode == TypeCode.Double)
{
input = input.Replace(fname, ((double)fieldValue).ToString("F1"));
}
else if (typeCode == TypeCode.Single)
{
input = input.Replace(fname, ((float)fieldValue).ToString("F1"));
}
else
{
input = input.Replace(fname, fieldValue.ToInvariantString());
}
}
}
catch
{
}
return(input);
}
//public void theout(object source, System.Timers.ElapsedEventArgs e)
//{
// swpc.Enable_Draw = true;
// // label14.Text = swpc.realRouteOverlay.Polygons.Count.ToString();
//}
private void Dv1_linkEvent(MAVState MAV, bool IsCome, string Ip)
{
if (IsCome)
{
if (Ip != dv1.ComPort)
{
return;
}
}
else
{
if (Ip != dv1.IP)
{
return;
}
}
flightdata1.pitchAndBank1.Bank = MAV.cs.roll;
flightdata1.pitchAndBank1.Pitch = -MAV.cs.pitch;
flightdata1.airSpeedIndicator1.AirSpeed = (int)MAV.cs.groundspeed; //地速airspeed 空速
flightdata1.altitudeMeter1.Altitude = MAV.cs.alt; //高度
flightdata1.LinkqualityGcsLab.Text = MAV.cs.linkqualitygcs.ToString() + "%"; //显示数传连接质量
flightdata1.lab_alt.Text = MAV.cs.alt.ToString("0.##"); //高度
flightdata1.lab_gs.Text = MAV.cs.groundspeed.ToString("0.##"); //地速
flightdata1.lab_wd.Text = MAV.cs.wp_dist.ToString("0.##"); //航点距离
flightdata1.lab_yaw.Text = MAV.cs.yaw.ToString("0.##"); //偏航
flightdata1.label33.Text = MAV.cs.mode;
flightdata1.Gps_nmulabel.Text = MAV.cs.satcount.ToString(); //显示GPS卫星数量
flightdata1.Hdop_label.Text = MAV.cs.gpshdop.ToString(); //显示GPS水平定位因子
if (dv1.comPort.isconnect)
{
//Connect_button.BackgroundImage = global::Amov.Planner.Properties.Resources.链接;
}
if (MAV.cs.gpshdop <= 3)//如果定位因子小于3，载具才会在GPS上显示
{
//flightplanner1.gMapControl.Position = new PointLatLng(MAV.cs.lat, MAV.cs.lng);
//if(vehiVleMarker)
flightplanner1.swpcp.addvehiclemarker(MAV.cs.lat, MAV.cs.lng, MAV.cs.yaw);//在地图上显示载具
flightdata1.gMap.Position = new PointLatLng(MAV.cs.lat, MAV.cs.lng);
//if(vehiVleMarker)
flightdata1.swpcd.addvehiclemarker(MAV.cs.lat, MAV.cs.lng, MAV.cs.yaw); //在地图上显示载具
}
if (massag_error != MAV.cs.messageHigh) //如果这次的报错和上次的不一样，就更新本次的报错内容，防止textbox重复循环打印报错
{
Error_box.AppendText("错误/警告:" + dv1.comPort.MAV.cs.messageHigh + "\r\n");
}
massag_error = MAV.cs.messageHigh;
if (MAV.cs.gpshdop < 3 && MAV.cs.satcount > 6)//GPS信号比较良好的情况下实时显示载具轨迹
{
// swpc.DrawWaypoint(flightdata1.Position, swpc.realRouteOverlay);
}
if (MAV.cs.armed == true)
{
Arm_button.BackgroundImage = global::Amov.Planner.Properties.Resources._7_1已解锁;
}
}
public Proximity(MAVState mavInt)
{
_parent = mavInt;
sub = mavInt.parent.SubscribeToPacketType(MAVLINK_MSG_ID.DISTANCE_SENSOR, messageReceived);
}
