diff --git a/.gitmodules b/.gitmodules index b825b907..14fc5342 100644 --- a/.gitmodules +++ b/.gitmodules @@ -13,3 +13,6 @@ [submodule "Libs/mini-v3-ioc"] path = Libs/mini-v3-ioc url = https://github.com/RaccoonLabHardware/mini-v3-ioc.git +[submodule "Libs/Canopen"] + path = Libs/Canopen + url = git@github.com:Ilyhadev/Canopen.git diff --git a/CMakeLists.txt b/CMakeLists.txt index 44dae4e3..e1a34321 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -9,6 +9,7 @@ set(CMAKE_DIR ${ROOT_DIR}/cmake) set(LIBPARAMS_PATH ${ROOT_DIR}/Libs/libparams) set(LIBPARAMS_CMAKE libparams.cmake) set(NC_LIBDCNODE_PATH "${ROOT_DIR}/Libs/Dronecan" CACHE PATH "Path to libdcnode/DroneCAN source tree") +set(NC_LIBCANOPEN_PATH "${ROOT_DIR}/Libs/Canopen" CACHE PATH "Path to libcanopen source tree") set(BUILD_ROOT_DIR ${ROOT_DIR}/build) include(${CMAKE_DIR}/module.cmake) @@ -329,9 +330,11 @@ elseif(BOARD_PLATFORM STREQUAL "stm32h753xx") set(CMAKE_EXE_LINKER_FLAGS "-T${ldFile} -Wl,-Map=${PROJECT_NAME}.map,--cref") include(${CMAKE_DIR}/toolchain-stm32h753xx.cmake) - add_compile_definitions(NUM_OF_CAN_BUSES=1) add_compile_definitions(CYPHAL_NUM_OF_CAN_BUSES=1) - add_compile_definitions(DRONECAN_FDCAN_PRIMARY=1) + set(DRONECAN_FDCAN_PRIMARY 1 CACHE STRING "Primary FDCAN controller used by DroneCAN") + set(CANOPEN_FDCAN_PRIMARY 2 CACHE STRING "Primary FDCAN controller used by CANopen") + add_compile_definitions(DRONECAN_FDCAN_PRIMARY=${DRONECAN_FDCAN_PRIMARY}) + add_compile_definitions(CANOPEN_FDCAN_PRIMARY=${CANOPEN_FDCAN_PRIMARY}) add_compile_definitions(USE_PWR_LDO_SUPPLY) else() message(FATAL_ERROR "Unknown BOARD_PLATFORM '${BOARD_PLATFORM}'. Expected: ubuntu, stm32f103, stm32g0b1, stm32h753xx.") diff --git a/Libs/Canopen b/Libs/Canopen new file mode 160000 index 00000000..b42828aa --- /dev/null +++ b/Libs/Canopen @@ -0,0 +1 @@ +Subproject commit b42828aad6c726604ab9a90d4ae725d07d9d4ddb diff --git a/Libs/Dronecan b/Libs/Dronecan index 6be0f6c4..b1befe3d 160000 --- a/Libs/Dronecan +++ b/Libs/Dronecan @@ -1 +1 @@ -Subproject commit 6be0f6c482e134002ff66454ab9f1733f2f7e00f +Subproject commit b1befe3deca1702e5d845b3c0f17270d5564c087 diff --git a/Src/boards/rl/node_v4/board/project.ioc b/Src/boards/rl/node_v4/board/project.ioc index fbdc1991..116e0033 100644 --- a/Src/boards/rl/node_v4/board/project.ioc +++ b/Src/boards/rl/node_v4/board/project.ioc @@ -43,11 +43,18 @@ FDCAN1.RxFifo1ElmtsNbr=1 FDCAN1.TransmitPause=ENABLE FDCAN1.TxBuffersNbr=16 FDCAN1.TxFifoQueueElmtsNbr=16 -FDCAN2.CalculateBaudRateNominal=2500000 -FDCAN2.CalculateTimeBitNominal=400 -FDCAN2.CalculateTimeQuantumNominal=133.33333333333334 -FDCAN2.IPParameters=CalculateTimeQuantumNominal,CalculateTimeBitNominal,CalculateBaudRateNominal,TxBuffersNbr -FDCAN2.TxBuffersNbr=32 +FDCAN2.CalculateBaudRateNominal=250000 +FDCAN2.CalculateTimeBitNominal=4000 +FDCAN2.CalculateTimeQuantumNominal=400.0 +FDCAN2.IPParameters=CalculateTimeQuantumNominal,CalculateTimeBitNominal,CalculateBaudRateNominal,TxBuffersNbr,MessageRAMOffset,RxFifo0ElmtsNbr,RxFifo1ElmtsNbr,TxFifoQueueElmtsNbr,NominalPrescaler,NominalTimeSeg1,NominalTimeSeg2 +FDCAN2.MessageRAMOffset=1024 +FDCAN2.NominalPrescaler=48 +FDCAN2.NominalTimeSeg1=7 +FDCAN2.NominalTimeSeg2=2 +FDCAN2.RxFifo0ElmtsNbr=32 +FDCAN2.RxFifo1ElmtsNbr=0 +FDCAN2.TxBuffersNbr=16 +FDCAN2.TxFifoQueueElmtsNbr=16 File.Version=6 GPIO.groupedBy=Group By Peripherals I2C2.IPParameters=Timing @@ -478,8 +485,8 @@ ProjectManager.MainLocation=Core/Src ProjectManager.NoMain=false ProjectManager.PreviousToolchain= ProjectManager.ProjectBuild=false -ProjectManager.ProjectFileName=STM32H753IIK6-V4.ioc -ProjectManager.ProjectName=STM32H753IIK6-V4 +ProjectManager.ProjectFileName=project.ioc +ProjectManager.ProjectName=project ProjectManager.ProjectStructure= ProjectManager.RegisterCallBack= ProjectManager.StackSize=0x400 diff --git a/Src/boards/rl/node_v4/dronecan.cmake b/Src/boards/rl/node_v4/dronecan.cmake index 843ee75d..1a65e0f5 100644 --- a/Src/boards/rl/node_v4/dronecan.cmake +++ b/Src/boards/rl/node_v4/dronecan.cmake @@ -10,6 +10,8 @@ rl_include_module(application) rl_include_module(system) rl_include_module(dronecan/core) +rl_include_module(dronecan/canopen) + if(NOT NODE_V4_DIAG_MINIMAL_MODULES) rl_include_module(dronecan/arming) diff --git a/Src/modules/dronecan/canopen/CMakeLists.txt b/Src/modules/dronecan/canopen/CMakeLists.txt index 142e5ed8..78481a6a 100644 --- a/Src/modules/dronecan/canopen/CMakeLists.txt +++ b/Src/modules/dronecan/canopen/CMakeLists.txt @@ -1,6 +1,23 @@ # Copyright (C) 2023 Dmitry Ponomarev # Distributed under the terms of the GPL v3 license, available in the file LICENSE. +if(NOT EXISTS "${NC_LIBCANOPEN_PATH}/CMakeLists.txt") + message(FATAL_ERROR "libcanopen is not found at NC_LIBCANOPEN_PATH='${NC_LIBCANOPEN_PATH}'.") +endif() + +if(NOT TARGET libcanopen) + add_subdirectory(${NC_LIBCANOPEN_PATH} ${CMAKE_BINARY_DIR}/libcanopen) +endif() + +include(${NC_LIBCANOPEN_PATH}/platform_specific/fdcan/config.cmake) + list(APPEND APPLICATION_SOURCES + ${CANOPEN_PLATFORM_SOURCES} ${CMAKE_CURRENT_LIST_DIR}/canopen.cpp ) + +list(APPEND APPLICATION_HEADERS + ${CANOPEN_PLATFORM_HEADERS} +) + +set(CONFIG_USE_CANOPEN ON) diff --git a/Src/modules/dronecan/canopen/canopen.cpp b/Src/modules/dronecan/canopen/canopen.cpp index d4342569..7caf27e7 100644 --- a/Src/modules/dronecan/canopen/canopen.cpp +++ b/Src/modules/dronecan/canopen/canopen.cpp @@ -2,44 +2,25 @@ * This program is free software under the GNU General Public License v3. * See for details. * Author: Dmitry Ponomarev + * Author: Ilia Kliantsevich */ #include "canopen.hpp" -#include "libdcnode/can_driver.h" -#include "common/algorithms.hpp" + +#include "libcanopen/fdcan.hpp" REGISTER_MODULE(CanopenModule) void CanopenModule::init() { - set_health(Status::OK); - - if (canDriverInit(1000000, CAN_DRIVER_SECOND) < 0) { - set_health(Status::FATAL_MALFANCTION); - } - - if (raw_command_sub.init(raw_command_cb) < 0) { - set_health(Status::FATAL_MALFANCTION); - } - + const libcanopen::TransportApi transport = canopenFdcanGetTransportApi(); + static libcanopen::Node node(transport); + static libcanopen::LssMaster lss_master(transport); + _node = &node; + _lss_master = &lss_master; + const int16_t result = _node->init(CANOPEN_BITRATE); + set_health(result >= 0 ? Status::OK : Status::MINOR_FAILURE); set_mode(Mode::STANDBY); } void CanopenModule::spin_once() { - // We probably want to check TTL here -} - -void CanopenModule::raw_command_cb(const RawCommand_t& msg) { - if (msg.size < RAW_COMMAND_CHANNEL + 1) { - return; - } - - auto raw_command_value = msg.raw_cmd[RAW_COMMAND_CHANNEL]; - uint8_t setpoint_percent = mapFloat((float)raw_command_value, 0.0f, 8191, 0.0f, 100.0f); - - CanardCANFrame frame; - frame.id = RAW_COMMAND_CHANNEL; - frame.data[0] = setpoint_percent; - frame.data_len = 1; - - canDriverTransmit(&frame, CAN_DRIVER_SECOND); } diff --git a/Src/modules/dronecan/canopen/canopen.hpp b/Src/modules/dronecan/canopen/canopen.hpp index bd1f3ee0..ed4fb11c 100644 --- a/Src/modules/dronecan/canopen/canopen.hpp +++ b/Src/modules/dronecan/canopen/canopen.hpp @@ -2,35 +2,36 @@ * This program is free software under the GNU General Public License v3. * See for details. * Author: Dmitry Ponomarev + * Author: Ilia Kliantsevich */ -#ifndef SRC_MODULES_CANOPEN_HPP_ -#define SRC_MODULES_CANOPEN_HPP_ +#ifndef SRC_MODULES_DRONECAN_CANOPEN_CANOPEN_HPP_ +#define SRC_MODULES_DRONECAN_CANOPEN_CANOPEN_HPP_ +#include "libcanopen/canopen.hpp" +#include "libcanopen/lss.hpp" #include "module.hpp" -#include "subscriber.hpp" - -#ifdef __cplusplus -extern "C" { -#endif class CanopenModule : public Module { public: - CanopenModule() : Module(10.0, Protocol::DRONECAN) {} + CanopenModule() : Module(10.0F, Protocol::DRONECAN) {} void init() override; + static libcanopen::Node* getNode() { + return _node; + } + + static libcanopen::LssMaster* getLssMaster() { + return _lss_master; + } + protected: void spin_once() override; private: - static void raw_command_cb(const RawCommand_t& msg); - static inline DronecanSubscriber raw_command_sub; - - static constexpr uint8_t RAW_COMMAND_CHANNEL = 0; + static constexpr uint32_t CANOPEN_BITRATE = 250000U; + static inline libcanopen::Node* _node{nullptr}; + static inline libcanopen::LssMaster* _lss_master{nullptr}; }; -#ifdef __cplusplus -} -#endif - -#endif // SRC_MODULES_CANOPEN_HPP_ +#endif // SRC_MODULES_DRONECAN_CANOPEN_CANOPEN_HPP_ diff --git a/Src/modules/dronecan/core/dronecan_module.cpp b/Src/modules/dronecan/core/dronecan_module.cpp index fe672bb3..e5d53e48 100644 --- a/Src/modules/dronecan/core/dronecan_module.cpp +++ b/Src/modules/dronecan/core/dronecan_module.cpp @@ -107,7 +107,6 @@ static void setDefaultNodeName(ParamIndex_t node_name_param_idx) { } void DronecanModule::init() { - ParamsApi params_api = { .getName = paramsGetName, .isInteger = paramsIsInteger, diff --git a/Src/platform/stm32h753xx/CMakeLists.txt b/Src/platform/stm32h753xx/CMakeLists.txt index 1fc27be5..0cfc6291 100644 --- a/Src/platform/stm32h753xx/CMakeLists.txt +++ b/Src/platform/stm32h753xx/CMakeLists.txt @@ -41,6 +41,9 @@ target_include_directories(${EXECUTABLE} PRIVATE if(CAN_PROTOCOL STREQUAL "dronecan" OR CAN_PROTOCOL STREQUAL "both") target_link_libraries(${EXECUTABLE} PRIVATE libdcnode::libdcnode) endif() +if(CONFIG_USE_CANOPEN) + target_link_libraries(${EXECUTABLE} PRIVATE libcanopen::libcanopen) +endif() if(CAN_PROTOCOL STREQUAL "cyphal" OR CAN_PROTOCOL STREQUAL "both") target_link_libraries(${EXECUTABLE} PRIVATE libcpnode::libcpnode) endif()