diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp
index 3733bde10a..53f2cb0010 100644
--- a/libraries/AP_Compass/AP_Compass.cpp
+++ b/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");
diff --git a/libraries/AP_Compass/AP_Compass_Backend.h b/libraries/AP_Compass/AP_Compass_Backend.h
index c2c0c91aec..104874129e 100644
--- a/libraries/AP_Compass/AP_Compass_Backend.h
+++ b/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
@@ -57,8 +60,8 @@ public:
DEVTYPE_AK09916 = 0x09,
DEVTYPE_IST8310 = 0x0A,
};
-
-
+
+
protected:
/*
@@ -94,13 +97,13 @@ protected:
// set rotation of an instance
void set_rotation(uint8_t instance, enum Rotation rotation);
-
+
// access to frontend
Compass &_compass;
// semaphore for access to shared frontend data
- AP_HAL::Semaphore *_sem;
-
+ AP_HAL::Semaphore *_sem;
+
private:
void apply_corrections(Vector3f &mag, uint8_t i);
};
diff --git a/libraries/AP_Compass/AP_Compass_UAVCAN.cpp b/libraries/AP_Compass/AP_Compass_UAVCAN.cpp
new file mode 100644
index 0000000000..6dd2ed65e2
--- /dev/null
+++ b/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 .
+ */
+
+#include
+
+#if HAL_WITH_UAVCAN
+
+#include "AP_Compass_UAVCAN.h"
+
+#include
+#include
+#include
+#include
+
+#include
+
+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
diff --git a/libraries/AP_Compass/AP_Compass_UAVCAN.h b/libraries/AP_Compass/AP_Compass_UAVCAN.h
new file mode 100644
index 0000000000..20872fd7fb
--- /dev/null
+++ b/libraries/AP_Compass/AP_Compass_UAVCAN.h
@@ -0,0 +1,26 @@
+#pragma once
+
+#include "AP_Compass.h"
+#include "AP_Compass_Backend.h"
+
+#include
+
+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;
+};