From 5ef553737192fd4e0b75ca64d6ffdf95e7b48a53 Mon Sep 17 00:00:00 2001 From: Siddharth Purohit Date: Fri, 20 Jul 2018 15:14:55 +0530 Subject: [PATCH] AP_Baro: move Baro UAVCAN subscribers and handlers to AP_Baro_UAVCAN --- libraries/AP_Baro/AP_Baro.cpp | 45 +++--- libraries/AP_Baro/AP_Baro_Backend.h | 3 - libraries/AP_Baro/AP_Baro_UAVCAN.cpp | 198 +++++++++++++++++++-------- libraries/AP_Baro/AP_Baro_UAVCAN.h | 41 ++++-- 4 files changed, 193 insertions(+), 94 deletions(-) diff --git a/libraries/AP_Baro/AP_Baro.cpp b/libraries/AP_Baro/AP_Baro.cpp index 78718d9a6a..0e74799e1f 100644 --- a/libraries/AP_Baro/AP_Baro.cpp +++ b/libraries/AP_Baro/AP_Baro.cpp @@ -155,7 +155,7 @@ const AP_Param::GroupInfo AP_Baro::var_info[] = { // @User: Advanced AP_GROUPINFO("PROBE_EXT", 14, AP_Baro, _baro_probe_ext, HAL_BARO_PROBE_EXT_DEFAULT), #endif - + AP_GROUPEND }; @@ -168,7 +168,7 @@ AP_Baro *AP_Baro::_instance; AP_Baro::AP_Baro() { _instance = this; - + AP_Param::setup_object_defaults(this, var_info); } @@ -426,15 +426,12 @@ void AP_Baro::init(void) ADD_BACKEND(new AP_Baro_SITL(*this)); return; #endif - + #if HAL_WITH_UAVCAN - bool added; - do { - added = _add_backend(AP_Baro_UAVCAN::probe(*this)); - if (_num_drivers == BARO_MAX_DRIVERS || _num_sensors == BARO_MAX_INSTANCES) { - return; - } - } while (added); + // Detect UAVCAN Modules, try as many times as there are driver slots + for (uint8_t i = 0; i < BARO_MAX_DRIVERS; i++) { + ADD_BACKEND(AP_Baro_UAVCAN::probe(*this)); + } #endif #if AP_FEATURE_BOARD_DETECT @@ -472,7 +469,7 @@ void AP_Baro::init(void) ADD_BACKEND(AP_Baro_MS56XX::probe(*this, std::move(hal.spi->get_device(HAL_BARO_MS5611_NAME)))); break; - + case AP_BoardConfig::PX4_BOARD_AEROFC: #ifdef HAL_BARO_MS5607_I2C_BUS ADD_BACKEND(AP_Baro_MS56XX::probe(*this, @@ -512,7 +509,7 @@ void AP_Baro::init(void) ADD_BACKEND(AP_Baro_MS56XX::probe(*this, std::move(hal.spi->get_device(HAL_BARO_MS5611_NAME)))); break; - + default: break; } @@ -556,13 +553,13 @@ void AP_Baro::init(void) std::move(hal.i2c_mgr->get_device(HAL_BARO_LPS25H_I2C_BUS, HAL_BARO_LPS25H_I2C_ADDR)), HAL_BARO_LPS25H_I2C_IMU_ADDR)); #elif HAL_BARO_DEFAULT == HAL_BARO_20789_I2C_I2C - ADD_BACKEND(AP_Baro_ICM20789::probe(*this, - std::move(hal.i2c_mgr->get_device(HAL_BARO_20789_I2C_BUS, HAL_BARO_20789_I2C_ADDR_PRESS)), - std::move(hal.i2c_mgr->get_device(HAL_BARO_20789_I2C_BUS, HAL_BARO_20789_I2C_ADDR_ICM)))); + ADD_BACKEND(AP_Baro_ICM20789::probe(*this, + std::move(hal.i2c_mgr->get_device(HAL_BARO_20789_I2C_BUS, HAL_BARO_20789_I2C_ADDR_PRESS)), + std::move(hal.i2c_mgr->get_device(HAL_BARO_20789_I2C_BUS, HAL_BARO_20789_I2C_ADDR_ICM)))); #elif HAL_BARO_DEFAULT == HAL_BARO_20789_I2C_SPI - ADD_BACKEND(AP_Baro_ICM20789::probe(*this, - std::move(hal.i2c_mgr->get_device(HAL_BARO_20789_I2C_BUS, HAL_BARO_20789_I2C_ADDR_PRESS)), - std::move(hal.spi->get_device("icm20789")))); + ADD_BACKEND(AP_Baro_ICM20789::probe(*this, + std::move(hal.i2c_mgr->get_device(HAL_BARO_20789_I2C_BUS, HAL_BARO_20789_I2C_ADDR_PRESS)), + std::move(hal.spi->get_device("icm20789")))); #elif HAL_BARO_DEFAULT == HAL_BARO_LPS22H_SPI ADD_BACKEND(AP_Baro_LPS2XH::probe(*this, std::move(hal.spi->get_device(HAL_BARO_LPS22H_NAME)))); @@ -570,17 +567,17 @@ void AP_Baro::init(void) #if defined(BOARD_I2C_BUS_EXT) && CONFIG_HAL_BOARD == HAL_BOARD_F4LIGHT ADD_BACKEND(AP_Baro_MS56XX::probe(*this, - std::move(hal.i2c_mgr->get_device(BOARD_I2C_BUS_EXT, HAL_BARO_MS5611_I2C_ADDR)))); + std::move(hal.i2c_mgr->get_device(BOARD_I2C_BUS_EXT, HAL_BARO_MS5611_I2C_ADDR)))); ADD_BACKEND(AP_Baro_BMP280::probe(*this, - std::move(hal.i2c_mgr->get_device(BOARD_I2C_BUS_EXT, HAL_BARO_BMP280_I2C_ADDR)))); + std::move(hal.i2c_mgr->get_device(BOARD_I2C_BUS_EXT, HAL_BARO_BMP280_I2C_ADDR)))); #ifdef HAL_BARO_BMP280_I2C_ADDR_ALT ADD_BACKEND(AP_Baro_BMP280::probe(*this, - std::move(hal.i2c_mgr->get_device(BOARD_I2C_BUS_EXT, HAL_BARO_BMP280_I2C_ADDR_ALT)))); + std::move(hal.i2c_mgr->get_device(BOARD_I2C_BUS_EXT, HAL_BARO_BMP280_I2C_ADDR_ALT)))); #endif ADD_BACKEND(AP_Baro_BMP085::probe(*this, - std::move(hal.i2c_mgr->get_device(BOARD_I2C_BUS_EXT, HAL_BARO_BMP085_I2C_ADDR)))); + std::move(hal.i2c_mgr->get_device(BOARD_I2C_BUS_EXT, HAL_BARO_BMP085_I2C_ADDR)))); #endif // can optionally have baro on I2C too @@ -605,14 +602,14 @@ void AP_Baro::init(void) #endif ADD_BACKEND(AP_Baro_BMP085::probe(*this, std::move(hal.i2c_mgr->get_device(_ext_bus, HAL_BARO_BMP085_I2C_ADDR)))); - #endif + #endif #endif } #ifdef HAL_PROBE_EXTERNAL_I2C_BAROS _probe_i2c_barometers(); #endif - + #if CONFIG_HAL_BOARD != HAL_BOARD_F4LIGHT && !defined(HAL_BARO_ALLOW_INIT_NO_BARO) // most boards requires external baro if (_num_drivers == 0 || _num_sensors == 0 || drivers[0] == nullptr) { diff --git a/libraries/AP_Baro/AP_Baro_Backend.h b/libraries/AP_Baro/AP_Baro_Backend.h index 6d13ab5095..388b59ff24 100644 --- a/libraries/AP_Baro/AP_Baro_Backend.h +++ b/libraries/AP_Baro/AP_Baro_Backend.h @@ -18,9 +18,6 @@ public: // trigger them to read the sensor virtual void accumulate(void) {} - // callback for UAVCAN messages - virtual void handle_baro_msg(float pressure, float temperature) {} - void backend_update(uint8_t instance); // Check that the baro valid by using a mean filter. diff --git a/libraries/AP_Baro/AP_Baro_UAVCAN.cpp b/libraries/AP_Baro/AP_Baro_UAVCAN.cpp index 7f871d05fa..4058ac12b3 100644 --- a/libraries/AP_Baro/AP_Baro_UAVCAN.cpp +++ b/libraries/AP_Baro/AP_Baro_UAVCAN.cpp @@ -5,98 +5,182 @@ #include "AP_Baro_UAVCAN.h" #include +#include + +#include +#include extern const AP_HAL::HAL& hal; #define debug_baro_uavcan(level_debug, can_driver, fmt, args...) do { if ((level_debug) <= AP::can().get_debug_level_driver(can_driver)) { printf(fmt, ##args); }} while (0) +//UAVCAN Frontend Registry Binder +UC_REGISTRY_BINDER(PressureCb, uavcan::equipment::air_data::StaticPressure); +UC_REGISTRY_BINDER(TemperatureCb, uavcan::equipment::air_data::StaticTemperature); + +AP_Baro_UAVCAN::DetectedModules AP_Baro_UAVCAN::_detected_modules[] = {0}; +AP_HAL::Semaphore* AP_Baro_UAVCAN::_sem_registry = nullptr; + /* constructor - registers instance at top Baro driver */ AP_Baro_UAVCAN::AP_Baro_UAVCAN(AP_Baro &baro) : AP_Baro_Backend(baro) -{ -} +{} -AP_Baro_UAVCAN::~AP_Baro_UAVCAN() +void AP_Baro_UAVCAN::subscribe_msgs(AP_UAVCAN* ap_uavcan) { - if (!_initialized) { - return; - } - - AP_UAVCAN *ap_uavcan = AP_UAVCAN::get_uavcan(_manager); if (ap_uavcan == nullptr) { return; } - ap_uavcan->remove_baro_listener(this); + auto* node = ap_uavcan->get_node(); - debug_baro_uavcan(2, _manager, "AP_Baro_UAVCAN destructed\n\r"); + uavcan::Subscriber *pressure_listener; + pressure_listener = new uavcan::Subscriber(*node); + // Msg Handler + const int pressure_listener_res = pressure_listener->start(PressureCb(ap_uavcan, &handle_pressure)); + if (pressure_listener_res < 0) { + AP_HAL::panic("UAVCAN Baro subscriber start problem\n\r"); + return; + } + + uavcan::Subscriber *temperature_listener; + temperature_listener = new uavcan::Subscriber(*node); + // Msg Handler + const int temperature_listener_res = temperature_listener->start(TemperatureCb(ap_uavcan, &handle_temperature)); + if (temperature_listener_res < 0) { + AP_HAL::panic("UAVCAN Baro subscriber start problem\n\r"); + return; + } } -AP_Baro_Backend *AP_Baro_UAVCAN::probe(AP_Baro &baro) +bool AP_Baro_UAVCAN::take_registry() { - uint8_t can_num_drivers = AP::can().get_num_drivers(); + if (_sem_registry == nullptr) { + _sem_registry = hal.util->new_semaphore(); + } + return _sem_registry->take(HAL_SEMAPHORE_BLOCK_FOREVER); +} - AP_Baro_UAVCAN *sensor; - for (uint8_t i = 0; i < can_num_drivers; i++) { - AP_UAVCAN *ap_uavcan = AP_UAVCAN::get_uavcan(i); - if (ap_uavcan == nullptr) { - continue; - } +void AP_Baro_UAVCAN::give_registry() +{ + _sem_registry->give(); +} - uint8_t freebaro = ap_uavcan->find_smallest_free_baro_node(); - if (freebaro == UINT8_MAX) { - continue; +AP_Baro_Backend* AP_Baro_UAVCAN::probe(AP_Baro &baro) +{ + if (!take_registry()) { + return nullptr; + } + AP_Baro_UAVCAN* backend = nullptr; + for (uint8_t i = 0; i < BARO_MAX_DRIVERS; i++) { + if (_detected_modules[i].driver == nullptr && _detected_modules[i].ap_uavcan != nullptr) { + backend = new AP_Baro_UAVCAN(baro); + if (backend == nullptr) { + debug_baro_uavcan(2, + _detected_modules[i].ap_uavcan->get_driver_index(), + "Failed register UAVCAN Baro Node %d on Bus %d\n", + _detected_modules[i].node_id, + _detected_modules[i].ap_uavcan->get_driver_index()); + } else { + _detected_modules[i].driver = backend; + backend->_ap_uavcan = _detected_modules[i].ap_uavcan; + backend->_node_id = _detected_modules[i].node_id; + backend->register_sensor(); + debug_baro_uavcan(2, + _detected_modules[i].ap_uavcan->get_driver_index(), + "Registered UAVCAN Baro Node %d on Bus %d\n", + _detected_modules[i].node_id, + _detected_modules[i].ap_uavcan->get_driver_index()); + } + break; } - sensor = new AP_Baro_UAVCAN(baro); - if (sensor->register_uavcan_baro(i, freebaro)) { - debug_baro_uavcan(2, i, "AP_Baro_UAVCAN probed, drv: %d, node: %d\n\r", i, freebaro); - return sensor; - } else { - delete sensor; + } + give_registry(); + return backend; +} + +AP_Baro_UAVCAN* AP_Baro_UAVCAN::get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t node_id, bool create_new) +{ + if (ap_uavcan == nullptr) { + return nullptr; + } + for (uint8_t i = 0; i < BARO_MAX_DRIVERS; i++) { + if (_detected_modules[i].driver != nullptr && + _detected_modules[i].ap_uavcan == ap_uavcan && + _detected_modules[i].node_id == node_id) { + return _detected_modules[i].driver; + } + } + + if (create_new) { + bool already_detected = false; + //Check if there's an empty spot for possible registeration + for (uint8_t i = 0; i < BARO_MAX_DRIVERS; i++) { + if (_detected_modules[i].ap_uavcan == ap_uavcan && _detected_modules[i].node_id == node_id) { + //Already Detected + already_detected = true; + break; + } + } + if (!already_detected) { + for (uint8_t i = 0; i < BARO_MAX_DRIVERS; i++) { + if (_detected_modules[i].ap_uavcan == nullptr) { + _detected_modules[i].ap_uavcan = ap_uavcan; + _detected_modules[i].node_id = node_id; + break; + } + } } } return nullptr; } +void AP_Baro_UAVCAN::handle_pressure(AP_UAVCAN* ap_uavcan, uint8_t node_id, const PressureCb &cb) +{ + if (take_registry()) { + AP_Baro_UAVCAN* driver = get_uavcan_backend(ap_uavcan, node_id, true); + if (driver == nullptr) { + give_registry(); + return; + } + { + WITH_SEMAPHORE(driver->_sem_baro); + driver->_pressure = cb.msg->static_pressure; + driver->new_pressure = true; + } + give_registry(); + } +} + +void AP_Baro_UAVCAN::handle_temperature(AP_UAVCAN* ap_uavcan, uint8_t node_id, const TemperatureCb &cb) +{ + if (take_registry()) { + AP_Baro_UAVCAN* driver = get_uavcan_backend(ap_uavcan, node_id, false); + if (driver == nullptr) { + give_registry(); + return; + } + { + WITH_SEMAPHORE(driver->_sem_baro); + driver->_temperature = cb.msg->static_temperature; + } + give_registry(); + } +} + // Read the sensor void AP_Baro_UAVCAN::update(void) { WITH_SEMAPHORE(_sem_baro); - _copy_to_frontend(_instance, _pressure, _temperature); + if (new_pressure) { + _copy_to_frontend(_instance, _pressure, _temperature); - _frontend.set_external_temperature(_temperature); -} - -void AP_Baro_UAVCAN::handle_baro_msg(float pressure, float temperature) -{ - WITH_SEMAPHORE(_sem_baro); - - _pressure = pressure; - _temperature = temperature - C_TO_KELVIN; - _last_timestamp = AP_HAL::micros64(); -} - -bool AP_Baro_UAVCAN::register_uavcan_baro(uint8_t mgr, uint8_t node) -{ - AP_UAVCAN *ap_uavcan = AP_UAVCAN::get_uavcan(mgr); - if (ap_uavcan == nullptr) { - return false; + _frontend.set_external_temperature(_temperature); + new_pressure = false; } - _manager = mgr; - - if (ap_uavcan->register_baro_listener_to_node(this, node)) { - _instance = _frontend.register_sensor(); - debug_baro_uavcan(2, mgr, "AP_Baro_UAVCAN loaded\n\r"); - - _initialized = true; - - return true; - } - - return false; } #endif // HAL_WITH_UAVCAN diff --git a/libraries/AP_Baro/AP_Baro_UAVCAN.h b/libraries/AP_Baro/AP_Baro_UAVCAN.h index efb5adbdf2..6eecb1ec91 100644 --- a/libraries/AP_Baro/AP_Baro_UAVCAN.h +++ b/libraries/AP_Baro/AP_Baro_UAVCAN.h @@ -1,30 +1,51 @@ #pragma once #include "AP_Baro_Backend.h" + #include +class PressureCb; +class TemperatureCb; + class AP_Baro_UAVCAN : public AP_Baro_Backend { public: - AP_Baro_UAVCAN(AP_Baro &); - ~AP_Baro_UAVCAN() override; - - static AP_Baro_Backend *probe(AP_Baro &baro); + AP_Baro_UAVCAN(AP_Baro &baro); void update() override; - // This method is called from UAVCAN thread - virtual void handle_baro_msg(float pressure, float temperature) override; + inline void register_sensor() { + _instance = _frontend.register_sensor(); + } - bool register_uavcan_baro(uint8_t mgr, uint8_t node); + 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); + + static void handle_pressure(AP_UAVCAN* ap_uavcan, uint8_t node_id, const PressureCb &cb); + static void handle_temperature(AP_UAVCAN* ap_uavcan, uint8_t node_id, const TemperatureCb &cb); private: + static bool take_registry(); + static void give_registry(); + uint8_t _instance; + + bool new_pressure; float _pressure; float _temperature; uint64_t _last_timestamp; - uint8_t _manager; - - bool _initialized; HAL_Semaphore _sem_baro; + + AP_UAVCAN* _ap_uavcan; + uint8_t _node_id; + + // Module Detection Registry + static struct DetectedModules { + AP_UAVCAN* ap_uavcan; + uint8_t node_id; + AP_Baro_UAVCAN* driver; + } _detected_modules[BARO_MAX_DRIVERS]; + + static AP_HAL::Semaphore *_sem_registry; };