AP_Compass: support for UAVCAN connected magnetometers

libraries/AP_Compass/AP_Compass.cpp

@@ -18,6 +18,9 @@
 #include "AP_Compass_qflight.h"
 #include "AP_Compass_LIS3MDL.h"
 #include "AP_Compass_AK09916.h"
+#if HAL_WITH_UAVCAN
+#include "AP_Compass_UAVCAN.h"
+#endif
 #include "AP_Compass.h"
 
 extern AP_HAL::HAL& hal;
@@ -687,6 +690,18 @@ 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_Backend.h

@@ -38,6 +38,9 @@ public:
     // backends
     virtual void accumulate(void) {};
 
+    // callback for UAVCAN messages
+    virtual void handle_mag_msg(Vector3f &mag) {};
+
     /*
       device driver IDs. These are used to fill in the devtype field
       of the device ID, which shows up as COMPASS*ID* parameters to

libraries/AP_Compass/AP_Compass_UAVCAN.cpp

@@ -0,0 +1,134 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <AP_HAL/AP_HAL.h>
+
+#if HAL_WITH_UAVCAN
+
+#include "AP_Compass_UAVCAN.h"
+
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <unistd.h>
+
+#include <AP_BoardConfig/AP_BoardConfig.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.
+
+/*
+  constructor - registers instance at top Compass driver
+ */
+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 (hal.can_mgr != nullptr) {
+        AP_UAVCAN *ap_uavcan = hal.can_mgr->get_UAVCAN();
+        if (ap_uavcan != nullptr) {
+            ap_uavcan->remove_mag_listener(this);
+
+            debug_mag_uavcan(2, "AP_Compass_UAVCAN destructed\n\r");
+        }
+    }
+}
+
+void AP_Compass_UAVCAN::read(void)
+{
+    // avoid division by zero if we haven't received any mag reports
+    if (_count == 0) {
+        return;
+    }
+
+    if (_mag_baro->take(0)) {
+        _sum /= _count;
+
+        publish_filtered_field(_sum, _instance);
+
+        _sum.zero();
+        _count = 0;
+        _mag_baro->give();
+    }
+}
+
+void AP_Compass_UAVCAN::handle_mag_msg(Vector3f &mag)
+{
+    Vector3f raw_field = mag * 1000.0;
+
+    // rotate raw_field from sensor frame to body frame
+    rotate_field(raw_field, _instance);
+
+    _last_timestamp = AP_HAL::micros64();
+    // publish raw_field (uncorrected point sample) for calibration use
+    publish_raw_field(raw_field, (uint32_t) _last_timestamp, _instance);
+
+    // correct raw_field for known errors
+    correct_field(raw_field, _instance);
+
+    if (_mag_baro->take(0)) {
+        // accumulate into averaging filter
+        _sum += raw_field;
+        _count++;
+        _mag_baro->give();
+    }
+}
+
+#endif

libraries/AP_Compass/AP_Compass_UAVCAN.h

@@ -0,0 +1,26 @@
+#pragma once
+
+#include "AP_Compass.h"
+#include "AP_Compass_Backend.h"
+
+#include <AP_UAVCAN/AP_UAVCAN.h>
+
+class AP_Compass_UAVCAN : public AP_Compass_Backend {
+public:
+    void        read(void) override;
+
+    AP_Compass_UAVCAN(Compass &compass);
+    ~AP_Compass_UAVCAN() override;
+
+    // This method is called from UAVCAN thread
+    void handle_mag_msg(Vector3f &mag);
+
+private:
+    uint8_t  _instance;
+    int      _mag_fd;
+    Vector3f _sum;
+    uint32_t _count;
+    uint64_t _last_timestamp;
+
+    AP_HAL::Semaphore *_mag_baro;
+};