diff --git a/.github/workflows/ctest-develop.yml b/.github/workflows/ctest-develop.yml new file mode 100644 index 0000000..b3489dd --- /dev/null +++ b/.github/workflows/ctest-develop.yml @@ -0,0 +1,22 @@ +name: GTest Sanity Check + +on: + push: + branches: [ "develop" ] + pull_request: + branches: [ "develop" ] + +jobs: + build: + + runs-on: ubuntu-latest + + steps: + - uses: actions/checkout@v4 + - name: Cmake + run: | + cmake -B build -DBUILD_TESTING=ON + cmake --build build + ctest --test-dir build + cd build && ctest && cd .. + \ No newline at end of file diff --git a/.github/workflows/release.yml b/.github/workflows/release.yml new file mode 100644 index 0000000..5365f81 --- /dev/null +++ b/.github/workflows/release.yml @@ -0,0 +1,46 @@ +name: Publish Multi-Target Release + +on: + push: + tags: + - 'v*' + branches: + - main + - release + + +jobs: + release: + runs-on: ubuntu-latest + + steps: + - name: Checkout repository + uses: actions/checkout@v2 + + - name: Install ARM Toolchain for STM + run: sudo apt-get update && sudo apt-get install -y gcc-arm-none-eabi + + # Build Linux version + - name: Build Linux target + run: | + cmake -B build_linux -DCMAKE_INSTALL_PREFIX=lib_linux -DBUILD_TESTING=OFF + cmake --build build_linux + cmake --install build_linux + cd lib_linux && zip -r ../linux_package.zip . && cd .. + + # Build STM32 version + - name: Build STM32 target + run: | + cmake -B build_stm -DCMAKE_INSTALL_PREFIX=lib_stm -DBUILD_TESTING=OFF -DTARGET_STM=ON + cmake --build build_stm + cmake --install build_stm + cd lib_stm && zip -r ../stm_package.zip . && cd .. + + # Upload both release artifacts + - name: Upload release artifacts + uses: actions/upload-artifact@v3 + with: + name: release-artifacts-${{ github.ref_name }} + path: | + linux_package.zip + stm_package.zip \ No newline at end of file diff --git a/.gitignore b/.gitignore index 259148f..2ed17b9 100644 --- a/.gitignore +++ b/.gitignore @@ -30,3 +30,8 @@ *.exe *.out *.app + +# Build Archive +build*/ +lib*/ +.vscode/ diff --git a/C++.gitignore b/C++.gitignore deleted file mode 100644 index 259148f..0000000 --- a/C++.gitignore +++ /dev/null @@ -1,32 +0,0 @@ -# Prerequisites -*.d - -# Compiled Object files -*.slo -*.lo -*.o -*.obj - -# Precompiled Headers -*.gch -*.pch - -# Compiled Dynamic libraries -*.so -*.dylib -*.dll - -# Fortran module files -*.mod -*.smod - -# Compiled Static libraries -*.lai -*.la -*.a -*.lib - -# Executables -*.exe -*.out -*.app diff --git a/CMake.gitignore b/CMake.gitignore deleted file mode 100644 index 11c7643..0000000 --- a/CMake.gitignore +++ /dev/null @@ -1,12 +0,0 @@ -CMakeLists.txt.user -CMakeCache.txt -CMakeFiles -CMakeScripts -Testing -Makefile -cmake_install.cmake -install_manifest.txt -compile_commands.json -CTestTestfile.cmake -_deps -CMakeUserPresets.json diff --git a/CMakeLists.txt b/CMakeLists.txt index 1c0a6c3..892a4f8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,22 +1,65 @@ -cmake_minimum_required(VERSION 3.2) +option(TARGET_STM "Build for STM32F0 using ARM toolchain" OFF) -project(cmake-project-template) +cmake_minimum_required(VERSION 3.14) +project(MotorControllerLib) -set(CMAKE_CXX_STANDARD 11) -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -O3") +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_INSTALL_PREFIX ${PROJECT_SOURCE_DIR}) +if(TARGET_STM) + message(STATUS "Configuring build for STM32F0") + set(CMAKE_TOOLCHAIN_FILE "${CMAKE_SOURCE_DIR}/cmake/arm-gcc-toolchain.cmake") + set(CMAKE_CXX_STANDARD 17) +else() + message(STATUS "Configuring build for native Linux") + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) +endif() -set(DIVISIBLE_INSTALL_INCLUDE_DIR ${PROJECT_SOURCE_DIR}/include) -set(DIVISIBLE_INSTALL_BIN_DIR ${PROJECT_SOURCE_DIR}/bin) -set(DIVISIBLE_INSTALL_LIB_DIR ${PROJECT_SOURCE_DIR}/lib) +# === Library Target === +add_library(Odrive STATIC + src/lib.cpp +) -set(DIVISION_HEADERS_DIR ${PROJECT_SOURCE_DIR}/src/division) +target_include_directories(Odrive PUBLIC ${CMAKE_SOURCE_DIR}/inc) -include_directories(${DIVISIBLE_INSTALL_INCLUDE_DIR}) -include_directories(${DIVISION_HEADERS_DIR}) +# === Install Targets === +install(TARGETS Odrive + ARCHIVE DESTINATION lib) -add_subdirectory(src) -add_subdirectory(test) +install(DIRECTORY inc/ DESTINATION include) +if(NOT CMAKE_CROSSCOMPILING) + # === Enable Testing === + include(CTest) # enables BUILD_TESTING option + if(BUILD_TESTING) + # Fetch GoogleTest + include(FetchContent) + FetchContent_Declare( + googletest + URL https://github.com/google/googletest/archive/03597a01ee50ed33e9dfd640b249b4be3799d395.zip + ) + set(gtest_force_shared_crt ON CACHE BOOL "" FORCE) + FetchContent_MakeAvailable(googletest) + + # === Test Executable === + add_executable(utils_test + test/test_can_header.cpp + test/test_can_msg.cpp + ) + + target_include_directories(utils_test PRIVATE ${CMAKE_SOURCE_DIR}/inc) + + target_link_libraries(utils_test + PRIVATE + Odrive # <-- your library + GTest::gtest_main + ) + + include(GoogleTest) + gtest_discover_tests(utils_test) + + endif() + +endif() \ No newline at end of file diff --git a/README.md b/README.md index 08fd58b..2c703ad 100644 --- a/README.md +++ b/README.md @@ -1,31 +1,123 @@ # Introduction -This is an official branch for all the device that need to interact with ODrive or Similar Motor Controller. Our team are currently using ODrive V3.6, ODrive Micro, and ODrive S1. +This is an official library repository for all the device that need to interact with ODrive micro or S1 or similar Motor Controller. Our team are currently using ODrive V3.6, ODrive Micro, and ODrive S1. Currently the command set support ODrive Micro and S1 using documentation 6.11. [ODrive Docs](https://docs.odriverobotics.com/v/latest/guides/getting-started.html) + +# Project Objective + +In short term, this project is aimed to support ODrive family. In long term, this project is also aiming to support custom FOC Device manufactured in house. + +- High-Level CAN Message Agent + - Device Identification + - Device Command Abstraction over CAN + - Grouping Device Command +- INS/ GNSS Fusion and Logging +- Device Error Handling +- Robotics Simulation Abstraction(Gazebo) + +Anyone using this library still need to integrate this library into the application/ simulation on top of this static library. + +On my computer the post-compiled library is libodrive.a. # Folder Structure -The project is divided into generic code & interface in the /inc and /src folder. Target specific code are linked in the /target folder. +The project ```bash . +├── C++.gitignore +├── CMake.gitignore +├── CMakeLists.txt +├── LICENSE +├── README.md +├── build ├── inc ├── src -└── target - ├── ros - ├── seeedxiao - └── stm +└── test ``` -# Project Objective +## Movement Command -In short term, this project is aimed to support ODrive family. In long term, this project is also aiming to support custom FOC Device manufactured in house. -- Device Identification -- Device Command Abstraction over CAN -- Grouping Device Command -- INS/ GNSS Fusion and Logging -- Device Error Handling -- Robotics Simulation Abstraction(Gazebo) +```c++ +enum class MvCmd { + Vel, + Pos +}; + +class MvCmd { + MvCmd mode; + uint16_t id; + float32 value[2]; //X, Y component for vel or pos +} +``` + +Note: Vel_FF, Torque_FF is preconfigured variable. + +## Get Command -Anyone using this library still need to integrate application/ simulation on top of this static library. +```cpp +class InsOut { + float fet_temp; + float bus_volt; + float bus_curr; + float pos_est; + float vel_est; + float torque_est; +} + +class Logging { + float fet_temp; + float bus_volt; + float bus_curr; + float pos_est; + float vel_est; + float torque_est; + float motor_temp; + float phase_current_setpoint; + float phase_current_measured; + float torque_tar; + float elec_power; + float mech_power; +} +``` + +# Compile Library and UnitTest + +```bash +# Linux Target +# Option 1: +mkdir build && cd build +cmake .. && cd .. +make + +# Option 2: +cmake -B build -DCMAKE_INSTALL_PREFIX=lib -DBUILD_TESTING=OFF +cmake --build build +cmake --install build +``` + +```cpp +# STM Target +cmake -B build_stm -DCMAKE_INSTALL_PREFIX=lib_stm -DBUILD_TESTING=OFF -DTARGET_STM=ON +cmake --build build_stm +cmake --install build_stm +``` + +# Testing and Validation + +```bash +cmake -B build -DBUILD_TESTING=ON +cmake --build build +ctest --test-dir build + +# Execute test +cd build && ctest && cd .. +``` + +## Triggering Actions + +```bash +git tag -a v0.0.3 -m "Release v0.0.1" +git push --follow-tags origin develop +``` # Acknowledgements diff --git a/cmake/arm-gcc-toolchain.cmake b/cmake/arm-gcc-toolchain.cmake new file mode 100644 index 0000000..4de4dad --- /dev/null +++ b/cmake/arm-gcc-toolchain.cmake @@ -0,0 +1,19 @@ +# Set compiler +set(CMAKE_SYSTEM_NAME Generic) +set(CMAKE_SYSTEM_PROCESSOR cortex-m0) + +# Specify cross compiler +set(CMAKE_C_COMPILER arm-none-eabi-gcc) +set(CMAKE_CXX_COMPILER arm-none-eabi-g++) +set(CMAKE_ASM_COMPILER arm-none-eabi-gcc) + +# No standard C library, no OS +set(CMAKE_TRY_COMPILE_TARGET_TYPE STATIC_LIBRARY) +set(CMAKE_EXE_LINKER_FLAGS_INIT "") +set(CMAKE_C_FLAGS_INIT "-mcpu=cortex-m0 -mthumb -mfloat-abi=soft -fno-exceptions -ffunction-sections -fdata-sections") +set(CMAKE_CXX_FLAGS_INIT "${CMAKE_C_FLAGS_INIT} -fno-rtti") +set(CMAKE_ASM_FLAGS_INIT "-mcpu=cortex-m0 -mthumb") + +# Prevent default CMake stdlib linkage +set(CMAKE_CXX_STANDARD_LIBRARIES "") +set(CMAKE_C_STANDARD_LIBRARIES "") \ No newline at end of file diff --git a/example/CMakeLists.txt b/example/CMakeLists.txt new file mode 100644 index 0000000..a236dcd --- /dev/null +++ b/example/CMakeLists.txt @@ -0,0 +1,19 @@ +cmake_minimum_required(VERSION 3.14) +project(LibExample) + + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +# Add the library manually +add_library(libOdrive STATIC IMPORTED GLOBAL) +set_target_properties(libOdrive PROPERTIES + IMPORTED_LOCATION ${CMAKE_SOURCE_DIR}/lib/odrive/lib/libOdrive.a + INTERFACE_INCLUDE_DIRECTORIES ${CMAKE_SOURCE_DIR}/lib/odrive/include +) + +# Create your executable +add_executable(exec main.cpp) + +# Link the imported static lib +target_link_libraries(exec PRIVATE libOdrive) \ No newline at end of file diff --git a/example/main.cpp b/example/main.cpp new file mode 100644 index 0000000..d52234c --- /dev/null +++ b/example/main.cpp @@ -0,0 +1,33 @@ +#include +#include // For memcpy +#include "utils.hpp" + +int main() { + using namespace CanHeader; + using namespace CanPayload; + + // Encode CAN header + HeaderInfo header(0x01, CmdMap::Get_Version); + uint16_t encoded = encode(header); + auto decoded = decode(encoded); + + std::cout << "[Header] Encoded: 0x" << std::hex << encoded << std::dec << "\n"; + std::cout << "[Header] Decoded ID: " << static_cast(decoded.id) + << ", CMD: " << static_cast(decoded.cmd) << "\n"; + + // Encode & decode payload + uint64_t packed = pack_GetVersionPayload(1, 2, 3, 4, 5, 6, 7, 8); + auto payload = unpack_GetVersionPayload(packed); + + std::cout << "[Payload] Packed: 0x" << std::hex << packed << std::dec << "\n"; + std::cout << "[Payload] Decoded: Proto=" << static_cast(payload.protocol_version) + << ", HW=" << static_cast(payload.hw_version_major) << "." + << static_cast(payload.hw_version_minor) << "." + << static_cast(payload.hw_version_variant) + << ", FW=" << static_cast(payload.fw_version_major) << "." + << static_cast(payload.fw_version_minor) << "." + << static_cast(payload.fw_version_revision) << "." + << static_cast(payload.fw_version_unreleased) << "\n"; + + return 0; +} \ No newline at end of file diff --git a/inc/can_simple.hpp b/inc/can_simple.hpp new file mode 100644 index 0000000..57b1f42 --- /dev/null +++ b/inc/can_simple.hpp @@ -0,0 +1,348 @@ +#pragma once +#include + +namespace CanHeader { + constexpr uint8_t broadcastNodeId = 0x3f; + constexpr uint16_t headerBound = 0x7ff; + constexpr uint8_t cmdBound = 0x1f; + constexpr uint8_t errorCode8 = static_cast(-1); + constexpr uint16_t errCode16 = static_cast(-1); + + /// @todo: Support Beacon Message for auto buad + constexpr bool autoBuad = false; + + enum class CmdMap : uint8_t { + Get_Version = 0x000, + Heartbeat = 0x001, + Estop = 0x002, + Get_Error = 0x003, + RxSdo = 0x004, + TxSdo = 0x005, + Address = 0x006, + Set_Axis_State = 0x007, + Get_Encoder_Estimates = 0x009, + Set_Controller_Mode = 0x00b, + Set_Input_Pos = 0x00c, + Set_Input_Vel = 0x00d, + Set_Input_Torque = 0x00e, + Set_Limits = 0x00f, + Set_Traj_Vel_Limit = 0x011, + Set_Traj_Accel_Limits = 0x012, + Set_Traj_Inertia = 0x013, + Get_Iq = 0x014, + Get_Temperature = 0x015, + Reboot = 0x016, + Get_Bus_Voltage_Current = 0x017, + Clear_Errors = 0x018, + Set_Absolute_Position = 0x019, + Set_Pos_Gain = 0x01a, + Set_Vel_Gains = 0x01b, + Get_Torques = 0x01c, + Get_Powers = 0x01d, + Enter_DFU_Mode = 0x01f + }; + + constexpr std::array valid_cmds = { + CmdMap::Get_Version, + CmdMap::Heartbeat, + CmdMap::Estop, + CmdMap::Get_Error, + CmdMap::RxSdo, + CmdMap::TxSdo, + CmdMap::Address, + CmdMap::Set_Axis_State, + CmdMap::Get_Encoder_Estimates, + CmdMap::Set_Controller_Mode, + CmdMap::Set_Input_Pos, + CmdMap::Set_Input_Vel, + CmdMap::Set_Input_Torque, + CmdMap::Set_Limits, + CmdMap::Set_Traj_Vel_Limit, + CmdMap::Set_Traj_Accel_Limits, + CmdMap::Set_Traj_Inertia, + CmdMap::Get_Iq, + CmdMap::Get_Temperature, + CmdMap::Reboot, + CmdMap::Get_Bus_Voltage_Current, + CmdMap::Clear_Errors, + CmdMap::Set_Absolute_Position, + CmdMap::Set_Pos_Gain, + CmdMap::Set_Vel_Gains, + CmdMap::Get_Torques, + CmdMap::Get_Powers, + CmdMap::Enter_DFU_Mode + }; +} + + + +namespace CanPayload { + #pragma pack(push, 1) + struct GetVersionPayload { + uint8_t protocol_version; + uint8_t hw_version_major; + uint8_t hw_version_minor; + uint8_t hw_version_variant; + uint8_t fw_version_major; + uint8_t fw_version_minor; + uint8_t fw_version_revision; + uint8_t fw_version_unreleased; + + GetVersionPayload() = default; + GetVersionPayload(uint8_t proto, uint8_t hw_maj, uint8_t hw_min, uint8_t hw_var, + uint8_t fw_maj, uint8_t fw_min, uint8_t fw_rev, uint8_t fw_unrel) + : protocol_version(proto), hw_version_major(hw_maj), hw_version_minor(hw_min), + hw_version_variant(hw_var), fw_version_major(fw_maj), fw_version_minor(fw_min), + fw_version_revision(fw_rev), fw_version_unreleased(fw_unrel) {} + }; + + struct HeartbeatPayload { + uint32_t axis_error{}; + uint8_t axis_state{}; + uint8_t procedure_result{}; + uint8_t trajectory_done{}; + uint8_t reserved{}; + + HeartbeatPayload() = default; + HeartbeatPayload(uint32_t err, uint8_t state, uint8_t result, uint8_t done, uint8_t res) + : axis_error(err), axis_state(state), procedure_result(result), trajectory_done(done), reserved(res) {} + }; + + struct GetErrorPayload { + uint32_t active_errors{}; + uint32_t disarm_reason{}; + + GetErrorPayload() = default; + GetErrorPayload(uint32_t errors, uint32_t reason) + : active_errors(errors), disarm_reason(reason) {} + }; + + struct RxSdoPayload { + uint8_t opcode{}; + uint16_t endpoint_id{}; + uint8_t reserved{}; + uint32_t value{}; + + RxSdoPayload() = default; + RxSdoPayload(uint8_t op, uint16_t ep, uint8_t res, uint32_t val) + : opcode(op), endpoint_id(ep), reserved(res), value(val) {} + }; + + struct TxSdoPayload { + uint8_t reserved0{}; + uint16_t endpoint_id{}; + uint8_t reserved1{}; + uint32_t value{}; + + TxSdoPayload() = default; + TxSdoPayload(uint8_t res0, uint16_t ep, uint8_t res1, uint32_t val) + : reserved0(res0), endpoint_id(ep), reserved1(res1), value(val) {} + }; + + struct AddressPayload { + uint8_t node_id{}; + uint32_t serial_number_start{}; + uint16_t serial_number_end{}; + uint8_t connection_id{}; + + AddressPayload() = default; + AddressPayload(uint8_t nid, uint32_t sn_start, uint16_t sn_end, uint8_t conn_id) + : node_id(nid), serial_number_start(sn_start), serial_number_end(sn_end), connection_id(conn_id) {} + }; + + struct EncoderEstimatesPayload { + float pos_estimate{}; + float vel_estimate{}; + + EncoderEstimatesPayload() = default; + EncoderEstimatesPayload(float pos, float vel) : pos_estimate(pos), vel_estimate(vel) {} + }; + + struct GetIqPayload { + float iq_setpoint{}; + float iq_measured{}; + + GetIqPayload() = default; + GetIqPayload(float set, float meas) : iq_setpoint(set), iq_measured(meas) {} + }; + + struct GetTemperaturePayload { + float fet_temperature{}; + float motor_temperature{}; + + GetTemperaturePayload() = default; + GetTemperaturePayload(float fet, float motor) : fet_temperature(fet), motor_temperature(motor) {} + }; + + struct BusVoltageCurrentPayload { + float bus_voltage{}; + float bus_current{}; + + BusVoltageCurrentPayload() = default; + BusVoltageCurrentPayload(float volt, float curr) : bus_voltage(volt), bus_current(curr) {} + }; + + struct GetTorquesPayload { + float torque_target{}; + float torque_estimate{}; + + GetTorquesPayload() = default; + GetTorquesPayload(float tgt, float est) : torque_target(tgt), torque_estimate(est) {} + }; + + struct GetPowersPayload { + float electrical_power{}; + float mechanical_power{}; + + GetPowersPayload() = default; + GetPowersPayload(float elec, float mech) : electrical_power(elec), mechanical_power(mech) {} + }; + + struct SetAxisStatePayload { + uint32_t axis_requested_state{}; + uint32_t reserved{}; + + SetAxisStatePayload() = default; + SetAxisStatePayload(uint32_t req, uint32_t res) : axis_requested_state(req), reserved(res) {} + }; + + struct SetControllerModePayload { + uint8_t control_mode{}; + uint8_t input_mode{}; + uint16_t reserved0{}; + uint32_t reserved1{}; + + SetControllerModePayload() = default; + SetControllerModePayload(uint8_t ctrl, uint8_t input, uint16_t res0, uint32_t res1) + : control_mode(ctrl), input_mode(input), reserved0(res0), reserved1(res1) {} + }; + + struct SetInputPosPayload { + float input_pos{}; + int16_t vel_ff{}; + int16_t torque_ff{}; + + SetInputPosPayload() = default; + SetInputPosPayload(float pos, int16_t vff, int16_t tff) : input_pos(pos), vel_ff(vff), torque_ff(tff) {} + }; + + struct SetInputVelPayload { + float input_vel{}; + float input_torque_ff{}; + + SetInputVelPayload() = default; + SetInputVelPayload(float vel, float torque_ff) : input_vel(vel), input_torque_ff(torque_ff) {} + }; + + struct SetInputTorquePayload { + float input_torque{}; + uint32_t reserved{}; + + SetInputTorquePayload() = default; + SetInputTorquePayload(float torque, uint32_t res) : input_torque(torque), reserved(res) {} + }; + + struct SetLimitsPayload { + float velocity_limit{}; + float current_limit{}; + + SetLimitsPayload() = default; + SetLimitsPayload(float vel, float curr) : velocity_limit(vel), current_limit(curr) {} + }; + + struct SetTrajVelLimitPayload { + float traj_vel_limit{}; + uint32_t reserved{}; + + SetTrajVelLimitPayload() = default; + SetTrajVelLimitPayload(float vel, uint32_t res) : traj_vel_limit(vel), reserved(res) {} + }; + + struct SetTrajAccelLimitsPayload { + float traj_accel_limit{}; + float traj_decel_limit{}; + + SetTrajAccelLimitsPayload() = default; + SetTrajAccelLimitsPayload(float accel, float decel) : traj_accel_limit(accel), traj_decel_limit(decel) {} + }; + + struct SetTrajInertiaPayload { + float traj_inertia{}; + uint32_t reserved{}; + + SetTrajInertiaPayload() = default; + SetTrajInertiaPayload(float inertia, uint32_t res) : traj_inertia(inertia), reserved(res) {} + }; + + struct RebootPayload { + uint8_t action{}; + uint8_t reserved1{}; + uint16_t reserved2{}; + uint32_t reserved3{}; + + RebootPayload() = default; + RebootPayload(uint8_t act, uint8_t r1, uint16_t r2, uint32_t r3) + : action(act), reserved1(r1), reserved2(r2), reserved3(r3) {} + }; + + struct ClearErrorsPayload { + uint32_t identify{}; + uint32_t reserved{}; + + ClearErrorsPayload() = default; + ClearErrorsPayload(uint32_t id, uint32_t res) : identify(id), reserved(res) {} + }; + + struct SetAbsolutePositionPayload { + float position{}; + uint32_t reserved{}; + + SetAbsolutePositionPayload() = default; + SetAbsolutePositionPayload(float pos, uint32_t res) : position(pos), reserved(res) {} + }; + + struct SetPosGainPayload { + float pos_gain{}; + uint32_t reserved{}; + + SetPosGainPayload() = default; + SetPosGainPayload(float gain, uint32_t res) : pos_gain(gain), reserved(res) {} + }; + + struct SetVelGainsPayload { + float vel_gain{}; + float vel_integrator_gain{}; + + SetVelGainsPayload() = default; + SetVelGainsPayload(float gain, float igain) : vel_gain(gain), vel_integrator_gain(igain) {} + }; + #pragma pack(pop) + + // Compile-time check that all CAN payloads are == 8 bytes + static_assert(sizeof(GetVersionPayload) == 8, "GetVersionPayload too large"); + static_assert(sizeof(HeartbeatPayload) == 8, "HeartbeatPayload too large"); + static_assert(sizeof(GetErrorPayload) == 8, "GetErrorPayload too large"); + static_assert(sizeof(RxSdoPayload) == 8, "RxSdoPayload too large"); + static_assert(sizeof(TxSdoPayload) == 8, "TxSdoPayload too large"); + static_assert(sizeof(AddressPayload) == 8, "AddressPayload too large"); + static_assert(sizeof(EncoderEstimatesPayload) == 8, "EncoderEstimatesPayload too large"); + static_assert(sizeof(GetIqPayload) == 8, "GetIqPayload too large"); + static_assert(sizeof(GetTemperaturePayload) == 8, "GetTemperaturePayload too large"); + static_assert(sizeof(BusVoltageCurrentPayload) == 8, "BusVoltageCurrentPayload too large"); + static_assert(sizeof(GetTorquesPayload) == 8, "GetTorquesPayload too large"); + static_assert(sizeof(GetPowersPayload) == 8, "GetPowersPayload too large"); + static_assert(sizeof(SetAxisStatePayload) == 8, "SetAxisStatePayload too large"); + static_assert(sizeof(SetControllerModePayload) == 8, "SetControllerModePayload too large"); + static_assert(sizeof(SetInputPosPayload) == 8, "SetInputPosPayload too large"); + static_assert(sizeof(SetInputVelPayload) == 8, "SetInputVelPayload too large"); + static_assert(sizeof(SetInputTorquePayload) == 8, "SetInputTorquePayload too large"); + static_assert(sizeof(SetLimitsPayload) == 8, "SetLimitsPayload too large"); + static_assert(sizeof(SetTrajVelLimitPayload) == 8, "SetTrajVelLimitPayload too large"); + static_assert(sizeof(SetTrajAccelLimitsPayload) == 8, "SetTrajAccelLimitsPayload too large"); + static_assert(sizeof(SetTrajInertiaPayload) == 8, "SetTrajInertiaPayload too large"); + static_assert(sizeof(RebootPayload) == 8, "RebootPayload too large"); + static_assert(sizeof(ClearErrorsPayload) == 8, "ClearErrorsPayload too large"); + static_assert(sizeof(SetAbsolutePositionPayload) == 8, "SetAbsolutePositionPayload too large"); + static_assert(sizeof(SetPosGainPayload) == 8, "SetPosGainPayload too large"); + static_assert(sizeof(SetVelGainsPayload) == 8, "SetVelGainsPayload too large"); +} \ No newline at end of file diff --git a/inc/utils.hpp b/inc/utils.hpp new file mode 100644 index 0000000..11b75da --- /dev/null +++ b/inc/utils.hpp @@ -0,0 +1,286 @@ +#pragma once +#include +#include +#include + +#include "can_simple.hpp" + +/** + * TODO: + * - compile to library using cmake + * - Support Error handling + * - support odrive config + * reference: https://github.com/odriverobotics/ODriveResources/blob/master/examples/can_restore_config.py + */ + +template +inline bool IsDefined(uint8_t value, const std::array& allowed) { + Enum candidate = static_cast(value); + return std::find(allowed.begin(), allowed.end(), candidate) != allowed.end(); +} + +template +inline uint64_t pack_struct_to_u64(const T& src) { + static_assert(sizeof(T) <= sizeof(uint64_t), "Struct too large to fit in uint64_t"); + uint64_t buf = 0; + memcpy(&buf, &src, sizeof(T)); + return buf; +} + +template +inline T unpack_u64_to_struct(uint64_t data) { + static_assert(sizeof(T) <= sizeof(uint64_t), "Struct too large to unpack from uint64_t"); + T result; + memcpy(&result, &data, sizeof(T)); + return result; +} + +namespace CanHeader { + struct HeaderInfo{ + uint8_t id; + CmdMap cmd; + + HeaderInfo(uint8_t id_val, CmdMap cmd_val) + : id(id_val), cmd(cmd_val) {} + }; + + + inline uint16_t encode(HeaderInfo header) { + // Boundary Check + if(header.id > broadcastNodeId) { + return errCode16; + } + return (static_cast(header.id) << 5) | static_cast(header.cmd); + } + + inline HeaderInfo decode(uint16_t buf) { + // Boundary Check + if(buf > headerBound) { + return HeaderInfo(errorCode8, static_cast(errorCode8)); + } + return HeaderInfo(buf>>5 & broadcastNodeId, static_cast(buf & cmdBound)); + } + + inline bool operator==(const HeaderInfo& lhs, const HeaderInfo& rhs) { + return lhs.id == rhs.id && lhs.cmd == rhs.cmd; + } + + inline bool is_defined(uint8_t cmd) { + return IsDefined(cmd, valid_cmds); + } +} + +namespace CanPayload { + inline uint64_t align_int48_t(uint32_t upper, uint16_t lower) { + return static_cast (upper << 16) & lower; + } + + inline uint64_t pack_GetVersionPayload(uint8_t proto, uint8_t hw_maj, uint8_t hw_min, uint8_t hw_var, + uint8_t fw_maj, uint8_t fw_min, uint8_t fw_rev, uint8_t fw_unrel) { + return pack_struct_to_u64(GetVersionPayload(proto, hw_maj, hw_min, hw_var, fw_maj, fw_min, fw_rev, fw_unrel)); + } + + inline uint64_t pack_HeartbeatPayload(uint32_t err, uint8_t state, uint8_t result, uint8_t done, uint8_t res) { + return pack_struct_to_u64(HeartbeatPayload(err, state, result, done, res)); + } + + inline uint64_t pack_GetErrorPayload(uint32_t errors, uint32_t reason) { + return pack_struct_to_u64(GetErrorPayload(errors, reason)); + } + + inline uint64_t pack_RxSdoPayload(uint8_t op, uint16_t ep, uint8_t res, uint32_t val) { + return pack_struct_to_u64(RxSdoPayload(op, ep, res, val)); + } + + inline uint64_t pack_TxSdoPayload(uint8_t res0, uint16_t ep, uint8_t res1, uint32_t val) { + return pack_struct_to_u64(TxSdoPayload(res0, ep, res1, val)); + } + + inline uint64_t pack_AddressPayload(uint8_t nid, uint32_t sn_start, uint16_t sn_end, uint8_t conn_id) { + return pack_struct_to_u64(AddressPayload(nid, sn_start, sn_end, conn_id)); + } + + inline uint64_t pack_EncoderEstimatesPayload(float pos, float vel) { + return pack_struct_to_u64(EncoderEstimatesPayload(pos, vel)); + } + + inline uint64_t pack_GetIqPayload(float set, float meas) { + return pack_struct_to_u64(GetIqPayload(set, meas)); + } + + inline uint64_t pack_GetTemperaturePayload(float fet, float motor) { + return pack_struct_to_u64(GetTemperaturePayload(fet, motor)); + } + + inline uint64_t pack_BusVoltageCurrentPayload(float volt, float curr) { + return pack_struct_to_u64(BusVoltageCurrentPayload(volt, curr)); + } + + inline uint64_t pack_GetTorquesPayload(float tgt, float est) { + return pack_struct_to_u64(GetTorquesPayload(tgt, est)); + } + + inline uint64_t pack_GetPowersPayload(float elec, float mech) { + return pack_struct_to_u64(GetPowersPayload(elec, mech)); + } + + inline uint64_t pack_SetAxisStatePayload(uint32_t req, uint32_t res) { + return pack_struct_to_u64(SetAxisStatePayload(req, res)); + } + + inline uint64_t pack_SetControllerModePayload(uint8_t ctrl, uint8_t input, uint16_t res0, uint32_t res1) { + return pack_struct_to_u64(SetControllerModePayload(ctrl, input, res0, res1)); + } + + inline uint64_t pack_SetInputPosPayload(float pos, int16_t vff, int16_t tff) { + return pack_struct_to_u64(SetInputPosPayload(pos, vff, tff)); + } + + inline uint64_t pack_SetInputVelPayload(float vel, float torque_ff) { + return pack_struct_to_u64(SetInputVelPayload(vel, torque_ff)); + } + + inline uint64_t pack_SetInputTorquePayload(float torque, uint32_t res) { + return pack_struct_to_u64(SetInputTorquePayload(torque, res)); + } + + inline uint64_t pack_SetLimitsPayload(float vel, float curr) { + return pack_struct_to_u64(SetLimitsPayload(vel, curr)); + } + + inline uint64_t pack_SetTrajVelLimitPayload(float vel, uint32_t res) { + return pack_struct_to_u64(SetTrajVelLimitPayload(vel, res)); + } + + inline uint64_t pack_SetTrajAccelLimitsPayload(float accel, float decel) { + return pack_struct_to_u64(SetTrajAccelLimitsPayload(accel, decel)); + } + + inline uint64_t pack_SetTrajInertiaPayload(float inertia, uint32_t res) { + return pack_struct_to_u64(SetTrajInertiaPayload(inertia, res)); + } + + inline uint64_t pack_RebootPayload(uint8_t act, uint8_t r1, uint16_t r2, uint32_t r3) { + return pack_struct_to_u64(RebootPayload(act, r1, r2, r3)); + } + + inline uint64_t pack_ClearErrorsPayload(uint32_t id, uint32_t res) { + return pack_struct_to_u64(ClearErrorsPayload(id, res)); + } + + inline uint64_t pack_SetAbsolutePositionPayload(float pos, uint32_t res) { + return pack_struct_to_u64(SetAbsolutePositionPayload(pos, res)); + } + + inline uint64_t pack_SetPosGainPayload(float gain, uint32_t res) { + return pack_struct_to_u64(SetPosGainPayload(gain, res)); + } + + inline uint64_t pack_SetVelGainsPayload(float gain, float igain) { + return pack_struct_to_u64(SetVelGainsPayload(gain, igain)); + } + + inline GetVersionPayload unpack_GetVersionPayload(uint64_t data) { + return unpack_u64_to_struct(data); + } + + inline HeartbeatPayload unpack_HeartbeatPayload(uint64_t data) { + return unpack_u64_to_struct(data); + } + + inline GetErrorPayload unpack_GetErrorPayload(uint64_t data) { + return unpack_u64_to_struct(data); + } + + inline RxSdoPayload unpack_RxSdoPayload(uint64_t data) { + return unpack_u64_to_struct(data); + } + + inline TxSdoPayload unpack_TxSdoPayload(uint64_t data) { + return unpack_u64_to_struct(data); + } + + inline AddressPayload unpack_AddressPayload(uint64_t data) { + return unpack_u64_to_struct(data); + } + + inline EncoderEstimatesPayload unpack_EncoderEstimatesPayload(uint64_t data) { + return unpack_u64_to_struct(data); + } + + inline GetIqPayload unpack_GetIqPayload(uint64_t data) { + return unpack_u64_to_struct(data); + } + + inline GetTemperaturePayload unpack_GetTemperaturePayload(uint64_t data) { + return unpack_u64_to_struct(data); + } + + inline BusVoltageCurrentPayload unpack_BusVoltageCurrentPayload(uint64_t data) { + return unpack_u64_to_struct(data); + } + + inline GetTorquesPayload unpack_GetTorquesPayload(uint64_t data) { + return unpack_u64_to_struct(data); + } + + inline GetPowersPayload unpack_GetPowersPayload(uint64_t data) { + return unpack_u64_to_struct(data); + } + + inline SetAxisStatePayload unpack_SetAxisStatePayload(uint64_t data) { + return unpack_u64_to_struct(data); + } + + inline SetControllerModePayload unpack_SetControllerModePayload(uint64_t data) { + return unpack_u64_to_struct(data); + } + + inline SetInputPosPayload unpack_SetInputPosPayload(uint64_t data) { + return unpack_u64_to_struct(data); + } + + inline SetInputVelPayload unpack_SetInputVelPayload(uint64_t data) { + return unpack_u64_to_struct(data); + } + + inline SetInputTorquePayload unpack_SetInputTorquePayload(uint64_t data) { + return unpack_u64_to_struct(data); + } + + inline SetLimitsPayload unpack_SetLimitsPayload(uint64_t data) { + return unpack_u64_to_struct(data); + } + + inline SetTrajVelLimitPayload unpack_SetTrajVelLimitPayload(uint64_t data) { + return unpack_u64_to_struct(data); + } + + inline SetTrajAccelLimitsPayload unpack_SetTrajAccelLimitsPayload(uint64_t data) { + return unpack_u64_to_struct(data); + } + + inline SetTrajInertiaPayload unpack_SetTrajInertiaPayload(uint64_t data) { + return unpack_u64_to_struct(data); + } + + inline RebootPayload unpack_RebootPayload(uint64_t data) { + return unpack_u64_to_struct(data); + } + + inline ClearErrorsPayload unpack_ClearErrorsPayload(uint64_t data) { + return unpack_u64_to_struct(data); + } + + inline SetAbsolutePositionPayload unpack_SetAbsolutePositionPayload(uint64_t data) { + return unpack_u64_to_struct(data); + } + + inline SetPosGainPayload unpack_SetPosGainPayload(uint64_t data) { + return unpack_u64_to_struct(data); + } + + inline SetVelGainsPayload unpack_SetVelGainsPayload(uint64_t data) { + return unpack_u64_to_struct(data); + } +} \ No newline at end of file diff --git a/src/lib.cpp b/src/lib.cpp new file mode 100644 index 0000000..abad712 --- /dev/null +++ b/src/lib.cpp @@ -0,0 +1,3 @@ +#include +#include // For memcpy +#include "utils.hpp" \ No newline at end of file diff --git a/target/ros/__init__.cpp b/target/ros/__init__.cpp deleted file mode 100644 index e69de29..0000000 diff --git a/target/seeedxiao/__init__.cpp b/target/seeedxiao/__init__.cpp deleted file mode 100644 index e69de29..0000000 diff --git a/target/stm/__init__.cpp b/target/stm/__init__.cpp deleted file mode 100644 index e69de29..0000000 diff --git a/test/test_can_header.cpp b/test/test_can_header.cpp new file mode 100644 index 0000000..0d2da1f --- /dev/null +++ b/test/test_can_header.cpp @@ -0,0 +1,95 @@ +#include +#include "utils.hpp" + +using namespace CanHeader; + +class CanHeaderEncode : public ::testing::Test { + protected: + void SetUp() override { + // Runs before each TEST_F + } + + void TearDown() override { + // Runs after each TEST_F + } + + const uint8_t testNodeId = 0x15; + const uint16_t testNodeHeader = 0x2a0; + const uint16_t testBroadNodeHeader = 0x7e0; + const uint8_t testOutBoundNodeId = 0x4a; +}; + +TEST_F(CanHeaderEncode, NormalIdNoCmd) { + uint16_t header = encode(HeaderInfo(testNodeId, CmdMap::Get_Version)); + ASSERT_EQ(header, testNodeHeader) << "[Error] Incorrect Node ID Shifting\n"; +} + +TEST_F(CanHeaderEncode, BroadCastIdNoCmd) { + uint16_t header = encode(HeaderInfo(broadcastNodeId, CmdMap::Get_Version)); + ASSERT_EQ(header, testBroadNodeHeader) << "[Error] Incorrect Broad Cast Node ID Shifting\n"; +} + +TEST_F(CanHeaderEncode, OutBoundIdNoCmd) { + uint16_t header = encode(HeaderInfo(testOutBoundNodeId, CmdMap::Get_Version)); + ASSERT_EQ(header, errCode16) << "[Error] Out-of-bound Node ID should return error\n"; +} + +class CanHeaderDecode : public ::testing::Test { + protected: + void SetUp() override { + // Runs before each TEST_F + } + + void TearDown() override { + // Runs after each TEST_F + } + + const uint8_t testNodeId = 0x15; + const uint16_t testNodeHeader = 0x2a0; + const uint16_t testBroadNodeHeader = 0x7e0; + const uint16_t testOutBoundNodeHeader = 0x940; +}; + +TEST_F(CanHeaderDecode, NormalIdNoCmd) { + HeaderInfo header = decode(testNodeHeader); + ASSERT_EQ(header, HeaderInfo(testNodeId, CmdMap::Get_Version)) << "[Error] Incorrect decode"; +} + +TEST_F(CanHeaderDecode, BroadCastIdNoCmd) { + HeaderInfo header = decode(testBroadNodeHeader); + ASSERT_EQ(header, HeaderInfo(broadcastNodeId, CmdMap::Get_Version)) << "[Error] Incorrect decode"; +} + +TEST_F(CanHeaderDecode, OutBoundIdNoCmd) { + HeaderInfo header = decode(testOutBoundNodeHeader); + ASSERT_EQ(header, HeaderInfo(errorCode8, static_cast(errorCode8))) << "[Error] Incorrect decode"; +} + +class CanCmdDecode : public ::testing::Test { + protected: + void SetUp() override { + // Runs before each TEST_F + } + + void TearDown() override { + // Runs after each TEST_F + } + + const uint8_t test_cmd = static_cast(CmdMap::Clear_Errors); + const uint8_t test_outbound_cmd = 0x20; + const uint8_t test_inbound_invalid_cmd = 0x08; +}; + +TEST_F(CanCmdDecode, ValidCmd) { + bool valid_cmd = is_defined(test_cmd); + ASSERT_EQ(valid_cmd, true) << "[Error] Incorrect Cmd Identification"; +} + +TEST_F(CanCmdDecode, InvalidCmd) { + bool valid_cmd = is_defined(test_outbound_cmd); + ASSERT_EQ(valid_cmd, false) << "[Error] Incorrect Cmd Identification"; +} +TEST_F(CanCmdDecode, InvalidInboundCmd) { + bool valid_cmd = is_defined(test_inbound_invalid_cmd); + ASSERT_EQ(valid_cmd, false) << "[Error] Incorrect Cmd Identification"; +} \ No newline at end of file diff --git a/test/test_can_msg.cpp b/test/test_can_msg.cpp new file mode 100644 index 0000000..4fadd20 --- /dev/null +++ b/test/test_can_msg.cpp @@ -0,0 +1,187 @@ +#include +#include "utils.hpp" + +using namespace CanPayload; + +class PackFixture : public ::testing::Test { +protected: + void SetUp() override {} + void TearDown() override {} +}; + +TEST_F(PackFixture, Pack_GetVersionPayload) { + auto val = pack_GetVersionPayload(207, 0, 188, 173, 34, 61, 203, 186); + auto result = unpack_GetVersionPayload(val); + EXPECT_EQ(result.protocol_version, 207); + EXPECT_EQ(result.hw_version_major, 0); + EXPECT_EQ(result.hw_version_minor, 188); + EXPECT_EQ(result.hw_version_variant, 173); + EXPECT_EQ(result.fw_version_major, 34); + EXPECT_EQ(result.fw_version_minor, 61); + EXPECT_EQ(result.fw_version_revision, 203); + EXPECT_EQ(result.fw_version_unreleased, 186); +} +TEST_F(PackFixture, Pack_HeartbeatPayload) { + auto val = pack_HeartbeatPayload(4106194757, 240, 176, 144, 232); + auto result = unpack_HeartbeatPayload(val); + EXPECT_EQ(result.axis_error, 4106194757); + EXPECT_EQ(result.axis_state, 240); + EXPECT_EQ(result.procedure_result, 176); + EXPECT_EQ(result.trajectory_done, 144); + EXPECT_EQ(result.reserved, 232); +} +TEST_F(PackFixture, Pack_GetErrorPayload) { + auto val = pack_GetErrorPayload(1126843295, 779179158); + auto result = unpack_GetErrorPayload(val); + EXPECT_EQ(result.active_errors, 1126843295); + EXPECT_EQ(result.disarm_reason, 779179158); +} +TEST_F(PackFixture, Pack_RxSdoPayload) { + auto val = pack_RxSdoPayload(82, 17176, 63, 2471323060); + auto result = unpack_RxSdoPayload(val); + EXPECT_EQ(result.opcode, 82); + EXPECT_EQ(result.endpoint_id, 17176); + EXPECT_EQ(result.reserved, 63); + EXPECT_EQ(result.value, 2471323060); +} +TEST_F(PackFixture, Pack_TxSdoPayload) { + auto val = pack_TxSdoPayload(181, 14034, 199, 1726867364); + auto result = unpack_TxSdoPayload(val); + EXPECT_EQ(result.reserved0, 181); + EXPECT_EQ(result.endpoint_id, 14034); + EXPECT_EQ(result.reserved1, 199); + EXPECT_EQ(result.value, 1726867364); +} +TEST_F(PackFixture, Pack_AddressPayload) { + auto val = pack_AddressPayload(158, 1687132453, 13548, 83); + auto result = unpack_AddressPayload(val); + EXPECT_EQ(result.node_id, 158); + EXPECT_EQ(result.serial_number_start, 1687132453); + EXPECT_EQ(result.serial_number_end, 13548); + EXPECT_EQ(result.connection_id, 83); +} +TEST_F(PackFixture, Pack_EncoderEstimatesPayload) { + auto val = pack_EncoderEstimatesPayload(939.9461059570312, -343.69403076171875); + auto result = unpack_EncoderEstimatesPayload(val); + EXPECT_EQ(result.pos_estimate, 939.9461059570312); + EXPECT_EQ(result.vel_estimate, -343.69403076171875); +} +TEST_F(PackFixture, Pack_GetIqPayload) { + auto val = pack_GetIqPayload(-753.4401245117188, -583.8773193359375); + auto result = unpack_GetIqPayload(val); + EXPECT_EQ(result.iq_setpoint, -753.4401245117188); + EXPECT_EQ(result.iq_measured, -583.8773193359375); +} +TEST_F(PackFixture, Pack_GetTemperaturePayload) { + auto val = pack_GetTemperaturePayload(179.77752685546875, 706.0946044921875); + auto result = unpack_GetTemperaturePayload(val); + EXPECT_EQ(result.fet_temperature, 179.77752685546875); + EXPECT_EQ(result.motor_temperature, 706.0946044921875); +} +TEST_F(PackFixture, Pack_BusVoltageCurrentPayload) { + auto val = pack_BusVoltageCurrentPayload(934.9306640625, 522.6814575195312); + auto result = unpack_BusVoltageCurrentPayload(val); + EXPECT_EQ(result.bus_voltage, 934.9306640625); + EXPECT_EQ(result.bus_current, 522.6814575195312); +} +TEST_F(PackFixture, Pack_GetTorquesPayload) { + auto val = pack_GetTorquesPayload(754.4702758789062, 615.1375732421875); + auto result = unpack_GetTorquesPayload(val); + EXPECT_EQ(result.torque_target, 754.4702758789062); + EXPECT_EQ(result.torque_estimate, 615.1375732421875); +} +TEST_F(PackFixture, Pack_GetPowersPayload) { + auto val = pack_GetPowersPayload(-879.1647338867188, 625.155029296875); + auto result = unpack_GetPowersPayload(val); + EXPECT_EQ(result.electrical_power, -879.1647338867188); + EXPECT_EQ(result.mechanical_power, 625.155029296875); +} +TEST_F(PackFixture, Pack_SetAxisStatePayload) { + auto val = pack_SetAxisStatePayload(3711643247, 3299453749); + auto result = unpack_SetAxisStatePayload(val); + EXPECT_EQ(result.axis_requested_state, 3711643247); + EXPECT_EQ(result.reserved, 3299453749); +} +TEST_F(PackFixture, Pack_SetControllerModePayload) { + auto val = pack_SetControllerModePayload(93, 193, 55955, 1026479264); + auto result = unpack_SetControllerModePayload(val); + EXPECT_EQ(result.control_mode, 93); + EXPECT_EQ(result.input_mode, 193); + EXPECT_EQ(result.reserved0, 55955); + EXPECT_EQ(result.reserved1, 1026479264); +} +TEST_F(PackFixture, Pack_SetInputPosPayload) { + auto val = pack_SetInputPosPayload(751.236572265625, -7048, -2865); + auto result = unpack_SetInputPosPayload(val); + EXPECT_EQ(result.input_pos, 751.236572265625); + EXPECT_EQ(result.vel_ff, -7048); + EXPECT_EQ(result.torque_ff, -2865); +} +TEST_F(PackFixture, Pack_SetInputVelPayload) { + auto val = pack_SetInputVelPayload(500.6752624511719, -409.4150390625); + auto result = unpack_SetInputVelPayload(val); + EXPECT_EQ(result.input_vel, 500.6752624511719); + EXPECT_EQ(result.input_torque_ff, -409.4150390625); +} +TEST_F(PackFixture, Pack_SetInputTorquePayload) { + auto val = pack_SetInputTorquePayload(979.186767578125, 1885310099); + auto result = unpack_SetInputTorquePayload(val); + EXPECT_EQ(result.input_torque, 979.186767578125); + EXPECT_EQ(result.reserved, 1885310099); +} +TEST_F(PackFixture, Pack_SetLimitsPayload) { + auto val = pack_SetLimitsPayload(-633.29638671875, 108.10169982910156); + auto result = unpack_SetLimitsPayload(val); + EXPECT_EQ(result.velocity_limit, -633.29638671875); + EXPECT_EQ(result.current_limit, 108.10169982910156); +} +TEST_F(PackFixture, Pack_SetTrajVelLimitPayload) { + auto val = pack_SetTrajVelLimitPayload(-996.68505859375, 4074180013); + auto result = unpack_SetTrajVelLimitPayload(val); + EXPECT_EQ(result.traj_vel_limit, -996.68505859375); + EXPECT_EQ(result.reserved, 4074180013); +} +TEST_F(PackFixture, Pack_SetTrajAccelLimitsPayload) { + auto val = pack_SetTrajAccelLimitsPayload(-635.3016357421875, -715.0675659179688); + auto result = unpack_SetTrajAccelLimitsPayload(val); + EXPECT_EQ(result.traj_accel_limit, -635.3016357421875); + EXPECT_EQ(result.traj_decel_limit, -715.0675659179688); +} +TEST_F(PackFixture, Pack_SetTrajInertiaPayload) { + auto val = pack_SetTrajInertiaPayload(255.33192443847656, 1464517241); + auto result = unpack_SetTrajInertiaPayload(val); + EXPECT_EQ(result.traj_inertia, 255.33192443847656); + EXPECT_EQ(result.reserved, 1464517241); +} +TEST_F(PackFixture, Pack_RebootPayload) { + auto val = pack_RebootPayload(98, 113, 53258, 1229868823); + auto result = unpack_RebootPayload(val); + EXPECT_EQ(result.action, 98); + EXPECT_EQ(result.reserved1, 113); + EXPECT_EQ(result.reserved2, 53258); + EXPECT_EQ(result.reserved3, 1229868823); +} +TEST_F(PackFixture, Pack_ClearErrorsPayload) { + auto val = pack_ClearErrorsPayload(3748010892, 1473327532); + auto result = unpack_ClearErrorsPayload(val); + EXPECT_EQ(result.identify, 3748010892); + EXPECT_EQ(result.reserved, 1473327532); +} +TEST_F(PackFixture, Pack_SetAbsolutePositionPayload) { + auto val = pack_SetAbsolutePositionPayload(-565.8623046875, 33672196); + auto result = unpack_SetAbsolutePositionPayload(val); + EXPECT_EQ(result.position, -565.8623046875); + EXPECT_EQ(result.reserved, 33672196); +} +TEST_F(PackFixture, Pack_SetPosGainPayload) { + auto val = pack_SetPosGainPayload(-497.8382263183594, 2656791018); + auto result = unpack_SetPosGainPayload(val); + EXPECT_EQ(result.pos_gain, -497.8382263183594); + EXPECT_EQ(result.reserved, 2656791018); +} +TEST_F(PackFixture, Pack_SetVelGainsPayload) { + auto val = pack_SetVelGainsPayload(-323.467041015625, 368.3490295410156); + auto result = unpack_SetVelGainsPayload(val); + EXPECT_EQ(result.vel_gain, -323.467041015625); + EXPECT_EQ(result.vel_integrator_gain, 368.3490295410156); +} \ No newline at end of file