From d16f31711e630bcb530ae9e503a069780f9c19bc Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 1 May 2020 08:09:27 +1000
Subject: [PATCH] AP_RangeFinder: fixed mixing UAVCAN and non-UAVCAN
 rangefinders

UAVCAN rangefinders add themselves to the frontend drivers as the
devices appear. If they turn up before RangeFinder::init() is run then
this prevented init() from scanning for the other rangefinders as
num_instances is non-zero

This also fixes a race condition in updating num_instances in the
UAVCAN backend
---
 libraries/AP_RangeFinder/AP_RangeFinder.cpp   | 22 ++++++++++++++++---
 libraries/AP_RangeFinder/AP_RangeFinder.h     |  2 ++
 .../AP_RangeFinder/AP_RangeFinder_UAVCAN.cpp  | 21 ++++++++++--------
 3 files changed, 33 insertions(+), 12 deletions(-)

diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.cpp b/libraries/AP_RangeFinder/AP_RangeFinder.cpp
index e883cf17a5..0f1139562d 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder.cpp
+++ b/libraries/AP_RangeFinder/AP_RangeFinder.cpp
@@ -255,10 +255,11 @@ void RangeFinder::convert_params(void) {
  */
 void RangeFinder::init(enum Rotation orientation_default)
 {
-    if (num_instances != 0) {
+    if (init_done) {
         // init called a 2nd time?
         return;
     }
+    init_done = true;
 
     convert_params();
 
@@ -270,11 +271,14 @@ void RangeFinder::init(enum Rotation orientation_default)
     for (uint8_t i=0, serial_instance = 0; i<RANGEFINDER_MAX_INSTANCES; i++) {
         // serial_instance will be increased inside detect_instance
         // if a serial driver is loaded for this instance
+        WITH_SEMAPHORE(detect_sem);
         detect_instance(i, serial_instance);
         if (drivers[i] != nullptr) {
             // we loaded a driver for this instance, so it must be
-            // present (although it may not be healthy)
-            num_instances = i+1;
+            // present (although it may not be healthy). We use MAX()
+            // here as a UAVCAN rangefinder may already have been
+            // found
+            num_instances = MAX(num_instances, i+1);
         }
 
         // initialise status
@@ -502,6 +506,18 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
             drivers[instance] = new AP_RangeFinder_LeddarVu8(state[instance], params[instance], serial_instance++);
         }
         break;
+
+#if HAL_WITH_UAVCAN
+    case Type::UAVCAN:
+        /*
+          the UAVCAN driver gets created when we first receive a
+          measurement. We take the instance slot now, even if we don't
+          yet have the driver
+         */
+        num_instances = MAX(num_instances, instance+1);
+        break;
+#endif
+
     default:
         break;
     }
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.h b/libraries/AP_RangeFinder/AP_RangeFinder.h
index 8214f95eba..917d8f9f43 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder.h
+++ b/libraries/AP_RangeFinder/AP_RangeFinder.h
@@ -185,6 +185,8 @@ private:
     RangeFinder_State state[RANGEFINDER_MAX_INSTANCES];
     AP_RangeFinder_Backend *drivers[RANGEFINDER_MAX_INSTANCES];
     uint8_t num_instances;
+    bool init_done;
+    HAL_Semaphore detect_sem;
     float estimated_terrain_height;
     Vector3f pos_offset_zero;   // allows returning position offsets of zero for invalid requests
 
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_UAVCAN.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_UAVCAN.cpp
index 5f8208ac2f..9db01cee5f 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_UAVCAN.cpp
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_UAVCAN.cpp
@@ -49,11 +49,12 @@ AP_RangeFinder_UAVCAN* AP_RangeFinder_UAVCAN::get_uavcan_backend(AP_UAVCAN* ap_u
         return nullptr;
     }
     AP_RangeFinder_UAVCAN* driver = nullptr;
+    RangeFinder &frontend = *AP::rangefinder();
     //Scan through the Rangefinder params to find UAVCAN RFND with matching address.
     for (uint8_t i = 0; i < RANGEFINDER_MAX_INSTANCES; i++) {
-        if ((RangeFinder::Type)AP::rangefinder()->params[i].type.get() == RangeFinder::Type::UAVCAN &&
-            AP::rangefinder()->params[i].address == address) {
-            driver = (AP_RangeFinder_UAVCAN*)AP::rangefinder()->drivers[i];
+        if ((RangeFinder::Type)frontend.params[i].type.get() == RangeFinder::Type::UAVCAN &&
+            frontend.params[i].address == address) {
+            driver = (AP_RangeFinder_UAVCAN*)frontend.drivers[i];
         }
         //Double check if the driver was initialised as UAVCAN Type
         if (driver != nullptr && (driver->_backend_type == RangeFinder::Type::UAVCAN)) {
@@ -70,19 +71,21 @@ AP_RangeFinder_UAVCAN* AP_RangeFinder_UAVCAN::get_uavcan_backend(AP_UAVCAN* ap_u
     
     if (create_new) {
         for (uint8_t i = 0; i < RANGEFINDER_MAX_INSTANCES; i++) {
-            if ((RangeFinder::Type)AP::rangefinder()->params[i].type.get() == RangeFinder::Type::UAVCAN &&
-                AP::rangefinder()->params[i].address == address) {
-                if (AP::rangefinder()->drivers[i] != nullptr) {
+            if ((RangeFinder::Type)frontend.params[i].type.get() == RangeFinder::Type::UAVCAN &&
+                frontend.params[i].address == address) {
+                WITH_SEMAPHORE(frontend.detect_sem);
+                if (frontend.drivers[i] != nullptr) {
                     //we probably initialised this driver as something else, reboot is required for setting
                     //it up as UAVCAN type
                     return nullptr;
                 }
-                AP::rangefinder()->drivers[i] = new AP_RangeFinder_UAVCAN(AP::rangefinder()->state[i], AP::rangefinder()->params[i]);
-                driver = (AP_RangeFinder_UAVCAN*)AP::rangefinder()->drivers[i];
+                frontend.drivers[i] = new AP_RangeFinder_UAVCAN(frontend.state[i], frontend.params[i]);
+                driver = (AP_RangeFinder_UAVCAN*)frontend.drivers[i];
                 if (driver == nullptr) {
                     break;
                 }
-                AP::rangefinder()->num_instances = MAX(i+1, AP::rangefinder()->num_instances);
+                gcs().send_text(MAV_SEVERITY_INFO, "RangeFinder[%u]: added UAVCAN node %u addr %u",
+                                unsigned(i), unsigned(node_id), unsigned(address));
                 //Assign node id and respective uavcan driver, for identification
                 if (driver->_ap_uavcan == nullptr) {
                     driver->_ap_uavcan = ap_uavcan;