diff --git a/libraries/AP_BoardConfig/AP_BoardConfig_CAN.cpp b/libraries/AP_BoardConfig/AP_BoardConfig_CAN.cpp index c85f86a44e..7e8deae0ef 100644 --- a/libraries/AP_BoardConfig/AP_BoardConfig_CAN.cpp +++ b/libraries/AP_BoardConfig/AP_BoardConfig_CAN.cpp @@ -90,18 +90,10 @@ void AP_BoardConfig_CAN::init() _st_can_debug[i] = (int8_t) _var_info_can[i]._can_debug; } -#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN - px4_setup_canbus(); -#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX - linux_setup_canbus(); -#endif + setup_canbus(); } -#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN -/* - setup CANBUS drivers - */ -void AP_BoardConfig_CAN::px4_setup_canbus(void) +void AP_BoardConfig_CAN::setup_canbus(void) { // Create all drivers that we need bool initret = true; @@ -110,7 +102,13 @@ void AP_BoardConfig_CAN::px4_setup_canbus(void) if (drv_num != 0 && drv_num <= MAX_NUMBER_OF_CAN_DRIVERS) { if (hal.can_mgr[drv_num - 1] == nullptr) { - const_cast (hal).can_mgr[drv_num - 1] = new PX4::PX4CANManager; + + #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN + const_cast (hal).can_mgr[drv_num - 1] = new PX4::PX4CANManager; + #elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX + const_cast (hal).can_mgr[drv_num - 1] = new Linux::CANManager; + #endif + } if (hal.can_mgr[drv_num - 1] != nullptr) { @@ -144,6 +142,7 @@ void AP_BoardConfig_CAN::px4_setup_canbus(void) } else { printf("Failed to initialize uavcan interface %d\n\r", i + 1); } + } else { AP_HAL::panic("Failed to allocate uavcan %d\n\r", i + 1); } @@ -160,38 +159,5 @@ void AP_BoardConfig_CAN::px4_setup_canbus(void) } } } -#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX -void AP_BoardConfig_CAN::linux_setup_canbus(void) { - for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_INTERFACES; i++) { - - uint8_t drv_num = _var_info_can[i]._driver_number; - - if (drv_num != 0 && drv_num <= MAX_NUMBER_OF_CAN_DRIVERS) { - if (hal.can_mgr[drv_num - 1] == nullptr) { - const_cast (hal).can_mgr[drv_num - 1] = new Linux::CANManager; - } - - if (hal.can_mgr[drv_num - 1] != nullptr) { - _var_info_can_protocol[i]._uavcan = new AP_UAVCAN; - if (_var_info_can_protocol[i]._uavcan != nullptr) { - 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]); - - if (!hal.can_mgr[i]->begin(_var_info_can[i]._can_bitrate, i)) { - hal.console->printf("Failed to initialize can_mgr\n"); - } else { - hal.console->printf("can_mgr initialized well\n"); - } - } else { - hal.console->printf("AP_UAVCAN failed to allocate\n"); - } - } - } - - } -} -#endif #endif diff --git a/libraries/AP_BoardConfig/AP_BoardConfig_CAN.h b/libraries/AP_BoardConfig/AP_BoardConfig_CAN.h index c909f2208c..bb382616ba 100644 --- a/libraries/AP_BoardConfig/AP_BoardConfig_CAN.h +++ b/libraries/AP_BoardConfig/AP_BoardConfig_CAN.h @@ -96,11 +96,8 @@ public: static int8_t _st_driver_number[MAX_NUMBER_OF_CAN_INTERFACES]; static int8_t _st_can_debug[MAX_NUMBER_OF_CAN_INTERFACES]; -#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN - void px4_setup_canbus(void); -#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX - void linux_setup_canbus(void); -#endif // HAL_BOARD_LINUX + + void setup_canbus(void); }; #endif diff --git a/libraries/AP_HAL_Linux/CAN.cpp b/libraries/AP_HAL_Linux/CAN.cpp index c1fe6785b3..e39031189e 100644 --- a/libraries/AP_HAL_Linux/CAN.cpp +++ b/libraries/AP_HAL_Linux/CAN.cpp @@ -475,24 +475,8 @@ bool CANManager::begin(uint32_t bitrate, uint8_t can_number) { if (init(can_number) >= 0) { _initialized = true; - - if (p_uavcan != nullptr) { - uint16_t UAVCAN_init_tries; - - // TODO: Limit number of times we try to init UAVCAN and also provide - // the reasonable actions when it fails. - for (UAVCAN_init_tries = 0; UAVCAN_init_tries < CAN_MAX_INIT_TRIES_COUNT; UAVCAN_init_tries++) { - if (p_uavcan->try_init() == true) { - return true; - } - printf("p_uavcan->try_init() false\n"); - hal.scheduler->delay(1); - } - } else { - printf("p_uavcan is nullptr! =("); - } } - return false; + return _initialized; } bool CANManager::is_initialized()