From 4ef8d7c3b6ead0a9680bf9b815668946ebb90546 Mon Sep 17 00:00:00 2001 From: Ilia Date: Wed, 15 Jul 2026 15:53:37 +0300 Subject: [PATCH 1/7] feat: initial piher activation --- .gitmodules | 3 + CMakeLists.txt | 1 + Libs/Canopen | 1 + Libs/Dronecan | 2 +- Src/boards/rl/node_v4/board/project.ioc | 21 +- Src/boards/rl/node_v4/dronecan.cmake | 1 + Src/modules/dronecan/canopen/CMakeLists.txt | 17 ++ Src/modules/dronecan/canopen/canopen.cpp | 91 ++++-- Src/modules/dronecan/canopen/canopen.hpp | 37 +-- Src/modules/dronecan/core/CMakeLists.txt | 1 + Src/modules/dronecan/core/dronecan_module.cpp | 34 +++ Src/peripheral/can/can_ownership.cpp | 37 +++ Src/peripheral/can/can_ownership.hpp | 18 ++ Src/platform/stm32h753xx/CMakeLists.txt | 3 + direct_piher_canopen.py | 280 ++++++++++++++++++ 15 files changed, 501 insertions(+), 46 deletions(-) create mode 160000 Libs/Canopen create mode 100644 Src/peripheral/can/can_ownership.cpp create mode 100644 Src/peripheral/can/can_ownership.hpp create mode 100644 direct_piher_canopen.py 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..9b70c3c0 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) diff --git a/Libs/Canopen b/Libs/Canopen new file mode 160000 index 00000000..47bffaf6 --- /dev/null +++ b/Libs/Canopen @@ -0,0 +1 @@ +Subproject commit 47bffaf6a3128c68db045966d200550d012671d6 diff --git a/Libs/Dronecan b/Libs/Dronecan index 6be0f6c4..11a6e6bd 160000 --- a/Libs/Dronecan +++ b/Libs/Dronecan @@ -1 +1 @@ -Subproject commit 6be0f6c482e134002ff66454ab9f1733f2f7e00f +Subproject commit 11a6e6bd23104f33c18d0320a6e328e09658e850 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..7c7c4806 100644 --- a/Src/boards/rl/node_v4/dronecan.cmake +++ b/Src/boards/rl/node_v4/dronecan.cmake @@ -10,6 +10,7 @@ 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..c0e41516 100644 --- a/Src/modules/dronecan/canopen/canopen.cpp +++ b/Src/modules/dronecan/canopen/canopen.cpp @@ -2,44 +2,93 @@ * 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 + +#include "libcanopen/fdcan.hpp" +#include "peripheral/can/can_ownership.hpp" + +extern "C" FDCAN_HandleTypeDef hfdcan2; REGISTER_MODULE(CanopenModule) void CanopenModule::init() { - set_health(Status::OK); - - if (canDriverInit(1000000, CAN_DRIVER_SECOND) < 0) { + if (!HAL::claimCanPeripheral(&hfdcan2, HAL::CanOwner::CANOPEN)) { set_health(Status::FATAL_MALFANCTION); + set_mode(Mode::STANDBY); + return; } - if (raw_command_sub.init(raw_command_cb) < 0) { + const CanopenFdcanInterfaceConfig interface{ + .handle = &hfdcan2, + .interface_id = 0U, + }; + if (canopenFdcanConfigure(&interface, 1U) < 0) { set_health(Status::FATAL_MALFANCTION); + set_mode(Mode::STANDBY); + return; } + static libcanopen::Node node(canopenFdcanGetTransportApi()); + _node = &node; + if (_node->init(CANOPEN_BITRATE, PIHER_NODE_ID) < 0) { + set_health(Status::FATAL_MALFANCTION); + } else if (_node->sendNmtStart() < 0) { + set_health(Status::MAJOR_FAILURE); + } else { + _nmt_attempts = 1U; + _last_nmt_ms = HAL_GetTick(); + _logger.log_info("PIHER NMT start sent: 000#017F"); + set_health(Status::OK); + } 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) { + if (_node == nullptr) { 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); + const uint32_t now_ms = HAL_GetTick(); + if (!_reported_first_tpdo && _nmt_attempts > 0U && _nmt_attempts < MAX_NMT_ATTEMPTS && + now_ms - _last_nmt_ms >= NMT_RETRY_INTERVAL_MS) { + if (_node->sendNmtStart() < 0) { + set_health(Status::MAJOR_FAILURE); + } else { + _nmt_attempts++; + _last_nmt_ms = now_ms; + } + } + const int16_t received = + _node->spinOnce(PIHER_TPDO_ID, PIHER_TPDO_DLC, MAX_RX_FRAMES_PER_SPIN); + if (received < 0) { + set_health(Status::MAJOR_FAILURE); + return; + } + if (received > 0) { + set_health(Status::OK); + if (!_reported_first_tpdo) { + libcanopen::Frame frame{}; + if (_node->getLastTpdo(frame)) { + char message[96]{}; + (void)snprintf(message, + sizeof(message), + "PIHER active: %03X#%02X%02X%02X%02X%02X%02X%02X%02X", + frame.id, + frame.data[0], + frame.data[1], + frame.data[2], + frame.data[3], + frame.data[4], + frame.data[5], + frame.data[6], + frame.data[7]); + _logger.log_info(message); + } + _reported_first_tpdo = true; + } + } } diff --git a/Src/modules/dronecan/canopen/canopen.hpp b/Src/modules/dronecan/canopen/canopen.hpp index bd1f3ee0..7440bda7 100644 --- a/Src/modules/dronecan/canopen/canopen.hpp +++ b/Src/modules/dronecan/canopen/canopen.hpp @@ -2,35 +2,38 @@ * 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 "common/logging.hpp" +#include "libcanopen/canopen.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; 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 constexpr uint8_t PIHER_NODE_ID = 0x7FU; + static constexpr uint16_t PIHER_TPDO_ID = 0x1FFU; + static constexpr uint8_t PIHER_TPDO_DLC = 8U; + static constexpr uint8_t MAX_RX_FRAMES_PER_SPIN = 8U; + static constexpr uint8_t MAX_NMT_ATTEMPTS = 5U; + static constexpr uint32_t NMT_RETRY_INTERVAL_MS = 700U; + + libcanopen::Node* _node{nullptr}; + uint32_t _last_nmt_ms{0U}; + uint8_t _nmt_attempts{0U}; + bool _reported_first_tpdo{false}; + Logging _logger{"canopen"}; }; -#ifdef __cplusplus -} -#endif - -#endif // SRC_MODULES_CANOPEN_HPP_ +#endif // SRC_MODULES_DRONECAN_CANOPEN_CANOPEN_HPP_ diff --git a/Src/modules/dronecan/core/CMakeLists.txt b/Src/modules/dronecan/core/CMakeLists.txt index c21150db..efdf8bd6 100644 --- a/Src/modules/dronecan/core/CMakeLists.txt +++ b/Src/modules/dronecan/core/CMakeLists.txt @@ -14,6 +14,7 @@ include(${NC_LIBDCNODE_PATH}/platform_specific/${CAN_PLATFORM}/config.cmake) list(APPEND APPLICATION_SOURCES ${libparamsSrc} ${DRONECAN_PLATFORM_SOURCES} + ${ROOT_DIR}/Src/peripheral/can/can_ownership.cpp ${CMAKE_CURRENT_LIST_DIR}/dronecan_module.cpp ) diff --git a/Src/modules/dronecan/core/dronecan_module.cpp b/Src/modules/dronecan/core/dronecan_module.cpp index fe672bb3..a661572b 100644 --- a/Src/modules/dronecan/core/dronecan_module.cpp +++ b/Src/modules/dronecan/core/dronecan_module.cpp @@ -12,6 +12,16 @@ #include "libdcnode/can_driver.h" #include "drivers/board_monitor/board_monitor.hpp" #include "peripheral/iwdg/iwdg.hpp" +#include "peripheral/can/can_ownership.hpp" + +#if defined(USE_PLATFORM_NODE_V3) || defined(USE_PLATFORM_NODE_V4) +#include "fdcan_config.h" + +extern FDCAN_HandleTypeDef hfdcan1; +#if defined(USE_PLATFORM_NODE_V3) +extern FDCAN_HandleTypeDef hfdcan2; +#endif +#endif #ifndef GIT_HASH #warning "GIT_HASH has been assigned to 0 by default." @@ -108,6 +118,30 @@ static void setDefaultNodeName(ParamIndex_t node_name_param_idx) { void DronecanModule::init() { +#if defined(USE_PLATFORM_NODE_V3) || defined(USE_PLATFORM_NODE_V4) + DronecanFdcanInterfaceConfig can_interfaces[2]{}; + size_t can_interface_count = 0U; + if (!HAL::claimCanPeripheral(&hfdcan1, HAL::CanOwner::DRONECAN)) { + set_health(Status::FATAL_MALFANCTION); + set_mode(Mode::STANDBY); + return; + } + can_interfaces[can_interface_count++] = {.handle = &hfdcan1, .interface_id = 0U}; +#if defined(USE_PLATFORM_NODE_V3) + if (!HAL::claimCanPeripheral(&hfdcan2, HAL::CanOwner::DRONECAN)) { + set_health(Status::FATAL_MALFANCTION); + set_mode(Mode::STANDBY); + return; + } + can_interfaces[can_interface_count++] = {.handle = &hfdcan2, .interface_id = 1U}; +#endif + if (dronecanFdcanConfigure(can_interfaces, can_interface_count) < 0) { + set_health(Status::FATAL_MALFANCTION); + set_mode(Mode::STANDBY); + return; + } +#endif + ParamsApi params_api = { .getName = paramsGetName, .isInteger = paramsIsInteger, diff --git a/Src/peripheral/can/can_ownership.cpp b/Src/peripheral/can/can_ownership.cpp new file mode 100644 index 00000000..c93533f5 --- /dev/null +++ b/Src/peripheral/can/can_ownership.cpp @@ -0,0 +1,37 @@ +#include "peripheral/can/can_ownership.hpp" + +#include + +namespace HAL { + +namespace { + +struct Claim { + const void* handle; + CanOwner owner; +}; + +constexpr size_t MAX_CAN_PERIPHERALS = 3U; +Claim claims[MAX_CAN_PERIPHERALS]{}; +size_t claim_count = 0U; + +} // namespace + +bool claimCanPeripheral(const void* handle, const CanOwner owner) { + if (handle == nullptr) { + return false; + } + for (size_t idx = 0U; idx < claim_count; idx++) { + if (claims[idx].handle == handle) { + return claims[idx].owner == owner; + } + } + if (claim_count >= MAX_CAN_PERIPHERALS) { + return false; + } + claims[claim_count] = {.handle = handle, .owner = owner}; + claim_count++; + return true; +} + +} // namespace HAL diff --git a/Src/peripheral/can/can_ownership.hpp b/Src/peripheral/can/can_ownership.hpp new file mode 100644 index 00000000..4d858c20 --- /dev/null +++ b/Src/peripheral/can/can_ownership.hpp @@ -0,0 +1,18 @@ +#ifndef SRC_PERIPHERAL_CAN_CAN_OWNERSHIP_HPP_ +#define SRC_PERIPHERAL_CAN_CAN_OWNERSHIP_HPP_ + +#include + +namespace HAL { + +enum class CanOwner : uint8_t { + DRONECAN = 0, + CANOPEN, + CYPHAL, +}; + +[[nodiscard]] bool claimCanPeripheral(const void* handle, CanOwner owner); + +} // namespace HAL + +#endif // SRC_PERIPHERAL_CAN_CAN_OWNERSHIP_HPP_ 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() diff --git a/direct_piher_canopen.py b/direct_piher_canopen.py new file mode 100644 index 00000000..d8c4e938 --- /dev/null +++ b/direct_piher_canopen.py @@ -0,0 +1,280 @@ +#!/usr/bin/env python3 +"""Direct LAWICEL/CANUSB test for PIHER PST360G2 CANopen.""" + +import argparse +import time + +import serial + + +BITRATE_COMMANDS = { + 10000: "S0", + 20000: "S1", + 50000: "S2", + 100000: "S3", + 125000: "S4", + 250000: "S5", + 500000: "S6", + 800000: "S7", + 1000000: "S8", +} + + +def parse_int(value): + return int(value, 0) + + +def parse_hex_payload(value): + value = value.replace(":", "").replace(" ", "") + if len(value) % 2: + raise argparse.ArgumentTypeError("payload must have an even number of hex digits") + payload = bytes.fromhex(value) + if len(payload) > 8: + raise argparse.ArgumentTypeError("classic CAN payload cannot exceed 8 bytes") + return payload + + +def parse_channel(value): + if "@" not in value: + return value, 1_000_000 + + port, baudrate = value.rsplit("@", 1) + return port, int(baudrate) + + +def read_response(ser, timeout): + deadline = time.monotonic() + timeout + response = bytearray() + + while time.monotonic() < deadline: + if ser.in_waiting: + byte = ser.read(1) + if not byte: + continue + response += byte + if byte == b"\r": + break + else: + time.sleep(0.001) + + return bytes(response) + + +def send_command(ser, command, timeout, expect_response=True): + ser.write(command.encode("ascii") + b"\r") + ser.flush() + + if not expect_response: + print(f"{command:<14} -> ") + return b"" + + response = read_response(ser, timeout) + printable = response.replace(b"\r", b"\\r") + print(f"{command:<14} -> {printable!r}") + return response + + +def standard_frame_command(can_id, payload): + return f"t{can_id:03X}{len(payload):X}{payload.hex().upper()}" + + +def extended_frame_command(can_id, payload): + return f"T{can_id:08X}{len(payload):X}{payload.hex().upper()}" + + +def parse_canusb_frame(response): + try: + frame = response.decode("ascii").strip() + except UnicodeDecodeError: + return None + + if len(frame) < 5: + return None + + if frame[0] == "t": + can_id_digits = 3 + is_extended = False + elif frame[0] == "T": + can_id_digits = 8 + is_extended = True + else: + return None + + dlc_pos = 1 + can_id_digits + if len(frame) < dlc_pos + 1: + return None + + try: + can_id = int(frame[1:dlc_pos], 16) + dlc = int(frame[dlc_pos], 16) + data = bytes.fromhex(frame[dlc_pos + 1 : dlc_pos + 1 + dlc * 2]) + except ValueError: + return None + + if len(data) != dlc: + return None + + return { + "raw": frame, + "can_id": can_id, + "dlc": dlc, + "data": data, + "is_extended": is_extended, + } + + +def describe_piher_pdo(frame): + if frame["is_extended"] or frame["can_id"] != 0x1FF or frame["dlc"] != 8: + return "" + + data = frame["data"] + angle_raw = (data[0] << 8) | data[1] + angle_deg = angle_raw * 360.0 / 4095.0 + return f" angle_raw={angle_raw} angle_deg={angle_deg:.2f}" + + +def init_canusb(ser, bitrate, timeout): + for _ in range(3): + send_command(ser, "", timeout) + + version = send_command(ser, "V", timeout) + if not version.startswith(b"V"): + raise RuntimeError("CANUSB did not return a version response") + + send_command(ser, "C", timeout) + + bitrate_command = BITRATE_COMMANDS.get(bitrate) + if bitrate_command is None: + supported = ", ".join(str(rate) for rate in sorted(BITRATE_COMMANDS)) + raise RuntimeError(f"unsupported bitrate {bitrate}; choose one of {supported}") + + if send_command(ser, bitrate_command, timeout) != b"\r": + raise RuntimeError(f"CANUSB rejected bitrate command {bitrate_command}") + + if send_command(ser, "O", timeout) != b"\r": + raise RuntimeError("CANUSB rejected open command") + + send_command(ser, "F", timeout) + + +def listen(ser, duration, timeout, expected_can_id=None): + deadline = time.monotonic() + duration + frames = [] + + while time.monotonic() < deadline: + response = read_response(ser, min(timeout, max(0.0, deadline - time.monotonic()))) + if response: + printable = response.replace(b"\r", b"\\r") + frame = parse_canusb_frame(response) + if frame: + frames.append(frame) + suffix = describe_piher_pdo(frame) + print( + f"RX -> {printable!r} " + f"id=0x{frame['can_id']:X} dlc={frame['dlc']} data={frame['data'].hex(' ').upper()}{suffix}" + ) + else: + print(f"RX -> {printable!r}") + + if expected_can_id is None: + return frames + + return [frame for frame in frames if frame["can_id"] == expected_can_id] + + +def startup_piher(ser, args): + startup_payload = bytes([0x01, args.node_id]) + startup_command = extended_frame_command(0x00000000, startup_payload) + + print("Checking whether PIHER is already operational...") + pdo_frames = listen(ser, args.verify_timeout, args.timeout, expected_can_id=0x1FF) + if len(pdo_frames) >= args.min_pdo: + print(f"PIHER already operational: received {len(pdo_frames)} TPDO frames") + return True + + print( + f"Starting PIHER with extended-wrapper NMT hack: {startup_command} " + f"(logical NMT start node 0x{args.node_id:02X})" + ) + total_pdo_frames = len(pdo_frames) + + for attempt in range(1, args.startup_retries + 1): + response = send_command(ser, startup_command, args.timeout) + if response != b"Z\r": + printable = response.replace(b"\r", b"\\r") if response else response + print(f"Startup command not accepted on attempt {attempt}: {printable!r}") + else: + print(f"Startup command accepted on attempt {attempt}") + + pdo_frames = listen(ser, args.verify_timeout, args.timeout, expected_can_id=0x1FF) + total_pdo_frames += len(pdo_frames) + if len(pdo_frames) >= args.min_pdo: + print(f"PIHER startup verified: received {len(pdo_frames)} TPDO frames after attempt {attempt}") + return True + + time.sleep(args.gap) + + print(f"PIHER startup not verified: received {total_pdo_frames} total TPDO frames") + return False + + +def main(): + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument( + "--channel", + default="/dev/serial/by-id/usb-STMicroelectronics_STM32_STLink_066AFF505372485067051653-if02@1000000", + ) + parser.add_argument("--bitrate", type=int, default=250000) + parser.add_argument("--timeout", type=float, default=0.5) + parser.add_argument("--listen", type=float, default=0.3) + parser.add_argument("--count", type=int, default=20) + parser.add_argument("--gap", type=float, default=0.2) + parser.add_argument("--node-id", type=parse_int, default=0x7F) + parser.add_argument("--can-id", type=parse_int, default=0x000) + parser.add_argument("--payload", type=parse_hex_payload, default=bytes([0x01, 0x7F])) + parser.add_argument("--extended", action="store_true") + parser.add_argument( + "--raw-send", + action="store_true", + help="send --can-id/--payload repeatedly instead of PIHER startup/verify", + ) + parser.add_argument("--startup-retries", type=int, default=5) + parser.add_argument("--verify-timeout", type=float, default=0.7) + parser.add_argument("--min-pdo", type=int, default=3) + args = parser.parse_args() + + port, serial_baudrate = parse_channel(args.channel) + command_factory = extended_frame_command if args.extended else standard_frame_command + + print(f"Connecting: {port} @ {serial_baudrate}") + print(f"CAN bitrate: {args.bitrate}") + + with serial.Serial(port, baudrate=serial_baudrate, timeout=args.timeout) as ser: + ser.reset_input_buffer() + init_canusb(ser, args.bitrate, args.timeout) + + if not args.raw_send: + ok = startup_piher(ser, args) + send_command(ser, "F", args.timeout) + send_command(ser, "C", args.timeout) + raise SystemExit(0 if ok else 1) + + for attempt in range(1, args.count + 1): + payload = args.payload + if args.can_id == 0x000 and payload == bytes([0x01, 0x7F]): + payload = bytes([0x01, args.node_id]) + + command = command_factory(args.can_id, payload) + response = send_command(ser, command, args.timeout) + if response not in (b"z\r", b"Z\r"): + print(f"TX command was not accepted on attempt {attempt}") + + listen(ser, args.listen, args.timeout) + time.sleep(args.gap) + + send_command(ser, "F", args.timeout) + send_command(ser, "C", args.timeout) + + +if __name__ == "__main__": + main() From 09745c3812bc3922e9b6aab187c1de427b183397 Mon Sep 17 00:00:00 2001 From: Ilia Date: Thu, 16 Jul 2026 16:26:14 +0300 Subject: [PATCH 2/7] feat: add piher angle publishin --- CMakeLists.txt | 6 +- Libs/Dronecan | 2 +- Src/boards/rl/node_v4/dronecan.cmake | 1 + Src/modules/angle_sensor/CMakeLists.txt | 10 ++ Src/modules/angle_sensor/angle_sensor.cpp | 121 ++++++++++++++++++ Src/modules/angle_sensor/angle_sensor.hpp | 46 +++++++ Src/modules/dronecan/canopen/canopen.cpp | 75 +---------- Src/modules/dronecan/canopen/canopen.hpp | 18 +-- Src/modules/dronecan/core/CMakeLists.txt | 1 - Src/modules/dronecan/core/dronecan_module.cpp | 35 ----- Src/peripheral/can/can_ownership.cpp | 37 ------ Src/peripheral/can/can_ownership.hpp | 18 --- 12 files changed, 190 insertions(+), 180 deletions(-) create mode 100644 Src/modules/angle_sensor/CMakeLists.txt create mode 100644 Src/modules/angle_sensor/angle_sensor.cpp create mode 100644 Src/modules/angle_sensor/angle_sensor.hpp delete mode 100644 Src/peripheral/can/can_ownership.cpp delete mode 100644 Src/peripheral/can/can_ownership.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 9b70c3c0..e1a34321 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -330,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/Dronecan b/Libs/Dronecan index 11a6e6bd..6be0f6c4 160000 --- a/Libs/Dronecan +++ b/Libs/Dronecan @@ -1 +1 @@ -Subproject commit 11a6e6bd23104f33c18d0320a6e328e09658e850 +Subproject commit 6be0f6c482e134002ff66454ab9f1733f2f7e00f diff --git a/Src/boards/rl/node_v4/dronecan.cmake b/Src/boards/rl/node_v4/dronecan.cmake index 7c7c4806..05bee64a 100644 --- a/Src/boards/rl/node_v4/dronecan.cmake +++ b/Src/boards/rl/node_v4/dronecan.cmake @@ -11,6 +11,7 @@ rl_include_module(system) rl_include_module(dronecan/core) rl_include_module(dronecan/canopen) +rl_include_module(angle_sensor) if(NOT NODE_V4_DIAG_MINIMAL_MODULES) rl_include_module(dronecan/arming) diff --git a/Src/modules/angle_sensor/CMakeLists.txt b/Src/modules/angle_sensor/CMakeLists.txt new file mode 100644 index 00000000..42c95e78 --- /dev/null +++ b/Src/modules/angle_sensor/CMakeLists.txt @@ -0,0 +1,10 @@ +# Copyright (C) 2026 Ilia Kliantsevich +# Distributed under the terms of the GPL v3 license, available in the file LICENSE. + +list(APPEND APPLICATION_SOURCES + ${CMAKE_CURRENT_LIST_DIR}/angle_sensor.cpp +) + +list(APPEND APPLICATION_HEADERS + ${CMAKE_CURRENT_LIST_DIR} +) diff --git a/Src/modules/angle_sensor/angle_sensor.cpp b/Src/modules/angle_sensor/angle_sensor.cpp new file mode 100644 index 00000000..fd5a0b1a --- /dev/null +++ b/Src/modules/angle_sensor/angle_sensor.cpp @@ -0,0 +1,121 @@ +/** + * This program is free software under the GNU General Public License v3. + * See for details. + * Author: Ilia Kliantsevich + */ + +#include "angle_sensor.hpp" + +#include + +#include "modules/dronecan/canopen/canopen.hpp" + +REGISTER_MODULE(AngleSensorModule) + +void AngleSensorModule::init() { + _node = CanopenModule::getNode(); + if (_node == nullptr) { + set_health(Status::FATAL_MALFANCTION); + set_mode(Mode::STANDBY); + return; + } + + set_mode(Mode::INITIALIZATION); + (void)sendNmtStart(); +} + +bool AngleSensorModule::sendNmtStart() { + _nmt_attempts++; + _last_nmt_ms = HAL_GetTick(); + if (_node->sendNmtStart(PIHER_NODE_ID) < 0) { + set_health(Status::MAJOR_FAILURE); + return false; + } + if (_nmt_attempts == 1U) { + _logger.log_info("PIHER NMT start sent: 000#017F"); + } + return true; +} + +bool AngleSensorModule::updateAngle() { + libcanopen::Frame frame{}; + if (!_node->getLastTpdo(frame)) { + return false; + } + + _active = true; + const uint16_t raw = static_cast((static_cast(frame.data[0]) << 8U) | + frame.data[1]); + if (raw > ANGLE_RAW_MAX) { + set_health(Status::MINOR_FAILURE); + return false; + } + + _angle_millidegrees = static_cast(raw) * 360000U / ANGLE_RAW_MAX; + if (_last_log_ms == 0U) { + logActivation(frame); + _last_log_ms = HAL_GetTick(); + } + set_health(Status::OK); + set_mode(Mode::STANDBY); + return true; +} + +void AngleSensorModule::logActivation(const libcanopen::Frame& frame) const { + char message[128]{}; + (void)snprintf(message, + sizeof(message), + "PIHER active: %03X#%02X%02X%02X%02X%02X%02X%02X%02X angle=%lu.%03lu deg", + frame.id, + frame.data[0], + frame.data[1], + frame.data[2], + frame.data[3], + frame.data[4], + frame.data[5], + frame.data[6], + frame.data[7], + static_cast(_angle_millidegrees / 1000U), + static_cast(_angle_millidegrees % 1000U)); + _logger.log_info(message); +} + +void AngleSensorModule::logAngle() const { + char message[48]{}; + (void)snprintf(message, + sizeof(message), + "PIHER angle=%lu.%03lu deg", + static_cast(_angle_millidegrees / 1000U), + static_cast(_angle_millidegrees % 1000U)); + _logger.log_info(message); +} + +void AngleSensorModule::spin_once() { + if (_node == nullptr) { + return; + } + + const uint32_t now_ms = HAL_GetTick(); + if (!_active && _nmt_attempts < MAX_NMT_ATTEMPTS && + now_ms - _last_nmt_ms >= NMT_RETRY_INTERVAL_MS) { + (void)sendNmtStart(); + } + + const int16_t received = + _node->spinOnce(PIHER_TPDO_ID, PIHER_TPDO_DLC, MAX_RX_FRAMES_PER_SPIN); + if (received < 0) { + set_health(Status::MAJOR_FAILURE); + return; + } + if (received > 0) { + (void)updateAngle(); + } else if (!_active && _nmt_attempts >= MAX_NMT_ATTEMPTS && + now_ms - _last_nmt_ms >= NMT_RETRY_INTERVAL_MS) { + set_health(Status::MAJOR_FAILURE); + } + + if (_active && _last_log_ms != 0U && now_ms - _last_log_ms >= LOG_INTERVAL_MS) { + logAngle(); + _last_log_ms = now_ms; + } +} diff --git a/Src/modules/angle_sensor/angle_sensor.hpp b/Src/modules/angle_sensor/angle_sensor.hpp new file mode 100644 index 00000000..b5eafddb --- /dev/null +++ b/Src/modules/angle_sensor/angle_sensor.hpp @@ -0,0 +1,46 @@ +/** + * This program is free software under the GNU General Public License v3. + * See for details. + * Author: Ilia Kliantsevich + */ + +#ifndef SRC_MODULES_ANGLE_SENSOR_ANGLE_SENSOR_HPP_ +#define SRC_MODULES_ANGLE_SENSOR_ANGLE_SENSOR_HPP_ + +#include "common/logging.hpp" +#include "libcanopen/canopen.hpp" +#include "module.hpp" + +class AngleSensorModule : public Module { +public: + AngleSensorModule() : Module(10.0F, Protocol::DRONECAN) {} + void init() override; + +protected: + void spin_once() override; + +private: + static constexpr uint8_t PIHER_NODE_ID = 0x7FU; + static constexpr uint16_t PIHER_TPDO_ID = 0x1FFU; + static constexpr uint8_t PIHER_TPDO_DLC = 8U; + static constexpr uint8_t MAX_RX_FRAMES_PER_SPIN = 8U; + static constexpr uint8_t MAX_NMT_ATTEMPTS = 5U; + static constexpr uint32_t NMT_RETRY_INTERVAL_MS = 700U; + static constexpr uint32_t LOG_INTERVAL_MS = 1000U; + static constexpr uint16_t ANGLE_RAW_MAX = 4095U; + + bool sendNmtStart(); + bool updateAngle(); + void logActivation(const libcanopen::Frame& frame) const; + void logAngle() const; + + libcanopen::Node* _node{nullptr}; + uint32_t _last_nmt_ms{0U}; + uint32_t _last_log_ms{0U}; + uint32_t _angle_millidegrees{0U}; + uint8_t _nmt_attempts{0U}; + bool _active{false}; + Logging _logger{"ANGLE"}; +}; + +#endif // SRC_MODULES_ANGLE_SENSOR_ANGLE_SENSOR_HPP_ diff --git a/Src/modules/dronecan/canopen/canopen.cpp b/Src/modules/dronecan/canopen/canopen.cpp index c0e41516..b13a2996 100644 --- a/Src/modules/dronecan/canopen/canopen.cpp +++ b/Src/modules/dronecan/canopen/canopen.cpp @@ -7,88 +7,17 @@ #include "canopen.hpp" -#include - #include "libcanopen/fdcan.hpp" -#include "peripheral/can/can_ownership.hpp" - -extern "C" FDCAN_HandleTypeDef hfdcan2; REGISTER_MODULE(CanopenModule) void CanopenModule::init() { - if (!HAL::claimCanPeripheral(&hfdcan2, HAL::CanOwner::CANOPEN)) { - set_health(Status::FATAL_MALFANCTION); - set_mode(Mode::STANDBY); - return; - } - - const CanopenFdcanInterfaceConfig interface{ - .handle = &hfdcan2, - .interface_id = 0U, - }; - if (canopenFdcanConfigure(&interface, 1U) < 0) { - set_health(Status::FATAL_MALFANCTION); - set_mode(Mode::STANDBY); - return; - } - static libcanopen::Node node(canopenFdcanGetTransportApi()); _node = &node; - if (_node->init(CANOPEN_BITRATE, PIHER_NODE_ID) < 0) { - set_health(Status::FATAL_MALFANCTION); - } else if (_node->sendNmtStart() < 0) { - set_health(Status::MAJOR_FAILURE); - } else { - _nmt_attempts = 1U; - _last_nmt_ms = HAL_GetTick(); - _logger.log_info("PIHER NMT start sent: 000#017F"); - set_health(Status::OK); - } + const int16_t result = _node->init(CANOPEN_BITRATE); + set_health(result >= 0 ? Status::OK : Status::FATAL_MALFANCTION); set_mode(Mode::STANDBY); } void CanopenModule::spin_once() { - if (_node == nullptr) { - return; - } - const uint32_t now_ms = HAL_GetTick(); - if (!_reported_first_tpdo && _nmt_attempts > 0U && _nmt_attempts < MAX_NMT_ATTEMPTS && - now_ms - _last_nmt_ms >= NMT_RETRY_INTERVAL_MS) { - if (_node->sendNmtStart() < 0) { - set_health(Status::MAJOR_FAILURE); - } else { - _nmt_attempts++; - _last_nmt_ms = now_ms; - } - } - const int16_t received = - _node->spinOnce(PIHER_TPDO_ID, PIHER_TPDO_DLC, MAX_RX_FRAMES_PER_SPIN); - if (received < 0) { - set_health(Status::MAJOR_FAILURE); - return; - } - if (received > 0) { - set_health(Status::OK); - if (!_reported_first_tpdo) { - libcanopen::Frame frame{}; - if (_node->getLastTpdo(frame)) { - char message[96]{}; - (void)snprintf(message, - sizeof(message), - "PIHER active: %03X#%02X%02X%02X%02X%02X%02X%02X%02X", - frame.id, - frame.data[0], - frame.data[1], - frame.data[2], - frame.data[3], - frame.data[4], - frame.data[5], - frame.data[6], - frame.data[7]); - _logger.log_info(message); - } - _reported_first_tpdo = true; - } - } } diff --git a/Src/modules/dronecan/canopen/canopen.hpp b/Src/modules/dronecan/canopen/canopen.hpp index 7440bda7..2731a4b3 100644 --- a/Src/modules/dronecan/canopen/canopen.hpp +++ b/Src/modules/dronecan/canopen/canopen.hpp @@ -8,7 +8,6 @@ #ifndef SRC_MODULES_DRONECAN_CANOPEN_CANOPEN_HPP_ #define SRC_MODULES_DRONECAN_CANOPEN_CANOPEN_HPP_ -#include "common/logging.hpp" #include "libcanopen/canopen.hpp" #include "module.hpp" @@ -17,23 +16,16 @@ class CanopenModule : public Module { CanopenModule() : Module(10.0F, Protocol::DRONECAN) {} void init() override; + static libcanopen::Node* getNode() { + return _node; + } + protected: void spin_once() override; private: static constexpr uint32_t CANOPEN_BITRATE = 250000U; - static constexpr uint8_t PIHER_NODE_ID = 0x7FU; - static constexpr uint16_t PIHER_TPDO_ID = 0x1FFU; - static constexpr uint8_t PIHER_TPDO_DLC = 8U; - static constexpr uint8_t MAX_RX_FRAMES_PER_SPIN = 8U; - static constexpr uint8_t MAX_NMT_ATTEMPTS = 5U; - static constexpr uint32_t NMT_RETRY_INTERVAL_MS = 700U; - - libcanopen::Node* _node{nullptr}; - uint32_t _last_nmt_ms{0U}; - uint8_t _nmt_attempts{0U}; - bool _reported_first_tpdo{false}; - Logging _logger{"canopen"}; + static inline libcanopen::Node* _node{nullptr}; }; #endif // SRC_MODULES_DRONECAN_CANOPEN_CANOPEN_HPP_ diff --git a/Src/modules/dronecan/core/CMakeLists.txt b/Src/modules/dronecan/core/CMakeLists.txt index efdf8bd6..c21150db 100644 --- a/Src/modules/dronecan/core/CMakeLists.txt +++ b/Src/modules/dronecan/core/CMakeLists.txt @@ -14,7 +14,6 @@ include(${NC_LIBDCNODE_PATH}/platform_specific/${CAN_PLATFORM}/config.cmake) list(APPEND APPLICATION_SOURCES ${libparamsSrc} ${DRONECAN_PLATFORM_SOURCES} - ${ROOT_DIR}/Src/peripheral/can/can_ownership.cpp ${CMAKE_CURRENT_LIST_DIR}/dronecan_module.cpp ) diff --git a/Src/modules/dronecan/core/dronecan_module.cpp b/Src/modules/dronecan/core/dronecan_module.cpp index a661572b..e5d53e48 100644 --- a/Src/modules/dronecan/core/dronecan_module.cpp +++ b/Src/modules/dronecan/core/dronecan_module.cpp @@ -12,16 +12,6 @@ #include "libdcnode/can_driver.h" #include "drivers/board_monitor/board_monitor.hpp" #include "peripheral/iwdg/iwdg.hpp" -#include "peripheral/can/can_ownership.hpp" - -#if defined(USE_PLATFORM_NODE_V3) || defined(USE_PLATFORM_NODE_V4) -#include "fdcan_config.h" - -extern FDCAN_HandleTypeDef hfdcan1; -#if defined(USE_PLATFORM_NODE_V3) -extern FDCAN_HandleTypeDef hfdcan2; -#endif -#endif #ifndef GIT_HASH #warning "GIT_HASH has been assigned to 0 by default." @@ -117,31 +107,6 @@ static void setDefaultNodeName(ParamIndex_t node_name_param_idx) { } void DronecanModule::init() { - -#if defined(USE_PLATFORM_NODE_V3) || defined(USE_PLATFORM_NODE_V4) - DronecanFdcanInterfaceConfig can_interfaces[2]{}; - size_t can_interface_count = 0U; - if (!HAL::claimCanPeripheral(&hfdcan1, HAL::CanOwner::DRONECAN)) { - set_health(Status::FATAL_MALFANCTION); - set_mode(Mode::STANDBY); - return; - } - can_interfaces[can_interface_count++] = {.handle = &hfdcan1, .interface_id = 0U}; -#if defined(USE_PLATFORM_NODE_V3) - if (!HAL::claimCanPeripheral(&hfdcan2, HAL::CanOwner::DRONECAN)) { - set_health(Status::FATAL_MALFANCTION); - set_mode(Mode::STANDBY); - return; - } - can_interfaces[can_interface_count++] = {.handle = &hfdcan2, .interface_id = 1U}; -#endif - if (dronecanFdcanConfigure(can_interfaces, can_interface_count) < 0) { - set_health(Status::FATAL_MALFANCTION); - set_mode(Mode::STANDBY); - return; - } -#endif - ParamsApi params_api = { .getName = paramsGetName, .isInteger = paramsIsInteger, diff --git a/Src/peripheral/can/can_ownership.cpp b/Src/peripheral/can/can_ownership.cpp deleted file mode 100644 index c93533f5..00000000 --- a/Src/peripheral/can/can_ownership.cpp +++ /dev/null @@ -1,37 +0,0 @@ -#include "peripheral/can/can_ownership.hpp" - -#include - -namespace HAL { - -namespace { - -struct Claim { - const void* handle; - CanOwner owner; -}; - -constexpr size_t MAX_CAN_PERIPHERALS = 3U; -Claim claims[MAX_CAN_PERIPHERALS]{}; -size_t claim_count = 0U; - -} // namespace - -bool claimCanPeripheral(const void* handle, const CanOwner owner) { - if (handle == nullptr) { - return false; - } - for (size_t idx = 0U; idx < claim_count; idx++) { - if (claims[idx].handle == handle) { - return claims[idx].owner == owner; - } - } - if (claim_count >= MAX_CAN_PERIPHERALS) { - return false; - } - claims[claim_count] = {.handle = handle, .owner = owner}; - claim_count++; - return true; -} - -} // namespace HAL diff --git a/Src/peripheral/can/can_ownership.hpp b/Src/peripheral/can/can_ownership.hpp deleted file mode 100644 index 4d858c20..00000000 --- a/Src/peripheral/can/can_ownership.hpp +++ /dev/null @@ -1,18 +0,0 @@ -#ifndef SRC_PERIPHERAL_CAN_CAN_OWNERSHIP_HPP_ -#define SRC_PERIPHERAL_CAN_CAN_OWNERSHIP_HPP_ - -#include - -namespace HAL { - -enum class CanOwner : uint8_t { - DRONECAN = 0, - CANOPEN, - CYPHAL, -}; - -[[nodiscard]] bool claimCanPeripheral(const void* handle, CanOwner owner); - -} // namespace HAL - -#endif // SRC_PERIPHERAL_CAN_CAN_OWNERSHIP_HPP_ From 82a64aedd500a2acb75c77a9cdf61746e0dcb04f Mon Sep 17 00:00:00 2001 From: Ilia Date: Thu, 16 Jul 2026 16:40:46 +0300 Subject: [PATCH 3/7] chore: commit libs --- Libs/Canopen | 2 +- Libs/Dronecan | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/Libs/Canopen b/Libs/Canopen index 47bffaf6..fee94f30 160000 --- a/Libs/Canopen +++ b/Libs/Canopen @@ -1 +1 @@ -Subproject commit 47bffaf6a3128c68db045966d200550d012671d6 +Subproject commit fee94f3031b92ca26bdeb8ab3af25cce8f391229 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 From f90a791e18a432577680ab6f8133a4715507105e Mon Sep 17 00:00:00 2001 From: Ilia Date: Fri, 17 Jul 2026 16:29:26 +0300 Subject: [PATCH 4/7] temp: testing of global lss node id change on piher --- Libs/Canopen | 2 +- Src/modules/angle_sensor/angle_sensor.cpp | 91 +++++++++++++++++++++-- Src/modules/angle_sensor/angle_sensor.hpp | 12 ++- Src/modules/dronecan/canopen/canopen.cpp | 5 +- Src/modules/dronecan/canopen/canopen.hpp | 6 ++ 5 files changed, 107 insertions(+), 9 deletions(-) diff --git a/Libs/Canopen b/Libs/Canopen index fee94f30..b42828aa 160000 --- a/Libs/Canopen +++ b/Libs/Canopen @@ -1 +1 @@ -Subproject commit fee94f3031b92ca26bdeb8ab3af25cce8f391229 +Subproject commit b42828aad6c726604ab9a90d4ae725d07d9d4ddb diff --git a/Src/modules/angle_sensor/angle_sensor.cpp b/Src/modules/angle_sensor/angle_sensor.cpp index fd5a0b1a..b10acc1d 100644 --- a/Src/modules/angle_sensor/angle_sensor.cpp +++ b/Src/modules/angle_sensor/angle_sensor.cpp @@ -12,27 +12,108 @@ REGISTER_MODULE(AngleSensorModule) +namespace { + +const char* lssStatusName(const libcanopen::LssStatus status) { + switch (status) { + case libcanopen::LssStatus::SUCCESS: + return "OK"; + case libcanopen::LssStatus::INVALID_ARGUMENT: + return "INVALID_ARGUMENT"; + case libcanopen::LssStatus::TIMEOUT: + return "TIMEOUT"; + case libcanopen::LssStatus::TRANSPORT_ERROR: + return "TRANSPORT_ERROR"; + case libcanopen::LssStatus::UNSUPPORTED: + return "UNSUPPORTED"; + case libcanopen::LssStatus::SERVER_REJECTED: + return "SERVER_REJECTED"; + case libcanopen::LssStatus::INVALID_RESPONSE: + return "INVALID_RESPONSE"; + default: + return "UNKNOWN"; + } +} + +} // namespace + void AngleSensorModule::init() { _node = CanopenModule::getNode(); - if (_node == nullptr) { + _lss_master = CanopenModule::getLssMaster(); + if (_node == nullptr || _lss_master == nullptr) { set_health(Status::FATAL_MALFANCTION); set_mode(Mode::STANDBY); return; } set_mode(Mode::INITIALIZATION); + _lss_configuration_ok = configureNodeIdWithLss(); (void)sendNmtStart(); } +void AngleSensorModule::logLssResult(const char* const operation, + const libcanopen::LssResult& result) const { + char message[112]{}; + (void)snprintf(message, + sizeof(message), + "PIHER LSS %s: %s error=%u extension=%u", + operation, + lssStatusName(result.status), + result.error_code, + result.error_extension); + if (result.status == libcanopen::LssStatus::SUCCESS) { + _logger.log_info(message); + } else { + _logger.log_error(message); + } +} + +bool AngleSensorModule::configureNodeIdWithLss() { + _logger.log_info("PIHER LSS test: global configuration, node 100 -> 127"); + const libcanopen::LssResult enter_result = + _lss_master->switchStateGlobal(libcanopen::LssState::CONFIGURATION); + logLssResult("enter configuration", enter_result); + + bool configuration_ok = enter_result.status == libcanopen::LssStatus::SUCCESS; + if (configuration_ok) { + const libcanopen::LssResult node_result = + _lss_master->configureNodeId(PIHER_NEW_NODE_ID, 10000); + logLssResult("configure node 127", node_result); + configuration_ok = node_result.status == libcanopen::LssStatus::SUCCESS; + if (configuration_ok) { + _node_id = PIHER_NEW_NODE_ID; + _tpdo_id = TPDO1_BASE_ID + PIHER_NEW_NODE_ID; + const libcanopen::LssResult store_result = _lss_master->storeConfiguration(); + logLssResult("store configuration", store_result); + configuration_ok = store_result.status == libcanopen::LssStatus::SUCCESS; + } + } + + const libcanopen::LssResult leave_result = + _lss_master->switchStateGlobal(libcanopen::LssState::WAITING); + logLssResult("enter waiting", leave_result); + configuration_ok = configuration_ok && + leave_result.status == libcanopen::LssStatus::SUCCESS; + if (!configuration_ok) { + set_health(Status::MAJOR_FAILURE); + } + return configuration_ok; +} + bool AngleSensorModule::sendNmtStart() { _nmt_attempts++; _last_nmt_ms = HAL_GetTick(); - if (_node->sendNmtStart(PIHER_NODE_ID) < 0) { + if (_node->sendNmtStart(_node_id) < 0) { set_health(Status::MAJOR_FAILURE); return false; } if (_nmt_attempts == 1U) { - _logger.log_info("PIHER NMT start sent: 000#017F"); + char message[64]{}; + (void)snprintf(message, + sizeof(message), + "PIHER NMT start sent: 000#01%02X", + _node_id); + _logger.log_info(message); } return true; } @@ -56,7 +137,7 @@ bool AngleSensorModule::updateAngle() { logActivation(frame); _last_log_ms = HAL_GetTick(); } - set_health(Status::OK); + set_health(_lss_configuration_ok ? Status::OK : Status::MAJOR_FAILURE); set_mode(Mode::STANDBY); return true; } @@ -102,7 +183,7 @@ void AngleSensorModule::spin_once() { } const int16_t received = - _node->spinOnce(PIHER_TPDO_ID, PIHER_TPDO_DLC, MAX_RX_FRAMES_PER_SPIN); + _node->spinOnce(_tpdo_id, PIHER_TPDO_DLC, MAX_RX_FRAMES_PER_SPIN); if (received < 0) { set_health(Status::MAJOR_FAILURE); return; diff --git a/Src/modules/angle_sensor/angle_sensor.hpp b/Src/modules/angle_sensor/angle_sensor.hpp index b5eafddb..78e1976b 100644 --- a/Src/modules/angle_sensor/angle_sensor.hpp +++ b/Src/modules/angle_sensor/angle_sensor.hpp @@ -9,6 +9,7 @@ #include "common/logging.hpp" #include "libcanopen/canopen.hpp" +#include "libcanopen/lss.hpp" #include "module.hpp" class AngleSensorModule : public Module { @@ -20,8 +21,9 @@ class AngleSensorModule : public Module { void spin_once() override; private: - static constexpr uint8_t PIHER_NODE_ID = 0x7FU; - static constexpr uint16_t PIHER_TPDO_ID = 0x1FFU; + static constexpr uint8_t PIHER_OLD_NODE_ID = 25U; + static constexpr uint8_t PIHER_NEW_NODE_ID = 0x7F; + static constexpr uint16_t TPDO1_BASE_ID = 0x180U; static constexpr uint8_t PIHER_TPDO_DLC = 8U; static constexpr uint8_t MAX_RX_FRAMES_PER_SPIN = 8U; static constexpr uint8_t MAX_NMT_ATTEMPTS = 5U; @@ -30,16 +32,22 @@ class AngleSensorModule : public Module { static constexpr uint16_t ANGLE_RAW_MAX = 4095U; bool sendNmtStart(); + bool configureNodeIdWithLss(); bool updateAngle(); + void logLssResult(const char* operation, const libcanopen::LssResult& result) const; void logActivation(const libcanopen::Frame& frame) const; void logAngle() const; libcanopen::Node* _node{nullptr}; + libcanopen::LssMaster* _lss_master{nullptr}; uint32_t _last_nmt_ms{0U}; uint32_t _last_log_ms{0U}; uint32_t _angle_millidegrees{0U}; uint8_t _nmt_attempts{0U}; + uint8_t _node_id{PIHER_OLD_NODE_ID}; + uint16_t _tpdo_id{TPDO1_BASE_ID + PIHER_OLD_NODE_ID}; bool _active{false}; + bool _lss_configuration_ok{false}; Logging _logger{"ANGLE"}; }; diff --git a/Src/modules/dronecan/canopen/canopen.cpp b/Src/modules/dronecan/canopen/canopen.cpp index b13a2996..97b869a6 100644 --- a/Src/modules/dronecan/canopen/canopen.cpp +++ b/Src/modules/dronecan/canopen/canopen.cpp @@ -12,8 +12,11 @@ REGISTER_MODULE(CanopenModule) void CanopenModule::init() { - static libcanopen::Node node(canopenFdcanGetTransportApi()); + 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::FATAL_MALFANCTION); set_mode(Mode::STANDBY); diff --git a/Src/modules/dronecan/canopen/canopen.hpp b/Src/modules/dronecan/canopen/canopen.hpp index 2731a4b3..ed4fb11c 100644 --- a/Src/modules/dronecan/canopen/canopen.hpp +++ b/Src/modules/dronecan/canopen/canopen.hpp @@ -9,6 +9,7 @@ #define SRC_MODULES_DRONECAN_CANOPEN_CANOPEN_HPP_ #include "libcanopen/canopen.hpp" +#include "libcanopen/lss.hpp" #include "module.hpp" class CanopenModule : public Module { @@ -20,12 +21,17 @@ class CanopenModule : public Module { return _node; } + static libcanopen::LssMaster* getLssMaster() { + return _lss_master; + } + protected: void spin_once() override; private: static constexpr uint32_t CANOPEN_BITRATE = 250000U; static inline libcanopen::Node* _node{nullptr}; + static inline libcanopen::LssMaster* _lss_master{nullptr}; }; #endif // SRC_MODULES_DRONECAN_CANOPEN_CANOPEN_HPP_ From f39fa1f3d8cbfcaaa253f98d1bd5898947bd5e22 Mon Sep 17 00:00:00 2001 From: Ilia Date: Mon, 20 Jul 2026 09:14:39 +0300 Subject: [PATCH 5/7] continue lss experiments --- Src/modules/angle_sensor/angle_sensor.cpp | 37 ++++++++++++++++++----- Src/modules/angle_sensor/angle_sensor.hpp | 6 ++-- 2 files changed, 33 insertions(+), 10 deletions(-) diff --git a/Src/modules/angle_sensor/angle_sensor.cpp b/Src/modules/angle_sensor/angle_sensor.cpp index b10acc1d..cc6c4fbe 100644 --- a/Src/modules/angle_sensor/angle_sensor.cpp +++ b/Src/modules/angle_sensor/angle_sensor.cpp @@ -69,21 +69,42 @@ void AngleSensorModule::logLssResult(const char* const operation, } bool AngleSensorModule::configureNodeIdWithLss() { - _logger.log_info("PIHER LSS test: global configuration, node 100 -> 127"); + char message[72]{}; + (void)snprintf(message, + sizeof(message), + "PIHER LSS test: global configuration, node %u -> %u", + PIHER_OLD_NODE_ID, + PIHER_NEW_NODE_ID); + _logger.log_info(message); + + if (_node->sendNmtPreOperational(PIHER_OLD_NODE_ID) < 0) { + _logger.log_error("PIHER LSS NMT pre-operational: TRANSPORT_ERROR"); + set_health(Status::MAJOR_FAILURE); + return false; + } + _logger.log_info("PIHER LSS NMT pre-operational sent"); + HAL_Delay(LSS_SETTLE_DELAY_MS); + const libcanopen::LssResult enter_result = _lss_master->switchStateGlobal(libcanopen::LssState::CONFIGURATION); logLssResult("enter configuration", enter_result); bool configuration_ok = enter_result.status == libcanopen::LssStatus::SUCCESS; if (configuration_ok) { + HAL_Delay(LSS_SETTLE_DELAY_MS); const libcanopen::LssResult node_result = - _lss_master->configureNodeId(PIHER_NEW_NODE_ID, 10000); - logLssResult("configure node 127", node_result); + _lss_master->configureNodeId(PIHER_NEW_NODE_ID, LSS_TIMEOUT_MS); + (void)snprintf(message, + sizeof(message), + "configure node %u", + PIHER_NEW_NODE_ID); + logLssResult(message, node_result); configuration_ok = node_result.status == libcanopen::LssStatus::SUCCESS; if (configuration_ok) { _node_id = PIHER_NEW_NODE_ID; _tpdo_id = TPDO1_BASE_ID + PIHER_NEW_NODE_ID; - const libcanopen::LssResult store_result = _lss_master->storeConfiguration(); + const libcanopen::LssResult store_result = + _lss_master->storeConfiguration(LSS_TIMEOUT_MS); logLssResult("store configuration", store_result); configuration_ok = store_result.status == libcanopen::LssStatus::SUCCESS; } @@ -156,8 +177,8 @@ void AngleSensorModule::logActivation(const libcanopen::Frame& frame) const { frame.data[5], frame.data[6], frame.data[7], - static_cast(_angle_millidegrees / 1000U), - static_cast(_angle_millidegrees % 1000U)); + static_cast(_angle_millidegrees / 1000U), + static_cast(_angle_millidegrees % 1000U)); _logger.log_info(message); } @@ -166,8 +187,8 @@ void AngleSensorModule::logAngle() const { (void)snprintf(message, sizeof(message), "PIHER angle=%lu.%03lu deg", - static_cast(_angle_millidegrees / 1000U), - static_cast(_angle_millidegrees % 1000U)); + static_cast(_angle_millidegrees / 1000U), + static_cast(_angle_millidegrees % 1000U)); _logger.log_info(message); } diff --git a/Src/modules/angle_sensor/angle_sensor.hpp b/Src/modules/angle_sensor/angle_sensor.hpp index 78e1976b..21c5b707 100644 --- a/Src/modules/angle_sensor/angle_sensor.hpp +++ b/Src/modules/angle_sensor/angle_sensor.hpp @@ -21,12 +21,14 @@ class AngleSensorModule : public Module { void spin_once() override; private: - static constexpr uint8_t PIHER_OLD_NODE_ID = 25U; - static constexpr uint8_t PIHER_NEW_NODE_ID = 0x7F; + static constexpr uint8_t PIHER_OLD_NODE_ID = 127U; + static constexpr uint8_t PIHER_NEW_NODE_ID = 52U; static constexpr uint16_t TPDO1_BASE_ID = 0x180U; static constexpr uint8_t PIHER_TPDO_DLC = 8U; static constexpr uint8_t MAX_RX_FRAMES_PER_SPIN = 8U; static constexpr uint8_t MAX_NMT_ATTEMPTS = 5U; + static constexpr uint32_t LSS_TIMEOUT_MS = 1000U; + static constexpr uint32_t LSS_SETTLE_DELAY_MS = 100U; static constexpr uint32_t NMT_RETRY_INTERVAL_MS = 700U; static constexpr uint32_t LOG_INTERVAL_MS = 1000U; static constexpr uint16_t ANGLE_RAW_MAX = 4095U; From 94b9618604257928175c07f84f5bcad1141a8702 Mon Sep 17 00:00:00 2001 From: Ilia Date: Wed, 22 Jul 2026 10:34:01 +0300 Subject: [PATCH 6/7] feat: remove angle sensor --- Src/boards/rl/node_v4/dronecan.cmake | 2 +- Src/modules/angle_sensor/CMakeLists.txt | 10 - Src/modules/angle_sensor/angle_sensor.cpp | 223 ---------------------- Src/modules/angle_sensor/angle_sensor.hpp | 56 ------ Src/modules/dronecan/canopen/canopen.cpp | 2 +- 5 files changed, 2 insertions(+), 291 deletions(-) delete mode 100644 Src/modules/angle_sensor/CMakeLists.txt delete mode 100644 Src/modules/angle_sensor/angle_sensor.cpp delete mode 100644 Src/modules/angle_sensor/angle_sensor.hpp diff --git a/Src/boards/rl/node_v4/dronecan.cmake b/Src/boards/rl/node_v4/dronecan.cmake index 05bee64a..1a65e0f5 100644 --- a/Src/boards/rl/node_v4/dronecan.cmake +++ b/Src/boards/rl/node_v4/dronecan.cmake @@ -11,7 +11,7 @@ rl_include_module(system) rl_include_module(dronecan/core) rl_include_module(dronecan/canopen) -rl_include_module(angle_sensor) + if(NOT NODE_V4_DIAG_MINIMAL_MODULES) rl_include_module(dronecan/arming) diff --git a/Src/modules/angle_sensor/CMakeLists.txt b/Src/modules/angle_sensor/CMakeLists.txt deleted file mode 100644 index 42c95e78..00000000 --- a/Src/modules/angle_sensor/CMakeLists.txt +++ /dev/null @@ -1,10 +0,0 @@ -# Copyright (C) 2026 Ilia Kliantsevich -# Distributed under the terms of the GPL v3 license, available in the file LICENSE. - -list(APPEND APPLICATION_SOURCES - ${CMAKE_CURRENT_LIST_DIR}/angle_sensor.cpp -) - -list(APPEND APPLICATION_HEADERS - ${CMAKE_CURRENT_LIST_DIR} -) diff --git a/Src/modules/angle_sensor/angle_sensor.cpp b/Src/modules/angle_sensor/angle_sensor.cpp deleted file mode 100644 index cc6c4fbe..00000000 --- a/Src/modules/angle_sensor/angle_sensor.cpp +++ /dev/null @@ -1,223 +0,0 @@ -/** - * This program is free software under the GNU General Public License v3. - * See for details. - * Author: Ilia Kliantsevich - */ - -#include "angle_sensor.hpp" - -#include - -#include "modules/dronecan/canopen/canopen.hpp" - -REGISTER_MODULE(AngleSensorModule) - -namespace { - -const char* lssStatusName(const libcanopen::LssStatus status) { - switch (status) { - case libcanopen::LssStatus::SUCCESS: - return "OK"; - case libcanopen::LssStatus::INVALID_ARGUMENT: - return "INVALID_ARGUMENT"; - case libcanopen::LssStatus::TIMEOUT: - return "TIMEOUT"; - case libcanopen::LssStatus::TRANSPORT_ERROR: - return "TRANSPORT_ERROR"; - case libcanopen::LssStatus::UNSUPPORTED: - return "UNSUPPORTED"; - case libcanopen::LssStatus::SERVER_REJECTED: - return "SERVER_REJECTED"; - case libcanopen::LssStatus::INVALID_RESPONSE: - return "INVALID_RESPONSE"; - default: - return "UNKNOWN"; - } -} - -} // namespace - -void AngleSensorModule::init() { - _node = CanopenModule::getNode(); - _lss_master = CanopenModule::getLssMaster(); - if (_node == nullptr || _lss_master == nullptr) { - set_health(Status::FATAL_MALFANCTION); - set_mode(Mode::STANDBY); - return; - } - - set_mode(Mode::INITIALIZATION); - _lss_configuration_ok = configureNodeIdWithLss(); - (void)sendNmtStart(); -} - -void AngleSensorModule::logLssResult(const char* const operation, - const libcanopen::LssResult& result) const { - char message[112]{}; - (void)snprintf(message, - sizeof(message), - "PIHER LSS %s: %s error=%u extension=%u", - operation, - lssStatusName(result.status), - result.error_code, - result.error_extension); - if (result.status == libcanopen::LssStatus::SUCCESS) { - _logger.log_info(message); - } else { - _logger.log_error(message); - } -} - -bool AngleSensorModule::configureNodeIdWithLss() { - char message[72]{}; - (void)snprintf(message, - sizeof(message), - "PIHER LSS test: global configuration, node %u -> %u", - PIHER_OLD_NODE_ID, - PIHER_NEW_NODE_ID); - _logger.log_info(message); - - if (_node->sendNmtPreOperational(PIHER_OLD_NODE_ID) < 0) { - _logger.log_error("PIHER LSS NMT pre-operational: TRANSPORT_ERROR"); - set_health(Status::MAJOR_FAILURE); - return false; - } - _logger.log_info("PIHER LSS NMT pre-operational sent"); - HAL_Delay(LSS_SETTLE_DELAY_MS); - - const libcanopen::LssResult enter_result = - _lss_master->switchStateGlobal(libcanopen::LssState::CONFIGURATION); - logLssResult("enter configuration", enter_result); - - bool configuration_ok = enter_result.status == libcanopen::LssStatus::SUCCESS; - if (configuration_ok) { - HAL_Delay(LSS_SETTLE_DELAY_MS); - const libcanopen::LssResult node_result = - _lss_master->configureNodeId(PIHER_NEW_NODE_ID, LSS_TIMEOUT_MS); - (void)snprintf(message, - sizeof(message), - "configure node %u", - PIHER_NEW_NODE_ID); - logLssResult(message, node_result); - configuration_ok = node_result.status == libcanopen::LssStatus::SUCCESS; - if (configuration_ok) { - _node_id = PIHER_NEW_NODE_ID; - _tpdo_id = TPDO1_BASE_ID + PIHER_NEW_NODE_ID; - const libcanopen::LssResult store_result = - _lss_master->storeConfiguration(LSS_TIMEOUT_MS); - logLssResult("store configuration", store_result); - configuration_ok = store_result.status == libcanopen::LssStatus::SUCCESS; - } - } - - const libcanopen::LssResult leave_result = - _lss_master->switchStateGlobal(libcanopen::LssState::WAITING); - logLssResult("enter waiting", leave_result); - configuration_ok = configuration_ok && - leave_result.status == libcanopen::LssStatus::SUCCESS; - if (!configuration_ok) { - set_health(Status::MAJOR_FAILURE); - } - return configuration_ok; -} - -bool AngleSensorModule::sendNmtStart() { - _nmt_attempts++; - _last_nmt_ms = HAL_GetTick(); - if (_node->sendNmtStart(_node_id) < 0) { - set_health(Status::MAJOR_FAILURE); - return false; - } - if (_nmt_attempts == 1U) { - char message[64]{}; - (void)snprintf(message, - sizeof(message), - "PIHER NMT start sent: 000#01%02X", - _node_id); - _logger.log_info(message); - } - return true; -} - -bool AngleSensorModule::updateAngle() { - libcanopen::Frame frame{}; - if (!_node->getLastTpdo(frame)) { - return false; - } - - _active = true; - const uint16_t raw = static_cast((static_cast(frame.data[0]) << 8U) | - frame.data[1]); - if (raw > ANGLE_RAW_MAX) { - set_health(Status::MINOR_FAILURE); - return false; - } - - _angle_millidegrees = static_cast(raw) * 360000U / ANGLE_RAW_MAX; - if (_last_log_ms == 0U) { - logActivation(frame); - _last_log_ms = HAL_GetTick(); - } - set_health(_lss_configuration_ok ? Status::OK : Status::MAJOR_FAILURE); - set_mode(Mode::STANDBY); - return true; -} - -void AngleSensorModule::logActivation(const libcanopen::Frame& frame) const { - char message[128]{}; - (void)snprintf(message, - sizeof(message), - "PIHER active: %03X#%02X%02X%02X%02X%02X%02X%02X%02X angle=%lu.%03lu deg", - frame.id, - frame.data[0], - frame.data[1], - frame.data[2], - frame.data[3], - frame.data[4], - frame.data[5], - frame.data[6], - frame.data[7], - static_cast(_angle_millidegrees / 1000U), - static_cast(_angle_millidegrees % 1000U)); - _logger.log_info(message); -} - -void AngleSensorModule::logAngle() const { - char message[48]{}; - (void)snprintf(message, - sizeof(message), - "PIHER angle=%lu.%03lu deg", - static_cast(_angle_millidegrees / 1000U), - static_cast(_angle_millidegrees % 1000U)); - _logger.log_info(message); -} - -void AngleSensorModule::spin_once() { - if (_node == nullptr) { - return; - } - - const uint32_t now_ms = HAL_GetTick(); - if (!_active && _nmt_attempts < MAX_NMT_ATTEMPTS && - now_ms - _last_nmt_ms >= NMT_RETRY_INTERVAL_MS) { - (void)sendNmtStart(); - } - - const int16_t received = - _node->spinOnce(_tpdo_id, PIHER_TPDO_DLC, MAX_RX_FRAMES_PER_SPIN); - if (received < 0) { - set_health(Status::MAJOR_FAILURE); - return; - } - if (received > 0) { - (void)updateAngle(); - } else if (!_active && _nmt_attempts >= MAX_NMT_ATTEMPTS && - now_ms - _last_nmt_ms >= NMT_RETRY_INTERVAL_MS) { - set_health(Status::MAJOR_FAILURE); - } - - if (_active && _last_log_ms != 0U && now_ms - _last_log_ms >= LOG_INTERVAL_MS) { - logAngle(); - _last_log_ms = now_ms; - } -} diff --git a/Src/modules/angle_sensor/angle_sensor.hpp b/Src/modules/angle_sensor/angle_sensor.hpp deleted file mode 100644 index 21c5b707..00000000 --- a/Src/modules/angle_sensor/angle_sensor.hpp +++ /dev/null @@ -1,56 +0,0 @@ -/** - * This program is free software under the GNU General Public License v3. - * See for details. - * Author: Ilia Kliantsevich - */ - -#ifndef SRC_MODULES_ANGLE_SENSOR_ANGLE_SENSOR_HPP_ -#define SRC_MODULES_ANGLE_SENSOR_ANGLE_SENSOR_HPP_ - -#include "common/logging.hpp" -#include "libcanopen/canopen.hpp" -#include "libcanopen/lss.hpp" -#include "module.hpp" - -class AngleSensorModule : public Module { -public: - AngleSensorModule() : Module(10.0F, Protocol::DRONECAN) {} - void init() override; - -protected: - void spin_once() override; - -private: - static constexpr uint8_t PIHER_OLD_NODE_ID = 127U; - static constexpr uint8_t PIHER_NEW_NODE_ID = 52U; - static constexpr uint16_t TPDO1_BASE_ID = 0x180U; - static constexpr uint8_t PIHER_TPDO_DLC = 8U; - static constexpr uint8_t MAX_RX_FRAMES_PER_SPIN = 8U; - static constexpr uint8_t MAX_NMT_ATTEMPTS = 5U; - static constexpr uint32_t LSS_TIMEOUT_MS = 1000U; - static constexpr uint32_t LSS_SETTLE_DELAY_MS = 100U; - static constexpr uint32_t NMT_RETRY_INTERVAL_MS = 700U; - static constexpr uint32_t LOG_INTERVAL_MS = 1000U; - static constexpr uint16_t ANGLE_RAW_MAX = 4095U; - - bool sendNmtStart(); - bool configureNodeIdWithLss(); - bool updateAngle(); - void logLssResult(const char* operation, const libcanopen::LssResult& result) const; - void logActivation(const libcanopen::Frame& frame) const; - void logAngle() const; - - libcanopen::Node* _node{nullptr}; - libcanopen::LssMaster* _lss_master{nullptr}; - uint32_t _last_nmt_ms{0U}; - uint32_t _last_log_ms{0U}; - uint32_t _angle_millidegrees{0U}; - uint8_t _nmt_attempts{0U}; - uint8_t _node_id{PIHER_OLD_NODE_ID}; - uint16_t _tpdo_id{TPDO1_BASE_ID + PIHER_OLD_NODE_ID}; - bool _active{false}; - bool _lss_configuration_ok{false}; - Logging _logger{"ANGLE"}; -}; - -#endif // SRC_MODULES_ANGLE_SENSOR_ANGLE_SENSOR_HPP_ diff --git a/Src/modules/dronecan/canopen/canopen.cpp b/Src/modules/dronecan/canopen/canopen.cpp index 97b869a6..7caf27e7 100644 --- a/Src/modules/dronecan/canopen/canopen.cpp +++ b/Src/modules/dronecan/canopen/canopen.cpp @@ -18,7 +18,7 @@ void CanopenModule::init() { _node = &node; _lss_master = &lss_master; const int16_t result = _node->init(CANOPEN_BITRATE); - set_health(result >= 0 ? Status::OK : Status::FATAL_MALFANCTION); + set_health(result >= 0 ? Status::OK : Status::MINOR_FAILURE); set_mode(Mode::STANDBY); } From 2d96a173ee5dd8412cadc0c4fe4b16b75db19251 Mon Sep 17 00:00:00 2001 From: Ilia Date: Wed, 22 Jul 2026 11:24:09 +0300 Subject: [PATCH 7/7] chore: remove unised script --- direct_piher_canopen.py | 280 ---------------------------------------- 1 file changed, 280 deletions(-) delete mode 100644 direct_piher_canopen.py diff --git a/direct_piher_canopen.py b/direct_piher_canopen.py deleted file mode 100644 index d8c4e938..00000000 --- a/direct_piher_canopen.py +++ /dev/null @@ -1,280 +0,0 @@ -#!/usr/bin/env python3 -"""Direct LAWICEL/CANUSB test for PIHER PST360G2 CANopen.""" - -import argparse -import time - -import serial - - -BITRATE_COMMANDS = { - 10000: "S0", - 20000: "S1", - 50000: "S2", - 100000: "S3", - 125000: "S4", - 250000: "S5", - 500000: "S6", - 800000: "S7", - 1000000: "S8", -} - - -def parse_int(value): - return int(value, 0) - - -def parse_hex_payload(value): - value = value.replace(":", "").replace(" ", "") - if len(value) % 2: - raise argparse.ArgumentTypeError("payload must have an even number of hex digits") - payload = bytes.fromhex(value) - if len(payload) > 8: - raise argparse.ArgumentTypeError("classic CAN payload cannot exceed 8 bytes") - return payload - - -def parse_channel(value): - if "@" not in value: - return value, 1_000_000 - - port, baudrate = value.rsplit("@", 1) - return port, int(baudrate) - - -def read_response(ser, timeout): - deadline = time.monotonic() + timeout - response = bytearray() - - while time.monotonic() < deadline: - if ser.in_waiting: - byte = ser.read(1) - if not byte: - continue - response += byte - if byte == b"\r": - break - else: - time.sleep(0.001) - - return bytes(response) - - -def send_command(ser, command, timeout, expect_response=True): - ser.write(command.encode("ascii") + b"\r") - ser.flush() - - if not expect_response: - print(f"{command:<14} -> ") - return b"" - - response = read_response(ser, timeout) - printable = response.replace(b"\r", b"\\r") - print(f"{command:<14} -> {printable!r}") - return response - - -def standard_frame_command(can_id, payload): - return f"t{can_id:03X}{len(payload):X}{payload.hex().upper()}" - - -def extended_frame_command(can_id, payload): - return f"T{can_id:08X}{len(payload):X}{payload.hex().upper()}" - - -def parse_canusb_frame(response): - try: - frame = response.decode("ascii").strip() - except UnicodeDecodeError: - return None - - if len(frame) < 5: - return None - - if frame[0] == "t": - can_id_digits = 3 - is_extended = False - elif frame[0] == "T": - can_id_digits = 8 - is_extended = True - else: - return None - - dlc_pos = 1 + can_id_digits - if len(frame) < dlc_pos + 1: - return None - - try: - can_id = int(frame[1:dlc_pos], 16) - dlc = int(frame[dlc_pos], 16) - data = bytes.fromhex(frame[dlc_pos + 1 : dlc_pos + 1 + dlc * 2]) - except ValueError: - return None - - if len(data) != dlc: - return None - - return { - "raw": frame, - "can_id": can_id, - "dlc": dlc, - "data": data, - "is_extended": is_extended, - } - - -def describe_piher_pdo(frame): - if frame["is_extended"] or frame["can_id"] != 0x1FF or frame["dlc"] != 8: - return "" - - data = frame["data"] - angle_raw = (data[0] << 8) | data[1] - angle_deg = angle_raw * 360.0 / 4095.0 - return f" angle_raw={angle_raw} angle_deg={angle_deg:.2f}" - - -def init_canusb(ser, bitrate, timeout): - for _ in range(3): - send_command(ser, "", timeout) - - version = send_command(ser, "V", timeout) - if not version.startswith(b"V"): - raise RuntimeError("CANUSB did not return a version response") - - send_command(ser, "C", timeout) - - bitrate_command = BITRATE_COMMANDS.get(bitrate) - if bitrate_command is None: - supported = ", ".join(str(rate) for rate in sorted(BITRATE_COMMANDS)) - raise RuntimeError(f"unsupported bitrate {bitrate}; choose one of {supported}") - - if send_command(ser, bitrate_command, timeout) != b"\r": - raise RuntimeError(f"CANUSB rejected bitrate command {bitrate_command}") - - if send_command(ser, "O", timeout) != b"\r": - raise RuntimeError("CANUSB rejected open command") - - send_command(ser, "F", timeout) - - -def listen(ser, duration, timeout, expected_can_id=None): - deadline = time.monotonic() + duration - frames = [] - - while time.monotonic() < deadline: - response = read_response(ser, min(timeout, max(0.0, deadline - time.monotonic()))) - if response: - printable = response.replace(b"\r", b"\\r") - frame = parse_canusb_frame(response) - if frame: - frames.append(frame) - suffix = describe_piher_pdo(frame) - print( - f"RX -> {printable!r} " - f"id=0x{frame['can_id']:X} dlc={frame['dlc']} data={frame['data'].hex(' ').upper()}{suffix}" - ) - else: - print(f"RX -> {printable!r}") - - if expected_can_id is None: - return frames - - return [frame for frame in frames if frame["can_id"] == expected_can_id] - - -def startup_piher(ser, args): - startup_payload = bytes([0x01, args.node_id]) - startup_command = extended_frame_command(0x00000000, startup_payload) - - print("Checking whether PIHER is already operational...") - pdo_frames = listen(ser, args.verify_timeout, args.timeout, expected_can_id=0x1FF) - if len(pdo_frames) >= args.min_pdo: - print(f"PIHER already operational: received {len(pdo_frames)} TPDO frames") - return True - - print( - f"Starting PIHER with extended-wrapper NMT hack: {startup_command} " - f"(logical NMT start node 0x{args.node_id:02X})" - ) - total_pdo_frames = len(pdo_frames) - - for attempt in range(1, args.startup_retries + 1): - response = send_command(ser, startup_command, args.timeout) - if response != b"Z\r": - printable = response.replace(b"\r", b"\\r") if response else response - print(f"Startup command not accepted on attempt {attempt}: {printable!r}") - else: - print(f"Startup command accepted on attempt {attempt}") - - pdo_frames = listen(ser, args.verify_timeout, args.timeout, expected_can_id=0x1FF) - total_pdo_frames += len(pdo_frames) - if len(pdo_frames) >= args.min_pdo: - print(f"PIHER startup verified: received {len(pdo_frames)} TPDO frames after attempt {attempt}") - return True - - time.sleep(args.gap) - - print(f"PIHER startup not verified: received {total_pdo_frames} total TPDO frames") - return False - - -def main(): - parser = argparse.ArgumentParser(description=__doc__) - parser.add_argument( - "--channel", - default="/dev/serial/by-id/usb-STMicroelectronics_STM32_STLink_066AFF505372485067051653-if02@1000000", - ) - parser.add_argument("--bitrate", type=int, default=250000) - parser.add_argument("--timeout", type=float, default=0.5) - parser.add_argument("--listen", type=float, default=0.3) - parser.add_argument("--count", type=int, default=20) - parser.add_argument("--gap", type=float, default=0.2) - parser.add_argument("--node-id", type=parse_int, default=0x7F) - parser.add_argument("--can-id", type=parse_int, default=0x000) - parser.add_argument("--payload", type=parse_hex_payload, default=bytes([0x01, 0x7F])) - parser.add_argument("--extended", action="store_true") - parser.add_argument( - "--raw-send", - action="store_true", - help="send --can-id/--payload repeatedly instead of PIHER startup/verify", - ) - parser.add_argument("--startup-retries", type=int, default=5) - parser.add_argument("--verify-timeout", type=float, default=0.7) - parser.add_argument("--min-pdo", type=int, default=3) - args = parser.parse_args() - - port, serial_baudrate = parse_channel(args.channel) - command_factory = extended_frame_command if args.extended else standard_frame_command - - print(f"Connecting: {port} @ {serial_baudrate}") - print(f"CAN bitrate: {args.bitrate}") - - with serial.Serial(port, baudrate=serial_baudrate, timeout=args.timeout) as ser: - ser.reset_input_buffer() - init_canusb(ser, args.bitrate, args.timeout) - - if not args.raw_send: - ok = startup_piher(ser, args) - send_command(ser, "F", args.timeout) - send_command(ser, "C", args.timeout) - raise SystemExit(0 if ok else 1) - - for attempt in range(1, args.count + 1): - payload = args.payload - if args.can_id == 0x000 and payload == bytes([0x01, 0x7F]): - payload = bytes([0x01, args.node_id]) - - command = command_factory(args.can_id, payload) - response = send_command(ser, command, args.timeout) - if response not in (b"z\r", b"Z\r"): - print(f"TX command was not accepted on attempt {attempt}") - - listen(ser, args.listen, args.timeout) - time.sleep(args.gap) - - send_command(ser, "F", args.timeout) - send_command(ser, "C", args.timeout) - - -if __name__ == "__main__": - main()