From 4d99c842bbc9caff1802c33a659e4c6797a89025 Mon Sep 17 00:00:00 2001 From: George Papamichail <152964062+gi0rg0sPapamichail@users.noreply.github.com> Date: Mon, 9 Nov 2026 14:43:27 +0200 Subject: [PATCH 1/4] Update SineWaveCAN.ino --- examples/SineWaveCAN/SineWaveCAN.ino | 411 ++++++++++++++++----------- 1 file changed, 240 insertions(+), 171 deletions(-) diff --git a/examples/SineWaveCAN/SineWaveCAN.ino b/examples/SineWaveCAN/SineWaveCAN.ino index 8f7607b..a1c0073 100644 --- a/examples/SineWaveCAN/SineWaveCAN.ino +++ b/examples/SineWaveCAN/SineWaveCAN.ino @@ -1,11 +1,9 @@ - #include #include "ODriveCAN.h" // Documentation for this example can be found here: // https://docs.odriverobotics.com/v/latest/guides/arduino-can-guide.html - /* Configuration of example sketch -------------------------------------------*/ // CAN bus baudrate. Make sure this matches for every device on the bus @@ -22,11 +20,11 @@ // #define IS_MCP2515 // Any board with external MCP2515 based extension module. See below to configure the module. // #define IS_STM32_BUILTIN // STM32 boards with built-in CAN interface (e.g. STM32F4 Discovery). // #define IS_ESP32_TWAI // ESP32 boards with built-in TWAI (CAN) interface. Directly uses the ESP-IDF TWAI driver. - +// #define IS_TCAN4X5X // RP2350 / Arduino boards with external TCAN4551 SPI controller. /* Board-specific includes ---------------------------------------------------*/ -#if defined(IS_TEENSY_BUILTIN) + defined(IS_ARDUINO_BUILTIN) + defined(IS_MCP2515) + defined(IS_STM32_BUILTIN) + defined(IS_ESP32_TWAI) != 1 +#if defined(IS_TEENSY_BUILTIN) + defined(IS_ARDUINO_BUILTIN) + defined(IS_MCP2515) + defined(IS_STM32_BUILTIN) + defined(IS_ESP32_TWAI) + defined(IS_TCAN4X5X) != 1 #warning "Select exactly one hardware option at the top of this file." #if CAN_HOWMANY > 0 || CANFD_HOWMANY > 0 @@ -72,11 +70,14 @@ struct ODriveStatus; // hack to prevent teensy compile error #include "ODriveESP32TWAI.hpp" #endif // IS_ESP32_TWAI - +#ifdef IS_TCAN4X5X +#include +#include +#include "ODriveTCAN4x5x.hpp" +#endif // IS_TCAN4X5X /* Board-specific settings ---------------------------------------------------*/ - /* Teensy */ #ifdef IS_TEENSY_BUILTIN @@ -84,18 +85,17 @@ struct ODriveStatus; // hack to prevent teensy compile error FlexCAN_T4 can_intf; bool setupCan() { - can_intf.begin(); - can_intf.setBaudRate(CAN_BAUDRATE); - can_intf.setMaxMB(16); - can_intf.enableFIFO(); - can_intf.enableFIFOInterrupt(); - can_intf.onReceive(onCanMessage); - return true; +can_intf.begin(); +can_intf.setBaudRate(CAN_BAUDRATE); +can_intf.setMaxMB(16); +can_intf.enableFIFO(); +can_intf.enableFIFOInterrupt(); +can_intf.onReceive(onCanMessage); +return true; } #endif // IS_TEENSY_BUILTIN - /* MCP2515-based extension modules -*/ #ifdef IS_MCP2515 @@ -109,35 +109,33 @@ MCP2515Class& can_intf = CAN; // NOTE: not all Arduino pins are interruptable, check the documentation for your board! #define MCP2515_INT 2 -// freqeuncy of the crystal oscillator on the MCP2515 breakout board. +// freqeuncy of the crystal oscillator on the MCP2515 breakout board. // common values are: 16 MHz, 12 MHz, 8 MHz #define MCP2515_CLK_HZ 8000000 - static inline void receiveCallback(int packet_size) { - if (packet_size > 8) { - return; // not supported - } - CanMsg msg = {.id = (unsigned int)CAN.packetId(), .len = (uint8_t)packet_size}; - CAN.readBytes(msg.buffer, packet_size); - onCanMessage(msg); +if (packet_size > 8) { +return; // not supported +} +CanMsg msg = {.id = (unsigned int)CAN.packetId(), .len = (uint8_t)packet_size}; +CAN.readBytes(msg.buffer, packet_size); +onCanMessage(msg); } bool setupCan() { - // configure and initialize the CAN bus interface - CAN.setPins(MCP2515_CS, MCP2515_INT); - CAN.setClockFrequency(MCP2515_CLK_HZ); - if (!CAN.begin(CAN_BAUDRATE)) { - return false; - } +// configure and initialize the CAN bus interface +CAN.setPins(MCP2515_CS, MCP2515_INT); +CAN.setClockFrequency(MCP2515_CLK_HZ); +if (!CAN.begin(CAN_BAUDRATE)) { +return false; +} - CAN.onReceive(receiveCallback); - return true; +CAN.onReceive(receiveCallback); +return true; } #endif // IS_MCP2515 - /* Arduinos with built-in CAN */ #ifdef IS_ARDUINO_BUILTIN @@ -145,12 +143,11 @@ bool setupCan() { HardwareCAN& can_intf = CAN; bool setupCan() { - return can_intf.begin((CanBitRate)CAN_BAUDRATE); +return can_intf.begin((CanBitRate)CAN_BAUDRATE); } #endif - /* STM32 boards with built-in CAN */ #ifdef IS_STM32_BUILTIN @@ -159,14 +156,13 @@ STM32_CAN Can1( CAN1 ); STM32_CAN& can_intf = Can1; bool setupCan() { - can_intf.begin(); - can_intf.setBaudRate(CAN_BAUDRATE); - return true; +can_intf.begin(); +can_intf.setBaudRate(CAN_BAUDRATE); +return true; } #endif // IS_STM32_BUILTIN - /* ESP32 boards with built-in TWAI (CAN) */ #ifdef IS_ESP32_TWAI @@ -178,41 +174,113 @@ bool setupCan() { ESP32TWAIIntf can_intf; bool setupCan() { - twai_general_config_t g_config = TWAI_GENERAL_CONFIG_DEFAULT( - (gpio_num_t)ESP32_TWAI_TX_PIN, - (gpio_num_t)ESP32_TWAI_RX_PIN, - TWAI_MODE_NORMAL - ); - - twai_timing_config_t t_config; - switch (CAN_BAUDRATE) { - case 1000000: t_config = TWAI_TIMING_CONFIG_1MBITS(); break; - case 800000: t_config = TWAI_TIMING_CONFIG_800KBITS(); break; - case 500000: t_config = TWAI_TIMING_CONFIG_500KBITS(); break; - case 250000: t_config = TWAI_TIMING_CONFIG_250KBITS(); break; - case 125000: t_config = TWAI_TIMING_CONFIG_125KBITS(); break; - case 100000: t_config = TWAI_TIMING_CONFIG_100KBITS(); break; - case 50000: t_config = TWAI_TIMING_CONFIG_50KBITS(); break; - case 25000: t_config = TWAI_TIMING_CONFIG_25KBITS(); break; - default: t_config = TWAI_TIMING_CONFIG_250KBITS(); break; - } - - twai_filter_config_t f_config = TWAI_FILTER_CONFIG_ACCEPT_ALL(); - - if (twai_driver_install(&g_config, &t_config, &f_config) != ESP_OK) { - return false; - } - - if (twai_start() != ESP_OK) { - twai_driver_uninstall(); - return false; - } - - return true; +twai_general_config_t g_config = TWAI_GENERAL_CONFIG_DEFAULT( +(gpio_num_t)ESP32_TWAI_TX_PIN, +(gpio_num_t)ESP32_TWAI_RX_PIN, +TWAI_MODE_NORMAL +); + +twai_timing_config_t t_config; +switch (CAN_BAUDRATE) { + case 1000000: t_config = TWAI_TIMING_CONFIG_1MBITS(); break; + case 800000: t_config = TWAI_TIMING_CONFIG_800KBITS(); break; + case 500000: t_config = TWAI_TIMING_CONFIG_500KBITS(); break; + case 250000: t_config = TWAI_TIMING_CONFIG_250KBITS(); break; + case 125000: t_config = TWAI_TIMING_CONFIG_125KBITS(); break; + case 100000: t_config = TWAI_TIMING_CONFIG_100KBITS(); break; + case 50000: t_config = TWAI_TIMING_CONFIG_50KBITS(); break; + case 25000: t_config = TWAI_TIMING_CONFIG_25KBITS(); break; + default: t_config = TWAI_TIMING_CONFIG_250KBITS(); break; +} + +twai_filter_config_t f_config = TWAI_FILTER_CONFIG_ACCEPT_ALL(); + +if (twai_driver_install(&g_config, &t_config, &f_config) != ESP_OK) { + return false; +} + +if (twai_start() != ESP_OK) { + twai_driver_uninstall(); + return false; +} + +return true; + + } #endif // IS_ESP32_TWAI +/* TCAN4551 SPI Controller */ + +#ifdef IS_TCAN4X5X + +const uint8_t MISO_PIN = 16; +const uint8_t CS_PIN = 17; +const uint8_t SCK_PIN = 18; +const uint8_t MOSI_PIN = 19; +const uint8_t INT1_PIN = 20; +const uint8_t INT2_PIN = 21; +const uint8_t RST_PIN = 0xFF; + +TCAN4x5x can_intf(CS_PIN, MISO_PIN, MOSI_PIN, SCK_PIN, RST_PIN); + +volatile bool tcanIrqAsserted = false; +void IRAM_ATTR onTcanPinFall() { +tcanIrqAsserted = true; +} + +bool setupCan() { +uint16_t prescaler = (uint16_t)(1000000UL / CAN_BAUDRATE); +if (prescaler < 1) prescaler = 1; +can_intf.setNominalBitRate(prescaler, 32, 8); + +TCAN4x5x_MRAM_Config customMRAM = {0}; +customMRAM.SIDNumElements = 1; +customMRAM.XIDNumElements = 1; +customMRAM.Rx0NumElements = 16; +customMRAM.Rx0ElementSize = MRAM_8_Byte_Data; +customMRAM.TxBufferNumElements = 4; +customMRAM.TxBufferElementSize = MRAM_8_Byte_Data; +can_intf.setMRAMConfig(&customMRAM); + +TCAN4x5x_MCAN_Global_Filter_Configuration gfc = {0}; +gfc.RRFE = 1; +gfc.RRFS = 1; +gfc.ANFE = TCAN4x5x_GFC_ACCEPT_INTO_RXFIFO0; +gfc.ANFS = TCAN4x5x_GFC_ACCEPT_INTO_RXFIFO0; +can_intf.setGlobalFilterConfig(&gfc); + +TCAN4x5x_DEV_CONFIG devCfg = {0}; +devCfg.SWE_DIS = 1; +devCfg.CLK_REF = 1; +devCfg.GPIO1_CONFIG = TCAN4x5x_DEV_CONFIG_GPIO1_CONFIG_GPO; +devCfg.GPIO1_GPO_CONFIG = TCAN4x5x_DEV_CONFIG_GPO1_MCAN_INT1; +devCfg.GPO2_CONFIG = TCAN4x5x_DEV_CONFIG_GPO2_MCAN_INT0; +devCfg.WD_ACTION = TCAN4x5x_DEV_CONFIG_WDT_ACTION_nINT; +devCfg.WAKE_CONFIG = TCAN4x5x_DEV_CONFIG_WAKE_BOTH_EDGES; +can_intf.setDeviceConfig(&devCfg); + +can_intf.attachInterruptPin(INT1_PIN, onTcanPinFall); +can_intf.attachInterruptPin(INT2_PIN, onTcanPinFall); + +can_intf.begin(&SPI); + +TCAN4x5x_MCAN_CCCR_Config cccr = {0}; +can_intf.MCAN_EnableProtectedRegisters(); +can_intf.MCAN_ReadCCCRRegister(&cccr); +cccr.FDOE = 0; +cccr.BRSE = 0; +can_intf.MCAN_ConfigureCCCRRegister(&cccr); +can_intf.MCAN_DisableProtectedRegisters(); + +can_intf.WDT_Disable(); +can_intf.Device_SetMode(TCAN4x5x_DEVICE_MODE_NORMAL); + +return (can_intf.Device_ReadDeviceVersion() != 0xFFFF && can_intf.Device_ReadMode() == TCAN4x5x_DEVICE_MODE_NORMAL); +} + +#endif // IS_TCAN4X5X /* Example sketch ------------------------------------------------------------*/ @@ -221,10 +289,10 @@ ODriveCAN odrv0(wrap_can_intf(can_intf), ODRV0_NODE_ID); // Standard CAN message ODriveCAN* odrives[] = {&odrv0}; // Make sure all ODriveCAN instances are accounted for here struct ODriveUserData { - Heartbeat_msg_t last_heartbeat; - bool received_heartbeat = false; - Get_Encoder_Estimates_msg_t last_feedback; - bool received_feedback = false; +Heartbeat_msg_t last_heartbeat; +bool received_heartbeat = false; +Get_Encoder_Estimates_msg_t last_feedback; +bool received_feedback = false; }; // Keep some application-specific user data for every ODrive. @@ -232,117 +300,118 @@ ODriveUserData odrv0_user_data; // Called every time a Heartbeat message arrives from the ODrive void onHeartbeat(Heartbeat_msg_t& msg, void* user_data) { - ODriveUserData* odrv_user_data = static_cast(user_data); - odrv_user_data->last_heartbeat = msg; - odrv_user_data->received_heartbeat = true; +ODriveUserData* odrv_user_data = static_cast(user_data); +odrv_user_data->last_heartbeat = msg; +odrv_user_data->received_heartbeat = true; } // Called every time a feedback message arrives from the ODrive void onFeedback(Get_Encoder_Estimates_msg_t& msg, void* user_data) { - ODriveUserData* odrv_user_data = static_cast(user_data); - odrv_user_data->last_feedback = msg; - odrv_user_data->received_feedback = true; +ODriveUserData* odrv_user_data = static_cast(user_data); +odrv_user_data->last_feedback = msg; +odrv_user_data->received_feedback = true; } // Called for every message that arrives on the CAN bus void onCanMessage(const CanMsg& msg) { - for (auto odrive: odrives) { - onReceive(msg, *odrive); - } +for (auto odrive: odrives) { +onReceive(msg, *odrive); +} } void setup() { - Serial.begin(115200); - - // Wait for up to 3 seconds for the serial port to be opened on the PC side. - // If no PC connects, continue anyway. - for (int i = 0; i < 30 && !Serial; ++i) { - delay(100); - } - delay(200); - - - Serial.println("Starting ODriveCAN demo"); - - // Register callbacks for the heartbeat and encoder feedback messages - odrv0.onFeedback(onFeedback, &odrv0_user_data); - odrv0.onStatus(onHeartbeat, &odrv0_user_data); - - // Configure and initialize the CAN bus interface. This function depends on - // your hardware and the CAN stack that you're using. - if (!setupCan()) { - Serial.println("CAN failed to initialize: reset required"); - while (true); // spin indefinitely - } - - Serial.println("Waiting for ODrive..."); - while (!odrv0_user_data.received_heartbeat) { - pumpEvents(can_intf); - } - - Serial.println("found ODrive"); - - // request bus voltage and current (1sec timeout) - Serial.println("attempting to read bus voltage and current"); - Get_Bus_Voltage_Current_msg_t vbus; - if (!odrv0.request(vbus, 1000)) { - Serial.println("vbus request failed!"); - while (true); // spin indefinitely - } - - Serial.print("DC voltage [V]: "); - Serial.println(vbus.Bus_Voltage); - Serial.print("DC current [A]: "); - Serial.println(vbus.Bus_Current); - - Serial.println("Enabling closed loop control..."); - while (odrv0_user_data.last_heartbeat.Axis_State != ODriveAxisState::AXIS_STATE_CLOSED_LOOP_CONTROL) { - odrv0.clearErrors(); - delay(1); - odrv0.setState(ODriveAxisState::AXIS_STATE_CLOSED_LOOP_CONTROL); - - // Pump events for 150ms. This delay is needed for two reasons; - // 1. If there is an error condition, such as missing DC power, the ODrive might - // briefly attempt to enter CLOSED_LOOP_CONTROL state, so we can't rely - // on the first heartbeat response, so we want to receive at least two - // heartbeats (100ms default interval). - // 2. If the bus is congested, the setState command won't get through - // immediately but can be delayed. - for (int i = 0; i < 15; ++i) { - delay(10); - pumpEvents(can_intf); - } - } - - Serial.println("ODrive running!"); +Serial.begin(115200); + +// Wait for up to 3 seconds for the serial port to be opened on the PC side. +// If no PC connects, continue anyway. +for (int i = 0; i < 30 && !Serial; ++i) { +delay(100); +} +delay(200); + +Serial.println("Starting ODriveCAN demo"); + +// Register callbacks for the heartbeat and encoder feedback messages +odrv0.onFeedback(onFeedback, &odrv0_user_data); +odrv0.onStatus(onHeartbeat, &odrv0_user_data); + +// Configure and initialize the CAN bus interface. This function depends on +// your hardware and the CAN stack that you're using. +if (!setupCan()) { +Serial.println("CAN failed to initialize: reset required"); +while (true); // spin indefinitely +} + +Serial.println("Waiting for ODrive..."); +while (!odrv0_user_data.received_heartbeat) { +pumpEvents(can_intf); +} + +Serial.println("found ODrive"); + +// request bus voltage and current (1sec timeout) +Serial.println("attempting to read bus voltage and current"); +Get_Bus_Voltage_Current_msg_t vbus; +if (!odrv0.request(vbus, 1000)) { +Serial.println("vbus request failed!"); +while (true); // spin indefinitely +} + +Serial.print("DC voltage [V]: "); +Serial.println(vbus.Bus_Voltage); +Serial.print("DC current [A]: "); +Serial.println(vbus.Bus_Current); + +Serial.println("Enabling closed loop control..."); +while (odrv0_user_data.last_heartbeat.Axis_State != ODriveAxisState::AXIS_STATE_CLOSED_LOOP_CONTROL) { +odrv0.clearErrors(); +delay(1); +odrv0.setState(ODriveAxisState::AXIS_STATE_CLOSED_LOOP_CONTROL); + +// Pump events for 150ms. This delay is needed for two reasons; +// 1. If there is an error condition, such as missing DC power, the ODrive might +// briefly attempt to enter CLOSED_LOOP_CONTROL state, so we can't rely +// on the first heartbeat response, so we want to receive at least two +// heartbeats (100ms default interval). +// 2. If the bus is congested, the setState command won't get through +// immediately but can be delayed. +for (int i = 0; i < 15; ++i) { + delay(10); + pumpEvents(can_intf); +} + + +} + +Serial.println("ODrive running!"); } void loop() { - pumpEvents(can_intf); // This is required on some platforms to handle incoming feedback CAN messages - // Note that on MCP2515-based platforms, this will delay for a fixed 10ms. - // - // This has been found to reduce the number of dropped messages, however it can be removed - // for applications requiring loop times over 100Hz. - - float SINE_PERIOD = 2.0f; // Period of the position command sine wave in seconds - - float t = 0.001 * millis(); - - float phase = t * (TWO_PI / SINE_PERIOD); - - odrv0.setPosition( - sin(phase), // position - cos(phase) * (TWO_PI / SINE_PERIOD) // velocity feedforward (optional) - ); - - // print position and velocity for Serial Plotter - if (odrv0_user_data.received_feedback) { - Get_Encoder_Estimates_msg_t feedback = odrv0_user_data.last_feedback; - odrv0_user_data.received_feedback = false; - Serial.print("odrv0-pos:"); - Serial.print(feedback.Pos_Estimate); - Serial.print(","); - Serial.print("odrv0-vel:"); - Serial.println(feedback.Vel_Estimate); - } +pumpEvents(can_intf); // This is required on some platforms to handle incoming feedback CAN messages +// Note that on MCP2515-based platforms, this will delay for a fixed 10ms. +// +// This has been found to reduce the number of dropped messages, however it can be removed +// for applications requiring loop times over 100Hz. + +float SINE_PERIOD = 2.0f; // Period of the position command sine wave in seconds + +float t = 0.001 * millis(); + +float phase = t * (TWO_PI / SINE_PERIOD); + +odrv0.setPosition( +sin(phase), // position +cos(phase) * (TWO_PI / SINE_PERIOD) // velocity feedforward (optional) +); + +// print position and velocity for Serial Plotter +if (odrv0_user_data.received_feedback) { +Get_Encoder_Estimates_msg_t feedback = odrv0_user_data.last_feedback; +odrv0_user_data.received_feedback = false; +Serial.print("odrv0-pos:"); +Serial.print(feedback.Pos_Estimate); +Serial.print(","); +Serial.print("odrv0-vel:"); +Serial.println(feedback.Vel_Estimate); +} } From 9a83a0df2d511ebcb480fb369d063a4b507a794c Mon Sep 17 00:00:00 2001 From: George Papamichail <152964062+gi0rg0sPapamichail@users.noreply.github.com> Date: Mon, 9 Nov 2026 14:44:59 +0200 Subject: [PATCH 2/4] Add files via upload --- src/ODriveTCAN4x5x.hpp | 96 ++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 96 insertions(+) create mode 100644 src/ODriveTCAN4x5x.hpp diff --git a/src/ODriveTCAN4x5x.hpp b/src/ODriveTCAN4x5x.hpp new file mode 100644 index 0000000..8ff7466 --- /dev/null +++ b/src/ODriveTCAN4x5x.hpp @@ -0,0 +1,96 @@ +#pragma once + +#include +#include +#include "ODriveCAN.h" +#include "TCAN4x5x.h" + +// Classical CAN frame container used by ODriveArduino sketches +struct CanMsg { + uint32_t id; // Bit 31 (0x80000000) indicates 29-bit Extended ID + uint8_t len; // 0..8 bytes + uint8_t buffer[8]; // Classical CAN payload +}; + +// Implemented in the main .ino sketch to dispatch frames to ODriveCAN instances +void onCanMessage(const CanMsg& msg); + +// 1. Transmit function called by ODriveCAN using your library's exact method names +static bool sendMsg(TCAN4x5x& can_intf, uint32_t id, uint8_t length, const uint8_t* data) { + if (length > 8) { + return false; // ODrive uses Classical CAN (max 8 bytes) + } + + TCAN4x5x_MCAN_TX_Header txHdr = {0}; + + // ODriveCAN sets bit 31 (0x80000000) if the frame uses a 29-bit Extended ID + if (id & 0x80000000UL) { + txHdr.XTD = 1; + txHdr.ID = id & 0x1FFFFFFFUL; + } else { + txHdr.XTD = 0; + txHdr.ID = id & 0x000007FFUL; // MCAN_WriteTXBuffer shifts standard IDs << 18 internally + } + + // ODriveCAN passes data == nullptr when sending an RTR request (e.g. odrv0.request()) + txHdr.RTR = (data == nullptr) ? 1 : 0; + txHdr.DLC = length; + txHdr.FDF = 0; // Must be 0 (Classical CAN) for ODrive + txHdr.BRS = 0; // Must be 0 (No Bit Rate Switching) + txHdr.ESI = 0; + txHdr.EFC = 0; + txHdr.MM = 0; + + // Provide a safe zeroed buffer when data == nullptr so MCAN_WriteTXBuffer doesn't fault + uint8_t payload[8] = {0}; + if (data != nullptr && length > 0) { + memcpy(payload, data, length); + } + + // Use your library's exact methods: MCAN_WriteTXBuffer & MCAN_TransmitBufferContents + if (can_intf.MCAN_WriteTXBuffer(0, &txHdr, payload) == 0) { + return false; + } + return can_intf.MCAN_TransmitBufferContents(0); +} + +// 2. Pass a received CanMsg into an ODriveCAN instance +static void onReceive(const CanMsg& msg, ODriveCAN& odrive) { + odrive.onReceive(msg.id, msg.len, msg.buffer); +} + +// 3. Event pump calling your library's exact method: MCAN_ReadNextFIFO +static void pumpEvents(TCAN4x5x& can_intf) { + while (true) { + TCAN4x5x_MCAN_RX_Header rxHdr = {0}; + uint8_t rxData[64] = {0}; // Safety buffer matching MRAM configuration + + // Sentinel value to detect empty FIFO + rxHdr.DLC = 0x0F; + uint8_t numBytes = can_intf.MCAN_ReadNextFIFO(RXFIFO0, &rxHdr, rxData); + + if (numBytes == 0 && rxHdr.DLC == 0x0F) { + break; // FIFO is completely empty + } + + // Ignore CAN-FD frames, Remote frames, or payloads > 8 bytes + if (rxHdr.FDF || rxHdr.RTR || numBytes > 8) { + continue; + } + + CanMsg msg = {0}; + msg.id = rxHdr.ID; + if (rxHdr.XTD) { + msg.id |= 0x80000000UL; + } + msg.len = numBytes; + if (numBytes > 0) { + memcpy(msg.buffer, rxData, numBytes); + } + + onCanMessage(msg); + } +} + +// 4. Generate wrap_can_intf(TCAN4x5x&) for ODriveCAN +CREATE_CAN_INTF_WRAPPER(TCAN4x5x) \ No newline at end of file From a5bee8b0a1afcd24c371ae66035fbda5fb1b5b49 Mon Sep 17 00:00:00 2001 From: George Papamichail <152964062+gi0rg0sPapamichail@users.noreply.github.com> Date: Mon, 9 Nov 2026 16:31:57 +0200 Subject: [PATCH 3/4] Update ODriveTCAN4x5x.hpp --- src/ODriveTCAN4x5x.hpp | 37 +++++++++++-------------------------- 1 file changed, 11 insertions(+), 26 deletions(-) diff --git a/src/ODriveTCAN4x5x.hpp b/src/ODriveTCAN4x5x.hpp index 8ff7466..c6508ce 100644 --- a/src/ODriveTCAN4x5x.hpp +++ b/src/ODriveTCAN4x5x.hpp @@ -5,75 +5,61 @@ #include "ODriveCAN.h" #include "TCAN4x5x.h" -// Classical CAN frame container used by ODriveArduino sketches struct CanMsg { - uint32_t id; // Bit 31 (0x80000000) indicates 29-bit Extended ID - uint8_t len; // 0..8 bytes - uint8_t buffer[8]; // Classical CAN payload + uint32_t id; + uint8_t len; + uint8_t buffer[8]; }; -// Implemented in the main .ino sketch to dispatch frames to ODriveCAN instances +// Must be defined by the application void onCanMessage(const CanMsg& msg); -// 1. Transmit function called by ODriveCAN using your library's exact method names static bool sendMsg(TCAN4x5x& can_intf, uint32_t id, uint8_t length, const uint8_t* data) { if (length > 8) { - return false; // ODrive uses Classical CAN (max 8 bytes) + return false; } - TCAN4x5x_MCAN_TX_Header txHdr = {0}; - - // ODriveCAN sets bit 31 (0x80000000) if the frame uses a 29-bit Extended ID if (id & 0x80000000UL) { txHdr.XTD = 1; txHdr.ID = id & 0x1FFFFFFFUL; } else { txHdr.XTD = 0; - txHdr.ID = id & 0x000007FFUL; // MCAN_WriteTXBuffer shifts standard IDs << 18 internally + txHdr.ID = id & 0x000007FFUL; } - // ODriveCAN passes data == nullptr when sending an RTR request (e.g. odrv0.request()) txHdr.RTR = (data == nullptr) ? 1 : 0; txHdr.DLC = length; - txHdr.FDF = 0; // Must be 0 (Classical CAN) for ODrive - txHdr.BRS = 0; // Must be 0 (No Bit Rate Switching) + txHdr.FDF = 0; + txHdr.BRS = 0; txHdr.ESI = 0; txHdr.EFC = 0; txHdr.MM = 0; - // Provide a safe zeroed buffer when data == nullptr so MCAN_WriteTXBuffer doesn't fault uint8_t payload[8] = {0}; if (data != nullptr && length > 0) { memcpy(payload, data, length); } - - // Use your library's exact methods: MCAN_WriteTXBuffer & MCAN_TransmitBufferContents if (can_intf.MCAN_WriteTXBuffer(0, &txHdr, payload) == 0) { return false; } return can_intf.MCAN_TransmitBufferContents(0); } -// 2. Pass a received CanMsg into an ODriveCAN instance static void onReceive(const CanMsg& msg, ODriveCAN& odrive) { odrive.onReceive(msg.id, msg.len, msg.buffer); } -// 3. Event pump calling your library's exact method: MCAN_ReadNextFIFO static void pumpEvents(TCAN4x5x& can_intf) { while (true) { TCAN4x5x_MCAN_RX_Header rxHdr = {0}; - uint8_t rxData[64] = {0}; // Safety buffer matching MRAM configuration - - // Sentinel value to detect empty FIFO + uint8_t rxData[64] = {0}; rxHdr.DLC = 0x0F; uint8_t numBytes = can_intf.MCAN_ReadNextFIFO(RXFIFO0, &rxHdr, rxData); if (numBytes == 0 && rxHdr.DLC == 0x0F) { - break; // FIFO is completely empty + break; } - // Ignore CAN-FD frames, Remote frames, or payloads > 8 bytes if (rxHdr.FDF || rxHdr.RTR || numBytes > 8) { continue; } @@ -92,5 +78,4 @@ static void pumpEvents(TCAN4x5x& can_intf) { } } -// 4. Generate wrap_can_intf(TCAN4x5x&) for ODriveCAN -CREATE_CAN_INTF_WRAPPER(TCAN4x5x) \ No newline at end of file +CREATE_CAN_INTF_WRAPPER(TCAN4x5x) From 66727c8d3c5780a70bcc225b0a3fae40cd11ee10 Mon Sep 17 00:00:00 2001 From: George Papamichail <152964062+gi0rg0sPapamichail@users.noreply.github.com> Date: Mon, 9 Nov 2026 16:48:14 +0200 Subject: [PATCH 4/4] Update SineWaveCAN.ino --- examples/SineWaveCAN/SineWaveCAN.ino | 331 ++++++++++++++------------- 1 file changed, 168 insertions(+), 163 deletions(-) diff --git a/examples/SineWaveCAN/SineWaveCAN.ino b/examples/SineWaveCAN/SineWaveCAN.ino index a1c0073..822d277 100644 --- a/examples/SineWaveCAN/SineWaveCAN.ino +++ b/examples/SineWaveCAN/SineWaveCAN.ino @@ -1,9 +1,11 @@ + #include #include "ODriveCAN.h" // Documentation for this example can be found here: // https://docs.odriverobotics.com/v/latest/guides/arduino-can-guide.html + /* Configuration of example sketch -------------------------------------------*/ // CAN bus baudrate. Make sure this matches for every device on the bus @@ -78,6 +80,7 @@ struct ODriveStatus; // hack to prevent teensy compile error /* Board-specific settings ---------------------------------------------------*/ + /* Teensy */ #ifdef IS_TEENSY_BUILTIN @@ -85,17 +88,18 @@ struct ODriveStatus; // hack to prevent teensy compile error FlexCAN_T4 can_intf; bool setupCan() { -can_intf.begin(); -can_intf.setBaudRate(CAN_BAUDRATE); -can_intf.setMaxMB(16); -can_intf.enableFIFO(); -can_intf.enableFIFOInterrupt(); -can_intf.onReceive(onCanMessage); -return true; + can_intf.begin(); + can_intf.setBaudRate(CAN_BAUDRATE); + can_intf.setMaxMB(16); + can_intf.enableFIFO(); + can_intf.enableFIFOInterrupt(); + can_intf.onReceive(onCanMessage); + return true; } #endif // IS_TEENSY_BUILTIN + /* MCP2515-based extension modules -*/ #ifdef IS_MCP2515 @@ -109,33 +113,35 @@ MCP2515Class& can_intf = CAN; // NOTE: not all Arduino pins are interruptable, check the documentation for your board! #define MCP2515_INT 2 -// freqeuncy of the crystal oscillator on the MCP2515 breakout board. +// freqeuncy of the crystal oscillator on the MCP2515 breakout board. // common values are: 16 MHz, 12 MHz, 8 MHz #define MCP2515_CLK_HZ 8000000 + static inline void receiveCallback(int packet_size) { -if (packet_size > 8) { -return; // not supported -} -CanMsg msg = {.id = (unsigned int)CAN.packetId(), .len = (uint8_t)packet_size}; -CAN.readBytes(msg.buffer, packet_size); -onCanMessage(msg); + if (packet_size > 8) { + return; // not supported + } + CanMsg msg = {.id = (unsigned int)CAN.packetId(), .len = (uint8_t)packet_size}; + CAN.readBytes(msg.buffer, packet_size); + onCanMessage(msg); } bool setupCan() { -// configure and initialize the CAN bus interface -CAN.setPins(MCP2515_CS, MCP2515_INT); -CAN.setClockFrequency(MCP2515_CLK_HZ); -if (!CAN.begin(CAN_BAUDRATE)) { -return false; -} + // configure and initialize the CAN bus interface + CAN.setPins(MCP2515_CS, MCP2515_INT); + CAN.setClockFrequency(MCP2515_CLK_HZ); + if (!CAN.begin(CAN_BAUDRATE)) { + return false; + } -CAN.onReceive(receiveCallback); -return true; + CAN.onReceive(receiveCallback); + return true; } #endif // IS_MCP2515 + /* Arduinos with built-in CAN */ #ifdef IS_ARDUINO_BUILTIN @@ -143,11 +149,12 @@ return true; HardwareCAN& can_intf = CAN; bool setupCan() { -return can_intf.begin((CanBitRate)CAN_BAUDRATE); + return can_intf.begin((CanBitRate)CAN_BAUDRATE); } #endif + /* STM32 boards with built-in CAN */ #ifdef IS_STM32_BUILTIN @@ -156,13 +163,14 @@ STM32_CAN Can1( CAN1 ); STM32_CAN& can_intf = Can1; bool setupCan() { -can_intf.begin(); -can_intf.setBaudRate(CAN_BAUDRATE); -return true; + can_intf.begin(); + can_intf.setBaudRate(CAN_BAUDRATE); + return true; } #endif // IS_STM32_BUILTIN + /* ESP32 boards with built-in TWAI (CAN) */ #ifdef IS_ESP32_TWAI @@ -174,39 +182,37 @@ return true; ESP32TWAIIntf can_intf; bool setupCan() { -twai_general_config_t g_config = TWAI_GENERAL_CONFIG_DEFAULT( -(gpio_num_t)ESP32_TWAI_TX_PIN, -(gpio_num_t)ESP32_TWAI_RX_PIN, -TWAI_MODE_NORMAL -); - -twai_timing_config_t t_config; -switch (CAN_BAUDRATE) { - case 1000000: t_config = TWAI_TIMING_CONFIG_1MBITS(); break; - case 800000: t_config = TWAI_TIMING_CONFIG_800KBITS(); break; - case 500000: t_config = TWAI_TIMING_CONFIG_500KBITS(); break; - case 250000: t_config = TWAI_TIMING_CONFIG_250KBITS(); break; - case 125000: t_config = TWAI_TIMING_CONFIG_125KBITS(); break; - case 100000: t_config = TWAI_TIMING_CONFIG_100KBITS(); break; - case 50000: t_config = TWAI_TIMING_CONFIG_50KBITS(); break; - case 25000: t_config = TWAI_TIMING_CONFIG_25KBITS(); break; - default: t_config = TWAI_TIMING_CONFIG_250KBITS(); break; -} - -twai_filter_config_t f_config = TWAI_FILTER_CONFIG_ACCEPT_ALL(); - -if (twai_driver_install(&g_config, &t_config, &f_config) != ESP_OK) { - return false; -} - -if (twai_start() != ESP_OK) { - twai_driver_uninstall(); - return false; -} - -return true; - - + twai_general_config_t g_config = TWAI_GENERAL_CONFIG_DEFAULT( + (gpio_num_t)ESP32_TWAI_TX_PIN, + (gpio_num_t)ESP32_TWAI_RX_PIN, + TWAI_MODE_NORMAL + ); + + twai_timing_config_t t_config; + switch (CAN_BAUDRATE) { + case 1000000: t_config = TWAI_TIMING_CONFIG_1MBITS(); break; + case 800000: t_config = TWAI_TIMING_CONFIG_800KBITS(); break; + case 500000: t_config = TWAI_TIMING_CONFIG_500KBITS(); break; + case 250000: t_config = TWAI_TIMING_CONFIG_250KBITS(); break; + case 125000: t_config = TWAI_TIMING_CONFIG_125KBITS(); break; + case 100000: t_config = TWAI_TIMING_CONFIG_100KBITS(); break; + case 50000: t_config = TWAI_TIMING_CONFIG_50KBITS(); break; + case 25000: t_config = TWAI_TIMING_CONFIG_25KBITS(); break; + default: t_config = TWAI_TIMING_CONFIG_250KBITS(); break; + } + + twai_filter_config_t f_config = TWAI_FILTER_CONFIG_ACCEPT_ALL(); + + if (twai_driver_install(&g_config, &t_config, &f_config) != ESP_OK) { + return false; + } + + if (twai_start() != ESP_OK) { + twai_driver_uninstall(); + return false; + } + + return true; } #endif // IS_ESP32_TWAI @@ -289,10 +295,10 @@ ODriveCAN odrv0(wrap_can_intf(can_intf), ODRV0_NODE_ID); // Standard CAN message ODriveCAN* odrives[] = {&odrv0}; // Make sure all ODriveCAN instances are accounted for here struct ODriveUserData { -Heartbeat_msg_t last_heartbeat; -bool received_heartbeat = false; -Get_Encoder_Estimates_msg_t last_feedback; -bool received_feedback = false; + Heartbeat_msg_t last_heartbeat; + bool received_heartbeat = false; + Get_Encoder_Estimates_msg_t last_feedback; + bool received_feedback = false; }; // Keep some application-specific user data for every ODrive. @@ -300,118 +306,117 @@ ODriveUserData odrv0_user_data; // Called every time a Heartbeat message arrives from the ODrive void onHeartbeat(Heartbeat_msg_t& msg, void* user_data) { -ODriveUserData* odrv_user_data = static_cast(user_data); -odrv_user_data->last_heartbeat = msg; -odrv_user_data->received_heartbeat = true; + ODriveUserData* odrv_user_data = static_cast(user_data); + odrv_user_data->last_heartbeat = msg; + odrv_user_data->received_heartbeat = true; } // Called every time a feedback message arrives from the ODrive void onFeedback(Get_Encoder_Estimates_msg_t& msg, void* user_data) { -ODriveUserData* odrv_user_data = static_cast(user_data); -odrv_user_data->last_feedback = msg; -odrv_user_data->received_feedback = true; + ODriveUserData* odrv_user_data = static_cast(user_data); + odrv_user_data->last_feedback = msg; + odrv_user_data->received_feedback = true; } // Called for every message that arrives on the CAN bus void onCanMessage(const CanMsg& msg) { -for (auto odrive: odrives) { -onReceive(msg, *odrive); -} + for (auto odrive: odrives) { + onReceive(msg, *odrive); + } } void setup() { -Serial.begin(115200); - -// Wait for up to 3 seconds for the serial port to be opened on the PC side. -// If no PC connects, continue anyway. -for (int i = 0; i < 30 && !Serial; ++i) { -delay(100); -} -delay(200); - -Serial.println("Starting ODriveCAN demo"); - -// Register callbacks for the heartbeat and encoder feedback messages -odrv0.onFeedback(onFeedback, &odrv0_user_data); -odrv0.onStatus(onHeartbeat, &odrv0_user_data); - -// Configure and initialize the CAN bus interface. This function depends on -// your hardware and the CAN stack that you're using. -if (!setupCan()) { -Serial.println("CAN failed to initialize: reset required"); -while (true); // spin indefinitely -} - -Serial.println("Waiting for ODrive..."); -while (!odrv0_user_data.received_heartbeat) { -pumpEvents(can_intf); -} - -Serial.println("found ODrive"); - -// request bus voltage and current (1sec timeout) -Serial.println("attempting to read bus voltage and current"); -Get_Bus_Voltage_Current_msg_t vbus; -if (!odrv0.request(vbus, 1000)) { -Serial.println("vbus request failed!"); -while (true); // spin indefinitely -} - -Serial.print("DC voltage [V]: "); -Serial.println(vbus.Bus_Voltage); -Serial.print("DC current [A]: "); -Serial.println(vbus.Bus_Current); - -Serial.println("Enabling closed loop control..."); -while (odrv0_user_data.last_heartbeat.Axis_State != ODriveAxisState::AXIS_STATE_CLOSED_LOOP_CONTROL) { -odrv0.clearErrors(); -delay(1); -odrv0.setState(ODriveAxisState::AXIS_STATE_CLOSED_LOOP_CONTROL); - -// Pump events for 150ms. This delay is needed for two reasons; -// 1. If there is an error condition, such as missing DC power, the ODrive might -// briefly attempt to enter CLOSED_LOOP_CONTROL state, so we can't rely -// on the first heartbeat response, so we want to receive at least two -// heartbeats (100ms default interval). -// 2. If the bus is congested, the setState command won't get through -// immediately but can be delayed. -for (int i = 0; i < 15; ++i) { - delay(10); - pumpEvents(can_intf); -} - - -} - -Serial.println("ODrive running!"); + Serial.begin(115200); + + // Wait for up to 3 seconds for the serial port to be opened on the PC side. + // If no PC connects, continue anyway. + for (int i = 0; i < 30 && !Serial; ++i) { + delay(100); + } + delay(200); + + + Serial.println("Starting ODriveCAN demo"); + + // Register callbacks for the heartbeat and encoder feedback messages + odrv0.onFeedback(onFeedback, &odrv0_user_data); + odrv0.onStatus(onHeartbeat, &odrv0_user_data); + + // Configure and initialize the CAN bus interface. This function depends on + // your hardware and the CAN stack that you're using. + if (!setupCan()) { + Serial.println("CAN failed to initialize: reset required"); + while (true); // spin indefinitely + } + + Serial.println("Waiting for ODrive..."); + while (!odrv0_user_data.received_heartbeat) { + pumpEvents(can_intf); + } + + Serial.println("found ODrive"); + + // request bus voltage and current (1sec timeout) + Serial.println("attempting to read bus voltage and current"); + Get_Bus_Voltage_Current_msg_t vbus; + if (!odrv0.request(vbus, 1000)) { + Serial.println("vbus request failed!"); + while (true); // spin indefinitely + } + + Serial.print("DC voltage [V]: "); + Serial.println(vbus.Bus_Voltage); + Serial.print("DC current [A]: "); + Serial.println(vbus.Bus_Current); + + Serial.println("Enabling closed loop control..."); + while (odrv0_user_data.last_heartbeat.Axis_State != ODriveAxisState::AXIS_STATE_CLOSED_LOOP_CONTROL) { + odrv0.clearErrors(); + delay(1); + odrv0.setState(ODriveAxisState::AXIS_STATE_CLOSED_LOOP_CONTROL); + + // Pump events for 150ms. This delay is needed for two reasons; + // 1. If there is an error condition, such as missing DC power, the ODrive might + // briefly attempt to enter CLOSED_LOOP_CONTROL state, so we can't rely + // on the first heartbeat response, so we want to receive at least two + // heartbeats (100ms default interval). + // 2. If the bus is congested, the setState command won't get through + // immediately but can be delayed. + for (int i = 0; i < 15; ++i) { + delay(10); + pumpEvents(can_intf); + } + } + + Serial.println("ODrive running!"); } void loop() { -pumpEvents(can_intf); // This is required on some platforms to handle incoming feedback CAN messages -// Note that on MCP2515-based platforms, this will delay for a fixed 10ms. -// -// This has been found to reduce the number of dropped messages, however it can be removed -// for applications requiring loop times over 100Hz. - -float SINE_PERIOD = 2.0f; // Period of the position command sine wave in seconds - -float t = 0.001 * millis(); - -float phase = t * (TWO_PI / SINE_PERIOD); - -odrv0.setPosition( -sin(phase), // position -cos(phase) * (TWO_PI / SINE_PERIOD) // velocity feedforward (optional) -); - -// print position and velocity for Serial Plotter -if (odrv0_user_data.received_feedback) { -Get_Encoder_Estimates_msg_t feedback = odrv0_user_data.last_feedback; -odrv0_user_data.received_feedback = false; -Serial.print("odrv0-pos:"); -Serial.print(feedback.Pos_Estimate); -Serial.print(","); -Serial.print("odrv0-vel:"); -Serial.println(feedback.Vel_Estimate); -} + pumpEvents(can_intf); // This is required on some platforms to handle incoming feedback CAN messages + // Note that on MCP2515-based platforms, this will delay for a fixed 10ms. + // + // This has been found to reduce the number of dropped messages, however it can be removed + // for applications requiring loop times over 100Hz. + + float SINE_PERIOD = 2.0f; // Period of the position command sine wave in seconds + + float t = 0.001 * millis(); + + float phase = t * (TWO_PI / SINE_PERIOD); + + odrv0.setPosition( + sin(phase), // position + cos(phase) * (TWO_PI / SINE_PERIOD) // velocity feedforward (optional) + ); + + // print position and velocity for Serial Plotter + if (odrv0_user_data.received_feedback) { + Get_Encoder_Estimates_msg_t feedback = odrv0_user_data.last_feedback; + odrv0_user_data.received_feedback = false; + Serial.print("odrv0-pos:"); + Serial.print(feedback.Pos_Estimate); + Serial.print(","); + Serial.print("odrv0-vel:"); + Serial.println(feedback.Vel_Estimate); + } }