ardupilot/libraries/AP_BoardConfig/AP_BoardConfig_CAN.cpp

224 lines
7.9 KiB
C++

/*
This program 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 program 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 <http://www.gnu.org/licenses/>.
*/
/*
* AP_BoardConfig_CAN - board specific configuration for CAN interface
*/
#include <AP_HAL/AP_HAL.h>
#include <AP_Common/AP_Common.h>
#include "AP_BoardConfig.h"
#include "AP_BoardConfig_CAN.h"
#if HAL_WITH_UAVCAN
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
#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>
#include <AP_SerialManager/AP_SerialManager.h>
#include <AP_PiccoloCAN/AP_PiccoloCAN.h>
extern const AP_HAL::HAL& hal;
// table of user settable parameters
const AP_Param::GroupInfo AP_BoardConfig_CAN::var_info[] = {
#if MAX_NUMBER_OF_CAN_INTERFACES > 0
// @Group: P1_
// @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_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_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(_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(_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(_drivers[2], "D3_", 6, AP_BoardConfig_CAN, AP_BoardConfig_CAN::Driver),
#endif
#if AP_UAVCAN_SLCAN_ENABLED
// @Group: SLCAN_
// @Path: ../AP_BoardConfig/canbus_slcan.cpp
AP_SUBGROUPINFO(_slcan, "SLCAN_", 7, AP_BoardConfig_CAN, AP_BoardConfig_CAN::SLCAN_Interface),
#endif
AP_GROUPEND
};
AP_BoardConfig_CAN *AP_BoardConfig_CAN::_singleton;
AP_BoardConfig_CAN::AP_BoardConfig_CAN()
{
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");
}
#endif // CONFIG_HAL_BOARD == HAL_BOARD_SITL
_singleton = this;
}
void AP_BoardConfig_CAN::init()
{
// Create all drivers that we need
bool initret = true;
#if AP_UAVCAN_SLCAN_ENABLED
reset_slcan_serial();
#endif
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
// So if this driver was not created before for other physical interface - do it
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
const_cast <AP_HAL::HAL&> (hal).can_mgr[drv_num - 1] = new Linux::CANManager;
#elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
const_cast <AP_HAL::HAL&> (hal).can_mgr[drv_num - 1] = new ChibiOS::CANManager;
#endif
}
// 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 AP_UAVCAN_SLCAN_ENABLED
if (_slcan._can_port == (i+1) && hal.can_mgr[drv_num - 1] != nullptr ) {
ChibiOS_CAN::CanDriver* drv = (ChibiOS_CAN::CanDriver*)hal.can_mgr[drv_num - 1]->get_driver();
ChibiOS_CAN::CanIface::slcan_router().init(drv->getIface(i), drv->getUpdateEvent());
}
#endif
} else {
printf("Failed to initialize can interface %d\n\r", i + 1);
}
}
}
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 (prot_type == Protocol_Type_UAVCAN) {
_drivers[i]._driver = _drivers[i]._uavcan = new AP_UAVCAN;
if (_drivers[i]._driver == nullptr) {
AP_HAL::panic("Failed to allocate uavcan %d\n\r", i + 1);
continue;
}
AP_Param::load_object_from_eeprom(_drivers[i]._uavcan, AP_UAVCAN::var_info);
} else if (prot_type == Protocol_Type_KDECAN) {
// To be replaced with macro saying if KDECAN library is included
#if APM_BUILD_TYPE(APM_BUILD_ArduCopter) || APM_BUILD_TYPE(APM_BUILD_ArduPlane) || APM_BUILD_TYPE(APM_BUILD_ArduSub)
_drivers[i]._driver = _drivers[i]._kdecan = new AP_KDECAN;
if (_drivers[i]._driver == nullptr) {
AP_HAL::panic("Failed to allocate KDECAN %d\n\r", i + 1);
continue;
}
AP_Param::load_object_from_eeprom(_drivers[i]._kdecan, AP_KDECAN::var_info);
#endif
} else if (prot_type == Protocol_Type_ToshibaCAN) {
_drivers[i]._driver = _drivers[i]._tcan = new AP_ToshibaCAN;
if (_drivers[i]._driver == nullptr) {
AP_BoardConfig::config_error("ToshibaCAN init failed");
continue;
}
#if HAL_PICCOLO_CAN_ENABLE
} else if (prot_type == Protocol_Type_PiccoloCAN) {
_drivers[i]._driver = _drivers[i]._pcan = new AP_PiccoloCAN;
if (_drivers[i]._driver == nullptr) {
AP_BoardConfig::config_error("PiccoloCAN init failed");
continue;
}
#endif
} else {
continue;
}
#if AP_UAVCAN_SLCAN_ENABLED
if (_slcan._can_port == 0) {
_drivers[i]._driver->init(i, true);
} else {
_drivers[i]._driver->init(i, false);
}
#else
_drivers[i]._driver->init(i, true);
#endif
}
}
}
#if AP_UAVCAN_SLCAN_ENABLED
AP_HAL::UARTDriver *AP_BoardConfig_CAN::get_slcan_serial()
{
if (_slcan._ser_port != -1) {
return AP::serialmanager().get_serial_by_id(_slcan._ser_port);
}
AP_HAL::UARTDriver *ser_port = AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_SLCAN, 0);
if (ser_port != nullptr) {
if (ser_port->is_initialized()) {
return ser_port;
}
}
return nullptr;
}
#endif
AP_BoardConfig_CAN& AP::can() {
return *AP_BoardConfig_CAN::get_singleton();
}
#endif