From 44e897cf2da7a390f4fec3979b3579686b9a8174 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Sun, 22 Jan 2023 18:31:16 +1100 Subject: [PATCH] AP_CANManager: setup CANManager for DroneCAN Driver tests --- libraries/AP_CANManager/AP_CANManager.cpp | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/libraries/AP_CANManager/AP_CANManager.cpp b/libraries/AP_CANManager/AP_CANManager.cpp index ab6c437e04..72b45648c6 100644 --- a/libraries/AP_CANManager/AP_CANManager.cpp +++ b/libraries/AP_CANManager/AP_CANManager.cpp @@ -115,6 +115,7 @@ AP_CANManager::AP_CANManager() _singleton = this; } +#if !AP_TEST_DRONECAN_DRIVERS void AP_CANManager::init() { WITH_SEMAPHORE(_sem); @@ -282,7 +283,26 @@ void AP_CANManager::init() _driver_type_cache[drv_num] = drv_type[drv_num]; } } +#else +void AP_CANManager::init() +{ + WITH_SEMAPHORE(_sem); + for (uint8_t i = 0; i < HAL_NUM_CAN_IFACES; i++) { + if ((Driver_Type) _drv_param[i]._driver_type.get() == Driver_Type_UAVCAN) { + _drivers[i] = _drv_param[i]._uavcan = new AP_UAVCAN(i); + if (_drivers[i] == nullptr) { + AP_BoardConfig::allocation_error("uavcan %d", i + 1); + continue; + } + + AP_Param::load_object_from_eeprom((AP_UAVCAN*)_drivers[i], AP_UAVCAN::var_info); + _drivers[i]->init(i, true); + _driver_type_cache[i] = (Driver_Type) _drv_param[i]._driver_type.get(); + } + } +} +#endif /* register a new CAN driver */