├── .clang-format ├── .github └── workflows │ ├── arduino.yaml │ ├── formatting.yaml │ └── platformio.yaml ├── .gitignore ├── CHANGELOG.md ├── LICENSE ├── README.md ├── examples ├── SineWaveCAN │ ├── .arduino-ci.yml │ └── SineWaveCAN.ino └── SineWaveUART │ ├── .arduino-ci.yml │ └── SineWaveUART.ino ├── format-ignore.txt ├── keywords.txt ├── library.properties ├── platformio.ini └── src ├── Doxyfile ├── ODriveCAN.cpp ├── ODriveCAN.h ├── ODriveEnums.h ├── ODriveFlexCAN.hpp ├── ODriveHardwareCAN.hpp ├── ODriveMCPCAN.hpp ├── ODriveSTM32CAN.hpp ├── ODriveUART.cpp ├── ODriveUART.h ├── can_helpers.hpp └── can_simple_messages.hpp /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | BasedOnStyle: Google 3 | IndentWidth: '4' 4 | ColumnLimit: '120' 5 | 6 | IncludeCategories: 7 | # main include automatically assigned to Priority 0 8 | - Regex: '^".*_(conf|config)\.(hpp|h)"$' # config headers 9 | Priority: 3 10 | - Regex: '^".*"$' # sibling & project headers 11 | Priority: 1 12 | - Regex: '^<.*>$' # system & library headers 13 | Priority: 2 14 | 15 | # Format function arguments and parameters 16 | BinPackArguments: 'false' 17 | BinPackParameters: 'false' 18 | AllowAllArgumentsOnNextLine: 'false' 19 | AllowAllParametersOfDeclarationOnNextLine: 'false' 20 | AlignAfterOpenBracket: BlockIndent 21 | SpaceBeforeParens: ControlStatements 22 | 23 | # Constructor Formatting 24 | PackConstructorInitializers: CurrentLine 25 | IndentAccessModifiers: 'false' 26 | AccessModifierOffset: '-4' 27 | SpaceBeforeCtorInitializerColon: 'true' 28 | BreakConstructorInitializers: BeforeColon 29 | LambdaBodyIndentation: OuterScope 30 | 31 | AllowShortCaseLabelsOnASingleLine: 'true' 32 | AllowShortBlocksOnASingleLine: 'false' 33 | AllowShortIfStatementsOnASingleLine: Never 34 | AllowShortLoopsOnASingleLine: 'false' 35 | AllowShortEnumsOnASingleLine: 'true' 36 | AllowShortFunctionsOnASingleLine: 'Inline' 37 | AllowShortLambdasOnASingleLine: 'All' 38 | 39 | # Switch / Case 40 | IndentCaseLabels: 'true' 41 | IndentCaseBlocks: 'false' 42 | 43 | # Preprocessor stuff 44 | AlignConsecutiveMacros: 'false' 45 | AlignEscapedNewlines: DontAlign 46 | AlignTrailingComments: 'false' 47 | SpacesBeforeTrailingComments: 1 48 | 49 | # Alignment of procedural code 50 | AlignConsecutiveAssignments: 'false' 51 | AlignConsecutiveDeclarations: 'false' 52 | AlignConsecutiveBitFields: Consecutive 53 | 54 | AlignOperands: AlignAfterOperator 55 | # BreakBeforeTernaryOperators: 'false' 56 | BreakBeforeBinaryOperators: 'true' 57 | 58 | # Pointers and East/West Const 59 | DerivePointerAlignment: 'false' 60 | PointerAlignment: Left 61 | QualifierAlignment: Leave 62 | # QualifierOrder: ['volatile', 'constexpr', 'static', 'inline', 'type', 'const'] 63 | 64 | Cpp11BracedListStyle: 'true' 65 | 66 | # Vertical Whitespace 67 | SeparateDefinitionBlocks: Leave 68 | EmptyLineBeforeAccessModifier: Always 69 | EmptyLineAfterAccessModifier: Never 70 | 71 | # AlignArrayOfStructures: Right 72 | # InsertTrailingCommas: 'Wrapped' 73 | 74 | 75 | AlwaysBreakAfterReturnType: None 76 | PenaltyReturnTypeOnItsOwnLine: 9999 # We really hate breaking after return types 77 | PenaltyBreakAssignment: 999 # Prefer not to break around = 78 | 79 | SpaceAfterTemplateKeyword: 'false' 80 | 81 | MaxEmptyLinesToKeep: 2 82 | 83 | FixNamespaceComments: 'true' 84 | #IndentPPDirectives: BeforeHash 85 | # NamespaceIndentation: All 86 | ... 87 | -------------------------------------------------------------------------------- /.github/workflows/arduino.yaml: -------------------------------------------------------------------------------- 1 | name: Arduino Lib Compliance 2 | 3 | on: 4 | pull_request: 5 | branches: [master] 6 | push: 7 | branches: [master] 8 | 9 | jobs: 10 | arduino_ci: 11 | runs-on: ubuntu-latest 12 | 13 | steps: 14 | - uses: actions/checkout@v3 15 | 16 | - uses: Arduino-CI/action@stable-1.x 17 | env: 18 | EXPECT_EXAMPLES: true 19 | -------------------------------------------------------------------------------- /.github/workflows/formatting.yaml: -------------------------------------------------------------------------------- 1 | name: Clang-Format Check 2 | 3 | on: 4 | push: 5 | branches: [master] 6 | pull_request: 7 | paths: 8 | - '**/*.cpp' 9 | - '**/*.h' 10 | - '**/*.hpp' 11 | 12 | jobs: 13 | clang-format-check: 14 | runs-on: ubuntu-latest 15 | 16 | steps: 17 | - name: Checkout code 18 | uses: actions/checkout@v4 19 | 20 | - name: Install Clang-Format 21 | run: sudo apt-get install -y clang-format 22 | 23 | - name: Run Clang-Format Check 24 | run: | 25 | files=$(find . -name "*.cpp" -o -name "*.hpp" -o -name "*.h") 26 | check_ignore=$(git -c core.excludesFile="format-ignore.txt" check-ignore --stdin --no-index --non-matching --verbose <<< "$files") 27 | files=$(awk '/^::/ {print $2}' <<< "$check_ignore") 28 | 29 | echo "Checking $(echo "$files" | wc -l) files" 30 | 31 | if [ -n "$files" ]; then 32 | clang-format -style=file -i $files 33 | git diff --exit-code || (echo "Clang-Format check failed!" && exit 1) 34 | else 35 | echo "No files to check." 36 | fi 37 | -------------------------------------------------------------------------------- /.github/workflows/platformio.yaml: -------------------------------------------------------------------------------- 1 | # Reference: https://docs.platformio.org/en/stable/integration/ci/github-actions.html 2 | 3 | name: PlatformIO CI 4 | 5 | on: 6 | push: 7 | branches: [master] 8 | pull_request: 9 | branches: [master] 10 | 11 | jobs: 12 | build: 13 | runs-on: ubuntu-latest 14 | steps: 15 | - uses: actions/checkout@v3 16 | 17 | - uses: actions/cache@v4 18 | with: 19 | path: | 20 | ~/.cache/pip 21 | ~/.platformio/.cache 22 | key: ${{ runner.os }}-pio 23 | 24 | - uses: actions/setup-python@v4 25 | with: 26 | python-version: '3.9' 27 | 28 | - name: Install PlatformIO Core 29 | run: pip install --upgrade platformio 30 | 31 | - name: Run PlatformIO 32 | run: | 33 | pio ci --lib="." --project-conf platformio.ini examples/SineWaveCAN/SineWaveCAN.ino 34 | #pio ci --lib="." examples/SineWaveCAN/SineWaveCAN.ino 35 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | 2 | # Doxygen 3 | /src/xml 4 | 5 | # PlatformIO 6 | /.pio 7 | 8 | # OS files 9 | .DS_Store 10 | -------------------------------------------------------------------------------- /CHANGELOG.md: -------------------------------------------------------------------------------- 1 | # [0.10.7] 2 | - Fixed int overflow issue w/ Arduino Mega example 3 | 4 | # [0.10.6] 5 | - Fixed getEndpoint not updating requested_msg_id_, causing periodic messages to be read as a TxSdo message. 6 | 7 | # [0.10.5] 8 | - Fixed SineWaveCAN example vbus request timeout 9 | 10 | # [0.10.4] 11 | 12 | - Updated can_helpers to remove some UB and make code run on Arduino boards more reliably 13 | 14 | # [0.10.3] 15 | 16 | - Fixed CAN awaitMsg timeout; now actually milliseconds instead of seconds 17 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2017-2023 ODrive Robotics Inc. 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ODriveArduino 2 | Arduino library for controlling [ODrive motor controllers](https://odriverobotics.com/). 3 | 4 | Supports UART and CAN communication. 5 | 6 | See https://docs.odriverobotics.com/v/latest/guides/arduino-uart-guide.html for usage instructions with UART. 7 | 8 | See https://docs.odriverobotics.com/v/latest/guides/arduino-can-guide.html for usage instructions with CAN. 9 | -------------------------------------------------------------------------------- /examples/SineWaveCAN/.arduino-ci.yml: -------------------------------------------------------------------------------- 1 | # See https://github.com/Arduino-CI/arduino_ci/blob/master/REFERENCE.md 2 | 3 | compile: 4 | platforms: 5 | - uno_minima_r4 6 | 7 | packages: 8 | teensy:teensy: 9 | url: https://www.pjrc.com/teensy/package_teensy_index.json 10 | arduino:renesas_uno: 11 | url: https://downloads.arduino.cc/packages/package_index.json 12 | 13 | platforms: 14 | uno_minima_r4: 15 | board: arduino:renesas_uno:minima 16 | package: arduino:renesas_uno 17 | gcc: 18 | features: 19 | defines: 20 | - IS_ARDUINO_BUILTIN 21 | warnings: 22 | flags: 23 | teensy41: 24 | board: teensy:teensy:teensy41 25 | package: teensy:teensy 26 | gcc: 27 | features: 28 | defines: 29 | - __IMXRT1062__ 30 | - ARDUINO_ARCH_TEENSY 31 | - TEENSY41 32 | - IS_TEENSY_BUILTIN 33 | warnings: 34 | flags: 35 | -------------------------------------------------------------------------------- /examples/SineWaveCAN/SineWaveCAN.ino: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include "ODriveCAN.h" 4 | 5 | // Documentation for this example can be found here: 6 | // https://docs.odriverobotics.com/v/latest/guides/arduino-can-guide.html 7 | 8 | 9 | /* Configuration of example sketch -------------------------------------------*/ 10 | 11 | // CAN bus baudrate. Make sure this matches for every device on the bus 12 | #define CAN_BAUDRATE 250000 13 | 14 | // ODrive node_id for odrv0 15 | #define ODRV0_NODE_ID 0 16 | 17 | // Uncomment below the line that corresponds to your hardware. 18 | // See also "Board-specific settings" to adapt the details for your hardware setup. 19 | 20 | // #define IS_TEENSY_BUILTIN // Teensy boards with built-in CAN interface (e.g. Teensy 4.1). See below to select which interface to use. 21 | // #define IS_ARDUINO_BUILTIN // Arduino boards with built-in CAN interface (e.g. Arduino Uno R4 Minima) 22 | // #define IS_MCP2515 // Any board with external MCP2515 based extension module. See below to configure the module. 23 | // #define IS_STM32_BUILTIN // STM32 boards with built-in CAN interface (e.g. STM32F4 Discovery). 24 | 25 | 26 | /* Board-specific includes ---------------------------------------------------*/ 27 | 28 | #if defined(IS_TEENSY_BUILTIN) + defined(IS_ARDUINO_BUILTIN) + defined(IS_MCP2515) + defined(IS_STM32_BUILTIN) != 1 29 | #warning "Select exactly one hardware option at the top of this file." 30 | 31 | #if CAN_HOWMANY > 0 || CANFD_HOWMANY > 0 32 | #define IS_ARDUINO_BUILTIN 33 | #warning "guessing that this uses HardwareCAN" 34 | #else 35 | #error "cannot guess hardware version" 36 | #endif 37 | 38 | #endif 39 | 40 | #ifdef IS_ARDUINO_BUILTIN 41 | // See https://github.com/arduino/ArduinoCore-API/blob/master/api/HardwareCAN.h 42 | // and https://github.com/arduino/ArduinoCore-renesas/tree/main/libraries/Arduino_CAN 43 | 44 | #include 45 | #include 46 | #endif // IS_ARDUINO_BUILTIN 47 | 48 | #ifdef IS_MCP2515 49 | // See https://github.com/sandeepmistry/arduino-CAN/ 50 | #include "MCP2515.h" 51 | #include "ODriveMCPCAN.hpp" 52 | #endif // IS_MCP2515 53 | 54 | #ifdef IS_TEENSY_BUILTIN 55 | // See https://github.com/tonton81/FlexCAN_T4 56 | // clone https://github.com/tonton81/FlexCAN_T4.git into /src 57 | #include 58 | #include "ODriveFlexCAN.hpp" 59 | struct ODriveStatus; // hack to prevent teensy compile error 60 | #endif // IS_TEENSY_BUILTIN 61 | 62 | #ifdef IS_STM32_BUILTIN 63 | // See https://github.com/pazi88/STM32_CAN 64 | #include 65 | #include "ODriveSTM32CAN.hpp" 66 | #endif // IS_STM32_BUILTIN 67 | 68 | 69 | 70 | /* Board-specific settings ---------------------------------------------------*/ 71 | 72 | 73 | /* Teensy */ 74 | 75 | #ifdef IS_TEENSY_BUILTIN 76 | 77 | FlexCAN_T4 can_intf; 78 | 79 | bool setupCan() { 80 | can_intf.begin(); 81 | can_intf.setBaudRate(CAN_BAUDRATE); 82 | can_intf.setMaxMB(16); 83 | can_intf.enableFIFO(); 84 | can_intf.enableFIFOInterrupt(); 85 | can_intf.onReceive(onCanMessage); 86 | return true; 87 | } 88 | 89 | #endif // IS_TEENSY_BUILTIN 90 | 91 | 92 | /* MCP2515-based extension modules -*/ 93 | 94 | #ifdef IS_MCP2515 95 | 96 | MCP2515Class& can_intf = CAN; 97 | 98 | // chip select pin used for the MCP2515 99 | #define MCP2515_CS 10 100 | 101 | // interrupt pin used for the MCP2515 102 | // NOTE: not all Arduino pins are interruptable, check the documentation for your board! 103 | #define MCP2515_INT 2 104 | 105 | // freqeuncy of the crystal oscillator on the MCP2515 breakout board. 106 | // common values are: 16 MHz, 12 MHz, 8 MHz 107 | #define MCP2515_CLK_HZ 8000000 108 | 109 | 110 | static inline void receiveCallback(int packet_size) { 111 | if (packet_size > 8) { 112 | return; // not supported 113 | } 114 | CanMsg msg = {.id = (unsigned int)CAN.packetId(), .len = (uint8_t)packet_size}; 115 | CAN.readBytes(msg.buffer, packet_size); 116 | onCanMessage(msg); 117 | } 118 | 119 | bool setupCan() { 120 | // configure and initialize the CAN bus interface 121 | CAN.setPins(MCP2515_CS, MCP2515_INT); 122 | CAN.setClockFrequency(MCP2515_CLK_HZ); 123 | if (!CAN.begin(CAN_BAUDRATE)) { 124 | return false; 125 | } 126 | 127 | CAN.onReceive(receiveCallback); 128 | return true; 129 | } 130 | 131 | #endif // IS_MCP2515 132 | 133 | 134 | /* Arduinos with built-in CAN */ 135 | 136 | #ifdef IS_ARDUINO_BUILTIN 137 | 138 | HardwareCAN& can_intf = CAN; 139 | 140 | bool setupCan() { 141 | return can_intf.begin((CanBitRate)CAN_BAUDRATE); 142 | } 143 | 144 | #endif 145 | 146 | 147 | /* STM32 boards with built-in CAN */ 148 | 149 | #ifdef IS_STM32_BUILTIN 150 | 151 | STM32_CAN Can1( CAN1 ); 152 | STM32_CAN& can_intf = Can1; 153 | 154 | bool setupCan() { 155 | can_intf.begin(); 156 | can_intf.setBaudRate(CAN_BAUDRATE); 157 | return true; 158 | } 159 | 160 | #endif // IS_STM32_BUILTIN 161 | 162 | 163 | /* Example sketch ------------------------------------------------------------*/ 164 | 165 | // Instantiate ODrive objects 166 | ODriveCAN odrv0(wrap_can_intf(can_intf), ODRV0_NODE_ID); // Standard CAN message ID 167 | ODriveCAN* odrives[] = {&odrv0}; // Make sure all ODriveCAN instances are accounted for here 168 | 169 | struct ODriveUserData { 170 | Heartbeat_msg_t last_heartbeat; 171 | bool received_heartbeat = false; 172 | Get_Encoder_Estimates_msg_t last_feedback; 173 | bool received_feedback = false; 174 | }; 175 | 176 | // Keep some application-specific user data for every ODrive. 177 | ODriveUserData odrv0_user_data; 178 | 179 | // Called every time a Heartbeat message arrives from the ODrive 180 | void onHeartbeat(Heartbeat_msg_t& msg, void* user_data) { 181 | ODriveUserData* odrv_user_data = static_cast(user_data); 182 | odrv_user_data->last_heartbeat = msg; 183 | odrv_user_data->received_heartbeat = true; 184 | } 185 | 186 | // Called every time a feedback message arrives from the ODrive 187 | void onFeedback(Get_Encoder_Estimates_msg_t& msg, void* user_data) { 188 | ODriveUserData* odrv_user_data = static_cast(user_data); 189 | odrv_user_data->last_feedback = msg; 190 | odrv_user_data->received_feedback = true; 191 | } 192 | 193 | // Called for every message that arrives on the CAN bus 194 | void onCanMessage(const CanMsg& msg) { 195 | for (auto odrive: odrives) { 196 | onReceive(msg, *odrive); 197 | } 198 | } 199 | 200 | void setup() { 201 | Serial.begin(115200); 202 | 203 | // Wait for up to 3 seconds for the serial port to be opened on the PC side. 204 | // If no PC connects, continue anyway. 205 | for (int i = 0; i < 30 && !Serial; ++i) { 206 | delay(100); 207 | } 208 | delay(200); 209 | 210 | 211 | Serial.println("Starting ODriveCAN demo"); 212 | 213 | // Register callbacks for the heartbeat and encoder feedback messages 214 | odrv0.onFeedback(onFeedback, &odrv0_user_data); 215 | odrv0.onStatus(onHeartbeat, &odrv0_user_data); 216 | 217 | // Configure and initialize the CAN bus interface. This function depends on 218 | // your hardware and the CAN stack that you're using. 219 | if (!setupCan()) { 220 | Serial.println("CAN failed to initialize: reset required"); 221 | while (true); // spin indefinitely 222 | } 223 | 224 | Serial.println("Waiting for ODrive..."); 225 | while (!odrv0_user_data.received_heartbeat) { 226 | pumpEvents(can_intf); 227 | delay(100); 228 | } 229 | 230 | Serial.println("found ODrive"); 231 | 232 | // request bus voltage and current (1sec timeout) 233 | Serial.println("attempting to read bus voltage and current"); 234 | Get_Bus_Voltage_Current_msg_t vbus; 235 | if (!odrv0.request(vbus, 1000)) { 236 | Serial.println("vbus request failed!"); 237 | while (true); // spin indefinitely 238 | } 239 | 240 | Serial.print("DC voltage [V]: "); 241 | Serial.println(vbus.Bus_Voltage); 242 | Serial.print("DC current [A]: "); 243 | Serial.println(vbus.Bus_Current); 244 | 245 | Serial.println("Enabling closed loop control..."); 246 | while (odrv0_user_data.last_heartbeat.Axis_State != ODriveAxisState::AXIS_STATE_CLOSED_LOOP_CONTROL) { 247 | odrv0.clearErrors(); 248 | delay(1); 249 | odrv0.setState(ODriveAxisState::AXIS_STATE_CLOSED_LOOP_CONTROL); 250 | 251 | // Pump events for 150ms. This delay is needed for two reasons; 252 | // 1. If there is an error condition, such as missing DC power, the ODrive might 253 | // briefly attempt to enter CLOSED_LOOP_CONTROL state, so we can't rely 254 | // on the first heartbeat response, so we want to receive at least two 255 | // heartbeats (100ms default interval). 256 | // 2. If the bus is congested, the setState command won't get through 257 | // immediately but can be delayed. 258 | for (int i = 0; i < 15; ++i) { 259 | delay(10); 260 | pumpEvents(can_intf); 261 | } 262 | } 263 | 264 | Serial.println("ODrive running!"); 265 | } 266 | 267 | void loop() { 268 | pumpEvents(can_intf); // This is required on some platforms to handle incoming feedback CAN messages 269 | // Note that on MCP2515-based platforms, this will delay for a fixed 10ms. 270 | // 271 | // This has been found to reduce the number of dropped messages, however it can be removed 272 | // for applications requiring loop times over 100Hz. 273 | 274 | float SINE_PERIOD = 2.0f; // Period of the position command sine wave in seconds 275 | 276 | float t = 0.001 * millis(); 277 | 278 | float phase = t * (TWO_PI / SINE_PERIOD); 279 | 280 | odrv0.setPosition( 281 | sin(phase), // position 282 | cos(phase) * (TWO_PI / SINE_PERIOD) // velocity feedforward (optional) 283 | ); 284 | 285 | // print position and velocity for Serial Plotter 286 | if (odrv0_user_data.received_feedback) { 287 | Get_Encoder_Estimates_msg_t feedback = odrv0_user_data.last_feedback; 288 | odrv0_user_data.received_feedback = false; 289 | Serial.print("odrv0-pos:"); 290 | Serial.print(feedback.Pos_Estimate); 291 | Serial.print(","); 292 | Serial.print("odrv0-vel:"); 293 | Serial.println(feedback.Vel_Estimate); 294 | } 295 | } 296 | -------------------------------------------------------------------------------- /examples/SineWaveUART/.arduino-ci.yml: -------------------------------------------------------------------------------- 1 | compile: 2 | # Example is written for Arduino Uno. 3 | # Most other boards either don't define `Serial` or don't support SoftwareSerial.h 4 | platforms: 5 | - uno 6 | # - due # not supported 7 | # - zero # not supported 8 | - leonardo 9 | - mega2560 10 | -------------------------------------------------------------------------------- /examples/SineWaveUART/SineWaveUART.ino: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include 4 | 5 | // Documentation for this example can be found here: 6 | // https://docs.odriverobotics.com/v/latest/guides/arduino-uart-guide.html 7 | 8 | 9 | //////////////////////////////// 10 | // Set up serial pins to the ODrive 11 | //////////////////////////////// 12 | 13 | // Below are some sample configurations. 14 | // You can comment out the default one and uncomment the one you wish to use. 15 | // You can of course use something different if you like 16 | // Don't forget to also connect ODrive ISOVDD and ISOGND to Arduino 3.3V/5V and GND. 17 | 18 | // Arduino without spare serial ports (such as Arduino UNO) have to use software serial. 19 | // Note that this is implemented poorly and can lead to wrong data sent or read. 20 | // pin 8: RX - connect to ODrive TX 21 | // pin 9: TX - connect to ODrive RX 22 | SoftwareSerial odrive_serial(8, 9); 23 | unsigned long baudrate = 19200; // Must match what you configure on the ODrive (see docs for details) 24 | 25 | // Teensy 3 and 4 (all versions) - Serial1 26 | // pin 0: RX - connect to ODrive TX 27 | // pin 1: TX - connect to ODrive RX 28 | // See https://www.pjrc.com/teensy/td_uart.html for other options on Teensy 29 | // HardwareSerial& odrive_serial = Serial1; 30 | // unsigned long baudrate = 115200; // Must match what you configure on the ODrive (see docs for details) 31 | 32 | // Arduino Mega or Due - Serial1 33 | // pin 19: RX - connect to ODrive TX 34 | // pin 18: TX - connect to ODrive RX 35 | // See https://www.arduino.cc/reference/en/language/functions/communication/serial/ for other options 36 | // HardwareSerial& odrive_serial = Serial1; 37 | // unsigned long baudrate = 115200; // Must match what you configure on the ODrive (see docs for details) 38 | 39 | 40 | ODriveUART odrive(odrive_serial); 41 | 42 | void setup() { 43 | odrive_serial.begin(baudrate); 44 | 45 | Serial.begin(115200); // Serial to PC 46 | 47 | delay(10); 48 | 49 | Serial.println("Waiting for ODrive..."); 50 | while (odrive.getState() == AXIS_STATE_UNDEFINED) { 51 | delay(100); 52 | } 53 | 54 | Serial.println("found ODrive"); 55 | 56 | Serial.print("DC voltage: "); 57 | Serial.println(odrive.getParameterAsFloat("vbus_voltage")); 58 | 59 | Serial.println("Enabling closed loop control..."); 60 | while (odrive.getState() != AXIS_STATE_CLOSED_LOOP_CONTROL) { 61 | odrive.clearErrors(); 62 | odrive.setState(AXIS_STATE_CLOSED_LOOP_CONTROL); 63 | delay(10); 64 | } 65 | 66 | Serial.println("ODrive running!"); 67 | } 68 | 69 | void loop() { 70 | float SINE_PERIOD = 2.0f; // Period of the position command sine wave in seconds 71 | 72 | float t = 0.001 * millis(); 73 | 74 | float phase = t * (TWO_PI / SINE_PERIOD); 75 | 76 | odrive.setPosition( 77 | sin(phase), // position 78 | cos(phase) * (TWO_PI / SINE_PERIOD) // velocity feedforward (optional) 79 | ); 80 | 81 | ODriveFeedback feedback = odrive.getFeedback(); 82 | Serial.print("pos:"); 83 | Serial.print(feedback.pos); 84 | Serial.print(", "); 85 | Serial.print("vel:"); 86 | Serial.print(feedback.vel); 87 | Serial.println(); 88 | } 89 | -------------------------------------------------------------------------------- /format-ignore.txt: -------------------------------------------------------------------------------- 1 | # autogenerated files 2 | /src/can_simple_messages.hpp 3 | /src/ODriveEnums.h 4 | -------------------------------------------------------------------------------- /keywords.txt: -------------------------------------------------------------------------------- 1 | 2 | # Classes 3 | 4 | ODriveArduino KEYWORD1 5 | 6 | # Functions 7 | 8 | clearErrors KEYWORD2 9 | setPosition KEYWORD2 10 | setPosition KEYWORD2 11 | setPosition KEYWORD2 12 | setVelocity KEYWORD2 13 | setVelocity KEYWORD2 14 | setCurrent KEYWORD2 15 | trapezoidalMove KEYWORD2 16 | getFeedback KEYWORD2 17 | getPosition KEYWORD2 18 | getVelocity KEYWORD2 19 | getParameterAsString KEYWORD2 20 | getParameterAsInt KEYWORD2 21 | getParameterAsFloat KEYWORD2 22 | setParameter KEYWORD2 23 | setParameter KEYWORD2 24 | setState KEYWORD2 25 | getState KEYWORD2 26 | runState KEYWORD2 27 | readLine KEYWORD2 28 | -------------------------------------------------------------------------------- /library.properties: -------------------------------------------------------------------------------- 1 | # https://arduino.github.io/arduino-cli/0.33/library-specification/ 2 | name=ODriveArduino 3 | version=0.10.7 4 | author=ODrive Robotics Inc. 5 | maintainer=ODrive Robotics Inc. 6 | sentence=Library to control ODrive motor controllers 7 | paragraph=Allows sending setpoints, reading feedback, and changing configuration. 8 | category=Device Control 9 | url=https://github.com/odriverobotics/ODriveArduino 10 | architectures=* 11 | includes=ODriveUART.h,ODriveCAN.h 12 | -------------------------------------------------------------------------------- /platformio.ini: -------------------------------------------------------------------------------- 1 | ; PlatformIO Project Configuration File 2 | ; 3 | ; Build options: build flags, source filter 4 | ; Upload options: custom upload port, speed and extra flags 5 | ; Library options: dependencies, extra library storages 6 | ; Advanced options: extra scripting 7 | ; 8 | ; Please visit documentation for the other options and examples 9 | ; https://docs.platformio.org/page/projectconf.html 10 | 11 | [platformio] 12 | src_dir = examples/SineWaveCAN 13 | lib_dir = . 14 | 15 | [env:teensy41] 16 | platform = teensy 17 | board = teensy41 18 | framework = arduino 19 | build_flags = -DIS_TEENSY_BUILTIN 20 | 21 | [env:uno_r4_minima] 22 | platform = renesas-ra 23 | board = uno_r4_minima 24 | framework = arduino 25 | build_flags = -DIS_ARDUINO_BUILTIN 26 | 27 | [env:uno] 28 | platform = atmelavr 29 | board = uno 30 | framework = arduino 31 | build_flags = -DIS_MCP2515 32 | lib_deps = 33 | https://github.com/sandeepmistry/arduino-CAN.git 34 | 35 | [env:stm32f405] 36 | platform = ststm32 37 | board = adafruit_feather_f405 38 | framework = arduino 39 | build_flags = 40 | -DIS_STM32_BUILTIN 41 | ; Enable serial. 42 | ; See https://github.com/platformio/platform-ststm32/issues/420#issuecomment-672277396 43 | -USBCON 44 | -DPIO_FRAMEWORK_ARDUINO_ENABLE_CDC 45 | ; Enable CAN module in HAL drivers. 46 | ; See https://github.com/pazi88/STM32_CAN 47 | -DHAL_CAN_MODULE_ENABLED 48 | lib_deps = 49 | https://github.com/pazi88/STM32_CAN.git#1.2.0 -------------------------------------------------------------------------------- /src/Doxyfile: -------------------------------------------------------------------------------- 1 | 2 | PROJECT_NAME = "ODriveArduino" 3 | 4 | FILE_PATTERNS = *.h 5 | 6 | GENERATE_TODOLIST = NO 7 | GENERATE_TESTLIST = NO 8 | GENERATE_BUGLIST = NO 9 | GENERATE_DEPRECATEDLIST= NO 10 | GENERATE_HTML = NO 11 | GENERATE_DOCSET = NO 12 | GENERATE_HTMLHELP = NO 13 | GENERATE_CHI = NO 14 | GENERATE_QHP = NO 15 | GENERATE_ECLIPSEHELP = NO 16 | GENERATE_TREEVIEW = NO 17 | GENERATE_LATEX = NO 18 | GENERATE_RTF = NO 19 | GENERATE_MAN = NO 20 | GENERATE_XML = YES 21 | GENERATE_DOCBOOK = NO 22 | GENERATE_AUTOGEN_DEF = NO 23 | GENERATE_PERLMOD = NO 24 | GENERATE_TAGFILE = NO 25 | GENERATE_LEGEND = NO 26 | -------------------------------------------------------------------------------- /src/ODriveCAN.cpp: -------------------------------------------------------------------------------- 1 | // Author: ODrive Robotics Inc. 2 | // License: MIT 3 | // Documentation: https://docs.odriverobotics.com/v/latest/guides/arduino-can-guide.html 4 | 5 | #include "ODriveCAN.h" 6 | 7 | #include // needed for debug printing 8 | 9 | bool ODriveCAN::clearErrors() { 10 | Clear_Errors_msg_t msg; 11 | return send(msg); 12 | } 13 | 14 | bool ODriveCAN::setPosition(float position, float velocity_feedforward, float torque_feedforward) { 15 | Set_Input_Pos_msg_t msg; 16 | 17 | msg.Input_Pos = position; 18 | msg.Vel_FF = velocity_feedforward; 19 | msg.Torque_FF = torque_feedforward; 20 | 21 | return send(msg); 22 | } 23 | 24 | bool ODriveCAN::setVelocity(float velocity, float torque_feedforward) { 25 | Set_Input_Vel_msg_t msg; 26 | 27 | msg.Input_Vel = velocity; 28 | msg.Input_Torque_FF = torque_feedforward; 29 | 30 | return send(msg); 31 | } 32 | 33 | bool ODriveCAN::setControllerMode(uint8_t control_mode, uint8_t input_mode) { 34 | Set_Controller_Mode_msg_t msg; 35 | 36 | msg.Control_Mode = control_mode; 37 | msg.Input_Mode = input_mode; 38 | 39 | return send(msg); 40 | } 41 | 42 | bool ODriveCAN::setTorque(float torque) { 43 | Set_Input_Torque_msg_t msg; 44 | 45 | msg.Input_Torque = torque; 46 | 47 | return send(msg); 48 | } 49 | 50 | bool ODriveCAN::setState(ODriveAxisState requested_state) { 51 | Set_Axis_State_msg_t msg; 52 | 53 | msg.Axis_Requested_State = (uint32_t)requested_state; 54 | 55 | return send(msg); 56 | } 57 | 58 | bool ODriveCAN::setLimits(float velocity_limit, float current_soft_max) { 59 | Set_Limits_msg_t msg; 60 | 61 | msg.Velocity_Limit = velocity_limit; 62 | msg.Current_Limit = current_soft_max; 63 | 64 | return send(msg); 65 | } 66 | 67 | bool ODriveCAN::setPosGain(float pos_gain) { 68 | Set_Pos_Gain_msg_t msg; 69 | 70 | msg.Pos_Gain = pos_gain; 71 | 72 | return send(msg); 73 | } 74 | 75 | bool ODriveCAN::setVelGains(float vel_gain, float vel_integrator_gain) { 76 | Set_Vel_Gains_msg_t msg; 77 | 78 | msg.Vel_Gain = vel_gain; 79 | msg.Vel_Integrator_Gain = vel_integrator_gain; 80 | 81 | return send(msg); 82 | } 83 | 84 | bool ODriveCAN::setAbsolutePosition(float abs_pos) { 85 | Set_Absolute_Position_msg_t msg; 86 | 87 | msg.Position = abs_pos; 88 | 89 | return send(msg); 90 | } 91 | 92 | bool ODriveCAN::setTrapezoidalVelLimit(float vel_limit) { 93 | Set_Traj_Vel_Limit_msg_t msg; 94 | 95 | msg.Traj_Vel_Limit = vel_limit; 96 | 97 | return send(msg); 98 | } 99 | 100 | bool ODriveCAN::setTrapezoidalAccelLimits(float accel_limit, float decel_limit) { 101 | Set_Traj_Accel_Limits_msg_t msg; 102 | 103 | msg.Traj_Accel_Limit = accel_limit; 104 | msg.Traj_Decel_Limit = decel_limit; 105 | 106 | return send(msg); 107 | } 108 | 109 | bool ODriveCAN::getCurrents(Get_Iq_msg_t& msg, uint16_t timeout_ms) { 110 | return request(msg, timeout_ms); 111 | } 112 | 113 | bool ODriveCAN::getTemperature(Get_Temperature_msg_t& msg, uint16_t timeout_ms) { 114 | return request(msg, timeout_ms); 115 | } 116 | 117 | bool ODriveCAN::getError(Get_Error_msg_t& msg, uint16_t timeout_ms) { 118 | return request(msg, timeout_ms); 119 | } 120 | 121 | bool ODriveCAN::getVersion(Get_Version_msg_t& msg, uint16_t timeout_ms) { 122 | return request(msg, timeout_ms); 123 | } 124 | 125 | bool ODriveCAN::getFeedback(Get_Encoder_Estimates_msg_t& msg, uint16_t timeout_ms) { 126 | return request(msg, timeout_ms); 127 | } 128 | 129 | bool ODriveCAN::getBusVI(Get_Bus_Voltage_Current_msg_t& msg, uint16_t timeout_ms) { 130 | return request(msg, timeout_ms); 131 | } 132 | 133 | bool ODriveCAN::getPower(Get_Powers_msg_t& msg, uint16_t timeout_ms) { 134 | return request(msg, timeout_ms); 135 | } 136 | 137 | bool ODriveCAN::reset(ResetAction action) { 138 | Reboot_msg_t msg; 139 | msg.Action = action; 140 | 141 | return send(msg); 142 | } 143 | 144 | void ODriveCAN::onReceive(uint32_t id, uint8_t length, const uint8_t* data) { 145 | #ifdef DEBUG 146 | int byte_index = length - 1; 147 | Serial.println(F("received:")); 148 | Serial.print(F(" id: 0x")); 149 | Serial.println(id, HEX); 150 | Serial.print(F(" data: 0x")); 151 | while (byte_index >= 0) 152 | Serial.print(msg.data[byte_index--], HEX); 153 | Serial.println(F("")); 154 | #endif // DEBUG 155 | // Check that the message is meant for this node. 156 | if (node_id_ != (id >> ODriveCAN::kNodeIdShift)) 157 | return; 158 | // If the message is requested, copy it in the request buffer and exit. 159 | if ((id & ODriveCAN::kCmdIdBits) == requested_msg_id_) { 160 | memcpy(buffer_, data, length); 161 | requested_msg_id_ = REQUEST_PENDING; 162 | return; 163 | }; 164 | // Check if any of the registered callback handlers apply. Useful for cyclic 165 | // messages. 166 | switch (id & ODriveCAN::kCmdIdBits) { 167 | case Get_Encoder_Estimates_msg_t::cmd_id: { 168 | Get_Encoder_Estimates_msg_t estimates; 169 | estimates.decode_buf(data); 170 | if (feedback_callback_) 171 | feedback_callback_(estimates, feedback_user_data_); 172 | break; 173 | } 174 | case Get_Torques_msg_t::cmd_id: { 175 | Get_Torques_msg_t estimates; 176 | estimates.decode_buf(data); 177 | if (torques_callback_) 178 | torques_callback_(estimates, torques_user_data_); 179 | break; 180 | } 181 | case Heartbeat_msg_t::cmd_id: { 182 | Heartbeat_msg_t status; 183 | status.decode_buf(data); 184 | if (axis_state_callback_ != nullptr) 185 | axis_state_callback_(status, axis_state_user_data_); 186 | else 187 | Serial.println(F("missing callback")); 188 | break; 189 | } 190 | case Get_Temperature_msg_t::cmd_id: { 191 | Get_Temperature_msg_t temperature; 192 | temperature.decode_buf(data); 193 | if (temperature_callback_) 194 | temperature_callback_(temperature, temperature_user_data_); 195 | break; 196 | } 197 | case Get_Bus_Voltage_Current_msg_t::cmd_id: { 198 | Get_Bus_Voltage_Current_msg_t bus_vi; 199 | bus_vi.decode_buf(data); 200 | if (busVI_callback_) 201 | busVI_callback_(bus_vi, busVI_user_data_); 202 | break; 203 | } 204 | case Get_Iq_msg_t::cmd_id: { 205 | Get_Iq_msg_t iq; 206 | iq.decode_buf(data); 207 | if (currents_callback_) 208 | currents_callback_(iq, currents_user_data_); 209 | break; 210 | } 211 | case Get_Error_msg_t::cmd_id: { 212 | Get_Error_msg_t error; 213 | error.decode_buf(data); 214 | if (error_callback_) 215 | error_callback_(error, error_user_data_); 216 | break; 217 | } 218 | default: { 219 | if (requested_msg_id_ == REQUEST_PENDING) 220 | return; 221 | #ifdef DEBUG 222 | Serial.print(F("waiting for: 0x")); 223 | Serial.println(requested_msg_id_, HEX); 224 | #endif // DEBUG 225 | } 226 | } 227 | } 228 | 229 | bool ODriveCAN::awaitMsg(uint16_t timeout_ms) { 230 | uint64_t start_time = micros(); 231 | while (requested_msg_id_ != REQUEST_PENDING) { 232 | can_intf_.pump_events(); // pump event loop while waiting 233 | if ((micros() - start_time) > (1000 * timeout_ms)) 234 | return false; 235 | } 236 | return true; 237 | } 238 | -------------------------------------------------------------------------------- /src/ODriveCAN.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "ODriveEnums.h" 4 | #include "can_helpers.hpp" 5 | #include "can_simple_messages.hpp" 6 | 7 | // #define DEBUG 8 | 9 | #define REQUEST_PENDING 0xff 10 | 11 | // clang-format off 12 | // (clang-format gets confused by the lambdas inside the macro) 13 | #define CREATE_CAN_INTF_WRAPPER(TIntf) \ 14 | static inline ODriveCanIntfWrapper wrap_can_intf(TIntf& intf) { \ 15 | return { \ 16 | &intf, \ 17 | [](void* intf, uint32_t id, uint8_t length, const uint8_t* data) { \ 18 | return sendMsg(*(TIntf*)intf, id, length, data); \ 19 | }, \ 20 | [](void* intf) { pumpEvents(*(TIntf*)intf); }, \ 21 | }; \ 22 | } 23 | // clang-format on 24 | 25 | struct ODriveCanIntfWrapper { 26 | bool sendMsg(uint32_t id, uint8_t length, const uint8_t* data) { return (*send_msg_)(can_intf_, id, length, data); } 27 | void pump_events() { (*pump_events_)(can_intf_); } 28 | 29 | void* can_intf_; 30 | bool (*send_msg_)(void* intf, uint32_t id, uint8_t length, const uint8_t* data); 31 | void (*pump_events_)(void* intf); 32 | }; 33 | 34 | class ODriveCAN { 35 | public: 36 | ODriveCAN(const ODriveCanIntfWrapper& can_intf, uint32_t node_id) : can_intf_(can_intf), node_id_(node_id) {} 37 | 38 | /** 39 | * @brief Clear all errors on the ODrive. 40 | * 41 | * This function returns immediately and does not check if the ODrive 42 | * received the CAN message. 43 | */ 44 | bool clearErrors(); 45 | 46 | /** 47 | * @brief Tells the ODrive to change its axis state. 48 | * 49 | * This function returns immediately and does not check if the ODrive 50 | * received the CAN message. 51 | */ 52 | bool setState(ODriveAxisState requested_state); 53 | 54 | /** 55 | * @brief Sets the control mode and input mode of the ODrive. 56 | * 57 | * This function returns immediately and does not check if the ODrive 58 | * received the CAN message. 59 | */ 60 | bool setControllerMode(uint8_t control_mode, uint8_t input_mode); 61 | 62 | /** 63 | * @brief Sends a position setpoint with optional velocity and torque feedforward. 64 | * 65 | * This function returns immediately and does not check if the ODrive 66 | * received the CAN message. 67 | */ 68 | bool setPosition(float position, float velocity_feedforward = 0.0f, float torque_feedforward = 0.0f); 69 | 70 | /** 71 | * @brief Sends a velocity setpoint with optional torque feedforward. 72 | * 73 | * This function returns immediately and does not check if the ODrive 74 | * received the CAN message. 75 | */ 76 | bool setVelocity(float velocity, float torque_feedforward = 0.0f); 77 | 78 | /** 79 | * @brief Sends a torque setpoint to the ODrive. 80 | * 81 | * This function returns immediately and does not check if the ODrive 82 | * received the CAN message. 83 | */ 84 | bool setTorque(float torque); 85 | 86 | /** 87 | * @brief Sets the velocity and current limits 88 | * 89 | * This function returns immediately and does not check if the ODrive 90 | * received the CAN message. 91 | */ 92 | bool setLimits(float velocity_limit, float current_soft_max); 93 | 94 | /** 95 | * @brief Sets the position gain 96 | * 97 | * This function returns immediately and does not check if the ODrive 98 | * received the CAN message. 99 | */ 100 | bool setPosGain(float pos_gain); 101 | 102 | /** 103 | * @brief Sets the velocity and velocity integrator gains 104 | * 105 | * This function returns immediately and does not check if the ODrive 106 | * received the CAN message. 107 | */ 108 | bool setVelGains(float vel_gain, float vel_integrator_gain); 109 | 110 | /** 111 | * @brief Sets the encoder's absolute position and enables absolute positioning 112 | * 113 | * This function returns immediately and does not check if the ODrive 114 | * received the CAN message. 115 | */ 116 | bool setAbsolutePosition(float abs_pos); 117 | 118 | /** 119 | * @brief Sets the coast velocity for subsequent trapezoidal moves 120 | * 121 | * This function returns immediately and does not check if the ODrive 122 | * received the CAN message. 123 | */ 124 | bool setTrapezoidalVelLimit(float vel_limit); 125 | 126 | /** 127 | * @brief Sets the acceleration and deceleration values for subsequent trapezoidal moves 128 | * 129 | * This function returns immediately and does not check if the ODrive 130 | * received the CAN message. 131 | */ 132 | bool setTrapezoidalAccelLimits(float accel_limit, float decel_limit); 133 | 134 | /** 135 | * @brief Requests motor current. Iq_measured represents torque-generating current 136 | * 137 | * This function will block and wait for up to timeout_ms (default 10msec) for ODrive to reply 138 | */ 139 | bool getCurrents(Get_Iq_msg_t& msg, uint16_t timeout_ms = 10); 140 | 141 | /** 142 | * @brief Requests motor temperature 143 | * 144 | * This function will block and wait for up to timeout_ms (default 10msec) for ODrive to reply 145 | */ 146 | bool getTemperature(Get_Temperature_msg_t& msg, uint16_t timeout_ms = 10); 147 | 148 | /** 149 | * @brief Requests error information 150 | * 151 | * This function will block and wait for up to timeout_ms (default 10msec) for ODrive to reply 152 | */ 153 | bool getError(Get_Error_msg_t& msg, uint16_t timeout_ms = 10); 154 | 155 | /** 156 | * @brief Requests hardware and firmware version information 157 | * 158 | * This function will block and wait for up to timeout_ms (default 10msec) for ODrive to reply 159 | */ 160 | bool getVersion(Get_Version_msg_t& msg, uint16_t timeout_ms = 10); 161 | 162 | /** 163 | * @brief Requests encoder feedback data. 164 | * 165 | * This function will block and wait for up to timeout_ms (default 10msec) for ODrive to reply 166 | */ 167 | bool getFeedback(Get_Encoder_Estimates_msg_t& msg, uint16_t timeout_ms = 10); 168 | 169 | /** 170 | * @brief Requests ODrive DC bus voltage and current 171 | * 172 | * This function will block and wait for up to timeout_ms (default 10msec) for ODrive to reply 173 | * May trigger onBusVI callback if it's registered. 174 | */ 175 | bool getBusVI(Get_Bus_Voltage_Current_msg_t& msg, uint16_t timeout_ms = 10); 176 | 177 | /** 178 | * @brief Requests mechanical and electrical power data (used for spinout detection) 179 | * 180 | * This function will block and wait for up to timeout_ms (default 10msec) for ODrive to reply 181 | */ 182 | bool getPower(Get_Powers_msg_t& msg, uint16_t timeout_ms = 10); 183 | 184 | enum ResetAction : uint8_t { 185 | Reboot = 0, 186 | SaveConfiguration = 1, 187 | EraseConfiguration = 2, 188 | }; 189 | 190 | /** 191 | * @brief Resets the ODrive with the given action 192 | * 193 | * Valid actions: 194 | * - Reboot (0) 195 | * - Save (1) 196 | * - Erase (2) 197 | * 198 | */ 199 | bool reset(ResetAction action = ResetAction::Reboot); 200 | 201 | /** 202 | * @brief Registers a callback for ODrive feedback processing. 203 | */ 204 | void onFeedback( 205 | void (*callback)(Get_Encoder_Estimates_msg_t& feedback, void* user_data), 206 | void* user_data = nullptr 207 | ) { 208 | feedback_callback_ = callback; 209 | feedback_user_data_ = user_data; 210 | } 211 | 212 | /** 213 | * @brief Registers a callback for ODrive axis state feedback. 214 | */ 215 | void onStatus(void (*callback)(Heartbeat_msg_t& feedback, void* user_data), void* user_data = nullptr) { 216 | axis_state_callback_ = callback; 217 | axis_state_user_data_ = user_data; 218 | } 219 | 220 | /** 221 | * @brief Registers a callback for ODrive torques feedback processing. 222 | */ 223 | void onTorques(void (*callback)(Get_Torques_msg_t& feedback, void* user_data), void* user_data = nullptr) { 224 | torques_callback_ = callback; 225 | torques_user_data_ = user_data; 226 | } 227 | 228 | /** 229 | * @brief Registers a callback for ODrive temperature feedback. 230 | */ 231 | void onTemperature(void (*callback)(Get_Temperature_msg_t& feedback, void* user_data), void* user_data = nullptr) { 232 | temperature_callback_ = callback; 233 | temperature_user_data_ = user_data; 234 | } 235 | 236 | /** 237 | * @brief Registers a callback for ODrive bus voltage/current feedback. 238 | */ 239 | void onBusVI( 240 | void (*callback)(Get_Bus_Voltage_Current_msg_t& feedback, void* user_data), 241 | void* user_data = nullptr 242 | ) { 243 | busVI_callback_ = callback; 244 | busVI_user_data_ = user_data; 245 | } 246 | 247 | /** 248 | * @brief Registers a callback for ODrive currents feedback. 249 | */ 250 | void onCurrents(void (*callback)(Get_Iq_msg_t& feedback, void* user_data), void* user_data = nullptr) { 251 | currents_callback_ = callback; 252 | currents_user_data_ = user_data; 253 | } 254 | 255 | /** 256 | * @brief Registers a callback for ODrive error messages. 257 | */ 258 | void onError(void (*callback)(Get_Error_msg_t& msg, void* user_data), void* user_data = nullptr) { 259 | error_callback_ = callback; 260 | error_user_data_ = user_data; 261 | } 262 | 263 | /** 264 | * @brief Processes received CAN messages for the ODrive. 265 | */ 266 | void onReceive(uint32_t id, uint8_t length, const uint8_t* data); 267 | 268 | /** 269 | * @brief Sends a request message and awaits a response. 270 | * 271 | * Blocks until the response is received or the timeout is reached. Returns 272 | * false if the ODrive does not respond within the specified timeout. 273 | */ 274 | template 275 | bool request(T& msg, uint16_t timeout_ms = 10) { 276 | requested_msg_id_ = msg.cmd_id; 277 | can_intf_.sendMsg( 278 | (node_id_ << ODriveCAN::kNodeIdShift) | msg.cmd_id, 279 | 0, // no data 280 | nullptr // RTR=1 281 | ); 282 | if (!awaitMsg(timeout_ms)) { 283 | return false; 284 | } 285 | msg.decode_buf(buffer_); 286 | return true; 287 | } 288 | 289 | /** 290 | * @brief Sends a specified message over CAN. 291 | */ 292 | template 293 | bool send(const T& msg) { 294 | uint8_t data[8] = {}; 295 | msg.encode_buf(data); 296 | return can_intf_.sendMsg((node_id_ << ODriveCAN::kNodeIdShift) | msg.cmd_id, msg.msg_length, data); 297 | } 298 | 299 | /** 300 | * @brief Get value at the endpoint 301 | * 302 | * @tparam T The data type expected from the endpoint 303 | * @param endpoint_id Unique ID from flat_endpoints.json 304 | * @param timeout_ms Time to wait for a response from ODrive 305 | * 306 | * @return T Data from the endpoint, or 0 on timeout 307 | * 308 | * Blocks until the response is received or the timeout is reached. 309 | * 310 | */ 311 | template 312 | T getEndpoint(uint16_t endpoint_id, uint16_t timeout_ms = 10) { 313 | uint8_t data[8] = {}; 314 | data[0] = 0; // Opcode read 315 | 316 | // Little-endian endpoint 317 | data[1] = (uint8_t)(endpoint_id); 318 | data[2] = (uint8_t)(endpoint_id >> 8); 319 | 320 | requested_msg_id_ = 0x005; // Await TxSdo message 321 | can_intf_.sendMsg((node_id_ << ODriveCAN::kNodeIdShift) | 0x004, 8, data); 322 | if (!awaitMsg(timeout_ms)) { 323 | return T{}; 324 | } 325 | 326 | T ret{}; 327 | memcpy(&ret, &buffer_[4], sizeof(T)); 328 | return ret; 329 | } 330 | 331 | /** 332 | * @brief Set endpoint to value 333 | * 334 | * @tparam T Type of the value from flat_endpoints.json 335 | * @param endpoint_id Unique ID of endpoint from flat_endpoints.json 336 | * @param value value to write to the endpoint 337 | * 338 | * This function returns immediately and does not check if the ODrive 339 | * received the CAN message. 340 | */ 341 | template 342 | bool setEndpoint(uint16_t endpoint_id, T value) { 343 | uint8_t data[8] = {}; 344 | data[0] = 1; // Opcode write 345 | 346 | // Endpoint 347 | data[1] = endpoint_id & 0xFF; 348 | data[2] = (endpoint_id >> 8) & 0xFF; 349 | 350 | // Value to write 351 | memcpy(&data[4], &value, sizeof(T)); 352 | 353 | can_intf_.sendMsg((node_id_ << ODriveCAN::kNodeIdShift) | 0x004, 8, data); 354 | return true; 355 | } 356 | 357 | private: 358 | bool awaitMsg(uint16_t timeout_ms); 359 | 360 | ODriveCanIntfWrapper can_intf_; 361 | uint32_t node_id_; 362 | 363 | volatile uint8_t requested_msg_id_ = REQUEST_PENDING; 364 | 365 | uint8_t buffer_[8]; 366 | 367 | static const uint8_t kNodeIdShift = 5; 368 | static const uint8_t kCmdIdBits = 0x1F; 369 | 370 | // TODO: More scalable / simplified callback handling. 371 | // See https://github.com/odriverobotics/ODriveArduino/pull/15#issuecomment-2826941741 372 | void* axis_state_user_data_; 373 | void* feedback_user_data_; 374 | void* torques_user_data_; 375 | void* temperature_user_data_; 376 | void* busVI_user_data_; 377 | void* currents_user_data_; 378 | void* error_user_data_; 379 | 380 | void (*axis_state_callback_)(Heartbeat_msg_t& feedback, void* user_data) = nullptr; 381 | void (*feedback_callback_)(Get_Encoder_Estimates_msg_t& feedback, void* user_data) = nullptr; 382 | void (*torques_callback_)(Get_Torques_msg_t& feedback, void* user_data) = nullptr; 383 | void (*temperature_callback_)(Get_Temperature_msg_t& feedback, void* user_data) = nullptr; 384 | void (*busVI_callback_)(Get_Bus_Voltage_Current_msg_t& feedback, void* user_data) = nullptr; 385 | void (*currents_callback_)(Get_Iq_msg_t& feedback, void* user_data) = nullptr; 386 | void (*error_callback_)(Get_Error_msg_t& msg, void* user_data) = nullptr; 387 | }; 388 | -------------------------------------------------------------------------------- /src/ODriveEnums.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef ODriveEnums_h 3 | #define ODriveEnums_h 4 | 5 | // ODrive.GpioMode 6 | enum ODriveGpioMode { 7 | GPIO_MODE_DIGITAL = 0, 8 | GPIO_MODE_DIGITAL_PULL_UP = 1, 9 | GPIO_MODE_DIGITAL_PULL_DOWN = 2, 10 | GPIO_MODE_ANALOG_IN = 3, 11 | GPIO_MODE_UART_A = 4, 12 | GPIO_MODE_UART_B = 5, 13 | GPIO_MODE_UART_C = 6, 14 | GPIO_MODE_CAN_A = 7, 15 | GPIO_MODE_I2C_A = 8, 16 | GPIO_MODE_SPI_A = 9, 17 | GPIO_MODE_PWM = 10, 18 | GPIO_MODE_ENC0 = 11, 19 | GPIO_MODE_ENC1 = 12, 20 | GPIO_MODE_ENC2 = 13, 21 | GPIO_MODE_MECH_BRAKE = 14, 22 | GPIO_MODE_STATUS = 15, 23 | GPIO_MODE_BRAKE_RES = 16, 24 | GPIO_MODE_AUTO = 17, 25 | }; 26 | 27 | // ODrive.StreamProtocolType 28 | enum ODriveStreamProtocolType { 29 | STREAM_PROTOCOL_TYPE_FIBRE = 0, 30 | STREAM_PROTOCOL_TYPE_ASCII = 1, 31 | STREAM_PROTOCOL_TYPE_STDOUT = 2, 32 | STREAM_PROTOCOL_TYPE_ASCII_AND_STDOUT = 3, 33 | STREAM_PROTOCOL_TYPE_OTHER = 4, 34 | }; 35 | 36 | // ODrive.Can.Protocol 37 | enum ODriveProtocol { 38 | PROTOCOL_NONE = 0x00000000, 39 | PROTOCOL_SIMPLE = 0x00000001, 40 | }; 41 | 42 | // ODrive.Axis.AxisState 43 | enum ODriveAxisState { 44 | AXIS_STATE_UNDEFINED = 0, 45 | AXIS_STATE_IDLE = 1, 46 | AXIS_STATE_STARTUP_SEQUENCE = 2, 47 | AXIS_STATE_FULL_CALIBRATION_SEQUENCE = 3, 48 | AXIS_STATE_MOTOR_CALIBRATION = 4, 49 | AXIS_STATE_ENCODER_INDEX_SEARCH = 6, 50 | AXIS_STATE_ENCODER_OFFSET_CALIBRATION = 7, 51 | AXIS_STATE_CLOSED_LOOP_CONTROL = 8, 52 | AXIS_STATE_LOCKIN_SPIN = 9, 53 | AXIS_STATE_ENCODER_DIR_FIND = 10, 54 | AXIS_STATE_HOMING = 11, 55 | AXIS_STATE_ENCODER_HALL_POLARITY_CALIBRATION = 12, 56 | AXIS_STATE_ENCODER_HALL_PHASE_CALIBRATION = 13, 57 | AXIS_STATE_ANTICOGGING_CALIBRATION = 14, 58 | AXIS_STATE_HARMONIC_CALIBRATION = 15, 59 | AXIS_STATE_HARMONIC_CALIBRATION_COMMUTATION = 16, 60 | }; 61 | 62 | // ODrive.Controller.ControlMode 63 | enum ODriveControlMode { 64 | CONTROL_MODE_VOLTAGE_CONTROL = 0, 65 | CONTROL_MODE_TORQUE_CONTROL = 1, 66 | CONTROL_MODE_VELOCITY_CONTROL = 2, 67 | CONTROL_MODE_POSITION_CONTROL = 3, 68 | }; 69 | 70 | // ODrive.ComponentStatus 71 | enum ODriveComponentStatus { 72 | COMPONENT_STATUS_NOMINAL = 0, 73 | COMPONENT_STATUS_NO_RESPONSE = 1, 74 | COMPONENT_STATUS_INVALID_RESPONSE_LENGTH = 2, 75 | COMPONENT_STATUS_PARITY_MISMATCH = 3, 76 | COMPONENT_STATUS_ILLEGAL_HALL_STATE = 4, 77 | COMPONENT_STATUS_POLARITY_NOT_CALIBRATED = 5, 78 | COMPONENT_STATUS_PHASES_NOT_CALIBRATED = 6, 79 | COMPONENT_STATUS_NUMERICAL_ERROR = 7, 80 | COMPONENT_STATUS_MISSING_INPUT = 8, 81 | COMPONENT_STATUS_RELATIVE_MODE = 9, 82 | COMPONENT_STATUS_UNCONFIGURED = 10, 83 | COMPONENT_STATUS_OVERSPEED = 11, 84 | COMPONENT_STATUS_INDEX_NOT_FOUND = 12, 85 | COMPONENT_STATUS_BAD_CONFIG = 13, 86 | COMPONENT_STATUS_NOT_ENABLED = 14, 87 | COMPONENT_STATUS_SPINOUT_DETECTED = 15, 88 | }; 89 | 90 | // ODrive.Error 91 | enum ODriveError { 92 | ODRIVE_ERROR_NONE = 0x00000000, 93 | ODRIVE_ERROR_INITIALIZING = 0x00000001, 94 | ODRIVE_ERROR_SYSTEM_LEVEL = 0x00000002, 95 | ODRIVE_ERROR_TIMING_ERROR = 0x00000004, 96 | ODRIVE_ERROR_MISSING_ESTIMATE = 0x00000008, 97 | ODRIVE_ERROR_BAD_CONFIG = 0x00000010, 98 | ODRIVE_ERROR_DRV_FAULT = 0x00000020, 99 | ODRIVE_ERROR_MISSING_INPUT = 0x00000040, 100 | ODRIVE_ERROR_DC_BUS_OVER_VOLTAGE = 0x00000100, 101 | ODRIVE_ERROR_DC_BUS_UNDER_VOLTAGE = 0x00000200, 102 | ODRIVE_ERROR_DC_BUS_OVER_CURRENT = 0x00000400, 103 | ODRIVE_ERROR_DC_BUS_OVER_REGEN_CURRENT = 0x00000800, 104 | ODRIVE_ERROR_CURRENT_LIMIT_VIOLATION = 0x00001000, 105 | ODRIVE_ERROR_MOTOR_OVER_TEMP = 0x00002000, 106 | ODRIVE_ERROR_INVERTER_OVER_TEMP = 0x00004000, 107 | ODRIVE_ERROR_VELOCITY_LIMIT_VIOLATION = 0x00008000, 108 | ODRIVE_ERROR_POSITION_LIMIT_VIOLATION = 0x00010000, 109 | ODRIVE_ERROR_WATCHDOG_TIMER_EXPIRED = 0x01000000, 110 | ODRIVE_ERROR_ESTOP_REQUESTED = 0x02000000, 111 | ODRIVE_ERROR_SPINOUT_DETECTED = 0x04000000, 112 | ODRIVE_ERROR_BRAKE_RESISTOR_DISARMED = 0x08000000, 113 | ODRIVE_ERROR_THERMISTOR_DISCONNECTED = 0x10000000, 114 | ODRIVE_ERROR_CALIBRATION_ERROR = 0x40000000, 115 | }; 116 | 117 | // ODrive.ProcedureResult 118 | enum ODriveProcedureResult { 119 | PROCEDURE_RESULT_SUCCESS = 0, 120 | PROCEDURE_RESULT_BUSY = 1, 121 | PROCEDURE_RESULT_CANCELLED = 2, 122 | PROCEDURE_RESULT_DISARMED = 3, 123 | PROCEDURE_RESULT_NO_RESPONSE = 4, 124 | PROCEDURE_RESULT_POLE_PAIR_CPR_MISMATCH = 5, 125 | PROCEDURE_RESULT_PHASE_RESISTANCE_OUT_OF_RANGE = 6, 126 | PROCEDURE_RESULT_PHASE_INDUCTANCE_OUT_OF_RANGE = 7, 127 | PROCEDURE_RESULT_UNBALANCED_PHASES = 8, 128 | PROCEDURE_RESULT_INVALID_MOTOR_TYPE = 9, 129 | PROCEDURE_RESULT_ILLEGAL_HALL_STATE = 10, 130 | PROCEDURE_RESULT_TIMEOUT = 11, 131 | PROCEDURE_RESULT_HOMING_WITHOUT_ENDSTOP = 12, 132 | PROCEDURE_RESULT_INVALID_STATE = 13, 133 | PROCEDURE_RESULT_NOT_CALIBRATED = 14, 134 | PROCEDURE_RESULT_NOT_CONVERGING = 15, 135 | }; 136 | 137 | // ODrive.EncoderId 138 | enum ODriveEncoderId { 139 | ENCODER_ID_NONE = 0, 140 | ENCODER_ID_INC_ENCODER0 = 1, 141 | ENCODER_ID_INC_ENCODER1 = 2, 142 | ENCODER_ID_INC_ENCODER2 = 3, 143 | ENCODER_ID_SENSORLESS_ESTIMATOR = 4, 144 | ENCODER_ID_SPI_ENCODER0 = 5, 145 | ENCODER_ID_SPI_ENCODER1 = 6, 146 | ENCODER_ID_SPI_ENCODER2 = 7, 147 | ENCODER_ID_HALL_ENCODER0 = 8, 148 | ENCODER_ID_HALL_ENCODER1 = 9, 149 | ENCODER_ID_RS485_ENCODER0 = 10, 150 | ENCODER_ID_RS485_ENCODER1 = 11, 151 | ENCODER_ID_RS485_ENCODER2 = 12, 152 | ENCODER_ID_ONBOARD_ENCODER0 = 13, 153 | ENCODER_ID_ONBOARD_ENCODER1 = 14, 154 | }; 155 | 156 | // ODrive.SpiEncoderMode 157 | enum ODriveSpiEncoderMode { 158 | SPI_ENCODER_MODE_DISABLED = 0, 159 | SPI_ENCODER_MODE_RLS = 1, 160 | SPI_ENCODER_MODE_AMS = 2, 161 | SPI_ENCODER_MODE_CUI = 3, 162 | SPI_ENCODER_MODE_AEAT = 4, 163 | SPI_ENCODER_MODE_MA732 = 5, 164 | SPI_ENCODER_MODE_TLE = 6, 165 | SPI_ENCODER_MODE_BISSC = 7, 166 | SPI_ENCODER_MODE_NOVOHALL = 8, 167 | }; 168 | 169 | // ODrive.IncrementalEncoderFilter 170 | enum ODriveIncrementalEncoderFilter { 171 | INCREMENTAL_ENCODER_FILTER_SPEED_10M = 0, 172 | INCREMENTAL_ENCODER_FILTER_SPEED_20M = 1, 173 | }; 174 | 175 | // ODrive.Rs485EncoderMode 176 | enum ODriveRs485EncoderMode { 177 | RS485_ENCODER_MODE_DISABLED = 0, 178 | RS485_ENCODER_MODE_AMT21_POLLING = 1, 179 | RS485_ENCODER_MODE_AMT21_EVENT_DRIVEN = 2, 180 | RS485_ENCODER_MODE_MBS = 3, 181 | RS485_ENCODER_MODE_ODRIVE_OA1 = 4, 182 | }; 183 | 184 | // ODrive.Controller.InputMode 185 | enum ODriveInputMode { 186 | INPUT_MODE_INACTIVE = 0, 187 | INPUT_MODE_PASSTHROUGH = 1, 188 | INPUT_MODE_VEL_RAMP = 2, 189 | INPUT_MODE_POS_FILTER = 3, 190 | INPUT_MODE_MIX_CHANNELS = 4, 191 | INPUT_MODE_TRAP_TRAJ = 5, 192 | INPUT_MODE_TORQUE_RAMP = 6, 193 | INPUT_MODE_MIRROR = 7, 194 | INPUT_MODE_TUNING = 8, 195 | }; 196 | 197 | // ODrive.MotorType 198 | enum ODriveMotorType { 199 | MOTOR_TYPE_PMSM_CURRENT_CONTROL = 0, 200 | MOTOR_TYPE_PMSM_VOLTAGE_CONTROL = 2, 201 | MOTOR_TYPE_ACIM = 3, 202 | }; 203 | 204 | // ODrive.ThermistorMode 205 | enum ODriveThermistorMode { 206 | THERMISTOR_MODE_NTC = 1, 207 | THERMISTOR_MODE_QUADRATIC = 2, 208 | THERMISTOR_MODE_PT1000 = 3, 209 | THERMISTOR_MODE_KTY84 = 4, 210 | THERMISTOR_MODE_KTY83_122 = 5, 211 | }; 212 | 213 | // ODrive.Can.Error 214 | enum ODriveCanError { 215 | CAN_ERROR_NONE = 0x00000000, 216 | CAN_ERROR_DUPLICATE_CAN_IDS = 0x00000001, 217 | CAN_ERROR_BUS_OFF = 0x00000002, 218 | CAN_ERROR_LOW_LEVEL = 0x00000004, 219 | CAN_ERROR_PROTOCOL_INIT = 0x00000008, 220 | }; 221 | 222 | #endif 223 | -------------------------------------------------------------------------------- /src/ODriveFlexCAN.hpp: -------------------------------------------------------------------------------- 1 | // Glue layer for FlexCAN-based CAN stacks. 2 | // See ODriveHardwareCAN.hpp for documentation. 3 | 4 | #pragma once 5 | 6 | #include "FlexCAN_T4.h" 7 | #include "ODriveCAN.h" 8 | 9 | using CanMsg = CAN_message_t; 10 | 11 | static bool sendMsg(FlexCAN_T4_Base& can_intf, uint32_t id, uint8_t length, const uint8_t* data) { 12 | CAN_message_t teensy_msg = { 13 | .id = id & 0x1fffffff, 14 | .flags = {.extended = (bool)(id & 0x80000000), .remote = !data}, 15 | .len = length, 16 | }; 17 | 18 | if (data) { 19 | memcpy(teensy_msg.buf, data, length); 20 | } 21 | 22 | can_intf.events(); // TODO: is this really needed? 23 | 24 | return (can_intf.write(teensy_msg) > 0); 25 | } 26 | 27 | void onReceive(const CAN_message_t& msg, ODriveCAN& odrive) { 28 | odrive.onReceive(msg.id | (msg.flags.extended ? 0x80000000 : 0), msg.len, msg.buf); 29 | } 30 | 31 | void pumpEvents(FlexCAN_T4_Base& can_intf) { 32 | can_intf.events(); 33 | } 34 | 35 | CREATE_CAN_INTF_WRAPPER(FlexCAN_T4_Base) 36 | -------------------------------------------------------------------------------- /src/ODriveHardwareCAN.hpp: -------------------------------------------------------------------------------- 1 | // Glue layer for platforms that implement the Arduino's official HardwareCAN API. 2 | 3 | #pragma once 4 | 5 | #include "ODriveCAN.h" 6 | 7 | #include 8 | 9 | // Must be defined by the application 10 | void onCanMessage(const CanMsg& msg); 11 | 12 | /** 13 | * @brief Sends a CAN message over the specified platform-specific interface. 14 | * 15 | * @param can_intf A platform-specific reference to the CAN interface to use. 16 | * @param id: The CAN message ID to send. 17 | * Bit 31 indicates if the ID is extended (29-bit) or standard (11-bit). 18 | * Bits 30 and 29 are reserved. 19 | * @param length: The length of the data in bytes (0-8). For RTR messages, this 20 | * should be 0. 21 | * @param data: A pointer to the data to send. If null, a remote transmission 22 | * request (RTR=1) is sent, if supported by the interface. 23 | * @return: True if the message was sent successfully, false otherwise. 24 | */ 25 | static bool sendMsg(HardwareCAN& can_intf, uint32_t id, uint8_t length, const uint8_t* data) { 26 | // Note: Arduino_CAN does not support the RTR bit. The ODrive interprets 27 | // zero-length packets the same as RTR=1, but it creates the possibility of 28 | // collisions. 29 | CanMsg msg{ 30 | (id & 0x80000000) ? CanExtendedId(id) : CanStandardId(id), 31 | length, 32 | data, 33 | }; 34 | return can_intf.write(msg) >= 0; 35 | } 36 | 37 | /** 38 | * @brief Receives a CAN message from the platform-specific interface and passes 39 | * it to the ODriveCAN instance. 40 | * 41 | * @param msg: The received CAN message in a platform-specific format. 42 | * @param odrive: The ODriveCAN instance to pass the message to. 43 | */ 44 | static void onReceive(const CanMsg& msg, ODriveCAN& odrive) { 45 | odrive.onReceive(msg.id, msg.data_length, msg.data); 46 | } 47 | 48 | /** 49 | * @brief Processes the CAN interface's RX buffer and calls onCanMessage for 50 | * each pending message. 51 | * 52 | * On hardware interfaces where onCanMessage() is already called from the 53 | * interrupt handler, this function is a no-op. 54 | * 55 | * @param intf: The platform-specific CAN interface to process. 56 | * @param max_events: The maximum number of events to process. This prevents 57 | * an infinite loop if messages come at a high rate. 58 | */ 59 | static void pumpEvents(HardwareCAN& intf, int max_events = 100) { 60 | // max_events prevents an infinite loop if messages come at a high rate 61 | while (intf.available() && max_events--) { 62 | onCanMessage(intf.read()); 63 | } 64 | } 65 | 66 | CREATE_CAN_INTF_WRAPPER(HardwareCAN) 67 | -------------------------------------------------------------------------------- /src/ODriveMCPCAN.hpp: -------------------------------------------------------------------------------- 1 | // Glue layer for MCP2515-based CAN interfaces. 2 | // See ODriveHardwareCAN.hpp for documentation. 3 | 4 | #pragma once 5 | 6 | #include "MCP2515.h" 7 | #include "ODriveCAN.h" 8 | 9 | // This is a convenience struct because the MCP2515 library doesn't have a 10 | // native message type. 11 | struct CanMsg { 12 | uint32_t id; 13 | uint8_t len; 14 | uint8_t buffer[8]; 15 | }; 16 | 17 | // Must be defined by the application if you want to use defaultCanReceiveCallback(). 18 | void onCanMessage(const CanMsg& msg); 19 | 20 | // MSB of id means "extended" 21 | // if data is null, it's a remote request frame 22 | static bool sendMsg(MCP2515Class& can_intf, uint32_t id, uint8_t length, const uint8_t* data) { 23 | if (id & 0x80000000) { 24 | can_intf.beginExtendedPacket(id & 0x1fffffff, length, !data); 25 | } else { 26 | can_intf.beginPacket(id, length, !data); 27 | } 28 | if (data) { 29 | for (int i = 0; i < length; ++i) { 30 | can_intf.write(data[i]); 31 | } 32 | } 33 | return can_intf.endPacket(); 34 | } 35 | 36 | static void onReceive(const CanMsg& msg, ODriveCAN& odrive) { 37 | odrive.onReceive(msg.id, msg.len, msg.buffer); 38 | } 39 | 40 | static void pumpEvents(MCP2515Class& intf) { 41 | // On other platforms, this polls and processes incoming CAN messages. 42 | // However, this is not possible on MCP2515-based platforms. 43 | // 44 | // A 10ms delay was found to reduce the number of dropped messages, however a 45 | // specific root cause has not been identified, and may be a quirk in the MCP2515. 46 | delay(10); 47 | } 48 | 49 | CREATE_CAN_INTF_WRAPPER(MCP2515Class) 50 | -------------------------------------------------------------------------------- /src/ODriveSTM32CAN.hpp: -------------------------------------------------------------------------------- 1 | // CAN glue layer for STM32 platforms. 2 | // See ODriveHardwareCAN.hpp for documentation. 3 | 4 | #pragma once 5 | 6 | #include "ODriveCAN.h" 7 | 8 | #include 9 | 10 | using CanMsg = CAN_message_t; 11 | 12 | // Must be defined by the application 13 | void onCanMessage(const CanMsg& msg); 14 | 15 | static bool sendMsg(STM32_CAN& can_intf, uint32_t id, uint8_t length, const uint8_t* data) { 16 | CanMsg msg; 17 | msg.id = id & 0x1ffffff; 18 | msg.flags.extended = id & 0x80000000; 19 | msg.flags.remote = (data == nullptr); 20 | msg.len = length; 21 | if (data) { 22 | for (int i = 0; i < length; ++i) { 23 | msg.buf[i] = data[i]; 24 | } 25 | } 26 | return can_intf.write(msg) >= 0; 27 | } 28 | 29 | static void onReceive(const CanMsg& msg, ODriveCAN& odrive) { 30 | odrive.onReceive(msg.id, msg.len, msg.buf); 31 | } 32 | 33 | static void pumpEvents(STM32_CAN& intf, int max_events = 100) { 34 | // max_events prevents an infinite loop if messages come at a high rate 35 | CanMsg msg; 36 | while (intf.read(msg) && max_events--) { 37 | onCanMessage(msg); 38 | } 39 | } 40 | 41 | CREATE_CAN_INTF_WRAPPER(STM32_CAN) 42 | -------------------------------------------------------------------------------- /src/ODriveUART.cpp: -------------------------------------------------------------------------------- 1 | // Author: ODrive Robotics Inc. 2 | // License: MIT 3 | // Documentation: https://docs.odriverobotics.com/v/latest/guides/arduino-uart-guide.html 4 | 5 | #include "ODriveUART.h" 6 | 7 | #include "Arduino.h" 8 | 9 | static const int kMotorNumber = 0; 10 | 11 | // Print with stream operator 12 | // clang-format off 13 | template inline Print& operator <<(Print &obj, T arg) { obj.print(arg); return obj; } 14 | template<> inline Print& operator <<(Print &obj, float arg) { obj.print(arg, 4); return obj; } 15 | // clang-format on 16 | 17 | ODriveUART::ODriveUART(Stream& serial) : serial_(serial) {} 18 | 19 | void ODriveUART::clearErrors() { 20 | serial_ << F("sc\n"); 21 | } 22 | 23 | void ODriveUART::setPosition(float position) { 24 | setPosition(position, 0.0f, 0.0f); 25 | } 26 | 27 | void ODriveUART::setPosition(float position, float velocity_feedforward) { 28 | setPosition(position, velocity_feedforward, 0.0f); 29 | } 30 | 31 | void ODriveUART::setPosition(float position, float velocity_feedforward, float torque_feedforward) { 32 | serial_ << F("p ") << kMotorNumber << F(" ") << position << F(" ") << velocity_feedforward << F(" ") 33 | << torque_feedforward << F("\n"); 34 | } 35 | 36 | void ODriveUART::setVelocity(float velocity) { 37 | setVelocity(velocity, 0.0f); 38 | } 39 | 40 | void ODriveUART::setVelocity(float velocity, float torque_feedforward) { 41 | serial_ << F("v ") << kMotorNumber << F(" ") << velocity << F(" ") << torque_feedforward << F("\n"); 42 | } 43 | 44 | void ODriveUART::setTorque(float torque) { 45 | serial_ << F("c ") << kMotorNumber << F(" ") << torque << F("\n"); 46 | } 47 | 48 | void ODriveUART::trapezoidalMove(float position) { 49 | serial_ << F("t ") << kMotorNumber << F(" ") << position << F("\n"); 50 | } 51 | 52 | ODriveFeedback ODriveUART::getFeedback() { 53 | // Flush RX 54 | while (serial_.available()) { 55 | serial_.read(); 56 | } 57 | 58 | serial_ << F("f ") << kMotorNumber << F("\n"); 59 | 60 | String response = readLine(); 61 | 62 | int spacePos = response.indexOf(' '); 63 | if (spacePos >= 0) { 64 | return { 65 | response.substring(0, spacePos).toFloat(), 66 | response.substring(spacePos + 1).toFloat(), 67 | }; 68 | } else { 69 | return {0.0f, 0.0f}; 70 | } 71 | } 72 | 73 | String ODriveUART::getParameterAsString(const String& path) { 74 | serial_ << F("r ") << path << F("\n"); 75 | return readLine(); 76 | } 77 | 78 | void ODriveUART::setParameter(const String& path, const String& value) { 79 | serial_ << F("w ") << path << F(" ") << value << F("\n"); 80 | } 81 | 82 | void ODriveUART::setState(ODriveAxisState requested_state) { 83 | setParameter(F("axis0.requested_state"), String((long)requested_state)); 84 | } 85 | 86 | ODriveAxisState ODriveUART::getState() { 87 | return (ODriveAxisState)getParameterAsInt(F("axis0.current_state")); 88 | } 89 | 90 | String ODriveUART::readLine(unsigned long timeout_ms) { 91 | String str = ""; 92 | unsigned long timeout_start = millis(); 93 | for (;;) { 94 | while (!serial_.available()) { 95 | if (millis() - timeout_start >= timeout_ms) { 96 | return str; 97 | } 98 | } 99 | char c = serial_.read(); 100 | if (c == '\n') 101 | break; 102 | str += c; 103 | } 104 | return str; 105 | } 106 | -------------------------------------------------------------------------------- /src/ODriveUART.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef ODriveUART_h 3 | #define ODriveUART_h 4 | 5 | #include "Arduino.h" 6 | #include "ODriveEnums.h" 7 | 8 | struct ODriveFeedback { 9 | float pos; 10 | float vel; 11 | }; 12 | 13 | class ODriveUART { 14 | public: 15 | /** 16 | * @brief Constructs an ODriveUART instance that will communicate over 17 | * the specified serial port. 18 | */ 19 | ODriveUART(Stream& serial); 20 | 21 | /** 22 | * @brief Clears the error status of the ODrive and restarts the brake 23 | * resistor if it was disabled due to an error. 24 | */ 25 | void clearErrors(); 26 | 27 | /** 28 | * @brief Sends a new position setpoint. 29 | */ 30 | void setPosition(float position); 31 | 32 | /** 33 | * @brief Sends a new position setpoint with a velocity feedforward term. 34 | */ 35 | void setPosition(float position, float velocity_feedforward); 36 | 37 | /** 38 | * @brief Sends a new position setpoint with velocity and torque feedforward terms. 39 | */ 40 | void setPosition(float position, float velocity_feedforward, float torque_feedforward); 41 | 42 | /** 43 | * @brief Sends a new velocity setpoint. 44 | */ 45 | void setVelocity(float velocity); 46 | 47 | /** 48 | * @brief Sends a new velocity setpoint with a torque feedforward term. 49 | */ 50 | void setVelocity(float velocity, float torque_feedforward); 51 | 52 | /** 53 | * @brief Sends a new torque setpoint. 54 | */ 55 | void setTorque(float torque); 56 | 57 | /** 58 | * @brief Puts the ODrive into trapezoidal trajectory mode and sends a new 59 | * position setpoint. 60 | */ 61 | void trapezoidalMove(float position); 62 | 63 | /** 64 | * @brief Requests the latest position and velocity estimates. 65 | * 66 | * Returns pos = 0.0 and vel = 0.0 in case of a communication error. 67 | */ 68 | ODriveFeedback getFeedback(); 69 | 70 | /** 71 | * @brief Requests the latest position estimate. 72 | * 73 | * Returns 0.0 in case of a communication error. 74 | */ 75 | float getPosition() { return getFeedback().pos; } 76 | 77 | /** 78 | * @brief Requests the latest velocity estimate. 79 | * 80 | * Returns 0.0 in case of a communication error. 81 | */ 82 | float getVelocity() { return getFeedback().vel; } 83 | 84 | // Generic parameter access 85 | String getParameterAsString(const String& path); 86 | long getParameterAsInt(const String& path) { return getParameterAsString(path).toInt(); } 87 | float getParameterAsFloat(const String& path) { return getParameterAsString(path).toFloat(); } 88 | void setParameter(const String& path, const String& value); 89 | void setParameter(const String& path, long value) { setParameter(path, String(value)); } 90 | 91 | /** 92 | * @brief Tells the ODrive to change its axis state. 93 | */ 94 | void setState(ODriveAxisState requested_state); 95 | 96 | /** 97 | * @brief Requests the current axis state from the ODrive. 98 | * 99 | * Returns AXIS_STATE_UNDEFINED in case of a communication error. 100 | */ 101 | ODriveAxisState getState(); 102 | 103 | private: 104 | String readLine(unsigned long timeout_ms = 10); 105 | 106 | Stream& serial_; 107 | }; 108 | 109 | #endif // ODriveUART_h 110 | -------------------------------------------------------------------------------- /src/can_helpers.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | 7 | template 8 | T can_get_signal_raw(const uint8_t* buf, const size_t startBit, const size_t length, const bool isIntel) { 9 | constexpr int N = 8; 10 | 11 | const uint64_t mask = length < 64 ? (1ULL << length) - 1ULL : -1ULL; 12 | const uint8_t shift = isIntel ? startBit : (64 - startBit) - length; 13 | 14 | uint64_t tempVal = 0; 15 | memcpy(&tempVal, buf, N); 16 | if (isIntel) { 17 | tempVal = (tempVal >> shift) & mask; 18 | } else { 19 | tempVal = __builtin_bswap64(tempVal); 20 | tempVal = (tempVal >> shift) & mask; 21 | } 22 | 23 | T retVal; 24 | memcpy(&retVal, &tempVal, sizeof(T)); 25 | 26 | return retVal; 27 | } 28 | 29 | template 30 | void can_set_signal_raw(uint8_t* buf, const T val, const size_t startBit, const size_t length, const bool isIntel) { 31 | constexpr int N = 8; 32 | 33 | const uint64_t mask = length < 64 ? (1ULL << length) - 1ULL : -1ULL; 34 | const uint8_t shift = isIntel ? startBit : (64 - startBit) - length; 35 | 36 | uint64_t valAsBits = 0; 37 | memcpy(&valAsBits, &val, sizeof(T)); 38 | 39 | uint64_t data = 0; 40 | memcpy(&data, buf, N); 41 | if (isIntel) { 42 | data &= ~(mask << shift); 43 | data |= valAsBits << shift; 44 | } else { 45 | data = __builtin_bswap64(data); 46 | data &= ~(mask << shift); 47 | data |= valAsBits << shift; 48 | data = __builtin_bswap64(data); 49 | } 50 | 51 | memcpy(buf, &data, N); 52 | } 53 | 54 | template 55 | float can_get_signal_raw( 56 | const uint8_t* buf, 57 | const size_t startBit, 58 | const size_t length, 59 | const bool isIntel, 60 | const float factor, 61 | const float offset 62 | ) { 63 | T retVal = can_get_signal_raw(buf, startBit, length, isIntel); 64 | return (retVal * factor) + offset; 65 | } 66 | 67 | template 68 | constexpr void can_set_signal_raw( 69 | uint8_t* buf, 70 | const float val, 71 | const size_t startBit, 72 | const size_t length, 73 | const bool isIntel, 74 | const float factor, 75 | const float offset 76 | ) { 77 | T scaledVal = static_cast((val - offset) / factor); 78 | can_set_signal_raw(buf, scaledVal, startBit, length, isIntel); 79 | } 80 | 81 | template 82 | constexpr T can_get_signal(const TMsg& msg, const size_t startBit, const size_t length, const bool isIntel) { 83 | return can_get_signal_raw(can_msg_get_payload(msg).data(), startBit, length, isIntel); 84 | } 85 | 86 | template 87 | constexpr void can_set_signal(TMsg& msg, const T val, const size_t startBit, const size_t length, const bool isIntel) { 88 | can_set_signal_raw(can_msg_get_payload(msg).data(), val, startBit, length, isIntel); 89 | } 90 | -------------------------------------------------------------------------------- /src/can_simple_messages.hpp: -------------------------------------------------------------------------------- 1 | 2 | #pragma once 3 | 4 | // This file is autogenerated using generate_can_messages.py from the DBC definitions in can_dbc.py 5 | 6 | #include 7 | 8 | struct Get_Version_msg_t final { 9 | constexpr Get_Version_msg_t() = default; 10 | 11 | #ifdef ODRIVE_CAN_MSG_TYPE 12 | Get_Version_msg_t(const TBoard::TCanIntf::TMsg& msg) { 13 | decode_msg(msg); 14 | } 15 | 16 | void encode_msg(TBoard::TCanIntf::TMsg& msg) { 17 | encode_buf(can_msg_get_payload(msg).data()); 18 | } 19 | 20 | void decode_msg(const TBoard::TCanIntf::TMsg& msg) { 21 | decode_buf(can_msg_get_payload(msg).data()); 22 | } 23 | #endif 24 | 25 | void encode_buf(uint8_t* buf) const { 26 | can_set_signal_raw(buf, Protocol_Version, 0, 8, true); 27 | can_set_signal_raw(buf, Hw_Version_Major, 8, 8, true); 28 | can_set_signal_raw(buf, Hw_Version_Minor, 16, 8, true); 29 | can_set_signal_raw(buf, Hw_Version_Variant, 24, 8, true); 30 | can_set_signal_raw(buf, Fw_Version_Major, 32, 8, true); 31 | can_set_signal_raw(buf, Fw_Version_Minor, 40, 8, true); 32 | can_set_signal_raw(buf, Fw_Version_Revision, 48, 8, true); 33 | can_set_signal_raw(buf, Fw_Version_Unreleased, 56, 8, true); 34 | } 35 | 36 | void decode_buf(const uint8_t* buf) { 37 | Protocol_Version = can_get_signal_raw(buf, 0, 8, true); 38 | Hw_Version_Major = can_get_signal_raw(buf, 8, 8, true); 39 | Hw_Version_Minor = can_get_signal_raw(buf, 16, 8, true); 40 | Hw_Version_Variant = can_get_signal_raw(buf, 24, 8, true); 41 | Fw_Version_Major = can_get_signal_raw(buf, 32, 8, true); 42 | Fw_Version_Minor = can_get_signal_raw(buf, 40, 8, true); 43 | Fw_Version_Revision = can_get_signal_raw(buf, 48, 8, true); 44 | Fw_Version_Unreleased = can_get_signal_raw(buf, 56, 8, true); 45 | } 46 | 47 | static const uint8_t cmd_id = 0x000; 48 | static const uint8_t msg_length = 8; 49 | 50 | uint8_t Protocol_Version = 0; 51 | uint8_t Hw_Version_Major = 0; 52 | uint8_t Hw_Version_Minor = 0; 53 | uint8_t Hw_Version_Variant = 0; 54 | uint8_t Fw_Version_Major = 0; 55 | uint8_t Fw_Version_Minor = 0; 56 | uint8_t Fw_Version_Revision = 0; 57 | uint8_t Fw_Version_Unreleased = 0; 58 | }; 59 | 60 | struct Heartbeat_msg_t final { 61 | constexpr Heartbeat_msg_t() = default; 62 | 63 | #ifdef ODRIVE_CAN_MSG_TYPE 64 | Heartbeat_msg_t(const TBoard::TCanIntf::TMsg& msg) { 65 | decode_msg(msg); 66 | } 67 | 68 | void encode_msg(TBoard::TCanIntf::TMsg& msg) { 69 | encode_buf(can_msg_get_payload(msg).data()); 70 | } 71 | 72 | void decode_msg(const TBoard::TCanIntf::TMsg& msg) { 73 | decode_buf(can_msg_get_payload(msg).data()); 74 | } 75 | #endif 76 | 77 | void encode_buf(uint8_t* buf) const { 78 | can_set_signal_raw(buf, Axis_Error, 0, 32, true); 79 | can_set_signal_raw(buf, Axis_State, 32, 8, true); 80 | can_set_signal_raw(buf, Procedure_Result, 40, 8, true); 81 | can_set_signal_raw(buf, Trajectory_Done_Flag, 48, 1, true); 82 | } 83 | 84 | void decode_buf(const uint8_t* buf) { 85 | Axis_Error = can_get_signal_raw(buf, 0, 32, true); 86 | Axis_State = can_get_signal_raw(buf, 32, 8, true); 87 | Procedure_Result = can_get_signal_raw(buf, 40, 8, true); 88 | Trajectory_Done_Flag = can_get_signal_raw(buf, 48, 1, true); 89 | } 90 | 91 | static const uint8_t cmd_id = 0x001; 92 | static const uint8_t msg_length = 8; 93 | 94 | uint32_t Axis_Error = 0; 95 | uint8_t Axis_State = 0; 96 | uint8_t Procedure_Result = 0; 97 | uint8_t Trajectory_Done_Flag = 0; 98 | }; 99 | 100 | struct Estop_msg_t final { 101 | constexpr Estop_msg_t() = default; 102 | 103 | #ifdef ODRIVE_CAN_MSG_TYPE 104 | Estop_msg_t(const TBoard::TCanIntf::TMsg& msg) { 105 | decode_msg(msg); 106 | } 107 | 108 | void encode_msg(TBoard::TCanIntf::TMsg& msg) { 109 | encode_buf(can_msg_get_payload(msg).data()); 110 | } 111 | 112 | void decode_msg(const TBoard::TCanIntf::TMsg& msg) { 113 | decode_buf(can_msg_get_payload(msg).data()); 114 | } 115 | #endif 116 | 117 | void encode_buf(uint8_t* buf) const { 118 | } 119 | 120 | void decode_buf(const uint8_t* buf) { 121 | } 122 | 123 | static const uint8_t cmd_id = 0x002; 124 | static const uint8_t msg_length = 0; 125 | 126 | }; 127 | 128 | struct Get_Error_msg_t final { 129 | constexpr Get_Error_msg_t() = default; 130 | 131 | #ifdef ODRIVE_CAN_MSG_TYPE 132 | Get_Error_msg_t(const TBoard::TCanIntf::TMsg& msg) { 133 | decode_msg(msg); 134 | } 135 | 136 | void encode_msg(TBoard::TCanIntf::TMsg& msg) { 137 | encode_buf(can_msg_get_payload(msg).data()); 138 | } 139 | 140 | void decode_msg(const TBoard::TCanIntf::TMsg& msg) { 141 | decode_buf(can_msg_get_payload(msg).data()); 142 | } 143 | #endif 144 | 145 | void encode_buf(uint8_t* buf) const { 146 | can_set_signal_raw(buf, Active_Errors, 0, 32, true); 147 | can_set_signal_raw(buf, Disarm_Reason, 32, 32, true); 148 | } 149 | 150 | void decode_buf(const uint8_t* buf) { 151 | Active_Errors = can_get_signal_raw(buf, 0, 32, true); 152 | Disarm_Reason = can_get_signal_raw(buf, 32, 32, true); 153 | } 154 | 155 | static const uint8_t cmd_id = 0x003; 156 | static const uint8_t msg_length = 8; 157 | 158 | uint32_t Active_Errors = 0; 159 | uint32_t Disarm_Reason = 0; 160 | }; 161 | 162 | struct Address_msg_t final { 163 | constexpr Address_msg_t() = default; 164 | 165 | #ifdef ODRIVE_CAN_MSG_TYPE 166 | Address_msg_t(const TBoard::TCanIntf::TMsg& msg) { 167 | decode_msg(msg); 168 | } 169 | 170 | void encode_msg(TBoard::TCanIntf::TMsg& msg) { 171 | encode_buf(can_msg_get_payload(msg).data()); 172 | } 173 | 174 | void decode_msg(const TBoard::TCanIntf::TMsg& msg) { 175 | decode_buf(can_msg_get_payload(msg).data()); 176 | } 177 | #endif 178 | 179 | void encode_buf(uint8_t* buf) const { 180 | can_set_signal_raw(buf, Node_ID, 0, 8, true); 181 | can_set_signal_raw(buf, Serial_Number, 8, 48, true); 182 | can_set_signal_raw(buf, Connection_ID, 56, 8, true); 183 | } 184 | 185 | void decode_buf(const uint8_t* buf) { 186 | Node_ID = can_get_signal_raw(buf, 0, 8, true); 187 | Serial_Number = can_get_signal_raw(buf, 8, 48, true); 188 | Connection_ID = can_get_signal_raw(buf, 56, 8, true); 189 | } 190 | 191 | static const uint8_t cmd_id = 0x006; 192 | static const uint8_t msg_length = 8; 193 | 194 | uint8_t Node_ID = 0; 195 | uint64_t Serial_Number = 0; 196 | uint8_t Connection_ID = 0; 197 | }; 198 | 199 | struct Set_Axis_State_msg_t final { 200 | constexpr Set_Axis_State_msg_t() = default; 201 | 202 | #ifdef ODRIVE_CAN_MSG_TYPE 203 | Set_Axis_State_msg_t(const TBoard::TCanIntf::TMsg& msg) { 204 | decode_msg(msg); 205 | } 206 | 207 | void encode_msg(TBoard::TCanIntf::TMsg& msg) { 208 | encode_buf(can_msg_get_payload(msg).data()); 209 | } 210 | 211 | void decode_msg(const TBoard::TCanIntf::TMsg& msg) { 212 | decode_buf(can_msg_get_payload(msg).data()); 213 | } 214 | #endif 215 | 216 | void encode_buf(uint8_t* buf) const { 217 | can_set_signal_raw(buf, Axis_Requested_State, 0, 32, true); 218 | } 219 | 220 | void decode_buf(const uint8_t* buf) { 221 | Axis_Requested_State = can_get_signal_raw(buf, 0, 32, true); 222 | } 223 | 224 | static const uint8_t cmd_id = 0x007; 225 | static const uint8_t msg_length = 8; 226 | 227 | uint32_t Axis_Requested_State = 0; 228 | }; 229 | 230 | struct Get_Encoder_Estimates_msg_t final { 231 | constexpr Get_Encoder_Estimates_msg_t() = default; 232 | 233 | #ifdef ODRIVE_CAN_MSG_TYPE 234 | Get_Encoder_Estimates_msg_t(const TBoard::TCanIntf::TMsg& msg) { 235 | decode_msg(msg); 236 | } 237 | 238 | void encode_msg(TBoard::TCanIntf::TMsg& msg) { 239 | encode_buf(can_msg_get_payload(msg).data()); 240 | } 241 | 242 | void decode_msg(const TBoard::TCanIntf::TMsg& msg) { 243 | decode_buf(can_msg_get_payload(msg).data()); 244 | } 245 | #endif 246 | 247 | void encode_buf(uint8_t* buf) const { 248 | can_set_signal_raw(buf, Pos_Estimate, 0, 32, true); 249 | can_set_signal_raw(buf, Vel_Estimate, 32, 32, true); 250 | } 251 | 252 | void decode_buf(const uint8_t* buf) { 253 | Pos_Estimate = can_get_signal_raw(buf, 0, 32, true); 254 | Vel_Estimate = can_get_signal_raw(buf, 32, 32, true); 255 | } 256 | 257 | static const uint8_t cmd_id = 0x009; 258 | static const uint8_t msg_length = 8; 259 | 260 | float Pos_Estimate = 0.0f; // [rev] 261 | float Vel_Estimate = 0.0f; // [rev/s] 262 | }; 263 | 264 | struct Set_Controller_Mode_msg_t final { 265 | constexpr Set_Controller_Mode_msg_t() = default; 266 | 267 | #ifdef ODRIVE_CAN_MSG_TYPE 268 | Set_Controller_Mode_msg_t(const TBoard::TCanIntf::TMsg& msg) { 269 | decode_msg(msg); 270 | } 271 | 272 | void encode_msg(TBoard::TCanIntf::TMsg& msg) { 273 | encode_buf(can_msg_get_payload(msg).data()); 274 | } 275 | 276 | void decode_msg(const TBoard::TCanIntf::TMsg& msg) { 277 | decode_buf(can_msg_get_payload(msg).data()); 278 | } 279 | #endif 280 | 281 | void encode_buf(uint8_t* buf) const { 282 | can_set_signal_raw(buf, Control_Mode, 0, 32, true); 283 | can_set_signal_raw(buf, Input_Mode, 32, 32, true); 284 | } 285 | 286 | void decode_buf(const uint8_t* buf) { 287 | Control_Mode = can_get_signal_raw(buf, 0, 32, true); 288 | Input_Mode = can_get_signal_raw(buf, 32, 32, true); 289 | } 290 | 291 | static const uint8_t cmd_id = 0x00B; 292 | static const uint8_t msg_length = 8; 293 | 294 | uint32_t Control_Mode = 0; 295 | uint32_t Input_Mode = 0; 296 | }; 297 | 298 | struct Set_Input_Pos_msg_t final { 299 | constexpr Set_Input_Pos_msg_t() = default; 300 | 301 | #ifdef ODRIVE_CAN_MSG_TYPE 302 | Set_Input_Pos_msg_t(const TBoard::TCanIntf::TMsg& msg) { 303 | decode_msg(msg); 304 | } 305 | 306 | void encode_msg(TBoard::TCanIntf::TMsg& msg) { 307 | encode_buf(can_msg_get_payload(msg).data()); 308 | } 309 | 310 | void decode_msg(const TBoard::TCanIntf::TMsg& msg) { 311 | decode_buf(can_msg_get_payload(msg).data()); 312 | } 313 | #endif 314 | 315 | void encode_buf(uint8_t* buf) const { 316 | can_set_signal_raw(buf, Input_Pos, 0, 32, true); 317 | can_set_signal_raw(buf, Vel_FF, 32, 16, true, 0.001f, 0.0f); 318 | can_set_signal_raw(buf, Torque_FF, 48, 16, true, 0.001f, 0.0f); 319 | } 320 | 321 | void decode_buf(const uint8_t* buf) { 322 | Input_Pos = can_get_signal_raw(buf, 0, 32, true); 323 | Vel_FF = can_get_signal_raw(buf, 32, 16, true, 0.001f, 0.0f); 324 | Torque_FF = can_get_signal_raw(buf, 48, 16, true, 0.001f, 0.0f); 325 | } 326 | 327 | static const uint8_t cmd_id = 0x00C; 328 | static const uint8_t msg_length = 8; 329 | 330 | float Input_Pos = 0.0f; // [rev] 331 | float Vel_FF = 0.0f; // [rev/s (default) [#vel-ff-scale]_] 332 | float Torque_FF = 0.0f; // [Nm (default) [#torque-ff-scale]_] 333 | }; 334 | 335 | struct Set_Input_Vel_msg_t final { 336 | constexpr Set_Input_Vel_msg_t() = default; 337 | 338 | #ifdef ODRIVE_CAN_MSG_TYPE 339 | Set_Input_Vel_msg_t(const TBoard::TCanIntf::TMsg& msg) { 340 | decode_msg(msg); 341 | } 342 | 343 | void encode_msg(TBoard::TCanIntf::TMsg& msg) { 344 | encode_buf(can_msg_get_payload(msg).data()); 345 | } 346 | 347 | void decode_msg(const TBoard::TCanIntf::TMsg& msg) { 348 | decode_buf(can_msg_get_payload(msg).data()); 349 | } 350 | #endif 351 | 352 | void encode_buf(uint8_t* buf) const { 353 | can_set_signal_raw(buf, Input_Vel, 0, 32, true); 354 | can_set_signal_raw(buf, Input_Torque_FF, 32, 32, true); 355 | } 356 | 357 | void decode_buf(const uint8_t* buf) { 358 | Input_Vel = can_get_signal_raw(buf, 0, 32, true); 359 | Input_Torque_FF = can_get_signal_raw(buf, 32, 32, true); 360 | } 361 | 362 | static const uint8_t cmd_id = 0x00D; 363 | static const uint8_t msg_length = 8; 364 | 365 | float Input_Vel = 0.0f; // [rev/s] 366 | float Input_Torque_FF = 0.0f; // [Nm] 367 | }; 368 | 369 | struct Set_Input_Torque_msg_t final { 370 | constexpr Set_Input_Torque_msg_t() = default; 371 | 372 | #ifdef ODRIVE_CAN_MSG_TYPE 373 | Set_Input_Torque_msg_t(const TBoard::TCanIntf::TMsg& msg) { 374 | decode_msg(msg); 375 | } 376 | 377 | void encode_msg(TBoard::TCanIntf::TMsg& msg) { 378 | encode_buf(can_msg_get_payload(msg).data()); 379 | } 380 | 381 | void decode_msg(const TBoard::TCanIntf::TMsg& msg) { 382 | decode_buf(can_msg_get_payload(msg).data()); 383 | } 384 | #endif 385 | 386 | void encode_buf(uint8_t* buf) const { 387 | can_set_signal_raw(buf, Input_Torque, 0, 32, true); 388 | } 389 | 390 | void decode_buf(const uint8_t* buf) { 391 | Input_Torque = can_get_signal_raw(buf, 0, 32, true); 392 | } 393 | 394 | static const uint8_t cmd_id = 0x00E; 395 | static const uint8_t msg_length = 8; 396 | 397 | float Input_Torque = 0.0f; // [Nm] 398 | }; 399 | 400 | struct Set_Limits_msg_t final { 401 | constexpr Set_Limits_msg_t() = default; 402 | 403 | #ifdef ODRIVE_CAN_MSG_TYPE 404 | Set_Limits_msg_t(const TBoard::TCanIntf::TMsg& msg) { 405 | decode_msg(msg); 406 | } 407 | 408 | void encode_msg(TBoard::TCanIntf::TMsg& msg) { 409 | encode_buf(can_msg_get_payload(msg).data()); 410 | } 411 | 412 | void decode_msg(const TBoard::TCanIntf::TMsg& msg) { 413 | decode_buf(can_msg_get_payload(msg).data()); 414 | } 415 | #endif 416 | 417 | void encode_buf(uint8_t* buf) const { 418 | can_set_signal_raw(buf, Velocity_Limit, 0, 32, true); 419 | can_set_signal_raw(buf, Current_Limit, 32, 32, true); 420 | } 421 | 422 | void decode_buf(const uint8_t* buf) { 423 | Velocity_Limit = can_get_signal_raw(buf, 0, 32, true); 424 | Current_Limit = can_get_signal_raw(buf, 32, 32, true); 425 | } 426 | 427 | static const uint8_t cmd_id = 0x00F; 428 | static const uint8_t msg_length = 8; 429 | 430 | float Velocity_Limit = 0.0f; // [rev/s] 431 | float Current_Limit = 0.0f; // [A] 432 | }; 433 | 434 | struct Set_Traj_Vel_Limit_msg_t final { 435 | constexpr Set_Traj_Vel_Limit_msg_t() = default; 436 | 437 | #ifdef ODRIVE_CAN_MSG_TYPE 438 | Set_Traj_Vel_Limit_msg_t(const TBoard::TCanIntf::TMsg& msg) { 439 | decode_msg(msg); 440 | } 441 | 442 | void encode_msg(TBoard::TCanIntf::TMsg& msg) { 443 | encode_buf(can_msg_get_payload(msg).data()); 444 | } 445 | 446 | void decode_msg(const TBoard::TCanIntf::TMsg& msg) { 447 | decode_buf(can_msg_get_payload(msg).data()); 448 | } 449 | #endif 450 | 451 | void encode_buf(uint8_t* buf) const { 452 | can_set_signal_raw(buf, Traj_Vel_Limit, 0, 32, true); 453 | } 454 | 455 | void decode_buf(const uint8_t* buf) { 456 | Traj_Vel_Limit = can_get_signal_raw(buf, 0, 32, true); 457 | } 458 | 459 | static const uint8_t cmd_id = 0x011; 460 | static const uint8_t msg_length = 8; 461 | 462 | float Traj_Vel_Limit = 0.0f; // [rev/s] 463 | }; 464 | 465 | struct Set_Traj_Accel_Limits_msg_t final { 466 | constexpr Set_Traj_Accel_Limits_msg_t() = default; 467 | 468 | #ifdef ODRIVE_CAN_MSG_TYPE 469 | Set_Traj_Accel_Limits_msg_t(const TBoard::TCanIntf::TMsg& msg) { 470 | decode_msg(msg); 471 | } 472 | 473 | void encode_msg(TBoard::TCanIntf::TMsg& msg) { 474 | encode_buf(can_msg_get_payload(msg).data()); 475 | } 476 | 477 | void decode_msg(const TBoard::TCanIntf::TMsg& msg) { 478 | decode_buf(can_msg_get_payload(msg).data()); 479 | } 480 | #endif 481 | 482 | void encode_buf(uint8_t* buf) const { 483 | can_set_signal_raw(buf, Traj_Accel_Limit, 0, 32, true); 484 | can_set_signal_raw(buf, Traj_Decel_Limit, 32, 32, true); 485 | } 486 | 487 | void decode_buf(const uint8_t* buf) { 488 | Traj_Accel_Limit = can_get_signal_raw(buf, 0, 32, true); 489 | Traj_Decel_Limit = can_get_signal_raw(buf, 32, 32, true); 490 | } 491 | 492 | static const uint8_t cmd_id = 0x012; 493 | static const uint8_t msg_length = 8; 494 | 495 | float Traj_Accel_Limit = 0.0f; // [rev/s^2] 496 | float Traj_Decel_Limit = 0.0f; // [rev/s^2] 497 | }; 498 | 499 | struct Set_Traj_Inertia_msg_t final { 500 | constexpr Set_Traj_Inertia_msg_t() = default; 501 | 502 | #ifdef ODRIVE_CAN_MSG_TYPE 503 | Set_Traj_Inertia_msg_t(const TBoard::TCanIntf::TMsg& msg) { 504 | decode_msg(msg); 505 | } 506 | 507 | void encode_msg(TBoard::TCanIntf::TMsg& msg) { 508 | encode_buf(can_msg_get_payload(msg).data()); 509 | } 510 | 511 | void decode_msg(const TBoard::TCanIntf::TMsg& msg) { 512 | decode_buf(can_msg_get_payload(msg).data()); 513 | } 514 | #endif 515 | 516 | void encode_buf(uint8_t* buf) const { 517 | can_set_signal_raw(buf, Traj_Inertia, 0, 32, true); 518 | } 519 | 520 | void decode_buf(const uint8_t* buf) { 521 | Traj_Inertia = can_get_signal_raw(buf, 0, 32, true); 522 | } 523 | 524 | static const uint8_t cmd_id = 0x013; 525 | static const uint8_t msg_length = 8; 526 | 527 | float Traj_Inertia = 0.0f; // [Nm/(rev/s^2)] 528 | }; 529 | 530 | struct Get_Iq_msg_t final { 531 | constexpr Get_Iq_msg_t() = default; 532 | 533 | #ifdef ODRIVE_CAN_MSG_TYPE 534 | Get_Iq_msg_t(const TBoard::TCanIntf::TMsg& msg) { 535 | decode_msg(msg); 536 | } 537 | 538 | void encode_msg(TBoard::TCanIntf::TMsg& msg) { 539 | encode_buf(can_msg_get_payload(msg).data()); 540 | } 541 | 542 | void decode_msg(const TBoard::TCanIntf::TMsg& msg) { 543 | decode_buf(can_msg_get_payload(msg).data()); 544 | } 545 | #endif 546 | 547 | void encode_buf(uint8_t* buf) const { 548 | can_set_signal_raw(buf, Iq_Setpoint, 0, 32, true); 549 | can_set_signal_raw(buf, Iq_Measured, 32, 32, true); 550 | } 551 | 552 | void decode_buf(const uint8_t* buf) { 553 | Iq_Setpoint = can_get_signal_raw(buf, 0, 32, true); 554 | Iq_Measured = can_get_signal_raw(buf, 32, 32, true); 555 | } 556 | 557 | static const uint8_t cmd_id = 0x014; 558 | static const uint8_t msg_length = 8; 559 | 560 | float Iq_Setpoint = 0.0f; // [A] 561 | float Iq_Measured = 0.0f; // [A] 562 | }; 563 | 564 | struct Get_Temperature_msg_t final { 565 | constexpr Get_Temperature_msg_t() = default; 566 | 567 | #ifdef ODRIVE_CAN_MSG_TYPE 568 | Get_Temperature_msg_t(const TBoard::TCanIntf::TMsg& msg) { 569 | decode_msg(msg); 570 | } 571 | 572 | void encode_msg(TBoard::TCanIntf::TMsg& msg) { 573 | encode_buf(can_msg_get_payload(msg).data()); 574 | } 575 | 576 | void decode_msg(const TBoard::TCanIntf::TMsg& msg) { 577 | decode_buf(can_msg_get_payload(msg).data()); 578 | } 579 | #endif 580 | 581 | void encode_buf(uint8_t* buf) const { 582 | can_set_signal_raw(buf, FET_Temperature, 0, 32, true); 583 | can_set_signal_raw(buf, Motor_Temperature, 32, 32, true); 584 | } 585 | 586 | void decode_buf(const uint8_t* buf) { 587 | FET_Temperature = can_get_signal_raw(buf, 0, 32, true); 588 | Motor_Temperature = can_get_signal_raw(buf, 32, 32, true); 589 | } 590 | 591 | static const uint8_t cmd_id = 0x015; 592 | static const uint8_t msg_length = 8; 593 | 594 | float FET_Temperature = 0.0f; // [deg C] 595 | float Motor_Temperature = 0.0f; // [deg C] 596 | }; 597 | 598 | struct Reboot_msg_t final { 599 | constexpr Reboot_msg_t() = default; 600 | 601 | #ifdef ODRIVE_CAN_MSG_TYPE 602 | Reboot_msg_t(const TBoard::TCanIntf::TMsg& msg) { 603 | decode_msg(msg); 604 | } 605 | 606 | void encode_msg(TBoard::TCanIntf::TMsg& msg) { 607 | encode_buf(can_msg_get_payload(msg).data()); 608 | } 609 | 610 | void decode_msg(const TBoard::TCanIntf::TMsg& msg) { 611 | decode_buf(can_msg_get_payload(msg).data()); 612 | } 613 | #endif 614 | 615 | void encode_buf(uint8_t* buf) const { 616 | can_set_signal_raw(buf, Action, 0, 8, true); 617 | } 618 | 619 | void decode_buf(const uint8_t* buf) { 620 | Action = can_get_signal_raw(buf, 0, 8, true); 621 | } 622 | 623 | static const uint8_t cmd_id = 0x016; 624 | static const uint8_t msg_length = 1; 625 | 626 | uint8_t Action = 0; 627 | }; 628 | 629 | struct Get_Bus_Voltage_Current_msg_t final { 630 | constexpr Get_Bus_Voltage_Current_msg_t() = default; 631 | 632 | #ifdef ODRIVE_CAN_MSG_TYPE 633 | Get_Bus_Voltage_Current_msg_t(const TBoard::TCanIntf::TMsg& msg) { 634 | decode_msg(msg); 635 | } 636 | 637 | void encode_msg(TBoard::TCanIntf::TMsg& msg) { 638 | encode_buf(can_msg_get_payload(msg).data()); 639 | } 640 | 641 | void decode_msg(const TBoard::TCanIntf::TMsg& msg) { 642 | decode_buf(can_msg_get_payload(msg).data()); 643 | } 644 | #endif 645 | 646 | void encode_buf(uint8_t* buf) const { 647 | can_set_signal_raw(buf, Bus_Voltage, 0, 32, true); 648 | can_set_signal_raw(buf, Bus_Current, 32, 32, true); 649 | } 650 | 651 | void decode_buf(const uint8_t* buf) { 652 | Bus_Voltage = can_get_signal_raw(buf, 0, 32, true); 653 | Bus_Current = can_get_signal_raw(buf, 32, 32, true); 654 | } 655 | 656 | static const uint8_t cmd_id = 0x017; 657 | static const uint8_t msg_length = 8; 658 | 659 | float Bus_Voltage = 0.0f; // [V] 660 | float Bus_Current = 0.0f; // [A] 661 | }; 662 | 663 | struct Clear_Errors_msg_t final { 664 | constexpr Clear_Errors_msg_t() = default; 665 | 666 | #ifdef ODRIVE_CAN_MSG_TYPE 667 | Clear_Errors_msg_t(const TBoard::TCanIntf::TMsg& msg) { 668 | decode_msg(msg); 669 | } 670 | 671 | void encode_msg(TBoard::TCanIntf::TMsg& msg) { 672 | encode_buf(can_msg_get_payload(msg).data()); 673 | } 674 | 675 | void decode_msg(const TBoard::TCanIntf::TMsg& msg) { 676 | decode_buf(can_msg_get_payload(msg).data()); 677 | } 678 | #endif 679 | 680 | void encode_buf(uint8_t* buf) const { 681 | can_set_signal_raw(buf, Identify, 0, 8, true); 682 | } 683 | 684 | void decode_buf(const uint8_t* buf) { 685 | Identify = can_get_signal_raw(buf, 0, 8, true); 686 | } 687 | 688 | static const uint8_t cmd_id = 0x018; 689 | static const uint8_t msg_length = 1; 690 | 691 | uint8_t Identify = 0; 692 | }; 693 | 694 | struct Set_Absolute_Position_msg_t final { 695 | constexpr Set_Absolute_Position_msg_t() = default; 696 | 697 | #ifdef ODRIVE_CAN_MSG_TYPE 698 | Set_Absolute_Position_msg_t(const TBoard::TCanIntf::TMsg& msg) { 699 | decode_msg(msg); 700 | } 701 | 702 | void encode_msg(TBoard::TCanIntf::TMsg& msg) { 703 | encode_buf(can_msg_get_payload(msg).data()); 704 | } 705 | 706 | void decode_msg(const TBoard::TCanIntf::TMsg& msg) { 707 | decode_buf(can_msg_get_payload(msg).data()); 708 | } 709 | #endif 710 | 711 | void encode_buf(uint8_t* buf) const { 712 | can_set_signal_raw(buf, Position, 0, 32, true); 713 | } 714 | 715 | void decode_buf(const uint8_t* buf) { 716 | Position = can_get_signal_raw(buf, 0, 32, true); 717 | } 718 | 719 | static const uint8_t cmd_id = 0x019; 720 | static const uint8_t msg_length = 8; 721 | 722 | float Position = 0.0f; // [rev] 723 | }; 724 | 725 | struct Set_Pos_Gain_msg_t final { 726 | constexpr Set_Pos_Gain_msg_t() = default; 727 | 728 | #ifdef ODRIVE_CAN_MSG_TYPE 729 | Set_Pos_Gain_msg_t(const TBoard::TCanIntf::TMsg& msg) { 730 | decode_msg(msg); 731 | } 732 | 733 | void encode_msg(TBoard::TCanIntf::TMsg& msg) { 734 | encode_buf(can_msg_get_payload(msg).data()); 735 | } 736 | 737 | void decode_msg(const TBoard::TCanIntf::TMsg& msg) { 738 | decode_buf(can_msg_get_payload(msg).data()); 739 | } 740 | #endif 741 | 742 | void encode_buf(uint8_t* buf) const { 743 | can_set_signal_raw(buf, Pos_Gain, 0, 32, true); 744 | } 745 | 746 | void decode_buf(const uint8_t* buf) { 747 | Pos_Gain = can_get_signal_raw(buf, 0, 32, true); 748 | } 749 | 750 | static const uint8_t cmd_id = 0x01A; 751 | static const uint8_t msg_length = 8; 752 | 753 | float Pos_Gain = 0.0f; // [(rev/s) / rev] 754 | }; 755 | 756 | struct Set_Vel_Gains_msg_t final { 757 | constexpr Set_Vel_Gains_msg_t() = default; 758 | 759 | #ifdef ODRIVE_CAN_MSG_TYPE 760 | Set_Vel_Gains_msg_t(const TBoard::TCanIntf::TMsg& msg) { 761 | decode_msg(msg); 762 | } 763 | 764 | void encode_msg(TBoard::TCanIntf::TMsg& msg) { 765 | encode_buf(can_msg_get_payload(msg).data()); 766 | } 767 | 768 | void decode_msg(const TBoard::TCanIntf::TMsg& msg) { 769 | decode_buf(can_msg_get_payload(msg).data()); 770 | } 771 | #endif 772 | 773 | void encode_buf(uint8_t* buf) const { 774 | can_set_signal_raw(buf, Vel_Gain, 0, 32, true); 775 | can_set_signal_raw(buf, Vel_Integrator_Gain, 32, 32, true); 776 | } 777 | 778 | void decode_buf(const uint8_t* buf) { 779 | Vel_Gain = can_get_signal_raw(buf, 0, 32, true); 780 | Vel_Integrator_Gain = can_get_signal_raw(buf, 32, 32, true); 781 | } 782 | 783 | static const uint8_t cmd_id = 0x01B; 784 | static const uint8_t msg_length = 8; 785 | 786 | float Vel_Gain = 0.0f; // [Nm / (rev/s)] 787 | float Vel_Integrator_Gain = 0.0f; // [Nm / rev] 788 | }; 789 | 790 | struct Get_Torques_msg_t final { 791 | constexpr Get_Torques_msg_t() = default; 792 | 793 | #ifdef ODRIVE_CAN_MSG_TYPE 794 | Get_Torques_msg_t(const TBoard::TCanIntf::TMsg& msg) { 795 | decode_msg(msg); 796 | } 797 | 798 | void encode_msg(TBoard::TCanIntf::TMsg& msg) { 799 | encode_buf(can_msg_get_payload(msg).data()); 800 | } 801 | 802 | void decode_msg(const TBoard::TCanIntf::TMsg& msg) { 803 | decode_buf(can_msg_get_payload(msg).data()); 804 | } 805 | #endif 806 | 807 | void encode_buf(uint8_t* buf) const { 808 | can_set_signal_raw(buf, Torque_Target, 0, 32, true); 809 | can_set_signal_raw(buf, Torque_Estimate, 32, 32, true); 810 | } 811 | 812 | void decode_buf(const uint8_t* buf) { 813 | Torque_Target = can_get_signal_raw(buf, 0, 32, true); 814 | Torque_Estimate = can_get_signal_raw(buf, 32, 32, true); 815 | } 816 | 817 | static const uint8_t cmd_id = 0x01C; 818 | static const uint8_t msg_length = 8; 819 | 820 | float Torque_Target = 0.0f; // [Nm] 821 | float Torque_Estimate = 0.0f; // [Nm] 822 | }; 823 | 824 | struct Get_Powers_msg_t final { 825 | constexpr Get_Powers_msg_t() = default; 826 | 827 | #ifdef ODRIVE_CAN_MSG_TYPE 828 | Get_Powers_msg_t(const TBoard::TCanIntf::TMsg& msg) { 829 | decode_msg(msg); 830 | } 831 | 832 | void encode_msg(TBoard::TCanIntf::TMsg& msg) { 833 | encode_buf(can_msg_get_payload(msg).data()); 834 | } 835 | 836 | void decode_msg(const TBoard::TCanIntf::TMsg& msg) { 837 | decode_buf(can_msg_get_payload(msg).data()); 838 | } 839 | #endif 840 | 841 | void encode_buf(uint8_t* buf) const { 842 | can_set_signal_raw(buf, Electrical_Power, 0, 32, true); 843 | can_set_signal_raw(buf, Mechanical_Power, 32, 32, true); 844 | } 845 | 846 | void decode_buf(const uint8_t* buf) { 847 | Electrical_Power = can_get_signal_raw(buf, 0, 32, true); 848 | Mechanical_Power = can_get_signal_raw(buf, 32, 32, true); 849 | } 850 | 851 | static const uint8_t cmd_id = 0x01D; 852 | static const uint8_t msg_length = 8; 853 | 854 | float Electrical_Power = 0.0f; // [W] 855 | float Mechanical_Power = 0.0f; // [W] 856 | }; 857 | 858 | struct Enter_DFU_Mode_msg_t final { 859 | constexpr Enter_DFU_Mode_msg_t() = default; 860 | 861 | #ifdef ODRIVE_CAN_MSG_TYPE 862 | Enter_DFU_Mode_msg_t(const TBoard::TCanIntf::TMsg& msg) { 863 | decode_msg(msg); 864 | } 865 | 866 | void encode_msg(TBoard::TCanIntf::TMsg& msg) { 867 | encode_buf(can_msg_get_payload(msg).data()); 868 | } 869 | 870 | void decode_msg(const TBoard::TCanIntf::TMsg& msg) { 871 | decode_buf(can_msg_get_payload(msg).data()); 872 | } 873 | #endif 874 | 875 | void encode_buf(uint8_t* buf) const { 876 | } 877 | 878 | void decode_buf(const uint8_t* buf) { 879 | } 880 | 881 | static const uint8_t cmd_id = 0x01F; 882 | static const uint8_t msg_length = 0; 883 | 884 | }; 885 | 886 | --------------------------------------------------------------------------------