Skip to content

Commit d5857b2

Browse files
committed
partial formatting pass
1 parent e1f918b commit d5857b2

File tree

6 files changed

+78
-71
lines changed

6 files changed

+78
-71
lines changed

src/ODriveCAN.h

Lines changed: 48 additions & 44 deletions
Original file line numberDiff line numberDiff line change
@@ -38,167 +38,167 @@ class ODriveCAN {
3838

3939
/**
4040
* @brief Clear all errors on the ODrive.
41-
*
41+
*
4242
* This function returns immediately and does not check if the ODrive
4343
* received the CAN message.
4444
*/
4545
bool clearErrors();
46-
46+
4747
/**
4848
* @brief Tells the ODrive to change its axis state.
49-
*
49+
*
5050
* This function returns immediately and does not check if the ODrive
5151
* received the CAN message.
5252
*/
5353
bool setState(ODriveAxisState requested_state);
54-
54+
5555
/**
5656
* @brief Sets the control mode and input mode of the ODrive.
57-
*
57+
*
5858
* This function returns immediately and does not check if the ODrive
5959
* received the CAN message.
6060
*/
6161
bool setControllerMode(uint8_t control_mode, uint8_t input_mode);
62-
62+
6363
/**
6464
* @brief Sends a position setpoint with optional velocity and torque feedforward.
65-
*
65+
*
6666
* This function returns immediately and does not check if the ODrive
6767
* received the CAN message.
6868
*/
6969
bool setPosition(float position, float velocity_feedforward = 0.0f, float torque_feedforward = 0.0f);
7070

7171
/**
7272
* @brief Sends a velocity setpoint with optional torque feedforward.
73-
*
73+
*
7474
* This function returns immediately and does not check if the ODrive
7575
* received the CAN message.
7676
*/
7777
bool setVelocity(float velocity, float torque_feedforward = 0.0f);
7878

7979
/**
8080
* @brief Sends a torque setpoint to the ODrive.
81-
*
81+
*
8282
* This function returns immediately and does not check if the ODrive
8383
* received the CAN message.
8484
*/
8585
bool setTorque(float torque);
8686

8787
/**
8888
* @brief Sets the velocity and current limits
89-
*
89+
*
9090
* This function returns immediately and does not check if the ODrive
9191
* received the CAN message.
9292
*/
9393
bool setLimits(float velocity_limit, float current_soft_max);
9494

9595
/**
9696
* @brief Sets the position gain
97-
*
97+
*
9898
* This function returns immediately and does not check if the ODrive
9999
* received the CAN message.
100100
*/
101101
bool setPosGain(float pos_gain);
102-
102+
103103
/**
104104
* @brief Sets the velocity and velocity integrator gains
105-
*
105+
*
106106
* This function returns immediately and does not check if the ODrive
107107
* received the CAN message.
108108
*/
109109
bool setVelGains(float vel_gain, float vel_integrator_gain);
110-
110+
111111
/**
112112
* @brief Sets the encoder's absolute position and enables absolute positioning
113-
*
113+
*
114114
* This function returns immediately and does not check if the ODrive
115115
* received the CAN message.
116116
*/
117117
bool setAbsolutePosition(float abs_pos);
118-
118+
119119
/**
120120
* @brief Sets the coast velocity for subsequent trapezoidal moves
121-
*
121+
*
122122
* This function returns immediately and does not check if the ODrive
123123
* received the CAN message.
124124
*/
125125
bool setTrapezoidalVelLimit(float vel_limit);
126-
126+
127127
/**
128128
* @brief Sets the acceleration and deceleration values for subsequent trapezoidal moves
129-
*
129+
*
130130
* This function returns immediately and does not check if the ODrive
131131
* received the CAN message.
132132
*/
133133
bool setTrapezoidalAccelLimits(float accel_limit, float decel_limit);
134134

135135
/**
136136
* @brief Requests motor current. Iq_measured represents torque-generating current
137-
*
137+
*
138138
* This function will block and wait for up to timeout_ms (default 10msec) for ODrive to reply
139139
*/
140140
bool getCurrents(Get_Iq_msg_t& msg, uint16_t timeout_ms = 10);
141141

142142
/**
143-
* @brief Requests motor temperature
144-
*
143+
* @brief Requests motor temperature
144+
*
145145
* This function will block and wait for up to timeout_ms (default 10msec) for ODrive to reply
146146
*/
147147
bool getTemperature(Get_Temperature_msg_t& msg, uint16_t timeout_ms = 10);
148148

149149
/**
150150
* @brief Requests error information
151-
*
151+
*
152152
* This function will block and wait for up to timeout_ms (default 10msec) for ODrive to reply
153153
*/
154154
bool getError(Get_Error_msg_t& msg, uint16_t timeout_ms = 10);
155155

156156
/**
157157
* @brief Requests hardware and firmware version information
158-
*
158+
*
159159
* This function will block and wait for up to timeout_ms (default 10msec) for ODrive to reply
160160
*/
161161
bool getVersion(Get_Version_msg_t& msg, uint16_t timeout_ms = 10);
162162

163163
/**
164164
* @brief Requests encoder feedback data.
165-
*
165+
*
166166
* This function will block and wait for up to timeout_ms (default 10msec) for ODrive to reply
167167
*/
168168
bool getFeedback(Get_Encoder_Estimates_msg_t& msg, uint16_t timeout_ms = 10);
169169

170170
/**
171171
* @brief Requests ODrive DC bus voltage and current
172-
*
172+
*
173173
* This function will block and wait for up to timeout_ms (default 10msec) for ODrive to reply
174174
* May trigger onBusVI callback if it's registered.
175175
*/
176176
bool getBusVI(Get_Bus_Voltage_Current_msg_t& msg, uint16_t timeout_ms = 10);
177177

178178
/**
179179
* @brief Requests mechanical and electrical power data (used for spinout detection)
180-
*
180+
*
181181
* This function will block and wait for up to timeout_ms (default 10msec) for ODrive to reply
182182
*/
183183
bool getPower(Get_Powers_msg_t& msg, uint16_t timeout_ms = 10);
184-
184+
185185
enum ResetAction {
186186
Reboot,
187187
SaveConfiguration,
188-
EraseConfiguration
188+
EraseConfiguration,
189189
};
190-
190+
191191
/**
192192
* @brief Resets the ODrive with the given action
193-
*
193+
*
194194
* Valid actions:
195195
* - Reboot (0)
196196
* - Save (1)
197197
* - Erase (2)
198198
*
199199
*/
200200
bool reset(ResetAction action = ResetAction::Reboot);
201-
201+
202202
/**
203203
* @brief Registers a callback for ODrive feedback processing.
204204
*/
@@ -211,15 +211,15 @@ class ODriveCAN {
211211
* @brief Registers a callback for ODrive axis state feedback.
212212
*/
213213
void onStatus(void (*callback)(Heartbeat_msg_t& feedback, void* user_data), void* user_data = nullptr) {
214-
axis_state_callback_ = callback;
214+
axis_state_callback_ = callback;
215215
axis_state_user_data_ = user_data;
216216
}
217-
217+
218218
/**
219219
* @brief Registers a callback for ODrive torques feedback processing.
220220
*/
221221
void onTorques(void (*callback)(Get_Torques_msg_t& feedback, void* user_data), void* user_data = nullptr) {
222-
torques_callback_ = callback;
222+
torques_callback_ = callback;
223223
torques_user_data_ = user_data;
224224
}
225225

@@ -262,7 +262,7 @@ class ODriveCAN {
262262

263263
/**
264264
* @brief Sends a request message and awaits a response.
265-
*
265+
*
266266
* Blocks until the response is received or the timeout is reached. Returns
267267
* false if the ODrive does not respond within the specified timeout.
268268
*/
@@ -274,7 +274,9 @@ class ODriveCAN {
274274
0, // no data
275275
nullptr // RTR=1
276276
);
277-
if (!awaitMsg(timeout_ms)) return false;
277+
if (!awaitMsg(timeout_ms)) {
278+
return false;
279+
}
278280
msg.decode_buf(buffer_);
279281
return true;
280282
}
@@ -295,17 +297,17 @@ class ODriveCAN {
295297

296298
/**
297299
* @brief Get value at the endpoint
298-
*
300+
*
299301
* @tparam T The data type expected from the endpoint
300302
* @param endpoint_id Unique ID from flat_endpoints.json
301303
* @param timeout_ms Time to wait for a response from ODrive
302-
*
304+
*
303305
* @return T Data from the endpoint, or 0 on timeout
304306
*
305307
* Blocks until the response is received or the timeout is reached.
306308
*
307309
*/
308-
template <typename T>
310+
template<typename T>
309311
T getEndpoint(uint16_t endpoint_id, uint16_t timeout_ms = 10) {
310312
uint8_t data[8] = {};
311313
data[0] = 0; // Opcode read
@@ -316,7 +318,9 @@ class ODriveCAN {
316318

317319
requested_msg_id_ = 0x005; // Await TxSdo message
318320
can_intf_.sendMsg((node_id_ << ODriveCAN::kNodeIdShift) | 0x004, 8, data);
319-
if (!awaitMsg(timeout_ms)) return T{};
321+
if (!awaitMsg(timeout_ms)) {
322+
return T{};
323+
}
320324

321325
T ret{};
322326
memcpy(&ret, &buffer_[4], sizeof(T));
@@ -325,15 +329,15 @@ class ODriveCAN {
325329

326330
/**
327331
* @brief Set endpoint to value
328-
*
332+
*
329333
* @tparam T Type of the value from flat_endpoints.json
330334
* @param endpoint_id Unique ID of endpoint from flat_endpoints.json
331335
* @param value value to write to the endpoint
332336
*
333337
* This function returns immediately and does not check if the ODrive
334338
* received the CAN message.
335339
*/
336-
template <typename T>
340+
template<typename T>
337341
bool setEndpoint(uint16_t endpoint_id, T value) {
338342
uint8_t data[8] = {};
339343
data[0] = 1; // Opcode write
@@ -371,7 +375,7 @@ class ODriveCAN {
371375
void* busVI_user_data_;
372376
void* currents_user_data_;
373377
void* error_user_data_;
374-
378+
375379
void (*axis_state_callback_)(Heartbeat_msg_t& feedback, void* user_data) = nullptr;
376380
void (*feedback_callback_)(Get_Encoder_Estimates_msg_t& feedback, void* user_data) = nullptr;
377381
void (*torques_callback_)(Get_Torques_msg_t& feedback, void* user_data) = nullptr;

src/ODriveFlexCAN.hpp

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -3,8 +3,8 @@
33

44
#pragma once
55

6-
#include "ODriveCAN.h"
76
#include "FlexCAN_T4.h"
7+
#include "ODriveCAN.h"
88

99
using CanMsg = CAN_message_t;
1010

@@ -14,7 +14,7 @@ static bool sendMsg(FlexCAN_T4_Base& can_intf, uint32_t id, uint8_t length, cons
1414
.flags = {.extended = (bool)(id & 0x80000000), .remote = !data},
1515
.len = length,
1616
};
17-
17+
1818
if (data) {
1919
memcpy(teensy_msg.buf, data, length);
2020
}

src/ODriveHardwareCAN.hpp

Lines changed: 5 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -3,14 +3,15 @@
33
#pragma once
44

55
#include "ODriveCAN.h"
6+
67
#include <api/HardwareCAN.h>
78

89
// Must be defined by the application
910
void onCanMessage(const CanMsg& msg);
1011

1112
/**
1213
* @brief Sends a CAN message over the specified platform-specific interface.
13-
*
14+
*
1415
* @param can_intf A platform-specific reference to the CAN interface to use.
1516
* @param id: The CAN message ID to send.
1617
* Bit 31 indicates if the ID is extended (29-bit) or standard (11-bit).
@@ -36,7 +37,7 @@ static bool sendMsg(HardwareCAN& can_intf, uint32_t id, uint8_t length, const ui
3637
/**
3738
* @brief Receives a CAN message from the platform-specific interface and passes
3839
* it to the ODriveCAN instance.
39-
*
40+
*
4041
* @param msg: The received CAN message in a platform-specific format.
4142
* @param odrive: The ODriveCAN instance to pass the message to.
4243
*/
@@ -47,10 +48,10 @@ static void onReceive(const CanMsg& msg, ODriveCAN& odrive) {
4748
/**
4849
* @brief Processes the CAN interface's RX buffer and calls onCanMessage for
4950
* each pending message.
50-
*
51+
*
5152
* On hardware interfaces where onCanMessage() is already called from the
5253
* interrupt handler, this function is a no-op.
53-
*
54+
*
5455
* @param intf: The platform-specific CAN interface to process.
5556
* @param max_events: The maximum number of events to process. This prevents
5657
* an infinite loop if messages come at a high rate.

src/ODriveSTM32CAN.hpp

Lines changed: 11 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -4,6 +4,7 @@
44
#pragma once
55

66
#include "ODriveCAN.h"
7+
78
#include <STM32_CAN.h>
89

910
using CanMsg = CAN_message_t;
@@ -12,17 +13,17 @@ using CanMsg = CAN_message_t;
1213
void onCanMessage(const CanMsg& msg);
1314

1415
static bool sendMsg(STM32_CAN& can_intf, uint32_t id, uint8_t length, const uint8_t* data) {
15-
CanMsg msg;
16-
msg.id = id & 0x1ffffff;
17-
msg.flags.extended = id & 0x80000000;
18-
msg.flags.remote = (data == nullptr);
19-
msg.len = length;
20-
if (data) {
21-
for (int i = 0; i < length; ++i) {
22-
msg.buf[i] = data[i];
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+
}
2325
}
24-
}
25-
return can_intf.write(msg) >= 0;
26+
return can_intf.write(msg) >= 0;
2627
}
2728

2829
static void onReceive(const CanMsg& msg, ODriveCAN& odrive) {

0 commit comments

Comments
 (0)