diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 332c4acff5..8ff3b47458 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/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; } diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index a0ac6373ec..6445df7b88 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/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 */ diff --git a/libraries/GCS_MAVLink/GCS_Dummy.h b/libraries/GCS_MAVLink/GCS_Dummy.h index a6e3334294..a33dfe11cb 100644 --- a/libraries/GCS_MAVLink/GCS_Dummy.h +++ b/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; }