AP_Baro: enumeration and multiple interfaces support

libraries/AP_Baro/AP_Baro.cpp

@@ -25,6 +25,7 @@
 #include <AP_HAL/AP_HAL.h>
 #include <AP_Math/AP_Math.h>
 #include <AP_BoardConfig/AP_BoardConfig.h>
+#include <AP_BoardConfig/AP_BoardConfig_CAN.h>
 #include <AP_Vehicle/AP_Vehicle_Type.h>
 
 #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;
     }
 }
+

libraries/AP_Baro/AP_Baro_UAVCAN.cpp

@@ -4,6 +4,7 @@
 
 #include "AP_Baro_UAVCAN.h"
 #include <AP_BoardConfig/AP_BoardConfig.h>
+#include <AP_BoardConfig/AP_BoardConfig_CAN.h>
 
 #include <sys/types.h>
 #include <sys/stat.h>
@@ -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,31 +21,48 @@ 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);
+    _sem_baro = hal.util->new_semaphore();
+}
 
-            debug_baro_uavcan(2, "AP_Baro_UAVCAN loaded\n\r");
+AP_Baro_UAVCAN::~AP_Baro_UAVCAN()
+{
+    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");
+            }
         }
     }
-
-    _sem_baro = hal.util->new_semaphore();
 }
 
-AP_Baro_UAVCAN::~AP_Baro_UAVCAN()
+AP_Baro_Backend *AP_Baro_UAVCAN::probe(AP_Baro &baro)
 {
-    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");
+    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
@@ -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
 

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;
 };