From 3e044c7b8a067807f28baa023d5301f207a566b7 Mon Sep 17 00:00:00 2001 From: Eugene Shamaev Date: Sun, 2 Apr 2017 17:56:15 +0300 Subject: [PATCH] AP_Compass: support for UAVCAN connected magnetometers --- libraries/AP_Compass/AP_Compass.cpp | 15 +++ libraries/AP_Compass/AP_Compass_Backend.h | 13 +- libraries/AP_Compass/AP_Compass_UAVCAN.cpp | 134 +++++++++++++++++++++ libraries/AP_Compass/AP_Compass_UAVCAN.h | 26 ++++ 4 files changed, 183 insertions(+), 5 deletions(-) create mode 100644 libraries/AP_Compass/AP_Compass_UAVCAN.cpp create mode 100644 libraries/AP_Compass/AP_Compass_UAVCAN.h 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; +};