diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 18c4c7133f..0d9de7e201 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -301,7 +301,6 @@ protected: virtual MAV_MODE base_mode() const = 0; virtual uint32_t custom_mode() const = 0; virtual MAV_STATE system_status() const = 0; - virtual void get_sensor_status_flags(uint32_t &present, uint32_t &enabled, uint32_t &health) = 0; bool waypoint_receiving; // currently receiving // the following two variables are only here because of Tracker @@ -775,6 +774,8 @@ public: // update uart pass-thru void update_passthru(); + virtual void get_sensor_status_flags(uint32_t &present, uint32_t &enabled, uint32_t &health) = 0; + private: static GCS *_singleton; diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 4a91910a8e..f64ed6bd4b 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -3804,7 +3804,7 @@ void GCS_MAVLINK::send_sys_status() uint32_t control_sensors_enabled; uint32_t control_sensors_health; - get_sensor_status_flags(control_sensors_present, control_sensors_enabled, control_sensors_health); + gcs().get_sensor_status_flags(control_sensors_present, control_sensors_enabled, control_sensors_health); mavlink_msg_sys_status_send( chan, diff --git a/libraries/GCS_MAVLink/GCS_Dummy.h b/libraries/GCS_MAVLink/GCS_Dummy.h index 2ee10934cd..1a45d14763 100644 --- a/libraries/GCS_MAVLink/GCS_Dummy.h +++ b/libraries/GCS_MAVLink/GCS_Dummy.h @@ -32,7 +32,6 @@ protected: MAV_MODE base_mode() const override { return (MAV_MODE)MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; } uint32_t custom_mode() const override { return 3; } // magic number MAV_STATE system_status() const override { return MAV_STATE_CALIBRATING; } - void get_sensor_status_flags(uint32_t &present, uint32_t &enabled, uint32_t &health) override { present = 0; enabled = 0; health = 0; } bool set_home_to_current_location(bool lock) override { return false; } bool set_home(const Location& loc, bool lock) override { return false; } @@ -54,4 +53,6 @@ class GCS_Dummy : public GCS const GCS_MAVLINK_Dummy &chan(const uint8_t ofs) const override { return dummy_backend; }; void send_statustext(MAV_SEVERITY severity, uint8_t dest_bitmask, const char *text) { hal.console->printf("TOGCS: %s\n", text); } + + void get_sensor_status_flags(uint32_t &present, uint32_t &enabled, uint32_t &health) override { present = 0; enabled = 0; health = 0; } }; diff --git a/libraries/GCS_MAVLink/examples/routing/routing.cpp b/libraries/GCS_MAVLink/examples/routing/routing.cpp index 8a4f7b9f89..9d044f90fb 100644 --- a/libraries/GCS_MAVLink/examples/routing/routing.cpp +++ b/libraries/GCS_MAVLink/examples/routing/routing.cpp @@ -38,7 +38,6 @@ protected: MAV_MODE base_mode() const override { return (MAV_MODE)MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; } uint32_t custom_mode() const override { return 3; } // magic number MAV_STATE system_status() const override { return MAV_STATE_CALIBRATING; } - void get_sensor_status_flags(uint32_t &present, uint32_t &enabled, uint32_t &health) override { present = 0; enabled = 0; health = 0; } void send_nav_controller_output() const override {}; bool set_home_to_current_location(bool lock) override { return false; }