AP_GPS: move UAVCAN GPS related code to AP_GPS_UAVCAN backend

libraries/AP_GPS/AP_GPS.cpp

@@ -414,50 +414,24 @@ void AP_GPS::detect_instance(uint8_t instance)
     state[instance].status = NO_GPS;
     state[instance].hdop = GPS_UNKNOWN_DOP;
     state[instance].vdop = GPS_UNKNOWN_DOP;
-    
+
     switch (_type[instance]) {
     // user has to explicitly set the MAV type, do not use AUTO
     // do not try to detect the MAV type, assume it's there
-    case GPS_TYPE_MAV: {
+    case GPS_TYPE_MAV:
         dstate->auto_detected_baud = false; // specified, not detected
         new_gps = new AP_GPS_MAV(*this, state[instance], nullptr);
         goto found_gps;
         break;
-    }
 
     // user has to explicitly set the UAVCAN type, do not use AUTO
-    case GPS_TYPE_UAVCAN: {
+    case GPS_TYPE_UAVCAN:
 #if HAL_WITH_UAVCAN
         dstate->auto_detected_baud = false; // specified, not detected
-
-        uint8_t can_num_drivers = AP::can().get_num_drivers();
-
-        for (uint8_t i = 0; i < can_num_drivers; i++) {
-            AP_UAVCAN *ap_uavcan = AP_UAVCAN::get_uavcan(i);
-            if (ap_uavcan == nullptr) {
-                continue;
-            }
-
-            uint8_t gps_node = ap_uavcan->find_gps_without_listener();
-            if (gps_node == UINT8_MAX) {
-                continue;
-            }
-
-            new_gps = new AP_GPS_UAVCAN(*this, state[instance], nullptr);
-            ((AP_GPS_UAVCAN*) new_gps)->set_uavcan_manager(i);
-            if (ap_uavcan->register_gps_listener_to_node(new_gps, gps_node)) {
-                if (AP::can().get_debug_level_driver(i) >= 2) {
-                    printf("AP_GPS_UAVCAN registered\n\r");
-                }
-                goto found_gps;
-            } else {
-                delete new_gps;
-            }
-        }
+        new_gps = AP_GPS_UAVCAN::probe(*this, state[instance]);
+        goto found_gps;
 #endif
-        return;
-    }
-
+        return; // We don't do anything here if UAVCAN is not supported
     default:
         break;
     }

libraries/AP_GPS/AP_GPS_UAVCAN.h

@@ -20,27 +20,50 @@
 
 #include <AP_Common/AP_Common.h>
 #include <AP_HAL/AP_HAL.h>
-
 #include "AP_GPS.h"
 #include "GPS_Backend.h"
 
+#include <AP_UAVCAN/AP_UAVCAN.h>
+
+class FixCb;
+class AuxCb;
+
 class AP_GPS_UAVCAN : public AP_GPS_Backend {
 public:
-    AP_GPS_UAVCAN(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port);
-    ~AP_GPS_UAVCAN() override;
+    AP_GPS_UAVCAN(AP_GPS &_gps, AP_GPS::GPS_State &_state);
+    ~AP_GPS_UAVCAN();
 
     bool read() override;
-    void set_uavcan_manager(uint8_t mgr);
-
-    // This method is called from UAVCAN thread
-    void handle_gnss_msg(const AP_GPS::GPS_State &msg) override;
 
     const char *name() const override { return "UAVCAN"; }
 
-private:
-    bool _new_data;
-    uint8_t _manager;
+    static void subscribe_msgs(AP_UAVCAN* ap_uavcan);
+    static AP_GPS_Backend* probe(AP_GPS &_gps, AP_GPS::GPS_State &_state);
 
-    AP_GPS::GPS_State _interm_state;
-    AP_HAL::Semaphore *_sem_gnss;
+    static void handle_fix_msg_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const FixCb &cb);
+    static void handle_aux_msg_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const AuxCb &cb);
+
+private:
+    void handle_fix_msg(const FixCb &cb);
+    void handle_aux_msg(const AuxCb &cb);
+
+    static bool take_registry();
+    static void give_registry();
+    static AP_GPS_UAVCAN* get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t node_id);
+
+    bool _new_data;
+    AP_GPS::GPS_State interim_state;
+
+    HAL_Semaphore sem;
+
+    uint8_t _detected_module;
+
+    // Module Detection Registry
+    static struct DetectedModules {
+        AP_UAVCAN* ap_uavcan;
+        uint8_t node_id;
+        AP_GPS_UAVCAN* driver;
+    } _detected_modules[GPS_MAX_RECEIVERS];
+
+    static AP_HAL::Semaphore *_sem_registry;
 };

libraries/AP_GPS/GPS_Backend.h

@@ -50,7 +50,6 @@ public:
     virtual void broadcast_configuration_failure_reason(void) const { return ; }
 
     virtual void handle_msg(const mavlink_message_t *msg) { return ; }
-    virtual void handle_gnss_msg(const AP_GPS::GPS_State &msg) { return ; }
 
     // driver specific lag, returns true if the driver is confident in the provided lag
     virtual bool get_lag(float &lag) const { lag = 0.2f; return true; }