From 520783d4b61f297df697c8183a4d64572e6beb93 Mon Sep 17 00:00:00 2001 From: Matt Quick Date: Mon, 12 Oct 2020 15:56:46 -0600 Subject: [PATCH 1/5] Refactor CAN interface for additional protocols --- Firmware/MotorControl/main.cpp | 20 ++++--- Firmware/Tupfile.lua | 4 +- .../communication/{ => can}/can_helpers.hpp | 0 .../communication/{ => can}/can_simple.cpp | 31 ++++++++++ .../communication/{ => can}/can_simple.hpp | 13 ++-- .../{interface_can.cpp => can/odrive_can.cpp} | 36 ++++------- Firmware/communication/can/odrive_can.hpp | 59 +++++++++++++++++++ Firmware/communication/communication.cpp | 2 +- Firmware/communication/interface_can.hpp | 55 +---------------- 9 files changed, 127 insertions(+), 93 deletions(-) rename Firmware/communication/{ => can}/can_helpers.hpp (100%) rename Firmware/communication/{ => can}/can_simple.cpp (90%) rename Firmware/communication/{ => can}/can_simple.hpp (94%) rename Firmware/communication/{interface_can.cpp => can/odrive_can.cpp} (89%) create mode 100644 Firmware/communication/can/odrive_can.hpp diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp index bba1ad2f..10f6378d 100644 --- a/Firmware/MotorControl/main.cpp +++ b/Firmware/MotorControl/main.cpp @@ -266,24 +266,23 @@ void vApplicationIdleHook(void) { odrv.system_stats_.max_stack_usage_usb = stack_size_usb_thread - uxTaskGetStackHighWaterMark(usb_thread) * sizeof(StackType_t); odrv.system_stats_.max_stack_usage_uart = stack_size_uart_thread - uxTaskGetStackHighWaterMark(uart_thread) * sizeof(StackType_t); odrv.system_stats_.max_stack_usage_startup = stack_size_default_task - uxTaskGetStackHighWaterMark(defaultTaskHandle) * sizeof(StackType_t); - odrv.system_stats_.max_stack_usage_can = odCAN->stack_size_ - uxTaskGetStackHighWaterMark(odCAN->thread_id_) * sizeof(StackType_t); + odrv.system_stats_.max_stack_usage_can = odCAN ? (odCAN->stack_size_ - uxTaskGetStackHighWaterMark(odCAN->thread_id_) * sizeof(StackType_t)) : 0; odrv.system_stats_.stack_size_axis = axes[0].stack_size_; odrv.system_stats_.stack_size_usb = stack_size_usb_thread; odrv.system_stats_.stack_size_uart = stack_size_uart_thread; odrv.system_stats_.stack_size_startup = stack_size_default_task; - odrv.system_stats_.stack_size_can = odCAN->stack_size_; + odrv.system_stats_.stack_size_can = odCAN ? odCAN->stack_size_ : 0; odrv.system_stats_.prio_axis = osThreadGetPriority(axes[0].thread_id_); odrv.system_stats_.prio_usb = osThreadGetPriority(usb_thread); odrv.system_stats_.prio_uart = osThreadGetPriority(uart_thread); odrv.system_stats_.prio_startup = osThreadGetPriority(defaultTaskHandle); - odrv.system_stats_.prio_can = osThreadGetPriority(odCAN->thread_id_); + odrv.system_stats_.prio_can = odCAN ? osThreadGetPriority(odCAN->thread_id_) : 0; status_led_controller.update(); } } - } /** @@ -432,9 +431,6 @@ void ODrive::control_loop_cb(uint32_t timestamp) { axis.max_endstop_.update(); } - MEASURE_TIME(axis.task_times_.can_heartbeat) - odCAN->send_cyclic(axis); - MEASURE_TIME(axis.task_times_.controller_update) axis.controller_.update(); // uses position and velocity from encoder @@ -834,7 +830,15 @@ extern "C" int main(void) { osSemaphoreWait(sem_can, 0); // Construct all objects. - odCAN = new ODriveCAN(can_config, &hcan1); + if (odrv.config_.enable_can_a) { + switch (can_config.protocol) { + case ODriveIntf::CanIntf::Protocol::PROTOCOL_SIMPLE: + odCAN = new CANSimple(can_config, &hcan1); + break; + default: + break; + } + } // Create main thread osThreadDef(defaultTask, rtos_main, osPriorityNormal, 0, stack_size_default_task / sizeof(StackType_t)); diff --git a/Firmware/Tupfile.lua b/Firmware/Tupfile.lua index 9fb17f2e..4e12774d 100644 --- a/Firmware/Tupfile.lua +++ b/Firmware/Tupfile.lua @@ -83,12 +83,12 @@ odrive_firmware_pkg = { 'Drivers/STM32/stm32_gpio.cpp', 'Drivers/STM32/stm32_nvm.c', 'Drivers/STM32/stm32_spi_arbiter.cpp', - 'communication/can_simple.cpp', + 'communication/can/can_simple.cpp', + 'communication/can/odrive_can.cpp', 'communication/communication.cpp', 'communication/ascii_protocol.cpp', 'communication/interface_uart.cpp', 'communication/interface_usb.cpp', - 'communication/interface_can.cpp', 'communication/interface_i2c.cpp', 'fibre/cpp/protocol.cpp', 'FreeRTOS-openocd.c', diff --git a/Firmware/communication/can_helpers.hpp b/Firmware/communication/can/can_helpers.hpp similarity index 100% rename from Firmware/communication/can_helpers.hpp rename to Firmware/communication/can/can_helpers.hpp diff --git a/Firmware/communication/can_simple.cpp b/Firmware/communication/can/can_simple.cpp similarity index 90% rename from Firmware/communication/can_simple.cpp rename to Firmware/communication/can/can_simple.cpp index af897495..2751f29e 100644 --- a/Firmware/communication/can_simple.cpp +++ b/Firmware/communication/can/can_simple.cpp @@ -291,6 +291,37 @@ void CANSimple::clear_errors_callback(Axis& axis, const can_Message_t& msg) { odrv.clear_errors(); // TODO: might want to clear axis errors only } +uint32_t CANSimple::service_stack() { + uint32_t nextServiceTime = UINT32_MAX; + uint32_t now = HAL_GetTick(); + + for (auto& a: axes) { + MEASURE_TIME(a.task_times_.can_heartbeat) { + if (a.config_.can.heartbeat_rate_ms > 0) { + if ((now - a.can_.last_heartbeat) >= a.config_.can.heartbeat_rate_ms) { + if (send_heartbeat(a) >= 0) + a.can_.last_heartbeat = now; + } + + int nextAxisService = a.can_.last_heartbeat + a.config_.can.heartbeat_rate_ms - now; + nextServiceTime = std::min(nextServiceTime, static_cast(std::max(0, nextAxisService))); + } + + if (a.config_.can.encoder_rate_ms > 0) { + if ((now - a.can_.last_encoder) >= a.config_.can.encoder_rate_ms) { + if (get_encoder_estimates_callback(a) >= 0) + a.can_.last_encoder = now; + } + + int nextAxisService = a.can_.last_encoder + a.config_.can.encoder_rate_ms - now; + nextServiceTime = std::min(nextServiceTime, static_cast(std::max(0, nextAxisService))); + } + } + } + + return nextServiceTime; +} + int32_t CANSimple::send_heartbeat(const Axis& axis) { can_Message_t txmsg; txmsg.id = axis.config_.can.node_id << NUM_CMD_ID_BITS; diff --git a/Firmware/communication/can_simple.hpp b/Firmware/communication/can/can_simple.hpp similarity index 94% rename from Firmware/communication/can_simple.hpp rename to Firmware/communication/can/can_simple.hpp index 81ce4150..56025168 100644 --- a/Firmware/communication/can_simple.hpp +++ b/Firmware/communication/can/can_simple.hpp @@ -1,9 +1,9 @@ #ifndef __CAN_SIMPLE_HPP_ #define __CAN_SIMPLE_HPP_ -#include "interface_can.hpp" +#include "odrive_can.hpp" -class CANSimple { +class CANSimple : public ODriveCAN { public: enum { MSG_CO_NMT_CTRL = 0x000, // CANOpen NMT Message REC @@ -34,14 +34,19 @@ class CANSimple { MSG_CO_HEARTBEAT_CMD = 0x700, // CANOpen NMT Heartbeat SEND }; - static void handle_can_message(const can_Message_t& msg); - static void doCommand(Axis& axis, const can_Message_t& cmd); + using ODriveCAN::ODriveCAN; + // Cyclic Senders static int32_t send_heartbeat(const Axis& axis); static void send_cyclic(Axis& axis); private: + uint32_t service_stack() final; + void handle_can_message(const can_Message_t& msg) final; + + static void doCommand(Axis& axis, const can_Message_t& cmd); + // Get functions (msg.rtr bit must be set) static int32_t get_motor_error_callback(const Axis& axis); static int32_t get_encoder_error_callback(const Axis& axis); diff --git a/Firmware/communication/interface_can.cpp b/Firmware/communication/can/odrive_can.cpp similarity index 89% rename from Firmware/communication/interface_can.cpp rename to Firmware/communication/can/odrive_can.cpp index 8a557878..53df5e7d 100644 --- a/Firmware/communication/interface_can.cpp +++ b/Firmware/communication/can/odrive_can.cpp @@ -1,14 +1,11 @@ -#include "interface_can.hpp" - -#include "fibre/crc.hpp" -#include "freertos_vars.h" -#include "utils.hpp" +#include "odrive_can.hpp" #include #include -// Specific CAN Protocols -#include "can_simple.hpp" +#include "fibre/crc.hpp" +#include "freertos_vars.h" +#include "utils.hpp" // Safer context handling via maps instead of arrays // #include @@ -25,18 +22,16 @@ void ODriveCAN::can_server_thread() { for (;;) { uint32_t status = HAL_CAN_GetError(handle_); if (status == HAL_CAN_ERROR_NONE) { - can_Message_t rxmsg; + uint32_t nextServiceTime = service_stack(); - osSemaphoreWait(sem_can, 10); // Poll every 10ms regardless of sempahore status - while (available()) { + uint32_t rxNum = available(); + for (uint32_t i = 0; i < rxNum; i++) { + can_Message_t rxmsg; read(rxmsg); - switch (config_.protocol) { - case PROTOCOL_SIMPLE: - CANSimple::handle_can_message(rxmsg); - break; - } + handle_can_message(rxmsg); } HAL_CAN_ActivateNotification(handle_, CAN_IT_RX_FIFO0_MSG_PENDING | CAN_IT_TX_MAILBOX_EMPTY); + osSemaphoreWait(sem_can, nextServiceTime); } else { if (status == HAL_CAN_ERROR_TIMEOUT) { HAL_CAN_ResetError(handle_); @@ -176,17 +171,6 @@ void ODriveCAN::set_error(Error error) { error_ |= error; } -// This function is called by each axis. -// It provides an abstraction from the specific CAN protocol in use -void ODriveCAN::send_cyclic(Axis &axis) { - // Handle heartbeat message - switch (config_.protocol) { - case PROTOCOL_SIMPLE: - CANSimple::send_cyclic(axis); - break; - } -} - void HAL_CAN_TxMailbox0CompleteCallback(CAN_HandleTypeDef *hcan) { HAL_CAN_DeactivateNotification(hcan, CAN_IT_TX_MAILBOX_EMPTY); osSemaphoreRelease(sem_can); diff --git a/Firmware/communication/can/odrive_can.hpp b/Firmware/communication/can/odrive_can.hpp new file mode 100644 index 00000000..d483c671 --- /dev/null +++ b/Firmware/communication/can/odrive_can.hpp @@ -0,0 +1,59 @@ +#ifndef __ODRIVE_CAN_HPP +#define __ODRIVE_CAN_HPP + +#include + +#include "can_helpers.hpp" +#include "fibre/protocol.hpp" +#include "odrive_main.h" + +#define CAN_CLK_HZ (42000000) +#define CAN_CLK_MHZ (42) + +// Anonymous enum for defining the most common CAN baud rates +enum { + CAN_BAUD_125K = 125000, + CAN_BAUD_250K = 250000, + CAN_BAUD_500K = 500000, + CAN_BAUD_1000K = 1000000, + CAN_BAUD_1M = 1000000 +}; + +class ODriveCAN : public ODriveIntf::CanIntf { + public: + struct Config_t { + uint32_t baud_rate = CAN_BAUD_250K; + Protocol protocol = PROTOCOL_SIMPLE; + }; + + ODriveCAN(ODriveCAN::Config_t &config, CAN_HandleTypeDef *handle); + + // Thread Relevant Data + osThreadId thread_id_; + const uint32_t stack_size_ = 1024; // Bytes + Error error_ = ERROR_NONE; + + volatile bool thread_id_valid_ = false; + bool start_can_server(); + void can_server_thread(); + void reinit_can(); + void set_error(Error error); + + // I/O Functions + uint32_t available(); + int32_t write(can_Message_t &txmsg); + bool read(can_Message_t &rxmsg); + + ODriveCAN::Config_t &config_; + + protected: + virtual uint32_t service_stack() = 0; + virtual void handle_can_message(const can_Message_t &msg) = 0; + + private: + CAN_HandleTypeDef *handle_ = nullptr; + + void set_baud_rate(uint32_t baudRate); +}; + +#endif // __ODRIVE_CAN_HPP diff --git a/Firmware/communication/communication.cpp b/Firmware/communication/communication.cpp index 6e882dcf..9d5671ed 100644 --- a/Firmware/communication/communication.cpp +++ b/Firmware/communication/communication.cpp @@ -55,7 +55,7 @@ void init_communication(void) { start_i2c_server(); } - if (odrv.config_.enable_can_a) { + if (odCAN && odrv.config_.enable_can_a) { odCAN->start_can_server(); } } diff --git a/Firmware/communication/interface_can.hpp b/Firmware/communication/interface_can.hpp index 89223f31..53d97321 100644 --- a/Firmware/communication/interface_can.hpp +++ b/Firmware/communication/interface_can.hpp @@ -1,56 +1,7 @@ #ifndef __INTERFACE_CAN_HPP #define __INTERFACE_CAN_HPP -#include -#include "fibre/protocol.hpp" -#include "odrive_main.h" -#include "can_helpers.hpp" +#include +// Other protocol implementations here -#define CAN_CLK_HZ (42000000) -#define CAN_CLK_MHZ (42) - -// Anonymous enum for defining the most common CAN baud rates -enum { - CAN_BAUD_125K = 125000, - CAN_BAUD_250K = 250000, - CAN_BAUD_500K = 500000, - CAN_BAUD_1000K = 1000000, - CAN_BAUD_1M = 1000000 -}; - -class ODriveCAN : public ODriveIntf::CanIntf { - public: - struct Config_t { - uint32_t baud_rate = CAN_BAUD_250K; - Protocol protocol = PROTOCOL_SIMPLE; - }; - - ODriveCAN(ODriveCAN::Config_t &config, CAN_HandleTypeDef *handle); - - // Thread Relevant Data - osThreadId thread_id_; - const uint32_t stack_size_ = 1024; // Bytes - Error error_ = ERROR_NONE; - - volatile bool thread_id_valid_ = false; - bool start_can_server(); - void can_server_thread(); - void send_cyclic(Axis& axis); - void reinit_can(); - - void set_error(Error error); - - // I/O Functions - uint32_t available(); - int32_t write(can_Message_t &txmsg); - bool read(can_Message_t &rxmsg); - - ODriveCAN::Config_t &config_; - -private: - CAN_HandleTypeDef *handle_ = nullptr; - - void set_baud_rate(uint32_t baudRate); -}; - -#endif // __INTERFACE_CAN_HPP +#endif From 4e350823b46f47fb185d7682a5f37cb048eec1e8 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Fri, 20 Nov 2020 18:22:14 +0100 Subject: [PATCH 2/5] refactor CAN --- CHANGELOG.md | 1 + Firmware/Board/v3/Src/can.c | 61 +++-- Firmware/Board/v3/board.cpp | 2 - Firmware/MotorControl/encoder.hpp | 4 +- Firmware/MotorControl/main.cpp | 21 +- Firmware/MotorControl/odrive_main.h | 7 +- Firmware/communication/can/can_simple.cpp | 106 +++++--- Firmware/communication/can/can_simple.hpp | 48 ++-- Firmware/communication/can/odrive_can.cpp | 313 ++++++++++++---------- Firmware/communication/can/odrive_can.hpp | 62 +++-- Firmware/communication/communication.cpp | 4 +- Firmware/odrive-interface.yaml | 9 +- tools/odrive/enums.py | 2 +- tools/odrive/tests/can_test.py | 1 + 14 files changed, 362 insertions(+), 279 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 2f919929..8355a361 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -61,6 +61,7 @@ Please add a note of your changes below this heading if you make a Pull Request. * `.encoder.config.offset` was renamed to ``.encoder.config.phase_offset` * `.encoder.config.offset_float` was renamed to ``.encoder.config.phase_offset_float` * `.config.brake_resistance == 0.0` is no longer a valid way to disable the brake resistor. Use `.config.enable_brake_resistor` instead. +* `.can.set_baud_rate()` was removed. The baudrate is now automatically updated when writing to `.can.config.baud_rate`. # Releases ## [0.5.1] - 2020-09-27 diff --git a/Firmware/Board/v3/Src/can.c b/Firmware/Board/v3/Src/can.c index 18086b27..132ff2eb 100644 --- a/Firmware/Board/v3/Src/can.c +++ b/Firmware/Board/v3/Src/can.c @@ -56,31 +56,46 @@ /* USER CODE END 0 */ -CAN_HandleTypeDef hcan1; +CAN_HandleTypeDef hcan1 = { + .Instance = CAN1, + .Init = { + .Prescaler = 8, + .Mode = CAN_MODE_NORMAL, + .SyncJumpWidth = CAN_SJW_4TQ, + .TimeSeg1 = CAN_BS1_16TQ, + .TimeSeg2 = CAN_BS2_4TQ, + .TimeTriggeredMode = DISABLE, + .AutoBusOff = ENABLE, + .AutoWakeUp = ENABLE, + .AutoRetransmission = ENABLE, + .ReceiveFifoLocked = DISABLE, + .TransmitFifoPriority = DISABLE, + } +}; /* CAN1 init function */ -void MX_CAN1_Init(void) -{ - - hcan1.Instance = CAN1; - hcan1.Init.Prescaler = 8; - hcan1.Init.Mode = CAN_MODE_NORMAL; - hcan1.Init.SyncJumpWidth = CAN_SJW_4TQ; - hcan1.Init.TimeSeg1 = CAN_BS1_16TQ; - hcan1.Init.TimeSeg2 = CAN_BS2_4TQ; - hcan1.Init.TimeTriggeredMode = DISABLE; - hcan1.Init.AutoBusOff = ENABLE; - hcan1.Init.AutoWakeUp = ENABLE; - hcan1.Init.AutoRetransmission = ENABLE; - hcan1.Init.ReceiveFifoLocked = DISABLE; - hcan1.Init.TransmitFifoPriority = DISABLE; - if (HAL_CAN_Init(&hcan1) != HAL_OK) - { - _Error_Handler(__FILE__, __LINE__); - } - -} - +//void MX_CAN1_Init(void) +//{ +// +// hcan1.Instance = CAN1; +// hcan1.Init.Prescaler = 8; +// hcan1.Init.Mode = CAN_MODE_NORMAL; +// hcan1.Init.SyncJumpWidth = CAN_SJW_4TQ; +// hcan1.Init.TimeSeg1 = CAN_BS1_16TQ; +// hcan1.Init.TimeSeg2 = CAN_BS2_4TQ; +// hcan1.Init.TimeTriggeredMode = DISABLE; +// hcan1.Init.AutoBusOff = ENABLE; +// hcan1.Init.AutoWakeUp = ENABLE; +// hcan1.Init.AutoRetransmission = ENABLE; +// hcan1.Init.ReceiveFifoLocked = DISABLE; +// hcan1.Init.TransmitFifoPriority = DISABLE; +// if (HAL_CAN_Init(&hcan1) != HAL_OK) +// { +// _Error_Handler(__FILE__, __LINE__); +// } +// +//} +// void HAL_CAN_MspInit(CAN_HandleTypeDef* canHandle) { diff --git a/Firmware/Board/v3/board.cpp b/Firmware/Board/v3/board.cpp index 68bc9912..5f7a3699 100644 --- a/Firmware/Board/v3/board.cpp +++ b/Firmware/Board/v3/board.cpp @@ -357,8 +357,6 @@ bool board_init() { // mode initialization won't override the CAN mode. if (odrv.config_.gpio_modes[15] != ODriveIntf::GPIO_MODE_CAN_A || odrv.config_.gpio_modes[16] != ODriveIntf::GPIO_MODE_CAN_A) { odrv.misconfigured_ = true; - } else { - MX_CAN1_Init(); } } diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index f20436ab..d764582f 100644 --- a/Firmware/MotorControl/encoder.hpp +++ b/Firmware/MotorControl/encoder.hpp @@ -1,7 +1,9 @@ #ifndef __ENCODER_HPP #define __ENCODER_HPP -#include +class Encoder; + +#include // needed for arm_math.h #include #include "utils.hpp" #include diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp index 10f6378d..ee7fc64c 100644 --- a/Firmware/MotorControl/main.cpp +++ b/Firmware/MotorControl/main.cpp @@ -28,7 +28,7 @@ extern char _estack; // provided by the linker script ODriveCAN::Config_t can_config; -ODriveCAN *odCAN = nullptr; +CANSimple *can_simple = nullptr; ODrive odrv{}; @@ -146,7 +146,7 @@ static void config_clear_all() { } static bool config_apply_all() { - bool success = true; + bool success = odrv.can_.apply_config(); for (size_t i = 0; (i < AXIS_COUNT) && success; ++i) { success = encoders[i].apply_config(motors[i].config_.motor_type) && axes[i].controller_.apply_config() @@ -266,19 +266,19 @@ void vApplicationIdleHook(void) { odrv.system_stats_.max_stack_usage_usb = stack_size_usb_thread - uxTaskGetStackHighWaterMark(usb_thread) * sizeof(StackType_t); odrv.system_stats_.max_stack_usage_uart = stack_size_uart_thread - uxTaskGetStackHighWaterMark(uart_thread) * sizeof(StackType_t); odrv.system_stats_.max_stack_usage_startup = stack_size_default_task - uxTaskGetStackHighWaterMark(defaultTaskHandle) * sizeof(StackType_t); - odrv.system_stats_.max_stack_usage_can = odCAN ? (odCAN->stack_size_ - uxTaskGetStackHighWaterMark(odCAN->thread_id_) * sizeof(StackType_t)) : 0; + odrv.system_stats_.max_stack_usage_can = odrv.can_.stack_size_ - uxTaskGetStackHighWaterMark(odrv.can_.thread_id_) * sizeof(StackType_t); odrv.system_stats_.stack_size_axis = axes[0].stack_size_; odrv.system_stats_.stack_size_usb = stack_size_usb_thread; odrv.system_stats_.stack_size_uart = stack_size_uart_thread; odrv.system_stats_.stack_size_startup = stack_size_default_task; - odrv.system_stats_.stack_size_can = odCAN ? odCAN->stack_size_ : 0; + odrv.system_stats_.stack_size_can = odrv.can_.stack_size_; odrv.system_stats_.prio_axis = osThreadGetPriority(axes[0].thread_id_); odrv.system_stats_.prio_usb = osThreadGetPriority(usb_thread); odrv.system_stats_.prio_uart = osThreadGetPriority(uart_thread); odrv.system_stats_.prio_startup = osThreadGetPriority(defaultTaskHandle); - odrv.system_stats_.prio_can = odCAN ? osThreadGetPriority(odCAN->thread_id_) : 0; + odrv.system_stats_.prio_can = osThreadGetPriority(odrv.can_.thread_id_); status_led_controller.update(); } @@ -829,17 +829,6 @@ extern "C" int main(void) { sem_can = osSemaphoreCreate(osSemaphore(sem_can), 1); osSemaphoreWait(sem_can, 0); - // Construct all objects. - if (odrv.config_.enable_can_a) { - switch (can_config.protocol) { - case ODriveIntf::CanIntf::Protocol::PROTOCOL_SIMPLE: - odCAN = new CANSimple(can_config, &hcan1); - break; - default: - break; - } - } - // Create main thread osThreadDef(defaultTask, rtos_main, osPriorityNormal, 0, stack_size_default_task / sizeof(StackType_t)); defaultTaskHandle = osThreadCreate(osThread(defaultTask), NULL); diff --git a/Firmware/MotorControl/odrive_main.h b/Firmware/MotorControl/odrive_main.h index 96bedf79..ad9873ba 100644 --- a/Firmware/MotorControl/odrive_main.h +++ b/Firmware/MotorControl/odrive_main.h @@ -120,9 +120,6 @@ struct TaskTimes { // Forward Declarations class Axis; class Motor; -class ODriveCAN; - -extern ODriveCAN *odCAN; // TODO: move // this is technically not thread-safe but practically it might be @@ -151,6 +148,7 @@ inline ENUMTYPE operator ~ (ENUMTYPE a) { return static_cast(~static_c #include #include #include +#include // Defined in autogen/version.c based on git-derived version numbers extern "C" { @@ -188,7 +186,6 @@ public: void control_loop_cb(uint32_t timestamp); Axis& get_axis(int num) { return axes[num]; } - ODriveCAN& get_can() { return *odCAN; } uint32_t get_interrupt_status(int32_t irqn); uint32_t get_dma_status(uint8_t stream_num); @@ -227,6 +224,8 @@ public: nullptr // data_src TODO: change data type }; + ODriveCAN can_; + BoardConfig_t config_; uint32_t user_config_loaded_ = 0; bool misconfigured_ = false; diff --git a/Firmware/communication/can/can_simple.cpp b/Firmware/communication/can/can_simple.cpp index 2751f29e..2e72adfe 100644 --- a/Firmware/communication/can/can_simple.cpp +++ b/Firmware/communication/can/can_simple.cpp @@ -3,6 +3,41 @@ #include +bool CANSimple::init() { + for (size_t i = 0; i < AXIS_COUNT; ++i) { + if (!renew_subscription(i)) { + return false; + } + } + + return true; +} + +bool CANSimple::renew_subscription(size_t i) { + Axis& axis = axes[i]; + + // TODO: remove these two lines (see comment in header) + node_ids_[i] = axis.config_.can.node_id; + extended_node_ids_[i] = axis.config_.can.is_extended; + + MsgIdFilterSpecs filter = { + .mask = (uint32_t)(0xffffffff << NUM_CMD_ID_BITS) + }; + if (axis.config_.can.is_extended) { + filter.id = (uint32_t)(axis.config_.can.node_id << NUM_CMD_ID_BITS); + } else { + filter.id = (uint16_t)(axis.config_.can.node_id << NUM_CMD_ID_BITS); + } + + if (subscription_handles_[i]) { + canbus_->unsubscribe(subscription_handles_[i]); + } + + return canbus_->subscribe(filter, [](void* ctx, const can_Message_t& msg) { + ((CANSimple*)ctx)->handle_can_message(msg); + }, this, &subscription_handles_[i]); +} + void CANSimple::handle_can_message(const can_Message_t& msg) { // Frame // nodeID | CMD @@ -11,13 +46,13 @@ void CANSimple::handle_can_message(const can_Message_t& msg) { for (auto& axis : axes) { if ((axis.config_.can.node_id == nodeID) && (axis.config_.can.is_extended == msg.isExt)) { - doCommand(axis, msg); + do_command(axis, msg); return; } } } -void CANSimple::doCommand(Axis& axis, const can_Message_t& msg) { +void CANSimple::do_command(Axis& axis, const can_Message_t& msg) { const uint32_t cmd = get_cmd_id(msg.id); axis.watchdog_feed(); switch (cmd) { @@ -118,7 +153,7 @@ void CANSimple::estop_callback(Axis& axis, const can_Message_t& msg) { axis.error_ |= Axis::ERROR_ESTOP_REQUESTED; } -int32_t CANSimple::get_motor_error_callback(const Axis& axis) { +bool CANSimple::get_motor_error_callback(const Axis& axis) { can_Message_t txmsg; txmsg.id = axis.config_.can.node_id << NUM_CMD_ID_BITS; txmsg.id += MSG_GET_MOTOR_ERROR; // heartbeat ID @@ -127,10 +162,10 @@ int32_t CANSimple::get_motor_error_callback(const Axis& axis) { can_setSignal(txmsg, axis.motor_.error_, 0, 32, true); - return odCAN->write(txmsg); + return canbus_->send_message(txmsg); } -int32_t CANSimple::get_encoder_error_callback(const Axis& axis) { +bool CANSimple::get_encoder_error_callback(const Axis& axis) { can_Message_t txmsg; txmsg.id = axis.config_.can.node_id << NUM_CMD_ID_BITS; txmsg.id += MSG_GET_ENCODER_ERROR; // heartbeat ID @@ -139,10 +174,10 @@ int32_t CANSimple::get_encoder_error_callback(const Axis& axis) { can_setSignal(txmsg, axis.encoder_.error_, 0, 32, true); - return odCAN->write(txmsg); + return canbus_->send_message(txmsg); } -int32_t CANSimple::get_sensorless_error_callback(const Axis& axis) { +bool CANSimple::get_sensorless_error_callback(const Axis& axis) { can_Message_t txmsg; txmsg.id = axis.config_.can.node_id << NUM_CMD_ID_BITS; txmsg.id += MSG_GET_SENSORLESS_ERROR; // heartbeat ID @@ -151,7 +186,7 @@ int32_t CANSimple::get_sensorless_error_callback(const Axis& axis) { can_setSignal(txmsg, axis.sensorless_estimator_.error_, 0, 32, true); - return odCAN->write(txmsg); + return canbus_->send_message(txmsg); } void CANSimple::set_axis_nodeid_callback(Axis& axis, const can_Message_t& msg) { @@ -166,7 +201,7 @@ void CANSimple::set_axis_startup_config_callback(Axis& axis, const can_Message_t // Not Implemented } -int32_t CANSimple::get_encoder_estimates_callback(const Axis& axis) { +bool CANSimple::get_encoder_estimates_callback(const Axis& axis) { can_Message_t txmsg; txmsg.id = axis.config_.can.node_id << NUM_CMD_ID_BITS; txmsg.id += MSG_GET_ENCODER_ESTIMATES; // heartbeat ID @@ -176,10 +211,10 @@ int32_t CANSimple::get_encoder_estimates_callback(const Axis& axis) { can_setSignal(txmsg, axis.encoder_.pos_estimate_.any().value_or(0.0f), 0, 32, true); can_setSignal(txmsg, axis.encoder_.vel_estimate_.any().value_or(0.0f), 32, 32, true); - return odCAN->write(txmsg); + return canbus_->send_message(txmsg); } -int32_t CANSimple::get_sensorless_estimates_callback(const Axis& axis) { +bool CANSimple::get_sensorless_estimates_callback(const Axis& axis) { can_Message_t txmsg; txmsg.id = axis.config_.can.node_id << NUM_CMD_ID_BITS; txmsg.id += MSG_GET_SENSORLESS_ESTIMATES; // heartbeat ID @@ -191,10 +226,10 @@ int32_t CANSimple::get_sensorless_estimates_callback(const Axis& axis) { can_setSignal(txmsg, axis.sensorless_estimator_.pll_pos_, 0, 32, true); can_setSignal(txmsg, axis.sensorless_estimator_.vel_estimate_.any().value_or(0.0f), 32, 32, true); - return odCAN->write(txmsg); + return canbus_->send_message(txmsg); } -int32_t CANSimple::get_encoder_count_callback(const Axis& axis) { +bool CANSimple::get_encoder_count_callback(const Axis& axis) { can_Message_t txmsg; txmsg.id = axis.config_.can.node_id << NUM_CMD_ID_BITS; txmsg.id += MSG_GET_ENCODER_COUNT; @@ -203,7 +238,7 @@ int32_t CANSimple::get_encoder_count_callback(const Axis& axis) { can_setSignal(txmsg, axis.encoder_.shadow_count_, 0, 32, true); can_setSignal(txmsg, axis.encoder_.count_in_cpr_, 32, 32, true); - return odCAN->write(txmsg); + return canbus_->send_message(txmsg); } void CANSimple::set_input_pos_callback(Axis& axis, const can_Message_t& msg) { @@ -252,7 +287,7 @@ void CANSimple::set_linear_count_callback(Axis& axis, const can_Message_t& msg){ axis.encoder_.set_linear_count(can_getSignal(msg, 0, 32, true)); } -int32_t CANSimple::get_iq_callback(const Axis& axis) { +bool CANSimple::get_iq_callback(const Axis& axis) { can_Message_t txmsg; txmsg.id = axis.config_.can.node_id << NUM_CMD_ID_BITS; txmsg.id += MSG_GET_IQ; @@ -269,10 +304,10 @@ int32_t CANSimple::get_iq_callback(const Axis& axis) { can_setSignal(txmsg, Idq_setpoint->first, 0, 32, true); can_setSignal(txmsg, Idq_setpoint->second, 32, 32, true); - return odCAN->write(txmsg); + return canbus_->send_message(txmsg); } -int32_t CANSimple::get_vbus_voltage_callback(const Axis& axis) { +bool CANSimple::get_vbus_voltage_callback(const Axis& axis) { can_Message_t txmsg; txmsg.id = axis.config_.can.node_id << NUM_CMD_ID_BITS; @@ -284,7 +319,7 @@ int32_t CANSimple::get_vbus_voltage_callback(const Axis& axis) { static_assert(sizeof(vbus_voltage) == sizeof(floatBytes)); can_setSignal(txmsg, vbus_voltage, 0, 32, true); - return odCAN->write(txmsg); + return canbus_->send_message(txmsg); } void CANSimple::clear_errors_callback(Axis& axis, const can_Message_t& msg) { @@ -295,11 +330,20 @@ uint32_t CANSimple::service_stack() { uint32_t nextServiceTime = UINT32_MAX; uint32_t now = HAL_GetTick(); + // TODO: remove this polling loop and replace with protocol hook + for (size_t i = 0; i < AXIS_COUNT; ++i) { + bool node_id_changed = (axes[i].config_.can.node_id != node_ids_[i]) + || (axes[i].config_.can.is_extended != extended_node_ids_[i]); + if (node_id_changed) { + renew_subscription(i); + } + } + for (auto& a: axes) { MEASURE_TIME(a.task_times_.can_heartbeat) { if (a.config_.can.heartbeat_rate_ms > 0) { if ((now - a.can_.last_heartbeat) >= a.config_.can.heartbeat_rate_ms) { - if (send_heartbeat(a) >= 0) + if (send_heartbeat(a)) a.can_.last_heartbeat = now; } @@ -309,7 +353,7 @@ uint32_t CANSimple::service_stack() { if (a.config_.can.encoder_rate_ms > 0) { if ((now - a.can_.last_encoder) >= a.config_.can.encoder_rate_ms) { - if (get_encoder_estimates_callback(a) >= 0) + if (get_encoder_estimates_callback(a)) a.can_.last_encoder = now; } @@ -322,7 +366,7 @@ uint32_t CANSimple::service_stack() { return nextServiceTime; } -int32_t CANSimple::send_heartbeat(const Axis& axis) { +bool CANSimple::send_heartbeat(const Axis& axis) { can_Message_t txmsg; txmsg.id = axis.config_.can.node_id << NUM_CMD_ID_BITS; txmsg.id += MSG_ODRIVE_HEARTBEAT; // heartbeat ID @@ -332,23 +376,5 @@ int32_t CANSimple::send_heartbeat(const Axis& axis) { can_setSignal(txmsg, axis.error_, 0, 32, true); can_setSignal(txmsg, axis.current_state_, 32, 32, true); - return odCAN->write(txmsg); -} - -void CANSimple::send_cyclic(Axis& axis) { - const uint32_t now = HAL_GetTick(); - - if (axis.config_.can.heartbeat_rate_ms > 0) { - if ((now - axis.can_.last_heartbeat) >= axis.config_.can.heartbeat_rate_ms) { - if(send_heartbeat(axis) >= 0) - axis.can_.last_heartbeat = now; - } - } - - if (axis.config_.can.encoder_rate_ms > 0) { - if ((now - axis.can_.last_encoder) >= axis.config_.can.encoder_rate_ms) { - if(get_encoder_estimates_callback(axis) >= 0) - axis.can_.last_encoder = now; - } - } + return canbus_->send_message(txmsg); } diff --git a/Firmware/communication/can/can_simple.hpp b/Firmware/communication/can/can_simple.hpp index 56025168..f2c2ebdf 100644 --- a/Firmware/communication/can/can_simple.hpp +++ b/Firmware/communication/can/can_simple.hpp @@ -1,9 +1,10 @@ #ifndef __CAN_SIMPLE_HPP_ #define __CAN_SIMPLE_HPP_ -#include "odrive_can.hpp" +#include "canbus.hpp" +#include "axis.hpp" -class CANSimple : public ODriveCAN { +class CANSimple { public: enum { MSG_CO_NMT_CTRL = 0x000, // CANOpen NMT Message REC @@ -34,29 +35,30 @@ class CANSimple : public ODriveCAN { MSG_CO_HEARTBEAT_CMD = 0x700, // CANOpen NMT Heartbeat SEND }; - using ODriveCAN::ODriveCAN; + CANSimple(CanBusBase* canbus) : canbus_(canbus) {} - - // Cyclic Senders - static int32_t send_heartbeat(const Axis& axis); - static void send_cyclic(Axis& axis); + bool init(); + uint32_t service_stack(); private: - uint32_t service_stack() final; - void handle_can_message(const can_Message_t& msg) final; - static void doCommand(Axis& axis, const can_Message_t& cmd); + bool renew_subscription(size_t i); + bool send_heartbeat(const Axis& axis); + + void handle_can_message(const can_Message_t& msg); + + void do_command(Axis& axis, const can_Message_t& cmd); // Get functions (msg.rtr bit must be set) - static int32_t get_motor_error_callback(const Axis& axis); - static int32_t get_encoder_error_callback(const Axis& axis); - static int32_t get_controller_error_callback(const Axis& axis); - static int32_t get_sensorless_error_callback(const Axis& axis); - static int32_t get_encoder_estimates_callback(const Axis& axis); - static int32_t get_encoder_count_callback(const Axis& axis); - static int32_t get_iq_callback(const Axis& axis); - static int32_t get_sensorless_estimates_callback(const Axis& axis); - static int32_t get_vbus_voltage_callback(const Axis& axis); + bool get_motor_error_callback(const Axis& axis); + bool get_encoder_error_callback(const Axis& axis); + bool get_controller_error_callback(const Axis& axis); + bool get_sensorless_error_callback(const Axis& axis); + bool get_encoder_estimates_callback(const Axis& axis); + bool get_encoder_count_callback(const Axis& axis); + bool get_iq_callback(const Axis& axis); + bool get_sensorless_estimates_callback(const Axis& axis); + bool get_vbus_voltage_callback(const Axis& axis); // Set functions static void set_axis_nodeid_callback(Axis& axis, const can_Message_t& msg); @@ -89,6 +91,14 @@ class CANSimple : public ODriveCAN { static constexpr uint8_t get_cmd_id(uint32_t msgID) { return (msgID & 0x01F); // Bottom 5 bits } + + CanBusBase* canbus_; + CanBusBase::CanSubscription* subscription_handles_[AXIS_COUNT]; + + // TODO: we this is a hack but actually we should use protocol hooks to + // renew our filter when the node ID changes + uint32_t node_ids_[AXIS_COUNT]; + bool extended_node_ids_[AXIS_COUNT]; }; #endif \ No newline at end of file diff --git a/Firmware/communication/can/odrive_can.cpp b/Firmware/communication/can/odrive_can.cpp index 53df5e7d..44fbd714 100644 --- a/Firmware/communication/can/odrive_can.cpp +++ b/Firmware/communication/can/odrive_can.cpp @@ -11,164 +11,195 @@ // #include // std::unordered_map ctxMap; -// Constructor is called by communication.cpp and the handle is assigned appropriately -ODriveCAN::ODriveCAN(ODriveCAN::Config_t &config, CAN_HandleTypeDef *handle) - : config_{config}, - handle_{handle} { - // ctxMap[handle_] = this; + +bool ODriveCAN::apply_config() { + config_.parent = this; + set_baud_rate(config_.baud_rate); + return true; +} + +bool ODriveCAN::reinit() { + HAL_CAN_Stop(handle_); + HAL_CAN_ResetError(handle_); + return (HAL_CAN_Init(handle_) == HAL_OK) + && (HAL_CAN_Start(handle_) == HAL_OK) + && (HAL_CAN_ActivateNotification(handle_, CAN_IT_RX_FIFO0_MSG_PENDING | CAN_IT_RX_FIFO1_MSG_PENDING | CAN_IT_TX_MAILBOX_EMPTY) == HAL_OK); +} + +bool ODriveCAN::start_server(CAN_HandleTypeDef* handle) { + handle_ = handle; + + if (!reinit()) { + return false; + } + + auto wrapper = [](void* ctx) { + ((ODriveCAN*)ctx)->can_server_thread(); + }; + osThreadDef(can_server_thread_def, wrapper, osPriorityNormal, 0, stack_size_ / sizeof(StackType_t)); + thread_id_ = osThreadCreate(osThread(can_server_thread_def), this); + + return true; } void ODriveCAN::can_server_thread() { + Protocol protocol = config_.protocol; + + if (protocol & PROTOCOL_SIMPLE) { + can_simple_.init(); + } + for (;;) { uint32_t status = HAL_CAN_GetError(handle_); if (status == HAL_CAN_ERROR_NONE) { - uint32_t nextServiceTime = service_stack(); + uint32_t next_service_time = UINT32_MAX; - uint32_t rxNum = available(); - for (uint32_t i = 0; i < rxNum; i++) { - can_Message_t rxmsg; - read(rxmsg); - handle_can_message(rxmsg); - } - HAL_CAN_ActivateNotification(handle_, CAN_IT_RX_FIFO0_MSG_PENDING | CAN_IT_TX_MAILBOX_EMPTY); - osSemaphoreWait(sem_can, nextServiceTime); - } else { - if (status == HAL_CAN_ERROR_TIMEOUT) { - HAL_CAN_ResetError(handle_); - status = HAL_CAN_Start(handle_); - if (status == HAL_OK) - status = HAL_CAN_ActivateNotification(handle_, CAN_IT_RX_FIFO0_MSG_PENDING | CAN_IT_TX_MAILBOX_EMPTY); + if (protocol & PROTOCOL_SIMPLE) { + next_service_time = std::min(can_simple_.service_stack(), next_service_time); } + + process_rx_fifo(CAN_RX_FIFO0); + process_rx_fifo(CAN_RX_FIFO1); + HAL_CAN_ActivateNotification(handle_, CAN_IT_RX_FIFO0_MSG_PENDING | CAN_IT_RX_FIFO1_MSG_PENDING | CAN_IT_TX_MAILBOX_EMPTY); + osSemaphoreWait(sem_can, next_service_time); + } else if (status == HAL_CAN_ERROR_TIMEOUT) { + HAL_CAN_ResetError(handle_); + status = HAL_CAN_Start(handle_); + if (status == HAL_OK) + status = HAL_CAN_ActivateNotification(handle_, CAN_IT_RX_FIFO0_MSG_PENDING | CAN_IT_TX_MAILBOX_EMPTY); } } } -static void can_server_thread_wrapper(void *ctx) { - reinterpret_cast(ctx)->can_server_thread(); - reinterpret_cast(ctx)->thread_id_valid_ = false; -} - -bool ODriveCAN::start_can_server() { - HAL_StatusTypeDef status; - - set_baud_rate(config_.baud_rate); - - status = HAL_CAN_Init(handle_); - - CAN_FilterTypeDef filter; - filter.FilterActivation = ENABLE; - filter.FilterBank = 0; - filter.FilterFIFOAssignment = CAN_RX_FIFO0; - filter.FilterIdHigh = 0x0000; - filter.FilterIdLow = 0x0000; - filter.FilterMaskIdHigh = 0x0000; - filter.FilterMaskIdLow = 0x0000; - filter.FilterMode = CAN_FILTERMODE_IDMASK; - filter.FilterScale = CAN_FILTERSCALE_32BIT; - - status = HAL_CAN_ConfigFilter(handle_, &filter); - - status = HAL_CAN_Start(handle_); - if (status == HAL_OK) - status = HAL_CAN_ActivateNotification(handle_, CAN_IT_RX_FIFO0_MSG_PENDING | CAN_IT_TX_MAILBOX_EMPTY); - - osThreadDef(can_server_thread_def, can_server_thread_wrapper, osPriorityNormal, 0, stack_size_ / sizeof(StackType_t)); - thread_id_ = osThreadCreate(osThread(can_server_thread_def), this); - thread_id_valid_ = true; - - return status; -} - -// Send a CAN message on the bus -int32_t ODriveCAN::write(can_Message_t &txmsg) { - if (HAL_CAN_GetError(handle_) == HAL_CAN_ERROR_NONE) { - CAN_TxHeaderTypeDef header; - header.StdId = txmsg.id; - header.ExtId = txmsg.id; - header.IDE = txmsg.isExt ? CAN_ID_EXT : CAN_ID_STD; - header.RTR = CAN_RTR_DATA; - header.DLC = txmsg.len; - header.TransmitGlobalTime = FunctionalState::DISABLE; - - uint32_t retTxMailbox = 0; - if (HAL_CAN_GetTxMailboxesFreeLevel(handle_) > 0) - HAL_CAN_AddTxMessage(handle_, &header, txmsg.buf, &retTxMailbox); - else - return -1; - return (int32_t)retTxMailbox; - } else { - return -1; - } -} - -uint32_t ODriveCAN::available() { - return (HAL_CAN_GetRxFifoFillLevel(handle_, CAN_RX_FIFO0) + HAL_CAN_GetRxFifoFillLevel(handle_, CAN_RX_FIFO1)); -} - -bool ODriveCAN::read(can_Message_t &rxmsg) { - CAN_RxHeaderTypeDef header; - bool validRead = false; - if (HAL_CAN_GetRxFifoFillLevel(handle_, CAN_RX_FIFO0) > 0) { - HAL_CAN_GetRxMessage(handle_, CAN_RX_FIFO0, &header, rxmsg.buf); - validRead = true; - } else if (HAL_CAN_GetRxFifoFillLevel(handle_, CAN_RX_FIFO1) > 0) { - HAL_CAN_GetRxMessage(handle_, CAN_RX_FIFO1, &header, rxmsg.buf); - validRead = true; - } - - rxmsg.isExt = header.IDE; - rxmsg.id = rxmsg.isExt ? header.ExtId : header.StdId; // If it's an extended message, pass the extended ID - rxmsg.len = header.DLC; - rxmsg.rtr = header.RTR; - - return validRead; -} - // Set one of only a few common baud rates. CAN doesn't do arbitrary baud rates well due to the time-quanta issue. // 21 TQ allows for easy sampling at exactly 80% (recommended by Vector Informatik GmbH for high reliability systems) // Conveniently, the CAN peripheral's 42MHz clock lets us easily create 21TQs for all common baud rates -void ODriveCAN::set_baud_rate(uint32_t baudRate) { - switch (baudRate) { - case CAN_BAUD_125K: - handle_->Init.Prescaler = CAN_FREQ / 125000UL; - config_.baud_rate = baudRate; - reinit_can(); - break; - - case CAN_BAUD_250K: - handle_->Init.Prescaler = CAN_FREQ / 250000UL; - config_.baud_rate = baudRate; - reinit_can(); - break; - - case CAN_BAUD_500K: - handle_->Init.Prescaler = CAN_FREQ / 500000UL; - config_.baud_rate = baudRate; - reinit_can(); - break; - - case CAN_BAUD_1000K: - handle_->Init.Prescaler = CAN_FREQ / 1000000UL; - config_.baud_rate = baudRate; - reinit_can(); - break; - - default: - // baudRate is invalid, so don't accept it. - break; +bool ODriveCAN::set_baud_rate(uint32_t baud_rate) { + uint32_t prescaler = CAN_FREQ / baud_rate; + if (prescaler * baud_rate == CAN_FREQ) { + // valid baud rate + config_.baud_rate = baud_rate; + if (handle_) { + handle_->Init.Prescaler = prescaler; + return reinit(); + } + return true; + } else { + // invalid baud rate - ignore + return false; } } -void ODriveCAN::reinit_can() { - HAL_CAN_Stop(handle_); - HAL_CAN_Init(handle_); - auto status = HAL_CAN_Start(handle_); - if (status == HAL_OK) - status = HAL_CAN_ActivateNotification(handle_, CAN_IT_RX_FIFO0_MSG_PENDING | CAN_IT_TX_MAILBOX_EMPTY); +void ODriveCAN::process_rx_fifo(uint32_t fifo) { + while (HAL_CAN_GetRxFifoFillLevel(handle_, fifo)) { + CAN_RxHeaderTypeDef header; + can_Message_t rxmsg; + HAL_CAN_GetRxMessage(handle_, fifo, &header, rxmsg.buf); + + rxmsg.isExt = header.IDE; + rxmsg.id = rxmsg.isExt ? header.ExtId : header.StdId; // If it's an extended message, pass the extended ID + rxmsg.len = header.DLC; + rxmsg.rtr = header.RTR; + + // TODO: this could be optimized with an ahead-of-time computed + // index-to-filter map + + size_t fifo0_idx = 0; + size_t fifo1_idx = 0; + + // Find the triggered subscription item based on header.FilterMatchIndex + auto it = std::find_if(subscriptions_.begin(), subscriptions_.end(), [&](auto& s) { + size_t current_idx = (s.fifo == 0 ? fifo0_idx : fifo1_idx)++; + return (header.FilterMatchIndex == current_idx) && (s.fifo == fifo); + }); + + if (it == subscriptions_.end()) { + continue; + } + + it->callback(it->ctx, rxmsg); + } } -void ODriveCAN::set_error(Error error) { - error_ |= error; +// Send a CAN message on the bus +bool ODriveCAN::send_message(const can_Message_t &txmsg) { + if (HAL_CAN_GetError(handle_) != HAL_CAN_ERROR_NONE) { + return false; + } + + CAN_TxHeaderTypeDef header; + header.StdId = txmsg.id; + header.ExtId = txmsg.id; + header.IDE = txmsg.isExt ? CAN_ID_EXT : CAN_ID_STD; + header.RTR = CAN_RTR_DATA; + header.DLC = txmsg.len; + header.TransmitGlobalTime = FunctionalState::DISABLE; + + uint32_t retTxMailbox = 0; + if (!HAL_CAN_GetTxMailboxesFreeLevel(handle_)) { + return false; + } + + return HAL_CAN_AddTxMessage(handle_, &header, (uint8_t*)txmsg.buf, &retTxMailbox) == HAL_OK; +} + +//void ODriveCAN::set_error(Error error) { +// error_ |= error; +//} + +bool ODriveCAN::subscribe(const MsgIdFilterSpecs& filter, on_can_message_cb_t callback, void* ctx, CanSubscription** handle) { + auto it = std::find_if(subscriptions_.begin(), subscriptions_.end(), [](auto& subscription) { + return subscription.fifo == kCanFifoNone; + }); + + if (it == subscriptions_.end()) { + return false; // all subscription slots in use + } + + it->callback = callback; + it->ctx = ctx; + it->fifo = CAN_RX_FIFO0; // TODO: make customizable + if (handle) { + *handle = &*it; + } + + bool is_extended = filter.id.index() == 1; + uint32_t id = is_extended ? + ((std::get<1>(filter.id) << 3) | (1 << 2)) : + (std::get<0>(filter.id) << 21); + uint32_t mask = (is_extended ? (filter.mask << 3) : (filter.mask << 21)) + | (1 << 2); // care about the is_extended bit + + CAN_FilterTypeDef hal_filter; + hal_filter.FilterActivation = ENABLE; + hal_filter.FilterBank = &*it - &subscriptions_[0]; + hal_filter.FilterFIFOAssignment = it->fifo; + hal_filter.FilterIdHigh = (id >> 16) & 0xffff; + hal_filter.FilterIdLow = id & 0xffff; + hal_filter.FilterMaskIdHigh = (mask >> 16) & 0xffff; + hal_filter.FilterMaskIdLow = mask & 0xffff; + hal_filter.FilterMode = CAN_FILTERMODE_IDMASK; + hal_filter.FilterScale = CAN_FILTERSCALE_32BIT; + + if (HAL_CAN_ConfigFilter(handle_, &hal_filter) != HAL_OK) { + return false; + } + return true; +} + +bool ODriveCAN::unsubscribe(CanSubscription* handle) { + ODriveCanSubscription* subscription = static_cast(handle); + if (subscription < subscriptions_.begin() || subscription >= subscriptions_.end()) { + return false; + } + if (subscription->fifo != kCanFifoNone) { + return false; // not in use + } + + subscription->fifo = kCanFifoNone; + + CAN_FilterTypeDef hal_filter = {.FilterActivation = DISABLE}; + return HAL_CAN_ConfigFilter(handle_, &hal_filter) == HAL_OK; } void HAL_CAN_TxMailbox0CompleteCallback(CAN_HandleTypeDef *hcan) { @@ -191,12 +222,14 @@ void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan) { osSemaphoreRelease(sem_can); } void HAL_CAN_RxFifo0FullCallback(CAN_HandleTypeDef *hcan) { - // osSemaphoreRelease(sem_can); + HAL_CAN_DeactivateNotification(hcan, CAN_IT_RX_FIFO1_MSG_PENDING); + osSemaphoreRelease(sem_can); } void HAL_CAN_RxFifo1MsgPendingCallback(CAN_HandleTypeDef *hcan) {} void HAL_CAN_RxFifo1FullCallback(CAN_HandleTypeDef *hcan) {} void HAL_CAN_SleepCallback(CAN_HandleTypeDef *hcan) {} void HAL_CAN_WakeUpFromRxMsgCallback(CAN_HandleTypeDef *hcan) {} + void HAL_CAN_ErrorCallback(CAN_HandleTypeDef *hcan) { - HAL_CAN_ResetError(hcan); + //HAL_CAN_ResetError(hcan); } diff --git a/Firmware/communication/can/odrive_can.hpp b/Firmware/communication/can/odrive_can.hpp index d483c671..23808c05 100644 --- a/Firmware/communication/can/odrive_can.hpp +++ b/Firmware/communication/can/odrive_can.hpp @@ -3,9 +3,10 @@ #include -#include "can_helpers.hpp" +#include "canbus.hpp" #include "fibre/protocol.hpp" -#include "odrive_main.h" +#include "can_simple.hpp" +#include #define CAN_CLK_HZ (42000000) #define CAN_CLK_MHZ (42) @@ -19,41 +20,50 @@ enum { CAN_BAUD_1M = 1000000 }; -class ODriveCAN : public ODriveIntf::CanIntf { - public: +class ODriveCAN : public CanBusBase, public ODriveIntf::CanIntf { +public: struct Config_t { uint32_t baud_rate = CAN_BAUD_250K; Protocol protocol = PROTOCOL_SIMPLE; + + ODriveCAN* parent = nullptr; // set in apply_config() + void set_baud_rate(uint32_t value) { parent->set_baud_rate(baud_rate); } }; - ODriveCAN(ODriveCAN::Config_t &config, CAN_HandleTypeDef *handle); + ODriveCAN() {} + + bool apply_config(); + bool start_server(CAN_HandleTypeDef* handle); - // Thread Relevant Data - osThreadId thread_id_; - const uint32_t stack_size_ = 1024; // Bytes Error error_ = ERROR_NONE; - volatile bool thread_id_valid_ = false; - bool start_can_server(); + Config_t config_; + CANSimple can_simple_{this}; + + osThreadId thread_id_; + const uint32_t stack_size_ = 1024; // Bytes + +private: + static const uint8_t kCanFifoNone = 0xff; + + struct ODriveCanSubscription : CanSubscription { + uint8_t fifo = kCanFifoNone; + on_can_message_cb_t callback; + void* ctx; + }; + + bool reinit(); void can_server_thread(); - void reinit_can(); - void set_error(Error error); + bool set_baud_rate(uint32_t baud_rate); + void process_rx_fifo(uint32_t fifo); + bool send_message(const can_Message_t& message) final; + bool subscribe(const MsgIdFilterSpecs& filter, on_can_message_cb_t callback, void* ctx, CanSubscription** handle) final; + bool unsubscribe(CanSubscription* handle) final; - // I/O Functions - uint32_t available(); - int32_t write(can_Message_t &txmsg); - bool read(can_Message_t &rxmsg); - - ODriveCAN::Config_t &config_; - - protected: - virtual uint32_t service_stack() = 0; - virtual void handle_can_message(const can_Message_t &msg) = 0; - - private: + // Hardware supports at most 28 filters unless we do optimizations. For now + // we don't need that many. + std::array subscriptions_; CAN_HandleTypeDef *handle_ = nullptr; - - void set_baud_rate(uint32_t baudRate); }; #endif // __ODRIVE_CAN_HPP diff --git a/Firmware/communication/communication.cpp b/Firmware/communication/communication.cpp index 9d5671ed..ba16f3f3 100644 --- a/Firmware/communication/communication.cpp +++ b/Firmware/communication/communication.cpp @@ -55,8 +55,8 @@ void init_communication(void) { start_i2c_server(); } - if (odCAN && odrv.config_.enable_can_a) { - odCAN->start_can_server(); + if (odrv.config_.enable_can_a) { + odrv.can_.start_server(&hcan1); } } diff --git a/Firmware/odrive-interface.yaml b/Firmware/odrive-interface.yaml index 1af868a4..9614bd2f 100644 --- a/Firmware/odrive-interface.yaml +++ b/Firmware/odrive-interface.yaml @@ -179,7 +179,7 @@ interfaces: Example: `step_gpio_pin` of both axes were set to the same GPIO. oscilloscope: {type: Oscilloscope} - can: {type: Can, c_name: get_can()} + can: {type: Can} test_property: uint32 functions: @@ -370,10 +370,8 @@ interfaces: config: c_is_class: False attributes: - baud_rate: readonly uint32 + baud_rate: {type: uint32, c_setter: 'set_baud_rate'} protocol: Protocol - functions: - set_baud_rate: {in: {baudRate: uint32}} ODrive.Endpoint: c_is_class: False @@ -1163,7 +1161,8 @@ valuetypes: Status: {doc: The pin is used for status output (see `config.error_gpio_pin`)} ODrive.Can.Protocol: - values: {Simple: } + flags: + Simple: ODrive.Axis.AxisState: # TODO: remove redundant "Axis" in name values: diff --git a/tools/odrive/enums.py b/tools/odrive/enums.py index 79d92623..669edd09 100644 --- a/tools/odrive/enums.py +++ b/tools/odrive/enums.py @@ -22,7 +22,7 @@ GPIO_MODE_MECH_BRAKE = 14 GPIO_MODE_STATUS = 15 # ODrive.Can.Protocol -PROTOCOL_SIMPLE = 0 +PROTOCOL_SIMPLE = 0x00000001 # ODrive.Axis.AxisState AXIS_STATE_UNDEFINED = 0 diff --git a/tools/odrive/tests/can_test.py b/tools/odrive/tests/can_test.py index be1ce798..6e76a95c 100644 --- a/tools/odrive/tests/can_test.py +++ b/tools/odrive/tests/can_test.py @@ -136,6 +136,7 @@ class TestSimpleCAN(): test_assert_eq(my_req('get_vbus_voltage')['vbus_voltage'], odrive.handle.vbus_voltage, accuracy=0.01) my_cmd('set_node_id', node_id=node_id+20) + time.sleep(0.1) # TODO: remove this hack (see note in firmware) asyncio.run(request(canbus.handle, node_id+20, extended_id, 'get_vbus_voltage')) test_assert_eq(axis.config.can.node_id, node_id+20) From 0fb0a9b3c6375b9327cf3f111d2c7c5102eeb8f2 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Thu, 3 Dec 2020 17:02:56 +0100 Subject: [PATCH 3/5] add canbus.hpp --- Firmware/communication/can/canbus.hpp | 43 +++++++++++++++++++++++++++ 1 file changed, 43 insertions(+) create mode 100644 Firmware/communication/can/canbus.hpp diff --git a/Firmware/communication/can/canbus.hpp b/Firmware/communication/can/canbus.hpp new file mode 100644 index 00000000..fbc3df1e --- /dev/null +++ b/Firmware/communication/can/canbus.hpp @@ -0,0 +1,43 @@ +#ifndef __CANBUS_HPP +#define __CANBUS_HPP + +#include "can_helpers.hpp" +#include + +struct MsgIdFilterSpecs { + std::variant id; + uint32_t mask; +}; + +class CanBusBase { +public: + typedef void(*on_can_message_cb_t)(void* ctx, const can_Message_t& message); + struct CanSubscription {}; + + /** + * @brief Sends the specified CAN message. + * + * @returns: true on success or false otherwise (e.g. if the send queue is + * full). + */ + virtual bool send_message(const can_Message_t& message) = 0; + + /** + * @brief Registers a callback that will be invoked for every incoming CAN + * message that matches the filter. + * + * @param handle: On success this handle is set to an opaque pointer that + * can be used to cancel the subscription. + * + * @returns: true on success or false otherwise (e.g. if the maximum number + * of subscriptions has been reached). + */ + virtual bool subscribe(const MsgIdFilterSpecs& filter, on_can_message_cb_t callback, void* ctx, CanSubscription** handle) = 0; + + /** + * @brief Deregisters a callback that was previously registered with subscribe(). + */ + virtual bool unsubscribe(CanSubscription* handle) = 0; +}; + +#endif // __CANBUS_HPP \ No newline at end of file From 187b83de7f38a7985021155896b28cd324c29a6b Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Thu, 3 Dec 2020 19:01:45 +0100 Subject: [PATCH 4/5] fix volatility of can config --- Firmware/MotorControl/main.cpp | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp index ee7fc64c..eb308774 100644 --- a/Firmware/MotorControl/main.cpp +++ b/Firmware/MotorControl/main.cpp @@ -27,8 +27,6 @@ uint32_t _reboot_cookie __attribute__ ((section (".noinit"))); extern char _estack; // provided by the linker script -ODriveCAN::Config_t can_config; -CANSimple *can_simple = nullptr; ODrive odrv{}; @@ -89,7 +87,7 @@ void StatusLedController::update() { static bool config_read_all() { bool success = board_read_config() && config_manager.read(&odrv.config_) && - config_manager.read(&can_config); + config_manager.read(&odrv.can_.config_); for (size_t i = 0; (i < AXIS_COUNT) && success; ++i) { success = config_manager.read(&encoders[i].config_) && config_manager.read(&axes[i].sensorless_estimator_.config_) && @@ -109,7 +107,7 @@ static bool config_read_all() { static bool config_write_all() { bool success = board_write_config() && config_manager.write(&odrv.config_) && - config_manager.write(&can_config); + config_manager.write(&odrv.can_.config_); for (size_t i = 0; (i < AXIS_COUNT) && success; ++i) { success = config_manager.write(&encoders[i].config_) && config_manager.write(&axes[i].sensorless_estimator_.config_) && @@ -128,7 +126,7 @@ static bool config_write_all() { static void config_clear_all() { odrv.config_ = {}; - can_config = {}; + odrv.can_.config_ = {}; for (size_t i = 0; i < AXIS_COUNT; ++i) { encoders[i].config_ = {}; axes[i].sensorless_estimator_.config_ = {}; From 52adf3392363b6a8690d2b02a0cd55032656e65d Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Mon, 7 Dec 2020 20:34:04 +0100 Subject: [PATCH 5/5] fix CAN initialization --- Firmware/communication/can/odrive_can.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/Firmware/communication/can/odrive_can.cpp b/Firmware/communication/can/odrive_can.cpp index 44fbd714..25c1440b 100644 --- a/Firmware/communication/can/odrive_can.cpp +++ b/Firmware/communication/can/odrive_can.cpp @@ -29,6 +29,7 @@ bool ODriveCAN::reinit() { bool ODriveCAN::start_server(CAN_HandleTypeDef* handle) { handle_ = handle; + handle_->Init.Prescaler = CAN_FREQ / config_.baud_rate; if (!reinit()) { return false; }