AP_Baro: support for UAVCAN connected barometers

libraries/AP_Baro/AP_Baro.cpp

@@ -34,6 +34,9 @@
 #include "AP_Baro_PX4.h"
 #include "AP_Baro_qflight.h"
 #include "AP_Baro_QURT.h"
+#if HAL_WITH_UAVCAN
+#include "AP_Baro_UAVCAN.h"
+#endif
 
 #define C_TO_KELVIN 273.15f
 // Gas Constant is from Aerodynamics for Engineering Students, Third Edition, E.L.Houghton and N.B.Carruthers
@@ -474,6 +477,19 @@ void AP_Baro::init(void)
 #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_HAL::panic("Baro: unable to initialise driver");
     }

libraries/AP_Baro/AP_Baro_Backend.h

@@ -6,6 +6,7 @@ class AP_Baro_Backend
 {
 public:
     AP_Baro_Backend(AP_Baro &baro);
+    virtual ~AP_Baro_Backend(void) {};
 
     // each driver must provide an update method to copy accumulated
     // data to the frontend
@@ -16,6 +17,9 @@ public:
     // trigger them to read the sensor
     virtual void accumulate(void) {}
 
+    // callback for UAVCAN messages
+    virtual void handle_baro_msg(float pressure, float temperature) {}
+
 protected:
     // reference to frontend object
     AP_Baro &_frontend;

libraries/AP_Baro/AP_Baro_UAVCAN.cpp

@@ -0,0 +1,74 @@
+#include <AP_HAL/AP_HAL.h>
+
+#if HAL_WITH_UAVCAN
+
+#include "AP_Baro_UAVCAN.h"
+#include <AP_BoardConfig/AP_BoardConfig.h>
+
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <unistd.h>
+
+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.
+
+/*
+  constructor - registers instance at top Baro driver
+ */
+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);
+
+            debug_baro_uavcan(2, "AP_Baro_UAVCAN loaded\n\r");
+        }
+    }
+
+    _sem_baro = hal.util->new_semaphore();
+}
+
+AP_Baro_UAVCAN::~AP_Baro_UAVCAN()
+{
+    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");
+        }
+    }
+}
+
+// Read the sensor
+void AP_Baro_UAVCAN::update(void)
+{
+    if (_sem_baro->take(0)) {
+        _copy_to_frontend(_instance, _pressure, _temperature);
+
+        _frontend.set_external_temperature(_temperature);
+        _sem_baro->give();
+    }
+}
+
+void AP_Baro_UAVCAN::handle_baro_msg(float pressure, float temperature)
+{
+    if (_sem_baro->take(0)) {
+        _pressure = pressure;
+        _temperature = temperature - 273.15f;
+        _last_timestamp = AP_HAL::micros64();
+        _sem_baro->give();
+    }
+}
+
+#endif // HAL_WITH_UAVCAN
+

libraries/AP_Baro/AP_Baro_UAVCAN.h

@@ -0,0 +1,23 @@
+#pragma once
+
+#include "AP_Baro_Backend.h"
+#include <AP_UAVCAN/AP_UAVCAN.h>
+
+class AP_Baro_UAVCAN : public AP_Baro_Backend {
+public:
+    AP_Baro_UAVCAN(AP_Baro &);
+    ~AP_Baro_UAVCAN() override;
+
+    void update() override;
+
+    // This method is called from UAVCAN thread
+    virtual void handle_baro_msg(float pressure, float temperature) override;
+
+private:
+    uint8_t _instance;
+    float _pressure;
+    float _temperature;
+    uint64_t _last_timestamp;
+
+    AP_HAL::Semaphore *_sem_baro;
+};