diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp index 3dfb879817..061f983def 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp @@ -68,6 +68,13 @@ const AP_Param::GroupInfo AP_ExternalAHRS::var_info[] = { // @User: Standard AP_GROUPINFO("_OPTIONS", 3, AP_ExternalAHRS, options, 0), + // @Param: _SENSORS + // @DisplayName: External AHRS sensors + // @Description: External AHRS sensors bitmask + // @Bitmask: 0:GPS,1:IMU,2:Baro,3:Compass + // @User: Advanced + AP_GROUPINFO("_SENSORS", 4, AP_ExternalAHRS, sensors, 0xF), + AP_GROUPEND }; @@ -101,9 +108,9 @@ bool AP_ExternalAHRS::enabled() const } // get serial port number for the uart, or -1 if not applicable -int8_t AP_ExternalAHRS::get_port(void) const +int8_t AP_ExternalAHRS::get_port(AvailableSensor sensor) const { - if (!backend) { + if (!backend || !has_sensor(sensor)) { return -1; } return backend->get_port(); diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h index 32b76701d7..bef32381c6 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h @@ -59,8 +59,15 @@ public: // Get model/type name const char* get_name() const; + enum class AvailableSensor { + GPS = (1U<<0), + IMU = (1U<<1), + BARO = (1U<<2), + COMPASS = (1U<<3), + }; + // get serial port number, -1 for not enabled - int8_t get_port(void) const; + int8_t get_port(AvailableSensor sensor) const; struct state_t { HAL_Semaphore sem; @@ -147,8 +154,14 @@ private: AP_Enum devtype; AP_Int16 rate; AP_Int16 options; + AP_Int16 sensors; static AP_ExternalAHRS *_singleton; + + // check if a sensor type is enabled + bool has_sensor(AvailableSensor sensor) const { + return (uint16_t(sensors.get()) & uint16_t(sensor)) != 0; + } }; namespace AP {