Skip to content

Commit

Permalink
HUD/PrearmStatus: fix failure messages
Browse files Browse the repository at this point in the history
When prearm checks are failing, the HUD displays the last failure message,
even if that message is very stale. This changes it so it displays the
latest failure message since the last time it passed the prearm check. The
prearm failure messages were also moved to the lowest priority, so that they
would not mask other sensor failures.

The PrearmStatus window had a similar problem, where it would show all
failure messages briefly when it first launched, instead of the latest
relevant ones.

Also, a drive-by fix of where the messageHigh setter function sets the
_messageHighTime variable, which was causing flickers.
  • Loading branch information
robertlong13 committed Mar 5, 2024
1 parent 065f88a commit 60c19e5
Show file tree
Hide file tree
Showing 2 changed files with 16 additions and 9 deletions.
4 changes: 2 additions & 2 deletions Controls/PrearmStatus.cs
Original file line number Diff line number Diff line change
Expand Up @@ -16,8 +16,8 @@ public partial class PrearmStatus : Form
{
private static readonly ILog log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);

private DateTime lastRequestTime = DateTime.MinValue;
private DateTime searchTime = DateTime.MinValue;
private DateTime lastRequestTime = DateTime.MaxValue;
private DateTime searchTime = DateTime.MaxValue;
public PrearmStatus()
{
InitializeComponent();
Expand Down
21 changes: 14 additions & 7 deletions ExtLibs/ArduPilot/CurrentState.cs
Original file line number Diff line number Diff line change
Expand Up @@ -174,6 +174,8 @@ public bool prearmstatus
get => connected && (sensors_health.prearm || !sensors_enabled.prearm);
}

private DateTime last_good_prearm = DateTime.MaxValue;

private bool useLocation;

/// <summary>
Expand Down Expand Up @@ -1105,10 +1107,10 @@ public string messageHigh
if (value == null || value == "")
return;
// check against get
_messageHighTime = DateTime.Now;
if (messageHigh == value)
return;
log.Info("messageHigh " + value);
_messageHighTime = DateTime.Now;
_messagehigh = value;
messageHighSeverity = MAVLink.MAV_SEVERITY.EMERGENCY;
}
Expand Down Expand Up @@ -2687,12 +2689,7 @@ private void Parent_OnPacketReceived(object sender, MAVLink.MAVLinkMessage mavLi
messageHigh = "InternalError 0x" + (errors_count1 + (errors_count2 << 16)).ToString("X");
}

if (!sensors_health.prearm && sensors_enabled.prearm && sensors_present.prearm)
{
messageHigh = messages.LastOrDefault(a => a.message.ToLower().Contains("prearm")).message
?.ToString();
}
else if (!sensors_health.gps && sensors_enabled.gps && sensors_present.gps)
if (!sensors_health.gps && sensors_enabled.gps && sensors_present.gps)
{
messageHigh = Strings.BadGPSHealth;
}
Expand Down Expand Up @@ -2768,6 +2765,16 @@ private void Parent_OnPacketReceived(object sender, MAVLink.MAVLinkMessage mavLi
{
messageHigh = Strings.BadAirspeed;
}
else if (!sensors_health.prearm && sensors_enabled.prearm && sensors_present.prearm)
{
messageHigh = messages.LastOrDefault(a =>
a.message.ToLower().Contains("prearm") &&
a.time > last_good_prearm).message?.ToString();
}
if (last_good_prearm > DateTime.Now || sensors_health.prearm || !sensors_enabled.prearm || !sensors_present.prearm)
{
last_good_prearm = DateTime.Now;
}
}

break;
Expand Down

0 comments on commit 60c19e5

Please sign in to comment.