AP_Airspeed: uavcan airspeed supported

libraries/AP_Airspeed/AP_Airspeed.cpp

@@ -30,6 +30,9 @@
 #include "AP_Airspeed_DLVR.h"
 #include "AP_Airspeed_analog.h"
 #include "AP_Airspeed_Backend.h"
+#if HAL_WITH_UAVCAN
+#include "AP_Airspeed_UAVCAN.h"
+#endif
 
 extern const AP_HAL::HAL &hal;
 
@@ -55,7 +58,7 @@ const AP_Param::GroupInfo AP_Airspeed::var_info[] = {
     // @Param: _TYPE
     // @DisplayName: Airspeed type
     // @Description: Type of airspeed sensor
-    // @Values: 0:None,1:I2C-MS4525D0,2:Analog,3:I2C-MS5525,4:I2C-MS5525 (0x76),5:I2C-MS5525 (0x77),6:I2C-SDP3X
+    // @Values: 0:None,1:I2C-MS4525D0,2:Analog,3:I2C-MS5525,4:I2C-MS5525 (0x76),5:I2C-MS5525 (0x77),6:I2C-SDP3X,7:I2C-DLVR,8:UAVCAN
     // @User: Standard
     AP_GROUPINFO_FLAGS("_TYPE", 0, AP_Airspeed, param[0].type, ARSPD_DEFAULT_TYPE, AP_PARAM_FLAG_ENABLE),
 
@@ -128,7 +131,7 @@ const AP_Param::GroupInfo AP_Airspeed::var_info[] = {
     // @Param: 2_TYPE
     // @DisplayName: Second Airspeed type
     // @Description: Type of 2nd airspeed sensor
-    // @Values: 0:None,1:I2C-MS4525D0,2:Analog,3:I2C-MS5525,4:I2C-MS5525 (0x76),5:I2C-MS5525 (0x77),6:I2C-SDP3X
+    // @Values: 0:None,1:I2C-MS4525D0,2:Analog,3:I2C-MS5525,4:I2C-MS5525 (0x76),5:I2C-MS5525 (0x77),6:I2C-SDP3X,7:I2C-DLVR,8:UAVCAN
     // @User: Standard
     AP_GROUPINFO_FLAGS("2_TYPE", 11, AP_Airspeed, param[1].type, 0, AP_PARAM_FLAG_ENABLE),
 
@@ -254,7 +257,13 @@ void AP_Airspeed::init()
             sensor[i] = new AP_Airspeed_DLVR(*this, i);
 #endif // !HAL_MINIMIZE_FEATURES
             break;
+        case TYPE_UAVCAN:
+#if HAL_WITH_UAVCAN
+            sensor[i] = AP_Airspeed_UAVCAN::probe(*this, i);
+#endif
+            break;
         }
+
         if (sensor[i] && !sensor[i]->init()) {
             gcs().send_text(MAV_SEVERITY_INFO, "Airspeed[%u] init failed", i);
             delete sensor[i];

libraries/AP_Airspeed/AP_Airspeed.h

@@ -163,6 +163,7 @@ public:
         TYPE_I2C_MS5525_ADDRESS_2=5,
         TYPE_I2C_SDP3X=6,
         TYPE_I2C_DLVR=7,
+        TYPE_UAVCAN=8,
     };
 
     // get current primary sensor

libraries/AP_Airspeed/AP_Airspeed_Backend.h

@@ -19,6 +19,7 @@
  */
 
 #include <AP_Common/AP_Common.h>
+#include <AP_Common/Semaphore.h>
 #include <AP_HAL/AP_HAL.h>
 #include "AP_Airspeed.h"
 

libraries/AP_Airspeed/AP_Airspeed_UAVCAN.cpp

@@ -0,0 +1,176 @@
+#include <AP_HAL/AP_HAL.h>
+
+#if HAL_WITH_UAVCAN
+
+#include "AP_Airspeed_UAVCAN.h"
+
+#include <AP_BoardConfig/AP_BoardConfig_CAN.h>
+#include <AP_UAVCAN/AP_UAVCAN.h>
+
+#include <uavcan/equipment/air_data/RawAirData.hpp>
+
+extern const AP_HAL::HAL& hal;
+
+#define debug_airspeed_uavcan(level_debug, can_driver, fmt, args...) do { if ((level_debug) <= AP::can().get_debug_level_driver(can_driver)) { printf(fmt, ##args); }} while (0)
+
+// UAVCAN Frontend Registry Binder
+UC_REGISTRY_BINDER(AirspeedCb, uavcan::equipment::air_data::RawAirData);
+
+AP_Airspeed_UAVCAN::DetectedModules AP_Airspeed_UAVCAN::_detected_modules[] = {0};
+AP_HAL::Semaphore* AP_Airspeed_UAVCAN::_sem_registry = nullptr;
+
+// constructor
+AP_Airspeed_UAVCAN::AP_Airspeed_UAVCAN(AP_Airspeed &_frontend, uint8_t _instance) :
+    AP_Airspeed_Backend(_frontend, _instance)
+{}
+
+void AP_Airspeed_UAVCAN::subscribe_msgs(AP_UAVCAN* ap_uavcan)
+{
+    if (ap_uavcan == nullptr) {
+        return;
+    }
+
+    auto* node = ap_uavcan->get_node();
+
+    uavcan::Subscriber<uavcan::equipment::air_data::RawAirData, AirspeedCb> *airspeed_listener;
+    airspeed_listener = new uavcan::Subscriber<uavcan::equipment::air_data::RawAirData, AirspeedCb>(*node);
+
+    const int airspeed_listener_res = airspeed_listener->start(AirspeedCb(ap_uavcan, &handle_airspeed));
+    if (airspeed_listener_res < 0) {
+        AP_HAL::panic("UAVCAN Airspeed subscriber start problem\n");
+    }
+}
+
+bool AP_Airspeed_UAVCAN::take_registry()
+{
+    if (_sem_registry == nullptr) {
+        _sem_registry = hal.util->new_semaphore();
+    }
+
+    return _sem_registry->take(HAL_SEMAPHORE_BLOCK_FOREVER);
+}
+
+void AP_Airspeed_UAVCAN::give_registry()
+{
+    _sem_registry->give();
+}
+
+AP_Airspeed_Backend* AP_Airspeed_UAVCAN::probe(AP_Airspeed &_frontend, uint8_t _instance)
+{
+    if (!take_registry()) {
+        return nullptr;
+    }
+
+    AP_Airspeed_UAVCAN* backend = nullptr;
+
+    for (uint8_t i = 0; i < AIRSPEED_MAX_SENSORS; i++) {
+        if (_detected_modules[i].driver == nullptr && _detected_modules[i].ap_uavcan != nullptr) {
+            backend = new AP_Airspeed_UAVCAN(_frontend, _instance);
+            if (backend == nullptr) {
+                debug_airspeed_uavcan(2,
+                                      _detected_modules[i].ap_uavcan->get_driver_index(),
+                                      "Failed register UAVCAN Airspeed Node %d on Bus %d\n",
+                                      _detected_modules[i].node_id,
+                                      _detected_modules[i].ap_uavcan->get_driver_index());
+            } else {
+                _detected_modules[i].driver = backend;
+                debug_airspeed_uavcan(2,
+                                      _detected_modules[i].ap_uavcan->get_driver_index(),
+                                      "Registered UAVCAN Airspeed Node %d on Bus %d\n",
+                                      _detected_modules[i].node_id,
+                                      _detected_modules[i].ap_uavcan->get_driver_index());
+            }
+            break;
+        }
+    }
+
+    give_registry();
+
+    return backend;
+}
+
+AP_Airspeed_UAVCAN* AP_Airspeed_UAVCAN::get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t node_id)
+{
+    if (ap_uavcan == nullptr) {
+        return nullptr;
+    }
+
+    for (uint8_t i = 0; i < AIRSPEED_MAX_SENSORS; i++) {
+        if (_detected_modules[i].driver != nullptr &&
+            _detected_modules[i].ap_uavcan == ap_uavcan &&
+            _detected_modules[i].node_id == node_id ) {
+            return _detected_modules[i].driver;
+        }
+    }
+
+    bool detected = false;
+    for (uint8_t i = 0; i < AIRSPEED_MAX_SENSORS; i++) {
+        if (_detected_modules[i].ap_uavcan == ap_uavcan && _detected_modules[i].node_id == node_id) {
+            // detected
+            detected = true;
+            break;
+        }
+    }
+
+    if (!detected) {
+        for (uint8_t i = 0; i < AIRSPEED_MAX_SENSORS; i++) {
+            if (_detected_modules[i].ap_uavcan == nullptr) {
+                _detected_modules[i].ap_uavcan = ap_uavcan;
+                _detected_modules[i].node_id = node_id;
+                break;
+            }
+        }
+    }
+
+    return nullptr;
+}
+
+void AP_Airspeed_UAVCAN::handle_airspeed(AP_UAVCAN* ap_uavcan, uint8_t node_id, const AirspeedCb &cb)
+{
+    if (take_registry()) {
+        AP_Airspeed_UAVCAN* driver = get_uavcan_backend(ap_uavcan, node_id);
+
+        if (driver != nullptr) {
+            WITH_SEMAPHORE(driver->_sem_airspeed);
+            driver->_pressure = cb.msg->differential_pressure;
+            driver->_temperature = cb.msg->static_air_temperature;
+            driver->_last_sample_time_ms = AP_HAL::millis();
+        }
+
+        give_registry();
+    }
+}
+
+bool AP_Airspeed_UAVCAN::init()
+{
+    // always returns true
+    return true;
+}
+
+bool AP_Airspeed_UAVCAN::get_differential_pressure(float &pressure)
+{
+    WITH_SEMAPHORE(_sem_airspeed);
+
+    if ((AP_HAL::millis() - _last_sample_time_ms) > 100) {
+        return false;
+    }
+
+    pressure = _pressure;
+
+    return true;
+}
+
+bool AP_Airspeed_UAVCAN::get_temperature(float &temperature)
+{
+    WITH_SEMAPHORE(_sem_airspeed);
+
+    if ((AP_HAL::millis() - _last_sample_time_ms) > 100) {
+        return false;
+    }
+
+    temperature = _temperature - C_TO_KELVIN;
+
+    return true;
+}
+
+#endif // HAL_WITH_UAVCAN

