From 14b701cff8c0dba549ba374b7c53d1cd754a1d4c Mon Sep 17 00:00:00 2001 From: Siddharth Purohit Date: Fri, 20 Jul 2018 16:48:00 +0530 Subject: [PATCH] AP_Compass: move UAVCAN mag subscribers and handlers to Compass Backend --- libraries/AP_Compass/AP_Compass.cpp | 11 +- libraries/AP_Compass/AP_Compass_Backend.h | 3 - libraries/AP_Compass/AP_Compass_UAVCAN.cpp | 283 ++++++++++++++------- libraries/AP_Compass/AP_Compass_UAVCAN.h | 46 +++- 4 files changed, 222 insertions(+), 121 deletions(-) diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp index 474de3c5e2..cdfe29527e 100644 --- a/libraries/AP_Compass/AP_Compass.cpp +++ b/libraries/AP_Compass/AP_Compass.cpp @@ -18,7 +18,6 @@ #include "AP_Compass_AK09916.h" #include "AP_Compass_QMC5883L.h" #if HAL_WITH_UAVCAN -#include #include "AP_Compass_UAVCAN.h" #endif #include "AP_Compass_MMC3416.h" @@ -1018,14 +1017,8 @@ void Compass::_detect_backends(void) #endif #if HAL_WITH_UAVCAN - if (_driver_enabled(DRIVER_UAVCAN)) { - bool added; - do { - added = _add_backend(AP_Compass_UAVCAN::probe(*this), "UAVCAN", true); - if (_backend_count == COMPASS_MAX_BACKEND || _compass_count == COMPASS_MAX_INSTANCES) { - return; - } - } while (added); + for (uint8_t i = 0; i < COMPASS_MAX_BACKEND; i++) { + ADD_BACKEND(DRIVER_UAVCAN, AP_Compass_UAVCAN::probe(*this), "UAVCAN", true); } #endif diff --git a/libraries/AP_Compass/AP_Compass_Backend.h b/libraries/AP_Compass/AP_Compass_Backend.h index 9d10dc0e1e..653fbacb6a 100644 --- a/libraries/AP_Compass/AP_Compass_Backend.h +++ b/libraries/AP_Compass/AP_Compass_Backend.h @@ -34,9 +34,6 @@ public: // read sensor data virtual void read(void) = 0; - // 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 diff --git a/libraries/AP_Compass/AP_Compass_UAVCAN.cpp b/libraries/AP_Compass/AP_Compass_UAVCAN.cpp index 6293680de3..66ddf3e0f4 100644 --- a/libraries/AP_Compass/AP_Compass_UAVCAN.cpp +++ b/libraries/AP_Compass/AP_Compass_UAVCAN.cpp @@ -20,126 +20,168 @@ #include "AP_Compass_UAVCAN.h" #include +#include +#include + +#include +#include extern const AP_HAL::HAL& hal; #define debug_mag_uavcan(level_debug, can_driver, fmt, args...) do { if ((level_debug) <= AP::can().get_debug_level_driver(can_driver)) { printf(fmt, ##args); }} while (0) + +// Frontend Registry Binders +UC_REGISTRY_BINDER(MagCb, uavcan::equipment::ahrs::MagneticFieldStrength); +UC_REGISTRY_BINDER(Mag2Cb, uavcan::equipment::ahrs::MagneticFieldStrength2); + +AP_Compass_UAVCAN::DetectedModules AP_Compass_UAVCAN::_detected_modules[] = {0}; +AP_HAL::Semaphore* AP_Compass_UAVCAN::_sem_registry = nullptr; + /* constructor - registers instance at top Compass driver */ -AP_Compass_UAVCAN::AP_Compass_UAVCAN(Compass &compass): - AP_Compass_Backend(compass) -{ - _sem_mag = hal.util->new_semaphore(); -} +AP_Compass_UAVCAN::AP_Compass_UAVCAN(Compass &compass, AP_UAVCAN* ap_uavcan, uint8_t node_id, uint8_t sensor_id): + AP_Compass_Backend(compass), + _ap_uavcan(ap_uavcan), + _node_id(node_id), + _sensor_id(sensor_id) +{} -AP_Compass_UAVCAN::~AP_Compass_UAVCAN() +void AP_Compass_UAVCAN::subscribe_msgs(AP_UAVCAN* ap_uavcan) { - if (!_initialized) { - return; - } - - AP_UAVCAN *ap_uavcan = AP_UAVCAN::get_uavcan(_manager); if (ap_uavcan == nullptr) { return; } - ap_uavcan->remove_mag_listener(this); - delete _sem_mag; + auto* node = ap_uavcan->get_node(); - debug_mag_uavcan(2, _manager, "AP_Compass_UAVCAN destructed\n\r"); + uavcan::Subscriber *mag_listener; + mag_listener = new uavcan::Subscriber(*node); + const int mag_listener_res = mag_listener->start(MagCb(ap_uavcan, &handle_magnetic_field)); + if (mag_listener_res < 0) { + AP_HAL::panic("UAVCAN Mag subscriber start problem\n\r"); + return; + } + + uavcan::Subscriber *mag2_listener; + mag2_listener = new uavcan::Subscriber(*node); + const int mag2_listener_res = mag2_listener->start(Mag2Cb(ap_uavcan, &handle_magnetic_field_2)); + if (mag2_listener_res < 0) { + AP_HAL::panic("UAVCAN Mag subscriber start problem\n\r"); + return; + } } -AP_Compass_Backend *AP_Compass_UAVCAN::probe(Compass &compass) +bool AP_Compass_UAVCAN::take_registry() { - uint8_t can_num_drivers = AP::can().get_num_drivers(); + if (_sem_registry == nullptr) { + _sem_registry = hal.util->new_semaphore(); + } + return _sem_registry->take(HAL_SEMAPHORE_BLOCK_FOREVER); +} - AP_Compass_UAVCAN *sensor; +void AP_Compass_UAVCAN::give_registry() +{ + _sem_registry->give(); +} - for (uint8_t i = 0; i < can_num_drivers; i++) { - AP_UAVCAN *ap_uavcan = AP_UAVCAN::get_uavcan(i); - if (ap_uavcan == nullptr) { - continue; +AP_Compass_Backend* AP_Compass_UAVCAN::probe(Compass& _frontend) +{ + if (!take_registry()) { + return nullptr; + } + AP_Compass_UAVCAN* driver = nullptr; + for (uint8_t i = 0; i < COMPASS_MAX_BACKEND; i++) { + if (!_detected_modules[i].driver && _detected_modules[i].ap_uavcan) { + // Register new Compass mode to a backend + driver = new AP_Compass_UAVCAN(_frontend, _detected_modules[i].ap_uavcan, _detected_modules[i].node_id, _detected_modules[i].sensor_id); + if (driver) { + _detected_modules[i].driver = driver; + driver->init(); + debug_mag_uavcan(2, + _detected_modules[i].ap_uavcan->get_driver_index(), + "Found Mag Node %d on Bus %d Sensor ID %d\n", + _detected_modules[i].node_id, + _detected_modules[i].ap_uavcan->get_driver_index(), + _detected_modules[i].sensor_id); + } + break; } - uint8_t freemag = ap_uavcan->find_smallest_free_mag_node(); - if (freemag == UINT8_MAX) { - continue; - } - sensor = new AP_Compass_UAVCAN(compass); + } + give_registry(); + return driver; +} - if (sensor->register_uavcan_compass(i, freemag)) { - debug_mag_uavcan(2, i, "AP_Compass_UAVCAN probed, drv: %d, node: %d\n\r", i, freemag); - return sensor; - } else { - delete sensor; +void AP_Compass_UAVCAN::init() +{ + _instance = register_compass(); + + 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 = _ap_uavcan->get_driver_index(); + d.devid_s.address = _node_id; + d.devid_s.devtype = 1; + + set_dev_id(_instance, d.devid); + set_external(_instance, true); + + _sum.zero(); + _count = 0; + + debug_mag_uavcan(2, _ap_uavcan->get_driver_index(), "AP_Compass_UAVCAN loaded\n\r"); +} + +AP_Compass_UAVCAN* AP_Compass_UAVCAN::get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t node_id, uint8_t sensor_id) +{ + if (ap_uavcan == nullptr) { + return nullptr; + } + for (uint8_t i=0; iregister_mag_listener_to_node(this, node)) { - _instance = register_compass(); - - 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 = mgr; - d.devid_s.address = node; - d.devid_s.devtype = 1; - - set_dev_id(_instance, d.devid); - set_external(_instance, true); - - _sum.zero(); - _count = 0; - - debug_mag_uavcan(2, mgr, "AP_Compass_UAVCAN loaded\n\r"); - - return true; - } - - return false; -} - -void AP_Compass_UAVCAN::read(void) -{ - // avoid division by zero if we haven't received any mag reports - if (_count == 0) { - return; - } - - if (_sem_mag->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { - _sum /= _count; - - publish_filtered_field(_sum, _instance); - - _sum.zero(); - _count = 0; - _sem_mag->give(); - } -} - -void AP_Compass_UAVCAN::handle_mag_msg(Vector3f &mag) +void AP_Compass_UAVCAN::handle_mag_msg(const Vector3f &mag) { Vector3f raw_field = mag * 1000.0; @@ -152,12 +194,59 @@ void AP_Compass_UAVCAN::handle_mag_msg(Vector3f &mag) // correct raw_field for known errors correct_field(raw_field, _instance); - if (_sem_mag->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { - // accumulate into averaging filter - _sum += raw_field; - _count++; - _sem_mag->give(); + WITH_SEMAPHORE(_sem_mag); + // accumulate into averaging filter + _sum += raw_field; + _count++; +} + +void AP_Compass_UAVCAN::handle_magnetic_field(AP_UAVCAN* ap_uavcan, uint8_t node_id, const MagCb &cb) +{ + if (take_registry()) { + Vector3f mag_vector; + AP_Compass_UAVCAN* driver = get_uavcan_backend(ap_uavcan, node_id, 0); + if (driver == nullptr) { + return; + } + mag_vector[0] = cb.msg->magnetic_field_ga[0]; + mag_vector[1] = cb.msg->magnetic_field_ga[1]; + mag_vector[2] = cb.msg->magnetic_field_ga[2]; + driver->handle_mag_msg(mag_vector); + give_registry(); } } +void AP_Compass_UAVCAN::handle_magnetic_field_2(AP_UAVCAN* ap_uavcan, uint8_t node_id, const Mag2Cb &cb) +{ + if (take_registry()) { + Vector3f mag_vector; + uint8_t sensor_id = cb.msg->sensor_id; + AP_Compass_UAVCAN* driver = get_uavcan_backend(ap_uavcan, node_id, sensor_id); + if (driver == nullptr) { + return; + } + mag_vector[0] = cb.msg->magnetic_field_ga[0]; + mag_vector[1] = cb.msg->magnetic_field_ga[1]; + mag_vector[2] = cb.msg->magnetic_field_ga[2]; + driver->handle_mag_msg(mag_vector); + give_registry(); + } +} + +void AP_Compass_UAVCAN::read(void) +{ + // avoid division by zero if we haven't received any mag reports + if (_count == 0) { + return; + } + + WITH_SEMAPHORE(_sem_mag); + _sum /= _count; + + publish_filtered_field(_sum, _instance); + + _sum.zero(); + _count = 0; +} + #endif diff --git a/libraries/AP_Compass/AP_Compass_UAVCAN.h b/libraries/AP_Compass/AP_Compass_UAVCAN.h index 4add83fc35..1922967411 100644 --- a/libraries/AP_Compass/AP_Compass_UAVCAN.h +++ b/libraries/AP_Compass/AP_Compass_UAVCAN.h @@ -5,28 +5,50 @@ #include +class MagCb; +class Mag2Cb; + class AP_Compass_UAVCAN : public AP_Compass_Backend { public: + AP_Compass_UAVCAN(Compass &compass, AP_UAVCAN* ap_uavcan, uint8_t node_id, uint8_t sensor_id); + void read(void) override; - AP_Compass_UAVCAN(Compass &compass); - ~AP_Compass_UAVCAN() override; + static void subscribe_msgs(AP_UAVCAN* ap_uavcan); + static AP_Compass_Backend* probe(Compass& _frontend); - static AP_Compass_Backend *probe(Compass &compass); - - bool register_uavcan_compass(uint8_t mgr, uint8_t node); - - // This method is called from UAVCAN thread - void handle_mag_msg(Vector3f &mag); + static void handle_magnetic_field(AP_UAVCAN* ap_uavcan, uint8_t node_id, const MagCb &cb); + static void handle_magnetic_field_2(AP_UAVCAN* ap_uavcan, uint8_t node_id, const Mag2Cb &cb); private: + void init(); + + // callback for UAVCAN messages + void handle_mag_msg(const Vector3f &mag); + + static bool take_registry(); + static void give_registry(); + static AP_Compass_UAVCAN* get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t node_id, uint8_t sensor_id); + uint8_t _instance; - int _mag_fd; + bool _initialized; + Vector3f _sum; uint32_t _count; - bool _initialized; - uint8_t _manager; + HAL_Semaphore _sem_mag; - AP_HAL::Semaphore *_sem_mag; + AP_UAVCAN* _ap_uavcan; + uint8_t _node_id; + uint8_t _sensor_id; + + // Module Detection Registry + static struct DetectedModules { + AP_UAVCAN* ap_uavcan; + uint8_t node_id; + uint8_t sensor_id; + AP_Compass_UAVCAN *driver; + } _detected_modules[COMPASS_MAX_BACKEND]; + + static AP_HAL::Semaphore *_sem_registry; };