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 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); }