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 2815f62f85..d66191cfb3 100644 --- a/ExtLibs/ArduPilot/CurrentState.cs +++ b/ExtLibs/ArduPilot/CurrentState.cs @@ -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; /// @@ -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; } @@ -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; } @@ -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;