libraries/AP_Airspeed/AP_Airspeed_UAVCAN.h

@@ -0,0 +1,48 @@
+#pragma once
+
+#include "AP_Airspeed_Backend.h"
+
+#include <AP_UAVCAN/AP_UAVCAN.h>
+
+class AirspeedCb;
+
+class AP_Airspeed_UAVCAN : public AP_Airspeed_Backend {
+public:
+    AP_Airspeed_UAVCAN(AP_Airspeed &_frontend, uint8_t _instance);
+
+    bool init(void) override;
+
+    // return the current differential_pressure in Pascal
+    bool get_differential_pressure(float &pressure) override;
+
+    // temperature not available via analog backend
+    bool get_temperature(float &temperature) override;
+
+    static void subscribe_msgs(AP_UAVCAN* ap_uavcan);
+
+    static AP_Airspeed_Backend* probe(AP_Airspeed &_fronted, uint8_t _instance);
+
+private:
+    static bool take_registry();
+
+    static void give_registry();
+
+    static void handle_airspeed(AP_UAVCAN* ap_uavcan, uint8_t node_id, const AirspeedCb &cb);
+
+    static AP_Airspeed_UAVCAN* get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t node_id);
+
+    float _pressure; // Pascal
+    float _temperature; // Kelvin
+    uint32_t _last_sample_time_ms;
+
+    HAL_Semaphore _sem_airspeed;
+
+    // Module Detection Registry
+    static struct DetectedModules {
+        AP_UAVCAN* ap_uavcan;
+        uint8_t node_id;
+        AP_Airspeed_UAVCAN *driver;
+    } _detected_modules[AIRSPEED_MAX_SENSORS];
+
+    static AP_HAL::Semaphore *_sem_registry;
+};