diff --git a/libraries/AP_Baro/AP_Baro.cpp b/libraries/AP_Baro/AP_Baro.cpp index 2ad6ad1ab8..2777f10857 100644 --- a/libraries/AP_Baro/AP_Baro.cpp +++ b/libraries/AP_Baro/AP_Baro.cpp @@ -172,6 +172,31 @@ const AP_Param::GroupInfo AP_Baro::var_info[] = { AP_GROUPINFO("PROBE_EXT", 14, AP_Baro, _baro_probe_ext, HAL_BARO_PROBE_EXT_DEFAULT), #endif + // @Param: BARO_ID + // @DisplayName: Baro ID + // @Description: Barometer sensor ID, taking into account its type, bus and instance + // @ReadOnly: True + // @User: Advanced + AP_GROUPINFO("BARO_ID", 15, AP_Baro, sensors[0].bus_id, 0), + +#if BARO_MAX_INSTANCES > 1 + // @Param: BARO2_ID + // @DisplayName: Baro ID2 + // @Description: Barometer2 sensor ID, taking into account its type, bus and instance + // @ReadOnly: True + // @User: Advanced + AP_GROUPINFO("BARO2_ID", 16, AP_Baro, sensors[1].bus_id, 0), +#endif + +#if BARO_MAX_INSTANCES > 2 + // @Param: BARO3_ID + // @DisplayName: Baro ID3 + // @Description: Barometer3 sensor ID, taking into account its type, bus and instance + // @ReadOnly: True + // @User: Advanced + AP_GROUPINFO("BARO3_ID", 17, AP_Baro, sensors[2].bus_id, 0), +#endif + AP_GROUPEND }; @@ -474,6 +499,11 @@ void AP_Baro::init(void) _user_ground_temperature.notify(); } + // zero bus IDs before probing + for (uint8_t i = 0; i < BARO_MAX_INSTANCES; i++) { + sensors[i].bus_id.set(0); + } + if (_hil_mode) { drivers[0] = new AP_Baro_HIL(*this); _num_drivers = 1; diff --git a/libraries/AP_Baro/AP_Baro.h b/libraries/AP_Baro/AP_Baro.h index bf30d78968..2f694f107d 100644 --- a/libraries/AP_Baro/AP_Baro.h +++ b/libraries/AP_Baro/AP_Baro.h @@ -230,6 +230,7 @@ private: bool healthy; // true if sensor is healthy bool alt_ok; // true if calculated altitude is ok bool calibrated; // true if calculated calibrated successfully + AP_Int32 bus_id; } sensors[BARO_MAX_INSTANCES]; AP_Float _alt_offset; diff --git a/libraries/AP_Baro/AP_Baro_BMP085.cpp b/libraries/AP_Baro/AP_Baro_BMP085.cpp index f9eefa1c1d..b45c04cdf2 100644 --- a/libraries/AP_Baro/AP_Baro_BMP085.cpp +++ b/libraries/AP_Baro/AP_Baro_BMP085.cpp @@ -137,6 +137,9 @@ bool AP_Baro_BMP085::_init() _instance = _frontend.register_sensor(); + _dev->set_device_type(DEVTYPE_BARO_BMP085); + set_bus_id(_instance, _dev->get_bus_id()); + _dev->register_periodic_callback(20000, FUNCTOR_BIND_MEMBER(&AP_Baro_BMP085::_timer, void)); return true; } diff --git a/libraries/AP_Baro/AP_Baro_BMP280.cpp b/libraries/AP_Baro/AP_Baro_BMP280.cpp index f668e12e41..ff533f348a 100644 --- a/libraries/AP_Baro/AP_Baro_BMP280.cpp +++ b/libraries/AP_Baro/AP_Baro_BMP280.cpp @@ -115,6 +115,9 @@ bool AP_Baro_BMP280::_init() _instance = _frontend.register_sensor(); + _dev->set_device_type(DEVTYPE_BARO_BMP280); + set_bus_id(_instance, _dev->get_bus_id()); + // request 50Hz update _dev->register_periodic_callback(20 * AP_USEC_PER_MSEC, FUNCTOR_BIND_MEMBER(&AP_Baro_BMP280::_timer, void)); diff --git a/libraries/AP_Baro/AP_Baro_BMP388.cpp b/libraries/AP_Baro/AP_Baro_BMP388.cpp index ab2315bf0f..83942e07ea 100644 --- a/libraries/AP_Baro/AP_Baro_BMP388.cpp +++ b/libraries/AP_Baro/AP_Baro_BMP388.cpp @@ -108,6 +108,9 @@ bool AP_Baro_BMP388::init() instance = _frontend.register_sensor(); + dev->set_device_type(DEVTYPE_BARO_BMP388); + set_bus_id(instance, dev->get_bus_id()); + // request 50Hz update dev->register_periodic_callback(20 * AP_USEC_PER_MSEC, FUNCTOR_BIND_MEMBER(&AP_Baro_BMP388::timer, void)); diff --git a/libraries/AP_Baro/AP_Baro_Backend.h b/libraries/AP_Baro/AP_Baro_Backend.h index 7ae28dc90d..c35c8757a9 100644 --- a/libraries/AP_Baro/AP_Baro_Backend.h +++ b/libraries/AP_Baro/AP_Baro_Backend.h @@ -23,6 +23,28 @@ public: // If the value further that filtrer_range from mean value, it is rejected. bool pressure_ok(float press); uint32_t get_error_count() const { return _error_count; } + + /* + device driver IDs. These are used to fill in the devtype field + of the device ID, which shows up as GND_BARO_ID* parameters to + users. + */ + enum DevTypes { + DEVTYPE_BARO_SITL = 0x01, + DEVTYPE_BARO_BMP085 = 0x02, + DEVTYPE_BARO_BMP280 = 0x03, + DEVTYPE_BARO_BMP388 = 0x04, + DEVTYPE_BARO_DPS280 = 0x05, + DEVTYPE_BARO_DPS310 = 0x06, + DEVTYPE_BARO_FBM320 = 0x07, + DEVTYPE_BARO_ICM20789 = 0x08, + DEVTYPE_BARO_KELLERLD = 0x09, + DEVTYPE_BARO_LPS2XH = 0x0A, + DEVTYPE_BARO_MS5611 = 0x0B, + DEVTYPE_BARO_SPL06 = 0x0C, + DEVTYPE_BARO_UAVCAN = 0x0D, + }; + protected: // reference to frontend object AP_Baro &_frontend; @@ -38,4 +60,9 @@ protected: float _mean_pressure; // number of dropped samples. Not used for now, but can be usable to choose more reliable sensor uint32_t _error_count; + + // set bus ID of this instance, for GND_BARO_ID parameters + void set_bus_id(uint8_t instance, uint32_t id) { + _frontend.sensors[instance].bus_id.set(int32_t(id)); + } }; diff --git a/libraries/AP_Baro/AP_Baro_DPS280.cpp b/libraries/AP_Baro/AP_Baro_DPS280.cpp index af7151a702..8af43081f2 100644 --- a/libraries/AP_Baro/AP_Baro_DPS280.cpp +++ b/libraries/AP_Baro/AP_Baro_DPS280.cpp @@ -163,6 +163,9 @@ bool AP_Baro_DPS280::init() instance = _frontend.register_sensor(); + dev->set_device_type(DEVTYPE_BARO_DPS280); + set_bus_id(instance, dev->get_bus_id()); + dev->get_semaphore()->give(); // request 64Hz update. New data will be available at 32Hz diff --git a/libraries/AP_Baro/AP_Baro_FBM320.cpp b/libraries/AP_Baro/AP_Baro_FBM320.cpp index 2b20073a29..33fee17a14 100644 --- a/libraries/AP_Baro/AP_Baro_FBM320.cpp +++ b/libraries/AP_Baro/AP_Baro_FBM320.cpp @@ -125,6 +125,9 @@ bool AP_Baro_FBM320::init() instance = _frontend.register_sensor(); + dev->set_device_type(DEVTYPE_BARO_FBM320); + set_bus_id(instance, dev->get_bus_id()); + dev->get_semaphore()->give(); // request 50Hz update diff --git a/libraries/AP_Baro/AP_Baro_ICM20789.cpp b/libraries/AP_Baro/AP_Baro_ICM20789.cpp index b772b3000b..740952929c 100644 --- a/libraries/AP_Baro/AP_Baro_ICM20789.cpp +++ b/libraries/AP_Baro/AP_Baro_ICM20789.cpp @@ -206,6 +206,9 @@ bool AP_Baro_ICM20789::init() instance = _frontend.register_sensor(); + dev->set_device_type(DEVTYPE_BARO_ICM20789); + set_bus_id(instance, dev->get_bus_id()); + dev->get_semaphore()->give(); debug("ICM20789: startup OK\n"); diff --git a/libraries/AP_Baro/AP_Baro_KellerLD.cpp b/libraries/AP_Baro/AP_Baro_KellerLD.cpp index 3baed90b9d..3bf23ec012 100644 --- a/libraries/AP_Baro/AP_Baro_KellerLD.cpp +++ b/libraries/AP_Baro/AP_Baro_KellerLD.cpp @@ -142,6 +142,9 @@ bool AP_Baro_KellerLD::_init() _instance = _frontend.register_sensor(); + _dev->set_device_type(DEVTYPE_BARO_KELLERLD); + set_bus_id(_instance, _dev->get_bus_id()); + _frontend.set_type(_instance, AP_Baro::BARO_TYPE_WATER); // lower retries for run diff --git a/libraries/AP_Baro/AP_Baro_LPS2XH.cpp b/libraries/AP_Baro/AP_Baro_LPS2XH.cpp index 6a3f558e97..f43b51eea6 100644 --- a/libraries/AP_Baro/AP_Baro_LPS2XH.cpp +++ b/libraries/AP_Baro/AP_Baro_LPS2XH.cpp @@ -179,6 +179,9 @@ bool AP_Baro_LPS2XH::_init() _instance = _frontend.register_sensor(); + _dev->set_device_type(DEVTYPE_BARO_LPS2XH); + set_bus_id(_instance, _dev->get_bus_id()); + _dev->get_semaphore()->give(); _dev->register_periodic_callback(CallTime, FUNCTOR_BIND_MEMBER(&AP_Baro_LPS2XH::_timer, void)); diff --git a/libraries/AP_Baro/AP_Baro_MS5611.cpp b/libraries/AP_Baro/AP_Baro_MS5611.cpp index 172fdde8f3..a659c85e61 100644 --- a/libraries/AP_Baro/AP_Baro_MS5611.cpp +++ b/libraries/AP_Baro/AP_Baro_MS5611.cpp @@ -134,6 +134,9 @@ bool AP_Baro_MS56XX::_init() _instance = _frontend.register_sensor(); + _dev->set_device_type(DEVTYPE_BARO_MS5611); + set_bus_id(_instance, _dev->get_bus_id()); + if (_ms56xx_type == BARO_MS5837) { _frontend.set_type(_instance, AP_Baro::BARO_TYPE_WATER); } diff --git a/libraries/AP_Baro/AP_Baro_SITL.cpp b/libraries/AP_Baro/AP_Baro_SITL.cpp index f8bc510261..ba43a3694e 100644 --- a/libraries/AP_Baro/AP_Baro_SITL.cpp +++ b/libraries/AP_Baro/AP_Baro_SITL.cpp @@ -20,6 +20,7 @@ AP_Baro_SITL::AP_Baro_SITL(AP_Baro &baro) : #if APM_BUILD_TYPE(APM_BUILD_ArduSub) _frontend.set_type(_instance, AP_Baro::BARO_TYPE_WATER); #endif + set_bus_id(_instance, AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_SITL, 0, _instance, DEVTYPE_BARO_SITL)); hal.scheduler->register_timer_process(FUNCTOR_BIND(this, &AP_Baro_SITL::_timer, void)); } } diff --git a/libraries/AP_Baro/AP_Baro_SPL06.cpp b/libraries/AP_Baro/AP_Baro_SPL06.cpp index b71f397979..f0c3573feb 100644 --- a/libraries/AP_Baro/AP_Baro_SPL06.cpp +++ b/libraries/AP_Baro/AP_Baro_SPL06.cpp @@ -140,6 +140,9 @@ bool AP_Baro_SPL06::_init() _instance = _frontend.register_sensor(); + _dev->set_device_type(DEVTYPE_BARO_SPL06); + set_bus_id(_instance, _dev->get_bus_id()); + // request 50Hz update _timer_counter = -1; _dev->register_periodic_callback(20 * AP_USEC_PER_MSEC, FUNCTOR_BIND_MEMBER(&AP_Baro_SPL06::_timer, void)); diff --git a/libraries/AP_Baro/AP_Baro_UAVCAN.cpp b/libraries/AP_Baro/AP_Baro_UAVCAN.cpp index 966ffab44d..6d02ce0f23 100644 --- a/libraries/AP_Baro/AP_Baro_UAVCAN.cpp +++ b/libraries/AP_Baro/AP_Baro_UAVCAN.cpp @@ -75,7 +75,12 @@ AP_Baro_Backend* AP_Baro_UAVCAN::probe(AP_Baro &baro) backend->_pressure_count = 0; backend->_ap_uavcan = _detected_modules[i].ap_uavcan; backend->_node_id = _detected_modules[i].node_id; - backend->register_sensor(); + + backend->_instance = backend->_frontend.register_sensor(); + backend->set_bus_id(backend->_instance, AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_UAVCAN, + _detected_modules[i].ap_uavcan->get_driver_index(), + backend->_node_id, 0)); + debug_baro_uavcan(2, _detected_modules[i].ap_uavcan->get_driver_index(), "Registered UAVCAN Baro Node %d on Bus %d\n", diff --git a/libraries/AP_Baro/AP_Baro_UAVCAN.h b/libraries/AP_Baro/AP_Baro_UAVCAN.h index edc537b6c7..5b368f4585 100644 --- a/libraries/AP_Baro/AP_Baro_UAVCAN.h +++ b/libraries/AP_Baro/AP_Baro_UAVCAN.h @@ -13,10 +13,6 @@ public: void update() override; - inline void register_sensor() { - _instance = _frontend.register_sensor(); - } - static void subscribe_msgs(AP_UAVCAN* ap_uavcan); static AP_Baro_UAVCAN* get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t node_id, bool create_new); static AP_Baro_Backend* probe(AP_Baro &baro);