AP_Compass: united enumeration on startup, multiple CAN drivers, correct dev_id based on network and node ID

libraries/AP_Compass/AP_Compass.cpp

@@ -20,6 +20,7 @@
 #include "AP_Compass_AK09916.h"
 #include "AP_Compass_QMC5883L.h"
 #if HAL_WITH_UAVCAN
+#include <AP_BoardConfig/AP_BoardConfig_CAN.h>
 #include "AP_Compass_UAVCAN.h"
 #endif
 #include "AP_Compass_MMC3416.h"
@@ -519,6 +520,16 @@ void Compass::_detect_backends(void)
     ADD_BACKEND(new AP_Compass_SITL(*this), nullptr, false);
     return;
 #endif
+    
+#if HAL_WITH_UAVCAN
+    bool added;
+    do {
+        added = _add_backend(AP_Compass_UAVCAN::probe(*this), "UAVCAN", true);
+        if (_backend_count == COMPASS_MAX_BACKEND || _compass_count == COMPASS_MAX_INSTANCES) {
+            return;
+        }
+    } while (added);
+#endif
 
 #if HAL_COMPASS_DEFAULT == HAL_COMPASS_HIL
     ADD_BACKEND(AP_Compass_HIL::detect(*this), nullptr, false);
@@ -719,18 +730,6 @@ void Compass::_detect_backends(void)
     #error Unrecognised HAL_COMPASS_TYPE setting
 #endif
 
-#if HAL_WITH_UAVCAN
-    if ((AP_BoardConfig::get_can_enable() != 0) && (hal.can_mgr != nullptr))
-    {
-        if((_backend_count < COMPASS_MAX_BACKEND) && (_compass_count < COMPASS_MAX_INSTANCES))
-        {
-            printf("Creating AP_Compass_UAVCAN\n\r");
-            _backends[_backend_count] = new AP_Compass_UAVCAN(*this);
-            _backend_count++;
-        }
-    }
-#endif
-
     if (_backend_count == 0 ||
         _compass_count == 0) {
         hal.console->printf("No Compass backends available\n");

libraries/AP_Compass/AP_Compass_UAVCAN.cpp

@@ -25,12 +25,11 @@
 #include <unistd.h>
 
 #include <AP_BoardConfig/AP_BoardConfig.h>
+#include <AP_BoardConfig/AP_BoardConfig_CAN.h>
 
 extern const AP_HAL::HAL& hal;
 
-#define debug_mag_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 magnetometer now.
+#define debug_mag_uavcan(level, fmt, args...) do { if ((level) <= AP_BoardConfig_CAN::get_can_debug()) { printf(fmt, ##args); }} while (0)
 
 /*
   constructor - registers instance at top Compass driver
@@ -38,57 +37,95 @@ extern const AP_HAL::HAL& hal;
 AP_Compass_UAVCAN::AP_Compass_UAVCAN(Compass &compass):
     AP_Compass_Backend(compass)
 {
-    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
-            _instance = register_compass();
-            hal.scheduler->delay(1000);
-            uint8_t listener_channel = ap_uavcan->register_mag_listener(this, 1);
-
-            struct DeviceStructure {
-                uint8_t bus_type : 3;
-                uint8_t bus: 5;
-                uint8_t address;
-                uint8_t devtype;
-            };
-            union DeviceId {
-                struct DeviceStructure devid_s;
-                uint32_t devid;
-            };
-            union DeviceId d;
-
-            d.devid_s.bus_type = 3;
-            d.devid_s.bus = 0;
-            d.devid_s.address = listener_channel;
-            d.devid_s.devtype = 0;
-
-            set_dev_id(_instance, d.devid);
-            set_external(_instance, true);
-
-            _sum.zero();
-            _count = 0;
-
-            accumulate();
-
-            debug_mag_uavcan(2, "AP_Compass_UAVCAN loaded\n\r");
+    _mag_baro = hal.util->new_semaphore();
+}
+
+AP_Compass_UAVCAN::~AP_Compass_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_mag_listener(this);
+
+                debug_mag_uavcan(2, "AP_Compass_UAVCAN destructed\n\r");
+            }
         }
     }
+}
 
-    _mag_baro = hal.util->new_semaphore();
+AP_Compass_Backend *AP_Compass_UAVCAN::probe(Compass &compass)
+{
+    AP_Compass_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 freemag = uavcan->find_smallest_free_mag_node();
+                    if (freemag != UINT8_MAX) {
+                        sensor = new AP_Compass_UAVCAN(compass);
+                        if (sensor->register_uavcan_compass(i, freemag)) {
+                            debug_mag_uavcan(2, "AP_Compass_UAVCAN probed, drv: %d, node: %d\n\r", i, freemag);
+                            return sensor;
+                        } else {
+                            delete sensor;
+                            sensor = nullptr;
+                        }
+                    }
+                }
+            }
+        }
+    }
+
+    return sensor;
 }
 
-AP_Compass_UAVCAN::~AP_Compass_UAVCAN()
+bool AP_Compass_UAVCAN::register_uavcan_compass(uint8_t mgr, uint8_t node)
 {
-    if (hal.can_mgr != nullptr) {
-        AP_UAVCAN *ap_uavcan = hal.can_mgr->get_UAVCAN();
+    if (hal.can_mgr[mgr] != nullptr) {
+        AP_UAVCAN *ap_uavcan = hal.can_mgr[mgr]->get_UAVCAN();
         if (ap_uavcan != nullptr) {
-            ap_uavcan->remove_mag_listener(this);
+            _manager = mgr;
+
+            if (ap_uavcan->register_mag_listener_to_node(this, node)) {
+                _instance = register_compass();
+
+                struct DeviceStructure {
+                    uint8_t bus_type : 3;
+                    uint8_t bus: 5;
+                    uint8_t address;
+                    uint8_t devtype;
+                };
+                union DeviceId {
+                    struct DeviceStructure devid_s;
+                    uint32_t devid;
+                };
+                union DeviceId d;
+
+                d.devid_s.bus_type = 3;
+                d.devid_s.bus = mgr;
+                d.devid_s.address = node;
+                d.devid_s.devtype = 1;
 
-            debug_mag_uavcan(2, "AP_Compass_UAVCAN destructed\n\r");
+                set_dev_id(_instance, d.devid);
+                set_external(_instance, true);
+
+                _sum.zero();
+                _count = 0;
+
+                accumulate();
+
+                debug_mag_uavcan(2, "AP_Compass_UAVCAN loaded\n\r");
+
+                return true;
+            }
         }
     }
+
+    return false;
 }
 
 void AP_Compass_UAVCAN::read(void)

libraries/AP_Compass/AP_Compass_UAVCAN.h

@@ -12,6 +12,10 @@ public:
     AP_Compass_UAVCAN(Compass &compass);
     ~AP_Compass_UAVCAN() override;
 
+    static AP_Compass_Backend *probe(Compass &compass);
+
+    bool register_uavcan_compass(uint8_t mgr, uint8_t node);
+
     // This method is called from UAVCAN thread
     void handle_mag_msg(Vector3f &mag);
 
@@ -22,5 +26,8 @@ private:
     uint32_t _count;
     uint64_t _last_timestamp;
 
+    bool _initialized;
+    uint8_t _manager;
+
     AP_HAL::Semaphore *_mag_baro;
 };