├── .bazelrc ├── .github └── workflows │ ├── compile.yaml │ ├── cpplint.yaml │ └── tests.yaml ├── .gitignore ├── .gitmodules ├── BUILD.bazel ├── LICENSE ├── Makefile ├── README.md ├── WORKSPACE ├── bd_tools ├── .gitignore ├── LICENSE ├── README.md ├── pyproject.toml ├── setup.py ├── src │ └── bd_tools │ │ ├── __about__.py │ │ ├── __init__.py │ │ ├── __main__.py │ │ ├── bin │ │ ├── __init__.py │ │ ├── calibrate_encoder.py │ │ ├── control_motor.py │ │ ├── read_sensor.py │ │ ├── update_calibration.py │ │ ├── upload_bootloader.py │ │ ├── upload_calibration.py │ │ ├── upload_firmware.py │ │ ├── view_control_loop.py │ │ └── view_time.py │ │ ├── boards.py │ │ ├── comms.py │ │ ├── dev │ │ ├── __init__.py │ │ ├── calibrate_link.py │ │ ├── calibrate_servo.py │ │ ├── characterize.py │ │ ├── crash_device.py │ │ ├── download_calibration.py │ │ ├── frequency.py │ │ ├── gen_gamma_table.py │ │ ├── gripper_test.py │ │ ├── lift_test.py │ │ ├── modulator_anim.py │ │ ├── openocd.cfg │ │ ├── position_control.py │ │ ├── test_rs485.py │ │ └── usb_latency.sh │ │ ├── livegraph.py │ │ ├── recorder │ │ ├── .gitignore │ │ ├── log.py │ │ ├── log_6phase.py │ │ ├── log_foc.py │ │ ├── log_ramp.py │ │ ├── log_static.py │ │ ├── plot.py │ │ └── plot_logs.py │ │ └── utils.py └── udev_latency │ ├── .gitignore │ └── low_latency.sh ├── bootloader ├── .gitignore ├── BUILD.bazel ├── CPPLINT.cfg ├── include │ ├── chconf.h │ ├── halconf.h │ ├── mcuconf.h │ ├── peripherals.hpp │ └── stm32f4xx_conf.h ├── link.ld └── src │ ├── bl_comms.cpp │ ├── main.cpp │ └── peripherals.cpp ├── common ├── .cproject ├── .project ├── BUILD.bazel ├── CPPLINT.cfg ├── board.c ├── board.h ├── cfg │ └── board.chcfg ├── include │ ├── comms.hpp │ ├── comms_defs.hpp │ ├── constants.hpp │ ├── crc16.h │ ├── flash.h │ ├── helper.h │ ├── hw_conf.h │ └── utils.h └── src │ ├── comms.cpp │ ├── constants.cpp │ ├── crc16.c │ ├── flash.c │ ├── helper.c │ └── utils.c ├── docs ├── disco_bus.md ├── protocol_v1.txt ├── protocol_v2.md └── protocol_v3.md ├── drivers ├── BUILD.bazel ├── CPPLINT.cfg ├── accel │ ├── BUILD.bazel │ ├── iis328dq.cpp │ └── iis328dq.hpp ├── encoder │ ├── BUILD.bazel │ ├── aeat6600.cpp │ ├── aeat6600.h │ ├── as5047d.cpp │ ├── as5047d.hpp │ ├── mlx90363.cpp │ └── mlx90363.h ├── gate_driver │ ├── BUILD.bazel │ ├── drv8301.cpp │ ├── drv8301.h │ ├── drv8312.cpp │ └── drv8312.hpp ├── imu │ ├── CPPLINT.cfg │ ├── lsm6ds3.cpp │ ├── lsm6ds3.h │ ├── lsm6ds3_ll_driver.c │ └── lsm6ds3_ll_driver.h └── temperature │ ├── BUILD.bazel │ ├── lm75b.cpp │ ├── lm75b.h │ ├── mcp9808.cpp │ └── mcp9808.hpp ├── firmware ├── .gitignore ├── BUILD.bazel ├── CPPLINT.cfg ├── include │ ├── LUTFunction.hpp │ ├── Recorder.hpp │ ├── SVM.hpp │ ├── chconf.h │ ├── control.hpp │ ├── crc_mlx.h │ ├── fast_math.hpp │ ├── firmware_peripherals.hpp │ ├── halconf.h │ ├── mcuconf.h │ ├── old_drivers │ │ ├── shell.h │ │ └── usbcfg.h │ ├── peripherals.hpp │ ├── pid.hpp │ ├── state.hpp │ ├── stm32f4xx_conf.h │ └── transforms.hpp ├── link.ld ├── proto │ ├── BUILD.bazel │ └── messages.proto └── src │ ├── LUTFunction.cpp │ ├── Recorder.cpp │ ├── SVM.cpp │ ├── control.cpp │ ├── crc_mlx.c │ ├── fast_math.cpp │ ├── fw_comms.cpp │ ├── main.cpp │ ├── old_drivers │ ├── CPPLINT.cfg │ ├── shell.c │ └── usbcfg.c │ ├── peripherals.cpp │ ├── pid.cpp │ ├── state.cpp │ └── transforms.cpp ├── pyproject.toml ├── requirements.txt ├── tests └── tools │ ├── integration │ └── imu_failure.py │ └── unit │ └── test_tools.py ├── third_party ├── BUILD.bazel └── variables.bzl ├── toolchains ├── BUILD.bazel ├── debug.bzl ├── flash.bzl ├── os_config.bzl ├── platforms.bzl └── programs │ └── BUILD.bazel └── util ├── BUILD.bazel └── time ├── BUILD.bazel └── usec.h /.bazelrc: -------------------------------------------------------------------------------- 1 | # Place all bazel build artifacts under this directory 2 | build --symlink_prefix=bazel-build/ 3 | 4 | # Enable toolchain resolution with cc 5 | build --incompatible_enable_cc_toolchain_resolution 6 | # This line breaks gazelle but hacks around generating the elf files with the 7 | # correct toolchain... Need to figure out how to better do this. 8 | build --platforms=@bazel_embedded//platforms:cortex_m4_fpu 9 | 10 | # Never strip debug symbols 11 | build --strip=never 12 | 13 | build --copt=-O2 14 | build --copt=-ggdb 15 | build --copt=-fomit-frame-pointer 16 | build --copt=-falign-functions=16 17 | build --cxxopt=-std=c++14 18 | build --linkopt=-lc 19 | build --linkopt=-lm 20 | build --linkopt=-lnosys 21 | 22 | # LTO Builds (Do not work with chibios 2.6) 23 | #build --copt=-ffat-lto-objects 24 | #build --linkopt=-ffat-lto-objects 25 | 26 | # Instructions 27 | build --copt=-fsingle-precision-constant 28 | build --copt=-mno-thumb-interwork 29 | 30 | # Code reduction options 31 | build --linkopt=-Wl,--gc-sections 32 | #build --linkopt=-Wl,--print-gc-sections 33 | -------------------------------------------------------------------------------- /.github/workflows/compile.yaml: -------------------------------------------------------------------------------- 1 | name: compile 2 | on: [pull_request] 3 | jobs: 4 | bazel-bootloader: 5 | runs-on: ubuntu-latest 6 | steps: 7 | - uses: actions/checkout@v2 8 | with: 9 | submodules: true 10 | - name: Install Bazelisk 11 | uses: bazelbuild/setup-bazelisk@v2 12 | - run: bazelisk build //bootloader 13 | bazel-firmware: 14 | runs-on: ubuntu-latest 15 | steps: 16 | - uses: actions/checkout@v2 17 | with: 18 | submodules: true 19 | - name: Install Bazelisk 20 | uses: bazelbuild/setup-bazelisk@v2 21 | - run: bazelisk build //firmware 22 | -------------------------------------------------------------------------------- /.github/workflows/cpplint.yaml: -------------------------------------------------------------------------------- 1 | name: linters 2 | on: [pull_request] 3 | jobs: 4 | cpplint: 5 | runs-on: ubuntu-latest 6 | strategy: 7 | matrix: 8 | python-version: [3.8] 9 | steps: 10 | - uses: actions/checkout@v3 11 | - uses: actions/setup-python@v3 12 | with: 13 | python-version: ${{ matrix.python-version }} 14 | - run: pip install -r requirements.txt 15 | - run: make lint_cpp 16 | python_lint: 17 | runs-on: ubuntu-latest 18 | strategy: 19 | matrix: 20 | python-version: [3.8] 21 | steps: 22 | - uses: actions/checkout@v3 23 | - uses: actions/setup-python@v3 24 | with: 25 | python-version: ${{ matrix.python-version }} 26 | - run: pip install -r requirements.txt 27 | - run: make lint_python 28 | -------------------------------------------------------------------------------- /.github/workflows/tests.yaml: -------------------------------------------------------------------------------- 1 | name: bd tools (install and tests) 2 | 3 | on: [pull_request] 4 | 5 | jobs: 6 | build: 7 | 8 | runs-on: ubuntu-latest 9 | strategy: 10 | matrix: 11 | python-version: [3.7, 3.8] 12 | 13 | steps: 14 | - uses: actions/checkout@v2 15 | - name: Set up Python ${{ matrix.python-version }} 16 | uses: actions/setup-python@v2 17 | with: 18 | python-version: ${{ matrix.python-version }} 19 | - name: Install dependencies 20 | run: | 21 | python -m pip install --upgrade pip 22 | if [ -f requirements.txt ]; then pip install -r requirements.txt; fi 23 | - name: Run tests 24 | run: | 25 | make pytest 26 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | .DS_Store 2 | *.pyc 3 | *.swp 4 | *.swo 5 | results/ 6 | spinner_log.txt 7 | .metadata 8 | RemoteSystemsTempFiles/ 9 | firmware/.cproject 10 | firmware/.project 11 | firmware/.settings/ 12 | 13 | bd_tools.egg-info/ 14 | build/ 15 | dist/ 16 | 17 | bazel-build/ 18 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "chibios"] 2 | path = third_party/chibios 3 | url = https://github.com/berkeleyopenarms/ChibiOS.git 4 | branch = stable_2.6.x 5 | -------------------------------------------------------------------------------- /BUILD.bazel: -------------------------------------------------------------------------------- 1 | load("@com_github_bazelbuild_buildtools//buildifier:def.bzl", "buildifier") 2 | 3 | buildifier( 4 | name = "buildifier", 5 | ) 6 | 7 | load("@bazel_gazelle//:def.bzl", "gazelle") 8 | 9 | gazelle( 10 | name = "gazelle", 11 | args = ["-lang=\"proto,cc\""], 12 | ) 13 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2019 Berkeley Open Arms 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | test: pytest lint 2 | 3 | pytest: install_python_packages 4 | python3 -m pytest tests/ 5 | 6 | lint: lint_python lint_cpp 7 | 8 | lint_python: 9 | isort --check tests bd_tools 10 | black --check bd_tools 11 | black --check tests 12 | pflake8 bd_tools tests 13 | 14 | lint_cpp: 15 | cpplint --recursive common/ bootloader/ firmware/ drivers/ 16 | 17 | format: format_python format_bazel 18 | 19 | format_python: 20 | isort tests bd_tools 21 | black bd_tools 22 | black tests 23 | 24 | format_bazel: 25 | bazel run //:buildifier 26 | 27 | setup: init_submodules install_python_packages 28 | 29 | init_submodules: 30 | git submodule update --recursive --init 31 | 32 | install_python_packages: 33 | python3 -m pip install -r requirements.txt --user 34 | python3 -m pip install -e bd_tools/ --user 35 | -------------------------------------------------------------------------------- /WORKSPACE: -------------------------------------------------------------------------------- 1 | load("@bazel_tools//tools/build_defs/repo:git.bzl", "git_repository") 2 | load("@bazel_tools//tools/build_defs/repo:http.bzl", "http_archive") 3 | 4 | ### BUILDIFIER DEPENDENCIES ### 5 | # buildifier is written in Go and hence needs rules_go to be built. 6 | # See https://github.com/bazelbuild/rules_go for the up to date setup instructions. 7 | load("@bazel_tools//tools/build_defs/repo:http.bzl", "http_archive") 8 | 9 | http_archive( 10 | name = "io_bazel_rules_go", 11 | sha256 = "16e9fca53ed6bd4ff4ad76facc9b7b651a89db1689a2877d6fd7b82aa824e366", 12 | urls = [ 13 | "https://mirror.bazel.build/github.com/bazelbuild/rules_go/releases/download/v0.34.0/rules_go-v0.34.0.zip", 14 | "https://github.com/bazelbuild/rules_go/releases/download/v0.34.0/rules_go-v0.34.0.zip", 15 | ], 16 | ) 17 | 18 | http_archive( 19 | name = "bazel_gazelle", 20 | sha256 = "de69a09dc70417580aabf20a28619bb3ef60d038470c7cf8442fafcf627c21cb", 21 | urls = [ 22 | "https://mirror.bazel.build/github.com/bazelbuild/bazel-gazelle/releases/download/v0.24.0/bazel-gazelle-v0.24.0.tar.gz", 23 | "https://github.com/bazelbuild/bazel-gazelle/releases/download/v0.24.0/bazel-gazelle-v0.24.0.tar.gz", 24 | ], 25 | ) 26 | 27 | load("@io_bazel_rules_go//go:deps.bzl", "go_register_toolchains", "go_rules_dependencies") 28 | load("@bazel_gazelle//:deps.bzl", "gazelle_dependencies") 29 | 30 | go_rules_dependencies() 31 | 32 | go_register_toolchains(version = "1.18.3") 33 | 34 | gazelle_dependencies() 35 | 36 | http_archive( 37 | name = "com_github_bazelbuild_buildtools", 38 | sha256 = "ae34c344514e08c23e90da0e2d6cb700fcd28e80c02e23e4d5715dddcb42f7b3", 39 | strip_prefix = "buildtools-4.2.2", 40 | urls = [ 41 | "https://github.com/bazelbuild/buildtools/archive/refs/tags/4.2.2.tar.gz", 42 | ], 43 | ) 44 | 45 | ### EMBEDDED TOOLCHAIN SUPPORT ### 46 | git_repository( 47 | name = "bazel_embedded", 48 | commit = "a9a2be070b5d01dc54bcd3aaa29c991d589a5110", 49 | remote = "https://github.com/bazelembedded/bazel-embedded.git", 50 | shallow_since = "1660526864 -0600", 51 | ) 52 | 53 | load("@bazel_embedded//:bazel_embedded_deps.bzl", "bazel_embedded_deps") 54 | 55 | bazel_embedded_deps() 56 | 57 | load("@bazel_embedded//platforms:execution_platforms.bzl", "register_platforms") 58 | 59 | register_platforms() 60 | 61 | load( 62 | "@bazel_embedded//toolchains/compilers/gcc_arm_none_eabi:gcc_arm_none_repository.bzl", 63 | "gcc_arm_none_compiler", 64 | ) 65 | 66 | gcc_arm_none_compiler() 67 | 68 | load("@bazel_embedded//toolchains/gcc_arm_none_eabi:gcc_arm_none_toolchain.bzl", "register_gcc_arm_none_toolchain") 69 | 70 | register_gcc_arm_none_toolchain() 71 | 72 | load("@bazel_embedded//tools/openocd:openocd_repository.bzl", "openocd_deps") 73 | 74 | openocd_deps() 75 | 76 | ### PROTOBUF SUPPORT ### 77 | git_repository( 78 | name = "com_github_nanopb_nanopb", 79 | commit = "0b653303026cb14802913a9f43da125e7cf02579", 80 | remote = "https://github.com/nanopb/nanopb.git", 81 | shallow_since = "1658641713 +0300", 82 | ) 83 | 84 | load("@com_github_nanopb_nanopb//extra/bazel:nanopb_deps.bzl", "nanopb_deps") 85 | 86 | nanopb_deps() 87 | 88 | load("@com_github_nanopb_nanopb//extra/bazel:python_deps.bzl", "nanopb_python_deps") 89 | 90 | nanopb_python_deps() 91 | 92 | load("@com_github_nanopb_nanopb//extra/bazel:nanopb_workspace.bzl", "nanopb_workspace") 93 | 94 | nanopb_workspace() 95 | 96 | http_archive( 97 | name = "com_google_protobuf", 98 | sha256 = "3bd7828aa5af4b13b99c191e8b1e884ebfa9ad371b0ce264605d347f135d2568", 99 | strip_prefix = "protobuf-3.19.4", 100 | urls = [ 101 | "https://github.com/protocolbuffers/protobuf/archive/v3.19.4.tar.gz", 102 | ], 103 | ) 104 | 105 | load("@com_google_protobuf//:protobuf_deps.bzl", "protobuf_deps") 106 | 107 | protobuf_deps() 108 | 109 | ### TRANSITION WRAPPING ### 110 | git_repository( 111 | name = "rules_meta", 112 | commit = "062936fe3bab149bc2fc664301d8151868a3c874", 113 | remote = "https://github.com/fmeum/rules_meta.git", 114 | shallow_since = "1647421183 +0100", 115 | ) 116 | -------------------------------------------------------------------------------- /bd_tools/.gitignore: -------------------------------------------------------------------------------- 1 | *.pkl 2 | *.txt 3 | *.png 4 | *.json 5 | -------------------------------------------------------------------------------- /bd_tools/LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2019 Berkeley Open Arms 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /bd_tools/README.md: -------------------------------------------------------------------------------- 1 | # bd_tools 2 | 3 | This package contains supporting scripts and tools for communicating with and 4 | testing betz drive motor controllers. 5 | 6 | There are 3 types of utilities: 7 | * Console based - these print data live in text form 8 | * Live graph - these read data live and plot it as a time-series 9 | * Recorder - these collect a fixed-size buffer that is saved on-board the MCU 10 | 11 | ## udev latency 12 | 13 | Some usb to serial converters support configurable batch transmission delays. 14 | This value is often an integer representing milliseconds. For optimal 15 | performance, there is a udev rule which configures the latency to be 1ms on 16 | connection of a usb serial device. If this rule is not in place/your tranciever 17 | is not fast enough, expect major communication issues. 18 | -------------------------------------------------------------------------------- /bd_tools/pyproject.toml: -------------------------------------------------------------------------------- 1 | [build-system] 2 | requires = [ 3 | "hatchling>=1.8.0", 4 | ] 5 | build-backend = "hatchling.build" 6 | 7 | [project] 8 | name = "bd-tools" 9 | description = "Python API for BetzDrive" 10 | readme = "README.md" 11 | license = "MIT" 12 | requires-python = ">=3.7" 13 | authors = [ 14 | { name = "Greg Balke", email = "gbalke@berkeley.edu" }, 15 | ] 16 | classifiers = [ 17 | "License :: OSI Approved :: MIT License", 18 | "Operating System :: OS Independent", 19 | "Programming Language :: Python :: 3.7", 20 | "Programming Language :: Python :: 3.8", 21 | ] 22 | dependencies = [ 23 | "crcmod", 24 | "matplotlib", 25 | "numpy", 26 | "pyserial", 27 | "scipy", 28 | ] 29 | dynamic = [ 30 | "version", 31 | ] 32 | 33 | [tool.black] 34 | line-length = 79 35 | 36 | [project.scripts] 37 | bdt = "bd_tools.__main__:main" 38 | 39 | [project.urls] 40 | Homepage = "http://github.com/betzdrive/bldc-controller" 41 | 42 | [tool.hatch.version] 43 | path = "src/bd_tools/__about__.py" 44 | 45 | [tool.hatch.build.targets.sdist] 46 | include = [ 47 | "/src/bd_tools", 48 | ] 49 | -------------------------------------------------------------------------------- /bd_tools/setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup 2 | 3 | setup() 4 | -------------------------------------------------------------------------------- /bd_tools/src/bd_tools/__about__.py: -------------------------------------------------------------------------------- 1 | __version__ = "0.1" 2 | -------------------------------------------------------------------------------- /bd_tools/src/bd_tools/__init__.py: -------------------------------------------------------------------------------- 1 | """The betz drive tools package.""" 2 | 3 | import bd_tools.bin 4 | import bd_tools.boards 5 | import bd_tools.comms # noqa: F401 6 | -------------------------------------------------------------------------------- /bd_tools/src/bd_tools/__main__.py: -------------------------------------------------------------------------------- 1 | """The betz drive tools package.""" 2 | import argparse 3 | import sys 4 | from pathlib import Path 5 | 6 | import bd_tools 7 | 8 | BIN_PATH = Path(__file__).parent / "bin" 9 | 10 | 11 | def get_tools(): 12 | """Returns all scripts in the bin directory""" 13 | return [tool.stem for tool in BIN_PATH.glob("[!__]*.py")] 14 | 15 | 16 | def parser_args(tools): 17 | parser = argparse.ArgumentParser(description="BetzDrive Tools Package") 18 | parser.add_argument( 19 | "tool", 20 | type=str, 21 | choices=tools, 22 | help="Tool from the following: %(choices)s", 23 | metavar="tool", 24 | ) 25 | return parser 26 | 27 | 28 | def action(args): 29 | file_name = BIN_PATH / (args.tool + ".py") 30 | # NOTE(greg): Shifts argv down one (and deletes the 0th arg) so the 31 | # sub-tool does not see its own name as the 1st arg. 32 | sys.argv = sys.argv[1:] 33 | # Correctly make arg0 the path to the file we're executing. 34 | sys.argv[0] = str(file_name) 35 | tool = getattr(bd_tools.bin, args.tool) 36 | tool.action(tool.parser_args()) 37 | 38 | 39 | def main(): 40 | # NOTE(greg): We have to hack the help to make sure it only operates on 41 | # this argparser if its the first arg. 42 | tool_help = False 43 | if "-h" in sys.argv or "--help" in sys.argv: 44 | if not (sys.argv[1] == "-h" or sys.argv[1] == "--help"): 45 | tool_help = True 46 | if "-h" in sys.argv: 47 | sys.argv.remove("-h") 48 | if "--help" in sys.argv: 49 | sys.argv.remove("--help") 50 | 51 | # Need to cache args to sub-command so the parser doesn't see them as 52 | # "unrecognized arguments" 53 | cache_args = [] 54 | if len(sys.argv) > 2: 55 | cache_args = sys.argv[2:] 56 | sys.argv = sys.argv[:2] 57 | 58 | args = parser_args(get_tools()).parse_args() 59 | 60 | sys.argv.extend(cache_args) 61 | 62 | # If we requested a tool help, inject it back into the sys args for the 63 | # sub-tool to process. 64 | if tool_help: 65 | sys.argv.append("--help") 66 | 67 | action(args) 68 | 69 | 70 | if __name__ == "__main__": 71 | main() 72 | -------------------------------------------------------------------------------- /bd_tools/src/bd_tools/bin/__init__.py: -------------------------------------------------------------------------------- 1 | """Executable tools for BetzDrive""" 2 | 3 | import bd_tools.bin.calibrate_encoder 4 | import bd_tools.bin.control_motor 5 | import bd_tools.bin.read_sensor 6 | import bd_tools.bin.update_calibration 7 | import bd_tools.bin.upload_bootloader 8 | import bd_tools.bin.upload_calibration 9 | import bd_tools.bin.upload_firmware 10 | import bd_tools.bin.view_control_loop # noqa: F401 11 | import bd_tools.bin.view_time # noqa: F401 12 | -------------------------------------------------------------------------------- /bd_tools/src/bd_tools/bin/control_motor.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | from __future__ import print_function 3 | 4 | import argparse 5 | import ast 6 | 7 | import serial 8 | 9 | from bd_tools import boards, comms, utils 10 | 11 | 12 | def parser_args(): 13 | parser = argparse.ArgumentParser( 14 | description="Drive motor module(s) with a given control mode." 15 | ) 16 | parser.add_argument( 17 | "--num_iters", 18 | type=int, 19 | help="Number of iterations to loop (default to infinity)", 20 | ) 21 | boards.addBoardArgs(parser) 22 | boards.addMotorControlArgs(parser) 23 | parser.set_defaults( 24 | baud_rate=comms.COMM_DEFAULT_BAUD_RATE, 25 | num_iters=0, 26 | offset=comms.COMM_BOOTLOADER_OFFSET, 27 | ) 28 | return parser.parse_args() 29 | 30 | 31 | def action(args): 32 | def make_list(x): 33 | return list(x) if (type(x) == list or type(x) == tuple) else [x] 34 | 35 | def make_type(x, to_type): 36 | return [to_type(y) for y in x] 37 | 38 | board_ids = make_type(make_list(ast.literal_eval(args.board_ids)), int) 39 | actuations = make_list(ast.literal_eval(args.actuations)) 40 | 41 | mode = args.mode 42 | 43 | ser = serial.Serial( 44 | port=args.serial, baudrate=args.baud_rate, timeout=0.001 45 | ) 46 | 47 | client = comms.BLDCControllerClient(ser) 48 | boards.initBoards(client, board_ids) 49 | 50 | client.leaveBootloader(board_ids) 51 | 52 | client.resetInputBuffer() 53 | 54 | boards.initMotor(client, board_ids) 55 | 56 | def callback() -> bool: 57 | boards.clearWDGRST(client) 58 | 59 | try: 60 | boards.driveMotor(client, board_ids, actuations, mode) 61 | except (comms.ProtocolError, comms.MalformedPacketError): 62 | return False 63 | 64 | return True 65 | 66 | loop = utils.DebugLoop(callback, args.num_iters, iters_per_print=1000) 67 | 68 | loop.loop() 69 | 70 | 71 | if __name__ == "__main__": 72 | action(parser_args()) 73 | -------------------------------------------------------------------------------- /bd_tools/src/bd_tools/bin/read_sensor.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import argparse 3 | import struct 4 | 5 | import serial 6 | 7 | from bd_tools import boards, comms, utils 8 | 9 | ReadOnlyRegs = {} 10 | ReadOnlyRegs["encoder"] = boards.COMM_ROR_ROTOR_POS 11 | ReadOnlyRegs["encoder_raw"] = boards.COMM_ROR_ROTOR_POS_RAW 12 | ReadOnlyRegs["velocity"] = boards.COMM_ROR_ROTOR_VEL 13 | ReadOnlyRegs["id"] = boards.COMM_ROR_CURRENT_DIRECT 14 | ReadOnlyRegs["iq"] = boards.COMM_ROR_CURRENT_QUADRATURE 15 | ReadOnlyRegs["supply"] = boards.COMM_ROR_SUPPLY_V 16 | ReadOnlyRegs["temp"] = boards.COMM_ROR_TEMPERATURE 17 | ReadOnlyRegs["imu"] = boards.COMM_ROR_ACC_X 18 | 19 | 20 | def parser_args(): 21 | parser = argparse.ArgumentParser(description="Read a sensor from boards.") 22 | boards.addBoardArgs(parser) 23 | parser.add_argument( 24 | "--num_iters", 25 | type=int, 26 | help="Number of iterations to loop (default to infinity)", 27 | ) 28 | parser.add_argument( 29 | "sensor", 30 | type=str, 31 | help="Choose sensor (encoder, encoder_raw, velocity, id, iq, supply, " 32 | "temp, imu)", 33 | ) 34 | parser.set_defaults( 35 | baud_rate=comms.COMM_DEFAULT_BAUD_RATE, 36 | num_iters=0, 37 | offset=comms.COMM_BOOTLOADER_OFFSET, 38 | ) 39 | return parser.parse_args() 40 | 41 | 42 | def action(args): 43 | ser = serial.Serial( 44 | port=args.serial, baudrate=args.baud_rate, timeout=0.004 45 | ) 46 | 47 | board_ids = [int(bid) for bid in args.board_ids.split(",")] 48 | 49 | client = comms.BLDCControllerClient(ser) 50 | 51 | boards.initBoards(client, board_ids) 52 | 53 | for bid in board_ids: 54 | client.leaveBootloader([bid]) 55 | 56 | sen = args.sensor 57 | address = ReadOnlyRegs[sen] 58 | decode = " bool: 85 | boards.clearWDGRST(client) 86 | 87 | try: 88 | responses = client.readRegisters( 89 | board_ids, [address] * num_boards, [num_regs] * num_boards 90 | ) 91 | for i in range(len(responses)): 92 | if not responses[i]: 93 | print("Board {} could not be read.".format(i)) 94 | continue 95 | 96 | val = struct.unpack(decode, responses[i]) 97 | bid = board_ids[i] 98 | print("Board:", bid, message.format(args.sensor, val)) 99 | 100 | except (comms.MalformedPacketError, comms.ProtocolError) as e: 101 | print(e) 102 | return False 103 | 104 | loop = utils.DebugLoop( 105 | callback=callback, num_iters=args.num_iters, iters_per_print=1000 106 | ) 107 | 108 | loop.loop() 109 | 110 | 111 | if __name__ == "__main__": 112 | action(parser_args()) 113 | -------------------------------------------------------------------------------- /bd_tools/src/bd_tools/bin/update_calibration.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import argparse 3 | import ast 4 | import time 5 | 6 | import serial 7 | 8 | from bd_tools import boards, comms 9 | 10 | 11 | def parser_args(): 12 | parser = argparse.ArgumentParser( 13 | description="Upload calibration values to motor driver board(s)" 14 | ) 15 | boards.addBoardArgs(parser) 16 | parser.set_defaults(baud_rate=comms.COMM_DEFAULT_BAUD_RATE) 17 | return parser.parse_args() 18 | 19 | 20 | def action(args): 21 | ser = serial.Serial(port=args.serial, baudrate=args.baud_rate, timeout=2.0) 22 | time.sleep(0.2) 23 | ser.reset_input_buffer() 24 | 25 | def make_list(x): 26 | return list(x) if (type(x) == list or type(x) == tuple) else [x] 27 | 28 | def make_int(x): 29 | return [int(y) for y in x] 30 | 31 | board_ids = make_int(make_list(ast.literal_eval(args.board_ids))) 32 | 33 | client = comms.BLDCControllerClient(ser) 34 | 35 | initialized = boards.initBoards(client, board_ids) 36 | 37 | client.resetInputBuffer() 38 | 39 | large_motor_tc = 1.45 40 | 41 | if initialized: 42 | for board_id in board_ids: 43 | client.leaveBootloader([board_id]) 44 | 45 | torque_const = client.getTorqueConstant([board_id])[0] 46 | 47 | client.setWatchdogTimeout([board_id], [1000]) 48 | 49 | # Setting gains for motor 50 | client.setDirectCurrentKp([board_id], [0.5]) 51 | client.setDirectCurrentKi([board_id], [0.1]) 52 | client.setQuadratureCurrentKp([board_id], [1.0]) 53 | client.setQuadratureCurrentKi([board_id], [0.2]) 54 | 55 | # Velocity controller is not used right now. Tunings need to be 56 | # adjusted. 57 | client.setVelocityKp([board_id], [0.5]) 58 | client.setVelocityKd([board_id], [0.01]) 59 | 60 | if torque_const > large_motor_tc - 0.01: 61 | print("Large motor detected") 62 | client.setPositionKp([board_id], [1.0]) 63 | client.setPositionKd([board_id], [1000.0]) 64 | else: 65 | print("Small motor detected") 66 | client.setPositionKp([board_id], [0.5]) 67 | client.setPositionKd([board_id], [100.0]) 68 | client.setVelocityKp([board_id], [0.2]) 69 | client.setVelocityKd([board_id], [10.0]) 70 | client.setHfVelocityAlpha([board_id], [0.001]) 71 | client.setLfVelocityAlpha([board_id], [1.0 - 0.9975]) 72 | 73 | # Modifying Limits 74 | client.setCurrentLimit([board_id], [2.0]) 75 | client.setTorqueLimit([board_id], [3.0]) 76 | client.setVelocityLimit([board_id], [10.0]) 77 | 78 | # Store Calibration struct to Parameters 79 | client.storeCalibration([board_id]) 80 | 81 | print("Updated:", board_id) 82 | 83 | print("Finished Updating Calibrations") 84 | 85 | ser.close() 86 | 87 | 88 | if __name__ == "__main__": 89 | action(parser_args()) 90 | -------------------------------------------------------------------------------- /bd_tools/src/bd_tools/bin/upload_bootloader.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import argparse 3 | import time 4 | 5 | import serial 6 | 7 | from bd_tools import boards, comms 8 | 9 | 10 | def parser_args(): 11 | parser = argparse.ArgumentParser( 12 | description="Upload bootloader to a motor controller board." 13 | ) 14 | boards.addBoardArgs(parser) 15 | parser.add_argument( 16 | "bin_file", type=str, help=".bin file containing bootloader image" 17 | ) 18 | parser.add_argument( 19 | "--offset", type=int, help="Offset address for bootloader image" 20 | ) 21 | parser.set_defaults( 22 | baud_rate=comms.COMM_DEFAULT_BAUD_RATE, 23 | offset=comms.COMM_BOOTLOADER_OFFSET, 24 | ) 25 | return parser.parse_args() 26 | 27 | 28 | def action(args): 29 | board_ids = [int(bid) for bid in args.board_ids.split(",")] 30 | 31 | ser = serial.Serial(port=args.serial, baudrate=args.baud_rate, timeout=2.0) 32 | time.sleep(0.1) 33 | 34 | client = comms.BLDCControllerClient(ser) 35 | 36 | initialized = boards.initBoards(client, board_ids) 37 | 38 | ser.reset_input_buffer() 39 | 40 | if initialized: 41 | crashed = client.checkWDGRST() 42 | if crashed: 43 | print( 44 | "Some boards have crashed, please power cycle before upload:", 45 | crashed, 46 | ) 47 | 48 | if not crashed: 49 | for board_id in board_ids: 50 | client.leaveBootloader([board_id]) 51 | time.sleep(0.2) # Wait for the controller to reset 52 | ser.reset_input_buffer() 53 | 54 | flash_sector_maps = client.getFlashSectorMap([board_id]) 55 | 56 | with open(args.bin_file, "rb") as bin_file: 57 | firmware_image = bin_file.read() 58 | 59 | success = False 60 | while not success: 61 | try: 62 | success = client.writeFlash( 63 | [board_id], 64 | args.offset, 65 | firmware_image, 66 | sector_map=flash_sector_maps, 67 | print_progress=True, 68 | ) 69 | except ( 70 | comms.MalformedPacketError, 71 | comms.ProtocolError, 72 | ) as e: 73 | print(f"Upload to board {board_id} failed with error:") 74 | print(e) 75 | print("Retrying...") 76 | continue 77 | 78 | if success: 79 | print("Upload to Board", board_id, "Succeeded") 80 | else: 81 | print("Upload to Board", board_id, "Failed") 82 | break 83 | 84 | ser.close() 85 | 86 | 87 | if __name__ == "__main__": 88 | action(parser_args()) 89 | -------------------------------------------------------------------------------- /bd_tools/src/bd_tools/bin/upload_calibration.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import argparse 4 | import ast 5 | import json 6 | import time 7 | 8 | import serial 9 | 10 | from bd_tools import boards, comms 11 | 12 | 13 | def parser_args(): 14 | parser = argparse.ArgumentParser( 15 | description="Upload calibration values to motor driver board(s)" 16 | ) 17 | boards.addBoardArgs(parser) 18 | parser.add_argument( 19 | "--calibration_file", 20 | type=str, 21 | help="The file which the calibration(s) is/are in", 22 | ) 23 | parser.set_defaults( 24 | baud_rate=comms.COMM_DEFAULT_BAUD_RATE, 25 | calibration_file="calibrations.json", 26 | ) 27 | return parser.parse_args() 28 | 29 | 30 | def action(args): 31 | ser = serial.Serial(port=args.serial, baudrate=args.baud_rate, timeout=2.0) 32 | time.sleep(0.2) 33 | ser.reset_input_buffer() 34 | 35 | def make_list(x): 36 | return list(x) if (type(x) == list or type(x) == tuple) else [x] 37 | 38 | def make_int(x): 39 | return [int(y) for y in x] 40 | 41 | board_ids = make_int(make_list(ast.literal_eval(args.board_ids))) 42 | 43 | # Load in Custom Values 44 | with open(args.calibration_file) as json_file: 45 | calibration = json.load(json_file) 46 | 47 | client = comms.BLDCControllerClient(ser) 48 | 49 | initialized = boards.initBoards(client, board_ids) 50 | 51 | client.resetInputBuffer() 52 | 53 | if initialized: 54 | for board_id, calib in zip(board_ids, calibration): 55 | print(board_id, "-", calib) 56 | client.leaveBootloader([board_id]) 57 | 58 | # Reset Calibration on Board 59 | client.clearCalibration([board_id]) 60 | 61 | boards.loadCalibrationFromJSON(client, board_id, calib) 62 | 63 | client.setWatchdogTimeout([board_id], [1000]) 64 | 65 | # Setting gains for motor 66 | client.setDirectCurrentKp([board_id], [0.5]) 67 | client.setDirectCurrentKi([board_id], [0.1]) 68 | client.setQuadratureCurrentKp([board_id], [1.0]) 69 | client.setQuadratureCurrentKi([board_id], [0.2]) 70 | 71 | # Store Calibration struct to Parameters 72 | client.storeCalibration([board_id]) 73 | 74 | ser.close() 75 | 76 | 77 | if __name__ == "__main__": 78 | action(parser_args()) 79 | -------------------------------------------------------------------------------- /bd_tools/src/bd_tools/bin/upload_firmware.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import argparse 3 | import time 4 | 5 | import serial 6 | 7 | from bd_tools import boards, comms 8 | 9 | 10 | def parser_args(): 11 | parser = argparse.ArgumentParser( 12 | description="Upload firmware to a motor controller board." 13 | ) 14 | boards.addBoardArgs(parser) 15 | parser.add_argument( 16 | "bin_file", type=str, help=".bin file containing firmware image" 17 | ) 18 | parser.add_argument( 19 | "--offset", type=int, help="Offset address for firmware image" 20 | ) 21 | parser.set_defaults( 22 | baud_rate=comms.COMM_DEFAULT_BAUD_RATE, 23 | offset=comms.COMM_FIRMWARE_OFFSET, 24 | ) 25 | return parser.parse_args() 26 | 27 | 28 | def action(args): 29 | board_ids = [int(bid) for bid in args.board_ids.split(",")] 30 | 31 | ser = serial.Serial(port=args.serial, baudrate=args.baud_rate, timeout=2.0) 32 | time.sleep(0.1) 33 | 34 | client = comms.BLDCControllerClient(ser) 35 | 36 | initialized = boards.initBoards(client, board_ids) 37 | 38 | ser.reset_input_buffer() 39 | 40 | if initialized: 41 | crashed = client.checkWDGRST() 42 | if crashed: 43 | print( 44 | "Some boards have crashed, please power cycle before upload:", 45 | crashed, 46 | ) 47 | 48 | if not crashed: 49 | for board_id in board_ids: 50 | flash_sector_maps = client.getFlashSectorMap([board_id]) 51 | 52 | with open(args.bin_file, "rb") as bin_file: 53 | firmware_image = bin_file.read() 54 | 55 | success = False 56 | while not success: 57 | try: 58 | success = client.writeFlash( 59 | [board_id], 60 | args.offset, 61 | firmware_image, 62 | sector_map=flash_sector_maps, 63 | print_progress=True, 64 | ) 65 | except ( 66 | comms.MalformedPacketError, 67 | comms.ProtocolError, 68 | ) as e: 69 | print(f"Upload to board {board_id} failed with error:") 70 | print(e) 71 | print("Retrying...") 72 | continue 73 | 74 | if success: 75 | print("Upload to Board", board_id, "Succeeded") 76 | else: 77 | print("Upload to Board", board_id, "Failed") 78 | break 79 | 80 | ser.close() 81 | 82 | 83 | if __name__ == "__main__": 84 | action(parser_args()) 85 | -------------------------------------------------------------------------------- /bd_tools/src/bd_tools/bin/view_control_loop.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import argparse 4 | import ast 5 | import struct 6 | import time 7 | 8 | import serial 9 | 10 | from bd_tools import boards, comms, livegraph 11 | 12 | 13 | def parser_args(): 14 | parser = argparse.ArgumentParser( 15 | description="Drive motor module(s) with a given control mode and plot " 16 | "current measurements." 17 | ) 18 | boards.addBoardArgs(parser) 19 | boards.addMotorControlArgs(parser) 20 | parser.set_defaults( 21 | baud_rate=comms.COMM_DEFAULT_BAUD_RATE, 22 | offset=comms.COMM_BOOTLOADER_OFFSET, 23 | ) 24 | return parser.parse_args() 25 | 26 | 27 | def action(args): 28 | make_list = ( 29 | lambda x: list(x) if (type(x) == list or type(x) == tuple) else [x] 30 | ) 31 | 32 | def make_int(x): 33 | return [int(y) for y in x] 34 | 35 | board_ids = make_int(make_list(ast.literal_eval(args.board_ids))) 36 | actuations = make_list(ast.literal_eval(args.actuations)) 37 | 38 | mode = args.mode 39 | 40 | ser = serial.Serial( 41 | port=args.serial, baudrate=args.baud_rate, timeout=0.05 42 | ) 43 | 44 | client = comms.BLDCControllerClient(ser) 45 | boards.initBoards(client, board_ids) 46 | 47 | client.leaveBootloader(board_ids) 48 | client.resetInputBuffer() 49 | 50 | boards.initMotor(client, board_ids) 51 | 52 | def updateCurrent(i): 53 | data = [] 54 | for board_id in board_ids: 55 | try: 56 | boards.driveMotor(client, board_ids, actuations, mode) 57 | # Read the iq calulated 58 | data.append( 59 | struct.unpack( 60 | " e: 30 | time.sleep(0.1) 31 | p = n 32 | n = readAngle(address) 33 | return n 34 | 35 | 36 | def readAngle(address): 37 | return struct.unpack(" next_step: 37 | print ("hellO") 38 | position_setpoint += 1000 39 | position_setpoint %= 2 ** 14 40 | next_step += 1 41 | -------------------------------------------------------------------------------- /bd_tools/src/bd_tools/dev/test_rs485.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | from __future__ import print_function 3 | 4 | import argparse 5 | import random 6 | import sys 7 | import time 8 | 9 | import numpy as np 10 | import serial 11 | 12 | DEFAULT_BAUD_RATE = 3000000 # 1 Mbps 13 | 14 | if __name__ == "__main__": 15 | parser = argparse.ArgumentParser( 16 | description="Drive motor module(s) with a given control mode." 17 | ) 18 | parser.add_argument( 19 | "transmit", 20 | type=str, 21 | help="Transmitter serial port (i.e. /dev/ttyUSB0)", 22 | ) 23 | parser.add_argument("receive", type=str, help="Receiver serial port") 24 | parser.add_argument("--baud_rate", type=int, help="Serial baud rate") 25 | parser.set_defaults(baud_rate=DEFAULT_BAUD_RATE) 26 | args = parser.parse_args() 27 | 28 | trn = serial.Serial( 29 | port=args.transmit, baudrate=args.baud_rate, timeout=2.0 30 | ) 31 | rec = serial.Serial( 32 | port=args.receive, baudrate=args.baud_rate, timeout=2.0 33 | ) 34 | 35 | trn.reset_input_buffer() 36 | rec.reset_input_buffer() 37 | 38 | # Generate 1k random bits 39 | s = random.randint(0, 2 ** 1000 - 1) 40 | msg = hex(s) 41 | print(msg) 42 | 43 | trn.write(msg) 44 | 45 | time.sleep(0.01) 46 | 47 | return_msg = rec.read_all() 48 | print(return_msg) 49 | 50 | print("Equal:", return_msg == msg) 51 | -------------------------------------------------------------------------------- /bd_tools/src/bd_tools/dev/usb_latency.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | cat /sys/bus/usb-serial/devices/ttyUSB0/latency_timer 3 | echo 1 > /sys/bus/usb-serial/devices/ttyUSB0/latency_timer 4 | cat /sys/bus/usb-serial/devices/ttyUSB0/latency_timer 5 | -------------------------------------------------------------------------------- /bd_tools/src/bd_tools/livegraph.py: -------------------------------------------------------------------------------- 1 | """ Live Graph Plotting - By: Greg Balke """ 2 | 3 | import threading 4 | import time 5 | 6 | import matplotlib.animation as animation 7 | import matplotlib.pyplot as plt 8 | from matplotlib import style 9 | from matplotlib.ticker import MaxNLocator 10 | 11 | 12 | class LiveGraph: 13 | 14 | """ 15 | This graph spawns a secondary thread to update your data while the main 16 | thread plots. Pressing 'p' during operation will toggle the plotting and 17 | display the entire dataset. 18 | 19 | """ 20 | 21 | def __init__( 22 | self, 23 | update_func, 24 | labels, 25 | sample_interval=1, 26 | plot_interval=100, 27 | window_size=1000, 28 | ): 29 | """ 30 | Initialize the livegraph visualizer. 31 | 32 | Arguments: 33 | update_func: user defined function called at the update interval 34 | Expect data to look like (x, [y1, y2, ..., yn]) 35 | labels: labels matching the dim of the y axis of returned data. 36 | sample_interval: interval at which the update function is run 37 | (in milliseconds). 38 | plot_interval: interval at which the plot is updated (in 39 | milliseconds). 40 | window_size: number of samples to show when plotting is not paused. 41 | """ 42 | self.labels = labels 43 | self.sample_interval = sample_interval / 1000.0 44 | self.plot_interval = plot_interval 45 | self.window_size = window_size 46 | self.update_func = update_func 47 | 48 | self.data = self.Data() 49 | self.graph = self.Graph( 50 | self.data, 51 | self.labels, 52 | interval=self.plot_interval, 53 | window_size=self.window_size, 54 | ) 55 | self.update_thread = self.Update( 56 | self.data, self.update_func, interval=self.sample_interval 57 | ) 58 | 59 | def start(self): 60 | self.update_thread.start() 61 | plt.show() 62 | 63 | self.data.running = False 64 | self.update_thread.join() 65 | 66 | class Graph: 67 | 68 | """Sub class to manage running the graph animation update interface.""" 69 | 70 | def __init__(self, data, labels, interval=10, window_size=100): 71 | self.data = data 72 | self.num_items = len(labels) 73 | self.labels = labels 74 | self.window_size = window_size 75 | 76 | style.use("fivethirtyeight") 77 | 78 | self.fig = plt.figure() 79 | self.ax = self.fig.add_subplot(1, 1, 1) 80 | 81 | self.pause = False 82 | self.drawn = False 83 | 84 | self.fig.canvas.mpl_connect("key_press_event", self.onPress) 85 | self.ani = animation.FuncAnimation( 86 | self.fig, self.redraw, interval=interval, repeat=True 87 | ) 88 | 89 | def redraw(self, i): 90 | # When paused, draw entire plot 91 | if self.pause: 92 | ys = self.data.ys 93 | xs = self.data.xs 94 | # When not paused, draw only the window 95 | else: 96 | ys = self.data.ys[-self.window_size :] 97 | xs = self.data.xs[-self.window_size :] 98 | 99 | if not self.drawn: 100 | self.ax.clear() 101 | self.ax.xaxis.set_major_locator(MaxNLocator(integer=True)) 102 | 103 | for item in range(self.num_items): 104 | dat = [y[item] for y in ys] 105 | self.ax.plot(xs, dat, label=self.labels[item]) 106 | 107 | self.ax.legend(loc="upper left") 108 | 109 | if self.pause: 110 | self.drawn = True 111 | else: 112 | self.drawn = False 113 | 114 | def onPress(self, event): 115 | if event.key == "p": 116 | self.pause ^= True 117 | 118 | class Data: 119 | 120 | """ 121 | Shared coordinate data object for the update and graph sub-classes. 122 | 123 | Attributes: 124 | running: This tracks the state of the graph such that when the 125 | graph closes, the updater stops. 126 | """ 127 | 128 | def __init__(self): 129 | self.xs = [] 130 | self.ys = [] 131 | self.running = True 132 | 133 | class Update(threading.Thread): 134 | 135 | """Handle updating user data from the referenced function!""" 136 | 137 | def __init__(self, data, update_func, interval=1): 138 | threading.Thread.__init__(self) 139 | 140 | self.data = data 141 | self.interval = interval 142 | self.nextCall = time.time() 143 | self.update_func = update_func 144 | 145 | def run(self): 146 | count = 0 147 | while self.data.running: 148 | x, y = self.update_func(count) 149 | if x is not None and y is not None: 150 | self.data.xs.append(x) 151 | self.data.ys.append(y) 152 | self.nextCall = self.nextCall + self.interval 153 | time.sleep(max(0, self.nextCall - time.time())) 154 | count += 1 155 | -------------------------------------------------------------------------------- /bd_tools/src/bd_tools/recorder/.gitignore: -------------------------------------------------------------------------------- 1 | old_logs/ 2 | -------------------------------------------------------------------------------- /bd_tools/src/bd_tools/recorder/log.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import sys 4 | 5 | sys.path.append("..") 6 | 7 | import argparse 8 | import pickle 9 | import pprint 10 | import time 11 | 12 | import serial 13 | from comms import * 14 | 15 | if __name__ == "__main__": 16 | parser = argparse.ArgumentParser( 17 | description="Calibrate the encoder on a motor controller board." 18 | ) 19 | parser.add_argument("serial", type=str, help="Serial port") 20 | parser.add_argument("--baud_rate", type=int, help="Serial baud rate") 21 | parser.add_argument("board_id", type=int, help="Board ID") 22 | parser.add_argument("file_name", type=str, help="File name to record data") 23 | parser.add_argument("--steps", type=int, help="Number of steps") 24 | parser.add_argument("--delay", type=float, help="Delay between steps") 25 | parser.set_defaults( 26 | baud_rate=COMM_DEFAULT_BAUD_RATE, 27 | duty_cycle=0.6, 28 | steps=50, 29 | delay=0.1, 30 | file_name="", 31 | ) 32 | args = parser.parse_args() 33 | 34 | # 35 | # Data collection 36 | # 37 | 38 | ser = serial.Serial(port=args.serial, baudrate=args.baud_rate, timeout=0.1) 39 | time.sleep(0.1) 40 | 41 | client = BLDCControllerClient(ser) 42 | 43 | client.enterBootloader([args.board_id]) 44 | time.sleep(0.2) 45 | try: 46 | print(client.enumerateBoards([args.board_id])) 47 | except: 48 | print("Failed to receive enumerate response") 49 | time.sleep(0.2) 50 | 51 | client.leaveBootloader([args.board_id]) 52 | time.sleep(0.2) # Wait for the controller to reset 53 | ser.reset_input_buffer() 54 | 55 | client.writeRegisters( 56 | [args.board_id], [0x1030], [1], [struct.pack("= self._num_iters and self._num_iters != 0: 63 | break 64 | except KeyboardInterrupt: 65 | print() # Clear line immediately after the ctrl-c 66 | print( 67 | f"Interrupted. Loop exiting. Completed {self._iters} " 68 | f"iterations." 69 | ) 70 | pass 71 | -------------------------------------------------------------------------------- /bd_tools/udev_latency/.gitignore: -------------------------------------------------------------------------------- 1 | *.rules 2 | -------------------------------------------------------------------------------- /bd_tools/udev_latency/low_latency.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | echo ACTION==\"add\", SUBSYSTEM==\"usb-serial\", DRIVER==\"ftdi_sio\", ATTR{latency_timer}=\"1\" > 99-bldc-controller-usb.rules 3 | sudo cp ./99-bldc-controller-usb.rules /etc/udev/rules.d/ 4 | sudo udevadm control --reload-rules 5 | sudo udevadm trigger --action=add 6 | -------------------------------------------------------------------------------- /bootloader/.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | .dep/ 3 | -------------------------------------------------------------------------------- /bootloader/BUILD.bazel: -------------------------------------------------------------------------------- 1 | load("@rules_cc//cc:defs.bzl", "cc_binary", "cc_library") 2 | load("//third_party:variables.bzl", "project_inc_paths") 3 | load("//toolchains:debug.bzl", "debug_gdb") 4 | load("//toolchains:platforms.bzl", "bootloader_binary", "gen_binary") 5 | load("//toolchains:os_config.bzl", "os_config") 6 | load("//toolchains:flash.bzl", "flash_remote") 7 | load("@bazel_embedded//tools/openocd:defs.bzl", "openocd_debug_server", "openocd_flash") 8 | 9 | bootloader_binary( 10 | name = "bootloader_raw", 11 | srcs = glob(["src/*.c"]) + glob(["src/*.cpp"]), 12 | copts = project_inc_paths, 13 | linkopts = [ 14 | "-T $(location :link.ld)", 15 | ], 16 | target_compatible_with = [ 17 | "@platforms//cpu:armv7e-m", 18 | ], 19 | visibility = ["//visibility:public"], 20 | deps = [ 21 | ":link.ld", 22 | ":main_libs", 23 | ], 24 | ) 25 | 26 | cc_library( 27 | name = "main_libs", 28 | hdrs = glob( 29 | ["include/*.h"], 30 | exclude = [ 31 | "include/stm32f4xx_conf.h", 32 | "include/chconf.h", 33 | "include/halconf.h", 34 | "include/mcuconf.h", 35 | ], 36 | ) + glob( 37 | ["include/*.hpp"], 38 | exclude = [ 39 | "include/peripherals.hpp", 40 | ], 41 | ), 42 | copts = project_inc_paths, 43 | includes = ["include"], 44 | deps = [ 45 | "//common", 46 | ], 47 | alwayslink = True, 48 | ) 49 | 50 | filegroup( 51 | name = "peripheral_config", 52 | srcs = ["include/peripherals.hpp"], 53 | visibility = ["//visibility:public"], 54 | ) 55 | 56 | os_config( 57 | name = "os_config", 58 | # NOTE: This gets used by comms.cpp in common to enable the jump code path. 59 | defines = ["BOOTLOADER"], 60 | program = "bootloader", 61 | ) 62 | 63 | gen_binary( 64 | name = "bootloader", 65 | src = ":bootloader_raw", 66 | ) 67 | 68 | flash_remote( 69 | name = "upload", 70 | image = ":bootloader.bin", 71 | upload_type = "bootloader", 72 | ) 73 | 74 | openocd_flash( 75 | name = "flash", 76 | device_configs = [ 77 | "target/stm32f4x.cfg", 78 | ], 79 | image = ":bootloader.bin", 80 | interface_configs = [ 81 | "interface/stlink.cfg", 82 | ], 83 | ) 84 | 85 | openocd_debug_server( 86 | name = "debug", 87 | device_configs = [ 88 | "target/stm32f4x.cfg", 89 | ], 90 | interface_configs = [ 91 | "interface/stlink.cfg", 92 | ], 93 | transport = "hla_swd", 94 | ) 95 | 96 | debug_gdb( 97 | name = "gdb", 98 | elf = ":bootloader.elf", 99 | ) 100 | -------------------------------------------------------------------------------- /bootloader/CPPLINT.cfg: -------------------------------------------------------------------------------- 1 | set noparent 2 | 3 | # Clang default formats one space before comments; cpplint wants 2. 4 | filter=-whitespace/comments 5 | # TODO: Add copyright to the top of every file. 6 | filter=-legal/copyright 7 | # Include structure of chibios is weird. Figure this out? 8 | filter=-build/include_subdir 9 | # TODO: Need to pass return arguments as pointers to show explicit modification 10 | # in-line. 11 | filter=-runtime/references 12 | 13 | # Ignore c files. 14 | extensions=hpp,cpp 15 | 16 | # Simplifies include guard naming. 17 | root=include/ 18 | -------------------------------------------------------------------------------- /bootloader/include/peripherals.hpp: -------------------------------------------------------------------------------- 1 | #ifndef PERIPHERALS_HPP_ 2 | #define PERIPHERALS_HPP_ 3 | 4 | #include 5 | 6 | #include "hal.h" 7 | 8 | namespace motor_driver { 9 | namespace peripherals { 10 | 11 | constexpr UARTDriver *rs485_uart_driver = &UARTD1; 12 | 13 | extern const PWMConfig led_pwm_config; 14 | 15 | void startPeripherals(); 16 | 17 | void setStatusLEDColor(uint8_t red, uint8_t green, uint8_t blue); 18 | 19 | void setStatusLEDColor(uint32_t color); 20 | 21 | void setCommsActivityLED(bool on); 22 | 23 | void setRS485TransmitMode(bool transmit); 24 | 25 | } // namespace peripherals 26 | } // namespace motor_driver 27 | 28 | #endif // PERIPHERALS_HPP_ 29 | -------------------------------------------------------------------------------- /bootloader/include/stm32f4xx_conf.h: -------------------------------------------------------------------------------- 1 | #ifndef STM32F4XX_CONF_H_ 2 | #define STM32F4XX_CONF_H_ 3 | 4 | #define USE_RTOS 0 5 | 6 | #include "misc.h" 7 | #include "stm32f4xx_adc.h" 8 | #include "stm32f4xx_dma.h" 9 | #include "stm32f4xx_exti.h" 10 | #include "stm32f4xx_flash.h" 11 | #include "stm32f4xx_i2c.h" 12 | #include "stm32f4xx_rcc.h" 13 | #include "stm32f4xx_syscfg.h" 14 | #include "stm32f4xx_tim.h" 15 | #include "stm32f4xx_wwdg.h" 16 | 17 | #ifdef USE_FULL_ASSERT 18 | #define assert_param(expr) \ 19 | ((expr) ? (void)0 : assert_failed((uint8_t *)__FILE__, __LINE__)) 20 | void assert_failed(uint8_t *file, uint32_t line); 21 | #else 22 | #define assert_param(expr) ((void)0) 23 | #endif 24 | 25 | #endif // STM32F4XX_CONF_H_ 26 | -------------------------------------------------------------------------------- /bootloader/link.ld: -------------------------------------------------------------------------------- 1 | /* 2 | ChibiOS/RT - Copyright (C) 2006-2013 Giovanni Di Sirio 3 | 4 | Licensed under the Apache License, Version 2.0 (the "License"); 5 | you may not use this file except in compliance with the License. 6 | You may obtain a copy of the License at 7 | 8 | http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | Unless required by applicable law or agreed to in writing, software 11 | distributed under the License is distributed on an "AS IS" BASIS, 12 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | See the License for the specific language governing permissions and 14 | limitations under the License. 15 | */ 16 | 17 | /* 18 | * STM32F405xG memory setup. 19 | */ 20 | __main_stack_size__ = 0x0400; 21 | __process_stack_size__ = 0x0400; 22 | 23 | MEMORY 24 | { 25 | flash_bl : org = 0x08000000, len = 32k 26 | flash_nvp : org = 0x08000000 + 32k, len = 16k 27 | flash_id : org = 0x08000000 + 48k, len = 16k 28 | flash_fw : org = 0x08000000 + 64k, len = 1M - 64k 29 | ram : org = 0x20000000, len = 112k 30 | ethram : org = 0x2001C000, len = 16k 31 | ccmram : org = 0x10000000, len = 64k 32 | } 33 | 34 | __ram_start__ = ORIGIN(ram); 35 | __ram_size__ = LENGTH(ram); 36 | __ram_end__ = __ram_start__ + __ram_size__; 37 | 38 | ENTRY(ResetHandler) 39 | 40 | SECTIONS 41 | { 42 | . = 0; 43 | _text = .; 44 | 45 | startup : ALIGN(16) SUBALIGN(16) 46 | { 47 | KEEP(*(vectors)) 48 | } > flash_bl 49 | 50 | constructors : ALIGN(4) SUBALIGN(4) 51 | { 52 | PROVIDE(__init_array_start = .); 53 | KEEP(*(SORT(.init_array.*))) 54 | KEEP(*(.init_array)) 55 | PROVIDE(__init_array_end = .); 56 | } > flash_bl 57 | 58 | destructors : ALIGN(4) SUBALIGN(4) 59 | { 60 | PROVIDE(__fini_array_start = .); 61 | KEEP(*(.fini_array)) 62 | KEEP(*(SORT(.fini_array.*))) 63 | PROVIDE(__fini_array_end = .); 64 | } > flash_bl 65 | 66 | .text : ALIGN(16) SUBALIGN(16) 67 | { 68 | *(.text.startup.*) 69 | *(.text) 70 | *(.text.*) 71 | *(.rodata) 72 | *(.rodata.*) 73 | *(.glue_7t) 74 | *(.glue_7) 75 | *(.gcc*) 76 | } > flash_bl 77 | 78 | .ARM.extab : 79 | { 80 | *(.ARM.extab* .gnu.linkonce.armextab.*) 81 | } > flash_bl 82 | 83 | .ARM.exidx : { 84 | PROVIDE(__exidx_start = .); 85 | *(.ARM.exidx* .gnu.linkonce.armexidx.*) 86 | PROVIDE(__exidx_end = .); 87 | } > flash_bl 88 | 89 | .eh_frame_hdr : 90 | { 91 | *(.eh_frame_hdr) 92 | } > flash_bl 93 | 94 | .eh_frame : ONLY_IF_RO 95 | { 96 | *(.eh_frame) 97 | } > flash_bl 98 | 99 | .textalign : ONLY_IF_RO 100 | { 101 | . = ALIGN(8); 102 | } > flash_bl 103 | 104 | . = ALIGN(4); 105 | _etext = .; 106 | _textdata = _etext; 107 | 108 | .stacks : 109 | { 110 | . = ALIGN(8); 111 | __main_stack_base__ = .; 112 | . += __main_stack_size__; 113 | . = ALIGN(8); 114 | __main_stack_end__ = .; 115 | __process_stack_base__ = .; 116 | __main_thread_stack_base__ = .; 117 | . += __process_stack_size__; 118 | . = ALIGN(8); 119 | __process_stack_end__ = .; 120 | __main_thread_stack_end__ = .; 121 | } > ram 122 | 123 | .data : 124 | { 125 | . = ALIGN(4); 126 | PROVIDE(_data = .); 127 | *(.data) 128 | . = ALIGN(4); 129 | *(.data.*) 130 | . = ALIGN(4); 131 | *(.ramtext) 132 | . = ALIGN(4); 133 | PROVIDE(_edata = .); 134 | } > ram AT > flash_bl 135 | 136 | .bss : 137 | { 138 | . = ALIGN(4); 139 | PROVIDE(_bss_start = .); 140 | *(.bss) 141 | . = ALIGN(4); 142 | *(.bss.*) 143 | . = ALIGN(4); 144 | *(COMMON) 145 | . = ALIGN(4); 146 | PROVIDE(_bss_end = .); 147 | } > ram 148 | } 149 | 150 | PROVIDE(end = .); 151 | _end = .; 152 | 153 | __heap_base__ = _end; 154 | __heap_end__ = __ram_end__; 155 | -------------------------------------------------------------------------------- /bootloader/src/bl_comms.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "comms.hpp" 4 | 5 | #include "hal.h" 6 | 7 | #include "ch.h" 8 | #include "peripherals.hpp" 9 | 10 | namespace motor_driver { 11 | namespace comms { 12 | 13 | size_t commsRegAccessHandler(comm_addr_t start_addr, size_t reg_count, 14 | uint8_t *buf, size_t buf_size, 15 | RegAccessType access_type, comm_errors_t &errors) { 16 | (void)buf; 17 | (void)buf_size; 18 | (void)access_type; 19 | 20 | size_t index = 0; 21 | 22 | for (comm_addr_t addr = start_addr; addr < start_addr + reg_count; addr++) { 23 | switch (addr) { 24 | // No registers 25 | 26 | default: 27 | errors |= COMM_ERRORS_INVALID_ARGS; 28 | return 0; 29 | } 30 | 31 | if (errors & COMM_ERRORS_BUF_LEN_MISMATCH) { 32 | break; 33 | } 34 | } 35 | 36 | // TODO(gbalke): check if there is still data left 37 | 38 | return index; // Number of bytes read/written 39 | } 40 | 41 | } // namespace comms 42 | } // namespace motor_driver 43 | -------------------------------------------------------------------------------- /bootloader/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include "hal.h" 2 | 3 | #include "ch.h" 4 | #include "comms.hpp" 5 | #include "constants.hpp" 6 | #include "helper.h" 7 | #include "peripherals.hpp" 8 | #include "stdbool.h" 9 | #include "stdlib.h" 10 | #include "stm32f4xx.h" 11 | #include "stm32f4xx_iwdg.h" 12 | #include "string.h" 13 | 14 | namespace motor_driver { 15 | 16 | static systime_t last_comms_activity_time = 0; 17 | 18 | static void comms_activity_callback() { 19 | last_comms_activity_time = chTimeNow(); 20 | } 21 | 22 | /* 23 | * LED blinker thread 24 | */ 25 | 26 | static WORKING_AREA(blinker_thread_wa, 512); 27 | static msg_t blinkerThreadRun(void *arg) { 28 | (void)arg; 29 | 30 | chRegSetThreadName("blinker"); 31 | 32 | int t = 0; 33 | 34 | peripherals::setCommsActivityLED(false); 35 | 36 | while (true) { 37 | uint8_t g = ::abs(t - 255); 38 | peripherals::setStatusLEDColor(0, 0, g); 39 | 40 | systime_t time_diff = chTimeNow() - last_comms_activity_time; 41 | 42 | peripherals::setCommsActivityLED( 43 | time_diff < MS2ST(consts::comms_activity_led_duration) && 44 | last_comms_activity_time != 0); 45 | 46 | t = (t + 10) % 510; 47 | chThdSleepMilliseconds(10); 48 | } 49 | 50 | // Should never get here. 51 | return CH_SUCCESS; 52 | } 53 | 54 | /* 55 | * Communications thread 56 | */ 57 | 58 | static WORKING_AREA(comms_thread_wa, 512); 59 | static msg_t commsThreadRun(void *arg) { 60 | (void)arg; 61 | 62 | chRegSetThreadName("comms"); 63 | 64 | comms::startComms(); 65 | 66 | while (true) { 67 | comms::runComms(); 68 | } 69 | 70 | // Should never get here. 71 | return CH_SUCCESS; 72 | } 73 | 74 | int main(void) { 75 | // Start RTOS. 76 | halInit(); 77 | chSysInit(); 78 | 79 | // Check if the system reset off the watchdog and bump back into firmware. 80 | if (RCC->CSR & RCC_CSR_WDGRSTF) { 81 | flashJumpApplication((uint32_t)consts::firmware_ptr); 82 | } 83 | 84 | // Start peripherals. 85 | peripherals::startPeripherals(); 86 | 87 | // Set comms activity callback. 88 | comms::comms_protocol_fsm.setActivityCallback(&comms_activity_callback); 89 | 90 | // Start threads. 91 | chThdCreateStatic(blinker_thread_wa, sizeof(blinker_thread_wa), LOWPRIO, 92 | blinkerThreadRun, NULL); 93 | chThdCreateStatic(comms_thread_wa, sizeof(comms_thread_wa), NORMALPRIO, 94 | commsThreadRun, NULL); 95 | 96 | // Wait forever. 97 | while (true) { 98 | chThdSleepMilliseconds(1000); 99 | } 100 | 101 | // Should never get here. 102 | return CH_SUCCESS; 103 | } 104 | 105 | } // namespace motor_driver 106 | 107 | // FIXME: hack 108 | int main(void) { return motor_driver::main(); } 109 | -------------------------------------------------------------------------------- /bootloader/src/peripherals.cpp: -------------------------------------------------------------------------------- 1 | #include "peripherals.hpp" 2 | 3 | #include "constants.hpp" 4 | 5 | namespace motor_driver { 6 | namespace peripherals { 7 | 8 | // Hz. 9 | constexpr unsigned int led_pwm_clock_freq = 84000000; 10 | // clock cycles. 11 | constexpr unsigned int led_pwm_period = 52500; 12 | 13 | const PWMConfig led_pwm_config = {led_pwm_clock_freq, 14 | led_pwm_period, 15 | NULL, 16 | {{PWM_OUTPUT_ACTIVE_LOW, NULL}, 17 | {PWM_OUTPUT_ACTIVE_LOW, NULL}, 18 | {PWM_OUTPUT_ACTIVE_LOW, NULL}, 19 | {PWM_OUTPUT_DISABLED, NULL}}, 20 | 0, 21 | 0}; 22 | 23 | void startPeripherals() { 24 | // Start LED PWM. 25 | pwmStart(&PWMD5, &led_pwm_config); 26 | } 27 | 28 | static uint16_t ledPWMPulseWidthFromIntensity(uint8_t intensity) { 29 | return consts::led_gamma_table[intensity]; 30 | } 31 | 32 | void setStatusLEDColor(uint8_t red, uint8_t green, uint8_t blue) { 33 | pwmEnableChannel(&PWMD5, 0, ledPWMPulseWidthFromIntensity(red)); 34 | pwmEnableChannel(&PWMD5, 2, ledPWMPulseWidthFromIntensity(green)); 35 | pwmEnableChannel(&PWMD5, 1, ledPWMPulseWidthFromIntensity(blue)); 36 | } 37 | 38 | void setStatusLEDColor(uint32_t color) { 39 | setStatusLEDColor(color >> 16, color >> 8, color); 40 | } 41 | 42 | void setCommsActivityLED(bool on) { 43 | palWritePad(GPIOB, GPIOB_LED_Y, static_cast(!on)); 44 | } 45 | 46 | void setRS485TransmitMode(bool transmit) { 47 | palWritePad(GPIOD, GPIOD_RS485_DIR, static_cast(transmit)); 48 | } 49 | 50 | } // namespace peripherals 51 | } // namespace motor_driver 52 | -------------------------------------------------------------------------------- /common/.cproject: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /common/.project: -------------------------------------------------------------------------------- 1 | 2 | 3 | ARMCM4-STM32F407-DISCOVERY 4 | 5 | 6 | 7 | 8 | 9 | org.eclipse.cdt.managedbuilder.core.genmakebuilder 10 | clean,full,incremental, 11 | 12 | 13 | ?name? 14 | 15 | 16 | 17 | org.eclipse.cdt.make.core.append_environment 18 | true 19 | 20 | 21 | org.eclipse.cdt.make.core.autoBuildTarget 22 | all 23 | 24 | 25 | org.eclipse.cdt.make.core.buildArguments 26 | -j 27 | 28 | 29 | org.eclipse.cdt.make.core.buildCommand 30 | make 31 | 32 | 33 | org.eclipse.cdt.make.core.cleanBuildTarget 34 | clean 35 | 36 | 37 | org.eclipse.cdt.make.core.contents 38 | org.eclipse.cdt.make.core.activeConfigSettings 39 | 40 | 41 | org.eclipse.cdt.make.core.enableAutoBuild 42 | false 43 | 44 | 45 | org.eclipse.cdt.make.core.enableCleanBuild 46 | true 47 | 48 | 49 | org.eclipse.cdt.make.core.enableFullBuild 50 | true 51 | 52 | 53 | org.eclipse.cdt.make.core.fullBuildTarget 54 | all 55 | 56 | 57 | org.eclipse.cdt.make.core.stopOnError 58 | true 59 | 60 | 61 | org.eclipse.cdt.make.core.useDefaultBuildCmd 62 | true 63 | 64 | 65 | 66 | 67 | org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder 68 | full,incremental, 69 | 70 | 71 | 72 | 73 | 74 | org.eclipse.cdt.core.cnature 75 | org.eclipse.cdt.managedbuilder.core.managedBuildNature 76 | org.eclipse.cdt.managedbuilder.core.ScannerConfigNature 77 | 78 | 79 | 80 | board 81 | 2 82 | CHIBIOS/boards/ST_STM32F4_DISCOVERY 83 | 84 | 85 | os 86 | 2 87 | CHIBIOS/os 88 | 89 | 90 | test 91 | 2 92 | CHIBIOS/test 93 | 94 | 95 | 96 | -------------------------------------------------------------------------------- /common/BUILD.bazel: -------------------------------------------------------------------------------- 1 | load("@rules_cc//cc:defs.bzl", "cc_library") 2 | load("//third_party:variables.bzl", "project_inc_paths") 3 | 4 | filegroup( 5 | name = "board_config_hdrs", 6 | srcs = ["board.h"], 7 | visibility = ["//visibility:public"], 8 | ) 9 | 10 | filegroup( 11 | name = "board_config_srcs", 12 | srcs = ["board.c"], 13 | visibility = ["//visibility:public"], 14 | ) 15 | 16 | cc_library( 17 | name = "common", 18 | srcs = glob([ 19 | "src/*.c", 20 | "src/*.cpp", 21 | ]), 22 | hdrs = ( 23 | glob([ 24 | "include/*.h", 25 | "include/*.hpp", 26 | ]) + select({ 27 | "//toolchains/programs:bootloader": ["//bootloader:peripheral_config"], 28 | "//toolchains/programs:firmware": ["//firmware:peripheral_config"], 29 | "//conditions:default": [], 30 | }) 31 | ), 32 | copts = project_inc_paths, 33 | visibility = ["//visibility:public"], 34 | deps = [ 35 | "//third_party:chibios_hal", 36 | ], 37 | alwayslink = True, 38 | ) 39 | -------------------------------------------------------------------------------- /common/CPPLINT.cfg: -------------------------------------------------------------------------------- 1 | set noparent 2 | 3 | # TODO: Add copyright to the top of every file. 4 | filter=-legal/copyright 5 | # Include structure of chibios is weird. Figure this out? 6 | filter=-build/include_subdir 7 | # TODO: Need to pass return arguments as pointers to show explicit modification 8 | # in-line. 9 | filter=-runtime/references 10 | # TODO: Use reinterpret case instead of c-casts. 11 | filter=-readability/casting 12 | 13 | # Clang default formats one space before comments; cpplint wants 2. 14 | filter=-whitespace/comments 15 | # Clang defaults to public/private being not indented; cpplint wants indent. 16 | filter=-whitespace/indent 17 | 18 | # Ignore c files. 19 | extensions=hpp,cpp 20 | 21 | # Simplifies include guard naming. 22 | root=include/ 23 | -------------------------------------------------------------------------------- /common/include/comms_defs.hpp: -------------------------------------------------------------------------------- 1 | #ifndef COMMS_DEFS_HPP_ 2 | #define COMMS_DEFS_HPP_ 3 | 4 | #include 5 | 6 | namespace motor_driver { 7 | namespace comms { 8 | 9 | constexpr uint8_t COMM_VERSION = 0xfe; 10 | 11 | using comm_errors_t = uint16_t; 12 | constexpr comm_errors_t COMM_ERRORS_NONE = 0; 13 | constexpr comm_errors_t COMM_ERRORS_OP_FAILED = 1; 14 | constexpr comm_errors_t COMM_ERRORS_MALFORMED = 2; 15 | constexpr comm_errors_t COMM_ERRORS_INVALID_FC = 4; 16 | constexpr comm_errors_t COMM_ERRORS_INVALID_ARGS = 8; 17 | constexpr comm_errors_t COMM_ERRORS_BUF_LEN_MISMATCH = 16; 18 | 19 | using comm_id_t = uint8_t; 20 | constexpr comm_id_t COMM_ID_BROADCAST = 0; 21 | constexpr comm_id_t COMM_ID_MIN = 1; 22 | constexpr comm_id_t COMM_ID_MAX = UINT8_MAX; 23 | 24 | // Flag Bits (These are toggle bits so each flag changes one bit). 25 | using comm_fg_t = uint8_t; 26 | constexpr comm_fg_t COMM_FG_COMP = 0b00000000; 27 | constexpr comm_fg_t COMM_FG_BOARD = 0b00000001; 28 | constexpr comm_fg_t COMM_FG_RESET = 0b00000010; 29 | constexpr comm_fg_t COMM_FG_TIMEOUT = 0b00000100; 30 | 31 | using comm_fc_t = uint8_t; 32 | constexpr comm_fc_t COMM_FC_NOP = 0x00; 33 | constexpr comm_fc_t COMM_FC_REG_READ = 0x01; 34 | constexpr comm_fc_t COMM_FC_REG_WRITE = 0x02; 35 | constexpr comm_fc_t COMM_FC_REG_READ_WRITE = 0x03; 36 | constexpr comm_fc_t COMM_FC_CLEAR_IWDGRST = 0x10; 37 | constexpr comm_fc_t COMM_FC_SYSTEM_RESET = 0x80; 38 | constexpr comm_fc_t COMM_FC_JUMP_TO_ADDR = 0x81; 39 | constexpr comm_fc_t COMM_FC_FLASH_SECTOR_COUNT = 0x82; 40 | constexpr comm_fc_t COMM_FC_FLASH_SECTOR_START = 0x83; 41 | constexpr comm_fc_t COMM_FC_FLASH_SECTOR_SIZE = 0x84; 42 | constexpr comm_fc_t COMM_FC_FLASH_SECTOR_ERASE = 0x85; 43 | constexpr comm_fc_t COMM_FC_FLASH_PROGRAM = 0x86; 44 | constexpr comm_fc_t COMM_FC_FLASH_READ = 0x87; 45 | constexpr comm_fc_t COMM_FC_FLASH_VERIFY = 0x88; 46 | constexpr comm_fc_t COMM_FC_FLASH_VERIFY_ERASED = 0x89; 47 | constexpr comm_fc_t COMM_FC_CONFIRM_ID = 0xFE; 48 | constexpr comm_fc_t COMM_FC_ENUMERATE = 0xFF; 49 | 50 | using comm_addr_t = uint16_t; 51 | 52 | using comm_reg_count_t = uint8_t; 53 | 54 | } // namespace comms 55 | } // namespace motor_driver 56 | 57 | #endif // COMMS_DEFS_HPP_ 58 | -------------------------------------------------------------------------------- /common/include/crc16.h: -------------------------------------------------------------------------------- 1 | /** 2 | * \file 3 | * Functions and types for CRC checks. 4 | * 5 | * Generated on Fri Jun 8 14:55:37 2018 6 | * by pycrc v0.9.1, https://pycrc.org 7 | * using the configuration: 8 | * - Width = 16 9 | * - Poly = 0x8005 10 | * - XorIn = 0x0000 11 | * - ReflectIn = True 12 | * - XorOut = 0x0000 13 | * - ReflectOut = True 14 | * - Algorithm = table-driven 15 | * 16 | * This file defines the functions crc16_init(), crc16_update() and 17 | * crc16_finalize(). 18 | * 19 | * The crc16_init() function returns the inital \c crc value and must be called 20 | * before the first call to crc16_update(). 21 | * Similarly, the crc16_finalize() function must be called after the last call 22 | * to crc16_update(), before the \c crc is being used. 23 | * is being used. 24 | * 25 | * The crc16_update() function can be called any number of times (including zero 26 | * times) in between the crc16_init() and crc16_finalize() calls. 27 | * 28 | * This pseudo-code shows an example usage of the API: 29 | * \code{.c} 30 | * crc16_t crc; 31 | * unsigned char data[MAX_DATA_LEN]; 32 | * size_t data_len; 33 | * 34 | * crc = crc16_init(); 35 | * while ((data_len = read_data(data, MAX_DATA_LEN)) > 0) { 36 | * crc = crc16_update(crc, data, data_len); 37 | * } 38 | * crc = crc16_finalize(crc); 39 | * \endcode 40 | */ 41 | #ifndef _CRC16_H_ 42 | #define _CRC16_H_ 43 | 44 | #include 45 | #include 46 | 47 | #ifdef __cplusplus 48 | extern "C" { 49 | #endif 50 | 51 | /** 52 | * The definition of the used algorithm. 53 | * 54 | * This is not used anywhere in the generated code, but it may be used by the 55 | * application code to call algorithm-specific code, if desired. 56 | */ 57 | #define CRC_ALGO_TABLE_DRIVEN 1 58 | 59 | /** 60 | * The type of the CRC values. 61 | * 62 | * This type must be big enough to contain at least 16 bits. 63 | */ 64 | typedef uint_fast16_t crc16_t; 65 | 66 | /** 67 | * Calculate the initial crc value. 68 | * 69 | * \return The initial crc value. 70 | */ 71 | static inline crc16_t crc16_init(void) { return 0x0000; } 72 | 73 | /** 74 | * Update the crc value with new data. 75 | * 76 | * \param[in] crc The current crc value. 77 | * \param[in] data Pointer to a buffer of \a data_len bytes. 78 | * \param[in] data_len Number of bytes in the \a data buffer. 79 | * \return The updated crc value. 80 | */ 81 | crc16_t crc16_update(crc16_t crc, const void *data, size_t data_len); 82 | 83 | /** 84 | * Calculate the final crc value. 85 | * 86 | * \param[in] crc The current crc value. 87 | * \return The final crc value. 88 | */ 89 | static inline crc16_t crc16_finalize(crc16_t crc) { return crc; } 90 | 91 | #ifdef __cplusplus 92 | } /* closing brace for extern "C" */ 93 | #endif 94 | 95 | #endif /* _CRC16_H_ */ 96 | -------------------------------------------------------------------------------- /common/include/helper.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef HELPER_H 3 | #define HELPER_H 4 | 5 | #include 6 | 7 | #define BOOTLOADER_SUCCESS 0 8 | #define BOOTLOADER_ERROR_NOCARD 1 9 | #define BOOTLOADER_ERROR_BADFS 2 10 | #define BOOTLOADER_ERROR_NOFILE 3 11 | #define BOOTLOADER_ERROR_READFAILURE 4 12 | #define BOOTLOADER_ERROR_BADHEX 5 13 | #define BOOTLOADER_ERROR_BADFLASH 6 14 | 15 | #ifdef __cplusplus 16 | extern "C" { 17 | #endif 18 | 19 | /** 20 | * @brief Jump to application located in flash. 21 | * @param address Address to jump to. 22 | * 23 | * @author Code stolen from "matis" 24 | * @link http://forum.chibios.org/phpbb/viewtopic.php?f=2&t=338 25 | */ 26 | void flashJumpApplication(uint32_t address); 27 | 28 | struct IWDG_Values { 29 | uint8_t prescaler; 30 | uint16_t reload; 31 | }; 32 | 33 | struct IWDG_Values pauseIWDG(void); 34 | void resumeIWDG(struct IWDG_Values save); 35 | 36 | #ifdef __cplusplus 37 | } 38 | #endif 39 | 40 | #endif /* HELPER_H */ 41 | -------------------------------------------------------------------------------- /common/include/hw_conf.h: -------------------------------------------------------------------------------- 1 | #ifndef _HW_CONF_H_ 2 | #define _HW_CONF_H_ 3 | 4 | #define CURR_A_CHANNEL ADC_CHANNEL_IN12 5 | #define CURR_B_CHANNEL ADC_CHANNEL_IN11 6 | #define CURR_C_CHANNEL ADC_CHANNEL_IN10 7 | 8 | #define VBUS_CHANNEL ADC_CHANNEL_IN13 9 | 10 | #define VSENSE_A_CHANNEL ADC_CHANNEL_IN8 11 | #define VSENSE_B_CHANNEL ADC_CHANNEL_IN15 12 | #define VSENSE_C_CHANNEL ADC_CHANNEL_IN14 13 | 14 | #endif /* _HW_CONF_H_ */ 15 | -------------------------------------------------------------------------------- /common/include/utils.h: -------------------------------------------------------------------------------- 1 | #ifndef _UTILS_H_ 2 | #define _UTILS_H_ 3 | 4 | #include "hal.h" 5 | 6 | #include "ch.h" 7 | 8 | struct IOPin { 9 | ioportid_t port; 10 | int pin; 11 | }; 12 | 13 | #ifdef __cplusplus 14 | extern "C" { 15 | #endif 16 | 17 | void pwmSetChannelOutputMode(PWMDriver *pwm_driver, pwmchannel_t channel, 18 | uint16_t output_mode); 19 | 20 | #ifdef __cplusplus 21 | } 22 | #endif 23 | 24 | #define STM32_TIM_CCMRX_OCXM_ACTIVE_HIGH (6U) 25 | #define STM32_TIM_CCMRX_OCXM_ACTIVE_LOW (7U) 26 | 27 | #define NS2RTT(nsec) \ 28 | (((halGetCounterFrequency() + 999999999UL) / 1000000000UL) * (nsec)) 29 | 30 | #endif /* _UTILS_H_ */ 31 | -------------------------------------------------------------------------------- /common/src/constants.cpp: -------------------------------------------------------------------------------- 1 | #include "constants.hpp" 2 | 3 | namespace motor_driver { 4 | namespace consts { 5 | 6 | const uint16_t led_gamma_table[] = { 7 | 0, 0, 0, 0, 0, 1, 1, 2, 3, 5, 6, 8 | 8, 10, 13, 16, 19, 23, 27, 31, 37, 42, 48, 9 | 55, 62, 70, 79, 88, 98, 108, 119, 131, 144, 157, 10 | 171, 186, 202, 219, 236, 254, 273, 294, 315, 336, 359, 11 | 383, 408, 434, 461, 489, 518, 548, 579, 612, 645, 680, 12 | 716, 753, 791, 831, 871, 913, 957, 1001, 1047, 1094, 1143, 13 | 1193, 1244, 1297, 1351, 1406, 1463, 1522, 1582, 1643, 1706, 1771, 14 | 1837, 1904, 1973, 2044, 2116, 2190, 2266, 2343, 2422, 2503, 2585, 15 | 2669, 2755, 2843, 2932, 3023, 3116, 3211, 3307, 3406, 3506, 3608, 16 | 3712, 3818, 3926, 4036, 4148, 4261, 4377, 4495, 4614, 4736, 4860, 17 | 4986, 5114, 5244, 5376, 5510, 5647, 5785, 5926, 6069, 6214, 6361, 18 | 6511, 6663, 6817, 6973, 7132, 7293, 7456, 7621, 7789, 7960, 8132, 19 | 8307, 8485, 8664, 8847, 9031, 9219, 9408, 9600, 9795, 9992, 10192, 20 | 10394, 10599, 10806, 11016, 11229, 11444, 11662, 11882, 12105, 12331, 12560, 21 | 12791, 13025, 13262, 13501, 13743, 13988, 14236, 14486, 14740, 14996, 15255, 22 | 15517, 15782, 16049, 16320, 16593, 16870, 17149, 17431, 17716, 18005, 18296, 23 | 18590, 18887, 19188, 19491, 19797, 20107, 20420, 20735, 21054, 21376, 21701, 24 | 22029, 22361, 22696, 23033, 23374, 23719, 24066, 24417, 24771, 25128, 25489, 25 | 25853, 26220, 26591, 26965, 27342, 27723, 28107, 28494, 28885, 29280, 29677, 26 | 30079, 30483, 30891, 31303, 31718, 32137, 32559, 32985, 33414, 33847, 34284, 27 | 34724, 35168, 35615, 36066, 36521, 36979, 37441, 37907, 38376, 38850, 39326, 28 | 39807, 40291, 40780, 41272, 41767, 42267, 42770, 43278, 43789, 44304, 44822, 29 | 45345, 45872, 46402, 46937, 47475, 48017, 48564, 49114, 49668, 50227, 50789, 30 | 51355, 51926, 52500, 31 | }; 32 | 33 | const void *calibration_ptr = (reinterpret_cast(0x08008000)); 34 | const uint8_t *board_id_ptr = (reinterpret_cast(0x0800c000)); 35 | const void *firmware_ptr = (reinterpret_cast(0x08010000)); 36 | 37 | } // namespace consts 38 | } // namespace motor_driver 39 | -------------------------------------------------------------------------------- /common/src/crc16.c: -------------------------------------------------------------------------------- 1 | /** 2 | * \file 3 | * Functions and types for CRC checks. 4 | * 5 | * Generated on Fri Jun 8 14:57:00 2018 6 | * by pycrc v0.9.1, https://pycrc.org 7 | * using the configuration: 8 | * - Width = 16 9 | * - Poly = 0x8005 10 | * - XorIn = 0x0000 11 | * - ReflectIn = True 12 | * - XorOut = 0x0000 13 | * - ReflectOut = True 14 | * - Algorithm = table-driven 15 | */ 16 | #include "crc16.h" /* include the header file generated with pycrc */ 17 | #include 18 | #include 19 | 20 | 21 | 22 | /** 23 | * Static table used for the table_driven implementation. 24 | */ 25 | static const crc16_t crc_table[256] = { 26 | 0x0000, 0xc0c1, 0xc181, 0x0140, 0xc301, 0x03c0, 0x0280, 0xc241, 27 | 0xc601, 0x06c0, 0x0780, 0xc741, 0x0500, 0xc5c1, 0xc481, 0x0440, 28 | 0xcc01, 0x0cc0, 0x0d80, 0xcd41, 0x0f00, 0xcfc1, 0xce81, 0x0e40, 29 | 0x0a00, 0xcac1, 0xcb81, 0x0b40, 0xc901, 0x09c0, 0x0880, 0xc841, 30 | 0xd801, 0x18c0, 0x1980, 0xd941, 0x1b00, 0xdbc1, 0xda81, 0x1a40, 31 | 0x1e00, 0xdec1, 0xdf81, 0x1f40, 0xdd01, 0x1dc0, 0x1c80, 0xdc41, 32 | 0x1400, 0xd4c1, 0xd581, 0x1540, 0xd701, 0x17c0, 0x1680, 0xd641, 33 | 0xd201, 0x12c0, 0x1380, 0xd341, 0x1100, 0xd1c1, 0xd081, 0x1040, 34 | 0xf001, 0x30c0, 0x3180, 0xf141, 0x3300, 0xf3c1, 0xf281, 0x3240, 35 | 0x3600, 0xf6c1, 0xf781, 0x3740, 0xf501, 0x35c0, 0x3480, 0xf441, 36 | 0x3c00, 0xfcc1, 0xfd81, 0x3d40, 0xff01, 0x3fc0, 0x3e80, 0xfe41, 37 | 0xfa01, 0x3ac0, 0x3b80, 0xfb41, 0x3900, 0xf9c1, 0xf881, 0x3840, 38 | 0x2800, 0xe8c1, 0xe981, 0x2940, 0xeb01, 0x2bc0, 0x2a80, 0xea41, 39 | 0xee01, 0x2ec0, 0x2f80, 0xef41, 0x2d00, 0xedc1, 0xec81, 0x2c40, 40 | 0xe401, 0x24c0, 0x2580, 0xe541, 0x2700, 0xe7c1, 0xe681, 0x2640, 41 | 0x2200, 0xe2c1, 0xe381, 0x2340, 0xe101, 0x21c0, 0x2080, 0xe041, 42 | 0xa001, 0x60c0, 0x6180, 0xa141, 0x6300, 0xa3c1, 0xa281, 0x6240, 43 | 0x6600, 0xa6c1, 0xa781, 0x6740, 0xa501, 0x65c0, 0x6480, 0xa441, 44 | 0x6c00, 0xacc1, 0xad81, 0x6d40, 0xaf01, 0x6fc0, 0x6e80, 0xae41, 45 | 0xaa01, 0x6ac0, 0x6b80, 0xab41, 0x6900, 0xa9c1, 0xa881, 0x6840, 46 | 0x7800, 0xb8c1, 0xb981, 0x7940, 0xbb01, 0x7bc0, 0x7a80, 0xba41, 47 | 0xbe01, 0x7ec0, 0x7f80, 0xbf41, 0x7d00, 0xbdc1, 0xbc81, 0x7c40, 48 | 0xb401, 0x74c0, 0x7580, 0xb541, 0x7700, 0xb7c1, 0xb681, 0x7640, 49 | 0x7200, 0xb2c1, 0xb381, 0x7340, 0xb101, 0x71c0, 0x7080, 0xb041, 50 | 0x5000, 0x90c1, 0x9181, 0x5140, 0x9301, 0x53c0, 0x5280, 0x9241, 51 | 0x9601, 0x56c0, 0x5780, 0x9741, 0x5500, 0x95c1, 0x9481, 0x5440, 52 | 0x9c01, 0x5cc0, 0x5d80, 0x9d41, 0x5f00, 0x9fc1, 0x9e81, 0x5e40, 53 | 0x5a00, 0x9ac1, 0x9b81, 0x5b40, 0x9901, 0x59c0, 0x5880, 0x9841, 54 | 0x8801, 0x48c0, 0x4980, 0x8941, 0x4b00, 0x8bc1, 0x8a81, 0x4a40, 55 | 0x4e00, 0x8ec1, 0x8f81, 0x4f40, 0x8d01, 0x4dc0, 0x4c80, 0x8c41, 56 | 0x4400, 0x84c1, 0x8581, 0x4540, 0x8701, 0x47c0, 0x4680, 0x8641, 57 | 0x8201, 0x42c0, 0x4380, 0x8341, 0x4100, 0x81c1, 0x8081, 0x4040 58 | }; 59 | 60 | 61 | crc16_t crc16_update(crc16_t crc, const void *data, size_t data_len) 62 | { 63 | const unsigned char *d = (const unsigned char *)data; 64 | unsigned int tbl_idx; 65 | 66 | while (data_len--) { 67 | tbl_idx = (crc ^ *d) & 0xff; 68 | crc = (crc_table[tbl_idx] ^ (crc >> 8)) & 0xffff; 69 | d++; 70 | } 71 | return crc & 0xffff; 72 | } 73 | -------------------------------------------------------------------------------- /common/src/helper.c: -------------------------------------------------------------------------------- 1 | 2 | #include "helper.h" 3 | #include "flash.h" 4 | #include "stm32f4xx_iwdg.h" 5 | #include 6 | #include 7 | 8 | void flashJumpApplication(uint32_t address) { 9 | typedef void (*funcPtr)(void); 10 | 11 | u32 jumpAddr = *(vu32 *)(address + 0x04); /* reset ptr in vector table */ 12 | funcPtr usrMain = (funcPtr)jumpAddr; 13 | 14 | /* Reset all interrupts to default */ 15 | chSysDisable(); 16 | 17 | /* Clear pending interrupts just to be on the save side */ 18 | SCB_ICSR = ICSR_PENDSVCLR; 19 | 20 | /* Disable all interrupts */ 21 | int i; 22 | for (i = 0; i < 8; ++i) 23 | NVIC->ICER[i] = NVIC->IABR[i]; 24 | 25 | /* Set stack pointer as in application's vector table */ 26 | __set_MSP(*(vu32 *)address); 27 | usrMain(); 28 | } 29 | 30 | struct IWDG_Values pauseIWDG() { 31 | // Save current configs. 32 | struct IWDG_Values save; 33 | save.prescaler = IWDG->PR; 34 | save.reload = IWDG->RLR; 35 | 36 | // Set config to very large wait time. 37 | IWDG_WriteAccessCmd(IWDG_WriteAccess_Enable); 38 | IWDG_SetPrescaler(IWDG_Prescaler_256); 39 | IWDG_SetReload(5000); 40 | IWDG_WriteAccessCmd(IWDG_WriteAccess_Disable); 41 | IWDG_ReloadCounter(); 42 | 43 | // Return the save to be re-loaded later. 44 | return save; 45 | } 46 | 47 | void resumeIWDG(struct IWDG_Values save) { 48 | IWDG_WriteAccessCmd(IWDG_WriteAccess_Enable); 49 | IWDG_SetPrescaler(save.prescaler); 50 | IWDG_SetReload(save.reload); 51 | IWDG_WriteAccessCmd(IWDG_WriteAccess_Disable); 52 | IWDG_ReloadCounter(); 53 | } 54 | -------------------------------------------------------------------------------- /common/src/utils.c: -------------------------------------------------------------------------------- 1 | #include "utils.h" 2 | 3 | void pwmSetChannelOutputMode(PWMDriver *pwm_driver, pwmchannel_t channel, 4 | uint16_t output_mode) { 5 | switch (channel) { 6 | case 0: 7 | pwm_driver->tim->CCMR1 = ((pwm_driver->tim->CCMR1 & ~TIM_CCMR1_OC1M) | 8 | STM32_TIM_CCMR1_OC1M(output_mode)); 9 | break; 10 | case 1: 11 | pwm_driver->tim->CCMR1 = ((pwm_driver->tim->CCMR1 & ~TIM_CCMR1_OC2M) | 12 | STM32_TIM_CCMR1_OC2M(output_mode)); 13 | break; 14 | case 2: 15 | pwm_driver->tim->CCMR2 = ((pwm_driver->tim->CCMR2 & ~TIM_CCMR2_OC3M) | 16 | STM32_TIM_CCMR2_OC3M(output_mode)); 17 | break; 18 | case 3: 19 | pwm_driver->tim->CCMR2 = ((pwm_driver->tim->CCMR2 & ~TIM_CCMR2_OC4M) | 20 | STM32_TIM_CCMR2_OC4M(output_mode)); 21 | break; 22 | default: 23 | break; 24 | } 25 | } 26 | 27 | // FIXME 28 | // NOTE: Commenting out this as I have no idea why it's actually in here. It 29 | // caused compiler issues when I started working on bazel firmware builds and 30 | // doesn't seem to cause any issues with the makefile builds. 31 | // void *__dso_handle = 0; 32 | void __cxa_pure_virtual(void) { 33 | while (1) 34 | ; 35 | } 36 | -------------------------------------------------------------------------------- /docs/disco_bus.md: -------------------------------------------------------------------------------- 1 | # Disco Bus Protocol 2 | This boot protocol is meant to enumerate a series of boards with ID numbers instead of requiring each board to have a unique identifier. 3 | 4 | The protocol is as follows: 5 | 1) While in bootloader, the board listens on the broadcast channel (0) for any packet with an ID that's not the broadcast ID with the enumerate function code 6 | 2) When one such packet is found, the board checks if its DISCO_BUS_IN is low; if so, this is now the board's ID 7 | 3) If this succeeds, the board will reply with its new board ID; if no response, try again (or check disco connection) 8 | 4) When successful communication is established, lock ID into the current board 9 | 5) The board that was just locked will now set their DISCO_BUS_OUT low, queueing the next board in series and unlocking the rest of the bootloader 10 | 11 | For the first board to work, the following is implemented: 12 | * Input pins are pulled low by external resistors 13 | * The hardware resistors decouple boards from one another (so their different grounds won't EMP hardware) 14 | * All outputs defaul drive low (will overpower the pull-ups ~30k [[table 48 in section 5.3.16](https://www.st.com/resource/en/datasheet/dm00037051.pdf)]) 15 | -------------------------------------------------------------------------------- /docs/protocol_v1.txt: -------------------------------------------------------------------------------- 1 | BLDC Servo Controller binary protocol "specification" version 1 2 | 3 | 4 | 5 | 6 | Request Message Format 7 | ----------------------------------------------------------------- 8 | | Message | 9 | +----------------+----------+---------------+------------+----------------+--------+-----+ 10 | Field Name | Message Length | Board ID | Function Code | Start Addr | Register Count | Values | CRC | 11 | +----------------+----------+---------------+------------+----------------+--------+-----+ 12 | Size (bytes) | 1 | 1 | 1 | 2 | 1 | n | 2 | 13 | +----------------+----------+---------------+------------+----------------+--------+-----+ 14 | where n is arbitrary. 15 | 16 | 17 | 18 | 19 | Response Message Format 20 | ------------------------------------------------ 21 | | Message | 22 | +----------------+----------+---------------+------------+--------+-------+ 23 | Field Name | Message Length | Board ID | Function Code | Errors | Values | CRC | 24 | +----------------+----------+---------------+------------+--------+-------+ 25 | Size (bytes) | 1 | 1 | 1 | 2 | n | 2 | 26 | +----------------+----------+---------------+------------+--------+-------+ 27 | where n is arbitrary. 28 | 29 | 30 | 31 | 32 | Example: Read from 16-bit register 0x0005 of board 6 33 | 34 | Request: 0x05, 0x06, 0x01, 0x05, 0x00, 0x01, 0xff, 0xff 35 | +----------------+----------+---------------+------------+----------------+--------+-------+ 36 | | Message Length | Board ID | Function Code | Start Addr | Register Count | Values | CRC | 37 | +----------------+----------+---------------+------------+----------------+--------+-------+ 38 | | 05 | 06 | 01 | 0500 | 01 | | ffff | 39 | +----------------+----------+---------------+------------+----------------+--------+-------+ 40 | 41 | Response: 0x06, 0x06, 0x01, 0x00, 0x00, 0xcd, 0xab, 0xff, 0xff 42 | +----------------+----------+---------------+------------+--------+-------+ 43 | | Message Length | Board ID | Function Code | Errors | Values | CRC | 44 | +----------------+----------+---------------+------------+--------+-------+ 45 | | 06 | 06 | 01 | 0000 | cdab | ffff | 46 | +----------------+----------+---------------+------------+--------+-------+ 47 | where 0xabcd is the 16-bit register value and ffff is the CRC checksum. 48 | 49 | 50 | 51 | 52 | Example: Write value 0x6789abcd to 32-bit register 0x1234 of board 2 53 | 54 | Request: 0x09, 0x02, 0x02, 0x34, 0x12, 0x01, 0xcd, 0xab, 0x89, 0x67, 0xff, 0xff 55 | +----------------+----------+---------------+------------+----------------+----------+-------+ 56 | | Message Length | Board ID | Function Code | Start Addr | Register Count | Values | CRC | 57 | +----------------+----------+---------------+------------+----------------+----------+-------+ 58 | | 09 | 02 | 02 | 3412 | 01 | cdab8967 | ffff | 59 | +----------------+----------+---------------+------------+----------------+----------+-------+ 60 | 61 | Response: 0x04, 0x02, 0x02, 0x00, 0x00, 0xff, 0xff 62 | +----------------+----------+---------------+------------+--------+-------+ 63 | | Message Length | Board ID | Function Code | Errors | Values | CRC | 64 | +----------------+----------+---------------+------------+--------+-------+ 65 | | 04 | 02 | 02 | 0000 | | ffff | 66 | +----------------+----------+---------------+------------+--------+-------+ 67 | 68 | 69 | 70 | 71 | Notes 72 | 73 | All values are little-endian (least significant byte first). 74 | Function code 0x01 is read, 0x02 is write. 75 | You can read or write multiple consecutive registers in a single request. Their values are concatenated together. 76 | The CRC checksum is not defined yet. You can provide a fake CRC in the request and ignore the CRC in the response. 77 | Registers can be any size. 78 | Errors == 0x0000 means a request was successful. 79 | -------------------------------------------------------------------------------- /drivers/BUILD.bazel: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BetzDrive/bldc-controller/f552a7fd2b8c955074b5e10b9afce30ba84b104f/drivers/BUILD.bazel -------------------------------------------------------------------------------- /drivers/CPPLINT.cfg: -------------------------------------------------------------------------------- 1 | set noparent 2 | 3 | # TODO: Add copyright to the top of every file. 4 | filter=-legal/copyright 5 | # Include structure of chibios is weird. Figure this out? 6 | filter=-build/include_subdir 7 | 8 | # TODO: Need to pass return arguments as pointers to show explicit modification 9 | # in-line. 10 | filter=-runtime/references 11 | # TODO: Constexprs should be renamed to kConstant format. 12 | filter=-runtime/arrays 13 | 14 | # Clang default formats one space before comments; cpplint wants 2. 15 | filter=-whitespace/comments 16 | # Clang defaults to public/private being not indented; cpplint wants indent. 17 | filter=-whitespace/indent 18 | 19 | # Ignore c files. 20 | extensions=hpp,cpp 21 | 22 | # Simplifies include guard naming. 23 | root=include/ 24 | -------------------------------------------------------------------------------- /drivers/accel/BUILD.bazel: -------------------------------------------------------------------------------- 1 | load("@rules_cc//cc:defs.bzl", "cc_library") 2 | load("//third_party:variables.bzl", "project_inc_paths") 3 | 4 | cc_library( 5 | name = "iis328dq", 6 | srcs = ["iis328dq.cpp"], 7 | hdrs = ["iis328dq.hpp"], 8 | copts = project_inc_paths, 9 | visibility = ["//visibility:public"], 10 | deps = [ 11 | "//common", 12 | ], 13 | alwayslink = True, 14 | ) 15 | -------------------------------------------------------------------------------- /drivers/accel/iis328dq.cpp: -------------------------------------------------------------------------------- 1 | // Datasheet: https://www.st.com/resource/en/datasheet/iis328dq.pdf 2 | 3 | #include "drivers/accel/iis328dq.hpp" 4 | 5 | namespace motor_driver { 6 | namespace peripherals { 7 | 8 | void IIS328DQ::start() { 9 | uint8_t config[2]; 10 | systime_t tmo = MS2ST(4); // 4 millisecond timeout 11 | 12 | config[0] = 0x20; 13 | config[1] = 0x27; 14 | 15 | // i2cStart should only be called once; for now called on temperature 16 | // sensor init. 17 | // i2cStart(i2c_driver_, &i2c_config_); 18 | 19 | i2cAcquireBus(i2c_driver_); 20 | 21 | i2cMasterTransmitTimeout(i2c_driver_, 22 | IIS328DQ_DEFAULT_ADDRESS, // Address 23 | config, 2, // TX Buffer, Len 24 | NULL, 0, // RX Buffer, Len 25 | tmo); // Timeout 26 | 27 | /* Select FS from Table 3 (CTRL REG4 section 7.5) */ 28 | config[0] = IIS328DQ_CTRL_REG4; 29 | /* Configure sensitivity for +-4g */ 30 | config[1] = 0b00010000; 31 | i2cMasterTransmitTimeout(i2c_driver_, IIS328DQ_DEFAULT_ADDRESS, config, 2, 32 | NULL, 0, tmo); 33 | 34 | i2cReleaseBus(i2c_driver_); 35 | } 36 | 37 | bool IIS328DQ::checkID() { 38 | uint8_t id = 0; 39 | 40 | bool success = IIS328DQ::receive(IIS328DQ_WHO_AM_I_ADDRESS, &id, 1); 41 | 42 | return (success && (id == IIS328DQ_WHO_AM_I)); 43 | } 44 | 45 | bool IIS328DQ::receive(uint8_t reg, uint8_t *data, size_t size) { 46 | systime_t tmo = MS2ST(4); // 4 millisecond timeout 47 | i2cAcquireBus(i2c_driver_); 48 | 49 | msg_t status = i2cMasterTransmitTimeout(i2c_driver_, IIS328DQ_DEFAULT_ADDRESS, 50 | ®, 1, data, size, tmo); 51 | 52 | i2cReleaseBus(i2c_driver_); 53 | return status == RDY_OK; 54 | } 55 | 56 | /* Expects accel_arr to be of size 3 and stores x,y,z in that order */ 57 | bool IIS328DQ::getAccel(int16_t *accel_arr) { 58 | // Set the register address msb (SUB) to autoincrement register address 59 | return IIS328DQ::receive(IIS328DQ_OUT_X_L | IIS328DQ_MASK_SUB, 60 | reinterpret_cast(accel_arr), 6); 61 | } 62 | 63 | } // namespace peripherals 64 | } // namespace motor_driver 65 | -------------------------------------------------------------------------------- /drivers/accel/iis328dq.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "hal.h" 4 | 5 | namespace motor_driver { 6 | namespace peripherals { 7 | 8 | constexpr uint8_t IIS328DQ_DEFAULT_ADDRESS = 0b0011000; 9 | 10 | constexpr uint8_t IIS328DQ_WHO_AM_I_ADDRESS = 0x0F; 11 | constexpr uint8_t IIS328DQ_WHO_AM_I = 0b00110010; 12 | 13 | constexpr uint8_t IIS328DQ_CTRL_REG4 = 0x23; 14 | 15 | constexpr uint8_t IIS328DQ_OUT_X_L = 0x28; 16 | constexpr uint8_t IIS328DQ_OUT_X_H = 0x29; 17 | 18 | constexpr uint8_t IIS328DQ_OUT_Y_L = 0x2A; 19 | constexpr uint8_t IIS328DQ_OUT_Y_H = 0x2B; 20 | 21 | constexpr uint8_t IIS328DQ_OUT_Z_L = 0x2C; 22 | constexpr uint8_t IIS328DQ_OUT_Z_H = 0x2D; 23 | 24 | constexpr uint8_t IIS328DQ_MASK_SUB = 0x80; 25 | 26 | class IIS328DQ { 27 | public: 28 | explicit IIS328DQ(I2CDriver &i2c_driver) : i2c_driver_(&i2c_driver) { 29 | i2c_config_.op_mode = OPMODE_I2C; 30 | i2c_config_.clock_speed = 400000; 31 | i2c_config_.duty_cycle = FAST_DUTY_CYCLE_2; 32 | } 33 | void start(); 34 | bool receive(uint8_t reg, uint8_t *data, size_t size); 35 | bool getAccel(int16_t *accel_arr); 36 | bool checkID(); 37 | 38 | private: 39 | I2CDriver *const i2c_driver_; 40 | I2CConfig i2c_config_; 41 | }; 42 | 43 | } // namespace peripherals 44 | } // namespace motor_driver 45 | -------------------------------------------------------------------------------- /drivers/encoder/BUILD.bazel: -------------------------------------------------------------------------------- 1 | load("@rules_cc//cc:defs.bzl", "cc_library") 2 | load("//third_party:variables.bzl", "project_inc_paths") 3 | 4 | cc_library( 5 | name = "as5047d", 6 | srcs = ["as5047d.cpp"], 7 | hdrs = ["as5047d.hpp"], 8 | copts = project_inc_paths, 9 | visibility = ["//visibility:public"], 10 | deps = [ 11 | "//common", 12 | ], 13 | alwayslink = True, 14 | ) 15 | -------------------------------------------------------------------------------- /drivers/encoder/aeat6600.cpp: -------------------------------------------------------------------------------- 1 | #include "drivers/encoder/aeat6600.h" 2 | 3 | static uint16_t getResultFromRxbuf(uint8_t *rxbuf) { 4 | return ((uint16_t)(rxbuf[0] & 0x3f) << 8) | (uint16_t)rxbuf[1]; 5 | } 6 | 7 | namespace motor_driver { 8 | 9 | void AEAT6600::start() { spiStart(spi_driver_, &spi_config_); } 10 | 11 | uint16_t AEAT6600::getAngle() { 12 | uint8_t rxbuf[2]; 13 | 14 | spiSelect(spi_driver_); 15 | halPolledDelay(NS2RTT(500)); 16 | spiReceive(spi_driver_, 2, rxbuf); 17 | spiUnselect(spi_driver_); 18 | 19 | return getResultFromRxbuf(rxbuf); 20 | } 21 | 22 | void AEAT6600::startPipelinedAngleReadI() { 23 | spiSelectI(spi_driver_); 24 | spiStartReceiveI(spi_driver_, 2, pipeline_rxbuf_); 25 | } 26 | 27 | uint16_t AEAT6600::getPipelinedResultI() { 28 | return getResultFromRxbuf(pipeline_rxbuf_); 29 | } 30 | 31 | void AEAT6600::spiEndCallbackStatic(SPIDriver *spi_driver) { 32 | AEAT6600SPIConfig *spi_config = 33 | reinterpret_cast(spi_driver->config); 34 | spi_config->aeat6600->spiEndCallback(spi_driver); 35 | } 36 | 37 | void AEAT6600::spiEndCallback(SPIDriver *spi_driver) { 38 | (void)spi_driver; 39 | 40 | chSysLockFromIsr(); 41 | spiUnselectI(spi_driver_); 42 | chSysUnlockFromIsr(); 43 | } 44 | 45 | } // namespace motor_driver 46 | -------------------------------------------------------------------------------- /drivers/encoder/aeat6600.h: -------------------------------------------------------------------------------- 1 | #ifndef _AEAT6600_H_ 2 | #define _AEAT6600_H_ 3 | 4 | #include "hal.h" 5 | #include "utils.h" 6 | 7 | namespace motor_driver { 8 | 9 | class AEAT6600; 10 | 11 | struct AEAT6600SPIConfig : SPIConfig { 12 | AEAT6600 *aeat6600; 13 | }; 14 | 15 | class AEAT6600 { 16 | public: 17 | AEAT6600(SPIDriver &spi_driver, IOPin csn) 18 | : spi_driver_(&spi_driver), csn_(csn) { 19 | spi_config_.end_cb = spiEndCallbackStatic; 20 | spi_config_.ssport = csn.port; 21 | spi_config_.sspad = csn.pin; 22 | spi_config_.cr1 = SPI_CR1_BR_1 | // SPI_CR1_BR_1 | //SPI_CR1_BR_0 | 23 | SPI_CR1_MSTR | SPI_CR1_CPOL; 24 | spi_config_.aeat6600 = this; 25 | } 26 | void start(); 27 | uint16_t getAngle(); 28 | void startPipelinedAngleReadI(); 29 | uint16_t getPipelinedResultI(); 30 | 31 | private: 32 | SPIDriver *const spi_driver_; 33 | AEAT6600SPIConfig spi_config_; 34 | const IOPin csn_; 35 | uint8_t pipeline_rxbuf_[2]; 36 | 37 | static void spiEndCallbackStatic(SPIDriver *spi_driver); 38 | void spiEndCallback(SPIDriver *spi_driver); 39 | }; 40 | 41 | } // namespace motor_driver 42 | 43 | #endif /* _AEAT6600_H_ */ 44 | -------------------------------------------------------------------------------- /drivers/encoder/as5047d.cpp: -------------------------------------------------------------------------------- 1 | #include "drivers/encoder/as5047d.hpp" 2 | 3 | static int hasEvenParity(uint8_t *buf, size_t len) { 4 | uint8_t acc = 0; 5 | 6 | for (size_t i = 0; i < len; i++) { 7 | acc ^= buf[i]; 8 | } 9 | 10 | acc ^= acc >> 4; 11 | acc ^= acc >> 2; 12 | acc ^= acc >> 1; 13 | return acc & 1; 14 | } 15 | 16 | static void prepareTxbufForRead(uint8_t *txbuf, uint16_t addr) { 17 | txbuf[0] = (uint8_t)(0x40 | ((addr >> 8) & 0x3f)); 18 | txbuf[1] = (uint8_t)(addr & 0xff); 19 | txbuf[0] |= (uint8_t)hasEvenParity(txbuf, 2) << 7; 20 | } 21 | 22 | static uint16_t getResultFromRxbuf(uint8_t *rxbuf) { 23 | return ((uint16_t)(rxbuf[0] & 0x3f) << 8) | (uint16_t)rxbuf[1]; 24 | } 25 | 26 | namespace motor_driver { 27 | namespace peripherals { 28 | 29 | void AS5047D::start() { spiStart(spi_driver_, &spi_config_); } 30 | 31 | uint16_t AS5047D::readRegister(uint16_t addr) { 32 | uint8_t txbuf[2]; 33 | uint8_t rxbuf[2]; 34 | 35 | prepareTxbufForRead(txbuf, addr); 36 | 37 | spiSelect(spi_driver_); 38 | spiSend(spi_driver_, 2, txbuf); 39 | spiUnselect(spi_driver_); 40 | 41 | halPolledDelay(NS2RTT(350)); 42 | 43 | // Do a dummy read 44 | txbuf[0] = 0xc0; 45 | txbuf[1] = 0x00; 46 | spiSelect(spi_driver_); 47 | spiExchange(spi_driver_, 2, txbuf, rxbuf); 48 | spiUnselect(spi_driver_); 49 | 50 | return getResultFromRxbuf(rxbuf); 51 | } 52 | 53 | uint16_t AS5047D::getAngle() { return readRegister(0x3fff); } 54 | 55 | uint16_t AS5047D::getDiagnostics() { return readRegister(0x3ffc); } 56 | 57 | void AS5047D::startPipelinedRegisterReadI(uint16_t addr) { 58 | prepareTxbufForRead(pipeline_txbuf_, addr); 59 | 60 | spiSelectI(spi_driver_); 61 | spiStartExchangeI(spi_driver_, 2, pipeline_txbuf_, pipeline_rxbuf_); 62 | } 63 | 64 | uint16_t AS5047D::getPipelinedRegisterReadResultI() { 65 | return getResultFromRxbuf(pipeline_rxbuf_); 66 | } 67 | 68 | void AS5047D::spiEndCallbackStatic(SPIDriver *spi_driver) { 69 | // TODO(gbalke): This is a hack around not being able to use `this` 70 | AS5047DSPIConfig *spi_config = const_cast( 71 | reinterpret_cast(spi_driver->config)); 72 | spi_config->as5047d->spiEndCallback(spi_driver); 73 | } 74 | 75 | void AS5047D::spiEndCallback(SPIDriver *spi_driver) { 76 | (void)spi_driver; 77 | 78 | chSysLockFromIsr(); 79 | spiUnselectI(spi_driver_); 80 | chSysUnlockFromIsr(); 81 | } 82 | 83 | } // namespace peripherals 84 | } // namespace motor_driver 85 | -------------------------------------------------------------------------------- /drivers/encoder/as5047d.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "hal.h" 4 | 5 | #include "utils.h" 6 | 7 | namespace motor_driver { 8 | namespace peripherals { 9 | 10 | class AS5047D; 11 | 12 | struct AS5047DSPIConfig : SPIConfig { 13 | AS5047D *as5047d; 14 | }; 15 | 16 | class AS5047D { 17 | public: 18 | AS5047D(SPIDriver &spi_driver, IOPin csn) 19 | : spi_driver_(&spi_driver), csn_(csn) { 20 | spi_config_.end_cb = spiEndCallbackStatic; 21 | spi_config_.ssport = csn.port; 22 | spi_config_.sspad = csn.pin; 23 | // f_PCLK/8. 24 | spi_config_.cr1 = SPI_CR1_BR_1 | SPI_CR1_MSTR | SPI_CR1_CPHA; 25 | spi_config_.as5047d = this; 26 | } 27 | void start(); 28 | uint16_t readRegister(uint16_t addr); 29 | // void writeRegister(uint16_t addr, uint16_t value); 30 | uint16_t getAngle(); 31 | uint16_t getDiagnostics(); 32 | void startPipelinedRegisterReadI(uint16_t addr); 33 | uint16_t getPipelinedRegisterReadResultI(); 34 | 35 | private: 36 | SPIDriver *const spi_driver_; 37 | AS5047DSPIConfig spi_config_; 38 | const IOPin csn_; 39 | uint8_t pipeline_txbuf_[2]; 40 | uint8_t pipeline_rxbuf_[2]; 41 | 42 | static void spiEndCallbackStatic(SPIDriver *spi_driver); 43 | void spiEndCallback(SPIDriver *spi_driver); 44 | }; 45 | 46 | } // namespace peripherals 47 | } // namespace motor_driver 48 | -------------------------------------------------------------------------------- /drivers/encoder/mlx90363.cpp: -------------------------------------------------------------------------------- 1 | #include "drivers/encoder/mlx90363.h" 2 | 3 | #include 4 | 5 | #include "crc_mlx.h" 6 | 7 | namespace motor_driver { 8 | 9 | void MLX90363::start() { 10 | spi_config_.end_cb = NULL; 11 | spiStart(spi_driver_, &spi_config_); 12 | } 13 | 14 | void MLX90363::startAsync() { 15 | spi_config_.end_cb = spiEndCallbackStatic; 16 | spiStart(spi_driver_, &spi_config_); 17 | } 18 | 19 | void MLX90363::sendMessage(const uint8_t *txbuf) { 20 | spiSelect(spi_driver_); 21 | spiSend(spi_driver_, 8, txbuf); 22 | spiUnselect(spi_driver_); 23 | } 24 | 25 | void MLX90363::receiveMessage(uint8_t *rxbuf) { 26 | // Need to send a dummy command 27 | uint8_t txbuf[8]; 28 | createNopMessage(txbuf, 0); 29 | 30 | spiSelect(spi_driver_); 31 | spiExchange(spi_driver_, 8, txbuf, rxbuf); 32 | spiUnselect(spi_driver_); 33 | } 34 | 35 | void MLX90363::exchangeMessage(const uint8_t *txbuf, uint8_t *rxbuf) { 36 | spiSelect(spi_driver_); 37 | spiExchange(spi_driver_, 8, txbuf, rxbuf); 38 | spiUnselect(spi_driver_); 39 | } 40 | 41 | void MLX90363::startAsyncExchangeMessageI(const uint8_t *txbuf) { 42 | std::copy(txbuf, txbuf + 8, async_txbuf_); 43 | 44 | spiSelectI(spi_driver_); 45 | spiStartExchangeI(spi_driver_, 8, async_txbuf_, async_rxbuf_); 46 | } 47 | 48 | void MLX90363::getAsyncExchangeMessageResultI(uint8_t *rxbuf) { 49 | std::copy(async_rxbuf_, async_rxbuf_ + 8, rxbuf); 50 | } 51 | 52 | void MLX90363::createNopMessage(uint8_t *txbuf, uint16_t key) { 53 | txbuf[0] = 0; 54 | txbuf[1] = 0; 55 | txbuf[2] = key & 0xff; 56 | txbuf[3] = (key >> 8) & 0xff; 57 | txbuf[4] = 0; 58 | txbuf[5] = 0; 59 | txbuf[6] = (3 << 6) | MLX90363_OPCODE_NOP; 60 | txbuf[7] = computeMessageCRC(txbuf); 61 | } 62 | 63 | void MLX90363::createGet1AlphaMessage(uint8_t *txbuf, uint16_t timeout) { 64 | txbuf[0] = 0; 65 | txbuf[1] = 0; 66 | txbuf[2] = timeout & 0xff; 67 | txbuf[3] = (timeout >> 8) & 0xff; 68 | txbuf[4] = 0; 69 | txbuf[5] = 0; 70 | txbuf[6] = (0 << 6) | MLX90363_OPCODE_GET1; 71 | txbuf[7] = computeMessageCRC(txbuf); 72 | } 73 | 74 | void MLX90363::createDiagnosticDetailsMessage(uint8_t *txbuf) { 75 | txbuf[0] = 0; 76 | txbuf[1] = 0; 77 | txbuf[2] = 0; 78 | txbuf[3] = 0; 79 | txbuf[4] = 0; 80 | txbuf[5] = 0; 81 | txbuf[6] = (3 << 6) | MLX90363_OPCODE_DIAGDETAILS; 82 | txbuf[7] = computeMessageCRC(txbuf); 83 | } 84 | 85 | mlx90363_status_t MLX90363::parseEchoMessage(const uint8_t *rxbuf, 86 | uint16_t *key_echo) { 87 | mlx90363_status_t status = MLX90363_STATUS_OK; 88 | 89 | if (rxbuf[7] != computeMessageCRC(rxbuf)) { 90 | status |= MLX90363_STATUS_WRONG_CRC; 91 | return status; 92 | } 93 | 94 | if (rxbuf[6] != ((3 << 6) | MLX90363_OPCODE_ECHO)) { 95 | status |= MLX90363_STATUS_WRONG_OPCODE; 96 | return status; 97 | } 98 | 99 | if (key_echo != nullptr) { 100 | *key_echo = ((uint16_t)rxbuf[3] << 8) | (uint16_t)rxbuf[2]; 101 | } 102 | 103 | return status; 104 | } 105 | 106 | mlx90363_status_t MLX90363::parseReadyMessage(const uint8_t *rxbuf, 107 | uint8_t *fw_version, 108 | uint8_t *hw_version) { 109 | mlx90363_status_t status = MLX90363_STATUS_OK; 110 | 111 | if (rxbuf[7] != computeMessageCRC(rxbuf)) { 112 | status |= MLX90363_STATUS_WRONG_CRC; 113 | return status; 114 | } 115 | 116 | if (rxbuf[6] != ((3 << 6) | MLX90363_OPCODE_READY)) { 117 | status |= MLX90363_STATUS_WRONG_OPCODE; 118 | return status; 119 | } 120 | 121 | if (fw_version != nullptr) { 122 | *fw_version = rxbuf[1]; 123 | } 124 | 125 | if (hw_version != nullptr) { 126 | *hw_version = rxbuf[0]; 127 | } 128 | 129 | return status; 130 | } 131 | 132 | mlx90363_status_t MLX90363::parseAlphaMessage(const uint8_t *rxbuf, 133 | uint16_t *alpha, uint8_t *vg) { 134 | mlx90363_status_t status = MLX90363_STATUS_OK; 135 | 136 | if (rxbuf[7] != computeMessageCRC(rxbuf)) { 137 | status |= MLX90363_STATUS_WRONG_CRC; 138 | return status; 139 | } 140 | 141 | if (alpha != nullptr) { 142 | *alpha = ((uint16_t)(rxbuf[1] & 0x3f) << 8) | (uint16_t)rxbuf[0]; 143 | } 144 | 145 | if (vg != nullptr) { 146 | *vg = rxbuf[4]; 147 | } 148 | 149 | return status; 150 | } 151 | 152 | mlx90363_status_t MLX90363::parseDiagnosticsAnswerMessage(const uint8_t *rxbuf, 153 | uint32_t *diag_bits, 154 | uint8_t *fsmerc, 155 | uint8_t *anadiagcnt) { 156 | mlx90363_status_t status = MLX90363_STATUS_OK; 157 | 158 | if (rxbuf[7] != computeMessageCRC(rxbuf)) { 159 | status |= MLX90363_STATUS_WRONG_CRC; 160 | return status; 161 | } 162 | 163 | if (rxbuf[6] != ((3 << 6) | MLX90363_OPCODE_DIAGANSWER)) { 164 | status |= MLX90363_STATUS_WRONG_OPCODE; 165 | return status; 166 | } 167 | 168 | if (diag_bits != nullptr) { 169 | *diag_bits = ((uint32_t)rxbuf[2] << 16) | ((uint32_t)rxbuf[1] << 8) | 170 | (uint32_t)rxbuf[0]; 171 | } 172 | 173 | if (fsmerc != nullptr) { 174 | *fsmerc = rxbuf[3] >> 6; 175 | } 176 | 177 | if (anadiagcnt != nullptr) { 178 | *anadiagcnt = rxbuf[3] & 0x3f; 179 | } 180 | 181 | return status; 182 | } 183 | 184 | uint8_t MLX90363::computeMessageCRC(const uint8_t *buf) { 185 | uint8_t crc = crc_mlx_init(); 186 | crc = crc_mlx_update(crc, buf, 7); 187 | return crc_mlx_finalize(crc); 188 | } 189 | 190 | void MLX90363::spiEndCallbackStatic(SPIDriver *spi_driver) { 191 | MLX90363SPIConfig *spi_config = 192 | reinterpret_cast(spi_driver->config); 193 | spi_config->mlx90363->spiEndCallback(spi_driver); 194 | } 195 | 196 | void MLX90363::spiEndCallback(SPIDriver *spi_driver) { 197 | (void)spi_driver; 198 | 199 | chSysLockFromIsr(); 200 | spiUnselectI(spi_driver_); 201 | chSysUnlockFromIsr(); 202 | } 203 | 204 | } // namespace motor_driver 205 | -------------------------------------------------------------------------------- /drivers/encoder/mlx90363.h: -------------------------------------------------------------------------------- 1 | #ifndef _MLX90363_H_ 2 | #define _MLX90363_H_ 3 | 4 | #include "hal.h" 5 | #include "utils.h" 6 | 7 | namespace motor_driver { 8 | 9 | class MLX90363; 10 | 11 | struct MLX90363SPIConfig : SPIConfig { 12 | MLX90363 *mlx90363; 13 | }; 14 | 15 | using mlx90363_status_t = int; 16 | constexpr mlx90363_status_t MLX90363_STATUS_OK = 0; 17 | constexpr mlx90363_status_t MLX90363_STATUS_WRONG_CRC = 1; 18 | constexpr mlx90363_status_t MLX90363_STATUS_WRONG_OPCODE = 2; 19 | 20 | using mlx90363_opcode_t = uint8_t; 21 | constexpr mlx90363_opcode_t MLX90363_OPCODE_NOP = 16; 22 | constexpr mlx90363_opcode_t MLX90363_OPCODE_ECHO = 17; 23 | constexpr mlx90363_opcode_t MLX90363_OPCODE_DIAGDETAILS = 22; 24 | constexpr mlx90363_opcode_t MLX90363_OPCODE_DIAGANSWER = 23; 25 | constexpr mlx90363_opcode_t MLX90363_OPCODE_GET1 = 19; 26 | constexpr mlx90363_opcode_t MLX90363_OPCODE_READY = 44; 27 | 28 | class MLX90363 { 29 | public: 30 | MLX90363(SPIDriver &spi_driver, IOPin csn) 31 | : spi_driver_(&spi_driver), csn_(csn) { 32 | spi_config_.ssport = csn.port; 33 | spi_config_.sspad = csn.pin; 34 | spi_config_.cr1 = SPI_CR1_BR_2 | SPI_CR1_MSTR | SPI_CR1_CPHA; // f_PCLK/32 35 | spi_config_.mlx90363 = this; 36 | } 37 | void start(); 38 | void startAsync(); 39 | void sendMessage(const uint8_t *txbuf); 40 | void receiveMessage(uint8_t *rxbuf); 41 | void exchangeMessage(const uint8_t *txbuf, uint8_t *rxbuf); 42 | void startAsyncExchangeMessageI(const uint8_t *txbuf); 43 | void getAsyncExchangeMessageResultI(uint8_t *rxbuf); 44 | void createNopMessage(uint8_t *txbuf, uint16_t key); 45 | void createGet1AlphaMessage(uint8_t *txbuf, uint16_t timeout); 46 | void createDiagnosticDetailsMessage(uint8_t *txbuf); 47 | mlx90363_status_t parseEchoMessage(const uint8_t *rxbuf, uint16_t *key_echo); 48 | mlx90363_status_t parseReadyMessage(const uint8_t *rxbuf, uint8_t *fw_version, 49 | uint8_t *hw_version); 50 | mlx90363_status_t parseAlphaMessage(const uint8_t *rxbuf, uint16_t *alpha, 51 | uint8_t *vg); 52 | mlx90363_status_t parseDiagnosticsAnswerMessage(const uint8_t *rxbuf, 53 | uint32_t *diag_bits, 54 | uint8_t *fsmerc, 55 | uint8_t *anadiagcnt); 56 | uint8_t computeMessageCRC(const uint8_t *buf); 57 | 58 | private: 59 | SPIDriver *const spi_driver_; 60 | MLX90363SPIConfig spi_config_; 61 | const IOPin csn_; 62 | uint8_t async_txbuf_[8]; 63 | uint8_t async_rxbuf_[8]; 64 | 65 | static void spiEndCallbackStatic(SPIDriver *spi_driver); 66 | void spiEndCallback(SPIDriver *spi_driver); 67 | }; 68 | 69 | } // namespace motor_driver 70 | 71 | #endif /* _MLX90363_H_ */ 72 | -------------------------------------------------------------------------------- /drivers/gate_driver/BUILD.bazel: -------------------------------------------------------------------------------- 1 | load("@rules_cc//cc:defs.bzl", "cc_library") 2 | load("//third_party:variables.bzl", "project_inc_paths") 3 | 4 | cc_library( 5 | name = "drv8312", 6 | srcs = ["drv8312.cpp"], 7 | hdrs = ["drv8312.hpp"], 8 | copts = project_inc_paths, 9 | visibility = ["//visibility:public"], 10 | deps = [ 11 | "//common", 12 | ], 13 | alwayslink = True, 14 | ) 15 | -------------------------------------------------------------------------------- /drivers/gate_driver/drv8301.cpp: -------------------------------------------------------------------------------- 1 | #include "DRV8301.h" 2 | 3 | namespace motor_driver { 4 | 5 | void DRV8301::start() { 6 | spiStart(spi_driver_, &spi_config_); 7 | 8 | // Set control pins to default values 9 | pwmEnableChannel(pwm_driver_, ch_a_, 0); 10 | pwmEnableChannel(pwm_driver_, ch_b_, 0); 11 | pwmEnableChannel(pwm_driver_, ch_c_, 0); 12 | palSetPad(nscs_.port, nscs_.pin); 13 | 14 | // Enable gate driver 15 | palSetPad(en_gate_.port, en_gate_.pin); 16 | 17 | // Configure parameters over SPI 18 | writeRegister(0x02, 0x0400); // Set gate drive peak current to 1.7 A 19 | } 20 | 21 | uint16_t DRV8301::readRegister(uint8_t addr) { 22 | uint8_t txbuf[2] = {(uint8_t)(0x80 | ((addr << 3) & 0x78)), 0}; 23 | uint8_t rxbuf[2]; 24 | 25 | palClearPad(nscs_.port, nscs_.pin); 26 | spiExchange(spi_driver_, 2, txbuf, rxbuf); 27 | palSetPad(nscs_.port, nscs_.pin); 28 | return ((uint16_t)(rxbuf[0] & 0x07) << 8) | (uint16_t)rxbuf[1]; 29 | } 30 | 31 | void DRV8301::writeRegister(uint8_t addr, uint16_t value) { 32 | uint8_t buf[2] = {(uint8_t)(((addr << 3) & 0x78) | ((value >> 8) & 0x07)), 33 | (uint8_t)(value & 0xff)}; 34 | 35 | palClearPad(nscs_.port, nscs_.pin); 36 | spiSend(spi_driver_, 2, buf); 37 | palSetPad(nscs_.port, nscs_.pin); 38 | } 39 | 40 | bool DRV8301::hasFault() { return !palReadPad(nfault_.port, nfault_.pin); } 41 | 42 | bool DRV8301::hasOCTW() { return !palReadPad(noctw_.port, noctw_.pin); } 43 | 44 | void DRV8301::setPWMPulseWidth(int phase, int32_t pulse_width) { 45 | pwmchannel_t ch; 46 | 47 | switch (phase) { 48 | case 0: 49 | ch = ch_a_; 50 | break; 51 | case 1: 52 | ch = ch_b_; 53 | break; 54 | case 2: 55 | ch = ch_c_; 56 | break; 57 | default: 58 | return; 59 | } 60 | 61 | pwmEnableChannel(pwm_driver_, ch, pulse_width); 62 | } 63 | 64 | void DRV8301::setPWMDutyCycle(int phase, float duty_cycle) { 65 | int32_t pulse_width = ::lround(duty_cycle * pwm_driver_->config->period); 66 | pulse_width = std::max( 67 | (int32_t)0, std::min((int32_t)pwm_driver_->config->period, pulse_width)); 68 | setPWMPulseWidth(phase, pulse_width); 69 | } 70 | 71 | } // namespace motor_driver 72 | -------------------------------------------------------------------------------- /drivers/gate_driver/drv8301.h: -------------------------------------------------------------------------------- 1 | #ifndef _DRV8301_H_ 2 | #define _DRV8301_H_ 3 | 4 | #include "ch.h" 5 | #include "hal.h" 6 | #include "utils.h" 7 | #include 8 | #include 9 | 10 | namespace motor_driver { 11 | 12 | /** 13 | * Texas Instruments DRV8301 Three-Phase Gate Driver With Dual Current Shunt 14 | * Amplifiers and Buck Regulator 15 | */ 16 | class DRV8301 { 17 | public: 18 | DRV8301(SPIDriver &spi_driver, PWMDriver &pwm_driver, const pwmchannel_t ch_a, 19 | const pwmchannel_t ch_b, const pwmchannel_t ch_c, const IOPin nscs, 20 | const IOPin en_gate, const IOPin nfault, const IOPin noctw) 21 | : spi_driver_(&spi_driver), pwm_driver_(&pwm_driver), ch_a_(ch_a), 22 | ch_b_(ch_b), ch_c_(ch_c), nscs_(nscs), en_gate_(en_gate), 23 | nfault_(nfault), noctw_(noctw) { 24 | spi_config_.end_cb = NULL; 25 | spi_config_.ssport = nscs.port; 26 | spi_config_.sspad = nscs.pin; 27 | spi_config_.cr1 = 28 | SPI_CR1_BR_1 | SPI_CR1_BR_0 | SPI_CR1_MSTR | 29 | SPI_CR1_CPHA; // f_PCLK/16, master mode, capture data on second edge 30 | } 31 | void start(); 32 | uint16_t readRegister(uint8_t addr); 33 | void writeRegister(uint8_t addr, uint16_t value); 34 | bool hasFault(); 35 | bool hasOCTW(); 36 | void setPWMPulseWidth(int phase, int32_t pulse_width); 37 | void setPWMDutyCycle(int phase, float duty_cycle); 38 | 39 | private: 40 | SPIDriver *const spi_driver_; 41 | SPIConfig spi_config_; 42 | PWMDriver *const pwm_driver_; 43 | const pwmchannel_t ch_a_; 44 | const pwmchannel_t ch_b_; 45 | const pwmchannel_t ch_c_; 46 | const IOPin nscs_; 47 | const IOPin en_gate_; 48 | const IOPin nfault_; 49 | const IOPin noctw_; 50 | }; 51 | 52 | } // namespace motor_driver 53 | 54 | #endif /* _DRV8301_H_ */ 55 | -------------------------------------------------------------------------------- /drivers/gate_driver/drv8312.cpp: -------------------------------------------------------------------------------- 1 | #include "drivers/gate_driver/drv8312.hpp" 2 | 3 | namespace motor_driver { 4 | namespace peripherals { 5 | 6 | void DRV8312::start() { 7 | // Set control pins to default values 8 | pwmEnableChannel(pwm_driver_, ch_a_, 0); 9 | pwmEnableChannel(pwm_driver_, ch_b_, 0); 10 | pwmEnableChannel(pwm_driver_, ch_c_, 0); 11 | 12 | // Disable control of gates. 13 | disableGates(); 14 | } 15 | 16 | bool DRV8312::hasFault() { return !palReadPad(nfault_.port, nfault_.pin); } 17 | 18 | bool DRV8312::hasOCTW() { return !palReadPad(noctw_.port, noctw_.pin); } 19 | 20 | void DRV8312::setPWMPulseWidth(int phase, int32_t pulse_width) { 21 | pwmchannel_t ch; 22 | 23 | switch (phase) { 24 | case 0: 25 | ch = ch_a_; 26 | break; 27 | case 1: 28 | ch = ch_b_; 29 | break; 30 | case 2: 31 | ch = ch_c_; 32 | break; 33 | default: 34 | return; 35 | } 36 | 37 | pwmEnableChannel(pwm_driver_, ch, pulse_width); 38 | } 39 | 40 | void DRV8312::setPWMDutyCycle(int phase, float duty_cycle) { 41 | int32_t pulse_width = 42 | (::lround((1 - duty_cycle) * pwm_driver_->config->period)); 43 | pulse_width = std::max( 44 | (int32_t)0, std::min((int32_t)pwm_driver_->config->period, pulse_width)); 45 | setPWMPulseWidth(phase, pulse_width); 46 | } 47 | 48 | void DRV8312::enableGates() { 49 | // Set reset pins to high to allow for control of motors (active low)! 50 | palSetPad(rst_a_.port, rst_a_.pin); 51 | palSetPad(rst_b_.port, rst_b_.pin); 52 | palSetPad(rst_c_.port, rst_c_.pin); 53 | } 54 | 55 | void DRV8312::disableGates() { 56 | // Set reset pins to low to stop control of motors (active low)! 57 | palClearPad(rst_a_.port, rst_a_.pin); 58 | palClearPad(rst_b_.port, rst_b_.pin); 59 | palClearPad(rst_c_.port, rst_c_.pin); 60 | } 61 | 62 | } // namespace peripherals 63 | } // namespace motor_driver 64 | -------------------------------------------------------------------------------- /drivers/gate_driver/drv8312.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include "hal.h" 7 | 8 | #include "ch.h" 9 | #include "utils.h" 10 | 11 | namespace motor_driver { 12 | namespace peripherals { 13 | 14 | // Texas Instruments DRV8312 Three-Phase Gate Driver With Dual Current 15 | // Shunt Amplifiers and Buck Regulator 16 | class DRV8312 { 17 | public: 18 | DRV8312(PWMDriver &pwm_driver, const pwmchannel_t ch_a, 19 | const pwmchannel_t ch_b, const pwmchannel_t ch_c, const IOPin rst_a, 20 | const IOPin rst_b, const IOPin rst_c, const IOPin nfault, 21 | const IOPin noctw) 22 | : pwm_driver_(&pwm_driver), ch_a_(ch_a), ch_b_(ch_b), ch_c_(ch_c), 23 | rst_a_(rst_a), rst_b_(rst_b), rst_c_(rst_c), nfault_(nfault), 24 | noctw_(noctw) {} 25 | void start(); 26 | bool hasFault(); 27 | bool hasOCTW(); 28 | void setPWMPulseWidth(int phase, int32_t pulse_width); 29 | void setPWMDutyCycle(int phase, float duty_cycle); 30 | void enableGates(); 31 | void disableGates(); 32 | 33 | private: 34 | PWMDriver *const pwm_driver_; 35 | const pwmchannel_t ch_a_; 36 | const pwmchannel_t ch_b_; 37 | const pwmchannel_t ch_c_; 38 | const IOPin rst_a_; 39 | const IOPin rst_b_; 40 | const IOPin rst_c_; 41 | const IOPin nfault_; 42 | const IOPin noctw_; 43 | }; 44 | 45 | } // namespace peripherals 46 | } // namespace motor_driver 47 | -------------------------------------------------------------------------------- /drivers/imu/CPPLINT.cfg: -------------------------------------------------------------------------------- 1 | exclude_files=lsm6ds3.*\.cpp 2 | -------------------------------------------------------------------------------- /drivers/temperature/BUILD.bazel: -------------------------------------------------------------------------------- 1 | load("@rules_cc//cc:defs.bzl", "cc_library") 2 | load("//third_party:variables.bzl", "project_inc_paths") 3 | 4 | cc_library( 5 | name = "mcp9808", 6 | srcs = ["mcp9808.cpp"], 7 | hdrs = ["mcp9808.hpp"], 8 | copts = project_inc_paths, 9 | visibility = ["//visibility:public"], 10 | deps = [ 11 | "//common", 12 | ], 13 | alwayslink = True, 14 | ) 15 | -------------------------------------------------------------------------------- /drivers/temperature/lm75b.cpp: -------------------------------------------------------------------------------- 1 | #include "LM75B.h" 2 | 3 | namespace motor_driver { 4 | 5 | void LM75B::start() { i2cStart(i2c_driver_, &i2c_config_); } 6 | 7 | bool LM75B::receive(uint16_t addr, uint8_t *data, size_t size) { 8 | systime_t tmo = MS2ST(4); // 4 millisecond timeout 9 | msg_t status = i2cMasterReceiveTimeout(i2c_driver_, addr, data, size, tmo); 10 | return status == RDY_OK; 11 | } 12 | 13 | bool LM75B::getTemperature(float *temp) { 14 | uint8_t data[2]; 15 | bool success = LM75B::receive(0x48, data, 2); 16 | if (success) { 17 | int16_t bits = (data[0] << 8) + data[1]; 18 | *temp = static_cast(bits >> 5) * 0.125f; 19 | } 20 | 21 | return success; 22 | } 23 | 24 | } // namespace motor_driver 25 | -------------------------------------------------------------------------------- /drivers/temperature/lm75b.h: -------------------------------------------------------------------------------- 1 | #ifndef _LM75B_H_ 2 | #define _LM75B_H_ 3 | 4 | #include "hal.h" 5 | 6 | namespace motor_driver { 7 | 8 | class LM75B { 9 | public: 10 | LM75B(I2CDriver &i2c_driver) : i2c_driver_(&i2c_driver) { 11 | i2c_config_.op_mode = OPMODE_I2C; 12 | i2c_config_.clock_speed = 400000; 13 | i2c_config_.duty_cycle = FAST_DUTY_CYCLE_2; 14 | } 15 | void start(); 16 | bool receive(uint16_t addr, uint8_t *data, size_t size); 17 | bool getTemperature(float *temp); 18 | 19 | private: 20 | I2CDriver *const i2c_driver_; 21 | I2CConfig i2c_config_; 22 | }; 23 | 24 | } // namespace motor_driver 25 | 26 | #endif /* _LM75B_H_ */ 27 | -------------------------------------------------------------------------------- /drivers/temperature/mcp9808.cpp: -------------------------------------------------------------------------------- 1 | #include "drivers/temperature/mcp9808.hpp" 2 | 3 | namespace motor_driver { 4 | namespace peripherals { 5 | 6 | void MCP9808::start() { 7 | uint8_t config[3]; 8 | // 4 millisecond timeout. 9 | systime_t tmo = MS2ST(4); 10 | 11 | config[0] = 0x01; 12 | config[1] = 0x00; 13 | config[2] = 0x08; 14 | 15 | i2cStart(i2c_driver_, &i2c_config_); 16 | 17 | i2cAcquireBus(i2c_driver_); 18 | 19 | i2cMasterTransmitTimeout(i2c_driver_, 20 | // Address. 21 | MCP9808_DEFAULT_ADDRESS, 22 | // TX Buffer, Len. 23 | config, 3, 24 | // RX Buffer, Len. 25 | NULL, 0, 26 | // Timeout. 27 | tmo); 28 | 29 | i2cReleaseBus(i2c_driver_); 30 | } 31 | 32 | bool MCP9808::checkID() { return true; } 33 | 34 | bool MCP9808::receive(uint8_t reg, uint8_t *data, size_t size) { 35 | // 4 millisecond timeout. 36 | systime_t tmo = MS2ST(4); 37 | i2cAcquireBus(i2c_driver_); 38 | 39 | msg_t status = i2cMasterTransmitTimeout(i2c_driver_, MCP9808_DEFAULT_ADDRESS, 40 | ®, 1, data, size, tmo); 41 | 42 | i2cReleaseBus(i2c_driver_); 43 | return status == RDY_OK; 44 | } 45 | 46 | /* **** Temperature byte packet structure **** * 47 | * | MSB(7) | (6) | (5) | (4) | (3) | (2) | (1) | (0) | * 48 | * Up |Ta vs Tc|Ta v Tu|Ta v Tl| Sign | 2^7C | 2^6C | 2^5C | 2^4C | * 49 | * Lo | 2^3C | 2^2C | 2^1C | 2^0C | 2^-1C | 2^-2C | 2^-3C | 2^-4C | */ 50 | bool MCP9808::getTemperature(float *temp) { 51 | uint8_t data[2]; 52 | bool success = MCP9808::receive(MCP9808_TEMP_AMBIENT, data, 2); 53 | 54 | if (success) { 55 | uint16_t bits = (((uint16_t)data[0] & 0x0F) << 8 | data[1]); 56 | // Mult by 2^-4. 57 | *temp = static_cast(bits) * 0.0625f; 58 | } 59 | 60 | return success; 61 | } 62 | 63 | } // namespace peripherals 64 | } // namespace motor_driver 65 | -------------------------------------------------------------------------------- /drivers/temperature/mcp9808.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "hal.h" 4 | 5 | namespace motor_driver { 6 | namespace peripherals { 7 | 8 | constexpr uint8_t MCP9808_DEFAULT_ADDRESS = 0b0011001; 9 | constexpr uint8_t MCP9808_TEMP_AMBIENT = 0x05; 10 | 11 | class MCP9808 { 12 | public: 13 | explicit MCP9808(I2CDriver &i2c_driver) : i2c_driver_(&i2c_driver) { 14 | i2c_config_.op_mode = OPMODE_I2C; 15 | i2c_config_.clock_speed = 400000; 16 | i2c_config_.duty_cycle = FAST_DUTY_CYCLE_2; 17 | } 18 | void start(); 19 | bool receive(uint8_t reg, uint8_t *data, size_t size); 20 | bool getTemperature(float *temp); 21 | bool checkID(); 22 | 23 | private: 24 | I2CDriver *const i2c_driver_; 25 | I2CConfig i2c_config_; 26 | }; 27 | 28 | } // namespace peripherals 29 | } // namespace motor_driver 30 | -------------------------------------------------------------------------------- /firmware/.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | .dep/ 3 | *.pb.c 4 | *.pb.h 5 | -------------------------------------------------------------------------------- /firmware/BUILD.bazel: -------------------------------------------------------------------------------- 1 | load("@rules_cc//cc:defs.bzl", "cc_binary", "cc_library") 2 | load("//third_party:variables.bzl", "project_inc_paths") 3 | load("//toolchains:debug.bzl", "debug_gdb") 4 | load("//toolchains:platforms.bzl", "firmware_binary", "gen_binary") 5 | load("//toolchains:os_config.bzl", "os_config") 6 | load("//toolchains:flash.bzl", "flash_remote") 7 | load( 8 | "@com_github_nanopb_nanopb//extra/bazel:nanopb_cc_proto_library.bzl", 9 | "cc_nanopb_proto_library", 10 | ) 11 | load("@bazel_embedded//tools/openocd:defs.bzl", "openocd_debug_server", "openocd_flash") 12 | 13 | firmware_binary( 14 | name = "firmware_raw", 15 | srcs = glob(["src/*.c"]) + glob(["src/*.cpp"]), 16 | copts = project_inc_paths, 17 | linkopts = [ 18 | "-T $(location :link.ld)", 19 | ], 20 | visibility = ["//visibility:public"], 21 | deps = [ 22 | ":link.ld", 23 | ":main_libs", 24 | ], 25 | ) 26 | 27 | cc_nanopb_proto_library( 28 | name = "messages_nanopb", 29 | protos = ["//firmware/proto:messages_proto"], 30 | visibility = ["//visibility:private"], 31 | ) 32 | 33 | cc_library( 34 | name = "main_libs", 35 | hdrs = glob( 36 | [ 37 | "include/*.h", 38 | "include/*.hpp", 39 | ], 40 | exclude = [ 41 | "include/stm32f4xx_conf.h", 42 | "include/chconf.h", 43 | "include/halconf.h", 44 | "include/mcuconf.h", 45 | ], 46 | ), 47 | copts = project_inc_paths, 48 | includes = ["include"], 49 | deps = [ 50 | ":messages_nanopb", 51 | "//common", 52 | "//drivers/accel:iis328dq", 53 | "//drivers/encoder:as5047d", 54 | "//drivers/gate_driver:drv8312", 55 | "//drivers/temperature:mcp9808", 56 | "//util/time:usec", 57 | "@com_github_nanopb_nanopb//:nanopb", 58 | ], 59 | alwayslink = True, 60 | ) 61 | 62 | filegroup( 63 | name = "peripheral_config", 64 | srcs = ["include/peripherals.hpp"], 65 | visibility = ["//visibility:public"], 66 | ) 67 | 68 | os_config( 69 | name = "os_config", 70 | program = "firmware", 71 | ) 72 | 73 | gen_binary( 74 | name = "firmware", 75 | src = ":firmware_raw", 76 | ) 77 | 78 | flash_remote( 79 | name = "upload", 80 | image = ":firmware.bin", 81 | upload_type = "firmware", 82 | ) 83 | 84 | openocd_flash( 85 | name = "flash", 86 | device_configs = [ 87 | "target/stm32f4x.cfg", 88 | ], 89 | flash_offset = "0x8010000", 90 | image = ":firmware.bin", 91 | interface_configs = [ 92 | "interface/stlink.cfg", 93 | ], 94 | ) 95 | 96 | openocd_debug_server( 97 | name = "debug", 98 | device_configs = [ 99 | "target/stm32f4x.cfg", 100 | ], 101 | interface_configs = [ 102 | "interface/stlink.cfg", 103 | ], 104 | transport = "hla_swd", 105 | ) 106 | 107 | debug_gdb( 108 | name = "gdb", 109 | elf = ":firmware.elf", 110 | ) 111 | -------------------------------------------------------------------------------- /firmware/CPPLINT.cfg: -------------------------------------------------------------------------------- 1 | set noparent 2 | 3 | # TODO: Add copyright to the top of every file. 4 | filter=-legal/copyright 5 | # Include structure of chibios is weird. Figure this out? 6 | filter=-build/include_subdir 7 | 8 | # TODO: Need to pass return arguments as pointers to show explicit modification 9 | # in-line. 10 | filter=-runtime/references 11 | # TODO: Constexprs should be renamed to kConstant format. 12 | filter=-runtime/arrays 13 | 14 | # Clang default formats one space before comments; cpplint wants 2. 15 | filter=-whitespace/comments 16 | # Clang defaults to public/private being not indented; cpplint wants indent. 17 | filter=-whitespace/indent 18 | 19 | # Ignore c files. 20 | extensions=hpp,cpp 21 | 22 | # Simplifies include guard naming. 23 | root=include/ 24 | -------------------------------------------------------------------------------- /firmware/include/LUTFunction.hpp: -------------------------------------------------------------------------------- 1 | #ifndef LUTFUNCTION_HPP_ 2 | #define LUTFUNCTION_HPP_ 3 | 4 | #include 5 | 6 | namespace motor_driver { 7 | namespace math { 8 | 9 | enum class LFFlipType { NONE, HORIZONTAL, VERTICAL, BOTH }; 10 | 11 | struct LFPeriodicity { 12 | size_t repetition_count; 13 | const LFFlipType *repetition_flips; 14 | }; 15 | 16 | template class LUTFunction { 17 | public: 18 | LUTFunction(float x_first, float x_last, const T *y, size_t y_len, 19 | LFPeriodicity periodicity) 20 | : x_first_(x_first), x_last_(x_last), y_(y), y_len_(y_len), 21 | periodicity_(periodicity) {} 22 | float lookup(float arg) const; 23 | float operator()(float arg) const { return lookup(arg); } 24 | 25 | private: 26 | const float x_first_; 27 | const float x_last_; 28 | const T *const y_; 29 | const size_t y_len_; 30 | const LFPeriodicity periodicity_; 31 | 32 | float lookupReduced(float reduced_arg) const; 33 | }; 34 | 35 | } // namespace math 36 | } // namespace motor_driver 37 | 38 | #endif // LUTFUNCTION_HPP_ 39 | -------------------------------------------------------------------------------- /firmware/include/Recorder.hpp: -------------------------------------------------------------------------------- 1 | #ifndef RECORDER_HPP_ 2 | #define RECORDER_HPP_ 3 | 4 | #include 5 | 6 | #include "constants.hpp" 7 | 8 | namespace motor_driver { 9 | namespace state { 10 | 11 | class Recorder { 12 | public: 13 | Recorder() : state_(State::READY), index_(0) {} 14 | 15 | bool startRecording(); 16 | 17 | void recordSample(float *recorder_new_data); 18 | 19 | float *read(); 20 | 21 | void reset(); 22 | 23 | uint16_t size(); 24 | 25 | private: 26 | enum class State { READY, RECORDING, FINISHED }; 27 | 28 | State state_; 29 | size_t index_; 30 | float record_buf_[(consts::recorder_max_samples * 31 | consts::recorder_channel_count)]; 32 | }; 33 | 34 | } // namespace state 35 | } // namespace motor_driver 36 | 37 | #endif // RECORDER_HPP_ 38 | -------------------------------------------------------------------------------- /firmware/include/SVM.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SVM_HPP_ 2 | #define SVM_HPP_ 3 | 4 | namespace motor_driver { 5 | namespace controller { 6 | 7 | enum class SVMStrategy { SINUSOIDAL, TOP_BOTTOM_CLAMP, MIDPOINT_CLAMP }; 8 | 9 | class SVM { 10 | public: 11 | explicit SVM(SVMStrategy strategy) : strategy_(strategy) {} 12 | 13 | void computeDutyCycles(float v_alpha, float v_beta, float &dc_a, float &dc_b, 14 | float &dc_c); 15 | 16 | float getMaxAmplitude() const; 17 | 18 | void setStrategy(SVMStrategy strategy) { strategy_ = strategy; } 19 | 20 | private: 21 | SVMStrategy strategy_; 22 | }; 23 | 24 | } // namespace controller 25 | } // namespace motor_driver 26 | 27 | #endif // SVM_HPP_ 28 | -------------------------------------------------------------------------------- /firmware/include/control.hpp: -------------------------------------------------------------------------------- 1 | #ifndef CONTROL_HPP_ 2 | #define CONTROL_HPP_ 3 | 4 | #include 5 | 6 | #include "constants.hpp" 7 | 8 | namespace motor_driver { 9 | namespace controller { 10 | 11 | struct RolledADC { 12 | uint16_t count = 0; 13 | 14 | uint16_t ia[consts::ivsense_rolling_average_count] = {0}; 15 | uint16_t ib[consts::ivsense_rolling_average_count] = {0}; 16 | uint16_t ic[consts::ivsense_rolling_average_count] = {0}; 17 | uint16_t vin[consts::ivsense_rolling_average_count] = {0}; 18 | 19 | RolledADC() {} 20 | }; 21 | 22 | void initControl(); 23 | 24 | void resumeInnerControlLoop(); 25 | 26 | void runInnerControlLoop(); 27 | 28 | void estimateState(); 29 | 30 | void runPositionControl(); 31 | 32 | void runVelocityControl(); 33 | 34 | void runCurrentControl(); 35 | 36 | void resetControlTimeout(); 37 | 38 | void brakeMotor(); 39 | 40 | } // namespace controller 41 | } // namespace motor_driver 42 | 43 | #endif // CONTROL_HPP_ 44 | -------------------------------------------------------------------------------- /firmware/include/crc_mlx.h: -------------------------------------------------------------------------------- 1 | /** 2 | * \file 3 | * Functions and types for CRC checks. 4 | * 5 | * Generated on Thu Jun 21 19:28:07 2018 6 | * by pycrc v0.9.1, https://pycrc.org 7 | * using the configuration: 8 | * - Width = 8 9 | * - Poly = 0x2f 10 | * - XorIn = 0xff 11 | * - ReflectIn = False 12 | * - XorOut = 0xff 13 | * - ReflectOut = False 14 | * - Algorithm = table-driven 15 | * 16 | * This file defines the functions crc_mlx_init(), crc_mlx_update() and 17 | * crc_mlx_finalize(). 18 | * 19 | * The crc_mlx_init() function returns the inital \c crc value and must be 20 | * called before the first call to crc_mlx_update(). Similarly, the 21 | * crc_mlx_finalize() function must be called after the last call to 22 | * crc_mlx_update(), before the \c crc is being used. is being used. 23 | * 24 | * The crc_mlx_update() function can be called any number of times (including 25 | * zero times) in between the crc_mlx_init() and crc_mlx_finalize() calls. 26 | * 27 | * This pseudo-code shows an example usage of the API: 28 | * \code{.c} 29 | * crc_mlx_t crc; 30 | * unsigned char data[MAX_DATA_LEN]; 31 | * size_t data_len; 32 | * 33 | * crc = crc_mlx_init(); 34 | * while ((data_len = read_data(data, MAX_DATA_LEN)) > 0) { 35 | * crc = crc_mlx_update(crc, data, data_len); 36 | * } 37 | * crc = crc_mlx_finalize(crc); 38 | * \endcode 39 | */ 40 | #ifndef _CRC_MLX_H_ 41 | #define _CRC_MLX_H_ 42 | 43 | #include 44 | #include 45 | 46 | #ifdef __cplusplus 47 | extern "C" { 48 | #endif 49 | 50 | /** 51 | * The definition of the used algorithm. 52 | * 53 | * This is not used anywhere in the generated code, but it may be used by the 54 | * application code to call algorithm-specific code, if desired. 55 | */ 56 | #define CRC_ALGO_TABLE_DRIVEN 1 57 | 58 | /** 59 | * The type of the CRC values. 60 | * 61 | * This type must be big enough to contain at least 8 bits. 62 | */ 63 | typedef uint_fast8_t crc_mlx_t; 64 | 65 | /** 66 | * Calculate the initial crc value. 67 | * 68 | * \return The initial crc value. 69 | */ 70 | static inline crc_mlx_t crc_mlx_init(void) { return 0xff; } 71 | 72 | /** 73 | * Update the crc value with new data. 74 | * 75 | * \param[in] crc The current crc value. 76 | * \param[in] data Pointer to a buffer of \a data_len bytes. 77 | * \param[in] data_len Number of bytes in the \a data buffer. 78 | * \return The updated crc value. 79 | */ 80 | crc_mlx_t crc_mlx_update(crc_mlx_t crc, const void *data, size_t data_len); 81 | 82 | /** 83 | * Calculate the final crc value. 84 | * 85 | * \param[in] crc The current crc value. 86 | * \return The final crc value. 87 | */ 88 | static inline crc_mlx_t crc_mlx_finalize(crc_mlx_t crc) { return crc ^ 0xff; } 89 | 90 | #ifdef __cplusplus 91 | } /* closing brace for extern "C" */ 92 | #endif 93 | 94 | #endif /* _CRC_MLX_H_ */ 95 | -------------------------------------------------------------------------------- /firmware/include/fast_math.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FAST_MATH_HPP_ 2 | #define FAST_MATH_HPP_ 3 | 4 | #include "LUTFunction.hpp" 5 | 6 | namespace motor_driver { 7 | namespace math { 8 | 9 | extern const LUTFunction fast_sin; 10 | extern const LUTFunction fast_cos; 11 | 12 | } // namespace math 13 | } // namespace motor_driver 14 | 15 | #endif // FAST_MATH_HPP_ 16 | -------------------------------------------------------------------------------- /firmware/include/firmware_peripherals.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "hal.h" 6 | 7 | #include "drivers/encoder/as5047d.hpp" 8 | #include "drivers/gate_driver/drv8312.hpp" 9 | #include "drivers/accel/iis328dq.hpp" 10 | #include "drivers/temperature/mcp9808.hpp" 11 | #include "constants.hpp" 12 | 13 | namespace motor_driver { 14 | namespace controller { 15 | extern void resumeInnerControlLoop(); 16 | } 17 | namespace peripherals { 18 | 19 | extern PWMConfig motor_pwm_config; 20 | 21 | extern DRV8312 gate_driver; 22 | 23 | extern const PWMConfig led_pwm_config; 24 | 25 | extern AS5047D encoder; 26 | 27 | extern MCP9808 temp_sensor; 28 | 29 | extern IIS328DQ acc; 30 | 31 | extern BinarySemaphore ivsense_adc_samples_bsem; 32 | 33 | extern volatile adcsample_t *vsense_adc_samples_ptr; 34 | extern volatile adcsample_t *curra_adc_samples_ptr; 35 | extern volatile adcsample_t *currb_adc_samples_ptr; 36 | extern volatile adcsample_t *currc_adc_samples_ptr; 37 | 38 | // extern volatile size_t ivsense_adc_samples_count; 39 | 40 | extern adcsample_t ivsense_sample_buf[(consts::ivsense_channel_count * 41 | consts::ivsense_sample_buf_depth)]; 42 | 43 | extern Mutex var_access_mutex; 44 | 45 | void initPeripherals(); 46 | 47 | void startPeripherals(); 48 | 49 | void startEncoder(); 50 | 51 | void setStatusLEDColor(uint8_t red, uint8_t green, uint8_t blue); 52 | 53 | void setStatusLEDColor(uint32_t color); 54 | 55 | void setADCOn(); 56 | 57 | void setCommsActivityLED(bool on); 58 | 59 | void setRS485TransmitMode(bool transmit); 60 | 61 | /** 62 | * Converts an ADC value to voltage (in volts) 63 | */ 64 | inline float adcValueToVoltage(uint16_t adc_value) { 65 | return static_cast(adc_value) * consts::vsense_voltage_per_count; 66 | } 67 | 68 | /** 69 | * Converts an ADC value to current (in amperes) 70 | */ 71 | inline float adcValueToCurrent(uint16_t adc_value) { 72 | return ((static_cast(adc_value) - consts::isense_count_zero_current) * 73 | consts::isense_current_per_count); 74 | } 75 | 76 | } // namespace peripherals 77 | } // namespace motor_driver 78 | -------------------------------------------------------------------------------- /firmware/include/old_drivers/shell.h: -------------------------------------------------------------------------------- 1 | /* 2 | ChibiOS/RT - Copyright (C) 2006-2013 Giovanni Di Sirio 3 | 4 | Licensed under the Apache License, Version 2.0 (the "License"); 5 | you may not use this file except in compliance with the License. 6 | You may obtain a copy of the License at 7 | 8 | http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | Unless required by applicable law or agreed to in writing, software 11 | distributed under the License is distributed on an "AS IS" BASIS, 12 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | See the License for the specific language governing permissions and 14 | limitations under the License. 15 | */ 16 | 17 | /** 18 | * @file shell.h 19 | * @brief Simple CLI shell header. 20 | * 21 | * @addtogroup SHELL 22 | * @{ 23 | */ 24 | 25 | #ifndef _SHELL_H_ 26 | #define _SHELL_H_ 27 | 28 | /** 29 | * @brief Shell maximum input line length. 30 | */ 31 | #if !defined(SHELL_MAX_LINE_LENGTH) || defined(__DOXYGEN__) 32 | #define SHELL_MAX_LINE_LENGTH 64 33 | #endif 34 | 35 | /** 36 | * @brief Shell maximum arguments per command. 37 | */ 38 | #if !defined(SHELL_MAX_ARGUMENTS) || defined(__DOXYGEN__) 39 | #define SHELL_MAX_ARGUMENTS 5 40 | #endif 41 | 42 | /** 43 | * @brief Command handler function type. 44 | */ 45 | typedef void (*shellcmd_t)(BaseSequentialStream *chp, int argc, char *argv[]); 46 | 47 | /** 48 | * @brief Custom command entry type. 49 | */ 50 | typedef struct { 51 | const char *sc_name; /**< @brief Command name. */ 52 | shellcmd_t sc_function; /**< @brief Command function. */ 53 | } ShellCommand; 54 | 55 | /** 56 | * @brief Shell descriptor type. 57 | */ 58 | typedef struct { 59 | BaseSequentialStream *sc_channel; /**< @brief I/O channel associated 60 | to the shell. */ 61 | const ShellCommand *sc_commands; /**< @brief Shell extra commands 62 | table. */ 63 | } ShellConfig; 64 | 65 | #if !defined(__DOXYGEN__) 66 | extern EventSource shell_terminated; 67 | #endif 68 | 69 | extern bool shellEchoEnabled; 70 | 71 | #ifdef __cplusplus 72 | extern "C" { 73 | #endif 74 | void shellInit(void); 75 | void shellExit(msg_t msg); 76 | Thread *shellCreate(const ShellConfig *scp, size_t size, tprio_t prio); 77 | Thread *shellCreateStatic(const ShellConfig *scp, void *wsp, size_t size, 78 | tprio_t prio); 79 | bool_t shellGetLine(BaseSequentialStream *chp, char *line, unsigned size); 80 | #ifdef __cplusplus 81 | } 82 | #endif 83 | 84 | #endif /* _SHELL_H_ */ 85 | 86 | /** @} */ 87 | -------------------------------------------------------------------------------- /firmware/include/old_drivers/usbcfg.h: -------------------------------------------------------------------------------- 1 | /* 2 | ChibiOS/RT - Copyright (C) 2006-2013 Giovanni Di Sirio 3 | 4 | Licensed under the Apache License, Version 2.0 (the "License"); 5 | you may not use this file except in compliance with the License. 6 | You may obtain a copy of the License at 7 | 8 | http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | Unless required by applicable law or agreed to in writing, software 11 | distributed under the License is distributed on an "AS IS" BASIS, 12 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | See the License for the specific language governing permissions and 14 | limitations under the License. 15 | */ 16 | 17 | #ifndef _USBCFG_H_ 18 | #define _USBCFG_H_ 19 | 20 | extern const USBConfig usb_config; 21 | extern SerialUSBConfig serial_usb_config; 22 | 23 | #endif /* _USBCFG_H_ */ 24 | 25 | /** @} */ 26 | -------------------------------------------------------------------------------- /firmware/include/peripherals.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "hal.h" 4 | 5 | namespace motor_driver { 6 | namespace peripherals { 7 | 8 | constexpr UARTDriver *rs485_uart_driver = &UARTD1; 9 | 10 | } // namespace peripherals 11 | } // namespace motor_driver 12 | -------------------------------------------------------------------------------- /firmware/include/pid.hpp: -------------------------------------------------------------------------------- 1 | #ifndef PID_HPP_ 2 | #define PID_HPP_ 3 | 4 | namespace motor_driver { 5 | namespace controller { 6 | 7 | class PID { 8 | private: 9 | float kp_, ki_, kd_; 10 | float min_, max_; 11 | float target_; 12 | 13 | float interval_; 14 | float windup_; 15 | 16 | float alpha_; 17 | float deriv_; 18 | float val_prev_; 19 | 20 | float computeIntegral(float err); 21 | float computeDerivative(float val); 22 | 23 | public: 24 | PID(float kp, float ki, float kd, float interval); 25 | 26 | void setGains(float kp, float ki, float kd); 27 | void setLimits(float min, float max); 28 | void setTarget(float target); 29 | void setAlpha(float alpha); 30 | 31 | float compute(float val); 32 | }; 33 | 34 | } // namespace controller 35 | } // namespace motor_driver 36 | 37 | #endif // PID_HPP_ 38 | -------------------------------------------------------------------------------- /firmware/include/stm32f4xx_conf.h: -------------------------------------------------------------------------------- 1 | #ifndef __STM32F4xx_CONF_H 2 | #define __STM32F4xx_CONF_H 3 | 4 | #define USE_RTOS 0 5 | 6 | #include "misc.h" 7 | #include "stm32f4xx_adc.h" 8 | #include "stm32f4xx_dma.h" 9 | #include "stm32f4xx_exti.h" 10 | #include "stm32f4xx_flash.h" 11 | #include "stm32f4xx_i2c.h" 12 | #include "stm32f4xx_rcc.h" 13 | #include "stm32f4xx_syscfg.h" 14 | #include "stm32f4xx_tim.h" 15 | #include "stm32f4xx_wwdg.h" 16 | 17 | #ifdef USE_FULL_ASSERT 18 | #define assert_param(expr) \ 19 | ((expr) ? (void)0 : assert_failed((uint8_t *)__FILE__, __LINE__)) 20 | void assert_failed(uint8_t *file, uint32_t line); 21 | #else 22 | #define assert_param(expr) ((void)0) 23 | #endif 24 | 25 | #endif 26 | -------------------------------------------------------------------------------- /firmware/include/transforms.hpp: -------------------------------------------------------------------------------- 1 | #ifndef TRANSFORMS_HPP_ 2 | #define TRANSFORMS_HPP_ 3 | 4 | namespace motor_driver { 5 | namespace math { 6 | 7 | void transformPark(float alpha, float beta, float cos_theta, float sin_theta, 8 | float &d, float &q); 9 | 10 | void transformInversePark(float d, float q, float cos_theta, float sin_theta, 11 | float &alpha, float &beta); 12 | 13 | void transformClarke(float a, float b, float c, float &alpha, float &beta); 14 | 15 | void transformInverseClarke(float alpha, float beta, float &a, float &b, 16 | float &c); 17 | 18 | } // namespace math 19 | } // namespace motor_driver 20 | 21 | #endif // TRANSFORMS_HPP_ 22 | -------------------------------------------------------------------------------- /firmware/link.ld: -------------------------------------------------------------------------------- 1 | /* 2 | ChibiOS/RT - Copyright (C) 2006-2013 Giovanni Di Sirio 3 | 4 | Licensed under the Apache License, Version 2.0 (the "License"); 5 | you may not use this file except in compliance with the License. 6 | You may obtain a copy of the License at 7 | 8 | http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | Unless required by applicable law or agreed to in writing, software 11 | distributed under the License is distributed on an "AS IS" BASIS, 12 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | See the License for the specific language governing permissions and 14 | limitations under the License. 15 | */ 16 | 17 | /* 18 | * STM32F405xG memory setup. 19 | */ 20 | __main_stack_size__ = 0x0400; 21 | __process_stack_size__ = 0x0400; 22 | 23 | MEMORY 24 | { 25 | flash_bl : org = 0x08000000, len = 32k 26 | flash_nvp : org = 0x08000000 + 32k, len = 16k 27 | flash_id : org = 0x08000000 + 48k, len = 16k 28 | flash_fw : org = 0x08000000 + 64k, len = 1M - 64k 29 | ram : org = 0x20000000, len = 112k 30 | ethram : org = 0x2001C000, len = 16k 31 | ccmram : org = 0x10000000, len = 64k 32 | } 33 | 34 | __ram_start__ = ORIGIN(ram); 35 | __ram_size__ = LENGTH(ram); 36 | __ram_end__ = __ram_start__ + __ram_size__; 37 | 38 | ENTRY(ResetHandler) 39 | 40 | SECTIONS 41 | { 42 | . = 0; 43 | _text = .; 44 | 45 | startup : ALIGN(16) SUBALIGN(16) 46 | { 47 | KEEP(*(vectors)) 48 | } > flash_fw 49 | 50 | constructors : ALIGN(4) SUBALIGN(4) 51 | { 52 | PROVIDE(__init_array_start = .); 53 | KEEP(*(SORT(.init_array.*))) 54 | KEEP(*(.init_array)) 55 | PROVIDE(__init_array_end = .); 56 | } > flash_fw 57 | 58 | destructors : ALIGN(4) SUBALIGN(4) 59 | { 60 | PROVIDE(__fini_array_start = .); 61 | KEEP(*(.fini_array)) 62 | KEEP(*(SORT(.fini_array.*))) 63 | PROVIDE(__fini_array_end = .); 64 | } > flash_fw 65 | 66 | .text : ALIGN(16) SUBALIGN(16) 67 | { 68 | *(.text.startup.*) 69 | *(.text) 70 | *(.text.*) 71 | *(.rodata) 72 | *(.rodata.*) 73 | *(.glue_7t) 74 | *(.glue_7) 75 | *(.gcc*) 76 | } > flash_fw 77 | 78 | .ARM.extab : 79 | { 80 | *(.ARM.extab* .gnu.linkonce.armextab.*) 81 | } > flash_fw 82 | 83 | .ARM.exidx : { 84 | PROVIDE(__exidx_start = .); 85 | *(.ARM.exidx* .gnu.linkonce.armexidx.*) 86 | PROVIDE(__exidx_end = .); 87 | } > flash_fw 88 | 89 | .eh_frame_hdr : 90 | { 91 | *(.eh_frame_hdr) 92 | } > flash_fw 93 | 94 | .eh_frame : ONLY_IF_RO 95 | { 96 | *(.eh_frame) 97 | } > flash_fw 98 | 99 | .textalign : ONLY_IF_RO 100 | { 101 | . = ALIGN(8); 102 | } > flash_fw 103 | 104 | . = ALIGN(4); 105 | _etext = .; 106 | _textdata = _etext; 107 | 108 | .stacks : 109 | { 110 | . = ALIGN(8); 111 | __main_stack_base__ = .; 112 | . += __main_stack_size__; 113 | . = ALIGN(8); 114 | __main_stack_end__ = .; 115 | __process_stack_base__ = .; 116 | __main_thread_stack_base__ = .; 117 | . += __process_stack_size__; 118 | . = ALIGN(8); 119 | __process_stack_end__ = .; 120 | __main_thread_stack_end__ = .; 121 | } > ram 122 | 123 | .data : 124 | { 125 | . = ALIGN(4); 126 | PROVIDE(_data = .); 127 | *(.data) 128 | . = ALIGN(4); 129 | *(.data.*) 130 | . = ALIGN(4); 131 | *(.ramtext) 132 | . = ALIGN(4); 133 | PROVIDE(_edata = .); 134 | } > ram AT > flash_fw 135 | 136 | .bss : 137 | { 138 | . = ALIGN(4); 139 | PROVIDE(_bss_start = .); 140 | *(.bss) 141 | . = ALIGN(4); 142 | *(.bss.*) 143 | . = ALIGN(4); 144 | *(COMMON) 145 | . = ALIGN(4); 146 | PROVIDE(_bss_end = .); 147 | } > ram 148 | } 149 | 150 | PROVIDE(end = .); 151 | _end = .; 152 | 153 | __heap_base__ = _end; 154 | __heap_end__ = __ram_end__; 155 | -------------------------------------------------------------------------------- /firmware/proto/BUILD.bazel: -------------------------------------------------------------------------------- 1 | load("@rules_proto//proto:defs.bzl", "proto_library") 2 | 3 | proto_library( 4 | name = "messages_proto", 5 | srcs = ["messages.proto"], 6 | visibility = ["//visibility:public"], 7 | deps = ["@com_github_nanopb_nanopb//:nanopb_proto"], 8 | ) 9 | -------------------------------------------------------------------------------- /firmware/proto/messages.proto: -------------------------------------------------------------------------------- 1 | syntax = "proto3"; 2 | import "nanopb.proto"; 3 | 4 | message motor_calibration_t { 5 | // Encoder reading at the start of an electrical revolution. 6 | uint32 erev_start = 1; 7 | // Electrical revolutions per mechanical revolution. 8 | uint32 erevs_per_mrev = 2; 9 | // Phases A, B, C are arranged in clockwise instead of ccw order. 10 | bool flip_phases = 3; 11 | // Proportional gain for FOC/d PI loop. 12 | float foc_kp_d = 4; 13 | // Integral gain for FOC/d PI loop. 14 | float foc_ki_d = 5; 15 | // Proportional gain for FOC/q PI loop. 16 | float foc_kp_q = 6; 17 | // Integral gain for FOC/q PI loop. 18 | float foc_ki_q = 7; 19 | // Proportional gain for velocity PI loop. 20 | float velocity_kp = 8; 21 | // Integral gain for velocity PI loop. 22 | float velocity_kd = 9; 23 | // Proportional gain for position PI loop. 24 | float position_kp = 10; 25 | // Integral gain for position PI loop. 26 | float position_kd = 11; 27 | // Current limit (A). 28 | float current_limit = 12; 29 | // Torque limit (N*m). 30 | float torque_limit = 13; 31 | // Velocity limit (rad/s). 32 | float velocity_limit = 14; 33 | // Position lower limit (rad). 34 | float position_lower_limit = 15; 35 | // Position upper limit (rad). 36 | float position_upper_limit = 16; 37 | // Motor resistance (ohm). 38 | float motor_resistance = 17; 39 | // Motor inductance (henries). 40 | float motor_inductance = 18; 41 | // Motor torque constant (newton-meters per ampere). 42 | float motor_torque_const = 19; 43 | // Control timeout (ms). 44 | uint32 control_timeout = 20; 45 | // Parameter for high frequency velocity estimate. 46 | float hf_velocity_filter_param = 21; 47 | // Parameter for low frequency velocity estimate. 48 | float lf_velocity_filter_param = 22; 49 | // Position offset. 50 | float position_offset = 23; 51 | // Current Offset for Phase A. 52 | float ia_offset = 24; 53 | // Current Offset for Phase B. 54 | float ib_offset = 25; 55 | // Current Offset for Phase C. 56 | float ic_offset = 26; 57 | // Encoder angle correction scale (rad). 58 | float enc_ang_corr_scale = 27; 59 | // Encoder angle correction offset (rad). 60 | float enc_ang_corr_offset = 28; 61 | // Encoder angle correction table values. 62 | bytes enc_ang_corr_table_values = 29 [ (nanopb).max_size = 257 ]; 63 | } 64 | -------------------------------------------------------------------------------- /firmware/src/LUTFunction.cpp: -------------------------------------------------------------------------------- 1 | #include "LUTFunction.hpp" 2 | 3 | #include 4 | 5 | #include 6 | 7 | namespace motor_driver { 8 | namespace math { 9 | 10 | template float LUTFunction::lookup(float arg) const { 11 | float norm_arg = (arg - x_first_) / (x_last_ - x_first_); 12 | float norm_arg_integral = std::floor(norm_arg); 13 | float norm_arg_fraction = norm_arg - norm_arg_integral; 14 | size_t flip_index = 15 | (static_cast(norm_arg_integral) % periodicity_.repetition_count + 16 | periodicity_.repetition_count) % 17 | periodicity_.repetition_count; 18 | 19 | switch (periodicity_.repetition_flips[flip_index]) { 20 | case LFFlipType::NONE: 21 | default: 22 | return lookupReduced(norm_arg_fraction); 23 | case LFFlipType::HORIZONTAL: 24 | return lookupReduced(1.0f - norm_arg_fraction); 25 | case LFFlipType::VERTICAL: 26 | return -lookupReduced(norm_arg_fraction); 27 | case LFFlipType::BOTH: 28 | return -lookupReduced(1.0f - norm_arg_fraction); 29 | } 30 | } 31 | 32 | template 33 | float LUTFunction::lookupReduced(float reduced_arg) const { 34 | if (reduced_arg <= 0.0f) { 35 | return y_[0]; 36 | } else if (reduced_arg >= 1.0f) { 37 | return y_[y_len_ - 1]; 38 | } else { 39 | float integral = std::floor(reduced_arg * (y_len_ - 1)); 40 | float fraction = reduced_arg * (y_len_ - 1) - integral; 41 | size_t index_before = (size_t)integral; 42 | // Linear interpolation. 43 | return (y_[index_before] * (1.0f - fraction) + 44 | y_[index_before + 1] * fraction); 45 | } 46 | } 47 | 48 | template class LUTFunction; 49 | template class LUTFunction; 50 | 51 | } // namespace math 52 | } // namespace motor_driver 53 | -------------------------------------------------------------------------------- /firmware/src/Recorder.cpp: -------------------------------------------------------------------------------- 1 | #include "Recorder.hpp" 2 | 3 | namespace motor_driver { 4 | namespace state { 5 | 6 | bool Recorder::startRecording() { 7 | // TODO(gbalke): start another record as soon as it is over, if already 8 | // recording. 9 | if (state_ == State::READY) { 10 | state_ = State::RECORDING; 11 | return true; 12 | } 13 | return false; 14 | } 15 | 16 | void Recorder::recordSample(float *recorder_new_data) { 17 | if (state_ == State::RECORDING) { 18 | for (size_t j = 0; j < consts::recorder_channel_count; j++) { 19 | record_buf_[index_ + j] = recorder_new_data[j]; 20 | } 21 | index_ += consts::recorder_channel_count; 22 | if (index_ >= 23 | consts::recorder_max_samples * consts::recorder_channel_count) { 24 | state_ = State::FINISHED; 25 | } 26 | } 27 | } 28 | 29 | float *Recorder::read() { 30 | if (state_ == State::FINISHED) { 31 | return record_buf_; 32 | } else { 33 | return nullptr; 34 | } 35 | } 36 | 37 | void Recorder::reset() { 38 | index_ = 0; 39 | state_ = State::READY; 40 | } 41 | 42 | uint16_t Recorder::size() { 43 | if (state_ == State::FINISHED) { 44 | return consts::recorder_max_samples * consts::recorder_channel_count; 45 | } else { 46 | return 0; 47 | } 48 | } 49 | 50 | } // namespace state 51 | } // namespace motor_driver 52 | -------------------------------------------------------------------------------- /firmware/src/SVM.cpp: -------------------------------------------------------------------------------- 1 | #include "SVM.hpp" 2 | 3 | #include 4 | 5 | #include 6 | 7 | #include "constants.hpp" 8 | 9 | namespace motor_driver { 10 | namespace controller { 11 | 12 | void SVM::computeDutyCycles(float v_alpha, float v_beta, float &dc_a, 13 | float &dc_b, float &dc_c) { 14 | dc_a = (v_alpha)*consts::one_div_sqrt3 + 0.5f; 15 | dc_b = ((-0.5f * v_alpha + consts::sqrt3_div_2 * v_beta) * 16 | consts::one_div_sqrt3 + 17 | 0.5f); 18 | dc_c = ((-0.5f * v_alpha - consts::sqrt3_div_2 * v_beta) * 19 | consts::one_div_sqrt3 + 20 | 0.5f); 21 | 22 | switch (strategy_) { 23 | case SVMStrategy::SINUSOIDAL: 24 | default: 25 | /* 26 | * Sinusoidal commutation. 27 | */ 28 | 29 | // Duty cycles are already in the right form. 30 | 31 | break; 32 | 33 | case SVMStrategy::TOP_BOTTOM_CLAMP: 34 | /* 35 | * Top and bottom clamping ZSM. 36 | */ 37 | 38 | { 39 | float top_shift = 1.0f - std::max({dc_a, dc_b, dc_c}); 40 | float bottom_shift = std::min({dc_a, dc_b, dc_c}); 41 | 42 | if (top_shift < bottom_shift) { 43 | dc_a += top_shift; 44 | dc_b += top_shift; 45 | dc_c += top_shift; 46 | } else { 47 | dc_a -= bottom_shift; 48 | dc_b -= bottom_shift; 49 | dc_c -= bottom_shift; 50 | } 51 | } 52 | 53 | break; 54 | 55 | case SVMStrategy::MIDPOINT_CLAMP: 56 | /* 57 | * Midpoint clamping ZSM 58 | */ 59 | 60 | { 61 | float shift = (0.5f * (1.0f - std::min({dc_a, dc_b, dc_c}) - 62 | std::max({dc_a, dc_b, dc_c}))); 63 | 64 | dc_a += shift; 65 | dc_b += shift; 66 | dc_c += shift; 67 | } 68 | 69 | break; 70 | } 71 | } 72 | 73 | float SVM::getMaxAmplitude() const { 74 | switch (strategy_) { 75 | case SVMStrategy::SINUSOIDAL: 76 | default: 77 | 78 | return consts::sqrt3_div_2; 79 | 80 | case SVMStrategy::TOP_BOTTOM_CLAMP: 81 | case SVMStrategy::MIDPOINT_CLAMP: 82 | 83 | return 1.0f; 84 | } 85 | } 86 | 87 | } // namespace controller 88 | } // namespace motor_driver 89 | -------------------------------------------------------------------------------- /firmware/src/crc_mlx.c: -------------------------------------------------------------------------------- 1 | /** 2 | * \file 3 | * Functions and types for CRC checks. 4 | * 5 | * Generated on Thu Jun 21 19:30:03 2018 6 | * by pycrc v0.9.1, https://pycrc.org 7 | * using the configuration: 8 | * - Width = 8 9 | * - Poly = 0x2f 10 | * - XorIn = 0xff 11 | * - ReflectIn = False 12 | * - XorOut = 0xff 13 | * - ReflectOut = False 14 | * - Algorithm = table-driven 15 | */ 16 | #include "crc_mlx.h" /* include the header file generated with pycrc */ 17 | 18 | #include 19 | #include 20 | 21 | /** 22 | * Static table used for the table_driven implementation. 23 | */ 24 | static const crc_mlx_t crc_table[256] = { 25 | 0x00, 0x2f, 0x5e, 0x71, 0xbc, 0x93, 0xe2, 0xcd, 0x57, 0x78, 0x09, 0x26, 26 | 0xeb, 0xc4, 0xb5, 0x9a, 0xae, 0x81, 0xf0, 0xdf, 0x12, 0x3d, 0x4c, 0x63, 27 | 0xf9, 0xd6, 0xa7, 0x88, 0x45, 0x6a, 0x1b, 0x34, 0x73, 0x5c, 0x2d, 0x02, 28 | 0xcf, 0xe0, 0x91, 0xbe, 0x24, 0x0b, 0x7a, 0x55, 0x98, 0xb7, 0xc6, 0xe9, 29 | 0xdd, 0xf2, 0x83, 0xac, 0x61, 0x4e, 0x3f, 0x10, 0x8a, 0xa5, 0xd4, 0xfb, 30 | 0x36, 0x19, 0x68, 0x47, 0xe6, 0xc9, 0xb8, 0x97, 0x5a, 0x75, 0x04, 0x2b, 31 | 0xb1, 0x9e, 0xef, 0xc0, 0x0d, 0x22, 0x53, 0x7c, 0x48, 0x67, 0x16, 0x39, 32 | 0xf4, 0xdb, 0xaa, 0x85, 0x1f, 0x30, 0x41, 0x6e, 0xa3, 0x8c, 0xfd, 0xd2, 33 | 0x95, 0xba, 0xcb, 0xe4, 0x29, 0x06, 0x77, 0x58, 0xc2, 0xed, 0x9c, 0xb3, 34 | 0x7e, 0x51, 0x20, 0x0f, 0x3b, 0x14, 0x65, 0x4a, 0x87, 0xa8, 0xd9, 0xf6, 35 | 0x6c, 0x43, 0x32, 0x1d, 0xd0, 0xff, 0x8e, 0xa1, 0xe3, 0xcc, 0xbd, 0x92, 36 | 0x5f, 0x70, 0x01, 0x2e, 0xb4, 0x9b, 0xea, 0xc5, 0x08, 0x27, 0x56, 0x79, 37 | 0x4d, 0x62, 0x13, 0x3c, 0xf1, 0xde, 0xaf, 0x80, 0x1a, 0x35, 0x44, 0x6b, 38 | 0xa6, 0x89, 0xf8, 0xd7, 0x90, 0xbf, 0xce, 0xe1, 0x2c, 0x03, 0x72, 0x5d, 39 | 0xc7, 0xe8, 0x99, 0xb6, 0x7b, 0x54, 0x25, 0x0a, 0x3e, 0x11, 0x60, 0x4f, 40 | 0x82, 0xad, 0xdc, 0xf3, 0x69, 0x46, 0x37, 0x18, 0xd5, 0xfa, 0x8b, 0xa4, 41 | 0x05, 0x2a, 0x5b, 0x74, 0xb9, 0x96, 0xe7, 0xc8, 0x52, 0x7d, 0x0c, 0x23, 42 | 0xee, 0xc1, 0xb0, 0x9f, 0xab, 0x84, 0xf5, 0xda, 0x17, 0x38, 0x49, 0x66, 43 | 0xfc, 0xd3, 0xa2, 0x8d, 0x40, 0x6f, 0x1e, 0x31, 0x76, 0x59, 0x28, 0x07, 44 | 0xca, 0xe5, 0x94, 0xbb, 0x21, 0x0e, 0x7f, 0x50, 0x9d, 0xb2, 0xc3, 0xec, 45 | 0xd8, 0xf7, 0x86, 0xa9, 0x64, 0x4b, 0x3a, 0x15, 0x8f, 0xa0, 0xd1, 0xfe, 46 | 0x33, 0x1c, 0x6d, 0x42}; 47 | 48 | crc_mlx_t crc_mlx_update(crc_mlx_t crc, const void *data, size_t data_len) { 49 | const unsigned char *d = (const unsigned char *)data; 50 | unsigned int tbl_idx; 51 | 52 | while (data_len--) { 53 | tbl_idx = crc ^ *d; 54 | crc = crc_table[tbl_idx] & 0xff; 55 | d++; 56 | } 57 | return crc & 0xff; 58 | } 59 | -------------------------------------------------------------------------------- /firmware/src/old_drivers/CPPLINT.cfg: -------------------------------------------------------------------------------- 1 | exclude_files=.*\.cpp 2 | -------------------------------------------------------------------------------- /firmware/src/pid.cpp: -------------------------------------------------------------------------------- 1 | #include "pid.hpp" 2 | 3 | namespace motor_driver { 4 | namespace controller { 5 | 6 | // Initialization and configuration functions. 7 | PID::PID(float kp, float ki, float kd, float interval) { 8 | interval_ = interval; 9 | kp_ = kp, ki_ = ki / interval, kd_ = kd; 10 | min_ = 0.0f; 11 | max_ = 0.0f; 12 | 13 | target_ = 0.0f; 14 | 15 | windup_ = 0.0f; 16 | 17 | alpha_ = 0.0f; 18 | deriv_ = 0.0f; 19 | val_prev_ = 0.0f; 20 | } 21 | 22 | void PID::setGains(float kp, float ki, float kd) { 23 | kp_ = kp; 24 | ki_ = ki; 25 | kd_ = kd; 26 | } 27 | 28 | void PID::setLimits(float min, float max) { 29 | min_ = min; 30 | max_ = max; 31 | target_ = (max + min) / 2.0f; 32 | } 33 | 34 | void PID::setTarget(float target) { target_ = target; } 35 | 36 | void PID::setAlpha(float alpha) { alpha_ = alpha; } 37 | // End of Initialization and Config Functions. 38 | 39 | // Compute code for PID. 40 | // Windup limited by proportional term exceeding bounds! 41 | float PID::computeIntegral(float err) { 42 | windup_ += err; 43 | 44 | if (windup_ * ki_ > max_) { 45 | windup_ = max_ / ki_; 46 | } else if (windup_ * ki_ < min_) { 47 | windup_ = min_ / ki_; 48 | } 49 | 50 | return windup_; 51 | } 52 | 53 | // Use alpha to approximate derivative over time (remove HF noise in err). 54 | float PID::computeDerivative(float val) { 55 | float delta = (-val) - val_prev_; 56 | deriv_ = (alpha_ * delta) + ((1 - alpha_) * deriv_); 57 | val_prev_ = -val; 58 | return deriv_; 59 | } 60 | 61 | float PID::compute(float val) { 62 | float err = target_ - val; 63 | float p, i, d; 64 | 65 | p = kp_ * err; 66 | i = ki_ * computeIntegral(err); 67 | d = kd_ * computeDerivative(val); 68 | 69 | float output = p + i + d; 70 | if (output > max_) { 71 | output = max_; 72 | } else if (output < min_) { 73 | output = min_; 74 | } 75 | 76 | return output; 77 | } 78 | // End of PID Compute. 79 | 80 | } // namespace controller 81 | } // namespace motor_driver 82 | -------------------------------------------------------------------------------- /firmware/src/transforms.cpp: -------------------------------------------------------------------------------- 1 | #include "transforms.hpp" 2 | 3 | #include 4 | 5 | #include "constants.hpp" 6 | #include "fast_math.hpp" 7 | 8 | namespace motor_driver { 9 | namespace math { 10 | 11 | void transformPark(float alpha, float beta, float cos_theta, float sin_theta, 12 | float &d, float &q) { 13 | d = cos_theta * alpha + sin_theta * beta; 14 | q = -sin_theta * alpha + cos_theta * beta; 15 | } 16 | 17 | void transformInversePark(float d, float q, float cos_theta, float sin_theta, 18 | float &alpha, float &beta) { 19 | alpha = cos_theta * d - sin_theta * q; 20 | beta = sin_theta * d + cos_theta * q; 21 | } 22 | 23 | void transformClarke(float a, float b, float c, float &alpha, float &beta) { 24 | alpha = (2.0f * a - b - c) / 3.0f; 25 | beta = consts::one_div_sqrt3 * (b - c); 26 | } 27 | 28 | void transformInverseClarke(float alpha, float beta, float &a, float &b, 29 | float &c) { 30 | a = alpha; 31 | b = -alpha / 2.0f + (consts::sqrt3_div_2 * beta); 32 | c = -alpha / 2.0f - (consts::sqrt3_div_2 * beta); 33 | // Multiply by 1/sqrt(3) because phase voltages have amplitude 1/sqrt(3) 34 | // of bus voltage. 35 | a *= consts::one_div_sqrt3; 36 | b *= consts::one_div_sqrt3; 37 | c *= consts::one_div_sqrt3; 38 | } 39 | 40 | } // namespace math 41 | } // namespace motor_driver 42 | -------------------------------------------------------------------------------- /pyproject.toml: -------------------------------------------------------------------------------- 1 | [tool.black] 2 | line-length = 79 3 | 4 | [tool.isort] 5 | profile = "black" 6 | 7 | [tool.flake8] 8 | exclude = """ 9 | bd_tools/src/bd_tools/recorder, 10 | bd_tools/src/bd_tools/dev, 11 | bd_tools/bd_tools.egg-info, 12 | bd_tools/build, 13 | """ 14 | extend-ignore = "E203," 15 | -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | # Tool requirements. 2 | crcmod 3 | matplotlib 4 | numpy 5 | scipy 6 | pyserial 7 | 8 | # Testing requirements. 9 | pytest 10 | pytest-mock 11 | 12 | # Linting and formatting requirements. 13 | click == 7.1.2 14 | black == 21.5b1 15 | isort == 5.8.0 16 | flake8 == 3.9.2 17 | flake8-black == 0.2.1 18 | pyproject-flake8 == 0.0.1a2 19 | cpplint <= 1.5.4 20 | -------------------------------------------------------------------------------- /tests/tools/integration/imu_failure.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import argparse 3 | import ast 4 | import struct 5 | import time 6 | 7 | import numpy as np 8 | import serial 9 | 10 | from bd_tools import boards, comms 11 | 12 | GRAVITY = 9.81 13 | 14 | if __name__ == "__main__": 15 | parser = argparse.ArgumentParser( 16 | description="Drive motor module(s) with a given control mode." 17 | ) 18 | parser.add_argument("serial", type=str, help="Serial port") 19 | parser.add_argument("--baud_rate", type=int, help="Serial baud rate") 20 | parser.add_argument( 21 | "board_ids", type=str, help="Board ID (separate with comma)" 22 | ) 23 | parser.add_argument( 24 | "mode", 25 | type=str, 26 | help="Control mode: \ 27 | current (Id[A], Iq[A]), \ 28 | phase (dc,dc,dc), \ 29 | torque (N*m), \ 30 | velocity (rad/s), \ 31 | position (rad), \ 32 | pos_vel (rad,rad/s), \ 33 | pos_ff (rad,ff[A]), \ 34 | pwm (dc)", 35 | ) 36 | parser.add_argument( 37 | "actuations", 38 | type=str, 39 | help="Actuation amount in the units of the selected mode (if requires " 40 | "multiple args, separate by comma)", 41 | ) 42 | parser.set_defaults( 43 | baud_rate=comms.COMM_DEFAULT_BAUD_RATE, 44 | offset=comms.COMM_BOOTLOADER_OFFSET, 45 | ) 46 | args = parser.parse_args() 47 | 48 | def make_list(x): 49 | return list(x) if (type(x) == list or type(x) == tuple) else [x] 50 | 51 | def make_type(x, to_type): 52 | return [to_type(y) for y in x] 53 | 54 | board_ids = make_type(make_list(ast.literal_eval(args.board_ids)), int) 55 | actuations = make_list(ast.literal_eval(args.actuations)) 56 | 57 | mode = args.mode 58 | 59 | ser = serial.Serial(port=args.serial, baudrate=args.baud_rate, timeout=0.1) 60 | 61 | client = comms.BLDCControllerClient(ser) 62 | initialized = boards.initBoards(client, board_ids) 63 | 64 | client.leaveBootloader(board_ids) 65 | 66 | client.resetInputBuffer() 67 | 68 | boards.initMotor(client, board_ids) 69 | 70 | start_time = time.time() 71 | count = 0 72 | rollover = 1000 73 | 74 | def getIMU(bids): 75 | return client.readRegisters( 76 | bids, [comms.COMM_ROR_ACC_X] * len(bids), [3] * len(bids) 77 | ) 78 | 79 | while True: 80 | # If there's a watchdog reset, clear the reset and perform any 81 | # configuration again 82 | crashed = client.checkWDGRST() 83 | if crashed != []: 84 | try: 85 | client.clearWDGRST(crashed) 86 | except (comms.MalformedPacketError, comms.ProtocolError): 87 | pass 88 | 89 | try: 90 | boards.driveMotor(client, board_ids, actuations, mode) 91 | responses = getIMU(board_ids) 92 | for i in range(len(responses)): 93 | val = struct.unpack("VAL until 19 | // UptimeMillis doesn't rollover. 20 | do { 21 | ms = msecISR(); 22 | st = SysTick->VAL; 23 | } while (ms != msecISR()); 24 | // NOTE(greg): The original implementation uses 32 bit values. This 25 | // implementation prevents the discarding of the upper ~10 bits when 26 | // multiplying ms by 1000.. 27 | return ms * 1000 - st / ((SysTick->LOAD + 1) / 1000); 28 | } 29 | 30 | inline float usecToSeconds(uint64_t usecs) { 31 | return (float)usecs / 1000000.0f; 32 | } 33 | } // namespace time 34 | --------------------------------------------------------------------------------