From 64e03a1d7d0bb8a57989f2d5fff3aac3ede6e0d5 Mon Sep 17 00:00:00 2001 From: Bob Long Date: Sat, 2 Dec 2023 09:56:26 +1100 Subject: [PATCH] HUD/PrearmStatus: fix failure messages 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. --- Controls/PrearmStatus.cs | 4 ++-- ExtLibs/ArduPilot/CurrentState.cs | 21 ++++++++++++++------- 2 files changed, 16 insertions(+), 9 deletions(-) diff --git a/Controls/PrearmStatus.cs b/Controls/PrearmStatus.cs index d47c1df4eb..1b2d315e61 100644 --- a/Controls/PrearmStatus.cs +++ b/Controls/PrearmStatus.cs @@ -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(); diff --git a/ExtLibs/ArduPilot/CurrentState.cs b/ExtLibs/ArduPilot/CurrentState.cs index b440ec8102..09c99fd0c5 100644 --- a/ExtLibs/ArduPilot/CurrentState.cs +++ b/ExtLibs/ArduPilot/CurrentState.cs @@ -179,6 +179,8 @@ public bool prearmstatus get => connected && (sensors_health.prearm || !sensors_enabled.prearm); } + private DateTime last_good_prearm = DateTime.MaxValue; + private bool useLocation; /// @@ -1228,10 +1230,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; } @@ -2886,12 +2888,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; } @@ -2967,6 +2964,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;