diff --git a/libraries/AP_Baro/AP_Baro_UAVCAN.cpp b/libraries/AP_Baro/AP_Baro_UAVCAN.cpp index eb6bc5fa9ca48..b976006e5a860 100644 --- a/libraries/AP_Baro/AP_Baro_UAVCAN.cpp +++ b/libraries/AP_Baro/AP_Baro_UAVCAN.cpp @@ -3,19 +3,12 @@ #if HAL_WITH_UAVCAN #include "AP_Baro_UAVCAN.h" -#include -#include -#if HAL_OS_POSIX_IO -#include -#include -#include -#include -#endif +#include extern const AP_HAL::HAL& hal; -#define debug_baro_uavcan(level, fmt, args...) do { if ((level) <= AP_BoardConfig_CAN::get_can_debug()) { printf(fmt, ##args); }} while (0) +#define debug_baro_uavcan(level_debug, can_driver, fmt, args...) do { if ((level_debug) <= AP::can().get_debug_level_driver(can_driver)) { printf(fmt, ##args); }} while (0) /* constructor - registers instance at top Baro driver @@ -31,39 +24,36 @@ AP_Baro_UAVCAN::~AP_Baro_UAVCAN() if (!_initialized) { return; } - + AP_UAVCAN *ap_uavcan = AP_UAVCAN::get_uavcan(_manager); if (ap_uavcan == nullptr) { return; } - + ap_uavcan->remove_baro_listener(this); delete _sem_baro; - - debug_baro_uavcan(2, "AP_Baro_UAVCAN destructed\n\r"); + + debug_baro_uavcan(2, _manager, "AP_Baro_UAVCAN destructed\n\r"); } AP_Baro_Backend *AP_Baro_UAVCAN::probe(AP_Baro &baro) { - - if (AP_BoardConfig_CAN::get_can_num_ifaces() == 0) { - return nullptr; - } + uint8_t can_num_drivers = AP::can().get_num_drivers(); AP_Baro_UAVCAN *sensor; - for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_DRIVERS; i++) { + for (uint8_t i = 0; i < can_num_drivers; i++) { AP_UAVCAN *ap_uavcan = AP_UAVCAN::get_uavcan(i); if (ap_uavcan == nullptr) { continue; } - + uint8_t freebaro = ap_uavcan->find_smallest_free_baro_node(); if (freebaro == UINT8_MAX) { continue; } sensor = new AP_Baro_UAVCAN(baro); if (sensor->register_uavcan_baro(i, freebaro)) { - debug_baro_uavcan(2, "AP_Baro_UAVCAN probed, drv: %d, node: %d\n\r", i, freebaro); + debug_baro_uavcan(2, i, "AP_Baro_UAVCAN probed, drv: %d, node: %d\n\r", i, freebaro); return sensor; } else { delete sensor; @@ -104,7 +94,7 @@ bool AP_Baro_UAVCAN::register_uavcan_baro(uint8_t mgr, uint8_t node) if (ap_uavcan->register_baro_listener_to_node(this, node)) { _instance = _frontend.register_sensor(); - debug_baro_uavcan(2, "AP_Baro_UAVCAN loaded\n\r"); + debug_baro_uavcan(2, mgr, "AP_Baro_UAVCAN loaded\n\r"); _initialized = true; diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp index 3706da099d34a..8acd82c89a23d 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp @@ -7,12 +7,11 @@ #include "AP_BattMonitor.h" #include "AP_BattMonitor_UAVCAN.h" -#include #include extern const AP_HAL::HAL& hal; -#define debug_bm_uavcan(level, fmt, args...) do { if ((level) <= AP_BoardConfig_CAN::get_can_debug()) { printf(fmt, ##args); }} while (0) +#define debug_bm_uavcan(level_debug, can_driver, fmt, args...) do { if ((level_debug) <= AP::can().get_debug_level_driver(can_driver)) { printf(fmt, ##args); }} while (0) /// Constructor AP_BattMonitor_UAVCAN::AP_BattMonitor_UAVCAN(AP_BattMonitor &mon, AP_BattMonitor::BattMonitor_State &mon_state, BattMonitor_UAVCAN_Type type, AP_BattMonitor_Params ¶ms) : @@ -25,20 +24,18 @@ AP_BattMonitor_UAVCAN::AP_BattMonitor_UAVCAN(AP_BattMonitor &mon, AP_BattMonitor void AP_BattMonitor_UAVCAN::init() { - if (AP_BoardConfig_CAN::get_can_num_ifaces() == 0) { - return; - } + uint8_t can_num_drivers = AP::can().get_num_drivers(); - for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_DRIVERS; i++) { + for (uint8_t i = 0; i < can_num_drivers; i++) { AP_UAVCAN *ap_uavcan = AP_UAVCAN::get_uavcan(i); if (ap_uavcan == nullptr) { continue; } - + switch (_type) { case UAVCAN_BATTERY_INFO: if (ap_uavcan->register_BM_bi_listener_to_id(this, _params._serial_number)) { - debug_bm_uavcan(2, "UAVCAN BattMonitor BatteryInfo registered id: %d\n\r", _params._serial_number); + debug_bm_uavcan(2, i, "UAVCAN BattMonitor BatteryInfo registered id: %d\n\r", _params._serial_number.get()); } break; } diff --git a/libraries/AP_BoardConfig/AP_BoardConfig_CAN.cpp b/libraries/AP_BoardConfig/AP_BoardConfig_CAN.cpp index 1b361fbd8f155..9483491b343d8 100644 --- a/libraries/AP_BoardConfig/AP_BoardConfig_CAN.cpp +++ b/libraries/AP_BoardConfig/AP_BoardConfig_CAN.cpp @@ -44,64 +44,63 @@ const AP_Param::GroupInfo AP_BoardConfig_CAN::var_info[] = { #if MAX_NUMBER_OF_CAN_INTERFACES > 0 // @Group: P1_ - // @Path: ../AP_BoardConfig/canbus.cpp - AP_SUBGROUPINFO(_var_info_can[0], "P1_", 1, AP_BoardConfig_CAN, AP_BoardConfig_CAN::CAN_if_var_info), + // @Path: ../AP_BoardConfig/canbus_interface.cpp + AP_SUBGROUPINFO(_interfaces[0], "P1_", 1, AP_BoardConfig_CAN, AP_BoardConfig_CAN::Interface), #endif #if MAX_NUMBER_OF_CAN_INTERFACES > 1 // @Group: P2_ - // @Path: ../AP_BoardConfig/canbus.cpp - AP_SUBGROUPINFO(_var_info_can[1], "P2_", 2, AP_BoardConfig_CAN, AP_BoardConfig_CAN::CAN_if_var_info), + // @Path: ../AP_BoardConfig/canbus_interface.cpp + AP_SUBGROUPINFO(_interfaces[1], "P2_", 2, AP_BoardConfig_CAN, AP_BoardConfig_CAN::Interface), #endif #if MAX_NUMBER_OF_CAN_INTERFACES > 2 // @Group: P3_ - // @Path: ../AP_BoardConfig/canbus.cpp - AP_SUBGROUPINFO(_var_info_can[2], "P3_", 3, AP_BoardConfig_CAN, AP_BoardConfig_CAN::CAN_if_var_info), + // @Path: ../AP_BoardConfig/canbus_interface.cpp + AP_SUBGROUPINFO(_interfaces[2], "P3_", 3, AP_BoardConfig_CAN, AP_BoardConfig_CAN::Interface), #endif #if MAX_NUMBER_OF_CAN_DRIVERS > 0 // @Group: D1_ // @Path: ../AP_BoardConfig/canbus_driver.cpp - AP_SUBGROUPINFO(_var_info_can_protocol[0], "D1_", 4, AP_BoardConfig_CAN, AP_BoardConfig_CAN::CAN_driver_var_info), + AP_SUBGROUPINFO(_drivers[0], "D1_", 4, AP_BoardConfig_CAN, AP_BoardConfig_CAN::Driver), #endif #if MAX_NUMBER_OF_CAN_DRIVERS > 1 // @Group: D2_ // @Path: ../AP_BoardConfig/canbus_driver.cpp - AP_SUBGROUPINFO(_var_info_can_protocol[1], "D2_", 5, AP_BoardConfig_CAN, AP_BoardConfig_CAN::CAN_driver_var_info), + AP_SUBGROUPINFO(_drivers[1], "D2_", 5, AP_BoardConfig_CAN, AP_BoardConfig_CAN::Driver), #endif #if MAX_NUMBER_OF_CAN_DRIVERS > 2 // @Group: D3_ // @Path: ../AP_BoardConfig/canbus_driver.cpp - AP_SUBGROUPINFO(_var_info_can_protocol[2], "D3_", 6, AP_BoardConfig_CAN, AP_BoardConfig_CAN::CAN_driver_var_info), + AP_SUBGROUPINFO(_drivers[2], "D3_", 6, AP_BoardConfig_CAN, AP_BoardConfig_CAN::Driver), #endif AP_GROUPEND }; -int8_t AP_BoardConfig_CAN::_st_driver_number[MAX_NUMBER_OF_CAN_INTERFACES]; -int8_t AP_BoardConfig_CAN::_st_can_debug[MAX_NUMBER_OF_CAN_INTERFACES]; +AP_BoardConfig_CAN *AP_BoardConfig_CAN::_singleton; -void AP_BoardConfig_CAN::init() +AP_BoardConfig_CAN::AP_BoardConfig_CAN() { - for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_INTERFACES; i++) - { - _st_driver_number[i] = (int8_t) _var_info_can[i]._driver_number; - _st_can_debug[i] = (int8_t) _var_info_can[i]._can_debug; + AP_Param::setup_object_defaults(this, var_info); +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + if (_singleton != nullptr) { + AP_HAL::panic("AP_BoardConfig_CAN must be singleton"); } - - setup_canbus(); +#endif // CONFIG_HAL_BOARD == HAL_BOARD_SITL + _singleton = this; } -void AP_BoardConfig_CAN::setup_canbus(void) +void AP_BoardConfig_CAN::init() { // Create all drivers that we need bool initret = true; for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_INTERFACES; i++) { // Check the driver number assigned to this physical interface - uint8_t drv_num = _var_info_can[i]._driver_number; + uint8_t drv_num = _interfaces[i]._driver_number_cache = _interfaces[i]._driver_number; if (drv_num != 0 && drv_num <= MAX_NUMBER_OF_CAN_DRIVERS) { if (hal.can_mgr[drv_num - 1] == nullptr) { @@ -118,52 +117,43 @@ void AP_BoardConfig_CAN::setup_canbus(void) // For this now existing driver (manager), start the physical interface if (hal.can_mgr[drv_num - 1] != nullptr) { - initret &= hal.can_mgr[drv_num - 1]->begin(_var_info_can[i]._can_bitrate, i); + initret = initret && hal.can_mgr[drv_num - 1]->begin(_interfaces[i]._bitrate, i); } else { printf("Failed to initialize can interface %d\n\r", i + 1); } } } - bool any_uavcan_present = false; - if (initret) { for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_DRIVERS; i++) { + Protocol_Type prot_type = _drivers[i]._protocol_type_cache = (Protocol_Type) _drivers[i]._protocol_type.get(); + if (hal.can_mgr[i] == nullptr) { continue; } + + _num_drivers = i + 1; hal.can_mgr[i]->initialized(true); printf("can_mgr %d initialized well\n\r", i + 1); - if (_var_info_can_protocol[i]._protocol == UAVCAN_PROTOCOL_ENABLE) { - _var_info_can_protocol[i]._uavcan = new AP_UAVCAN; + if (prot_type == Protocol_Type_UAVCAN) { + _drivers[i]._driver = new AP_UAVCAN; - if (_var_info_can_protocol[i]._uavcan == nullptr) { + if (_drivers[i]._driver == nullptr) { AP_HAL::panic("Failed to allocate uavcan %d\n\r", i + 1); continue; } - - AP_Param::load_object_from_eeprom(_var_info_can_protocol[i]._uavcan, AP_UAVCAN::var_info); - hal.can_mgr[i]->set_UAVCAN(_var_info_can_protocol[i]._uavcan); - _var_info_can_protocol[i]._uavcan->set_parent_can_mgr(hal.can_mgr[i]); + AP_Param::load_object_from_eeprom(_drivers[i]._driver, AP_UAVCAN::var_info); - if (_var_info_can_protocol[i]._uavcan->try_init() == true) { - any_uavcan_present = true; - } else { - printf("Failed to initialize uavcan interface %d\n\r", i + 1); - } + _drivers[i]._driver->init(i); } } - - if (any_uavcan_present) { - // start UAVCAN working thread - hal.scheduler->create_uavcan_thread(); - - // Delay for magnetometer and barometer discovery - hal.scheduler->delay(5000); - } } } + +AP_BoardConfig_CAN& AP::can() { + return *AP_BoardConfig_CAN::get_singleton(); +} #endif diff --git a/libraries/AP_BoardConfig/AP_BoardConfig_CAN.h b/libraries/AP_BoardConfig/AP_BoardConfig_CAN.h index bb382616ba197..0cd7f9da89347 100644 --- a/libraries/AP_BoardConfig/AP_BoardConfig_CAN.h +++ b/libraries/AP_BoardConfig/AP_BoardConfig_CAN.h @@ -1,35 +1,97 @@ #pragma once #include -#include "AP_BoardConfig.h" -#include + +#if HAL_WITH_UAVCAN + #include -#if CONFIG_HAL_BOARD != HAL_BOARD_CHIBIOS //we don't have ioctls in ChibiOS -#include + +#ifndef AP_CAN_DEBUG + #define AP_CAN_DEBUG 0 #endif -#if HAL_WITH_UAVCAN -#define UAVCAN_PROTOCOL_ENABLE 1 class AP_BoardConfig_CAN { public: - AP_BoardConfig_CAN() { - AP_Param::setup_object_defaults(this, var_info); - }; + AP_BoardConfig_CAN(); /* Do not allow copies */ AP_BoardConfig_CAN(const AP_BoardConfig_CAN &other) = delete; AP_BoardConfig_CAN &operator=(const AP_BoardConfig_CAN&) = delete; + static AP_BoardConfig_CAN* get_singleton() { + return _singleton; + } + + enum Protocol_Type : uint8_t { + Protocol_Type_None = 0, + Protocol_Type_UAVCAN = 1, + }; + void init(void); + // returns number of active CAN drivers + uint8_t get_num_drivers(void) { return _num_drivers; } + + // return debug level for interface i + uint8_t get_debug_level(uint8_t i) { +#if AP_CAN_DEBUG + if (i < MAX_NUMBER_OF_CAN_INTERFACES) { + return _interfaces[i]._driver_number_cache ? _interfaces[i]._debug_level : 0; + } +#endif + return 0; + } + + // return maximum level of debug of all interfaces + uint8_t get_debug_level(void) { + uint8_t ret = 0; +#if AP_CAN_DEBUG + for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_INTERFACES; i++) { + uint8_t dbg = get_debug_level(i); + ret = (dbg > ret) ? dbg : ret; + } +#endif + return ret; + } + + // return maximum level of debug for driver index i + uint8_t get_debug_level_driver(uint8_t i) { + uint8_t ret = 0; +#if AP_CAN_DEBUG + for (uint8_t j = 0; j < MAX_NUMBER_OF_CAN_INTERFACES; j++) { + if (_interfaces[j]._driver_number_cache == i) { + uint8_t dbg = get_debug_level(j); + ret = (dbg > ret) ? dbg : ret; + } + } +#endif + return ret; + } + + // return driver for index i + AP_HAL::CANProtocol* get_driver(uint8_t i) { + if (i < MAX_NUMBER_OF_CAN_DRIVERS) { + return _drivers[i]._driver; + } + return nullptr; + } + + // return protocol type index i + Protocol_Type get_protocol_type(uint8_t i) { + if (i < MAX_NUMBER_OF_CAN_DRIVERS) { + return _drivers[i]._protocol_type_cache; + } + return Protocol_Type_None; + } + static const struct AP_Param::GroupInfo var_info[]; - class CAN_if_var_info { +private: + class Interface { friend class AP_BoardConfig_CAN; public: - CAN_if_var_info() - { + Interface() { AP_Param::setup_object_defaults(this, var_info); } @@ -37,67 +99,37 @@ class AP_BoardConfig_CAN { private: AP_Int8 _driver_number; - AP_Int8 _can_debug; - AP_Int32 _can_bitrate; + uint8_t _driver_number_cache; + AP_Int32 _bitrate; +#if AP_CAN_DEBUG + AP_Int8 _debug_level; +#endif }; - class CAN_driver_var_info { + class Driver { friend class AP_BoardConfig_CAN; public: - CAN_driver_var_info() : - _uavcan(nullptr) - { + Driver() { AP_Param::setup_object_defaults(this, var_info); } + static const struct AP_Param::GroupInfo var_info[]; private: - AP_Int8 _protocol; - - AP_UAVCAN* _uavcan; + AP_Int8 _protocol_type; + Protocol_Type _protocol_type_cache; + AP_HAL::CANProtocol* _driver; }; - // returns number of enabled CAN interfaces - static int8_t get_can_num_ifaces(void) - { - uint8_t ret = 0; - - for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_INTERFACES; i++) { - if (_st_driver_number[i]) { - ret++; - } - } - - return ret; - } - - static int8_t get_can_debug(uint8_t i) - { - if (i < MAX_NUMBER_OF_CAN_INTERFACES) { - return _st_can_debug[i]; - } - return 0; - } - - // return maximum level of debug - static int8_t get_can_debug(void) - { - uint8_t ret = 0; - for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_INTERFACES; i++) { - uint8_t dbg = get_can_debug(i); - ret = (dbg > ret) ? dbg : ret; - } - return ret; - } - - CAN_if_var_info _var_info_can[MAX_NUMBER_OF_CAN_INTERFACES]; - CAN_driver_var_info _var_info_can_protocol[MAX_NUMBER_OF_CAN_DRIVERS]; - - static int8_t _st_driver_number[MAX_NUMBER_OF_CAN_INTERFACES]; - static int8_t _st_can_debug[MAX_NUMBER_OF_CAN_INTERFACES]; - - void setup_canbus(void); + Interface _interfaces[MAX_NUMBER_OF_CAN_INTERFACES]; + Driver _drivers[MAX_NUMBER_OF_CAN_DRIVERS]; + uint8_t _num_drivers; + static AP_BoardConfig_CAN *_singleton; }; + +namespace AP { + AP_BoardConfig_CAN& can(); +} #endif diff --git a/libraries/AP_BoardConfig/canbus_driver.cpp b/libraries/AP_BoardConfig/canbus_driver.cpp index a22b11ba9fc18..49b35a538c4d9 100644 --- a/libraries/AP_BoardConfig/canbus_driver.cpp +++ b/libraries/AP_BoardConfig/canbus_driver.cpp @@ -14,26 +14,24 @@ */ #include -#include -#include "AP_BoardConfig.h" #if HAL_WITH_UAVCAN #include "AP_BoardConfig_CAN.h" #include // table of user settable CAN bus parameters -const AP_Param::GroupInfo AP_BoardConfig_CAN::CAN_driver_var_info::var_info[] = { +const AP_Param::GroupInfo AP_BoardConfig_CAN::Driver::var_info[] = { // @Param: PROTOCOL // @DisplayName: Enable use of specific protocol over virtual driver // @Description: Enabling this option starts selected protocol that will use this virtual driver // @Values: 0:Disabled,1:UAVCAN // @User: Advanced // @RebootRequired: True - AP_GROUPINFO("PROTOCOL", 1, AP_BoardConfig_CAN::CAN_driver_var_info, _protocol, UAVCAN_PROTOCOL_ENABLE), + AP_GROUPINFO("PROTOCOL", 1, AP_BoardConfig_CAN::Driver, _protocol_type, AP_BoardConfig_CAN::Protocol_Type_UAVCAN), // @Group: UC_ // @Path: ../AP_UAVCAN/AP_UAVCAN.cpp - AP_SUBGROUPPTR(_uavcan, "UC_", 2, AP_BoardConfig_CAN::CAN_driver_var_info, AP_UAVCAN), + AP_SUBGROUPPTR(_driver, "UC_", 2, AP_BoardConfig_CAN::Driver, AP_UAVCAN), AP_GROUPEND }; diff --git a/libraries/AP_BoardConfig/canbus.cpp b/libraries/AP_BoardConfig/canbus_interface.cpp similarity index 75% rename from libraries/AP_BoardConfig/canbus.cpp rename to libraries/AP_BoardConfig/canbus_interface.cpp index c90cc5042224d..e80060b2ba05c 100644 --- a/libraries/AP_BoardConfig/canbus.cpp +++ b/libraries/AP_BoardConfig/canbus_interface.cpp @@ -14,36 +14,35 @@ */ #include -#include -#include "AP_BoardConfig.h" #if HAL_WITH_UAVCAN #include "AP_BoardConfig_CAN.h" -#include // table of user settable CAN bus parameters -const AP_Param::GroupInfo AP_BoardConfig_CAN::CAN_if_var_info::var_info[] = { +const AP_Param::GroupInfo AP_BoardConfig_CAN::Interface::var_info[] = { // @Param: DRIVER // @DisplayName: Index of virtual driver to be used with physical CAN interface // @Description: Enabling this option enables use of CAN buses. // @Values: 0:Disabled,1:First driver,2:Second driver // @User: Standard // @RebootRequired: True - AP_GROUPINFO_FLAGS("DRIVER", 1, AP_BoardConfig_CAN::CAN_if_var_info, _driver_number, HAL_CAN_DRIVER_DEFAULT, AP_PARAM_FLAG_ENABLE), + AP_GROUPINFO_FLAGS("DRIVER", 1, AP_BoardConfig_CAN::Interface, _driver_number, HAL_CAN_DRIVER_DEFAULT, AP_PARAM_FLAG_ENABLE), // @Param: BITRATE // @DisplayName: Bitrate of CAN interface // @Description: Bit rate can be set up to from 10000 to 1000000 // @Range: 10000 1000000 // @User: Advanced - AP_GROUPINFO("BITRATE", 2, AP_BoardConfig_CAN::CAN_if_var_info, _can_bitrate, 1000000), + AP_GROUPINFO("BITRATE", 2, AP_BoardConfig_CAN::Interface, _bitrate, 1000000), +#if AP_CAN_DEBUG // @Param: DEBUG // @DisplayName: Level of debug for CAN devices // @Description: Enabling this option will provide debug messages // @Values: 0:Disabled,1:Major messages,2:All messages // @User: Advanced - AP_GROUPINFO("DEBUG", 3, AP_BoardConfig_CAN::CAN_if_var_info, _can_debug, 2), + AP_GROUPINFO("DEBUG", 3, AP_BoardConfig_CAN::Interface, _debug_level, 1), +#endif AP_GROUPEND }; diff --git a/libraries/AP_Compass/AP_Compass_UAVCAN.cpp b/libraries/AP_Compass/AP_Compass_UAVCAN.cpp index 815415450cf0f..6293680de3063 100644 --- a/libraries/AP_Compass/AP_Compass_UAVCAN.cpp +++ b/libraries/AP_Compass/AP_Compass_UAVCAN.cpp @@ -19,19 +19,11 @@ #include "AP_Compass_UAVCAN.h" -#if HAL_OS_POSIX_IO -#include -#include -#include -#include -#endif - -#include #include extern const AP_HAL::HAL& hal; -#define debug_mag_uavcan(level, fmt, args...) do { if ((level) <= AP_BoardConfig_CAN::get_can_debug()) { printf(fmt, ##args); }} while (0) +#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) /* constructor - registers instance at top Compass driver @@ -47,27 +39,25 @@ AP_Compass_UAVCAN::~AP_Compass_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; - - debug_mag_uavcan(2, "AP_Compass_UAVCAN destructed\n\r"); + + debug_mag_uavcan(2, _manager, "AP_Compass_UAVCAN destructed\n\r"); } AP_Compass_Backend *AP_Compass_UAVCAN::probe(Compass &compass) { - if (AP_BoardConfig_CAN::get_can_num_ifaces() == 0) { - return nullptr; - } + uint8_t can_num_drivers = AP::can().get_num_drivers(); AP_Compass_UAVCAN *sensor; - for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_DRIVERS; i++) { + for (uint8_t i = 0; i < can_num_drivers; i++) { AP_UAVCAN *ap_uavcan = AP_UAVCAN::get_uavcan(i); if (ap_uavcan == nullptr) { continue; @@ -77,9 +67,9 @@ AP_Compass_Backend *AP_Compass_UAVCAN::probe(Compass &compass) continue; } sensor = new AP_Compass_UAVCAN(compass); - + if (sensor->register_uavcan_compass(i, freemag)) { - debug_mag_uavcan(2, "AP_Compass_UAVCAN probed, drv: %d, node: %d\n\r", i, freemag); + debug_mag_uavcan(2, i, "AP_Compass_UAVCAN probed, drv: %d, node: %d\n\r", i, freemag); return sensor; } else { delete sensor; @@ -123,11 +113,11 @@ bool AP_Compass_UAVCAN::register_uavcan_compass(uint8_t mgr, uint8_t node) _sum.zero(); _count = 0; - debug_mag_uavcan(2, "AP_Compass_UAVCAN loaded\n\r"); + debug_mag_uavcan(2, mgr, "AP_Compass_UAVCAN loaded\n\r"); return true; } - + return false; } diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 4b51f97cdbaaf..6a807e86ae1a5 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -418,25 +418,26 @@ void AP_GPS::detect_instance(uint8_t instance) switch (_type[instance]) { // user has to explicitly set the MAV type, do not use AUTO // do not try to detect the MAV type, assume it's there - case GPS_TYPE_MAV: + case GPS_TYPE_MAV: { dstate->auto_detected_baud = false; // specified, not detected new_gps = new AP_GPS_MAV(*this, state[instance], nullptr); goto found_gps; break; + } -#if HAL_WITH_UAVCAN // user has to explicitly set the UAVCAN type, do not use AUTO - case GPS_TYPE_UAVCAN: + case GPS_TYPE_UAVCAN: { +#if HAL_WITH_UAVCAN dstate->auto_detected_baud = false; // specified, not detected - if (AP_BoardConfig_CAN::get_can_num_ifaces() == 0) { - return; - } - for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_DRIVERS; i++) { + + uint8_t can_num_drivers = AP::can().get_num_drivers(); + + for (uint8_t i = 0; i < can_num_drivers; i++) { AP_UAVCAN *ap_uavcan = AP_UAVCAN::get_uavcan(i); if (ap_uavcan == nullptr) { continue; } - + uint8_t gps_node = ap_uavcan->find_gps_without_listener(); if (gps_node == UINT8_MAX) { continue; @@ -445,7 +446,7 @@ void AP_GPS::detect_instance(uint8_t instance) new_gps = new AP_GPS_UAVCAN(*this, state[instance], nullptr); ((AP_GPS_UAVCAN*) new_gps)->set_uavcan_manager(i); if (ap_uavcan->register_gps_listener_to_node(new_gps, gps_node)) { - if (AP_BoardConfig_CAN::get_can_debug() >= 2) { + if (AP::can().get_debug_level_driver(i) >= 2) { printf("AP_GPS_UAVCAN registered\n\r"); } goto found_gps; @@ -453,8 +454,9 @@ void AP_GPS::detect_instance(uint8_t instance) delete new_gps; } } - return; #endif + return; + } default: break; diff --git a/libraries/AP_GPS/AP_GPS_UAVCAN.cpp b/libraries/AP_GPS/AP_GPS_UAVCAN.cpp index 363271d9a13a0..6e0c727a3f8e2 100644 --- a/libraries/AP_GPS/AP_GPS_UAVCAN.cpp +++ b/libraries/AP_GPS/AP_GPS_UAVCAN.cpp @@ -21,12 +21,12 @@ #if HAL_WITH_UAVCAN #include -#include + #include extern const AP_HAL::HAL& hal; -#define debug_gps_uavcan(level, fmt, args...) do { if ((level) <= AP_BoardConfig_CAN::get_can_debug()) { printf(fmt, ##args); }} while (0) +#define debug_gps_uavcan(level_debug, can_driver, fmt, args...) do { if ((level_debug) <= AP::can().get_debug_level_driver(can_driver)) { printf(fmt, ##args); }} while (0) AP_GPS_UAVCAN::AP_GPS_UAVCAN(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port) : AP_GPS_Backend(_gps, _state, _port) @@ -41,11 +41,11 @@ AP_GPS_UAVCAN::~AP_GPS_UAVCAN() if (ap_uavcan == nullptr) { return; } - + ap_uavcan->remove_gps_listener(this); delete _sem_gnss; - - debug_gps_uavcan(2, "AP_GPS_UAVCAN destructed\n\r"); + + debug_gps_uavcan(2, _manager, "AP_GPS_UAVCAN destructed\n\r"); } void AP_GPS_UAVCAN::set_uavcan_manager(uint8_t mgr) diff --git a/libraries/AP_HAL/AP_HAL_Namespace.h b/libraries/AP_HAL/AP_HAL_Namespace.h index 158e20c8da9a4..464213b606a2b 100644 --- a/libraries/AP_HAL/AP_HAL_Namespace.h +++ b/libraries/AP_HAL/AP_HAL_Namespace.h @@ -29,6 +29,7 @@ namespace AP_HAL { class Semaphore; class OpticalFlow; + class CANProtocol; class CANManager; class CAN; diff --git a/libraries/AP_HAL/CAN.h b/libraries/AP_HAL/CAN.h index 663bf15fd37e7..917d7ba46f30f 100644 --- a/libraries/AP_HAL/CAN.h +++ b/libraries/AP_HAL/CAN.h @@ -32,7 +32,18 @@ #define MAX_NUMBER_OF_CAN_INTERFACES 2 #define MAX_NUMBER_OF_CAN_DRIVERS 2 -class AP_UAVCAN; +/** + * Interface that CAN protocols need to implement + */ +class AP_HAL::CANProtocol { +public: + /* method called when initializing the CAN interfaces + * + * if initialization doesn't have errors, protocol class + * should create a thread to do send and receive operations + */ + virtual void init(uint8_t driver_index) = 0; +}; /** * Single non-blocking CAN interface. @@ -115,8 +126,6 @@ class AP_HAL::CANManager { virtual bool is_initialized() = 0; virtual void initialized(bool val) = 0; - virtual AP_UAVCAN *get_UAVCAN(void) = 0; - virtual void set_UAVCAN(AP_UAVCAN *uavcan) = 0; uavcan::ICanDriver* get_driver() { return _driver; } private: uavcan::ICanDriver* _driver; diff --git a/libraries/AP_HAL/Scheduler.h b/libraries/AP_HAL/Scheduler.h index 77a47b0ef473e..76c51d3660014 100644 --- a/libraries/AP_HAL/Scheduler.h +++ b/libraries/AP_HAL/Scheduler.h @@ -63,8 +63,6 @@ class AP_HAL::Scheduler { virtual bool in_main_thread() const = 0; - virtual void create_uavcan_thread() {}; - /* disable interrupts and return a context that can be used to restore the interrupt state. This can be used to protect diff --git a/libraries/AP_HAL_ChibiOS/CAN.cpp b/libraries/AP_HAL_ChibiOS/CAN.cpp index 2cca664a173b6..fd6a19aa0c4ac 100644 --- a/libraries/AP_HAL_ChibiOS/CAN.cpp +++ b/libraries/AP_HAL_ChibiOS/CAN.cpp @@ -43,11 +43,7 @@ uavcan::MonotonicTime clock::getMonotonic() bool CANManager::begin(uint32_t bitrate, uint8_t can_number) { - if (can_helper.init(bitrate, CanIface::OperatingMode::NormalMode, can_number) == 0) { - bitrate_ = bitrate; - initialized_ = true; - } - return initialized_; + return (can_helper.init(bitrate, CanIface::OperatingMode::NormalMode, can_number) == 0); } bool CANManager::is_initialized() @@ -60,23 +56,4 @@ void CANManager::initialized(bool val) initialized_ = val; } -AP_UAVCAN *CANManager::get_UAVCAN(void) -{ - return p_uavcan; -} - -void CANManager::set_UAVCAN(AP_UAVCAN *uavcan) -{ - p_uavcan = uavcan; -} - -void CANManager::_timer_tick() -{ - if (!initialized_) return; - - if (p_uavcan != nullptr) { - p_uavcan->do_cyclic(); - } -} - #endif //HAL_WITH_UAVCAN diff --git a/libraries/AP_HAL_ChibiOS/CAN.h b/libraries/AP_HAL_ChibiOS/CAN.h index 7f69839850180..04d49cbf57c9f 100644 --- a/libraries/AP_HAL_ChibiOS/CAN.h +++ b/libraries/AP_HAL_ChibiOS/CAN.h @@ -21,7 +21,7 @@ #if HAL_WITH_UAVCAN -#define UAVCAN_STM32_LOG(fmt, ...) hal.console->printf("CANManager: " fmt "\n", ##__VA_ARGS__) +#define CAN_STM32_LOG(fmt, ...) hal.console->printf("CANManager: " fmt "\n", ##__VA_ARGS__) #include #include @@ -30,12 +30,9 @@ #include #include -#include - #define MAX_NUMBER_OF_CAN_INTERFACES 2 #define MAX_NUMBER_OF_CAN_DRIVERS 2 #define CAN_STM32_RX_QUEUE_SIZE 64 -class AP_UAVCAN; namespace ChibiOS { /** @@ -67,14 +64,8 @@ class CANManager: public AP_HAL::CANManager { bool is_initialized() override; void initialized(bool val) override; - AP_UAVCAN *get_UAVCAN(void) override; - void set_UAVCAN(AP_UAVCAN *uavcan) override; - void _timer_tick(); - private: - AP_UAVCAN *p_uavcan; bool initialized_; - uint32_t bitrate_; uavcan_stm32::CanInitHelper can_helper; }; diff --git a/libraries/AP_HAL_ChibiOS/Scheduler.cpp b/libraries/AP_HAL_ChibiOS/Scheduler.cpp index 6f0f5aeb2662e..eacf2477f1880 100644 --- a/libraries/AP_HAL_ChibiOS/Scheduler.cpp +++ b/libraries/AP_HAL_ChibiOS/Scheduler.cpp @@ -43,9 +43,6 @@ THD_WORKING_AREA(_timer_thread_wa, 2048); THD_WORKING_AREA(_rcin_thread_wa, 512); THD_WORKING_AREA(_io_thread_wa, 2048); THD_WORKING_AREA(_storage_thread_wa, 2048); -#if HAL_WITH_UAVCAN -THD_WORKING_AREA(_uavcan_thread_wa, 4096); -#endif Scheduler::Scheduler() { @@ -62,14 +59,6 @@ void Scheduler::init() _timer_thread, /* Thread function. */ this); /* Thread parameter. */ - // setup the uavcan thread - this will call tasks at 1kHz -#if HAL_WITH_UAVCAN - _uavcan_thread_ctx = chThdCreateStatic(_uavcan_thread_wa, - sizeof(_uavcan_thread_wa), - APM_UAVCAN_PRIORITY, /* Initial priority. */ - _uavcan_thread, /* Thread function. */ - this); /* Thread parameter. */ -#endif // setup the RCIN thread - this will call tasks at 1kHz _rcin_thread_ctx = chThdCreateStatic(_rcin_thread_wa, sizeof(_rcin_thread_wa), @@ -286,24 +275,6 @@ void Scheduler::_timer_thread(void *arg) hal.rcout->timer_tick(); } } -#if HAL_WITH_UAVCAN -void Scheduler::_uavcan_thread(void *arg) -{ - Scheduler *sched = (Scheduler *)arg; - chRegSetThreadName("apm_uavcan"); - while (!sched->_hal_initialized) { - sched->delay_microseconds(20000); - } - while (true) { - sched->delay_microseconds(300); - for (int i = 0; i < MAX_NUMBER_OF_CAN_INTERFACES; i++) { - if (AP_UAVCAN::get_uavcan(i) != nullptr) { - CANManager::from(hal.can_mgr[i])->_timer_tick(); - } - } - } -} -#endif void Scheduler::_rcin_thread(void *arg) { diff --git a/libraries/AP_HAL_ChibiOS/Scheduler.h b/libraries/AP_HAL_ChibiOS/Scheduler.h index 4e86a5189ad9e..3d5ac04c62cf3 100644 --- a/libraries/AP_HAL_ChibiOS/Scheduler.h +++ b/libraries/AP_HAL_ChibiOS/Scheduler.h @@ -43,12 +43,8 @@ #define APM_SPI_PRIORITY 181 #endif -#ifndef APM_UAVCAN_PRIORITY -#define APM_UAVCAN_PRIORITY 178 -#endif - #ifndef APM_CAN_PRIORITY -#define APM_CAN_PRIORITY 177 +#define APM_CAN_PRIORITY 178 #endif #ifndef APM_I2C_PRIORITY @@ -96,14 +92,14 @@ class ChibiOS::Scheduler : public AP_HAL::Scheduler { create a new thread */ bool thread_create(AP_HAL::MemberProc, const char *name, uint32_t stack_size, priority_base base, int8_t priority) override; - + private: bool _initialized; volatile bool _hal_initialized; AP_HAL::Proc _failsafe; bool _called_boost; bool _priority_boosted; - + AP_HAL::MemberProc _timer_proc[CHIBIOS_SCHEDULER_MAX_TIMER_PROCS]; uint8_t _num_timer_procs; @@ -117,9 +113,7 @@ class ChibiOS::Scheduler : public AP_HAL::Scheduler { thread_t* _rcin_thread_ctx; thread_t* _io_thread_ctx; thread_t* _storage_thread_ctx; -#if HAL_WITH_UAVCAN - thread_t* _uavcan_thread_ctx; -#endif + #if CH_CFG_USE_SEMAPHORES == TRUE binary_semaphore_t _timer_semaphore; binary_semaphore_t _io_semaphore; @@ -129,9 +123,7 @@ class ChibiOS::Scheduler : public AP_HAL::Scheduler { static void _io_thread(void *arg); static void _storage_thread(void *arg); static void _uart_thread(void *arg); -#if HAL_WITH_UAVCAN - static void _uavcan_thread(void *arg); -#endif + void _run_timers(); void _run_io(void); static void thread_create_trampoline(void *ctx); diff --git a/libraries/AP_HAL_Linux/CAN.cpp b/libraries/AP_HAL_Linux/CAN.cpp index 712339abfbe03..170b15521c240 100644 --- a/libraries/AP_HAL_Linux/CAN.cpp +++ b/libraries/AP_HAL_Linux/CAN.cpp @@ -31,8 +31,6 @@ #include "CAN.h" -#include - #include #include @@ -461,17 +459,6 @@ void CANManager::IfaceWrapper::updateDownStatusFromPollResult(const pollfd& pfd) } } -void CANManager::_timer_tick() -{ - if (!_initialized) return; - - if (p_uavcan != nullptr) { - p_uavcan->do_cyclic(); - } else { - hal.console->printf("p_uavcan is nullptr"); - } -} - bool CANManager::begin(uint32_t bitrate, uint8_t can_number) { if (init(can_number) >= 0) { @@ -490,16 +477,6 @@ void CANManager::initialized(bool val) _initialized = val; } -AP_UAVCAN *CANManager::get_UAVCAN(void) -{ - return p_uavcan; -} - -void CANManager::set_UAVCAN(AP_UAVCAN *uavcan) -{ - p_uavcan = uavcan; -} - CAN* CANManager::getIface(uint8_t iface_index) { return (iface_index >= _ifaces.size()) ? nullptr : _ifaces[iface_index].get(); diff --git a/libraries/AP_HAL_Linux/CAN.h b/libraries/AP_HAL_Linux/CAN.h index dde0099dface9..2de8b77dfb9d7 100644 --- a/libraries/AP_HAL_Linux/CAN.h +++ b/libraries/AP_HAL_Linux/CAN.h @@ -180,8 +180,6 @@ class CANManager: public AP_HAL::CANManager, public uavcan::ICanDriver { CANManager() : AP_HAL::CANManager(this) { _ifaces.reserve(uavcan::MaxCanIfaces); } ~CANManager() { } - void _timer_tick(); - //These methods belong to AP_HAL::CANManager virtual bool begin(uint32_t bitrate, uint8_t can_number) override; @@ -189,12 +187,7 @@ class CANManager: public AP_HAL::CANManager, public uavcan::ICanDriver { virtual void initialized(bool val); virtual bool is_initialized(); - AP_UAVCAN *p_uavcan; - - virtual AP_UAVCAN *get_UAVCAN(void) override; - virtual void set_UAVCAN(AP_UAVCAN *uavcan) override; - - //These methods belong to ICanDriver (which is an ancestor of AP_HAL::CANManager) + //These methods belong to ICanDriver virtual CAN* getIface(uint8_t iface_index) override; diff --git a/libraries/AP_HAL_Linux/Scheduler.cpp b/libraries/AP_HAL_Linux/Scheduler.cpp index a3844bad79832..9b8347ba65200 100644 --- a/libraries/AP_HAL_Linux/Scheduler.cpp +++ b/libraries/AP_HAL_Linux/Scheduler.cpp @@ -19,10 +19,6 @@ #include "UARTDriver.h" #include "Util.h" -#if HAL_WITH_UAVCAN -#include "CAN.h" -#endif - using namespace Linux; extern const AP_HAL::HAL& hal; @@ -222,16 +218,6 @@ void Scheduler::_timer_task() } _in_timer_proc = false; - -#if HAL_WITH_UAVCAN -#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX - for (i = 0; i < MAX_NUMBER_OF_CAN_INTERFACES; i++) { - if(hal.can_mgr[i] != nullptr) { - CANManager::from(hal.can_mgr[i])->_timer_tick(); - } - } -#endif -#endif } void Scheduler::_run_io(void) diff --git a/libraries/AP_HAL_PX4/CAN.cpp b/libraries/AP_HAL_PX4/CAN.cpp index 3fee6b7a33bb3..375bddb8446d7 100644 --- a/libraries/AP_HAL_PX4/CAN.cpp +++ b/libraries/AP_HAL_PX4/CAN.cpp @@ -8,8 +8,6 @@ #include #include -#include -#include #if HAL_WITH_UAVCAN @@ -25,6 +23,8 @@ #include "Scheduler.h" +#include + /* * FOR INVESTIGATION: * AP_HAL::micros64() was called for monotonic time counter @@ -35,8 +35,6 @@ extern const AP_HAL::HAL& hal; -#include - extern "C" { static int can1_irq(const int irq, void*); #if CAN_STM32_NUM_IFACES > 1 @@ -174,7 +172,7 @@ int PX4CAN::computeTimings(const uint32_t target_bitrate, Timings& out_timings) const uint8_t max_quanta_per_bit = (target_bitrate >= 1000000) ? 10 : 17; if (max_quanta_per_bit > (MaxBS1 + MaxBS2)) { - if (AP_BoardConfig_CAN::get_can_debug() >= 1) { + if (AP::can().get_debug_level(self_index_) >= 1) { printf("PX4CAN::computeTimings max_quanta_per_bit problem\n\r"); } } @@ -242,7 +240,7 @@ int PX4CAN::computeTimings(const uint32_t target_bitrate, Timings& out_timings) bs1(arg_bs1), bs2(uint8_t(bs1_bs2_sum - bs1)), sample_point_permill( uint16_t(1000 * (1 + bs1) / (1 + bs1 + bs2))) { - if (bs1_bs2_sum <= arg_bs1 && (AP_BoardConfig_CAN::get_can_debug() >= 1)) { + if (bs1_bs2_sum <= arg_bs1) { AP_HAL::panic("PX4CAN::computeTimings bs1_bs2_sum <= arg_bs1"); } } @@ -271,13 +269,13 @@ int PX4CAN::computeTimings(const uint32_t target_bitrate, Timings& out_timings) * Final validation */ if ((target_bitrate != (pclk / (prescaler * (1 + solution.bs1 + solution.bs2)))) || !solution.isValid()) { - if (AP_BoardConfig_CAN::get_can_debug() >= 1) { + if (AP::can().get_debug_level(self_index_) >= 1) { printf("PX4CAN::computeTimings target_bitrate error\n\r"); } return -ErrLogic; } - if (AP_BoardConfig_CAN::get_can_debug() >= 2) { + if (AP::can().get_debug_level(self_index_) >= 2) { printf("PX4CAN::computeTimings Timings: quanta/bit: %d, sample point location: %.1f%%\n\r", int(1 + solution.bs1 + solution.bs2), double(solution.sample_point_permill / 10.0)); } @@ -479,7 +477,7 @@ int PX4CAN::init(const uint32_t bitrate, const OperatingMode mode) } if (!waitMsrINakBitStateChange(true)) { - if (AP_BoardConfig_CAN::get_can_debug() >= 1) { + if (AP::can().get_debug_level(self_index_) >= 1) { printf("PX4CAN::init MSR INAK not set\n\r"); } can_->MCR = bxcan::MCR_RESET; @@ -505,7 +503,7 @@ int PX4CAN::init(const uint32_t bitrate, const OperatingMode mode) can_->MCR = bxcan::MCR_RESET; return timings_res; } - if (AP_BoardConfig_CAN::get_can_debug() >= 2) { + if (AP::can().get_debug_level(self_index_) >= 2) { printf("PX4CAN::init Timings: presc=%u sjw=%u bs1=%u bs2=%u\n\r", unsigned(timings.prescaler), unsigned(timings.sjw), unsigned(timings.bs1), unsigned(timings.bs2)); } @@ -525,7 +523,7 @@ int PX4CAN::init(const uint32_t bitrate, const OperatingMode mode) can_->MCR &= ~bxcan::MCR_INRQ; // Leave init mode if (!waitMsrINakBitStateChange(false)) { - if (AP_BoardConfig_CAN::get_can_debug() >= 1) { + if (AP::can().get_debug_level(self_index_) >= 1) { printf("PX4CAN::init MSR INAK not cleared\n\r"); } can_->MCR = bxcan::MCR_RESET; @@ -826,7 +824,7 @@ int32_t PX4CAN::tx_pending() PX4CANManager::PX4CANManager() : AP_HAL::CANManager(this), update_event_(*this), if0_(bxcan::Can[0], nullptr, 0, CAN_STM32_RX_QUEUE_SIZE), if1_( - bxcan::Can[1], nullptr, 1, CAN_STM32_RX_QUEUE_SIZE), initialized_(false), p_uavcan(nullptr) + bxcan::Can[1], nullptr, 1, CAN_STM32_RX_QUEUE_SIZE), initialized_(false) { uavcan::StaticAssert<(CAN_STM32_RX_QUEUE_SIZE <= PX4CAN::MaxRxQueueCapacity)>::check(); @@ -966,7 +964,7 @@ int PX4CANManager::init(const uint32_t bitrate, const PX4CAN::OperatingMode mode int res = 0; - if (AP_BoardConfig_CAN::get_can_debug(can_number) >= 2) { + if (AP::can().get_debug_level(can_number) >= 2) { printf("PX4CANManager::init Bitrate %lu mode %d bus %d\n\r", static_cast(bitrate), static_cast(mode), static_cast(can_number)); } @@ -977,7 +975,7 @@ int PX4CANManager::init(const uint32_t bitrate, const PX4CAN::OperatingMode mode _ifaces_num++; _ifaces_out_to_in[can_number] = _ifaces_num - 1; - if (AP_BoardConfig_CAN::get_can_debug(can_number) >= 2) { + if (AP::can().get_debug_level(can_number) >= 2) { printf("PX4CANManager::init First initialization bus %d\n\r", static_cast(can_number)); } @@ -988,7 +986,7 @@ int PX4CANManager::init(const uint32_t bitrate, const PX4CAN::OperatingMode mode * CAN1 */ if (can_number == 0) { - if (AP_BoardConfig_CAN::get_can_debug(0) >= 2) { + if (AP::can().get_debug_level(0) >= 2) { printf("PX4CANManager::init Initing iface 0...\n\r"); } ifaces[_ifaces_out_to_in[can_number]] = &if0_; // This link must be initialized first, @@ -999,7 +997,7 @@ int PX4CANManager::init(const uint32_t bitrate, const PX4CAN::OperatingMode mode * CAN2 */ if (can_number == 1) { - if (AP_BoardConfig_CAN::get_can_debug(1) >= 2) { + if (AP::can().get_debug_level(1) >= 2) { printf("PX4CANManager::init Initing iface 1...\n\r"); } ifaces[_ifaces_out_to_in[can_number]] = &if1_; // Same thing here. @@ -1013,7 +1011,7 @@ int PX4CANManager::init(const uint32_t bitrate, const PX4CAN::OperatingMode mode return res; } - if (AP_BoardConfig_CAN::get_can_debug(can_number) >= 2) { + if (AP::can().get_debug_level(can_number) >= 2) { printf("PX4CANManager::init CAN drv init OK, res = %d\n\r", res); } @@ -1076,16 +1074,6 @@ void PX4CANManager::initialized(bool val) initialized_ = val; } -AP_UAVCAN *PX4CANManager::get_UAVCAN(void) -{ - return p_uavcan; -} - -void PX4CANManager::set_UAVCAN(AP_UAVCAN *uavcan) -{ - p_uavcan = uavcan; -} - /* * Interrupt handlers */ diff --git a/libraries/AP_HAL_PX4/CAN.h b/libraries/AP_HAL_PX4/CAN.h index 02609ad1cfa55..aa7a4364f02cd 100644 --- a/libraries/AP_HAL_PX4/CAN.h +++ b/libraries/AP_HAL_PX4/CAN.h @@ -271,8 +271,6 @@ class PX4CANManager: public AP_HAL::CANManager, public uavcan::ICanDriver { uint8_t _ifaces_num; uint8_t _ifaces_out_to_in[CAN_STM32_NUM_IFACES]; - AP_UAVCAN *p_uavcan; - public: PX4CANManager(); @@ -310,8 +308,5 @@ class PX4CANManager: public AP_HAL::CANManager, public uavcan::ICanDriver { bool is_initialized() override; void initialized(bool val) override; - - AP_UAVCAN *get_UAVCAN(void) override; - void set_UAVCAN(AP_UAVCAN *uavcan) override; }; } diff --git a/libraries/AP_HAL_PX4/Scheduler.cpp b/libraries/AP_HAL_PX4/Scheduler.cpp index 400f9bf018d70..955e2c905169c 100644 --- a/libraries/AP_HAL_PX4/Scheduler.cpp +++ b/libraries/AP_HAL_PX4/Scheduler.cpp @@ -24,11 +24,6 @@ #include #include -#if HAL_WITH_UAVCAN -#include "CAN.h" -#include -#endif - using namespace PX4; extern const AP_HAL::HAL& hal; @@ -90,33 +85,6 @@ void PX4Scheduler::init() pthread_create(&_storage_thread_ctx, &thread_attr, &PX4Scheduler::_storage_thread, this); } -void PX4Scheduler::create_uavcan_thread() -{ -#if HAL_WITH_UAVCAN - pthread_attr_t thread_attr; - struct sched_param param; - - //the UAVCAN thread runs at medium priority - pthread_attr_init(&thread_attr); - pthread_attr_setstacksize(&thread_attr, 8192); - - param.sched_priority = APM_CAN_PRIORITY; - (void) pthread_attr_setschedparam(&thread_attr, ¶m); - pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO); - - for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_DRIVERS; i++) { - if (AP_UAVCAN::get_uavcan(i) == nullptr) { - continue; - } - _uavcan_thread_arg *arg = new _uavcan_thread_arg; - arg->sched = this; - arg->uavcan_number = i; - - pthread_create(&_uavcan_thread_ctx, &thread_attr, &PX4Scheduler::_uavcan_thread, arg); - } -#endif -} - /** delay for a specified number of microseconds using a semaphore wait */ @@ -379,36 +347,6 @@ void *PX4Scheduler::_storage_thread(void *arg) return nullptr; } -#if HAL_WITH_UAVCAN -void *PX4Scheduler::_uavcan_thread(void *arg) -{ - PX4Scheduler *sched = ((_uavcan_thread_arg *) arg)->sched; - uint8_t uavcan_number = ((_uavcan_thread_arg *) arg)->uavcan_number; - - char name[15]; - snprintf(name, sizeof(name), "apm_uavcan:%u", uavcan_number); - pthread_setname_np(pthread_self(), name); - - while (!sched->_hal_initialized) { - poll(nullptr, 0, 1); - } - - while (!_px4_thread_should_exit) { - if (((PX4CANManager *)hal.can_mgr[uavcan_number])->is_initialized()) { - if (((PX4CANManager *)hal.can_mgr[uavcan_number])->get_UAVCAN() != nullptr) { - (((PX4CANManager *)hal.can_mgr[uavcan_number])->get_UAVCAN())->do_cyclic(); - } else { - sched->delay_microseconds_semaphore(10000); - } - } else { - sched->delay_microseconds_semaphore(10000); - } - } - - return nullptr; -} -#endif - bool PX4Scheduler::in_main_thread() const { return getpid() == _main_task_pid; diff --git a/libraries/AP_HAL_PX4/Scheduler.h b/libraries/AP_HAL_PX4/Scheduler.h index 58667a2ed2baf..b7e1a614fd7ca 100644 --- a/libraries/AP_HAL_PX4/Scheduler.h +++ b/libraries/AP_HAL_PX4/Scheduler.h @@ -60,8 +60,6 @@ class PX4::PX4Scheduler : public AP_HAL::Scheduler { void system_initialized(); void hal_initialized() { _hal_initialized = true; } - void create_uavcan_thread() override; - /* disable interrupts and return a context that can be used to restore the interrupt state. This can be used to protect @@ -99,18 +97,11 @@ class PX4::PX4Scheduler : public AP_HAL::Scheduler { pthread_t _io_thread_ctx; pthread_t _storage_thread_ctx; pthread_t _uart_thread_ctx; - pthread_t _uavcan_thread_ctx; - - struct _uavcan_thread_arg { - PX4Scheduler *sched; - uint8_t uavcan_number; - }; static void *_timer_thread(void *arg); static void *_io_thread(void *arg); static void *_storage_thread(void *arg); static void *_uart_thread(void *arg); - static void *_uavcan_thread(void *arg); void _run_timers(); void _run_io(void); diff --git a/libraries/AP_HAL_SITL/CAN.h b/libraries/AP_HAL_SITL/CAN.h deleted file mode 100644 index 718f580b4a727..0000000000000 --- a/libraries/AP_HAL_SITL/CAN.h +++ /dev/null @@ -1,62 +0,0 @@ -/* Copyright (C) 2017 Eugene Shamaev - * - * This file 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 file 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 . - */ - -#pragma once - -#include "AP_HAL_SITL.h" - -#if HAL_WITH_UAVCAN -#include - -class HALSITL::HALSITLCAN : public AP_HAL::CAN { -public: - HALSITLCAN() { } - ~HALSITLCAN() { } - int16_t send(const uavcan::CanFrame& frame, uavcan::MonotonicTime tx_deadline, uavcan::CanIOFlags flags) override; - int16_t receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_ts_monotonic, uavcan::UtcTime& out_ts_utc, - uavcan::CanIOFlags& out_flags) override; - int16_t configureFilters(const uavcan::CanFilterConfig* filter_configs, uint16_t num_configs) override; - uint16_t getNumFilters() const override; - uint64_t getErrorCount() const override; - - bool begin(uint32_t bitrate) override; - void end() override; - void reset() override; - bool is_initialized() override; - int32_t tx_pending() override; - int32_t available() override; - - -private: - uint32_t _baudrate; - volatile bool _initialised; - volatile bool _in_timer; -}; - -class HALSITL::HALSITLCANDriver : public AP_HAL::CANManager -{ -public: - HALSITLCANDriver() { } - ~HALSITLCANDriver() { } - uavcan::ICanIface* getIface(uint8_t iface_index) override; - const uavcan::ICanIface* getIface(uint8_t iface_index) const override; - uint8_t getNumIfaces() const override; - int16_t select(uavcan::CanSelectMasks& inout_masks, - const uavcan::CanFrame* (& pending_tx)[uavcan::MaxCanIfaces], - uavcan::MonotonicTime blocking_deadline) override; -}; - -#endif diff --git a/libraries/AP_HAL_VRBRAIN/CAN.cpp b/libraries/AP_HAL_VRBRAIN/CAN.cpp index 970d462135349..23270da41bacc 100644 --- a/libraries/AP_HAL_VRBRAIN/CAN.cpp +++ b/libraries/AP_HAL_VRBRAIN/CAN.cpp @@ -8,9 +8,6 @@ #include #include -#include -#include - #if HAL_WITH_UAVCAN #include @@ -25,6 +22,8 @@ #include "Scheduler.h" +#include + /* * FOR INVESTIGATION: * AP_HAL::micros64() was called for monotonic time counter @@ -35,8 +34,6 @@ extern const AP_HAL::HAL& hal; -#include - extern "C" { static int can1_irq(const int irq, void*); #if CAN_STM32_NUM_IFACES > 1 @@ -174,7 +171,7 @@ int VRBRAINCAN::computeTimings(const uint32_t target_bitrate, Timings& out_timin const uint8_t max_quanta_per_bit = (target_bitrate >= 1000000) ? 10 : 17; if (max_quanta_per_bit > (MaxBS1 + MaxBS2)) { - if (AP_BoardConfig_CAN::get_can_debug() >= 1) { + if (AP::can().get_debug_level(self_index_) >= 1) { printf("VRBRAINCAN::computeTimings max_quanta_per_bit problem\n\r"); } } @@ -243,7 +240,7 @@ int VRBRAINCAN::computeTimings(const uint32_t target_bitrate, Timings& out_timin uint16_t(1000 * (1 + bs1) / (1 + bs1 + bs2))) { if (bs1_bs2_sum <= arg_bs1) { - if (AP_BoardConfig_CAN::get_can_debug() >= 1) { + if (AP::can().get_debug_level(self_index_) >= 1) { AP_HAL::panic("VRBRAINCAN::computeTimings bs1_bs2_sum <= arg_bs1"); } } @@ -273,13 +270,13 @@ int VRBRAINCAN::computeTimings(const uint32_t target_bitrate, Timings& out_timin * Final validation */ if ((target_bitrate != (pclk / (prescaler * (1 + solution.bs1 + solution.bs2)))) || !solution.isValid()) { - if (AP_BoardConfig_CAN::get_can_debug() >= 1) { + if (AP::can().get_debug_level(self_index_) >= 1) { printf("VRBRAINCAN::computeTimings target_bitrate error\n\r"); } return -ErrLogic; } - if (AP_BoardConfig_CAN::get_can_debug() >= 2) { + if (AP::can().get_debug_level(self_index_) >= 2) { printf("VRBRAINCAN::computeTimings Timings: quanta/bit: %d, sample point location: %.1f%%\n\r", int(1 + solution.bs1 + solution.bs2), double(solution.sample_point_permill / 10.0)); } @@ -481,7 +478,7 @@ int VRBRAINCAN::init(const uint32_t bitrate, const OperatingMode mode) } if (!waitMsrINakBitStateChange(true)) { - if (AP_BoardConfig_CAN::get_can_debug() >= 1) { + if (AP::can().get_debug_level(self_index_) >= 1) { printf("VRBRAINCAN::init MSR INAK not set\n\r"); } can_->MCR = bxcan::MCR_RESET; @@ -507,7 +504,7 @@ int VRBRAINCAN::init(const uint32_t bitrate, const OperatingMode mode) can_->MCR = bxcan::MCR_RESET; return timings_res; } - if (AP_BoardConfig_CAN::get_can_debug() >= 2) { + if (AP::can().get_debug_level(self_index_) >= 2) { printf("VRBRAINCAN::init Timings: presc=%u sjw=%u bs1=%u bs2=%u\n\r", unsigned(timings.prescaler), unsigned(timings.sjw), unsigned(timings.bs1), unsigned(timings.bs2)); } @@ -527,7 +524,7 @@ int VRBRAINCAN::init(const uint32_t bitrate, const OperatingMode mode) can_->MCR &= ~bxcan::MCR_INRQ; // Leave init mode if (!waitMsrINakBitStateChange(false)) { - if (AP_BoardConfig_CAN::get_can_debug() >= 1) { + if (AP::can().get_debug_level(self_index_) >= 1) { printf("VRBRAINCAN::init MSR INAK not cleared\n\r"); } can_->MCR = bxcan::MCR_RESET; @@ -829,7 +826,7 @@ int32_t VRBRAINCAN::tx_pending() VRBRAINCANManager::VRBRAINCANManager() : update_event_(*this), if0_(bxcan::Can[0], nullptr, 0, CAN_STM32_RX_QUEUE_SIZE), if1_( - bxcan::Can[1], nullptr, 1, CAN_STM32_RX_QUEUE_SIZE), initialized_(false), p_uavcan(nullptr) + bxcan::Can[1], nullptr, 1, CAN_STM32_RX_QUEUE_SIZE), initialized_(false) { uavcan::StaticAssert<(CAN_STM32_RX_QUEUE_SIZE <= VRBRAINCAN::MaxRxQueueCapacity)>::check(); @@ -971,7 +968,7 @@ int VRBRAINCANManager::init(const uint32_t bitrate, const VRBRAINCAN::OperatingM int res = 0; - if (AP_BoardConfig_CAN::get_can_debug(can_number) >= 2) { + if (AP::can().get_debug_level(can_number) >= 2) { printf("VRBRAINCANManager::init Bitrate %lu mode %d bus %d\n\r", static_cast(bitrate), static_cast(mode), static_cast(can_number)); } @@ -982,7 +979,7 @@ int VRBRAINCANManager::init(const uint32_t bitrate, const VRBRAINCAN::OperatingM _ifaces_num++; _ifaces_out_to_in[can_number] = _ifaces_num - 1; - if (AP_BoardConfig_CAN::get_can_debug(can_number) >= 2) { + if (AP::can().get_debug_level(can_number) >= 2) { printf("VRBRAINCANManager::init First initialization bus %d\n\r", static_cast(can_number)); } @@ -993,7 +990,7 @@ int VRBRAINCANManager::init(const uint32_t bitrate, const VRBRAINCAN::OperatingM * CAN1 */ if (can_number == 0) { - if (AP_BoardConfig_CAN::get_can_debug(0) >= 2) { + if (AP::can().get_debug_level(0) >= 2) { printf("VRBRAINCANManager::init Initing iface 0...\n\r"); } ifaces[_ifaces_out_to_in[can_number]] = &if0_; // This link must be initialized first, @@ -1004,7 +1001,7 @@ int VRBRAINCANManager::init(const uint32_t bitrate, const VRBRAINCAN::OperatingM * CAN2 */ if (can_number == 1) { - if (AP_BoardConfig_CAN::get_can_debug(1) >= 2) { + if (AP::can().get_debug_level(1) >= 2) { printf("VRBRAINCANManager::init Initing iface 1...\n\r"); } ifaces[_ifaces_out_to_in[can_number]] = &if1_; // Same thing here. @@ -1018,7 +1015,7 @@ int VRBRAINCANManager::init(const uint32_t bitrate, const VRBRAINCAN::OperatingM return res; } - if (AP_BoardConfig_CAN::get_can_debug(can_number) >= 2) { + if (AP::can().get_debug_level(can_number) >= 2) { printf("VRBRAINCANManager::init CAN drv init OK, res = %d\n\r", res); } @@ -1081,16 +1078,6 @@ void VRBRAINCANManager::initialized(bool val) initialized_ = val; } -AP_UAVCAN *VRBRAINCANManager::get_UAVCAN(void) -{ - return p_uavcan; -} - -void VRBRAINCANManager::set_UAVCAN(AP_UAVCAN *uavcan) -{ - p_uavcan = uavcan; -} - /* * Interrupt handlers */ diff --git a/libraries/AP_HAL_VRBRAIN/CAN.h b/libraries/AP_HAL_VRBRAIN/CAN.h index f69fb2b0875e8..230b72ffe1cd0 100644 --- a/libraries/AP_HAL_VRBRAIN/CAN.h +++ b/libraries/AP_HAL_VRBRAIN/CAN.h @@ -271,8 +271,6 @@ class VRBRAINCANManager: public AP_HAL::CANManager { uint8_t _ifaces_num; uint8_t _ifaces_out_to_in[CAN_STM32_NUM_IFACES]; - AP_UAVCAN *p_uavcan; - public: VRBRAINCANManager(); @@ -310,8 +308,5 @@ class VRBRAINCANManager: public AP_HAL::CANManager { bool is_initialized() override; void initialized(bool val) override; - - AP_UAVCAN *get_UAVCAN(void) override; - void set_UAVCAN(AP_UAVCAN *uavcan) override; }; } diff --git a/libraries/AP_HAL_VRBRAIN/Scheduler.cpp b/libraries/AP_HAL_VRBRAIN/Scheduler.cpp index 61b05eb5f8965..c2eb53d35c20c 100644 --- a/libraries/AP_HAL_VRBRAIN/Scheduler.cpp +++ b/libraries/AP_HAL_VRBRAIN/Scheduler.cpp @@ -24,11 +24,6 @@ #include #include -#if HAL_WITH_UAVCAN -#include "CAN.h" -#include -#endif - using namespace VRBRAIN; extern const AP_HAL::HAL& hal; @@ -90,34 +85,6 @@ void VRBRAINScheduler::init() pthread_create(&_storage_thread_ctx, &thread_attr, &VRBRAIN::VRBRAINScheduler::_storage_thread, this); } -void VRBRAINScheduler::create_uavcan_thread() -{ -#if HAL_WITH_UAVCAN - pthread_attr_t thread_attr; - struct sched_param param; - - //the UAVCAN thread runs at medium priority - pthread_attr_init(&thread_attr); - pthread_attr_setstacksize(&thread_attr, 8192); - - param.sched_priority = APM_CAN_PRIORITY; - (void) pthread_attr_setschedparam(&thread_attr, ¶m); - pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO); - - for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_DRIVERS; i++) { - if (AP_UAVCAN::get_uavcan(i) == nullptr) { - continue; - } - - _uavcan_thread_arg *arg = new _uavcan_thread_arg; - arg->sched = this; - arg->uavcan_number = i; - - pthread_create(&_uavcan_thread_ctx, &thread_attr, &VRBRAINScheduler::_uavcan_thread, arg); - } -#endif -} - /** delay for a specified number of microseconds using a semaphore wait */ @@ -380,36 +347,6 @@ void *VRBRAINScheduler::_storage_thread(void *arg) return nullptr; } -#if HAL_WITH_UAVCAN -void *VRBRAINScheduler::_uavcan_thread(void *arg) -{ - VRBRAINScheduler *sched = ((_uavcan_thread_arg *) arg)->sched; - uint8_t uavcan_number = ((_uavcan_thread_arg *) arg)->uavcan_number; - - char name[15]; - snprintf(name, sizeof(name), "apm_uavcan:%u", uavcan_number); - pthread_setname_np(pthread_self(), name); - - while (!sched->_hal_initialized) { - poll(nullptr, 0, 1); - } - - while (!_px4_thread_should_exit) { - if (((VRBRAINCANManager *)hal.can_mgr[uavcan_number])->is_initialized()) { - if (((VRBRAINCANManager *)hal.can_mgr[uavcan_number])->get_UAVCAN() != nullptr) { - (((VRBRAINCANManager *)hal.can_mgr[uavcan_number])->get_UAVCAN())->do_cyclic(); - } else { - sched->delay_microseconds_semaphore(10000); - } - } else { - sched->delay_microseconds_semaphore(10000); - } - } - - return nullptr; -} -#endif - bool VRBRAINScheduler::in_main_thread() const { return getpid() == _main_task_pid; diff --git a/libraries/AP_HAL_VRBRAIN/Scheduler.h b/libraries/AP_HAL_VRBRAIN/Scheduler.h index b4a74372a41ab..136e2cceb8822 100644 --- a/libraries/AP_HAL_VRBRAIN/Scheduler.h +++ b/libraries/AP_HAL_VRBRAIN/Scheduler.h @@ -59,8 +59,6 @@ class VRBRAIN::VRBRAINScheduler : public AP_HAL::Scheduler { void system_initialized(); void hal_initialized() { _hal_initialized = true; } - void create_uavcan_thread() override; - private: bool _initialized; volatile bool _hal_initialized; @@ -79,18 +77,11 @@ class VRBRAIN::VRBRAINScheduler : public AP_HAL::Scheduler { pthread_t _io_thread_ctx; pthread_t _storage_thread_ctx; pthread_t _uart_thread_ctx; - pthread_t _uavcan_thread_ctx; - - struct _uavcan_thread_arg { - VRBRAINScheduler *sched; - uint8_t uavcan_number; - }; static void *_timer_thread(void *arg); static void *_io_thread(void *arg); static void *_storage_thread(void *arg); static void *_uart_thread(void *arg); - static void *_uavcan_thread(void *arg); void _run_timers(); void _run_io(void); diff --git a/libraries/AP_Notify/UAVCAN_RGB_LED.cpp b/libraries/AP_Notify/UAVCAN_RGB_LED.cpp index 6261c62b89227..6393b1ae95ea9 100644 --- a/libraries/AP_Notify/UAVCAN_RGB_LED.cpp +++ b/libraries/AP_Notify/UAVCAN_RGB_LED.cpp @@ -17,8 +17,6 @@ #include #include -#include - #if HAL_WITH_UAVCAN #include "UAVCAN_RGB_LED.h" @@ -56,14 +54,12 @@ bool UAVCAN_RGB_LED::hw_init() bool UAVCAN_RGB_LED::hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue) { bool success = false; - if (AP_BoardConfig_CAN::get_can_num_ifaces() != 0) { - for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_DRIVERS; i++) { - if (hal.can_mgr[i] != nullptr) { - AP_UAVCAN *uavcan = hal.can_mgr[i]->get_UAVCAN(); - if (uavcan != nullptr) { - success |= uavcan->led_write(_led_index, red, green, blue); - } - } + uint8_t can_num_drivers = AP::can().get_num_drivers(); + + for (uint8_t i = 0; i < can_num_drivers; i++) { + AP_UAVCAN *uavcan = AP_UAVCAN::get_uavcan(i); + if (uavcan != nullptr) { + success = uavcan->led_write(_led_index, red, green, blue) || success; } } return success; diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.cpp b/libraries/AP_UAVCAN/AP_UAVCAN.cpp index 5b1d7875089d7..2e29a59d265ea 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.cpp +++ b/libraries/AP_UAVCAN/AP_UAVCAN.cpp @@ -15,6 +15,10 @@ #include #include +#if !HAL_MINIMIZE_FEATURES + #include +#endif + // Zubax GPS and other GPS, baro, magnetic sensors #include #include @@ -35,9 +39,11 @@ #include +#define LED_DELAY_US 50000 + extern const AP_HAL::HAL& hal; -#define debug_uavcan(level, fmt, args...) do { if ((level) <= AP_BoardConfig_CAN::get_can_debug()) { hal.console->printf(fmt, ##args); }} while (0) +#define debug_uavcan(level_debug, fmt, args...) do { if ((level_debug) <= AP::can().get_debug_level_driver(_driver_index)) { printf(fmt, ##args); }} while (0) // Translation of all messages from UAVCAN structures into AP structures is done // in AP_UAVCAN and not in corresponding drivers. @@ -78,7 +84,7 @@ const AP_Param::GroupInfo AP_UAVCAN::var_info[] = { // @Units: Hz // @User: Advanced AP_GROUPINFO("SRV_RT", 4, AP_UAVCAN, _servo_rate_hz, 50), - + AP_GROUPEND }; @@ -92,7 +98,7 @@ static void gnss_fix_cb(const uavcan::ReceivedDataStructurefind_gps_node(msg.getSrcNodeID().get()); if (state == nullptr) { return; @@ -202,7 +208,7 @@ static void gnss_aux_cb(const uavcan::ReceivedDataStructurefind_gps_node(msg.getSrcNodeID().get()); if (state == nullptr) { return; @@ -230,7 +236,7 @@ static void magnetic_cb(const uavcan::ReceivedDataStructurefind_mag_node(msg.getSrcNodeID().get(), 0); if (state == nullptr) { return; @@ -250,14 +256,14 @@ static void magnetic_cb1(const uavcan::ReceivedDataStructure& msg) = { magnetic_cb0, magnetic_cb1 }; - + static void magnetic_cb_2(const uavcan::ReceivedDataStructure& msg, uint8_t mgr) { AP_UAVCAN *ap_uavcan = AP_UAVCAN::get_uavcan(mgr); if (ap_uavcan == nullptr) { return; } - + AP_UAVCAN::Mag_Info *state = ap_uavcan->find_mag_node(msg.getSrcNodeID().get(), msg.sensor_id); if (state == nullptr) { return; @@ -284,7 +290,7 @@ static void air_data_sp_cb(const uavcan::ReceivedDataStructurefind_baro_node(msg.getSrcNodeID().get()); if (state == nullptr) { return; @@ -311,7 +317,7 @@ static void air_data_st_cb(const uavcan::ReceivedDataStructurefind_baro_node(msg.getSrcNodeID().get()); if (state == nullptr) { return; @@ -334,7 +340,7 @@ static void battery_info_st_cb(const uavcan::ReceivedDataStructurefind_bi_id((uint16_t) msg.battery_id); if (state == nullptr) { return; @@ -419,172 +425,241 @@ AP_UAVCAN::~AP_UAVCAN() { } -bool AP_UAVCAN::try_init(void) +AP_UAVCAN *AP_UAVCAN::get_uavcan(uint8_t driver_index) { - if (_parent_can_mgr == nullptr) { - return false; + if (driver_index >= AP::can().get_num_drivers() || + AP::can().get_protocol_type(driver_index) != AP_BoardConfig_CAN::Protocol_Type_UAVCAN) { + return nullptr; } + return static_cast(AP::can().get_driver(driver_index)); +} +void AP_UAVCAN::init(uint8_t driver_index) +{ if (_initialized) { - return true; + debug_uavcan(2, "UAVCAN: init called more than once\n\r"); + return; } - - if (!_parent_can_mgr->is_initialized()) { - return false; + + _driver_index = driver_index; + + AP_HAL::CANManager* can_mgr = hal.can_mgr[driver_index]; + if (can_mgr == nullptr) { + debug_uavcan(2, "UAVCAN: init called for inexisting CAN driver\n\r"); + return; } - - _uavcan_i = UINT8_MAX; - for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_DRIVERS; i++) { - if (_parent_can_mgr == hal.can_mgr[i]) { - _uavcan_i = i; - break; - } + + if (!can_mgr->is_initialized()) { + debug_uavcan(1, "UAVCAN: CAN driver not initialized\n\r"); + return; } - if(_uavcan_i == UINT8_MAX) { - return false; + uavcan::ICanDriver* driver = can_mgr->get_driver(); + if (driver == nullptr) { + debug_uavcan(2, "UAVCAN: can't get UAVCAN interface driver\n\r"); + return; } - auto *node = get_node(); + _node = new uavcan::Node<0>(*driver, SystemClock::instance(), _node_allocator); - if (node == nullptr) { - return false; + if (_node == nullptr) { + debug_uavcan(1, "UAVCAN: couldn't allocate node\n\r"); + return; } - - if (node->isStarted()) { - return false; + + if (_node->isStarted()) { + debug_uavcan(2, "UAVCAN: node was already started?\n\r"); + return; } - + uavcan::NodeID self_node_id(_uavcan_node); - node->setNodeID(self_node_id); + _node->setNodeID(self_node_id); char ndname[20]; - snprintf(ndname, sizeof(ndname), "org.ardupilot:%u", _uavcan_i); + snprintf(ndname, sizeof(ndname), "org.ardupilot:%u", driver_index); uavcan::NodeStatusProvider::NodeName name(ndname); - node->setName(name); + _node->setName(name); uavcan::protocol::SoftwareVersion sw_version; // Standard type uavcan.protocol.SoftwareVersion sw_version.major = AP_UAVCAN_SW_VERS_MAJOR; sw_version.minor = AP_UAVCAN_SW_VERS_MINOR; - node->setSoftwareVersion(sw_version); + _node->setSoftwareVersion(sw_version); uavcan::protocol::HardwareVersion hw_version; // Standard type uavcan.protocol.HardwareVersion hw_version.major = AP_UAVCAN_HW_VERS_MAJOR; hw_version.minor = AP_UAVCAN_HW_VERS_MINOR; - node->setHardwareVersion(hw_version); + _node->setHardwareVersion(hw_version); - const int node_start_res = node->start(); - if (node_start_res < 0) { - debug_uavcan(1, "UAVCAN: node start problem\n\r"); + int start_res = _node->start(); + if (start_res < 0) { + debug_uavcan(1, "UAVCAN: node start problem, error %d\n\r", start_res); + return; } uavcan::Subscriber *gnss_fix; - gnss_fix = new uavcan::Subscriber(*node); + gnss_fix = new uavcan::Subscriber(*_node); + start_res = gnss_fix->start(gnss_fix_cb_arr[driver_index]); - const int gnss_fix_start_res = gnss_fix->start(gnss_fix_cb_arr[_uavcan_i]); - if (gnss_fix_start_res < 0) { - debug_uavcan(1, "UAVCAN GNSS subscriber start problem\n\r"); - return false; + if (start_res < 0) { + debug_uavcan(1, "UAVCAN GNSS subscriber start problem, error %d\n\r", start_res); + return; } uavcan::Subscriber *gnss_aux; - gnss_aux = new uavcan::Subscriber(*node); - const int gnss_aux_start_res = gnss_aux->start(gnss_aux_cb_arr[_uavcan_i]); - if (gnss_aux_start_res < 0) { - debug_uavcan(1, "UAVCAN GNSS Aux subscriber start problem\n\r"); - return false; + gnss_aux = new uavcan::Subscriber(*_node); + start_res = gnss_aux->start(gnss_aux_cb_arr[driver_index]); + + if (start_res < 0) { + debug_uavcan(1, "UAVCAN GNSS Aux subscriber start problem, error %d\n\r", start_res); + return; } uavcan::Subscriber *magnetic; - magnetic = new uavcan::Subscriber(*node); - const int magnetic_start_res = magnetic->start(magnetic_cb_arr[_uavcan_i]); - if (magnetic_start_res < 0) { - debug_uavcan(1, "UAVCAN Compass subscriber start problem\n\r"); - return false; + magnetic = new uavcan::Subscriber(*_node); + start_res = magnetic->start(magnetic_cb_arr[driver_index]); + + if (start_res < 0) { + debug_uavcan(1, "UAVCAN Compass subscriber start problem, error %d\n\r", start_res); + return; } uavcan::Subscriber *magnetic2; - magnetic2 = new uavcan::Subscriber(*node); - const int magnetic_start_res_2 = magnetic2->start(magnetic_cb_2_arr[_uavcan_i]); - if (magnetic_start_res_2 < 0) { - debug_uavcan(1, "UAVCAN Compass for multiple mags subscriber start problem\n\r"); - return false; + magnetic2 = new uavcan::Subscriber(*_node); + start_res = magnetic2->start(magnetic_cb_2_arr[driver_index]); + + if (start_res < 0) { + debug_uavcan(1, "UAVCAN Compass for multiple mags subscriber start problem, error %d\n\r", start_res); + return; } uavcan::Subscriber *air_data_sp; - air_data_sp = new uavcan::Subscriber(*node); - const int air_data_sp_start_res = air_data_sp->start(air_data_sp_cb_arr[_uavcan_i]); - if (air_data_sp_start_res < 0) { - debug_uavcan(1, "UAVCAN Baro subscriber start problem\n\r"); - return false; + air_data_sp = new uavcan::Subscriber(*_node); + start_res = air_data_sp->start(air_data_sp_cb_arr[driver_index]); + + if (start_res < 0) { + debug_uavcan(1, "UAVCAN Baro subscriber start problem, error %d\n\r", start_res); + return; } - + uavcan::Subscriber *air_data_st; - air_data_st = new uavcan::Subscriber(*node); - const int air_data_st_start_res = air_data_st->start(air_data_st_cb_arr[_uavcan_i]); - if (air_data_st_start_res < 0) { - debug_uavcan(1, "UAVCAN Temperature subscriber start problem\n\r"); - return false; + air_data_st = new uavcan::Subscriber(*_node); + start_res = air_data_st->start(air_data_st_cb_arr[driver_index]); + + if (start_res < 0) { + debug_uavcan(1, "UAVCAN Temperature subscriber start problem, error %d\n\r", start_res); + return; } - + uavcan::Subscriber *battery_info_st; - battery_info_st = new uavcan::Subscriber(*node); - const int battery_info_start_res = battery_info_st->start(battery_info_st_cb_arr[_uavcan_i]); - if (battery_info_start_res < 0) { - debug_uavcan(1, "UAVCAN BatteryInfo subscriber start problem\n\r"); - return false; + battery_info_st = new uavcan::Subscriber(*_node); + start_res = battery_info_st->start(battery_info_st_cb_arr[driver_index]); + + if (start_res < 0) { + debug_uavcan(1, "UAVCAN BatteryInfo subscriber start problem, error %d\n\r", start_res); + return; } - act_out_array[_uavcan_i] = new uavcan::Publisher(*node); - act_out_array[_uavcan_i]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20)); - act_out_array[_uavcan_i]->setPriority(uavcan::TransferPriority::OneLowerThanHighest); + act_out_array[driver_index] = new uavcan::Publisher(*_node); + act_out_array[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(2)); + act_out_array[driver_index]->setPriority(uavcan::TransferPriority::OneLowerThanHighest); - esc_raw[_uavcan_i] = new uavcan::Publisher(*node); - esc_raw[_uavcan_i]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20)); - esc_raw[_uavcan_i]->setPriority(uavcan::TransferPriority::OneLowerThanHighest); + esc_raw[driver_index] = new uavcan::Publisher(*_node); + esc_raw[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(2)); + esc_raw[driver_index]->setPriority(uavcan::TransferPriority::OneLowerThanHighest); - rgb_led[_uavcan_i] = new uavcan::Publisher(*node); - rgb_led[_uavcan_i]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20)); - rgb_led[_uavcan_i]->setPriority(uavcan::TransferPriority::OneHigherThanLowest); + rgb_led[driver_index] = new uavcan::Publisher(*_node); + rgb_led[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20)); + rgb_led[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest); _led_conf.devices_count = 0; +#if !HAL_MINIMIZE_FEATURES + configureCanAcceptanceFilters(*_node); +#endif + /* * Informing other nodes that we're ready to work. * Default mode is INITIALIZING. */ - node->setModeOperational(); + _node->setModeOperational(); - _initialized = true; + // Spin node for device discovery + _node->spin(uavcan::MonotonicDuration::fromMSec(5000)); - debug_uavcan(1, "UAVCAN: init done\n\r"); + snprintf(_thread_name, sizeof(_thread_name), "uavcan_%u", driver_index); - return true; -} + if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_UAVCAN::loop, void), _thread_name, 4096, AP_HAL::Scheduler::PRIORITY_CAN, 0)) { + _node->setModeOfflineAndPublish(); + debug_uavcan(1, "UAVCAN: couldn't create thread\n\r"); + return; + } -void AP_UAVCAN::SRV_sem_take() -{ - SRV_sem->take_blocking(); + _initialized = true; + debug_uavcan(2, "UAVCAN: init done\n\r"); } -void AP_UAVCAN::SRV_sem_give() +void AP_UAVCAN::loop(void) { - SRV_sem->give(); + while (true) { + if (!_initialized) { + hal.scheduler->delay_microseconds(1000); + continue; + } + + const int error = _node->spin(uavcan::MonotonicDuration::fromMSec(1)); + + if (error < 0) { + hal.scheduler->delay_microseconds(100); + continue; + } + + if (_SRV_armed) { + bool sent_servos = false; + + if (_servo_bm > 0) { + // if we have any Servos in bitmask + uint32_t now = AP_HAL::micros(); + const uint32_t servo_period_us = 1000000UL / unsigned(_servo_rate_hz.get()); + if (now - _SRV_last_send_us >= servo_period_us) { + _SRV_last_send_us = now; + SRV_send_actuator(); + sent_servos = true; + for (uint8_t i = 0; i < UAVCAN_SRV_NUMBER; i++) { + _SRV_conf[i].servo_pending = false; + } + } + } + + // if we have any ESC's in bitmask + if (_esc_bm > 0 && !sent_servos) { + SRV_send_esc(); + } + + for (uint8_t i = 0; i < UAVCAN_SRV_NUMBER; i++) { + _SRV_conf[i].esc_pending = false; + } + } + + led_out_send(); + } } -void AP_UAVCAN::SRV_send_servos(void) + +///// SRV output ///// + +void AP_UAVCAN::SRV_send_actuator(void) { uint8_t starting_servo = 0; bool repeat_send; + SRV_sem->take_blocking(); + do { repeat_send = false; uavcan::equipment::actuator::ArrayCommand msg; - SRV_sem_take(); - uint8_t i; // UAVCAN can hold maximum of 15 commands in one frame for (i = 0; starting_servo < UAVCAN_SRV_NUMBER && i < 15; starting_servo++) { @@ -614,16 +689,16 @@ void AP_UAVCAN::SRV_send_servos(void) } } - SRV_sem_give(); - if (i > 0) { - act_out_array[_uavcan_i]->broadcast(msg); + act_out_array[_driver_index]->broadcast(msg); if (i == 15) { repeat_send = true; } } } while (repeat_send); + + SRV_sem->give(); } void AP_UAVCAN::SRV_send_esc(void) @@ -634,6 +709,8 @@ void AP_UAVCAN::SRV_send_esc(void) uint8_t active_esc_num = 0, max_esc_num = 0; uint8_t k = 0; + SRV_sem->take_blocking(); + // find out how many esc we have enabled and if they are active at all for (uint8_t i = 0; i < UAVCAN_SRV_NUMBER; i++) { if ((((uint32_t) 1) << i) & _esc_bm) { @@ -648,8 +725,6 @@ void AP_UAVCAN::SRV_send_esc(void) if (active_esc_num > 0) { k = 0; - SRV_sem_take(); - for (uint8_t i = 0; i < max_esc_num && k < 20; i++) { if ((((uint32_t) 1) << i) & _esc_bm) { // TODO: ESC negative scaling for reverse thrust and reverse rotation @@ -664,181 +739,109 @@ void AP_UAVCAN::SRV_send_esc(void) k++; } - SRV_sem_give(); - esc_raw[_uavcan_i]->broadcast(esc_msg); + esc_raw[_driver_index]->broadcast(esc_msg); } + + SRV_sem->give(); } -void AP_UAVCAN::do_cyclic(void) +void AP_UAVCAN::SRV_push_servos() { - if (!_initialized) { - hal.scheduler->delay_microseconds(1000); - return; + SRV_sem->take_blocking(); + + for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) { + // Check if this channels has any function assigned + if (SRV_Channels::channel_function(i)) { + _SRV_conf[i].pulse = SRV_Channels::srv_channel(i)->get_output_pwm(); + _SRV_conf[i].esc_pending = true; + _SRV_conf[i].servo_pending = true; + } } - auto *node = get_node(); + SRV_sem->give(); - const int error = node->spin(uavcan::MonotonicDuration::fromMSec(1)); + _SRV_armed = hal.util->get_soft_armed() && hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED; +} - if (error < 0) { - hal.scheduler->delay_microseconds(100); - return; - } - if (_SRV_armed) { - bool sent_servos = false; - - if (_servo_bm > 0) { - // if we have any Servos in bitmask - uint32_t now = AP_HAL::micros(); - const uint32_t servo_period_us = 1000000UL / unsigned(_servo_rate_hz.get()); - if (now - _SRV_last_send_us >= servo_period_us) { - _SRV_last_send_us = now; - SRV_send_servos(); - sent_servos = true; - for (uint8_t i = 0; i < UAVCAN_SRV_NUMBER; i++) { - _SRV_conf[i].servo_pending = false; - } - } - } +///// LED ///// - // if we have any ESC's in bitmask - if (_esc_bm > 0 && !sent_servos) { - SRV_send_esc(); - } - - for (uint8_t i = 0; i < UAVCAN_SRV_NUMBER; i++) { - _SRV_conf[i].esc_pending = false; - } - } +void AP_UAVCAN::led_out_send() +{ + uint64_t now = AP_HAL::micros64(); - if (led_out_sem_take()) { + if ((now - _led_conf.last_update) < LED_DELAY_US) { + return; + } - led_out_send(); - led_out_sem_give(); + if (!_led_out_sem->take(1)) { + return; } -} -bool AP_UAVCAN::led_out_sem_take() -{ - bool sem_ret = _led_out_sem->take(10); - if (!sem_ret) { - debug_uavcan(1, "AP_UAVCAN LEDOut semaphore fail\n\r"); + if (_led_conf.devices_count == 0) { + _led_out_sem->give(); + return; } - return sem_ret; -} -void AP_UAVCAN::led_out_sem_give() -{ - _led_out_sem->give(); -} + uavcan::equipment::indication::LightsCommand msg; + uavcan::equipment::indication::SingleLightCommand cmd; -void AP_UAVCAN::led_out_send() -{ - if (_led_conf.broadcast_enabled && ((AP_HAL::micros64() - _led_conf.last_update) > (AP_UAVCAN_LED_DELAY_MILLISECONDS * 1000))) { - uavcan::equipment::indication::LightsCommand msg; - uavcan::equipment::indication::SingleLightCommand cmd; - - for (uint8_t i = 0; i < _led_conf.devices_count; i++) { - if (_led_conf.devices[i].enabled) { - cmd.light_id =_led_conf.devices[i].led_index; - cmd.color = _led_conf.devices[i].rgb565_color; - msg.commands.push_back(cmd); - } - } + for (uint8_t i = 0; i < _led_conf.devices_count; i++) { + cmd.light_id =_led_conf.devices[i].led_index; + cmd.color.red = _led_conf.devices[i].red >> 3; + cmd.color.green = _led_conf.devices[i].green >> 2; + cmd.color.blue = _led_conf.devices[i].blue >> 3; - rgb_led[_uavcan_i]->broadcast(msg); - _led_conf.last_update = AP_HAL::micros64(); + msg.commands.push_back(cmd); } -} -uavcan::ISystemClock & AP_UAVCAN::get_system_clock() -{ - return SystemClock::instance(); -} + _led_out_sem->give(); -uavcan::ICanDriver * AP_UAVCAN::get_can_driver() -{ - if (_parent_can_mgr != nullptr) { - if (_parent_can_mgr->is_initialized() == false) { - return nullptr; - } else { - return _parent_can_mgr->get_driver(); - } - } - return nullptr; + rgb_led[_driver_index]->broadcast(msg); + _led_conf.last_update = now; } -uavcan::Node<0> *AP_UAVCAN::get_node() +bool AP_UAVCAN::led_write(uint8_t led_index, uint8_t red, uint8_t green, uint8_t blue) { - if (_node == nullptr && get_can_driver() != nullptr) { - _node = new uavcan::Node<0>(*get_can_driver(), get_system_clock(), _node_allocator); + if (_led_conf.devices_count >= AP_UAVCAN_MAX_LED_DEVICES) { + return false; } - return _node; -} - -void AP_UAVCAN::SRV_set_safety_pwm(uint32_t chmask, uint16_t pulse_len) -{ - for (uint8_t i = 0; i < UAVCAN_SRV_NUMBER; i++) { - if (chmask & (((uint32_t) 1) << i)) { - _SRV_conf[i].safety_pulse = pulse_len; - } + if (!_led_out_sem->take(1)) { + return false; } -} -void AP_UAVCAN::SRV_set_failsafe_pwm(uint32_t chmask, uint16_t pulse_len) -{ - for (uint8_t i = 0; i < UAVCAN_SRV_NUMBER; i++) { - if (chmask & (((uint32_t) 1) << i)) { - _SRV_conf[i].failsafe_pulse = pulse_len; + // check if a device instance exists. if so, break so the instance index is remembered + uint8_t instance = 0; + for (; instance < _led_conf.devices_count; instance++) { + if (_led_conf.devices[instance].led_index == led_index) { + break; } } -} -void AP_UAVCAN::SRV_force_safety_on(void) -{ - _SRV_safety = true; -} + // load into the correct instance. + // if an existing instance was found in above for loop search, + // then instance value is < _led_conf.devices_count. + // otherwise a new one was just found so we increment the count. + // Either way, the correct instance is the current value of instance + _led_conf.devices[instance].led_index = led_index; + _led_conf.devices[instance].red = red; + _led_conf.devices[instance].green = green; + _led_conf.devices[instance].blue = blue; -void AP_UAVCAN::SRV_force_safety_off(void) -{ - _SRV_safety = false; -} + if (instance == _led_conf.devices_count) { + _led_conf.devices_count++; + } -void AP_UAVCAN::SRV_arm_actuators(bool arm) -{ - _SRV_armed = arm; -} + _led_out_sem->give(); -void AP_UAVCAN::SRV_write(uint16_t pulse_len, uint8_t ch) -{ - _SRV_conf[ch].pulse = pulse_len; - _SRV_conf[ch].esc_pending = true; - _SRV_conf[ch].servo_pending = true; + return true; } -void AP_UAVCAN::SRV_push_servos() -{ - SRV_sem_take(); - - for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) { - // Check if this channels has any function assigned - if (SRV_Channels::channel_function(i)) { - SRV_write(SRV_Channels::srv_channel(i)->get_output_pwm(), i); - } - } - SRV_sem_give(); - - if (hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED) { - SRV_arm_actuators(true); - } else { - SRV_arm_actuators(false); - } -} +///// GPS ///// uint8_t AP_UAVCAN::find_gps_without_listener(void) { @@ -970,6 +973,25 @@ void AP_UAVCAN::update_gps_state(uint8_t node) } } + +///// BARO ///// + +/* + * Find discovered not taken baro node with smallest node ID + */ +uint8_t AP_UAVCAN::find_smallest_free_baro_node() +{ + uint8_t ret = UINT8_MAX; + + for (uint8_t i = 0; i < AP_UAVCAN_MAX_BARO_NODES; i++) { + if (_baro_node_taken[i] == 0) { + ret = MIN(ret, _baro_nodes[i]); + } + } + + return ret; +} + uint8_t AP_UAVCAN::register_baro_listener(AP_Baro_Backend* new_listener, uint8_t preferred_channel) { uint8_t sel_place = UINT8_MAX, ret = 0; @@ -1091,16 +1113,22 @@ void AP_UAVCAN::update_baro_state(uint8_t node) } } + +///// COMPASS ///// + /* - * Find discovered not taken baro node with smallest node ID + * Find discovered mag node with smallest node ID and which is taken N times, + * where N is less than its maximum sensor id. + * This allows multiple AP_Compass_UAVCAN instanses listening multiple compasses + * that are on one node. */ -uint8_t AP_UAVCAN::find_smallest_free_baro_node() +uint8_t AP_UAVCAN::find_smallest_free_mag_node() { uint8_t ret = UINT8_MAX; - for (uint8_t i = 0; i < AP_UAVCAN_MAX_BARO_NODES; i++) { - if (_baro_node_taken[i] == 0) { - ret = MIN(ret, _baro_nodes[i]); + for (uint8_t i = 0; i < AP_UAVCAN_MAX_MAG_NODES; i++) { + if (_mag_node_taken[i] < _mag_node_max_sensorid_count[i]) { + ret = MIN(ret, _mag_nodes[i]); } } @@ -1221,25 +1249,6 @@ AP_UAVCAN::Mag_Info *AP_UAVCAN::find_mag_node(uint8_t node, uint8_t sensor_id) return nullptr; } -/* - * Find discovered mag node with smallest node ID and which is taken N times, - * where N is less than its maximum sensor id. - * This allows multiple AP_Compass_UAVCAN instanses listening multiple compasses - * that are on one node. - */ -uint8_t AP_UAVCAN::find_smallest_free_mag_node() -{ - uint8_t ret = UINT8_MAX; - - for (uint8_t i = 0; i < AP_UAVCAN_MAX_MAG_NODES; i++) { - if (_mag_node_taken[i] < _mag_node_max_sensorid_count[i]) { - ret = MIN(ret, _mag_nodes[i]); - } - } - - return ret; -} - void AP_UAVCAN::update_mag_state(uint8_t node, uint8_t sensor_id) { // Go through all listeners of specified node and call their's update methods @@ -1278,6 +1287,22 @@ void AP_UAVCAN::update_mag_state(uint8_t node, uint8_t sensor_id) } } + +///// BATTERY ///// + +uint8_t AP_UAVCAN::find_smallest_free_bi_id() +{ + uint8_t ret = UINT8_MAX; + + for (uint8_t i = 0; i < AP_UAVCAN_MAX_BI_NUMBER; i++) { + if (_bi_id_taken[i] == 0) { + ret = MIN(ret, _bi_id[i]); + } + } + + return ret; +} + uint8_t AP_UAVCAN::register_BM_bi_listener_to_id(AP_BattMonitor_Backend* new_listener, uint8_t id) { uint8_t sel_place = UINT8_MAX, ret = 0; @@ -1348,19 +1373,6 @@ AP_UAVCAN::BatteryInfo_Info *AP_UAVCAN::find_bi_id(uint8_t id) return nullptr; } -uint8_t AP_UAVCAN::find_smallest_free_bi_id() -{ - uint8_t ret = UINT8_MAX; - - for (uint8_t i = 0; i < AP_UAVCAN_MAX_BI_NUMBER; i++) { - if (_bi_id_taken[i] == 0) { - ret = MIN(ret, _bi_id[i]); - } - } - - return ret; -} - void AP_UAVCAN::update_bi_state(uint8_t id) { // Go through all listeners of specified node and call their's update methods @@ -1377,52 +1389,4 @@ void AP_UAVCAN::update_bi_state(uint8_t id) } } -bool AP_UAVCAN::led_write(uint8_t led_index, uint8_t red, uint8_t green, uint8_t blue) { - - if (_led_conf.devices_count >= AP_UAVCAN_MAX_LED_DEVICES) { - return false; - } - if (!led_out_sem_take()) { - return false; - } - - uavcan::equipment::indication::RGB565 color; - - color.red = (red >> 3); - color.green = (green >> 2); - color.blue = (blue >> 3); - - // check if a device instance exists. if so, break so the instance index is remembered - uint8_t instance = 0; - for (; instance < _led_conf.devices_count; instance++) { - if (!_led_conf.devices[instance].enabled || (_led_conf.devices[instance].led_index == led_index)) { - break; - } - } - - // load into the correct instance. - // if an existing instance was found in above for loop search, - // then instance value is < _led_conf.devices_count. - // otherwise a new one was just found so we increment the count. - // Either way, the correct instance is the cirrent value of instance - _led_conf.devices[instance].led_index = led_index; - _led_conf.devices[instance].rgb565_color = color; - _led_conf.devices[instance].enabled = true; - if (instance == _led_conf.devices_count) { - _led_conf.devices_count++; - } - - _led_conf.broadcast_enabled = true; - led_out_sem_give(); - return true; -} - -AP_UAVCAN *AP_UAVCAN::get_uavcan(uint8_t iface) -{ - if (iface >= MAX_NUMBER_OF_CAN_INTERFACES || !hal.can_mgr[iface]) { - return nullptr; - } - return hal.can_mgr[iface]->get_UAVCAN(); -} - #endif // HAL_WITH_UAVCAN diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.h b/libraries/AP_UAVCAN/AP_UAVCAN.h index 985d98c6edf23..749475a53b76f 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.h +++ b/libraries/AP_UAVCAN/AP_UAVCAN.h @@ -18,7 +18,6 @@ #include #include -#include #ifndef UAVCAN_NODE_POOL_SIZE #define UAVCAN_NODE_POOL_SIZE 8192 @@ -45,17 +44,29 @@ #define AP_UAVCAN_HW_VERS_MINOR 0 #define AP_UAVCAN_MAX_LED_DEVICES 4 -#define AP_UAVCAN_LED_DELAY_MILLISECONDS 50 -class AP_UAVCAN { +class AP_UAVCAN : public AP_HAL::CANProtocol { public: AP_UAVCAN(); ~AP_UAVCAN(); static const struct AP_Param::GroupInfo var_info[]; - // Return uavcan from @iface or nullptr if it's not ready or doesn't exist - static AP_UAVCAN *get_uavcan(uint8_t iface); + // Return uavcan from @driver_index or nullptr if it's not ready or doesn't exist + static AP_UAVCAN *get_uavcan(uint8_t driver_index); + + void init(uint8_t driver_index) override; + + + ///// SRV output ///// + void SRV_push_servos(void); + + ///// LED ///// + bool led_write(uint8_t led_index, uint8_t red, uint8_t green, uint8_t blue); + + ///// GPS ///// + + uint8_t find_gps_without_listener(void); // this function will register the listening class on a first free channel or on the specified channel // if preferred_channel = 0 then free channel will be searched for @@ -66,8 +77,6 @@ class AP_UAVCAN { uint8_t register_gps_listener_to_node(AP_GPS_Backend* new_listener, uint8_t node); - uint8_t find_gps_without_listener(void); - // Removes specified listener from all nodes void remove_gps_listener(AP_GPS_Backend* rem_listener); @@ -78,6 +87,8 @@ class AP_UAVCAN { // Updates all listeners of specified node void update_gps_state(uint8_t node); + ///// BARO ///// + struct Baro_Info { float pressure; float pressure_variance; @@ -85,24 +96,28 @@ class AP_UAVCAN { float temperature_variance; }; + uint8_t find_smallest_free_baro_node(); uint8_t register_baro_listener(AP_Baro_Backend* new_listener, uint8_t preferred_channel); uint8_t register_baro_listener_to_node(AP_Baro_Backend* new_listener, uint8_t node); void remove_baro_listener(AP_Baro_Backend* rem_listener); Baro_Info *find_baro_node(uint8_t node); - uint8_t find_smallest_free_baro_node(); void update_baro_state(uint8_t node); + ///// COMPASS ///// + struct Mag_Info { Vector3f mag_vector; }; + uint8_t find_smallest_free_mag_node(); uint8_t register_mag_listener(AP_Compass_Backend* new_listener, uint8_t preferred_channel); + uint8_t register_mag_listener_to_node(AP_Compass_Backend* new_listener, uint8_t node); void remove_mag_listener(AP_Compass_Backend* rem_listener); Mag_Info *find_mag_node(uint8_t node, uint8_t sensor_id); - uint8_t find_smallest_free_mag_node(); - uint8_t register_mag_listener_to_node(AP_Compass_Backend* new_listener, uint8_t node); void update_mag_state(uint8_t node, uint8_t sensor_id); + ///// BATTERY ///// + struct BatteryInfo_Info { float temperature; float voltage; @@ -112,47 +127,117 @@ class AP_UAVCAN { uint8_t status_flags; }; + uint8_t find_smallest_free_bi_id(); uint8_t register_BM_bi_listener_to_id(AP_BattMonitor_Backend* new_listener, uint8_t id); void remove_BM_bi_listener(AP_BattMonitor_Backend* rem_listener); BatteryInfo_Info *find_bi_id(uint8_t id); - uint8_t find_smallest_free_bi_id(); void update_bi_state(uint8_t id); - // synchronization for RC output - void SRV_sem_take(); - void SRV_sem_give(); - - // synchronization for LED output - bool led_out_sem_take(); - void led_out_sem_give(); - void led_out_send(); +private: + class SystemClock: public uavcan::ISystemClock, uavcan::Noncopyable { + public: + SystemClock() = default; - // output from do_cyclic - void SRV_send_servos(); + void adjustUtc(uavcan::UtcDuration adjustment) override { + utc_adjustment_usec = adjustment.toUSec(); + } + + uavcan::MonotonicTime getMonotonic() const override { + return uavcan::MonotonicTime::fromUSec(AP_HAL::micros64()); + } + + uavcan::UtcTime getUtc() const override { + return uavcan::UtcTime::fromUSec(AP_HAL::micros64() + utc_adjustment_usec); + } + + static SystemClock& instance() { + static SystemClock inst; + return inst; + } + + private: + int64_t utc_adjustment_usec; + }; + + // This will be needed to implement if UAVCAN is used with multithreading + // Such cases will be firmware update, etc. + class RaiiSynchronizer {}; + + void loop(void); + + ///// SRV output ///// + void SRV_send_actuator(); void SRV_send_esc(); -private: - // ------------------------- GPS + ///// LED ///// + void led_out_send(); + + + // UAVCAN parameters + AP_Int8 _uavcan_node; + AP_Int32 _servo_bm; + AP_Int32 _esc_bm; + AP_Int16 _servo_rate_hz; + + + uavcan::Node<0> *_node; + uavcan::HeapBasedPoolAllocator _node_allocator; + uint8_t _driver_index; + char _thread_name[9]; + bool _initialized; + + + ///// SRV output ///// + struct { + uint16_t pulse; + bool esc_pending; + bool servo_pending; + } _SRV_conf[UAVCAN_SRV_NUMBER]; + + uint8_t _SRV_armed; + uint32_t _SRV_last_send_us; + AP_HAL::Semaphore *SRV_sem; + + ///// LED ///// + struct led_device { + uint8_t led_index; + uint8_t red; + uint8_t green; + uint8_t blue; + }; + + struct { + led_device devices[AP_UAVCAN_MAX_LED_DEVICES]; + uint8_t devices_count; + uint64_t last_update; + } _led_conf; + + AP_HAL::Semaphore *_led_out_sem; + + ///// GPS ///// // 255 - means free node uint8_t _gps_nodes[AP_UAVCAN_MAX_GPS_NODES]; + // Counter of how many listeners are connected to this source uint8_t _gps_node_taken[AP_UAVCAN_MAX_GPS_NODES]; + // GPS data of the sources AP_GPS::GPS_State _gps_node_state[AP_UAVCAN_MAX_GPS_NODES]; // 255 - means no connection uint8_t _gps_listener_to_node[AP_UAVCAN_MAX_LISTENERS]; + // Listeners with callbacks to be updated AP_GPS_Backend* _gps_listeners[AP_UAVCAN_MAX_LISTENERS]; - // ------------------------- BARO + ///// BARO ///// uint8_t _baro_nodes[AP_UAVCAN_MAX_BARO_NODES]; uint8_t _baro_node_taken[AP_UAVCAN_MAX_BARO_NODES]; Baro_Info _baro_node_state[AP_UAVCAN_MAX_BARO_NODES]; uint8_t _baro_listener_to_node[AP_UAVCAN_MAX_LISTENERS]; AP_Baro_Backend* _baro_listeners[AP_UAVCAN_MAX_LISTENERS]; - // ------------------------- MAG + ///// COMPASS ///// uint8_t _mag_nodes[AP_UAVCAN_MAX_MAG_NODES]; uint8_t _mag_node_taken[AP_UAVCAN_MAX_MAG_NODES]; Mag_Info _mag_node_state[AP_UAVCAN_MAX_MAG_NODES]; @@ -161,124 +246,12 @@ class AP_UAVCAN { AP_Compass_Backend* _mag_listeners[AP_UAVCAN_MAX_LISTENERS]; uint8_t _mag_listener_sensor_ids[AP_UAVCAN_MAX_LISTENERS]; - // ------------------------- BatteryInfo + ///// BATTERY ///// uint16_t _bi_id[AP_UAVCAN_MAX_BI_NUMBER]; uint16_t _bi_id_taken[AP_UAVCAN_MAX_BI_NUMBER]; BatteryInfo_Info _bi_id_state[AP_UAVCAN_MAX_BI_NUMBER]; uint16_t _bi_BM_listener_to_id[AP_UAVCAN_MAX_LISTENERS]; AP_BattMonitor_Backend* _bi_BM_listeners[AP_UAVCAN_MAX_LISTENERS]; - - struct { - uint16_t pulse; - uint16_t safety_pulse; - uint16_t failsafe_pulse; - bool esc_pending; - bool servo_pending; - } _SRV_conf[UAVCAN_SRV_NUMBER]; - - bool _initialized; - uint8_t _SRV_armed; - uint8_t _SRV_safety; - uint32_t _SRV_last_send_us; - - typedef struct { - bool enabled; - uint8_t led_index; - uavcan::equipment::indication::RGB565 rgb565_color; - } _led_device; - - struct { - bool broadcast_enabled; - _led_device devices[AP_UAVCAN_MAX_LED_DEVICES]; - uint8_t devices_count; - uint64_t last_update; - } _led_conf; - - AP_HAL::Semaphore *SRV_sem; - AP_HAL::Semaphore *_led_out_sem; - - class SystemClock: public uavcan::ISystemClock, uavcan::Noncopyable { - SystemClock() - { - } - - uavcan::UtcDuration utc_adjustment; - virtual void adjustUtc(uavcan::UtcDuration adjustment) - { - utc_adjustment = adjustment; - } - - public: - virtual uavcan::MonotonicTime getMonotonic() const - { - uavcan::uint64_t usec = 0; - usec = AP_HAL::micros64(); - return uavcan::MonotonicTime::fromUSec(usec); - } - virtual uavcan::UtcTime getUtc() const - { - uavcan::UtcTime utc; - uavcan::uint64_t usec = 0; - usec = AP_HAL::micros64(); - utc.fromUSec(usec); - utc += utc_adjustment; - return utc; - } - - static SystemClock& instance() - { - static SystemClock inst; - return inst; - } - }; - - uavcan::Node<0> *_node = nullptr; - - uavcan::ISystemClock& get_system_clock(); - uavcan::ICanDriver* get_can_driver(); - uavcan::Node<0>* get_node(); - - // This will be needed to implement if UAVCAN is used with multithreading - // Such cases will be firmware update, etc. - class RaiiSynchronizer { - public: - RaiiSynchronizer() - { - } - - ~RaiiSynchronizer() - { - } - }; - - uavcan::HeapBasedPoolAllocator _node_allocator; - - AP_Int8 _uavcan_node; - AP_Int32 _servo_bm; - AP_Int32 _esc_bm; - AP_Int16 _servo_rate_hz; - - uint8_t _uavcan_i; - - AP_HAL::CANManager* _parent_can_mgr; - -public: - void do_cyclic(void); - bool try_init(void); - - void SRV_set_safety_pwm(uint32_t chmask, uint16_t pulse_len); - void SRV_set_failsafe_pwm(uint32_t chmask, uint16_t pulse_len); - void SRV_force_safety_on(void); - void SRV_force_safety_off(void); - void SRV_arm_actuators(bool arm); - void SRV_write(uint16_t pulse_len, uint8_t ch); - void SRV_push_servos(void); - bool led_write(uint8_t led_index, uint8_t red, uint8_t green, uint8_t blue); - - void set_parent_can_mgr(AP_HAL::CANManager* parent_can_mgr) - { - _parent_can_mgr = parent_can_mgr; - } }; #endif /* AP_UAVCAN_H_ */ diff --git a/libraries/AP_UAVCAN/examples/UAVCAN_sniffer/UAVCAN_sniffer.cpp b/libraries/AP_UAVCAN/examples/UAVCAN_sniffer/UAVCAN_sniffer.cpp index 8332520d11133..da6860cdb43f9 100644 --- a/libraries/AP_UAVCAN/examples/UAVCAN_sniffer/UAVCAN_sniffer.cpp +++ b/libraries/AP_UAVCAN/examples/UAVCAN_sniffer/UAVCAN_sniffer.cpp @@ -6,14 +6,15 @@ #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS && HAL_WITH_UAVCAN -#include - #include #include #include +#include + +#include + #include -#include #include #include @@ -42,7 +43,7 @@ const AP_HAL::HAL& hal = AP_HAL::get_HAL(); #define UAVCAN_NODE_POOL_SIZE 8192 #define UAVCAN_NODE_POOL_BLOCK_SIZE 256 -#define debug_uavcan(level, fmt, args...) do { hal.console->printf(fmt, ##args); } while (0) +#define debug_uavcan(fmt, args...) do { hal.console->printf(fmt, ##args); } while (0) class UAVCAN_sniffer { public: @@ -52,9 +53,9 @@ class UAVCAN_sniffer { void init(void); void loop(void); void print_stats(void); - + private: - AP_HAL::Semaphore *_led_out_sem; + uint8_t driver_index = 0; class SystemClock: public uavcan::ISystemClock, uavcan::Noncopyable { SystemClock() @@ -94,8 +95,6 @@ class UAVCAN_sniffer { uavcan::Node<0> *_node; uavcan::ISystemClock& get_system_clock(); - uavcan::ICanDriver* get_can_driver(); - uavcan::Node<0>* get_node(); // This will be needed to implement if UAVCAN is used with multithreading // Such cases will be firmware update, etc. @@ -111,13 +110,6 @@ class UAVCAN_sniffer { }; uavcan::HeapBasedPoolAllocator _node_allocator; - - AP_HAL::CANManager* _parent_can_mgr; - - void set_parent_can_mgr(AP_HAL::CANManager* parent_can_mgr) - { - _parent_can_mgr = parent_can_mgr; - } }; static struct { @@ -145,92 +137,97 @@ static void count_msg(const char *name) MSG_CB(uavcan::protocol::NodeStatus, NodeStatus) MSG_CB(uavcan::equipment::gnss::Fix, Fix) +MSG_CB(uavcan::equipment::gnss::Auxiliary, Auxiliary) MSG_CB(uavcan::equipment::ahrs::MagneticFieldStrength, MagneticFieldStrength) +MSG_CB(uavcan::equipment::ahrs::MagneticFieldStrength2, MagneticFieldStrength2); MSG_CB(uavcan::equipment::air_data::StaticPressure, StaticPressure) MSG_CB(uavcan::equipment::air_data::StaticTemperature, StaticTemperature) -MSG_CB(uavcan::equipment::gnss::Auxiliary, Auxiliary) +MSG_CB(uavcan::equipment::power::BatteryInfo, BatteryInfo); MSG_CB(uavcan::equipment::actuator::ArrayCommand, ArrayCommand) MSG_CB(uavcan::equipment::esc::RawCommand, RawCommand) +MSG_CB(uavcan::equipment::indication::LightsCommand, LightsCommand); void UAVCAN_sniffer::init(void) { - uint8_t inum = 0; - const_cast (hal).can_mgr[inum] = new ChibiOS::CANManager; - hal.can_mgr[0]->begin(1000000, inum); - - set_parent_can_mgr(hal.can_mgr[inum]); + uint8_t interface = 0; + AP_HAL::CANManager* can_mgr = new ChibiOS::CANManager; - if (!_parent_can_mgr->is_initialized()) { - hal.console->printf("Can not initialised\n"); + if (can_mgr == nullptr) { + AP_HAL::panic("Couldn't allocate CANManager, something is very wrong"); + } + + const_cast (hal).can_mgr[driver_index] = can_mgr; + can_mgr->begin(1000000, interface); + can_mgr->initialized(true); + + if (!can_mgr->is_initialized()) { + debug_uavcan("Can not initialised\n"); + return; + } + + uavcan::ICanDriver* driver = can_mgr->get_driver(); + if (driver == nullptr) { + return; + } + + _node = new uavcan::Node<0>(*driver, get_system_clock(), _node_allocator); + + if (_node == nullptr) { return; } - auto *node = get_node(); + if (_node->isStarted()) { + return; + } uavcan::NodeID self_node_id(9); - node->setNodeID(self_node_id); - + _node->setNodeID(self_node_id); + char ndname[20]; - snprintf(ndname, sizeof(ndname), "org.ardupilot:%u", inum); - + snprintf(ndname, sizeof(ndname), "org.ardupilot:%u", driver_index); + uavcan::NodeStatusProvider::NodeName name(ndname); - node->setName(name); - + _node->setName(name); + uavcan::protocol::SoftwareVersion sw_version; // Standard type uavcan.protocol.SoftwareVersion sw_version.major = AP_UAVCAN_SW_VERS_MAJOR; sw_version.minor = AP_UAVCAN_SW_VERS_MINOR; - node->setSoftwareVersion(sw_version); - + _node->setSoftwareVersion(sw_version); + uavcan::protocol::HardwareVersion hw_version; // Standard type uavcan.protocol.HardwareVersion - + hw_version.major = AP_UAVCAN_HW_VERS_MAJOR; hw_version.minor = AP_UAVCAN_HW_VERS_MINOR; - node->setHardwareVersion(hw_version); - - const int node_start_res = node->start(); - if (node_start_res < 0) { - debug_uavcan(1, "UAVCAN: node start problem\n\r"); + _node->setHardwareVersion(hw_version); + + int start_res = _node->start(); + if (start_res < 0) { + debug_uavcan("UAVCAN: node start problem\n\r"); + return; } -#define START_CB(mtype, cbname) (new uavcan::Subscriber(*node))->start(cb_ ## cbname) - +#define START_CB(mtype, cbname) (new uavcan::Subscriber(*_node))->start(cb_ ## cbname) + START_CB(uavcan::protocol::NodeStatus, NodeStatus); START_CB(uavcan::equipment::gnss::Fix, Fix); + START_CB(uavcan::equipment::gnss::Auxiliary, Auxiliary); START_CB(uavcan::equipment::ahrs::MagneticFieldStrength, MagneticFieldStrength); + START_CB(uavcan::equipment::ahrs::MagneticFieldStrength2, MagneticFieldStrength2); START_CB(uavcan::equipment::air_data::StaticPressure, StaticPressure); START_CB(uavcan::equipment::air_data::StaticTemperature, StaticTemperature); - START_CB(uavcan::equipment::gnss::Auxiliary, Auxiliary); + START_CB(uavcan::equipment::power::BatteryInfo, BatteryInfo); START_CB(uavcan::equipment::actuator::ArrayCommand, ArrayCommand); START_CB(uavcan::equipment::esc::RawCommand, RawCommand); - + START_CB(uavcan::equipment::indication::LightsCommand, LightsCommand); + + /* * Informing other nodes that we're ready to work. * Default mode is INITIALIZING. */ - node->setModeOperational(); - - debug_uavcan(1, "UAVCAN: init done\n\r"); -} - -uavcan::Node<0> *UAVCAN_sniffer::get_node() -{ - if (_node == nullptr && get_can_driver() != nullptr) { - _node = new uavcan::Node<0>(*get_can_driver(), get_system_clock(), _node_allocator); - } - - return _node; -} + _node->setModeOperational(); -uavcan::ICanDriver * UAVCAN_sniffer::get_can_driver() -{ - if (_parent_can_mgr != nullptr) { - if (_parent_can_mgr->is_initialized() == false) { - return nullptr; - } else { - return _parent_can_mgr->get_driver(); - } - } - return nullptr; + debug_uavcan("UAVCAN: init done\n\r"); } uavcan::ISystemClock & UAVCAN_sniffer::get_system_clock() @@ -240,12 +237,16 @@ uavcan::ISystemClock & UAVCAN_sniffer::get_system_clock() void UAVCAN_sniffer::loop(void) { - auto *node = get_node(); - node->spin(uavcan::MonotonicDuration::fromMSec(1)); + if (_node == nullptr) { + return; + } + + _node->spin(uavcan::MonotonicDuration::fromMSec(1)); } void UAVCAN_sniffer::print_stats(void) { + hal.console->printf("%lu\n", AP_HAL::micros()); for (uint16_t i=0;i<100;i++) { if (counters[i].msg_name == nullptr) { break; @@ -271,7 +272,6 @@ void setup(void) hal.scheduler->delay(2000); hal.console->printf("Starting UAVCAN sniffer\n"); sniffer.init(); - } void loop(void) diff --git a/libraries/SRV_Channel/SRV_Channels.cpp b/libraries/SRV_Channel/SRV_Channels.cpp index a195c941ea4cc..f13fc7ed18c9a 100644 --- a/libraries/SRV_Channel/SRV_Channels.cpp +++ b/libraries/SRV_Channel/SRV_Channels.cpp @@ -138,7 +138,7 @@ const AP_Param::GroupInfo SRV_Channels::var_info[] = { // @Path: ../AP_BLHeli/AP_BLHeli.cpp AP_SUBGROUPINFO(blheli, "_BLH_", 21, SRV_Channels, AP_BLHeli), #endif - + AP_GROUPEND }; @@ -224,7 +224,7 @@ void SRV_Channels::cork() void SRV_Channels::push() { hal.rcout->push(); - + // give volz library a chance to update volz_ptr->update(); @@ -238,8 +238,8 @@ void SRV_Channels::push() #if HAL_WITH_UAVCAN // push outputs to UAVCAN - uint8_t can_num_ifaces = AP_BoardConfig_CAN::get_can_num_ifaces(); - for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_DRIVERS && i < can_num_ifaces; i++) { + uint8_t can_num_drivers = AP::can().get_num_drivers(); + for (uint8_t i = 0; i < can_num_drivers; i++) { AP_UAVCAN *ap_uavcan = AP_UAVCAN::get_uavcan(i); if (ap_uavcan == nullptr) { continue; diff --git a/modules/uavcan b/modules/uavcan index 93d2f8e7ace96..72d4b9aecb5b2 160000 --- a/modules/uavcan +++ b/modules/uavcan @@ -1 +1 @@ -Subproject commit 93d2f8e7ace96f21c4c9326428182b7b403e29fe +Subproject commit 72d4b9aecb5b23a142827bea962f636423d99041