Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_BoardConfig: add config for SLCAN routing

This commit is contained in:
Siddharth Purohit 2018-11-14 16:05:52 +08:00 committed by Andrew Tridgell
parent eb29a7aa44
commit 2790d27976
3 changed files with 22 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
libraries/AP_BoardConfig/AP_BoardConfig_CAN.cpp
View File

@ -34,10 +34,11 @@
#include <AP_HAL_Linux/CAN.h>
#elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
#include <AP_HAL_ChibiOS/CAN.h>
#include <AP_HAL_ChibiOS/CANSerialRouter.h>
#endif
#include <AP_Vehicle/AP_Vehicle.h>
#include <AP_UAVCAN/AP_UAVCAN_SLCAN.h>
#include <AP_UAVCAN/AP_UAVCAN.h>
#include <AP_KDECAN/AP_KDECAN.h>
#include <AP_ToshibaCAN/AP_ToshibaCAN.h>
@ -83,6 +84,13 @@ const AP_Param::GroupInfo AP_BoardConfig_CAN::var_info[] = {
AP_SUBGROUPINFO(_drivers[2], "D3_", 6, AP_BoardConfig_CAN, AP_BoardConfig_CAN::Driver),
#endif
// @Param: SLCAN_RT
// @DisplayName: SLCAN Route
// @Description: CAN Driver ID to be routed to SLCAN, 0 means no routing
// @Range: 0 3
// @User: Advanced
AP_GROUPINFO("SLCAN_RT", 7, AP_BoardConfig_CAN, _slcan_rt, 0),
AP_GROUPEND
};
@ -106,7 +114,6 @@ void AP_BoardConfig_CAN::init()
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 = _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) {
// CAN Manager is the driver
@ -123,6 +130,12 @@ void AP_BoardConfig_CAN::init()
// For this now existing driver (manager), start the physical interface
if (hal.can_mgr[drv_num - 1] != nullptr) {
initret = initret && hal.can_mgr[drv_num - 1]->begin(_interfaces[i]._bitrate, i);
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
if (_slcan_rt == (i+1) && hal.can_mgr[drv_num - 1] != nullptr ) {
ChibiOS_CAN::CanDriver* drv = (ChibiOS_CAN::CanDriver*)hal.can_mgr[drv_num - 1]->get_driver();
slcan_router().init(drv->getIface(i), drv->getUpdateEvent());
}
#endif
} else {
printf("Failed to initialize can interface %d\n\r", i + 1);
}
@ -172,8 +185,11 @@ void AP_BoardConfig_CAN::init()
} else {
continue;
}
_drivers[i]._driver->init(i);
if (_slcan_rt == 0) {
_drivers[i]._driver->init(i, true);
} else {
_drivers[i]._driver->init(i, false);
}
}
}
}

2
libraries/AP_BoardConfig/AP_BoardConfig_CAN.h
View File

@ -130,7 +130,7 @@ private:
Interface _interfaces[MAX_NUMBER_OF_CAN_INTERFACES];
Driver _drivers[MAX_NUMBER_OF_CAN_DRIVERS];
uint8_t _num_drivers;
AP_Int8 _slcan_rt;
static AP_BoardConfig_CAN *_singleton;
};

1
libraries/AP_UAVCAN/AP_UAVCAN_SLCAN.cpp
View File

@ -37,6 +37,7 @@ static uint8_t nibble2hex(uint8_t x)
}
static bool hex2nibble_error;
SLCAN::SLCANRouter* SLCAN::SLCANRouter::_singleton = nullptr;
static uint8_t hex2nibble(char ch)
{