GCS_MAVLink: any internal error means MAV_STATE_CRITICAL

libraries/GCS_MAVLink/GCS.h

@@ -289,7 +289,8 @@ protected:
     void set_ekf_origin(const Location& loc);
 
     virtual MAV_MODE base_mode() const = 0;
-    virtual MAV_STATE system_status() const = 0;
+    MAV_STATE system_status() const;
+    virtual MAV_STATE vehicle_system_status() const = 0;
 
     virtual MAV_VTOL_STATE vtol_state() const { return MAV_VTOL_STATE_UNDEFINED; }
     virtual MAV_LANDED_STATE landed_state() const { return MAV_LANDED_STATE_UNDEFINED; }

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -2220,6 +2220,22 @@ void GCS_MAVLINK::send_gps_global_origin() const
         AP_HAL::micros64());
 }
 
+MAV_STATE GCS_MAVLINK::system_status() const
+{
+    MAV_STATE _system_status = vehicle_system_status();
+    if (_system_status < MAV_STATE_CRITICAL) {
+        // note that POWEROFF and FLIGHT_TERMINATION are both >
+        // CRITICAL, so we will not overwrite POWEROFF and
+        // FLIGHT_TERMINATION even if we have internal errors.  If new
+        // enum entries are added then this may also not overwrite
+        // those.
+        if (AP::internalerror().errors()) {
+            _system_status = MAV_STATE_CRITICAL;
+        }
+    }
+    return _system_status;
+}
+
 /*
   Send MAVLink heartbeat
  */

libraries/GCS_MAVLink/GCS_Dummy.h

@@ -41,7 +41,7 @@ protected:
 
     // dummy information:
     MAV_MODE base_mode() const override { return (MAV_MODE)MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; }
-    MAV_STATE system_status() const override { return MAV_STATE_CALIBRATING; }
+    MAV_STATE vehicle_system_status() const override { return MAV_STATE_CALIBRATING; }
 
     bool set_home_to_current_location(bool lock) override { return false; }
     bool set_home(const Location& loc, bool lock) override { return false; }