Skip to content

Commit

Permalink
Fix ProximityUI concurrency issue
Browse files Browse the repository at this point in the history
  • Loading branch information
@EosBandi
EosBandi committed Oct 31, 2023
1 parent 3d4bc25 commit f2a9cf9
Show file tree
Hide file tree
Showing 2 changed files with 38 additions and 25 deletions.
30 changes: 19 additions & 11 deletions Controls/ProximityControl.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand All @@ -111,38 +114,42 @@ 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;
var length = location.length();

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();
Expand Down Expand Up @@ -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()
Expand Down
33 changes: 19 additions & 14 deletions ExtLibs/ArduPilot/Proximity.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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;
Expand All @@ -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<mavlink_obstacle_distance_t>();
Expand All @@ -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;

Expand Down Expand Up @@ -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)
{
Expand All @@ -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();
}
}

/// <summary>
Expand Down Expand Up @@ -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)
{
Expand Down

0 comments on commit f2a9cf9

Please sign in to comment.