diff --git a/libraries/AP_Proximity/AP_Proximity.cpp b/libraries/AP_Proximity/AP_Proximity.cpp index d2b1765552..b91403d088 100644 --- a/libraries/AP_Proximity/AP_Proximity.cpp +++ b/libraries/AP_Proximity/AP_Proximity.cpp @@ -437,4 +437,17 @@ AP_Proximity::Proximity_Type AP_Proximity::get_type(uint8_t instance) const return Proximity_Type_None; } +bool AP_Proximity::sensor_present() const +{ + return get_status() != Proximity_NotConnected; +} +bool AP_Proximity::sensor_enabled() const +{ + return _type[primary_instance] != Proximity_Type_None; +} +bool AP_Proximity::sensor_failed() const +{ + return get_status() != Proximity_Good; +} + AP_Proximity *AP_Proximity::_singleton; diff --git a/libraries/AP_Proximity/AP_Proximity.h b/libraries/AP_Proximity/AP_Proximity.h index cd0540cfc0..5be475b6f6 100644 --- a/libraries/AP_Proximity/AP_Proximity.h +++ b/libraries/AP_Proximity/AP_Proximity.h @@ -134,6 +134,12 @@ public: static AP_Proximity *get_singleton(void) { return _singleton; }; + // methods for mavlink SYS_STATUS message (send_extended_status1) + // these methods cover only the primary instance + bool sensor_present() const; + bool sensor_enabled() const; + bool sensor_failed() const; + private: static AP_Proximity *_singleton; Proximity_State state[PROXIMITY_MAX_INSTANCES];