Merge pull request #13 from Rouniy/fix/upstream-safety-reliability

Port upstream safety and reliability fixes

Author: Rouniy

ExtLibs/ArduPilot/CurrentState.cs

@@ -181,6 +181,8 @@ public bool prearmstatus
             get => connected && (sensors_health.prearm || !sensors_enabled.prearm);
         }
 
+        private readonly PrearmFailureTracker _prearmFailureTracker = new PrearmFailureTracker();
+
         private bool useLocation;
 
         /// <summary>
@@ -1274,10 +1276,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;
             }
@@ -2967,16 +2969,17 @@ private void Parent_OnPacketReceived(object sender, MAVLink.MAVLinkMessage mavLi
 
                             safetyactive = !sensors_enabled.motor_control;
 
+                            string latestPrearmFailure = _prearmFailureTracker.Update(
+                                sensors_health.prearm,
+                                sensors_enabled.prearm,
+                                sensors_present.prearm,
+                                messages,
+                                DateTime.Now);
+
                             if (errors_count1 > 0 || errors_count2 > 0)
                             {
                                 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)
                             {
                                 messageHigh = Strings.BadGPSHealth;
@@ -3053,6 +3056,10 @@ private void Parent_OnPacketReceived(object sender, MAVLink.MAVLinkMessage mavLi
                             {
                                 messageHigh = Strings.BadAirspeed;
                             }
+                            else if (!string.IsNullOrEmpty(latestPrearmFailure))
+                            {
+                                messageHigh = latestPrearmFailure;
+                            }
                         }
 
                         break;
@@ -4056,11 +4063,11 @@ private void Parent_OnPacketReceived(object sender, MAVLink.MAVLinkMessage mavLi
                     case (uint)MAVLink.MAVLINK_MSG_ID.HIGHRES_IMU:
                         {
                             const ushort HIGHRES_IMU_UPDATED_XACC = 0x01;
-                            const ushort HIGHRES_IMU_UPDATED_XGYRO = 0x08;
-                            const ushort HIGHRES_IMU_UPDATED_XMAG = 0x40;
-                            const ushort HIGHRES_IMU_UPDATED_ABS_PRESSURE = 0x200;
-                            const ushort HIGHRES_IMU_UPDATED_PRESSURE_ALT = 0x800;
-                            const ushort HIGHRES_IMU_UPDATED_TEMPERATURE = 0x1000;
+                            const ushort HIGHRES_IMU_UPDATED_XGYRO = 0x08;
+                            const ushort HIGHRES_IMU_UPDATED_XMAG = 0x40;
+                            const ushort HIGHRES_IMU_UPDATED_ABS_PRESSURE = 0x200;
+                            const ushort HIGHRES_IMU_UPDATED_PRESSURE_ALT = 0x800;
+                            const ushort HIGHRES_IMU_UPDATED_TEMPERATURE = 0x1000;
 
                             var imu = mavLinkMessage.ToStructure<MAVLink.mavlink_highres_imu_t>();
                             if (imu.id == 0)
@@ -4643,8 +4650,8 @@ public void UpdateCurrentSettings(Action<CurrentState> bs, bool updatenow,
                             mavinterface.requestDatastream(MAVLink.MAV_DATA_STREAM.RC_CHANNELS, MAV.cs.raterc,
                                 MAV.sysid,
                                 MAV.compid); // request rc info
-                            MAV.Camera?.RequestMessageIntervals(MAV.cs.ratestatus); // use ratestatus until we create a new setting for this
-                            MAV.GimbalManager?.Discover();
+                            // Use ratestatus until camera-specific configuration is available.
+                            MAV.Camera?.UpdateRateIfChanged(MAV.cs.ratestatus);
                         }
                         catch
                         {