From 6e6efa7e1bea7a0133631ba6979693a040d2361e Mon Sep 17 00:00:00 2001 From: Eugene Shamaev Date: Sat, 6 May 2017 12:13:05 +0300 Subject: [PATCH] AP_Baro: enumeration and multiple interfaces support --- libraries/AP_Baro/AP_Baro.cpp | 25 +++++---- libraries/AP_Baro/AP_Baro_UAVCAN.cpp | 81 ++++++++++++++++++++-------- libraries/AP_Baro/AP_Baro_UAVCAN.h | 7 +++ 3 files changed, 79 insertions(+), 34 deletions(-) diff --git a/libraries/AP_Baro/AP_Baro.cpp b/libraries/AP_Baro/AP_Baro.cpp index 9d599d69fd..dd7d9cbe45 100644 --- a/libraries/AP_Baro/AP_Baro.cpp +++ b/libraries/AP_Baro/AP_Baro.cpp @@ -25,6 +25,7 @@ #include #include #include +#include #include #include "AP_Baro_SITL.h" @@ -394,6 +395,16 @@ void AP_Baro::init(void) 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); +#endif + #if HAL_BARO_DEFAULT == HAL_BARO_PX4 || HAL_BARO_DEFAULT == HAL_BARO_VRBRAIN switch (AP_BoardConfig::get_board_type()) { case AP_BoardConfig::PX4_BOARD_PX4V1: @@ -479,19 +490,6 @@ void AP_Baro::init(void) std::move(hal.i2c_mgr->get_device(_ext_bus, HAL_BARO_MS5611_I2C_ADDR)))); #endif } - -#if HAL_WITH_UAVCAN - // If there is place left - allocate one UAVCAN based baro - if ((AP_BoardConfig::get_can_enable() != 0) && (hal.can_mgr != nullptr)) - { - if(_num_drivers < BARO_MAX_DRIVERS && _num_sensors < BARO_MAX_INSTANCES) - { - printf("Creating AP_Baro_UAVCAN\n\r"); - drivers[_num_drivers] = new AP_Baro_UAVCAN(*this); - _num_drivers++; - } - } -#endif if (_num_drivers == 0 || _num_sensors == 0 || drivers[0] == nullptr) { AP_BoardConfig::sensor_config_error("Baro: unable to initialise driver"); @@ -617,3 +615,4 @@ void AP_Baro::set_pressure_correction(uint8_t instance, float p_correction) sensors[instance].p_correction = p_correction; } } + diff --git a/libraries/AP_Baro/AP_Baro_UAVCAN.cpp b/libraries/AP_Baro/AP_Baro_UAVCAN.cpp index aa1c8bb058..432e5ea921 100644 --- a/libraries/AP_Baro/AP_Baro_UAVCAN.cpp +++ b/libraries/AP_Baro/AP_Baro_UAVCAN.cpp @@ -4,6 +4,7 @@ #include "AP_Baro_UAVCAN.h" #include +#include #include #include @@ -12,9 +13,7 @@ extern const AP_HAL::HAL& hal; -#define debug_baro_uavcan(level, fmt, args...) do { if ((level) <= AP_BoardConfig::get_can_debug()) { hal.console->printf(fmt, ##args); }} while (0) - -// There is limitation to use only one UAVCAN barometer now. +#define debug_baro_uavcan(level, fmt, args...) do { if ((level) <= AP_BoardConfig_CAN::get_can_debug()) { printf(fmt, ##args); }} while (0) /* constructor - registers instance at top Baro driver @@ -22,33 +21,50 @@ extern const AP_HAL::HAL& hal; AP_Baro_UAVCAN::AP_Baro_UAVCAN(AP_Baro &baro) : AP_Baro_Backend(baro) { - _instance = _frontend.register_sensor(); - if (hal.can_mgr != nullptr) { - AP_UAVCAN *ap_uavcan = hal.can_mgr->get_UAVCAN(); - if (ap_uavcan != nullptr) { - // Give time to receive some packets from CAN if baro sensor is present - // This way it will get calibrated correctly - hal.scheduler->delay(1000); - ap_uavcan->register_baro_listener(this, 1); - - debug_baro_uavcan(2, "AP_Baro_UAVCAN loaded\n\r"); - } - } - _sem_baro = hal.util->new_semaphore(); } AP_Baro_UAVCAN::~AP_Baro_UAVCAN() { - if (hal.can_mgr != nullptr) { - AP_UAVCAN *ap_uavcan = hal.can_mgr->get_UAVCAN(); - if (ap_uavcan != nullptr) { - ap_uavcan->remove_baro_listener(this); - debug_baro_uavcan(2, "AP_Baro_UAVCAN destructed\n\r"); + if (_initialized) { + if (hal.can_mgr[_manager] != nullptr) { + AP_UAVCAN *ap_uavcan = hal.can_mgr[_manager]->get_UAVCAN(); + if (ap_uavcan != nullptr) { + ap_uavcan->remove_baro_listener(this); + debug_baro_uavcan(2, "AP_Baro_UAVCAN destructed\n\r"); + } } } } +AP_Baro_Backend *AP_Baro_UAVCAN::probe(AP_Baro &baro) +{ + AP_Baro_UAVCAN *sensor = nullptr; + + if (AP_BoardConfig_CAN::get_can_num_ifaces() != 0) { + for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_DRIVERS; i++) { + if (hal.can_mgr[i] != nullptr) { + AP_UAVCAN *uavcan = hal.can_mgr[i]->get_UAVCAN(); + if (uavcan != nullptr) { + uint8_t freebaro = uavcan->find_smallest_free_baro_node(); + if (freebaro != UINT8_MAX) { + sensor = new AP_Baro_UAVCAN(baro); + if (sensor->register_uavcan_baro(i, freebaro)) { + debug_baro_uavcan(2, "AP_Baro_UAVCAN probed, drv: %d, node: %d\n\r", i, freebaro); + return sensor; + } else { + delete sensor; + sensor = nullptr; + } + } + } + } + } + } + + return sensor; +} + // Read the sensor void AP_Baro_UAVCAN::update(void) { @@ -70,5 +86,28 @@ void AP_Baro_UAVCAN::handle_baro_msg(float pressure, float temperature) } } +bool AP_Baro_UAVCAN::register_uavcan_baro(uint8_t mgr, uint8_t node) +{ + if (hal.can_mgr[mgr] != nullptr) { + AP_UAVCAN *ap_uavcan = hal.can_mgr[mgr]->get_UAVCAN(); + + if (ap_uavcan != nullptr) { + _manager = mgr; + + if (ap_uavcan->register_baro_listener_to_node(this, node)) + { + _instance = _frontend.register_sensor(); + debug_baro_uavcan(2, "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 4d81897f9f..15c20d6702 100644 --- a/libraries/AP_Baro/AP_Baro_UAVCAN.h +++ b/libraries/AP_Baro/AP_Baro_UAVCAN.h @@ -8,16 +8,23 @@ public: AP_Baro_UAVCAN(AP_Baro &); ~AP_Baro_UAVCAN() override; + static AP_Baro_Backend *probe(AP_Baro &baro); + void update() override; // This method is called from UAVCAN thread virtual void handle_baro_msg(float pressure, float temperature) override; + bool register_uavcan_baro(uint8_t mgr, uint8_t node); + private: uint8_t _instance; float _pressure; float _temperature; uint64_t _last_timestamp; + uint8_t _manager; + + bool _initialized; AP_HAL::Semaphore *_sem_baro; };