diff --git a/Controls/ProximityControl.cs b/Controls/ProximityControl.cs index 43cd008733..6a42f1471c 100644 --- a/Controls/ProximityControl.cs +++ b/Controls/ProximityControl.cs @@ -18,6 +18,7 @@ public class ProximityControl : Form MAVState _parent; private Proximity.directionState _dS => _parent?.Proximity?.DirectionState; + private bool _drawingInProgress => _parent?.Proximity?.DrawingInProgress ?? false; private Timer timer1; private IContainer components; @@ -90,6 +91,8 @@ private void Temp_KeyPress(object sender, KeyPressEventArgs e) private void Temp_Paint(object sender, PaintEventArgs e) { + if (_parent.Proximity != null) + _parent.Proximity.DrawingInProgress = true; e.Graphics.Clear(BackColor); var midx = e.ClipRectangle.Width / 2.0f; @@ -111,29 +114,33 @@ private void Temp_Paint(object sender, PaintEventArgs e) e.Graphics.DrawImage(Resources.quadicon, midx - imw, midy - imw, size, size); break; } - + if (_dS == null) + { + if (_parent.Proximity != null) + _parent.Proximity.DrawingInProgress = false; return; + } Pen redpen = new Pen(Color.Red, 3); Pen yellowpen = new Pen(Color.Yellow, 3); var font = new Font(SystemFonts.DefaultFont.FontFamily, SystemFonts.DefaultFont.Size + 2, FontStyle.Bold); - + float move = 5; - for (float x = 50f; x <= screenradius; x+=50f) + for (float x = 50f; x <= screenradius; x += 50f) { Vector3 location = new Vector3(0, (x) / scale, 0); var doublelength = location.length() * 2.0f; var length = location.length(); - e.Graphics.DrawArc(Pens.DimGray, (float) (midx - length), (float) (midy - length), - (float) doublelength, (float) doublelength, 0f, - (float) 360); + e.Graphics.DrawArc(Pens.DimGray, (float)(midx - length), (float)(midy - length), + (float)doublelength, (float)doublelength, 0f, + (float)360); e.Graphics.DrawString((x / 100).ToString("0.0m"), font, System.Drawing.Brushes.Green, midx - (float)location.x + move, midy - (float)location.y); } - for (float x = 0; x < 360; x+=45f) + for (float x = 0; x < 360; x += 45f) { Vector3 location = new Vector3(0, screenradius / scale, 0); var doublelength = location.length() * 2.0f; @@ -141,8 +148,8 @@ private void Temp_Paint(object sender, PaintEventArgs e) location.rotate(x); e.Graphics.DrawString((x).ToString("0"), font, System.Drawing.Brushes.DimGray, midx - (float)location.x - move * 2, midy - (float)location.y + move); - e.Graphics.DrawLine(Pens.DimGray, (float) (midx), (float) (midy), midx-(float)location.X, - midy-(float)location.Y); + e.Graphics.DrawLine(Pens.DimGray, (float)(midx), (float)(midy), midx - (float)location.X, + midy - (float)location.Y); } var rawdata = _dS.GetRaw(); @@ -199,12 +206,13 @@ private void Temp_Paint(object sender, PaintEventArgs e) case MAV_SENSOR_ORIENTATION.MAV_SENSOR_ROTATION_CUSTOM: location.rotate(temp.Angle); //e.Graphics.DrawString((temp.Distance / 100).ToString("0.0m"), font, System.Drawing.Brushes.Green, midx - (float)location.x + move, midy - (float)location.y); - e.Graphics.DrawArc(yellowpen, (float) (midx - length), (float) (midy - length), - (float) doublelength, (float) doublelength, (float)temp.Angle - ((float)temp.Size / 2.0f) - 90f, + e.Graphics.DrawArc(yellowpen, (float)(midx - length), (float)(midy - length), + (float)doublelength, (float)doublelength, (float)temp.Angle - ((float)temp.Size / 2.0f) - 90f, (float)temp.Size); break; } } + _parent.Proximity.DrawingInProgress = false; } public new void Show() diff --git a/ExtLibs/ArduPilot/Proximity.cs b/ExtLibs/ArduPilot/Proximity.cs index c9eeab63ad..81e2bc8f9d 100644 --- a/ExtLibs/ArduPilot/Proximity.cs +++ b/ExtLibs/ArduPilot/Proximity.cs @@ -24,8 +24,9 @@ public class Proximity : IDisposable private byte compid; public bool DataAvailable { get; set; } = false; + public bool DrawingInProgress { get; set; } = false; - public Proximity(MAVState mavInt, byte sysid, byte compid) + public Proximity(MAVState mavInt, byte sysid, byte compid) { this.sysid = sysid; this.compid = compid; @@ -47,6 +48,7 @@ public Proximity(MAVState mavInt, byte sysid, byte compid) private bool messageReceived(MAVLinkMessage arg) { + if (DrawingInProgress) return true; //accept any compid, but filter sysid if (arg.sysid != _parent.sysid) return true; @@ -64,7 +66,7 @@ private bool messageReceived(MAVLinkMessage arg) _dS.Add(dist.id, (MAV_SENSOR_ORIENTATION)dist.orientation, dist.current_distance, DateTime.Now, 3); DataAvailable = true; - } + } else if (arg.msgid == (uint) MAVLINK_MSG_ID.OBSTACLE_DISTANCE) { var dists = arg.ToStructure(); @@ -89,7 +91,7 @@ private bool messageReceived(MAVLinkMessage arg) if(dists.distances[a] == ushort.MaxValue) continue; if(dists.distances[a] > dists.max_distance) - continue; + continue; if(dists.distances[a] < dists.min_distance) continue; @@ -135,7 +137,7 @@ public data(uint id, MAV_SENSOR_ORIENTATION orientation, double distance, DateTi Distance = distance; Received = received; Age = age; - } + } public data(uint id, double angle, double size, double distance, DateTime received, double age = 1) { @@ -161,20 +163,23 @@ public void Add(uint id, MAV_SENSOR_ORIENTATION orientation, double distance, Da _dists.Add(new data(id, orientation, distance, received, age)); expire(); - } - + } + public void Add(uint id, double angle, double size, double distance, DateTime received, double age = 1) { - var existing = _dists.Where((a) => { return a.SensorId == id && a.Angle == angle; }); - - foreach (var item in existing.ToList()) + lock (this) { - _dists.Remove(item); - } + var existing = _dists.Where((a) => { return a.SensorId == id && a.Angle == angle; }); - _dists.Add(new data(id, angle, size, distance, received, age)); + foreach (var item in existing.ToList()) + { + _dists.Remove(item); + } - expire(); + _dists.Add(new data(id, angle, size, distance, received, age)); + + expire(); + } } /// @@ -230,7 +235,7 @@ void expire() { for (int a = 0; a < _dists.Count; a++) { - var expireat = _dists[a].ExpireTime; + var expireat = _dists[a]?.ExpireTime; if (expireat < DateTime.Now) {