diff --git a/.github/workflows/arduino.yml b/.github/workflows/arduino.yml new file mode 100644 index 00000000..cadd8ccc --- /dev/null +++ b/.github/workflows/arduino.yml @@ -0,0 +1,53 @@ +name: AVR +on: [push, pull_request] +jobs: + lint: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v2 + - uses: arduino/arduino-lint-action@v1 + with: + library-manager: update + project-type: library + build: + name: Test compiling + runs-on: ubuntu-latest + + strategy: + matrix: + arduino-boards-fqbn: + - arduino:avr:uno # arudino uno + - arduino:sam:arduino_due_x # arduino due + - arduino:avr:mega # arduino mega2650 + - arduino:avr:leonardo # arduino leonardo + + include: + - arduino-boards-fqbn: arduino:avr:uno # arudino uno - compiling almost all examples + sketch-names: '**.ino' + required-libraries: PciManager + sketches-exclude: teensy4_current_control_low_side, full_control_serial, angle_control, bluepill_position_control, esp32_position_control, esp32_i2c_dual_bus_example, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, osc_esp32_3pwm, osc_esp32_fullcontrol, nano33IoT_velocity_control, smartstepper_control,esp32_current_control_low_side, stm32_spi_alt_example, esp32_spi_alt_example, B_G431B_ESC1, odrive_example_spi, odrive_example_encoder, single_full_control_example, double_full_control_example, stm32_current_control_low_side, open_loop_velocity_6pwm + + - arduino-boards-fqbn: arduino:sam:arduino_due_x # arduino due - one full example + sketch-names: single_full_control_example.ino + + - arduino-boards-fqbn: arduino:avr:leonardo # arduino leonardo - one full example + sketch-names: open_loop_position_example.ino + + - arduino-boards-fqbn: arduino:avr:mega # arduino mega2660 - one full example + sketch-names: single_full_control_example.ino + + + # Do not cancel all jobs / architectures if one job fails + fail-fast: false + steps: + - name: Checkout + uses: actions/checkout@master + - name: Compile all examples + uses: ArminJo/arduino-test-compile@master + with: + arduino-board-fqbn: ${{ matrix.arduino-boards-fqbn }} + required-libraries: ${{ matrix.required-libraries }} + platform-url: ${{ matrix.platform-url }} + sketch-names: ${{ matrix.sketch-names }} + sketches-exclude: ${{ matrix.sketches-exclude }} + build-properties: ${{ toJson(matrix.build-properties) }} diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml deleted file mode 100644 index 6c396ea9..00000000 --- a/.github/workflows/ccpp.yml +++ /dev/null @@ -1,109 +0,0 @@ -name: Library Compile -on: [push, pull_request] -jobs: - lint: - runs-on: ubuntu-latest - steps: - - uses: actions/checkout@v2 - - uses: arduino/arduino-lint-action@v1 - with: - library-manager: update - project-type: library - build: - name: Test compiling - runs-on: ubuntu-latest - - strategy: - matrix: - arduino-boards-fqbn: - - arduino:avr:uno # arudino uno - - arduino:sam:arduino_due_x # arduino due - - arduino:avr:mega # arduino mega2650 - - arduino:avr:leonardo # arduino leonardo - - arduino:samd:nano_33_iot # samd21 - - adafruit:samd:adafruit_metro_m4 # samd51 - - esp32:esp32:esp32doit-devkit-v1 # esp32 - - esp32:esp32:esp32s2 # esp32s2 - - esp32:esp32:esp32s3 # esp32s3 - - STMicroelectronics:stm32:GenF1:pnum=BLUEPILL_F103C8 # stm32 bluepill - - STMicroelectronics:stm32:Nucleo_64:pnum=NUCLEO_F411RE # stm32 nucleo - - STMicroelectronics:stm32:GenF4:pnum=GENERIC_F405RGTX # stm32f405 - odrive - - STMicroelectronics:stm32:GenL4:pnum=GENERIC_L475RGTX # stm32l475 - - STMicroelectronics:stm32:Disco:pnum=B_G431B_ESC1 # B-G431-ESC1 - - arduino:mbed_rp2040:pico # rpi pico - - include: - - arduino-boards-fqbn: arduino:avr:uno # arudino uno - compiling almost all examples - sketch-names: '**.ino' - required-libraries: PciManager - sketches-exclude: full_control_serial, angle_control, bluepill_position_control, esp32_position_control, esp32_i2c_dual_bus_example, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, osc_esp32_3pwm, osc_esp32_fullcontrol, nano33IoT_velocity_control, smartstepper_control,esp32_current_control_low_side, stm32_spi_alt_example, esp32_spi_alt_example, B_G431B_ESC1, odrive_example_spi, odrive_example_encoder, single_full_control_example, double_full_control_example, stm32_current_control_low_side, open_loop_velocity_6pwm - - - arduino-boards-fqbn: arduino:sam:arduino_due_x # arduino due - one full example - sketch-names: single_full_control_example.ino - - - arduino-boards-fqbn: arduino:avr:leonardo # arduino leonardo - one full example - sketch-names: open_loop_position_example.ino - - - arduino-boards-fqbn: arduino:avr:mega # arduino mega2660 - one full example - sketch-names: single_full_control_example.ino - - - arduino-boards-fqbn: arduino:samd:nano_33_iot # samd21 - sketch-names: nano33IoT_velocity_control.ino, smartstepper_control.ino - - - arduino-boards-fqbn: arduino:mbed_rp2040:pico # raspberry pi pico - one example - sketch-names: open_loop_position_example.ino - - - arduino-boards-fqbn: adafruit:samd:adafruit_metro_m4 # samd51 - one full example - platform-url: https://adafruit.github.io/arduino-board-index/package_adafruit_index.json - sketch-names: single_full_control_example.ino - - - arduino-boards-fqbn: esp32:esp32:esp32s2 # esp32s2 - platform-url: https://espressif.github.io/arduino-esp32/package_esp32_index.json - sketch-names: bldc_driver_3pwm_standalone.ino,stepper_driver_2pwm_standalone.ino,stepper_driver_4pwm_standalone.ino - - - arduino-boards-fqbn: esp32:esp32:esp32s3 # esp32s3 - platform-url: https://espressif.github.io/arduino-esp32/package_esp32_index.json - sketch-names: esp32_position_control.ino, esp32_i2c_dual_bus_example.ino - - - arduino-boards-fqbn: esp32:esp32:esp32doit-devkit-v1 # esp32 - platform-url: https://espressif.github.io/arduino-esp32/package_esp32_index.json - sketch-names: esp32_position_control.ino, esp32_i2c_dual_bus_example.ino, esp32_current_control_low_side.ino, esp32_spi_alt_example.ino - - - arduino-boards-fqbn: STMicroelectronics:stm32:GenF1:pnum=BLUEPILL_F103C8 # bluepill - hs examples - platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json - sketch-names: bluepill_position_control.ino, stm32_i2c_dual_bus_example.ino, stm32_spi_alt_example.ino - - - arduino-boards-fqbn: STMicroelectronics:stm32:Disco:pnum=B_G431B_ESC1 # B-G431-ESC1 - platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json - sketch-names: B_G431B_ESC1.ino - build-properties: - B_G431B_ESC1: - -DHAL_OPAMP_MODULE_ENABLED - - - arduino-boards-fqbn: STMicroelectronics:stm32:GenF4:pnum=GENERIC_F405RGTX # stm32f405 - odrive - platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json - sketch-names: odrive_example_encoder.ino, odrive_example_spi.ino, stm32_current_control_low_side.ino - - - arduino-boards-fqbn: STMicroelectronics:stm32:GenL4:pnum=GENERIC_L475RGTX # stm32l475 - platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json - sketch-names: single_full_control_example.ino, stm32_spi_alt_example.ino, double_full_control_example.ino, stm32_current_control_low_side.ino - - - arduino-boards-fqbn: STMicroelectronics:stm32:Nucleo_64:pnum=NUCLEO_F411RE # nucleo one full example - platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json - sketch-names: single_full_control_example.ino, stm32_spi_alt_example.ino, double_full_control_example.ino, stm32_current_control_low_side.ino - - - # Do not cancel all jobs / architectures if one job fails - fail-fast: false - steps: - - name: Checkout - uses: actions/checkout@master - - name: Compile all examples - uses: ArminJo/arduino-test-compile@master - with: - arduino-board-fqbn: ${{ matrix.arduino-boards-fqbn }} - required-libraries: ${{ matrix.required-libraries }} - platform-url: ${{ matrix.platform-url }} - sketch-names: ${{ matrix.sketch-names }} - sketches-exclude: ${{ matrix.sketches-exclude }} - build-properties: ${{ toJson(matrix.build-properties) }} diff --git a/.github/workflows/esp32.yml b/.github/workflows/esp32.yml new file mode 100644 index 00000000..e0899bf1 --- /dev/null +++ b/.github/workflows/esp32.yml @@ -0,0 +1,50 @@ +name: ESP32 +on: [push, pull_request] +jobs: + lint: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v2 + - uses: arduino/arduino-lint-action@v1 + with: + library-manager: update + project-type: library + build: + name: Test compiling + runs-on: ubuntu-latest + + strategy: + matrix: + arduino-boards-fqbn: + - esp32:esp32:esp32doit-devkit-v1 # esp32 + - esp32:esp32:esp32s2 # esp32s2 + - esp32:esp32:esp32s3 # esp32s3 + + include: + + - arduino-boards-fqbn: esp32:esp32:esp32s2 # esp32s2 + platform-url: https://espressif.github.io/arduino-esp32/package_esp32_index.json + sketch-names: bldc_driver_3pwm_standalone.ino,stepper_driver_2pwm_standalone.ino,stepper_driver_4pwm_standalone.ino + + - arduino-boards-fqbn: esp32:esp32:esp32s3 # esp32s3 + platform-url: https://espressif.github.io/arduino-esp32/package_esp32_index.json + sketch-names: esp32_position_control.ino, esp32_i2c_dual_bus_example.ino + + - arduino-boards-fqbn: esp32:esp32:esp32doit-devkit-v1 # esp32 + platform-url: https://espressif.github.io/arduino-esp32/package_esp32_index.json + sketch-names: esp32_position_control.ino, esp32_i2c_dual_bus_example.ino, esp32_current_control_low_side.ino, esp32_spi_alt_example.ino + + # Do not cancel all jobs / architectures if one job fails + fail-fast: false + steps: + - name: Checkout + uses: actions/checkout@master + - name: Compile all examples + uses: ArminJo/arduino-test-compile@master + with: + arduino-board-fqbn: ${{ matrix.arduino-boards-fqbn }} + required-libraries: ${{ matrix.required-libraries }} + platform-url: ${{ matrix.platform-url }} + sketch-names: ${{ matrix.sketch-names }} + sketches-exclude: ${{ matrix.sketches-exclude }} + build-properties: ${{ toJson(matrix.build-properties) }} diff --git a/.github/workflows/rpi.yml b/.github/workflows/rpi.yml new file mode 100644 index 00000000..d6d72b95 --- /dev/null +++ b/.github/workflows/rpi.yml @@ -0,0 +1,39 @@ +name: RP2040 +on: [push, pull_request] +jobs: + lint: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v2 + - uses: arduino/arduino-lint-action@v1 + with: + library-manager: update + project-type: library + build: + name: Test compiling + runs-on: ubuntu-latest + + strategy: + matrix: + arduino-boards-fqbn: + - arduino:mbed_rp2040:pico # rpi pico + + include: + + - arduino-boards-fqbn: arduino:mbed_rp2040:pico # raspberry pi pico - one example + sketch-names: open_loop_position_example.ino + + # Do not cancel all jobs / architectures if one job fails + fail-fast: false + steps: + - name: Checkout + uses: actions/checkout@master + - name: Compile all examples + uses: ArminJo/arduino-test-compile@master + with: + arduino-board-fqbn: ${{ matrix.arduino-boards-fqbn }} + required-libraries: ${{ matrix.required-libraries }} + platform-url: ${{ matrix.platform-url }} + sketch-names: ${{ matrix.sketch-names }} + sketches-exclude: ${{ matrix.sketches-exclude }} + build-properties: ${{ toJson(matrix.build-properties) }} diff --git a/.github/workflows/samd.yml b/.github/workflows/samd.yml new file mode 100644 index 00000000..c4329869 --- /dev/null +++ b/.github/workflows/samd.yml @@ -0,0 +1,44 @@ +name: SAMD +on: [push, pull_request] +jobs: + lint: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v2 + - uses: arduino/arduino-lint-action@v1 + with: + library-manager: update + project-type: library + build: + name: Test compiling + runs-on: ubuntu-latest + + strategy: + matrix: + arduino-boards-fqbn: + - arduino:samd:nano_33_iot # samd21 + - adafruit:samd:adafruit_metro_m4 # samd51 + + include: + + - arduino-boards-fqbn: arduino:samd:nano_33_iot # samd21 + sketch-names: nano33IoT_velocity_control.ino, smartstepper_control.ino + + - arduino-boards-fqbn: adafruit:samd:adafruit_metro_m4 # samd51 - one full example + platform-url: https://adafruit.github.io/arduino-board-index/package_adafruit_index.json + sketch-names: single_full_control_example.ino + + # Do not cancel all jobs / architectures if one job fails + fail-fast: false + steps: + - name: Checkout + uses: actions/checkout@master + - name: Compile all examples + uses: ArminJo/arduino-test-compile@master + with: + arduino-board-fqbn: ${{ matrix.arduino-boards-fqbn }} + required-libraries: ${{ matrix.required-libraries }} + platform-url: ${{ matrix.platform-url }} + sketch-names: ${{ matrix.sketch-names }} + sketches-exclude: ${{ matrix.sketches-exclude }} + build-properties: ${{ toJson(matrix.build-properties) }} diff --git a/.github/workflows/stm32.yml b/.github/workflows/stm32.yml new file mode 100644 index 00000000..52b5cc94 --- /dev/null +++ b/.github/workflows/stm32.yml @@ -0,0 +1,68 @@ +name: STM32 +on: [push, pull_request] +jobs: + lint: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v2 + - uses: arduino/arduino-lint-action@v1 + with: + library-manager: update + project-type: library + build: + name: Test compiling + runs-on: ubuntu-latest + + strategy: + matrix: + arduino-boards-fqbn: + - STMicroelectronics:stm32:GenF1:pnum=BLUEPILL_F103C8 # stm32 bluepill + - STMicroelectronics:stm32:Nucleo_64:pnum=NUCLEO_F411RE # stm32 nucleo + - STMicroelectronics:stm32:Nucleo_144:pnum=NUCLEO_F746ZG # stm32 nucleo f746zg + - STMicroelectronics:stm32:GenF4:pnum=GENERIC_F405RGTX # stm32f405 - odrive + - STMicroelectronics:stm32:GenL4:pnum=GENERIC_L475RGTX # stm32l475 + - STMicroelectronics:stm32:Disco:pnum=B_G431B_ESC1 # B-G431-ESC1 + + include: + - arduino-boards-fqbn: STMicroelectronics:stm32:GenF1:pnum=BLUEPILL_F103C8 # bluepill - hs examples + platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json + sketch-names: bluepill_position_control.ino, stm32_i2c_dual_bus_example.ino, stm32_spi_alt_example.ino + + - arduino-boards-fqbn: STMicroelectronics:stm32:Disco:pnum=B_G431B_ESC1 # B-G431-ESC1 + platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json + sketch-names: B_G431B_ESC1.ino + build-properties: + B_G431B_ESC1: + -DHAL_OPAMP_MODULE_ENABLED + + - arduino-boards-fqbn: STMicroelectronics:stm32:GenF4:pnum=GENERIC_F405RGTX # stm32f405 - odrive + platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json + sketch-names: odrive_example_encoder.ino, odrive_example_spi.ino, stm32_current_control_low_side.ino + + - arduino-boards-fqbn: STMicroelectronics:stm32:GenL4:pnum=GENERIC_L475RGTX # stm32l475 + platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json + sketch-names: single_full_control_example.ino, stm32_spi_alt_example.ino, double_full_control_example.ino, stm32_current_control_low_side.ino + + - arduino-boards-fqbn: STMicroelectronics:stm32:Nucleo_64:pnum=NUCLEO_F411RE # nucleo one full example + platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json + sketch-names: single_full_control_example.ino, stm32_spi_alt_example.ino, double_full_control_example.ino, stm32_current_control_low_side.ino + + - arduino-boards-fqbn: STMicroelectronics:stm32:Nucleo_144:pnum=NUCLEO_F746ZG # nucleo f7 one full example + platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json + sketch-names: single_full_control_example.ino, stm32_spi_alt_example.ino, double_full_control_example.ino, stm32_current_control_low_side.ino + + + # Do not cancel all jobs / architectures if one job fails + fail-fast: false + steps: + - name: Checkout + uses: actions/checkout@master + - name: Compile all examples + uses: ArminJo/arduino-test-compile@master + with: + arduino-board-fqbn: ${{ matrix.arduino-boards-fqbn }} + required-libraries: ${{ matrix.required-libraries }} + platform-url: ${{ matrix.platform-url }} + sketch-names: ${{ matrix.sketch-names }} + sketches-exclude: ${{ matrix.sketches-exclude }} + build-properties: ${{ toJson(matrix.build-properties) }} diff --git a/.github/workflows/teensy.yml b/.github/workflows/teensy.yml index 9fc88b9a..6ad953fe 100644 --- a/.github/workflows/teensy.yml +++ b/.github/workflows/teensy.yml @@ -1,4 +1,4 @@ -name: PlatformIO - Teensy build +name: Teensy on: [push] @@ -29,6 +29,11 @@ jobs: run: pio ci --lib="." --board=teensy41 --board=teensy40 env: PLATFORMIO_CI_SRC: examples/hardware_specific_examples/Teensy/Teensy4/open_loop_velocity_6pwm/open_loop_velocity_6pwm.ino + + - name: PIO Run Teensy 4 + run: pio ci --lib="." --board=teensy41 --board=teensy40 + env: + PLATFORMIO_CI_SRC: examples/hardware_specific_examples/DRV8302_driver/teensy4_current_control_low_side/teensy4_current_control_low_side.ino - name: PIO Run Teensy 3 run: pio ci --lib="." --board=teensy31 --board=teensy30 --board=teensy35 --board=teensy36 diff --git a/README.md b/README.md index 75475bec..86551f79 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,12 @@ # SimpleFOClibrary - **Simple** Field Oriented Control (FOC) **library**
### A Cross-Platform FOC implementation for BLDC and Stepper motors
based on the Arduino IDE and PlatformIO -![Library Compile](https://github.com/simplefoc/Arduino-FOC/workflows/Library%20Compile/badge.svg) [![PlatformIO - Teensy build](https://github.com/simplefoc/Arduino-FOC/actions/workflows/teensy.yml/badge.svg)](https://github.com/simplefoc/Arduino-FOC/actions/workflows/teensy.yml) +[![AVR build](https://github.com/simplefoc/Arduino-FOC/actions/workflows/arduino.yml/badge.svg)](https://github.com/simplefoc/Arduino-FOC/actions/workflows/arduino.yml) +[![STM32 build](https://github.com/simplefoc/Arduino-FOC/actions/workflows/stm32.yml/badge.svg)](https://github.com/simplefoc/Arduino-FOC/actions/workflows/stm32.yml) +[![ESP32 build](https://github.com/simplefoc/Arduino-FOC/actions/workflows/esp32.yml/badge.svg)](https://github.com/simplefoc/Arduino-FOC/actions/workflows/esp32.yml) +[![RP2040 build](https://github.com/simplefoc/Arduino-FOC/actions/workflows/rpi.yml/badge.svg)](https://github.com/simplefoc/Arduino-FOC/actions/workflows/rpi.yml) +[![SAMD build](https://github.com/simplefoc/Arduino-FOC/actions/workflows/samd.yml/badge.svg)](https://github.com/simplefoc/Arduino-FOC/actions/workflows/samd.yml) +[![Teensy build](https://github.com/simplefoc/Arduino-FOC/actions/workflows/teensy.yml/badge.svg)](https://github.com/simplefoc/Arduino-FOC/actions/workflows/teensy.yml) ![GitHub release (latest by date)](https://img.shields.io/github/v/release/simplefoc/arduino-foc) ![GitHub Release Date](https://img.shields.io/github/release-date/simplefoc/arduino-foc?color=blue) @@ -19,26 +24,25 @@ Additionally, most of the efforts at this moment are still channeled towards the Therefore this is an attempt to: - 🎯 Demystify FOC algorithm and make a robust but simple Arduino library: [Arduino *SimpleFOClibrary*](https://docs.simplefoc.com/arduino_simplefoc_library_showcase) - Support as many motor + sensor + driver + mcu combinations out there -- 🎯 Develop a modular FOC supporting BLDC driver boards: - - ***NEW*** 📢: *Minimalistic* BLDC driver (<3Amps) : [SimpleFOCMini ](https://github.com/simplefoc/SimpleFOCMini). - - *Low-power* gimbal driver (<5Amps) : [*Arduino Simple**FOC**Shield*](https://docs.simplefoc.com/arduino_simplefoc_shield_showcase). - - *Medium-power* BLDC driver (<30Amps): [Arduino SimpleFOCPowerShield ](https://github.com/simplefoc/Arduino-SimpleFOC-PowerShield). - - See also [@byDagor](https://github.com/byDagor)'s *fully-integrated* ESP32 based board: [Dagor Brushless Controller](https://github.com/byDagor/Dagor-Brushless-Controller) - -> NEW RELEASE 📢 : SimpleFOClibrary v2.3.2 -> - Improved [space vector modulation code](https://github.com/simplefoc/Arduino-FOC/pull/309) thanks to [@Candas1](https://github.com/Candas1) -> - Bugfix for stepper motor initialization -> - Bugfix for current sensing when only 2 phase currents available - please re-check your current sense PID tuning -> - Bugfix for teensy3.2 - [#321](https://github.com/simplefoc/Arduino-FOC/pull/321) -> - Added teensy3/4 compile to the github CI using platformio -> - Fix compile issues with recent versions of ESP32 framework -> - Add ADC calibration on STM32 MCUs -> - Bugfix for crash when using ADC2 on ESP32s - [thanks to @mcells](https://github.com/simplefoc/Arduino-FOC/pull/346) -> - Bugfix for renesas PWM on UNO R4 WiFi - [thanks to @facchinm](https://github.com/simplefoc/Arduino-FOC/pull/322) -> - And more bugfixes - see the complete list of 2.3.2 [fixes and PRs](https://github.com/simplefoc/Arduino-FOC/milestone/9?closed=1) - - -## Arduino *SimpleFOClibrary* v2.3.2 +- 🎯 Develop modular and easy to use FOC supporting BLDC driver boards + - For official driver boards see [SimpleFOCBoards](https://docs.simplefoc.com/boards) + - Many many more boards developed by the community members, see [SimpleFOCCommunity](https://community.simplefoc.com/) + +> NEW RELEASE 📢 : SimpleFOClibrary v2.3.3 +> - Teensy4 +> - support for low-side current sensing [#392](https://github.com/simplefoc/Arduino-FOC/pull/392) +> - support for center aligned 6pwm and 3pwm (optional) [#392](https://github.com/simplefoc/Arduino-FOC/pull/392) +> - stm32 +> - support for center aligned pwm (even across multiple timers and motors/drivers) [#374](https://github.com/simplefoc/Arduino-FOC/pull/374), [#388](https://github.com/simplefoc/Arduino-FOC/pull/388) +> - support for DMA based low-side current sensing: [#383](https://github.com/simplefoc/Arduino-FOC/pull/383),[#378](https://github.com/simplefoc/Arduino-FOC/pull/378) +> - support for f7 architecture [#388](https://github.com/simplefoc/Arduino-FOC/pull/388),[#394](https://github.com/simplefoc/Arduino-FOC/pull/394) +> - KV rating calculation fix [#347](https://github.com/simplefoc/Arduino-FOC/pull/347) +> - Much more performant Space Vector PWM calculation [#340](https://github.com/simplefoc/Arduino-FOC/pull/340) +> - And much more: +> - See the complete list of bugfixes and new features of v2.3.3 [fixes and PRs](https://github.com/simplefoc/Arduino-FOC/milestone/10?closed=1) + + +## Arduino *SimpleFOClibrary* v2.3.3

diff --git a/examples/hardware_specific_examples/DRV8302_driver/teensy4_current_control_low_side/teensy4_current_control_low_side.ino b/examples/hardware_specific_examples/DRV8302_driver/teensy4_current_control_low_side/teensy4_current_control_low_side.ino new file mode 100644 index 00000000..841134d8 --- /dev/null +++ b/examples/hardware_specific_examples/DRV8302_driver/teensy4_current_control_low_side/teensy4_current_control_low_side.ino @@ -0,0 +1,168 @@ +/** + * Comprehensive BLDC motor control example using encoder and the DRV8302 board + * + * Using serial terminal user can send motor commands and configure the motor and FOC in real-time: + * - configure PID controller constants + * - change motion control loops + * - monitor motor variabels + * - set target values + * - check all the configuration values + * + * check the https://docs.simplefoc.com for full list of motor commands + * + */ +#include + +// DRV8302 pins connections +// don't forget to connect the common ground pin +#define EN_GATE 11 +#define M_PWM 22 +#define GAIN 20 +#define M_OC 23 +#define OC_ADJ 19 + +#define INH_A 2 +#define INL_A 3 +#define INH_B 8 +#define INL_B 7 +#define INH_C 6 +#define INL_C 9 + +#define IOUTA 14 +#define IOUTB 15 +#define IOUTC 16 + +// Motor instance +BLDCMotor motor = BLDCMotor(7); +BLDCDriver3PWM driver = BLDCDriver3PWM(INH_A, INH_B, INH_C, EN_GATE); + +// DRV8302 board has 0.005Ohm shunt resistors and the gain of 12.22 V/V +LowsideCurrentSense cs = LowsideCurrentSense(0.005f, 12.22f, IOUTA, IOUTB); + +// encoder instance +Encoder encoder = Encoder(10, 11, 2048); + +// Interrupt routine intialisation +// channel A and B callbacks +void doA(){encoder.handleA();} +void doB(){encoder.handleB();} + + +// commander interface +Commander command = Commander(Serial); +void onMotor(char* cmd){ command.motor(&motor, cmd); } + +void setup() { + + // initialize encoder sensor hardware + encoder.init(); + encoder.enableInterrupts(doA, doB); + // link the motor to the sensor + motor.linkSensor(&encoder); + + // DRV8302 specific code + // M_OC - enable overcurrent protection + pinMode(M_OC,OUTPUT); + digitalWrite(M_OC,LOW); + // M_PWM - enable 6pwm mode + pinMode(M_PWM, OUTPUT); + digitalWrite(M_PWM,LOW); // high for 3pwm + // OD_ADJ - set the maximum overcurrent limit possible + // Better option would be to use voltage divisor to set exact value + pinMode(OC_ADJ,OUTPUT); + digitalWrite(OC_ADJ,HIGH); + + + // driver config + // power supply voltage [V] + driver.voltage_power_supply = 19; + driver.pwm_frequency = 20000; // suggested not higher than 22khz + driver.init(); + // link the motor and the driver + motor.linkDriver(&driver); + // link current sense and the driver + cs.linkDriver(&driver); + + // align voltage + motor.voltage_sensor_align = 0.5; + + // control loop type and torque mode + motor.torque_controller = TorqueControlType::voltage; + motor.controller = MotionControlType::torque; + motor.motion_downsample = 0.0; + + // velocity loop PID + motor.PID_velocity.P = 0.2; + motor.PID_velocity.I = 5.0; + // Low pass filtering time constant + motor.LPF_velocity.Tf = 0.02; + // angle loop PID + motor.P_angle.P = 20.0; + // Low pass filtering time constant + motor.LPF_angle.Tf = 0.0; + // current q loop PID + motor.PID_current_q.P = 3.0; + motor.PID_current_q.I = 100.0; + // Low pass filtering time constant + motor.LPF_current_q.Tf = 0.02; + // current d loop PID + motor.PID_current_d.P = 3.0; + motor.PID_current_d.I = 100.0; + // Low pass filtering time constant + motor.LPF_current_d.Tf = 0.02; + + // Limits + motor.velocity_limit = 100.0; // 100 rad/s velocity limit + motor.voltage_limit = 12.0; // 12 Volt limit + motor.current_limit = 2.0; // 2 Amp current limit + + + // use monitoring with serial for motor init + // monitoring port + Serial.begin(115200); + // comment out if not needed + motor.useMonitoring(Serial); + motor.monitor_variables = _MON_CURR_Q | _MON_CURR_D; // monitor the two currents d and q + motor.monitor_downsample = 0; + + // initialise motor + motor.init(); + + cs.init(); + // driver 8302 has inverted gains on all channels + cs.gain_a *=-1; + cs.gain_b *=-1; + cs.gain_c *=-1; + motor.linkCurrentSense(&cs); + + // align encoder and start FOC + motor.initFOC(); + + // set the inital target value + motor.target = 0; + + // define the motor id + command.add('M', onMotor, "motor"); + + Serial.println(F("Full control example: ")); + Serial.println(F("Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) \n ")); + Serial.println(F("Initial motion control loop is voltage loop.")); + Serial.println(F("Initial target voltage 2V.")); + + _delay(1000); +} + + +void loop() { + // iterative setting FOC phase voltage + motor.loopFOC(); + + // iterative function setting the outter loop target + motor.move(); + + // monitoring the state variables + motor.monitor(); + + // user communication + command.run(); +} \ No newline at end of file diff --git a/examples/utils/calibration/find_kv_rating/encoder/find_kv_rating/find_kv_rating.ino b/examples/utils/calibration/find_kv_rating/encoder/find_kv_rating/find_kv_rating.ino index 76fb46b0..c304d195 100644 --- a/examples/utils/calibration/find_kv_rating/encoder/find_kv_rating/find_kv_rating.ino +++ b/examples/utils/calibration/find_kv_rating/encoder/find_kv_rating/find_kv_rating.ino @@ -36,7 +36,7 @@ Commander command = Commander(Serial); void doTarget(char* cmd) { command.scalar(&target_voltage, cmd); } void calcKV(char* cmd) { // calculate the KV - Serial.println(motor.shaft_velocity/motor.target*30.0f/_PI); + Serial.println(motor.shaft_velocity/motor.target/_SQRT3*30.0f/_PI); } diff --git a/examples/utils/calibration/find_kv_rating/hall_sensor/find_kv_rating/find_kv_rating.ino b/examples/utils/calibration/find_kv_rating/hall_sensor/find_kv_rating/find_kv_rating.ino index 3abef467..575968f4 100644 --- a/examples/utils/calibration/find_kv_rating/hall_sensor/find_kv_rating/find_kv_rating.ino +++ b/examples/utils/calibration/find_kv_rating/hall_sensor/find_kv_rating/find_kv_rating.ino @@ -33,7 +33,7 @@ Commander command = Commander(Serial); void doTarget(char* cmd) { command.scalar(&target_voltage, cmd); } void calcKV(char* cmd) { // calculate the KV - Serial.println(motor.shaft_velocity/motor.target*30.0f/_PI); + Serial.println(motor.shaft_velocity/motor.target/_SQRT3*30.0f/_PI); } diff --git a/examples/utils/calibration/find_kv_rating/magnetic_sensor/find_kv_rating/find_kv_rating.ino b/examples/utils/calibration/find_kv_rating/magnetic_sensor/find_kv_rating/find_kv_rating.ino index f3dd74a1..a9d29d86 100644 --- a/examples/utils/calibration/find_kv_rating/magnetic_sensor/find_kv_rating/find_kv_rating.ino +++ b/examples/utils/calibration/find_kv_rating/magnetic_sensor/find_kv_rating/find_kv_rating.ino @@ -31,7 +31,7 @@ Commander command = Commander(Serial); void doTarget(char* cmd) { command.scalar(&target_voltage, cmd); } void calcKV(char* cmd) { // calculate the KV - Serial.println(motor.shaft_velocity/motor.target*30.0f/_PI); + Serial.println(motor.shaft_velocity/motor.target/_SQRT3*30.0f/_PI); } diff --git a/library.json b/library.json new file mode 100644 index 00000000..25e2a353 --- /dev/null +++ b/library.json @@ -0,0 +1,5 @@ +{ + "build": { + "libArchive": false + } +} diff --git a/library.properties b/library.properties index 7242d57c..0a2606b2 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=Simple FOC -version=2.3.2 +version=2.3.3 author=Simplefoc maintainer=Simplefoc sentence=A library demistifying FOC for BLDC motors diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index 8dc2f5cd..fe14feff 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -46,7 +46,7 @@ BLDCMotor::BLDCMotor(int pp, float _R, float _KV, float _inductance) // 1/sqrt(2) - rms value KV_rating = NOT_SET; if (_isset(_KV)) - KV_rating = _KV*_SQRT2; + KV_rating = _KV; // save phase inductance phase_inductance = _inductance; @@ -111,6 +111,8 @@ void BLDCMotor::init() { // disable motor driver void BLDCMotor::disable() { + // disable the current sense + if(current_sense) current_sense->disable(); // set zero to PWM driver->setPwm(0, 0, 0); // disable the driver @@ -125,6 +127,13 @@ void BLDCMotor::enable() driver->enable(); // set zero to PWM driver->setPwm(0, 0, 0); + // enable the current sense + if(current_sense) current_sense->enable(); + // reset the pids + PID_velocity.reset(); + P_angle.reset(); + PID_current_q.reset(); + PID_current_d.reset(); // motor status update enabled = 1; } @@ -142,32 +151,36 @@ int BLDCMotor::initFOC() { // alignment necessary for encoders! // sensor and motor alignment - can be skipped // by setting motor.sensor_direction and motor.zero_electric_angle - _delay(500); if(sensor){ exit_flag *= alignSensor(); // added the shaft_angle update sensor->update(); shaft_angle = shaftAngle(); - }else { - exit_flag = 0; // no FOC without sensor - SIMPLEFOC_DEBUG("MOT: No sensor."); - } - // aligning the current sensor - can be skipped - // checks if driver phases are the same as current sense phases - // and checks the direction of measuremnt. - _delay(500); - if(exit_flag){ - if(current_sense){ - if (!current_sense->initialized) { - motor_status = FOCMotorStatus::motor_calib_failed; - SIMPLEFOC_DEBUG("MOT: Init FOC error, current sense not initialized"); - exit_flag = 0; - }else{ - exit_flag *= alignCurrentSense(); + // aligning the current sensor - can be skipped + // checks if driver phases are the same as current sense phases + // and checks the direction of measuremnt. + if(exit_flag){ + if(current_sense){ + if (!current_sense->initialized) { + motor_status = FOCMotorStatus::motor_calib_failed; + SIMPLEFOC_DEBUG("MOT: Init FOC error, current sense not initialized"); + exit_flag = 0; + }else{ + exit_flag *= alignCurrentSense(); + } } + else { SIMPLEFOC_DEBUG("MOT: No current sense."); } + } + + } else { + SIMPLEFOC_DEBUG("MOT: No sensor."); + if ((controller == MotionControlType::angle_openloop || controller == MotionControlType::velocity_openloop)){ + exit_flag = 1; + SIMPLEFOC_DEBUG("MOT: Openloop only!"); + }else{ + exit_flag = 0; // no FOC without sensor } - else { SIMPLEFOC_DEBUG("MOT: No current sense."); } } if(exit_flag){ @@ -212,6 +225,10 @@ int BLDCMotor::alignSensor() { // stop init if not found index if(!exit_flag) return exit_flag; + // v2.3.3 fix for R_AVR_7_PCREL against symbol" bug for AVR boards + // TODO figure out why this works + float voltage_align = voltage_sensor_align; + // if unknown natural direction if(sensor_direction==Direction::UNKNOWN){ @@ -219,7 +236,7 @@ int BLDCMotor::alignSensor() { // move one electrical revolution forward for (int i = 0; i <=500; i++ ) { float angle = _3PI_2 + _2PI * i / 500.0f; - setPhaseVoltage(voltage_sensor_align, 0, angle); + setPhaseVoltage(voltage_align, 0, angle); sensor->update(); _delay(2); } @@ -229,13 +246,13 @@ int BLDCMotor::alignSensor() { // move one electrical revolution backwards for (int i = 500; i >=0; i-- ) { float angle = _3PI_2 + _2PI * i / 500.0f ; - setPhaseVoltage(voltage_sensor_align, 0, angle); + setPhaseVoltage(voltage_align, 0, angle); sensor->update(); _delay(2); } sensor->update(); float end_angle = sensor->getAngle(); - setPhaseVoltage(0, 0, 0); + // setPhaseVoltage(0, 0, 0); _delay(200); // determine the direction the sensor moved float moved = fabs(mid_angle - end_angle); @@ -250,7 +267,8 @@ int BLDCMotor::alignSensor() { sensor_direction = Direction::CW; } // check pole pair number - if( fabs(moved*pole_pairs - _2PI) > 0.5f ) { // 0.5f is arbitrary number it can be lower or higher! + pp_check_result = !(fabs(moved*pole_pairs - _2PI) > 0.5f); // 0.5f is arbitrary number it can be lower or higher! + if( pp_check_result==false ) { SIMPLEFOC_DEBUG("MOT: PP check: fail - estimated pp: ", _2PI/moved); } else { SIMPLEFOC_DEBUG("MOT: PP check: OK!"); @@ -262,7 +280,7 @@ int BLDCMotor::alignSensor() { if(!_isset(zero_electric_angle)){ // align the electrical phases of the motor and sensor // set angle -90(270 = 3PI/2) degrees - setPhaseVoltage(voltage_sensor_align, 0, _3PI_2); + setPhaseVoltage(voltage_align, 0, _3PI_2); _delay(700); // read the sensor sensor->update(); @@ -396,7 +414,7 @@ void BLDCMotor::move(float new_target) { if(_isset(new_target)) target = new_target; // calculate the back-emf voltage if KV_rating available U_bemf = vel*(1/KV) - if (_isset(KV_rating)) voltage_bemf = shaft_velocity/KV_rating/_RPM_TO_RADS; + if (_isset(KV_rating)) voltage_bemf = shaft_velocity/(KV_rating*_SQRT3)/_RPM_TO_RADS; // estimate the motor current if phase reistance available and current_sense not available if(!current_sense && _isset(phase_resistance)) current.q = (voltage.q - voltage_bemf)/phase_resistance; diff --git a/src/BLDCMotor.h b/src/BLDCMotor.h index a1f196ab..a7155c1f 100644 --- a/src/BLDCMotor.h +++ b/src/BLDCMotor.h @@ -69,8 +69,7 @@ class BLDCMotor: public FOCMotor void move(float target = NOT_SET) override; float Ua, Ub, Uc;//!< Current phase voltages Ua,Ub and Uc set to motor - float Ualpha, Ubeta; //!< Phase voltages U alpha and U beta used for inverse Park and Clarke transform - + /** * Method using FOC to set Uq to the motor at the optimal angle * Heart of the FOC algorithm diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index 7831f0bf..5d519f29 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -16,7 +16,7 @@ StepperMotor::StepperMotor(int pp, float _R, float _KV, float _inductance) phase_resistance = _R; // save back emf constant KV = 1/K_bemf // usually used rms - KV_rating = _KV*_SQRT2; + KV_rating = _KV; // save phase inductance phase_inductance = _inductance; @@ -114,7 +114,15 @@ int StepperMotor::initFOC() { // added the shaft_angle update sensor->update(); shaft_angle = sensor->getAngle(); - } else { SIMPLEFOC_DEBUG("MOT: No sensor."); } + } else { + SIMPLEFOC_DEBUG("MOT: No sensor."); + if ((controller == MotionControlType::angle_openloop || controller == MotionControlType::velocity_openloop)){ + exit_flag = 1; + SIMPLEFOC_DEBUG("MOT: Openloop only!"); + }else{ + exit_flag = 0; // no FOC without sensor + } + } if(exit_flag){ SIMPLEFOC_DEBUG("MOT: Ready."); @@ -133,6 +141,10 @@ int StepperMotor::alignSensor() { int exit_flag = 1; //success SIMPLEFOC_DEBUG("MOT: Align sensor."); + // v2.3.3 fix for R_AVR_7_PCREL against symbol" bug for AVR boards + // TODO figure out why this works + float voltage_align = voltage_sensor_align; + // if unknown natural direction if(sensor_direction == Direction::UNKNOWN){ // check if sensor needs zero search @@ -144,7 +156,7 @@ int StepperMotor::alignSensor() { // move one electrical revolution forward for (int i = 0; i <=500; i++ ) { float angle = _3PI_2 + _2PI * i / 500.0f; - setPhaseVoltage(voltage_sensor_align, 0, angle); + setPhaseVoltage(voltage_align, 0, angle); sensor->update(); _delay(2); } @@ -154,7 +166,7 @@ int StepperMotor::alignSensor() { // move one electrical revolution backwards for (int i = 500; i >=0; i-- ) { float angle = _3PI_2 + _2PI * i / 500.0f ; - setPhaseVoltage(voltage_sensor_align, 0, angle); + setPhaseVoltage(voltage_align, 0, angle); sensor->update(); _delay(2); } @@ -175,7 +187,8 @@ int StepperMotor::alignSensor() { } // check pole pair number float moved = fabs(mid_angle - end_angle); - if( fabs(moved*pole_pairs - _2PI) > 0.5f ) { // 0.5f is arbitrary number it can be lower or higher! + pp_check_result = !(fabs(moved*pole_pairs - _2PI) > 0.5f); // 0.5f is arbitrary number it can be lower or higher! + if( pp_check_result==false ) { SIMPLEFOC_DEBUG("MOT: PP check: fail - estimated pp: ", _2PI/moved); } else { SIMPLEFOC_DEBUG("MOT: PP check: OK!"); @@ -189,7 +202,7 @@ int StepperMotor::alignSensor() { if(!_isset(zero_electric_angle)){ // align the electrical phases of the motor and sensor // set angle -90(270 = 3PI/2) degrees - setPhaseVoltage(voltage_sensor_align, 0, _3PI_2); + setPhaseVoltage(voltage_align, 0, _3PI_2); _delay(700); // read the sensor sensor->update(); @@ -292,7 +305,7 @@ void StepperMotor::move(float new_target) { if(_isset(new_target) ) target = new_target; // calculate the back-emf voltage if KV_rating available U_bemf = vel*(1/KV) - if (_isset(KV_rating)) voltage_bemf = shaft_velocity/KV_rating/_RPM_TO_RADS; + if (_isset(KV_rating)) voltage_bemf = shaft_velocity/(KV_rating*_SQRT3)/_RPM_TO_RADS; // estimate the motor current if phase reistance available and current_sense not available if(!current_sense && _isset(phase_resistance)) current.q = (voltage.q - voltage_bemf)/phase_resistance; diff --git a/src/StepperMotor.h b/src/StepperMotor.h index 7eda3167..45d20c63 100644 --- a/src/StepperMotor.h +++ b/src/StepperMotor.h @@ -73,8 +73,6 @@ class StepperMotor: public FOCMotor */ void move(float target = NOT_SET) override; - float Ualpha,Ubeta; //!< Phase voltages U alpha and U beta used for inverse Park and Clarke transform - /** * Method using FOC to set Uq to the motor at the optimal angle * Heart of the FOC algorithm diff --git a/src/common/base_classes/CurrentSense.cpp b/src/common/base_classes/CurrentSense.cpp index 217e8a67..03ea19ea 100644 --- a/src/common/base_classes/CurrentSense.cpp +++ b/src/common/base_classes/CurrentSense.cpp @@ -7,33 +7,12 @@ float CurrentSense::getDCCurrent(float motor_electrical_angle){ // read current phase currents PhaseCurrent_s current = getPhaseCurrents(); - // currnet sign - if motor angle not provided the magnitude is always positive - float sign = 1; - + // calculate clarke transform - float i_alpha, i_beta; - if(!current.c){ - // if only two measured currents - i_alpha = current.a; - i_beta = _1_SQRT3 * current.a + _2_SQRT3 * current.b; - }else if(!current.a){ - // if only two measured currents - float a = -current.c - current.b; - i_alpha = a; - i_beta = _1_SQRT3 * a + _2_SQRT3 * current.b; - }else if(!current.b){ - // if only two measured currents - float b = -current.a - current.c; - i_alpha = current.a; - i_beta = _1_SQRT3 * current.a + _2_SQRT3 * b; - }else{ - // signal filtering using identity a + b + c = 0. Assumes measurement error is normally distributed. - float mid = (1.f/3) * (current.a + current.b + current.c); - float a = current.a - mid; - float b = current.b - mid; - i_alpha = a; - i_beta = _1_SQRT3 * a + _2_SQRT3 * b; - } + ABCurrent_s ABcurrent = getABCurrents(current); + + // current sign - if motor angle not provided the magnitude is always positive + float sign = 1; // if motor angle provided function returns signed value of the current // determine the sign of the current @@ -42,20 +21,34 @@ float CurrentSense::getDCCurrent(float motor_electrical_angle){ float ct; float st; _sincos(motor_electrical_angle, &st, &ct); - sign = (i_beta*ct - i_alpha*st) > 0 ? 1 : -1; + sign = (ABcurrent.beta*ct - ABcurrent.alpha*st) > 0 ? 1 : -1; } // return current magnitude - return sign*_sqrt(i_alpha*i_alpha + i_beta*i_beta); + return sign*_sqrt(ABcurrent.alpha*ABcurrent.alpha + ABcurrent.beta*ABcurrent.beta); } // function used with the foc algorihtm // calculating DQ currents from phase currents // - function calculating park and clarke transform of the phase currents -// - using getPhaseCurrents internally +// - using getPhaseCurrents and getABCurrents internally DQCurrent_s CurrentSense::getFOCCurrents(float angle_el){ // read current phase currents PhaseCurrent_s current = getPhaseCurrents(); + // calculate clarke transform + ABCurrent_s ABcurrent = getABCurrents(current); + + // calculate park transform + DQCurrent_s return_current = getDQCurrents(ABcurrent,angle_el); + + return return_current; +} + +// function used with the foc algorihtm +// calculating Alpha Beta currents from phase currents +// - function calculating Clarke transform of the phase currents +ABCurrent_s CurrentSense::getABCurrents(PhaseCurrent_s current){ + // calculate clarke transform float i_alpha, i_beta; if(!current.c){ @@ -81,13 +74,23 @@ DQCurrent_s CurrentSense::getFOCCurrents(float angle_el){ i_beta = _1_SQRT3 * a + _2_SQRT3 * b; } - // calculate park transform + ABCurrent_s return_ABcurrent; + return_ABcurrent.alpha = i_alpha; + return_ABcurrent.beta = i_beta; + return return_ABcurrent; +} + +// function used with the foc algorihtm +// calculating D and Q currents from Alpha Beta currents and electrical angle +// - function calculating Clarke transform of the phase currents +DQCurrent_s CurrentSense::getDQCurrents(ABCurrent_s current, float angle_el){ + // calculate park transform float ct; float st; _sincos(angle_el, &st, &ct); DQCurrent_s return_current; - return_current.d = i_alpha * ct + i_beta * st; - return_current.q = i_beta * ct - i_alpha * st; + return_current.d = current.alpha * ct + current.beta * st; + return_current.q = current.beta * ct - current.alpha * st; return return_current; } @@ -96,4 +99,13 @@ DQCurrent_s CurrentSense::getFOCCurrents(float angle_el){ */ void CurrentSense::linkDriver(BLDCDriver* _driver) { driver = _driver; -} \ No newline at end of file +} + + +void CurrentSense::enable(){ + // nothing is done here, but you can override this function +}; + +void CurrentSense::disable(){ + // nothing is done here, but you can override this function +}; \ No newline at end of file diff --git a/src/common/base_classes/CurrentSense.h b/src/common/base_classes/CurrentSense.h index ad9f926d..1c839053 100644 --- a/src/common/base_classes/CurrentSense.h +++ b/src/common/base_classes/CurrentSense.h @@ -53,7 +53,7 @@ class CurrentSense{ virtual PhaseCurrent_s getPhaseCurrents() = 0; /** * Function reading the magnitude of the current set to the motor - * It returns the abosolute or signed magnitude if possible + * It returns the absolute or signed magnitude if possible * It can receive the motor electrical angle to help with calculation * This function is used with the current control (not foc) * @@ -62,14 +62,42 @@ class CurrentSense{ virtual float getDCCurrent(float angle_el = 0); /** - * Function used for FOC contorl, it reads the DQ currents of the motor + * Function used for FOC control, it reads the DQ currents of the motor * It uses the function getPhaseCurrents internally * * @param angle_el - motor electrical angle */ DQCurrent_s getFOCCurrents(float angle_el); + /** + * Function used for Clarke transform in FOC control + * It reads the phase currents of the motor + * It returns the alpha and beta currents + * + * @param current - phase current + */ + ABCurrent_s getABCurrents(PhaseCurrent_s current); + + /** + * Function used for Park transform in FOC control + * It reads the Alpha Beta currents and electircal angle of the motor + * It returns the D and Q currents + * + * @param current - phase current + */ + DQCurrent_s getDQCurrents(ABCurrent_s current,float angle_el); + + /** + * enable the current sense. default implementation does nothing, but you can + * override it to do something useful. + */ + virtual void enable(); + /** + * disable the current sense. default implementation does nothing, but you can + * override it to do something useful. + */ + virtual void disable(); }; #endif \ No newline at end of file diff --git a/src/common/base_classes/FOCMotor.cpp b/src/common/base_classes/FOCMotor.cpp index d1427bcf..5d8f8127 100644 --- a/src/common/base_classes/FOCMotor.cpp +++ b/src/common/base_classes/FOCMotor.cpp @@ -32,6 +32,10 @@ FOCMotor::FOCMotor() // voltage bemf voltage_bemf = 0; + + // Initialize phase voltages U alpha and U beta used for inverse Park and Clarke transform + Ualpha = 0; + Ubeta = 0; //monitor_port monitor_port = nullptr; @@ -91,7 +95,7 @@ void FOCMotor::useMonitoring(Print &print){ // utility function intended to be used with serial plotter to monitor motor variables // significantly slowing the execution down!!!! void FOCMotor::monitor() { - if( !monitor_downsample || monitor_cnt++ < monitor_downsample ) return; + if( !monitor_downsample || monitor_cnt++ < (monitor_downsample-1) ) return; monitor_cnt = 0; if(!monitor_port) return; bool printed = 0; diff --git a/src/common/base_classes/FOCMotor.h b/src/common/base_classes/FOCMotor.h index 318be99a..b5ba2e96 100644 --- a/src/common/base_classes/FOCMotor.h +++ b/src/common/base_classes/FOCMotor.h @@ -161,6 +161,8 @@ class FOCMotor DQVoltage_s voltage;//!< current d and q voltage set to the motor DQCurrent_s current;//!< current d and q current measured float voltage_bemf; //!< estimated backemf voltage (if provided KV constant) + float Ualpha, Ubeta; //!< Phase voltages U alpha and U beta used for inverse Park and Clarke transform + // motor configuration parameters float voltage_sensor_align;//!< sensor and motor align voltage parameter @@ -206,6 +208,7 @@ class FOCMotor float sensor_offset; //!< user defined sensor zero offset float zero_electric_angle = NOT_SET;//!< absolute zero electric angle - if available Direction sensor_direction = Direction::UNKNOWN; //!< default is CW. if sensor_direction == Direction::CCW then direction will be flipped compared to CW. Set to UNKNOWN to set by calibration + bool pp_check_result = false; //!< the result of the PP check, if run during loopFOC /** * Function providing BLDCMotor class with the diff --git a/src/common/base_classes/Sensor.cpp b/src/common/base_classes/Sensor.cpp index b0a8f4db..db17e92e 100644 --- a/src/common/base_classes/Sensor.cpp +++ b/src/common/base_classes/Sensor.cpp @@ -19,7 +19,7 @@ void Sensor::update() { /** get current angular velocity (rad/s) */ float Sensor::getVelocity() { // calculate sample time - float Ts = (angle_prev_ts - vel_angle_prev_ts)*1e-6; + float Ts = (angle_prev_ts - vel_angle_prev_ts)*1e-6f; if (Ts < 0.0f) { // handle micros() overflow - we need to reset vel_angle_prev_ts vel_angle_prev = angle_prev; vel_full_rotations = full_rotations; diff --git a/src/common/defaults.h b/src/common/defaults.h index c0a46182..ac57daa1 100644 --- a/src/common/defaults.h +++ b/src/common/defaults.h @@ -43,7 +43,7 @@ // align voltage #define DEF_VOLTAGE_SENSOR_ALIGN 3.0f //!< default voltage for sensor and motor zero alignemt // low pass filter velocity -#define DEF_VEL_FILTER_Tf 0.005 //!< default velocity filter time constant +#define DEF_VEL_FILTER_Tf 0.005f //!< default velocity filter time constant // current sense default parameters #define DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf 0.0f //!< default currnet sense per phase low pass filter time constant diff --git a/src/common/foc_utils.cpp b/src/common/foc_utils.cpp index 233bd246..c938d363 100644 --- a/src/common/foc_utils.cpp +++ b/src/common/foc_utils.cpp @@ -9,7 +9,7 @@ __attribute__((weak)) float _sin(float a){ // 16 bit precision on sine value, 8 bit fractional value for interpolation, 6bit LUT size // resulting precision compared to stdlib sine is 0.00006480 (RMS difference in range -PI,PI for 3217 steps) static uint16_t sine_array[65] = {0,804,1608,2411,3212,4011,4808,5602,6393,7180,7962,8740,9512,10279,11039,11793,12540,13279,14010,14733,15447,16151,16846,17531,18205,18868,19520,20160,20788,21403,22006,22595,23170,23732,24279,24812,25330,25833,26320,26791,27246,27684,28106,28511,28899,29269,29622,29957,30274,30572,30853,31114,31357,31581,31786,31972,32138,32286,32413,32522,32610,32679,32729,32758,32768}; - unsigned int i = (unsigned int)(a * (64*4*256.0 /_2PI)); + unsigned int i = (unsigned int)(a * (64*4*256.0f/_2PI)); int t1, t2, frac = i & 0xff; i = (i >> 8) & 0xff; if (i < 64) { diff --git a/src/common/foc_utils.h b/src/common/foc_utils.h index abdeebf8..f2bc9ef6 100644 --- a/src/common/foc_utils.h +++ b/src/common/foc_utils.h @@ -56,6 +56,12 @@ struct DQVoltage_s float d; float q; }; +// alpha beta current structure +struct ABCurrent_s +{ + float alpha; + float beta; +}; /** diff --git a/src/communication/SimpleFOCDebug.cpp b/src/communication/SimpleFOCDebug.cpp index e969d8a2..3bb62bce 100644 --- a/src/communication/SimpleFOCDebug.cpp +++ b/src/communication/SimpleFOCDebug.cpp @@ -58,6 +58,12 @@ void SimpleFOCDebug::println(const char* str, int val) { _debugPrint->println(val); } } +void SimpleFOCDebug::println(const char* str, char val) { + if (_debugPrint != NULL) { + _debugPrint->print(str); + _debugPrint->println(val); + } +} void SimpleFOCDebug::println(const __FlashStringHelper* str, int val) { if (_debugPrint != NULL) { diff --git a/src/communication/SimpleFOCDebug.h b/src/communication/SimpleFOCDebug.h index 614e6371..4fcfd538 100644 --- a/src/communication/SimpleFOCDebug.h +++ b/src/communication/SimpleFOCDebug.h @@ -45,6 +45,7 @@ class SimpleFOCDebug { static void println(const char* msg, float val); static void println(const __FlashStringHelper* msg, int val); static void println(const char* msg, int val); + static void println(const char* msg, char val); static void println(); static void println(int val); static void println(float val); diff --git a/src/current_sense/InlineCurrentSense.cpp b/src/current_sense/InlineCurrentSense.cpp index 492b3ac9..c3db74ef 100644 --- a/src/current_sense/InlineCurrentSense.cpp +++ b/src/current_sense/InlineCurrentSense.cpp @@ -156,9 +156,9 @@ int InlineCurrentSense::driverAlign(float voltage){ // read the current 50 times for (int i = 0; i < 100; i++) { PhaseCurrent_s c1 = getPhaseCurrents(); - c.a = c.a*0.6 + 0.4f*c1.a; - c.b = c.b*0.6 + 0.4f*c1.b; - c.c = c.c*0.6 + 0.4f*c1.c; + c.a = c.a*0.6f + 0.4f*c1.a; + c.b = c.b*0.6f + 0.4f*c1.b; + c.c = c.c*0.6f + 0.4f*c1.c; _delay(3); } driver->setPwm(0, 0, 0); @@ -203,9 +203,9 @@ int InlineCurrentSense::driverAlign(float voltage){ // read the adc voltage 500 times ( arbitrary number ) for (int i = 0; i < 100; i++) { PhaseCurrent_s c1 = getPhaseCurrents(); - c.a = c.a*0.6 + 0.4f*c1.a; - c.b = c.b*0.6 + 0.4f*c1.b; - c.c = c.c*0.6 + 0.4f*c1.c; + c.a = c.a*0.6f + 0.4f*c1.a; + c.b = c.b*0.6f + 0.4f*c1.b; + c.c = c.c*0.6f + 0.4f*c1.c; _delay(3); } driver->setPwm(0, 0, 0); diff --git a/src/current_sense/LowsideCurrentSense.cpp b/src/current_sense/LowsideCurrentSense.cpp index a706beeb..aeb8dea0 100644 --- a/src/current_sense/LowsideCurrentSense.cpp +++ b/src/current_sense/LowsideCurrentSense.cpp @@ -157,9 +157,9 @@ int LowsideCurrentSense::driverAlign(float voltage){ // read the current 50 times for (int i = 0; i < 100; i++) { PhaseCurrent_s c1 = getPhaseCurrents(); - c.a = c.a*0.6 + 0.4f*c1.a; - c.b = c.b*0.6 + 0.4f*c1.b; - c.c = c.c*0.6 + 0.4f*c1.c; + c.a = c.a*0.6f + 0.4f*c1.a; + c.b = c.b*0.6f + 0.4f*c1.b; + c.c = c.c*0.6f + 0.4f*c1.c; _delay(3); } driver->setPwm(0, 0, 0); @@ -204,9 +204,9 @@ int LowsideCurrentSense::driverAlign(float voltage){ // read the adc voltage 500 times ( arbitrary number ) for (int i = 0; i < 100; i++) { PhaseCurrent_s c1 = getPhaseCurrents(); - c.a = c.a*0.6 + 0.4f*c1.a; - c.b = c.b*0.6 + 0.4f*c1.b; - c.c = c.c*0.6 + 0.4f*c1.c; + c.a = c.a*0.6f + 0.4f*c1.a; + c.b = c.b*0.6f + 0.4f*c1.b; + c.c = c.c*0.6f + 0.4f*c1.c; _delay(3); } driver->setPwm(0, 0, 0); diff --git a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp index 27e11958..d3bea81e 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp @@ -67,7 +67,7 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive hadc.Init.DiscontinuousConvMode = DISABLE; hadc.Init.ExternalTrigConv = ADC_SOFTWARE_START; hadc.Init.DataAlign = ADC_DATAALIGN_RIGHT; - hadc.Init.NbrOfConversion = 0; + hadc.Init.NbrOfConversion = 1; HAL_ADC_Init(&hadc); /**Configure for the selected ADC regular channel to be converted. */ @@ -124,10 +124,6 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected); } - // enable interrupt - HAL_NVIC_SetPriority(ADC1_2_IRQn, 0, 0); - HAL_NVIC_EnableIRQ(ADC1_2_IRQn); - cs_params->adc_handle = &hadc; return 0; @@ -156,7 +152,6 @@ extern "C" { { HAL_ADC_IRQHandler(&hadc); } - } #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp index 51b494f4..5f090c20 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp @@ -19,6 +19,12 @@ bool needs_downsample[3] = {1}; // downsampling variable - per adc (3) uint8_t tim_downsample[3] = {0}; +#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT +uint8_t use_adc_interrupt = 1; +#else +uint8_t use_adc_interrupt = 0; +#endif + int _adcToIndex(ADC_HandleTypeDef *AdcHandle){ if(AdcHandle->Instance == ADC1) return 0; #ifdef ADC2 // if ADC2 exists @@ -63,6 +69,14 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ cs_params->timer_handle->getHandle()->Instance->CNT = cs_params->timer_handle->getHandle()->Instance->ARR; // remember that this timer has repetition counter - no need to downasmple needs_downsample[_adcToIndex(cs_params->adc_handle)] = 0; + }else{ + if(!use_adc_interrupt){ + // If the timer has no repetition counter, it needs to use the interrupt to downsample for low side sensing + use_adc_interrupt = 1; + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: timer has no repetition counter, ADC interrupt has to be used"); + #endif + } } // set the trigger output event LL_TIM_SetTriggerOutput(cs_params->timer_handle->getHandle()->Instance, LL_TIM_TRGO_UPDATE); @@ -71,7 +85,15 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ HAL_ADCEx_Calibration_Start(cs_params->adc_handle); // start the adc - HAL_ADCEx_InjectedStart_IT(cs_params->adc_handle); + if(use_adc_interrupt){ + HAL_NVIC_SetPriority(ADC1_2_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC1_2_IRQn); + + HAL_ADCEx_InjectedStart_IT(cs_params->adc_handle); + }else{ + HAL_ADCEx_InjectedStart(cs_params->adc_handle); + } + // restart all the timers of the driver _startTimers(driver_params->timers, 6); @@ -81,13 +103,19 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ // function reading an ADC value and returning the read voltage float _readADCVoltageLowSide(const int pin, const void* cs_params){ for(int i=0; i < 3; i++){ - if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]) // found in the buffer - return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]){ // found in the buffer + if (use_adc_interrupt){ + return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + }else{ + // an optimized way to go from i to the channel i=0 -> channel 1, i=1 -> channel 2, i=2 -> channel 3 + uint32_t channel = (i == 0) ? ADC_INJECTED_RANK_1 : (i == 1) ? ADC_INJECTED_RANK_2 : ADC_INJECTED_RANK_3;; + return HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle, channel) * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + } + } } return 0; } - extern "C" { void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef *AdcHandle){ // calculate the instance diff --git a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp index 68a9d094..bd0df4b6 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp @@ -135,10 +135,6 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive } } - // enable interrupt - HAL_NVIC_SetPriority(ADC_IRQn, 0, 0); - HAL_NVIC_EnableIRQ(ADC_IRQn); - cs_params->adc_handle = &hadc; return 0; } diff --git a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp index 60344781..6388e8e0 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp @@ -22,6 +22,12 @@ bool needs_downsample[3] = {1}; // downsampling variable - per adc (3) uint8_t tim_downsample[3] = {0}; +#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT +uint8_t use_adc_interrupt = 1; +#else +uint8_t use_adc_interrupt = 0; +#endif + void* _configureADCLowSide(const void* driver_params, const int pinA, const int pinB, const int pinC){ Stm32CurrentSenseParams* cs_params= new Stm32CurrentSenseParams { @@ -55,11 +61,28 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ cs_params->timer_handle->getHandle()->Instance->CNT = cs_params->timer_handle->getHandle()->Instance->ARR; // remember that this timer has repetition counter - no need to downasmple needs_downsample[_adcToIndex(cs_params->adc_handle)] = 0; + }else{ + if(!use_adc_interrupt){ + // If the timer has no repetition counter, it needs to use the interrupt to downsample for low side sensing + use_adc_interrupt = 1; + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: timer has no repetition counter, ADC interrupt has to be used"); + #endif + } } // set the trigger output event LL_TIM_SetTriggerOutput(cs_params->timer_handle->getHandle()->Instance, LL_TIM_TRGO_UPDATE); - // start the adc - HAL_ADCEx_InjectedStart_IT(cs_params->adc_handle); + + // start the adc + if (use_adc_interrupt){ + // enable interrupt + HAL_NVIC_SetPriority(ADC_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC_IRQn); + + HAL_ADCEx_InjectedStart_IT(cs_params->adc_handle); + }else{ + HAL_ADCEx_InjectedStart(cs_params->adc_handle); + } // restart all the timers of the driver _startTimers(driver_params->timers, 6); @@ -69,13 +92,19 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ // function reading an ADC value and returning the read voltage float _readADCVoltageLowSide(const int pin, const void* cs_params){ for(int i=0; i < 3; i++){ - if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]) // found in the buffer - return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]){ // found in the buffer + if (use_adc_interrupt){ + return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + }else{ + // an optimized way to go from i to the channel i=0 -> channel 1, i=1 -> channel 2, i=2 -> channel 3 + uint32_t channel = (i == 0) ? ADC_INJECTED_RANK_1 : (i == 1) ? ADC_INJECTED_RANK_2 : ADC_INJECTED_RANK_3; + return HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle, channel) * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + } + } } return 0; } - extern "C" { void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef *AdcHandle){ // calculate the instance diff --git a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.cpp b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.cpp new file mode 100644 index 00000000..d4cffec6 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.cpp @@ -0,0 +1,185 @@ +#include "stm32f7_hal.h" + +#if defined(STM32F7xx) + +//#define SIMPLEFOC_STM32_DEBUG + +#include "../../../../communication/SimpleFOCDebug.h" +#define _TRGO_NOT_AVAILABLE 12345 + +ADC_HandleTypeDef hadc; + +int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params) +{ + ADC_InjectionConfTypeDef sConfigInjected; + + // check if all pins belong to the same ADC + ADC_TypeDef* adc_pin1 = (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[0]), PinMap_ADC); + ADC_TypeDef* adc_pin2 = (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[1]), PinMap_ADC); + ADC_TypeDef* adc_pin3 = _isset(cs_params->pins[2]) ? (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[2]), PinMap_ADC) : nullptr; + if ( (adc_pin1 != adc_pin2) || ( (adc_pin3) && (adc_pin1 != adc_pin3) )){ +#ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: ERR: Analog pins dont belong to the same ADC!"); +#endif + return -1; + } + + + /**Configure the global features of the ADC (Clock, Resolution, Data Alignment and number of conversion) + */ + hadc.Instance = (ADC_TypeDef *)pinmap_peripheral(analogInputToPinName(cs_params->pins[0]), PinMap_ADC); + + if(hadc.Instance == ADC1) __HAL_RCC_ADC1_CLK_ENABLE(); +#ifdef ADC2 // if defined ADC2 + else if(hadc.Instance == ADC2) __HAL_RCC_ADC2_CLK_ENABLE(); +#endif +#ifdef ADC3 // if defined ADC3 + else if(hadc.Instance == ADC3) __HAL_RCC_ADC3_CLK_ENABLE(); +#endif + else{ +#ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: ERR: Pin does not belong to any ADC!"); +#endif + return -1; // error not a valid ADC instance + } + +#ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: Using ADC: ", _adcToIndex(&hadc)+1); +#endif + + hadc.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV2; + hadc.Init.Resolution = ADC_RESOLUTION_12B; + hadc.Init.ScanConvMode = ENABLE; + hadc.Init.ContinuousConvMode = DISABLE; + hadc.Init.DiscontinuousConvMode = DISABLE; + hadc.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE; + hadc.Init.ExternalTrigConv = ADC_SOFTWARE_START; // for now + hadc.Init.DataAlign = ADC_DATAALIGN_RIGHT; + hadc.Init.NbrOfConversion = 1; + hadc.Init.DMAContinuousRequests = DISABLE; + hadc.Init.EOCSelection = ADC_EOC_SINGLE_CONV; + if ( HAL_ADC_Init(&hadc) != HAL_OK){ +#ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init ADC!"); +#endif + return -1; + } + + /**Configures for the selected ADC injected channel its corresponding rank in the sequencer and its sample time + */ + sConfigInjected.InjectedNbrOfConversion = _isset(cs_params->pins[2]) ? 3 : 2; + sConfigInjected.InjectedSamplingTime = ADC_SAMPLETIME_3CYCLES; + sConfigInjected.ExternalTrigInjecConvEdge = ADC_EXTERNALTRIGINJECCONVEDGE_RISINGFALLING; + sConfigInjected.AutoInjectedConv = DISABLE; + sConfigInjected.InjectedDiscontinuousConvMode = DISABLE; + sConfigInjected.InjectedOffset = 0; + + // automating TRGO flag finding - hardware specific + uint8_t tim_num = 0; + for (size_t i=0; i<6; i++) { + HardwareTimer *timer_to_check = driver_params->timers[tim_num++]; + TIM_TypeDef *instance_to_check = timer_to_check->getHandle()->Instance; + + // bool TRGO_already_configured = instance_to_check->CR2 & LL_TIM_TRGO_UPDATE; + // if(TRGO_already_configured) continue; + + uint32_t trigger_flag = _timerToInjectedTRGO(timer_to_check); + if(trigger_flag == _TRGO_NOT_AVAILABLE) continue; // timer does not have valid trgo for injected channels + + // if the code comes here, it has found the timer available + // timer does have trgo flag for injected channels + sConfigInjected.ExternalTrigInjecConv = trigger_flag; + + // this will be the timer with which the ADC will sync + cs_params->timer_handle = timer_to_check; + if (!IS_TIM_REPETITION_COUNTER_INSTANCE(instance_to_check)) { + // workaround for errata 2.2.1 in ES0290 Rev 7 + // https://www.st.com/resource/en/errata_sheet/es0290-stm32f74xxx-and-stm32f75xxx-device-limitations-stmicroelectronics.pdf + __HAL_RCC_DAC_CLK_ENABLE(); + } + // done + break; + } + if( cs_params->timer_handle == NP ){ + // not possible to use these timers for low-side current sense + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot sync any timer to injected channels!"); + #endif + return -1; + } + // display which timer is being used + #ifdef SIMPLEFOC_STM32_DEBUG + // it would be better to use the getTimerNumber from driver + SIMPLEFOC_DEBUG("STM32-CS: injected trigger for timer index: ", get_timer_index(cs_params->timer_handle->getHandle()->Instance) + 1); + #endif + + + // first channel + sConfigInjected.InjectedRank = ADC_INJECTED_RANK_1; + sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[0])); + if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ +#ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[0])) ); +#endif + return -1; + } + + // second channel + sConfigInjected.InjectedRank = ADC_INJECTED_RANK_2; + sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[1])); + if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ +#ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[1]))) ; +#endif + return -1; + } + + // third channel - if exists + if(_isset(cs_params->pins[2])){ + sConfigInjected.InjectedRank = ADC_INJECTED_RANK_3; + sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[2])); + if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ +#ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[2]))) ; +#endif + return -1; + } + } + + #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT + // enable interrupt + HAL_NVIC_SetPriority(ADC_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC_IRQn); + #endif + + cs_params->adc_handle = &hadc; + return 0; +} + +void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC) +{ + uint8_t cnt = 0; + if(_isset(pinA)){ + pinmap_pinout(analogInputToPinName(pinA), PinMap_ADC); + cs_params->pins[cnt++] = pinA; + } + if(_isset(pinB)){ + pinmap_pinout(analogInputToPinName(pinB), PinMap_ADC); + cs_params->pins[cnt++] = pinB; + } + if(_isset(pinC)){ + pinmap_pinout(analogInputToPinName(pinC), PinMap_ADC); + cs_params->pins[cnt] = pinC; + } +} + +#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT +extern "C" { + void ADC_IRQHandler(void) + { + HAL_ADC_IRQHandler(&hadc); + } +} +#endif + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.h b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.h new file mode 100644 index 00000000..0a3614b5 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.h @@ -0,0 +1,15 @@ +#pragma once + +#include "Arduino.h" + +#if defined(STM32F7xx) +#include "stm32f7xx_hal.h" +#include "../../../../common/foc_utils.h" +#include "../../../../drivers/hardware_specific/stm32/stm32_mcu.h" +#include "../stm32_mcu.h" +#include "stm32f7_utils.h" + +int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params); +void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC); + +#endif diff --git a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_mcu.cpp new file mode 100644 index 00000000..3040ea46 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_mcu.cpp @@ -0,0 +1,111 @@ +#include "../../../hardware_api.h" + +#if defined(STM32F7xx) +#include "../../../../common/foc_utils.h" +#include "../../../../drivers/hardware_api.h" +#include "../../../../drivers/hardware_specific/stm32/stm32_mcu.h" +#include "../../../hardware_api.h" +#include "../stm32_mcu.h" +#include "stm32f7_hal.h" +#include "stm32f7_utils.h" +#include "Arduino.h" + + +#define _ADC_VOLTAGE 3.3f +#define _ADC_RESOLUTION 4096.0f + + +// array of values of 4 injected channels per adc instance (3) +uint32_t adc_val[3][4]={0}; +// does adc interrupt need a downsample - per adc (3) +bool needs_downsample[3] = {1}; +// downsampling variable - per adc (3) +uint8_t tim_downsample[3] = {1}; + +void* _configureADCLowSide(const void* driver_params, const int pinA, const int pinB, const int pinC){ + + Stm32CurrentSenseParams* cs_params= new Stm32CurrentSenseParams { + .pins={(int)NOT_SET,(int)NOT_SET,(int)NOT_SET}, + .adc_voltage_conv = (_ADC_VOLTAGE) / (_ADC_RESOLUTION) + }; + _adc_gpio_init(cs_params, pinA,pinB,pinC); + if(_adc_init(cs_params, (STM32DriverParams*)driver_params) != 0) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; + return cs_params; +} + + +void _driverSyncLowSide(void* _driver_params, void* _cs_params){ + STM32DriverParams* driver_params = (STM32DriverParams*)_driver_params; + Stm32CurrentSenseParams* cs_params = (Stm32CurrentSenseParams*)_cs_params; + + // if compatible timer has not been found + if (cs_params->timer_handle == NULL) return; + + // stop all the timers for the driver + _stopTimers(driver_params->timers, 6); + + // if timer has repetition counter - it will downsample using it + // and it does not need the software downsample + if( IS_TIM_REPETITION_COUNTER_INSTANCE(cs_params->timer_handle->getHandle()->Instance) ){ + // adjust the initial timer state such that the trigger + // - for DMA transfer aligns with the pwm peaks instead of throughs. + // - for interrupt based ADC transfer + // - only necessary for the timers that have repetition counters + + cs_params->timer_handle->getHandle()->Instance->CR1 |= TIM_CR1_DIR; + cs_params->timer_handle->getHandle()->Instance->CNT = cs_params->timer_handle->getHandle()->Instance->ARR; + // remember that this timer has repetition counter - no need to downasmple + needs_downsample[_adcToIndex(cs_params->adc_handle)] = 0; + } + // set the trigger output event + LL_TIM_SetTriggerOutput(cs_params->timer_handle->getHandle()->Instance, LL_TIM_TRGO_UPDATE); + + // start the adc + #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT + HAL_ADCEx_InjectedStart_IT(cs_params->adc_handle); + #else + HAL_ADCEx_InjectedStart(cs_params->adc_handle); + #endif + + // restart all the timers of the driver + _startTimers(driver_params->timers, 6); +} + + +// function reading an ADC value and returning the read voltage +float _readADCVoltageLowSide(const int pin, const void* cs_params){ + for(int i=0; i < 3; i++){ + if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]){ // found in the buffer + #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT + return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + #else + // an optimized way to go from i to the channel i=0 -> channel 1, i=1 -> channel 2, i=2 -> channel 3 + uint32_t channel = (i == 0) ? ADC_INJECTED_RANK_1 : (i == 1) ? ADC_INJECTED_RANK_2 : ADC_INJECTED_RANK_3; + return HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle, channel) * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + #endif + } + } + return 0; +} + +#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT +extern "C" { + void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef *AdcHandle){ + + // calculate the instance + int adc_index = _adcToIndex(AdcHandle); + + // if the timer han't repetition counter - downsample two times + if( needs_downsample[adc_index] && tim_downsample[adc_index]++ > 0) { + tim_downsample[adc_index] = 0; + return; + } + + adc_val[adc_index][0]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_1); + adc_val[adc_index][1]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_2); + adc_val[adc_index][2]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_3); + } +} +#endif + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_utils.cpp b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_utils.cpp new file mode 100644 index 00000000..d5f8c6b2 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_utils.cpp @@ -0,0 +1,255 @@ +#include "stm32f7_utils.h" + +#if defined(STM32F7xx) + +/* Exported Functions */ + + +PinName analog_to_pin(uint32_t pin) { + PinName pin_name = analogInputToPinName(pin); + if (pin_name == NC) { + return (PinName) pin; + } + return pin_name; +} + + +/** + * @brief Return ADC HAL channel linked to a PinName + * @param pin: PinName + * @retval Valid HAL channel + */ +uint32_t _getADCChannel(PinName pin) +{ + uint32_t function = pinmap_function(pin, PinMap_ADC); + uint32_t channel = 0; + switch (STM_PIN_CHANNEL(function)) { +#ifdef ADC_CHANNEL_0 + case 0: + channel = ADC_CHANNEL_0; + break; +#endif + case 1: + channel = ADC_CHANNEL_1; + break; + case 2: + channel = ADC_CHANNEL_2; + break; + case 3: + channel = ADC_CHANNEL_3; + break; + case 4: + channel = ADC_CHANNEL_4; + break; + case 5: + channel = ADC_CHANNEL_5; + break; + case 6: + channel = ADC_CHANNEL_6; + break; + case 7: + channel = ADC_CHANNEL_7; + break; + case 8: + channel = ADC_CHANNEL_8; + break; + case 9: + channel = ADC_CHANNEL_9; + break; + case 10: + channel = ADC_CHANNEL_10; + break; + case 11: + channel = ADC_CHANNEL_11; + break; + case 12: + channel = ADC_CHANNEL_12; + break; + case 13: + channel = ADC_CHANNEL_13; + break; + case 14: + channel = ADC_CHANNEL_14; + break; + case 15: + channel = ADC_CHANNEL_15; + break; +#ifdef ADC_CHANNEL_16 + case 16: + channel = ADC_CHANNEL_16; + break; +#endif + case 17: + channel = ADC_CHANNEL_17; + break; +#ifdef ADC_CHANNEL_18 + case 18: + channel = ADC_CHANNEL_18; + break; +#endif +#ifdef ADC_CHANNEL_19 + case 19: + channel = ADC_CHANNEL_19; + break; +#endif +#ifdef ADC_CHANNEL_20 + case 20: + channel = ADC_CHANNEL_20; + break; + case 21: + channel = ADC_CHANNEL_21; + break; + case 22: + channel = ADC_CHANNEL_22; + break; + case 23: + channel = ADC_CHANNEL_23; + break; +#ifdef ADC_CHANNEL_24 + case 24: + channel = ADC_CHANNEL_24; + break; + case 25: + channel = ADC_CHANNEL_25; + break; + case 26: + channel = ADC_CHANNEL_26; + break; +#ifdef ADC_CHANNEL_27 + case 27: + channel = ADC_CHANNEL_27; + break; + case 28: + channel = ADC_CHANNEL_28; + break; + case 29: + channel = ADC_CHANNEL_29; + break; + case 30: + channel = ADC_CHANNEL_30; + break; + case 31: + channel = ADC_CHANNEL_31; + break; +#endif +#endif +#endif + default: + _Error_Handler("ADC: Unknown adc channel", (int)(STM_PIN_CHANNEL(function))); + break; + } + return channel; +} +/* +TIM1 +TIM2 +TIM3 +TIM4 +TIM5 +TIM6 +TIM7 +TIM12 +TIM13 +TIM14 + +ADC_EXTERNALTRIGINJECCONV_T1_TRGO +ADC_EXTERNALTRIGINJECCONV_T2_TRGO +ADC_EXTERNALTRIGINJECCONV_T4_TRGO + +ADC_EXTERNALTRIGINJECCONV_T1_TRGO2 +ADC_EXTERNALTRIGINJECCONV_T8_TRGO2 +ADC_EXTERNALTRIGINJECCONV_T5_TRGO +ADC_EXTERNALTRIGINJECCONV_T6_TRGO +*/ +// timer to injected TRGO +// https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_adc_ex.h#L179 +uint32_t _timerToInjectedTRGO(HardwareTimer* timer){ + + if(timer->getHandle()->Instance == TIM1) + return ADC_EXTERNALTRIGINJECCONV_T1_TRGO; +#ifdef TIM2 // if defined timer 2 + else if(timer->getHandle()->Instance == TIM2) + return ADC_EXTERNALTRIGINJECCONV_T2_TRGO; +#endif +#ifdef TIM4 // if defined timer 4 + else if(timer->getHandle()->Instance == TIM4) + return ADC_EXTERNALTRIGINJECCONV_T4_TRGO; +#endif +#ifdef TIM5 // if defined timer 5 + else if(timer->getHandle()->Instance == TIM5) + return ADC_EXTERNALTRIGINJECCONV_T5_TRGO; +#endif +#ifdef TIM6 // if defined timer 6 + else if(timer->getHandle()->Instance == TIM6) + return ADC_EXTERNALTRIGINJECCONV_T6_TRGO; +#endif +#ifdef TIM8 // if defined timer 8 + else if(timer->getHandle()->Instance == TIM8) + return ADC_EXTERNALTRIGINJECCONV_T8_TRGO; +#endif + else + return _TRGO_NOT_AVAILABLE; +} +/* + +ADC_EXTERNALTRIGCONV_T5_TRGO +ADC_EXTERNALTRIGCONV_T8_TRGO +ADC_EXTERNALTRIGCONV_T8_TRGO2 +ADC_EXTERNALTRIGCONV_T1_TRGO +ADC_EXTERNALTRIGCONV_T1_TRGO2 +ADC_EXTERNALTRIGCONV_T2_TRGO +ADC_EXTERNALTRIGCONV_T4_TRGO +ADC_EXTERNALTRIGCONV_T6_TRGO +*/ + +// timer to regular TRGO +// https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_adc.h#L331 +uint32_t _timerToRegularTRGO(HardwareTimer* timer){ + if(timer->getHandle()->Instance == TIM1) + return ADC_EXTERNALTRIGCONV_T1_TRGO; +#ifdef TIM2 // if defined timer 2 + else if(timer->getHandle()->Instance == TIM2) + return ADC_EXTERNALTRIGCONV_T2_TRGO; +#endif +#ifdef TIM4 // if defined timer 4 + else if(timer->getHandle()->Instance == TIM4) + return ADC_EXTERNALTRIGCONV_T4_TRGO; +#endif +#ifdef TIM5 // if defined timer 5 + else if(timer->getHandle()->Instance == TIM5) + return ADC_EXTERNALTRIGCONV_T5_TRGO; +#endif +#ifdef TIM6 // if defined timer 6 + else if(timer->getHandle()->Instance == TIM6) + return ADC_EXTERNALTRIGCONV_T6_TRGO; +#endif +#ifdef TIM8 // if defined timer 8 + else if(timer->getHandle()->Instance == TIM8) + return ADC_EXTERNALTRIGCONV_T8_TRGO; +#endif + else + return _TRGO_NOT_AVAILABLE; +} + + +int _adcToIndex(ADC_TypeDef *AdcHandle){ + if(AdcHandle == ADC1) return 0; +#ifdef ADC2 // if ADC2 exists + else if(AdcHandle == ADC2) return 1; +#endif +#ifdef ADC3 // if ADC3 exists + else if(AdcHandle == ADC3) return 2; +#endif +#ifdef ADC4 // if ADC4 exists + else if(AdcHandle == ADC4) return 3; +#endif +#ifdef ADC5 // if ADC5 exists + else if(AdcHandle == ADC5) return 4; +#endif + return 0; +} +int _adcToIndex(ADC_HandleTypeDef *AdcHandle){ + return _adcToIndex(AdcHandle->Instance); +} + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_utils.h b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_utils.h new file mode 100644 index 00000000..017ff464 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_utils.h @@ -0,0 +1,30 @@ +#pragma once + +#include "Arduino.h" + +#if defined(STM32F7xx) + +#define _TRGO_NOT_AVAILABLE 12345 + + +/* Exported Functions */ +/** + * @brief Return ADC HAL channel linked to a PinName + * @param pin: PinName + * @retval Valid HAL channel + */ +uint32_t _getADCChannel(PinName pin); + +// timer to injected TRGO +// https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_adc_ex.h#L179 +uint32_t _timerToInjectedTRGO(HardwareTimer* timer); + +// timer to regular TRGO +// https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_adc.h#L331 +uint32_t _timerToRegularTRGO(HardwareTimer* timer); + +// function returning index of the ADC instance +int _adcToIndex(ADC_HandleTypeDef *AdcHandle); +int _adcToIndex(ADC_TypeDef *AdcHandle); + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp index 91322112..fd1090ae 100644 --- a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp @@ -100,7 +100,7 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive hadc.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE; hadc.Init.ExternalTrigConv = ADC_SOFTWARE_START; // for now hadc.Init.DataAlign = ADC_DATAALIGN_RIGHT; - hadc.Init.NbrOfConversion = 2; + hadc.Init.NbrOfConversion = 1; hadc.Init.DMAContinuousRequests = DISABLE; hadc.Init.EOCSelection = ADC_EOC_SINGLE_CONV; hadc.Init.Overrun = ADC_OVR_DATA_PRESERVED; @@ -180,42 +180,6 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive } } - - - if(hadc.Instance == ADC1) { - // enable interrupt - HAL_NVIC_SetPriority(ADC1_2_IRQn, 0, 0); - HAL_NVIC_EnableIRQ(ADC1_2_IRQn); - } -#ifdef ADC2 - else if (hadc.Instance == ADC2) { - // enable interrupt - HAL_NVIC_SetPriority(ADC1_2_IRQn, 0, 0); - HAL_NVIC_EnableIRQ(ADC1_2_IRQn); - } -#endif -#ifdef ADC3 - else if (hadc.Instance == ADC3) { - // enable interrupt - HAL_NVIC_SetPriority(ADC3_IRQn, 0, 0); - HAL_NVIC_EnableIRQ(ADC3_IRQn); - } -#endif -#ifdef ADC4 - else if (hadc.Instance == ADC4) { - // enable interrupt - HAL_NVIC_SetPriority(ADC4_IRQn, 0, 0); - HAL_NVIC_EnableIRQ(ADC4_IRQn); - } -#endif -#ifdef ADC5 - else if (hadc.Instance == ADC5) { - // enable interrupt - HAL_NVIC_SetPriority(ADC5_IRQn, 0, 0); - HAL_NVIC_EnableIRQ(ADC5_IRQn); - } -#endif - cs_params->adc_handle = &hadc; return 0; } diff --git a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp index ae96bad7..fb32d36b 100644 --- a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp @@ -11,6 +11,7 @@ #include "stm32g4_utils.h" #include "Arduino.h" +// #define SIMPLEFOC_STM32_ADC_INTERRUPT #define _ADC_VOLTAGE_G4 3.3f #define _ADC_RESOLUTION_G4 4096.0f @@ -23,6 +24,11 @@ bool needs_downsample[5] = {1}; // downsampling variable - per adc (5) uint8_t tim_downsample[5] = {0}; +#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT +uint8_t use_adc_interrupt = 1; +#else +uint8_t use_adc_interrupt = 0; +#endif void* _configureADCLowSide(const void* driver_params, const int pinA, const int pinB, const int pinC){ @@ -57,12 +63,64 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ cs_params->timer_handle->getHandle()->Instance->CNT = cs_params->timer_handle->getHandle()->Instance->ARR; // remember that this timer has repetition counter - no need to downasmple needs_downsample[_adcToIndex(cs_params->adc_handle)] = 0; + }else{ + if(!use_adc_interrupt){ + // If the timer has no repetition counter, it needs to use the interrupt to downsample for low side sensing + use_adc_interrupt = 1; + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: timer has no repetition counter, ADC interrupt has to be used"); + #endif + } } // set the trigger output event LL_TIM_SetTriggerOutput(cs_params->timer_handle->getHandle()->Instance, LL_TIM_TRGO_UPDATE); - // start the adc - HAL_ADCEx_InjectedStart_IT(cs_params->adc_handle); + + // Start the adc calibration + HAL_ADCEx_Calibration_Start(cs_params->adc_handle,ADC_SINGLE_ENDED); + + // start the adc + if (use_adc_interrupt){ + // enable interrupt + if(cs_params->adc_handle->Instance == ADC1) { + // enable interrupt + HAL_NVIC_SetPriority(ADC1_2_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC1_2_IRQn); + } + #ifdef ADC2 + else if (cs_params->adc_handle->Instance == ADC2) { + // enable interrupt + HAL_NVIC_SetPriority(ADC1_2_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC1_2_IRQn); + } + #endif + #ifdef ADC3 + else if (cs_params->adc_handle->Instance == ADC3) { + // enable interrupt + HAL_NVIC_SetPriority(ADC3_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC3_IRQn); + } + #endif + #ifdef ADC4 + else if (cs_params->adc_handle->Instance == ADC4) { + // enable interrupt + HAL_NVIC_SetPriority(ADC4_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC4_IRQn); + } + #endif + #ifdef ADC5 + else if (cs_params->adc_handle->Instance == ADC5) { + // enable interrupt + HAL_NVIC_SetPriority(ADC5_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC5_IRQn); + } + #endif + + HAL_ADCEx_InjectedStart_IT(cs_params->adc_handle); + }else{ + HAL_ADCEx_InjectedStart(cs_params->adc_handle); + } + // restart all the timers of the driver _startTimers(driver_params->timers, 6); } @@ -71,13 +129,19 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ // function reading an ADC value and returning the read voltage float _readADCVoltageLowSide(const int pin, const void* cs_params){ for(int i=0; i < 3; i++){ - if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]) // found in the buffer - return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]){ // found in the buffer + if (use_adc_interrupt){ + return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + }else{ + // an optimized way to go from i to the channel i=0 -> channel 1, i=1 -> channel 2, i=2 -> channel 3 + uint32_t channel = (i == 0) ? ADC_INJECTED_RANK_1 : (i == 1) ? ADC_INJECTED_RANK_2 : ADC_INJECTED_RANK_3; + return HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle, channel) * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + } + } } return 0; } - extern "C" { void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef *AdcHandle){ // calculate the instance diff --git a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp index bfec2175..67a0473b 100644 --- a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp @@ -99,7 +99,7 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive hadc.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE; hadc.Init.ExternalTrigConv = ADC_SOFTWARE_START; // for now hadc.Init.DataAlign = ADC_DATAALIGN_RIGHT; - hadc.Init.NbrOfConversion = 2; + hadc.Init.NbrOfConversion = 1; hadc.Init.DMAContinuousRequests = DISABLE; hadc.Init.EOCSelection = ADC_EOC_SINGLE_CONV; hadc.Init.Overrun = ADC_OVR_DATA_PRESERVED; @@ -179,42 +179,6 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive } } - - - if(hadc.Instance == ADC1) { - // enable interrupt - HAL_NVIC_SetPriority(ADC1_2_IRQn, 0, 0); - HAL_NVIC_EnableIRQ(ADC1_2_IRQn); - } -#ifdef ADC2 - else if (hadc.Instance == ADC2) { - // enable interrupt - HAL_NVIC_SetPriority(ADC1_2_IRQn, 0, 0); - HAL_NVIC_EnableIRQ(ADC1_2_IRQn); - } -#endif -#ifdef ADC3 - else if (hadc.Instance == ADC3) { - // enable interrupt - HAL_NVIC_SetPriority(ADC3_IRQn, 0, 0); - HAL_NVIC_EnableIRQ(ADC3_IRQn); - } -#endif -#ifdef ADC4 - else if (hadc.Instance == ADC4) { - // enable interrupt - HAL_NVIC_SetPriority(ADC4_IRQn, 0, 0); - HAL_NVIC_EnableIRQ(ADC4_IRQn); - } -#endif -#ifdef ADC5 - else if (hadc.Instance == ADC5) { - // enable interrupt - HAL_NVIC_SetPriority(ADC5_IRQn, 0, 0); - HAL_NVIC_EnableIRQ(ADC5_IRQn); - } -#endif - cs_params->adc_handle = &hadc; return 0; } diff --git a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp index edac6414..1fb0ab6b 100644 --- a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp @@ -23,6 +23,11 @@ bool needs_downsample[5] = {1}; // downsampling variable - per adc (5) uint8_t tim_downsample[5] = {0}; +#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT +uint8_t use_adc_interrupt = 1; +#else +uint8_t use_adc_interrupt = 0; +#endif void* _configureADCLowSide(const void* driver_params, const int pinA, const int pinB, const int pinC){ @@ -57,12 +62,62 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ cs_params->timer_handle->getHandle()->Instance->CNT = cs_params->timer_handle->getHandle()->Instance->ARR; // remember that this timer has repetition counter - no need to downasmple needs_downsample[_adcToIndex(cs_params->adc_handle)] = 0; + }else{ + if(!use_adc_interrupt){ + // If the timer has no repetition counter, it needs to use the interrupt to downsample for low side sensing + use_adc_interrupt = 1; + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: timer has no repetition counter, ADC interrupt has to be used"); + #endif + } } // set the trigger output event LL_TIM_SetTriggerOutput(cs_params->timer_handle->getHandle()->Instance, LL_TIM_TRGO_UPDATE); - // start the adc + + // Start the adc calibration + HAL_ADCEx_Calibration_Start(cs_params->adc_handle,ADC_SINGLE_ENDED); + + // start the adc + if (use_adc_interrupt){ + if(cs_params->adc_handle->Instance == ADC1) { + // enable interrupt + HAL_NVIC_SetPriority(ADC1_2_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC1_2_IRQn); + } + #ifdef ADC2 + else if (cs_params->adc_handle->Instance == ADC2) { + // enable interrupt + HAL_NVIC_SetPriority(ADC1_2_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC1_2_IRQn); + } + #endif + #ifdef ADC3 + else if (cs_params->adc_handle->Instance == ADC3) { + // enable interrupt + HAL_NVIC_SetPriority(ADC3_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC3_IRQn); + } + #endif + #ifdef ADC4 + else if (cs_params->adc_handle->Instance == ADC4) { + // enable interrupt + HAL_NVIC_SetPriority(ADC4_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC4_IRQn); + } + #endif + #ifdef ADC5 + else if (cs_params->adc_handle->Instance == ADC5) { + // enable interrupt + HAL_NVIC_SetPriority(ADC5_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC5_IRQn); + } + #endif HAL_ADCEx_InjectedStart_IT(cs_params->adc_handle); + }else{ + HAL_ADCEx_InjectedStart(cs_params->adc_handle); + } + // restart all the timers of the driver _startTimers(driver_params->timers, 6); } @@ -71,13 +126,19 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ // function reading an ADC value and returning the read voltage float _readADCVoltageLowSide(const int pin, const void* cs_params){ for(int i=0; i < 3; i++){ - if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]) // found in the buffer - return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]){ // found in the buffer + if (use_adc_interrupt){ + return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + }else{ + // an optimized way to go from i to the channel i=0 -> channel 1, i=1 -> channel 2, i=2 -> channel 3 + uint32_t channel = (i == 0) ? ADC_INJECTED_RANK_1 : (i == 1) ? ADC_INJECTED_RANK_2 : ADC_INJECTED_RANK_3; + return HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle,channel) * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + } + } } return 0; } - extern "C" { void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef *AdcHandle){ // calculate the instance diff --git a/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp b/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp new file mode 100644 index 00000000..70815d0f --- /dev/null +++ b/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp @@ -0,0 +1,244 @@ +#include "teensy4_mcu.h" +#include "../../../drivers/hardware_specific/teensy/teensy4_mcu.h" +// #include "../../../common/lowpass_filter.h" +#include "../../../common/foc_utils.h" +#include "../../../communication/SimpleFOCDebug.h" + +// if defined +// - Teensy 4.0 +// - Teensy 4.1 +#if defined(__arm__) && defined(CORE_TEENSY) && ( defined(__IMXRT1062__) || defined(ARDUINO_TEENSY40) || defined(ARDUINO_TEENSY41) || defined(ARDUINO_TEENSY_MICROMOD) ) + +// #define SIMPLEFOC_TEENSY4_ADC_INTERRUPT_DEBUG + + +volatile uint32_t val0, val1, val2; + +// #define _BANDWIDTH_CS 10000.0f // [Hz] bandwidth for the current sense +// LowPassFilter lp1 = LowPassFilter(1.0/_BANDWIDTH_CS); +// LowPassFilter lp2 = LowPassFilter(1.0/_BANDWIDTH_CS); +// LowPassFilter lp3 = LowPassFilter(1.0/_BANDWIDTH_CS); + +void read_currents(uint32_t *a, uint32_t*b, uint32_t *c=nullptr){ + *a = val0; + *b = val1; + *c = val2; +} + +// interrupt service routine for the ADC_ETC0 +// reading the ADC values and clearing the interrupt +void adcetc0_isr() { +#ifdef SIMPLEFOC_TEENSY4_ADC_INTERRUPT_DEBUG + digitalWrite(30,HIGH); +#endif + // page 3509 , section 66.5.1.3.3 + ADC_ETC_DONE0_1_IRQ |= 1; // clear Done0 for trg0 at 1st bit + // val0 = lp1(ADC_ETC_TRIG0_RESULT_1_0 & 4095); + val0 = (ADC_ETC_TRIG0_RESULT_1_0 & 4095); + // val1 = lp2((ADC_ETC_TRIG0_RESULT_1_0 >> 16) & 4095); + val1 = (ADC_ETC_TRIG0_RESULT_1_0 >> 16) & 4095; +#ifdef SIMPLEFOC_TEENSY4_ADC_INTERRUPT_DEBUG + digitalWrite(30,LOW); +#endif +} + + +void adcetc1_isr() { +#ifdef SIMPLEFOC_TEENSY4_ADC_INTERRUPT_DEBUG + digitalWrite(30,HIGH); +#endif + // page 3509 , section 66.5.1.3.3 + ADC_ETC_DONE0_1_IRQ |= 1 << 16; // clear Done1 for trg0 at 16th bit + val2 = ADC_ETC_TRIG0_RESULT_3_2 & 4095; +// val2 = lp3( ADC_ETC_TRIG0_RESULT_3_2 & 4095); +#ifdef SIMPLEFOC_TEENSY4_ADC_INTERRUPT_DEBUG + digitalWrite(30,LOW); +#endif +} + +// function initializing the ADC2 +// and the ADC_ETC trigger for the low side current sensing +void adc1_init(int pin1, int pin2, int pin3=NOT_SET) { + //Tried many configurations, but this seems to be best: + ADC1_CFG = ADC_CFG_OVWREN //Allow overwriting of the next converted Data onto the existing + | ADC_CFG_ADICLK(0) // input clock select - IPG clock + | ADC_CFG_MODE(2) // 12-bit conversion 0 8-bit conversion 1 10-bit conversion 2 12-bit conversion + | ADC_CFG_ADIV(2) // Input clock / 2 (0 for /1, 1 for /2 and 2 for / 4) (1 is faster and maybe with some filtering could provide better results but 2 for now) + | ADC_CFG_ADSTS(0) // Sample period (ADC clocks) = 3 if ADLSMP=0b + | ADC_CFG_ADHSC // High speed operation + | ADC_CFG_ADTRG; // Hardware trigger selected + + + //Calibration of ADC1 + ADC1_GC |= ADC_GC_CAL; // begin cal ADC1 + while (ADC1_GC & ADC_GC_CAL) ; + + ADC1_HC0 = 16; // ADC_ETC channel + // use the second interrupt if necessary (for more than 2 channels) + if(_isset(pin3)) { + ADC1_HC1 = 16; + } +} + +// function initializing the ADC2 +// and the ADC_ETC trigger for the low side current sensing +void adc2_init(){ + + // configuring ADC2 + //Tried many configurations, but this seems to be best: + ADC1_CFG = ADC_CFG_OVWREN //Allow overwriting of the next converted Data onto the existing + | ADC_CFG_ADICLK(0) // input clock select - IPG clock + | ADC_CFG_MODE(2) // 12-bit conversion 0 8-bit conversion 1 10-bit conversion 2 12-bit conversion + | ADC_CFG_ADIV(2) // Input clock / 2 (0 for /1, 1 for /2 and 2 for / 4) + | ADC_CFG_ADSTS(0) // Sample period (ADC clocks) = 3 if ADLSMP=0b + | ADC_CFG_ADHSC // High speed operation + | ADC_CFG_ADTRG; // Hardware trigger selected + + //Calibration of ADC2 + ADC2_GC |= ADC_GC_CAL; // begin cal ADC2 + while (ADC2_GC & ADC_GC_CAL) ; + + ADC2_HC0 = 16; // ADC_ETC channel + // use the second interrupt if necessary (for more than 2 channels) + // ADC2_HC1 = 16; +} + +// function initializing the ADC_ETC trigger for the low side current sensing +// it uses only the ADC1 +// if the pin3 is not set it uses only 2 channels +void adc_etc_init(int pin1, int pin2, int pin3=NOT_SET) { + ADC_ETC_CTRL &= ~(1 << 31); // SOFTRST + ADC_ETC_CTRL = 0x40000001; // start with trigger 0 + ADC_ETC_TRIG0_CTRL = ADC_ETC_TRIG_CTRL_TRIG_CHAIN( _isset(pin3) ? 2 : 1) ; // 2 if 3 channels, 1 if 2 channels + + // ADC1 7 8, chain channel, HWTS, IE, B2B + // pg 3516, section 66.5.1.8 + ADC_ETC_TRIG0_CHAIN_1_0 = + ADC_ETC_TRIG_CHAIN_IE1(0) | // no interrupt on first or set 2 if interrupt when Done1 + ADC_ETC_TRIG_CHAIN_B2B1 | // Enable B2B, back to back ADC trigger + ADC_ETC_TRIG_CHAIN_HWTS1(1) | + ADC_ETC_TRIG_CHAIN_CSEL1(pin_to_channel[pin1]) | // ADC channel 8 + ADC_ETC_TRIG_CHAIN_IE0(1) | // interrupt when Done0 + ADC_ETC_TRIG_CHAIN_B2B1 | // Enable B2B, back to back ADC trigger + ADC_ETC_TRIG_CHAIN_HWTS0(1) | + ADC_ETC_TRIG_CHAIN_CSEL0(pin_to_channel[pin2]); // ADC channel 7 + + attachInterruptVector(IRQ_ADC_ETC0, adcetc0_isr); + NVIC_ENABLE_IRQ(IRQ_ADC_ETC0); + // use the second interrupt if necessary (for more than 2 channels) + if(_isset(pin3)) { + ADC_ETC_TRIG0_CHAIN_3_2 = + ADC_ETC_TRIG_CHAIN_IE0(2) | // interrupt when Done1 + ADC_ETC_TRIG_CHAIN_B2B0 | // Enable B2B, back to back ADC trigger + ADC_ETC_TRIG_CHAIN_HWTS0(1) | + ADC_ETC_TRIG_CHAIN_CSEL0(pin_to_channel[pin3]); + + attachInterruptVector(IRQ_ADC_ETC1, adcetc1_isr); + NVIC_ENABLE_IRQ(IRQ_ADC_ETC1); + } +} + + + +// function reading an ADC value and returning the read voltage +float _readADCVoltageLowSide(const int pinA, const void* cs_params){ + + if(!_isset(pinA)) return 0.0; // if the pin is not set return 0 + GenericCurrentSenseParams* params = (GenericCurrentSenseParams*) cs_params; + float adc_voltage_conv = params->adc_voltage_conv; + if (pinA == params->pins[0]) { + return val0 * adc_voltage_conv; + } else if (pinA == params->pins[1]) { + return val1 * adc_voltage_conv; + }else if (pinA == params->pins[2]) { + return val2 * adc_voltage_conv; + } + return 0.0; +} + +// Configure low side for generic mcu +// cannot do much but +void* _configureADCLowSide(const void* driver_params, const int pinA,const int pinB,const int pinC){ + Teensy4DriverParams* par = (Teensy4DriverParams*) ((TeensyDriverParams*)driver_params)->additional_params; + if(par == nullptr){ + SIMPLEFOC_DEBUG("TEENSY-CS: Low side current sense failed, driver not supported!"); + return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; + } + + SIMPLEFOC_DEBUG("TEENSY-CS: Configuring low side current sense!"); + +#ifdef SIMPLEFOC_TEENSY4_ADC_INTERRUPT_DEBUG + pinMode(30,OUTPUT); +#endif + + if( _isset(pinA) ) pinMode(pinA, INPUT); + if( _isset(pinB) ) pinMode(pinB, INPUT); + if( _isset(pinC) ) pinMode(pinC, INPUT); + + // check if either of the pins are not set + // and dont use it if it isn't + int pin_count = 0; + int pins[3] = {NOT_SET, NOT_SET, NOT_SET}; + if(_isset(pinA)) pins[pin_count++] = pinA; + if(_isset(pinB)) pins[pin_count++] = pinB; + if(_isset(pinC)) pins[pin_count++] = pinC; + + + adc1_init(pins[0], pins[1], pins[2]); + adc_etc_init(pins[0], pins[1], pins[2]); + + xbar_init(); + + GenericCurrentSenseParams* params = new GenericCurrentSenseParams { + .pins = {pins[0], pins[1], pins[2] }, + .adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION) + }; + return params; +} + +// sync driver and the adc +void _driverSyncLowSide(void* driver_params, void* cs_params){ + Teensy4DriverParams* par = (Teensy4DriverParams*) ((TeensyDriverParams*)driver_params)->additional_params; + IMXRT_FLEXPWM_t* flexpwm = par->flextimers[0]; + int submodule = par->submodules[0]; + + SIMPLEFOC_DEBUG("TEENSY-CS: Syncing low side current sense!"); + char buff[50]; + sprintf(buff, "TEENSY-CS: Syncing to FlexPWM: %d, Submodule: %d", flexpwm_to_index(flexpwm), submodule); + SIMPLEFOC_DEBUG(buff); + + // find the xbar trigger for the flexpwm + int xbar_trig_pwm = flexpwm_submodule_to_trig(flexpwm, submodule); + if(xbar_trig_pwm<0) return; + + // allow theFlexPWM to trigger the ADC_ETC + xbar_connect((uint32_t)xbar_trig_pwm, XBARA1_OUT_ADC_ETC_TRIG00); //FlexPWM to adc_etc + + // setup the ADC_ETC trigger to be triggered by the FlexPWM channel 1 (val1) + //This val1 interrupt on match is in the center of the PWM + flexpwm->SM[submodule].TCTRL = FLEXPWM_SMTCTRL_OUT_TRIG_EN(1<<1); + + + // if needed the interrupt can be moved to some other point in the PWM cycle by using an addional val register example: VAL4 + // setup the ADC_ETC trigger to be triggered by the FlexPWM channel 4 (val4) + // flexpwm->SM[submodule].TCTRL = FLEXPWM_SMTCTRL_OUT_TRIG_EN(1<<4); + // setup this val4 for interrupt on match for ADC sync + // this code assumes that the val4 is not used for anything else! + // reading two ADC takes about 2.5us. So put the interrupt 2.5us befor the center + // flexpwm->SM[submodule].VAL4 = int(flexpwm->SM[submodule].VAL1*(1.0f - 2.5e-6*par->pwm_frequency)) ; // 2.5us before center + + +#ifdef SIMPLEFOC_TEENSY4_ADC_INTERRUPT_DEBUG + // pin 4 observes out trigger line for 'scope + xbar_connect (xbar_trig_pwm, XBARA1_OUT_IOMUX_XBAR_INOUT08) ; + IOMUXC_GPR_GPR6 |= IOMUXC_GPR_GPR6_IOMUXC_XBAR_DIR_SEL_8 ; // select output mode for INOUT8 + // Select alt 3 for EMC_06 (XBAR), rather than original 5 (GPIO) + CORE_PIN4_CONFIG = 3 ; // shorthand for IOMUXC_SW_MUX_CTL_PAD_GPIO_EMC_06 = 3 ; + // turn up drive & speed as very short pulse + IOMUXC_SW_PAD_CTL_PAD_GPIO_EMC_06 = IOMUXC_PAD_DSE(7) | IOMUXC_PAD_SPEED(3) | IOMUXC_PAD_SRE ; +#endif + +} + + +#endif diff --git a/src/current_sense/hardware_specific/teensy/teensy4_mcu.h b/src/current_sense/hardware_specific/teensy/teensy4_mcu.h new file mode 100644 index 00000000..2cf77dfb --- /dev/null +++ b/src/current_sense/hardware_specific/teensy/teensy4_mcu.h @@ -0,0 +1,77 @@ + +#ifndef TEENSY4_CURRENTSENSE_MCU_DEF +#define TEENSY4_CURRENTSENSE_MCU_DEF + +#include "../../hardware_api.h" +#include "../../../common/foc_utils.h" + +// if defined +// - Teensy 4.0 +// - Teensy 4.1 +#if defined(__arm__) && defined(CORE_TEENSY) && ( defined(__IMXRT1062__) || defined(ARDUINO_TEENSY40) || defined(ARDUINO_TEENSY41) || defined(ARDUINO_TEENSY_MICROMOD) ) + +#define _ADC_VOLTAGE 3.3f +#define _ADC_RESOLUTION 4026.0f + +// generic implementation of the hardware specific structure +// containing all the necessary current sense parameters +// will be returned as a void pointer from the _configureADCx functions +// will be provided to the _readADCVoltageX() as a void pointer +typedef struct Teensy4CurrentSenseParams { + int pins[3] = {(int)NOT_SET}; + float adc_voltage_conv; +} Teensy4CurrentSenseParams; + + + +const uint8_t pin_to_channel[] = { // pg 482 + 7, // 0/A0 AD_B1_02 + 8, // 1/A1 AD_B1_03 + 12, // 2/A2 AD_B1_07 + 11, // 3/A3 AD_B1_06 + 6, // 4/A4 AD_B1_01 + 5, // 5/A5 AD_B1_00 + 15, // 6/A6 AD_B1_10 + 0, // 7/A7 AD_B1_11 + 13, // 8/A8 AD_B1_08 + 14, // 9/A9 AD_B1_09 + 1, // 24/A10 AD_B0_12 + 2, // 25/A11 AD_B0_13 + 128+3, // 26/A12 AD_B1_14 - only on ADC2, 3 + 128+4, // 27/A13 AD_B1_15 - only on ADC2, 4 + 7, // 14/A0 AD_B1_02 + 8, // 15/A1 AD_B1_03 + 12, // 16/A2 AD_B1_07 + 11, // 17/A3 AD_B1_06 + 6, // 18/A4 AD_B1_01 + 5, // 19/A5 AD_B1_00 + 15, // 20/A6 AD_B1_10 + 0, // 21/A7 AD_B1_11 + 13, // 22/A8 AD_B1_08 + 14, // 23/A9 AD_B1_09 + 1, // 24/A10 AD_B0_12 + 2, // 25/A11 AD_B0_13 + 128+3, // 26/A12 AD_B1_14 - only on ADC2, 3 + 128+4, // 27/A13 AD_B1_15 - only on ADC2, 4 +#ifdef ARDUINO_TEENSY41 + 255, // 28 + 255, // 29 + 255, // 30 + 255, // 31 + 255, // 32 + 255, // 33 + 255, // 34 + 255, // 35 + 255, // 36 + 255, // 37 + 128+1, // 38/A14 AD_B1_12 - only on ADC2, 1 + 128+2, // 39/A15 AD_B1_13 - only on ADC2, 2 + 9, // 40/A16 AD_B1_04 + 10, // 41/A17 AD_B1_05 +#endif +}; + + +#endif + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/teensy_mcu.cpp b/src/current_sense/hardware_specific/teensy/teensy_mcu.cpp similarity index 94% rename from src/current_sense/hardware_specific/teensy_mcu.cpp rename to src/current_sense/hardware_specific/teensy/teensy_mcu.cpp index 7ab370a4..7669edc8 100644 --- a/src/current_sense/hardware_specific/teensy_mcu.cpp +++ b/src/current_sense/hardware_specific/teensy/teensy_mcu.cpp @@ -1,4 +1,4 @@ -#include "../hardware_api.h" +#include "../../hardware_api.h" #if defined(__arm__) && defined(CORE_TEENSY) diff --git a/src/drivers/hardware_specific/atmega/atmega2560_mcu.cpp b/src/drivers/hardware_specific/atmega/atmega2560_mcu.cpp index 8a7fbbec..532b3ce3 100644 --- a/src/drivers/hardware_specific/atmega/atmega2560_mcu.cpp +++ b/src/drivers/hardware_specific/atmega/atmega2560_mcu.cpp @@ -57,7 +57,8 @@ void* _configure1PWM(long pwm_frequency,const int pinA) { _pinHighFrequency(pinA, pwm_frequency); GenericDriverParams* params = new GenericDriverParams { .pins = { pinA }, - .pwm_frequency = pwm_frequency + .pwm_frequency = pwm_frequency, + .dead_zone = 0.0f }; return params; } @@ -75,7 +76,8 @@ void* _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { _pinHighFrequency(pinB, pwm_frequency); GenericDriverParams* params = new GenericDriverParams { .pins = { pinA, pinB }, - .pwm_frequency = pwm_frequency + .pwm_frequency = pwm_frequency, + .dead_zone = 0.0f }; return params; } @@ -94,7 +96,8 @@ void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const in _pinHighFrequency(pinC, pwm_frequency); GenericDriverParams* params = new GenericDriverParams { .pins = { pinA, pinB, pinC }, - .pwm_frequency = pwm_frequency + .pwm_frequency = pwm_frequency, + .dead_zone = 0.0f }; // _syncAllTimers(); return params; @@ -141,7 +144,8 @@ void* _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const _pinHighFrequency(pin2B,pwm_frequency); GenericDriverParams* params = new GenericDriverParams { .pins = { pin1A, pin1B, pin2A, pin2B }, - .pwm_frequency = pwm_frequency + .pwm_frequency = pwm_frequency, + .dead_zone = 0.0f }; return params; } diff --git a/src/drivers/hardware_specific/atmega/atmega328_mcu.cpp b/src/drivers/hardware_specific/atmega/atmega328_mcu.cpp index 53fb1086..9df9b8a7 100644 --- a/src/drivers/hardware_specific/atmega/atmega328_mcu.cpp +++ b/src/drivers/hardware_specific/atmega/atmega328_mcu.cpp @@ -53,7 +53,8 @@ void* _configure1PWM(long pwm_frequency,const int pinA) { _pinHighFrequency(pinA, pwm_frequency); GenericDriverParams* params = new GenericDriverParams { .pins = { pinA }, - .pwm_frequency = pwm_frequency + .pwm_frequency = pwm_frequency, + .dead_zone = 0.0f }; return params; } @@ -70,7 +71,8 @@ void* _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { _pinHighFrequency(pinB, pwm_frequency); GenericDriverParams* params = new GenericDriverParams { .pins = { pinA, pinB }, - .pwm_frequency = pwm_frequency + .pwm_frequency = pwm_frequency, + .dead_zone = 0.0f }; return params; } @@ -89,7 +91,8 @@ void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const in _pinHighFrequency(pinC, pwm_frequency); GenericDriverParams* params = new GenericDriverParams { .pins = { pinA, pinB, pinC }, - .pwm_frequency = pwm_frequency + .pwm_frequency = pwm_frequency, + .dead_zone = 0.0f }; _syncAllTimers(); return params; @@ -141,7 +144,8 @@ void* _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const _pinHighFrequency(pin2B,pwm_frequency); GenericDriverParams* params = new GenericDriverParams { .pins = { pin1A, pin1B, pin2A, pin2B }, - .pwm_frequency = pwm_frequency + .pwm_frequency = pwm_frequency, + .dead_zone = 0.0f }; return params; } diff --git a/src/drivers/hardware_specific/atmega/atmega32u4_mcu.cpp b/src/drivers/hardware_specific/atmega/atmega32u4_mcu.cpp index b4ad3101..4cf454ae 100644 --- a/src/drivers/hardware_specific/atmega/atmega32u4_mcu.cpp +++ b/src/drivers/hardware_specific/atmega/atmega32u4_mcu.cpp @@ -41,7 +41,8 @@ void* _configure1PWM(long pwm_frequency,const int pinA) { _pinHighFrequency(pinA); GenericDriverParams* params = new GenericDriverParams { .pins = { pinA }, - .pwm_frequency = pwm_frequency + .pwm_frequency = pwm_frequency, + .dead_zone = 0.0f }; return params; } @@ -56,7 +57,8 @@ void* _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { _pinHighFrequency(pinB); GenericDriverParams* params = new GenericDriverParams { .pins = { pinA, pinB }, - .pwm_frequency = pwm_frequency + .pwm_frequency = pwm_frequency, + .dead_zone = 0.0f }; return params; } @@ -72,7 +74,8 @@ void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const in _pinHighFrequency(pinC); GenericDriverParams* params = new GenericDriverParams { .pins = { pinA, pinB, pinC }, - .pwm_frequency = pwm_frequency + .pwm_frequency = pwm_frequency, + .dead_zone = 0.0f }; return params; } @@ -119,7 +122,8 @@ void* _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const _pinHighFrequency(pin2B); GenericDriverParams* params = new GenericDriverParams { .pins = { pin1A, pin1B, pin2A, pin2B }, - .pwm_frequency = pwm_frequency + .pwm_frequency = pwm_frequency, + .dead_zone = 0.0f }; return params; } diff --git a/src/drivers/hardware_specific/rp2040/rp2040_mcu.cpp b/src/drivers/hardware_specific/rp2040/rp2040_mcu.cpp index eee5797b..f187fbef 100644 --- a/src/drivers/hardware_specific/rp2040/rp2040_mcu.cpp +++ b/src/drivers/hardware_specific/rp2040/rp2040_mcu.cpp @@ -2,6 +2,10 @@ /** * Support for the RP2040 MCU, as found on the Raspberry Pi Pico. */ + +#include "./rp2040_mcu.h" + + #if defined(TARGET_RP2040) @@ -13,9 +17,9 @@ #define SIMPLEFOC_DEBUG_RP2040 #include "../../hardware_api.h" -#include "./rp2040_mcu.h" #include "hardware/pwm.h" #include "hardware/clocks.h" +#include #define _PWM_FREQUENCY 24000 #define _PWM_FREQUENCY_MAX 66000 @@ -30,7 +34,8 @@ uint16_t wrapvalues[NUM_PWM_SLICES]; // TODO add checks which channels are already used... -void setupPWM(int pin, long pwm_frequency, bool invert, RP2040DriverParams* params, uint8_t index) { +void setupPWM(int pin_nr, long pwm_frequency, bool invert, RP2040DriverParams* params, uint8_t index) { + uint pin = (uint)digitalPinToPinName(pin_nr); // we could check for -DBOARD_HAS_PIN_REMAP ? gpio_set_function(pin, GPIO_FUNC_PWM); uint slice = pwm_gpio_to_slice_num(pin); uint chan = pwm_gpio_to_channel(pin); @@ -45,7 +50,7 @@ void setupPWM(int pin, long pwm_frequency, bool invert, RP2040DriverParams* para uint32_t wrapvalue = (sysclock_hz * 8) / div / pwm_frequency - 1; #ifdef SIMPLEFOC_DEBUG_RP2040 SimpleFOCDebug::print("Configuring pin "); - SimpleFOCDebug::print(pin); + SimpleFOCDebug::print((int)pin); SimpleFOCDebug::print(" slice "); SimpleFOCDebug::print((int)slice); SimpleFOCDebug::print(" channel "); diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp index 65dad9f4..0fdec2f0 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp @@ -4,13 +4,12 @@ #if defined(_STM32_DEF_) +#define SIMPLEFOC_STM32_DEBUG #pragma message("") #pragma message("SimpleFOC: compiling for STM32") #pragma message("") -//#define SIMPLEFOC_STM32_DEBUG - #ifdef SIMPLEFOC_STM32_DEBUG void printTimerCombination(int numPins, PinMap* timers[], int score); int getTimerNumber(int timerIndex); @@ -27,7 +26,15 @@ PinMap* timerPinsUsed[SIMPLEFOC_STM32_MAX_PINTIMERSUSED]; +bool _getPwmState(void* params) { + // assume timers are synchronized and that there's at least one timer + HardwareTimer* pHT = ((STM32DriverParams*)params)->timers[0]; + TIM_HandleTypeDef* htim = pHT->getHandle(); + + bool dir = __HAL_TIM_IS_TIM_COUNTING_DOWN(htim); + return dir; +} // setting pwm to hardware pin - instead analogWrite() @@ -204,17 +211,128 @@ void _stopTimers(HardwareTimer **timers_to_stop, int timer_num) } } -// align the timers to end the init -void _startTimers(HardwareTimer **timers_to_start, int timer_num) -{ - // TODO - sart each timer only once - // sart timers - for (int i=0; i < timer_num; i++) { - if(timers_to_start[i] == NP) return; - timers_to_start[i]->resume(); - #ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-DRV: Starting timer ", getTimerNumber(get_timer_index(timers_to_start[i]->getHandle()->Instance))); + +#if defined(STM32G4xx) +// function finds the appropriate timer source trigger for the master/slave timer combination +// returns -1 if no trigger source is found +// currently supports the master timers to be from TIM1 to TIM4 and TIM8 +int _getInternalSourceTrigger(HardwareTimer* master, HardwareTimer* slave) { // put master and slave in temp variables to avoid arrows + TIM_TypeDef *TIM_master = master->getHandle()->Instance; + #if defined(TIM1) && defined(LL_TIM_TS_ITR0) + if (TIM_master == TIM1) return LL_TIM_TS_ITR0;// return TIM_TS_ITR0; + #endif + #if defined(TIM2) && defined(LL_TIM_TS_ITR1) + else if (TIM_master == TIM2) return LL_TIM_TS_ITR1;//return TIM_TS_ITR1; + #endif + #if defined(TIM3) && defined(LL_TIM_TS_ITR2) + else if (TIM_master == TIM3) return LL_TIM_TS_ITR2;//return TIM_TS_ITR2; + #endif + #if defined(TIM4) && defined(LL_TIM_TS_ITR3) + else if (TIM_master == TIM4) return LL_TIM_TS_ITR3;//return TIM_TS_ITR3; + #endif + #if defined(TIM5) && defined(LL_TIM_TS_ITR4) + else if (TIM_master == TIM5) return LL_TIM_TS_ITR4;//return TIM_TS_ITR4; + #endif + #if defined(TIM8) && defined(LL_TIM_TS_ITR5) + else if (TIM_master == TIM8) return LL_TIM_TS_ITR5;//return TIM_TS_ITR5; + #endif + return -1; +} +#elif defined(STM32F4xx) || defined(STM32F1xx) || defined(STM32L4xx) || defined(STM32F7xx) + +// function finds the appropriate timer source trigger for the master/slave timer combination +// returns -1 if no trigger source is found +// currently supports the master timers to be from TIM1 to TIM4 and TIM8 +int _getInternalSourceTrigger(HardwareTimer* master, HardwareTimer* slave) { + // put master and slave in temp variables to avoid arrows + TIM_TypeDef *TIM_master = master->getHandle()->Instance; + TIM_TypeDef *TIM_slave = slave->getHandle()->Instance; + #if defined(TIM1) && defined(LL_TIM_TS_ITR0) + if (TIM_master == TIM1){ + if(TIM_slave == TIM2 || TIM_slave == TIM3 || TIM_slave == TIM4) return LL_TIM_TS_ITR0; + #if defined(TIM8) + else if(TIM_slave == TIM8) return LL_TIM_TS_ITR0; + #endif + } + #endif + #if defined(TIM2) && defined(LL_TIM_TS_ITR1) + else if (TIM_master == TIM2){ + if(TIM_slave == TIM1 || TIM_slave == TIM3 || TIM_slave == TIM4) return LL_TIM_TS_ITR1; + #if defined(TIM8) + else if(TIM_slave == TIM8) return LL_TIM_TS_ITR1; + #endif + #if defined(TIM5) + else if(TIM_slave == TIM5) return LL_TIM_TS_ITR0; + #endif + } + #endif + #if defined(TIM3) && defined(LL_TIM_TS_ITR2) + else if (TIM_master == TIM3){ + if(TIM_slave== TIM1 || TIM_slave == TIM2 || TIM_slave == TIM4) return LL_TIM_TS_ITR2; + #if defined(TIM5) + else if(TIM_slave == TIM5) return LL_TIM_TS_ITR1; + #endif + } + #endif + #if defined(TIM4) && defined(LL_TIM_TS_ITR3) + else if (TIM_master == TIM4){ + if(TIM_slave == TIM1 || TIM_slave == TIM2 || TIM_slave == TIM3) return LL_TIM_TS_ITR3; + #if defined(TIM8) + else if(TIM_slave == TIM8) return LL_TIM_TS_ITR2; + #endif + #if defined(TIM5) + else if(TIM_slave == TIM5) return LL_TIM_TS_ITR1; + #endif + } + #endif + #if defined(TIM5) + else if (TIM_master == TIM5){ + #if !defined(STM32L4xx) // only difference between F4,F1 and L4 + if(TIM_slave == TIM1) return LL_TIM_TS_ITR0; + else if(TIM_slave == TIM3) return LL_TIM_TS_ITR2; + #endif + #if defined(TIM8) + if(TIM_slave == TIM8) return LL_TIM_TS_ITR3; + #endif + } + #endif + #if defined(TIM8) + else if (TIM_master == TIM8){ + if(TIM_slave==TIM2) return LL_TIM_TS_ITR1; + else if(TIM_slave ==TIM4 || TIM_slave ==TIM5) return LL_TIM_TS_ITR3; + } + #endif + return -1; // combination not supported +} +#else +// Alignment not supported for this architecture +int _getInternalSourceTrigger(HardwareTimer* master, HardwareTimer* slave) { + return -1; +} +#endif + +void syncTimerFrequency(long pwm_frequency, HardwareTimer *timers[], uint8_t num_timers) { + uint32_t max_frequency = 0; + uint32_t min_frequency = UINT32_MAX; + for (size_t i=0; igetTimerClkFreq(); + if (freq > max_frequency) { + max_frequency = freq; + } else if (freq < min_frequency) { + min_frequency = freq; + } + } + if (max_frequency==min_frequency) return; + uint32_t overflow_value = min_frequency/pwm_frequency; + for (size_t i=0; igetTimerClkFreq()/min_frequency; + #ifdef SIMPLEFOC_DEBUG + SIMPLEFOC_DEBUG("STM32-DRV: Setting prescale to ", (float)prescale_factor); + SIMPLEFOC_DEBUG("STM32-DRV: Setting Overflow to ", (float)overflow_value); #endif + timers[i]->setPrescaleFactor(prescale_factor); + timers[i]->setOverflow(overflow_value,TICK_FORMAT); + timers[i]->refresh(); } } @@ -222,7 +340,7 @@ void _alignTimersNew() { int numTimers = 0; HardwareTimer *timers[numTimerPinsUsed]; - // reset timer counters + // find the timers used for (int i=0; iperipheral); HardwareTimer *timer = (HardwareTimer *)(HardwareTimer_Handle[index]->__this); @@ -237,10 +355,66 @@ void _alignTimersNew() { timers[numTimers++] = timer; } + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-DRV: Syncronising timers! Timer no. ", numTimers); + #endif + + // see if there is more then 1 timers used for the pwm + // if yes, try to align timers + if(numTimers > 1){ + // find the master timer + int16_t master_index = -1; + int triggerEvent = -1; + for (int i=0; igetHandle()->Instance)) { + // check if timer already configured in TRGO update mode (used for ADC triggering) + // in that case we should not change its TRGO configuration + if(timers[i]->getHandle()->Instance->CR2 & LL_TIM_TRGO_UPDATE) continue; + // check if the timer has the supported internal trigger for other timers + for (int slave_i=0; slave_igetHandle()->Instance))); + #endif + // make the master timer generate ITRGx event + // if it was already configured in slave mode + LL_TIM_SetSlaveMode(timers[master_index]->getHandle()->Instance, LL_TIM_SLAVEMODE_DISABLED ); + // Configure the master timer to send a trigger signal on enable + LL_TIM_SetTriggerOutput(timers[master_index]->getHandle()->Instance, LL_TIM_TRGO_ENABLE); + // configure other timers to get the input trigger from the master timer + for (int slave_index=0; slave_index < numTimers; slave_index++) { + if (slave_index == master_index) + continue; + // Configure the slave timer to be triggered by the master enable signal + LL_TIM_SetTriggerInput(timers[slave_index]->getHandle()->Instance, _getInternalSourceTrigger(timers[master_index], timers[slave_index])); + LL_TIM_SetSlaveMode(timers[slave_index]->getHandle()->Instance, LL_TIM_SLAVEMODE_TRIGGER); + } + } + } + // enable timer clock for (int i=0; ipause(); - timers[i]->refresh(); + // timers[i]->refresh(); #ifdef SIMPLEFOC_STM32_DEBUG SIMPLEFOC_DEBUG("STM32-DRV: Restarting timer ", getTimerNumber(get_timer_index(timers[i]->getHandle()->Instance))); #endif @@ -254,6 +428,20 @@ void _alignTimersNew() { +// align the timers to end the init +void _startTimers(HardwareTimer **timers_to_start, int timer_num) +{ + // // TODO - start each timer only once + // // start timers + // for (int i=0; i < timer_num; i++) { + // if(timers_to_start[i] == NP) return; + // timers_to_start[i]->resume(); + // #ifdef SIMPLEFOC_STM32_DEBUG + // SIMPLEFOC_DEBUG("STM32-DRV: Starting timer ", getTimerNumber(get_timer_index(timers_to_start[i]->getHandle()->Instance))); + // #endif + // } + _alignTimersNew(); +} // configure hardware 6pwm for a complementary pair of channels @@ -540,7 +728,7 @@ void* _configure1PWM(long pwm_frequency, const int pinA) { return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinTimers[0]);\ - // allign the timers + // align the timers _alignTimersNew(); uint32_t channel1 = STM_PIN_CHANNEL(pinTimers[0]->function); @@ -579,6 +767,8 @@ void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinTimers[0]); HardwareTimer* HT2 = _initPinPWM(pwm_frequency, pinTimers[1]); + HardwareTimer *timers[2] = {HT1, HT2}; + syncTimerFrequency(pwm_frequency, timers, 2); // allign the timers _alignPWMTimers(HT1, HT2, HT2); @@ -598,6 +788,8 @@ void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { +TIM_MasterConfigTypeDef sMasterConfig; +TIM_SlaveConfigTypeDef sSlaveConfig; // function setting the high pwm frequency to the supplied pins // - BLDC motor - 3PWM setting @@ -620,7 +812,10 @@ void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const in HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinTimers[0]); HardwareTimer* HT2 = _initPinPWM(pwm_frequency, pinTimers[1]); HardwareTimer* HT3 = _initPinPWM(pwm_frequency, pinTimers[2]); - + + HardwareTimer *timers[3] = {HT1, HT2, HT3}; + syncTimerFrequency(pwm_frequency, timers, 3); + uint32_t channel1 = STM_PIN_CHANNEL(pinTimers[0]->function); uint32_t channel2 = STM_PIN_CHANNEL(pinTimers[1]->function); uint32_t channel3 = STM_PIN_CHANNEL(pinTimers[2]->function); @@ -663,6 +858,8 @@ void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const in HardwareTimer* HT2 = _initPinPWM(pwm_frequency, pinTimers[1]); HardwareTimer* HT3 = _initPinPWM(pwm_frequency, pinTimers[2]); HardwareTimer* HT4 = _initPinPWM(pwm_frequency, pinTimers[3]); + HardwareTimer *timers[4] = {HT1, HT2, HT3, HT4}; + syncTimerFrequency(pwm_frequency, timers, 4); // allign the timers _alignPWMTimers(HT1, HT2, HT3, HT4); @@ -760,6 +957,8 @@ void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, cons HardwareTimer* HT4 = _initPinPWMLow(pwm_frequency, pinTimers[3]); HardwareTimer* HT5 = _initPinPWMHigh(pwm_frequency, pinTimers[4]); HardwareTimer* HT6 = _initPinPWMLow(pwm_frequency, pinTimers[5]); + HardwareTimer *timers[6] = {HT1, HT2, HT3, HT4, HT5, HT6}; + syncTimerFrequency(pwm_frequency, timers, 6); uint32_t channel1 = STM_PIN_CHANNEL(pinTimers[0]->function); uint32_t channel2 = STM_PIN_CHANNEL(pinTimers[1]->function); uint32_t channel3 = STM_PIN_CHANNEL(pinTimers[2]->function); @@ -945,8 +1144,4 @@ void printTimerCombination(int numPins, PinMap* timers[], int score) { #endif - - - - #endif diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.h b/src/drivers/hardware_specific/stm32/stm32_mcu.h index fa6280e9..411c43b2 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.h +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.h @@ -28,5 +28,8 @@ typedef struct STM32DriverParams { void _stopTimers(HardwareTimer **timers_to_stop, int timer_num=6); void _startTimers(HardwareTimer **timers_to_start, int timer_num=6); +// timer query functions +bool _getPwmState(void* params); + #endif #endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp b/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp index 322d5a34..47108447 100644 --- a/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp +++ b/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp @@ -1,4 +1,3 @@ -#include "teensy_mcu.h" #include "teensy4_mcu.h" #include "../../../communication/SimpleFOCDebug.h" @@ -7,16 +6,150 @@ // - Teensy 4.1 #if defined(__arm__) && defined(CORE_TEENSY) && ( defined(__IMXRT1062__) || defined(ARDUINO_TEENSY40) || defined(ARDUINO_TEENSY41) || defined(ARDUINO_TEENSY_MICROMOD) ) - #pragma message("") #pragma message("SimpleFOC: compiling for Teensy 4.x") #pragma message("") +// #define SIMPLEFOC_TEENSY4_FORCE_CENTER_ALIGNED_3PWM + + +// function finding the TRIG event given the flexpwm timer and the submodule +// returning -1 if the submodule is not valid or no trigger is available +// allowing flexpwm1-4 and submodule 0-3 +// +// the flags are defined in the imxrt.h file +// https://github.com/PaulStoffregen/cores/blob/dd6aa8419ee173a0a6593eab669fbff54ed85f48/teensy4/imxrt.h#L9662-L9693 +int flexpwm_submodule_to_trig(IMXRT_FLEXPWM_t* flexpwm, int submodule){ + if(submodule <0 && submodule > 3) return -1; + if(flexpwm == &IMXRT_FLEXPWM1){ + return XBARA1_IN_FLEXPWM1_PWM1_OUT_TRIG0 + submodule; + }else if(flexpwm == &IMXRT_FLEXPWM2){ + return XBARA1_IN_FLEXPWM2_PWM1_OUT_TRIG0 + submodule; + }else if(flexpwm == &IMXRT_FLEXPWM3){ + return XBARA1_IN_FLEXPWM3_PWM1_OUT_TRIG0 + submodule; + }else if(flexpwm == &IMXRT_FLEXPWM4){ + return XBARA1_IN_FLEXPWM4_PWM1_OUT_TRIG0 + submodule; + } + return -1; +} + +// function finding the EXT_SYNC event given the flexpwm timer and the submodule +// returning -1 if the submodule is not valid or no trigger is available +// allowing flexpwm1-4 and submodule 0-3 +// +// the flags are defined in the imxrt.h file +// https://github.com/PaulStoffregen/cores/blob/dd6aa8419ee173a0a6593eab669fbff54ed85f48/teensy4/imxrt.h#L9757 +int flexpwm_submodule_to_ext_sync(IMXRT_FLEXPWM_t* flexpwm, int submodule){ + if(submodule < 0 && submodule > 3) return -1; + if(flexpwm == &IMXRT_FLEXPWM1){ + return XBARA1_OUT_FLEXPWM1_PWM0_EXT_SYNC + submodule; + }else if(flexpwm == &IMXRT_FLEXPWM2){ + return XBARA1_OUT_FLEXPWM2_PWM0_EXT_SYNC + submodule; + }else if(flexpwm == &IMXRT_FLEXPWM3){ + return XBARA1_OUT_FLEXPWM3_EXT_SYNC0 + submodule; // TODO verify why they are not called PWM0_EXT_SYNC but EXT_SYNC0 + }else if(flexpwm == &IMXRT_FLEXPWM4){ + return XBARA1_OUT_FLEXPWM4_EXT_SYNC0 + submodule; // TODO verify why they are not called PWM0_EXT_SYNC but EXT_SYNC0 + } + return -1; +} + +// function finding the flexpwm instance given the submodule +int flexpwm_to_index(IMXRT_FLEXPWM_t* flexpwm){ + if(flexpwm == &IMXRT_FLEXPWM1) return 1; + if(flexpwm == &IMXRT_FLEXPWM2) return 2; + if(flexpwm == &IMXRT_FLEXPWM3) return 3; + if(flexpwm == &IMXRT_FLEXPWM4) return 4; + return -1; +} + +// The i.MXRT1062 uses one config register per two XBAR outputs, so a helper +// function to make code more readable. +void xbar_connect(unsigned int input, unsigned int output) +{ + if (input >= 88) return; + if (output >= 132) return; + volatile uint16_t *xbar = &XBARA1_SEL0 + (output / 2); + uint16_t val = *xbar; + if (!(output & 1)) { + val = (val & 0xFF00) | input; + } else { + val = (val & 0x00FF) | (input << 8); + } + *xbar = val; +} + +void xbar_init() { + CCM_CCGR2 |= CCM_CCGR2_XBAR1(CCM_CCGR_ON); //turn clock on for xbara1 +} -// half_cycle of the PWM variable -int half_cycle = 0; +// function which finds the flexpwm instance for a pin +// if it does not belong to the flexpwm timer it returns a null-pointer +IMXRT_FLEXPWM_t* get_flexpwm(uint8_t pin){ + + const struct pwm_pin_info_struct *info; + info = pwm_pin_info + pin; + if (pin >= CORE_NUM_DIGITAL || info->type == 2) { +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: ERR: Pin: %d not Flextimer pin!", pin); + SIMPLEFOC_DEBUG(s); +#endif + return nullptr; + } + // FlexPWM pin + IMXRT_FLEXPWM_t *flexpwm; + switch ((info->module >> 4) & 3) { + case 0: flexpwm = &IMXRT_FLEXPWM1; break; + case 1: flexpwm = &IMXRT_FLEXPWM2; break; + case 2: flexpwm = &IMXRT_FLEXPWM3; break; + default: flexpwm = &IMXRT_FLEXPWM4; + } + if(flexpwm != nullptr){ +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: Pin: %d on Flextimer %d.", pin, ((info->module >> 4) & 3) + 1); + SIMPLEFOC_DEBUG(s); +#endif + return flexpwm; + } + return nullptr; +} +// function which finds the timer submodule for a pin +// if it does not belong to the submodule it returns a -1 +int get_submodule(uint8_t pin){ + + const struct pwm_pin_info_struct *info; + if (pin >= CORE_NUM_DIGITAL){ +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: ERR: Pin: %d not Flextimer pin!", pin); + SIMPLEFOC_DEBUG(s); +#endif + return -1; + } + + info = pwm_pin_info + pin; + int sm1 = info->module&0x3; + + if (sm1 >= 0 && sm1 < 4) { +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: Pin %d on submodule %d.", pin, sm1); + SIMPLEFOC_DEBUG(s); +#endif + return sm1; + } else { +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[50]; + sprintf (s, "TEENSY-DRV: ERR: Pin: %d not in submodule!", pin); + SIMPLEFOC_DEBUG(s); +#endif + return -1; + } +} + // function which finds the flexpwm instance for a pair of pins // if they do not belong to the same timer it returns a nullpointer IMXRT_FLEXPWM_t* get_flexpwm(uint8_t pin, uint8_t pin1){ @@ -98,11 +231,33 @@ int get_submodule(uint8_t pin, uint8_t pin1){ } +// function which finds the channel for flexpwm timer pin +// 0 - X +// 1 - A +// 2 - B +int get_channel(uint8_t pin){ + const struct pwm_pin_info_struct *info; + info = pwm_pin_info + pin; + if (pin >= CORE_NUM_DIGITAL || info->type == 2){ +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[90]; + sprintf (s, "TEENSY-DRV: ERR: Pin: %d not Flextimer pin!", pin); + SIMPLEFOC_DEBUG(s); +#endif + return -1; + } +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: Pin: %d on channel %s.", pin, info->channel==0 ? "X" : info->channel==1 ? "A" : "B"); + SIMPLEFOC_DEBUG(s); +#endif + return info->channel; +} + // function which finds the timer submodule for a pair of pins // if they do not belong to the same submodule it returns a -1 int get_inverted_channel(uint8_t pin, uint8_t pin1){ - const struct pwm_pin_info_struct *info; if (pin >= CORE_NUM_DIGITAL || pin1 >= CORE_NUM_DIGITAL){ #ifdef SIMPLEFOC_TEENSY_DEBUG char s[60]; @@ -112,10 +267,8 @@ int get_inverted_channel(uint8_t pin, uint8_t pin1){ return -1; } - info = pwm_pin_info + pin; - int ch1 = info->channel; - info = pwm_pin_info + pin1; - int ch2 = info->channel; + int ch1 = get_channel(pin); + int ch2 = get_channel(pin1); if (ch1 != 1) { #ifdef SIMPLEFOC_TEENSY_DEBUG @@ -145,7 +298,7 @@ return ch2; // can configure sync, prescale and B inversion. // sets the desired frequency of the PWM // sets the center-aligned pwm -void setup_pwm_pair (IMXRT_FLEXPWM_t * flexpwm, int submodule, const long frequency, float dead_zone ) +void setup_pwm_pair (IMXRT_FLEXPWM_t * flexpwm, int submodule, bool ext_sync, const long frequency, float dead_zone ) { int submodule_mask = 1 << submodule ; flexpwm->MCTRL &= ~ FLEXPWM_MCTRL_RUN (submodule_mask) ; // stop it if its already running @@ -167,33 +320,98 @@ void setup_pwm_pair (IMXRT_FLEXPWM_t * flexpwm, int submodule, const long freque } // the halfcycle of the PWM - half_cycle = int(newdiv/2.0f); + int half_cycle = int(newdiv/2.0f); int dead_time = int(dead_zone*half_cycle); //default dead-time - 2% int mid_pwm = int((half_cycle)/2.0f); + // if the timer should be externally synced with the master timer + int sel = ext_sync ? 3 : 0; + // setup the timer // https://github.com/PaulStoffregen/cores/blob/master/teensy4/imxrt.h flexpwm->SM[submodule].CTRL2 = FLEXPWM_SMCTRL2_WAITEN | FLEXPWM_SMCTRL2_DBGEN | - FLEXPWM_SMCTRL2_FRCEN | FLEXPWM_SMCTRL2_INIT_SEL(0) | FLEXPWM_SMCTRL2_FORCE_SEL(6); + FLEXPWM_SMCTRL2_FRCEN | FLEXPWM_SMCTRL2_INIT_SEL(sel) | FLEXPWM_SMCTRL2_FORCE_SEL(6); flexpwm->SM[submodule].CTRL = FLEXPWM_SMCTRL_FULL | FLEXPWM_SMCTRL_HALF | FLEXPWM_SMCTRL_PRSC(prescale) ; // https://github.com/PaulStoffregen/cores/blob/70ba01accd728abe75ebfc8dcd8b3d3a8f3e3f25/teensy4/imxrt.h#L4948 - flexpwm->SM[submodule].OCTRL = 0;//channel_to_invert==2 ? 0 : FLEXPWM_SMOCTRL_POLA | FLEXPWM_SMOCTRL_POLB ; + flexpwm->SM[submodule].OCTRL = 0; //FLEXPWM_SMOCTRL_POLA | FLEXPWM_SMOCTRL_POLB;//channel_to_invert==2 ? 0 : FLEXPWM_SMOCTRL_POLA | FLEXPWM_SMOCTRL_POLB ; + if (!ext_sync) flexpwm->SM[submodule].TCTRL = FLEXPWM_SMTCTRL_OUT_TRIG_EN (0b000010) ; // sync trig out on VAL1 match if master timer flexpwm->SM[submodule].DTCNT0 = dead_time ; // should try this out (deadtime control) flexpwm->SM[submodule].DTCNT1 = dead_time ; // should try this out (deadtime control) - // flexpwm->SM[submodule].TCTRL = FLEXPWM_SMTCTRL_OUT_TRIG_EN (0b000010) ; // sync trig out on VAL1 match. flexpwm->SM[submodule].INIT = -half_cycle; // count from -HALFCYCLE to +HALFCYCLE - flexpwm->SM[submodule].VAL0 = 0 ; + flexpwm->SM[submodule].VAL0 = 0; flexpwm->SM[submodule].VAL1 = half_cycle ; flexpwm->SM[submodule].VAL2 = -mid_pwm ; flexpwm->SM[submodule].VAL3 = +mid_pwm ; - // flexpwm->SM[submodule].VAL4 = -mid_pwm ; - // flexpwm->SM[submodule].VAL5 = +mid_pwm ; + + flexpwm->MCTRL |= FLEXPWM_MCTRL_LDOK (submodule_mask) ; // loading reenabled + flexpwm->MCTRL |= FLEXPWM_MCTRL_RUN (submodule_mask) ; // start it running +} + + +// Helper to set up a FlexPWM submodule. +// can configure sync, prescale +// sets the desired frequency of the PWM +// sets the center-aligned pwm +void setup_pwm_timer_submodule (IMXRT_FLEXPWM_t * flexpwm, int submodule, bool ext_sync, const long frequency) +{ + int submodule_mask = 1 << submodule ; + flexpwm->MCTRL &= ~ FLEXPWM_MCTRL_RUN (submodule_mask) ; // stop it if its already running + flexpwm->MCTRL |= FLEXPWM_MCTRL_CLDOK (submodule_mask) ; // clear load OK + // calculate the counter and prescaler for the desired pwm frequency + uint32_t newdiv = (uint32_t)((float)F_BUS_ACTUAL / frequency + 0.5f); + uint32_t prescale = 0; + //printf(" div=%lu\n", newdiv); + while (newdiv > 65535 && prescale < 7) { + newdiv = newdiv >> 1; + prescale = prescale + 1; + } + if (newdiv > 65535) { + newdiv = 65535; + } else if (newdiv < 2) { + newdiv = 2; + } + + // the halfcycle of the PWM + int half_cycle = int(newdiv/2.0f); + + // if the timer should be externally synced with the master timer + int sel = ext_sync ? 3 : 0; + + // setup the timer + // https://github.com/PaulStoffregen/cores/blob/master/teensy4/imxrt.h + flexpwm->SM[submodule].CTRL2 = FLEXPWM_SMCTRL2_INDEP | FLEXPWM_SMCTRL2_WAITEN | FLEXPWM_SMCTRL2_DBGEN | + FLEXPWM_SMCTRL2_FRCEN | FLEXPWM_SMCTRL2_INIT_SEL(sel) | FLEXPWM_SMCTRL2_FORCE_SEL(6); + flexpwm->SM[submodule].CTRL = FLEXPWM_SMCTRL_FULL | FLEXPWM_SMCTRL_HALF | FLEXPWM_SMCTRL_PRSC(prescale) ; + // https://github.com/PaulStoffregen/cores/blob/70ba01accd728abe75ebfc8dcd8b3d3a8f3e3f25/teensy4/imxrt.h#L4948 + flexpwm->SM[submodule].OCTRL = 0; //FLEXPWM_SMOCTRL_POLA | FLEXPWM_SMOCTRL_POLB;//channel_to_invert==2 ? 0 : FLEXPWM_SMOCTRL_POLA | FLEXPWM_SMOCTRL_POLB ; + if (!ext_sync) flexpwm->SM[submodule].TCTRL = FLEXPWM_SMTCTRL_OUT_TRIG_EN (0b000010) ; // sync trig out on VAL1 match if master timer + flexpwm->SM[submodule].DTCNT0 = 0 ; + flexpwm->SM[submodule].DTCNT1 = 0 ; + flexpwm->SM[submodule].INIT = -half_cycle; // count from -HALFCYCLE to +HALFCYCLE + flexpwm->SM[submodule].VAL0 = 0; + flexpwm->SM[submodule].VAL1 = half_cycle; + flexpwm->SM[submodule].VAL2 = 0 ; + flexpwm->SM[submodule].VAL3 = 0 ; + flexpwm->SM[submodule].VAL2 = 0 ; + flexpwm->SM[submodule].VAL3 = 0 ; + flexpwm->MCTRL |= FLEXPWM_MCTRL_LDOK (submodule_mask) ; // loading reenabled flexpwm->MCTRL |= FLEXPWM_MCTRL_RUN (submodule_mask) ; // start it running } +// staring the PWM on A and B channels of the submodule +void startup_pwm_pair (IMXRT_FLEXPWM_t * flexpwm, int submodule, int channel) +{ + int submodule_mask = 1 << submodule ; + + if(channel==1) flexpwm->OUTEN |= FLEXPWM_OUTEN_PWMA_EN (submodule_mask); // enable A output + else if(channel==2) flexpwm->OUTEN |= FLEXPWM_OUTEN_PWMB_EN (submodule_mask); // enable B output +} + + + // staring the PWM on A and B channels of the submodule void startup_pwm_pair (IMXRT_FLEXPWM_t * flexpwm, int submodule) { @@ -207,20 +425,15 @@ void startup_pwm_pair (IMXRT_FLEXPWM_t * flexpwm, int submodule) // PWM setting on the high and low pair of the PWM channels void write_pwm_pair(IMXRT_FLEXPWM_t * flexpwm, int submodule, float duty){ - + int half_cycle = int(flexpwm->SM[submodule].VAL1); int mid_pwm = int((half_cycle)/2.0f); int count_pwm = int(mid_pwm*(duty*2-1)) + mid_pwm; - flexpwm->SM[submodule].VAL2 = count_pwm; // A on - flexpwm->SM[submodule].VAL3 = -count_pwm ; // A off - // flexpwm->SM[submodule].VAL4 = - count_pwm ; // B off (assuming B inverted) - // flexpwm->SM[submodule].VAL5 = + count_pwm ; // B on + flexpwm->SM[submodule].VAL2 = -count_pwm; // A on + flexpwm->SM[submodule].VAL3 = count_pwm ; // A off flexpwm->MCTRL |= FLEXPWM_MCTRL_LDOK (1<additional_params; _UNUSED(phase_state); - write_pwm_pair (((Teensy4DriverParams*)params)->flextimers[0], ((Teensy4DriverParams*)params)->submodules[0], dc_a); - write_pwm_pair (((Teensy4DriverParams*)params)->flextimers[1], ((Teensy4DriverParams*)params)->submodules[1], dc_b); - write_pwm_pair (((Teensy4DriverParams*)params)->flextimers[2], ((Teensy4DriverParams*)params)->submodules[2], dc_c); + write_pwm_pair (p->flextimers[0], p->submodules[0], dc_a); + write_pwm_pair (p->flextimers[1], p->submodules[1], dc_b); + write_pwm_pair (p->flextimers[2], p->submodules[2], dc_c); +} + +void write_pwm_on_pin(IMXRT_FLEXPWM_t *p, unsigned int submodule, uint8_t channel, float duty) +{ + uint16_t mask = 1 << submodule; + uint32_t half_cycle = p->SM[submodule].VAL1; + int mid_pwm = int((half_cycle)/2.0f); + int cval = int(mid_pwm*(duty*2-1)) + mid_pwm; + + //printf("flexpwmWrite, p=%08lX, sm=%d, ch=%c, cval=%ld\n", + //(uint32_t)p, submodule, channel == 0 ? 'X' : (channel == 1 ? 'A' : 'B'), cval); + p->MCTRL |= FLEXPWM_MCTRL_CLDOK(mask); + switch (channel) { + case 0: // X + p->SM[submodule].VAL0 = half_cycle - cval; + p->OUTEN |= FLEXPWM_OUTEN_PWMX_EN(mask); + //printf(" write channel X\n"); + break; + case 1: // A + p->SM[submodule].VAL2 = -cval; + p->SM[submodule].VAL3 = cval; + p->OUTEN |= FLEXPWM_OUTEN_PWMA_EN(mask); + //printf(" write channel A\n"); + break; + case 2: // B + p->SM[submodule].VAL4 = -cval; + p->SM[submodule].VAL5 = cval; + p->OUTEN |= FLEXPWM_OUTEN_PWMB_EN(mask); + //printf(" write channel B\n"); + } + p->MCTRL |= FLEXPWM_MCTRL_LDOK(mask); } -#endif \ No newline at end of file +#ifdef SIMPLEFOC_TEENSY4_FORCE_CENTER_ALIGNED_3PWM + +// function setting the high pwm frequency to the supplied pins +// - BLDC motor - 3PWM setting +// - hardware speciffic +// in generic case dont do anything + void* _configureCenterAligned3PMW(long pwm_frequency,const int pinA, const int pinB, const int pinC) { + + if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + + IMXRT_FLEXPWM_t *flexpwmA,*flexpwmB,*flexpwmC; + int submoduleA,submoduleB,submoduleC; + flexpwmA = get_flexpwm(pinA); + submoduleA = get_submodule(pinA); + flexpwmB = get_flexpwm(pinB); + submoduleB = get_submodule(pinB); + flexpwmC = get_flexpwm(pinC); + submoduleC = get_submodule(pinC); + int channelA = get_channel(pinA); + int channelB = get_channel(pinB); + int channelC = get_channel(pinC); + + + // if pins belong to the flextimers and they only use submodules A and B + // we can configure the center-aligned pwm + if((flexpwmA != nullptr) && (flexpwmB != nullptr) && (flexpwmC != nullptr) && (channelA > 0) && (channelB > 0) && (channelC > 0) ){ + #ifdef SIMPLEFOC_TEENSY_DEBUG + SIMPLEFOC_DEBUG("TEENSY-DRV: All pins on Flexpwm A or B channels - Configuring center-aligned pwm!"); + #endif + + // Configure FlexPWM units + setup_pwm_timer_submodule (flexpwmA, submoduleA, true, pwm_frequency) ; // others externally synced + setup_pwm_timer_submodule (flexpwmB, submoduleB, true, pwm_frequency) ; // others externally synced + setup_pwm_timer_submodule (flexpwmC, submoduleC, false, pwm_frequency) ; // this is the master, internally synced + delayMicroseconds (100) ; + + + #ifdef SIMPLEFOC_TEENSY_DEBUG + char buff[100]; + sprintf(buff, "TEENSY-CS: Syncing to Master FlexPWM: %d, Submodule: %d", flexpwm_to_index(flexpwmC), submoduleC); + SIMPLEFOC_DEBUG(buff); + sprintf(buff, "TEENSY-CS: Slave timers FlexPWM: %d, Submodule: %d and FlexPWM: %d, Submodule: %d", flexpwm_to_index(flexpwmA), submoduleA, flexpwm_to_index(flexpwmB), submoduleB); + SIMPLEFOC_DEBUG(buff); + #endif + + // // turn on XBAR1 clock for all but stop mode + xbar_init() ; + + // // Connect trigger to synchronize all timers + xbar_connect (flexpwm_submodule_to_trig(flexpwmC, submoduleC), flexpwm_submodule_to_ext_sync(flexpwmA, submoduleA)) ; + xbar_connect (flexpwm_submodule_to_trig(flexpwmC, submoduleC), flexpwm_submodule_to_ext_sync(flexpwmB, submoduleB)) ; + + TeensyDriverParams* params = new TeensyDriverParams { + .pins = { pinA, pinB, pinC }, + .pwm_frequency = pwm_frequency, + .additional_params = new Teensy4DriverParams { + .flextimers = { flexpwmA, flexpwmB, flexpwmC}, + .submodules = { submoduleA, submoduleB, submoduleC}, + .channels = {channelA, channelB, channelC}, + } + }; + + startup_pwm_pair (flexpwmA, submoduleA, channelA) ; + startup_pwm_pair (flexpwmB, submoduleB, channelB) ; + startup_pwm_pair (flexpwmC, submoduleC, channelC) ; + + // // config the pins 2/3/6/9/8/7 as their FLEXPWM alternates. + *portConfigRegister(pinA) = pwm_pin_info[pinA].muxval ; + *portConfigRegister(pinB) = pwm_pin_info[pinB].muxval ; + *portConfigRegister(pinC) = pwm_pin_info[pinC].muxval ; + + return params; + }else{ + #ifdef SIMPLEFOC_TEENSY_DEBUG + SIMPLEFOC_DEBUG("TEENSY-DRV: Not all pins on Flexpwm A and B channels - cannot configure center-aligned pwm!"); + #endif + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + +} + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 6PWM setting +// - hardware specific +void _writeCenterAligned3PMW(float dc_a, float dc_b, float dc_c, void* params){ + Teensy4DriverParams* p = (Teensy4DriverParams*)((TeensyDriverParams*)params)->additional_params; + write_pwm_on_pin (p->flextimers[0], p->submodules[0], p->channels[0], dc_a); + write_pwm_on_pin (p->flextimers[1], p->submodules[1], p->channels[1], dc_b); + write_pwm_on_pin (p->flextimers[2], p->submodules[2], p->channels[2], dc_c); +} + +#endif + +#endif diff --git a/src/drivers/hardware_specific/teensy/teensy4_mcu.h b/src/drivers/hardware_specific/teensy/teensy4_mcu.h index 5e384623..aed64826 100644 --- a/src/drivers/hardware_specific/teensy/teensy4_mcu.h +++ b/src/drivers/hardware_specific/teensy/teensy4_mcu.h @@ -1,3 +1,6 @@ +#ifndef TEENSY4_MCU_DRIVER_H +#define TEENSY4_MCU_DRIVER_H + #include "teensy_mcu.h" // if defined @@ -9,7 +12,7 @@ typedef struct Teensy4DriverParams { IMXRT_FLEXPWM_t* flextimers[3] = {NULL}; int submodules[3]; - long pwm_frequency; + int channels[6]; float dead_zone; } Teensy4DriverParams; @@ -105,6 +108,18 @@ const struct pwm_pin_info_struct pwm_pin_info[] = { #endif }; +// function finding the flexpwm instance given the submodule +int flexpwm_to_index(IMXRT_FLEXPWM_t* flexpwm); +// find the trigger TRG0 for the given timer and submodule +int flexpwm_submodule_to_trig(IMXRT_FLEXPWM_t* flexpwm, int submodule); +// find the external trigger for the given timer and submodule +int flexpwm_submodule_to_ext_sync(IMXRT_FLEXPWM_t* flexpwm, int submodule); +// function to connecting the triggers +void xbar_connect(unsigned int input, unsigned int output); +// function to initialize the xbar +void xbar_init(); + #endif +#endif #endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/teensy/teensy4_mcu1.cpp.new b/src/drivers/hardware_specific/teensy/teensy4_mcu1.cpp.new new file mode 100644 index 00000000..5dcac90d --- /dev/null +++ b/src/drivers/hardware_specific/teensy/teensy4_mcu1.cpp.new @@ -0,0 +1,543 @@ +#include "teensy_mcu.h" +#include "teensy4_mcu.h" +#include "../../../communication/SimpleFOCDebug.h" + +// if defined +// - Teensy 4.0 +// - Teensy 4.1 +#if defined(__arm__) && defined(CORE_TEENSY) && ( defined(__IMXRT1062__) || defined(ARDUINO_TEENSY40) || defined(ARDUINO_TEENSY41) || defined(ARDUINO_TEENSY_MICROMOD) ) + + +#pragma message("") +#pragma message("SimpleFOC: compiling for Teensy 4.x") +#pragma message("") + + + +// function finding the TRIG event given the flexpwm timer and the submodule +// returning -1 if the submodule is not valid or no trigger is available +// allowing flexpwm1-4 and submodule 0-3 +// +// the flags are defined in the imxrt.h file +// https://github.com/PaulStoffregen/cores/blob/dd6aa8419ee173a0a6593eab669fbff54ed85f48/teensy4/imxrt.h#L9662-L9693 +int flexpwm_submodule_to_trig(IMXRT_FLEXPWM_t* flexpwm, int submodule){ + if(submodule <0 && submodule > 3) return -1; + if(flexpwm == &IMXRT_FLEXPWM1){ + return XBARA1_IN_FLEXPWM1_PWM1_OUT_TRIG0 + submodule; + }else if(flexpwm == &IMXRT_FLEXPWM2){ + return XBARA1_IN_FLEXPWM2_PWM1_OUT_TRIG0 + submodule; + }else if(flexpwm == &IMXRT_FLEXPWM3){ + return XBARA1_IN_FLEXPWM3_PWM1_OUT_TRIG0 + submodule; + }else if(flexpwm == &IMXRT_FLEXPWM4){ + return XBARA1_IN_FLEXPWM4_PWM1_OUT_TRIG0 + submodule; + } + return -1; +} + +// function finding the EXT_SYNC event given the flexpwm timer and the submodule +// returning -1 if the submodule is not valid or no trigger is available +// allowing flexpwm1-4 and submodule 0-3 +// +// the flags are defined in the imxrt.h file +// https://github.com/PaulStoffregen/cores/blob/dd6aa8419ee173a0a6593eab669fbff54ed85f48/teensy4/imxrt.h#L9757 +int flexpwm_submodule_to_ext_sync(IMXRT_FLEXPWM_t* flexpwm, int submodule){ + if(submodule <0 && submodule > 3) return -1; + if(flexpwm == &IMXRT_FLEXPWM1){ + return XBARA1_OUT_FLEXPWM1_PWM0_EXT_SYNC + submodule; + }else if(flexpwm == &IMXRT_FLEXPWM2){ + return XBARA1_OUT_FLEXPWM2_PWM0_EXT_SYNC + submodule; + }else if(flexpwm == &IMXRT_FLEXPWM3){ + return XBARA1_OUT_FLEXPWM3_EXT_SYNC0 + submodule; // TODO verify why they are not called PWM0_EXT_SYNC but EXT_SYNC0 + }else if(flexpwm == &IMXRT_FLEXPWM4){ + return XBARA1_OUT_FLEXPWM4_EXT_SYNC0 + submodule; // TODO verify why they are not called PWM0_EXT_SYNC but EXT_SYNC0 + } + return -1; +} + +// The i.MXRT1062 uses one config register per two XBAR outputs, so a helper +// function to make code more readable. +void xbar_connect(unsigned int input, unsigned int output) +{ + if (input >= 88) return; + if (output >= 132) return; + volatile uint16_t *xbar = &XBARA1_SEL0 + (output / 2); + uint16_t val = *xbar; + if (!(output & 1)) { + val = (val & 0xFF00) | input; + } else { + val = (val & 0x00FF) | (input << 8); + } + *xbar = val; +} + +void xbar_init() { + CCM_CCGR2 |= CCM_CCGR2_XBAR1(CCM_CCGR_ON); //turn clock on for xbara1 +} + +// half_cycle of the PWM variable +int half_cycle = 0; + +// function which finds the flexpwm instance for a pin +// if it does not belong to the flexpwm timer it returns a nullpointer +IMXRT_FLEXPWM_t* get_flexpwm(uint8_t pin){ + + const struct pwm_pin_info_struct *info; + if (pin >= CORE_NUM_DIGITAL) { +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: ERR: Pin: %d not Flextimer pin!", pin); + SIMPLEFOC_DEBUG(s); +#endif + return nullptr; + } + info = pwm_pin_info + pin; + // FlexPWM pin + IMXRT_FLEXPWM_t *flexpwm; + switch ((info->module >> 4) & 3) { + case 0: flexpwm = &IMXRT_FLEXPWM1; break; + case 1: flexpwm = &IMXRT_FLEXPWM2; break; + case 2: flexpwm = &IMXRT_FLEXPWM3; break; + default: flexpwm = &IMXRT_FLEXPWM4; + } + if(flexpwm != nullptr){ +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: Pin: %d on Flextimer %d.", pin, ((info->module >> 4) & 3) + 1); + SIMPLEFOC_DEBUG(s); +#endif + return flexpwm; + } + return nullptr; +} + + +// function which finds the timer submodule for a pin +// if it does not belong to the submodule it returns a -1 +int get_submodule(uint8_t pin){ + + const struct pwm_pin_info_struct *info; + if (pin >= CORE_NUM_DIGITAL){ +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: ERR: Pin: %d not Flextimer pin!", pin); + SIMPLEFOC_DEBUG(s); +#endif + return -1; + } + + info = pwm_pin_info + pin; + int sm1 = info->module&0x3; + + if (sm1 >= 0 && sm1 < 4) { +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: Pin %d on submodule %d.", pin, sm1); + SIMPLEFOC_DEBUG(s); +#endif + return sm1; + } else { +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[50]; + sprintf (s, "TEENSY-DRV: ERR: Pin: %d not in submodule!", pin); + SIMPLEFOC_DEBUG(s); +#endif + return -1; + } +} + +// function which finds the flexpwm instance for a pair of pins +// if they do not belong to the same timer it returns a nullpointer +IMXRT_FLEXPWM_t* get_flexpwm(uint8_t pin, uint8_t pin1){ + + const struct pwm_pin_info_struct *info; + if (pin >= CORE_NUM_DIGITAL || pin1 >= CORE_NUM_DIGITAL) { +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: ERR: Pins: %d, %d not Flextimer pins!", pin, pin1); + SIMPLEFOC_DEBUG(s); +#endif + return nullptr; + } + info = pwm_pin_info + pin; + // FlexPWM pin + IMXRT_FLEXPWM_t *flexpwm1,*flexpwm2; + switch ((info->module >> 4) & 3) { + case 0: flexpwm1 = &IMXRT_FLEXPWM1; break; + case 1: flexpwm1 = &IMXRT_FLEXPWM2; break; + case 2: flexpwm1 = &IMXRT_FLEXPWM3; break; + default: flexpwm1 = &IMXRT_FLEXPWM4; + } + + info = pwm_pin_info + pin1; + switch ((info->module >> 4) & 3) { + case 0: flexpwm2 = &IMXRT_FLEXPWM1; break; + case 1: flexpwm2 = &IMXRT_FLEXPWM2; break; + case 2: flexpwm2 = &IMXRT_FLEXPWM3; break; + default: flexpwm2 = &IMXRT_FLEXPWM4; + } + if(flexpwm1 == flexpwm2){ + char s[60]; + sprintf (s, "TEENSY-DRV: Pins: %d, %d on Flextimer %d.", pin, pin1, ((info->module >> 4) & 3) + 1); + SIMPLEFOC_DEBUG(s); + return flexpwm1; + } else { +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: ERR: Pins: %d, %d not in same Flextimer!", pin, pin1); + SIMPLEFOC_DEBUG(s); +#endif + return nullptr; + } +} + + +// function which finds the timer submodule for a pair of pins +// if they do not belong to the same submodule it returns a -1 +int get_submodule(uint8_t pin, uint8_t pin1){ + + const struct pwm_pin_info_struct *info; + if (pin >= CORE_NUM_DIGITAL || pin1 >= CORE_NUM_DIGITAL){ +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: ERR: Pins: %d, %d not Flextimer pins!", pin, pin1); + SIMPLEFOC_DEBUG(s); +#endif + return -1; + } + + info = pwm_pin_info + pin; + int sm1 = info->module&0x3; + info = pwm_pin_info + pin1; + int sm2 = info->module&0x3; + + if (sm1 == sm2) { + char s[60]; + sprintf (s, "TEENSY-DRV: Pins: %d, %d on submodule %d.", pin, pin1, sm1); + SIMPLEFOC_DEBUG(s); + return sm1; + } else { +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[50]; + sprintf (s, "TEENSY-DRV: ERR: Pins: %d, %d not in same submodule!", pin, pin1); + SIMPLEFOC_DEBUG(s); +#endif + return -1; + } +} + + +// function which finds the timer submodule for a pair of pins +// if they do not belong to the same submodule it returns a -1 +int get_inverted_channel(uint8_t pin, uint8_t pin1){ + + const struct pwm_pin_info_struct *info; + if (pin >= CORE_NUM_DIGITAL || pin1 >= CORE_NUM_DIGITAL){ +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: ERR: Pins: %d, %d not Flextimer pins!", pin, pin1); + SIMPLEFOC_DEBUG(s); +#endif + return -1; + } + + info = pwm_pin_info + pin; + int ch1 = info->channel; + info = pwm_pin_info + pin1; + int ch2 = info->channel; + + if (ch1 != 1) { +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: ERR: Pin: %d on channel %s - only A supported", pin1, ch1==2 ? "B" : "X"); + SIMPLEFOC_DEBUG(s); +#endif + return -1; + } else if (ch2 != 2) { +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: ERR: Inverted pin: %d on channel %s - only B supported", pin1, ch2==1 ? "A" : "X"); + SIMPLEFOC_DEBUG(s); +#endif + return -1; + } else { +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: Pin: %d on channel B inverted.", pin1); + SIMPLEFOC_DEBUG(s); +#endif +return ch2; + } +} + +// Helper to set up A/B pair on a FlexPWM submodule. +// can configure sync, prescale and B inversion. +// sets the desired frequency of the PWM +// sets the center-aligned pwm +void setup_pwm_pair (IMXRT_FLEXPWM_t * flexpwm, int submodule, bool ext_sync, const long frequency, float dead_zone ) +{ + int submodule_mask = 1 << submodule ; + flexpwm->MCTRL &= ~ FLEXPWM_MCTRL_RUN (submodule_mask) ; // stop it if its already running + flexpwm->MCTRL |= FLEXPWM_MCTRL_CLDOK (submodule_mask) ; // clear load OK + + + // calculate the counter and prescaler for the desired pwm frequency + uint32_t newdiv = (uint32_t)((float)F_BUS_ACTUAL / frequency + 0.5f); + uint32_t prescale = 0; + //printf(" div=%lu\n", newdiv); + while (newdiv > 65535 && prescale < 7) { + newdiv = newdiv >> 1; + prescale = prescale + 1; + } + if (newdiv > 65535) { + newdiv = 65535; + } else if (newdiv < 2) { + newdiv = 2; + } + + // the halfcycle of the PWM + half_cycle = int(newdiv/2.0f); + int dead_time = int(dead_zone*half_cycle); //default dead-time - 2% + int mid_pwm = int((half_cycle)/2.0f); + + // if the timer should be externally synced with the master timer + int sel = ext_sync ? 3 : 0; + + // setup the timer + // https://github.com/PaulStoffregen/cores/blob/master/teensy4/imxrt.h + flexpwm->SM[submodule].CTRL2 = FLEXPWM_SMCTRL2_WAITEN | FLEXPWM_SMCTRL2_DBGEN | + FLEXPWM_SMCTRL2_FRCEN | FLEXPWM_SMCTRL2_INIT_SEL(sel) | FLEXPWM_SMCTRL2_FORCE_SEL(6); + flexpwm->SM[submodule].CTRL = FLEXPWM_SMCTRL_FULL | FLEXPWM_SMCTRL_HALF | FLEXPWM_SMCTRL_PRSC(prescale) ; + // https://github.com/PaulStoffregen/cores/blob/70ba01accd728abe75ebfc8dcd8b3d3a8f3e3f25/teensy4/imxrt.h#L4948 + flexpwm->SM[submodule].OCTRL = 0; //FLEXPWM_SMOCTRL_POLA | FLEXPWM_SMOCTRL_POLB;//channel_to_invert==2 ? 0 : FLEXPWM_SMOCTRL_POLA | FLEXPWM_SMOCTRL_POLB ; + if (!ext_sync) flexpwm->SM[submodule].TCTRL = FLEXPWM_SMTCTRL_OUT_TRIG_EN (0b000010) ; // sync trig out on VAL1 match if master timer + flexpwm->SM[submodule].DTCNT0 = dead_time ; // should try this out (deadtime control) + flexpwm->SM[submodule].DTCNT1 = dead_time ; // should try this out (deadtime control) + flexpwm->SM[submodule].INIT = -half_cycle; // count from -HALFCYCLE to +HALFCYCLE + flexpwm->SM[submodule].VAL0 = 0; + flexpwm->SM[submodule].VAL1 = half_cycle ; + flexpwm->SM[submodule].VAL2 = -mid_pwm ; + flexpwm->SM[submodule].VAL3 = +mid_pwm ; + + flexpwm->MCTRL |= FLEXPWM_MCTRL_LDOK (submodule_mask) ; // loading reenabled + flexpwm->MCTRL |= FLEXPWM_MCTRL_RUN (submodule_mask) ; // start it running +} + + +// staring the PWM on A and B channels of the submodule +void startup_pwm_pair (IMXRT_FLEXPWM_t * flexpwm, int submodule) +{ + int submodule_mask = 1 << submodule ; + + flexpwm->OUTEN |= FLEXPWM_OUTEN_PWMA_EN (submodule_mask); // enable A output + flexpwm->OUTEN |= FLEXPWM_OUTEN_PWMB_EN (submodule_mask); // enable B output +} + + + +// PWM setting on the high and low pair of the PWM channels +void write_pwm_pair(IMXRT_FLEXPWM_t * flexpwm, int submodule, float duty){ + int mid_pwm = int((half_cycle)/2.0f); + int count_pwm = int(mid_pwm*(duty*2-1)) + mid_pwm; + + flexpwm->SM[submodule].VAL2 = -count_pwm; // A on + flexpwm->SM[submodule].VAL3 = count_pwm ; // A off + flexpwm->MCTRL |= FLEXPWM_MCTRL_LDOK (1<SM[submodule].VAL1; + int count_pwm = int(count*duty); + + SIMPLEFOC_DEBUG("VAL0: ",flexpwm->SM[submodule].VAL0); + SIMPLEFOC_DEBUG("VAL1: ",flexpwm->SM[submodule].VAL1); + SIMPLEFOC_DEBUG("count: ",count_pwm); + + // flexpwm->SM[submodule].VAL1 = 0; // A on + flexpwm->SM[submodule].VAL2 = count_pwm ; // A off + flexpwm->SM[submodule].VAL3 = count_pwm; // A on + flexpwm->SM[submodule].VAL4 = count_pwm ; // A off + flexpwm->SM[submodule].VAL5 = count_pwm ; // A off + flexpwm->MCTRL |= FLEXPWM_MCTRL_LDOK (1<SM[submodule].VAL1; + uint32_t cval = ((uint32_t)val * (modulo + 1)) >> analog_write_res; + if (cval > modulo) cval = modulo; // TODO: is this check correct? + + //printf("flexpwmWrite, p=%08lX, sm=%d, ch=%c, cval=%ld\n", + //(uint32_t)p, submodule, channel == 0 ? 'X' : (channel == 1 ? 'A' : 'B'), cval); + p->MCTRL |= FLEXPWM_MCTRL_CLDOK(mask); + switch (channel) { + case 0: // X + p->SM[submodule].VAL0 = modulo - cval; + p->OUTEN |= FLEXPWM_OUTEN_PWMX_EN(mask); + //printf(" write channel X\n"); + break; + case 1: // A + p->SM[submodule].VAL3 = cval; + p->OUTEN |= FLEXPWM_OUTEN_PWMA_EN(mask); + //printf(" write channel A\n"); + break; + case 2: // B + p->SM[submodule].VAL5 = cval; + p->OUTEN |= FLEXPWM_OUTEN_PWMB_EN(mask); + //printf(" write channel B\n"); + } + p->MCTRL |= FLEXPWM_MCTRL_LDOK(mask); +} + + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 6PWM setting +// - hardware specific +void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) { + if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + + IMXRT_FLEXPWM_t *flexpwmA,*flexpwmB,*flexpwmC; + int submoduleA,submoduleB,submoduleC; + int inverted_channelA,inverted_channelB,inverted_channelC; + flexpwmA = get_flexpwm(pinA_h,pinA_l); + submoduleA = get_submodule(pinA_h,pinA_l); + inverted_channelA = get_inverted_channel(pinA_h,pinA_l); + flexpwmB = get_flexpwm(pinB_h,pinB_l); + submoduleB = get_submodule(pinB_h,pinB_l); + inverted_channelB = get_inverted_channel(pinB_h,pinB_l); + flexpwmC = get_flexpwm(pinC_h,pinC_l); + submoduleC = get_submodule(pinC_h,pinC_l); + inverted_channelC = get_inverted_channel(pinC_h,pinC_l); + + if((flexpwmA == nullptr) || (flexpwmB == nullptr) || (flexpwmC == nullptr) ){ +#ifdef SIMPLEFOC_TEENSY_DEBUG + SIMPLEFOC_DEBUG("TEENSY-DRV: ERR: Flextimer problem - failed driver config!"); +#endif + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + if((submoduleA < 0) || (submoduleB < 0) || (submoduleC < 0) ){ +#ifdef SIMPLEFOC_TEENSY_DEBUG + SIMPLEFOC_DEBUG("TEENSY-DRV: ERR: Flextimer submodule problem - failed driver config!"); +#endif + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + if((inverted_channelA < 0) || (inverted_channelB < 0) || (inverted_channelC < 0) ){ +#ifdef SIMPLEFOC_TEENSY_DEBUG + SIMPLEFOC_DEBUG("TEENSY-DRV: ERR: Flextimer channel problem - failed driver config!"); +#endif + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + + + Teensy4DriverParams* params = new Teensy4DriverParams { + .pins = { pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l }, + .flextimers = { flexpwmA, flexpwmB, flexpwmC}, + .submodules = { submoduleA, submoduleB, submoduleC}, + .pwm_frequency = pwm_frequency, + .dead_zone = dead_zone + }; + + + // Configure FlexPWM units, each driving A/B pair, B inverted. + // full speed about 80kHz, prescale 2 (div by 4) gives 20kHz + setup_pwm_pair (flexpwmA, submoduleA, true, pwm_frequency, dead_zone) ; // this is the master, internally synced + setup_pwm_pair (flexpwmB, submoduleB, true, pwm_frequency, dead_zone) ; // others externally synced + setup_pwm_pair (flexpwmC, submoduleC, false, pwm_frequency, dead_zone) ; + delayMicroseconds (100) ; + + // turn on XBAR1 clock for all but stop mode + xbar_init() ; + + // // Connect trigger to synchronize all timers + xbar_connect (flexpwm_submodule_to_trig(flexpwmC, submoduleC), flexpwm_submodule_to_ext_sync(flexpwmA, submoduleA)) ; + xbar_connect (flexpwm_submodule_to_trig(flexpwmC, submoduleC), flexpwm_submodule_to_ext_sync(flexpwmB, submoduleB)) ; + + startup_pwm_pair (flexpwmA, submoduleA) ; + startup_pwm_pair (flexpwmB, submoduleB) ; + startup_pwm_pair (flexpwmC, submoduleC) ; + + + delayMicroseconds(50) ; + // config the pins 2/3/6/9/8/7 as their FLEXPWM alternates. + *portConfigRegister(pinA_h) = pwm_pin_info[pinA_h].muxval ; + *portConfigRegister(pinA_l) = pwm_pin_info[pinA_l].muxval ; + *portConfigRegister(pinB_h) = pwm_pin_info[pinB_h].muxval ; + *portConfigRegister(pinB_l) = pwm_pin_info[pinB_l].muxval ; + *portConfigRegister(pinC_h) = pwm_pin_info[pinC_h].muxval ; + *portConfigRegister(pinC_l) = pwm_pin_info[pinC_l].muxval ; + + return params; +} + + + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 6PWM setting +// - hardware specific +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState *phase_state, void* params){ + _UNUSED(phase_state); + write_pwm_pair (((Teensy4DriverParams*)params)->flextimers[0], ((Teensy4DriverParams*)params)->submodules[0], dc_a); + write_pwm_pair (((Teensy4DriverParams*)params)->flextimers[1], ((Teensy4DriverParams*)params)->submodules[1], dc_b); + write_pwm_pair (((Teensy4DriverParams*)params)->flextimers[2], ((Teensy4DriverParams*)params)->submodules[2], dc_c); +} + + +// function setting the high pwm frequency to the supplied pins +// - BLDC motor - 3PWM setting +// - hardware speciffic +// in generic case dont do anything + void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { + + if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + + IMXRT_FLEXPWM_t *flexpwmA,*flexpwmB,*flexpwmC; + int submoduleA,submoduleB,submoduleC; + int inverted_channelA,inverted_channelB,inverted_channelC; + flexpwmA = get_flexpwm(pinA); + submoduleA = get_submodule(pinA); + flexpwmB = get_flexpwm(pinB); + submoduleB = get_submodule(pinB); + flexpwmC = get_flexpwm(pinC); + submoduleC = get_submodule(pinC); + + Teensy4DriverParams* params = new Teensy4DriverParams { + .pins = { pinA, pinB, pinC }, + .flextimers = { flexpwmA, flexpwmB, flexpwmC}, + .submodules = { submoduleA, submoduleB, submoduleC}, + .pwm_frequency = pwm_frequency, + }; + + startup_pwm_pair (flexpwmA, submoduleA) ; + startup_pwm_pair (flexpwmB, submoduleB) ; + startup_pwm_pair (flexpwmC, submoduleC) ; + + // analogWriteFrequency(pinA, pwm_frequency); + // analogWriteFrequency(pinB, pwm_frequency); + // analogWriteFrequency(pinC, pwm_frequency); + // analogWrite(pinA, 0); + // analogWrite(pinB, 0); + // analogWrite(pinC, 0); + + // // config the pins 2/3/6/9/8/7 as their FLEXPWM alternates. + // *portConfigRegister(pinA) = pwm_pin_info[pinA].muxval ; + // *portConfigRegister(pinB) = pwm_pin_info[pinB].muxval ; + // *portConfigRegister(pinC) = pwm_pin_info[pinC].muxval ; + + return params; +} + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 6PWM setting +// - hardware specific +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params){ + write_pwm_on_pin (((Teensy4DriverParams*)params)->flextimers[0], ((Teensy4DriverParams*)params)->submodules[0], dc_a); + write_pwm_on_pin (((Teensy4DriverParams*)params)->flextimers[1], ((Teensy4DriverParams*)params)->submodules[1], dc_b); + write_pwm_on_pin (((Teensy4DriverParams*)params)->flextimers[2], ((Teensy4DriverParams*)params)->submodules[2], dc_c); +} + + +#endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/teensy/teensy_mcu.cpp b/src/drivers/hardware_specific/teensy/teensy_mcu.cpp index dcfa3e15..196f07fd 100644 --- a/src/drivers/hardware_specific/teensy/teensy_mcu.cpp +++ b/src/drivers/hardware_specific/teensy/teensy_mcu.cpp @@ -2,6 +2,8 @@ #if defined(__arm__) && defined(CORE_TEENSY) +#include "../../../communication/SimpleFOCDebug.h" + // configure High PWM frequency void _setHighFrequency(const long freq, const int pin){ analogWrite(pin, 0); @@ -11,14 +13,15 @@ void _setHighFrequency(const long freq, const int pin){ // function setting the high pwm frequency to the supplied pins // - Stepper motor - 2PWM setting -// - hardware speciffic +// - hardware specific void* _configure1PWM(long pwm_frequency, const int pinA) { if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max _setHighFrequency(pwm_frequency, pinA); - GenericDriverParams* params = new GenericDriverParams { + TeensyDriverParams* params = new TeensyDriverParams { .pins = { pinA }, - .pwm_frequency = pwm_frequency + .pwm_frequency = pwm_frequency, + .additional_params = nullptr }; return params; } @@ -26,38 +29,54 @@ void* _configure1PWM(long pwm_frequency, const int pinA) { // function setting the high pwm frequency to the supplied pins // - Stepper motor - 2PWM setting -// - hardware speciffic +// - hardware specific void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max _setHighFrequency(pwm_frequency, pinA); _setHighFrequency(pwm_frequency, pinB); - GenericDriverParams* params = new GenericDriverParams { + TeensyDriverParams* params = new TeensyDriverParams { .pins = { pinA, pinB }, - .pwm_frequency = pwm_frequency + .pwm_frequency = pwm_frequency, + .additional_params = nullptr }; return params; } +// inital weak implementation of the center aligned 3pwm configuration +// teensy 4 and 3 have center aligned pwm +__attribute__((weak)) void* _configureCenterAligned3PMW(long pwm_frequency, const int pinA, const int pinB, const int pinC) { + return SIMPLEFOC_DRIVER_INIT_FAILED; +} + // function setting the high pwm frequency to the supplied pins // - BLDC motor - 3PWM setting -// - hardware speciffic +// - hardware specific void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max - _setHighFrequency(pwm_frequency, pinA); - _setHighFrequency(pwm_frequency, pinB); - _setHighFrequency(pwm_frequency, pinC); - GenericDriverParams* params = new GenericDriverParams { - .pins = { pinA, pinB, pinC }, - .pwm_frequency = pwm_frequency - }; + + // try configuring center aligned pwm + void* p = _configureCenterAligned3PMW(pwm_frequency, pinA, pinB, pinC); + if(p != SIMPLEFOC_DRIVER_INIT_FAILED){ + return p; // if center aligned pwm is available return the params + }else{ // if center aligned pwm is not available use fast pwm + SIMPLEFOC_DEBUG("TEENSY-DRV: Configuring 3PWM with fast pwm. Please consider using center aligned pwm for better performance!"); + _setHighFrequency(pwm_frequency, pinA); + _setHighFrequency(pwm_frequency, pinB); + _setHighFrequency(pwm_frequency, pinC); + TeensyDriverParams* params = new TeensyDriverParams { + .pins = { pinA, pinB, pinC }, + .pwm_frequency = pwm_frequency, + .additional_params = nullptr + }; return params; + } } // function setting the high pwm frequency to the supplied pins // - Stepper motor - 4PWM setting -// - hardware speciffic +// - hardware specific void* _configure4PWM(long pwm_frequency, const int pinA, const int pinB, const int pinC, const int pinD) { if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max @@ -65,9 +84,10 @@ void* _configure4PWM(long pwm_frequency, const int pinA, const int pinB, const i _setHighFrequency(pwm_frequency, pinB); _setHighFrequency(pwm_frequency, pinC); _setHighFrequency(pwm_frequency, pinD); - GenericDriverParams* params = new GenericDriverParams { + TeensyDriverParams* params = new TeensyDriverParams { .pins = { pinA, pinB, pinC, pinD }, - .pwm_frequency = pwm_frequency + .pwm_frequency = pwm_frequency, + .additional_params = nullptr }; return params; } @@ -75,42 +95,57 @@ void* _configure4PWM(long pwm_frequency, const int pinA, const int pinB, const i // function setting the pwm duty cycle to the hardware // - Stepper motor - 2PWM setting -// - hardware speciffic +// - hardware specific void _writeDutyCycle1PWM(float dc_a, void* params) { // transform duty cycle from [0,1] to [0,255] - analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a); + analogWrite(((TeensyDriverParams*)params)->pins[0], 255.0f*dc_a); } // function setting the pwm duty cycle to the hardware // - Stepper motor - 2PWM setting -// - hardware speciffic +// - hardware specific void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params) { // transform duty cycle from [0,1] to [0,255] - analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a); - analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_b); + analogWrite(((TeensyDriverParams*)params)->pins[0], 255.0f*dc_a); + analogWrite(((TeensyDriverParams*)params)->pins[1], 255.0f*dc_b); +} + +// inital weak implementation of the center aligned 3pwm configuration +// teensy 4 and 3 have center aligned pwm implementation of this function +__attribute__((weak)) void _writeCenterAligned3PMW(float dc_a, float dc_b, float dc_c, void* params){ + _UNUSED(dc_a); + _UNUSED(dc_b); + _UNUSED(dc_c); + _UNUSED(params); } // function setting the pwm duty cycle to the hardware // - BLDC motor - 3PWM setting -// - hardware speciffic +// - hardware specific void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params){ - // transform duty cycle from [0,1] to [0,255] - analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a); - analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_b); - analogWrite(((GenericDriverParams*)params)->pins[2], 255.0f*dc_c); + + TeensyDriverParams* p = (TeensyDriverParams*)params; + if(p->additional_params != nullptr){ + _writeCenterAligned3PMW(dc_a, dc_b, dc_c, p); + }else{ + // transform duty cycle from [0,1] to [0,255] + analogWrite(p->pins[0], 255.0f*dc_a); + analogWrite(p->pins[1], 255.0f*dc_b); + analogWrite(p->pins[2], 255.0f*dc_c); + } } // function setting the pwm duty cycle to the hardware // - Stepper motor - 4PWM setting -// - hardware speciffic +// - hardware specific void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, void* params){ // transform duty cycle from [0,1] to [0,255] - analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_1a); - analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_1b); - analogWrite(((GenericDriverParams*)params)->pins[2], 255.0f*dc_2a); - analogWrite(((GenericDriverParams*)params)->pins[3], 255.0f*dc_2b); + analogWrite(((TeensyDriverParams*)params)->pins[0], 255.0f*dc_1a); + analogWrite(((TeensyDriverParams*)params)->pins[1], 255.0f*dc_1b); + analogWrite(((TeensyDriverParams*)params)->pins[2], 255.0f*dc_2a); + analogWrite(((TeensyDriverParams*)params)->pins[3], 255.0f*dc_2b); } #endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/teensy/teensy_mcu.h b/src/drivers/hardware_specific/teensy/teensy_mcu.h index 7956ea90..266f4b69 100644 --- a/src/drivers/hardware_specific/teensy/teensy_mcu.h +++ b/src/drivers/hardware_specific/teensy/teensy_mcu.h @@ -1,3 +1,6 @@ +#ifndef TEENSY_MCU_DRIVER_H +#define TEENSY_MCU_DRIVER_H + #include "../../hardware_api.h" #if defined(__arm__) && defined(CORE_TEENSY) @@ -8,7 +11,17 @@ // debugging output #define SIMPLEFOC_TEENSY_DEBUG +typedef struct TeensyDriverParams { + int pins[6] = {(int)NOT_SET}; + long pwm_frequency; + void* additional_params; +} TeensyDriverParams; + // configure High PWM frequency void _setHighFrequency(const long freq, const int pin); +void* _configureCenterAligned3PMW(long pwm_frequency, const int pinA, const int pinB, const int pinC); +void _writeCenterAligned3PMW(float dc_a, float dc_b, float dc_c, void* params); + +#endif #endif \ No newline at end of file diff --git a/src/sensors/MagneticSensorI2C.cpp b/src/sensors/MagneticSensorI2C.cpp index ac2b273b..7b4ea77e 100644 --- a/src/sensors/MagneticSensorI2C.cpp +++ b/src/sensors/MagneticSensorI2C.cpp @@ -58,6 +58,10 @@ MagneticSensorI2C::MagneticSensorI2C(MagneticSensorI2CConfig_s config){ wire = &Wire; } +MagneticSensorI2C MagneticSensorI2C::AS5600() { + return {AS5600_I2C}; +} + void MagneticSensorI2C::init(TwoWire* _wire){ wire = _wire; diff --git a/src/sensors/MagneticSensorPWM.cpp b/src/sensors/MagneticSensorPWM.cpp index cf211644..04b7cc75 100644 --- a/src/sensors/MagneticSensorPWM.cpp +++ b/src/sensors/MagneticSensorPWM.cpp @@ -85,7 +85,7 @@ float MagneticSensorPWM::getSensorAngle(){ int MagneticSensorPWM::getRawCount(){ if (!is_interrupt_based){ // if it's not interrupt based read the value in a blocking way pulse_timestamp = _micros(); // ideally this should be done right at the rising edge of the pulse - pulse_length_us = pulseIn(pinPWM, HIGH, 1200); // 1200us timeout, should this be configurable? + pulse_length_us = pulseIn(pinPWM, HIGH, timeout_us); // 1200us timeout, should this be configurable? } return pulse_length_us; } diff --git a/src/sensors/MagneticSensorPWM.h b/src/sensors/MagneticSensorPWM.h index 48492c84..a5fd7e6e 100644 --- a/src/sensors/MagneticSensorPWM.h +++ b/src/sensors/MagneticSensorPWM.h @@ -44,6 +44,8 @@ class MagneticSensorPWM: public Sensor{ void enableInterrupt(void (*doPWM)()); unsigned long pulse_length_us; + unsigned int timeout_us = 1200; + private: // raw count (typically in range of 0-1023) int raw_count;