diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index fe8490c2e..c784b2310 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -22,16 +22,8 @@ jobs: lfs: "true" # This makes sure that $GITHUB_WORKSPACE is the catkin workspace path path: "src/mrover" - - name: Ensure Python Requirements - run: . /home/mrover/catkin_ws/src/mrover/venv/bin/activate && pip install --no-cache-dir -e "$GITHUB_WORKSPACE/src/mrover[dev]" - name: Style Check run: . /home/mrover/catkin_ws/src/mrover/venv/bin/activate && cd $GITHUB_WORKSPACE/src/mrover/ && ./style.sh - - name: Update ROS APT - if: github.event.pull_request.draft == false - run: runuser -u mrover -- rosdep update - - name: Ensure ROS APT Requirements - if: github.event.pull_request.draft == false - run: runuser -u mrover -- rosdep install --from-paths "$GITHUB_WORKSPACE/src" --ignore-src -r -y --rosdistro noetic - name: Copy Catkin Profiles if: github.event.pull_request.draft == false run: rsync -r $GITHUB_WORKSPACE/src/mrover/ansible/roles/build/files/profiles $GITHUB_WORKSPACE/.catkin_tools @@ -42,7 +34,7 @@ jobs: if: github.event.pull_request.draft == false && github.event.pull_request.base.ref != 'refs/heads/master' run: . /opt/ros/noetic/setup.sh && . /home/mrover/catkin_ws/src/mrover/venv/bin/activate && catkin build - name: Build With Clang Tidy - if : github.event_name == 'push' && github.event.pull_request.base.ref == 'refs/heads/master' + if: github.event_name == 'push' && github.event.pull_request.base.ref == 'refs/heads/master' run: . /opt/ros/noetic/setup.sh && . /home/mrover/catkin_ws/src/mrover/venv/bin/activate && catkin build -DCMAKE_CXX_CLANG_TIDY=clang-tidy-16 - name: Test if: github.event.pull_request.draft == false diff --git a/.gitmodules b/.gitmodules index e5319baf4..a5bd0ee01 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,20 +1,10 @@ -[submodule "deps/libde265"] - path = deps/libde265 - url = https://github.com/strukturag/libde265.git - shallow = true - branch = v1.0.15 [submodule "deps/dawn"] path = deps/dawn url = https://dawn.googlesource.com/dawn shallow = true - branch = chromium/6108 -[submodule "deps/emsdk"] - path = deps/emsdk - url = git@github.com:emscripten-core/emsdk.git - shallow = true - branch = 3.1.53 + branch = chromium/6376 [submodule "deps/manif"] path = deps/manif url = https://github.com/artivis/manif.git shallow = true - branch = master + branch = devel diff --git a/CMakeLists.txt b/CMakeLists.txt index e12e6a58b..e02e2f0a7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,6 +1,18 @@ cmake_minimum_required(VERSION 3.16) project(mrover VERSION 2024.0.0 LANGUAGES CXX) +option(MROVER_IS_CI "Build for CI" OFF) +option(MROVER_RUN_CLANG_TIDY "Run clang-tidy" OFF) +option(MROVER_IS_JETSON "Build for the Jetson" OFF) + +set(CMAKE_CXX_STANDARD 23) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) # Generate compile_commands.json for clangd + +### ============= ### +### OS & Compiler ### +### ============= ### + if (APPLE) # Ensures that homebrew packages are never used over miniforge packages set(CMAKE_IGNORE_PATH /opt/homebrew) @@ -15,23 +27,30 @@ if (APPLE) link_libraries(fmt::fmt) else () # TODO(quintin): Fix this - find_package(TBB REQUIRED) - link_libraries(TBB::tbb) + find_package(TBB QUIET) + if (TBB_FOUND) + link_libraries(TBB::tbb) + endif () endif () -include_directories(BEFORE SYSTEM src/preload) +if (NOT APPLE AND CMAKE_CXX_COMPILER_ID STREQUAL "Clang") # Link with LLVM lld instead of GNU ld when using Clang on Linux, it is faster + add_link_options(-fuse-ld=lld) +endif () -set(CMAKE_CXX_STANDARD 23) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_EXPORT_COMPILE_COMMANDS ON) # Generate compile_commands.json for clangd -if (NOT APPLE AND CMAKE_CXX_COMPILER_ID STREQUAL "Clang") - add_link_options(-fuse-ld=lld) # LLVM lld is faster than GNU ld +if (CMAKE_CXX_COMPILER_ID STREQUAL "GNU" OR CMAKE_CXX_COMPILER_ID STREQUAL "Clang") + set(MROVER_CPP_COMPILE_OPTIONS -Wall -Wextra -pedantic -Wno-missing-field-initializers) + if (MROVER_IS_CI) # Make warnings errors in CI + list(APPEND MROVER_CPP_COMPILE_OPTIONS -Werror) + endif () endif () -set(MROVER_CPP_COMPILE_OPTIONS -Wall -Wextra -pedantic -Wno-missing-field-initializers) -if (MROVER_CI) - list(APPEND MROVER_CPP_COMPILE_OPTIONS -Werror) + +if (MROVER_IS_JETSON) + add_definitions(-DMROVER_IS_JETSON) endif () +# Inject header files that get included before anything else, including system headers +include_directories(BEFORE SYSTEM src/preload) + include(cmake/macros.cmake) # ROS packages list @@ -48,15 +67,19 @@ set(MROVER_ROS_DEPENDENCIES tf2 tf2_ros tf2_geometry_msgs + actionlib_msgs ) extract_filenames(msg/*.msg MROVER_MESSAGE_FILES) extract_filenames(srv/*.srv MROVER_SERVICE_FILES) +extract_filenames(action/*.action MROVER_ACTION_FILES) + set(MROVER_MESSAGE_DEPENDENCIES std_msgs sensor_msgs + actionlib_msgs ) set(MROVER_PARAMETERS @@ -79,7 +102,7 @@ if (ZED_FOUND) set(CMAKE_CUDA_STANDARD 17) set(CMAKE_CUDA_STANDARD_REQUIRED ON) set(CMAKE_CUDA_SEPARABLE_COMPILATION ON) - set(CMAKE_CUDA_FLAGS "--diag-suppress 108,68") + set(CMAKE_CUDA_FLAGS "--diag-suppress 20012") # Jetson Xavier NX/AGX has Volta 7.2 architecture # Perception Laptop (A4000, Quadro version of RTX 3080) has Ampere 8.6 architecture set(CMAKE_CUDA_ARCHITECTURES 72 86) @@ -104,6 +127,8 @@ add_message_files(FILES ${MROVER_MESSAGE_FILES}) add_service_files(FILES ${MROVER_SERVICE_FILES}) +add_action_files(DIRECTORY action FILES ${MROVER_ACTION_FILES}) + generate_messages(DEPENDENCIES ${MROVER_MESSAGE_DEPENDENCIES}) generate_dynamic_reconfigure_options(${MROVER_PARAMETERS}) @@ -124,48 +149,44 @@ target_link_libraries(lie PUBLIC MANIF::manif) ## ESW -if (NOT APPLE) - mrover_add_vendor_header_only_library(moteus src/esw/mjbots) - mrover_add_header_only_library(can_device src/esw/can_device) - mrover_add_library(motor_library src/esw/motor_library/*.cpp src/esw/motor_library) - target_link_libraries(motor_library PUBLIC can_device moteus) - - if (NetLink_FOUND AND NetLinkRoute_FOUND) - mrover_add_nodelet(can_driver src/esw/can_driver/*.cpp src/esw/can_driver src/esw/can_driver/pch.hpp) - mrover_nodelet_link_libraries(can_driver nl-3 nl-route-3) - # TODO(quintin): Fix this CMake hard coding - #mrover_nodelet_link_libraries(can ${NetLink_LIBRARIES} ${NetLinkRoute_LIBRARIES}}) - mrover_nodelet_include_directories(can_driver /usr/include/libnl3) - endif () +mrover_add_vendor_header_only_library(moteus deps/mjbots) +mrover_add_header_only_library(can_device src/esw/can_device) +mrover_add_library(motor_library src/esw/motor_library/*.cpp src/esw/motor_library) +target_link_libraries(motor_library PUBLIC can_device moteus) - macro(mrover_add_esw_bridge_node name sources) - mrover_add_node(${name} ${sources}) - target_link_libraries(${name} PRIVATE can_device motor_library) - endmacro() - - mrover_add_esw_bridge_node(arm_hw_bridge src/esw/arm_hw_bridge/*.cpp) - mrover_add_esw_bridge_node(arm_translator_bridge src/esw/arm_translator_bridge/*.cpp src/esw/arm_translator_bridge/*.hpp) - mrover_add_esw_bridge_node(cache_bridge src/esw/cache_bridge/*.cpp) - mrover_add_esw_bridge_node(drive_bridge src/esw/drive_bridge/*.cpp) - mrover_add_esw_bridge_node(led_hw_bridge src/esw/led_hw_bridge/*.cpp) - mrover_add_esw_bridge_node(led src/esw/led/*.cpp) - mrover_add_esw_bridge_node(mast_gimbal_bridge src/esw/mast_gimbal_bridge/*.cpp) - mrover_add_esw_bridge_node(pdb_bridge src/esw/pdb_bridge/*.cpp) - mrover_add_esw_bridge_node(sa_hw_bridge src/esw/sa_hw_bridge/*.cpp) - mrover_add_esw_bridge_node(sa_translator_bridge src/esw/sa_translator_bridge/*.cpp) - mrover_add_esw_bridge_node(science_bridge src/esw/science_bridge/*.cpp) - mrover_add_esw_bridge_node(brushless_test_bridge src/esw/brushless_test_bridge/*.cpp motor_library) - mrover_add_esw_bridge_node(brushed_test_bridge src/esw/brushed_test_bridge/*.cpp) - mrover_add_esw_bridge_node(test_arm_bridge src/esw/test_arm_bridge/*.cpp) - mrover_add_esw_bridge_node(pdb_test_bridge src/esw/pdb_test_bridge/*.cpp) - mrover_add_esw_bridge_node(arm_position_test_bridge src/esw/arm_position_test_bridge/*.cpp) - # mrover_add_esw_bridge_node(sa_sensor src/esw/fw/dirt_sensor/dirt_sensor_ros/dirt_sensor_ros.ino) +if (NetLink_FOUND AND NetLinkRoute_FOUND) + mrover_add_nodelet(can_driver src/esw/can_driver/*.cpp src/esw/can_driver src/esw/can_driver/pch.hpp) + mrover_nodelet_link_libraries(can_driver ${NetLink_LIBRARIES} ${NetLinkRoute_LIBRARIES}) + mrover_nodelet_include_directories(can_driver ${NetLink_INCLUDE_DIRS} ${NetLinkRoute_INCLUDE_DIRS}) endif () +macro(mrover_add_esw_bridge_node name sources) + mrover_add_node(${name} ${sources}) + target_link_libraries(${name} PRIVATE can_device motor_library) +endmacro() + +mrover_add_esw_bridge_node(arm_hw_bridge src/esw/arm_hw_bridge/*.cpp) +mrover_add_esw_bridge_node(arm_translator_bridge src/esw/arm_translator_bridge/*.cpp src/esw/arm_translator_bridge/*.hpp) +mrover_add_esw_bridge_node(cache_bridge src/esw/cache_bridge/*.cpp) +mrover_add_esw_bridge_node(drive_bridge src/esw/drive_bridge/*.cpp) +mrover_add_esw_bridge_node(led_hw_bridge src/esw/led_hw_bridge/*.cpp) +mrover_add_esw_bridge_node(led src/esw/led/*.cpp) +mrover_add_esw_bridge_node(mast_gimbal_bridge src/esw/mast_gimbal_bridge/*.cpp) +mrover_add_esw_bridge_node(pdb_bridge src/esw/pdb_bridge/*.cpp) +mrover_add_esw_bridge_node(sa_bridge src/esw/sa_bridge/*.cpp) +mrover_add_esw_bridge_node(science_bridge src/esw/science_bridge/*.cpp) +mrover_add_esw_bridge_node(brushless_test_bridge src/esw/brushless_test_bridge/*.cpp) +mrover_add_esw_bridge_node(brushed_test_bridge src/esw/brushed_test_bridge/*.cpp) +mrover_add_esw_bridge_node(science_test_bridge src/esw/science_test_bridge/*.cpp) +mrover_add_esw_bridge_node(test_arm_bridge src/esw/test_arm_bridge/*.cpp) +mrover_add_esw_bridge_node(pdb_test_bridge src/esw/pdb_test_bridge/*.cpp) +# mrover_add_esw_bridge_node(arm_position_test_bridge src/esw/arm_position_test_bridge/*.cpp) +# mrover_add_esw_bridge_node(sa_sensor src/esw/fw/dirt_sensor/dirt_sensor_ros/dirt_sensor_ros.ino) + ## Perception -mrover_add_library(streaming src/esw/streaming/*.cpp src/esw/streaming) -target_compile_definitions(streaming PUBLIC BOOST_ASIO_NO_DEPRECATED) +mrover_add_library(websocket_server src/esw/websocket_server/*.cpp src/esw/websocket_server) +target_compile_definitions(websocket_server PUBLIC BOOST_ASIO_NO_DEPRECATED) mrover_add_nodelet(zed_tag_detector src/perception/tag_detector/zed/*.cpp src/perception/tag_detector/zed src/perception/tag_detector/zed/pch.hpp) mrover_nodelet_link_libraries(zed_tag_detector opencv_core opencv_objdetect opencv_aruco opencv_imgproc lie) @@ -177,19 +198,16 @@ mrover_add_nodelet(usb_camera src/perception/usb_camera/*.cpp src/perception/usb mrover_nodelet_link_libraries(usb_camera opencv_core opencv_objdetect opencv_aruco opencv_imgproc opencv_highgui) if (CUDA_FOUND) -# mrover_add_node(nv_vid_codec_h265_enc src/esw/nv_vid_codec_h265_enc/*.c*) -# target_link_libraries(nv_vid_codec_h265_enc PUBLIC cuda nvidia-encode opencv_core opencv_imgproc streaming) -# target_include_directories(nv_vid_codec_h265_enc SYSTEM PUBLIC deps/nvenc) + # mrover_add_node(nv_vid_codec_h265_enc src/esw/nv_vid_codec_h265_enc/*.c*) + # target_link_libraries(nv_vid_codec_h265_enc PUBLIC cuda nvidia-encode opencv_core opencv_imgproc streaming) + # target_include_directories(nv_vid_codec_h265_enc SYSTEM PUBLIC deps/nvenc) endif () -mrover_add_nodelet(nv_gst_h265_enc src/esw/nv_gst_h265_enc/*.c* src/esw/nv_gst_h265_enc src/esw/nv_gst_h265_enc/pch.hpp) -mrover_nodelet_link_libraries(nv_gst_h265_enc PRIVATE streaming gstreamer-1.0 gstapp-1.0 glib-2.0 gobject-2.0) -if (MROVER_IS_JETSON) - mrover_nodelet_defines(nv_gst_h265_enc MROVER_IS_JETSON) - mrover_nodelet_include_directories(nv_gst_h265_enc /usr/lib/aarch64-linux-gnu/glib-2.0/include) +if (Gst_FOUND AND GstApp_FOUND AND LibUsb_FOUND AND LibUdev_FOUND) + mrover_add_nodelet(gst_websocket_streamer src/esw/gst_websocket_streamer/*.c* src/esw/gst_websocket_streamer src/esw/gst_websocket_streamer/pch.hpp) + mrover_nodelet_link_libraries(gst_websocket_streamer PRIVATE websocket_server ${Gst_LIBRARIES} ${GstApp_LIBRARIES} ${LibUsb_LIBRARIES} ${LibUdev_LIBRARIES} opencv_core opencv_imgproc) + mrover_nodelet_include_directories(gst_websocket_streamer ${Gst_INCLUDE_DIRS} ${GstApp_INCLUDE_DIRS} ${LibUsb_INCLUDE_DIRS} ${LibUdev_INCLUDE_DIRS}) endif () -# TODO(quintin): Fix this CMake hard coding -mrover_nodelet_include_directories(nv_gst_h265_enc /usr/include/gstreamer-1.0 /usr/include/glib-2.0 /usr/lib/x86_64-linux-gnu/glib-2.0/include) if (ZED_FOUND) mrover_add_nodelet(object_detector src/perception/object_detector/*.c* src/perception/object_detector src/perception/object_detector/pch.hpp) @@ -214,6 +232,7 @@ endif () ## Teleoperation mrover_add_node(arm_controller src/teleoperation/arm_controller/*.cpp) +target_link_libraries(arm_controller PRIVATE lie) ## Simulator @@ -224,8 +243,6 @@ if (MROVER_BUILD_SIM) mrover_nodelet_defines(simulator BOOST_THREAD_PROVIDES_FUTURE) endif () -# TODO - ### ======= ### ### Testing ### ### ======= ### diff --git a/action/ArmAction.action b/action/ArmAction.action new file mode 100644 index 000000000..9cc4f5321 --- /dev/null +++ b/action/ArmAction.action @@ -0,0 +1,3 @@ +string name +--- +--- diff --git a/ansible/roles/build/files/profiles/ci/config.yaml b/ansible/roles/build/files/profiles/ci/config.yaml index 607178fe5..c0346527b 100644 --- a/ansible/roles/build/files/profiles/ci/config.yaml +++ b/ansible/roles/build/files/profiles/ci/config.yaml @@ -5,10 +5,10 @@ catkin_make_args: [ ] cmake_args: - -DCMAKE_BUILD_TYPE=Release - -DCMAKE_C_COMPILER=clang-16 - - -DCMAKE_CXX_FLAGS=-pipe + - -DCMAKE_CXX_FLAGS=-pipe -fgnuc-version=13.1.0 - -DCMAKE_CXX_COMPILER=clang++-16 - - -DCMAKE_CUDA_HOST_COMPILER=clang++-16 - - -DMROVER_CI=ON + - -DCMAKE_CUDA_HOST_COMPILER=g++-9 + - -DMROVER_IS_CI=ON - -Wno-dev devel_layout: linked devel_space: devel diff --git a/ansible/roles/build/files/profiles/debug/config.yaml b/ansible/roles/build/files/profiles/debug/config.yaml index 71005790a..434ae5ae9 100644 --- a/ansible/roles/build/files/profiles/debug/config.yaml +++ b/ansible/roles/build/files/profiles/debug/config.yaml @@ -5,11 +5,11 @@ catkin_make_args: [ ] cmake_args: - -DCMAKE_BUILD_TYPE=Debug - -DCMAKE_C_COMPILER=clang-16 - - -DCMAKE_CXX_FLAGS=-pipe + - -DCMAKE_CXX_FLAGS=-pipe -fgnuc-version=13.1.0 - -DCMAKE_CXX_COMPILER=clang++-16 - -DCMAKE_CXX_COMPILER_LAUNCHER=ccache - -DCMAKE_CUDA_COMPILER_LAUNCHER=ccache - - -DCMAKE_CUDA_HOST_COMPILER=clang++-16 + - -DCMAKE_CUDA_HOST_COMPILER=g++-9 - -Wno-dev devel_layout: linked devel_space: devel diff --git a/ansible/roles/build/files/profiles/release/config.yaml b/ansible/roles/build/files/profiles/release/config.yaml index f0c29e538..ef35c5cb7 100644 --- a/ansible/roles/build/files/profiles/release/config.yaml +++ b/ansible/roles/build/files/profiles/release/config.yaml @@ -5,11 +5,11 @@ catkin_make_args: [ ] cmake_args: - -DCMAKE_BUILD_TYPE=Release - -DCMAKE_C_COMPILER=clang-16 - - -DCMAKE_CXX_FLAGS=-march=native -pipe + - -DCMAKE_CXX_FLAGS=-march=native -pipe -fgnuc-version=13.1.0 - -DCMAKE_CXX_COMPILER=clang++-16 - -DCMAKE_CXX_COMPILER_LAUNCHER=ccache - -DCMAKE_CUDA_COMPILER_LAUNCHER=ccache - - -DCMAKE_CUDA_HOST_COMPILER=clang++-16 + - -DCMAKE_CUDA_HOST_COMPILER=g++-9 - -DCMAKE_INTERPROCEDURAL_OPTIMIZATION=ON - -Wno-dev devel_layout: linked diff --git a/ansible/roles/build/tasks/main.yml b/ansible/roles/build/tasks/main.yml index 18003e214..c2c6e022f 100644 --- a/ansible/roles/build/tasks/main.yml +++ b/ansible/roles/build/tasks/main.yml @@ -96,6 +96,8 @@ - libopencv-dev - libgstreamer1.0-dev - libgstreamer-plugins-base1.0-dev + - vainfo + - x264 - name: Install Local APT Packages become: True @@ -190,6 +192,14 @@ link: /usr/bin/lld priority: 160 +- name: Set clang-tidy 16 as Default + become: True + alternatives: + name: clang-tidy + path: /usr/bin/clang-tidy-16 + link: /usr/bin/clang-tidy + priority: 160 + - name: Setup Python Virtual Environment pip: name: @@ -197,3 +207,8 @@ - "{{ catkin_workspace }}/src/mrover[dev]" virtualenv: "{{ catkin_workspace }}/src/mrover/venv" virtualenv_command: /usr/bin/python3.10 -m venv + +- name: Install Chromium + become: True + snap: + name: chromium diff --git a/ansible/roles/ci/tasks/main.yml b/ansible/roles/ci/tasks/main.yml index 075e88d01..81d80a3b2 100644 --- a/ansible/roles/ci/tasks/main.yml +++ b/ansible/roles/ci/tasks/main.yml @@ -79,6 +79,8 @@ - libnl-route-3-dev - libtbb-dev - libopencv-dev + - libgstreamer1.0-dev + - libgstreamer-plugins-base1.0-dev - name: Install Local APT Packages become: True @@ -173,6 +175,14 @@ link: /usr/bin/lld priority: 160 +- name: Set clang-tidy 16 as Default + become: True + alternatives: + name: clang-tidy + path: /usr/bin/clang-tidy-16 + link: /usr/bin/clang-tidy + priority: 160 + - name: Setup Python Virtual Environment pip: name: diff --git a/ansible/roles/dev/files/home/.zshrc b/ansible/roles/dev/files/home/.zshrc index 9f5ba8429..6cf4f05bd 100644 --- a/ansible/roles/dev/files/home/.zshrc +++ b/ansible/roles/dev/files/home/.zshrc @@ -70,7 +70,7 @@ ZSH_THEME="mrover" # Custom plugins may be added to $ZSH_CUSTOM/plugins/ # Example format: plugins=(rails git textmate ruby lighthouse) # Add wisely, as too many plugins slow down shell startup. -plugins=(git virtualenvwrapper fzf) +plugins=(git virtualenvwrapper fzf zsh-autosuggestions zsh-syntax-highlighting) source $ZSH/oh-my-zsh.sh diff --git a/ansible/roles/dev/tasks/main.yml b/ansible/roles/dev/tasks/main.yml index 641a81aad..e064a2571 100644 --- a/ansible/roles/dev/tasks/main.yml +++ b/ansible/roles/dev/tasks/main.yml @@ -13,6 +13,16 @@ dest: ~/ recursive: true +- name: ZSH Automatic Suggestions Plugin + git: + repo: https://github.com/zsh-users/zsh-autosuggestions + dest: ~/.oh-my-zsh/custom/plugins/zsh-autosuggestions + +- name: ZSH Syntax Highlighting Plugin + git: + repo: https://github.com/zsh-users/zsh-syntax-highlighting + dest: ~/.oh-my-zsh/custom/plugins/zsh-syntax-highlighting + - name: Use ZSH as a default shell become: true command: chsh --shell /usr/bin/zsh {{ ansible_user_id }} diff --git a/ansible/roles/esw/tasks/main.yml b/ansible/roles/esw/tasks/main.yml index c734c770b..9ae41142b 100644 --- a/ansible/roles/esw/tasks/main.yml +++ b/ansible/roles/esw/tasks/main.yml @@ -13,6 +13,13 @@ become: true command: udevadm trigger --subsystem-match=tty +- name: Install fdcanusb Binary + become: true + copy: + src: files/bin/fdcanusb_daemon + dest: /usr/local/bin/ + mode: 0755 + - name: Install Moteus GUI pip: - name: moteus_gui + name: moteus-gui diff --git a/ansible/roles/jetson_build/tasks/main.yml b/ansible/roles/jetson_build/tasks/main.yml index e4258e594..9736001c0 100644 --- a/ansible/roles/jetson_build/tasks/main.yml +++ b/ansible/roles/jetson_build/tasks/main.yml @@ -23,17 +23,17 @@ state: latest name: - zstd # Required to unpack ZED installer - - cuda + - cuda-12-3 - nvidia-jetpack - name: ZED SDK Download get_url: - url: https://download.stereolabs.com/zedsdk/4.0/l4t35.4/jetsons - dest: /tmp/ZED_SDK_Tegra_L4T35.4_v4.0.7.zstd.run + url: https://download.stereolabs.com/zedsdk/4.1/l4t35.4/jetsons + dest: /tmp/ZED_SDK_Tegra_L4T35.4_v4.1.zstd.run mode: 0755 - name: ZED SDK Install # Silent mode prevents any user input prompting - command: /tmp/Downloads/ZED_SDK_Tegra_L4T35.4_v4.0.7.zstd.run -- silent + command: /tmp/Downloads/ZED_SDK_Tegra_L4T35.4_v4.1.zstd.run -- silent args: creates: /usr/local/zed diff --git a/ansible/roles/jetson_services/files/99-usb-serial.rules b/ansible/roles/jetson_services/files/99-usb-serial.rules deleted file mode 100644 index dfdc52298..000000000 --- a/ansible/roles/jetson_services/files/99-usb-serial.rules +++ /dev/null @@ -1,2 +0,0 @@ -SUBSYSTEM=="tty", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="374b", SYMLINK+="raman" -SUBSYSTEM=="tty", ATTRS{idVendor}=="2341", ATTRS{idProduct}=="0043", SYMLINK+="auton_led" diff --git a/ansible/roles/jetson_services/files/rover-base-station-gstreamer-0.service b/ansible/roles/jetson_services/files/rover-base-station-gstreamer-0.service deleted file mode 100644 index 7480d3f85..000000000 --- a/ansible/roles/jetson_services/files/rover-base-station-gstreamer-0.service +++ /dev/null @@ -1,9 +0,0 @@ -[Unit] -Description=Base Station Gstreamer Service 0 - -[Service] -Environment="DISPLAY=:1" -Environment="XAUTHORITY=/home/mrover/.Xauthority" -User=mrover -ExecStart=/usr/bin/gst-launch-1.0 udpsrc port=5000 ! "application/x-rtp, encoding-name=(string)H264, payload=96" ! rtph264depay ! decodebin ! videoconvert ! autovideosink -Restart=on-failure \ No newline at end of file diff --git a/ansible/roles/jetson_services/files/rover-base-station-gstreamer-0.timer b/ansible/roles/jetson_services/files/rover-base-station-gstreamer-0.timer deleted file mode 100644 index 402352bf1..000000000 --- a/ansible/roles/jetson_services/files/rover-base-station-gstreamer-0.timer +++ /dev/null @@ -1,5 +0,0 @@ -[Timer] -OnBootSec=20s - -[Install] -WantedBy=multi-user.target \ No newline at end of file diff --git a/ansible/roles/jetson_services/files/rover-base-station-gstreamer-1.service b/ansible/roles/jetson_services/files/rover-base-station-gstreamer-1.service deleted file mode 100644 index 1032633da..000000000 --- a/ansible/roles/jetson_services/files/rover-base-station-gstreamer-1.service +++ /dev/null @@ -1,9 +0,0 @@ -[Unit] -Description=Base Station Gstreamer Service 1 - -[Service] -Environment="DISPLAY=:1" -Environment="XAUTHORITY=/home/mrover/.Xauthority" -User=mrover -ExecStart=/usr/bin/gst-launch-1.0 udpsrc port=5001 ! "application/x-rtp, encoding-name=(string)H264, payload=96" ! rtph264depay ! decodebin ! videoconvert ! autovideosink -Restart=on-failure \ No newline at end of file diff --git a/ansible/roles/jetson_services/files/rover-base-station-gstreamer-1.timer b/ansible/roles/jetson_services/files/rover-base-station-gstreamer-1.timer deleted file mode 100644 index 402352bf1..000000000 --- a/ansible/roles/jetson_services/files/rover-base-station-gstreamer-1.timer +++ /dev/null @@ -1,5 +0,0 @@ -[Timer] -OnBootSec=20s - -[Install] -WantedBy=multi-user.target \ No newline at end of file diff --git a/ansible/roles/jetson_services/files/rover-base-station-gstreamer-2.service b/ansible/roles/jetson_services/files/rover-base-station-gstreamer-2.service deleted file mode 100644 index deb6543dc..000000000 --- a/ansible/roles/jetson_services/files/rover-base-station-gstreamer-2.service +++ /dev/null @@ -1,9 +0,0 @@ -[Unit] -Description=Base Station Gstreamer Service 2 - -[Service] -Environment="DISPLAY=:1" -Environment="XAUTHORITY=/home/mrover/.Xauthority" -User=mrover -ExecStart=/usr/bin/gst-launch-1.0 udpsrc port=5002 ! "application/x-rtp, encoding-name=(string)H264, payload=96" ! rtph264depay ! decodebin ! videoconvert ! autovideosink -Restart=on-failure \ No newline at end of file diff --git a/ansible/roles/jetson_services/files/rover-base-station-gstreamer-2.timer b/ansible/roles/jetson_services/files/rover-base-station-gstreamer-2.timer deleted file mode 100644 index 402352bf1..000000000 --- a/ansible/roles/jetson_services/files/rover-base-station-gstreamer-2.timer +++ /dev/null @@ -1,5 +0,0 @@ -[Timer] -OnBootSec=20s - -[Install] -WantedBy=multi-user.target \ No newline at end of file diff --git a/ansible/roles/jetson_services/files/rover-base-station-gstreamer-3.service b/ansible/roles/jetson_services/files/rover-base-station-gstreamer-3.service deleted file mode 100644 index 06be0fc58..000000000 --- a/ansible/roles/jetson_services/files/rover-base-station-gstreamer-3.service +++ /dev/null @@ -1,9 +0,0 @@ -[Unit] -Description=Base Station Gstreamer Service 3 - -[Service] -Environment="DISPLAY=:1" -Environment="XAUTHORITY=/home/mrover/.Xauthority" -User=mrover -ExecStart=/usr/bin/gst-launch-1.0 udpsrc port=5003 ! "application/x-rtp, encoding-name=(string)H264, payload=96" ! rtph264depay ! decodebin ! videoconvert ! autovideosink -Restart=on-failure \ No newline at end of file diff --git a/ansible/roles/jetson_services/files/rover-base-station-gstreamer-3.timer b/ansible/roles/jetson_services/files/rover-base-station-gstreamer-3.timer deleted file mode 100644 index 402352bf1..000000000 --- a/ansible/roles/jetson_services/files/rover-base-station-gstreamer-3.timer +++ /dev/null @@ -1,5 +0,0 @@ -[Timer] -OnBootSec=20s - -[Install] -WantedBy=multi-user.target \ No newline at end of file diff --git a/ansible/roles/jetson_services/files/rules/99-usb-serial.rules b/ansible/roles/jetson_services/files/rules/99-usb-serial.rules new file mode 100644 index 000000000..8d0736fdd --- /dev/null +++ b/ansible/roles/jetson_services/files/rules/99-usb-serial.rules @@ -0,0 +1,6 @@ +SUBSYSTEM=="tty", ATTRS{idVendor}=="1546", ATTRS{idProduct}=="01a9", SYMLINK+="gps_%n" +SUBSYSTEM=="tty", ATTRS{idVendor}=="2341", ATTRS{idProduct}=="0058", SYMLINK+="imu" +SUBSYSTEM=="video4linux", KERNEL=="video*", ATTRS{idVendor}=="1bcf", ATTRS{idProduct}=="0b12", ATTR{index}=="0", GROUP="video", SYMLINK+="arducam" +SUBSYSTEM=="video4linux", KERNEL=="video*", ATTRS{idVendor}=="2b03", ATTRS{idProduct}=="f582", ATTR{index}=="0", GROUP="video", SYMLINK+="zed" +SUBSYSTEM=="video4linux", KERNEL=="video*", ATTRS{idVendor}=="05a3", ATTRS{idProduct}=="9601", ATTR{index}=="0", GROUP="video", SYMLINK+="mobcam" +SUBSYSTEM=="video4linux", KERNEL=="video*", ATTRS{idVendor}=="32e4", ATTRS{idProduct}=="9601", ATTR{index}=="0", GROUP="video", SYMLINK+="staticcam" diff --git a/ansible/roles/jetson_services/tasks/main.yml b/ansible/roles/jetson_services/tasks/main.yml index e69de29bb..74c048ea2 100644 --- a/ansible/roles/jetson_services/tasks/main.yml +++ b/ansible/roles/jetson_services/tasks/main.yml @@ -0,0 +1,14 @@ +- name: USB Rules + become: true + synchronize: + src: files/rules/ + dest: /etc/udev/rules.d + recursive: true + +- name: udev Reload Rules + become: true + command: udevadm control --reload-rules + +- name: udev TTY Trigger + become: true + command: udevadm trigger --subsystem-match=tty diff --git a/bun.lockb b/bun.lockb deleted file mode 100755 index 79ea8baf4..000000000 Binary files a/bun.lockb and /dev/null differ diff --git a/cmake/deps.cmake b/cmake/deps.cmake index 6302378bf..b3bb4a520 100644 --- a/cmake/deps.cmake +++ b/cmake/deps.cmake @@ -1,3 +1,9 @@ +# The simulator will only be built if Dawn (low-level graphics API) is found: +# 1) Installed system-wide with the .deb package in the pkg/ folder. This is ONLY for Ubuntu 20 +# If this is the case then "find_package" will set "dawn_FOUND" to true +# 2) Built from source with the build_dawn.sh script. This is for all other systems (non-Ubuntu, macOS, etc.) +# If this is the case libwebgpu_dawn.* will be found in deps/dawn/out/Release and "dawn_FOUND" will be set to true + find_package(dawn QUIET) if (dawn_FOUND) message(STATUS "Using Dawn system install") @@ -26,34 +32,29 @@ option(MROVER_BUILD_SIM "Build the simulator" ${dawn_FOUND}) if (MROVER_BUILD_SIM) # Apparently Assimp has different names on different systems # find_package is case-sensitive so try both - find_package(Assimp QUIET) - find_package(assimp QUIET) - if (NOT Assimp_FOUND AND NOT assimp_FOUND) + find_package(Assimp NAMES Assimp assimp QUIET) + if (NOT Assimp_FOUND) message(FATAL_ERROR "Assimp not found") endif () find_package(Bullet REQUIRED) find_package(glfw3 REQUIRED) - add_subdirectory(deps/glfw3webgpu SYSTEM) - add_subdirectory(deps/imgui SYSTEM) - add_subdirectory(deps/webgpuhpp SYSTEM) - - set_target_properties(glfw3webgpu PROPERTIES CXX_CLANG_TIDY "") - set_target_properties(imgui PROPERTIES CXX_CLANG_TIDY "") - set_target_properties(webgpu_hpp PROPERTIES CXX_CLANG_TIDY "") + add_subdirectory(deps/glfw3webgpu SYSTEM EXCLUDE_FROM_ALL) + add_subdirectory(deps/imgui SYSTEM EXCLUDE_FROM_ALL) + add_subdirectory(deps/webgpuhpp SYSTEM EXCLUDE_FROM_ALL) endif () find_package(OpenCV REQUIRED) find_package(ZED QUIET) find_package(Eigen3 REQUIRED) +# Same idea as dawn, ideally installed via a package, but if not then build from source find_package(manif QUIET) if (NOT manif_FOUND) if (EXISTS ${CMAKE_CURRENT_LIST_DIR}/../deps/manif/include/manif) add_subdirectory(deps/manif SYSTEM) add_library(MANIF::manif ALIAS manif) - set_target_properties(manif PROPERTIES CXX_CLANG_TIDY "") set(manif_FOUND TRUE) else () @@ -61,7 +62,12 @@ if (NOT manif_FOUND) endif () endif () +# These are old packages so they do not support "find_package" and must be found with pkg-config +# Thankfully CMake has a built-in module for this find_package(PkgConfig REQUIRED) pkg_search_module(NetLink libnl-3.0 QUIET) pkg_search_module(NetLinkRoute libnl-route-3.0 QUIET) -pkg_search_module(gstreamer QUIET) +pkg_search_module(Gst gstreamer-1.0 QUIET) +pkg_search_module(GstApp gstreamer-app-1.0 QUIET) +pkg_search_module(LibUsb libusb-1.0 QUIET) +pkg_search_module(LibUdev libudev QUIET) diff --git a/cmake/macros.cmake b/cmake/macros.cmake index 5b8aaafc0..41674a8cb 100644 --- a/cmake/macros.cmake +++ b/cmake/macros.cmake @@ -11,6 +11,10 @@ macro(target_rosify target) LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) + + if (MROVER_RUN_CLANG_TIDY) + set_target_properties(${target} PROPERTIES CXX_CLANG_TIDY clang-tidy) + endif () endmacro() macro(mrover_add_library name sources includes) diff --git a/config/esw.yaml b/config/esw.yaml index a66911e3f..62402f471 100644 --- a/config/esw.yaml +++ b/config/esw.yaml @@ -1,14 +1,23 @@ +right_gps_driver: + port: "/dev/ttyACM0" + baud: 38400 + frame_id: "base_link" + +left_gps_driver: + port: "/dev/gps" + baud: 38400 + frame_id: "base_link" + +basestation_gps_driver: + port: "/dev/gps" + baud: 38400 + frame_id: "base_link" + imu_driver: port: "/dev/imu" baud: 115200 frame_id: "imu_link" -gps: - port: "/dev/gps" - baud: 115200 - useRMC: false # get covariance instead of velocity, see wiki for more info - frame_id: "base_link" - can: devices: - name: "jetson" @@ -17,36 +26,42 @@ can: - name: "jetson" bus: 1 id: 0x10 + - name: "jetson" + bus: 2 + id: 0x10 + - name: "jetson" + bus: 3 + id: 0x10 - name: "front_left" - bus: 0 + bus: 2 id: 0x12 - name: "front_right" - bus: 0 + bus: 2 id: 0x11 - name: "middle_left" - bus: 0 + bus: 2 id: 0x14 - name: "middle_right" - bus: 0 + bus: 2 id: 0x13 - name: "back_left" - bus: 0 + bus: 2 id: 0x16 - name: "back_right" - bus: 0 + bus: 2 id: 0x15 - name: "joint_a" - bus: 0 - id: 0x20 + bus: 1 + id: 0x20 - name: "joint_b" - bus: 0 + bus: 1 id: 0x21 - name: "joint_c" bus: 1 id: 0x22 - name: "joint_de_0" bus: 1 - id: 0x31 + id: 0x23 - name: "joint_de_1" bus: 1 id: 0x24 @@ -57,11 +72,11 @@ can: bus: 1 id: 0x26 - name: "mast_gimbal_y" - bus: 1 - id: 0x28 - - name: "mast_gimbal_z" - bus: 1 + bus: 3 id: 0x27 + - name: "mast_gimbal_z" + bus: 3 + id: 0x28 - name: "sa_x" bus: 1 id: 0x29 @@ -78,47 +93,48 @@ can: bus: 1 id: 0x33 - name: "cache" - bus: 1 + bus: 2 id: 0x34 - name: "pdlb" - bus: 1 + bus: 3 id: 0x50 - name: "science" - bus: 0 + bus: 2 id: 0x51 brushless_motors: controllers: + # TODO(quintin): These units are in terms of the rotor revolutions, not the output shaft. + # At some point we should modify the moteus firmware to set the rotor to output ratio. front_left: - velocity_multiplier: -1.0 - min_velocity: -70.0 - max_velocity: 70.0 + min_velocity: -20.0 + max_velocity: 20.0 + max_torque: 5.0 front_right: - velocity_multiplier: 1.0 - min_velocity: -70.0 - max_velocity: 70.0 + min_velocity: -20.0 + max_velocity: 20.0 + max_torque: 5.0 middle_left: - velocity_multiplier: -1.0 - min_velocity: -70.0 - max_velocity: 70.0 + min_velocity: -20.0 + max_velocity: 20.0 + max_torque: 5.0 middle_right: - velocity_multiplier: 1.0 - min_velocity: -70.0 - max_velocity: 70.0 + min_velocity: -20.0 + max_velocity: 20.0 + max_torque: 5.0 back_left: - velocity_multiplier: -1.0 - min_velocity: -70.0 - max_velocity: 70.0 + min_velocity: -20.0 + max_velocity: 20.0 + max_torque: 5.0 back_right: - velocity_multiplier: 1.0 - min_velocity: -70.0 - max_velocity: 70.0 + min_velocity: -20.0 + max_velocity: 20.0 + max_torque: 5.0 joint_a: - velocity_multiplier: 1.0 - min_velocity: -100.0 - max_velocity: 100.0 - is_linear: true - rad_to_meters_ratio: 1236.8475 # 5.0 = 5 motor radians -> 1 meter + # 1236.8475 rad/meter, or 196.850393476 rev/meter, and we want a max vel of 40 meter/s. + # gear ratio is currently at 0.005080 revolutions = 1 meter + min_velocity: -0.05 # this means -0.10 meters per second. + max_velocity: 0.05 limit_0_present: true limit_1_present: true limit_0_enabled: true @@ -129,30 +145,34 @@ brushless_motors: limit_1_is_active_high: false limit_0_used_for_readjustment: true limit_1_used_for_readjustment: true - limit_0_readjust_position: 0.0 # radians TODO - limit_1_readjust_position: 494.739 # radians + limit_0_readjust_position: 0.4 + limit_1_readjust_position: 0.0 limit_max_forward_pos: true limit_max_backward_pos: true max_forward_pos: 0.4 max_backward_pos: 0.0 + max_torque: 20.0 joint_c: - velocity_multiplier: -1.0 - min_velocity: -250.0 # gear ratio: 484:1 - max_velocity: 250.0 # max output shaft speed: 5 rpm (for now) + min_velocity: -0.03 # in terms of output + max_velocity: 0.03 # max output shaft speed: 5 rpm (for now) + min_position: -0.125 + max_position: 0.30 # 220 degrees of motion is the entire range + max_torque: 200.0 joint_de_0: - velocity_multiplier: 1.0 - min_velocity: -67.0 - max_velocity: 67.0 + min_velocity: -5.0 + max_velocity: 5.0 + min_position: -10000.0 + max_position: 10000.0 + max_torque: 20.0 joint_de_1: - velocity_multiplier: 1.0 - min_velocity: -67.0 - max_velocity: 67.0 + min_velocity: -5.0 + max_velocity: 5.0 + min_position: -10000.0 + max_position: 10000.0 + max_torque: 20.0 sa_z: - velocity_multiplier: 1.0 - min_velocity: -100.0 - max_velocity: 100.0 - is_linear: true - rad_to_meters_ratio: 157.48 #rad/m + min_velocity: -0.05 + max_velocity: 0.05 limit_0_present: false limit_1_present: false limit_0_enabled: true @@ -165,10 +185,7 @@ brushless_motors: limit_1_used_for_readjustment: true limit_0_readjust_position: 0.0 # radians TODO limit_1_readjust_position: 0.0 # radians TODO - max_torque: 0.5 -joint_de: - pitch_offset: 0.0 - roll_offset: 0.0 + max_torque: 30.0 brushed_motors: controllers: @@ -213,16 +230,15 @@ brushed_motors: # max_backward_pos: 0.0 # calibration_throttle: 5.0 joint_b: - velocity_multiplier: 1.0 + is_inverted: False gear_ratio: 1.0 # motor ratio compared to joint, e.g. if 5, then 5 rotations of motor = 1 rotation of joint - limit_0_present: false - limit_0_enabled: false + limit_0_present: true + limit_0_enabled: true limit_0_is_active_high: false limit_0_limits_fwd: false - limit_0_used_for_readjustment: true + limit_0_used_for_readjustment: false abs_present: true abs_is_fwd_polarity: true - limit_0_readjust_position: 0.0 # radians abs_ratio: 1.0 # encoder ratio compared to motor abs_offset: 0.0 # 0 for joint position corresponds to this radians reading by absolute encoder driver_voltage: 12.0 # used to calculate max pwm @@ -234,7 +250,6 @@ brushed_motors: max_velocity: 1.0 calibration_throttle: 0.5 # throttle during calibration allen_key: - velocity_multiplier: 1.0 gear_ratio: 30.0 # motor ratio compared to joint, e.g. if 5, then 5 rotations of motor = 1 rotation of joint limit_0_present: true limit_1_present: true @@ -253,9 +268,8 @@ brushed_motors: quad_ratio: 1.0 # encoder ratio compared to motor driver_voltage: 12.0 # used to calculate max pwm motor_max_voltage: 12.0 # used to calculate max pwm - calibration_throttle: 0.5 # throttle during calibration + calibration_throttle: 0.9 # throttle during calibration, does not move unless close to max voltage gripper: - velocity_multiplier: 1.0 gear_ratio: 47.0 # motor ratio compared to joint, e.g. if 5, then 5 rotations of motor = 1 rotation of joint quad_present: false quad_is_fwd_polarity: true @@ -266,19 +280,15 @@ brushed_motors: motor_max_voltage: 12.0 # used to calculate max pwm calibration_throttle: 0.5 # throttle during calibration mast_gimbal_y: - velocity_multiplier: 1.0 gear_ratio: 1000.0 # motor ratio compared to joint, e.g. if 5, then 5 rotations of motor = 1 rotation of joint driver_voltage: 12.0 # used to calculate max pwm motor_max_voltage: 12.0 # used to calculate max pwm mast_gimbal_z: - velocity_multiplier: 1.0 gear_ratio: 1000.0 # motor ratio compared to joint, e.g. if 5, then 5 rotations of motor = 1 rotation of joint driver_voltage: 12.0 # used to calculate max pwm motor_max_voltage: 12.0 # used to calculate max pwm sa_x: - velocity_multiplier: 1.0 gear_ratio: 75.0 # motor ratio compared to joint, e.g. if 5, then 5 rotations of motor = 1 rotation of joint - is_linear: true rad_to_meters_ratio: 1.0 #TODO 5.0 = 5 motor radians -> 1 meter limit_0_present: false limit_1_present: false @@ -306,12 +316,10 @@ brushed_motors: max_velocity: 1.0 calibration_throttle: 0.5 # throttle during calibration sa_y: - velocity_multiplier: -1.0 gear_ratio: 75.0 # motor ratio compared to joint, e.g. if 5, then 5 rotations of motor = 1 rotation of joint - is_linear: true rad_to_meters_ratio: 1.0 #TODO 5.0 = 5 motor radians -> 1 meter - limit_0_present: false - limit_1_present: false + limit_0_present: true + limit_1_present: true limit_0_enabled: true limit_1_enabled: true limit_0_is_active_high: false @@ -332,7 +340,6 @@ brushed_motors: calibration_throttle: 0.5 # throttle during calibration sampler: gear_ratio: 75.0 # motor ratio compared to joint, e.g. if 5, then 5 rotations of motor = 1 rotation of joint - is_linear: true rad_to_meters_ratio: 1.0 #TODO 5.0 = 5 motor radians -> 1 meter limit_0_present: false limit_1_present: false @@ -353,7 +360,6 @@ brushed_motors: motor_max_voltage: 6.0 # used to calculate max pwm calibration_throttle: 0.5 # throttle during calibration sensor_actuator: - velocity_multiplier: 1.0 gear_ratio: 1.0 # motor ratio compared to joint, e.g. if 5, then 5 rotations of motor = 1 rotation of joint limit_0_present: false limit_1_present: false @@ -370,7 +376,6 @@ brushed_motors: driver_voltage: 12.0 # used to calculate max pwm motor_max_voltage: 6.0 # used to calculate max pwm cache: - velocity_multiplier: 1.0 gear_ratio: 1.0 # motor ratio compared to joint, e.g. if 5, then 5 rotations of motor = 1 rotation of joint limit_0_present: false limit_1_present: false @@ -512,7 +517,7 @@ cameras: rover: length: 0.86 width: 0.86 - max_speed: 2.0 + max_speed: 1.25 wheel: gear_ratio: 50.0 @@ -532,24 +537,25 @@ motors_group: - "mast_gimbal_z" cache: - "cache_motor" - sa_hw: + sa: - "sa_x" - "sa_y" - "sa_z" - "sampler" - "sensor_actuator" - drive: + drive_left: - "front_left" - - "front_right" - "middle_left" - - "middle_right" - "back_left" + drive_right: + - "front_right" + - "middle_right" - "back_right" motors: controllers: joint_a: - type: "brushless" + type: "brushless_linear" joint_b: type: "brushed" joint_c: @@ -594,5 +600,8 @@ motors: default_network_iface: "enp0s31f6" auton_led_driver: - port: "/dev/ttyACM0" + port: "/dev/led" baud: 115200 + +science: + shutoff_temp: 50.0f diff --git a/config/localization.yaml b/config/localization.yaml index 000c48ad8..698a69056 100644 --- a/config/localization.yaml +++ b/config/localization.yaml @@ -8,3 +8,4 @@ gps_linearization: reference_point_longitude: -83.7096706 reference_point_altitude: 234.1 reference_heading: 90.0 + use_both_gps: false diff --git a/config/moteus/back_left.cfg b/config/moteus/back_left.cfg new file mode 100644 index 000000000..ad86539b4 --- /dev/null +++ b/config/moteus/back_left.cfg @@ -0,0 +1,1397 @@ +uuid.uuid.0 149 +uuid.uuid.1 226 +uuid.uuid.2 10 +uuid.uuid.3 140 +uuid.uuid.4 38 +uuid.uuid.5 208 +uuid.uuid.6 66 +uuid.uuid.7 69 +uuid.uuid.8 174 +uuid.uuid.9 196 +uuid.uuid.10 56 +uuid.uuid.11 198 +uuid.uuid.12 118 +uuid.uuid.13 164 +uuid.uuid.14 192 +uuid.uuid.15 153 +clock.hsitrim 64 +aux1.i2c.i2c_hz 400000 +aux1.i2c.i2c_mode 1 +aux1.i2c.pullup 0 +aux1.i2c.devices.0.type 0 +aux1.i2c.devices.0.address 64 +aux1.i2c.devices.0.poll_ms 10 +aux1.i2c.devices.1.type 0 +aux1.i2c.devices.1.address 64 +aux1.i2c.devices.1.poll_ms 10 +aux1.i2c.devices.2.type 0 +aux1.i2c.devices.2.address 64 +aux1.i2c.devices.2.poll_ms 10 +aux1.spi.mode 1 +aux1.spi.rate_hz 12000000 +aux1.spi.filter_us 64 +aux1.spi.bct 0 +aux1.uart.mode 0 +aux1.uart.baud_rate 115200 +aux1.uart.poll_rate_us 100 +aux1.uart.rs422 0 +aux1.uart.cui_amt21_address 84 +aux1.quadrature.enabled 0 +aux1.quadrature.cpr 16384 +aux1.hall.enabled 1 +aux1.hall.polarity 0 +aux1.index.enabled 0 +aux1.sine_cosine.enabled 0 +aux1.sine_cosine.common 1700 +aux1.i2c_startup_delay_ms 30 +aux1.pins.0.mode 6 +aux1.pins.0.pull 1 +aux1.pins.1.mode 6 +aux1.pins.1.pull 1 +aux1.pins.2.mode 6 +aux1.pins.2.pull 1 +aux1.pins.3.mode 0 +aux1.pins.3.pull 0 +aux1.pins.4.mode 0 +aux1.pins.4.pull 0 +aux2.i2c.i2c_hz 400000 +aux2.i2c.i2c_mode 1 +aux2.i2c.pullup 0 +aux2.i2c.devices.0.type 0 +aux2.i2c.devices.0.address 64 +aux2.i2c.devices.0.poll_ms 10 +aux2.i2c.devices.1.type 0 +aux2.i2c.devices.1.address 64 +aux2.i2c.devices.1.poll_ms 10 +aux2.i2c.devices.2.type 0 +aux2.i2c.devices.2.address 64 +aux2.i2c.devices.2.poll_ms 10 +aux2.spi.mode 1 +aux2.spi.rate_hz 12000000 +aux2.spi.filter_us 64 +aux2.spi.bct 0 +aux2.uart.mode 0 +aux2.uart.baud_rate 115200 +aux2.uart.poll_rate_us 100 +aux2.uart.rs422 0 +aux2.uart.cui_amt21_address 84 +aux2.quadrature.enabled 0 +aux2.quadrature.cpr 16384 +aux2.hall.enabled 1 +aux2.hall.polarity 0 +aux2.index.enabled 0 +aux2.sine_cosine.enabled 0 +aux2.sine_cosine.common 1700 +aux2.i2c_startup_delay_ms 30 +aux2.pins.0.mode 6 +aux2.pins.0.pull 1 +aux2.pins.1.mode 6 +aux2.pins.1.pull 1 +aux2.pins.2.mode 6 +aux2.pins.2.pull 1 +aux2.pins.3.mode 0 +aux2.pins.3.pull 0 +aux2.pins.4.mode 0 +aux2.pins.4.pull 0 +motor_position.sources.0.aux_number 1 +motor_position.sources.0.type 4 +motor_position.sources.0.i2c_device 0 +motor_position.sources.0.incremental_index -1 +motor_position.sources.0.cpr 84 +motor_position.sources.0.offset -3.000000 +motor_position.sources.0.sign 1 +motor_position.sources.0.debug_override -1 +motor_position.sources.0.reference 0 +motor_position.sources.0.pll_filter_hz 80.000000 +motor_position.sources.0.compensation_table.0 0.000000 +motor_position.sources.0.compensation_table.1 0.000000 +motor_position.sources.0.compensation_table.2 0.000000 +motor_position.sources.0.compensation_table.3 0.000000 +motor_position.sources.0.compensation_table.4 0.000000 +motor_position.sources.0.compensation_table.5 0.000000 +motor_position.sources.0.compensation_table.6 0.000000 +motor_position.sources.0.compensation_table.7 0.000000 +motor_position.sources.0.compensation_table.8 0.000000 +motor_position.sources.0.compensation_table.9 0.000000 +motor_position.sources.0.compensation_table.10 0.000000 +motor_position.sources.0.compensation_table.11 0.000000 +motor_position.sources.0.compensation_table.12 0.000000 +motor_position.sources.0.compensation_table.13 0.000000 +motor_position.sources.0.compensation_table.14 0.000000 +motor_position.sources.0.compensation_table.15 0.000000 +motor_position.sources.0.compensation_table.16 0.000000 +motor_position.sources.0.compensation_table.17 0.000000 +motor_position.sources.0.compensation_table.18 0.000000 +motor_position.sources.0.compensation_table.19 0.000000 +motor_position.sources.0.compensation_table.20 0.000000 +motor_position.sources.0.compensation_table.21 0.000000 +motor_position.sources.0.compensation_table.22 0.000000 +motor_position.sources.0.compensation_table.23 0.000000 +motor_position.sources.0.compensation_table.24 0.000000 +motor_position.sources.0.compensation_table.25 0.000000 +motor_position.sources.0.compensation_table.26 0.000000 +motor_position.sources.0.compensation_table.27 0.000000 +motor_position.sources.0.compensation_table.28 0.000000 +motor_position.sources.0.compensation_table.29 0.000000 +motor_position.sources.0.compensation_table.30 0.000000 +motor_position.sources.0.compensation_table.31 0.000000 +motor_position.sources.1.aux_number 1 +motor_position.sources.1.type 0 +motor_position.sources.1.i2c_device 0 +motor_position.sources.1.incremental_index -1 +motor_position.sources.1.cpr 16384 +motor_position.sources.1.offset 0.000000 +motor_position.sources.1.sign 1 +motor_position.sources.1.debug_override -1 +motor_position.sources.1.reference 0 +motor_position.sources.1.pll_filter_hz 400.000000 +motor_position.sources.1.compensation_table.0 0.000000 +motor_position.sources.1.compensation_table.1 0.000000 +motor_position.sources.1.compensation_table.2 0.000000 +motor_position.sources.1.compensation_table.3 0.000000 +motor_position.sources.1.compensation_table.4 0.000000 +motor_position.sources.1.compensation_table.5 0.000000 +motor_position.sources.1.compensation_table.6 0.000000 +motor_position.sources.1.compensation_table.7 0.000000 +motor_position.sources.1.compensation_table.8 0.000000 +motor_position.sources.1.compensation_table.9 0.000000 +motor_position.sources.1.compensation_table.10 0.000000 +motor_position.sources.1.compensation_table.11 0.000000 +motor_position.sources.1.compensation_table.12 0.000000 +motor_position.sources.1.compensation_table.13 0.000000 +motor_position.sources.1.compensation_table.14 0.000000 +motor_position.sources.1.compensation_table.15 0.000000 +motor_position.sources.1.compensation_table.16 0.000000 +motor_position.sources.1.compensation_table.17 0.000000 +motor_position.sources.1.compensation_table.18 0.000000 +motor_position.sources.1.compensation_table.19 0.000000 +motor_position.sources.1.compensation_table.20 0.000000 +motor_position.sources.1.compensation_table.21 0.000000 +motor_position.sources.1.compensation_table.22 0.000000 +motor_position.sources.1.compensation_table.23 0.000000 +motor_position.sources.1.compensation_table.24 0.000000 +motor_position.sources.1.compensation_table.25 0.000000 +motor_position.sources.1.compensation_table.26 0.000000 +motor_position.sources.1.compensation_table.27 0.000000 +motor_position.sources.1.compensation_table.28 0.000000 +motor_position.sources.1.compensation_table.29 0.000000 +motor_position.sources.1.compensation_table.30 0.000000 +motor_position.sources.1.compensation_table.31 0.000000 +motor_position.sources.2.aux_number 1 +motor_position.sources.2.type 0 +motor_position.sources.2.i2c_device 0 +motor_position.sources.2.incremental_index -1 +motor_position.sources.2.cpr 16384 +motor_position.sources.2.offset 0.000000 +motor_position.sources.2.sign 1 +motor_position.sources.2.debug_override -1 +motor_position.sources.2.reference 0 +motor_position.sources.2.pll_filter_hz 400.000000 +motor_position.sources.2.compensation_table.0 0.000000 +motor_position.sources.2.compensation_table.1 0.000000 +motor_position.sources.2.compensation_table.2 0.000000 +motor_position.sources.2.compensation_table.3 0.000000 +motor_position.sources.2.compensation_table.4 0.000000 +motor_position.sources.2.compensation_table.5 0.000000 +motor_position.sources.2.compensation_table.6 0.000000 +motor_position.sources.2.compensation_table.7 0.000000 +motor_position.sources.2.compensation_table.8 0.000000 +motor_position.sources.2.compensation_table.9 0.000000 +motor_position.sources.2.compensation_table.10 0.000000 +motor_position.sources.2.compensation_table.11 0.000000 +motor_position.sources.2.compensation_table.12 0.000000 +motor_position.sources.2.compensation_table.13 0.000000 +motor_position.sources.2.compensation_table.14 0.000000 +motor_position.sources.2.compensation_table.15 0.000000 +motor_position.sources.2.compensation_table.16 0.000000 +motor_position.sources.2.compensation_table.17 0.000000 +motor_position.sources.2.compensation_table.18 0.000000 +motor_position.sources.2.compensation_table.19 0.000000 +motor_position.sources.2.compensation_table.20 0.000000 +motor_position.sources.2.compensation_table.21 0.000000 +motor_position.sources.2.compensation_table.22 0.000000 +motor_position.sources.2.compensation_table.23 0.000000 +motor_position.sources.2.compensation_table.24 0.000000 +motor_position.sources.2.compensation_table.25 0.000000 +motor_position.sources.2.compensation_table.26 0.000000 +motor_position.sources.2.compensation_table.27 0.000000 +motor_position.sources.2.compensation_table.28 0.000000 +motor_position.sources.2.compensation_table.29 0.000000 +motor_position.sources.2.compensation_table.30 0.000000 +motor_position.sources.2.compensation_table.31 0.000000 +motor_position.commutation_source 0 +motor_position.output.source 0 +motor_position.output.offset 0.000000 +motor_position.output.sign -1 +motor_position.output.reference_source -1 +motor_position.rotor_to_output_ratio 1.000000 +motor.poles 28 +motor.phase_invert 0 +motor.resistance_ohm 0.258464 +motor.v_per_hz 0.409335 +motor.offset.0 0.000000 +motor.offset.1 0.000000 +motor.offset.2 0.000000 +motor.offset.3 0.000000 +motor.offset.4 0.000000 +motor.offset.5 0.000000 +motor.offset.6 0.000000 +motor.offset.7 0.000000 +motor.offset.8 0.000000 +motor.offset.9 0.000000 +motor.offset.10 0.000000 +motor.offset.11 0.000000 +motor.offset.12 0.000000 +motor.offset.13 0.000000 +motor.offset.14 0.000000 +motor.offset.15 0.000000 +motor.offset.16 0.000000 +motor.offset.17 0.000000 +motor.offset.18 0.000000 +motor.offset.19 0.000000 +motor.offset.20 0.000000 +motor.offset.21 0.000000 +motor.offset.22 0.000000 +motor.offset.23 0.000000 +motor.offset.24 0.000000 +motor.offset.25 0.000000 +motor.offset.26 0.000000 +motor.offset.27 0.000000 +motor.offset.28 0.000000 +motor.offset.29 0.000000 +motor.offset.30 0.000000 +motor.offset.31 0.000000 +motor.offset.32 0.000000 +motor.offset.33 0.000000 +motor.offset.34 0.000000 +motor.offset.35 0.000000 +motor.offset.36 0.000000 +motor.offset.37 0.000000 +motor.offset.38 0.000000 +motor.offset.39 0.000000 +motor.offset.40 0.000000 +motor.offset.41 0.000000 +motor.offset.42 0.000000 +motor.offset.43 0.000000 +motor.offset.44 0.000000 +motor.offset.45 0.000000 +motor.offset.46 0.000000 +motor.offset.47 0.000000 +motor.offset.48 0.000000 +motor.offset.49 0.000000 +motor.offset.50 0.000000 +motor.offset.51 0.000000 +motor.offset.52 0.000000 +motor.offset.53 0.000000 +motor.offset.54 0.000000 +motor.offset.55 0.000000 +motor.offset.56 0.000000 +motor.offset.57 0.000000 +motor.offset.58 0.000000 +motor.offset.59 0.000000 +motor.offset.60 0.000000 +motor.offset.61 0.000000 +motor.offset.62 0.000000 +motor.offset.63 0.000000 +motor.rotation_current_cutoff_A 10000.000000 +motor.rotation_current_scale 0.050000 +motor.rotation_torque_scale 14.700000 +motor.cogging_dq_scale 0.000000 +motor.cogging_dq_comp.0 0 +motor.cogging_dq_comp.1 0 +motor.cogging_dq_comp.2 0 +motor.cogging_dq_comp.3 0 +motor.cogging_dq_comp.4 0 +motor.cogging_dq_comp.5 0 +motor.cogging_dq_comp.6 0 +motor.cogging_dq_comp.7 0 +motor.cogging_dq_comp.8 0 +motor.cogging_dq_comp.9 0 +motor.cogging_dq_comp.10 0 +motor.cogging_dq_comp.11 0 +motor.cogging_dq_comp.12 0 +motor.cogging_dq_comp.13 0 +motor.cogging_dq_comp.14 0 +motor.cogging_dq_comp.15 0 +motor.cogging_dq_comp.16 0 +motor.cogging_dq_comp.17 0 +motor.cogging_dq_comp.18 0 +motor.cogging_dq_comp.19 0 +motor.cogging_dq_comp.20 0 +motor.cogging_dq_comp.21 0 +motor.cogging_dq_comp.22 0 +motor.cogging_dq_comp.23 0 +motor.cogging_dq_comp.24 0 +motor.cogging_dq_comp.25 0 +motor.cogging_dq_comp.26 0 +motor.cogging_dq_comp.27 0 +motor.cogging_dq_comp.28 0 +motor.cogging_dq_comp.29 0 +motor.cogging_dq_comp.30 0 +motor.cogging_dq_comp.31 0 +motor.cogging_dq_comp.32 0 +motor.cogging_dq_comp.33 0 +motor.cogging_dq_comp.34 0 +motor.cogging_dq_comp.35 0 +motor.cogging_dq_comp.36 0 +motor.cogging_dq_comp.37 0 +motor.cogging_dq_comp.38 0 +motor.cogging_dq_comp.39 0 +motor.cogging_dq_comp.40 0 +motor.cogging_dq_comp.41 0 +motor.cogging_dq_comp.42 0 +motor.cogging_dq_comp.43 0 +motor.cogging_dq_comp.44 0 +motor.cogging_dq_comp.45 0 +motor.cogging_dq_comp.46 0 +motor.cogging_dq_comp.47 0 +motor.cogging_dq_comp.48 0 +motor.cogging_dq_comp.49 0 +motor.cogging_dq_comp.50 0 +motor.cogging_dq_comp.51 0 +motor.cogging_dq_comp.52 0 +motor.cogging_dq_comp.53 0 +motor.cogging_dq_comp.54 0 +motor.cogging_dq_comp.55 0 +motor.cogging_dq_comp.56 0 +motor.cogging_dq_comp.57 0 +motor.cogging_dq_comp.58 0 +motor.cogging_dq_comp.59 0 +motor.cogging_dq_comp.60 0 +motor.cogging_dq_comp.61 0 +motor.cogging_dq_comp.62 0 +motor.cogging_dq_comp.63 0 +motor.cogging_dq_comp.64 0 +motor.cogging_dq_comp.65 0 +motor.cogging_dq_comp.66 0 +motor.cogging_dq_comp.67 0 +motor.cogging_dq_comp.68 0 +motor.cogging_dq_comp.69 0 +motor.cogging_dq_comp.70 0 +motor.cogging_dq_comp.71 0 +motor.cogging_dq_comp.72 0 +motor.cogging_dq_comp.73 0 +motor.cogging_dq_comp.74 0 +motor.cogging_dq_comp.75 0 +motor.cogging_dq_comp.76 0 +motor.cogging_dq_comp.77 0 +motor.cogging_dq_comp.78 0 +motor.cogging_dq_comp.79 0 +motor.cogging_dq_comp.80 0 +motor.cogging_dq_comp.81 0 +motor.cogging_dq_comp.82 0 +motor.cogging_dq_comp.83 0 +motor.cogging_dq_comp.84 0 +motor.cogging_dq_comp.85 0 +motor.cogging_dq_comp.86 0 +motor.cogging_dq_comp.87 0 +motor.cogging_dq_comp.88 0 +motor.cogging_dq_comp.89 0 +motor.cogging_dq_comp.90 0 +motor.cogging_dq_comp.91 0 +motor.cogging_dq_comp.92 0 +motor.cogging_dq_comp.93 0 +motor.cogging_dq_comp.94 0 +motor.cogging_dq_comp.95 0 +motor.cogging_dq_comp.96 0 +motor.cogging_dq_comp.97 0 +motor.cogging_dq_comp.98 0 +motor.cogging_dq_comp.99 0 +motor.cogging_dq_comp.100 0 +motor.cogging_dq_comp.101 0 +motor.cogging_dq_comp.102 0 +motor.cogging_dq_comp.103 0 +motor.cogging_dq_comp.104 0 +motor.cogging_dq_comp.105 0 +motor.cogging_dq_comp.106 0 +motor.cogging_dq_comp.107 0 +motor.cogging_dq_comp.108 0 +motor.cogging_dq_comp.109 0 +motor.cogging_dq_comp.110 0 +motor.cogging_dq_comp.111 0 +motor.cogging_dq_comp.112 0 +motor.cogging_dq_comp.113 0 +motor.cogging_dq_comp.114 0 +motor.cogging_dq_comp.115 0 +motor.cogging_dq_comp.116 0 +motor.cogging_dq_comp.117 0 +motor.cogging_dq_comp.118 0 +motor.cogging_dq_comp.119 0 +motor.cogging_dq_comp.120 0 +motor.cogging_dq_comp.121 0 +motor.cogging_dq_comp.122 0 +motor.cogging_dq_comp.123 0 +motor.cogging_dq_comp.124 0 +motor.cogging_dq_comp.125 0 +motor.cogging_dq_comp.126 0 +motor.cogging_dq_comp.127 0 +motor.cogging_dq_comp.128 0 +motor.cogging_dq_comp.129 0 +motor.cogging_dq_comp.130 0 +motor.cogging_dq_comp.131 0 +motor.cogging_dq_comp.132 0 +motor.cogging_dq_comp.133 0 +motor.cogging_dq_comp.134 0 +motor.cogging_dq_comp.135 0 +motor.cogging_dq_comp.136 0 +motor.cogging_dq_comp.137 0 +motor.cogging_dq_comp.138 0 +motor.cogging_dq_comp.139 0 +motor.cogging_dq_comp.140 0 +motor.cogging_dq_comp.141 0 +motor.cogging_dq_comp.142 0 +motor.cogging_dq_comp.143 0 +motor.cogging_dq_comp.144 0 +motor.cogging_dq_comp.145 0 +motor.cogging_dq_comp.146 0 +motor.cogging_dq_comp.147 0 +motor.cogging_dq_comp.148 0 +motor.cogging_dq_comp.149 0 +motor.cogging_dq_comp.150 0 +motor.cogging_dq_comp.151 0 +motor.cogging_dq_comp.152 0 +motor.cogging_dq_comp.153 0 +motor.cogging_dq_comp.154 0 +motor.cogging_dq_comp.155 0 +motor.cogging_dq_comp.156 0 +motor.cogging_dq_comp.157 0 +motor.cogging_dq_comp.158 0 +motor.cogging_dq_comp.159 0 +motor.cogging_dq_comp.160 0 +motor.cogging_dq_comp.161 0 +motor.cogging_dq_comp.162 0 +motor.cogging_dq_comp.163 0 +motor.cogging_dq_comp.164 0 +motor.cogging_dq_comp.165 0 +motor.cogging_dq_comp.166 0 +motor.cogging_dq_comp.167 0 +motor.cogging_dq_comp.168 0 +motor.cogging_dq_comp.169 0 +motor.cogging_dq_comp.170 0 +motor.cogging_dq_comp.171 0 +motor.cogging_dq_comp.172 0 +motor.cogging_dq_comp.173 0 +motor.cogging_dq_comp.174 0 +motor.cogging_dq_comp.175 0 +motor.cogging_dq_comp.176 0 +motor.cogging_dq_comp.177 0 +motor.cogging_dq_comp.178 0 +motor.cogging_dq_comp.179 0 +motor.cogging_dq_comp.180 0 +motor.cogging_dq_comp.181 0 +motor.cogging_dq_comp.182 0 +motor.cogging_dq_comp.183 0 +motor.cogging_dq_comp.184 0 +motor.cogging_dq_comp.185 0 +motor.cogging_dq_comp.186 0 +motor.cogging_dq_comp.187 0 +motor.cogging_dq_comp.188 0 +motor.cogging_dq_comp.189 0 +motor.cogging_dq_comp.190 0 +motor.cogging_dq_comp.191 0 +motor.cogging_dq_comp.192 0 +motor.cogging_dq_comp.193 0 +motor.cogging_dq_comp.194 0 +motor.cogging_dq_comp.195 0 +motor.cogging_dq_comp.196 0 +motor.cogging_dq_comp.197 0 +motor.cogging_dq_comp.198 0 +motor.cogging_dq_comp.199 0 +motor.cogging_dq_comp.200 0 +motor.cogging_dq_comp.201 0 +motor.cogging_dq_comp.202 0 +motor.cogging_dq_comp.203 0 +motor.cogging_dq_comp.204 0 +motor.cogging_dq_comp.205 0 +motor.cogging_dq_comp.206 0 +motor.cogging_dq_comp.207 0 +motor.cogging_dq_comp.208 0 +motor.cogging_dq_comp.209 0 +motor.cogging_dq_comp.210 0 +motor.cogging_dq_comp.211 0 +motor.cogging_dq_comp.212 0 +motor.cogging_dq_comp.213 0 +motor.cogging_dq_comp.214 0 +motor.cogging_dq_comp.215 0 +motor.cogging_dq_comp.216 0 +motor.cogging_dq_comp.217 0 +motor.cogging_dq_comp.218 0 +motor.cogging_dq_comp.219 0 +motor.cogging_dq_comp.220 0 +motor.cogging_dq_comp.221 0 +motor.cogging_dq_comp.222 0 +motor.cogging_dq_comp.223 0 +motor.cogging_dq_comp.224 0 +motor.cogging_dq_comp.225 0 +motor.cogging_dq_comp.226 0 +motor.cogging_dq_comp.227 0 +motor.cogging_dq_comp.228 0 +motor.cogging_dq_comp.229 0 +motor.cogging_dq_comp.230 0 +motor.cogging_dq_comp.231 0 +motor.cogging_dq_comp.232 0 +motor.cogging_dq_comp.233 0 +motor.cogging_dq_comp.234 0 +motor.cogging_dq_comp.235 0 +motor.cogging_dq_comp.236 0 +motor.cogging_dq_comp.237 0 +motor.cogging_dq_comp.238 0 +motor.cogging_dq_comp.239 0 +motor.cogging_dq_comp.240 0 +motor.cogging_dq_comp.241 0 +motor.cogging_dq_comp.242 0 +motor.cogging_dq_comp.243 0 +motor.cogging_dq_comp.244 0 +motor.cogging_dq_comp.245 0 +motor.cogging_dq_comp.246 0 +motor.cogging_dq_comp.247 0 +motor.cogging_dq_comp.248 0 +motor.cogging_dq_comp.249 0 +motor.cogging_dq_comp.250 0 +motor.cogging_dq_comp.251 0 +motor.cogging_dq_comp.252 0 +motor.cogging_dq_comp.253 0 +motor.cogging_dq_comp.254 0 +motor.cogging_dq_comp.255 0 +motor.cogging_dq_comp.256 0 +motor.cogging_dq_comp.257 0 +motor.cogging_dq_comp.258 0 +motor.cogging_dq_comp.259 0 +motor.cogging_dq_comp.260 0 +motor.cogging_dq_comp.261 0 +motor.cogging_dq_comp.262 0 +motor.cogging_dq_comp.263 0 +motor.cogging_dq_comp.264 0 +motor.cogging_dq_comp.265 0 +motor.cogging_dq_comp.266 0 +motor.cogging_dq_comp.267 0 +motor.cogging_dq_comp.268 0 +motor.cogging_dq_comp.269 0 +motor.cogging_dq_comp.270 0 +motor.cogging_dq_comp.271 0 +motor.cogging_dq_comp.272 0 +motor.cogging_dq_comp.273 0 +motor.cogging_dq_comp.274 0 +motor.cogging_dq_comp.275 0 +motor.cogging_dq_comp.276 0 +motor.cogging_dq_comp.277 0 +motor.cogging_dq_comp.278 0 +motor.cogging_dq_comp.279 0 +motor.cogging_dq_comp.280 0 +motor.cogging_dq_comp.281 0 +motor.cogging_dq_comp.282 0 +motor.cogging_dq_comp.283 0 +motor.cogging_dq_comp.284 0 +motor.cogging_dq_comp.285 0 +motor.cogging_dq_comp.286 0 +motor.cogging_dq_comp.287 0 +motor.cogging_dq_comp.288 0 +motor.cogging_dq_comp.289 0 +motor.cogging_dq_comp.290 0 +motor.cogging_dq_comp.291 0 +motor.cogging_dq_comp.292 0 +motor.cogging_dq_comp.293 0 +motor.cogging_dq_comp.294 0 +motor.cogging_dq_comp.295 0 +motor.cogging_dq_comp.296 0 +motor.cogging_dq_comp.297 0 +motor.cogging_dq_comp.298 0 +motor.cogging_dq_comp.299 0 +motor.cogging_dq_comp.300 0 +motor.cogging_dq_comp.301 0 +motor.cogging_dq_comp.302 0 +motor.cogging_dq_comp.303 0 +motor.cogging_dq_comp.304 0 +motor.cogging_dq_comp.305 0 +motor.cogging_dq_comp.306 0 +motor.cogging_dq_comp.307 0 +motor.cogging_dq_comp.308 0 +motor.cogging_dq_comp.309 0 +motor.cogging_dq_comp.310 0 +motor.cogging_dq_comp.311 0 +motor.cogging_dq_comp.312 0 +motor.cogging_dq_comp.313 0 +motor.cogging_dq_comp.314 0 +motor.cogging_dq_comp.315 0 +motor.cogging_dq_comp.316 0 +motor.cogging_dq_comp.317 0 +motor.cogging_dq_comp.318 0 +motor.cogging_dq_comp.319 0 +motor.cogging_dq_comp.320 0 +motor.cogging_dq_comp.321 0 +motor.cogging_dq_comp.322 0 +motor.cogging_dq_comp.323 0 +motor.cogging_dq_comp.324 0 +motor.cogging_dq_comp.325 0 +motor.cogging_dq_comp.326 0 +motor.cogging_dq_comp.327 0 +motor.cogging_dq_comp.328 0 +motor.cogging_dq_comp.329 0 +motor.cogging_dq_comp.330 0 +motor.cogging_dq_comp.331 0 +motor.cogging_dq_comp.332 0 +motor.cogging_dq_comp.333 0 +motor.cogging_dq_comp.334 0 +motor.cogging_dq_comp.335 0 +motor.cogging_dq_comp.336 0 +motor.cogging_dq_comp.337 0 +motor.cogging_dq_comp.338 0 +motor.cogging_dq_comp.339 0 +motor.cogging_dq_comp.340 0 +motor.cogging_dq_comp.341 0 +motor.cogging_dq_comp.342 0 +motor.cogging_dq_comp.343 0 +motor.cogging_dq_comp.344 0 +motor.cogging_dq_comp.345 0 +motor.cogging_dq_comp.346 0 +motor.cogging_dq_comp.347 0 +motor.cogging_dq_comp.348 0 +motor.cogging_dq_comp.349 0 +motor.cogging_dq_comp.350 0 +motor.cogging_dq_comp.351 0 +motor.cogging_dq_comp.352 0 +motor.cogging_dq_comp.353 0 +motor.cogging_dq_comp.354 0 +motor.cogging_dq_comp.355 0 +motor.cogging_dq_comp.356 0 +motor.cogging_dq_comp.357 0 +motor.cogging_dq_comp.358 0 +motor.cogging_dq_comp.359 0 +motor.cogging_dq_comp.360 0 +motor.cogging_dq_comp.361 0 +motor.cogging_dq_comp.362 0 +motor.cogging_dq_comp.363 0 +motor.cogging_dq_comp.364 0 +motor.cogging_dq_comp.365 0 +motor.cogging_dq_comp.366 0 +motor.cogging_dq_comp.367 0 +motor.cogging_dq_comp.368 0 +motor.cogging_dq_comp.369 0 +motor.cogging_dq_comp.370 0 +motor.cogging_dq_comp.371 0 +motor.cogging_dq_comp.372 0 +motor.cogging_dq_comp.373 0 +motor.cogging_dq_comp.374 0 +motor.cogging_dq_comp.375 0 +motor.cogging_dq_comp.376 0 +motor.cogging_dq_comp.377 0 +motor.cogging_dq_comp.378 0 +motor.cogging_dq_comp.379 0 +motor.cogging_dq_comp.380 0 +motor.cogging_dq_comp.381 0 +motor.cogging_dq_comp.382 0 +motor.cogging_dq_comp.383 0 +motor.cogging_dq_comp.384 0 +motor.cogging_dq_comp.385 0 +motor.cogging_dq_comp.386 0 +motor.cogging_dq_comp.387 0 +motor.cogging_dq_comp.388 0 +motor.cogging_dq_comp.389 0 +motor.cogging_dq_comp.390 0 +motor.cogging_dq_comp.391 0 +motor.cogging_dq_comp.392 0 +motor.cogging_dq_comp.393 0 +motor.cogging_dq_comp.394 0 +motor.cogging_dq_comp.395 0 +motor.cogging_dq_comp.396 0 +motor.cogging_dq_comp.397 0 +motor.cogging_dq_comp.398 0 +motor.cogging_dq_comp.399 0 +motor.cogging_dq_comp.400 0 +motor.cogging_dq_comp.401 0 +motor.cogging_dq_comp.402 0 +motor.cogging_dq_comp.403 0 +motor.cogging_dq_comp.404 0 +motor.cogging_dq_comp.405 0 +motor.cogging_dq_comp.406 0 +motor.cogging_dq_comp.407 0 +motor.cogging_dq_comp.408 0 +motor.cogging_dq_comp.409 0 +motor.cogging_dq_comp.410 0 +motor.cogging_dq_comp.411 0 +motor.cogging_dq_comp.412 0 +motor.cogging_dq_comp.413 0 +motor.cogging_dq_comp.414 0 +motor.cogging_dq_comp.415 0 +motor.cogging_dq_comp.416 0 +motor.cogging_dq_comp.417 0 +motor.cogging_dq_comp.418 0 +motor.cogging_dq_comp.419 0 +motor.cogging_dq_comp.420 0 +motor.cogging_dq_comp.421 0 +motor.cogging_dq_comp.422 0 +motor.cogging_dq_comp.423 0 +motor.cogging_dq_comp.424 0 +motor.cogging_dq_comp.425 0 +motor.cogging_dq_comp.426 0 +motor.cogging_dq_comp.427 0 +motor.cogging_dq_comp.428 0 +motor.cogging_dq_comp.429 0 +motor.cogging_dq_comp.430 0 +motor.cogging_dq_comp.431 0 +motor.cogging_dq_comp.432 0 +motor.cogging_dq_comp.433 0 +motor.cogging_dq_comp.434 0 +motor.cogging_dq_comp.435 0 +motor.cogging_dq_comp.436 0 +motor.cogging_dq_comp.437 0 +motor.cogging_dq_comp.438 0 +motor.cogging_dq_comp.439 0 +motor.cogging_dq_comp.440 0 +motor.cogging_dq_comp.441 0 +motor.cogging_dq_comp.442 0 +motor.cogging_dq_comp.443 0 +motor.cogging_dq_comp.444 0 +motor.cogging_dq_comp.445 0 +motor.cogging_dq_comp.446 0 +motor.cogging_dq_comp.447 0 +motor.cogging_dq_comp.448 0 +motor.cogging_dq_comp.449 0 +motor.cogging_dq_comp.450 0 +motor.cogging_dq_comp.451 0 +motor.cogging_dq_comp.452 0 +motor.cogging_dq_comp.453 0 +motor.cogging_dq_comp.454 0 +motor.cogging_dq_comp.455 0 +motor.cogging_dq_comp.456 0 +motor.cogging_dq_comp.457 0 +motor.cogging_dq_comp.458 0 +motor.cogging_dq_comp.459 0 +motor.cogging_dq_comp.460 0 +motor.cogging_dq_comp.461 0 +motor.cogging_dq_comp.462 0 +motor.cogging_dq_comp.463 0 +motor.cogging_dq_comp.464 0 +motor.cogging_dq_comp.465 0 +motor.cogging_dq_comp.466 0 +motor.cogging_dq_comp.467 0 +motor.cogging_dq_comp.468 0 +motor.cogging_dq_comp.469 0 +motor.cogging_dq_comp.470 0 +motor.cogging_dq_comp.471 0 +motor.cogging_dq_comp.472 0 +motor.cogging_dq_comp.473 0 +motor.cogging_dq_comp.474 0 +motor.cogging_dq_comp.475 0 +motor.cogging_dq_comp.476 0 +motor.cogging_dq_comp.477 0 +motor.cogging_dq_comp.478 0 +motor.cogging_dq_comp.479 0 +motor.cogging_dq_comp.480 0 +motor.cogging_dq_comp.481 0 +motor.cogging_dq_comp.482 0 +motor.cogging_dq_comp.483 0 +motor.cogging_dq_comp.484 0 +motor.cogging_dq_comp.485 0 +motor.cogging_dq_comp.486 0 +motor.cogging_dq_comp.487 0 +motor.cogging_dq_comp.488 0 +motor.cogging_dq_comp.489 0 +motor.cogging_dq_comp.490 0 +motor.cogging_dq_comp.491 0 +motor.cogging_dq_comp.492 0 +motor.cogging_dq_comp.493 0 +motor.cogging_dq_comp.494 0 +motor.cogging_dq_comp.495 0 +motor.cogging_dq_comp.496 0 +motor.cogging_dq_comp.497 0 +motor.cogging_dq_comp.498 0 +motor.cogging_dq_comp.499 0 +motor.cogging_dq_comp.500 0 +motor.cogging_dq_comp.501 0 +motor.cogging_dq_comp.502 0 +motor.cogging_dq_comp.503 0 +motor.cogging_dq_comp.504 0 +motor.cogging_dq_comp.505 0 +motor.cogging_dq_comp.506 0 +motor.cogging_dq_comp.507 0 +motor.cogging_dq_comp.508 0 +motor.cogging_dq_comp.509 0 +motor.cogging_dq_comp.510 0 +motor.cogging_dq_comp.511 0 +motor.cogging_dq_comp.512 0 +motor.cogging_dq_comp.513 0 +motor.cogging_dq_comp.514 0 +motor.cogging_dq_comp.515 0 +motor.cogging_dq_comp.516 0 +motor.cogging_dq_comp.517 0 +motor.cogging_dq_comp.518 0 +motor.cogging_dq_comp.519 0 +motor.cogging_dq_comp.520 0 +motor.cogging_dq_comp.521 0 +motor.cogging_dq_comp.522 0 +motor.cogging_dq_comp.523 0 +motor.cogging_dq_comp.524 0 +motor.cogging_dq_comp.525 0 +motor.cogging_dq_comp.526 0 +motor.cogging_dq_comp.527 0 +motor.cogging_dq_comp.528 0 +motor.cogging_dq_comp.529 0 +motor.cogging_dq_comp.530 0 +motor.cogging_dq_comp.531 0 +motor.cogging_dq_comp.532 0 +motor.cogging_dq_comp.533 0 +motor.cogging_dq_comp.534 0 +motor.cogging_dq_comp.535 0 +motor.cogging_dq_comp.536 0 +motor.cogging_dq_comp.537 0 +motor.cogging_dq_comp.538 0 +motor.cogging_dq_comp.539 0 +motor.cogging_dq_comp.540 0 +motor.cogging_dq_comp.541 0 +motor.cogging_dq_comp.542 0 +motor.cogging_dq_comp.543 0 +motor.cogging_dq_comp.544 0 +motor.cogging_dq_comp.545 0 +motor.cogging_dq_comp.546 0 +motor.cogging_dq_comp.547 0 +motor.cogging_dq_comp.548 0 +motor.cogging_dq_comp.549 0 +motor.cogging_dq_comp.550 0 +motor.cogging_dq_comp.551 0 +motor.cogging_dq_comp.552 0 +motor.cogging_dq_comp.553 0 +motor.cogging_dq_comp.554 0 +motor.cogging_dq_comp.555 0 +motor.cogging_dq_comp.556 0 +motor.cogging_dq_comp.557 0 +motor.cogging_dq_comp.558 0 +motor.cogging_dq_comp.559 0 +motor.cogging_dq_comp.560 0 +motor.cogging_dq_comp.561 0 +motor.cogging_dq_comp.562 0 +motor.cogging_dq_comp.563 0 +motor.cogging_dq_comp.564 0 +motor.cogging_dq_comp.565 0 +motor.cogging_dq_comp.566 0 +motor.cogging_dq_comp.567 0 +motor.cogging_dq_comp.568 0 +motor.cogging_dq_comp.569 0 +motor.cogging_dq_comp.570 0 +motor.cogging_dq_comp.571 0 +motor.cogging_dq_comp.572 0 +motor.cogging_dq_comp.573 0 +motor.cogging_dq_comp.574 0 +motor.cogging_dq_comp.575 0 +motor.cogging_dq_comp.576 0 +motor.cogging_dq_comp.577 0 +motor.cogging_dq_comp.578 0 +motor.cogging_dq_comp.579 0 +motor.cogging_dq_comp.580 0 +motor.cogging_dq_comp.581 0 +motor.cogging_dq_comp.582 0 +motor.cogging_dq_comp.583 0 +motor.cogging_dq_comp.584 0 +motor.cogging_dq_comp.585 0 +motor.cogging_dq_comp.586 0 +motor.cogging_dq_comp.587 0 +motor.cogging_dq_comp.588 0 +motor.cogging_dq_comp.589 0 +motor.cogging_dq_comp.590 0 +motor.cogging_dq_comp.591 0 +motor.cogging_dq_comp.592 0 +motor.cogging_dq_comp.593 0 +motor.cogging_dq_comp.594 0 +motor.cogging_dq_comp.595 0 +motor.cogging_dq_comp.596 0 +motor.cogging_dq_comp.597 0 +motor.cogging_dq_comp.598 0 +motor.cogging_dq_comp.599 0 +motor.cogging_dq_comp.600 0 +motor.cogging_dq_comp.601 0 +motor.cogging_dq_comp.602 0 +motor.cogging_dq_comp.603 0 +motor.cogging_dq_comp.604 0 +motor.cogging_dq_comp.605 0 +motor.cogging_dq_comp.606 0 +motor.cogging_dq_comp.607 0 +motor.cogging_dq_comp.608 0 +motor.cogging_dq_comp.609 0 +motor.cogging_dq_comp.610 0 +motor.cogging_dq_comp.611 0 +motor.cogging_dq_comp.612 0 +motor.cogging_dq_comp.613 0 +motor.cogging_dq_comp.614 0 +motor.cogging_dq_comp.615 0 +motor.cogging_dq_comp.616 0 +motor.cogging_dq_comp.617 0 +motor.cogging_dq_comp.618 0 +motor.cogging_dq_comp.619 0 +motor.cogging_dq_comp.620 0 +motor.cogging_dq_comp.621 0 +motor.cogging_dq_comp.622 0 +motor.cogging_dq_comp.623 0 +motor.cogging_dq_comp.624 0 +motor.cogging_dq_comp.625 0 +motor.cogging_dq_comp.626 0 +motor.cogging_dq_comp.627 0 +motor.cogging_dq_comp.628 0 +motor.cogging_dq_comp.629 0 +motor.cogging_dq_comp.630 0 +motor.cogging_dq_comp.631 0 +motor.cogging_dq_comp.632 0 +motor.cogging_dq_comp.633 0 +motor.cogging_dq_comp.634 0 +motor.cogging_dq_comp.635 0 +motor.cogging_dq_comp.636 0 +motor.cogging_dq_comp.637 0 +motor.cogging_dq_comp.638 0 +motor.cogging_dq_comp.639 0 +motor.cogging_dq_comp.640 0 +motor.cogging_dq_comp.641 0 +motor.cogging_dq_comp.642 0 +motor.cogging_dq_comp.643 0 +motor.cogging_dq_comp.644 0 +motor.cogging_dq_comp.645 0 +motor.cogging_dq_comp.646 0 +motor.cogging_dq_comp.647 0 +motor.cogging_dq_comp.648 0 +motor.cogging_dq_comp.649 0 +motor.cogging_dq_comp.650 0 +motor.cogging_dq_comp.651 0 +motor.cogging_dq_comp.652 0 +motor.cogging_dq_comp.653 0 +motor.cogging_dq_comp.654 0 +motor.cogging_dq_comp.655 0 +motor.cogging_dq_comp.656 0 +motor.cogging_dq_comp.657 0 +motor.cogging_dq_comp.658 0 +motor.cogging_dq_comp.659 0 +motor.cogging_dq_comp.660 0 +motor.cogging_dq_comp.661 0 +motor.cogging_dq_comp.662 0 +motor.cogging_dq_comp.663 0 +motor.cogging_dq_comp.664 0 +motor.cogging_dq_comp.665 0 +motor.cogging_dq_comp.666 0 +motor.cogging_dq_comp.667 0 +motor.cogging_dq_comp.668 0 +motor.cogging_dq_comp.669 0 +motor.cogging_dq_comp.670 0 +motor.cogging_dq_comp.671 0 +motor.cogging_dq_comp.672 0 +motor.cogging_dq_comp.673 0 +motor.cogging_dq_comp.674 0 +motor.cogging_dq_comp.675 0 +motor.cogging_dq_comp.676 0 +motor.cogging_dq_comp.677 0 +motor.cogging_dq_comp.678 0 +motor.cogging_dq_comp.679 0 +motor.cogging_dq_comp.680 0 +motor.cogging_dq_comp.681 0 +motor.cogging_dq_comp.682 0 +motor.cogging_dq_comp.683 0 +motor.cogging_dq_comp.684 0 +motor.cogging_dq_comp.685 0 +motor.cogging_dq_comp.686 0 +motor.cogging_dq_comp.687 0 +motor.cogging_dq_comp.688 0 +motor.cogging_dq_comp.689 0 +motor.cogging_dq_comp.690 0 +motor.cogging_dq_comp.691 0 +motor.cogging_dq_comp.692 0 +motor.cogging_dq_comp.693 0 +motor.cogging_dq_comp.694 0 +motor.cogging_dq_comp.695 0 +motor.cogging_dq_comp.696 0 +motor.cogging_dq_comp.697 0 +motor.cogging_dq_comp.698 0 +motor.cogging_dq_comp.699 0 +motor.cogging_dq_comp.700 0 +motor.cogging_dq_comp.701 0 +motor.cogging_dq_comp.702 0 +motor.cogging_dq_comp.703 0 +motor.cogging_dq_comp.704 0 +motor.cogging_dq_comp.705 0 +motor.cogging_dq_comp.706 0 +motor.cogging_dq_comp.707 0 +motor.cogging_dq_comp.708 0 +motor.cogging_dq_comp.709 0 +motor.cogging_dq_comp.710 0 +motor.cogging_dq_comp.711 0 +motor.cogging_dq_comp.712 0 +motor.cogging_dq_comp.713 0 +motor.cogging_dq_comp.714 0 +motor.cogging_dq_comp.715 0 +motor.cogging_dq_comp.716 0 +motor.cogging_dq_comp.717 0 +motor.cogging_dq_comp.718 0 +motor.cogging_dq_comp.719 0 +motor.cogging_dq_comp.720 0 +motor.cogging_dq_comp.721 0 +motor.cogging_dq_comp.722 0 +motor.cogging_dq_comp.723 0 +motor.cogging_dq_comp.724 0 +motor.cogging_dq_comp.725 0 +motor.cogging_dq_comp.726 0 +motor.cogging_dq_comp.727 0 +motor.cogging_dq_comp.728 0 +motor.cogging_dq_comp.729 0 +motor.cogging_dq_comp.730 0 +motor.cogging_dq_comp.731 0 +motor.cogging_dq_comp.732 0 +motor.cogging_dq_comp.733 0 +motor.cogging_dq_comp.734 0 +motor.cogging_dq_comp.735 0 +motor.cogging_dq_comp.736 0 +motor.cogging_dq_comp.737 0 +motor.cogging_dq_comp.738 0 +motor.cogging_dq_comp.739 0 +motor.cogging_dq_comp.740 0 +motor.cogging_dq_comp.741 0 +motor.cogging_dq_comp.742 0 +motor.cogging_dq_comp.743 0 +motor.cogging_dq_comp.744 0 +motor.cogging_dq_comp.745 0 +motor.cogging_dq_comp.746 0 +motor.cogging_dq_comp.747 0 +motor.cogging_dq_comp.748 0 +motor.cogging_dq_comp.749 0 +motor.cogging_dq_comp.750 0 +motor.cogging_dq_comp.751 0 +motor.cogging_dq_comp.752 0 +motor.cogging_dq_comp.753 0 +motor.cogging_dq_comp.754 0 +motor.cogging_dq_comp.755 0 +motor.cogging_dq_comp.756 0 +motor.cogging_dq_comp.757 0 +motor.cogging_dq_comp.758 0 +motor.cogging_dq_comp.759 0 +motor.cogging_dq_comp.760 0 +motor.cogging_dq_comp.761 0 +motor.cogging_dq_comp.762 0 +motor.cogging_dq_comp.763 0 +motor.cogging_dq_comp.764 0 +motor.cogging_dq_comp.765 0 +motor.cogging_dq_comp.766 0 +motor.cogging_dq_comp.767 0 +motor.cogging_dq_comp.768 0 +motor.cogging_dq_comp.769 0 +motor.cogging_dq_comp.770 0 +motor.cogging_dq_comp.771 0 +motor.cogging_dq_comp.772 0 +motor.cogging_dq_comp.773 0 +motor.cogging_dq_comp.774 0 +motor.cogging_dq_comp.775 0 +motor.cogging_dq_comp.776 0 +motor.cogging_dq_comp.777 0 +motor.cogging_dq_comp.778 0 +motor.cogging_dq_comp.779 0 +motor.cogging_dq_comp.780 0 +motor.cogging_dq_comp.781 0 +motor.cogging_dq_comp.782 0 +motor.cogging_dq_comp.783 0 +motor.cogging_dq_comp.784 0 +motor.cogging_dq_comp.785 0 +motor.cogging_dq_comp.786 0 +motor.cogging_dq_comp.787 0 +motor.cogging_dq_comp.788 0 +motor.cogging_dq_comp.789 0 +motor.cogging_dq_comp.790 0 +motor.cogging_dq_comp.791 0 +motor.cogging_dq_comp.792 0 +motor.cogging_dq_comp.793 0 +motor.cogging_dq_comp.794 0 +motor.cogging_dq_comp.795 0 +motor.cogging_dq_comp.796 0 +motor.cogging_dq_comp.797 0 +motor.cogging_dq_comp.798 0 +motor.cogging_dq_comp.799 0 +motor.cogging_dq_comp.800 0 +motor.cogging_dq_comp.801 0 +motor.cogging_dq_comp.802 0 +motor.cogging_dq_comp.803 0 +motor.cogging_dq_comp.804 0 +motor.cogging_dq_comp.805 0 +motor.cogging_dq_comp.806 0 +motor.cogging_dq_comp.807 0 +motor.cogging_dq_comp.808 0 +motor.cogging_dq_comp.809 0 +motor.cogging_dq_comp.810 0 +motor.cogging_dq_comp.811 0 +motor.cogging_dq_comp.812 0 +motor.cogging_dq_comp.813 0 +motor.cogging_dq_comp.814 0 +motor.cogging_dq_comp.815 0 +motor.cogging_dq_comp.816 0 +motor.cogging_dq_comp.817 0 +motor.cogging_dq_comp.818 0 +motor.cogging_dq_comp.819 0 +motor.cogging_dq_comp.820 0 +motor.cogging_dq_comp.821 0 +motor.cogging_dq_comp.822 0 +motor.cogging_dq_comp.823 0 +motor.cogging_dq_comp.824 0 +motor.cogging_dq_comp.825 0 +motor.cogging_dq_comp.826 0 +motor.cogging_dq_comp.827 0 +motor.cogging_dq_comp.828 0 +motor.cogging_dq_comp.829 0 +motor.cogging_dq_comp.830 0 +motor.cogging_dq_comp.831 0 +motor.cogging_dq_comp.832 0 +motor.cogging_dq_comp.833 0 +motor.cogging_dq_comp.834 0 +motor.cogging_dq_comp.835 0 +motor.cogging_dq_comp.836 0 +motor.cogging_dq_comp.837 0 +motor.cogging_dq_comp.838 0 +motor.cogging_dq_comp.839 0 +motor.cogging_dq_comp.840 0 +motor.cogging_dq_comp.841 0 +motor.cogging_dq_comp.842 0 +motor.cogging_dq_comp.843 0 +motor.cogging_dq_comp.844 0 +motor.cogging_dq_comp.845 0 +motor.cogging_dq_comp.846 0 +motor.cogging_dq_comp.847 0 +motor.cogging_dq_comp.848 0 +motor.cogging_dq_comp.849 0 +motor.cogging_dq_comp.850 0 +motor.cogging_dq_comp.851 0 +motor.cogging_dq_comp.852 0 +motor.cogging_dq_comp.853 0 +motor.cogging_dq_comp.854 0 +motor.cogging_dq_comp.855 0 +motor.cogging_dq_comp.856 0 +motor.cogging_dq_comp.857 0 +motor.cogging_dq_comp.858 0 +motor.cogging_dq_comp.859 0 +motor.cogging_dq_comp.860 0 +motor.cogging_dq_comp.861 0 +motor.cogging_dq_comp.862 0 +motor.cogging_dq_comp.863 0 +motor.cogging_dq_comp.864 0 +motor.cogging_dq_comp.865 0 +motor.cogging_dq_comp.866 0 +motor.cogging_dq_comp.867 0 +motor.cogging_dq_comp.868 0 +motor.cogging_dq_comp.869 0 +motor.cogging_dq_comp.870 0 +motor.cogging_dq_comp.871 0 +motor.cogging_dq_comp.872 0 +motor.cogging_dq_comp.873 0 +motor.cogging_dq_comp.874 0 +motor.cogging_dq_comp.875 0 +motor.cogging_dq_comp.876 0 +motor.cogging_dq_comp.877 0 +motor.cogging_dq_comp.878 0 +motor.cogging_dq_comp.879 0 +motor.cogging_dq_comp.880 0 +motor.cogging_dq_comp.881 0 +motor.cogging_dq_comp.882 0 +motor.cogging_dq_comp.883 0 +motor.cogging_dq_comp.884 0 +motor.cogging_dq_comp.885 0 +motor.cogging_dq_comp.886 0 +motor.cogging_dq_comp.887 0 +motor.cogging_dq_comp.888 0 +motor.cogging_dq_comp.889 0 +motor.cogging_dq_comp.890 0 +motor.cogging_dq_comp.891 0 +motor.cogging_dq_comp.892 0 +motor.cogging_dq_comp.893 0 +motor.cogging_dq_comp.894 0 +motor.cogging_dq_comp.895 0 +motor.cogging_dq_comp.896 0 +motor.cogging_dq_comp.897 0 +motor.cogging_dq_comp.898 0 +motor.cogging_dq_comp.899 0 +motor.cogging_dq_comp.900 0 +motor.cogging_dq_comp.901 0 +motor.cogging_dq_comp.902 0 +motor.cogging_dq_comp.903 0 +motor.cogging_dq_comp.904 0 +motor.cogging_dq_comp.905 0 +motor.cogging_dq_comp.906 0 +motor.cogging_dq_comp.907 0 +motor.cogging_dq_comp.908 0 +motor.cogging_dq_comp.909 0 +motor.cogging_dq_comp.910 0 +motor.cogging_dq_comp.911 0 +motor.cogging_dq_comp.912 0 +motor.cogging_dq_comp.913 0 +motor.cogging_dq_comp.914 0 +motor.cogging_dq_comp.915 0 +motor.cogging_dq_comp.916 0 +motor.cogging_dq_comp.917 0 +motor.cogging_dq_comp.918 0 +motor.cogging_dq_comp.919 0 +motor.cogging_dq_comp.920 0 +motor.cogging_dq_comp.921 0 +motor.cogging_dq_comp.922 0 +motor.cogging_dq_comp.923 0 +motor.cogging_dq_comp.924 0 +motor.cogging_dq_comp.925 0 +motor.cogging_dq_comp.926 0 +motor.cogging_dq_comp.927 0 +motor.cogging_dq_comp.928 0 +motor.cogging_dq_comp.929 0 +motor.cogging_dq_comp.930 0 +motor.cogging_dq_comp.931 0 +motor.cogging_dq_comp.932 0 +motor.cogging_dq_comp.933 0 +motor.cogging_dq_comp.934 0 +motor.cogging_dq_comp.935 0 +motor.cogging_dq_comp.936 0 +motor.cogging_dq_comp.937 0 +motor.cogging_dq_comp.938 0 +motor.cogging_dq_comp.939 0 +motor.cogging_dq_comp.940 0 +motor.cogging_dq_comp.941 0 +motor.cogging_dq_comp.942 0 +motor.cogging_dq_comp.943 0 +motor.cogging_dq_comp.944 0 +motor.cogging_dq_comp.945 0 +motor.cogging_dq_comp.946 0 +motor.cogging_dq_comp.947 0 +motor.cogging_dq_comp.948 0 +motor.cogging_dq_comp.949 0 +motor.cogging_dq_comp.950 0 +motor.cogging_dq_comp.951 0 +motor.cogging_dq_comp.952 0 +motor.cogging_dq_comp.953 0 +motor.cogging_dq_comp.954 0 +motor.cogging_dq_comp.955 0 +motor.cogging_dq_comp.956 0 +motor.cogging_dq_comp.957 0 +motor.cogging_dq_comp.958 0 +motor.cogging_dq_comp.959 0 +motor.cogging_dq_comp.960 0 +motor.cogging_dq_comp.961 0 +motor.cogging_dq_comp.962 0 +motor.cogging_dq_comp.963 0 +motor.cogging_dq_comp.964 0 +motor.cogging_dq_comp.965 0 +motor.cogging_dq_comp.966 0 +motor.cogging_dq_comp.967 0 +motor.cogging_dq_comp.968 0 +motor.cogging_dq_comp.969 0 +motor.cogging_dq_comp.970 0 +motor.cogging_dq_comp.971 0 +motor.cogging_dq_comp.972 0 +motor.cogging_dq_comp.973 0 +motor.cogging_dq_comp.974 0 +motor.cogging_dq_comp.975 0 +motor.cogging_dq_comp.976 0 +motor.cogging_dq_comp.977 0 +motor.cogging_dq_comp.978 0 +motor.cogging_dq_comp.979 0 +motor.cogging_dq_comp.980 0 +motor.cogging_dq_comp.981 0 +motor.cogging_dq_comp.982 0 +motor.cogging_dq_comp.983 0 +motor.cogging_dq_comp.984 0 +motor.cogging_dq_comp.985 0 +motor.cogging_dq_comp.986 0 +motor.cogging_dq_comp.987 0 +motor.cogging_dq_comp.988 0 +motor.cogging_dq_comp.989 0 +motor.cogging_dq_comp.990 0 +motor.cogging_dq_comp.991 0 +motor.cogging_dq_comp.992 0 +motor.cogging_dq_comp.993 0 +motor.cogging_dq_comp.994 0 +motor.cogging_dq_comp.995 0 +motor.cogging_dq_comp.996 0 +motor.cogging_dq_comp.997 0 +motor.cogging_dq_comp.998 0 +motor.cogging_dq_comp.999 0 +motor.cogging_dq_comp.1000 0 +motor.cogging_dq_comp.1001 0 +motor.cogging_dq_comp.1002 0 +motor.cogging_dq_comp.1003 0 +motor.cogging_dq_comp.1004 0 +motor.cogging_dq_comp.1005 0 +motor.cogging_dq_comp.1006 0 +motor.cogging_dq_comp.1007 0 +motor.cogging_dq_comp.1008 0 +motor.cogging_dq_comp.1009 0 +motor.cogging_dq_comp.1010 0 +motor.cogging_dq_comp.1011 0 +motor.cogging_dq_comp.1012 0 +motor.cogging_dq_comp.1013 0 +motor.cogging_dq_comp.1014 0 +motor.cogging_dq_comp.1015 0 +motor.cogging_dq_comp.1016 0 +motor.cogging_dq_comp.1017 0 +motor.cogging_dq_comp.1018 0 +motor.cogging_dq_comp.1019 0 +motor.cogging_dq_comp.1020 0 +motor.cogging_dq_comp.1021 0 +motor.cogging_dq_comp.1022 0 +motor.cogging_dq_comp.1023 0 +drv8323_conf.dis_cpuv 0 +drv8323_conf.dis_gdf 0 +drv8323_conf.otw_rep 0 +drv8323_conf.pwm_mode 1 +drv8323_conf.pwm_1x_asynchronous 0 +drv8323_conf.pwm_1x_dir 0 +drv8323_conf.idrivep_hs_ma 100 +drv8323_conf.idriven_hs_ma 200 +drv8323_conf.cbc 1 +drv8323_conf.tdrive_ns 1000 +drv8323_conf.idrivep_ls_ma 100 +drv8323_conf.idriven_ls_ma 200 +drv8323_conf.tretry 0 +drv8323_conf.dead_time_ns 50 +drv8323_conf.ocp_mode 0 +drv8323_conf.ocp_deg_us 4 +drv8323_conf.vds_lvl_mv 700 +drv8323_conf.csa_fet 0 +drv8323_conf.vref_div 1 +drv8323_conf.ls_ref 0 +drv8323_conf.csa_gain 20 +drv8323_conf.dis_sen 0 +drv8323_conf.sen_lvl_mv 500 +servo.pwm_rate_hz 30000 +servo.i_gain 20.000000 +servo.current_sense_ohm 0.000500 +servo.pwm_comp_off 0.027000 +servo.pwm_comp_mag 0.005000 +servo.pwm_scale 1.000000 +servo.max_voltage 46.000000 +servo.max_power_W 450.000000 +servo.derate_temperature 50.000000 +servo.fault_temperature 75.000000 +servo.enable_motor_temperature 0 +servo.motor_derate_temperature 50.000000 +servo.motor_fault_temperature nan +servo.velocity_threshold 0.000000 +servo.position_derate 0.020000 +servo.adc_cur_cycles 2 +servo.adc_aux_cycles 47 +servo.pid_dq.kp 0.234858 +servo.pid_dq.ki 162.397873 +servo.pid_position.kp 0.500000 +servo.pid_position.ki 0.000000 +servo.pid_position.kd 0.050000 +servo.pid_position.iratelimit -1.000000 +servo.pid_position.ilimit 0.000000 +servo.pid_position.max_desired_rate 0.000000 +servo.pid_position.sign -1 +servo.current_feedforward 1.000000 +servo.bemf_feedforward 1.000000 +servo.default_velocity_limit nan +servo.default_accel_limit nan +servo.voltage_mode_control 0 +servo.fixed_voltage_mode 0 +servo.fixed_voltage_control_V 0.000000 +servo.max_position_slip 10.000000 +servo.default_timeout_s 0.100000 +servo.timeout_max_torque_Nm 5.000000 +servo.timeout_mode 12 +servo.flux_brake_min_voltage 43.500000 +servo.flux_brake_resistance_ohm 0.025000 +servo.max_current_A 7.500000 +servo.derate_current_A -20.000000 +servo.max_velocity 500.000000 +servo.max_velocity_derate 2.000000 +servo.cooldown_cycles 256 +servo.velocity_zero_capture_threshold 0.050000 +servo.emit_debug 0 +servopos.position_min nan +servopos.position_max nan +id.id 22 +can.prefix 0 + diff --git a/config/moteus/back_right.cfg b/config/moteus/back_right.cfg new file mode 100644 index 000000000..920c0382b --- /dev/null +++ b/config/moteus/back_right.cfg @@ -0,0 +1,350 @@ +motor.cogging_dq_comp.748 0 +motor.cogging_dq_comp.749 0 +motor.cogging_dq_comp.750 0 +motor.cogging_dq_comp.751 0 +motor.cogging_dq_comp.752 0 +motor.cogging_dq_comp.753 0 +motor.cogging_dq_comp.754 0 +motor.cogging_dq_comp.755 0 +motor.cogging_dq_comp.756 0 +motor.cogging_dq_comp.757 0 +motor.cogging_dq_comp.758 0 +motor.cogging_dq_comp.759 0 +motor.cogging_dq_comp.760 0 +motor.cogging_dq_comp.761 0 +motor.cogging_dq_comp.762 0 +motor.cogging_dq_comp.763 0 +motor.cogging_dq_comp.764 0 +motor.cogging_dq_comp.765 0 +motor.cogging_dq_comp.766 0 +motor.cogging_dq_comp.767 0 +motor.cogging_dq_comp.768 0 +motor.cogging_dq_comp.769 0 +motor.cogging_dq_comp.770 0 +motor.cogging_dq_comp.771 0 +motor.cogging_dq_comp.772 0 +motor.cogging_dq_comp.773 0 +motor.cogging_dq_comp.774 0 +motor.cogging_dq_comp.775 0 +motor.cogging_dq_comp.776 0 +motor.cogging_dq_comp.777 0 +motor.cogging_dq_comp.778 0 +motor.cogging_dq_comp.779 0 +motor.cogging_dq_comp.780 0 +motor.cogging_dq_comp.781 0 +motor.cogging_dq_comp.782 0 +motor.cogging_dq_comp.783 0 +motor.cogging_dq_comp.784 0 +motor.cogging_dq_comp.785 0 +motor.cogging_dq_comp.786 0 +motor.cogging_dq_comp.787 0 +motor.cogging_dq_comp.788 0 +motor.cogging_dq_comp.789 0 +motor.cogging_dq_comp.790 0 +motor.cogging_dq_comp.791 0 +motor.cogging_dq_comp.792 0 +motor.cogging_dq_comp.793 0 +motor.cogging_dq_comp.794 0 +motor.cogging_dq_comp.795 0 +motor.cogging_dq_comp.796 0 +motor.cogging_dq_comp.797 0 +motor.cogging_dq_comp.798 0 +motor.cogging_dq_comp.799 0 +motor.cogging_dq_comp.800 0 +motor.cogging_dq_comp.801 0 +motor.cogging_dq_comp.802 0 +motor.cogging_dq_comp.803 0 +motor.cogging_dq_comp.804 0 +motor.cogging_dq_comp.805 0 +motor.cogging_dq_comp.806 0 +motor.cogging_dq_comp.807 0 +motor.cogging_dq_comp.808 0 +motor.cogging_dq_comp.809 0 +motor.cogging_dq_comp.810 0 +motor.cogging_dq_comp.811 0 +motor.cogging_dq_comp.812 0 +motor.cogging_dq_comp.813 0 +motor.cogging_dq_comp.814 0 +motor.cogging_dq_comp.815 0 +motor.cogging_dq_comp.816 0 +motor.cogging_dq_comp.817 0 +motor.cogging_dq_comp.818 0 +motor.cogging_dq_comp.819 0 +motor.cogging_dq_comp.820 0 +motor.cogging_dq_comp.821 0 +motor.cogging_dq_comp.822 0 +motor.cogging_dq_comp.823 0 +motor.cogging_dq_comp.824 0 +motor.cogging_dq_comp.825 0 +motor.cogging_dq_comp.826 0 +motor.cogging_dq_comp.827 0 +motor.cogging_dq_comp.828 0 +motor.cogging_dq_comp.829 0 +motor.cogging_dq_comp.830 0 +motor.cogging_dq_comp.831 0 +motor.cogging_dq_comp.832 0 +motor.cogging_dq_comp.833 0 +motor.cogging_dq_comp.834 0 +motor.cogging_dq_comp.835 0 +motor.cogging_dq_comp.836 0 +motor.cogging_dq_comp.837 0 +motor.cogging_dq_comp.838 0 +motor.cogging_dq_comp.839 0 +motor.cogging_dq_comp.840 0 +motor.cogging_dq_comp.841 0 +motor.cogging_dq_comp.842 0 +motor.cogging_dq_comp.843 0 +motor.cogging_dq_comp.844 0 +motor.cogging_dq_comp.845 0 +motor.cogging_dq_comp.846 0 +motor.cogging_dq_comp.847 0 +motor.cogging_dq_comp.848 0 +motor.cogging_dq_comp.849 0 +motor.cogging_dq_comp.850 0 +motor.cogging_dq_comp.851 0 +motor.cogging_dq_comp.852 0 +motor.cogging_dq_comp.853 0 +motor.cogging_dq_comp.854 0 +motor.cogging_dq_comp.855 0 +motor.cogging_dq_comp.856 0 +motor.cogging_dq_comp.857 0 +motor.cogging_dq_comp.858 0 +motor.cogging_dq_comp.859 0 +motor.cogging_dq_comp.860 0 +motor.cogging_dq_comp.861 0 +motor.cogging_dq_comp.862 0 +motor.cogging_dq_comp.863 0 +motor.cogging_dq_comp.864 0 +motor.cogging_dq_comp.865 0 +motor.cogging_dq_comp.866 0 +motor.cogging_dq_comp.867 0 +motor.cogging_dq_comp.868 0 +motor.cogging_dq_comp.869 0 +motor.cogging_dq_comp.870 0 +motor.cogging_dq_comp.871 0 +motor.cogging_dq_comp.872 0 +motor.cogging_dq_comp.873 0 +motor.cogging_dq_comp.874 0 +motor.cogging_dq_comp.875 0 +motor.cogging_dq_comp.876 0 +motor.cogging_dq_comp.877 0 +motor.cogging_dq_comp.878 0 +motor.cogging_dq_comp.879 0 +motor.cogging_dq_comp.880 0 +motor.cogging_dq_comp.881 0 +motor.cogging_dq_comp.882 0 +motor.cogging_dq_comp.883 0 +motor.cogging_dq_comp.884 0 +motor.cogging_dq_comp.885 0 +motor.cogging_dq_comp.886 0 +motor.cogging_dq_comp.887 0 +motor.cogging_dq_comp.888 0 +motor.cogging_dq_comp.889 0 +motor.cogging_dq_comp.890 0 +motor.cogging_dq_comp.891 0 +motor.cogging_dq_comp.892 0 +motor.cogging_dq_comp.893 0 +motor.cogging_dq_comp.894 0 +motor.cogging_dq_comp.895 0 +motor.cogging_dq_comp.896 0 +motor.cogging_dq_comp.897 0 +motor.cogging_dq_comp.898 0 +motor.cogging_dq_comp.899 0 +motor.cogging_dq_comp.900 0 +motor.cogging_dq_comp.901 0 +motor.cogging_dq_comp.902 0 +motor.cogging_dq_comp.903 0 +motor.cogging_dq_comp.904 0 +motor.cogging_dq_comp.905 0 +motor.cogging_dq_comp.906 0 +motor.cogging_dq_comp.907 0 +motor.cogging_dq_comp.908 0 +motor.cogging_dq_comp.909 0 +motor.cogging_dq_comp.910 0 +motor.cogging_dq_comp.911 0 +motor.cogging_dq_comp.912 0 +motor.cogging_dq_comp.913 0 +motor.cogging_dq_comp.914 0 +motor.cogging_dq_comp.915 0 +motor.cogging_dq_comp.916 0 +motor.cogging_dq_comp.917 0 +motor.cogging_dq_comp.918 0 +motor.cogging_dq_comp.919 0 +motor.cogging_dq_comp.920 0 +motor.cogging_dq_comp.921 0 +motor.cogging_dq_comp.922 0 +motor.cogging_dq_comp.923 0 +motor.cogging_dq_comp.924 0 +motor.cogging_dq_comp.925 0 +motor.cogging_dq_comp.926 0 +motor.cogging_dq_comp.927 0 +motor.cogging_dq_comp.928 0 +motor.cogging_dq_comp.929 0 +motor.cogging_dq_comp.930 0 +motor.cogging_dq_comp.931 0 +motor.cogging_dq_comp.932 0 +motor.cogging_dq_comp.933 0 +motor.cogging_dq_comp.934 0 +motor.cogging_dq_comp.935 0 +motor.cogging_dq_comp.936 0 +motor.cogging_dq_comp.937 0 +motor.cogging_dq_comp.938 0 +motor.cogging_dq_comp.939 0 +motor.cogging_dq_comp.940 0 +motor.cogging_dq_comp.941 0 +motor.cogging_dq_comp.942 0 +motor.cogging_dq_comp.943 0 +motor.cogging_dq_comp.944 0 +motor.cogging_dq_comp.945 0 +motor.cogging_dq_comp.946 0 +motor.cogging_dq_comp.947 0 +motor.cogging_dq_comp.948 0 +motor.cogging_dq_comp.949 0 +motor.cogging_dq_comp.950 0 +motor.cogging_dq_comp.951 0 +motor.cogging_dq_comp.952 0 +motor.cogging_dq_comp.953 0 +motor.cogging_dq_comp.954 0 +motor.cogging_dq_comp.955 0 +motor.cogging_dq_comp.956 0 +motor.cogging_dq_comp.957 0 +motor.cogging_dq_comp.958 0 +motor.cogging_dq_comp.959 0 +motor.cogging_dq_comp.960 0 +motor.cogging_dq_comp.961 0 +motor.cogging_dq_comp.962 0 +motor.cogging_dq_comp.963 0 +motor.cogging_dq_comp.964 0 +motor.cogging_dq_comp.965 0 +motor.cogging_dq_comp.966 0 +motor.cogging_dq_comp.967 0 +motor.cogging_dq_comp.968 0 +motor.cogging_dq_comp.969 0 +motor.cogging_dq_comp.970 0 +motor.cogging_dq_comp.971 0 +motor.cogging_dq_comp.972 0 +motor.cogging_dq_comp.973 0 +motor.cogging_dq_comp.974 0 +motor.cogging_dq_comp.975 0 +motor.cogging_dq_comp.976 0 +motor.cogging_dq_comp.977 0 +motor.cogging_dq_comp.978 0 +motor.cogging_dq_comp.979 0 +motor.cogging_dq_comp.980 0 +motor.cogging_dq_comp.981 0 +motor.cogging_dq_comp.982 0 +motor.cogging_dq_comp.983 0 +motor.cogging_dq_comp.984 0 +motor.cogging_dq_comp.985 0 +motor.cogging_dq_comp.986 0 +motor.cogging_dq_comp.987 0 +motor.cogging_dq_comp.988 0 +motor.cogging_dq_comp.989 0 +motor.cogging_dq_comp.990 0 +motor.cogging_dq_comp.991 0 +motor.cogging_dq_comp.992 0 +motor.cogging_dq_comp.993 0 +motor.cogging_dq_comp.994 0 +motor.cogging_dq_comp.995 0 +motor.cogging_dq_comp.996 0 +motor.cogging_dq_comp.997 0 +motor.cogging_dq_comp.998 0 +motor.cogging_dq_comp.999 0 +motor.cogging_dq_comp.1000 0 +motor.cogging_dq_comp.1001 0 +motor.cogging_dq_comp.1002 0 +motor.cogging_dq_comp.1003 0 +motor.cogging_dq_comp.1004 0 +motor.cogging_dq_comp.1005 0 +motor.cogging_dq_comp.1006 0 +motor.cogging_dq_comp.1007 0 +motor.cogging_dq_comp.1008 0 +motor.cogging_dq_comp.1009 0 +motor.cogging_dq_comp.1010 0 +motor.cogging_dq_comp.1011 0 +motor.cogging_dq_comp.1012 0 +motor.cogging_dq_comp.1013 0 +motor.cogging_dq_comp.1014 0 +motor.cogging_dq_comp.1015 0 +motor.cogging_dq_comp.1016 0 +motor.cogging_dq_comp.1017 0 +motor.cogging_dq_comp.1018 0 +motor.cogging_dq_comp.1019 0 +motor.cogging_dq_comp.1020 0 +motor.cogging_dq_comp.1021 0 +motor.cogging_dq_comp.1022 0 +motor.cogging_dq_comp.1023 0 +drv8323_conf.dis_cpuv 0 +drv8323_conf.dis_gdf 0 +drv8323_conf.otw_rep 0 +drv8323_conf.pwm_mode 1 +drv8323_conf.pwm_1x_asynchronous 0 +drv8323_conf.pwm_1x_dir 0 +drv8323_conf.idrivep_hs_ma 100 +drv8323_conf.idriven_hs_ma 200 +drv8323_conf.cbc 1 +drv8323_conf.tdrive_ns 1000 +drv8323_conf.idrivep_ls_ma 100 +drv8323_conf.idriven_ls_ma 200 +drv8323_conf.tretry 0 +drv8323_conf.dead_time_ns 50 +drv8323_conf.ocp_mode 0 +drv8323_conf.ocp_deg_us 4 +drv8323_conf.vds_lvl_mv 700 +drv8323_conf.csa_fet 0 +drv8323_conf.vref_div 1 +drv8323_conf.ls_ref 0 +drv8323_conf.csa_gain 20 +drv8323_conf.dis_sen 0 +drv8323_conf.sen_lvl_mv 500 +servo.pwm_rate_hz 30000 +servo.i_gain 20.000000 +servo.current_sense_ohm 0.000500 +servo.pwm_comp_off 0.027000 +servo.pwm_comp_mag 0.005000 +servo.pwm_scale 1.000000 +servo.max_voltage 46.000000 +servo.max_power_W 450.000000 +servo.derate_temperature 50.000000 +servo.fault_temperature 75.000000 +servo.enable_motor_temperature 0 +servo.motor_derate_temperature 50.000000 +servo.motor_fault_temperature nan +servo.velocity_threshold 0.000000 +servo.position_derate 0.020000 +servo.adc_cur_cycles 2 +servo.adc_aux_cycles 47 +servo.pid_dq.kp 0.200000 +servo.pid_dq.ki 157.382980 +servo.pid_position.kp 0.500000 +servo.pid_position.ki 0.000000 +servo.pid_position.kd 0.050000 +servo.pid_position.iratelimit -1.000000 +servo.pid_position.ilimit 0.000000 +servo.pid_position.max_desired_rate 0.000000 +servo.pid_position.sign -1 +servo.current_feedforward 1.000000 +servo.bemf_feedforward 1.000000 +servo.default_velocity_limit nan +servo.default_accel_limit nan +servo.voltage_mode_control 0 +servo.fixed_voltage_mode 0 +servo.fixed_voltage_control_V 0.000000 +servo.max_position_slip 10.000000 +servo.default_timeout_s 0.100000 +servo.timeout_max_torque_Nm 5.000000 +servo.timeout_mode 12 +servo.flux_brake_min_voltage 43.500000 +servo.flux_brake_resistance_ohm 0.025000 +servo.max_current_A 4.000000 +servo.derate_current_A -20.000000 +servo.max_velocity 500.000000 +servo.max_velocity_derate 2.000000 +servo.cooldown_cycles 256 +servo.velocity_zero_capture_threshold 0.050000 +servo.emit_debug 0 +servopos.position_min nan +servopos.position_max nan +id.id 21 +can.prefix 0 + diff --git a/config/moteus/front_left.cfg b/config/moteus/front_left.cfg new file mode 100644 index 000000000..21c7f1cfd --- /dev/null +++ b/config/moteus/front_left.cfg @@ -0,0 +1,1397 @@ +uuid.uuid.0 229 +uuid.uuid.1 88 +uuid.uuid.2 217 +uuid.uuid.3 47 +uuid.uuid.4 29 +uuid.uuid.5 180 +uuid.uuid.6 75 +uuid.uuid.7 173 +uuid.uuid.8 178 +uuid.uuid.9 53 +uuid.uuid.10 239 +uuid.uuid.11 190 +uuid.uuid.12 69 +uuid.uuid.13 242 +uuid.uuid.14 159 +uuid.uuid.15 48 +clock.hsitrim 64 +aux1.i2c.i2c_hz 400000 +aux1.i2c.i2c_mode 1 +aux1.i2c.pullup 0 +aux1.i2c.devices.0.type 0 +aux1.i2c.devices.0.address 64 +aux1.i2c.devices.0.poll_ms 10 +aux1.i2c.devices.1.type 0 +aux1.i2c.devices.1.address 64 +aux1.i2c.devices.1.poll_ms 10 +aux1.i2c.devices.2.type 0 +aux1.i2c.devices.2.address 64 +aux1.i2c.devices.2.poll_ms 10 +aux1.spi.mode 1 +aux1.spi.rate_hz 12000000 +aux1.spi.filter_us 64 +aux1.spi.bct 0 +aux1.uart.mode 0 +aux1.uart.baud_rate 115200 +aux1.uart.poll_rate_us 100 +aux1.uart.rs422 0 +aux1.uart.cui_amt21_address 84 +aux1.quadrature.enabled 0 +aux1.quadrature.cpr 16384 +aux1.hall.enabled 1 +aux1.hall.polarity 0 +aux1.index.enabled 0 +aux1.sine_cosine.enabled 0 +aux1.sine_cosine.common 1700 +aux1.i2c_startup_delay_ms 30 +aux1.pins.0.mode 6 +aux1.pins.0.pull 1 +aux1.pins.1.mode 6 +aux1.pins.1.pull 1 +aux1.pins.2.mode 6 +aux1.pins.2.pull 1 +aux1.pins.3.mode 0 +aux1.pins.3.pull 0 +aux1.pins.4.mode 0 +aux1.pins.4.pull 0 +aux2.i2c.i2c_hz 400000 +aux2.i2c.i2c_mode 1 +aux2.i2c.pullup 0 +aux2.i2c.devices.0.type 0 +aux2.i2c.devices.0.address 64 +aux2.i2c.devices.0.poll_ms 10 +aux2.i2c.devices.1.type 0 +aux2.i2c.devices.1.address 64 +aux2.i2c.devices.1.poll_ms 10 +aux2.i2c.devices.2.type 0 +aux2.i2c.devices.2.address 64 +aux2.i2c.devices.2.poll_ms 10 +aux2.spi.mode 1 +aux2.spi.rate_hz 12000000 +aux2.spi.filter_us 64 +aux2.spi.bct 0 +aux2.uart.mode 0 +aux2.uart.baud_rate 115200 +aux2.uart.poll_rate_us 100 +aux2.uart.rs422 0 +aux2.uart.cui_amt21_address 84 +aux2.quadrature.enabled 0 +aux2.quadrature.cpr 16384 +aux2.hall.enabled 0 +aux2.hall.polarity 0 +aux2.index.enabled 0 +aux2.sine_cosine.enabled 0 +aux2.sine_cosine.common 1700 +aux2.i2c_startup_delay_ms 30 +aux2.pins.0.mode 0 +aux2.pins.0.pull 0 +aux2.pins.1.mode 0 +aux2.pins.1.pull 0 +aux2.pins.2.mode 0 +aux2.pins.2.pull 0 +aux2.pins.3.mode 0 +aux2.pins.3.pull 0 +aux2.pins.4.mode 0 +aux2.pins.4.pull 0 +motor_position.sources.0.aux_number 1 +motor_position.sources.0.type 4 +motor_position.sources.0.i2c_device 0 +motor_position.sources.0.incremental_index -1 +motor_position.sources.0.cpr 84 +motor_position.sources.0.offset -5.000000 +motor_position.sources.0.sign 1 +motor_position.sources.0.debug_override -1 +motor_position.sources.0.reference 0 +motor_position.sources.0.pll_filter_hz 61.997570 +motor_position.sources.0.compensation_table.0 0.000000 +motor_position.sources.0.compensation_table.1 0.000000 +motor_position.sources.0.compensation_table.2 0.000000 +motor_position.sources.0.compensation_table.3 0.000000 +motor_position.sources.0.compensation_table.4 0.000000 +motor_position.sources.0.compensation_table.5 0.000000 +motor_position.sources.0.compensation_table.6 0.000000 +motor_position.sources.0.compensation_table.7 0.000000 +motor_position.sources.0.compensation_table.8 0.000000 +motor_position.sources.0.compensation_table.9 0.000000 +motor_position.sources.0.compensation_table.10 0.000000 +motor_position.sources.0.compensation_table.11 0.000000 +motor_position.sources.0.compensation_table.12 0.000000 +motor_position.sources.0.compensation_table.13 0.000000 +motor_position.sources.0.compensation_table.14 0.000000 +motor_position.sources.0.compensation_table.15 0.000000 +motor_position.sources.0.compensation_table.16 0.000000 +motor_position.sources.0.compensation_table.17 0.000000 +motor_position.sources.0.compensation_table.18 0.000000 +motor_position.sources.0.compensation_table.19 0.000000 +motor_position.sources.0.compensation_table.20 0.000000 +motor_position.sources.0.compensation_table.21 0.000000 +motor_position.sources.0.compensation_table.22 0.000000 +motor_position.sources.0.compensation_table.23 0.000000 +motor_position.sources.0.compensation_table.24 0.000000 +motor_position.sources.0.compensation_table.25 0.000000 +motor_position.sources.0.compensation_table.26 0.000000 +motor_position.sources.0.compensation_table.27 0.000000 +motor_position.sources.0.compensation_table.28 0.000000 +motor_position.sources.0.compensation_table.29 0.000000 +motor_position.sources.0.compensation_table.30 0.000000 +motor_position.sources.0.compensation_table.31 0.000000 +motor_position.sources.1.aux_number 1 +motor_position.sources.1.type 0 +motor_position.sources.1.i2c_device 0 +motor_position.sources.1.incremental_index -1 +motor_position.sources.1.cpr 16384 +motor_position.sources.1.offset 0.000000 +motor_position.sources.1.sign 1 +motor_position.sources.1.debug_override -1 +motor_position.sources.1.reference 0 +motor_position.sources.1.pll_filter_hz 400.000000 +motor_position.sources.1.compensation_table.0 0.000000 +motor_position.sources.1.compensation_table.1 0.000000 +motor_position.sources.1.compensation_table.2 0.000000 +motor_position.sources.1.compensation_table.3 0.000000 +motor_position.sources.1.compensation_table.4 0.000000 +motor_position.sources.1.compensation_table.5 0.000000 +motor_position.sources.1.compensation_table.6 0.000000 +motor_position.sources.1.compensation_table.7 0.000000 +motor_position.sources.1.compensation_table.8 0.000000 +motor_position.sources.1.compensation_table.9 0.000000 +motor_position.sources.1.compensation_table.10 0.000000 +motor_position.sources.1.compensation_table.11 0.000000 +motor_position.sources.1.compensation_table.12 0.000000 +motor_position.sources.1.compensation_table.13 0.000000 +motor_position.sources.1.compensation_table.14 0.000000 +motor_position.sources.1.compensation_table.15 0.000000 +motor_position.sources.1.compensation_table.16 0.000000 +motor_position.sources.1.compensation_table.17 0.000000 +motor_position.sources.1.compensation_table.18 0.000000 +motor_position.sources.1.compensation_table.19 0.000000 +motor_position.sources.1.compensation_table.20 0.000000 +motor_position.sources.1.compensation_table.21 0.000000 +motor_position.sources.1.compensation_table.22 0.000000 +motor_position.sources.1.compensation_table.23 0.000000 +motor_position.sources.1.compensation_table.24 0.000000 +motor_position.sources.1.compensation_table.25 0.000000 +motor_position.sources.1.compensation_table.26 0.000000 +motor_position.sources.1.compensation_table.27 0.000000 +motor_position.sources.1.compensation_table.28 0.000000 +motor_position.sources.1.compensation_table.29 0.000000 +motor_position.sources.1.compensation_table.30 0.000000 +motor_position.sources.1.compensation_table.31 0.000000 +motor_position.sources.2.aux_number 1 +motor_position.sources.2.type 0 +motor_position.sources.2.i2c_device 0 +motor_position.sources.2.incremental_index -1 +motor_position.sources.2.cpr 16384 +motor_position.sources.2.offset 0.000000 +motor_position.sources.2.sign 1 +motor_position.sources.2.debug_override -1 +motor_position.sources.2.reference 0 +motor_position.sources.2.pll_filter_hz 400.000000 +motor_position.sources.2.compensation_table.0 0.000000 +motor_position.sources.2.compensation_table.1 0.000000 +motor_position.sources.2.compensation_table.2 0.000000 +motor_position.sources.2.compensation_table.3 0.000000 +motor_position.sources.2.compensation_table.4 0.000000 +motor_position.sources.2.compensation_table.5 0.000000 +motor_position.sources.2.compensation_table.6 0.000000 +motor_position.sources.2.compensation_table.7 0.000000 +motor_position.sources.2.compensation_table.8 0.000000 +motor_position.sources.2.compensation_table.9 0.000000 +motor_position.sources.2.compensation_table.10 0.000000 +motor_position.sources.2.compensation_table.11 0.000000 +motor_position.sources.2.compensation_table.12 0.000000 +motor_position.sources.2.compensation_table.13 0.000000 +motor_position.sources.2.compensation_table.14 0.000000 +motor_position.sources.2.compensation_table.15 0.000000 +motor_position.sources.2.compensation_table.16 0.000000 +motor_position.sources.2.compensation_table.17 0.000000 +motor_position.sources.2.compensation_table.18 0.000000 +motor_position.sources.2.compensation_table.19 0.000000 +motor_position.sources.2.compensation_table.20 0.000000 +motor_position.sources.2.compensation_table.21 0.000000 +motor_position.sources.2.compensation_table.22 0.000000 +motor_position.sources.2.compensation_table.23 0.000000 +motor_position.sources.2.compensation_table.24 0.000000 +motor_position.sources.2.compensation_table.25 0.000000 +motor_position.sources.2.compensation_table.26 0.000000 +motor_position.sources.2.compensation_table.27 0.000000 +motor_position.sources.2.compensation_table.28 0.000000 +motor_position.sources.2.compensation_table.29 0.000000 +motor_position.sources.2.compensation_table.30 0.000000 +motor_position.sources.2.compensation_table.31 0.000000 +motor_position.commutation_source 0 +motor_position.output.source 0 +motor_position.output.offset 0.000000 +motor_position.output.sign -1 +motor_position.output.reference_source -1 +motor_position.rotor_to_output_ratio 1.000000 +motor.poles 28 +motor.phase_invert 0 +motor.resistance_ohm 0.235030 +motor.v_per_hz 0.437548 +motor.offset.0 0.000000 +motor.offset.1 0.000000 +motor.offset.2 0.000000 +motor.offset.3 0.000000 +motor.offset.4 0.000000 +motor.offset.5 0.000000 +motor.offset.6 0.000000 +motor.offset.7 0.000000 +motor.offset.8 0.000000 +motor.offset.9 0.000000 +motor.offset.10 0.000000 +motor.offset.11 0.000000 +motor.offset.12 0.000000 +motor.offset.13 0.000000 +motor.offset.14 0.000000 +motor.offset.15 0.000000 +motor.offset.16 0.000000 +motor.offset.17 0.000000 +motor.offset.18 0.000000 +motor.offset.19 0.000000 +motor.offset.20 0.000000 +motor.offset.21 0.000000 +motor.offset.22 0.000000 +motor.offset.23 0.000000 +motor.offset.24 0.000000 +motor.offset.25 0.000000 +motor.offset.26 0.000000 +motor.offset.27 0.000000 +motor.offset.28 0.000000 +motor.offset.29 0.000000 +motor.offset.30 0.000000 +motor.offset.31 0.000000 +motor.offset.32 0.000000 +motor.offset.33 0.000000 +motor.offset.34 0.000000 +motor.offset.35 0.000000 +motor.offset.36 0.000000 +motor.offset.37 0.000000 +motor.offset.38 0.000000 +motor.offset.39 0.000000 +motor.offset.40 0.000000 +motor.offset.41 0.000000 +motor.offset.42 0.000000 +motor.offset.43 0.000000 +motor.offset.44 0.000000 +motor.offset.45 0.000000 +motor.offset.46 0.000000 +motor.offset.47 0.000000 +motor.offset.48 0.000000 +motor.offset.49 0.000000 +motor.offset.50 0.000000 +motor.offset.51 0.000000 +motor.offset.52 0.000000 +motor.offset.53 0.000000 +motor.offset.54 0.000000 +motor.offset.55 0.000000 +motor.offset.56 0.000000 +motor.offset.57 0.000000 +motor.offset.58 0.000000 +motor.offset.59 0.000000 +motor.offset.60 0.000000 +motor.offset.61 0.000000 +motor.offset.62 0.000000 +motor.offset.63 0.000000 +motor.rotation_current_cutoff_A 10000.000000 +motor.rotation_current_scale 0.050000 +motor.rotation_torque_scale 14.700000 +motor.cogging_dq_scale 0.000000 +motor.cogging_dq_comp.0 0 +motor.cogging_dq_comp.1 0 +motor.cogging_dq_comp.2 0 +motor.cogging_dq_comp.3 0 +motor.cogging_dq_comp.4 0 +motor.cogging_dq_comp.5 0 +motor.cogging_dq_comp.6 0 +motor.cogging_dq_comp.7 0 +motor.cogging_dq_comp.8 0 +motor.cogging_dq_comp.9 0 +motor.cogging_dq_comp.10 0 +motor.cogging_dq_comp.11 0 +motor.cogging_dq_comp.12 0 +motor.cogging_dq_comp.13 0 +motor.cogging_dq_comp.14 0 +motor.cogging_dq_comp.15 0 +motor.cogging_dq_comp.16 0 +motor.cogging_dq_comp.17 0 +motor.cogging_dq_comp.18 0 +motor.cogging_dq_comp.19 0 +motor.cogging_dq_comp.20 0 +motor.cogging_dq_comp.21 0 +motor.cogging_dq_comp.22 0 +motor.cogging_dq_comp.23 0 +motor.cogging_dq_comp.24 0 +motor.cogging_dq_comp.25 0 +motor.cogging_dq_comp.26 0 +motor.cogging_dq_comp.27 0 +motor.cogging_dq_comp.28 0 +motor.cogging_dq_comp.29 0 +motor.cogging_dq_comp.30 0 +motor.cogging_dq_comp.31 0 +motor.cogging_dq_comp.32 0 +motor.cogging_dq_comp.33 0 +motor.cogging_dq_comp.34 0 +motor.cogging_dq_comp.35 0 +motor.cogging_dq_comp.36 0 +motor.cogging_dq_comp.37 0 +motor.cogging_dq_comp.38 0 +motor.cogging_dq_comp.39 0 +motor.cogging_dq_comp.40 0 +motor.cogging_dq_comp.41 0 +motor.cogging_dq_comp.42 0 +motor.cogging_dq_comp.43 0 +motor.cogging_dq_comp.44 0 +motor.cogging_dq_comp.45 0 +motor.cogging_dq_comp.46 0 +motor.cogging_dq_comp.47 0 +motor.cogging_dq_comp.48 0 +motor.cogging_dq_comp.49 0 +motor.cogging_dq_comp.50 0 +motor.cogging_dq_comp.51 0 +motor.cogging_dq_comp.52 0 +motor.cogging_dq_comp.53 0 +motor.cogging_dq_comp.54 0 +motor.cogging_dq_comp.55 0 +motor.cogging_dq_comp.56 0 +motor.cogging_dq_comp.57 0 +motor.cogging_dq_comp.58 0 +motor.cogging_dq_comp.59 0 +motor.cogging_dq_comp.60 0 +motor.cogging_dq_comp.61 0 +motor.cogging_dq_comp.62 0 +motor.cogging_dq_comp.63 0 +motor.cogging_dq_comp.64 0 +motor.cogging_dq_comp.65 0 +motor.cogging_dq_comp.66 0 +motor.cogging_dq_comp.67 0 +motor.cogging_dq_comp.68 0 +motor.cogging_dq_comp.69 0 +motor.cogging_dq_comp.70 0 +motor.cogging_dq_comp.71 0 +motor.cogging_dq_comp.72 0 +motor.cogging_dq_comp.73 0 +motor.cogging_dq_comp.74 0 +motor.cogging_dq_comp.75 0 +motor.cogging_dq_comp.76 0 +motor.cogging_dq_comp.77 0 +motor.cogging_dq_comp.78 0 +motor.cogging_dq_comp.79 0 +motor.cogging_dq_comp.80 0 +motor.cogging_dq_comp.81 0 +motor.cogging_dq_comp.82 0 +motor.cogging_dq_comp.83 0 +motor.cogging_dq_comp.84 0 +motor.cogging_dq_comp.85 0 +motor.cogging_dq_comp.86 0 +motor.cogging_dq_comp.87 0 +motor.cogging_dq_comp.88 0 +motor.cogging_dq_comp.89 0 +motor.cogging_dq_comp.90 0 +motor.cogging_dq_comp.91 0 +motor.cogging_dq_comp.92 0 +motor.cogging_dq_comp.93 0 +motor.cogging_dq_comp.94 0 +motor.cogging_dq_comp.95 0 +motor.cogging_dq_comp.96 0 +motor.cogging_dq_comp.97 0 +motor.cogging_dq_comp.98 0 +motor.cogging_dq_comp.99 0 +motor.cogging_dq_comp.100 0 +motor.cogging_dq_comp.101 0 +motor.cogging_dq_comp.102 0 +motor.cogging_dq_comp.103 0 +motor.cogging_dq_comp.104 0 +motor.cogging_dq_comp.105 0 +motor.cogging_dq_comp.106 0 +motor.cogging_dq_comp.107 0 +motor.cogging_dq_comp.108 0 +motor.cogging_dq_comp.109 0 +motor.cogging_dq_comp.110 0 +motor.cogging_dq_comp.111 0 +motor.cogging_dq_comp.112 0 +motor.cogging_dq_comp.113 0 +motor.cogging_dq_comp.114 0 +motor.cogging_dq_comp.115 0 +motor.cogging_dq_comp.116 0 +motor.cogging_dq_comp.117 0 +motor.cogging_dq_comp.118 0 +motor.cogging_dq_comp.119 0 +motor.cogging_dq_comp.120 0 +motor.cogging_dq_comp.121 0 +motor.cogging_dq_comp.122 0 +motor.cogging_dq_comp.123 0 +motor.cogging_dq_comp.124 0 +motor.cogging_dq_comp.125 0 +motor.cogging_dq_comp.126 0 +motor.cogging_dq_comp.127 0 +motor.cogging_dq_comp.128 0 +motor.cogging_dq_comp.129 0 +motor.cogging_dq_comp.130 0 +motor.cogging_dq_comp.131 0 +motor.cogging_dq_comp.132 0 +motor.cogging_dq_comp.133 0 +motor.cogging_dq_comp.134 0 +motor.cogging_dq_comp.135 0 +motor.cogging_dq_comp.136 0 +motor.cogging_dq_comp.137 0 +motor.cogging_dq_comp.138 0 +motor.cogging_dq_comp.139 0 +motor.cogging_dq_comp.140 0 +motor.cogging_dq_comp.141 0 +motor.cogging_dq_comp.142 0 +motor.cogging_dq_comp.143 0 +motor.cogging_dq_comp.144 0 +motor.cogging_dq_comp.145 0 +motor.cogging_dq_comp.146 0 +motor.cogging_dq_comp.147 0 +motor.cogging_dq_comp.148 0 +motor.cogging_dq_comp.149 0 +motor.cogging_dq_comp.150 0 +motor.cogging_dq_comp.151 0 +motor.cogging_dq_comp.152 0 +motor.cogging_dq_comp.153 0 +motor.cogging_dq_comp.154 0 +motor.cogging_dq_comp.155 0 +motor.cogging_dq_comp.156 0 +motor.cogging_dq_comp.157 0 +motor.cogging_dq_comp.158 0 +motor.cogging_dq_comp.159 0 +motor.cogging_dq_comp.160 0 +motor.cogging_dq_comp.161 0 +motor.cogging_dq_comp.162 0 +motor.cogging_dq_comp.163 0 +motor.cogging_dq_comp.164 0 +motor.cogging_dq_comp.165 0 +motor.cogging_dq_comp.166 0 +motor.cogging_dq_comp.167 0 +motor.cogging_dq_comp.168 0 +motor.cogging_dq_comp.169 0 +motor.cogging_dq_comp.170 0 +motor.cogging_dq_comp.171 0 +motor.cogging_dq_comp.172 0 +motor.cogging_dq_comp.173 0 +motor.cogging_dq_comp.174 0 +motor.cogging_dq_comp.175 0 +motor.cogging_dq_comp.176 0 +motor.cogging_dq_comp.177 0 +motor.cogging_dq_comp.178 0 +motor.cogging_dq_comp.179 0 +motor.cogging_dq_comp.180 0 +motor.cogging_dq_comp.181 0 +motor.cogging_dq_comp.182 0 +motor.cogging_dq_comp.183 0 +motor.cogging_dq_comp.184 0 +motor.cogging_dq_comp.185 0 +motor.cogging_dq_comp.186 0 +motor.cogging_dq_comp.187 0 +motor.cogging_dq_comp.188 0 +motor.cogging_dq_comp.189 0 +motor.cogging_dq_comp.190 0 +motor.cogging_dq_comp.191 0 +motor.cogging_dq_comp.192 0 +motor.cogging_dq_comp.193 0 +motor.cogging_dq_comp.194 0 +motor.cogging_dq_comp.195 0 +motor.cogging_dq_comp.196 0 +motor.cogging_dq_comp.197 0 +motor.cogging_dq_comp.198 0 +motor.cogging_dq_comp.199 0 +motor.cogging_dq_comp.200 0 +motor.cogging_dq_comp.201 0 +motor.cogging_dq_comp.202 0 +motor.cogging_dq_comp.203 0 +motor.cogging_dq_comp.204 0 +motor.cogging_dq_comp.205 0 +motor.cogging_dq_comp.206 0 +motor.cogging_dq_comp.207 0 +motor.cogging_dq_comp.208 0 +motor.cogging_dq_comp.209 0 +motor.cogging_dq_comp.210 0 +motor.cogging_dq_comp.211 0 +motor.cogging_dq_comp.212 0 +motor.cogging_dq_comp.213 0 +motor.cogging_dq_comp.214 0 +motor.cogging_dq_comp.215 0 +motor.cogging_dq_comp.216 0 +motor.cogging_dq_comp.217 0 +motor.cogging_dq_comp.218 0 +motor.cogging_dq_comp.219 0 +motor.cogging_dq_comp.220 0 +motor.cogging_dq_comp.221 0 +motor.cogging_dq_comp.222 0 +motor.cogging_dq_comp.223 0 +motor.cogging_dq_comp.224 0 +motor.cogging_dq_comp.225 0 +motor.cogging_dq_comp.226 0 +motor.cogging_dq_comp.227 0 +motor.cogging_dq_comp.228 0 +motor.cogging_dq_comp.229 0 +motor.cogging_dq_comp.230 0 +motor.cogging_dq_comp.231 0 +motor.cogging_dq_comp.232 0 +motor.cogging_dq_comp.233 0 +motor.cogging_dq_comp.234 0 +motor.cogging_dq_comp.235 0 +motor.cogging_dq_comp.236 0 +motor.cogging_dq_comp.237 0 +motor.cogging_dq_comp.238 0 +motor.cogging_dq_comp.239 0 +motor.cogging_dq_comp.240 0 +motor.cogging_dq_comp.241 0 +motor.cogging_dq_comp.242 0 +motor.cogging_dq_comp.243 0 +motor.cogging_dq_comp.244 0 +motor.cogging_dq_comp.245 0 +motor.cogging_dq_comp.246 0 +motor.cogging_dq_comp.247 0 +motor.cogging_dq_comp.248 0 +motor.cogging_dq_comp.249 0 +motor.cogging_dq_comp.250 0 +motor.cogging_dq_comp.251 0 +motor.cogging_dq_comp.252 0 +motor.cogging_dq_comp.253 0 +motor.cogging_dq_comp.254 0 +motor.cogging_dq_comp.255 0 +motor.cogging_dq_comp.256 0 +motor.cogging_dq_comp.257 0 +motor.cogging_dq_comp.258 0 +motor.cogging_dq_comp.259 0 +motor.cogging_dq_comp.260 0 +motor.cogging_dq_comp.261 0 +motor.cogging_dq_comp.262 0 +motor.cogging_dq_comp.263 0 +motor.cogging_dq_comp.264 0 +motor.cogging_dq_comp.265 0 +motor.cogging_dq_comp.266 0 +motor.cogging_dq_comp.267 0 +motor.cogging_dq_comp.268 0 +motor.cogging_dq_comp.269 0 +motor.cogging_dq_comp.270 0 +motor.cogging_dq_comp.271 0 +motor.cogging_dq_comp.272 0 +motor.cogging_dq_comp.273 0 +motor.cogging_dq_comp.274 0 +motor.cogging_dq_comp.275 0 +motor.cogging_dq_comp.276 0 +motor.cogging_dq_comp.277 0 +motor.cogging_dq_comp.278 0 +motor.cogging_dq_comp.279 0 +motor.cogging_dq_comp.280 0 +motor.cogging_dq_comp.281 0 +motor.cogging_dq_comp.282 0 +motor.cogging_dq_comp.283 0 +motor.cogging_dq_comp.284 0 +motor.cogging_dq_comp.285 0 +motor.cogging_dq_comp.286 0 +motor.cogging_dq_comp.287 0 +motor.cogging_dq_comp.288 0 +motor.cogging_dq_comp.289 0 +motor.cogging_dq_comp.290 0 +motor.cogging_dq_comp.291 0 +motor.cogging_dq_comp.292 0 +motor.cogging_dq_comp.293 0 +motor.cogging_dq_comp.294 0 +motor.cogging_dq_comp.295 0 +motor.cogging_dq_comp.296 0 +motor.cogging_dq_comp.297 0 +motor.cogging_dq_comp.298 0 +motor.cogging_dq_comp.299 0 +motor.cogging_dq_comp.300 0 +motor.cogging_dq_comp.301 0 +motor.cogging_dq_comp.302 0 +motor.cogging_dq_comp.303 0 +motor.cogging_dq_comp.304 0 +motor.cogging_dq_comp.305 0 +motor.cogging_dq_comp.306 0 +motor.cogging_dq_comp.307 0 +motor.cogging_dq_comp.308 0 +motor.cogging_dq_comp.309 0 +motor.cogging_dq_comp.310 0 +motor.cogging_dq_comp.311 0 +motor.cogging_dq_comp.312 0 +motor.cogging_dq_comp.313 0 +motor.cogging_dq_comp.314 0 +motor.cogging_dq_comp.315 0 +motor.cogging_dq_comp.316 0 +motor.cogging_dq_comp.317 0 +motor.cogging_dq_comp.318 0 +motor.cogging_dq_comp.319 0 +motor.cogging_dq_comp.320 0 +motor.cogging_dq_comp.321 0 +motor.cogging_dq_comp.322 0 +motor.cogging_dq_comp.323 0 +motor.cogging_dq_comp.324 0 +motor.cogging_dq_comp.325 0 +motor.cogging_dq_comp.326 0 +motor.cogging_dq_comp.327 0 +motor.cogging_dq_comp.328 0 +motor.cogging_dq_comp.329 0 +motor.cogging_dq_comp.330 0 +motor.cogging_dq_comp.331 0 +motor.cogging_dq_comp.332 0 +motor.cogging_dq_comp.333 0 +motor.cogging_dq_comp.334 0 +motor.cogging_dq_comp.335 0 +motor.cogging_dq_comp.336 0 +motor.cogging_dq_comp.337 0 +motor.cogging_dq_comp.338 0 +motor.cogging_dq_comp.339 0 +motor.cogging_dq_comp.340 0 +motor.cogging_dq_comp.341 0 +motor.cogging_dq_comp.342 0 +motor.cogging_dq_comp.343 0 +motor.cogging_dq_comp.344 0 +motor.cogging_dq_comp.345 0 +motor.cogging_dq_comp.346 0 +motor.cogging_dq_comp.347 0 +motor.cogging_dq_comp.348 0 +motor.cogging_dq_comp.349 0 +motor.cogging_dq_comp.350 0 +motor.cogging_dq_comp.351 0 +motor.cogging_dq_comp.352 0 +motor.cogging_dq_comp.353 0 +motor.cogging_dq_comp.354 0 +motor.cogging_dq_comp.355 0 +motor.cogging_dq_comp.356 0 +motor.cogging_dq_comp.357 0 +motor.cogging_dq_comp.358 0 +motor.cogging_dq_comp.359 0 +motor.cogging_dq_comp.360 0 +motor.cogging_dq_comp.361 0 +motor.cogging_dq_comp.362 0 +motor.cogging_dq_comp.363 0 +motor.cogging_dq_comp.364 0 +motor.cogging_dq_comp.365 0 +motor.cogging_dq_comp.366 0 +motor.cogging_dq_comp.367 0 +motor.cogging_dq_comp.368 0 +motor.cogging_dq_comp.369 0 +motor.cogging_dq_comp.370 0 +motor.cogging_dq_comp.371 0 +motor.cogging_dq_comp.372 0 +motor.cogging_dq_comp.373 0 +motor.cogging_dq_comp.374 0 +motor.cogging_dq_comp.375 0 +motor.cogging_dq_comp.376 0 +motor.cogging_dq_comp.377 0 +motor.cogging_dq_comp.378 0 +motor.cogging_dq_comp.379 0 +motor.cogging_dq_comp.380 0 +motor.cogging_dq_comp.381 0 +motor.cogging_dq_comp.382 0 +motor.cogging_dq_comp.383 0 +motor.cogging_dq_comp.384 0 +motor.cogging_dq_comp.385 0 +motor.cogging_dq_comp.386 0 +motor.cogging_dq_comp.387 0 +motor.cogging_dq_comp.388 0 +motor.cogging_dq_comp.389 0 +motor.cogging_dq_comp.390 0 +motor.cogging_dq_comp.391 0 +motor.cogging_dq_comp.392 0 +motor.cogging_dq_comp.393 0 +motor.cogging_dq_comp.394 0 +motor.cogging_dq_comp.395 0 +motor.cogging_dq_comp.396 0 +motor.cogging_dq_comp.397 0 +motor.cogging_dq_comp.398 0 +motor.cogging_dq_comp.399 0 +motor.cogging_dq_comp.400 0 +motor.cogging_dq_comp.401 0 +motor.cogging_dq_comp.402 0 +motor.cogging_dq_comp.403 0 +motor.cogging_dq_comp.404 0 +motor.cogging_dq_comp.405 0 +motor.cogging_dq_comp.406 0 +motor.cogging_dq_comp.407 0 +motor.cogging_dq_comp.408 0 +motor.cogging_dq_comp.409 0 +motor.cogging_dq_comp.410 0 +motor.cogging_dq_comp.411 0 +motor.cogging_dq_comp.412 0 +motor.cogging_dq_comp.413 0 +motor.cogging_dq_comp.414 0 +motor.cogging_dq_comp.415 0 +motor.cogging_dq_comp.416 0 +motor.cogging_dq_comp.417 0 +motor.cogging_dq_comp.418 0 +motor.cogging_dq_comp.419 0 +motor.cogging_dq_comp.420 0 +motor.cogging_dq_comp.421 0 +motor.cogging_dq_comp.422 0 +motor.cogging_dq_comp.423 0 +motor.cogging_dq_comp.424 0 +motor.cogging_dq_comp.425 0 +motor.cogging_dq_comp.426 0 +motor.cogging_dq_comp.427 0 +motor.cogging_dq_comp.428 0 +motor.cogging_dq_comp.429 0 +motor.cogging_dq_comp.430 0 +motor.cogging_dq_comp.431 0 +motor.cogging_dq_comp.432 0 +motor.cogging_dq_comp.433 0 +motor.cogging_dq_comp.434 0 +motor.cogging_dq_comp.435 0 +motor.cogging_dq_comp.436 0 +motor.cogging_dq_comp.437 0 +motor.cogging_dq_comp.438 0 +motor.cogging_dq_comp.439 0 +motor.cogging_dq_comp.440 0 +motor.cogging_dq_comp.441 0 +motor.cogging_dq_comp.442 0 +motor.cogging_dq_comp.443 0 +motor.cogging_dq_comp.444 0 +motor.cogging_dq_comp.445 0 +motor.cogging_dq_comp.446 0 +motor.cogging_dq_comp.447 0 +motor.cogging_dq_comp.448 0 +motor.cogging_dq_comp.449 0 +motor.cogging_dq_comp.450 0 +motor.cogging_dq_comp.451 0 +motor.cogging_dq_comp.452 0 +motor.cogging_dq_comp.453 0 +motor.cogging_dq_comp.454 0 +motor.cogging_dq_comp.455 0 +motor.cogging_dq_comp.456 0 +motor.cogging_dq_comp.457 0 +motor.cogging_dq_comp.458 0 +motor.cogging_dq_comp.459 0 +motor.cogging_dq_comp.460 0 +motor.cogging_dq_comp.461 0 +motor.cogging_dq_comp.462 0 +motor.cogging_dq_comp.463 0 +motor.cogging_dq_comp.464 0 +motor.cogging_dq_comp.465 0 +motor.cogging_dq_comp.466 0 +motor.cogging_dq_comp.467 0 +motor.cogging_dq_comp.468 0 +motor.cogging_dq_comp.469 0 +motor.cogging_dq_comp.470 0 +motor.cogging_dq_comp.471 0 +motor.cogging_dq_comp.472 0 +motor.cogging_dq_comp.473 0 +motor.cogging_dq_comp.474 0 +motor.cogging_dq_comp.475 0 +motor.cogging_dq_comp.476 0 +motor.cogging_dq_comp.477 0 +motor.cogging_dq_comp.478 0 +motor.cogging_dq_comp.479 0 +motor.cogging_dq_comp.480 0 +motor.cogging_dq_comp.481 0 +motor.cogging_dq_comp.482 0 +motor.cogging_dq_comp.483 0 +motor.cogging_dq_comp.484 0 +motor.cogging_dq_comp.485 0 +motor.cogging_dq_comp.486 0 +motor.cogging_dq_comp.487 0 +motor.cogging_dq_comp.488 0 +motor.cogging_dq_comp.489 0 +motor.cogging_dq_comp.490 0 +motor.cogging_dq_comp.491 0 +motor.cogging_dq_comp.492 0 +motor.cogging_dq_comp.493 0 +motor.cogging_dq_comp.494 0 +motor.cogging_dq_comp.495 0 +motor.cogging_dq_comp.496 0 +motor.cogging_dq_comp.497 0 +motor.cogging_dq_comp.498 0 +motor.cogging_dq_comp.499 0 +motor.cogging_dq_comp.500 0 +motor.cogging_dq_comp.501 0 +motor.cogging_dq_comp.502 0 +motor.cogging_dq_comp.503 0 +motor.cogging_dq_comp.504 0 +motor.cogging_dq_comp.505 0 +motor.cogging_dq_comp.506 0 +motor.cogging_dq_comp.507 0 +motor.cogging_dq_comp.508 0 +motor.cogging_dq_comp.509 0 +motor.cogging_dq_comp.510 0 +motor.cogging_dq_comp.511 0 +motor.cogging_dq_comp.512 0 +motor.cogging_dq_comp.513 0 +motor.cogging_dq_comp.514 0 +motor.cogging_dq_comp.515 0 +motor.cogging_dq_comp.516 0 +motor.cogging_dq_comp.517 0 +motor.cogging_dq_comp.518 0 +motor.cogging_dq_comp.519 0 +motor.cogging_dq_comp.520 0 +motor.cogging_dq_comp.521 0 +motor.cogging_dq_comp.522 0 +motor.cogging_dq_comp.523 0 +motor.cogging_dq_comp.524 0 +motor.cogging_dq_comp.525 0 +motor.cogging_dq_comp.526 0 +motor.cogging_dq_comp.527 0 +motor.cogging_dq_comp.528 0 +motor.cogging_dq_comp.529 0 +motor.cogging_dq_comp.530 0 +motor.cogging_dq_comp.531 0 +motor.cogging_dq_comp.532 0 +motor.cogging_dq_comp.533 0 +motor.cogging_dq_comp.534 0 +motor.cogging_dq_comp.535 0 +motor.cogging_dq_comp.536 0 +motor.cogging_dq_comp.537 0 +motor.cogging_dq_comp.538 0 +motor.cogging_dq_comp.539 0 +motor.cogging_dq_comp.540 0 +motor.cogging_dq_comp.541 0 +motor.cogging_dq_comp.542 0 +motor.cogging_dq_comp.543 0 +motor.cogging_dq_comp.544 0 +motor.cogging_dq_comp.545 0 +motor.cogging_dq_comp.546 0 +motor.cogging_dq_comp.547 0 +motor.cogging_dq_comp.548 0 +motor.cogging_dq_comp.549 0 +motor.cogging_dq_comp.550 0 +motor.cogging_dq_comp.551 0 +motor.cogging_dq_comp.552 0 +motor.cogging_dq_comp.553 0 +motor.cogging_dq_comp.554 0 +motor.cogging_dq_comp.555 0 +motor.cogging_dq_comp.556 0 +motor.cogging_dq_comp.557 0 +motor.cogging_dq_comp.558 0 +motor.cogging_dq_comp.559 0 +motor.cogging_dq_comp.560 0 +motor.cogging_dq_comp.561 0 +motor.cogging_dq_comp.562 0 +motor.cogging_dq_comp.563 0 +motor.cogging_dq_comp.564 0 +motor.cogging_dq_comp.565 0 +motor.cogging_dq_comp.566 0 +motor.cogging_dq_comp.567 0 +motor.cogging_dq_comp.568 0 +motor.cogging_dq_comp.569 0 +motor.cogging_dq_comp.570 0 +motor.cogging_dq_comp.571 0 +motor.cogging_dq_comp.572 0 +motor.cogging_dq_comp.573 0 +motor.cogging_dq_comp.574 0 +motor.cogging_dq_comp.575 0 +motor.cogging_dq_comp.576 0 +motor.cogging_dq_comp.577 0 +motor.cogging_dq_comp.578 0 +motor.cogging_dq_comp.579 0 +motor.cogging_dq_comp.580 0 +motor.cogging_dq_comp.581 0 +motor.cogging_dq_comp.582 0 +motor.cogging_dq_comp.583 0 +motor.cogging_dq_comp.584 0 +motor.cogging_dq_comp.585 0 +motor.cogging_dq_comp.586 0 +motor.cogging_dq_comp.587 0 +motor.cogging_dq_comp.588 0 +motor.cogging_dq_comp.589 0 +motor.cogging_dq_comp.590 0 +motor.cogging_dq_comp.591 0 +motor.cogging_dq_comp.592 0 +motor.cogging_dq_comp.593 0 +motor.cogging_dq_comp.594 0 +motor.cogging_dq_comp.595 0 +motor.cogging_dq_comp.596 0 +motor.cogging_dq_comp.597 0 +motor.cogging_dq_comp.598 0 +motor.cogging_dq_comp.599 0 +motor.cogging_dq_comp.600 0 +motor.cogging_dq_comp.601 0 +motor.cogging_dq_comp.602 0 +motor.cogging_dq_comp.603 0 +motor.cogging_dq_comp.604 0 +motor.cogging_dq_comp.605 0 +motor.cogging_dq_comp.606 0 +motor.cogging_dq_comp.607 0 +motor.cogging_dq_comp.608 0 +motor.cogging_dq_comp.609 0 +motor.cogging_dq_comp.610 0 +motor.cogging_dq_comp.611 0 +motor.cogging_dq_comp.612 0 +motor.cogging_dq_comp.613 0 +motor.cogging_dq_comp.614 0 +motor.cogging_dq_comp.615 0 +motor.cogging_dq_comp.616 0 +motor.cogging_dq_comp.617 0 +motor.cogging_dq_comp.618 0 +motor.cogging_dq_comp.619 0 +motor.cogging_dq_comp.620 0 +motor.cogging_dq_comp.621 0 +motor.cogging_dq_comp.622 0 +motor.cogging_dq_comp.623 0 +motor.cogging_dq_comp.624 0 +motor.cogging_dq_comp.625 0 +motor.cogging_dq_comp.626 0 +motor.cogging_dq_comp.627 0 +motor.cogging_dq_comp.628 0 +motor.cogging_dq_comp.629 0 +motor.cogging_dq_comp.630 0 +motor.cogging_dq_comp.631 0 +motor.cogging_dq_comp.632 0 +motor.cogging_dq_comp.633 0 +motor.cogging_dq_comp.634 0 +motor.cogging_dq_comp.635 0 +motor.cogging_dq_comp.636 0 +motor.cogging_dq_comp.637 0 +motor.cogging_dq_comp.638 0 +motor.cogging_dq_comp.639 0 +motor.cogging_dq_comp.640 0 +motor.cogging_dq_comp.641 0 +motor.cogging_dq_comp.642 0 +motor.cogging_dq_comp.643 0 +motor.cogging_dq_comp.644 0 +motor.cogging_dq_comp.645 0 +motor.cogging_dq_comp.646 0 +motor.cogging_dq_comp.647 0 +motor.cogging_dq_comp.648 0 +motor.cogging_dq_comp.649 0 +motor.cogging_dq_comp.650 0 +motor.cogging_dq_comp.651 0 +motor.cogging_dq_comp.652 0 +motor.cogging_dq_comp.653 0 +motor.cogging_dq_comp.654 0 +motor.cogging_dq_comp.655 0 +motor.cogging_dq_comp.656 0 +motor.cogging_dq_comp.657 0 +motor.cogging_dq_comp.658 0 +motor.cogging_dq_comp.659 0 +motor.cogging_dq_comp.660 0 +motor.cogging_dq_comp.661 0 +motor.cogging_dq_comp.662 0 +motor.cogging_dq_comp.663 0 +motor.cogging_dq_comp.664 0 +motor.cogging_dq_comp.665 0 +motor.cogging_dq_comp.666 0 +motor.cogging_dq_comp.667 0 +motor.cogging_dq_comp.668 0 +motor.cogging_dq_comp.669 0 +motor.cogging_dq_comp.670 0 +motor.cogging_dq_comp.671 0 +motor.cogging_dq_comp.672 0 +motor.cogging_dq_comp.673 0 +motor.cogging_dq_comp.674 0 +motor.cogging_dq_comp.675 0 +motor.cogging_dq_comp.676 0 +motor.cogging_dq_comp.677 0 +motor.cogging_dq_comp.678 0 +motor.cogging_dq_comp.679 0 +motor.cogging_dq_comp.680 0 +motor.cogging_dq_comp.681 0 +motor.cogging_dq_comp.682 0 +motor.cogging_dq_comp.683 0 +motor.cogging_dq_comp.684 0 +motor.cogging_dq_comp.685 0 +motor.cogging_dq_comp.686 0 +motor.cogging_dq_comp.687 0 +motor.cogging_dq_comp.688 0 +motor.cogging_dq_comp.689 0 +motor.cogging_dq_comp.690 0 +motor.cogging_dq_comp.691 0 +motor.cogging_dq_comp.692 0 +motor.cogging_dq_comp.693 0 +motor.cogging_dq_comp.694 0 +motor.cogging_dq_comp.695 0 +motor.cogging_dq_comp.696 0 +motor.cogging_dq_comp.697 0 +motor.cogging_dq_comp.698 0 +motor.cogging_dq_comp.699 0 +motor.cogging_dq_comp.700 0 +motor.cogging_dq_comp.701 0 +motor.cogging_dq_comp.702 0 +motor.cogging_dq_comp.703 0 +motor.cogging_dq_comp.704 0 +motor.cogging_dq_comp.705 0 +motor.cogging_dq_comp.706 0 +motor.cogging_dq_comp.707 0 +motor.cogging_dq_comp.708 0 +motor.cogging_dq_comp.709 0 +motor.cogging_dq_comp.710 0 +motor.cogging_dq_comp.711 0 +motor.cogging_dq_comp.712 0 +motor.cogging_dq_comp.713 0 +motor.cogging_dq_comp.714 0 +motor.cogging_dq_comp.715 0 +motor.cogging_dq_comp.716 0 +motor.cogging_dq_comp.717 0 +motor.cogging_dq_comp.718 0 +motor.cogging_dq_comp.719 0 +motor.cogging_dq_comp.720 0 +motor.cogging_dq_comp.721 0 +motor.cogging_dq_comp.722 0 +motor.cogging_dq_comp.723 0 +motor.cogging_dq_comp.724 0 +motor.cogging_dq_comp.725 0 +motor.cogging_dq_comp.726 0 +motor.cogging_dq_comp.727 0 +motor.cogging_dq_comp.728 0 +motor.cogging_dq_comp.729 0 +motor.cogging_dq_comp.730 0 +motor.cogging_dq_comp.731 0 +motor.cogging_dq_comp.732 0 +motor.cogging_dq_comp.733 0 +motor.cogging_dq_comp.734 0 +motor.cogging_dq_comp.735 0 +motor.cogging_dq_comp.736 0 +motor.cogging_dq_comp.737 0 +motor.cogging_dq_comp.738 0 +motor.cogging_dq_comp.739 0 +motor.cogging_dq_comp.740 0 +motor.cogging_dq_comp.741 0 +motor.cogging_dq_comp.742 0 +motor.cogging_dq_comp.743 0 +motor.cogging_dq_comp.744 0 +motor.cogging_dq_comp.745 0 +motor.cogging_dq_comp.746 0 +motor.cogging_dq_comp.747 0 +motor.cogging_dq_comp.748 0 +motor.cogging_dq_comp.749 0 +motor.cogging_dq_comp.750 0 +motor.cogging_dq_comp.751 0 +motor.cogging_dq_comp.752 0 +motor.cogging_dq_comp.753 0 +motor.cogging_dq_comp.754 0 +motor.cogging_dq_comp.755 0 +motor.cogging_dq_comp.756 0 +motor.cogging_dq_comp.757 0 +motor.cogging_dq_comp.758 0 +motor.cogging_dq_comp.759 0 +motor.cogging_dq_comp.760 0 +motor.cogging_dq_comp.761 0 +motor.cogging_dq_comp.762 0 +motor.cogging_dq_comp.763 0 +motor.cogging_dq_comp.764 0 +motor.cogging_dq_comp.765 0 +motor.cogging_dq_comp.766 0 +motor.cogging_dq_comp.767 0 +motor.cogging_dq_comp.768 0 +motor.cogging_dq_comp.769 0 +motor.cogging_dq_comp.770 0 +motor.cogging_dq_comp.771 0 +motor.cogging_dq_comp.772 0 +motor.cogging_dq_comp.773 0 +motor.cogging_dq_comp.774 0 +motor.cogging_dq_comp.775 0 +motor.cogging_dq_comp.776 0 +motor.cogging_dq_comp.777 0 +motor.cogging_dq_comp.778 0 +motor.cogging_dq_comp.779 0 +motor.cogging_dq_comp.780 0 +motor.cogging_dq_comp.781 0 +motor.cogging_dq_comp.782 0 +motor.cogging_dq_comp.783 0 +motor.cogging_dq_comp.784 0 +motor.cogging_dq_comp.785 0 +motor.cogging_dq_comp.786 0 +motor.cogging_dq_comp.787 0 +motor.cogging_dq_comp.788 0 +motor.cogging_dq_comp.789 0 +motor.cogging_dq_comp.790 0 +motor.cogging_dq_comp.791 0 +motor.cogging_dq_comp.792 0 +motor.cogging_dq_comp.793 0 +motor.cogging_dq_comp.794 0 +motor.cogging_dq_comp.795 0 +motor.cogging_dq_comp.796 0 +motor.cogging_dq_comp.797 0 +motor.cogging_dq_comp.798 0 +motor.cogging_dq_comp.799 0 +motor.cogging_dq_comp.800 0 +motor.cogging_dq_comp.801 0 +motor.cogging_dq_comp.802 0 +motor.cogging_dq_comp.803 0 +motor.cogging_dq_comp.804 0 +motor.cogging_dq_comp.805 0 +motor.cogging_dq_comp.806 0 +motor.cogging_dq_comp.807 0 +motor.cogging_dq_comp.808 0 +motor.cogging_dq_comp.809 0 +motor.cogging_dq_comp.810 0 +motor.cogging_dq_comp.811 0 +motor.cogging_dq_comp.812 0 +motor.cogging_dq_comp.813 0 +motor.cogging_dq_comp.814 0 +motor.cogging_dq_comp.815 0 +motor.cogging_dq_comp.816 0 +motor.cogging_dq_comp.817 0 +motor.cogging_dq_comp.818 0 +motor.cogging_dq_comp.819 0 +motor.cogging_dq_comp.820 0 +motor.cogging_dq_comp.821 0 +motor.cogging_dq_comp.822 0 +motor.cogging_dq_comp.823 0 +motor.cogging_dq_comp.824 0 +motor.cogging_dq_comp.825 0 +motor.cogging_dq_comp.826 0 +motor.cogging_dq_comp.827 0 +motor.cogging_dq_comp.828 0 +motor.cogging_dq_comp.829 0 +motor.cogging_dq_comp.830 0 +motor.cogging_dq_comp.831 0 +motor.cogging_dq_comp.832 0 +motor.cogging_dq_comp.833 0 +motor.cogging_dq_comp.834 0 +motor.cogging_dq_comp.835 0 +motor.cogging_dq_comp.836 0 +motor.cogging_dq_comp.837 0 +motor.cogging_dq_comp.838 0 +motor.cogging_dq_comp.839 0 +motor.cogging_dq_comp.840 0 +motor.cogging_dq_comp.841 0 +motor.cogging_dq_comp.842 0 +motor.cogging_dq_comp.843 0 +motor.cogging_dq_comp.844 0 +motor.cogging_dq_comp.845 0 +motor.cogging_dq_comp.846 0 +motor.cogging_dq_comp.847 0 +motor.cogging_dq_comp.848 0 +motor.cogging_dq_comp.849 0 +motor.cogging_dq_comp.850 0 +motor.cogging_dq_comp.851 0 +motor.cogging_dq_comp.852 0 +motor.cogging_dq_comp.853 0 +motor.cogging_dq_comp.854 0 +motor.cogging_dq_comp.855 0 +motor.cogging_dq_comp.856 0 +motor.cogging_dq_comp.857 0 +motor.cogging_dq_comp.858 0 +motor.cogging_dq_comp.859 0 +motor.cogging_dq_comp.860 0 +motor.cogging_dq_comp.861 0 +motor.cogging_dq_comp.862 0 +motor.cogging_dq_comp.863 0 +motor.cogging_dq_comp.864 0 +motor.cogging_dq_comp.865 0 +motor.cogging_dq_comp.866 0 +motor.cogging_dq_comp.867 0 +motor.cogging_dq_comp.868 0 +motor.cogging_dq_comp.869 0 +motor.cogging_dq_comp.870 0 +motor.cogging_dq_comp.871 0 +motor.cogging_dq_comp.872 0 +motor.cogging_dq_comp.873 0 +motor.cogging_dq_comp.874 0 +motor.cogging_dq_comp.875 0 +motor.cogging_dq_comp.876 0 +motor.cogging_dq_comp.877 0 +motor.cogging_dq_comp.878 0 +motor.cogging_dq_comp.879 0 +motor.cogging_dq_comp.880 0 +motor.cogging_dq_comp.881 0 +motor.cogging_dq_comp.882 0 +motor.cogging_dq_comp.883 0 +motor.cogging_dq_comp.884 0 +motor.cogging_dq_comp.885 0 +motor.cogging_dq_comp.886 0 +motor.cogging_dq_comp.887 0 +motor.cogging_dq_comp.888 0 +motor.cogging_dq_comp.889 0 +motor.cogging_dq_comp.890 0 +motor.cogging_dq_comp.891 0 +motor.cogging_dq_comp.892 0 +motor.cogging_dq_comp.893 0 +motor.cogging_dq_comp.894 0 +motor.cogging_dq_comp.895 0 +motor.cogging_dq_comp.896 0 +motor.cogging_dq_comp.897 0 +motor.cogging_dq_comp.898 0 +motor.cogging_dq_comp.899 0 +motor.cogging_dq_comp.900 0 +motor.cogging_dq_comp.901 0 +motor.cogging_dq_comp.902 0 +motor.cogging_dq_comp.903 0 +motor.cogging_dq_comp.904 0 +motor.cogging_dq_comp.905 0 +motor.cogging_dq_comp.906 0 +motor.cogging_dq_comp.907 0 +motor.cogging_dq_comp.908 0 +motor.cogging_dq_comp.909 0 +motor.cogging_dq_comp.910 0 +motor.cogging_dq_comp.911 0 +motor.cogging_dq_comp.912 0 +motor.cogging_dq_comp.913 0 +motor.cogging_dq_comp.914 0 +motor.cogging_dq_comp.915 0 +motor.cogging_dq_comp.916 0 +motor.cogging_dq_comp.917 0 +motor.cogging_dq_comp.918 0 +motor.cogging_dq_comp.919 0 +motor.cogging_dq_comp.920 0 +motor.cogging_dq_comp.921 0 +motor.cogging_dq_comp.922 0 +motor.cogging_dq_comp.923 0 +motor.cogging_dq_comp.924 0 +motor.cogging_dq_comp.925 0 +motor.cogging_dq_comp.926 0 +motor.cogging_dq_comp.927 0 +motor.cogging_dq_comp.928 0 +motor.cogging_dq_comp.929 0 +motor.cogging_dq_comp.930 0 +motor.cogging_dq_comp.931 0 +motor.cogging_dq_comp.932 0 +motor.cogging_dq_comp.933 0 +motor.cogging_dq_comp.934 0 +motor.cogging_dq_comp.935 0 +motor.cogging_dq_comp.936 0 +motor.cogging_dq_comp.937 0 +motor.cogging_dq_comp.938 0 +motor.cogging_dq_comp.939 0 +motor.cogging_dq_comp.940 0 +motor.cogging_dq_comp.941 0 +motor.cogging_dq_comp.942 0 +motor.cogging_dq_comp.943 0 +motor.cogging_dq_comp.944 0 +motor.cogging_dq_comp.945 0 +motor.cogging_dq_comp.946 0 +motor.cogging_dq_comp.947 0 +motor.cogging_dq_comp.948 0 +motor.cogging_dq_comp.949 0 +motor.cogging_dq_comp.950 0 +motor.cogging_dq_comp.951 0 +motor.cogging_dq_comp.952 0 +motor.cogging_dq_comp.953 0 +motor.cogging_dq_comp.954 0 +motor.cogging_dq_comp.955 0 +motor.cogging_dq_comp.956 0 +motor.cogging_dq_comp.957 0 +motor.cogging_dq_comp.958 0 +motor.cogging_dq_comp.959 0 +motor.cogging_dq_comp.960 0 +motor.cogging_dq_comp.961 0 +motor.cogging_dq_comp.962 0 +motor.cogging_dq_comp.963 0 +motor.cogging_dq_comp.964 0 +motor.cogging_dq_comp.965 0 +motor.cogging_dq_comp.966 0 +motor.cogging_dq_comp.967 0 +motor.cogging_dq_comp.968 0 +motor.cogging_dq_comp.969 0 +motor.cogging_dq_comp.970 0 +motor.cogging_dq_comp.971 0 +motor.cogging_dq_comp.972 0 +motor.cogging_dq_comp.973 0 +motor.cogging_dq_comp.974 0 +motor.cogging_dq_comp.975 0 +motor.cogging_dq_comp.976 0 +motor.cogging_dq_comp.977 0 +motor.cogging_dq_comp.978 0 +motor.cogging_dq_comp.979 0 +motor.cogging_dq_comp.980 0 +motor.cogging_dq_comp.981 0 +motor.cogging_dq_comp.982 0 +motor.cogging_dq_comp.983 0 +motor.cogging_dq_comp.984 0 +motor.cogging_dq_comp.985 0 +motor.cogging_dq_comp.986 0 +motor.cogging_dq_comp.987 0 +motor.cogging_dq_comp.988 0 +motor.cogging_dq_comp.989 0 +motor.cogging_dq_comp.990 0 +motor.cogging_dq_comp.991 0 +motor.cogging_dq_comp.992 0 +motor.cogging_dq_comp.993 0 +motor.cogging_dq_comp.994 0 +motor.cogging_dq_comp.995 0 +motor.cogging_dq_comp.996 0 +motor.cogging_dq_comp.997 0 +motor.cogging_dq_comp.998 0 +motor.cogging_dq_comp.999 0 +motor.cogging_dq_comp.1000 0 +motor.cogging_dq_comp.1001 0 +motor.cogging_dq_comp.1002 0 +motor.cogging_dq_comp.1003 0 +motor.cogging_dq_comp.1004 0 +motor.cogging_dq_comp.1005 0 +motor.cogging_dq_comp.1006 0 +motor.cogging_dq_comp.1007 0 +motor.cogging_dq_comp.1008 0 +motor.cogging_dq_comp.1009 0 +motor.cogging_dq_comp.1010 0 +motor.cogging_dq_comp.1011 0 +motor.cogging_dq_comp.1012 0 +motor.cogging_dq_comp.1013 0 +motor.cogging_dq_comp.1014 0 +motor.cogging_dq_comp.1015 0 +motor.cogging_dq_comp.1016 0 +motor.cogging_dq_comp.1017 0 +motor.cogging_dq_comp.1018 0 +motor.cogging_dq_comp.1019 0 +motor.cogging_dq_comp.1020 0 +motor.cogging_dq_comp.1021 0 +motor.cogging_dq_comp.1022 0 +motor.cogging_dq_comp.1023 0 +drv8323_conf.dis_cpuv 0 +drv8323_conf.dis_gdf 0 +drv8323_conf.otw_rep 0 +drv8323_conf.pwm_mode 1 +drv8323_conf.pwm_1x_asynchronous 0 +drv8323_conf.pwm_1x_dir 0 +drv8323_conf.idrivep_hs_ma 100 +drv8323_conf.idriven_hs_ma 200 +drv8323_conf.cbc 1 +drv8323_conf.tdrive_ns 1000 +drv8323_conf.idrivep_ls_ma 100 +drv8323_conf.idriven_ls_ma 200 +drv8323_conf.tretry 0 +drv8323_conf.dead_time_ns 50 +drv8323_conf.ocp_mode 0 +drv8323_conf.ocp_deg_us 4 +drv8323_conf.vds_lvl_mv 700 +drv8323_conf.csa_fet 0 +drv8323_conf.vref_div 1 +drv8323_conf.ls_ref 0 +drv8323_conf.csa_gain 20 +drv8323_conf.dis_sen 0 +drv8323_conf.sen_lvl_mv 500 +servo.pwm_rate_hz 30000 +servo.i_gain 20.000000 +servo.current_sense_ohm 0.000500 +servo.pwm_comp_off 0.027000 +servo.pwm_comp_mag 0.005000 +servo.pwm_scale 1.000000 +servo.max_voltage 46.000000 +servo.max_power_W 450.000000 +servo.derate_temperature 50.000000 +servo.fault_temperature 75.000000 +servo.enable_motor_temperature 0 +servo.motor_derate_temperature 50.000000 +servo.motor_fault_temperature nan +servo.velocity_threshold 0.000000 +servo.position_derate 0.020000 +servo.adc_cur_cycles 2 +servo.adc_aux_cycles 47 +servo.pid_dq.kp 0.202691 +servo.pid_dq.ki 147.673920 +servo.pid_position.kp 0.500000 +servo.pid_position.ki 0.000000 +servo.pid_position.kd 0.050000 +servo.pid_position.iratelimit -1.000000 +servo.pid_position.ilimit 0.000000 +servo.pid_position.max_desired_rate 0.000000 +servo.pid_position.sign -1 +servo.current_feedforward 1.000000 +servo.bemf_feedforward 1.000000 +servo.default_velocity_limit nan +servo.default_accel_limit nan +servo.voltage_mode_control 0 +servo.fixed_voltage_mode 0 +servo.fixed_voltage_control_V 0.000000 +servo.max_position_slip 10.000000 +servo.default_timeout_s 0.100000 +servo.timeout_max_torque_Nm 5.000000 +servo.timeout_mode 12 +servo.flux_brake_min_voltage 43.500000 +servo.flux_brake_resistance_ohm 0.025000 +servo.max_current_A 7.500000 +servo.derate_current_A -20.000000 +servo.max_velocity 500.000000 +servo.max_velocity_derate 2.000000 +servo.cooldown_cycles 256 +servo.velocity_zero_capture_threshold 0.050000 +servo.emit_debug 0 +servopos.position_min nan +servopos.position_max nan +id.id 18 +can.prefix 0 + diff --git a/config/moteus/front_right.cfg b/config/moteus/front_right.cfg new file mode 100644 index 000000000..658a54eb5 --- /dev/null +++ b/config/moteus/front_right.cfg @@ -0,0 +1,1397 @@ +uuid.uuid.0 15 +uuid.uuid.1 46 +uuid.uuid.2 69 +uuid.uuid.3 9 +uuid.uuid.4 98 +uuid.uuid.5 199 +uuid.uuid.6 66 +uuid.uuid.7 137 +uuid.uuid.8 171 +uuid.uuid.9 14 +uuid.uuid.10 84 +uuid.uuid.11 252 +uuid.uuid.12 189 +uuid.uuid.13 133 +uuid.uuid.14 74 +uuid.uuid.15 234 +clock.hsitrim 64 +aux1.i2c.i2c_hz 400000 +aux1.i2c.i2c_mode 1 +aux1.i2c.pullup 0 +aux1.i2c.devices.0.type 0 +aux1.i2c.devices.0.address 64 +aux1.i2c.devices.0.poll_ms 10 +aux1.i2c.devices.1.type 0 +aux1.i2c.devices.1.address 64 +aux1.i2c.devices.1.poll_ms 10 +aux1.i2c.devices.2.type 0 +aux1.i2c.devices.2.address 64 +aux1.i2c.devices.2.poll_ms 10 +aux1.spi.mode 1 +aux1.spi.rate_hz 12000000 +aux1.spi.filter_us 64 +aux1.spi.bct 0 +aux1.uart.mode 0 +aux1.uart.baud_rate 115200 +aux1.uart.poll_rate_us 100 +aux1.uart.rs422 0 +aux1.uart.cui_amt21_address 84 +aux1.quadrature.enabled 0 +aux1.quadrature.cpr 16384 +aux1.hall.enabled 1 +aux1.hall.polarity 0 +aux1.index.enabled 0 +aux1.sine_cosine.enabled 0 +aux1.sine_cosine.common 1700 +aux1.i2c_startup_delay_ms 30 +aux1.pins.0.mode 6 +aux1.pins.0.pull 1 +aux1.pins.1.mode 6 +aux1.pins.1.pull 1 +aux1.pins.2.mode 6 +aux1.pins.2.pull 1 +aux1.pins.3.mode 0 +aux1.pins.3.pull 0 +aux1.pins.4.mode 0 +aux1.pins.4.pull 0 +aux2.i2c.i2c_hz 400000 +aux2.i2c.i2c_mode 1 +aux2.i2c.pullup 0 +aux2.i2c.devices.0.type 0 +aux2.i2c.devices.0.address 64 +aux2.i2c.devices.0.poll_ms 10 +aux2.i2c.devices.1.type 0 +aux2.i2c.devices.1.address 64 +aux2.i2c.devices.1.poll_ms 10 +aux2.i2c.devices.2.type 0 +aux2.i2c.devices.2.address 64 +aux2.i2c.devices.2.poll_ms 10 +aux2.spi.mode 0 +aux2.spi.rate_hz 12000000 +aux2.spi.filter_us 64 +aux2.spi.bct 0 +aux2.uart.mode 0 +aux2.uart.baud_rate 115200 +aux2.uart.poll_rate_us 100 +aux2.uart.rs422 0 +aux2.uart.cui_amt21_address 84 +aux2.quadrature.enabled 0 +aux2.quadrature.cpr 16384 +aux2.hall.enabled 0 +aux2.hall.polarity 0 +aux2.index.enabled 0 +aux2.sine_cosine.enabled 0 +aux2.sine_cosine.common 1700 +aux2.i2c_startup_delay_ms 30 +aux2.pins.0.mode 0 +aux2.pins.0.pull 0 +aux2.pins.1.mode 0 +aux2.pins.1.pull 0 +aux2.pins.2.mode 0 +aux2.pins.2.pull 0 +aux2.pins.3.mode 0 +aux2.pins.3.pull 0 +aux2.pins.4.mode 0 +aux2.pins.4.pull 0 +motor_position.sources.0.aux_number 1 +motor_position.sources.0.type 4 +motor_position.sources.0.i2c_device 0 +motor_position.sources.0.incremental_index -1 +motor_position.sources.0.cpr 84 +motor_position.sources.0.offset -1.000000 +motor_position.sources.0.sign 1 +motor_position.sources.0.debug_override -1 +motor_position.sources.0.reference 0 +motor_position.sources.0.pll_filter_hz 71.065834 +motor_position.sources.0.compensation_table.0 0.000000 +motor_position.sources.0.compensation_table.1 0.000000 +motor_position.sources.0.compensation_table.2 0.000000 +motor_position.sources.0.compensation_table.3 0.000000 +motor_position.sources.0.compensation_table.4 0.000000 +motor_position.sources.0.compensation_table.5 0.000000 +motor_position.sources.0.compensation_table.6 0.000000 +motor_position.sources.0.compensation_table.7 0.000000 +motor_position.sources.0.compensation_table.8 0.000000 +motor_position.sources.0.compensation_table.9 0.000000 +motor_position.sources.0.compensation_table.10 0.000000 +motor_position.sources.0.compensation_table.11 0.000000 +motor_position.sources.0.compensation_table.12 0.000000 +motor_position.sources.0.compensation_table.13 0.000000 +motor_position.sources.0.compensation_table.14 0.000000 +motor_position.sources.0.compensation_table.15 0.000000 +motor_position.sources.0.compensation_table.16 0.000000 +motor_position.sources.0.compensation_table.17 0.000000 +motor_position.sources.0.compensation_table.18 0.000000 +motor_position.sources.0.compensation_table.19 0.000000 +motor_position.sources.0.compensation_table.20 0.000000 +motor_position.sources.0.compensation_table.21 0.000000 +motor_position.sources.0.compensation_table.22 0.000000 +motor_position.sources.0.compensation_table.23 0.000000 +motor_position.sources.0.compensation_table.24 0.000000 +motor_position.sources.0.compensation_table.25 0.000000 +motor_position.sources.0.compensation_table.26 0.000000 +motor_position.sources.0.compensation_table.27 0.000000 +motor_position.sources.0.compensation_table.28 0.000000 +motor_position.sources.0.compensation_table.29 0.000000 +motor_position.sources.0.compensation_table.30 0.000000 +motor_position.sources.0.compensation_table.31 0.000000 +motor_position.sources.1.aux_number 1 +motor_position.sources.1.type 0 +motor_position.sources.1.i2c_device 0 +motor_position.sources.1.incremental_index -1 +motor_position.sources.1.cpr 84 +motor_position.sources.1.offset 0.000000 +motor_position.sources.1.sign 1 +motor_position.sources.1.debug_override -1 +motor_position.sources.1.reference 0 +motor_position.sources.1.pll_filter_hz 10.000000 +motor_position.sources.1.compensation_table.0 0.000000 +motor_position.sources.1.compensation_table.1 0.000000 +motor_position.sources.1.compensation_table.2 0.000000 +motor_position.sources.1.compensation_table.3 0.000000 +motor_position.sources.1.compensation_table.4 0.000000 +motor_position.sources.1.compensation_table.5 0.000000 +motor_position.sources.1.compensation_table.6 0.000000 +motor_position.sources.1.compensation_table.7 0.000000 +motor_position.sources.1.compensation_table.8 0.000000 +motor_position.sources.1.compensation_table.9 0.000000 +motor_position.sources.1.compensation_table.10 0.000000 +motor_position.sources.1.compensation_table.11 0.000000 +motor_position.sources.1.compensation_table.12 0.000000 +motor_position.sources.1.compensation_table.13 0.000000 +motor_position.sources.1.compensation_table.14 0.000000 +motor_position.sources.1.compensation_table.15 0.000000 +motor_position.sources.1.compensation_table.16 0.000000 +motor_position.sources.1.compensation_table.17 0.000000 +motor_position.sources.1.compensation_table.18 0.000000 +motor_position.sources.1.compensation_table.19 0.000000 +motor_position.sources.1.compensation_table.20 0.000000 +motor_position.sources.1.compensation_table.21 0.000000 +motor_position.sources.1.compensation_table.22 0.000000 +motor_position.sources.1.compensation_table.23 0.000000 +motor_position.sources.1.compensation_table.24 0.000000 +motor_position.sources.1.compensation_table.25 0.000000 +motor_position.sources.1.compensation_table.26 0.000000 +motor_position.sources.1.compensation_table.27 0.000000 +motor_position.sources.1.compensation_table.28 0.000000 +motor_position.sources.1.compensation_table.29 0.000000 +motor_position.sources.1.compensation_table.30 0.000000 +motor_position.sources.1.compensation_table.31 0.000000 +motor_position.sources.2.aux_number 1 +motor_position.sources.2.type 0 +motor_position.sources.2.i2c_device 0 +motor_position.sources.2.incremental_index -1 +motor_position.sources.2.cpr 16384 +motor_position.sources.2.offset 0.000000 +motor_position.sources.2.sign 1 +motor_position.sources.2.debug_override -1 +motor_position.sources.2.reference 0 +motor_position.sources.2.pll_filter_hz 400.000000 +motor_position.sources.2.compensation_table.0 0.000000 +motor_position.sources.2.compensation_table.1 0.000000 +motor_position.sources.2.compensation_table.2 0.000000 +motor_position.sources.2.compensation_table.3 0.000000 +motor_position.sources.2.compensation_table.4 0.000000 +motor_position.sources.2.compensation_table.5 0.000000 +motor_position.sources.2.compensation_table.6 0.000000 +motor_position.sources.2.compensation_table.7 0.000000 +motor_position.sources.2.compensation_table.8 0.000000 +motor_position.sources.2.compensation_table.9 0.000000 +motor_position.sources.2.compensation_table.10 0.000000 +motor_position.sources.2.compensation_table.11 0.000000 +motor_position.sources.2.compensation_table.12 0.000000 +motor_position.sources.2.compensation_table.13 0.000000 +motor_position.sources.2.compensation_table.14 0.000000 +motor_position.sources.2.compensation_table.15 0.000000 +motor_position.sources.2.compensation_table.16 0.000000 +motor_position.sources.2.compensation_table.17 0.000000 +motor_position.sources.2.compensation_table.18 0.000000 +motor_position.sources.2.compensation_table.19 0.000000 +motor_position.sources.2.compensation_table.20 0.000000 +motor_position.sources.2.compensation_table.21 0.000000 +motor_position.sources.2.compensation_table.22 0.000000 +motor_position.sources.2.compensation_table.23 0.000000 +motor_position.sources.2.compensation_table.24 0.000000 +motor_position.sources.2.compensation_table.25 0.000000 +motor_position.sources.2.compensation_table.26 0.000000 +motor_position.sources.2.compensation_table.27 0.000000 +motor_position.sources.2.compensation_table.28 0.000000 +motor_position.sources.2.compensation_table.29 0.000000 +motor_position.sources.2.compensation_table.30 0.000000 +motor_position.sources.2.compensation_table.31 0.000000 +motor_position.commutation_source 0 +motor_position.output.source 0 +motor_position.output.offset 0.000000 +motor_position.output.sign 1 +motor_position.output.reference_source -1 +motor_position.rotor_to_output_ratio 1.000000 +motor.poles 28 +motor.phase_invert 0 +motor.resistance_ohm 0.240511 +motor.v_per_hz 0.444322 +motor.offset.0 0.000000 +motor.offset.1 0.000000 +motor.offset.2 0.000000 +motor.offset.3 0.000000 +motor.offset.4 0.000000 +motor.offset.5 0.000000 +motor.offset.6 0.000000 +motor.offset.7 0.000000 +motor.offset.8 0.000000 +motor.offset.9 0.000000 +motor.offset.10 0.000000 +motor.offset.11 0.000000 +motor.offset.12 0.000000 +motor.offset.13 0.000000 +motor.offset.14 0.000000 +motor.offset.15 0.000000 +motor.offset.16 0.000000 +motor.offset.17 0.000000 +motor.offset.18 0.000000 +motor.offset.19 0.000000 +motor.offset.20 0.000000 +motor.offset.21 0.000000 +motor.offset.22 0.000000 +motor.offset.23 0.000000 +motor.offset.24 0.000000 +motor.offset.25 0.000000 +motor.offset.26 0.000000 +motor.offset.27 0.000000 +motor.offset.28 0.000000 +motor.offset.29 0.000000 +motor.offset.30 0.000000 +motor.offset.31 0.000000 +motor.offset.32 0.000000 +motor.offset.33 0.000000 +motor.offset.34 0.000000 +motor.offset.35 0.000000 +motor.offset.36 0.000000 +motor.offset.37 0.000000 +motor.offset.38 0.000000 +motor.offset.39 0.000000 +motor.offset.40 0.000000 +motor.offset.41 0.000000 +motor.offset.42 0.000000 +motor.offset.43 0.000000 +motor.offset.44 0.000000 +motor.offset.45 0.000000 +motor.offset.46 0.000000 +motor.offset.47 0.000000 +motor.offset.48 0.000000 +motor.offset.49 0.000000 +motor.offset.50 0.000000 +motor.offset.51 0.000000 +motor.offset.52 0.000000 +motor.offset.53 0.000000 +motor.offset.54 0.000000 +motor.offset.55 0.000000 +motor.offset.56 0.000000 +motor.offset.57 0.000000 +motor.offset.58 0.000000 +motor.offset.59 0.000000 +motor.offset.60 0.000000 +motor.offset.61 0.000000 +motor.offset.62 0.000000 +motor.offset.63 0.000000 +motor.rotation_current_cutoff_A 10000.000000 +motor.rotation_current_scale 0.050000 +motor.rotation_torque_scale 14.700000 +motor.cogging_dq_scale 0.000000 +motor.cogging_dq_comp.0 0 +motor.cogging_dq_comp.1 0 +motor.cogging_dq_comp.2 0 +motor.cogging_dq_comp.3 0 +motor.cogging_dq_comp.4 0 +motor.cogging_dq_comp.5 0 +motor.cogging_dq_comp.6 0 +motor.cogging_dq_comp.7 0 +motor.cogging_dq_comp.8 0 +motor.cogging_dq_comp.9 0 +motor.cogging_dq_comp.10 0 +motor.cogging_dq_comp.11 0 +motor.cogging_dq_comp.12 0 +motor.cogging_dq_comp.13 0 +motor.cogging_dq_comp.14 0 +motor.cogging_dq_comp.15 0 +motor.cogging_dq_comp.16 0 +motor.cogging_dq_comp.17 0 +motor.cogging_dq_comp.18 0 +motor.cogging_dq_comp.19 0 +motor.cogging_dq_comp.20 0 +motor.cogging_dq_comp.21 0 +motor.cogging_dq_comp.22 0 +motor.cogging_dq_comp.23 0 +motor.cogging_dq_comp.24 0 +motor.cogging_dq_comp.25 0 +motor.cogging_dq_comp.26 0 +motor.cogging_dq_comp.27 0 +motor.cogging_dq_comp.28 0 +motor.cogging_dq_comp.29 0 +motor.cogging_dq_comp.30 0 +motor.cogging_dq_comp.31 0 +motor.cogging_dq_comp.32 0 +motor.cogging_dq_comp.33 0 +motor.cogging_dq_comp.34 0 +motor.cogging_dq_comp.35 0 +motor.cogging_dq_comp.36 0 +motor.cogging_dq_comp.37 0 +motor.cogging_dq_comp.38 0 +motor.cogging_dq_comp.39 0 +motor.cogging_dq_comp.40 0 +motor.cogging_dq_comp.41 0 +motor.cogging_dq_comp.42 0 +motor.cogging_dq_comp.43 0 +motor.cogging_dq_comp.44 0 +motor.cogging_dq_comp.45 0 +motor.cogging_dq_comp.46 0 +motor.cogging_dq_comp.47 0 +motor.cogging_dq_comp.48 0 +motor.cogging_dq_comp.49 0 +motor.cogging_dq_comp.50 0 +motor.cogging_dq_comp.51 0 +motor.cogging_dq_comp.52 0 +motor.cogging_dq_comp.53 0 +motor.cogging_dq_comp.54 0 +motor.cogging_dq_comp.55 0 +motor.cogging_dq_comp.56 0 +motor.cogging_dq_comp.57 0 +motor.cogging_dq_comp.58 0 +motor.cogging_dq_comp.59 0 +motor.cogging_dq_comp.60 0 +motor.cogging_dq_comp.61 0 +motor.cogging_dq_comp.62 0 +motor.cogging_dq_comp.63 0 +motor.cogging_dq_comp.64 0 +motor.cogging_dq_comp.65 0 +motor.cogging_dq_comp.66 0 +motor.cogging_dq_comp.67 0 +motor.cogging_dq_comp.68 0 +motor.cogging_dq_comp.69 0 +motor.cogging_dq_comp.70 0 +motor.cogging_dq_comp.71 0 +motor.cogging_dq_comp.72 0 +motor.cogging_dq_comp.73 0 +motor.cogging_dq_comp.74 0 +motor.cogging_dq_comp.75 0 +motor.cogging_dq_comp.76 0 +motor.cogging_dq_comp.77 0 +motor.cogging_dq_comp.78 0 +motor.cogging_dq_comp.79 0 +motor.cogging_dq_comp.80 0 +motor.cogging_dq_comp.81 0 +motor.cogging_dq_comp.82 0 +motor.cogging_dq_comp.83 0 +motor.cogging_dq_comp.84 0 +motor.cogging_dq_comp.85 0 +motor.cogging_dq_comp.86 0 +motor.cogging_dq_comp.87 0 +motor.cogging_dq_comp.88 0 +motor.cogging_dq_comp.89 0 +motor.cogging_dq_comp.90 0 +motor.cogging_dq_comp.91 0 +motor.cogging_dq_comp.92 0 +motor.cogging_dq_comp.93 0 +motor.cogging_dq_comp.94 0 +motor.cogging_dq_comp.95 0 +motor.cogging_dq_comp.96 0 +motor.cogging_dq_comp.97 0 +motor.cogging_dq_comp.98 0 +motor.cogging_dq_comp.99 0 +motor.cogging_dq_comp.100 0 +motor.cogging_dq_comp.101 0 +motor.cogging_dq_comp.102 0 +motor.cogging_dq_comp.103 0 +motor.cogging_dq_comp.104 0 +motor.cogging_dq_comp.105 0 +motor.cogging_dq_comp.106 0 +motor.cogging_dq_comp.107 0 +motor.cogging_dq_comp.108 0 +motor.cogging_dq_comp.109 0 +motor.cogging_dq_comp.110 0 +motor.cogging_dq_comp.111 0 +motor.cogging_dq_comp.112 0 +motor.cogging_dq_comp.113 0 +motor.cogging_dq_comp.114 0 +motor.cogging_dq_comp.115 0 +motor.cogging_dq_comp.116 0 +motor.cogging_dq_comp.117 0 +motor.cogging_dq_comp.118 0 +motor.cogging_dq_comp.119 0 +motor.cogging_dq_comp.120 0 +motor.cogging_dq_comp.121 0 +motor.cogging_dq_comp.122 0 +motor.cogging_dq_comp.123 0 +motor.cogging_dq_comp.124 0 +motor.cogging_dq_comp.125 0 +motor.cogging_dq_comp.126 0 +motor.cogging_dq_comp.127 0 +motor.cogging_dq_comp.128 0 +motor.cogging_dq_comp.129 0 +motor.cogging_dq_comp.130 0 +motor.cogging_dq_comp.131 0 +motor.cogging_dq_comp.132 0 +motor.cogging_dq_comp.133 0 +motor.cogging_dq_comp.134 0 +motor.cogging_dq_comp.135 0 +motor.cogging_dq_comp.136 0 +motor.cogging_dq_comp.137 0 +motor.cogging_dq_comp.138 0 +motor.cogging_dq_comp.139 0 +motor.cogging_dq_comp.140 0 +motor.cogging_dq_comp.141 0 +motor.cogging_dq_comp.142 0 +motor.cogging_dq_comp.143 0 +motor.cogging_dq_comp.144 0 +motor.cogging_dq_comp.145 0 +motor.cogging_dq_comp.146 0 +motor.cogging_dq_comp.147 0 +motor.cogging_dq_comp.148 0 +motor.cogging_dq_comp.149 0 +motor.cogging_dq_comp.150 0 +motor.cogging_dq_comp.151 0 +motor.cogging_dq_comp.152 0 +motor.cogging_dq_comp.153 0 +motor.cogging_dq_comp.154 0 +motor.cogging_dq_comp.155 0 +motor.cogging_dq_comp.156 0 +motor.cogging_dq_comp.157 0 +motor.cogging_dq_comp.158 0 +motor.cogging_dq_comp.159 0 +motor.cogging_dq_comp.160 0 +motor.cogging_dq_comp.161 0 +motor.cogging_dq_comp.162 0 +motor.cogging_dq_comp.163 0 +motor.cogging_dq_comp.164 0 +motor.cogging_dq_comp.165 0 +motor.cogging_dq_comp.166 0 +motor.cogging_dq_comp.167 0 +motor.cogging_dq_comp.168 0 +motor.cogging_dq_comp.169 0 +motor.cogging_dq_comp.170 0 +motor.cogging_dq_comp.171 0 +motor.cogging_dq_comp.172 0 +motor.cogging_dq_comp.173 0 +motor.cogging_dq_comp.174 0 +motor.cogging_dq_comp.175 0 +motor.cogging_dq_comp.176 0 +motor.cogging_dq_comp.177 0 +motor.cogging_dq_comp.178 0 +motor.cogging_dq_comp.179 0 +motor.cogging_dq_comp.180 0 +motor.cogging_dq_comp.181 0 +motor.cogging_dq_comp.182 0 +motor.cogging_dq_comp.183 0 +motor.cogging_dq_comp.184 0 +motor.cogging_dq_comp.185 0 +motor.cogging_dq_comp.186 0 +motor.cogging_dq_comp.187 0 +motor.cogging_dq_comp.188 0 +motor.cogging_dq_comp.189 0 +motor.cogging_dq_comp.190 0 +motor.cogging_dq_comp.191 0 +motor.cogging_dq_comp.192 0 +motor.cogging_dq_comp.193 0 +motor.cogging_dq_comp.194 0 +motor.cogging_dq_comp.195 0 +motor.cogging_dq_comp.196 0 +motor.cogging_dq_comp.197 0 +motor.cogging_dq_comp.198 0 +motor.cogging_dq_comp.199 0 +motor.cogging_dq_comp.200 0 +motor.cogging_dq_comp.201 0 +motor.cogging_dq_comp.202 0 +motor.cogging_dq_comp.203 0 +motor.cogging_dq_comp.204 0 +motor.cogging_dq_comp.205 0 +motor.cogging_dq_comp.206 0 +motor.cogging_dq_comp.207 0 +motor.cogging_dq_comp.208 0 +motor.cogging_dq_comp.209 0 +motor.cogging_dq_comp.210 0 +motor.cogging_dq_comp.211 0 +motor.cogging_dq_comp.212 0 +motor.cogging_dq_comp.213 0 +motor.cogging_dq_comp.214 0 +motor.cogging_dq_comp.215 0 +motor.cogging_dq_comp.216 0 +motor.cogging_dq_comp.217 0 +motor.cogging_dq_comp.218 0 +motor.cogging_dq_comp.219 0 +motor.cogging_dq_comp.220 0 +motor.cogging_dq_comp.221 0 +motor.cogging_dq_comp.222 0 +motor.cogging_dq_comp.223 0 +motor.cogging_dq_comp.224 0 +motor.cogging_dq_comp.225 0 +motor.cogging_dq_comp.226 0 +motor.cogging_dq_comp.227 0 +motor.cogging_dq_comp.228 0 +motor.cogging_dq_comp.229 0 +motor.cogging_dq_comp.230 0 +motor.cogging_dq_comp.231 0 +motor.cogging_dq_comp.232 0 +motor.cogging_dq_comp.233 0 +motor.cogging_dq_comp.234 0 +motor.cogging_dq_comp.235 0 +motor.cogging_dq_comp.236 0 +motor.cogging_dq_comp.237 0 +motor.cogging_dq_comp.238 0 +motor.cogging_dq_comp.239 0 +motor.cogging_dq_comp.240 0 +motor.cogging_dq_comp.241 0 +motor.cogging_dq_comp.242 0 +motor.cogging_dq_comp.243 0 +motor.cogging_dq_comp.244 0 +motor.cogging_dq_comp.245 0 +motor.cogging_dq_comp.246 0 +motor.cogging_dq_comp.247 0 +motor.cogging_dq_comp.248 0 +motor.cogging_dq_comp.249 0 +motor.cogging_dq_comp.250 0 +motor.cogging_dq_comp.251 0 +motor.cogging_dq_comp.252 0 +motor.cogging_dq_comp.253 0 +motor.cogging_dq_comp.254 0 +motor.cogging_dq_comp.255 0 +motor.cogging_dq_comp.256 0 +motor.cogging_dq_comp.257 0 +motor.cogging_dq_comp.258 0 +motor.cogging_dq_comp.259 0 +motor.cogging_dq_comp.260 0 +motor.cogging_dq_comp.261 0 +motor.cogging_dq_comp.262 0 +motor.cogging_dq_comp.263 0 +motor.cogging_dq_comp.264 0 +motor.cogging_dq_comp.265 0 +motor.cogging_dq_comp.266 0 +motor.cogging_dq_comp.267 0 +motor.cogging_dq_comp.268 0 +motor.cogging_dq_comp.269 0 +motor.cogging_dq_comp.270 0 +motor.cogging_dq_comp.271 0 +motor.cogging_dq_comp.272 0 +motor.cogging_dq_comp.273 0 +motor.cogging_dq_comp.274 0 +motor.cogging_dq_comp.275 0 +motor.cogging_dq_comp.276 0 +motor.cogging_dq_comp.277 0 +motor.cogging_dq_comp.278 0 +motor.cogging_dq_comp.279 0 +motor.cogging_dq_comp.280 0 +motor.cogging_dq_comp.281 0 +motor.cogging_dq_comp.282 0 +motor.cogging_dq_comp.283 0 +motor.cogging_dq_comp.284 0 +motor.cogging_dq_comp.285 0 +motor.cogging_dq_comp.286 0 +motor.cogging_dq_comp.287 0 +motor.cogging_dq_comp.288 0 +motor.cogging_dq_comp.289 0 +motor.cogging_dq_comp.290 0 +motor.cogging_dq_comp.291 0 +motor.cogging_dq_comp.292 0 +motor.cogging_dq_comp.293 0 +motor.cogging_dq_comp.294 0 +motor.cogging_dq_comp.295 0 +motor.cogging_dq_comp.296 0 +motor.cogging_dq_comp.297 0 +motor.cogging_dq_comp.298 0 +motor.cogging_dq_comp.299 0 +motor.cogging_dq_comp.300 0 +motor.cogging_dq_comp.301 0 +motor.cogging_dq_comp.302 0 +motor.cogging_dq_comp.303 0 +motor.cogging_dq_comp.304 0 +motor.cogging_dq_comp.305 0 +motor.cogging_dq_comp.306 0 +motor.cogging_dq_comp.307 0 +motor.cogging_dq_comp.308 0 +motor.cogging_dq_comp.309 0 +motor.cogging_dq_comp.310 0 +motor.cogging_dq_comp.311 0 +motor.cogging_dq_comp.312 0 +motor.cogging_dq_comp.313 0 +motor.cogging_dq_comp.314 0 +motor.cogging_dq_comp.315 0 +motor.cogging_dq_comp.316 0 +motor.cogging_dq_comp.317 0 +motor.cogging_dq_comp.318 0 +motor.cogging_dq_comp.319 0 +motor.cogging_dq_comp.320 0 +motor.cogging_dq_comp.321 0 +motor.cogging_dq_comp.322 0 +motor.cogging_dq_comp.323 0 +motor.cogging_dq_comp.324 0 +motor.cogging_dq_comp.325 0 +motor.cogging_dq_comp.326 0 +motor.cogging_dq_comp.327 0 +motor.cogging_dq_comp.328 0 +motor.cogging_dq_comp.329 0 +motor.cogging_dq_comp.330 0 +motor.cogging_dq_comp.331 0 +motor.cogging_dq_comp.332 0 +motor.cogging_dq_comp.333 0 +motor.cogging_dq_comp.334 0 +motor.cogging_dq_comp.335 0 +motor.cogging_dq_comp.336 0 +motor.cogging_dq_comp.337 0 +motor.cogging_dq_comp.338 0 +motor.cogging_dq_comp.339 0 +motor.cogging_dq_comp.340 0 +motor.cogging_dq_comp.341 0 +motor.cogging_dq_comp.342 0 +motor.cogging_dq_comp.343 0 +motor.cogging_dq_comp.344 0 +motor.cogging_dq_comp.345 0 +motor.cogging_dq_comp.346 0 +motor.cogging_dq_comp.347 0 +motor.cogging_dq_comp.348 0 +motor.cogging_dq_comp.349 0 +motor.cogging_dq_comp.350 0 +motor.cogging_dq_comp.351 0 +motor.cogging_dq_comp.352 0 +motor.cogging_dq_comp.353 0 +motor.cogging_dq_comp.354 0 +motor.cogging_dq_comp.355 0 +motor.cogging_dq_comp.356 0 +motor.cogging_dq_comp.357 0 +motor.cogging_dq_comp.358 0 +motor.cogging_dq_comp.359 0 +motor.cogging_dq_comp.360 0 +motor.cogging_dq_comp.361 0 +motor.cogging_dq_comp.362 0 +motor.cogging_dq_comp.363 0 +motor.cogging_dq_comp.364 0 +motor.cogging_dq_comp.365 0 +motor.cogging_dq_comp.366 0 +motor.cogging_dq_comp.367 0 +motor.cogging_dq_comp.368 0 +motor.cogging_dq_comp.369 0 +motor.cogging_dq_comp.370 0 +motor.cogging_dq_comp.371 0 +motor.cogging_dq_comp.372 0 +motor.cogging_dq_comp.373 0 +motor.cogging_dq_comp.374 0 +motor.cogging_dq_comp.375 0 +motor.cogging_dq_comp.376 0 +motor.cogging_dq_comp.377 0 +motor.cogging_dq_comp.378 0 +motor.cogging_dq_comp.379 0 +motor.cogging_dq_comp.380 0 +motor.cogging_dq_comp.381 0 +motor.cogging_dq_comp.382 0 +motor.cogging_dq_comp.383 0 +motor.cogging_dq_comp.384 0 +motor.cogging_dq_comp.385 0 +motor.cogging_dq_comp.386 0 +motor.cogging_dq_comp.387 0 +motor.cogging_dq_comp.388 0 +motor.cogging_dq_comp.389 0 +motor.cogging_dq_comp.390 0 +motor.cogging_dq_comp.391 0 +motor.cogging_dq_comp.392 0 +motor.cogging_dq_comp.393 0 +motor.cogging_dq_comp.394 0 +motor.cogging_dq_comp.395 0 +motor.cogging_dq_comp.396 0 +motor.cogging_dq_comp.397 0 +motor.cogging_dq_comp.398 0 +motor.cogging_dq_comp.399 0 +motor.cogging_dq_comp.400 0 +motor.cogging_dq_comp.401 0 +motor.cogging_dq_comp.402 0 +motor.cogging_dq_comp.403 0 +motor.cogging_dq_comp.404 0 +motor.cogging_dq_comp.405 0 +motor.cogging_dq_comp.406 0 +motor.cogging_dq_comp.407 0 +motor.cogging_dq_comp.408 0 +motor.cogging_dq_comp.409 0 +motor.cogging_dq_comp.410 0 +motor.cogging_dq_comp.411 0 +motor.cogging_dq_comp.412 0 +motor.cogging_dq_comp.413 0 +motor.cogging_dq_comp.414 0 +motor.cogging_dq_comp.415 0 +motor.cogging_dq_comp.416 0 +motor.cogging_dq_comp.417 0 +motor.cogging_dq_comp.418 0 +motor.cogging_dq_comp.419 0 +motor.cogging_dq_comp.420 0 +motor.cogging_dq_comp.421 0 +motor.cogging_dq_comp.422 0 +motor.cogging_dq_comp.423 0 +motor.cogging_dq_comp.424 0 +motor.cogging_dq_comp.425 0 +motor.cogging_dq_comp.426 0 +motor.cogging_dq_comp.427 0 +motor.cogging_dq_comp.428 0 +motor.cogging_dq_comp.429 0 +motor.cogging_dq_comp.430 0 +motor.cogging_dq_comp.431 0 +motor.cogging_dq_comp.432 0 +motor.cogging_dq_comp.433 0 +motor.cogging_dq_comp.434 0 +motor.cogging_dq_comp.435 0 +motor.cogging_dq_comp.436 0 +motor.cogging_dq_comp.437 0 +motor.cogging_dq_comp.438 0 +motor.cogging_dq_comp.439 0 +motor.cogging_dq_comp.440 0 +motor.cogging_dq_comp.441 0 +motor.cogging_dq_comp.442 0 +motor.cogging_dq_comp.443 0 +motor.cogging_dq_comp.444 0 +motor.cogging_dq_comp.445 0 +motor.cogging_dq_comp.446 0 +motor.cogging_dq_comp.447 0 +motor.cogging_dq_comp.448 0 +motor.cogging_dq_comp.449 0 +motor.cogging_dq_comp.450 0 +motor.cogging_dq_comp.451 0 +motor.cogging_dq_comp.452 0 +motor.cogging_dq_comp.453 0 +motor.cogging_dq_comp.454 0 +motor.cogging_dq_comp.455 0 +motor.cogging_dq_comp.456 0 +motor.cogging_dq_comp.457 0 +motor.cogging_dq_comp.458 0 +motor.cogging_dq_comp.459 0 +motor.cogging_dq_comp.460 0 +motor.cogging_dq_comp.461 0 +motor.cogging_dq_comp.462 0 +motor.cogging_dq_comp.463 0 +motor.cogging_dq_comp.464 0 +motor.cogging_dq_comp.465 0 +motor.cogging_dq_comp.466 0 +motor.cogging_dq_comp.467 0 +motor.cogging_dq_comp.468 0 +motor.cogging_dq_comp.469 0 +motor.cogging_dq_comp.470 0 +motor.cogging_dq_comp.471 0 +motor.cogging_dq_comp.472 0 +motor.cogging_dq_comp.473 0 +motor.cogging_dq_comp.474 0 +motor.cogging_dq_comp.475 0 +motor.cogging_dq_comp.476 0 +motor.cogging_dq_comp.477 0 +motor.cogging_dq_comp.478 0 +motor.cogging_dq_comp.479 0 +motor.cogging_dq_comp.480 0 +motor.cogging_dq_comp.481 0 +motor.cogging_dq_comp.482 0 +motor.cogging_dq_comp.483 0 +motor.cogging_dq_comp.484 0 +motor.cogging_dq_comp.485 0 +motor.cogging_dq_comp.486 0 +motor.cogging_dq_comp.487 0 +motor.cogging_dq_comp.488 0 +motor.cogging_dq_comp.489 0 +motor.cogging_dq_comp.490 0 +motor.cogging_dq_comp.491 0 +motor.cogging_dq_comp.492 0 +motor.cogging_dq_comp.493 0 +motor.cogging_dq_comp.494 0 +motor.cogging_dq_comp.495 0 +motor.cogging_dq_comp.496 0 +motor.cogging_dq_comp.497 0 +motor.cogging_dq_comp.498 0 +motor.cogging_dq_comp.499 0 +motor.cogging_dq_comp.500 0 +motor.cogging_dq_comp.501 0 +motor.cogging_dq_comp.502 0 +motor.cogging_dq_comp.503 0 +motor.cogging_dq_comp.504 0 +motor.cogging_dq_comp.505 0 +motor.cogging_dq_comp.506 0 +motor.cogging_dq_comp.507 0 +motor.cogging_dq_comp.508 0 +motor.cogging_dq_comp.509 0 +motor.cogging_dq_comp.510 0 +motor.cogging_dq_comp.511 0 +motor.cogging_dq_comp.512 0 +motor.cogging_dq_comp.513 0 +motor.cogging_dq_comp.514 0 +motor.cogging_dq_comp.515 0 +motor.cogging_dq_comp.516 0 +motor.cogging_dq_comp.517 0 +motor.cogging_dq_comp.518 0 +motor.cogging_dq_comp.519 0 +motor.cogging_dq_comp.520 0 +motor.cogging_dq_comp.521 0 +motor.cogging_dq_comp.522 0 +motor.cogging_dq_comp.523 0 +motor.cogging_dq_comp.524 0 +motor.cogging_dq_comp.525 0 +motor.cogging_dq_comp.526 0 +motor.cogging_dq_comp.527 0 +motor.cogging_dq_comp.528 0 +motor.cogging_dq_comp.529 0 +motor.cogging_dq_comp.530 0 +motor.cogging_dq_comp.531 0 +motor.cogging_dq_comp.532 0 +motor.cogging_dq_comp.533 0 +motor.cogging_dq_comp.534 0 +motor.cogging_dq_comp.535 0 +motor.cogging_dq_comp.536 0 +motor.cogging_dq_comp.537 0 +motor.cogging_dq_comp.538 0 +motor.cogging_dq_comp.539 0 +motor.cogging_dq_comp.540 0 +motor.cogging_dq_comp.541 0 +motor.cogging_dq_comp.542 0 +motor.cogging_dq_comp.543 0 +motor.cogging_dq_comp.544 0 +motor.cogging_dq_comp.545 0 +motor.cogging_dq_comp.546 0 +motor.cogging_dq_comp.547 0 +motor.cogging_dq_comp.548 0 +motor.cogging_dq_comp.549 0 +motor.cogging_dq_comp.550 0 +motor.cogging_dq_comp.551 0 +motor.cogging_dq_comp.552 0 +motor.cogging_dq_comp.553 0 +motor.cogging_dq_comp.554 0 +motor.cogging_dq_comp.555 0 +motor.cogging_dq_comp.556 0 +motor.cogging_dq_comp.557 0 +motor.cogging_dq_comp.558 0 +motor.cogging_dq_comp.559 0 +motor.cogging_dq_comp.560 0 +motor.cogging_dq_comp.561 0 +motor.cogging_dq_comp.562 0 +motor.cogging_dq_comp.563 0 +motor.cogging_dq_comp.564 0 +motor.cogging_dq_comp.565 0 +motor.cogging_dq_comp.566 0 +motor.cogging_dq_comp.567 0 +motor.cogging_dq_comp.568 0 +motor.cogging_dq_comp.569 0 +motor.cogging_dq_comp.570 0 +motor.cogging_dq_comp.571 0 +motor.cogging_dq_comp.572 0 +motor.cogging_dq_comp.573 0 +motor.cogging_dq_comp.574 0 +motor.cogging_dq_comp.575 0 +motor.cogging_dq_comp.576 0 +motor.cogging_dq_comp.577 0 +motor.cogging_dq_comp.578 0 +motor.cogging_dq_comp.579 0 +motor.cogging_dq_comp.580 0 +motor.cogging_dq_comp.581 0 +motor.cogging_dq_comp.582 0 +motor.cogging_dq_comp.583 0 +motor.cogging_dq_comp.584 0 +motor.cogging_dq_comp.585 0 +motor.cogging_dq_comp.586 0 +motor.cogging_dq_comp.587 0 +motor.cogging_dq_comp.588 0 +motor.cogging_dq_comp.589 0 +motor.cogging_dq_comp.590 0 +motor.cogging_dq_comp.591 0 +motor.cogging_dq_comp.592 0 +motor.cogging_dq_comp.593 0 +motor.cogging_dq_comp.594 0 +motor.cogging_dq_comp.595 0 +motor.cogging_dq_comp.596 0 +motor.cogging_dq_comp.597 0 +motor.cogging_dq_comp.598 0 +motor.cogging_dq_comp.599 0 +motor.cogging_dq_comp.600 0 +motor.cogging_dq_comp.601 0 +motor.cogging_dq_comp.602 0 +motor.cogging_dq_comp.603 0 +motor.cogging_dq_comp.604 0 +motor.cogging_dq_comp.605 0 +motor.cogging_dq_comp.606 0 +motor.cogging_dq_comp.607 0 +motor.cogging_dq_comp.608 0 +motor.cogging_dq_comp.609 0 +motor.cogging_dq_comp.610 0 +motor.cogging_dq_comp.611 0 +motor.cogging_dq_comp.612 0 +motor.cogging_dq_comp.613 0 +motor.cogging_dq_comp.614 0 +motor.cogging_dq_comp.615 0 +motor.cogging_dq_comp.616 0 +motor.cogging_dq_comp.617 0 +motor.cogging_dq_comp.618 0 +motor.cogging_dq_comp.619 0 +motor.cogging_dq_comp.620 0 +motor.cogging_dq_comp.621 0 +motor.cogging_dq_comp.622 0 +motor.cogging_dq_comp.623 0 +motor.cogging_dq_comp.624 0 +motor.cogging_dq_comp.625 0 +motor.cogging_dq_comp.626 0 +motor.cogging_dq_comp.627 0 +motor.cogging_dq_comp.628 0 +motor.cogging_dq_comp.629 0 +motor.cogging_dq_comp.630 0 +motor.cogging_dq_comp.631 0 +motor.cogging_dq_comp.632 0 +motor.cogging_dq_comp.633 0 +motor.cogging_dq_comp.634 0 +motor.cogging_dq_comp.635 0 +motor.cogging_dq_comp.636 0 +motor.cogging_dq_comp.637 0 +motor.cogging_dq_comp.638 0 +motor.cogging_dq_comp.639 0 +motor.cogging_dq_comp.640 0 +motor.cogging_dq_comp.641 0 +motor.cogging_dq_comp.642 0 +motor.cogging_dq_comp.643 0 +motor.cogging_dq_comp.644 0 +motor.cogging_dq_comp.645 0 +motor.cogging_dq_comp.646 0 +motor.cogging_dq_comp.647 0 +motor.cogging_dq_comp.648 0 +motor.cogging_dq_comp.649 0 +motor.cogging_dq_comp.650 0 +motor.cogging_dq_comp.651 0 +motor.cogging_dq_comp.652 0 +motor.cogging_dq_comp.653 0 +motor.cogging_dq_comp.654 0 +motor.cogging_dq_comp.655 0 +motor.cogging_dq_comp.656 0 +motor.cogging_dq_comp.657 0 +motor.cogging_dq_comp.658 0 +motor.cogging_dq_comp.659 0 +motor.cogging_dq_comp.660 0 +motor.cogging_dq_comp.661 0 +motor.cogging_dq_comp.662 0 +motor.cogging_dq_comp.663 0 +motor.cogging_dq_comp.664 0 +motor.cogging_dq_comp.665 0 +motor.cogging_dq_comp.666 0 +motor.cogging_dq_comp.667 0 +motor.cogging_dq_comp.668 0 +motor.cogging_dq_comp.669 0 +motor.cogging_dq_comp.670 0 +motor.cogging_dq_comp.671 0 +motor.cogging_dq_comp.672 0 +motor.cogging_dq_comp.673 0 +motor.cogging_dq_comp.674 0 +motor.cogging_dq_comp.675 0 +motor.cogging_dq_comp.676 0 +motor.cogging_dq_comp.677 0 +motor.cogging_dq_comp.678 0 +motor.cogging_dq_comp.679 0 +motor.cogging_dq_comp.680 0 +motor.cogging_dq_comp.681 0 +motor.cogging_dq_comp.682 0 +motor.cogging_dq_comp.683 0 +motor.cogging_dq_comp.684 0 +motor.cogging_dq_comp.685 0 +motor.cogging_dq_comp.686 0 +motor.cogging_dq_comp.687 0 +motor.cogging_dq_comp.688 0 +motor.cogging_dq_comp.689 0 +motor.cogging_dq_comp.690 0 +motor.cogging_dq_comp.691 0 +motor.cogging_dq_comp.692 0 +motor.cogging_dq_comp.693 0 +motor.cogging_dq_comp.694 0 +motor.cogging_dq_comp.695 0 +motor.cogging_dq_comp.696 0 +motor.cogging_dq_comp.697 0 +motor.cogging_dq_comp.698 0 +motor.cogging_dq_comp.699 0 +motor.cogging_dq_comp.700 0 +motor.cogging_dq_comp.701 0 +motor.cogging_dq_comp.702 0 +motor.cogging_dq_comp.703 0 +motor.cogging_dq_comp.704 0 +motor.cogging_dq_comp.705 0 +motor.cogging_dq_comp.706 0 +motor.cogging_dq_comp.707 0 +motor.cogging_dq_comp.708 0 +motor.cogging_dq_comp.709 0 +motor.cogging_dq_comp.710 0 +motor.cogging_dq_comp.711 0 +motor.cogging_dq_comp.712 0 +motor.cogging_dq_comp.713 0 +motor.cogging_dq_comp.714 0 +motor.cogging_dq_comp.715 0 +motor.cogging_dq_comp.716 0 +motor.cogging_dq_comp.717 0 +motor.cogging_dq_comp.718 0 +motor.cogging_dq_comp.719 0 +motor.cogging_dq_comp.720 0 +motor.cogging_dq_comp.721 0 +motor.cogging_dq_comp.722 0 +motor.cogging_dq_comp.723 0 +motor.cogging_dq_comp.724 0 +motor.cogging_dq_comp.725 0 +motor.cogging_dq_comp.726 0 +motor.cogging_dq_comp.727 0 +motor.cogging_dq_comp.728 0 +motor.cogging_dq_comp.729 0 +motor.cogging_dq_comp.730 0 +motor.cogging_dq_comp.731 0 +motor.cogging_dq_comp.732 0 +motor.cogging_dq_comp.733 0 +motor.cogging_dq_comp.734 0 +motor.cogging_dq_comp.735 0 +motor.cogging_dq_comp.736 0 +motor.cogging_dq_comp.737 0 +motor.cogging_dq_comp.738 0 +motor.cogging_dq_comp.739 0 +motor.cogging_dq_comp.740 0 +motor.cogging_dq_comp.741 0 +motor.cogging_dq_comp.742 0 +motor.cogging_dq_comp.743 0 +motor.cogging_dq_comp.744 0 +motor.cogging_dq_comp.745 0 +motor.cogging_dq_comp.746 0 +motor.cogging_dq_comp.747 0 +motor.cogging_dq_comp.748 0 +motor.cogging_dq_comp.749 0 +motor.cogging_dq_comp.750 0 +motor.cogging_dq_comp.751 0 +motor.cogging_dq_comp.752 0 +motor.cogging_dq_comp.753 0 +motor.cogging_dq_comp.754 0 +motor.cogging_dq_comp.755 0 +motor.cogging_dq_comp.756 0 +motor.cogging_dq_comp.757 0 +motor.cogging_dq_comp.758 0 +motor.cogging_dq_comp.759 0 +motor.cogging_dq_comp.760 0 +motor.cogging_dq_comp.761 0 +motor.cogging_dq_comp.762 0 +motor.cogging_dq_comp.763 0 +motor.cogging_dq_comp.764 0 +motor.cogging_dq_comp.765 0 +motor.cogging_dq_comp.766 0 +motor.cogging_dq_comp.767 0 +motor.cogging_dq_comp.768 0 +motor.cogging_dq_comp.769 0 +motor.cogging_dq_comp.770 0 +motor.cogging_dq_comp.771 0 +motor.cogging_dq_comp.772 0 +motor.cogging_dq_comp.773 0 +motor.cogging_dq_comp.774 0 +motor.cogging_dq_comp.775 0 +motor.cogging_dq_comp.776 0 +motor.cogging_dq_comp.777 0 +motor.cogging_dq_comp.778 0 +motor.cogging_dq_comp.779 0 +motor.cogging_dq_comp.780 0 +motor.cogging_dq_comp.781 0 +motor.cogging_dq_comp.782 0 +motor.cogging_dq_comp.783 0 +motor.cogging_dq_comp.784 0 +motor.cogging_dq_comp.785 0 +motor.cogging_dq_comp.786 0 +motor.cogging_dq_comp.787 0 +motor.cogging_dq_comp.788 0 +motor.cogging_dq_comp.789 0 +motor.cogging_dq_comp.790 0 +motor.cogging_dq_comp.791 0 +motor.cogging_dq_comp.792 0 +motor.cogging_dq_comp.793 0 +motor.cogging_dq_comp.794 0 +motor.cogging_dq_comp.795 0 +motor.cogging_dq_comp.796 0 +motor.cogging_dq_comp.797 0 +motor.cogging_dq_comp.798 0 +motor.cogging_dq_comp.799 0 +motor.cogging_dq_comp.800 0 +motor.cogging_dq_comp.801 0 +motor.cogging_dq_comp.802 0 +motor.cogging_dq_comp.803 0 +motor.cogging_dq_comp.804 0 +motor.cogging_dq_comp.805 0 +motor.cogging_dq_comp.806 0 +motor.cogging_dq_comp.807 0 +motor.cogging_dq_comp.808 0 +motor.cogging_dq_comp.809 0 +motor.cogging_dq_comp.810 0 +motor.cogging_dq_comp.811 0 +motor.cogging_dq_comp.812 0 +motor.cogging_dq_comp.813 0 +motor.cogging_dq_comp.814 0 +motor.cogging_dq_comp.815 0 +motor.cogging_dq_comp.816 0 +motor.cogging_dq_comp.817 0 +motor.cogging_dq_comp.818 0 +motor.cogging_dq_comp.819 0 +motor.cogging_dq_comp.820 0 +motor.cogging_dq_comp.821 0 +motor.cogging_dq_comp.822 0 +motor.cogging_dq_comp.823 0 +motor.cogging_dq_comp.824 0 +motor.cogging_dq_comp.825 0 +motor.cogging_dq_comp.826 0 +motor.cogging_dq_comp.827 0 +motor.cogging_dq_comp.828 0 +motor.cogging_dq_comp.829 0 +motor.cogging_dq_comp.830 0 +motor.cogging_dq_comp.831 0 +motor.cogging_dq_comp.832 0 +motor.cogging_dq_comp.833 0 +motor.cogging_dq_comp.834 0 +motor.cogging_dq_comp.835 0 +motor.cogging_dq_comp.836 0 +motor.cogging_dq_comp.837 0 +motor.cogging_dq_comp.838 0 +motor.cogging_dq_comp.839 0 +motor.cogging_dq_comp.840 0 +motor.cogging_dq_comp.841 0 +motor.cogging_dq_comp.842 0 +motor.cogging_dq_comp.843 0 +motor.cogging_dq_comp.844 0 +motor.cogging_dq_comp.845 0 +motor.cogging_dq_comp.846 0 +motor.cogging_dq_comp.847 0 +motor.cogging_dq_comp.848 0 +motor.cogging_dq_comp.849 0 +motor.cogging_dq_comp.850 0 +motor.cogging_dq_comp.851 0 +motor.cogging_dq_comp.852 0 +motor.cogging_dq_comp.853 0 +motor.cogging_dq_comp.854 0 +motor.cogging_dq_comp.855 0 +motor.cogging_dq_comp.856 0 +motor.cogging_dq_comp.857 0 +motor.cogging_dq_comp.858 0 +motor.cogging_dq_comp.859 0 +motor.cogging_dq_comp.860 0 +motor.cogging_dq_comp.861 0 +motor.cogging_dq_comp.862 0 +motor.cogging_dq_comp.863 0 +motor.cogging_dq_comp.864 0 +motor.cogging_dq_comp.865 0 +motor.cogging_dq_comp.866 0 +motor.cogging_dq_comp.867 0 +motor.cogging_dq_comp.868 0 +motor.cogging_dq_comp.869 0 +motor.cogging_dq_comp.870 0 +motor.cogging_dq_comp.871 0 +motor.cogging_dq_comp.872 0 +motor.cogging_dq_comp.873 0 +motor.cogging_dq_comp.874 0 +motor.cogging_dq_comp.875 0 +motor.cogging_dq_comp.876 0 +motor.cogging_dq_comp.877 0 +motor.cogging_dq_comp.878 0 +motor.cogging_dq_comp.879 0 +motor.cogging_dq_comp.880 0 +motor.cogging_dq_comp.881 0 +motor.cogging_dq_comp.882 0 +motor.cogging_dq_comp.883 0 +motor.cogging_dq_comp.884 0 +motor.cogging_dq_comp.885 0 +motor.cogging_dq_comp.886 0 +motor.cogging_dq_comp.887 0 +motor.cogging_dq_comp.888 0 +motor.cogging_dq_comp.889 0 +motor.cogging_dq_comp.890 0 +motor.cogging_dq_comp.891 0 +motor.cogging_dq_comp.892 0 +motor.cogging_dq_comp.893 0 +motor.cogging_dq_comp.894 0 +motor.cogging_dq_comp.895 0 +motor.cogging_dq_comp.896 0 +motor.cogging_dq_comp.897 0 +motor.cogging_dq_comp.898 0 +motor.cogging_dq_comp.899 0 +motor.cogging_dq_comp.900 0 +motor.cogging_dq_comp.901 0 +motor.cogging_dq_comp.902 0 +motor.cogging_dq_comp.903 0 +motor.cogging_dq_comp.904 0 +motor.cogging_dq_comp.905 0 +motor.cogging_dq_comp.906 0 +motor.cogging_dq_comp.907 0 +motor.cogging_dq_comp.908 0 +motor.cogging_dq_comp.909 0 +motor.cogging_dq_comp.910 0 +motor.cogging_dq_comp.911 0 +motor.cogging_dq_comp.912 0 +motor.cogging_dq_comp.913 0 +motor.cogging_dq_comp.914 0 +motor.cogging_dq_comp.915 0 +motor.cogging_dq_comp.916 0 +motor.cogging_dq_comp.917 0 +motor.cogging_dq_comp.918 0 +motor.cogging_dq_comp.919 0 +motor.cogging_dq_comp.920 0 +motor.cogging_dq_comp.921 0 +motor.cogging_dq_comp.922 0 +motor.cogging_dq_comp.923 0 +motor.cogging_dq_comp.924 0 +motor.cogging_dq_comp.925 0 +motor.cogging_dq_comp.926 0 +motor.cogging_dq_comp.927 0 +motor.cogging_dq_comp.928 0 +motor.cogging_dq_comp.929 0 +motor.cogging_dq_comp.930 0 +motor.cogging_dq_comp.931 0 +motor.cogging_dq_comp.932 0 +motor.cogging_dq_comp.933 0 +motor.cogging_dq_comp.934 0 +motor.cogging_dq_comp.935 0 +motor.cogging_dq_comp.936 0 +motor.cogging_dq_comp.937 0 +motor.cogging_dq_comp.938 0 +motor.cogging_dq_comp.939 0 +motor.cogging_dq_comp.940 0 +motor.cogging_dq_comp.941 0 +motor.cogging_dq_comp.942 0 +motor.cogging_dq_comp.943 0 +motor.cogging_dq_comp.944 0 +motor.cogging_dq_comp.945 0 +motor.cogging_dq_comp.946 0 +motor.cogging_dq_comp.947 0 +motor.cogging_dq_comp.948 0 +motor.cogging_dq_comp.949 0 +motor.cogging_dq_comp.950 0 +motor.cogging_dq_comp.951 0 +motor.cogging_dq_comp.952 0 +motor.cogging_dq_comp.953 0 +motor.cogging_dq_comp.954 0 +motor.cogging_dq_comp.955 0 +motor.cogging_dq_comp.956 0 +motor.cogging_dq_comp.957 0 +motor.cogging_dq_comp.958 0 +motor.cogging_dq_comp.959 0 +motor.cogging_dq_comp.960 0 +motor.cogging_dq_comp.961 0 +motor.cogging_dq_comp.962 0 +motor.cogging_dq_comp.963 0 +motor.cogging_dq_comp.964 0 +motor.cogging_dq_comp.965 0 +motor.cogging_dq_comp.966 0 +motor.cogging_dq_comp.967 0 +motor.cogging_dq_comp.968 0 +motor.cogging_dq_comp.969 0 +motor.cogging_dq_comp.970 0 +motor.cogging_dq_comp.971 0 +motor.cogging_dq_comp.972 0 +motor.cogging_dq_comp.973 0 +motor.cogging_dq_comp.974 0 +motor.cogging_dq_comp.975 0 +motor.cogging_dq_comp.976 0 +motor.cogging_dq_comp.977 0 +motor.cogging_dq_comp.978 0 +motor.cogging_dq_comp.979 0 +motor.cogging_dq_comp.980 0 +motor.cogging_dq_comp.981 0 +motor.cogging_dq_comp.982 0 +motor.cogging_dq_comp.983 0 +motor.cogging_dq_comp.984 0 +motor.cogging_dq_comp.985 0 +motor.cogging_dq_comp.986 0 +motor.cogging_dq_comp.987 0 +motor.cogging_dq_comp.988 0 +motor.cogging_dq_comp.989 0 +motor.cogging_dq_comp.990 0 +motor.cogging_dq_comp.991 0 +motor.cogging_dq_comp.992 0 +motor.cogging_dq_comp.993 0 +motor.cogging_dq_comp.994 0 +motor.cogging_dq_comp.995 0 +motor.cogging_dq_comp.996 0 +motor.cogging_dq_comp.997 0 +motor.cogging_dq_comp.998 0 +motor.cogging_dq_comp.999 0 +motor.cogging_dq_comp.1000 0 +motor.cogging_dq_comp.1001 0 +motor.cogging_dq_comp.1002 0 +motor.cogging_dq_comp.1003 0 +motor.cogging_dq_comp.1004 0 +motor.cogging_dq_comp.1005 0 +motor.cogging_dq_comp.1006 0 +motor.cogging_dq_comp.1007 0 +motor.cogging_dq_comp.1008 0 +motor.cogging_dq_comp.1009 0 +motor.cogging_dq_comp.1010 0 +motor.cogging_dq_comp.1011 0 +motor.cogging_dq_comp.1012 0 +motor.cogging_dq_comp.1013 0 +motor.cogging_dq_comp.1014 0 +motor.cogging_dq_comp.1015 0 +motor.cogging_dq_comp.1016 0 +motor.cogging_dq_comp.1017 0 +motor.cogging_dq_comp.1018 0 +motor.cogging_dq_comp.1019 0 +motor.cogging_dq_comp.1020 0 +motor.cogging_dq_comp.1021 0 +motor.cogging_dq_comp.1022 0 +motor.cogging_dq_comp.1023 0 +drv8323_conf.dis_cpuv 0 +drv8323_conf.dis_gdf 0 +drv8323_conf.otw_rep 0 +drv8323_conf.pwm_mode 1 +drv8323_conf.pwm_1x_asynchronous 0 +drv8323_conf.pwm_1x_dir 0 +drv8323_conf.idrivep_hs_ma 100 +drv8323_conf.idriven_hs_ma 200 +drv8323_conf.cbc 1 +drv8323_conf.tdrive_ns 1000 +drv8323_conf.idrivep_ls_ma 100 +drv8323_conf.idriven_ls_ma 200 +drv8323_conf.tretry 0 +drv8323_conf.dead_time_ns 50 +drv8323_conf.ocp_mode 0 +drv8323_conf.ocp_deg_us 4 +drv8323_conf.vds_lvl_mv 700 +drv8323_conf.csa_fet 0 +drv8323_conf.vref_div 1 +drv8323_conf.ls_ref 0 +drv8323_conf.csa_gain 20 +drv8323_conf.dis_sen 0 +drv8323_conf.sen_lvl_mv 500 +servo.pwm_rate_hz 30000 +servo.i_gain 20.000000 +servo.current_sense_ohm 0.000500 +servo.pwm_comp_off 0.027000 +servo.pwm_comp_mag 0.005000 +servo.pwm_scale 1.000000 +servo.max_voltage 46.000000 +servo.max_power_W 450.000000 +servo.derate_temperature 50.000000 +servo.fault_temperature 75.000000 +servo.enable_motor_temperature 0 +servo.motor_derate_temperature 50.000000 +servo.motor_fault_temperature nan +servo.velocity_threshold 0.000000 +servo.position_derate 0.020000 +servo.adc_cur_cycles 2 +servo.adc_aux_cycles 47 +servo.pid_dq.kp 0.176827 +servo.pid_dq.ki 151.117355 +servo.pid_position.kp 0.500000 +servo.pid_position.ki 0.000000 +servo.pid_position.kd 0.050000 +servo.pid_position.iratelimit -1.000000 +servo.pid_position.ilimit 0.000000 +servo.pid_position.max_desired_rate 0.000000 +servo.pid_position.sign -1 +servo.current_feedforward 1.000000 +servo.bemf_feedforward 1.000000 +servo.default_velocity_limit nan +servo.default_accel_limit nan +servo.voltage_mode_control 0 +servo.fixed_voltage_mode 0 +servo.fixed_voltage_control_V 0.000000 +servo.max_position_slip 10.000000 +servo.default_timeout_s 0.100000 +servo.timeout_max_torque_Nm 5.000000 +servo.timeout_mode 12 +servo.flux_brake_min_voltage 43.500000 +servo.flux_brake_resistance_ohm 0.025000 +servo.max_current_A 4.000000 +servo.derate_current_A -20.000000 +servo.max_velocity 500.000000 +servo.max_velocity_derate 2.000000 +servo.cooldown_cycles 256 +servo.velocity_zero_capture_threshold 0.050000 +servo.emit_debug 0 +servopos.position_min nan +servopos.position_max nan +id.id 17 +can.prefix 0 + diff --git a/config/moteus/joint_a.cfg b/config/moteus/joint_a.cfg new file mode 100644 index 000000000..aaab74389 --- /dev/null +++ b/config/moteus/joint_a.cfg @@ -0,0 +1,1399 @@ +uuid.uuid.0 55 +uuid.uuid.1 182 +uuid.uuid.2 228 +uuid.uuid.3 110 +uuid.uuid.4 120 +uuid.uuid.5 156 +uuid.uuid.6 75 +uuid.uuid.7 137 +uuid.uuid.8 129 +uuid.uuid.9 230 +uuid.uuid.10 178 +uuid.uuid.11 201 +uuid.uuid.12 154 +uuid.uuid.13 163 +uuid.uuid.14 21 +uuid.uuid.15 188 +clock.hsitrim 64 +aux1.i2c.i2c_hz 400000 +aux1.i2c.i2c_mode 1 +aux1.i2c.pullup 0 +aux1.i2c.devices.0.type 0 +aux1.i2c.devices.0.address 64 +aux1.i2c.devices.0.poll_rate_us 1000 +aux1.i2c.devices.1.type 0 +aux1.i2c.devices.1.address 64 +aux1.i2c.devices.1.poll_rate_us 1000 +aux1.i2c.devices.2.type 0 +aux1.i2c.devices.2.address 64 +aux1.i2c.devices.2.poll_rate_us 1000 +aux1.spi.mode 1 +aux1.spi.rate_hz 12000000 +aux1.spi.filter_us 64 +aux1.spi.bct 0 +aux1.uart.mode 0 +aux1.uart.baud_rate 115200 +aux1.uart.poll_rate_us 100 +aux1.uart.rs422 0 +aux1.uart.cui_amt21_address 84 +aux1.quadrature.enabled 0 +aux1.quadrature.cpr 16384 +aux1.hall.enabled 1 +aux1.hall.polarity 0 +aux1.index.enabled 0 +aux1.sine_cosine.enabled 0 +aux1.sine_cosine.common 1700 +aux1.i2c_startup_delay_ms 30 +aux1.pins.0.mode 0 +aux1.pins.0.pull 0 +aux1.pins.1.mode 6 +aux1.pins.1.pull 1 +aux1.pins.2.mode 6 +aux1.pins.2.pull 1 +aux1.pins.3.mode 6 +aux1.pins.3.pull 1 +aux1.pins.4.mode 0 +aux1.pins.4.pull 0 +aux2.i2c.i2c_hz 400000 +aux2.i2c.i2c_mode 1 +aux2.i2c.pullup 0 +aux2.i2c.devices.0.type 0 +aux2.i2c.devices.0.address 64 +aux2.i2c.devices.0.poll_rate_us 1000 +aux2.i2c.devices.1.type 0 +aux2.i2c.devices.1.address 64 +aux2.i2c.devices.1.poll_rate_us 1000 +aux2.i2c.devices.2.type 0 +aux2.i2c.devices.2.address 64 +aux2.i2c.devices.2.poll_rate_us 1000 +aux2.spi.mode 1 +aux2.spi.rate_hz 12000000 +aux2.spi.filter_us 64 +aux2.spi.bct 0 +aux2.uart.mode 0 +aux2.uart.baud_rate 115200 +aux2.uart.poll_rate_us 100 +aux2.uart.rs422 0 +aux2.uart.cui_amt21_address 84 +aux2.quadrature.enabled 0 +aux2.quadrature.cpr 16384 +aux2.hall.enabled 0 +aux2.hall.polarity 0 +aux2.index.enabled 0 +aux2.sine_cosine.enabled 0 +aux2.sine_cosine.common 1700 +aux2.i2c_startup_delay_ms 30 +aux2.pins.0.mode 14 +aux2.pins.0.pull 1 +aux2.pins.1.mode 14 +aux2.pins.1.pull 1 +aux2.pins.2.mode 0 +aux2.pins.2.pull 0 +aux2.pins.3.mode 0 +aux2.pins.3.pull 0 +aux2.pins.4.mode 0 +aux2.pins.4.pull 0 +motor_position.sources.0.aux_number 1 +motor_position.sources.0.type 4 +motor_position.sources.0.i2c_device 0 +motor_position.sources.0.incremental_index -1 +motor_position.sources.0.cpr 42 +motor_position.sources.0.offset -5.000000 +motor_position.sources.0.sign -1 +motor_position.sources.0.debug_override -1 +motor_position.sources.0.reference 0 +motor_position.sources.0.pll_filter_hz 100.000000 +motor_position.sources.0.compensation_table.0 0.000000 +motor_position.sources.0.compensation_table.1 0.000000 +motor_position.sources.0.compensation_table.2 0.000000 +motor_position.sources.0.compensation_table.3 0.000000 +motor_position.sources.0.compensation_table.4 0.000000 +motor_position.sources.0.compensation_table.5 0.000000 +motor_position.sources.0.compensation_table.6 0.000000 +motor_position.sources.0.compensation_table.7 0.000000 +motor_position.sources.0.compensation_table.8 0.000000 +motor_position.sources.0.compensation_table.9 0.000000 +motor_position.sources.0.compensation_table.10 0.000000 +motor_position.sources.0.compensation_table.11 0.000000 +motor_position.sources.0.compensation_table.12 0.000000 +motor_position.sources.0.compensation_table.13 0.000000 +motor_position.sources.0.compensation_table.14 0.000000 +motor_position.sources.0.compensation_table.15 0.000000 +motor_position.sources.0.compensation_table.16 0.000000 +motor_position.sources.0.compensation_table.17 0.000000 +motor_position.sources.0.compensation_table.18 0.000000 +motor_position.sources.0.compensation_table.19 0.000000 +motor_position.sources.0.compensation_table.20 0.000000 +motor_position.sources.0.compensation_table.21 0.000000 +motor_position.sources.0.compensation_table.22 0.000000 +motor_position.sources.0.compensation_table.23 0.000000 +motor_position.sources.0.compensation_table.24 0.000000 +motor_position.sources.0.compensation_table.25 0.000000 +motor_position.sources.0.compensation_table.26 0.000000 +motor_position.sources.0.compensation_table.27 0.000000 +motor_position.sources.0.compensation_table.28 0.000000 +motor_position.sources.0.compensation_table.29 0.000000 +motor_position.sources.0.compensation_table.30 0.000000 +motor_position.sources.0.compensation_table.31 0.000000 +motor_position.sources.1.aux_number 1 +motor_position.sources.1.type 0 +motor_position.sources.1.i2c_device 0 +motor_position.sources.1.incremental_index -1 +motor_position.sources.1.cpr 16384 +motor_position.sources.1.offset 0.000000 +motor_position.sources.1.sign 1 +motor_position.sources.1.debug_override -1 +motor_position.sources.1.reference 0 +motor_position.sources.1.pll_filter_hz 400.000000 +motor_position.sources.1.compensation_table.0 0.000000 +motor_position.sources.1.compensation_table.1 0.000000 +motor_position.sources.1.compensation_table.2 0.000000 +motor_position.sources.1.compensation_table.3 0.000000 +motor_position.sources.1.compensation_table.4 0.000000 +motor_position.sources.1.compensation_table.5 0.000000 +motor_position.sources.1.compensation_table.6 0.000000 +motor_position.sources.1.compensation_table.7 0.000000 +motor_position.sources.1.compensation_table.8 0.000000 +motor_position.sources.1.compensation_table.9 0.000000 +motor_position.sources.1.compensation_table.10 0.000000 +motor_position.sources.1.compensation_table.11 0.000000 +motor_position.sources.1.compensation_table.12 0.000000 +motor_position.sources.1.compensation_table.13 0.000000 +motor_position.sources.1.compensation_table.14 0.000000 +motor_position.sources.1.compensation_table.15 0.000000 +motor_position.sources.1.compensation_table.16 0.000000 +motor_position.sources.1.compensation_table.17 0.000000 +motor_position.sources.1.compensation_table.18 0.000000 +motor_position.sources.1.compensation_table.19 0.000000 +motor_position.sources.1.compensation_table.20 0.000000 +motor_position.sources.1.compensation_table.21 0.000000 +motor_position.sources.1.compensation_table.22 0.000000 +motor_position.sources.1.compensation_table.23 0.000000 +motor_position.sources.1.compensation_table.24 0.000000 +motor_position.sources.1.compensation_table.25 0.000000 +motor_position.sources.1.compensation_table.26 0.000000 +motor_position.sources.1.compensation_table.27 0.000000 +motor_position.sources.1.compensation_table.28 0.000000 +motor_position.sources.1.compensation_table.29 0.000000 +motor_position.sources.1.compensation_table.30 0.000000 +motor_position.sources.1.compensation_table.31 0.000000 +motor_position.sources.2.aux_number 1 +motor_position.sources.2.type 0 +motor_position.sources.2.i2c_device 0 +motor_position.sources.2.incremental_index -1 +motor_position.sources.2.cpr 16384 +motor_position.sources.2.offset 0.000000 +motor_position.sources.2.sign 1 +motor_position.sources.2.debug_override -1 +motor_position.sources.2.reference 0 +motor_position.sources.2.pll_filter_hz 400.000000 +motor_position.sources.2.compensation_table.0 0.000000 +motor_position.sources.2.compensation_table.1 0.000000 +motor_position.sources.2.compensation_table.2 0.000000 +motor_position.sources.2.compensation_table.3 0.000000 +motor_position.sources.2.compensation_table.4 0.000000 +motor_position.sources.2.compensation_table.5 0.000000 +motor_position.sources.2.compensation_table.6 0.000000 +motor_position.sources.2.compensation_table.7 0.000000 +motor_position.sources.2.compensation_table.8 0.000000 +motor_position.sources.2.compensation_table.9 0.000000 +motor_position.sources.2.compensation_table.10 0.000000 +motor_position.sources.2.compensation_table.11 0.000000 +motor_position.sources.2.compensation_table.12 0.000000 +motor_position.sources.2.compensation_table.13 0.000000 +motor_position.sources.2.compensation_table.14 0.000000 +motor_position.sources.2.compensation_table.15 0.000000 +motor_position.sources.2.compensation_table.16 0.000000 +motor_position.sources.2.compensation_table.17 0.000000 +motor_position.sources.2.compensation_table.18 0.000000 +motor_position.sources.2.compensation_table.19 0.000000 +motor_position.sources.2.compensation_table.20 0.000000 +motor_position.sources.2.compensation_table.21 0.000000 +motor_position.sources.2.compensation_table.22 0.000000 +motor_position.sources.2.compensation_table.23 0.000000 +motor_position.sources.2.compensation_table.24 0.000000 +motor_position.sources.2.compensation_table.25 0.000000 +motor_position.sources.2.compensation_table.26 0.000000 +motor_position.sources.2.compensation_table.27 0.000000 +motor_position.sources.2.compensation_table.28 0.000000 +motor_position.sources.2.compensation_table.29 0.000000 +motor_position.sources.2.compensation_table.30 0.000000 +motor_position.sources.2.compensation_table.31 0.000000 +motor_position.commutation_source 0 +motor_position.output.source 0 +motor_position.output.offset 0.000000 +motor_position.output.sign 1 +motor_position.output.reference_source -1 +motor_position.rotor_to_output_ratio 0.005080 +motor.poles 14 +motor.phase_invert 0 +motor.resistance_ohm 0.160509 +motor.v_per_hz 0.132894 +motor.offset.0 0.000000 +motor.offset.1 0.000000 +motor.offset.2 0.000000 +motor.offset.3 0.000000 +motor.offset.4 0.000000 +motor.offset.5 0.000000 +motor.offset.6 0.000000 +motor.offset.7 0.000000 +motor.offset.8 0.000000 +motor.offset.9 0.000000 +motor.offset.10 0.000000 +motor.offset.11 0.000000 +motor.offset.12 0.000000 +motor.offset.13 0.000000 +motor.offset.14 0.000000 +motor.offset.15 0.000000 +motor.offset.16 0.000000 +motor.offset.17 0.000000 +motor.offset.18 0.000000 +motor.offset.19 0.000000 +motor.offset.20 0.000000 +motor.offset.21 0.000000 +motor.offset.22 0.000000 +motor.offset.23 0.000000 +motor.offset.24 0.000000 +motor.offset.25 0.000000 +motor.offset.26 0.000000 +motor.offset.27 0.000000 +motor.offset.28 0.000000 +motor.offset.29 0.000000 +motor.offset.30 0.000000 +motor.offset.31 0.000000 +motor.offset.32 0.000000 +motor.offset.33 0.000000 +motor.offset.34 0.000000 +motor.offset.35 0.000000 +motor.offset.36 0.000000 +motor.offset.37 0.000000 +motor.offset.38 0.000000 +motor.offset.39 0.000000 +motor.offset.40 0.000000 +motor.offset.41 0.000000 +motor.offset.42 0.000000 +motor.offset.43 0.000000 +motor.offset.44 0.000000 +motor.offset.45 0.000000 +motor.offset.46 0.000000 +motor.offset.47 0.000000 +motor.offset.48 0.000000 +motor.offset.49 0.000000 +motor.offset.50 0.000000 +motor.offset.51 0.000000 +motor.offset.52 0.000000 +motor.offset.53 0.000000 +motor.offset.54 0.000000 +motor.offset.55 0.000000 +motor.offset.56 0.000000 +motor.offset.57 0.000000 +motor.offset.58 0.000000 +motor.offset.59 0.000000 +motor.offset.60 0.000000 +motor.offset.61 0.000000 +motor.offset.62 0.000000 +motor.offset.63 0.000000 +motor.rotation_current_cutoff_A 10000.000000 +motor.rotation_current_scale 0.050000 +motor.rotation_torque_scale 14.700000 +motor.cogging_dq_scale 0.000000 +motor.cogging_dq_comp.0 0 +motor.cogging_dq_comp.1 0 +motor.cogging_dq_comp.2 0 +motor.cogging_dq_comp.3 0 +motor.cogging_dq_comp.4 0 +motor.cogging_dq_comp.5 0 +motor.cogging_dq_comp.6 0 +motor.cogging_dq_comp.7 0 +motor.cogging_dq_comp.8 0 +motor.cogging_dq_comp.9 0 +motor.cogging_dq_comp.10 0 +motor.cogging_dq_comp.11 0 +motor.cogging_dq_comp.12 0 +motor.cogging_dq_comp.13 0 +motor.cogging_dq_comp.14 0 +motor.cogging_dq_comp.15 0 +motor.cogging_dq_comp.16 0 +motor.cogging_dq_comp.17 0 +motor.cogging_dq_comp.18 0 +motor.cogging_dq_comp.19 0 +motor.cogging_dq_comp.20 0 +motor.cogging_dq_comp.21 0 +motor.cogging_dq_comp.22 0 +motor.cogging_dq_comp.23 0 +motor.cogging_dq_comp.24 0 +motor.cogging_dq_comp.25 0 +motor.cogging_dq_comp.26 0 +motor.cogging_dq_comp.27 0 +motor.cogging_dq_comp.28 0 +motor.cogging_dq_comp.29 0 +motor.cogging_dq_comp.30 0 +motor.cogging_dq_comp.31 0 +motor.cogging_dq_comp.32 0 +motor.cogging_dq_comp.33 0 +motor.cogging_dq_comp.34 0 +motor.cogging_dq_comp.35 0 +motor.cogging_dq_comp.36 0 +motor.cogging_dq_comp.37 0 +motor.cogging_dq_comp.38 0 +motor.cogging_dq_comp.39 0 +motor.cogging_dq_comp.40 0 +motor.cogging_dq_comp.41 0 +motor.cogging_dq_comp.42 0 +motor.cogging_dq_comp.43 0 +motor.cogging_dq_comp.44 0 +motor.cogging_dq_comp.45 0 +motor.cogging_dq_comp.46 0 +motor.cogging_dq_comp.47 0 +motor.cogging_dq_comp.48 0 +motor.cogging_dq_comp.49 0 +motor.cogging_dq_comp.50 0 +motor.cogging_dq_comp.51 0 +motor.cogging_dq_comp.52 0 +motor.cogging_dq_comp.53 0 +motor.cogging_dq_comp.54 0 +motor.cogging_dq_comp.55 0 +motor.cogging_dq_comp.56 0 +motor.cogging_dq_comp.57 0 +motor.cogging_dq_comp.58 0 +motor.cogging_dq_comp.59 0 +motor.cogging_dq_comp.60 0 +motor.cogging_dq_comp.61 0 +motor.cogging_dq_comp.62 0 +motor.cogging_dq_comp.63 0 +motor.cogging_dq_comp.64 0 +motor.cogging_dq_comp.65 0 +motor.cogging_dq_comp.66 0 +motor.cogging_dq_comp.67 0 +motor.cogging_dq_comp.68 0 +motor.cogging_dq_comp.69 0 +motor.cogging_dq_comp.70 0 +motor.cogging_dq_comp.71 0 +motor.cogging_dq_comp.72 0 +motor.cogging_dq_comp.73 0 +motor.cogging_dq_comp.74 0 +motor.cogging_dq_comp.75 0 +motor.cogging_dq_comp.76 0 +motor.cogging_dq_comp.77 0 +motor.cogging_dq_comp.78 0 +motor.cogging_dq_comp.79 0 +motor.cogging_dq_comp.80 0 +motor.cogging_dq_comp.81 0 +motor.cogging_dq_comp.82 0 +motor.cogging_dq_comp.83 0 +motor.cogging_dq_comp.84 0 +motor.cogging_dq_comp.85 0 +motor.cogging_dq_comp.86 0 +motor.cogging_dq_comp.87 0 +motor.cogging_dq_comp.88 0 +motor.cogging_dq_comp.89 0 +motor.cogging_dq_comp.90 0 +motor.cogging_dq_comp.91 0 +motor.cogging_dq_comp.92 0 +motor.cogging_dq_comp.93 0 +motor.cogging_dq_comp.94 0 +motor.cogging_dq_comp.95 0 +motor.cogging_dq_comp.96 0 +motor.cogging_dq_comp.97 0 +motor.cogging_dq_comp.98 0 +motor.cogging_dq_comp.99 0 +motor.cogging_dq_comp.100 0 +motor.cogging_dq_comp.101 0 +motor.cogging_dq_comp.102 0 +motor.cogging_dq_comp.103 0 +motor.cogging_dq_comp.104 0 +motor.cogging_dq_comp.105 0 +motor.cogging_dq_comp.106 0 +motor.cogging_dq_comp.107 0 +motor.cogging_dq_comp.108 0 +motor.cogging_dq_comp.109 0 +motor.cogging_dq_comp.110 0 +motor.cogging_dq_comp.111 0 +motor.cogging_dq_comp.112 0 +motor.cogging_dq_comp.113 0 +motor.cogging_dq_comp.114 0 +motor.cogging_dq_comp.115 0 +motor.cogging_dq_comp.116 0 +motor.cogging_dq_comp.117 0 +motor.cogging_dq_comp.118 0 +motor.cogging_dq_comp.119 0 +motor.cogging_dq_comp.120 0 +motor.cogging_dq_comp.121 0 +motor.cogging_dq_comp.122 0 +motor.cogging_dq_comp.123 0 +motor.cogging_dq_comp.124 0 +motor.cogging_dq_comp.125 0 +motor.cogging_dq_comp.126 0 +motor.cogging_dq_comp.127 0 +motor.cogging_dq_comp.128 0 +motor.cogging_dq_comp.129 0 +motor.cogging_dq_comp.130 0 +motor.cogging_dq_comp.131 0 +motor.cogging_dq_comp.132 0 +motor.cogging_dq_comp.133 0 +motor.cogging_dq_comp.134 0 +motor.cogging_dq_comp.135 0 +motor.cogging_dq_comp.136 0 +motor.cogging_dq_comp.137 0 +motor.cogging_dq_comp.138 0 +motor.cogging_dq_comp.139 0 +motor.cogging_dq_comp.140 0 +motor.cogging_dq_comp.141 0 +motor.cogging_dq_comp.142 0 +motor.cogging_dq_comp.143 0 +motor.cogging_dq_comp.144 0 +motor.cogging_dq_comp.145 0 +motor.cogging_dq_comp.146 0 +motor.cogging_dq_comp.147 0 +motor.cogging_dq_comp.148 0 +motor.cogging_dq_comp.149 0 +motor.cogging_dq_comp.150 0 +motor.cogging_dq_comp.151 0 +motor.cogging_dq_comp.152 0 +motor.cogging_dq_comp.153 0 +motor.cogging_dq_comp.154 0 +motor.cogging_dq_comp.155 0 +motor.cogging_dq_comp.156 0 +motor.cogging_dq_comp.157 0 +motor.cogging_dq_comp.158 0 +motor.cogging_dq_comp.159 0 +motor.cogging_dq_comp.160 0 +motor.cogging_dq_comp.161 0 +motor.cogging_dq_comp.162 0 +motor.cogging_dq_comp.163 0 +motor.cogging_dq_comp.164 0 +motor.cogging_dq_comp.165 0 +motor.cogging_dq_comp.166 0 +motor.cogging_dq_comp.167 0 +motor.cogging_dq_comp.168 0 +motor.cogging_dq_comp.169 0 +motor.cogging_dq_comp.170 0 +motor.cogging_dq_comp.171 0 +motor.cogging_dq_comp.172 0 +motor.cogging_dq_comp.173 0 +motor.cogging_dq_comp.174 0 +motor.cogging_dq_comp.175 0 +motor.cogging_dq_comp.176 0 +motor.cogging_dq_comp.177 0 +motor.cogging_dq_comp.178 0 +motor.cogging_dq_comp.179 0 +motor.cogging_dq_comp.180 0 +motor.cogging_dq_comp.181 0 +motor.cogging_dq_comp.182 0 +motor.cogging_dq_comp.183 0 +motor.cogging_dq_comp.184 0 +motor.cogging_dq_comp.185 0 +motor.cogging_dq_comp.186 0 +motor.cogging_dq_comp.187 0 +motor.cogging_dq_comp.188 0 +motor.cogging_dq_comp.189 0 +motor.cogging_dq_comp.190 0 +motor.cogging_dq_comp.191 0 +motor.cogging_dq_comp.192 0 +motor.cogging_dq_comp.193 0 +motor.cogging_dq_comp.194 0 +motor.cogging_dq_comp.195 0 +motor.cogging_dq_comp.196 0 +motor.cogging_dq_comp.197 0 +motor.cogging_dq_comp.198 0 +motor.cogging_dq_comp.199 0 +motor.cogging_dq_comp.200 0 +motor.cogging_dq_comp.201 0 +motor.cogging_dq_comp.202 0 +motor.cogging_dq_comp.203 0 +motor.cogging_dq_comp.204 0 +motor.cogging_dq_comp.205 0 +motor.cogging_dq_comp.206 0 +motor.cogging_dq_comp.207 0 +motor.cogging_dq_comp.208 0 +motor.cogging_dq_comp.209 0 +motor.cogging_dq_comp.210 0 +motor.cogging_dq_comp.211 0 +motor.cogging_dq_comp.212 0 +motor.cogging_dq_comp.213 0 +motor.cogging_dq_comp.214 0 +motor.cogging_dq_comp.215 0 +motor.cogging_dq_comp.216 0 +motor.cogging_dq_comp.217 0 +motor.cogging_dq_comp.218 0 +motor.cogging_dq_comp.219 0 +motor.cogging_dq_comp.220 0 +motor.cogging_dq_comp.221 0 +motor.cogging_dq_comp.222 0 +motor.cogging_dq_comp.223 0 +motor.cogging_dq_comp.224 0 +motor.cogging_dq_comp.225 0 +motor.cogging_dq_comp.226 0 +motor.cogging_dq_comp.227 0 +motor.cogging_dq_comp.228 0 +motor.cogging_dq_comp.229 0 +motor.cogging_dq_comp.230 0 +motor.cogging_dq_comp.231 0 +motor.cogging_dq_comp.232 0 +motor.cogging_dq_comp.233 0 +motor.cogging_dq_comp.234 0 +motor.cogging_dq_comp.235 0 +motor.cogging_dq_comp.236 0 +motor.cogging_dq_comp.237 0 +motor.cogging_dq_comp.238 0 +motor.cogging_dq_comp.239 0 +motor.cogging_dq_comp.240 0 +motor.cogging_dq_comp.241 0 +motor.cogging_dq_comp.242 0 +motor.cogging_dq_comp.243 0 +motor.cogging_dq_comp.244 0 +motor.cogging_dq_comp.245 0 +motor.cogging_dq_comp.246 0 +motor.cogging_dq_comp.247 0 +motor.cogging_dq_comp.248 0 +motor.cogging_dq_comp.249 0 +motor.cogging_dq_comp.250 0 +motor.cogging_dq_comp.251 0 +motor.cogging_dq_comp.252 0 +motor.cogging_dq_comp.253 0 +motor.cogging_dq_comp.254 0 +motor.cogging_dq_comp.255 0 +motor.cogging_dq_comp.256 0 +motor.cogging_dq_comp.257 0 +motor.cogging_dq_comp.258 0 +motor.cogging_dq_comp.259 0 +motor.cogging_dq_comp.260 0 +motor.cogging_dq_comp.261 0 +motor.cogging_dq_comp.262 0 +motor.cogging_dq_comp.263 0 +motor.cogging_dq_comp.264 0 +motor.cogging_dq_comp.265 0 +motor.cogging_dq_comp.266 0 +motor.cogging_dq_comp.267 0 +motor.cogging_dq_comp.268 0 +motor.cogging_dq_comp.269 0 +motor.cogging_dq_comp.270 0 +motor.cogging_dq_comp.271 0 +motor.cogging_dq_comp.272 0 +motor.cogging_dq_comp.273 0 +motor.cogging_dq_comp.274 0 +motor.cogging_dq_comp.275 0 +motor.cogging_dq_comp.276 0 +motor.cogging_dq_comp.277 0 +motor.cogging_dq_comp.278 0 +motor.cogging_dq_comp.279 0 +motor.cogging_dq_comp.280 0 +motor.cogging_dq_comp.281 0 +motor.cogging_dq_comp.282 0 +motor.cogging_dq_comp.283 0 +motor.cogging_dq_comp.284 0 +motor.cogging_dq_comp.285 0 +motor.cogging_dq_comp.286 0 +motor.cogging_dq_comp.287 0 +motor.cogging_dq_comp.288 0 +motor.cogging_dq_comp.289 0 +motor.cogging_dq_comp.290 0 +motor.cogging_dq_comp.291 0 +motor.cogging_dq_comp.292 0 +motor.cogging_dq_comp.293 0 +motor.cogging_dq_comp.294 0 +motor.cogging_dq_comp.295 0 +motor.cogging_dq_comp.296 0 +motor.cogging_dq_comp.297 0 +motor.cogging_dq_comp.298 0 +motor.cogging_dq_comp.299 0 +motor.cogging_dq_comp.300 0 +motor.cogging_dq_comp.301 0 +motor.cogging_dq_comp.302 0 +motor.cogging_dq_comp.303 0 +motor.cogging_dq_comp.304 0 +motor.cogging_dq_comp.305 0 +motor.cogging_dq_comp.306 0 +motor.cogging_dq_comp.307 0 +motor.cogging_dq_comp.308 0 +motor.cogging_dq_comp.309 0 +motor.cogging_dq_comp.310 0 +motor.cogging_dq_comp.311 0 +motor.cogging_dq_comp.312 0 +motor.cogging_dq_comp.313 0 +motor.cogging_dq_comp.314 0 +motor.cogging_dq_comp.315 0 +motor.cogging_dq_comp.316 0 +motor.cogging_dq_comp.317 0 +motor.cogging_dq_comp.318 0 +motor.cogging_dq_comp.319 0 +motor.cogging_dq_comp.320 0 +motor.cogging_dq_comp.321 0 +motor.cogging_dq_comp.322 0 +motor.cogging_dq_comp.323 0 +motor.cogging_dq_comp.324 0 +motor.cogging_dq_comp.325 0 +motor.cogging_dq_comp.326 0 +motor.cogging_dq_comp.327 0 +motor.cogging_dq_comp.328 0 +motor.cogging_dq_comp.329 0 +motor.cogging_dq_comp.330 0 +motor.cogging_dq_comp.331 0 +motor.cogging_dq_comp.332 0 +motor.cogging_dq_comp.333 0 +motor.cogging_dq_comp.334 0 +motor.cogging_dq_comp.335 0 +motor.cogging_dq_comp.336 0 +motor.cogging_dq_comp.337 0 +motor.cogging_dq_comp.338 0 +motor.cogging_dq_comp.339 0 +motor.cogging_dq_comp.340 0 +motor.cogging_dq_comp.341 0 +motor.cogging_dq_comp.342 0 +motor.cogging_dq_comp.343 0 +motor.cogging_dq_comp.344 0 +motor.cogging_dq_comp.345 0 +motor.cogging_dq_comp.346 0 +motor.cogging_dq_comp.347 0 +motor.cogging_dq_comp.348 0 +motor.cogging_dq_comp.349 0 +motor.cogging_dq_comp.350 0 +motor.cogging_dq_comp.351 0 +motor.cogging_dq_comp.352 0 +motor.cogging_dq_comp.353 0 +motor.cogging_dq_comp.354 0 +motor.cogging_dq_comp.355 0 +motor.cogging_dq_comp.356 0 +motor.cogging_dq_comp.357 0 +motor.cogging_dq_comp.358 0 +motor.cogging_dq_comp.359 0 +motor.cogging_dq_comp.360 0 +motor.cogging_dq_comp.361 0 +motor.cogging_dq_comp.362 0 +motor.cogging_dq_comp.363 0 +motor.cogging_dq_comp.364 0 +motor.cogging_dq_comp.365 0 +motor.cogging_dq_comp.366 0 +motor.cogging_dq_comp.367 0 +motor.cogging_dq_comp.368 0 +motor.cogging_dq_comp.369 0 +motor.cogging_dq_comp.370 0 +motor.cogging_dq_comp.371 0 +motor.cogging_dq_comp.372 0 +motor.cogging_dq_comp.373 0 +motor.cogging_dq_comp.374 0 +motor.cogging_dq_comp.375 0 +motor.cogging_dq_comp.376 0 +motor.cogging_dq_comp.377 0 +motor.cogging_dq_comp.378 0 +motor.cogging_dq_comp.379 0 +motor.cogging_dq_comp.380 0 +motor.cogging_dq_comp.381 0 +motor.cogging_dq_comp.382 0 +motor.cogging_dq_comp.383 0 +motor.cogging_dq_comp.384 0 +motor.cogging_dq_comp.385 0 +motor.cogging_dq_comp.386 0 +motor.cogging_dq_comp.387 0 +motor.cogging_dq_comp.388 0 +motor.cogging_dq_comp.389 0 +motor.cogging_dq_comp.390 0 +motor.cogging_dq_comp.391 0 +motor.cogging_dq_comp.392 0 +motor.cogging_dq_comp.393 0 +motor.cogging_dq_comp.394 0 +motor.cogging_dq_comp.395 0 +motor.cogging_dq_comp.396 0 +motor.cogging_dq_comp.397 0 +motor.cogging_dq_comp.398 0 +motor.cogging_dq_comp.399 0 +motor.cogging_dq_comp.400 0 +motor.cogging_dq_comp.401 0 +motor.cogging_dq_comp.402 0 +motor.cogging_dq_comp.403 0 +motor.cogging_dq_comp.404 0 +motor.cogging_dq_comp.405 0 +motor.cogging_dq_comp.406 0 +motor.cogging_dq_comp.407 0 +motor.cogging_dq_comp.408 0 +motor.cogging_dq_comp.409 0 +motor.cogging_dq_comp.410 0 +motor.cogging_dq_comp.411 0 +motor.cogging_dq_comp.412 0 +motor.cogging_dq_comp.413 0 +motor.cogging_dq_comp.414 0 +motor.cogging_dq_comp.415 0 +motor.cogging_dq_comp.416 0 +motor.cogging_dq_comp.417 0 +motor.cogging_dq_comp.418 0 +motor.cogging_dq_comp.419 0 +motor.cogging_dq_comp.420 0 +motor.cogging_dq_comp.421 0 +motor.cogging_dq_comp.422 0 +motor.cogging_dq_comp.423 0 +motor.cogging_dq_comp.424 0 +motor.cogging_dq_comp.425 0 +motor.cogging_dq_comp.426 0 +motor.cogging_dq_comp.427 0 +motor.cogging_dq_comp.428 0 +motor.cogging_dq_comp.429 0 +motor.cogging_dq_comp.430 0 +motor.cogging_dq_comp.431 0 +motor.cogging_dq_comp.432 0 +motor.cogging_dq_comp.433 0 +motor.cogging_dq_comp.434 0 +motor.cogging_dq_comp.435 0 +motor.cogging_dq_comp.436 0 +motor.cogging_dq_comp.437 0 +motor.cogging_dq_comp.438 0 +motor.cogging_dq_comp.439 0 +motor.cogging_dq_comp.440 0 +motor.cogging_dq_comp.441 0 +motor.cogging_dq_comp.442 0 +motor.cogging_dq_comp.443 0 +motor.cogging_dq_comp.444 0 +motor.cogging_dq_comp.445 0 +motor.cogging_dq_comp.446 0 +motor.cogging_dq_comp.447 0 +motor.cogging_dq_comp.448 0 +motor.cogging_dq_comp.449 0 +motor.cogging_dq_comp.450 0 +motor.cogging_dq_comp.451 0 +motor.cogging_dq_comp.452 0 +motor.cogging_dq_comp.453 0 +motor.cogging_dq_comp.454 0 +motor.cogging_dq_comp.455 0 +motor.cogging_dq_comp.456 0 +motor.cogging_dq_comp.457 0 +motor.cogging_dq_comp.458 0 +motor.cogging_dq_comp.459 0 +motor.cogging_dq_comp.460 0 +motor.cogging_dq_comp.461 0 +motor.cogging_dq_comp.462 0 +motor.cogging_dq_comp.463 0 +motor.cogging_dq_comp.464 0 +motor.cogging_dq_comp.465 0 +motor.cogging_dq_comp.466 0 +motor.cogging_dq_comp.467 0 +motor.cogging_dq_comp.468 0 +motor.cogging_dq_comp.469 0 +motor.cogging_dq_comp.470 0 +motor.cogging_dq_comp.471 0 +motor.cogging_dq_comp.472 0 +motor.cogging_dq_comp.473 0 +motor.cogging_dq_comp.474 0 +motor.cogging_dq_comp.475 0 +motor.cogging_dq_comp.476 0 +motor.cogging_dq_comp.477 0 +motor.cogging_dq_comp.478 0 +motor.cogging_dq_comp.479 0 +motor.cogging_dq_comp.480 0 +motor.cogging_dq_comp.481 0 +motor.cogging_dq_comp.482 0 +motor.cogging_dq_comp.483 0 +motor.cogging_dq_comp.484 0 +motor.cogging_dq_comp.485 0 +motor.cogging_dq_comp.486 0 +motor.cogging_dq_comp.487 0 +motor.cogging_dq_comp.488 0 +motor.cogging_dq_comp.489 0 +motor.cogging_dq_comp.490 0 +motor.cogging_dq_comp.491 0 +motor.cogging_dq_comp.492 0 +motor.cogging_dq_comp.493 0 +motor.cogging_dq_comp.494 0 +motor.cogging_dq_comp.495 0 +motor.cogging_dq_comp.496 0 +motor.cogging_dq_comp.497 0 +motor.cogging_dq_comp.498 0 +motor.cogging_dq_comp.499 0 +motor.cogging_dq_comp.500 0 +motor.cogging_dq_comp.501 0 +motor.cogging_dq_comp.502 0 +motor.cogging_dq_comp.503 0 +motor.cogging_dq_comp.504 0 +motor.cogging_dq_comp.505 0 +motor.cogging_dq_comp.506 0 +motor.cogging_dq_comp.507 0 +motor.cogging_dq_comp.508 0 +motor.cogging_dq_comp.509 0 +motor.cogging_dq_comp.510 0 +motor.cogging_dq_comp.511 0 +motor.cogging_dq_comp.512 0 +motor.cogging_dq_comp.513 0 +motor.cogging_dq_comp.514 0 +motor.cogging_dq_comp.515 0 +motor.cogging_dq_comp.516 0 +motor.cogging_dq_comp.517 0 +motor.cogging_dq_comp.518 0 +motor.cogging_dq_comp.519 0 +motor.cogging_dq_comp.520 0 +motor.cogging_dq_comp.521 0 +motor.cogging_dq_comp.522 0 +motor.cogging_dq_comp.523 0 +motor.cogging_dq_comp.524 0 +motor.cogging_dq_comp.525 0 +motor.cogging_dq_comp.526 0 +motor.cogging_dq_comp.527 0 +motor.cogging_dq_comp.528 0 +motor.cogging_dq_comp.529 0 +motor.cogging_dq_comp.530 0 +motor.cogging_dq_comp.531 0 +motor.cogging_dq_comp.532 0 +motor.cogging_dq_comp.533 0 +motor.cogging_dq_comp.534 0 +motor.cogging_dq_comp.535 0 +motor.cogging_dq_comp.536 0 +motor.cogging_dq_comp.537 0 +motor.cogging_dq_comp.538 0 +motor.cogging_dq_comp.539 0 +motor.cogging_dq_comp.540 0 +motor.cogging_dq_comp.541 0 +motor.cogging_dq_comp.542 0 +motor.cogging_dq_comp.543 0 +motor.cogging_dq_comp.544 0 +motor.cogging_dq_comp.545 0 +motor.cogging_dq_comp.546 0 +motor.cogging_dq_comp.547 0 +motor.cogging_dq_comp.548 0 +motor.cogging_dq_comp.549 0 +motor.cogging_dq_comp.550 0 +motor.cogging_dq_comp.551 0 +motor.cogging_dq_comp.552 0 +motor.cogging_dq_comp.553 0 +motor.cogging_dq_comp.554 0 +motor.cogging_dq_comp.555 0 +motor.cogging_dq_comp.556 0 +motor.cogging_dq_comp.557 0 +motor.cogging_dq_comp.558 0 +motor.cogging_dq_comp.559 0 +motor.cogging_dq_comp.560 0 +motor.cogging_dq_comp.561 0 +motor.cogging_dq_comp.562 0 +motor.cogging_dq_comp.563 0 +motor.cogging_dq_comp.564 0 +motor.cogging_dq_comp.565 0 +motor.cogging_dq_comp.566 0 +motor.cogging_dq_comp.567 0 +motor.cogging_dq_comp.568 0 +motor.cogging_dq_comp.569 0 +motor.cogging_dq_comp.570 0 +motor.cogging_dq_comp.571 0 +motor.cogging_dq_comp.572 0 +motor.cogging_dq_comp.573 0 +motor.cogging_dq_comp.574 0 +motor.cogging_dq_comp.575 0 +motor.cogging_dq_comp.576 0 +motor.cogging_dq_comp.577 0 +motor.cogging_dq_comp.578 0 +motor.cogging_dq_comp.579 0 +motor.cogging_dq_comp.580 0 +motor.cogging_dq_comp.581 0 +motor.cogging_dq_comp.582 0 +motor.cogging_dq_comp.583 0 +motor.cogging_dq_comp.584 0 +motor.cogging_dq_comp.585 0 +motor.cogging_dq_comp.586 0 +motor.cogging_dq_comp.587 0 +motor.cogging_dq_comp.588 0 +motor.cogging_dq_comp.589 0 +motor.cogging_dq_comp.590 0 +motor.cogging_dq_comp.591 0 +motor.cogging_dq_comp.592 0 +motor.cogging_dq_comp.593 0 +motor.cogging_dq_comp.594 0 +motor.cogging_dq_comp.595 0 +motor.cogging_dq_comp.596 0 +motor.cogging_dq_comp.597 0 +motor.cogging_dq_comp.598 0 +motor.cogging_dq_comp.599 0 +motor.cogging_dq_comp.600 0 +motor.cogging_dq_comp.601 0 +motor.cogging_dq_comp.602 0 +motor.cogging_dq_comp.603 0 +motor.cogging_dq_comp.604 0 +motor.cogging_dq_comp.605 0 +motor.cogging_dq_comp.606 0 +motor.cogging_dq_comp.607 0 +motor.cogging_dq_comp.608 0 +motor.cogging_dq_comp.609 0 +motor.cogging_dq_comp.610 0 +motor.cogging_dq_comp.611 0 +motor.cogging_dq_comp.612 0 +motor.cogging_dq_comp.613 0 +motor.cogging_dq_comp.614 0 +motor.cogging_dq_comp.615 0 +motor.cogging_dq_comp.616 0 +motor.cogging_dq_comp.617 0 +motor.cogging_dq_comp.618 0 +motor.cogging_dq_comp.619 0 +motor.cogging_dq_comp.620 0 +motor.cogging_dq_comp.621 0 +motor.cogging_dq_comp.622 0 +motor.cogging_dq_comp.623 0 +motor.cogging_dq_comp.624 0 +motor.cogging_dq_comp.625 0 +motor.cogging_dq_comp.626 0 +motor.cogging_dq_comp.627 0 +motor.cogging_dq_comp.628 0 +motor.cogging_dq_comp.629 0 +motor.cogging_dq_comp.630 0 +motor.cogging_dq_comp.631 0 +motor.cogging_dq_comp.632 0 +motor.cogging_dq_comp.633 0 +motor.cogging_dq_comp.634 0 +motor.cogging_dq_comp.635 0 +motor.cogging_dq_comp.636 0 +motor.cogging_dq_comp.637 0 +motor.cogging_dq_comp.638 0 +motor.cogging_dq_comp.639 0 +motor.cogging_dq_comp.640 0 +motor.cogging_dq_comp.641 0 +motor.cogging_dq_comp.642 0 +motor.cogging_dq_comp.643 0 +motor.cogging_dq_comp.644 0 +motor.cogging_dq_comp.645 0 +motor.cogging_dq_comp.646 0 +motor.cogging_dq_comp.647 0 +motor.cogging_dq_comp.648 0 +motor.cogging_dq_comp.649 0 +motor.cogging_dq_comp.650 0 +motor.cogging_dq_comp.651 0 +motor.cogging_dq_comp.652 0 +motor.cogging_dq_comp.653 0 +motor.cogging_dq_comp.654 0 +motor.cogging_dq_comp.655 0 +motor.cogging_dq_comp.656 0 +motor.cogging_dq_comp.657 0 +motor.cogging_dq_comp.658 0 +motor.cogging_dq_comp.659 0 +motor.cogging_dq_comp.660 0 +motor.cogging_dq_comp.661 0 +motor.cogging_dq_comp.662 0 +motor.cogging_dq_comp.663 0 +motor.cogging_dq_comp.664 0 +motor.cogging_dq_comp.665 0 +motor.cogging_dq_comp.666 0 +motor.cogging_dq_comp.667 0 +motor.cogging_dq_comp.668 0 +motor.cogging_dq_comp.669 0 +motor.cogging_dq_comp.670 0 +motor.cogging_dq_comp.671 0 +motor.cogging_dq_comp.672 0 +motor.cogging_dq_comp.673 0 +motor.cogging_dq_comp.674 0 +motor.cogging_dq_comp.675 0 +motor.cogging_dq_comp.676 0 +motor.cogging_dq_comp.677 0 +motor.cogging_dq_comp.678 0 +motor.cogging_dq_comp.679 0 +motor.cogging_dq_comp.680 0 +motor.cogging_dq_comp.681 0 +motor.cogging_dq_comp.682 0 +motor.cogging_dq_comp.683 0 +motor.cogging_dq_comp.684 0 +motor.cogging_dq_comp.685 0 +motor.cogging_dq_comp.686 0 +motor.cogging_dq_comp.687 0 +motor.cogging_dq_comp.688 0 +motor.cogging_dq_comp.689 0 +motor.cogging_dq_comp.690 0 +motor.cogging_dq_comp.691 0 +motor.cogging_dq_comp.692 0 +motor.cogging_dq_comp.693 0 +motor.cogging_dq_comp.694 0 +motor.cogging_dq_comp.695 0 +motor.cogging_dq_comp.696 0 +motor.cogging_dq_comp.697 0 +motor.cogging_dq_comp.698 0 +motor.cogging_dq_comp.699 0 +motor.cogging_dq_comp.700 0 +motor.cogging_dq_comp.701 0 +motor.cogging_dq_comp.702 0 +motor.cogging_dq_comp.703 0 +motor.cogging_dq_comp.704 0 +motor.cogging_dq_comp.705 0 +motor.cogging_dq_comp.706 0 +motor.cogging_dq_comp.707 0 +motor.cogging_dq_comp.708 0 +motor.cogging_dq_comp.709 0 +motor.cogging_dq_comp.710 0 +motor.cogging_dq_comp.711 0 +motor.cogging_dq_comp.712 0 +motor.cogging_dq_comp.713 0 +motor.cogging_dq_comp.714 0 +motor.cogging_dq_comp.715 0 +motor.cogging_dq_comp.716 0 +motor.cogging_dq_comp.717 0 +motor.cogging_dq_comp.718 0 +motor.cogging_dq_comp.719 0 +motor.cogging_dq_comp.720 0 +motor.cogging_dq_comp.721 0 +motor.cogging_dq_comp.722 0 +motor.cogging_dq_comp.723 0 +motor.cogging_dq_comp.724 0 +motor.cogging_dq_comp.725 0 +motor.cogging_dq_comp.726 0 +motor.cogging_dq_comp.727 0 +motor.cogging_dq_comp.728 0 +motor.cogging_dq_comp.729 0 +motor.cogging_dq_comp.730 0 +motor.cogging_dq_comp.731 0 +motor.cogging_dq_comp.732 0 +motor.cogging_dq_comp.733 0 +motor.cogging_dq_comp.734 0 +motor.cogging_dq_comp.735 0 +motor.cogging_dq_comp.736 0 +motor.cogging_dq_comp.737 0 +motor.cogging_dq_comp.738 0 +motor.cogging_dq_comp.739 0 +motor.cogging_dq_comp.740 0 +motor.cogging_dq_comp.741 0 +motor.cogging_dq_comp.742 0 +motor.cogging_dq_comp.743 0 +motor.cogging_dq_comp.744 0 +motor.cogging_dq_comp.745 0 +motor.cogging_dq_comp.746 0 +motor.cogging_dq_comp.747 0 +motor.cogging_dq_comp.748 0 +motor.cogging_dq_comp.749 0 +motor.cogging_dq_comp.750 0 +motor.cogging_dq_comp.751 0 +motor.cogging_dq_comp.752 0 +motor.cogging_dq_comp.753 0 +motor.cogging_dq_comp.754 0 +motor.cogging_dq_comp.755 0 +motor.cogging_dq_comp.756 0 +motor.cogging_dq_comp.757 0 +motor.cogging_dq_comp.758 0 +motor.cogging_dq_comp.759 0 +motor.cogging_dq_comp.760 0 +motor.cogging_dq_comp.761 0 +motor.cogging_dq_comp.762 0 +motor.cogging_dq_comp.763 0 +motor.cogging_dq_comp.764 0 +motor.cogging_dq_comp.765 0 +motor.cogging_dq_comp.766 0 +motor.cogging_dq_comp.767 0 +motor.cogging_dq_comp.768 0 +motor.cogging_dq_comp.769 0 +motor.cogging_dq_comp.770 0 +motor.cogging_dq_comp.771 0 +motor.cogging_dq_comp.772 0 +motor.cogging_dq_comp.773 0 +motor.cogging_dq_comp.774 0 +motor.cogging_dq_comp.775 0 +motor.cogging_dq_comp.776 0 +motor.cogging_dq_comp.777 0 +motor.cogging_dq_comp.778 0 +motor.cogging_dq_comp.779 0 +motor.cogging_dq_comp.780 0 +motor.cogging_dq_comp.781 0 +motor.cogging_dq_comp.782 0 +motor.cogging_dq_comp.783 0 +motor.cogging_dq_comp.784 0 +motor.cogging_dq_comp.785 0 +motor.cogging_dq_comp.786 0 +motor.cogging_dq_comp.787 0 +motor.cogging_dq_comp.788 0 +motor.cogging_dq_comp.789 0 +motor.cogging_dq_comp.790 0 +motor.cogging_dq_comp.791 0 +motor.cogging_dq_comp.792 0 +motor.cogging_dq_comp.793 0 +motor.cogging_dq_comp.794 0 +motor.cogging_dq_comp.795 0 +motor.cogging_dq_comp.796 0 +motor.cogging_dq_comp.797 0 +motor.cogging_dq_comp.798 0 +motor.cogging_dq_comp.799 0 +motor.cogging_dq_comp.800 0 +motor.cogging_dq_comp.801 0 +motor.cogging_dq_comp.802 0 +motor.cogging_dq_comp.803 0 +motor.cogging_dq_comp.804 0 +motor.cogging_dq_comp.805 0 +motor.cogging_dq_comp.806 0 +motor.cogging_dq_comp.807 0 +motor.cogging_dq_comp.808 0 +motor.cogging_dq_comp.809 0 +motor.cogging_dq_comp.810 0 +motor.cogging_dq_comp.811 0 +motor.cogging_dq_comp.812 0 +motor.cogging_dq_comp.813 0 +motor.cogging_dq_comp.814 0 +motor.cogging_dq_comp.815 0 +motor.cogging_dq_comp.816 0 +motor.cogging_dq_comp.817 0 +motor.cogging_dq_comp.818 0 +motor.cogging_dq_comp.819 0 +motor.cogging_dq_comp.820 0 +motor.cogging_dq_comp.821 0 +motor.cogging_dq_comp.822 0 +motor.cogging_dq_comp.823 0 +motor.cogging_dq_comp.824 0 +motor.cogging_dq_comp.825 0 +motor.cogging_dq_comp.826 0 +motor.cogging_dq_comp.827 0 +motor.cogging_dq_comp.828 0 +motor.cogging_dq_comp.829 0 +motor.cogging_dq_comp.830 0 +motor.cogging_dq_comp.831 0 +motor.cogging_dq_comp.832 0 +motor.cogging_dq_comp.833 0 +motor.cogging_dq_comp.834 0 +motor.cogging_dq_comp.835 0 +motor.cogging_dq_comp.836 0 +motor.cogging_dq_comp.837 0 +motor.cogging_dq_comp.838 0 +motor.cogging_dq_comp.839 0 +motor.cogging_dq_comp.840 0 +motor.cogging_dq_comp.841 0 +motor.cogging_dq_comp.842 0 +motor.cogging_dq_comp.843 0 +motor.cogging_dq_comp.844 0 +motor.cogging_dq_comp.845 0 +motor.cogging_dq_comp.846 0 +motor.cogging_dq_comp.847 0 +motor.cogging_dq_comp.848 0 +motor.cogging_dq_comp.849 0 +motor.cogging_dq_comp.850 0 +motor.cogging_dq_comp.851 0 +motor.cogging_dq_comp.852 0 +motor.cogging_dq_comp.853 0 +motor.cogging_dq_comp.854 0 +motor.cogging_dq_comp.855 0 +motor.cogging_dq_comp.856 0 +motor.cogging_dq_comp.857 0 +motor.cogging_dq_comp.858 0 +motor.cogging_dq_comp.859 0 +motor.cogging_dq_comp.860 0 +motor.cogging_dq_comp.861 0 +motor.cogging_dq_comp.862 0 +motor.cogging_dq_comp.863 0 +motor.cogging_dq_comp.864 0 +motor.cogging_dq_comp.865 0 +motor.cogging_dq_comp.866 0 +motor.cogging_dq_comp.867 0 +motor.cogging_dq_comp.868 0 +motor.cogging_dq_comp.869 0 +motor.cogging_dq_comp.870 0 +motor.cogging_dq_comp.871 0 +motor.cogging_dq_comp.872 0 +motor.cogging_dq_comp.873 0 +motor.cogging_dq_comp.874 0 +motor.cogging_dq_comp.875 0 +motor.cogging_dq_comp.876 0 +motor.cogging_dq_comp.877 0 +motor.cogging_dq_comp.878 0 +motor.cogging_dq_comp.879 0 +motor.cogging_dq_comp.880 0 +motor.cogging_dq_comp.881 0 +motor.cogging_dq_comp.882 0 +motor.cogging_dq_comp.883 0 +motor.cogging_dq_comp.884 0 +motor.cogging_dq_comp.885 0 +motor.cogging_dq_comp.886 0 +motor.cogging_dq_comp.887 0 +motor.cogging_dq_comp.888 0 +motor.cogging_dq_comp.889 0 +motor.cogging_dq_comp.890 0 +motor.cogging_dq_comp.891 0 +motor.cogging_dq_comp.892 0 +motor.cogging_dq_comp.893 0 +motor.cogging_dq_comp.894 0 +motor.cogging_dq_comp.895 0 +motor.cogging_dq_comp.896 0 +motor.cogging_dq_comp.897 0 +motor.cogging_dq_comp.898 0 +motor.cogging_dq_comp.899 0 +motor.cogging_dq_comp.900 0 +motor.cogging_dq_comp.901 0 +motor.cogging_dq_comp.902 0 +motor.cogging_dq_comp.903 0 +motor.cogging_dq_comp.904 0 +motor.cogging_dq_comp.905 0 +motor.cogging_dq_comp.906 0 +motor.cogging_dq_comp.907 0 +motor.cogging_dq_comp.908 0 +motor.cogging_dq_comp.909 0 +motor.cogging_dq_comp.910 0 +motor.cogging_dq_comp.911 0 +motor.cogging_dq_comp.912 0 +motor.cogging_dq_comp.913 0 +motor.cogging_dq_comp.914 0 +motor.cogging_dq_comp.915 0 +motor.cogging_dq_comp.916 0 +motor.cogging_dq_comp.917 0 +motor.cogging_dq_comp.918 0 +motor.cogging_dq_comp.919 0 +motor.cogging_dq_comp.920 0 +motor.cogging_dq_comp.921 0 +motor.cogging_dq_comp.922 0 +motor.cogging_dq_comp.923 0 +motor.cogging_dq_comp.924 0 +motor.cogging_dq_comp.925 0 +motor.cogging_dq_comp.926 0 +motor.cogging_dq_comp.927 0 +motor.cogging_dq_comp.928 0 +motor.cogging_dq_comp.929 0 +motor.cogging_dq_comp.930 0 +motor.cogging_dq_comp.931 0 +motor.cogging_dq_comp.932 0 +motor.cogging_dq_comp.933 0 +motor.cogging_dq_comp.934 0 +motor.cogging_dq_comp.935 0 +motor.cogging_dq_comp.936 0 +motor.cogging_dq_comp.937 0 +motor.cogging_dq_comp.938 0 +motor.cogging_dq_comp.939 0 +motor.cogging_dq_comp.940 0 +motor.cogging_dq_comp.941 0 +motor.cogging_dq_comp.942 0 +motor.cogging_dq_comp.943 0 +motor.cogging_dq_comp.944 0 +motor.cogging_dq_comp.945 0 +motor.cogging_dq_comp.946 0 +motor.cogging_dq_comp.947 0 +motor.cogging_dq_comp.948 0 +motor.cogging_dq_comp.949 0 +motor.cogging_dq_comp.950 0 +motor.cogging_dq_comp.951 0 +motor.cogging_dq_comp.952 0 +motor.cogging_dq_comp.953 0 +motor.cogging_dq_comp.954 0 +motor.cogging_dq_comp.955 0 +motor.cogging_dq_comp.956 0 +motor.cogging_dq_comp.957 0 +motor.cogging_dq_comp.958 0 +motor.cogging_dq_comp.959 0 +motor.cogging_dq_comp.960 0 +motor.cogging_dq_comp.961 0 +motor.cogging_dq_comp.962 0 +motor.cogging_dq_comp.963 0 +motor.cogging_dq_comp.964 0 +motor.cogging_dq_comp.965 0 +motor.cogging_dq_comp.966 0 +motor.cogging_dq_comp.967 0 +motor.cogging_dq_comp.968 0 +motor.cogging_dq_comp.969 0 +motor.cogging_dq_comp.970 0 +motor.cogging_dq_comp.971 0 +motor.cogging_dq_comp.972 0 +motor.cogging_dq_comp.973 0 +motor.cogging_dq_comp.974 0 +motor.cogging_dq_comp.975 0 +motor.cogging_dq_comp.976 0 +motor.cogging_dq_comp.977 0 +motor.cogging_dq_comp.978 0 +motor.cogging_dq_comp.979 0 +motor.cogging_dq_comp.980 0 +motor.cogging_dq_comp.981 0 +motor.cogging_dq_comp.982 0 +motor.cogging_dq_comp.983 0 +motor.cogging_dq_comp.984 0 +motor.cogging_dq_comp.985 0 +motor.cogging_dq_comp.986 0 +motor.cogging_dq_comp.987 0 +motor.cogging_dq_comp.988 0 +motor.cogging_dq_comp.989 0 +motor.cogging_dq_comp.990 0 +motor.cogging_dq_comp.991 0 +motor.cogging_dq_comp.992 0 +motor.cogging_dq_comp.993 0 +motor.cogging_dq_comp.994 0 +motor.cogging_dq_comp.995 0 +motor.cogging_dq_comp.996 0 +motor.cogging_dq_comp.997 0 +motor.cogging_dq_comp.998 0 +motor.cogging_dq_comp.999 0 +motor.cogging_dq_comp.1000 0 +motor.cogging_dq_comp.1001 0 +motor.cogging_dq_comp.1002 0 +motor.cogging_dq_comp.1003 0 +motor.cogging_dq_comp.1004 0 +motor.cogging_dq_comp.1005 0 +motor.cogging_dq_comp.1006 0 +motor.cogging_dq_comp.1007 0 +motor.cogging_dq_comp.1008 0 +motor.cogging_dq_comp.1009 0 +motor.cogging_dq_comp.1010 0 +motor.cogging_dq_comp.1011 0 +motor.cogging_dq_comp.1012 0 +motor.cogging_dq_comp.1013 0 +motor.cogging_dq_comp.1014 0 +motor.cogging_dq_comp.1015 0 +motor.cogging_dq_comp.1016 0 +motor.cogging_dq_comp.1017 0 +motor.cogging_dq_comp.1018 0 +motor.cogging_dq_comp.1019 0 +motor.cogging_dq_comp.1020 0 +motor.cogging_dq_comp.1021 0 +motor.cogging_dq_comp.1022 0 +motor.cogging_dq_comp.1023 0 +drv8323_conf.dis_cpuv 0 +drv8323_conf.dis_gdf 0 +drv8323_conf.otw_rep 0 +drv8323_conf.pwm_mode 1 +drv8323_conf.pwm_1x_asynchronous 0 +drv8323_conf.pwm_1x_dir 0 +drv8323_conf.idrivep_hs_ma 100 +drv8323_conf.idriven_hs_ma 200 +drv8323_conf.cbc 1 +drv8323_conf.tdrive_ns 1000 +drv8323_conf.idrivep_ls_ma 100 +drv8323_conf.idriven_ls_ma 200 +drv8323_conf.tretry 0 +drv8323_conf.dead_time_ns 50 +drv8323_conf.ocp_mode 0 +drv8323_conf.ocp_deg_us 4 +drv8323_conf.vds_lvl_mv 700 +drv8323_conf.csa_fet 0 +drv8323_conf.vref_div 1 +drv8323_conf.ls_ref 0 +drv8323_conf.csa_gain 20 +drv8323_conf.dis_sen 0 +drv8323_conf.sen_lvl_mv 500 +servo.pwm_rate_hz 30000 +servo.i_gain 20.000000 +servo.current_sense_ohm 0.000500 +servo.pwm_comp_off 0.027000 +servo.pwm_comp_mag 0.005000 +servo.pwm_scale 1.000000 +servo.max_voltage 46.000000 +servo.max_power_W 450.000000 +servo.derate_temperature 50.000000 +servo.fault_temperature 75.000000 +servo.enable_motor_temperature 0 +servo.motor_derate_temperature 50.000000 +servo.motor_fault_temperature nan +servo.velocity_threshold 0.000000 +servo.position_derate 0.020000 +servo.adc_cur_cycles 2 +servo.adc_aux_cycles 47 +servo.pid_dq.kp 0.060857 +servo.pid_dq.ki 100.850487 +servo.pid_position.kp 3000.000000 +servo.pid_position.ki 0.000000 +servo.pid_position.kd 200.000000 +servo.pid_position.iratelimit -1.000000 +servo.pid_position.ilimit 0.000000 +servo.pid_position.max_desired_rate 0.000000 +servo.pid_position.sign -1 +servo.current_feedforward 1.000000 +servo.bemf_feedforward 1.000000 +servo.default_velocity_limit 0.150000 +servo.default_accel_limit 0.300000 +servo.voltage_mode_control 0 +servo.fixed_voltage_mode 0 +servo.fixed_voltage_control_V 0.000000 +servo.max_position_slip nan +servo.max_velocity_slip nan +servo.default_timeout_s 0.100000 +servo.timeout_max_torque_Nm 5.000000 +servo.timeout_mode 12 +servo.flux_brake_min_voltage 43.500000 +servo.flux_brake_resistance_ohm 0.025000 +servo.max_current_A 100.000000 +servo.derate_current_A -20.000000 +servo.max_velocity 0.180000 +servo.max_velocity_derate 2.000000 +servo.cooldown_cycles 256 +servo.velocity_zero_capture_threshold 0.050000 +servo.timing_fault 0 +servo.emit_debug 0 +servopos.position_min nan +servopos.position_max nan +id.id 32 +can.prefix 0 + diff --git a/config/moteus/joint_c.cfg b/config/moteus/joint_c.cfg new file mode 100644 index 000000000..761535fc1 --- /dev/null +++ b/config/moteus/joint_c.cfg @@ -0,0 +1,1398 @@ +uuid.uuid.0 23 +uuid.uuid.1 245 +uuid.uuid.2 107 +uuid.uuid.3 1 +uuid.uuid.4 115 +uuid.uuid.5 71 +uuid.uuid.6 69 +uuid.uuid.7 218 +uuid.uuid.8 176 +uuid.uuid.9 234 +uuid.uuid.10 13 +uuid.uuid.11 64 +uuid.uuid.12 20 +uuid.uuid.13 175 +uuid.uuid.14 129 +uuid.uuid.15 210 +clock.hsitrim 64 +aux1.i2c.i2c_hz 100000 +aux1.i2c.i2c_mode 1 +aux1.i2c.pullup 1 +aux1.i2c.devices.0.type 1 +aux1.i2c.devices.0.address 64 +aux1.i2c.devices.0.poll_rate_us 1000 +aux1.i2c.devices.1.type 0 +aux1.i2c.devices.1.address 64 +aux1.i2c.devices.1.poll_rate_us 1000 +aux1.i2c.devices.2.type 0 +aux1.i2c.devices.2.address 64 +aux1.i2c.devices.2.poll_rate_us 1000 +aux1.spi.mode 1 +aux1.spi.rate_hz 12000000 +aux1.spi.filter_us 64 +aux1.spi.bct 0 +aux1.uart.mode 0 +aux1.uart.baud_rate 115200 +aux1.uart.poll_rate_us 100 +aux1.uart.rs422 0 +aux1.uart.cui_amt21_address 84 +aux1.quadrature.enabled 0 +aux1.quadrature.cpr 16384 +aux1.hall.enabled 0 +aux1.hall.polarity 0 +aux1.index.enabled 0 +aux1.sine_cosine.enabled 0 +aux1.sine_cosine.common 1700 +aux1.i2c_startup_delay_ms 30 +aux1.pins.0.mode 0 +aux1.pins.0.pull 0 +aux1.pins.1.mode 0 +aux1.pins.1.pull 0 +aux1.pins.2.mode 0 +aux1.pins.2.pull 0 +aux1.pins.3.mode 13 +aux1.pins.3.pull 1 +aux1.pins.4.mode 13 +aux1.pins.4.pull 1 +aux2.i2c.i2c_hz 400000 +aux2.i2c.i2c_mode 1 +aux2.i2c.pullup 0 +aux2.i2c.devices.0.type 0 +aux2.i2c.devices.0.address 64 +aux2.i2c.devices.0.poll_rate_us 1000 +aux2.i2c.devices.1.type 0 +aux2.i2c.devices.1.address 64 +aux2.i2c.devices.1.poll_rate_us 1000 +aux2.i2c.devices.2.type 0 +aux2.i2c.devices.2.address 64 +aux2.i2c.devices.2.poll_rate_us 1000 +aux2.spi.mode 1 +aux2.spi.rate_hz 12000000 +aux2.spi.filter_us 64 +aux2.spi.bct 0 +aux2.uart.mode 0 +aux2.uart.baud_rate 115200 +aux2.uart.poll_rate_us 100 +aux2.uart.rs422 0 +aux2.uart.cui_amt21_address 84 +aux2.quadrature.enabled 0 +aux2.quadrature.cpr 16384 +aux2.hall.enabled 1 +aux2.hall.polarity 0 +aux2.index.enabled 0 +aux2.sine_cosine.enabled 0 +aux2.sine_cosine.common 1700 +aux2.i2c_startup_delay_ms 30 +aux2.pins.0.mode 6 +aux2.pins.0.pull 1 +aux2.pins.1.mode 6 +aux2.pins.1.pull 1 +aux2.pins.2.mode 6 +aux2.pins.2.pull 1 +aux2.pins.3.mode 0 +aux2.pins.3.pull 0 +aux2.pins.4.mode 0 +aux2.pins.4.pull 0 +motor_position.sources.0.aux_number 2 +motor_position.sources.0.type 4 +motor_position.sources.0.i2c_device 0 +motor_position.sources.0.incremental_index -1 +motor_position.sources.0.cpr 42 +motor_position.sources.0.offset -1.000000 +motor_position.sources.0.sign -1 +motor_position.sources.0.debug_override -1 +motor_position.sources.0.reference 0 +motor_position.sources.0.pll_filter_hz 100.000000 +motor_position.sources.0.compensation_table.0 0.000000 +motor_position.sources.0.compensation_table.1 0.000000 +motor_position.sources.0.compensation_table.2 0.000000 +motor_position.sources.0.compensation_table.3 0.000000 +motor_position.sources.0.compensation_table.4 0.000000 +motor_position.sources.0.compensation_table.5 0.000000 +motor_position.sources.0.compensation_table.6 0.000000 +motor_position.sources.0.compensation_table.7 0.000000 +motor_position.sources.0.compensation_table.8 0.000000 +motor_position.sources.0.compensation_table.9 0.000000 +motor_position.sources.0.compensation_table.10 0.000000 +motor_position.sources.0.compensation_table.11 0.000000 +motor_position.sources.0.compensation_table.12 0.000000 +motor_position.sources.0.compensation_table.13 0.000000 +motor_position.sources.0.compensation_table.14 0.000000 +motor_position.sources.0.compensation_table.15 0.000000 +motor_position.sources.0.compensation_table.16 0.000000 +motor_position.sources.0.compensation_table.17 0.000000 +motor_position.sources.0.compensation_table.18 0.000000 +motor_position.sources.0.compensation_table.19 0.000000 +motor_position.sources.0.compensation_table.20 0.000000 +motor_position.sources.0.compensation_table.21 0.000000 +motor_position.sources.0.compensation_table.22 0.000000 +motor_position.sources.0.compensation_table.23 0.000000 +motor_position.sources.0.compensation_table.24 0.000000 +motor_position.sources.0.compensation_table.25 0.000000 +motor_position.sources.0.compensation_table.26 0.000000 +motor_position.sources.0.compensation_table.27 0.000000 +motor_position.sources.0.compensation_table.28 0.000000 +motor_position.sources.0.compensation_table.29 0.000000 +motor_position.sources.0.compensation_table.30 0.000000 +motor_position.sources.0.compensation_table.31 0.000000 +motor_position.sources.1.aux_number 1 +motor_position.sources.1.type 7 +motor_position.sources.1.i2c_device 0 +motor_position.sources.1.incremental_index -1 +motor_position.sources.1.cpr 65536 +motor_position.sources.1.offset 0.000000 +motor_position.sources.1.sign -1 +motor_position.sources.1.debug_override -1 +motor_position.sources.1.reference 1 +motor_position.sources.1.pll_filter_hz 100.000000 +motor_position.sources.1.compensation_table.0 0.000000 +motor_position.sources.1.compensation_table.1 0.000000 +motor_position.sources.1.compensation_table.2 0.000000 +motor_position.sources.1.compensation_table.3 0.000000 +motor_position.sources.1.compensation_table.4 0.000000 +motor_position.sources.1.compensation_table.5 0.000000 +motor_position.sources.1.compensation_table.6 0.000000 +motor_position.sources.1.compensation_table.7 0.000000 +motor_position.sources.1.compensation_table.8 0.000000 +motor_position.sources.1.compensation_table.9 0.000000 +motor_position.sources.1.compensation_table.10 0.000000 +motor_position.sources.1.compensation_table.11 0.000000 +motor_position.sources.1.compensation_table.12 0.000000 +motor_position.sources.1.compensation_table.13 0.000000 +motor_position.sources.1.compensation_table.14 0.000000 +motor_position.sources.1.compensation_table.15 0.000000 +motor_position.sources.1.compensation_table.16 0.000000 +motor_position.sources.1.compensation_table.17 0.000000 +motor_position.sources.1.compensation_table.18 0.000000 +motor_position.sources.1.compensation_table.19 0.000000 +motor_position.sources.1.compensation_table.20 0.000000 +motor_position.sources.1.compensation_table.21 0.000000 +motor_position.sources.1.compensation_table.22 0.000000 +motor_position.sources.1.compensation_table.23 0.000000 +motor_position.sources.1.compensation_table.24 0.000000 +motor_position.sources.1.compensation_table.25 0.000000 +motor_position.sources.1.compensation_table.26 0.000000 +motor_position.sources.1.compensation_table.27 0.000000 +motor_position.sources.1.compensation_table.28 0.000000 +motor_position.sources.1.compensation_table.29 0.000000 +motor_position.sources.1.compensation_table.30 0.000000 +motor_position.sources.1.compensation_table.31 0.000000 +motor_position.sources.2.aux_number 1 +motor_position.sources.2.type 0 +motor_position.sources.2.i2c_device 0 +motor_position.sources.2.incremental_index -1 +motor_position.sources.2.cpr 16384 +motor_position.sources.2.offset 0.000000 +motor_position.sources.2.sign 1 +motor_position.sources.2.debug_override -1 +motor_position.sources.2.reference 0 +motor_position.sources.2.pll_filter_hz 400.000000 +motor_position.sources.2.compensation_table.0 0.000000 +motor_position.sources.2.compensation_table.1 0.000000 +motor_position.sources.2.compensation_table.2 0.000000 +motor_position.sources.2.compensation_table.3 0.000000 +motor_position.sources.2.compensation_table.4 0.000000 +motor_position.sources.2.compensation_table.5 0.000000 +motor_position.sources.2.compensation_table.6 0.000000 +motor_position.sources.2.compensation_table.7 0.000000 +motor_position.sources.2.compensation_table.8 0.000000 +motor_position.sources.2.compensation_table.9 0.000000 +motor_position.sources.2.compensation_table.10 0.000000 +motor_position.sources.2.compensation_table.11 0.000000 +motor_position.sources.2.compensation_table.12 0.000000 +motor_position.sources.2.compensation_table.13 0.000000 +motor_position.sources.2.compensation_table.14 0.000000 +motor_position.sources.2.compensation_table.15 0.000000 +motor_position.sources.2.compensation_table.16 0.000000 +motor_position.sources.2.compensation_table.17 0.000000 +motor_position.sources.2.compensation_table.18 0.000000 +motor_position.sources.2.compensation_table.19 0.000000 +motor_position.sources.2.compensation_table.20 0.000000 +motor_position.sources.2.compensation_table.21 0.000000 +motor_position.sources.2.compensation_table.22 0.000000 +motor_position.sources.2.compensation_table.23 0.000000 +motor_position.sources.2.compensation_table.24 0.000000 +motor_position.sources.2.compensation_table.25 0.000000 +motor_position.sources.2.compensation_table.26 0.000000 +motor_position.sources.2.compensation_table.27 0.000000 +motor_position.sources.2.compensation_table.28 0.000000 +motor_position.sources.2.compensation_table.29 0.000000 +motor_position.sources.2.compensation_table.30 0.000000 +motor_position.sources.2.compensation_table.31 0.000000 +motor_position.commutation_source 0 +motor_position.output.source 0 +motor_position.output.offset 0.297363 +motor_position.output.sign 1 +motor_position.output.reference_source 1 +motor_position.rotor_to_output_ratio 0.002066 +motor.poles 14 +motor.phase_invert 0 +motor.resistance_ohm 0.162850 +motor.v_per_hz 0.000505 +motor.offset.0 0.000000 +motor.offset.1 0.000000 +motor.offset.2 0.000000 +motor.offset.3 0.000000 +motor.offset.4 0.000000 +motor.offset.5 0.000000 +motor.offset.6 0.000000 +motor.offset.7 0.000000 +motor.offset.8 0.000000 +motor.offset.9 0.000000 +motor.offset.10 0.000000 +motor.offset.11 0.000000 +motor.offset.12 0.000000 +motor.offset.13 0.000000 +motor.offset.14 0.000000 +motor.offset.15 0.000000 +motor.offset.16 0.000000 +motor.offset.17 0.000000 +motor.offset.18 0.000000 +motor.offset.19 0.000000 +motor.offset.20 0.000000 +motor.offset.21 0.000000 +motor.offset.22 0.000000 +motor.offset.23 0.000000 +motor.offset.24 0.000000 +motor.offset.25 0.000000 +motor.offset.26 0.000000 +motor.offset.27 0.000000 +motor.offset.28 0.000000 +motor.offset.29 0.000000 +motor.offset.30 0.000000 +motor.offset.31 0.000000 +motor.offset.32 0.000000 +motor.offset.33 0.000000 +motor.offset.34 0.000000 +motor.offset.35 0.000000 +motor.offset.36 0.000000 +motor.offset.37 0.000000 +motor.offset.38 0.000000 +motor.offset.39 0.000000 +motor.offset.40 0.000000 +motor.offset.41 0.000000 +motor.offset.42 0.000000 +motor.offset.43 0.000000 +motor.offset.44 0.000000 +motor.offset.45 0.000000 +motor.offset.46 0.000000 +motor.offset.47 0.000000 +motor.offset.48 0.000000 +motor.offset.49 0.000000 +motor.offset.50 0.000000 +motor.offset.51 0.000000 +motor.offset.52 0.000000 +motor.offset.53 0.000000 +motor.offset.54 0.000000 +motor.offset.55 0.000000 +motor.offset.56 0.000000 +motor.offset.57 0.000000 +motor.offset.58 0.000000 +motor.offset.59 0.000000 +motor.offset.60 0.000000 +motor.offset.61 0.000000 +motor.offset.62 0.000000 +motor.offset.63 0.000000 +motor.rotation_current_cutoff_A 10000.000000 +motor.rotation_current_scale 0.050000 +motor.rotation_torque_scale 14.700000 +motor.cogging_dq_scale 0.000000 +motor.cogging_dq_comp.0 0 +motor.cogging_dq_comp.1 0 +motor.cogging_dq_comp.2 0 +motor.cogging_dq_comp.3 0 +motor.cogging_dq_comp.4 0 +motor.cogging_dq_comp.5 0 +motor.cogging_dq_comp.6 0 +motor.cogging_dq_comp.7 0 +motor.cogging_dq_comp.8 0 +motor.cogging_dq_comp.9 0 +motor.cogging_dq_comp.10 0 +motor.cogging_dq_comp.11 0 +motor.cogging_dq_comp.12 0 +motor.cogging_dq_comp.13 0 +motor.cogging_dq_comp.14 0 +motor.cogging_dq_comp.15 0 +motor.cogging_dq_comp.16 0 +motor.cogging_dq_comp.17 0 +motor.cogging_dq_comp.18 0 +motor.cogging_dq_comp.19 0 +motor.cogging_dq_comp.20 0 +motor.cogging_dq_comp.21 0 +motor.cogging_dq_comp.22 0 +motor.cogging_dq_comp.23 0 +motor.cogging_dq_comp.24 0 +motor.cogging_dq_comp.25 0 +motor.cogging_dq_comp.26 0 +motor.cogging_dq_comp.27 0 +motor.cogging_dq_comp.28 0 +motor.cogging_dq_comp.29 0 +motor.cogging_dq_comp.30 0 +motor.cogging_dq_comp.31 0 +motor.cogging_dq_comp.32 0 +motor.cogging_dq_comp.33 0 +motor.cogging_dq_comp.34 0 +motor.cogging_dq_comp.35 0 +motor.cogging_dq_comp.36 0 +motor.cogging_dq_comp.37 0 +motor.cogging_dq_comp.38 0 +motor.cogging_dq_comp.39 0 +motor.cogging_dq_comp.40 0 +motor.cogging_dq_comp.41 0 +motor.cogging_dq_comp.42 0 +motor.cogging_dq_comp.43 0 +motor.cogging_dq_comp.44 0 +motor.cogging_dq_comp.45 0 +motor.cogging_dq_comp.46 0 +motor.cogging_dq_comp.47 0 +motor.cogging_dq_comp.48 0 +motor.cogging_dq_comp.49 0 +motor.cogging_dq_comp.50 0 +motor.cogging_dq_comp.51 0 +motor.cogging_dq_comp.52 0 +motor.cogging_dq_comp.53 0 +motor.cogging_dq_comp.54 0 +motor.cogging_dq_comp.55 0 +motor.cogging_dq_comp.56 0 +motor.cogging_dq_comp.57 0 +motor.cogging_dq_comp.58 0 +motor.cogging_dq_comp.59 0 +motor.cogging_dq_comp.60 0 +motor.cogging_dq_comp.61 0 +motor.cogging_dq_comp.62 0 +motor.cogging_dq_comp.63 0 +motor.cogging_dq_comp.64 0 +motor.cogging_dq_comp.65 0 +motor.cogging_dq_comp.66 0 +motor.cogging_dq_comp.67 0 +motor.cogging_dq_comp.68 0 +motor.cogging_dq_comp.69 0 +motor.cogging_dq_comp.70 0 +motor.cogging_dq_comp.71 0 +motor.cogging_dq_comp.72 0 +motor.cogging_dq_comp.73 0 +motor.cogging_dq_comp.74 0 +motor.cogging_dq_comp.75 0 +motor.cogging_dq_comp.76 0 +motor.cogging_dq_comp.77 0 +motor.cogging_dq_comp.78 0 +motor.cogging_dq_comp.79 0 +motor.cogging_dq_comp.80 0 +motor.cogging_dq_comp.81 0 +motor.cogging_dq_comp.82 0 +motor.cogging_dq_comp.83 0 +motor.cogging_dq_comp.84 0 +motor.cogging_dq_comp.85 0 +motor.cogging_dq_comp.86 0 +motor.cogging_dq_comp.87 0 +motor.cogging_dq_comp.88 0 +motor.cogging_dq_comp.89 0 +motor.cogging_dq_comp.90 0 +motor.cogging_dq_comp.91 0 +motor.cogging_dq_comp.92 0 +motor.cogging_dq_comp.93 0 +motor.cogging_dq_comp.94 0 +motor.cogging_dq_comp.95 0 +motor.cogging_dq_comp.96 0 +motor.cogging_dq_comp.97 0 +motor.cogging_dq_comp.98 0 +motor.cogging_dq_comp.99 0 +motor.cogging_dq_comp.100 0 +motor.cogging_dq_comp.101 0 +motor.cogging_dq_comp.102 0 +motor.cogging_dq_comp.103 0 +motor.cogging_dq_comp.104 0 +motor.cogging_dq_comp.105 0 +motor.cogging_dq_comp.106 0 +motor.cogging_dq_comp.107 0 +motor.cogging_dq_comp.108 0 +motor.cogging_dq_comp.109 0 +motor.cogging_dq_comp.110 0 +motor.cogging_dq_comp.111 0 +motor.cogging_dq_comp.112 0 +motor.cogging_dq_comp.113 0 +motor.cogging_dq_comp.114 0 +motor.cogging_dq_comp.115 0 +motor.cogging_dq_comp.116 0 +motor.cogging_dq_comp.117 0 +motor.cogging_dq_comp.118 0 +motor.cogging_dq_comp.119 0 +motor.cogging_dq_comp.120 0 +motor.cogging_dq_comp.121 0 +motor.cogging_dq_comp.122 0 +motor.cogging_dq_comp.123 0 +motor.cogging_dq_comp.124 0 +motor.cogging_dq_comp.125 0 +motor.cogging_dq_comp.126 0 +motor.cogging_dq_comp.127 0 +motor.cogging_dq_comp.128 0 +motor.cogging_dq_comp.129 0 +motor.cogging_dq_comp.130 0 +motor.cogging_dq_comp.131 0 +motor.cogging_dq_comp.132 0 +motor.cogging_dq_comp.133 0 +motor.cogging_dq_comp.134 0 +motor.cogging_dq_comp.135 0 +motor.cogging_dq_comp.136 0 +motor.cogging_dq_comp.137 0 +motor.cogging_dq_comp.138 0 +motor.cogging_dq_comp.139 0 +motor.cogging_dq_comp.140 0 +motor.cogging_dq_comp.141 0 +motor.cogging_dq_comp.142 0 +motor.cogging_dq_comp.143 0 +motor.cogging_dq_comp.144 0 +motor.cogging_dq_comp.145 0 +motor.cogging_dq_comp.146 0 +motor.cogging_dq_comp.147 0 +motor.cogging_dq_comp.148 0 +motor.cogging_dq_comp.149 0 +motor.cogging_dq_comp.150 0 +motor.cogging_dq_comp.151 0 +motor.cogging_dq_comp.152 0 +motor.cogging_dq_comp.153 0 +motor.cogging_dq_comp.154 0 +motor.cogging_dq_comp.155 0 +motor.cogging_dq_comp.156 0 +motor.cogging_dq_comp.157 0 +motor.cogging_dq_comp.158 0 +motor.cogging_dq_comp.159 0 +motor.cogging_dq_comp.160 0 +motor.cogging_dq_comp.161 0 +motor.cogging_dq_comp.162 0 +motor.cogging_dq_comp.163 0 +motor.cogging_dq_comp.164 0 +motor.cogging_dq_comp.165 0 +motor.cogging_dq_comp.166 0 +motor.cogging_dq_comp.167 0 +motor.cogging_dq_comp.168 0 +motor.cogging_dq_comp.169 0 +motor.cogging_dq_comp.170 0 +motor.cogging_dq_comp.171 0 +motor.cogging_dq_comp.172 0 +motor.cogging_dq_comp.173 0 +motor.cogging_dq_comp.174 0 +motor.cogging_dq_comp.175 0 +motor.cogging_dq_comp.176 0 +motor.cogging_dq_comp.177 0 +motor.cogging_dq_comp.178 0 +motor.cogging_dq_comp.179 0 +motor.cogging_dq_comp.180 0 +motor.cogging_dq_comp.181 0 +motor.cogging_dq_comp.182 0 +motor.cogging_dq_comp.183 0 +motor.cogging_dq_comp.184 0 +motor.cogging_dq_comp.185 0 +motor.cogging_dq_comp.186 0 +motor.cogging_dq_comp.187 0 +motor.cogging_dq_comp.188 0 +motor.cogging_dq_comp.189 0 +motor.cogging_dq_comp.190 0 +motor.cogging_dq_comp.191 0 +motor.cogging_dq_comp.192 0 +motor.cogging_dq_comp.193 0 +motor.cogging_dq_comp.194 0 +motor.cogging_dq_comp.195 0 +motor.cogging_dq_comp.196 0 +motor.cogging_dq_comp.197 0 +motor.cogging_dq_comp.198 0 +motor.cogging_dq_comp.199 0 +motor.cogging_dq_comp.200 0 +motor.cogging_dq_comp.201 0 +motor.cogging_dq_comp.202 0 +motor.cogging_dq_comp.203 0 +motor.cogging_dq_comp.204 0 +motor.cogging_dq_comp.205 0 +motor.cogging_dq_comp.206 0 +motor.cogging_dq_comp.207 0 +motor.cogging_dq_comp.208 0 +motor.cogging_dq_comp.209 0 +motor.cogging_dq_comp.210 0 +motor.cogging_dq_comp.211 0 +motor.cogging_dq_comp.212 0 +motor.cogging_dq_comp.213 0 +motor.cogging_dq_comp.214 0 +motor.cogging_dq_comp.215 0 +motor.cogging_dq_comp.216 0 +motor.cogging_dq_comp.217 0 +motor.cogging_dq_comp.218 0 +motor.cogging_dq_comp.219 0 +motor.cogging_dq_comp.220 0 +motor.cogging_dq_comp.221 0 +motor.cogging_dq_comp.222 0 +motor.cogging_dq_comp.223 0 +motor.cogging_dq_comp.224 0 +motor.cogging_dq_comp.225 0 +motor.cogging_dq_comp.226 0 +motor.cogging_dq_comp.227 0 +motor.cogging_dq_comp.228 0 +motor.cogging_dq_comp.229 0 +motor.cogging_dq_comp.230 0 +motor.cogging_dq_comp.231 0 +motor.cogging_dq_comp.232 0 +motor.cogging_dq_comp.233 0 +motor.cogging_dq_comp.234 0 +motor.cogging_dq_comp.235 0 +motor.cogging_dq_comp.236 0 +motor.cogging_dq_comp.237 0 +motor.cogging_dq_comp.238 0 +motor.cogging_dq_comp.239 0 +motor.cogging_dq_comp.240 0 +motor.cogging_dq_comp.241 0 +motor.cogging_dq_comp.242 0 +motor.cogging_dq_comp.243 0 +motor.cogging_dq_comp.244 0 +motor.cogging_dq_comp.245 0 +motor.cogging_dq_comp.246 0 +motor.cogging_dq_comp.247 0 +motor.cogging_dq_comp.248 0 +motor.cogging_dq_comp.249 0 +motor.cogging_dq_comp.250 0 +motor.cogging_dq_comp.251 0 +motor.cogging_dq_comp.252 0 +motor.cogging_dq_comp.253 0 +motor.cogging_dq_comp.254 0 +motor.cogging_dq_comp.255 0 +motor.cogging_dq_comp.256 0 +motor.cogging_dq_comp.257 0 +motor.cogging_dq_comp.258 0 +motor.cogging_dq_comp.259 0 +motor.cogging_dq_comp.260 0 +motor.cogging_dq_comp.261 0 +motor.cogging_dq_comp.262 0 +motor.cogging_dq_comp.263 0 +motor.cogging_dq_comp.264 0 +motor.cogging_dq_comp.265 0 +motor.cogging_dq_comp.266 0 +motor.cogging_dq_comp.267 0 +motor.cogging_dq_comp.268 0 +motor.cogging_dq_comp.269 0 +motor.cogging_dq_comp.270 0 +motor.cogging_dq_comp.271 0 +motor.cogging_dq_comp.272 0 +motor.cogging_dq_comp.273 0 +motor.cogging_dq_comp.274 0 +motor.cogging_dq_comp.275 0 +motor.cogging_dq_comp.276 0 +motor.cogging_dq_comp.277 0 +motor.cogging_dq_comp.278 0 +motor.cogging_dq_comp.279 0 +motor.cogging_dq_comp.280 0 +motor.cogging_dq_comp.281 0 +motor.cogging_dq_comp.282 0 +motor.cogging_dq_comp.283 0 +motor.cogging_dq_comp.284 0 +motor.cogging_dq_comp.285 0 +motor.cogging_dq_comp.286 0 +motor.cogging_dq_comp.287 0 +motor.cogging_dq_comp.288 0 +motor.cogging_dq_comp.289 0 +motor.cogging_dq_comp.290 0 +motor.cogging_dq_comp.291 0 +motor.cogging_dq_comp.292 0 +motor.cogging_dq_comp.293 0 +motor.cogging_dq_comp.294 0 +motor.cogging_dq_comp.295 0 +motor.cogging_dq_comp.296 0 +motor.cogging_dq_comp.297 0 +motor.cogging_dq_comp.298 0 +motor.cogging_dq_comp.299 0 +motor.cogging_dq_comp.300 0 +motor.cogging_dq_comp.301 0 +motor.cogging_dq_comp.302 0 +motor.cogging_dq_comp.303 0 +motor.cogging_dq_comp.304 0 +motor.cogging_dq_comp.305 0 +motor.cogging_dq_comp.306 0 +motor.cogging_dq_comp.307 0 +motor.cogging_dq_comp.308 0 +motor.cogging_dq_comp.309 0 +motor.cogging_dq_comp.310 0 +motor.cogging_dq_comp.311 0 +motor.cogging_dq_comp.312 0 +motor.cogging_dq_comp.313 0 +motor.cogging_dq_comp.314 0 +motor.cogging_dq_comp.315 0 +motor.cogging_dq_comp.316 0 +motor.cogging_dq_comp.317 0 +motor.cogging_dq_comp.318 0 +motor.cogging_dq_comp.319 0 +motor.cogging_dq_comp.320 0 +motor.cogging_dq_comp.321 0 +motor.cogging_dq_comp.322 0 +motor.cogging_dq_comp.323 0 +motor.cogging_dq_comp.324 0 +motor.cogging_dq_comp.325 0 +motor.cogging_dq_comp.326 0 +motor.cogging_dq_comp.327 0 +motor.cogging_dq_comp.328 0 +motor.cogging_dq_comp.329 0 +motor.cogging_dq_comp.330 0 +motor.cogging_dq_comp.331 0 +motor.cogging_dq_comp.332 0 +motor.cogging_dq_comp.333 0 +motor.cogging_dq_comp.334 0 +motor.cogging_dq_comp.335 0 +motor.cogging_dq_comp.336 0 +motor.cogging_dq_comp.337 0 +motor.cogging_dq_comp.338 0 +motor.cogging_dq_comp.339 0 +motor.cogging_dq_comp.340 0 +motor.cogging_dq_comp.341 0 +motor.cogging_dq_comp.342 0 +motor.cogging_dq_comp.343 0 +motor.cogging_dq_comp.344 0 +motor.cogging_dq_comp.345 0 +motor.cogging_dq_comp.346 0 +motor.cogging_dq_comp.347 0 +motor.cogging_dq_comp.348 0 +motor.cogging_dq_comp.349 0 +motor.cogging_dq_comp.350 0 +motor.cogging_dq_comp.351 0 +motor.cogging_dq_comp.352 0 +motor.cogging_dq_comp.353 0 +motor.cogging_dq_comp.354 0 +motor.cogging_dq_comp.355 0 +motor.cogging_dq_comp.356 0 +motor.cogging_dq_comp.357 0 +motor.cogging_dq_comp.358 0 +motor.cogging_dq_comp.359 0 +motor.cogging_dq_comp.360 0 +motor.cogging_dq_comp.361 0 +motor.cogging_dq_comp.362 0 +motor.cogging_dq_comp.363 0 +motor.cogging_dq_comp.364 0 +motor.cogging_dq_comp.365 0 +motor.cogging_dq_comp.366 0 +motor.cogging_dq_comp.367 0 +motor.cogging_dq_comp.368 0 +motor.cogging_dq_comp.369 0 +motor.cogging_dq_comp.370 0 +motor.cogging_dq_comp.371 0 +motor.cogging_dq_comp.372 0 +motor.cogging_dq_comp.373 0 +motor.cogging_dq_comp.374 0 +motor.cogging_dq_comp.375 0 +motor.cogging_dq_comp.376 0 +motor.cogging_dq_comp.377 0 +motor.cogging_dq_comp.378 0 +motor.cogging_dq_comp.379 0 +motor.cogging_dq_comp.380 0 +motor.cogging_dq_comp.381 0 +motor.cogging_dq_comp.382 0 +motor.cogging_dq_comp.383 0 +motor.cogging_dq_comp.384 0 +motor.cogging_dq_comp.385 0 +motor.cogging_dq_comp.386 0 +motor.cogging_dq_comp.387 0 +motor.cogging_dq_comp.388 0 +motor.cogging_dq_comp.389 0 +motor.cogging_dq_comp.390 0 +motor.cogging_dq_comp.391 0 +motor.cogging_dq_comp.392 0 +motor.cogging_dq_comp.393 0 +motor.cogging_dq_comp.394 0 +motor.cogging_dq_comp.395 0 +motor.cogging_dq_comp.396 0 +motor.cogging_dq_comp.397 0 +motor.cogging_dq_comp.398 0 +motor.cogging_dq_comp.399 0 +motor.cogging_dq_comp.400 0 +motor.cogging_dq_comp.401 0 +motor.cogging_dq_comp.402 0 +motor.cogging_dq_comp.403 0 +motor.cogging_dq_comp.404 0 +motor.cogging_dq_comp.405 0 +motor.cogging_dq_comp.406 0 +motor.cogging_dq_comp.407 0 +motor.cogging_dq_comp.408 0 +motor.cogging_dq_comp.409 0 +motor.cogging_dq_comp.410 0 +motor.cogging_dq_comp.411 0 +motor.cogging_dq_comp.412 0 +motor.cogging_dq_comp.413 0 +motor.cogging_dq_comp.414 0 +motor.cogging_dq_comp.415 0 +motor.cogging_dq_comp.416 0 +motor.cogging_dq_comp.417 0 +motor.cogging_dq_comp.418 0 +motor.cogging_dq_comp.419 0 +motor.cogging_dq_comp.420 0 +motor.cogging_dq_comp.421 0 +motor.cogging_dq_comp.422 0 +motor.cogging_dq_comp.423 0 +motor.cogging_dq_comp.424 0 +motor.cogging_dq_comp.425 0 +motor.cogging_dq_comp.426 0 +motor.cogging_dq_comp.427 0 +motor.cogging_dq_comp.428 0 +motor.cogging_dq_comp.429 0 +motor.cogging_dq_comp.430 0 +motor.cogging_dq_comp.431 0 +motor.cogging_dq_comp.432 0 +motor.cogging_dq_comp.433 0 +motor.cogging_dq_comp.434 0 +motor.cogging_dq_comp.435 0 +motor.cogging_dq_comp.436 0 +motor.cogging_dq_comp.437 0 +motor.cogging_dq_comp.438 0 +motor.cogging_dq_comp.439 0 +motor.cogging_dq_comp.440 0 +motor.cogging_dq_comp.441 0 +motor.cogging_dq_comp.442 0 +motor.cogging_dq_comp.443 0 +motor.cogging_dq_comp.444 0 +motor.cogging_dq_comp.445 0 +motor.cogging_dq_comp.446 0 +motor.cogging_dq_comp.447 0 +motor.cogging_dq_comp.448 0 +motor.cogging_dq_comp.449 0 +motor.cogging_dq_comp.450 0 +motor.cogging_dq_comp.451 0 +motor.cogging_dq_comp.452 0 +motor.cogging_dq_comp.453 0 +motor.cogging_dq_comp.454 0 +motor.cogging_dq_comp.455 0 +motor.cogging_dq_comp.456 0 +motor.cogging_dq_comp.457 0 +motor.cogging_dq_comp.458 0 +motor.cogging_dq_comp.459 0 +motor.cogging_dq_comp.460 0 +motor.cogging_dq_comp.461 0 +motor.cogging_dq_comp.462 0 +motor.cogging_dq_comp.463 0 +motor.cogging_dq_comp.464 0 +motor.cogging_dq_comp.465 0 +motor.cogging_dq_comp.466 0 +motor.cogging_dq_comp.467 0 +motor.cogging_dq_comp.468 0 +motor.cogging_dq_comp.469 0 +motor.cogging_dq_comp.470 0 +motor.cogging_dq_comp.471 0 +motor.cogging_dq_comp.472 0 +motor.cogging_dq_comp.473 0 +motor.cogging_dq_comp.474 0 +motor.cogging_dq_comp.475 0 +motor.cogging_dq_comp.476 0 +motor.cogging_dq_comp.477 0 +motor.cogging_dq_comp.478 0 +motor.cogging_dq_comp.479 0 +motor.cogging_dq_comp.480 0 +motor.cogging_dq_comp.481 0 +motor.cogging_dq_comp.482 0 +motor.cogging_dq_comp.483 0 +motor.cogging_dq_comp.484 0 +motor.cogging_dq_comp.485 0 +motor.cogging_dq_comp.486 0 +motor.cogging_dq_comp.487 0 +motor.cogging_dq_comp.488 0 +motor.cogging_dq_comp.489 0 +motor.cogging_dq_comp.490 0 +motor.cogging_dq_comp.491 0 +motor.cogging_dq_comp.492 0 +motor.cogging_dq_comp.493 0 +motor.cogging_dq_comp.494 0 +motor.cogging_dq_comp.495 0 +motor.cogging_dq_comp.496 0 +motor.cogging_dq_comp.497 0 +motor.cogging_dq_comp.498 0 +motor.cogging_dq_comp.499 0 +motor.cogging_dq_comp.500 0 +motor.cogging_dq_comp.501 0 +motor.cogging_dq_comp.502 0 +motor.cogging_dq_comp.503 0 +motor.cogging_dq_comp.504 0 +motor.cogging_dq_comp.505 0 +motor.cogging_dq_comp.506 0 +motor.cogging_dq_comp.507 0 +motor.cogging_dq_comp.508 0 +motor.cogging_dq_comp.509 0 +motor.cogging_dq_comp.510 0 +motor.cogging_dq_comp.511 0 +motor.cogging_dq_comp.512 0 +motor.cogging_dq_comp.513 0 +motor.cogging_dq_comp.514 0 +motor.cogging_dq_comp.515 0 +motor.cogging_dq_comp.516 0 +motor.cogging_dq_comp.517 0 +motor.cogging_dq_comp.518 0 +motor.cogging_dq_comp.519 0 +motor.cogging_dq_comp.520 0 +motor.cogging_dq_comp.521 0 +motor.cogging_dq_comp.522 0 +motor.cogging_dq_comp.523 0 +motor.cogging_dq_comp.524 0 +motor.cogging_dq_comp.525 0 +motor.cogging_dq_comp.526 0 +motor.cogging_dq_comp.527 0 +motor.cogging_dq_comp.528 0 +motor.cogging_dq_comp.529 0 +motor.cogging_dq_comp.530 0 +motor.cogging_dq_comp.531 0 +motor.cogging_dq_comp.532 0 +motor.cogging_dq_comp.533 0 +motor.cogging_dq_comp.534 0 +motor.cogging_dq_comp.535 0 +motor.cogging_dq_comp.536 0 +motor.cogging_dq_comp.537 0 +motor.cogging_dq_comp.538 0 +motor.cogging_dq_comp.539 0 +motor.cogging_dq_comp.540 0 +motor.cogging_dq_comp.541 0 +motor.cogging_dq_comp.542 0 +motor.cogging_dq_comp.543 0 +motor.cogging_dq_comp.544 0 +motor.cogging_dq_comp.545 0 +motor.cogging_dq_comp.546 0 +motor.cogging_dq_comp.547 0 +motor.cogging_dq_comp.548 0 +motor.cogging_dq_comp.549 0 +motor.cogging_dq_comp.550 0 +motor.cogging_dq_comp.551 0 +motor.cogging_dq_comp.552 0 +motor.cogging_dq_comp.553 0 +motor.cogging_dq_comp.554 0 +motor.cogging_dq_comp.555 0 +motor.cogging_dq_comp.556 0 +motor.cogging_dq_comp.557 0 +motor.cogging_dq_comp.558 0 +motor.cogging_dq_comp.559 0 +motor.cogging_dq_comp.560 0 +motor.cogging_dq_comp.561 0 +motor.cogging_dq_comp.562 0 +motor.cogging_dq_comp.563 0 +motor.cogging_dq_comp.564 0 +motor.cogging_dq_comp.565 0 +motor.cogging_dq_comp.566 0 +motor.cogging_dq_comp.567 0 +motor.cogging_dq_comp.568 0 +motor.cogging_dq_comp.569 0 +motor.cogging_dq_comp.570 0 +motor.cogging_dq_comp.571 0 +motor.cogging_dq_comp.572 0 +motor.cogging_dq_comp.573 0 +motor.cogging_dq_comp.574 0 +motor.cogging_dq_comp.575 0 +motor.cogging_dq_comp.576 0 +motor.cogging_dq_comp.577 0 +motor.cogging_dq_comp.578 0 +motor.cogging_dq_comp.579 0 +motor.cogging_dq_comp.580 0 +motor.cogging_dq_comp.581 0 +motor.cogging_dq_comp.582 0 +motor.cogging_dq_comp.583 0 +motor.cogging_dq_comp.584 0 +motor.cogging_dq_comp.585 0 +motor.cogging_dq_comp.586 0 +motor.cogging_dq_comp.587 0 +motor.cogging_dq_comp.588 0 +motor.cogging_dq_comp.589 0 +motor.cogging_dq_comp.590 0 +motor.cogging_dq_comp.591 0 +motor.cogging_dq_comp.592 0 +motor.cogging_dq_comp.593 0 +motor.cogging_dq_comp.594 0 +motor.cogging_dq_comp.595 0 +motor.cogging_dq_comp.596 0 +motor.cogging_dq_comp.597 0 +motor.cogging_dq_comp.598 0 +motor.cogging_dq_comp.599 0 +motor.cogging_dq_comp.600 0 +motor.cogging_dq_comp.601 0 +motor.cogging_dq_comp.602 0 +motor.cogging_dq_comp.603 0 +motor.cogging_dq_comp.604 0 +motor.cogging_dq_comp.605 0 +motor.cogging_dq_comp.606 0 +motor.cogging_dq_comp.607 0 +motor.cogging_dq_comp.608 0 +motor.cogging_dq_comp.609 0 +motor.cogging_dq_comp.610 0 +motor.cogging_dq_comp.611 0 +motor.cogging_dq_comp.612 0 +motor.cogging_dq_comp.613 0 +motor.cogging_dq_comp.614 0 +motor.cogging_dq_comp.615 0 +motor.cogging_dq_comp.616 0 +motor.cogging_dq_comp.617 0 +motor.cogging_dq_comp.618 0 +motor.cogging_dq_comp.619 0 +motor.cogging_dq_comp.620 0 +motor.cogging_dq_comp.621 0 +motor.cogging_dq_comp.622 0 +motor.cogging_dq_comp.623 0 +motor.cogging_dq_comp.624 0 +motor.cogging_dq_comp.625 0 +motor.cogging_dq_comp.626 0 +motor.cogging_dq_comp.627 0 +motor.cogging_dq_comp.628 0 +motor.cogging_dq_comp.629 0 +motor.cogging_dq_comp.630 0 +motor.cogging_dq_comp.631 0 +motor.cogging_dq_comp.632 0 +motor.cogging_dq_comp.633 0 +motor.cogging_dq_comp.634 0 +motor.cogging_dq_comp.635 0 +motor.cogging_dq_comp.636 0 +motor.cogging_dq_comp.637 0 +motor.cogging_dq_comp.638 0 +motor.cogging_dq_comp.639 0 +motor.cogging_dq_comp.640 0 +motor.cogging_dq_comp.641 0 +motor.cogging_dq_comp.642 0 +motor.cogging_dq_comp.643 0 +motor.cogging_dq_comp.644 0 +motor.cogging_dq_comp.645 0 +motor.cogging_dq_comp.646 0 +motor.cogging_dq_comp.647 0 +motor.cogging_dq_comp.648 0 +motor.cogging_dq_comp.649 0 +motor.cogging_dq_comp.650 0 +motor.cogging_dq_comp.651 0 +motor.cogging_dq_comp.652 0 +motor.cogging_dq_comp.653 0 +motor.cogging_dq_comp.654 0 +motor.cogging_dq_comp.655 0 +motor.cogging_dq_comp.656 0 +motor.cogging_dq_comp.657 0 +motor.cogging_dq_comp.658 0 +motor.cogging_dq_comp.659 0 +motor.cogging_dq_comp.660 0 +motor.cogging_dq_comp.661 0 +motor.cogging_dq_comp.662 0 +motor.cogging_dq_comp.663 0 +motor.cogging_dq_comp.664 0 +motor.cogging_dq_comp.665 0 +motor.cogging_dq_comp.666 0 +motor.cogging_dq_comp.667 0 +motor.cogging_dq_comp.668 0 +motor.cogging_dq_comp.669 0 +motor.cogging_dq_comp.670 0 +motor.cogging_dq_comp.671 0 +motor.cogging_dq_comp.672 0 +motor.cogging_dq_comp.673 0 +motor.cogging_dq_comp.674 0 +motor.cogging_dq_comp.675 0 +motor.cogging_dq_comp.676 0 +motor.cogging_dq_comp.677 0 +motor.cogging_dq_comp.678 0 +motor.cogging_dq_comp.679 0 +motor.cogging_dq_comp.680 0 +motor.cogging_dq_comp.681 0 +motor.cogging_dq_comp.682 0 +motor.cogging_dq_comp.683 0 +motor.cogging_dq_comp.684 0 +motor.cogging_dq_comp.685 0 +motor.cogging_dq_comp.686 0 +motor.cogging_dq_comp.687 0 +motor.cogging_dq_comp.688 0 +motor.cogging_dq_comp.689 0 +motor.cogging_dq_comp.690 0 +motor.cogging_dq_comp.691 0 +motor.cogging_dq_comp.692 0 +motor.cogging_dq_comp.693 0 +motor.cogging_dq_comp.694 0 +motor.cogging_dq_comp.695 0 +motor.cogging_dq_comp.696 0 +motor.cogging_dq_comp.697 0 +motor.cogging_dq_comp.698 0 +motor.cogging_dq_comp.699 0 +motor.cogging_dq_comp.700 0 +motor.cogging_dq_comp.701 0 +motor.cogging_dq_comp.702 0 +motor.cogging_dq_comp.703 0 +motor.cogging_dq_comp.704 0 +motor.cogging_dq_comp.705 0 +motor.cogging_dq_comp.706 0 +motor.cogging_dq_comp.707 0 +motor.cogging_dq_comp.708 0 +motor.cogging_dq_comp.709 0 +motor.cogging_dq_comp.710 0 +motor.cogging_dq_comp.711 0 +motor.cogging_dq_comp.712 0 +motor.cogging_dq_comp.713 0 +motor.cogging_dq_comp.714 0 +motor.cogging_dq_comp.715 0 +motor.cogging_dq_comp.716 0 +motor.cogging_dq_comp.717 0 +motor.cogging_dq_comp.718 0 +motor.cogging_dq_comp.719 0 +motor.cogging_dq_comp.720 0 +motor.cogging_dq_comp.721 0 +motor.cogging_dq_comp.722 0 +motor.cogging_dq_comp.723 0 +motor.cogging_dq_comp.724 0 +motor.cogging_dq_comp.725 0 +motor.cogging_dq_comp.726 0 +motor.cogging_dq_comp.727 0 +motor.cogging_dq_comp.728 0 +motor.cogging_dq_comp.729 0 +motor.cogging_dq_comp.730 0 +motor.cogging_dq_comp.731 0 +motor.cogging_dq_comp.732 0 +motor.cogging_dq_comp.733 0 +motor.cogging_dq_comp.734 0 +motor.cogging_dq_comp.735 0 +motor.cogging_dq_comp.736 0 +motor.cogging_dq_comp.737 0 +motor.cogging_dq_comp.738 0 +motor.cogging_dq_comp.739 0 +motor.cogging_dq_comp.740 0 +motor.cogging_dq_comp.741 0 +motor.cogging_dq_comp.742 0 +motor.cogging_dq_comp.743 0 +motor.cogging_dq_comp.744 0 +motor.cogging_dq_comp.745 0 +motor.cogging_dq_comp.746 0 +motor.cogging_dq_comp.747 0 +motor.cogging_dq_comp.748 0 +motor.cogging_dq_comp.749 0 +motor.cogging_dq_comp.750 0 +motor.cogging_dq_comp.751 0 +motor.cogging_dq_comp.752 0 +motor.cogging_dq_comp.753 0 +motor.cogging_dq_comp.754 0 +motor.cogging_dq_comp.755 0 +motor.cogging_dq_comp.756 0 +motor.cogging_dq_comp.757 0 +motor.cogging_dq_comp.758 0 +motor.cogging_dq_comp.759 0 +motor.cogging_dq_comp.760 0 +motor.cogging_dq_comp.761 0 +motor.cogging_dq_comp.762 0 +motor.cogging_dq_comp.763 0 +motor.cogging_dq_comp.764 0 +motor.cogging_dq_comp.765 0 +motor.cogging_dq_comp.766 0 +motor.cogging_dq_comp.767 0 +motor.cogging_dq_comp.768 0 +motor.cogging_dq_comp.769 0 +motor.cogging_dq_comp.770 0 +motor.cogging_dq_comp.771 0 +motor.cogging_dq_comp.772 0 +motor.cogging_dq_comp.773 0 +motor.cogging_dq_comp.774 0 +motor.cogging_dq_comp.775 0 +motor.cogging_dq_comp.776 0 +motor.cogging_dq_comp.777 0 +motor.cogging_dq_comp.778 0 +motor.cogging_dq_comp.779 0 +motor.cogging_dq_comp.780 0 +motor.cogging_dq_comp.781 0 +motor.cogging_dq_comp.782 0 +motor.cogging_dq_comp.783 0 +motor.cogging_dq_comp.784 0 +motor.cogging_dq_comp.785 0 +motor.cogging_dq_comp.786 0 +motor.cogging_dq_comp.787 0 +motor.cogging_dq_comp.788 0 +motor.cogging_dq_comp.789 0 +motor.cogging_dq_comp.790 0 +motor.cogging_dq_comp.791 0 +motor.cogging_dq_comp.792 0 +motor.cogging_dq_comp.793 0 +motor.cogging_dq_comp.794 0 +motor.cogging_dq_comp.795 0 +motor.cogging_dq_comp.796 0 +motor.cogging_dq_comp.797 0 +motor.cogging_dq_comp.798 0 +motor.cogging_dq_comp.799 0 +motor.cogging_dq_comp.800 0 +motor.cogging_dq_comp.801 0 +motor.cogging_dq_comp.802 0 +motor.cogging_dq_comp.803 0 +motor.cogging_dq_comp.804 0 +motor.cogging_dq_comp.805 0 +motor.cogging_dq_comp.806 0 +motor.cogging_dq_comp.807 0 +motor.cogging_dq_comp.808 0 +motor.cogging_dq_comp.809 0 +motor.cogging_dq_comp.810 0 +motor.cogging_dq_comp.811 0 +motor.cogging_dq_comp.812 0 +motor.cogging_dq_comp.813 0 +motor.cogging_dq_comp.814 0 +motor.cogging_dq_comp.815 0 +motor.cogging_dq_comp.816 0 +motor.cogging_dq_comp.817 0 +motor.cogging_dq_comp.818 0 +motor.cogging_dq_comp.819 0 +motor.cogging_dq_comp.820 0 +motor.cogging_dq_comp.821 0 +motor.cogging_dq_comp.822 0 +motor.cogging_dq_comp.823 0 +motor.cogging_dq_comp.824 0 +motor.cogging_dq_comp.825 0 +motor.cogging_dq_comp.826 0 +motor.cogging_dq_comp.827 0 +motor.cogging_dq_comp.828 0 +motor.cogging_dq_comp.829 0 +motor.cogging_dq_comp.830 0 +motor.cogging_dq_comp.831 0 +motor.cogging_dq_comp.832 0 +motor.cogging_dq_comp.833 0 +motor.cogging_dq_comp.834 0 +motor.cogging_dq_comp.835 0 +motor.cogging_dq_comp.836 0 +motor.cogging_dq_comp.837 0 +motor.cogging_dq_comp.838 0 +motor.cogging_dq_comp.839 0 +motor.cogging_dq_comp.840 0 +motor.cogging_dq_comp.841 0 +motor.cogging_dq_comp.842 0 +motor.cogging_dq_comp.843 0 +motor.cogging_dq_comp.844 0 +motor.cogging_dq_comp.845 0 +motor.cogging_dq_comp.846 0 +motor.cogging_dq_comp.847 0 +motor.cogging_dq_comp.848 0 +motor.cogging_dq_comp.849 0 +motor.cogging_dq_comp.850 0 +motor.cogging_dq_comp.851 0 +motor.cogging_dq_comp.852 0 +motor.cogging_dq_comp.853 0 +motor.cogging_dq_comp.854 0 +motor.cogging_dq_comp.855 0 +motor.cogging_dq_comp.856 0 +motor.cogging_dq_comp.857 0 +motor.cogging_dq_comp.858 0 +motor.cogging_dq_comp.859 0 +motor.cogging_dq_comp.860 0 +motor.cogging_dq_comp.861 0 +motor.cogging_dq_comp.862 0 +motor.cogging_dq_comp.863 0 +motor.cogging_dq_comp.864 0 +motor.cogging_dq_comp.865 0 +motor.cogging_dq_comp.866 0 +motor.cogging_dq_comp.867 0 +motor.cogging_dq_comp.868 0 +motor.cogging_dq_comp.869 0 +motor.cogging_dq_comp.870 0 +motor.cogging_dq_comp.871 0 +motor.cogging_dq_comp.872 0 +motor.cogging_dq_comp.873 0 +motor.cogging_dq_comp.874 0 +motor.cogging_dq_comp.875 0 +motor.cogging_dq_comp.876 0 +motor.cogging_dq_comp.877 0 +motor.cogging_dq_comp.878 0 +motor.cogging_dq_comp.879 0 +motor.cogging_dq_comp.880 0 +motor.cogging_dq_comp.881 0 +motor.cogging_dq_comp.882 0 +motor.cogging_dq_comp.883 0 +motor.cogging_dq_comp.884 0 +motor.cogging_dq_comp.885 0 +motor.cogging_dq_comp.886 0 +motor.cogging_dq_comp.887 0 +motor.cogging_dq_comp.888 0 +motor.cogging_dq_comp.889 0 +motor.cogging_dq_comp.890 0 +motor.cogging_dq_comp.891 0 +motor.cogging_dq_comp.892 0 +motor.cogging_dq_comp.893 0 +motor.cogging_dq_comp.894 0 +motor.cogging_dq_comp.895 0 +motor.cogging_dq_comp.896 0 +motor.cogging_dq_comp.897 0 +motor.cogging_dq_comp.898 0 +motor.cogging_dq_comp.899 0 +motor.cogging_dq_comp.900 0 +motor.cogging_dq_comp.901 0 +motor.cogging_dq_comp.902 0 +motor.cogging_dq_comp.903 0 +motor.cogging_dq_comp.904 0 +motor.cogging_dq_comp.905 0 +motor.cogging_dq_comp.906 0 +motor.cogging_dq_comp.907 0 +motor.cogging_dq_comp.908 0 +motor.cogging_dq_comp.909 0 +motor.cogging_dq_comp.910 0 +motor.cogging_dq_comp.911 0 +motor.cogging_dq_comp.912 0 +motor.cogging_dq_comp.913 0 +motor.cogging_dq_comp.914 0 +motor.cogging_dq_comp.915 0 +motor.cogging_dq_comp.916 0 +motor.cogging_dq_comp.917 0 +motor.cogging_dq_comp.918 0 +motor.cogging_dq_comp.919 0 +motor.cogging_dq_comp.920 0 +motor.cogging_dq_comp.921 0 +motor.cogging_dq_comp.922 0 +motor.cogging_dq_comp.923 0 +motor.cogging_dq_comp.924 0 +motor.cogging_dq_comp.925 0 +motor.cogging_dq_comp.926 0 +motor.cogging_dq_comp.927 0 +motor.cogging_dq_comp.928 0 +motor.cogging_dq_comp.929 0 +motor.cogging_dq_comp.930 0 +motor.cogging_dq_comp.931 0 +motor.cogging_dq_comp.932 0 +motor.cogging_dq_comp.933 0 +motor.cogging_dq_comp.934 0 +motor.cogging_dq_comp.935 0 +motor.cogging_dq_comp.936 0 +motor.cogging_dq_comp.937 0 +motor.cogging_dq_comp.938 0 +motor.cogging_dq_comp.939 0 +motor.cogging_dq_comp.940 0 +motor.cogging_dq_comp.941 0 +motor.cogging_dq_comp.942 0 +motor.cogging_dq_comp.943 0 +motor.cogging_dq_comp.944 0 +motor.cogging_dq_comp.945 0 +motor.cogging_dq_comp.946 0 +motor.cogging_dq_comp.947 0 +motor.cogging_dq_comp.948 0 +motor.cogging_dq_comp.949 0 +motor.cogging_dq_comp.950 0 +motor.cogging_dq_comp.951 0 +motor.cogging_dq_comp.952 0 +motor.cogging_dq_comp.953 0 +motor.cogging_dq_comp.954 0 +motor.cogging_dq_comp.955 0 +motor.cogging_dq_comp.956 0 +motor.cogging_dq_comp.957 0 +motor.cogging_dq_comp.958 0 +motor.cogging_dq_comp.959 0 +motor.cogging_dq_comp.960 0 +motor.cogging_dq_comp.961 0 +motor.cogging_dq_comp.962 0 +motor.cogging_dq_comp.963 0 +motor.cogging_dq_comp.964 0 +motor.cogging_dq_comp.965 0 +motor.cogging_dq_comp.966 0 +motor.cogging_dq_comp.967 0 +motor.cogging_dq_comp.968 0 +motor.cogging_dq_comp.969 0 +motor.cogging_dq_comp.970 0 +motor.cogging_dq_comp.971 0 +motor.cogging_dq_comp.972 0 +motor.cogging_dq_comp.973 0 +motor.cogging_dq_comp.974 0 +motor.cogging_dq_comp.975 0 +motor.cogging_dq_comp.976 0 +motor.cogging_dq_comp.977 0 +motor.cogging_dq_comp.978 0 +motor.cogging_dq_comp.979 0 +motor.cogging_dq_comp.980 0 +motor.cogging_dq_comp.981 0 +motor.cogging_dq_comp.982 0 +motor.cogging_dq_comp.983 0 +motor.cogging_dq_comp.984 0 +motor.cogging_dq_comp.985 0 +motor.cogging_dq_comp.986 0 +motor.cogging_dq_comp.987 0 +motor.cogging_dq_comp.988 0 +motor.cogging_dq_comp.989 0 +motor.cogging_dq_comp.990 0 +motor.cogging_dq_comp.991 0 +motor.cogging_dq_comp.992 0 +motor.cogging_dq_comp.993 0 +motor.cogging_dq_comp.994 0 +motor.cogging_dq_comp.995 0 +motor.cogging_dq_comp.996 0 +motor.cogging_dq_comp.997 0 +motor.cogging_dq_comp.998 0 +motor.cogging_dq_comp.999 0 +motor.cogging_dq_comp.1000 0 +motor.cogging_dq_comp.1001 0 +motor.cogging_dq_comp.1002 0 +motor.cogging_dq_comp.1003 0 +motor.cogging_dq_comp.1004 0 +motor.cogging_dq_comp.1005 0 +motor.cogging_dq_comp.1006 0 +motor.cogging_dq_comp.1007 0 +motor.cogging_dq_comp.1008 0 +motor.cogging_dq_comp.1009 0 +motor.cogging_dq_comp.1010 0 +motor.cogging_dq_comp.1011 0 +motor.cogging_dq_comp.1012 0 +motor.cogging_dq_comp.1013 0 +motor.cogging_dq_comp.1014 0 +motor.cogging_dq_comp.1015 0 +motor.cogging_dq_comp.1016 0 +motor.cogging_dq_comp.1017 0 +motor.cogging_dq_comp.1018 0 +motor.cogging_dq_comp.1019 0 +motor.cogging_dq_comp.1020 0 +motor.cogging_dq_comp.1021 0 +motor.cogging_dq_comp.1022 0 +motor.cogging_dq_comp.1023 0 +drv8323_conf.dis_cpuv 0 +drv8323_conf.dis_gdf 0 +drv8323_conf.otw_rep 0 +drv8323_conf.pwm_mode 1 +drv8323_conf.pwm_1x_asynchronous 0 +drv8323_conf.pwm_1x_dir 0 +drv8323_conf.idrivep_hs_ma 150 +drv8323_conf.idriven_hs_ma 300 +drv8323_conf.cbc 1 +drv8323_conf.tdrive_ns 1000 +drv8323_conf.idrivep_ls_ma 150 +drv8323_conf.idriven_ls_ma 300 +drv8323_conf.tretry 0 +drv8323_conf.dead_time_ns 50 +drv8323_conf.ocp_mode 0 +drv8323_conf.ocp_deg_us 4 +drv8323_conf.vds_lvl_mv 700 +drv8323_conf.csa_fet 0 +drv8323_conf.vref_div 1 +drv8323_conf.ls_ref 0 +drv8323_conf.csa_gain 20 +drv8323_conf.dis_sen 0 +drv8323_conf.sen_lvl_mv 500 +servo.pwm_rate_hz 30000 +servo.i_gain 20.000000 +servo.current_sense_ohm 0.000500 +servo.pwm_comp_off 0.027000 +servo.pwm_comp_mag 0.005000 +servo.pwm_scale 1.000000 +servo.max_voltage 56.000000 +servo.max_power_W 450.000000 +servo.derate_temperature 50.000000 +servo.fault_temperature 75.000000 +servo.enable_motor_temperature 0 +servo.motor_derate_temperature 50.000000 +servo.motor_fault_temperature nan +servo.velocity_threshold 0.000000 +servo.position_derate 0.020000 +servo.adc_cur_cycles 2 +servo.adc_aux_cycles 47 +servo.pid_dq.kp 0.060606 +servo.pid_dq.ki 102.321388 +servo.pid_position.kp 800.000000 +servo.pid_position.ki 0.000000 +servo.pid_position.kd 50.000000 +servo.pid_position.iratelimit -1.000000 +servo.pid_position.ilimit 0.000000 +servo.pid_position.max_desired_rate 0.000000 +servo.pid_position.sign -1 +servo.current_feedforward 1.000000 +servo.bemf_feedforward 1.000000 +servo.default_velocity_limit 0.080000 +servo.default_accel_limit 0.200000 +servo.voltage_mode_control 0 +servo.fixed_voltage_mode 0 +servo.fixed_voltage_control_V 0.000000 +servo.max_position_slip 0.100000 +servo.default_timeout_s 0.100000 +servo.timeout_max_torque_Nm 5.000000 +servo.timeout_mode 12 +servo.flux_brake_min_voltage 53.000000 +servo.flux_brake_resistance_ohm 0.025000 +servo.max_current_A 100.000000 +servo.derate_current_A -20.000000 +servo.max_velocity 0.100000 +servo.max_velocity_derate 2.000000 +servo.cooldown_cycles 256 +servo.velocity_zero_capture_threshold 0.050000 +servo.timing_fault 0 +servo.emit_debug 0 +servopos.position_min nan +servopos.position_max nan +id.id 34 +can.prefix 0 + diff --git a/config/moteus/joint_de_0.cfg b/config/moteus/joint_de_0.cfg new file mode 100644 index 000000000..c116946d8 --- /dev/null +++ b/config/moteus/joint_de_0.cfg @@ -0,0 +1,1399 @@ +uuid.uuid.0 174 +uuid.uuid.1 214 +uuid.uuid.2 243 +uuid.uuid.3 248 +uuid.uuid.4 175 +uuid.uuid.5 138 +uuid.uuid.6 65 +uuid.uuid.7 14 +uuid.uuid.8 153 +uuid.uuid.9 120 +uuid.uuid.10 43 +uuid.uuid.11 164 +uuid.uuid.12 127 +uuid.uuid.13 181 +uuid.uuid.14 167 +uuid.uuid.15 144 +clock.hsitrim 64 +aux1.i2c.i2c_hz 400000 +aux1.i2c.i2c_mode 1 +aux1.i2c.pullup 1 +aux1.i2c.devices.0.type 1 +aux1.i2c.devices.0.address 64 +aux1.i2c.devices.0.poll_rate_us 1000 +aux1.i2c.devices.1.type 0 +aux1.i2c.devices.1.address 64 +aux1.i2c.devices.1.poll_rate_us 1000 +aux1.i2c.devices.2.type 0 +aux1.i2c.devices.2.address 64 +aux1.i2c.devices.2.poll_rate_us 1000 +aux1.spi.mode 1 +aux1.spi.rate_hz 12000000 +aux1.spi.filter_us 64 +aux1.spi.bct 0 +aux1.uart.mode 0 +aux1.uart.baud_rate 115200 +aux1.uart.poll_rate_us 100 +aux1.uart.rs422 0 +aux1.uart.cui_amt21_address 84 +aux1.quadrature.enabled 0 +aux1.quadrature.cpr 16384 +aux1.hall.enabled 0 +aux1.hall.polarity 0 +aux1.index.enabled 0 +aux1.sine_cosine.enabled 0 +aux1.sine_cosine.common 1700 +aux1.i2c_startup_delay_ms 30 +aux1.pins.0.mode 0 +aux1.pins.0.pull 0 +aux1.pins.1.mode 0 +aux1.pins.1.pull 0 +aux1.pins.2.mode 0 +aux1.pins.2.pull 0 +aux1.pins.3.mode 13 +aux1.pins.3.pull 1 +aux1.pins.4.mode 13 +aux1.pins.4.pull 1 +aux2.i2c.i2c_hz 400000 +aux2.i2c.i2c_mode 1 +aux2.i2c.pullup 0 +aux2.i2c.devices.0.type 0 +aux2.i2c.devices.0.address 64 +aux2.i2c.devices.0.poll_rate_us 1000 +aux2.i2c.devices.1.type 0 +aux2.i2c.devices.1.address 64 +aux2.i2c.devices.1.poll_rate_us 1000 +aux2.i2c.devices.2.type 0 +aux2.i2c.devices.2.address 64 +aux2.i2c.devices.2.poll_rate_us 1000 +aux2.spi.mode 1 +aux2.spi.rate_hz 12000000 +aux2.spi.filter_us 64 +aux2.spi.bct 0 +aux2.uart.mode 0 +aux2.uart.baud_rate 115200 +aux2.uart.poll_rate_us 100 +aux2.uart.rs422 0 +aux2.uart.cui_amt21_address 84 +aux2.quadrature.enabled 0 +aux2.quadrature.cpr 16384 +aux2.hall.enabled 1 +aux2.hall.polarity 0 +aux2.index.enabled 0 +aux2.sine_cosine.enabled 0 +aux2.sine_cosine.common 1700 +aux2.i2c_startup_delay_ms 30 +aux2.pins.0.mode 6 +aux2.pins.0.pull 1 +aux2.pins.1.mode 6 +aux2.pins.1.pull 1 +aux2.pins.2.mode 6 +aux2.pins.2.pull 1 +aux2.pins.3.mode 0 +aux2.pins.3.pull 0 +aux2.pins.4.mode 0 +aux2.pins.4.pull 0 +motor_position.sources.0.aux_number 2 +motor_position.sources.0.type 4 +motor_position.sources.0.i2c_device 0 +motor_position.sources.0.incremental_index -1 +motor_position.sources.0.cpr 48 +motor_position.sources.0.offset -3.000000 +motor_position.sources.0.sign -1 +motor_position.sources.0.debug_override -1 +motor_position.sources.0.reference 0 +motor_position.sources.0.pll_filter_hz 100.000000 +motor_position.sources.0.compensation_table.0 0.000000 +motor_position.sources.0.compensation_table.1 0.000000 +motor_position.sources.0.compensation_table.2 0.000000 +motor_position.sources.0.compensation_table.3 0.000000 +motor_position.sources.0.compensation_table.4 0.000000 +motor_position.sources.0.compensation_table.5 0.000000 +motor_position.sources.0.compensation_table.6 0.000000 +motor_position.sources.0.compensation_table.7 0.000000 +motor_position.sources.0.compensation_table.8 0.000000 +motor_position.sources.0.compensation_table.9 0.000000 +motor_position.sources.0.compensation_table.10 0.000000 +motor_position.sources.0.compensation_table.11 0.000000 +motor_position.sources.0.compensation_table.12 0.000000 +motor_position.sources.0.compensation_table.13 0.000000 +motor_position.sources.0.compensation_table.14 0.000000 +motor_position.sources.0.compensation_table.15 0.000000 +motor_position.sources.0.compensation_table.16 0.000000 +motor_position.sources.0.compensation_table.17 0.000000 +motor_position.sources.0.compensation_table.18 0.000000 +motor_position.sources.0.compensation_table.19 0.000000 +motor_position.sources.0.compensation_table.20 0.000000 +motor_position.sources.0.compensation_table.21 0.000000 +motor_position.sources.0.compensation_table.22 0.000000 +motor_position.sources.0.compensation_table.23 0.000000 +motor_position.sources.0.compensation_table.24 0.000000 +motor_position.sources.0.compensation_table.25 0.000000 +motor_position.sources.0.compensation_table.26 0.000000 +motor_position.sources.0.compensation_table.27 0.000000 +motor_position.sources.0.compensation_table.28 0.000000 +motor_position.sources.0.compensation_table.29 0.000000 +motor_position.sources.0.compensation_table.30 0.000000 +motor_position.sources.0.compensation_table.31 0.000000 +motor_position.sources.1.aux_number 1 +motor_position.sources.1.type 7 +motor_position.sources.1.i2c_device 0 +motor_position.sources.1.incremental_index -1 +motor_position.sources.1.cpr 65536 +motor_position.sources.1.offset -34783.000000 +motor_position.sources.1.sign 1 +motor_position.sources.1.debug_override -1 +motor_position.sources.1.reference 0 +motor_position.sources.1.pll_filter_hz 100.000000 +motor_position.sources.1.compensation_table.0 0.000000 +motor_position.sources.1.compensation_table.1 0.000000 +motor_position.sources.1.compensation_table.2 0.000000 +motor_position.sources.1.compensation_table.3 0.000000 +motor_position.sources.1.compensation_table.4 0.000000 +motor_position.sources.1.compensation_table.5 0.000000 +motor_position.sources.1.compensation_table.6 0.000000 +motor_position.sources.1.compensation_table.7 0.000000 +motor_position.sources.1.compensation_table.8 0.000000 +motor_position.sources.1.compensation_table.9 0.000000 +motor_position.sources.1.compensation_table.10 0.000000 +motor_position.sources.1.compensation_table.11 0.000000 +motor_position.sources.1.compensation_table.12 0.000000 +motor_position.sources.1.compensation_table.13 0.000000 +motor_position.sources.1.compensation_table.14 0.000000 +motor_position.sources.1.compensation_table.15 0.000000 +motor_position.sources.1.compensation_table.16 0.000000 +motor_position.sources.1.compensation_table.17 0.000000 +motor_position.sources.1.compensation_table.18 0.000000 +motor_position.sources.1.compensation_table.19 0.000000 +motor_position.sources.1.compensation_table.20 0.000000 +motor_position.sources.1.compensation_table.21 0.000000 +motor_position.sources.1.compensation_table.22 0.000000 +motor_position.sources.1.compensation_table.23 0.000000 +motor_position.sources.1.compensation_table.24 0.000000 +motor_position.sources.1.compensation_table.25 0.000000 +motor_position.sources.1.compensation_table.26 0.000000 +motor_position.sources.1.compensation_table.27 0.000000 +motor_position.sources.1.compensation_table.28 0.000000 +motor_position.sources.1.compensation_table.29 0.000000 +motor_position.sources.1.compensation_table.30 0.000000 +motor_position.sources.1.compensation_table.31 0.000000 +motor_position.sources.2.aux_number 1 +motor_position.sources.2.type 0 +motor_position.sources.2.i2c_device 0 +motor_position.sources.2.incremental_index -1 +motor_position.sources.2.cpr 16384 +motor_position.sources.2.offset 0.000000 +motor_position.sources.2.sign 1 +motor_position.sources.2.debug_override -1 +motor_position.sources.2.reference 0 +motor_position.sources.2.pll_filter_hz 400.000000 +motor_position.sources.2.compensation_table.0 0.000000 +motor_position.sources.2.compensation_table.1 0.000000 +motor_position.sources.2.compensation_table.2 0.000000 +motor_position.sources.2.compensation_table.3 0.000000 +motor_position.sources.2.compensation_table.4 0.000000 +motor_position.sources.2.compensation_table.5 0.000000 +motor_position.sources.2.compensation_table.6 0.000000 +motor_position.sources.2.compensation_table.7 0.000000 +motor_position.sources.2.compensation_table.8 0.000000 +motor_position.sources.2.compensation_table.9 0.000000 +motor_position.sources.2.compensation_table.10 0.000000 +motor_position.sources.2.compensation_table.11 0.000000 +motor_position.sources.2.compensation_table.12 0.000000 +motor_position.sources.2.compensation_table.13 0.000000 +motor_position.sources.2.compensation_table.14 0.000000 +motor_position.sources.2.compensation_table.15 0.000000 +motor_position.sources.2.compensation_table.16 0.000000 +motor_position.sources.2.compensation_table.17 0.000000 +motor_position.sources.2.compensation_table.18 0.000000 +motor_position.sources.2.compensation_table.19 0.000000 +motor_position.sources.2.compensation_table.20 0.000000 +motor_position.sources.2.compensation_table.21 0.000000 +motor_position.sources.2.compensation_table.22 0.000000 +motor_position.sources.2.compensation_table.23 0.000000 +motor_position.sources.2.compensation_table.24 0.000000 +motor_position.sources.2.compensation_table.25 0.000000 +motor_position.sources.2.compensation_table.26 0.000000 +motor_position.sources.2.compensation_table.27 0.000000 +motor_position.sources.2.compensation_table.28 0.000000 +motor_position.sources.2.compensation_table.29 0.000000 +motor_position.sources.2.compensation_table.30 0.000000 +motor_position.sources.2.compensation_table.31 0.000000 +motor_position.commutation_source 0 +motor_position.output.source 0 +motor_position.output.offset 0.000000 +motor_position.output.sign 1 +motor_position.output.reference_source -1 +motor_position.rotor_to_output_ratio 1.000000 +motor.poles 16 +motor.phase_invert 0 +motor.resistance_ohm 3.711431 +motor.v_per_hz 0.576525 +motor.offset.0 0.000000 +motor.offset.1 0.000000 +motor.offset.2 0.000000 +motor.offset.3 0.000000 +motor.offset.4 0.000000 +motor.offset.5 0.000000 +motor.offset.6 0.000000 +motor.offset.7 0.000000 +motor.offset.8 0.000000 +motor.offset.9 0.000000 +motor.offset.10 0.000000 +motor.offset.11 0.000000 +motor.offset.12 0.000000 +motor.offset.13 0.000000 +motor.offset.14 0.000000 +motor.offset.15 0.000000 +motor.offset.16 0.000000 +motor.offset.17 0.000000 +motor.offset.18 0.000000 +motor.offset.19 0.000000 +motor.offset.20 0.000000 +motor.offset.21 0.000000 +motor.offset.22 0.000000 +motor.offset.23 0.000000 +motor.offset.24 0.000000 +motor.offset.25 0.000000 +motor.offset.26 0.000000 +motor.offset.27 0.000000 +motor.offset.28 0.000000 +motor.offset.29 0.000000 +motor.offset.30 0.000000 +motor.offset.31 0.000000 +motor.offset.32 0.000000 +motor.offset.33 0.000000 +motor.offset.34 0.000000 +motor.offset.35 0.000000 +motor.offset.36 0.000000 +motor.offset.37 0.000000 +motor.offset.38 0.000000 +motor.offset.39 0.000000 +motor.offset.40 0.000000 +motor.offset.41 0.000000 +motor.offset.42 0.000000 +motor.offset.43 0.000000 +motor.offset.44 0.000000 +motor.offset.45 0.000000 +motor.offset.46 0.000000 +motor.offset.47 0.000000 +motor.offset.48 0.000000 +motor.offset.49 0.000000 +motor.offset.50 0.000000 +motor.offset.51 0.000000 +motor.offset.52 0.000000 +motor.offset.53 0.000000 +motor.offset.54 0.000000 +motor.offset.55 0.000000 +motor.offset.56 0.000000 +motor.offset.57 0.000000 +motor.offset.58 0.000000 +motor.offset.59 0.000000 +motor.offset.60 0.000000 +motor.offset.61 0.000000 +motor.offset.62 0.000000 +motor.offset.63 0.000000 +motor.rotation_current_cutoff_A 10000.000000 +motor.rotation_current_scale 0.050000 +motor.rotation_torque_scale 14.700000 +motor.cogging_dq_scale 0.000000 +motor.cogging_dq_comp.0 0 +motor.cogging_dq_comp.1 0 +motor.cogging_dq_comp.2 0 +motor.cogging_dq_comp.3 0 +motor.cogging_dq_comp.4 0 +motor.cogging_dq_comp.5 0 +motor.cogging_dq_comp.6 0 +motor.cogging_dq_comp.7 0 +motor.cogging_dq_comp.8 0 +motor.cogging_dq_comp.9 0 +motor.cogging_dq_comp.10 0 +motor.cogging_dq_comp.11 0 +motor.cogging_dq_comp.12 0 +motor.cogging_dq_comp.13 0 +motor.cogging_dq_comp.14 0 +motor.cogging_dq_comp.15 0 +motor.cogging_dq_comp.16 0 +motor.cogging_dq_comp.17 0 +motor.cogging_dq_comp.18 0 +motor.cogging_dq_comp.19 0 +motor.cogging_dq_comp.20 0 +motor.cogging_dq_comp.21 0 +motor.cogging_dq_comp.22 0 +motor.cogging_dq_comp.23 0 +motor.cogging_dq_comp.24 0 +motor.cogging_dq_comp.25 0 +motor.cogging_dq_comp.26 0 +motor.cogging_dq_comp.27 0 +motor.cogging_dq_comp.28 0 +motor.cogging_dq_comp.29 0 +motor.cogging_dq_comp.30 0 +motor.cogging_dq_comp.31 0 +motor.cogging_dq_comp.32 0 +motor.cogging_dq_comp.33 0 +motor.cogging_dq_comp.34 0 +motor.cogging_dq_comp.35 0 +motor.cogging_dq_comp.36 0 +motor.cogging_dq_comp.37 0 +motor.cogging_dq_comp.38 0 +motor.cogging_dq_comp.39 0 +motor.cogging_dq_comp.40 0 +motor.cogging_dq_comp.41 0 +motor.cogging_dq_comp.42 0 +motor.cogging_dq_comp.43 0 +motor.cogging_dq_comp.44 0 +motor.cogging_dq_comp.45 0 +motor.cogging_dq_comp.46 0 +motor.cogging_dq_comp.47 0 +motor.cogging_dq_comp.48 0 +motor.cogging_dq_comp.49 0 +motor.cogging_dq_comp.50 0 +motor.cogging_dq_comp.51 0 +motor.cogging_dq_comp.52 0 +motor.cogging_dq_comp.53 0 +motor.cogging_dq_comp.54 0 +motor.cogging_dq_comp.55 0 +motor.cogging_dq_comp.56 0 +motor.cogging_dq_comp.57 0 +motor.cogging_dq_comp.58 0 +motor.cogging_dq_comp.59 0 +motor.cogging_dq_comp.60 0 +motor.cogging_dq_comp.61 0 +motor.cogging_dq_comp.62 0 +motor.cogging_dq_comp.63 0 +motor.cogging_dq_comp.64 0 +motor.cogging_dq_comp.65 0 +motor.cogging_dq_comp.66 0 +motor.cogging_dq_comp.67 0 +motor.cogging_dq_comp.68 0 +motor.cogging_dq_comp.69 0 +motor.cogging_dq_comp.70 0 +motor.cogging_dq_comp.71 0 +motor.cogging_dq_comp.72 0 +motor.cogging_dq_comp.73 0 +motor.cogging_dq_comp.74 0 +motor.cogging_dq_comp.75 0 +motor.cogging_dq_comp.76 0 +motor.cogging_dq_comp.77 0 +motor.cogging_dq_comp.78 0 +motor.cogging_dq_comp.79 0 +motor.cogging_dq_comp.80 0 +motor.cogging_dq_comp.81 0 +motor.cogging_dq_comp.82 0 +motor.cogging_dq_comp.83 0 +motor.cogging_dq_comp.84 0 +motor.cogging_dq_comp.85 0 +motor.cogging_dq_comp.86 0 +motor.cogging_dq_comp.87 0 +motor.cogging_dq_comp.88 0 +motor.cogging_dq_comp.89 0 +motor.cogging_dq_comp.90 0 +motor.cogging_dq_comp.91 0 +motor.cogging_dq_comp.92 0 +motor.cogging_dq_comp.93 0 +motor.cogging_dq_comp.94 0 +motor.cogging_dq_comp.95 0 +motor.cogging_dq_comp.96 0 +motor.cogging_dq_comp.97 0 +motor.cogging_dq_comp.98 0 +motor.cogging_dq_comp.99 0 +motor.cogging_dq_comp.100 0 +motor.cogging_dq_comp.101 0 +motor.cogging_dq_comp.102 0 +motor.cogging_dq_comp.103 0 +motor.cogging_dq_comp.104 0 +motor.cogging_dq_comp.105 0 +motor.cogging_dq_comp.106 0 +motor.cogging_dq_comp.107 0 +motor.cogging_dq_comp.108 0 +motor.cogging_dq_comp.109 0 +motor.cogging_dq_comp.110 0 +motor.cogging_dq_comp.111 0 +motor.cogging_dq_comp.112 0 +motor.cogging_dq_comp.113 0 +motor.cogging_dq_comp.114 0 +motor.cogging_dq_comp.115 0 +motor.cogging_dq_comp.116 0 +motor.cogging_dq_comp.117 0 +motor.cogging_dq_comp.118 0 +motor.cogging_dq_comp.119 0 +motor.cogging_dq_comp.120 0 +motor.cogging_dq_comp.121 0 +motor.cogging_dq_comp.122 0 +motor.cogging_dq_comp.123 0 +motor.cogging_dq_comp.124 0 +motor.cogging_dq_comp.125 0 +motor.cogging_dq_comp.126 0 +motor.cogging_dq_comp.127 0 +motor.cogging_dq_comp.128 0 +motor.cogging_dq_comp.129 0 +motor.cogging_dq_comp.130 0 +motor.cogging_dq_comp.131 0 +motor.cogging_dq_comp.132 0 +motor.cogging_dq_comp.133 0 +motor.cogging_dq_comp.134 0 +motor.cogging_dq_comp.135 0 +motor.cogging_dq_comp.136 0 +motor.cogging_dq_comp.137 0 +motor.cogging_dq_comp.138 0 +motor.cogging_dq_comp.139 0 +motor.cogging_dq_comp.140 0 +motor.cogging_dq_comp.141 0 +motor.cogging_dq_comp.142 0 +motor.cogging_dq_comp.143 0 +motor.cogging_dq_comp.144 0 +motor.cogging_dq_comp.145 0 +motor.cogging_dq_comp.146 0 +motor.cogging_dq_comp.147 0 +motor.cogging_dq_comp.148 0 +motor.cogging_dq_comp.149 0 +motor.cogging_dq_comp.150 0 +motor.cogging_dq_comp.151 0 +motor.cogging_dq_comp.152 0 +motor.cogging_dq_comp.153 0 +motor.cogging_dq_comp.154 0 +motor.cogging_dq_comp.155 0 +motor.cogging_dq_comp.156 0 +motor.cogging_dq_comp.157 0 +motor.cogging_dq_comp.158 0 +motor.cogging_dq_comp.159 0 +motor.cogging_dq_comp.160 0 +motor.cogging_dq_comp.161 0 +motor.cogging_dq_comp.162 0 +motor.cogging_dq_comp.163 0 +motor.cogging_dq_comp.164 0 +motor.cogging_dq_comp.165 0 +motor.cogging_dq_comp.166 0 +motor.cogging_dq_comp.167 0 +motor.cogging_dq_comp.168 0 +motor.cogging_dq_comp.169 0 +motor.cogging_dq_comp.170 0 +motor.cogging_dq_comp.171 0 +motor.cogging_dq_comp.172 0 +motor.cogging_dq_comp.173 0 +motor.cogging_dq_comp.174 0 +motor.cogging_dq_comp.175 0 +motor.cogging_dq_comp.176 0 +motor.cogging_dq_comp.177 0 +motor.cogging_dq_comp.178 0 +motor.cogging_dq_comp.179 0 +motor.cogging_dq_comp.180 0 +motor.cogging_dq_comp.181 0 +motor.cogging_dq_comp.182 0 +motor.cogging_dq_comp.183 0 +motor.cogging_dq_comp.184 0 +motor.cogging_dq_comp.185 0 +motor.cogging_dq_comp.186 0 +motor.cogging_dq_comp.187 0 +motor.cogging_dq_comp.188 0 +motor.cogging_dq_comp.189 0 +motor.cogging_dq_comp.190 0 +motor.cogging_dq_comp.191 0 +motor.cogging_dq_comp.192 0 +motor.cogging_dq_comp.193 0 +motor.cogging_dq_comp.194 0 +motor.cogging_dq_comp.195 0 +motor.cogging_dq_comp.196 0 +motor.cogging_dq_comp.197 0 +motor.cogging_dq_comp.198 0 +motor.cogging_dq_comp.199 0 +motor.cogging_dq_comp.200 0 +motor.cogging_dq_comp.201 0 +motor.cogging_dq_comp.202 0 +motor.cogging_dq_comp.203 0 +motor.cogging_dq_comp.204 0 +motor.cogging_dq_comp.205 0 +motor.cogging_dq_comp.206 0 +motor.cogging_dq_comp.207 0 +motor.cogging_dq_comp.208 0 +motor.cogging_dq_comp.209 0 +motor.cogging_dq_comp.210 0 +motor.cogging_dq_comp.211 0 +motor.cogging_dq_comp.212 0 +motor.cogging_dq_comp.213 0 +motor.cogging_dq_comp.214 0 +motor.cogging_dq_comp.215 0 +motor.cogging_dq_comp.216 0 +motor.cogging_dq_comp.217 0 +motor.cogging_dq_comp.218 0 +motor.cogging_dq_comp.219 0 +motor.cogging_dq_comp.220 0 +motor.cogging_dq_comp.221 0 +motor.cogging_dq_comp.222 0 +motor.cogging_dq_comp.223 0 +motor.cogging_dq_comp.224 0 +motor.cogging_dq_comp.225 0 +motor.cogging_dq_comp.226 0 +motor.cogging_dq_comp.227 0 +motor.cogging_dq_comp.228 0 +motor.cogging_dq_comp.229 0 +motor.cogging_dq_comp.230 0 +motor.cogging_dq_comp.231 0 +motor.cogging_dq_comp.232 0 +motor.cogging_dq_comp.233 0 +motor.cogging_dq_comp.234 0 +motor.cogging_dq_comp.235 0 +motor.cogging_dq_comp.236 0 +motor.cogging_dq_comp.237 0 +motor.cogging_dq_comp.238 0 +motor.cogging_dq_comp.239 0 +motor.cogging_dq_comp.240 0 +motor.cogging_dq_comp.241 0 +motor.cogging_dq_comp.242 0 +motor.cogging_dq_comp.243 0 +motor.cogging_dq_comp.244 0 +motor.cogging_dq_comp.245 0 +motor.cogging_dq_comp.246 0 +motor.cogging_dq_comp.247 0 +motor.cogging_dq_comp.248 0 +motor.cogging_dq_comp.249 0 +motor.cogging_dq_comp.250 0 +motor.cogging_dq_comp.251 0 +motor.cogging_dq_comp.252 0 +motor.cogging_dq_comp.253 0 +motor.cogging_dq_comp.254 0 +motor.cogging_dq_comp.255 0 +motor.cogging_dq_comp.256 0 +motor.cogging_dq_comp.257 0 +motor.cogging_dq_comp.258 0 +motor.cogging_dq_comp.259 0 +motor.cogging_dq_comp.260 0 +motor.cogging_dq_comp.261 0 +motor.cogging_dq_comp.262 0 +motor.cogging_dq_comp.263 0 +motor.cogging_dq_comp.264 0 +motor.cogging_dq_comp.265 0 +motor.cogging_dq_comp.266 0 +motor.cogging_dq_comp.267 0 +motor.cogging_dq_comp.268 0 +motor.cogging_dq_comp.269 0 +motor.cogging_dq_comp.270 0 +motor.cogging_dq_comp.271 0 +motor.cogging_dq_comp.272 0 +motor.cogging_dq_comp.273 0 +motor.cogging_dq_comp.274 0 +motor.cogging_dq_comp.275 0 +motor.cogging_dq_comp.276 0 +motor.cogging_dq_comp.277 0 +motor.cogging_dq_comp.278 0 +motor.cogging_dq_comp.279 0 +motor.cogging_dq_comp.280 0 +motor.cogging_dq_comp.281 0 +motor.cogging_dq_comp.282 0 +motor.cogging_dq_comp.283 0 +motor.cogging_dq_comp.284 0 +motor.cogging_dq_comp.285 0 +motor.cogging_dq_comp.286 0 +motor.cogging_dq_comp.287 0 +motor.cogging_dq_comp.288 0 +motor.cogging_dq_comp.289 0 +motor.cogging_dq_comp.290 0 +motor.cogging_dq_comp.291 0 +motor.cogging_dq_comp.292 0 +motor.cogging_dq_comp.293 0 +motor.cogging_dq_comp.294 0 +motor.cogging_dq_comp.295 0 +motor.cogging_dq_comp.296 0 +motor.cogging_dq_comp.297 0 +motor.cogging_dq_comp.298 0 +motor.cogging_dq_comp.299 0 +motor.cogging_dq_comp.300 0 +motor.cogging_dq_comp.301 0 +motor.cogging_dq_comp.302 0 +motor.cogging_dq_comp.303 0 +motor.cogging_dq_comp.304 0 +motor.cogging_dq_comp.305 0 +motor.cogging_dq_comp.306 0 +motor.cogging_dq_comp.307 0 +motor.cogging_dq_comp.308 0 +motor.cogging_dq_comp.309 0 +motor.cogging_dq_comp.310 0 +motor.cogging_dq_comp.311 0 +motor.cogging_dq_comp.312 0 +motor.cogging_dq_comp.313 0 +motor.cogging_dq_comp.314 0 +motor.cogging_dq_comp.315 0 +motor.cogging_dq_comp.316 0 +motor.cogging_dq_comp.317 0 +motor.cogging_dq_comp.318 0 +motor.cogging_dq_comp.319 0 +motor.cogging_dq_comp.320 0 +motor.cogging_dq_comp.321 0 +motor.cogging_dq_comp.322 0 +motor.cogging_dq_comp.323 0 +motor.cogging_dq_comp.324 0 +motor.cogging_dq_comp.325 0 +motor.cogging_dq_comp.326 0 +motor.cogging_dq_comp.327 0 +motor.cogging_dq_comp.328 0 +motor.cogging_dq_comp.329 0 +motor.cogging_dq_comp.330 0 +motor.cogging_dq_comp.331 0 +motor.cogging_dq_comp.332 0 +motor.cogging_dq_comp.333 0 +motor.cogging_dq_comp.334 0 +motor.cogging_dq_comp.335 0 +motor.cogging_dq_comp.336 0 +motor.cogging_dq_comp.337 0 +motor.cogging_dq_comp.338 0 +motor.cogging_dq_comp.339 0 +motor.cogging_dq_comp.340 0 +motor.cogging_dq_comp.341 0 +motor.cogging_dq_comp.342 0 +motor.cogging_dq_comp.343 0 +motor.cogging_dq_comp.344 0 +motor.cogging_dq_comp.345 0 +motor.cogging_dq_comp.346 0 +motor.cogging_dq_comp.347 0 +motor.cogging_dq_comp.348 0 +motor.cogging_dq_comp.349 0 +motor.cogging_dq_comp.350 0 +motor.cogging_dq_comp.351 0 +motor.cogging_dq_comp.352 0 +motor.cogging_dq_comp.353 0 +motor.cogging_dq_comp.354 0 +motor.cogging_dq_comp.355 0 +motor.cogging_dq_comp.356 0 +motor.cogging_dq_comp.357 0 +motor.cogging_dq_comp.358 0 +motor.cogging_dq_comp.359 0 +motor.cogging_dq_comp.360 0 +motor.cogging_dq_comp.361 0 +motor.cogging_dq_comp.362 0 +motor.cogging_dq_comp.363 0 +motor.cogging_dq_comp.364 0 +motor.cogging_dq_comp.365 0 +motor.cogging_dq_comp.366 0 +motor.cogging_dq_comp.367 0 +motor.cogging_dq_comp.368 0 +motor.cogging_dq_comp.369 0 +motor.cogging_dq_comp.370 0 +motor.cogging_dq_comp.371 0 +motor.cogging_dq_comp.372 0 +motor.cogging_dq_comp.373 0 +motor.cogging_dq_comp.374 0 +motor.cogging_dq_comp.375 0 +motor.cogging_dq_comp.376 0 +motor.cogging_dq_comp.377 0 +motor.cogging_dq_comp.378 0 +motor.cogging_dq_comp.379 0 +motor.cogging_dq_comp.380 0 +motor.cogging_dq_comp.381 0 +motor.cogging_dq_comp.382 0 +motor.cogging_dq_comp.383 0 +motor.cogging_dq_comp.384 0 +motor.cogging_dq_comp.385 0 +motor.cogging_dq_comp.386 0 +motor.cogging_dq_comp.387 0 +motor.cogging_dq_comp.388 0 +motor.cogging_dq_comp.389 0 +motor.cogging_dq_comp.390 0 +motor.cogging_dq_comp.391 0 +motor.cogging_dq_comp.392 0 +motor.cogging_dq_comp.393 0 +motor.cogging_dq_comp.394 0 +motor.cogging_dq_comp.395 0 +motor.cogging_dq_comp.396 0 +motor.cogging_dq_comp.397 0 +motor.cogging_dq_comp.398 0 +motor.cogging_dq_comp.399 0 +motor.cogging_dq_comp.400 0 +motor.cogging_dq_comp.401 0 +motor.cogging_dq_comp.402 0 +motor.cogging_dq_comp.403 0 +motor.cogging_dq_comp.404 0 +motor.cogging_dq_comp.405 0 +motor.cogging_dq_comp.406 0 +motor.cogging_dq_comp.407 0 +motor.cogging_dq_comp.408 0 +motor.cogging_dq_comp.409 0 +motor.cogging_dq_comp.410 0 +motor.cogging_dq_comp.411 0 +motor.cogging_dq_comp.412 0 +motor.cogging_dq_comp.413 0 +motor.cogging_dq_comp.414 0 +motor.cogging_dq_comp.415 0 +motor.cogging_dq_comp.416 0 +motor.cogging_dq_comp.417 0 +motor.cogging_dq_comp.418 0 +motor.cogging_dq_comp.419 0 +motor.cogging_dq_comp.420 0 +motor.cogging_dq_comp.421 0 +motor.cogging_dq_comp.422 0 +motor.cogging_dq_comp.423 0 +motor.cogging_dq_comp.424 0 +motor.cogging_dq_comp.425 0 +motor.cogging_dq_comp.426 0 +motor.cogging_dq_comp.427 0 +motor.cogging_dq_comp.428 0 +motor.cogging_dq_comp.429 0 +motor.cogging_dq_comp.430 0 +motor.cogging_dq_comp.431 0 +motor.cogging_dq_comp.432 0 +motor.cogging_dq_comp.433 0 +motor.cogging_dq_comp.434 0 +motor.cogging_dq_comp.435 0 +motor.cogging_dq_comp.436 0 +motor.cogging_dq_comp.437 0 +motor.cogging_dq_comp.438 0 +motor.cogging_dq_comp.439 0 +motor.cogging_dq_comp.440 0 +motor.cogging_dq_comp.441 0 +motor.cogging_dq_comp.442 0 +motor.cogging_dq_comp.443 0 +motor.cogging_dq_comp.444 0 +motor.cogging_dq_comp.445 0 +motor.cogging_dq_comp.446 0 +motor.cogging_dq_comp.447 0 +motor.cogging_dq_comp.448 0 +motor.cogging_dq_comp.449 0 +motor.cogging_dq_comp.450 0 +motor.cogging_dq_comp.451 0 +motor.cogging_dq_comp.452 0 +motor.cogging_dq_comp.453 0 +motor.cogging_dq_comp.454 0 +motor.cogging_dq_comp.455 0 +motor.cogging_dq_comp.456 0 +motor.cogging_dq_comp.457 0 +motor.cogging_dq_comp.458 0 +motor.cogging_dq_comp.459 0 +motor.cogging_dq_comp.460 0 +motor.cogging_dq_comp.461 0 +motor.cogging_dq_comp.462 0 +motor.cogging_dq_comp.463 0 +motor.cogging_dq_comp.464 0 +motor.cogging_dq_comp.465 0 +motor.cogging_dq_comp.466 0 +motor.cogging_dq_comp.467 0 +motor.cogging_dq_comp.468 0 +motor.cogging_dq_comp.469 0 +motor.cogging_dq_comp.470 0 +motor.cogging_dq_comp.471 0 +motor.cogging_dq_comp.472 0 +motor.cogging_dq_comp.473 0 +motor.cogging_dq_comp.474 0 +motor.cogging_dq_comp.475 0 +motor.cogging_dq_comp.476 0 +motor.cogging_dq_comp.477 0 +motor.cogging_dq_comp.478 0 +motor.cogging_dq_comp.479 0 +motor.cogging_dq_comp.480 0 +motor.cogging_dq_comp.481 0 +motor.cogging_dq_comp.482 0 +motor.cogging_dq_comp.483 0 +motor.cogging_dq_comp.484 0 +motor.cogging_dq_comp.485 0 +motor.cogging_dq_comp.486 0 +motor.cogging_dq_comp.487 0 +motor.cogging_dq_comp.488 0 +motor.cogging_dq_comp.489 0 +motor.cogging_dq_comp.490 0 +motor.cogging_dq_comp.491 0 +motor.cogging_dq_comp.492 0 +motor.cogging_dq_comp.493 0 +motor.cogging_dq_comp.494 0 +motor.cogging_dq_comp.495 0 +motor.cogging_dq_comp.496 0 +motor.cogging_dq_comp.497 0 +motor.cogging_dq_comp.498 0 +motor.cogging_dq_comp.499 0 +motor.cogging_dq_comp.500 0 +motor.cogging_dq_comp.501 0 +motor.cogging_dq_comp.502 0 +motor.cogging_dq_comp.503 0 +motor.cogging_dq_comp.504 0 +motor.cogging_dq_comp.505 0 +motor.cogging_dq_comp.506 0 +motor.cogging_dq_comp.507 0 +motor.cogging_dq_comp.508 0 +motor.cogging_dq_comp.509 0 +motor.cogging_dq_comp.510 0 +motor.cogging_dq_comp.511 0 +motor.cogging_dq_comp.512 0 +motor.cogging_dq_comp.513 0 +motor.cogging_dq_comp.514 0 +motor.cogging_dq_comp.515 0 +motor.cogging_dq_comp.516 0 +motor.cogging_dq_comp.517 0 +motor.cogging_dq_comp.518 0 +motor.cogging_dq_comp.519 0 +motor.cogging_dq_comp.520 0 +motor.cogging_dq_comp.521 0 +motor.cogging_dq_comp.522 0 +motor.cogging_dq_comp.523 0 +motor.cogging_dq_comp.524 0 +motor.cogging_dq_comp.525 0 +motor.cogging_dq_comp.526 0 +motor.cogging_dq_comp.527 0 +motor.cogging_dq_comp.528 0 +motor.cogging_dq_comp.529 0 +motor.cogging_dq_comp.530 0 +motor.cogging_dq_comp.531 0 +motor.cogging_dq_comp.532 0 +motor.cogging_dq_comp.533 0 +motor.cogging_dq_comp.534 0 +motor.cogging_dq_comp.535 0 +motor.cogging_dq_comp.536 0 +motor.cogging_dq_comp.537 0 +motor.cogging_dq_comp.538 0 +motor.cogging_dq_comp.539 0 +motor.cogging_dq_comp.540 0 +motor.cogging_dq_comp.541 0 +motor.cogging_dq_comp.542 0 +motor.cogging_dq_comp.543 0 +motor.cogging_dq_comp.544 0 +motor.cogging_dq_comp.545 0 +motor.cogging_dq_comp.546 0 +motor.cogging_dq_comp.547 0 +motor.cogging_dq_comp.548 0 +motor.cogging_dq_comp.549 0 +motor.cogging_dq_comp.550 0 +motor.cogging_dq_comp.551 0 +motor.cogging_dq_comp.552 0 +motor.cogging_dq_comp.553 0 +motor.cogging_dq_comp.554 0 +motor.cogging_dq_comp.555 0 +motor.cogging_dq_comp.556 0 +motor.cogging_dq_comp.557 0 +motor.cogging_dq_comp.558 0 +motor.cogging_dq_comp.559 0 +motor.cogging_dq_comp.560 0 +motor.cogging_dq_comp.561 0 +motor.cogging_dq_comp.562 0 +motor.cogging_dq_comp.563 0 +motor.cogging_dq_comp.564 0 +motor.cogging_dq_comp.565 0 +motor.cogging_dq_comp.566 0 +motor.cogging_dq_comp.567 0 +motor.cogging_dq_comp.568 0 +motor.cogging_dq_comp.569 0 +motor.cogging_dq_comp.570 0 +motor.cogging_dq_comp.571 0 +motor.cogging_dq_comp.572 0 +motor.cogging_dq_comp.573 0 +motor.cogging_dq_comp.574 0 +motor.cogging_dq_comp.575 0 +motor.cogging_dq_comp.576 0 +motor.cogging_dq_comp.577 0 +motor.cogging_dq_comp.578 0 +motor.cogging_dq_comp.579 0 +motor.cogging_dq_comp.580 0 +motor.cogging_dq_comp.581 0 +motor.cogging_dq_comp.582 0 +motor.cogging_dq_comp.583 0 +motor.cogging_dq_comp.584 0 +motor.cogging_dq_comp.585 0 +motor.cogging_dq_comp.586 0 +motor.cogging_dq_comp.587 0 +motor.cogging_dq_comp.588 0 +motor.cogging_dq_comp.589 0 +motor.cogging_dq_comp.590 0 +motor.cogging_dq_comp.591 0 +motor.cogging_dq_comp.592 0 +motor.cogging_dq_comp.593 0 +motor.cogging_dq_comp.594 0 +motor.cogging_dq_comp.595 0 +motor.cogging_dq_comp.596 0 +motor.cogging_dq_comp.597 0 +motor.cogging_dq_comp.598 0 +motor.cogging_dq_comp.599 0 +motor.cogging_dq_comp.600 0 +motor.cogging_dq_comp.601 0 +motor.cogging_dq_comp.602 0 +motor.cogging_dq_comp.603 0 +motor.cogging_dq_comp.604 0 +motor.cogging_dq_comp.605 0 +motor.cogging_dq_comp.606 0 +motor.cogging_dq_comp.607 0 +motor.cogging_dq_comp.608 0 +motor.cogging_dq_comp.609 0 +motor.cogging_dq_comp.610 0 +motor.cogging_dq_comp.611 0 +motor.cogging_dq_comp.612 0 +motor.cogging_dq_comp.613 0 +motor.cogging_dq_comp.614 0 +motor.cogging_dq_comp.615 0 +motor.cogging_dq_comp.616 0 +motor.cogging_dq_comp.617 0 +motor.cogging_dq_comp.618 0 +motor.cogging_dq_comp.619 0 +motor.cogging_dq_comp.620 0 +motor.cogging_dq_comp.621 0 +motor.cogging_dq_comp.622 0 +motor.cogging_dq_comp.623 0 +motor.cogging_dq_comp.624 0 +motor.cogging_dq_comp.625 0 +motor.cogging_dq_comp.626 0 +motor.cogging_dq_comp.627 0 +motor.cogging_dq_comp.628 0 +motor.cogging_dq_comp.629 0 +motor.cogging_dq_comp.630 0 +motor.cogging_dq_comp.631 0 +motor.cogging_dq_comp.632 0 +motor.cogging_dq_comp.633 0 +motor.cogging_dq_comp.634 0 +motor.cogging_dq_comp.635 0 +motor.cogging_dq_comp.636 0 +motor.cogging_dq_comp.637 0 +motor.cogging_dq_comp.638 0 +motor.cogging_dq_comp.639 0 +motor.cogging_dq_comp.640 0 +motor.cogging_dq_comp.641 0 +motor.cogging_dq_comp.642 0 +motor.cogging_dq_comp.643 0 +motor.cogging_dq_comp.644 0 +motor.cogging_dq_comp.645 0 +motor.cogging_dq_comp.646 0 +motor.cogging_dq_comp.647 0 +motor.cogging_dq_comp.648 0 +motor.cogging_dq_comp.649 0 +motor.cogging_dq_comp.650 0 +motor.cogging_dq_comp.651 0 +motor.cogging_dq_comp.652 0 +motor.cogging_dq_comp.653 0 +motor.cogging_dq_comp.654 0 +motor.cogging_dq_comp.655 0 +motor.cogging_dq_comp.656 0 +motor.cogging_dq_comp.657 0 +motor.cogging_dq_comp.658 0 +motor.cogging_dq_comp.659 0 +motor.cogging_dq_comp.660 0 +motor.cogging_dq_comp.661 0 +motor.cogging_dq_comp.662 0 +motor.cogging_dq_comp.663 0 +motor.cogging_dq_comp.664 0 +motor.cogging_dq_comp.665 0 +motor.cogging_dq_comp.666 0 +motor.cogging_dq_comp.667 0 +motor.cogging_dq_comp.668 0 +motor.cogging_dq_comp.669 0 +motor.cogging_dq_comp.670 0 +motor.cogging_dq_comp.671 0 +motor.cogging_dq_comp.672 0 +motor.cogging_dq_comp.673 0 +motor.cogging_dq_comp.674 0 +motor.cogging_dq_comp.675 0 +motor.cogging_dq_comp.676 0 +motor.cogging_dq_comp.677 0 +motor.cogging_dq_comp.678 0 +motor.cogging_dq_comp.679 0 +motor.cogging_dq_comp.680 0 +motor.cogging_dq_comp.681 0 +motor.cogging_dq_comp.682 0 +motor.cogging_dq_comp.683 0 +motor.cogging_dq_comp.684 0 +motor.cogging_dq_comp.685 0 +motor.cogging_dq_comp.686 0 +motor.cogging_dq_comp.687 0 +motor.cogging_dq_comp.688 0 +motor.cogging_dq_comp.689 0 +motor.cogging_dq_comp.690 0 +motor.cogging_dq_comp.691 0 +motor.cogging_dq_comp.692 0 +motor.cogging_dq_comp.693 0 +motor.cogging_dq_comp.694 0 +motor.cogging_dq_comp.695 0 +motor.cogging_dq_comp.696 0 +motor.cogging_dq_comp.697 0 +motor.cogging_dq_comp.698 0 +motor.cogging_dq_comp.699 0 +motor.cogging_dq_comp.700 0 +motor.cogging_dq_comp.701 0 +motor.cogging_dq_comp.702 0 +motor.cogging_dq_comp.703 0 +motor.cogging_dq_comp.704 0 +motor.cogging_dq_comp.705 0 +motor.cogging_dq_comp.706 0 +motor.cogging_dq_comp.707 0 +motor.cogging_dq_comp.708 0 +motor.cogging_dq_comp.709 0 +motor.cogging_dq_comp.710 0 +motor.cogging_dq_comp.711 0 +motor.cogging_dq_comp.712 0 +motor.cogging_dq_comp.713 0 +motor.cogging_dq_comp.714 0 +motor.cogging_dq_comp.715 0 +motor.cogging_dq_comp.716 0 +motor.cogging_dq_comp.717 0 +motor.cogging_dq_comp.718 0 +motor.cogging_dq_comp.719 0 +motor.cogging_dq_comp.720 0 +motor.cogging_dq_comp.721 0 +motor.cogging_dq_comp.722 0 +motor.cogging_dq_comp.723 0 +motor.cogging_dq_comp.724 0 +motor.cogging_dq_comp.725 0 +motor.cogging_dq_comp.726 0 +motor.cogging_dq_comp.727 0 +motor.cogging_dq_comp.728 0 +motor.cogging_dq_comp.729 0 +motor.cogging_dq_comp.730 0 +motor.cogging_dq_comp.731 0 +motor.cogging_dq_comp.732 0 +motor.cogging_dq_comp.733 0 +motor.cogging_dq_comp.734 0 +motor.cogging_dq_comp.735 0 +motor.cogging_dq_comp.736 0 +motor.cogging_dq_comp.737 0 +motor.cogging_dq_comp.738 0 +motor.cogging_dq_comp.739 0 +motor.cogging_dq_comp.740 0 +motor.cogging_dq_comp.741 0 +motor.cogging_dq_comp.742 0 +motor.cogging_dq_comp.743 0 +motor.cogging_dq_comp.744 0 +motor.cogging_dq_comp.745 0 +motor.cogging_dq_comp.746 0 +motor.cogging_dq_comp.747 0 +motor.cogging_dq_comp.748 0 +motor.cogging_dq_comp.749 0 +motor.cogging_dq_comp.750 0 +motor.cogging_dq_comp.751 0 +motor.cogging_dq_comp.752 0 +motor.cogging_dq_comp.753 0 +motor.cogging_dq_comp.754 0 +motor.cogging_dq_comp.755 0 +motor.cogging_dq_comp.756 0 +motor.cogging_dq_comp.757 0 +motor.cogging_dq_comp.758 0 +motor.cogging_dq_comp.759 0 +motor.cogging_dq_comp.760 0 +motor.cogging_dq_comp.761 0 +motor.cogging_dq_comp.762 0 +motor.cogging_dq_comp.763 0 +motor.cogging_dq_comp.764 0 +motor.cogging_dq_comp.765 0 +motor.cogging_dq_comp.766 0 +motor.cogging_dq_comp.767 0 +motor.cogging_dq_comp.768 0 +motor.cogging_dq_comp.769 0 +motor.cogging_dq_comp.770 0 +motor.cogging_dq_comp.771 0 +motor.cogging_dq_comp.772 0 +motor.cogging_dq_comp.773 0 +motor.cogging_dq_comp.774 0 +motor.cogging_dq_comp.775 0 +motor.cogging_dq_comp.776 0 +motor.cogging_dq_comp.777 0 +motor.cogging_dq_comp.778 0 +motor.cogging_dq_comp.779 0 +motor.cogging_dq_comp.780 0 +motor.cogging_dq_comp.781 0 +motor.cogging_dq_comp.782 0 +motor.cogging_dq_comp.783 0 +motor.cogging_dq_comp.784 0 +motor.cogging_dq_comp.785 0 +motor.cogging_dq_comp.786 0 +motor.cogging_dq_comp.787 0 +motor.cogging_dq_comp.788 0 +motor.cogging_dq_comp.789 0 +motor.cogging_dq_comp.790 0 +motor.cogging_dq_comp.791 0 +motor.cogging_dq_comp.792 0 +motor.cogging_dq_comp.793 0 +motor.cogging_dq_comp.794 0 +motor.cogging_dq_comp.795 0 +motor.cogging_dq_comp.796 0 +motor.cogging_dq_comp.797 0 +motor.cogging_dq_comp.798 0 +motor.cogging_dq_comp.799 0 +motor.cogging_dq_comp.800 0 +motor.cogging_dq_comp.801 0 +motor.cogging_dq_comp.802 0 +motor.cogging_dq_comp.803 0 +motor.cogging_dq_comp.804 0 +motor.cogging_dq_comp.805 0 +motor.cogging_dq_comp.806 0 +motor.cogging_dq_comp.807 0 +motor.cogging_dq_comp.808 0 +motor.cogging_dq_comp.809 0 +motor.cogging_dq_comp.810 0 +motor.cogging_dq_comp.811 0 +motor.cogging_dq_comp.812 0 +motor.cogging_dq_comp.813 0 +motor.cogging_dq_comp.814 0 +motor.cogging_dq_comp.815 0 +motor.cogging_dq_comp.816 0 +motor.cogging_dq_comp.817 0 +motor.cogging_dq_comp.818 0 +motor.cogging_dq_comp.819 0 +motor.cogging_dq_comp.820 0 +motor.cogging_dq_comp.821 0 +motor.cogging_dq_comp.822 0 +motor.cogging_dq_comp.823 0 +motor.cogging_dq_comp.824 0 +motor.cogging_dq_comp.825 0 +motor.cogging_dq_comp.826 0 +motor.cogging_dq_comp.827 0 +motor.cogging_dq_comp.828 0 +motor.cogging_dq_comp.829 0 +motor.cogging_dq_comp.830 0 +motor.cogging_dq_comp.831 0 +motor.cogging_dq_comp.832 0 +motor.cogging_dq_comp.833 0 +motor.cogging_dq_comp.834 0 +motor.cogging_dq_comp.835 0 +motor.cogging_dq_comp.836 0 +motor.cogging_dq_comp.837 0 +motor.cogging_dq_comp.838 0 +motor.cogging_dq_comp.839 0 +motor.cogging_dq_comp.840 0 +motor.cogging_dq_comp.841 0 +motor.cogging_dq_comp.842 0 +motor.cogging_dq_comp.843 0 +motor.cogging_dq_comp.844 0 +motor.cogging_dq_comp.845 0 +motor.cogging_dq_comp.846 0 +motor.cogging_dq_comp.847 0 +motor.cogging_dq_comp.848 0 +motor.cogging_dq_comp.849 0 +motor.cogging_dq_comp.850 0 +motor.cogging_dq_comp.851 0 +motor.cogging_dq_comp.852 0 +motor.cogging_dq_comp.853 0 +motor.cogging_dq_comp.854 0 +motor.cogging_dq_comp.855 0 +motor.cogging_dq_comp.856 0 +motor.cogging_dq_comp.857 0 +motor.cogging_dq_comp.858 0 +motor.cogging_dq_comp.859 0 +motor.cogging_dq_comp.860 0 +motor.cogging_dq_comp.861 0 +motor.cogging_dq_comp.862 0 +motor.cogging_dq_comp.863 0 +motor.cogging_dq_comp.864 0 +motor.cogging_dq_comp.865 0 +motor.cogging_dq_comp.866 0 +motor.cogging_dq_comp.867 0 +motor.cogging_dq_comp.868 0 +motor.cogging_dq_comp.869 0 +motor.cogging_dq_comp.870 0 +motor.cogging_dq_comp.871 0 +motor.cogging_dq_comp.872 0 +motor.cogging_dq_comp.873 0 +motor.cogging_dq_comp.874 0 +motor.cogging_dq_comp.875 0 +motor.cogging_dq_comp.876 0 +motor.cogging_dq_comp.877 0 +motor.cogging_dq_comp.878 0 +motor.cogging_dq_comp.879 0 +motor.cogging_dq_comp.880 0 +motor.cogging_dq_comp.881 0 +motor.cogging_dq_comp.882 0 +motor.cogging_dq_comp.883 0 +motor.cogging_dq_comp.884 0 +motor.cogging_dq_comp.885 0 +motor.cogging_dq_comp.886 0 +motor.cogging_dq_comp.887 0 +motor.cogging_dq_comp.888 0 +motor.cogging_dq_comp.889 0 +motor.cogging_dq_comp.890 0 +motor.cogging_dq_comp.891 0 +motor.cogging_dq_comp.892 0 +motor.cogging_dq_comp.893 0 +motor.cogging_dq_comp.894 0 +motor.cogging_dq_comp.895 0 +motor.cogging_dq_comp.896 0 +motor.cogging_dq_comp.897 0 +motor.cogging_dq_comp.898 0 +motor.cogging_dq_comp.899 0 +motor.cogging_dq_comp.900 0 +motor.cogging_dq_comp.901 0 +motor.cogging_dq_comp.902 0 +motor.cogging_dq_comp.903 0 +motor.cogging_dq_comp.904 0 +motor.cogging_dq_comp.905 0 +motor.cogging_dq_comp.906 0 +motor.cogging_dq_comp.907 0 +motor.cogging_dq_comp.908 0 +motor.cogging_dq_comp.909 0 +motor.cogging_dq_comp.910 0 +motor.cogging_dq_comp.911 0 +motor.cogging_dq_comp.912 0 +motor.cogging_dq_comp.913 0 +motor.cogging_dq_comp.914 0 +motor.cogging_dq_comp.915 0 +motor.cogging_dq_comp.916 0 +motor.cogging_dq_comp.917 0 +motor.cogging_dq_comp.918 0 +motor.cogging_dq_comp.919 0 +motor.cogging_dq_comp.920 0 +motor.cogging_dq_comp.921 0 +motor.cogging_dq_comp.922 0 +motor.cogging_dq_comp.923 0 +motor.cogging_dq_comp.924 0 +motor.cogging_dq_comp.925 0 +motor.cogging_dq_comp.926 0 +motor.cogging_dq_comp.927 0 +motor.cogging_dq_comp.928 0 +motor.cogging_dq_comp.929 0 +motor.cogging_dq_comp.930 0 +motor.cogging_dq_comp.931 0 +motor.cogging_dq_comp.932 0 +motor.cogging_dq_comp.933 0 +motor.cogging_dq_comp.934 0 +motor.cogging_dq_comp.935 0 +motor.cogging_dq_comp.936 0 +motor.cogging_dq_comp.937 0 +motor.cogging_dq_comp.938 0 +motor.cogging_dq_comp.939 0 +motor.cogging_dq_comp.940 0 +motor.cogging_dq_comp.941 0 +motor.cogging_dq_comp.942 0 +motor.cogging_dq_comp.943 0 +motor.cogging_dq_comp.944 0 +motor.cogging_dq_comp.945 0 +motor.cogging_dq_comp.946 0 +motor.cogging_dq_comp.947 0 +motor.cogging_dq_comp.948 0 +motor.cogging_dq_comp.949 0 +motor.cogging_dq_comp.950 0 +motor.cogging_dq_comp.951 0 +motor.cogging_dq_comp.952 0 +motor.cogging_dq_comp.953 0 +motor.cogging_dq_comp.954 0 +motor.cogging_dq_comp.955 0 +motor.cogging_dq_comp.956 0 +motor.cogging_dq_comp.957 0 +motor.cogging_dq_comp.958 0 +motor.cogging_dq_comp.959 0 +motor.cogging_dq_comp.960 0 +motor.cogging_dq_comp.961 0 +motor.cogging_dq_comp.962 0 +motor.cogging_dq_comp.963 0 +motor.cogging_dq_comp.964 0 +motor.cogging_dq_comp.965 0 +motor.cogging_dq_comp.966 0 +motor.cogging_dq_comp.967 0 +motor.cogging_dq_comp.968 0 +motor.cogging_dq_comp.969 0 +motor.cogging_dq_comp.970 0 +motor.cogging_dq_comp.971 0 +motor.cogging_dq_comp.972 0 +motor.cogging_dq_comp.973 0 +motor.cogging_dq_comp.974 0 +motor.cogging_dq_comp.975 0 +motor.cogging_dq_comp.976 0 +motor.cogging_dq_comp.977 0 +motor.cogging_dq_comp.978 0 +motor.cogging_dq_comp.979 0 +motor.cogging_dq_comp.980 0 +motor.cogging_dq_comp.981 0 +motor.cogging_dq_comp.982 0 +motor.cogging_dq_comp.983 0 +motor.cogging_dq_comp.984 0 +motor.cogging_dq_comp.985 0 +motor.cogging_dq_comp.986 0 +motor.cogging_dq_comp.987 0 +motor.cogging_dq_comp.988 0 +motor.cogging_dq_comp.989 0 +motor.cogging_dq_comp.990 0 +motor.cogging_dq_comp.991 0 +motor.cogging_dq_comp.992 0 +motor.cogging_dq_comp.993 0 +motor.cogging_dq_comp.994 0 +motor.cogging_dq_comp.995 0 +motor.cogging_dq_comp.996 0 +motor.cogging_dq_comp.997 0 +motor.cogging_dq_comp.998 0 +motor.cogging_dq_comp.999 0 +motor.cogging_dq_comp.1000 0 +motor.cogging_dq_comp.1001 0 +motor.cogging_dq_comp.1002 0 +motor.cogging_dq_comp.1003 0 +motor.cogging_dq_comp.1004 0 +motor.cogging_dq_comp.1005 0 +motor.cogging_dq_comp.1006 0 +motor.cogging_dq_comp.1007 0 +motor.cogging_dq_comp.1008 0 +motor.cogging_dq_comp.1009 0 +motor.cogging_dq_comp.1010 0 +motor.cogging_dq_comp.1011 0 +motor.cogging_dq_comp.1012 0 +motor.cogging_dq_comp.1013 0 +motor.cogging_dq_comp.1014 0 +motor.cogging_dq_comp.1015 0 +motor.cogging_dq_comp.1016 0 +motor.cogging_dq_comp.1017 0 +motor.cogging_dq_comp.1018 0 +motor.cogging_dq_comp.1019 0 +motor.cogging_dq_comp.1020 0 +motor.cogging_dq_comp.1021 0 +motor.cogging_dq_comp.1022 0 +motor.cogging_dq_comp.1023 0 +drv8323_conf.dis_cpuv 0 +drv8323_conf.dis_gdf 0 +drv8323_conf.otw_rep 0 +drv8323_conf.pwm_mode 1 +drv8323_conf.pwm_1x_asynchronous 0 +drv8323_conf.pwm_1x_dir 0 +drv8323_conf.idrivep_hs_ma 150 +drv8323_conf.idriven_hs_ma 300 +drv8323_conf.cbc 1 +drv8323_conf.tdrive_ns 1000 +drv8323_conf.idrivep_ls_ma 150 +drv8323_conf.idriven_ls_ma 300 +drv8323_conf.tretry 0 +drv8323_conf.dead_time_ns 50 +drv8323_conf.ocp_mode 0 +drv8323_conf.ocp_deg_us 4 +drv8323_conf.vds_lvl_mv 700 +drv8323_conf.csa_fet 0 +drv8323_conf.vref_div 1 +drv8323_conf.ls_ref 0 +drv8323_conf.csa_gain 20 +drv8323_conf.dis_sen 0 +drv8323_conf.sen_lvl_mv 500 +servo.pwm_rate_hz 30000 +servo.i_gain 20.000000 +servo.current_sense_ohm 0.000500 +servo.pwm_comp_off 0.027000 +servo.pwm_comp_mag 0.005000 +servo.pwm_scale 1.000000 +servo.max_voltage 56.000000 +servo.max_power_W 450.000000 +servo.derate_temperature 50.000000 +servo.fault_temperature 75.000000 +servo.enable_motor_temperature 0 +servo.motor_derate_temperature 50.000000 +servo.motor_fault_temperature nan +servo.velocity_threshold 0.000000 +servo.position_derate 0.020000 +servo.adc_cur_cycles 2 +servo.adc_aux_cycles 47 +servo.pid_dq.kp 1.387107 +servo.pid_dq.ki 2331.960449 +servo.pid_position.kp 1.300000 +servo.pid_position.ki 0.000000 +servo.pid_position.kd 0.010000 +servo.pid_position.iratelimit -1.000000 +servo.pid_position.ilimit 0.000000 +servo.pid_position.max_desired_rate 0.000000 +servo.pid_position.sign -1 +servo.current_feedforward 1.000000 +servo.bemf_feedforward 1.000000 +servo.default_velocity_limit 5.000000 +servo.default_accel_limit 10.000000 +servo.voltage_mode_control 0 +servo.fixed_voltage_mode 0 +servo.fixed_voltage_control_V 0.000000 +servo.max_position_slip nan +servo.max_velocity_slip nan +servo.default_timeout_s 0.100000 +servo.timeout_max_torque_Nm 5.000000 +servo.timeout_mode 12 +servo.flux_brake_min_voltage 53.000000 +servo.flux_brake_resistance_ohm 0.025000 +servo.max_current_A 5.000000 +servo.derate_current_A -20.000000 +servo.max_velocity 10.000000 +servo.max_velocity_derate 2.000000 +servo.cooldown_cycles 256 +servo.velocity_zero_capture_threshold 0.050000 +servo.timing_fault 0 +servo.emit_debug 0 +servopos.position_min nan +servopos.position_max nan +id.id 35 +can.prefix 0 + diff --git a/config/moteus/joint_de_1.cfg b/config/moteus/joint_de_1.cfg new file mode 100644 index 000000000..68c5e7f78 --- /dev/null +++ b/config/moteus/joint_de_1.cfg @@ -0,0 +1,1399 @@ +uuid.uuid.0 210 +uuid.uuid.1 42 +uuid.uuid.2 55 +uuid.uuid.3 197 +uuid.uuid.4 115 +uuid.uuid.5 183 +uuid.uuid.6 70 +uuid.uuid.7 194 +uuid.uuid.8 179 +uuid.uuid.9 54 +uuid.uuid.10 50 +uuid.uuid.11 137 +uuid.uuid.12 91 +uuid.uuid.13 149 +uuid.uuid.14 200 +uuid.uuid.15 194 +clock.hsitrim 64 +aux1.i2c.i2c_hz 400000 +aux1.i2c.i2c_mode 1 +aux1.i2c.pullup 1 +aux1.i2c.devices.0.type 1 +aux1.i2c.devices.0.address 64 +aux1.i2c.devices.0.poll_rate_us 1000 +aux1.i2c.devices.1.type 0 +aux1.i2c.devices.1.address 64 +aux1.i2c.devices.1.poll_rate_us 1000 +aux1.i2c.devices.2.type 0 +aux1.i2c.devices.2.address 64 +aux1.i2c.devices.2.poll_rate_us 1000 +aux1.spi.mode 1 +aux1.spi.rate_hz 12000000 +aux1.spi.filter_us 64 +aux1.spi.bct 0 +aux1.uart.mode 0 +aux1.uart.baud_rate 115200 +aux1.uart.poll_rate_us 100 +aux1.uart.rs422 0 +aux1.uart.cui_amt21_address 84 +aux1.quadrature.enabled 0 +aux1.quadrature.cpr 16384 +aux1.hall.enabled 0 +aux1.hall.polarity 0 +aux1.index.enabled 0 +aux1.sine_cosine.enabled 0 +aux1.sine_cosine.common 1700 +aux1.i2c_startup_delay_ms 30 +aux1.pins.0.mode 0 +aux1.pins.0.pull 0 +aux1.pins.1.mode 0 +aux1.pins.1.pull 0 +aux1.pins.2.mode 0 +aux1.pins.2.pull 0 +aux1.pins.3.mode 13 +aux1.pins.3.pull 1 +aux1.pins.4.mode 13 +aux1.pins.4.pull 1 +aux2.i2c.i2c_hz 400000 +aux2.i2c.i2c_mode 1 +aux2.i2c.pullup 0 +aux2.i2c.devices.0.type 0 +aux2.i2c.devices.0.address 64 +aux2.i2c.devices.0.poll_rate_us 1000 +aux2.i2c.devices.1.type 0 +aux2.i2c.devices.1.address 64 +aux2.i2c.devices.1.poll_rate_us 1000 +aux2.i2c.devices.2.type 0 +aux2.i2c.devices.2.address 64 +aux2.i2c.devices.2.poll_rate_us 1000 +aux2.spi.mode 1 +aux2.spi.rate_hz 12000000 +aux2.spi.filter_us 64 +aux2.spi.bct 0 +aux2.uart.mode 0 +aux2.uart.baud_rate 115200 +aux2.uart.poll_rate_us 100 +aux2.uart.rs422 0 +aux2.uart.cui_amt21_address 84 +aux2.quadrature.enabled 0 +aux2.quadrature.cpr 16384 +aux2.hall.enabled 1 +aux2.hall.polarity 0 +aux2.index.enabled 0 +aux2.sine_cosine.enabled 0 +aux2.sine_cosine.common 1700 +aux2.i2c_startup_delay_ms 30 +aux2.pins.0.mode 6 +aux2.pins.0.pull 1 +aux2.pins.1.mode 6 +aux2.pins.1.pull 1 +aux2.pins.2.mode 6 +aux2.pins.2.pull 1 +aux2.pins.3.mode 0 +aux2.pins.3.pull 0 +aux2.pins.4.mode 0 +aux2.pins.4.pull 0 +motor_position.sources.0.aux_number 2 +motor_position.sources.0.type 4 +motor_position.sources.0.i2c_device 0 +motor_position.sources.0.incremental_index -1 +motor_position.sources.0.cpr 48 +motor_position.sources.0.offset -1.000000 +motor_position.sources.0.sign -1 +motor_position.sources.0.debug_override -1 +motor_position.sources.0.reference 0 +motor_position.sources.0.pll_filter_hz 100.000000 +motor_position.sources.0.compensation_table.0 0.000000 +motor_position.sources.0.compensation_table.1 0.000000 +motor_position.sources.0.compensation_table.2 0.000000 +motor_position.sources.0.compensation_table.3 0.000000 +motor_position.sources.0.compensation_table.4 0.000000 +motor_position.sources.0.compensation_table.5 0.000000 +motor_position.sources.0.compensation_table.6 0.000000 +motor_position.sources.0.compensation_table.7 0.000000 +motor_position.sources.0.compensation_table.8 0.000000 +motor_position.sources.0.compensation_table.9 0.000000 +motor_position.sources.0.compensation_table.10 0.000000 +motor_position.sources.0.compensation_table.11 0.000000 +motor_position.sources.0.compensation_table.12 0.000000 +motor_position.sources.0.compensation_table.13 0.000000 +motor_position.sources.0.compensation_table.14 0.000000 +motor_position.sources.0.compensation_table.15 0.000000 +motor_position.sources.0.compensation_table.16 0.000000 +motor_position.sources.0.compensation_table.17 0.000000 +motor_position.sources.0.compensation_table.18 0.000000 +motor_position.sources.0.compensation_table.19 0.000000 +motor_position.sources.0.compensation_table.20 0.000000 +motor_position.sources.0.compensation_table.21 0.000000 +motor_position.sources.0.compensation_table.22 0.000000 +motor_position.sources.0.compensation_table.23 0.000000 +motor_position.sources.0.compensation_table.24 0.000000 +motor_position.sources.0.compensation_table.25 0.000000 +motor_position.sources.0.compensation_table.26 0.000000 +motor_position.sources.0.compensation_table.27 0.000000 +motor_position.sources.0.compensation_table.28 0.000000 +motor_position.sources.0.compensation_table.29 0.000000 +motor_position.sources.0.compensation_table.30 0.000000 +motor_position.sources.0.compensation_table.31 0.000000 +motor_position.sources.1.aux_number 1 +motor_position.sources.1.type 7 +motor_position.sources.1.i2c_device 0 +motor_position.sources.1.incremental_index -1 +motor_position.sources.1.cpr 65536 +motor_position.sources.1.offset -52120.000000 +motor_position.sources.1.sign 1 +motor_position.sources.1.debug_override -1 +motor_position.sources.1.reference 1 +motor_position.sources.1.pll_filter_hz 100.000000 +motor_position.sources.1.compensation_table.0 0.000000 +motor_position.sources.1.compensation_table.1 0.000000 +motor_position.sources.1.compensation_table.2 0.000000 +motor_position.sources.1.compensation_table.3 0.000000 +motor_position.sources.1.compensation_table.4 0.000000 +motor_position.sources.1.compensation_table.5 0.000000 +motor_position.sources.1.compensation_table.6 0.000000 +motor_position.sources.1.compensation_table.7 0.000000 +motor_position.sources.1.compensation_table.8 0.000000 +motor_position.sources.1.compensation_table.9 0.000000 +motor_position.sources.1.compensation_table.10 0.000000 +motor_position.sources.1.compensation_table.11 0.000000 +motor_position.sources.1.compensation_table.12 0.000000 +motor_position.sources.1.compensation_table.13 0.000000 +motor_position.sources.1.compensation_table.14 0.000000 +motor_position.sources.1.compensation_table.15 0.000000 +motor_position.sources.1.compensation_table.16 0.000000 +motor_position.sources.1.compensation_table.17 0.000000 +motor_position.sources.1.compensation_table.18 0.000000 +motor_position.sources.1.compensation_table.19 0.000000 +motor_position.sources.1.compensation_table.20 0.000000 +motor_position.sources.1.compensation_table.21 0.000000 +motor_position.sources.1.compensation_table.22 0.000000 +motor_position.sources.1.compensation_table.23 0.000000 +motor_position.sources.1.compensation_table.24 0.000000 +motor_position.sources.1.compensation_table.25 0.000000 +motor_position.sources.1.compensation_table.26 0.000000 +motor_position.sources.1.compensation_table.27 0.000000 +motor_position.sources.1.compensation_table.28 0.000000 +motor_position.sources.1.compensation_table.29 0.000000 +motor_position.sources.1.compensation_table.30 0.000000 +motor_position.sources.1.compensation_table.31 0.000000 +motor_position.sources.2.aux_number 1 +motor_position.sources.2.type 0 +motor_position.sources.2.i2c_device 0 +motor_position.sources.2.incremental_index -1 +motor_position.sources.2.cpr 16384 +motor_position.sources.2.offset 0.000000 +motor_position.sources.2.sign 1 +motor_position.sources.2.debug_override -1 +motor_position.sources.2.reference 0 +motor_position.sources.2.pll_filter_hz 400.000000 +motor_position.sources.2.compensation_table.0 0.000000 +motor_position.sources.2.compensation_table.1 0.000000 +motor_position.sources.2.compensation_table.2 0.000000 +motor_position.sources.2.compensation_table.3 0.000000 +motor_position.sources.2.compensation_table.4 0.000000 +motor_position.sources.2.compensation_table.5 0.000000 +motor_position.sources.2.compensation_table.6 0.000000 +motor_position.sources.2.compensation_table.7 0.000000 +motor_position.sources.2.compensation_table.8 0.000000 +motor_position.sources.2.compensation_table.9 0.000000 +motor_position.sources.2.compensation_table.10 0.000000 +motor_position.sources.2.compensation_table.11 0.000000 +motor_position.sources.2.compensation_table.12 0.000000 +motor_position.sources.2.compensation_table.13 0.000000 +motor_position.sources.2.compensation_table.14 0.000000 +motor_position.sources.2.compensation_table.15 0.000000 +motor_position.sources.2.compensation_table.16 0.000000 +motor_position.sources.2.compensation_table.17 0.000000 +motor_position.sources.2.compensation_table.18 0.000000 +motor_position.sources.2.compensation_table.19 0.000000 +motor_position.sources.2.compensation_table.20 0.000000 +motor_position.sources.2.compensation_table.21 0.000000 +motor_position.sources.2.compensation_table.22 0.000000 +motor_position.sources.2.compensation_table.23 0.000000 +motor_position.sources.2.compensation_table.24 0.000000 +motor_position.sources.2.compensation_table.25 0.000000 +motor_position.sources.2.compensation_table.26 0.000000 +motor_position.sources.2.compensation_table.27 0.000000 +motor_position.sources.2.compensation_table.28 0.000000 +motor_position.sources.2.compensation_table.29 0.000000 +motor_position.sources.2.compensation_table.30 0.000000 +motor_position.sources.2.compensation_table.31 0.000000 +motor_position.commutation_source 0 +motor_position.output.source 0 +motor_position.output.offset 0.000000 +motor_position.output.sign 1 +motor_position.output.reference_source -1 +motor_position.rotor_to_output_ratio 1.000000 +motor.poles 16 +motor.phase_invert 0 +motor.resistance_ohm 3.353775 +motor.v_per_hz 0.571181 +motor.offset.0 0.000000 +motor.offset.1 0.000000 +motor.offset.2 0.000000 +motor.offset.3 0.000000 +motor.offset.4 0.000000 +motor.offset.5 0.000000 +motor.offset.6 0.000000 +motor.offset.7 0.000000 +motor.offset.8 0.000000 +motor.offset.9 0.000000 +motor.offset.10 0.000000 +motor.offset.11 0.000000 +motor.offset.12 0.000000 +motor.offset.13 0.000000 +motor.offset.14 0.000000 +motor.offset.15 0.000000 +motor.offset.16 0.000000 +motor.offset.17 0.000000 +motor.offset.18 0.000000 +motor.offset.19 0.000000 +motor.offset.20 0.000000 +motor.offset.21 0.000000 +motor.offset.22 0.000000 +motor.offset.23 0.000000 +motor.offset.24 0.000000 +motor.offset.25 0.000000 +motor.offset.26 0.000000 +motor.offset.27 0.000000 +motor.offset.28 0.000000 +motor.offset.29 0.000000 +motor.offset.30 0.000000 +motor.offset.31 0.000000 +motor.offset.32 0.000000 +motor.offset.33 0.000000 +motor.offset.34 0.000000 +motor.offset.35 0.000000 +motor.offset.36 0.000000 +motor.offset.37 0.000000 +motor.offset.38 0.000000 +motor.offset.39 0.000000 +motor.offset.40 0.000000 +motor.offset.41 0.000000 +motor.offset.42 0.000000 +motor.offset.43 0.000000 +motor.offset.44 0.000000 +motor.offset.45 0.000000 +motor.offset.46 0.000000 +motor.offset.47 0.000000 +motor.offset.48 0.000000 +motor.offset.49 0.000000 +motor.offset.50 0.000000 +motor.offset.51 0.000000 +motor.offset.52 0.000000 +motor.offset.53 0.000000 +motor.offset.54 0.000000 +motor.offset.55 0.000000 +motor.offset.56 0.000000 +motor.offset.57 0.000000 +motor.offset.58 0.000000 +motor.offset.59 0.000000 +motor.offset.60 0.000000 +motor.offset.61 0.000000 +motor.offset.62 0.000000 +motor.offset.63 0.000000 +motor.rotation_current_cutoff_A 10000.000000 +motor.rotation_current_scale 0.050000 +motor.rotation_torque_scale 14.700000 +motor.cogging_dq_scale 0.000000 +motor.cogging_dq_comp.0 0 +motor.cogging_dq_comp.1 0 +motor.cogging_dq_comp.2 0 +motor.cogging_dq_comp.3 0 +motor.cogging_dq_comp.4 0 +motor.cogging_dq_comp.5 0 +motor.cogging_dq_comp.6 0 +motor.cogging_dq_comp.7 0 +motor.cogging_dq_comp.8 0 +motor.cogging_dq_comp.9 0 +motor.cogging_dq_comp.10 0 +motor.cogging_dq_comp.11 0 +motor.cogging_dq_comp.12 0 +motor.cogging_dq_comp.13 0 +motor.cogging_dq_comp.14 0 +motor.cogging_dq_comp.15 0 +motor.cogging_dq_comp.16 0 +motor.cogging_dq_comp.17 0 +motor.cogging_dq_comp.18 0 +motor.cogging_dq_comp.19 0 +motor.cogging_dq_comp.20 0 +motor.cogging_dq_comp.21 0 +motor.cogging_dq_comp.22 0 +motor.cogging_dq_comp.23 0 +motor.cogging_dq_comp.24 0 +motor.cogging_dq_comp.25 0 +motor.cogging_dq_comp.26 0 +motor.cogging_dq_comp.27 0 +motor.cogging_dq_comp.28 0 +motor.cogging_dq_comp.29 0 +motor.cogging_dq_comp.30 0 +motor.cogging_dq_comp.31 0 +motor.cogging_dq_comp.32 0 +motor.cogging_dq_comp.33 0 +motor.cogging_dq_comp.34 0 +motor.cogging_dq_comp.35 0 +motor.cogging_dq_comp.36 0 +motor.cogging_dq_comp.37 0 +motor.cogging_dq_comp.38 0 +motor.cogging_dq_comp.39 0 +motor.cogging_dq_comp.40 0 +motor.cogging_dq_comp.41 0 +motor.cogging_dq_comp.42 0 +motor.cogging_dq_comp.43 0 +motor.cogging_dq_comp.44 0 +motor.cogging_dq_comp.45 0 +motor.cogging_dq_comp.46 0 +motor.cogging_dq_comp.47 0 +motor.cogging_dq_comp.48 0 +motor.cogging_dq_comp.49 0 +motor.cogging_dq_comp.50 0 +motor.cogging_dq_comp.51 0 +motor.cogging_dq_comp.52 0 +motor.cogging_dq_comp.53 0 +motor.cogging_dq_comp.54 0 +motor.cogging_dq_comp.55 0 +motor.cogging_dq_comp.56 0 +motor.cogging_dq_comp.57 0 +motor.cogging_dq_comp.58 0 +motor.cogging_dq_comp.59 0 +motor.cogging_dq_comp.60 0 +motor.cogging_dq_comp.61 0 +motor.cogging_dq_comp.62 0 +motor.cogging_dq_comp.63 0 +motor.cogging_dq_comp.64 0 +motor.cogging_dq_comp.65 0 +motor.cogging_dq_comp.66 0 +motor.cogging_dq_comp.67 0 +motor.cogging_dq_comp.68 0 +motor.cogging_dq_comp.69 0 +motor.cogging_dq_comp.70 0 +motor.cogging_dq_comp.71 0 +motor.cogging_dq_comp.72 0 +motor.cogging_dq_comp.73 0 +motor.cogging_dq_comp.74 0 +motor.cogging_dq_comp.75 0 +motor.cogging_dq_comp.76 0 +motor.cogging_dq_comp.77 0 +motor.cogging_dq_comp.78 0 +motor.cogging_dq_comp.79 0 +motor.cogging_dq_comp.80 0 +motor.cogging_dq_comp.81 0 +motor.cogging_dq_comp.82 0 +motor.cogging_dq_comp.83 0 +motor.cogging_dq_comp.84 0 +motor.cogging_dq_comp.85 0 +motor.cogging_dq_comp.86 0 +motor.cogging_dq_comp.87 0 +motor.cogging_dq_comp.88 0 +motor.cogging_dq_comp.89 0 +motor.cogging_dq_comp.90 0 +motor.cogging_dq_comp.91 0 +motor.cogging_dq_comp.92 0 +motor.cogging_dq_comp.93 0 +motor.cogging_dq_comp.94 0 +motor.cogging_dq_comp.95 0 +motor.cogging_dq_comp.96 0 +motor.cogging_dq_comp.97 0 +motor.cogging_dq_comp.98 0 +motor.cogging_dq_comp.99 0 +motor.cogging_dq_comp.100 0 +motor.cogging_dq_comp.101 0 +motor.cogging_dq_comp.102 0 +motor.cogging_dq_comp.103 0 +motor.cogging_dq_comp.104 0 +motor.cogging_dq_comp.105 0 +motor.cogging_dq_comp.106 0 +motor.cogging_dq_comp.107 0 +motor.cogging_dq_comp.108 0 +motor.cogging_dq_comp.109 0 +motor.cogging_dq_comp.110 0 +motor.cogging_dq_comp.111 0 +motor.cogging_dq_comp.112 0 +motor.cogging_dq_comp.113 0 +motor.cogging_dq_comp.114 0 +motor.cogging_dq_comp.115 0 +motor.cogging_dq_comp.116 0 +motor.cogging_dq_comp.117 0 +motor.cogging_dq_comp.118 0 +motor.cogging_dq_comp.119 0 +motor.cogging_dq_comp.120 0 +motor.cogging_dq_comp.121 0 +motor.cogging_dq_comp.122 0 +motor.cogging_dq_comp.123 0 +motor.cogging_dq_comp.124 0 +motor.cogging_dq_comp.125 0 +motor.cogging_dq_comp.126 0 +motor.cogging_dq_comp.127 0 +motor.cogging_dq_comp.128 0 +motor.cogging_dq_comp.129 0 +motor.cogging_dq_comp.130 0 +motor.cogging_dq_comp.131 0 +motor.cogging_dq_comp.132 0 +motor.cogging_dq_comp.133 0 +motor.cogging_dq_comp.134 0 +motor.cogging_dq_comp.135 0 +motor.cogging_dq_comp.136 0 +motor.cogging_dq_comp.137 0 +motor.cogging_dq_comp.138 0 +motor.cogging_dq_comp.139 0 +motor.cogging_dq_comp.140 0 +motor.cogging_dq_comp.141 0 +motor.cogging_dq_comp.142 0 +motor.cogging_dq_comp.143 0 +motor.cogging_dq_comp.144 0 +motor.cogging_dq_comp.145 0 +motor.cogging_dq_comp.146 0 +motor.cogging_dq_comp.147 0 +motor.cogging_dq_comp.148 0 +motor.cogging_dq_comp.149 0 +motor.cogging_dq_comp.150 0 +motor.cogging_dq_comp.151 0 +motor.cogging_dq_comp.152 0 +motor.cogging_dq_comp.153 0 +motor.cogging_dq_comp.154 0 +motor.cogging_dq_comp.155 0 +motor.cogging_dq_comp.156 0 +motor.cogging_dq_comp.157 0 +motor.cogging_dq_comp.158 0 +motor.cogging_dq_comp.159 0 +motor.cogging_dq_comp.160 0 +motor.cogging_dq_comp.161 0 +motor.cogging_dq_comp.162 0 +motor.cogging_dq_comp.163 0 +motor.cogging_dq_comp.164 0 +motor.cogging_dq_comp.165 0 +motor.cogging_dq_comp.166 0 +motor.cogging_dq_comp.167 0 +motor.cogging_dq_comp.168 0 +motor.cogging_dq_comp.169 0 +motor.cogging_dq_comp.170 0 +motor.cogging_dq_comp.171 0 +motor.cogging_dq_comp.172 0 +motor.cogging_dq_comp.173 0 +motor.cogging_dq_comp.174 0 +motor.cogging_dq_comp.175 0 +motor.cogging_dq_comp.176 0 +motor.cogging_dq_comp.177 0 +motor.cogging_dq_comp.178 0 +motor.cogging_dq_comp.179 0 +motor.cogging_dq_comp.180 0 +motor.cogging_dq_comp.181 0 +motor.cogging_dq_comp.182 0 +motor.cogging_dq_comp.183 0 +motor.cogging_dq_comp.184 0 +motor.cogging_dq_comp.185 0 +motor.cogging_dq_comp.186 0 +motor.cogging_dq_comp.187 0 +motor.cogging_dq_comp.188 0 +motor.cogging_dq_comp.189 0 +motor.cogging_dq_comp.190 0 +motor.cogging_dq_comp.191 0 +motor.cogging_dq_comp.192 0 +motor.cogging_dq_comp.193 0 +motor.cogging_dq_comp.194 0 +motor.cogging_dq_comp.195 0 +motor.cogging_dq_comp.196 0 +motor.cogging_dq_comp.197 0 +motor.cogging_dq_comp.198 0 +motor.cogging_dq_comp.199 0 +motor.cogging_dq_comp.200 0 +motor.cogging_dq_comp.201 0 +motor.cogging_dq_comp.202 0 +motor.cogging_dq_comp.203 0 +motor.cogging_dq_comp.204 0 +motor.cogging_dq_comp.205 0 +motor.cogging_dq_comp.206 0 +motor.cogging_dq_comp.207 0 +motor.cogging_dq_comp.208 0 +motor.cogging_dq_comp.209 0 +motor.cogging_dq_comp.210 0 +motor.cogging_dq_comp.211 0 +motor.cogging_dq_comp.212 0 +motor.cogging_dq_comp.213 0 +motor.cogging_dq_comp.214 0 +motor.cogging_dq_comp.215 0 +motor.cogging_dq_comp.216 0 +motor.cogging_dq_comp.217 0 +motor.cogging_dq_comp.218 0 +motor.cogging_dq_comp.219 0 +motor.cogging_dq_comp.220 0 +motor.cogging_dq_comp.221 0 +motor.cogging_dq_comp.222 0 +motor.cogging_dq_comp.223 0 +motor.cogging_dq_comp.224 0 +motor.cogging_dq_comp.225 0 +motor.cogging_dq_comp.226 0 +motor.cogging_dq_comp.227 0 +motor.cogging_dq_comp.228 0 +motor.cogging_dq_comp.229 0 +motor.cogging_dq_comp.230 0 +motor.cogging_dq_comp.231 0 +motor.cogging_dq_comp.232 0 +motor.cogging_dq_comp.233 0 +motor.cogging_dq_comp.234 0 +motor.cogging_dq_comp.235 0 +motor.cogging_dq_comp.236 0 +motor.cogging_dq_comp.237 0 +motor.cogging_dq_comp.238 0 +motor.cogging_dq_comp.239 0 +motor.cogging_dq_comp.240 0 +motor.cogging_dq_comp.241 0 +motor.cogging_dq_comp.242 0 +motor.cogging_dq_comp.243 0 +motor.cogging_dq_comp.244 0 +motor.cogging_dq_comp.245 0 +motor.cogging_dq_comp.246 0 +motor.cogging_dq_comp.247 0 +motor.cogging_dq_comp.248 0 +motor.cogging_dq_comp.249 0 +motor.cogging_dq_comp.250 0 +motor.cogging_dq_comp.251 0 +motor.cogging_dq_comp.252 0 +motor.cogging_dq_comp.253 0 +motor.cogging_dq_comp.254 0 +motor.cogging_dq_comp.255 0 +motor.cogging_dq_comp.256 0 +motor.cogging_dq_comp.257 0 +motor.cogging_dq_comp.258 0 +motor.cogging_dq_comp.259 0 +motor.cogging_dq_comp.260 0 +motor.cogging_dq_comp.261 0 +motor.cogging_dq_comp.262 0 +motor.cogging_dq_comp.263 0 +motor.cogging_dq_comp.264 0 +motor.cogging_dq_comp.265 0 +motor.cogging_dq_comp.266 0 +motor.cogging_dq_comp.267 0 +motor.cogging_dq_comp.268 0 +motor.cogging_dq_comp.269 0 +motor.cogging_dq_comp.270 0 +motor.cogging_dq_comp.271 0 +motor.cogging_dq_comp.272 0 +motor.cogging_dq_comp.273 0 +motor.cogging_dq_comp.274 0 +motor.cogging_dq_comp.275 0 +motor.cogging_dq_comp.276 0 +motor.cogging_dq_comp.277 0 +motor.cogging_dq_comp.278 0 +motor.cogging_dq_comp.279 0 +motor.cogging_dq_comp.280 0 +motor.cogging_dq_comp.281 0 +motor.cogging_dq_comp.282 0 +motor.cogging_dq_comp.283 0 +motor.cogging_dq_comp.284 0 +motor.cogging_dq_comp.285 0 +motor.cogging_dq_comp.286 0 +motor.cogging_dq_comp.287 0 +motor.cogging_dq_comp.288 0 +motor.cogging_dq_comp.289 0 +motor.cogging_dq_comp.290 0 +motor.cogging_dq_comp.291 0 +motor.cogging_dq_comp.292 0 +motor.cogging_dq_comp.293 0 +motor.cogging_dq_comp.294 0 +motor.cogging_dq_comp.295 0 +motor.cogging_dq_comp.296 0 +motor.cogging_dq_comp.297 0 +motor.cogging_dq_comp.298 0 +motor.cogging_dq_comp.299 0 +motor.cogging_dq_comp.300 0 +motor.cogging_dq_comp.301 0 +motor.cogging_dq_comp.302 0 +motor.cogging_dq_comp.303 0 +motor.cogging_dq_comp.304 0 +motor.cogging_dq_comp.305 0 +motor.cogging_dq_comp.306 0 +motor.cogging_dq_comp.307 0 +motor.cogging_dq_comp.308 0 +motor.cogging_dq_comp.309 0 +motor.cogging_dq_comp.310 0 +motor.cogging_dq_comp.311 0 +motor.cogging_dq_comp.312 0 +motor.cogging_dq_comp.313 0 +motor.cogging_dq_comp.314 0 +motor.cogging_dq_comp.315 0 +motor.cogging_dq_comp.316 0 +motor.cogging_dq_comp.317 0 +motor.cogging_dq_comp.318 0 +motor.cogging_dq_comp.319 0 +motor.cogging_dq_comp.320 0 +motor.cogging_dq_comp.321 0 +motor.cogging_dq_comp.322 0 +motor.cogging_dq_comp.323 0 +motor.cogging_dq_comp.324 0 +motor.cogging_dq_comp.325 0 +motor.cogging_dq_comp.326 0 +motor.cogging_dq_comp.327 0 +motor.cogging_dq_comp.328 0 +motor.cogging_dq_comp.329 0 +motor.cogging_dq_comp.330 0 +motor.cogging_dq_comp.331 0 +motor.cogging_dq_comp.332 0 +motor.cogging_dq_comp.333 0 +motor.cogging_dq_comp.334 0 +motor.cogging_dq_comp.335 0 +motor.cogging_dq_comp.336 0 +motor.cogging_dq_comp.337 0 +motor.cogging_dq_comp.338 0 +motor.cogging_dq_comp.339 0 +motor.cogging_dq_comp.340 0 +motor.cogging_dq_comp.341 0 +motor.cogging_dq_comp.342 0 +motor.cogging_dq_comp.343 0 +motor.cogging_dq_comp.344 0 +motor.cogging_dq_comp.345 0 +motor.cogging_dq_comp.346 0 +motor.cogging_dq_comp.347 0 +motor.cogging_dq_comp.348 0 +motor.cogging_dq_comp.349 0 +motor.cogging_dq_comp.350 0 +motor.cogging_dq_comp.351 0 +motor.cogging_dq_comp.352 0 +motor.cogging_dq_comp.353 0 +motor.cogging_dq_comp.354 0 +motor.cogging_dq_comp.355 0 +motor.cogging_dq_comp.356 0 +motor.cogging_dq_comp.357 0 +motor.cogging_dq_comp.358 0 +motor.cogging_dq_comp.359 0 +motor.cogging_dq_comp.360 0 +motor.cogging_dq_comp.361 0 +motor.cogging_dq_comp.362 0 +motor.cogging_dq_comp.363 0 +motor.cogging_dq_comp.364 0 +motor.cogging_dq_comp.365 0 +motor.cogging_dq_comp.366 0 +motor.cogging_dq_comp.367 0 +motor.cogging_dq_comp.368 0 +motor.cogging_dq_comp.369 0 +motor.cogging_dq_comp.370 0 +motor.cogging_dq_comp.371 0 +motor.cogging_dq_comp.372 0 +motor.cogging_dq_comp.373 0 +motor.cogging_dq_comp.374 0 +motor.cogging_dq_comp.375 0 +motor.cogging_dq_comp.376 0 +motor.cogging_dq_comp.377 0 +motor.cogging_dq_comp.378 0 +motor.cogging_dq_comp.379 0 +motor.cogging_dq_comp.380 0 +motor.cogging_dq_comp.381 0 +motor.cogging_dq_comp.382 0 +motor.cogging_dq_comp.383 0 +motor.cogging_dq_comp.384 0 +motor.cogging_dq_comp.385 0 +motor.cogging_dq_comp.386 0 +motor.cogging_dq_comp.387 0 +motor.cogging_dq_comp.388 0 +motor.cogging_dq_comp.389 0 +motor.cogging_dq_comp.390 0 +motor.cogging_dq_comp.391 0 +motor.cogging_dq_comp.392 0 +motor.cogging_dq_comp.393 0 +motor.cogging_dq_comp.394 0 +motor.cogging_dq_comp.395 0 +motor.cogging_dq_comp.396 0 +motor.cogging_dq_comp.397 0 +motor.cogging_dq_comp.398 0 +motor.cogging_dq_comp.399 0 +motor.cogging_dq_comp.400 0 +motor.cogging_dq_comp.401 0 +motor.cogging_dq_comp.402 0 +motor.cogging_dq_comp.403 0 +motor.cogging_dq_comp.404 0 +motor.cogging_dq_comp.405 0 +motor.cogging_dq_comp.406 0 +motor.cogging_dq_comp.407 0 +motor.cogging_dq_comp.408 0 +motor.cogging_dq_comp.409 0 +motor.cogging_dq_comp.410 0 +motor.cogging_dq_comp.411 0 +motor.cogging_dq_comp.412 0 +motor.cogging_dq_comp.413 0 +motor.cogging_dq_comp.414 0 +motor.cogging_dq_comp.415 0 +motor.cogging_dq_comp.416 0 +motor.cogging_dq_comp.417 0 +motor.cogging_dq_comp.418 0 +motor.cogging_dq_comp.419 0 +motor.cogging_dq_comp.420 0 +motor.cogging_dq_comp.421 0 +motor.cogging_dq_comp.422 0 +motor.cogging_dq_comp.423 0 +motor.cogging_dq_comp.424 0 +motor.cogging_dq_comp.425 0 +motor.cogging_dq_comp.426 0 +motor.cogging_dq_comp.427 0 +motor.cogging_dq_comp.428 0 +motor.cogging_dq_comp.429 0 +motor.cogging_dq_comp.430 0 +motor.cogging_dq_comp.431 0 +motor.cogging_dq_comp.432 0 +motor.cogging_dq_comp.433 0 +motor.cogging_dq_comp.434 0 +motor.cogging_dq_comp.435 0 +motor.cogging_dq_comp.436 0 +motor.cogging_dq_comp.437 0 +motor.cogging_dq_comp.438 0 +motor.cogging_dq_comp.439 0 +motor.cogging_dq_comp.440 0 +motor.cogging_dq_comp.441 0 +motor.cogging_dq_comp.442 0 +motor.cogging_dq_comp.443 0 +motor.cogging_dq_comp.444 0 +motor.cogging_dq_comp.445 0 +motor.cogging_dq_comp.446 0 +motor.cogging_dq_comp.447 0 +motor.cogging_dq_comp.448 0 +motor.cogging_dq_comp.449 0 +motor.cogging_dq_comp.450 0 +motor.cogging_dq_comp.451 0 +motor.cogging_dq_comp.452 0 +motor.cogging_dq_comp.453 0 +motor.cogging_dq_comp.454 0 +motor.cogging_dq_comp.455 0 +motor.cogging_dq_comp.456 0 +motor.cogging_dq_comp.457 0 +motor.cogging_dq_comp.458 0 +motor.cogging_dq_comp.459 0 +motor.cogging_dq_comp.460 0 +motor.cogging_dq_comp.461 0 +motor.cogging_dq_comp.462 0 +motor.cogging_dq_comp.463 0 +motor.cogging_dq_comp.464 0 +motor.cogging_dq_comp.465 0 +motor.cogging_dq_comp.466 0 +motor.cogging_dq_comp.467 0 +motor.cogging_dq_comp.468 0 +motor.cogging_dq_comp.469 0 +motor.cogging_dq_comp.470 0 +motor.cogging_dq_comp.471 0 +motor.cogging_dq_comp.472 0 +motor.cogging_dq_comp.473 0 +motor.cogging_dq_comp.474 0 +motor.cogging_dq_comp.475 0 +motor.cogging_dq_comp.476 0 +motor.cogging_dq_comp.477 0 +motor.cogging_dq_comp.478 0 +motor.cogging_dq_comp.479 0 +motor.cogging_dq_comp.480 0 +motor.cogging_dq_comp.481 0 +motor.cogging_dq_comp.482 0 +motor.cogging_dq_comp.483 0 +motor.cogging_dq_comp.484 0 +motor.cogging_dq_comp.485 0 +motor.cogging_dq_comp.486 0 +motor.cogging_dq_comp.487 0 +motor.cogging_dq_comp.488 0 +motor.cogging_dq_comp.489 0 +motor.cogging_dq_comp.490 0 +motor.cogging_dq_comp.491 0 +motor.cogging_dq_comp.492 0 +motor.cogging_dq_comp.493 0 +motor.cogging_dq_comp.494 0 +motor.cogging_dq_comp.495 0 +motor.cogging_dq_comp.496 0 +motor.cogging_dq_comp.497 0 +motor.cogging_dq_comp.498 0 +motor.cogging_dq_comp.499 0 +motor.cogging_dq_comp.500 0 +motor.cogging_dq_comp.501 0 +motor.cogging_dq_comp.502 0 +motor.cogging_dq_comp.503 0 +motor.cogging_dq_comp.504 0 +motor.cogging_dq_comp.505 0 +motor.cogging_dq_comp.506 0 +motor.cogging_dq_comp.507 0 +motor.cogging_dq_comp.508 0 +motor.cogging_dq_comp.509 0 +motor.cogging_dq_comp.510 0 +motor.cogging_dq_comp.511 0 +motor.cogging_dq_comp.512 0 +motor.cogging_dq_comp.513 0 +motor.cogging_dq_comp.514 0 +motor.cogging_dq_comp.515 0 +motor.cogging_dq_comp.516 0 +motor.cogging_dq_comp.517 0 +motor.cogging_dq_comp.518 0 +motor.cogging_dq_comp.519 0 +motor.cogging_dq_comp.520 0 +motor.cogging_dq_comp.521 0 +motor.cogging_dq_comp.522 0 +motor.cogging_dq_comp.523 0 +motor.cogging_dq_comp.524 0 +motor.cogging_dq_comp.525 0 +motor.cogging_dq_comp.526 0 +motor.cogging_dq_comp.527 0 +motor.cogging_dq_comp.528 0 +motor.cogging_dq_comp.529 0 +motor.cogging_dq_comp.530 0 +motor.cogging_dq_comp.531 0 +motor.cogging_dq_comp.532 0 +motor.cogging_dq_comp.533 0 +motor.cogging_dq_comp.534 0 +motor.cogging_dq_comp.535 0 +motor.cogging_dq_comp.536 0 +motor.cogging_dq_comp.537 0 +motor.cogging_dq_comp.538 0 +motor.cogging_dq_comp.539 0 +motor.cogging_dq_comp.540 0 +motor.cogging_dq_comp.541 0 +motor.cogging_dq_comp.542 0 +motor.cogging_dq_comp.543 0 +motor.cogging_dq_comp.544 0 +motor.cogging_dq_comp.545 0 +motor.cogging_dq_comp.546 0 +motor.cogging_dq_comp.547 0 +motor.cogging_dq_comp.548 0 +motor.cogging_dq_comp.549 0 +motor.cogging_dq_comp.550 0 +motor.cogging_dq_comp.551 0 +motor.cogging_dq_comp.552 0 +motor.cogging_dq_comp.553 0 +motor.cogging_dq_comp.554 0 +motor.cogging_dq_comp.555 0 +motor.cogging_dq_comp.556 0 +motor.cogging_dq_comp.557 0 +motor.cogging_dq_comp.558 0 +motor.cogging_dq_comp.559 0 +motor.cogging_dq_comp.560 0 +motor.cogging_dq_comp.561 0 +motor.cogging_dq_comp.562 0 +motor.cogging_dq_comp.563 0 +motor.cogging_dq_comp.564 0 +motor.cogging_dq_comp.565 0 +motor.cogging_dq_comp.566 0 +motor.cogging_dq_comp.567 0 +motor.cogging_dq_comp.568 0 +motor.cogging_dq_comp.569 0 +motor.cogging_dq_comp.570 0 +motor.cogging_dq_comp.571 0 +motor.cogging_dq_comp.572 0 +motor.cogging_dq_comp.573 0 +motor.cogging_dq_comp.574 0 +motor.cogging_dq_comp.575 0 +motor.cogging_dq_comp.576 0 +motor.cogging_dq_comp.577 0 +motor.cogging_dq_comp.578 0 +motor.cogging_dq_comp.579 0 +motor.cogging_dq_comp.580 0 +motor.cogging_dq_comp.581 0 +motor.cogging_dq_comp.582 0 +motor.cogging_dq_comp.583 0 +motor.cogging_dq_comp.584 0 +motor.cogging_dq_comp.585 0 +motor.cogging_dq_comp.586 0 +motor.cogging_dq_comp.587 0 +motor.cogging_dq_comp.588 0 +motor.cogging_dq_comp.589 0 +motor.cogging_dq_comp.590 0 +motor.cogging_dq_comp.591 0 +motor.cogging_dq_comp.592 0 +motor.cogging_dq_comp.593 0 +motor.cogging_dq_comp.594 0 +motor.cogging_dq_comp.595 0 +motor.cogging_dq_comp.596 0 +motor.cogging_dq_comp.597 0 +motor.cogging_dq_comp.598 0 +motor.cogging_dq_comp.599 0 +motor.cogging_dq_comp.600 0 +motor.cogging_dq_comp.601 0 +motor.cogging_dq_comp.602 0 +motor.cogging_dq_comp.603 0 +motor.cogging_dq_comp.604 0 +motor.cogging_dq_comp.605 0 +motor.cogging_dq_comp.606 0 +motor.cogging_dq_comp.607 0 +motor.cogging_dq_comp.608 0 +motor.cogging_dq_comp.609 0 +motor.cogging_dq_comp.610 0 +motor.cogging_dq_comp.611 0 +motor.cogging_dq_comp.612 0 +motor.cogging_dq_comp.613 0 +motor.cogging_dq_comp.614 0 +motor.cogging_dq_comp.615 0 +motor.cogging_dq_comp.616 0 +motor.cogging_dq_comp.617 0 +motor.cogging_dq_comp.618 0 +motor.cogging_dq_comp.619 0 +motor.cogging_dq_comp.620 0 +motor.cogging_dq_comp.621 0 +motor.cogging_dq_comp.622 0 +motor.cogging_dq_comp.623 0 +motor.cogging_dq_comp.624 0 +motor.cogging_dq_comp.625 0 +motor.cogging_dq_comp.626 0 +motor.cogging_dq_comp.627 0 +motor.cogging_dq_comp.628 0 +motor.cogging_dq_comp.629 0 +motor.cogging_dq_comp.630 0 +motor.cogging_dq_comp.631 0 +motor.cogging_dq_comp.632 0 +motor.cogging_dq_comp.633 0 +motor.cogging_dq_comp.634 0 +motor.cogging_dq_comp.635 0 +motor.cogging_dq_comp.636 0 +motor.cogging_dq_comp.637 0 +motor.cogging_dq_comp.638 0 +motor.cogging_dq_comp.639 0 +motor.cogging_dq_comp.640 0 +motor.cogging_dq_comp.641 0 +motor.cogging_dq_comp.642 0 +motor.cogging_dq_comp.643 0 +motor.cogging_dq_comp.644 0 +motor.cogging_dq_comp.645 0 +motor.cogging_dq_comp.646 0 +motor.cogging_dq_comp.647 0 +motor.cogging_dq_comp.648 0 +motor.cogging_dq_comp.649 0 +motor.cogging_dq_comp.650 0 +motor.cogging_dq_comp.651 0 +motor.cogging_dq_comp.652 0 +motor.cogging_dq_comp.653 0 +motor.cogging_dq_comp.654 0 +motor.cogging_dq_comp.655 0 +motor.cogging_dq_comp.656 0 +motor.cogging_dq_comp.657 0 +motor.cogging_dq_comp.658 0 +motor.cogging_dq_comp.659 0 +motor.cogging_dq_comp.660 0 +motor.cogging_dq_comp.661 0 +motor.cogging_dq_comp.662 0 +motor.cogging_dq_comp.663 0 +motor.cogging_dq_comp.664 0 +motor.cogging_dq_comp.665 0 +motor.cogging_dq_comp.666 0 +motor.cogging_dq_comp.667 0 +motor.cogging_dq_comp.668 0 +motor.cogging_dq_comp.669 0 +motor.cogging_dq_comp.670 0 +motor.cogging_dq_comp.671 0 +motor.cogging_dq_comp.672 0 +motor.cogging_dq_comp.673 0 +motor.cogging_dq_comp.674 0 +motor.cogging_dq_comp.675 0 +motor.cogging_dq_comp.676 0 +motor.cogging_dq_comp.677 0 +motor.cogging_dq_comp.678 0 +motor.cogging_dq_comp.679 0 +motor.cogging_dq_comp.680 0 +motor.cogging_dq_comp.681 0 +motor.cogging_dq_comp.682 0 +motor.cogging_dq_comp.683 0 +motor.cogging_dq_comp.684 0 +motor.cogging_dq_comp.685 0 +motor.cogging_dq_comp.686 0 +motor.cogging_dq_comp.687 0 +motor.cogging_dq_comp.688 0 +motor.cogging_dq_comp.689 0 +motor.cogging_dq_comp.690 0 +motor.cogging_dq_comp.691 0 +motor.cogging_dq_comp.692 0 +motor.cogging_dq_comp.693 0 +motor.cogging_dq_comp.694 0 +motor.cogging_dq_comp.695 0 +motor.cogging_dq_comp.696 0 +motor.cogging_dq_comp.697 0 +motor.cogging_dq_comp.698 0 +motor.cogging_dq_comp.699 0 +motor.cogging_dq_comp.700 0 +motor.cogging_dq_comp.701 0 +motor.cogging_dq_comp.702 0 +motor.cogging_dq_comp.703 0 +motor.cogging_dq_comp.704 0 +motor.cogging_dq_comp.705 0 +motor.cogging_dq_comp.706 0 +motor.cogging_dq_comp.707 0 +motor.cogging_dq_comp.708 0 +motor.cogging_dq_comp.709 0 +motor.cogging_dq_comp.710 0 +motor.cogging_dq_comp.711 0 +motor.cogging_dq_comp.712 0 +motor.cogging_dq_comp.713 0 +motor.cogging_dq_comp.714 0 +motor.cogging_dq_comp.715 0 +motor.cogging_dq_comp.716 0 +motor.cogging_dq_comp.717 0 +motor.cogging_dq_comp.718 0 +motor.cogging_dq_comp.719 0 +motor.cogging_dq_comp.720 0 +motor.cogging_dq_comp.721 0 +motor.cogging_dq_comp.722 0 +motor.cogging_dq_comp.723 0 +motor.cogging_dq_comp.724 0 +motor.cogging_dq_comp.725 0 +motor.cogging_dq_comp.726 0 +motor.cogging_dq_comp.727 0 +motor.cogging_dq_comp.728 0 +motor.cogging_dq_comp.729 0 +motor.cogging_dq_comp.730 0 +motor.cogging_dq_comp.731 0 +motor.cogging_dq_comp.732 0 +motor.cogging_dq_comp.733 0 +motor.cogging_dq_comp.734 0 +motor.cogging_dq_comp.735 0 +motor.cogging_dq_comp.736 0 +motor.cogging_dq_comp.737 0 +motor.cogging_dq_comp.738 0 +motor.cogging_dq_comp.739 0 +motor.cogging_dq_comp.740 0 +motor.cogging_dq_comp.741 0 +motor.cogging_dq_comp.742 0 +motor.cogging_dq_comp.743 0 +motor.cogging_dq_comp.744 0 +motor.cogging_dq_comp.745 0 +motor.cogging_dq_comp.746 0 +motor.cogging_dq_comp.747 0 +motor.cogging_dq_comp.748 0 +motor.cogging_dq_comp.749 0 +motor.cogging_dq_comp.750 0 +motor.cogging_dq_comp.751 0 +motor.cogging_dq_comp.752 0 +motor.cogging_dq_comp.753 0 +motor.cogging_dq_comp.754 0 +motor.cogging_dq_comp.755 0 +motor.cogging_dq_comp.756 0 +motor.cogging_dq_comp.757 0 +motor.cogging_dq_comp.758 0 +motor.cogging_dq_comp.759 0 +motor.cogging_dq_comp.760 0 +motor.cogging_dq_comp.761 0 +motor.cogging_dq_comp.762 0 +motor.cogging_dq_comp.763 0 +motor.cogging_dq_comp.764 0 +motor.cogging_dq_comp.765 0 +motor.cogging_dq_comp.766 0 +motor.cogging_dq_comp.767 0 +motor.cogging_dq_comp.768 0 +motor.cogging_dq_comp.769 0 +motor.cogging_dq_comp.770 0 +motor.cogging_dq_comp.771 0 +motor.cogging_dq_comp.772 0 +motor.cogging_dq_comp.773 0 +motor.cogging_dq_comp.774 0 +motor.cogging_dq_comp.775 0 +motor.cogging_dq_comp.776 0 +motor.cogging_dq_comp.777 0 +motor.cogging_dq_comp.778 0 +motor.cogging_dq_comp.779 0 +motor.cogging_dq_comp.780 0 +motor.cogging_dq_comp.781 0 +motor.cogging_dq_comp.782 0 +motor.cogging_dq_comp.783 0 +motor.cogging_dq_comp.784 0 +motor.cogging_dq_comp.785 0 +motor.cogging_dq_comp.786 0 +motor.cogging_dq_comp.787 0 +motor.cogging_dq_comp.788 0 +motor.cogging_dq_comp.789 0 +motor.cogging_dq_comp.790 0 +motor.cogging_dq_comp.791 0 +motor.cogging_dq_comp.792 0 +motor.cogging_dq_comp.793 0 +motor.cogging_dq_comp.794 0 +motor.cogging_dq_comp.795 0 +motor.cogging_dq_comp.796 0 +motor.cogging_dq_comp.797 0 +motor.cogging_dq_comp.798 0 +motor.cogging_dq_comp.799 0 +motor.cogging_dq_comp.800 0 +motor.cogging_dq_comp.801 0 +motor.cogging_dq_comp.802 0 +motor.cogging_dq_comp.803 0 +motor.cogging_dq_comp.804 0 +motor.cogging_dq_comp.805 0 +motor.cogging_dq_comp.806 0 +motor.cogging_dq_comp.807 0 +motor.cogging_dq_comp.808 0 +motor.cogging_dq_comp.809 0 +motor.cogging_dq_comp.810 0 +motor.cogging_dq_comp.811 0 +motor.cogging_dq_comp.812 0 +motor.cogging_dq_comp.813 0 +motor.cogging_dq_comp.814 0 +motor.cogging_dq_comp.815 0 +motor.cogging_dq_comp.816 0 +motor.cogging_dq_comp.817 0 +motor.cogging_dq_comp.818 0 +motor.cogging_dq_comp.819 0 +motor.cogging_dq_comp.820 0 +motor.cogging_dq_comp.821 0 +motor.cogging_dq_comp.822 0 +motor.cogging_dq_comp.823 0 +motor.cogging_dq_comp.824 0 +motor.cogging_dq_comp.825 0 +motor.cogging_dq_comp.826 0 +motor.cogging_dq_comp.827 0 +motor.cogging_dq_comp.828 0 +motor.cogging_dq_comp.829 0 +motor.cogging_dq_comp.830 0 +motor.cogging_dq_comp.831 0 +motor.cogging_dq_comp.832 0 +motor.cogging_dq_comp.833 0 +motor.cogging_dq_comp.834 0 +motor.cogging_dq_comp.835 0 +motor.cogging_dq_comp.836 0 +motor.cogging_dq_comp.837 0 +motor.cogging_dq_comp.838 0 +motor.cogging_dq_comp.839 0 +motor.cogging_dq_comp.840 0 +motor.cogging_dq_comp.841 0 +motor.cogging_dq_comp.842 0 +motor.cogging_dq_comp.843 0 +motor.cogging_dq_comp.844 0 +motor.cogging_dq_comp.845 0 +motor.cogging_dq_comp.846 0 +motor.cogging_dq_comp.847 0 +motor.cogging_dq_comp.848 0 +motor.cogging_dq_comp.849 0 +motor.cogging_dq_comp.850 0 +motor.cogging_dq_comp.851 0 +motor.cogging_dq_comp.852 0 +motor.cogging_dq_comp.853 0 +motor.cogging_dq_comp.854 0 +motor.cogging_dq_comp.855 0 +motor.cogging_dq_comp.856 0 +motor.cogging_dq_comp.857 0 +motor.cogging_dq_comp.858 0 +motor.cogging_dq_comp.859 0 +motor.cogging_dq_comp.860 0 +motor.cogging_dq_comp.861 0 +motor.cogging_dq_comp.862 0 +motor.cogging_dq_comp.863 0 +motor.cogging_dq_comp.864 0 +motor.cogging_dq_comp.865 0 +motor.cogging_dq_comp.866 0 +motor.cogging_dq_comp.867 0 +motor.cogging_dq_comp.868 0 +motor.cogging_dq_comp.869 0 +motor.cogging_dq_comp.870 0 +motor.cogging_dq_comp.871 0 +motor.cogging_dq_comp.872 0 +motor.cogging_dq_comp.873 0 +motor.cogging_dq_comp.874 0 +motor.cogging_dq_comp.875 0 +motor.cogging_dq_comp.876 0 +motor.cogging_dq_comp.877 0 +motor.cogging_dq_comp.878 0 +motor.cogging_dq_comp.879 0 +motor.cogging_dq_comp.880 0 +motor.cogging_dq_comp.881 0 +motor.cogging_dq_comp.882 0 +motor.cogging_dq_comp.883 0 +motor.cogging_dq_comp.884 0 +motor.cogging_dq_comp.885 0 +motor.cogging_dq_comp.886 0 +motor.cogging_dq_comp.887 0 +motor.cogging_dq_comp.888 0 +motor.cogging_dq_comp.889 0 +motor.cogging_dq_comp.890 0 +motor.cogging_dq_comp.891 0 +motor.cogging_dq_comp.892 0 +motor.cogging_dq_comp.893 0 +motor.cogging_dq_comp.894 0 +motor.cogging_dq_comp.895 0 +motor.cogging_dq_comp.896 0 +motor.cogging_dq_comp.897 0 +motor.cogging_dq_comp.898 0 +motor.cogging_dq_comp.899 0 +motor.cogging_dq_comp.900 0 +motor.cogging_dq_comp.901 0 +motor.cogging_dq_comp.902 0 +motor.cogging_dq_comp.903 0 +motor.cogging_dq_comp.904 0 +motor.cogging_dq_comp.905 0 +motor.cogging_dq_comp.906 0 +motor.cogging_dq_comp.907 0 +motor.cogging_dq_comp.908 0 +motor.cogging_dq_comp.909 0 +motor.cogging_dq_comp.910 0 +motor.cogging_dq_comp.911 0 +motor.cogging_dq_comp.912 0 +motor.cogging_dq_comp.913 0 +motor.cogging_dq_comp.914 0 +motor.cogging_dq_comp.915 0 +motor.cogging_dq_comp.916 0 +motor.cogging_dq_comp.917 0 +motor.cogging_dq_comp.918 0 +motor.cogging_dq_comp.919 0 +motor.cogging_dq_comp.920 0 +motor.cogging_dq_comp.921 0 +motor.cogging_dq_comp.922 0 +motor.cogging_dq_comp.923 0 +motor.cogging_dq_comp.924 0 +motor.cogging_dq_comp.925 0 +motor.cogging_dq_comp.926 0 +motor.cogging_dq_comp.927 0 +motor.cogging_dq_comp.928 0 +motor.cogging_dq_comp.929 0 +motor.cogging_dq_comp.930 0 +motor.cogging_dq_comp.931 0 +motor.cogging_dq_comp.932 0 +motor.cogging_dq_comp.933 0 +motor.cogging_dq_comp.934 0 +motor.cogging_dq_comp.935 0 +motor.cogging_dq_comp.936 0 +motor.cogging_dq_comp.937 0 +motor.cogging_dq_comp.938 0 +motor.cogging_dq_comp.939 0 +motor.cogging_dq_comp.940 0 +motor.cogging_dq_comp.941 0 +motor.cogging_dq_comp.942 0 +motor.cogging_dq_comp.943 0 +motor.cogging_dq_comp.944 0 +motor.cogging_dq_comp.945 0 +motor.cogging_dq_comp.946 0 +motor.cogging_dq_comp.947 0 +motor.cogging_dq_comp.948 0 +motor.cogging_dq_comp.949 0 +motor.cogging_dq_comp.950 0 +motor.cogging_dq_comp.951 0 +motor.cogging_dq_comp.952 0 +motor.cogging_dq_comp.953 0 +motor.cogging_dq_comp.954 0 +motor.cogging_dq_comp.955 0 +motor.cogging_dq_comp.956 0 +motor.cogging_dq_comp.957 0 +motor.cogging_dq_comp.958 0 +motor.cogging_dq_comp.959 0 +motor.cogging_dq_comp.960 0 +motor.cogging_dq_comp.961 0 +motor.cogging_dq_comp.962 0 +motor.cogging_dq_comp.963 0 +motor.cogging_dq_comp.964 0 +motor.cogging_dq_comp.965 0 +motor.cogging_dq_comp.966 0 +motor.cogging_dq_comp.967 0 +motor.cogging_dq_comp.968 0 +motor.cogging_dq_comp.969 0 +motor.cogging_dq_comp.970 0 +motor.cogging_dq_comp.971 0 +motor.cogging_dq_comp.972 0 +motor.cogging_dq_comp.973 0 +motor.cogging_dq_comp.974 0 +motor.cogging_dq_comp.975 0 +motor.cogging_dq_comp.976 0 +motor.cogging_dq_comp.977 0 +motor.cogging_dq_comp.978 0 +motor.cogging_dq_comp.979 0 +motor.cogging_dq_comp.980 0 +motor.cogging_dq_comp.981 0 +motor.cogging_dq_comp.982 0 +motor.cogging_dq_comp.983 0 +motor.cogging_dq_comp.984 0 +motor.cogging_dq_comp.985 0 +motor.cogging_dq_comp.986 0 +motor.cogging_dq_comp.987 0 +motor.cogging_dq_comp.988 0 +motor.cogging_dq_comp.989 0 +motor.cogging_dq_comp.990 0 +motor.cogging_dq_comp.991 0 +motor.cogging_dq_comp.992 0 +motor.cogging_dq_comp.993 0 +motor.cogging_dq_comp.994 0 +motor.cogging_dq_comp.995 0 +motor.cogging_dq_comp.996 0 +motor.cogging_dq_comp.997 0 +motor.cogging_dq_comp.998 0 +motor.cogging_dq_comp.999 0 +motor.cogging_dq_comp.1000 0 +motor.cogging_dq_comp.1001 0 +motor.cogging_dq_comp.1002 0 +motor.cogging_dq_comp.1003 0 +motor.cogging_dq_comp.1004 0 +motor.cogging_dq_comp.1005 0 +motor.cogging_dq_comp.1006 0 +motor.cogging_dq_comp.1007 0 +motor.cogging_dq_comp.1008 0 +motor.cogging_dq_comp.1009 0 +motor.cogging_dq_comp.1010 0 +motor.cogging_dq_comp.1011 0 +motor.cogging_dq_comp.1012 0 +motor.cogging_dq_comp.1013 0 +motor.cogging_dq_comp.1014 0 +motor.cogging_dq_comp.1015 0 +motor.cogging_dq_comp.1016 0 +motor.cogging_dq_comp.1017 0 +motor.cogging_dq_comp.1018 0 +motor.cogging_dq_comp.1019 0 +motor.cogging_dq_comp.1020 0 +motor.cogging_dq_comp.1021 0 +motor.cogging_dq_comp.1022 0 +motor.cogging_dq_comp.1023 0 +drv8323_conf.dis_cpuv 0 +drv8323_conf.dis_gdf 0 +drv8323_conf.otw_rep 0 +drv8323_conf.pwm_mode 1 +drv8323_conf.pwm_1x_asynchronous 0 +drv8323_conf.pwm_1x_dir 0 +drv8323_conf.idrivep_hs_ma 150 +drv8323_conf.idriven_hs_ma 300 +drv8323_conf.cbc 1 +drv8323_conf.tdrive_ns 1000 +drv8323_conf.idrivep_ls_ma 150 +drv8323_conf.idriven_ls_ma 300 +drv8323_conf.tretry 0 +drv8323_conf.dead_time_ns 50 +drv8323_conf.ocp_mode 0 +drv8323_conf.ocp_deg_us 4 +drv8323_conf.vds_lvl_mv 700 +drv8323_conf.csa_fet 0 +drv8323_conf.vref_div 1 +drv8323_conf.ls_ref 0 +drv8323_conf.csa_gain 20 +drv8323_conf.dis_sen 0 +drv8323_conf.sen_lvl_mv 500 +servo.pwm_rate_hz 30000 +servo.i_gain 20.000000 +servo.current_sense_ohm 0.000500 +servo.pwm_comp_off 0.027000 +servo.pwm_comp_mag 0.005000 +servo.pwm_scale 1.000000 +servo.max_voltage 56.000000 +servo.max_power_W 450.000000 +servo.derate_temperature 50.000000 +servo.fault_temperature 75.000000 +servo.enable_motor_temperature 0 +servo.motor_derate_temperature 50.000000 +servo.motor_fault_temperature nan +servo.velocity_threshold 0.000000 +servo.position_derate 0.020000 +servo.adc_cur_cycles 2 +servo.adc_aux_cycles 47 +servo.pid_dq.kp 3.135870 +servo.pid_dq.ki 4214.477539 +servo.pid_position.kp 0.900000 +servo.pid_position.ki 0.000000 +servo.pid_position.kd 0.085000 +servo.pid_position.iratelimit -1.000000 +servo.pid_position.ilimit 0.000000 +servo.pid_position.max_desired_rate 0.000000 +servo.pid_position.sign -1 +servo.current_feedforward 1.000000 +servo.bemf_feedforward 1.000000 +servo.default_velocity_limit 5.000000 +servo.default_accel_limit 10.000000 +servo.voltage_mode_control 0 +servo.fixed_voltage_mode 0 +servo.fixed_voltage_control_V 0.000000 +servo.max_position_slip nan +servo.max_velocity_slip nan +servo.default_timeout_s 0.100000 +servo.timeout_max_torque_Nm 5.000000 +servo.timeout_mode 12 +servo.flux_brake_min_voltage 53.000000 +servo.flux_brake_resistance_ohm 0.025000 +servo.max_current_A 5.000000 +servo.derate_current_A -20.000000 +servo.max_velocity 10.000000 +servo.max_velocity_derate 2.000000 +servo.cooldown_cycles 256 +servo.velocity_zero_capture_threshold 0.050000 +servo.timing_fault 0 +servo.emit_debug 0 +servopos.position_min nan +servopos.position_max nan +id.id 36 +can.prefix 0 + diff --git a/config/moteus/middle_left.cfg b/config/moteus/middle_left.cfg new file mode 100644 index 000000000..6b0689bc5 --- /dev/null +++ b/config/moteus/middle_left.cfg @@ -0,0 +1,1397 @@ +uuid.uuid.0 251 +uuid.uuid.1 136 +uuid.uuid.2 255 +uuid.uuid.3 85 +uuid.uuid.4 115 +uuid.uuid.5 82 +uuid.uuid.6 71 +uuid.uuid.7 65 +uuid.uuid.8 167 +uuid.uuid.9 42 +uuid.uuid.10 202 +uuid.uuid.11 217 +uuid.uuid.12 60 +uuid.uuid.13 130 +uuid.uuid.14 211 +uuid.uuid.15 64 +clock.hsitrim 64 +aux1.i2c.i2c_hz 400000 +aux1.i2c.i2c_mode 1 +aux1.i2c.pullup 0 +aux1.i2c.devices.0.type 0 +aux1.i2c.devices.0.address 64 +aux1.i2c.devices.0.poll_ms 10 +aux1.i2c.devices.1.type 0 +aux1.i2c.devices.1.address 64 +aux1.i2c.devices.1.poll_ms 10 +aux1.i2c.devices.2.type 0 +aux1.i2c.devices.2.address 64 +aux1.i2c.devices.2.poll_ms 10 +aux1.spi.mode 1 +aux1.spi.rate_hz 12000000 +aux1.spi.filter_us 64 +aux1.spi.bct 0 +aux1.uart.mode 0 +aux1.uart.baud_rate 115200 +aux1.uart.poll_rate_us 100 +aux1.uart.rs422 0 +aux1.uart.cui_amt21_address 84 +aux1.quadrature.enabled 0 +aux1.quadrature.cpr 16384 +aux1.hall.enabled 1 +aux1.hall.polarity 0 +aux1.index.enabled 0 +aux1.sine_cosine.enabled 0 +aux1.sine_cosine.common 1700 +aux1.i2c_startup_delay_ms 30 +aux1.pins.0.mode 6 +aux1.pins.0.pull 1 +aux1.pins.1.mode 6 +aux1.pins.1.pull 1 +aux1.pins.2.mode 6 +aux1.pins.2.pull 1 +aux1.pins.3.mode 0 +aux1.pins.3.pull 0 +aux1.pins.4.mode 0 +aux1.pins.4.pull 0 +aux2.i2c.i2c_hz 400000 +aux2.i2c.i2c_mode 1 +aux2.i2c.pullup 0 +aux2.i2c.devices.0.type 0 +aux2.i2c.devices.0.address 64 +aux2.i2c.devices.0.poll_ms 10 +aux2.i2c.devices.1.type 0 +aux2.i2c.devices.1.address 64 +aux2.i2c.devices.1.poll_ms 10 +aux2.i2c.devices.2.type 0 +aux2.i2c.devices.2.address 64 +aux2.i2c.devices.2.poll_ms 10 +aux2.spi.mode 1 +aux2.spi.rate_hz 12000000 +aux2.spi.filter_us 64 +aux2.spi.bct 0 +aux2.uart.mode 0 +aux2.uart.baud_rate 115200 +aux2.uart.poll_rate_us 100 +aux2.uart.rs422 0 +aux2.uart.cui_amt21_address 84 +aux2.quadrature.enabled 0 +aux2.quadrature.cpr 16384 +aux2.hall.enabled 0 +aux2.hall.polarity 0 +aux2.index.enabled 0 +aux2.sine_cosine.enabled 0 +aux2.sine_cosine.common 1700 +aux2.i2c_startup_delay_ms 30 +aux2.pins.0.mode 0 +aux2.pins.0.pull 0 +aux2.pins.1.mode 0 +aux2.pins.1.pull 0 +aux2.pins.2.mode 0 +aux2.pins.2.pull 0 +aux2.pins.3.mode 0 +aux2.pins.3.pull 0 +aux2.pins.4.mode 0 +aux2.pins.4.pull 0 +motor_position.sources.0.aux_number 1 +motor_position.sources.0.type 4 +motor_position.sources.0.i2c_device 0 +motor_position.sources.0.incremental_index -1 +motor_position.sources.0.cpr 84 +motor_position.sources.0.offset -5.000000 +motor_position.sources.0.sign 1 +motor_position.sources.0.debug_override -1 +motor_position.sources.0.reference 0 +motor_position.sources.0.pll_filter_hz 65.305252 +motor_position.sources.0.compensation_table.0 0.000000 +motor_position.sources.0.compensation_table.1 0.000000 +motor_position.sources.0.compensation_table.2 0.000000 +motor_position.sources.0.compensation_table.3 0.000000 +motor_position.sources.0.compensation_table.4 0.000000 +motor_position.sources.0.compensation_table.5 0.000000 +motor_position.sources.0.compensation_table.6 0.000000 +motor_position.sources.0.compensation_table.7 0.000000 +motor_position.sources.0.compensation_table.8 0.000000 +motor_position.sources.0.compensation_table.9 0.000000 +motor_position.sources.0.compensation_table.10 0.000000 +motor_position.sources.0.compensation_table.11 0.000000 +motor_position.sources.0.compensation_table.12 0.000000 +motor_position.sources.0.compensation_table.13 0.000000 +motor_position.sources.0.compensation_table.14 0.000000 +motor_position.sources.0.compensation_table.15 0.000000 +motor_position.sources.0.compensation_table.16 0.000000 +motor_position.sources.0.compensation_table.17 0.000000 +motor_position.sources.0.compensation_table.18 0.000000 +motor_position.sources.0.compensation_table.19 0.000000 +motor_position.sources.0.compensation_table.20 0.000000 +motor_position.sources.0.compensation_table.21 0.000000 +motor_position.sources.0.compensation_table.22 0.000000 +motor_position.sources.0.compensation_table.23 0.000000 +motor_position.sources.0.compensation_table.24 0.000000 +motor_position.sources.0.compensation_table.25 0.000000 +motor_position.sources.0.compensation_table.26 0.000000 +motor_position.sources.0.compensation_table.27 0.000000 +motor_position.sources.0.compensation_table.28 0.000000 +motor_position.sources.0.compensation_table.29 0.000000 +motor_position.sources.0.compensation_table.30 0.000000 +motor_position.sources.0.compensation_table.31 0.000000 +motor_position.sources.1.aux_number 1 +motor_position.sources.1.type 0 +motor_position.sources.1.i2c_device 0 +motor_position.sources.1.incremental_index -1 +motor_position.sources.1.cpr 16384 +motor_position.sources.1.offset 0.000000 +motor_position.sources.1.sign 1 +motor_position.sources.1.debug_override -1 +motor_position.sources.1.reference 0 +motor_position.sources.1.pll_filter_hz 400.000000 +motor_position.sources.1.compensation_table.0 0.000000 +motor_position.sources.1.compensation_table.1 0.000000 +motor_position.sources.1.compensation_table.2 0.000000 +motor_position.sources.1.compensation_table.3 0.000000 +motor_position.sources.1.compensation_table.4 0.000000 +motor_position.sources.1.compensation_table.5 0.000000 +motor_position.sources.1.compensation_table.6 0.000000 +motor_position.sources.1.compensation_table.7 0.000000 +motor_position.sources.1.compensation_table.8 0.000000 +motor_position.sources.1.compensation_table.9 0.000000 +motor_position.sources.1.compensation_table.10 0.000000 +motor_position.sources.1.compensation_table.11 0.000000 +motor_position.sources.1.compensation_table.12 0.000000 +motor_position.sources.1.compensation_table.13 0.000000 +motor_position.sources.1.compensation_table.14 0.000000 +motor_position.sources.1.compensation_table.15 0.000000 +motor_position.sources.1.compensation_table.16 0.000000 +motor_position.sources.1.compensation_table.17 0.000000 +motor_position.sources.1.compensation_table.18 0.000000 +motor_position.sources.1.compensation_table.19 0.000000 +motor_position.sources.1.compensation_table.20 0.000000 +motor_position.sources.1.compensation_table.21 0.000000 +motor_position.sources.1.compensation_table.22 0.000000 +motor_position.sources.1.compensation_table.23 0.000000 +motor_position.sources.1.compensation_table.24 0.000000 +motor_position.sources.1.compensation_table.25 0.000000 +motor_position.sources.1.compensation_table.26 0.000000 +motor_position.sources.1.compensation_table.27 0.000000 +motor_position.sources.1.compensation_table.28 0.000000 +motor_position.sources.1.compensation_table.29 0.000000 +motor_position.sources.1.compensation_table.30 0.000000 +motor_position.sources.1.compensation_table.31 0.000000 +motor_position.sources.2.aux_number 1 +motor_position.sources.2.type 0 +motor_position.sources.2.i2c_device 0 +motor_position.sources.2.incremental_index -1 +motor_position.sources.2.cpr 16384 +motor_position.sources.2.offset 0.000000 +motor_position.sources.2.sign 1 +motor_position.sources.2.debug_override -1 +motor_position.sources.2.reference 0 +motor_position.sources.2.pll_filter_hz 400.000000 +motor_position.sources.2.compensation_table.0 0.000000 +motor_position.sources.2.compensation_table.1 0.000000 +motor_position.sources.2.compensation_table.2 0.000000 +motor_position.sources.2.compensation_table.3 0.000000 +motor_position.sources.2.compensation_table.4 0.000000 +motor_position.sources.2.compensation_table.5 0.000000 +motor_position.sources.2.compensation_table.6 0.000000 +motor_position.sources.2.compensation_table.7 0.000000 +motor_position.sources.2.compensation_table.8 0.000000 +motor_position.sources.2.compensation_table.9 0.000000 +motor_position.sources.2.compensation_table.10 0.000000 +motor_position.sources.2.compensation_table.11 0.000000 +motor_position.sources.2.compensation_table.12 0.000000 +motor_position.sources.2.compensation_table.13 0.000000 +motor_position.sources.2.compensation_table.14 0.000000 +motor_position.sources.2.compensation_table.15 0.000000 +motor_position.sources.2.compensation_table.16 0.000000 +motor_position.sources.2.compensation_table.17 0.000000 +motor_position.sources.2.compensation_table.18 0.000000 +motor_position.sources.2.compensation_table.19 0.000000 +motor_position.sources.2.compensation_table.20 0.000000 +motor_position.sources.2.compensation_table.21 0.000000 +motor_position.sources.2.compensation_table.22 0.000000 +motor_position.sources.2.compensation_table.23 0.000000 +motor_position.sources.2.compensation_table.24 0.000000 +motor_position.sources.2.compensation_table.25 0.000000 +motor_position.sources.2.compensation_table.26 0.000000 +motor_position.sources.2.compensation_table.27 0.000000 +motor_position.sources.2.compensation_table.28 0.000000 +motor_position.sources.2.compensation_table.29 0.000000 +motor_position.sources.2.compensation_table.30 0.000000 +motor_position.sources.2.compensation_table.31 0.000000 +motor_position.commutation_source 0 +motor_position.output.source 0 +motor_position.output.offset 0.000000 +motor_position.output.sign -1 +motor_position.output.reference_source -1 +motor_position.rotor_to_output_ratio 1.000000 +motor.poles 28 +motor.phase_invert 0 +motor.resistance_ohm 0.248204 +motor.v_per_hz 0.338219 +motor.offset.0 0.000000 +motor.offset.1 0.000000 +motor.offset.2 0.000000 +motor.offset.3 0.000000 +motor.offset.4 0.000000 +motor.offset.5 0.000000 +motor.offset.6 0.000000 +motor.offset.7 0.000000 +motor.offset.8 0.000000 +motor.offset.9 0.000000 +motor.offset.10 0.000000 +motor.offset.11 0.000000 +motor.offset.12 0.000000 +motor.offset.13 0.000000 +motor.offset.14 0.000000 +motor.offset.15 0.000000 +motor.offset.16 0.000000 +motor.offset.17 0.000000 +motor.offset.18 0.000000 +motor.offset.19 0.000000 +motor.offset.20 0.000000 +motor.offset.21 0.000000 +motor.offset.22 0.000000 +motor.offset.23 0.000000 +motor.offset.24 0.000000 +motor.offset.25 0.000000 +motor.offset.26 0.000000 +motor.offset.27 0.000000 +motor.offset.28 0.000000 +motor.offset.29 0.000000 +motor.offset.30 0.000000 +motor.offset.31 0.000000 +motor.offset.32 0.000000 +motor.offset.33 0.000000 +motor.offset.34 0.000000 +motor.offset.35 0.000000 +motor.offset.36 0.000000 +motor.offset.37 0.000000 +motor.offset.38 0.000000 +motor.offset.39 0.000000 +motor.offset.40 0.000000 +motor.offset.41 0.000000 +motor.offset.42 0.000000 +motor.offset.43 0.000000 +motor.offset.44 0.000000 +motor.offset.45 0.000000 +motor.offset.46 0.000000 +motor.offset.47 0.000000 +motor.offset.48 0.000000 +motor.offset.49 0.000000 +motor.offset.50 0.000000 +motor.offset.51 0.000000 +motor.offset.52 0.000000 +motor.offset.53 0.000000 +motor.offset.54 0.000000 +motor.offset.55 0.000000 +motor.offset.56 0.000000 +motor.offset.57 0.000000 +motor.offset.58 0.000000 +motor.offset.59 0.000000 +motor.offset.60 0.000000 +motor.offset.61 0.000000 +motor.offset.62 0.000000 +motor.offset.63 0.000000 +motor.rotation_current_cutoff_A 10000.000000 +motor.rotation_current_scale 0.050000 +motor.rotation_torque_scale 14.700000 +motor.cogging_dq_scale 0.000000 +motor.cogging_dq_comp.0 0 +motor.cogging_dq_comp.1 0 +motor.cogging_dq_comp.2 0 +motor.cogging_dq_comp.3 0 +motor.cogging_dq_comp.4 0 +motor.cogging_dq_comp.5 0 +motor.cogging_dq_comp.6 0 +motor.cogging_dq_comp.7 0 +motor.cogging_dq_comp.8 0 +motor.cogging_dq_comp.9 0 +motor.cogging_dq_comp.10 0 +motor.cogging_dq_comp.11 0 +motor.cogging_dq_comp.12 0 +motor.cogging_dq_comp.13 0 +motor.cogging_dq_comp.14 0 +motor.cogging_dq_comp.15 0 +motor.cogging_dq_comp.16 0 +motor.cogging_dq_comp.17 0 +motor.cogging_dq_comp.18 0 +motor.cogging_dq_comp.19 0 +motor.cogging_dq_comp.20 0 +motor.cogging_dq_comp.21 0 +motor.cogging_dq_comp.22 0 +motor.cogging_dq_comp.23 0 +motor.cogging_dq_comp.24 0 +motor.cogging_dq_comp.25 0 +motor.cogging_dq_comp.26 0 +motor.cogging_dq_comp.27 0 +motor.cogging_dq_comp.28 0 +motor.cogging_dq_comp.29 0 +motor.cogging_dq_comp.30 0 +motor.cogging_dq_comp.31 0 +motor.cogging_dq_comp.32 0 +motor.cogging_dq_comp.33 0 +motor.cogging_dq_comp.34 0 +motor.cogging_dq_comp.35 0 +motor.cogging_dq_comp.36 0 +motor.cogging_dq_comp.37 0 +motor.cogging_dq_comp.38 0 +motor.cogging_dq_comp.39 0 +motor.cogging_dq_comp.40 0 +motor.cogging_dq_comp.41 0 +motor.cogging_dq_comp.42 0 +motor.cogging_dq_comp.43 0 +motor.cogging_dq_comp.44 0 +motor.cogging_dq_comp.45 0 +motor.cogging_dq_comp.46 0 +motor.cogging_dq_comp.47 0 +motor.cogging_dq_comp.48 0 +motor.cogging_dq_comp.49 0 +motor.cogging_dq_comp.50 0 +motor.cogging_dq_comp.51 0 +motor.cogging_dq_comp.52 0 +motor.cogging_dq_comp.53 0 +motor.cogging_dq_comp.54 0 +motor.cogging_dq_comp.55 0 +motor.cogging_dq_comp.56 0 +motor.cogging_dq_comp.57 0 +motor.cogging_dq_comp.58 0 +motor.cogging_dq_comp.59 0 +motor.cogging_dq_comp.60 0 +motor.cogging_dq_comp.61 0 +motor.cogging_dq_comp.62 0 +motor.cogging_dq_comp.63 0 +motor.cogging_dq_comp.64 0 +motor.cogging_dq_comp.65 0 +motor.cogging_dq_comp.66 0 +motor.cogging_dq_comp.67 0 +motor.cogging_dq_comp.68 0 +motor.cogging_dq_comp.69 0 +motor.cogging_dq_comp.70 0 +motor.cogging_dq_comp.71 0 +motor.cogging_dq_comp.72 0 +motor.cogging_dq_comp.73 0 +motor.cogging_dq_comp.74 0 +motor.cogging_dq_comp.75 0 +motor.cogging_dq_comp.76 0 +motor.cogging_dq_comp.77 0 +motor.cogging_dq_comp.78 0 +motor.cogging_dq_comp.79 0 +motor.cogging_dq_comp.80 0 +motor.cogging_dq_comp.81 0 +motor.cogging_dq_comp.82 0 +motor.cogging_dq_comp.83 0 +motor.cogging_dq_comp.84 0 +motor.cogging_dq_comp.85 0 +motor.cogging_dq_comp.86 0 +motor.cogging_dq_comp.87 0 +motor.cogging_dq_comp.88 0 +motor.cogging_dq_comp.89 0 +motor.cogging_dq_comp.90 0 +motor.cogging_dq_comp.91 0 +motor.cogging_dq_comp.92 0 +motor.cogging_dq_comp.93 0 +motor.cogging_dq_comp.94 0 +motor.cogging_dq_comp.95 0 +motor.cogging_dq_comp.96 0 +motor.cogging_dq_comp.97 0 +motor.cogging_dq_comp.98 0 +motor.cogging_dq_comp.99 0 +motor.cogging_dq_comp.100 0 +motor.cogging_dq_comp.101 0 +motor.cogging_dq_comp.102 0 +motor.cogging_dq_comp.103 0 +motor.cogging_dq_comp.104 0 +motor.cogging_dq_comp.105 0 +motor.cogging_dq_comp.106 0 +motor.cogging_dq_comp.107 0 +motor.cogging_dq_comp.108 0 +motor.cogging_dq_comp.109 0 +motor.cogging_dq_comp.110 0 +motor.cogging_dq_comp.111 0 +motor.cogging_dq_comp.112 0 +motor.cogging_dq_comp.113 0 +motor.cogging_dq_comp.114 0 +motor.cogging_dq_comp.115 0 +motor.cogging_dq_comp.116 0 +motor.cogging_dq_comp.117 0 +motor.cogging_dq_comp.118 0 +motor.cogging_dq_comp.119 0 +motor.cogging_dq_comp.120 0 +motor.cogging_dq_comp.121 0 +motor.cogging_dq_comp.122 0 +motor.cogging_dq_comp.123 0 +motor.cogging_dq_comp.124 0 +motor.cogging_dq_comp.125 0 +motor.cogging_dq_comp.126 0 +motor.cogging_dq_comp.127 0 +motor.cogging_dq_comp.128 0 +motor.cogging_dq_comp.129 0 +motor.cogging_dq_comp.130 0 +motor.cogging_dq_comp.131 0 +motor.cogging_dq_comp.132 0 +motor.cogging_dq_comp.133 0 +motor.cogging_dq_comp.134 0 +motor.cogging_dq_comp.135 0 +motor.cogging_dq_comp.136 0 +motor.cogging_dq_comp.137 0 +motor.cogging_dq_comp.138 0 +motor.cogging_dq_comp.139 0 +motor.cogging_dq_comp.140 0 +motor.cogging_dq_comp.141 0 +motor.cogging_dq_comp.142 0 +motor.cogging_dq_comp.143 0 +motor.cogging_dq_comp.144 0 +motor.cogging_dq_comp.145 0 +motor.cogging_dq_comp.146 0 +motor.cogging_dq_comp.147 0 +motor.cogging_dq_comp.148 0 +motor.cogging_dq_comp.149 0 +motor.cogging_dq_comp.150 0 +motor.cogging_dq_comp.151 0 +motor.cogging_dq_comp.152 0 +motor.cogging_dq_comp.153 0 +motor.cogging_dq_comp.154 0 +motor.cogging_dq_comp.155 0 +motor.cogging_dq_comp.156 0 +motor.cogging_dq_comp.157 0 +motor.cogging_dq_comp.158 0 +motor.cogging_dq_comp.159 0 +motor.cogging_dq_comp.160 0 +motor.cogging_dq_comp.161 0 +motor.cogging_dq_comp.162 0 +motor.cogging_dq_comp.163 0 +motor.cogging_dq_comp.164 0 +motor.cogging_dq_comp.165 0 +motor.cogging_dq_comp.166 0 +motor.cogging_dq_comp.167 0 +motor.cogging_dq_comp.168 0 +motor.cogging_dq_comp.169 0 +motor.cogging_dq_comp.170 0 +motor.cogging_dq_comp.171 0 +motor.cogging_dq_comp.172 0 +motor.cogging_dq_comp.173 0 +motor.cogging_dq_comp.174 0 +motor.cogging_dq_comp.175 0 +motor.cogging_dq_comp.176 0 +motor.cogging_dq_comp.177 0 +motor.cogging_dq_comp.178 0 +motor.cogging_dq_comp.179 0 +motor.cogging_dq_comp.180 0 +motor.cogging_dq_comp.181 0 +motor.cogging_dq_comp.182 0 +motor.cogging_dq_comp.183 0 +motor.cogging_dq_comp.184 0 +motor.cogging_dq_comp.185 0 +motor.cogging_dq_comp.186 0 +motor.cogging_dq_comp.187 0 +motor.cogging_dq_comp.188 0 +motor.cogging_dq_comp.189 0 +motor.cogging_dq_comp.190 0 +motor.cogging_dq_comp.191 0 +motor.cogging_dq_comp.192 0 +motor.cogging_dq_comp.193 0 +motor.cogging_dq_comp.194 0 +motor.cogging_dq_comp.195 0 +motor.cogging_dq_comp.196 0 +motor.cogging_dq_comp.197 0 +motor.cogging_dq_comp.198 0 +motor.cogging_dq_comp.199 0 +motor.cogging_dq_comp.200 0 +motor.cogging_dq_comp.201 0 +motor.cogging_dq_comp.202 0 +motor.cogging_dq_comp.203 0 +motor.cogging_dq_comp.204 0 +motor.cogging_dq_comp.205 0 +motor.cogging_dq_comp.206 0 +motor.cogging_dq_comp.207 0 +motor.cogging_dq_comp.208 0 +motor.cogging_dq_comp.209 0 +motor.cogging_dq_comp.210 0 +motor.cogging_dq_comp.211 0 +motor.cogging_dq_comp.212 0 +motor.cogging_dq_comp.213 0 +motor.cogging_dq_comp.214 0 +motor.cogging_dq_comp.215 0 +motor.cogging_dq_comp.216 0 +motor.cogging_dq_comp.217 0 +motor.cogging_dq_comp.218 0 +motor.cogging_dq_comp.219 0 +motor.cogging_dq_comp.220 0 +motor.cogging_dq_comp.221 0 +motor.cogging_dq_comp.222 0 +motor.cogging_dq_comp.223 0 +motor.cogging_dq_comp.224 0 +motor.cogging_dq_comp.225 0 +motor.cogging_dq_comp.226 0 +motor.cogging_dq_comp.227 0 +motor.cogging_dq_comp.228 0 +motor.cogging_dq_comp.229 0 +motor.cogging_dq_comp.230 0 +motor.cogging_dq_comp.231 0 +motor.cogging_dq_comp.232 0 +motor.cogging_dq_comp.233 0 +motor.cogging_dq_comp.234 0 +motor.cogging_dq_comp.235 0 +motor.cogging_dq_comp.236 0 +motor.cogging_dq_comp.237 0 +motor.cogging_dq_comp.238 0 +motor.cogging_dq_comp.239 0 +motor.cogging_dq_comp.240 0 +motor.cogging_dq_comp.241 0 +motor.cogging_dq_comp.242 0 +motor.cogging_dq_comp.243 0 +motor.cogging_dq_comp.244 0 +motor.cogging_dq_comp.245 0 +motor.cogging_dq_comp.246 0 +motor.cogging_dq_comp.247 0 +motor.cogging_dq_comp.248 0 +motor.cogging_dq_comp.249 0 +motor.cogging_dq_comp.250 0 +motor.cogging_dq_comp.251 0 +motor.cogging_dq_comp.252 0 +motor.cogging_dq_comp.253 0 +motor.cogging_dq_comp.254 0 +motor.cogging_dq_comp.255 0 +motor.cogging_dq_comp.256 0 +motor.cogging_dq_comp.257 0 +motor.cogging_dq_comp.258 0 +motor.cogging_dq_comp.259 0 +motor.cogging_dq_comp.260 0 +motor.cogging_dq_comp.261 0 +motor.cogging_dq_comp.262 0 +motor.cogging_dq_comp.263 0 +motor.cogging_dq_comp.264 0 +motor.cogging_dq_comp.265 0 +motor.cogging_dq_comp.266 0 +motor.cogging_dq_comp.267 0 +motor.cogging_dq_comp.268 0 +motor.cogging_dq_comp.269 0 +motor.cogging_dq_comp.270 0 +motor.cogging_dq_comp.271 0 +motor.cogging_dq_comp.272 0 +motor.cogging_dq_comp.273 0 +motor.cogging_dq_comp.274 0 +motor.cogging_dq_comp.275 0 +motor.cogging_dq_comp.276 0 +motor.cogging_dq_comp.277 0 +motor.cogging_dq_comp.278 0 +motor.cogging_dq_comp.279 0 +motor.cogging_dq_comp.280 0 +motor.cogging_dq_comp.281 0 +motor.cogging_dq_comp.282 0 +motor.cogging_dq_comp.283 0 +motor.cogging_dq_comp.284 0 +motor.cogging_dq_comp.285 0 +motor.cogging_dq_comp.286 0 +motor.cogging_dq_comp.287 0 +motor.cogging_dq_comp.288 0 +motor.cogging_dq_comp.289 0 +motor.cogging_dq_comp.290 0 +motor.cogging_dq_comp.291 0 +motor.cogging_dq_comp.292 0 +motor.cogging_dq_comp.293 0 +motor.cogging_dq_comp.294 0 +motor.cogging_dq_comp.295 0 +motor.cogging_dq_comp.296 0 +motor.cogging_dq_comp.297 0 +motor.cogging_dq_comp.298 0 +motor.cogging_dq_comp.299 0 +motor.cogging_dq_comp.300 0 +motor.cogging_dq_comp.301 0 +motor.cogging_dq_comp.302 0 +motor.cogging_dq_comp.303 0 +motor.cogging_dq_comp.304 0 +motor.cogging_dq_comp.305 0 +motor.cogging_dq_comp.306 0 +motor.cogging_dq_comp.307 0 +motor.cogging_dq_comp.308 0 +motor.cogging_dq_comp.309 0 +motor.cogging_dq_comp.310 0 +motor.cogging_dq_comp.311 0 +motor.cogging_dq_comp.312 0 +motor.cogging_dq_comp.313 0 +motor.cogging_dq_comp.314 0 +motor.cogging_dq_comp.315 0 +motor.cogging_dq_comp.316 0 +motor.cogging_dq_comp.317 0 +motor.cogging_dq_comp.318 0 +motor.cogging_dq_comp.319 0 +motor.cogging_dq_comp.320 0 +motor.cogging_dq_comp.321 0 +motor.cogging_dq_comp.322 0 +motor.cogging_dq_comp.323 0 +motor.cogging_dq_comp.324 0 +motor.cogging_dq_comp.325 0 +motor.cogging_dq_comp.326 0 +motor.cogging_dq_comp.327 0 +motor.cogging_dq_comp.328 0 +motor.cogging_dq_comp.329 0 +motor.cogging_dq_comp.330 0 +motor.cogging_dq_comp.331 0 +motor.cogging_dq_comp.332 0 +motor.cogging_dq_comp.333 0 +motor.cogging_dq_comp.334 0 +motor.cogging_dq_comp.335 0 +motor.cogging_dq_comp.336 0 +motor.cogging_dq_comp.337 0 +motor.cogging_dq_comp.338 0 +motor.cogging_dq_comp.339 0 +motor.cogging_dq_comp.340 0 +motor.cogging_dq_comp.341 0 +motor.cogging_dq_comp.342 0 +motor.cogging_dq_comp.343 0 +motor.cogging_dq_comp.344 0 +motor.cogging_dq_comp.345 0 +motor.cogging_dq_comp.346 0 +motor.cogging_dq_comp.347 0 +motor.cogging_dq_comp.348 0 +motor.cogging_dq_comp.349 0 +motor.cogging_dq_comp.350 0 +motor.cogging_dq_comp.351 0 +motor.cogging_dq_comp.352 0 +motor.cogging_dq_comp.353 0 +motor.cogging_dq_comp.354 0 +motor.cogging_dq_comp.355 0 +motor.cogging_dq_comp.356 0 +motor.cogging_dq_comp.357 0 +motor.cogging_dq_comp.358 0 +motor.cogging_dq_comp.359 0 +motor.cogging_dq_comp.360 0 +motor.cogging_dq_comp.361 0 +motor.cogging_dq_comp.362 0 +motor.cogging_dq_comp.363 0 +motor.cogging_dq_comp.364 0 +motor.cogging_dq_comp.365 0 +motor.cogging_dq_comp.366 0 +motor.cogging_dq_comp.367 0 +motor.cogging_dq_comp.368 0 +motor.cogging_dq_comp.369 0 +motor.cogging_dq_comp.370 0 +motor.cogging_dq_comp.371 0 +motor.cogging_dq_comp.372 0 +motor.cogging_dq_comp.373 0 +motor.cogging_dq_comp.374 0 +motor.cogging_dq_comp.375 0 +motor.cogging_dq_comp.376 0 +motor.cogging_dq_comp.377 0 +motor.cogging_dq_comp.378 0 +motor.cogging_dq_comp.379 0 +motor.cogging_dq_comp.380 0 +motor.cogging_dq_comp.381 0 +motor.cogging_dq_comp.382 0 +motor.cogging_dq_comp.383 0 +motor.cogging_dq_comp.384 0 +motor.cogging_dq_comp.385 0 +motor.cogging_dq_comp.386 0 +motor.cogging_dq_comp.387 0 +motor.cogging_dq_comp.388 0 +motor.cogging_dq_comp.389 0 +motor.cogging_dq_comp.390 0 +motor.cogging_dq_comp.391 0 +motor.cogging_dq_comp.392 0 +motor.cogging_dq_comp.393 0 +motor.cogging_dq_comp.394 0 +motor.cogging_dq_comp.395 0 +motor.cogging_dq_comp.396 0 +motor.cogging_dq_comp.397 0 +motor.cogging_dq_comp.398 0 +motor.cogging_dq_comp.399 0 +motor.cogging_dq_comp.400 0 +motor.cogging_dq_comp.401 0 +motor.cogging_dq_comp.402 0 +motor.cogging_dq_comp.403 0 +motor.cogging_dq_comp.404 0 +motor.cogging_dq_comp.405 0 +motor.cogging_dq_comp.406 0 +motor.cogging_dq_comp.407 0 +motor.cogging_dq_comp.408 0 +motor.cogging_dq_comp.409 0 +motor.cogging_dq_comp.410 0 +motor.cogging_dq_comp.411 0 +motor.cogging_dq_comp.412 0 +motor.cogging_dq_comp.413 0 +motor.cogging_dq_comp.414 0 +motor.cogging_dq_comp.415 0 +motor.cogging_dq_comp.416 0 +motor.cogging_dq_comp.417 0 +motor.cogging_dq_comp.418 0 +motor.cogging_dq_comp.419 0 +motor.cogging_dq_comp.420 0 +motor.cogging_dq_comp.421 0 +motor.cogging_dq_comp.422 0 +motor.cogging_dq_comp.423 0 +motor.cogging_dq_comp.424 0 +motor.cogging_dq_comp.425 0 +motor.cogging_dq_comp.426 0 +motor.cogging_dq_comp.427 0 +motor.cogging_dq_comp.428 0 +motor.cogging_dq_comp.429 0 +motor.cogging_dq_comp.430 0 +motor.cogging_dq_comp.431 0 +motor.cogging_dq_comp.432 0 +motor.cogging_dq_comp.433 0 +motor.cogging_dq_comp.434 0 +motor.cogging_dq_comp.435 0 +motor.cogging_dq_comp.436 0 +motor.cogging_dq_comp.437 0 +motor.cogging_dq_comp.438 0 +motor.cogging_dq_comp.439 0 +motor.cogging_dq_comp.440 0 +motor.cogging_dq_comp.441 0 +motor.cogging_dq_comp.442 0 +motor.cogging_dq_comp.443 0 +motor.cogging_dq_comp.444 0 +motor.cogging_dq_comp.445 0 +motor.cogging_dq_comp.446 0 +motor.cogging_dq_comp.447 0 +motor.cogging_dq_comp.448 0 +motor.cogging_dq_comp.449 0 +motor.cogging_dq_comp.450 0 +motor.cogging_dq_comp.451 0 +motor.cogging_dq_comp.452 0 +motor.cogging_dq_comp.453 0 +motor.cogging_dq_comp.454 0 +motor.cogging_dq_comp.455 0 +motor.cogging_dq_comp.456 0 +motor.cogging_dq_comp.457 0 +motor.cogging_dq_comp.458 0 +motor.cogging_dq_comp.459 0 +motor.cogging_dq_comp.460 0 +motor.cogging_dq_comp.461 0 +motor.cogging_dq_comp.462 0 +motor.cogging_dq_comp.463 0 +motor.cogging_dq_comp.464 0 +motor.cogging_dq_comp.465 0 +motor.cogging_dq_comp.466 0 +motor.cogging_dq_comp.467 0 +motor.cogging_dq_comp.468 0 +motor.cogging_dq_comp.469 0 +motor.cogging_dq_comp.470 0 +motor.cogging_dq_comp.471 0 +motor.cogging_dq_comp.472 0 +motor.cogging_dq_comp.473 0 +motor.cogging_dq_comp.474 0 +motor.cogging_dq_comp.475 0 +motor.cogging_dq_comp.476 0 +motor.cogging_dq_comp.477 0 +motor.cogging_dq_comp.478 0 +motor.cogging_dq_comp.479 0 +motor.cogging_dq_comp.480 0 +motor.cogging_dq_comp.481 0 +motor.cogging_dq_comp.482 0 +motor.cogging_dq_comp.483 0 +motor.cogging_dq_comp.484 0 +motor.cogging_dq_comp.485 0 +motor.cogging_dq_comp.486 0 +motor.cogging_dq_comp.487 0 +motor.cogging_dq_comp.488 0 +motor.cogging_dq_comp.489 0 +motor.cogging_dq_comp.490 0 +motor.cogging_dq_comp.491 0 +motor.cogging_dq_comp.492 0 +motor.cogging_dq_comp.493 0 +motor.cogging_dq_comp.494 0 +motor.cogging_dq_comp.495 0 +motor.cogging_dq_comp.496 0 +motor.cogging_dq_comp.497 0 +motor.cogging_dq_comp.498 0 +motor.cogging_dq_comp.499 0 +motor.cogging_dq_comp.500 0 +motor.cogging_dq_comp.501 0 +motor.cogging_dq_comp.502 0 +motor.cogging_dq_comp.503 0 +motor.cogging_dq_comp.504 0 +motor.cogging_dq_comp.505 0 +motor.cogging_dq_comp.506 0 +motor.cogging_dq_comp.507 0 +motor.cogging_dq_comp.508 0 +motor.cogging_dq_comp.509 0 +motor.cogging_dq_comp.510 0 +motor.cogging_dq_comp.511 0 +motor.cogging_dq_comp.512 0 +motor.cogging_dq_comp.513 0 +motor.cogging_dq_comp.514 0 +motor.cogging_dq_comp.515 0 +motor.cogging_dq_comp.516 0 +motor.cogging_dq_comp.517 0 +motor.cogging_dq_comp.518 0 +motor.cogging_dq_comp.519 0 +motor.cogging_dq_comp.520 0 +motor.cogging_dq_comp.521 0 +motor.cogging_dq_comp.522 0 +motor.cogging_dq_comp.523 0 +motor.cogging_dq_comp.524 0 +motor.cogging_dq_comp.525 0 +motor.cogging_dq_comp.526 0 +motor.cogging_dq_comp.527 0 +motor.cogging_dq_comp.528 0 +motor.cogging_dq_comp.529 0 +motor.cogging_dq_comp.530 0 +motor.cogging_dq_comp.531 0 +motor.cogging_dq_comp.532 0 +motor.cogging_dq_comp.533 0 +motor.cogging_dq_comp.534 0 +motor.cogging_dq_comp.535 0 +motor.cogging_dq_comp.536 0 +motor.cogging_dq_comp.537 0 +motor.cogging_dq_comp.538 0 +motor.cogging_dq_comp.539 0 +motor.cogging_dq_comp.540 0 +motor.cogging_dq_comp.541 0 +motor.cogging_dq_comp.542 0 +motor.cogging_dq_comp.543 0 +motor.cogging_dq_comp.544 0 +motor.cogging_dq_comp.545 0 +motor.cogging_dq_comp.546 0 +motor.cogging_dq_comp.547 0 +motor.cogging_dq_comp.548 0 +motor.cogging_dq_comp.549 0 +motor.cogging_dq_comp.550 0 +motor.cogging_dq_comp.551 0 +motor.cogging_dq_comp.552 0 +motor.cogging_dq_comp.553 0 +motor.cogging_dq_comp.554 0 +motor.cogging_dq_comp.555 0 +motor.cogging_dq_comp.556 0 +motor.cogging_dq_comp.557 0 +motor.cogging_dq_comp.558 0 +motor.cogging_dq_comp.559 0 +motor.cogging_dq_comp.560 0 +motor.cogging_dq_comp.561 0 +motor.cogging_dq_comp.562 0 +motor.cogging_dq_comp.563 0 +motor.cogging_dq_comp.564 0 +motor.cogging_dq_comp.565 0 +motor.cogging_dq_comp.566 0 +motor.cogging_dq_comp.567 0 +motor.cogging_dq_comp.568 0 +motor.cogging_dq_comp.569 0 +motor.cogging_dq_comp.570 0 +motor.cogging_dq_comp.571 0 +motor.cogging_dq_comp.572 0 +motor.cogging_dq_comp.573 0 +motor.cogging_dq_comp.574 0 +motor.cogging_dq_comp.575 0 +motor.cogging_dq_comp.576 0 +motor.cogging_dq_comp.577 0 +motor.cogging_dq_comp.578 0 +motor.cogging_dq_comp.579 0 +motor.cogging_dq_comp.580 0 +motor.cogging_dq_comp.581 0 +motor.cogging_dq_comp.582 0 +motor.cogging_dq_comp.583 0 +motor.cogging_dq_comp.584 0 +motor.cogging_dq_comp.585 0 +motor.cogging_dq_comp.586 0 +motor.cogging_dq_comp.587 0 +motor.cogging_dq_comp.588 0 +motor.cogging_dq_comp.589 0 +motor.cogging_dq_comp.590 0 +motor.cogging_dq_comp.591 0 +motor.cogging_dq_comp.592 0 +motor.cogging_dq_comp.593 0 +motor.cogging_dq_comp.594 0 +motor.cogging_dq_comp.595 0 +motor.cogging_dq_comp.596 0 +motor.cogging_dq_comp.597 0 +motor.cogging_dq_comp.598 0 +motor.cogging_dq_comp.599 0 +motor.cogging_dq_comp.600 0 +motor.cogging_dq_comp.601 0 +motor.cogging_dq_comp.602 0 +motor.cogging_dq_comp.603 0 +motor.cogging_dq_comp.604 0 +motor.cogging_dq_comp.605 0 +motor.cogging_dq_comp.606 0 +motor.cogging_dq_comp.607 0 +motor.cogging_dq_comp.608 0 +motor.cogging_dq_comp.609 0 +motor.cogging_dq_comp.610 0 +motor.cogging_dq_comp.611 0 +motor.cogging_dq_comp.612 0 +motor.cogging_dq_comp.613 0 +motor.cogging_dq_comp.614 0 +motor.cogging_dq_comp.615 0 +motor.cogging_dq_comp.616 0 +motor.cogging_dq_comp.617 0 +motor.cogging_dq_comp.618 0 +motor.cogging_dq_comp.619 0 +motor.cogging_dq_comp.620 0 +motor.cogging_dq_comp.621 0 +motor.cogging_dq_comp.622 0 +motor.cogging_dq_comp.623 0 +motor.cogging_dq_comp.624 0 +motor.cogging_dq_comp.625 0 +motor.cogging_dq_comp.626 0 +motor.cogging_dq_comp.627 0 +motor.cogging_dq_comp.628 0 +motor.cogging_dq_comp.629 0 +motor.cogging_dq_comp.630 0 +motor.cogging_dq_comp.631 0 +motor.cogging_dq_comp.632 0 +motor.cogging_dq_comp.633 0 +motor.cogging_dq_comp.634 0 +motor.cogging_dq_comp.635 0 +motor.cogging_dq_comp.636 0 +motor.cogging_dq_comp.637 0 +motor.cogging_dq_comp.638 0 +motor.cogging_dq_comp.639 0 +motor.cogging_dq_comp.640 0 +motor.cogging_dq_comp.641 0 +motor.cogging_dq_comp.642 0 +motor.cogging_dq_comp.643 0 +motor.cogging_dq_comp.644 0 +motor.cogging_dq_comp.645 0 +motor.cogging_dq_comp.646 0 +motor.cogging_dq_comp.647 0 +motor.cogging_dq_comp.648 0 +motor.cogging_dq_comp.649 0 +motor.cogging_dq_comp.650 0 +motor.cogging_dq_comp.651 0 +motor.cogging_dq_comp.652 0 +motor.cogging_dq_comp.653 0 +motor.cogging_dq_comp.654 0 +motor.cogging_dq_comp.655 0 +motor.cogging_dq_comp.656 0 +motor.cogging_dq_comp.657 0 +motor.cogging_dq_comp.658 0 +motor.cogging_dq_comp.659 0 +motor.cogging_dq_comp.660 0 +motor.cogging_dq_comp.661 0 +motor.cogging_dq_comp.662 0 +motor.cogging_dq_comp.663 0 +motor.cogging_dq_comp.664 0 +motor.cogging_dq_comp.665 0 +motor.cogging_dq_comp.666 0 +motor.cogging_dq_comp.667 0 +motor.cogging_dq_comp.668 0 +motor.cogging_dq_comp.669 0 +motor.cogging_dq_comp.670 0 +motor.cogging_dq_comp.671 0 +motor.cogging_dq_comp.672 0 +motor.cogging_dq_comp.673 0 +motor.cogging_dq_comp.674 0 +motor.cogging_dq_comp.675 0 +motor.cogging_dq_comp.676 0 +motor.cogging_dq_comp.677 0 +motor.cogging_dq_comp.678 0 +motor.cogging_dq_comp.679 0 +motor.cogging_dq_comp.680 0 +motor.cogging_dq_comp.681 0 +motor.cogging_dq_comp.682 0 +motor.cogging_dq_comp.683 0 +motor.cogging_dq_comp.684 0 +motor.cogging_dq_comp.685 0 +motor.cogging_dq_comp.686 0 +motor.cogging_dq_comp.687 0 +motor.cogging_dq_comp.688 0 +motor.cogging_dq_comp.689 0 +motor.cogging_dq_comp.690 0 +motor.cogging_dq_comp.691 0 +motor.cogging_dq_comp.692 0 +motor.cogging_dq_comp.693 0 +motor.cogging_dq_comp.694 0 +motor.cogging_dq_comp.695 0 +motor.cogging_dq_comp.696 0 +motor.cogging_dq_comp.697 0 +motor.cogging_dq_comp.698 0 +motor.cogging_dq_comp.699 0 +motor.cogging_dq_comp.700 0 +motor.cogging_dq_comp.701 0 +motor.cogging_dq_comp.702 0 +motor.cogging_dq_comp.703 0 +motor.cogging_dq_comp.704 0 +motor.cogging_dq_comp.705 0 +motor.cogging_dq_comp.706 0 +motor.cogging_dq_comp.707 0 +motor.cogging_dq_comp.708 0 +motor.cogging_dq_comp.709 0 +motor.cogging_dq_comp.710 0 +motor.cogging_dq_comp.711 0 +motor.cogging_dq_comp.712 0 +motor.cogging_dq_comp.713 0 +motor.cogging_dq_comp.714 0 +motor.cogging_dq_comp.715 0 +motor.cogging_dq_comp.716 0 +motor.cogging_dq_comp.717 0 +motor.cogging_dq_comp.718 0 +motor.cogging_dq_comp.719 0 +motor.cogging_dq_comp.720 0 +motor.cogging_dq_comp.721 0 +motor.cogging_dq_comp.722 0 +motor.cogging_dq_comp.723 0 +motor.cogging_dq_comp.724 0 +motor.cogging_dq_comp.725 0 +motor.cogging_dq_comp.726 0 +motor.cogging_dq_comp.727 0 +motor.cogging_dq_comp.728 0 +motor.cogging_dq_comp.729 0 +motor.cogging_dq_comp.730 0 +motor.cogging_dq_comp.731 0 +motor.cogging_dq_comp.732 0 +motor.cogging_dq_comp.733 0 +motor.cogging_dq_comp.734 0 +motor.cogging_dq_comp.735 0 +motor.cogging_dq_comp.736 0 +motor.cogging_dq_comp.737 0 +motor.cogging_dq_comp.738 0 +motor.cogging_dq_comp.739 0 +motor.cogging_dq_comp.740 0 +motor.cogging_dq_comp.741 0 +motor.cogging_dq_comp.742 0 +motor.cogging_dq_comp.743 0 +motor.cogging_dq_comp.744 0 +motor.cogging_dq_comp.745 0 +motor.cogging_dq_comp.746 0 +motor.cogging_dq_comp.747 0 +motor.cogging_dq_comp.748 0 +motor.cogging_dq_comp.749 0 +motor.cogging_dq_comp.750 0 +motor.cogging_dq_comp.751 0 +motor.cogging_dq_comp.752 0 +motor.cogging_dq_comp.753 0 +motor.cogging_dq_comp.754 0 +motor.cogging_dq_comp.755 0 +motor.cogging_dq_comp.756 0 +motor.cogging_dq_comp.757 0 +motor.cogging_dq_comp.758 0 +motor.cogging_dq_comp.759 0 +motor.cogging_dq_comp.760 0 +motor.cogging_dq_comp.761 0 +motor.cogging_dq_comp.762 0 +motor.cogging_dq_comp.763 0 +motor.cogging_dq_comp.764 0 +motor.cogging_dq_comp.765 0 +motor.cogging_dq_comp.766 0 +motor.cogging_dq_comp.767 0 +motor.cogging_dq_comp.768 0 +motor.cogging_dq_comp.769 0 +motor.cogging_dq_comp.770 0 +motor.cogging_dq_comp.771 0 +motor.cogging_dq_comp.772 0 +motor.cogging_dq_comp.773 0 +motor.cogging_dq_comp.774 0 +motor.cogging_dq_comp.775 0 +motor.cogging_dq_comp.776 0 +motor.cogging_dq_comp.777 0 +motor.cogging_dq_comp.778 0 +motor.cogging_dq_comp.779 0 +motor.cogging_dq_comp.780 0 +motor.cogging_dq_comp.781 0 +motor.cogging_dq_comp.782 0 +motor.cogging_dq_comp.783 0 +motor.cogging_dq_comp.784 0 +motor.cogging_dq_comp.785 0 +motor.cogging_dq_comp.786 0 +motor.cogging_dq_comp.787 0 +motor.cogging_dq_comp.788 0 +motor.cogging_dq_comp.789 0 +motor.cogging_dq_comp.790 0 +motor.cogging_dq_comp.791 0 +motor.cogging_dq_comp.792 0 +motor.cogging_dq_comp.793 0 +motor.cogging_dq_comp.794 0 +motor.cogging_dq_comp.795 0 +motor.cogging_dq_comp.796 0 +motor.cogging_dq_comp.797 0 +motor.cogging_dq_comp.798 0 +motor.cogging_dq_comp.799 0 +motor.cogging_dq_comp.800 0 +motor.cogging_dq_comp.801 0 +motor.cogging_dq_comp.802 0 +motor.cogging_dq_comp.803 0 +motor.cogging_dq_comp.804 0 +motor.cogging_dq_comp.805 0 +motor.cogging_dq_comp.806 0 +motor.cogging_dq_comp.807 0 +motor.cogging_dq_comp.808 0 +motor.cogging_dq_comp.809 0 +motor.cogging_dq_comp.810 0 +motor.cogging_dq_comp.811 0 +motor.cogging_dq_comp.812 0 +motor.cogging_dq_comp.813 0 +motor.cogging_dq_comp.814 0 +motor.cogging_dq_comp.815 0 +motor.cogging_dq_comp.816 0 +motor.cogging_dq_comp.817 0 +motor.cogging_dq_comp.818 0 +motor.cogging_dq_comp.819 0 +motor.cogging_dq_comp.820 0 +motor.cogging_dq_comp.821 0 +motor.cogging_dq_comp.822 0 +motor.cogging_dq_comp.823 0 +motor.cogging_dq_comp.824 0 +motor.cogging_dq_comp.825 0 +motor.cogging_dq_comp.826 0 +motor.cogging_dq_comp.827 0 +motor.cogging_dq_comp.828 0 +motor.cogging_dq_comp.829 0 +motor.cogging_dq_comp.830 0 +motor.cogging_dq_comp.831 0 +motor.cogging_dq_comp.832 0 +motor.cogging_dq_comp.833 0 +motor.cogging_dq_comp.834 0 +motor.cogging_dq_comp.835 0 +motor.cogging_dq_comp.836 0 +motor.cogging_dq_comp.837 0 +motor.cogging_dq_comp.838 0 +motor.cogging_dq_comp.839 0 +motor.cogging_dq_comp.840 0 +motor.cogging_dq_comp.841 0 +motor.cogging_dq_comp.842 0 +motor.cogging_dq_comp.843 0 +motor.cogging_dq_comp.844 0 +motor.cogging_dq_comp.845 0 +motor.cogging_dq_comp.846 0 +motor.cogging_dq_comp.847 0 +motor.cogging_dq_comp.848 0 +motor.cogging_dq_comp.849 0 +motor.cogging_dq_comp.850 0 +motor.cogging_dq_comp.851 0 +motor.cogging_dq_comp.852 0 +motor.cogging_dq_comp.853 0 +motor.cogging_dq_comp.854 0 +motor.cogging_dq_comp.855 0 +motor.cogging_dq_comp.856 0 +motor.cogging_dq_comp.857 0 +motor.cogging_dq_comp.858 0 +motor.cogging_dq_comp.859 0 +motor.cogging_dq_comp.860 0 +motor.cogging_dq_comp.861 0 +motor.cogging_dq_comp.862 0 +motor.cogging_dq_comp.863 0 +motor.cogging_dq_comp.864 0 +motor.cogging_dq_comp.865 0 +motor.cogging_dq_comp.866 0 +motor.cogging_dq_comp.867 0 +motor.cogging_dq_comp.868 0 +motor.cogging_dq_comp.869 0 +motor.cogging_dq_comp.870 0 +motor.cogging_dq_comp.871 0 +motor.cogging_dq_comp.872 0 +motor.cogging_dq_comp.873 0 +motor.cogging_dq_comp.874 0 +motor.cogging_dq_comp.875 0 +motor.cogging_dq_comp.876 0 +motor.cogging_dq_comp.877 0 +motor.cogging_dq_comp.878 0 +motor.cogging_dq_comp.879 0 +motor.cogging_dq_comp.880 0 +motor.cogging_dq_comp.881 0 +motor.cogging_dq_comp.882 0 +motor.cogging_dq_comp.883 0 +motor.cogging_dq_comp.884 0 +motor.cogging_dq_comp.885 0 +motor.cogging_dq_comp.886 0 +motor.cogging_dq_comp.887 0 +motor.cogging_dq_comp.888 0 +motor.cogging_dq_comp.889 0 +motor.cogging_dq_comp.890 0 +motor.cogging_dq_comp.891 0 +motor.cogging_dq_comp.892 0 +motor.cogging_dq_comp.893 0 +motor.cogging_dq_comp.894 0 +motor.cogging_dq_comp.895 0 +motor.cogging_dq_comp.896 0 +motor.cogging_dq_comp.897 0 +motor.cogging_dq_comp.898 0 +motor.cogging_dq_comp.899 0 +motor.cogging_dq_comp.900 0 +motor.cogging_dq_comp.901 0 +motor.cogging_dq_comp.902 0 +motor.cogging_dq_comp.903 0 +motor.cogging_dq_comp.904 0 +motor.cogging_dq_comp.905 0 +motor.cogging_dq_comp.906 0 +motor.cogging_dq_comp.907 0 +motor.cogging_dq_comp.908 0 +motor.cogging_dq_comp.909 0 +motor.cogging_dq_comp.910 0 +motor.cogging_dq_comp.911 0 +motor.cogging_dq_comp.912 0 +motor.cogging_dq_comp.913 0 +motor.cogging_dq_comp.914 0 +motor.cogging_dq_comp.915 0 +motor.cogging_dq_comp.916 0 +motor.cogging_dq_comp.917 0 +motor.cogging_dq_comp.918 0 +motor.cogging_dq_comp.919 0 +motor.cogging_dq_comp.920 0 +motor.cogging_dq_comp.921 0 +motor.cogging_dq_comp.922 0 +motor.cogging_dq_comp.923 0 +motor.cogging_dq_comp.924 0 +motor.cogging_dq_comp.925 0 +motor.cogging_dq_comp.926 0 +motor.cogging_dq_comp.927 0 +motor.cogging_dq_comp.928 0 +motor.cogging_dq_comp.929 0 +motor.cogging_dq_comp.930 0 +motor.cogging_dq_comp.931 0 +motor.cogging_dq_comp.932 0 +motor.cogging_dq_comp.933 0 +motor.cogging_dq_comp.934 0 +motor.cogging_dq_comp.935 0 +motor.cogging_dq_comp.936 0 +motor.cogging_dq_comp.937 0 +motor.cogging_dq_comp.938 0 +motor.cogging_dq_comp.939 0 +motor.cogging_dq_comp.940 0 +motor.cogging_dq_comp.941 0 +motor.cogging_dq_comp.942 0 +motor.cogging_dq_comp.943 0 +motor.cogging_dq_comp.944 0 +motor.cogging_dq_comp.945 0 +motor.cogging_dq_comp.946 0 +motor.cogging_dq_comp.947 0 +motor.cogging_dq_comp.948 0 +motor.cogging_dq_comp.949 0 +motor.cogging_dq_comp.950 0 +motor.cogging_dq_comp.951 0 +motor.cogging_dq_comp.952 0 +motor.cogging_dq_comp.953 0 +motor.cogging_dq_comp.954 0 +motor.cogging_dq_comp.955 0 +motor.cogging_dq_comp.956 0 +motor.cogging_dq_comp.957 0 +motor.cogging_dq_comp.958 0 +motor.cogging_dq_comp.959 0 +motor.cogging_dq_comp.960 0 +motor.cogging_dq_comp.961 0 +motor.cogging_dq_comp.962 0 +motor.cogging_dq_comp.963 0 +motor.cogging_dq_comp.964 0 +motor.cogging_dq_comp.965 0 +motor.cogging_dq_comp.966 0 +motor.cogging_dq_comp.967 0 +motor.cogging_dq_comp.968 0 +motor.cogging_dq_comp.969 0 +motor.cogging_dq_comp.970 0 +motor.cogging_dq_comp.971 0 +motor.cogging_dq_comp.972 0 +motor.cogging_dq_comp.973 0 +motor.cogging_dq_comp.974 0 +motor.cogging_dq_comp.975 0 +motor.cogging_dq_comp.976 0 +motor.cogging_dq_comp.977 0 +motor.cogging_dq_comp.978 0 +motor.cogging_dq_comp.979 0 +motor.cogging_dq_comp.980 0 +motor.cogging_dq_comp.981 0 +motor.cogging_dq_comp.982 0 +motor.cogging_dq_comp.983 0 +motor.cogging_dq_comp.984 0 +motor.cogging_dq_comp.985 0 +motor.cogging_dq_comp.986 0 +motor.cogging_dq_comp.987 0 +motor.cogging_dq_comp.988 0 +motor.cogging_dq_comp.989 0 +motor.cogging_dq_comp.990 0 +motor.cogging_dq_comp.991 0 +motor.cogging_dq_comp.992 0 +motor.cogging_dq_comp.993 0 +motor.cogging_dq_comp.994 0 +motor.cogging_dq_comp.995 0 +motor.cogging_dq_comp.996 0 +motor.cogging_dq_comp.997 0 +motor.cogging_dq_comp.998 0 +motor.cogging_dq_comp.999 0 +motor.cogging_dq_comp.1000 0 +motor.cogging_dq_comp.1001 0 +motor.cogging_dq_comp.1002 0 +motor.cogging_dq_comp.1003 0 +motor.cogging_dq_comp.1004 0 +motor.cogging_dq_comp.1005 0 +motor.cogging_dq_comp.1006 0 +motor.cogging_dq_comp.1007 0 +motor.cogging_dq_comp.1008 0 +motor.cogging_dq_comp.1009 0 +motor.cogging_dq_comp.1010 0 +motor.cogging_dq_comp.1011 0 +motor.cogging_dq_comp.1012 0 +motor.cogging_dq_comp.1013 0 +motor.cogging_dq_comp.1014 0 +motor.cogging_dq_comp.1015 0 +motor.cogging_dq_comp.1016 0 +motor.cogging_dq_comp.1017 0 +motor.cogging_dq_comp.1018 0 +motor.cogging_dq_comp.1019 0 +motor.cogging_dq_comp.1020 0 +motor.cogging_dq_comp.1021 0 +motor.cogging_dq_comp.1022 0 +motor.cogging_dq_comp.1023 0 +drv8323_conf.dis_cpuv 0 +drv8323_conf.dis_gdf 0 +drv8323_conf.otw_rep 0 +drv8323_conf.pwm_mode 1 +drv8323_conf.pwm_1x_asynchronous 0 +drv8323_conf.pwm_1x_dir 0 +drv8323_conf.idrivep_hs_ma 100 +drv8323_conf.idriven_hs_ma 200 +drv8323_conf.cbc 1 +drv8323_conf.tdrive_ns 1000 +drv8323_conf.idrivep_ls_ma 100 +drv8323_conf.idriven_ls_ma 200 +drv8323_conf.tretry 0 +drv8323_conf.dead_time_ns 50 +drv8323_conf.ocp_mode 0 +drv8323_conf.ocp_deg_us 4 +drv8323_conf.vds_lvl_mv 700 +drv8323_conf.csa_fet 0 +drv8323_conf.vref_div 1 +drv8323_conf.ls_ref 0 +drv8323_conf.csa_gain 20 +drv8323_conf.dis_sen 0 +drv8323_conf.sen_lvl_mv 500 +servo.pwm_rate_hz 30000 +servo.i_gain 20.000000 +servo.current_sense_ohm 0.000500 +servo.pwm_comp_off 0.027000 +servo.pwm_comp_mag 0.005000 +servo.pwm_scale 1.000000 +servo.max_voltage 46.000000 +servo.max_power_W 450.000000 +servo.derate_temperature 50.000000 +servo.fault_temperature 75.000000 +servo.enable_motor_temperature 0 +servo.motor_derate_temperature 50.000000 +servo.motor_fault_temperature nan +servo.velocity_threshold 0.000000 +servo.position_derate 0.020000 +servo.adc_cur_cycles 2 +servo.adc_aux_cycles 47 +servo.pid_dq.kp 0.192425 +servo.pid_dq.ki 155.951324 +servo.pid_position.kp 0.500000 +servo.pid_position.ki 0.000000 +servo.pid_position.kd 0.050000 +servo.pid_position.iratelimit -1.000000 +servo.pid_position.ilimit 0.000000 +servo.pid_position.max_desired_rate 0.000000 +servo.pid_position.sign -1 +servo.current_feedforward 1.000000 +servo.bemf_feedforward 1.000000 +servo.default_velocity_limit nan +servo.default_accel_limit nan +servo.voltage_mode_control 0 +servo.fixed_voltage_mode 0 +servo.fixed_voltage_control_V 0.000000 +servo.max_position_slip 10.000000 +servo.default_timeout_s 0.100000 +servo.timeout_max_torque_Nm 5.000000 +servo.timeout_mode 12 +servo.flux_brake_min_voltage 43.500000 +servo.flux_brake_resistance_ohm 0.025000 +servo.max_current_A 7.500000 +servo.derate_current_A -20.000000 +servo.max_velocity 45.000000 +servo.max_velocity_derate 2.000000 +servo.cooldown_cycles 256 +servo.velocity_zero_capture_threshold 0.050000 +servo.emit_debug 0 +servopos.position_min nan +servopos.position_max nan +id.id 20 +can.prefix 0 + diff --git a/config/moteus/middle_right.cfg b/config/moteus/middle_right.cfg new file mode 100644 index 000000000..c8a4cf73c --- /dev/null +++ b/config/moteus/middle_right.cfg @@ -0,0 +1,1397 @@ +uuid.uuid.0 105 +uuid.uuid.1 13 +uuid.uuid.2 118 +uuid.uuid.3 61 +uuid.uuid.4 27 +uuid.uuid.5 255 +uuid.uuid.6 74 +uuid.uuid.7 237 +uuid.uuid.8 130 +uuid.uuid.9 123 +uuid.uuid.10 70 +uuid.uuid.11 92 +uuid.uuid.12 86 +uuid.uuid.13 59 +uuid.uuid.14 11 +uuid.uuid.15 26 +clock.hsitrim 64 +aux1.i2c.i2c_hz 400000 +aux1.i2c.i2c_mode 1 +aux1.i2c.pullup 0 +aux1.i2c.devices.0.type 0 +aux1.i2c.devices.0.address 64 +aux1.i2c.devices.0.poll_ms 10 +aux1.i2c.devices.1.type 0 +aux1.i2c.devices.1.address 64 +aux1.i2c.devices.1.poll_ms 10 +aux1.i2c.devices.2.type 0 +aux1.i2c.devices.2.address 64 +aux1.i2c.devices.2.poll_ms 10 +aux1.spi.mode 1 +aux1.spi.rate_hz 12000000 +aux1.spi.filter_us 64 +aux1.spi.bct 0 +aux1.uart.mode 0 +aux1.uart.baud_rate 115200 +aux1.uart.poll_rate_us 100 +aux1.uart.rs422 0 +aux1.uart.cui_amt21_address 84 +aux1.quadrature.enabled 0 +aux1.quadrature.cpr 16384 +aux1.hall.enabled 1 +aux1.hall.polarity 0 +aux1.index.enabled 0 +aux1.sine_cosine.enabled 0 +aux1.sine_cosine.common 1700 +aux1.i2c_startup_delay_ms 30 +aux1.pins.0.mode 6 +aux1.pins.0.pull 1 +aux1.pins.1.mode 6 +aux1.pins.1.pull 1 +aux1.pins.2.mode 6 +aux1.pins.2.pull 1 +aux1.pins.3.mode 0 +aux1.pins.3.pull 0 +aux1.pins.4.mode 0 +aux1.pins.4.pull 0 +aux2.i2c.i2c_hz 400000 +aux2.i2c.i2c_mode 1 +aux2.i2c.pullup 0 +aux2.i2c.devices.0.type 0 +aux2.i2c.devices.0.address 64 +aux2.i2c.devices.0.poll_ms 10 +aux2.i2c.devices.1.type 0 +aux2.i2c.devices.1.address 64 +aux2.i2c.devices.1.poll_ms 10 +aux2.i2c.devices.2.type 0 +aux2.i2c.devices.2.address 64 +aux2.i2c.devices.2.poll_ms 10 +aux2.spi.mode 0 +aux2.spi.rate_hz 12000000 +aux2.spi.filter_us 64 +aux2.spi.bct 0 +aux2.uart.mode 0 +aux2.uart.baud_rate 115200 +aux2.uart.poll_rate_us 100 +aux2.uart.rs422 0 +aux2.uart.cui_amt21_address 84 +aux2.quadrature.enabled 0 +aux2.quadrature.cpr 16384 +aux2.hall.enabled 0 +aux2.hall.polarity 0 +aux2.index.enabled 0 +aux2.sine_cosine.enabled 0 +aux2.sine_cosine.common 1700 +aux2.i2c_startup_delay_ms 30 +aux2.pins.0.mode 0 +aux2.pins.0.pull 0 +aux2.pins.1.mode 0 +aux2.pins.1.pull 0 +aux2.pins.2.mode 0 +aux2.pins.2.pull 0 +aux2.pins.3.mode 0 +aux2.pins.3.pull 0 +aux2.pins.4.mode 0 +aux2.pins.4.pull 0 +motor_position.sources.0.aux_number 1 +motor_position.sources.0.type 4 +motor_position.sources.0.i2c_device 0 +motor_position.sources.0.incremental_index -1 +motor_position.sources.0.cpr 84 +motor_position.sources.0.offset -5.000000 +motor_position.sources.0.sign 1 +motor_position.sources.0.debug_override -1 +motor_position.sources.0.reference 0 +motor_position.sources.0.pll_filter_hz 60.370068 +motor_position.sources.0.compensation_table.0 0.000000 +motor_position.sources.0.compensation_table.1 0.000000 +motor_position.sources.0.compensation_table.2 0.000000 +motor_position.sources.0.compensation_table.3 0.000000 +motor_position.sources.0.compensation_table.4 0.000000 +motor_position.sources.0.compensation_table.5 0.000000 +motor_position.sources.0.compensation_table.6 0.000000 +motor_position.sources.0.compensation_table.7 0.000000 +motor_position.sources.0.compensation_table.8 0.000000 +motor_position.sources.0.compensation_table.9 0.000000 +motor_position.sources.0.compensation_table.10 0.000000 +motor_position.sources.0.compensation_table.11 0.000000 +motor_position.sources.0.compensation_table.12 0.000000 +motor_position.sources.0.compensation_table.13 0.000000 +motor_position.sources.0.compensation_table.14 0.000000 +motor_position.sources.0.compensation_table.15 0.000000 +motor_position.sources.0.compensation_table.16 0.000000 +motor_position.sources.0.compensation_table.17 0.000000 +motor_position.sources.0.compensation_table.18 0.000000 +motor_position.sources.0.compensation_table.19 0.000000 +motor_position.sources.0.compensation_table.20 0.000000 +motor_position.sources.0.compensation_table.21 0.000000 +motor_position.sources.0.compensation_table.22 0.000000 +motor_position.sources.0.compensation_table.23 0.000000 +motor_position.sources.0.compensation_table.24 0.000000 +motor_position.sources.0.compensation_table.25 0.000000 +motor_position.sources.0.compensation_table.26 0.000000 +motor_position.sources.0.compensation_table.27 0.000000 +motor_position.sources.0.compensation_table.28 0.000000 +motor_position.sources.0.compensation_table.29 0.000000 +motor_position.sources.0.compensation_table.30 0.000000 +motor_position.sources.0.compensation_table.31 0.000000 +motor_position.sources.1.aux_number 1 +motor_position.sources.1.type 0 +motor_position.sources.1.i2c_device 0 +motor_position.sources.1.incremental_index -1 +motor_position.sources.1.cpr 16384 +motor_position.sources.1.offset 0.000000 +motor_position.sources.1.sign 1 +motor_position.sources.1.debug_override -1 +motor_position.sources.1.reference 0 +motor_position.sources.1.pll_filter_hz 10.000000 +motor_position.sources.1.compensation_table.0 0.000000 +motor_position.sources.1.compensation_table.1 0.000000 +motor_position.sources.1.compensation_table.2 0.000000 +motor_position.sources.1.compensation_table.3 0.000000 +motor_position.sources.1.compensation_table.4 0.000000 +motor_position.sources.1.compensation_table.5 0.000000 +motor_position.sources.1.compensation_table.6 0.000000 +motor_position.sources.1.compensation_table.7 0.000000 +motor_position.sources.1.compensation_table.8 0.000000 +motor_position.sources.1.compensation_table.9 0.000000 +motor_position.sources.1.compensation_table.10 0.000000 +motor_position.sources.1.compensation_table.11 0.000000 +motor_position.sources.1.compensation_table.12 0.000000 +motor_position.sources.1.compensation_table.13 0.000000 +motor_position.sources.1.compensation_table.14 0.000000 +motor_position.sources.1.compensation_table.15 0.000000 +motor_position.sources.1.compensation_table.16 0.000000 +motor_position.sources.1.compensation_table.17 0.000000 +motor_position.sources.1.compensation_table.18 0.000000 +motor_position.sources.1.compensation_table.19 0.000000 +motor_position.sources.1.compensation_table.20 0.000000 +motor_position.sources.1.compensation_table.21 0.000000 +motor_position.sources.1.compensation_table.22 0.000000 +motor_position.sources.1.compensation_table.23 0.000000 +motor_position.sources.1.compensation_table.24 0.000000 +motor_position.sources.1.compensation_table.25 0.000000 +motor_position.sources.1.compensation_table.26 0.000000 +motor_position.sources.1.compensation_table.27 0.000000 +motor_position.sources.1.compensation_table.28 0.000000 +motor_position.sources.1.compensation_table.29 0.000000 +motor_position.sources.1.compensation_table.30 0.000000 +motor_position.sources.1.compensation_table.31 0.000000 +motor_position.sources.2.aux_number 1 +motor_position.sources.2.type 0 +motor_position.sources.2.i2c_device 0 +motor_position.sources.2.incremental_index -1 +motor_position.sources.2.cpr 16384 +motor_position.sources.2.offset 0.000000 +motor_position.sources.2.sign 1 +motor_position.sources.2.debug_override -1 +motor_position.sources.2.reference 0 +motor_position.sources.2.pll_filter_hz 400.000000 +motor_position.sources.2.compensation_table.0 0.000000 +motor_position.sources.2.compensation_table.1 0.000000 +motor_position.sources.2.compensation_table.2 0.000000 +motor_position.sources.2.compensation_table.3 0.000000 +motor_position.sources.2.compensation_table.4 0.000000 +motor_position.sources.2.compensation_table.5 0.000000 +motor_position.sources.2.compensation_table.6 0.000000 +motor_position.sources.2.compensation_table.7 0.000000 +motor_position.sources.2.compensation_table.8 0.000000 +motor_position.sources.2.compensation_table.9 0.000000 +motor_position.sources.2.compensation_table.10 0.000000 +motor_position.sources.2.compensation_table.11 0.000000 +motor_position.sources.2.compensation_table.12 0.000000 +motor_position.sources.2.compensation_table.13 0.000000 +motor_position.sources.2.compensation_table.14 0.000000 +motor_position.sources.2.compensation_table.15 0.000000 +motor_position.sources.2.compensation_table.16 0.000000 +motor_position.sources.2.compensation_table.17 0.000000 +motor_position.sources.2.compensation_table.18 0.000000 +motor_position.sources.2.compensation_table.19 0.000000 +motor_position.sources.2.compensation_table.20 0.000000 +motor_position.sources.2.compensation_table.21 0.000000 +motor_position.sources.2.compensation_table.22 0.000000 +motor_position.sources.2.compensation_table.23 0.000000 +motor_position.sources.2.compensation_table.24 0.000000 +motor_position.sources.2.compensation_table.25 0.000000 +motor_position.sources.2.compensation_table.26 0.000000 +motor_position.sources.2.compensation_table.27 0.000000 +motor_position.sources.2.compensation_table.28 0.000000 +motor_position.sources.2.compensation_table.29 0.000000 +motor_position.sources.2.compensation_table.30 0.000000 +motor_position.sources.2.compensation_table.31 0.000000 +motor_position.commutation_source 0 +motor_position.output.source 0 +motor_position.output.offset 0.000000 +motor_position.output.sign 1 +motor_position.output.reference_source -1 +motor_position.rotor_to_output_ratio 1.000000 +motor.poles 28 +motor.phase_invert 0 +motor.resistance_ohm 0.217140 +motor.v_per_hz 0.436649 +motor.offset.0 0.000000 +motor.offset.1 0.000000 +motor.offset.2 0.000000 +motor.offset.3 0.000000 +motor.offset.4 0.000000 +motor.offset.5 0.000000 +motor.offset.6 0.000000 +motor.offset.7 0.000000 +motor.offset.8 0.000000 +motor.offset.9 0.000000 +motor.offset.10 0.000000 +motor.offset.11 0.000000 +motor.offset.12 0.000000 +motor.offset.13 0.000000 +motor.offset.14 0.000000 +motor.offset.15 0.000000 +motor.offset.16 0.000000 +motor.offset.17 0.000000 +motor.offset.18 0.000000 +motor.offset.19 0.000000 +motor.offset.20 0.000000 +motor.offset.21 0.000000 +motor.offset.22 0.000000 +motor.offset.23 0.000000 +motor.offset.24 0.000000 +motor.offset.25 0.000000 +motor.offset.26 0.000000 +motor.offset.27 0.000000 +motor.offset.28 0.000000 +motor.offset.29 0.000000 +motor.offset.30 0.000000 +motor.offset.31 0.000000 +motor.offset.32 0.000000 +motor.offset.33 0.000000 +motor.offset.34 0.000000 +motor.offset.35 0.000000 +motor.offset.36 0.000000 +motor.offset.37 0.000000 +motor.offset.38 0.000000 +motor.offset.39 0.000000 +motor.offset.40 0.000000 +motor.offset.41 0.000000 +motor.offset.42 0.000000 +motor.offset.43 0.000000 +motor.offset.44 0.000000 +motor.offset.45 0.000000 +motor.offset.46 0.000000 +motor.offset.47 0.000000 +motor.offset.48 0.000000 +motor.offset.49 0.000000 +motor.offset.50 0.000000 +motor.offset.51 0.000000 +motor.offset.52 0.000000 +motor.offset.53 0.000000 +motor.offset.54 0.000000 +motor.offset.55 0.000000 +motor.offset.56 0.000000 +motor.offset.57 0.000000 +motor.offset.58 0.000000 +motor.offset.59 0.000000 +motor.offset.60 0.000000 +motor.offset.61 0.000000 +motor.offset.62 0.000000 +motor.offset.63 0.000000 +motor.rotation_current_cutoff_A 10000.000000 +motor.rotation_current_scale 0.050000 +motor.rotation_torque_scale 14.700000 +motor.cogging_dq_scale 0.000000 +motor.cogging_dq_comp.0 0 +motor.cogging_dq_comp.1 0 +motor.cogging_dq_comp.2 0 +motor.cogging_dq_comp.3 0 +motor.cogging_dq_comp.4 0 +motor.cogging_dq_comp.5 0 +motor.cogging_dq_comp.6 0 +motor.cogging_dq_comp.7 0 +motor.cogging_dq_comp.8 0 +motor.cogging_dq_comp.9 0 +motor.cogging_dq_comp.10 0 +motor.cogging_dq_comp.11 0 +motor.cogging_dq_comp.12 0 +motor.cogging_dq_comp.13 0 +motor.cogging_dq_comp.14 0 +motor.cogging_dq_comp.15 0 +motor.cogging_dq_comp.16 0 +motor.cogging_dq_comp.17 0 +motor.cogging_dq_comp.18 0 +motor.cogging_dq_comp.19 0 +motor.cogging_dq_comp.20 0 +motor.cogging_dq_comp.21 0 +motor.cogging_dq_comp.22 0 +motor.cogging_dq_comp.23 0 +motor.cogging_dq_comp.24 0 +motor.cogging_dq_comp.25 0 +motor.cogging_dq_comp.26 0 +motor.cogging_dq_comp.27 0 +motor.cogging_dq_comp.28 0 +motor.cogging_dq_comp.29 0 +motor.cogging_dq_comp.30 0 +motor.cogging_dq_comp.31 0 +motor.cogging_dq_comp.32 0 +motor.cogging_dq_comp.33 0 +motor.cogging_dq_comp.34 0 +motor.cogging_dq_comp.35 0 +motor.cogging_dq_comp.36 0 +motor.cogging_dq_comp.37 0 +motor.cogging_dq_comp.38 0 +motor.cogging_dq_comp.39 0 +motor.cogging_dq_comp.40 0 +motor.cogging_dq_comp.41 0 +motor.cogging_dq_comp.42 0 +motor.cogging_dq_comp.43 0 +motor.cogging_dq_comp.44 0 +motor.cogging_dq_comp.45 0 +motor.cogging_dq_comp.46 0 +motor.cogging_dq_comp.47 0 +motor.cogging_dq_comp.48 0 +motor.cogging_dq_comp.49 0 +motor.cogging_dq_comp.50 0 +motor.cogging_dq_comp.51 0 +motor.cogging_dq_comp.52 0 +motor.cogging_dq_comp.53 0 +motor.cogging_dq_comp.54 0 +motor.cogging_dq_comp.55 0 +motor.cogging_dq_comp.56 0 +motor.cogging_dq_comp.57 0 +motor.cogging_dq_comp.58 0 +motor.cogging_dq_comp.59 0 +motor.cogging_dq_comp.60 0 +motor.cogging_dq_comp.61 0 +motor.cogging_dq_comp.62 0 +motor.cogging_dq_comp.63 0 +motor.cogging_dq_comp.64 0 +motor.cogging_dq_comp.65 0 +motor.cogging_dq_comp.66 0 +motor.cogging_dq_comp.67 0 +motor.cogging_dq_comp.68 0 +motor.cogging_dq_comp.69 0 +motor.cogging_dq_comp.70 0 +motor.cogging_dq_comp.71 0 +motor.cogging_dq_comp.72 0 +motor.cogging_dq_comp.73 0 +motor.cogging_dq_comp.74 0 +motor.cogging_dq_comp.75 0 +motor.cogging_dq_comp.76 0 +motor.cogging_dq_comp.77 0 +motor.cogging_dq_comp.78 0 +motor.cogging_dq_comp.79 0 +motor.cogging_dq_comp.80 0 +motor.cogging_dq_comp.81 0 +motor.cogging_dq_comp.82 0 +motor.cogging_dq_comp.83 0 +motor.cogging_dq_comp.84 0 +motor.cogging_dq_comp.85 0 +motor.cogging_dq_comp.86 0 +motor.cogging_dq_comp.87 0 +motor.cogging_dq_comp.88 0 +motor.cogging_dq_comp.89 0 +motor.cogging_dq_comp.90 0 +motor.cogging_dq_comp.91 0 +motor.cogging_dq_comp.92 0 +motor.cogging_dq_comp.93 0 +motor.cogging_dq_comp.94 0 +motor.cogging_dq_comp.95 0 +motor.cogging_dq_comp.96 0 +motor.cogging_dq_comp.97 0 +motor.cogging_dq_comp.98 0 +motor.cogging_dq_comp.99 0 +motor.cogging_dq_comp.100 0 +motor.cogging_dq_comp.101 0 +motor.cogging_dq_comp.102 0 +motor.cogging_dq_comp.103 0 +motor.cogging_dq_comp.104 0 +motor.cogging_dq_comp.105 0 +motor.cogging_dq_comp.106 0 +motor.cogging_dq_comp.107 0 +motor.cogging_dq_comp.108 0 +motor.cogging_dq_comp.109 0 +motor.cogging_dq_comp.110 0 +motor.cogging_dq_comp.111 0 +motor.cogging_dq_comp.112 0 +motor.cogging_dq_comp.113 0 +motor.cogging_dq_comp.114 0 +motor.cogging_dq_comp.115 0 +motor.cogging_dq_comp.116 0 +motor.cogging_dq_comp.117 0 +motor.cogging_dq_comp.118 0 +motor.cogging_dq_comp.119 0 +motor.cogging_dq_comp.120 0 +motor.cogging_dq_comp.121 0 +motor.cogging_dq_comp.122 0 +motor.cogging_dq_comp.123 0 +motor.cogging_dq_comp.124 0 +motor.cogging_dq_comp.125 0 +motor.cogging_dq_comp.126 0 +motor.cogging_dq_comp.127 0 +motor.cogging_dq_comp.128 0 +motor.cogging_dq_comp.129 0 +motor.cogging_dq_comp.130 0 +motor.cogging_dq_comp.131 0 +motor.cogging_dq_comp.132 0 +motor.cogging_dq_comp.133 0 +motor.cogging_dq_comp.134 0 +motor.cogging_dq_comp.135 0 +motor.cogging_dq_comp.136 0 +motor.cogging_dq_comp.137 0 +motor.cogging_dq_comp.138 0 +motor.cogging_dq_comp.139 0 +motor.cogging_dq_comp.140 0 +motor.cogging_dq_comp.141 0 +motor.cogging_dq_comp.142 0 +motor.cogging_dq_comp.143 0 +motor.cogging_dq_comp.144 0 +motor.cogging_dq_comp.145 0 +motor.cogging_dq_comp.146 0 +motor.cogging_dq_comp.147 0 +motor.cogging_dq_comp.148 0 +motor.cogging_dq_comp.149 0 +motor.cogging_dq_comp.150 0 +motor.cogging_dq_comp.151 0 +motor.cogging_dq_comp.152 0 +motor.cogging_dq_comp.153 0 +motor.cogging_dq_comp.154 0 +motor.cogging_dq_comp.155 0 +motor.cogging_dq_comp.156 0 +motor.cogging_dq_comp.157 0 +motor.cogging_dq_comp.158 0 +motor.cogging_dq_comp.159 0 +motor.cogging_dq_comp.160 0 +motor.cogging_dq_comp.161 0 +motor.cogging_dq_comp.162 0 +motor.cogging_dq_comp.163 0 +motor.cogging_dq_comp.164 0 +motor.cogging_dq_comp.165 0 +motor.cogging_dq_comp.166 0 +motor.cogging_dq_comp.167 0 +motor.cogging_dq_comp.168 0 +motor.cogging_dq_comp.169 0 +motor.cogging_dq_comp.170 0 +motor.cogging_dq_comp.171 0 +motor.cogging_dq_comp.172 0 +motor.cogging_dq_comp.173 0 +motor.cogging_dq_comp.174 0 +motor.cogging_dq_comp.175 0 +motor.cogging_dq_comp.176 0 +motor.cogging_dq_comp.177 0 +motor.cogging_dq_comp.178 0 +motor.cogging_dq_comp.179 0 +motor.cogging_dq_comp.180 0 +motor.cogging_dq_comp.181 0 +motor.cogging_dq_comp.182 0 +motor.cogging_dq_comp.183 0 +motor.cogging_dq_comp.184 0 +motor.cogging_dq_comp.185 0 +motor.cogging_dq_comp.186 0 +motor.cogging_dq_comp.187 0 +motor.cogging_dq_comp.188 0 +motor.cogging_dq_comp.189 0 +motor.cogging_dq_comp.190 0 +motor.cogging_dq_comp.191 0 +motor.cogging_dq_comp.192 0 +motor.cogging_dq_comp.193 0 +motor.cogging_dq_comp.194 0 +motor.cogging_dq_comp.195 0 +motor.cogging_dq_comp.196 0 +motor.cogging_dq_comp.197 0 +motor.cogging_dq_comp.198 0 +motor.cogging_dq_comp.199 0 +motor.cogging_dq_comp.200 0 +motor.cogging_dq_comp.201 0 +motor.cogging_dq_comp.202 0 +motor.cogging_dq_comp.203 0 +motor.cogging_dq_comp.204 0 +motor.cogging_dq_comp.205 0 +motor.cogging_dq_comp.206 0 +motor.cogging_dq_comp.207 0 +motor.cogging_dq_comp.208 0 +motor.cogging_dq_comp.209 0 +motor.cogging_dq_comp.210 0 +motor.cogging_dq_comp.211 0 +motor.cogging_dq_comp.212 0 +motor.cogging_dq_comp.213 0 +motor.cogging_dq_comp.214 0 +motor.cogging_dq_comp.215 0 +motor.cogging_dq_comp.216 0 +motor.cogging_dq_comp.217 0 +motor.cogging_dq_comp.218 0 +motor.cogging_dq_comp.219 0 +motor.cogging_dq_comp.220 0 +motor.cogging_dq_comp.221 0 +motor.cogging_dq_comp.222 0 +motor.cogging_dq_comp.223 0 +motor.cogging_dq_comp.224 0 +motor.cogging_dq_comp.225 0 +motor.cogging_dq_comp.226 0 +motor.cogging_dq_comp.227 0 +motor.cogging_dq_comp.228 0 +motor.cogging_dq_comp.229 0 +motor.cogging_dq_comp.230 0 +motor.cogging_dq_comp.231 0 +motor.cogging_dq_comp.232 0 +motor.cogging_dq_comp.233 0 +motor.cogging_dq_comp.234 0 +motor.cogging_dq_comp.235 0 +motor.cogging_dq_comp.236 0 +motor.cogging_dq_comp.237 0 +motor.cogging_dq_comp.238 0 +motor.cogging_dq_comp.239 0 +motor.cogging_dq_comp.240 0 +motor.cogging_dq_comp.241 0 +motor.cogging_dq_comp.242 0 +motor.cogging_dq_comp.243 0 +motor.cogging_dq_comp.244 0 +motor.cogging_dq_comp.245 0 +motor.cogging_dq_comp.246 0 +motor.cogging_dq_comp.247 0 +motor.cogging_dq_comp.248 0 +motor.cogging_dq_comp.249 0 +motor.cogging_dq_comp.250 0 +motor.cogging_dq_comp.251 0 +motor.cogging_dq_comp.252 0 +motor.cogging_dq_comp.253 0 +motor.cogging_dq_comp.254 0 +motor.cogging_dq_comp.255 0 +motor.cogging_dq_comp.256 0 +motor.cogging_dq_comp.257 0 +motor.cogging_dq_comp.258 0 +motor.cogging_dq_comp.259 0 +motor.cogging_dq_comp.260 0 +motor.cogging_dq_comp.261 0 +motor.cogging_dq_comp.262 0 +motor.cogging_dq_comp.263 0 +motor.cogging_dq_comp.264 0 +motor.cogging_dq_comp.265 0 +motor.cogging_dq_comp.266 0 +motor.cogging_dq_comp.267 0 +motor.cogging_dq_comp.268 0 +motor.cogging_dq_comp.269 0 +motor.cogging_dq_comp.270 0 +motor.cogging_dq_comp.271 0 +motor.cogging_dq_comp.272 0 +motor.cogging_dq_comp.273 0 +motor.cogging_dq_comp.274 0 +motor.cogging_dq_comp.275 0 +motor.cogging_dq_comp.276 0 +motor.cogging_dq_comp.277 0 +motor.cogging_dq_comp.278 0 +motor.cogging_dq_comp.279 0 +motor.cogging_dq_comp.280 0 +motor.cogging_dq_comp.281 0 +motor.cogging_dq_comp.282 0 +motor.cogging_dq_comp.283 0 +motor.cogging_dq_comp.284 0 +motor.cogging_dq_comp.285 0 +motor.cogging_dq_comp.286 0 +motor.cogging_dq_comp.287 0 +motor.cogging_dq_comp.288 0 +motor.cogging_dq_comp.289 0 +motor.cogging_dq_comp.290 0 +motor.cogging_dq_comp.291 0 +motor.cogging_dq_comp.292 0 +motor.cogging_dq_comp.293 0 +motor.cogging_dq_comp.294 0 +motor.cogging_dq_comp.295 0 +motor.cogging_dq_comp.296 0 +motor.cogging_dq_comp.297 0 +motor.cogging_dq_comp.298 0 +motor.cogging_dq_comp.299 0 +motor.cogging_dq_comp.300 0 +motor.cogging_dq_comp.301 0 +motor.cogging_dq_comp.302 0 +motor.cogging_dq_comp.303 0 +motor.cogging_dq_comp.304 0 +motor.cogging_dq_comp.305 0 +motor.cogging_dq_comp.306 0 +motor.cogging_dq_comp.307 0 +motor.cogging_dq_comp.308 0 +motor.cogging_dq_comp.309 0 +motor.cogging_dq_comp.310 0 +motor.cogging_dq_comp.311 0 +motor.cogging_dq_comp.312 0 +motor.cogging_dq_comp.313 0 +motor.cogging_dq_comp.314 0 +motor.cogging_dq_comp.315 0 +motor.cogging_dq_comp.316 0 +motor.cogging_dq_comp.317 0 +motor.cogging_dq_comp.318 0 +motor.cogging_dq_comp.319 0 +motor.cogging_dq_comp.320 0 +motor.cogging_dq_comp.321 0 +motor.cogging_dq_comp.322 0 +motor.cogging_dq_comp.323 0 +motor.cogging_dq_comp.324 0 +motor.cogging_dq_comp.325 0 +motor.cogging_dq_comp.326 0 +motor.cogging_dq_comp.327 0 +motor.cogging_dq_comp.328 0 +motor.cogging_dq_comp.329 0 +motor.cogging_dq_comp.330 0 +motor.cogging_dq_comp.331 0 +motor.cogging_dq_comp.332 0 +motor.cogging_dq_comp.333 0 +motor.cogging_dq_comp.334 0 +motor.cogging_dq_comp.335 0 +motor.cogging_dq_comp.336 0 +motor.cogging_dq_comp.337 0 +motor.cogging_dq_comp.338 0 +motor.cogging_dq_comp.339 0 +motor.cogging_dq_comp.340 0 +motor.cogging_dq_comp.341 0 +motor.cogging_dq_comp.342 0 +motor.cogging_dq_comp.343 0 +motor.cogging_dq_comp.344 0 +motor.cogging_dq_comp.345 0 +motor.cogging_dq_comp.346 0 +motor.cogging_dq_comp.347 0 +motor.cogging_dq_comp.348 0 +motor.cogging_dq_comp.349 0 +motor.cogging_dq_comp.350 0 +motor.cogging_dq_comp.351 0 +motor.cogging_dq_comp.352 0 +motor.cogging_dq_comp.353 0 +motor.cogging_dq_comp.354 0 +motor.cogging_dq_comp.355 0 +motor.cogging_dq_comp.356 0 +motor.cogging_dq_comp.357 0 +motor.cogging_dq_comp.358 0 +motor.cogging_dq_comp.359 0 +motor.cogging_dq_comp.360 0 +motor.cogging_dq_comp.361 0 +motor.cogging_dq_comp.362 0 +motor.cogging_dq_comp.363 0 +motor.cogging_dq_comp.364 0 +motor.cogging_dq_comp.365 0 +motor.cogging_dq_comp.366 0 +motor.cogging_dq_comp.367 0 +motor.cogging_dq_comp.368 0 +motor.cogging_dq_comp.369 0 +motor.cogging_dq_comp.370 0 +motor.cogging_dq_comp.371 0 +motor.cogging_dq_comp.372 0 +motor.cogging_dq_comp.373 0 +motor.cogging_dq_comp.374 0 +motor.cogging_dq_comp.375 0 +motor.cogging_dq_comp.376 0 +motor.cogging_dq_comp.377 0 +motor.cogging_dq_comp.378 0 +motor.cogging_dq_comp.379 0 +motor.cogging_dq_comp.380 0 +motor.cogging_dq_comp.381 0 +motor.cogging_dq_comp.382 0 +motor.cogging_dq_comp.383 0 +motor.cogging_dq_comp.384 0 +motor.cogging_dq_comp.385 0 +motor.cogging_dq_comp.386 0 +motor.cogging_dq_comp.387 0 +motor.cogging_dq_comp.388 0 +motor.cogging_dq_comp.389 0 +motor.cogging_dq_comp.390 0 +motor.cogging_dq_comp.391 0 +motor.cogging_dq_comp.392 0 +motor.cogging_dq_comp.393 0 +motor.cogging_dq_comp.394 0 +motor.cogging_dq_comp.395 0 +motor.cogging_dq_comp.396 0 +motor.cogging_dq_comp.397 0 +motor.cogging_dq_comp.398 0 +motor.cogging_dq_comp.399 0 +motor.cogging_dq_comp.400 0 +motor.cogging_dq_comp.401 0 +motor.cogging_dq_comp.402 0 +motor.cogging_dq_comp.403 0 +motor.cogging_dq_comp.404 0 +motor.cogging_dq_comp.405 0 +motor.cogging_dq_comp.406 0 +motor.cogging_dq_comp.407 0 +motor.cogging_dq_comp.408 0 +motor.cogging_dq_comp.409 0 +motor.cogging_dq_comp.410 0 +motor.cogging_dq_comp.411 0 +motor.cogging_dq_comp.412 0 +motor.cogging_dq_comp.413 0 +motor.cogging_dq_comp.414 0 +motor.cogging_dq_comp.415 0 +motor.cogging_dq_comp.416 0 +motor.cogging_dq_comp.417 0 +motor.cogging_dq_comp.418 0 +motor.cogging_dq_comp.419 0 +motor.cogging_dq_comp.420 0 +motor.cogging_dq_comp.421 0 +motor.cogging_dq_comp.422 0 +motor.cogging_dq_comp.423 0 +motor.cogging_dq_comp.424 0 +motor.cogging_dq_comp.425 0 +motor.cogging_dq_comp.426 0 +motor.cogging_dq_comp.427 0 +motor.cogging_dq_comp.428 0 +motor.cogging_dq_comp.429 0 +motor.cogging_dq_comp.430 0 +motor.cogging_dq_comp.431 0 +motor.cogging_dq_comp.432 0 +motor.cogging_dq_comp.433 0 +motor.cogging_dq_comp.434 0 +motor.cogging_dq_comp.435 0 +motor.cogging_dq_comp.436 0 +motor.cogging_dq_comp.437 0 +motor.cogging_dq_comp.438 0 +motor.cogging_dq_comp.439 0 +motor.cogging_dq_comp.440 0 +motor.cogging_dq_comp.441 0 +motor.cogging_dq_comp.442 0 +motor.cogging_dq_comp.443 0 +motor.cogging_dq_comp.444 0 +motor.cogging_dq_comp.445 0 +motor.cogging_dq_comp.446 0 +motor.cogging_dq_comp.447 0 +motor.cogging_dq_comp.448 0 +motor.cogging_dq_comp.449 0 +motor.cogging_dq_comp.450 0 +motor.cogging_dq_comp.451 0 +motor.cogging_dq_comp.452 0 +motor.cogging_dq_comp.453 0 +motor.cogging_dq_comp.454 0 +motor.cogging_dq_comp.455 0 +motor.cogging_dq_comp.456 0 +motor.cogging_dq_comp.457 0 +motor.cogging_dq_comp.458 0 +motor.cogging_dq_comp.459 0 +motor.cogging_dq_comp.460 0 +motor.cogging_dq_comp.461 0 +motor.cogging_dq_comp.462 0 +motor.cogging_dq_comp.463 0 +motor.cogging_dq_comp.464 0 +motor.cogging_dq_comp.465 0 +motor.cogging_dq_comp.466 0 +motor.cogging_dq_comp.467 0 +motor.cogging_dq_comp.468 0 +motor.cogging_dq_comp.469 0 +motor.cogging_dq_comp.470 0 +motor.cogging_dq_comp.471 0 +motor.cogging_dq_comp.472 0 +motor.cogging_dq_comp.473 0 +motor.cogging_dq_comp.474 0 +motor.cogging_dq_comp.475 0 +motor.cogging_dq_comp.476 0 +motor.cogging_dq_comp.477 0 +motor.cogging_dq_comp.478 0 +motor.cogging_dq_comp.479 0 +motor.cogging_dq_comp.480 0 +motor.cogging_dq_comp.481 0 +motor.cogging_dq_comp.482 0 +motor.cogging_dq_comp.483 0 +motor.cogging_dq_comp.484 0 +motor.cogging_dq_comp.485 0 +motor.cogging_dq_comp.486 0 +motor.cogging_dq_comp.487 0 +motor.cogging_dq_comp.488 0 +motor.cogging_dq_comp.489 0 +motor.cogging_dq_comp.490 0 +motor.cogging_dq_comp.491 0 +motor.cogging_dq_comp.492 0 +motor.cogging_dq_comp.493 0 +motor.cogging_dq_comp.494 0 +motor.cogging_dq_comp.495 0 +motor.cogging_dq_comp.496 0 +motor.cogging_dq_comp.497 0 +motor.cogging_dq_comp.498 0 +motor.cogging_dq_comp.499 0 +motor.cogging_dq_comp.500 0 +motor.cogging_dq_comp.501 0 +motor.cogging_dq_comp.502 0 +motor.cogging_dq_comp.503 0 +motor.cogging_dq_comp.504 0 +motor.cogging_dq_comp.505 0 +motor.cogging_dq_comp.506 0 +motor.cogging_dq_comp.507 0 +motor.cogging_dq_comp.508 0 +motor.cogging_dq_comp.509 0 +motor.cogging_dq_comp.510 0 +motor.cogging_dq_comp.511 0 +motor.cogging_dq_comp.512 0 +motor.cogging_dq_comp.513 0 +motor.cogging_dq_comp.514 0 +motor.cogging_dq_comp.515 0 +motor.cogging_dq_comp.516 0 +motor.cogging_dq_comp.517 0 +motor.cogging_dq_comp.518 0 +motor.cogging_dq_comp.519 0 +motor.cogging_dq_comp.520 0 +motor.cogging_dq_comp.521 0 +motor.cogging_dq_comp.522 0 +motor.cogging_dq_comp.523 0 +motor.cogging_dq_comp.524 0 +motor.cogging_dq_comp.525 0 +motor.cogging_dq_comp.526 0 +motor.cogging_dq_comp.527 0 +motor.cogging_dq_comp.528 0 +motor.cogging_dq_comp.529 0 +motor.cogging_dq_comp.530 0 +motor.cogging_dq_comp.531 0 +motor.cogging_dq_comp.532 0 +motor.cogging_dq_comp.533 0 +motor.cogging_dq_comp.534 0 +motor.cogging_dq_comp.535 0 +motor.cogging_dq_comp.536 0 +motor.cogging_dq_comp.537 0 +motor.cogging_dq_comp.538 0 +motor.cogging_dq_comp.539 0 +motor.cogging_dq_comp.540 0 +motor.cogging_dq_comp.541 0 +motor.cogging_dq_comp.542 0 +motor.cogging_dq_comp.543 0 +motor.cogging_dq_comp.544 0 +motor.cogging_dq_comp.545 0 +motor.cogging_dq_comp.546 0 +motor.cogging_dq_comp.547 0 +motor.cogging_dq_comp.548 0 +motor.cogging_dq_comp.549 0 +motor.cogging_dq_comp.550 0 +motor.cogging_dq_comp.551 0 +motor.cogging_dq_comp.552 0 +motor.cogging_dq_comp.553 0 +motor.cogging_dq_comp.554 0 +motor.cogging_dq_comp.555 0 +motor.cogging_dq_comp.556 0 +motor.cogging_dq_comp.557 0 +motor.cogging_dq_comp.558 0 +motor.cogging_dq_comp.559 0 +motor.cogging_dq_comp.560 0 +motor.cogging_dq_comp.561 0 +motor.cogging_dq_comp.562 0 +motor.cogging_dq_comp.563 0 +motor.cogging_dq_comp.564 0 +motor.cogging_dq_comp.565 0 +motor.cogging_dq_comp.566 0 +motor.cogging_dq_comp.567 0 +motor.cogging_dq_comp.568 0 +motor.cogging_dq_comp.569 0 +motor.cogging_dq_comp.570 0 +motor.cogging_dq_comp.571 0 +motor.cogging_dq_comp.572 0 +motor.cogging_dq_comp.573 0 +motor.cogging_dq_comp.574 0 +motor.cogging_dq_comp.575 0 +motor.cogging_dq_comp.576 0 +motor.cogging_dq_comp.577 0 +motor.cogging_dq_comp.578 0 +motor.cogging_dq_comp.579 0 +motor.cogging_dq_comp.580 0 +motor.cogging_dq_comp.581 0 +motor.cogging_dq_comp.582 0 +motor.cogging_dq_comp.583 0 +motor.cogging_dq_comp.584 0 +motor.cogging_dq_comp.585 0 +motor.cogging_dq_comp.586 0 +motor.cogging_dq_comp.587 0 +motor.cogging_dq_comp.588 0 +motor.cogging_dq_comp.589 0 +motor.cogging_dq_comp.590 0 +motor.cogging_dq_comp.591 0 +motor.cogging_dq_comp.592 0 +motor.cogging_dq_comp.593 0 +motor.cogging_dq_comp.594 0 +motor.cogging_dq_comp.595 0 +motor.cogging_dq_comp.596 0 +motor.cogging_dq_comp.597 0 +motor.cogging_dq_comp.598 0 +motor.cogging_dq_comp.599 0 +motor.cogging_dq_comp.600 0 +motor.cogging_dq_comp.601 0 +motor.cogging_dq_comp.602 0 +motor.cogging_dq_comp.603 0 +motor.cogging_dq_comp.604 0 +motor.cogging_dq_comp.605 0 +motor.cogging_dq_comp.606 0 +motor.cogging_dq_comp.607 0 +motor.cogging_dq_comp.608 0 +motor.cogging_dq_comp.609 0 +motor.cogging_dq_comp.610 0 +motor.cogging_dq_comp.611 0 +motor.cogging_dq_comp.612 0 +motor.cogging_dq_comp.613 0 +motor.cogging_dq_comp.614 0 +motor.cogging_dq_comp.615 0 +motor.cogging_dq_comp.616 0 +motor.cogging_dq_comp.617 0 +motor.cogging_dq_comp.618 0 +motor.cogging_dq_comp.619 0 +motor.cogging_dq_comp.620 0 +motor.cogging_dq_comp.621 0 +motor.cogging_dq_comp.622 0 +motor.cogging_dq_comp.623 0 +motor.cogging_dq_comp.624 0 +motor.cogging_dq_comp.625 0 +motor.cogging_dq_comp.626 0 +motor.cogging_dq_comp.627 0 +motor.cogging_dq_comp.628 0 +motor.cogging_dq_comp.629 0 +motor.cogging_dq_comp.630 0 +motor.cogging_dq_comp.631 0 +motor.cogging_dq_comp.632 0 +motor.cogging_dq_comp.633 0 +motor.cogging_dq_comp.634 0 +motor.cogging_dq_comp.635 0 +motor.cogging_dq_comp.636 0 +motor.cogging_dq_comp.637 0 +motor.cogging_dq_comp.638 0 +motor.cogging_dq_comp.639 0 +motor.cogging_dq_comp.640 0 +motor.cogging_dq_comp.641 0 +motor.cogging_dq_comp.642 0 +motor.cogging_dq_comp.643 0 +motor.cogging_dq_comp.644 0 +motor.cogging_dq_comp.645 0 +motor.cogging_dq_comp.646 0 +motor.cogging_dq_comp.647 0 +motor.cogging_dq_comp.648 0 +motor.cogging_dq_comp.649 0 +motor.cogging_dq_comp.650 0 +motor.cogging_dq_comp.651 0 +motor.cogging_dq_comp.652 0 +motor.cogging_dq_comp.653 0 +motor.cogging_dq_comp.654 0 +motor.cogging_dq_comp.655 0 +motor.cogging_dq_comp.656 0 +motor.cogging_dq_comp.657 0 +motor.cogging_dq_comp.658 0 +motor.cogging_dq_comp.659 0 +motor.cogging_dq_comp.660 0 +motor.cogging_dq_comp.661 0 +motor.cogging_dq_comp.662 0 +motor.cogging_dq_comp.663 0 +motor.cogging_dq_comp.664 0 +motor.cogging_dq_comp.665 0 +motor.cogging_dq_comp.666 0 +motor.cogging_dq_comp.667 0 +motor.cogging_dq_comp.668 0 +motor.cogging_dq_comp.669 0 +motor.cogging_dq_comp.670 0 +motor.cogging_dq_comp.671 0 +motor.cogging_dq_comp.672 0 +motor.cogging_dq_comp.673 0 +motor.cogging_dq_comp.674 0 +motor.cogging_dq_comp.675 0 +motor.cogging_dq_comp.676 0 +motor.cogging_dq_comp.677 0 +motor.cogging_dq_comp.678 0 +motor.cogging_dq_comp.679 0 +motor.cogging_dq_comp.680 0 +motor.cogging_dq_comp.681 0 +motor.cogging_dq_comp.682 0 +motor.cogging_dq_comp.683 0 +motor.cogging_dq_comp.684 0 +motor.cogging_dq_comp.685 0 +motor.cogging_dq_comp.686 0 +motor.cogging_dq_comp.687 0 +motor.cogging_dq_comp.688 0 +motor.cogging_dq_comp.689 0 +motor.cogging_dq_comp.690 0 +motor.cogging_dq_comp.691 0 +motor.cogging_dq_comp.692 0 +motor.cogging_dq_comp.693 0 +motor.cogging_dq_comp.694 0 +motor.cogging_dq_comp.695 0 +motor.cogging_dq_comp.696 0 +motor.cogging_dq_comp.697 0 +motor.cogging_dq_comp.698 0 +motor.cogging_dq_comp.699 0 +motor.cogging_dq_comp.700 0 +motor.cogging_dq_comp.701 0 +motor.cogging_dq_comp.702 0 +motor.cogging_dq_comp.703 0 +motor.cogging_dq_comp.704 0 +motor.cogging_dq_comp.705 0 +motor.cogging_dq_comp.706 0 +motor.cogging_dq_comp.707 0 +motor.cogging_dq_comp.708 0 +motor.cogging_dq_comp.709 0 +motor.cogging_dq_comp.710 0 +motor.cogging_dq_comp.711 0 +motor.cogging_dq_comp.712 0 +motor.cogging_dq_comp.713 0 +motor.cogging_dq_comp.714 0 +motor.cogging_dq_comp.715 0 +motor.cogging_dq_comp.716 0 +motor.cogging_dq_comp.717 0 +motor.cogging_dq_comp.718 0 +motor.cogging_dq_comp.719 0 +motor.cogging_dq_comp.720 0 +motor.cogging_dq_comp.721 0 +motor.cogging_dq_comp.722 0 +motor.cogging_dq_comp.723 0 +motor.cogging_dq_comp.724 0 +motor.cogging_dq_comp.725 0 +motor.cogging_dq_comp.726 0 +motor.cogging_dq_comp.727 0 +motor.cogging_dq_comp.728 0 +motor.cogging_dq_comp.729 0 +motor.cogging_dq_comp.730 0 +motor.cogging_dq_comp.731 0 +motor.cogging_dq_comp.732 0 +motor.cogging_dq_comp.733 0 +motor.cogging_dq_comp.734 0 +motor.cogging_dq_comp.735 0 +motor.cogging_dq_comp.736 0 +motor.cogging_dq_comp.737 0 +motor.cogging_dq_comp.738 0 +motor.cogging_dq_comp.739 0 +motor.cogging_dq_comp.740 0 +motor.cogging_dq_comp.741 0 +motor.cogging_dq_comp.742 0 +motor.cogging_dq_comp.743 0 +motor.cogging_dq_comp.744 0 +motor.cogging_dq_comp.745 0 +motor.cogging_dq_comp.746 0 +motor.cogging_dq_comp.747 0 +motor.cogging_dq_comp.748 0 +motor.cogging_dq_comp.749 0 +motor.cogging_dq_comp.750 0 +motor.cogging_dq_comp.751 0 +motor.cogging_dq_comp.752 0 +motor.cogging_dq_comp.753 0 +motor.cogging_dq_comp.754 0 +motor.cogging_dq_comp.755 0 +motor.cogging_dq_comp.756 0 +motor.cogging_dq_comp.757 0 +motor.cogging_dq_comp.758 0 +motor.cogging_dq_comp.759 0 +motor.cogging_dq_comp.760 0 +motor.cogging_dq_comp.761 0 +motor.cogging_dq_comp.762 0 +motor.cogging_dq_comp.763 0 +motor.cogging_dq_comp.764 0 +motor.cogging_dq_comp.765 0 +motor.cogging_dq_comp.766 0 +motor.cogging_dq_comp.767 0 +motor.cogging_dq_comp.768 0 +motor.cogging_dq_comp.769 0 +motor.cogging_dq_comp.770 0 +motor.cogging_dq_comp.771 0 +motor.cogging_dq_comp.772 0 +motor.cogging_dq_comp.773 0 +motor.cogging_dq_comp.774 0 +motor.cogging_dq_comp.775 0 +motor.cogging_dq_comp.776 0 +motor.cogging_dq_comp.777 0 +motor.cogging_dq_comp.778 0 +motor.cogging_dq_comp.779 0 +motor.cogging_dq_comp.780 0 +motor.cogging_dq_comp.781 0 +motor.cogging_dq_comp.782 0 +motor.cogging_dq_comp.783 0 +motor.cogging_dq_comp.784 0 +motor.cogging_dq_comp.785 0 +motor.cogging_dq_comp.786 0 +motor.cogging_dq_comp.787 0 +motor.cogging_dq_comp.788 0 +motor.cogging_dq_comp.789 0 +motor.cogging_dq_comp.790 0 +motor.cogging_dq_comp.791 0 +motor.cogging_dq_comp.792 0 +motor.cogging_dq_comp.793 0 +motor.cogging_dq_comp.794 0 +motor.cogging_dq_comp.795 0 +motor.cogging_dq_comp.796 0 +motor.cogging_dq_comp.797 0 +motor.cogging_dq_comp.798 0 +motor.cogging_dq_comp.799 0 +motor.cogging_dq_comp.800 0 +motor.cogging_dq_comp.801 0 +motor.cogging_dq_comp.802 0 +motor.cogging_dq_comp.803 0 +motor.cogging_dq_comp.804 0 +motor.cogging_dq_comp.805 0 +motor.cogging_dq_comp.806 0 +motor.cogging_dq_comp.807 0 +motor.cogging_dq_comp.808 0 +motor.cogging_dq_comp.809 0 +motor.cogging_dq_comp.810 0 +motor.cogging_dq_comp.811 0 +motor.cogging_dq_comp.812 0 +motor.cogging_dq_comp.813 0 +motor.cogging_dq_comp.814 0 +motor.cogging_dq_comp.815 0 +motor.cogging_dq_comp.816 0 +motor.cogging_dq_comp.817 0 +motor.cogging_dq_comp.818 0 +motor.cogging_dq_comp.819 0 +motor.cogging_dq_comp.820 0 +motor.cogging_dq_comp.821 0 +motor.cogging_dq_comp.822 0 +motor.cogging_dq_comp.823 0 +motor.cogging_dq_comp.824 0 +motor.cogging_dq_comp.825 0 +motor.cogging_dq_comp.826 0 +motor.cogging_dq_comp.827 0 +motor.cogging_dq_comp.828 0 +motor.cogging_dq_comp.829 0 +motor.cogging_dq_comp.830 0 +motor.cogging_dq_comp.831 0 +motor.cogging_dq_comp.832 0 +motor.cogging_dq_comp.833 0 +motor.cogging_dq_comp.834 0 +motor.cogging_dq_comp.835 0 +motor.cogging_dq_comp.836 0 +motor.cogging_dq_comp.837 0 +motor.cogging_dq_comp.838 0 +motor.cogging_dq_comp.839 0 +motor.cogging_dq_comp.840 0 +motor.cogging_dq_comp.841 0 +motor.cogging_dq_comp.842 0 +motor.cogging_dq_comp.843 0 +motor.cogging_dq_comp.844 0 +motor.cogging_dq_comp.845 0 +motor.cogging_dq_comp.846 0 +motor.cogging_dq_comp.847 0 +motor.cogging_dq_comp.848 0 +motor.cogging_dq_comp.849 0 +motor.cogging_dq_comp.850 0 +motor.cogging_dq_comp.851 0 +motor.cogging_dq_comp.852 0 +motor.cogging_dq_comp.853 0 +motor.cogging_dq_comp.854 0 +motor.cogging_dq_comp.855 0 +motor.cogging_dq_comp.856 0 +motor.cogging_dq_comp.857 0 +motor.cogging_dq_comp.858 0 +motor.cogging_dq_comp.859 0 +motor.cogging_dq_comp.860 0 +motor.cogging_dq_comp.861 0 +motor.cogging_dq_comp.862 0 +motor.cogging_dq_comp.863 0 +motor.cogging_dq_comp.864 0 +motor.cogging_dq_comp.865 0 +motor.cogging_dq_comp.866 0 +motor.cogging_dq_comp.867 0 +motor.cogging_dq_comp.868 0 +motor.cogging_dq_comp.869 0 +motor.cogging_dq_comp.870 0 +motor.cogging_dq_comp.871 0 +motor.cogging_dq_comp.872 0 +motor.cogging_dq_comp.873 0 +motor.cogging_dq_comp.874 0 +motor.cogging_dq_comp.875 0 +motor.cogging_dq_comp.876 0 +motor.cogging_dq_comp.877 0 +motor.cogging_dq_comp.878 0 +motor.cogging_dq_comp.879 0 +motor.cogging_dq_comp.880 0 +motor.cogging_dq_comp.881 0 +motor.cogging_dq_comp.882 0 +motor.cogging_dq_comp.883 0 +motor.cogging_dq_comp.884 0 +motor.cogging_dq_comp.885 0 +motor.cogging_dq_comp.886 0 +motor.cogging_dq_comp.887 0 +motor.cogging_dq_comp.888 0 +motor.cogging_dq_comp.889 0 +motor.cogging_dq_comp.890 0 +motor.cogging_dq_comp.891 0 +motor.cogging_dq_comp.892 0 +motor.cogging_dq_comp.893 0 +motor.cogging_dq_comp.894 0 +motor.cogging_dq_comp.895 0 +motor.cogging_dq_comp.896 0 +motor.cogging_dq_comp.897 0 +motor.cogging_dq_comp.898 0 +motor.cogging_dq_comp.899 0 +motor.cogging_dq_comp.900 0 +motor.cogging_dq_comp.901 0 +motor.cogging_dq_comp.902 0 +motor.cogging_dq_comp.903 0 +motor.cogging_dq_comp.904 0 +motor.cogging_dq_comp.905 0 +motor.cogging_dq_comp.906 0 +motor.cogging_dq_comp.907 0 +motor.cogging_dq_comp.908 0 +motor.cogging_dq_comp.909 0 +motor.cogging_dq_comp.910 0 +motor.cogging_dq_comp.911 0 +motor.cogging_dq_comp.912 0 +motor.cogging_dq_comp.913 0 +motor.cogging_dq_comp.914 0 +motor.cogging_dq_comp.915 0 +motor.cogging_dq_comp.916 0 +motor.cogging_dq_comp.917 0 +motor.cogging_dq_comp.918 0 +motor.cogging_dq_comp.919 0 +motor.cogging_dq_comp.920 0 +motor.cogging_dq_comp.921 0 +motor.cogging_dq_comp.922 0 +motor.cogging_dq_comp.923 0 +motor.cogging_dq_comp.924 0 +motor.cogging_dq_comp.925 0 +motor.cogging_dq_comp.926 0 +motor.cogging_dq_comp.927 0 +motor.cogging_dq_comp.928 0 +motor.cogging_dq_comp.929 0 +motor.cogging_dq_comp.930 0 +motor.cogging_dq_comp.931 0 +motor.cogging_dq_comp.932 0 +motor.cogging_dq_comp.933 0 +motor.cogging_dq_comp.934 0 +motor.cogging_dq_comp.935 0 +motor.cogging_dq_comp.936 0 +motor.cogging_dq_comp.937 0 +motor.cogging_dq_comp.938 0 +motor.cogging_dq_comp.939 0 +motor.cogging_dq_comp.940 0 +motor.cogging_dq_comp.941 0 +motor.cogging_dq_comp.942 0 +motor.cogging_dq_comp.943 0 +motor.cogging_dq_comp.944 0 +motor.cogging_dq_comp.945 0 +motor.cogging_dq_comp.946 0 +motor.cogging_dq_comp.947 0 +motor.cogging_dq_comp.948 0 +motor.cogging_dq_comp.949 0 +motor.cogging_dq_comp.950 0 +motor.cogging_dq_comp.951 0 +motor.cogging_dq_comp.952 0 +motor.cogging_dq_comp.953 0 +motor.cogging_dq_comp.954 0 +motor.cogging_dq_comp.955 0 +motor.cogging_dq_comp.956 0 +motor.cogging_dq_comp.957 0 +motor.cogging_dq_comp.958 0 +motor.cogging_dq_comp.959 0 +motor.cogging_dq_comp.960 0 +motor.cogging_dq_comp.961 0 +motor.cogging_dq_comp.962 0 +motor.cogging_dq_comp.963 0 +motor.cogging_dq_comp.964 0 +motor.cogging_dq_comp.965 0 +motor.cogging_dq_comp.966 0 +motor.cogging_dq_comp.967 0 +motor.cogging_dq_comp.968 0 +motor.cogging_dq_comp.969 0 +motor.cogging_dq_comp.970 0 +motor.cogging_dq_comp.971 0 +motor.cogging_dq_comp.972 0 +motor.cogging_dq_comp.973 0 +motor.cogging_dq_comp.974 0 +motor.cogging_dq_comp.975 0 +motor.cogging_dq_comp.976 0 +motor.cogging_dq_comp.977 0 +motor.cogging_dq_comp.978 0 +motor.cogging_dq_comp.979 0 +motor.cogging_dq_comp.980 0 +motor.cogging_dq_comp.981 0 +motor.cogging_dq_comp.982 0 +motor.cogging_dq_comp.983 0 +motor.cogging_dq_comp.984 0 +motor.cogging_dq_comp.985 0 +motor.cogging_dq_comp.986 0 +motor.cogging_dq_comp.987 0 +motor.cogging_dq_comp.988 0 +motor.cogging_dq_comp.989 0 +motor.cogging_dq_comp.990 0 +motor.cogging_dq_comp.991 0 +motor.cogging_dq_comp.992 0 +motor.cogging_dq_comp.993 0 +motor.cogging_dq_comp.994 0 +motor.cogging_dq_comp.995 0 +motor.cogging_dq_comp.996 0 +motor.cogging_dq_comp.997 0 +motor.cogging_dq_comp.998 0 +motor.cogging_dq_comp.999 0 +motor.cogging_dq_comp.1000 0 +motor.cogging_dq_comp.1001 0 +motor.cogging_dq_comp.1002 0 +motor.cogging_dq_comp.1003 0 +motor.cogging_dq_comp.1004 0 +motor.cogging_dq_comp.1005 0 +motor.cogging_dq_comp.1006 0 +motor.cogging_dq_comp.1007 0 +motor.cogging_dq_comp.1008 0 +motor.cogging_dq_comp.1009 0 +motor.cogging_dq_comp.1010 0 +motor.cogging_dq_comp.1011 0 +motor.cogging_dq_comp.1012 0 +motor.cogging_dq_comp.1013 0 +motor.cogging_dq_comp.1014 0 +motor.cogging_dq_comp.1015 0 +motor.cogging_dq_comp.1016 0 +motor.cogging_dq_comp.1017 0 +motor.cogging_dq_comp.1018 0 +motor.cogging_dq_comp.1019 0 +motor.cogging_dq_comp.1020 0 +motor.cogging_dq_comp.1021 0 +motor.cogging_dq_comp.1022 0 +motor.cogging_dq_comp.1023 0 +drv8323_conf.dis_cpuv 0 +drv8323_conf.dis_gdf 0 +drv8323_conf.otw_rep 0 +drv8323_conf.pwm_mode 1 +drv8323_conf.pwm_1x_asynchronous 0 +drv8323_conf.pwm_1x_dir 0 +drv8323_conf.idrivep_hs_ma 100 +drv8323_conf.idriven_hs_ma 200 +drv8323_conf.cbc 1 +drv8323_conf.tdrive_ns 1000 +drv8323_conf.idrivep_ls_ma 100 +drv8323_conf.idriven_ls_ma 200 +drv8323_conf.tretry 0 +drv8323_conf.dead_time_ns 50 +drv8323_conf.ocp_mode 0 +drv8323_conf.ocp_deg_us 4 +drv8323_conf.vds_lvl_mv 700 +drv8323_conf.csa_fet 0 +drv8323_conf.vref_div 1 +drv8323_conf.ls_ref 0 +drv8323_conf.csa_gain 20 +drv8323_conf.dis_sen 0 +drv8323_conf.sen_lvl_mv 500 +servo.pwm_rate_hz 30000 +servo.i_gain 20.000000 +servo.current_sense_ohm 0.000500 +servo.pwm_comp_off 0.027000 +servo.pwm_comp_mag 0.005000 +servo.pwm_scale 1.000000 +servo.max_voltage 46.000000 +servo.max_power_W 450.000000 +servo.derate_temperature 50.000000 +servo.fault_temperature 75.000000 +servo.enable_motor_temperature 0 +servo.motor_derate_temperature 50.000000 +servo.motor_fault_temperature nan +servo.velocity_threshold 0.000000 +servo.position_derate 0.020000 +servo.adc_cur_cycles 2 +servo.adc_aux_cycles 47 +servo.pid_dq.kp 0.208156 +servo.pid_dq.ki 136.432968 +servo.pid_position.kp 0.500000 +servo.pid_position.ki 0.000000 +servo.pid_position.kd 0.050000 +servo.pid_position.iratelimit -1.000000 +servo.pid_position.ilimit 0.000000 +servo.pid_position.max_desired_rate 0.000000 +servo.pid_position.sign -1 +servo.current_feedforward 1.000000 +servo.bemf_feedforward 1.000000 +servo.default_velocity_limit nan +servo.default_accel_limit nan +servo.voltage_mode_control 0 +servo.fixed_voltage_mode 0 +servo.fixed_voltage_control_V 0.000000 +servo.max_position_slip 10.000000 +servo.default_timeout_s 0.100000 +servo.timeout_max_torque_Nm 5.000000 +servo.timeout_mode 12 +servo.flux_brake_min_voltage 43.500000 +servo.flux_brake_resistance_ohm 0.025000 +servo.max_current_A 7.500000 +servo.derate_current_A -20.000000 +servo.max_velocity 500.000000 +servo.max_velocity_derate 2.000000 +servo.cooldown_cycles 256 +servo.velocity_zero_capture_threshold 0.050000 +servo.emit_debug 0 +servopos.position_min nan +servopos.position_max nan +id.id 19 +can.prefix 0 + diff --git a/config/moteus/sa_z.cfg b/config/moteus/sa_z.cfg new file mode 100644 index 000000000..b0654bd1c --- /dev/null +++ b/config/moteus/sa_z.cfg @@ -0,0 +1,1399 @@ +uuid.uuid.0 95 +uuid.uuid.1 37 +uuid.uuid.2 12 +uuid.uuid.3 27 +uuid.uuid.4 4 +uuid.uuid.5 16 +uuid.uuid.6 65 +uuid.uuid.7 193 +uuid.uuid.8 152 +uuid.uuid.9 64 +uuid.uuid.10 17 +uuid.uuid.11 161 +uuid.uuid.12 239 +uuid.uuid.13 203 +uuid.uuid.14 94 +uuid.uuid.15 81 +clock.hsitrim 64 +aux1.i2c.i2c_hz 400000 +aux1.i2c.i2c_mode 1 +aux1.i2c.pullup 0 +aux1.i2c.devices.0.type 0 +aux1.i2c.devices.0.address 64 +aux1.i2c.devices.0.poll_rate_us 1000 +aux1.i2c.devices.1.type 0 +aux1.i2c.devices.1.address 64 +aux1.i2c.devices.1.poll_rate_us 1000 +aux1.i2c.devices.2.type 0 +aux1.i2c.devices.2.address 64 +aux1.i2c.devices.2.poll_rate_us 1000 +aux1.spi.mode 1 +aux1.spi.rate_hz 12000000 +aux1.spi.filter_us 64 +aux1.spi.bct 0 +aux1.uart.mode 0 +aux1.uart.baud_rate 115200 +aux1.uart.poll_rate_us 100 +aux1.uart.rs422 0 +aux1.uart.cui_amt21_address 84 +aux1.quadrature.enabled 0 +aux1.quadrature.cpr 16384 +aux1.hall.enabled 1 +aux1.hall.polarity 0 +aux1.index.enabled 0 +aux1.sine_cosine.enabled 0 +aux1.sine_cosine.common 1700 +aux1.i2c_startup_delay_ms 30 +aux1.pins.0.mode 0 +aux1.pins.0.pull 0 +aux1.pins.1.mode 6 +aux1.pins.1.pull 1 +aux1.pins.2.mode 6 +aux1.pins.2.pull 1 +aux1.pins.3.mode 6 +aux1.pins.3.pull 1 +aux1.pins.4.mode 0 +aux1.pins.4.pull 0 +aux2.i2c.i2c_hz 400000 +aux2.i2c.i2c_mode 1 +aux2.i2c.pullup 0 +aux2.i2c.devices.0.type 0 +aux2.i2c.devices.0.address 64 +aux2.i2c.devices.0.poll_rate_us 1000 +aux2.i2c.devices.1.type 0 +aux2.i2c.devices.1.address 64 +aux2.i2c.devices.1.poll_rate_us 1000 +aux2.i2c.devices.2.type 0 +aux2.i2c.devices.2.address 64 +aux2.i2c.devices.2.poll_rate_us 1000 +aux2.spi.mode 1 +aux2.spi.rate_hz 12000000 +aux2.spi.filter_us 64 +aux2.spi.bct 0 +aux2.uart.mode 0 +aux2.uart.baud_rate 115200 +aux2.uart.poll_rate_us 100 +aux2.uart.rs422 0 +aux2.uart.cui_amt21_address 84 +aux2.quadrature.enabled 0 +aux2.quadrature.cpr 16384 +aux2.hall.enabled 0 +aux2.hall.polarity 0 +aux2.index.enabled 0 +aux2.sine_cosine.enabled 0 +aux2.sine_cosine.common 1700 +aux2.i2c_startup_delay_ms 30 +aux2.pins.0.mode 14 +aux2.pins.0.pull 1 +aux2.pins.1.mode 14 +aux2.pins.1.pull 1 +aux2.pins.2.mode 0 +aux2.pins.2.pull 0 +aux2.pins.3.mode 0 +aux2.pins.3.pull 0 +aux2.pins.4.mode 0 +aux2.pins.4.pull 0 +motor_position.sources.0.aux_number 1 +motor_position.sources.0.type 4 +motor_position.sources.0.i2c_device 0 +motor_position.sources.0.incremental_index -1 +motor_position.sources.0.cpr 48 +motor_position.sources.0.offset -3.000000 +motor_position.sources.0.sign 1 +motor_position.sources.0.debug_override -1 +motor_position.sources.0.reference 0 +motor_position.sources.0.pll_filter_hz 100.000000 +motor_position.sources.0.compensation_table.0 0.000000 +motor_position.sources.0.compensation_table.1 0.000000 +motor_position.sources.0.compensation_table.2 0.000000 +motor_position.sources.0.compensation_table.3 0.000000 +motor_position.sources.0.compensation_table.4 0.000000 +motor_position.sources.0.compensation_table.5 0.000000 +motor_position.sources.0.compensation_table.6 0.000000 +motor_position.sources.0.compensation_table.7 0.000000 +motor_position.sources.0.compensation_table.8 0.000000 +motor_position.sources.0.compensation_table.9 0.000000 +motor_position.sources.0.compensation_table.10 0.000000 +motor_position.sources.0.compensation_table.11 0.000000 +motor_position.sources.0.compensation_table.12 0.000000 +motor_position.sources.0.compensation_table.13 0.000000 +motor_position.sources.0.compensation_table.14 0.000000 +motor_position.sources.0.compensation_table.15 0.000000 +motor_position.sources.0.compensation_table.16 0.000000 +motor_position.sources.0.compensation_table.17 0.000000 +motor_position.sources.0.compensation_table.18 0.000000 +motor_position.sources.0.compensation_table.19 0.000000 +motor_position.sources.0.compensation_table.20 0.000000 +motor_position.sources.0.compensation_table.21 0.000000 +motor_position.sources.0.compensation_table.22 0.000000 +motor_position.sources.0.compensation_table.23 0.000000 +motor_position.sources.0.compensation_table.24 0.000000 +motor_position.sources.0.compensation_table.25 0.000000 +motor_position.sources.0.compensation_table.26 0.000000 +motor_position.sources.0.compensation_table.27 0.000000 +motor_position.sources.0.compensation_table.28 0.000000 +motor_position.sources.0.compensation_table.29 0.000000 +motor_position.sources.0.compensation_table.30 0.000000 +motor_position.sources.0.compensation_table.31 0.000000 +motor_position.sources.1.aux_number 1 +motor_position.sources.1.type 0 +motor_position.sources.1.i2c_device 0 +motor_position.sources.1.incremental_index -1 +motor_position.sources.1.cpr 16384 +motor_position.sources.1.offset 0.000000 +motor_position.sources.1.sign 1 +motor_position.sources.1.debug_override -1 +motor_position.sources.1.reference 0 +motor_position.sources.1.pll_filter_hz 400.000000 +motor_position.sources.1.compensation_table.0 0.000000 +motor_position.sources.1.compensation_table.1 0.000000 +motor_position.sources.1.compensation_table.2 0.000000 +motor_position.sources.1.compensation_table.3 0.000000 +motor_position.sources.1.compensation_table.4 0.000000 +motor_position.sources.1.compensation_table.5 0.000000 +motor_position.sources.1.compensation_table.6 0.000000 +motor_position.sources.1.compensation_table.7 0.000000 +motor_position.sources.1.compensation_table.8 0.000000 +motor_position.sources.1.compensation_table.9 0.000000 +motor_position.sources.1.compensation_table.10 0.000000 +motor_position.sources.1.compensation_table.11 0.000000 +motor_position.sources.1.compensation_table.12 0.000000 +motor_position.sources.1.compensation_table.13 0.000000 +motor_position.sources.1.compensation_table.14 0.000000 +motor_position.sources.1.compensation_table.15 0.000000 +motor_position.sources.1.compensation_table.16 0.000000 +motor_position.sources.1.compensation_table.17 0.000000 +motor_position.sources.1.compensation_table.18 0.000000 +motor_position.sources.1.compensation_table.19 0.000000 +motor_position.sources.1.compensation_table.20 0.000000 +motor_position.sources.1.compensation_table.21 0.000000 +motor_position.sources.1.compensation_table.22 0.000000 +motor_position.sources.1.compensation_table.23 0.000000 +motor_position.sources.1.compensation_table.24 0.000000 +motor_position.sources.1.compensation_table.25 0.000000 +motor_position.sources.1.compensation_table.26 0.000000 +motor_position.sources.1.compensation_table.27 0.000000 +motor_position.sources.1.compensation_table.28 0.000000 +motor_position.sources.1.compensation_table.29 0.000000 +motor_position.sources.1.compensation_table.30 0.000000 +motor_position.sources.1.compensation_table.31 0.000000 +motor_position.sources.2.aux_number 1 +motor_position.sources.2.type 0 +motor_position.sources.2.i2c_device 0 +motor_position.sources.2.incremental_index -1 +motor_position.sources.2.cpr 16384 +motor_position.sources.2.offset 0.000000 +motor_position.sources.2.sign 1 +motor_position.sources.2.debug_override -1 +motor_position.sources.2.reference 0 +motor_position.sources.2.pll_filter_hz 400.000000 +motor_position.sources.2.compensation_table.0 0.000000 +motor_position.sources.2.compensation_table.1 0.000000 +motor_position.sources.2.compensation_table.2 0.000000 +motor_position.sources.2.compensation_table.3 0.000000 +motor_position.sources.2.compensation_table.4 0.000000 +motor_position.sources.2.compensation_table.5 0.000000 +motor_position.sources.2.compensation_table.6 0.000000 +motor_position.sources.2.compensation_table.7 0.000000 +motor_position.sources.2.compensation_table.8 0.000000 +motor_position.sources.2.compensation_table.9 0.000000 +motor_position.sources.2.compensation_table.10 0.000000 +motor_position.sources.2.compensation_table.11 0.000000 +motor_position.sources.2.compensation_table.12 0.000000 +motor_position.sources.2.compensation_table.13 0.000000 +motor_position.sources.2.compensation_table.14 0.000000 +motor_position.sources.2.compensation_table.15 0.000000 +motor_position.sources.2.compensation_table.16 0.000000 +motor_position.sources.2.compensation_table.17 0.000000 +motor_position.sources.2.compensation_table.18 0.000000 +motor_position.sources.2.compensation_table.19 0.000000 +motor_position.sources.2.compensation_table.20 0.000000 +motor_position.sources.2.compensation_table.21 0.000000 +motor_position.sources.2.compensation_table.22 0.000000 +motor_position.sources.2.compensation_table.23 0.000000 +motor_position.sources.2.compensation_table.24 0.000000 +motor_position.sources.2.compensation_table.25 0.000000 +motor_position.sources.2.compensation_table.26 0.000000 +motor_position.sources.2.compensation_table.27 0.000000 +motor_position.sources.2.compensation_table.28 0.000000 +motor_position.sources.2.compensation_table.29 0.000000 +motor_position.sources.2.compensation_table.30 0.000000 +motor_position.sources.2.compensation_table.31 0.000000 +motor_position.commutation_source 0 +motor_position.output.source 0 +motor_position.output.offset 0.000000 +motor_position.output.sign 1 +motor_position.output.reference_source -1 +motor_position.rotor_to_output_ratio 0.001587 +motor.poles 16 +motor.phase_invert 0 +motor.resistance_ohm 4.000340 +motor.v_per_hz 0.575865 +motor.offset.0 0.000000 +motor.offset.1 0.000000 +motor.offset.2 0.000000 +motor.offset.3 0.000000 +motor.offset.4 0.000000 +motor.offset.5 0.000000 +motor.offset.6 0.000000 +motor.offset.7 0.000000 +motor.offset.8 0.000000 +motor.offset.9 0.000000 +motor.offset.10 0.000000 +motor.offset.11 0.000000 +motor.offset.12 0.000000 +motor.offset.13 0.000000 +motor.offset.14 0.000000 +motor.offset.15 0.000000 +motor.offset.16 0.000000 +motor.offset.17 0.000000 +motor.offset.18 0.000000 +motor.offset.19 0.000000 +motor.offset.20 0.000000 +motor.offset.21 0.000000 +motor.offset.22 0.000000 +motor.offset.23 0.000000 +motor.offset.24 0.000000 +motor.offset.25 0.000000 +motor.offset.26 0.000000 +motor.offset.27 0.000000 +motor.offset.28 0.000000 +motor.offset.29 0.000000 +motor.offset.30 0.000000 +motor.offset.31 0.000000 +motor.offset.32 0.000000 +motor.offset.33 0.000000 +motor.offset.34 0.000000 +motor.offset.35 0.000000 +motor.offset.36 0.000000 +motor.offset.37 0.000000 +motor.offset.38 0.000000 +motor.offset.39 0.000000 +motor.offset.40 0.000000 +motor.offset.41 0.000000 +motor.offset.42 0.000000 +motor.offset.43 0.000000 +motor.offset.44 0.000000 +motor.offset.45 0.000000 +motor.offset.46 0.000000 +motor.offset.47 0.000000 +motor.offset.48 0.000000 +motor.offset.49 0.000000 +motor.offset.50 0.000000 +motor.offset.51 0.000000 +motor.offset.52 0.000000 +motor.offset.53 0.000000 +motor.offset.54 0.000000 +motor.offset.55 0.000000 +motor.offset.56 0.000000 +motor.offset.57 0.000000 +motor.offset.58 0.000000 +motor.offset.59 0.000000 +motor.offset.60 0.000000 +motor.offset.61 0.000000 +motor.offset.62 0.000000 +motor.offset.63 0.000000 +motor.rotation_current_cutoff_A 10000.000000 +motor.rotation_current_scale 0.050000 +motor.rotation_torque_scale 14.700000 +motor.cogging_dq_scale 0.000000 +motor.cogging_dq_comp.0 0 +motor.cogging_dq_comp.1 0 +motor.cogging_dq_comp.2 0 +motor.cogging_dq_comp.3 0 +motor.cogging_dq_comp.4 0 +motor.cogging_dq_comp.5 0 +motor.cogging_dq_comp.6 0 +motor.cogging_dq_comp.7 0 +motor.cogging_dq_comp.8 0 +motor.cogging_dq_comp.9 0 +motor.cogging_dq_comp.10 0 +motor.cogging_dq_comp.11 0 +motor.cogging_dq_comp.12 0 +motor.cogging_dq_comp.13 0 +motor.cogging_dq_comp.14 0 +motor.cogging_dq_comp.15 0 +motor.cogging_dq_comp.16 0 +motor.cogging_dq_comp.17 0 +motor.cogging_dq_comp.18 0 +motor.cogging_dq_comp.19 0 +motor.cogging_dq_comp.20 0 +motor.cogging_dq_comp.21 0 +motor.cogging_dq_comp.22 0 +motor.cogging_dq_comp.23 0 +motor.cogging_dq_comp.24 0 +motor.cogging_dq_comp.25 0 +motor.cogging_dq_comp.26 0 +motor.cogging_dq_comp.27 0 +motor.cogging_dq_comp.28 0 +motor.cogging_dq_comp.29 0 +motor.cogging_dq_comp.30 0 +motor.cogging_dq_comp.31 0 +motor.cogging_dq_comp.32 0 +motor.cogging_dq_comp.33 0 +motor.cogging_dq_comp.34 0 +motor.cogging_dq_comp.35 0 +motor.cogging_dq_comp.36 0 +motor.cogging_dq_comp.37 0 +motor.cogging_dq_comp.38 0 +motor.cogging_dq_comp.39 0 +motor.cogging_dq_comp.40 0 +motor.cogging_dq_comp.41 0 +motor.cogging_dq_comp.42 0 +motor.cogging_dq_comp.43 0 +motor.cogging_dq_comp.44 0 +motor.cogging_dq_comp.45 0 +motor.cogging_dq_comp.46 0 +motor.cogging_dq_comp.47 0 +motor.cogging_dq_comp.48 0 +motor.cogging_dq_comp.49 0 +motor.cogging_dq_comp.50 0 +motor.cogging_dq_comp.51 0 +motor.cogging_dq_comp.52 0 +motor.cogging_dq_comp.53 0 +motor.cogging_dq_comp.54 0 +motor.cogging_dq_comp.55 0 +motor.cogging_dq_comp.56 0 +motor.cogging_dq_comp.57 0 +motor.cogging_dq_comp.58 0 +motor.cogging_dq_comp.59 0 +motor.cogging_dq_comp.60 0 +motor.cogging_dq_comp.61 0 +motor.cogging_dq_comp.62 0 +motor.cogging_dq_comp.63 0 +motor.cogging_dq_comp.64 0 +motor.cogging_dq_comp.65 0 +motor.cogging_dq_comp.66 0 +motor.cogging_dq_comp.67 0 +motor.cogging_dq_comp.68 0 +motor.cogging_dq_comp.69 0 +motor.cogging_dq_comp.70 0 +motor.cogging_dq_comp.71 0 +motor.cogging_dq_comp.72 0 +motor.cogging_dq_comp.73 0 +motor.cogging_dq_comp.74 0 +motor.cogging_dq_comp.75 0 +motor.cogging_dq_comp.76 0 +motor.cogging_dq_comp.77 0 +motor.cogging_dq_comp.78 0 +motor.cogging_dq_comp.79 0 +motor.cogging_dq_comp.80 0 +motor.cogging_dq_comp.81 0 +motor.cogging_dq_comp.82 0 +motor.cogging_dq_comp.83 0 +motor.cogging_dq_comp.84 0 +motor.cogging_dq_comp.85 0 +motor.cogging_dq_comp.86 0 +motor.cogging_dq_comp.87 0 +motor.cogging_dq_comp.88 0 +motor.cogging_dq_comp.89 0 +motor.cogging_dq_comp.90 0 +motor.cogging_dq_comp.91 0 +motor.cogging_dq_comp.92 0 +motor.cogging_dq_comp.93 0 +motor.cogging_dq_comp.94 0 +motor.cogging_dq_comp.95 0 +motor.cogging_dq_comp.96 0 +motor.cogging_dq_comp.97 0 +motor.cogging_dq_comp.98 0 +motor.cogging_dq_comp.99 0 +motor.cogging_dq_comp.100 0 +motor.cogging_dq_comp.101 0 +motor.cogging_dq_comp.102 0 +motor.cogging_dq_comp.103 0 +motor.cogging_dq_comp.104 0 +motor.cogging_dq_comp.105 0 +motor.cogging_dq_comp.106 0 +motor.cogging_dq_comp.107 0 +motor.cogging_dq_comp.108 0 +motor.cogging_dq_comp.109 0 +motor.cogging_dq_comp.110 0 +motor.cogging_dq_comp.111 0 +motor.cogging_dq_comp.112 0 +motor.cogging_dq_comp.113 0 +motor.cogging_dq_comp.114 0 +motor.cogging_dq_comp.115 0 +motor.cogging_dq_comp.116 0 +motor.cogging_dq_comp.117 0 +motor.cogging_dq_comp.118 0 +motor.cogging_dq_comp.119 0 +motor.cogging_dq_comp.120 0 +motor.cogging_dq_comp.121 0 +motor.cogging_dq_comp.122 0 +motor.cogging_dq_comp.123 0 +motor.cogging_dq_comp.124 0 +motor.cogging_dq_comp.125 0 +motor.cogging_dq_comp.126 0 +motor.cogging_dq_comp.127 0 +motor.cogging_dq_comp.128 0 +motor.cogging_dq_comp.129 0 +motor.cogging_dq_comp.130 0 +motor.cogging_dq_comp.131 0 +motor.cogging_dq_comp.132 0 +motor.cogging_dq_comp.133 0 +motor.cogging_dq_comp.134 0 +motor.cogging_dq_comp.135 0 +motor.cogging_dq_comp.136 0 +motor.cogging_dq_comp.137 0 +motor.cogging_dq_comp.138 0 +motor.cogging_dq_comp.139 0 +motor.cogging_dq_comp.140 0 +motor.cogging_dq_comp.141 0 +motor.cogging_dq_comp.142 0 +motor.cogging_dq_comp.143 0 +motor.cogging_dq_comp.144 0 +motor.cogging_dq_comp.145 0 +motor.cogging_dq_comp.146 0 +motor.cogging_dq_comp.147 0 +motor.cogging_dq_comp.148 0 +motor.cogging_dq_comp.149 0 +motor.cogging_dq_comp.150 0 +motor.cogging_dq_comp.151 0 +motor.cogging_dq_comp.152 0 +motor.cogging_dq_comp.153 0 +motor.cogging_dq_comp.154 0 +motor.cogging_dq_comp.155 0 +motor.cogging_dq_comp.156 0 +motor.cogging_dq_comp.157 0 +motor.cogging_dq_comp.158 0 +motor.cogging_dq_comp.159 0 +motor.cogging_dq_comp.160 0 +motor.cogging_dq_comp.161 0 +motor.cogging_dq_comp.162 0 +motor.cogging_dq_comp.163 0 +motor.cogging_dq_comp.164 0 +motor.cogging_dq_comp.165 0 +motor.cogging_dq_comp.166 0 +motor.cogging_dq_comp.167 0 +motor.cogging_dq_comp.168 0 +motor.cogging_dq_comp.169 0 +motor.cogging_dq_comp.170 0 +motor.cogging_dq_comp.171 0 +motor.cogging_dq_comp.172 0 +motor.cogging_dq_comp.173 0 +motor.cogging_dq_comp.174 0 +motor.cogging_dq_comp.175 0 +motor.cogging_dq_comp.176 0 +motor.cogging_dq_comp.177 0 +motor.cogging_dq_comp.178 0 +motor.cogging_dq_comp.179 0 +motor.cogging_dq_comp.180 0 +motor.cogging_dq_comp.181 0 +motor.cogging_dq_comp.182 0 +motor.cogging_dq_comp.183 0 +motor.cogging_dq_comp.184 0 +motor.cogging_dq_comp.185 0 +motor.cogging_dq_comp.186 0 +motor.cogging_dq_comp.187 0 +motor.cogging_dq_comp.188 0 +motor.cogging_dq_comp.189 0 +motor.cogging_dq_comp.190 0 +motor.cogging_dq_comp.191 0 +motor.cogging_dq_comp.192 0 +motor.cogging_dq_comp.193 0 +motor.cogging_dq_comp.194 0 +motor.cogging_dq_comp.195 0 +motor.cogging_dq_comp.196 0 +motor.cogging_dq_comp.197 0 +motor.cogging_dq_comp.198 0 +motor.cogging_dq_comp.199 0 +motor.cogging_dq_comp.200 0 +motor.cogging_dq_comp.201 0 +motor.cogging_dq_comp.202 0 +motor.cogging_dq_comp.203 0 +motor.cogging_dq_comp.204 0 +motor.cogging_dq_comp.205 0 +motor.cogging_dq_comp.206 0 +motor.cogging_dq_comp.207 0 +motor.cogging_dq_comp.208 0 +motor.cogging_dq_comp.209 0 +motor.cogging_dq_comp.210 0 +motor.cogging_dq_comp.211 0 +motor.cogging_dq_comp.212 0 +motor.cogging_dq_comp.213 0 +motor.cogging_dq_comp.214 0 +motor.cogging_dq_comp.215 0 +motor.cogging_dq_comp.216 0 +motor.cogging_dq_comp.217 0 +motor.cogging_dq_comp.218 0 +motor.cogging_dq_comp.219 0 +motor.cogging_dq_comp.220 0 +motor.cogging_dq_comp.221 0 +motor.cogging_dq_comp.222 0 +motor.cogging_dq_comp.223 0 +motor.cogging_dq_comp.224 0 +motor.cogging_dq_comp.225 0 +motor.cogging_dq_comp.226 0 +motor.cogging_dq_comp.227 0 +motor.cogging_dq_comp.228 0 +motor.cogging_dq_comp.229 0 +motor.cogging_dq_comp.230 0 +motor.cogging_dq_comp.231 0 +motor.cogging_dq_comp.232 0 +motor.cogging_dq_comp.233 0 +motor.cogging_dq_comp.234 0 +motor.cogging_dq_comp.235 0 +motor.cogging_dq_comp.236 0 +motor.cogging_dq_comp.237 0 +motor.cogging_dq_comp.238 0 +motor.cogging_dq_comp.239 0 +motor.cogging_dq_comp.240 0 +motor.cogging_dq_comp.241 0 +motor.cogging_dq_comp.242 0 +motor.cogging_dq_comp.243 0 +motor.cogging_dq_comp.244 0 +motor.cogging_dq_comp.245 0 +motor.cogging_dq_comp.246 0 +motor.cogging_dq_comp.247 0 +motor.cogging_dq_comp.248 0 +motor.cogging_dq_comp.249 0 +motor.cogging_dq_comp.250 0 +motor.cogging_dq_comp.251 0 +motor.cogging_dq_comp.252 0 +motor.cogging_dq_comp.253 0 +motor.cogging_dq_comp.254 0 +motor.cogging_dq_comp.255 0 +motor.cogging_dq_comp.256 0 +motor.cogging_dq_comp.257 0 +motor.cogging_dq_comp.258 0 +motor.cogging_dq_comp.259 0 +motor.cogging_dq_comp.260 0 +motor.cogging_dq_comp.261 0 +motor.cogging_dq_comp.262 0 +motor.cogging_dq_comp.263 0 +motor.cogging_dq_comp.264 0 +motor.cogging_dq_comp.265 0 +motor.cogging_dq_comp.266 0 +motor.cogging_dq_comp.267 0 +motor.cogging_dq_comp.268 0 +motor.cogging_dq_comp.269 0 +motor.cogging_dq_comp.270 0 +motor.cogging_dq_comp.271 0 +motor.cogging_dq_comp.272 0 +motor.cogging_dq_comp.273 0 +motor.cogging_dq_comp.274 0 +motor.cogging_dq_comp.275 0 +motor.cogging_dq_comp.276 0 +motor.cogging_dq_comp.277 0 +motor.cogging_dq_comp.278 0 +motor.cogging_dq_comp.279 0 +motor.cogging_dq_comp.280 0 +motor.cogging_dq_comp.281 0 +motor.cogging_dq_comp.282 0 +motor.cogging_dq_comp.283 0 +motor.cogging_dq_comp.284 0 +motor.cogging_dq_comp.285 0 +motor.cogging_dq_comp.286 0 +motor.cogging_dq_comp.287 0 +motor.cogging_dq_comp.288 0 +motor.cogging_dq_comp.289 0 +motor.cogging_dq_comp.290 0 +motor.cogging_dq_comp.291 0 +motor.cogging_dq_comp.292 0 +motor.cogging_dq_comp.293 0 +motor.cogging_dq_comp.294 0 +motor.cogging_dq_comp.295 0 +motor.cogging_dq_comp.296 0 +motor.cogging_dq_comp.297 0 +motor.cogging_dq_comp.298 0 +motor.cogging_dq_comp.299 0 +motor.cogging_dq_comp.300 0 +motor.cogging_dq_comp.301 0 +motor.cogging_dq_comp.302 0 +motor.cogging_dq_comp.303 0 +motor.cogging_dq_comp.304 0 +motor.cogging_dq_comp.305 0 +motor.cogging_dq_comp.306 0 +motor.cogging_dq_comp.307 0 +motor.cogging_dq_comp.308 0 +motor.cogging_dq_comp.309 0 +motor.cogging_dq_comp.310 0 +motor.cogging_dq_comp.311 0 +motor.cogging_dq_comp.312 0 +motor.cogging_dq_comp.313 0 +motor.cogging_dq_comp.314 0 +motor.cogging_dq_comp.315 0 +motor.cogging_dq_comp.316 0 +motor.cogging_dq_comp.317 0 +motor.cogging_dq_comp.318 0 +motor.cogging_dq_comp.319 0 +motor.cogging_dq_comp.320 0 +motor.cogging_dq_comp.321 0 +motor.cogging_dq_comp.322 0 +motor.cogging_dq_comp.323 0 +motor.cogging_dq_comp.324 0 +motor.cogging_dq_comp.325 0 +motor.cogging_dq_comp.326 0 +motor.cogging_dq_comp.327 0 +motor.cogging_dq_comp.328 0 +motor.cogging_dq_comp.329 0 +motor.cogging_dq_comp.330 0 +motor.cogging_dq_comp.331 0 +motor.cogging_dq_comp.332 0 +motor.cogging_dq_comp.333 0 +motor.cogging_dq_comp.334 0 +motor.cogging_dq_comp.335 0 +motor.cogging_dq_comp.336 0 +motor.cogging_dq_comp.337 0 +motor.cogging_dq_comp.338 0 +motor.cogging_dq_comp.339 0 +motor.cogging_dq_comp.340 0 +motor.cogging_dq_comp.341 0 +motor.cogging_dq_comp.342 0 +motor.cogging_dq_comp.343 0 +motor.cogging_dq_comp.344 0 +motor.cogging_dq_comp.345 0 +motor.cogging_dq_comp.346 0 +motor.cogging_dq_comp.347 0 +motor.cogging_dq_comp.348 0 +motor.cogging_dq_comp.349 0 +motor.cogging_dq_comp.350 0 +motor.cogging_dq_comp.351 0 +motor.cogging_dq_comp.352 0 +motor.cogging_dq_comp.353 0 +motor.cogging_dq_comp.354 0 +motor.cogging_dq_comp.355 0 +motor.cogging_dq_comp.356 0 +motor.cogging_dq_comp.357 0 +motor.cogging_dq_comp.358 0 +motor.cogging_dq_comp.359 0 +motor.cogging_dq_comp.360 0 +motor.cogging_dq_comp.361 0 +motor.cogging_dq_comp.362 0 +motor.cogging_dq_comp.363 0 +motor.cogging_dq_comp.364 0 +motor.cogging_dq_comp.365 0 +motor.cogging_dq_comp.366 0 +motor.cogging_dq_comp.367 0 +motor.cogging_dq_comp.368 0 +motor.cogging_dq_comp.369 0 +motor.cogging_dq_comp.370 0 +motor.cogging_dq_comp.371 0 +motor.cogging_dq_comp.372 0 +motor.cogging_dq_comp.373 0 +motor.cogging_dq_comp.374 0 +motor.cogging_dq_comp.375 0 +motor.cogging_dq_comp.376 0 +motor.cogging_dq_comp.377 0 +motor.cogging_dq_comp.378 0 +motor.cogging_dq_comp.379 0 +motor.cogging_dq_comp.380 0 +motor.cogging_dq_comp.381 0 +motor.cogging_dq_comp.382 0 +motor.cogging_dq_comp.383 0 +motor.cogging_dq_comp.384 0 +motor.cogging_dq_comp.385 0 +motor.cogging_dq_comp.386 0 +motor.cogging_dq_comp.387 0 +motor.cogging_dq_comp.388 0 +motor.cogging_dq_comp.389 0 +motor.cogging_dq_comp.390 0 +motor.cogging_dq_comp.391 0 +motor.cogging_dq_comp.392 0 +motor.cogging_dq_comp.393 0 +motor.cogging_dq_comp.394 0 +motor.cogging_dq_comp.395 0 +motor.cogging_dq_comp.396 0 +motor.cogging_dq_comp.397 0 +motor.cogging_dq_comp.398 0 +motor.cogging_dq_comp.399 0 +motor.cogging_dq_comp.400 0 +motor.cogging_dq_comp.401 0 +motor.cogging_dq_comp.402 0 +motor.cogging_dq_comp.403 0 +motor.cogging_dq_comp.404 0 +motor.cogging_dq_comp.405 0 +motor.cogging_dq_comp.406 0 +motor.cogging_dq_comp.407 0 +motor.cogging_dq_comp.408 0 +motor.cogging_dq_comp.409 0 +motor.cogging_dq_comp.410 0 +motor.cogging_dq_comp.411 0 +motor.cogging_dq_comp.412 0 +motor.cogging_dq_comp.413 0 +motor.cogging_dq_comp.414 0 +motor.cogging_dq_comp.415 0 +motor.cogging_dq_comp.416 0 +motor.cogging_dq_comp.417 0 +motor.cogging_dq_comp.418 0 +motor.cogging_dq_comp.419 0 +motor.cogging_dq_comp.420 0 +motor.cogging_dq_comp.421 0 +motor.cogging_dq_comp.422 0 +motor.cogging_dq_comp.423 0 +motor.cogging_dq_comp.424 0 +motor.cogging_dq_comp.425 0 +motor.cogging_dq_comp.426 0 +motor.cogging_dq_comp.427 0 +motor.cogging_dq_comp.428 0 +motor.cogging_dq_comp.429 0 +motor.cogging_dq_comp.430 0 +motor.cogging_dq_comp.431 0 +motor.cogging_dq_comp.432 0 +motor.cogging_dq_comp.433 0 +motor.cogging_dq_comp.434 0 +motor.cogging_dq_comp.435 0 +motor.cogging_dq_comp.436 0 +motor.cogging_dq_comp.437 0 +motor.cogging_dq_comp.438 0 +motor.cogging_dq_comp.439 0 +motor.cogging_dq_comp.440 0 +motor.cogging_dq_comp.441 0 +motor.cogging_dq_comp.442 0 +motor.cogging_dq_comp.443 0 +motor.cogging_dq_comp.444 0 +motor.cogging_dq_comp.445 0 +motor.cogging_dq_comp.446 0 +motor.cogging_dq_comp.447 0 +motor.cogging_dq_comp.448 0 +motor.cogging_dq_comp.449 0 +motor.cogging_dq_comp.450 0 +motor.cogging_dq_comp.451 0 +motor.cogging_dq_comp.452 0 +motor.cogging_dq_comp.453 0 +motor.cogging_dq_comp.454 0 +motor.cogging_dq_comp.455 0 +motor.cogging_dq_comp.456 0 +motor.cogging_dq_comp.457 0 +motor.cogging_dq_comp.458 0 +motor.cogging_dq_comp.459 0 +motor.cogging_dq_comp.460 0 +motor.cogging_dq_comp.461 0 +motor.cogging_dq_comp.462 0 +motor.cogging_dq_comp.463 0 +motor.cogging_dq_comp.464 0 +motor.cogging_dq_comp.465 0 +motor.cogging_dq_comp.466 0 +motor.cogging_dq_comp.467 0 +motor.cogging_dq_comp.468 0 +motor.cogging_dq_comp.469 0 +motor.cogging_dq_comp.470 0 +motor.cogging_dq_comp.471 0 +motor.cogging_dq_comp.472 0 +motor.cogging_dq_comp.473 0 +motor.cogging_dq_comp.474 0 +motor.cogging_dq_comp.475 0 +motor.cogging_dq_comp.476 0 +motor.cogging_dq_comp.477 0 +motor.cogging_dq_comp.478 0 +motor.cogging_dq_comp.479 0 +motor.cogging_dq_comp.480 0 +motor.cogging_dq_comp.481 0 +motor.cogging_dq_comp.482 0 +motor.cogging_dq_comp.483 0 +motor.cogging_dq_comp.484 0 +motor.cogging_dq_comp.485 0 +motor.cogging_dq_comp.486 0 +motor.cogging_dq_comp.487 0 +motor.cogging_dq_comp.488 0 +motor.cogging_dq_comp.489 0 +motor.cogging_dq_comp.490 0 +motor.cogging_dq_comp.491 0 +motor.cogging_dq_comp.492 0 +motor.cogging_dq_comp.493 0 +motor.cogging_dq_comp.494 0 +motor.cogging_dq_comp.495 0 +motor.cogging_dq_comp.496 0 +motor.cogging_dq_comp.497 0 +motor.cogging_dq_comp.498 0 +motor.cogging_dq_comp.499 0 +motor.cogging_dq_comp.500 0 +motor.cogging_dq_comp.501 0 +motor.cogging_dq_comp.502 0 +motor.cogging_dq_comp.503 0 +motor.cogging_dq_comp.504 0 +motor.cogging_dq_comp.505 0 +motor.cogging_dq_comp.506 0 +motor.cogging_dq_comp.507 0 +motor.cogging_dq_comp.508 0 +motor.cogging_dq_comp.509 0 +motor.cogging_dq_comp.510 0 +motor.cogging_dq_comp.511 0 +motor.cogging_dq_comp.512 0 +motor.cogging_dq_comp.513 0 +motor.cogging_dq_comp.514 0 +motor.cogging_dq_comp.515 0 +motor.cogging_dq_comp.516 0 +motor.cogging_dq_comp.517 0 +motor.cogging_dq_comp.518 0 +motor.cogging_dq_comp.519 0 +motor.cogging_dq_comp.520 0 +motor.cogging_dq_comp.521 0 +motor.cogging_dq_comp.522 0 +motor.cogging_dq_comp.523 0 +motor.cogging_dq_comp.524 0 +motor.cogging_dq_comp.525 0 +motor.cogging_dq_comp.526 0 +motor.cogging_dq_comp.527 0 +motor.cogging_dq_comp.528 0 +motor.cogging_dq_comp.529 0 +motor.cogging_dq_comp.530 0 +motor.cogging_dq_comp.531 0 +motor.cogging_dq_comp.532 0 +motor.cogging_dq_comp.533 0 +motor.cogging_dq_comp.534 0 +motor.cogging_dq_comp.535 0 +motor.cogging_dq_comp.536 0 +motor.cogging_dq_comp.537 0 +motor.cogging_dq_comp.538 0 +motor.cogging_dq_comp.539 0 +motor.cogging_dq_comp.540 0 +motor.cogging_dq_comp.541 0 +motor.cogging_dq_comp.542 0 +motor.cogging_dq_comp.543 0 +motor.cogging_dq_comp.544 0 +motor.cogging_dq_comp.545 0 +motor.cogging_dq_comp.546 0 +motor.cogging_dq_comp.547 0 +motor.cogging_dq_comp.548 0 +motor.cogging_dq_comp.549 0 +motor.cogging_dq_comp.550 0 +motor.cogging_dq_comp.551 0 +motor.cogging_dq_comp.552 0 +motor.cogging_dq_comp.553 0 +motor.cogging_dq_comp.554 0 +motor.cogging_dq_comp.555 0 +motor.cogging_dq_comp.556 0 +motor.cogging_dq_comp.557 0 +motor.cogging_dq_comp.558 0 +motor.cogging_dq_comp.559 0 +motor.cogging_dq_comp.560 0 +motor.cogging_dq_comp.561 0 +motor.cogging_dq_comp.562 0 +motor.cogging_dq_comp.563 0 +motor.cogging_dq_comp.564 0 +motor.cogging_dq_comp.565 0 +motor.cogging_dq_comp.566 0 +motor.cogging_dq_comp.567 0 +motor.cogging_dq_comp.568 0 +motor.cogging_dq_comp.569 0 +motor.cogging_dq_comp.570 0 +motor.cogging_dq_comp.571 0 +motor.cogging_dq_comp.572 0 +motor.cogging_dq_comp.573 0 +motor.cogging_dq_comp.574 0 +motor.cogging_dq_comp.575 0 +motor.cogging_dq_comp.576 0 +motor.cogging_dq_comp.577 0 +motor.cogging_dq_comp.578 0 +motor.cogging_dq_comp.579 0 +motor.cogging_dq_comp.580 0 +motor.cogging_dq_comp.581 0 +motor.cogging_dq_comp.582 0 +motor.cogging_dq_comp.583 0 +motor.cogging_dq_comp.584 0 +motor.cogging_dq_comp.585 0 +motor.cogging_dq_comp.586 0 +motor.cogging_dq_comp.587 0 +motor.cogging_dq_comp.588 0 +motor.cogging_dq_comp.589 0 +motor.cogging_dq_comp.590 0 +motor.cogging_dq_comp.591 0 +motor.cogging_dq_comp.592 0 +motor.cogging_dq_comp.593 0 +motor.cogging_dq_comp.594 0 +motor.cogging_dq_comp.595 0 +motor.cogging_dq_comp.596 0 +motor.cogging_dq_comp.597 0 +motor.cogging_dq_comp.598 0 +motor.cogging_dq_comp.599 0 +motor.cogging_dq_comp.600 0 +motor.cogging_dq_comp.601 0 +motor.cogging_dq_comp.602 0 +motor.cogging_dq_comp.603 0 +motor.cogging_dq_comp.604 0 +motor.cogging_dq_comp.605 0 +motor.cogging_dq_comp.606 0 +motor.cogging_dq_comp.607 0 +motor.cogging_dq_comp.608 0 +motor.cogging_dq_comp.609 0 +motor.cogging_dq_comp.610 0 +motor.cogging_dq_comp.611 0 +motor.cogging_dq_comp.612 0 +motor.cogging_dq_comp.613 0 +motor.cogging_dq_comp.614 0 +motor.cogging_dq_comp.615 0 +motor.cogging_dq_comp.616 0 +motor.cogging_dq_comp.617 0 +motor.cogging_dq_comp.618 0 +motor.cogging_dq_comp.619 0 +motor.cogging_dq_comp.620 0 +motor.cogging_dq_comp.621 0 +motor.cogging_dq_comp.622 0 +motor.cogging_dq_comp.623 0 +motor.cogging_dq_comp.624 0 +motor.cogging_dq_comp.625 0 +motor.cogging_dq_comp.626 0 +motor.cogging_dq_comp.627 0 +motor.cogging_dq_comp.628 0 +motor.cogging_dq_comp.629 0 +motor.cogging_dq_comp.630 0 +motor.cogging_dq_comp.631 0 +motor.cogging_dq_comp.632 0 +motor.cogging_dq_comp.633 0 +motor.cogging_dq_comp.634 0 +motor.cogging_dq_comp.635 0 +motor.cogging_dq_comp.636 0 +motor.cogging_dq_comp.637 0 +motor.cogging_dq_comp.638 0 +motor.cogging_dq_comp.639 0 +motor.cogging_dq_comp.640 0 +motor.cogging_dq_comp.641 0 +motor.cogging_dq_comp.642 0 +motor.cogging_dq_comp.643 0 +motor.cogging_dq_comp.644 0 +motor.cogging_dq_comp.645 0 +motor.cogging_dq_comp.646 0 +motor.cogging_dq_comp.647 0 +motor.cogging_dq_comp.648 0 +motor.cogging_dq_comp.649 0 +motor.cogging_dq_comp.650 0 +motor.cogging_dq_comp.651 0 +motor.cogging_dq_comp.652 0 +motor.cogging_dq_comp.653 0 +motor.cogging_dq_comp.654 0 +motor.cogging_dq_comp.655 0 +motor.cogging_dq_comp.656 0 +motor.cogging_dq_comp.657 0 +motor.cogging_dq_comp.658 0 +motor.cogging_dq_comp.659 0 +motor.cogging_dq_comp.660 0 +motor.cogging_dq_comp.661 0 +motor.cogging_dq_comp.662 0 +motor.cogging_dq_comp.663 0 +motor.cogging_dq_comp.664 0 +motor.cogging_dq_comp.665 0 +motor.cogging_dq_comp.666 0 +motor.cogging_dq_comp.667 0 +motor.cogging_dq_comp.668 0 +motor.cogging_dq_comp.669 0 +motor.cogging_dq_comp.670 0 +motor.cogging_dq_comp.671 0 +motor.cogging_dq_comp.672 0 +motor.cogging_dq_comp.673 0 +motor.cogging_dq_comp.674 0 +motor.cogging_dq_comp.675 0 +motor.cogging_dq_comp.676 0 +motor.cogging_dq_comp.677 0 +motor.cogging_dq_comp.678 0 +motor.cogging_dq_comp.679 0 +motor.cogging_dq_comp.680 0 +motor.cogging_dq_comp.681 0 +motor.cogging_dq_comp.682 0 +motor.cogging_dq_comp.683 0 +motor.cogging_dq_comp.684 0 +motor.cogging_dq_comp.685 0 +motor.cogging_dq_comp.686 0 +motor.cogging_dq_comp.687 0 +motor.cogging_dq_comp.688 0 +motor.cogging_dq_comp.689 0 +motor.cogging_dq_comp.690 0 +motor.cogging_dq_comp.691 0 +motor.cogging_dq_comp.692 0 +motor.cogging_dq_comp.693 0 +motor.cogging_dq_comp.694 0 +motor.cogging_dq_comp.695 0 +motor.cogging_dq_comp.696 0 +motor.cogging_dq_comp.697 0 +motor.cogging_dq_comp.698 0 +motor.cogging_dq_comp.699 0 +motor.cogging_dq_comp.700 0 +motor.cogging_dq_comp.701 0 +motor.cogging_dq_comp.702 0 +motor.cogging_dq_comp.703 0 +motor.cogging_dq_comp.704 0 +motor.cogging_dq_comp.705 0 +motor.cogging_dq_comp.706 0 +motor.cogging_dq_comp.707 0 +motor.cogging_dq_comp.708 0 +motor.cogging_dq_comp.709 0 +motor.cogging_dq_comp.710 0 +motor.cogging_dq_comp.711 0 +motor.cogging_dq_comp.712 0 +motor.cogging_dq_comp.713 0 +motor.cogging_dq_comp.714 0 +motor.cogging_dq_comp.715 0 +motor.cogging_dq_comp.716 0 +motor.cogging_dq_comp.717 0 +motor.cogging_dq_comp.718 0 +motor.cogging_dq_comp.719 0 +motor.cogging_dq_comp.720 0 +motor.cogging_dq_comp.721 0 +motor.cogging_dq_comp.722 0 +motor.cogging_dq_comp.723 0 +motor.cogging_dq_comp.724 0 +motor.cogging_dq_comp.725 0 +motor.cogging_dq_comp.726 0 +motor.cogging_dq_comp.727 0 +motor.cogging_dq_comp.728 0 +motor.cogging_dq_comp.729 0 +motor.cogging_dq_comp.730 0 +motor.cogging_dq_comp.731 0 +motor.cogging_dq_comp.732 0 +motor.cogging_dq_comp.733 0 +motor.cogging_dq_comp.734 0 +motor.cogging_dq_comp.735 0 +motor.cogging_dq_comp.736 0 +motor.cogging_dq_comp.737 0 +motor.cogging_dq_comp.738 0 +motor.cogging_dq_comp.739 0 +motor.cogging_dq_comp.740 0 +motor.cogging_dq_comp.741 0 +motor.cogging_dq_comp.742 0 +motor.cogging_dq_comp.743 0 +motor.cogging_dq_comp.744 0 +motor.cogging_dq_comp.745 0 +motor.cogging_dq_comp.746 0 +motor.cogging_dq_comp.747 0 +motor.cogging_dq_comp.748 0 +motor.cogging_dq_comp.749 0 +motor.cogging_dq_comp.750 0 +motor.cogging_dq_comp.751 0 +motor.cogging_dq_comp.752 0 +motor.cogging_dq_comp.753 0 +motor.cogging_dq_comp.754 0 +motor.cogging_dq_comp.755 0 +motor.cogging_dq_comp.756 0 +motor.cogging_dq_comp.757 0 +motor.cogging_dq_comp.758 0 +motor.cogging_dq_comp.759 0 +motor.cogging_dq_comp.760 0 +motor.cogging_dq_comp.761 0 +motor.cogging_dq_comp.762 0 +motor.cogging_dq_comp.763 0 +motor.cogging_dq_comp.764 0 +motor.cogging_dq_comp.765 0 +motor.cogging_dq_comp.766 0 +motor.cogging_dq_comp.767 0 +motor.cogging_dq_comp.768 0 +motor.cogging_dq_comp.769 0 +motor.cogging_dq_comp.770 0 +motor.cogging_dq_comp.771 0 +motor.cogging_dq_comp.772 0 +motor.cogging_dq_comp.773 0 +motor.cogging_dq_comp.774 0 +motor.cogging_dq_comp.775 0 +motor.cogging_dq_comp.776 0 +motor.cogging_dq_comp.777 0 +motor.cogging_dq_comp.778 0 +motor.cogging_dq_comp.779 0 +motor.cogging_dq_comp.780 0 +motor.cogging_dq_comp.781 0 +motor.cogging_dq_comp.782 0 +motor.cogging_dq_comp.783 0 +motor.cogging_dq_comp.784 0 +motor.cogging_dq_comp.785 0 +motor.cogging_dq_comp.786 0 +motor.cogging_dq_comp.787 0 +motor.cogging_dq_comp.788 0 +motor.cogging_dq_comp.789 0 +motor.cogging_dq_comp.790 0 +motor.cogging_dq_comp.791 0 +motor.cogging_dq_comp.792 0 +motor.cogging_dq_comp.793 0 +motor.cogging_dq_comp.794 0 +motor.cogging_dq_comp.795 0 +motor.cogging_dq_comp.796 0 +motor.cogging_dq_comp.797 0 +motor.cogging_dq_comp.798 0 +motor.cogging_dq_comp.799 0 +motor.cogging_dq_comp.800 0 +motor.cogging_dq_comp.801 0 +motor.cogging_dq_comp.802 0 +motor.cogging_dq_comp.803 0 +motor.cogging_dq_comp.804 0 +motor.cogging_dq_comp.805 0 +motor.cogging_dq_comp.806 0 +motor.cogging_dq_comp.807 0 +motor.cogging_dq_comp.808 0 +motor.cogging_dq_comp.809 0 +motor.cogging_dq_comp.810 0 +motor.cogging_dq_comp.811 0 +motor.cogging_dq_comp.812 0 +motor.cogging_dq_comp.813 0 +motor.cogging_dq_comp.814 0 +motor.cogging_dq_comp.815 0 +motor.cogging_dq_comp.816 0 +motor.cogging_dq_comp.817 0 +motor.cogging_dq_comp.818 0 +motor.cogging_dq_comp.819 0 +motor.cogging_dq_comp.820 0 +motor.cogging_dq_comp.821 0 +motor.cogging_dq_comp.822 0 +motor.cogging_dq_comp.823 0 +motor.cogging_dq_comp.824 0 +motor.cogging_dq_comp.825 0 +motor.cogging_dq_comp.826 0 +motor.cogging_dq_comp.827 0 +motor.cogging_dq_comp.828 0 +motor.cogging_dq_comp.829 0 +motor.cogging_dq_comp.830 0 +motor.cogging_dq_comp.831 0 +motor.cogging_dq_comp.832 0 +motor.cogging_dq_comp.833 0 +motor.cogging_dq_comp.834 0 +motor.cogging_dq_comp.835 0 +motor.cogging_dq_comp.836 0 +motor.cogging_dq_comp.837 0 +motor.cogging_dq_comp.838 0 +motor.cogging_dq_comp.839 0 +motor.cogging_dq_comp.840 0 +motor.cogging_dq_comp.841 0 +motor.cogging_dq_comp.842 0 +motor.cogging_dq_comp.843 0 +motor.cogging_dq_comp.844 0 +motor.cogging_dq_comp.845 0 +motor.cogging_dq_comp.846 0 +motor.cogging_dq_comp.847 0 +motor.cogging_dq_comp.848 0 +motor.cogging_dq_comp.849 0 +motor.cogging_dq_comp.850 0 +motor.cogging_dq_comp.851 0 +motor.cogging_dq_comp.852 0 +motor.cogging_dq_comp.853 0 +motor.cogging_dq_comp.854 0 +motor.cogging_dq_comp.855 0 +motor.cogging_dq_comp.856 0 +motor.cogging_dq_comp.857 0 +motor.cogging_dq_comp.858 0 +motor.cogging_dq_comp.859 0 +motor.cogging_dq_comp.860 0 +motor.cogging_dq_comp.861 0 +motor.cogging_dq_comp.862 0 +motor.cogging_dq_comp.863 0 +motor.cogging_dq_comp.864 0 +motor.cogging_dq_comp.865 0 +motor.cogging_dq_comp.866 0 +motor.cogging_dq_comp.867 0 +motor.cogging_dq_comp.868 0 +motor.cogging_dq_comp.869 0 +motor.cogging_dq_comp.870 0 +motor.cogging_dq_comp.871 0 +motor.cogging_dq_comp.872 0 +motor.cogging_dq_comp.873 0 +motor.cogging_dq_comp.874 0 +motor.cogging_dq_comp.875 0 +motor.cogging_dq_comp.876 0 +motor.cogging_dq_comp.877 0 +motor.cogging_dq_comp.878 0 +motor.cogging_dq_comp.879 0 +motor.cogging_dq_comp.880 0 +motor.cogging_dq_comp.881 0 +motor.cogging_dq_comp.882 0 +motor.cogging_dq_comp.883 0 +motor.cogging_dq_comp.884 0 +motor.cogging_dq_comp.885 0 +motor.cogging_dq_comp.886 0 +motor.cogging_dq_comp.887 0 +motor.cogging_dq_comp.888 0 +motor.cogging_dq_comp.889 0 +motor.cogging_dq_comp.890 0 +motor.cogging_dq_comp.891 0 +motor.cogging_dq_comp.892 0 +motor.cogging_dq_comp.893 0 +motor.cogging_dq_comp.894 0 +motor.cogging_dq_comp.895 0 +motor.cogging_dq_comp.896 0 +motor.cogging_dq_comp.897 0 +motor.cogging_dq_comp.898 0 +motor.cogging_dq_comp.899 0 +motor.cogging_dq_comp.900 0 +motor.cogging_dq_comp.901 0 +motor.cogging_dq_comp.902 0 +motor.cogging_dq_comp.903 0 +motor.cogging_dq_comp.904 0 +motor.cogging_dq_comp.905 0 +motor.cogging_dq_comp.906 0 +motor.cogging_dq_comp.907 0 +motor.cogging_dq_comp.908 0 +motor.cogging_dq_comp.909 0 +motor.cogging_dq_comp.910 0 +motor.cogging_dq_comp.911 0 +motor.cogging_dq_comp.912 0 +motor.cogging_dq_comp.913 0 +motor.cogging_dq_comp.914 0 +motor.cogging_dq_comp.915 0 +motor.cogging_dq_comp.916 0 +motor.cogging_dq_comp.917 0 +motor.cogging_dq_comp.918 0 +motor.cogging_dq_comp.919 0 +motor.cogging_dq_comp.920 0 +motor.cogging_dq_comp.921 0 +motor.cogging_dq_comp.922 0 +motor.cogging_dq_comp.923 0 +motor.cogging_dq_comp.924 0 +motor.cogging_dq_comp.925 0 +motor.cogging_dq_comp.926 0 +motor.cogging_dq_comp.927 0 +motor.cogging_dq_comp.928 0 +motor.cogging_dq_comp.929 0 +motor.cogging_dq_comp.930 0 +motor.cogging_dq_comp.931 0 +motor.cogging_dq_comp.932 0 +motor.cogging_dq_comp.933 0 +motor.cogging_dq_comp.934 0 +motor.cogging_dq_comp.935 0 +motor.cogging_dq_comp.936 0 +motor.cogging_dq_comp.937 0 +motor.cogging_dq_comp.938 0 +motor.cogging_dq_comp.939 0 +motor.cogging_dq_comp.940 0 +motor.cogging_dq_comp.941 0 +motor.cogging_dq_comp.942 0 +motor.cogging_dq_comp.943 0 +motor.cogging_dq_comp.944 0 +motor.cogging_dq_comp.945 0 +motor.cogging_dq_comp.946 0 +motor.cogging_dq_comp.947 0 +motor.cogging_dq_comp.948 0 +motor.cogging_dq_comp.949 0 +motor.cogging_dq_comp.950 0 +motor.cogging_dq_comp.951 0 +motor.cogging_dq_comp.952 0 +motor.cogging_dq_comp.953 0 +motor.cogging_dq_comp.954 0 +motor.cogging_dq_comp.955 0 +motor.cogging_dq_comp.956 0 +motor.cogging_dq_comp.957 0 +motor.cogging_dq_comp.958 0 +motor.cogging_dq_comp.959 0 +motor.cogging_dq_comp.960 0 +motor.cogging_dq_comp.961 0 +motor.cogging_dq_comp.962 0 +motor.cogging_dq_comp.963 0 +motor.cogging_dq_comp.964 0 +motor.cogging_dq_comp.965 0 +motor.cogging_dq_comp.966 0 +motor.cogging_dq_comp.967 0 +motor.cogging_dq_comp.968 0 +motor.cogging_dq_comp.969 0 +motor.cogging_dq_comp.970 0 +motor.cogging_dq_comp.971 0 +motor.cogging_dq_comp.972 0 +motor.cogging_dq_comp.973 0 +motor.cogging_dq_comp.974 0 +motor.cogging_dq_comp.975 0 +motor.cogging_dq_comp.976 0 +motor.cogging_dq_comp.977 0 +motor.cogging_dq_comp.978 0 +motor.cogging_dq_comp.979 0 +motor.cogging_dq_comp.980 0 +motor.cogging_dq_comp.981 0 +motor.cogging_dq_comp.982 0 +motor.cogging_dq_comp.983 0 +motor.cogging_dq_comp.984 0 +motor.cogging_dq_comp.985 0 +motor.cogging_dq_comp.986 0 +motor.cogging_dq_comp.987 0 +motor.cogging_dq_comp.988 0 +motor.cogging_dq_comp.989 0 +motor.cogging_dq_comp.990 0 +motor.cogging_dq_comp.991 0 +motor.cogging_dq_comp.992 0 +motor.cogging_dq_comp.993 0 +motor.cogging_dq_comp.994 0 +motor.cogging_dq_comp.995 0 +motor.cogging_dq_comp.996 0 +motor.cogging_dq_comp.997 0 +motor.cogging_dq_comp.998 0 +motor.cogging_dq_comp.999 0 +motor.cogging_dq_comp.1000 0 +motor.cogging_dq_comp.1001 0 +motor.cogging_dq_comp.1002 0 +motor.cogging_dq_comp.1003 0 +motor.cogging_dq_comp.1004 0 +motor.cogging_dq_comp.1005 0 +motor.cogging_dq_comp.1006 0 +motor.cogging_dq_comp.1007 0 +motor.cogging_dq_comp.1008 0 +motor.cogging_dq_comp.1009 0 +motor.cogging_dq_comp.1010 0 +motor.cogging_dq_comp.1011 0 +motor.cogging_dq_comp.1012 0 +motor.cogging_dq_comp.1013 0 +motor.cogging_dq_comp.1014 0 +motor.cogging_dq_comp.1015 0 +motor.cogging_dq_comp.1016 0 +motor.cogging_dq_comp.1017 0 +motor.cogging_dq_comp.1018 0 +motor.cogging_dq_comp.1019 0 +motor.cogging_dq_comp.1020 0 +motor.cogging_dq_comp.1021 0 +motor.cogging_dq_comp.1022 0 +motor.cogging_dq_comp.1023 0 +drv8323_conf.dis_cpuv 0 +drv8323_conf.dis_gdf 0 +drv8323_conf.otw_rep 0 +drv8323_conf.pwm_mode 1 +drv8323_conf.pwm_1x_asynchronous 0 +drv8323_conf.pwm_1x_dir 0 +drv8323_conf.idrivep_hs_ma 100 +drv8323_conf.idriven_hs_ma 200 +drv8323_conf.cbc 1 +drv8323_conf.tdrive_ns 1000 +drv8323_conf.idrivep_ls_ma 100 +drv8323_conf.idriven_ls_ma 200 +drv8323_conf.tretry 0 +drv8323_conf.dead_time_ns 50 +drv8323_conf.ocp_mode 0 +drv8323_conf.ocp_deg_us 4 +drv8323_conf.vds_lvl_mv 700 +drv8323_conf.csa_fet 0 +drv8323_conf.vref_div 1 +drv8323_conf.ls_ref 0 +drv8323_conf.csa_gain 20 +drv8323_conf.dis_sen 0 +drv8323_conf.sen_lvl_mv 500 +servo.pwm_rate_hz 30000 +servo.i_gain 20.000000 +servo.current_sense_ohm 0.000500 +servo.pwm_comp_off 0.027000 +servo.pwm_comp_mag 0.005000 +servo.pwm_scale 1.000000 +servo.max_voltage 46.000000 +servo.max_power_W 450.000000 +servo.derate_temperature 50.000000 +servo.fault_temperature 75.000000 +servo.enable_motor_temperature 0 +servo.motor_derate_temperature 50.000000 +servo.motor_fault_temperature nan +servo.velocity_threshold 0.000000 +servo.position_derate 0.020000 +servo.adc_cur_cycles 2 +servo.adc_aux_cycles 47 +servo.pid_dq.kp 1.341388 +servo.pid_dq.ki 2513.487549 +servo.pid_position.kp 1500.000000 +servo.pid_position.ki 0.000000 +servo.pid_position.kd 0.000000 +servo.pid_position.iratelimit -1.000000 +servo.pid_position.ilimit 0.000000 +servo.pid_position.max_desired_rate 0.000000 +servo.pid_position.sign -1 +servo.current_feedforward 1.000000 +servo.bemf_feedforward 1.000000 +servo.default_velocity_limit 0.050000 +servo.default_accel_limit 0.200000 +servo.voltage_mode_control 0 +servo.fixed_voltage_mode 0 +servo.fixed_voltage_control_V 0.000000 +servo.max_position_slip nan +servo.max_velocity_slip nan +servo.default_timeout_s 0.100000 +servo.timeout_max_torque_Nm 5.000000 +servo.timeout_mode 12 +servo.flux_brake_min_voltage 43.500000 +servo.flux_brake_resistance_ohm 0.025000 +servo.max_current_A 100.000000 +servo.derate_current_A -20.000000 +servo.max_velocity 0.100000 +servo.max_velocity_derate 2.000000 +servo.cooldown_cycles 256 +servo.velocity_zero_capture_threshold 0.050000 +servo.timing_fault 0 +servo.emit_debug 0 +servopos.position_min nan +servopos.position_max nan +id.id 49 +can.prefix 0 + diff --git a/config/teleop.yaml b/config/teleop.yaml index aeec76864..27158d2ee 100644 --- a/config/teleop.yaml +++ b/config/teleop.yaml @@ -4,50 +4,38 @@ teleop: ra_names: ["joint_a", "joint_b", "joint_c", "joint_de_pitch", "joint_de_roll", "allen_key", "gripper"] ra_controls: joint_a: - multiplier: 1 + multiplier: -1 slow_mode_multiplier: 0.5 - invert: False - xbox_index: 0 joint_b: - multiplier: -1 + multiplier: 1 slow_mode_multiplier: 1 - invert: True - xbox_index: 1 joint_c: - multiplier: -1 + multiplier: 1 slow_mode_multiplier: 0.6 - invert: True - xbox_index: 4 joint_de_pitch: - multiplier: 0.5 + multiplier: -1 slow_mode_multiplier: 1 - invert: False - xbox_index: 2 joint_de_roll: - multiplier: 0.5 + multiplier: 1 slow_mode_multiplier: 1 - invert: False - xbox_index: 5 allen_key: multiplier: 1 slow_mode_multiplier: 1 - invert: False gripper: multiplier: 1 slow_mode_multiplier: 1 - invert: False sa_controls: sa_x: multiplier: 1 - slow_mode_multiplier: 0.5 + slow_mode_multiplier: 1.0 # 0.5 xbox_index: 1 sa_y: multiplier: -1 - slow_mode_multiplier: 0.5 + slow_mode_multiplier: 1.0 # 0.5 xbox_index: 0 sa_z: - multiplier: -1 + multiplier: 1 slow_mode_multiplier: 0.5 xbox_index: 4 sampler: @@ -69,23 +57,34 @@ teleop: enabled: true multiplier: 1 twist: - multiplier: 0.7 + multiplier: 0.6 + tilt: + multiplier: 1 + pan: + multiplier: 1 xbox_mappings: - left_js_x: 0 - left_js_y: 1 - left_trigger: 2 - right_trigger: 5 - right_js_x: 3 - right_js_y: 4 - right_bumper: 5 - left_bumper: 4 - d_pad_x: 6 - d_pad_y: 7 + left_x: 0 + left_y: 1 + right_x: 2 + right_y: 3 a: 0 b: 1 x: 2 y: 3 + left_bumper: 4 + right_bumper: 5 + left_trigger: 6 + right_trigger: 7 + back: 8 + forward: 9 + left_stick_click: 10 + right_stick_click: 11 + dpad_up: 12 + dpad_down: 13 + dpad_left: 14 + dpad_right: 15 + home: 16 joystick_mappings: left_right: 0 @@ -123,6 +122,6 @@ teleop: bar: ["/tf_static", "/rosout", "/tf"] ik_multipliers: - x: 1 - y: 1 - z: 1 \ No newline at end of file + x: 0.1 + y: 0.1 + z: 0.1 \ No newline at end of file diff --git a/deps/imgui/backends/imgui_impl_wgpu.cpp b/deps/imgui/backends/imgui_impl_wgpu.cpp index f41b8c8d5..94caded3a 100644 --- a/deps/imgui/backends/imgui_impl_wgpu.cpp +++ b/deps/imgui/backends/imgui_impl_wgpu.cpp @@ -16,6 +16,10 @@ // CHANGELOG // (minor and older changes stripped away, please see git history for details) +// 2024-01-22: Added configurable PipelineMultisampleState struct. (#7240) +// 2024-01-22: (Breaking) ImGui_ImplWGPU_Init() now takes a ImGui_ImplWGPU_InitInfo structure instead of variety of parameters, allowing for easier further changes. +// 2024-01-22: Fixed pipeline layout leak. (#7245) +// 2024-01-17: Explicitly fill all of WGPUDepthStencilState since standard removed defaults. // 2023-07-13: Use WGPUShaderModuleWGSLDescriptor's code instead of source. use WGPUMipmapFilterMode_Linear instead of WGPUFilterMode_Linear. (#6602) // 2023-04-11: Align buffer sizes. Use WGSL shaders instead of precompiled SPIR-V. // 2023-04-11: Reorganized backend to pull data from a single structure to facilitate usage with multiple-contexts (all g_XXXX access changed to bd->XXXX). @@ -72,16 +76,17 @@ struct Uniforms struct ImGui_ImplWGPU_Data { - WGPUDevice wgpuDevice = nullptr; - WGPUQueue defaultQueue = nullptr; - WGPUTextureFormat renderTargetFormat = WGPUTextureFormat_Undefined; - WGPUTextureFormat depthStencilFormat = WGPUTextureFormat_Undefined; - WGPURenderPipeline pipelineState = nullptr; - - RenderResources renderResources; - FrameResources* pFrameResources = nullptr; - unsigned int numFramesInFlight = 0; - unsigned int frameIndex = UINT_MAX; + ImGui_ImplWGPU_InitInfo initInfo; + WGPUDevice wgpuDevice = nullptr; + WGPUQueue defaultQueue = nullptr; + WGPUTextureFormat renderTargetFormat = WGPUTextureFormat_Undefined; + WGPUTextureFormat depthStencilFormat = WGPUTextureFormat_Undefined; + WGPURenderPipeline pipelineState = nullptr; + + RenderResources renderResources; + FrameResources* pFrameResources = nullptr; + unsigned int numFramesInFlight = 0; + unsigned int frameIndex = UINT_MAX; }; // Backend data stored in io.BackendRendererUserData to allow support for multiple Dear ImGui contexts @@ -187,6 +192,12 @@ static void SafeRelease(WGPUBuffer& res) wgpuBufferRelease(res); res = nullptr; } +static void SafeRelease(WGPUPipelineLayout& res) +{ + if (res) + wgpuPipelineLayoutRelease(res); + res = nullptr; +} static void SafeRelease(WGPURenderPipeline& res) { if (res) @@ -573,9 +584,7 @@ bool ImGui_ImplWGPU_CreateDeviceObjects() graphics_pipeline_desc.primitive.stripIndexFormat = WGPUIndexFormat_Undefined; graphics_pipeline_desc.primitive.frontFace = WGPUFrontFace_CW; graphics_pipeline_desc.primitive.cullMode = WGPUCullMode_None; - graphics_pipeline_desc.multisample.count = 1; - graphics_pipeline_desc.multisample.mask = UINT_MAX; - graphics_pipeline_desc.multisample.alphaToCoverageEnabled = false; + graphics_pipeline_desc.multisample = bd->initInfo.PipelineMultisampleState; // Bind group layouts WGPUBindGroupLayoutEntry common_bg_layout_entries[2] = {}; @@ -667,7 +676,13 @@ bool ImGui_ImplWGPU_CreateDeviceObjects() depth_stencil_state.depthWriteEnabled = false; depth_stencil_state.depthCompare = WGPUCompareFunction_Always; depth_stencil_state.stencilFront.compare = WGPUCompareFunction_Always; + depth_stencil_state.stencilFront.failOp = WGPUStencilOperation_Keep; + depth_stencil_state.stencilFront.depthFailOp = WGPUStencilOperation_Keep; + depth_stencil_state.stencilFront.passOp = WGPUStencilOperation_Keep; depth_stencil_state.stencilBack.compare = WGPUCompareFunction_Always; + depth_stencil_state.stencilBack.failOp = WGPUStencilOperation_Keep; + depth_stencil_state.stencilBack.depthFailOp = WGPUStencilOperation_Keep; + depth_stencil_state.stencilBack.passOp = WGPUStencilOperation_Keep; // Configure disabled depth-stencil state graphics_pipeline_desc.depthStencil = (bd->depthStencilFormat == WGPUTextureFormat_Undefined) ? nullptr : &depth_stencil_state; @@ -697,6 +712,7 @@ bool ImGui_ImplWGPU_CreateDeviceObjects() SafeRelease(vertex_shader_desc.module); SafeRelease(pixel_shader_desc.module); + SafeRelease(graphics_pipeline_desc.layout); SafeRelease(bg_layouts[0]); return true; @@ -718,7 +734,7 @@ void ImGui_ImplWGPU_InvalidateDeviceObjects() SafeRelease(bd->pFrameResources[i]); } -bool ImGui_ImplWGPU_Init(WGPUDevice device, int num_frames_in_flight, WGPUTextureFormat rt_format, WGPUTextureFormat depth_format) +bool ImGui_ImplWGPU_Init(ImGui_ImplWGPU_InitInfo* init_info) { ImGuiIO& io = ImGui::GetIO(); IM_ASSERT(io.BackendRendererUserData == nullptr && "Already initialized a renderer backend!"); @@ -729,11 +745,12 @@ bool ImGui_ImplWGPU_Init(WGPUDevice device, int num_frames_in_flight, WGPUTextur io.BackendRendererName = "imgui_impl_webgpu"; io.BackendFlags |= ImGuiBackendFlags_RendererHasVtxOffset; // We can honor the ImDrawCmd::VtxOffset field, allowing for large meshes. - bd->wgpuDevice = device; + bd->initInfo = *init_info; + bd->wgpuDevice = init_info->Device; bd->defaultQueue = wgpuDeviceGetQueue(bd->wgpuDevice); - bd->renderTargetFormat = rt_format; - bd->depthStencilFormat = depth_format; - bd->numFramesInFlight = num_frames_in_flight; + bd->renderTargetFormat = init_info->RenderTargetFormat; + bd->depthStencilFormat = init_info->DepthStencilFormat; + bd->numFramesInFlight = init_info->NumFramesInFlight; bd->frameIndex = UINT_MAX; bd->renderResources.FontTexture = nullptr; @@ -746,8 +763,8 @@ bool ImGui_ImplWGPU_Init(WGPUDevice device, int num_frames_in_flight, WGPUTextur bd->renderResources.ImageBindGroupLayout = nullptr; // Create buffers with a default size (they will later be grown as needed) - bd->pFrameResources = new FrameResources[num_frames_in_flight]; - for (int i = 0; i < num_frames_in_flight; i++) + bd->pFrameResources = new FrameResources[bd->numFramesInFlight]; + for (int i = 0; i < bd->numFramesInFlight; i++) { FrameResources* fr = &bd->pFrameResources[i]; fr->IndexBuffer = nullptr; diff --git a/deps/imgui/backends/imgui_impl_wgpu.h b/deps/imgui/backends/imgui_impl_wgpu.h index 14d9fccc1..57ddd78fc 100644 --- a/deps/imgui/backends/imgui_impl_wgpu.h +++ b/deps/imgui/backends/imgui_impl_wgpu.h @@ -20,7 +20,24 @@ #include -IMGUI_IMPL_API bool ImGui_ImplWGPU_Init(WGPUDevice device, int num_frames_in_flight, WGPUTextureFormat rt_format, WGPUTextureFormat depth_format = WGPUTextureFormat_Undefined); +// Initialization data, for ImGui_ImplWGPU_Init() +struct ImGui_ImplWGPU_InitInfo +{ + WGPUDevice Device; + int NumFramesInFlight = 3; + WGPUTextureFormat RenderTargetFormat = WGPUTextureFormat_Undefined; + WGPUTextureFormat DepthStencilFormat = WGPUTextureFormat_Undefined; + WGPUMultisampleState PipelineMultisampleState = {}; + + ImGui_ImplWGPU_InitInfo() + { + PipelineMultisampleState.count = 1; + PipelineMultisampleState.mask = UINT32_MAX; + PipelineMultisampleState.alphaToCoverageEnabled = false; + } +}; + +IMGUI_IMPL_API bool ImGui_ImplWGPU_Init(ImGui_ImplWGPU_InitInfo* init_info); IMGUI_IMPL_API void ImGui_ImplWGPU_Shutdown(); IMGUI_IMPL_API void ImGui_ImplWGPU_NewFrame(); IMGUI_IMPL_API void ImGui_ImplWGPU_RenderDrawData(ImDrawData* draw_data, WGPURenderPassEncoder pass_encoder); @@ -29,4 +46,4 @@ IMGUI_IMPL_API void ImGui_ImplWGPU_RenderDrawData(ImDrawData* draw_data, WGPURen IMGUI_IMPL_API void ImGui_ImplWGPU_InvalidateDeviceObjects(); IMGUI_IMPL_API bool ImGui_ImplWGPU_CreateDeviceObjects(); -#endif // #ifndef IMGUI_DISABLE +#endif // #ifndef IMGUI_DISABLE \ No newline at end of file diff --git a/src/esw/mjbots/README.md b/deps/mjbots/README.md similarity index 100% rename from src/esw/mjbots/README.md rename to deps/mjbots/README.md diff --git a/src/esw/mjbots/moteus/moteus.h b/deps/mjbots/moteus/moteus.h similarity index 96% rename from src/esw/mjbots/moteus/moteus.h rename to deps/mjbots/moteus/moteus.h index b2df3b77d..b1f3d7db1 100644 --- a/src/esw/mjbots/moteus/moteus.h +++ b/deps/mjbots/moteus/moteus.h @@ -424,6 +424,38 @@ class Controller { } + ///////////////////////////////////////// + // RecapturePositionVelocity + + CanFdFrame MakeRecapturePositionVelocity( + const RecapturePositionVelocity::Command& cmd = {}, + const RecapturePositionVelocity::Format* command_override = nullptr, + const Query::Format* query_override = nullptr) { + return MakeFrame(RecapturePositionVelocity(), cmd, + (command_override == nullptr ? + RecapturePositionVelocity::Format() : *command_override), + query_override); + } + + Optional SetRecapturePositionVelocity( + const RecapturePositionVelocity::Command& cmd, + const RecapturePositionVelocity::Format* command_override = nullptr, + const Query::Format* query_override = nullptr) { + return ExecuteSingleCommand( + MakeRecapturePositionVelocity(cmd, command_override, query_override)); + } + + void AsyncRecapturePositionVelocity( + const RecapturePositionVelocity::Command& cmd, + Result* result, CompletionCallback callback, + const RecapturePositionVelocity::Format* command_override = nullptr, + const Query::Format* query_override = nullptr) { + AsyncStartSingleCommand( + MakeRecapturePositionVelocity(cmd, command_override, query_override), + result, callback); + } + + ///////////////////////////////////////// // ClockTrim diff --git a/src/esw/mjbots/moteus/moteus_multiplex.h b/deps/mjbots/moteus/moteus_multiplex.h similarity index 100% rename from src/esw/mjbots/moteus/moteus_multiplex.h rename to deps/mjbots/moteus/moteus_multiplex.h diff --git a/src/esw/mjbots/moteus/moteus_optional.h b/deps/mjbots/moteus/moteus_optional.h similarity index 100% rename from src/esw/mjbots/moteus/moteus_optional.h rename to deps/mjbots/moteus/moteus_optional.h diff --git a/src/esw/mjbots/moteus/moteus_protocol.h b/deps/mjbots/moteus/moteus_protocol.h similarity index 98% rename from src/esw/mjbots/moteus/moteus_protocol.h rename to deps/mjbots/moteus/moteus_protocol.h index 0e0595c4b..dc0cb7920 100644 --- a/src/esw/mjbots/moteus/moteus_protocol.h +++ b/deps/mjbots/moteus/moteus_protocol.h @@ -26,6 +26,7 @@ #ifndef ARDUINO #include +#include #define NaN std::numeric_limits::quiet_NaN(); #else @@ -197,6 +198,7 @@ enum Register : uint16_t { kSetOutputNearest = 0x130, kSetOutputExact = 0x131, kRequireReindex = 0x132, + kRecapturePositionVelocity = 0x133, kDriverFault1 = 0x140, kDriverFault2 = 0x141, @@ -399,7 +401,7 @@ struct Query { // below. if (required_registers > 512) { ::abort(); } - Resolution resolutions[required_registers]; + std::vector resolutions(required_registers); ::memset(&resolutions[0], 0, sizeof(Resolution) * required_registers); for (int16_t this_register = min_register_number, index = 0; @@ -415,7 +417,7 @@ struct Query { } WriteCombiner combiner( frame, 0x10, min_register_number, - resolutions, required_registers); + &resolutions[0], required_registers); for (uint16_t i = 0; i < required_registers; i++) { combiner.MaybeWrite(); } @@ -640,6 +642,7 @@ struct Query { { R::kSetOutputNearest, 3, MP::kInt, }, // { R::kSetOutputExact, 1, MP::kInt, }, // { R::kRequireReindex, 1, MP::kInt, }, + // { R::kRecapturePositionVelocity, 1, MP::kInt, } { R::kDriverFault1, 2, MP::kInt, }, // { R::kDriverFault2, 1, MP::kInt, }, @@ -721,7 +724,7 @@ struct GenericQuery { // below. if (required_registers > 512) { ::abort(); } - Resolution resolutions[required_registers]; + std::vector resolutions(required_registers); ::memset(&resolutions[0], 0, sizeof(Resolution) * required_registers); for (int16_t this_register = min_register_number, index = 0; @@ -737,7 +740,7 @@ struct GenericQuery { } WriteCombiner combiner( frame, 0x10, min_register_number, - resolutions, required_registers); + &resolutions[0], required_registers); for (uint16_t i = 0; i < required_registers; i++) { combiner.MaybeWrite(); } @@ -1150,6 +1153,18 @@ struct RequireReindex { } }; +struct RecapturePositionVelocity { + struct Command {}; + struct Format {}; + + static uint8_t Make(WriteCanData* frame, const Command&, const Format&) { + frame->Write(Multiplex::kWriteInt8 | 0x01); + frame->WriteVaruint(Register::kRecapturePositionVelocity); + frame->Write(1); + return 0; + } +}; + struct DiagnosticWrite { struct Command { int8_t channel = 1; diff --git a/src/esw/mjbots/moteus/moteus_tokenizer.h b/deps/mjbots/moteus/moteus_tokenizer.h similarity index 100% rename from src/esw/mjbots/moteus/moteus_tokenizer.h rename to deps/mjbots/moteus/moteus_tokenizer.h diff --git a/src/esw/mjbots/moteus/moteus_transport.h b/deps/mjbots/moteus/moteus_transport.h similarity index 98% rename from src/esw/mjbots/moteus/moteus_transport.h rename to deps/mjbots/moteus/moteus_transport.h index 976a9fc14..822c3c9d3 100644 --- a/src/esw/mjbots/moteus/moteus_transport.h +++ b/deps/mjbots/moteus/moteus_transport.h @@ -45,6 +45,10 @@ #include "moteus_protocol.h" #include "moteus_tokenizer.h" +#ifdef CANFD_FDF +#define MJBOTS_MOTEUS_ENABLE_SOCKETCAN 1 +#endif + namespace mjbots { namespace moteus { @@ -313,7 +317,8 @@ class TimeoutTransport : public Transport { expected_ok_count++; CHILD_SendCanFdFrame(frames[i]); if (frames[i].reply_required) { - if ((frames[i].destination + 1) > expected_reply_count_.size()) { + if ((frames[i].destination + 1) > + static_cast(expected_reply_count_.size())) { expected_reply_count_.resize(frames[i].destination + 1); } expected_reply_count_[frames[i].destination]++; @@ -528,7 +533,7 @@ class Fdcanusb : public details::TimeoutTransport { virtual ConsumeCount CHILD_ConsumeData( std::vector* replies, - int expected_ok_count, + int /* expected_ok_count */, std::vector* expected_reply_count) override { // Read into our line buffer. const int to_read = sizeof(line_buffer_) - line_buffer_pos_; @@ -620,7 +625,8 @@ class Fdcanusb : public details::TimeoutTransport { } if (expected_reply_count) { - if (this_frame.source < expected_reply_count->size()) { + if (this_frame.source < + static_cast(expected_reply_count->size())) { (*expected_reply_count)[this_frame.source] = std::max( (*expected_reply_count)[this_frame.source] - 1, 0); } @@ -754,6 +760,7 @@ class Fdcanusb : public details::TimeoutTransport { }; +#ifdef MJBOTS_MOTEUS_ENABLE_SOCKETCAN class Socketcan : public details::TimeoutTransport { public: struct Options : details::TimeoutTransport::Options { @@ -836,7 +843,7 @@ class Socketcan : public details::TimeoutTransport { virtual ConsumeCount CHILD_ConsumeData( std::vector* replies, - int expected_ok_count, + int /* expected_ok_count */, std::vector* expected_reply_count) override { struct canfd_frame recv_frame = {}; FailIf(::read(socket_, &recv_frame, sizeof(recv_frame)) < 0, @@ -857,7 +864,8 @@ class Socketcan : public details::TimeoutTransport { this_frame.size = recv_frame.len; if (expected_reply_count) { - if (this_frame.source < expected_reply_count->size()) { + if (this_frame.source < + static_cast(expected_reply_count->size())) { (*expected_reply_count)[this_frame.source] = std::max( (*expected_reply_count)[this_frame.source] - 1, 0); } @@ -878,7 +886,7 @@ class Socketcan : public details::TimeoutTransport { const Options options_; details::FileDescriptor socket_; }; - +#endif // MJBOTS_MOTEUS_ENABLE_SOCKETCAN /// A factory which can create transports given an optional set of /// commandline arguments. @@ -968,6 +976,7 @@ class FdcanusbFactory : public TransportFactory { } }; +#ifdef MJBOTS_MOTEUS_ENABLE_SOCKETCAN class SocketcanFactory : public TransportFactory { public: virtual ~SocketcanFactory() {} @@ -1019,6 +1028,7 @@ class SocketcanFactory : public TransportFactory { return false; } }; +#endif // MJBOTS_MOTEUS_ENABLE_SOCKETCAN class TransportRegistry { public: @@ -1106,7 +1116,9 @@ class TransportRegistry { private: TransportRegistry() { Register(); +#ifdef MJBOTS_MOTEUS_ENABLE_SOCKETCAN Register(); +#endif } std::vector> items_; diff --git a/deps/webgpuhpp/webgpu.hpp b/deps/webgpuhpp/webgpu.hpp index 9cb503b0f..7fc8a2478 100644 --- a/deps/webgpuhpp/webgpu.hpp +++ b/deps/webgpuhpp/webgpu.hpp @@ -3,7 +3,7 @@ * https://github.com/eliemichel/LearnWebGPU * * MIT License - * Copyright (c) 2022 Elie Michel + * Copyright (c) 2022-2024 Elie Michel * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -59,6 +59,7 @@ class Type { \ public: \ typedef Type S; /* S == Self */ \ typedef WGPU ## Type W; /* W == WGPU Type */ \ + Type() : m_raw(nullptr) {} \ Type(const W& w) : m_raw(w) {} \ operator W&() { return m_raw; } \ operator const W&() const { return m_raw; } \ @@ -118,35 +119,50 @@ private: \ #define END }; - +constexpr static uint32_t kDepthSliceUndefined = std::numeric_limits::max(); // Other type aliases using Flags = uint32_t; using Bool = uint32_t; using BufferUsageFlags = WGPUFlags; using ColorWriteMaskFlags = WGPUFlags; +using HeapPropertyFlags = WGPUFlags; using MapModeFlags = WGPUFlags; using ShaderStageFlags = WGPUFlags; using TextureUsageFlags = WGPUFlags; // Enumerations +ENUM(WGSLFeatureName) + ENUM_ENTRY(Undefined, 0x00000000) + ENUM_ENTRY(ReadonlyAndReadwriteStorageTextures, 0x00000001) + ENUM_ENTRY(Packed4x8IntegerDotProduct, 0x00000002) + ENUM_ENTRY(UnrestrictedPointerParameters, 0x00000003) + ENUM_ENTRY(PointerCompositeAccess, 0x00000004) + ENUM_ENTRY(ChromiumTestingUnimplemented, 0x000003E8) + ENUM_ENTRY(ChromiumTestingUnsafeExperimental, 0x000003E9) + ENUM_ENTRY(ChromiumTestingExperimental, 0x000003EA) + ENUM_ENTRY(ChromiumTestingShippedWithKillswitch, 0x000003EB) + ENUM_ENTRY(ChromiumTestingShipped, 0x000003EC) + ENUM_ENTRY(Force32, 0x7FFFFFFF) +END ENUM(AdapterType) - ENUM_ENTRY(DiscreteGPU, 0x00000000) - ENUM_ENTRY(IntegratedGPU, 0x00000001) - ENUM_ENTRY(CPU, 0x00000002) - ENUM_ENTRY(Unknown, 0x00000003) + ENUM_ENTRY(DiscreteGPU, 0x00000001) + ENUM_ENTRY(IntegratedGPU, 0x00000002) + ENUM_ENTRY(CPU, 0x00000003) + ENUM_ENTRY(Unknown, 0x00000004) ENUM_ENTRY(Force32, 0x7FFFFFFF) END ENUM(AddressMode) - ENUM_ENTRY(Repeat, 0x00000000) - ENUM_ENTRY(MirrorRepeat, 0x00000001) - ENUM_ENTRY(ClampToEdge, 0x00000002) + ENUM_ENTRY(Undefined, 0x00000000) + ENUM_ENTRY(ClampToEdge, 0x00000001) + ENUM_ENTRY(Repeat, 0x00000002) + ENUM_ENTRY(MirrorRepeat, 0x00000003) ENUM_ENTRY(Force32, 0x7FFFFFFF) END ENUM(AlphaMode) - ENUM_ENTRY(Premultiplied, 0x00000000) - ENUM_ENTRY(Unpremultiplied, 0x00000001) - ENUM_ENTRY(Opaque, 0x00000002) + ENUM_ENTRY(Opaque, 0x00000001) + ENUM_ENTRY(Premultiplied, 0x00000002) + ENUM_ENTRY(Unpremultiplied, 0x00000003) ENUM_ENTRY(Force32, 0x7FFFFFFF) END ENUM(BackendType) @@ -162,31 +178,33 @@ ENUM(BackendType) ENUM_ENTRY(Force32, 0x7FFFFFFF) END ENUM(BlendFactor) - ENUM_ENTRY(Zero, 0x00000000) - ENUM_ENTRY(One, 0x00000001) - ENUM_ENTRY(Src, 0x00000002) - ENUM_ENTRY(OneMinusSrc, 0x00000003) - ENUM_ENTRY(SrcAlpha, 0x00000004) - ENUM_ENTRY(OneMinusSrcAlpha, 0x00000005) - ENUM_ENTRY(Dst, 0x00000006) - ENUM_ENTRY(OneMinusDst, 0x00000007) - ENUM_ENTRY(DstAlpha, 0x00000008) - ENUM_ENTRY(OneMinusDstAlpha, 0x00000009) - ENUM_ENTRY(SrcAlphaSaturated, 0x0000000A) - ENUM_ENTRY(Constant, 0x0000000B) - ENUM_ENTRY(OneMinusConstant, 0x0000000C) - ENUM_ENTRY(Src1, 0x0000000D) - ENUM_ENTRY(OneMinusSrc1, 0x0000000E) - ENUM_ENTRY(Src1Alpha, 0x0000000F) - ENUM_ENTRY(OneMinusSrc1Alpha, 0x00000010) + ENUM_ENTRY(Undefined, 0x00000000) + ENUM_ENTRY(Zero, 0x00000001) + ENUM_ENTRY(One, 0x00000002) + ENUM_ENTRY(Src, 0x00000003) + ENUM_ENTRY(OneMinusSrc, 0x00000004) + ENUM_ENTRY(SrcAlpha, 0x00000005) + ENUM_ENTRY(OneMinusSrcAlpha, 0x00000006) + ENUM_ENTRY(Dst, 0x00000007) + ENUM_ENTRY(OneMinusDst, 0x00000008) + ENUM_ENTRY(DstAlpha, 0x00000009) + ENUM_ENTRY(OneMinusDstAlpha, 0x0000000A) + ENUM_ENTRY(SrcAlphaSaturated, 0x0000000B) + ENUM_ENTRY(Constant, 0x0000000C) + ENUM_ENTRY(OneMinusConstant, 0x0000000D) + ENUM_ENTRY(Src1, 0x0000000E) + ENUM_ENTRY(OneMinusSrc1, 0x0000000F) + ENUM_ENTRY(Src1Alpha, 0x00000010) + ENUM_ENTRY(OneMinusSrc1Alpha, 0x00000011) ENUM_ENTRY(Force32, 0x7FFFFFFF) END ENUM(BlendOperation) - ENUM_ENTRY(Add, 0x00000000) - ENUM_ENTRY(Subtract, 0x00000001) - ENUM_ENTRY(ReverseSubtract, 0x00000002) - ENUM_ENTRY(Min, 0x00000003) - ENUM_ENTRY(Max, 0x00000004) + ENUM_ENTRY(Undefined, 0x00000000) + ENUM_ENTRY(Add, 0x00000001) + ENUM_ENTRY(Subtract, 0x00000002) + ENUM_ENTRY(ReverseSubtract, 0x00000003) + ENUM_ENTRY(Min, 0x00000004) + ENUM_ENTRY(Max, 0x00000005) ENUM_ENTRY(Force32, 0x7FFFFFFF) END ENUM(BufferBindingType) @@ -198,20 +216,21 @@ ENUM(BufferBindingType) END ENUM(BufferMapAsyncStatus) ENUM_ENTRY(Success, 0x00000000) - ENUM_ENTRY(ValidationError, 0x00000001) - ENUM_ENTRY(Unknown, 0x00000002) - ENUM_ENTRY(DeviceLost, 0x00000003) - ENUM_ENTRY(DestroyedBeforeCallback, 0x00000004) - ENUM_ENTRY(UnmappedBeforeCallback, 0x00000005) - ENUM_ENTRY(MappingAlreadyPending, 0x00000006) - ENUM_ENTRY(OffsetOutOfRange, 0x00000007) - ENUM_ENTRY(SizeOutOfRange, 0x00000008) + ENUM_ENTRY(InstanceDropped, 0x00000001) + ENUM_ENTRY(ValidationError, 0x00000002) + ENUM_ENTRY(Unknown, 0x00000003) + ENUM_ENTRY(DeviceLost, 0x00000004) + ENUM_ENTRY(DestroyedBeforeCallback, 0x00000005) + ENUM_ENTRY(UnmappedBeforeCallback, 0x00000006) + ENUM_ENTRY(MappingAlreadyPending, 0x00000007) + ENUM_ENTRY(OffsetOutOfRange, 0x00000008) + ENUM_ENTRY(SizeOutOfRange, 0x00000009) ENUM_ENTRY(Force32, 0x7FFFFFFF) END ENUM(BufferMapState) - ENUM_ENTRY(Unmapped, 0x00000000) - ENUM_ENTRY(Pending, 0x00000001) - ENUM_ENTRY(Mapped, 0x00000002) + ENUM_ENTRY(Unmapped, 0x00000001) + ENUM_ENTRY(Pending, 0x00000002) + ENUM_ENTRY(Mapped, 0x00000003) ENUM_ENTRY(Force32, 0x7FFFFFFF) END ENUM(CallbackMode) @@ -224,40 +243,43 @@ ENUM(CompareFunction) ENUM_ENTRY(Undefined, 0x00000000) ENUM_ENTRY(Never, 0x00000001) ENUM_ENTRY(Less, 0x00000002) - ENUM_ENTRY(LessEqual, 0x00000003) - ENUM_ENTRY(Greater, 0x00000004) - ENUM_ENTRY(GreaterEqual, 0x00000005) - ENUM_ENTRY(Equal, 0x00000006) - ENUM_ENTRY(NotEqual, 0x00000007) + ENUM_ENTRY(Equal, 0x00000003) + ENUM_ENTRY(LessEqual, 0x00000004) + ENUM_ENTRY(Greater, 0x00000005) + ENUM_ENTRY(NotEqual, 0x00000006) + ENUM_ENTRY(GreaterEqual, 0x00000007) ENUM_ENTRY(Always, 0x00000008) ENUM_ENTRY(Force32, 0x7FFFFFFF) END ENUM(CompilationInfoRequestStatus) ENUM_ENTRY(Success, 0x00000000) - ENUM_ENTRY(Error, 0x00000001) - ENUM_ENTRY(DeviceLost, 0x00000002) - ENUM_ENTRY(Unknown, 0x00000003) + ENUM_ENTRY(InstanceDropped, 0x00000001) + ENUM_ENTRY(Error, 0x00000002) + ENUM_ENTRY(DeviceLost, 0x00000003) + ENUM_ENTRY(Unknown, 0x00000004) ENUM_ENTRY(Force32, 0x7FFFFFFF) END ENUM(CompilationMessageType) - ENUM_ENTRY(Error, 0x00000000) - ENUM_ENTRY(Warning, 0x00000001) - ENUM_ENTRY(Info, 0x00000002) + ENUM_ENTRY(Error, 0x00000001) + ENUM_ENTRY(Warning, 0x00000002) + ENUM_ENTRY(Info, 0x00000003) ENUM_ENTRY(Force32, 0x7FFFFFFF) END ENUM(CreatePipelineAsyncStatus) ENUM_ENTRY(Success, 0x00000000) - ENUM_ENTRY(ValidationError, 0x00000001) - ENUM_ENTRY(InternalError, 0x00000002) - ENUM_ENTRY(DeviceLost, 0x00000003) - ENUM_ENTRY(DeviceDestroyed, 0x00000004) - ENUM_ENTRY(Unknown, 0x00000005) + ENUM_ENTRY(InstanceDropped, 0x00000001) + ENUM_ENTRY(ValidationError, 0x00000002) + ENUM_ENTRY(InternalError, 0x00000003) + ENUM_ENTRY(DeviceLost, 0x00000004) + ENUM_ENTRY(DeviceDestroyed, 0x00000005) + ENUM_ENTRY(Unknown, 0x00000006) ENUM_ENTRY(Force32, 0x7FFFFFFF) END ENUM(CullMode) - ENUM_ENTRY(None, 0x00000000) - ENUM_ENTRY(Front, 0x00000001) - ENUM_ENTRY(Back, 0x00000002) + ENUM_ENTRY(Undefined, 0x00000000) + ENUM_ENTRY(None, 0x00000001) + ENUM_ENTRY(Front, 0x00000002) + ENUM_ENTRY(Back, 0x00000003) ENUM_ENTRY(Force32, 0x7FFFFFFF) END ENUM(DeviceLostReason) @@ -266,9 +288,9 @@ ENUM(DeviceLostReason) ENUM_ENTRY(Force32, 0x7FFFFFFF) END ENUM(ErrorFilter) - ENUM_ENTRY(Validation, 0x00000000) - ENUM_ENTRY(OutOfMemory, 0x00000001) - ENUM_ENTRY(Internal, 0x00000002) + ENUM_ENTRY(Validation, 0x00000001) + ENUM_ENTRY(OutOfMemory, 0x00000002) + ENUM_ENTRY(Internal, 0x00000003) ENUM_ENTRY(Force32, 0x7FFFFFFF) END ENUM(ErrorType) @@ -303,7 +325,6 @@ ENUM(FeatureName) ENUM_ENTRY(DawnInternalUsages, 0x000003EA) ENUM_ENTRY(DawnMultiPlanarFormats, 0x000003EB) ENUM_ENTRY(DawnNative, 0x000003EC) - ENUM_ENTRY(ChromiumExperimentalDp4a, 0x000003ED) ENUM_ENTRY(ChromiumExperimentalTimestampQueryInsidePasses, 0x000003EE) ENUM_ENTRY(ImplicitDeviceSynchronization, 0x000003EF) ENUM_ENTRY(SurfaceCapabilities, 0x000003F0) @@ -314,7 +335,6 @@ ENUM(FeatureName) ENUM_ENTRY(ANGLETextureSharing, 0x000003F5) ENUM_ENTRY(ChromiumExperimentalSubgroups, 0x000003F6) ENUM_ENTRY(ChromiumExperimentalSubgroupUniformControlFlow, 0x000003F7) - ENUM_ENTRY(ChromiumExperimentalReadWriteStorageTexture, 0x000003F8) ENUM_ENTRY(PixelLocalStorageCoherent, 0x000003F9) ENUM_ENTRY(PixelLocalStorageNonCoherent, 0x000003FA) ENUM_ENTRY(Norm16TextureFormats, 0x000003FB) @@ -322,6 +342,15 @@ ENUM(FeatureName) ENUM_ENTRY(MultiPlanarFormatP010, 0x000003FD) ENUM_ENTRY(HostMappedPointer, 0x000003FE) ENUM_ENTRY(MultiPlanarRenderTargets, 0x000003FF) + ENUM_ENTRY(MultiPlanarFormatNv12a, 0x00000400) + ENUM_ENTRY(FramebufferFetch, 0x00000401) + ENUM_ENTRY(BufferMapExtendedUsages, 0x00000402) + ENUM_ENTRY(AdapterPropertiesMemoryHeaps, 0x00000403) + ENUM_ENTRY(AdapterPropertiesD3D, 0x00000404) + ENUM_ENTRY(AdapterPropertiesVk, 0x00000405) + ENUM_ENTRY(R8UnormStorage, 0x00000406) + ENUM_ENTRY(FormatCapabilities, 0x00000407) + ENUM_ENTRY(DrmFormatCapabilities, 0x00000408) ENUM_ENTRY(SharedTextureMemoryVkDedicatedAllocation, 0x0000044C) ENUM_ENTRY(SharedTextureMemoryAHardwareBuffer, 0x0000044D) ENUM_ENTRY(SharedTextureMemoryDmaBuf, 0x0000044E) @@ -336,16 +365,20 @@ ENUM(FeatureName) ENUM_ENTRY(SharedFenceVkSemaphoreZirconHandle, 0x000004B2) ENUM_ENTRY(SharedFenceDXGISharedHandle, 0x000004B3) ENUM_ENTRY(SharedFenceMTLSharedEvent, 0x000004B4) + ENUM_ENTRY(SharedBufferMemoryD3D12Resource, 0x000004B5) + ENUM_ENTRY(StaticSamplers, 0x000004B6) ENUM_ENTRY(Force32, 0x7FFFFFFF) END ENUM(FilterMode) - ENUM_ENTRY(Nearest, 0x00000000) - ENUM_ENTRY(Linear, 0x00000001) + ENUM_ENTRY(Undefined, 0x00000000) + ENUM_ENTRY(Nearest, 0x00000001) + ENUM_ENTRY(Linear, 0x00000002) ENUM_ENTRY(Force32, 0x7FFFFFFF) END ENUM(FrontFace) - ENUM_ENTRY(CCW, 0x00000000) - ENUM_ENTRY(CW, 0x00000001) + ENUM_ENTRY(Undefined, 0x00000000) + ENUM_ENTRY(CCW, 0x00000001) + ENUM_ENTRY(CW, 0x00000002) ENUM_ENTRY(Force32, 0x7FFFFFFF) END ENUM(IndexFormat) @@ -361,15 +394,21 @@ ENUM(LoadOp) ENUM_ENTRY(Force32, 0x7FFFFFFF) END ENUM(LoggingType) - ENUM_ENTRY(Verbose, 0x00000000) - ENUM_ENTRY(Info, 0x00000001) - ENUM_ENTRY(Warning, 0x00000002) - ENUM_ENTRY(Error, 0x00000003) + ENUM_ENTRY(Verbose, 0x00000001) + ENUM_ENTRY(Info, 0x00000002) + ENUM_ENTRY(Warning, 0x00000003) + ENUM_ENTRY(Error, 0x00000004) ENUM_ENTRY(Force32, 0x7FFFFFFF) END ENUM(MipmapFilterMode) - ENUM_ENTRY(Nearest, 0x00000000) - ENUM_ENTRY(Linear, 0x00000001) + ENUM_ENTRY(Undefined, 0x00000000) + ENUM_ENTRY(Nearest, 0x00000001) + ENUM_ENTRY(Linear, 0x00000002) + ENUM_ENTRY(Force32, 0x7FFFFFFF) +END +ENUM(PopErrorScopeStatus) + ENUM_ENTRY(Success, 0x00000000) + ENUM_ENTRY(InstanceDropped, 0x00000001) ENUM_ENTRY(Force32, 0x7FFFFFFF) END ENUM(PowerPreference) @@ -379,42 +418,46 @@ ENUM(PowerPreference) ENUM_ENTRY(Force32, 0x7FFFFFFF) END ENUM(PresentMode) - ENUM_ENTRY(Fifo, 0x00000000) - ENUM_ENTRY(Immediate, 0x00000002) - ENUM_ENTRY(Mailbox, 0x00000003) + ENUM_ENTRY(Fifo, 0x00000001) + ENUM_ENTRY(Immediate, 0x00000003) + ENUM_ENTRY(Mailbox, 0x00000004) ENUM_ENTRY(Force32, 0x7FFFFFFF) END ENUM(PrimitiveTopology) - ENUM_ENTRY(PointList, 0x00000000) - ENUM_ENTRY(LineList, 0x00000001) - ENUM_ENTRY(LineStrip, 0x00000002) - ENUM_ENTRY(TriangleList, 0x00000003) - ENUM_ENTRY(TriangleStrip, 0x00000004) + ENUM_ENTRY(Undefined, 0x00000000) + ENUM_ENTRY(PointList, 0x00000001) + ENUM_ENTRY(LineList, 0x00000002) + ENUM_ENTRY(LineStrip, 0x00000003) + ENUM_ENTRY(TriangleList, 0x00000004) + ENUM_ENTRY(TriangleStrip, 0x00000005) ENUM_ENTRY(Force32, 0x7FFFFFFF) END ENUM(QueryType) - ENUM_ENTRY(Occlusion, 0x00000000) - ENUM_ENTRY(Timestamp, 0x00000001) + ENUM_ENTRY(Occlusion, 0x00000001) + ENUM_ENTRY(Timestamp, 0x00000002) ENUM_ENTRY(Force32, 0x7FFFFFFF) END ENUM(QueueWorkDoneStatus) ENUM_ENTRY(Success, 0x00000000) - ENUM_ENTRY(Error, 0x00000001) - ENUM_ENTRY(Unknown, 0x00000002) - ENUM_ENTRY(DeviceLost, 0x00000003) + ENUM_ENTRY(InstanceDropped, 0x00000001) + ENUM_ENTRY(Error, 0x00000002) + ENUM_ENTRY(Unknown, 0x00000003) + ENUM_ENTRY(DeviceLost, 0x00000004) ENUM_ENTRY(Force32, 0x7FFFFFFF) END ENUM(RequestAdapterStatus) ENUM_ENTRY(Success, 0x00000000) - ENUM_ENTRY(Unavailable, 0x00000001) - ENUM_ENTRY(Error, 0x00000002) - ENUM_ENTRY(Unknown, 0x00000003) + ENUM_ENTRY(InstanceDropped, 0x00000001) + ENUM_ENTRY(Unavailable, 0x00000002) + ENUM_ENTRY(Error, 0x00000003) + ENUM_ENTRY(Unknown, 0x00000004) ENUM_ENTRY(Force32, 0x7FFFFFFF) END ENUM(RequestDeviceStatus) ENUM_ENTRY(Success, 0x00000000) - ENUM_ENTRY(Error, 0x00000001) - ENUM_ENTRY(Unknown, 0x00000002) + ENUM_ENTRY(InstanceDropped, 0x00000001) + ENUM_ENTRY(Error, 0x00000002) + ENUM_ENTRY(Unknown, 0x00000003) ENUM_ENTRY(Force32, 0x7FFFFFFF) END ENUM(SType) @@ -434,6 +477,7 @@ ENUM(SType) ENUM_ENTRY(SurfaceDescriptorFromWindowsSwapChainPanel, 0x0000000E) ENUM_ENTRY(RenderPassDescriptorMaxDrawCount, 0x0000000F) ENUM_ENTRY(DepthStencilStateDepthWriteDefinedDawn, 0x00000010) + ENUM_ENTRY(TextureBindingViewDimensionDescriptor, 0x00000011) ENUM_ENTRY(DawnTextureInternalUsageDescriptor, 0x000003E8) ENUM_ENTRY(DawnEncoderInternalUsageDescriptor, 0x000003EB) ENUM_ENTRY(DawnInstanceDescriptor, 0x000003EC) @@ -451,6 +495,13 @@ ENUM(SType) ENUM_ENTRY(PipelineLayoutPixelLocalStorage, 0x000003F8) ENUM_ENTRY(BufferHostMappedPointer, 0x000003F9) ENUM_ENTRY(DawnExperimentalSubgroupLimits, 0x000003FA) + ENUM_ENTRY(AdapterPropertiesMemoryHeaps, 0x000003FB) + ENUM_ENTRY(AdapterPropertiesD3D, 0x000003FC) + ENUM_ENTRY(AdapterPropertiesVk, 0x000003FD) + ENUM_ENTRY(DawnComputePipelineFullSubgroups, 0x000003FE) + ENUM_ENTRY(DawnWireWGSLControl, 0x000003FF) + ENUM_ENTRY(DawnWGSLBlocklist, 0x00000400) + ENUM_ENTRY(DrmFormatCapabilities, 0x00000401) ENUM_ENTRY(SharedTextureMemoryVkImageDescriptor, 0x0000044C) ENUM_ENTRY(SharedTextureMemoryVkDedicatedAllocationDescriptor, 0x0000044D) ENUM_ENTRY(SharedTextureMemoryAHardwareBufferDescriptor, 0x0000044E) @@ -475,6 +526,7 @@ ENUM(SType) ENUM_ENTRY(SharedFenceDXGISharedHandleExportInfo, 0x000004BB) ENUM_ENTRY(SharedFenceMTLSharedEventDescriptor, 0x000004BC) ENUM_ENTRY(SharedFenceMTLSharedEventExportInfo, 0x000004BD) + ENUM_ENTRY(SharedBufferMemoryD3D12ResourceDescriptor, 0x000004BE) ENUM_ENTRY(Force32, 0x7FFFFFFF) END ENUM(SamplerBindingType) @@ -494,14 +546,15 @@ ENUM(SharedFenceType) ENUM_ENTRY(Force32, 0x7FFFFFFF) END ENUM(StencilOperation) - ENUM_ENTRY(Keep, 0x00000000) - ENUM_ENTRY(Zero, 0x00000001) - ENUM_ENTRY(Replace, 0x00000002) - ENUM_ENTRY(Invert, 0x00000003) - ENUM_ENTRY(IncrementClamp, 0x00000004) - ENUM_ENTRY(DecrementClamp, 0x00000005) - ENUM_ENTRY(IncrementWrap, 0x00000006) - ENUM_ENTRY(DecrementWrap, 0x00000007) + ENUM_ENTRY(Undefined, 0x00000000) + ENUM_ENTRY(Keep, 0x00000001) + ENUM_ENTRY(Zero, 0x00000002) + ENUM_ENTRY(Replace, 0x00000003) + ENUM_ENTRY(Invert, 0x00000004) + ENUM_ENTRY(IncrementClamp, 0x00000005) + ENUM_ENTRY(DecrementClamp, 0x00000006) + ENUM_ENTRY(IncrementWrap, 0x00000007) + ENUM_ENTRY(DecrementWrap, 0x00000008) ENUM_ENTRY(Force32, 0x7FFFFFFF) END ENUM(StorageTextureAccess) @@ -518,17 +571,20 @@ ENUM(StoreOp) ENUM_ENTRY(Force32, 0x7FFFFFFF) END ENUM(TextureAspect) - ENUM_ENTRY(All, 0x00000000) - ENUM_ENTRY(StencilOnly, 0x00000001) - ENUM_ENTRY(DepthOnly, 0x00000002) - ENUM_ENTRY(Plane0Only, 0x00000003) - ENUM_ENTRY(Plane1Only, 0x00000004) + ENUM_ENTRY(Undefined, 0x00000000) + ENUM_ENTRY(All, 0x00000001) + ENUM_ENTRY(StencilOnly, 0x00000002) + ENUM_ENTRY(DepthOnly, 0x00000003) + ENUM_ENTRY(Plane0Only, 0x00000004) + ENUM_ENTRY(Plane1Only, 0x00000005) + ENUM_ENTRY(Plane2Only, 0x00000006) ENUM_ENTRY(Force32, 0x7FFFFFFF) END ENUM(TextureDimension) - ENUM_ENTRY(_1D, 0x00000000) - ENUM_ENTRY(_2D, 0x00000001) - ENUM_ENTRY(_3D, 0x00000002) + ENUM_ENTRY(Undefined, 0x00000000) + ENUM_ENTRY(_1D, 0x00000001) + ENUM_ENTRY(_2D, 0x00000002) + ENUM_ENTRY(_3D, 0x00000003) ENUM_ENTRY(Force32, 0x7FFFFFFF) END ENUM(TextureFormat) @@ -636,6 +692,7 @@ ENUM(TextureFormat) ENUM_ENTRY(RGBA16Snorm, 0x00000065) ENUM_ENTRY(R8BG8Biplanar420Unorm, 0x00000066) ENUM_ENTRY(R10X6BG10X6Biplanar420Unorm, 0x00000067) + ENUM_ENTRY(R8BG8A8Triplanar420Unorm, 0x00000068) ENUM_ENTRY(Force32, 0x7FFFFFFF) END ENUM(TextureSampleType) @@ -693,9 +750,10 @@ ENUM(VertexFormat) ENUM_ENTRY(Force32, 0x7FFFFFFF) END ENUM(VertexStepMode) - ENUM_ENTRY(Vertex, 0x00000000) - ENUM_ENTRY(Instance, 0x00000001) - ENUM_ENTRY(VertexBufferNotUsed, 0x00000002) + ENUM_ENTRY(Undefined, 0x00000000) + ENUM_ENTRY(VertexBufferNotUsed, 0x00000001) + ENUM_ENTRY(Vertex, 0x00000002) + ENUM_ENTRY(Instance, 0x00000003) ENUM_ENTRY(Force32, 0x7FFFFFFF) END ENUM(WaitStatus) @@ -730,6 +788,15 @@ ENUM(ColorWriteMask) ENUM_ENTRY(All, 0x0000000F) ENUM_ENTRY(Force32, 0x7FFFFFFF) END +ENUM(HeapProperty) + ENUM_ENTRY(Undefined, 0x00000000) + ENUM_ENTRY(DeviceLocal, 0x00000001) + ENUM_ENTRY(HostVisible, 0x00000002) + ENUM_ENTRY(HostCoherent, 0x00000004) + ENUM_ENTRY(HostUncached, 0x00000008) + ENUM_ENTRY(HostCached, 0x00000010) + ENUM_ENTRY(Force32, 0x7FFFFFFF) +END ENUM(MapMode) ENUM_ENTRY(None, 0x00000000) ENUM_ENTRY(Read, 0x00000001) @@ -764,6 +831,14 @@ STRUCT(ChainedStructOut) void setDefault(); END +STRUCT(AdapterPropertiesD3D) + void setDefault(); +END + +STRUCT(AdapterPropertiesVk) + void setDefault(); +END + STRUCT(BlendComponent) void setDefault(); END @@ -781,6 +856,10 @@ STRUCT(ComputePassTimestampWrites) void setDefault(); END +STRUCT(DawnWGSLBlocklist) + void setDefault(); +END + STRUCT(DawnAdapterPropertiesPowerPreference) void setDefault(); END @@ -793,6 +872,10 @@ STRUCT(DawnCacheDeviceDescriptor) void setDefault(); END +STRUCT(DawnComputePipelineFullSubgroups) + void setDefault(); +END + STRUCT(DawnEncoderInternalUsageDescriptor) void setDefault(); END @@ -821,10 +904,18 @@ STRUCT(DawnTogglesDescriptor) void setDefault(); END +STRUCT(DawnWireWGSLControl) + void setDefault(); +END + STRUCT(DepthStencilStateDepthWriteDefinedDawn) void setDefault(); END +STRUCT(DrmFormatProperties) + void setDefault(); +END + STRUCT(Extent2D) void setDefault(); END @@ -850,6 +941,10 @@ STRUCT(Limits) void setDefault(); END +STRUCT(MemoryHeapInfo) + void setDefault(); +END + STRUCT(Origin2D) void setDefault(); END @@ -923,23 +1018,23 @@ STRUCT(SharedFenceVkSemaphoreZirconHandleExportInfo) void setDefault(); END -STRUCT(SharedTextureMemoryAHardwareBufferDescriptor) +STRUCT(SharedTextureMemoryDXGISharedHandleDescriptor) void setDefault(); END -STRUCT(SharedTextureMemoryDmaBufDescriptor) +STRUCT(SharedTextureMemoryEGLImageDescriptor) void setDefault(); END -STRUCT(SharedTextureMemoryDXGISharedHandleDescriptor) +STRUCT(SharedTextureMemoryIOSurfaceDescriptor) void setDefault(); END -STRUCT(SharedTextureMemoryEGLImageDescriptor) +STRUCT(SharedTextureMemoryAHardwareBufferDescriptor) void setDefault(); END -STRUCT(SharedTextureMemoryIOSurfaceDescriptor) +STRUCT(SharedTextureMemoryDmaBufPlane) void setDefault(); END @@ -983,11 +1078,11 @@ STRUCT(SurfaceDescriptorFromWaylandSurface) void setDefault(); END -STRUCT(SurfaceDescriptorFromWindowsCoreWindow) +STRUCT(SurfaceDescriptorFromWindowsHWND) void setDefault(); END -STRUCT(SurfaceDescriptorFromWindowsHWND) +STRUCT(SurfaceDescriptorFromWindowsCoreWindow) void setDefault(); END @@ -999,14 +1094,28 @@ STRUCT(SurfaceDescriptorFromXlibWindow) void setDefault(); END +STRUCT(TextureBindingViewDimensionDescriptor) + void setDefault(); +END + STRUCT(VertexAttribute) void setDefault(); END +STRUCT(AdapterPropertiesMemoryHeaps) + void setDefault(); + void freeMembers(); +END + STRUCT(BlendState) void setDefault(); END +STRUCT(DrmFormatCapabilities) + void setDefault(); + void freeMembers(); +END + STRUCT(FutureWaitInfo) void setDefault(); END @@ -1015,6 +1124,10 @@ STRUCT(PipelineLayoutPixelLocalStorage) void setDefault(); END +STRUCT(SharedTextureMemoryDmaBufDescriptor) + void setDefault(); +END + STRUCT(SharedTextureMemoryVkImageDescriptor) void setDefault(); END @@ -1058,6 +1171,10 @@ DESCRIPTOR(CommandEncoderDescriptor) void setDefault(); END +DESCRIPTOR(CompilationInfoCallbackInfo) + void setDefault(); +END + DESCRIPTOR(CompilationMessage) void setDefault(); END @@ -1070,6 +1187,18 @@ DESCRIPTOR(CopyTextureForBrowserOptions) void setDefault(); END +DESCRIPTOR(CreateComputePipelineAsyncCallbackInfo) + void setDefault(); +END + +DESCRIPTOR(CreateRenderPipelineAsyncCallbackInfo) + void setDefault(); +END + +DESCRIPTOR(FormatCapabilities) + void setDefault(); +END + DESCRIPTOR(InstanceFeatures) void setDefault(); END @@ -1086,6 +1215,10 @@ DESCRIPTOR(PipelineLayoutStorageAttachment) void setDefault(); END +DESCRIPTOR(PopErrorScopeCallbackInfo) + void setDefault(); +END + DESCRIPTOR(PrimitiveState) void setDefault(); END @@ -1110,10 +1243,18 @@ DESCRIPTOR(RenderBundleEncoderDescriptor) void setDefault(); END +DESCRIPTOR(RequestAdapterCallbackInfo) + void setDefault(); +END + DESCRIPTOR(RequestAdapterOptions) void setDefault(); END +DESCRIPTOR(RequestDeviceCallbackInfo) + void setDefault(); +END + DESCRIPTOR(SamplerBindingLayout) void setDefault(); END @@ -1126,6 +1267,23 @@ DESCRIPTOR(ShaderModuleDescriptor) void setDefault(); END +DESCRIPTOR(SharedBufferMemoryBeginAccessDescriptor) + void setDefault(); +END + +DESCRIPTOR(SharedBufferMemoryDescriptor) + void setDefault(); +END + +DESCRIPTOR(SharedBufferMemoryEndAccessState) + void setDefault(); + void freeMembers(); +END + +DESCRIPTOR(SharedBufferMemoryProperties) + void setDefault(); +END + DESCRIPTOR(SharedFenceDescriptor) void setDefault(); END @@ -1293,6 +1451,7 @@ class RenderPassEncoder; class RenderPipeline; class Sampler; class ShaderModule; +class SharedBufferMemory; class SharedFence; class SharedTextureMemory; class Surface; @@ -1308,6 +1467,7 @@ using CreateRenderPipelineAsyncCallback = std::function; using ErrorCallback = std::function; using LoggingCallback = std::function; +using PopErrorScopeCallback = std::function; using QueueWorkDoneCallback = std::function; using RequestAdapterCallback = std::function; using RequestDeviceCallback = std::function; @@ -1320,11 +1480,13 @@ HANDLE(Adapter) Device createDevice(const DeviceDescriptor& descriptor); Device createDevice(); size_t enumerateFeatures(FeatureName * features); + Bool getFormatCapabilities(TextureFormat format, FormatCapabilities * capabilities); Instance getInstance(); Bool getLimits(SupportedLimits * limits); void getProperties(AdapterProperties * properties); Bool hasFeature(FeatureName feature); std::unique_ptr requestDevice(const DeviceDescriptor& descriptor, RequestDeviceCallback&& callback); + Future requestDeviceF(const DeviceDescriptor& options, RequestDeviceCallbackInfo callbackInfo); void reference(); void release(); Device requestDevice(const DeviceDescriptor& descriptor); @@ -1350,6 +1512,7 @@ HANDLE(Buffer) uint64_t getSize(); BufferUsageFlags getUsage(); std::unique_ptr mapAsync(MapModeFlags mode, size_t offset, size_t size, BufferMapCallback&& callback); + Future mapAsyncF(MapModeFlags mode, size_t offset, size_t size, BufferMapCallbackInfo callbackInfo); void setLabel(char const * label); void unmap(); void reference(); @@ -1417,6 +1580,7 @@ HANDLE(Device) CommandEncoder createCommandEncoder(); ComputePipeline createComputePipeline(const ComputePipelineDescriptor& descriptor); std::unique_ptr createComputePipelineAsync(const ComputePipelineDescriptor& descriptor, CreateComputePipelineAsyncCallback&& callback); + Future createComputePipelineAsyncF(const ComputePipelineDescriptor& descriptor, CreateComputePipelineAsyncCallbackInfo callbackInfo); Buffer createErrorBuffer(const BufferDescriptor& descriptor); ExternalTexture createErrorExternalTexture(); ShaderModule createErrorShaderModule(const ShaderModuleDescriptor& descriptor, char const * errorMessage); @@ -1427,6 +1591,7 @@ HANDLE(Device) RenderBundleEncoder createRenderBundleEncoder(const RenderBundleEncoderDescriptor& descriptor); RenderPipeline createRenderPipeline(const RenderPipelineDescriptor& descriptor); std::unique_ptr createRenderPipelineAsync(const RenderPipelineDescriptor& descriptor, CreateRenderPipelineAsyncCallback&& callback); + Future createRenderPipelineAsyncF(const RenderPipelineDescriptor& descriptor, CreateRenderPipelineAsyncCallbackInfo callbackInfo); Sampler createSampler(const SamplerDescriptor& descriptor); Sampler createSampler(); ShaderModule createShaderModule(const ShaderModuleDescriptor& descriptor); @@ -1440,10 +1605,11 @@ HANDLE(Device) Queue getQueue(); TextureUsageFlags getSupportedSurfaceUsage(Surface surface); Bool hasFeature(FeatureName feature); + SharedBufferMemory importSharedBufferMemory(const SharedBufferMemoryDescriptor& descriptor); SharedFence importSharedFence(const SharedFenceDescriptor& descriptor); SharedTextureMemory importSharedTextureMemory(const SharedTextureMemoryDescriptor& descriptor); void injectError(ErrorType type, char const * message); - std::unique_ptr popErrorScope(ErrorCallback&& callback); + Future popErrorScopeF(PopErrorScopeCallbackInfo callbackInfo); void pushErrorScope(ErrorFilter filter); std::unique_ptr setDeviceLostCallback(DeviceLostCallback&& callback); void setLabel(char const * label); @@ -1466,8 +1632,11 @@ END HANDLE(Instance) Surface createSurface(const SurfaceDescriptor& descriptor); + size_t enumerateWGSLLanguageFeatures(WGSLFeatureName * features); + Bool hasWGSLLanguageFeature(WGSLFeatureName feature); void processEvents(); std::unique_ptr requestAdapter(const RequestAdapterOptions& options, RequestAdapterCallback&& callback); + Future requestAdapterF(const RequestAdapterOptions& options, RequestAdapterCallbackInfo callbackInfo); WaitStatus waitAny(size_t futureCount, FutureWaitInfo * futures, uint64_t timeoutNS); void reference(); void release(); @@ -1577,6 +1746,19 @@ END HANDLE(ShaderModule) std::unique_ptr getCompilationInfo(CompilationInfoCallback&& callback); + Future getCompilationInfoF(CompilationInfoCallbackInfo callbackInfo); + void setLabel(char const * label); + void reference(); + void release(); +END + +HANDLE(SharedBufferMemory) + Bool beginAccess(Buffer buffer, const SharedBufferMemoryBeginAccessDescriptor& descriptor); + Buffer createBuffer(const BufferDescriptor& descriptor); + Buffer createBuffer(); + Bool endAccess(Buffer buffer, SharedBufferMemoryEndAccessState * descriptor); + void getProperties(SharedBufferMemoryProperties * properties); + Bool isDeviceLost(); void setLabel(char const * label); void reference(); void release(); @@ -1594,12 +1776,14 @@ HANDLE(SharedTextureMemory) Texture createTexture(); Bool endAccess(Texture texture, SharedTextureMemoryEndAccessState * descriptor); void getProperties(SharedTextureMemoryProperties * properties); + Bool isDeviceLost(); void setLabel(char const * label); void reference(); void release(); END HANDLE(Surface) + TextureFormat getPreferredFormat(Adapter adapter); void reference(); void release(); END @@ -1613,6 +1797,8 @@ HANDLE(SwapChain) END HANDLE(Texture) + TextureView createErrorView(const TextureViewDescriptor& descriptor); + TextureView createErrorView(); TextureView createView(const TextureViewDescriptor& descriptor); TextureView createView(); void destroy(); @@ -1667,6 +1853,20 @@ void AdapterProperties::freeMembers() { } +// Methods of AdapterPropertiesD3D +void AdapterPropertiesD3D::setDefault() { + ((ChainedStructOut*)&chain)->setDefault(); + chain.sType = SType::AdapterPropertiesD3D; +} + + +// Methods of AdapterPropertiesVk +void AdapterPropertiesVk::setDefault() { + ((ChainedStructOut*)&chain)->setDefault(); + chain.sType = SType::AdapterPropertiesVk; +} + + // Methods of BindGroupEntry void BindGroupEntry::setDefault() { offset = 0; @@ -1722,6 +1922,11 @@ void CommandEncoderDescriptor::setDefault() { } +// Methods of CompilationInfoCallbackInfo +void CompilationInfoCallbackInfo::setDefault() { +} + + // Methods of CompilationMessage void CompilationMessage::setDefault() { } @@ -1742,6 +1947,23 @@ void CopyTextureForBrowserOptions::setDefault() { } +// Methods of CreateComputePipelineAsyncCallbackInfo +void CreateComputePipelineAsyncCallbackInfo::setDefault() { +} + + +// Methods of CreateRenderPipelineAsyncCallbackInfo +void CreateRenderPipelineAsyncCallbackInfo::setDefault() { +} + + +// Methods of DawnWGSLBlocklist +void DawnWGSLBlocklist::setDefault() { + ((ChainedStruct*)&chain)->setDefault(); + chain.sType = SType::DawnWGSLBlocklist; +} + + // Methods of DawnAdapterPropertiesPowerPreference void DawnAdapterPropertiesPowerPreference::setDefault() { powerPreference = PowerPreference::Undefined; @@ -1764,6 +1986,13 @@ void DawnCacheDeviceDescriptor::setDefault() { } +// Methods of DawnComputePipelineFullSubgroups +void DawnComputePipelineFullSubgroups::setDefault() { + ((ChainedStruct*)&chain)->setDefault(); + chain.sType = SType::DawnComputePipelineFullSubgroups; +} + + // Methods of DawnEncoderInternalUsageDescriptor void DawnEncoderInternalUsageDescriptor::setDefault() { ((ChainedStruct*)&chain)->setDefault(); @@ -1813,6 +2042,13 @@ void DawnTogglesDescriptor::setDefault() { } +// Methods of DawnWireWGSLControl +void DawnWireWGSLControl::setDefault() { + ((ChainedStruct*)&chain)->setDefault(); + chain.sType = SType::DawnWireWGSLControl; +} + + // Methods of DepthStencilStateDepthWriteDefinedDawn void DepthStencilStateDepthWriteDefinedDawn::setDefault() { ((ChainedStruct*)&chain)->setDefault(); @@ -1820,6 +2056,11 @@ void DepthStencilStateDepthWriteDefinedDawn::setDefault() { } +// Methods of DrmFormatProperties +void DrmFormatProperties::setDefault() { +} + + // Methods of Extent2D void Extent2D::setDefault() { } @@ -1846,6 +2087,11 @@ void ExternalTextureBindingLayout::setDefault() { } +// Methods of FormatCapabilities +void FormatCapabilities::setDefault() { +} + + // Methods of Future void Future::setDefault() { } @@ -1892,6 +2138,11 @@ void Limits::setDefault() { } +// Methods of MemoryHeapInfo +void MemoryHeapInfo::setDefault() { +} + + // Methods of MultisampleState void MultisampleState::setDefault() { count = 1; @@ -1924,6 +2175,11 @@ void PipelineLayoutStorageAttachment::setDefault() { } +// Methods of PopErrorScopeCallbackInfo +void PopErrorScopeCallbackInfo::setDefault() { +} + + // Methods of PrimitiveDepthClipControl void PrimitiveDepthClipControl::setDefault() { unclippedDepth = false; @@ -1994,6 +2250,11 @@ void RenderPassTimestampWrites::setDefault() { } +// Methods of RequestAdapterCallbackInfo +void RequestAdapterCallbackInfo::setDefault() { +} + + // Methods of RequestAdapterOptions void RequestAdapterOptions::setDefault() { powerPreference = PowerPreference::Undefined; @@ -2002,6 +2263,11 @@ void RequestAdapterOptions::setDefault() { } +// Methods of RequestDeviceCallbackInfo +void RequestDeviceCallbackInfo::setDefault() { +} + + // Methods of SamplerBindingLayout void SamplerBindingLayout::setDefault() { type = SamplerBindingType::Filtering; @@ -2022,11 +2288,6 @@ void SamplerDescriptor::setDefault() { } -// Methods of ShaderModuleDescriptor -void ShaderModuleDescriptor::setDefault() { -} - - // Methods of ShaderModuleSPIRVDescriptor void ShaderModuleSPIRVDescriptor::setDefault() { ((ChainedStruct*)&chain)->setDefault(); @@ -2041,8 +2302,31 @@ void ShaderModuleWGSLDescriptor::setDefault() { } -// Methods of SharedFenceDescriptor -void SharedFenceDescriptor::setDefault() { +// Methods of ShaderModuleDescriptor +void ShaderModuleDescriptor::setDefault() { +} + + +// Methods of SharedBufferMemoryBeginAccessDescriptor +void SharedBufferMemoryBeginAccessDescriptor::setDefault() { +} + + +// Methods of SharedBufferMemoryDescriptor +void SharedBufferMemoryDescriptor::setDefault() { +} + + +// Methods of SharedBufferMemoryEndAccessState +void SharedBufferMemoryEndAccessState::setDefault() { +} +void SharedBufferMemoryEndAccessState::freeMembers() { + return wgpuSharedBufferMemoryEndAccessStateFreeMembers(*this); +} + + +// Methods of SharedBufferMemoryProperties +void SharedBufferMemoryProperties::setDefault() { } @@ -2060,12 +2344,6 @@ void SharedFenceDXGISharedHandleExportInfo::setDefault() { } -// Methods of SharedFenceExportInfo -void SharedFenceExportInfo::setDefault() { - type = SharedFenceType::Undefined; -} - - // Methods of SharedFenceMTLSharedEventDescriptor void SharedFenceMTLSharedEventDescriptor::setDefault() { ((ChainedStruct*)&chain)->setDefault(); @@ -2080,6 +2358,17 @@ void SharedFenceMTLSharedEventExportInfo::setDefault() { } +// Methods of SharedFenceDescriptor +void SharedFenceDescriptor::setDefault() { +} + + +// Methods of SharedFenceExportInfo +void SharedFenceExportInfo::setDefault() { + type = SharedFenceType::Undefined; +} + + // Methods of SharedFenceVkSemaphoreOpaqueFDDescriptor void SharedFenceVkSemaphoreOpaqueFDDescriptor::setDefault() { ((ChainedStruct*)&chain)->setDefault(); @@ -2122,41 +2411,46 @@ void SharedFenceVkSemaphoreZirconHandleExportInfo::setDefault() { } -// Methods of SharedTextureMemoryAHardwareBufferDescriptor -void SharedTextureMemoryAHardwareBufferDescriptor::setDefault() { +// Methods of SharedTextureMemoryDXGISharedHandleDescriptor +void SharedTextureMemoryDXGISharedHandleDescriptor::setDefault() { ((ChainedStruct*)&chain)->setDefault(); - chain.sType = SType::SharedTextureMemoryAHardwareBufferDescriptor; + chain.sType = SType::SharedTextureMemoryDXGISharedHandleDescriptor; } -// Methods of SharedTextureMemoryBeginAccessDescriptor -void SharedTextureMemoryBeginAccessDescriptor::setDefault() { +// Methods of SharedTextureMemoryEGLImageDescriptor +void SharedTextureMemoryEGLImageDescriptor::setDefault() { + ((ChainedStruct*)&chain)->setDefault(); + chain.sType = SType::SharedTextureMemoryEGLImageDescriptor; } -// Methods of SharedTextureMemoryDescriptor -void SharedTextureMemoryDescriptor::setDefault() { +// Methods of SharedTextureMemoryIOSurfaceDescriptor +void SharedTextureMemoryIOSurfaceDescriptor::setDefault() { + ((ChainedStruct*)&chain)->setDefault(); + chain.sType = SType::SharedTextureMemoryIOSurfaceDescriptor; } -// Methods of SharedTextureMemoryDmaBufDescriptor -void SharedTextureMemoryDmaBufDescriptor::setDefault() { +// Methods of SharedTextureMemoryAHardwareBufferDescriptor +void SharedTextureMemoryAHardwareBufferDescriptor::setDefault() { ((ChainedStruct*)&chain)->setDefault(); - chain.sType = SType::SharedTextureMemoryDmaBufDescriptor; + chain.sType = SType::SharedTextureMemoryAHardwareBufferDescriptor; } -// Methods of SharedTextureMemoryDXGISharedHandleDescriptor -void SharedTextureMemoryDXGISharedHandleDescriptor::setDefault() { - ((ChainedStruct*)&chain)->setDefault(); - chain.sType = SType::SharedTextureMemoryDXGISharedHandleDescriptor; +// Methods of SharedTextureMemoryBeginAccessDescriptor +void SharedTextureMemoryBeginAccessDescriptor::setDefault() { } -// Methods of SharedTextureMemoryEGLImageDescriptor -void SharedTextureMemoryEGLImageDescriptor::setDefault() { - ((ChainedStruct*)&chain)->setDefault(); - chain.sType = SType::SharedTextureMemoryEGLImageDescriptor; +// Methods of SharedTextureMemoryDescriptor +void SharedTextureMemoryDescriptor::setDefault() { +} + + +// Methods of SharedTextureMemoryDmaBufPlane +void SharedTextureMemoryDmaBufPlane::setDefault() { } @@ -2168,13 +2462,6 @@ void SharedTextureMemoryEndAccessState::freeMembers() { } -// Methods of SharedTextureMemoryIOSurfaceDescriptor -void SharedTextureMemoryIOSurfaceDescriptor::setDefault() { - ((ChainedStruct*)&chain)->setDefault(); - chain.sType = SType::SharedTextureMemoryIOSurfaceDescriptor; -} - - // Methods of SharedTextureMemoryOpaqueFDDescriptor void SharedTextureMemoryOpaqueFDDescriptor::setDefault() { ((ChainedStruct*)&chain)->setDefault(); @@ -2260,17 +2547,17 @@ void SurfaceDescriptorFromWaylandSurface::setDefault() { } -// Methods of SurfaceDescriptorFromWindowsCoreWindow -void SurfaceDescriptorFromWindowsCoreWindow::setDefault() { +// Methods of SurfaceDescriptorFromWindowsHWND +void SurfaceDescriptorFromWindowsHWND::setDefault() { ((ChainedStruct*)&chain)->setDefault(); - chain.sType = SType::SurfaceDescriptorFromWindowsCoreWindow; + chain.sType = SType::SurfaceDescriptorFromWindowsHWND; } -// Methods of SurfaceDescriptorFromWindowsHWND -void SurfaceDescriptorFromWindowsHWND::setDefault() { +// Methods of SurfaceDescriptorFromWindowsCoreWindow +void SurfaceDescriptorFromWindowsCoreWindow::setDefault() { ((ChainedStruct*)&chain)->setDefault(); - chain.sType = SType::SurfaceDescriptorFromWindowsHWND; + chain.sType = SType::SurfaceDescriptorFromWindowsCoreWindow; } @@ -2302,6 +2589,14 @@ void TextureBindingLayout::setDefault() { } +// Methods of TextureBindingViewDimensionDescriptor +void TextureBindingViewDimensionDescriptor::setDefault() { + textureBindingViewDimension = TextureViewDimension::Undefined; + ((ChainedStruct*)&chain)->setDefault(); + chain.sType = SType::TextureBindingViewDimensionDescriptor; +} + + // Methods of TextureDataLayout void TextureDataLayout::setDefault() { } @@ -2323,6 +2618,16 @@ void VertexAttribute::setDefault() { } +// Methods of AdapterPropertiesMemoryHeaps +void AdapterPropertiesMemoryHeaps::setDefault() { + ((ChainedStructOut*)&chain)->setDefault(); + chain.sType = SType::AdapterPropertiesMemoryHeaps; +} +void AdapterPropertiesMemoryHeaps::freeMembers() { + return wgpuAdapterPropertiesMemoryHeapsFreeMembers(*this); +} + + // Methods of BindGroupDescriptor void BindGroupDescriptor::setDefault() { } @@ -2372,6 +2677,16 @@ void DepthStencilState::setDefault() { } +// Methods of DrmFormatCapabilities +void DrmFormatCapabilities::setDefault() { + ((ChainedStructOut*)&chain)->setDefault(); + chain.sType = SType::DrmFormatCapabilities; +} +void DrmFormatCapabilities::freeMembers() { + return wgpuDrmFormatCapabilitiesFreeMembers(*this); +} + + // Methods of ExternalTextureDescriptor void ExternalTextureDescriptor::setDefault() { ((Origin2D*)&visibleOrigin)->setDefault(); @@ -2446,6 +2761,14 @@ void RequiredLimits::setDefault() { } +// Methods of SharedTextureMemoryDmaBufDescriptor +void SharedTextureMemoryDmaBufDescriptor::setDefault() { + ((ChainedStruct*)&chain)->setDefault(); + ((Extent3D*)&size)->setDefault(); + chain.sType = SType::SharedTextureMemoryDmaBufDescriptor; +} + + // Methods of SharedTextureMemoryProperties void SharedTextureMemoryProperties::setDefault() { format = TextureFormat::Undefined; @@ -2546,6 +2869,9 @@ Device Adapter::createDevice() { size_t Adapter::enumerateFeatures(FeatureName * features) { return wgpuAdapterEnumerateFeatures(m_raw, reinterpret_cast(features)); } +Bool Adapter::getFormatCapabilities(TextureFormat format, FormatCapabilities * capabilities) { + return wgpuAdapterGetFormatCapabilities(m_raw, static_cast(format), capabilities); +} Instance Adapter::getInstance() { return wgpuAdapterGetInstance(m_raw); } @@ -2567,6 +2893,9 @@ std::unique_ptr Adapter::requestDevice(const DeviceDescri wgpuAdapterRequestDevice(m_raw, &descriptor, cCallback, reinterpret_cast(handle.get())); return handle; } +Future Adapter::requestDeviceF(const DeviceDescriptor& options, RequestDeviceCallbackInfo callbackInfo) { + return wgpuAdapterRequestDeviceF(m_raw, &options, callbackInfo); +} void Adapter::reference() { return wgpuAdapterReference(m_raw); } @@ -2627,6 +2956,9 @@ std::unique_ptr Buffer::mapAsync(MapModeFlags mode, size_t of wgpuBufferMapAsync(m_raw, mode, offset, size, cCallback, reinterpret_cast(handle.get())); return handle; } +Future Buffer::mapAsyncF(MapModeFlags mode, size_t offset, size_t size, BufferMapCallbackInfo callbackInfo) { + return wgpuBufferMapAsyncF(m_raw, mode, offset, size, callbackInfo); +} void Buffer::setLabel(char const * label) { return wgpuBufferSetLabel(m_raw, label); } @@ -2804,6 +3136,9 @@ std::unique_ptr Device::createComputePipelin wgpuDeviceCreateComputePipelineAsync(m_raw, &descriptor, cCallback, reinterpret_cast(handle.get())); return handle; } +Future Device::createComputePipelineAsyncF(const ComputePipelineDescriptor& descriptor, CreateComputePipelineAsyncCallbackInfo callbackInfo) { + return wgpuDeviceCreateComputePipelineAsyncF(m_raw, &descriptor, callbackInfo); +} Buffer Device::createErrorBuffer(const BufferDescriptor& descriptor) { return wgpuDeviceCreateErrorBuffer(m_raw, &descriptor); } @@ -2840,6 +3175,9 @@ std::unique_ptr Device::createRenderPipelineA wgpuDeviceCreateRenderPipelineAsync(m_raw, &descriptor, cCallback, reinterpret_cast(handle.get())); return handle; } +Future Device::createRenderPipelineAsyncF(const RenderPipelineDescriptor& descriptor, CreateRenderPipelineAsyncCallbackInfo callbackInfo) { + return wgpuDeviceCreateRenderPipelineAsyncF(m_raw, &descriptor, callbackInfo); +} Sampler Device::createSampler(const SamplerDescriptor& descriptor) { return wgpuDeviceCreateSampler(m_raw, &descriptor); } @@ -2879,6 +3217,9 @@ TextureUsageFlags Device::getSupportedSurfaceUsage(Surface surface) { Bool Device::hasFeature(FeatureName feature) { return wgpuDeviceHasFeature(m_raw, static_cast(feature)); } +SharedBufferMemory Device::importSharedBufferMemory(const SharedBufferMemoryDescriptor& descriptor) { + return wgpuDeviceImportSharedBufferMemory(m_raw, &descriptor); +} SharedFence Device::importSharedFence(const SharedFenceDescriptor& descriptor) { return wgpuDeviceImportSharedFence(m_raw, &descriptor); } @@ -2888,14 +3229,8 @@ SharedTextureMemory Device::importSharedTextureMemory(const SharedTextureMemoryD void Device::injectError(ErrorType type, char const * message) { return wgpuDeviceInjectError(m_raw, static_cast(type), message); } -std::unique_ptr Device::popErrorScope(ErrorCallback&& callback) { - auto handle = std::make_unique(callback); - static auto cCallback = [](WGPUErrorType type, char const * message, void * userdata) -> void { - ErrorCallback& callback = *reinterpret_cast(userdata); - callback(static_cast(type), message); - }; - wgpuDevicePopErrorScope(m_raw, cCallback, reinterpret_cast(handle.get())); - return handle; +Future Device::popErrorScopeF(PopErrorScopeCallbackInfo callbackInfo) { + return wgpuDevicePopErrorScopeF(m_raw, callbackInfo); } void Device::pushErrorScope(ErrorFilter filter) { return wgpuDevicePushErrorScope(m_raw, static_cast(filter)); @@ -2969,6 +3304,12 @@ void ExternalTexture::release() { Surface Instance::createSurface(const SurfaceDescriptor& descriptor) { return wgpuInstanceCreateSurface(m_raw, &descriptor); } +size_t Instance::enumerateWGSLLanguageFeatures(WGSLFeatureName * features) { + return wgpuInstanceEnumerateWGSLLanguageFeatures(m_raw, reinterpret_cast(features)); +} +Bool Instance::hasWGSLLanguageFeature(WGSLFeatureName feature) { + return wgpuInstanceHasWGSLLanguageFeature(m_raw, static_cast(feature)); +} void Instance::processEvents() { return wgpuInstanceProcessEvents(m_raw); } @@ -2981,6 +3322,9 @@ std::unique_ptr Instance::requestAdapter(const RequestAd wgpuInstanceRequestAdapter(m_raw, &options, cCallback, reinterpret_cast(handle.get())); return handle; } +Future Instance::requestAdapterF(const RequestAdapterOptions& options, RequestAdapterCallbackInfo callbackInfo) { + return wgpuInstanceRequestAdapterF(m_raw, &options, callbackInfo); +} WaitStatus Instance::waitAny(size_t futureCount, FutureWaitInfo * futures, uint64_t timeoutNS) { return static_cast(wgpuInstanceWaitAny(m_raw, futureCount, futures, timeoutNS)); } @@ -3263,6 +3607,9 @@ std::unique_ptr ShaderModule::getCompilationInfo(Compil wgpuShaderModuleGetCompilationInfo(m_raw, cCallback, reinterpret_cast(handle.get())); return handle; } +Future ShaderModule::getCompilationInfoF(CompilationInfoCallbackInfo callbackInfo) { + return wgpuShaderModuleGetCompilationInfoF(m_raw, callbackInfo); +} void ShaderModule::setLabel(char const * label) { return wgpuShaderModuleSetLabel(m_raw, label); } @@ -3274,6 +3621,36 @@ void ShaderModule::release() { } +// Methods of SharedBufferMemory +Bool SharedBufferMemory::beginAccess(Buffer buffer, const SharedBufferMemoryBeginAccessDescriptor& descriptor) { + return wgpuSharedBufferMemoryBeginAccess(m_raw, buffer, &descriptor); +} +Buffer SharedBufferMemory::createBuffer(const BufferDescriptor& descriptor) { + return wgpuSharedBufferMemoryCreateBuffer(m_raw, &descriptor); +} +Buffer SharedBufferMemory::createBuffer() { + return wgpuSharedBufferMemoryCreateBuffer(m_raw, nullptr); +} +Bool SharedBufferMemory::endAccess(Buffer buffer, SharedBufferMemoryEndAccessState * descriptor) { + return wgpuSharedBufferMemoryEndAccess(m_raw, buffer, descriptor); +} +void SharedBufferMemory::getProperties(SharedBufferMemoryProperties * properties) { + return wgpuSharedBufferMemoryGetProperties(m_raw, properties); +} +Bool SharedBufferMemory::isDeviceLost() { + return wgpuSharedBufferMemoryIsDeviceLost(m_raw); +} +void SharedBufferMemory::setLabel(char const * label) { + return wgpuSharedBufferMemorySetLabel(m_raw, label); +} +void SharedBufferMemory::reference() { + return wgpuSharedBufferMemoryReference(m_raw); +} +void SharedBufferMemory::release() { + return wgpuSharedBufferMemoryRelease(m_raw); +} + + // Methods of SharedFence void SharedFence::exportInfo(SharedFenceExportInfo * info) { return wgpuSharedFenceExportInfo(m_raw, info); @@ -3302,6 +3679,9 @@ Bool SharedTextureMemory::endAccess(Texture texture, SharedTextureMemoryEndAcces void SharedTextureMemory::getProperties(SharedTextureMemoryProperties * properties) { return wgpuSharedTextureMemoryGetProperties(m_raw, properties); } +Bool SharedTextureMemory::isDeviceLost() { + return wgpuSharedTextureMemoryIsDeviceLost(m_raw); +} void SharedTextureMemory::setLabel(char const * label) { return wgpuSharedTextureMemorySetLabel(m_raw, label); } @@ -3314,6 +3694,9 @@ void SharedTextureMemory::release() { // Methods of Surface +TextureFormat Surface::getPreferredFormat(Adapter adapter) { + return static_cast(wgpuSurfaceGetPreferredFormat(m_raw, adapter)); +} void Surface::reference() { return wgpuSurfaceReference(m_raw); } @@ -3341,6 +3724,12 @@ void SwapChain::release() { // Methods of Texture +TextureView Texture::createErrorView(const TextureViewDescriptor& descriptor) { + return wgpuTextureCreateErrorView(m_raw, &descriptor); +} +TextureView Texture::createErrorView() { + return wgpuTextureCreateErrorView(m_raw, nullptr); +} TextureView Texture::createView(const TextureViewDescriptor& descriptor) { return wgpuTextureCreateView(m_raw, &descriptor); } diff --git a/launch/autonomy.launch b/launch/autonomy.launch index 2312591c3..bbfc5b521 100644 --- a/launch/autonomy.launch +++ b/launch/autonomy.launch @@ -3,7 +3,7 @@ - + diff --git a/launch/basestation.launch b/launch/basestation.launch index fc32e47fa..7b0c24ccc 100644 --- a/launch/basestation.launch +++ b/launch/basestation.launch @@ -4,13 +4,19 @@ + + + + - - + + + + @@ -18,6 +24,8 @@ + + diff --git a/launch/esw_base.launch b/launch/esw_base.launch index b9d321f13..29a320a22 100644 --- a/launch/esw_base.launch +++ b/launch/esw_base.launch @@ -1,32 +1,67 @@ - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/esw_science.launch b/launch/esw_science.launch index 587d4add4..07e1dbd37 100644 --- a/launch/esw_science.launch +++ b/launch/esw_science.launch @@ -2,13 +2,11 @@ - - - + + - - + \ No newline at end of file diff --git a/launch/jetson_es.launch b/launch/jetson_es.launch index 4ae3ac717..3dca9c2e3 100644 --- a/launch/jetson_es.launch +++ b/launch/jetson_es.launch @@ -1,7 +1,4 @@ - - \ No newline at end of file diff --git a/launch/localization.launch b/launch/localization.launch index f9491cafa..93b2694f4 100644 --- a/launch/localization.launch +++ b/launch/localization.launch @@ -1,7 +1,8 @@ - + + @@ -25,9 +26,13 @@ - - - + + + + + + + \ No newline at end of file diff --git a/launch/rtk_basestation.launch b/launch/rtk_basestation.launch new file mode 100644 index 000000000..2feb352d4 --- /dev/null +++ b/launch/rtk_basestation.launch @@ -0,0 +1,9 @@ + + + + + + + + \ No newline at end of file diff --git a/launch/rtk_rover.launch b/launch/rtk_rover.launch new file mode 100644 index 000000000..07012b4ae --- /dev/null +++ b/launch/rtk_rover.launch @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/launch/simulator.launch b/launch/simulator.launch index 23230736f..6c3e96e33 100644 --- a/launch/simulator.launch +++ b/launch/simulator.launch @@ -17,6 +17,11 @@ + + + + diff --git a/msg/AutonCommand.msg b/msg/AutonCommand.msg new file mode 100644 index 000000000..7b2628c7d --- /dev/null +++ b/msg/AutonCommand.msg @@ -0,0 +1,2 @@ +bool is_enabled +GPSWaypoint[] waypoints \ No newline at end of file diff --git a/msg/IK.msg b/msg/IK.msg index e6803ee8b..2dd6ec8a5 100644 --- a/msg/IK.msg +++ b/msg/IK.msg @@ -1 +1 @@ -geometry_msgs/Pose pose \ No newline at end of file +geometry_msgs/PoseStamped target \ No newline at end of file diff --git a/msg/MotorsAdjust.msg b/msg/MotorsAdjust.msg new file mode 100644 index 000000000..93ddff782 --- /dev/null +++ b/msg/MotorsAdjust.msg @@ -0,0 +1,2 @@ +string[] names # names of joints +float32[] values # position should be in radians for the joints \ No newline at end of file diff --git a/msg/Spectral.msg b/msg/Spectral.msg index c18050aad..4c0376ddb 100644 --- a/msg/Spectral.msg +++ b/msg/Spectral.msg @@ -1,2 +1,3 @@ -int32[6] data +uint8 site +float32[6] data bool error \ No newline at end of file diff --git a/msg/SpectralGroup.msg b/msg/SpectralGroup.msg deleted file mode 100644 index 015dc773a..000000000 --- a/msg/SpectralGroup.msg +++ /dev/null @@ -1 +0,0 @@ -Spectral[3] spectrals \ No newline at end of file diff --git a/msg/rtkStatus.msg b/msg/rtkStatus.msg new file mode 100644 index 000000000..bca134e45 --- /dev/null +++ b/msg/rtkStatus.msg @@ -0,0 +1,5 @@ +uint8 NO_RTK = 0 +uint8 FLOATING_RTK = 1 +uint8 RTK_FIX = 2 + +uint8 RTK_FIX_TYPE \ No newline at end of file diff --git a/package-lock.json b/package-lock.json deleted file mode 100644 index 881ebac7d..000000000 --- a/package-lock.json +++ /dev/null @@ -1,1123 +0,0 @@ -{ - "name": "mrover", - "lockfileVersion": 3, - "requires": true, - "packages": { - "": { - "dependencies": { - "@tsconfig/node18": "^18.2.2", - "@vitejs/plugin-vue": "^4.4.0", - "bootstrap": "^4.6.2", - "bootstrap-vue": "^2.23.1", - "vite": "^4.4.11", - "vue": "^3.3.4", - "vue-router": "^4.2.5" - }, - "devDependencies": { - "sass": "^1.69.2" - } - }, - "node_modules/@babel/parser": { - "version": "7.23.0", - "resolved": "https://registry.npmjs.org/@babel/parser/-/parser-7.23.0.tgz", - "integrity": "sha512-vvPKKdMemU85V9WE/l5wZEmImpCtLqbnTvqDS2U1fJ96KrxoW7KrXhNsNCblQlg8Ck4b85yxdTyelsMUgFUXiw==", - "bin": { - "parser": "bin/babel-parser.js" - }, - "engines": { - "node": ">=6.0.0" - } - }, - "node_modules/@esbuild/android-arm": { - "version": "0.18.20", - "resolved": "https://registry.npmjs.org/@esbuild/android-arm/-/android-arm-0.18.20.tgz", - "integrity": "sha512-fyi7TDI/ijKKNZTUJAQqiG5T7YjJXgnzkURqmGj13C6dCqckZBLdl4h7bkhHt/t0WP+zO9/zwroDvANaOqO5Sw==", - "cpu": [ - "arm" - ], - "optional": true, - "os": [ - "android" - ], - "engines": { - "node": ">=12" - } - }, - "node_modules/@esbuild/android-arm64": { - "version": "0.18.20", - "resolved": "https://registry.npmjs.org/@esbuild/android-arm64/-/android-arm64-0.18.20.tgz", - "integrity": "sha512-Nz4rJcchGDtENV0eMKUNa6L12zz2zBDXuhj/Vjh18zGqB44Bi7MBMSXjgunJgjRhCmKOjnPuZp4Mb6OKqtMHLQ==", - "cpu": [ - "arm64" - ], - "optional": true, - "os": [ - "android" - ], - "engines": { - "node": ">=12" - } - }, - "node_modules/@esbuild/android-x64": { - "version": "0.18.20", - "resolved": "https://registry.npmjs.org/@esbuild/android-x64/-/android-x64-0.18.20.tgz", - "integrity": "sha512-8GDdlePJA8D6zlZYJV/jnrRAi6rOiNaCC/JclcXpB+KIuvfBN4owLtgzY2bsxnx666XjJx2kDPUmnTtR8qKQUg==", - "cpu": [ - "x64" - ], - "optional": true, - "os": [ - "android" - ], - "engines": { - "node": ">=12" - } - }, - "node_modules/@esbuild/darwin-arm64": { - "version": "0.18.20", - "resolved": "https://registry.npmjs.org/@esbuild/darwin-arm64/-/darwin-arm64-0.18.20.tgz", - "integrity": "sha512-bxRHW5kHU38zS2lPTPOyuyTm+S+eobPUnTNkdJEfAddYgEcll4xkT8DB9d2008DtTbl7uJag2HuE5NZAZgnNEA==", - "cpu": [ - "arm64" - ], - "optional": true, - "os": [ - "darwin" - ], - "engines": { - "node": ">=12" - } - }, - "node_modules/@esbuild/darwin-x64": { - "version": "0.18.20", - "resolved": "https://registry.npmjs.org/@esbuild/darwin-x64/-/darwin-x64-0.18.20.tgz", - "integrity": "sha512-pc5gxlMDxzm513qPGbCbDukOdsGtKhfxD1zJKXjCCcU7ju50O7MeAZ8c4krSJcOIJGFR+qx21yMMVYwiQvyTyQ==", - "cpu": [ - "x64" - ], - "optional": true, - "os": [ - "darwin" - ], - "engines": { - "node": ">=12" - } - }, - "node_modules/@esbuild/freebsd-arm64": { - "version": "0.18.20", - "resolved": "https://registry.npmjs.org/@esbuild/freebsd-arm64/-/freebsd-arm64-0.18.20.tgz", - "integrity": "sha512-yqDQHy4QHevpMAaxhhIwYPMv1NECwOvIpGCZkECn8w2WFHXjEwrBn3CeNIYsibZ/iZEUemj++M26W3cNR5h+Tw==", - "cpu": [ - "arm64" - ], - "optional": true, - "os": [ - "freebsd" - ], - "engines": { - "node": ">=12" - } - }, - "node_modules/@esbuild/freebsd-x64": { - "version": "0.18.20", - "resolved": "https://registry.npmjs.org/@esbuild/freebsd-x64/-/freebsd-x64-0.18.20.tgz", - "integrity": "sha512-tgWRPPuQsd3RmBZwarGVHZQvtzfEBOreNuxEMKFcd5DaDn2PbBxfwLcj4+aenoh7ctXcbXmOQIn8HI6mCSw5MQ==", - "cpu": [ - "x64" - ], - "optional": true, - "os": [ - "freebsd" - ], - "engines": { - "node": ">=12" - } - }, - "node_modules/@esbuild/linux-arm": { - "version": "0.18.20", - "resolved": "https://registry.npmjs.org/@esbuild/linux-arm/-/linux-arm-0.18.20.tgz", - "integrity": "sha512-/5bHkMWnq1EgKr1V+Ybz3s1hWXok7mDFUMQ4cG10AfW3wL02PSZi5kFpYKrptDsgb2WAJIvRcDm+qIvXf/apvg==", - "cpu": [ - "arm" - ], - "optional": true, - "os": [ - "linux" - ], - "engines": { - "node": ">=12" - } - }, - "node_modules/@esbuild/linux-arm64": { - "version": "0.18.20", - "resolved": "https://registry.npmjs.org/@esbuild/linux-arm64/-/linux-arm64-0.18.20.tgz", - "integrity": "sha512-2YbscF+UL7SQAVIpnWvYwM+3LskyDmPhe31pE7/aoTMFKKzIc9lLbyGUpmmb8a8AixOL61sQ/mFh3jEjHYFvdA==", - "cpu": [ - "arm64" - ], - "optional": true, - "os": [ - "linux" - ], - "engines": { - "node": ">=12" - } - }, - "node_modules/@esbuild/linux-ia32": { - "version": "0.18.20", - "resolved": "https://registry.npmjs.org/@esbuild/linux-ia32/-/linux-ia32-0.18.20.tgz", - "integrity": "sha512-P4etWwq6IsReT0E1KHU40bOnzMHoH73aXp96Fs8TIT6z9Hu8G6+0SHSw9i2isWrD2nbx2qo5yUqACgdfVGx7TA==", - "cpu": [ - "ia32" - ], - "optional": true, - "os": [ - "linux" - ], - "engines": { - "node": ">=12" - } - }, - "node_modules/@esbuild/linux-loong64": { - "version": "0.18.20", - "resolved": "https://registry.npmjs.org/@esbuild/linux-loong64/-/linux-loong64-0.18.20.tgz", - "integrity": "sha512-nXW8nqBTrOpDLPgPY9uV+/1DjxoQ7DoB2N8eocyq8I9XuqJ7BiAMDMf9n1xZM9TgW0J8zrquIb/A7s3BJv7rjg==", - "cpu": [ - "loong64" - ], - "optional": true, - "os": [ - "linux" - ], - "engines": { - "node": ">=12" - } - }, - "node_modules/@esbuild/linux-mips64el": { - "version": "0.18.20", - "resolved": "https://registry.npmjs.org/@esbuild/linux-mips64el/-/linux-mips64el-0.18.20.tgz", - "integrity": "sha512-d5NeaXZcHp8PzYy5VnXV3VSd2D328Zb+9dEq5HE6bw6+N86JVPExrA6O68OPwobntbNJ0pzCpUFZTo3w0GyetQ==", - "cpu": [ - "mips64el" - ], - "optional": true, - "os": [ - "linux" - ], - "engines": { - "node": ">=12" - } - }, - "node_modules/@esbuild/linux-ppc64": { - "version": "0.18.20", - "resolved": "https://registry.npmjs.org/@esbuild/linux-ppc64/-/linux-ppc64-0.18.20.tgz", - "integrity": "sha512-WHPyeScRNcmANnLQkq6AfyXRFr5D6N2sKgkFo2FqguP44Nw2eyDlbTdZwd9GYk98DZG9QItIiTlFLHJHjxP3FA==", - "cpu": [ - "ppc64" - ], - "optional": true, - "os": [ - "linux" - ], - "engines": { - "node": ">=12" - } - }, - "node_modules/@esbuild/linux-riscv64": { - "version": "0.18.20", - "resolved": "https://registry.npmjs.org/@esbuild/linux-riscv64/-/linux-riscv64-0.18.20.tgz", - "integrity": "sha512-WSxo6h5ecI5XH34KC7w5veNnKkju3zBRLEQNY7mv5mtBmrP/MjNBCAlsM2u5hDBlS3NGcTQpoBvRzqBcRtpq1A==", - "cpu": [ - "riscv64" - ], - "optional": true, - "os": [ - "linux" - ], - "engines": { - "node": ">=12" - } - }, - "node_modules/@esbuild/linux-s390x": { - "version": "0.18.20", - "resolved": "https://registry.npmjs.org/@esbuild/linux-s390x/-/linux-s390x-0.18.20.tgz", - "integrity": "sha512-+8231GMs3mAEth6Ja1iK0a1sQ3ohfcpzpRLH8uuc5/KVDFneH6jtAJLFGafpzpMRO6DzJ6AvXKze9LfFMrIHVQ==", - "cpu": [ - "s390x" - ], - "optional": true, - "os": [ - "linux" - ], - "engines": { - "node": ">=12" - } - }, - "node_modules/@esbuild/linux-x64": { - "version": "0.18.20", - "resolved": "https://registry.npmjs.org/@esbuild/linux-x64/-/linux-x64-0.18.20.tgz", - "integrity": "sha512-UYqiqemphJcNsFEskc73jQ7B9jgwjWrSayxawS6UVFZGWrAAtkzjxSqnoclCXxWtfwLdzU+vTpcNYhpn43uP1w==", - "cpu": [ - "x64" - ], - "optional": true, - "os": [ - "linux" - ], - "engines": { - "node": ">=12" - } - }, - "node_modules/@esbuild/netbsd-x64": { - "version": "0.18.20", - "resolved": "https://registry.npmjs.org/@esbuild/netbsd-x64/-/netbsd-x64-0.18.20.tgz", - "integrity": "sha512-iO1c++VP6xUBUmltHZoMtCUdPlnPGdBom6IrO4gyKPFFVBKioIImVooR5I83nTew5UOYrk3gIJhbZh8X44y06A==", - "cpu": [ - "x64" - ], - "optional": true, - "os": [ - "netbsd" - ], - "engines": { - "node": ">=12" - } - }, - "node_modules/@esbuild/openbsd-x64": { - "version": "0.18.20", - "resolved": "https://registry.npmjs.org/@esbuild/openbsd-x64/-/openbsd-x64-0.18.20.tgz", - "integrity": "sha512-e5e4YSsuQfX4cxcygw/UCPIEP6wbIL+se3sxPdCiMbFLBWu0eiZOJ7WoD+ptCLrmjZBK1Wk7I6D/I3NglUGOxg==", - "cpu": [ - "x64" - ], - "optional": true, - "os": [ - "openbsd" - ], - "engines": { - "node": ">=12" - } - }, - "node_modules/@esbuild/sunos-x64": { - "version": "0.18.20", - "resolved": "https://registry.npmjs.org/@esbuild/sunos-x64/-/sunos-x64-0.18.20.tgz", - "integrity": "sha512-kDbFRFp0YpTQVVrqUd5FTYmWo45zGaXe0X8E1G/LKFC0v8x0vWrhOWSLITcCn63lmZIxfOMXtCfti/RxN/0wnQ==", - "cpu": [ - "x64" - ], - "optional": true, - "os": [ - "sunos" - ], - "engines": { - "node": ">=12" - } - }, - "node_modules/@esbuild/win32-arm64": { - "version": "0.18.20", - "resolved": "https://registry.npmjs.org/@esbuild/win32-arm64/-/win32-arm64-0.18.20.tgz", - "integrity": "sha512-ddYFR6ItYgoaq4v4JmQQaAI5s7npztfV4Ag6NrhiaW0RrnOXqBkgwZLofVTlq1daVTQNhtI5oieTvkRPfZrePg==", - "cpu": [ - "arm64" - ], - "optional": true, - "os": [ - "win32" - ], - "engines": { - "node": ">=12" - } - }, - "node_modules/@esbuild/win32-ia32": { - "version": "0.18.20", - "resolved": "https://registry.npmjs.org/@esbuild/win32-ia32/-/win32-ia32-0.18.20.tgz", - "integrity": "sha512-Wv7QBi3ID/rROT08SABTS7eV4hX26sVduqDOTe1MvGMjNd3EjOz4b7zeexIR62GTIEKrfJXKL9LFxTYgkyeu7g==", - "cpu": [ - "ia32" - ], - "optional": true, - "os": [ - "win32" - ], - "engines": { - "node": ">=12" - } - }, - "node_modules/@esbuild/win32-x64": { - "version": "0.18.20", - "resolved": "https://registry.npmjs.org/@esbuild/win32-x64/-/win32-x64-0.18.20.tgz", - "integrity": "sha512-kTdfRcSiDfQca/y9QIkng02avJ+NCaQvrMejlsB3RRv5sE9rRoeBPISaZpKxHELzRxZyLvNts1P27W3wV+8geQ==", - "cpu": [ - "x64" - ], - "optional": true, - "os": [ - "win32" - ], - "engines": { - "node": ">=12" - } - }, - "node_modules/@jridgewell/sourcemap-codec": { - "version": "1.4.15", - "resolved": "https://registry.npmjs.org/@jridgewell/sourcemap-codec/-/sourcemap-codec-1.4.15.tgz", - "integrity": "sha512-eF2rxCRulEKXHTRiDrDy6erMYWqNw4LPdQ8UQA4huuxaQsVeRPFl2oM8oDGxMFhJUWZf9McpLtJasDDZb/Bpeg==" - }, - "node_modules/@nuxt/opencollective": { - "version": "0.3.3", - "resolved": "https://registry.npmjs.org/@nuxt/opencollective/-/opencollective-0.3.3.tgz", - "integrity": "sha512-6IKCd+gP0HliixqZT/p8nW3tucD6Sv/u/eR2A9X4rxT/6hXlMzA4GZQzq4d2qnBAwSwGpmKyzkyTjNjrhaA25A==", - "dependencies": { - "chalk": "^4.1.0", - "consola": "^2.15.0", - "node-fetch": "^2.6.7" - }, - "bin": { - "opencollective": "bin/opencollective.js" - }, - "engines": { - "node": ">=8.0.0", - "npm": ">=5.0.0" - } - }, - "node_modules/@tsconfig/node18": { - "version": "18.2.2", - "resolved": "https://registry.npmjs.org/@tsconfig/node18/-/node18-18.2.2.tgz", - "integrity": "sha512-d6McJeGsuoRlwWZmVIeE8CUA27lu6jLjvv1JzqmpsytOYYbVi1tHZEnwCNVOXnj4pyLvneZlFlpXUK+X9wBWyw==" - }, - "node_modules/@vitejs/plugin-vue": { - "version": "4.4.0", - "resolved": "https://registry.npmjs.org/@vitejs/plugin-vue/-/plugin-vue-4.4.0.tgz", - "integrity": "sha512-xdguqb+VUwiRpSg+nsc2HtbAUSGak25DXYvpQQi4RVU1Xq1uworyoH/md9Rfd8zMmPR/pSghr309QNcftUVseg==", - "engines": { - "node": "^14.18.0 || >=16.0.0" - }, - "peerDependencies": { - "vite": "^4.0.0", - "vue": "^3.2.25" - } - }, - "node_modules/@vue/compiler-core": { - "version": "3.3.4", - "resolved": "https://registry.npmjs.org/@vue/compiler-core/-/compiler-core-3.3.4.tgz", - "integrity": "sha512-cquyDNvZ6jTbf/+x+AgM2Arrp6G4Dzbb0R64jiG804HRMfRiFXWI6kqUVqZ6ZR0bQhIoQjB4+2bhNtVwndW15g==", - "dependencies": { - "@babel/parser": "^7.21.3", - "@vue/shared": "3.3.4", - "estree-walker": "^2.0.2", - "source-map-js": "^1.0.2" - } - }, - "node_modules/@vue/compiler-dom": { - "version": "3.3.4", - "resolved": "https://registry.npmjs.org/@vue/compiler-dom/-/compiler-dom-3.3.4.tgz", - "integrity": "sha512-wyM+OjOVpuUukIq6p5+nwHYtj9cFroz9cwkfmP9O1nzH68BenTTv0u7/ndggT8cIQlnBeOo6sUT/gvHcIkLA5w==", - "dependencies": { - "@vue/compiler-core": "3.3.4", - "@vue/shared": "3.3.4" - } - }, - "node_modules/@vue/compiler-sfc": { - "version": "3.3.4", - "resolved": "https://registry.npmjs.org/@vue/compiler-sfc/-/compiler-sfc-3.3.4.tgz", - "integrity": "sha512-6y/d8uw+5TkCuzBkgLS0v3lSM3hJDntFEiUORM11pQ/hKvkhSKZrXW6i69UyXlJQisJxuUEJKAWEqWbWsLeNKQ==", - "dependencies": { - "@babel/parser": "^7.20.15", - "@vue/compiler-core": "3.3.4", - "@vue/compiler-dom": "3.3.4", - "@vue/compiler-ssr": "3.3.4", - "@vue/reactivity-transform": "3.3.4", - "@vue/shared": "3.3.4", - "estree-walker": "^2.0.2", - "magic-string": "^0.30.0", - "postcss": "^8.1.10", - "source-map-js": "^1.0.2" - } - }, - "node_modules/@vue/compiler-ssr": { - "version": "3.3.4", - "resolved": "https://registry.npmjs.org/@vue/compiler-ssr/-/compiler-ssr-3.3.4.tgz", - "integrity": "sha512-m0v6oKpup2nMSehwA6Uuu+j+wEwcy7QmwMkVNVfrV9P2qE5KshC6RwOCq8fjGS/Eak/uNb8AaWekfiXxbBB6gQ==", - "dependencies": { - "@vue/compiler-dom": "3.3.4", - "@vue/shared": "3.3.4" - } - }, - "node_modules/@vue/devtools-api": { - "version": "6.5.1", - "resolved": "https://registry.npmjs.org/@vue/devtools-api/-/devtools-api-6.5.1.tgz", - "integrity": "sha512-+KpckaAQyfbvshdDW5xQylLni1asvNSGme1JFs8I1+/H5pHEhqUKMEQD/qn3Nx5+/nycBq11qAEi8lk+LXI2dA==" - }, - "node_modules/@vue/reactivity": { - "version": "3.3.4", - "resolved": "https://registry.npmjs.org/@vue/reactivity/-/reactivity-3.3.4.tgz", - "integrity": "sha512-kLTDLwd0B1jG08NBF3R5rqULtv/f8x3rOFByTDz4J53ttIQEDmALqKqXY0J+XQeN0aV2FBxY8nJDf88yvOPAqQ==", - "dependencies": { - "@vue/shared": "3.3.4" - } - }, - "node_modules/@vue/reactivity-transform": { - "version": "3.3.4", - "resolved": "https://registry.npmjs.org/@vue/reactivity-transform/-/reactivity-transform-3.3.4.tgz", - "integrity": "sha512-MXgwjako4nu5WFLAjpBnCj/ieqcjE2aJBINUNQzkZQfzIZA4xn+0fV1tIYBJvvva3N3OvKGofRLvQIwEQPpaXw==", - "dependencies": { - "@babel/parser": "^7.20.15", - "@vue/compiler-core": "3.3.4", - "@vue/shared": "3.3.4", - "estree-walker": "^2.0.2", - "magic-string": "^0.30.0" - } - }, - "node_modules/@vue/runtime-core": { - "version": "3.3.4", - "resolved": "https://registry.npmjs.org/@vue/runtime-core/-/runtime-core-3.3.4.tgz", - "integrity": "sha512-R+bqxMN6pWO7zGI4OMlmvePOdP2c93GsHFM/siJI7O2nxFRzj55pLwkpCedEY+bTMgp5miZ8CxfIZo3S+gFqvA==", - "dependencies": { - "@vue/reactivity": "3.3.4", - "@vue/shared": "3.3.4" - } - }, - "node_modules/@vue/runtime-dom": { - "version": "3.3.4", - "resolved": "https://registry.npmjs.org/@vue/runtime-dom/-/runtime-dom-3.3.4.tgz", - "integrity": "sha512-Aj5bTJ3u5sFsUckRghsNjVTtxZQ1OyMWCr5dZRAPijF/0Vy4xEoRCwLyHXcj4D0UFbJ4lbx3gPTgg06K/GnPnQ==", - "dependencies": { - "@vue/runtime-core": "3.3.4", - "@vue/shared": "3.3.4", - "csstype": "^3.1.1" - } - }, - "node_modules/@vue/server-renderer": { - "version": "3.3.4", - "resolved": "https://registry.npmjs.org/@vue/server-renderer/-/server-renderer-3.3.4.tgz", - "integrity": "sha512-Q6jDDzR23ViIb67v+vM1Dqntu+HUexQcsWKhhQa4ARVzxOY2HbC7QRW/ggkDBd5BU+uM1sV6XOAP0b216o34JQ==", - "dependencies": { - "@vue/compiler-ssr": "3.3.4", - "@vue/shared": "3.3.4" - }, - "peerDependencies": { - "vue": "3.3.4" - } - }, - "node_modules/@vue/shared": { - "version": "3.3.4", - "resolved": "https://registry.npmjs.org/@vue/shared/-/shared-3.3.4.tgz", - "integrity": "sha512-7OjdcV8vQ74eiz1TZLzZP4JwqM5fA94K6yntPS5Z25r9HDuGNzaGdgvwKYq6S+MxwF0TFRwe50fIR/MYnakdkQ==" - }, - "node_modules/ansi-styles": { - "version": "4.3.0", - "resolved": "https://registry.npmjs.org/ansi-styles/-/ansi-styles-4.3.0.tgz", - "integrity": "sha512-zbB9rCJAT1rbjiVDb2hqKFHNYLxgtk8NURxZ3IZwD3F6NtxbXZQCnnSi1Lkx+IDohdPlFp222wVALIheZJQSEg==", - "dependencies": { - "color-convert": "^2.0.1" - }, - "engines": { - "node": ">=8" - }, - "funding": { - "url": "https://github.com/chalk/ansi-styles?sponsor=1" - } - }, - "node_modules/anymatch": { - "version": "3.1.3", - "resolved": "https://registry.npmjs.org/anymatch/-/anymatch-3.1.3.tgz", - "integrity": "sha512-KMReFUr0B4t+D+OBkjR3KYqvocp2XaSzO55UcB6mgQMd3KbcE+mWTyvVV7D/zsdEbNnV6acZUutkiHQXvTr1Rw==", - "devOptional": true, - "dependencies": { - "normalize-path": "^3.0.0", - "picomatch": "^2.0.4" - }, - "engines": { - "node": ">= 8" - } - }, - "node_modules/binary-extensions": { - "version": "2.2.0", - "resolved": "https://registry.npmjs.org/binary-extensions/-/binary-extensions-2.2.0.tgz", - "integrity": "sha512-jDctJ/IVQbZoJykoeHbhXpOlNBqGNcwXJKJog42E5HDPUwQTSdjCHdihjj0DlnheQ7blbT6dHOafNAiS8ooQKA==", - "devOptional": true, - "engines": { - "node": ">=8" - } - }, - "node_modules/bootstrap": { - "version": "4.6.2", - "resolved": "https://registry.npmjs.org/bootstrap/-/bootstrap-4.6.2.tgz", - "integrity": "sha512-51Bbp/Uxr9aTuy6ca/8FbFloBUJZLHwnhTcnjIeRn2suQWsWzcuJhGjKDB5eppVte/8oCdOL3VuwxvZDUggwGQ==", - "funding": [ - { - "type": "github", - "url": "https://github.com/sponsors/twbs" - }, - { - "type": "opencollective", - "url": "https://opencollective.com/bootstrap" - } - ], - "peerDependencies": { - "jquery": "1.9.1 - 3", - "popper.js": "^1.16.1" - } - }, - "node_modules/bootstrap-vue": { - "version": "2.23.1", - "resolved": "https://registry.npmjs.org/bootstrap-vue/-/bootstrap-vue-2.23.1.tgz", - "integrity": "sha512-SEWkG4LzmMuWjQdSYmAQk1G/oOKm37dtNfjB5kxq0YafnL2W6qUAmeDTcIZVbPiQd2OQlIkWOMPBRGySk/zGsg==", - "hasInstallScript": true, - "dependencies": { - "@nuxt/opencollective": "^0.3.2", - "bootstrap": "^4.6.1", - "popper.js": "^1.16.1", - "portal-vue": "^2.1.7", - "vue-functional-data-merge": "^3.1.0" - } - }, - "node_modules/bootstrap-vue/node_modules/@vue/compiler-sfc": { - "version": "2.7.14", - "resolved": "https://registry.npmjs.org/@vue/compiler-sfc/-/compiler-sfc-2.7.14.tgz", - "integrity": "sha512-aNmNHyLPsw+sVvlQFQ2/8sjNuLtK54TC6cuKnVzAY93ks4ZBrvwQSnkkIh7bsbNhum5hJBS00wSDipQ937f5DA==", - "peer": true, - "dependencies": { - "@babel/parser": "^7.18.4", - "postcss": "^8.4.14", - "source-map": "^0.6.1" - } - }, - "node_modules/bootstrap-vue/node_modules/portal-vue": { - "version": "2.1.7", - "resolved": "https://registry.npmjs.org/portal-vue/-/portal-vue-2.1.7.tgz", - "integrity": "sha512-+yCno2oB3xA7irTt0EU5Ezw22L2J51uKAacE/6hMPMoO/mx3h4rXFkkBkT4GFsMDv/vEe8TNKC3ujJJ0PTwb6g==", - "peerDependencies": { - "vue": "^2.5.18" - } - }, - "node_modules/bootstrap-vue/node_modules/vue": { - "version": "2.7.14", - "resolved": "https://registry.npmjs.org/vue/-/vue-2.7.14.tgz", - "integrity": "sha512-b2qkFyOM0kwqWFuQmgd4o+uHGU7T+2z3T+WQp8UBjADfEv2n4FEMffzBmCKNP0IGzOEEfYjvtcC62xaSKeQDrQ==", - "peer": true, - "dependencies": { - "@vue/compiler-sfc": "2.7.14", - "csstype": "^3.1.0" - } - }, - "node_modules/braces": { - "version": "3.0.2", - "resolved": "https://registry.npmjs.org/braces/-/braces-3.0.2.tgz", - "integrity": "sha512-b8um+L1RzM3WDSzvhm6gIz1yfTbBt6YTlcEKAvsmqCZZFw46z626lVj9j1yEPW33H5H+lBQpZMP1k8l+78Ha0A==", - "devOptional": true, - "dependencies": { - "fill-range": "^7.0.1" - }, - "engines": { - "node": ">=8" - } - }, - "node_modules/chalk": { - "version": "4.1.2", - "resolved": "https://registry.npmjs.org/chalk/-/chalk-4.1.2.tgz", - "integrity": "sha512-oKnbhFyRIXpUuez8iBMmyEa4nbj4IOQyuhc/wy9kY7/WVPcwIO9VA668Pu8RkO7+0G76SLROeyw9CpQ061i4mA==", - "dependencies": { - "ansi-styles": "^4.1.0", - "supports-color": "^7.1.0" - }, - "engines": { - "node": ">=10" - }, - "funding": { - "url": "https://github.com/chalk/chalk?sponsor=1" - } - }, - "node_modules/chokidar": { - "version": "3.5.3", - "resolved": "https://registry.npmjs.org/chokidar/-/chokidar-3.5.3.tgz", - "integrity": "sha512-Dr3sfKRP6oTcjf2JmUmFJfeVMvXBdegxB0iVQ5eb2V10uFJUCAS8OByZdVAyVb8xXNz3GjjTgj9kLWsZTqE6kw==", - "devOptional": true, - "funding": [ - { - "type": "individual", - "url": "https://paulmillr.com/funding/" - } - ], - "dependencies": { - "anymatch": "~3.1.2", - "braces": "~3.0.2", - "glob-parent": "~5.1.2", - "is-binary-path": "~2.1.0", - "is-glob": "~4.0.1", - "normalize-path": "~3.0.0", - "readdirp": "~3.6.0" - }, - "engines": { - "node": ">= 8.10.0" - }, - "optionalDependencies": { - "fsevents": "~2.3.2" - } - }, - "node_modules/color-convert": { - "version": "2.0.1", - "resolved": "https://registry.npmjs.org/color-convert/-/color-convert-2.0.1.tgz", - "integrity": "sha512-RRECPsj7iu/xb5oKYcsFHSppFNnsj/52OVTRKb4zP5onXwVF3zVmmToNcOfGC+CRDpfK/U584fMg38ZHCaElKQ==", - "dependencies": { - "color-name": "~1.1.4" - }, - "engines": { - "node": ">=7.0.0" - } - }, - "node_modules/color-name": { - "version": "1.1.4", - "resolved": "https://registry.npmjs.org/color-name/-/color-name-1.1.4.tgz", - "integrity": "sha512-dOy+3AuW3a2wNbZHIuMZpTcgjGuLU/uBL/ubcZF9OXbDo8ff4O8yVp5Bf0efS8uEoYo5q4Fx7dY9OgQGXgAsQA==" - }, - "node_modules/consola": { - "version": "2.15.3", - "resolved": "https://registry.npmjs.org/consola/-/consola-2.15.3.tgz", - "integrity": "sha512-9vAdYbHj6x2fLKC4+oPH0kFzY/orMZyG2Aj+kNylHxKGJ/Ed4dpNyAQYwJOdqO4zdM7XpVHmyejQDcQHrnuXbw==" - }, - "node_modules/csstype": { - "version": "3.1.2", - "resolved": "https://registry.npmjs.org/csstype/-/csstype-3.1.2.tgz", - "integrity": "sha512-I7K1Uu0MBPzaFKg4nI5Q7Vs2t+3gWWW648spaF+Rg7pI9ds18Ugn+lvg4SHczUdKlHI5LWBXyqfS8+DufyBsgQ==" - }, - "node_modules/esbuild": { - "version": "0.18.20", - "resolved": "https://registry.npmjs.org/esbuild/-/esbuild-0.18.20.tgz", - "integrity": "sha512-ceqxoedUrcayh7Y7ZX6NdbbDzGROiyVBgC4PriJThBKSVPWnnFHZAkfI1lJT8QFkOwH4qOS2SJkS4wvpGl8BpA==", - "hasInstallScript": true, - "bin": { - "esbuild": "bin/esbuild" - }, - "engines": { - "node": ">=12" - }, - "optionalDependencies": { - "@esbuild/android-arm": "0.18.20", - "@esbuild/android-arm64": "0.18.20", - "@esbuild/android-x64": "0.18.20", - "@esbuild/darwin-arm64": "0.18.20", - "@esbuild/darwin-x64": "0.18.20", - "@esbuild/freebsd-arm64": "0.18.20", - "@esbuild/freebsd-x64": "0.18.20", - "@esbuild/linux-arm": "0.18.20", - "@esbuild/linux-arm64": "0.18.20", - "@esbuild/linux-ia32": "0.18.20", - "@esbuild/linux-loong64": "0.18.20", - "@esbuild/linux-mips64el": "0.18.20", - "@esbuild/linux-ppc64": "0.18.20", - "@esbuild/linux-riscv64": "0.18.20", - "@esbuild/linux-s390x": "0.18.20", - "@esbuild/linux-x64": "0.18.20", - "@esbuild/netbsd-x64": "0.18.20", - "@esbuild/openbsd-x64": "0.18.20", - "@esbuild/sunos-x64": "0.18.20", - "@esbuild/win32-arm64": "0.18.20", - "@esbuild/win32-ia32": "0.18.20", - "@esbuild/win32-x64": "0.18.20" - } - }, - "node_modules/estree-walker": { - "version": "2.0.2", - "resolved": "https://registry.npmjs.org/estree-walker/-/estree-walker-2.0.2.tgz", - "integrity": "sha512-Rfkk/Mp/DL7JVje3u18FxFujQlTNR2q6QfMSMB7AvCBx91NGj/ba3kCfza0f6dVDbw7YlRf/nDrn7pQrCCyQ/w==" - }, - "node_modules/fill-range": { - "version": "7.0.1", - "resolved": "https://registry.npmjs.org/fill-range/-/fill-range-7.0.1.tgz", - "integrity": "sha512-qOo9F+dMUmC2Lcb4BbVvnKJxTPjCm+RRpe4gDuGrzkL7mEVl/djYSu2OdQ2Pa302N4oqkSg9ir6jaLWJ2USVpQ==", - "devOptional": true, - "dependencies": { - "to-regex-range": "^5.0.1" - }, - "engines": { - "node": ">=8" - } - }, - "node_modules/fsevents": { - "version": "2.3.3", - "resolved": "https://registry.npmjs.org/fsevents/-/fsevents-2.3.3.tgz", - "integrity": "sha512-5xoDfX+fL7faATnagmWPpbFtwh/R77WmMMqqHGS65C3vvB0YHrgF+B1YmZ3441tMj5n63k0212XNoJwzlhffQw==", - "hasInstallScript": true, - "optional": true, - "os": [ - "darwin" - ], - "engines": { - "node": "^8.16.0 || ^10.6.0 || >=11.0.0" - } - }, - "node_modules/glob-parent": { - "version": "5.1.2", - "resolved": "https://registry.npmjs.org/glob-parent/-/glob-parent-5.1.2.tgz", - "integrity": "sha512-AOIgSQCepiJYwP3ARnGx+5VnTu2HBYdzbGP45eLw1vr3zB3vZLeyed1sC9hnbcOc9/SrMyM5RPQrkGz4aS9Zow==", - "devOptional": true, - "dependencies": { - "is-glob": "^4.0.1" - }, - "engines": { - "node": ">= 6" - } - }, - "node_modules/has-flag": { - "version": "4.0.0", - "resolved": "https://registry.npmjs.org/has-flag/-/has-flag-4.0.0.tgz", - "integrity": "sha512-EykJT/Q1KjTWctppgIAgfSO0tKVuZUjhgMr17kqTumMl6Afv3EISleU7qZUzoXDFTAHTDC4NOoG/ZxU3EvlMPQ==", - "engines": { - "node": ">=8" - } - }, - "node_modules/immutable": { - "version": "4.3.4", - "resolved": "https://registry.npmjs.org/immutable/-/immutable-4.3.4.tgz", - "integrity": "sha512-fsXeu4J4i6WNWSikpI88v/PcVflZz+6kMhUfIwc5SY+poQRPnaf5V7qds6SUyUN3cVxEzuCab7QIoLOQ+DQ1wA==", - "devOptional": true - }, - "node_modules/is-binary-path": { - "version": "2.1.0", - "resolved": "https://registry.npmjs.org/is-binary-path/-/is-binary-path-2.1.0.tgz", - "integrity": "sha512-ZMERYes6pDydyuGidse7OsHxtbI7WVeUEozgR/g7rd0xUimYNlvZRE/K2MgZTjWy725IfelLeVcEM97mmtRGXw==", - "devOptional": true, - "dependencies": { - "binary-extensions": "^2.0.0" - }, - "engines": { - "node": ">=8" - } - }, - "node_modules/is-extglob": { - "version": "2.1.1", - "resolved": "https://registry.npmjs.org/is-extglob/-/is-extglob-2.1.1.tgz", - "integrity": "sha512-SbKbANkN603Vi4jEZv49LeVJMn4yGwsbzZworEoyEiutsN3nJYdbO36zfhGJ6QEDpOZIFkDtnq5JRxmvl3jsoQ==", - "devOptional": true, - "engines": { - "node": ">=0.10.0" - } - }, - "node_modules/is-glob": { - "version": "4.0.3", - "resolved": "https://registry.npmjs.org/is-glob/-/is-glob-4.0.3.tgz", - "integrity": "sha512-xelSayHH36ZgE7ZWhli7pW34hNbNl8Ojv5KVmkJD4hBdD3th8Tfk9vYasLM+mXWOZhFkgZfxhLSnrwRr4elSSg==", - "devOptional": true, - "dependencies": { - "is-extglob": "^2.1.1" - }, - "engines": { - "node": ">=0.10.0" - } - }, - "node_modules/is-number": { - "version": "7.0.0", - "resolved": "https://registry.npmjs.org/is-number/-/is-number-7.0.0.tgz", - "integrity": "sha512-41Cifkg6e8TylSpdtTpeLVMqvSBEVzTttHvERD741+pnZ8ANv0004MRL43QKPDlK9cGvNp6NZWZUBlbGXYxxng==", - "devOptional": true, - "engines": { - "node": ">=0.12.0" - } - }, - "node_modules/jquery": { - "version": "3.7.1", - "resolved": "https://registry.npmjs.org/jquery/-/jquery-3.7.1.tgz", - "integrity": "sha512-m4avr8yL8kmFN8psrbFFFmB/If14iN5o9nw/NgnnM+kybDJpRsAynV2BsfpTYrTRysYUdADVD7CkUUizgkpLfg==", - "peer": true - }, - "node_modules/magic-string": { - "version": "0.30.4", - "resolved": "https://registry.npmjs.org/magic-string/-/magic-string-0.30.4.tgz", - "integrity": "sha512-Q/TKtsC5BPm0kGqgBIF9oXAs/xEf2vRKiIB4wCRQTJOQIByZ1d+NnUOotvJOvNpi5RNIgVOMC3pOuaP1ZTDlVg==", - "dependencies": { - "@jridgewell/sourcemap-codec": "^1.4.15" - }, - "engines": { - "node": ">=12" - } - }, - "node_modules/nanoid": { - "version": "3.3.6", - "resolved": "https://registry.npmjs.org/nanoid/-/nanoid-3.3.6.tgz", - "integrity": "sha512-BGcqMMJuToF7i1rt+2PWSNVnWIkGCU78jBG3RxO/bZlnZPK2Cmi2QaffxGO/2RvWi9sL+FAiRiXMgsyxQ1DIDA==", - "funding": [ - { - "type": "github", - "url": "https://github.com/sponsors/ai" - } - ], - "bin": { - "nanoid": "bin/nanoid.cjs" - }, - "engines": { - "node": "^10 || ^12 || ^13.7 || ^14 || >=15.0.1" - } - }, - "node_modules/node-fetch": { - "version": "2.7.0", - "resolved": "https://registry.npmjs.org/node-fetch/-/node-fetch-2.7.0.tgz", - "integrity": "sha512-c4FRfUm/dbcWZ7U+1Wq0AwCyFL+3nt2bEw05wfxSz+DWpWsitgmSgYmy2dQdWyKC1694ELPqMs/YzUSNozLt8A==", - "dependencies": { - "whatwg-url": "^5.0.0" - }, - "engines": { - "node": "4.x || >=6.0.0" - }, - "peerDependencies": { - "encoding": "^0.1.0" - }, - "peerDependenciesMeta": { - "encoding": { - "optional": true - } - } - }, - "node_modules/normalize-path": { - "version": "3.0.0", - "resolved": "https://registry.npmjs.org/normalize-path/-/normalize-path-3.0.0.tgz", - "integrity": "sha512-6eZs5Ls3WtCisHWp9S2GUy8dqkpGi4BVSz3GaqiE6ezub0512ESztXUwUB6C6IKbQkY2Pnb/mD4WYojCRwcwLA==", - "devOptional": true, - "engines": { - "node": ">=0.10.0" - } - }, - "node_modules/picocolors": { - "version": "1.0.0", - "resolved": "https://registry.npmjs.org/picocolors/-/picocolors-1.0.0.tgz", - "integrity": "sha512-1fygroTLlHu66zi26VoTDv8yRgm0Fccecssto+MhsZ0D/DGW2sm8E8AjW7NU5VVTRt5GxbeZ5qBuJr+HyLYkjQ==" - }, - "node_modules/picomatch": { - "version": "2.3.1", - "resolved": "https://registry.npmjs.org/picomatch/-/picomatch-2.3.1.tgz", - "integrity": "sha512-JU3teHTNjmE2VCGFzuY8EXzCDVwEqB2a8fsIvwaStHhAWJEeVd1o1QD80CU6+ZdEXXSLbSsuLwJjkCBWqRQUVA==", - "devOptional": true, - "engines": { - "node": ">=8.6" - }, - "funding": { - "url": "https://github.com/sponsors/jonschlinkert" - } - }, - "node_modules/popper.js": { - "version": "1.16.1", - "resolved": "https://registry.npmjs.org/popper.js/-/popper.js-1.16.1.tgz", - "integrity": "sha512-Wb4p1J4zyFTbM+u6WuO4XstYx4Ky9Cewe4DWrel7B0w6VVICvPwdOpotjzcf6eD8TsckVnIMNONQyPIUFOUbCQ==", - "deprecated": "You can find the new Popper v2 at @popperjs/core, this package is dedicated to the legacy v1", - "funding": { - "type": "opencollective", - "url": "https://opencollective.com/popperjs" - } - }, - "node_modules/postcss": { - "version": "8.4.31", - "resolved": "https://registry.npmjs.org/postcss/-/postcss-8.4.31.tgz", - "integrity": "sha512-PS08Iboia9mts/2ygV3eLpY5ghnUcfLV/EXTOW1E2qYxJKGGBUtNjN76FYHnMs36RmARn41bC0AZmn+rR0OVpQ==", - "funding": [ - { - "type": "opencollective", - "url": "https://opencollective.com/postcss/" - }, - { - "type": "tidelift", - "url": "https://tidelift.com/funding/github/npm/postcss" - }, - { - "type": "github", - "url": "https://github.com/sponsors/ai" - } - ], - "dependencies": { - "nanoid": "^3.3.6", - "picocolors": "^1.0.0", - "source-map-js": "^1.0.2" - }, - "engines": { - "node": "^10 || ^12 || >=14" - } - }, - "node_modules/readdirp": { - "version": "3.6.0", - "resolved": "https://registry.npmjs.org/readdirp/-/readdirp-3.6.0.tgz", - "integrity": "sha512-hOS089on8RduqdbhvQ5Z37A0ESjsqz6qnRcffsMU3495FuTdqSm+7bhJ29JvIOsBDEEnan5DPu9t3To9VRlMzA==", - "devOptional": true, - "dependencies": { - "picomatch": "^2.2.1" - }, - "engines": { - "node": ">=8.10.0" - } - }, - "node_modules/rollup": { - "version": "3.29.4", - "resolved": "https://registry.npmjs.org/rollup/-/rollup-3.29.4.tgz", - "integrity": "sha512-oWzmBZwvYrU0iJHtDmhsm662rC15FRXmcjCk1xD771dFDx5jJ02ufAQQTn0etB2emNk4J9EZg/yWKpsn9BWGRw==", - "bin": { - "rollup": "dist/bin/rollup" - }, - "engines": { - "node": ">=14.18.0", - "npm": ">=8.0.0" - }, - "optionalDependencies": { - "fsevents": "~2.3.2" - } - }, - "node_modules/sass": { - "version": "1.69.2", - "resolved": "https://registry.npmjs.org/sass/-/sass-1.69.2.tgz", - "integrity": "sha512-48lDtG/9OuSQZ9oNmJMUXI2QdCakAWrAGjpX/Fy6j4Og8dEAyE598x5GqCqnHkwV7+I5w8DJpqjm581q5HNh3w==", - "devOptional": true, - "dependencies": { - "chokidar": ">=3.0.0 <4.0.0", - "immutable": "^4.0.0", - "source-map-js": ">=0.6.2 <2.0.0" - }, - "bin": { - "sass": "sass.js" - }, - "engines": { - "node": ">=14.0.0" - } - }, - "node_modules/source-map": { - "version": "0.6.1", - "resolved": "https://registry.npmjs.org/source-map/-/source-map-0.6.1.tgz", - "integrity": "sha512-UjgapumWlbMhkBgzT7Ykc5YXUT46F0iKu8SGXq0bcwP5dz/h0Plj6enJqjz1Zbq2l5WaqYnrVbwWOWMyF3F47g==", - "peer": true, - "engines": { - "node": ">=0.10.0" - } - }, - "node_modules/source-map-js": { - "version": "1.0.2", - "resolved": "https://registry.npmjs.org/source-map-js/-/source-map-js-1.0.2.tgz", - "integrity": "sha512-R0XvVJ9WusLiqTCEiGCmICCMplcCkIwwR11mOSD9CR5u+IXYdiseeEuXCVAjS54zqwkLcPNnmU4OeJ6tUrWhDw==", - "engines": { - "node": ">=0.10.0" - } - }, - "node_modules/supports-color": { - "version": "7.2.0", - "resolved": "https://registry.npmjs.org/supports-color/-/supports-color-7.2.0.tgz", - "integrity": "sha512-qpCAvRl9stuOHveKsn7HncJRvv501qIacKzQlO/+Lwxc9+0q2wLyv4Dfvt80/DPn2pqOBsJdDiogXGR9+OvwRw==", - "dependencies": { - "has-flag": "^4.0.0" - }, - "engines": { - "node": ">=8" - } - }, - "node_modules/to-regex-range": { - "version": "5.0.1", - "resolved": "https://registry.npmjs.org/to-regex-range/-/to-regex-range-5.0.1.tgz", - "integrity": "sha512-65P7iz6X5yEr1cwcgvQxbbIw7Uk3gOy5dIdtZ4rDveLqhrdJP+Li/Hx6tyK0NEb+2GCyneCMJiGqrADCSNk8sQ==", - "devOptional": true, - "dependencies": { - "is-number": "^7.0.0" - }, - "engines": { - "node": ">=8.0" - } - }, - "node_modules/tr46": { - "version": "0.0.3", - "resolved": "https://registry.npmjs.org/tr46/-/tr46-0.0.3.tgz", - "integrity": "sha512-N3WMsuqV66lT30CrXNbEjx4GEwlow3v6rr4mCcv6prnfwhS01rkgyFdjPNBYd9br7LpXV1+Emh01fHnq2Gdgrw==" - }, - "node_modules/vite": { - "version": "4.4.11", - "resolved": "https://registry.npmjs.org/vite/-/vite-4.4.11.tgz", - "integrity": "sha512-ksNZJlkcU9b0lBwAGZGGaZHCMqHsc8OpgtoYhsQ4/I2v5cnpmmmqe5pM4nv/4Hn6G/2GhTdj0DhZh2e+Er1q5A==", - "dependencies": { - "esbuild": "^0.18.10", - "postcss": "^8.4.27", - "rollup": "^3.27.1" - }, - "bin": { - "vite": "bin/vite.js" - }, - "engines": { - "node": "^14.18.0 || >=16.0.0" - }, - "funding": { - "url": "https://github.com/vitejs/vite?sponsor=1" - }, - "optionalDependencies": { - "fsevents": "~2.3.2" - }, - "peerDependencies": { - "@types/node": ">= 14", - "less": "*", - "lightningcss": "^1.21.0", - "sass": "*", - "stylus": "*", - "sugarss": "*", - "terser": "^5.4.0" - }, - "peerDependenciesMeta": { - "@types/node": { - "optional": true - }, - "less": { - "optional": true - }, - "lightningcss": { - "optional": true - }, - "sass": { - "optional": true - }, - "stylus": { - "optional": true - }, - "sugarss": { - "optional": true - }, - "terser": { - "optional": true - } - } - }, - "node_modules/vue": { - "version": "3.3.4", - "resolved": "https://registry.npmjs.org/vue/-/vue-3.3.4.tgz", - "integrity": "sha512-VTyEYn3yvIeY1Py0WaYGZsXnz3y5UnGi62GjVEqvEGPl6nxbOrCXbVOTQWBEJUqAyTUk2uJ5JLVnYJ6ZzGbrSw==", - "dependencies": { - "@vue/compiler-dom": "3.3.4", - "@vue/compiler-sfc": "3.3.4", - "@vue/runtime-dom": "3.3.4", - "@vue/server-renderer": "3.3.4", - "@vue/shared": "3.3.4" - } - }, - "node_modules/vue-functional-data-merge": { - "version": "3.1.0", - "resolved": "https://registry.npmjs.org/vue-functional-data-merge/-/vue-functional-data-merge-3.1.0.tgz", - "integrity": "sha512-leT4kdJVQyeZNY1kmnS1xiUlQ9z1B/kdBFCILIjYYQDqZgLqCLa0UhjSSeRX6c3mUe6U5qYeM8LrEqkHJ1B4LA==" - }, - "node_modules/vue-router": { - "version": "4.2.5", - "resolved": "https://registry.npmjs.org/vue-router/-/vue-router-4.2.5.tgz", - "integrity": "sha512-DIUpKcyg4+PTQKfFPX88UWhlagBEBEfJ5A8XDXRJLUnZOvcpMF8o/dnL90vpVkGaPbjvXazV/rC1qBKrZlFugw==", - "dependencies": { - "@vue/devtools-api": "^6.5.0" - }, - "funding": { - "url": "https://github.com/sponsors/posva" - }, - "peerDependencies": { - "vue": "^3.2.0" - } - }, - "node_modules/webidl-conversions": { - "version": "3.0.1", - "resolved": "https://registry.npmjs.org/webidl-conversions/-/webidl-conversions-3.0.1.tgz", - "integrity": "sha512-2JAn3z8AR6rjK8Sm8orRC0h/bcl/DqL7tRPdGZ4I1CjdF+EaMLmYxBHyXuKL849eucPFhvBoxMsflfOb8kxaeQ==" - }, - "node_modules/whatwg-url": { - "version": "5.0.0", - "resolved": "https://registry.npmjs.org/whatwg-url/-/whatwg-url-5.0.0.tgz", - "integrity": "sha512-saE57nupxk6v3HY35+jzBwYa0rKSy0XR8JSxZPwgLr7ys0IBzhGviA1/TUGJLmSVqs8pb9AnvICXEuOHLprYTw==", - "dependencies": { - "tr46": "~0.0.3", - "webidl-conversions": "^3.0.0" - } - } - } -} diff --git a/package.json b/package.json deleted file mode 100644 index 8916bfced..000000000 --- a/package.json +++ /dev/null @@ -1 +0,0 @@ -{ "dependencies": { "@tsconfig/node18": "^18.2.2", "@vitejs/plugin-vue": "^4.4.0", "bootstrap": "^4.6.2", "bootstrap-vue": "^2.23.1", "html2canvas": "^1.4.1", "vite": "^4.4.11", "vue": "^3.3.4", "vue-router": "^4.2.5" }, "devDependencies": { "sass": "^1.69.2" } } diff --git a/package.xml b/package.xml index dd2037b9f..9ab172f38 100644 --- a/package.xml +++ b/package.xml @@ -64,12 +64,16 @@ rviz_imu_plugin robot_localization + rtcm_msgs teleop_twist_joy teleop_twist_keyboard + rqt_tf_tree + rosbag + rqt_bag rosunit @@ -78,12 +82,12 @@ - - - - - - - + + + + + + + diff --git a/pkg/libdawn-dev.deb b/pkg/libdawn-dev.deb index 950699c32..98a82d987 100644 --- a/pkg/libdawn-dev.deb +++ b/pkg/libdawn-dev.deb @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:252984a13ad2dfa510c4414720044fc1f9148d27f27796fbdcbcbc37276bbe7e -size 5208066 +oid sha256:a71d028745d79b179b6f1fab01bb87bccef31326c7656e3298ca6ba163ebb5f6 +size 4385188 diff --git a/plugins/gst_websocket_streamer.xml b/plugins/gst_websocket_streamer.xml new file mode 100644 index 000000000..89175d4b7 --- /dev/null +++ b/plugins/gst_websocket_streamer.xml @@ -0,0 +1,6 @@ + + + + \ No newline at end of file diff --git a/plugins/nv_gst_h265_enc.xml b/plugins/nv_gst_h265_enc.xml deleted file mode 100644 index 2f18c8792..000000000 --- a/plugins/nv_gst_h265_enc.xml +++ /dev/null @@ -1,6 +0,0 @@ - - - - \ No newline at end of file diff --git a/pyproject.toml b/pyproject.toml index 6a52bf226..b1580f196 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -14,18 +14,19 @@ dependencies = [ "netifaces==0.11.0", "defusedxml==0.7.1", # MRover dependencies - "Django==4.2.7", + "Django==4.2.11", "empy==3.3.4", "numpy==1.26.4", "pandas==2.2.0", "pyarrow==15.0.0", "shapely==2.0.1", "pyserial==3.5", - "moteus==0.3.59", + "moteus==0.3.67", "pymap3d==3.0.1", "aenum==3.1.15", "daphne==4.0.0", "channels==4.0.0", + "pyubx2==1.2.35", "opencv-python==4.9.0.80", ] diff --git a/science-spectral.zip b/science-spectral.zip new file mode 100644 index 000000000..6576025c2 Binary files /dev/null and b/science-spectral.zip differ diff --git a/scripts/bag_to_csv.py b/scripts/bag_to_csv.py new file mode 100644 index 000000000..3cce392ff --- /dev/null +++ b/scripts/bag_to_csv.py @@ -0,0 +1,95 @@ +import rosbag +import csv + +# bag_file_path = "../../short_range_rtk_2024-01-28-14-32-22.bag" + +# bag = rosbag.Bag(bag_file_path) + +# topics = ["/left_gps_driver/fix", "/right_gps_driver/fix"] + +# csv_file_path = "stationary.csv" + +# with open(csv_file_path, "w", newline="") as csv_file: +# # Create a CSV writer +# csv_writer = csv.writer(csv_file) + +# csv_writer.writerow(["GPS Type", "Timestamp", "Latitude", "Longitude", "Altitude"]) + +# for i in topics: +# for _, msg, timestamp in bag.read_messages(topics=i): +# # Extract relevant data from the message +# timestamp_secs = msg.header.stamp.secs + msg.header.stamp.nsecs * 1e-9 +# latitude = msg.latitude +# longitude = msg.longitude +# altitude = msg.altitude +# csv_writer.writerow([i, timestamp_secs, latitude, longitude, altitude]) + +# bag.close() + + +# bag_file_path = "../../short_range_rtk_2024-01-28-14-32-22.bag" + +# bag = rosbag.Bag(bag_file_path) + +# topic = "/linearized_pose" + +# csv_file_path = "pose.csv" + +# with open(csv_file_path, "w", newline="") as csv_file: +# csv_writer = csv.writer(csv_file) + +# csv_writer.writerow( +# [ +# "Timestamp", +# "Position_X", +# "Position_Y", +# "Position_Z", +# "Orientation_X", +# "Orientation_Y", +# "Orientation_Z", +# "Orientation_W", +# ] +# ) + +# for _, msg, timestamp in bag.read_messages(topics=topic): +# timestamp_secs = msg.header.stamp.secs + msg.header.stamp.nsecs * 1e-9 +# pos_x = msg.pose.pose.position.x +# pos_y = msg.pose.pose.position.y +# pos_z = msg.pose.pose.position.z +# ori_x = msg.pose.pose.orientation.x +# ori_y = msg.pose.pose.orientation.y +# ori_z = msg.pose.pose.orientation.z +# ori_w = msg.pose.pose.orientation.w + +# csv_writer.writerow([timestamp_secs, pos_x, pos_y, pos_z, ori_x, ori_y, ori_z, ori_w]) + +# bag.close() + +bag_file_path = "../../short_range_rtk_2024-01-28-14-32-22.bag" + +bag = rosbag.Bag(bag_file_path) + +topics = ["/left_gps_driver/rtk_fix_status", "/right_gps_driver/rtk_fix_status"] + +csv_file_left = "left_fix_status.csv" +csv_file_right = "right_fix_status.csv" + +with open(csv_file_left, "w", newline="") as csv_file: + csv_writer = csv.writer(csv_file) + + csv_writer.writerow(["Topic Name", "RTK_fix"]) + + for _, msg, _ in bag.read_messages(topics="/left_gps_driver/rtk_fix_status"): + rtk_fix_type = int(msg.RTK_FIX_TYPE) + csv_writer.writerow(["/left_gps_driver/rtk_fix_status", rtk_fix_type]) + +with open(csv_file_right, "w", newline="") as csv_file: + csv_writer = csv.writer(csv_file) + + csv_writer.writerow(["Topic Name", "RTK_fix"]) + + for _, msg, _ in bag.read_messages(topics="/right_gps_driver/rtk_fix_status"): + rtk_fix_type = int(msg.RTK_FIX_TYPE) + csv_writer.writerow(["/right_gps_driver/rtk_fix_status", rtk_fix_type]) + +bag.close() diff --git a/scripts/debug_arm_ik.py b/scripts/debug_arm_ik.py index 4757343cc..8fb2e95e4 100755 --- a/scripts/debug_arm_ik.py +++ b/scripts/debug_arm_ik.py @@ -1,7 +1,8 @@ #!/usr/bin/env python3 import rospy -from geometry_msgs.msg import Pose, Quaternion, Point, PointStamped +from geometry_msgs.msg import Pose, PoseStamped, Quaternion, Point, PointStamped +from std_msgs.msg import Header from mrover.msg import IK if __name__ == "__main__": @@ -13,18 +14,24 @@ pub.publish( IK( - pose=Pose( - position=Point(x=0.5, y=0.5, z=0.5), - orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + target=PoseStamped( + header=Header(stamp=rospy.Time.now(), frame_id="base_link"), + pose=Pose( + position=Point(x=0.8, y=1.0, z=0.5), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), ) ) ) def on_clicked_point(clicked_point: PointStamped): ik = IK( - pose=Pose( - position=clicked_point.point, - orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + target=PoseStamped( + header=Header(stamp=rospy.Time.now(), frame_id="base_link"), + pose=Pose( + position=clicked_point.point, + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), ) ) pub.publish(ik) diff --git a/scripts/gps_plotting.py b/scripts/gps_plotting.py new file mode 100644 index 000000000..7b88af137 --- /dev/null +++ b/scripts/gps_plotting.py @@ -0,0 +1,112 @@ +# import matplotlib.pyplot as plt +# from pymap3d.enu import geodetic2enu +# import pandas as pd +# import numpy as np + +# ref_lat = 42.293195 +# ref_lon = -83.7096706 +# ref_alt = 234.1 + +# linearized_pose = pd.read_csv("/home/daniel/catkin_ws/src/mrover/src/localization/linearized_pose.csv") + + +# lin_pose_x = linearized_pose["Orientation_X"].to_numpy() +# lin_pose_y = linearized_pose["Orientation_Y"].to_numpy() +# lin_pose_z = linearized_pose["Orientation_Z"].to_numpy() +# lin_pose_w = linearized_pose["Orientation_W"].to_numpy() + +# time = linearized_pose["Timestamp"].to_numpy() + + +# rec_yaw = [] +# rec_time = [] + +# for x, y, z, w, time in zip(lin_pose_x, lin_pose_y, lin_pose_z, lin_pose_w, time): +# siny_cosp = 2 * (w * z + x * y) +# cosy_cosp = 1 - 2 * (y * y + z * z) +# yaw = np.arctan2(siny_cosp, cosy_cosp) + +# rec_yaw.append(yaw) +# rec_time.append(time) + +# for i in range(1, len(rec_time)): +# rec_time[i] = rec_time[i] - rec_time[0] + +# rec_time[0] = 0 + +# plt.figure(figsize=(10, 10)) +# plt.plot(rec_time, rec_yaw, color="red", label="right") +# plt.xlabel("time (s)") +# plt.ylabel("yaw (rad)") +# plt.title("yaw vs time stationary") +# plt.legend() +# plt.savefig("rtk_plot.png") +# plt.show() + +import matplotlib.pyplot as plt +from pymap3d.enu import geodetic2enu +import pandas as pd +import numpy as np + +ref_lat = 42.293195 +ref_lon = -83.7096706 +ref_alt = 234.1 +gps_readings = pd.read_csv("/home/daniel/catkin_ws/src/mrover/src/localization/stationary.csv") +left_fix = pd.read_csv("/home/daniel/catkin_ws/src/mrover/src/localization/left_fix_status.csv") +right_fix = pd.read_csv("/home/daniel/catkin_ws/src/mrover/src/localization/right_fix_status.csv") + +left = left_fix["RTK_fix"].to_numpy() +right = right_fix["RTK_fix"].to_numpy() + +left_arr = [] +right_arr = [] +for i in left_fix: + left_arr.append(i) +for j in right_fix: + right_arr.append(i) + +gps_time = gps_readings["Timestamp"].to_numpy() +gps_type = gps_readings["GPS Type"].to_numpy() +gps_lat = gps_readings["Latitude"].to_numpy() +gps_long = gps_readings["Longitude"].to_numpy() +gps_alt = gps_readings["Altitude"].to_numpy() +print(len(left), len(right), len(gps_time)) + +left_time = [] +right_time = [] +left_x = [] +left_y = [] +left_z = [] +right_x = [] +right_y = [] +right_z = [] + +for gps_type, lat, lon, alt, time in zip(gps_type, gps_lat, gps_long, gps_alt, gps_time): + if gps_type == "/left_gps_driver/fix": + pose = geodetic2enu(lat, lon, alt, ref_lat, ref_lon, ref_alt, deg=True) + left_x.append(pose[0]) + left_y.append(pose[1]) + left_z.append(pose[2]) + left_time.append(time) + else: + pose = geodetic2enu(lat, lon, alt, ref_lat, ref_lon, ref_alt, deg=True) + right_x.append(pose[0]) + right_y.append(pose[1]) + right_z.append(pose[2]) + right_time.append(time) + + +plt.figure(figsize=(10, 10)) +plt.scatter(left_time, left_x, left_y, color="black", label="left x and left y") +# plt.scatter(left_time, left_y, color="orange", label="left y") +plt.scatter(right_time, right_x, right_y, color="green", label="right x and right y") +# plt.scatter(right_time, right_y, color="blue", label="right x") +plt.scatter(left_time, left, color="red", label="left fix") +plt.scatter(right_time, right, color="violet", label="right fix") +# plt.scatter(left_x, left_y, color='blue', label='left', s=1) +plt.xlabel("time") +plt.ylabel("pos") +plt.title("RTK Stationary test") +plt.legend() +plt.show() +# plt.savefig("rtk_plot.png") diff --git a/scripts/moteusConfigManage.py b/scripts/moteusConfigManage.py new file mode 100755 index 000000000..1886ec1f9 --- /dev/null +++ b/scripts/moteusConfigManage.py @@ -0,0 +1,98 @@ +#!/usr/bin/env python3 +# \/\/\/\/ Help displayed below: \/\/\/\/ +help = """ +moteusConfigConvert: a script to help with saving and retrieving past moteus configs + +How to use: + +To save a config: + ./scripts/moteusConfigConvert save + +To list configs saved: + ./scripts/moteusConfigConvert list + +To flash a saved config onto the moteus: + ./scripts/moteusConfigConvert flash + +Then, follow the provided instructions. + +To retrieve a saved config: + +""" +import sys +import shutil +import os +import subprocess +from pathlib import Path + +MOTEUS_SAVE_DIR = str(Path.cwd()) + "/config/moteus/" # Need to change this for other laptops. + + +def list_cfgs(): + files = os.listdir(MOTEUS_SAVE_DIR) + print("Saved configs:\n\t") + print(*files, sep="\n\t") + + +def main(): + command: str + if len(sys.argv) >= 2: + command = sys.argv[1] + else: + command = input("Please enter a command {save, flash, list}: ") + + if command == "save": + saveFName = input("Please enter name for saved config: ") + if not saveFName.endswith("cfg"): + saveFName += ".cfg" + id = int(input("Please enter ID of moteus: ")) + + print("Saving config - this may ~30 seconds") + result = subprocess.run(["moteus_tool", "-t", str(id), "--dump-config"], capture_output=True) + if result.returncode != 0: + print( + "Moteus_tool returned an error trying to load config from moteus. Check moteus is correctly connected and that you chose the correct ID" + ) + print("Moteus_tool stdout: ", result.stdout.decode()) + print("Moteus_tool stderr: ", result.stderr.decode()) + return + + saveFile = open(MOTEUS_SAVE_DIR + saveFName, "w") + saveFile.write(result.stdout.decode()) + saveFile.close() + + print(f"Saved config {saveFName}") + + elif command == "list": + list_cfgs() + + elif command == "flash": + list_cfgs() + storedConfigName = input("Please enter name for config to flash: ") + id = int(input("Please enter ID of moteus: ")) + + files = os.listdir(MOTEUS_SAVE_DIR) + if storedConfigName not in files: + print("Error: given config name not in saved configs") + return + print("Flashing config - this may ~30 seconds") + result = subprocess.run( + ["moteus_tool", "-t", str(id), "--restore-config", MOTEUS_SAVE_DIR + storedConfigName], capture_output=True + ) + if result.returncode != 0: + print( + "Moteus_tool returned an error trying to save config to moteus. Check moteus is correctly connected and that you chose the correct ID" + ) + print("Moteus_tool stdout: ", result.stdout.decode()) + print("Moteus_tool stderr: ", result.stderr.decode()) + return + + print("Wrote config to moteus") + + else: + print("Unknown command entered - \/please see usage\/") + print(help) + + +if __name__ == "__main__": + main() diff --git a/scripts/moteusConfigConvert.py b/scripts/moteus_config_tool.py similarity index 98% rename from scripts/moteusConfigConvert.py rename to scripts/moteus_config_tool.py index 6503829ce..e33a1e056 100644 --- a/scripts/moteusConfigConvert.py +++ b/scripts/moteus_config_tool.py @@ -1,3 +1,5 @@ +#!/usr/bin/env python3 + """ Script for converting moteus config into a writable format. Takes in file generated from: diff --git a/scripts/plot_bdcmc_debug_messages.py b/scripts/plot_bdcmc_debug_messages.py index 72937d3c2..d8cfaffdd 100644 --- a/scripts/plot_bdcmc_debug_messages.py +++ b/scripts/plot_bdcmc_debug_messages.py @@ -9,6 +9,8 @@ import pyqtgraph as pg from pyqtgraph.Qt import QtCore +from math import pi + if __name__ == "__main__": app = pg.mkQApp() @@ -24,12 +26,12 @@ # curve3 = plot.plot(pen='r') plot.enableAutoRange("xy", True) - plot.setYRange(0, 1 << 14) + plot.setYRange(-pi, pi) xs = np.zeros(40) ys = np.zeros((40, 4)) - with can.interface.Bus(channel="vcan0", interface="socketcan", fd=True) as bus: + with can.interface.Bus(channel="can1", interface="socketcan", fd=True) as bus: def update(): message = bus.recv() diff --git a/scripts/rtk_animation.py b/scripts/rtk_animation.py new file mode 100644 index 000000000..ea12cd559 --- /dev/null +++ b/scripts/rtk_animation.py @@ -0,0 +1,73 @@ +import matplotlib.pyplot as plt +import matplotlib.animation as animation +from pymap3d.enu import geodetic2enu +import pandas as pd +import numpy as np + + +def brownian(x0, N, sigma): + result = np.zeros(N) + result[0] = x0 + for i in range(1, N): + result[i] = result[i - 1] + np.random.normal(0, sigma) + return result + + +ref_lat = 42.293195 +ref_lon = -83.7096706 +ref_alt = 234.1 +rtk = pd.read_csv("~/catkin_ws/src/mrover/bag/rtk_linearized_pose.csv") +no_rtk = pd.read_csv("~/catkin_ws/src/mrover/bag/no_rtk_linearized_pose.csv") + +rtk_xs = rtk["field.pose.pose.position.x"].to_numpy() +rtk_ys = rtk["field.pose.pose.position.y"].to_numpy() +no_rtk_xs = no_rtk["field.pose.pose.position.x"].to_numpy() +no_rtk_ys = no_rtk["field.pose.pose.position.y"].to_numpy() +# fake_rtk_xs = rtk_xs + np.random.normal(0, 0.5, len(rtk_xs)) +# fake_rtk_ys = rtk_ys + np.random.normal(0, 0.5, len(rtk_ys)) +fake_rtk_xs = rtk_xs + brownian(0, len(rtk_xs), 0.3) +fake_rtk_ys = rtk_ys + brownian(0, len(rtk_ys), 0.3) +no_rtk_xs = fake_rtk_xs +no_rtk_ys = fake_rtk_ys + +# plt.figure() +# plt.scatter(rtk_xs, rtk_ys, color='red', label='RTK', s=1) +# plt.scatter(no_rtk_xs, no_rtk_ys, color='blue', label='No RTK', s=1) +# plt.xlabel('x (m)') +# plt.ylabel('y (m)') +# plt.title('RTK vs No RTK') +# plt.legend() +# plt.grid(True) +# plt.show() + +fig, ax = plt.subplots(1, 2) +scat1 = ax[0].scatter(rtk_xs[0], rtk_ys[0], color="red", label="RTK", s=3) +scat2 = ax[1].scatter(no_rtk_xs[0], no_rtk_ys[0], color="blue", label="No RTK", s=3) +# ax[0].set(xlim=(-50, 10), ylim=(-5, 90), xlabel='x (m)', ylabel='y (m)', title='RTK vs No RTK') +# ax[0].grid(True) +ax[0].set(xlim=(-50, 10), ylim=(-5, 90), xlabel="x (m)", ylabel="y (m)") +ax[0].set_title("RTK", fontsize=20) +ax[0].grid(True) +ax[1].set(xlim=(-50, 10), ylim=(-5, 90), xlabel="x (m)", ylabel="y (m)") +ax[1].set_title("No RTK", fontsize=20) +ax[1].grid(True) +# ax[2].legend() + + +def update(frame): + rtk_x = rtk_xs[:frame] + rtk_y = rtk_ys[:frame] + data = np.stack((rtk_x, rtk_y)).T + scat1.set_offsets(data) + + no_rtk_x = no_rtk_xs[:frame] + no_rtk_y = no_rtk_ys[:frame] + data = np.stack((no_rtk_x, no_rtk_y)).T + scat2.set_offsets(data) + + return scat1, scat2 + + +ani = animation.FuncAnimation(fig, update, frames=range(len(rtk_xs)), blit=True, interval=10) +plt.show() +ani.save("rtk_vs_no_rtk.gif", writer="imagemagick", fps=30) diff --git a/setup.py b/setup.py index 48ba751d1..e5807a321 100644 --- a/setup.py +++ b/setup.py @@ -13,6 +13,7 @@ "navigation.failure_identification", "esw", "teleoperation.teleoperation", + "teleoperation.arm_controller", ], package_dir={"": "src"}, ) diff --git a/src/esw/arm_hw_bridge/main.cpp b/src/esw/arm_hw_bridge/main.cpp index 7a3762427..28480cc13 100644 --- a/src/esw/arm_hw_bridge/main.cpp +++ b/src/esw/arm_hw_bridge/main.cpp @@ -13,7 +13,6 @@ auto armLaserCallback(std_srvs::SetBool::Request& req, std_srvs::SetBool::Respon } auto main(int argc, char** argv) -> int { - // Initialize the ROS node ros::init(argc, argv, "arm_hw_bridge"); ros::NodeHandle nh; @@ -23,7 +22,6 @@ auto main(int argc, char** argv) -> int { ros::ServiceServer armLaserService = nh.advertiseService("enable_arm_laser", armLaserCallback); - // Enter the ROS event loop ros::spin(); return EXIT_SUCCESS; diff --git a/src/esw/arm_position_test_bridge/arm_position.cpp b/src/esw/arm_position_test_bridge/arm_position.cpp index 77abf0428..8229c70b5 100644 --- a/src/esw/arm_position_test_bridge/arm_position.cpp +++ b/src/esw/arm_position_test_bridge/arm_position.cpp @@ -2,135 +2,73 @@ #include "ros/ros.h" #include "std_srvs/SetBool.h" #include -#include -#include #include #include #include void sleep(int ms); -void set_arm_position(ros::Publisher& publisher, std::vector positions, int delay); +void set_arm_position(ros::Publisher& publisher, float position); -std::vector position_data; +float position_data[7] = {0, 0, 0, 0, 0, 0, 0}; -auto arm_position_callback(sensor_msgs::JointState::ConstPtr const& msg) { - ROS_INFO(";)"); +auto arm_position_callback(Position::ConstPtr const& msg) { position_data = msg->position; - ROS_INFO("hey"); -} - -void reset_pos(ros::Publisher armPublisher, int delay) { - set_arm_position(armPublisher, {0, 0, 0, 0, 0, 0, 0}, delay); -} - -void de_roll(float position, ros::Publisher armPublisher, int delay = 5000) { - set_arm_position(armPublisher, {0, 0, 0, 0, 0, position, 0}, delay); - ROS_INFO("Joint_DE_ROLL: %f", position_data[4]); - reset_pos(armPublisher, delay); -} - -void de_pitch(float position, ros::Publisher armPublisher, int delay = 5000) { - set_arm_position(armPublisher, {0, 0, 0, position, 0, 0, 0}, delay); - ROS_INFO("Joint_DE_PITCH: %f", position_data[3]); - reset_pos(armPublisher, delay); } int main(int argc, char** argv) { - // Delay between operations - int delay = 5000; - position_data.resize(7); - ros::init(argc, argv, "arm_position_test_bridge"); ros::NodeHandle nh; ros::Rate rate(10); - ros::Publisher armPublisher = nh.advertise("arm_position_cmd", 1); - ros::Subscriber armSubscriber = nh.subscribe("arm_joint_data", 10, arm_position_callback); - while (ros::ok()) { /* Arm Position Test */ - ROS_INFO("****BEGIN BASIC AUTON ARM POSITION TEST****"); - + ROS_INFO("****BEGIN AUTON ARM POSITION TEST****"); + ros::Publisher armPublisher = nh.advertise("arm_position_cmd", 1); + ros::Subscriber armSubscriber = nh.subscribe("arm_joint_data", 10, arm_position_callback); ROS_INFO("MOVE A"); - - set_arm_position(armPublisher, {0.4, 0, 0, 0, 0, 0, 0}, delay); + set_arm_position(armPublisher, {0.4, 0, 0, 0, 0, 0, 0}); ROS_INFO("Joint_A: %f", position_data[0]); - //reset_pos(armPublisher, delay); - - // Test forwards limit switch - //set_arm_position(armPublisher, {1, 0, 0, 0, 0, 0, 0}, delay); - //ROS_INFO("Joint_A: %f", position_data[0]); - //reset_pos(armPublisher, delay); - - // Test backwards limit switch - //set_arm_position(armPublisher, {-0.4, 0, 0, 0, 0, 0, 0}, delay); - //ROS_INFO("Joint_A: %f", position_data[0]); - //reset_pos(armPublisher, delay); - - - // ROS_INFO("MOVE B"); - // set_arm_position(armPublisher, {0, -0.70, 0, 0, 0, 0, 0}, delay); - // ROS_INFO("Joint_B: %f", position_data[1]); - // reset_pos(armPublisher, delay); - - // ROS_INFO("MOVE C"); - // set_arm_position(armPublisher, {0, 0, -2.0, 0, 0, 0, 0}, delay); - // ROS_INFO("Joint_C: %f", position_data[2]); - // reset_pos(armPublisher, delay); - - // set_arm_position(armPublisher, {0, 0, 1.7, 0, 0, 0, 0}, delay); - // ROS_INFO("Joint_C: %f", position_data[2]); - // reset_pos(armPublisher, delay); - - // ROS_INFO("MOVE DE PITCH"); - // float test_de_pitch_positions[4] = {2.35, -2.35, 1.17, -1.17}; - // for(float value : test_de_pitch_positions) { - // de_pitch(value, armPublisher); - // } - - // ROS_INFO("MOVE DE ROLL"); - // float test_de_roll_positions[4] = {6.28, -6.28, 3.14, -3.14}; - // for(float value : test_de_roll_positions){ - // de_roll(value, armPublisher); - // } - ROS_INFO("****END BASIC ARM POSITION TEST****"); - // ROS_INFO("****BEGIN COMPLEX AUTON ARM POSITION TEST****"); - - // ROS_INFO("BOTH DE"); - // for(int i = 0; i < 4; i++){ - // set_arm_position(armPublisher, {0,0,0,0,test_de_pitch_positions[i],test_de_roll_positions[i],0}, delay); - // ROS_INFO("Joint_DE_ROLL: %f", position_data[4]); - // ROS_INFO("Joint_DE_PITCH: %f", position_data[3]); - // reset_pos(armPublisher, delay); - // } - - // // Joint A: [0.0, 1.0] - // // Joint B: [-0.78540, 0.0] - // // Joint C: [-2.094, 1.745] 220 degrees of motion - // // Joint DE-Pitch: [-2.268, 2.268] - // // Joint DE-Roll: [-inf,inf] - // ROS_INFO("A,B,C"); - // //min position - // set_arm_position(armPublisher, {0,-0.78,-2.0,0,0,0,0}, delay); - // ROS_INFO("Joint_A: %f", position_data[0]); - // ROS_INFO("Joint_B: %f", position_data[1]); - // ROS_INFO("Joint_C: %f", position_data[2]); - // reset_pos(armPublisher, delay); - - // // max position - // set_arm_position(armPublisher, {0.9,0.0,1.7,0,0,0,0}, delay); - // ROS_INFO("Joint_A: %f", position_data[0]); - // ROS_INFO("Joint_B: %f", position_data[1]); - // ROS_INFO("Joint_C: %f", position_data[2]); - // reset_pos(armPublisher, delay); - - // set_arm_position(armPublisher, {0.3,-0.32,-1.2,0,0,0,0}, delay); - // ROS_INFO("Joint_A: %f", position_data[0]); - // ROS_INFO("Joint_B: %f", position_data[1]); - // ROS_INFO("Joint_C: %f", position_data[2]); - // ROS_INFO("****END COMPLEX AUTON ARM POSITION TEST****"); + sleep(5000); + set_arm_position(armPublisher, {0, 0, 0, 0, 0, 0, 0}); + ROS_INFO("Joint_A: %f", position_data[0]); + sleep(5000); + + ROS_INFO("MOVE B"); + set_arm_position(armPublisher, {0, -0.78, 0, 0, 0, 0, 0}); + ROS_INFO("Joint_B: %f", position_data[1]); + sleep(5000); + set_arm_position(armPublisher, {0, 0, 0, 0, 0, 0, 0}); + ROS_INFO("Joint_B: %f", position_data[1]); + sleep(5000); + + ROS_INFO("MOVE C"); + set_arm_position(armPublisher, {0, 0, 0, 0, 0, 0, 0}); + ROS_INFO("Joint_C: %f", position_data[2]); + sleep(5000); + set_arm_position(armPublisher, {0, 0, 0, 0, 0, 0, 0}); + ROS_INFO("Joint_C: %f", position_data[2]); + sleep(5000); + + ROS_INFO("MOVE DE PITCH"); + set_arm_position(armPublisher, {0, 0, 0, 0, 0, 0, 0}); + ROS_INFO("Joint_DE_PITCH: %f", position_data[3]); + sleep(5000); + set_arm_position(armPublisher, {0, 0, 0, 0, 0, 0, 0}); + ROS_INFO("Joint_DE_PITCH: %f", position_data[3]); + sleep(5000); + + ROS_INFO("MOVE DE ROLL"); + set_arm_position(armPublisher, {0, 0, 0, 0, 0, 0, 0}); + ROS_INFO("Joint_DE_ROLL: %f", position_data[4]); + sleep(5000); + set_arm_position(armPublisher, {0, 0, 0, 0, 0, 0, 0}); + ROS_INFO("Joint_DE_ROLL: %f", position_data[4]); + + ROS_INFO("****END ARM POSITION TEST****"); + + nh.spinOnce(); } return 0; @@ -140,15 +78,10 @@ void sleep(int ms) { std::this_thread::sleep_for(std::chrono::milliseconds(ms)); } -void set_arm_position(ros::Publisher& publisher, std::vector positions, int delay) { +void set_arm_position(ros::Publisher& publisher, float positions_arr[7]) { mrover::Position armMsg; // TODO: figure out how to set allen_key and gripper to nan armMsg.names = {"joint_a", "joint_b", "joint_c", "joint_de_pitch", "joint_de_roll", "allen_key", "gripper"}; - armMsg.positions = positions; - ROS_INFO("[0]: %f, [1]: %f, [2]: %f", armMsg.positions[0], armMsg.positions[1], armMsg.positions[2]); - - for (int i = 0; i < delay; i += 50) { - sleep(50); - publisher.publish(armMsg); - } + armMsg.positions = positions_arr; + publisher.publish(armMsg); } diff --git a/src/esw/arm_translator_bridge/arm_translator.cpp b/src/esw/arm_translator_bridge/arm_translator.cpp index 0896b004a..0be5f66fe 100644 --- a/src/esw/arm_translator_bridge/arm_translator.cpp +++ b/src/esw/arm_translator_bridge/arm_translator.cpp @@ -1,318 +1,171 @@ #include "arm_translator.hpp" -#include "joint_de_translation.hpp" -#include "linear_joint_translation.hpp" +#include -#include #include +#include +#include namespace mrover { - ArmTranslator::ArmTranslator(ros::NodeHandle& nh) { - assert(mJointDEPitchIndex == mJointDE0Index); - assert(mJointDERollIndex == mJointDE1Index); - assert(mArmHWNames.size() == mRawArmNames.size()); - ROS_INFO("hi"); - for (std::size_t i = 0; i < mRawArmNames.size(); ++i) { - if (i != mJointDEPitchIndex && i != mJointDERollIndex) { - assert(mArmHWNames.at(i) == mRawArmNames.at(i)); - } - { - auto rawName = static_cast(mRawArmNames[i]); - auto [_, was_inserted] = mAdjustServersByRawArmNames.try_emplace(rawName, nh.advertiseService(std::format("{}_adjust", rawName), &ArmTranslator::adjustServiceCallback, this)); - assert(was_inserted); - } - { - auto hwName = static_cast(mArmHWNames[i]); - auto [_, was_inserted] = mAdjustClientsByArmHWNames.try_emplace(hwName, nh.serviceClient(std::format("{}_adjust", hwName))); - assert(was_inserted); - } - } - - mJointDEPitchOffset = requireParamAsUnit(nh, "joint_de/pitch_offset"); - mJointDERollOffset = requireParamAsUnit(nh, "joint_de/roll_offset"); - - mMinRadPerSecDE0 = requireParamAsUnit(nh, "brushless_motors/controllers/joint_de_0/min_velocity"); - mMaxRadPerSecDE0 = requireParamAsUnit(nh, "brushless_motors/controllers/joint_de_0/max_velocity"); - mMinRadPerSecDE1 = requireParamAsUnit(nh, "brushless_motors/controllers/joint_de_1/min_velocity"); - mMaxRadPerSecDE1 = requireParamAsUnit(nh, "brushless_motors/controllers/joint_de_1/max_velocity"); + // Maps pitch and roll values to the DE0 and DE1 motors outputs + // For example when only pitching the motor, both controllers should be moving in the same direction + // When rolling, the controllers should move in opposite directions + auto static const PITCH_ROLL_TO_0_1 = (Matrix2{} << -1, -1, -1, 1).finished(); + Dimensionless static constexpr PITCH_ROLL_TO_01_SCALE{40}; - mJointDEPitchPosSub = nh.subscribe("joint_de_pitch_raw_position_data", 1, &ArmTranslator::processPitchRawPositionData, this); - mJointDERollPosSub = nh.subscribe("joint_de_roll_raw_position_data", 1, &ArmTranslator::processRollRawPositionData, this); + // How often we send an adjust command to the DE motors + // This corrects the HALL-effect motor source on the Moteus based on the absolute encoder readings + double static constexpr DE_OFFSET_TIMER_PERIOD = 1; - mJointALinMult = requireParamAsUnit(nh, "brushless_motors/controllers/joint_a/rad_to_meters_ratio"); + ArmTranslator::ArmTranslator(ros::NodeHandle& nh) { + for (std::string const& hwName: mArmHWNames) { + [[maybe_unused]] auto [_, was_inserted] = mAdjustClientsByArmHwNames.try_emplace(hwName, nh.serviceClient(std::format("{}_adjust", hwName))); + assert(was_inserted); + } mThrottleSub = nh.subscribe("arm_throttle_cmd", 1, &ArmTranslator::processThrottleCmd, this); mVelocitySub = nh.subscribe("arm_velocity_cmd", 1, &ArmTranslator::processVelocityCmd, this); mPositionSub = nh.subscribe("arm_position_cmd", 1, &ArmTranslator::processPositionCmd, this); - mArmHWJointDataSub = nh.subscribe("arm_hw_joint_data", 1, &ArmTranslator::processArmHWJointData, this); + mJointDataSub = nh.subscribe("arm_hw_joint_data", 1, &ArmTranslator::processJointState, this); mThrottlePub = std::make_unique(nh.advertise("arm_hw_throttle_cmd", 1)); mVelocityPub = std::make_unique(nh.advertise("arm_hw_velocity_cmd", 1)); mPositionPub = std::make_unique(nh.advertise("arm_hw_position_cmd", 1)); mJointDataPub = std::make_unique(nh.advertise("arm_joint_data", 1)); + + mDeOffsetTimer = nh.createTimer(ros::Duration{DE_OFFSET_TIMER_PERIOD}, &ArmTranslator::updateDeOffsets, this); } - // void ArmTranslator::clampValues(float& val1, float& val2, float minValue1, float maxValue1, float minValue2, float maxValue2) { - // val1 = (val1 - (-80)) / (maxValue1 - minValue1) * (); - // val2 if (val1 < minValue1) { - // float const ratio = minValue1 / val1; - // val1 *= ratio; - // val2 *= ratio; - // } - // if (maxValue1 < val1) { - // float const ratio = maxValue1 / val1; - // val1 *= ratio; - // val2 *= ratio; - // } - // if (val2 < minValue2) { - // float const ratio = minValue2 / val2; - // val1 *= ratio; - // val2 *= ratio; - // } - // if (maxValue2 < val2) { - // float const ratio = maxValue2 / val2; - // val1 *= ratio; - // val2 *= ratio; - // } - // } - - auto ArmTranslator::mapValue(float& val, float inputMinValue, float inputMaxValue, float outputMinValue, float outputMaxValue) -> void { - val = (val - inputMinValue) / (inputMaxValue - inputMinValue) * (outputMaxValue - outputMinValue) + outputMinValue; + auto findJointByName(std::vector const& names, std::string const& name) -> std::optional { + auto it = std::ranges::find(names, name); + return it == names.end() ? std::nullopt : std::make_optional(std::distance(names.begin(), it)); } auto ArmTranslator::processThrottleCmd(Throttle::ConstPtr const& msg) const -> void { - if (mRawArmNames != msg->names || mRawArmNames.size() != msg->throttles.size()) { - ROS_ERROR("Throttle requests for arm is ignored!"); + if (msg->names.size() != msg->throttles.size()) { + ROS_ERROR("Name count and value count mismatched!"); return; } Throttle throttle = *msg; - ROS_INFO("pitch throttle: %f roll throttle: %f", msg->throttles.at(mJointDEPitchIndex), msg->throttles.at(mJointDERollIndex)); - - auto [joint_de_0_throttle, joint_de_1_throttle] = transformPitchRollToMotorOutputs( - msg->throttles.at(mJointDEPitchIndex), - msg->throttles.at(mJointDERollIndex)); - - ROS_INFO("pre-mapped values: de_0 %f de_1 %f", joint_de_0_throttle, joint_de_1_throttle); - mapValue( - joint_de_0_throttle, - -80.0f, - 80.0f, - -1.0f, - 1.0f); + // If present, replace the entries for DE0 and DE1 with the pitch and roll values respectively + std::optional jointDePitchIndex = findJointByName(msg->names, "joint_de_pitch"), jointDeRollIndex = findJointByName(msg->names, "joint_de_roll"); + if (jointDePitchIndex && jointDeRollIndex) { - mapValue( - joint_de_1_throttle, - -80.0f, - 80.0f, - -1.0f, - 1.0f); + std::size_t pitchIndex = jointDePitchIndex.value(), rollIndex = jointDeRollIndex.value(); - throttle.names.at(mJointDEPitchIndex) = "joint_de_0"; - throttle.names.at(mJointDERollIndex) = "joint_de_1"; - throttle.throttles.at(mJointDEPitchIndex) = joint_de_0_throttle; - throttle.throttles.at(mJointDERollIndex) = joint_de_1_throttle; + Vector2 pitchRollThrottles{msg->throttles.at(pitchIndex), msg->throttles.at(rollIndex)}; + Vector2 motorThrottles = PITCH_ROLL_TO_0_1 * pitchRollThrottles; - ROS_INFO("post-mapped values: de_0 %f de_1 %f", joint_de_0_throttle, joint_de_1_throttle); + throttle.names[pitchIndex] = "joint_de_0"; + throttle.names[rollIndex] = "joint_de_1"; + throttle.throttles[pitchIndex] = motorThrottles[0].get(); + throttle.throttles[rollIndex] = motorThrottles[1].get(); + } mThrottlePub->publish(throttle); } - auto ArmTranslator::jointDEIsCalibrated() const -> bool { - return mJointDE0PosOffset.has_value() && mJointDE1PosOffset.has_value(); - } - - auto ArmTranslator::updatePositionOffsets() -> void { - if (!mCurrentRawJointDEPitch.has_value() || !mCurrentRawJointDERoll.has_value() || !mCurrentRawJointDE0Position.has_value() || !mCurrentRawJointDE1Position.has_value()) { + auto ArmTranslator::processVelocityCmd(Velocity::ConstPtr const& msg) -> void { + if (msg->names.size() != msg->velocities.size()) { + ROS_ERROR("Name count and value count mismatched!"); return; } - std::pair expected_motor_outputs = transformPitchRollToMotorOutputs(mCurrentRawJointDEPitch->get(), mCurrentRawJointDERoll->get()); - if (mCurrentRawJointDE0Position.has_value()) { - mJointDE0PosOffset = *mCurrentRawJointDE0Position - Radians{expected_motor_outputs.first}; - } - if (mCurrentRawJointDE1Position.has_value()) { - mJointDE1PosOffset = *mCurrentRawJointDE1Position - Radians{expected_motor_outputs.second}; - } - } + Velocity velocity = *msg; - auto ArmTranslator::processPitchRawPositionData(std_msgs::Float32::ConstPtr const& msg) -> void { - mCurrentRawJointDEPitch = Radians{msg->data}; - updatePositionOffsets(); - } + // TODO(quintin): Decrease code duplication + // If present, replace the entries for DE0 and DE1 with the pitch and roll values respectively + std::optional jointDePitchIndex = findJointByName(msg->names, "joint_de_pitch"), jointDeRollIndex = findJointByName(msg->names, "joint_de_roll"); + if (jointDePitchIndex && jointDeRollIndex) { + std::size_t pitchIndex = jointDePitchIndex.value(), rollIndex = jointDeRollIndex.value(); - auto ArmTranslator::processRollRawPositionData(std_msgs::Float32::ConstPtr const& msg) -> void { - mCurrentRawJointDERoll = Radians{msg->data}; - updatePositionOffsets(); - } + Vector2 pitchRollVelocities{msg->velocities.at(pitchIndex), msg->velocities.at(rollIndex)}; + Vector2 motorVelocities = PITCH_ROLL_TO_0_1 * pitchRollVelocities; - auto ArmTranslator::processVelocityCmd(Velocity::ConstPtr const& msg) -> void { - if (mRawArmNames != msg->names || mRawArmNames.size() != msg->velocities.size()) { - ROS_ERROR("Velocity requests for arm is ignored!"); - return; + velocity.names[pitchIndex] = "joint_de_0"; + velocity.names[rollIndex] = "joint_de_1"; + velocity.velocities[pitchIndex] = motorVelocities[0].get(); + velocity.velocities[rollIndex] = motorVelocities[1].get(); } - Velocity velocity = *msg; - - auto [joint_de_0_vel, joint_de_1_vel] = transformPitchRollToMotorOutputs( - msg->velocities.at(mJointDEPitchIndex), - msg->velocities.at(mJointDERollIndex)); - - mapValue( - joint_de_0_vel, - -800.0, - 800.0, - mMinRadPerSecDE1.get(), - mMaxRadPerSecDE1.get()); - - mapValue( - joint_de_1_vel, - -800.0, - 800.0, - mMinRadPerSecDE1.get(), - mMaxRadPerSecDE1.get()); - - velocity.names.at(mJointDEPitchIndex) = "joint_de_0"; - velocity.names.at(mJointDERollIndex) = "joint_de_1"; - velocity.velocities.at(mJointDEPitchIndex) = joint_de_0_vel; - velocity.velocities.at(mJointDERollIndex) = joint_de_1_vel; - - // joint a convert linear velocity (meters/s) to revolution/s - auto joint_a_vel = convertLinVel(msg->velocities.at(mJointAIndex), static_cast(mJointALinMult.get())); - velocity.velocities.at(mJointAIndex) = joint_a_vel; - ROS_INFO("joint a velocity after conversion: %f", joint_a_vel); - mVelocityPub->publish(velocity); } auto ArmTranslator::processPositionCmd(Position::ConstPtr const& msg) -> void { - ROS_INFO("msg: %f", msg->positions[0]); - if (mRawArmNames != msg->names || mRawArmNames.size() != msg->positions.size()) { - ROS_ERROR("Position requests for arm is ignored!"); - return; - } - - if (!jointDEIsCalibrated()) { - ROS_ERROR("Position requests for arm is ignored because jointDEIsNotCalibrated!"); + if (msg->names.size() != msg->positions.size()) { + ROS_ERROR("Name count and value count mismatched!"); return; } Position position = *msg; - auto [joint_de_0_raw_pos, joint_de_1_raw_pos] = transformPitchRollToMotorOutputs( - msg->positions.at(mJointDEPitchIndex), - msg->positions.at(mJointDERollIndex)); + // TODO(quintin): Decrease code duplication + // If present, replace the entries for DE0 and DE1 with the pitch and roll values respectively + std::optional jointDePitchIndex = findJointByName(msg->names, "joint_de_pitch"), jointDeRollIndex = findJointByName(msg->names, "joint_de_roll"); + if (jointDePitchIndex && jointDeRollIndex) { + std::size_t pitchIndex = jointDePitchIndex.value(), rollIndex = jointDeRollIndex.value(); - float joint_de_0_pos = joint_de_0_raw_pos + mJointDE0PosOffset->get(); - float joint_de_1_pos = joint_de_1_raw_pos + mJointDE1PosOffset->get(); + Vector2 pitchRoll{msg->positions.at(pitchIndex), msg->positions.at(rollIndex)}; + Vector2 motorPositions = PITCH_ROLL_TO_01_SCALE * PITCH_ROLL_TO_0_1 * pitchRoll; - position.names.at(mJointDEPitchIndex) = "joint_de_0"; - position.names.at(mJointDERollIndex) = "joint_de_1"; - position.positions.at(mJointDEPitchIndex) = joint_de_0_pos; - position.positions.at(mJointDERollIndex) = joint_de_1_pos; + position.names[pitchIndex] = "joint_de_0"; + position.names[rollIndex] = "joint_de_1"; + position.positions[pitchIndex] = motorPositions[0].get(); + position.positions[rollIndex] = motorPositions[1].get(); + } - // joint a convert linear position (meters) to radians - auto joint_a_pos = convertLinPos(msg->positions.at(mJointAIndex), mJointALinMult.get()); - position.positions.at(mJointAIndex) = joint_a_pos; - mPositionPub->publish(position); } - auto ArmTranslator::processArmHWJointData(sensor_msgs::JointState::ConstPtr const& msg) -> void { + auto wrapAngle(float angle) -> float { + constexpr float pi = std::numbers::pi_v; + constexpr float tau = 2 * pi; + return std::fmod(angle + pi, tau) - pi; + } + + auto ArmTranslator::processJointState(sensor_msgs::JointState::ConstPtr const& msg) -> void { if (mArmHWNames != msg->name || mArmHWNames.size() != msg->position.size() || mArmHWNames.size() != msg->velocity.size() || mArmHWNames.size() != msg->effort.size()) { ROS_ERROR("Forwarding joint data for arm is ignored!"); return; } - // Convert joint state of joint a from radians/revolutions to meters - auto jointALinVel = convertLinVel(static_cast(msg->velocity.at(mJointAIndex)), mJointALinMult.get()); - auto jointALinPos = convertLinPos(static_cast(msg->position.at(mJointAIndex)), mJointALinMult.get()); - - sensor_msgs::JointState jointState = *msg; - auto [jointDEPitchVel, jointDERollVel] = transformMotorOutputsToPitchRoll( - static_cast(msg->velocity.at(mJointDE0Index)), - static_cast(msg->velocity.at(mJointDE1Index))); - - [[maybe_unused]] auto [jointDEPitchPos, jointDERollPos] = transformMotorOutputsToPitchRoll( - static_cast(msg->position.at(mJointDE0Index)), - static_cast(msg->position.at(mJointDE1Index))); - - auto [jointDEPitchEff, jointDERollEff] = transformMotorOutputsToPitchRoll( - static_cast(msg->effort.at(mJointDE0Index)), - static_cast(msg->effort.at(mJointDE1Index))); - - mCurrentRawJointDE0Position = Radians{msg->position.at(mJointDE0Index)}; - mCurrentRawJointDE1Position = Radians{msg->position.at(mJointDE1Index)}; - - jointState.name.at(mJointDE0Index) = "joint_de_0"; - jointState.name.at(mJointDE1Index) = "joint_de_1"; - jointState.velocity.at(mJointDE0Index) = jointDEPitchVel; - jointState.velocity.at(mJointDE1Index) = jointDERollVel; - jointState.position.at(mJointDE0Index) = jointDEPitchVel; - jointState.position.at(mJointDE1Index) = jointDERollVel; - jointState.effort.at(mJointDE0Index) = jointDEPitchEff; - jointState.effort.at(mJointDE1Index) = jointDERollEff; - - jointState.velocity.at(mJointAIndex) = jointALinVel; - jointState.position.at(mJointAIndex) = jointALinPos; + std::optional jointDe0Index = findJointByName(msg->name, "joint_de_0"), jointDe1Index = findJointByName(msg->name, "joint_de_1"); + if (jointDe0Index && jointDe1Index) { + // The Moteus reports auxiliary motor positions in the range [0, tau) instead of [-pi, pi) + // Wrap to better align with IK conventions + auto pitchWrapped = wrapAngle(static_cast(msg->position.at(jointDe0Index.value()))); + auto rollWrapped = wrapAngle(static_cast(msg->position.at(jointDe1Index.value()))); + mJointDePitchRoll = {pitchWrapped, rollWrapped}; + jointState.name[jointDe0Index.value()] = "joint_de_pitch"; + jointState.name[jointDe1Index.value()] = "joint_de_roll"; + jointState.position[jointDe0Index.value()] = pitchWrapped; + jointState.position[jointDe1Index.value()] = rollWrapped; + } mJointDataPub->publish(jointState); } - auto ArmTranslator::adjustServiceCallback(AdjustMotor::Request& req, AdjustMotor::Response& res) -> bool { - - if (req.name == "joint_de_roll") { - mJointDERollAdjust = req.value; - } else if (req.name == "joint_de_pitch") { - mJointDEPitchAdjust = req.value; - } else if (req.name == "joint_a") { - AdjustMotor::Request controllerReq; - AdjustMotor::Response controllerRes = res; - controllerReq.value = convertLinPos(static_cast(req.value), mJointALinMult.get()); - - mAdjustClientsByArmHWNames[req.name].call(controllerReq, controllerRes); - res.success = controllerRes.success; - } else { - AdjustMotor::Request controllerReq = req; - AdjustMotor::Response controllerRes = res; - mAdjustClientsByArmHWNames[req.name].call(controllerReq, controllerRes); - res.success = controllerRes.success; - } + auto ArmTranslator::updateDeOffsets(ros::TimerEvent const&) -> void { + if (!mJointDePitchRoll) return; - if (mJointDEPitchAdjust && mJointDERollAdjust) { - // convert DE_roll and DE_pitch into DE_0 and DE_1 (outgoing message to arm_hw_bridge) - auto [joint_de_0_raw_value, joint_de_1_raw_value] = transformPitchRollToMotorOutputs( - mJointDEPitchAdjust.value(), - mJointDERollAdjust.value()); - mJointDEPitchAdjust = std::nullopt; - mJointDERollAdjust = std::nullopt; - - float joint_de_0_value = joint_de_0_raw_value + mJointDE0PosOffset->get(); - float joint_de_1_value = joint_de_1_raw_value + mJointDE1PosOffset->get(); - - AdjustMotor::Response controllerResDE0; - AdjustMotor::Request controllerReqDE0; - controllerReqDE0.name = "joint_de_0"; - controllerReqDE0.value = joint_de_0_value; - mAdjustClientsByArmHWNames[controllerReqDE0.name].call(controllerResDE0, controllerReqDE0); - - AdjustMotor::Response controllerResDE1; - AdjustMotor::Request controllerReqDE1; - controllerReqDE1.name = "joint_de_1"; - controllerReqDE1.value = joint_de_1_value; - mAdjustClientsByArmHWNames[controllerReqDE1.name].call(controllerReqDE1, controllerResDE1); - - res.success = controllerResDE0.success && controllerResDE1.success; - } else { - // adjust service was for de, but both de joints have not adjusted yet - res.success = false; + Vector2 motorPositions = PITCH_ROLL_TO_01_SCALE * PITCH_ROLL_TO_0_1 * mJointDePitchRoll.value(); + { + AdjustMotor adjust; + adjust.request.name = "joint_de_0"; + adjust.request.value = motorPositions[0].get(); + mAdjustClientsByArmHwNames["joint_de_0"].call(adjust); + } + { + AdjustMotor adjust; + adjust.request.name = "joint_de_1"; + adjust.request.value = motorPositions[1].get(); + mAdjustClientsByArmHwNames["joint_de_1"].call(adjust); } - return true; } - -} // namespace mrover \ No newline at end of file +} // namespace mrover diff --git a/src/esw/arm_translator_bridge/arm_translator.hpp b/src/esw/arm_translator_bridge/arm_translator.hpp index 14beae37e..cbab3a91a 100644 --- a/src/esw/arm_translator_bridge/arm_translator.hpp +++ b/src/esw/arm_translator_bridge/arm_translator.hpp @@ -1,12 +1,13 @@ #pragma once -#include "joint_de_translation.hpp" -#include "matrix_helper.hpp" - +#include +#include #include #include +#include #include +#include #include #include @@ -14,23 +15,33 @@ #include #include #include +#include #include #include +#include + #include -#include namespace mrover { + template + using Vector2 = Eigen::Matrix; + + template + using Matrix2 = Eigen::Matrix; + class ArmTranslator { public: ArmTranslator() = default; - explicit ArmTranslator(ros::NodeHandle& nh); + ArmTranslator(ArmTranslator const&) = delete; + ArmTranslator(ArmTranslator&&) = delete; - auto processPitchRawPositionData(std_msgs::Float32::ConstPtr const& msg) -> void; + auto operator=(ArmTranslator const&) -> ArmTranslator& = delete; + auto operator=(ArmTranslator&&) -> ArmTranslator& = delete; - auto processRollRawPositionData(std_msgs::Float32::ConstPtr const& msg) -> void; + explicit ArmTranslator(ros::NodeHandle& nh); auto processVelocityCmd(Velocity::ConstPtr const& msg) -> void; @@ -38,66 +49,28 @@ namespace mrover { auto processThrottleCmd(Throttle::ConstPtr const& msg) const -> void; - auto processArmHWJointData(sensor_msgs::JointState::ConstPtr const& msg) -> void; + auto processJointState(sensor_msgs::JointState::ConstPtr const& msg) -> void; - auto adjustServiceCallback(AdjustMotor::Request& req, AdjustMotor::Response& res) -> bool; + auto updateDeOffsets(ros::TimerEvent const&) -> void; private: - // static void clampValues(float& val1, float& val2, float minValue1, float maxValue1, float minValue2, float maxValue2); - - static void mapValue(float& val, float inputMinValue, float inputMaxValue, float outputMinValue, float outputMaxValue); - - [[nodiscard]] auto jointDEIsCalibrated() const -> bool; + std::vector const mArmHWNames{"joint_a", "joint_b", "joint_c", "joint_de_0", "joint_de_1", "allen_key", "gripper"}; - auto updatePositionOffsets() -> void; - - const std::vector mRawArmNames{"joint_a", "joint_b", "joint_c", "joint_de_pitch", "joint_de_roll", "allen_key", "gripper"}; - const std::vector mArmHWNames{"joint_a", "joint_b", "joint_c", "joint_de_0", "joint_de_1", "allen_key", "gripper"}; std::unique_ptr mThrottlePub; std::unique_ptr mVelocityPub; std::unique_ptr mPositionPub; std::unique_ptr mJointDataPub; - size_t const mJointDEPitchIndex = std::find(mRawArmNames.begin(), mRawArmNames.end(), "joint_de_pitch") - mRawArmNames.begin(); - size_t const mJointDERollIndex = std::find(mRawArmNames.begin(), mRawArmNames.end(), "joint_de_roll") - mRawArmNames.begin(); - size_t const mJointDE0Index = std::find(mArmHWNames.begin(), mArmHWNames.end(), "joint_de_0") - mArmHWNames.begin(); - size_t const mJointDE1Index = std::find(mArmHWNames.begin(), mArmHWNames.end(), "joint_de_1") - mArmHWNames.begin(); - - size_t const mJointAIndex = std::find(mArmHWNames.begin(), mArmHWNames.end(), "joint_a") - mArmHWNames.begin(); - std::optional mJointDE0PosOffset = Radians{0}; - std::optional mJointDE1PosOffset = Radians{0}; - - Radians mJointDEPitchOffset; - Radians mJointDERollOffset; - - std::optional mCurrentRawJointDEPitch; - std::optional mCurrentRawJointDERoll; - std::optional mCurrentRawJointDE0Position; - std::optional mCurrentRawJointDE1Position; - RadiansPerSecond mMinRadPerSecDE0{}; - RadiansPerSecond mMinRadPerSecDE1{}; - RadiansPerSecond mMaxRadPerSecDE0{}; - RadiansPerSecond mMaxRadPerSecDE1{}; + ros::Timer mDeOffsetTimer; - RadiansPerMeter mJointALinMult{}; // TODO: need to be rev/meter for velocity.... - - ros::Subscriber mJointDEPitchPosSub; - ros::Subscriber mJointDERollPosSub; + std::optional> mJointDePitchRoll; ros::Subscriber mThrottleSub; ros::Subscriber mVelocitySub; ros::Subscriber mPositionSub; - ros::Subscriber mArmHWJointDataSub; - - // TODO:(owen) unique_ptr servers? unique_ptr clients? Both? Neither? The world may never know. (try to learn) - std::unordered_map mAdjustServersByRawArmNames; - // std::unordered_map > mCalibrateServer; - - std::unordered_map mAdjustClientsByArmHWNames; - // std::unique_ptr mCalibrateClient; + ros::Subscriber mJointDataSub; - std::optional mJointDEPitchAdjust; - std::optional mJointDERollAdjust; + std::unordered_map mAdjustClientsByArmHwNames; }; } // namespace mrover diff --git a/src/esw/arm_translator_bridge/joint_de_translation.hpp b/src/esw/arm_translator_bridge/joint_de_translation.hpp deleted file mode 100644 index 3108c321d..000000000 --- a/src/esw/arm_translator_bridge/joint_de_translation.hpp +++ /dev/null @@ -1,33 +0,0 @@ -#pragma once - -#include "matrix_helper.hpp" -#include - -namespace mrover { - - constexpr std::array, 2> CONVERT_PITCH_ROLL_TO_MOTORS_MATRIX = {{{{40, 40}}, - {{40, -40}}}}; - - constexpr std::array, 2> CONVERT_MOTORS_TO_PITCH_ROLL_MATRIX = inverseMatrix(CONVERT_PITCH_ROLL_TO_MOTORS_MATRIX); - - [[maybe_unused]] static auto transformMotorOutputsToPitchRoll(float motor_0, float motor_1) -> std::pair { - - std::array motorsVector = {motor_0, motor_1}; - - // Perform matrix multiplication - auto [pitch, roll] = multiplyMatrixByVector(motorsVector, CONVERT_MOTORS_TO_PITCH_ROLL_MATRIX); - - return std::make_pair(pitch, roll); - } - - // Function to transform coordinates - [[maybe_unused]] static auto transformPitchRollToMotorOutputs(float pitch, float roll) -> std::pair { - // Create the input vector - std::array pitchRollVector = {pitch, roll}; - - // Perform matrix multiplication - auto [m1, m2] = multiplyMatrixByVector(pitchRollVector, CONVERT_PITCH_ROLL_TO_MOTORS_MATRIX); - - return std::make_pair(m1, m2); - } -} // namespace mrover diff --git a/src/esw/arm_translator_bridge/matrix_helper.hpp b/src/esw/arm_translator_bridge/matrix_helper.hpp deleted file mode 100644 index 3b893861e..000000000 --- a/src/esw/arm_translator_bridge/matrix_helper.hpp +++ /dev/null @@ -1,24 +0,0 @@ -#pragma once - -#include - -namespace mrover { - - constexpr std::array, 2> inverseMatrix(std::array, 2> const& matrix) { - float determinant = matrix[0][0] * matrix[1][1] - matrix[0][1] * matrix[1][0]; - float invDet = 1.0f / determinant; - - std::array, 2> result = {{{matrix[1][1] * invDet, -matrix[0][1] * invDet}, - {-matrix[1][0] * invDet, matrix[0][0] * invDet}}}; - - return result; - } - - static std::array multiplyMatrixByVector(std::array const& vector, std::array, 2> const& matrix) { - std::array result = { - matrix[0][0] * vector[0] + matrix[0][1] * vector[1], - matrix[1][0] * vector[0] + matrix[1][1] * vector[1]}; - return result; - } - -} // namespace mrover diff --git a/src/esw/brushless_test_bridge/brushless_test_bridge.cpp b/src/esw/brushless_test_bridge/brushless_test_bridge.cpp index 74eabdcfd..b8b615121 100644 --- a/src/esw/brushless_test_bridge/brushless_test_bridge.cpp +++ b/src/esw/brushless_test_bridge/brushless_test_bridge.cpp @@ -1,3 +1,4 @@ +#include #include #include @@ -5,6 +6,27 @@ #include #include + +// void test_joint_de(ros::NodeHandle& nh) { + +// auto brushlessController_de0 = std::make_unique(nh, "jetson", "joint_de_0"); +// auto brushlessController_de1 = std::make_unique(nh, "jetson", "joint_de_1"); + +// brushlessController_de0->setStop(); +// brushlessController_de1->setStop(); + +// ros::Rate rate{20}; +// while (ros::ok()) { +// brushlessController_de0->setDesiredVelocity(mrover::RadiansPerSecond{60.0}); +// brushlessController_de1->setDesiredVelocity(mrover::RadiansPerSecond{60.0}); + +// ros::spinOnce(); +// rate.sleep(); +// } + + +// } + auto main(int argc, char** argv) -> int { // Initialize the ROS node ros::init(argc, argv, "brushless_test_bridge"); @@ -24,44 +46,64 @@ auto main(int argc, char** argv) -> int { // auto brushlessController_de0 = std::make_unique(nh, "jetson", "joint_de_0"); // auto brushlessController_de1 = std::make_unique(nh, "jetson", "joint_de_1"); - + // fake DE publisher: - auto DEPub = std::make_unique(nh.advertise("arm_velocity_cmd", 1)); - auto SAPub = std::make_unique(nh.advertise("sa_velocity_cmd", 1)); + // auto DEPub = std::make_unique(nh.advertise("arm_velocity_cmd", 1)); + // auto SAPub = std::make_unique(nh.advertise("sa_velocity_cmd", 1)); + auto dePub = nh.advertise("arm_position_cmd", 1); - mrover::Velocity armMsg; - mrover::Velocity saMsg; + mrover::Position armMsg; armMsg.names = {"joint_a", "joint_b", "joint_c", "joint_de_pitch", "joint_de_roll", "allen_key", "gripper"}; - armMsg.velocities = {0, 0, 0, 20, 20, 0, 0}; + armMsg.positions = {0, 0, 0, 0, 0, 0, 0}; + + std::size_t index = 0; + auto timer = ros::Timer(nh.createTimer(ros::Duration{10}, [&](ros::TimerEvent const&) { + armMsg.positions[3] = std::array{-1, 1, 0, 0, 0, 0, -1}[index]; + armMsg.positions[4] = std::array{0, 0, 0, -3.14, 3.14, 0, 3.14}[index]; + index = (index + 1) % 7; + })); + + ros::Rate rate{20}; + while (ros::ok()) { + dePub.publish(armMsg); + ros::spinOnce(); + rate.sleep(); + } + + // mrover::Velocity armMsg; + // mrover::Velocity saMsg; + // armMsg.names = {"joint_a", "joint_b", "joint_c", "joint_de_pitch", "joint_de_roll", "allen_key", "gripper"}; + // armMsg.velocities = {0, 0, 0, 0, -10, 0, 0}; - saMsg.names = {"sa_x", "sa_y", "sa_z", "sampler", "sensor_actuator"}; - saMsg.velocities = {0, 0, 0.07,0, 0}; + // saMsg.names = {"sa_x", "sa_y", "sa_z", "sampler", "sensor_actuator"}; + // saMsg.velocities = {0, 0, 0.07,0, 0}; // brushlessController_de0->setStop(); // brushlessController_de1->setStop(); - ros::Rate rate{15}; - - int count = 0; - - while(ros::ok()){ - // publish DE velocity: - DEPub->publish(armMsg); - SAPub->publish(saMsg); - count++; + // ros::Rate rate{15}; + // + // int count = 0; + // + // + // while (ros::ok()) { + // // publish DE velocity: + // DEPub->publish(armMsg); + // // SAPub->publish(saMsg); + // count++; + // + // if (count > 100) { + // armMsg.velocities[3] *= -1; + // armMsg.velocities[4] *= -1; + // count = 0; + // } + // + // ros::spinOnce(); + // rate.sleep(); + // } - if(count > 100) { - armMsg.velocities[3] *= -1; - armMsg.velocities[4] *= -1; - count = 0; - } - - ros::spinOnce(); - rate.sleep(); - } return EXIT_SUCCESS; } - diff --git a/src/esw/can_device/can_device.hpp b/src/esw/can_device/can_device.hpp index 9c966deca..464d80bf2 100644 --- a/src/esw/can_device/can_device.hpp +++ b/src/esw/can_device/can_device.hpp @@ -11,7 +11,6 @@ #include -#define CANFD_FDF 0x04 #include namespace mrover { diff --git a/src/esw/can_driver/can_driver.cpp b/src/esw/can_driver/can_driver.cpp index 7b9aa81c2..6355105f8 100644 --- a/src/esw/can_driver/can_driver.cpp +++ b/src/esw/can_driver/can_driver.cpp @@ -69,7 +69,7 @@ namespace mrover { } } else { NODELET_WARN("No CAN devices specified or config was invalid. Did you forget to load the correct ROS parameters?"); - NODELET_WARN("For example before testing the devboard run: \"rosparam load config/esw_devboard.yml\""); + NODELET_WARN("For example before testing the devboard run: \"rosparam load config/esw.yml\""); } mCanNetLink = CanNetLink{mInterface}; @@ -201,7 +201,7 @@ namespace mrover { } catch (boost::system::system_error const& error) { // check if ran out of buffer space if (error.code() == boost::asio::error::no_buffer_space) { - NODELET_WARN_STREAM_THROTTLE(1, "No buffer space available to send CAN message"); + NODELET_WARN_STREAM_THROTTLE(1, "No buffer space available to send CAN message. This usually indicates an electrical problem with the bus. CAN will avoid sending out messages if it can not see other devices."); return; } ros::shutdown(); diff --git a/src/esw/drive_bridge/main.cpp b/src/esw/drive_bridge/main.cpp index 7a895dd21..44f44ad7c 100644 --- a/src/esw/drive_bridge/main.cpp +++ b/src/esw/drive_bridge/main.cpp @@ -4,71 +4,82 @@ #include #include #include +#include + +/* + * Converts from a Twist (linear and angular velocity) to the individual wheel velocities + */ using namespace mrover; void moveDrive(geometry_msgs::Twist::ConstPtr const& msg); -std::unique_ptr driveManager; -std::vector driveNames{"front_left", "front_right", "middle_left", "middle_right", "back_left", "back_right"}; +ros::Publisher leftVelocityPub, rightVelocityPub; Meters WHEEL_DISTANCE_INNER; Meters WHEEL_DISTANCE_OUTER; compound_unit> WHEEL_LINEAR_TO_ANGULAR; -RadiansPerSecond MAX_MOTOR_SPEED; auto main(int argc, char** argv) -> int { - // Initialize the ROS node ros::init(argc, argv, "drive_bridge"); ros::NodeHandle nh; - // Load rover dimensions and other parameters from the ROS parameter server auto roverWidth = requireParamAsUnit(nh, "rover/width"); auto roverLength = requireParamAsUnit(nh, "rover/length"); WHEEL_DISTANCE_INNER = roverWidth / 2; - WHEEL_DISTANCE_OUTER = sqrt(((roverWidth / 2) * (roverWidth / 2)) + ((roverLength / 2) * (roverLength / 2))); + WHEEL_DISTANCE_OUTER = sqrt((roverWidth / 2) * (roverWidth / 2) + (roverLength / 2) * (roverLength / 2)); auto ratioMotorToWheel = requireParamAsUnit(nh, "wheel/gear_ratio"); auto wheelRadius = requireParamAsUnit(nh, "wheel/radius"); - // TODO(quintin) is dividing by ratioMotorToWheel right? WHEEL_LINEAR_TO_ANGULAR = Radians{1} / wheelRadius * ratioMotorToWheel; - auto maxLinearSpeed = requireParamAsUnit(nh, "rover/max_speed"); - assert(maxLinearSpeed > 0_mps); - - MAX_MOTOR_SPEED = maxLinearSpeed * WHEEL_LINEAR_TO_ANGULAR; + auto leftGroup = std::make_unique(nh, "drive_left"); + auto rightGroup = std::make_unique(nh, "drive_right"); - driveManager = std::make_unique(nh, "drive"); + leftVelocityPub = nh.advertise("drive_left_velocity_cmd", 1); + rightVelocityPub = nh.advertise("drive_right_velocity_cmd", 1); - // Subscribe to the ROS topic for drive commands - ros::Subscriber moveDriveSubscriber = nh.subscribe("cmd_vel", 1, moveDrive); + auto twistSubscriber = nh.subscribe("cmd_vel", 1, moveDrive); - // Enter the ROS event loop ros::spin(); return 0; } void moveDrive(geometry_msgs::Twist::ConstPtr const& msg) { - // See 11.5.1 in "Controls Engineering in the FIRST Robotics Competition" for the math + // See 13.3.1.4 in "Modern Robotics" for the math + // Link: https://hades.mech.northwestern.edu/images/7/7f/MR.pdf auto forward = MetersPerSecond{msg->linear.x}; auto turn = RadiansPerSecond{msg->angular.z}; // TODO(quintin) Don't ask me to explain perfectly why we need to cancel out a meters unit in the numerator // I think it comes from the fact that there is a unit vector in the denominator of the equation - auto delta = turn / Radians{1} * WHEEL_DISTANCE_INNER; // should be in m/s - RadiansPerSecond left = (forward - delta) * WHEEL_LINEAR_TO_ANGULAR; - RadiansPerSecond right = (forward + delta) * WHEEL_LINEAR_TO_ANGULAR; - - std::unordered_map driveCommandVelocities{ - {"front_left", left}, - {"front_right", right}, - {"middle_left", left}, - {"middle_right", right}, - {"back_left", left}, - {"back_right", right}, - }; - - for (auto [name, angularVelocity]: driveCommandVelocities) { - driveManager->getController(name).setDesiredVelocity(angularVelocity); + auto delta_inner = turn / Radians{1} * WHEEL_DISTANCE_INNER; // should be in m/s + auto delta_outer = turn / Radians{1} * WHEEL_DISTANCE_OUTER; + RadiansPerSecond left_inner = (forward - delta_inner) * WHEEL_LINEAR_TO_ANGULAR; + RadiansPerSecond right_inner = (forward + delta_inner) * WHEEL_LINEAR_TO_ANGULAR; + RadiansPerSecond left_outer = (forward - delta_outer) * WHEEL_LINEAR_TO_ANGULAR; + RadiansPerSecond right_outer = (forward + delta_outer) * WHEEL_LINEAR_TO_ANGULAR; + + constexpr auto MAX_SPEED = RadiansPerSecond{15 * 2 * M_PI}; + RadiansPerSecond maximal = std::max(abs(left_outer), abs(right_outer)); + if (maximal > MAX_SPEED) { + Dimensionless changeRatio = MAX_SPEED / maximal; + left_inner = left_inner * changeRatio; + right_inner = right_inner * changeRatio; + left_outer = left_outer * changeRatio; + right_outer = right_outer * changeRatio; + } + + { + Velocity leftVelocity; + leftVelocity.names = {"front_left", "middle_left", "back_left"}; + leftVelocity.velocities = {left_outer.get(), left_inner.get(), left_outer.get()}; + leftVelocityPub.publish(leftVelocity); + } + { + Velocity rightVelocity; + rightVelocity.names = {"front_right", "middle_right", "back_right"}; + rightVelocity.velocities = {right_outer.get(), right_inner.get(), right_outer.get()}; + rightVelocityPub.publish(rightVelocity); } } diff --git a/src/esw/fw/bdcmc/Core/Inc/common.hpp b/src/esw/fw/bdcmc/Core/Inc/common.hpp new file mode 100644 index 000000000..c51dcca6a --- /dev/null +++ b/src/esw/fw/bdcmc/Core/Inc/common.hpp @@ -0,0 +1,23 @@ +#pragma once + +#include + +namespace mrover { + + constexpr auto TAU_F = 2 * std::numbers::pi_v; + + constexpr auto CLOCK_FREQ = Hertz{140000000}; + + // Counts (ticks) per radian (NOT per rotation) + using CountsPerRad = compound_unit>; + + constexpr auto RELATIVE_CPR = CountsPerRad{3355 / TAU_F}; // Measured empirically from the devboard + constexpr auto ABSOLUTE_CPR = CountsPerRad{(1 << 14) / TAU_F}; // Corresponds to the AS5048B + + // NOTE: Change This For Each Motor Controller + constexpr static std::uint8_t DEVICE_ID = 0x21; // currently set for joint_b + + // Usually this is the Jetson + constexpr static std::uint8_t DESTINATION_DEVICE_ID = 0x10; + +} // namespace mrover diff --git a/src/esw/fw/bdcmc/Core/Inc/controller.hpp b/src/esw/fw/bdcmc/Core/Inc/controller.hpp index 90adcc4b0..4d8d6902d 100644 --- a/src/esw/fw/bdcmc/Core/Inc/controller.hpp +++ b/src/esw/fw/bdcmc/Core/Inc/controller.hpp @@ -11,6 +11,7 @@ #include #include +#include "common.hpp" #include "encoders.hpp" #include "hbridge.hpp" #include "testing.hpp" @@ -35,6 +36,8 @@ namespace mrover { bool m_watchdog_enabled{}; TIM_HandleTypeDef* m_encoder_timer{}; TIM_HandleTypeDef* m_encoder_elapsed_timer{}; + TIM_HandleTypeDef* m_throttle_timer{}; + TIM_HandleTypeDef* m_pidf_timer{}; I2C_HandleTypeDef* m_absolute_encoder_i2c{}; std::optional m_relative_encoder; std::optional m_absolute_encoder; @@ -45,9 +48,16 @@ namespace mrover { // "Desired" since it may be overridden. // For example if we are trying to drive into a limit switch it will be overriden to zero. Percent m_desired_output; + // Actual output after throttling. + // Changing the output quickly can result in back-EMF which can damage the board. + // This is a temporary fix until EHW adds TVS diodes. + Percent m_throttled_output; + using PercentPerSecond = compound_unit>; + PercentPerSecond m_throttle_rate{100}; std::optional m_uncalib_position; std::optional m_velocity; BDCMCErrorInfo m_error = BDCMCErrorInfo::DEFAULT_START_UP_NOT_CONFIGURED; + std::size_t m_missed_absolute_encoder_reads{0}; struct StateAfterConfig { Dimensionless gear_ratio; @@ -85,8 +95,8 @@ namespace mrover { m_uncalib_position = position; m_velocity = velocity; } else { - m_uncalib_position = std::nullopt; - m_velocity = std::nullopt; + m_uncalib_position.reset(); + m_velocity.reset(); } } @@ -99,9 +109,11 @@ namespace mrover { if (limit_switch.pressed()) { if (std::optional readjustment_position = limit_switch.get_readjustment_position()) { - if (!m_state_after_calib) m_state_after_calib.emplace(); + if (m_uncalib_position) { + if (!m_state_after_calib) m_state_after_calib.emplace(); - m_state_after_calib->offset_position = m_uncalib_position.value() - readjustment_position.value(); + m_state_after_calib->offset_position = m_uncalib_position.value() - readjustment_position.value(); + } } } } @@ -112,14 +124,12 @@ namespace mrover { if (m_state_after_config) { // TODO: verify this is correct - bool limit_forward = m_desired_output > 0_percent && ( - std::ranges::any_of(m_limit_switches, &LimitSwitch::limit_forward) - //|| m_uncalib_position > m_state_after_config->max_position - ); - bool limit_backward = m_desired_output < 0_percent && ( - std::ranges::any_of(m_limit_switches, &LimitSwitch::limit_backward) - //|| m_uncalib_position < m_state_after_config->min_position - ); + bool limit_forward = m_desired_output > 0_percent && (std::ranges::any_of(m_limit_switches, &LimitSwitch::limit_forward) + //|| m_uncalib_position > m_state_after_config->max_position + ); + bool limit_backward = m_desired_output < 0_percent && (std::ranges::any_of(m_limit_switches, &LimitSwitch::limit_backward) + //|| m_uncalib_position < m_state_after_config->min_position + ); if (limit_forward || limit_backward) { m_error = BDCMCErrorInfo::OUTPUT_SET_TO_ZERO_SINCE_EXCEEDING_LIMITS; } else { @@ -127,7 +137,27 @@ namespace mrover { } } - m_motor_driver.write(output.value_or(0_percent)); + Percent output_after_limit = output.value_or(0_percent); + Percent delta = output_after_limit - m_throttled_output; + + Seconds dt = cycle_time(m_throttle_timer, CLOCK_FREQ); + + Percent applied_delta = m_throttle_rate * dt; + + if (signum(output_after_limit) != signum(m_throttled_output)) { + // If we are changing directions, go straight to zero + // This also includes when going to zero from a non-zero value (since signum(0) == 0), helpful for when you want to stop moving quickly on input release + m_throttled_output = 0_percent; + } + + if (abs(delta) < applied_delta) { + // We are very close to the desired output, just set it + m_throttled_output = output_after_limit; + } else { + m_throttled_output += applied_delta * signum(delta); + } + + m_motor_driver.write(m_throttled_output); } auto process_command(AdjustCommand const& message) -> void { @@ -140,24 +170,23 @@ namespace mrover { } auto process_command(ConfigCommand const& message) -> void { - // Initialize values StateAfterConfig config{.gear_ratio = message.gear_ratio}; if (message.enc_info.quad_present) { - Ratio multiplier = (message.enc_info.quad_is_forward_polarity ? 1 : -1) * message.enc_info.quad_ratio; - if (!m_relative_encoder) m_relative_encoder.emplace(m_encoder_timer, multiplier, m_encoder_elapsed_timer); + if (!m_relative_encoder) m_relative_encoder.emplace(m_encoder_timer, message.enc_info.quad_ratio, m_encoder_elapsed_timer); } if (message.enc_info.abs_present) { - Ratio multiplier = (message.enc_info.abs_is_forward_polarity ? 1 : -1) * message.enc_info.abs_ratio; - if (!m_absolute_encoder) m_absolute_encoder.emplace(AbsoluteEncoderReader::AS5048B_Bus{m_absolute_encoder_i2c}, message.enc_info.abs_offset, multiplier, m_encoder_elapsed_timer); + if (!m_absolute_encoder) m_absolute_encoder.emplace(AbsoluteEncoderReader::AS5048B_Bus{m_absolute_encoder_i2c}, message.enc_info.abs_offset, message.enc_info.abs_ratio, m_encoder_elapsed_timer); } m_motor_driver.change_max_pwm(message.max_pwm); + m_motor_driver.change_inverted(message.is_inverted); config.min_position = message.min_position; config.max_position = message.max_position; - // TODO: verify this is correct + // Boolean configs are stored as bitfields so we have to extract them carefully + for (std::size_t i = 0; i < m_limit_switches.size(); ++i) { if (GET_BIT_AT_INDEX(message.limit_switch_info.present, i)) { bool enabled = GET_BIT_AT_INDEX(message.limit_switch_info.enabled, i); @@ -179,7 +208,6 @@ namespace mrover { } auto process_command(IdleCommand const&) -> void { - // TODO - what is the expected behavior? just afk? if (!m_state_after_config) { m_error = BDCMCErrorInfo::RECEIVING_COMMANDS_WHEN_NOT_CONFIGURED; return; @@ -217,14 +245,13 @@ namespace mrover { mode.pidf.with_d(message.d); mode.pidf.with_ff(message.ff); mode.pidf.with_output_bound(-1.0, 1.0); - // TODO(quintin): Use timer for dt - m_desired_output = mode.pidf.calculate(input, target, Seconds{0.01}); + m_desired_output = mode.pidf.calculate(input, target, cycle_time(m_pidf_timer, CLOCK_FREQ)); m_error = BDCMCErrorInfo::NO_ERROR; - m_fdcan.broadcast(OutBoundMessage{DebugState{ - .f1 = m_velocity.value().get(), - .f2 = message.velocity.get(), - }}); + // m_fdcan.broadcast(OutBoundMessage{DebugState{ + // .f1 = m_velocity.value().get(), + // .f2 = message.velocity.get(), + // }}); } auto process_command(PositionCommand const& message, PositionMode& mode) -> void { @@ -248,8 +275,7 @@ namespace mrover { // mode.pidf.with_i(message.i); mode.pidf.with_d(message.d); mode.pidf.with_output_bound(-1.0, 1.0); - // TODO(quintin): Use timer for dt - m_desired_output = mode.pidf.calculate(input, target, Seconds{0.01}); + m_desired_output = mode.pidf.calculate(input, target, cycle_time(m_pidf_timer, CLOCK_FREQ)); m_error = BDCMCErrorInfo::NO_ERROR; } @@ -277,14 +303,22 @@ namespace mrover { public: Controller() = default; - Controller(TIM_HandleTypeDef* hbridge_output, Pin hbridge_forward_pin, Pin hbridge_backward_pin, FDCAN const& fdcan, TIM_HandleTypeDef* watchdog_timer, TIM_HandleTypeDef* encoder_tick_timer, TIM_HandleTypeDef* encoder_elapsed_timer, I2C_HandleTypeDef* absolute_encoder_i2c, std::array const& limit_switches) + Controller(TIM_HandleTypeDef* hbridge_output, Pin hbridge_forward_pin, Pin hbridge_backward_pin, + FDCAN const& fdcan, TIM_HandleTypeDef* watchdog_timer, + TIM_HandleTypeDef* encoder_tick_timer, + TIM_HandleTypeDef* encoder_elapsed_timer, TIM_HandleTypeDef* throttle_timer, TIM_HandleTypeDef* pid_timer, + I2C_HandleTypeDef* absolute_encoder_i2c, + std::array const& limit_switches) : m_fdcan{fdcan}, m_motor_driver{HBridge(hbridge_output, hbridge_forward_pin, hbridge_backward_pin)}, m_watchdog_timer{watchdog_timer}, m_encoder_timer{encoder_tick_timer}, m_encoder_elapsed_timer{encoder_elapsed_timer}, + m_throttle_timer{throttle_timer}, + m_pidf_timer{pid_timer}, m_absolute_encoder_i2c{absolute_encoder_i2c}, - m_limit_switches{limit_switches} {} + m_limit_switches{limit_switches} { + } template auto process_command(Command const& command) -> void { @@ -421,10 +455,17 @@ namespace mrover { update(); } + // Max hits before we remove the calibration state + constexpr static std::size_t MAX_MISSED_ABSOLUTE_ENCODER_READS = 32; + auto request_absolute_encoder_data() -> void { // Only read the encoder if we are configured if (m_absolute_encoder) { m_absolute_encoder->request_raw_angle(); + // TODO(quintin): No magic numbers + if (m_missed_absolute_encoder_reads++ > MAX_MISSED_ABSOLUTE_ENCODER_READS) { + m_state_after_calib.reset(); + } } } @@ -434,6 +475,9 @@ namespace mrover { } } + /** + * Called after a successful I2C transaction + */ auto update_absolute_encoder() -> void { if (!m_absolute_encoder) return; @@ -441,18 +485,20 @@ namespace mrover { auto const& [position, velocity] = reading.value(); if (!m_state_after_calib) m_state_after_calib.emplace(); + m_missed_absolute_encoder_reads = 0; + // TODO(quintin): This is pretty stupid m_state_after_calib->offset_position = -position; - m_fdcan.broadcast(OutBoundMessage{DebugState{ - .f1 = position.get(), - }}); + // m_fdcan.broadcast(OutBoundMessage{DebugState{ + // .f1 = position.get(), + // }}); m_uncalib_position.emplace(); // Reset to zero m_velocity = velocity; } else { - m_uncalib_position = std::nullopt; - m_velocity = std::nullopt; + m_uncalib_position.reset(); + m_velocity.reset(); } } }; diff --git a/src/esw/fw/bdcmc/Core/Inc/encoders.hpp b/src/esw/fw/bdcmc/Core/Inc/encoders.hpp index 275085eb2..fcb24e25d 100644 --- a/src/esw/fw/bdcmc/Core/Inc/encoders.hpp +++ b/src/esw/fw/bdcmc/Core/Inc/encoders.hpp @@ -8,19 +8,11 @@ #include #include +#include "common.hpp" #include "filtering.hpp" namespace mrover { - constexpr auto tau = 2 * std::numbers::pi_v; - - // Counts (ticks) per radian (NOT per rotation) - using CountsPerRad = compound_unit>; - - constexpr auto RELATIVE_CPR = CountsPerRad{3355 / tau}; // Measured empirically - constexpr auto ABSOLUTE_CPR = CountsPerRad{(1 << 14) / tau}; - auto const CLOCK_FREQ = Hertz{HAL_RCC_GetHCLKFreq()}; - struct EncoderReading { Radians position; RadiansPerSecond velocity; diff --git a/src/esw/fw/bdcmc/Core/Inc/hbridge.hpp b/src/esw/fw/bdcmc/Core/Inc/hbridge.hpp index c759ff1bf..e01f59fa4 100644 --- a/src/esw/fw/bdcmc/Core/Inc/hbridge.hpp +++ b/src/esw/fw/bdcmc/Core/Inc/hbridge.hpp @@ -7,25 +7,28 @@ namespace mrover { - struct HBridge { + class HBridge { + Pin m_positive_pin{}; + Pin m_negative_pin{}; + TIM_HandleTypeDef* m_timer{}; + std::uint32_t m_channel = TIM_CHANNEL_1; + Percent m_max_pwm{}; + bool m_is_inverted = false; + + public: HBridge() = default; - explicit HBridge(TIM_HandleTypeDef* timer, Pin forward_pin, Pin reverse_pin); + explicit HBridge(TIM_HandleTypeDef* timer, Pin positive_pin, Pin negative_pin); - void write(Percent output) const; + auto write(Percent output) const -> void; - void change_max_pwm(Percent max_pwm); + auto set_direction_pins(Percent duty_cycle) const -> void; - private: - void set_direction_pins(Percent duty_cycle) const; + auto set_duty_cycle(Percent duty_cycle, Percent max_duty_cycle) const -> void; - void set_duty_cycle(Percent duty_cycle, Percent max_duty_cycle) const; + auto change_max_pwm(Percent max_pwm) -> void; - Pin m_forward_pins{}; - Pin m_reverse_pins{}; - TIM_HandleTypeDef* m_timer{}; - std::uint32_t m_channel = TIM_CHANNEL_1; - Percent m_max_pwm{}; + auto change_inverted(bool inverted) -> void; }; } // namespace mrover diff --git a/src/esw/fw/bdcmc/Core/Inc/main.h b/src/esw/fw/bdcmc/Core/Inc/main.h index 10cf7d765..0814ce08a 100644 --- a/src/esw/fw/bdcmc/Core/Inc/main.h +++ b/src/esw/fw/bdcmc/Core/Inc/main.h @@ -89,14 +89,8 @@ void HAL_PostInit(); #define MOTOR_0_DIR_GPIO_Port GPIOB #define MOTOR_1_DIR_Pin GPIO_PIN_6 #define MOTOR_1_DIR_GPIO_Port GPIOC -#define MOTOR_1_PWM_Pin GPIO_PIN_8 -#define MOTOR_1_PWM_GPIO_Port GPIOA #define CAN_STANDBY_Pin GPIO_PIN_15 #define CAN_STANDBY_GPIO_Port GPIOA -#define QUAD_0_A_Pin GPIO_PIN_6 -#define QUAD_0_A_GPIO_Port GPIOB -#define QUAD_0_B_Pin GPIO_PIN_7 -#define QUAD_0_B_GPIO_Port GPIOB /* USER CODE BEGIN Private defines */ diff --git a/src/esw/fw/bdcmc/Core/Inc/stm32g4xx_it.h b/src/esw/fw/bdcmc/Core/Inc/stm32g4xx_it.h index cc9b4bf35..df1ee310e 100644 --- a/src/esw/fw/bdcmc/Core/Inc/stm32g4xx_it.h +++ b/src/esw/fw/bdcmc/Core/Inc/stm32g4xx_it.h @@ -63,6 +63,7 @@ void TIM1_TRG_COM_TIM17_IRQHandler(void); void TIM2_IRQHandler(void); void I2C1_EV_IRQHandler(void); void I2C1_ER_IRQHandler(void); +void TIM6_DAC_IRQHandler(void); void TIM7_IRQHandler(void); /* USER CODE BEGIN EFP */ diff --git a/src/esw/fw/bdcmc/Core/Src/controller.cpp b/src/esw/fw/bdcmc/Core/Src/controller.cpp index e80c52490..248f0bccb 100644 --- a/src/esw/fw/bdcmc/Core/Src/controller.cpp +++ b/src/esw/fw/bdcmc/Core/Src/controller.cpp @@ -13,40 +13,35 @@ extern I2C_HandleTypeDef hi2c1; #define ABSOLUTE_I2C &hi2c1 /** - * For each timer, the update rate is determined by the .ioc file. + * For each repeating timer, the update rate is determined by the .ioc file. * * Specifically the ARR value. You can use the following equation: ARR = (MCU Clock Speed) / (Update Rate) / (Prescaler + 1) - 1 * For the STM32G4 we have a 140 MHz clock speed configured. * - * You must also set auto reload to true so the interurpt gets called on a cycle. + * You must also set auto reload to true so the interrupt gets called on a cycle. */ extern TIM_HandleTypeDef htim2; -extern TIM_HandleTypeDef htim4; extern TIM_HandleTypeDef htim3; extern TIM_HandleTypeDef htim6; extern TIM_HandleTypeDef htim7; +extern TIM_HandleTypeDef htim8; extern TIM_HandleTypeDef htim15; extern TIM_HandleTypeDef htim16; extern TIM_HandleTypeDef htim17; -#define QUADRATURE_TICK_TIMER_1 &htim3 // Special encoder timer which externally reads quadrature encoder ticks +#define QUADRATURE_TICK_TIMER_1 &htim3 // Special encoder timer which externally reads quadrature encoder ticks // #define QUADRATURE_TIMER_2 &htim4 -#define QUADRATURE_ELAPSED_TIMER_1 &htim17 // Measures time since the lsat quadrature tick reading -#define ABSOLUTE_ENCODER_TIMER &htim2 -// #define UPDATE_TIMER &htim6 -#define SEND_TIMER &htim7 // 20 Hz FDCAN repeating timer -#define PWM_TIMER_1 &htim15 // H-Bridge PWM -#define FDCAN_WATCHDOG_TIMER &htim16 // FDCAN watchdog timer that needs to be reset every time a message is received +#define QUADRATURE_ELAPSED_TIMER_1 &htim17 // Measures time since the lsat quadrature tick reading +#define ABSOLUTE_ENCODER_TIMER &htim2 // 20 Hz repeating timer to kick off I2C transactions with the absolute encoder +#define THROTTLE_LIMIT_TIMER &htim6 // Measures time since the last throttle command +#define SEND_TIMER &htim7 // 20 Hz FDCAN repeating timer +#define PIDF_TIMER &htim8 // Measures time since the last PIDF update, used for the "D" term +#define PWM_TIMER_1 &htim15 // H-Bridge PWM +#define FDCAN_WATCHDOG_TIMER &htim16 // FDCAN watchdog timer that needs to be reset every time a message is received namespace mrover { - // NOTE: Change This For Each Motor Controller - constexpr static std::uint8_t DEVICE_ID = 0x25; // currently set for joint_b - - // Usually this is the Jetson - constexpr static std::uint8_t DESTINATION_DEVICE_ID = 0x10; - FDCAN fdcan_bus; Controller controller; @@ -60,6 +55,8 @@ namespace mrover { FDCAN_WATCHDOG_TIMER, QUADRATURE_TICK_TIMER_1, QUADRATURE_ELAPSED_TIMER_1, + THROTTLE_LIMIT_TIMER, + PIDF_TIMER, ABSOLUTE_I2C, { LimitSwitch{Pin{LIMIT_0_0_GPIO_Port, LIMIT_0_0_Pin}}, @@ -71,7 +68,8 @@ namespace mrover { // TODO: these should probably be in the controller / encoders themselves // Necessary for the timer interrupt to work - // check(HAL_TIM_Base_Start_IT(UPDATE_TIMER) == HAL_OK, Error_Handler); + check(HAL_TIM_Base_Start(THROTTLE_LIMIT_TIMER) == HAL_OK, Error_Handler); + check(HAL_TIM_Base_Start(PIDF_TIMER) == HAL_OK, Error_Handler); check(HAL_TIM_Base_Start_IT(SEND_TIMER) == HAL_OK, Error_Handler); check(HAL_TIM_Base_Start_IT(ABSOLUTE_ENCODER_TIMER) == HAL_OK, Error_Handler); } @@ -155,7 +153,6 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef* htim) { } else if (htim == ABSOLUTE_ENCODER_TIMER) { mrover::request_absolute_encoder_data_callback(); } - // TODO: check for slow update timer and call on controller to send out i2c frame } void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef* htim) { @@ -176,13 +173,13 @@ void HAL_FDCAN_RxFifo0Callback(FDCAN_HandleTypeDef* hfdcan, uint32_t RxFifo0ITs) } } -// TODO: implement +// TODO(quintin): Error handling void HAL_FDCAN_ErrorCallback(FDCAN_HandleTypeDef* hfdcan) {} void HAL_FDCAN_ErrorStatusCallback(FDCAN_HandleTypeDef* hfdcan, uint32_t ErrorStatusITs) {} -// TODO DMA receive callback, this should eventually call a function on the controller with most up to date info +// TODO(quintin): Do we need these two callbacks? void HAL_I2C_SlaveRxCpltCallback(I2C_HandleTypeDef* hi2c) {} diff --git a/src/esw/fw/bdcmc/Core/Src/encoders/absolute_encoder.cpp b/src/esw/fw/bdcmc/Core/Src/encoders/absolute_encoder.cpp index 32a5ca893..404fec1a0 100644 --- a/src/esw/fw/bdcmc/Core/Src/encoders/absolute_encoder.cpp +++ b/src/esw/fw/bdcmc/Core/Src/encoders/absolute_encoder.cpp @@ -52,17 +52,27 @@ namespace mrover { return angle; } + /** + * \return Wrapped angle in the range [-𝜏/2, 𝜏/2), same as [-π, π) + */ + auto wrap_angle(Radians angle) -> Radians { + constexpr Radians PI_F{std::numbers::pi}; + return fmod(angle + PI_F, TAU_F) - PI_F; + } + [[nodiscard]] auto AbsoluteEncoderReader::read() -> std::optional { if (std::optional count = try_read_buffer()) { - std::uint32_t elapsed_count = std::exchange(__HAL_TIM_GetCounter(m_elapsed_timer), 0); - Seconds elapsed_time = 1 / CLOCK_FREQ * elapsed_count; + Seconds elapsed_time = cycle_time(m_elapsed_timer, CLOCK_FREQ); - m_position = m_multiplier * Ticks{count.value()} / ABSOLUTE_CPR + m_offset; - m_velocity_filter.add_reading((m_position - m_position_prev) / elapsed_time); + // Absolute encoder returns [0, COUNTS_PER_REVOLUTION) + // We need to convert this to [-𝜏/2, 𝜏/2) + // Angles always need to be wrapped after addition/subtraction + m_position = wrap_angle(m_multiplier * Ticks{count.value()} / ABSOLUTE_CPR + m_offset); + m_velocity_filter.add_reading(wrap_angle(m_position - m_position_prev) / elapsed_time); m_position_prev = m_position; } return std::make_optional(m_position, m_velocity_filter.get_filtered()); } -} // namespace mrover \ No newline at end of file +} // namespace mrover diff --git a/src/esw/fw/bdcmc/Core/Src/encoders/quadrature_encoder.cpp b/src/esw/fw/bdcmc/Core/Src/encoders/quadrature_encoder.cpp index 31cd7a42f..635f5593d 100644 --- a/src/esw/fw/bdcmc/Core/Src/encoders/quadrature_encoder.cpp +++ b/src/esw/fw/bdcmc/Core/Src/encoders/quadrature_encoder.cpp @@ -30,14 +30,14 @@ namespace mrover { std::uint32_t now = __HAL_TIM_GetCounter(timer); std::uint32_t max_value = __HAL_TIM_GetAutoreload(timer); - // adapted from: https://electronics.stackexchange.com/questions/605278/how-to-increase-stm32-timer-encoder-mode-counter-value - // handle when timer wraps around - std::int64_t c64 = static_cast(now) - max_value / 2; // remove half period to determine (+/-) sign of the wrap + // Adapted from: https://electronics.stackexchange.com/questions/605278/how-to-increase-stm32-timer-encoder-mode-counter-value + // Handles when the timer wraps around + std::int64_t c64 = static_cast(now) - max_value / 2; // Remove half period to determine (+/-) sign of the wrap std::int64_t dif = c64 - store; // prev + (current - prev) = current - // wrap difference from -HALF_PERIOD to HALF_PERIOD. modulo prevents differences after the wrap from having an incorrect result + // Wrap difference from -HALF_PERIOD to HALF_PERIOD. The modulo prevents differences after the wrap from having an incorrect result std::int64_t mod_dif = (dif + max_value / 2) % max_value - max_value / 2; - if (dif < -max_value / 2) mod_dif += max_value; // account for mod of negative number behavior in C + if (dif < -max_value / 2) mod_dif += max_value; // Account for the behavior of the modulo operator with negative numbers in C++ std::int64_t unwrapped = store + mod_dif; store = unwrapped; @@ -51,18 +51,13 @@ namespace mrover { } auto QuadratureEncoderReader::update() -> void { - std::uint32_t elapsed_count = std::exchange(__HAL_TIM_GetCounter(m_elapsed_timer), 0); + Seconds elapsed_time = cycle_time(m_elapsed_timer, CLOCK_FREQ); std::int64_t delta_ticks = count_delta(m_counts_unwrapped_prev, m_tick_timer); - - // if (delta_ticks == 0 || elapsed_count == 0) return; - Radians delta_angle = m_multiplier * Ticks{delta_ticks} / RELATIVE_CPR; - Seconds elapsed_time = 1 / CLOCK_FREQ * elapsed_count; m_position += delta_angle; // m_velocity = delta_angle / elapsed_time; m_velocity_filter.add_reading(delta_angle / elapsed_time); } - } // namespace mrover diff --git a/src/esw/fw/bdcmc/Core/Src/hbridge.cpp b/src/esw/fw/bdcmc/Core/Src/hbridge.cpp index 2ca1e595a..3e5f48e2c 100644 --- a/src/esw/fw/bdcmc/Core/Src/hbridge.cpp +++ b/src/esw/fw/bdcmc/Core/Src/hbridge.cpp @@ -5,9 +5,9 @@ namespace mrover { - HBridge::HBridge(TIM_HandleTypeDef* timer, Pin forward_pin, Pin reverse_pin) - : m_forward_pins{forward_pin}, - m_reverse_pins{reverse_pin}, + HBridge::HBridge(TIM_HandleTypeDef* timer, Pin positive_pin, Pin negative_pin) + : m_positive_pin{positive_pin}, + m_negative_pin{negative_pin}, m_timer{timer}, m_max_pwm{0_percent} { @@ -16,18 +16,22 @@ namespace mrover { HAL_TIM_PWM_Start(m_timer, m_channel); } - void HBridge::write(Percent output) const { + auto HBridge::write(Percent output) const -> void { // Set direction pins/duty cycle set_direction_pins(output); set_duty_cycle(output, m_max_pwm); } - void HBridge::set_direction_pins(Percent duty_cycle) const { - m_forward_pins.write(duty_cycle < 0_percent ? GPIO_PIN_SET : GPIO_PIN_RESET); - m_reverse_pins.write(duty_cycle > 0_percent ? GPIO_PIN_SET : GPIO_PIN_RESET); + auto HBridge::set_direction_pins(Percent duty_cycle) const -> void { + // TODO(quintin): Guthrie says only one of these pins is actually used? + GPIO_PinState positive_state = duty_cycle > 0_percent ? GPIO_PIN_SET : GPIO_PIN_RESET; + GPIO_PinState negative_state = duty_cycle < 0_percent ? GPIO_PIN_SET : GPIO_PIN_RESET; + if (m_is_inverted) std::swap(positive_state, negative_state); + m_positive_pin.write(positive_state); + m_negative_pin.write(negative_state); } - void HBridge::set_duty_cycle(Percent duty_cycle, Percent max_duty_cycle) const { + auto HBridge::set_duty_cycle(Percent duty_cycle, Percent max_duty_cycle) const -> void { // Clamp absolute value of the duty cycle to the supported range duty_cycle = std::clamp(abs(duty_cycle), 0_percent, max_duty_cycle); @@ -39,8 +43,12 @@ namespace mrover { // TODO(quintin) we should error if the registers are null pointers } - void HBridge::change_max_pwm(Percent max_pwm) { + auto HBridge::change_max_pwm(Percent max_pwm) -> void { m_max_pwm = max_pwm; } + auto HBridge::change_inverted(bool inverted) -> void { + m_is_inverted = inverted; + } + } // namespace mrover \ No newline at end of file diff --git a/src/esw/fw/bdcmc/Core/Src/main.c b/src/esw/fw/bdcmc/Core/Src/main.c index 3a51135d8..f0f572550 100644 --- a/src/esw/fw/bdcmc/Core/Src/main.c +++ b/src/esw/fw/bdcmc/Core/Src/main.c @@ -47,11 +47,11 @@ I2C_HandleTypeDef hi2c1; DMA_HandleTypeDef hdma_i2c1_rx; DMA_HandleTypeDef hdma_i2c1_tx; -TIM_HandleTypeDef htim1; TIM_HandleTypeDef htim2; TIM_HandleTypeDef htim3; -TIM_HandleTypeDef htim4; +TIM_HandleTypeDef htim6; TIM_HandleTypeDef htim7; +TIM_HandleTypeDef htim8; TIM_HandleTypeDef htim15; TIM_HandleTypeDef htim16; TIM_HandleTypeDef htim17; @@ -67,13 +67,13 @@ static void MX_DMA_Init(void); static void MX_FDCAN1_Init(void); static void MX_I2C1_Init(void); static void MX_TIM15_Init(void); -static void MX_TIM4_Init(void); static void MX_TIM3_Init(void); static void MX_TIM7_Init(void); static void MX_TIM16_Init(void); static void MX_TIM2_Init(void); -static void MX_TIM1_Init(void); static void MX_TIM17_Init(void); +static void MX_TIM6_Init(void); +static void MX_TIM8_Init(void); /* USER CODE BEGIN PFP */ void HAL_PostInit(); /* USER CODE END PFP */ @@ -143,13 +143,13 @@ int main(void) MX_FDCAN1_Init(); MX_I2C1_Init(); MX_TIM15_Init(); - MX_TIM4_Init(); MX_TIM3_Init(); MX_TIM7_Init(); MX_TIM16_Init(); MX_TIM2_Init(); - MX_TIM1_Init(); MX_TIM17_Init(); + MX_TIM6_Init(); + MX_TIM8_Init(); /* USER CODE BEGIN 2 */ HAL_PostInit(); @@ -244,7 +244,7 @@ static void MX_FDCAN1_Init(void) hfdcan1.Init.DataTimeSeg1 = 14; hfdcan1.Init.DataTimeSeg2 = 13; hfdcan1.Init.StdFiltersNbr = 0; - hfdcan1.Init.ExtFiltersNbr = 0; + hfdcan1.Init.ExtFiltersNbr = 1; hfdcan1.Init.TxFifoQueueMode = FDCAN_TX_FIFO_OPERATION; if (HAL_FDCAN_Init(&hfdcan1) != HAL_OK) { @@ -272,7 +272,7 @@ static void MX_I2C1_Init(void) /* USER CODE END I2C1_Init 1 */ hi2c1.Instance = I2C1; - hi2c1.Init.Timing = 0x20B0CCFF; + hi2c1.Init.Timing = 0x00D04BFF; hi2c1.Init.OwnAddress1 = 0; hi2c1.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT; hi2c1.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE; @@ -304,78 +304,6 @@ static void MX_I2C1_Init(void) } -/** - * @brief TIM1 Initialization Function - * @param None - * @retval None - */ -static void MX_TIM1_Init(void) -{ - - /* USER CODE BEGIN TIM1_Init 0 */ - - /* USER CODE END TIM1_Init 0 */ - - TIM_MasterConfigTypeDef sMasterConfig = {0}; - TIM_OC_InitTypeDef sConfigOC = {0}; - TIM_BreakDeadTimeConfigTypeDef sBreakDeadTimeConfig = {0}; - - /* USER CODE BEGIN TIM1_Init 1 */ - - /* USER CODE END TIM1_Init 1 */ - htim1.Instance = TIM1; - htim1.Init.Prescaler = 0; - htim1.Init.CounterMode = TIM_COUNTERMODE_UP; - htim1.Init.Period = 65535; - htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; - htim1.Init.RepetitionCounter = 0; - htim1.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; - if (HAL_TIM_OC_Init(&htim1) != HAL_OK) - { - Error_Handler(); - } - sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; - sMasterConfig.MasterOutputTrigger2 = TIM_TRGO2_RESET; - sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; - if (HAL_TIMEx_MasterConfigSynchronization(&htim1, &sMasterConfig) != HAL_OK) - { - Error_Handler(); - } - sConfigOC.OCMode = TIM_OCMODE_TIMING; - sConfigOC.Pulse = 0; - sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH; - sConfigOC.OCNPolarity = TIM_OCNPOLARITY_HIGH; - sConfigOC.OCFastMode = TIM_OCFAST_DISABLE; - sConfigOC.OCIdleState = TIM_OCIDLESTATE_RESET; - sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_RESET; - if (HAL_TIM_OC_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_1) != HAL_OK) - { - Error_Handler(); - } - sBreakDeadTimeConfig.OffStateRunMode = TIM_OSSR_DISABLE; - sBreakDeadTimeConfig.OffStateIDLEMode = TIM_OSSI_DISABLE; - sBreakDeadTimeConfig.LockLevel = TIM_LOCKLEVEL_OFF; - sBreakDeadTimeConfig.DeadTime = 0; - sBreakDeadTimeConfig.BreakState = TIM_BREAK_DISABLE; - sBreakDeadTimeConfig.BreakPolarity = TIM_BREAKPOLARITY_HIGH; - sBreakDeadTimeConfig.BreakFilter = 0; - sBreakDeadTimeConfig.BreakAFMode = TIM_BREAK_AFMODE_INPUT; - sBreakDeadTimeConfig.Break2State = TIM_BREAK2_DISABLE; - sBreakDeadTimeConfig.Break2Polarity = TIM_BREAK2POLARITY_HIGH; - sBreakDeadTimeConfig.Break2Filter = 0; - sBreakDeadTimeConfig.Break2AFMode = TIM_BREAK_AFMODE_INPUT; - sBreakDeadTimeConfig.AutomaticOutput = TIM_AUTOMATICOUTPUT_DISABLE; - if (HAL_TIMEx_ConfigBreakDeadTime(&htim1, &sBreakDeadTimeConfig) != HAL_OK) - { - Error_Handler(); - } - /* USER CODE BEGIN TIM1_Init 2 */ - - /* USER CODE END TIM1_Init 2 */ - HAL_TIM_MspPostInit(&htim1); - -} - /** * @brief TIM2 Initialization Function * @param None @@ -471,51 +399,40 @@ static void MX_TIM3_Init(void) } /** - * @brief TIM4 Initialization Function + * @brief TIM6 Initialization Function * @param None * @retval None */ -static void MX_TIM4_Init(void) +static void MX_TIM6_Init(void) { - /* USER CODE BEGIN TIM4_Init 0 */ + /* USER CODE BEGIN TIM6_Init 0 */ - /* USER CODE END TIM4_Init 0 */ + /* USER CODE END TIM6_Init 0 */ - TIM_Encoder_InitTypeDef sConfig = {0}; TIM_MasterConfigTypeDef sMasterConfig = {0}; - /* USER CODE BEGIN TIM4_Init 1 */ + /* USER CODE BEGIN TIM6_Init 1 */ - /* USER CODE END TIM4_Init 1 */ - htim4.Instance = TIM4; - htim4.Init.Prescaler = 0; - htim4.Init.CounterMode = TIM_COUNTERMODE_UP; - htim4.Init.Period = 65535; - htim4.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; - htim4.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; - sConfig.EncoderMode = TIM_ENCODERMODE_TI1; - sConfig.IC1Polarity = TIM_ICPOLARITY_RISING; - sConfig.IC1Selection = TIM_ICSELECTION_DIRECTTI; - sConfig.IC1Prescaler = TIM_ICPSC_DIV1; - sConfig.IC1Filter = 0; - sConfig.IC2Polarity = TIM_ICPOLARITY_RISING; - sConfig.IC2Selection = TIM_ICSELECTION_DIRECTTI; - sConfig.IC2Prescaler = TIM_ICPSC_DIV1; - sConfig.IC2Filter = 0; - if (HAL_TIM_Encoder_Init(&htim4, &sConfig) != HAL_OK) + /* USER CODE END TIM6_Init 1 */ + htim6.Instance = TIM6; + htim6.Init.Prescaler = 0; + htim6.Init.CounterMode = TIM_COUNTERMODE_UP; + htim6.Init.Period = 65535; + htim6.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; + if (HAL_TIM_Base_Init(&htim6) != HAL_OK) { Error_Handler(); } sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; - if (HAL_TIMEx_MasterConfigSynchronization(&htim4, &sMasterConfig) != HAL_OK) + if (HAL_TIMEx_MasterConfigSynchronization(&htim6, &sMasterConfig) != HAL_OK) { Error_Handler(); } - /* USER CODE BEGIN TIM4_Init 2 */ + /* USER CODE BEGIN TIM6_Init 2 */ - /* USER CODE END TIM4_Init 2 */ + /* USER CODE END TIM6_Init 2 */ } @@ -557,6 +474,53 @@ static void MX_TIM7_Init(void) } +/** + * @brief TIM8 Initialization Function + * @param None + * @retval None + */ +static void MX_TIM8_Init(void) +{ + + /* USER CODE BEGIN TIM8_Init 0 */ + + /* USER CODE END TIM8_Init 0 */ + + TIM_ClockConfigTypeDef sClockSourceConfig = {0}; + TIM_MasterConfigTypeDef sMasterConfig = {0}; + + /* USER CODE BEGIN TIM8_Init 1 */ + + /* USER CODE END TIM8_Init 1 */ + htim8.Instance = TIM8; + htim8.Init.Prescaler = 0; + htim8.Init.CounterMode = TIM_COUNTERMODE_UP; + htim8.Init.Period = 65535; + htim8.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; + htim8.Init.RepetitionCounter = 0; + htim8.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; + if (HAL_TIM_Base_Init(&htim8) != HAL_OK) + { + Error_Handler(); + } + sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL; + if (HAL_TIM_ConfigClockSource(&htim8, &sClockSourceConfig) != HAL_OK) + { + Error_Handler(); + } + sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; + sMasterConfig.MasterOutputTrigger2 = TIM_TRGO2_RESET; + sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; + if (HAL_TIMEx_MasterConfigSynchronization(&htim8, &sMasterConfig) != HAL_OK) + { + Error_Handler(); + } + /* USER CODE BEGIN TIM8_Init 2 */ + + /* USER CODE END TIM8_Init 2 */ + +} + /** * @brief TIM15 Initialization Function * @param None @@ -766,13 +730,13 @@ static void MX_GPIO_Init(void) /*Configure GPIO pins : LIMIT_0_0_Pin LIMIT_0_1_Pin LIMIT_0_2_Pin LIMIT_1_0_Pin */ GPIO_InitStruct.Pin = LIMIT_0_0_Pin|LIMIT_0_1_Pin|LIMIT_0_2_Pin|LIMIT_1_0_Pin; GPIO_InitStruct.Mode = GPIO_MODE_INPUT; - GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Pull = GPIO_PULLUP; HAL_GPIO_Init(GPIOC, &GPIO_InitStruct); /*Configure GPIO pin : LIMIT_0_3_Pin */ GPIO_InitStruct.Pin = LIMIT_0_3_Pin; GPIO_InitStruct.Mode = GPIO_MODE_INPUT; - GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Pull = GPIO_PULLUP; HAL_GPIO_Init(LIMIT_0_3_GPIO_Port, &GPIO_InitStruct); /*Configure GPIO pins : DEBUG_LED_0_Pin DEBUG_LED_1_Pin DEBUG_LED_2_Pin CAN_STANDBY_Pin */ @@ -785,7 +749,7 @@ static void MX_GPIO_Init(void) /*Configure GPIO pins : LIMIT_1_1_Pin LIMIT_1_2_Pin LIMIT_1_3_Pin */ GPIO_InitStruct.Pin = LIMIT_1_1_Pin|LIMIT_1_2_Pin|LIMIT_1_3_Pin; GPIO_InitStruct.Mode = GPIO_MODE_INPUT; - GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Pull = GPIO_PULLUP; HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); /*Configure GPIO pin : MOTOR_0_DIR_Pin */ diff --git a/src/esw/fw/bdcmc/Core/Src/stm32g4xx_hal_msp.c b/src/esw/fw/bdcmc/Core/Src/stm32g4xx_hal_msp.c index 12d98f61d..eb37487cb 100644 --- a/src/esw/fw/bdcmc/Core/Src/stm32g4xx_hal_msp.c +++ b/src/esw/fw/bdcmc/Core/Src/stm32g4xx_hal_msp.c @@ -63,7 +63,7 @@ extern DMA_HandleTypeDef hdma_i2c1_tx; /* USER CODE END 0 */ void HAL_TIM_MspPostInit(TIM_HandleTypeDef *htim); - /** + /** * Initializes the Global MSP. */ void HAL_MspInit(void) @@ -200,10 +200,14 @@ void HAL_I2C_MspInit(I2C_HandleTypeDef* hi2c) GPIO_InitStruct.Pin = GPIO_PIN_8|GPIO_PIN_9; GPIO_InitStruct.Mode = GPIO_MODE_AF_OD; GPIO_InitStruct.Pull = GPIO_NOPULL; - GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH; GPIO_InitStruct.Alternate = GPIO_AF4_I2C1; HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); + __HAL_SYSCFG_FASTMODEPLUS_ENABLE(SYSCFG_FASTMODEPLUS_PB8); + + __HAL_SYSCFG_FASTMODEPLUS_ENABLE(SYSCFG_FASTMODEPLUS_PB9); + /* Peripheral clock enable */ __HAL_RCC_I2C1_CLK_ENABLE(); @@ -292,33 +296,6 @@ void HAL_I2C_MspDeInit(I2C_HandleTypeDef* hi2c) } -/** -* @brief TIM_OC MSP Initialization -* This function configures the hardware resources used in this example -* @param htim_oc: TIM_OC handle pointer -* @retval None -*/ -void HAL_TIM_OC_MspInit(TIM_HandleTypeDef* htim_oc) -{ - if(htim_oc->Instance==TIM1) - { - /* USER CODE BEGIN TIM1_MspInit 0 */ - - /* USER CODE END TIM1_MspInit 0 */ - /* Peripheral clock enable */ - __HAL_RCC_TIM1_CLK_ENABLE(); - /* TIM1 interrupt Init */ - HAL_NVIC_SetPriority(TIM1_UP_TIM16_IRQn, 1, 0); - HAL_NVIC_EnableIRQ(TIM1_UP_TIM16_IRQn); - HAL_NVIC_SetPriority(TIM1_TRG_COM_TIM17_IRQn, 0, 0); - HAL_NVIC_EnableIRQ(TIM1_TRG_COM_TIM17_IRQn); - /* USER CODE BEGIN TIM1_MspInit 1 */ - - /* USER CODE END TIM1_MspInit 1 */ - } - -} - /** * @brief TIM_Base MSP Initialization * This function configures the hardware resources used in this example @@ -341,6 +318,20 @@ void HAL_TIM_Base_MspInit(TIM_HandleTypeDef* htim_base) /* USER CODE END TIM2_MspInit 1 */ } + else if(htim_base->Instance==TIM6) + { + /* USER CODE BEGIN TIM6_MspInit 0 */ + + /* USER CODE END TIM6_MspInit 0 */ + /* Peripheral clock enable */ + __HAL_RCC_TIM6_CLK_ENABLE(); + /* TIM6 interrupt Init */ + HAL_NVIC_SetPriority(TIM6_DAC_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(TIM6_DAC_IRQn); + /* USER CODE BEGIN TIM6_MspInit 1 */ + + /* USER CODE END TIM6_MspInit 1 */ + } else if(htim_base->Instance==TIM7) { /* USER CODE BEGIN TIM7_MspInit 0 */ @@ -355,6 +346,17 @@ void HAL_TIM_Base_MspInit(TIM_HandleTypeDef* htim_base) /* USER CODE END TIM7_MspInit 1 */ } + else if(htim_base->Instance==TIM8) + { + /* USER CODE BEGIN TIM8_MspInit 0 */ + + /* USER CODE END TIM8_MspInit 0 */ + /* Peripheral clock enable */ + __HAL_RCC_TIM8_CLK_ENABLE(); + /* USER CODE BEGIN TIM8_MspInit 1 */ + + /* USER CODE END TIM8_MspInit 1 */ + } else if(htim_base->Instance==TIM16) { /* USER CODE BEGIN TIM16_MspInit 0 */ @@ -419,30 +421,6 @@ void HAL_TIM_Encoder_MspInit(TIM_HandleTypeDef* htim_encoder) /* USER CODE END TIM3_MspInit 1 */ } - else if(htim_encoder->Instance==TIM4) - { - /* USER CODE BEGIN TIM4_MspInit 0 */ - - /* USER CODE END TIM4_MspInit 0 */ - /* Peripheral clock enable */ - __HAL_RCC_TIM4_CLK_ENABLE(); - - __HAL_RCC_GPIOB_CLK_ENABLE(); - /**TIM4 GPIO Configuration - PB6 ------> TIM4_CH1 - PB7 ------> TIM4_CH2 - */ - GPIO_InitStruct.Pin = QUAD_0_A_Pin|QUAD_0_B_Pin; - GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; - GPIO_InitStruct.Pull = GPIO_NOPULL; - GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; - GPIO_InitStruct.Alternate = GPIO_AF2_TIM4; - HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); - - /* USER CODE BEGIN TIM4_MspInit 1 */ - - /* USER CODE END TIM4_MspInit 1 */ - } } @@ -471,27 +449,7 @@ void HAL_TIM_PWM_MspInit(TIM_HandleTypeDef* htim_pwm) void HAL_TIM_MspPostInit(TIM_HandleTypeDef* htim) { GPIO_InitTypeDef GPIO_InitStruct = {0}; - if(htim->Instance==TIM1) - { - /* USER CODE BEGIN TIM1_MspPostInit 0 */ - - /* USER CODE END TIM1_MspPostInit 0 */ - __HAL_RCC_GPIOA_CLK_ENABLE(); - /**TIM1 GPIO Configuration - PA8 ------> TIM1_CH1 - */ - GPIO_InitStruct.Pin = MOTOR_1_PWM_Pin; - GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; - GPIO_InitStruct.Pull = GPIO_NOPULL; - GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; - GPIO_InitStruct.Alternate = GPIO_AF6_TIM1; - HAL_GPIO_Init(MOTOR_1_PWM_GPIO_Port, &GPIO_InitStruct); - - /* USER CODE BEGIN TIM1_MspPostInit 1 */ - - /* USER CODE END TIM1_MspPostInit 1 */ - } - else if(htim->Instance==TIM15) + if(htim->Instance==TIM15) { /* USER CODE BEGIN TIM15_MspPostInit 0 */ @@ -514,46 +472,6 @@ void HAL_TIM_MspPostInit(TIM_HandleTypeDef* htim) } } -/** -* @brief TIM_OC MSP De-Initialization -* This function freeze the hardware resources used in this example -* @param htim_oc: TIM_OC handle pointer -* @retval None -*/ -void HAL_TIM_OC_MspDeInit(TIM_HandleTypeDef* htim_oc) -{ - if(htim_oc->Instance==TIM1) - { - /* USER CODE BEGIN TIM1_MspDeInit 0 */ - - /* USER CODE END TIM1_MspDeInit 0 */ - /* Peripheral clock disable */ - __HAL_RCC_TIM1_CLK_DISABLE(); - - /* TIM1 interrupt DeInit */ - /* USER CODE BEGIN TIM1:TIM1_UP_TIM16_IRQn disable */ - /** - * Uncomment the line below to disable the "TIM1_UP_TIM16_IRQn" interrupt - * Be aware, disabling shared interrupt may affect other IPs - */ - /* HAL_NVIC_DisableIRQ(TIM1_UP_TIM16_IRQn); */ - /* USER CODE END TIM1:TIM1_UP_TIM16_IRQn disable */ - - /* USER CODE BEGIN TIM1:TIM1_TRG_COM_TIM17_IRQn disable */ - /** - * Uncomment the line below to disable the "TIM1_TRG_COM_TIM17_IRQn" interrupt - * Be aware, disabling shared interrupt may affect other IPs - */ - /* HAL_NVIC_DisableIRQ(TIM1_TRG_COM_TIM17_IRQn); */ - /* USER CODE END TIM1:TIM1_TRG_COM_TIM17_IRQn disable */ - - /* USER CODE BEGIN TIM1_MspDeInit 1 */ - - /* USER CODE END TIM1_MspDeInit 1 */ - } - -} - /** * @brief TIM_Base MSP De-Initialization * This function freeze the hardware resources used in this example @@ -576,6 +494,20 @@ void HAL_TIM_Base_MspDeInit(TIM_HandleTypeDef* htim_base) /* USER CODE END TIM2_MspDeInit 1 */ } + else if(htim_base->Instance==TIM6) + { + /* USER CODE BEGIN TIM6_MspDeInit 0 */ + + /* USER CODE END TIM6_MspDeInit 0 */ + /* Peripheral clock disable */ + __HAL_RCC_TIM6_CLK_DISABLE(); + + /* TIM6 interrupt DeInit */ + HAL_NVIC_DisableIRQ(TIM6_DAC_IRQn); + /* USER CODE BEGIN TIM6_MspDeInit 1 */ + + /* USER CODE END TIM6_MspDeInit 1 */ + } else if(htim_base->Instance==TIM7) { /* USER CODE BEGIN TIM7_MspDeInit 0 */ @@ -590,6 +522,17 @@ void HAL_TIM_Base_MspDeInit(TIM_HandleTypeDef* htim_base) /* USER CODE END TIM7_MspDeInit 1 */ } + else if(htim_base->Instance==TIM8) + { + /* USER CODE BEGIN TIM8_MspDeInit 0 */ + + /* USER CODE END TIM8_MspDeInit 0 */ + /* Peripheral clock disable */ + __HAL_RCC_TIM8_CLK_DISABLE(); + /* USER CODE BEGIN TIM8_MspDeInit 1 */ + + /* USER CODE END TIM8_MspDeInit 1 */ + } else if(htim_base->Instance==TIM16) { /* USER CODE BEGIN TIM16_MspDeInit 0 */ @@ -599,14 +542,7 @@ void HAL_TIM_Base_MspDeInit(TIM_HandleTypeDef* htim_base) __HAL_RCC_TIM16_CLK_DISABLE(); /* TIM16 interrupt DeInit */ - /* USER CODE BEGIN TIM16:TIM1_UP_TIM16_IRQn disable */ - /** - * Uncomment the line below to disable the "TIM1_UP_TIM16_IRQn" interrupt - * Be aware, disabling shared interrupt may affect other IPs - */ - /* HAL_NVIC_DisableIRQ(TIM1_UP_TIM16_IRQn); */ - /* USER CODE END TIM16:TIM1_UP_TIM16_IRQn disable */ - + HAL_NVIC_DisableIRQ(TIM1_UP_TIM16_IRQn); /* USER CODE BEGIN TIM16_MspDeInit 1 */ /* USER CODE END TIM16_MspDeInit 1 */ @@ -620,14 +556,7 @@ void HAL_TIM_Base_MspDeInit(TIM_HandleTypeDef* htim_base) __HAL_RCC_TIM17_CLK_DISABLE(); /* TIM17 interrupt DeInit */ - /* USER CODE BEGIN TIM17:TIM1_TRG_COM_TIM17_IRQn disable */ - /** - * Uncomment the line below to disable the "TIM1_TRG_COM_TIM17_IRQn" interrupt - * Be aware, disabling shared interrupt may affect other IPs - */ - /* HAL_NVIC_DisableIRQ(TIM1_TRG_COM_TIM17_IRQn); */ - /* USER CODE END TIM17:TIM1_TRG_COM_TIM17_IRQn disable */ - + HAL_NVIC_DisableIRQ(TIM1_TRG_COM_TIM17_IRQn); /* USER CODE BEGIN TIM17_MspDeInit 1 */ /* USER CODE END TIM17_MspDeInit 1 */ @@ -661,24 +590,6 @@ void HAL_TIM_Encoder_MspDeInit(TIM_HandleTypeDef* htim_encoder) /* USER CODE END TIM3_MspDeInit 1 */ } - else if(htim_encoder->Instance==TIM4) - { - /* USER CODE BEGIN TIM4_MspDeInit 0 */ - - /* USER CODE END TIM4_MspDeInit 0 */ - /* Peripheral clock disable */ - __HAL_RCC_TIM4_CLK_DISABLE(); - - /**TIM4 GPIO Configuration - PB6 ------> TIM4_CH1 - PB7 ------> TIM4_CH2 - */ - HAL_GPIO_DeInit(GPIOB, QUAD_0_A_Pin|QUAD_0_B_Pin); - - /* USER CODE BEGIN TIM4_MspDeInit 1 */ - - /* USER CODE END TIM4_MspDeInit 1 */ - } } diff --git a/src/esw/fw/bdcmc/Core/Src/stm32g4xx_it.c b/src/esw/fw/bdcmc/Core/Src/stm32g4xx_it.c index 1fafa705a..eabb9dc4f 100644 --- a/src/esw/fw/bdcmc/Core/Src/stm32g4xx_it.c +++ b/src/esw/fw/bdcmc/Core/Src/stm32g4xx_it.c @@ -59,8 +59,8 @@ extern FDCAN_HandleTypeDef hfdcan1; extern DMA_HandleTypeDef hdma_i2c1_rx; extern DMA_HandleTypeDef hdma_i2c1_tx; extern I2C_HandleTypeDef hi2c1; -extern TIM_HandleTypeDef htim1; extern TIM_HandleTypeDef htim2; +extern TIM_HandleTypeDef htim6; extern TIM_HandleTypeDef htim7; extern TIM_HandleTypeDef htim16; extern TIM_HandleTypeDef htim17; @@ -256,7 +256,6 @@ void TIM1_UP_TIM16_IRQHandler(void) /* USER CODE BEGIN TIM1_UP_TIM16_IRQn 0 */ /* USER CODE END TIM1_UP_TIM16_IRQn 0 */ - HAL_TIM_IRQHandler(&htim1); HAL_TIM_IRQHandler(&htim16); /* USER CODE BEGIN TIM1_UP_TIM16_IRQn 1 */ @@ -271,7 +270,6 @@ void TIM1_TRG_COM_TIM17_IRQHandler(void) /* USER CODE BEGIN TIM1_TRG_COM_TIM17_IRQn 0 */ /* USER CODE END TIM1_TRG_COM_TIM17_IRQn 0 */ - HAL_TIM_IRQHandler(&htim1); HAL_TIM_IRQHandler(&htim17); /* USER CODE BEGIN TIM1_TRG_COM_TIM17_IRQn 1 */ @@ -320,6 +318,20 @@ void I2C1_ER_IRQHandler(void) /* USER CODE END I2C1_ER_IRQn 1 */ } +/** + * @brief This function handles TIM6 global interrupt, DAC1 and DAC3 channel underrun error interrupts. + */ +void TIM6_DAC_IRQHandler(void) +{ + /* USER CODE BEGIN TIM6_DAC_IRQn 0 */ + + /* USER CODE END TIM6_DAC_IRQn 0 */ + HAL_TIM_IRQHandler(&htim6); + /* USER CODE BEGIN TIM6_DAC_IRQn 1 */ + + /* USER CODE END TIM6_DAC_IRQn 1 */ +} + /** * @brief This function handles TIM7 global interrupt. */ diff --git a/src/esw/fw/bdcmc/Drivers/CMSIS/Device/ST/STM32G4xx/Include/stm32g431xx.h b/src/esw/fw/bdcmc/Drivers/CMSIS/Device/ST/STM32G4xx/Include/stm32g431xx.h index 7e3265dcc..e370c6a81 100644 --- a/src/esw/fw/bdcmc/Drivers/CMSIS/Device/ST/STM32G4xx/Include/stm32g431xx.h +++ b/src/esw/fw/bdcmc/Drivers/CMSIS/Device/ST/STM32G4xx/Include/stm32g431xx.h @@ -699,8 +699,7 @@ typedef struct typedef struct { - __IO uint32_t GCR; /*!< SAI global configuration register, Address offset: 0x00 */ - uint32_t RESERVED[16]; /*!< Reserved, Address offset: 0x04 to 0x40 */ + uint32_t RESERVED[17]; /*!< Reserved, Address offset: 0x00 to 0x40 */ __IO uint32_t PDMCR; /*!< SAI PDM control register, Address offset: 0x44 */ __IO uint32_t PDMDLY; /*!< SAI PDM delay register, Address offset: 0x48 */ } SAI_TypeDef; @@ -1216,7 +1215,7 @@ typedef struct /******************************************************************************/ /* - * @brief Specific device feature definitions (not present on all devices in the STM32G4 serie) + * @brief Specific device feature definitions (not present on all devices in the STM32G4 series) */ #define ADC_MULTIMODE_SUPPORT /*!< ADC feature available only on specific devices: multimode available on devices with several ADC instances */ @@ -1362,7 +1361,7 @@ typedef struct #define ADC_CFGR_AUTDLY ADC_CFGR_AUTDLY_Msk /*!< ADC low power auto wait */ #define ADC_CFGR_ALIGN_Pos (15U) #define ADC_CFGR_ALIGN_Msk (0x1UL << ADC_CFGR_ALIGN_Pos) /*!< 0x00008000 */ -#define ADC_CFGR_ALIGN ADC_CFGR_ALIGN_Msk /*!< ADC data alignement */ +#define ADC_CFGR_ALIGN ADC_CFGR_ALIGN_Msk /*!< ADC data alignment */ #define ADC_CFGR_DISCEN_Pos (16U) #define ADC_CFGR_DISCEN_Msk (0x1UL << ADC_CFGR_DISCEN_Pos) /*!< 0x00010000 */ #define ADC_CFGR_DISCEN ADC_CFGR_DISCEN_Msk /*!< ADC group regular sequencer discontinuous mode */ @@ -4063,7 +4062,7 @@ typedef struct /***************** Bit definition for FDCAN_ENDN register *******************/ #define FDCAN_ENDN_ETV_Pos (0U) #define FDCAN_ENDN_ETV_Msk (0xFFFFFFFFUL << FDCAN_ENDN_ETV_Pos) /*!< 0xFFFFFFFF */ -#define FDCAN_ENDN_ETV FDCAN_ENDN_ETV_Msk /*!State = HAL_FDCAN_STATE_RESET; \ (__HANDLE__)->MspInitCallback = NULL; \ (__HANDLE__)->MspDeInitCallback = NULL; \ @@ -1164,7 +1171,7 @@ HAL_StatusTypeDef HAL_FDCAN_UnRegisterErrorStatusCallback(FDCAN_HandleTypeDef *h * @{ */ /* Configuration functions ****************************************************/ -HAL_StatusTypeDef HAL_FDCAN_ConfigFilter(FDCAN_HandleTypeDef *hfdcan, FDCAN_FilterTypeDef *sFilterConfig); +HAL_StatusTypeDef HAL_FDCAN_ConfigFilter(FDCAN_HandleTypeDef *hfdcan, const FDCAN_FilterTypeDef *sFilterConfig); HAL_StatusTypeDef HAL_FDCAN_ConfigGlobalFilter(FDCAN_HandleTypeDef *hfdcan, uint32_t NonMatchingStd, uint32_t NonMatchingExt, uint32_t RejectRemoteStd, uint32_t RejectRemoteExt); @@ -1174,13 +1181,13 @@ HAL_StatusTypeDef HAL_FDCAN_ConfigRamWatchdog(FDCAN_HandleTypeDef *hfdcan, uint3 HAL_StatusTypeDef HAL_FDCAN_ConfigTimestampCounter(FDCAN_HandleTypeDef *hfdcan, uint32_t TimestampPrescaler); HAL_StatusTypeDef HAL_FDCAN_EnableTimestampCounter(FDCAN_HandleTypeDef *hfdcan, uint32_t TimestampOperation); HAL_StatusTypeDef HAL_FDCAN_DisableTimestampCounter(FDCAN_HandleTypeDef *hfdcan); -uint16_t HAL_FDCAN_GetTimestampCounter(FDCAN_HandleTypeDef *hfdcan); +uint16_t HAL_FDCAN_GetTimestampCounter(const FDCAN_HandleTypeDef *hfdcan); HAL_StatusTypeDef HAL_FDCAN_ResetTimestampCounter(FDCAN_HandleTypeDef *hfdcan); HAL_StatusTypeDef HAL_FDCAN_ConfigTimeoutCounter(FDCAN_HandleTypeDef *hfdcan, uint32_t TimeoutOperation, uint32_t TimeoutPeriod); HAL_StatusTypeDef HAL_FDCAN_EnableTimeoutCounter(FDCAN_HandleTypeDef *hfdcan); HAL_StatusTypeDef HAL_FDCAN_DisableTimeoutCounter(FDCAN_HandleTypeDef *hfdcan); -uint16_t HAL_FDCAN_GetTimeoutCounter(FDCAN_HandleTypeDef *hfdcan); +uint16_t HAL_FDCAN_GetTimeoutCounter(const FDCAN_HandleTypeDef *hfdcan); HAL_StatusTypeDef HAL_FDCAN_ResetTimeoutCounter(FDCAN_HandleTypeDef *hfdcan); HAL_StatusTypeDef HAL_FDCAN_ConfigTxDelayCompensation(FDCAN_HandleTypeDef *hfdcan, uint32_t TdcOffset, uint32_t TdcFilter); @@ -1200,21 +1207,23 @@ HAL_StatusTypeDef HAL_FDCAN_DisableEdgeFiltering(FDCAN_HandleTypeDef *hfdcan); /* Control functions **********************************************************/ HAL_StatusTypeDef HAL_FDCAN_Start(FDCAN_HandleTypeDef *hfdcan); HAL_StatusTypeDef HAL_FDCAN_Stop(FDCAN_HandleTypeDef *hfdcan); -HAL_StatusTypeDef HAL_FDCAN_AddMessageToTxFifoQ(FDCAN_HandleTypeDef *hfdcan, FDCAN_TxHeaderTypeDef *pTxHeader, - uint8_t *pTxData); -uint32_t HAL_FDCAN_GetLatestTxFifoQRequestBuffer(FDCAN_HandleTypeDef *hfdcan); +HAL_StatusTypeDef HAL_FDCAN_AddMessageToTxFifoQ(FDCAN_HandleTypeDef *hfdcan, const FDCAN_TxHeaderTypeDef *pTxHeader, + const uint8_t *pTxData); +uint32_t HAL_FDCAN_GetLatestTxFifoQRequestBuffer(const FDCAN_HandleTypeDef *hfdcan); HAL_StatusTypeDef HAL_FDCAN_AbortTxRequest(FDCAN_HandleTypeDef *hfdcan, uint32_t BufferIndex); HAL_StatusTypeDef HAL_FDCAN_GetRxMessage(FDCAN_HandleTypeDef *hfdcan, uint32_t RxLocation, FDCAN_RxHeaderTypeDef *pRxHeader, uint8_t *pRxData); HAL_StatusTypeDef HAL_FDCAN_GetTxEvent(FDCAN_HandleTypeDef *hfdcan, FDCAN_TxEventFifoTypeDef *pTxEvent); -HAL_StatusTypeDef HAL_FDCAN_GetHighPriorityMessageStatus(FDCAN_HandleTypeDef *hfdcan, +HAL_StatusTypeDef HAL_FDCAN_GetHighPriorityMessageStatus(const FDCAN_HandleTypeDef *hfdcan, FDCAN_HpMsgStatusTypeDef *HpMsgStatus); -HAL_StatusTypeDef HAL_FDCAN_GetProtocolStatus(FDCAN_HandleTypeDef *hfdcan, FDCAN_ProtocolStatusTypeDef *ProtocolStatus); -HAL_StatusTypeDef HAL_FDCAN_GetErrorCounters(FDCAN_HandleTypeDef *hfdcan, FDCAN_ErrorCountersTypeDef *ErrorCounters); -uint32_t HAL_FDCAN_IsTxBufferMessagePending(FDCAN_HandleTypeDef *hfdcan, uint32_t TxBufferIndex); -uint32_t HAL_FDCAN_GetRxFifoFillLevel(FDCAN_HandleTypeDef *hfdcan, uint32_t RxFifo); -uint32_t HAL_FDCAN_GetTxFifoFreeLevel(FDCAN_HandleTypeDef *hfdcan); -uint32_t HAL_FDCAN_IsRestrictedOperationMode(FDCAN_HandleTypeDef *hfdcan); +HAL_StatusTypeDef HAL_FDCAN_GetProtocolStatus(const FDCAN_HandleTypeDef *hfdcan, + FDCAN_ProtocolStatusTypeDef *ProtocolStatus); +HAL_StatusTypeDef HAL_FDCAN_GetErrorCounters(const FDCAN_HandleTypeDef *hfdcan, + FDCAN_ErrorCountersTypeDef *ErrorCounters); +uint32_t HAL_FDCAN_IsTxBufferMessagePending(const FDCAN_HandleTypeDef *hfdcan, uint32_t TxBufferIndex); +uint32_t HAL_FDCAN_GetRxFifoFillLevel(const FDCAN_HandleTypeDef *hfdcan, uint32_t RxFifo); +uint32_t HAL_FDCAN_GetTxFifoFreeLevel(const FDCAN_HandleTypeDef *hfdcan); +uint32_t HAL_FDCAN_IsRestrictedOperationMode(const FDCAN_HandleTypeDef *hfdcan); HAL_StatusTypeDef HAL_FDCAN_ExitRestrictedOperationMode(FDCAN_HandleTypeDef *hfdcan); /** * @} @@ -1256,8 +1265,8 @@ void HAL_FDCAN_ErrorStatusCallback(FDCAN_HandleTypeDef *hfdcan, uint32_t ErrorSt * @{ */ /* Peripheral State functions *************************************************/ -uint32_t HAL_FDCAN_GetError(FDCAN_HandleTypeDef *hfdcan); -HAL_FDCAN_StateTypeDef HAL_FDCAN_GetState(FDCAN_HandleTypeDef *hfdcan); +uint32_t HAL_FDCAN_GetError(const FDCAN_HandleTypeDef *hfdcan); +HAL_FDCAN_StateTypeDef HAL_FDCAN_GetState(const FDCAN_HandleTypeDef *hfdcan); /** * @} */ @@ -1406,6 +1415,10 @@ HAL_FDCAN_StateTypeDef HAL_FDCAN_GetState(FDCAN_HandleTypeDef *hfdcan); ((OPERATION) == FDCAN_TIMEOUT_TX_EVENT_FIFO) || \ ((OPERATION) == FDCAN_TIMEOUT_RX_FIFO0 ) || \ ((OPERATION) == FDCAN_TIMEOUT_RX_FIFO1 )) + +#define FDCAN_CHECK_IT_SOURCE(__IE__, __IT__) ((((__IE__) & (__IT__)) == (__IT__)) ? SET : RESET) + +#define FDCAN_CHECK_FLAG(__IR__, __FLAG__) ((((__IR__) & (__FLAG__)) == (__FLAG__)) ? SET : RESET) /** * @} */ diff --git a/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_hal_flash.h b/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_hal_flash.h index f13aa0b8a..f25764035 100644 --- a/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_hal_flash.h +++ b/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_hal_flash.h @@ -873,7 +873,8 @@ HAL_StatusTypeDef FLASH_WaitForLastOperation(uint32_t Timeout); #define FLASH_SIZE ((((*((uint16_t *)FLASH_SIZE_DATA_REGISTER)) == 0xFFFFU)) ? (0x200UL << 10U) : \ (((*((uint32_t *)FLASH_SIZE_DATA_REGISTER)) & 0xFFFFUL) << 10U)) #define FLASH_BANK_SIZE (FLASH_SIZE >> 1) -#define FLASH_PAGE_NB 128U +#define FLASH_PAGE_NB ((FLASH_SIZE == 0x00080000U) ? 128U : \ + ((FLASH_SIZE == 0x00040000U) ? 64U : 32U)) #define FLASH_PAGE_SIZE_128_BITS 0x1000U /* 4 KB */ #else #define FLASH_SIZE ((((*((uint16_t *)FLASH_SIZE_DATA_REGISTER)) == 0xFFFFU)) ? (0x80UL << 10U) : \ diff --git a/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_hal_i2c.h b/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_hal_i2c.h index a2ae59124..84bd7bb77 100644 --- a/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_hal_i2c.h +++ b/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_hal_i2c.h @@ -118,8 +118,6 @@ typedef enum HAL_I2C_STATE_BUSY_RX_LISTEN = 0x2AU, /*!< Address Listen Mode and Data Reception process is ongoing */ HAL_I2C_STATE_ABORT = 0x60U, /*!< Abort user request ongoing */ - HAL_I2C_STATE_TIMEOUT = 0xA0U, /*!< Timeout state */ - HAL_I2C_STATE_ERROR = 0xE0U /*!< Error */ } HAL_I2C_StateTypeDef; @@ -207,6 +205,7 @@ typedef struct __I2C_HandleTypeDef DMA_HandleTypeDef *hdmarx; /*!< I2C Rx DMA handle parameters */ + HAL_LockTypeDef Lock; /*!< I2C locking object */ __IO HAL_I2C_StateTypeDef State; /*!< I2C communication state */ @@ -217,6 +216,10 @@ typedef struct __I2C_HandleTypeDef __IO uint32_t AddrEventCount; /*!< I2C Address Event counter */ + __IO uint32_t Devaddress; /*!< I2C Target device address */ + + __IO uint32_t Memaddress; /*!< I2C Target memory address */ + #if (USE_HAL_I2C_REGISTER_CALLBACKS == 1) void (* MasterTxCpltCallback)(struct __I2C_HandleTypeDef *hi2c); /*!< I2C Master Tx Transfer completed callback */ @@ -705,9 +708,9 @@ void HAL_I2C_AbortCpltCallback(I2C_HandleTypeDef *hi2c); * @{ */ /* Peripheral State, Mode and Error functions *********************************/ -HAL_I2C_StateTypeDef HAL_I2C_GetState(I2C_HandleTypeDef *hi2c); -HAL_I2C_ModeTypeDef HAL_I2C_GetMode(I2C_HandleTypeDef *hi2c); -uint32_t HAL_I2C_GetError(I2C_HandleTypeDef *hi2c); +HAL_I2C_StateTypeDef HAL_I2C_GetState(const I2C_HandleTypeDef *hi2c); +HAL_I2C_ModeTypeDef HAL_I2C_GetMode(const I2C_HandleTypeDef *hi2c); +uint32_t HAL_I2C_GetError(const I2C_HandleTypeDef *hi2c); /** * @} @@ -800,8 +803,8 @@ uint32_t HAL_I2C_GetError(I2C_HandleTypeDef *hi2c); (I2C_CR2_START) | (I2C_CR2_AUTOEND)) & \ (~I2C_CR2_RD_WRN)) : \ (uint32_t)((((uint32_t)(__ADDRESS__) & (I2C_CR2_SADD)) | \ - (I2C_CR2_ADD10) | (I2C_CR2_START)) & \ - (~I2C_CR2_RD_WRN))) + (I2C_CR2_ADD10) | (I2C_CR2_START) | \ + (I2C_CR2_AUTOEND)) & (~I2C_CR2_RD_WRN))) #define I2C_CHECK_FLAG(__ISR__, __FLAG__) ((((__ISR__) & ((__FLAG__) & I2C_FLAG_MASK)) == \ ((__FLAG__) & I2C_FLAG_MASK)) ? SET : RESET) diff --git a/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_hal_rcc.h b/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_hal_rcc.h index c0969dc9d..3415d2039 100644 --- a/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_hal_rcc.h +++ b/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_hal_rcc.h @@ -1042,7 +1042,7 @@ typedef struct #define __HAL_RCC_RTCAPB_CLK_DISABLE() CLEAR_BIT(RCC->APB1ENR1, RCC_APB1ENR1_RTCAPBEN); -#define __HAL_RCC_WWDG_CLK_DISABLE() CLEAR_BIT(RCC->APB1ENR1, RCC_APB1ENR1_WWDG2EN) +#define __HAL_RCC_WWDG_CLK_DISABLE() CLEAR_BIT(RCC->APB1ENR1, RCC_APB1ENR1_WWDGEN) #define __HAL_RCC_SPI2_CLK_DISABLE() CLEAR_BIT(RCC->APB1ENR1, RCC_APB1ENR1_SPI2EN) @@ -3107,7 +3107,7 @@ typedef struct * @arg @ref RCC_MCO1SOURCE_NOCLOCK MCO output disabled * @arg @ref RCC_MCO1SOURCE_SYSCLK System clock selected as MCO source * @arg @ref RCC_MCO1SOURCE_HSI HSI clock selected as MCO source - * @arg @ref RCC_MCO1SOURCE_HSE HSE clock selected as MCO sourcee + * @arg @ref RCC_MCO1SOURCE_HSE HSE clock selected as MCO source * @arg @ref RCC_MCO1SOURCE_PLLCLK Main PLL clock selected as MCO source * @arg @ref RCC_MCO1SOURCE_LSI LSI clock selected as MCO source * @arg @ref RCC_MCO1SOURCE_LSE LSE clock selected as MCO source diff --git a/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_hal_rcc_ex.h b/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_hal_rcc_ex.h index 3dcc2f807..714355e3a 100644 --- a/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_hal_rcc_ex.h +++ b/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_hal_rcc_ex.h @@ -515,7 +515,7 @@ typedef struct /** @defgroup RCCEx_CRS_HSI48CalibrationDefault RCCEx CRS HSI48CalibrationDefault * @{ */ -#define RCC_CRS_HSI48CALIBRATION_DEFAULT 0x00000020U /*!< The default value is 32, which corresponds to the middle of the trimming interval. +#define RCC_CRS_HSI48CALIBRATION_DEFAULT 0x00000040U /*!< The default value is 64, which corresponds to the middle of the trimming interval. The trimming step is around 67 kHz between two consecutive TRIM steps. A higher TRIM value corresponds to a higher output frequency */ /** diff --git a/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_hal_tim.h b/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_hal_tim.h index acc47aa1e..64caee8f6 100644 --- a/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_hal_tim.h +++ b/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_hal_tim.h @@ -416,29 +416,28 @@ typedef struct */ typedef enum { - HAL_TIM_BASE_MSPINIT_CB_ID = 0x00U /*!< TIM Base MspInit Callback ID */ - , HAL_TIM_BASE_MSPDEINIT_CB_ID = 0x01U /*!< TIM Base MspDeInit Callback ID */ - , HAL_TIM_IC_MSPINIT_CB_ID = 0x02U /*!< TIM IC MspInit Callback ID */ - , HAL_TIM_IC_MSPDEINIT_CB_ID = 0x03U /*!< TIM IC MspDeInit Callback ID */ - , HAL_TIM_OC_MSPINIT_CB_ID = 0x04U /*!< TIM OC MspInit Callback ID */ - , HAL_TIM_OC_MSPDEINIT_CB_ID = 0x05U /*!< TIM OC MspDeInit Callback ID */ - , HAL_TIM_PWM_MSPINIT_CB_ID = 0x06U /*!< TIM PWM MspInit Callback ID */ - , HAL_TIM_PWM_MSPDEINIT_CB_ID = 0x07U /*!< TIM PWM MspDeInit Callback ID */ - , HAL_TIM_ONE_PULSE_MSPINIT_CB_ID = 0x08U /*!< TIM One Pulse MspInit Callback ID */ - , HAL_TIM_ONE_PULSE_MSPDEINIT_CB_ID = 0x09U /*!< TIM One Pulse MspDeInit Callback ID */ - , HAL_TIM_ENCODER_MSPINIT_CB_ID = 0x0AU /*!< TIM Encoder MspInit Callback ID */ - , HAL_TIM_ENCODER_MSPDEINIT_CB_ID = 0x0BU /*!< TIM Encoder MspDeInit Callback ID */ - , HAL_TIM_HALL_SENSOR_MSPINIT_CB_ID = 0x0CU /*!< TIM Hall Sensor MspDeInit Callback ID */ - , HAL_TIM_HALL_SENSOR_MSPDEINIT_CB_ID = 0x0DU /*!< TIM Hall Sensor MspDeInit Callback ID */ + HAL_TIM_BASE_MSPINIT_CB_ID = 0x00U /*!< TIM Base MspInit Callback ID */ + , HAL_TIM_BASE_MSPDEINIT_CB_ID = 0x01U /*!< TIM Base MspDeInit Callback ID */ + , HAL_TIM_IC_MSPINIT_CB_ID = 0x02U /*!< TIM IC MspInit Callback ID */ + , HAL_TIM_IC_MSPDEINIT_CB_ID = 0x03U /*!< TIM IC MspDeInit Callback ID */ + , HAL_TIM_OC_MSPINIT_CB_ID = 0x04U /*!< TIM OC MspInit Callback ID */ + , HAL_TIM_OC_MSPDEINIT_CB_ID = 0x05U /*!< TIM OC MspDeInit Callback ID */ + , HAL_TIM_PWM_MSPINIT_CB_ID = 0x06U /*!< TIM PWM MspInit Callback ID */ + , HAL_TIM_PWM_MSPDEINIT_CB_ID = 0x07U /*!< TIM PWM MspDeInit Callback ID */ + , HAL_TIM_ONE_PULSE_MSPINIT_CB_ID = 0x08U /*!< TIM One Pulse MspInit Callback ID */ + , HAL_TIM_ONE_PULSE_MSPDEINIT_CB_ID = 0x09U /*!< TIM One Pulse MspDeInit Callback ID */ + , HAL_TIM_ENCODER_MSPINIT_CB_ID = 0x0AU /*!< TIM Encoder MspInit Callback ID */ + , HAL_TIM_ENCODER_MSPDEINIT_CB_ID = 0x0BU /*!< TIM Encoder MspDeInit Callback ID */ + , HAL_TIM_HALL_SENSOR_MSPINIT_CB_ID = 0x0CU /*!< TIM Hall Sensor MspDeInit Callback ID */ + , HAL_TIM_HALL_SENSOR_MSPDEINIT_CB_ID = 0x0DU /*!< TIM Hall Sensor MspDeInit Callback ID */ , HAL_TIM_PERIOD_ELAPSED_CB_ID = 0x0EU /*!< TIM Period Elapsed Callback ID */ , HAL_TIM_PERIOD_ELAPSED_HALF_CB_ID = 0x0FU /*!< TIM Period Elapsed half complete Callback ID */ , HAL_TIM_TRIGGER_CB_ID = 0x10U /*!< TIM Trigger Callback ID */ , HAL_TIM_TRIGGER_HALF_CB_ID = 0x11U /*!< TIM Trigger half complete Callback ID */ - , HAL_TIM_IC_CAPTURE_CB_ID = 0x12U /*!< TIM Input Capture Callback ID */ , HAL_TIM_IC_CAPTURE_HALF_CB_ID = 0x13U /*!< TIM Input Capture half complete Callback ID */ , HAL_TIM_OC_DELAY_ELAPSED_CB_ID = 0x14U /*!< TIM Output Compare Delay Elapsed Callback ID */ - , HAL_TIM_PWM_PULSE_FINISHED_CB_ID = 0x15U /*!< TIM PWM Pulse Finished Callback ID */ + , HAL_TIM_PWM_PULSE_FINISHED_CB_ID = 0x15U /*!< TIM PWM Pulse Finished Callback ID */ , HAL_TIM_PWM_PULSE_FINISHED_HALF_CB_ID = 0x16U /*!< TIM PWM Pulse Finished half complete Callback ID */ , HAL_TIM_ERROR_CB_ID = 0x17U /*!< TIM Error Callback ID */ , HAL_TIM_COMMUTATION_CB_ID = 0x18U /*!< TIM Commutation Callback ID */ @@ -781,6 +780,15 @@ typedef void (*pTIM_CallbackTypeDef)(TIM_HandleTypeDef *htim); /*!< pointer to * @} */ +/** @defgroup TIM_CC_DMA_Request CCx DMA request selection + * @{ + */ +#define TIM_CCDMAREQUEST_CC 0x00000000U /*!< CCx DMA request sent when capture or compare match event occurs */ +#define TIM_CCDMAREQUEST_UPDATE TIM_CR2_CCDS /*!< CCx DMA requests sent when update event occurs */ +/** + * @} + */ + /** @defgroup TIM_Flag_definition TIM Flag Definition * @{ */ @@ -825,16 +833,16 @@ typedef void (*pTIM_CallbackTypeDef)(TIM_HandleTypeDef *htim); /*!< pointer to /** @defgroup TIM_Clock_Source TIM Clock Source * @{ */ -#define TIM_CLOCKSOURCE_ETRMODE2 TIM_SMCR_ETPS_1 /*!< External clock source mode 2 */ #define TIM_CLOCKSOURCE_INTERNAL TIM_SMCR_ETPS_0 /*!< Internal clock source */ +#define TIM_CLOCKSOURCE_ETRMODE1 TIM_TS_ETRF /*!< External clock source mode 1 (ETRF) */ +#define TIM_CLOCKSOURCE_ETRMODE2 TIM_SMCR_ETPS_1 /*!< External clock source mode 2 */ +#define TIM_CLOCKSOURCE_TI1ED TIM_TS_TI1F_ED /*!< External clock source mode 1 (TTI1FP1 + edge detect.) */ +#define TIM_CLOCKSOURCE_TI1 TIM_TS_TI1FP1 /*!< External clock source mode 1 (TTI1FP1) */ +#define TIM_CLOCKSOURCE_TI2 TIM_TS_TI2FP2 /*!< External clock source mode 1 (TTI2FP2) */ #define TIM_CLOCKSOURCE_ITR0 TIM_TS_ITR0 /*!< External clock source mode 1 (ITR0) */ #define TIM_CLOCKSOURCE_ITR1 TIM_TS_ITR1 /*!< External clock source mode 1 (ITR1) */ #define TIM_CLOCKSOURCE_ITR2 TIM_TS_ITR2 /*!< External clock source mode 1 (ITR2) */ #define TIM_CLOCKSOURCE_ITR3 TIM_TS_ITR3 /*!< External clock source mode 1 (ITR3) */ -#define TIM_CLOCKSOURCE_TI1ED TIM_TS_TI1F_ED /*!< External clock source mode 1 (TTI1FP1 + edge detect.) */ -#define TIM_CLOCKSOURCE_TI1 TIM_TS_TI1FP1 /*!< External clock source mode 1 (TTI1FP1) */ -#define TIM_CLOCKSOURCE_TI2 TIM_TS_TI2FP2 /*!< External clock source mode 1 (TTI2FP2) */ -#define TIM_CLOCKSOURCE_ETRMODE1 TIM_TS_ETRF /*!< External clock source mode 1 (ETRF) */ #if defined (TIM5) #define TIM_CLOCKSOURCE_ITR4 TIM_TS_ITR4 /*!< External clock source mode 1 (ITR4) */ #endif /* TIM5 */ @@ -1073,8 +1081,8 @@ typedef void (*pTIM_CallbackTypeDef)(TIM_HandleTypeDef *htim); /*!< pointer to #define TIM_OCMODE_RETRIGERRABLE_OPM2 (TIM_CCMR1_OC1M_3 | TIM_CCMR1_OC1M_0) /*!< Retrigerrable OPM mode 2 */ #define TIM_OCMODE_COMBINED_PWM1 (TIM_CCMR1_OC1M_3 | TIM_CCMR1_OC1M_2) /*!< Combined PWM mode 1 */ #define TIM_OCMODE_COMBINED_PWM2 (TIM_CCMR1_OC1M_3 | TIM_CCMR1_OC1M_0 | TIM_CCMR1_OC1M_2) /*!< Combined PWM mode 2 */ -#define TIM_OCMODE_ASSYMETRIC_PWM1 (TIM_CCMR1_OC1M_3 | TIM_CCMR1_OC1M_1 | TIM_CCMR1_OC1M_2) /*!< Asymmetric PWM mode 1 */ -#define TIM_OCMODE_ASSYMETRIC_PWM2 TIM_CCMR1_OC1M /*!< Asymmetric PWM mode 2 */ +#define TIM_OCMODE_ASYMMETRIC_PWM1 (TIM_CCMR1_OC1M_3 | TIM_CCMR1_OC1M_1 | TIM_CCMR1_OC1M_2) /*!< Asymmetric PWM mode 1 */ +#define TIM_OCMODE_ASYMMETRIC_PWM2 TIM_CCMR1_OC1M /*!< Asymmetric PWM mode 2 */ #define TIM_OCMODE_PULSE_ON_COMPARE (TIM_CCMR2_OC3M_3 | TIM_CCMR2_OC3M_1) /*!< Pulse on compare (CH3&CH4 only) */ #define TIM_OCMODE_DIRECTION_OUTPUT (TIM_CCMR2_OC3M_3 | TIM_CCMR2_OC3M_1 | TIM_CCMR2_OC3M_0) /*!< Direction output (CH3&CH4 only) */ /** @@ -1088,10 +1096,6 @@ typedef void (*pTIM_CallbackTypeDef)(TIM_HandleTypeDef *htim); /*!< pointer to #define TIM_TS_ITR1 TIM_SMCR_TS_0 /*!< Internal Trigger 1 (ITR1) */ #define TIM_TS_ITR2 TIM_SMCR_TS_1 /*!< Internal Trigger 2 (ITR2) */ #define TIM_TS_ITR3 (TIM_SMCR_TS_0 | TIM_SMCR_TS_1) /*!< Internal Trigger 3 (ITR3) */ -#define TIM_TS_TI1F_ED TIM_SMCR_TS_2 /*!< TI1 Edge Detector (TI1F_ED) */ -#define TIM_TS_TI1FP1 (TIM_SMCR_TS_0 | TIM_SMCR_TS_2) /*!< Filtered Timer Input 1 (TI1FP1) */ -#define TIM_TS_TI2FP2 (TIM_SMCR_TS_1 | TIM_SMCR_TS_2) /*!< Filtered Timer Input 2 (TI2FP2) */ -#define TIM_TS_ETRF (TIM_SMCR_TS_0 | TIM_SMCR_TS_1 | TIM_SMCR_TS_2) /*!< Filtered External Trigger input (ETRF) */ #if defined (TIM5) #define TIM_TS_ITR4 TIM_SMCR_TS_3 /*!< Internal Trigger 4 (ITR9) */ #endif /* TIM5 */ @@ -1104,7 +1108,11 @@ typedef void (*pTIM_CallbackTypeDef)(TIM_HandleTypeDef *htim); /*!< pointer to #endif /* TIM20 */ #define TIM_TS_ITR10 (TIM_SMCR_TS_1 | TIM_SMCR_TS_2 | TIM_SMCR_TS_3) /*!< Internal Trigger 10 (ITR10) */ #define TIM_TS_ITR11 (TIM_SMCR_TS_0 | TIM_SMCR_TS_1 | TIM_SMCR_TS_2 | TIM_SMCR_TS_3) /*!< Internal Trigger 11 (ITR11) */ -#define TIM_TS_NONE 0xFFFFFFFFU /*!< No trigger selected */ +#define TIM_TS_TI1F_ED TIM_SMCR_TS_2 /*!< TI1 Edge Detector (TI1F_ED) */ +#define TIM_TS_TI1FP1 (TIM_SMCR_TS_0 | TIM_SMCR_TS_2) /*!< Filtered Timer Input 1 (TI1FP1) */ +#define TIM_TS_TI2FP2 (TIM_SMCR_TS_1 | TIM_SMCR_TS_2) /*!< Filtered Timer Input 2 (TI2FP2) */ +#define TIM_TS_ETRF (TIM_SMCR_TS_0 | TIM_SMCR_TS_1 | TIM_SMCR_TS_2) /*!< Filtered External Trigger input (ETRF) */ +#define TIM_TS_NONE 0x0000FFFFU /*!< No trigger selected */ /** * @} */ @@ -1825,6 +1833,17 @@ mode. TIM_SET_CAPTUREPOLARITY((__HANDLE__), (__CHANNEL__), (__POLARITY__)); \ }while(0) +/** @brief Select the Capture/compare DMA request source. + * @param __HANDLE__ specifies the TIM Handle. + * @param __CCDMA__ specifies Capture/compare DMA request source + * This parameter can be one of the following values: + * @arg TIM_CCDMAREQUEST_CC: CCx DMA request generated on Capture/Compare event + * @arg TIM_CCDMAREQUEST_UPDATE: CCx DMA request generated on Update event + * @retval None + */ +#define __HAL_TIM_SELECT_CCDMAREQUEST(__HANDLE__, __CCDMA__) \ + MODIFY_REG((__HANDLE__)->Instance->CR2, TIM_CR2_CCDS, (__CCDMA__)) + /** * @} */ @@ -1903,7 +1922,7 @@ mode. ((__MODE__) == TIM_COUNTERMODE_CENTERALIGNED3)) #define IS_TIM_UIFREMAP_MODE(__MODE__) (((__MODE__) == TIM_UIFREMAP_DISABLE) || \ - ((__MODE__) == TIM_UIFREMAP_ENALE)) + ((__MODE__) == TIM_UIFREMAP_ENABLE)) #define IS_TIM_CLOCKDIVISION_DIV(__DIV__) (((__DIV__) == TIM_CLOCKDIVISION_DIV1) || \ ((__DIV__) == TIM_CLOCKDIVISION_DIV2) || \ @@ -1943,6 +1962,10 @@ mode. ((__PRESCALER__) == TIM_ICPSC_DIV4) || \ ((__PRESCALER__) == TIM_ICPSC_DIV8)) +#define IS_TIM_CCX_CHANNEL(__INSTANCE__, __CHANNEL__) (IS_TIM_CCX_INSTANCE(__INSTANCE__, __CHANNEL__) && \ + ((__CHANNEL__) != (TIM_CHANNEL_5)) && \ + ((__CHANNEL__) != (TIM_CHANNEL_6))) + #define IS_TIM_OPM_MODE(__MODE__) (((__MODE__) == TIM_OPMODE_SINGLE) || \ ((__MODE__) == TIM_OPMODE_REPETITIVE)) @@ -1969,6 +1992,10 @@ mode. #define IS_TIM_OPM_CHANNELS(__CHANNEL__) (((__CHANNEL__) == TIM_CHANNEL_1) || \ ((__CHANNEL__) == TIM_CHANNEL_2)) +#define IS_TIM_PERIOD(__HANDLE__, __PERIOD__) ((IS_TIM_32B_COUNTER_INSTANCE(((__HANDLE__)->Instance)) == 0U) ? \ + (((__PERIOD__) > 0U) && ((__PERIOD__) <= 0x0000FFFFU)) : \ + ((__PERIOD__) > 0U)) + #define IS_TIM_COMPLEMENTARY_CHANNELS(__CHANNEL__) (((__CHANNEL__) == TIM_CHANNEL_1) || \ ((__CHANNEL__) == TIM_CHANNEL_2) || \ ((__CHANNEL__) == TIM_CHANNEL_3) || \ @@ -1976,15 +2003,15 @@ mode. #if defined(TIM5) && defined(TIM20) #define IS_TIM_CLOCKSOURCE(__CLOCK__) (((__CLOCK__) == TIM_CLOCKSOURCE_INTERNAL) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE2) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_TI1ED) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_TI1) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_TI2) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR0) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR2) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR3) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_TI1ED) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_TI1) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_TI2) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR4) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR5) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR6) || \ @@ -1995,15 +2022,15 @@ mode. ((__CLOCK__) == TIM_CLOCKSOURCE_ITR11)) #elif defined(TIM5) #define IS_TIM_CLOCKSOURCE(__CLOCK__) (((__CLOCK__) == TIM_CLOCKSOURCE_INTERNAL) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE2) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_TI1ED) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_TI1) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_TI2) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR0) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR2) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR3) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_TI1ED) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_TI1) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_TI2) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR4) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR5) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR6) || \ @@ -2012,15 +2039,15 @@ mode. ((__CLOCK__) == TIM_CLOCKSOURCE_ITR11)) #elif defined(TIM20) #define IS_TIM_CLOCKSOURCE(__CLOCK__) (((__CLOCK__) == TIM_CLOCKSOURCE_INTERNAL) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE2) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_TI1ED) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_TI1) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_TI2) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR0) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR2) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR3) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_TI1ED) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_TI1) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_TI2) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR5) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR6) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR7) || \ @@ -2029,15 +2056,15 @@ mode. ((__CLOCK__) == TIM_CLOCKSOURCE_ITR11)) #else #define IS_TIM_CLOCKSOURCE(__CLOCK__) (((__CLOCK__) == TIM_CLOCKSOURCE_INTERNAL) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE2) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_TI1ED) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_TI1) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_TI2) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR0) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR2) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR3) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_TI1ED) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_TI1) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_TI2) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR5) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR6) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR7) || \ @@ -2081,7 +2108,6 @@ mode. #define IS_TIM_BREAK_FILTER(__BRKFILTER__) ((__BRKFILTER__) <= 0xFUL) - #define IS_TIM_BREAK_STATE(__STATE__) (((__STATE__) == TIM_BREAK_ENABLE) || \ ((__STATE__) == TIM_BREAK_DISABLE)) @@ -2150,8 +2176,8 @@ mode. ((__MODE__) == TIM_OCMODE_PWM2) || \ ((__MODE__) == TIM_OCMODE_COMBINED_PWM1) || \ ((__MODE__) == TIM_OCMODE_COMBINED_PWM2) || \ - ((__MODE__) == TIM_OCMODE_ASSYMETRIC_PWM1) || \ - ((__MODE__) == TIM_OCMODE_ASSYMETRIC_PWM2)) + ((__MODE__) == TIM_OCMODE_ASYMMETRIC_PWM1) || \ + ((__MODE__) == TIM_OCMODE_ASYMMETRIC_PWM2)) #define IS_TIM_OC_MODE(__MODE__) (((__MODE__) == TIM_OCMODE_TIMING) || \ ((__MODE__) == TIM_OCMODE_ACTIVE) || \ @@ -2164,59 +2190,6 @@ mode. ((__MODE__) == TIM_OCMODE_DIRECTION_OUTPUT) || \ ((__MODE__) == TIM_OCMODE_PULSE_ON_COMPARE)) -#if defined (TIM5) && defined(TIM20) - -#define IS_TIM_INTERNAL_TRIGGEREVENT_SELECTION(__SELECTION__) (((__SELECTION__) == TIM_TS_ITR0) || \ - ((__SELECTION__) == TIM_TS_ITR1) || \ - ((__SELECTION__) == TIM_TS_ITR2) || \ - ((__SELECTION__) == TIM_TS_ITR3) || \ - ((__SELECTION__) == TIM_TS_ITR4) || \ - ((__SELECTION__) == TIM_TS_ITR5) || \ - ((__SELECTION__) == TIM_TS_ITR6) || \ - ((__SELECTION__) == TIM_TS_ITR7) || \ - ((__SELECTION__) == TIM_TS_ITR8) || \ - ((__SELECTION__) == TIM_TS_ITR9) || \ - ((__SELECTION__) == TIM_TS_ITR10)|| \ - ((__SELECTION__) == TIM_TS_ITR11)|| \ - ((__SELECTION__) == TIM_TS_NONE)) -#elif defined (TIM5) -#define IS_TIM_INTERNAL_TRIGGEREVENT_SELECTION(__SELECTION__) (((__SELECTION__) == TIM_TS_ITR0) || \ - ((__SELECTION__) == TIM_TS_ITR1) || \ - ((__SELECTION__) == TIM_TS_ITR2) || \ - ((__SELECTION__) == TIM_TS_ITR3) || \ - ((__SELECTION__) == TIM_TS_ITR4) || \ - ((__SELECTION__) == TIM_TS_ITR5) || \ - ((__SELECTION__) == TIM_TS_ITR6) || \ - ((__SELECTION__) == TIM_TS_ITR7) || \ - ((__SELECTION__) == TIM_TS_ITR8) || \ - ((__SELECTION__) == TIM_TS_ITR10)|| \ - ((__SELECTION__) == TIM_TS_ITR11)|| \ - ((__SELECTION__) == TIM_TS_NONE)) -#elif defined (TIM20) -#define IS_TIM_INTERNAL_TRIGGEREVENT_SELECTION(__SELECTION__) (((__SELECTION__) == TIM_TS_ITR0) || \ - ((__SELECTION__) == TIM_TS_ITR1) || \ - ((__SELECTION__) == TIM_TS_ITR2) || \ - ((__SELECTION__) == TIM_TS_ITR3) || \ - ((__SELECTION__) == TIM_TS_ITR5) || \ - ((__SELECTION__) == TIM_TS_ITR6) || \ - ((__SELECTION__) == TIM_TS_ITR7) || \ - ((__SELECTION__) == TIM_TS_ITR8) || \ - ((__SELECTION__) == TIM_TS_ITR9) || \ - ((__SELECTION__) == TIM_TS_ITR11)|| \ - ((__SELECTION__) == TIM_TS_NONE)) -#else -#define IS_TIM_INTERNAL_TRIGGEREVENT_SELECTION(__SELECTION__) (((__SELECTION__) == TIM_TS_ITR0) || \ - ((__SELECTION__) == TIM_TS_ITR1) || \ - ((__SELECTION__) == TIM_TS_ITR2) || \ - ((__SELECTION__) == TIM_TS_ITR3) || \ - ((__SELECTION__) == TIM_TS_ITR5) || \ - ((__SELECTION__) == TIM_TS_ITR6) || \ - ((__SELECTION__) == TIM_TS_ITR7) || \ - ((__SELECTION__) == TIM_TS_ITR8) || \ - ((__SELECTION__) == TIM_TS_ITR11)|| \ - ((__SELECTION__) == TIM_TS_NONE)) -#endif /* TIM5 && TIM20 */ - #define IS_TIM_TRIGGERPOLARITY(__POLARITY__) (((__POLARITY__) == TIM_TRIGGERPOLARITY_INVERTED ) || \ ((__POLARITY__) == TIM_TRIGGERPOLARITY_NONINVERTED) || \ ((__POLARITY__) == TIM_TRIGGERPOLARITY_RISING ) || \ @@ -2381,7 +2354,7 @@ HAL_StatusTypeDef HAL_TIM_Base_Stop(TIM_HandleTypeDef *htim); HAL_StatusTypeDef HAL_TIM_Base_Start_IT(TIM_HandleTypeDef *htim); HAL_StatusTypeDef HAL_TIM_Base_Stop_IT(TIM_HandleTypeDef *htim); /* Non-Blocking mode: DMA */ -HAL_StatusTypeDef HAL_TIM_Base_Start_DMA(TIM_HandleTypeDef *htim, uint32_t *pData, uint16_t Length); +HAL_StatusTypeDef HAL_TIM_Base_Start_DMA(TIM_HandleTypeDef *htim, const uint32_t *pData, uint16_t Length); HAL_StatusTypeDef HAL_TIM_Base_Stop_DMA(TIM_HandleTypeDef *htim); /** * @} @@ -2403,7 +2376,8 @@ HAL_StatusTypeDef HAL_TIM_OC_Stop(TIM_HandleTypeDef *htim, uint32_t Channel); HAL_StatusTypeDef HAL_TIM_OC_Start_IT(TIM_HandleTypeDef *htim, uint32_t Channel); HAL_StatusTypeDef HAL_TIM_OC_Stop_IT(TIM_HandleTypeDef *htim, uint32_t Channel); /* Non-Blocking mode: DMA */ -HAL_StatusTypeDef HAL_TIM_OC_Start_DMA(TIM_HandleTypeDef *htim, uint32_t Channel, uint32_t *pData, uint16_t Length); +HAL_StatusTypeDef HAL_TIM_OC_Start_DMA(TIM_HandleTypeDef *htim, uint32_t Channel, const uint32_t *pData, + uint16_t Length); HAL_StatusTypeDef HAL_TIM_OC_Stop_DMA(TIM_HandleTypeDef *htim, uint32_t Channel); /** * @} @@ -2425,7 +2399,8 @@ HAL_StatusTypeDef HAL_TIM_PWM_Stop(TIM_HandleTypeDef *htim, uint32_t Channel); HAL_StatusTypeDef HAL_TIM_PWM_Start_IT(TIM_HandleTypeDef *htim, uint32_t Channel); HAL_StatusTypeDef HAL_TIM_PWM_Stop_IT(TIM_HandleTypeDef *htim, uint32_t Channel); /* Non-Blocking mode: DMA */ -HAL_StatusTypeDef HAL_TIM_PWM_Start_DMA(TIM_HandleTypeDef *htim, uint32_t Channel, uint32_t *pData, uint16_t Length); +HAL_StatusTypeDef HAL_TIM_PWM_Start_DMA(TIM_HandleTypeDef *htim, uint32_t Channel, const uint32_t *pData, + uint16_t Length); HAL_StatusTypeDef HAL_TIM_PWM_Stop_DMA(TIM_HandleTypeDef *htim, uint32_t Channel); /** * @} @@ -2477,7 +2452,7 @@ HAL_StatusTypeDef HAL_TIM_OnePulse_Stop_IT(TIM_HandleTypeDef *htim, uint32_t Out * @{ */ /* Timer Encoder functions ****************************************************/ -HAL_StatusTypeDef HAL_TIM_Encoder_Init(TIM_HandleTypeDef *htim, TIM_Encoder_InitTypeDef *sConfig); +HAL_StatusTypeDef HAL_TIM_Encoder_Init(TIM_HandleTypeDef *htim, const TIM_Encoder_InitTypeDef *sConfig); HAL_StatusTypeDef HAL_TIM_Encoder_DeInit(TIM_HandleTypeDef *htim); void HAL_TIM_Encoder_MspInit(TIM_HandleTypeDef *htim); void HAL_TIM_Encoder_MspDeInit(TIM_HandleTypeDef *htim); @@ -2510,21 +2485,26 @@ void HAL_TIM_IRQHandler(TIM_HandleTypeDef *htim); * @{ */ /* Control functions *********************************************************/ -HAL_StatusTypeDef HAL_TIM_OC_ConfigChannel(TIM_HandleTypeDef *htim, TIM_OC_InitTypeDef *sConfig, uint32_t Channel); -HAL_StatusTypeDef HAL_TIM_PWM_ConfigChannel(TIM_HandleTypeDef *htim, TIM_OC_InitTypeDef *sConfig, uint32_t Channel); -HAL_StatusTypeDef HAL_TIM_IC_ConfigChannel(TIM_HandleTypeDef *htim, TIM_IC_InitTypeDef *sConfig, uint32_t Channel); +HAL_StatusTypeDef HAL_TIM_OC_ConfigChannel(TIM_HandleTypeDef *htim, const TIM_OC_InitTypeDef *sConfig, + uint32_t Channel); +HAL_StatusTypeDef HAL_TIM_PWM_ConfigChannel(TIM_HandleTypeDef *htim, const TIM_OC_InitTypeDef *sConfig, + uint32_t Channel); +HAL_StatusTypeDef HAL_TIM_IC_ConfigChannel(TIM_HandleTypeDef *htim, const TIM_IC_InitTypeDef *sConfig, + uint32_t Channel); HAL_StatusTypeDef HAL_TIM_OnePulse_ConfigChannel(TIM_HandleTypeDef *htim, TIM_OnePulse_InitTypeDef *sConfig, uint32_t OutputChannel, uint32_t InputChannel); -HAL_StatusTypeDef HAL_TIM_ConfigOCrefClear(TIM_HandleTypeDef *htim, TIM_ClearInputConfigTypeDef *sClearInputConfig, +HAL_StatusTypeDef HAL_TIM_ConfigOCrefClear(TIM_HandleTypeDef *htim, + const TIM_ClearInputConfigTypeDef *sClearInputConfig, uint32_t Channel); -HAL_StatusTypeDef HAL_TIM_ConfigClockSource(TIM_HandleTypeDef *htim, TIM_ClockConfigTypeDef *sClockSourceConfig); +HAL_StatusTypeDef HAL_TIM_ConfigClockSource(TIM_HandleTypeDef *htim, const TIM_ClockConfigTypeDef *sClockSourceConfig); HAL_StatusTypeDef HAL_TIM_ConfigTI1Input(TIM_HandleTypeDef *htim, uint32_t TI1_Selection); -HAL_StatusTypeDef HAL_TIM_SlaveConfigSynchro(TIM_HandleTypeDef *htim, TIM_SlaveConfigTypeDef *sSlaveConfig); -HAL_StatusTypeDef HAL_TIM_SlaveConfigSynchro_IT(TIM_HandleTypeDef *htim, TIM_SlaveConfigTypeDef *sSlaveConfig); +HAL_StatusTypeDef HAL_TIM_SlaveConfigSynchro(TIM_HandleTypeDef *htim, const TIM_SlaveConfigTypeDef *sSlaveConfig); +HAL_StatusTypeDef HAL_TIM_SlaveConfigSynchro_IT(TIM_HandleTypeDef *htim, const TIM_SlaveConfigTypeDef *sSlaveConfig); HAL_StatusTypeDef HAL_TIM_DMABurst_WriteStart(TIM_HandleTypeDef *htim, uint32_t BurstBaseAddress, - uint32_t BurstRequestSrc, uint32_t *BurstBuffer, uint32_t BurstLength); + uint32_t BurstRequestSrc, const uint32_t *BurstBuffer, + uint32_t BurstLength); HAL_StatusTypeDef HAL_TIM_DMABurst_MultiWriteStart(TIM_HandleTypeDef *htim, uint32_t BurstBaseAddress, - uint32_t BurstRequestSrc, uint32_t *BurstBuffer, + uint32_t BurstRequestSrc, const uint32_t *BurstBuffer, uint32_t BurstLength, uint32_t DataLength); HAL_StatusTypeDef HAL_TIM_DMABurst_WriteStop(TIM_HandleTypeDef *htim, uint32_t BurstRequestSrc); HAL_StatusTypeDef HAL_TIM_DMABurst_ReadStart(TIM_HandleTypeDef *htim, uint32_t BurstBaseAddress, @@ -2534,7 +2514,7 @@ HAL_StatusTypeDef HAL_TIM_DMABurst_MultiReadStart(TIM_HandleTypeDef *htim, uint3 uint32_t BurstLength, uint32_t DataLength); HAL_StatusTypeDef HAL_TIM_DMABurst_ReadStop(TIM_HandleTypeDef *htim, uint32_t BurstRequestSrc); HAL_StatusTypeDef HAL_TIM_GenerateEvent(TIM_HandleTypeDef *htim, uint32_t EventSource); -uint32_t HAL_TIM_ReadCapturedValue(TIM_HandleTypeDef *htim, uint32_t Channel); +uint32_t HAL_TIM_ReadCapturedValue(const TIM_HandleTypeDef *htim, uint32_t Channel); /** * @} */ @@ -2571,17 +2551,17 @@ HAL_StatusTypeDef HAL_TIM_UnRegisterCallback(TIM_HandleTypeDef *htim, HAL_TIM_Ca * @{ */ /* Peripheral State functions ************************************************/ -HAL_TIM_StateTypeDef HAL_TIM_Base_GetState(TIM_HandleTypeDef *htim); -HAL_TIM_StateTypeDef HAL_TIM_OC_GetState(TIM_HandleTypeDef *htim); -HAL_TIM_StateTypeDef HAL_TIM_PWM_GetState(TIM_HandleTypeDef *htim); -HAL_TIM_StateTypeDef HAL_TIM_IC_GetState(TIM_HandleTypeDef *htim); -HAL_TIM_StateTypeDef HAL_TIM_OnePulse_GetState(TIM_HandleTypeDef *htim); -HAL_TIM_StateTypeDef HAL_TIM_Encoder_GetState(TIM_HandleTypeDef *htim); +HAL_TIM_StateTypeDef HAL_TIM_Base_GetState(const TIM_HandleTypeDef *htim); +HAL_TIM_StateTypeDef HAL_TIM_OC_GetState(const TIM_HandleTypeDef *htim); +HAL_TIM_StateTypeDef HAL_TIM_PWM_GetState(const TIM_HandleTypeDef *htim); +HAL_TIM_StateTypeDef HAL_TIM_IC_GetState(const TIM_HandleTypeDef *htim); +HAL_TIM_StateTypeDef HAL_TIM_OnePulse_GetState(const TIM_HandleTypeDef *htim); +HAL_TIM_StateTypeDef HAL_TIM_Encoder_GetState(const TIM_HandleTypeDef *htim); /* Peripheral Channel state functions ************************************************/ -HAL_TIM_ActiveChannel HAL_TIM_GetActiveChannel(TIM_HandleTypeDef *htim); -HAL_TIM_ChannelStateTypeDef HAL_TIM_GetChannelState(TIM_HandleTypeDef *htim, uint32_t Channel); -HAL_TIM_DMABurstStateTypeDef HAL_TIM_DMABurstState(TIM_HandleTypeDef *htim); +HAL_TIM_ActiveChannel HAL_TIM_GetActiveChannel(const TIM_HandleTypeDef *htim); +HAL_TIM_ChannelStateTypeDef HAL_TIM_GetChannelState(const TIM_HandleTypeDef *htim, uint32_t Channel); +HAL_TIM_DMABurstStateTypeDef HAL_TIM_DMABurstState(const TIM_HandleTypeDef *htim); /** * @} */ @@ -2595,9 +2575,9 @@ HAL_TIM_DMABurstStateTypeDef HAL_TIM_DMABurstState(TIM_HandleTypeDef *htim); /** @defgroup TIM_Private_Functions TIM Private Functions * @{ */ -void TIM_Base_SetConfig(TIM_TypeDef *TIMx, TIM_Base_InitTypeDef *Structure); +void TIM_Base_SetConfig(TIM_TypeDef *TIMx, const TIM_Base_InitTypeDef *Structure); void TIM_TI1_SetConfig(TIM_TypeDef *TIMx, uint32_t TIM_ICPolarity, uint32_t TIM_ICSelection, uint32_t TIM_ICFilter); -void TIM_OC2_SetConfig(TIM_TypeDef *TIMx, TIM_OC_InitTypeDef *OC_Config); +void TIM_OC2_SetConfig(TIM_TypeDef *TIMx, const TIM_OC_InitTypeDef *OC_Config); void TIM_ETR_SetConfig(TIM_TypeDef *TIMx, uint32_t TIM_ExtTRGPrescaler, uint32_t TIM_ExtTRGPolarity, uint32_t ExtTRGFilter); diff --git a/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_hal_tim_ex.h b/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_hal_tim_ex.h index b40d85c60..f314ac86a 100644 --- a/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_hal_tim_ex.h +++ b/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_hal_tim_ex.h @@ -104,153 +104,153 @@ typedef struct /** @defgroup TIMEx_Remap TIM Extended Remapping * @{ */ -#define TIM_TIM1_ETR_GPIO 0x00000000U /* !< ETR input is connected to GPIO */ -#define TIM_TIM1_ETR_COMP1 TIM1_AF1_ETRSEL_0 /* !< ETR input is connected to COMP1_OUT */ -#define TIM_TIM1_ETR_COMP2 TIM1_AF1_ETRSEL_1 /* !< ETR input is connected to COMP2_OUT */ -#define TIM_TIM1_ETR_COMP3 (TIM1_AF1_ETRSEL_1 | TIM1_AF1_ETRSEL_0) /* !< ETR input is connected to COMP3_OUT */ -#define TIM_TIM1_ETR_COMP4 TIM1_AF1_ETRSEL_2 /* !< ETR input is connected to COMP4_OUT */ +#define TIM_TIM1_ETR_GPIO 0x00000000U /*!< ETR input is connected to GPIO */ +#define TIM_TIM1_ETR_COMP1 TIM1_AF1_ETRSEL_0 /*!< ETR input is connected to COMP1_OUT */ +#define TIM_TIM1_ETR_COMP2 TIM1_AF1_ETRSEL_1 /*!< ETR input is connected to COMP2_OUT */ +#define TIM_TIM1_ETR_COMP3 (TIM1_AF1_ETRSEL_1 | TIM1_AF1_ETRSEL_0) /*!< ETR input is connected to COMP3_OUT */ +#define TIM_TIM1_ETR_COMP4 TIM1_AF1_ETRSEL_2 /*!< ETR input is connected to COMP4_OUT */ #if defined(COMP5) -#define TIM_TIM1_ETR_COMP5 (TIM1_AF1_ETRSEL_2 | TIM1_AF1_ETRSEL_0) /* !< ETR input is connected to COMP5_OUT */ +#define TIM_TIM1_ETR_COMP5 (TIM1_AF1_ETRSEL_2 | TIM1_AF1_ETRSEL_0) /*!< ETR input is connected to COMP5_OUT */ #endif /* COMP5 */ #if defined(COMP6) -#define TIM_TIM1_ETR_COMP6 (TIM1_AF1_ETRSEL_2 | TIM1_AF1_ETRSEL_1) /* !< ETR input is connected to COMP6_OUT */ +#define TIM_TIM1_ETR_COMP6 (TIM1_AF1_ETRSEL_2 | TIM1_AF1_ETRSEL_1) /*!< ETR input is connected to COMP6_OUT */ #endif /* COMP6 */ #if defined(COMP7) -#define TIM_TIM1_ETR_COMP7 (TIM1_AF1_ETRSEL_2 | TIM1_AF1_ETRSEL_1 | TIM1_AF1_ETRSEL_0) /* !< ETR input is connected to COMP7_OUT */ +#define TIM_TIM1_ETR_COMP7 (TIM1_AF1_ETRSEL_2 | TIM1_AF1_ETRSEL_1 | TIM1_AF1_ETRSEL_0) /*!< ETR input is connected to COMP7_OUT */ #endif /* COMP7 */ -#define TIM_TIM1_ETR_ADC1_AWD1 TIM1_AF1_ETRSEL_3 /* !< ADC1 analog watchdog 1 */ -#define TIM_TIM1_ETR_ADC1_AWD2 (TIM1_AF1_ETRSEL_3 | TIM1_AF1_ETRSEL_0) /* !< ADC1 analog watchdog 2 */ -#define TIM_TIM1_ETR_ADC1_AWD3 (TIM1_AF1_ETRSEL_3 | TIM1_AF1_ETRSEL_1) /* !< ADC1 analog watchdog 3 */ +#define TIM_TIM1_ETR_ADC1_AWD1 TIM1_AF1_ETRSEL_3 /*!< ADC1 analog watchdog 1 */ +#define TIM_TIM1_ETR_ADC1_AWD2 (TIM1_AF1_ETRSEL_3 | TIM1_AF1_ETRSEL_0) /*!< ADC1 analog watchdog 2 */ +#define TIM_TIM1_ETR_ADC1_AWD3 (TIM1_AF1_ETRSEL_3 | TIM1_AF1_ETRSEL_1) /*!< ADC1 analog watchdog 3 */ #if defined (ADC4) -#define TIM_TIM1_ETR_ADC4_AWD1 (TIM1_AF1_ETRSEL_3 | TIM1_AF1_ETRSEL_1 | TIM1_AF1_ETRSEL_0) /* !< ADC4 analog watchdog 1 */ -#define TIM_TIM1_ETR_ADC4_AWD2 (TIM1_AF1_ETRSEL_3 | TIM1_AF1_ETRSEL_2) /* !< ADC4 analog watchdog 2 */ -#define TIM_TIM1_ETR_ADC4_AWD3 (TIM1_AF1_ETRSEL_3 | TIM1_AF1_ETRSEL_2 | TIM1_AF1_ETRSEL_0) /* !< ADC4 analog watchdog 3 */ +#define TIM_TIM1_ETR_ADC4_AWD1 (TIM1_AF1_ETRSEL_3 | TIM1_AF1_ETRSEL_1 | TIM1_AF1_ETRSEL_0) /*!< ADC4 analog watchdog 1 */ +#define TIM_TIM1_ETR_ADC4_AWD2 (TIM1_AF1_ETRSEL_3 | TIM1_AF1_ETRSEL_2) /*!< ADC4 analog watchdog 2 */ +#define TIM_TIM1_ETR_ADC4_AWD3 (TIM1_AF1_ETRSEL_3 | TIM1_AF1_ETRSEL_2 | TIM1_AF1_ETRSEL_0) /*!< ADC4 analog watchdog 3 */ #endif /* ADC4 */ -#define TIM_TIM2_ETR_GPIO 0x00000000U /* !< ETR input is connected to GPIO */ -#define TIM_TIM2_ETR_COMP1 TIM1_AF1_ETRSEL_0 /* !< ETR input is connected to COMP1_OUT */ -#define TIM_TIM2_ETR_COMP2 TIM1_AF1_ETRSEL_1 /* !< ETR input is connected to COMP2_OUT */ -#define TIM_TIM2_ETR_COMP3 (TIM1_AF1_ETRSEL_1 | TIM1_AF1_ETRSEL_0) /* !< ETR input is connected to COMP3_OUT */ -#define TIM_TIM2_ETR_COMP4 TIM1_AF1_ETRSEL_2 /* !< ETR input is connected to COMP4_OUT */ +#define TIM_TIM2_ETR_GPIO 0x00000000U /*!< ETR input is connected to GPIO */ +#define TIM_TIM2_ETR_COMP1 TIM1_AF1_ETRSEL_0 /*!< ETR input is connected to COMP1_OUT */ +#define TIM_TIM2_ETR_COMP2 TIM1_AF1_ETRSEL_1 /*!< ETR input is connected to COMP2_OUT */ +#define TIM_TIM2_ETR_COMP3 (TIM1_AF1_ETRSEL_1 | TIM1_AF1_ETRSEL_0) /*!< ETR input is connected to COMP3_OUT */ +#define TIM_TIM2_ETR_COMP4 TIM1_AF1_ETRSEL_2 /*!< ETR input is connected to COMP4_OUT */ #if defined(COMP5) -#define TIM_TIM2_ETR_COMP5 (TIM1_AF1_ETRSEL_2 | TIM1_AF1_ETRSEL_0) /* !< ETR input is connected to COMP5_OUT */ +#define TIM_TIM2_ETR_COMP5 (TIM1_AF1_ETRSEL_2 | TIM1_AF1_ETRSEL_0) /*!< ETR input is connected to COMP5_OUT */ #endif /* COMP5 */ #if defined(COMP6) -#define TIM_TIM2_ETR_COMP6 (TIM1_AF1_ETRSEL_2 | TIM1_AF1_ETRSEL_1) /* !< ETR input is connected to COMP6_OUT */ +#define TIM_TIM2_ETR_COMP6 (TIM1_AF1_ETRSEL_2 | TIM1_AF1_ETRSEL_1) /*!< ETR input is connected to COMP6_OUT */ #endif /* COMP6 */ #if defined(COMP7) -#define TIM_TIM2_ETR_COMP7 (TIM1_AF1_ETRSEL_2 | TIM1_AF1_ETRSEL_1 | TIM1_AF1_ETRSEL_0)/* !< ETR input is connected to COMP7_OUT */ +#define TIM_TIM2_ETR_COMP7 (TIM1_AF1_ETRSEL_2 | TIM1_AF1_ETRSEL_1 | TIM1_AF1_ETRSEL_0)/*!< ETR input is connected to COMP7_OUT */ #endif /* COMP7 */ -#define TIM_TIM2_ETR_TIM3_ETR TIM1_AF1_ETRSEL_3 /* !< ETR input is connected to TIM3 ETR */ -#define TIM_TIM2_ETR_TIM4_ETR (TIM1_AF1_ETRSEL_3 | TIM1_AF1_ETRSEL_0) /* !< ETR input is connected to TIM4 ETR */ +#define TIM_TIM2_ETR_TIM3_ETR TIM1_AF1_ETRSEL_3 /*!< ETR input is connected to TIM3 ETR */ +#define TIM_TIM2_ETR_TIM4_ETR (TIM1_AF1_ETRSEL_3 | TIM1_AF1_ETRSEL_0) /*!< ETR input is connected to TIM4 ETR */ #if defined (TIM5) -#define TIM_TIM2_ETR_TIM5_ETR (TIM1_AF1_ETRSEL_3 | TIM1_AF1_ETRSEL_1) /* !< ETR input is connected to TIM5 ETR */ +#define TIM_TIM2_ETR_TIM5_ETR (TIM1_AF1_ETRSEL_3 | TIM1_AF1_ETRSEL_1) /*!< ETR input is connected to TIM5 ETR */ #endif /* TIM5 */ -#define TIM_TIM2_ETR_LSE (TIM1_AF1_ETRSEL_3 | TIM1_AF1_ETRSEL_1 | TIM1_AF1_ETRSEL_0) /* !< ETR input is connected to LSE */ +#define TIM_TIM2_ETR_LSE (TIM1_AF1_ETRSEL_3 | TIM1_AF1_ETRSEL_1 | TIM1_AF1_ETRSEL_0) /*!< ETR input is connected to LSE */ -#define TIM_TIM3_ETR_GPIO 0x00000000U /* !< ETR input is connected to GPIO */ -#define TIM_TIM3_ETR_COMP1 TIM1_AF1_ETRSEL_0 /* !< ETR input is connected to COMP1_OUT */ -#define TIM_TIM3_ETR_COMP2 TIM1_AF1_ETRSEL_1 /* !< ETR input is connected to COMP2_OUT */ -#define TIM_TIM3_ETR_COMP3 (TIM1_AF1_ETRSEL_1 | TIM1_AF1_ETRSEL_0) /* !< ETR input is connected to COMP3_OUT */ -#define TIM_TIM3_ETR_COMP4 TIM1_AF1_ETRSEL_2 /* !< ETR input is connected to COMP4_OUT */ +#define TIM_TIM3_ETR_GPIO 0x00000000U /*!< ETR input is connected to GPIO */ +#define TIM_TIM3_ETR_COMP1 TIM1_AF1_ETRSEL_0 /*!< ETR input is connected to COMP1_OUT */ +#define TIM_TIM3_ETR_COMP2 TIM1_AF1_ETRSEL_1 /*!< ETR input is connected to COMP2_OUT */ +#define TIM_TIM3_ETR_COMP3 (TIM1_AF1_ETRSEL_1 | TIM1_AF1_ETRSEL_0) /*!< ETR input is connected to COMP3_OUT */ +#define TIM_TIM3_ETR_COMP4 TIM1_AF1_ETRSEL_2 /*!< ETR input is connected to COMP4_OUT */ #if defined(COMP5) -#define TIM_TIM3_ETR_COMP5 (TIM1_AF1_ETRSEL_2 | TIM1_AF1_ETRSEL_0) /* !< ETR input is connected to COMP5_OUT */ +#define TIM_TIM3_ETR_COMP5 (TIM1_AF1_ETRSEL_2 | TIM1_AF1_ETRSEL_0) /*!< ETR input is connected to COMP5_OUT */ #endif /* COMP5 */ #if defined(COMP6) -#define TIM_TIM3_ETR_COMP6 (TIM1_AF1_ETRSEL_2 | TIM1_AF1_ETRSEL_1) /* !< ETR input is connected to COMP6_OUT */ +#define TIM_TIM3_ETR_COMP6 (TIM1_AF1_ETRSEL_2 | TIM1_AF1_ETRSEL_1) /*!< ETR input is connected to COMP6_OUT */ #endif /* COMP6 */ #if defined(COMP7) -#define TIM_TIM3_ETR_COMP7 (TIM1_AF1_ETRSEL_2 | TIM1_AF1_ETRSEL_1 | TIM1_AF1_ETRSEL_0) /* !< ETR input is connected to COMP7_OUT */ +#define TIM_TIM3_ETR_COMP7 (TIM1_AF1_ETRSEL_2 | TIM1_AF1_ETRSEL_1 | TIM1_AF1_ETRSEL_0) /*!< ETR input is connected to COMP7_OUT */ #endif /* COMP7 */ -#define TIM_TIM3_ETR_TIM2_ETR TIM1_AF1_ETRSEL_3 /* !< ETR input is connected to TIM2 ETR */ -#define TIM_TIM3_ETR_TIM4_ETR (TIM1_AF1_ETRSEL_3 | TIM1_AF1_ETRSEL_0) /* !< ETR input is connected to TIM4 ETR */ -#define TIM_TIM3_ETR_ADC2_AWD1 (TIM1_AF1_ETRSEL_3 | TIM1_AF1_ETRSEL_1 | TIM1_AF1_ETRSEL_0) /* !< ADC2 analog watchdog 1 */ -#define TIM_TIM3_ETR_ADC2_AWD2 (TIM1_AF1_ETRSEL_3 | TIM1_AF1_ETRSEL_2) /* !< ADC2 analog watchdog 2 */ -#define TIM_TIM3_ETR_ADC2_AWD3 (TIM1_AF1_ETRSEL_3 | TIM1_AF1_ETRSEL_2 | TIM1_AF1_ETRSEL_0) /* !< ADC2 analog watchdog 3 */ - -#define TIM_TIM4_ETR_GPIO 0x00000000U /* !< ETR input is connected to GPIO */ -#define TIM_TIM4_ETR_COMP1 TIM1_AF1_ETRSEL_0 /* !< ETR input is connected to COMP1_OUT */ -#define TIM_TIM4_ETR_COMP2 TIM1_AF1_ETRSEL_1 /* !< ETR input is connected to COMP2_OUT */ -#define TIM_TIM4_ETR_COMP3 (TIM1_AF1_ETRSEL_1 | TIM1_AF1_ETRSEL_0) /* !< ETR input is connected to COMP3_OUT */ -#define TIM_TIM4_ETR_COMP4 TIM1_AF1_ETRSEL_2 /* !< ETR input is connected to COMP4_OUT */ +#define TIM_TIM3_ETR_TIM2_ETR TIM1_AF1_ETRSEL_3 /*!< ETR input is connected to TIM2 ETR */ +#define TIM_TIM3_ETR_TIM4_ETR (TIM1_AF1_ETRSEL_3 | TIM1_AF1_ETRSEL_0) /*!< ETR input is connected to TIM4 ETR */ +#define TIM_TIM3_ETR_ADC2_AWD1 (TIM1_AF1_ETRSEL_3 | TIM1_AF1_ETRSEL_1 | TIM1_AF1_ETRSEL_0) /*!< ADC2 analog watchdog 1 */ +#define TIM_TIM3_ETR_ADC2_AWD2 (TIM1_AF1_ETRSEL_3 | TIM1_AF1_ETRSEL_2) /*!< ADC2 analog watchdog 2 */ +#define TIM_TIM3_ETR_ADC2_AWD3 (TIM1_AF1_ETRSEL_3 | TIM1_AF1_ETRSEL_2 | TIM1_AF1_ETRSEL_0) /*!< ADC2 analog watchdog 3 */ + +#define TIM_TIM4_ETR_GPIO 0x00000000U /*!< ETR input is connected to GPIO */ +#define TIM_TIM4_ETR_COMP1 TIM1_AF1_ETRSEL_0 /*!< ETR input is connected to COMP1_OUT */ +#define TIM_TIM4_ETR_COMP2 TIM1_AF1_ETRSEL_1 /*!< ETR input is connected to COMP2_OUT */ +#define TIM_TIM4_ETR_COMP3 (TIM1_AF1_ETRSEL_1 | TIM1_AF1_ETRSEL_0) /*!< ETR input is connected to COMP3_OUT */ +#define TIM_TIM4_ETR_COMP4 TIM1_AF1_ETRSEL_2 /*!< ETR input is connected to COMP4_OUT */ #if defined(COMP5) -#define TIM_TIM4_ETR_COMP5 (TIM1_AF1_ETRSEL_2 | TIM1_AF1_ETRSEL_0) /* !< ETR input is connected to COMP5_OUT */ +#define TIM_TIM4_ETR_COMP5 (TIM1_AF1_ETRSEL_2 | TIM1_AF1_ETRSEL_0) /*!< ETR input is connected to COMP5_OUT */ #endif /* COMP5 */ #if defined(COMP6) -#define TIM_TIM4_ETR_COMP6 (TIM1_AF1_ETRSEL_2 | TIM1_AF1_ETRSEL_1) /* !< ETR input is connected to COMP6_OUT */ +#define TIM_TIM4_ETR_COMP6 (TIM1_AF1_ETRSEL_2 | TIM1_AF1_ETRSEL_1) /*!< ETR input is connected to COMP6_OUT */ #endif /* COMP6 */ #if defined(COMP7) -#define TIM_TIM4_ETR_COMP7 (TIM1_AF1_ETRSEL_2 | TIM1_AF1_ETRSEL_1 | TIM1_AF1_ETRSEL_0) /* !< ETR input is connected to COMP7_OUT */ +#define TIM_TIM4_ETR_COMP7 (TIM1_AF1_ETRSEL_2 | TIM1_AF1_ETRSEL_1 | TIM1_AF1_ETRSEL_0) /*!< ETR input is connected to COMP7_OUT */ #endif /* COMP7 */ -#define TIM_TIM4_ETR_TIM3_ETR TIM1_AF1_ETRSEL_3 /* !< ETR input is connected to TIM3 ETR */ +#define TIM_TIM4_ETR_TIM3_ETR TIM1_AF1_ETRSEL_3 /*!< ETR input is connected to TIM3 ETR */ #if defined (TIM5) -#define TIM_TIM4_ETR_TIM5_ETR (TIM1_AF1_ETRSEL_3 | TIM1_AF1_ETRSEL_0) /* !< ETR input is connected to TIM5 ETR */ +#define TIM_TIM4_ETR_TIM5_ETR (TIM1_AF1_ETRSEL_3 | TIM1_AF1_ETRSEL_0) /*!< ETR input is connected to TIM5 ETR */ #endif /* TIM5 */ #if defined (TIM5) -#define TIM_TIM5_ETR_GPIO 0x00000000U /* !< ETR input is connected to GPIO */ -#define TIM_TIM5_ETR_COMP1 TIM1_AF1_ETRSEL_0 /* !< ETR input is connected to COMP1_OUT */ -#define TIM_TIM5_ETR_COMP2 TIM1_AF1_ETRSEL_1 /* !< ETR input is connected to COMP2_OUT */ -#define TIM_TIM5_ETR_COMP3 (TIM1_AF1_ETRSEL_1 | TIM1_AF1_ETRSEL_0) /* !< ETR input is connected to COMP3_OUT */ -#define TIM_TIM5_ETR_COMP4 TIM1_AF1_ETRSEL_2 /* !< ETR input is connected to COMP4_OUT */ +#define TIM_TIM5_ETR_GPIO 0x00000000U /*!< ETR input is connected to GPIO */ +#define TIM_TIM5_ETR_COMP1 TIM1_AF1_ETRSEL_0 /*!< ETR input is connected to COMP1_OUT */ +#define TIM_TIM5_ETR_COMP2 TIM1_AF1_ETRSEL_1 /*!< ETR input is connected to COMP2_OUT */ +#define TIM_TIM5_ETR_COMP3 (TIM1_AF1_ETRSEL_1 | TIM1_AF1_ETRSEL_0) /*!< ETR input is connected to COMP3_OUT */ +#define TIM_TIM5_ETR_COMP4 TIM1_AF1_ETRSEL_2 /*!< ETR input is connected to COMP4_OUT */ #if defined(COMP5) -#define TIM_TIM5_ETR_COMP5 (TIM1_AF1_ETRSEL_2 | TIM1_AF1_ETRSEL_0) /* !< ETR input is connected to COMP5_OUT */ +#define TIM_TIM5_ETR_COMP5 (TIM1_AF1_ETRSEL_2 | TIM1_AF1_ETRSEL_0) /*!< ETR input is connected to COMP5_OUT */ #endif /* COMP5 */ #if defined(COMP6) -#define TIM_TIM5_ETR_COMP6 (TIM1_AF1_ETRSEL_2 | TIM1_AF1_ETRSEL_1) /* !< ETR input is connected to COMP6_OUT */ +#define TIM_TIM5_ETR_COMP6 (TIM1_AF1_ETRSEL_2 | TIM1_AF1_ETRSEL_1) /*!< ETR input is connected to COMP6_OUT */ #endif /* COMP6 */ #if defined(COMP7) -#define TIM_TIM5_ETR_COMP7 (TIM1_AF1_ETRSEL_2 | TIM1_AF1_ETRSEL_1 | TIM1_AF1_ETRSEL_0) /* !< ETR input is connected to COMP7_OUT */ +#define TIM_TIM5_ETR_COMP7 (TIM1_AF1_ETRSEL_2 | TIM1_AF1_ETRSEL_1 | TIM1_AF1_ETRSEL_0) /*!< ETR input is connected to COMP7_OUT */ #endif /* COMP7 */ -#define TIM_TIM5_ETR_TIM2_ETR TIM1_AF1_ETRSEL_3 /* !< ETR input is connected to TIM2 ETR */ -#define TIM_TIM5_ETR_TIM3_ETR (TIM1_AF1_ETRSEL_3 | TIM1_AF1_ETRSEL_0) /* !< ETR input is connected to TIM3 ETR */ +#define TIM_TIM5_ETR_TIM2_ETR TIM1_AF1_ETRSEL_3 /*!< ETR input is connected to TIM2 ETR */ +#define TIM_TIM5_ETR_TIM3_ETR (TIM1_AF1_ETRSEL_3 | TIM1_AF1_ETRSEL_0) /*!< ETR input is connected to TIM3 ETR */ #endif /* TIM5 */ -#define TIM_TIM8_ETR_GPIO 0x00000000U /* !< ETR input is connected to GPIO */ -#define TIM_TIM8_ETR_COMP1 TIM1_AF1_ETRSEL_0 /* !< ETR input is connected to COMP1_OUT */ -#define TIM_TIM8_ETR_COMP2 TIM1_AF1_ETRSEL_1 /* !< ETR input is connected to COMP2_OUT */ -#define TIM_TIM8_ETR_COMP3 (TIM1_AF1_ETRSEL_1 | TIM1_AF1_ETRSEL_0) /* !< ETR input is connected to COMP3_OUT */ -#define TIM_TIM8_ETR_COMP4 TIM1_AF1_ETRSEL_2 /* !< ETR input is connected to COMP4_OUT */ +#define TIM_TIM8_ETR_GPIO 0x00000000U /*!< ETR input is connected to GPIO */ +#define TIM_TIM8_ETR_COMP1 TIM1_AF1_ETRSEL_0 /*!< ETR input is connected to COMP1_OUT */ +#define TIM_TIM8_ETR_COMP2 TIM1_AF1_ETRSEL_1 /*!< ETR input is connected to COMP2_OUT */ +#define TIM_TIM8_ETR_COMP3 (TIM1_AF1_ETRSEL_1 | TIM1_AF1_ETRSEL_0) /*!< ETR input is connected to COMP3_OUT */ +#define TIM_TIM8_ETR_COMP4 TIM1_AF1_ETRSEL_2 /*!< ETR input is connected to COMP4_OUT */ #if defined(COMP5) -#define TIM_TIM8_ETR_COMP5 (TIM1_AF1_ETRSEL_2 | TIM1_AF1_ETRSEL_0) /* !< ETR input is connected to COMP5_OUT */ +#define TIM_TIM8_ETR_COMP5 (TIM1_AF1_ETRSEL_2 | TIM1_AF1_ETRSEL_0) /*!< ETR input is connected to COMP5_OUT */ #endif /* COMP5 */ #if defined(COMP6) -#define TIM_TIM8_ETR_COMP6 (TIM1_AF1_ETRSEL_2 | TIM1_AF1_ETRSEL_1) /* !< ETR input is connected to COMP6_OUT */ +#define TIM_TIM8_ETR_COMP6 (TIM1_AF1_ETRSEL_2 | TIM1_AF1_ETRSEL_1) /*!< ETR input is connected to COMP6_OUT */ #endif /* COMP6 */ #if defined(COMP7) -#define TIM_TIM8_ETR_COMP7 (TIM1_AF1_ETRSEL_2 | TIM1_AF1_ETRSEL_1 | TIM1_AF1_ETRSEL_0) /* !< ETR input is connected to COMP7_OUT */ +#define TIM_TIM8_ETR_COMP7 (TIM1_AF1_ETRSEL_2 | TIM1_AF1_ETRSEL_1 | TIM1_AF1_ETRSEL_0) /*!< ETR input is connected to COMP7_OUT */ #endif /* COMP7 */ -#define TIM_TIM8_ETR_ADC2_AWD1 TIM1_AF1_ETRSEL_3 /* !< ADC2 analog watchdog 1 */ -#define TIM_TIM8_ETR_ADC2_AWD2 (TIM1_AF1_ETRSEL_3 | TIM1_AF1_ETRSEL_0) /* !< ADC2 analog watchdog 2 */ -#define TIM_TIM8_ETR_ADC2_AWD3 (TIM1_AF1_ETRSEL_3 | TIM1_AF1_ETRSEL_1) /* !< ADC2 analog watchdog 3 */ +#define TIM_TIM8_ETR_ADC2_AWD1 TIM1_AF1_ETRSEL_3 /*!< ADC2 analog watchdog 1 */ +#define TIM_TIM8_ETR_ADC2_AWD2 (TIM1_AF1_ETRSEL_3 | TIM1_AF1_ETRSEL_0) /*!< ADC2 analog watchdog 2 */ +#define TIM_TIM8_ETR_ADC2_AWD3 (TIM1_AF1_ETRSEL_3 | TIM1_AF1_ETRSEL_1) /*!< ADC2 analog watchdog 3 */ #if defined (ADC3) -#define TIM_TIM8_ETR_ADC3_AWD1 (TIM1_AF1_ETRSEL_3 | TIM1_AF1_ETRSEL_1 | TIM1_AF1_ETRSEL_0) /* !< ADC3 analog watchdog 1 */ -#define TIM_TIM8_ETR_ADC3_AWD2 (TIM1_AF1_ETRSEL_3 | TIM1_AF1_ETRSEL_2) /* !< ADC3 analog watchdog 2 */ -#define TIM_TIM8_ETR_ADC3_AWD3 (TIM1_AF1_ETRSEL_3 | TIM1_AF1_ETRSEL_2 | TIM1_AF1_ETRSEL_0) /* !< ADC3 analog watchdog 3 */ +#define TIM_TIM8_ETR_ADC3_AWD1 (TIM1_AF1_ETRSEL_3 | TIM1_AF1_ETRSEL_1 | TIM1_AF1_ETRSEL_0) /*!< ADC3 analog watchdog 1 */ +#define TIM_TIM8_ETR_ADC3_AWD2 (TIM1_AF1_ETRSEL_3 | TIM1_AF1_ETRSEL_2) /*!< ADC3 analog watchdog 2 */ +#define TIM_TIM8_ETR_ADC3_AWD3 (TIM1_AF1_ETRSEL_3 | TIM1_AF1_ETRSEL_2 | TIM1_AF1_ETRSEL_0) /*!< ADC3 analog watchdog 3 */ #endif /* ADC3 */ #if defined (TIM20) -#define TIM_TIM20_ETR_GPIO 0x00000000U /* !< ETR input is connected to GPIO */ -#define TIM_TIM20_ETR_COMP1 TIM1_AF1_ETRSEL_0 /* !< ETR input is connected to COMP1_OUT */ -#define TIM_TIM20_ETR_COMP2 TIM1_AF1_ETRSEL_1 /* !< ETR input is connected to COMP2_OUT */ -#define TIM_TIM20_ETR_COMP3 (TIM1_AF1_ETRSEL_1 | TIM1_AF1_ETRSEL_0) /* !< ETR input is connected to COMP3_OUT */ -#define TIM_TIM20_ETR_COMP4 TIM1_AF1_ETRSEL_2 /* !< ETR input is connected to COMP4_OUT */ +#define TIM_TIM20_ETR_GPIO 0x00000000U /*!< ETR input is connected to GPIO */ +#define TIM_TIM20_ETR_COMP1 TIM1_AF1_ETRSEL_0 /*!< ETR input is connected to COMP1_OUT */ +#define TIM_TIM20_ETR_COMP2 TIM1_AF1_ETRSEL_1 /*!< ETR input is connected to COMP2_OUT */ +#define TIM_TIM20_ETR_COMP3 (TIM1_AF1_ETRSEL_1 | TIM1_AF1_ETRSEL_0) /*!< ETR input is connected to COMP3_OUT */ +#define TIM_TIM20_ETR_COMP4 TIM1_AF1_ETRSEL_2 /*!< ETR input is connected to COMP4_OUT */ #if defined(COMP5) -#define TIM_TIM20_ETR_COMP5 (TIM1_AF1_ETRSEL_2 | TIM1_AF1_ETRSEL_0) /* !< ETR input is connected to COMP5_OUT */ +#define TIM_TIM20_ETR_COMP5 (TIM1_AF1_ETRSEL_2 | TIM1_AF1_ETRSEL_0) /*!< ETR input is connected to COMP5_OUT */ #endif /* COMP5 */ #if defined(COMP6) -#define TIM_TIM20_ETR_COMP6 (TIM1_AF1_ETRSEL_2 | TIM1_AF1_ETRSEL_1) /* !< ETR input is connected to COMP6_OUT */ +#define TIM_TIM20_ETR_COMP6 (TIM1_AF1_ETRSEL_2 | TIM1_AF1_ETRSEL_1) /*!< ETR input is connected to COMP6_OUT */ #endif /* COMP6 */ #if defined(COMP7) -#define TIM_TIM20_ETR_COMP7 (TIM1_AF1_ETRSEL_2 | TIM1_AF1_ETRSEL_1 | TIM1_AF1_ETRSEL_0) /* !< ETR input is connected to COMP7_OUT */ +#define TIM_TIM20_ETR_COMP7 (TIM1_AF1_ETRSEL_2 | TIM1_AF1_ETRSEL_1 | TIM1_AF1_ETRSEL_0) /*!< ETR input is connected to COMP7_OUT */ #endif /* COMP7 */ -#define TIM_TIM20_ETR_ADC3_AWD1 TIM1_AF1_ETRSEL_3 /* !< ADC3 analog watchdog 1 */ -#define TIM_TIM20_ETR_ADC3_AWD2 (TIM1_AF1_ETRSEL_3 | TIM1_AF1_ETRSEL_0) /* !< ADC3 analog watchdog 2 */ -#define TIM_TIM20_ETR_ADC3_AWD3 (TIM1_AF1_ETRSEL_3 | TIM1_AF1_ETRSEL_1) /* !< ADC3 analog watchdog 3 */ +#define TIM_TIM20_ETR_ADC3_AWD1 TIM1_AF1_ETRSEL_3 /*!< ADC3 analog watchdog 1 */ +#define TIM_TIM20_ETR_ADC3_AWD2 (TIM1_AF1_ETRSEL_3 | TIM1_AF1_ETRSEL_0) /*!< ADC3 analog watchdog 2 */ +#define TIM_TIM20_ETR_ADC3_AWD3 (TIM1_AF1_ETRSEL_3 | TIM1_AF1_ETRSEL_1) /*!< ADC3 analog watchdog 3 */ #if defined (ADC5) -#define TIM_TIM20_ETR_ADC5_AWD1 (TIM1_AF1_ETRSEL_3 | TIM1_AF1_ETRSEL_1 | TIM1_AF1_ETRSEL_0) /* !< ADC5 analog watchdog 1 */ -#define TIM_TIM20_ETR_ADC5_AWD2 (TIM1_AF1_ETRSEL_3 | TIM1_AF1_ETRSEL_2) /* !< ADC5 analog watchdog 2 */ -#define TIM_TIM20_ETR_ADC5_AWD3 (TIM1_AF1_ETRSEL_3 | TIM1_AF1_ETRSEL_2 | TIM1_AF1_ETRSEL_0) /* !< ADC5 analog watchdog 3 */ +#define TIM_TIM20_ETR_ADC5_AWD1 (TIM1_AF1_ETRSEL_3 | TIM1_AF1_ETRSEL_1 | TIM1_AF1_ETRSEL_0) /*!< ADC5 analog watchdog 1 */ +#define TIM_TIM20_ETR_ADC5_AWD2 (TIM1_AF1_ETRSEL_3 | TIM1_AF1_ETRSEL_2) /*!< ADC5 analog watchdog 2 */ +#define TIM_TIM20_ETR_ADC5_AWD3 (TIM1_AF1_ETRSEL_3 | TIM1_AF1_ETRSEL_2 | TIM1_AF1_ETRSEL_0) /*!< ADC5 analog watchdog 3 */ #endif /* ADC5 */ #endif /* TIM20 */ /** @@ -269,19 +269,19 @@ typedef struct /** @defgroup TIMEx_Break_Input_Source TIM Extended Break input source * @{ */ -#define TIM_BREAKINPUTSOURCE_BKIN 0x00000001U /* !< An external source (GPIO) is connected to the BKIN pin */ -#define TIM_BREAKINPUTSOURCE_COMP1 0x00000002U /* !< The COMP1 output is connected to the break input */ -#define TIM_BREAKINPUTSOURCE_COMP2 0x00000004U /* !< The COMP2 output is connected to the break input */ -#define TIM_BREAKINPUTSOURCE_COMP3 0x00000008U /* !< The COMP3 output is connected to the break input */ -#define TIM_BREAKINPUTSOURCE_COMP4 0x00000010U /* !< The COMP4 output is connected to the break input */ +#define TIM_BREAKINPUTSOURCE_BKIN 0x00000001U /*!< An external source (GPIO) is connected to the BKIN pin */ +#define TIM_BREAKINPUTSOURCE_COMP1 0x00000002U /*!< The COMP1 output is connected to the break input */ +#define TIM_BREAKINPUTSOURCE_COMP2 0x00000004U /*!< The COMP2 output is connected to the break input */ +#define TIM_BREAKINPUTSOURCE_COMP3 0x00000008U /*!< The COMP3 output is connected to the break input */ +#define TIM_BREAKINPUTSOURCE_COMP4 0x00000010U /*!< The COMP4 output is connected to the break input */ #if defined(COMP5) -#define TIM_BREAKINPUTSOURCE_COMP5 0x00000020U /* !< The COMP5 output is connected to the break input */ +#define TIM_BREAKINPUTSOURCE_COMP5 0x00000020U /*!< The COMP5 output is connected to the break input */ #endif /* COMP5 */ #if defined(COMP6) -#define TIM_BREAKINPUTSOURCE_COMP6 0x00000040U /* !< The COMP6 output is connected to the break input */ +#define TIM_BREAKINPUTSOURCE_COMP6 0x00000040U /*!< The COMP6 output is connected to the break input */ #endif /* COMP6 */ #if defined(COMP7) -#define TIM_BREAKINPUTSOURCE_COMP7 0x00000080U /* !< The COMP7 output is connected to the break input */ +#define TIM_BREAKINPUTSOURCE_COMP7 0x00000080U /*!< The COMP7 output is connected to the break input */ #endif /* COMP7 */ /** * @} @@ -715,14 +715,14 @@ typedef struct #define IS_TIM_CLOCKSOURCE_INSTANCE(INSTANCE, __CLOCK__) \ ((((INSTANCE) == TIM1) && \ (((__CLOCK__) == TIM_CLOCKSOURCE_INTERNAL) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE2) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ITR1) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ITR2) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ITR3) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_TI1ED) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_TI1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_TI2) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE1) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ITR1) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ITR2) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ITR3) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR4) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR5) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR6) || \ @@ -733,14 +733,14 @@ typedef struct || \ (((INSTANCE) == TIM2) && \ (((__CLOCK__) == TIM_CLOCKSOURCE_INTERNAL) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE2) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ITR0) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ITR2) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ITR3) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_TI1ED) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_TI1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_TI2) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE1) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ITR0) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ITR2) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ITR3) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR4) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR5) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR6) || \ @@ -752,14 +752,14 @@ typedef struct || \ (((INSTANCE) == TIM3) && \ (((__CLOCK__) == TIM_CLOCKSOURCE_INTERNAL) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE2) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ITR0) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ITR1) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ITR3) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_TI1ED) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_TI1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_TI2) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE1) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ITR0) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ITR1) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ITR3) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR4) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR5) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR6) || \ @@ -770,15 +770,15 @@ typedef struct || \ (((INSTANCE) == TIM4) && \ (((__CLOCK__) == TIM_CLOCKSOURCE_INTERNAL) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE2) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_TI1ED) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_TI1) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_TI2) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR0) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR2) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR4) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_TI1ED) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_TI1) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_TI2) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR5) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR6) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR7) || \ @@ -788,15 +788,15 @@ typedef struct || \ (((INSTANCE) == TIM5) && \ (((__CLOCK__) == TIM_CLOCKSOURCE_INTERNAL) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE2) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_TI1ED) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_TI1) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_TI2) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR0) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR2) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR3) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_TI1ED) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_TI1) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_TI2) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR5) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR6) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR7) || \ @@ -806,15 +806,15 @@ typedef struct || \ (((INSTANCE) == TIM8) && \ (((__CLOCK__) == TIM_CLOCKSOURCE_INTERNAL) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE2) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_TI1ED) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_TI1) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_TI2) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR0) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR2) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR3) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_TI1ED) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_TI1) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_TI2) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR4) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR6) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR7) || \ @@ -824,13 +824,13 @@ typedef struct || \ (((INSTANCE) == TIM15) && \ (((__CLOCK__) == TIM_CLOCKSOURCE_INTERNAL) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_TI1ED) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_TI1) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_TI2) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR0) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR2) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR3) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_TI1ED) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_TI1) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_TI2) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR4) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR5) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR7) || \ @@ -840,15 +840,15 @@ typedef struct || \ (((INSTANCE) == TIM20) && \ (((__CLOCK__) == TIM_CLOCKSOURCE_INTERNAL) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE2) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_TI1ED) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_TI1) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_TI2) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR0) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR2) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR3) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_TI1ED) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_TI1) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_TI2) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR4) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR5) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR6) || \ @@ -858,13 +858,13 @@ typedef struct #define IS_TIM_TRIGGER_INSTANCE(INSTANCE, __SELECTION__) \ ((((INSTANCE) == TIM1) && \ - (((__SELECTION__) == TIM_TS_ITR1) || \ - ((__SELECTION__) == TIM_TS_ITR2) || \ - ((__SELECTION__) == TIM_TS_ITR3) || \ - ((__SELECTION__) == TIM_TS_TI1F_ED) || \ + (((__SELECTION__) == TIM_TS_TI1F_ED) || \ ((__SELECTION__) == TIM_TS_TI1FP1) || \ ((__SELECTION__) == TIM_TS_TI2FP2) || \ ((__SELECTION__) == TIM_TS_ETRF) || \ + ((__SELECTION__) == TIM_TS_ITR1) || \ + ((__SELECTION__) == TIM_TS_ITR2) || \ + ((__SELECTION__) == TIM_TS_ITR3) || \ ((__SELECTION__) == TIM_TS_ITR4) || \ ((__SELECTION__) == TIM_TS_ITR5) || \ ((__SELECTION__) == TIM_TS_ITR6) || \ @@ -874,13 +874,13 @@ typedef struct ((__SELECTION__) == TIM_TS_ITR10))) \ || \ (((INSTANCE) == TIM2) && \ - (((__SELECTION__) == TIM_TS_ITR0) || \ - ((__SELECTION__) == TIM_TS_ITR2) || \ - ((__SELECTION__) == TIM_TS_ITR3) || \ - ((__SELECTION__) == TIM_TS_TI1F_ED) || \ + (((__SELECTION__) == TIM_TS_TI1F_ED) || \ ((__SELECTION__) == TIM_TS_TI1FP1) || \ ((__SELECTION__) == TIM_TS_TI2FP2) || \ ((__SELECTION__) == TIM_TS_ETRF) || \ + ((__SELECTION__) == TIM_TS_ITR0) || \ + ((__SELECTION__) == TIM_TS_ITR2) || \ + ((__SELECTION__) == TIM_TS_ITR3) || \ ((__SELECTION__) == TIM_TS_ITR4) || \ ((__SELECTION__) == TIM_TS_ITR5) || \ ((__SELECTION__) == TIM_TS_ITR6) || \ @@ -891,13 +891,13 @@ typedef struct ((__SELECTION__) == TIM_TS_ITR11))) \ || \ (((INSTANCE) == TIM3) && \ - (((__SELECTION__) == TIM_TS_ITR0) || \ - ((__SELECTION__) == TIM_TS_ITR1) || \ - ((__SELECTION__) == TIM_TS_ITR3) || \ - ((__SELECTION__) == TIM_TS_TI1F_ED) || \ + (((__SELECTION__) == TIM_TS_TI1F_ED) || \ ((__SELECTION__) == TIM_TS_TI1FP1) || \ ((__SELECTION__) == TIM_TS_TI2FP2) || \ ((__SELECTION__) == TIM_TS_ETRF) || \ + ((__SELECTION__) == TIM_TS_ITR0) || \ + ((__SELECTION__) == TIM_TS_ITR1) || \ + ((__SELECTION__) == TIM_TS_ITR3) || \ ((__SELECTION__) == TIM_TS_ITR4) || \ ((__SELECTION__) == TIM_TS_ITR5) || \ ((__SELECTION__) == TIM_TS_ITR6) || \ @@ -907,14 +907,14 @@ typedef struct ((__SELECTION__) == TIM_TS_ITR10))) \ || \ (((INSTANCE) == TIM4) && \ - (((__SELECTION__) == TIM_TS_ITR0) || \ - ((__SELECTION__) == TIM_TS_ITR1) || \ - ((__SELECTION__) == TIM_TS_ITR2) || \ - ((__SELECTION__) == TIM_TS_ITR4) || \ - ((__SELECTION__) == TIM_TS_TI1F_ED) || \ + (((__SELECTION__) == TIM_TS_TI1F_ED) || \ ((__SELECTION__) == TIM_TS_TI1FP1) || \ ((__SELECTION__) == TIM_TS_TI2FP2) || \ ((__SELECTION__) == TIM_TS_ETRF) || \ + ((__SELECTION__) == TIM_TS_ITR0) || \ + ((__SELECTION__) == TIM_TS_ITR1) || \ + ((__SELECTION__) == TIM_TS_ITR2) || \ + ((__SELECTION__) == TIM_TS_ITR4) || \ ((__SELECTION__) == TIM_TS_ITR5) || \ ((__SELECTION__) == TIM_TS_ITR6) || \ ((__SELECTION__) == TIM_TS_ITR7) || \ @@ -923,14 +923,14 @@ typedef struct ((__SELECTION__) == TIM_TS_ITR10))) \ || \ (((INSTANCE) == TIM5) && \ - (((__SELECTION__) == TIM_TS_ITR0) || \ - ((__SELECTION__) == TIM_TS_ITR1) || \ - ((__SELECTION__) == TIM_TS_ITR2) || \ - ((__SELECTION__) == TIM_TS_ITR3) || \ - ((__SELECTION__) == TIM_TS_TI1F_ED) || \ + (((__SELECTION__) == TIM_TS_TI1F_ED) || \ ((__SELECTION__) == TIM_TS_TI1FP1) || \ ((__SELECTION__) == TIM_TS_TI2FP2) || \ ((__SELECTION__) == TIM_TS_ETRF) || \ + ((__SELECTION__) == TIM_TS_ITR0) || \ + ((__SELECTION__) == TIM_TS_ITR1) || \ + ((__SELECTION__) == TIM_TS_ITR2) || \ + ((__SELECTION__) == TIM_TS_ITR3) || \ ((__SELECTION__) == TIM_TS_ITR5) || \ ((__SELECTION__) == TIM_TS_ITR6) || \ ((__SELECTION__) == TIM_TS_ITR7) || \ @@ -939,14 +939,14 @@ typedef struct ((__SELECTION__) == TIM_TS_ITR10))) \ || \ (((INSTANCE) == TIM8) && \ - (((__SELECTION__) == TIM_TS_ITR0) || \ - ((__SELECTION__) == TIM_TS_ITR1) || \ - ((__SELECTION__) == TIM_TS_ITR2) || \ - ((__SELECTION__) == TIM_TS_ITR3) || \ - ((__SELECTION__) == TIM_TS_TI1F_ED) || \ + (((__SELECTION__) == TIM_TS_TI1F_ED) || \ ((__SELECTION__) == TIM_TS_TI1FP1) || \ ((__SELECTION__) == TIM_TS_TI2FP2) || \ ((__SELECTION__) == TIM_TS_ETRF) || \ + ((__SELECTION__) == TIM_TS_ITR0) || \ + ((__SELECTION__) == TIM_TS_ITR1) || \ + ((__SELECTION__) == TIM_TS_ITR2) || \ + ((__SELECTION__) == TIM_TS_ITR3) || \ ((__SELECTION__) == TIM_TS_ITR4) || \ ((__SELECTION__) == TIM_TS_ITR6) || \ ((__SELECTION__) == TIM_TS_ITR7) || \ @@ -955,13 +955,13 @@ typedef struct ((__SELECTION__) == TIM_TS_ITR10))) \ || \ (((INSTANCE) == TIM15) && \ - (((__SELECTION__) == TIM_TS_ITR0) || \ + (((__SELECTION__) == TIM_TS_TI1F_ED) || \ + ((__SELECTION__) == TIM_TS_TI1FP1) || \ + ((__SELECTION__) == TIM_TS_TI2FP2) || \ + ((__SELECTION__) == TIM_TS_ITR0) || \ ((__SELECTION__) == TIM_TS_ITR1) || \ ((__SELECTION__) == TIM_TS_ITR2) || \ ((__SELECTION__) == TIM_TS_ITR3) || \ - ((__SELECTION__) == TIM_TS_TI1F_ED) || \ - ((__SELECTION__) == TIM_TS_TI1FP1) || \ - ((__SELECTION__) == TIM_TS_TI2FP2) || \ ((__SELECTION__) == TIM_TS_ITR4) || \ ((__SELECTION__) == TIM_TS_ITR5) || \ ((__SELECTION__) == TIM_TS_ITR7) || \ @@ -970,14 +970,14 @@ typedef struct ((__SELECTION__) == TIM_TS_ITR10))) \ || \ (((INSTANCE) == TIM20) && \ - (((__SELECTION__) == TIM_TS_ITR0) || \ - ((__SELECTION__) == TIM_TS_ITR1) || \ - ((__SELECTION__) == TIM_TS_ITR2) || \ - ((__SELECTION__) == TIM_TS_ITR3) || \ - ((__SELECTION__) == TIM_TS_TI1F_ED) || \ + (((__SELECTION__) == TIM_TS_TI1F_ED) || \ ((__SELECTION__) == TIM_TS_TI1FP1) || \ ((__SELECTION__) == TIM_TS_TI2FP2) || \ ((__SELECTION__) == TIM_TS_ETRF) || \ + ((__SELECTION__) == TIM_TS_ITR0) || \ + ((__SELECTION__) == TIM_TS_ITR1) || \ + ((__SELECTION__) == TIM_TS_ITR2) || \ + ((__SELECTION__) == TIM_TS_ITR3) || \ ((__SELECTION__) == TIM_TS_ITR4) || \ ((__SELECTION__) == TIM_TS_ITR5) || \ ((__SELECTION__) == TIM_TS_ITR6) || \ @@ -1095,14 +1095,14 @@ typedef struct #define IS_TIM_CLOCKSOURCE_INSTANCE(INSTANCE, __CLOCK__) \ ((((INSTANCE) == TIM1) && \ (((__CLOCK__) == TIM_CLOCKSOURCE_INTERNAL) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE2) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ITR1) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ITR2) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ITR3) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_TI1ED) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_TI1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_TI2) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE1) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ITR1) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ITR2) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ITR3) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR4) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR5) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR6) || \ @@ -1111,14 +1111,14 @@ typedef struct || \ (((INSTANCE) == TIM2) && \ (((__CLOCK__) == TIM_CLOCKSOURCE_INTERNAL) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE2) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ITR0) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ITR2) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ITR3) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_TI1ED) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_TI1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_TI2) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE1) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ITR0) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ITR2) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ITR3) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR4) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR5) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR6) || \ @@ -1128,14 +1128,14 @@ typedef struct || \ (((INSTANCE) == TIM3) && \ (((__CLOCK__) == TIM_CLOCKSOURCE_INTERNAL) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE2) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ITR0) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ITR1) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ITR3) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_TI1ED) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_TI1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_TI2) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE1) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ITR0) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ITR1) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ITR3) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR4) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR5) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR6) || \ @@ -1144,14 +1144,14 @@ typedef struct || \ (((INSTANCE) == TIM4) && \ (((__CLOCK__) == TIM_CLOCKSOURCE_INTERNAL) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE2) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ITR0) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ITR1) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ITR2) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_TI1ED) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_TI1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_TI2) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE1) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ITR0) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ITR1) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ITR2) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR4) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR5) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR6) || \ @@ -1160,15 +1160,15 @@ typedef struct || \ (((INSTANCE) == TIM5) && \ (((__CLOCK__) == TIM_CLOCKSOURCE_INTERNAL) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE2) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_TI1ED) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_TI1) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_TI2) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR0) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR2) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR3) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_TI1ED) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_TI1) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_TI2) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR5) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR6) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR7) || \ @@ -1176,15 +1176,15 @@ typedef struct || \ (((INSTANCE) == TIM8) && \ (((__CLOCK__) == TIM_CLOCKSOURCE_INTERNAL) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE2) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_TI1ED) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_TI1) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_TI2) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR0) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR2) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR3) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_TI1ED) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_TI1) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_TI2) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR4) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR6) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR7) || \ @@ -1192,13 +1192,13 @@ typedef struct || \ (((INSTANCE) == TIM15) && \ (((__CLOCK__) == TIM_CLOCKSOURCE_INTERNAL) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_TI1ED) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_TI1) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_TI2) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR0) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR2) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR3) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_TI1ED) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_TI1) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_TI2) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR4) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR5) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR7) || \ @@ -1206,13 +1206,13 @@ typedef struct #define IS_TIM_TRIGGER_INSTANCE(INSTANCE, __SELECTION__) \ ((((INSTANCE) == TIM1) && \ - (((__SELECTION__) == TIM_TS_ITR1) || \ - ((__SELECTION__) == TIM_TS_ITR2) || \ - ((__SELECTION__) == TIM_TS_ITR3) || \ - ((__SELECTION__) == TIM_TS_TI1F_ED) || \ + (((__SELECTION__) == TIM_TS_TI1F_ED) || \ ((__SELECTION__) == TIM_TS_TI1FP1) || \ ((__SELECTION__) == TIM_TS_TI2FP2) || \ ((__SELECTION__) == TIM_TS_ETRF) || \ + ((__SELECTION__) == TIM_TS_ITR1) || \ + ((__SELECTION__) == TIM_TS_ITR2) || \ + ((__SELECTION__) == TIM_TS_ITR3) || \ ((__SELECTION__) == TIM_TS_ITR4) || \ ((__SELECTION__) == TIM_TS_ITR5) || \ ((__SELECTION__) == TIM_TS_ITR6) || \ @@ -1220,13 +1220,13 @@ typedef struct ((__SELECTION__) == TIM_TS_ITR8))) \ || \ (((INSTANCE) == TIM2) && \ - (((__SELECTION__) == TIM_TS_ITR0) || \ - ((__SELECTION__) == TIM_TS_ITR2) || \ - ((__SELECTION__) == TIM_TS_ITR3) || \ - ((__SELECTION__) == TIM_TS_TI1F_ED) || \ + (((__SELECTION__) == TIM_TS_TI1F_ED) || \ ((__SELECTION__) == TIM_TS_TI1FP1) || \ ((__SELECTION__) == TIM_TS_TI2FP2) || \ ((__SELECTION__) == TIM_TS_ETRF) || \ + ((__SELECTION__) == TIM_TS_ITR0) || \ + ((__SELECTION__) == TIM_TS_ITR2) || \ + ((__SELECTION__) == TIM_TS_ITR3) || \ ((__SELECTION__) == TIM_TS_ITR4) || \ ((__SELECTION__) == TIM_TS_ITR5) || \ ((__SELECTION__) == TIM_TS_ITR6) || \ @@ -1235,13 +1235,13 @@ typedef struct ((__SELECTION__) == TIM_TS_ITR11))) \ || \ (((INSTANCE) == TIM3) && \ - (((__SELECTION__) == TIM_TS_ITR0) || \ - ((__SELECTION__) == TIM_TS_ITR1) || \ - ((__SELECTION__) == TIM_TS_ITR3) || \ - ((__SELECTION__) == TIM_TS_TI1F_ED) || \ + (((__SELECTION__) == TIM_TS_TI1F_ED) || \ ((__SELECTION__) == TIM_TS_TI1FP1) || \ ((__SELECTION__) == TIM_TS_TI2FP2) || \ ((__SELECTION__) == TIM_TS_ETRF) || \ + ((__SELECTION__) == TIM_TS_ITR0) || \ + ((__SELECTION__) == TIM_TS_ITR1) || \ + ((__SELECTION__) == TIM_TS_ITR3) || \ ((__SELECTION__) == TIM_TS_ITR4) || \ ((__SELECTION__) == TIM_TS_ITR5) || \ ((__SELECTION__) == TIM_TS_ITR6) || \ @@ -1249,13 +1249,13 @@ typedef struct ((__SELECTION__) == TIM_TS_ITR8))) \ || \ (((INSTANCE) == TIM4) && \ - (((__SELECTION__) == TIM_TS_ITR0) || \ - ((__SELECTION__) == TIM_TS_ITR1) || \ - ((__SELECTION__) == TIM_TS_ITR2) || \ - ((__SELECTION__) == TIM_TS_TI1F_ED) || \ + (((__SELECTION__) == TIM_TS_TI1F_ED) || \ ((__SELECTION__) == TIM_TS_TI1FP1) || \ ((__SELECTION__) == TIM_TS_TI2FP2) || \ ((__SELECTION__) == TIM_TS_ETRF) || \ + ((__SELECTION__) == TIM_TS_ITR0) || \ + ((__SELECTION__) == TIM_TS_ITR1) || \ + ((__SELECTION__) == TIM_TS_ITR2) || \ ((__SELECTION__) == TIM_TS_ITR4) || \ ((__SELECTION__) == TIM_TS_ITR5) || \ ((__SELECTION__) == TIM_TS_ITR6) || \ @@ -1263,41 +1263,41 @@ typedef struct ((__SELECTION__) == TIM_TS_ITR8))) \ || \ (((INSTANCE) == TIM5) && \ - (((__SELECTION__) == TIM_TS_ITR0) || \ - ((__SELECTION__) == TIM_TS_ITR1) || \ - ((__SELECTION__) == TIM_TS_ITR2) || \ - ((__SELECTION__) == TIM_TS_ITR3) || \ - ((__SELECTION__) == TIM_TS_TI1F_ED) || \ + (((__SELECTION__) == TIM_TS_TI1F_ED) || \ ((__SELECTION__) == TIM_TS_TI1FP1) || \ ((__SELECTION__) == TIM_TS_TI2FP2) || \ ((__SELECTION__) == TIM_TS_ETRF) || \ + ((__SELECTION__) == TIM_TS_ITR0) || \ + ((__SELECTION__) == TIM_TS_ITR1) || \ + ((__SELECTION__) == TIM_TS_ITR2) || \ + ((__SELECTION__) == TIM_TS_ITR3) || \ ((__SELECTION__) == TIM_TS_ITR5) || \ ((__SELECTION__) == TIM_TS_ITR6) || \ ((__SELECTION__) == TIM_TS_ITR7) || \ ((__SELECTION__) == TIM_TS_ITR8))) \ || \ (((INSTANCE) == TIM8) && \ - (((__SELECTION__) == TIM_TS_ITR0) || \ - ((__SELECTION__) == TIM_TS_ITR1) || \ - ((__SELECTION__) == TIM_TS_ITR2) || \ - ((__SELECTION__) == TIM_TS_ITR3) || \ - ((__SELECTION__) == TIM_TS_TI1F_ED) || \ + (((__SELECTION__) == TIM_TS_TI1F_ED) || \ ((__SELECTION__) == TIM_TS_TI1FP1) || \ ((__SELECTION__) == TIM_TS_TI2FP2) || \ ((__SELECTION__) == TIM_TS_ETRF) || \ + ((__SELECTION__) == TIM_TS_ITR0) || \ + ((__SELECTION__) == TIM_TS_ITR1) || \ + ((__SELECTION__) == TIM_TS_ITR2) || \ + ((__SELECTION__) == TIM_TS_ITR3) || \ ((__SELECTION__) == TIM_TS_ITR4) || \ ((__SELECTION__) == TIM_TS_ITR6) || \ ((__SELECTION__) == TIM_TS_ITR7) || \ ((__SELECTION__) == TIM_TS_ITR8))) \ || \ (((INSTANCE) == TIM15) && \ - (((__SELECTION__) == TIM_TS_ITR0) || \ + (((__SELECTION__) == TIM_TS_TI1F_ED) || \ + ((__SELECTION__) == TIM_TS_TI1FP1) || \ + ((__SELECTION__) == TIM_TS_TI2FP2) || \ + ((__SELECTION__) == TIM_TS_ITR0) || \ ((__SELECTION__) == TIM_TS_ITR1) || \ ((__SELECTION__) == TIM_TS_ITR2) || \ ((__SELECTION__) == TIM_TS_ITR3) || \ - ((__SELECTION__) == TIM_TS_TI1F_ED) || \ - ((__SELECTION__) == TIM_TS_TI1FP1) || \ - ((__SELECTION__) == TIM_TS_TI2FP2) || \ ((__SELECTION__) == TIM_TS_ITR4) || \ ((__SELECTION__) == TIM_TS_ITR5) || \ ((__SELECTION__) == TIM_TS_ITR7) || \ @@ -1385,14 +1385,14 @@ typedef struct #define IS_TIM_CLOCKSOURCE_INSTANCE(INSTANCE, __CLOCK__) \ ((((INSTANCE) == TIM1) && \ (((__CLOCK__) == TIM_CLOCKSOURCE_INTERNAL) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE2) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ITR1) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ITR2) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ITR3) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_TI1ED) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_TI1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_TI2) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE1) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ITR1) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ITR2) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ITR3) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR5) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR6) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR7) || \ @@ -1401,14 +1401,14 @@ typedef struct || \ (((INSTANCE) == TIM2) && \ (((__CLOCK__) == TIM_CLOCKSOURCE_INTERNAL) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE2) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ITR0) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ITR2) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ITR3) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_TI1ED) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_TI1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_TI2) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE1) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ITR0) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ITR2) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ITR3) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR5) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR6) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR7) || \ @@ -1418,14 +1418,14 @@ typedef struct || \ (((INSTANCE) == TIM3) && \ (((__CLOCK__) == TIM_CLOCKSOURCE_INTERNAL) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE2) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ITR0) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ITR1) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ITR3) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_TI1ED) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_TI1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_TI2) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE1) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ITR0) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ITR1) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ITR3) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR5) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR6) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR7) || \ @@ -1434,14 +1434,14 @@ typedef struct || \ (((INSTANCE) == TIM4) && \ (((__CLOCK__) == TIM_CLOCKSOURCE_INTERNAL) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE2) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ITR0) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ITR1) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ITR2) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_TI1ED) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_TI1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_TI2) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE1) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ITR0) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ITR1) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ITR2) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR5) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR6) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR7) || \ @@ -1450,15 +1450,15 @@ typedef struct || \ (((INSTANCE) == TIM8) && \ (((__CLOCK__) == TIM_CLOCKSOURCE_INTERNAL) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE2) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_TI1ED) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_TI1) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_TI2) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR0) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR2) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR3) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_TI1ED) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_TI1) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_TI2) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR6) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR7) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR8) || \ @@ -1466,13 +1466,13 @@ typedef struct || \ (((INSTANCE) == TIM15) && \ (((__CLOCK__) == TIM_CLOCKSOURCE_INTERNAL) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_TI1ED) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_TI1) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_TI2) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR0) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR2) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR3) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_TI1ED) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_TI1) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_TI2) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR5) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR7) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR8) || \ @@ -1480,15 +1480,15 @@ typedef struct || \ (((INSTANCE) == TIM20) && \ (((__CLOCK__) == TIM_CLOCKSOURCE_INTERNAL) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE2) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_TI1ED) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_TI1) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_TI2) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR0) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR2) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR3) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_TI1ED) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_TI1) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_TI2) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR5) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR6) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR7) || \ @@ -1496,13 +1496,13 @@ typedef struct #define IS_TIM_TRIGGER_INSTANCE(INSTANCE, __SELECTION__) \ ((((INSTANCE) == TIM1) && \ - (((__SELECTION__) == TIM_TS_ITR1) || \ - ((__SELECTION__) == TIM_TS_ITR2) || \ - ((__SELECTION__) == TIM_TS_ITR3) || \ - ((__SELECTION__) == TIM_TS_TI1F_ED) || \ + (((__SELECTION__) == TIM_TS_TI1F_ED) || \ ((__SELECTION__) == TIM_TS_TI1FP1) || \ ((__SELECTION__) == TIM_TS_TI2FP2) || \ ((__SELECTION__) == TIM_TS_ETRF) || \ + ((__SELECTION__) == TIM_TS_ITR1) || \ + ((__SELECTION__) == TIM_TS_ITR2) || \ + ((__SELECTION__) == TIM_TS_ITR3) || \ ((__SELECTION__) == TIM_TS_ITR5) || \ ((__SELECTION__) == TIM_TS_ITR6) || \ ((__SELECTION__) == TIM_TS_ITR7) || \ @@ -1510,13 +1510,13 @@ typedef struct ((__SELECTION__) == TIM_TS_ITR9))) \ || \ (((INSTANCE) == TIM2) && \ - (((__SELECTION__) == TIM_TS_ITR0) || \ - ((__SELECTION__) == TIM_TS_ITR2) || \ - ((__SELECTION__) == TIM_TS_ITR3) || \ - ((__SELECTION__) == TIM_TS_TI1F_ED) || \ + (((__SELECTION__) == TIM_TS_TI1F_ED) || \ ((__SELECTION__) == TIM_TS_TI1FP1) || \ ((__SELECTION__) == TIM_TS_TI2FP2) || \ ((__SELECTION__) == TIM_TS_ETRF) || \ + ((__SELECTION__) == TIM_TS_ITR0) || \ + ((__SELECTION__) == TIM_TS_ITR2) || \ + ((__SELECTION__) == TIM_TS_ITR3) || \ ((__SELECTION__) == TIM_TS_ITR5) || \ ((__SELECTION__) == TIM_TS_ITR6) || \ ((__SELECTION__) == TIM_TS_ITR7) || \ @@ -1525,13 +1525,13 @@ typedef struct ((__SELECTION__) == TIM_TS_ITR11))) \ || \ (((INSTANCE) == TIM3) && \ - (((__SELECTION__) == TIM_TS_ITR0) || \ - ((__SELECTION__) == TIM_TS_ITR1) || \ - ((__SELECTION__) == TIM_TS_ITR3) || \ - ((__SELECTION__) == TIM_TS_TI1F_ED) || \ + (((__SELECTION__) == TIM_TS_TI1F_ED) || \ ((__SELECTION__) == TIM_TS_TI1FP1) || \ ((__SELECTION__) == TIM_TS_TI2FP2) || \ ((__SELECTION__) == TIM_TS_ETRF) || \ + ((__SELECTION__) == TIM_TS_ITR0) || \ + ((__SELECTION__) == TIM_TS_ITR1) || \ + ((__SELECTION__) == TIM_TS_ITR3) || \ ((__SELECTION__) == TIM_TS_ITR5) || \ ((__SELECTION__) == TIM_TS_ITR6) || \ ((__SELECTION__) == TIM_TS_ITR7) || \ @@ -1539,13 +1539,13 @@ typedef struct ((__SELECTION__) == TIM_TS_ITR9))) \ || \ (((INSTANCE) == TIM4) && \ - (((__SELECTION__) == TIM_TS_ITR0) || \ - ((__SELECTION__) == TIM_TS_ITR1) || \ - ((__SELECTION__) == TIM_TS_ITR2) || \ - ((__SELECTION__) == TIM_TS_TI1F_ED) || \ + (((__SELECTION__) == TIM_TS_TI1F_ED) || \ ((__SELECTION__) == TIM_TS_TI1FP1) || \ ((__SELECTION__) == TIM_TS_TI2FP2) || \ ((__SELECTION__) == TIM_TS_ETRF) || \ + ((__SELECTION__) == TIM_TS_ITR0) || \ + ((__SELECTION__) == TIM_TS_ITR1) || \ + ((__SELECTION__) == TIM_TS_ITR2) || \ ((__SELECTION__) == TIM_TS_ITR5) || \ ((__SELECTION__) == TIM_TS_ITR6) || \ ((__SELECTION__) == TIM_TS_ITR7) || \ @@ -1553,41 +1553,41 @@ typedef struct ((__SELECTION__) == TIM_TS_ITR9))) \ || \ (((INSTANCE) == TIM8) && \ - (((__SELECTION__) == TIM_TS_ITR0) || \ - ((__SELECTION__) == TIM_TS_ITR1) || \ - ((__SELECTION__) == TIM_TS_ITR2) || \ - ((__SELECTION__) == TIM_TS_ITR3) || \ - ((__SELECTION__) == TIM_TS_TI1F_ED) || \ + (((__SELECTION__) == TIM_TS_TI1F_ED) || \ ((__SELECTION__) == TIM_TS_TI1FP1) || \ ((__SELECTION__) == TIM_TS_TI2FP2) || \ ((__SELECTION__) == TIM_TS_ETRF) || \ + ((__SELECTION__) == TIM_TS_ITR0) || \ + ((__SELECTION__) == TIM_TS_ITR1) || \ + ((__SELECTION__) == TIM_TS_ITR2) || \ + ((__SELECTION__) == TIM_TS_ITR3) || \ ((__SELECTION__) == TIM_TS_ITR6) || \ ((__SELECTION__) == TIM_TS_ITR7) || \ ((__SELECTION__) == TIM_TS_ITR8) || \ ((__SELECTION__) == TIM_TS_ITR9))) \ || \ (((INSTANCE) == TIM15) && \ - (((__SELECTION__) == TIM_TS_ITR0) || \ + (((__SELECTION__) == TIM_TS_TI1F_ED) || \ + ((__SELECTION__) == TIM_TS_TI1FP1) || \ + ((__SELECTION__) == TIM_TS_TI2FP2) || \ + ((__SELECTION__) == TIM_TS_ITR0) || \ ((__SELECTION__) == TIM_TS_ITR1) || \ ((__SELECTION__) == TIM_TS_ITR2) || \ ((__SELECTION__) == TIM_TS_ITR3) || \ - ((__SELECTION__) == TIM_TS_TI1F_ED) || \ - ((__SELECTION__) == TIM_TS_TI1FP1) || \ - ((__SELECTION__) == TIM_TS_TI2FP2) || \ ((__SELECTION__) == TIM_TS_ITR5) || \ ((__SELECTION__) == TIM_TS_ITR7) || \ ((__SELECTION__) == TIM_TS_ITR8) || \ ((__SELECTION__) == TIM_TS_ITR9))) \ || \ (((INSTANCE) == TIM20) && \ - (((__SELECTION__) == TIM_TS_ITR0) || \ - ((__SELECTION__) == TIM_TS_ITR1) || \ - ((__SELECTION__) == TIM_TS_ITR2) || \ - ((__SELECTION__) == TIM_TS_ITR3) || \ - ((__SELECTION__) == TIM_TS_TI1F_ED) || \ + (((__SELECTION__) == TIM_TS_TI1F_ED) || \ ((__SELECTION__) == TIM_TS_TI1FP1) || \ ((__SELECTION__) == TIM_TS_TI2FP2) || \ ((__SELECTION__) == TIM_TS_ETRF) || \ + ((__SELECTION__) == TIM_TS_ITR0) || \ + ((__SELECTION__) == TIM_TS_ITR1) || \ + ((__SELECTION__) == TIM_TS_ITR2) || \ + ((__SELECTION__) == TIM_TS_ITR3) || \ ((__SELECTION__) == TIM_TS_ITR5) || \ ((__SELECTION__) == TIM_TS_ITR6) || \ ((__SELECTION__) == TIM_TS_ITR7) || \ @@ -1675,14 +1675,14 @@ typedef struct #define IS_TIM_CLOCKSOURCE_INSTANCE(INSTANCE, __CLOCK__) \ ((((INSTANCE) == TIM1) && \ (((__CLOCK__) == TIM_CLOCKSOURCE_INTERNAL) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE2) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ITR1) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ITR2) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ITR3) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_TI1ED) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_TI1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_TI2) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE1) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ITR1) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ITR2) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ITR3) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR5) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR6) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR7) || \ @@ -1691,13 +1691,13 @@ typedef struct (((INSTANCE) == TIM2) && \ (((__CLOCK__) == TIM_CLOCKSOURCE_INTERNAL) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE2) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ITR0) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ITR2) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ITR3) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_TI1ED) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_TI1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_TI2) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE1) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ITR0) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ITR2) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ITR3) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR5) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR6) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR7) || \ @@ -1706,14 +1706,14 @@ typedef struct || \ (((INSTANCE) == TIM3) && \ (((__CLOCK__) == TIM_CLOCKSOURCE_INTERNAL) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE2) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ITR0) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ITR1) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ITR3) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_TI1ED) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_TI1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_TI2) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE1) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ITR0) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ITR1) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ITR3) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR5) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR6) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR7) || \ @@ -1722,13 +1722,13 @@ typedef struct (((INSTANCE) == TIM4) && \ (((__CLOCK__) == TIM_CLOCKSOURCE_INTERNAL) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE2) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ITR0) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ITR1) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ITR2) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_TI1ED) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_TI1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_TI2) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE1) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ITR0) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ITR1) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ITR2) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR5) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR6) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR7) || \ @@ -1737,53 +1737,53 @@ typedef struct (((INSTANCE) == TIM8) && \ (((__CLOCK__) == TIM_CLOCKSOURCE_INTERNAL) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE2) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE1) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_TI1ED) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_TI1) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_TI2) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR0) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR2) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR3) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_TI1ED) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_TI1) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_TI2) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_ETRMODE1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR6) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR7) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR8))) \ || \ (((INSTANCE) == TIM15) && \ (((__CLOCK__) == TIM_CLOCKSOURCE_INTERNAL) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_TI1ED) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_TI1) || \ + ((__CLOCK__) == TIM_CLOCKSOURCE_TI2) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR0) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR1) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR2) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR3) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_TI1ED) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_TI1) || \ - ((__CLOCK__) == TIM_CLOCKSOURCE_TI2) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR5) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR7) || \ ((__CLOCK__) == TIM_CLOCKSOURCE_ITR8)))) #define IS_TIM_TRIGGER_INSTANCE(INSTANCE, __SELECTION__) \ ((((INSTANCE) == TIM1) && \ - (((__SELECTION__) == TIM_TS_ITR1) || \ - ((__SELECTION__) == TIM_TS_ITR2) || \ - ((__SELECTION__) == TIM_TS_ITR3) || \ - ((__SELECTION__) == TIM_TS_TI1F_ED) || \ + (((__SELECTION__) == TIM_TS_TI1F_ED) || \ ((__SELECTION__) == TIM_TS_TI1FP1) || \ ((__SELECTION__) == TIM_TS_TI2FP2) || \ ((__SELECTION__) == TIM_TS_ETRF) || \ + ((__SELECTION__) == TIM_TS_ITR1) || \ + ((__SELECTION__) == TIM_TS_ITR2) || \ + ((__SELECTION__) == TIM_TS_ITR3) || \ ((__SELECTION__) == TIM_TS_ITR5) || \ ((__SELECTION__) == TIM_TS_ITR6) || \ ((__SELECTION__) == TIM_TS_ITR7) || \ ((__SELECTION__) == TIM_TS_ITR8))) \ || \ (((INSTANCE) == TIM2) && \ - (((__SELECTION__) == TIM_TS_ITR0) || \ - ((__SELECTION__) == TIM_TS_ITR2) || \ - ((__SELECTION__) == TIM_TS_ITR3) || \ - ((__SELECTION__) == TIM_TS_TI1F_ED) || \ + (((__SELECTION__) == TIM_TS_TI1F_ED) || \ ((__SELECTION__) == TIM_TS_TI1FP1) || \ ((__SELECTION__) == TIM_TS_TI2FP2) || \ ((__SELECTION__) == TIM_TS_ETRF) || \ + ((__SELECTION__) == TIM_TS_ITR0) || \ + ((__SELECTION__) == TIM_TS_ITR2) || \ + ((__SELECTION__) == TIM_TS_ITR3) || \ ((__SELECTION__) == TIM_TS_ITR5) || \ ((__SELECTION__) == TIM_TS_ITR6) || \ ((__SELECTION__) == TIM_TS_ITR7) || \ @@ -1791,52 +1791,52 @@ typedef struct ((__SELECTION__) == TIM_TS_ITR11))) \ || \ (((INSTANCE) == TIM3) && \ - (((__SELECTION__) == TIM_TS_ITR0) || \ - ((__SELECTION__) == TIM_TS_ITR1) || \ - ((__SELECTION__) == TIM_TS_ITR3) || \ - ((__SELECTION__) == TIM_TS_TI1F_ED) || \ + (((__SELECTION__) == TIM_TS_TI1F_ED) || \ ((__SELECTION__) == TIM_TS_TI1FP1) || \ ((__SELECTION__) == TIM_TS_TI2FP2) || \ ((__SELECTION__) == TIM_TS_ETRF) || \ + ((__SELECTION__) == TIM_TS_ITR0) || \ + ((__SELECTION__) == TIM_TS_ITR1) || \ + ((__SELECTION__) == TIM_TS_ITR3) || \ ((__SELECTION__) == TIM_TS_ITR5) || \ ((__SELECTION__) == TIM_TS_ITR6) || \ ((__SELECTION__) == TIM_TS_ITR7) || \ ((__SELECTION__) == TIM_TS_ITR8))) \ || \ (((INSTANCE) == TIM4) && \ - (((__SELECTION__) == TIM_TS_ITR0) || \ - ((__SELECTION__) == TIM_TS_ITR1) || \ - ((__SELECTION__) == TIM_TS_ITR2) || \ - ((__SELECTION__) == TIM_TS_TI1F_ED) || \ + (((__SELECTION__) == TIM_TS_TI1F_ED) || \ ((__SELECTION__) == TIM_TS_TI1FP1) || \ ((__SELECTION__) == TIM_TS_TI2FP2) || \ ((__SELECTION__) == TIM_TS_ETRF) || \ + ((__SELECTION__) == TIM_TS_ITR0) || \ + ((__SELECTION__) == TIM_TS_ITR1) || \ + ((__SELECTION__) == TIM_TS_ITR2) || \ ((__SELECTION__) == TIM_TS_ITR5) || \ ((__SELECTION__) == TIM_TS_ITR6) || \ ((__SELECTION__) == TIM_TS_ITR7) || \ ((__SELECTION__) == TIM_TS_ITR8))) \ || \ (((INSTANCE) == TIM8) && \ - (((__SELECTION__) == TIM_TS_ITR0) || \ - ((__SELECTION__) == TIM_TS_ITR1) || \ - ((__SELECTION__) == TIM_TS_ITR2) || \ - ((__SELECTION__) == TIM_TS_ITR3) || \ - ((__SELECTION__) == TIM_TS_TI1F_ED) || \ + (((__SELECTION__) == TIM_TS_TI1F_ED) || \ ((__SELECTION__) == TIM_TS_TI1FP1) || \ ((__SELECTION__) == TIM_TS_TI2FP2) || \ ((__SELECTION__) == TIM_TS_ETRF) || \ + ((__SELECTION__) == TIM_TS_ITR0) || \ + ((__SELECTION__) == TIM_TS_ITR1) || \ + ((__SELECTION__) == TIM_TS_ITR2) || \ + ((__SELECTION__) == TIM_TS_ITR3) || \ ((__SELECTION__) == TIM_TS_ITR6) || \ ((__SELECTION__) == TIM_TS_ITR7) || \ ((__SELECTION__) == TIM_TS_ITR8))) \ || \ (((INSTANCE) == TIM15) && \ - (((__SELECTION__) == TIM_TS_ITR0) || \ + (((__SELECTION__) == TIM_TS_TI1F_ED) || \ + ((__SELECTION__) == TIM_TS_TI1FP1) || \ + ((__SELECTION__) == TIM_TS_TI2FP2) || \ + ((__SELECTION__) == TIM_TS_ITR0) || \ ((__SELECTION__) == TIM_TS_ITR1) || \ ((__SELECTION__) == TIM_TS_ITR2) || \ ((__SELECTION__) == TIM_TS_ITR3) || \ - ((__SELECTION__) == TIM_TS_TI1F_ED) || \ - ((__SELECTION__) == TIM_TS_TI1FP1) || \ - ((__SELECTION__) == TIM_TS_TI2FP2) || \ ((__SELECTION__) == TIM_TS_ITR5) || \ ((__SELECTION__) == TIM_TS_ITR7) || \ ((__SELECTION__) == TIM_TS_ITR8)))) @@ -1904,6 +1904,7 @@ typedef struct ((__SELECTION__) == TIM_TS_NONE)))) #endif /* TIM5 && TIM20 */ + #define IS_TIM_OC_CHANNEL_MODE(__MODE__, __CHANNEL__) \ (IS_TIM_OC_MODE(__MODE__) \ && ((((__MODE__) == TIM_OCMODE_DIRECTION_OUTPUT) || ((__MODE__) == TIM_OCMODE_PULSE_ON_COMPARE)) \ @@ -1942,7 +1943,6 @@ typedef struct #define IS_TIM_ENCODERINDEX_DIRECTION(__DIRECTION__) (((__DIRECTION__) == TIM_ENCODERINDEX_DIRECTION_UP_DOWN) || \ ((__DIRECTION__) == TIM_ENCODERINDEX_DIRECTION_UP) || \ ((__DIRECTION__) == TIM_ENCODERINDEX_DIRECTION_DOWN)) - /** * @} */ @@ -1958,7 +1958,7 @@ typedef struct * @{ */ /* Timer Hall Sensor functions **********************************************/ -HAL_StatusTypeDef HAL_TIMEx_HallSensor_Init(TIM_HandleTypeDef *htim, TIM_HallSensor_InitTypeDef *sConfig); +HAL_StatusTypeDef HAL_TIMEx_HallSensor_Init(TIM_HandleTypeDef *htim, const TIM_HallSensor_InitTypeDef *sConfig); HAL_StatusTypeDef HAL_TIMEx_HallSensor_DeInit(TIM_HandleTypeDef *htim); void HAL_TIMEx_HallSensor_MspInit(TIM_HandleTypeDef *htim); @@ -1991,7 +1991,8 @@ HAL_StatusTypeDef HAL_TIMEx_OCN_Start_IT(TIM_HandleTypeDef *htim, uint32_t Chann HAL_StatusTypeDef HAL_TIMEx_OCN_Stop_IT(TIM_HandleTypeDef *htim, uint32_t Channel); /* Non-Blocking mode: DMA */ -HAL_StatusTypeDef HAL_TIMEx_OCN_Start_DMA(TIM_HandleTypeDef *htim, uint32_t Channel, uint32_t *pData, uint16_t Length); +HAL_StatusTypeDef HAL_TIMEx_OCN_Start_DMA(TIM_HandleTypeDef *htim, uint32_t Channel, const uint32_t *pData, + uint16_t Length); HAL_StatusTypeDef HAL_TIMEx_OCN_Stop_DMA(TIM_HandleTypeDef *htim, uint32_t Channel); /** * @} @@ -2010,7 +2011,8 @@ HAL_StatusTypeDef HAL_TIMEx_PWMN_Stop(TIM_HandleTypeDef *htim, uint32_t Channel) HAL_StatusTypeDef HAL_TIMEx_PWMN_Start_IT(TIM_HandleTypeDef *htim, uint32_t Channel); HAL_StatusTypeDef HAL_TIMEx_PWMN_Stop_IT(TIM_HandleTypeDef *htim, uint32_t Channel); /* Non-Blocking mode: DMA */ -HAL_StatusTypeDef HAL_TIMEx_PWMN_Start_DMA(TIM_HandleTypeDef *htim, uint32_t Channel, uint32_t *pData, uint16_t Length); +HAL_StatusTypeDef HAL_TIMEx_PWMN_Start_DMA(TIM_HandleTypeDef *htim, uint32_t Channel, const uint32_t *pData, + uint16_t Length); HAL_StatusTypeDef HAL_TIMEx_PWMN_Stop_DMA(TIM_HandleTypeDef *htim, uint32_t Channel); /** * @} @@ -2044,17 +2046,17 @@ HAL_StatusTypeDef HAL_TIMEx_ConfigCommutEvent_IT(TIM_HandleTypeDef *htim, uint32 HAL_StatusTypeDef HAL_TIMEx_ConfigCommutEvent_DMA(TIM_HandleTypeDef *htim, uint32_t InputTrigger, uint32_t CommutationSource); HAL_StatusTypeDef HAL_TIMEx_MasterConfigSynchronization(TIM_HandleTypeDef *htim, - TIM_MasterConfigTypeDef *sMasterConfig); + const TIM_MasterConfigTypeDef *sMasterConfig); HAL_StatusTypeDef HAL_TIMEx_ConfigBreakDeadTime(TIM_HandleTypeDef *htim, - TIM_BreakDeadTimeConfigTypeDef *sBreakDeadTimeConfig); + const TIM_BreakDeadTimeConfigTypeDef *sBreakDeadTimeConfig); HAL_StatusTypeDef HAL_TIMEx_ConfigBreakInput(TIM_HandleTypeDef *htim, uint32_t BreakInput, - TIMEx_BreakInputConfigTypeDef *sBreakInputConfig); + const TIMEx_BreakInputConfigTypeDef *sBreakInputConfig); HAL_StatusTypeDef HAL_TIMEx_GroupChannel5(TIM_HandleTypeDef *htim, uint32_t Channels); HAL_StatusTypeDef HAL_TIMEx_RemapConfig(TIM_HandleTypeDef *htim, uint32_t Remap); HAL_StatusTypeDef HAL_TIMEx_TISelection(TIM_HandleTypeDef *htim, uint32_t TISelection, uint32_t Channel); HAL_StatusTypeDef HAL_TIMEx_DisarmBreakInput(TIM_HandleTypeDef *htim, uint32_t BreakInput); -HAL_StatusTypeDef HAL_TIMEx_ReArmBreakInput(TIM_HandleTypeDef *htim, uint32_t BreakInput); +HAL_StatusTypeDef HAL_TIMEx_ReArmBreakInput(const TIM_HandleTypeDef *htim, uint32_t BreakInput); HAL_StatusTypeDef HAL_TIMEx_DitheringEnable(TIM_HandleTypeDef *htim); HAL_StatusTypeDef HAL_TIMEx_DitheringDisable(TIM_HandleTypeDef *htim); HAL_StatusTypeDef HAL_TIMEx_OC_ConfigPulseOnCompare(TIM_HandleTypeDef *htim, uint32_t PulseWidthPrescaler, @@ -2100,8 +2102,8 @@ void HAL_TIMEx_TransitionErrorCallback(TIM_HandleTypeDef *htim); * @{ */ /* Extended Peripheral State functions ***************************************/ -HAL_TIM_StateTypeDef HAL_TIMEx_HallSensor_GetState(TIM_HandleTypeDef *htim); -HAL_TIM_ChannelStateTypeDef HAL_TIMEx_GetChannelNState(TIM_HandleTypeDef *htim, uint32_t ChannelN); +HAL_TIM_StateTypeDef HAL_TIMEx_HallSensor_GetState(const TIM_HandleTypeDef *htim); +HAL_TIM_ChannelStateTypeDef HAL_TIMEx_GetChannelNState(const TIM_HandleTypeDef *htim, uint32_t ChannelN); /** * @} */ diff --git a/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_ll_i2c.h b/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_ll_i2c.h index 935078195..5e92fc5b2 100644 --- a/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_ll_i2c.h +++ b/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_ll_i2c.h @@ -451,7 +451,7 @@ __STATIC_INLINE void LL_I2C_Disable(I2C_TypeDef *I2Cx) * @param I2Cx I2C Instance. * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_I2C_IsEnabled(I2C_TypeDef *I2Cx) +__STATIC_INLINE uint32_t LL_I2C_IsEnabled(const I2C_TypeDef *I2Cx) { return ((READ_BIT(I2Cx->CR1, I2C_CR1_PE) == (I2C_CR1_PE)) ? 1UL : 0UL); } @@ -500,7 +500,7 @@ __STATIC_INLINE void LL_I2C_SetDigitalFilter(I2C_TypeDef *I2Cx, uint32_t Digital * @param I2Cx I2C Instance. * @retval Value between Min_Data=0x0 and Max_Data=0xF */ -__STATIC_INLINE uint32_t LL_I2C_GetDigitalFilter(I2C_TypeDef *I2Cx) +__STATIC_INLINE uint32_t LL_I2C_GetDigitalFilter(const I2C_TypeDef *I2Cx) { return (uint32_t)(READ_BIT(I2Cx->CR1, I2C_CR1_DNF) >> I2C_CR1_DNF_Pos); } @@ -535,7 +535,7 @@ __STATIC_INLINE void LL_I2C_DisableAnalogFilter(I2C_TypeDef *I2Cx) * @param I2Cx I2C Instance. * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_I2C_IsEnabledAnalogFilter(I2C_TypeDef *I2Cx) +__STATIC_INLINE uint32_t LL_I2C_IsEnabledAnalogFilter(const I2C_TypeDef *I2Cx) { return ((READ_BIT(I2Cx->CR1, I2C_CR1_ANFOFF) != (I2C_CR1_ANFOFF)) ? 1UL : 0UL); } @@ -568,7 +568,7 @@ __STATIC_INLINE void LL_I2C_DisableDMAReq_TX(I2C_TypeDef *I2Cx) * @param I2Cx I2C Instance. * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_I2C_IsEnabledDMAReq_TX(I2C_TypeDef *I2Cx) +__STATIC_INLINE uint32_t LL_I2C_IsEnabledDMAReq_TX(const I2C_TypeDef *I2Cx) { return ((READ_BIT(I2Cx->CR1, I2C_CR1_TXDMAEN) == (I2C_CR1_TXDMAEN)) ? 1UL : 0UL); } @@ -601,7 +601,7 @@ __STATIC_INLINE void LL_I2C_DisableDMAReq_RX(I2C_TypeDef *I2Cx) * @param I2Cx I2C Instance. * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_I2C_IsEnabledDMAReq_RX(I2C_TypeDef *I2Cx) +__STATIC_INLINE uint32_t LL_I2C_IsEnabledDMAReq_RX(const I2C_TypeDef *I2Cx) { return ((READ_BIT(I2Cx->CR1, I2C_CR1_RXDMAEN) == (I2C_CR1_RXDMAEN)) ? 1UL : 0UL); } @@ -616,7 +616,7 @@ __STATIC_INLINE uint32_t LL_I2C_IsEnabledDMAReq_RX(I2C_TypeDef *I2Cx) * @arg @ref LL_I2C_DMA_REG_DATA_RECEIVE * @retval Address of data register */ -__STATIC_INLINE uint32_t LL_I2C_DMA_GetRegAddr(I2C_TypeDef *I2Cx, uint32_t Direction) +__STATIC_INLINE uint32_t LL_I2C_DMA_GetRegAddr(const I2C_TypeDef *I2Cx, uint32_t Direction) { uint32_t data_reg_addr; @@ -664,7 +664,7 @@ __STATIC_INLINE void LL_I2C_DisableClockStretching(I2C_TypeDef *I2Cx) * @param I2Cx I2C Instance. * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_I2C_IsEnabledClockStretching(I2C_TypeDef *I2Cx) +__STATIC_INLINE uint32_t LL_I2C_IsEnabledClockStretching(const I2C_TypeDef *I2Cx) { return ((READ_BIT(I2Cx->CR1, I2C_CR1_NOSTRETCH) != (I2C_CR1_NOSTRETCH)) ? 1UL : 0UL); } @@ -697,7 +697,7 @@ __STATIC_INLINE void LL_I2C_DisableSlaveByteControl(I2C_TypeDef *I2Cx) * @param I2Cx I2C Instance. * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_I2C_IsEnabledSlaveByteControl(I2C_TypeDef *I2Cx) +__STATIC_INLINE uint32_t LL_I2C_IsEnabledSlaveByteControl(const I2C_TypeDef *I2Cx) { return ((READ_BIT(I2Cx->CR1, I2C_CR1_SBC) == (I2C_CR1_SBC)) ? 1UL : 0UL); } @@ -737,7 +737,7 @@ __STATIC_INLINE void LL_I2C_DisableWakeUpFromStop(I2C_TypeDef *I2Cx) * @param I2Cx I2C Instance. * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_I2C_IsEnabledWakeUpFromStop(I2C_TypeDef *I2Cx) +__STATIC_INLINE uint32_t LL_I2C_IsEnabledWakeUpFromStop(const I2C_TypeDef *I2Cx) { return ((READ_BIT(I2Cx->CR1, I2C_CR1_WUPEN) == (I2C_CR1_WUPEN)) ? 1UL : 0UL); } @@ -772,7 +772,7 @@ __STATIC_INLINE void LL_I2C_DisableGeneralCall(I2C_TypeDef *I2Cx) * @param I2Cx I2C Instance. * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_I2C_IsEnabledGeneralCall(I2C_TypeDef *I2Cx) +__STATIC_INLINE uint32_t LL_I2C_IsEnabledGeneralCall(const I2C_TypeDef *I2Cx) { return ((READ_BIT(I2Cx->CR1, I2C_CR1_GCEN) == (I2C_CR1_GCEN)) ? 1UL : 0UL); } @@ -800,7 +800,7 @@ __STATIC_INLINE void LL_I2C_SetMasterAddressingMode(I2C_TypeDef *I2Cx, uint32_t * @arg @ref LL_I2C_ADDRESSING_MODE_7BIT * @arg @ref LL_I2C_ADDRESSING_MODE_10BIT */ -__STATIC_INLINE uint32_t LL_I2C_GetMasterAddressingMode(I2C_TypeDef *I2Cx) +__STATIC_INLINE uint32_t LL_I2C_GetMasterAddressingMode(const I2C_TypeDef *I2Cx) { return (uint32_t)(READ_BIT(I2Cx->CR2, I2C_CR2_ADD10)); } @@ -849,7 +849,7 @@ __STATIC_INLINE void LL_I2C_DisableOwnAddress1(I2C_TypeDef *I2Cx) * @param I2Cx I2C Instance. * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_I2C_IsEnabledOwnAddress1(I2C_TypeDef *I2Cx) +__STATIC_INLINE uint32_t LL_I2C_IsEnabledOwnAddress1(const I2C_TypeDef *I2Cx) { return ((READ_BIT(I2Cx->OAR1, I2C_OAR1_OA1EN) == (I2C_OAR1_OA1EN)) ? 1UL : 0UL); } @@ -905,7 +905,7 @@ __STATIC_INLINE void LL_I2C_DisableOwnAddress2(I2C_TypeDef *I2Cx) * @param I2Cx I2C Instance. * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_I2C_IsEnabledOwnAddress2(I2C_TypeDef *I2Cx) +__STATIC_INLINE uint32_t LL_I2C_IsEnabledOwnAddress2(const I2C_TypeDef *I2Cx) { return ((READ_BIT(I2Cx->OAR2, I2C_OAR2_OA2EN) == (I2C_OAR2_OA2EN)) ? 1UL : 0UL); } @@ -930,7 +930,7 @@ __STATIC_INLINE void LL_I2C_SetTiming(I2C_TypeDef *I2Cx, uint32_t Timing) * @param I2Cx I2C Instance. * @retval Value between Min_Data=0x0 and Max_Data=0xF */ -__STATIC_INLINE uint32_t LL_I2C_GetTimingPrescaler(I2C_TypeDef *I2Cx) +__STATIC_INLINE uint32_t LL_I2C_GetTimingPrescaler(const I2C_TypeDef *I2Cx) { return (uint32_t)(READ_BIT(I2Cx->TIMINGR, I2C_TIMINGR_PRESC) >> I2C_TIMINGR_PRESC_Pos); } @@ -941,7 +941,7 @@ __STATIC_INLINE uint32_t LL_I2C_GetTimingPrescaler(I2C_TypeDef *I2Cx) * @param I2Cx I2C Instance. * @retval Value between Min_Data=0x00 and Max_Data=0xFF */ -__STATIC_INLINE uint32_t LL_I2C_GetClockLowPeriod(I2C_TypeDef *I2Cx) +__STATIC_INLINE uint32_t LL_I2C_GetClockLowPeriod(const I2C_TypeDef *I2Cx) { return (uint32_t)(READ_BIT(I2Cx->TIMINGR, I2C_TIMINGR_SCLL) >> I2C_TIMINGR_SCLL_Pos); } @@ -952,7 +952,7 @@ __STATIC_INLINE uint32_t LL_I2C_GetClockLowPeriod(I2C_TypeDef *I2Cx) * @param I2Cx I2C Instance. * @retval Value between Min_Data=0x00 and Max_Data=0xFF */ -__STATIC_INLINE uint32_t LL_I2C_GetClockHighPeriod(I2C_TypeDef *I2Cx) +__STATIC_INLINE uint32_t LL_I2C_GetClockHighPeriod(const I2C_TypeDef *I2Cx) { return (uint32_t)(READ_BIT(I2Cx->TIMINGR, I2C_TIMINGR_SCLH) >> I2C_TIMINGR_SCLH_Pos); } @@ -963,7 +963,7 @@ __STATIC_INLINE uint32_t LL_I2C_GetClockHighPeriod(I2C_TypeDef *I2Cx) * @param I2Cx I2C Instance. * @retval Value between Min_Data=0x0 and Max_Data=0xF */ -__STATIC_INLINE uint32_t LL_I2C_GetDataHoldTime(I2C_TypeDef *I2Cx) +__STATIC_INLINE uint32_t LL_I2C_GetDataHoldTime(const I2C_TypeDef *I2Cx) { return (uint32_t)(READ_BIT(I2Cx->TIMINGR, I2C_TIMINGR_SDADEL) >> I2C_TIMINGR_SDADEL_Pos); } @@ -974,7 +974,7 @@ __STATIC_INLINE uint32_t LL_I2C_GetDataHoldTime(I2C_TypeDef *I2Cx) * @param I2Cx I2C Instance. * @retval Value between Min_Data=0x0 and Max_Data=0xF */ -__STATIC_INLINE uint32_t LL_I2C_GetDataSetupTime(I2C_TypeDef *I2Cx) +__STATIC_INLINE uint32_t LL_I2C_GetDataSetupTime(const I2C_TypeDef *I2Cx) { return (uint32_t)(READ_BIT(I2Cx->TIMINGR, I2C_TIMINGR_SCLDEL) >> I2C_TIMINGR_SCLDEL_Pos); } @@ -1011,7 +1011,7 @@ __STATIC_INLINE void LL_I2C_SetMode(I2C_TypeDef *I2Cx, uint32_t PeripheralMode) * @arg @ref LL_I2C_MODE_SMBUS_DEVICE * @arg @ref LL_I2C_MODE_SMBUS_DEVICE_ARP */ -__STATIC_INLINE uint32_t LL_I2C_GetMode(I2C_TypeDef *I2Cx) +__STATIC_INLINE uint32_t LL_I2C_GetMode(const I2C_TypeDef *I2Cx) { return (uint32_t)(READ_BIT(I2Cx->CR1, I2C_CR1_SMBHEN | I2C_CR1_SMBDEN)); } @@ -1060,7 +1060,7 @@ __STATIC_INLINE void LL_I2C_DisableSMBusAlert(I2C_TypeDef *I2Cx) * @param I2Cx I2C Instance. * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_I2C_IsEnabledSMBusAlert(I2C_TypeDef *I2Cx) +__STATIC_INLINE uint32_t LL_I2C_IsEnabledSMBusAlert(const I2C_TypeDef *I2Cx) { return ((READ_BIT(I2Cx->CR1, I2C_CR1_ALERTEN) == (I2C_CR1_ALERTEN)) ? 1UL : 0UL); } @@ -1099,7 +1099,7 @@ __STATIC_INLINE void LL_I2C_DisableSMBusPEC(I2C_TypeDef *I2Cx) * @param I2Cx I2C Instance. * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_I2C_IsEnabledSMBusPEC(I2C_TypeDef *I2Cx) +__STATIC_INLINE uint32_t LL_I2C_IsEnabledSMBusPEC(const I2C_TypeDef *I2Cx) { return ((READ_BIT(I2Cx->CR1, I2C_CR1_PECEN) == (I2C_CR1_PECEN)) ? 1UL : 0UL); } @@ -1150,7 +1150,7 @@ __STATIC_INLINE void LL_I2C_SetSMBusTimeoutA(I2C_TypeDef *I2Cx, uint32_t Timeout * @param I2Cx I2C Instance. * @retval Value between Min_Data=0 and Max_Data=0xFFF */ -__STATIC_INLINE uint32_t LL_I2C_GetSMBusTimeoutA(I2C_TypeDef *I2Cx) +__STATIC_INLINE uint32_t LL_I2C_GetSMBusTimeoutA(const I2C_TypeDef *I2Cx) { return (uint32_t)(READ_BIT(I2Cx->TIMEOUTR, I2C_TIMEOUTR_TIMEOUTA)); } @@ -1182,7 +1182,7 @@ __STATIC_INLINE void LL_I2C_SetSMBusTimeoutAMode(I2C_TypeDef *I2Cx, uint32_t Tim * @arg @ref LL_I2C_SMBUS_TIMEOUTA_MODE_SCL_LOW * @arg @ref LL_I2C_SMBUS_TIMEOUTA_MODE_SDA_SCL_HIGH */ -__STATIC_INLINE uint32_t LL_I2C_GetSMBusTimeoutAMode(I2C_TypeDef *I2Cx) +__STATIC_INLINE uint32_t LL_I2C_GetSMBusTimeoutAMode(const I2C_TypeDef *I2Cx) { return (uint32_t)(READ_BIT(I2Cx->TIMEOUTR, I2C_TIMEOUTR_TIDLE)); } @@ -1210,7 +1210,7 @@ __STATIC_INLINE void LL_I2C_SetSMBusTimeoutB(I2C_TypeDef *I2Cx, uint32_t Timeout * @param I2Cx I2C Instance. * @retval Value between Min_Data=0 and Max_Data=0xFFF */ -__STATIC_INLINE uint32_t LL_I2C_GetSMBusTimeoutB(I2C_TypeDef *I2Cx) +__STATIC_INLINE uint32_t LL_I2C_GetSMBusTimeoutB(const I2C_TypeDef *I2Cx) { return (uint32_t)(READ_BIT(I2Cx->TIMEOUTR, I2C_TIMEOUTR_TIMEOUTB) >> I2C_TIMEOUTR_TIMEOUTB_Pos); } @@ -1264,7 +1264,7 @@ __STATIC_INLINE void LL_I2C_DisableSMBusTimeout(I2C_TypeDef *I2Cx, uint32_t Cloc * @arg @ref LL_I2C_SMBUS_ALL_TIMEOUT * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_I2C_IsEnabledSMBusTimeout(I2C_TypeDef *I2Cx, uint32_t ClockTimeout) +__STATIC_INLINE uint32_t LL_I2C_IsEnabledSMBusTimeout(const I2C_TypeDef *I2Cx, uint32_t ClockTimeout) { return ((READ_BIT(I2Cx->TIMEOUTR, (I2C_TIMEOUTR_TIMOUTEN | I2C_TIMEOUTR_TEXTEN)) == \ (ClockTimeout)) ? 1UL : 0UL); @@ -1306,7 +1306,7 @@ __STATIC_INLINE void LL_I2C_DisableIT_TX(I2C_TypeDef *I2Cx) * @param I2Cx I2C Instance. * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_I2C_IsEnabledIT_TX(I2C_TypeDef *I2Cx) +__STATIC_INLINE uint32_t LL_I2C_IsEnabledIT_TX(const I2C_TypeDef *I2Cx) { return ((READ_BIT(I2Cx->CR1, I2C_CR1_TXIE) == (I2C_CR1_TXIE)) ? 1UL : 0UL); } @@ -1339,7 +1339,7 @@ __STATIC_INLINE void LL_I2C_DisableIT_RX(I2C_TypeDef *I2Cx) * @param I2Cx I2C Instance. * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_I2C_IsEnabledIT_RX(I2C_TypeDef *I2Cx) +__STATIC_INLINE uint32_t LL_I2C_IsEnabledIT_RX(const I2C_TypeDef *I2Cx) { return ((READ_BIT(I2Cx->CR1, I2C_CR1_RXIE) == (I2C_CR1_RXIE)) ? 1UL : 0UL); } @@ -1372,7 +1372,7 @@ __STATIC_INLINE void LL_I2C_DisableIT_ADDR(I2C_TypeDef *I2Cx) * @param I2Cx I2C Instance. * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_I2C_IsEnabledIT_ADDR(I2C_TypeDef *I2Cx) +__STATIC_INLINE uint32_t LL_I2C_IsEnabledIT_ADDR(const I2C_TypeDef *I2Cx) { return ((READ_BIT(I2Cx->CR1, I2C_CR1_ADDRIE) == (I2C_CR1_ADDRIE)) ? 1UL : 0UL); } @@ -1405,7 +1405,7 @@ __STATIC_INLINE void LL_I2C_DisableIT_NACK(I2C_TypeDef *I2Cx) * @param I2Cx I2C Instance. * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_I2C_IsEnabledIT_NACK(I2C_TypeDef *I2Cx) +__STATIC_INLINE uint32_t LL_I2C_IsEnabledIT_NACK(const I2C_TypeDef *I2Cx) { return ((READ_BIT(I2Cx->CR1, I2C_CR1_NACKIE) == (I2C_CR1_NACKIE)) ? 1UL : 0UL); } @@ -1438,7 +1438,7 @@ __STATIC_INLINE void LL_I2C_DisableIT_STOP(I2C_TypeDef *I2Cx) * @param I2Cx I2C Instance. * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_I2C_IsEnabledIT_STOP(I2C_TypeDef *I2Cx) +__STATIC_INLINE uint32_t LL_I2C_IsEnabledIT_STOP(const I2C_TypeDef *I2Cx) { return ((READ_BIT(I2Cx->CR1, I2C_CR1_STOPIE) == (I2C_CR1_STOPIE)) ? 1UL : 0UL); } @@ -1477,7 +1477,7 @@ __STATIC_INLINE void LL_I2C_DisableIT_TC(I2C_TypeDef *I2Cx) * @param I2Cx I2C Instance. * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_I2C_IsEnabledIT_TC(I2C_TypeDef *I2Cx) +__STATIC_INLINE uint32_t LL_I2C_IsEnabledIT_TC(const I2C_TypeDef *I2Cx) { return ((READ_BIT(I2Cx->CR1, I2C_CR1_TCIE) == (I2C_CR1_TCIE)) ? 1UL : 0UL); } @@ -1528,7 +1528,7 @@ __STATIC_INLINE void LL_I2C_DisableIT_ERR(I2C_TypeDef *I2Cx) * @param I2Cx I2C Instance. * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_I2C_IsEnabledIT_ERR(I2C_TypeDef *I2Cx) +__STATIC_INLINE uint32_t LL_I2C_IsEnabledIT_ERR(const I2C_TypeDef *I2Cx) { return ((READ_BIT(I2Cx->CR1, I2C_CR1_ERRIE) == (I2C_CR1_ERRIE)) ? 1UL : 0UL); } @@ -1549,7 +1549,7 @@ __STATIC_INLINE uint32_t LL_I2C_IsEnabledIT_ERR(I2C_TypeDef *I2Cx) * @param I2Cx I2C Instance. * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_I2C_IsActiveFlag_TXE(I2C_TypeDef *I2Cx) +__STATIC_INLINE uint32_t LL_I2C_IsActiveFlag_TXE(const I2C_TypeDef *I2Cx) { return ((READ_BIT(I2Cx->ISR, I2C_ISR_TXE) == (I2C_ISR_TXE)) ? 1UL : 0UL); } @@ -1562,7 +1562,7 @@ __STATIC_INLINE uint32_t LL_I2C_IsActiveFlag_TXE(I2C_TypeDef *I2Cx) * @param I2Cx I2C Instance. * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_I2C_IsActiveFlag_TXIS(I2C_TypeDef *I2Cx) +__STATIC_INLINE uint32_t LL_I2C_IsActiveFlag_TXIS(const I2C_TypeDef *I2Cx) { return ((READ_BIT(I2Cx->ISR, I2C_ISR_TXIS) == (I2C_ISR_TXIS)) ? 1UL : 0UL); } @@ -1575,7 +1575,7 @@ __STATIC_INLINE uint32_t LL_I2C_IsActiveFlag_TXIS(I2C_TypeDef *I2Cx) * @param I2Cx I2C Instance. * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_I2C_IsActiveFlag_RXNE(I2C_TypeDef *I2Cx) +__STATIC_INLINE uint32_t LL_I2C_IsActiveFlag_RXNE(const I2C_TypeDef *I2Cx) { return ((READ_BIT(I2Cx->ISR, I2C_ISR_RXNE) == (I2C_ISR_RXNE)) ? 1UL : 0UL); } @@ -1588,7 +1588,7 @@ __STATIC_INLINE uint32_t LL_I2C_IsActiveFlag_RXNE(I2C_TypeDef *I2Cx) * @param I2Cx I2C Instance. * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_I2C_IsActiveFlag_ADDR(I2C_TypeDef *I2Cx) +__STATIC_INLINE uint32_t LL_I2C_IsActiveFlag_ADDR(const I2C_TypeDef *I2Cx) { return ((READ_BIT(I2Cx->ISR, I2C_ISR_ADDR) == (I2C_ISR_ADDR)) ? 1UL : 0UL); } @@ -1601,7 +1601,7 @@ __STATIC_INLINE uint32_t LL_I2C_IsActiveFlag_ADDR(I2C_TypeDef *I2Cx) * @param I2Cx I2C Instance. * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_I2C_IsActiveFlag_NACK(I2C_TypeDef *I2Cx) +__STATIC_INLINE uint32_t LL_I2C_IsActiveFlag_NACK(const I2C_TypeDef *I2Cx) { return ((READ_BIT(I2Cx->ISR, I2C_ISR_NACKF) == (I2C_ISR_NACKF)) ? 1UL : 0UL); } @@ -1614,7 +1614,7 @@ __STATIC_INLINE uint32_t LL_I2C_IsActiveFlag_NACK(I2C_TypeDef *I2Cx) * @param I2Cx I2C Instance. * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_I2C_IsActiveFlag_STOP(I2C_TypeDef *I2Cx) +__STATIC_INLINE uint32_t LL_I2C_IsActiveFlag_STOP(const I2C_TypeDef *I2Cx) { return ((READ_BIT(I2Cx->ISR, I2C_ISR_STOPF) == (I2C_ISR_STOPF)) ? 1UL : 0UL); } @@ -1627,7 +1627,7 @@ __STATIC_INLINE uint32_t LL_I2C_IsActiveFlag_STOP(I2C_TypeDef *I2Cx) * @param I2Cx I2C Instance. * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_I2C_IsActiveFlag_TC(I2C_TypeDef *I2Cx) +__STATIC_INLINE uint32_t LL_I2C_IsActiveFlag_TC(const I2C_TypeDef *I2Cx) { return ((READ_BIT(I2Cx->ISR, I2C_ISR_TC) == (I2C_ISR_TC)) ? 1UL : 0UL); } @@ -1640,7 +1640,7 @@ __STATIC_INLINE uint32_t LL_I2C_IsActiveFlag_TC(I2C_TypeDef *I2Cx) * @param I2Cx I2C Instance. * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_I2C_IsActiveFlag_TCR(I2C_TypeDef *I2Cx) +__STATIC_INLINE uint32_t LL_I2C_IsActiveFlag_TCR(const I2C_TypeDef *I2Cx) { return ((READ_BIT(I2Cx->ISR, I2C_ISR_TCR) == (I2C_ISR_TCR)) ? 1UL : 0UL); } @@ -1653,7 +1653,7 @@ __STATIC_INLINE uint32_t LL_I2C_IsActiveFlag_TCR(I2C_TypeDef *I2Cx) * @param I2Cx I2C Instance. * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_I2C_IsActiveFlag_BERR(I2C_TypeDef *I2Cx) +__STATIC_INLINE uint32_t LL_I2C_IsActiveFlag_BERR(const I2C_TypeDef *I2Cx) { return ((READ_BIT(I2Cx->ISR, I2C_ISR_BERR) == (I2C_ISR_BERR)) ? 1UL : 0UL); } @@ -1666,7 +1666,7 @@ __STATIC_INLINE uint32_t LL_I2C_IsActiveFlag_BERR(I2C_TypeDef *I2Cx) * @param I2Cx I2C Instance. * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_I2C_IsActiveFlag_ARLO(I2C_TypeDef *I2Cx) +__STATIC_INLINE uint32_t LL_I2C_IsActiveFlag_ARLO(const I2C_TypeDef *I2Cx) { return ((READ_BIT(I2Cx->ISR, I2C_ISR_ARLO) == (I2C_ISR_ARLO)) ? 1UL : 0UL); } @@ -1679,7 +1679,7 @@ __STATIC_INLINE uint32_t LL_I2C_IsActiveFlag_ARLO(I2C_TypeDef *I2Cx) * @param I2Cx I2C Instance. * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_I2C_IsActiveFlag_OVR(I2C_TypeDef *I2Cx) +__STATIC_INLINE uint32_t LL_I2C_IsActiveFlag_OVR(const I2C_TypeDef *I2Cx) { return ((READ_BIT(I2Cx->ISR, I2C_ISR_OVR) == (I2C_ISR_OVR)) ? 1UL : 0UL); } @@ -1694,7 +1694,7 @@ __STATIC_INLINE uint32_t LL_I2C_IsActiveFlag_OVR(I2C_TypeDef *I2Cx) * @param I2Cx I2C Instance. * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_I2C_IsActiveSMBusFlag_PECERR(I2C_TypeDef *I2Cx) +__STATIC_INLINE uint32_t LL_I2C_IsActiveSMBusFlag_PECERR(const I2C_TypeDef *I2Cx) { return ((READ_BIT(I2Cx->ISR, I2C_ISR_PECERR) == (I2C_ISR_PECERR)) ? 1UL : 0UL); } @@ -1709,7 +1709,7 @@ __STATIC_INLINE uint32_t LL_I2C_IsActiveSMBusFlag_PECERR(I2C_TypeDef *I2Cx) * @param I2Cx I2C Instance. * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_I2C_IsActiveSMBusFlag_TIMEOUT(I2C_TypeDef *I2Cx) +__STATIC_INLINE uint32_t LL_I2C_IsActiveSMBusFlag_TIMEOUT(const I2C_TypeDef *I2Cx) { return ((READ_BIT(I2Cx->ISR, I2C_ISR_TIMEOUT) == (I2C_ISR_TIMEOUT)) ? 1UL : 0UL); } @@ -1725,7 +1725,7 @@ __STATIC_INLINE uint32_t LL_I2C_IsActiveSMBusFlag_TIMEOUT(I2C_TypeDef *I2Cx) * @param I2Cx I2C Instance. * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_I2C_IsActiveSMBusFlag_ALERT(I2C_TypeDef *I2Cx) +__STATIC_INLINE uint32_t LL_I2C_IsActiveSMBusFlag_ALERT(const I2C_TypeDef *I2Cx) { return ((READ_BIT(I2Cx->ISR, I2C_ISR_ALERT) == (I2C_ISR_ALERT)) ? 1UL : 0UL); } @@ -1738,7 +1738,7 @@ __STATIC_INLINE uint32_t LL_I2C_IsActiveSMBusFlag_ALERT(I2C_TypeDef *I2Cx) * @param I2Cx I2C Instance. * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_I2C_IsActiveFlag_BUSY(I2C_TypeDef *I2Cx) +__STATIC_INLINE uint32_t LL_I2C_IsActiveFlag_BUSY(const I2C_TypeDef *I2Cx) { return ((READ_BIT(I2Cx->ISR, I2C_ISR_BUSY) == (I2C_ISR_BUSY)) ? 1UL : 0UL); } @@ -1899,7 +1899,7 @@ __STATIC_INLINE void LL_I2C_DisableAutoEndMode(I2C_TypeDef *I2Cx) * @param I2Cx I2C Instance. * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_I2C_IsEnabledAutoEndMode(I2C_TypeDef *I2Cx) +__STATIC_INLINE uint32_t LL_I2C_IsEnabledAutoEndMode(const I2C_TypeDef *I2Cx) { return ((READ_BIT(I2Cx->CR2, I2C_CR2_AUTOEND) == (I2C_CR2_AUTOEND)) ? 1UL : 0UL); } @@ -1934,7 +1934,7 @@ __STATIC_INLINE void LL_I2C_DisableReloadMode(I2C_TypeDef *I2Cx) * @param I2Cx I2C Instance. * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_I2C_IsEnabledReloadMode(I2C_TypeDef *I2Cx) +__STATIC_INLINE uint32_t LL_I2C_IsEnabledReloadMode(const I2C_TypeDef *I2Cx) { return ((READ_BIT(I2Cx->CR2, I2C_CR2_RELOAD) == (I2C_CR2_RELOAD)) ? 1UL : 0UL); } @@ -1958,7 +1958,7 @@ __STATIC_INLINE void LL_I2C_SetTransferSize(I2C_TypeDef *I2Cx, uint32_t Transfer * @param I2Cx I2C Instance. * @retval Value between Min_Data=0x0 and Max_Data=0xFF */ -__STATIC_INLINE uint32_t LL_I2C_GetTransferSize(I2C_TypeDef *I2Cx) +__STATIC_INLINE uint32_t LL_I2C_GetTransferSize(const I2C_TypeDef *I2Cx) { return (uint32_t)(READ_BIT(I2Cx->CR2, I2C_CR2_NBYTES) >> I2C_CR2_NBYTES_Pos); } @@ -2035,7 +2035,7 @@ __STATIC_INLINE void LL_I2C_DisableAuto10BitRead(I2C_TypeDef *I2Cx) * @param I2Cx I2C Instance. * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_I2C_IsEnabledAuto10BitRead(I2C_TypeDef *I2Cx) +__STATIC_INLINE uint32_t LL_I2C_IsEnabledAuto10BitRead(const I2C_TypeDef *I2Cx) { return ((READ_BIT(I2Cx->CR2, I2C_CR2_HEAD10R) != (I2C_CR2_HEAD10R)) ? 1UL : 0UL); } @@ -2063,7 +2063,7 @@ __STATIC_INLINE void LL_I2C_SetTransferRequest(I2C_TypeDef *I2Cx, uint32_t Trans * @arg @ref LL_I2C_REQUEST_WRITE * @arg @ref LL_I2C_REQUEST_READ */ -__STATIC_INLINE uint32_t LL_I2C_GetTransferRequest(I2C_TypeDef *I2Cx) +__STATIC_INLINE uint32_t LL_I2C_GetTransferRequest(const I2C_TypeDef *I2Cx) { return (uint32_t)(READ_BIT(I2Cx->CR2, I2C_CR2_RD_WRN)); } @@ -2087,7 +2087,7 @@ __STATIC_INLINE void LL_I2C_SetSlaveAddr(I2C_TypeDef *I2Cx, uint32_t SlaveAddr) * @param I2Cx I2C Instance. * @retval Value between Min_Data=0x0 and Max_Data=0x3F */ -__STATIC_INLINE uint32_t LL_I2C_GetSlaveAddr(I2C_TypeDef *I2Cx) +__STATIC_INLINE uint32_t LL_I2C_GetSlaveAddr(const I2C_TypeDef *I2Cx) { return (uint32_t)(READ_BIT(I2Cx->CR2, I2C_CR2_SADD)); } @@ -2133,11 +2133,18 @@ __STATIC_INLINE uint32_t LL_I2C_GetSlaveAddr(I2C_TypeDef *I2Cx) __STATIC_INLINE void LL_I2C_HandleTransfer(I2C_TypeDef *I2Cx, uint32_t SlaveAddr, uint32_t SlaveAddrSize, uint32_t TransferSize, uint32_t EndMode, uint32_t Request) { + /* Declaration of tmp to prevent undefined behavior of volatile usage */ + uint32_t tmp = ((uint32_t)(((uint32_t)SlaveAddr & I2C_CR2_SADD) | \ + ((uint32_t)SlaveAddrSize & I2C_CR2_ADD10) | \ + (((uint32_t)TransferSize << I2C_CR2_NBYTES_Pos) & I2C_CR2_NBYTES) | \ + (uint32_t)EndMode | (uint32_t)Request) & (~0x80000000U)); + + /* update CR2 register */ MODIFY_REG(I2Cx->CR2, I2C_CR2_SADD | I2C_CR2_ADD10 | (I2C_CR2_RD_WRN & (uint32_t)(Request >> (31U - I2C_CR2_RD_WRN_Pos))) | I2C_CR2_START | I2C_CR2_STOP | I2C_CR2_RELOAD | I2C_CR2_NBYTES | I2C_CR2_AUTOEND | I2C_CR2_HEAD10R, - SlaveAddr | SlaveAddrSize | (TransferSize << I2C_CR2_NBYTES_Pos) | EndMode | Request); + tmp); } /** @@ -2150,7 +2157,7 @@ __STATIC_INLINE void LL_I2C_HandleTransfer(I2C_TypeDef *I2Cx, uint32_t SlaveAddr * @arg @ref LL_I2C_DIRECTION_WRITE * @arg @ref LL_I2C_DIRECTION_READ */ -__STATIC_INLINE uint32_t LL_I2C_GetTransferDirection(I2C_TypeDef *I2Cx) +__STATIC_INLINE uint32_t LL_I2C_GetTransferDirection(const I2C_TypeDef *I2Cx) { return (uint32_t)(READ_BIT(I2Cx->ISR, I2C_ISR_DIR)); } @@ -2161,7 +2168,7 @@ __STATIC_INLINE uint32_t LL_I2C_GetTransferDirection(I2C_TypeDef *I2Cx) * @param I2Cx I2C Instance. * @retval Value between Min_Data=0x00 and Max_Data=0x3F */ -__STATIC_INLINE uint32_t LL_I2C_GetAddressMatchCode(I2C_TypeDef *I2Cx) +__STATIC_INLINE uint32_t LL_I2C_GetAddressMatchCode(const I2C_TypeDef *I2Cx) { return (uint32_t)(READ_BIT(I2Cx->ISR, I2C_ISR_ADDCODE) >> I2C_ISR_ADDCODE_Pos << 1); } @@ -2191,7 +2198,7 @@ __STATIC_INLINE void LL_I2C_EnableSMBusPECCompare(I2C_TypeDef *I2Cx) * @param I2Cx I2C Instance. * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_I2C_IsEnabledSMBusPECCompare(I2C_TypeDef *I2Cx) +__STATIC_INLINE uint32_t LL_I2C_IsEnabledSMBusPECCompare(const I2C_TypeDef *I2Cx) { return ((READ_BIT(I2Cx->CR2, I2C_CR2_PECBYTE) == (I2C_CR2_PECBYTE)) ? 1UL : 0UL); } @@ -2204,7 +2211,7 @@ __STATIC_INLINE uint32_t LL_I2C_IsEnabledSMBusPECCompare(I2C_TypeDef *I2Cx) * @param I2Cx I2C Instance. * @retval Value between Min_Data=0x00 and Max_Data=0xFF */ -__STATIC_INLINE uint32_t LL_I2C_GetSMBusPEC(I2C_TypeDef *I2Cx) +__STATIC_INLINE uint32_t LL_I2C_GetSMBusPEC(const I2C_TypeDef *I2Cx) { return (uint32_t)(READ_BIT(I2Cx->PECR, I2C_PECR_PEC)); } @@ -2215,7 +2222,7 @@ __STATIC_INLINE uint32_t LL_I2C_GetSMBusPEC(I2C_TypeDef *I2Cx) * @param I2Cx I2C Instance. * @retval Value between Min_Data=0x00 and Max_Data=0xFF */ -__STATIC_INLINE uint8_t LL_I2C_ReceiveData8(I2C_TypeDef *I2Cx) +__STATIC_INLINE uint8_t LL_I2C_ReceiveData8(const I2C_TypeDef *I2Cx) { return (uint8_t)(READ_BIT(I2Cx->RXDR, I2C_RXDR_RXDATA)); } @@ -2241,8 +2248,8 @@ __STATIC_INLINE void LL_I2C_TransmitData8(I2C_TypeDef *I2Cx, uint8_t Data) * @{ */ -ErrorStatus LL_I2C_Init(I2C_TypeDef *I2Cx, LL_I2C_InitTypeDef *I2C_InitStruct); -ErrorStatus LL_I2C_DeInit(I2C_TypeDef *I2Cx); +ErrorStatus LL_I2C_Init(I2C_TypeDef *I2Cx, const LL_I2C_InitTypeDef *I2C_InitStruct); +ErrorStatus LL_I2C_DeInit(const I2C_TypeDef *I2Cx); void LL_I2C_StructInit(LL_I2C_InitTypeDef *I2C_InitStruct); diff --git a/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_ll_rcc.h b/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_ll_rcc.h index 4c1802570..bb68a51af 100644 --- a/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_ll_rcc.h +++ b/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_ll_rcc.h @@ -360,19 +360,19 @@ typedef struct /** @defgroup RCC_LL_EC_I2Cx_CLKSOURCE Peripheral I2C clock source selection * @{ */ -#define LL_RCC_I2C1_CLKSOURCE_PCLK1 ((RCC_OFFSET_CCIPR << 24U) | (RCC_CCIPR_I2C1SEL_Pos << 16U)) /*!< PCLK1 clock used as I2C1 clock source */ -#define LL_RCC_I2C1_CLKSOURCE_SYSCLK ((RCC_OFFSET_CCIPR << 24U) | (RCC_CCIPR_I2C1SEL_Pos << 16U) | (RCC_CCIPR_I2C1SEL_0 >> RCC_CCIPR_I2C1SEL_Pos)) /*!< SYSCLK clock used as I2C1 clock source */ -#define LL_RCC_I2C1_CLKSOURCE_HSI ((RCC_OFFSET_CCIPR << 24U) | (RCC_CCIPR_I2C1SEL_Pos << 16U) | (RCC_CCIPR_I2C1SEL_1 >> RCC_CCIPR_I2C1SEL_Pos)) /*!< HSI clock used as I2C1 clock source */ -#define LL_RCC_I2C2_CLKSOURCE_PCLK1 ((RCC_OFFSET_CCIPR << 24U) | (RCC_CCIPR_I2C2SEL_Pos << 16U)) /*!< PCLK1 clock used as I2C2 clock source */ -#define LL_RCC_I2C2_CLKSOURCE_SYSCLK ((RCC_OFFSET_CCIPR << 24U) | (RCC_CCIPR_I2C2SEL_Pos << 16U) | (RCC_CCIPR_I2C2SEL_0 >> RCC_CCIPR_I2C2SEL_Pos)) /*!< SYSCLK clock used as I2C2 clock source */ -#define LL_RCC_I2C2_CLKSOURCE_HSI ((RCC_OFFSET_CCIPR << 24U) | (RCC_CCIPR_I2C2SEL_Pos << 16U) | (RCC_CCIPR_I2C2SEL_1 >> RCC_CCIPR_I2C2SEL_Pos)) /*!< HSI clock used as I2C2 clock source */ -#define LL_RCC_I2C3_CLKSOURCE_PCLK1 ((RCC_OFFSET_CCIPR << 24U) | (RCC_CCIPR_I2C3SEL_Pos << 16U)) /*!< PCLK1 clock used as I2C3 clock source */ -#define LL_RCC_I2C3_CLKSOURCE_SYSCLK ((RCC_OFFSET_CCIPR << 24U) | (RCC_CCIPR_I2C3SEL_Pos << 16U) | (RCC_CCIPR_I2C3SEL_0 >> RCC_CCIPR_I2C3SEL_Pos)) /*!< SYSCLK clock used as I2C3 clock source */ -#define LL_RCC_I2C3_CLKSOURCE_HSI ((RCC_OFFSET_CCIPR << 24U) | (RCC_CCIPR_I2C3SEL_Pos << 16U) | (RCC_CCIPR_I2C3SEL_1 >> RCC_CCIPR_I2C3SEL_Pos)) /*!< HSI clock used as I2C3 clock source */ +#define LL_RCC_I2C1_CLKSOURCE_PCLK1 (((uint32_t)RCC_OFFSET_CCIPR << 24U) | ((uint32_t)RCC_CCIPR_I2C1SEL_Pos << 16U)) /*!< PCLK1 clock used as I2C1 clock source */ +#define LL_RCC_I2C1_CLKSOURCE_SYSCLK (((uint32_t)RCC_OFFSET_CCIPR << 24U) | ((uint32_t)RCC_CCIPR_I2C1SEL_Pos << 16U) | (RCC_CCIPR_I2C1SEL_0 >> RCC_CCIPR_I2C1SEL_Pos)) /*!< SYSCLK clock used as I2C1 clock source */ +#define LL_RCC_I2C1_CLKSOURCE_HSI (((uint32_t)RCC_OFFSET_CCIPR << 24U) | ((uint32_t)RCC_CCIPR_I2C1SEL_Pos << 16U) | (RCC_CCIPR_I2C1SEL_1 >> RCC_CCIPR_I2C1SEL_Pos)) /*!< HSI clock used as I2C1 clock source */ +#define LL_RCC_I2C2_CLKSOURCE_PCLK1 (((uint32_t)RCC_OFFSET_CCIPR << 24U) | ((uint32_t)RCC_CCIPR_I2C2SEL_Pos << 16U)) /*!< PCLK1 clock used as I2C2 clock source */ +#define LL_RCC_I2C2_CLKSOURCE_SYSCLK (((uint32_t)RCC_OFFSET_CCIPR << 24U) | ((uint32_t)RCC_CCIPR_I2C2SEL_Pos << 16U) | (RCC_CCIPR_I2C2SEL_0 >> RCC_CCIPR_I2C2SEL_Pos)) /*!< SYSCLK clock used as I2C2 clock source */ +#define LL_RCC_I2C2_CLKSOURCE_HSI (((uint32_t)RCC_OFFSET_CCIPR << 24U) | ((uint32_t)RCC_CCIPR_I2C2SEL_Pos << 16U) | (RCC_CCIPR_I2C2SEL_1 >> RCC_CCIPR_I2C2SEL_Pos)) /*!< HSI clock used as I2C2 clock source */ +#define LL_RCC_I2C3_CLKSOURCE_PCLK1 (((uint32_t)RCC_OFFSET_CCIPR << 24U) | ((uint32_t)RCC_CCIPR_I2C3SEL_Pos << 16U)) /*!< PCLK1 clock used as I2C3 clock source */ +#define LL_RCC_I2C3_CLKSOURCE_SYSCLK (((uint32_t)RCC_OFFSET_CCIPR << 24U) | ((uint32_t)RCC_CCIPR_I2C3SEL_Pos << 16U) | (RCC_CCIPR_I2C3SEL_0 >> RCC_CCIPR_I2C3SEL_Pos)) /*!< SYSCLK clock used as I2C3 clock source */ +#define LL_RCC_I2C3_CLKSOURCE_HSI (((uint32_t)RCC_OFFSET_CCIPR << 24U) | ((uint32_t)RCC_CCIPR_I2C3SEL_Pos << 16U) | (RCC_CCIPR_I2C3SEL_1 >> RCC_CCIPR_I2C3SEL_Pos)) /*!< HSI clock used as I2C3 clock source */ #if defined(RCC_CCIPR2_I2C4SEL) -#define LL_RCC_I2C4_CLKSOURCE_PCLK1 ((RCC_OFFSET_CCIPR2 << 24U) | (RCC_CCIPR2_I2C4SEL_Pos << 16U)) /*!< PCLK1 clock used as I2C4 clock source */ -#define LL_RCC_I2C4_CLKSOURCE_SYSCLK ((RCC_OFFSET_CCIPR2 << 24U) | (RCC_CCIPR2_I2C4SEL_Pos << 16U) | (RCC_CCIPR2_I2C4SEL_0 >> RCC_CCIPR2_I2C4SEL_Pos)) /*!< SYSCLK clock used as I2C4 clock source */ -#define LL_RCC_I2C4_CLKSOURCE_HSI ((RCC_OFFSET_CCIPR2 << 24U) | (RCC_CCIPR2_I2C4SEL_Pos << 16U) | (RCC_CCIPR2_I2C4SEL_1 >> RCC_CCIPR2_I2C4SEL_Pos)) /*!< HSI clock used as I2C4 clock source */ +#define LL_RCC_I2C4_CLKSOURCE_PCLK1 (((uint32_t)RCC_OFFSET_CCIPR2 << 24U) | ((uint32_t)RCC_CCIPR2_I2C4SEL_Pos << 16U)) /*!< PCLK1 clock used as I2C4 clock source */ +#define LL_RCC_I2C4_CLKSOURCE_SYSCLK (((uint32_t)RCC_OFFSET_CCIPR2 << 24U) | ((uint32_t)RCC_CCIPR2_I2C4SEL_Pos << 16U) | (RCC_CCIPR2_I2C4SEL_0 >> RCC_CCIPR2_I2C4SEL_Pos)) /*!< SYSCLK clock used as I2C4 clock source */ +#define LL_RCC_I2C4_CLKSOURCE_HSI (((uint32_t)RCC_OFFSET_CCIPR2 << 24U) | ((uint32_t)RCC_CCIPR2_I2C4SEL_Pos << 16U) | (RCC_CCIPR2_I2C4SEL_1 >> RCC_CCIPR2_I2C4SEL_Pos)) /*!< HSI clock used as I2C4 clock source */ #endif /* RCC_CCIPR2_I2C4SEL */ /** * @} @@ -444,13 +444,13 @@ typedef struct /** @defgroup RCC_LL_EC_ADC_CLKSOURCE Peripheral ADC clock source selection * @{ */ -#define LL_RCC_ADC12_CLKSOURCE_NONE ((RCC_OFFSET_CCIPR << 24U) | (RCC_CCIPR_ADC12SEL_Pos << 16U)) /*!< No clock used as ADC12 clock source */ -#define LL_RCC_ADC12_CLKSOURCE_PLL ((RCC_OFFSET_CCIPR << 24U) | (RCC_CCIPR_ADC12SEL_Pos << 16U) | (RCC_CCIPR_ADC12SEL_0 >> RCC_CCIPR_ADC12SEL_Pos)) /*!< PLL clock used as ADC12 clock source */ -#define LL_RCC_ADC12_CLKSOURCE_SYSCLK ((RCC_OFFSET_CCIPR << 24U) | (RCC_CCIPR_ADC12SEL_Pos << 16U) | (RCC_CCIPR_ADC12SEL_1 >> RCC_CCIPR_ADC12SEL_Pos)) /*!< SYSCLK clock used as ADC12 clock source */ +#define LL_RCC_ADC12_CLKSOURCE_NONE (((uint32_t)RCC_OFFSET_CCIPR << 24U) | ((uint32_t)RCC_CCIPR_ADC12SEL_Pos << 16U)) /*!< No clock used as ADC12 clock source */ +#define LL_RCC_ADC12_CLKSOURCE_PLL (((uint32_t)RCC_OFFSET_CCIPR << 24U) | ((uint32_t)RCC_CCIPR_ADC12SEL_Pos << 16U) | (RCC_CCIPR_ADC12SEL_0 >> RCC_CCIPR_ADC12SEL_Pos)) /*!< PLL clock used as ADC12 clock source */ +#define LL_RCC_ADC12_CLKSOURCE_SYSCLK (((uint32_t)RCC_OFFSET_CCIPR << 24U) | ((uint32_t)RCC_CCIPR_ADC12SEL_Pos << 16U) | (RCC_CCIPR_ADC12SEL_1 >> RCC_CCIPR_ADC12SEL_Pos)) /*!< SYSCLK clock used as ADC12 clock source */ #if defined(RCC_CCIPR_ADC345SEL) -#define LL_RCC_ADC345_CLKSOURCE_NONE ((RCC_OFFSET_CCIPR << 24U) | (RCC_CCIPR_ADC345SEL_Pos << 16U)) /*!< No clock used as ADC345 clock source */ -#define LL_RCC_ADC345_CLKSOURCE_PLL ((RCC_OFFSET_CCIPR << 24U) | (RCC_CCIPR_ADC345SEL_Pos << 16U) | (RCC_CCIPR_ADC345SEL_0 >> RCC_CCIPR_ADC345SEL_Pos)) /*!< PLL clock used as ADC345 clock source */ -#define LL_RCC_ADC345_CLKSOURCE_SYSCLK ((RCC_OFFSET_CCIPR << 24U) | (RCC_CCIPR_ADC345SEL_Pos << 16U) | (RCC_CCIPR_ADC345SEL_1 >> RCC_CCIPR_ADC345SEL_Pos)) /*!< SYSCLK clock used as ADC345 clock source */ +#define LL_RCC_ADC345_CLKSOURCE_NONE (((uint32_t)RCC_OFFSET_CCIPR << 24U) | ((uint32_t)RCC_CCIPR_ADC345SEL_Pos << 16U)) /*!< No clock used as ADC345 clock source */ +#define LL_RCC_ADC345_CLKSOURCE_PLL (((uint32_t)RCC_OFFSET_CCIPR << 24U) | ((uint32_t)RCC_CCIPR_ADC345SEL_Pos << 16U) | (RCC_CCIPR_ADC345SEL_0 >> RCC_CCIPR_ADC345SEL_Pos)) /*!< PLL clock used as ADC345 clock source */ +#define LL_RCC_ADC345_CLKSOURCE_SYSCLK (((uint32_t)RCC_OFFSET_CCIPR << 24U) | ((uint32_t)RCC_CCIPR_ADC345SEL_Pos << 16U) | (RCC_CCIPR_ADC345SEL_1 >> RCC_CCIPR_ADC345SEL_Pos)) /*!< SYSCLK clock used as ADC345 clock source */ #endif /* RCC_CCIPR_ADC345SEL */ /** * @} @@ -501,11 +501,11 @@ typedef struct /** @defgroup RCC_LL_EC_I2C1 Peripheral I2C get clock source * @{ */ -#define LL_RCC_I2C1_CLKSOURCE ((RCC_OFFSET_CCIPR << 24U) | (RCC_CCIPR_I2C1SEL_Pos << 16U) | (RCC_CCIPR_I2C1SEL >> RCC_CCIPR_I2C1SEL_Pos)) /*!< I2C1 Clock source selection */ -#define LL_RCC_I2C2_CLKSOURCE ((RCC_OFFSET_CCIPR << 24U) | (RCC_CCIPR_I2C2SEL_Pos << 16U) | (RCC_CCIPR_I2C2SEL >> RCC_CCIPR_I2C2SEL_Pos)) /*!< I2C2 Clock source selection */ -#define LL_RCC_I2C3_CLKSOURCE ((RCC_OFFSET_CCIPR << 24U) | (RCC_CCIPR_I2C3SEL_Pos << 16U) | (RCC_CCIPR_I2C3SEL >> RCC_CCIPR_I2C3SEL_Pos)) /*!< I2C3 Clock source selection */ +#define LL_RCC_I2C1_CLKSOURCE (((uint32_t)RCC_OFFSET_CCIPR << 24U) | ((uint32_t)RCC_CCIPR_I2C1SEL_Pos << 16U) | (RCC_CCIPR_I2C1SEL >> RCC_CCIPR_I2C1SEL_Pos)) /*!< I2C1 Clock source selection */ +#define LL_RCC_I2C2_CLKSOURCE (((uint32_t)RCC_OFFSET_CCIPR << 24U) | ((uint32_t)RCC_CCIPR_I2C2SEL_Pos << 16U) | (RCC_CCIPR_I2C2SEL >> RCC_CCIPR_I2C2SEL_Pos)) /*!< I2C2 Clock source selection */ +#define LL_RCC_I2C3_CLKSOURCE (((uint32_t)RCC_OFFSET_CCIPR << 24U) | ((uint32_t)RCC_CCIPR_I2C3SEL_Pos << 16U) | (RCC_CCIPR_I2C3SEL >> RCC_CCIPR_I2C3SEL_Pos)) /*!< I2C3 Clock source selection */ #if defined(RCC_CCIPR2_I2C4SEL) -#define LL_RCC_I2C4_CLKSOURCE ((RCC_OFFSET_CCIPR2 << 24U) | (RCC_CCIPR2_I2C4SEL_Pos << 16U) | (RCC_CCIPR2_I2C4SEL >> RCC_CCIPR2_I2C4SEL_Pos)) /*!< I2C4 Clock source selection */ +#define LL_RCC_I2C4_CLKSOURCE (((uint32_t)RCC_OFFSET_CCIPR2 << 24U) | ((uint32_t)RCC_CCIPR2_I2C4SEL_Pos << 16U) | (RCC_CCIPR2_I2C4SEL >> RCC_CCIPR2_I2C4SEL_Pos)) /*!< I2C4 Clock source selection */ #endif /* RCC_CCIPR2_I2C4SEL */ /** * @} @@ -565,9 +565,9 @@ typedef struct /** @defgroup RCC_LL_EC_ADC Peripheral ADC get clock source * @{ */ -#define LL_RCC_ADC12_CLKSOURCE ((RCC_OFFSET_CCIPR << 24U) | (RCC_CCIPR_ADC12SEL_Pos << 16U) | (RCC_CCIPR_ADC12SEL >> RCC_CCIPR_ADC12SEL_Pos)) /*!< ADC12 Clock source selection */ +#define LL_RCC_ADC12_CLKSOURCE (((uint32_t)RCC_OFFSET_CCIPR << 24U) | ((uint32_t)RCC_CCIPR_ADC12SEL_Pos << 16U) | (RCC_CCIPR_ADC12SEL >> RCC_CCIPR_ADC12SEL_Pos)) /*!< ADC12 Clock source selection */ #if defined(RCC_CCIPR_ADC345SEL_Pos) -#define LL_RCC_ADC345_CLKSOURCE ((RCC_OFFSET_CCIPR << 24U) | (RCC_CCIPR_ADC345SEL_Pos << 16U) | (RCC_CCIPR_ADC345SEL >> RCC_CCIPR_ADC345SEL_Pos)) /*!< ADC345 Clock source selection */ +#define LL_RCC_ADC345_CLKSOURCE (((uint32_t)RCC_OFFSET_CCIPR << 24U) | ((uint32_t)RCC_CCIPR_ADC345SEL_Pos << 16U) | (RCC_CCIPR_ADC345SEL >> RCC_CCIPR_ADC345SEL_Pos)) /*!< ADC345 Clock source selection */ #endif /* RCC_CCIPR_ADC345SEL_Pos */ /** * @} diff --git a/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_ll_tim.h b/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_ll_tim.h index d42f1587d..f14c000d2 100644 --- a/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_ll_tim.h +++ b/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_ll_tim.h @@ -671,10 +671,10 @@ typedef struct /** @defgroup TIM_LL_EC_COUNTERMODE Counter Mode * @{ */ -#define LL_TIM_COUNTERMODE_UP 0x00000000U /*!= (__CNTCLK__)) ? (uint32_t)(((__TIMCLK__)/(__CNTCLK__)) - 1U) : 0U) + (((__TIMCLK__) >= (__CNTCLK__)) ? (uint32_t)((((__TIMCLK__) + (__CNTCLK__)/2U)/(__CNTCLK__)) - 1U) : 0U) /** * @brief HELPER macro calculating the auto-reload value to achieve the required output signal frequency. @@ -1987,11 +2002,6 @@ typedef struct ((uint32_t)(0x01U << (((__ICPSC__) >> 16U) >> TIM_CCMR1_IC1PSC_Pos))) -/** - * @} - */ - - /** * @} */ @@ -2032,7 +2042,7 @@ __STATIC_INLINE void LL_TIM_DisableCounter(TIM_TypeDef *TIMx) * @param TIMx Timer instance * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_TIM_IsEnabledCounter(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_IsEnabledCounter(const TIM_TypeDef *TIMx) { return ((READ_BIT(TIMx->CR1, TIM_CR1_CEN) == (TIM_CR1_CEN)) ? 1UL : 0UL); } @@ -2065,7 +2075,7 @@ __STATIC_INLINE void LL_TIM_DisableUpdateEvent(TIM_TypeDef *TIMx) * @param TIMx Timer instance * @retval Inverted state of bit (0 or 1). */ -__STATIC_INLINE uint32_t LL_TIM_IsEnabledUpdateEvent(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_IsEnabledUpdateEvent(const TIM_TypeDef *TIMx) { return ((READ_BIT(TIMx->CR1, TIM_CR1_UDIS) == (uint32_t)RESET) ? 1UL : 0UL); } @@ -2099,7 +2109,7 @@ __STATIC_INLINE void LL_TIM_SetUpdateSource(TIM_TypeDef *TIMx, uint32_t UpdateSo * @arg @ref LL_TIM_UPDATESOURCE_REGULAR * @arg @ref LL_TIM_UPDATESOURCE_COUNTER */ -__STATIC_INLINE uint32_t LL_TIM_GetUpdateSource(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_GetUpdateSource(const TIM_TypeDef *TIMx) { return (uint32_t)(READ_BIT(TIMx->CR1, TIM_CR1_URS)); } @@ -2126,7 +2136,7 @@ __STATIC_INLINE void LL_TIM_SetOnePulseMode(TIM_TypeDef *TIMx, uint32_t OnePulse * @arg @ref LL_TIM_ONEPULSEMODE_SINGLE * @arg @ref LL_TIM_ONEPULSEMODE_REPETITIVE */ -__STATIC_INLINE uint32_t LL_TIM_GetOnePulseMode(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_GetOnePulseMode(const TIM_TypeDef *TIMx) { return (uint32_t)(READ_BIT(TIMx->CR1, TIM_CR1_OPM)); } @@ -2170,7 +2180,7 @@ __STATIC_INLINE void LL_TIM_SetCounterMode(TIM_TypeDef *TIMx, uint32_t CounterMo * @arg @ref LL_TIM_COUNTERMODE_CENTER_DOWN * @arg @ref LL_TIM_COUNTERMODE_CENTER_UP_DOWN */ -__STATIC_INLINE uint32_t LL_TIM_GetCounterMode(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_GetCounterMode(const TIM_TypeDef *TIMx) { uint32_t counter_mode; @@ -2212,7 +2222,7 @@ __STATIC_INLINE void LL_TIM_DisableARRPreload(TIM_TypeDef *TIMx) * @param TIMx Timer instance * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_TIM_IsEnabledARRPreload(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_IsEnabledARRPreload(const TIM_TypeDef *TIMx) { return ((READ_BIT(TIMx->CR1, TIM_CR1_ARPE) == (TIM_CR1_ARPE)) ? 1UL : 0UL); } @@ -2249,7 +2259,7 @@ __STATIC_INLINE void LL_TIM_SetClockDivision(TIM_TypeDef *TIMx, uint32_t ClockDi * @arg @ref LL_TIM_CLOCKDIVISION_DIV2 * @arg @ref LL_TIM_CLOCKDIVISION_DIV4 */ -__STATIC_INLINE uint32_t LL_TIM_GetClockDivision(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_GetClockDivision(const TIM_TypeDef *TIMx) { return (uint32_t)(READ_BIT(TIMx->CR1, TIM_CR1_CKD)); } @@ -2278,7 +2288,7 @@ __STATIC_INLINE void LL_TIM_SetCounter(TIM_TypeDef *TIMx, uint32_t Counter) * @param TIMx Timer instance * @retval Counter value (between Min_Data=0 and Max_Data=0xFFFF or 0xFFFFFFFF) */ -__STATIC_INLINE uint32_t LL_TIM_GetCounter(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_GetCounter(const TIM_TypeDef *TIMx) { return (uint32_t)(READ_REG(TIMx->CNT)); } @@ -2291,7 +2301,7 @@ __STATIC_INLINE uint32_t LL_TIM_GetCounter(TIM_TypeDef *TIMx) * @arg @ref LL_TIM_COUNTERDIRECTION_UP * @arg @ref LL_TIM_COUNTERDIRECTION_DOWN */ -__STATIC_INLINE uint32_t LL_TIM_GetDirection(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_GetDirection(const TIM_TypeDef *TIMx) { return (uint32_t)(READ_BIT(TIMx->CR1, TIM_CR1_DIR)); } @@ -2318,7 +2328,7 @@ __STATIC_INLINE void LL_TIM_SetPrescaler(TIM_TypeDef *TIMx, uint32_t Prescaler) * @param TIMx Timer instance * @retval Prescaler value between Min_Data=0 and Max_Data=65535 */ -__STATIC_INLINE uint32_t LL_TIM_GetPrescaler(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_GetPrescaler(const TIM_TypeDef *TIMx) { return (uint32_t)(READ_REG(TIMx->PSC)); } @@ -2350,7 +2360,7 @@ __STATIC_INLINE void LL_TIM_SetAutoReload(TIM_TypeDef *TIMx, uint32_t AutoReload * @param TIMx Timer instance * @retval Auto-reload value */ -__STATIC_INLINE uint32_t LL_TIM_GetAutoReload(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_GetAutoReload(const TIM_TypeDef *TIMx) { return (uint32_t)(READ_REG(TIMx->ARR)); } @@ -2378,7 +2388,7 @@ __STATIC_INLINE void LL_TIM_SetRepetitionCounter(TIM_TypeDef *TIMx, uint32_t Rep * @param TIMx Timer instance * @retval Repetition counter value */ -__STATIC_INLINE uint32_t LL_TIM_GetRepetitionCounter(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_GetRepetitionCounter(const TIM_TypeDef *TIMx) { return (uint32_t)(READ_REG(TIMx->RCR)); } @@ -2412,7 +2422,7 @@ __STATIC_INLINE void LL_TIM_DisableUIFRemap(TIM_TypeDef *TIMx) * @param Counter Counter value * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_TIM_IsActiveUIFCPY(uint32_t Counter) +__STATIC_INLINE uint32_t LL_TIM_IsActiveUIFCPY(const uint32_t Counter) { return (((Counter & TIM_CNT_UIFCPY) == (TIM_CNT_UIFCPY)) ? 1UL : 0UL); } @@ -2451,7 +2461,7 @@ __STATIC_INLINE void LL_TIM_DisableDithering(TIM_TypeDef *TIMx) * @param TIMx Timer instance * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_TIM_IsEnabledDithering(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_IsEnabledDithering(const TIM_TypeDef *TIMx) { return ((READ_BIT(TIMx->CR1, TIM_CR1_DITHEN) == (TIM_CR1_DITHEN)) ? 1UL : 0UL); } @@ -2492,6 +2502,17 @@ __STATIC_INLINE void LL_TIM_CC_DisablePreload(TIM_TypeDef *TIMx) CLEAR_BIT(TIMx->CR2, TIM_CR2_CCPC); } +/** + * @brief Indicates whether the capture/compare control bits (CCxE, CCxNE and OCxM) preload is enabled. + * @rmtoll CR2 CCPC LL_TIM_CC_IsEnabledPreload + * @param TIMx Timer instance + * @retval State of bit (1 or 0). + */ +__STATIC_INLINE uint32_t LL_TIM_CC_IsEnabledPreload(const TIM_TypeDef *TIMx) +{ + return ((READ_BIT(TIMx->CR2, TIM_CR2_CCPC) == (TIM_CR2_CCPC)) ? 1UL : 0UL); +} + /** * @brief Set the updated source of the capture/compare control bits (CCxE, CCxNE and OCxM). * @note Macro IS_TIM_COMMUTATION_EVENT_INSTANCE(TIMx) can be used to check @@ -2530,7 +2551,7 @@ __STATIC_INLINE void LL_TIM_CC_SetDMAReqTrigger(TIM_TypeDef *TIMx, uint32_t DMAR * @arg @ref LL_TIM_CCDMAREQUEST_CC * @arg @ref LL_TIM_CCDMAREQUEST_UPDATE */ -__STATIC_INLINE uint32_t LL_TIM_CC_GetDMAReqTrigger(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_CC_GetDMAReqTrigger(const TIM_TypeDef *TIMx) { return (uint32_t)(READ_BIT(TIMx->CR2, TIM_CR2_CCDS)); } @@ -2642,7 +2663,7 @@ __STATIC_INLINE void LL_TIM_CC_DisableChannel(TIM_TypeDef *TIMx, uint32_t Channe * @arg @ref LL_TIM_CHANNEL_CH6 * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_TIM_CC_IsEnabledChannel(TIM_TypeDef *TIMx, uint32_t Channels) +__STATIC_INLINE uint32_t LL_TIM_CC_IsEnabledChannel(const TIM_TypeDef *TIMx, uint32_t Channels) { return ((READ_BIT(TIMx->CCER, Channels) == (Channels)) ? 1UL : 0UL); } @@ -2728,8 +2749,8 @@ __STATIC_INLINE void LL_TIM_OC_ConfigOutput(TIM_TypeDef *TIMx, uint32_t Channel, * @arg @ref LL_TIM_OCMODE_RETRIG_OPM2 * @arg @ref LL_TIM_OCMODE_COMBINED_PWM1 * @arg @ref LL_TIM_OCMODE_COMBINED_PWM2 - * @arg @ref LL_TIM_OCMODE_ASSYMETRIC_PWM1 - * @arg @ref LL_TIM_OCMODE_ASSYMETRIC_PWM2 + * @arg @ref LL_TIM_OCMODE_ASYMMETRIC_PWM1 + * @arg @ref LL_TIM_OCMODE_ASYMMETRIC_PWM2 * @arg @ref LL_TIM_OCMODE_PULSE_ON_COMPARE (for channel 3 or channel 4 only) * @arg @ref LL_TIM_OCMODE_DIRECTION_OUTPUT (for channel 3 or channel 4 only) * @retval None @@ -2770,12 +2791,12 @@ __STATIC_INLINE void LL_TIM_OC_SetMode(TIM_TypeDef *TIMx, uint32_t Channel, uint * @arg @ref LL_TIM_OCMODE_RETRIG_OPM2 * @arg @ref LL_TIM_OCMODE_COMBINED_PWM1 * @arg @ref LL_TIM_OCMODE_COMBINED_PWM2 - * @arg @ref LL_TIM_OCMODE_ASSYMETRIC_PWM1 - * @arg @ref LL_TIM_OCMODE_ASSYMETRIC_PWM2 + * @arg @ref LL_TIM_OCMODE_ASYMMETRIC_PWM1 + * @arg @ref LL_TIM_OCMODE_ASYMMETRIC_PWM2 * @arg @ref LL_TIM_OCMODE_PULSE_ON_COMPARE (for channel 3 or channel 4 only) * @arg @ref LL_TIM_OCMODE_DIRECTION_OUTPUT (for channel 3 or channel 4 only) */ -__STATIC_INLINE uint32_t LL_TIM_OC_GetMode(TIM_TypeDef *TIMx, uint32_t Channel) +__STATIC_INLINE uint32_t LL_TIM_OC_GetMode(const TIM_TypeDef *TIMx, uint32_t Channel) { uint8_t iChannel = TIM_GET_CHANNEL_INDEX(Channel); const __IO uint32_t *pReg = (__IO uint32_t *)((uint32_t)((uint32_t)(&TIMx->CCMR1) + OFFSET_TAB_CCMRx[iChannel])); @@ -2845,7 +2866,7 @@ __STATIC_INLINE void LL_TIM_OC_SetPolarity(TIM_TypeDef *TIMx, uint32_t Channel, * @arg @ref LL_TIM_OCPOLARITY_HIGH * @arg @ref LL_TIM_OCPOLARITY_LOW */ -__STATIC_INLINE uint32_t LL_TIM_OC_GetPolarity(TIM_TypeDef *TIMx, uint32_t Channel) +__STATIC_INLINE uint32_t LL_TIM_OC_GetPolarity(const TIM_TypeDef *TIMx, uint32_t Channel) { uint8_t iChannel = TIM_GET_CHANNEL_INDEX(Channel); return (READ_BIT(TIMx->CCER, (TIM_CCER_CC1P << SHIFT_TAB_CCxP[iChannel])) >> SHIFT_TAB_CCxP[iChannel]); @@ -2918,7 +2939,7 @@ __STATIC_INLINE void LL_TIM_OC_SetIdleState(TIM_TypeDef *TIMx, uint32_t Channel, * @arg @ref LL_TIM_OCIDLESTATE_LOW * @arg @ref LL_TIM_OCIDLESTATE_HIGH */ -__STATIC_INLINE uint32_t LL_TIM_OC_GetIdleState(TIM_TypeDef *TIMx, uint32_t Channel) +__STATIC_INLINE uint32_t LL_TIM_OC_GetIdleState(const TIM_TypeDef *TIMx, uint32_t Channel) { uint8_t iChannel = TIM_GET_CHANNEL_INDEX(Channel); return (READ_BIT(TIMx->CR2, (TIM_CR2_OIS1 << SHIFT_TAB_OISx[iChannel])) >> SHIFT_TAB_OISx[iChannel]); @@ -2995,7 +3016,7 @@ __STATIC_INLINE void LL_TIM_OC_DisableFast(TIM_TypeDef *TIMx, uint32_t Channel) * @arg @ref LL_TIM_CHANNEL_CH6 * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_TIM_OC_IsEnabledFast(TIM_TypeDef *TIMx, uint32_t Channel) +__STATIC_INLINE uint32_t LL_TIM_OC_IsEnabledFast(const TIM_TypeDef *TIMx, uint32_t Channel) { uint8_t iChannel = TIM_GET_CHANNEL_INDEX(Channel); const __IO uint32_t *pReg = (__IO uint32_t *)((uint32_t)((uint32_t)(&TIMx->CCMR1) + OFFSET_TAB_CCMRx[iChannel])); @@ -3071,7 +3092,7 @@ __STATIC_INLINE void LL_TIM_OC_DisablePreload(TIM_TypeDef *TIMx, uint32_t Channe * @arg @ref LL_TIM_CHANNEL_CH6 * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_TIM_OC_IsEnabledPreload(TIM_TypeDef *TIMx, uint32_t Channel) +__STATIC_INLINE uint32_t LL_TIM_OC_IsEnabledPreload(const TIM_TypeDef *TIMx, uint32_t Channel) { uint8_t iChannel = TIM_GET_CHANNEL_INDEX(Channel); const __IO uint32_t *pReg = (__IO uint32_t *)((uint32_t)((uint32_t)(&TIMx->CCMR1) + OFFSET_TAB_CCMRx[iChannel])); @@ -3156,7 +3177,7 @@ __STATIC_INLINE void LL_TIM_OC_DisableClear(TIM_TypeDef *TIMx, uint32_t Channel) * @arg @ref LL_TIM_CHANNEL_CH6 * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_TIM_OC_IsEnabledClear(TIM_TypeDef *TIMx, uint32_t Channel) +__STATIC_INLINE uint32_t LL_TIM_OC_IsEnabledClear(const TIM_TypeDef *TIMx, uint32_t Channel) { uint8_t iChannel = TIM_GET_CHANNEL_INDEX(Channel); const __IO uint32_t *pReg = (__IO uint32_t *)((uint32_t)((uint32_t)(&TIMx->CCMR1) + OFFSET_TAB_CCMRx[iChannel])); @@ -3294,7 +3315,7 @@ __STATIC_INLINE void LL_TIM_OC_SetCompareCH6(TIM_TypeDef *TIMx, uint32_t Compare * @param TIMx Timer instance * @retval CompareValue (between Min_Data=0 and Max_Data=65535) */ -__STATIC_INLINE uint32_t LL_TIM_OC_GetCompareCH1(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_OC_GetCompareCH1(const TIM_TypeDef *TIMx) { return (uint32_t)(READ_REG(TIMx->CCR1)); } @@ -3311,7 +3332,7 @@ __STATIC_INLINE uint32_t LL_TIM_OC_GetCompareCH1(TIM_TypeDef *TIMx) * @param TIMx Timer instance * @retval CompareValue (between Min_Data=0 and Max_Data=65535) */ -__STATIC_INLINE uint32_t LL_TIM_OC_GetCompareCH2(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_OC_GetCompareCH2(const TIM_TypeDef *TIMx) { return (uint32_t)(READ_REG(TIMx->CCR2)); } @@ -3328,7 +3349,7 @@ __STATIC_INLINE uint32_t LL_TIM_OC_GetCompareCH2(TIM_TypeDef *TIMx) * @param TIMx Timer instance * @retval CompareValue (between Min_Data=0 and Max_Data=65535) */ -__STATIC_INLINE uint32_t LL_TIM_OC_GetCompareCH3(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_OC_GetCompareCH3(const TIM_TypeDef *TIMx) { return (uint32_t)(READ_REG(TIMx->CCR3)); } @@ -3345,7 +3366,7 @@ __STATIC_INLINE uint32_t LL_TIM_OC_GetCompareCH3(TIM_TypeDef *TIMx) * @param TIMx Timer instance * @retval CompareValue (between Min_Data=0 and Max_Data=65535) */ -__STATIC_INLINE uint32_t LL_TIM_OC_GetCompareCH4(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_OC_GetCompareCH4(const TIM_TypeDef *TIMx) { return (uint32_t)(READ_REG(TIMx->CCR4)); } @@ -3359,7 +3380,7 @@ __STATIC_INLINE uint32_t LL_TIM_OC_GetCompareCH4(TIM_TypeDef *TIMx) * @param TIMx Timer instance * @retval CompareValue (between Min_Data=0 and Max_Data=65535) */ -__STATIC_INLINE uint32_t LL_TIM_OC_GetCompareCH5(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_OC_GetCompareCH5(const TIM_TypeDef *TIMx) { return (uint32_t)(READ_BIT(TIMx->CCR5, TIM_CCR5_CCR5)); } @@ -3373,7 +3394,7 @@ __STATIC_INLINE uint32_t LL_TIM_OC_GetCompareCH5(TIM_TypeDef *TIMx) * @param TIMx Timer instance * @retval CompareValue (between Min_Data=0 and Max_Data=65535) */ -__STATIC_INLINE uint32_t LL_TIM_OC_GetCompareCH6(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_OC_GetCompareCH6(const TIM_TypeDef *TIMx) { return (uint32_t)(READ_REG(TIMx->CCR6)); } @@ -3438,7 +3459,7 @@ __STATIC_INLINE void LL_TIM_OC_SetPulseWidthPrescaler(TIM_TypeDef *TIMx, uint32_ * @arg @ref LL_TIM_PWPRSC_X64 * @arg @ref LL_TIM_PWPRSC_X128 */ -__STATIC_INLINE uint32_t LL_TIM_OC_GetPulseWidthPrescaler(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_OC_GetPulseWidthPrescaler(const TIM_TypeDef *TIMx) { return (uint32_t)(READ_BIT(TIMx->ECR, TIM_ECR_PWPRSC)); } @@ -3467,7 +3488,7 @@ __STATIC_INLINE void LL_TIM_OC_SetPulseWidth(TIM_TypeDef *TIMx, uint32_t PulseWi * @param TIMx Timer instance * @retval Returned value can be between Min_Data=0 and Max_Data=255: */ -__STATIC_INLINE uint32_t LL_TIM_OC_GetPulseWidth(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_OC_GetPulseWidth(const TIM_TypeDef *TIMx) { return (uint32_t)(READ_BIT(TIMx->ECR, TIM_ECR_PW)); } @@ -3567,7 +3588,7 @@ __STATIC_INLINE void LL_TIM_IC_SetActiveInput(TIM_TypeDef *TIMx, uint32_t Channe * @arg @ref LL_TIM_ACTIVEINPUT_INDIRECTTI * @arg @ref LL_TIM_ACTIVEINPUT_TRC */ -__STATIC_INLINE uint32_t LL_TIM_IC_GetActiveInput(TIM_TypeDef *TIMx, uint32_t Channel) +__STATIC_INLINE uint32_t LL_TIM_IC_GetActiveInput(const TIM_TypeDef *TIMx, uint32_t Channel) { uint8_t iChannel = TIM_GET_CHANNEL_INDEX(Channel); const __IO uint32_t *pReg = (__IO uint32_t *)((uint32_t)((uint32_t)(&TIMx->CCMR1) + OFFSET_TAB_CCMRx[iChannel])); @@ -3618,7 +3639,7 @@ __STATIC_INLINE void LL_TIM_IC_SetPrescaler(TIM_TypeDef *TIMx, uint32_t Channel, * @arg @ref LL_TIM_ICPSC_DIV4 * @arg @ref LL_TIM_ICPSC_DIV8 */ -__STATIC_INLINE uint32_t LL_TIM_IC_GetPrescaler(TIM_TypeDef *TIMx, uint32_t Channel) +__STATIC_INLINE uint32_t LL_TIM_IC_GetPrescaler(const TIM_TypeDef *TIMx, uint32_t Channel) { uint8_t iChannel = TIM_GET_CHANNEL_INDEX(Channel); const __IO uint32_t *pReg = (__IO uint32_t *)((uint32_t)((uint32_t)(&TIMx->CCMR1) + OFFSET_TAB_CCMRx[iChannel])); @@ -3693,7 +3714,7 @@ __STATIC_INLINE void LL_TIM_IC_SetFilter(TIM_TypeDef *TIMx, uint32_t Channel, ui * @arg @ref LL_TIM_IC_FILTER_FDIV32_N6 * @arg @ref LL_TIM_IC_FILTER_FDIV32_N8 */ -__STATIC_INLINE uint32_t LL_TIM_IC_GetFilter(TIM_TypeDef *TIMx, uint32_t Channel) +__STATIC_INLINE uint32_t LL_TIM_IC_GetFilter(const TIM_TypeDef *TIMx, uint32_t Channel) { uint8_t iChannel = TIM_GET_CHANNEL_INDEX(Channel); const __IO uint32_t *pReg = (__IO uint32_t *)((uint32_t)((uint32_t)(&TIMx->CCMR1) + OFFSET_TAB_CCMRx[iChannel])); @@ -3750,7 +3771,7 @@ __STATIC_INLINE void LL_TIM_IC_SetPolarity(TIM_TypeDef *TIMx, uint32_t Channel, * @arg @ref LL_TIM_IC_POLARITY_FALLING * @arg @ref LL_TIM_IC_POLARITY_BOTHEDGE */ -__STATIC_INLINE uint32_t LL_TIM_IC_GetPolarity(TIM_TypeDef *TIMx, uint32_t Channel) +__STATIC_INLINE uint32_t LL_TIM_IC_GetPolarity(const TIM_TypeDef *TIMx, uint32_t Channel) { uint8_t iChannel = TIM_GET_CHANNEL_INDEX(Channel); return (READ_BIT(TIMx->CCER, ((TIM_CCER_CC1NP | TIM_CCER_CC1P) << SHIFT_TAB_CCxP[iChannel])) >> @@ -3791,7 +3812,7 @@ __STATIC_INLINE void LL_TIM_IC_DisableXORCombination(TIM_TypeDef *TIMx) * @param TIMx Timer instance * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_TIM_IC_IsEnabledXORCombination(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_IC_IsEnabledXORCombination(const TIM_TypeDef *TIMx) { return ((READ_BIT(TIMx->CR2, TIM_CR2_TI1S) == (TIM_CR2_TI1S)) ? 1UL : 0UL); } @@ -3808,7 +3829,7 @@ __STATIC_INLINE uint32_t LL_TIM_IC_IsEnabledXORCombination(TIM_TypeDef *TIMx) * @param TIMx Timer instance * @retval CapturedValue (between Min_Data=0 and Max_Data=65535) */ -__STATIC_INLINE uint32_t LL_TIM_IC_GetCaptureCH1(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_IC_GetCaptureCH1(const TIM_TypeDef *TIMx) { return (uint32_t)(READ_REG(TIMx->CCR1)); } @@ -3825,7 +3846,7 @@ __STATIC_INLINE uint32_t LL_TIM_IC_GetCaptureCH1(TIM_TypeDef *TIMx) * @param TIMx Timer instance * @retval CapturedValue (between Min_Data=0 and Max_Data=65535) */ -__STATIC_INLINE uint32_t LL_TIM_IC_GetCaptureCH2(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_IC_GetCaptureCH2(const TIM_TypeDef *TIMx) { return (uint32_t)(READ_REG(TIMx->CCR2)); } @@ -3842,7 +3863,7 @@ __STATIC_INLINE uint32_t LL_TIM_IC_GetCaptureCH2(TIM_TypeDef *TIMx) * @param TIMx Timer instance * @retval CapturedValue (between Min_Data=0 and Max_Data=65535) */ -__STATIC_INLINE uint32_t LL_TIM_IC_GetCaptureCH3(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_IC_GetCaptureCH3(const TIM_TypeDef *TIMx) { return (uint32_t)(READ_REG(TIMx->CCR3)); } @@ -3859,7 +3880,7 @@ __STATIC_INLINE uint32_t LL_TIM_IC_GetCaptureCH3(TIM_TypeDef *TIMx) * @param TIMx Timer instance * @retval CapturedValue (between Min_Data=0 and Max_Data=65535) */ -__STATIC_INLINE uint32_t LL_TIM_IC_GetCaptureCH4(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_IC_GetCaptureCH4(const TIM_TypeDef *TIMx) { return (uint32_t)(READ_REG(TIMx->CCR4)); } @@ -3906,7 +3927,7 @@ __STATIC_INLINE void LL_TIM_DisableExternalClock(TIM_TypeDef *TIMx) * @param TIMx Timer instance * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_TIM_IsEnabledExternalClock(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_IsEnabledExternalClock(const TIM_TypeDef *TIMx) { return ((READ_BIT(TIMx->SMCR, TIM_SMCR_ECE) == (TIM_SMCR_ECE)) ? 1UL : 0UL); } @@ -4049,10 +4070,6 @@ __STATIC_INLINE void LL_TIM_SetSlaveMode(TIM_TypeDef *TIMx, uint32_t SlaveMode) * @arg @ref LL_TIM_TS_ITR1 * @arg @ref LL_TIM_TS_ITR2 * @arg @ref LL_TIM_TS_ITR3 - * @arg @ref LL_TIM_TS_TI1F_ED - * @arg @ref LL_TIM_TS_TI1FP1 - * @arg @ref LL_TIM_TS_TI2FP2 - * @arg @ref LL_TIM_TS_ETRF * @arg @ref LL_TIM_TS_ITR4 * @arg @ref LL_TIM_TS_ITR5 * @arg @ref LL_TIM_TS_ITR6 @@ -4061,6 +4078,10 @@ __STATIC_INLINE void LL_TIM_SetSlaveMode(TIM_TypeDef *TIMx, uint32_t SlaveMode) * @arg @ref LL_TIM_TS_ITR9 * @arg @ref LL_TIM_TS_ITR10 * @arg @ref LL_TIM_TS_ITR11 + * @arg @ref LL_TIM_TS_TI1F_ED + * @arg @ref LL_TIM_TS_TI1FP1 + * @arg @ref LL_TIM_TS_TI2FP2 + * @arg @ref LL_TIM_TS_ETRF * @retval None */ __STATIC_INLINE void LL_TIM_SetTriggerInput(TIM_TypeDef *TIMx, uint32_t TriggerInput) @@ -4102,7 +4123,7 @@ __STATIC_INLINE void LL_TIM_DisableMasterSlaveMode(TIM_TypeDef *TIMx) * @param TIMx Timer instance * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_TIM_IsEnabledMasterSlaveMode(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_IsEnabledMasterSlaveMode(const TIM_TypeDef *TIMx) { return ((READ_BIT(TIMx->SMCR, TIM_SMCR_MSM) == (TIM_SMCR_MSM)) ? 1UL : 0UL); } @@ -4309,7 +4330,7 @@ __STATIC_INLINE void LL_TIM_DisableSMSPreload(TIM_TypeDef *TIMx) * @param TIMx Timer instance * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_TIM_IsEnabledSMSPreload(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_IsEnabledSMSPreload(const TIM_TypeDef *TIMx) { return ((READ_BIT(TIMx->SMCR, TIM_SMCR_SMSPE) == (TIM_SMCR_SMSPE)) ? 1UL : 0UL); } @@ -4340,7 +4361,7 @@ __STATIC_INLINE void LL_TIM_SetSMSPreloadSource(TIM_TypeDef *TIMx, uint32_t Prel * @arg @ref LL_TIM_SMSPS_TIMUPDATE * @arg @ref LL_TIM_SMSPS_INDEX */ -__STATIC_INLINE uint32_t LL_TIM_GetSMSPreloadSource(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_GetSMSPreloadSource(const TIM_TypeDef *TIMx) { return (uint32_t)(READ_BIT(TIMx->SMCR, TIM_SMCR_SMSPS)); } @@ -4441,18 +4462,6 @@ __STATIC_INLINE void LL_TIM_DisarmBRK(TIM_TypeDef *TIMx) SET_BIT(TIMx->BDTR, TIM_BDTR_BKDSRM); } -/** - * @brief Re-arm the break input (when it operates in bidirectional mode). - * @note The Break input is automatically armed as soon as MOE bit is set. - * @rmtoll BDTR BKDSRM LL_TIM_ReArmBRK - * @param TIMx Timer instance - * @retval None - */ -__STATIC_INLINE void LL_TIM_ReArmBRK(TIM_TypeDef *TIMx) -{ - CLEAR_BIT(TIMx->BDTR, TIM_BDTR_BKDSRM); -} - /** * @brief Enable the break 2 function. * @note Macro IS_TIM_BKIN2_INSTANCE(TIMx) can be used to check whether or not @@ -4542,18 +4551,6 @@ __STATIC_INLINE void LL_TIM_DisarmBRK2(TIM_TypeDef *TIMx) SET_BIT(TIMx->BDTR, TIM_BDTR_BK2DSRM); } -/** - * @brief Re-arm the break 2 input (when it operates in bidirectional mode). - * @note The Break 2 input is automatically armed as soon as MOE bit is set. - * @rmtoll BDTR BK2DSRM LL_TIM_ReArmBRK2 - * @param TIMx Timer instance - * @retval None - */ -__STATIC_INLINE void LL_TIM_ReArmBRK2(TIM_TypeDef *TIMx) -{ - CLEAR_BIT(TIMx->BDTR, TIM_BDTR_BK2DSRM); -} - /** * @brief Select the outputs off state (enabled v.s. disabled) in Idle and Run modes. * @note Macro IS_TIM_BREAK_INSTANCE(TIMx) can be used to check whether or not @@ -4608,7 +4605,7 @@ __STATIC_INLINE void LL_TIM_DisableAutomaticOutput(TIM_TypeDef *TIMx) * @param TIMx Timer instance * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_TIM_IsEnabledAutomaticOutput(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_IsEnabledAutomaticOutput(const TIM_TypeDef *TIMx) { return ((READ_BIT(TIMx->BDTR, TIM_BDTR_AOE) == (TIM_BDTR_AOE)) ? 1UL : 0UL); } @@ -4651,7 +4648,7 @@ __STATIC_INLINE void LL_TIM_DisableAllOutputs(TIM_TypeDef *TIMx) * @param TIMx Timer instance * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_TIM_IsEnabledAllOutputs(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_IsEnabledAllOutputs(const TIM_TypeDef *TIMx) { return ((READ_BIT(TIMx->BDTR, TIM_BDTR_MOE) == (TIM_BDTR_MOE)) ? 1UL : 0UL); } @@ -4711,14 +4708,14 @@ __STATIC_INLINE void LL_TIM_EnableBreakInputSource(TIM_TypeDef *TIMx, uint32_t B * AF1 BKCMP5E LL_TIM_DisableBreakInputSource\n * AF1 BKCMP6E LL_TIM_DisableBreakInputSource\n * AF1 BKCMP7E LL_TIM_DisableBreakInputSource\n - * AF2 BKINE LL_TIM_DisableBreakInputSource\n - * AF2 BKCMP1E LL_TIM_DisableBreakInputSource\n - * AF2 BKCMP2E LL_TIM_DisableBreakInputSource\n - * AF2 BKCMP3E LL_TIM_DisableBreakInputSource\n - * AF2 BKCMP4E LL_TIM_DisableBreakInputSource\n - * AF2 BKCMP5E LL_TIM_DisableBreakInputSource\n - * AF2 BKCMP6E LL_TIM_DisableBreakInputSource\n - * AF2 BKCMP7E LL_TIM_DisableBreakInputSource + * AF2 BK2INE LL_TIM_DisableBreakInputSource\n + * AF2 BK2CMP1E LL_TIM_DisableBreakInputSource\n + * AF2 BK2CMP2E LL_TIM_DisableBreakInputSource\n + * AF2 BK2CMP3E LL_TIM_DisableBreakInputSource\n + * AF2 BK2CMP4E LL_TIM_DisableBreakInputSource\n + * AF2 BK2CMP5E LL_TIM_DisableBreakInputSource\n + * AF2 BK2CMP6E LL_TIM_DisableBreakInputSource\n + * AF2 BK2CMP7E LL_TIM_DisableBreakInputSource * @param TIMx Timer instance * @param BreakInput This parameter can be one of the following values: * @arg @ref LL_TIM_BREAK_INPUT_BKIN @@ -4751,11 +4748,17 @@ __STATIC_INLINE void LL_TIM_DisableBreakInputSource(TIM_TypeDef *TIMx, uint32_t * AF1 BKCMP2P LL_TIM_SetBreakInputSourcePolarity\n * AF1 BKCMP3P LL_TIM_SetBreakInputSourcePolarity\n * AF1 BKCMP4P LL_TIM_SetBreakInputSourcePolarity\n + * AF1 BKCMP5P LL_TIM_SetBreakInputSourcePolarity\n + * AF1 BKCMP6P LL_TIM_SetBreakInputSourcePolarity\n + * AF1 BKCMP7P LL_TIM_SetBreakInputSourcePolarity\n * AF2 BK2INP LL_TIM_SetBreakInputSourcePolarity\n * AF2 BK2CMP1P LL_TIM_SetBreakInputSourcePolarity\n * AF2 BK2CMP2P LL_TIM_SetBreakInputSourcePolarity\n * AF2 BK2CMP3P LL_TIM_SetBreakInputSourcePolarity\n - * AF2 BK2CMP4P LL_TIM_SetBreakInputSourcePolarity + * AF2 BK2CMP4P LL_TIM_SetBreakInputSourcePolarity\n + * AF2 BK2CMP5P LL_TIM_SetBreakInputSourcePolarity\n + * AF2 BK2CMP6P LL_TIM_SetBreakInputSourcePolarity\n + * AF2 BK2CMP7P LL_TIM_SetBreakInputSourcePolarity * @param TIMx Timer instance * @param BreakInput This parameter can be one of the following values: * @arg @ref LL_TIM_BREAK_INPUT_BKIN @@ -4766,9 +4769,14 @@ __STATIC_INLINE void LL_TIM_DisableBreakInputSource(TIM_TypeDef *TIMx, uint32_t * @arg @ref LL_TIM_BKIN_SOURCE_BKCOMP2 * @arg @ref LL_TIM_BKIN_SOURCE_BKCOMP3 * @arg @ref LL_TIM_BKIN_SOURCE_BKCOMP4 + * @arg @ref LL_TIM_BKIN_SOURCE_BKCOMP5 (*) + * @arg @ref LL_TIM_BKIN_SOURCE_BKCOMP6 (*) + * @arg @ref LL_TIM_BKIN_SOURCE_BKCOMP7 (*) * @param Polarity This parameter can be one of the following values: * @arg @ref LL_TIM_BKIN_POLARITY_LOW * @arg @ref LL_TIM_BKIN_POLARITY_HIGH + * + * (*) Value not defined in all devices. * @retval None */ __STATIC_INLINE void LL_TIM_SetBreakInputSourcePolarity(TIM_TypeDef *TIMx, uint32_t BreakInput, uint32_t Source, @@ -4811,7 +4819,7 @@ __STATIC_INLINE void LL_TIM_DisableAsymmetricalDeadTime(TIM_TypeDef *TIMx) * @param TIMx Timer instance * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_TIM_IsEnabledAsymmetricalDeadTime(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_IsEnabledAsymmetricalDeadTime(const TIM_TypeDef *TIMx) { return ((READ_BIT(TIMx->DTR2, TIM_DTR2_DTAE) == (TIM_DTR2_DTAE)) ? 1UL : 0UL); } @@ -4845,7 +4853,7 @@ __STATIC_INLINE void LL_TIM_SetFallingDeadTime(TIM_TypeDef *TIMx, uint32_t DeadT * @param TIMx Timer instance * @retval Returned value can be between Min_Data=0 and Max_Data=255: */ -__STATIC_INLINE uint32_t LL_TIM_GetFallingDeadTime(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_GetFallingDeadTime(const TIM_TypeDef *TIMx) { return (uint32_t)(READ_BIT(TIMx->DTR2, TIM_DTR2_DTGF)); } @@ -4884,7 +4892,7 @@ __STATIC_INLINE void LL_TIM_DisableDeadTimePreload(TIM_TypeDef *TIMx) * @param TIMx Timer instance * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_TIM_IsEnabledDeadTimePreload(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_IsEnabledDeadTimePreload(const TIM_TypeDef *TIMx) { return ((READ_BIT(TIMx->DTR2, TIM_DTR2_DTPE) == (TIM_DTR2_DTPE)) ? 1UL : 0UL); } @@ -5007,7 +5015,7 @@ __STATIC_INLINE void LL_TIM_DisableEncoderIndex(TIM_TypeDef *TIMx) * @param TIMx Timer instance * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_TIM_IsEnabledEncoderIndex(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_IsEnabledEncoderIndex(const TIM_TypeDef *TIMx) { return ((READ_BIT(TIMx->ECR, TIM_ECR_IE) == (TIM_ECR_IE)) ? 1U : 0U); } @@ -5040,7 +5048,7 @@ __STATIC_INLINE void LL_TIM_SetIndexDirection(TIM_TypeDef *TIMx, uint32_t IndexD * @arg @ref LL_TIM_INDEX_UP * @arg @ref LL_TIM_INDEX_DOWN */ -__STATIC_INLINE uint32_t LL_TIM_GetIndexDirection(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_GetIndexDirection(const TIM_TypeDef *TIMx) { return (uint32_t)(READ_BIT(TIMx->ECR, TIM_ECR_IDIR)); } @@ -5079,7 +5087,7 @@ __STATIC_INLINE void LL_TIM_DisableFirstIndex(TIM_TypeDef *TIMx) * @param TIMx Timer instance * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_TIM_IsEnabledFirstIndex(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_IsEnabledFirstIndex(const TIM_TypeDef *TIMx) { return ((READ_BIT(TIMx->ECR, TIM_ECR_FIDX) == (TIM_ECR_FIDX)) ? 1UL : 0UL); } @@ -5118,7 +5126,7 @@ __STATIC_INLINE void LL_TIM_SetIndexPositionning(TIM_TypeDef *TIMx, uint32_t Ind * @arg @ref LL_TIM_INDEX_POSITION_DOWN * @arg @ref LL_TIM_INDEX_POSITION_UP */ -__STATIC_INLINE uint32_t LL_TIM_GetIndexPositionning(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_GetIndexPositionning(const TIM_TypeDef *TIMx) { return (uint32_t)(READ_BIT(TIMx->ECR, TIM_ECR_IPOS)); } @@ -5391,7 +5399,7 @@ __STATIC_INLINE void LL_TIM_DisableHSE32(TIM_TypeDef *TIMx) * @param TIMx Timer instance * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_TIM_IsEnabledHSE32(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_IsEnabledHSE32(const TIM_TypeDef *TIMx) { return ((READ_BIT(TIMx->OR, TIM_OR_HSE32EN) == (TIM_OR_HSE32EN)) ? 1UL : 0UL); } @@ -5455,7 +5463,7 @@ __STATIC_INLINE void LL_TIM_ClearFlag_UPDATE(TIM_TypeDef *TIMx) * @param TIMx Timer instance * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_TIM_IsActiveFlag_UPDATE(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_IsActiveFlag_UPDATE(const TIM_TypeDef *TIMx) { return ((READ_BIT(TIMx->SR, TIM_SR_UIF) == (TIM_SR_UIF)) ? 1UL : 0UL); } @@ -5477,7 +5485,7 @@ __STATIC_INLINE void LL_TIM_ClearFlag_CC1(TIM_TypeDef *TIMx) * @param TIMx Timer instance * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_TIM_IsActiveFlag_CC1(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_IsActiveFlag_CC1(const TIM_TypeDef *TIMx) { return ((READ_BIT(TIMx->SR, TIM_SR_CC1IF) == (TIM_SR_CC1IF)) ? 1UL : 0UL); } @@ -5499,7 +5507,7 @@ __STATIC_INLINE void LL_TIM_ClearFlag_CC2(TIM_TypeDef *TIMx) * @param TIMx Timer instance * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_TIM_IsActiveFlag_CC2(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_IsActiveFlag_CC2(const TIM_TypeDef *TIMx) { return ((READ_BIT(TIMx->SR, TIM_SR_CC2IF) == (TIM_SR_CC2IF)) ? 1UL : 0UL); } @@ -5521,7 +5529,7 @@ __STATIC_INLINE void LL_TIM_ClearFlag_CC3(TIM_TypeDef *TIMx) * @param TIMx Timer instance * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_TIM_IsActiveFlag_CC3(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_IsActiveFlag_CC3(const TIM_TypeDef *TIMx) { return ((READ_BIT(TIMx->SR, TIM_SR_CC3IF) == (TIM_SR_CC3IF)) ? 1UL : 0UL); } @@ -5543,7 +5551,7 @@ __STATIC_INLINE void LL_TIM_ClearFlag_CC4(TIM_TypeDef *TIMx) * @param TIMx Timer instance * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_TIM_IsActiveFlag_CC4(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_IsActiveFlag_CC4(const TIM_TypeDef *TIMx) { return ((READ_BIT(TIMx->SR, TIM_SR_CC4IF) == (TIM_SR_CC4IF)) ? 1UL : 0UL); } @@ -5565,7 +5573,7 @@ __STATIC_INLINE void LL_TIM_ClearFlag_CC5(TIM_TypeDef *TIMx) * @param TIMx Timer instance * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_TIM_IsActiveFlag_CC5(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_IsActiveFlag_CC5(const TIM_TypeDef *TIMx) { return ((READ_BIT(TIMx->SR, TIM_SR_CC5IF) == (TIM_SR_CC5IF)) ? 1UL : 0UL); } @@ -5587,7 +5595,7 @@ __STATIC_INLINE void LL_TIM_ClearFlag_CC6(TIM_TypeDef *TIMx) * @param TIMx Timer instance * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_TIM_IsActiveFlag_CC6(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_IsActiveFlag_CC6(const TIM_TypeDef *TIMx) { return ((READ_BIT(TIMx->SR, TIM_SR_CC6IF) == (TIM_SR_CC6IF)) ? 1UL : 0UL); } @@ -5609,7 +5617,7 @@ __STATIC_INLINE void LL_TIM_ClearFlag_COM(TIM_TypeDef *TIMx) * @param TIMx Timer instance * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_TIM_IsActiveFlag_COM(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_IsActiveFlag_COM(const TIM_TypeDef *TIMx) { return ((READ_BIT(TIMx->SR, TIM_SR_COMIF) == (TIM_SR_COMIF)) ? 1UL : 0UL); } @@ -5631,7 +5639,7 @@ __STATIC_INLINE void LL_TIM_ClearFlag_TRIG(TIM_TypeDef *TIMx) * @param TIMx Timer instance * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_TIM_IsActiveFlag_TRIG(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_IsActiveFlag_TRIG(const TIM_TypeDef *TIMx) { return ((READ_BIT(TIMx->SR, TIM_SR_TIF) == (TIM_SR_TIF)) ? 1UL : 0UL); } @@ -5653,7 +5661,7 @@ __STATIC_INLINE void LL_TIM_ClearFlag_BRK(TIM_TypeDef *TIMx) * @param TIMx Timer instance * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_TIM_IsActiveFlag_BRK(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_IsActiveFlag_BRK(const TIM_TypeDef *TIMx) { return ((READ_BIT(TIMx->SR, TIM_SR_BIF) == (TIM_SR_BIF)) ? 1UL : 0UL); } @@ -5675,7 +5683,7 @@ __STATIC_INLINE void LL_TIM_ClearFlag_BRK2(TIM_TypeDef *TIMx) * @param TIMx Timer instance * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_TIM_IsActiveFlag_BRK2(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_IsActiveFlag_BRK2(const TIM_TypeDef *TIMx) { return ((READ_BIT(TIMx->SR, TIM_SR_B2IF) == (TIM_SR_B2IF)) ? 1UL : 0UL); } @@ -5698,7 +5706,7 @@ __STATIC_INLINE void LL_TIM_ClearFlag_CC1OVR(TIM_TypeDef *TIMx) * @param TIMx Timer instance * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_TIM_IsActiveFlag_CC1OVR(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_IsActiveFlag_CC1OVR(const TIM_TypeDef *TIMx) { return ((READ_BIT(TIMx->SR, TIM_SR_CC1OF) == (TIM_SR_CC1OF)) ? 1UL : 0UL); } @@ -5721,7 +5729,7 @@ __STATIC_INLINE void LL_TIM_ClearFlag_CC2OVR(TIM_TypeDef *TIMx) * @param TIMx Timer instance * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_TIM_IsActiveFlag_CC2OVR(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_IsActiveFlag_CC2OVR(const TIM_TypeDef *TIMx) { return ((READ_BIT(TIMx->SR, TIM_SR_CC2OF) == (TIM_SR_CC2OF)) ? 1UL : 0UL); } @@ -5744,7 +5752,7 @@ __STATIC_INLINE void LL_TIM_ClearFlag_CC3OVR(TIM_TypeDef *TIMx) * @param TIMx Timer instance * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_TIM_IsActiveFlag_CC3OVR(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_IsActiveFlag_CC3OVR(const TIM_TypeDef *TIMx) { return ((READ_BIT(TIMx->SR, TIM_SR_CC3OF) == (TIM_SR_CC3OF)) ? 1UL : 0UL); } @@ -5767,7 +5775,7 @@ __STATIC_INLINE void LL_TIM_ClearFlag_CC4OVR(TIM_TypeDef *TIMx) * @param TIMx Timer instance * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_TIM_IsActiveFlag_CC4OVR(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_IsActiveFlag_CC4OVR(const TIM_TypeDef *TIMx) { return ((READ_BIT(TIMx->SR, TIM_SR_CC4OF) == (TIM_SR_CC4OF)) ? 1UL : 0UL); } @@ -5789,7 +5797,7 @@ __STATIC_INLINE void LL_TIM_ClearFlag_SYSBRK(TIM_TypeDef *TIMx) * @param TIMx Timer instance * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_TIM_IsActiveFlag_SYSBRK(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_IsActiveFlag_SYSBRK(const TIM_TypeDef *TIMx) { return ((READ_BIT(TIMx->SR, TIM_SR_SBIF) == (TIM_SR_SBIF)) ? 1UL : 0UL); } @@ -5815,7 +5823,7 @@ __STATIC_INLINE void LL_TIM_ClearFlag_TERR(TIM_TypeDef *TIMx) * @param TIMx Timer instance * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_TIM_IsActiveFlag_TERR(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_IsActiveFlag_TERR(const TIM_TypeDef *TIMx) { return ((READ_BIT(TIMx->SR, TIM_SR_TERRF) == (TIM_SR_TERRF)) ? 1UL : 0UL); } @@ -5841,7 +5849,7 @@ __STATIC_INLINE void LL_TIM_ClearFlag_IERR(TIM_TypeDef *TIMx) * @param TIMx Timer instance * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_TIM_IsActiveFlag_IERR(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_IsActiveFlag_IERR(const TIM_TypeDef *TIMx) { return ((READ_BIT(TIMx->SR, TIM_SR_IERRF) == (TIM_SR_IERRF)) ? 1UL : 0UL); } @@ -5867,7 +5875,7 @@ __STATIC_INLINE void LL_TIM_ClearFlag_DIR(TIM_TypeDef *TIMx) * @param TIMx Timer instance * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_TIM_IsActiveFlag_DIR(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_IsActiveFlag_DIR(const TIM_TypeDef *TIMx) { return ((READ_BIT(TIMx->SR, TIM_SR_DIRF) == (TIM_SR_DIRF)) ? 1UL : 0UL); } @@ -5893,7 +5901,7 @@ __STATIC_INLINE void LL_TIM_ClearFlag_IDX(TIM_TypeDef *TIMx) * @param TIMx Timer instance * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_TIM_IsActiveFlag_IDX(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_IsActiveFlag_IDX(const TIM_TypeDef *TIMx) { return ((READ_BIT(TIMx->SR, TIM_SR_IDXF) == (TIM_SR_IDXF)) ? 1UL : 0UL); } @@ -5932,7 +5940,7 @@ __STATIC_INLINE void LL_TIM_DisableIT_UPDATE(TIM_TypeDef *TIMx) * @param TIMx Timer instance * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_TIM_IsEnabledIT_UPDATE(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_IsEnabledIT_UPDATE(const TIM_TypeDef *TIMx) { return ((READ_BIT(TIMx->DIER, TIM_DIER_UIE) == (TIM_DIER_UIE)) ? 1UL : 0UL); } @@ -5965,7 +5973,7 @@ __STATIC_INLINE void LL_TIM_DisableIT_CC1(TIM_TypeDef *TIMx) * @param TIMx Timer instance * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_TIM_IsEnabledIT_CC1(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_IsEnabledIT_CC1(const TIM_TypeDef *TIMx) { return ((READ_BIT(TIMx->DIER, TIM_DIER_CC1IE) == (TIM_DIER_CC1IE)) ? 1UL : 0UL); } @@ -5998,7 +6006,7 @@ __STATIC_INLINE void LL_TIM_DisableIT_CC2(TIM_TypeDef *TIMx) * @param TIMx Timer instance * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_TIM_IsEnabledIT_CC2(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_IsEnabledIT_CC2(const TIM_TypeDef *TIMx) { return ((READ_BIT(TIMx->DIER, TIM_DIER_CC2IE) == (TIM_DIER_CC2IE)) ? 1UL : 0UL); } @@ -6031,7 +6039,7 @@ __STATIC_INLINE void LL_TIM_DisableIT_CC3(TIM_TypeDef *TIMx) * @param TIMx Timer instance * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_TIM_IsEnabledIT_CC3(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_IsEnabledIT_CC3(const TIM_TypeDef *TIMx) { return ((READ_BIT(TIMx->DIER, TIM_DIER_CC3IE) == (TIM_DIER_CC3IE)) ? 1UL : 0UL); } @@ -6064,7 +6072,7 @@ __STATIC_INLINE void LL_TIM_DisableIT_CC4(TIM_TypeDef *TIMx) * @param TIMx Timer instance * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_TIM_IsEnabledIT_CC4(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_IsEnabledIT_CC4(const TIM_TypeDef *TIMx) { return ((READ_BIT(TIMx->DIER, TIM_DIER_CC4IE) == (TIM_DIER_CC4IE)) ? 1UL : 0UL); } @@ -6097,7 +6105,7 @@ __STATIC_INLINE void LL_TIM_DisableIT_COM(TIM_TypeDef *TIMx) * @param TIMx Timer instance * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_TIM_IsEnabledIT_COM(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_IsEnabledIT_COM(const TIM_TypeDef *TIMx) { return ((READ_BIT(TIMx->DIER, TIM_DIER_COMIE) == (TIM_DIER_COMIE)) ? 1UL : 0UL); } @@ -6130,7 +6138,7 @@ __STATIC_INLINE void LL_TIM_DisableIT_TRIG(TIM_TypeDef *TIMx) * @param TIMx Timer instance * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_TIM_IsEnabledIT_TRIG(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_IsEnabledIT_TRIG(const TIM_TypeDef *TIMx) { return ((READ_BIT(TIMx->DIER, TIM_DIER_TIE) == (TIM_DIER_TIE)) ? 1UL : 0UL); } @@ -6163,7 +6171,7 @@ __STATIC_INLINE void LL_TIM_DisableIT_BRK(TIM_TypeDef *TIMx) * @param TIMx Timer instance * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_TIM_IsEnabledIT_BRK(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_IsEnabledIT_BRK(const TIM_TypeDef *TIMx) { return ((READ_BIT(TIMx->DIER, TIM_DIER_BIE) == (TIM_DIER_BIE)) ? 1UL : 0UL); } @@ -6202,7 +6210,7 @@ __STATIC_INLINE void LL_TIM_DisableIT_TERR(TIM_TypeDef *TIMx) * @param TIMx Timer instance * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_TIM_IsEnabledIT_TERR(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_IsEnabledIT_TERR(const TIM_TypeDef *TIMx) { return ((READ_BIT(TIMx->DIER, TIM_DIER_TERRIE) == (TIM_DIER_TERRIE)) ? 1UL : 0UL); } @@ -6241,7 +6249,7 @@ __STATIC_INLINE void LL_TIM_DisableIT_IERR(TIM_TypeDef *TIMx) * @param TIMx Timer instance * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_TIM_IsEnabledIT_IERR(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_IsEnabledIT_IERR(const TIM_TypeDef *TIMx) { return ((READ_BIT(TIMx->DIER, TIM_DIER_IERRIE) == (TIM_DIER_IERRIE)) ? 1UL : 0UL); } @@ -6280,7 +6288,7 @@ __STATIC_INLINE void LL_TIM_DisableIT_DIR(TIM_TypeDef *TIMx) * @param TIMx Timer instance * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_TIM_IsEnabledIT_DIR(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_IsEnabledIT_DIR(const TIM_TypeDef *TIMx) { return ((READ_BIT(TIMx->DIER, TIM_DIER_DIRIE) == (TIM_DIER_DIRIE)) ? 1UL : 0UL); } @@ -6319,7 +6327,7 @@ __STATIC_INLINE void LL_TIM_DisableIT_IDX(TIM_TypeDef *TIMx) * @param TIMx Timer instance * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_TIM_IsEnabledIT_IDX(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_IsEnabledIT_IDX(const TIM_TypeDef *TIMx) { return ((READ_BIT(TIMx->DIER, TIM_DIER_IDXIE) == (TIM_DIER_IDXIE)) ? 1UL : 0UL); } @@ -6359,7 +6367,7 @@ __STATIC_INLINE void LL_TIM_DisableDMAReq_UPDATE(TIM_TypeDef *TIMx) * @param TIMx Timer instance * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_TIM_IsEnabledDMAReq_UPDATE(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_IsEnabledDMAReq_UPDATE(const TIM_TypeDef *TIMx) { return ((READ_BIT(TIMx->DIER, TIM_DIER_UDE) == (TIM_DIER_UDE)) ? 1UL : 0UL); } @@ -6392,7 +6400,7 @@ __STATIC_INLINE void LL_TIM_DisableDMAReq_CC1(TIM_TypeDef *TIMx) * @param TIMx Timer instance * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_TIM_IsEnabledDMAReq_CC1(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_IsEnabledDMAReq_CC1(const TIM_TypeDef *TIMx) { return ((READ_BIT(TIMx->DIER, TIM_DIER_CC1DE) == (TIM_DIER_CC1DE)) ? 1UL : 0UL); } @@ -6425,7 +6433,7 @@ __STATIC_INLINE void LL_TIM_DisableDMAReq_CC2(TIM_TypeDef *TIMx) * @param TIMx Timer instance * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_TIM_IsEnabledDMAReq_CC2(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_IsEnabledDMAReq_CC2(const TIM_TypeDef *TIMx) { return ((READ_BIT(TIMx->DIER, TIM_DIER_CC2DE) == (TIM_DIER_CC2DE)) ? 1UL : 0UL); } @@ -6458,7 +6466,7 @@ __STATIC_INLINE void LL_TIM_DisableDMAReq_CC3(TIM_TypeDef *TIMx) * @param TIMx Timer instance * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_TIM_IsEnabledDMAReq_CC3(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_IsEnabledDMAReq_CC3(const TIM_TypeDef *TIMx) { return ((READ_BIT(TIMx->DIER, TIM_DIER_CC3DE) == (TIM_DIER_CC3DE)) ? 1UL : 0UL); } @@ -6491,7 +6499,7 @@ __STATIC_INLINE void LL_TIM_DisableDMAReq_CC4(TIM_TypeDef *TIMx) * @param TIMx Timer instance * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_TIM_IsEnabledDMAReq_CC4(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_IsEnabledDMAReq_CC4(const TIM_TypeDef *TIMx) { return ((READ_BIT(TIMx->DIER, TIM_DIER_CC4DE) == (TIM_DIER_CC4DE)) ? 1UL : 0UL); } @@ -6524,7 +6532,7 @@ __STATIC_INLINE void LL_TIM_DisableDMAReq_COM(TIM_TypeDef *TIMx) * @param TIMx Timer instance * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_TIM_IsEnabledDMAReq_COM(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_IsEnabledDMAReq_COM(const TIM_TypeDef *TIMx) { return ((READ_BIT(TIMx->DIER, TIM_DIER_COMDE) == (TIM_DIER_COMDE)) ? 1UL : 0UL); } @@ -6557,7 +6565,7 @@ __STATIC_INLINE void LL_TIM_DisableDMAReq_TRIG(TIM_TypeDef *TIMx) * @param TIMx Timer instance * @retval State of bit (1 or 0). */ -__STATIC_INLINE uint32_t LL_TIM_IsEnabledDMAReq_TRIG(TIM_TypeDef *TIMx) +__STATIC_INLINE uint32_t LL_TIM_IsEnabledDMAReq_TRIG(const TIM_TypeDef *TIMx) { return ((READ_BIT(TIMx->DIER, TIM_DIER_TDE) == (TIM_DIER_TDE)) ? 1UL : 0UL); } @@ -6677,19 +6685,19 @@ __STATIC_INLINE void LL_TIM_GenerateEvent_BRK2(TIM_TypeDef *TIMx) * @{ */ -ErrorStatus LL_TIM_DeInit(TIM_TypeDef *TIMx); +ErrorStatus LL_TIM_DeInit(const TIM_TypeDef *TIMx); void LL_TIM_StructInit(LL_TIM_InitTypeDef *TIM_InitStruct); -ErrorStatus LL_TIM_Init(TIM_TypeDef *TIMx, LL_TIM_InitTypeDef *TIM_InitStruct); +ErrorStatus LL_TIM_Init(TIM_TypeDef *TIMx, const LL_TIM_InitTypeDef *TIM_InitStruct); void LL_TIM_OC_StructInit(LL_TIM_OC_InitTypeDef *TIM_OC_InitStruct); -ErrorStatus LL_TIM_OC_Init(TIM_TypeDef *TIMx, uint32_t Channel, LL_TIM_OC_InitTypeDef *TIM_OC_InitStruct); +ErrorStatus LL_TIM_OC_Init(TIM_TypeDef *TIMx, uint32_t Channel, const LL_TIM_OC_InitTypeDef *TIM_OC_InitStruct); void LL_TIM_IC_StructInit(LL_TIM_IC_InitTypeDef *TIM_ICInitStruct); -ErrorStatus LL_TIM_IC_Init(TIM_TypeDef *TIMx, uint32_t Channel, LL_TIM_IC_InitTypeDef *TIM_IC_InitStruct); +ErrorStatus LL_TIM_IC_Init(TIM_TypeDef *TIMx, uint32_t Channel, const LL_TIM_IC_InitTypeDef *TIM_IC_InitStruct); void LL_TIM_ENCODER_StructInit(LL_TIM_ENCODER_InitTypeDef *TIM_EncoderInitStruct); -ErrorStatus LL_TIM_ENCODER_Init(TIM_TypeDef *TIMx, LL_TIM_ENCODER_InitTypeDef *TIM_EncoderInitStruct); +ErrorStatus LL_TIM_ENCODER_Init(TIM_TypeDef *TIMx, const LL_TIM_ENCODER_InitTypeDef *TIM_EncoderInitStruct); void LL_TIM_HALLSENSOR_StructInit(LL_TIM_HALLSENSOR_InitTypeDef *TIM_HallSensorInitStruct); -ErrorStatus LL_TIM_HALLSENSOR_Init(TIM_TypeDef *TIMx, LL_TIM_HALLSENSOR_InitTypeDef *TIM_HallSensorInitStruct); +ErrorStatus LL_TIM_HALLSENSOR_Init(TIM_TypeDef *TIMx, const LL_TIM_HALLSENSOR_InitTypeDef *TIM_HallSensorInitStruct); void LL_TIM_BDTR_StructInit(LL_TIM_BDTR_InitTypeDef *TIM_BDTRInitStruct); -ErrorStatus LL_TIM_BDTR_Init(TIM_TypeDef *TIMx, LL_TIM_BDTR_InitTypeDef *TIM_BDTRInitStruct); +ErrorStatus LL_TIM_BDTR_Init(TIM_TypeDef *TIMx, const LL_TIM_BDTR_InitTypeDef *TIM_BDTRInitStruct); /** * @} */ diff --git a/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_ll_utils.h b/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_ll_utils.h index 6a12eb723..aa496bf3b 100644 --- a/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_ll_utils.h +++ b/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_ll_utils.h @@ -158,18 +158,31 @@ typedef struct /** @defgroup UTILS_EC_PACKAGETYPE PACKAGE TYPE * @{ */ -#define LL_UTILS_PACKAGETYPE_LQFP64 0x00000000U /*!< LQFP64 package type */ -#define LL_UTILS_PACKAGETYPE_LQFP100 0x00000002U /*!< LQFP100 package type */ -#define LL_UTILS_PACKAGETYPE_WLCSP81 0x00000005U /*!< WLCSP81 package type */ -#define LL_UTILS_PACKAGETYPE_LQFP128 0x00000007U /*!< LQFP128 package type */ -#define LL_UTILS_PACKAGETYPE_UFQFPN32 0x00000008U /*!< UFQFPN32 package type */ -#define LL_UTILS_PACKAGETYPE_LQFP32 0x00000009U /*!< LQFP32 package type */ -#define LL_UTILS_PACKAGETYPE_UFQFPN48 0x0000000AU /*!< UFQFPN48 package type */ -#define LL_UTILS_PACKAGETYPE_LQFP48 0x0000000BU /*!< LQFP48 package type */ -#define LL_UTILS_PACKAGETYPE_WLCSP49 0x0000000CU /*!< WLCSP49 package type */ -#define LL_UTILS_PACKAGETYPE_UFBGA64 0x0000000DU /*!< UFBGA64 package type */ -#define LL_UTILS_PACKAGETYPE_UFBGA100 0x0000000EU /*!< UFBGA100 package type */ -#define LL_UTILS_PACKAGETYPE_LQFP48_EBIKE 0x00000010U /*!< LQFP48 EBIKE package type */ +#define LL_UTILS_PACKAGETYPE_LQFP64 0x00000000U /*!< LQFP64 package type */ +#define LL_UTILS_PACKAGETYPE_WLCSP64 0x00000001U /*!< WLCSP64 package type */ +#if defined (STM32G431xx) || defined (STM32G441xx) || defined (STM32G471xx) || \ + defined (STM32G473xx) || defined (STM32G483xx) || defined (STM32G474xx) || \ + defined (STM32G484xx) +#define LL_UTILS_PACKAGETYPE_LQFP100_LQFP80 0x00000002U /*!< LQFP100 \ LQFP80 package type */ +#define LL_UTILS_PACKAGETYPE_LQFP100 LL_UTILS_PACKAGETYPE_LQFP100_LQFP80 /*!< For backward compatibility */ +#else +#define LL_UTILS_PACKAGETYPE_LQFP100 0x00000002U /*!< LQFP100 package type */ +#endif /* STM32G431xx || STM32G441xx || STM32G471xx || STM32G473xx || STM32G483xx ||STM32G474xx || STM32G484xx */ +#define LL_UTILS_PACKAGETYPE_WLCSP81 0x00000005U /*!< WLCSP81 package type */ +#define LL_UTILS_PACKAGETYPE_LQFP128_UFBGA121 0x00000007U /*!< LQFP128 \ UFBGA121 package type */ +#define LL_UTILS_PACKAGETYPE_LQFP128 LL_UTILS_PACKAGETYPE_LQFP128_UFBGA121 /*!< For backward compatibility */ +#define LL_UTILS_PACKAGETYPE_UFQFPN32 0x00000008U /*!< UFQFPN32 package type */ +#define LL_UTILS_PACKAGETYPE_LQFP32 0x00000009U /*!< LQFP32 package type */ +#define LL_UTILS_PACKAGETYPE_UFQFPN48 0x0000000AU /*!< UFQFPN48 package type */ +#define LL_UTILS_PACKAGETYPE_LQFP48 0x0000000BU /*!< LQFP48 package type */ +#define LL_UTILS_PACKAGETYPE_WLCSP49 0x0000000CU /*!< WLCSP49 package type */ +#define LL_UTILS_PACKAGETYPE_UFBGA64 0x0000000DU /*!< UFBGA64 package type */ +#define LL_UTILS_PACKAGETYPE_TFBGA100 0x0000000EU /*!< TFBGA100 package type */ +#define LL_UTILS_PACKAGETYPE_UFBGA100 LL_UTILS_PACKAGETYPE_TFBGA100 /*!< For backward compatibility */ +#define LL_UTILS_PACKAGETYPE_LQFP48_EBIKE 0x00000010U /*!< LQFP48 EBIKE package type */ +#if defined (STM32G491xx) || defined (STM32G4A1xx) +#define LL_UTILS_PACKAGETYPE_LQFP80 0x00000011U /*!< LQFP80 package type */ +#endif /* STM32G491xx || STM32G4A1xx */ /** * @} @@ -263,7 +276,7 @@ __STATIC_INLINE uint32_t LL_GetPackageType(void) * @param HCLKFrequency HCLK frequency in Hz (can be calculated thanks to RCC helper macro) * @note When a RTOS is used, it is recommended to avoid changing the SysTick * configuration by calling this function, for a delay use rather osDelay RTOS service. - * @param Ticks Number of ticks + * @param Ticks Frequency of Ticks (Hz) * @retval None */ __STATIC_INLINE void LL_InitTick(uint32_t HCLKFrequency, uint32_t Ticks) diff --git a/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal.c b/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal.c index 3d9e92f8e..2fa9e4898 100644 --- a/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal.c +++ b/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal.c @@ -48,11 +48,11 @@ /* Private typedef -----------------------------------------------------------*/ /* Private define ------------------------------------------------------------*/ /** - * @brief STM32G4xx HAL Driver version number V1.2.2 + * @brief STM32G4xx HAL Driver version number V1.2.3 */ #define __STM32G4xx_HAL_VERSION_MAIN (0x01U) /*!< [31:24] main version */ #define __STM32G4xx_HAL_VERSION_SUB1 (0x02U) /*!< [23:16] sub1 version */ -#define __STM32G4xx_HAL_VERSION_SUB2 (0x02U) /*!< [15:8] sub2 version */ +#define __STM32G4xx_HAL_VERSION_SUB2 (0x03U) /*!< [15:8] sub2 version */ #define __STM32G4xx_HAL_VERSION_RC (0x00U) /*!< [7:0] release candidate */ #define __STM32G4xx_HAL_VERSION ((__STM32G4xx_HAL_VERSION_MAIN << 24U)\ |(__STM32G4xx_HAL_VERSION_SUB1 << 16U)\ @@ -378,7 +378,8 @@ HAL_StatusTypeDef HAL_SetTickFreq(uint32_t Freq) /** * @brief Returns tick frequency. - * @retval tick period in Hz + * @retval Tick frequency. + * Value of @ref HAL_TickFreqTypeDef. */ uint32_t HAL_GetTickFreq(void) { @@ -471,6 +472,33 @@ uint32_t HAL_GetDEVID(void) return (DBGMCU->IDCODE & DBGMCU_IDCODE_DEV_ID); } +/** + * @brief Return the first word of the unique device identifier (UID based on 96 bits) + * @retval Device identifier + */ +uint32_t HAL_GetUIDw0(void) +{ + return (READ_REG(*((uint32_t *)UID_BASE))); +} + +/** + * @brief Return the second word of the unique device identifier (UID based on 96 bits) + * @retval Device identifier + */ +uint32_t HAL_GetUIDw1(void) +{ + return (READ_REG(*((uint32_t *)(UID_BASE + 4U)))); +} + +/** + * @brief Return the third word of the unique device identifier (UID based on 96 bits) + * @retval Device identifier + */ +uint32_t HAL_GetUIDw2(void) +{ + return (READ_REG(*((uint32_t *)(UID_BASE + 8U)))); +} + /** * @} */ diff --git a/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_exti.c b/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_exti.c index 94af4ccc5..559cde87d 100644 --- a/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_exti.c +++ b/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_exti.c @@ -65,7 +65,7 @@ (++) Provide exiting handle as parameter. (++) Provide pointer on EXTI_ConfigTypeDef structure as second parameter. - (#) Clear Exti configuration of a dedicated line using HAL_EXTI_GetConfigLine(). + (#) Clear Exti configuration of a dedicated line using HAL_EXTI_ClearConfigLine(). (++) Provide exiting handle as parameter. (#) Register callback to treat Exti interrupts using HAL_EXTI_RegisterCallback(). @@ -348,7 +348,7 @@ HAL_StatusTypeDef HAL_EXTI_GetConfigLine(EXTI_HandleTypeDef *hexti, EXTI_ConfigT assert_param(IS_EXTI_GPIO_PIN(linepos)); regval = SYSCFG->EXTICR[linepos >> 2u]; - pExtiConfig->GPIOSel = ((regval >> (SYSCFG_EXTICR1_EXTI1_Pos * ((linepos & 0x03u))))); + pExtiConfig->GPIOSel = (regval >> (SYSCFG_EXTICR1_EXTI1_Pos * (linepos & 0x03u))) & SYSCFG_EXTICR1_EXTI0; } } diff --git a/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_fdcan.c b/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_fdcan.c index 4869e493e..2a9a45551 100644 --- a/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_fdcan.c +++ b/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_fdcan.c @@ -96,7 +96,7 @@ *** Callback registration *** ============================================= - The compilation define USE_HAL_FDCAN_REGISTER_CALLBACKS when set to 1 + The compilation define USE_HAL_FDCAN_REGISTER_CALLBACKS when set to 1 allows the user to configure dynamically the driver callbacks. Use Function HAL_FDCAN_RegisterCallback() or HAL_FDCAN_RegisterXXXCallback() to register an interrupt callback. @@ -114,7 +114,7 @@ For specific callbacks TxEventFifoCallback, RxFifo0Callback, RxFifo1Callback, TxBufferCompleteCallback, TxBufferAbortCallback and ErrorStatusCallback use dedicated - register callbacks : respectively HAL_FDCAN_RegisterTxEventFifoCallback(), + register callbacks: respectively HAL_FDCAN_RegisterTxEventFifoCallback(), HAL_FDCAN_RegisterRxFifo0Callback(), HAL_FDCAN_RegisterRxFifo1Callback(), HAL_FDCAN_RegisterTxBufferCompleteCallback(), HAL_FDCAN_RegisterTxBufferAbortCallback() and HAL_FDCAN_RegisterErrorStatusCallback(). @@ -134,7 +134,7 @@ For specific callbacks TxEventFifoCallback, RxFifo0Callback, RxFifo1Callback, TxBufferCompleteCallback and TxBufferAbortCallback, use dedicated - unregister callbacks : respectively HAL_FDCAN_UnRegisterTxEventFifoCallback(), + unregister callbacks: respectively HAL_FDCAN_UnRegisterTxEventFifoCallback(), HAL_FDCAN_UnRegisterRxFifo0Callback(), HAL_FDCAN_UnRegisterRxFifo1Callback(), HAL_FDCAN_UnRegisterTxBufferCompleteCallback(), HAL_FDCAN_UnRegisterTxBufferAbortCallback() and HAL_FDCAN_UnRegisterErrorStatusCallback(). @@ -249,9 +249,15 @@ static const uint8_t DLCtoBytes[] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 12, 16, 20, 24, */ /* Private function prototypes -----------------------------------------------*/ +/** @addtogroup FDCAN_Private_Functions_Prototypes + * @{ + */ static void FDCAN_CalcultateRamBlockAddresses(FDCAN_HandleTypeDef *hfdcan); -static void FDCAN_CopyMessageToRAM(FDCAN_HandleTypeDef *hfdcan, FDCAN_TxHeaderTypeDef *pTxHeader, uint8_t *pTxData, - uint32_t BufferIndex); +static void FDCAN_CopyMessageToRAM(const FDCAN_HandleTypeDef *hfdcan, const FDCAN_TxHeaderTypeDef *pTxHeader, + const uint8_t *pTxData, uint32_t BufferIndex); +/** + * @} + */ /* Exported functions --------------------------------------------------------*/ /** @defgroup FDCAN_Exported_Functions FDCAN Exported Functions @@ -327,22 +333,17 @@ HAL_StatusTypeDef HAL_FDCAN_Init(FDCAN_HandleTypeDef *hfdcan) hfdcan->Lock = HAL_UNLOCKED; /* Reset callbacks to legacy functions */ - hfdcan->TxEventFifoCallback = HAL_FDCAN_TxEventFifoCallback; /* Legacy weak TxEventFifoCallback */ - hfdcan->RxFifo0Callback = HAL_FDCAN_RxFifo0Callback; /* Legacy weak RxFifo0Callback */ - hfdcan->RxFifo1Callback = HAL_FDCAN_RxFifo1Callback; /* Legacy weak RxFifo1Callback */ - hfdcan->TxFifoEmptyCallback = HAL_FDCAN_TxFifoEmptyCallback; /* Legacy weak TxFifoEmptyCallback */ - hfdcan->TxBufferCompleteCallback = HAL_FDCAN_TxBufferCompleteCallback; /* Legacy weak - TxBufferCompleteCallback */ - hfdcan->TxBufferAbortCallback = HAL_FDCAN_TxBufferAbortCallback; /* Legacy weak - TxBufferAbortCallback */ - hfdcan->HighPriorityMessageCallback = HAL_FDCAN_HighPriorityMessageCallback; /* Legacy weak - HighPriorityMessageCallback */ - hfdcan->TimestampWraparoundCallback = HAL_FDCAN_TimestampWraparoundCallback; /* Legacy weak - TimestampWraparoundCallback */ - hfdcan->TimeoutOccurredCallback = HAL_FDCAN_TimeoutOccurredCallback; /* Legacy weak - TimeoutOccurredCallback */ - hfdcan->ErrorCallback = HAL_FDCAN_ErrorCallback; /* Legacy weak ErrorCallback */ - hfdcan->ErrorStatusCallback = HAL_FDCAN_ErrorStatusCallback; /* Legacy weak ErrorStatusCallback */ + hfdcan->TxEventFifoCallback = HAL_FDCAN_TxEventFifoCallback; /* TxEventFifoCallback */ + hfdcan->RxFifo0Callback = HAL_FDCAN_RxFifo0Callback; /* RxFifo0Callback */ + hfdcan->RxFifo1Callback = HAL_FDCAN_RxFifo1Callback; /* RxFifo1Callback */ + hfdcan->TxFifoEmptyCallback = HAL_FDCAN_TxFifoEmptyCallback; /* TxFifoEmptyCallback */ + hfdcan->TxBufferCompleteCallback = HAL_FDCAN_TxBufferCompleteCallback; /* TxBufferCompleteCallback */ + hfdcan->TxBufferAbortCallback = HAL_FDCAN_TxBufferAbortCallback; /* TxBufferAbortCallback */ + hfdcan->HighPriorityMessageCallback = HAL_FDCAN_HighPriorityMessageCallback; /* HighPriorityMessageCallback */ + hfdcan->TimestampWraparoundCallback = HAL_FDCAN_TimestampWraparoundCallback; /* TimestampWraparoundCallback */ + hfdcan->TimeoutOccurredCallback = HAL_FDCAN_TimeoutOccurredCallback; /* TimeoutOccurredCallback */ + hfdcan->ErrorCallback = HAL_FDCAN_ErrorCallback; /* ErrorCallback */ + hfdcan->ErrorStatusCallback = HAL_FDCAN_ErrorStatusCallback; /* ErrorStatusCallback */ if (hfdcan->MspInitCallback == NULL) { @@ -582,7 +583,7 @@ __weak void HAL_FDCAN_MspInit(FDCAN_HandleTypeDef *hfdcan) { /* Prevent unused argument(s) compilation warning */ UNUSED(hfdcan); - /* NOTE : This function Should not be modified, when the callback is needed, + /* NOTE: This function Should not be modified, when the callback is needed, the HAL_FDCAN_MspInit could be implemented in the user file */ } @@ -597,7 +598,7 @@ __weak void HAL_FDCAN_MspDeInit(FDCAN_HandleTypeDef *hfdcan) { /* Prevent unused argument(s) compilation warning */ UNUSED(hfdcan); - /* NOTE : This function Should not be modified, when the callback is needed, + /* NOTE: This function Should not be modified, when the callback is needed, the HAL_FDCAN_MspDeInit could be implemented in the user file */ } @@ -1292,7 +1293,7 @@ HAL_StatusTypeDef HAL_FDCAN_UnRegisterErrorStatusCallback(FDCAN_HandleTypeDef *h * contains the filter configuration information * @retval HAL status */ -HAL_StatusTypeDef HAL_FDCAN_ConfigFilter(FDCAN_HandleTypeDef *hfdcan, FDCAN_FilterTypeDef *sFilterConfig) +HAL_StatusTypeDef HAL_FDCAN_ConfigFilter(FDCAN_HandleTypeDef *hfdcan, const FDCAN_FilterTypeDef *sFilterConfig) { uint32_t FilterElementW1; uint32_t FilterElementW2; @@ -1417,7 +1418,7 @@ HAL_StatusTypeDef HAL_FDCAN_ConfigGlobalFilter(FDCAN_HandleTypeDef *hfdcan, * @param hfdcan pointer to an FDCAN_HandleTypeDef structure that contains * the configuration information for the specified FDCAN. * @param Mask Extended ID Mask. - This parameter must be a number between 0 and 0x1FFFFFFF + * This parameter must be a number between 0 and 0x1FFFFFFF. * @retval HAL status */ HAL_StatusTypeDef HAL_FDCAN_ConfigExtendedIdMask(FDCAN_HandleTypeDef *hfdcan, uint32_t Mask) @@ -1607,7 +1608,7 @@ HAL_StatusTypeDef HAL_FDCAN_DisableTimestampCounter(FDCAN_HandleTypeDef *hfdcan) * the configuration information for the specified FDCAN. * @retval Timestamp counter value */ -uint16_t HAL_FDCAN_GetTimestampCounter(FDCAN_HandleTypeDef *hfdcan) +uint16_t HAL_FDCAN_GetTimestampCounter(const FDCAN_HandleTypeDef *hfdcan) { return (uint16_t)(hfdcan->Instance->TSCV); } @@ -1730,7 +1731,7 @@ HAL_StatusTypeDef HAL_FDCAN_DisableTimeoutCounter(FDCAN_HandleTypeDef *hfdcan) * the configuration information for the specified FDCAN. * @retval Timeout counter value */ -uint16_t HAL_FDCAN_GetTimeoutCounter(FDCAN_HandleTypeDef *hfdcan) +uint16_t HAL_FDCAN_GetTimeoutCounter(const FDCAN_HandleTypeDef *hfdcan) { return (uint16_t)(hfdcan->Instance->TOCV); } @@ -2103,8 +2104,8 @@ HAL_StatusTypeDef HAL_FDCAN_Stop(FDCAN_HandleTypeDef *hfdcan) * @param pTxData pointer to a buffer containing the payload of the Tx frame. * @retval HAL status */ -HAL_StatusTypeDef HAL_FDCAN_AddMessageToTxFifoQ(FDCAN_HandleTypeDef *hfdcan, FDCAN_TxHeaderTypeDef *pTxHeader, - uint8_t *pTxData) +HAL_StatusTypeDef HAL_FDCAN_AddMessageToTxFifoQ(FDCAN_HandleTypeDef *hfdcan, const FDCAN_TxHeaderTypeDef *pTxHeader, + const uint8_t *pTxData) { uint32_t PutIndex; @@ -2171,7 +2172,7 @@ HAL_StatusTypeDef HAL_FDCAN_AddMessageToTxFifoQ(FDCAN_HandleTypeDef *hfdcan, FDC * - Any value of @arg FDCAN_Tx_location if Tx request has been submitted. * - 0 if no Tx FIFO/Queue request have been submitted. */ -uint32_t HAL_FDCAN_GetLatestTxFifoQRequestBuffer(FDCAN_HandleTypeDef *hfdcan) +uint32_t HAL_FDCAN_GetLatestTxFifoQRequestBuffer(const FDCAN_HandleTypeDef *hfdcan) { /* Return Last Tx FIFO/Queue Request Buffer */ return hfdcan->LatestTxFifoQRequest; @@ -2223,7 +2224,7 @@ HAL_StatusTypeDef HAL_FDCAN_GetRxMessage(FDCAN_HandleTypeDef *hfdcan, uint32_t R uint32_t *RxAddress; uint8_t *pData; uint32_t ByteCounter; - uint32_t GetIndex; + uint32_t GetIndex = 0; HAL_FDCAN_StateTypeDef state = hfdcan->State; /* Check function parameters */ @@ -2243,8 +2244,20 @@ HAL_StatusTypeDef HAL_FDCAN_GetRxMessage(FDCAN_HandleTypeDef *hfdcan, uint32_t R } else { + /* Check that the Rx FIFO 0 is full & overwrite mode is on */ + if (((hfdcan->Instance->RXF0S & FDCAN_RXF0S_F0F) >> FDCAN_RXF0S_F0F_Pos) == 1U) + { + if (((hfdcan->Instance->RXGFC & FDCAN_RXGFC_F0OM) >> FDCAN_RXGFC_F0OM_Pos) == FDCAN_RX_FIFO_OVERWRITE) + { + /* When overwrite status is on discard first message in FIFO */ + GetIndex = 1U; + } + } + + /* Calculate Rx FIFO 0 element index */ + GetIndex += ((hfdcan->Instance->RXF0S & FDCAN_RXF0S_F0GI) >> FDCAN_RXF0S_F0GI_Pos); + /* Calculate Rx FIFO 0 element address */ - GetIndex = ((hfdcan->Instance->RXF0S & FDCAN_RXF0S_F0GI) >> FDCAN_RXF0S_F0GI_Pos); RxAddress = (uint32_t *)(hfdcan->msgRam.RxFIFO0SA + (GetIndex * SRAMCAN_RF0_SIZE)); } } @@ -2260,8 +2273,19 @@ HAL_StatusTypeDef HAL_FDCAN_GetRxMessage(FDCAN_HandleTypeDef *hfdcan, uint32_t R } else { + /* Check that the Rx FIFO 1 is full & overwrite mode is on */ + if (((hfdcan->Instance->RXF1S & FDCAN_RXF1S_F1F) >> FDCAN_RXF1S_F1F_Pos) == 1U) + { + if (((hfdcan->Instance->RXGFC & FDCAN_RXGFC_F1OM) >> FDCAN_RXGFC_F1OM_Pos) == FDCAN_RX_FIFO_OVERWRITE) + { + /* When overwrite status is on discard first message in FIFO */ + GetIndex = 1U; + } + } + + /* Calculate Rx FIFO 1 element index */ + GetIndex += ((hfdcan->Instance->RXF1S & FDCAN_RXF1S_F1GI) >> FDCAN_RXF1S_F1GI_Pos); /* Calculate Rx FIFO 1 element address */ - GetIndex = ((hfdcan->Instance->RXF1S & FDCAN_RXF1S_F1GI) >> FDCAN_RXF1S_F1GI_Pos); RxAddress = (uint32_t *)(hfdcan->msgRam.RxFIFO1SA + (GetIndex * SRAMCAN_RF1_SIZE)); } } @@ -2292,7 +2316,7 @@ HAL_StatusTypeDef HAL_FDCAN_GetRxMessage(FDCAN_HandleTypeDef *hfdcan, uint32_t R pRxHeader->RxTimestamp = (*RxAddress & FDCAN_ELEMENT_MASK_TS); /* Retrieve DataLength */ - pRxHeader->DataLength = (*RxAddress & FDCAN_ELEMENT_MASK_DLC); + pRxHeader->DataLength = ((*RxAddress & FDCAN_ELEMENT_MASK_DLC) >> 16U); /* Retrieve BitRateSwitch */ pRxHeader->BitRateSwitch = (*RxAddress & FDCAN_ELEMENT_MASK_BRS); @@ -2311,7 +2335,7 @@ HAL_StatusTypeDef HAL_FDCAN_GetRxMessage(FDCAN_HandleTypeDef *hfdcan, uint32_t R /* Retrieve Rx payload */ pData = (uint8_t *)RxAddress; - for (ByteCounter = 0; ByteCounter < DLCtoBytes[pRxHeader->DataLength >> 16U]; ByteCounter++) + for (ByteCounter = 0; ByteCounter < DLCtoBytes[pRxHeader->DataLength]; ByteCounter++) { pRxData[ByteCounter] = pData[ByteCounter]; } @@ -2393,7 +2417,7 @@ HAL_StatusTypeDef HAL_FDCAN_GetTxEvent(FDCAN_HandleTypeDef *hfdcan, FDCAN_TxEven pTxEvent->TxTimestamp = (*TxEventAddress & FDCAN_ELEMENT_MASK_TS); /* Retrieve DataLength */ - pTxEvent->DataLength = (*TxEventAddress & FDCAN_ELEMENT_MASK_DLC); + pTxEvent->DataLength = ((*TxEventAddress & FDCAN_ELEMENT_MASK_DLC) >> 16U); /* Retrieve BitRateSwitch */ pTxEvent->BitRateSwitch = (*TxEventAddress & FDCAN_ELEMENT_MASK_BRS); @@ -2429,7 +2453,7 @@ HAL_StatusTypeDef HAL_FDCAN_GetTxEvent(FDCAN_HandleTypeDef *hfdcan, FDCAN_TxEven * @param HpMsgStatus pointer to an FDCAN_HpMsgStatusTypeDef structure. * @retval HAL status */ -HAL_StatusTypeDef HAL_FDCAN_GetHighPriorityMessageStatus(FDCAN_HandleTypeDef *hfdcan, +HAL_StatusTypeDef HAL_FDCAN_GetHighPriorityMessageStatus(const FDCAN_HandleTypeDef *hfdcan, FDCAN_HpMsgStatusTypeDef *HpMsgStatus) { HpMsgStatus->FilterList = ((hfdcan->Instance->HPMS & FDCAN_HPMS_FLST) >> FDCAN_HPMS_FLST_Pos); @@ -2448,7 +2472,8 @@ HAL_StatusTypeDef HAL_FDCAN_GetHighPriorityMessageStatus(FDCAN_HandleTypeDef *hf * @param ProtocolStatus pointer to an FDCAN_ProtocolStatusTypeDef structure. * @retval HAL status */ -HAL_StatusTypeDef HAL_FDCAN_GetProtocolStatus(FDCAN_HandleTypeDef *hfdcan, FDCAN_ProtocolStatusTypeDef *ProtocolStatus) +HAL_StatusTypeDef HAL_FDCAN_GetProtocolStatus(const FDCAN_HandleTypeDef *hfdcan, + FDCAN_ProtocolStatusTypeDef *ProtocolStatus) { uint32_t StatusReg; @@ -2479,7 +2504,8 @@ HAL_StatusTypeDef HAL_FDCAN_GetProtocolStatus(FDCAN_HandleTypeDef *hfdcan, FDCAN * @param ErrorCounters pointer to an FDCAN_ErrorCountersTypeDef structure. * @retval HAL status */ -HAL_StatusTypeDef HAL_FDCAN_GetErrorCounters(FDCAN_HandleTypeDef *hfdcan, FDCAN_ErrorCountersTypeDef *ErrorCounters) +HAL_StatusTypeDef HAL_FDCAN_GetErrorCounters(const FDCAN_HandleTypeDef *hfdcan, + FDCAN_ErrorCountersTypeDef *ErrorCounters) { uint32_t CountersReg; @@ -2503,10 +2529,10 @@ HAL_StatusTypeDef HAL_FDCAN_GetErrorCounters(FDCAN_HandleTypeDef *hfdcan, FDCAN_ * @param TxBufferIndex Tx buffer index. * This parameter can be any combination of @arg FDCAN_Tx_location. * @retval Status - * - 0 : No pending transmission request on TxBufferIndex list + * - 0 : No pending transmission request on TxBufferIndex list. * - 1 : Pending transmission request on TxBufferIndex. */ -uint32_t HAL_FDCAN_IsTxBufferMessagePending(FDCAN_HandleTypeDef *hfdcan, uint32_t TxBufferIndex) +uint32_t HAL_FDCAN_IsTxBufferMessagePending(const FDCAN_HandleTypeDef *hfdcan, uint32_t TxBufferIndex) { /* Check function parameters */ assert_param(IS_FDCAN_TX_LOCATION_LIST(TxBufferIndex)); @@ -2529,7 +2555,7 @@ uint32_t HAL_FDCAN_IsTxBufferMessagePending(FDCAN_HandleTypeDef *hfdcan, uint32_ * @arg FDCAN_RX_FIFO1: Rx FIFO 1 * @retval Rx FIFO fill level. */ -uint32_t HAL_FDCAN_GetRxFifoFillLevel(FDCAN_HandleTypeDef *hfdcan, uint32_t RxFifo) +uint32_t HAL_FDCAN_GetRxFifoFillLevel(const FDCAN_HandleTypeDef *hfdcan, uint32_t RxFifo) { uint32_t FillLevel; @@ -2556,7 +2582,7 @@ uint32_t HAL_FDCAN_GetRxFifoFillLevel(FDCAN_HandleTypeDef *hfdcan, uint32_t RxFi * the configuration information for the specified FDCAN. * @retval Tx FIFO free level. */ -uint32_t HAL_FDCAN_GetTxFifoFreeLevel(FDCAN_HandleTypeDef *hfdcan) +uint32_t HAL_FDCAN_GetTxFifoFreeLevel(const FDCAN_HandleTypeDef *hfdcan) { uint32_t FreeLevel; @@ -2574,7 +2600,7 @@ uint32_t HAL_FDCAN_GetTxFifoFreeLevel(FDCAN_HandleTypeDef *hfdcan) * - 0 : Normal FDCAN operation. * - 1 : Restricted Operation Mode active. */ -uint32_t HAL_FDCAN_IsRestrictedOperationMode(FDCAN_HandleTypeDef *hfdcan) +uint32_t HAL_FDCAN_IsRestrictedOperationMode(const FDCAN_HandleTypeDef *hfdcan) { uint32_t OperationMode; @@ -2885,6 +2911,8 @@ void HAL_FDCAN_IRQHandler(FDCAN_HandleTypeDef *hfdcan) uint32_t ErrorStatusITs; uint32_t TransmittedBuffers; uint32_t AbortedBuffers; + uint32_t itsource; + uint32_t itflag; TxEventFifoITs = hfdcan->Instance->IR & FDCAN_TX_EVENT_FIFO_MASK; TxEventFifoITs &= hfdcan->Instance->IE; @@ -2896,11 +2924,13 @@ void HAL_FDCAN_IRQHandler(FDCAN_HandleTypeDef *hfdcan) Errors &= hfdcan->Instance->IE; ErrorStatusITs = hfdcan->Instance->IR & FDCAN_ERROR_STATUS_MASK; ErrorStatusITs &= hfdcan->Instance->IE; + itsource = hfdcan->Instance->IE; + itflag = hfdcan->Instance->IR; /* High Priority Message interrupt management *******************************/ - if (__HAL_FDCAN_GET_FLAG(hfdcan, FDCAN_FLAG_RX_HIGH_PRIORITY_MSG) != 0U) + if (FDCAN_CHECK_FLAG(itflag, FDCAN_FLAG_RX_HIGH_PRIORITY_MSG) != RESET) { - if (__HAL_FDCAN_GET_IT_SOURCE(hfdcan, FDCAN_IT_RX_HIGH_PRIORITY_MSG) != 0U) + if (FDCAN_CHECK_IT_SOURCE(itsource, FDCAN_IT_RX_HIGH_PRIORITY_MSG) != RESET) { /* Clear the High Priority Message flag */ __HAL_FDCAN_CLEAR_FLAG(hfdcan, FDCAN_FLAG_RX_HIGH_PRIORITY_MSG); @@ -2916,9 +2946,9 @@ void HAL_FDCAN_IRQHandler(FDCAN_HandleTypeDef *hfdcan) } /* Transmission Abort interrupt management **********************************/ - if (__HAL_FDCAN_GET_FLAG(hfdcan, FDCAN_FLAG_TX_ABORT_COMPLETE) != 0U) + if (FDCAN_CHECK_FLAG(itflag, FDCAN_FLAG_TX_ABORT_COMPLETE) != RESET) { - if (__HAL_FDCAN_GET_IT_SOURCE(hfdcan, FDCAN_IT_TX_ABORT_COMPLETE) != 0U) + if (FDCAN_CHECK_IT_SOURCE(itsource, FDCAN_IT_TX_ABORT_COMPLETE) != RESET) { /* List of aborted monitored buffers */ AbortedBuffers = hfdcan->Instance->TXBCF; @@ -2983,9 +3013,9 @@ void HAL_FDCAN_IRQHandler(FDCAN_HandleTypeDef *hfdcan) } /* Tx FIFO empty interrupt management ***************************************/ - if (__HAL_FDCAN_GET_FLAG(hfdcan, FDCAN_FLAG_TX_FIFO_EMPTY) != 0U) + if (FDCAN_CHECK_FLAG(itflag, FDCAN_FLAG_TX_FIFO_EMPTY) != RESET) { - if (__HAL_FDCAN_GET_IT_SOURCE(hfdcan, FDCAN_IT_TX_FIFO_EMPTY) != 0U) + if (FDCAN_CHECK_IT_SOURCE(itsource, FDCAN_IT_TX_FIFO_EMPTY) != RESET) { /* Clear the Tx FIFO empty flag */ __HAL_FDCAN_CLEAR_FLAG(hfdcan, FDCAN_FLAG_TX_FIFO_EMPTY); @@ -3001,9 +3031,9 @@ void HAL_FDCAN_IRQHandler(FDCAN_HandleTypeDef *hfdcan) } /* Transmission Complete interrupt management *******************************/ - if (__HAL_FDCAN_GET_FLAG(hfdcan, FDCAN_FLAG_TX_COMPLETE) != 0U) + if (FDCAN_CHECK_FLAG(itflag, FDCAN_FLAG_TX_COMPLETE) != RESET) { - if (__HAL_FDCAN_GET_IT_SOURCE(hfdcan, FDCAN_IT_TX_COMPLETE) != 0U) + if (FDCAN_CHECK_IT_SOURCE(itsource, FDCAN_IT_TX_COMPLETE) != RESET) { /* List of transmitted monitored buffers */ TransmittedBuffers = hfdcan->Instance->TXBTO; @@ -3023,9 +3053,9 @@ void HAL_FDCAN_IRQHandler(FDCAN_HandleTypeDef *hfdcan) } /* Timestamp Wraparound interrupt management ********************************/ - if (__HAL_FDCAN_GET_FLAG(hfdcan, FDCAN_FLAG_TIMESTAMP_WRAPAROUND) != 0U) + if (FDCAN_CHECK_FLAG(itflag, FDCAN_FLAG_TIMESTAMP_WRAPAROUND) != RESET) { - if (__HAL_FDCAN_GET_IT_SOURCE(hfdcan, FDCAN_IT_TIMESTAMP_WRAPAROUND) != 0U) + if (FDCAN_CHECK_IT_SOURCE(itsource, FDCAN_IT_TIMESTAMP_WRAPAROUND) != RESET) { /* Clear the Timestamp Wraparound flag */ __HAL_FDCAN_CLEAR_FLAG(hfdcan, FDCAN_FLAG_TIMESTAMP_WRAPAROUND); @@ -3041,9 +3071,9 @@ void HAL_FDCAN_IRQHandler(FDCAN_HandleTypeDef *hfdcan) } /* Timeout Occurred interrupt management ************************************/ - if (__HAL_FDCAN_GET_FLAG(hfdcan, FDCAN_FLAG_TIMEOUT_OCCURRED) != 0U) + if (FDCAN_CHECK_FLAG(itflag, FDCAN_FLAG_TIMEOUT_OCCURRED) != RESET) { - if (__HAL_FDCAN_GET_IT_SOURCE(hfdcan, FDCAN_IT_TIMEOUT_OCCURRED) != 0U) + if (FDCAN_CHECK_IT_SOURCE(itsource, FDCAN_IT_TIMEOUT_OCCURRED) != RESET) { /* Clear the Timeout Occurred flag */ __HAL_FDCAN_CLEAR_FLAG(hfdcan, FDCAN_FLAG_TIMEOUT_OCCURRED); @@ -3059,9 +3089,9 @@ void HAL_FDCAN_IRQHandler(FDCAN_HandleTypeDef *hfdcan) } /* Message RAM access failure interrupt management **************************/ - if (__HAL_FDCAN_GET_FLAG(hfdcan, FDCAN_FLAG_RAM_ACCESS_FAILURE) != 0U) + if (FDCAN_CHECK_FLAG(itflag, FDCAN_FLAG_RAM_ACCESS_FAILURE) != RESET) { - if (__HAL_FDCAN_GET_IT_SOURCE(hfdcan, FDCAN_IT_RAM_ACCESS_FAILURE) != 0U) + if (FDCAN_CHECK_IT_SOURCE(itsource, FDCAN_IT_RAM_ACCESS_FAILURE) != RESET) { /* Clear the Message RAM access failure flag */ __HAL_FDCAN_CLEAR_FLAG(hfdcan, FDCAN_FLAG_RAM_ACCESS_FAILURE); @@ -3141,7 +3171,7 @@ void HAL_FDCAN_IRQHandler(FDCAN_HandleTypeDef *hfdcan) * @brief Tx Event callback. * @param hfdcan pointer to an FDCAN_HandleTypeDef structure that contains * the configuration information for the specified FDCAN. - * @param TxEventFifoITs indicates which Tx Event FIFO interrupts are signalled. + * @param TxEventFifoITs indicates which Tx Event FIFO interrupts are signaled. * This parameter can be any combination of @arg FDCAN_Tx_Event_Fifo_Interrupts. * @retval None */ @@ -3151,7 +3181,7 @@ __weak void HAL_FDCAN_TxEventFifoCallback(FDCAN_HandleTypeDef *hfdcan, uint32_t UNUSED(hfdcan); UNUSED(TxEventFifoITs); - /* NOTE : This function Should not be modified, when the callback is needed, + /* NOTE: This function Should not be modified, when the callback is needed, the HAL_FDCAN_TxEventFifoCallback could be implemented in the user file */ } @@ -3160,7 +3190,7 @@ __weak void HAL_FDCAN_TxEventFifoCallback(FDCAN_HandleTypeDef *hfdcan, uint32_t * @brief Rx FIFO 0 callback. * @param hfdcan pointer to an FDCAN_HandleTypeDef structure that contains * the configuration information for the specified FDCAN. - * @param RxFifo0ITs indicates which Rx FIFO 0 interrupts are signalled. + * @param RxFifo0ITs indicates which Rx FIFO 0 interrupts are signaled. * This parameter can be any combination of @arg FDCAN_Rx_Fifo0_Interrupts. * @retval None */ @@ -3170,7 +3200,7 @@ __weak void HAL_FDCAN_RxFifo0Callback(FDCAN_HandleTypeDef *hfdcan, uint32_t RxFi UNUSED(hfdcan); UNUSED(RxFifo0ITs); - /* NOTE : This function Should not be modified, when the callback is needed, + /* NOTE: This function Should not be modified, when the callback is needed, the HAL_FDCAN_RxFifo0Callback could be implemented in the user file */ } @@ -3179,7 +3209,7 @@ __weak void HAL_FDCAN_RxFifo0Callback(FDCAN_HandleTypeDef *hfdcan, uint32_t RxFi * @brief Rx FIFO 1 callback. * @param hfdcan pointer to an FDCAN_HandleTypeDef structure that contains * the configuration information for the specified FDCAN. - * @param RxFifo1ITs indicates which Rx FIFO 1 interrupts are signalled. + * @param RxFifo1ITs indicates which Rx FIFO 1 interrupts are signaled. * This parameter can be any combination of @arg FDCAN_Rx_Fifo1_Interrupts. * @retval None */ @@ -3189,7 +3219,7 @@ __weak void HAL_FDCAN_RxFifo1Callback(FDCAN_HandleTypeDef *hfdcan, uint32_t RxFi UNUSED(hfdcan); UNUSED(RxFifo1ITs); - /* NOTE : This function Should not be modified, when the callback is needed, + /* NOTE: This function Should not be modified, when the callback is needed, the HAL_FDCAN_RxFifo1Callback could be implemented in the user file */ } @@ -3205,7 +3235,7 @@ __weak void HAL_FDCAN_TxFifoEmptyCallback(FDCAN_HandleTypeDef *hfdcan) /* Prevent unused argument(s) compilation warning */ UNUSED(hfdcan); - /* NOTE : This function Should not be modified, when the callback is needed, + /* NOTE: This function Should not be modified, when the callback is needed, the HAL_FDCAN_TxFifoEmptyCallback could be implemented in the user file */ } @@ -3224,7 +3254,7 @@ __weak void HAL_FDCAN_TxBufferCompleteCallback(FDCAN_HandleTypeDef *hfdcan, uint UNUSED(hfdcan); UNUSED(BufferIndexes); - /* NOTE : This function Should not be modified, when the callback is needed, + /* NOTE: This function Should not be modified, when the callback is needed, the HAL_FDCAN_TxBufferCompleteCallback could be implemented in the user file */ } @@ -3243,7 +3273,7 @@ __weak void HAL_FDCAN_TxBufferAbortCallback(FDCAN_HandleTypeDef *hfdcan, uint32_ UNUSED(hfdcan); UNUSED(BufferIndexes); - /* NOTE : This function Should not be modified, when the callback is needed, + /* NOTE: This function Should not be modified, when the callback is needed, the HAL_FDCAN_TxBufferAbortCallback could be implemented in the user file */ } @@ -3259,7 +3289,7 @@ __weak void HAL_FDCAN_TimestampWraparoundCallback(FDCAN_HandleTypeDef *hfdcan) /* Prevent unused argument(s) compilation warning */ UNUSED(hfdcan); - /* NOTE : This function Should not be modified, when the callback is needed, + /* NOTE: This function Should not be modified, when the callback is needed, the HAL_FDCAN_TimestampWraparoundCallback could be implemented in the user file */ } @@ -3275,7 +3305,7 @@ __weak void HAL_FDCAN_TimeoutOccurredCallback(FDCAN_HandleTypeDef *hfdcan) /* Prevent unused argument(s) compilation warning */ UNUSED(hfdcan); - /* NOTE : This function Should not be modified, when the callback is needed, + /* NOTE: This function Should not be modified, when the callback is needed, the HAL_FDCAN_TimeoutOccurredCallback could be implemented in the user file */ } @@ -3291,7 +3321,7 @@ __weak void HAL_FDCAN_HighPriorityMessageCallback(FDCAN_HandleTypeDef *hfdcan) /* Prevent unused argument(s) compilation warning */ UNUSED(hfdcan); - /* NOTE : This function Should not be modified, when the callback is needed, + /* NOTE: This function Should not be modified, when the callback is needed, the HAL_FDCAN_HighPriorityMessageCallback could be implemented in the user file */ } @@ -3307,7 +3337,7 @@ __weak void HAL_FDCAN_ErrorCallback(FDCAN_HandleTypeDef *hfdcan) /* Prevent unused argument(s) compilation warning */ UNUSED(hfdcan); - /* NOTE : This function Should not be modified, when the callback is needed, + /* NOTE: This function Should not be modified, when the callback is needed, the HAL_FDCAN_ErrorCallback could be implemented in the user file */ } @@ -3326,7 +3356,7 @@ __weak void HAL_FDCAN_ErrorStatusCallback(FDCAN_HandleTypeDef *hfdcan, uint32_t UNUSED(hfdcan); UNUSED(ErrorStatusITs); - /* NOTE : This function Should not be modified, when the callback is needed, + /* NOTE: This function Should not be modified, when the callback is needed, the HAL_FDCAN_ErrorStatusCallback could be implemented in the user file */ } @@ -3356,7 +3386,7 @@ __weak void HAL_FDCAN_ErrorStatusCallback(FDCAN_HandleTypeDef *hfdcan, uint32_t * the configuration information for the specified FDCAN. * @retval HAL state */ -HAL_FDCAN_StateTypeDef HAL_FDCAN_GetState(FDCAN_HandleTypeDef *hfdcan) +HAL_FDCAN_StateTypeDef HAL_FDCAN_GetState(const FDCAN_HandleTypeDef *hfdcan) { /* Return FDCAN state */ return hfdcan->State; @@ -3368,7 +3398,7 @@ HAL_FDCAN_StateTypeDef HAL_FDCAN_GetState(FDCAN_HandleTypeDef *hfdcan) * the configuration information for the specified FDCAN. * @retval FDCAN Error Code */ -uint32_t HAL_FDCAN_GetError(FDCAN_HandleTypeDef *hfdcan) +uint32_t HAL_FDCAN_GetError(const FDCAN_HandleTypeDef *hfdcan) { /* Return FDCAN error code */ return hfdcan->ErrorCode; @@ -3450,8 +3480,8 @@ static void FDCAN_CalcultateRamBlockAddresses(FDCAN_HandleTypeDef *hfdcan) * @param BufferIndex index of the buffer to be configured. * @retval none */ -static void FDCAN_CopyMessageToRAM(FDCAN_HandleTypeDef *hfdcan, FDCAN_TxHeaderTypeDef *pTxHeader, uint8_t *pTxData, - uint32_t BufferIndex) +static void FDCAN_CopyMessageToRAM(const FDCAN_HandleTypeDef *hfdcan, const FDCAN_TxHeaderTypeDef *pTxHeader, + const uint8_t *pTxData, uint32_t BufferIndex) { uint32_t TxElementW1; uint32_t TxElementW2; @@ -3479,7 +3509,7 @@ static void FDCAN_CopyMessageToRAM(FDCAN_HandleTypeDef *hfdcan, FDCAN_TxHeaderTy pTxHeader->TxEventFifoControl | pTxHeader->FDFormat | pTxHeader->BitRateSwitch | - pTxHeader->DataLength); + (pTxHeader->DataLength << 16U)); /* Calculate Tx element address */ TxAddress = (uint32_t *)(hfdcan->msgRam.TxFIFOQSA + (BufferIndex * SRAMCAN_TFQ_SIZE)); @@ -3491,7 +3521,7 @@ static void FDCAN_CopyMessageToRAM(FDCAN_HandleTypeDef *hfdcan, FDCAN_TxHeaderTy TxAddress++; /* Write Tx payload to the message RAM */ - for (ByteCounter = 0; ByteCounter < DLCtoBytes[pTxHeader->DataLength >> 16U]; ByteCounter += 4U) + for (ByteCounter = 0; ByteCounter < DLCtoBytes[pTxHeader->DataLength]; ByteCounter += 4U) { *TxAddress = (((uint32_t)pTxData[ByteCounter + 3U] << 24U) | ((uint32_t)pTxData[ByteCounter + 2U] << 16U) | diff --git a/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_flash.c b/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_flash.c index 9139817e2..d2de5ed99 100644 --- a/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_flash.c +++ b/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_flash.c @@ -188,6 +188,17 @@ HAL_StatusTypeDef HAL_FLASH_Program(uint32_t TypeProgram, uint32_t Address, uint { pFlash.ErrorCode = HAL_FLASH_ERROR_NONE; + /* Deactivate the data cache if they are activated to avoid data misbehavior */ + if(READ_BIT(FLASH->ACR, FLASH_ACR_DCEN) != 0U) + { + /* Disable data cache */ + __HAL_FLASH_DATA_CACHE_DISABLE(); + pFlash.CacheToReactivate = FLASH_CACHE_DCACHE_ENABLED; + } + else + { + pFlash.CacheToReactivate = FLASH_CACHE_DISABLED; + } if (TypeProgram == FLASH_TYPEPROGRAM_DOUBLEWORD) { /* Program double-word (64-bit) at a specified address */ @@ -218,6 +229,9 @@ HAL_StatusTypeDef HAL_FLASH_Program(uint32_t TypeProgram, uint32_t Address, uint { CLEAR_BIT(FLASH->CR, prog_bit); } + + /* Flush the caches to be sure of the data consistency */ + FLASH_FlushCaches(); } /* Process Unlocked */ @@ -251,6 +265,18 @@ HAL_StatusTypeDef HAL_FLASH_Program_IT(uint32_t TypeProgram, uint32_t Address, u /* Reset error code */ pFlash.ErrorCode = HAL_FLASH_ERROR_NONE; + /* Deactivate the data cache if they are activated to avoid data misbehavior */ + if(READ_BIT(FLASH->ACR, FLASH_ACR_DCEN) != 0U) + { + /* Disable data cache */ + __HAL_FLASH_DATA_CACHE_DISABLE(); + pFlash.CacheToReactivate = FLASH_CACHE_DCACHE_ENABLED; + } + else + { + pFlash.CacheToReactivate = FLASH_CACHE_DISABLED; + } + /* Wait for last operation to be completed */ status = FLASH_WaitForLastOperation(FLASH_TIMEOUT_VALUE); @@ -328,7 +354,7 @@ void HAL_FLASH_IRQHandler(void) __HAL_FLASH_CLEAR_FLAG(error); /* Flush the caches to be sure of the data consistency */ - FLASH_FlushCaches() ; + FLASH_FlushCaches(); /* FLASH error interrupt user callback */ procedure = pFlash.ProcedureOnGoing; @@ -384,7 +410,7 @@ void HAL_FLASH_IRQHandler(void) pFlash.ProcedureOnGoing = FLASH_PROC_NONE; /* Flush the caches to be sure of the data consistency */ - FLASH_FlushCaches() ; + FLASH_FlushCaches(); /* FLASH EOP interrupt user callback */ HAL_FLASH_EndOfOperationCallback(pFlash.Page); @@ -393,7 +419,7 @@ void HAL_FLASH_IRQHandler(void) else { /* Flush the caches to be sure of the data consistency */ - FLASH_FlushCaches() ; + FLASH_FlushCaches(); procedure = pFlash.ProcedureOnGoing; if (procedure == FLASH_PROC_MASS_ERASE) diff --git a/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_flash_ex.c b/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_flash_ex.c index fd64b1083..6da8e9f74 100644 --- a/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_flash_ex.c +++ b/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_flash_ex.c @@ -477,10 +477,14 @@ HAL_StatusTypeDef HAL_FLASHEx_EnableSecMemProtection(uint32_t Bank) } } else -#endif { SET_BIT(FLASH->CR, FLASH_CR_SEC_PROT1); } +#else + /* Prevent unused argument(s) compilation warning */ + UNUSED(Bank); + SET_BIT(FLASH->CR, FLASH_CR_SEC_PROT1); +#endif /* FLASH_OPTR_DBANK */ return HAL_OK; } @@ -598,7 +602,10 @@ void FLASH_PageErase(uint32_t Page, uint32_t Banks) SET_BIT(FLASH->CR, FLASH_CR_BKER); } } -#endif +#else + /* Prevent unused argument(s) compilation warning */ + UNUSED(Banks); +#endif /* FLASH_OPTR_DBANK */ /* Proceed to erase the page */ MODIFY_REG(FLASH->CR, FLASH_CR_PNB, ((Page & 0xFFU) << FLASH_CR_PNB_Pos)); @@ -790,6 +797,18 @@ static HAL_StatusTypeDef FLASH_OB_UserConfig(uint32_t UserType, uint32_t UserCon if (status == HAL_OK) { +#if defined(FLASH_OPTR_PB4_PUPEN) + if ((UserType & OB_USER_PB4_PUPEN) != 0U) + { + /* PB4_PUPEN option byte should be modified */ + assert_param(IS_OB_USER_PB4_PUPEN(UserConfig & FLASH_OPTR_PB4_PUPEN)); + + /* Set value and mask for PB4_PUPEN option byte */ + optr_reg_val |= (UserConfig & FLASH_OPTR_PB4_PUPEN); + optr_reg_mask |= FLASH_OPTR_PB4_PUPEN; + } +#endif /* FLASH_OPTR_PB4_PUPEN */ + if ((UserType & OB_USER_BOR_LEV) != 0U) { /* BOR level option byte should be modified */ diff --git a/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_i2c.c b/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_i2c.c index 54c8246c6..3cbb94982 100644 --- a/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_i2c.c +++ b/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_i2c.c @@ -400,9 +400,15 @@ * @} */ -/* Private macro -------------------------------------------------------------*/ +/* Private macros ------------------------------------------------------------*/ +/** @addtogroup I2C_Private_Macro + * @{ + */ /* Macro to get remaining data to transfer on DMA side */ #define I2C_GET_DMA_REMAIN_DATA(__HANDLE__) __HAL_DMA_GET_COUNTER(__HANDLE__) +/** + * @} + */ /* Private variables ---------------------------------------------------------*/ /* Private function prototypes -----------------------------------------------*/ @@ -418,6 +424,7 @@ static void I2C_DMASlaveReceiveCplt(DMA_HandleTypeDef *hdma); static void I2C_DMAError(DMA_HandleTypeDef *hdma); static void I2C_DMAAbort(DMA_HandleTypeDef *hdma); + /* Private functions to handle IT transfer */ static void I2C_ITAddrCplt(I2C_HandleTypeDef *hi2c, uint32_t ITFlags); static void I2C_ITMasterSeqCplt(I2C_HandleTypeDef *hi2c); @@ -438,10 +445,14 @@ static HAL_StatusTypeDef I2C_RequestMemoryRead(I2C_HandleTypeDef *hi2c, uint16_t /* Private functions for I2C transfer IRQ handler */ static HAL_StatusTypeDef I2C_Master_ISR_IT(struct __I2C_HandleTypeDef *hi2c, uint32_t ITFlags, uint32_t ITSources); +static HAL_StatusTypeDef I2C_Mem_ISR_IT(struct __I2C_HandleTypeDef *hi2c, uint32_t ITFlags, + uint32_t ITSources); static HAL_StatusTypeDef I2C_Slave_ISR_IT(struct __I2C_HandleTypeDef *hi2c, uint32_t ITFlags, uint32_t ITSources); static HAL_StatusTypeDef I2C_Master_ISR_DMA(struct __I2C_HandleTypeDef *hi2c, uint32_t ITFlags, uint32_t ITSources); +static HAL_StatusTypeDef I2C_Mem_ISR_DMA(struct __I2C_HandleTypeDef *hi2c, uint32_t ITFlags, + uint32_t ITSources); static HAL_StatusTypeDef I2C_Slave_ISR_DMA(struct __I2C_HandleTypeDef *hi2c, uint32_t ITFlags, uint32_t ITSources); @@ -597,7 +608,12 @@ HAL_StatusTypeDef HAL_I2C_Init(I2C_HandleTypeDef *hi2c) /* Configure I2Cx: Addressing Master mode */ if (hi2c->Init.AddressingMode == I2C_ADDRESSINGMODE_10BIT) { - hi2c->Instance->CR2 = (I2C_CR2_ADD10); + SET_BIT(hi2c->Instance->CR2, I2C_CR2_ADD10); + } + else + { + /* Clear the I2C ADD10 bit */ + CLEAR_BIT(hi2c->Instance->CR2, I2C_CR2_ADD10); } /* Enable the AUTOEND by default, and enable NACK (should be disable only during Slave process */ hi2c->Instance->CR2 |= (I2C_CR2_AUTOEND | I2C_CR2_NACK); @@ -707,6 +723,8 @@ __weak void HAL_I2C_MspDeInit(I2C_HandleTypeDef *hi2c) /** * @brief Register a User I2C Callback * To be used instead of the weak predefined callback + * @note The HAL_I2C_RegisterCallback() may be called before HAL_I2C_Init() in HAL_I2C_STATE_RESET + * to register callbacks for HAL_I2C_MSPINIT_CB_ID and HAL_I2C_MSPDEINIT_CB_ID. * @param hi2c Pointer to a I2C_HandleTypeDef structure that contains * the configuration information for the specified I2C. * @param CallbackID ID of the callback to be registered @@ -737,8 +755,6 @@ HAL_StatusTypeDef HAL_I2C_RegisterCallback(I2C_HandleTypeDef *hi2c, HAL_I2C_Call return HAL_ERROR; } - /* Process locked */ - __HAL_LOCK(hi2c); if (HAL_I2C_STATE_READY == hi2c->State) { @@ -827,14 +843,14 @@ HAL_StatusTypeDef HAL_I2C_RegisterCallback(I2C_HandleTypeDef *hi2c, HAL_I2C_Call status = HAL_ERROR; } - /* Release Lock */ - __HAL_UNLOCK(hi2c); return status; } /** * @brief Unregister an I2C Callback * I2C callback is redirected to the weak predefined callback + * @note The HAL_I2C_UnRegisterCallback() may be called before HAL_I2C_Init() in HAL_I2C_STATE_RESET + * to un-register callbacks for HAL_I2C_MSPINIT_CB_ID and HAL_I2C_MSPDEINIT_CB_ID. * @param hi2c Pointer to a I2C_HandleTypeDef structure that contains * the configuration information for the specified I2C. * @param CallbackID ID of the callback to be unregistered @@ -857,9 +873,6 @@ HAL_StatusTypeDef HAL_I2C_UnRegisterCallback(I2C_HandleTypeDef *hi2c, HAL_I2C_Ca { HAL_StatusTypeDef status = HAL_OK; - /* Process locked */ - __HAL_LOCK(hi2c); - if (HAL_I2C_STATE_READY == hi2c->State) { switch (CallbackID) @@ -947,8 +960,6 @@ HAL_StatusTypeDef HAL_I2C_UnRegisterCallback(I2C_HandleTypeDef *hi2c, HAL_I2C_Ca status = HAL_ERROR; } - /* Release Lock */ - __HAL_UNLOCK(hi2c); return status; } @@ -971,8 +982,6 @@ HAL_StatusTypeDef HAL_I2C_RegisterAddrCallback(I2C_HandleTypeDef *hi2c, pI2C_Add return HAL_ERROR; } - /* Process locked */ - __HAL_LOCK(hi2c); if (HAL_I2C_STATE_READY == hi2c->State) { @@ -987,8 +996,6 @@ HAL_StatusTypeDef HAL_I2C_RegisterAddrCallback(I2C_HandleTypeDef *hi2c, pI2C_Add status = HAL_ERROR; } - /* Release Lock */ - __HAL_UNLOCK(hi2c); return status; } @@ -1003,9 +1010,6 @@ HAL_StatusTypeDef HAL_I2C_UnRegisterAddrCallback(I2C_HandleTypeDef *hi2c) { HAL_StatusTypeDef status = HAL_OK; - /* Process locked */ - __HAL_LOCK(hi2c); - if (HAL_I2C_STATE_READY == hi2c->State) { hi2c->AddrCallback = HAL_I2C_AddrCallback; /* Legacy weak AddrCallback */ @@ -1019,8 +1023,6 @@ HAL_StatusTypeDef HAL_I2C_UnRegisterAddrCallback(I2C_HandleTypeDef *hi2c) status = HAL_ERROR; } - /* Release Lock */ - __HAL_UNLOCK(hi2c); return status; } @@ -1118,6 +1120,7 @@ HAL_StatusTypeDef HAL_I2C_Master_Transmit(I2C_HandleTypeDef *hi2c, uint16_t DevA uint16_t Size, uint32_t Timeout) { uint32_t tickstart; + uint32_t xfermode; if (hi2c->State == HAL_I2C_STATE_READY) { @@ -1141,18 +1144,39 @@ HAL_StatusTypeDef HAL_I2C_Master_Transmit(I2C_HandleTypeDef *hi2c, uint16_t DevA hi2c->XferCount = Size; hi2c->XferISR = NULL; - /* Send Slave Address */ - /* Set NBYTES to write and reload if hi2c->XferCount > MAX_NBYTE_SIZE and generate RESTART */ if (hi2c->XferCount > MAX_NBYTE_SIZE) { hi2c->XferSize = MAX_NBYTE_SIZE; - I2C_TransferConfig(hi2c, DevAddress, (uint8_t)hi2c->XferSize, I2C_RELOAD_MODE, - I2C_GENERATE_START_WRITE); + xfermode = I2C_RELOAD_MODE; } else { hi2c->XferSize = hi2c->XferCount; - I2C_TransferConfig(hi2c, DevAddress, (uint8_t)hi2c->XferSize, I2C_AUTOEND_MODE, + xfermode = I2C_AUTOEND_MODE; + } + + if (hi2c->XferSize > 0U) + { + /* Preload TX register */ + /* Write data to TXDR */ + hi2c->Instance->TXDR = *hi2c->pBuffPtr; + + /* Increment Buffer pointer */ + hi2c->pBuffPtr++; + + hi2c->XferCount--; + hi2c->XferSize--; + + /* Send Slave Address */ + /* Set NBYTES to write and reload if hi2c->XferCount > MAX_NBYTE_SIZE and generate RESTART */ + I2C_TransferConfig(hi2c, DevAddress, (uint8_t)(hi2c->XferSize + 1U), xfermode, + I2C_GENERATE_START_WRITE); + } + else + { + /* Send Slave Address */ + /* Set NBYTES to write and reload if hi2c->XferCount > MAX_NBYTE_SIZE and generate RESTART */ + I2C_TransferConfig(hi2c, DevAddress, (uint8_t)hi2c->XferSize, xfermode, I2C_GENERATE_START_WRITE); } @@ -1355,6 +1379,8 @@ HAL_StatusTypeDef HAL_I2C_Slave_Transmit(I2C_HandleTypeDef *hi2c, uint8_t *pData uint32_t Timeout) { uint32_t tickstart; + uint16_t tmpXferCount; + HAL_StatusTypeDef error; if (hi2c->State == HAL_I2C_STATE_READY) { @@ -1389,6 +1415,19 @@ HAL_StatusTypeDef HAL_I2C_Slave_Transmit(I2C_HandleTypeDef *hi2c, uint8_t *pData return HAL_ERROR; } + /* Preload TX data if no stretch enable */ + if (hi2c->Init.NoStretchMode == I2C_NOSTRETCH_ENABLE) + { + /* Preload TX register */ + /* Write data to TXDR */ + hi2c->Instance->TXDR = *hi2c->pBuffPtr; + + /* Increment Buffer pointer */ + hi2c->pBuffPtr++; + + hi2c->XferCount--; + } + /* Clear ADDR flag */ __HAL_I2C_CLEAR_FLAG(hi2c, I2C_FLAG_ADDR); @@ -1435,31 +1474,48 @@ HAL_StatusTypeDef HAL_I2C_Slave_Transmit(I2C_HandleTypeDef *hi2c, uint8_t *pData } /* Wait until AF flag is set */ - if (I2C_WaitOnFlagUntilTimeout(hi2c, I2C_FLAG_AF, RESET, Timeout, tickstart) != HAL_OK) + error = I2C_WaitOnFlagUntilTimeout(hi2c, I2C_FLAG_AF, RESET, Timeout, tickstart); + + if (error != HAL_OK) { - /* Disable Address Acknowledge */ - hi2c->Instance->CR2 |= I2C_CR2_NACK; - return HAL_ERROR; + /* Check that I2C transfer finished */ + /* if yes, normal use case, a NACK is sent by the MASTER when Transfer is finished */ + /* Mean XferCount == 0 */ + + tmpXferCount = hi2c->XferCount; + if ((hi2c->ErrorCode == HAL_I2C_ERROR_AF) && (tmpXferCount == 0U)) + { + /* Reset ErrorCode to NONE */ + hi2c->ErrorCode = HAL_I2C_ERROR_NONE; + } + else + { + /* Disable Address Acknowledge */ + hi2c->Instance->CR2 |= I2C_CR2_NACK; + return HAL_ERROR; + } } + else + { + /* Flush TX register */ + I2C_Flush_TXDR(hi2c); - /* Flush TX register */ - I2C_Flush_TXDR(hi2c); + /* Clear AF flag */ + __HAL_I2C_CLEAR_FLAG(hi2c, I2C_FLAG_AF); - /* Clear AF flag */ - __HAL_I2C_CLEAR_FLAG(hi2c, I2C_FLAG_AF); + /* Wait until STOP flag is set */ + if (I2C_WaitOnSTOPFlagUntilTimeout(hi2c, Timeout, tickstart) != HAL_OK) + { + /* Disable Address Acknowledge */ + hi2c->Instance->CR2 |= I2C_CR2_NACK; - /* Wait until STOP flag is set */ - if (I2C_WaitOnSTOPFlagUntilTimeout(hi2c, Timeout, tickstart) != HAL_OK) - { - /* Disable Address Acknowledge */ - hi2c->Instance->CR2 |= I2C_CR2_NACK; + return HAL_ERROR; + } - return HAL_ERROR; + /* Clear STOP flag */ + __HAL_I2C_CLEAR_FLAG(hi2c, I2C_FLAG_STOPF); } - /* Clear STOP flag */ - __HAL_I2C_CLEAR_FLAG(hi2c, I2C_FLAG_STOPF); - /* Wait until BUSY flag is reset */ if (I2C_WaitOnFlagUntilTimeout(hi2c, I2C_FLAG_BUSY, SET, Timeout, tickstart) != HAL_OK) { @@ -1662,7 +1718,26 @@ HAL_StatusTypeDef HAL_I2C_Master_Transmit_IT(I2C_HandleTypeDef *hi2c, uint16_t D /* Send Slave Address */ /* Set NBYTES to write and reload if hi2c->XferCount > MAX_NBYTE_SIZE */ - I2C_TransferConfig(hi2c, DevAddress, (uint8_t)hi2c->XferSize, xfermode, I2C_GENERATE_START_WRITE); + if (hi2c->XferSize > 0U) + { + /* Preload TX register */ + /* Write data to TXDR */ + hi2c->Instance->TXDR = *hi2c->pBuffPtr; + + /* Increment Buffer pointer */ + hi2c->pBuffPtr++; + + hi2c->XferCount--; + hi2c->XferSize--; + + I2C_TransferConfig(hi2c, DevAddress, (uint8_t)(hi2c->XferSize + 1U), xfermode, + I2C_GENERATE_START_WRITE); + } + else + { + I2C_TransferConfig(hi2c, DevAddress, (uint8_t)hi2c->XferSize, xfermode, + I2C_GENERATE_START_WRITE); + } /* Process Unlocked */ __HAL_UNLOCK(hi2c); @@ -1785,6 +1860,20 @@ HAL_StatusTypeDef HAL_I2C_Slave_Transmit_IT(I2C_HandleTypeDef *hi2c, uint8_t *pD hi2c->XferOptions = I2C_NO_OPTION_FRAME; hi2c->XferISR = I2C_Slave_ISR_IT; + /* Preload TX data if no stretch enable */ + if (hi2c->Init.NoStretchMode == I2C_NOSTRETCH_ENABLE) + { + /* Preload TX register */ + /* Write data to TXDR */ + hi2c->Instance->TXDR = *hi2c->pBuffPtr; + + /* Increment Buffer pointer */ + hi2c->pBuffPtr++; + + hi2c->XferCount--; + hi2c->XferSize--; + } + /* Process Unlocked */ __HAL_UNLOCK(hi2c); @@ -1871,6 +1960,7 @@ HAL_StatusTypeDef HAL_I2C_Master_Transmit_DMA(I2C_HandleTypeDef *hi2c, uint16_t { uint32_t xfermode; HAL_StatusTypeDef dmaxferstatus; + uint32_t sizetoxfer = 0U; if (hi2c->State == HAL_I2C_STATE_READY) { @@ -1903,6 +1993,20 @@ HAL_StatusTypeDef HAL_I2C_Master_Transmit_DMA(I2C_HandleTypeDef *hi2c, uint16_t xfermode = I2C_AUTOEND_MODE; } + if (hi2c->XferSize > 0U) + { + /* Preload TX register */ + /* Write data to TXDR */ + hi2c->Instance->TXDR = *hi2c->pBuffPtr; + + /* Increment Buffer pointer */ + hi2c->pBuffPtr++; + + sizetoxfer = hi2c->XferSize; + hi2c->XferCount--; + hi2c->XferSize--; + } + if (hi2c->XferSize > 0U) { if (hi2c->hdmatx != NULL) @@ -1918,8 +2022,8 @@ HAL_StatusTypeDef HAL_I2C_Master_Transmit_DMA(I2C_HandleTypeDef *hi2c, uint16_t hi2c->hdmatx->XferAbortCallback = NULL; /* Enable the DMA channel */ - dmaxferstatus = HAL_DMA_Start_IT(hi2c->hdmatx, (uint32_t)pData, (uint32_t)&hi2c->Instance->TXDR, - hi2c->XferSize); + dmaxferstatus = HAL_DMA_Start_IT(hi2c->hdmatx, (uint32_t)hi2c->pBuffPtr, + (uint32_t)&hi2c->Instance->TXDR, hi2c->XferSize); } else { @@ -1940,7 +2044,8 @@ HAL_StatusTypeDef HAL_I2C_Master_Transmit_DMA(I2C_HandleTypeDef *hi2c, uint16_t { /* Send Slave Address */ /* Set NBYTES to write and reload if hi2c->XferCount > MAX_NBYTE_SIZE and generate RESTART */ - I2C_TransferConfig(hi2c, DevAddress, (uint8_t)hi2c->XferSize, xfermode, I2C_GENERATE_START_WRITE); + I2C_TransferConfig(hi2c, DevAddress, (uint8_t)(hi2c->XferSize + 1U), + xfermode, I2C_GENERATE_START_WRITE); /* Update XferCount value */ hi2c->XferCount -= hi2c->XferSize; @@ -1979,7 +2084,7 @@ HAL_StatusTypeDef HAL_I2C_Master_Transmit_DMA(I2C_HandleTypeDef *hi2c, uint16_t /* Send Slave Address */ /* Set NBYTES to write and generate START condition */ - I2C_TransferConfig(hi2c, DevAddress, (uint8_t)hi2c->XferSize, I2C_AUTOEND_MODE, + I2C_TransferConfig(hi2c, DevAddress, (uint8_t)sizetoxfer, I2C_AUTOEND_MODE, I2C_GENERATE_START_WRITE); /* Process Unlocked */ @@ -2135,11 +2240,11 @@ HAL_StatusTypeDef HAL_I2C_Master_Receive_DMA(I2C_HandleTypeDef *hi2c, uint16_t D /* Note : The I2C interrupts must be enabled after unlocking current process to avoid the risk of I2C interrupt handle execution before current process unlock */ - /* Enable ERR, TC, STOP, NACK, TXI interrupt */ + /* Enable ERR, TC, STOP, NACK, RXI interrupt */ /* possible to enable all of these */ /* I2C_IT_ERRI | I2C_IT_TCI | I2C_IT_STOPI | I2C_IT_NACKI | I2C_IT_ADDRI | I2C_IT_RXI | I2C_IT_TXI */ - I2C_Enable_IRQ(hi2c, I2C_XFER_TX_IT); + I2C_Enable_IRQ(hi2c, I2C_XFER_RX_IT); } return HAL_OK; @@ -2183,67 +2288,99 @@ HAL_StatusTypeDef HAL_I2C_Slave_Transmit_DMA(I2C_HandleTypeDef *hi2c, uint8_t *p hi2c->XferOptions = I2C_NO_OPTION_FRAME; hi2c->XferISR = I2C_Slave_ISR_DMA; - if (hi2c->hdmatx != NULL) + /* Preload TX data if no stretch enable */ + if (hi2c->Init.NoStretchMode == I2C_NOSTRETCH_ENABLE) { - /* Set the I2C DMA transfer complete callback */ - hi2c->hdmatx->XferCpltCallback = I2C_DMASlaveTransmitCplt; - - /* Set the DMA error callback */ - hi2c->hdmatx->XferErrorCallback = I2C_DMAError; + /* Preload TX register */ + /* Write data to TXDR */ + hi2c->Instance->TXDR = *hi2c->pBuffPtr; - /* Set the unused DMA callbacks to NULL */ - hi2c->hdmatx->XferHalfCpltCallback = NULL; - hi2c->hdmatx->XferAbortCallback = NULL; + /* Increment Buffer pointer */ + hi2c->pBuffPtr++; - /* Enable the DMA channel */ - dmaxferstatus = HAL_DMA_Start_IT(hi2c->hdmatx, (uint32_t)pData, (uint32_t)&hi2c->Instance->TXDR, - hi2c->XferSize); + hi2c->XferCount--; + hi2c->XferSize--; } - else + + if (hi2c->XferCount != 0U) { - /* Update I2C state */ - hi2c->State = HAL_I2C_STATE_LISTEN; - hi2c->Mode = HAL_I2C_MODE_NONE; + if (hi2c->hdmatx != NULL) + { + /* Set the I2C DMA transfer complete callback */ + hi2c->hdmatx->XferCpltCallback = I2C_DMASlaveTransmitCplt; - /* Update I2C error code */ - hi2c->ErrorCode |= HAL_I2C_ERROR_DMA_PARAM; + /* Set the DMA error callback */ + hi2c->hdmatx->XferErrorCallback = I2C_DMAError; - /* Process Unlocked */ - __HAL_UNLOCK(hi2c); + /* Set the unused DMA callbacks to NULL */ + hi2c->hdmatx->XferHalfCpltCallback = NULL; + hi2c->hdmatx->XferAbortCallback = NULL; - return HAL_ERROR; - } + /* Enable the DMA channel */ + dmaxferstatus = HAL_DMA_Start_IT(hi2c->hdmatx, + (uint32_t)hi2c->pBuffPtr, (uint32_t)&hi2c->Instance->TXDR, + hi2c->XferSize); + } + else + { + /* Update I2C state */ + hi2c->State = HAL_I2C_STATE_LISTEN; + hi2c->Mode = HAL_I2C_MODE_NONE; - if (dmaxferstatus == HAL_OK) - { - /* Enable Address Acknowledge */ - hi2c->Instance->CR2 &= ~I2C_CR2_NACK; + /* Update I2C error code */ + hi2c->ErrorCode |= HAL_I2C_ERROR_DMA_PARAM; - /* Process Unlocked */ - __HAL_UNLOCK(hi2c); + /* Process Unlocked */ + __HAL_UNLOCK(hi2c); - /* Note : The I2C interrupts must be enabled after unlocking current process - to avoid the risk of I2C interrupt handle execution before current - process unlock */ - /* Enable ERR, STOP, NACK, ADDR interrupts */ - I2C_Enable_IRQ(hi2c, I2C_XFER_LISTEN_IT); + return HAL_ERROR; + } - /* Enable DMA Request */ - hi2c->Instance->CR1 |= I2C_CR1_TXDMAEN; + if (dmaxferstatus == HAL_OK) + { + /* Enable Address Acknowledge */ + hi2c->Instance->CR2 &= ~I2C_CR2_NACK; + + /* Process Unlocked */ + __HAL_UNLOCK(hi2c); + + /* Note : The I2C interrupts must be enabled after unlocking current process + to avoid the risk of I2C interrupt handle execution before current + process unlock */ + /* Enable ERR, STOP, NACK, ADDR interrupts */ + I2C_Enable_IRQ(hi2c, I2C_XFER_LISTEN_IT); + + /* Enable DMA Request */ + hi2c->Instance->CR1 |= I2C_CR1_TXDMAEN; + } + else + { + /* Update I2C state */ + hi2c->State = HAL_I2C_STATE_LISTEN; + hi2c->Mode = HAL_I2C_MODE_NONE; + + /* Update I2C error code */ + hi2c->ErrorCode |= HAL_I2C_ERROR_DMA; + + /* Process Unlocked */ + __HAL_UNLOCK(hi2c); + + return HAL_ERROR; + } } else { - /* Update I2C state */ - hi2c->State = HAL_I2C_STATE_LISTEN; - hi2c->Mode = HAL_I2C_MODE_NONE; - - /* Update I2C error code */ - hi2c->ErrorCode |= HAL_I2C_ERROR_DMA; + /* Enable Address Acknowledge */ + hi2c->Instance->CR2 &= ~I2C_CR2_NACK; /* Process Unlocked */ __HAL_UNLOCK(hi2c); - return HAL_ERROR; + /* Note : The I2C interrupts must be enabled after unlocking current process + to avoid the risk of I2C interrupt handle execution before current + process unlock */ + /* Enable ERR, STOP, NACK, ADDR interrupts */ + I2C_Enable_IRQ(hi2c, I2C_XFER_LISTEN_IT); } return HAL_OK; @@ -2357,6 +2494,7 @@ HAL_StatusTypeDef HAL_I2C_Slave_Receive_DMA(I2C_HandleTypeDef *hi2c, uint8_t *pD return HAL_BUSY; } } + /** * @brief Write an amount of data in blocking mode to a specific memory address * @param hi2c Pointer to a I2C_HandleTypeDef structure that contains @@ -2647,9 +2785,6 @@ HAL_StatusTypeDef HAL_I2C_Mem_Read(I2C_HandleTypeDef *hi2c, uint16_t DevAddress, HAL_StatusTypeDef HAL_I2C_Mem_Write_IT(I2C_HandleTypeDef *hi2c, uint16_t DevAddress, uint16_t MemAddress, uint16_t MemAddSize, uint8_t *pData, uint16_t Size) { - uint32_t tickstart; - uint32_t xfermode; - /* Check the parameters */ assert_param(IS_I2C_MEMADD_SIZE(MemAddSize)); @@ -2669,41 +2804,38 @@ HAL_StatusTypeDef HAL_I2C_Mem_Write_IT(I2C_HandleTypeDef *hi2c, uint16_t DevAddr /* Process Locked */ __HAL_LOCK(hi2c); - /* Init tickstart for timeout management*/ - tickstart = HAL_GetTick(); - hi2c->State = HAL_I2C_STATE_BUSY_TX; hi2c->Mode = HAL_I2C_MODE_MEM; hi2c->ErrorCode = HAL_I2C_ERROR_NONE; /* Prepare transfer parameters */ + hi2c->XferSize = 0U; hi2c->pBuffPtr = pData; hi2c->XferCount = Size; hi2c->XferOptions = I2C_NO_OPTION_FRAME; - hi2c->XferISR = I2C_Master_ISR_IT; + hi2c->XferISR = I2C_Mem_ISR_IT; + hi2c->Devaddress = DevAddress; - if (hi2c->XferCount > MAX_NBYTE_SIZE) + /* If Memory address size is 8Bit */ + if (MemAddSize == I2C_MEMADD_SIZE_8BIT) { - hi2c->XferSize = MAX_NBYTE_SIZE; - xfermode = I2C_RELOAD_MODE; + /* Prefetch Memory Address */ + hi2c->Instance->TXDR = I2C_MEM_ADD_LSB(MemAddress); + + /* Reset Memaddress content */ + hi2c->Memaddress = 0xFFFFFFFFU; } + /* If Memory address size is 16Bit */ else { - hi2c->XferSize = hi2c->XferCount; - xfermode = I2C_AUTOEND_MODE; - } + /* Prefetch Memory Address (MSB part, LSB will be manage through interrupt) */ + hi2c->Instance->TXDR = I2C_MEM_ADD_MSB(MemAddress); - /* Send Slave Address and Memory Address */ - if (I2C_RequestMemoryWrite(hi2c, DevAddress, MemAddress, MemAddSize, I2C_TIMEOUT_FLAG, tickstart) - != HAL_OK) - { - /* Process Unlocked */ - __HAL_UNLOCK(hi2c); - return HAL_ERROR; + /* Prepare Memaddress buffer for LSB part */ + hi2c->Memaddress = I2C_MEM_ADD_LSB(MemAddress); } - - /* Set NBYTES to write and reload if hi2c->XferCount > MAX_NBYTE_SIZE and generate RESTART */ - I2C_TransferConfig(hi2c, DevAddress, (uint8_t)hi2c->XferSize, xfermode, I2C_NO_STARTSTOP); + /* Send Slave Address and Memory Address */ + I2C_TransferConfig(hi2c, DevAddress, (uint8_t)MemAddSize, I2C_RELOAD_MODE, I2C_GENERATE_START_WRITE); /* Process Unlocked */ __HAL_UNLOCK(hi2c); @@ -2741,9 +2873,6 @@ HAL_StatusTypeDef HAL_I2C_Mem_Write_IT(I2C_HandleTypeDef *hi2c, uint16_t DevAddr HAL_StatusTypeDef HAL_I2C_Mem_Read_IT(I2C_HandleTypeDef *hi2c, uint16_t DevAddress, uint16_t MemAddress, uint16_t MemAddSize, uint8_t *pData, uint16_t Size) { - uint32_t tickstart; - uint32_t xfermode; - /* Check the parameters */ assert_param(IS_I2C_MEMADD_SIZE(MemAddSize)); @@ -2763,9 +2892,6 @@ HAL_StatusTypeDef HAL_I2C_Mem_Read_IT(I2C_HandleTypeDef *hi2c, uint16_t DevAddre /* Process Locked */ __HAL_LOCK(hi2c); - /* Init tickstart for timeout management*/ - tickstart = HAL_GetTick(); - hi2c->State = HAL_I2C_STATE_BUSY_RX; hi2c->Mode = HAL_I2C_MODE_MEM; hi2c->ErrorCode = HAL_I2C_ERROR_NONE; @@ -2774,29 +2900,29 @@ HAL_StatusTypeDef HAL_I2C_Mem_Read_IT(I2C_HandleTypeDef *hi2c, uint16_t DevAddre hi2c->pBuffPtr = pData; hi2c->XferCount = Size; hi2c->XferOptions = I2C_NO_OPTION_FRAME; - hi2c->XferISR = I2C_Master_ISR_IT; + hi2c->XferISR = I2C_Mem_ISR_IT; + hi2c->Devaddress = DevAddress; - if (hi2c->XferCount > MAX_NBYTE_SIZE) + /* If Memory address size is 8Bit */ + if (MemAddSize == I2C_MEMADD_SIZE_8BIT) { - hi2c->XferSize = MAX_NBYTE_SIZE; - xfermode = I2C_RELOAD_MODE; + /* Prefetch Memory Address */ + hi2c->Instance->TXDR = I2C_MEM_ADD_LSB(MemAddress); + + /* Reset Memaddress content */ + hi2c->Memaddress = 0xFFFFFFFFU; } + /* If Memory address size is 16Bit */ else { - hi2c->XferSize = hi2c->XferCount; - xfermode = I2C_AUTOEND_MODE; - } + /* Prefetch Memory Address (MSB part, LSB will be manage through interrupt) */ + hi2c->Instance->TXDR = I2C_MEM_ADD_MSB(MemAddress); - /* Send Slave Address and Memory Address */ - if (I2C_RequestMemoryRead(hi2c, DevAddress, MemAddress, MemAddSize, I2C_TIMEOUT_FLAG, tickstart) != HAL_OK) - { - /* Process Unlocked */ - __HAL_UNLOCK(hi2c); - return HAL_ERROR; + /* Prepare Memaddress buffer for LSB part */ + hi2c->Memaddress = I2C_MEM_ADD_LSB(MemAddress); } - - /* Set NBYTES to write and reload if hi2c->XferCount > MAX_NBYTE_SIZE and generate RESTART */ - I2C_TransferConfig(hi2c, DevAddress, (uint8_t)hi2c->XferSize, xfermode, I2C_GENERATE_START_READ); + /* Send Slave Address and Memory Address */ + I2C_TransferConfig(hi2c, DevAddress, (uint8_t)MemAddSize, I2C_SOFTEND_MODE, I2C_GENERATE_START_WRITE); /* Process Unlocked */ __HAL_UNLOCK(hi2c); @@ -2805,11 +2931,11 @@ HAL_StatusTypeDef HAL_I2C_Mem_Read_IT(I2C_HandleTypeDef *hi2c, uint16_t DevAddre to avoid the risk of I2C interrupt handle execution before current process unlock */ - /* Enable ERR, TC, STOP, NACK, RXI interrupt */ + /* Enable ERR, TC, STOP, NACK, TXI interrupt */ /* possible to enable all of these */ /* I2C_IT_ERRI | I2C_IT_TCI | I2C_IT_STOPI | I2C_IT_NACKI | I2C_IT_ADDRI | I2C_IT_RXI | I2C_IT_TXI */ - I2C_Enable_IRQ(hi2c, I2C_XFER_RX_IT); + I2C_Enable_IRQ(hi2c, I2C_XFER_TX_IT); return HAL_OK; } @@ -2818,6 +2944,7 @@ HAL_StatusTypeDef HAL_I2C_Mem_Read_IT(I2C_HandleTypeDef *hi2c, uint16_t DevAddre return HAL_BUSY; } } + /** * @brief Write an amount of data in non-blocking mode with DMA to a specific memory address * @param hi2c Pointer to a I2C_HandleTypeDef structure that contains @@ -2833,8 +2960,6 @@ HAL_StatusTypeDef HAL_I2C_Mem_Read_IT(I2C_HandleTypeDef *hi2c, uint16_t DevAddre HAL_StatusTypeDef HAL_I2C_Mem_Write_DMA(I2C_HandleTypeDef *hi2c, uint16_t DevAddress, uint16_t MemAddress, uint16_t MemAddSize, uint8_t *pData, uint16_t Size) { - uint32_t tickstart; - uint32_t xfermode; HAL_StatusTypeDef dmaxferstatus; /* Check the parameters */ @@ -2856,9 +2981,6 @@ HAL_StatusTypeDef HAL_I2C_Mem_Write_DMA(I2C_HandleTypeDef *hi2c, uint16_t DevAdd /* Process Locked */ __HAL_LOCK(hi2c); - /* Init tickstart for timeout management*/ - tickstart = HAL_GetTick(); - hi2c->State = HAL_I2C_STATE_BUSY_TX; hi2c->Mode = HAL_I2C_MODE_MEM; hi2c->ErrorCode = HAL_I2C_ERROR_NONE; @@ -2867,28 +2989,36 @@ HAL_StatusTypeDef HAL_I2C_Mem_Write_DMA(I2C_HandleTypeDef *hi2c, uint16_t DevAdd hi2c->pBuffPtr = pData; hi2c->XferCount = Size; hi2c->XferOptions = I2C_NO_OPTION_FRAME; - hi2c->XferISR = I2C_Master_ISR_DMA; + hi2c->XferISR = I2C_Mem_ISR_DMA; + hi2c->Devaddress = DevAddress; if (hi2c->XferCount > MAX_NBYTE_SIZE) { hi2c->XferSize = MAX_NBYTE_SIZE; - xfermode = I2C_RELOAD_MODE; } else { hi2c->XferSize = hi2c->XferCount; - xfermode = I2C_AUTOEND_MODE; } - /* Send Slave Address and Memory Address */ - if (I2C_RequestMemoryWrite(hi2c, DevAddress, MemAddress, MemAddSize, I2C_TIMEOUT_FLAG, tickstart) - != HAL_OK) + /* If Memory address size is 8Bit */ + if (MemAddSize == I2C_MEMADD_SIZE_8BIT) { - /* Process Unlocked */ - __HAL_UNLOCK(hi2c); - return HAL_ERROR; + /* Prefetch Memory Address */ + hi2c->Instance->TXDR = I2C_MEM_ADD_LSB(MemAddress); + + /* Reset Memaddress content */ + hi2c->Memaddress = 0xFFFFFFFFU; } + /* If Memory address size is 16Bit */ + else + { + /* Prefetch Memory Address (MSB part, LSB will be manage through interrupt) */ + hi2c->Instance->TXDR = I2C_MEM_ADD_MSB(MemAddress); + /* Prepare Memaddress buffer for LSB part */ + hi2c->Memaddress = I2C_MEM_ADD_LSB(MemAddress); + } if (hi2c->hdmatx != NULL) { @@ -2923,12 +3053,8 @@ HAL_StatusTypeDef HAL_I2C_Mem_Write_DMA(I2C_HandleTypeDef *hi2c, uint16_t DevAdd if (dmaxferstatus == HAL_OK) { - /* Send Slave Address */ - /* Set NBYTES to write and reload if hi2c->XferCount > MAX_NBYTE_SIZE and generate RESTART */ - I2C_TransferConfig(hi2c, DevAddress, (uint8_t)hi2c->XferSize, xfermode, I2C_NO_STARTSTOP); - - /* Update XferCount value */ - hi2c->XferCount -= hi2c->XferSize; + /* Send Slave Address and Memory Address */ + I2C_TransferConfig(hi2c, DevAddress, (uint8_t)MemAddSize, I2C_RELOAD_MODE, I2C_GENERATE_START_WRITE); /* Process Unlocked */ __HAL_UNLOCK(hi2c); @@ -2936,11 +3062,11 @@ HAL_StatusTypeDef HAL_I2C_Mem_Write_DMA(I2C_HandleTypeDef *hi2c, uint16_t DevAdd /* Note : The I2C interrupts must be enabled after unlocking current process to avoid the risk of I2C interrupt handle execution before current process unlock */ - /* Enable ERR and NACK interrupts */ - I2C_Enable_IRQ(hi2c, I2C_XFER_ERROR_IT); - - /* Enable DMA Request */ - hi2c->Instance->CR1 |= I2C_CR1_TXDMAEN; + /* Enable ERR, TC, STOP, NACK, TXI interrupt */ + /* possible to enable all of these */ + /* I2C_IT_ERRI | I2C_IT_TCI | I2C_IT_STOPI | I2C_IT_NACKI | + I2C_IT_ADDRI | I2C_IT_RXI | I2C_IT_TXI */ + I2C_Enable_IRQ(hi2c, I2C_XFER_TX_IT); } else { @@ -2980,8 +3106,6 @@ HAL_StatusTypeDef HAL_I2C_Mem_Write_DMA(I2C_HandleTypeDef *hi2c, uint16_t DevAdd HAL_StatusTypeDef HAL_I2C_Mem_Read_DMA(I2C_HandleTypeDef *hi2c, uint16_t DevAddress, uint16_t MemAddress, uint16_t MemAddSize, uint8_t *pData, uint16_t Size) { - uint32_t tickstart; - uint32_t xfermode; HAL_StatusTypeDef dmaxferstatus; /* Check the parameters */ @@ -3003,9 +3127,6 @@ HAL_StatusTypeDef HAL_I2C_Mem_Read_DMA(I2C_HandleTypeDef *hi2c, uint16_t DevAddr /* Process Locked */ __HAL_LOCK(hi2c); - /* Init tickstart for timeout management*/ - tickstart = HAL_GetTick(); - hi2c->State = HAL_I2C_STATE_BUSY_RX; hi2c->Mode = HAL_I2C_MODE_MEM; hi2c->ErrorCode = HAL_I2C_ERROR_NONE; @@ -3014,25 +3135,35 @@ HAL_StatusTypeDef HAL_I2C_Mem_Read_DMA(I2C_HandleTypeDef *hi2c, uint16_t DevAddr hi2c->pBuffPtr = pData; hi2c->XferCount = Size; hi2c->XferOptions = I2C_NO_OPTION_FRAME; - hi2c->XferISR = I2C_Master_ISR_DMA; + hi2c->XferISR = I2C_Mem_ISR_DMA; + hi2c->Devaddress = DevAddress; if (hi2c->XferCount > MAX_NBYTE_SIZE) { hi2c->XferSize = MAX_NBYTE_SIZE; - xfermode = I2C_RELOAD_MODE; } else { hi2c->XferSize = hi2c->XferCount; - xfermode = I2C_AUTOEND_MODE; } - /* Send Slave Address and Memory Address */ - if (I2C_RequestMemoryRead(hi2c, DevAddress, MemAddress, MemAddSize, I2C_TIMEOUT_FLAG, tickstart) != HAL_OK) + /* If Memory address size is 8Bit */ + if (MemAddSize == I2C_MEMADD_SIZE_8BIT) { - /* Process Unlocked */ - __HAL_UNLOCK(hi2c); - return HAL_ERROR; + /* Prefetch Memory Address */ + hi2c->Instance->TXDR = I2C_MEM_ADD_LSB(MemAddress); + + /* Reset Memaddress content */ + hi2c->Memaddress = 0xFFFFFFFFU; + } + /* If Memory address size is 16Bit */ + else + { + /* Prefetch Memory Address (MSB part, LSB will be manage through interrupt) */ + hi2c->Instance->TXDR = I2C_MEM_ADD_MSB(MemAddress); + + /* Prepare Memaddress buffer for LSB part */ + hi2c->Memaddress = I2C_MEM_ADD_LSB(MemAddress); } if (hi2c->hdmarx != NULL) @@ -3068,11 +3199,8 @@ HAL_StatusTypeDef HAL_I2C_Mem_Read_DMA(I2C_HandleTypeDef *hi2c, uint16_t DevAddr if (dmaxferstatus == HAL_OK) { - /* Set NBYTES to write and reload if hi2c->XferCount > MAX_NBYTE_SIZE and generate RESTART */ - I2C_TransferConfig(hi2c, DevAddress, (uint8_t)hi2c->XferSize, xfermode, I2C_GENERATE_START_READ); - - /* Update XferCount value */ - hi2c->XferCount -= hi2c->XferSize; + /* Send Slave Address and Memory Address */ + I2C_TransferConfig(hi2c, DevAddress, (uint8_t)MemAddSize, I2C_SOFTEND_MODE, I2C_GENERATE_START_WRITE); /* Process Unlocked */ __HAL_UNLOCK(hi2c); @@ -3080,11 +3208,11 @@ HAL_StatusTypeDef HAL_I2C_Mem_Read_DMA(I2C_HandleTypeDef *hi2c, uint16_t DevAddr /* Note : The I2C interrupts must be enabled after unlocking current process to avoid the risk of I2C interrupt handle execution before current process unlock */ - /* Enable ERR and NACK interrupts */ - I2C_Enable_IRQ(hi2c, I2C_XFER_ERROR_IT); - - /* Enable DMA Request */ - hi2c->Instance->CR1 |= I2C_CR1_RXDMAEN; + /* Enable ERR, TC, STOP, NACK, TXI interrupt */ + /* possible to enable all of these */ + /* I2C_IT_ERRI | I2C_IT_TCI | I2C_IT_STOPI | I2C_IT_NACKI | + I2C_IT_ADDRI | I2C_IT_RXI | I2C_IT_TXI */ + I2C_Enable_IRQ(hi2c, I2C_XFER_TX_IT); } else { @@ -3213,22 +3341,6 @@ HAL_StatusTypeDef HAL_I2C_IsDeviceReady(I2C_HandleTypeDef *hi2c, uint16_t DevAdd __HAL_I2C_CLEAR_FLAG(hi2c, I2C_FLAG_STOPF); } - /* Check if the maximum allowed number of trials has been reached */ - if (I2C_Trials == Trials) - { - /* Generate Stop */ - hi2c->Instance->CR2 |= I2C_CR2_STOP; - - /* Wait until STOPF flag is reset */ - if (I2C_WaitOnFlagUntilTimeout(hi2c, I2C_FLAG_STOPF, RESET, Timeout, tickstart) != HAL_OK) - { - return HAL_ERROR; - } - - /* Clear STOP Flag */ - __HAL_I2C_CLEAR_FLAG(hi2c, I2C_FLAG_STOPF); - } - /* Increment Trials */ I2C_Trials++; } while (I2C_Trials < Trials); @@ -3267,6 +3379,7 @@ HAL_StatusTypeDef HAL_I2C_Master_Seq_Transmit_IT(I2C_HandleTypeDef *hi2c, uint16 { uint32_t xfermode; uint32_t xferrequest = I2C_GENERATE_START_WRITE; + uint32_t sizetoxfer = 0U; /* Check the parameters */ assert_param(IS_I2C_TRANSFER_OPTIONS_REQUEST(XferOptions)); @@ -3298,6 +3411,21 @@ HAL_StatusTypeDef HAL_I2C_Master_Seq_Transmit_IT(I2C_HandleTypeDef *hi2c, uint16 xfermode = hi2c->XferOptions; } + if ((hi2c->XferSize > 0U) && ((XferOptions == I2C_FIRST_FRAME) || \ + (XferOptions == I2C_FIRST_AND_LAST_FRAME))) + { + /* Preload TX register */ + /* Write data to TXDR */ + hi2c->Instance->TXDR = *hi2c->pBuffPtr; + + /* Increment Buffer pointer */ + hi2c->pBuffPtr++; + + sizetoxfer = hi2c->XferSize; + hi2c->XferCount--; + hi2c->XferSize--; + } + /* If transfer direction not change and there is no request to start another frame, do not generate Restart Condition */ /* Mean Previous state is same as current state */ @@ -3319,7 +3447,14 @@ HAL_StatusTypeDef HAL_I2C_Master_Seq_Transmit_IT(I2C_HandleTypeDef *hi2c, uint16 } /* Send Slave Address and set NBYTES to write */ - I2C_TransferConfig(hi2c, DevAddress, (uint8_t)hi2c->XferSize, xfermode, xferrequest); + if ((XferOptions == I2C_FIRST_FRAME) || (XferOptions == I2C_FIRST_AND_LAST_FRAME)) + { + I2C_TransferConfig(hi2c, DevAddress, (uint8_t)sizetoxfer, xfermode, xferrequest); + } + else + { + I2C_TransferConfig(hi2c, DevAddress, (uint8_t)hi2c->XferSize, xfermode, xferrequest); + } /* Process Unlocked */ __HAL_UNLOCK(hi2c); @@ -3327,6 +3462,10 @@ HAL_StatusTypeDef HAL_I2C_Master_Seq_Transmit_IT(I2C_HandleTypeDef *hi2c, uint16 /* Note : The I2C interrupts must be enabled after unlocking current process to avoid the risk of I2C interrupt handle execution before current process unlock */ + /* Enable ERR, TC, STOP, NACK, TXI interrupt */ + /* possible to enable all of these */ + /* I2C_IT_ERRI | I2C_IT_TCI | I2C_IT_STOPI | I2C_IT_NACKI | + I2C_IT_ADDRI | I2C_IT_RXI | I2C_IT_TXI */ I2C_Enable_IRQ(hi2c, I2C_XFER_TX_IT); return HAL_OK; @@ -3355,6 +3494,7 @@ HAL_StatusTypeDef HAL_I2C_Master_Seq_Transmit_DMA(I2C_HandleTypeDef *hi2c, uint1 uint32_t xfermode; uint32_t xferrequest = I2C_GENERATE_START_WRITE; HAL_StatusTypeDef dmaxferstatus; + uint32_t sizetoxfer = 0U; /* Check the parameters */ assert_param(IS_I2C_TRANSFER_OPTIONS_REQUEST(XferOptions)); @@ -3386,6 +3526,21 @@ HAL_StatusTypeDef HAL_I2C_Master_Seq_Transmit_DMA(I2C_HandleTypeDef *hi2c, uint1 xfermode = hi2c->XferOptions; } + if ((hi2c->XferSize > 0U) && ((XferOptions == I2C_FIRST_FRAME) || \ + (XferOptions == I2C_FIRST_AND_LAST_FRAME))) + { + /* Preload TX register */ + /* Write data to TXDR */ + hi2c->Instance->TXDR = *hi2c->pBuffPtr; + + /* Increment Buffer pointer */ + hi2c->pBuffPtr++; + + sizetoxfer = hi2c->XferSize; + hi2c->XferCount--; + hi2c->XferSize--; + } + /* If transfer direction not change and there is no request to start another frame, do not generate Restart Condition */ /* Mean Previous state is same as current state */ @@ -3421,8 +3576,8 @@ HAL_StatusTypeDef HAL_I2C_Master_Seq_Transmit_DMA(I2C_HandleTypeDef *hi2c, uint1 hi2c->hdmatx->XferAbortCallback = NULL; /* Enable the DMA channel */ - dmaxferstatus = HAL_DMA_Start_IT(hi2c->hdmatx, (uint32_t)pData, (uint32_t)&hi2c->Instance->TXDR, - hi2c->XferSize); + dmaxferstatus = HAL_DMA_Start_IT(hi2c->hdmatx, (uint32_t)hi2c->pBuffPtr, + (uint32_t)&hi2c->Instance->TXDR, hi2c->XferSize); } else { @@ -3442,7 +3597,14 @@ HAL_StatusTypeDef HAL_I2C_Master_Seq_Transmit_DMA(I2C_HandleTypeDef *hi2c, uint1 if (dmaxferstatus == HAL_OK) { /* Send Slave Address and set NBYTES to write */ - I2C_TransferConfig(hi2c, DevAddress, (uint8_t)hi2c->XferSize, xfermode, xferrequest); + if ((XferOptions == I2C_FIRST_FRAME) || (XferOptions == I2C_FIRST_AND_LAST_FRAME)) + { + I2C_TransferConfig(hi2c, DevAddress, (uint8_t)sizetoxfer, xfermode, xferrequest); + } + else + { + I2C_TransferConfig(hi2c, DevAddress, (uint8_t)hi2c->XferSize, xfermode, xferrequest); + } /* Update XferCount value */ hi2c->XferCount -= hi2c->XferSize; @@ -3481,8 +3643,14 @@ HAL_StatusTypeDef HAL_I2C_Master_Seq_Transmit_DMA(I2C_HandleTypeDef *hi2c, uint1 /* Send Slave Address */ /* Set NBYTES to write and generate START condition */ - I2C_TransferConfig(hi2c, DevAddress, (uint8_t)hi2c->XferSize, I2C_AUTOEND_MODE, - I2C_GENERATE_START_WRITE); + if ((XferOptions == I2C_FIRST_FRAME) || (XferOptions == I2C_FIRST_AND_LAST_FRAME)) + { + I2C_TransferConfig(hi2c, DevAddress, (uint8_t)sizetoxfer, xfermode, xferrequest); + } + else + { + I2C_TransferConfig(hi2c, DevAddress, (uint8_t)hi2c->XferSize, xfermode, xferrequest); + } /* Process Unlocked */ __HAL_UNLOCK(hi2c); @@ -3745,11 +3913,11 @@ HAL_StatusTypeDef HAL_I2C_Master_Seq_Receive_DMA(I2C_HandleTypeDef *hi2c, uint16 /* Note : The I2C interrupts must be enabled after unlocking current process to avoid the risk of I2C interrupt handle execution before current process unlock */ - /* Enable ERR, TC, STOP, NACK, TXI interrupt */ + /* Enable ERR, TC, STOP, NACK, RXI interrupt */ /* possible to enable all of these */ /* I2C_IT_ERRI | I2C_IT_TCI | I2C_IT_STOPI | I2C_IT_NACKI | I2C_IT_ADDRI | I2C_IT_RXI | I2C_IT_TXI */ - I2C_Enable_IRQ(hi2c, I2C_XFER_TX_IT); + I2C_Enable_IRQ(hi2c, I2C_XFER_RX_IT); } return HAL_OK; @@ -3773,6 +3941,9 @@ HAL_StatusTypeDef HAL_I2C_Master_Seq_Receive_DMA(I2C_HandleTypeDef *hi2c, uint16 HAL_StatusTypeDef HAL_I2C_Slave_Seq_Transmit_IT(I2C_HandleTypeDef *hi2c, uint8_t *pData, uint16_t Size, uint32_t XferOptions) { + /* Declaration of tmp to prevent undefined behavior of volatile usage */ + FlagStatus tmp; + /* Check the parameters */ assert_param(IS_I2C_TRANSFER_OPTIONS_REQUEST(XferOptions)); @@ -3832,7 +4003,8 @@ HAL_StatusTypeDef HAL_I2C_Slave_Seq_Transmit_IT(I2C_HandleTypeDef *hi2c, uint8_t hi2c->XferOptions = XferOptions; hi2c->XferISR = I2C_Slave_ISR_IT; - if (I2C_GET_DIR(hi2c) == I2C_DIRECTION_RECEIVE) + tmp = __HAL_I2C_GET_FLAG(hi2c, I2C_FLAG_ADDR); + if ((I2C_GET_DIR(hi2c) == I2C_DIRECTION_RECEIVE) && (tmp != RESET)) { /* Clear ADDR flag after prepare the transfer parameters */ /* This action will generate an acknowledge to the Master */ @@ -3869,6 +4041,8 @@ HAL_StatusTypeDef HAL_I2C_Slave_Seq_Transmit_IT(I2C_HandleTypeDef *hi2c, uint8_t HAL_StatusTypeDef HAL_I2C_Slave_Seq_Transmit_DMA(I2C_HandleTypeDef *hi2c, uint8_t *pData, uint16_t Size, uint32_t XferOptions) { + /* Declaration of tmp to prevent undefined behavior of volatile usage */ + FlagStatus tmp; HAL_StatusTypeDef dmaxferstatus; /* Check the parameters */ @@ -3903,7 +4077,7 @@ HAL_StatusTypeDef HAL_I2C_Slave_Seq_Transmit_DMA(I2C_HandleTypeDef *hi2c, uint8_ hi2c->Instance->CR1 &= ~I2C_CR1_RXDMAEN; /* Set the I2C DMA Abort callback : - will lead to call HAL_I2C_ErrorCallback() at end of DMA abort procedure */ + will lead to call HAL_I2C_ErrorCallback() at end of DMA abort procedure */ hi2c->hdmarx->XferAbortCallback = I2C_DMAAbort; /* Abort DMA RX */ @@ -3925,7 +4099,7 @@ HAL_StatusTypeDef HAL_I2C_Slave_Seq_Transmit_DMA(I2C_HandleTypeDef *hi2c, uint8_ if (hi2c->hdmatx != NULL) { /* Set the I2C DMA Abort callback : - will lead to call HAL_I2C_ErrorCallback() at end of DMA abort procedure */ + will lead to call HAL_I2C_ErrorCallback() at end of DMA abort procedure */ hi2c->hdmatx->XferAbortCallback = I2C_DMAAbort; /* Abort DMA TX */ @@ -4010,7 +4184,8 @@ HAL_StatusTypeDef HAL_I2C_Slave_Seq_Transmit_DMA(I2C_HandleTypeDef *hi2c, uint8_ return HAL_ERROR; } - if (I2C_GET_DIR(hi2c) == I2C_DIRECTION_RECEIVE) + tmp = __HAL_I2C_GET_FLAG(hi2c, I2C_FLAG_ADDR); + if ((I2C_GET_DIR(hi2c) == I2C_DIRECTION_RECEIVE) && (tmp != RESET)) { /* Clear ADDR flag after prepare the transfer parameters */ /* This action will generate an acknowledge to the Master */ @@ -4050,6 +4225,9 @@ HAL_StatusTypeDef HAL_I2C_Slave_Seq_Transmit_DMA(I2C_HandleTypeDef *hi2c, uint8_ HAL_StatusTypeDef HAL_I2C_Slave_Seq_Receive_IT(I2C_HandleTypeDef *hi2c, uint8_t *pData, uint16_t Size, uint32_t XferOptions) { + /* Declaration of tmp to prevent undefined behavior of volatile usage */ + FlagStatus tmp; + /* Check the parameters */ assert_param(IS_I2C_TRANSFER_OPTIONS_REQUEST(XferOptions)); @@ -4109,7 +4287,8 @@ HAL_StatusTypeDef HAL_I2C_Slave_Seq_Receive_IT(I2C_HandleTypeDef *hi2c, uint8_t hi2c->XferOptions = XferOptions; hi2c->XferISR = I2C_Slave_ISR_IT; - if (I2C_GET_DIR(hi2c) == I2C_DIRECTION_TRANSMIT) + tmp = __HAL_I2C_GET_FLAG(hi2c, I2C_FLAG_ADDR); + if ((I2C_GET_DIR(hi2c) == I2C_DIRECTION_TRANSMIT) && (tmp != RESET)) { /* Clear ADDR flag after prepare the transfer parameters */ /* This action will generate an acknowledge to the Master */ @@ -4146,6 +4325,8 @@ HAL_StatusTypeDef HAL_I2C_Slave_Seq_Receive_IT(I2C_HandleTypeDef *hi2c, uint8_t HAL_StatusTypeDef HAL_I2C_Slave_Seq_Receive_DMA(I2C_HandleTypeDef *hi2c, uint8_t *pData, uint16_t Size, uint32_t XferOptions) { + /* Declaration of tmp to prevent undefined behavior of volatile usage */ + FlagStatus tmp; HAL_StatusTypeDef dmaxferstatus; /* Check the parameters */ @@ -4287,7 +4468,8 @@ HAL_StatusTypeDef HAL_I2C_Slave_Seq_Receive_DMA(I2C_HandleTypeDef *hi2c, uint8_t return HAL_ERROR; } - if (I2C_GET_DIR(hi2c) == I2C_DIRECTION_TRANSMIT) + tmp = __HAL_I2C_GET_FLAG(hi2c, I2C_FLAG_ADDR); + if ((I2C_GET_DIR(hi2c) == I2C_DIRECTION_TRANSMIT) && (tmp != RESET)) { /* Clear ADDR flag after prepare the transfer parameters */ /* This action will generate an acknowledge to the Master */ @@ -4439,7 +4621,7 @@ HAL_StatusTypeDef HAL_I2C_Master_Abort_IT(I2C_HandleTypeDef *hi2c, uint16_t DevA * the configuration information for the specified I2C. * @retval None */ -void HAL_I2C_EV_IRQHandler(I2C_HandleTypeDef *hi2c) +void HAL_I2C_EV_IRQHandler(I2C_HandleTypeDef *hi2c) /* Derogation MISRAC2012-Rule-8.13 */ { /* Get current IT Flags and IT sources value */ uint32_t itflags = READ_REG(hi2c->Instance->ISR); @@ -4692,7 +4874,7 @@ __weak void HAL_I2C_AbortCpltCallback(I2C_HandleTypeDef *hi2c) * the configuration information for the specified I2C. * @retval HAL state */ -HAL_I2C_StateTypeDef HAL_I2C_GetState(I2C_HandleTypeDef *hi2c) +HAL_I2C_StateTypeDef HAL_I2C_GetState(const I2C_HandleTypeDef *hi2c) { /* Return I2C handle state */ return hi2c->State; @@ -4704,7 +4886,7 @@ HAL_I2C_StateTypeDef HAL_I2C_GetState(I2C_HandleTypeDef *hi2c) * the configuration information for I2C module * @retval HAL mode */ -HAL_I2C_ModeTypeDef HAL_I2C_GetMode(I2C_HandleTypeDef *hi2c) +HAL_I2C_ModeTypeDef HAL_I2C_GetMode(const I2C_HandleTypeDef *hi2c) { return hi2c->Mode; } @@ -4715,7 +4897,7 @@ HAL_I2C_ModeTypeDef HAL_I2C_GetMode(I2C_HandleTypeDef *hi2c) * the configuration information for the specified I2C. * @retval I2C Error Code */ -uint32_t HAL_I2C_GetError(I2C_HandleTypeDef *hi2c) +uint32_t HAL_I2C_GetError(const I2C_HandleTypeDef *hi2c) { return hi2c->ErrorCode; } @@ -4778,17 +4960,22 @@ static HAL_StatusTypeDef I2C_Master_ISR_IT(struct __I2C_HandleTypeDef *hi2c, uin hi2c->XferSize--; hi2c->XferCount--; } - else if ((I2C_CHECK_FLAG(tmpITFlags, I2C_FLAG_TXIS) != RESET) && \ - (I2C_CHECK_IT_SOURCE(ITSources, I2C_IT_TXI) != RESET)) + else if ((I2C_CHECK_FLAG(tmpITFlags, I2C_FLAG_TC) == RESET) && \ + ((I2C_CHECK_FLAG(tmpITFlags, I2C_FLAG_TXIS) != RESET) && \ + (I2C_CHECK_IT_SOURCE(ITSources, I2C_IT_TXI) != RESET))) { /* Write data to TXDR */ - hi2c->Instance->TXDR = *hi2c->pBuffPtr; + if (hi2c->XferCount != 0U) + { + /* Write data to TXDR */ + hi2c->Instance->TXDR = *hi2c->pBuffPtr; - /* Increment Buffer pointer */ - hi2c->pBuffPtr++; + /* Increment Buffer pointer */ + hi2c->pBuffPtr++; - hi2c->XferSize--; - hi2c->XferCount--; + hi2c->XferSize--; + hi2c->XferCount--; + } } else if ((I2C_CHECK_FLAG(tmpITFlags, I2C_FLAG_TCR) != RESET) && \ (I2C_CHECK_IT_SOURCE(ITSources, I2C_IT_TCI) != RESET)) @@ -4879,87 +5066,229 @@ static HAL_StatusTypeDef I2C_Master_ISR_IT(struct __I2C_HandleTypeDef *hi2c, uin } /** - * @brief Interrupt Sub-Routine which handle the Interrupt Flags Slave Mode with Interrupt. + * @brief Interrupt Sub-Routine which handle the Interrupt Flags Memory Mode with Interrupt. * @param hi2c Pointer to a I2C_HandleTypeDef structure that contains * the configuration information for the specified I2C. * @param ITFlags Interrupt flags to handle. * @param ITSources Interrupt sources enabled. * @retval HAL status */ -static HAL_StatusTypeDef I2C_Slave_ISR_IT(struct __I2C_HandleTypeDef *hi2c, uint32_t ITFlags, - uint32_t ITSources) +static HAL_StatusTypeDef I2C_Mem_ISR_IT(struct __I2C_HandleTypeDef *hi2c, uint32_t ITFlags, + uint32_t ITSources) { - uint32_t tmpoptions = hi2c->XferOptions; + uint32_t direction = I2C_GENERATE_START_WRITE; uint32_t tmpITFlags = ITFlags; - /* Process locked */ + /* Process Locked */ __HAL_LOCK(hi2c); - /* Check if STOPF is set */ - if ((I2C_CHECK_FLAG(tmpITFlags, I2C_FLAG_STOPF) != RESET) && \ - (I2C_CHECK_IT_SOURCE(ITSources, I2C_IT_STOPI) != RESET)) - { - /* Call I2C Slave complete process */ - I2C_ITSlaveCplt(hi2c, tmpITFlags); - } - if ((I2C_CHECK_FLAG(tmpITFlags, I2C_FLAG_AF) != RESET) && \ (I2C_CHECK_IT_SOURCE(ITSources, I2C_IT_NACKI) != RESET)) { - /* Check that I2C transfer finished */ - /* if yes, normal use case, a NACK is sent by the MASTER when Transfer is finished */ - /* Mean XferCount == 0*/ - /* So clear Flag NACKF only */ - if (hi2c->XferCount == 0U) - { - if ((hi2c->State == HAL_I2C_STATE_LISTEN) && (tmpoptions == I2C_FIRST_AND_LAST_FRAME)) - /* Same action must be done for (tmpoptions == I2C_LAST_FRAME) which removed for - Warning[Pa134]: left and right operands are identical */ - { - /* Call I2C Listen complete process */ - I2C_ITListenCplt(hi2c, tmpITFlags); - } - else if ((hi2c->State == HAL_I2C_STATE_BUSY_TX_LISTEN) && (tmpoptions != I2C_NO_OPTION_FRAME)) - { - /* Clear NACK Flag */ - __HAL_I2C_CLEAR_FLAG(hi2c, I2C_FLAG_AF); - - /* Flush TX register */ - I2C_Flush_TXDR(hi2c); - - /* Last Byte is Transmitted */ - /* Call I2C Slave Sequential complete process */ - I2C_ITSlaveSeqCplt(hi2c); - } - else - { - /* Clear NACK Flag */ - __HAL_I2C_CLEAR_FLAG(hi2c, I2C_FLAG_AF); - } - } - else - { - /* if no, error use case, a Non-Acknowledge of last Data is generated by the MASTER*/ - /* Clear NACK Flag */ - __HAL_I2C_CLEAR_FLAG(hi2c, I2C_FLAG_AF); + /* Clear NACK Flag */ + __HAL_I2C_CLEAR_FLAG(hi2c, I2C_FLAG_AF); - /* Set ErrorCode corresponding to a Non-Acknowledge */ - hi2c->ErrorCode |= HAL_I2C_ERROR_AF; + /* Set corresponding Error Code */ + /* No need to generate STOP, it is automatically done */ + /* Error callback will be send during stop flag treatment */ + hi2c->ErrorCode |= HAL_I2C_ERROR_AF; - if ((tmpoptions == I2C_FIRST_FRAME) || (tmpoptions == I2C_NEXT_FRAME)) - { - /* Call the corresponding callback to inform upper layer of End of Transfer */ - I2C_ITError(hi2c, hi2c->ErrorCode); - } - } + /* Flush TX register */ + I2C_Flush_TXDR(hi2c); } else if ((I2C_CHECK_FLAG(tmpITFlags, I2C_FLAG_RXNE) != RESET) && \ (I2C_CHECK_IT_SOURCE(ITSources, I2C_IT_RXI) != RESET)) { - if (hi2c->XferCount > 0U) - { - /* Read data from RXDR */ - *hi2c->pBuffPtr = (uint8_t)hi2c->Instance->RXDR; + /* Remove RXNE flag on temporary variable as read done */ + tmpITFlags &= ~I2C_FLAG_RXNE; + + /* Read data from RXDR */ + *hi2c->pBuffPtr = (uint8_t)hi2c->Instance->RXDR; + + /* Increment Buffer pointer */ + hi2c->pBuffPtr++; + + hi2c->XferSize--; + hi2c->XferCount--; + } + else if ((I2C_CHECK_FLAG(tmpITFlags, I2C_FLAG_TXIS) != RESET) && \ + (I2C_CHECK_IT_SOURCE(ITSources, I2C_IT_TXI) != RESET)) + { + if (hi2c->Memaddress == 0xFFFFFFFFU) + { + /* Write data to TXDR */ + hi2c->Instance->TXDR = *hi2c->pBuffPtr; + + /* Increment Buffer pointer */ + hi2c->pBuffPtr++; + + hi2c->XferSize--; + hi2c->XferCount--; + } + else + { + /* Write LSB part of Memory Address */ + hi2c->Instance->TXDR = hi2c->Memaddress; + + /* Reset Memaddress content */ + hi2c->Memaddress = 0xFFFFFFFFU; + } + } + else if ((I2C_CHECK_FLAG(tmpITFlags, I2C_FLAG_TCR) != RESET) && \ + (I2C_CHECK_IT_SOURCE(ITSources, I2C_IT_TCI) != RESET)) + { + if ((hi2c->XferCount != 0U) && (hi2c->XferSize == 0U)) + { + if (hi2c->XferCount > MAX_NBYTE_SIZE) + { + hi2c->XferSize = MAX_NBYTE_SIZE; + I2C_TransferConfig(hi2c, (uint16_t)hi2c->Devaddress, (uint8_t)hi2c->XferSize, + I2C_RELOAD_MODE, I2C_NO_STARTSTOP); + } + else + { + hi2c->XferSize = hi2c->XferCount; + I2C_TransferConfig(hi2c, (uint16_t)hi2c->Devaddress, (uint8_t)hi2c->XferSize, + I2C_AUTOEND_MODE, I2C_NO_STARTSTOP); + } + } + else + { + /* Wrong size Status regarding TCR flag event */ + /* Call the corresponding callback to inform upper layer of End of Transfer */ + I2C_ITError(hi2c, HAL_I2C_ERROR_SIZE); + } + } + else if ((I2C_CHECK_FLAG(tmpITFlags, I2C_FLAG_TC) != RESET) && \ + (I2C_CHECK_IT_SOURCE(ITSources, I2C_IT_TCI) != RESET)) + { + /* Disable Interrupt related to address step */ + I2C_Disable_IRQ(hi2c, I2C_XFER_TX_IT); + + /* Enable ERR, TC, STOP, NACK and RXI interrupts */ + I2C_Enable_IRQ(hi2c, I2C_XFER_RX_IT); + + if (hi2c->State == HAL_I2C_STATE_BUSY_RX) + { + direction = I2C_GENERATE_START_READ; + } + + if (hi2c->XferCount > MAX_NBYTE_SIZE) + { + hi2c->XferSize = MAX_NBYTE_SIZE; + + /* Set NBYTES to write and reload if hi2c->XferCount > MAX_NBYTE_SIZE and generate RESTART */ + I2C_TransferConfig(hi2c, (uint16_t)hi2c->Devaddress, (uint8_t)hi2c->XferSize, + I2C_RELOAD_MODE, direction); + } + else + { + hi2c->XferSize = hi2c->XferCount; + + /* Set NBYTES to write and generate RESTART */ + I2C_TransferConfig(hi2c, (uint16_t)hi2c->Devaddress, (uint8_t)hi2c->XferSize, + I2C_AUTOEND_MODE, direction); + } + } + else + { + /* Nothing to do */ + } + + if ((I2C_CHECK_FLAG(tmpITFlags, I2C_FLAG_STOPF) != RESET) && \ + (I2C_CHECK_IT_SOURCE(ITSources, I2C_IT_STOPI) != RESET)) + { + /* Call I2C Master complete process */ + I2C_ITMasterCplt(hi2c, tmpITFlags); + } + + /* Process Unlocked */ + __HAL_UNLOCK(hi2c); + + return HAL_OK; +} + +/** + * @brief Interrupt Sub-Routine which handle the Interrupt Flags Slave Mode with Interrupt. + * @param hi2c Pointer to a I2C_HandleTypeDef structure that contains + * the configuration information for the specified I2C. + * @param ITFlags Interrupt flags to handle. + * @param ITSources Interrupt sources enabled. + * @retval HAL status + */ +static HAL_StatusTypeDef I2C_Slave_ISR_IT(struct __I2C_HandleTypeDef *hi2c, uint32_t ITFlags, + uint32_t ITSources) +{ + uint32_t tmpoptions = hi2c->XferOptions; + uint32_t tmpITFlags = ITFlags; + + /* Process locked */ + __HAL_LOCK(hi2c); + + /* Check if STOPF is set */ + if ((I2C_CHECK_FLAG(tmpITFlags, I2C_FLAG_STOPF) != RESET) && \ + (I2C_CHECK_IT_SOURCE(ITSources, I2C_IT_STOPI) != RESET)) + { + /* Call I2C Slave complete process */ + I2C_ITSlaveCplt(hi2c, tmpITFlags); + } + else if ((I2C_CHECK_FLAG(tmpITFlags, I2C_FLAG_AF) != RESET) && \ + (I2C_CHECK_IT_SOURCE(ITSources, I2C_IT_NACKI) != RESET)) + { + /* Check that I2C transfer finished */ + /* if yes, normal use case, a NACK is sent by the MASTER when Transfer is finished */ + /* Mean XferCount == 0*/ + /* So clear Flag NACKF only */ + if (hi2c->XferCount == 0U) + { + if ((hi2c->State == HAL_I2C_STATE_LISTEN) && (tmpoptions == I2C_FIRST_AND_LAST_FRAME)) + /* Same action must be done for (tmpoptions == I2C_LAST_FRAME) which removed for + Warning[Pa134]: left and right operands are identical */ + { + /* Call I2C Listen complete process */ + I2C_ITListenCplt(hi2c, tmpITFlags); + } + else if ((hi2c->State == HAL_I2C_STATE_BUSY_TX_LISTEN) && (tmpoptions != I2C_NO_OPTION_FRAME)) + { + /* Clear NACK Flag */ + __HAL_I2C_CLEAR_FLAG(hi2c, I2C_FLAG_AF); + + /* Flush TX register */ + I2C_Flush_TXDR(hi2c); + + /* Last Byte is Transmitted */ + /* Call I2C Slave Sequential complete process */ + I2C_ITSlaveSeqCplt(hi2c); + } + else + { + /* Clear NACK Flag */ + __HAL_I2C_CLEAR_FLAG(hi2c, I2C_FLAG_AF); + } + } + else + { + /* if no, error use case, a Non-Acknowledge of last Data is generated by the MASTER*/ + /* Clear NACK Flag */ + __HAL_I2C_CLEAR_FLAG(hi2c, I2C_FLAG_AF); + + /* Set ErrorCode corresponding to a Non-Acknowledge */ + hi2c->ErrorCode |= HAL_I2C_ERROR_AF; + + if ((tmpoptions == I2C_FIRST_FRAME) || (tmpoptions == I2C_NEXT_FRAME)) + { + /* Call the corresponding callback to inform upper layer of End of Transfer */ + I2C_ITError(hi2c, hi2c->ErrorCode); + } + } + } + else if ((I2C_CHECK_FLAG(tmpITFlags, I2C_FLAG_RXNE) != RESET) && \ + (I2C_CHECK_IT_SOURCE(ITSources, I2C_IT_RXI) != RESET)) + { + if (hi2c->XferCount > 0U) + { + /* Read data from RXDR */ + *hi2c->pBuffPtr = (uint8_t)hi2c->Instance->RXDR; /* Increment Buffer pointer */ hi2c->pBuffPtr++; @@ -5159,6 +5488,154 @@ static HAL_StatusTypeDef I2C_Master_ISR_DMA(struct __I2C_HandleTypeDef *hi2c, ui return HAL_OK; } +/** + * @brief Interrupt Sub-Routine which handle the Interrupt Flags Memory Mode with DMA. + * @param hi2c Pointer to a I2C_HandleTypeDef structure that contains + * the configuration information for the specified I2C. + * @param ITFlags Interrupt flags to handle. + * @param ITSources Interrupt sources enabled. + * @retval HAL status + */ +static HAL_StatusTypeDef I2C_Mem_ISR_DMA(struct __I2C_HandleTypeDef *hi2c, uint32_t ITFlags, + uint32_t ITSources) +{ + uint32_t direction = I2C_GENERATE_START_WRITE; + + /* Process Locked */ + __HAL_LOCK(hi2c); + + if ((I2C_CHECK_FLAG(ITFlags, I2C_FLAG_AF) != RESET) && \ + (I2C_CHECK_IT_SOURCE(ITSources, I2C_IT_NACKI) != RESET)) + { + /* Clear NACK Flag */ + __HAL_I2C_CLEAR_FLAG(hi2c, I2C_FLAG_AF); + + /* Set corresponding Error Code */ + hi2c->ErrorCode |= HAL_I2C_ERROR_AF; + + /* No need to generate STOP, it is automatically done */ + /* But enable STOP interrupt, to treat it */ + /* Error callback will be send during stop flag treatment */ + I2C_Enable_IRQ(hi2c, I2C_XFER_CPLT_IT); + + /* Flush TX register */ + I2C_Flush_TXDR(hi2c); + } + else if ((I2C_CHECK_FLAG(ITFlags, I2C_FLAG_TXIS) != RESET) && \ + (I2C_CHECK_IT_SOURCE(ITSources, I2C_IT_TXI) != RESET)) + { + /* Write LSB part of Memory Address */ + hi2c->Instance->TXDR = hi2c->Memaddress; + + /* Reset Memaddress content */ + hi2c->Memaddress = 0xFFFFFFFFU; + } + else if ((I2C_CHECK_FLAG(ITFlags, I2C_FLAG_TCR) != RESET) && \ + (I2C_CHECK_IT_SOURCE(ITSources, I2C_IT_TCI) != RESET)) + { + /* Disable Interrupt related to address step */ + I2C_Disable_IRQ(hi2c, I2C_XFER_TX_IT); + + /* Enable only Error interrupt */ + I2C_Enable_IRQ(hi2c, I2C_XFER_ERROR_IT); + + if (hi2c->XferCount != 0U) + { + /* Prepare the new XferSize to transfer */ + if (hi2c->XferCount > MAX_NBYTE_SIZE) + { + hi2c->XferSize = MAX_NBYTE_SIZE; + I2C_TransferConfig(hi2c, (uint16_t)hi2c->Devaddress, (uint8_t)hi2c->XferSize, + I2C_RELOAD_MODE, I2C_NO_STARTSTOP); + } + else + { + hi2c->XferSize = hi2c->XferCount; + I2C_TransferConfig(hi2c, (uint16_t)hi2c->Devaddress, (uint8_t)hi2c->XferSize, + I2C_AUTOEND_MODE, I2C_NO_STARTSTOP); + } + + /* Update XferCount value */ + hi2c->XferCount -= hi2c->XferSize; + + /* Enable DMA Request */ + if (hi2c->State == HAL_I2C_STATE_BUSY_RX) + { + hi2c->Instance->CR1 |= I2C_CR1_RXDMAEN; + } + else + { + hi2c->Instance->CR1 |= I2C_CR1_TXDMAEN; + } + } + else + { + /* Wrong size Status regarding TCR flag event */ + /* Call the corresponding callback to inform upper layer of End of Transfer */ + I2C_ITError(hi2c, HAL_I2C_ERROR_SIZE); + } + } + else if ((I2C_CHECK_FLAG(ITFlags, I2C_FLAG_TC) != RESET) && \ + (I2C_CHECK_IT_SOURCE(ITSources, I2C_IT_TCI) != RESET)) + { + /* Disable Interrupt related to address step */ + I2C_Disable_IRQ(hi2c, I2C_XFER_TX_IT); + + /* Enable only Error and NACK interrupt for data transfer */ + I2C_Enable_IRQ(hi2c, I2C_XFER_ERROR_IT); + + if (hi2c->State == HAL_I2C_STATE_BUSY_RX) + { + direction = I2C_GENERATE_START_READ; + } + + if (hi2c->XferCount > MAX_NBYTE_SIZE) + { + hi2c->XferSize = MAX_NBYTE_SIZE; + + /* Set NBYTES to write and reload if hi2c->XferCount > MAX_NBYTE_SIZE and generate RESTART */ + I2C_TransferConfig(hi2c, (uint16_t)hi2c->Devaddress, (uint8_t)hi2c->XferSize, + I2C_RELOAD_MODE, direction); + } + else + { + hi2c->XferSize = hi2c->XferCount; + + /* Set NBYTES to write and generate RESTART */ + I2C_TransferConfig(hi2c, (uint16_t)hi2c->Devaddress, (uint8_t)hi2c->XferSize, + I2C_AUTOEND_MODE, direction); + } + + /* Update XferCount value */ + hi2c->XferCount -= hi2c->XferSize; + + /* Enable DMA Request */ + if (hi2c->State == HAL_I2C_STATE_BUSY_RX) + { + hi2c->Instance->CR1 |= I2C_CR1_RXDMAEN; + } + else + { + hi2c->Instance->CR1 |= I2C_CR1_TXDMAEN; + } + } + else if ((I2C_CHECK_FLAG(ITFlags, I2C_FLAG_STOPF) != RESET) && \ + (I2C_CHECK_IT_SOURCE(ITSources, I2C_IT_STOPI) != RESET)) + { + /* Call I2C Master complete process */ + I2C_ITMasterCplt(hi2c, ITFlags); + } + else + { + /* Nothing to do */ + } + + /* Process Unlocked */ + __HAL_UNLOCK(hi2c); + + return HAL_OK; +} + /** * @brief Interrupt Sub-Routine which handle the Interrupt Flags Slave Mode with DMA. * @param hi2c Pointer to a I2C_HandleTypeDef structure that contains @@ -5184,9 +5661,8 @@ static HAL_StatusTypeDef I2C_Slave_ISR_DMA(struct __I2C_HandleTypeDef *hi2c, uin /* Call I2C Slave complete process */ I2C_ITSlaveCplt(hi2c, ITFlags); } - - if ((I2C_CHECK_FLAG(ITFlags, I2C_FLAG_AF) != RESET) && \ - (I2C_CHECK_IT_SOURCE(ITSources, I2C_IT_NACKI) != RESET)) + else if ((I2C_CHECK_FLAG(ITFlags, I2C_FLAG_AF) != RESET) && \ + (I2C_CHECK_IT_SOURCE(ITSources, I2C_IT_NACKI) != RESET)) { /* Check that I2C transfer finished */ /* if yes, normal use case, a NACK is sent by the MASTER when Transfer is finished */ @@ -5785,6 +6261,7 @@ static void I2C_ITSlaveCplt(I2C_HandleTypeDef *hi2c, uint32_t ITFlags) { uint32_t tmpcr1value = READ_REG(hi2c->Instance->CR1); uint32_t tmpITFlags = ITFlags; + uint32_t tmpoptions = hi2c->XferOptions; HAL_I2C_StateTypeDef tmpstate = hi2c->State; /* Clear STOP Flag */ @@ -5801,6 +6278,11 @@ static void I2C_ITSlaveCplt(I2C_HandleTypeDef *hi2c, uint32_t ITFlags) I2C_Disable_IRQ(hi2c, I2C_XFER_LISTEN_IT | I2C_XFER_RX_IT); hi2c->PreviousState = I2C_STATE_SLAVE_BUSY_RX; } + else if (tmpstate == HAL_I2C_STATE_LISTEN) + { + I2C_Disable_IRQ(hi2c, I2C_XFER_LISTEN_IT | I2C_XFER_TX_IT | I2C_XFER_RX_IT); + hi2c->PreviousState = I2C_STATE_NONE; + } else { /* Do nothing */ @@ -5867,6 +6349,57 @@ static void I2C_ITSlaveCplt(I2C_HandleTypeDef *hi2c, uint32_t ITFlags) hi2c->ErrorCode |= HAL_I2C_ERROR_AF; } + if ((I2C_CHECK_FLAG(tmpITFlags, I2C_FLAG_AF) != RESET) && \ + (I2C_CHECK_IT_SOURCE(tmpcr1value, I2C_IT_NACKI) != RESET)) + { + /* Check that I2C transfer finished */ + /* if yes, normal use case, a NACK is sent by the MASTER when Transfer is finished */ + /* Mean XferCount == 0*/ + /* So clear Flag NACKF only */ + if (hi2c->XferCount == 0U) + { + if ((hi2c->State == HAL_I2C_STATE_LISTEN) && (tmpoptions == I2C_FIRST_AND_LAST_FRAME)) + /* Same action must be done for (tmpoptions == I2C_LAST_FRAME) which removed for + Warning[Pa134]: left and right operands are identical */ + { + /* Call I2C Listen complete process */ + I2C_ITListenCplt(hi2c, tmpITFlags); + } + else if ((hi2c->State == HAL_I2C_STATE_BUSY_TX_LISTEN) && (tmpoptions != I2C_NO_OPTION_FRAME)) + { + /* Clear NACK Flag */ + __HAL_I2C_CLEAR_FLAG(hi2c, I2C_FLAG_AF); + + /* Flush TX register */ + I2C_Flush_TXDR(hi2c); + + /* Last Byte is Transmitted */ + /* Call I2C Slave Sequential complete process */ + I2C_ITSlaveSeqCplt(hi2c); + } + else + { + /* Clear NACK Flag */ + __HAL_I2C_CLEAR_FLAG(hi2c, I2C_FLAG_AF); + } + } + else + { + /* if no, error use case, a Non-Acknowledge of last Data is generated by the MASTER*/ + /* Clear NACK Flag */ + __HAL_I2C_CLEAR_FLAG(hi2c, I2C_FLAG_AF); + + /* Set ErrorCode corresponding to a Non-Acknowledge */ + hi2c->ErrorCode |= HAL_I2C_ERROR_AF; + + if ((tmpoptions == I2C_FIRST_FRAME) || (tmpoptions == I2C_NEXT_FRAME)) + { + /* Call the corresponding callback to inform upper layer of End of Transfer */ + I2C_ITError(hi2c, hi2c->ErrorCode); + } + } + } + hi2c->Mode = HAL_I2C_MODE_NONE; hi2c->XferISR = NULL; @@ -5994,6 +6527,7 @@ static void I2C_ITListenCplt(I2C_HandleTypeDef *hi2c, uint32_t ITFlags) static void I2C_ITError(I2C_HandleTypeDef *hi2c, uint32_t ErrorCode) { HAL_I2C_StateTypeDef tmpstate = hi2c->State; + uint32_t tmppreviousstate; /* Reset handle parameters */ @@ -6021,18 +6555,36 @@ static void I2C_ITError(I2C_HandleTypeDef *hi2c, uint32_t ErrorCode) /* Disable all interrupts */ I2C_Disable_IRQ(hi2c, I2C_XFER_LISTEN_IT | I2C_XFER_RX_IT | I2C_XFER_TX_IT); + /* Flush TX register */ + I2C_Flush_TXDR(hi2c); + /* If state is an abort treatment on going, don't change state */ /* This change will be do later */ if (hi2c->State != HAL_I2C_STATE_ABORT) { /* Set HAL_I2C_STATE_READY */ hi2c->State = HAL_I2C_STATE_READY; + + /* Check if a STOPF is detected */ + if (__HAL_I2C_GET_FLAG(hi2c, I2C_FLAG_STOPF) == SET) + { + if (__HAL_I2C_GET_FLAG(hi2c, I2C_FLAG_AF) == SET) + { + __HAL_I2C_CLEAR_FLAG(hi2c, I2C_FLAG_AF); + hi2c->ErrorCode |= HAL_I2C_ERROR_AF; + } + + /* Clear STOP Flag */ + __HAL_I2C_CLEAR_FLAG(hi2c, I2C_FLAG_STOPF); + } + } hi2c->XferISR = NULL; } /* Abort DMA TX transfer if any */ tmppreviousstate = hi2c->PreviousState; + if ((hi2c->hdmatx != NULL) && ((tmppreviousstate == I2C_STATE_MASTER_BUSY_TX) || \ (tmppreviousstate == I2C_STATE_SLAVE_BUSY_TX))) { @@ -6207,6 +6759,7 @@ static void I2C_DMAMasterTransmitCplt(DMA_HandleTypeDef *hdma) } } + /** * @brief DMA I2C slave transmit process complete callback. * @param hdma DMA handle @@ -6235,6 +6788,7 @@ static void I2C_DMASlaveTransmitCplt(DMA_HandleTypeDef *hdma) } } + /** * @brief DMA I2C master receive process complete callback. * @param hdma DMA handle @@ -6285,6 +6839,7 @@ static void I2C_DMAMasterReceiveCplt(DMA_HandleTypeDef *hdma) } } + /** * @brief DMA I2C slave receive process complete callback. * @param hdma DMA handle @@ -6313,6 +6868,7 @@ static void I2C_DMASlaveReceiveCplt(DMA_HandleTypeDef *hdma) } } + /** * @brief DMA I2C communication error callback. * @param hdma DMA handle @@ -6330,6 +6886,7 @@ static void I2C_DMAError(DMA_HandleTypeDef *hdma) I2C_ITError(hi2c, HAL_I2C_ERROR_DMA); } + /** * @brief DMA I2C communication abort callback * (To be called at end of DMA Abort procedure). @@ -6354,6 +6911,7 @@ static void I2C_DMAAbort(DMA_HandleTypeDef *hdma) I2C_TreatErrorCallback(hi2c); } + /** * @brief This function handles I2C Communication Timeout. It waits * until a flag is no longer in the specified status. @@ -6370,18 +6928,27 @@ static HAL_StatusTypeDef I2C_WaitOnFlagUntilTimeout(I2C_HandleTypeDef *hi2c, uin { while (__HAL_I2C_GET_FLAG(hi2c, Flag) == Status) { + /* Check if an error is detected */ + if (I2C_IsErrorOccurred(hi2c, Timeout, Tickstart) != HAL_OK) + { + return HAL_ERROR; + } + /* Check for the Timeout */ if (Timeout != HAL_MAX_DELAY) { if (((HAL_GetTick() - Tickstart) > Timeout) || (Timeout == 0U)) { - hi2c->ErrorCode |= HAL_I2C_ERROR_TIMEOUT; - hi2c->State = HAL_I2C_STATE_READY; - hi2c->Mode = HAL_I2C_MODE_NONE; + if ((__HAL_I2C_GET_FLAG(hi2c, Flag) == Status)) + { + hi2c->ErrorCode |= HAL_I2C_ERROR_TIMEOUT; + hi2c->State = HAL_I2C_STATE_READY; + hi2c->Mode = HAL_I2C_MODE_NONE; - /* Process Unlocked */ - __HAL_UNLOCK(hi2c); - return HAL_ERROR; + /* Process Unlocked */ + __HAL_UNLOCK(hi2c); + return HAL_ERROR; + } } } } @@ -6412,14 +6979,17 @@ static HAL_StatusTypeDef I2C_WaitOnTXISFlagUntilTimeout(I2C_HandleTypeDef *hi2c, { if (((HAL_GetTick() - Tickstart) > Timeout) || (Timeout == 0U)) { - hi2c->ErrorCode |= HAL_I2C_ERROR_TIMEOUT; - hi2c->State = HAL_I2C_STATE_READY; - hi2c->Mode = HAL_I2C_MODE_NONE; + if ((__HAL_I2C_GET_FLAG(hi2c, I2C_FLAG_TXIS) == RESET)) + { + hi2c->ErrorCode |= HAL_I2C_ERROR_TIMEOUT; + hi2c->State = HAL_I2C_STATE_READY; + hi2c->Mode = HAL_I2C_MODE_NONE; - /* Process Unlocked */ - __HAL_UNLOCK(hi2c); + /* Process Unlocked */ + __HAL_UNLOCK(hi2c); - return HAL_ERROR; + return HAL_ERROR; + } } } } @@ -6448,14 +7018,17 @@ static HAL_StatusTypeDef I2C_WaitOnSTOPFlagUntilTimeout(I2C_HandleTypeDef *hi2c, /* Check for the Timeout */ if (((HAL_GetTick() - Tickstart) > Timeout) || (Timeout == 0U)) { - hi2c->ErrorCode |= HAL_I2C_ERROR_TIMEOUT; - hi2c->State = HAL_I2C_STATE_READY; - hi2c->Mode = HAL_I2C_MODE_NONE; + if ((__HAL_I2C_GET_FLAG(hi2c, I2C_FLAG_STOPF) == RESET)) + { + hi2c->ErrorCode |= HAL_I2C_ERROR_TIMEOUT; + hi2c->State = HAL_I2C_STATE_READY; + hi2c->Mode = HAL_I2C_MODE_NONE; - /* Process Unlocked */ - __HAL_UNLOCK(hi2c); + /* Process Unlocked */ + __HAL_UNLOCK(hi2c); - return HAL_ERROR; + return HAL_ERROR; + } } } return HAL_OK; @@ -6472,16 +7045,18 @@ static HAL_StatusTypeDef I2C_WaitOnSTOPFlagUntilTimeout(I2C_HandleTypeDef *hi2c, static HAL_StatusTypeDef I2C_WaitOnRXNEFlagUntilTimeout(I2C_HandleTypeDef *hi2c, uint32_t Timeout, uint32_t Tickstart) { - while (__HAL_I2C_GET_FLAG(hi2c, I2C_FLAG_RXNE) == RESET) + HAL_StatusTypeDef status = HAL_OK; + + while ((__HAL_I2C_GET_FLAG(hi2c, I2C_FLAG_RXNE) == RESET) && (status == HAL_OK)) { /* Check if an error is detected */ if (I2C_IsErrorOccurred(hi2c, Timeout, Tickstart) != HAL_OK) { - return HAL_ERROR; + status = HAL_ERROR; } /* Check if a STOPF is detected */ - if (__HAL_I2C_GET_FLAG(hi2c, I2C_FLAG_STOPF) == SET) + if ((__HAL_I2C_GET_FLAG(hi2c, I2C_FLAG_STOPF) == SET) && (status == HAL_OK)) { /* Check if an RXNE is pending */ /* Store Last receive data if any */ @@ -6489,19 +7064,14 @@ static HAL_StatusTypeDef I2C_WaitOnRXNEFlagUntilTimeout(I2C_HandleTypeDef *hi2c, { /* Return HAL_OK */ /* The Reading of data from RXDR will be done in caller function */ - return HAL_OK; + status = HAL_OK; } - else + + /* Check a no-acknowledge have been detected */ + if (__HAL_I2C_GET_FLAG(hi2c, I2C_FLAG_AF) == SET) { - if (__HAL_I2C_GET_FLAG(hi2c, I2C_FLAG_AF) == SET) - { - __HAL_I2C_CLEAR_FLAG(hi2c, I2C_FLAG_AF); - hi2c->ErrorCode = HAL_I2C_ERROR_AF; - } - else - { - hi2c->ErrorCode = HAL_I2C_ERROR_NONE; - } + __HAL_I2C_CLEAR_FLAG(hi2c, I2C_FLAG_AF); + hi2c->ErrorCode = HAL_I2C_ERROR_AF; /* Clear STOP Flag */ __HAL_I2C_CLEAR_FLAG(hi2c, I2C_FLAG_STOPF); @@ -6515,23 +7085,30 @@ static HAL_StatusTypeDef I2C_WaitOnRXNEFlagUntilTimeout(I2C_HandleTypeDef *hi2c, /* Process Unlocked */ __HAL_UNLOCK(hi2c); - return HAL_ERROR; + status = HAL_ERROR; + } + else + { + hi2c->ErrorCode = HAL_I2C_ERROR_NONE; } } /* Check for the Timeout */ - if (((HAL_GetTick() - Tickstart) > Timeout) || (Timeout == 0U)) + if ((((HAL_GetTick() - Tickstart) > Timeout) || (Timeout == 0U)) && (status == HAL_OK)) { - hi2c->ErrorCode |= HAL_I2C_ERROR_TIMEOUT; - hi2c->State = HAL_I2C_STATE_READY; + if ((__HAL_I2C_GET_FLAG(hi2c, I2C_FLAG_RXNE) == RESET)) + { + hi2c->ErrorCode |= HAL_I2C_ERROR_TIMEOUT; + hi2c->State = HAL_I2C_STATE_READY; - /* Process Unlocked */ - __HAL_UNLOCK(hi2c); + /* Process Unlocked */ + __HAL_UNLOCK(hi2c); - return HAL_ERROR; + status = HAL_ERROR; + } } } - return HAL_OK; + return status; } /** @@ -6575,24 +7152,21 @@ static HAL_StatusTypeDef I2C_IsErrorOccurred(I2C_HandleTypeDef *hi2c, uint32_t T { /* Generate Stop */ hi2c->Instance->CR2 |= I2C_CR2_STOP; - + /* Update Tick with new reference */ tickstart = HAL_GetTick(); } - + while (__HAL_I2C_GET_FLAG(hi2c, I2C_FLAG_STOPF) == RESET) { /* Check for the Timeout */ if ((HAL_GetTick() - tickstart) > I2C_TIMEOUT_STOPF) { - hi2c->ErrorCode |= HAL_I2C_ERROR_TIMEOUT; - hi2c->State = HAL_I2C_STATE_READY; - hi2c->Mode = HAL_I2C_MODE_NONE; - - /* Process Unlocked */ - __HAL_UNLOCK(hi2c); - + error_code |= HAL_I2C_ERROR_TIMEOUT; + status = HAL_ERROR; + + break; } } } @@ -6696,14 +7270,14 @@ static void I2C_TransferConfig(I2C_HandleTypeDef *hi2c, uint16_t DevAddress, uin /* Declaration of tmp to prevent undefined behavior of volatile usage */ uint32_t tmp = ((uint32_t)(((uint32_t)DevAddress & I2C_CR2_SADD) | \ - (((uint32_t)Size << I2C_CR2_NBYTES_Pos) & I2C_CR2_NBYTES) | \ - (uint32_t)Mode | (uint32_t)Request) & (~0x80000000U)); + (((uint32_t)Size << I2C_CR2_NBYTES_Pos) & I2C_CR2_NBYTES) | \ + (uint32_t)Mode | (uint32_t)Request) & (~0x80000000U)); /* update CR2 register */ MODIFY_REG(hi2c->Instance->CR2, \ ((I2C_CR2_SADD | I2C_CR2_NBYTES | I2C_CR2_RELOAD | I2C_CR2_AUTOEND | \ (I2C_CR2_RD_WRN & (uint32_t)(Request >> (31U - I2C_CR2_RD_WRN_Pos))) | \ - I2C_CR2_START | I2C_CR2_STOP)), tmp); + I2C_CR2_START | I2C_CR2_STOP)), tmp); } /** @@ -6717,8 +7291,9 @@ static void I2C_Enable_IRQ(I2C_HandleTypeDef *hi2c, uint16_t InterruptRequest) { uint32_t tmpisr = 0U; - if ((hi2c->XferISR == I2C_Master_ISR_DMA) || \ - (hi2c->XferISR == I2C_Slave_ISR_DMA)) + if ((hi2c->XferISR != I2C_Master_ISR_DMA) && \ + (hi2c->XferISR != I2C_Slave_ISR_DMA) && \ + (hi2c->XferISR != I2C_Mem_ISR_DMA)) { if ((InterruptRequest & I2C_XFER_LISTEN_IT) == I2C_XFER_LISTEN_IT) { @@ -6726,6 +7301,18 @@ static void I2C_Enable_IRQ(I2C_HandleTypeDef *hi2c, uint16_t InterruptRequest) tmpisr |= I2C_IT_ADDRI | I2C_IT_STOPI | I2C_IT_NACKI | I2C_IT_ERRI; } + if ((InterruptRequest & I2C_XFER_TX_IT) == I2C_XFER_TX_IT) + { + /* Enable ERR, TC, STOP, NACK and TXI interrupts */ + tmpisr |= I2C_IT_ERRI | I2C_IT_TCI | I2C_IT_STOPI | I2C_IT_NACKI | I2C_IT_TXI; + } + + if ((InterruptRequest & I2C_XFER_RX_IT) == I2C_XFER_RX_IT) + { + /* Enable ERR, TC, STOP, NACK and RXI interrupts */ + tmpisr |= I2C_IT_ERRI | I2C_IT_TCI | I2C_IT_STOPI | I2C_IT_NACKI | I2C_IT_RXI; + } + if (InterruptRequest == I2C_XFER_ERROR_IT) { /* Enable ERR and NACK interrupts */ @@ -6735,39 +7322,46 @@ static void I2C_Enable_IRQ(I2C_HandleTypeDef *hi2c, uint16_t InterruptRequest) if (InterruptRequest == I2C_XFER_CPLT_IT) { /* Enable STOP interrupts */ - tmpisr |= (I2C_IT_STOPI | I2C_IT_TCI); - } - - if (InterruptRequest == I2C_XFER_RELOAD_IT) - { - /* Enable TC interrupts */ - tmpisr |= I2C_IT_TCI; + tmpisr |= I2C_IT_STOPI; } } + else { if ((InterruptRequest & I2C_XFER_LISTEN_IT) == I2C_XFER_LISTEN_IT) { - /* Enable ERR, STOP, NACK, and ADDR interrupts */ + /* Enable ERR, STOP, NACK and ADDR interrupts */ tmpisr |= I2C_IT_ADDRI | I2C_IT_STOPI | I2C_IT_NACKI | I2C_IT_ERRI; } if ((InterruptRequest & I2C_XFER_TX_IT) == I2C_XFER_TX_IT) { - /* Enable ERR, TC, STOP, NACK and RXI interrupts */ + /* Enable ERR, TC, STOP, NACK and TXI interrupts */ tmpisr |= I2C_IT_ERRI | I2C_IT_TCI | I2C_IT_STOPI | I2C_IT_NACKI | I2C_IT_TXI; } if ((InterruptRequest & I2C_XFER_RX_IT) == I2C_XFER_RX_IT) { - /* Enable ERR, TC, STOP, NACK and TXI interrupts */ + /* Enable ERR, TC, STOP, NACK and RXI interrupts */ tmpisr |= I2C_IT_ERRI | I2C_IT_TCI | I2C_IT_STOPI | I2C_IT_NACKI | I2C_IT_RXI; } + if (InterruptRequest == I2C_XFER_ERROR_IT) + { + /* Enable ERR and NACK interrupts */ + tmpisr |= I2C_IT_ERRI | I2C_IT_NACKI; + } + if (InterruptRequest == I2C_XFER_CPLT_IT) { /* Enable STOP interrupts */ - tmpisr |= I2C_IT_STOPI; + tmpisr |= (I2C_IT_STOPI | I2C_IT_TCI); + } + + if (InterruptRequest == I2C_XFER_RELOAD_IT) + { + /* Enable TC interrupts */ + tmpisr |= I2C_IT_TCI; } } diff --git a/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_rcc.c b/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_rcc.c index 8c6fe27a9..3d614256c 100644 --- a/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_rcc.c +++ b/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_rcc.c @@ -632,7 +632,7 @@ HAL_StatusTypeDef HAL_RCC_OscConfig(RCC_OscInitTypeDef *RCC_OscInitStruct) /* Get Start Tick*/ tickstart = HAL_GetTick(); - /* Wait till PLL is ready */ + /* Wait till PLL is disabled */ while (READ_BIT(RCC->CR, RCC_CR_PLLRDY) != 0U) { if ((HAL_GetTick() - tickstart) > PLL_TIMEOUT_VALUE) @@ -672,10 +672,6 @@ HAL_StatusTypeDef HAL_RCC_OscConfig(RCC_OscInitTypeDef *RCC_OscInitStruct) /* Disable the main PLL. */ __HAL_RCC_PLL_DISABLE(); - /* Disable all PLL outputs to save power if no PLLs on */ - MODIFY_REG(RCC->PLLCFGR, RCC_PLLCFGR_PLLSRC, RCC_PLLSOURCE_NONE); - __HAL_RCC_PLLCLKOUT_DISABLE(RCC_PLL_SYSCLK | RCC_PLL_48M1CLK | RCC_PLL_ADCCLK); - /* Get Start Tick*/ tickstart = HAL_GetTick(); @@ -687,6 +683,9 @@ HAL_StatusTypeDef HAL_RCC_OscConfig(RCC_OscInitTypeDef *RCC_OscInitStruct) return HAL_TIMEOUT; } } + + /* Unselect PLL clock source and disable outputs to save power */ + RCC->PLLCFGR &= ~(RCC_PLLCFGR_PLLSRC | RCC_PLL_SYSCLK | RCC_PLL_48M1CLK | RCC_PLL_ADCCLK); } } else @@ -978,7 +977,7 @@ HAL_StatusTypeDef HAL_RCC_ClockConfig(RCC_ClkInitTypeDef *RCC_ClkInitStruct, ui * @arg @ref RCC_MCO1SOURCE_NOCLOCK MCO output disabled, no clock on MCO * @arg @ref RCC_MCO1SOURCE_SYSCLK system clock selected as MCO source * @arg @ref RCC_MCO1SOURCE_HSI HSI clock selected as MCO source - * @arg @ref RCC_MCO1SOURCE_HSE HSE clock selected as MCO sourcee + * @arg @ref RCC_MCO1SOURCE_HSE HSE clock selected as MCO source * @arg @ref RCC_MCO1SOURCE_PLLCLK main PLL clock selected as MCO source * @arg @ref RCC_MCO1SOURCE_LSI LSI clock selected as MCO source * @arg @ref RCC_MCO1SOURCE_LSE LSE clock selected as MCO source diff --git a/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_tim.c b/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_tim.c index 02cbdb4dc..fba3dd20a 100644 --- a/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_tim.c +++ b/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_tim.c @@ -217,11 +217,11 @@ all interrupt callbacks are set to the corresponding weak functions: /** @addtogroup TIM_Private_Functions * @{ */ -static void TIM_OC1_SetConfig(TIM_TypeDef *TIMx, TIM_OC_InitTypeDef *OC_Config); -static void TIM_OC3_SetConfig(TIM_TypeDef *TIMx, TIM_OC_InitTypeDef *OC_Config); -static void TIM_OC4_SetConfig(TIM_TypeDef *TIMx, TIM_OC_InitTypeDef *OC_Config); -static void TIM_OC5_SetConfig(TIM_TypeDef *TIMx, TIM_OC_InitTypeDef *OC_Config); -static void TIM_OC6_SetConfig(TIM_TypeDef *TIMx, TIM_OC_InitTypeDef *OC_Config); +static void TIM_OC1_SetConfig(TIM_TypeDef *TIMx, const TIM_OC_InitTypeDef *OC_Config); +static void TIM_OC3_SetConfig(TIM_TypeDef *TIMx, const TIM_OC_InitTypeDef *OC_Config); +static void TIM_OC4_SetConfig(TIM_TypeDef *TIMx, const TIM_OC_InitTypeDef *OC_Config); +static void TIM_OC5_SetConfig(TIM_TypeDef *TIMx, const TIM_OC_InitTypeDef *OC_Config); +static void TIM_OC6_SetConfig(TIM_TypeDef *TIMx, const TIM_OC_InitTypeDef *OC_Config); static void TIM_TI1_ConfigInputStage(TIM_TypeDef *TIMx, uint32_t TIM_ICPolarity, uint32_t TIM_ICFilter); static void TIM_TI2_SetConfig(TIM_TypeDef *TIMx, uint32_t TIM_ICPolarity, uint32_t TIM_ICSelection, uint32_t TIM_ICFilter); @@ -237,7 +237,7 @@ static void TIM_DMADelayPulseCplt(DMA_HandleTypeDef *hdma); static void TIM_DMATriggerCplt(DMA_HandleTypeDef *hdma); static void TIM_DMATriggerHalfCplt(DMA_HandleTypeDef *hdma); static HAL_StatusTypeDef TIM_SlaveTimer_SetConfig(TIM_HandleTypeDef *htim, - TIM_SlaveConfigTypeDef *sSlaveConfig); + const TIM_SlaveConfigTypeDef *sSlaveConfig); /** * @} */ @@ -290,6 +290,7 @@ HAL_StatusTypeDef HAL_TIM_Base_Init(TIM_HandleTypeDef *htim) assert_param(IS_TIM_INSTANCE(htim->Instance)); assert_param(IS_TIM_COUNTER_MODE(htim->Init.CounterMode)); assert_param(IS_TIM_CLOCKDIVISION_DIV(htim->Init.ClockDivision)); + assert_param(IS_TIM_PERIOD(htim, htim->Init.Period)); assert_param(IS_TIM_AUTORELOAD_PRELOAD(htim->Init.AutoReloadPreload)); if (htim->State == HAL_TIM_STATE_RESET) @@ -537,7 +538,7 @@ HAL_StatusTypeDef HAL_TIM_Base_Stop_IT(TIM_HandleTypeDef *htim) * @param Length The length of data to be transferred from memory to peripheral. * @retval HAL status */ -HAL_StatusTypeDef HAL_TIM_Base_Start_DMA(TIM_HandleTypeDef *htim, uint32_t *pData, uint16_t Length) +HAL_StatusTypeDef HAL_TIM_Base_Start_DMA(TIM_HandleTypeDef *htim, const uint32_t *pData, uint16_t Length) { uint32_t tmpsmcr; @@ -551,7 +552,7 @@ HAL_StatusTypeDef HAL_TIM_Base_Start_DMA(TIM_HandleTypeDef *htim, uint32_t *pDat } else if (htim->State == HAL_TIM_STATE_READY) { - if ((pData == NULL) && (Length > 0U)) + if ((pData == NULL) || (Length == 0U)) { return HAL_ERROR; } @@ -673,6 +674,7 @@ HAL_StatusTypeDef HAL_TIM_OC_Init(TIM_HandleTypeDef *htim) assert_param(IS_TIM_INSTANCE(htim->Instance)); assert_param(IS_TIM_COUNTER_MODE(htim->Init.CounterMode)); assert_param(IS_TIM_CLOCKDIVISION_DIV(htim->Init.ClockDivision)); + assert_param(IS_TIM_PERIOD(htim, htim->Init.Period)); assert_param(IS_TIM_AUTORELOAD_PRELOAD(htim->Init.AutoReloadPreload)); if (htim->State == HAL_TIM_STATE_RESET) @@ -898,7 +900,7 @@ HAL_StatusTypeDef HAL_TIM_OC_Start_IT(TIM_HandleTypeDef *htim, uint32_t Channel) uint32_t tmpsmcr; /* Check the parameters */ - assert_param(IS_TIM_CCX_INSTANCE(htim->Instance, Channel)); + assert_param(IS_TIM_CCX_CHANNEL(htim->Instance, Channel)); /* Check the TIM channel state */ if (TIM_CHANNEL_STATE_GET(htim, Channel) != HAL_TIM_CHANNEL_STATE_READY) @@ -990,7 +992,7 @@ HAL_StatusTypeDef HAL_TIM_OC_Stop_IT(TIM_HandleTypeDef *htim, uint32_t Channel) HAL_StatusTypeDef status = HAL_OK; /* Check the parameters */ - assert_param(IS_TIM_CCX_INSTANCE(htim->Instance, Channel)); + assert_param(IS_TIM_CCX_CHANNEL(htim->Instance, Channel)); switch (Channel) { @@ -1062,13 +1064,14 @@ HAL_StatusTypeDef HAL_TIM_OC_Stop_IT(TIM_HandleTypeDef *htim, uint32_t Channel) * @param Length The length of data to be transferred from memory to TIM peripheral * @retval HAL status */ -HAL_StatusTypeDef HAL_TIM_OC_Start_DMA(TIM_HandleTypeDef *htim, uint32_t Channel, uint32_t *pData, uint16_t Length) +HAL_StatusTypeDef HAL_TIM_OC_Start_DMA(TIM_HandleTypeDef *htim, uint32_t Channel, const uint32_t *pData, + uint16_t Length) { HAL_StatusTypeDef status = HAL_OK; uint32_t tmpsmcr; /* Check the parameters */ - assert_param(IS_TIM_CCX_INSTANCE(htim->Instance, Channel)); + assert_param(IS_TIM_CCX_CHANNEL(htim->Instance, Channel)); /* Set the TIM channel state */ if (TIM_CHANNEL_STATE_GET(htim, Channel) == HAL_TIM_CHANNEL_STATE_BUSY) @@ -1077,7 +1080,7 @@ HAL_StatusTypeDef HAL_TIM_OC_Start_DMA(TIM_HandleTypeDef *htim, uint32_t Channel } else if (TIM_CHANNEL_STATE_GET(htim, Channel) == HAL_TIM_CHANNEL_STATE_READY) { - if ((pData == NULL) && (Length > 0U)) + if ((pData == NULL) || (Length == 0U)) { return HAL_ERROR; } @@ -1230,7 +1233,7 @@ HAL_StatusTypeDef HAL_TIM_OC_Stop_DMA(TIM_HandleTypeDef *htim, uint32_t Channel) HAL_StatusTypeDef status = HAL_OK; /* Check the parameters */ - assert_param(IS_TIM_CCX_INSTANCE(htim->Instance, Channel)); + assert_param(IS_TIM_CCX_CHANNEL(htim->Instance, Channel)); switch (Channel) { @@ -1340,6 +1343,7 @@ HAL_StatusTypeDef HAL_TIM_PWM_Init(TIM_HandleTypeDef *htim) assert_param(IS_TIM_INSTANCE(htim->Instance)); assert_param(IS_TIM_COUNTER_MODE(htim->Init.CounterMode)); assert_param(IS_TIM_CLOCKDIVISION_DIV(htim->Init.ClockDivision)); + assert_param(IS_TIM_PERIOD(htim, htim->Init.Period)); assert_param(IS_TIM_AUTORELOAD_PRELOAD(htim->Init.AutoReloadPreload)); if (htim->State == HAL_TIM_STATE_RESET) @@ -1565,7 +1569,7 @@ HAL_StatusTypeDef HAL_TIM_PWM_Start_IT(TIM_HandleTypeDef *htim, uint32_t Channel uint32_t tmpsmcr; /* Check the parameters */ - assert_param(IS_TIM_CCX_INSTANCE(htim->Instance, Channel)); + assert_param(IS_TIM_CCX_CHANNEL(htim->Instance, Channel)); /* Check the TIM channel state */ if (TIM_CHANNEL_STATE_GET(htim, Channel) != HAL_TIM_CHANNEL_STATE_READY) @@ -1657,7 +1661,7 @@ HAL_StatusTypeDef HAL_TIM_PWM_Stop_IT(TIM_HandleTypeDef *htim, uint32_t Channel) HAL_StatusTypeDef status = HAL_OK; /* Check the parameters */ - assert_param(IS_TIM_CCX_INSTANCE(htim->Instance, Channel)); + assert_param(IS_TIM_CCX_CHANNEL(htim->Instance, Channel)); switch (Channel) { @@ -1729,13 +1733,14 @@ HAL_StatusTypeDef HAL_TIM_PWM_Stop_IT(TIM_HandleTypeDef *htim, uint32_t Channel) * @param Length The length of data to be transferred from memory to TIM peripheral * @retval HAL status */ -HAL_StatusTypeDef HAL_TIM_PWM_Start_DMA(TIM_HandleTypeDef *htim, uint32_t Channel, uint32_t *pData, uint16_t Length) +HAL_StatusTypeDef HAL_TIM_PWM_Start_DMA(TIM_HandleTypeDef *htim, uint32_t Channel, const uint32_t *pData, + uint16_t Length) { HAL_StatusTypeDef status = HAL_OK; uint32_t tmpsmcr; /* Check the parameters */ - assert_param(IS_TIM_CCX_INSTANCE(htim->Instance, Channel)); + assert_param(IS_TIM_CCX_CHANNEL(htim->Instance, Channel)); /* Set the TIM channel state */ if (TIM_CHANNEL_STATE_GET(htim, Channel) == HAL_TIM_CHANNEL_STATE_BUSY) @@ -1744,7 +1749,7 @@ HAL_StatusTypeDef HAL_TIM_PWM_Start_DMA(TIM_HandleTypeDef *htim, uint32_t Channe } else if (TIM_CHANNEL_STATE_GET(htim, Channel) == HAL_TIM_CHANNEL_STATE_READY) { - if ((pData == NULL) && (Length > 0U)) + if ((pData == NULL) || (Length == 0U)) { return HAL_ERROR; } @@ -1896,7 +1901,7 @@ HAL_StatusTypeDef HAL_TIM_PWM_Stop_DMA(TIM_HandleTypeDef *htim, uint32_t Channel HAL_StatusTypeDef status = HAL_OK; /* Check the parameters */ - assert_param(IS_TIM_CCX_INSTANCE(htim->Instance, Channel)); + assert_param(IS_TIM_CCX_CHANNEL(htim->Instance, Channel)); switch (Channel) { @@ -2006,6 +2011,7 @@ HAL_StatusTypeDef HAL_TIM_IC_Init(TIM_HandleTypeDef *htim) assert_param(IS_TIM_INSTANCE(htim->Instance)); assert_param(IS_TIM_COUNTER_MODE(htim->Init.CounterMode)); assert_param(IS_TIM_CLOCKDIVISION_DIV(htim->Init.ClockDivision)); + assert_param(IS_TIM_PERIOD(htim, htim->Init.Period)); assert_param(IS_TIM_AUTORELOAD_PRELOAD(htim->Init.AutoReloadPreload)); if (htim->State == HAL_TIM_STATE_RESET) @@ -2139,7 +2145,7 @@ HAL_StatusTypeDef HAL_TIM_IC_Start(TIM_HandleTypeDef *htim, uint32_t Channel) HAL_TIM_ChannelStateTypeDef complementary_channel_state = TIM_CHANNEL_N_STATE_GET(htim, Channel); /* Check the parameters */ - assert_param(IS_TIM_CCX_INSTANCE(htim->Instance, Channel)); + assert_param(IS_TIM_CCX_CHANNEL(htim->Instance, Channel)); /* Check the TIM channel state */ if ((channel_state != HAL_TIM_CHANNEL_STATE_READY) @@ -2187,7 +2193,7 @@ HAL_StatusTypeDef HAL_TIM_IC_Start(TIM_HandleTypeDef *htim, uint32_t Channel) HAL_StatusTypeDef HAL_TIM_IC_Stop(TIM_HandleTypeDef *htim, uint32_t Channel) { /* Check the parameters */ - assert_param(IS_TIM_CCX_INSTANCE(htim->Instance, Channel)); + assert_param(IS_TIM_CCX_CHANNEL(htim->Instance, Channel)); /* Disable the Input Capture channel */ TIM_CCxChannelCmd(htim->Instance, Channel, TIM_CCx_DISABLE); @@ -2223,7 +2229,7 @@ HAL_StatusTypeDef HAL_TIM_IC_Start_IT(TIM_HandleTypeDef *htim, uint32_t Channel) HAL_TIM_ChannelStateTypeDef complementary_channel_state = TIM_CHANNEL_N_STATE_GET(htim, Channel); /* Check the parameters */ - assert_param(IS_TIM_CCX_INSTANCE(htim->Instance, Channel)); + assert_param(IS_TIM_CCX_CHANNEL(htim->Instance, Channel)); /* Check the TIM channel state */ if ((channel_state != HAL_TIM_CHANNEL_STATE_READY) @@ -2311,7 +2317,7 @@ HAL_StatusTypeDef HAL_TIM_IC_Stop_IT(TIM_HandleTypeDef *htim, uint32_t Channel) HAL_StatusTypeDef status = HAL_OK; /* Check the parameters */ - assert_param(IS_TIM_CCX_INSTANCE(htim->Instance, Channel)); + assert_param(IS_TIM_CCX_CHANNEL(htim->Instance, Channel)); switch (Channel) { @@ -2387,7 +2393,7 @@ HAL_StatusTypeDef HAL_TIM_IC_Start_DMA(TIM_HandleTypeDef *htim, uint32_t Channel HAL_TIM_ChannelStateTypeDef complementary_channel_state = TIM_CHANNEL_N_STATE_GET(htim, Channel); /* Check the parameters */ - assert_param(IS_TIM_CCX_INSTANCE(htim->Instance, Channel)); + assert_param(IS_TIM_CCX_CHANNEL(htim->Instance, Channel)); assert_param(IS_TIM_DMA_CC_INSTANCE(htim->Instance)); /* Set the TIM channel state */ @@ -2399,7 +2405,7 @@ HAL_StatusTypeDef HAL_TIM_IC_Start_DMA(TIM_HandleTypeDef *htim, uint32_t Channel else if ((channel_state == HAL_TIM_CHANNEL_STATE_READY) && (complementary_channel_state == HAL_TIM_CHANNEL_STATE_READY)) { - if ((pData == NULL) && (Length > 0U)) + if ((pData == NULL) || (Length == 0U)) { return HAL_ERROR; } @@ -2542,7 +2548,7 @@ HAL_StatusTypeDef HAL_TIM_IC_Stop_DMA(TIM_HandleTypeDef *htim, uint32_t Channel) HAL_StatusTypeDef status = HAL_OK; /* Check the parameters */ - assert_param(IS_TIM_CCX_INSTANCE(htim->Instance, Channel)); + assert_param(IS_TIM_CCX_CHANNEL(htim->Instance, Channel)); assert_param(IS_TIM_DMA_CC_INSTANCE(htim->Instance)); /* Disable the Input Capture channel */ @@ -2655,6 +2661,7 @@ HAL_StatusTypeDef HAL_TIM_OnePulse_Init(TIM_HandleTypeDef *htim, uint32_t OnePul assert_param(IS_TIM_COUNTER_MODE(htim->Init.CounterMode)); assert_param(IS_TIM_CLOCKDIVISION_DIV(htim->Init.ClockDivision)); assert_param(IS_TIM_OPM_MODE(OnePulseMode)); + assert_param(IS_TIM_PERIOD(htim, htim->Init.Period)); assert_param(IS_TIM_AUTORELOAD_PRELOAD(htim->Init.AutoReloadPreload)); if (htim->State == HAL_TIM_STATE_RESET) @@ -3032,7 +3039,7 @@ HAL_StatusTypeDef HAL_TIM_OnePulse_Stop_IT(TIM_HandleTypeDef *htim, uint32_t Out * @param sConfig TIM Encoder Interface configuration structure * @retval HAL status */ -HAL_StatusTypeDef HAL_TIM_Encoder_Init(TIM_HandleTypeDef *htim, TIM_Encoder_InitTypeDef *sConfig) +HAL_StatusTypeDef HAL_TIM_Encoder_Init(TIM_HandleTypeDef *htim, const TIM_Encoder_InitTypeDef *sConfig) { uint32_t tmpsmcr; uint32_t tmpccmr1; @@ -3058,6 +3065,7 @@ HAL_StatusTypeDef HAL_TIM_Encoder_Init(TIM_HandleTypeDef *htim, TIM_Encoder_Ini assert_param(IS_TIM_IC_PRESCALER(sConfig->IC2Prescaler)); assert_param(IS_TIM_IC_FILTER(sConfig->IC1Filter)); assert_param(IS_TIM_IC_FILTER(sConfig->IC2Filter)); + assert_param(IS_TIM_PERIOD(htim, htim->Init.Period)); if (htim->State == HAL_TIM_STATE_RESET) { @@ -3567,7 +3575,7 @@ HAL_StatusTypeDef HAL_TIM_Encoder_Start_DMA(TIM_HandleTypeDef *htim, uint32_t Ch else if ((channel_1_state == HAL_TIM_CHANNEL_STATE_READY) && (complementary_channel_1_state == HAL_TIM_CHANNEL_STATE_READY)) { - if ((pData1 == NULL) && (Length > 0U)) + if ((pData1 == NULL) || (Length == 0U)) { return HAL_ERROR; } @@ -3592,7 +3600,7 @@ HAL_StatusTypeDef HAL_TIM_Encoder_Start_DMA(TIM_HandleTypeDef *htim, uint32_t Ch else if ((channel_2_state == HAL_TIM_CHANNEL_STATE_READY) && (complementary_channel_2_state == HAL_TIM_CHANNEL_STATE_READY)) { - if ((pData2 == NULL) && (Length > 0U)) + if ((pData2 == NULL) || (Length == 0U)) { return HAL_ERROR; } @@ -3621,7 +3629,7 @@ HAL_StatusTypeDef HAL_TIM_Encoder_Start_DMA(TIM_HandleTypeDef *htim, uint32_t Ch && (complementary_channel_1_state == HAL_TIM_CHANNEL_STATE_READY) && (complementary_channel_2_state == HAL_TIM_CHANNEL_STATE_READY)) { - if ((((pData1 == NULL) || (pData2 == NULL))) && (Length > 0U)) + if ((((pData1 == NULL) || (pData2 == NULL))) || (Length == 0U)) { return HAL_ERROR; } @@ -3837,13 +3845,16 @@ HAL_StatusTypeDef HAL_TIM_Encoder_Stop_DMA(TIM_HandleTypeDef *htim, uint32_t Cha */ void HAL_TIM_IRQHandler(TIM_HandleTypeDef *htim) { + uint32_t itsource = htim->Instance->DIER; + uint32_t itflag = htim->Instance->SR; + /* Capture compare 1 event */ - if (__HAL_TIM_GET_FLAG(htim, TIM_FLAG_CC1) != RESET) + if ((itflag & (TIM_FLAG_CC1)) == (TIM_FLAG_CC1)) { - if (__HAL_TIM_GET_IT_SOURCE(htim, TIM_IT_CC1) != RESET) + if ((itsource & (TIM_IT_CC1)) == (TIM_IT_CC1)) { { - __HAL_TIM_CLEAR_IT(htim, TIM_IT_CC1); + __HAL_TIM_CLEAR_FLAG(htim, TIM_FLAG_CC1); htim->Channel = HAL_TIM_ACTIVE_CHANNEL_1; /* Input capture event */ @@ -3871,11 +3882,11 @@ void HAL_TIM_IRQHandler(TIM_HandleTypeDef *htim) } } /* Capture compare 2 event */ - if (__HAL_TIM_GET_FLAG(htim, TIM_FLAG_CC2) != RESET) + if ((itflag & (TIM_FLAG_CC2)) == (TIM_FLAG_CC2)) { - if (__HAL_TIM_GET_IT_SOURCE(htim, TIM_IT_CC2) != RESET) + if ((itsource & (TIM_IT_CC2)) == (TIM_IT_CC2)) { - __HAL_TIM_CLEAR_IT(htim, TIM_IT_CC2); + __HAL_TIM_CLEAR_FLAG(htim, TIM_FLAG_CC2); htim->Channel = HAL_TIM_ACTIVE_CHANNEL_2; /* Input capture event */ if ((htim->Instance->CCMR1 & TIM_CCMR1_CC2S) != 0x00U) @@ -3901,11 +3912,11 @@ void HAL_TIM_IRQHandler(TIM_HandleTypeDef *htim) } } /* Capture compare 3 event */ - if (__HAL_TIM_GET_FLAG(htim, TIM_FLAG_CC3) != RESET) + if ((itflag & (TIM_FLAG_CC3)) == (TIM_FLAG_CC3)) { - if (__HAL_TIM_GET_IT_SOURCE(htim, TIM_IT_CC3) != RESET) + if ((itsource & (TIM_IT_CC3)) == (TIM_IT_CC3)) { - __HAL_TIM_CLEAR_IT(htim, TIM_IT_CC3); + __HAL_TIM_CLEAR_FLAG(htim, TIM_FLAG_CC3); htim->Channel = HAL_TIM_ACTIVE_CHANNEL_3; /* Input capture event */ if ((htim->Instance->CCMR2 & TIM_CCMR2_CC3S) != 0x00U) @@ -3931,11 +3942,11 @@ void HAL_TIM_IRQHandler(TIM_HandleTypeDef *htim) } } /* Capture compare 4 event */ - if (__HAL_TIM_GET_FLAG(htim, TIM_FLAG_CC4) != RESET) + if ((itflag & (TIM_FLAG_CC4)) == (TIM_FLAG_CC4)) { - if (__HAL_TIM_GET_IT_SOURCE(htim, TIM_IT_CC4) != RESET) + if ((itsource & (TIM_IT_CC4)) == (TIM_IT_CC4)) { - __HAL_TIM_CLEAR_IT(htim, TIM_IT_CC4); + __HAL_TIM_CLEAR_FLAG(htim, TIM_FLAG_CC4); htim->Channel = HAL_TIM_ACTIVE_CHANNEL_4; /* Input capture event */ if ((htim->Instance->CCMR2 & TIM_CCMR2_CC4S) != 0x00U) @@ -3961,11 +3972,11 @@ void HAL_TIM_IRQHandler(TIM_HandleTypeDef *htim) } } /* TIM Update event */ - if (__HAL_TIM_GET_FLAG(htim, TIM_FLAG_UPDATE) != RESET) + if ((itflag & (TIM_FLAG_UPDATE)) == (TIM_FLAG_UPDATE)) { - if (__HAL_TIM_GET_IT_SOURCE(htim, TIM_IT_UPDATE) != RESET) + if ((itsource & (TIM_IT_UPDATE)) == (TIM_IT_UPDATE)) { - __HAL_TIM_CLEAR_IT(htim, TIM_IT_UPDATE); + __HAL_TIM_CLEAR_FLAG(htim, TIM_FLAG_UPDATE); #if (USE_HAL_TIM_REGISTER_CALLBACKS == 1) htim->PeriodElapsedCallback(htim); #else @@ -3974,11 +3985,12 @@ void HAL_TIM_IRQHandler(TIM_HandleTypeDef *htim) } } /* TIM Break input event */ - if (__HAL_TIM_GET_FLAG(htim, TIM_FLAG_BREAK) != RESET) + if (((itflag & (TIM_FLAG_BREAK)) == (TIM_FLAG_BREAK)) || \ + ((itflag & (TIM_FLAG_SYSTEM_BREAK)) == (TIM_FLAG_SYSTEM_BREAK))) { - if (__HAL_TIM_GET_IT_SOURCE(htim, TIM_IT_BREAK) != RESET) + if ((itsource & (TIM_IT_BREAK)) == (TIM_IT_BREAK)) { - __HAL_TIM_CLEAR_IT(htim, TIM_IT_BREAK); + __HAL_TIM_CLEAR_FLAG(htim, TIM_FLAG_BREAK | TIM_FLAG_SYSTEM_BREAK); #if (USE_HAL_TIM_REGISTER_CALLBACKS == 1) htim->BreakCallback(htim); #else @@ -3987,9 +3999,9 @@ void HAL_TIM_IRQHandler(TIM_HandleTypeDef *htim) } } /* TIM Break2 input event */ - if (__HAL_TIM_GET_FLAG(htim, TIM_FLAG_BREAK2) != RESET) + if ((itflag & (TIM_FLAG_BREAK2)) == (TIM_FLAG_BREAK2)) { - if (__HAL_TIM_GET_IT_SOURCE(htim, TIM_IT_BREAK) != RESET) + if ((itsource & (TIM_IT_BREAK)) == (TIM_IT_BREAK)) { __HAL_TIM_CLEAR_FLAG(htim, TIM_FLAG_BREAK2); #if (USE_HAL_TIM_REGISTER_CALLBACKS == 1) @@ -4000,11 +4012,11 @@ void HAL_TIM_IRQHandler(TIM_HandleTypeDef *htim) } } /* TIM Trigger detection event */ - if (__HAL_TIM_GET_FLAG(htim, TIM_FLAG_TRIGGER) != RESET) + if ((itflag & (TIM_FLAG_TRIGGER)) == (TIM_FLAG_TRIGGER)) { - if (__HAL_TIM_GET_IT_SOURCE(htim, TIM_IT_TRIGGER) != RESET) + if ((itsource & (TIM_IT_TRIGGER)) == (TIM_IT_TRIGGER)) { - __HAL_TIM_CLEAR_IT(htim, TIM_IT_TRIGGER); + __HAL_TIM_CLEAR_FLAG(htim, TIM_FLAG_TRIGGER); #if (USE_HAL_TIM_REGISTER_CALLBACKS == 1) htim->TriggerCallback(htim); #else @@ -4013,11 +4025,11 @@ void HAL_TIM_IRQHandler(TIM_HandleTypeDef *htim) } } /* TIM commutation event */ - if (__HAL_TIM_GET_FLAG(htim, TIM_FLAG_COM) != RESET) + if ((itflag & (TIM_FLAG_COM)) == (TIM_FLAG_COM)) { - if (__HAL_TIM_GET_IT_SOURCE(htim, TIM_IT_COM) != RESET) + if ((itsource & (TIM_IT_COM)) == (TIM_IT_COM)) { - __HAL_TIM_CLEAR_IT(htim, TIM_FLAG_COM); + __HAL_TIM_CLEAR_FLAG(htim, TIM_FLAG_COM); #if (USE_HAL_TIM_REGISTER_CALLBACKS == 1) htim->CommutationCallback(htim); #else @@ -4026,11 +4038,11 @@ void HAL_TIM_IRQHandler(TIM_HandleTypeDef *htim) } } /* TIM Encoder index event */ - if (__HAL_TIM_GET_FLAG(htim, TIM_FLAG_IDX) != RESET) + if ((itflag & (TIM_FLAG_IDX)) == (TIM_FLAG_IDX)) { - if (__HAL_TIM_GET_IT_SOURCE(htim, TIM_IT_IDX) != RESET) + if ((itsource & (TIM_IT_IDX)) == (TIM_IT_IDX)) { - __HAL_TIM_CLEAR_IT(htim, TIM_FLAG_IDX); + __HAL_TIM_CLEAR_FLAG(htim, TIM_FLAG_IDX); #if (USE_HAL_TIM_REGISTER_CALLBACKS == 1) htim->EncoderIndexCallback(htim); #else @@ -4039,11 +4051,11 @@ void HAL_TIM_IRQHandler(TIM_HandleTypeDef *htim) } } /* TIM Direction change event */ - if (__HAL_TIM_GET_FLAG(htim, TIM_FLAG_DIR) != RESET) + if ((itflag & (TIM_FLAG_DIR)) == (TIM_FLAG_DIR)) { - if (__HAL_TIM_GET_IT_SOURCE(htim, TIM_IT_DIR) != RESET) + if ((itsource & (TIM_IT_DIR)) == (TIM_IT_DIR)) { - __HAL_TIM_CLEAR_IT(htim, TIM_FLAG_DIR); + __HAL_TIM_CLEAR_FLAG(htim, TIM_FLAG_DIR); #if (USE_HAL_TIM_REGISTER_CALLBACKS == 1) htim->DirectionChangeCallback(htim); #else @@ -4052,11 +4064,11 @@ void HAL_TIM_IRQHandler(TIM_HandleTypeDef *htim) } } /* TIM Index error event */ - if (__HAL_TIM_GET_FLAG(htim, TIM_FLAG_IERR) != RESET) + if ((itflag & (TIM_FLAG_IERR)) == (TIM_FLAG_IERR)) { - if (__HAL_TIM_GET_IT_SOURCE(htim, TIM_IT_IERR) != RESET) + if ((itsource & (TIM_IT_IERR)) == (TIM_IT_IERR)) { - __HAL_TIM_CLEAR_IT(htim, TIM_FLAG_IERR); + __HAL_TIM_CLEAR_FLAG(htim, TIM_FLAG_IERR); #if (USE_HAL_TIM_REGISTER_CALLBACKS == 1) htim->IndexErrorCallback(htim); #else @@ -4065,11 +4077,11 @@ void HAL_TIM_IRQHandler(TIM_HandleTypeDef *htim) } } /* TIM Transition error event */ - if (__HAL_TIM_GET_FLAG(htim, TIM_FLAG_TERR) != RESET) + if ((itflag & (TIM_FLAG_TERR)) == (TIM_FLAG_TERR)) { - if (__HAL_TIM_GET_IT_SOURCE(htim, TIM_IT_TERR) != RESET) + if ((itsource & (TIM_IT_TERR)) == (TIM_IT_TERR)) { - __HAL_TIM_CLEAR_IT(htim, TIM_FLAG_TERR); + __HAL_TIM_CLEAR_FLAG(htim, TIM_FLAG_TERR); #if (USE_HAL_TIM_REGISTER_CALLBACKS == 1) htim->TransitionErrorCallback(htim); #else @@ -4118,7 +4130,7 @@ void HAL_TIM_IRQHandler(TIM_HandleTypeDef *htim) * @retval HAL status */ HAL_StatusTypeDef HAL_TIM_OC_ConfigChannel(TIM_HandleTypeDef *htim, - TIM_OC_InitTypeDef *sConfig, + const TIM_OC_InitTypeDef *sConfig, uint32_t Channel) { HAL_StatusTypeDef status = HAL_OK; @@ -4216,7 +4228,7 @@ HAL_StatusTypeDef HAL_TIM_OC_ConfigChannel(TIM_HandleTypeDef *htim, * @arg TIM_CHANNEL_4: TIM Channel 4 selected * @retval HAL status */ -HAL_StatusTypeDef HAL_TIM_IC_ConfigChannel(TIM_HandleTypeDef *htim, TIM_IC_InitTypeDef *sConfig, uint32_t Channel) +HAL_StatusTypeDef HAL_TIM_IC_ConfigChannel(TIM_HandleTypeDef *htim, const TIM_IC_InitTypeDef *sConfig, uint32_t Channel) { HAL_StatusTypeDef status = HAL_OK; @@ -4318,7 +4330,7 @@ HAL_StatusTypeDef HAL_TIM_IC_ConfigChannel(TIM_HandleTypeDef *htim, TIM_IC_InitT * @retval HAL status */ HAL_StatusTypeDef HAL_TIM_PWM_ConfigChannel(TIM_HandleTypeDef *htim, - TIM_OC_InitTypeDef *sConfig, + const TIM_OC_InitTypeDef *sConfig, uint32_t Channel) { HAL_StatusTypeDef status = HAL_OK; @@ -4623,7 +4635,8 @@ HAL_StatusTypeDef HAL_TIM_OnePulse_ConfigChannel(TIM_HandleTypeDef *htim, TIM_O * @retval HAL status */ HAL_StatusTypeDef HAL_TIM_DMABurst_WriteStart(TIM_HandleTypeDef *htim, uint32_t BurstBaseAddress, - uint32_t BurstRequestSrc, uint32_t *BurstBuffer, uint32_t BurstLength) + uint32_t BurstRequestSrc, const uint32_t *BurstBuffer, + uint32_t BurstLength) { HAL_StatusTypeDef status; @@ -4684,7 +4697,7 @@ HAL_StatusTypeDef HAL_TIM_DMABurst_WriteStart(TIM_HandleTypeDef *htim, uint32_t * @retval HAL status */ HAL_StatusTypeDef HAL_TIM_DMABurst_MultiWriteStart(TIM_HandleTypeDef *htim, uint32_t BurstBaseAddress, - uint32_t BurstRequestSrc, uint32_t *BurstBuffer, + uint32_t BurstRequestSrc, const uint32_t *BurstBuffer, uint32_t BurstLength, uint32_t DataLength) { HAL_StatusTypeDef status = HAL_OK; @@ -5345,7 +5358,7 @@ HAL_StatusTypeDef HAL_TIM_GenerateEvent(TIM_HandleTypeDef *htim, uint32_t EventS * @retval HAL status */ HAL_StatusTypeDef HAL_TIM_ConfigOCrefClear(TIM_HandleTypeDef *htim, - TIM_ClearInputConfigTypeDef *sClearInputConfig, + const TIM_ClearInputConfigTypeDef *sClearInputConfig, uint32_t Channel) { HAL_StatusTypeDef status = HAL_OK; @@ -5396,10 +5409,10 @@ HAL_StatusTypeDef HAL_TIM_ConfigOCrefClear(TIM_HandleTypeDef *htim, { /* Clear the OCREF clear selection bit */ CLEAR_BIT(htim->Instance->SMCR, TIM_SMCR_OCCS); - - /* Clear TIM1_AF2_OCRSEL (reset value) */ - MODIFY_REG(htim->Instance->AF2, TIMx_AF2_OCRSEL, sClearInputConfig->ClearInputSource); } + + /* Set the clear input source */ + MODIFY_REG(htim->Instance->AF2, TIMx_AF2_OCRSEL, sClearInputConfig->ClearInputSource); break; } @@ -5546,7 +5559,7 @@ HAL_StatusTypeDef HAL_TIM_ConfigOCrefClear(TIM_HandleTypeDef *htim, * contains the clock source information for the TIM peripheral. * @retval HAL status */ -HAL_StatusTypeDef HAL_TIM_ConfigClockSource(TIM_HandleTypeDef *htim, TIM_ClockConfigTypeDef *sClockSourceConfig) +HAL_StatusTypeDef HAL_TIM_ConfigClockSource(TIM_HandleTypeDef *htim, const TIM_ClockConfigTypeDef *sClockSourceConfig) { HAL_StatusTypeDef status = HAL_OK; uint32_t tmpsmcr; @@ -5746,7 +5759,7 @@ HAL_StatusTypeDef HAL_TIM_ConfigTI1Input(TIM_HandleTypeDef *htim, uint32_t TI1_S * (Disable, Reset, Gated, Trigger, External clock mode 1, Reset + Trigger, Gated + Reset). * @retval HAL status */ -HAL_StatusTypeDef HAL_TIM_SlaveConfigSynchro(TIM_HandleTypeDef *htim, TIM_SlaveConfigTypeDef *sSlaveConfig) +HAL_StatusTypeDef HAL_TIM_SlaveConfigSynchro(TIM_HandleTypeDef *htim, const TIM_SlaveConfigTypeDef *sSlaveConfig) { /* Check the parameters */ assert_param(IS_TIM_SLAVE_INSTANCE(htim->Instance)); @@ -5787,7 +5800,7 @@ HAL_StatusTypeDef HAL_TIM_SlaveConfigSynchro(TIM_HandleTypeDef *htim, TIM_SlaveC * @retval HAL status */ HAL_StatusTypeDef HAL_TIM_SlaveConfigSynchro_IT(TIM_HandleTypeDef *htim, - TIM_SlaveConfigTypeDef *sSlaveConfig) + const TIM_SlaveConfigTypeDef *sSlaveConfig) { /* Check the parameters */ assert_param(IS_TIM_SLAVE_INSTANCE(htim->Instance)); @@ -5829,7 +5842,7 @@ HAL_StatusTypeDef HAL_TIM_SlaveConfigSynchro_IT(TIM_HandleTypeDef *htim, * @arg TIM_CHANNEL_4: TIM Channel 4 selected * @retval Captured value */ -uint32_t HAL_TIM_ReadCapturedValue(TIM_HandleTypeDef *htim, uint32_t Channel) +uint32_t HAL_TIM_ReadCapturedValue(const TIM_HandleTypeDef *htim, uint32_t Channel) { uint32_t tmpreg = 0U; @@ -6112,8 +6125,6 @@ HAL_StatusTypeDef HAL_TIM_RegisterCallback(TIM_HandleTypeDef *htim, HAL_TIM_Call { return HAL_ERROR; } - /* Process locked */ - __HAL_LOCK(htim); if (htim->State == HAL_TIM_STATE_READY) { @@ -6325,9 +6336,6 @@ HAL_StatusTypeDef HAL_TIM_RegisterCallback(TIM_HandleTypeDef *htim, HAL_TIM_Call status = HAL_ERROR; } - /* Release Lock */ - __HAL_UNLOCK(htim); - return status; } @@ -6375,9 +6383,6 @@ HAL_StatusTypeDef HAL_TIM_UnRegisterCallback(TIM_HandleTypeDef *htim, HAL_TIM_Ca { HAL_StatusTypeDef status = HAL_OK; - /* Process locked */ - __HAL_LOCK(htim); - if (htim->State == HAL_TIM_STATE_READY) { switch (CallbackID) @@ -6634,9 +6639,6 @@ HAL_StatusTypeDef HAL_TIM_UnRegisterCallback(TIM_HandleTypeDef *htim, HAL_TIM_Ca status = HAL_ERROR; } - /* Release Lock */ - __HAL_UNLOCK(htim); - return status; } #endif /* USE_HAL_TIM_REGISTER_CALLBACKS */ @@ -6665,7 +6667,7 @@ HAL_StatusTypeDef HAL_TIM_UnRegisterCallback(TIM_HandleTypeDef *htim, HAL_TIM_Ca * @param htim TIM Base handle * @retval HAL state */ -HAL_TIM_StateTypeDef HAL_TIM_Base_GetState(TIM_HandleTypeDef *htim) +HAL_TIM_StateTypeDef HAL_TIM_Base_GetState(const TIM_HandleTypeDef *htim) { return htim->State; } @@ -6675,7 +6677,7 @@ HAL_TIM_StateTypeDef HAL_TIM_Base_GetState(TIM_HandleTypeDef *htim) * @param htim TIM Output Compare handle * @retval HAL state */ -HAL_TIM_StateTypeDef HAL_TIM_OC_GetState(TIM_HandleTypeDef *htim) +HAL_TIM_StateTypeDef HAL_TIM_OC_GetState(const TIM_HandleTypeDef *htim) { return htim->State; } @@ -6685,7 +6687,7 @@ HAL_TIM_StateTypeDef HAL_TIM_OC_GetState(TIM_HandleTypeDef *htim) * @param htim TIM handle * @retval HAL state */ -HAL_TIM_StateTypeDef HAL_TIM_PWM_GetState(TIM_HandleTypeDef *htim) +HAL_TIM_StateTypeDef HAL_TIM_PWM_GetState(const TIM_HandleTypeDef *htim) { return htim->State; } @@ -6695,7 +6697,7 @@ HAL_TIM_StateTypeDef HAL_TIM_PWM_GetState(TIM_HandleTypeDef *htim) * @param htim TIM IC handle * @retval HAL state */ -HAL_TIM_StateTypeDef HAL_TIM_IC_GetState(TIM_HandleTypeDef *htim) +HAL_TIM_StateTypeDef HAL_TIM_IC_GetState(const TIM_HandleTypeDef *htim) { return htim->State; } @@ -6705,7 +6707,7 @@ HAL_TIM_StateTypeDef HAL_TIM_IC_GetState(TIM_HandleTypeDef *htim) * @param htim TIM OPM handle * @retval HAL state */ -HAL_TIM_StateTypeDef HAL_TIM_OnePulse_GetState(TIM_HandleTypeDef *htim) +HAL_TIM_StateTypeDef HAL_TIM_OnePulse_GetState(const TIM_HandleTypeDef *htim) { return htim->State; } @@ -6715,7 +6717,7 @@ HAL_TIM_StateTypeDef HAL_TIM_OnePulse_GetState(TIM_HandleTypeDef *htim) * @param htim TIM Encoder Interface handle * @retval HAL state */ -HAL_TIM_StateTypeDef HAL_TIM_Encoder_GetState(TIM_HandleTypeDef *htim) +HAL_TIM_StateTypeDef HAL_TIM_Encoder_GetState(const TIM_HandleTypeDef *htim) { return htim->State; } @@ -6725,7 +6727,7 @@ HAL_TIM_StateTypeDef HAL_TIM_Encoder_GetState(TIM_HandleTypeDef *htim) * @param htim TIM handle * @retval Active channel */ -HAL_TIM_ActiveChannel HAL_TIM_GetActiveChannel(TIM_HandleTypeDef *htim) +HAL_TIM_ActiveChannel HAL_TIM_GetActiveChannel(const TIM_HandleTypeDef *htim) { return htim->Channel; } @@ -6743,7 +6745,7 @@ HAL_TIM_ActiveChannel HAL_TIM_GetActiveChannel(TIM_HandleTypeDef *htim) * @arg TIM_CHANNEL_6: TIM Channel 6 * @retval TIM Channel state */ -HAL_TIM_ChannelStateTypeDef HAL_TIM_GetChannelState(TIM_HandleTypeDef *htim, uint32_t Channel) +HAL_TIM_ChannelStateTypeDef HAL_TIM_GetChannelState(const TIM_HandleTypeDef *htim, uint32_t Channel) { HAL_TIM_ChannelStateTypeDef channel_state; @@ -6760,7 +6762,7 @@ HAL_TIM_ChannelStateTypeDef HAL_TIM_GetChannelState(TIM_HandleTypeDef *htim, ui * @param htim TIM handle * @retval DMA burst state */ -HAL_TIM_DMABurstStateTypeDef HAL_TIM_DMABurstState(TIM_HandleTypeDef *htim) +HAL_TIM_DMABurstStateTypeDef HAL_TIM_DMABurstState(const TIM_HandleTypeDef *htim) { /* Check the parameters */ assert_param(IS_TIM_DMABURST_INSTANCE(htim->Instance)); @@ -7103,7 +7105,7 @@ static void TIM_DMATriggerHalfCplt(DMA_HandleTypeDef *hdma) * @param Structure TIM Base configuration structure * @retval None */ -void TIM_Base_SetConfig(TIM_TypeDef *TIMx, TIM_Base_InitTypeDef *Structure) +void TIM_Base_SetConfig(TIM_TypeDef *TIMx, const TIM_Base_InitTypeDef *Structure) { uint32_t tmpcr1; tmpcr1 = TIMx->CR1; @@ -7143,6 +7145,13 @@ void TIM_Base_SetConfig(TIM_TypeDef *TIMx, TIM_Base_InitTypeDef *Structure) /* Generate an update event to reload the Prescaler and the repetition counter (only for advanced timer) value immediately */ TIMx->EGR = TIM_EGR_UG; + + /* Check if the update flag is set after the Update Generation, if so clear the UIF flag */ + if (HAL_IS_BIT_SET(TIMx->SR, TIM_FLAG_UPDATE)) + { + /* Clear the update flag */ + CLEAR_BIT(TIMx->SR, TIM_FLAG_UPDATE); + } } /** @@ -7151,17 +7160,18 @@ void TIM_Base_SetConfig(TIM_TypeDef *TIMx, TIM_Base_InitTypeDef *Structure) * @param OC_Config The output configuration structure * @retval None */ -static void TIM_OC1_SetConfig(TIM_TypeDef *TIMx, TIM_OC_InitTypeDef *OC_Config) +static void TIM_OC1_SetConfig(TIM_TypeDef *TIMx, const TIM_OC_InitTypeDef *OC_Config) { uint32_t tmpccmrx; uint32_t tmpccer; uint32_t tmpcr2; + /* Get the TIMx CCER register value */ + tmpccer = TIMx->CCER; + /* Disable the Channel 1: Reset the CC1E Bit */ TIMx->CCER &= ~TIM_CCER_CC1E; - /* Get the TIMx CCER register value */ - tmpccer = TIMx->CCER; /* Get the TIMx CR2 register value */ tmpcr2 = TIMx->CR2; @@ -7226,17 +7236,18 @@ static void TIM_OC1_SetConfig(TIM_TypeDef *TIMx, TIM_OC_InitTypeDef *OC_Config) * @param OC_Config The output configuration structure * @retval None */ -void TIM_OC2_SetConfig(TIM_TypeDef *TIMx, TIM_OC_InitTypeDef *OC_Config) +void TIM_OC2_SetConfig(TIM_TypeDef *TIMx, const TIM_OC_InitTypeDef *OC_Config) { uint32_t tmpccmrx; uint32_t tmpccer; uint32_t tmpcr2; + /* Get the TIMx CCER register value */ + tmpccer = TIMx->CCER; + /* Disable the Channel 2: Reset the CC2E Bit */ TIMx->CCER &= ~TIM_CCER_CC2E; - /* Get the TIMx CCER register value */ - tmpccer = TIMx->CCER; /* Get the TIMx CR2 register value */ tmpcr2 = TIMx->CR2; @@ -7265,7 +7276,6 @@ void TIM_OC2_SetConfig(TIM_TypeDef *TIMx, TIM_OC_InitTypeDef *OC_Config) tmpccer |= (OC_Config->OCNPolarity << 4U); /* Reset the Output N State */ tmpccer &= ~TIM_CCER_CC2NE; - } if (IS_TIM_BREAK_INSTANCE(TIMx)) @@ -7302,17 +7312,18 @@ void TIM_OC2_SetConfig(TIM_TypeDef *TIMx, TIM_OC_InitTypeDef *OC_Config) * @param OC_Config The output configuration structure * @retval None */ -static void TIM_OC3_SetConfig(TIM_TypeDef *TIMx, TIM_OC_InitTypeDef *OC_Config) +static void TIM_OC3_SetConfig(TIM_TypeDef *TIMx, const TIM_OC_InitTypeDef *OC_Config) { uint32_t tmpccmrx; uint32_t tmpccer; uint32_t tmpcr2; + /* Get the TIMx CCER register value */ + tmpccer = TIMx->CCER; + /* Disable the Channel 3: Reset the CC2E Bit */ TIMx->CCER &= ~TIM_CCER_CC3E; - /* Get the TIMx CCER register value */ - tmpccer = TIMx->CCER; /* Get the TIMx CR2 register value */ tmpcr2 = TIMx->CR2; @@ -7376,17 +7387,18 @@ static void TIM_OC3_SetConfig(TIM_TypeDef *TIMx, TIM_OC_InitTypeDef *OC_Config) * @param OC_Config The output configuration structure * @retval None */ -static void TIM_OC4_SetConfig(TIM_TypeDef *TIMx, TIM_OC_InitTypeDef *OC_Config) +static void TIM_OC4_SetConfig(TIM_TypeDef *TIMx, const TIM_OC_InitTypeDef *OC_Config) { uint32_t tmpccmrx; uint32_t tmpccer; uint32_t tmpcr2; + /* Get the TIMx CCER register value */ + tmpccer = TIMx->CCER; + /* Disable the Channel 4: Reset the CC4E Bit */ TIMx->CCER &= ~TIM_CCER_CC4E; - /* Get the TIMx CCER register value */ - tmpccer = TIMx->CCER; /* Get the TIMx CR2 register value */ tmpcr2 = TIMx->CR2; @@ -7454,17 +7466,18 @@ static void TIM_OC4_SetConfig(TIM_TypeDef *TIMx, TIM_OC_InitTypeDef *OC_Config) * @retval None */ static void TIM_OC5_SetConfig(TIM_TypeDef *TIMx, - TIM_OC_InitTypeDef *OC_Config) + const TIM_OC_InitTypeDef *OC_Config) { uint32_t tmpccmrx; uint32_t tmpccer; uint32_t tmpcr2; + /* Get the TIMx CCER register value */ + tmpccer = TIMx->CCER; + /* Disable the output: Reset the CCxE Bit */ TIMx->CCER &= ~TIM_CCER_CC5E; - /* Get the TIMx CCER register value */ - tmpccer = TIMx->CCER; /* Get the TIMx CR2 register value */ tmpcr2 = TIMx->CR2; /* Get the TIMx CCMR1 register value */ @@ -7507,17 +7520,18 @@ static void TIM_OC5_SetConfig(TIM_TypeDef *TIMx, * @retval None */ static void TIM_OC6_SetConfig(TIM_TypeDef *TIMx, - TIM_OC_InitTypeDef *OC_Config) + const TIM_OC_InitTypeDef *OC_Config) { uint32_t tmpccmrx; uint32_t tmpccer; uint32_t tmpcr2; + /* Get the TIMx CCER register value */ + tmpccer = TIMx->CCER; + /* Disable the output: Reset the CCxE Bit */ TIMx->CCER &= ~TIM_CCER_CC6E; - /* Get the TIMx CCER register value */ - tmpccer = TIMx->CCER; /* Get the TIMx CR2 register value */ tmpcr2 = TIMx->CR2; /* Get the TIMx CCMR1 register value */ @@ -7561,7 +7575,7 @@ static void TIM_OC6_SetConfig(TIM_TypeDef *TIMx, * @retval None */ static HAL_StatusTypeDef TIM_SlaveTimer_SetConfig(TIM_HandleTypeDef *htim, - TIM_SlaveConfigTypeDef *sSlaveConfig) + const TIM_SlaveConfigTypeDef *sSlaveConfig) { HAL_StatusTypeDef status = HAL_OK; uint32_t tmpsmcr; @@ -7716,9 +7730,9 @@ void TIM_TI1_SetConfig(TIM_TypeDef *TIMx, uint32_t TIM_ICPolarity, uint32_t TIM_ uint32_t tmpccer; /* Disable the Channel 1: Reset the CC1E Bit */ + tmpccer = TIMx->CCER; TIMx->CCER &= ~TIM_CCER_CC1E; tmpccmr1 = TIMx->CCMR1; - tmpccer = TIMx->CCER; /* Select the Input */ if (IS_TIM_CC2_INSTANCE(TIMx) != RESET) @@ -7806,9 +7820,9 @@ static void TIM_TI2_SetConfig(TIM_TypeDef *TIMx, uint32_t TIM_ICPolarity, uint32 uint32_t tmpccer; /* Disable the Channel 2: Reset the CC2E Bit */ + tmpccer = TIMx->CCER; TIMx->CCER &= ~TIM_CCER_CC2E; tmpccmr1 = TIMx->CCMR1; - tmpccer = TIMx->CCER; /* Select the Input */ tmpccmr1 &= ~TIM_CCMR1_CC2S; @@ -7845,9 +7859,9 @@ static void TIM_TI2_ConfigInputStage(TIM_TypeDef *TIMx, uint32_t TIM_ICPolarity, uint32_t tmpccer; /* Disable the Channel 2: Reset the CC2E Bit */ + tmpccer = TIMx->CCER; TIMx->CCER &= ~TIM_CCER_CC2E; tmpccmr1 = TIMx->CCMR1; - tmpccer = TIMx->CCER; /* Set the filter */ tmpccmr1 &= ~TIM_CCMR1_IC2F; @@ -7889,9 +7903,9 @@ static void TIM_TI3_SetConfig(TIM_TypeDef *TIMx, uint32_t TIM_ICPolarity, uint32 uint32_t tmpccer; /* Disable the Channel 3: Reset the CC3E Bit */ + tmpccer = TIMx->CCER; TIMx->CCER &= ~TIM_CCER_CC3E; tmpccmr2 = TIMx->CCMR2; - tmpccer = TIMx->CCER; /* Select the Input */ tmpccmr2 &= ~TIM_CCMR2_CC3S; @@ -7937,9 +7951,9 @@ static void TIM_TI4_SetConfig(TIM_TypeDef *TIMx, uint32_t TIM_ICPolarity, uint32 uint32_t tmpccer; /* Disable the Channel 4: Reset the CC4E Bit */ + tmpccer = TIMx->CCER; TIMx->CCER &= ~TIM_CCER_CC4E; tmpccmr2 = TIMx->CCMR2; - tmpccer = TIMx->CCER; /* Select the Input */ tmpccmr2 &= ~TIM_CCMR2_CC4S; @@ -7967,10 +7981,6 @@ static void TIM_TI4_SetConfig(TIM_TypeDef *TIMx, uint32_t TIM_ICPolarity, uint32 * @arg TIM_TS_ITR1: Internal Trigger 1 * @arg TIM_TS_ITR2: Internal Trigger 2 * @arg TIM_TS_ITR3: Internal Trigger 3 - * @arg TIM_TS_TI1F_ED: TI1 Edge Detector - * @arg TIM_TS_TI1FP1: Filtered Timer Input 1 - * @arg TIM_TS_TI2FP2: Filtered Timer Input 2 - * @arg TIM_TS_ETRF: External Trigger input * @arg TIM_TS_ITR4: Internal Trigger 4 (*) * @arg TIM_TS_ITR5: Internal Trigger 5 * @arg TIM_TS_ITR6: Internal Trigger 6 @@ -7979,6 +7989,10 @@ static void TIM_TI4_SetConfig(TIM_TypeDef *TIMx, uint32_t TIM_ICPolarity, uint32 * @arg TIM_TS_ITR9: Internal Trigger 9 (*) * @arg TIM_TS_ITR10: Internal Trigger 10 * @arg TIM_TS_ITR11: Internal Trigger 11 + * @arg TIM_TS_TI1F_ED: TI1 Edge Detector + * @arg TIM_TS_TI1FP1: Filtered Timer Input 1 + * @arg TIM_TS_TI2FP2: Filtered Timer Input 2 + * @arg TIM_TS_ETRF: External Trigger input * * (*) Value not defined in all devices. * diff --git a/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_tim_ex.c b/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_tim_ex.c index 0ffa8d191..20b1afcc3 100644 --- a/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_tim_ex.c +++ b/src/esw/fw/bdcmc/Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_tim_ex.c @@ -154,7 +154,7 @@ static void TIM_CCxNChannelCmd(TIM_TypeDef *TIMx, uint32_t Channel, uint32_t Cha * @param sConfig TIM Hall Sensor configuration structure * @retval HAL status */ -HAL_StatusTypeDef HAL_TIMEx_HallSensor_Init(TIM_HandleTypeDef *htim, TIM_HallSensor_InitTypeDef *sConfig) +HAL_StatusTypeDef HAL_TIMEx_HallSensor_Init(TIM_HandleTypeDef *htim, const TIM_HallSensor_InitTypeDef *sConfig) { TIM_OC_InitTypeDef OC_Config; @@ -170,6 +170,7 @@ HAL_StatusTypeDef HAL_TIMEx_HallSensor_Init(TIM_HandleTypeDef *htim, TIM_HallSen assert_param(IS_TIM_CLOCKDIVISION_DIV(htim->Init.ClockDivision)); assert_param(IS_TIM_AUTORELOAD_PRELOAD(htim->Init.AutoReloadPreload)); assert_param(IS_TIM_IC_POLARITY(sConfig->IC1Polarity)); + assert_param(IS_TIM_PERIOD(htim, htim->Init.Period)); assert_param(IS_TIM_IC_PRESCALER(sConfig->IC1Prescaler)); assert_param(IS_TIM_IC_FILTER(sConfig->IC1Filter)); @@ -520,7 +521,7 @@ HAL_StatusTypeDef HAL_TIMEx_HallSensor_Start_DMA(TIM_HandleTypeDef *htim, uint32 else if ((channel_1_state == HAL_TIM_CHANNEL_STATE_READY) && (complementary_channel_1_state == HAL_TIM_CHANNEL_STATE_READY)) { - if ((pData == NULL) && (Length > 0U)) + if ((pData == NULL) || (Length == 0U)) { return HAL_ERROR; } @@ -871,7 +872,7 @@ HAL_StatusTypeDef HAL_TIMEx_OCN_Stop_IT(TIM_HandleTypeDef *htim, uint32_t Channe /* Disable the TIM Break interrupt (only if no more channel is active) */ tmpccer = htim->Instance->CCER; - if ((tmpccer & (TIM_CCER_CC1NE | TIM_CCER_CC2NE | TIM_CCER_CC3NE | TIM_CCER_CC4NE)) == (uint32_t)RESET) + if ((tmpccer & TIM_CCER_CCxNE_MASK) == (uint32_t)RESET) { __HAL_TIM_DISABLE_IT(htim, TIM_IT_BREAK); } @@ -904,7 +905,8 @@ HAL_StatusTypeDef HAL_TIMEx_OCN_Stop_IT(TIM_HandleTypeDef *htim, uint32_t Channe * @param Length The length of data to be transferred from memory to TIM peripheral * @retval HAL status */ -HAL_StatusTypeDef HAL_TIMEx_OCN_Start_DMA(TIM_HandleTypeDef *htim, uint32_t Channel, uint32_t *pData, uint16_t Length) +HAL_StatusTypeDef HAL_TIMEx_OCN_Start_DMA(TIM_HandleTypeDef *htim, uint32_t Channel, const uint32_t *pData, + uint16_t Length) { HAL_StatusTypeDef status = HAL_OK; uint32_t tmpsmcr; @@ -919,7 +921,7 @@ HAL_StatusTypeDef HAL_TIMEx_OCN_Start_DMA(TIM_HandleTypeDef *htim, uint32_t Chan } else if (TIM_CHANNEL_N_STATE_GET(htim, Channel) == HAL_TIM_CHANNEL_STATE_READY) { - if ((pData == NULL) && (Length > 0U)) + if ((pData == NULL) || (Length == 0U)) { return HAL_ERROR; } @@ -1147,17 +1149,6 @@ HAL_StatusTypeDef HAL_TIMEx_OCN_Stop_DMA(TIM_HandleTypeDef *htim, uint32_t Chann (+) Stop the Complementary PWM and disable interrupts. (+) Start the Complementary PWM and enable DMA transfers. (+) Stop the Complementary PWM and disable DMA transfers. - (+) Start the Complementary Input Capture measurement. - (+) Stop the Complementary Input Capture. - (+) Start the Complementary Input Capture and enable interrupts. - (+) Stop the Complementary Input Capture and disable interrupts. - (+) Start the Complementary Input Capture and enable DMA transfers. - (+) Stop the Complementary Input Capture and disable DMA transfers. - (+) Start the Complementary One Pulse generation. - (+) Stop the Complementary One Pulse. - (+) Start the Complementary One Pulse and enable interrupts. - (+) Stop the Complementary One Pulse and disable interrupts. - @endverbatim * @{ */ @@ -1401,7 +1392,7 @@ HAL_StatusTypeDef HAL_TIMEx_PWMN_Stop_IT(TIM_HandleTypeDef *htim, uint32_t Chann /* Disable the TIM Break interrupt (only if no more channel is active) */ tmpccer = htim->Instance->CCER; - if ((tmpccer & (TIM_CCER_CC1NE | TIM_CCER_CC2NE | TIM_CCER_CC3NE | TIM_CCER_CC4NE)) == (uint32_t)RESET) + if ((tmpccer & TIM_CCER_CCxNE_MASK) == (uint32_t)RESET) { __HAL_TIM_DISABLE_IT(htim, TIM_IT_BREAK); } @@ -1434,7 +1425,8 @@ HAL_StatusTypeDef HAL_TIMEx_PWMN_Stop_IT(TIM_HandleTypeDef *htim, uint32_t Chann * @param Length The length of data to be transferred from memory to TIM peripheral * @retval HAL status */ -HAL_StatusTypeDef HAL_TIMEx_PWMN_Start_DMA(TIM_HandleTypeDef *htim, uint32_t Channel, uint32_t *pData, uint16_t Length) +HAL_StatusTypeDef HAL_TIMEx_PWMN_Start_DMA(TIM_HandleTypeDef *htim, uint32_t Channel, const uint32_t *pData, + uint16_t Length) { HAL_StatusTypeDef status = HAL_OK; uint32_t tmpsmcr; @@ -1449,7 +1441,7 @@ HAL_StatusTypeDef HAL_TIMEx_PWMN_Start_DMA(TIM_HandleTypeDef *htim, uint32_t Cha } else if (TIM_CHANNEL_N_STATE_GET(htim, Channel) == HAL_TIM_CHANNEL_STATE_READY) { - if ((pData == NULL) && (Length > 0U)) + if ((pData == NULL) || (Length == 0U)) { return HAL_ERROR; } @@ -2186,7 +2178,7 @@ HAL_StatusTypeDef HAL_TIMEx_ConfigCommutEvent_DMA(TIM_HandleTypeDef *htim, uint3 * @retval HAL status */ HAL_StatusTypeDef HAL_TIMEx_MasterConfigSynchronization(TIM_HandleTypeDef *htim, - TIM_MasterConfigTypeDef *sMasterConfig) + const TIM_MasterConfigTypeDef *sMasterConfig) { uint32_t tmpcr2; uint32_t tmpsmcr; @@ -2259,7 +2251,7 @@ HAL_StatusTypeDef HAL_TIMEx_MasterConfigSynchronization(TIM_HandleTypeDef *htim, * @retval HAL status */ HAL_StatusTypeDef HAL_TIMEx_ConfigBreakDeadTime(TIM_HandleTypeDef *htim, - TIM_BreakDeadTimeConfigTypeDef *sBreakDeadTimeConfig) + const TIM_BreakDeadTimeConfigTypeDef *sBreakDeadTimeConfig) { /* Keep this variable initialized to 0 as it is used to configure BDTR register */ uint32_t tmpbdtr = 0U; @@ -2274,6 +2266,7 @@ HAL_StatusTypeDef HAL_TIMEx_ConfigBreakDeadTime(TIM_HandleTypeDef *htim, assert_param(IS_TIM_BREAK_POLARITY(sBreakDeadTimeConfig->BreakPolarity)); assert_param(IS_TIM_BREAK_FILTER(sBreakDeadTimeConfig->BreakFilter)); assert_param(IS_TIM_AUTOMATIC_OUTPUT_STATE(sBreakDeadTimeConfig->AutomaticOutput)); + assert_param(IS_TIM_BREAK_AFMODE(sBreakDeadTimeConfig->BreakAFMode)); /* Check input state */ __HAL_LOCK(htim); @@ -2290,15 +2283,7 @@ HAL_StatusTypeDef HAL_TIMEx_ConfigBreakDeadTime(TIM_HandleTypeDef *htim, MODIFY_REG(tmpbdtr, TIM_BDTR_BKP, sBreakDeadTimeConfig->BreakPolarity); MODIFY_REG(tmpbdtr, TIM_BDTR_AOE, sBreakDeadTimeConfig->AutomaticOutput); MODIFY_REG(tmpbdtr, TIM_BDTR_BKF, (sBreakDeadTimeConfig->BreakFilter << TIM_BDTR_BKF_Pos)); - - if (IS_TIM_ADVANCED_INSTANCE(htim->Instance)) - { - /* Check the parameters */ - assert_param(IS_TIM_BREAK_AFMODE(sBreakDeadTimeConfig->BreakAFMode)); - - /* Set BREAK AF mode */ - MODIFY_REG(tmpbdtr, TIM_BDTR_BKBID, sBreakDeadTimeConfig->BreakAFMode); - } + MODIFY_REG(tmpbdtr, TIM_BDTR_BKBID, sBreakDeadTimeConfig->BreakAFMode); if (IS_TIM_BKIN2_INSTANCE(htim->Instance)) { @@ -2306,20 +2291,13 @@ HAL_StatusTypeDef HAL_TIMEx_ConfigBreakDeadTime(TIM_HandleTypeDef *htim, assert_param(IS_TIM_BREAK2_STATE(sBreakDeadTimeConfig->Break2State)); assert_param(IS_TIM_BREAK2_POLARITY(sBreakDeadTimeConfig->Break2Polarity)); assert_param(IS_TIM_BREAK_FILTER(sBreakDeadTimeConfig->Break2Filter)); + assert_param(IS_TIM_BREAK2_AFMODE(sBreakDeadTimeConfig->Break2AFMode)); /* Set the BREAK2 input related BDTR bits */ MODIFY_REG(tmpbdtr, TIM_BDTR_BK2F, (sBreakDeadTimeConfig->Break2Filter << TIM_BDTR_BK2F_Pos)); MODIFY_REG(tmpbdtr, TIM_BDTR_BK2E, sBreakDeadTimeConfig->Break2State); MODIFY_REG(tmpbdtr, TIM_BDTR_BK2P, sBreakDeadTimeConfig->Break2Polarity); - - if (IS_TIM_ADVANCED_INSTANCE(htim->Instance)) - { - /* Check the parameters */ - assert_param(IS_TIM_BREAK2_AFMODE(sBreakDeadTimeConfig->Break2AFMode)); - - /* Set BREAK2 AF mode */ - MODIFY_REG(tmpbdtr, TIM_BDTR_BK2BID, sBreakDeadTimeConfig->Break2AFMode); - } + MODIFY_REG(tmpbdtr, TIM_BDTR_BK2BID, sBreakDeadTimeConfig->Break2AFMode); } /* Set TIMx_BDTR */ @@ -2342,8 +2320,7 @@ HAL_StatusTypeDef HAL_TIMEx_ConfigBreakDeadTime(TIM_HandleTypeDef *htim, */ HAL_StatusTypeDef HAL_TIMEx_ConfigBreakInput(TIM_HandleTypeDef *htim, uint32_t BreakInput, - TIMEx_BreakInputConfigTypeDef *sBreakInputConfig) - + const TIMEx_BreakInputConfigTypeDef *sBreakInputConfig) { HAL_StatusTypeDef status = HAL_OK; uint32_t tmporx; @@ -2679,7 +2656,7 @@ HAL_StatusTypeDef HAL_TIMEx_RemapConfig(TIM_HandleTypeDef *htim, uint32_t Remap) * * @arg TIM_TIM3_TI3_GPIO: TIM3 TI3 is connected to GPIO * @arg TIM_TIM3_TI3_COMP3: TIM3 TI3 is connected to COMP3 output - + * * For TIM4 this parameter can be one of the following values: * @arg TIM_TIM4_TI1_GPIO: TIM4 TI1 is connected to GPIO * @arg TIM_TIM4_TI1_COMP1: TIM4 TI1 is connected to COMP1 output @@ -2877,7 +2854,7 @@ HAL_StatusTypeDef HAL_TIMEx_DisarmBreakInput(TIM_HandleTypeDef *htim, uint32_t B uint32_t tmpbdtr; /* Check the parameters */ - assert_param(IS_TIM_ADVANCED_INSTANCE(htim->Instance)); + assert_param(IS_TIM_BREAK_INSTANCE(htim->Instance)); assert_param(IS_TIM_BREAKINPUT(BreakInput)); switch (BreakInput) @@ -2894,7 +2871,6 @@ HAL_StatusTypeDef HAL_TIMEx_DisarmBreakInput(TIM_HandleTypeDef *htim, uint32_t B } break; } - case TIM_BREAKINPUT_BRK2: { /* Check initial conditions */ @@ -2926,13 +2902,13 @@ HAL_StatusTypeDef HAL_TIMEx_DisarmBreakInput(TIM_HandleTypeDef *htim, uint32_t B * @note Break input is automatically armed as soon as MOE bit is set. * @retval HAL status */ -HAL_StatusTypeDef HAL_TIMEx_ReArmBreakInput(TIM_HandleTypeDef *htim, uint32_t BreakInput) +HAL_StatusTypeDef HAL_TIMEx_ReArmBreakInput(const TIM_HandleTypeDef *htim, uint32_t BreakInput) { HAL_StatusTypeDef status = HAL_OK; uint32_t tickstart; /* Check the parameters */ - assert_param(IS_TIM_ADVANCED_INSTANCE(htim->Instance)); + assert_param(IS_TIM_BREAK_INSTANCE(htim->Instance)); assert_param(IS_TIM_BREAKINPUT(BreakInput)); switch (BreakInput) @@ -3344,7 +3320,7 @@ HAL_StatusTypeDef HAL_TIMEx_DisableEncoderFirstIndex(TIM_HandleTypeDef *htim) */ /** - * @brief Hall commutation changed callback in non-blocking mode + * @brief Commutation callback in non-blocking mode * @param htim TIM handle * @retval None */ @@ -3358,7 +3334,7 @@ __weak void HAL_TIMEx_CommutCallback(TIM_HandleTypeDef *htim) */ } /** - * @brief Hall commutation changed half complete callback in non-blocking mode + * @brief Commutation half complete callback in non-blocking mode * @param htim TIM handle * @retval None */ @@ -3373,7 +3349,7 @@ __weak void HAL_TIMEx_CommutHalfCpltCallback(TIM_HandleTypeDef *htim) } /** - * @brief Hall Break detection callback in non-blocking mode + * @brief Break detection callback in non-blocking mode * @param htim TIM handle * @retval None */ @@ -3388,7 +3364,7 @@ __weak void HAL_TIMEx_BreakCallback(TIM_HandleTypeDef *htim) } /** - * @brief Hall Break2 detection callback in non blocking mode + * @brief Break2 detection callback in non blocking mode * @param htim: TIM handle * @retval None */ @@ -3486,7 +3462,7 @@ __weak void HAL_TIMEx_TransitionErrorCallback(TIM_HandleTypeDef *htim) * @param htim TIM Hall Sensor handle * @retval HAL state */ -HAL_TIM_StateTypeDef HAL_TIMEx_HallSensor_GetState(TIM_HandleTypeDef *htim) +HAL_TIM_StateTypeDef HAL_TIMEx_HallSensor_GetState(const TIM_HandleTypeDef *htim) { return htim->State; } @@ -3502,7 +3478,7 @@ HAL_TIM_StateTypeDef HAL_TIMEx_HallSensor_GetState(TIM_HandleTypeDef *htim) * @arg TIM_CHANNEL_4: TIM Channel 4 * @retval TIM Complementary channel state */ -HAL_TIM_ChannelStateTypeDef HAL_TIMEx_GetChannelNState(TIM_HandleTypeDef *htim, uint32_t ChannelN) +HAL_TIM_ChannelStateTypeDef HAL_TIMEx_GetChannelNState(const TIM_HandleTypeDef *htim, uint32_t ChannelN) { HAL_TIM_ChannelStateTypeDef channel_state; @@ -3648,6 +3624,11 @@ static void TIM_DMAErrorCCxN(DMA_HandleTypeDef *hdma) htim->Channel = HAL_TIM_ACTIVE_CHANNEL_3; TIM_CHANNEL_N_STATE_SET(htim, TIM_CHANNEL_3, HAL_TIM_CHANNEL_STATE_READY); } + else if (hdma == htim->hdma[TIM_DMA_ID_CC4]) + { + htim->Channel = HAL_TIM_ACTIVE_CHANNEL_4; + TIM_CHANNEL_N_STATE_SET(htim, TIM_CHANNEL_4, HAL_TIM_CHANNEL_STATE_READY); + } else { /* nothing to do */ @@ -3679,13 +3660,13 @@ static void TIM_CCxNChannelCmd(TIM_TypeDef *TIMx, uint32_t Channel, uint32_t Cha { uint32_t tmp; - tmp = TIM_CCER_CC1NE << (Channel & 0x1FU); /* 0x1FU = 31 bits max shift */ + tmp = TIM_CCER_CC1NE << (Channel & 0xFU); /* 0xFU = 15 bits max shift */ /* Reset the CCxNE Bit */ TIMx->CCER &= ~tmp; /* Set or reset the CCxNE Bit */ - TIMx->CCER |= (uint32_t)(ChannelNState << (Channel & 0x1FU)); /* 0x1FU = 31 bits max shift */ + TIMx->CCER |= (uint32_t)(ChannelNState << (Channel & 0xFU)); /* 0xFU = 15 bits max shift */ } /** * @} diff --git a/src/esw/fw/bdcmc/bdcmc.ioc b/src/esw/fw/bdcmc/bdcmc.ioc index c94c505ae..df4ab75e2 100644 --- a/src/esw/fw/bdcmc/bdcmc.ioc +++ b/src/esw/fw/bdcmc/bdcmc.ioc @@ -1,353 +1,359 @@ -#MicroXplorer Configuration settings - do not modify -CAD.formats= -CAD.pinconfig= -CAD.provider= -Dma.I2C1_RX.0.Direction=DMA_PERIPH_TO_MEMORY -Dma.I2C1_RX.0.EventEnable=DISABLE -Dma.I2C1_RX.0.Instance=DMA1_Channel1 -Dma.I2C1_RX.0.MemDataAlignment=DMA_MDATAALIGN_BYTE -Dma.I2C1_RX.0.MemInc=DMA_MINC_ENABLE -Dma.I2C1_RX.0.Mode=DMA_NORMAL -Dma.I2C1_RX.0.PeriphDataAlignment=DMA_PDATAALIGN_BYTE -Dma.I2C1_RX.0.PeriphInc=DMA_PINC_DISABLE -Dma.I2C1_RX.0.Polarity=HAL_DMAMUX_REQ_GEN_RISING -Dma.I2C1_RX.0.Priority=DMA_PRIORITY_MEDIUM -Dma.I2C1_RX.0.RequestNumber=1 -Dma.I2C1_RX.0.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,SignalID,Polarity,RequestNumber,SyncSignalID,SyncPolarity,SyncEnable,EventEnable,SyncRequestNumber -Dma.I2C1_RX.0.SignalID=NONE -Dma.I2C1_RX.0.SyncEnable=DISABLE -Dma.I2C1_RX.0.SyncPolarity=HAL_DMAMUX_SYNC_NO_EVENT -Dma.I2C1_RX.0.SyncRequestNumber=1 -Dma.I2C1_RX.0.SyncSignalID=NONE -Dma.I2C1_TX.1.Direction=DMA_MEMORY_TO_PERIPH -Dma.I2C1_TX.1.EventEnable=DISABLE -Dma.I2C1_TX.1.Instance=DMA1_Channel2 -Dma.I2C1_TX.1.MemDataAlignment=DMA_MDATAALIGN_BYTE -Dma.I2C1_TX.1.MemInc=DMA_MINC_ENABLE -Dma.I2C1_TX.1.Mode=DMA_NORMAL -Dma.I2C1_TX.1.PeriphDataAlignment=DMA_PDATAALIGN_BYTE -Dma.I2C1_TX.1.PeriphInc=DMA_PINC_DISABLE -Dma.I2C1_TX.1.Polarity=HAL_DMAMUX_REQ_GEN_RISING -Dma.I2C1_TX.1.Priority=DMA_PRIORITY_MEDIUM -Dma.I2C1_TX.1.RequestNumber=1 -Dma.I2C1_TX.1.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,SignalID,Polarity,RequestNumber,SyncSignalID,SyncPolarity,SyncEnable,EventEnable,SyncRequestNumber -Dma.I2C1_TX.1.SignalID=NONE -Dma.I2C1_TX.1.SyncEnable=DISABLE -Dma.I2C1_TX.1.SyncPolarity=HAL_DMAMUX_SYNC_NO_EVENT -Dma.I2C1_TX.1.SyncRequestNumber=1 -Dma.I2C1_TX.1.SyncSignalID=NONE -Dma.Request0=I2C1_RX -Dma.Request1=I2C1_TX -Dma.RequestsNb=2 -FDCAN1.AutoRetransmission=ENABLE -FDCAN1.CalculateBaudRateNominal=1000000 -FDCAN1.CalculateTimeBitNominal=1000 -FDCAN1.CalculateTimeQuantumNominal=7.142857142857143 -FDCAN1.DataPrescaler=1 -FDCAN1.DataSyncJumpWidth=13 -FDCAN1.DataTimeSeg1=14 -FDCAN1.DataTimeSeg2=13 -FDCAN1.FrameFormat=FDCAN_FRAME_FD_NO_BRS -FDCAN1.IPParameters=CalculateTimeQuantumNominal,CalculateTimeBitNominal,CalculateBaudRateNominal,FrameFormat,NominalSyncJumpWidth,DataPrescaler,DataSyncJumpWidth,DataTimeSeg1,DataTimeSeg2,NominalPrescaler,NominalTimeSeg1,NominalTimeSeg2,Mode,AutoRetransmission -FDCAN1.Mode=FDCAN_MODE_NORMAL -FDCAN1.NominalPrescaler=1 -FDCAN1.NominalSyncJumpWidth=19 -FDCAN1.NominalTimeSeg1=120 -FDCAN1.NominalTimeSeg2=19 -File.Version=6 -I2C1.IPParameters=Timing -I2C1.Timing=0x20B0CCFF -KeepUserPlacement=false -Mcu.CPN=STM32G431CBU3 -Mcu.Family=STM32G4 -Mcu.IP0=DMA -Mcu.IP1=FDCAN1 -Mcu.IP10=TIM7 -Mcu.IP11=TIM15 -Mcu.IP12=TIM16 -Mcu.IP13=TIM17 -Mcu.IP2=I2C1 -Mcu.IP3=NVIC -Mcu.IP4=RCC -Mcu.IP5=SYS -Mcu.IP6=TIM1 -Mcu.IP7=TIM2 -Mcu.IP8=TIM3 -Mcu.IP9=TIM4 -Mcu.IPNb=14 -Mcu.Name=STM32G431C(6-8-B)Ux -Mcu.Package=UFQFPN48 -Mcu.Pin0=PC13 -Mcu.Pin1=PC14-OSC32_IN -Mcu.Pin10=PB0 -Mcu.Pin11=PB1 -Mcu.Pin12=PB2 -Mcu.Pin13=PB14 -Mcu.Pin14=PB15 -Mcu.Pin15=PC6 -Mcu.Pin16=PA8 -Mcu.Pin17=PA11 -Mcu.Pin18=PA12 -Mcu.Pin19=PA13 -Mcu.Pin2=PC15-OSC32_OUT -Mcu.Pin20=PA14 -Mcu.Pin21=PA15 -Mcu.Pin22=PB6 -Mcu.Pin23=PB7 -Mcu.Pin24=PB8-BOOT0 -Mcu.Pin25=PB9 -Mcu.Pin26=VP_SYS_VS_Systick -Mcu.Pin27=VP_SYS_VS_DBSignals -Mcu.Pin28=VP_TIM2_VS_ClockSourceINT -Mcu.Pin29=VP_TIM7_VS_ClockSourceINT -Mcu.Pin3=PF0-OSC_IN -Mcu.Pin30=VP_TIM16_VS_ClockSourceINT -Mcu.Pin31=VP_TIM17_VS_ClockSourceINT -Mcu.Pin32=VP_TIM17_VS_no_output1 -Mcu.Pin4=PA1 -Mcu.Pin5=PA2 -Mcu.Pin6=PA3 -Mcu.Pin7=PA4 -Mcu.Pin8=PA6 -Mcu.Pin9=PC4 -Mcu.PinsNb=33 -Mcu.ThirdPartyNb=0 -Mcu.UserConstants= -Mcu.UserName=STM32G431CBUx -MxCube.Version=6.10.0 -MxDb.Version=DB.6.0.100 -NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false -NVIC.DMA1_Channel1_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true -NVIC.DMA1_Channel2_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true -NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false -NVIC.FDCAN1_IT0_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true -NVIC.ForceEnableDMAVector=true -NVIC.HardFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false -NVIC.I2C1_ER_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true -NVIC.I2C1_EV_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true -NVIC.MemoryManagement_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false -NVIC.NonMaskableInt_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false -NVIC.PendSV_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false -NVIC.PriorityGroup=NVIC_PRIORITYGROUP_4 -NVIC.SVCall_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false -NVIC.SysTick_IRQn=true\:0\:0\:true\:false\:true\:false\:true\:false -NVIC.TIM1_TRG_COM_TIM17_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true -NVIC.TIM1_UP_TIM16_IRQn=true\:1\:0\:true\:false\:true\:true\:true\:true -NVIC.TIM2_IRQn=true\:1\:0\:true\:false\:true\:true\:true\:true -NVIC.TIM7_IRQn=true\:1\:0\:true\:false\:true\:true\:true\:true -NVIC.UsageFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false -PA1.GPIOParameters=GPIO_Label -PA1.GPIO_Label=DEBUG LED 0 -PA1.Locked=true -PA1.Signal=GPIO_Output -PA11.GPIOParameters=GPIO_Speed -PA11.GPIO_Speed=GPIO_SPEED_FREQ_VERY_HIGH -PA11.Mode=FDCAN_Activate -PA11.Signal=FDCAN1_RX -PA12.GPIOParameters=GPIO_Speed -PA12.GPIO_Speed=GPIO_SPEED_FREQ_VERY_HIGH -PA12.Locked=true -PA12.Mode=FDCAN_Activate -PA12.Signal=FDCAN1_TX -PA13.Mode=Serial_Wire -PA13.Signal=SYS_JTMS-SWDIO -PA14.Mode=Serial_Wire -PA14.Signal=SYS_JTCK-SWCLK -PA15.GPIOParameters=GPIO_Label -PA15.GPIO_Label=CAN STANDBY -PA15.Locked=true -PA15.Signal=GPIO_Output -PA2.GPIOParameters=GPIO_Label -PA2.GPIO_Label=DEBUG LED 1 -PA2.Locked=true -PA2.Signal=GPIO_Output -PA3.GPIOParameters=GPIO_Label -PA3.GPIO_Label=DEBUG LED 2 -PA3.Locked=true -PA3.Signal=GPIO_Output -PA4.GPIOParameters=GPIO_Label -PA4.GPIO_Label=QUAD 1 B -PA4.Signal=S_TIM3_CH2 -PA6.GPIOParameters=GPIO_Label -PA6.GPIO_Label=QUAD 1 A -PA6.Signal=S_TIM3_CH1 -PA8.GPIOParameters=GPIO_Label -PA8.GPIO_Label=MOTOR 1 PWM -PA8.Signal=S_TIM1_CH1 -PB0.GPIOParameters=GPIO_Label -PB0.GPIO_Label=LIMIT 1-1 -PB0.Locked=true -PB0.Signal=GPIO_Input -PB1.GPIOParameters=GPIO_Label -PB1.GPIO_Label=LIMIT 1-2 -PB1.Locked=true -PB1.Signal=GPIO_Input -PB14.GPIOParameters=GPIO_Label -PB14.GPIO_Label=MOTOR 0 PWM -PB14.Locked=true -PB14.Signal=S_TIM15_CH1 -PB15.GPIOParameters=GPIO_Label -PB15.GPIO_Label=MOTOR 0 DIR -PB15.Locked=true -PB15.Signal=GPIO_Output -PB2.GPIOParameters=GPIO_Label -PB2.GPIO_Label=LIMIT 1-3 -PB2.Locked=true -PB2.Signal=GPIO_Input -PB6.GPIOParameters=GPIO_Label -PB6.GPIO_Label=QUAD 0 A -PB6.Signal=S_TIM4_CH1 -PB7.GPIOParameters=GPIO_Label -PB7.GPIO_Label=QUAD 0 B -PB7.Signal=S_TIM4_CH2 -PB8-BOOT0.Locked=true -PB8-BOOT0.Mode=I2C -PB8-BOOT0.Signal=I2C1_SCL -PB9.Mode=I2C -PB9.Signal=I2C1_SDA -PC13.GPIOParameters=GPIO_Label -PC13.GPIO_Label=LIMIT 0-0 -PC13.Locked=true -PC13.Signal=GPIO_Input -PC14-OSC32_IN.GPIOParameters=GPIO_Label -PC14-OSC32_IN.GPIO_Label=LIMIT 0-1 -PC14-OSC32_IN.Locked=true -PC14-OSC32_IN.Signal=GPIO_Input -PC15-OSC32_OUT.GPIOParameters=GPIO_Label -PC15-OSC32_OUT.GPIO_Label=LIMIT 0-2 -PC15-OSC32_OUT.Locked=true -PC15-OSC32_OUT.Signal=GPIO_Input -PC4.GPIOParameters=GPIO_Label -PC4.GPIO_Label=LIMIT 1-0 -PC4.Locked=true -PC4.Signal=GPIO_Input -PC6.GPIOParameters=GPIO_Label -PC6.GPIO_Label=MOTOR 1 DIR -PC6.Locked=true -PC6.Signal=GPIO_Output -PF0-OSC_IN.GPIOParameters=GPIO_Label -PF0-OSC_IN.GPIO_Label=LIMIT 0-3 -PF0-OSC_IN.Locked=true -PF0-OSC_IN.Signal=GPIO_Input -PinOutPanel.RotationAngle=0 -ProjectManager.AskForMigrate=true -ProjectManager.BackupPrevious=false -ProjectManager.CompilerOptimize=6 -ProjectManager.ComputerToolchain=false -ProjectManager.CoupleFile=false -ProjectManager.CustomerFirmwarePackage= -ProjectManager.DefaultFWLocation=true -ProjectManager.DeletePrevious=true -ProjectManager.DeviceId=STM32G431CBUx -ProjectManager.FirmwarePackage=STM32Cube FW_G4 V1.5.1 -ProjectManager.FreePins=false -ProjectManager.HalAssertFull=false -ProjectManager.HeapSize=0x200 -ProjectManager.KeepUserCode=true -ProjectManager.LastFirmware=false -ProjectManager.LibraryCopy=1 -ProjectManager.MainLocation=Core/Src -ProjectManager.NoMain=false -ProjectManager.PreviousToolchain=STM32CubeIDE -ProjectManager.ProjectBuild=false -ProjectManager.ProjectFileName=bdcmc.ioc -ProjectManager.ProjectName=bdcmc -ProjectManager.ProjectStructure= -ProjectManager.RegisterCallBack= -ProjectManager.StackSize=0x400 -ProjectManager.TargetToolchain=STM32CubeIDE -ProjectManager.ToolChainLocation= -ProjectManager.UAScriptAfterPath= -ProjectManager.UAScriptBeforePath= -ProjectManager.UnderRoot=true -ProjectManager.functionlistsort=1-SystemClock_Config-RCC-false-HAL-false,2-MX_GPIO_Init-GPIO-false-HAL-true,3-MX_DMA_Init-DMA-false-HAL-true,4-MX_FDCAN1_Init-FDCAN1-false-HAL-true,5-MX_I2C1_Init-I2C1-false-HAL-true,6-MX_TIM15_Init-TIM15-false-HAL-true,7-MX_TIM4_Init-TIM4-false-HAL-true,8-MX_TIM3_Init-TIM3-false-HAL-true,9-MX_TIM7_Init-TIM7-false-HAL-true,10-MX_TIM16_Init-TIM16-false-HAL-true,11-MX_TIM2_Init-TIM2-false-HAL-true,12-MX_TIM1_Init-TIM1-false-HAL-true,13-MX_TIM17_Init-TIM17-false-HAL-true -RCC.ADC12Freq_Value=140000000 -RCC.AHBFreq_Value=140000000 -RCC.APB1Freq_Value=140000000 -RCC.APB1TimFreq_Value=140000000 -RCC.APB2Freq_Value=140000000 -RCC.APB2TimFreq_Value=140000000 -RCC.CRSFreq_Value=48000000 -RCC.CortexFreq_Value=140000000 -RCC.EXTERNAL_CLOCK_VALUE=12288000 -RCC.FCLKCortexFreq_Value=140000000 -RCC.FDCANFreq_Value=140000000 -RCC.FamilyName=M -RCC.HCLKFreq_Value=140000000 -RCC.HSE_VALUE=8000000 -RCC.HSI48_VALUE=48000000 -RCC.HSI_VALUE=16000000 -RCC.I2C1Freq_Value=140000000 -RCC.I2C2Freq_Value=140000000 -RCC.I2C3Freq_Value=140000000 -RCC.I2SFreq_Value=140000000 -RCC.IPParameters=ADC12Freq_Value,AHBFreq_Value,APB1Freq_Value,APB1TimFreq_Value,APB2Freq_Value,APB2TimFreq_Value,CRSFreq_Value,CortexFreq_Value,EXTERNAL_CLOCK_VALUE,FCLKCortexFreq_Value,FDCANFreq_Value,FamilyName,HCLKFreq_Value,HSE_VALUE,HSI48_VALUE,HSI_VALUE,I2C1Freq_Value,I2C2Freq_Value,I2C3Freq_Value,I2SFreq_Value,LPTIM1Freq_Value,LPUART1Freq_Value,LSCOPinFreq_Value,LSE_VALUE,LSI_VALUE,MCO1PinFreq_Value,PLLM,PLLN,PLLPoutputFreq_Value,PLLQoutputFreq_Value,PLLRCLKFreq_Value,PWRFreq_Value,RNGFreq_Value,SAI1Freq_Value,SYSCLKFreq_VALUE,SYSCLKSource,USART1Freq_Value,USART2Freq_Value,USART3Freq_Value,USBFreq_Value,VCOInputFreq_Value,VCOOutputFreq_Value -RCC.LPTIM1Freq_Value=140000000 -RCC.LPUART1Freq_Value=140000000 -RCC.LSCOPinFreq_Value=32000 -RCC.LSE_VALUE=32768 -RCC.LSI_VALUE=32000 -RCC.MCO1PinFreq_Value=16000000 -RCC.PLLM=RCC_PLLM_DIV2 -RCC.PLLN=35 -RCC.PLLPoutputFreq_Value=140000000 -RCC.PLLQoutputFreq_Value=140000000 -RCC.PLLRCLKFreq_Value=140000000 -RCC.PWRFreq_Value=140000000 -RCC.RNGFreq_Value=140000000 -RCC.SAI1Freq_Value=140000000 -RCC.SYSCLKFreq_VALUE=140000000 -RCC.SYSCLKSource=RCC_SYSCLKSOURCE_PLLCLK -RCC.USART1Freq_Value=140000000 -RCC.USART2Freq_Value=140000000 -RCC.USART3Freq_Value=140000000 -RCC.USBFreq_Value=140000000 -RCC.VCOInputFreq_Value=8000000 -RCC.VCOOutputFreq_Value=280000000 -SH.S_TIM15_CH1.0=TIM15_CH1,PWM Generation1 CH1 -SH.S_TIM15_CH1.ConfNb=1 -SH.S_TIM1_CH1.0=TIM1_CH1,Output Compare1 CH1 -SH.S_TIM1_CH1.ConfNb=1 -SH.S_TIM3_CH1.0=TIM3_CH1,Encoder_Interface -SH.S_TIM3_CH1.ConfNb=1 -SH.S_TIM3_CH2.0=TIM3_CH2,Encoder_Interface -SH.S_TIM3_CH2.ConfNb=1 -SH.S_TIM4_CH1.0=TIM4_CH1,Encoder_Interface -SH.S_TIM4_CH1.ConfNb=1 -SH.S_TIM4_CH2.0=TIM4_CH2,Encoder_Interface -SH.S_TIM4_CH2.ConfNb=1 -TIM1.Channel-Output\ Compare1\ CH1=TIM_CHANNEL_1 -TIM1.IPParameters=Channel-Output Compare1 CH1 -TIM15.Channel-PWM\ Generation1\ CH1=TIM_CHANNEL_1 -TIM15.IPParameters=Prescaler,PeriodNoDither,Channel-PWM Generation1 CH1 -TIM15.PeriodNoDither=99 -TIM15.Prescaler=63 -TIM16.IPParameters=Prescaler,PeriodNoDither -TIM16.PeriodNoDither=54580 -TIM16.Prescaler=512 -TIM17.Channel=TIM_CHANNEL_1 -TIM17.IPParameters=Channel -TIM2.AutoReloadPreload=TIM_AUTORELOAD_PRELOAD_ENABLE -TIM2.IPParameters=Prescaler,PeriodNoDither,AutoReloadPreload -TIM2.PeriodNoDither=58333 -TIM2.Prescaler=119 -TIM7.AutoReloadPreload=TIM_AUTORELOAD_PRELOAD_ENABLE -TIM7.IPParameters=AutoReloadPreload,PeriodNoDither,Prescaler -TIM7.PeriodNoDither=49999 -TIM7.Prescaler=139 -VP_SYS_VS_DBSignals.Mode=DisableDeadBatterySignals -VP_SYS_VS_DBSignals.Signal=SYS_VS_DBSignals -VP_SYS_VS_Systick.Mode=SysTick -VP_SYS_VS_Systick.Signal=SYS_VS_Systick -VP_TIM16_VS_ClockSourceINT.Mode=Enable_Timer -VP_TIM16_VS_ClockSourceINT.Signal=TIM16_VS_ClockSourceINT -VP_TIM17_VS_ClockSourceINT.Mode=Enable_Timer -VP_TIM17_VS_ClockSourceINT.Signal=TIM17_VS_ClockSourceINT -VP_TIM17_VS_no_output1.Mode=Output Compare1 No Output -VP_TIM17_VS_no_output1.Signal=TIM17_VS_no_output1 -VP_TIM2_VS_ClockSourceINT.Mode=Internal -VP_TIM2_VS_ClockSourceINT.Signal=TIM2_VS_ClockSourceINT -VP_TIM7_VS_ClockSourceINT.Mode=Enable_Timer -VP_TIM7_VS_ClockSourceINT.Signal=TIM7_VS_ClockSourceINT -board=custom -isbadioc=false +#MicroXplorer Configuration settings - do not modify +CAD.formats= +CAD.pinconfig= +CAD.provider= +Dma.I2C1_RX.0.Direction=DMA_PERIPH_TO_MEMORY +Dma.I2C1_RX.0.EventEnable=DISABLE +Dma.I2C1_RX.0.Instance=DMA1_Channel1 +Dma.I2C1_RX.0.MemDataAlignment=DMA_MDATAALIGN_BYTE +Dma.I2C1_RX.0.MemInc=DMA_MINC_ENABLE +Dma.I2C1_RX.0.Mode=DMA_NORMAL +Dma.I2C1_RX.0.PeriphDataAlignment=DMA_PDATAALIGN_BYTE +Dma.I2C1_RX.0.PeriphInc=DMA_PINC_DISABLE +Dma.I2C1_RX.0.Polarity=HAL_DMAMUX_REQ_GEN_RISING +Dma.I2C1_RX.0.Priority=DMA_PRIORITY_MEDIUM +Dma.I2C1_RX.0.RequestNumber=1 +Dma.I2C1_RX.0.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,SignalID,Polarity,RequestNumber,SyncSignalID,SyncPolarity,SyncEnable,EventEnable,SyncRequestNumber +Dma.I2C1_RX.0.SignalID=NONE +Dma.I2C1_RX.0.SyncEnable=DISABLE +Dma.I2C1_RX.0.SyncPolarity=HAL_DMAMUX_SYNC_NO_EVENT +Dma.I2C1_RX.0.SyncRequestNumber=1 +Dma.I2C1_RX.0.SyncSignalID=NONE +Dma.I2C1_TX.1.Direction=DMA_MEMORY_TO_PERIPH +Dma.I2C1_TX.1.EventEnable=DISABLE +Dma.I2C1_TX.1.Instance=DMA1_Channel2 +Dma.I2C1_TX.1.MemDataAlignment=DMA_MDATAALIGN_BYTE +Dma.I2C1_TX.1.MemInc=DMA_MINC_ENABLE +Dma.I2C1_TX.1.Mode=DMA_NORMAL +Dma.I2C1_TX.1.PeriphDataAlignment=DMA_PDATAALIGN_BYTE +Dma.I2C1_TX.1.PeriphInc=DMA_PINC_DISABLE +Dma.I2C1_TX.1.Polarity=HAL_DMAMUX_REQ_GEN_RISING +Dma.I2C1_TX.1.Priority=DMA_PRIORITY_MEDIUM +Dma.I2C1_TX.1.RequestNumber=1 +Dma.I2C1_TX.1.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,SignalID,Polarity,RequestNumber,SyncSignalID,SyncPolarity,SyncEnable,EventEnable,SyncRequestNumber +Dma.I2C1_TX.1.SignalID=NONE +Dma.I2C1_TX.1.SyncEnable=DISABLE +Dma.I2C1_TX.1.SyncPolarity=HAL_DMAMUX_SYNC_NO_EVENT +Dma.I2C1_TX.1.SyncRequestNumber=1 +Dma.I2C1_TX.1.SyncSignalID=NONE +Dma.Request0=I2C1_RX +Dma.Request1=I2C1_TX +Dma.RequestsNb=2 +FDCAN1.AutoRetransmission=ENABLE +FDCAN1.CalculateBaudRateNominal=1000000 +FDCAN1.CalculateTimeBitNominal=1000 +FDCAN1.CalculateTimeQuantumNominal=7.142857142857143 +FDCAN1.DataPrescaler=1 +FDCAN1.DataSyncJumpWidth=13 +FDCAN1.DataTimeSeg1=14 +FDCAN1.DataTimeSeg2=13 +FDCAN1.ExtFiltersNbr=1 +FDCAN1.FrameFormat=FDCAN_FRAME_FD_NO_BRS +FDCAN1.IPParameters=CalculateTimeQuantumNominal,CalculateTimeBitNominal,CalculateBaudRateNominal,FrameFormat,NominalSyncJumpWidth,DataPrescaler,DataSyncJumpWidth,DataTimeSeg1,DataTimeSeg2,NominalPrescaler,NominalTimeSeg1,NominalTimeSeg2,Mode,AutoRetransmission,ExtFiltersNbr +FDCAN1.Mode=FDCAN_MODE_NORMAL +FDCAN1.NominalPrescaler=1 +FDCAN1.NominalSyncJumpWidth=19 +FDCAN1.NominalTimeSeg1=120 +FDCAN1.NominalTimeSeg2=19 +File.Version=6 +I2C1.I2C_Speed_Mode=I2C_Fast +I2C1.IPParameters=Timing,I2C_Speed_Mode +I2C1.Timing=0x00D04BFF +KeepUserPlacement=false +Mcu.CPN=STM32G431CBU3 +Mcu.Family=STM32G4 +Mcu.IP0=DMA +Mcu.IP1=FDCAN1 +Mcu.IP10=TIM8 +Mcu.IP11=TIM15 +Mcu.IP12=TIM16 +Mcu.IP13=TIM17 +Mcu.IP2=I2C1 +Mcu.IP3=NVIC +Mcu.IP4=RCC +Mcu.IP5=SYS +Mcu.IP6=TIM2 +Mcu.IP7=TIM3 +Mcu.IP8=TIM6 +Mcu.IP9=TIM7 +Mcu.IPNb=14 +Mcu.Name=STM32G431C(6-8-B)Ux +Mcu.Package=UFQFPN48 +Mcu.Pin0=PC13 +Mcu.Pin1=PC14-OSC32_IN +Mcu.Pin10=PB0 +Mcu.Pin11=PB1 +Mcu.Pin12=PB2 +Mcu.Pin13=PB14 +Mcu.Pin14=PB15 +Mcu.Pin15=PC6 +Mcu.Pin16=PA11 +Mcu.Pin17=PA12 +Mcu.Pin18=PA13 +Mcu.Pin19=PA14 +Mcu.Pin2=PC15-OSC32_OUT +Mcu.Pin20=PA15 +Mcu.Pin21=PB8-BOOT0 +Mcu.Pin22=PB9 +Mcu.Pin23=VP_SYS_VS_Systick +Mcu.Pin24=VP_SYS_VS_DBSignals +Mcu.Pin25=VP_TIM2_VS_ClockSourceINT +Mcu.Pin26=VP_TIM6_VS_ClockSourceINT +Mcu.Pin27=VP_TIM7_VS_ClockSourceINT +Mcu.Pin28=VP_TIM8_VS_ClockSourceINT +Mcu.Pin29=VP_TIM16_VS_ClockSourceINT +Mcu.Pin3=PF0-OSC_IN +Mcu.Pin30=VP_TIM17_VS_ClockSourceINT +Mcu.Pin31=VP_TIM17_VS_no_output1 +Mcu.Pin4=PA1 +Mcu.Pin5=PA2 +Mcu.Pin6=PA3 +Mcu.Pin7=PA4 +Mcu.Pin8=PA6 +Mcu.Pin9=PC4 +Mcu.PinsNb=32 +Mcu.ThirdPartyNb=0 +Mcu.UserConstants= +Mcu.UserName=STM32G431CBUx +MxCube.Version=6.10.0 +MxDb.Version=DB.6.0.100 +NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +NVIC.DMA1_Channel1_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DMA1_Channel2_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +NVIC.FDCAN1_IT0_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true +NVIC.ForceEnableDMAVector=true +NVIC.HardFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +NVIC.I2C1_ER_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true +NVIC.I2C1_EV_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true +NVIC.MemoryManagement_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +NVIC.NonMaskableInt_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +NVIC.PendSV_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +NVIC.PriorityGroup=NVIC_PRIORITYGROUP_4 +NVIC.SVCall_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +NVIC.SysTick_IRQn=true\:0\:0\:true\:false\:true\:false\:true\:false +NVIC.TIM1_TRG_COM_TIM17_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true +NVIC.TIM1_UP_TIM16_IRQn=true\:1\:0\:true\:false\:true\:true\:true\:true +NVIC.TIM2_IRQn=true\:1\:0\:true\:false\:true\:true\:true\:true +NVIC.TIM6_DAC_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true +NVIC.TIM7_IRQn=true\:1\:0\:true\:false\:true\:true\:true\:true +NVIC.UsageFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +PA1.GPIOParameters=GPIO_Label +PA1.GPIO_Label=DEBUG LED 0 +PA1.Locked=true +PA1.Signal=GPIO_Output +PA11.GPIOParameters=GPIO_Speed +PA11.GPIO_Speed=GPIO_SPEED_FREQ_VERY_HIGH +PA11.Mode=FDCAN_Activate +PA11.Signal=FDCAN1_RX +PA12.GPIOParameters=GPIO_Speed +PA12.GPIO_Speed=GPIO_SPEED_FREQ_VERY_HIGH +PA12.Locked=true +PA12.Mode=FDCAN_Activate +PA12.Signal=FDCAN1_TX +PA13.Mode=Serial_Wire +PA13.Signal=SYS_JTMS-SWDIO +PA14.Mode=Serial_Wire +PA14.Signal=SYS_JTCK-SWCLK +PA15.GPIOParameters=GPIO_Label +PA15.GPIO_Label=CAN STANDBY +PA15.Locked=true +PA15.Signal=GPIO_Output +PA2.GPIOParameters=GPIO_Label +PA2.GPIO_Label=DEBUG LED 1 +PA2.Locked=true +PA2.Signal=GPIO_Output +PA3.GPIOParameters=GPIO_Label +PA3.GPIO_Label=DEBUG LED 2 +PA3.Locked=true +PA3.Signal=GPIO_Output +PA4.GPIOParameters=GPIO_Label +PA4.GPIO_Label=QUAD 1 B +PA4.Signal=S_TIM3_CH2 +PA6.GPIOParameters=GPIO_Label +PA6.GPIO_Label=QUAD 1 A +PA6.Signal=S_TIM3_CH1 +PB0.GPIOParameters=GPIO_PuPd,GPIO_Label +PB0.GPIO_Label=LIMIT 1-1 +PB0.GPIO_PuPd=GPIO_PULLUP +PB0.Locked=true +PB0.Signal=GPIO_Input +PB1.GPIOParameters=GPIO_PuPd,GPIO_Label +PB1.GPIO_Label=LIMIT 1-2 +PB1.GPIO_PuPd=GPIO_PULLUP +PB1.Locked=true +PB1.Signal=GPIO_Input +PB14.GPIOParameters=GPIO_Label +PB14.GPIO_Label=MOTOR 0 PWM +PB14.Locked=true +PB14.Signal=S_TIM15_CH1 +PB15.GPIOParameters=GPIO_Label +PB15.GPIO_Label=MOTOR 0 DIR +PB15.Locked=true +PB15.Signal=GPIO_Output +PB2.GPIOParameters=GPIO_PuPd,GPIO_Label +PB2.GPIO_Label=LIMIT 1-3 +PB2.GPIO_PuPd=GPIO_PULLUP +PB2.Locked=true +PB2.Signal=GPIO_Input +PB8-BOOT0.GPIOParameters=GPIO_Speed,GPIO_FM8,GPIO_Pu +PB8-BOOT0.GPIO_FM8=SYSCFG_FASTMODEPLUS_PB8 +PB8-BOOT0.GPIO_Pu=GPIO_NOPULL +PB8-BOOT0.GPIO_Speed=GPIO_SPEED_FREQ_VERY_HIGH +PB8-BOOT0.Locked=true +PB8-BOOT0.Mode=I2C +PB8-BOOT0.Signal=I2C1_SCL +PB9.GPIOParameters=GPIO_Speed,GPIO_Pu,GPIO_FM9 +PB9.GPIO_FM9=SYSCFG_FASTMODEPLUS_PB9 +PB9.GPIO_Pu=GPIO_NOPULL +PB9.GPIO_Speed=GPIO_SPEED_FREQ_VERY_HIGH +PB9.Mode=I2C +PB9.Signal=I2C1_SDA +PC13.GPIOParameters=GPIO_PuPd,GPIO_Label +PC13.GPIO_Label=LIMIT 0-0 +PC13.GPIO_PuPd=GPIO_PULLUP +PC13.Locked=true +PC13.Signal=GPIO_Input +PC14-OSC32_IN.GPIOParameters=GPIO_PuPd,GPIO_Label +PC14-OSC32_IN.GPIO_Label=LIMIT 0-1 +PC14-OSC32_IN.GPIO_PuPd=GPIO_PULLUP +PC14-OSC32_IN.Locked=true +PC14-OSC32_IN.Signal=GPIO_Input +PC15-OSC32_OUT.GPIOParameters=GPIO_PuPd,GPIO_Label +PC15-OSC32_OUT.GPIO_Label=LIMIT 0-2 +PC15-OSC32_OUT.GPIO_PuPd=GPIO_PULLUP +PC15-OSC32_OUT.Locked=true +PC15-OSC32_OUT.Signal=GPIO_Input +PC4.GPIOParameters=GPIO_PuPd,GPIO_Label +PC4.GPIO_Label=LIMIT 1-0 +PC4.GPIO_PuPd=GPIO_PULLUP +PC4.Locked=true +PC4.Signal=GPIO_Input +PC6.GPIOParameters=GPIO_Label +PC6.GPIO_Label=MOTOR 1 DIR +PC6.Locked=true +PC6.Signal=GPIO_Output +PF0-OSC_IN.GPIOParameters=GPIO_PuPd,GPIO_Label +PF0-OSC_IN.GPIO_Label=LIMIT 0-3 +PF0-OSC_IN.GPIO_PuPd=GPIO_PULLUP +PF0-OSC_IN.Locked=true +PF0-OSC_IN.Signal=GPIO_Input +PinOutPanel.RotationAngle=0 +ProjectManager.AskForMigrate=true +ProjectManager.BackupPrevious=false +ProjectManager.CompilerOptimize=6 +ProjectManager.ComputerToolchain=false +ProjectManager.CoupleFile=false +ProjectManager.CustomerFirmwarePackage= +ProjectManager.DefaultFWLocation=true +ProjectManager.DeletePrevious=true +ProjectManager.DeviceId=STM32G431CBUx +ProjectManager.FirmwarePackage=STM32Cube FW_G4 V1.5.2 +ProjectManager.FreePins=false +ProjectManager.HalAssertFull=false +ProjectManager.HeapSize=0x200 +ProjectManager.KeepUserCode=true +ProjectManager.LastFirmware=true +ProjectManager.LibraryCopy=1 +ProjectManager.MainLocation=Core/Src +ProjectManager.NoMain=false +ProjectManager.PreviousToolchain=STM32CubeIDE +ProjectManager.ProjectBuild=false +ProjectManager.ProjectFileName=bdcmc.ioc +ProjectManager.ProjectName=bdcmc +ProjectManager.ProjectStructure= +ProjectManager.RegisterCallBack= +ProjectManager.StackSize=0x400 +ProjectManager.TargetToolchain=STM32CubeIDE +ProjectManager.ToolChainLocation= +ProjectManager.UAScriptAfterPath= +ProjectManager.UAScriptBeforePath= +ProjectManager.UnderRoot=true +ProjectManager.functionlistsort=1-SystemClock_Config-RCC-false-HAL-false,2-MX_GPIO_Init-GPIO-false-HAL-true,3-MX_DMA_Init-DMA-false-HAL-true,4-MX_FDCAN1_Init-FDCAN1-false-HAL-true,5-MX_I2C1_Init-I2C1-false-HAL-true,6-MX_TIM15_Init-TIM15-false-HAL-true,7-MX_TIM3_Init-TIM3-false-HAL-true,8-MX_TIM7_Init-TIM7-false-HAL-true,9-MX_TIM16_Init-TIM16-false-HAL-true,10-MX_TIM2_Init-TIM2-false-HAL-true,11-MX_TIM17_Init-TIM17-false-HAL-true,12-MX_TIM6_Init-TIM6-false-HAL-true,13-MX_TIM8_Init-TIM8-false-HAL-true +RCC.ADC12Freq_Value=140000000 +RCC.AHBFreq_Value=140000000 +RCC.APB1Freq_Value=140000000 +RCC.APB1TimFreq_Value=140000000 +RCC.APB2Freq_Value=140000000 +RCC.APB2TimFreq_Value=140000000 +RCC.CRSFreq_Value=48000000 +RCC.CortexFreq_Value=140000000 +RCC.EXTERNAL_CLOCK_VALUE=12288000 +RCC.FCLKCortexFreq_Value=140000000 +RCC.FDCANFreq_Value=140000000 +RCC.FamilyName=M +RCC.HCLKFreq_Value=140000000 +RCC.HSE_VALUE=8000000 +RCC.HSI48_VALUE=48000000 +RCC.HSI_VALUE=16000000 +RCC.I2C1Freq_Value=140000000 +RCC.I2C2Freq_Value=140000000 +RCC.I2C3Freq_Value=140000000 +RCC.I2SFreq_Value=140000000 +RCC.IPParameters=ADC12Freq_Value,AHBFreq_Value,APB1Freq_Value,APB1TimFreq_Value,APB2Freq_Value,APB2TimFreq_Value,CRSFreq_Value,CortexFreq_Value,EXTERNAL_CLOCK_VALUE,FCLKCortexFreq_Value,FDCANFreq_Value,FamilyName,HCLKFreq_Value,HSE_VALUE,HSI48_VALUE,HSI_VALUE,I2C1Freq_Value,I2C2Freq_Value,I2C3Freq_Value,I2SFreq_Value,LPTIM1Freq_Value,LPUART1Freq_Value,LSCOPinFreq_Value,LSE_VALUE,LSI_VALUE,MCO1PinFreq_Value,PLLM,PLLN,PLLPoutputFreq_Value,PLLQoutputFreq_Value,PLLRCLKFreq_Value,PWRFreq_Value,RNGFreq_Value,SAI1Freq_Value,SYSCLKFreq_VALUE,SYSCLKSource,UART4Freq_Value,USART1Freq_Value,USART2Freq_Value,USART3Freq_Value,USBFreq_Value,VCOInputFreq_Value,VCOOutputFreq_Value +RCC.LPTIM1Freq_Value=140000000 +RCC.LPUART1Freq_Value=140000000 +RCC.LSCOPinFreq_Value=32000 +RCC.LSE_VALUE=32768 +RCC.LSI_VALUE=32000 +RCC.MCO1PinFreq_Value=16000000 +RCC.PLLM=RCC_PLLM_DIV2 +RCC.PLLN=35 +RCC.PLLPoutputFreq_Value=140000000 +RCC.PLLQoutputFreq_Value=140000000 +RCC.PLLRCLKFreq_Value=140000000 +RCC.PWRFreq_Value=140000000 +RCC.RNGFreq_Value=140000000 +RCC.SAI1Freq_Value=140000000 +RCC.SYSCLKFreq_VALUE=140000000 +RCC.SYSCLKSource=RCC_SYSCLKSOURCE_PLLCLK +RCC.UART4Freq_Value=140000000 +RCC.USART1Freq_Value=140000000 +RCC.USART2Freq_Value=140000000 +RCC.USART3Freq_Value=140000000 +RCC.USBFreq_Value=140000000 +RCC.VCOInputFreq_Value=8000000 +RCC.VCOOutputFreq_Value=280000000 +SH.S_TIM15_CH1.0=TIM15_CH1,PWM Generation1 CH1 +SH.S_TIM15_CH1.ConfNb=1 +SH.S_TIM3_CH1.0=TIM3_CH1,Encoder_Interface +SH.S_TIM3_CH1.ConfNb=1 +SH.S_TIM3_CH2.0=TIM3_CH2,Encoder_Interface +SH.S_TIM3_CH2.ConfNb=1 +TIM15.Channel-PWM\ Generation1\ CH1=TIM_CHANNEL_1 +TIM15.IPParameters=Prescaler,PeriodNoDither,Channel-PWM Generation1 CH1 +TIM15.PeriodNoDither=99 +TIM15.Prescaler=63 +TIM16.IPParameters=Prescaler,PeriodNoDither +TIM16.PeriodNoDither=54580 +TIM16.Prescaler=512 +TIM17.Channel=TIM_CHANNEL_1 +TIM17.IPParameters=Channel +TIM2.AutoReloadPreload=TIM_AUTORELOAD_PRELOAD_ENABLE +TIM2.IPParameters=Prescaler,PeriodNoDither,AutoReloadPreload +TIM2.PeriodNoDither=58333 +TIM2.Prescaler=119 +TIM7.AutoReloadPreload=TIM_AUTORELOAD_PRELOAD_ENABLE +TIM7.IPParameters=AutoReloadPreload,PeriodNoDither,Prescaler +TIM7.PeriodNoDither=49999 +TIM7.Prescaler=139 +VP_SYS_VS_DBSignals.Mode=DisableDeadBatterySignals +VP_SYS_VS_DBSignals.Signal=SYS_VS_DBSignals +VP_SYS_VS_Systick.Mode=SysTick +VP_SYS_VS_Systick.Signal=SYS_VS_Systick +VP_TIM16_VS_ClockSourceINT.Mode=Enable_Timer +VP_TIM16_VS_ClockSourceINT.Signal=TIM16_VS_ClockSourceINT +VP_TIM17_VS_ClockSourceINT.Mode=Enable_Timer +VP_TIM17_VS_ClockSourceINT.Signal=TIM17_VS_ClockSourceINT +VP_TIM17_VS_no_output1.Mode=Output Compare1 No Output +VP_TIM17_VS_no_output1.Signal=TIM17_VS_no_output1 +VP_TIM2_VS_ClockSourceINT.Mode=Internal +VP_TIM2_VS_ClockSourceINT.Signal=TIM2_VS_ClockSourceINT +VP_TIM6_VS_ClockSourceINT.Mode=Enable_Timer +VP_TIM6_VS_ClockSourceINT.Signal=TIM6_VS_ClockSourceINT +VP_TIM7_VS_ClockSourceINT.Mode=Enable_Timer +VP_TIM7_VS_ClockSourceINT.Signal=TIM7_VS_ClockSourceINT +VP_TIM8_VS_ClockSourceINT.Mode=Internal +VP_TIM8_VS_ClockSourceINT.Signal=TIM8_VS_ClockSourceINT +board=custom +isbadioc=false diff --git a/src/esw/fw/dirt_sensor/dirt_sensor_ros/dirt_sensor_ros.ino b/src/esw/fw/dirt_sensor/dirt_sensor_ros/dirt_sensor_ros.ino index da8cbf0b0..b6b227b0b 100644 --- a/src/esw/fw/dirt_sensor/dirt_sensor_ros/dirt_sensor_ros.ino +++ b/src/esw/fw/dirt_sensor/dirt_sensor_ros/dirt_sensor_ros.ino @@ -1,5 +1,5 @@ -#include #include +#include #include #include @@ -14,11 +14,11 @@ ros::Publisher humidity_pub("sa_humidity_data", &humidity_data); DFRobot_SHT20 sht20(&Wire, SHT20_I2C_ADDR); void setup(){ - + nh.getHardware()->setBaud(57600); // Have to set this parameter nh.initNode(); nh.advertise(temperature_pub); nh.advertise(humidity_pub); - + //Serial.begin(9600); sht20.initSHT20(); delay(100); } @@ -27,6 +27,12 @@ void loop(){ float temp = sht20.readTemperature(); float humidity = sht20.readHumidity() / 100.0; +/* + Serial.print(temp); + Serial.print(" "); + Serial.print(humidity); + Serial.print("\n"); + */ temperature_data.temperature = temp; temperature_pub.publish(&temperature_data); diff --git a/src/esw/fw/pdlb/Core/Inc/diag_temp_sensor.hpp b/src/esw/fw/pdlb/Core/Inc/diag_temp_sensor.hpp index 151f77556..39e9ccd85 100644 --- a/src/esw/fw/pdlb/Core/Inc/diag_temp_sensor.hpp +++ b/src/esw/fw/pdlb/Core/Inc/diag_temp_sensor.hpp @@ -9,6 +9,8 @@ #include #include +// TMP6431DECR + namespace mrover { class DiagTempSensor { diff --git a/src/esw/fw/pdlb/Core/Src/diag_temp_sensor.cpp b/src/esw/fw/pdlb/Core/Src/diag_temp_sensor.cpp index 8c7bcd597..f3f2a4c95 100644 --- a/src/esw/fw/pdlb/Core/Src/diag_temp_sensor.cpp +++ b/src/esw/fw/pdlb/Core/Src/diag_temp_sensor.cpp @@ -5,6 +5,8 @@ #include +// TMP6431DECR + namespace mrover { constexpr static float DIAG_TEMP_COEFFICIENT = 0.0064f; @@ -23,6 +25,7 @@ namespace mrover { void DiagTempSensor::update_temp() { uint16_t adc_val = m_adc_sensor->get_raw_channel_value(m_channel); float measured_volts = (adc_val * 3.3f) / 4096.0f; + m_temp = (THRM_A4 * powf(measured_volts,4)) + (THRM_A3 * powf(measured_volts,3)) + (THRM_A2 * powf(measured_volts,2)) + (THRM_A1 * measured_volts) + THRM_A0; } diff --git a/src/esw/fw/pdlb/Core/Src/main.c b/src/esw/fw/pdlb/Core/Src/main.c index 10d641360..98a6fe28c 100644 --- a/src/esw/fw/pdlb/Core/Src/main.c +++ b/src/esw/fw/pdlb/Core/Src/main.c @@ -205,6 +205,7 @@ int main(void) /* Start scheduler */ osKernelStart(); + /* We should never get here as control is now taken by the scheduler */ /* Infinite loop */ /* USER CODE BEGIN WHILE */ @@ -317,7 +318,7 @@ static void MX_ADC1_Init(void) */ sConfig.Channel = ADC_CHANNEL_1; sConfig.Rank = ADC_REGULAR_RANK_1; - sConfig.SamplingTime = ADC_SAMPLETIME_2CYCLES_5; + sConfig.SamplingTime = ADC_SAMPLETIME_12CYCLES_5; sConfig.SingleDiff = ADC_SINGLE_ENDED; sConfig.OffsetNumber = ADC_OFFSET_NONE; sConfig.Offset = 0; @@ -457,7 +458,7 @@ static void MX_ADC2_Init(void) */ sConfig.Channel = ADC_CHANNEL_10; sConfig.Rank = ADC_REGULAR_RANK_1; - sConfig.SamplingTime = ADC_SAMPLETIME_2CYCLES_5; + sConfig.SamplingTime = ADC_SAMPLETIME_640CYCLES_5; sConfig.SingleDiff = ADC_SINGLE_ENDED; sConfig.OffsetNumber = ADC_OFFSET_NONE; sConfig.Offset = 0; @@ -511,7 +512,7 @@ static void MX_FDCAN1_Init(void) hfdcan1.Init.DataTimeSeg1 = 14; hfdcan1.Init.DataTimeSeg2 = 13; hfdcan1.Init.StdFiltersNbr = 0; - hfdcan1.Init.ExtFiltersNbr = 0; + hfdcan1.Init.ExtFiltersNbr = 1; hfdcan1.Init.TxFifoQueueMode = FDCAN_TX_FIFO_OPERATION; if (HAL_FDCAN_Init(&hfdcan1) != HAL_OK) { @@ -624,7 +625,7 @@ void HAL_FDCAN_RxFifo0Callback(FDCAN_HandleTypeDef *hfdcan, uint32_t RxFifo0ITs) void SendCurrentTemperature(void* argument) { for(;;) { - uint32_t tick = osKernelGetTickCount() + osKernelGetTickFreq(); // 1 Hz + uint32_t tick = osKernelGetTickCount() + (osKernelGetTickFreq() / 10.0); // 10 Hz update_and_send_current_temp(); osDelayUntil(tick); diff --git a/src/esw/fw/pdlb/Core/Src/stm32g4xx_hal_msp.c b/src/esw/fw/pdlb/Core/Src/stm32g4xx_hal_msp.c index e8a63291c..4685060eb 100644 --- a/src/esw/fw/pdlb/Core/Src/stm32g4xx_hal_msp.c +++ b/src/esw/fw/pdlb/Core/Src/stm32g4xx_hal_msp.c @@ -20,6 +20,7 @@ /* Includes ------------------------------------------------------------------*/ #include "main.h" + /* USER CODE BEGIN Includes */ /* USER CODE END Includes */ @@ -152,8 +153,8 @@ void HAL_ADC_MspInit(ADC_HandleTypeDef* hadc) hdma_adc1.Init.Direction = DMA_PERIPH_TO_MEMORY; hdma_adc1.Init.PeriphInc = DMA_PINC_DISABLE; hdma_adc1.Init.MemInc = DMA_MINC_ENABLE; - hdma_adc1.Init.PeriphDataAlignment = DMA_PDATAALIGN_HALFWORD; - hdma_adc1.Init.MemDataAlignment = DMA_MDATAALIGN_HALFWORD; + hdma_adc1.Init.PeriphDataAlignment = DMA_PDATAALIGN_WORD; + hdma_adc1.Init.MemDataAlignment = DMA_MDATAALIGN_WORD; hdma_adc1.Init.Mode = DMA_NORMAL; hdma_adc1.Init.Priority = DMA_PRIORITY_LOW; if (HAL_DMA_Init(&hdma_adc1) != HAL_OK) @@ -211,8 +212,8 @@ void HAL_ADC_MspInit(ADC_HandleTypeDef* hadc) hdma_adc2.Init.Direction = DMA_PERIPH_TO_MEMORY; hdma_adc2.Init.PeriphInc = DMA_PINC_DISABLE; hdma_adc2.Init.MemInc = DMA_MINC_ENABLE; - hdma_adc2.Init.PeriphDataAlignment = DMA_PDATAALIGN_HALFWORD; - hdma_adc2.Init.MemDataAlignment = DMA_MDATAALIGN_HALFWORD; + hdma_adc2.Init.PeriphDataAlignment = DMA_PDATAALIGN_WORD; + hdma_adc2.Init.MemDataAlignment = DMA_MDATAALIGN_WORD; hdma_adc2.Init.Mode = DMA_NORMAL; hdma_adc2.Init.Priority = DMA_PRIORITY_LOW; if (HAL_DMA_Init(&hdma_adc2) != HAL_OK) diff --git a/src/esw/fw/pdlb/pdlb.ioc b/src/esw/fw/pdlb/pdlb.ioc index 7d0bf72af..90d1e4a67 100644 --- a/src/esw/fw/pdlb/pdlb.ioc +++ b/src/esw/fw/pdlb/pdlb.ioc @@ -33,16 +33,16 @@ ADC1.Rank-6\#ChannelRegularConversion=7 ADC1.Rank-7\#ChannelRegularConversion=8 ADC1.Rank-8\#ChannelRegularConversion=9 ADC1.Rank-9\#ChannelRegularConversion=10 -ADC1.SamplingTime-0\#ChannelRegularConversion=ADC_SAMPLETIME_2CYCLES_5 -ADC1.SamplingTime-1\#ChannelRegularConversion=ADC_SAMPLETIME_2CYCLES_5 -ADC1.SamplingTime-2\#ChannelRegularConversion=ADC_SAMPLETIME_2CYCLES_5 -ADC1.SamplingTime-3\#ChannelRegularConversion=ADC_SAMPLETIME_2CYCLES_5 -ADC1.SamplingTime-4\#ChannelRegularConversion=ADC_SAMPLETIME_2CYCLES_5 -ADC1.SamplingTime-5\#ChannelRegularConversion=ADC_SAMPLETIME_2CYCLES_5 -ADC1.SamplingTime-6\#ChannelRegularConversion=ADC_SAMPLETIME_2CYCLES_5 -ADC1.SamplingTime-7\#ChannelRegularConversion=ADC_SAMPLETIME_2CYCLES_5 -ADC1.SamplingTime-8\#ChannelRegularConversion=ADC_SAMPLETIME_2CYCLES_5 -ADC1.SamplingTime-9\#ChannelRegularConversion=ADC_SAMPLETIME_2CYCLES_5 +ADC1.SamplingTime-0\#ChannelRegularConversion=ADC_SAMPLETIME_12CYCLES_5 +ADC1.SamplingTime-1\#ChannelRegularConversion=ADC_SAMPLETIME_12CYCLES_5 +ADC1.SamplingTime-2\#ChannelRegularConversion=ADC_SAMPLETIME_12CYCLES_5 +ADC1.SamplingTime-3\#ChannelRegularConversion=ADC_SAMPLETIME_12CYCLES_5 +ADC1.SamplingTime-4\#ChannelRegularConversion=ADC_SAMPLETIME_12CYCLES_5 +ADC1.SamplingTime-5\#ChannelRegularConversion=ADC_SAMPLETIME_12CYCLES_5 +ADC1.SamplingTime-6\#ChannelRegularConversion=ADC_SAMPLETIME_12CYCLES_5 +ADC1.SamplingTime-7\#ChannelRegularConversion=ADC_SAMPLETIME_12CYCLES_5 +ADC1.SamplingTime-8\#ChannelRegularConversion=ADC_SAMPLETIME_12CYCLES_5 +ADC1.SamplingTime-9\#ChannelRegularConversion=ADC_SAMPLETIME_12CYCLES_5 ADC1.master=1 ADC2.Channel-0\#ChannelRegularConversion=ADC_CHANNEL_10 ADC2.Channel-1\#ChannelRegularConversion=ADC_CHANNEL_15 @@ -54,18 +54,18 @@ ADC2.OffsetNumber-0\#ChannelRegularConversion=ADC_OFFSET_NONE ADC2.OffsetNumber-1\#ChannelRegularConversion=ADC_OFFSET_NONE ADC2.Rank-0\#ChannelRegularConversion=1 ADC2.Rank-1\#ChannelRegularConversion=2 -ADC2.SamplingTime-0\#ChannelRegularConversion=ADC_SAMPLETIME_2CYCLES_5 -ADC2.SamplingTime-1\#ChannelRegularConversion=ADC_SAMPLETIME_2CYCLES_5 +ADC2.SamplingTime-0\#ChannelRegularConversion=ADC_SAMPLETIME_640CYCLES_5 +ADC2.SamplingTime-1\#ChannelRegularConversion=ADC_SAMPLETIME_640CYCLES_5 CAD.formats= CAD.pinconfig= CAD.provider= Dma.ADC1.0.Direction=DMA_PERIPH_TO_MEMORY Dma.ADC1.0.EventEnable=DISABLE Dma.ADC1.0.Instance=DMA1_Channel1 -Dma.ADC1.0.MemDataAlignment=DMA_MDATAALIGN_HALFWORD +Dma.ADC1.0.MemDataAlignment=DMA_MDATAALIGN_WORD Dma.ADC1.0.MemInc=DMA_MINC_ENABLE Dma.ADC1.0.Mode=DMA_NORMAL -Dma.ADC1.0.PeriphDataAlignment=DMA_PDATAALIGN_HALFWORD +Dma.ADC1.0.PeriphDataAlignment=DMA_PDATAALIGN_WORD Dma.ADC1.0.PeriphInc=DMA_PINC_DISABLE Dma.ADC1.0.Polarity=HAL_DMAMUX_REQ_GEN_RISING Dma.ADC1.0.Priority=DMA_PRIORITY_LOW @@ -79,10 +79,10 @@ Dma.ADC1.0.SyncSignalID=NONE Dma.ADC2.1.Direction=DMA_PERIPH_TO_MEMORY Dma.ADC2.1.EventEnable=DISABLE Dma.ADC2.1.Instance=DMA1_Channel2 -Dma.ADC2.1.MemDataAlignment=DMA_MDATAALIGN_HALFWORD +Dma.ADC2.1.MemDataAlignment=DMA_MDATAALIGN_WORD Dma.ADC2.1.MemInc=DMA_MINC_ENABLE Dma.ADC2.1.Mode=DMA_NORMAL -Dma.ADC2.1.PeriphDataAlignment=DMA_PDATAALIGN_HALFWORD +Dma.ADC2.1.PeriphDataAlignment=DMA_PDATAALIGN_WORD Dma.ADC2.1.PeriphInc=DMA_PINC_DISABLE Dma.ADC2.1.Polarity=HAL_DMAMUX_REQ_GEN_RISING Dma.ADC2.1.Priority=DMA_PRIORITY_LOW @@ -104,8 +104,9 @@ FDCAN1.DataPrescaler=5 FDCAN1.DataSyncJumpWidth=13 FDCAN1.DataTimeSeg1=14 FDCAN1.DataTimeSeg2=13 +FDCAN1.ExtFiltersNbr=1 FDCAN1.FrameFormat=FDCAN_FRAME_FD_NO_BRS -FDCAN1.IPParameters=CalculateTimeQuantumNominal,CalculateTimeBitNominal,CalculateBaudRateNominal,FrameFormat,DataSyncJumpWidth,NominalSyncJumpWidth,NominalTimeSeg1,NominalTimeSeg2,NominalPrescaler,DataTimeSeg1,DataTimeSeg2,TransmitPause,AutoRetransmission,DataPrescaler +FDCAN1.IPParameters=CalculateTimeQuantumNominal,CalculateTimeBitNominal,CalculateBaudRateNominal,FrameFormat,DataSyncJumpWidth,NominalSyncJumpWidth,NominalTimeSeg1,NominalTimeSeg2,NominalPrescaler,DataTimeSeg1,DataTimeSeg2,TransmitPause,AutoRetransmission,DataPrescaler,ExtFiltersNbr FDCAN1.NominalPrescaler=1 FDCAN1.NominalSyncJumpWidth=19 FDCAN1.NominalTimeSeg1=120 @@ -157,8 +158,8 @@ Mcu.PinsNb=24 Mcu.ThirdPartyNb=0 Mcu.UserConstants= Mcu.UserName=STM32G431CBUx -MxCube.Version=6.8.1 -MxDb.Version=DB.6.0.81 +MxCube.Version=6.10.0 +MxDb.Version=DB.6.0.100 NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false NVIC.DMA1_Channel1_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true NVIC.DMA1_Channel2_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true @@ -276,7 +277,7 @@ ProjectManager.FreePins=false ProjectManager.HalAssertFull=false ProjectManager.HeapSize=0x200 ProjectManager.KeepUserCode=true -ProjectManager.LastFirmware=true +ProjectManager.LastFirmware=false ProjectManager.LibraryCopy=1 ProjectManager.MainLocation=Core/Src ProjectManager.NoMain=false diff --git a/src/esw/fw/science/.cproject b/src/esw/fw/science/.cproject index 823c48d23..5dbf61419 100644 --- a/src/esw/fw/science/.cproject +++ b/src/esw/fw/science/.cproject @@ -25,6 +25,7 @@