Mirrors_Framework/ODrive
1
0
Fork 0
mirror of https://github.com/odriverobotics/ODrive.git synced 2026-09-21 15:34:33 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

Merge branch 'devel' into ascii_cleanup

Browse Source
This commit is contained in:
Samuel Sadok
2020-08-05 19:08:23 +02:00
parent b56f7c1ff2 30bc561231
commit aaab9490df
158 changed files with 24717 additions and 3043 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.github/workflows/compile.yaml
+117
View File
@@ -0,0 +1,117 @@
name: Tests
on:
pull_request:
branches: [master, devel]
tags: ['fw-v*']
push:
branches: [master, devel]
tags: ['fw-v*']
jobs:
compile:
strategy:
fail-fast: false
matrix:
os: [ubuntu-16.04, ubuntu-latest, windows-latest, macOS-latest]
board_version: [v3.6-56V]
debug: [true]
include:
- {os: ubuntu-latest, board_version: v3.2, debug: false}
- {os: ubuntu-latest, board_version: v3.3, debug: false}
- {os: ubuntu-latest, board_version: v3.4-24V, debug: false}
- {os: ubuntu-latest, board_version: v3.4-48V, debug: false}
- {os: ubuntu-latest, board_version: v3.5-24V, debug: false}
- {os: ubuntu-latest, board_version: v3.5-48V, debug: false}
- {os: ubuntu-latest, board_version: v3.6-24V, debug: false}
- {os: ubuntu-latest, board_version: v3.6-56V, debug: false}
runs-on: ${{ matrix.os }}
steps:
- uses: actions/checkout@v2
- name: Install prerequisites (Debian)
if: startsWith(matrix.os, 'ubuntu-')
run: |
DEBIAN_VERSION="$(lsb_release --release --short)"
echo Debian version: $DEBIAN_VERSION
if [ "$DEBIAN_VERSION" -gt 9 ]; then
sudo apt-get install gcc-arm-none-eabi
else
# Ubuntu 16.04 (Debian 9) is on ARM GCC 4.9 which is too old for us
sudo add-apt-repository ppa:team-gcc-arm-embedded/ppa
sudo apt-get update
sudo apt-get install gcc-arm-embedded
fi
if ! (apt-cache search tup | grep "^tup - "); then
sudo add-apt-repository ppa:jonathonf/tup
sudo apt-get update
fi
sudo apt-get install tup
sudo apt install python3 python3-yaml python3-jinja2 python3-jsonschema
- name: Install prerequisites (macOS)
if: startsWith(matrix.os, 'macOS-')
run: |
brew install armmbed/formulae/arm-none-eabi-gcc
brew cask install osxfuse && brew install tup
pip3 install PyYAML Jinja2 jsonschema
- name: Cache chocolatey
uses: actions/cache@v2
if: startsWith(matrix.os, 'windows-')
with:
path: C:\Users\runneradmin\AppData\Local\Temp\chocolatey\gcc-arm-embedded
key: ${{ runner.os }}-gcc-arm-embedded
restore-keys: |
${{ runner.os }}-gcc-arm-embedded
- name: Install prerequisites (Windows)
if: startsWith(matrix.os, 'windows-')
run: |
Invoke-WebRequest -Uri "http://gittup.org/tup/win32/tup-latest.zip" -OutFile ".\tup-latest.zip"
Expand-Archive ".\tup-latest.zip" -DestinationPath ".\tup-latest" -Force
echo "::add-path::$(Resolve-Path .)\tup-latest"
choco install gcc-arm-embedded # downloads https://developer.arm.com/-/media/Files/downloads/gnu-rm/9-2019q4/gcc-arm-none-eabi-9-2019-q4-major-win32.zip
pip install PyYAML Jinja2 jsonschema
- name: Prepare Compilation
run: |
# for debugging
arm-none-eabi-gcc --version
python --version
cd ${{ github.workspace }}/Firmware
echo "CONFIG_BOARD_VERSION=${{ matrix.board_version }}" >> tup.config
echo "CONFIG_STRICT=true" >> tup.config
echo "CONFIG_DEBUG=${{ matrix.release }}" >> tup.config
tup init
tup generate ./tup_build.sh
- name: Compile (Unix)
if: "!startsWith(matrix.os, 'windows-')"
run: |
cd ${{ github.workspace }}/Firmware
bash -xe ./tup_build.sh
- name: Compile (Windows)
if: startsWith(matrix.os, 'windows-')
run: |
cd ${{ github.workspace }}/Firmware
mv tup_build.sh tup_build.bat # in reality this is a .bat script on windows
.\tup_build.bat
#code-checks:
# runs-on: ubuntu-latest
# steps:
# TODO:
# - check if enums.py is consistent with yaml
# - clang-format check
# - check if interface_generator outputs the same thing with Python 3.5 and Python 3.8
.github/workflows/documentation.yml
+67
View File
@@ -0,0 +1,67 @@
name: Build and publish HTML documentation website
on:
push:
branches: [ feature/doc_autogen ]
jobs:
jekyll:
runs-on: ubuntu-16.04
steps:
- uses: actions/checkout@v2
- name: Setup Python
uses: actions/setup-python@v2
with:
python-version: '3.x'
# Use GitHub Actions' cache for ruby and python packages to shorten build times and decrease load on servers
- name: Cache gems
uses: actions/cache@v2
with:
path: docs/vendor/bundle
key: ${{ runner.os }}-gems-${{ hashFiles('docs/Gemfile.lock') }}
restore-keys: |
${{ runner.os }}-gems-
- name: Cache pip
uses: actions/cache@v2
with:
path: ~/.cache/pip
key: ${{ runner.os }}-pip-PyYAML-Jinja2-jsonschema
restore-keys: |
${{ runner.os }}-pip-
${{ runner.os }}-
- name: Install Python dependencies
run: pip install PyYAML Jinja2 jsonschema
# Autogenerate the API reference .md files in the python in the python/python3 container
- name: Autogenerate the API reference .md files in the python container
run: |
mkdir -p docs/_api docs/_includes
python Firmware/interface_generator_stub.py --definitions Firmware/odrive-interface.yaml --template docs/_layouts/api_documentation_template.j2 --outputs docs/_api/#.md
python Firmware/interface_generator_stub.py --definitions Firmware/odrive-interface.yaml --template docs/_layouts/api_index_template.j2 --output docs/_includes/apiindex.html
- name: Build the site in the jekyll/builder container
run: |
docker run \
-v ${{ github.workspace }}:/srv/jekyll -e PAGES_REPO_NWO=${GITHUB_REPOSITORY} \
ruby:2.7-buster /bin/sh -c "
chmod 777 /srv/jekyll/docs && \
cd /srv/jekyll/docs && \
bundle config path vendor/bundle && \
bundle install && \
JEKYLL_ENV=production bundle exec jekyll build
"
touch .nojekyll
- name: Push to documentation branch
run: |
git config user.name "${GITHUB_ACTOR}"
git config user.email "${GITHUB_ACTOR}@users.noreply.github.com"
git add -f docs/_site
git commit -m "jekyll build from Action ${GITHUB_SHA}"
git push --force origin HEAD:${REMOTE_BRANCH}
env:
REMOTE_BRANCH: gh-pages
.github/workflows/nightly.yaml
+34
View File
@@ -0,0 +1,34 @@
name: pip install odrive (nightly)
on:
schedule:
- cron: '0 2 * * *' # run at 2 AM UTC
jobs:
nightly:
strategy:
fail-fast: false
matrix:
os: [ubuntu-latest, windows-latest, macOS-latest]
#pip: [pip2, pip3]
runs-on: ${{ matrix.os }}
steps:
- name: Install odrivetool
run: |
pip install monotonic # TODO: this is dishonest. Must be removed as soon as v0.5.0 is published!
pip install odrive
# This one currently fails because Github Actions runs pip as non-root
#- name: Check if udev rules were set up properly
# if: matrix.os == 'ubuntu-latest'
# run: test -f /etc/udev/rules.d/91-odrive.rules
# This step is mentioned in the user guide
- name: Add ~/.local/bin to path
if: matrix.os == 'ubuntu-latest'
run: echo "::add-path::~/.local/bin"
- name: Launch odrivetool
# This returns a non-zero exit code if the odrivetool throws an exception
run: echo 'quit()' | odrivetool shell
.gitignore
+26 -6
View File
@@ -24,9 +24,6 @@ coverage.xml
# Django stuff:
*.log
# Sphinx documentation
docs/_build/
# PyBuilder
target/
@@ -37,7 +34,30 @@ target/
.tup
tup.config
/ruby-bundle
/_site
/.bundle
# Sphinx documentation
/docs/_build/
# Autogenerated API reference
/docs/_api
/docs/_includes/apiindex.html
# Jekyll HTML documention and artifacts
/docs/ruby-bundle
/docs/_site
/docs/.bundle/config
/docs/.jekyll-metadata
*.exe
ODrive\.config
ODrive\.creator
ODrive\.creator\.user
ODrive\.files
ODrive\.includes
Firmware/Tests/bin/
.travis.yml
-70
View File
@@ -1,70 +0,0 @@
# adapted from https://github.com/andysworkshop/stm32plus/blob/master/.travis.yml
branches:
only:
- master
- devel
- /^fw-v/
language: c
sudo: false
addons:
apt:
packages:
libc6-i386
cache:
directories:
- "$HOME/dl"
install:
# - export GCC_DIR=$HOME/dl/gcc-arm-none-eabi-5_2-2015q4
# - export GCC_ARCHIVE=$HOME/dl/gcc-arm-none-eabi-5_2-2015q4-20151219-linux.tar.bz2
# - export GCC_URL=https://launchpad.net/gcc-arm-embedded/5.0/5-2015-q4-major/+download/gcc-arm-none-eabi-5_2-2015q4-20151219-linux.tar.bz2
# - if [ ! -e $GCC_DIR/bin/arm-none-eabi-gcc ]; then wget $GCC_URL -O $GCC_ARCHIVE; tar xfj $GCC_ARCHIVE -C $HOME/dl; fi
# - export PATH=$PATH:$GCC_DIR/bin
- export GCC_DIR=$HOME/dl/gcc-arm-none-eabi-7-2017-q4-major
- export GCC_ARCHIVE=$HOME/dl/gcc-arm-none-eabi-7-2017-q4-major-linux.tar.bz2
- export GCC_URL=https://developer.arm.com/-/media/Files/downloads/gnu-rm/7-2017q4/gcc-arm-none-eabi-7-2017-q4-major-linux.tar.bz2
- if [ ! -e $GCC_DIR/bin/arm-none-eabi-gcc ]; then wget $GCC_URL -O $GCC_ARCHIVE; tar xfj $GCC_ARCHIVE -C $HOME/dl; fi
- export PATH=$PATH:$GCC_DIR/bin
- export TUP_VER=tup_0.7.8-3~16.04.york0_amd64
- export TUP_DIR=$HOME/dl/$TUP_VER
- export TUP_ARCHIVE=$HOME/dl/$TUP_VER.deb
- export TUP_URL=http://ppa.launchpad.net/jonathonf/tup/ubuntu/pool/main/t/tup/$TUP_VER.deb
- if [ ! -e $TUP_DIR/bin/tup ]; then wget $TUP_URL -O $TUP_ARCHIVE; dpkg-deb -R $TUP_ARCHIVE $TUP_DIR; fi
- export PATH=$PATH:$TUP_DIR/usr/bin
env:
# Build default configuration for each board
- CONFIG_BOARD_VERSION=v3.2 DEPLOY=v3.2
- CONFIG_BOARD_VERSION=v3.3 DEPLOY=v3.3
- CONFIG_BOARD_VERSION=v3.4-24V DEPLOY=v3.4-24V
- CONFIG_BOARD_VERSION=v3.4-48V DEPLOY=v3.4-48V
- CONFIG_BOARD_VERSION=v3.5-24V DEPLOY=v3.5-24V
- CONFIG_BOARD_VERSION=v3.5-48V DEPLOY=v3.5-48V
- CONFIG_BOARD_VERSION=v3.6-24V DEPLOY=v3.6-24V
- CONFIG_BOARD_VERSION=v3.6-56V DEPLOY=v3.6-56V
# Various protocol combinations
#- CONFIG_BOARD_VERSION=v3.4-24V CONFIG_USB_PROTOCOL=native-stream CONFIG_UART_PROTOCOL=native
#- CONFIG_BOARD_VERSION=v3.4-24V CONFIG_USB_PROTOCOL=stdout CONFIG_UART_PROTOCOL=stdout
#- CONFIG_BOARD_VERSION=v3.4-24V CONFIG_USB_PROTOCOL=none CONFIG_UART_PROTOCOL=none
script:
- "./Firmware/build.sh"
deploy:
provider: releases
api_key:
secure: RM66joGTn11Z5PmG7Nlj8cRcVY0w7ga5qUl2ahinbDm6jMV8OxroaXKivEa478alx9fygMHVeKdJZUlrAkRJ5OYJ1trfpW+43S3OYEnfy1nyXEXRwhgeIlb9LqdrumXVAp7TZ0Vppfom8A2ZWbxKxW3lG/EAmA4G9fnxHf0S9rF0y95YVfGrxdapTKcxvbP7Yojo53474ZI6+VYrqx8lq0JAnn4FwNT9ZJ1QASrmIw4w08f60XXv25BzndCTscvLb2qUu0AaGLbQUosde0Bb7P+aQsBVY6uSkg9MWV8gWPQjtO3u5IRR1bTshxf2kPqtzwK+SpcYrddoGN6BkKAB3lVorJIW5VguUkRmtPZ1K9+NhIztNevB2qr0ASutumNLF3aqMt19KL3A+SRx6froj5VhRHf4i/Xjm3SDLTaTcc8ZIh2PEE6scUMUMs5Mzu8LQWjInRe25MSb+pQB1mNOHmoFBtVb0J3u7Nvs8jdImN5gQWvvowWXfXRNE0ncT1YsLmevwi3q+YdEjpAIPnrD/rouY8WaqQZ/vE15JM9uwdQRqKAbzGtMaKHDk7EZ7ANTyaP+UrQ/M5cVDa0bWsWSvqSqDJMy4IVHRlirYA/5u74lXNhmA8DGDB/gFVlVCmoEzas/pnYiAE1hh4RpsYxts78Ix+wbeo1hmt7t65X8cyo=
skip_cleanup: true
file_glob: true
file: Firmware/deploy/*
on:
repo: madcowswe/ODrive
branch: master
tags: true
Arduino/ODriveArduino/ODriveArduino.cpp
+1 -1
View File
@@ -66,7 +66,7 @@ bool ODriveArduino::run_state(int axis, int requested_state, bool wait) {
do {
delay(100);
serial_ << "r axis" << axis << ".current_state\n";
} while (readInt() != AXIS_STATE_IDLE && --timeout_ctr > 0);
} while (readInt() != requested_state && --timeout_ctr > 0);
}
return timeout_ctr > 0;
CHANGELOG.md
+65
View File
@@ -1,7 +1,68 @@
# Unreleased Features
Please add a note of your changes below this heading if you make a Pull Request.
# Release Candidate
## [0.5.1] - Date TBD
### Added
* Added motor `torque_constant`: units of torque are now [Nm] instead of just motor current.
* [Motor thermistors support](docs/thermistors.md)
* Enable/disable of thermistor thermal limits according `setting axis.<thermistor>.enabled`.
* Introduced `odrive-interface.yaml` as a root source for the ODrive's API. `odrivetool` connects much faster as a side effect.
* Added torque_constant and torque_lim to motor config
### Changed
* **`input_pos`, `input_vel`, `pos_estimate_linear`, `pos_estimate_circular`, are now in units of [turns] or [turns/s] instead of [counts] or [counts/s]**
* `axis.motor.thermal_current_lim` has been removed. Instead a new property is available `axis.motor.effective_current_lim` which contains the effective current limit including any thermal limits.
* `axis.motor.get_inverter_temp()`, `axis.motor.inverter_temp_limit_lower` and `axis.motor.inverter_temp_limit_upper` have been moved to seperate fet thermistor object under `axis.fet_thermistor`. `get_inverter_temp()` function has been renamed to `temp` and is now a read-only property.
# Releases
## [0.5.0] - 2020-08-03
### Added
* AC Induction Motor support.
* Tracking of rotor flux through rotor time constant
* Automatic d axis current for Maximum Torque Per Amp (MTPA)
* ASCII "w" commands now execute write hooks.
* Simplified control interface ("Input Filter" branch)
* New input variables: `input_pos`, `input_vel`, and `input_current`
* New setting `input_mode` to switch between different input behaviours
* Passthrough
* Velocity Ramp
* 2nd Order Position Filter
* Trapezoidal Trajectory Planner
* Removed `set_xxx_setpoint()` functions and made `xxx_setpoint` variables read-only
* [Preliminary support for Absolute Encoders](docs/encoders.md)
* [Preliminary support for endstops and homing](docs/endstops.md)
* [CAN Communication with CANSimple stack](can-protocol.md)
* Gain scheduling for anti-hunt when close to 0 position error
* Velocity Limiting in Current Control mode according to `vel_limit` and `vel_gain`
* Regen current limiting according to `max_regen_current`, in Amps
* DC Bus hard current limiting according to `dc_max_negative_current` and `dc_max_positive_current`
* Brake resistor logic now attempts to clamp voltage according to `odrv.config.dc_bus_overvoltage_ramp_start` and `odrv.config.dc_bus_overvoltage_ramp_end`
* Unit Testing with Doctest has been started for select algorithms, see [Firmware/Tests/test_runner.cpp](Firmware/Tests/test_runner.cpp)
* Added support for Flylint VSCode Extension for static code analysis
* Using an STM32F405 .svd file allows CortexDebug to view registers during debugging
* Added scripts for building via docker.
* Added ability to change uart baudrate via fibre
### Changed
* Changed ratiometric `motor.config.current_lim_tolerance` to absolute `motor.config.current_lim_margin`
* Moved `controller.vel_ramp_enable` to INPUT_MODE_VEL_RAMP.
* Anticogging map is temporarily forced to 0.1 deg precision, but saves with the config
* Some Encoder settings have been made read-only
* Cleaned up VSCode C/C++ Configuration settings on Windows with recursive includePath
* Now compiling with C++17
* Fixed a firmware hang that could occur from unlikely but possible user input
* Added JSON caching to Fibre. This drastically reduces the time odrivetool needs to connect to an ODrive (except for the first time or after firmware updates).
* Fix IPython `RuntimeWarning` that would occur every time `odrivetool` was started.
* Reboot on `erase_configuration()`. This avoids unexpected behavior of a subsequent `save_configuration()` call, since the configuration is only erased from NVM, not from RAM.
* Change `motor.get_inverter_temp()` to use a property which was already being sampled at `motor.inverter_temp`
* Fixed a numerical issue in the trajectory planner that could cause sudden jumps of the position setpoint
## [0.4.12] - 2020-05-06
### Fixed
* Fixed a numerical issue in the trajectory planner that could cause sudden jumps of the position setpoint
## [0.4.11] - 2019-07-25
### Added
* Separate lockin configs for sensorless, index search, and general.
@@ -26,6 +87,8 @@ Please add a note of your changes below this heading if you make a Pull Request.
* Script to enable using a hall signal as index edge.
### Changed
* Moved `traptraj.A_per_css` to `controller.inertia`
* Refactored velocity ramp mode into the new general input filtering structure
* Encoder index search now based on the new lock-in drive feature
### Fixed
@@ -101,6 +164,8 @@ Please add a note of your changes below this heading if you make a Pull Request.
## [0.4.3] - 2018-08-30
### Added
* `min_endstop` and `max_endstop` objects can be configured on GPIO
* Axes can be homed if `min_endstop` is enabled
* Encoder position count "homed" to zero when index is found.
### Changed
Dockerfile
+22
View File
@@ -0,0 +1,22 @@
FROM ubuntu:bionic
# Prepare the build environment and dependencies
RUN apt-get update
RUN apt-get -y install software-properties-common
RUN add-apt-repository ppa:team-gcc-arm-embedded/ppa
RUN add-apt-repository ppa:jonathonf/tup
RUN apt-get update
RUN apt-get -y upgrade
RUN apt-get -y install gcc-arm-embedded openocd tup python3.7 build-essential git
# Build step below does not know about debian's python naming schemme
RUN ln -s /usr/bin/python3.7 /usr/bin/python
# Copy the firmware tree into the container
RUN mkdir ODrive
COPY . ODrive
WORKDIR ODrive/Firmware
# Hack around Tup's dependency on FUSE
RUN tup generate build.sh
RUN ./build.sh
Firmware/.clang-format
+1 -2
View File
@@ -1,7 +1,6 @@
---
BasedOnStyle: Google
AlignConsecutiveAssignments: 'true'
AllowShortCaseLabelsOnASingleLine: 'true'
IndentWidth: '4'
ColumnLimit: '0'
...
Firmware/.gitignore
+1
View File
@@ -1,5 +1,6 @@
#build folder
autogen/
build/
deploy/
.dep/
Firmware/.vscode/c_cpp_properties.json
Vendored
+12 -74
View File
@@ -3,21 +3,7 @@
{
"name": "Win32",
"includePath": [
"${workspaceRoot}",
"${workspaceRoot}/fibre/cpp/include/**",
"${workspaceRoot}/MotorControl",
"${workspaceRoot}/communication",
"${workspaceRoot}/Drivers/DRV8301",
"${workspaceRoot}/Board/v3/Inc",
"${workspaceRoot}/Board/v3/Drivers/CMSIS/Include",
"${workspaceRoot}/Board/v3/Drivers/CMSIS/Device/ST/STM32F4xx/Include",
"${workspaceRoot}/Board/v3/Drivers/STM32F4xx_HAL_Driver/Inc",
"${workspaceRoot}/Board/v3/Drivers/STM32F4xx_HAL_Driver/Inc/Legacy",
"${workspaceRoot}/Board/v3/Middlewares/ST/STM32_USB_Device_Library/Core/Inc",
"${workspaceRoot}/Board/v3/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc",
"${workspaceRoot}/Board/v3/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS",
"${workspaceRoot}/Board/v3/Middlewares/Third_Party/FreeRTOS/Source/include",
"${workspaceRoot}/Board/v3/Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F"
"${workspaceFolder}/**"
],
"defines": [
"STM32F405xx",
@@ -30,75 +16,35 @@
"__packed=\"__attribute__((__packed__))\"",
"__GNUC__"
],
"intelliSenseMode": "clang-x64",
"compilerPath": "\"${ARM_GCC_ROOT}/bin/arm-none-eabi-gcc.exe\" -mthumb -mcpu=cortex-m4 -mfpu=fpv4-sp-d16 -mfloat-abi=hard -specs=nosys.specs -specs=nano.specs -u _printf_float -u _scanf_float",
"intelliSenseMode": "gcc-x64",
"compilerPath": "\"${ARM_GCC_ROOT}/bin/arm-none-eabi-g++.exe\" -mthumb -mcpu=cortex-m4 -mfpu=fpv4-sp-d16 -mfloat-abi=hard -specs=nosys.specs -specs=nano.specs -u _printf_float -u _scanf_float",
"cStandard": "c11",
"cppStandard": "c++14"
"cppStandard": "c++17"
},
{
"name": "Linux",
"includePath": [
"${workspaceRoot}",
"${workspaceRoot}/fibre/cpp/include/**",
"${workspaceRoot}/MotorControl",
"${workspaceRoot}/communication",
"${workspaceRoot}/Drivers/DRV8301",
"${workspaceRoot}/Board/v3/Inc",
"${workspaceRoot}/Board/v3/Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F",
"${workspaceRoot}/Board/v3/Middlewares/Third_Party/FreeRTOS/Source/include",
"${workspaceRoot}/Board/v3/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS",
"${workspaceRoot}/Board/v3/Middlewares/ST/STM32_USB_Device_Library/Core/Inc",
"${workspaceRoot}/Board/v3/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc",
"${workspaceRoot}/Board/v3/Drivers/STM32F4xx_HAL_Driver/Inc",
"${workspaceRoot}/Board/v3/Drivers/STM32F4xx_HAL_Driver/Inc/Legacy",
"${workspaceRoot}/Board/v3/Drivers/CMSIS/Device/ST/STM32F4xx/Include",
"${workspaceRoot}/Board/v3/Drivers/CMSIS/Include",
"${ARM_GCC_ROOT}/arm-none-eabi/include/**",
"${ARM_GCC_ROOT}/lib/gcc/arm-none-eabi/**"
"${workspaceFolder}/**"
],
"defines": [
"STM32F405xx",
"USE_HAL_DRIVER",
"HW_VERSION_MAJOR=3",
"HW_VERSION_MINOR=4",
"HW_VERSION_VOLTAGE=24",
"HW_VERSION_MINOR=6",
"HW_VERSION_VOLTAGE=56",
"__weak=\"__attribute__((weak))\"",
"__packed=\"__attribute__((__packed__))\"",
"__GNUC__"
],
"intelliSenseMode": "clang-x64",
"browse": {
"path": [
"${workspaceRoot}",
"${ARM_GCC_ROOT}"
],
"limitSymbolsToIncludedHeaders": true,
"databaseFilename": ""
},
"compilerPath": "arm-none-eabi-gcc -mthumb -mcpu=cortex-m4 -mfpu=fpv4-sp-d16 -mfloat-abi=hard -specs=nosys.specs -specs=nano.specs -u _printf_float -u _scanf_float",
"intelliSenseMode": "gcc-x64",
"compilerPath": "arm-none-eabi-g++ -mthumb -mcpu=cortex-m4 -mfpu=fpv4-sp-d16 -mfloat-abi=hard -specs=nosys.specs -specs=nano.specs -u _printf_float -u _scanf_float",
"cStandard": "c11",
"cppStandard": "c++14"
"cppStandard": "c++17"
},
{
"name": "Mac",
"includePath": [
"${workspaceRoot}",
"${workspaceRoot}/fibre/cpp/include/**",
"${workspaceRoot}/MotorControl",
"${workspaceRoot}/communication",
"${workspaceRoot}/Drivers/DRV8301",
"${workspaceRoot}/Board/v3/Inc",
"${workspaceRoot}/Board/v3/Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F",
"${workspaceRoot}/Board/v3/Middlewares/Third_Party/FreeRTOS/Source/include",
"${workspaceRoot}/Board/v3/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS",
"${workspaceRoot}/Board/v3/Middlewares/ST/STM32_USB_Device_Library/Core/Inc",
"${workspaceRoot}/Board/v3/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc",
"${workspaceRoot}/Board/v3/Drivers/STM32F4xx_HAL_Driver/Inc",
"${workspaceRoot}/Board/v3/Drivers/STM32F4xx_HAL_Driver/Inc/Legacy",
"${workspaceRoot}/Board/v3/Drivers/CMSIS/Device/ST/STM32F4xx/Include",
"${workspaceRoot}/Board/v3/Drivers/CMSIS/Include",
"${ARM_GCC_ROOT}/arm-none-eabi/include/**",
"${ARM_GCC_ROOT}/lib/gcc/arm-none-eabi/**"
"${workspaceFolder}/**"
],
"defines": [
"STM32F405xx",
@@ -110,15 +56,7 @@
"__packed=\"__attribute__((__packed__))\"",
"__GNUC__"
],
"intelliSenseMode": "clang-x64",
"browse": {
"path": [
"${workspaceRoot}",
"${ARM_GCC_ROOT}"
],
"limitSymbolsToIncludedHeaders": true,
"databaseFilename": ""
},
"intelliSenseMode": "gcc-x64",
"cStandard": "c11",
"cppStandard": "c++17"
}
Firmware/.vscode/launch.json
Vendored
+36 -2
View File
@@ -9,7 +9,7 @@
"type": "cortex-debug",
"servertype": "openocd",
"request": "launch",
"name": "Debug ODrive",
"name": "Debug ODrive - ST-Link",
"executable": "${workspaceRoot}/build/ODriveFirmware.elf",
"configFiles": [
"interface/stlink-v2.cfg",
@@ -23,14 +23,48 @@
"type": "cortex-debug",
"servertype": "openocd",
"request": "launch",
"name": "Debug ODrive - FreeRTOS",
"name": "Debug ODrive - ST-Link - FreeRTOS",
"executable": "${workspaceRoot}/build/ODriveFirmware.elf",
"rtos": "FreeRTOS",
"configFiles": [
"interface/stlink-v2.cfg",
"target/stm32f4x_stlink.cfg",
],
"svdFile": "${workspaceRoot}/Board/v3/STM32F40x.svd",
"cwd": "${workspaceRoot}"
},
{
// For the Cortex-Debug extension
// ssh -t odrv -L3333:localhost:3333 bash -c "\"openocd '-f' 'interface/stlink-v2.cfg' '-f' 'target/stm32f4x_stlink.cfg'\""
"type": "cortex-debug",
"servertype": "external",
"gdbTarget": "localhost:3333",
"preLaunchCommands": [
"load"
],
"request": "launch",
"name": "Debug ODrive via external server",
"executable": "${workspaceRoot}/build/ODriveFirmware.elf",
"configFiles": [
"interface/stlink-v2.cfg",
"target/stm32f4x_stlink.cfg",
],
"svdFile": "${workspaceRoot}/Board/v3/STM32F40x.svd",
"cwd": "${workspaceRoot}"
},
{
// For the Cortex-Debug extensions
"type": "cortex-debug",
"servertype": "bmp",
"request": "launch",
"name": "Debug ODrive - Black Magic Probe",
"executable": "${workspaceRoot}/build/ODriveFirmware.elf",
"device": "STM32F4xx",
"BMPGDBSerialPort": "${env:BMP_PORT}",
"interface": "swd",
"targetId": 1,
"armToolchainPath": "${env:ARM_GCC_ROOT}/bin/",
"cwd": "${workspaceRoot}"
}
]
}
Firmware/.vscode/settings.json
Vendored
-3
View File
@@ -1,9 +1,6 @@
{
"C_Cpp.intelliSenseEngine": "Default",
"C_Cpp.intelliSenseEngineFallback": "Disabled",
"files.exclude": {
"build": true
},
"files.associations": {
"memory": "cpp",
"utility": "cpp",
Firmware/.vscode/tasks.json
Vendored
+9 -3
View File
@@ -4,7 +4,7 @@
"version": "2.0.0",
"tasks": [
{
"taskName": "build",
"label": "build",
"type": "shell",
"command": "make",
"group": {
@@ -19,13 +19,19 @@
]
},
{
"taskName": "flash",
"label": "flash - ST-Link",
"type": "shell",
"command": "make flash",
"problemMatcher": []
},
{
"taskName": "openocd",
"label": "flash - Black Magic Probe",
"type": "shell",
"command": "make flashbmp",
"problemMatcher": []
},
{
"label": "openocd",
"type": "shell",
"command": "openocd -f \"interface/stlink-v2.cfg\" -f \"target/stm32f4x_stlink.cfg\" -c \"gdb_port 3333; log_output openocd.log\"",
"problemMatcher": []
Firmware/Board/v3/Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_ll_usb.c
+16 -2
View File
@@ -56,6 +56,20 @@
/* Includes ------------------------------------------------------------------*/
#include "stm32f4xx_hal.h"
// taken from https://github.com/ARMmbed/mbed-os/blob/master/cmsis/TARGET_CORTEX_M/cmsis_compiler.h
#ifndef __PACKED_STRUCT
#define __PACKED_STRUCT struct __attribute__((packed))
#endif
#ifndef __UNALIGNED_UINT32_WRITE
__PACKED_STRUCT T_UINT32_WRITE { uint32_t v; };
#define __UNALIGNED_UINT32_WRITE(addr, val) (void)((((struct T_UINT32_WRITE *)(void *)(addr))->v) = (val))
#endif
#ifndef __UNALIGNED_UINT32_READ
__PACKED_STRUCT T_UINT32_READ { uint32_t v; };
#define __UNALIGNED_UINT32_READ(addr) (((const struct T_UINT32_READ *)(const void *)(addr))->v)
#endif
/** @addtogroup STM32F4xx_LL_USB_DRIVER
* @{
*/
@@ -883,7 +897,7 @@ HAL_StatusTypeDef USB_WritePacket(USB_OTG_GlobalTypeDef *USBx, uint8_t *src, uin
count32b = (len + 3U) / 4U;
for (i = 0U; i < count32b; i++, src += 4U)
{
USBx_DFIFO(ch_ep_num) = *((__packed uint32_t *)src);
USBx_DFIFO(ch_ep_num) = __UNALIGNED_UINT32_READ(src);
}
}
return HAL_OK;
@@ -909,7 +923,7 @@ void *USB_ReadPacket(USB_OTG_GlobalTypeDef *USBx, uint8_t *dest, uint16_t len)
for ( i = 0U; i < count32b; i++, dest += 4U )
{
*(__packed uint32_t *)dest = USBx_DFIFO(0U);
__UNALIGNED_UINT32_WRITE(dest, USBx_DFIFO(0U));
}
return ((void *)dest);
Firmware/Board/v3/Inc/freertos_vars.h
+3
View File
@@ -7,8 +7,11 @@ extern osSemaphoreId sem_usb_irq;
extern osSemaphoreId sem_uart_dma;
extern osSemaphoreId sem_usb_rx;
extern osSemaphoreId sem_usb_tx;
extern osSemaphoreId sem_can;
extern osThreadId defaultTaskHandle;
extern osThreadId usb_irq_thread;
extern const uint32_t stack_size_usb_irq_thread;
extern const uint32_t stack_size_default_task;
#endif /* __FREERTOS_H */
Firmware/Board/v3/Inc/gpio.h
+1 -4
View File
@@ -72,13 +72,10 @@ void MX_GPIO_Init(void);
void SetGPIO12toUART();
bool GPIO_subscribe(GPIO_TypeDef* GPIO_port, uint16_t GPIO_pin,
uint32_t pull_up_down,
void (*callback)(void*), void* ctx);
uint32_t pull_up_down, void (*callback)(void*), void* ctx);
void GPIO_unsubscribe(GPIO_TypeDef* GPIO_port, uint16_t GPIO_pin);
void GPIO_set_to_analog(GPIO_TypeDef* GPIO_port, uint16_t GPIO_pin);
uint16_t get_gpio_pin_by_pin(uint16_t GPIO_pin);
GPIO_TypeDef* get_gpio_port_by_pin(uint16_t GPIO_pin);
#if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR <= 4
#define GPIO_COUNT 5
Firmware/Board/v3/Inc/main.h
-2
View File
@@ -173,10 +173,8 @@
#if HW_VERSION_VOLTAGE >= 48
#define VBUS_S_DIVIDER_RATIO 19.0f
#define VBUS_OVERVOLTAGE_LEVEL 52.0f
#elif HW_VERSION_VOLTAGE == 24
#define VBUS_S_DIVIDER_RATIO 11.0f
#define VBUS_OVERVOLTAGE_LEVEL 26.0f
#else
#error "unknown board voltage"
#endif
Firmware/Board/v3/Src/can.c
+6 -6
View File
@@ -63,15 +63,15 @@ void MX_CAN1_Init(void)
{
hcan1.Instance = CAN1;
hcan1.Init.Prescaler = 7;
hcan1.Init.Prescaler = 8;
hcan1.Init.Mode = CAN_MODE_NORMAL;
hcan1.Init.SyncJumpWidth = CAN_SJW_1TQ;
hcan1.Init.TimeSeg1 = CAN_BS1_6TQ;
hcan1.Init.TimeSeg2 = CAN_BS2_5TQ;
hcan1.Init.SyncJumpWidth = CAN_SJW_4TQ;
hcan1.Init.TimeSeg1 = CAN_BS1_16TQ;
hcan1.Init.TimeSeg2 = CAN_BS2_4TQ;
hcan1.Init.TimeTriggeredMode = DISABLE;
hcan1.Init.AutoBusOff = DISABLE;
hcan1.Init.AutoBusOff = ENABLE;
hcan1.Init.AutoWakeUp = ENABLE;
hcan1.Init.AutoRetransmission = DISABLE;
hcan1.Init.AutoRetransmission = ENABLE;
hcan1.Init.ReceiveFifoLocked = DISABLE;
hcan1.Init.TransmitFifoPriority = DISABLE;
if (HAL_CAN_Init(&hcan1) != HAL_OK)
Firmware/Board/v3/Src/freertos.c
+11 -2
View File
@@ -61,6 +61,7 @@
extern PCD_HandleTypeDef hpcd_USB_OTG_FS;
int odrive_main(void);
int load_configuration(void);
int construct_objects(void);
/* USER CODE END Includes */
/* Private typedef -----------------------------------------------------------*/
@@ -85,14 +86,17 @@ osSemaphoreId sem_usb_irq;
osSemaphoreId sem_uart_dma;
osSemaphoreId sem_usb_rx;
osSemaphoreId sem_usb_tx;
osSemaphoreId sem_can;
osThreadId usb_irq_thread;
const uint32_t stack_size_usb_irq_thread = 2048; // Bytes
// Place FreeRTOS heap in core coupled memory for better performance
__attribute__((section(".ccmram")))
uint8_t ucHeap[configTOTAL_HEAP_SIZE];
/* USER CODE END Variables */
osThreadId defaultTaskHandle;
const uint32_t stack_size_default_task = 2048; // Bytes
/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN FunctionPrototypes */
@@ -148,7 +152,7 @@ void usb_deferred_interrupt_thread(void * ctx) {
void init_deferred_interrupts(void) {
// Start USB interrupt handler thread
osThreadDef(task_usb_pump, usb_deferred_interrupt_thread, osPriorityAboveNormal, 0, 512);
osThreadDef(task_usb_pump, usb_deferred_interrupt_thread, osPriorityAboveNormal, 0, stack_size_usb_irq_thread / sizeof(StackType_t));
usb_irq_thread = osThreadCreate(osThread(task_usb_pump), NULL);
}
@@ -187,10 +191,15 @@ void MX_FREERTOS_Init(void) {
osSemaphoreDef(sem_usb_tx);
sem_usb_tx = osSemaphoreCreate(osSemaphore(sem_usb_tx), 1);
osSemaphoreDef(sem_can);
sem_can = osSemaphoreCreate(osSemaphore(sem_can), 1);
osSemaphoreWait(sem_can, 0);
init_deferred_interrupts();
// Load persistent configuration (or defaults)
load_configuration();
construct_objects();
/* USER CODE END RTOS_SEMAPHORES */
/* USER CODE BEGIN RTOS_TIMERS */
@@ -199,7 +208,7 @@ void MX_FREERTOS_Init(void) {
/* Create the thread(s) */
/* definition and creation of defaultTask */
osThreadDef(defaultTask, StartDefaultTask, osPriorityNormal, 0, 256);
osThreadDef(defaultTask, StartDefaultTask, osPriorityNormal, 0, stack_size_default_task / sizeof(StackType_t));
defaultTaskHandle = osThreadCreate(osThread(defaultTask), NULL);
/* USER CODE BEGIN RTOS_THREADS */
Firmware/Board/v3/Src/gpio.c
+1 -44
View File
@@ -205,8 +205,7 @@ size_t n_subscriptions = 0;
// on a rising edge of the GPIO.
// @param pull_up_down: one of GPIO_NOPULL, GPIO_PULLUP or GPIO_PULLDOWN
bool GPIO_subscribe(GPIO_TypeDef* GPIO_port, uint16_t GPIO_pin,
uint32_t pull_up_down,
void (*callback)(void*), void* ctx) {
uint32_t pull_up_down, void (*callback)(void*), void* ctx) {
// Register handler (or reuse existing registration)
// TODO: make thread safe
@@ -279,49 +278,7 @@ void HAL_GPIO_EXTI_Callback(uint16_t GPIO_pin) {
}
}
GPIO_TypeDef* get_gpio_port_by_pin(uint16_t GPIO_pin){
switch(GPIO_pin){
case 1: return GPIO_1_GPIO_Port; break;
case 2: return GPIO_2_GPIO_Port; break;
case 3: return GPIO_3_GPIO_Port; break;
case 4: return GPIO_4_GPIO_Port; break;
#ifdef GPIO_5_GPIO_Port
case 5: return GPIO_5_GPIO_Port; break;
#endif
#ifdef GPIO_6_GPIO_Port
case 6: return GPIO_6_GPIO_Port; break;
#endif
#ifdef GPIO_7_GPIO_Port
case 7: return GPIO_7_GPIO_Port; break;
#endif
#ifdef GPIO_8_GPIO_Port
case 8: return GPIO_8_GPIO_Port; break;
#endif
default: return GPIO_1_GPIO_Port;
}
}
uint16_t get_gpio_pin_by_pin(uint16_t GPIO_pin){
switch(GPIO_pin){
case 1: return GPIO_1_Pin; break;
case 2: return GPIO_2_Pin; break;
case 3: return GPIO_3_Pin; break;
case 4: return GPIO_4_Pin; break;
#ifdef GPIO_5_Pin
case 5: return GPIO_5_Pin; break;
#endif
#ifdef GPIO_6_Pin
case 6: return GPIO_6_Pin; break;
#endif
#ifdef GPIO_7_Pin
case 7: return GPIO_7_Pin; break;
#endif
#ifdef GPIO_8_Pin
case 8: return GPIO_8_Pin; break;
#endif
default: return GPIO_1_Pin;
}
}
/* USER CODE END 2 */
Firmware/Board/v3/Src/spi.c
+17 -5
View File
@@ -103,7 +103,7 @@ void HAL_SPI_MspInit(SPI_HandleTypeDef* spiHandle)
*/
GPIO_InitStruct.Pin = GPIO_PIN_10|GPIO_PIN_11|GPIO_PIN_12;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Pull = GPIO_PULLUP; // required for disconnect detection on SPI encoders
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
GPIO_InitStruct.Alternate = GPIO_AF6_SPI3;
HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
@@ -115,8 +115,15 @@ void HAL_SPI_MspInit(SPI_HandleTypeDef* spiHandle)
hdma_spi3_tx.Init.Direction = DMA_MEMORY_TO_PERIPH;
hdma_spi3_tx.Init.PeriphInc = DMA_PINC_DISABLE;
hdma_spi3_tx.Init.MemInc = DMA_MINC_ENABLE;
hdma_spi3_tx.Init.PeriphDataAlignment = DMA_PDATAALIGN_HALFWORD;
hdma_spi3_tx.Init.MemDataAlignment = DMA_MDATAALIGN_HALFWORD;
if(spiHandle->Init.DataSize == SPI_DATASIZE_8BIT){
hdma_spi3_tx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
hdma_spi3_tx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
} else {
hdma_spi3_tx.Init.PeriphDataAlignment = DMA_PDATAALIGN_HALFWORD;
hdma_spi3_tx.Init.MemDataAlignment = DMA_MDATAALIGN_HALFWORD;
}
hdma_spi3_tx.Init.Mode = DMA_NORMAL;
hdma_spi3_tx.Init.Priority = DMA_PRIORITY_MEDIUM;
hdma_spi3_tx.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
@@ -133,8 +140,13 @@ void HAL_SPI_MspInit(SPI_HandleTypeDef* spiHandle)
hdma_spi3_rx.Init.Direction = DMA_PERIPH_TO_MEMORY;
hdma_spi3_rx.Init.PeriphInc = DMA_PINC_DISABLE;
hdma_spi3_rx.Init.MemInc = DMA_MINC_ENABLE;
hdma_spi3_rx.Init.PeriphDataAlignment = DMA_PDATAALIGN_HALFWORD;
hdma_spi3_rx.Init.MemDataAlignment = DMA_MDATAALIGN_HALFWORD;
if (spiHandle->Init.DataSize == SPI_DATASIZE_8BIT) {
hdma_spi3_rx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
hdma_spi3_rx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
} else {
hdma_spi3_rx.Init.PeriphDataAlignment = DMA_PDATAALIGN_HALFWORD;
hdma_spi3_rx.Init.MemDataAlignment = DMA_MDATAALIGN_HALFWORD;
}
hdma_spi3_rx.Init.Mode = DMA_NORMAL;
hdma_spi3_rx.Init.Priority = DMA_PRIORITY_MEDIUM;
hdma_spi3_rx.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
Firmware/Drivers/DRV8301/drv8301.c
+1 -1
View File
@@ -45,7 +45,7 @@
// drivers
#include "drv8301.h"
#include "utils.h"
#include "utils.hpp"
// **************************************************************************
Firmware/Makefile
+9
View File
@@ -20,6 +20,15 @@ flash: all
-c 'reset run' \
-c exit
flashbmp: all
arm-none-eabi-gdb --ex 'target extended-remote $(BMP_PORT)' \
--ex 'monitor swdp_scan' \
--ex 'attach 1' \
--ex 'load' \
--ex 'detach' \
--ex 'quit' \
$(FIRMWARE)
gdb: all
arm-none-eabi-gdb $(FIRMWARE) -x openocd.gdbinit
Firmware/MotorControl/axis.cpp
+224 -73
View File
@@ -3,8 +3,10 @@
#include <functional>
#include "gpio.h"
#include "utils.h"
#include "odrive_main.h"
#include "utils.hpp"
#include "gpio_utils.hpp"
#include "communication/interface_can.hpp"
Axis::Axis(int axis_num,
const AxisHardwareConfig_t& hw_config,
@@ -12,25 +14,42 @@ Axis::Axis(int axis_num,
Encoder& encoder,
SensorlessEstimator& sensorless_estimator,
Controller& controller,
OnboardThermistorCurrentLimiter& fet_thermistor,
OffboardThermistorCurrentLimiter& motor_thermistor,
Motor& motor,
TrapezoidalTrajectory& trap)
TrapezoidalTrajectory& trap,
Endstop& min_endstop,
Endstop& max_endstop)
: axis_num_(axis_num),
hw_config_(hw_config),
config_(config),
encoder_(encoder),
sensorless_estimator_(sensorless_estimator),
controller_(controller),
fet_thermistor_(fet_thermistor),
motor_thermistor_(motor_thermistor),
motor_(motor),
trap_(trap)
trap_traj_(trap),
min_endstop_(min_endstop),
max_endstop_(max_endstop),
current_limiters_(make_array(
static_cast<CurrentLimiter*>(&fet_thermistor),
static_cast<CurrentLimiter*>(&motor_thermistor))),
thermistors_(make_array(
static_cast<ThermistorCurrentLimiter*>(&fet_thermistor),
static_cast<ThermistorCurrentLimiter*>(&motor_thermistor)))
{
encoder_.axis_ = this;
sensorless_estimator_.axis_ = this;
controller_.axis_ = this;
fet_thermistor_.axis_ = this;
motor_thermistor.axis_ = this;
motor_.axis_ = this;
trap_.axis_ = this;
trap_traj_.axis_ = this;
min_endstop_.axis_ = this;
max_endstop_.axis_ = this;
decode_step_dir_pins();
update_watchdog_settings();
watchdog_feed();
}
Axis::LockinConfig_t Axis::default_calibration() {
@@ -65,10 +84,10 @@ static void step_cb_wrapper(void* ctx) {
reinterpret_cast<Axis*>(ctx)->step_cb();
}
// @brief Sets up all components of the axis,
// such as gate driver and encoder hardware.
void Axis::setup() {
encoder_.setup();
motor_.setup();
}
@@ -79,7 +98,7 @@ static void run_state_machine_loop_wrapper(void* ctx) {
// @brief Starts run_state_machine_loop in a new thread
void Axis::start_thread() {
osThreadDef(thread_def, run_state_machine_loop_wrapper, hw_config_.thread_priority, 0, 4*512);
osThreadDef(thread_def, run_state_machine_loop_wrapper, hw_config_.thread_priority, 0, stack_size_ / sizeof(StackType_t));
thread_id_ = osThreadCreate(osThread(thread_def), this);
thread_id_valid_ = true;
}
@@ -99,11 +118,10 @@ bool Axis::wait_for_current_meas() {
// step/direction interface
void Axis::step_cb() {
if (step_dir_active_) {
GPIO_PinState dir_pin = HAL_GPIO_ReadPin(dir_port_, dir_pin_);
float dir = (dir_pin == GPIO_PIN_SET) ? 1.0f : -1.0f;
controller_.pos_setpoint_ += dir * config_.counts_per_step;
}
const bool dir_pin = dir_port_->IDR & dir_pin_;
const int32_t dir = (-1 + 2 * dir_pin) * step_dir_active_;
controller_.input_pos_ += dir * config_.turns_per_step;
controller_.input_pos_updated();
};
void Axis::load_default_step_dir_pin_config(
@@ -112,6 +130,10 @@ void Axis::load_default_step_dir_pin_config(
config->dir_gpio_pin = hw_config.dir_gpio_pin;
}
void Axis::load_default_can_id(const int& id, Config_t& config){
config.can_node_id = id;
}
void Axis::decode_step_dir_pins() {
step_port_ = get_gpio_port_by_pin(config_.step_gpio_pin);
step_pin_ = get_gpio_pin_by_pin(config_.step_gpio_pin);
@@ -119,21 +141,6 @@ void Axis::decode_step_dir_pins() {
dir_pin_ = get_gpio_pin_by_pin(config_.dir_gpio_pin);
}
// @brief: Setup the watchdog reset value from the configuration watchdog timeout interval.
void Axis::update_watchdog_settings() {
if(config_.watchdog_timeout <= 0.0f) { // watchdog disabled
watchdog_reset_value_ = 0;
} else if(config_.watchdog_timeout >= UINT32_MAX / (current_meas_hz+1)) { //overflow!
watchdog_reset_value_ = UINT32_MAX;
} else {
watchdog_reset_value_ = static_cast<uint32_t>(config_.watchdog_timeout * current_meas_hz);
}
// Do a feed to avoid instant timeout
watchdog_feed();
}
// @brief (de)activates step/dir input
void Axis::set_step_dir_active(bool active) {
if (active) {
@@ -145,8 +152,7 @@ void Axis::set_step_dir_active(bool active) {
HAL_GPIO_Init(dir_port_, &GPIO_InitStruct);
// Subscribe to rising edges of the step GPIO
GPIO_subscribe(step_port_, step_pin_, GPIO_PULLDOWN,
step_cb_wrapper, this);
GPIO_subscribe(step_port_, step_pin_, GPIO_PULLDOWN, step_cb_wrapper, this);
step_dir_active_ = true;
} else {
@@ -165,40 +171,56 @@ bool Axis::do_checks() {
if ((current_state_ != AXIS_STATE_IDLE) && (motor_.armed_state_ == Motor::ARMED_STATE_DISARMED))
// motor got disarmed in something other than the idle loop
error_ |= ERROR_MOTOR_DISARMED;
if (!(vbus_voltage >= board_config.dc_bus_undervoltage_trip_level))
if (!(vbus_voltage >= odrv.config_.dc_bus_undervoltage_trip_level))
error_ |= ERROR_DC_BUS_UNDER_VOLTAGE;
if (!(vbus_voltage <= board_config.dc_bus_overvoltage_trip_level))
if (!(vbus_voltage <= odrv.config_.dc_bus_overvoltage_trip_level))
error_ |= ERROR_DC_BUS_OVER_VOLTAGE;
// Sub-components should use set_error which will propegate to this error_
for (ThermistorCurrentLimiter* thermistor : thermistors_) {
thermistor->do_checks();
}
motor_.do_checks();
encoder_.do_checks();
// encoder_.do_checks();
// sensorless_estimator_.do_checks();
// controller_.do_checks();
// Check for endstop presses
if (min_endstop_.config_.enabled && min_endstop_.get_state() && !(current_state_ == AXIS_STATE_HOMING)) {
error_ |= ERROR_MIN_ENDSTOP_PRESSED;
} else if (max_endstop_.config_.enabled && max_endstop_.get_state() && !(current_state_ == AXIS_STATE_HOMING)) {
error_ |= ERROR_MAX_ENDSTOP_PRESSED;
}
return check_for_errors();
}
// @brief Update all esitmators
bool Axis::do_updates() {
// Sub-components should use set_error which will propegate to this error_
for (ThermistorCurrentLimiter* thermistor : thermistors_) {
thermistor->update();
}
encoder_.update();
sensorless_estimator_.update();
return check_for_errors();
min_endstop_.update();
max_endstop_.update();
bool ret = check_for_errors();
odCAN->send_heartbeat(this);
return ret;
}
// @brief Feed the watchdog to prevent watchdog timeouts.
void Axis::watchdog_feed() {
watchdog_current_value_ = watchdog_reset_value_;
watchdog_current_value_ = get_watchdog_reset();
}
// @brief Check the watchdog timer for expiration. Also sets the watchdog error bit if expired.
// @brief Check the watchdog timer for expiration. Also sets the watchdog error bit if expired.
bool Axis::watchdog_check() {
// reset value = 0 means watchdog disabled.
if(watchdog_reset_value_ == 0) return true;
if (!config_.enable_watchdog) return true;
// explicit check here to ensure that we don't underflow back to UINT32_MAX
if(watchdog_current_value_ > 0) {
if (watchdog_current_value_ > 0) {
watchdog_current_value_--;
return true;
} else {
@@ -229,9 +251,9 @@ bool Axis::run_lockin_spin(const LockinConfig_t &lockin_config) {
auto spin_done = [&](bool vel_override = false) -> bool {
bool done = false;
if (lockin_config.finish_on_vel || vel_override)
done = done || fabsf(vel) >= fabsf(lockin_config.vel);
done = done || std::abs(vel) >= std::abs(lockin_config.vel);
if (lockin_config.finish_on_distance)
done = done || fabsf(distance) >= fabsf(lockin_config.finish_distance);
done = done || std::abs(distance) >= std::abs(lockin_config.finish_distance);
if (lockin_config.finish_on_enc_idx)
done = done || encoder_.index_found_;
return done;
@@ -272,15 +294,18 @@ bool Axis::run_lockin_spin(const LockinConfig_t &lockin_config) {
// Note run_sensorless_control_loop and run_closed_loop_control_loop are very similar and differ only in where we get the estimate from.
bool Axis::run_sensorless_control_loop() {
run_control_loop([this](){
if (controller_.config_.control_mode >= Controller::CTRL_MODE_POSITION_CONTROL)
return error_ |= ERROR_POS_CTRL_DURING_SENSORLESS, false;
controller_.pos_estimate_linear_src_ = nullptr;
controller_.pos_estimate_circular_src_ = nullptr;
controller_.pos_estimate_valid_src_ = nullptr;
controller_.vel_estimate_src_ = &sensorless_estimator_.vel_estimate_;
controller_.vel_estimate_valid_src_ = &sensorless_estimator_.vel_estimate_valid_;
run_control_loop([this](){
// Note that all estimators are updated in the loop prefix in run_control_loop
float current_setpoint;
if (!controller_.update(sensorless_estimator_.pll_pos_, sensorless_estimator_.vel_estimate_, &current_setpoint))
float torque_setpoint;
if (!controller_.update(&torque_setpoint))
return error_ |= ERROR_CONTROLLER_FAILED, false;
if (!motor_.update(current_setpoint, sensorless_estimator_.phase_, sensorless_estimator_.vel_estimate_))
if (!motor_.update(torque_setpoint, sensorless_estimator_.phase_, sensorless_estimator_.vel_estimate_))
return false; // set_error should update axis.error_
return true;
});
@@ -288,20 +313,146 @@ bool Axis::run_sensorless_control_loop() {
}
bool Axis::run_closed_loop_control_loop() {
if (!controller_.select_encoder(controller_.config_.load_encoder_axis)) {
return error_ |= ERROR_CONTROLLER_FAILED, false;
}
// To avoid any transient on startup, we intialize the setpoint to be the current position
controller_.pos_setpoint_ = encoder_.pos_estimate_;
if (controller_.config_.circular_setpoints) {
if (!controller_.pos_estimate_circular_src_) {
return error_ |= ERROR_CONTROLLER_FAILED, false;
}
else {
controller_.pos_setpoint_ = *controller_.pos_estimate_circular_src_;
controller_.input_pos_ = *controller_.pos_estimate_circular_src_;
}
}
else {
if (!controller_.pos_estimate_linear_src_) {
return error_ |= ERROR_CONTROLLER_FAILED, false;
}
else {
controller_.pos_setpoint_ = *controller_.pos_estimate_linear_src_;
controller_.input_pos_ = *controller_.pos_estimate_linear_src_;
}
}
controller_.input_pos_updated();
// Avoid integrator windup issues
controller_.vel_integrator_torque_ = 0.0f;
set_step_dir_active(config_.enable_step_dir);
run_control_loop([this](){
// Note that all estimators are updated in the loop prefix in run_control_loop
float current_setpoint;
if (!controller_.update(encoder_.pos_estimate_, encoder_.vel_estimate_, &current_setpoint))
return error_ |= ERROR_CONTROLLER_FAILED, false; //TODO: Make controller.set_error
float phase_vel = 2*M_PI * encoder_.vel_estimate_ / (float)encoder_.config_.cpr * motor_.config_.pole_pairs;
if (!motor_.update(current_setpoint, encoder_.phase_, phase_vel))
float torque_setpoint;
if (!controller_.update(&torque_setpoint))
return error_ |= ERROR_CONTROLLER_FAILED, false;
float phase_vel = (2*M_PI) * encoder_.vel_estimate_ * motor_.config_.pole_pairs;
if (!motor_.update(torque_setpoint, encoder_.phase_, phase_vel))
return false; // set_error should update axis.error_
return true;
});
set_step_dir_active(false);
set_step_dir_active(config_.enable_step_dir && config_.step_dir_always_on);
return check_for_errors();
}
// Slowly drive in the negative direction at homing_speed until the min endstop is pressed
// When pressed, set the linear count to the offset (default 0), and then go to position 0
bool Axis::run_homing() {
Controller::ControlMode stored_control_mode = controller_.config_.control_mode;
Controller::InputMode stored_input_mode = controller_.config_.input_mode;
// TODO: theoretically this check should be inside the update loop,
// otherwise someone could disable the endstop while homing is in progress.
if (!min_endstop_.config_.enabled) {
return error_ |= ERROR_HOMING_WITHOUT_ENDSTOP, false;
}
controller_.config_.control_mode = Controller::CONTROL_MODE_VELOCITY_CONTROL;
controller_.config_.input_mode = Controller::INPUT_MODE_VEL_RAMP;
controller_.input_pos_ = 0.0f;
controller_.input_pos_updated();
controller_.input_vel_ = -controller_.config_.homing_speed;
controller_.input_torque_ = 0.0f;
homing_.is_homed = false;
if (!controller_.select_encoder(controller_.config_.load_encoder_axis)) {
return error_ |= ERROR_CONTROLLER_FAILED, false;
}
// To avoid any transient on startup, we intialize the setpoint to be the current position
// note - input_pos_ is not set here. It is set to 0 earlier in this method and velocity control is used.
if (controller_.config_.circular_setpoints) {
if (!controller_.pos_estimate_circular_src_) {
return error_ |= ERROR_CONTROLLER_FAILED, false;
}
else {
controller_.pos_setpoint_ = *controller_.pos_estimate_circular_src_;
}
}
else {
if (!controller_.pos_estimate_linear_src_) {
return error_ |= ERROR_CONTROLLER_FAILED, false;
}
else {
controller_.pos_setpoint_ = *controller_.pos_estimate_linear_src_;
}
}
// Avoid integrator windup issues
controller_.vel_integrator_torque_ = 0.0f;
run_control_loop([this](){
// Note that all estimators are updated in the loop prefix in run_control_loop
float torque_setpoint;
if (!controller_.update(&torque_setpoint))
return error_ |= ERROR_CONTROLLER_FAILED, false;
float phase_vel = (2*M_PI) * encoder_.vel_estimate_ * motor_.config_.pole_pairs;
if (!motor_.update(torque_setpoint, encoder_.phase_, phase_vel))
return false; // set_error should update axis.error_
return !min_endstop_.get_state();
});
error_ &= ~ERROR_MIN_ENDSTOP_PRESSED; // clear this error since we deliberately drove into the endstop
// pos_setpoint is the starting position for the trap_traj so we need to set it.
controller_.pos_setpoint_ = min_endstop_.config_.offset;
controller_.vel_setpoint_ = 0.0f; // Change directions without decelerating
// Set our current position in encoder counts to make control more logical
encoder_.set_linear_count((int32_t)controller_.pos_setpoint_);
controller_.config_.control_mode = Controller::CONTROL_MODE_POSITION_CONTROL;
controller_.config_.input_mode = Controller::INPUT_MODE_TRAP_TRAJ;
controller_.input_pos_ = 0.0f;
controller_.input_pos_updated();
controller_.input_vel_ = 0.0f;
controller_.input_torque_ = 0.0f;
run_control_loop([this](){
// Note that all estimators are updated in the loop prefix in run_control_loop
float torque_setpoint;
if (!controller_.update(&torque_setpoint))
return error_ |= ERROR_CONTROLLER_FAILED, false;
float phase_vel = (2*M_PI) * encoder_.vel_estimate_ * motor_.config_.pole_pairs;
if (!motor_.update(torque_setpoint, encoder_.phase_, phase_vel))
return false; // set_error should update axis.error_
return !controller_.trajectory_done_;
});
controller_.config_.control_mode = stored_control_mode;
controller_.config_.input_mode = stored_input_mode;
homing_.is_homed = true;
return check_for_errors();
}
@@ -309,7 +460,8 @@ bool Axis::run_idle_loop() {
// run_control_loop ignores missed modulation timing updates
// if and only if we're in AXIS_STATE_IDLE
safety_critical_disarm_motor_pwm(motor_);
run_control_loop([this](){
set_step_dir_active(config_.enable_step_dir && config_.step_dir_always_on);
run_control_loop([this]() {
return true;
});
return check_for_errors();
@@ -318,20 +470,9 @@ bool Axis::run_idle_loop() {
// Infinite loop that does calibration and enters main control loop as appropriate
void Axis::run_state_machine_loop() {
// Allocate the map for anti-cogging algorithm and initialize all values to 0.0f
// TODO: Move this somewhere else
// TODO: respect changes of CPR
int encoder_cpr = encoder_.config_.cpr;
controller_.anticogging_.cogging_map = (float*)malloc(encoder_cpr * sizeof(float));
if (controller_.anticogging_.cogging_map != NULL) {
for (int i = 0; i < encoder_cpr; i++) {
controller_.anticogging_.cogging_map[i] = 0.0f;
}
}
// arm!
motor_.arm();
for (;;) {
// Load the task chain if a specific request is pending
if (requested_state_ != AXIS_STATE_UNDEFINED) {
@@ -343,6 +484,8 @@ void Axis::run_state_machine_loop() {
task_chain_[pos++] = AXIS_STATE_ENCODER_INDEX_SEARCH;
if (config_.startup_encoder_offset_calibration)
task_chain_[pos++] = AXIS_STATE_ENCODER_OFFSET_CALIBRATION;
if (config_.startup_homing)
task_chain_[pos++] = AXIS_STATE_HOMING;
if (config_.startup_closed_loop_control)
task_chain_[pos++] = AXIS_STATE_CLOSED_LOOP_CONTROL;
else if (config_.startup_sensorless_control)
@@ -358,7 +501,7 @@ void Axis::run_state_machine_loop() {
task_chain_[pos++] = requested_state_;
task_chain_[pos++] = AXIS_STATE_IDLE;
}
task_chain_[pos++] = AXIS_STATE_UNDEFINED; // TODO: bounds checking
task_chain_[pos++] = AXIS_STATE_UNDEFINED; // TODO: bounds checking
requested_state_ = AXIS_STATE_UNDEFINED;
// Auto-clear any invalid state error
error_ &= ~ERROR_INVALID_STATE;
@@ -390,6 +533,10 @@ void Axis::run_state_machine_loop() {
status = encoder_.run_direction_find();
} break;
case AXIS_STATE_HOMING: {
status = run_homing();
} break;
case AXIS_STATE_ENCODER_OFFSET_CALIBRATION: {
if (!motor_.is_calibrated_)
goto invalid_state_label;
@@ -399,7 +546,7 @@ void Axis::run_state_machine_loop() {
case AXIS_STATE_LOCKIN_SPIN: {
if (!motor_.is_calibrated_ || motor_.config_.direction==0)
goto invalid_state_label;
status = run_lockin_spin(config_.lockin);
status = run_lockin_spin(config_.general_lockin);
} break;
case AXIS_STATE_SENSORLESS_CONTROL: {
@@ -419,6 +566,7 @@ void Axis::run_state_machine_loop() {
goto invalid_state_label;
if (!encoder_.is_ready_)
goto invalid_state_label;
watchdog_feed();
status = run_closed_loop_control_loop();
} break;
@@ -430,14 +578,17 @@ void Axis::run_state_machine_loop() {
default:
invalid_state_label:
error_ |= ERROR_INVALID_STATE;
status = false; // this will set the state to idle
status = false; // this will set the state to idle
break;
}
// If the state failed, go to idle, else advance task chain
if (!status)
if (!status) {
std::fill(task_chain_.begin(), task_chain_.end(), AXIS_STATE_UNDEFINED);
current_state_ = AXIS_STATE_IDLE;
else
memcpy(task_chain_, task_chain_ + 1, sizeof(task_chain_) - sizeof(task_chain_[0]));
} else {
std::rotate(task_chain_.begin(), task_chain_.begin() + 1, task_chain_.end());
task_chain_.back() = AXIS_STATE_UNDEFINED;
}
}
}
Firmware/MotorControl/axis.hpp
+64 -114
View File
@@ -5,38 +5,10 @@
#error "This file should not be included directly. Include odrive_main.h instead."
#endif
class Axis {
#include <array>
class Axis : public ODriveIntf::AxisIntf {
public:
enum Error_t {
ERROR_NONE = 0x00,
ERROR_INVALID_STATE = 0x01, //<! an invalid state was requested
ERROR_DC_BUS_UNDER_VOLTAGE = 0x02,
ERROR_DC_BUS_OVER_VOLTAGE = 0x04,
ERROR_CURRENT_MEASUREMENT_TIMEOUT = 0x08,
ERROR_BRAKE_RESISTOR_DISARMED = 0x10, //<! the brake resistor was unexpectedly disarmed
ERROR_MOTOR_DISARMED = 0x20, //<! the motor was unexpectedly disarmed
ERROR_MOTOR_FAILED = 0x40, // Go to motor.hpp for information, check odrvX.axisX.motor.error for error value
ERROR_SENSORLESS_ESTIMATOR_FAILED = 0x80,
ERROR_ENCODER_FAILED = 0x100, // Go to encoder.hpp for information, check odrvX.axisX.encoder.error for error value
ERROR_CONTROLLER_FAILED = 0x200,
ERROR_POS_CTRL_DURING_SENSORLESS = 0x400,
ERROR_WATCHDOG_TIMER_EXPIRED = 0x800,
};
enum State_t {
AXIS_STATE_UNDEFINED = 0, //<! will fall through to idle
AXIS_STATE_IDLE = 1, //<! disable PWM and do nothing
AXIS_STATE_STARTUP_SEQUENCE = 2, //<! the actual sequence is defined by the config.startup_... flags
AXIS_STATE_FULL_CALIBRATION_SEQUENCE = 3, //<! run all calibration procedures, then idle
AXIS_STATE_MOTOR_CALIBRATION = 4, //<! run motor calibration
AXIS_STATE_SENSORLESS_CONTROL = 5, //<! run sensorless control
AXIS_STATE_ENCODER_INDEX_SEARCH = 6, //<! run encoder index search
AXIS_STATE_ENCODER_OFFSET_CALIBRATION = 7, //<! run encoder offset calibration
AXIS_STATE_CLOSED_LOOP_CONTROL = 8, //<! run closed loop control
AXIS_STATE_LOCKIN_SPIN = 9, //<! run lockin spin
AXIS_STATE_ENCODER_DIR_FIND = 10,
};
struct LockinConfig_t {
float current = 10.0f; // [A]
float ramp_time = 0.4f; // [s]
@@ -60,11 +32,19 @@ public:
bool startup_encoder_offset_calibration = false; //<! run encoder offset calibration after startup, skip otherwise
bool startup_closed_loop_control = false; //<! enable closed loop control after calibration/startup
bool startup_sensorless_control = false; //<! enable sensorless control after calibration/startup
bool startup_homing = false; //<! enable homing after calibration/startup
bool enable_step_dir = false; //<! enable step/dir input after calibration
// For M0 this has no effect if enable_uart is true
float counts_per_step = 2.0f;
bool step_dir_always_on = false; //<! Keep step/dir enabled while the motor is disabled.
//<! This is ignored if enable_step_dir is false.
//<! This setting only takes effect on a state transition
//<! into idle or out of closed loop control.
float watchdog_timeout = 0.0f; // [s] (0 disables watchdog)
float turns_per_step = 1.0f / 1024.0f;
float watchdog_timeout = 0.0f; // [s]
bool enable_watchdog = false;
// Defaults loaded from hw_config in load_configuration in main.cpp
uint16_t step_gpio_pin = 0;
@@ -72,28 +52,37 @@ public:
LockinConfig_t calibration_lockin = default_calibration();
LockinConfig_t sensorless_ramp = default_sensorless();
LockinConfig_t lockin;
LockinConfig_t general_lockin;
uint32_t can_node_id = 0; // Both axes will have the same id to start
bool can_node_id_extended = false;
uint32_t can_heartbeat_rate_ms = 100;
// custom setters
Axis* parent = nullptr;
void set_step_gpio_pin(uint16_t value) { step_gpio_pin = value; parent->decode_step_dir_pins(); }
void set_dir_gpio_pin(uint16_t value) { dir_gpio_pin = value; parent->decode_step_dir_pins(); }
};
struct Homing_t {
bool is_homed = false;
};
enum thread_signals {
M_SIGNAL_PH_CURRENT_MEAS = 1u << 0
};
enum LockinState_t {
LOCKIN_STATE_INACTIVE,
LOCKIN_STATE_RAMP,
LOCKIN_STATE_ACCELERATE,
LOCKIN_STATE_CONST_VEL,
};
Axis(int axis_num,
const AxisHardwareConfig_t& hw_config,
Config_t& config,
Encoder& encoder,
SensorlessEstimator& sensorless_estimator,
Controller& controller,
OnboardThermistorCurrentLimiter& fet_thermistor,
OffboardThermistorCurrentLimiter& motor_thermistor,
Motor& motor,
TrapezoidalTrajectory& trap);
TrapezoidalTrajectory& trap,
Endstop& min_endstop,
Endstop& max_endstop);
void setup();
void start_thread();
@@ -103,10 +92,10 @@ public:
void step_cb();
void set_step_dir_active(bool enable);
void decode_step_dir_pins();
void update_watchdog_settings();
static void load_default_step_dir_pin_config(
const AxisHardwareConfig_t& hw_config, Config_t* config);
static void load_default_can_id(const int& id, Config_t& config);
bool check_DRV_fault();
bool check_PSU_brownout();
@@ -116,6 +105,15 @@ public:
void watchdog_feed();
bool watchdog_check();
void clear_errors() {
motor_.error_ = Motor::ERROR_NONE;
controller_.error_ = Controller::ERROR_NONE;
sensorless_estimator_.error_ = SensorlessEstimator::ERROR_NONE;
encoder_.error_ = Encoder::ERROR_NONE;
encoder_.spi_error_rate_ = 0.0f;
error_ = ERROR_NONE;
}
// True if there are no errors
bool inline check_for_errors() {
@@ -186,8 +184,13 @@ public:
bool run_lockin_spin(const LockinConfig_t &lockin_config);
bool run_sensorless_control_loop();
bool run_closed_loop_control_loop();
bool run_homing();
bool run_idle_loop();
constexpr uint32_t get_watchdog_reset() {
return static_cast<uint32_t>(std::clamp<float>(config_.watchdog_timeout, 0, UINT32_MAX / (current_meas_hz + 1)) * current_meas_hz);
}
void run_state_machine_loop();
int axis_num_;
@@ -197,14 +200,24 @@ public:
Encoder& encoder_;
SensorlessEstimator& sensorless_estimator_;
Controller& controller_;
OnboardThermistorCurrentLimiter& fet_thermistor_;
OffboardThermistorCurrentLimiter& motor_thermistor_;
Motor& motor_;
TrapezoidalTrajectory& trap_;
TrapezoidalTrajectory& trap_traj_;
Endstop& min_endstop_;
Endstop& max_endstop_;
// List of current_limiters and thermistors to
// provide easy iteration.
std::array<CurrentLimiter*, 2> current_limiters_;
std::array<ThermistorCurrentLimiter*, 2> thermistors_;
osThreadId thread_id_;
const uint32_t stack_size_ = 2048; // Bytes
volatile bool thread_id_valid_ = false;
// variables exposed on protocol
Error_t error_ = ERROR_NONE;
Error error_ = ERROR_NONE;
bool step_dir_active_ = false; // auto enabled after calibration, based on config.enable_step_dir
// updated from config in constructor, and on protocol hook
@@ -213,80 +226,17 @@ public:
GPIO_TypeDef* dir_port_;
uint16_t dir_pin_;
State_t requested_state_ = AXIS_STATE_STARTUP_SEQUENCE;
State_t task_chain_[10] = { AXIS_STATE_UNDEFINED };
State_t& current_state_ = task_chain_[0];
AxisState requested_state_ = AXIS_STATE_STARTUP_SEQUENCE;
std::array<AxisState, 10> task_chain_ = { AXIS_STATE_UNDEFINED };
AxisState& current_state_ = task_chain_.front();
uint32_t loop_counter_ = 0;
LockinState_t lockin_state_ = LOCKIN_STATE_INACTIVE;
LockinState lockin_state_ = LOCKIN_STATE_INACTIVE;
Homing_t homing_;
uint32_t last_heartbeat_ = 0;
// watchdog
uint32_t watchdog_reset_value_ = 0; //computed from config_.watchdog_timeout in update_watchdog_settings()
uint32_t watchdog_current_value_= 0;
// Communication protocol definitions
auto make_protocol_definitions() {
return make_protocol_member_list(
make_protocol_property("error", &error_),
make_protocol_ro_property("step_dir_active", &step_dir_active_),
make_protocol_ro_property("current_state", &current_state_),
make_protocol_property("requested_state", &requested_state_),
make_protocol_ro_property("loop_counter", &loop_counter_),
make_protocol_ro_property("lockin_state", &lockin_state_),
make_protocol_object("config",
make_protocol_property("startup_motor_calibration", &config_.startup_motor_calibration),
make_protocol_property("startup_encoder_index_search", &config_.startup_encoder_index_search),
make_protocol_property("startup_encoder_offset_calibration", &config_.startup_encoder_offset_calibration),
make_protocol_property("startup_closed_loop_control", &config_.startup_closed_loop_control),
make_protocol_property("startup_sensorless_control", &config_.startup_sensorless_control),
make_protocol_property("enable_step_dir", &config_.enable_step_dir),
make_protocol_property("counts_per_step", &config_.counts_per_step),
make_protocol_property("watchdog_timeout", &config_.watchdog_timeout,
[](void* ctx) { static_cast<Axis*>(ctx)->update_watchdog_settings(); }, this),
make_protocol_property("step_gpio_pin", &config_.step_gpio_pin,
[](void* ctx) { static_cast<Axis*>(ctx)->decode_step_dir_pins(); }, this),
make_protocol_property("dir_gpio_pin", &config_.dir_gpio_pin,
[](void* ctx) { static_cast<Axis*>(ctx)->decode_step_dir_pins(); }, this),
make_protocol_object("calibration_lockin",
make_protocol_property("current", &config_.calibration_lockin.current),
make_protocol_property("ramp_time", &config_.calibration_lockin.ramp_time),
make_protocol_property("ramp_distance", &config_.calibration_lockin.ramp_distance),
make_protocol_property("accel", &config_.calibration_lockin.accel),
make_protocol_property("vel", &config_.calibration_lockin.vel)
),
make_protocol_object("sensorless_ramp",
make_protocol_property("current", &config_.sensorless_ramp.current),
make_protocol_property("ramp_time", &config_.sensorless_ramp.ramp_time),
make_protocol_property("ramp_distance", &config_.sensorless_ramp.ramp_distance),
make_protocol_property("accel", &config_.sensorless_ramp.accel),
make_protocol_property("vel", &config_.sensorless_ramp.vel),
make_protocol_property("finish_distance", &config_.sensorless_ramp.finish_distance),
make_protocol_property("finish_on_vel", &config_.sensorless_ramp.finish_on_vel),
make_protocol_property("finish_on_distance", &config_.sensorless_ramp.finish_on_distance),
make_protocol_property("finish_on_enc_idx", &config_.sensorless_ramp.finish_on_enc_idx)
),
make_protocol_object("general_lockin",
make_protocol_property("current", &config_.lockin.current),
make_protocol_property("ramp_time", &config_.lockin.ramp_time),
make_protocol_property("ramp_distance", &config_.lockin.ramp_distance),
make_protocol_property("accel", &config_.lockin.accel),
make_protocol_property("vel", &config_.lockin.vel),
make_protocol_property("finish_distance", &config_.lockin.finish_distance),
make_protocol_property("finish_on_vel", &config_.lockin.finish_on_vel),
make_protocol_property("finish_on_distance", &config_.lockin.finish_on_distance),
make_protocol_property("finish_on_enc_idx", &config_.lockin.finish_on_enc_idx)
)
),
make_protocol_object("motor", motor_.make_protocol_definitions()),
make_protocol_object("controller", controller_.make_protocol_definitions()),
make_protocol_object("encoder", encoder_.make_protocol_definitions()),
make_protocol_object("sensorless_estimator", sensorless_estimator_.make_protocol_definitions()),
make_protocol_object("trap_traj", trap_.make_protocol_definitions()),
make_protocol_function("watchdog_feed", *this, &Axis::watchdog_feed)
);
}
};
DEFINE_ENUM_FLAG_OPERATORS(Axis::Error_t)
#endif /* __AXIS_HPP */
Firmware/MotorControl/board_config_v3.h
+22 -8
View File
@@ -36,13 +36,18 @@ typedef struct {
uint16_t hallB_pin;
GPIO_TypeDef* hallC_port;
uint16_t hallC_pin;
SPI_HandleTypeDef* spi;
} EncoderHardwareConfig_t;
typedef struct {
TIM_HandleTypeDef* timer;
uint16_t control_deadline;
float shunt_conductance;
size_t inverter_thermistor_adc_ch;
} MotorHardwareConfig_t;
typedef struct {
const float* const coeffs;
size_t num_coeffs;
size_t adc_ch;
} ThermistorHardwareConfig_t;
typedef struct {
SPI_HandleTypeDef* spi;
GPIO_TypeDef* enable_port;
@@ -56,18 +61,17 @@ typedef struct {
AxisHardwareConfig_t axis_config;
EncoderHardwareConfig_t encoder_config;
MotorHardwareConfig_t motor_config;
ThermistorHardwareConfig_t thermistor_config;
GateDriverHardwareConfig_t gate_driver_config;
} BoardHardwareConfig_t;
extern const BoardHardwareConfig_t hw_configs[2];
extern const float thermistor_poly_coeffs[];
extern const size_t thermistor_num_coeffs;
//TODO stick this in a C file
#ifdef __MAIN_CPP__
const float thermistor_poly_coeffs[] =
const float fet_thermistor_poly_coeffs[] =
{363.93910201f, -462.15369634f, 307.55129571f, -27.72569531f};
const size_t thermistor_num_coeffs = sizeof(thermistor_poly_coeffs)/sizeof(thermistor_poly_coeffs[1]);
const size_t fet_thermistor_num_coeffs = sizeof(fet_thermistor_poly_coeffs)/sizeof(fet_thermistor_poly_coeffs[1]);
const BoardHardwareConfig_t hw_configs[2] = { {
//M0
@@ -86,12 +90,17 @@ const BoardHardwareConfig_t hw_configs[2] = { {
.hallB_pin = M0_ENC_B_Pin,
.hallC_port = M0_ENC_Z_GPIO_Port,
.hallC_pin = M0_ENC_Z_Pin,
.spi = &hspi3,
},
.motor_config = {
.timer = &htim1,
.control_deadline = TIM_1_8_PERIOD_CLOCKS,
.shunt_conductance = 1.0f / SHUNT_RESISTANCE, //[S]
.inverter_thermistor_adc_ch = 15,
},
.thermistor_config = {
.coeffs = &fet_thermistor_poly_coeffs[0],
.num_coeffs = fet_thermistor_num_coeffs,
.adc_ch = 15,
},
.gate_driver_config = {
.spi = &hspi3,
@@ -125,15 +134,20 @@ const BoardHardwareConfig_t hw_configs[2] = { {
.hallB_pin = M1_ENC_B_Pin,
.hallC_port = M1_ENC_Z_GPIO_Port,
.hallC_pin = M1_ENC_Z_Pin,
.spi = &hspi3,
},
.motor_config = {
.timer = &htim8,
.control_deadline = (3 * TIM_1_8_PERIOD_CLOCKS) / 2,
.shunt_conductance = 1.0f / SHUNT_RESISTANCE, //[S]
},
.thermistor_config = {
.coeffs = &fet_thermistor_poly_coeffs[0],
.num_coeffs = fet_thermistor_num_coeffs,
#if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 3
.inverter_thermistor_adc_ch = 4,
.adc_ch = 4,
#else
.inverter_thermistor_adc_ch = 1,
.adc_ch = 1,
#endif
},
.gate_driver_config = {
Firmware/MotorControl/controller.cpp
+246 -121
View File
@@ -1,19 +1,23 @@
#include "odrive_main.h"
#include <algorithm>
#include <algorithm>
Controller::Controller(Config_t& config) :
config_(config)
{}
{
update_filter_gains();
}
void Controller::reset() {
pos_setpoint_ = 0.0f;
vel_setpoint_ = 0.0f;
vel_integrator_current_ = 0.0f;
current_setpoint_ = 0.0f;
vel_integrator_torque_ = 0.0f;
torque_setpoint_ = 0.0f;
}
void Controller::set_error(Error_t error) {
void Controller::set_error(Error error) {
error_ |= error;
axis_->error_ |= Axis::ERROR_CONTROLLER_FAILED;
}
@@ -22,58 +26,49 @@ void Controller::set_error(Error_t error) {
// Command Handling
//--------------------------------
void Controller::set_pos_setpoint(float pos_setpoint, float vel_feed_forward, float current_feed_forward) {
pos_setpoint_ = pos_setpoint;
vel_setpoint_ = vel_feed_forward;
current_setpoint_ = current_feed_forward;
config_.control_mode = CTRL_MODE_POSITION_CONTROL;
#ifdef DEBUG_PRINT
printf("POSITION_CONTROL %6.0f %3.3f %3.3f\n", pos_setpoint, vel_setpoint_, current_setpoint_);
#endif
}
void Controller::set_vel_setpoint(float vel_setpoint, float current_feed_forward) {
vel_setpoint_ = vel_setpoint;
current_setpoint_ = current_feed_forward;
config_.control_mode = CTRL_MODE_VELOCITY_CONTROL;
#ifdef DEBUG_PRINT
printf("VELOCITY_CONTROL %3.3f %3.3f\n", vel_setpoint_, motor->current_setpoint_);
#endif
}
void Controller::set_current_setpoint(float current_setpoint) {
current_setpoint_ = current_setpoint;
config_.control_mode = CTRL_MODE_CURRENT_CONTROL;
#ifdef DEBUG_PRINT
printf("CURRENT_CONTROL %3.3f\n", current_setpoint_);
#endif
bool Controller::select_encoder(size_t encoder_num) {
if (encoder_num < AXIS_COUNT) {
Axis* ax = axes[encoder_num];
pos_estimate_circular_src_ = &ax->encoder_.pos_circular_;
pos_wrap_src_ = &config_.circular_setpoint_range;
pos_estimate_linear_src_ = &ax->encoder_.pos_estimate_;
pos_estimate_valid_src_ = &ax->encoder_.pos_estimate_valid_;
vel_estimate_src_ = &ax->encoder_.vel_estimate_;
vel_estimate_valid_src_ = &ax->encoder_.vel_estimate_valid_;
return true;
} else {
return set_error(Controller::ERROR_INVALID_LOAD_ENCODER), false;
}
}
void Controller::move_to_pos(float goal_point) {
axis_->trap_.planTrapezoidal(goal_point, pos_setpoint_, vel_setpoint_,
axis_->trap_.config_.vel_limit,
axis_->trap_.config_.accel_limit,
axis_->trap_.config_.decel_limit);
traj_start_loop_count_ = axis_->loop_counter_;
config_.control_mode = CTRL_MODE_TRAJECTORY_CONTROL;
goal_point_ = goal_point;
axis_->trap_traj_.planTrapezoidal(goal_point, pos_setpoint_, vel_setpoint_,
axis_->trap_traj_.config_.vel_limit,
axis_->trap_traj_.config_.accel_limit,
axis_->trap_traj_.config_.decel_limit);
axis_->trap_traj_.t_ = 0.0f;
trajectory_done_ = false;
}
void Controller::move_incremental(float displacement, bool from_goal_point = true){
if(from_goal_point){
move_to_pos(goal_point_ + displacement);
void Controller::move_incremental(float displacement, bool from_input_pos = true){
if(from_input_pos){
input_pos_ += displacement;
} else{
move_to_pos(pos_setpoint_ + displacement);
input_pos_ = pos_setpoint_ + displacement;
}
input_pos_updated();
}
void Controller::start_anticogging_calibration() {
// Ensure the cogging map was correctly allocated earlier and that the motor is capable of calibrating
if (anticogging_.cogging_map != NULL && axis_->error_ == Axis::ERROR_NONE) {
anticogging_.calib_anticogging = true;
if (axis_->error_ == Axis::ERROR_NONE) {
config_.anticogging.calib_anticogging = true;
}
}
/*
* This anti-cogging implementation iterates through each encoder position,
* waits for zero velocity & position error,
@@ -82,140 +77,270 @@ void Controller::start_anticogging_calibration() {
* This holding current is added as a feedforward term in the control loop.
*/
bool Controller::anticogging_calibration(float pos_estimate, float vel_estimate) {
if (anticogging_.calib_anticogging && anticogging_.cogging_map != NULL) {
float pos_err = anticogging_.index - pos_estimate;
if (fabsf(pos_err) <= anticogging_.calib_pos_threshold &&
fabsf(vel_estimate) < anticogging_.calib_vel_threshold) {
anticogging_.cogging_map[anticogging_.index++] = vel_integrator_current_;
}
if (anticogging_.index < axis_->encoder_.config_.cpr) { // TODO: remove the dependency on encoder CPR
set_pos_setpoint(anticogging_.index, 0.0f, 0.0f);
return false;
} else {
anticogging_.index = 0;
set_pos_setpoint(0.0f, 0.0f, 0.0f); // Send the motor home
anticogging_.use_anticogging = true; // We're good to go, enable anti-cogging
anticogging_.calib_anticogging = false;
return true;
}
float pos_err = input_pos_ - pos_estimate;
if (std::abs(pos_err) <= config_.anticogging.calib_pos_threshold / (float)axis_->encoder_.config_.cpr &&
std::abs(vel_estimate) < config_.anticogging.calib_vel_threshold / (float)axis_->encoder_.config_.cpr) {
config_.anticogging.cogging_map[std::clamp<uint32_t>(config_.anticogging.index++, 0, 3600)] = vel_integrator_torque_;
}
if (config_.anticogging.index < 3600) {
config_.control_mode = CONTROL_MODE_POSITION_CONTROL;
input_pos_ = config_.anticogging.index * axis_->encoder_.getCoggingRatio();
input_vel_ = 0.0f;
input_torque_ = 0.0f;
input_pos_updated();
return false;
} else {
config_.anticogging.index = 0;
config_.control_mode = CONTROL_MODE_POSITION_CONTROL;
input_pos_ = 0.0f; // Send the motor home
input_vel_ = 0.0f;
input_torque_ = 0.0f;
input_pos_updated();
anticogging_valid_ = true;
config_.anticogging.calib_anticogging = false;
return true;
}
return false;
}
bool Controller::update(float pos_estimate, float vel_estimate, float* current_setpoint_output) {
// Only runs if anticogging_.calib_anticogging is true; non-blocking
anticogging_calibration(pos_estimate, vel_estimate);
float anticogging_pos = pos_estimate;
void Controller::update_filter_gains() {
float bandwidth = std::min(config_.input_filter_bandwidth, 0.25f * current_meas_hz);
input_filter_ki_ = 2.0f * bandwidth; // basic conversion to discrete time
input_filter_kp_ = 0.25f * (input_filter_ki_ * input_filter_ki_); // Critically damped
}
// Trajectory control
if (config_.control_mode == CTRL_MODE_TRAJECTORY_CONTROL) {
// Note: uint32_t loop count delta is OK across overflow
// Beware of negative deltas, as they will not be well behaved due to uint!
float t = (axis_->loop_counter_ - traj_start_loop_count_) * current_meas_period;
if (t > axis_->trap_.Tf_) {
// Drop into position control mode when done to avoid problems on loop counter delta overflow
config_.control_mode = CTRL_MODE_POSITION_CONTROL;
// pos_setpoint already set by trajectory
vel_setpoint_ = 0.0f;
current_setpoint_ = 0.0f;
} else {
TrapezoidalTrajectory::Step_t traj_step = axis_->trap_.eval(t);
pos_setpoint_ = traj_step.Y;
vel_setpoint_ = traj_step.Yd;
current_setpoint_ = traj_step.Ydd * axis_->trap_.config_.A_per_css;
static float limitVel(const float vel_limit, const float vel_estimate, const float vel_gain, const float torque) {
float Tmax = (vel_limit - vel_estimate) * vel_gain;
float Tmin = (-vel_limit - vel_estimate) * vel_gain;
return std::clamp(torque, Tmin, Tmax);
}
bool Controller::update(float* torque_setpoint_output) {
float* pos_estimate_linear = (pos_estimate_valid_src_ && *pos_estimate_valid_src_)
? pos_estimate_linear_src_ : nullptr;
float* pos_estimate_circular = (pos_estimate_valid_src_ && *pos_estimate_valid_src_)
? pos_estimate_circular_src_ : nullptr;
float* vel_estimate_src = (vel_estimate_valid_src_ && *vel_estimate_valid_src_)
? vel_estimate_src_ : nullptr;
// Calib_anticogging is only true when calibration is occurring, so we can't block anticogging_pos
float anticogging_pos = axis_->encoder_.pos_estimate_ / axis_->encoder_.getCoggingRatio();
if (config_.anticogging.calib_anticogging) {
if (!axis_->encoder_.pos_estimate_valid_ || !axis_->encoder_.vel_estimate_valid_) {
set_error(ERROR_INVALID_ESTIMATE);
return false;
}
anticogging_pos = pos_setpoint_; // FF the position setpoint instead of the pos_estimate
// non-blocking
anticogging_calibration(axis_->encoder_.pos_estimate_, axis_->encoder_.vel_estimate_);
}
// Ramp rate limited velocity setpoint
if (config_.control_mode == CTRL_MODE_VELOCITY_CONTROL && vel_ramp_enable_) {
float max_step_size = current_meas_period * config_.vel_ramp_rate;
float full_step = vel_ramp_target_ - vel_setpoint_;
float step;
if (fabsf(full_step) > max_step_size) {
step = std::copysignf(max_step_size, full_step);
} else {
step = full_step;
// TODO also enable circular deltas for 2nd order filter, etc.
if (config_.circular_setpoints) {
// Keep pos setpoint from drifting
input_pos_ = fmodf_pos(input_pos_, config_.circular_setpoint_range);
}
// Update inputs
switch (config_.input_mode) {
case INPUT_MODE_INACTIVE: {
// do nothing
} break;
case INPUT_MODE_PASSTHROUGH: {
pos_setpoint_ = input_pos_;
vel_setpoint_ = input_vel_;
torque_setpoint_ = input_torque_;
} break;
case INPUT_MODE_VEL_RAMP: {
float max_step_size = std::abs(current_meas_period * config_.vel_ramp_rate);
float full_step = input_vel_ - vel_setpoint_;
float step = std::clamp(full_step, -max_step_size, max_step_size);
vel_setpoint_ += step;
torque_setpoint_ = (step / current_meas_period) * config_.inertia;
} break;
case INPUT_MODE_TORQUE_RAMP: {
float max_step_size = std::abs(current_meas_period * config_.torque_ramp_rate);
float full_step = input_torque_ - torque_setpoint_;
float step = std::clamp(full_step, -max_step_size, max_step_size);
torque_setpoint_ += step;
} break;
case INPUT_MODE_POS_FILTER: {
// 2nd order pos tracking filter
float delta_pos = input_pos_ - pos_setpoint_; // Pos error
float delta_vel = input_vel_ - vel_setpoint_; // Vel error
float accel = input_filter_kp_*delta_pos + input_filter_ki_*delta_vel; // Feedback
torque_setpoint_ = accel * config_.inertia; // Accel
vel_setpoint_ += current_meas_period * accel; // delta vel
pos_setpoint_ += current_meas_period * vel_setpoint_; // Delta pos
} break;
case INPUT_MODE_MIRROR: {
if (config_.axis_to_mirror < AXIS_COUNT) {
pos_setpoint_ = axes[config_.axis_to_mirror]->encoder_.pos_estimate_ * config_.mirror_ratio;
vel_setpoint_ = axes[config_.axis_to_mirror]->encoder_.vel_estimate_ * config_.mirror_ratio;
} else {
set_error(ERROR_INVALID_MIRROR_AXIS);
return false;
}
} break;
// case INPUT_MODE_MIX_CHANNELS: {
// // NOT YET IMPLEMENTED
// } break;
case INPUT_MODE_TRAP_TRAJ: {
if(input_pos_updated_){
move_to_pos(input_pos_);
input_pos_updated_ = false;
}
// Avoid updating uninitialized trajectory
if (trajectory_done_)
break;
if (axis_->trap_traj_.t_ > axis_->trap_traj_.Tf_) {
// Drop into position control mode when done to avoid problems on loop counter delta overflow
config_.control_mode = CONTROL_MODE_POSITION_CONTROL;
pos_setpoint_ = input_pos_;
vel_setpoint_ = 0.0f;
torque_setpoint_ = 0.0f;
trajectory_done_ = true;
} else {
TrapezoidalTrajectory::Step_t traj_step = axis_->trap_traj_.eval(axis_->trap_traj_.t_);
pos_setpoint_ = traj_step.Y;
vel_setpoint_ = traj_step.Yd;
torque_setpoint_ = traj_step.Ydd * config_.inertia;
axis_->trap_traj_.t_ += current_meas_period;
}
anticogging_pos = pos_setpoint_; // FF the position setpoint instead of the pos_estimate
} break;
default: {
set_error(ERROR_INVALID_INPUT_MODE);
return false;
}
vel_setpoint_ += step;
}
// Position control
// TODO Decide if we want to use encoder or pll position here
float gain_scheduling_multiplier = 1.0f;
float vel_des = vel_setpoint_;
if (config_.control_mode >= CTRL_MODE_POSITION_CONTROL) {
if (config_.control_mode >= CONTROL_MODE_POSITION_CONTROL) {
float pos_err;
if (config_.setpoints_in_cpr) {
// TODO this breaks the semantics that estimates come in on the arguments.
// It's probably better to call a get_estimate that will arbitrate (enc vs sensorless) instead.
float cpr = (float)(axis_->encoder_.config_.cpr);
if (config_.circular_setpoints) {
if(!pos_estimate_circular) {
set_error(ERROR_INVALID_ESTIMATE);
return false;
}
// Keep pos setpoint from drifting
pos_setpoint_ = fmodf_pos(pos_setpoint_, cpr);
pos_setpoint_ = fmodf_pos(pos_setpoint_, *pos_wrap_src_);
// Circular delta
pos_err = pos_setpoint_ - axis_->encoder_.pos_cpr_;
pos_err = wrap_pm(pos_err, 0.5f * cpr);
pos_err = pos_setpoint_ - *pos_estimate_circular;
pos_err = wrap_pm(pos_err, 0.5f * *pos_wrap_src_);
} else {
pos_err = pos_setpoint_ - pos_estimate;
if(!pos_estimate_linear) {
set_error(ERROR_INVALID_ESTIMATE);
return false;
}
pos_err = pos_setpoint_ - *pos_estimate_linear;
}
vel_des += config_.pos_gain * pos_err;
// V-shaped gain shedule based on position error
float abs_pos_err = std::abs(pos_err);
if (config_.enable_gain_scheduling && abs_pos_err <= config_.gain_scheduling_width) {
gain_scheduling_multiplier = abs_pos_err / config_.gain_scheduling_width;
}
}
// Velocity limiting
float vel_lim = config_.vel_limit;
if (vel_des > vel_lim) vel_des = vel_lim;
if (vel_des < -vel_lim) vel_des = -vel_lim;
if (config_.enable_vel_limit) {
vel_des = std::clamp(vel_des, -vel_lim, vel_lim);
}
// Check for overspeed fault (done in this module (controller) for cohesion with vel_lim)
if (config_.vel_limit_tolerance > 0.0f) { // 0.0f to disable
if (fabsf(vel_estimate) > config_.vel_limit_tolerance * vel_lim) {
if (config_.enable_overspeed_error) { // 0.0f to disable
if (!vel_estimate_src) {
set_error(ERROR_INVALID_ESTIMATE);
return false;
}
if (std::abs(*vel_estimate_src) > config_.vel_limit_tolerance * vel_lim) {
set_error(ERROR_OVERSPEED);
return false;
}
}
// TODO: Change to controller working in torque units
// Torque per amp gain scheduling (ACIM)
float vel_gain = config_.vel_gain;
float vel_integrator_gain = config_.vel_integrator_gain;
if (axis_->motor_.config_.motor_type == Motor::MOTOR_TYPE_ACIM) {
float effective_flux = axis_->motor_.current_control_.acim_rotor_flux;
float minflux = axis_->motor_.config_.acim_gain_min_flux;
if (fabsf(effective_flux) < minflux)
effective_flux = std::copysignf(minflux, effective_flux);
vel_gain /= effective_flux;
vel_integrator_gain /= effective_flux;
// TODO: also scale the integral value which is also changing units.
// (or again just do control in torque units)
}
// Velocity control
float Iq = current_setpoint_;
float torque = torque_setpoint_;
// Anti-cogging is enabled after calibration
// We get the current position and apply a current feed-forward
// ensuring that we handle negative encoder positions properly (-1 == motor->encoder.encoder_cpr - 1)
if (anticogging_.use_anticogging) {
Iq += anticogging_.cogging_map[mod(static_cast<int>(anticogging_pos), axis_->encoder_.config_.cpr)];
if (anticogging_valid_ && config_.anticogging.anticogging_enabled) {
torque += config_.anticogging.cogging_map[std::clamp(mod((int)anticogging_pos, 3600), 0, 3600)];
}
float v_err = vel_des - vel_estimate;
if (config_.control_mode >= CTRL_MODE_VELOCITY_CONTROL) {
Iq += config_.vel_gain * v_err;
float v_err = 0.0f;
if (config_.control_mode >= CONTROL_MODE_VELOCITY_CONTROL) {
if (!vel_estimate_src) {
set_error(ERROR_INVALID_ESTIMATE);
return false;
}
v_err = vel_des - *vel_estimate_src;
torque += (vel_gain * gain_scheduling_multiplier) * v_err;
// Velocity integral action before limiting
torque += vel_integrator_torque_;
}
// Velocity integral action before limiting
Iq += vel_integrator_current_;
// Velocity limiting in current mode
if (config_.control_mode < CONTROL_MODE_VELOCITY_CONTROL && config_.enable_current_mode_vel_limit) {
if (!vel_estimate_src) {
set_error(ERROR_INVALID_ESTIMATE);
return false;
}
torque = limitVel(config_.vel_limit, *vel_estimate_src, vel_gain, torque);
}
// Current limiting
// Torque limiting
bool limited = false;
float Ilim = axis_->motor_.effective_current_lim();
if (Iq > Ilim) {
float Tlim = axis_->motor_.max_available_torque();
if (torque > Tlim) {
limited = true;
Iq = Ilim;
torque = Tlim;
}
if (Iq < -Ilim) {
if (torque < -Tlim) {
limited = true;
Iq = -Ilim;
torque = -Tlim;
}
// Velocity integrator (behaviour dependent on limiting)
if (config_.control_mode < CTRL_MODE_VELOCITY_CONTROL) {
if (config_.control_mode < CONTROL_MODE_VELOCITY_CONTROL) {
// reset integral if not in use
vel_integrator_current_ = 0.0f;
vel_integrator_torque_ = 0.0f;
} else {
if (limited) {
// TODO make decayfactor configurable
vel_integrator_current_ *= 0.99f;
vel_integrator_torque_ *= 0.99f;
} else {
vel_integrator_current_ += (config_.vel_integrator_gain * current_meas_period) * v_err;
vel_integrator_torque_ += ((vel_integrator_gain * gain_scheduling_multiplier) * current_meas_period) * v_err;
}
}
if (current_setpoint_output) *current_setpoint_output = Iq;
if (torque_setpoint_output) *torque_setpoint_output = torque;
return true;
}
Firmware/MotorControl/controller.hpp
+72 -93
View File
@@ -5,42 +5,59 @@
#error "This file should not be included directly. Include odrive_main.h instead."
#endif
class Controller {
class Controller : public ODriveIntf::ControllerIntf {
public:
enum Error_t {
ERROR_NONE = 0,
ERROR_OVERSPEED = 0x01,
};
// Note: these should be sorted from lowest level of control to
// highest level of control, to allow "<" style comparisons.
enum ControlMode_t{
CTRL_MODE_VOLTAGE_CONTROL = 0,
CTRL_MODE_CURRENT_CONTROL = 1,
CTRL_MODE_VELOCITY_CONTROL = 2,
CTRL_MODE_POSITION_CONTROL = 3,
CTRL_MODE_TRAJECTORY_CONTROL = 4
};
typedef struct {
uint32_t index = 0;
float cogging_map[3600];
bool pre_calibrated = false;
bool calib_anticogging = false;
float calib_pos_threshold = 1.0f;
float calib_vel_threshold = 1.0f;
float cogging_ratio = 1.0f;
bool anticogging_enabled = true;
} Anticogging_t;
struct Config_t {
ControlMode_t control_mode = CTRL_MODE_POSITION_CONTROL; //see: Motor_control_mode_t
float pos_gain = 20.0f; // [(counts/s) / counts]
float vel_gain = 5.0f / 10000.0f; // [A/(counts/s)]
// float vel_gain = 5.0f / 200.0f, // [A/(rad/s)] <sensorless example>
float vel_integrator_gain = 10.0f / 10000.0f; // [A/(counts/s * s)]
float vel_limit = 20000.0f; // [counts/s]
float vel_limit_tolerance = 1.2f; // ratio to vel_lim. 0.0f to disable
float vel_ramp_rate = 10000.0f; // [(counts/s) / s]
bool setpoints_in_cpr = false;
ControlMode control_mode = CONTROL_MODE_POSITION_CONTROL; //see: ControlMode_t
InputMode input_mode = INPUT_MODE_PASSTHROUGH; //see: InputMode_t
float pos_gain = 20.0f; // [(turn/s) / turn]
float vel_gain = 1.0f / 6.0f; // [Nm/(turn/s)]
// float vel_gain = 0.2f / 200.0f, // [Nm/(rad/s)] <sensorless example>
float vel_integrator_gain = 2.0f / 6.0f; // [Nm/(turn/s * s)]
float vel_limit = 2.0f; // [turn/s] Infinity to disable.
float vel_limit_tolerance = 1.2f; // ratio to vel_lim. Infinity to disable.
float vel_ramp_rate = 1.0f; // [(turn/s) / s]
float torque_ramp_rate = 0.01f; // Nm / sec
bool circular_setpoints = false;
float circular_setpoint_range = 1.0f; // Circular range when circular_setpoints is true. [turn]
float inertia = 0.0f; // [Nm/(turn/s^2)]
float input_filter_bandwidth = 2.0f; // [1/s]
float homing_speed = 0.25f; // [turn/s]
Anticogging_t anticogging;
float gain_scheduling_width = 10.0f;
bool enable_gain_scheduling = false;
bool enable_vel_limit = true;
bool enable_overspeed_error = true;
bool enable_current_mode_vel_limit = true; // enable velocity limit in current control mode (requires a valid velocity estimator)
uint8_t axis_to_mirror = -1;
float mirror_ratio = 1.0f;
uint8_t load_encoder_axis = -1; // default depends on Axis number and is set in load_configuration()
// custom setters
Controller* parent;
void set_input_filter_bandwidth(float value) { input_filter_bandwidth = value; parent->update_filter_gains(); }
};
explicit Controller(Config_t& config);
void reset();
void set_error(Error_t error);
void set_error(Error error);
void set_pos_setpoint(float pos_setpoint, float vel_feed_forward, float current_feed_forward);
void set_vel_setpoint(float vel_setpoint, float current_feed_forward);
void set_current_setpoint(float current_setpoint);
constexpr void input_pos_updated() {
input_pos_updated_ = true;
}
bool select_encoder(size_t encoder_num);
// Trajectory-Planned control
void move_to_pos(float goal_point);
@@ -50,81 +67,43 @@ public:
void start_anticogging_calibration();
bool anticogging_calibration(float pos_estimate, float vel_estimate);
bool update(float pos_estimate, float vel_estimate, float* current_setpoint);
void update_filter_gains();
bool update(float* torque_setpoint);
Config_t& config_;
Axis* axis_ = nullptr; // set by Axis constructor
// TODO: anticogging overhaul:
// - expose selected (all?) variables on protocol
// - make calibration user experience similar to motor & encoder calibration
// - use python tools to Fourier transform and write back the smoothed map or Fourier coefficients
// - make the calibration persistent
Error error_ = ERROR_NONE;
typedef struct {
int index;
float *cogging_map;
bool use_anticogging;
bool calib_anticogging;
float calib_pos_threshold;
float calib_vel_threshold;
} Anticogging_t;
Anticogging_t anticogging_ = {
.index = 0,
.cogging_map = nullptr,
.use_anticogging = false,
.calib_anticogging = false,
.calib_pos_threshold = 1.0f,
.calib_vel_threshold = 1.0f,
};
float* pos_estimate_linear_src_ = nullptr;
float* pos_estimate_circular_src_ = nullptr;
bool* pos_estimate_valid_src_ = nullptr;
float* vel_estimate_src_ = nullptr;
bool* vel_estimate_valid_src_ = nullptr;
float* pos_wrap_src_ = nullptr;
Error_t error_ = ERROR_NONE;
// variables exposed on protocol
float pos_setpoint_ = 0.0f;
float vel_setpoint_ = 0.0f;
float pos_setpoint_ = 0.0f; // [turns]
float vel_setpoint_ = 0.0f; // [turn/s]
// float vel_setpoint = 800.0f; <sensorless example>
float vel_integrator_current_ = 0.0f; // [A]
float current_setpoint_ = 0.0f; // [A]
float vel_ramp_target_ = 0.0f;
bool vel_ramp_enable_ = false;
float vel_integrator_torque_ = 0.0f; // [Nm]
float torque_setpoint_ = 0.0f; // [Nm]
uint32_t traj_start_loop_count_ = 0;
float input_pos_ = 0.0f; // [turns]
float input_vel_ = 0.0f; // [turn/s]
float input_torque_ = 0.0f; // [Nm]
float input_filter_kp_ = 0.0f;
float input_filter_ki_ = 0.0f;
float goal_point_ = 0.0f;
bool input_pos_updated_ = false;
bool trajectory_done_ = true;
bool anticogging_valid_ = false;
// custom setters
void set_input_pos(float value) { input_pos_ = value; input_pos_updated(); }
// Communication protocol definitions
auto make_protocol_definitions() {
return make_protocol_member_list(
make_protocol_property("error", &error_),
make_protocol_property("pos_setpoint", &pos_setpoint_),
make_protocol_property("vel_setpoint", &vel_setpoint_),
make_protocol_property("vel_integrator_current", &vel_integrator_current_),
make_protocol_property("current_setpoint", &current_setpoint_),
make_protocol_property("vel_ramp_target", &vel_ramp_target_),
make_protocol_property("vel_ramp_enable", &vel_ramp_enable_),
make_protocol_object("config",
make_protocol_property("control_mode", &config_.control_mode),
make_protocol_property("pos_gain", &config_.pos_gain),
make_protocol_property("vel_gain", &config_.vel_gain),
make_protocol_property("vel_integrator_gain", &config_.vel_integrator_gain),
make_protocol_property("vel_limit", &config_.vel_limit),
make_protocol_property("vel_limit_tolerance", &config_.vel_limit_tolerance),
make_protocol_property("vel_ramp_rate", &config_.vel_ramp_rate),
make_protocol_property("setpoints_in_cpr", &config_.setpoints_in_cpr)
),
make_protocol_function("set_pos_setpoint", *this, &Controller::set_pos_setpoint,
"pos_setpoint", "vel_feed_forward", "current_feed_forward"),
make_protocol_function("set_vel_setpoint", *this, &Controller::set_vel_setpoint,
"vel_setpoint", "current_feed_forward"),
make_protocol_function("set_current_setpoint", *this, &Controller::set_current_setpoint,
"current_setpoint"),
make_protocol_function("move_to_pos", *this, &Controller::move_to_pos, "pos_setpoint"),
make_protocol_function("move_incremental", *this, &Controller::move_incremental, "displacement", "from_goal_point"),
make_protocol_function("start_anticogging_calibration", *this, &Controller::start_anticogging_calibration)
);
}
};
DEFINE_ENUM_FLAG_OPERATORS(Controller::Error_t)
#endif // __CONTROLLER_HPP
Firmware/MotorControl/current_limiter.hpp
+14
View File
@@ -0,0 +1,14 @@
#ifndef __CURRENT_LIMITER_HPP
#define __CURRENT_LIMITER_HPP
#ifndef __ODRIVE_MAIN_H
#error "This file should not be included directly. Include odrive_main.h instead."
#endif
class CurrentLimiter {
public:
virtual ~CurrentLimiter() = default;
virtual float get_current_limit(float base_current_lim) const = 0;
};
#endif // __CURRENT_LIMITER_HPP
Firmware/MotorControl/encoder.cpp
+222 -40
View File
@@ -3,14 +3,17 @@
Encoder::Encoder(const EncoderHardwareConfig_t& hw_config,
Config_t& config) :
Config_t& config, const Motor::Config_t& motor_config) :
hw_config_(hw_config),
config_(config)
{
update_pll_gains();
if (config.pre_calibrated && (config.mode == Encoder::MODE_HALL || config.mode == Encoder::MODE_SINCOS)) {
is_ready_ = true;
if (config.pre_calibrated) {
if (config.mode == Encoder::MODE_HALL || config.mode == Encoder::MODE_SINCOS)
is_ready_ = true;
if (motor_config.motor_type == Motor::MOTOR_TYPE_ACIM)
is_ready_ = true;
}
}
@@ -21,9 +24,20 @@ static void enc_index_cb_wrapper(void* ctx) {
void Encoder::setup() {
HAL_TIM_Encoder_Start(hw_config_.timer, TIM_CHANNEL_ALL);
set_idx_subscribe();
mode_ = config_.mode;
if(mode_ & MODE_FLAG_ABS){
abs_spi_cs_pin_init();
abs_spi_init();
if (axis_->controller_.config_.anticogging.pre_calibrated) {
axis_->controller_.anticogging_valid_ = true;
}
}
}
void Encoder::set_error(Error_t error) {
void Encoder::set_error(Error error) {
vel_estimate_valid_ = false;
pos_estimate_valid_ = false;
error_ |= error;
axis_->error_ |= Axis::ERROR_ENCODER_FAILED;
}
@@ -46,6 +60,9 @@ void Encoder::enc_index_cb() {
set_linear_count(0); // Avoid position control transient after search
if (config_.pre_calibrated) {
is_ready_ = true;
if(axis_->controller_.config_.anticogging.pre_calibrated){
axis_->controller_.anticogging_valid_ = true;
}
} else {
// We can't use the update_offset facility in set_circular_count because
// we also set the linear count before there is a chance to update. Therefore:
@@ -79,9 +96,10 @@ void Encoder::update_pll_gains() {
}
void Encoder::check_pre_calibrated() {
if (!is_ready_)
// TODO: restoring config from python backup is fragile here (ACIM motor type must be set first)
if (!is_ready_ && axis_->motor_.config_.motor_type != Motor::MOTOR_TYPE_ACIM)
config_.pre_calibrated = false;
if (config_.mode == MODE_INCREMENTAL && !index_found_)
if (mode_ == MODE_INCREMENTAL && !index_found_)
config_.pre_calibrated = false;
}
@@ -92,7 +110,7 @@ void Encoder::set_linear_count(int32_t count) {
// Update states
shadow_count_ = count;
pos_estimate_ = (float)count;
pos_estimate_counts_ = (float)count;
tim_cnt_sample_ = count;
//Write hardware last
@@ -114,7 +132,7 @@ void Encoder::set_circular_count(int32_t count, bool update_offset) {
// Update states
count_in_cpr_ = mod(count, config_.cpr);
pos_cpr_ = (float)count_in_cpr_;
pos_cpr_counts_ = (float)count_in_cpr_;
cpu_exit_critical(prim);
}
@@ -133,11 +151,14 @@ bool Encoder::run_index_search() {
bool Encoder::run_direction_find() {
int32_t init_enc_val = shadow_count_;
bool orig_finish_on_distance = axis_->config_.calibration_lockin.finish_on_distance;
axis_->config_.calibration_lockin.finish_on_distance = true;
axis_->motor_.config_.direction = 1; // Must test spin forwards for direction detect logic
bool status = axis_->run_lockin_spin(axis_->config_.calibration_lockin);
axis_->config_.calibration_lockin.finish_on_distance = orig_finish_on_distance;
Axis::LockinConfig_t lockin_config = axis_->config_.calibration_lockin;
lockin_config.finish_distance = lockin_config.vel * 3.0f; // run for 3 seconds
lockin_config.finish_on_distance = true;
lockin_config.finish_on_enc_idx = false;
lockin_config.finish_on_vel = false;
bool status = axis_->run_lockin_spin(lockin_config);
if (status) {
// Check response and direction
@@ -160,8 +181,8 @@ bool Encoder::run_direction_find() {
// and the encoder state 0.
// TODO: Do the scan with current, not voltage!
bool Encoder::run_offset_calibration() {
static const float start_lock_duration = 1.0f;
static const int num_steps = (int)(config_.calib_scan_distance / config_.calib_scan_omega * (float)current_meas_hz);
const float start_lock_duration = 1.0f;
const int num_steps = (int)(config_.calib_scan_distance / config_.calib_scan_omega * (float)current_meas_hz);
// Require index found if enabled
if (config_.use_index && !index_found_) {
@@ -186,7 +207,7 @@ bool Encoder::run_offset_calibration() {
axis_->run_control_loop([&](){
if (!axis_->motor_.enqueue_voltage_timings(voltage_magnitude, 0.0f))
return false; // error set inside enqueue_voltage_timings
axis_->motor_.log_timing(Motor::TIMING_LOG_ENC_CALIB);
axis_->motor_.log_timing(TIMING_LOG_ENC_CALIB);
return ++i < start_lock_duration * current_meas_hz;
});
if (axis_->error_ != Axis::ERROR_NONE)
@@ -197,13 +218,13 @@ bool Encoder::run_offset_calibration() {
// scan forward
i = 0;
axis_->run_control_loop([&](){
axis_->run_control_loop([&]() {
float phase = wrap_pm_pi(config_.calib_scan_distance * (float)i / (float)num_steps - config_.calib_scan_distance / 2.0f);
float v_alpha = voltage_magnitude * our_arm_cos_f32(phase);
float v_beta = voltage_magnitude * our_arm_sin_f32(phase);
if (!axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta))
return false; // error set inside enqueue_voltage_timings
axis_->motor_.log_timing(Motor::TIMING_LOG_ENC_CALIB);
axis_->motor_.log_timing(TIMING_LOG_ENC_CALIB);
encvaluesum += shadow_count_;
@@ -229,22 +250,21 @@ bool Encoder::run_offset_calibration() {
// Check CPR
float elec_rad_per_enc = axis_->motor_.config_.pole_pairs * 2 * M_PI * (1.0f / (float)(config_.cpr));
float expected_encoder_delta = config_.calib_scan_distance / elec_rad_per_enc;
calib_scan_response_ = fabsf(shadow_count_-init_enc_val);
if(fabsf(calib_scan_response_ - expected_encoder_delta)/expected_encoder_delta > config_.calib_range)
{
set_error(ERROR_CPR_OUT_OF_RANGE);
calib_scan_response_ = std::abs(shadow_count_ - init_enc_val);
if (std::abs(calib_scan_response_ - expected_encoder_delta) / expected_encoder_delta > config_.calib_range) {
set_error(ERROR_CPR_POLEPAIRS_MISMATCH);
return false;
}
// scan backwards
i = 0;
axis_->run_control_loop([&](){
axis_->run_control_loop([&]() {
float phase = wrap_pm_pi(-config_.calib_scan_distance * (float)i / (float)num_steps + config_.calib_scan_distance / 2.0f);
float v_alpha = voltage_magnitude * our_arm_cos_f32(phase);
float v_beta = voltage_magnitude * our_arm_sin_f32(phase);
if (!axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta))
return false; // error set inside enqueue_voltage_timings
axis_->motor_.log_timing(Motor::TIMING_LOG_ENC_CALIB);
axis_->motor_.log_timing(TIMING_LOG_ENC_CALIB);
encvaluesum += shadow_count_;
@@ -255,7 +275,7 @@ bool Encoder::run_offset_calibration() {
config_.offset = encvaluesum / (num_steps * 2);
int32_t residual = encvaluesum - ((int64_t)config_.offset * (int64_t)(num_steps * 2));
config_.offset_float = (float)residual / (float)(num_steps * 2) + 0.5f; // add 0.5 to center-align state to phase
config_.offset_float = (float)residual / (float)(num_steps * 2) + 0.5f; // add 0.5 to center-align state to phase
is_ready_ = true;
return true;
@@ -274,7 +294,7 @@ static bool decode_hall(uint8_t hall_state, int32_t* hall_cnt) {
}
void Encoder::sample_now() {
switch (config_.mode) {
switch (mode_) {
case MODE_INCREMENTAL: {
tim_cnt_sample_ = (int16_t)hw_config_.timer->Instance->CNT;
} break;
@@ -284,8 +304,17 @@ void Encoder::sample_now() {
} break;
case MODE_SINCOS: {
sincos_sample_s_ = (get_adc_voltage(GPIO_3_GPIO_Port, GPIO_3_Pin) / 3.3f) - 0.5f;
sincos_sample_c_ = (get_adc_voltage(GPIO_4_GPIO_Port, GPIO_4_Pin) / 3.3f) - 0.5f;
sincos_sample_s_ = (get_adc_voltage(get_gpio_port_by_pin(config_.sincos_gpio_pin_sin), get_gpio_pin_by_pin(config_.sincos_gpio_pin_sin)) / 3.3f) - 0.5f;
sincos_sample_c_ = (get_adc_voltage(get_gpio_port_by_pin(config_.sincos_gpio_pin_cos), get_gpio_pin_by_pin(config_.sincos_gpio_pin_cos)) / 3.3f) - 0.5f;
} break;
case MODE_SPI_ABS_AMS:
case MODE_SPI_ABS_CUI:
case MODE_SPI_ABS_AEAT:
case MODE_SPI_ABS_RLS:
{
axis_->motor_.log_timing(TIMING_LOG_SAMPLE_NOW);
// Do nothing
} break;
default: {
@@ -294,10 +323,126 @@ void Encoder::sample_now() {
}
}
bool Encoder::abs_spi_init(){
if ((mode_ & MODE_FLAG_ABS) == 0x0)
return false;
SPI_HandleTypeDef * spi = hw_config_.spi;
spi->Init.Mode = SPI_MODE_MASTER;
spi->Init.Direction = SPI_DIRECTION_2LINES;
spi->Init.DataSize = SPI_DATASIZE_16BIT;
spi->Init.CLKPolarity = SPI_POLARITY_LOW;
spi->Init.CLKPhase = SPI_PHASE_2EDGE;
spi->Init.NSS = SPI_NSS_SOFT;
spi->Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_32;
spi->Init.FirstBit = SPI_FIRSTBIT_MSB;
spi->Init.TIMode = SPI_TIMODE_DISABLE;
spi->Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;
spi->Init.CRCPolynomial = 10;
if (mode_ == MODE_SPI_ABS_AEAT) {
spi->Init.CLKPolarity = SPI_POLARITY_HIGH;
}
HAL_SPI_DeInit(spi);
HAL_SPI_Init(spi);
return true;
}
bool Encoder::abs_spi_start_transaction(){
if (mode_ & MODE_FLAG_ABS){
axis_->motor_.log_timing(TIMING_LOG_SPI_START);
if(hw_config_.spi->State != HAL_SPI_STATE_READY){
set_error(ERROR_ABS_SPI_NOT_READY);
return false;
}
HAL_GPIO_WritePin(abs_spi_cs_port_, abs_spi_cs_pin_, GPIO_PIN_RESET);
HAL_SPI_TransmitReceive_DMA(hw_config_.spi, (uint8_t*)abs_spi_dma_tx_, (uint8_t*)abs_spi_dma_rx_, 1);
}
return true;
}
uint8_t ams_parity(uint16_t v) {
v ^= v >> 8;
v ^= v >> 4;
v ^= v >> 2;
v ^= v >> 1;
return v & 1;
}
uint8_t cui_parity(uint16_t v) {
v ^= v >> 8;
v ^= v >> 4;
v ^= v >> 2;
return ~v & 3;
}
void Encoder::abs_spi_cb(){
HAL_GPIO_WritePin(abs_spi_cs_port_, abs_spi_cs_pin_, GPIO_PIN_SET);
axis_->motor_.log_timing(TIMING_LOG_SPI_END);
uint16_t pos;
switch (mode_) {
case MODE_SPI_ABS_AMS: {
uint16_t rawVal = abs_spi_dma_rx_[0];
// check if parity is correct (even) and error flag clear
if (ams_parity(rawVal) || ((rawVal >> 14) & 1)) {
return;
}
pos = rawVal & 0x3fff;
} break;
case MODE_SPI_ABS_CUI: {
uint16_t rawVal = abs_spi_dma_rx_[0];
// check if parity is correct
if (cui_parity(rawVal)) {
return;
}
pos = rawVal & 0x3fff;
} break;
case MODE_SPI_ABS_RLS: {
uint16_t rawVal = abs_spi_dma_rx_[0];
pos = (rawVal >> 2) & 0x3fff;
} break;
default: {
set_error(ERROR_UNSUPPORTED_ENCODER_MODE);
return;
} break;
}
pos_abs_ = pos;
abs_spi_pos_updated_ = true;
if (config_.pre_calibrated) {
is_ready_ = true;
}
}
void Encoder::abs_spi_cs_pin_init(){
// Decode cs pin
abs_spi_cs_port_ = get_gpio_port_by_pin(config_.abs_spi_cs_gpio_pin);
abs_spi_cs_pin_ = get_gpio_pin_by_pin(config_.abs_spi_cs_gpio_pin);
// Init cs pin
HAL_GPIO_DeInit(abs_spi_cs_port_, abs_spi_cs_pin_);
GPIO_InitTypeDef GPIO_InitStruct;
GPIO_InitStruct.Pin = abs_spi_cs_pin_;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Pull = GPIO_PULLUP;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
HAL_GPIO_Init(abs_spi_cs_port_, &GPIO_InitStruct);
// Write pin high
HAL_GPIO_WritePin(abs_spi_cs_port_, abs_spi_cs_pin_, GPIO_PIN_SET);
}
bool Encoder::update() {
// update internal encoder state.
int32_t delta_enc = 0;
switch (config_.mode) {
int32_t pos_abs_latched = pos_abs_; //LATCH
switch (mode_) {
case MODE_INCREMENTAL: {
//TODO: use count_in_cpr_ instead as shadow_count_ can overflow
//or use 64 bit
@@ -331,6 +476,28 @@ bool Encoder::update() {
delta_enc -= 6283;
} break;
case MODE_SPI_ABS_RLS:
case MODE_SPI_ABS_AMS:
case MODE_SPI_ABS_CUI:
case MODE_SPI_ABS_AEAT: {
if (abs_spi_pos_updated_ == false) {
// Low pass filter the error
spi_error_rate_ += current_meas_period * (1.0f - spi_error_rate_);
if (spi_error_rate_ > 0.005f)
set_error(ERROR_ABS_SPI_COM_FAIL);
} else {
// Low pass filter the error
spi_error_rate_ += current_meas_period * (0.0f - spi_error_rate_);
}
abs_spi_pos_updated_ = false;
delta_enc = pos_abs_latched - count_in_cpr_; //LATCH
delta_enc = mod(delta_enc, config_.cpr);
if (delta_enc > config_.cpr/2) {
delta_enc -= config_.cpr;
}
}break;
default: {
set_error(ERROR_UNSUPPORTED_ENCODER_MODE);
return false;
@@ -341,38 +508,51 @@ bool Encoder::update() {
count_in_cpr_ += delta_enc;
count_in_cpr_ = mod(count_in_cpr_, config_.cpr);
if(mode_ & MODE_FLAG_ABS)
count_in_cpr_ = pos_abs_latched;
//// run pll (for now pll is in units of encoder counts)
// Predict current pos
pos_estimate_ += current_meas_period * vel_estimate_;
pos_cpr_ += current_meas_period * vel_estimate_;
pos_estimate_counts_ += current_meas_period * vel_estimate_counts_;
pos_cpr_counts_ += current_meas_period * vel_estimate_counts_;
// discrete phase detector
float delta_pos = (float)(shadow_count_ - (int32_t)floorf(pos_estimate_));
float delta_pos_cpr = (float)(count_in_cpr_ - (int32_t)floorf(pos_cpr_));
delta_pos_cpr = wrap_pm(delta_pos_cpr, 0.5f * (float)(config_.cpr));
float delta_pos_counts = (float)(shadow_count_ - (int32_t)std::floor(pos_estimate_counts_));
float delta_pos_cpr_counts = (float)(count_in_cpr_ - (int32_t)std::floor(pos_cpr_counts_));
delta_pos_cpr_counts = wrap_pm(delta_pos_cpr_counts, 0.5f * (float)(config_.cpr));
// pll feedback
pos_estimate_ += current_meas_period * pll_kp_ * delta_pos;
pos_cpr_ += current_meas_period * pll_kp_ * delta_pos_cpr;
pos_cpr_ = fmodf_pos(pos_cpr_, (float)(config_.cpr));
vel_estimate_ += current_meas_period * pll_ki_ * delta_pos_cpr;
pos_estimate_counts_ += current_meas_period * pll_kp_ * delta_pos_counts;
pos_cpr_counts_ += current_meas_period * pll_kp_ * delta_pos_cpr_counts;
pos_cpr_counts_ = fmodf_pos(pos_cpr_counts_, (float)(config_.cpr));
vel_estimate_counts_ += current_meas_period * pll_ki_ * delta_pos_cpr_counts;
bool snap_to_zero_vel = false;
if (fabsf(vel_estimate_) < 0.5f * current_meas_period * pll_ki_) {
vel_estimate_ = 0.0f; //align delta-sigma on zero to prevent jitter
if (std::abs(vel_estimate_counts_) < 0.5f * current_meas_period * pll_ki_) {
vel_estimate_counts_ = 0.0f; //align delta-sigma on zero to prevent jitter
snap_to_zero_vel = true;
}
// Outputs from Encoder for Controller
float pos_cpr_last = pos_cpr_;
pos_estimate_ = pos_estimate_counts_ / (float)config_.cpr;
vel_estimate_ = vel_estimate_counts_ / (float)config_.cpr;
pos_cpr_= pos_cpr_counts_ / (float)config_.cpr;
float delta_pos_cpr = wrap_pm(pos_cpr_ - pos_cpr_last, 0.5f);
pos_circular_ += delta_pos_cpr;
pos_circular_ = fmodf_pos(pos_circular_, axis_->controller_.config_.circular_setpoint_range);
//// run encoder count interpolation
int32_t corrected_enc = count_in_cpr_ - config_.offset;
// if we are stopped, make sure we don't randomly drift
if (snap_to_zero_vel || !config_.enable_phase_interpolation) {
interpolation_ = 0.5f;
// reset interpolation if encoder edge comes
// TODO: This isn't correct. At high velocities the first phase in this count may very well not be at the edge.
} else if (delta_enc > 0) {
interpolation_ = 0.0f;
} else if (delta_enc < 0) {
interpolation_ = 1.0f;
} else {
// Interpolate (predict) between encoder counts using vel_estimate,
interpolation_ += current_meas_period * vel_estimate_;
interpolation_ += current_meas_period * vel_estimate_counts_;
// don't allow interpolation indicated position outside of [enc, enc+1)
if (interpolation_ > 1.0f) interpolation_ = 1.0f;
if (interpolation_ < 0.0f) interpolation_ = 0.0f;
@@ -386,5 +566,7 @@ bool Encoder::update() {
// ph = fmodf(ph, 2*M_PI);
phase_ = wrap_pm_pi(ph);
vel_estimate_valid_ = true;
pos_estimate_valid_ = true;
return true;
}
Firmware/MotorControl/encoder.hpp
+46 -68
View File
@@ -5,26 +5,12 @@
#error "This file should not be included directly. Include odrive_main.h instead."
#endif
class Encoder {
class Encoder : public ODriveIntf::EncoderIntf {
public:
enum Error_t {
ERROR_NONE = 0,
ERROR_UNSTABLE_GAIN = 0x01,
ERROR_CPR_OUT_OF_RANGE = 0x02,
ERROR_NO_RESPONSE = 0x04,
ERROR_UNSUPPORTED_ENCODER_MODE = 0x08,
ERROR_ILLEGAL_HALL_STATE = 0x10,
ERROR_INDEX_NOT_FOUND_YET = 0x20,
};
enum Mode_t {
MODE_INCREMENTAL,
MODE_HALL,
MODE_SINCOS
};
const uint32_t MODE_FLAG_ABS = 0x100;
struct Config_t {
Encoder::Mode_t mode = Encoder::MODE_INCREMENTAL;
Mode mode = MODE_INCREMENTAL;
bool use_index = false;
bool pre_calibrated = false; // If true, this means the offset stored in
// configuration is valid and does not need
@@ -43,13 +29,24 @@ public:
bool find_idx_on_lockin_only = false; // Only be sensitive during lockin scan constant vel state
bool idx_search_unidirectional = false; // Only allow index search in known direction
bool ignore_illegal_hall_state = false; // dont error on bad states like 000 or 111
uint16_t abs_spi_cs_gpio_pin = 1;
uint16_t sincos_gpio_pin_sin = 3;
uint16_t sincos_gpio_pin_cos = 4;
// custom setters
Encoder* parent = nullptr;
void set_use_index(bool value) { use_index = value; parent->set_idx_subscribe(); }
void set_find_idx_on_lockin_only(bool value) { find_idx_on_lockin_only = value; parent->set_idx_subscribe(); }
void set_abs_spi_cs_gpio_pin(uint16_t value) { abs_spi_cs_gpio_pin = value; parent->abs_spi_cs_pin_init(); }
void set_pre_calibrated(bool value) { pre_calibrated = value; parent->check_pre_calibrated(); }
void set_bandwidth(float value) { bandwidth = value; parent->update_pll_gains(); }
};
Encoder(const EncoderHardwareConfig_t& hw_config,
Config_t& config);
Config_t& config, const Motor::Config_t& motor_config);
void setup();
void set_error(Error_t error);
void set_error(Error error);
bool do_checks();
void enc_index_cb();
@@ -67,25 +64,33 @@ public:
void sample_now();
bool update();
const EncoderHardwareConfig_t& hw_config_;
Config_t& config_;
Axis* axis_ = nullptr; // set by Axis constructor
Error_t error_ = ERROR_NONE;
Error error_ = ERROR_NONE;
bool index_found_ = false;
bool is_ready_ = false;
int32_t shadow_count_ = 0;
int32_t count_in_cpr_ = 0;
float interpolation_ = 0.0f;
float phase_ = 0.0f; // [count]
float pos_estimate_ = 0.0f; // [count]
float pos_cpr_ = 0.0f; // [count]
float vel_estimate_ = 0.0f; // [count/s]
float phase_ = 0.0f; // [count]
float pos_estimate_counts_ = 0.0f; // [count]
float pos_cpr_counts_ = 0.0f; // [count]
float vel_estimate_counts_ = 0.0f; // [count/s]
float pll_kp_ = 0.0f; // [count/s / count]
float pll_ki_ = 0.0f; // [(count/s^2) / count]
float calib_scan_response_ = 0.0f; // debug report from offset calib
int32_t pos_abs_ = 0;
float spi_error_rate_ = 0.0f;
float pos_estimate_ = 0.0f; // [turn]
float vel_estimate_ = 0.0f; // [turn/s]
float pos_cpr_ = 0.0f; // [turn]
float pos_circular_ = 0.0f; // [turn]
bool pos_estimate_valid_ = false;
bool vel_estimate_valid_ = false;
int16_t tim_cnt_sample_ = 0; //
// Updated by low_level pwm_adc_cb
@@ -93,49 +98,22 @@ public:
float sincos_sample_s_ = 0.0f;
float sincos_sample_c_ = 0.0f;
// Communication protocol definitions
auto make_protocol_definitions() {
return make_protocol_member_list(
make_protocol_property("error", &error_),
make_protocol_property("is_ready", &is_ready_),
make_protocol_property("index_found", const_cast<bool*>(&index_found_)),
make_protocol_property("shadow_count", &shadow_count_),
make_protocol_property("count_in_cpr", &count_in_cpr_),
make_protocol_property("interpolation", &interpolation_),
make_protocol_ro_property("phase", &phase_),
make_protocol_property("pos_estimate", &pos_estimate_),
make_protocol_property("pos_cpr", &pos_cpr_),
make_protocol_ro_property("hall_state", &hall_state_),
make_protocol_property("vel_estimate", &vel_estimate_),
make_protocol_ro_property("calib_scan_response", &calib_scan_response_),
// make_protocol_property("pll_kp", &pll_kp_),
// make_protocol_property("pll_ki", &pll_ki_),
make_protocol_object("config",
make_protocol_property("mode", &config_.mode),
make_protocol_property("use_index", &config_.use_index,
[](void* ctx) { static_cast<Encoder*>(ctx)->set_idx_subscribe(); }, this),
make_protocol_property("find_idx_on_lockin_only", &config_.find_idx_on_lockin_only,
[](void* ctx) { static_cast<Encoder*>(ctx)->set_idx_subscribe(); }, this),
make_protocol_property("pre_calibrated", &config_.pre_calibrated,
[](void* ctx) { static_cast<Encoder*>(ctx)->check_pre_calibrated(); }, this),
make_protocol_property("zero_count_on_find_idx", &config_.zero_count_on_find_idx),
make_protocol_property("cpr", &config_.cpr),
make_protocol_property("offset", &config_.offset),
make_protocol_property("offset_float", &config_.offset_float),
make_protocol_property("enable_phase_interpolation", &config_.enable_phase_interpolation),
make_protocol_property("bandwidth", &config_.bandwidth,
[](void* ctx) { static_cast<Encoder*>(ctx)->update_pll_gains(); }, this),
make_protocol_property("calib_range", &config_.calib_range),
make_protocol_property("calib_scan_distance", &config_.calib_scan_distance),
make_protocol_property("calib_scan_omega", &config_.calib_scan_omega),
make_protocol_property("idx_search_unidirectional", &config_.idx_search_unidirectional),
make_protocol_property("ignore_illegal_hall_state", &config_.ignore_illegal_hall_state)
),
make_protocol_function("set_linear_count", *this, &Encoder::set_linear_count, "count")
);
bool abs_spi_init();
bool abs_spi_start_transaction();
void abs_spi_cb();
void abs_spi_cs_pin_init();
uint16_t abs_spi_dma_tx_[1] = {0xFFFF};
uint16_t abs_spi_dma_rx_[1];
bool abs_spi_pos_updated_ = false;
Mode mode_ = MODE_INCREMENTAL;
GPIO_TypeDef* abs_spi_cs_port_;
uint16_t abs_spi_cs_pin_;
uint32_t abs_spi_cr1;
uint32_t abs_spi_cr2;
constexpr float getCoggingRatio(){
return 1.0f / 3600.0f;
}
};
DEFINE_ENUM_FLAG_OPERATORS(Encoder::Error_t)
#endif // __ENCODER_HPP
Firmware/MotorControl/endstop.cpp
+55
View File
@@ -0,0 +1,55 @@
#include <odrive_main.h>
Endstop::Endstop(Endstop::Config_t& config)
: config_(config) {
update_config();
debounceTimer_.setIncrement(current_meas_period);
}
void Endstop::update() {
debounceTimer_.update();
if (config_.enabled) {
bool last_pin_state = pin_state_;
uint16_t gpio_pin = get_gpio_pin_by_pin(config_.gpio_num);
GPIO_TypeDef* gpio_port = get_gpio_port_by_pin(config_.gpio_num);
pin_state_ = HAL_GPIO_ReadPin(gpio_port, gpio_pin);
// If the pin state has changed, reset the timer
if (pin_state_ != last_pin_state)
debounceTimer_.reset();
if (debounceTimer_.expired())
endstop_state_ = config_.is_active_high ? pin_state_ : !pin_state_; // endstop_state is the logical state
} else {
endstop_state_ = false;
}
}
bool Endstop::get_state() {
return endstop_state_;
}
void Endstop::update_config() {
set_enabled(config_.enabled);
debounceTimer_.setIncrement(config_.debounce_ms * 0.001f);
}
void Endstop::set_enabled(bool enable) {
debounceTimer_.reset();
if (config_.gpio_num != 0) {
uint16_t gpio_pin = get_gpio_pin_by_pin(config_.gpio_num);
GPIO_TypeDef* gpio_port = get_gpio_port_by_pin(config_.gpio_num);
if (enable) {
HAL_GPIO_DeInit(gpio_port, gpio_pin);
GPIO_InitTypeDef GPIO_InitStruct;
GPIO_InitStruct.Pin = gpio_pin;
GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
GPIO_InitStruct.Pull = config_.pullup ? GPIO_PULLUP : GPIO_PULLDOWN;
HAL_GPIO_Init(gpio_port, &GPIO_InitStruct);
debounceTimer_.start();
} else
debounceTimer_.stop();
}
}
Firmware/MotorControl/endstop.hpp
+40
View File
@@ -0,0 +1,40 @@
#ifndef __ENDSTOP_HPP
#define __ENDSTOP_HPP
#include "timer.hpp"
class Endstop {
public:
struct Config_t {
float offset = 0;
uint32_t debounce_ms = 50;
uint16_t gpio_num = 0;
bool enabled = false;
bool is_active_high = false;
bool pullup = true;
// custom setters
Endstop* parent = nullptr;
void set_gpio_num(uint16_t value) { gpio_num = value; parent->update_config(); }
void set_enabled(uint32_t value) { enabled = value; parent->update_config(); }
void set_debounce_ms(uint32_t value) { debounce_ms = value; parent->update_config(); }
};
explicit Endstop(Endstop::Config_t& config);
Endstop::Config_t& config_;
Axis* axis_ = nullptr;
void update_config();
void set_enabled(bool enabled);
void update();
bool get_state();
bool endstop_state_ = false;
private:
bool pin_state_ = false;
float pos_when_pressed_ = 0.0f;
Timer<float> debounceTimer_;
};
#endif
Firmware/MotorControl/gpio_utils.hpp
+46
View File
@@ -0,0 +1,46 @@
#pragma once
#include "gpio.h"
constexpr GPIO_TypeDef* get_gpio_port_by_pin(uint16_t GPIO_pin){
switch(GPIO_pin){
case 1: return GPIO_1_GPIO_Port; break;
case 2: return GPIO_2_GPIO_Port; break;
case 3: return GPIO_3_GPIO_Port; break;
case 4: return GPIO_4_GPIO_Port; break;
#ifdef GPIO_5_GPIO_Port
case 5: return GPIO_5_GPIO_Port; break;
#endif
#ifdef GPIO_6_GPIO_Port
case 6: return GPIO_6_GPIO_Port; break;
#endif
#ifdef GPIO_7_GPIO_Port
case 7: return GPIO_7_GPIO_Port; break;
#endif
#ifdef GPIO_8_GPIO_Port
case 8: return GPIO_8_GPIO_Port; break;
#endif
default: return GPIO_1_GPIO_Port;
}
}
constexpr uint16_t get_gpio_pin_by_pin(uint16_t GPIO_pin){
switch(GPIO_pin){
case 1: return GPIO_1_Pin; break;
case 2: return GPIO_2_Pin; break;
case 3: return GPIO_3_Pin; break;
case 4: return GPIO_4_Pin; break;
#ifdef GPIO_5_Pin
case 5: return GPIO_5_Pin; break;
#endif
#ifdef GPIO_6_Pin
case 6: return GPIO_6_Pin; break;
#endif
#ifdef GPIO_7_Pin
case 7: return GPIO_7_Pin; break;
#endif
#ifdef GPIO_8_Pin
case 8: return GPIO_8_Pin; break;
#endif
default: return GPIO_1_Pin;
}
}
Firmware/MotorControl/low_level.cpp
+108 -44
View File
@@ -17,7 +17,7 @@
#include <main.h>
#include <spi.h>
#include <tim.h>
#include <utils.h>
#include <utils.hpp>
#include "odrive_main.h"
@@ -28,14 +28,16 @@
/* Private macros ------------------------------------------------------------*/
/* Private typedef -----------------------------------------------------------*/
/* Global constant data ------------------------------------------------------*/
const float adc_full_scale = (float)(1 << 12);
const float adc_ref_voltage = 3.3f;
constexpr float adc_full_scale = static_cast<float>(1UL << 12UL);
constexpr float adc_ref_voltage = 3.3f;
/* Global variables ----------------------------------------------------------*/
// This value is updated by the DC-bus reading ADC.
// Arbitrary non-zero inital value to avoid division by zero if ADC reading is late
float vbus_voltage = 12.0f;
float ibus_ = 0.0f; // exposed for monitoring only
bool brake_resistor_armed = false;
bool brake_resistor_saturated = false;
/* Private constant data -----------------------------------------------------*/
static const GPIO_TypeDef* GPIOs_to_samp[] = { GPIOA, GPIOB, GPIOC };
static const int num_GPIO = sizeof(GPIOs_to_samp) / sizeof(GPIOs_to_samp[0]);
@@ -82,7 +84,7 @@ static uint16_t GPIO_port_samples [2][num_GPIO];
*/
// @brief Floats ALL phases immediately and disarms both motors and the brake resistor.
void low_level_fault(Motor::Error_t error) {
void low_level_fault(Motor::Error error) {
// Disable all motors NOW!
for (size_t i = 0; i < AXIS_COUNT; ++i) {
safety_critical_disarm_motor_pwm(axes[i]->motor_);
@@ -212,6 +214,7 @@ void start_adc_pwm() {
// Ensure that debug halting of the core doesn't leave the motor PWM running
__HAL_DBGMCU_FREEZE_TIM1();
__HAL_DBGMCU_FREEZE_TIM8();
__HAL_DBGMCU_FREEZE_TIM13();
start_pwm(&htim1);
start_pwm(&htim8);
@@ -259,6 +262,20 @@ void start_pwm(TIM_HandleTypeDef* htim) {
HAL_TIM_PWM_Start_IT(htim, TIM_CHANNEL_4);
}
/*
* Initial intention of this function:
* Synchronize TIM1, TIM8 and TIM13 such that:
* 1. The triangle waveform of TIM1 leads the triangle waveform of TIM8 by a
* 90° phase shift.
* 2. The timer update events of TIM1 and TIM8 are symmetrically interleaved.
* 3. Each TIM13 reload coincides with a TIM1 lower update event.
*
* However right now this function only ensures point (1) and (3) but because
* TIM1 and TIM3 only trigger an update on every third reload, this does not
* imply (or even allow for) (2).
*
* TODO: revisit the timing topic in general.
*/
void sync_timers(TIM_HandleTypeDef* htim_a, TIM_HandleTypeDef* htim_b,
uint16_t TIM_CLOCKSOURCE_ITRx, uint16_t count_offset,
TIM_HandleTypeDef* htim_refbase) {
@@ -373,8 +390,16 @@ void start_general_purpose_adc() {
// 21000kHz / (15+26) / 16 = 32kHz
// The true frequency is slightly lower because of the injected vbus
// measurements
float get_adc_voltage(GPIO_TypeDef* GPIO_port, uint16_t GPIO_pin) {
uint32_t channel = UINT32_MAX;
float get_adc_voltage(const GPIO_TypeDef* const GPIO_port, uint16_t GPIO_pin) {
const uint16_t channel = channel_from_gpio(GPIO_port, GPIO_pin);
return get_adc_voltage_channel(channel);
}
// @brief Given a GPIO_port and pin return the associated adc_channel.
// returns UINT16_MAX if there is no adc_channel;
uint16_t channel_from_gpio(const GPIO_TypeDef* const GPIO_port, uint16_t GPIO_pin)
{
uint16_t channel = UINT16_MAX;
if (GPIO_port == GPIOA) {
if (GPIO_pin == GPIO_PIN_0)
channel = 0;
@@ -411,6 +436,13 @@ float get_adc_voltage(GPIO_TypeDef* GPIO_port, uint16_t GPIO_pin) {
else if (GPIO_pin == GPIO_PIN_5)
channel = 15;
}
return channel;
}
// @brief Given an adc channel return the measured voltage.
// returns NaN if the channel is not valid.
float get_adc_voltage_channel(uint16_t channel)
{
if (channel < ADC_CHANNEL_COUNT)
return ((float)adc_measurements_[channel]) * (adc_ref_voltage / adc_full_scale);
else
@@ -422,15 +454,10 @@ float get_adc_voltage(GPIO_TypeDef* GPIO_port, uint16_t GPIO_pin) {
//--------------------------------
void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc, bool injected) {
static const float voltage_scale = adc_ref_voltage * VBUS_S_DIVIDER_RATIO / adc_full_scale;
constexpr float voltage_scale = adc_ref_voltage * VBUS_S_DIVIDER_RATIO / adc_full_scale;
// Only one conversion in sequence, so only rank1
uint32_t ADCValue = HAL_ADCEx_InjectedGetValue(hadc, ADC_INJECTED_RANK_1);
vbus_voltage = ADCValue * voltage_scale;
if (axes[0] && !axes[0]->error_ && axes[1] && !axes[1]->error_) {
if (oscilloscope_pos >= OSCILLOSCOPE_SIZE)
oscilloscope_pos = 0;
oscilloscope[oscilloscope_pos++] = vbus_voltage;
}
}
static void decode_hall_samples(Encoder& enc, uint16_t GPIO_samples[num_GPIO]) {
@@ -466,7 +493,7 @@ static void decode_hall_samples(Encoder& enc, uint16_t GPIO_samples[num_GPIO]) {
// TODO: Document how the phasing is done, link to timing diagram
void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) {
#define calib_tau 0.2f //@TOTO make more easily configurable
static const float calib_filter_k = CURRENT_MEAS_PERIOD / calib_tau;
constexpr float calib_filter_k = CURRENT_MEAS_PERIOD / calib_tau;
// Ensure ADCs are expected ones to simplify the logic below
if (!(hadc == &hadc2 || hadc == &hadc3)) {
@@ -486,9 +513,9 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) {
// Check the timing of the sequencing
if (current_meas_not_DC_CAL)
axis.motor_.log_timing(Motor::TIMING_LOG_ADC_CB_I);
axis.motor_.log_timing(TIMING_LOG_ADC_CB_I);
else
axis.motor_.log_timing(Motor::TIMING_LOG_ADC_CB_DC);
axis.motor_.log_timing(TIMING_LOG_ADC_CB_DC);
bool update_timings = false;
if (hadc == &hadc2) {
@@ -496,6 +523,15 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) {
update_timings = true; // update timings of M0
else if (&axis == axes[0] && !counting_down)
update_timings = true; // update timings of M1
// TODO: this is out of place here. However when moving it somewhere
// else we have to consider the timing requirements to prevent the SPI
// transfers of axis0 and axis1 from conflicting.
// Also see comment on sync_timers.
if((current_meas_not_DC_CAL && !axis_num) ||
(axis_num && !current_meas_not_DC_CAL)){
axis.encoder_.abs_spi_start_transaction();
}
}
// Load next timings for the motor that we're not currently sampling
@@ -590,22 +626,46 @@ void update_brake_current() {
Ibus_sum += axes[i]->motor_.current_control_.Ibus;
}
}
float brake_current = -Ibus_sum;
// Clip negative values to 0.0f
if (brake_current < 0.0f) brake_current = 0.0f;
float brake_duty = brake_current * board_config.brake_resistance / vbus_voltage;
// Duty limit at 90% to allow bootstrap caps to charge
// If brake_duty is NaN, this expression will also evaluate to false
if ((brake_duty >= 0.0f) && (brake_duty <= 0.9f)) {
int high_on = static_cast<int>(TIM_APB1_PERIOD_CLOCKS * (1.0f - brake_duty));
int low_off = high_on - TIM_APB1_DEADTIME_CLOCKS;
if (low_off < 0) low_off = 0;
safety_critical_apply_brake_resistor_timings(low_off, high_on);
} else {
//shuts off all motors AND brake resistor, sets error code on all motors.
low_level_fault(Motor::ERROR_BRAKE_CURRENT_OUT_OF_RANGE);
// Don't start braking until -Ibus > regen_current_allowed
float brake_current = -Ibus_sum - odrv.config_.max_regen_current;
float brake_duty = brake_current * odrv.config_.brake_resistance / vbus_voltage;
if (odrv.config_.enable_dc_bus_overvoltage_ramp && (odrv.config_.brake_resistance > 0.0f) && (odrv.config_.dc_bus_overvoltage_ramp_start < odrv.config_.dc_bus_overvoltage_ramp_end)) {
brake_duty += std::fmax((vbus_voltage - odrv.config_.dc_bus_overvoltage_ramp_start) / (odrv.config_.dc_bus_overvoltage_ramp_end - odrv.config_.dc_bus_overvoltage_ramp_start), 0.0f);
}
if (std::isnan(brake_duty)) {
// Shuts off all motors AND brake resistor, sets error code on all motors.
low_level_fault(Motor::ERROR_BRAKE_DUTY_CYCLE_NAN);
return;
}
if (brake_duty >= 0.95f) {
brake_resistor_saturated = true;
}
// Duty limit at 95% to allow bootstrap caps to charge
brake_duty = std::clamp(brake_duty, 0.0f, 0.95f);
// Special handling to avoid the case 0.0/0.0 == NaN.
Ibus_sum += brake_duty ? (brake_duty * vbus_voltage / odrv.config_.brake_resistance) : 0.0f;
ibus_ += odrv.ibus_report_filter_k_ * (Ibus_sum - ibus_);
if (Ibus_sum > odrv.config_.dc_max_positive_current) {
low_level_fault(Motor::ERROR_DC_BUS_OVER_CURRENT);
return;
}
if (Ibus_sum < odrv.config_.dc_max_negative_current) {
low_level_fault(Motor::ERROR_DC_BUS_OVER_REGEN_CURRENT);
return;
}
int high_on = (int)(TIM_APB1_PERIOD_CLOCKS * (1.0f - brake_duty));
int low_off = high_on - TIM_APB1_DEADTIME_CLOCKS;
if (low_off < 0) low_off = 0;
safety_critical_apply_brake_resistor_timings(low_off, high_on);
}
@@ -670,7 +730,7 @@ void pwm_in_init() {
#else
int gpio_num = 4; {
#endif
if (is_endpoint_ref_valid(board_config.pwm_mappings[gpio_num - 1].endpoint)) {
if (fibre::is_endpoint_ref_valid(odrv.config_.pwm_mappings[gpio_num - 1].endpoint)) {
GPIO_InitStruct.Pin = get_gpio_pin_by_pin(gpio_num);
HAL_GPIO_DeInit(get_gpio_port_by_pin(gpio_num), get_gpio_pin_by_pin(gpio_num));
HAL_GPIO_Init(get_gpio_port_by_pin(gpio_num), &GPIO_InitStruct);
@@ -697,14 +757,10 @@ void handle_pulse(int gpio_num, uint32_t high_time) {
if (high_time > PWM_MAX_HIGH_TIME)
high_time = PWM_MAX_HIGH_TIME;
float fraction = (float)(high_time - PWM_MIN_HIGH_TIME) / (float)(PWM_MAX_HIGH_TIME - PWM_MIN_HIGH_TIME);
float value = board_config.pwm_mappings[gpio_num - 1].min +
(fraction * (board_config.pwm_mappings[gpio_num - 1].max - board_config.pwm_mappings[gpio_num - 1].min));
float value = odrv.config_.pwm_mappings[gpio_num - 1].min +
(fraction * (odrv.config_.pwm_mappings[gpio_num - 1].max - odrv.config_.pwm_mappings[gpio_num - 1].min));
Endpoint* endpoint = get_endpoint(board_config.pwm_mappings[gpio_num - 1].endpoint);
if (!endpoint)
return;
endpoint->set_from_float(value);
fibre::set_endpoint_from_float(odrv.config_.pwm_mappings[gpio_num - 1].endpoint, value);
}
void pwm_in_cb(int channel, uint32_t timestamp) {
@@ -735,24 +791,32 @@ static void update_analog_endpoint(const struct PWMMapping_t *map, int gpio)
{
float fraction = get_adc_voltage(get_gpio_port_by_pin(gpio), get_gpio_pin_by_pin(gpio)) / 3.3f;
float value = map->min + (fraction * (map->max - map->min));
get_endpoint(map->endpoint)->set_from_float(value);
fibre::set_endpoint_from_float(map->endpoint, value);
}
static void analog_polling_thread(void *)
{
while (true) {
for (int i = 0; i < GPIO_COUNT; i++) {
struct PWMMapping_t *map = &board_config.analog_mappings[i];
struct PWMMapping_t *map = &odrv.config_.analog_mappings[i];
if (is_endpoint_ref_valid(map->endpoint))
if (fibre::is_endpoint_ref_valid(map->endpoint))
update_analog_endpoint(map, i + 1);
}
osDelay(10);
}
}
void start_analog_thread()
{
osThreadDef(thread_def, analog_polling_thread, osPriorityLow, 0, 4*512);
void start_analog_thread() {
osThreadDef(thread_def, analog_polling_thread, osPriorityLow, 0, 512 / sizeof(StackType_t));
osThreadCreate(osThread(thread_def), NULL);
}
void HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef *hspi)
{
if(hspi->pRxBuffPtr == (uint8_t*)axes[0]->encoder_.abs_spi_dma_rx_)
axes[0]->encoder_.abs_spi_cb();
else if (hspi->pRxBuffPtr == (uint8_t*)axes[1]->encoder_.abs_spi_dma_rx_)
axes[1]->encoder_.abs_spi_cb();
}
Firmware/MotorControl/low_level.h
+5 -1
View File
@@ -22,7 +22,9 @@ extern const float adc_full_scale;
extern const float adc_ref_voltage;
/* Exported variables --------------------------------------------------------*/
extern float vbus_voltage;
extern float ibus_;
extern bool brake_resistor_armed;
extern bool brake_resistor_saturated;
extern uint16_t adc_measurements_[ADC_CHANNEL_COUNT];
/* Exported macro ------------------------------------------------------------*/
/* Exported functions --------------------------------------------------------*/
@@ -49,7 +51,9 @@ void sync_timers(TIM_HandleTypeDef* htim_a, TIM_HandleTypeDef* htim_b,
uint16_t TIM_CLOCKSOURCE_ITRx, uint16_t count_offset,
TIM_HandleTypeDef* htim_refbase = nullptr);
void start_general_purpose_adc();
float get_adc_voltage(GPIO_TypeDef* GPIO_port, uint16_t GPIO_pin);
float get_adc_voltage(const GPIO_TypeDef* const GPIO_port, uint16_t GPIO_pin);
uint16_t channel_from_gpio(const GPIO_TypeDef* const GPIO_port, uint16_t GPIO_pin);
float get_adc_voltage_channel(uint16_t channel);
void pwm_in_init();
void start_analog_thread();
Firmware/MotorControl/main.cpp
+120 -44
View File
@@ -3,45 +3,60 @@
#include "odrive_main.h"
#include "nvm_config.hpp"
#include "usart.h"
#include "freertos_vars.h"
#include <communication/interface_usb.h>
#include <communication/interface_uart.h>
#include <communication/interface_i2c.h>
#include <communication/interface_can.hpp>
BoardConfig_t board_config;
ODriveCAN::Config_t can_config;
Encoder::Config_t encoder_configs[AXIS_COUNT];
SensorlessEstimator::Config_t sensorless_configs[AXIS_COUNT];
Controller::Config_t controller_configs[AXIS_COUNT];
Motor::Config_t motor_configs[AXIS_COUNT];
OnboardThermistorCurrentLimiter::Config_t fet_thermistor_configs[AXIS_COUNT];
OffboardThermistorCurrentLimiter::Config_t motor_thermistor_configs[AXIS_COUNT];
Axis::Config_t axis_configs[AXIS_COUNT];
TrapezoidalTrajectory::Config_t trap_configs[AXIS_COUNT];
bool user_config_loaded_;
Endstop::Config_t min_endstop_configs[AXIS_COUNT];
Endstop::Config_t max_endstop_configs[AXIS_COUNT];
SystemStats_t system_stats_ = { 0 };
Axis *axes[AXIS_COUNT];
std::array<Axis*, AXIS_COUNT> axes;
ODriveCAN *odCAN = nullptr;
ODrive odrv{};
typedef Config<
BoardConfig_t,
ODriveCAN::Config_t,
Encoder::Config_t[AXIS_COUNT],
SensorlessEstimator::Config_t[AXIS_COUNT],
Controller::Config_t[AXIS_COUNT],
Motor::Config_t[AXIS_COUNT],
OnboardThermistorCurrentLimiter::Config_t[AXIS_COUNT],
OffboardThermistorCurrentLimiter::Config_t[AXIS_COUNT],
TrapezoidalTrajectory::Config_t[AXIS_COUNT],
Endstop::Config_t[AXIS_COUNT],
Endstop::Config_t[AXIS_COUNT],
Axis::Config_t[AXIS_COUNT]> ConfigFormat;
void save_configuration(void) {
void ODrive::save_configuration(void) {
if (ConfigFormat::safe_store_config(
&board_config,
&odrv.config_,
&can_config,
&encoder_configs,
&sensorless_configs,
&controller_configs,
&motor_configs,
&fet_thermistor_configs,
&motor_thermistor_configs,
&trap_configs,
&min_endstop_configs,
&max_endstop_configs,
&axis_configs)) {
//printf("saving configuration failed\r\n"); osDelay(5);
printf("saving configuration failed\r\n"); osDelay(5);
} else {
user_config_loaded_ = true;
odrv.user_config_loaded_ = true;
}
}
@@ -49,37 +64,56 @@ extern "C" int load_configuration(void) {
// Try to load configs
if (NVM_init() ||
ConfigFormat::safe_load_config(
&board_config,
&odrv.config_,
&can_config,
&encoder_configs,
&sensorless_configs,
&controller_configs,
&motor_configs,
&fet_thermistor_configs,
&motor_thermistor_configs,
&trap_configs,
&min_endstop_configs,
&max_endstop_configs,
&axis_configs)) {
//If loading failed, restore defaults
board_config = BoardConfig_t();
odrv.config_ = BoardConfig_t();
can_config = ODriveCAN::Config_t();
for (size_t i = 0; i < AXIS_COUNT; ++i) {
encoder_configs[i] = Encoder::Config_t();
sensorless_configs[i] = SensorlessEstimator::Config_t();
controller_configs[i] = Controller::Config_t();
motor_configs[i] = Motor::Config_t();
fet_thermistor_configs[i] = OnboardThermistorCurrentLimiter::Config_t();
motor_thermistor_configs[i] = OffboardThermistorCurrentLimiter::Config_t();
trap_configs[i] = TrapezoidalTrajectory::Config_t();
axis_configs[i] = Axis::Config_t();
// Default step/dir pins are different, so we need to explicitly load them
Axis::load_default_step_dir_pin_config(hw_configs[i].axis_config, &axis_configs[i]);
Axis::load_default_can_id(i, axis_configs[i]);
min_endstop_configs[i] = Endstop::Config_t();
max_endstop_configs[i] = Endstop::Config_t();
controller_configs[i].load_encoder_axis = i;
}
} else {
user_config_loaded_ = true;
odrv.user_config_loaded_ = true;
}
return user_config_loaded_;
return odrv.user_config_loaded_;
}
void erase_configuration(void) {
void ODrive::erase_configuration(void) {
NVM_erase();
// FIXME: this reboot is a workaround because we don't want the next save_configuration
// to write back the old configuration from RAM to NVM. The proper action would
// be to reset the values in RAM to default. However right now that's not
// practical because several startup actions depend on the config. The
// other problem is that the stack overflows if we reset to default here.
NVIC_SystemReset();
}
void enter_dfu_mode() {
if ((hw_version_major == 3) && (hw_version_minor >= 5)) {
void ODrive::enter_dfu_mode() {
if ((hw_version_major_ == 3) && (hw_version_minor_ >= 5)) {
__asm volatile ("CPSID I\n\t":::"memory"); // disable interrupts
_reboot_cookie = 0xDEADBEEF;
NVIC_SystemReset();
@@ -96,30 +130,9 @@ void enter_dfu_mode() {
}
}
extern "C" {
int odrive_main(void);
void vApplicationStackOverflowHook(xTaskHandle *pxTask, signed portCHAR *pcTaskName) {
for (;;); // TODO: safe action
}
void vApplicationIdleHook(void) {
if (system_stats_.fully_booted) {
system_stats_.uptime = xTaskGetTickCount();
system_stats_.min_heap_space = xPortGetMinimumEverFreeHeapSize();
system_stats_.min_stack_space_comms = uxTaskGetStackHighWaterMark(comm_thread) * sizeof(StackType_t);
system_stats_.min_stack_space_axis0 = uxTaskGetStackHighWaterMark(axes[0]->thread_id_) * sizeof(StackType_t);
system_stats_.min_stack_space_axis1 = uxTaskGetStackHighWaterMark(axes[1]->thread_id_) * sizeof(StackType_t);
system_stats_.min_stack_space_usb = uxTaskGetStackHighWaterMark(usb_thread) * sizeof(StackType_t);
system_stats_.min_stack_space_uart = uxTaskGetStackHighWaterMark(uart_thread) * sizeof(StackType_t);
system_stats_.min_stack_space_usb_irq = uxTaskGetStackHighWaterMark(usb_irq_thread) * sizeof(StackType_t);
system_stats_.min_stack_space_startup = uxTaskGetStackHighWaterMark(defaultTaskHandle) * sizeof(StackType_t);
}
}
}
int odrive_main(void) {
extern "C" int construct_objects(){
#if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 3
if (board_config.enable_i2c_instead_of_can) {
if (odrv.config_.enable_i2c_instead_of_can) {
// Set up the direction GPIO as input
GPIO_InitTypeDef GPIO_InitStruct;
GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
@@ -142,6 +155,10 @@ int odrive_main(void) {
#endif
MX_CAN1_Init();
HAL_UART_DeInit(&huart4);
huart4.Init.BaudRate = odrv.config_.uart_baudrate;
HAL_UART_Init(&huart4);
// Init general user ADC on some GPIOs.
GPIO_InitTypeDef GPIO_InitStruct;
GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
@@ -160,25 +177,80 @@ int odrive_main(void) {
#endif
// Construct all objects.
odCAN = new ODriveCAN(can_config, &hcan1);
for (size_t i = 0; i < AXIS_COUNT; ++i) {
Encoder *encoder = new Encoder(hw_configs[i].encoder_config,
encoder_configs[i]);
encoder_configs[i], motor_configs[i]);
SensorlessEstimator *sensorless_estimator = new SensorlessEstimator(sensorless_configs[i]);
Controller *controller = new Controller(controller_configs[i]);
OnboardThermistorCurrentLimiter *fet_thermistor = new OnboardThermistorCurrentLimiter(hw_configs[i].thermistor_config,
fet_thermistor_configs[i]);
OffboardThermistorCurrentLimiter *motor_thermistor = new OffboardThermistorCurrentLimiter(motor_thermistor_configs[i]);
Motor *motor = new Motor(hw_configs[i].motor_config,
hw_configs[i].gate_driver_config,
motor_configs[i]);
TrapezoidalTrajectory *trap = new TrapezoidalTrajectory(trap_configs[i]);
Endstop *min_endstop = new Endstop(min_endstop_configs[i]);
Endstop *max_endstop = new Endstop(max_endstop_configs[i]);
axes[i] = new Axis(i, hw_configs[i].axis_config, axis_configs[i],
*encoder, *sensorless_estimator, *controller, *motor, *trap);
*encoder, *sensorless_estimator, *controller, *fet_thermistor,
*motor_thermistor, *motor, *trap, *min_endstop, *max_endstop);
controller_configs[i].parent = controller;
encoder_configs[i].parent = encoder;
motor_thermistor_configs[i].parent = motor_thermistor;
motor_configs[i].parent = motor;
min_endstop_configs[i].parent = min_endstop;
max_endstop_configs[i].parent = max_endstop;
axis_configs[i].parent = axes[i];
}
return 0;
}
extern "C" {
int odrive_main(void);
void vApplicationStackOverflowHook(xTaskHandle *pxTask, signed portCHAR *pcTaskName) {
for(auto& axis : axes){
safety_critical_disarm_motor_pwm(axis->motor_);
}
safety_critical_disarm_brake_resistor();
for (;;); // TODO: safe action
}
void vApplicationIdleHook(void) {
if (odrv.system_stats_.fully_booted) {
odrv.system_stats_.uptime = xTaskGetTickCount();
odrv.system_stats_.min_heap_space = xPortGetMinimumEverFreeHeapSize();
odrv.system_stats_.min_stack_space_comms = uxTaskGetStackHighWaterMark(comm_thread) * sizeof(StackType_t);
odrv.system_stats_.min_stack_space_axis0 = uxTaskGetStackHighWaterMark(axes[0]->thread_id_) * sizeof(StackType_t);
odrv.system_stats_.min_stack_space_axis1 = uxTaskGetStackHighWaterMark(axes[1]->thread_id_) * sizeof(StackType_t);
odrv.system_stats_.min_stack_space_usb = uxTaskGetStackHighWaterMark(usb_thread) * sizeof(StackType_t);
odrv.system_stats_.min_stack_space_uart = uxTaskGetStackHighWaterMark(uart_thread) * sizeof(StackType_t);
odrv.system_stats_.min_stack_space_usb_irq = uxTaskGetStackHighWaterMark(usb_irq_thread) * sizeof(StackType_t);
odrv.system_stats_.min_stack_space_startup = uxTaskGetStackHighWaterMark(defaultTaskHandle) * sizeof(StackType_t);
odrv.system_stats_.min_stack_space_can = uxTaskGetStackHighWaterMark(odCAN->thread_id_) * sizeof(StackType_t);
// Actual usage, in bytes, so we don't have to math
odrv.system_stats_.stack_usage_axis0 = axes[0]->stack_size_ - odrv.system_stats_.min_stack_space_axis0;
odrv.system_stats_.stack_usage_axis1 = axes[1]->stack_size_ - odrv.system_stats_.min_stack_space_axis1;
odrv.system_stats_.stack_usage_comms = stack_size_comm_thread - odrv.system_stats_.min_stack_space_comms;
odrv.system_stats_.stack_usage_usb = stack_size_usb_thread - odrv.system_stats_.min_stack_space_usb;
odrv.system_stats_.stack_usage_uart = stack_size_uart_thread - odrv.system_stats_.min_stack_space_uart;
odrv.system_stats_.stack_usage_usb_irq = stack_size_usb_irq_thread - odrv.system_stats_.min_stack_space_usb_irq;
odrv.system_stats_.stack_usage_startup = stack_size_default_task - odrv.system_stats_.min_stack_space_startup;
odrv.system_stats_.stack_usage_can = odCAN->stack_size_ - odrv.system_stats_.min_stack_space_can;
}
}
}
int odrive_main(void) {
// Start ADC for temperature measurements and user measurements
start_general_purpose_adc();
// TODO: make dynamically reconfigurable
#if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 3
if (board_config.enable_uart) {
if (odrv.config_.enable_uart) {
SetGPIO12toUART();
}
#endif
@@ -195,6 +267,10 @@ int odrive_main(void) {
axes[i]->setup();
}
for(auto& axis : axes){
axis->encoder_.setup();
}
// Start PWM and enable adc interrupts/callbacks
start_adc_pwm();
@@ -215,6 +291,6 @@ int odrive_main(void) {
start_analog_thread();
system_stats_.fully_booted = true;
odrv.system_stats_.fully_booted = true;
return 0;
}
Firmware/MotorControl/motor.cpp
+118 -57
View File
@@ -50,6 +50,7 @@ bool Motor::arm() {
void Motor::reset_current_control() {
current_control_.v_current_control_integral_d = 0.0f;
current_control_.v_current_control_integral_q = 0.0f;
current_control_.acim_rotor_flux = 0.0f;
}
// @brief Tune the current controller based on phase resistance and inductance
@@ -72,9 +73,9 @@ void Motor::DRV8301_setup() {
// Solve for exact gain, then snap down to have equal or larger range as requested
// or largest possible range otherwise
static const float kMargin = 0.90f;
static const float kTripMargin = 1.0f; // Trip level is at edge of linear range of amplifer
static const float max_output_swing = 1.35f; // [V] out of amplifier
constexpr float kMargin = 0.90f;
constexpr float kTripMargin = 1.0f; // Trip level is at edge of linear range of amplifer
constexpr float max_output_swing = 1.35f; // [V] out of amplifier
float max_unity_gain_current = kMargin * max_output_swing * hw_config_.shunt_conductance; // [A]
float requested_gain = max_unity_gain_current / config_.requested_current_range; // [V/V]
@@ -126,7 +127,7 @@ bool Motor::check_DRV_fault() {
GPIO_PinState nFAULT_state = HAL_GPIO_ReadPin(gate_driver_config_.nFAULT_port, gate_driver_config_.nFAULT_pin);
if (nFAULT_state == GPIO_PIN_RESET) {
// Update DRV Fault Code
drv_fault_ = DRV8301_getFaultType(&gate_driver_);
gate_driver_exported_.drv_fault = (GateDriverIntf::DrvFault)DRV8301_getFaultType(&gate_driver_);
// Update/Cache all SPI device registers
// DRV_SPI_8301_Vars_t* local_regs = &gate_driver_regs_;
// local_regs->RcvCmd = true;
@@ -136,43 +137,19 @@ bool Motor::check_DRV_fault() {
return true;
}
void Motor::set_error(Motor::Error_t error){
void Motor::set_error(Motor::Error error){
error_ |= error;
axis_->error_ |= Axis::ERROR_MOTOR_FAILED;
safety_critical_disarm_motor_pwm(*this);
update_brake_current();
}
float Motor::get_inverter_temp() {
float adc = adc_measurements_[hw_config_.inverter_thermistor_adc_ch];
float normalized_voltage = adc / adc_full_scale;
return horner_fma(normalized_voltage, thermistor_poly_coeffs, thermistor_num_coeffs);
}
bool Motor::update_thermal_limits() {
float fet_temp = get_inverter_temp();
float temp_margin = config_.inverter_temp_limit_upper - fet_temp;
float derating_range = config_.inverter_temp_limit_upper - config_.inverter_temp_limit_lower;
thermal_current_lim_ = config_.current_lim * (temp_margin / derating_range);
if (!(thermal_current_lim_ >= 0.0f)) { //Funny polarity to also catch NaN
thermal_current_lim_ = 0.0f;
}
if (fet_temp > config_.inverter_temp_limit_upper + 5) {
set_error(ERROR_INVERTER_OVER_TEMP);
return false;
}
return true;
}
bool Motor::do_checks() {
if (!check_DRV_fault()) {
set_error(ERROR_DRV_FAULT);
return false;
}
if (!update_thermal_limits()) {
//error already set in function
return false;
}
return true;
}
@@ -181,14 +158,34 @@ float Motor::effective_current_lim() {
float current_lim = config_.current_lim;
// Hardware limit
if (axis_->motor_.config_.motor_type == Motor::MOTOR_TYPE_GIMBAL) {
current_lim = std::min(current_lim, 0.98f*one_by_sqrt3*vbus_voltage);
current_lim = std::min(current_lim, 0.98f*one_by_sqrt3*vbus_voltage); //gimbal motor is voltage control
} else {
current_lim = std::min(current_lim, axis_->motor_.current_control_.max_allowed_current);
}
// Thermal limit
current_lim = std::min(current_lim, thermal_current_lim_);
return current_lim;
// Apply axis current limiters
for (const CurrentLimiter* const limiter : axis_->current_limiters_) {
current_lim = std::min(current_lim, limiter->get_current_limit(config_.current_lim));
}
effective_current_lim_ = current_lim;
return effective_current_lim_;
}
//return the maximum available torque for the motor.
//Note - for ACIM motors, available torque is allowed to be 0.
float Motor::max_available_torque() {
if (config_.motor_type == Motor::MOTOR_TYPE_ACIM) {
float max_torque = effective_current_lim() * config_.torque_constant * current_control_.acim_rotor_flux;
max_torque = std::clamp(max_torque, 0.0f, config_.torque_lim);
return max_torque;
}
else {
float max_torque = effective_current_lim() * config_.torque_constant;
max_torque = std::clamp(max_torque, 0.0f, config_.torque_lim);
return max_torque;
}
}
void Motor::log_timing(TimingLog_t log_idx) {
@@ -215,7 +212,7 @@ float Motor::phase_current_from_adcval(uint32_t ADCValue) {
// TODO check Ibeta balance to verify good motor connection
bool Motor::measure_phase_resistance(float test_current, float max_voltage) {
static const float kI = 10.0f; // [(V/s)/A]
static const int num_test_cycles = static_cast<int>(3.0f / CURRENT_MEAS_PERIOD); // Test runs for 3s
static const int num_test_cycles = (int)(3.0f / CURRENT_MEAS_PERIOD); // Test runs for 3s
float test_voltage = 0.0f;
size_t i = 0;
@@ -284,7 +281,8 @@ bool Motor::measure_phase_inductance(float voltage_low, float voltage_high) {
bool Motor::run_calibration() {
float R_calib_max_voltage = config_.resistance_calib_max_voltage;
if (config_.motor_type == MOTOR_TYPE_HIGH_CURRENT) {
if (config_.motor_type == MOTOR_TYPE_HIGH_CURRENT
|| config_.motor_type == MOTOR_TYPE_ACIM) {
if (!measure_phase_resistance(config_.calibration_current, R_calib_max_voltage))
return false;
if (!measure_phase_inductance(-R_calib_max_voltage, R_calib_max_voltage))
@@ -327,7 +325,7 @@ bool Motor::FOC_voltage(float v_d, float v_q, float pwm_phase) {
float c = our_arm_cos_f32(pwm_phase);
float s = our_arm_sin_f32(pwm_phase);
float v_alpha = c*v_d - s*v_q;
float v_beta = c*v_q + s*v_d;
float v_beta = c*v_q + s*v_d;
return enqueue_voltage_timings(v_alpha, v_beta);
}
@@ -339,8 +337,7 @@ bool Motor::FOC_current(float Id_des, float Iq_des, float I_phase, float pwm_pha
ictrl.Iq_setpoint = Iq_des;
// Check for current sense saturation
if (fabsf(current_meas_.phB) > ictrl.overcurrent_trip_level
|| fabsf(current_meas_.phC) > ictrl.overcurrent_trip_level) {
if (std::abs(current_meas_.phB) > ictrl.overcurrent_trip_level || std::abs(current_meas_.phC) > ictrl.overcurrent_trip_level) {
set_error(ERROR_CURRENT_SENSE_SATURATION);
return false;
}
@@ -358,9 +355,9 @@ bool Motor::FOC_current(float Id_des, float Iq_des, float I_phase, float pwm_pha
ictrl.Id_measured += ictrl.I_measured_report_filter_k * (Id - ictrl.Id_measured);
// Check for violation of current limit
float I_trip = config_.current_lim_tolerance * effective_current_lim();
float I_trip = effective_current_lim() + config_.current_lim_margin;
if (SQ(Id) + SQ(Iq) > SQ(I_trip)) {
set_error(ERROR_CURRENT_UNSTABLE);
set_error(ERROR_CURRENT_LIMIT_VIOLATION);
return false;
}
@@ -399,7 +396,7 @@ bool Motor::FOC_current(float Id_des, float Iq_des, float I_phase, float pwm_pha
float c_p = our_arm_cos_f32(pwm_phase);
float s_p = our_arm_sin_f32(pwm_phase);
float mod_alpha = c_p * mod_d - s_p * mod_q;
float mod_beta = c_p * mod_q + s_p * mod_d;
float mod_beta = c_p * mod_q + s_p * mod_d;
// Report final applied voltage in stationary frame (for sensorles estimator)
ictrl.final_v_alpha = mod_to_V * mod_alpha;
@@ -410,30 +407,94 @@ bool Motor::FOC_current(float Id_des, float Iq_des, float I_phase, float pwm_pha
return false; // error set inside enqueue_modulation_timings
log_timing(TIMING_LOG_FOC_CURRENT);
if (axis_->axis_num_ == 0) {
// Edit these to suit your capture needs
float trigger_data = ictrl.v_current_control_integral_d;
float trigger_threshold = 0.5f;
float sample_data = Ialpha;
static bool ready = false;
static bool capturing = false;
if (trigger_data < trigger_threshold) {
ready = true;
}
if (ready && trigger_data >= trigger_threshold) {
capturing = true;
ready = false;
}
if (capturing) {
oscilloscope[oscilloscope_pos] = sample_data;
if (++oscilloscope_pos >= OSCILLOSCOPE_SIZE) {
oscilloscope_pos = 0;
capturing = false;
}
}
}
return true;
}
bool Motor::update(float current_setpoint, float phase, float phase_vel) {
current_setpoint *= config_.direction;
// torque_setpoint [Nm]
// phase [rad electrical]
// phase_vel [rad/s electrical]
bool Motor::update(float torque_setpoint, float phase, float phase_vel) {
float current_setpoint = 0.0f;
phase *= config_.direction;
phase_vel *= config_.direction;
if (config_.motor_type == MOTOR_TYPE_ACIM) {
current_setpoint = torque_setpoint / (config_.torque_constant * fmax(current_control_.acim_rotor_flux, config_.acim_gain_min_flux));
}
else {
current_setpoint = torque_setpoint / config_.torque_constant;
}
current_setpoint *= config_.direction;
// TODO: 2-norm vs independent clamping (current could be sqrt(2) bigger)
float ilim = effective_current_lim();
float id = std::clamp(current_control_.Id_setpoint, -ilim, ilim);
float iq = std::clamp(current_setpoint, -ilim, ilim);
if (config_.motor_type == MOTOR_TYPE_ACIM) {
// Note that the effect of the current commands on the real currents is actually 1.5 PWM cycles later
// However the rotor time constant is (usually) so slow that it doesn't matter
// So we elect to write it as if the effect is immediate, to have cleaner code
if (config_.acim_autoflux_enable) {
float abs_iq = fabsf(iq);
float gain = abs_iq > id ? config_.acim_autoflux_attack_gain : config_.acim_autoflux_decay_gain;
id += gain * (abs_iq - id) * current_meas_period;
id = std::clamp(id, config_.acim_autoflux_min_Id, ilim);
current_control_.Id_setpoint = id;
}
// acim_rotor_flux is normalized to units of [A] tracking Id; rotor inductance is unspecified
float dflux_by_dt = config_.acim_slip_velocity * (id - current_control_.acim_rotor_flux);
current_control_.acim_rotor_flux += dflux_by_dt * current_meas_period;
float slip_velocity = config_.acim_slip_velocity * (iq / current_control_.acim_rotor_flux);
// Check for issues with small denominator. Polarity of check to catch NaN too
bool acceptable_vel = fabsf(slip_velocity) <= 0.1f * (float)current_meas_hz;
if (!acceptable_vel)
slip_velocity = 0.0f;
phase_vel += slip_velocity;
// reporting only:
current_control_.async_phase_vel = slip_velocity;
current_control_.async_phase_offset += slip_velocity * current_meas_period;
current_control_.async_phase_offset = wrap_pm_pi(current_control_.async_phase_offset);
phase += current_control_.async_phase_offset;
phase = wrap_pm_pi(phase);
}
float pwm_phase = phase + 1.5f * current_meas_period * phase_vel;
// Execute current command
// TODO: move this into the mot
if (config_.motor_type == MOTOR_TYPE_HIGH_CURRENT) {
if(!FOC_current(0.0f, current_setpoint, phase, pwm_phase)){
return false;
}
} else if (config_.motor_type == MOTOR_TYPE_GIMBAL) {
//In gimbal motor mode, current is reinterptreted as voltage.
if(!FOC_voltage(0.0f, current_setpoint, pwm_phase))
return false;
} else {
set_error(ERROR_NOT_IMPLEMENTED_MOTOR_TYPE);
return false;
switch(config_.motor_type){
case MOTOR_TYPE_HIGH_CURRENT: return FOC_current(id, iq, phase, pwm_phase); break;
case MOTOR_TYPE_ACIM: return FOC_current(id, iq, phase, pwm_phase); break;
case MOTOR_TYPE_GIMBAL: return FOC_voltage(id, iq, pwm_phase); break;
default: set_error(ERROR_NOT_IMPLEMENTED_MOTOR_TYPE); return false; break;
}
return true;
}
Firmware/MotorControl/motor.hpp
+44 -124
View File
@@ -7,31 +7,8 @@
#include "drv8301.h"
class Motor {
class Motor : public ODriveIntf::MotorIntf {
public:
enum Error_t {
ERROR_NONE = 0,
ERROR_PHASE_RESISTANCE_OUT_OF_RANGE = 0x0001,
ERROR_PHASE_INDUCTANCE_OUT_OF_RANGE = 0x0002,
ERROR_ADC_FAILED = 0x0004,
ERROR_DRV_FAULT = 0x0008,
ERROR_CONTROL_DEADLINE_MISSED = 0x0010,
ERROR_NOT_IMPLEMENTED_MOTOR_TYPE = 0x0020,
ERROR_BRAKE_CURRENT_OUT_OF_RANGE = 0x0040,
ERROR_MODULATION_MAGNITUDE = 0x0080,
ERROR_BRAKE_DEADTIME_VIOLATION = 0x0100,
ERROR_UNEXPECTED_TIMER_CALLBACK = 0x0200,
ERROR_CURRENT_SENSE_SATURATION = 0x0400,
ERROR_INVERTER_OVER_TEMP = 0x0800,
ERROR_CURRENT_UNSTABLE = 0x1000
};
enum MotorType_t {
MOTOR_TYPE_HIGH_CURRENT = 0,
// MOTOR_TYPE_LOW_CURRENT = 1, //Not yet implemented
MOTOR_TYPE_GIMBAL = 2
};
struct Iph_BC_t {
float phB;
float phC;
@@ -46,16 +23,20 @@ public:
// Voltage applied at end of cycle:
float final_v_alpha; // [V]
float final_v_beta; // [V]
float Id_setpoint; // [A]
float Iq_setpoint; // [A]
float Iq_measured; // [A]
float Id_measured; // [A]
float I_measured_report_filter_k;
float max_allowed_current; // [A]
float overcurrent_trip_level; // [A]
float acim_rotor_flux; // [A]
float async_phase_vel; // [rad/s electrical]
float async_phase_offset; // [rad electrical]
};
// NOTE: for gimbal motors, all units of A are instead V.
// example: vel_gain is [V/(count/s)] instead of [A/(count/s)]
// NOTE: for gimbal motors, all units of Nm are instead V.
// example: vel_gain is [V/(turn/s)] instead of [Nm/(turn/s)]
// example: current_lim and calibration_current will instead determine the maximum voltage applied to the motor.
struct Config_t {
bool pre_calibrated = false; // can be set to true to indicate that all values here are valid
@@ -64,37 +45,35 @@ public:
float resistance_calib_max_voltage = 2.0f; // [V] - You may need to increase this if this voltage isn't sufficient to drive calibration_current through the motor.
float phase_inductance = 0.0f; // to be set by measure_phase_inductance
float phase_resistance = 0.0f; // to be set by measure_phase_resistance
float torque_constant = 0.04f; // [Nm/A] for PM motors, [Nm/A^2] for induction motors. Equal to 8.27/Kv of the motor
int32_t direction = 0; // 1 or -1 (0 = unspecified)
MotorType_t motor_type = MOTOR_TYPE_HIGH_CURRENT;
MotorType motor_type = MOTOR_TYPE_HIGH_CURRENT;
// Read out max_allowed_current to see max supported value for current_lim.
// float current_lim = 70.0f; //[A]
float current_lim = 10.0f; //[A]
float current_lim_tolerance = 1.25f; // multiple of current_lim
float current_lim = 10.0f; //[A]
float current_lim_margin = 8.0f; // Maximum violation of current_lim
float torque_lim = std::numeric_limits<float>::infinity(); //[Nm].
// Value used to compute shunt amplifier gains
float requested_current_range = 60.0f; // [A]
float current_control_bandwidth = 1000.0f; // [rad/s]
float inverter_temp_limit_lower = 100;
float inverter_temp_limit_upper = 120;
};
float acim_slip_velocity = 14.706f; // [rad/s electrical] = 1/rotor_tau
float acim_gain_min_flux = 10; // [A]
float acim_autoflux_min_Id = 10; // [A]
bool acim_autoflux_enable = false;
float acim_autoflux_attack_gain = 10.0f;
float acim_autoflux_decay_gain = 1.0f;
enum TimingLog_t {
TIMING_LOG_GENERAL,
TIMING_LOG_ADC_CB_I,
TIMING_LOG_ADC_CB_DC,
TIMING_LOG_MEAS_R,
TIMING_LOG_MEAS_L,
TIMING_LOG_ENC_CALIB,
TIMING_LOG_IDX_SEARCH,
TIMING_LOG_FOC_VOLTAGE,
TIMING_LOG_FOC_CURRENT,
TIMING_LOG_NUM_SLOTS
};
enum ArmedState_t {
ARMED_STATE_DISARMED,
ARMED_STATE_WAITING_FOR_TIMINGS,
ARMED_STATE_WAITING_FOR_UPDATE,
ARMED_STATE_ARMED,
// custom property setters
Motor* parent = nullptr;
void set_pre_calibrated(bool value) {
pre_calibrated = value;
parent->is_calibrated_ = parent->is_calibrated_ || parent->config_.pre_calibrated;
}
void set_phase_inductance(float value) { phase_inductance = value; parent->update_current_controller_gains(); }
void set_phase_resistance(float value) { phase_resistance = value; parent->update_current_controller_gains(); }
void set_current_control_bandwidth(float value) { current_control_bandwidth = value; parent->update_current_controller_gains(); }
};
Motor(const MotorHardwareConfig_t& hw_config,
@@ -111,11 +90,10 @@ public:
void update_current_controller_gains();
void DRV8301_setup();
bool check_DRV_fault();
void set_error(Error_t error);
void set_error(Error error);
bool do_checks();
float get_inverter_temp();
bool update_thermal_limits();
float effective_current_lim();
float max_available_torque();
void log_timing(TimingLog_t log_idx);
float phase_current_from_adcval(uint32_t ADCValue);
bool measure_phase_resistance(float test_current, float max_voltage);
@@ -143,13 +121,17 @@ public:
bool next_timings_valid_ = false;
uint16_t last_cpu_time_ = 0;
int timing_log_index_ = 0;
uint16_t timing_log_[TIMING_LOG_NUM_SLOTS] = { 0 };
struct {
uint16_t& operator[](size_t idx) { return content[idx]; }
uint16_t& get(size_t idx) { return content[idx]; }
uint16_t content[TIMING_LOG_NUM_SLOTS];
} timing_log_;
// variables exposed on protocol
Error_t error_ = ERROR_NONE;
Error error_ = ERROR_NONE;
// Do not write to this variable directly!
// It is for exclusive use by the safety_critical_... functions.
ArmedState_t armed_state_ = ARMED_STATE_DISARMED;
ArmedState armed_state_ = ARMED_STATE_DISARMED;
bool is_calibrated_ = config_.pre_calibrated;
Iph_BC_t current_meas_ = {0.0f, 0.0f};
Iph_BC_t DC_calib_ = {0.0f, 0.0f};
@@ -162,84 +144,22 @@ public:
.Ibus = 0.0f,
.final_v_alpha = 0.0f,
.final_v_beta = 0.0f,
.Id_setpoint = 0.0f,
.Iq_setpoint = 0.0f,
.Iq_measured = 0.0f,
.Id_measured = 0.0f,
.I_measured_report_filter_k = 1.0f,
.max_allowed_current = 0.0f,
.overcurrent_trip_level = 0.0f,
.acim_rotor_flux = 0.0f,
.async_phase_vel = 0.0f,
.async_phase_offset = 0.0f,
};
DRV8301_FaultType_e drv_fault_ = DRV8301_FaultType_NoFault;
struct : GateDriverIntf {
DrvFault drv_fault = DRV_FAULT_NO_FAULT;
} gate_driver_exported_;
DRV_SPI_8301_Vars_t gate_driver_regs_; //Local view of DRV registers (initialized by DRV8301_setup)
float thermal_current_lim_ = 10.0f; //[A]
// Communication protocol definitions
auto make_protocol_definitions() {
return make_protocol_member_list(
make_protocol_property("error", &error_),
make_protocol_ro_property("armed_state", &armed_state_),
make_protocol_ro_property("is_calibrated", &is_calibrated_),
make_protocol_ro_property("current_meas_phB", &current_meas_.phB),
make_protocol_ro_property("current_meas_phC", &current_meas_.phC),
make_protocol_property("DC_calib_phB", &DC_calib_.phB),
make_protocol_property("DC_calib_phC", &DC_calib_.phC),
make_protocol_property("phase_current_rev_gain", &phase_current_rev_gain_),
make_protocol_ro_property("thermal_current_lim", &thermal_current_lim_),
make_protocol_function("get_inverter_temp", *this, &Motor::get_inverter_temp),
make_protocol_object("current_control",
make_protocol_property("p_gain", &current_control_.p_gain),
make_protocol_property("i_gain", &current_control_.i_gain),
make_protocol_property("v_current_control_integral_d", &current_control_.v_current_control_integral_d),
make_protocol_property("v_current_control_integral_q", &current_control_.v_current_control_integral_q),
make_protocol_property("Ibus", &current_control_.Ibus),
make_protocol_property("final_v_alpha", &current_control_.final_v_alpha),
make_protocol_property("final_v_beta", &current_control_.final_v_beta),
make_protocol_property("Iq_setpoint", &current_control_.Iq_setpoint),
make_protocol_property("Iq_measured", &current_control_.Iq_measured),
make_protocol_property("Id_measured", &current_control_.Id_measured),
make_protocol_property("I_measured_report_filter_k", &current_control_.I_measured_report_filter_k),
make_protocol_ro_property("max_allowed_current", &current_control_.max_allowed_current),
make_protocol_ro_property("overcurrent_trip_level", &current_control_.overcurrent_trip_level)
),
make_protocol_object("gate_driver",
make_protocol_ro_property("drv_fault", &drv_fault_)
// make_protocol_ro_property("status_reg_1", &gate_driver_regs_.Stat_Reg_1_Value),
// make_protocol_ro_property("status_reg_2", &gate_driver_regs_.Stat_Reg_2_Value),
// make_protocol_ro_property("ctrl_reg_1", &gate_driver_regs_.Ctrl_Reg_1_Value),
// make_protocol_ro_property("ctrl_reg_2", &gate_driver_regs_.Ctrl_Reg_2_Value)
),
make_protocol_object("timing_log",
make_protocol_ro_property("TIMING_LOG_GENERAL", &timing_log_[TIMING_LOG_GENERAL]),
make_protocol_ro_property("TIMING_LOG_ADC_CB_I", &timing_log_[TIMING_LOG_ADC_CB_I]),
make_protocol_ro_property("TIMING_LOG_ADC_CB_DC", &timing_log_[TIMING_LOG_ADC_CB_DC]),
make_protocol_ro_property("TIMING_LOG_MEAS_R", &timing_log_[TIMING_LOG_MEAS_R]),
make_protocol_ro_property("TIMING_LOG_MEAS_L", &timing_log_[TIMING_LOG_MEAS_L]),
make_protocol_ro_property("TIMING_LOG_ENC_CALIB", &timing_log_[TIMING_LOG_ENC_CALIB]),
make_protocol_ro_property("TIMING_LOG_IDX_SEARCH", &timing_log_[TIMING_LOG_IDX_SEARCH]),
make_protocol_ro_property("TIMING_LOG_FOC_VOLTAGE", &timing_log_[TIMING_LOG_FOC_VOLTAGE]),
make_protocol_ro_property("TIMING_LOG_FOC_CURRENT", &timing_log_[TIMING_LOG_FOC_CURRENT])
),
make_protocol_object("config",
make_protocol_property("pre_calibrated", &config_.pre_calibrated),
make_protocol_property("pole_pairs", &config_.pole_pairs),
make_protocol_property("calibration_current", &config_.calibration_current),
make_protocol_property("resistance_calib_max_voltage", &config_.resistance_calib_max_voltage),
make_protocol_property("phase_inductance", &config_.phase_inductance),
make_protocol_property("phase_resistance", &config_.phase_resistance),
make_protocol_property("direction", &config_.direction),
make_protocol_property("motor_type", &config_.motor_type),
make_protocol_property("current_lim", &config_.current_lim),
make_protocol_property("current_lim_tolerance", &config_.current_lim_tolerance),
make_protocol_property("inverter_temp_limit_lower", &config_.inverter_temp_limit_lower),
make_protocol_property("inverter_temp_limit_upper", &config_.inverter_temp_limit_upper),
make_protocol_property("requested_current_range", &config_.requested_current_range),
make_protocol_property("current_control_bandwidth", &config_.current_control_bandwidth,
[](void* ctx) { static_cast<Motor*>(ctx)->update_current_controller_gains(); }, this)
)
);
}
float effective_current_lim_ = 10.0f;
};
DEFINE_ENUM_FLAG_OPERATORS(Motor::Error_t)
#endif // __MOTOR_HPP
Firmware/MotorControl/nvm.c
+4 -2
View File
@@ -362,10 +362,12 @@ int NVM_commit(void) {
read_sector_ = 1 - read_sector_;
// invalidate the other sector
if (read_sector->index < read_sector->n_data)
if (read_sector->index < read_sector->n_data) {
status = set_allocation_state(read_sector, read_sector->index, 1, INVALID);
else
read_sector->index += 1;
} else {
status = erase(read_sector);
}
return status;
}
Firmware/MotorControl/odrive_main.h
+183 -18
View File
@@ -10,6 +10,8 @@
#ifdef __cplusplus
#include <fibre/protocol.hpp>
#include <communication/interface_usb.h>
#include <communication/interface_i2c.h>
extern "C" {
#endif
@@ -33,16 +35,21 @@ extern "C" {
//default timeout waiting for phase measurement signals
#define PH_CURRENT_MEAS_TIMEOUT 2 // [ms]
//TODO clean this up
// Period in [s]
static const float current_meas_period = CURRENT_MEAS_PERIOD;
// Frequency in [Hz]
static const int current_meas_hz = CURRENT_MEAS_HZ;
// extern const float elec_rad_per_enc;
extern uint32_t _reboot_cookie;
extern bool user_config_loaded_;
extern uint64_t serial_number;
extern char serial_number_str[13];
#ifdef __cplusplus
}
typedef struct {
bool fully_booted;
uint32_t uptime; // [ms]
@@ -54,14 +61,23 @@ typedef struct {
uint32_t min_stack_space_uart;
uint32_t min_stack_space_usb_irq;
uint32_t min_stack_space_startup;
} SystemStats_t;
extern SystemStats_t system_stats_;
uint32_t min_stack_space_can;
#ifdef __cplusplus
}
uint32_t stack_usage_axis0;
uint32_t stack_usage_axis1;
uint32_t stack_usage_comms;
uint32_t stack_usage_usb;
uint32_t stack_usage_uart;
uint32_t stack_usage_usb_irq;
uint32_t stack_usage_startup;
uint32_t stack_usage_can;
USBStats_t& usb = usb_stats_;
I2CStats_t& i2c = i2c_stats_;
} SystemStats_t;
struct PWMMapping_t {
endpoint_ref_t endpoint = { 0 };
endpoint_ref_t endpoint;
float min = 0;
float max = 0;
};
@@ -71,6 +87,7 @@ struct BoardConfig_t {
bool enable_uart = true;
bool enable_i2c_instead_of_can = false;
bool enable_ascii_protocol_on_usb = true;
float max_regen_current = 0.0f;
#if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 5 && HW_VERSION_VOLTAGE >= 48
float brake_resistance = 2.0f; // [ohm]
#else
@@ -81,20 +98,71 @@ struct BoardConfig_t {
//<! This protects against cases in which the power supply fails to dissipate
//<! the brake power if the brake resistor is disabled.
//<! The default is 26V for the 24V board version and 52V for the 48V board version.
/**
* If enabled, if the measured DC voltage exceeds `dc_bus_overvoltage_ramp_start`,
* the ODrive will sink more power than usual into the the brake resistor
* in an attempt to bring the voltage down again.
*
* The brake duty cycle is increased by the following amount:
* vbus_voltage == dc_bus_overvoltage_ramp_start => brake_duty_cycle += 0%
* vbus_voltage == dc_bus_overvoltage_ramp_end => brake_duty_cycle += 100%
*
* Remarks:
* - This feature is active even when all motors are disarmed.
* - This feature is disabled if `brake_resistance` is non-positive.
*/
bool enable_dc_bus_overvoltage_ramp = false;
float dc_bus_overvoltage_ramp_start = 1.07f * HW_VERSION_VOLTAGE; //!< See `enable_dc_bus_overvoltage_ramp`.
//!< Do not set this lower than your usual vbus_voltage,
//!< unless you like fried brake resistors.
float dc_bus_overvoltage_ramp_end = 1.07f * HW_VERSION_VOLTAGE; //!< See `enable_dc_bus_overvoltage_ramp`.
//!< Must be larger than `dc_bus_overvoltage_ramp_start`,
//!< otherwise the ramp feature is disabled.
float dc_max_positive_current = INFINITY; // Max current [A] the power supply can source
float dc_max_negative_current = -0.000001f; // Max current [A] the power supply can sink. You most likely want a non-positive value here. Set to -INFINITY to disable.
PWMMapping_t pwm_mappings[GPIO_COUNT];
PWMMapping_t analog_mappings[GPIO_COUNT];
};
extern BoardConfig_t board_config;
extern bool user_config_loaded_;
/**
* Defines the baudrate used on the UART interface.
* Some baudrates will have a small timing error due to hardware limitations.
*
* Here's an (incomplete) list of baudrates for ODrive v3.x:
*
* Configured | Actual | Error [%]
* -------------|---------------|-----------
* 1.2 KBps | 1.2 KBps | 0
* 2.4 KBps | 2.4 KBps | 0
* 9.6 KBps | 9.6 KBps | 0
* 19.2 KBps | 19.195 KBps | 0.02
* 38.4 KBps | 38.391 KBps | 0.02
* 57.6 KBps | 57.613 KBps | 0.02
* 115.2 KBps | 115.068 KBps | 0.11
* 230.4 KBps | 230.769 KBps | 0.16
* 460.8 KBps | 461.538 KBps | 0.16
* 921.6 KBps | 913.043 KBps | 0.93
* 1.792 MBps | 1.826 MBps | 1.9
* 1.8432 MBps | 1.826 MBps | 0.93
*
* For more information refer to Section 30.3.4 and Table 142 (the column with f_PCLK = 42 MHz) in the STM datasheet:
* https://www.st.com/content/ccc/resource/technical/document/reference_manual/3d/6d/5a/66/b4/99/40/d4/DM00031020.pdf/files/DM00031020.pdf/jcr:content/translations/en.DM00031020.pdf
*/
uint32_t uart_baudrate = 115200;
};
// Forward Declarations
class Axis;
class Motor;
class ODriveCAN;
constexpr size_t AXIS_COUNT = 2;
extern Axis *axes[AXIS_COUNT];
extern std::array<Axis*, AXIS_COUNT> axes;
extern ODriveCAN *odCAN;
// if you use the oscilloscope feature you can bump up this value
#define OSCILLOSCOPE_SIZE 128
#define OSCILLOSCOPE_SIZE 4096
extern float oscilloscope[OSCILLOSCOPE_SIZE];
extern size_t oscilloscope_pos;
@@ -110,23 +178,120 @@ inline ENUMTYPE &operator ^= (ENUMTYPE &a, ENUMTYPE b) { return reinterpret_cast
inline ENUMTYPE operator ~ (ENUMTYPE a) { return static_cast<ENUMTYPE>(~static_cast<std::underlying_type_t<ENUMTYPE>>(a)); }
enum TimingLog_t {
TIMING_LOG_GENERAL,
TIMING_LOG_ADC_CB_I,
TIMING_LOG_ADC_CB_DC,
TIMING_LOG_MEAS_R,
TIMING_LOG_MEAS_L,
TIMING_LOG_ENC_CALIB,
TIMING_LOG_IDX_SEARCH,
TIMING_LOG_FOC_VOLTAGE,
TIMING_LOG_FOC_CURRENT,
TIMING_LOG_SPI_START,
TIMING_LOG_SAMPLE_NOW,
TIMING_LOG_SPI_END,
TIMING_LOG_NUM_SLOTS
};
#include "autogen/interfaces.hpp"
// ODrive specific includes
#include <utils.h>
#include <utils.hpp>
#include <gpio_utils.hpp>
#include <low_level.h>
#include <motor.hpp>
#include <encoder.hpp>
#include <sensorless_estimator.hpp>
#include <controller.hpp>
#include <motor.hpp>
#include <current_limiter.hpp>
#include <thermistor.hpp>
#include <trapTraj.hpp>
#include <endstop.hpp>
#include <axis.hpp>
#include <communication/communication.h>
#endif // __cplusplus
// Defined in autogen/version.c based on git-derived version numbers
extern "C" {
extern const unsigned char fw_version_major_;
extern const unsigned char fw_version_minor_;
extern const unsigned char fw_version_revision_;
extern const unsigned char fw_version_unreleased_;
}
// general system functions defined in main.cpp
void save_configuration(void);
void erase_configuration(void);
void enter_dfu_mode(void);
class ODrive : public ODriveIntf {
public:
void save_configuration() override;
void erase_configuration() override;
void reboot() override { NVIC_SystemReset(); }
void enter_dfu_mode() override;
float get_oscilloscope_val(uint32_t index) override {
return oscilloscope[index];
}
float get_adc_voltage(uint32_t gpio) override {
return ::get_adc_voltage(get_gpio_port_by_pin(gpio), get_gpio_pin_by_pin(gpio));
}
int32_t test_function(int32_t delta) override {
static int cnt = 0;
return cnt += delta;
}
Axis& get_axis(int num) { return *axes[num]; }
ODriveCAN& get_can() { return *odCAN; }
float& vbus_voltage_ = ::vbus_voltage; // TODO: make this the actual variable
float& ibus_ = ::ibus_; // TODO: make this the actual variable
float ibus_report_filter_k_ = 1.0f;
const uint64_t& serial_number_ = ::serial_number;
#if HW_VERSION_MAJOR == 3
// Determine start address of the OTP struct:
// The OTP is organized into 16-byte blocks.
// If the first block starts with "0xfe" we use the first block.
// If the first block starts with "0x00" and the second block starts with "0xfe",
// we use the second block. This gives the user the chance to screw up once.
// If none of the above is the case, we consider the OTP invalid (otp_ptr will be NULL).
const uint8_t* otp_ptr =
(*(uint8_t*)FLASH_OTP_BASE == 0xfe) ? (uint8_t*)FLASH_OTP_BASE :
(*(uint8_t*)FLASH_OTP_BASE != 0x00) ? NULL :
(*(uint8_t*)(FLASH_OTP_BASE + 0x10) != 0xfe) ? NULL :
(uint8_t*)(FLASH_OTP_BASE + 0x10);
// Read hardware version from OTP if available, otherwise fall back
// to software defined version.
const uint8_t hw_version_major_ = otp_ptr ? otp_ptr[3] : HW_VERSION_MAJOR;
const uint8_t hw_version_minor_ = otp_ptr ? otp_ptr[4] : HW_VERSION_MINOR;
const uint8_t hw_version_variant_ = otp_ptr ? otp_ptr[5] : HW_VERSION_VOLTAGE;
#else
#error "not implemented"
#endif
// the corresponding macros are defined in the autogenerated version.h
const uint8_t fw_version_major_ = ::fw_version_major_;
const uint8_t fw_version_minor_ = ::fw_version_minor_;
const uint8_t fw_version_revision_ = ::fw_version_revision_;
const uint8_t fw_version_unreleased_ = ::fw_version_unreleased_; // 0 for official releases, 1 otherwise
bool& brake_resistor_armed_ = ::brake_resistor_armed; // TODO: make this the actual variable
bool& brake_resistor_saturated_ = ::brake_resistor_saturated; // TODO: make this the actual variable
SystemStats_t system_stats_;
BoardConfig_t config_;
bool user_config_loaded_;
uint32_t test_property_ = 0;
};
extern ODrive odrv; // defined in main.cpp
#endif // __cplusplus
#endif /* __ODRIVE_MAIN_H */
Firmware/MotorControl/sensorless_estimator.cpp
+2
View File
@@ -65,6 +65,7 @@ bool SensorlessEstimator::update() {
// Check that we don't get problems with discrete time approximation
if (!(current_meas_period * pll_kp < 1.0f)) {
error_ |= ERROR_UNSTABLE_GAIN;
vel_estimate_valid_ = false;
return false;
}
@@ -77,5 +78,6 @@ bool SensorlessEstimator::update() {
// update PLL velocity
vel_estimate_ += current_meas_period * pll_ki * delta_phase;
vel_estimate_valid_ = true;
return true;
};
Firmware/MotorControl/sensorless_estimator.hpp
+3 -26
View File
@@ -1,13 +1,8 @@
#ifndef __SENSORLESS_ESTIMATOR_HPP
#define __SENSORLESS_ESTIMATOR_HPP
class SensorlessEstimator {
class SensorlessEstimator : public ODriveIntf::SensorlessEstimatorIntf {
public:
enum Error_t {
ERROR_NONE = 0,
ERROR_UNSTABLE_GAIN = 0x01,
};
struct Config_t {
float observer_gain = 1000.0f; // [rad/s]
float pll_bandwidth = 1000.0f; // [rad/s]
@@ -22,34 +17,16 @@ public:
Config_t& config_;
// TODO: expose on protocol
Error_t error_ = ERROR_NONE;
Error error_ = ERROR_NONE;
float phase_ = 0.0f; // [rad]
float pll_pos_ = 0.0f; // [rad]
float vel_estimate_ = 0.0f; // [rad/s]
bool vel_estimate_valid_ = false;
// float pll_kp_ = 0.0f; // [rad/s / rad]
// float pll_ki_ = 0.0f; // [(rad/s^2) / rad]
float flux_state_[2] = {0.0f, 0.0f}; // [Vs]
float V_alpha_beta_memory_[2] = {0.0f, 0.0f}; // [V]
bool estimator_good_ = false;
// Communication protocol definitions
auto make_protocol_definitions() {
return make_protocol_member_list(
make_protocol_property("error", &error_),
make_protocol_property("phase", &phase_),
make_protocol_property("pll_pos", &pll_pos_),
make_protocol_property("vel_estimate", &vel_estimate_),
// make_protocol_property("pll_kp", &pll_kp_),
// make_protocol_property("pll_ki", &pll_ki_),
make_protocol_object("config",
make_protocol_property("observer_gain", &config_.observer_gain),
make_protocol_property("pll_bandwidth", &config_.pll_bandwidth),
make_protocol_property("pm_flux_linkage", &config_.pm_flux_linkage)
)
);
}
};
DEFINE_ENUM_FLAG_OPERATORS(SensorlessEstimator::Error_t)
#endif /* __SENSORLESS_ESTIMATOR_HPP */
Firmware/MotorControl/thermistor.cpp
+80
View File
@@ -0,0 +1,80 @@
#include "odrive_main.h"
#include "low_level.h"
ThermistorCurrentLimiter::ThermistorCurrentLimiter(uint16_t adc_channel,
const float* const coefficients,
size_t num_coeffs,
const float& temp_limit_lower,
const float& temp_limit_upper,
const bool& enabled) :
adc_channel_(adc_channel),
coefficients_(coefficients),
num_coeffs_(num_coeffs),
temperature_(NAN),
temp_limit_lower_(temp_limit_lower),
temp_limit_upper_(temp_limit_upper),
enabled_(enabled),
error_(ERROR_NONE)
{
}
void ThermistorCurrentLimiter::update() {
const float voltage = get_adc_voltage_channel(adc_channel_);
const float normalized_voltage = voltage / adc_ref_voltage;
temperature_ = horner_fma(normalized_voltage, coefficients_, num_coeffs_);
}
bool ThermistorCurrentLimiter::do_checks() {
if (enabled_ && temperature_ >= temp_limit_upper_ + 5) {
error_ = ERROR_OVER_TEMP;
axis_->error_ |= Axis::ERROR_OVER_TEMP;
return false;
}
return true;
}
float ThermistorCurrentLimiter::get_current_limit(float base_current_lim) const {
if (!enabled_) {
return base_current_lim;
}
const float temp_margin = temp_limit_upper_ - temperature_;
const float derating_range = temp_limit_upper_ - temp_limit_lower_;
float thermal_current_lim = base_current_lim * (temp_margin / derating_range);
if (!(thermal_current_lim >= 0.0f)) { // Funny polarity to also catch NaN
thermal_current_lim = 0.0f;
}
return std::min(thermal_current_lim, base_current_lim);
}
OnboardThermistorCurrentLimiter::OnboardThermistorCurrentLimiter(const ThermistorHardwareConfig_t& hw_config, Config_t& config) :
ThermistorCurrentLimiter(hw_config.adc_ch,
hw_config.coeffs,
hw_config.num_coeffs,
config.temp_limit_lower,
config.temp_limit_upper,
config.enabled),
config_(config)
{
}
OffboardThermistorCurrentLimiter::OffboardThermistorCurrentLimiter(Config_t& config) :
ThermistorCurrentLimiter(UINT16_MAX,
&config.thermistor_poly_coeffs[0],
num_coeffs_,
config.temp_limit_lower,
config.temp_limit_upper,
config.enabled),
config_(config)
{
decode_pin();
}
void OffboardThermistorCurrentLimiter::decode_pin() {
const GPIO_TypeDef* const port = get_gpio_port_by_pin(config_.gpio_pin);
const uint16_t pin = get_gpio_pin_by_pin(config_.gpio_pin);
adc_channel_ = channel_from_gpio(port, pin);
}
Firmware/MotorControl/thermistor.hpp
+74
View File
@@ -0,0 +1,74 @@
#ifndef __THERMISTOR_HPP
#define __THERMISTOR_HPP
#ifndef __ODRIVE_MAIN_H
#error "This file should not be included directly. Include odrive_main.h instead."
#endif
class ThermistorCurrentLimiter : public CurrentLimiter, public ODriveIntf::ThermistorCurrentLimiterIntf {
public:
virtual ~ThermistorCurrentLimiter() = default;
ThermistorCurrentLimiter(uint16_t adc_channel,
const float* const coefficients,
size_t num_coeffs,
const float& temp_limit_lower,
const float& temp_limit_upper,
const bool& enabled);
void update();
bool do_checks();
float get_current_limit(float base_current_lim) const override;
uint16_t adc_channel_;
const float* const coefficients_;
const size_t num_coeffs_;
float temperature_;
const float& temp_limit_lower_;
const float& temp_limit_upper_;
const bool& enabled_;
Error error_;
Axis* axis_ = nullptr; // set by Axis constructor
};
class OnboardThermistorCurrentLimiter : public ThermistorCurrentLimiter, public ODriveIntf::OnboardThermistorCurrentLimiterIntf {
public:
struct Config_t {
float temp_limit_lower = 100;
float temp_limit_upper = 120;
bool enabled = true;
};
virtual ~OnboardThermistorCurrentLimiter() = default;
OnboardThermistorCurrentLimiter(const ThermistorHardwareConfig_t& hw_config, Config_t& config);
Config_t& config_;
};
class OffboardThermistorCurrentLimiter : public ThermistorCurrentLimiter, public ODriveIntf::OffboardThermistorCurrentLimiterIntf {
public:
static const size_t num_coeffs_ = 4;
struct Config_t {
float thermistor_poly_coeffs[num_coeffs_];
uint16_t gpio_pin = 4;
float temp_limit_lower = 100;
float temp_limit_upper = 120;
bool enabled = false;
// custom setters
OffboardThermistorCurrentLimiter* parent;
void set_gpio_pin(uint16_t value) { gpio_pin = value; parent->decode_pin(); }
};
virtual ~OffboardThermistorCurrentLimiter() = default;
OffboardThermistorCurrentLimiter(Config_t& config);
Config_t& config_;
private:
void decode_pin();
};
#endif // __THERMISTOR_HPP
Firmware/MotorControl/timer.hpp
+42
View File
@@ -0,0 +1,42 @@
#pragma once
#include <algorithm>
template <class T>
class Timer {
public:
void setTimeout(const T timeout) {
timeout_ = timeout;
}
void setIncrement(const T increment) {
increment_ = increment;
}
void start() {
running_ = true;
}
void stop() {
running_ = false;
}
// If the timer is started, increment the timer
void update() {
if (running_)
timer_ = std::min<T>(timer_ + increment_, timeout_);
}
void reset() {
timer_ = static_cast<T>(0);
}
bool expired() {
return timer_ >= timeout_;
}
private:
T timer_ = static_cast<T>(0); // Current state
T timeout_ = static_cast<T>(0); // Time to count
T increment_ = static_cast<T>(0); // Amount to increment each time update() is called
bool running_ = false; // update() only increments if runing_ is true
};
Firmware/MotorControl/trapTraj.cpp
+2 -2
View File
@@ -1,6 +1,6 @@
#include <math.h>
#include "odrive_main.h"
#include "utils.h"
#include "utils.hpp"
// A sign function where input 0 has positive sign (not 0)
float sign_hard(float val) {
@@ -44,7 +44,7 @@ bool TrapezoidalTrajectory::planTrapezoidal(float Xf, float Xi, float Vi,
// Are we displacing enough to reach cruising speed?
if (s*dX < s*dXmin) {
// Short move (triangle profile)
Vr_ = s * sqrtf((Dr_*SQ(Vi) + 2*Ar_*Dr_*dX) / (Dr_ - Ar_));
Vr_ = s * sqrtf(std::fmax((Dr_*SQ(Vi) + 2*Ar_*Dr_*dX) / (Dr_ - Ar_), 0.0f));
Ta_ = std::max(0.0f, (Vr_ - Vi) / Ar_);
Td_ = std::max(0.0f, -Vr_ / Dr_);
Tv_ = 0.0f;
Firmware/MotorControl/trapTraj.hpp
+5 -15
View File
@@ -4,10 +4,9 @@
class TrapezoidalTrajectory {
public:
struct Config_t {
float vel_limit = 20000.0f; // [count/s]
float accel_limit = 5000.0f; // [count/s^2]
float decel_limit = 5000.0f; // [count/s^2]
float A_per_css = 0.0f; // [A/(count/s^2)]
float vel_limit = 2.0f; // [turn/s]
float accel_limit = 0.5f; // [turn/s^2]
float decel_limit = 0.5f; // [turn/s^2]
};
struct Step_t {
@@ -21,17 +20,6 @@ public:
float Vmax, float Amax, float Dmax);
Step_t eval(float t);
auto make_protocol_definitions() {
return make_protocol_member_list(
make_protocol_object("config",
make_protocol_property("vel_limit", &config_.vel_limit),
make_protocol_property("accel_limit", &config_.accel_limit),
make_protocol_property("decel_limit", &config_.decel_limit),
make_protocol_property("A_per_css", &config_.A_per_css)
)
);
}
Axis* axis_ = nullptr; // set by Axis constructor
Config_t& config_;
@@ -49,6 +37,8 @@ public:
float Tf_;
float yAccel_;
float t_;
};
#endif
Firmware/MotorControl/utils.c → Firmware/MotorControl/utils.cpp
+2 -2
View File
@@ -1,5 +1,5 @@
#include <utils.h>
#include <utils.hpp>
#include <math.h>
#include <float.h>
#include <cmsis_os.h>
@@ -151,7 +151,7 @@ float fast_atan2(float y, float x) {
// p(x) = coeffs[0] * x^deg + ... + coeffs[deg], for some degree "deg"
float horner_fma(float x, const float *coeffs, size_t count) {
float result = 0.0f;
for (int idx = 0; idx < count; ++idx)
for (size_t idx = 0; idx < count; ++idx)
result = fmaf(result, x, coeffs[idx]);
return result;
}

Some files were not shown because too many files have changed in this diff Show More