diff --git a/libraries/AP_HAL_VRBRAIN/CAN.cpp b/libraries/AP_HAL_VRBRAIN/CAN.cpp index 970d462135..2916a4b950 100644 --- a/libraries/AP_HAL_VRBRAIN/CAN.cpp +++ b/libraries/AP_HAL_VRBRAIN/CAN.cpp @@ -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 @@ -829,7 +827,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(); @@ -1081,16 +1079,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 f69fb2b087..230b72ffe1 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 @@ public: 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 61b05eb5f8..c2eb53d35c 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 b4a74372a4..136e2cceb8 100644 --- a/libraries/AP_HAL_VRBRAIN/Scheduler.h +++ b/libraries/AP_HAL_VRBRAIN/Scheduler.h @@ -59,8 +59,6 @@ public: 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 @@ private: 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);