feat(canopen): basic impementation

.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

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.")

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

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)

Src/modules/dronecan/canopen/CMakeLists.txt

@@ -1,6 +1,23 @@
 # Copyright (C) 2023 Dmitry Ponomarev <ponomarevda96@gmail.com>
 # 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)

Src/modules/dronecan/canopen/canopen.cpp

@@ -2,44 +2,25 @@
  * This program is free software under the GNU General Public License v3.
  * See <https://www.gnu.org/licenses/> for details.
  * Author: Dmitry Ponomarev <ponomarevda96@gmail.com>
+ * Author: Ilia Kliantsevich <iliawork112005@gmail.com>
  */
 
 #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);
 }

Src/modules/dronecan/canopen/canopen.hpp

@@ -2,35 +2,36 @@
  * This program is free software under the GNU General Public License v3.
  * See <https://www.gnu.org/licenses/> for details.
  * Author: Dmitry Ponomarev <ponomarevda96@gmail.com>
+ * Author: Ilia Kliantsevich <iliawork112005@gmail.com>
  */
 
-#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<RawCommand_t> 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_

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,

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()