diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..2bf3c2c --- /dev/null +++ b/.clang-format @@ -0,0 +1,86 @@ +--- +BasedOnStyle: Google +IndentWidth: '4' +ColumnLimit: '120' + +IncludeCategories: + # main include automatically assigned to Priority 0 + - Regex: '^".*_(conf|config)\.(hpp|h)"$' # config headers + Priority: 3 + - Regex: '^".*"$' # sibling & project headers + Priority: 1 + - Regex: '^<.*>$' # system & library headers + Priority: 2 + +# Format function arguments and parameters +BinPackArguments: 'false' +BinPackParameters: 'false' +AllowAllArgumentsOnNextLine: 'false' +AllowAllParametersOfDeclarationOnNextLine: 'false' +AlignAfterOpenBracket: BlockIndent +SpaceBeforeParens: ControlStatements + +# Constructor Formatting +PackConstructorInitializers: CurrentLine +IndentAccessModifiers: 'false' +AccessModifierOffset: '-4' +SpaceBeforeCtorInitializerColon: 'true' +BreakConstructorInitializers: BeforeColon +LambdaBodyIndentation: OuterScope + +AllowShortCaseLabelsOnASingleLine: 'true' +AllowShortBlocksOnASingleLine: 'false' +AllowShortIfStatementsOnASingleLine: Never +AllowShortLoopsOnASingleLine: 'false' +AllowShortEnumsOnASingleLine: 'true' +AllowShortFunctionsOnASingleLine: 'Inline' +AllowShortLambdasOnASingleLine: 'All' + +# Switch / Case +IndentCaseLabels: 'true' +IndentCaseBlocks: 'false' + +# Preprocessor stuff +AlignConsecutiveMacros: 'false' +AlignEscapedNewlines: DontAlign +AlignTrailingComments: 'false' +SpacesBeforeTrailingComments: 1 + +# Alignment of procedural code +AlignConsecutiveAssignments: 'false' +AlignConsecutiveDeclarations: 'false' +AlignConsecutiveBitFields: Consecutive + +AlignOperands: AlignAfterOperator +# BreakBeforeTernaryOperators: 'false' +BreakBeforeBinaryOperators: 'true' + +# Pointers and East/West Const +DerivePointerAlignment: 'false' +PointerAlignment: Left +QualifierAlignment: Leave +# QualifierOrder: ['volatile', 'constexpr', 'static', 'inline', 'type', 'const'] + +Cpp11BracedListStyle: 'true' + +# Vertical Whitespace +SeparateDefinitionBlocks: Leave +EmptyLineBeforeAccessModifier: Always +EmptyLineAfterAccessModifier: Never + +# AlignArrayOfStructures: Right +# InsertTrailingCommas: 'Wrapped' + + +AlwaysBreakAfterReturnType: None +PenaltyReturnTypeOnItsOwnLine: 9999 # We really hate breaking after return types +PenaltyBreakAssignment: 999 # Prefer not to break around = + +SpaceAfterTemplateKeyword: 'false' + +MaxEmptyLinesToKeep: 2 + +FixNamespaceComments: 'true' +#IndentPPDirectives: BeforeHash +# NamespaceIndentation: All +... diff --git a/.github/workflows/formatting.yaml b/.github/workflows/formatting.yaml new file mode 100644 index 0000000..9032dc1 --- /dev/null +++ b/.github/workflows/formatting.yaml @@ -0,0 +1,36 @@ +name: Clang-Format Check + +on: + push: + branches: [master] + pull_request: + paths: + - '**/*.cpp' + - '**/*.h' + - '**/*.hpp' + +jobs: + clang-format-check: + runs-on: ubuntu-latest + + steps: + - name: Checkout code + uses: actions/checkout@v4 + + - name: Install Clang-Format + run: sudo apt-get install -y clang-format + + - name: Run Clang-Format Check + run: | + files=$(find . -name "*.cpp" -o -name "*.hpp" -o -name "*.h") + check_ignore=$(git -c core.excludesFile="format-ignore.txt" check-ignore --stdin --no-index --non-matching --verbose <<< "$files") + files=$(awk '/^::/ {print $2}' <<< "$check_ignore") + + echo "Checking $(echo "$files" | wc -l) files" + + if [ -n "$files" ]; then + clang-format -style=file -i $files + git diff --exit-code || (echo "Clang-Format check failed!" && exit 1) + else + echo "No files to check." + fi diff --git a/format-ignore.txt b/format-ignore.txt new file mode 100644 index 0000000..b44c884 --- /dev/null +++ b/format-ignore.txt @@ -0,0 +1,3 @@ +# autogenerated files +/src/can_simple_messages.hpp +/src/ODriveEnums.h diff --git a/src/ODriveCAN.cpp b/src/ODriveCAN.cpp index 995d761..bce61f6 100644 --- a/src/ODriveCAN.cpp +++ b/src/ODriveCAN.cpp @@ -134,8 +134,7 @@ bool ODriveCAN::getPower(Get_Powers_msg_t& msg, uint16_t timeout_ms) { return request(msg, timeout_ms); } -bool ODriveCAN::reset(ResetAction action) -{ +bool ODriveCAN::reset(ResetAction action) { Reboot_msg_t msg; msg.Action = action; diff --git a/src/ODriveCAN.h b/src/ODriveCAN.h index e95c1e5..f3ce537 100644 --- a/src/ODriveCAN.h +++ b/src/ODriveCAN.h @@ -8,23 +8,23 @@ #define REQUEST_PENDING 0xff +// clang-format off +// (clang-format gets confused by the lambdas inside the macro) #define CREATE_CAN_INTF_WRAPPER(TIntf) \ static inline ODriveCanIntfWrapper wrap_can_intf(TIntf& intf) { \ return { \ &intf, \ - [](void* intf, uint32_t id, uint8_t length, const uint8_t* data) { return sendMsg(*(TIntf*)intf, id, length, data); }, \ - [](void* intf) { pumpEvents(*(TIntf*)intf); } \ + [](void* intf, uint32_t id, uint8_t length, const uint8_t* data) { \ + return sendMsg(*(TIntf*)intf, id, length, data); \ + }, \ + [](void* intf) { pumpEvents(*(TIntf*)intf); }, \ }; \ } - +// clang-format on struct ODriveCanIntfWrapper { - bool sendMsg(uint32_t id, uint8_t length, const uint8_t* data) { - return (*send_msg_)(can_intf_, id, length, data); - } - void pump_events() { - (*pump_events_)(can_intf_); - } + bool sendMsg(uint32_t id, uint8_t length, const uint8_t* data) { return (*send_msg_)(can_intf_, id, length, data); } + void pump_events() { (*pump_events_)(can_intf_); } void* can_intf_; bool (*send_msg_)(void* intf, uint32_t id, uint8_t length, const uint8_t* data); @@ -33,36 +33,35 @@ struct ODriveCanIntfWrapper { class ODriveCAN { public: - ODriveCAN(const ODriveCanIntfWrapper& can_intf, uint32_t node_id) - : can_intf_(can_intf), node_id_(node_id) {}; + ODriveCAN(const ODriveCanIntfWrapper& can_intf, uint32_t node_id) : can_intf_(can_intf), node_id_(node_id) {} /** * @brief Clear all errors on the ODrive. - * + * * This function returns immediately and does not check if the ODrive * received the CAN message. */ bool clearErrors(); - + /** * @brief Tells the ODrive to change its axis state. - * + * * This function returns immediately and does not check if the ODrive * received the CAN message. */ bool setState(ODriveAxisState requested_state); - + /** * @brief Sets the control mode and input mode of the ODrive. - * + * * This function returns immediately and does not check if the ODrive * received the CAN message. */ bool setControllerMode(uint8_t control_mode, uint8_t input_mode); - + /** * @brief Sends a position setpoint with optional velocity and torque feedforward. - * + * * This function returns immediately and does not check if the ODrive * received the CAN message. */ @@ -70,7 +69,7 @@ class ODriveCAN { /** * @brief Sends a velocity setpoint with optional torque feedforward. - * + * * This function returns immediately and does not check if the ODrive * received the CAN message. */ @@ -78,7 +77,7 @@ class ODriveCAN { /** * @brief Sends a torque setpoint to the ODrive. - * + * * This function returns immediately and does not check if the ODrive * received the CAN message. */ @@ -86,7 +85,7 @@ class ODriveCAN { /** * @brief Sets the velocity and current limits - * + * * This function returns immediately and does not check if the ODrive * received the CAN message. */ @@ -94,39 +93,39 @@ class ODriveCAN { /** * @brief Sets the position gain - * + * * This function returns immediately and does not check if the ODrive * received the CAN message. */ bool setPosGain(float pos_gain); - + /** * @brief Sets the velocity and velocity integrator gains - * + * * This function returns immediately and does not check if the ODrive * received the CAN message. */ bool setVelGains(float vel_gain, float vel_integrator_gain); - + /** * @brief Sets the encoder's absolute position and enables absolute positioning - * + * * This function returns immediately and does not check if the ODrive * received the CAN message. */ bool setAbsolutePosition(float abs_pos); - + /** * @brief Sets the coast velocity for subsequent trapezoidal moves - * + * * This function returns immediately and does not check if the ODrive * received the CAN message. */ bool setTrapezoidalVelLimit(float vel_limit); - + /** * @brief Sets the acceleration and deceleration values for subsequent trapezoidal moves - * + * * This function returns immediately and does not check if the ODrive * received the CAN message. */ @@ -134,42 +133,42 @@ class ODriveCAN { /** * @brief Requests motor current. Iq_measured represents torque-generating current - * + * * This function will block and wait for up to timeout_ms (default 10msec) for ODrive to reply */ bool getCurrents(Get_Iq_msg_t& msg, uint16_t timeout_ms = 10); /** - * @brief Requests motor temperature - * + * @brief Requests motor temperature + * * This function will block and wait for up to timeout_ms (default 10msec) for ODrive to reply */ bool getTemperature(Get_Temperature_msg_t& msg, uint16_t timeout_ms = 10); /** * @brief Requests error information - * + * * This function will block and wait for up to timeout_ms (default 10msec) for ODrive to reply */ bool getError(Get_Error_msg_t& msg, uint16_t timeout_ms = 10); /** * @brief Requests hardware and firmware version information - * + * * This function will block and wait for up to timeout_ms (default 10msec) for ODrive to reply */ bool getVersion(Get_Version_msg_t& msg, uint16_t timeout_ms = 10); /** * @brief Requests encoder feedback data. - * + * * This function will block and wait for up to timeout_ms (default 10msec) for ODrive to reply */ bool getFeedback(Get_Encoder_Estimates_msg_t& msg, uint16_t timeout_ms = 10); /** * @brief Requests ODrive DC bus voltage and current - * + * * This function will block and wait for up to timeout_ms (default 10msec) for ODrive to reply * May trigger onBusVI callback if it's registered. */ @@ -177,20 +176,20 @@ class ODriveCAN { /** * @brief Requests mechanical and electrical power data (used for spinout detection) - * + * * This function will block and wait for up to timeout_ms (default 10msec) for ODrive to reply */ bool getPower(Get_Powers_msg_t& msg, uint16_t timeout_ms = 10); - + enum ResetAction : uint8_t { Reboot = 0, SaveConfiguration = 1, EraseConfiguration = 2, }; - + /** * @brief Resets the ODrive with the given action - * + * * Valid actions: * - Reboot (0) * - Save (1) @@ -198,12 +197,15 @@ class ODriveCAN { * */ bool reset(ResetAction action = ResetAction::Reboot); - + /** * @brief Registers a callback for ODrive feedback processing. */ - void onFeedback(void (*callback)(Get_Encoder_Estimates_msg_t& feedback, void* user_data), void* user_data = nullptr) { - feedback_callback_ = callback; + void onFeedback( + void (*callback)(Get_Encoder_Estimates_msg_t& feedback, void* user_data), + void* user_data = nullptr + ) { + feedback_callback_ = callback; feedback_user_data_ = user_data; } @@ -211,15 +213,15 @@ class ODriveCAN { * @brief Registers a callback for ODrive axis state feedback. */ void onStatus(void (*callback)(Heartbeat_msg_t& feedback, void* user_data), void* user_data = nullptr) { - axis_state_callback_ = callback; + axis_state_callback_ = callback; axis_state_user_data_ = user_data; } - + /** * @brief Registers a callback for ODrive torques feedback processing. */ void onTorques(void (*callback)(Get_Torques_msg_t& feedback, void* user_data), void* user_data = nullptr) { - torques_callback_ = callback; + torques_callback_ = callback; torques_user_data_ = user_data; } @@ -234,7 +236,10 @@ class ODriveCAN { /** * @brief Registers a callback for ODrive bus voltage/current feedback. */ - void onBusVI(void (*callback)(Get_Bus_Voltage_Current_msg_t& feedback, void* user_data), void* user_data = nullptr) { + void onBusVI( + void (*callback)(Get_Bus_Voltage_Current_msg_t& feedback, void* user_data), + void* user_data = nullptr + ) { busVI_callback_ = callback; busVI_user_data_ = user_data; } @@ -262,7 +267,7 @@ class ODriveCAN { /** * @brief Sends a request message and awaits a response. - * + * * Blocks until the response is received or the timeout is reached. Returns * false if the ODrive does not respond within the specified timeout. */ @@ -274,7 +279,9 @@ class ODriveCAN { 0, // no data nullptr // RTR=1 ); - if (!awaitMsg(timeout_ms)) return false; + if (!awaitMsg(timeout_ms)) { + return false; + } msg.decode_buf(buffer_); return true; } @@ -286,26 +293,22 @@ class ODriveCAN { bool send(const T& msg) { uint8_t data[8] = {}; msg.encode_buf(data); - return can_intf_.sendMsg( - (node_id_ << ODriveCAN::kNodeIdShift) | msg.cmd_id, - msg.msg_length, - data - ); + return can_intf_.sendMsg((node_id_ << ODriveCAN::kNodeIdShift) | msg.cmd_id, msg.msg_length, data); } /** * @brief Get value at the endpoint - * + * * @tparam T The data type expected from the endpoint * @param endpoint_id Unique ID from flat_endpoints.json * @param timeout_ms Time to wait for a response from ODrive - * + * * @return T Data from the endpoint, or 0 on timeout * * Blocks until the response is received or the timeout is reached. * */ - template + template T getEndpoint(uint16_t endpoint_id, uint16_t timeout_ms = 10) { uint8_t data[8] = {}; data[0] = 0; // Opcode read @@ -316,7 +319,9 @@ class ODriveCAN { requested_msg_id_ = 0x005; // Await TxSdo message can_intf_.sendMsg((node_id_ << ODriveCAN::kNodeIdShift) | 0x004, 8, data); - if (!awaitMsg(timeout_ms)) return T{}; + if (!awaitMsg(timeout_ms)) { + return T{}; + } T ret{}; memcpy(&ret, &buffer_[4], sizeof(T)); @@ -325,7 +330,7 @@ class ODriveCAN { /** * @brief Set endpoint to value - * + * * @tparam T Type of the value from flat_endpoints.json * @param endpoint_id Unique ID of endpoint from flat_endpoints.json * @param value value to write to the endpoint @@ -333,7 +338,7 @@ class ODriveCAN { * This function returns immediately and does not check if the ODrive * received the CAN message. */ - template + template bool setEndpoint(uint16_t endpoint_id, T value) { uint8_t data[8] = {}; data[0] = 1; // Opcode write @@ -371,7 +376,7 @@ class ODriveCAN { void* busVI_user_data_; void* currents_user_data_; void* error_user_data_; - + void (*axis_state_callback_)(Heartbeat_msg_t& feedback, void* user_data) = nullptr; void (*feedback_callback_)(Get_Encoder_Estimates_msg_t& feedback, void* user_data) = nullptr; void (*torques_callback_)(Get_Torques_msg_t& feedback, void* user_data) = nullptr; diff --git a/src/ODriveFlexCAN.hpp b/src/ODriveFlexCAN.hpp index 623912d..b5a0edf 100644 --- a/src/ODriveFlexCAN.hpp +++ b/src/ODriveFlexCAN.hpp @@ -3,8 +3,8 @@ #pragma once -#include "ODriveCAN.h" #include "FlexCAN_T4.h" +#include "ODriveCAN.h" using CanMsg = CAN_message_t; @@ -14,7 +14,7 @@ static bool sendMsg(FlexCAN_T4_Base& can_intf, uint32_t id, uint8_t length, cons .flags = {.extended = (bool)(id & 0x80000000), .remote = !data}, .len = length, }; - + if (data) { memcpy(teensy_msg.buf, data, length); } diff --git a/src/ODriveHardwareCAN.hpp b/src/ODriveHardwareCAN.hpp index 940c844..945410b 100644 --- a/src/ODriveHardwareCAN.hpp +++ b/src/ODriveHardwareCAN.hpp @@ -3,6 +3,7 @@ #pragma once #include "ODriveCAN.h" + #include // Must be defined by the application @@ -10,7 +11,7 @@ void onCanMessage(const CanMsg& msg); /** * @brief Sends a CAN message over the specified platform-specific interface. - * + * * @param can_intf A platform-specific reference to the CAN interface to use. * @param id: The CAN message ID to send. * Bit 31 indicates if the ID is extended (29-bit) or standard (11-bit). @@ -25,18 +26,18 @@ static bool sendMsg(HardwareCAN& can_intf, uint32_t id, uint8_t length, const ui // Note: Arduino_CAN does not support the RTR bit. The ODrive interprets // zero-length packets the same as RTR=1, but it creates the possibility of // collisions. - CanMsg msg( + CanMsg msg{ (id & 0x80000000) ? CanExtendedId(id) : CanStandardId(id), length, - data - ); + data, + }; return can_intf.write(msg) >= 0; } /** * @brief Receives a CAN message from the platform-specific interface and passes * it to the ODriveCAN instance. - * + * * @param msg: The received CAN message in a platform-specific format. * @param odrive: The ODriveCAN instance to pass the message to. */ @@ -47,10 +48,10 @@ static void onReceive(const CanMsg& msg, ODriveCAN& odrive) { /** * @brief Processes the CAN interface's RX buffer and calls onCanMessage for * each pending message. - * + * * On hardware interfaces where onCanMessage() is already called from the * interrupt handler, this function is a no-op. - * + * * @param intf: The platform-specific CAN interface to process. * @param max_events: The maximum number of events to process. This prevents * an infinite loop if messages come at a high rate. diff --git a/src/ODriveSTM32CAN.hpp b/src/ODriveSTM32CAN.hpp index 5ca7598..2fa5b25 100644 --- a/src/ODriveSTM32CAN.hpp +++ b/src/ODriveSTM32CAN.hpp @@ -4,6 +4,7 @@ #pragma once #include "ODriveCAN.h" + #include using CanMsg = CAN_message_t; @@ -12,17 +13,17 @@ using CanMsg = CAN_message_t; void onCanMessage(const CanMsg& msg); static bool sendMsg(STM32_CAN& can_intf, uint32_t id, uint8_t length, const uint8_t* data) { - CanMsg msg; - msg.id = id & 0x1ffffff; - msg.flags.extended = id & 0x80000000; - msg.flags.remote = (data == nullptr); - msg.len = length; - if (data) { - for (int i = 0; i < length; ++i) { - msg.buf[i] = data[i]; + CanMsg msg; + msg.id = id & 0x1ffffff; + msg.flags.extended = id & 0x80000000; + msg.flags.remote = (data == nullptr); + msg.len = length; + if (data) { + for (int i = 0; i < length; ++i) { + msg.buf[i] = data[i]; + } } - } - return can_intf.write(msg) >= 0; + return can_intf.write(msg) >= 0; } static void onReceive(const CanMsg& msg, ODriveCAN& odrive) { diff --git a/src/ODriveUART.cpp b/src/ODriveUART.cpp index 10e8823..088112b 100644 --- a/src/ODriveUART.cpp +++ b/src/ODriveUART.cpp @@ -2,17 +2,19 @@ // License: MIT // Documentation: https://docs.odriverobotics.com/v/latest/guides/arduino-uart-guide.html -#include "Arduino.h" #include "ODriveUART.h" +#include "Arduino.h" + static const int kMotorNumber = 0; // Print with stream operator +// clang-format off template inline Print& operator <<(Print &obj, T arg) { obj.print(arg); return obj; } template<> inline Print& operator <<(Print &obj, float arg) { obj.print(arg, 4); return obj; } +// clang-format on -ODriveUART::ODriveUART(Stream& serial) - : serial_(serial) {} +ODriveUART::ODriveUART(Stream& serial) : serial_(serial) {} void ODriveUART::clearErrors() { serial_ << F("sc\n"); @@ -27,7 +29,8 @@ void ODriveUART::setPosition(float position, float velocity_feedforward) { } void ODriveUART::setPosition(float position, float velocity_feedforward, float torque_feedforward) { - serial_ << F("p ") << kMotorNumber << F(" ") << position << F(" ") << velocity_feedforward << F(" ") << torque_feedforward << F("\n"); + serial_ << F("p ") << kMotorNumber << F(" ") << position << F(" ") << velocity_feedforward << F(" ") + << torque_feedforward << F("\n"); } void ODriveUART::setVelocity(float velocity) { @@ -35,7 +38,7 @@ void ODriveUART::setVelocity(float velocity) { } void ODriveUART::setVelocity(float velocity, float torque_feedforward) { - serial_ << F("v ") << kMotorNumber << F(" ") << velocity << F(" ") << torque_feedforward << F("\n"); + serial_ << F("v ") << kMotorNumber << F(" ") << velocity << F(" ") << torque_feedforward << F("\n"); } void ODriveUART::setTorque(float torque) { @@ -60,7 +63,7 @@ ODriveFeedback ODriveUART::getFeedback() { if (spacePos >= 0) { return { response.substring(0, spacePos).toFloat(), - response.substring(spacePos+1).toFloat() + response.substring(spacePos + 1).toFloat(), }; } else { return {0.0f, 0.0f}; diff --git a/src/ODriveUART.h b/src/ODriveUART.h index 714015a..81ab389 100644 --- a/src/ODriveUART.h +++ b/src/ODriveUART.h @@ -62,21 +62,21 @@ class ODriveUART { /** * @brief Requests the latest position and velocity estimates. - * + * * Returns pos = 0.0 and vel = 0.0 in case of a communication error. */ ODriveFeedback getFeedback(); /** * @brief Requests the latest position estimate. - * + * * Returns 0.0 in case of a communication error. */ float getPosition() { return getFeedback().pos; } /** * @brief Requests the latest velocity estimate. - * + * * Returns 0.0 in case of a communication error. */ float getVelocity() { return getFeedback().vel; } @@ -95,7 +95,7 @@ class ODriveUART { /** * @brief Requests the current axis state from the ODrive. - * + * * Returns AXIS_STATE_UNDEFINED in case of a communication error. */ ODriveAxisState getState(); @@ -106,4 +106,4 @@ class ODriveUART { Stream& serial_; }; -#endif //ODriveUART_h +#endif // ODriveUART_h