diff --git a/.github/workflows/arduino.yml b/.github/workflows/arduino.yml new file mode 100644 index 00000000..37d12ab6 --- /dev/null +++ b/.github/workflows/arduino.yml @@ -0,0 +1,61 @@ +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: align_current_sense, measure_inductance_and_resistance, + 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, + efr32_hall_sensor_velocity_6pwm, efr32_open_loop_velocity_6pwm, efr32_torque_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/arduino_mbed.yml b/.github/workflows/arduino_mbed.yml new file mode 100644 index 00000000..8d1b47e5 --- /dev/null +++ b/.github/workflows/arduino_mbed.yml @@ -0,0 +1,51 @@ +name: MDED +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:giga # arudino giga + - arduino:mbed:nanorp2040connect # arduino nano rp2040 connect + - arduino:mbed:nano33ble # arduino nano 33 ble + - arduino:mbed:envie_m7 # arduino portenta + + include: + - arduino-boards-fqbn: arduino:mbed:nanorp2040connect + sketch-names: open_loop_position_example.ino + + - arduino-boards-fqbn: arduino:mbed:nano33ble + sketch-names: open_loop_position_example.ino + + - arduino-boards-fqbn: arduino:mbed:envie_m7 + sketch-names: open_loop_position_example.ino + + # - arduino-boards-fqbn: arduino:mbed:giga + # 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..2f5accd5 --- /dev/null +++ b/.github/workflows/esp32.yml @@ -0,0 +1,55 @@ +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 + - esp32:esp32:esp32c3 # esp32c3 + + 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:esp32c3 # esp32c3 + platform-url: https://espressif.github.io/arduino-esp32/package_esp32_index.json + sketch-names: esp32_position_control.ino, stepper_driver_2pwm_standalone.ino, stepper_driver_4pwm_standalone.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/silabs.yml b/.github/workflows/silabs.yml new file mode 100644 index 00000000..7d78b5bd --- /dev/null +++ b/.github/workflows/silabs.yml @@ -0,0 +1,41 @@ +name: SILABS +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: + - SiliconLabs:silabs:nano_matter # efr32 nano matter + + include: + + - arduino-boards-fqbn: SiliconLabs:silabs:nano_matter + platform-url: https://siliconlabs.github.io/arduino/package_arduinosilabs_index.json + sketch-names: efr32_hall_sensor_velocity_6pwm.ino, efr32_open_loop_velocity_6pwm.ino, efr32_torque_velocity_6pwm.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 7d8150d0..5d31c84d 100644 --- a/README.md +++ b/README.md @@ -1,14 +1,21 @@ # 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) +[![MBED build](https://github.com/simplefoc/Arduino-FOC/actions/workflows/arduino_mbed.yml/badge.svg)](https://github.com/simplefoc/Arduino-FOC/actions/workflows/arduino_mbed.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) ![GitHub commits since tagged version](https://img.shields.io/github/commits-since/simplefoc/arduino-foc/latest/dev) ![GitHub commit activity (branch)](https://img.shields.io/github/commit-activity/m/simplefoc/arduino-foc/dev) -[![arduino-library-badge](https://www.ardu-badge.com/badge/Simple%20FOC.svg?)](https://www.ardu-badge.com/badge/Simple%20FOC.svg) +[![arduino-library-badge](https://www.ardu-badge.com/badge/Simple%20FOC.svg)](https://www.ardu-badge.com/badge/Simple%20FOC.svg) +[![PlatformIO Registry](https://badges.registry.platformio.org/packages/askuric/library/Simple%20FOC.svg)](https://registry.platformio.org/libraries/askuric/Simple%20FOC) [![License: MIT](https://img.shields.io/badge/License-MIT-yellow.svg)](https://opensource.org/licenses/MIT) [![status](https://joss.theoj.org/papers/4382445f249e064e9f0a7f6c1bb06b1d/status.svg)](https://joss.theoj.org/papers/4382445f249e064e9f0a7f6c1bb06b1d) @@ -19,17 +26,26 @@ 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.3 -> - And more bugfixes - see the complete list of 2.3.3 [fixes and PRs](https://github.com/simplefoc/Arduino-FOC/milestone/10?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/) + +> NEXT RELEASE 📢 : SimpleFOClibrary v2.3.6 +> +> - STM32 bugfix +> - BG341 low-side current sense sync was lost in v2.3.5 - fixed [#482](https://github.com/simplefoc/Arduino-FOC/pull/482) +> - ESP32 bugfix +> - Now compiles for all v3.x arduino-esp32 versions (v2.3.5 was compatible with v3.2.x) +> - `adcRead` small refactor - no more magic numbers +> - New fuctionality +> - Add current and voltage feed forward terms to motor classes by @Copper280z in https://github.com/simplefoc/Arduino-FOC/pull/454 +> - Velocity Calculation rework by @Copper280z in https://github.com/simplefoc/Arduino-FOC/pull/45 +> - Examples +> - `align_current_sense.ino` example added to the `examples/utls/current_sense_test` allowing to verify the alignment between the driver and the current sense phases + + + +## Arduino *SimpleFOClibrary* v2.3.6

diff --git a/examples/hardware_specific_examples/B_G431B_ESC1/B_G431B_ESC1.ino b/examples/hardware_specific_examples/B_G431B_ESC1/B_G431B_ESC1.ino index ab8e3bba..f1eeefd2 100644 --- a/examples/hardware_specific_examples/B_G431B_ESC1/B_G431B_ESC1.ino +++ b/examples/hardware_specific_examples/B_G431B_ESC1/B_G431B_ESC1.ino @@ -7,6 +7,7 @@ // Motor instance BLDCMotor motor = BLDCMotor(11); BLDCDriver6PWM driver = BLDCDriver6PWM(A_PHASE_UH, A_PHASE_UL, A_PHASE_VH, A_PHASE_VL, A_PHASE_WH, A_PHASE_WL); +// Gain calculation shown at https://community.simplefoc.com/t/b-g431b-esc1-current-control/521/21 LowsideCurrentSense currentSense = LowsideCurrentSense(0.003f, -64.0f/7.0f, A_OP1_OUT, A_OP2_OUT, A_OP3_OUT); @@ -25,6 +26,12 @@ void doTarget(char* cmd) { command.motion(&motor, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -75,9 +82,6 @@ void setup() { // maximal velocity of the position control motor.velocity_limit = 4; - - // use monitoring with serial - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); @@ -107,4 +111,4 @@ void loop() { // user communication command.run(); -} \ No newline at end of file +} diff --git a/examples/hardware_specific_examples/Bluepill_examples/encoder/bluepill_position_control/bluepill_position_control.ino b/examples/hardware_specific_examples/Bluepill_examples/encoder/bluepill_position_control/bluepill_position_control.ino index be853f0e..b29e45d8 100644 --- a/examples/hardware_specific_examples/Bluepill_examples/encoder/bluepill_position_control/bluepill_position_control.ino +++ b/examples/hardware_specific_examples/Bluepill_examples/encoder/bluepill_position_control/bluepill_position_control.ino @@ -33,6 +33,12 @@ void doTarget(char* cmd) { command.scalar(&target_angle, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB, doI); @@ -75,8 +81,6 @@ void setup() { motor.velocity_limit = 4; - // use monitoring with serial - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/hardware_specific_examples/Bluepill_examples/magnetic_sensor/bluepill_position_control/bluepill_position_control.ino b/examples/hardware_specific_examples/Bluepill_examples/magnetic_sensor/bluepill_position_control/bluepill_position_control.ino index e27f1e7a..caef7ffe 100644 --- a/examples/hardware_specific_examples/Bluepill_examples/magnetic_sensor/bluepill_position_control/bluepill_position_control.ino +++ b/examples/hardware_specific_examples/Bluepill_examples/magnetic_sensor/bluepill_position_control/bluepill_position_control.ino @@ -36,6 +36,12 @@ void doTarget(char* cmd) { command.scalar(&target_angle, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialise magnetic sensor hardware sensor.init(); // link the motor to the sensor @@ -72,8 +78,6 @@ void setup() { // maximal velocity of the position control motor.velocity_limit = 40; - // use monitoring with serial - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/hardware_specific_examples/DRV8302_driver/3pwm_example/encoder/full_control_serial/full_control_serial.ino b/examples/hardware_specific_examples/DRV8302_driver/3pwm_example/encoder/full_control_serial/full_control_serial.ino index 842f383a..3d90db16 100644 --- a/examples/hardware_specific_examples/DRV8302_driver/3pwm_example/encoder/full_control_serial/full_control_serial.ino +++ b/examples/hardware_specific_examples/DRV8302_driver/3pwm_example/encoder/full_control_serial/full_control_serial.ino @@ -43,6 +43,12 @@ void onMotor(char* cmd){ command.motor(&motor, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -90,9 +96,6 @@ void setup() { // angle loop velocity limit motor.velocity_limit = 50; - // use monitoring with serial for motor init - // monitoring port - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/hardware_specific_examples/DRV8302_driver/6pwm_example/encoder/full_control_serial/full_control_serial.ino b/examples/hardware_specific_examples/DRV8302_driver/6pwm_example/encoder/full_control_serial/full_control_serial.ino index 2b4cf6f1..af56e067 100644 --- a/examples/hardware_specific_examples/DRV8302_driver/6pwm_example/encoder/full_control_serial/full_control_serial.ino +++ b/examples/hardware_specific_examples/DRV8302_driver/6pwm_example/encoder/full_control_serial/full_control_serial.ino @@ -46,6 +46,12 @@ void onMotor(char* cmd){ command.motor(&motor, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -91,9 +97,6 @@ void setup() { // angle loop velocity limit motor.velocity_limit = 50; - // use monitoring with serial for motor init - // monitoring port - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/hardware_specific_examples/DRV8302_driver/esp32_current_control_low_side/esp32_current_control_low_side.ino b/examples/hardware_specific_examples/DRV8302_driver/esp32_current_control_low_side/esp32_current_control_low_side.ino index 84033b42..7d7fe14f 100644 --- a/examples/hardware_specific_examples/DRV8302_driver/esp32_current_control_low_side/esp32_current_control_low_side.ino +++ b/examples/hardware_specific_examples/DRV8302_driver/esp32_current_control_low_side/esp32_current_control_low_side.ino @@ -51,6 +51,12 @@ void onMotor(char* cmd){ command.motor(&motor, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -115,10 +121,6 @@ void setup() { 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 diff --git a/examples/hardware_specific_examples/DRV8302_driver/stm32_current_control_low_side/stm32_current_control_low_side.ino b/examples/hardware_specific_examples/DRV8302_driver/stm32_current_control_low_side/stm32_current_control_low_side.ino index 3e3f04ab..e1b4c392 100644 --- a/examples/hardware_specific_examples/DRV8302_driver/stm32_current_control_low_side/stm32_current_control_low_side.ino +++ b/examples/hardware_specific_examples/DRV8302_driver/stm32_current_control_low_side/stm32_current_control_low_side.ino @@ -51,6 +51,12 @@ void onMotor(char* cmd){ command.motor(&motor, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -115,10 +121,6 @@ void setup() { 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 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..c9b4516f --- /dev/null +++ b/examples/hardware_specific_examples/DRV8302_driver/teensy4_current_control_low_side/teensy4_current_control_low_side.ino @@ -0,0 +1,170 @@ +/** + * 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() { + + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + + // 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 + + // 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/hardware_specific_examples/ESP32/encoder/esp32_position_control/esp32_position_control.ino b/examples/hardware_specific_examples/ESP32/encoder/esp32_position_control/esp32_position_control.ino index f6dac940..7f6b33ce 100644 --- a/examples/hardware_specific_examples/ESP32/encoder/esp32_position_control/esp32_position_control.ino +++ b/examples/hardware_specific_examples/ESP32/encoder/esp32_position_control/esp32_position_control.ino @@ -24,6 +24,12 @@ void doTarget(char* cmd) { command.scalar(&target_angle, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -66,9 +72,6 @@ void setup() { // maximal velocity of the position control motor.velocity_limit = 4; - - // use monitoring with serial - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino b/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino index f025895b..9ba9604a 100644 --- a/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino +++ b/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino @@ -33,6 +33,12 @@ void doTarget(char* cmd) { command.scalar(&target_angle, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialise magnetic sensor hardware sensor.init(); // link the motor to the sensor @@ -69,8 +75,6 @@ void setup() { // maximal velocity of the position control motor.velocity_limit = 40; - // use monitoring with serial - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/hardware_specific_examples/HMBGC_example/position_control/position_control.ino b/examples/hardware_specific_examples/HMBGC_example/position_control/position_control.ino index 5b5c8e40..d9107654 100644 --- a/examples/hardware_specific_examples/HMBGC_example/position_control/position_control.ino +++ b/examples/hardware_specific_examples/HMBGC_example/position_control/position_control.ino @@ -42,6 +42,9 @@ void doTarget(char* cmd) { command.scalar(&target_angle, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // initialise encoder hardware encoder.init(); // interrupt initialization @@ -85,9 +88,6 @@ void setup() { // maximal velocity of the position control motor.velocity_limit = 4; - - // use monitoring with serial - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/hardware_specific_examples/Odrive_examples/odrive_example_encoder/odrive_example_encoder.ino b/examples/hardware_specific_examples/Odrive_examples/odrive_example_encoder/odrive_example_encoder.ino index b89c215f..504ab9c8 100644 --- a/examples/hardware_specific_examples/Odrive_examples/odrive_example_encoder/odrive_example_encoder.ino +++ b/examples/hardware_specific_examples/Odrive_examples/odrive_example_encoder/odrive_example_encoder.ino @@ -70,6 +70,12 @@ void doI(){encoder.handleIndex();} void setup(){ + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // pwm frequency to be used [Hz] driver.pwm_frequency = 20000; // power supply voltage [V] @@ -96,8 +102,6 @@ void setup(){ // alignment voltage limit motor.voltage_sensor_align = 0.5; - - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); motor.monitor_variables = _MON_CURR_Q | _MON_CURR_D; diff --git a/examples/hardware_specific_examples/Odrive_examples/odrive_example_spi/odrive_example_spi.ino b/examples/hardware_specific_examples/Odrive_examples/odrive_example_spi/odrive_example_spi.ino index 2aed6bfb..7abcde54 100644 --- a/examples/hardware_specific_examples/Odrive_examples/odrive_example_spi/odrive_example_spi.ino +++ b/examples/hardware_specific_examples/Odrive_examples/odrive_example_spi/odrive_example_spi.ino @@ -69,6 +69,12 @@ SPIClass SPI_3(SPI3_MOSO, SPI3_MISO, SPI3_SCL); void setup(){ + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // pwm frequency to be used [Hz] driver.pwm_frequency = 20000; // power supply voltage [V] @@ -94,8 +100,6 @@ void setup(){ // alignment voltage limit motor.voltage_sensor_align = 0.5; - - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); motor.monitor_variables = _MON_CURR_Q | _MON_CURR_D; diff --git a/examples/hardware_specific_examples/SAMD_examples/nano33IoT/nano33IoT_velocity_control/nano33IoT_velocity_control.ino b/examples/hardware_specific_examples/SAMD_examples/nano33IoT/nano33IoT_velocity_control/nano33IoT_velocity_control.ino index 70ec28b0..650050ef 100644 --- a/examples/hardware_specific_examples/SAMD_examples/nano33IoT/nano33IoT_velocity_control/nano33IoT_velocity_control.ino +++ b/examples/hardware_specific_examples/SAMD_examples/nano33IoT/nano33IoT_velocity_control/nano33IoT_velocity_control.ino @@ -23,7 +23,12 @@ void doTarget(char* cmd) { command.scalar(&target_velocity, cmd); } void setup() { + // use monitoring with serial Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + delay(1000); Serial.println("Initializing..."); diff --git a/examples/hardware_specific_examples/Silabs/efr32_hall_sensor_velocity_6pwm/efr32_hall_sensor_velocity_6pwm.ino b/examples/hardware_specific_examples/Silabs/efr32_hall_sensor_velocity_6pwm/efr32_hall_sensor_velocity_6pwm.ino new file mode 100644 index 00000000..8ca6bfa2 --- /dev/null +++ b/examples/hardware_specific_examples/Silabs/efr32_hall_sensor_velocity_6pwm/efr32_hall_sensor_velocity_6pwm.ino @@ -0,0 +1,168 @@ +/** + * Silabs MG24 6PWM closed loop velocity control example with HALL sensor based rotor position + * + * HARDWARE CONFIGURATION: + * CPU Board: Arduino Nano Matter + * Motor Driver Board: BOOSTXL-DRV8305EVM + * BLDC Motor: Newark DF45M024053-A2 + */ + +#include + +#define HALL_SENSOR_IRQ 1 +#define ENABLE_MONITOR 0 + +static bool allow_run = false; + +// BLDC motor instance +BLDCMotor *motor; + +// BLDC driver instance +BLDCDriver6PWM *driver; + +// Commander instance +Commander *command; + +// Hall sensor instance +HallSensor *sensor; + +// Interrupt routine initialization +// channel A and B callbacks +void doA() { sensor->handleA(); } +void doB() { sensor->handleB(); } +void doC() { sensor->handleC(); } + +void doMotor(char* cmd) { + if (!command) return; + command->motor(motor, cmd); +} + +void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + + // Commander + command = new Commander(Serial); + if (!command) return; + + // Driver + driver = new BLDCDriver6PWM(6, 7, 8, 9, 10, 11, 12); + if (!driver) return; + + // Driver config + // power supply voltage [V] + driver->voltage_power_supply = 24; + // pwm frequency to be used [Hz] + driver->pwm_frequency = 20000; // 20 kHz + // Max DC voltage allowed - default voltage_power_supply + driver->voltage_limit = 12; + // dead zone percentage of the duty cycle - default 0.02 - 2% + // Can set value to 0 because the DRV8305 will provide the + // required dead-time. + driver->dead_zone = 0; + + // init driver + if (!driver->init()) { + Serial.println("Driver init failed!"); + return; + } + driver->enable(); + + // HallSensor(int encA, int encB, int encC, int pp) + // - encA, encB, encC - HallSensor A, B and C pins + // - pp - pole pairs + sensor = new HallSensor(5, 4, 13, 8); + if (!sensor) return; + + // initialize sensor sensor hardware + sensor->init(); + +#if HALL_SENSOR_IRQ + sensor->enableInterrupts(doA, doB, doC); +#else + // Note: There is a bug when initializing HallSensor in heap, attribute + // `use_interrupt` gets value not `false` even `enableInterrupts` has not been + // called. So we initialize this attribute value `false` after creating a + // `HallSensor` instance. + sensor->use_interrupt = false; +#endif + + // Motor + motor = new BLDCMotor(8); + if (!motor) return; + + // link the motor and the driver + motor->linkDriver(driver); + + // link the motor to the sensor + motor->linkSensor(sensor); + + // Set below the motor's max 5600 RPM limit = 586 rad/s + motor->velocity_limit = 530.0f; + + // set motion control loop to be used + motor->controller = MotionControlType::velocity; + + // choose FOC modulation (optional) - SinePWM or SpaceVectorPWM + motor->foc_modulation = FOCModulationType::SpaceVectorPWM; + + // controller configuration + // velocity PI controller parameters + motor->PID_velocity.P = 0.05f; + motor->PID_velocity.I = 1; + + // velocity low pass filtering time constant + motor->LPF_velocity.Tf = 0.01f; + +#if ENABLE_MONITOR + motor->useMonitoring(Serial); + motor->monitor_variables = _MON_TARGET | _MON_VEL; +#endif + + // initialize motor + if (!motor->init()) { + Serial.println("Motor init failed!"); + return; + } + + // align sensor and start FOC + if (!motor->initFOC()) { + Serial.println("FOC init failed!"); + return; + } + + // add target command M + command->add('M', doMotor, "motor"); + + Serial.println("Motor ready!"); + Serial.println("Set target velocity [rad/s]"); + + allow_run = true; + _delay(1000); +} + +void loop() { + if (!allow_run) return; + + // main FOC algorithm function + // the faster you run this function the better + motor->loopFOC(); + + // Motion control function + // velocity, position or voltage (defined in motor.controller) + // this function can be run at much lower frequency than loopFOC() function + // You can also use motor.move() and set the motor.target in the code + motor->move(); + +#if ENABLE_MONITOR + // function intended to be used with serial plotter to monitor motor variables + // significantly slowing the execution down!!!! + motor->monitor(); +#endif + + // user communication + command->run(); +} diff --git a/examples/hardware_specific_examples/Silabs/efr32_open_loop_velocity_6pwm/efr32_open_loop_velocity_6pwm.ino b/examples/hardware_specific_examples/Silabs/efr32_open_loop_velocity_6pwm/efr32_open_loop_velocity_6pwm.ino new file mode 100644 index 00000000..0dcd02be --- /dev/null +++ b/examples/hardware_specific_examples/Silabs/efr32_open_loop_velocity_6pwm/efr32_open_loop_velocity_6pwm.ino @@ -0,0 +1,104 @@ +/** + * Silabs MG24 6PWM open loop velocity control example + * + * HARDWARE CONFIGURATION: + * CPU Board: Arduino Nano Matter + * Motor Driver Board: BOOSTXL-DRV8305EVM + * BLDC Motor: Newark DF45M024053-A2 + */ + +#include + +static bool allow_run = false; + +// BLDC motor instance +BLDCMotor *motor; + +// BLDC driver instance +BLDCDriver6PWM *driver; + +// Commander instance +Commander *command; + +void doMotor(char* cmd) { + if (!command) return; + command->motor(motor, cmd); +} + +void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + + // Commander + command = new Commander(Serial); + if (!command) return; + + // Driver + driver = new BLDCDriver6PWM(6, 7, 8, 9, 10, 11, 12); + if (!driver) return; + + // Driver config + // power supply voltage [V] + driver->voltage_power_supply = 24; + // pwm frequency to be used [Hz] + driver->pwm_frequency = 20000; // 20 kHz + // Max DC voltage allowed - default voltage_power_supply + driver->voltage_limit = 12; + // dead zone percentage of the duty cycle - default 0.02 - 2% + // Can set value to 0 because the DRV8305 will provide the + // required dead-time. + driver->dead_zone = 0; + + // init driver + if (!driver->init()) { + Serial.println("Driver init failed!"); + return; + } + driver->enable(); + + // Motor + motor = new BLDCMotor(8); + if (!motor) return; + + // link the motor and the driver + motor->linkDriver(driver); + + // default voltage_power_supply + motor->voltage_limit = 0.8f; + + // set motion control loop to be used + motor->controller = MotionControlType::velocity_openloop; + + // choose FOC modulation (optional) - SinePWM or SpaceVectorPWM + motor->foc_modulation = FOCModulationType::SpaceVectorPWM; + + // initialize motor + if (!motor->init()) { + Serial.println("Motor init failed!"); + return; + } + + // add target command M + command->add('M', doMotor, "motor"); + + Serial.println("Motor ready!"); + Serial.println("Set target velocity [rad/s]"); + + allow_run = true; + _delay(1000); +} + + +void loop() { + if (!allow_run) return; + + // open loop velocity movement + // using motor.voltage_limit + motor->move(); + + // user communication + command->run(); +} diff --git a/examples/hardware_specific_examples/Silabs/efr32_torque_velocity_6pwm/efr32_torque_velocity_6pwm.ino b/examples/hardware_specific_examples/Silabs/efr32_torque_velocity_6pwm/efr32_torque_velocity_6pwm.ino new file mode 100644 index 00000000..4f1218cc --- /dev/null +++ b/examples/hardware_specific_examples/Silabs/efr32_torque_velocity_6pwm/efr32_torque_velocity_6pwm.ino @@ -0,0 +1,205 @@ +/** + * Silabs MG24 6PWM closed loop velocity and torque control example with HALL sensor based rotor position + * + * HARDWARE CONFIGURATION: + * CPU Board: Arduino Nano Matter + * Motor Driver Board: BOOSTXL-DRV8305EVM + * BLDC Motor: Newark DF45M024053-A2 + */ + +#include + +#define HALL_SENSOR_IRQ 1 +#define ENABLE_MONITOR 0 + +static bool allow_run = false; + +// BLDC motor instance +BLDCMotor *motor; + +// BLDC driver instance +BLDCDriver6PWM *driver; + +// Commander instance +Commander *command; + +// Hall sensor instance +HallSensor *sensor; + +// Current sensor +LowsideCurrentSense *current_sense; + +// Interrupt routine initialization +// channel A and B callbacks +void doA() { sensor->handleA(); } +void doB() { sensor->handleB(); } +void doC() { sensor->handleC(); } + +void doMotor(char* cmd) { + if (!command) return; + command->motor(motor, cmd); +} + +void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + + // Commander + command = new Commander(Serial); + if (!command) return; + + // Driver + driver = new BLDCDriver6PWM(6, 7, 8, 9, 10, 11, 12); + if (!driver) return; + + // Driver config + // power supply voltage [V] + driver->voltage_power_supply = 24; + // pwm frequency to be used [Hz] + driver->pwm_frequency = 20000; // 20 kHz + // Max DC voltage allowed - default voltage_power_supply + driver->voltage_limit = 12; + // dead zone percentage of the duty cycle - default 0.02 - 2% + // Can set value to 0 because the DRV8305 will provide the + // required dead-time. + driver->dead_zone = 0; + + // init driver + if (!driver->init()) { + Serial.println("Driver init failed!"); + return; + } + driver->enable(); + + // HallSensor(int encA, int encB, int encC, int pp) + // - encA, encB, encC - HallSensor A, B and C pins + // - pp - pole pairs + sensor = new HallSensor(5, 4, 13, 8); + if (!sensor) return; + + // initialize sensor sensor hardware + sensor->init(); + +#if HALL_SENSOR_IRQ + sensor->enableInterrupts(doA, doB, doC); +#else + // Note: There is a bug when initializing HallSensor in heap, attribute + // `use_interrupt` gets value not `false` even `enableInterrupts` has not been + // called. So we initialize this attribute value `false` after creating a + // `HallSensor` instance. + sensor->use_interrupt = false; +#endif + + // Motor + motor = new BLDCMotor(8); + if (!motor) return; + + // link the motor and the driver + motor->linkDriver(driver); + + // link the motor to the sensor + motor->linkSensor(sensor); + + // Set below the motor's max 5600 RPM limit = 586 rad/s + motor->velocity_limit = 530.0f; + + // set torque mode + motor->torque_controller = TorqueControlType::foc_current; + + // set motion control loop to be used + motor->controller = MotionControlType::velocity; + + // choose FOC modulation (optional) - SinePWM or SpaceVectorPWM + motor->foc_modulation = FOCModulationType::SpaceVectorPWM; + + // controller configuration + // velocity PI controller parameters + motor->PID_velocity.P = 0.025f; + motor->PID_velocity.I = 0.4f; + + // velocity low pass filtering time constant + motor->LPF_velocity.Tf = 0.01f; + + // current q loop PID + motor->PID_current_q.P = 1.0f; + motor->PID_current_q.I = 100.0f; + motor->LPF_current_q.Tf = 0.01f; + + // current d loop PID + motor->PID_current_d.P = 1.0f; + motor->PID_current_d.I = 100.0f; + motor->LPF_current_d.Tf = 0.01f; + +#if ENABLE_MONITOR + // comment out if not needed + motor->useMonitoring(Serial); + motor->monitor_variables = _MON_TARGET | _MON_CURR_Q | _MON_CURR_D | _MON_VEL; +#endif + + // initialize motor + if (!motor->init()) { + Serial.println("Motor init failed!"); + return; + } + + // Current Sense + current_sense = new LowsideCurrentSense(0.007f, 10.0f, A0, A1, A2); + if (!current_sense) return; + + // link current sense and the driver + current_sense->linkDriver(driver); + + if (!current_sense->init()) { + Serial.println("Current Sense init failed!"); + return; + } + + // DRV8305 has inverted gains on all channels + current_sense->gain_a *= -1; + current_sense->gain_b *= -1; + current_sense->gain_c *= -1; + + // link the motor and the driver + motor->linkCurrentSense(current_sense); + + // align sensor and start FOC + if (!motor->initFOC()) { + Serial.println("FOC init failed!"); + return; + } + + // add target command M + command->add('M', doMotor, "motor"); + + Serial.println("Motor ready!"); + Serial.println("Set target velocity [rad/s]"); + + allow_run = true; + _delay(1000); +} + +void loop() { + if (!allow_run) return; + + // main FOC algorithm function + // the faster you run this function the better + motor->loopFOC(); + + // Motion control function + // velocity, position or voltage (defined in motor.controller) + // this function can be run at much lower frequency than loopFOC() function + // You can also use motor.move() and set the motor.target in the code + motor->move(); + +#if ENABLE_MONITOR + // function intended to be used with serial plotter to monitor motor variables + // significantly slowing the execution down!!!! + motor->monitor(); +#endif + + // user communication + command->run(); +} diff --git a/examples/hardware_specific_examples/Silabs/hw_setup.png b/examples/hardware_specific_examples/Silabs/hw_setup.png new file mode 100644 index 00000000..56a3af6f Binary files /dev/null and b/examples/hardware_specific_examples/Silabs/hw_setup.png differ diff --git a/examples/hardware_specific_examples/Silabs/nanoMatterMC_Wiring.drawio b/examples/hardware_specific_examples/Silabs/nanoMatterMC_Wiring.drawio new file mode 100644 index 00000000..3b993400 --- /dev/null +++ b/examples/hardware_specific_examples/Silabs/nanoMatterMC_Wiring.drawio @@ -0,0 +1,177 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/hardware_specific_examples/Silabs/nanoMatterMC_Wiring.png b/examples/hardware_specific_examples/Silabs/nanoMatterMC_Wiring.png new file mode 100644 index 00000000..0c11c2d4 Binary files /dev/null and b/examples/hardware_specific_examples/Silabs/nanoMatterMC_Wiring.png differ diff --git a/examples/hardware_specific_examples/Silabs/readme.md b/examples/hardware_specific_examples/Silabs/readme.md new file mode 100644 index 00000000..2837dcfe --- /dev/null +++ b/examples/hardware_specific_examples/Silabs/readme.md @@ -0,0 +1,144 @@ +# Silabs Hardware-Specific Examples for SimpleFOC + +This folder contains **hardware-specific example projects** for running the [SimpleFOC](https://docs.simplefoc.com/) library on **Silicon Labs EFR32 microcontrollers** (tested on Arduino Nano Matter). Each example demonstrates a motor control use case using a **6PWM BLDC driver**. + +**Example List** + +- **efr32_hall_sensor_velocity_6pwm** + - Demonstrates velocity control of a BLDC motor using a Hall sensor for feedback and a 6PWM driver on an EFR32 board. + +- **efr32_open_loop_velocity_6pwm** + - Shows how to run a BLDC motor in open-loop velocity mode (no sensor feedback) using a 6PWM driver on EFR32. + +- **efr32_torque_velocity_6pwm** + - Example of torque and velocity control for a BLDC motor using a 6PWM driver on EFR32, suitable for advanced control scenarios. + +## Hardware Setup + +Required hardware: + +* **Arduino Nano Matter (EFR32MG24)** [https://docs.arduino.cc/hardware/nano-matter/](https://docs.arduino.cc/hardware/nano-matter/) +* **DRV8305 BoosterPack (BOOSTXL-DRV8305EVM)** +* **BLDC Motor: DF45M024053 – A2** +* USB cable for programming and serial monitor + +A dedicated interface board set-up connecting the Motor - Power Stage - Nano Matter. Jump wires can be used as well to connect the boards. + +![hw_setup](hw_setup.png) + +Connect the Arduino Nano Matter board to the DRV8305EVM according to the board pin mapping (phase outputs, PWM inputs, and Hall sensor connections). + +![MC Wiring](nanoMatterMC_Wiring.png) + +### Wiring Table: Arduino Nano Matter to BOOSTXL-DRV8305 & BLDC Motor + +This table describes the connections between the Arduino Nano Matter, the TI BOOSTXL-DRV8305 driver board, and a 3-phase BLDC motor with Hall sensors. + +| From (Nano Matter Pin) | To (DRV8305 BoosterPack Pin) | BLDC MOTOR | Function / Description | +| :--- | :--- | :--- | :---| +| `3.3V` | `3V3` | HALL/ENC Supply* | 3.3V Power the BoosterPack provides 3.3V through an LDO | +| `GND` | `PowerSupply GND` | HALL/ENC sensor GND | Common Ground | +| N/A | `PowerSupply 12V` | N/A | Power supply for power stage 4.4 to 45 V, consider motor power| +| `A0` | `ISENA` | N/A | Phase A current sense | +| `A1` | `ISENB` | N/A | Phase B current sense | +| `A2` | `ISENC` | N/A | Phase C current sense | +| `A3` | `VSENA` | N/A | Phase A Voltage sense (Optional, not mandatory to run examples) | +| `A4` | `VSENB` | N/A | Phase B Voltage sense (Optional, not mandatory to run examples) | +| `A5` | `VSENC` | N/A | Phase C Voltage sense (Optional, not mandatory to run examples)| +| `A6` | `VSENVPVDD` | N/A | DC BUS Voltage sense (Optional, not mandatory to run examples)| +| `D0` (MOSI1) | `SDI` | N/A | DRV8035 SPI connection, configuration and status reading (Optional, not mandatory to run examples)| +| `D1` (MISO1) | `SDO` | N/A | DRV8035 SPI connection, configuration and status reading (Optional, not mandatory to run examples)| +| `D2` (SCK1) | `SCLK`| N/A | DRV8035 SPI clock, configuration and status reading (Optional, not mandatory to run examples)| +| `D3` (SS1) | `SCS` | N/A | DRV8035 SPI chip select, configuration and status reading (Optional, not mandatory to run examples)| +| `D4` | N/A | HALL A or Encoder A | Motor sensor connection (Hall configuration in examples) | +| `D5` | N/A | HALL B or Encoder B | Motor sensor connection (Hall configuration in examples) | +| `D6` | `PWMHA` | N/A | PWM Phase A High-Side Gate Signal | +| `D7` | `PWMLA` | N/A | PWM Phase A Low-Side Gate Signal | +| `D8` | `PWMHB` | N/A | PWM Phase B High-Side Gate Signal | +| `D9` | `PWMLB` | N/A | PWM Phase B Low-Side Gate Signal | +| `D10` | `PWMHC` | N/A | PWM Phase C High-Side Gate Signal | +| `D11` | `PWMLC` | N/A | PWM Phase C Low-Side Gate Signal | +| `D12` | `ENGATE` | N/A | Enable DRV8305 gate driver | +| `D13` | N/A | HALL C or Encoder Index | Motor sensor connection (Hall configuration in examples) | +| N/A | `PHASE U` | `PHASE U` | Motor phase connection | +| N/A | `PHASE V` | `PHASE V` | Motor phase connection | +| N/A | `PHASE W` | `PHASE W` | Motor phase connection | + +**Important Notes:** +* **Power:** Ensure the DRV8305's `PVDD` and `GVDD` jumpers are correctly set for your motor's voltage. The power supply should be rated at least twice the motor’s nominal power. The BoosterPack can supply the Nano Matter if it is not connected to USB. +* **Rotor sensor:** Some Encoder or Hall sensors might require 5V supply, make sure of proper level shifting if required. +* **SPI:** The SPI connection (`nSCS`, `SPI_CLK`, `SPI_MOSI`, `SPI_MISO`) is used to configure the DRV8305 driver IC (e.g., set gain, fault parameters). It is optional for the examples. The examples are using the default gate driver configuration. Only needed if you wish to change the default gate driver configuration (e.g., dead time, fault parameters). + +--- + +## Software Setup + +1. **Arduino IDE** + + * Use Arduino IDE **2.3.4 or later**. + * [Download here](https://www.arduino.cc/en/software). + +2. **Silicon Labs Arduino Core** + + * Open *Boards Manager* in Arduino IDE. + * Search for *Silicon Labs* and install the latest version (**2.3.0** or newer). + * If not found, add this URL under *Preferences → Additional Boards Manager URLs*: + + ``` + https://siliconlabs.github.io/arduino/package_arduinosilabs_index.json + ``` + +3. **SimpleFOC (Silabs-modified version)** + + * Open *Library Manager* in Arduino IDE. + * Search and add *Simple FOC* library + +4. **(Optional) SimpleFOC Studio or web viewer** + + * For runtime tuning and monitoring. + * [Docs](https://docs.simplefoc.com/studio) + * [Enable Monitoring](https://docs.simplefoc.com/monitoring) + +--- + +## Running the Examples + +1. Open the `.ino` file in Arduino IDE. +2. Select your **Arduino Nano Matter** board. +3. Compile & upload. +4. Open the Serial Monitor (115200 baud) +5. (Optional) Connect with monitoring tools. + 1. Modification of the example code is necessary to enable monitoring feature. + 2. [Enable Monitoring](https://docs.simplefoc.com/monitoring) + +### Example Commands + +Send commands to control the motor: + +``` bash +M50 # Run clockwise at 50 rad/s +M-50 # Run counter-clockwise at 50 rad/s +M0 # Stop motor +``` + +--- + +## Notes on EFR32 Porting + +The EFR32 port of SimpleFOC includes: + +* Full **6PWM driver support** (deadtime insertion, duty cycle updates). +* **Current sense integration** (low-side sensing tested). +* **Arduino-style pin mapping** for portability. + +--- + +## References + +* [SimpleFOC Documentation](https://docs.simplefoc.com/) +* [Commander Interface](https://docs.simplefoc.com/commander_interface) +* [Arduino Nano Matter Manual](https://docs.arduino.cc/tutorials/nano-matter/user-manual/) +* [Silicon Labs Arduino Core](https://github.com/SiliconLabs/arduino) +* [SimpleFOC Studio](https://github.com/JorgeMaker/SimpleFOCStudio) + + diff --git a/examples/hardware_specific_examples/SimpleFOC-PowerShield/version_v02/single_full_control_example/single_full_control_example.ino b/examples/hardware_specific_examples/SimpleFOC-PowerShield/version_v02/single_full_control_example/single_full_control_example.ino index eb52f51b..bc387437 100644 --- a/examples/hardware_specific_examples/SimpleFOC-PowerShield/version_v02/single_full_control_example/single_full_control_example.ino +++ b/examples/hardware_specific_examples/SimpleFOC-PowerShield/version_v02/single_full_control_example/single_full_control_example.ino @@ -21,6 +21,12 @@ void doTarget(char* cmd){ command.scalar(&motor.target, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -56,9 +62,6 @@ void setup() { // angle loop velocity limit motor.velocity_limit = 20; - // use monitoring with serial for motor init - // monitoring port - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); motor.monitor_downsample = 0; // disable intially diff --git a/examples/hardware_specific_examples/SimpleFOCMini/angle_control/angle_control.ino b/examples/hardware_specific_examples/SimpleFOCMini/angle_control/angle_control.ino index 9102c89a..8eb122a0 100644 --- a/examples/hardware_specific_examples/SimpleFOCMini/angle_control/angle_control.ino +++ b/examples/hardware_specific_examples/SimpleFOCMini/angle_control/angle_control.ino @@ -38,6 +38,12 @@ Commander command = Commander(Serial); void doMotor(char* cmd) { command.motor(&motor, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // if SimpleFOCMini is stacked in arduino headers // on pins 12,11,10,9,8 // pin 12 is used as ground @@ -84,9 +90,6 @@ void setup() { // maximal velocity of the position control motor.velocity_limit = 4; - - // use monitoring with serial - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/hardware_specific_examples/SimpleFOCMini/open_loop/open_loop.ino b/examples/hardware_specific_examples/SimpleFOCMini/open_loop/open_loop.ino new file mode 100644 index 00000000..c458718b --- /dev/null +++ b/examples/hardware_specific_examples/SimpleFOCMini/open_loop/open_loop.ino @@ -0,0 +1,93 @@ +/** + * + * SimpleFOCMini motor control example + * + * For Arduino UNO or the other boards with the UNO headers + * the most convenient way to use the board is to stack it to the pins: + * - 12 - ENABLE + * - 11 - IN1 + * - 10 - IN2 + * - 9 - IN3 + * + */ +#include + + +// BLDC motor & driver instance +BLDCMotor motor = BLDCMotor(11); +// BLDCDriver3PWM driver = BLDCDriver3PWM(11, 10, 9, 8); // mini v1.0 +BLDCDriver3PWM driver = BLDCDriver3PWM(9, 10, 11, 12); // mini v1.1 + +// instantiate the commander +Commander command = Commander(Serial); +void doMotor(char* cmd) { command.motor(&motor, cmd); } + +void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + + // if SimpleFOCMini is stacked in arduino headers + // on pins 12,11,10,9,8 + // pin 12 is used as ground + pinMode(12,OUTPUT); + pinMode(12,LOW); + + // driver config + // power supply voltage [V] + driver.voltage_power_supply = 12; + driver.init(); + // link the motor and the driver + motor.linkDriver(&driver); + + // aligning voltage [V] + motor.voltage_sensor_align = 3; + + // set motion control loop to be used + motor.controller = MotionControlType::velocity_openloop; + + // default voltage_power_supply + motor.voltage_limit = 2; // Volts + + // comment out if not needed + motor.useMonitoring(Serial); + + // initialize motor + motor.init(); + // align encoder and start FOC + motor.initFOC(); + + // add target command M + command.add('M', doMotor, "motor"); + + Serial.println(F("Motor ready.")); + Serial.println(F("Set the target velocity using serial terminal:")); + + motor.target = 1; //initial target velocity 1 rad/s + Serial.println("Target velocity: 1 rad/s"); + Serial.println("Voltage limit 2V"); + _delay(1000); +} + +void loop() { + // main FOC algorithm function + // the faster you run this function the better + // Arduino UNO loop ~1kHz + // Bluepill loop ~10kHz + motor.loopFOC(); + + // Motion control function + // velocity, position or voltage (defined in motor.controller) + // this function can be run at much lower frequency than loopFOC() function + // You can also use motor.move() and set the motor.target in the code + motor.move(); + + // function intended to be used with serial plotter to monitor motor variables + // significantly slowing the execution down!!!! + // motor.monitor(); + + // user communication + command.run(); +} \ No newline at end of file diff --git a/examples/hardware_specific_examples/SimpleFOCShield/version_v1/double_full_control_example/double_full_control_example.ino b/examples/hardware_specific_examples/SimpleFOCShield/version_v1/double_full_control_example/double_full_control_example.ino index c048956b..fb5e1563 100644 --- a/examples/hardware_specific_examples/SimpleFOCShield/version_v1/double_full_control_example/double_full_control_example.ino +++ b/examples/hardware_specific_examples/SimpleFOCShield/version_v1/double_full_control_example/double_full_control_example.ino @@ -28,6 +28,12 @@ void doMotor2(char* cmd){ command.motor(&motor2, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder1.init(); encoder1.enableInterrupts(doA1, doB1); @@ -72,9 +78,6 @@ void setup() { motor1.velocity_limit = 20; motor2.velocity_limit = 20; - // use monitoring with serial for motor init - // monitoring port - Serial.begin(115200); // comment out if not needed motor1.useMonitoring(Serial); motor2.useMonitoring(Serial); diff --git a/examples/hardware_specific_examples/SimpleFOCShield/version_v1/single_full_control_example/single_full_control_example.ino b/examples/hardware_specific_examples/SimpleFOCShield/version_v1/single_full_control_example/single_full_control_example.ino index 10e26c9f..3faa38b2 100644 --- a/examples/hardware_specific_examples/SimpleFOCShield/version_v1/single_full_control_example/single_full_control_example.ino +++ b/examples/hardware_specific_examples/SimpleFOCShield/version_v1/single_full_control_example/single_full_control_example.ino @@ -17,6 +17,12 @@ void doMotor(char* cmd){ command.motor(&motor, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -48,9 +54,6 @@ void setup() { // angle loop velocity limit motor.velocity_limit = 20; - // use monitoring with serial for motor init - // monitoring port - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); motor.monitor_downsample = 0; // disable intially diff --git a/examples/hardware_specific_examples/SimpleFOCShield/version_v2/double_full_control_example/double_full_control_example.ino b/examples/hardware_specific_examples/SimpleFOCShield/version_v2/double_full_control_example/double_full_control_example.ino index b40ab62a..8eb3a86d 100644 --- a/examples/hardware_specific_examples/SimpleFOCShield/version_v2/double_full_control_example/double_full_control_example.ino +++ b/examples/hardware_specific_examples/SimpleFOCShield/version_v2/double_full_control_example/double_full_control_example.ino @@ -39,6 +39,12 @@ void doTarget2(char* cmd){ command.scalar(&motor2.target, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder1.init(); encoder1.enableInterrupts(doA1, doB1); @@ -88,9 +94,6 @@ void setup() { motor1.velocity_limit = 20; motor2.velocity_limit = 20; - // use monitoring with serial for motor init - // monitoring port - Serial.begin(115200); // comment out if not needed motor1.useMonitoring(Serial); motor2.useMonitoring(Serial); diff --git a/examples/hardware_specific_examples/SimpleFOCShield/version_v2/single_full_control_example/single_full_control_example.ino b/examples/hardware_specific_examples/SimpleFOCShield/version_v2/single_full_control_example/single_full_control_example.ino index 76a7296a..b65a9cb0 100644 --- a/examples/hardware_specific_examples/SimpleFOCShield/version_v2/single_full_control_example/single_full_control_example.ino +++ b/examples/hardware_specific_examples/SimpleFOCShield/version_v2/single_full_control_example/single_full_control_example.ino @@ -22,6 +22,12 @@ void doMotion(char* cmd){ command.motion(&motor, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -55,9 +61,6 @@ void setup() { // angle loop velocity limit motor.velocity_limit = 20; - // use monitoring with serial for motor init - // monitoring port - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); motor.monitor_downsample = 0; // disable intially diff --git a/examples/hardware_specific_examples/SimpleFOCShield/version_v3/single_full_control_example/single_full_control_example.ino b/examples/hardware_specific_examples/SimpleFOCShield/version_v3/single_full_control_example/single_full_control_example.ino index 02593286..6359f201 100644 --- a/examples/hardware_specific_examples/SimpleFOCShield/version_v3/single_full_control_example/single_full_control_example.ino +++ b/examples/hardware_specific_examples/SimpleFOCShield/version_v3/single_full_control_example/single_full_control_example.ino @@ -13,7 +13,7 @@ void doB(){encoder.handleB();} // inline current sensor instance // ACS712-05B has the resolution of 0.185mV per Amp -InlineCurrentSense current_sense = InlineCurrentSense(1.0f, 0.185f, A0, A2); +InlineCurrentSense current_sense = InlineCurrentSense(185.0f, A0, A2); // commander communication instance Commander command = Commander(Serial); @@ -22,6 +22,12 @@ void doMotion(char* cmd){ command.motion(&motor, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -55,9 +61,6 @@ void setup() { // angle loop velocity limit motor.velocity_limit = 20; - // use monitoring with serial for motor init - // monitoring port - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); motor.monitor_downsample = 0; // disable intially diff --git a/examples/hardware_specific_examples/Smart_Stepper/smartstepper_control/smartstepper_control.ino b/examples/hardware_specific_examples/Smart_Stepper/smartstepper_control/smartstepper_control.ino index 80f2fe64..18047108 100644 --- a/examples/hardware_specific_examples/Smart_Stepper/smartstepper_control/smartstepper_control.ino +++ b/examples/hardware_specific_examples/Smart_Stepper/smartstepper_control/smartstepper_control.ino @@ -18,6 +18,12 @@ void doMotor(char* cmd) { command.motor(&motor, cmd); } void setup() { + // use monitoring with SerialUSB + SerialUSB.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&SerialUSB); + // initialise magnetic sensor hardware sensor.init(); // link the motor to the sensor @@ -31,8 +37,6 @@ void setup() { // set motion control loop to be used motor.controller = MotionControlType::torque; - // use monitoring with SerialUSB - SerialUSB.begin(115200); // comment out if not needed motor.useMonitoring(SerialUSB); diff --git a/examples/hardware_specific_examples/Teensy/Teensy3/open_loop_velocity_6pwm/open_loop_velocity_6pwm.ino b/examples/hardware_specific_examples/Teensy/Teensy3/open_loop_velocity_6pwm/open_loop_velocity_6pwm.ino index 40924ee3..64651392 100644 --- a/examples/hardware_specific_examples/Teensy/Teensy3/open_loop_velocity_6pwm/open_loop_velocity_6pwm.ino +++ b/examples/hardware_specific_examples/Teensy/Teensy3/open_loop_velocity_6pwm/open_loop_velocity_6pwm.ino @@ -18,6 +18,12 @@ void doLimit(char* cmd) { command.scalar(&motor.voltage_limit, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // driver config // power supply voltage [V] driver.voltage_power_supply = 12; @@ -32,7 +38,7 @@ void setup() { // limiting motor movements // limit the voltage to be set to the motor // start very low for high resistance motors - // currnet = resistance*voltage, so try to be well under 1Amp + // currnet = voltage/resistance, so try to be well under 1Amp motor.voltage_limit = 3; // [V] // open loop control config @@ -48,7 +54,6 @@ void setup() { command.add('T', doTarget, "target velocity"); command.add('L', doLimit, "voltage limit"); - Serial.begin(115200); Serial.println("Motor ready!"); Serial.println("Set target velocity [rad/s]"); _delay(1000); diff --git a/examples/hardware_specific_examples/Teensy/Teensy4/open_loop_velocity_6pwm/open_loop_velocity_6pwm.ino b/examples/hardware_specific_examples/Teensy/Teensy4/open_loop_velocity_6pwm/open_loop_velocity_6pwm.ino index 9c6eea03..c4627020 100644 --- a/examples/hardware_specific_examples/Teensy/Teensy4/open_loop_velocity_6pwm/open_loop_velocity_6pwm.ino +++ b/examples/hardware_specific_examples/Teensy/Teensy4/open_loop_velocity_6pwm/open_loop_velocity_6pwm.ino @@ -33,6 +33,12 @@ void doLimit(char* cmd) { command.scalar(&motor.voltage_limit, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // driver config // power supply voltage [V] driver.voltage_power_supply = 12; @@ -47,7 +53,7 @@ void setup() { // limiting motor movements // limit the voltage to be set to the motor // start very low for high resistance motors - // currnet = resistance*voltage, so try to be well under 1Amp + // currnet = voltage/resistance, so try to be well under 1Amp motor.voltage_limit = 3; // [V] // open loop control config @@ -63,7 +69,6 @@ void setup() { command.add('T', doTarget, "target velocity"); command.add('L', doLimit, "voltage limit"); - Serial.begin(115200); Serial.println("Motor ready!"); Serial.println("Set target velocity [rad/s]"); _delay(1000); diff --git a/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino b/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino index 5a915dd8..6db1b979 100644 --- a/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino +++ b/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino @@ -23,6 +23,12 @@ void doVelocity(char* cmd) { command.scalar(&motor.velocity_limit, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // driver config // power supply voltage [V] driver.voltage_power_supply = 12; @@ -30,14 +36,17 @@ void setup() { // as a protection measure for the low-resistance motors // this value is fixed on startup driver.voltage_limit = 6; - driver.init(); + if(!driver.init()){ + Serial.println("Driver init failed!"); + return; + } // link the motor and the driver motor.linkDriver(&driver); // limiting motor movements // limit the voltage to be set to the motor // start very low for high resistance motors - // currnet = resistance*voltage, so try to be well under 1Amp + // currnet = voltage/resistance, so try to be well under 1Amp motor.voltage_limit = 3; // [V] // limit/set the velocity of the transition in between // target angles @@ -46,14 +55,16 @@ void setup() { motor.controller = MotionControlType::angle_openloop; // init motor hardware - motor.init(); + if(!motor.init()){ + Serial.println("Motor init failed!"); + return; + } // add target command T command.add('T', doTarget, "target angle"); command.add('L', doLimit, "voltage limit"); command.add('V', doLimit, "movement velocity"); - Serial.begin(115200); Serial.println("Motor ready!"); Serial.println("Set target position [rad]"); _delay(1000); @@ -67,4 +78,4 @@ void loop() { // user communication command.run(); -} \ No newline at end of file +} diff --git a/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino b/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino index 43fc6f73..b1bc2760 100644 --- a/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino +++ b/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino @@ -23,6 +23,12 @@ void doLimit(char* cmd) { command.scalar(&motor.voltage_limit, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // driver config // power supply voltage [V] driver.voltage_power_supply = 12; @@ -30,7 +36,10 @@ void setup() { // as a protection measure for the low-resistance motors // this value is fixed on startup driver.voltage_limit = 6; - driver.init(); + if(!driver.init()){ + Serial.println("Driver init failed!"); + return; + } // link the motor and the driver motor.linkDriver(&driver); @@ -44,13 +53,15 @@ void setup() { motor.controller = MotionControlType::velocity_openloop; // init motor hardware - motor.init(); + if(!motor.init()){ + Serial.println("Motor init failed!"); + return; + } // add target command T command.add('T', doTarget, "target velocity"); command.add('L', doLimit, "voltage limit"); - Serial.begin(115200); Serial.println("Motor ready!"); Serial.println("Set target velocity [rad/s]"); _delay(1000); diff --git a/examples/motion_control/position_motion_control/encoder/angle_control/angle_control.ino b/examples/motion_control/position_motion_control/encoder/angle_control/angle_control.ino index ccb3980e..03e6ea75 100644 --- a/examples/motion_control/position_motion_control/encoder/angle_control/angle_control.ino +++ b/examples/motion_control/position_motion_control/encoder/angle_control/angle_control.ino @@ -48,6 +48,12 @@ void doTarget(char* cmd) { command.scalar(&target_angle, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -88,9 +94,6 @@ void setup() { // maximal velocity of the position control motor.velocity_limit = 4; - - // use monitoring with serial - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/motion_control/position_motion_control/magnetic_sensor/angle_control/angle_control.ino b/examples/motion_control/position_motion_control/magnetic_sensor/angle_control/angle_control.ino index 752030c9..f0f9b27c 100644 --- a/examples/motion_control/position_motion_control/magnetic_sensor/angle_control/angle_control.ino +++ b/examples/motion_control/position_motion_control/magnetic_sensor/angle_control/angle_control.ino @@ -31,6 +31,12 @@ void doTarget(char* cmd) { command.scalar(&target_angle, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialise magnetic sensor hardware sensor.init(); // link the motor to the sensor @@ -67,9 +73,7 @@ void setup() { motor.P_angle.P = 20; // maximal velocity of the position control motor.velocity_limit = 20; - - // use monitoring with serial - Serial.begin(115200); + // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/motion_control/torque_control/encoder/current_control/current_control.ino b/examples/motion_control/torque_control/encoder/current_control/current_control.ino index c9123401..82aec86a 100644 --- a/examples/motion_control/torque_control/encoder/current_control/current_control.ino +++ b/examples/motion_control/torque_control/encoder/current_control/current_control.ino @@ -27,6 +27,12 @@ void doTarget(char* cmd) { command.scalar(&target_current, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -70,8 +76,6 @@ void setup() { // motor.LPF_current_q.Tf = 0.002f; // 1ms default // motor.LPF_current_d.Tf = 0.002f; // 1ms default - // use monitoring with serial - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/motion_control/torque_control/encoder/voltage_control/voltage_control.ino b/examples/motion_control/torque_control/encoder/voltage_control/voltage_control.ino index 66ff4569..219637c9 100644 --- a/examples/motion_control/torque_control/encoder/voltage_control/voltage_control.ino +++ b/examples/motion_control/torque_control/encoder/voltage_control/voltage_control.ino @@ -33,6 +33,12 @@ void doTarget(char* cmd) { command.scalar(&target_voltage, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -55,8 +61,6 @@ void setup() { // set motion control loop to be used motor.controller = MotionControlType::torque; - // use monitoring with serial - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/motion_control/torque_control/hall_sensor/voltage_control/voltage_control.ino b/examples/motion_control/torque_control/hall_sensor/voltage_control/voltage_control.ino index 7273d156..c72c9224 100644 --- a/examples/motion_control/torque_control/hall_sensor/voltage_control/voltage_control.ino +++ b/examples/motion_control/torque_control/hall_sensor/voltage_control/voltage_control.ino @@ -35,6 +35,12 @@ void doTarget(char* cmd) { command.scalar(&target_voltage, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware sensor.init(); sensor.enableInterrupts(doA, doB, doC); @@ -57,8 +63,6 @@ void setup() { // set motion control loop to be used motor.controller = MotionControlType::torque; - // use monitoring with serial - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/motion_control/torque_control/magnetic_sensor/voltage_control/voltage_control.ino b/examples/motion_control/torque_control/magnetic_sensor/voltage_control/voltage_control.ino index d869f4dd..dbb56080 100644 --- a/examples/motion_control/torque_control/magnetic_sensor/voltage_control/voltage_control.ino +++ b/examples/motion_control/torque_control/magnetic_sensor/voltage_control/voltage_control.ino @@ -30,6 +30,12 @@ void doTarget(char* cmd) { command.scalar(&target_voltage, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialise magnetic sensor hardware sensor.init(); // link the motor to the sensor @@ -47,8 +53,6 @@ void setup() { // set motion control loop to be used motor.controller = MotionControlType::torque; - // use monitoring with serial - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/motion_control/velocity_motion_control/hall_sensor/velocity_control/velocity_control.ino b/examples/motion_control/velocity_motion_control/hall_sensor/velocity_control/velocity_control.ino index 1d39173a..4fb47f02 100644 --- a/examples/motion_control/velocity_motion_control/hall_sensor/velocity_control/velocity_control.ino +++ b/examples/motion_control/velocity_motion_control/hall_sensor/velocity_control/velocity_control.ino @@ -47,6 +47,12 @@ void doTarget(char* cmd) { command.scalar(&target_velocity, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize sensor sensor hardware sensor.init(); sensor.enableInterrupts(doA, doB); //, doC); @@ -84,8 +90,6 @@ void setup() { // velocity low pass filtering time constant motor.LPF_velocity.Tf = 0.01f; - // use monitoring with serial - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/motion_control/velocity_motion_control/magnetic_sensor/velocity_control/velocity_control.ino b/examples/motion_control/velocity_motion_control/magnetic_sensor/velocity_control/velocity_control.ino index 98e09425..40299b8f 100644 --- a/examples/motion_control/velocity_motion_control/magnetic_sensor/velocity_control/velocity_control.ino +++ b/examples/motion_control/velocity_motion_control/magnetic_sensor/velocity_control/velocity_control.ino @@ -33,6 +33,12 @@ void doTarget(char* cmd) { command.scalar(&target_velocity, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialise magnetic sensor hardware sensor.init(); // link the motor to the sensor @@ -66,8 +72,6 @@ void setup() { // the lower the less filtered motor.LPF_velocity.Tf = 0.01f; - // use monitoring with serial - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino b/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino index 186d257e..d6ddc6b4 100644 --- a/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino +++ b/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino @@ -34,6 +34,12 @@ void onMotor(char* cmd){ command.motor(&motor, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -68,9 +74,6 @@ void setup() { // angle loop velocity limit motor.velocity_limit = 50; - // use monitoring with serial for motor init - // monitoring port - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/motor_commands_serial_examples/hall_sensor/full_control_serial/full_control_serial.ino b/examples/motor_commands_serial_examples/hall_sensor/full_control_serial/full_control_serial.ino index df7b76ac..ff6fe7f3 100644 --- a/examples/motor_commands_serial_examples/hall_sensor/full_control_serial/full_control_serial.ino +++ b/examples/motor_commands_serial_examples/hall_sensor/full_control_serial/full_control_serial.ino @@ -41,6 +41,12 @@ void onMotor(char* cmd){ command.motor(&motor, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware sensor.init(); sensor.enableInterrupts(doA, doB); //, doC); @@ -76,9 +82,6 @@ void setup() { // angle loop velocity limit motor.velocity_limit = 50; - // use monitoring with serial for motor init - // monitoring port - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/motor_commands_serial_examples/magnetic_sensor/full_control_serial/full_control_serial.ino b/examples/motor_commands_serial_examples/magnetic_sensor/full_control_serial/full_control_serial.ino index 098f6888..e863d8c9 100644 --- a/examples/motor_commands_serial_examples/magnetic_sensor/full_control_serial/full_control_serial.ino +++ b/examples/motor_commands_serial_examples/magnetic_sensor/full_control_serial/full_control_serial.ino @@ -33,6 +33,12 @@ void onMotor(char* cmd){ command.motor(&motor, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialise magnetic sensor hardware sensor.init(); // link the motor to the sensor @@ -66,9 +72,6 @@ void setup() { // angle loop velocity limit motor.velocity_limit = 50; - // use monitoring with serial for motor init - // monitoring port - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/osc_control_examples/osc_esp32_3pwm/osc_esp32_3pwm.ino b/examples/osc_control_examples/osc_esp32_3pwm/osc_esp32_3pwm.ino index afb95294..14d4867c 100644 --- a/examples/osc_control_examples/osc_esp32_3pwm/osc_esp32_3pwm.ino +++ b/examples/osc_control_examples/osc_esp32_3pwm/osc_esp32_3pwm.ino @@ -67,8 +67,12 @@ BLDCDriver3PWM driver = BLDCDriver3PWM(25, 26, 27); Commander command = Commander(Serial); void setup() { + // use monitoring with serial Serial.begin(115200); - + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + WiFi.begin(ssid, pass); Serial.print("Connecting WiFi "); diff --git a/examples/osc_control_examples/osc_esp32_fullcontrol/osc_esp32_fullcontrol.ino b/examples/osc_control_examples/osc_esp32_fullcontrol/osc_esp32_fullcontrol.ino index 7c6aa4f0..940b59c2 100644 --- a/examples/osc_control_examples/osc_esp32_fullcontrol/osc_esp32_fullcontrol.ino +++ b/examples/osc_control_examples/osc_esp32_fullcontrol/osc_esp32_fullcontrol.ino @@ -104,7 +104,12 @@ void setup() { digitalWrite(26, 0); digitalWrite(27, 0); + // use monitoring with serial Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + delay(200); WiFi.begin(ssid, pass); diff --git a/examples/utils/calibration/alignment_and_cogging_test/alignment_and_cogging_test.ino b/examples/utils/calibration/alignment_and_cogging_test/alignment_and_cogging_test.ino index 7324edf0..9c523c22 100644 --- a/examples/utils/calibration/alignment_and_cogging_test/alignment_and_cogging_test.ino +++ b/examples/utils/calibration/alignment_and_cogging_test/alignment_and_cogging_test.ino @@ -80,8 +80,11 @@ void testAlignmentAndCogging(int direction) { void setup() { + // use monitoring with serial Serial.begin(115200); - while (!Serial) ; + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); // driver config driver.voltage_power_supply = 12; 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..c39657b1 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,12 +36,18 @@ 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); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware sensor.init(); sensor.enableInterrupts(doA, doB); @@ -62,8 +68,6 @@ void setup() { // set motion control loop to be used motor.controller = MotionControlType::torque; - // use monitoring with serial - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); 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..4eadd376 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,12 +33,18 @@ 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); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware sensor.init(); sensor.enableInterrupts(doA, doB, doC); @@ -58,9 +64,7 @@ void setup() { // set motion control loop to be used motor.controller = MotionControlType::torque; - - // use monitoring with serial - Serial.begin(115200); + // comment out if not needed motor.useMonitoring(Serial); 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..1df631af 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,12 +31,18 @@ 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); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware sensor.init(); // link the motor to the sensor @@ -56,8 +62,6 @@ void setup() { // set motion control loop to be used motor.controller = MotionControlType::torque; - // use monitoring with serial - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/utils/calibration/find_pole_pair_number/encoder/find_pole_pairs_number/find_pole_pairs_number.ino b/examples/utils/calibration/find_pole_pair_number/encoder/find_pole_pairs_number/find_pole_pairs_number.ino index aac879cd..ad8e69ce 100644 --- a/examples/utils/calibration/find_pole_pair_number/encoder/find_pole_pairs_number/find_pole_pairs_number.ino +++ b/examples/utils/calibration/find_pole_pair_number/encoder/find_pole_pairs_number/find_pole_pairs_number.ino @@ -32,6 +32,12 @@ void doB(){encoder.handleB();} void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialise encoder hardware encoder.init(); // hardware interrupt enable @@ -48,8 +54,6 @@ void setup() { // initialize motor motor.init(); - // monitoring port - Serial.begin(115200); // pole pairs calculation routine Serial.println("Pole pairs (PP) estimator"); diff --git a/examples/utils/calibration/find_pole_pair_number/magnetic_sensor/find_pole_pairs_number/find_pole_pairs_number.ino b/examples/utils/calibration/find_pole_pair_number/magnetic_sensor/find_pole_pairs_number/find_pole_pairs_number.ino index e98a4529..b44bc0bb 100644 --- a/examples/utils/calibration/find_pole_pair_number/magnetic_sensor/find_pole_pairs_number/find_pole_pairs_number.ino +++ b/examples/utils/calibration/find_pole_pair_number/magnetic_sensor/find_pole_pairs_number/find_pole_pairs_number.ino @@ -33,6 +33,12 @@ MagneticSensorSPI sensor = MagneticSensorSPI(10, 14, 0x3FFF); void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialise magnetic sensor hardware sensor.init(); // link the motor to the sensor @@ -47,9 +53,6 @@ void setup() { // initialize motor hardware motor.init(); - // monitoring port - Serial.begin(115200); - // pole pairs calculation routine Serial.println("Pole pairs (PP) estimator"); Serial.println("-\n"); diff --git a/examples/utils/calibration/find_sensor_offset_and_direction/find_sensor_offset_and_direction.ino b/examples/utils/calibration/find_sensor_offset_and_direction/find_sensor_offset_and_direction.ino index 27163dfb..407469fc 100644 --- a/examples/utils/calibration/find_sensor_offset_and_direction/find_sensor_offset_and_direction.ino +++ b/examples/utils/calibration/find_sensor_offset_and_direction/find_sensor_offset_and_direction.ino @@ -29,6 +29,12 @@ BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // power supply voltage driver.voltage_power_supply = 12; driver.init(); @@ -54,7 +60,6 @@ void setup() { motor.initFOC(); - Serial.begin(115200); Serial.println("Sensor zero offset is:"); Serial.println(motor.zero_electric_angle, 4); Serial.println("Sensor natural direction is: "); diff --git a/examples/utils/calibration/measure_inductance_and_resistance/measure_inductance_and_resistance.ino b/examples/utils/calibration/measure_inductance_and_resistance/measure_inductance_and_resistance.ino new file mode 100644 index 00000000..83a70a80 --- /dev/null +++ b/examples/utils/calibration/measure_inductance_and_resistance/measure_inductance_and_resistance.ino @@ -0,0 +1,119 @@ +/** + * + * Motor characterisation example sketch. + * + */ +#include + + +// BLDC motor & driver instance +BLDCMotor motor = BLDCMotor(11); +BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); + +// encoder instance +Encoder encoder = Encoder(2, 3, 500); +// channel A and B callbacks +void doA(){encoder.handleA();} +void doB(){encoder.handleB();} + +// current sensor +InlineCurrentSense current_sense = InlineCurrentSense(0.01f, 50.0f, A0, A2); + +// characterisation voltage set point variable +float characteriseVolts = 0.0f; + +// instantiate the commander +Commander command = Commander(Serial); +void onMotor(char* cmd){command.motor(&motor,cmd);} +void characterise(char* cmd) { + command.scalar(&characteriseVolts, cmd); + motor.characteriseMotor(characteriseVolts); +} + +void setup() { + + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + + // initialize encoder sensor hardware + encoder.init(); + encoder.enableInterrupts(doA, doB); + // link the motor to the sensor + motor.linkSensor(&encoder); + + // driver config + // power supply voltage [V] + driver.voltage_power_supply = 12; + driver.init(); + // link driver + motor.linkDriver(&driver); + // link current sense and the driver + current_sense.linkDriver(&driver); + + // current sense init hardware + current_sense.init(); + // link the current sense to the motor + motor.linkCurrentSense(¤t_sense); + + // set torque mode: + // TorqueControlType::dc_current + // TorqueControlType::voltage + // TorqueControlType::foc_current + motor.torque_controller = TorqueControlType::foc_current; + // set motion control loop to be used + motor.controller = MotionControlType::torque; + + // foc current control parameters (Arduino UNO/Mega) + motor.PID_current_q.P = 5; + motor.PID_current_q.I= 300; + motor.PID_current_d.P= 5; + motor.PID_current_d.I = 300; + motor.LPF_current_q.Tf = 0.01f; + motor.LPF_current_d.Tf = 0.01f; + // foc current control parameters (stm/esp/due/teensy) + // motor.PID_current_q.P = 5; + // motor.PID_current_q.I= 1000; + // motor.PID_current_d.P= 5; + // motor.PID_current_d.I = 1000; + // motor.LPF_current_q.Tf = 0.002f; // 1ms default + // motor.LPF_current_d.Tf = 0.002f; // 1ms default + + // comment out if not needed + motor.useMonitoring(Serial); + + // initialize motor + motor.init(); + // align sensor and start FOC + motor.initFOC(); + + // add commands M & L + command.add('M',&onMotor,"Control motor"); + command.add('L', characterise, "Characterise motor L & R with the given voltage"); + + motor.disable(); + + Serial.println(F("Motor disabled and ready.")); + Serial.println(F("Control the motor and measure the inductance using the terminal. Type \"?\" for available commands:")); + _delay(1000); +} + +void loop() { + + // main FOC algorithm function + // the faster you run this function the better + // Arduino UNO loop ~1kHz + // Bluepill loop ~10kHz + motor.loopFOC(); + + // Motion control function + // velocity, position or torque (defined in motor.controller) + // this function can be run at much lower frequency than loopFOC() function + // You can also use motor.move() and set the motor.target in the code + motor.move(); + + // user communication + command.run(); +} \ No newline at end of file diff --git a/examples/utils/communication_test/commander/commander_tune_custom_loop/commander_tune_custom_loop.ino b/examples/utils/communication_test/commander/commander_tune_custom_loop/commander_tune_custom_loop.ino index cbeb6ecb..074538ad 100644 --- a/examples/utils/communication_test/commander/commander_tune_custom_loop/commander_tune_custom_loop.ino +++ b/examples/utils/communication_test/commander/commander_tune_custom_loop/commander_tune_custom_loop.ino @@ -29,6 +29,12 @@ void doTarget(char* cmd) { command.scalar(&target_velocity, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -46,7 +52,6 @@ void setup() { motor.controller = MotionControlType::torque; // use monitoring with serial - Serial.begin(115200); motor.useMonitoring(Serial); // initialize motor motor.init(); diff --git a/examples/utils/communication_test/step_dir/step_dir_motor_example/step_dir_motor_example.ino b/examples/utils/communication_test/step_dir/step_dir_motor_example/step_dir_motor_example.ino index f8978577..4cd87970 100644 --- a/examples/utils/communication_test/step_dir/step_dir_motor_example/step_dir_motor_example.ino +++ b/examples/utils/communication_test/step_dir/step_dir_motor_example/step_dir_motor_example.ino @@ -24,6 +24,12 @@ void onStep() { step_dir.handle(); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -64,9 +70,7 @@ void setup() { motor.P_angle.P = 10; // maximal velocity of the position control motor.velocity_limit = 100; - - // use monitoring with serial - Serial.begin(115200); + // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/utils/current_sense_test/align_current_sense/align_current_sense.ino b/examples/utils/current_sense_test/align_current_sense/align_current_sense.ino new file mode 100644 index 00000000..3d51215e --- /dev/null +++ b/examples/utils/current_sense_test/align_current_sense/align_current_sense.ino @@ -0,0 +1,181 @@ +/** + * This is an example code for visual aligning current sense and the driver phases as well + * it is used to test the current sense implementation. + * + * In this example it uses the BLDCMotor and BLDCDriver3PWM classes to control a BLDC motor + * and the InlineCurrentSense class to read the phase currents. + * > In your application you can use any other motor, driver and current sense implementation. + * > The rest of the code will stay the same + * + * The example uses the teleplot (https://teleplot.fr) service to visualize the phase currents and voltages. + * Its really awesome tool and you can use it to visualize any data you want. + */ +#include + +// BLDC motor & driver instance +// NOTE: replace with your motor and driver configuration +BLDCMotor motor = BLDCMotor(11); +BLDCDriver3PWM driver = BLDCDriver3PWM(6, 10, 5, 8); + +// Current sense instance +// NOTE: replace with your current sense configuration +// inline current sensor instance +// ACS712-05B has the resolution of 0.185mV per Amp +InlineCurrentSense current_sense = InlineCurrentSense(185.0f, A0, A2); +// or some other current sense +// LowsideCurrentSense current_sense = LowsideCurrentSense(185.0f, A0, A2); // ex. lowside current sense + +// commander communication instance +Commander command = Commander(Serial); + + +bool start = false; // flag to start printing phase currents and voltages +float frequency = 10000; // frequency of printing phase currents and voltages + +void doStart(char* cmd){ + // toggle the start flag + start = !start; + if(start){ + SIMPLEFOC_DEBUG("Start printing phase currents and voltages"); + } else { + SIMPLEFOC_DEBUG("Stop printing phase currents and voltages"); + } +} + +void doCurrentA(char* cmd){ + SIMPLEFOC_DEBUG("Inverted cs A gain"); + current_sense.gain_a = -current_sense.gain_a; + SIMPLEFOC_DEBUG("New gain A: ", current_sense.gain_a); +} +void doCurrentB(char* cmd){ + SIMPLEFOC_DEBUG("Inverted cs B gain"); + current_sense.gain_b = -current_sense.gain_b; + SIMPLEFOC_DEBUG("New gain B: ", current_sense.gain_b); +} +void doCurrentC(char* cmd){ + SIMPLEFOC_DEBUG("Inverted cs C gain"); + current_sense.gain_c = -current_sense.gain_c; + SIMPLEFOC_DEBUG("New gain C: ", current_sense.gain_c); +} + +void doMotorLimit(char* cmd){ + // set the voltage limit for the motor + command.scalar(&motor.voltage_limit, cmd); +} + +void doTarget(char* cmd){ + // set the target value for the motor + command.scalar(&motor.target, cmd); +} + +void doFrequency(char* cmd){ + // set the frequency of printing phase currents and voltages + command.scalar(&frequency, cmd); +} + +void setup() { + + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + + // driver config + // power supply voltage [V] + driver.voltage_power_supply = 20; + driver.init(); + // link driver + motor.linkDriver(&driver); + // link current sense and the driver + current_sense.linkDriver(&driver); + + // set control loop type to be used + motor.controller = MotionControlType::velocity_openloop; + + motor.voltage_limit = 1; // voltage limit for the motor + + // initialise motor + motor.init(); + + // current sense init and linking + current_sense.init(); + motor.linkCurrentSense(¤t_sense); + + // align encoder and start FOC + motor.initFOC(); + + // set the inital target value + motor.target = 0.5; + + + // subscribe motor to the commander + //command.add('T', doMotion, "motion control"); + command.add('A', doCurrentA, "Invert cs A gain"); + command.add('B', doCurrentB, "Invert cs B gain"); + command.add('C', doCurrentC, "Invert cs C gain"); + command.add('L', doMotorLimit, "Set motor voltage limit"); + command.add('T', doTarget, "Set motor target"); + command.add('S', doStart, "Start/Stop printing phase currents and voltages"); + command.add('F', doFrequency, "Set frequency of printing phase currents and voltages"); + + SIMPLEFOC_DEBUG("To use this example:"); + SIMPLEFOC_DEBUG(" - use 'L' to control the motor voltage limit"); + SIMPLEFOC_DEBUG(" - use 'T' to set the motor target"); + SIMPLEFOC_DEBUG(" - use 'A', 'B', 'C' to invert current sense gains"); + SIMPLEFOC_DEBUG(" - use 'F' to set frequency of printing phase currents and voltages (100Hz by default)"); + SIMPLEFOC_DEBUG(" - use 'S' to start/stop printing phase currents and voltages"); + SIMPLEFOC_DEBUG("IMPORTANT: Use teleplot to visualize the phase currents and voltages: https://teleplot.fr/"); + + _delay(1000); + +} + +float normalize_voltage(float v){ + return (v - driver.voltage_power_supply/2.0)/motor.voltage_limit; +} + +float max_current = 0.0f; // max current for normalization +LowPassFilter lp_filter_maxc(0.3f); // low pass filter for current normalization +void normalize_currents(PhaseCurrent_s& c, float& max_current){ + static unsigned long timestamp = _micros(); + // normalize current to the max current + + float m_current = 0.0f; + if(fabs(c.a) > m_current) m_current = fabs(c.a); + if(fabs(c.b) > m_current) m_current = fabs(c.b); + if(fabs(c.c) > m_current) m_current = fabs(c.c); + // filter the max current + max_current = lp_filter_maxc(m_current); + + c.a = c.a / max_current; + c.b = c.b / max_current; + c.c = c.c / max_current; +} + +unsigned long t = _micros(); + +void loop() { + motor.loopFOC(); + motor.move(); + + // print each + if( start & (_micros() - t > (1.0/frequency * 1e6))){ + // read phase currents + PhaseCurrent_s currents = current_sense.getPhaseCurrents(); + // normalize currents + normalize_currents(currents, max_current); + // print phase currents + SIMPLEFOC_DEBUG(">c.a:",currents.a); + SIMPLEFOC_DEBUG(">c.b:",currents.b); + SIMPLEFOC_DEBUG(">c.c:",currents.c); + // print phase voltages + SIMPLEFOC_DEBUG(">v.a:",normalize_voltage(motor.Ua)); + SIMPLEFOC_DEBUG(">v.b:",normalize_voltage(motor.Ub)); + SIMPLEFOC_DEBUG(">v.c:",normalize_voltage(motor.Uc)); + t = _micros(); + } + + // user communication + command.run(); +} \ No newline at end of file diff --git a/examples/utils/current_sense_test/generic_current_sense/generic_current_sense.ino b/examples/utils/current_sense_test/generic_current_sense/generic_current_sense.ino index e999de23..2448a51c 100644 --- a/examples/utils/current_sense_test/generic_current_sense/generic_current_sense.ino +++ b/examples/utils/current_sense_test/generic_current_sense/generic_current_sense.ino @@ -31,6 +31,14 @@ GenericCurrentSense current_sense = GenericCurrentSense(readCurrentSense, initCu void setup() { + + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + + // if callbacks are not provided in the constructor // they can be assigned directly: //current_sense.readCallback = readCurrentSense; @@ -40,7 +48,6 @@ void setup() { current_sense.init(); - Serial.begin(115200); Serial.println("Current sense ready."); } diff --git a/examples/utils/current_sense_test/inline_current_sense_test/inline_current_sense_test.ino b/examples/utils/current_sense_test/inline_current_sense_test/inline_current_sense_test.ino index c1a5139b..1198cdcd 100644 --- a/examples/utils/current_sense_test/inline_current_sense_test/inline_current_sense_test.ino +++ b/examples/utils/current_sense_test/inline_current_sense_test/inline_current_sense_test.ino @@ -11,13 +11,22 @@ InlineCurrentSense current_sense = InlineCurrentSense(0.01f, 50.0f, A0, A2); void setup() { + + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialise the current sensing - current_sense.init(); + if(!current_sense.init()){ + Serial.println("Current sense init failed."); + return; + } // for SimpleFOCShield v2.01/v2.0.2 current_sense.gain_b *= -1; - Serial.begin(115200); Serial.println("Current sense ready."); } diff --git a/examples/utils/driver_standalone_test/bldc_driver_3pwm_standalone/bldc_driver_3pwm_standalone.ino b/examples/utils/driver_standalone_test/bldc_driver_3pwm_standalone/bldc_driver_3pwm_standalone.ino index 1235fccd..eef793d7 100644 --- a/examples/utils/driver_standalone_test/bldc_driver_3pwm_standalone/bldc_driver_3pwm_standalone.ino +++ b/examples/utils/driver_standalone_test/bldc_driver_3pwm_standalone/bldc_driver_3pwm_standalone.ino @@ -7,6 +7,12 @@ BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // pwm frequency to be used [Hz] // for atmega328 fixed to 32kHz // esp32/stm32/teensy configurable @@ -17,11 +23,14 @@ void setup() { driver.voltage_limit = 12; // driver init - driver.init(); + if (!driver.init()){ + Serial.println("Driver init failed!"); + return; + } // enable driver driver.enable(); - + Serial.println("Driver ready!"); _delay(1000); } diff --git a/examples/utils/driver_standalone_test/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino b/examples/utils/driver_standalone_test/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino index 478d3974..56a1afbe 100644 --- a/examples/utils/driver_standalone_test/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino +++ b/examples/utils/driver_standalone_test/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino @@ -6,6 +6,12 @@ BLDCDriver6PWM driver = BLDCDriver6PWM(5, 6, 9,10, 3, 11, 8); void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // pwm frequency to be used [Hz] // for atmega328 fixed to 32kHz // esp32/stm32/teensy configurable @@ -18,11 +24,14 @@ void setup() { driver.dead_zone = 0.05f; // driver init - driver.init(); + if (!driver.init()){ + Serial.println("Driver init failed!"); + return; + } // enable driver driver.enable(); - + Serial.println("Driver ready!"); _delay(1000); } diff --git a/examples/utils/driver_standalone_test/stepper_driver_2pwm_standalone/stepper_driver_2pwm_standalone.ino b/examples/utils/driver_standalone_test/stepper_driver_2pwm_standalone/stepper_driver_2pwm_standalone.ino index 4627efa9..59343a14 100644 --- a/examples/utils/driver_standalone_test/stepper_driver_2pwm_standalone/stepper_driver_2pwm_standalone.ino +++ b/examples/utils/driver_standalone_test/stepper_driver_2pwm_standalone/stepper_driver_2pwm_standalone.ino @@ -13,6 +13,12 @@ StepperDriver2PWM driver = StepperDriver2PWM(3, in1, 10 , in2, 11, 12); void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // pwm frequency to be used [Hz] // for atmega328 fixed to 32kHz // esp32/stm32/teensy configurable @@ -23,11 +29,14 @@ void setup() { driver.voltage_limit = 12; // driver init - driver.init(); + if (!driver.init()){ + Serial.println("Driver init failed!"); + return; + } // enable driver driver.enable(); - + Serial.println("Driver ready!"); _delay(1000); } diff --git a/examples/utils/driver_standalone_test/stepper_driver_4pwm_standalone/stepper_driver_4pwm_standalone.ino b/examples/utils/driver_standalone_test/stepper_driver_4pwm_standalone/stepper_driver_4pwm_standalone.ino index e028a737..a58d7940 100644 --- a/examples/utils/driver_standalone_test/stepper_driver_4pwm_standalone/stepper_driver_4pwm_standalone.ino +++ b/examples/utils/driver_standalone_test/stepper_driver_4pwm_standalone/stepper_driver_4pwm_standalone.ino @@ -8,6 +8,12 @@ StepperDriver4PWM driver = StepperDriver4PWM(5, 6, 9,10, 7, 8); void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // pwm frequency to be used [Hz] // for atmega328 fixed to 32kHz // esp32/stm32/teensy configurable @@ -18,11 +24,14 @@ void setup() { driver.voltage_limit = 12; // driver init - driver.init(); + if (!driver.init()){ + Serial.println("Driver init failed!"); + return; + } // enable driver driver.enable(); - + Serial.println("Driver ready!"); _delay(1000); } diff --git a/examples/utils/sensor_test/hall_sensors/hall_sensor_example/hall_sensor_example.ino b/examples/utils/sensor_test/hall_sensors/hall_sensor_example/hall_sensor_example.ino index c4777baa..cc8dfdb8 100644 --- a/examples/utils/sensor_test/hall_sensors/hall_sensor_example/hall_sensor_example.ino +++ b/examples/utils/sensor_test/hall_sensors/hall_sensor_example/hall_sensor_example.ino @@ -13,13 +13,6 @@ // - pp - pole pairs HallSensor sensor = HallSensor(2, 3, 4, 14); -// Interrupt routine intialisation -// channel A and B callbacks -void doA(){sensor.handleA();} -void doB(){sensor.handleB();} -void doC(){sensor.handleC();} - - void setup() { // monitoring port Serial.begin(115200); @@ -29,8 +22,6 @@ void setup() { // initialise encoder hardware sensor.init(); - // hardware interrupt enable - sensor.enableInterrupts(doA, doB, doC); Serial.println("Sensor ready"); _delay(1000); diff --git a/examples/utils/sensor_test/hall_sensors/hall_sensor_hardware_interrupts_example/hall_sensor_hardware_interrupts_example.ino b/examples/utils/sensor_test/hall_sensors/hall_sensor_hardware_interrupts_example/hall_sensor_hardware_interrupts_example.ino new file mode 100644 index 00000000..c4777baa --- /dev/null +++ b/examples/utils/sensor_test/hall_sensors/hall_sensor_hardware_interrupts_example/hall_sensor_hardware_interrupts_example.ino @@ -0,0 +1,48 @@ +/** + * Hall sensor example code + * + * This is a code intended to test the hall sensors connections and to demonstrate the hall sensor setup. + * + */ + +#include + +// Hall sensor instance +// HallSensor(int hallA, int hallB , int cpr, int index) +// - hallA, hallB, hallC - HallSensor A, B and C pins +// - pp - pole pairs +HallSensor sensor = HallSensor(2, 3, 4, 14); + +// Interrupt routine intialisation +// channel A and B callbacks +void doA(){sensor.handleA();} +void doB(){sensor.handleB();} +void doC(){sensor.handleC();} + + +void setup() { + // monitoring port + Serial.begin(115200); + + // check if you need internal pullups + sensor.pullup = Pullup::USE_EXTERN; + + // initialise encoder hardware + sensor.init(); + // hardware interrupt enable + sensor.enableInterrupts(doA, doB, doC); + + Serial.println("Sensor ready"); + _delay(1000); +} + +void loop() { + // iterative function updating the sensor internal variables + // it is usually called in motor.loopFOC() + sensor.update(); + // display the angle and the angular velocity to the terminal + Serial.print(sensor.getAngle()); + Serial.print("\t"); + Serial.println(sensor.getVelocity()); + delay(100); +} diff --git a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c/magnetic_sensor_i2c_dual_bus_examples/esp32_i2c_dual_bus_example/esp32_i2c_dual_bus_example.ino b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c/magnetic_sensor_i2c_dual_bus_examples/esp32_i2c_dual_bus_example/esp32_i2c_dual_bus_example.ino index 0516ede1..a25e1c04 100644 --- a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c/magnetic_sensor_i2c_dual_bus_examples/esp32_i2c_dual_bus_example/esp32_i2c_dual_bus_example.ino +++ b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c/magnetic_sensor_i2c_dual_bus_examples/esp32_i2c_dual_bus_example/esp32_i2c_dual_bus_example.ino @@ -1,4 +1,5 @@ #include +#include /** Annoyingly some i2c sensors (e.g. AS5600) have a fixed chip address. This means only one of these devices can be addressed on a single bus * This example shows how a second i2c bus can be used to communicate with a second sensor. @@ -7,6 +8,8 @@ MagneticSensorI2C sensor0 = MagneticSensorI2C(AS5600_I2C); MagneticSensorI2C sensor1 = MagneticSensorI2C(AS5600_I2C); +// example of esp32 defining 2nd bus, if not already defined +//TwoWire Wire1(1); void setup() { diff --git a/library.properties b/library.properties index 0a2606b2..c50cf1e3 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=Simple FOC -version=2.3.3 +version=2.3.6 author=Simplefoc maintainer=Simplefoc sentence=A library demistifying FOC for BLDC motors diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index 9dbc0a48..d61d2c34 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -63,11 +63,11 @@ void BLDCMotor::linkDriver(BLDCDriver* _driver) { } // init hardware pins -void BLDCMotor::init() { +int BLDCMotor::init() { if (!driver || !driver->initialized) { motor_status = FOCMotorStatus::motor_init_failed; SIMPLEFOC_DEBUG("MOT: Init not possible, driver not initialized"); - return; + return 0; } motor_status = FOCMotorStatus::motor_initializing; SIMPLEFOC_DEBUG("MOT: Init"); @@ -93,10 +93,13 @@ void BLDCMotor::init() { P_angle.limit = velocity_limit; // if using open loop control, set a CW as the default direction if not already set - if ((controller==MotionControlType::angle_openloop - ||controller==MotionControlType::velocity_openloop) - && (sensor_direction == Direction::UNKNOWN)) { - sensor_direction = Direction::CW; + // only if no sensor is used + if(!sensor){ + if ((controller==MotionControlType::angle_openloop + ||controller==MotionControlType::velocity_openloop) + && (sensor_direction == Direction::UNKNOWN)) { + sensor_direction = Direction::CW; + } } _delay(500); @@ -105,6 +108,7 @@ void BLDCMotor::init() { enable(); _delay(500); motor_status = FOCMotorStatus::motor_uncalibrated; + return 1; } @@ -156,25 +160,31 @@ int BLDCMotor::initFOC() { // 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. - 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){ @@ -196,7 +206,7 @@ int BLDCMotor::alignCurrentSense() { SIMPLEFOC_DEBUG("MOT: Align current sense."); // align current sense and the driver - exit_flag = current_sense->driverAlign(voltage_sensor_align); + exit_flag = current_sense->driverAlign(voltage_sensor_align, modulation_centered); if(!exit_flag){ // error in current sense - phase either not measured or bad connection SIMPLEFOC_DEBUG("MOT: Align error!"); @@ -219,6 +229,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){ @@ -226,7 +240,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); } @@ -236,13 +250,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); @@ -270,7 +284,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(); @@ -279,9 +293,7 @@ int BLDCMotor::alignSensor() { zero_electric_angle = electricalAngle(); //zero_electric_angle = _normalizeAngle(_electricalAngle(sensor_direction*sensor->getAngle(), pole_pairs)); _delay(20); - if(monitor_port){ - SIMPLEFOC_DEBUG("MOT: Zero elec. angle: ", zero_electric_angle); - } + SIMPLEFOC_DEBUG("MOT: Zero elec. angle: ", zero_electric_angle); // stop everything setPhaseVoltage(0, 0, 0); _delay(200); @@ -346,23 +358,23 @@ void BLDCMotor::loopFOC() { // read overall current magnitude current.q = current_sense->getDCCurrent(electrical_angle); // filter the value values - current.q = LPF_current_q(current.q); + current.q = LPF_current_q(current.q) + feed_forward_current.q; // calculate the phase voltage - voltage.q = PID_current_q(current_sp - current.q); + voltage.q = PID_current_q(current_sp - current.q)+feed_forward_voltage.q; // d voltage - lag compensation - if(_isset(phase_inductance)) voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit); - else voltage.d = 0; + if(_isset(phase_inductance)) voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance+feed_forward_voltage.d, -voltage_limit, voltage_limit); + else voltage.d = feed_forward_voltage.d; break; case TorqueControlType::foc_current: if(!current_sense) return; // read dq currents current = current_sense->getFOCCurrents(electrical_angle); // filter values - current.q = LPF_current_q(current.q); - current.d = LPF_current_d(current.d); + current.q = LPF_current_q(current.q) + feed_forward_current.q; + current.d = LPF_current_d(current.d) + feed_forward_current.d; // calculate the phase voltages - voltage.q = PID_current_q(current_sp - current.q); - voltage.d = PID_current_d(-current.d); + voltage.q = PID_current_q(current_sp - current.q)+feed_forward_voltage.q; + voltage.d = PID_current_d(-current.d)+feed_forward_voltage.d; // d voltage - lag compensation - TODO verify // if(_isset(phase_inductance)) voltage.d = _constrain( voltage.d - current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit); break; @@ -371,7 +383,6 @@ void BLDCMotor::loopFOC() { SIMPLEFOC_DEBUG("MOT: no torque control selected!"); break; } - // set the phase voltage - FOC heart function :) setPhaseVoltage(voltage.q, voltage.d, electrical_angle); } @@ -383,6 +394,9 @@ void BLDCMotor::loopFOC() { // - if target is not set it uses motor.target value void BLDCMotor::move(float new_target) { + // set internal target variable + if(_isset(new_target)) target = new_target; + // downsampling (optional) if(motion_cnt++ < motion_downsample) return; motion_cnt = 0; @@ -400,8 +414,6 @@ void BLDCMotor::move(float new_target) { // if disabled do nothing if(!enabled) return; - // set internal target variable - 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*_SQRT3)/_RPM_TO_RADS; @@ -437,10 +449,10 @@ void BLDCMotor::move(float new_target) { if(torque_controller == TorqueControlType::voltage){ // use voltage if phase-resistance not provided if(!_isset(phase_resistance)) voltage.q = current_sp; - else voltage.q = _constrain( current_sp*phase_resistance + voltage_bemf , -voltage_limit, voltage_limit); + else voltage.q = _constrain( (current_sp+feed_forward_current.q)*phase_resistance + voltage_bemf, -voltage_limit, voltage_limit); // set d-component (lag compensation if known inductance) if(!_isset(phase_inductance)) voltage.d = 0; - else voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit); + else voltage.d = _constrain( -(current_sp+feed_forward_current.d)*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit); } break; case MotionControlType::velocity: @@ -452,10 +464,10 @@ void BLDCMotor::move(float new_target) { if(torque_controller == TorqueControlType::voltage){ // use voltage if phase-resistance not provided if(!_isset(phase_resistance)) voltage.q = current_sp; - else voltage.q = _constrain( current_sp*phase_resistance + voltage_bemf , -voltage_limit, voltage_limit); + else voltage.q = _constrain( (current_sp+feed_forward_current.q)*phase_resistance + voltage_bemf, -voltage_limit, voltage_limit); // set d-component (lag compensation if known inductance) if(!_isset(phase_inductance)) voltage.d = 0; - else voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit); + else voltage.d = _constrain( -(current_sp+feed_forward_current.d)*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit); } break; case MotionControlType::velocity_openloop: diff --git a/src/BLDCMotor.h b/src/BLDCMotor.h index a1f196ab..386c5f19 100644 --- a/src/BLDCMotor.h +++ b/src/BLDCMotor.h @@ -4,6 +4,7 @@ #include "Arduino.h" #include "common/base_classes/FOCMotor.h" #include "common/base_classes/Sensor.h" +#include "common/base_classes/FOCDriver.h" #include "common/base_classes/BLDCDriver.h" #include "common/foc_utils.h" #include "common/time_utils.h" @@ -39,7 +40,7 @@ class BLDCMotor: public FOCMotor BLDCDriver* driver; /** Motor hardware init function */ - void init() override; + int init() override; /** Motor disable function */ void disable() override; /** Motor enable function */ @@ -69,8 +70,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 @@ -81,6 +81,16 @@ class BLDCMotor: public FOCMotor */ void setPhaseVoltage(float Uq, float Ud, float angle_el) override; + /** + * Measure resistance and inductance of a BLDCMotor and print results to debug. + * If a sensor is available, an estimate of zero electric angle will be reported too. + * @param voltage The voltage applied to the motor + * @returns 0 for success, >0 for failure + */ + int characteriseMotor(float voltage){ + return FOCMotor::characteriseMotor(voltage, 1.5f); + } + private: // FOC methods diff --git a/src/HybridStepperMotor.cpp b/src/HybridStepperMotor.cpp new file mode 100644 index 00000000..618985d9 --- /dev/null +++ b/src/HybridStepperMotor.cpp @@ -0,0 +1,592 @@ +#include "HybridStepperMotor.h" +#include "./communication/SimpleFOCDebug.h" + +// HybridStepperMotor(int pp) +// - pp - pole pair number +// - R - motor phase resistance +// - KV - motor kv rating (rmp/v) +// - L - motor phase inductance [H] +HybridStepperMotor::HybridStepperMotor(int pp, float _R, float _KV, float _inductance) + : FOCMotor() +{ + // number od pole pairs + pole_pairs = pp; + // save phase resistance number + phase_resistance = _R; + // save back emf constant KV = 1/K_bemf + // usually used rms + KV_rating = _KV * _SQRT2; + // save phase inductance + phase_inductance = _inductance; + + // torque control type is voltage by default + // current and foc_current not supported yet + torque_controller = TorqueControlType::voltage; +} + +/** + Link the driver which controls the motor +*/ +void HybridStepperMotor::linkDriver(BLDCDriver *_driver) +{ + driver = _driver; + SIMPLEFOC_DEBUG("MOT: BLDCDriver linked, using pin C as the mid-phase"); +} + +// init hardware pins +int HybridStepperMotor::init() +{ + if (!driver || !driver->initialized) + { + motor_status = FOCMotorStatus::motor_init_failed; + SIMPLEFOC_DEBUG("MOT: Init not possible, driver not initialized"); + return 0; + } + motor_status = FOCMotorStatus::motor_initializing; + SIMPLEFOC_DEBUG("MOT: Init"); + + // sanity check for the voltage limit configuration + if (voltage_limit > driver->voltage_limit) + voltage_limit = driver->voltage_limit; + // constrain voltage for sensor alignment + if (voltage_sensor_align > voltage_limit) + voltage_sensor_align = voltage_limit; + + // update the controller limits + if (_isset(phase_resistance)) + { + // velocity control loop controls current + PID_velocity.limit = current_limit; + } + else + { + PID_velocity.limit = voltage_limit; + } + P_angle.limit = velocity_limit; + + // if using open loop control, set a CW as the default direction if not already set + // only if no sensor is used + if(!sensor){ + if ((controller==MotionControlType::angle_openloop + ||controller==MotionControlType::velocity_openloop) + && (sensor_direction == Direction::UNKNOWN)) { + sensor_direction = Direction::CW; + } + } + + _delay(500); + // enable motor + SIMPLEFOC_DEBUG("MOT: Enable driver."); + enable(); + _delay(500); + + motor_status = FOCMotorStatus::motor_uncalibrated; + return 1; +} + +// disable motor driver +void HybridStepperMotor::disable() +{ + // set zero to PWM + driver->setPwm(0, 0, 0); + // disable driver + driver->disable(); + // motor status update + enabled = 0; +} +// enable motor driver +void HybridStepperMotor::enable() +{ + // disable enable + driver->enable(); + // set zero to PWM + driver->setPwm(0, 0, 0); + // motor status update + enabled = 1; +} + +/** + * FOC functions + */ +int HybridStepperMotor::initFOC() { + int exit_flag = 1; + + motor_status = FOCMotorStatus::motor_calibrating; + + // align motor if necessary + // alignment necessary for encoders! + // sensor and motor alignment - can be skipped + // by setting motor.sensor_direction and motor.zero_electric_angle + if(sensor){ + exit_flag *= alignSensor(); + // added the shaft_angle update + sensor->update(); + shaft_angle = shaftAngle(); + + // 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){ + current_sense->driver_type = DriverType::Hybrid; + 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 + } + } + + if(exit_flag){ + SIMPLEFOC_DEBUG("MOT: Ready."); + motor_status = FOCMotorStatus::motor_ready; + }else{ + SIMPLEFOC_DEBUG("MOT: Init FOC failed."); + motor_status = FOCMotorStatus::motor_calib_failed; + disable(); + } + + return exit_flag; +} + +// Calibrate the motor and current sense phases +int HybridStepperMotor::alignCurrentSense() { + int exit_flag = 1; // success + + SIMPLEFOC_DEBUG("MOT: Align current sense."); + + // align current sense and the driver + exit_flag = current_sense->driverAlign(voltage_sensor_align, modulation_centered); + if(!exit_flag){ + // error in current sense - phase either not measured or bad connection + SIMPLEFOC_DEBUG("MOT: Align error!"); + exit_flag = 0; + }else{ + // output the alignment status flag + SIMPLEFOC_DEBUG("MOT: Success: ", exit_flag); + } + + return exit_flag > 0; +} + +// Encoder alignment to electrical 0 angle +int HybridStepperMotor::alignSensor() +{ + int exit_flag = 1; // success + SIMPLEFOC_DEBUG("MOT: Align sensor."); + + // if unknown natural direction + if(sensor_direction==Direction::UNKNOWN) { + // check if sensor needs zero search + if (sensor->needsSearch()) + exit_flag = absoluteZeroSearch(); + // stop init if not found index + if (!exit_flag) + return exit_flag; + + // find natural direction + // 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); + sensor->update(); + _delay(2); + } + // take and angle in the middle + sensor->update(); + float mid_angle = sensor->getAngle(); + // 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); + sensor->update(); + _delay(2); + } + sensor->update(); + float end_angle = sensor->getAngle(); + setPhaseVoltage(0, 0, 0); + _delay(200); + // determine the direction the sensor moved + if (mid_angle == end_angle) + { + SIMPLEFOC_DEBUG("MOT: Failed to notice movement"); + return 0; // failed calibration + } + else if (mid_angle < end_angle) + { + SIMPLEFOC_DEBUG("MOT: sensor_direction==CCW"); + sensor_direction = Direction::CCW; + } + else + { + SIMPLEFOC_DEBUG("MOT: sensor_direction==CW"); + sensor_direction = Direction::CW; + } + // 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! + SIMPLEFOC_DEBUG("MOT: PP check: fail - estimated pp: ", _2PI / moved); + } + else + SIMPLEFOC_DEBUG("MOT: PP check: OK!"); + } + else + SIMPLEFOC_DEBUG("MOT: Skip dir calib."); + + // zero electric angle not known + 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); + _delay(700); + // read the sensor + sensor->update(); + // get the current zero electric angle + zero_electric_angle = 0; + zero_electric_angle = electricalAngle(); + _delay(20); + if (monitor_port) + { + SIMPLEFOC_DEBUG("MOT: Zero elec. angle: ", zero_electric_angle); + } + // stop everything + setPhaseVoltage(0, 0, 0); + _delay(200); + } + else + SIMPLEFOC_DEBUG("MOT: Skip offset calib."); + return exit_flag; +} + +// Encoder alignment the absolute zero angle +// - to the index +int HybridStepperMotor::absoluteZeroSearch() +{ + + SIMPLEFOC_DEBUG("MOT: Index search..."); + // search the absolute zero with small velocity + float limit_vel = velocity_limit; + float limit_volt = voltage_limit; + velocity_limit = velocity_index_search; + voltage_limit = voltage_sensor_align; + shaft_angle = 0; + while (sensor->needsSearch() && shaft_angle < _2PI) + { + angleOpenloop(1.5f * _2PI); + // call important for some sensors not to loose count + // not needed for the search + sensor->update(); + } + // disable motor + setPhaseVoltage(0, 0, 0); + // reinit the limits + velocity_limit = limit_vel; + voltage_limit = limit_volt; + // check if the zero found + if (monitor_port) + { + if (sensor->needsSearch()) + SIMPLEFOC_DEBUG("MOT: Error: Not found!"); + else + SIMPLEFOC_DEBUG("MOT: Success!"); + } + return !sensor->needsSearch(); +} + +// Iterative function looping FOC algorithm, setting Uq on the Motor +// The faster it can be run the better +void HybridStepperMotor::loopFOC() { + + // update sensor - do this even in open-loop mode, as user may be switching between modes and we could lose track + // of full rotations otherwise. + if (sensor) sensor->update(); + + // if open-loop do nothing + if( controller==MotionControlType::angle_openloop || controller==MotionControlType::velocity_openloop ) return; + + // if disabled do nothing + if(!enabled) return; + + // Needs the update() to be called first + // This function will not have numerical issues because it uses Sensor::getMechanicalAngle() + // which is in range 0-2PI + electrical_angle = electricalAngle(); + switch (torque_controller) { + case TorqueControlType::voltage: + // no need to do anything really + break; + case TorqueControlType::dc_current: + if(!current_sense) return; + // read overall current magnitude + current.q = current_sense->getDCCurrent(electrical_angle); + // filter the value values + current.q = LPF_current_q(current.q); + // calculate the phase voltage + voltage.q = PID_current_q(current_sp - current.q); + // d voltage - lag compensation + if(_isset(phase_inductance)) voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit); + else voltage.d = 0; + break; + case TorqueControlType::foc_current: + if(!current_sense) return; + // read dq currents + current = current_sense->getFOCCurrents(electrical_angle); + // filter values + current.q = LPF_current_q(current.q); + current.d = LPF_current_d(current.d); + // calculate the phase voltages + voltage.q = PID_current_q(current_sp - current.q); + voltage.d = PID_current_d(-current.d); + // d voltage - lag compensation - TODO verify + // if(_isset(phase_inductance)) voltage.d = _constrain( voltage.d - current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit); + break; + default: + // no torque control selected + SIMPLEFOC_DEBUG("MOT: no torque control selected!"); + break; + } + // set the phase voltage - FOC heart function :) + setPhaseVoltage(voltage.q, voltage.d, electrical_angle); +} + +// Iterative function running outer loop of the FOC algorithm +// Behavior of this function is determined by the motor.controller variable +// It runs either angle, velocity or voltage loop +// - needs to be called iteratively it is asynchronous function +// - if target is not set it uses motor.target value +void HybridStepperMotor::move(float new_target) { + + // set internal target variable + if(_isset(new_target) ) target = new_target; + + // downsampling (optional) + if(motion_cnt++ < motion_downsample) return; + motion_cnt = 0; + + // shaft angle/velocity need the update() to be called first + // get shaft angle + // TODO sensor precision: the shaft_angle actually stores the complete position, including full rotations, as a float + // For this reason it is NOT precise when the angles become large. + // Additionally, the way LPF works on angle is a precision issue, and the angle-LPF is a problem + // when switching to a 2-component representation. + if( controller!=MotionControlType::angle_openloop && controller!=MotionControlType::velocity_openloop ) + shaft_angle = shaftAngle(); // read value even if motor is disabled to keep the monitoring updated but not in openloop mode + // get angular velocity + shaft_velocity = shaftVelocity(); // read value even if motor is disabled to keep the monitoring updated + + // if disabled do nothing + if(!enabled) return; + + + // calculate the back-emf voltage if KV_rating available U_bemf = vel*(1/KV) + 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; + + // upgrade the current based voltage limit + switch (controller) { + case MotionControlType::torque: + if(torque_controller == TorqueControlType::voltage){ // if voltage torque control + if(!_isset(phase_resistance)) voltage.q = target; + else voltage.q = target*phase_resistance + voltage_bemf; + voltage.q = _constrain(voltage.q, -voltage_limit, voltage_limit); + // set d-component (lag compensation if known inductance) + if(!_isset(phase_inductance)) voltage.d = 0; + else voltage.d = _constrain( -target*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit); + }else{ + current_sp = target; // if current/foc_current torque control + } + break; + case MotionControlType::angle: + // TODO sensor precision: this calculation is not numerically precise. The target value cannot express precise positions when + // the angles are large. This results in not being able to command small changes at high position values. + // to solve this, the delta-angle has to be calculated in a numerically precise way. + // angle set point + shaft_angle_sp = target; + // calculate velocity set point + shaft_velocity_sp = feed_forward_velocity + P_angle( shaft_angle_sp - shaft_angle ); + shaft_velocity_sp = _constrain(shaft_velocity_sp,-velocity_limit, velocity_limit); + // calculate the torque command - sensor precision: this calculation is ok, but based on bad value from previous calculation + current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if voltage torque control + // if torque controlled through voltage + if(torque_controller == TorqueControlType::voltage){ + // use voltage if phase-resistance not provided + if(!_isset(phase_resistance)) voltage.q = current_sp; + else voltage.q = _constrain( current_sp*phase_resistance + voltage_bemf , -voltage_limit, voltage_limit); + // set d-component (lag compensation if known inductance) + if(!_isset(phase_inductance)) voltage.d = 0; + else voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit); + } + break; + case MotionControlType::velocity: + // velocity set point - sensor precision: this calculation is numerically precise. + shaft_velocity_sp = target; + // calculate the torque command + current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if current/foc_current torque control + // if torque controlled through voltage control + if(torque_controller == TorqueControlType::voltage){ + // use voltage if phase-resistance not provided + if(!_isset(phase_resistance)) voltage.q = current_sp; + else voltage.q = _constrain( current_sp*phase_resistance + voltage_bemf , -voltage_limit, voltage_limit); + // set d-component (lag compensation if known inductance) + if(!_isset(phase_inductance)) voltage.d = 0; + else voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit); + } + break; + case MotionControlType::velocity_openloop: + // velocity control in open loop - sensor precision: this calculation is numerically precise. + shaft_velocity_sp = target; + voltage.q = velocityOpenloop(shaft_velocity_sp); // returns the voltage that is set to the motor + voltage.d = 0; + break; + case MotionControlType::angle_openloop: + // angle control in open loop - + // TODO sensor precision: this calculation NOT numerically precise, and subject + // to the same problems in small set-point changes at high angles + // as the closed loop version. + shaft_angle_sp = target; + voltage.q = angleOpenloop(shaft_angle_sp); // returns the voltage that is set to the motor + voltage.d = 0; + break; + } +} + +void HybridStepperMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) +{ + angle_el = _normalizeAngle(angle_el); + float _ca = _cos(angle_el); + float _sa = _sin(angle_el); + float center; + + switch (foc_modulation) { + case FOCModulationType::Trapezoid_120: + case FOCModulationType::Trapezoid_150: + // not handled + Ua = 0; + Ub = 0; + Uc = 0; + break; + case FOCModulationType::SinePWM: + // C phase is fixed at half-rail to provide bias point for A, B legs + Ua = (_ca * Ud) - (_sa * Uq); + Ub = (_sa * Ud) + (_ca * Uq); + + center = driver->voltage_limit / 2; + + Ua += center; + Ub += center; + Uc = center; + break; + + case FOCModulationType::SpaceVectorPWM: + // C phase moves in order to increase max bias on coils + Ua = (_ca * Ud) - (_sa * Uq); + Ub = (_sa * Ud) + (_ca * Uq); + + float Umin = fmin(fmin(Ua, Ub), 0); + float Umax = fmax(fmax(Ua, Ub), 0); + float Vo = -(Umin + Umax)/2 + driver->voltage_limit/2; + + Ua = Ua + Vo; + Ub = Ub + Vo; + Uc = Vo; + } + driver->setPwm(Ua, Ub, Uc); + +} + +// Function (iterative) generating open loop movement for target velocity +// - target_velocity - rad/s +// it uses voltage_limit variable +float HybridStepperMotor::velocityOpenloop(float target_velocity) +{ + // get current timestamp + unsigned long now_us = _micros(); + // calculate the sample time from last call + float Ts = (now_us - open_loop_timestamp) * 1e-6f; + // quick fix for strange cases (micros overflow + timestamp not defined) + if (Ts <= 0 || Ts > 0.5f) + Ts = 1e-3f; + + // calculate the necessary angle to achieve target velocity + shaft_angle = _normalizeAngle(shaft_angle + target_velocity * Ts); + // for display purposes + shaft_velocity = target_velocity; + + // use voltage limit or current limit + float Uq = voltage_limit; + if (_isset(phase_resistance)) + { + Uq = _constrain(current_limit * phase_resistance + fabs(voltage_bemf), -voltage_limit, voltage_limit); + // recalculate the current + current.q = (Uq - fabs(voltage_bemf)) / phase_resistance; + } + + // set the maximal allowed voltage (voltage_limit) with the necessary angle + setPhaseVoltage(Uq, 0, _electricalAngle(shaft_angle, pole_pairs)); + + // save timestamp for next call + open_loop_timestamp = now_us; + + return Uq; +} + +// Function (iterative) generating open loop movement towards the target angle +// - target_angle - rad +// it uses voltage_limit and velocity_limit variables +float HybridStepperMotor::angleOpenloop(float target_angle) +{ + // get current timestamp + unsigned long now_us = _micros(); + // calculate the sample time from last call + float Ts = (now_us - open_loop_timestamp) * 1e-6f; + // quick fix for strange cases (micros overflow + timestamp not defined) + if (Ts <= 0 || Ts > 0.5f) + Ts = 1e-3f; + + // calculate the necessary angle to move from current position towards target angle + // with maximal velocity (velocity_limit) + if (abs(target_angle - shaft_angle) > abs(velocity_limit * Ts)) + { + shaft_angle += _sign(target_angle - shaft_angle) * abs(velocity_limit) * Ts; + shaft_velocity = velocity_limit; + } + else + { + shaft_angle = target_angle; + shaft_velocity = 0; + } + + // use voltage limit or current limit + float Uq = voltage_limit; + if (_isset(phase_resistance)) + { + Uq = _constrain(current_limit * phase_resistance + fabs(voltage_bemf), -voltage_limit, voltage_limit); + // recalculate the current + current.q = (Uq - fabs(voltage_bemf)) / phase_resistance; + } + // set the maximal allowed voltage (voltage_limit) with the necessary angle + setPhaseVoltage(Uq, 0, _electricalAngle((shaft_angle), pole_pairs)); + + // save timestamp for next call + open_loop_timestamp = now_us; + + return Uq; +} diff --git a/src/HybridStepperMotor.h b/src/HybridStepperMotor.h new file mode 100644 index 00000000..d75ef8cd --- /dev/null +++ b/src/HybridStepperMotor.h @@ -0,0 +1,113 @@ +/** + * @file HybridStepperMotor.h + * + */ + +#ifndef HybridStepperMotor_h +#define HybridStepperMotor_h + +#include "Arduino.h" +#include "common/base_classes/FOCMotor.h" +#include "common/base_classes/StepperDriver.h" +#include "common/base_classes/Sensor.h" +#include "common/foc_utils.h" +#include "common/time_utils.h" +#include "common/defaults.h" + +/** + Stepper Motor class +*/ +class HybridStepperMotor : public FOCMotor +{ +public: + /** + HybridStepperMotor class constructor + @param pp pole pair number + @param R motor phase resistance - [Ohm] + @param KV motor KV rating (1/K_bemf) - rpm/V + @param L motor phase inductance - [H] + */ + HybridStepperMotor(int pp, float R = NOT_SET, float KV = NOT_SET, float L = NOT_SET); + + /** + * Function linking a motor and a foc driver + * + * @param driver BLDCDriver handle for hardware peripheral control + */ + void linkDriver(BLDCDriver *driver); + + /** + * BLDCDriver link: + */ + BLDCDriver *driver; + + /** Motor hardware init function */ + int init() override; + /** Motor disable function */ + void disable() override; + /** Motor enable function */ + void enable() override; + + /** + * Function initializing FOC algorithm + * and aligning sensor's and motors' zero position + */ + int initFOC() override; + + /** + * Function running FOC algorithm in real-time + * it calculates the gets motor angle and sets the appropriate voltages + * to the phase pwm signals + * - the faster you can run it the better Arduino UNO ~1ms, Bluepill ~ 100us + */ + void loopFOC() override; + + /** + * Function executing the control loops set by the controller parameter of the HybridStepperMotor. + * + * @param target Either voltage, angle or velocity based on the motor.controller + * If it is not set the motor will use the target set in its variable motor.target + * + * This function doesn't need to be run upon each loop execution - depends of the use case + */ + void move(float target = NOT_SET) override; + + float Ua, Ub, Uc; //!< Phase voltages 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 + * + * @param Uq Current voltage in q axis to set to the motor + * @param Ud Current voltage in d axis to set to the motor + * @param angle_el current electrical angle of the motor + */ + void setPhaseVoltage(float Uq, float Ud, float angle_el) override; + +private: + int alignCurrentSense(); + /** Sensor alignment to electrical 0 angle of the motor */ + int alignSensor(); + /** Motor and sensor alignment to the sensors absolute 0 angle */ + int absoluteZeroSearch(); + + // Open loop motion control + /** + * Function (iterative) generating open loop movement for target velocity + * it uses voltage_limit variable + * + * @param target_velocity - rad/s + */ + float velocityOpenloop(float target_velocity); + /** + * Function (iterative) generating open loop movement towards the target angle + * it uses voltage_limit and velocity_limit variables + * + * @param target_angle - rad + */ + float angleOpenloop(float target_angle); + // open loop variables + long open_loop_timestamp; +}; + +#endif diff --git a/src/SimpleFOC.h b/src/SimpleFOC.h index 4e6815e5..b4f48f29 100644 --- a/src/SimpleFOC.h +++ b/src/SimpleFOC.h @@ -98,6 +98,7 @@ void loop() { #include "BLDCMotor.h" #include "StepperMotor.h" +#include "HybridStepperMotor.h" #include "sensors/Encoder.h" #include "sensors/MagneticSensorSPI.h" #include "sensors/MagneticSensorI2C.h" diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index 697ed277..1628a540 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -33,11 +33,11 @@ void StepperMotor::linkDriver(StepperDriver* _driver) { } // init hardware pins -void StepperMotor::init() { +int StepperMotor::init() { if (!driver || !driver->initialized) { motor_status = FOCMotorStatus::motor_init_failed; SIMPLEFOC_DEBUG("MOT: Init not possible, driver not initialized"); - return; + return 0; } motor_status = FOCMotorStatus::motor_initializing; SIMPLEFOC_DEBUG("MOT: Init"); @@ -57,10 +57,13 @@ void StepperMotor::init() { P_angle.limit = velocity_limit; // if using open loop control, set a CW as the default direction if not already set - if ((controller==MotionControlType::angle_openloop - ||controller==MotionControlType::velocity_openloop) - && (sensor_direction == Direction::UNKNOWN)) { - sensor_direction = Direction::CW; + // only if no sensor is used + if(!sensor){ + if ((controller==MotionControlType::angle_openloop + ||controller==MotionControlType::velocity_openloop) + && (sensor_direction == Direction::UNKNOWN)) { + sensor_direction = Direction::CW; + } } _delay(500); @@ -70,6 +73,7 @@ void StepperMotor::init() { _delay(500); motor_status = FOCMotorStatus::motor_uncalibrated; + return 1; } @@ -108,13 +112,37 @@ int StepperMotor::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 = sensor->getAngle(); - } else { SIMPLEFOC_DEBUG("MOT: No sensor."); } + shaft_angle = shaftAngle(); + + // 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 + } + } if(exit_flag){ SIMPLEFOC_DEBUG("MOT: Ready."); @@ -128,11 +156,35 @@ int StepperMotor::initFOC() { return exit_flag; } +// Calibrate the motor and current sense phases +int StepperMotor::alignCurrentSense() { + int exit_flag = 1; // success + + SIMPLEFOC_DEBUG("MOT: Align current sense."); + + // align current sense and the driver + exit_flag = current_sense->driverAlign(voltage_sensor_align, modulation_centered); + if(!exit_flag){ + // error in current sense - phase either not measured or bad connection + SIMPLEFOC_DEBUG("MOT: Align error!"); + exit_flag = 0; + }else{ + // output the alignment status flag + SIMPLEFOC_DEBUG("MOT: Success: ", exit_flag); + } + + return exit_flag > 0; +} + // Encoder alignment to electrical 0 angle 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 +196,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 +206,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); } @@ -190,7 +242,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(); @@ -249,8 +301,6 @@ void StepperMotor::loopFOC() { // if open-loop do nothing if( controller==MotionControlType::angle_openloop || controller==MotionControlType::velocity_openloop ) return; - // shaft angle - shaft_angle = shaftAngle(); // if disabled do nothing if(!enabled) return; @@ -259,7 +309,40 @@ void StepperMotor::loopFOC() { // This function will not have numerical issues because it uses Sensor::getMechanicalAngle() // which is in range 0-2PI electrical_angle = electricalAngle(); - + switch (torque_controller) { + case TorqueControlType::voltage: + // no need to do anything really + break; + case TorqueControlType::dc_current: + if(!current_sense) return; + // read overall current magnitude + current.q = current_sense->getDCCurrent(electrical_angle); + // filter the value values + current.q = LPF_current_q(current.q) + feed_forward_current.q; + // calculate the phase voltage + voltage.q = PID_current_q(current_sp - current.q); + // d voltage - lag compensation + if(_isset(phase_inductance)) voltage.d = _constrain( -(current_sp+feed_forward_current.q)*shaft_velocity*pole_pairs*phase_inductance+feed_forward_voltage.d, -voltage_limit, voltage_limit); + else voltage.d = feed_forward_voltage.d; + break; + case TorqueControlType::foc_current: + if(!current_sense) return; + // read dq currents + current = current_sense->getFOCCurrents(electrical_angle); + // filter values + current.q = LPF_current_q(current.q)+feed_forward_current.q; + current.d = LPF_current_d(current.d)+feed_forward_current.d; + // calculate the phase voltages + voltage.q = PID_current_q(current_sp - current.q)+feed_forward_voltage.q; + voltage.d = PID_current_d(-current.d)+feed_forward_voltage.d; + // d voltage - lag compensation - TODO verify + // if(_isset(phase_inductance)) voltage.d = _constrain( voltage.d - current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit); + break; + default: + // no torque control selected + SIMPLEFOC_DEBUG("MOT: no torque control selected!"); + break; + } // set the phase voltage - FOC heart function :) setPhaseVoltage(voltage.q, voltage.d, electrical_angle); } @@ -271,6 +354,9 @@ void StepperMotor::loopFOC() { // - if target is not set it uses motor.target value void StepperMotor::move(float new_target) { + // set internal target variable + if(_isset(new_target) ) target = new_target; + // downsampling (optional) if(motion_cnt++ < motion_downsample) return; motion_cnt = 0; @@ -289,64 +375,76 @@ void StepperMotor::move(float new_target) { // if disabled do nothing if(!enabled) return; - // set internal target variable - 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*_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; - // choose control loop + // upgrade the current based voltage limit switch (controller) { case MotionControlType::torque: - if(!_isset(phase_resistance)) voltage.q = target; // if voltage torque control - else voltage.q = target*phase_resistance + voltage_bemf; - voltage.q = _constrain(voltage.q, -voltage_limit, voltage_limit); - // set d-component (lag compensation if known inductance) - if(!_isset(phase_inductance)) voltage.d = 0; - else voltage.d = _constrain( -target*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit); + if(torque_controller == TorqueControlType::voltage){ // if voltage torque control + if(!_isset(phase_resistance)) voltage.q = target; + else voltage.q = target*phase_resistance + voltage_bemf; + voltage.q = _constrain(voltage.q, -voltage_limit, voltage_limit); + // set d-component (lag compensation if known inductance) + if(!_isset(phase_inductance)) voltage.d = 0; + else voltage.d = _constrain( -target*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit); + }else{ + current_sp = target; // if current/foc_current torque control + } break; case MotionControlType::angle: + // TODO sensor precision: this calculation is not numerically precise. The target value cannot express precise positions when + // the angles are large. This results in not being able to command small changes at high position values. + // to solve this, the delta-angle has to be calculated in a numerically precise way. // angle set point shaft_angle_sp = target; // calculate velocity set point shaft_velocity_sp = feed_forward_velocity + P_angle( shaft_angle_sp - shaft_angle ); - shaft_velocity_sp = _constrain(shaft_velocity_sp, -velocity_limit, velocity_limit); - // calculate the torque command + shaft_velocity_sp = _constrain(shaft_velocity_sp,-velocity_limit, velocity_limit); + // calculate the torque command - sensor precision: this calculation is ok, but based on bad value from previous calculation current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if voltage torque control // if torque controlled through voltage - // use voltage if phase-resistance not provided - if(!_isset(phase_resistance)) voltage.q = current_sp; - else voltage.q = _constrain( current_sp*phase_resistance + voltage_bemf , -voltage_limit, voltage_limit); - // set d-component (lag compensation if known inductance) - if(!_isset(phase_inductance)) voltage.d = 0; - else voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit); + if(torque_controller == TorqueControlType::voltage){ + // use voltage if phase-resistance not provided + if(!_isset(phase_resistance)) voltage.q = current_sp; + else voltage.q = _constrain( current_sp*phase_resistance + voltage_bemf , -voltage_limit, voltage_limit); + // set d-component (lag compensation if known inductance) + if(!_isset(phase_inductance)) voltage.d = 0; + else voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit); + } break; case MotionControlType::velocity: - // velocity set point + // velocity set point - sensor precision: this calculation is numerically precise. shaft_velocity_sp = target; // calculate the torque command current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if current/foc_current torque control // if torque controlled through voltage control - // use voltage if phase-resistance not provided - if(!_isset(phase_resistance)) voltage.q = current_sp; - else voltage.q = _constrain( current_sp*phase_resistance + voltage_bemf , -voltage_limit, voltage_limit); - // set d-component (lag compensation if known inductance) - if(!_isset(phase_inductance)) voltage.d = 0; - else voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit); + if(torque_controller == TorqueControlType::voltage){ + // use voltage if phase-resistance not provided + if(!_isset(phase_resistance)) voltage.q = current_sp; + else voltage.q = _constrain( current_sp*phase_resistance + voltage_bemf , -voltage_limit, voltage_limit); + // set d-component (lag compensation if known inductance) + if(!_isset(phase_inductance)) voltage.d = 0; + else voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit); + } break; case MotionControlType::velocity_openloop: - // velocity control in open loop + // velocity control in open loop - sensor precision: this calculation is numerically precise. shaft_velocity_sp = target; voltage.q = velocityOpenloop(shaft_velocity_sp); // returns the voltage that is set to the motor - voltage.d = 0; // TODO d-component lag-compensation + voltage.d = 0; break; case MotionControlType::angle_openloop: - // angle control in open loop + // angle control in open loop - + // TODO sensor precision: this calculation NOT numerically precise, and subject + // to the same problems in small set-point changes at high angles + // as the closed loop version. shaft_angle_sp = target; voltage.q = angleOpenloop(shaft_angle_sp); // returns the voltage that is set to the motor - voltage.d = 0; // TODO d-component lag-compensation + voltage.d = 0; break; } } diff --git a/src/StepperMotor.h b/src/StepperMotor.h index 7eda3167..208e453e 100644 --- a/src/StepperMotor.h +++ b/src/StepperMotor.h @@ -43,7 +43,7 @@ class StepperMotor: public FOCMotor StepperDriver* driver; /** Motor hardware init function */ - void init() override; + int init() override; /** Motor disable function */ void disable() override; /** Motor enable function */ @@ -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 @@ -85,12 +83,25 @@ class StepperMotor: public FOCMotor */ void setPhaseVoltage(float Uq, float Ud, float angle_el) override; + /** + * Measure resistance and inductance of a StepperMotor and print results to debug. + * If a sensor is available, an estimate of zero electric angle will be reported too. + * TODO: determine the correction factor + * @param voltage The voltage applied to the motor + * @returns 0 for success, >0 for failure + */ + int characteriseMotor(float voltage){ + return FOCMotor::characteriseMotor(voltage, 1.0f); + } + private: /** Sensor alignment to electrical 0 angle of the motor */ int alignSensor(); /** Motor and sensor alignment to the sensors absolute 0 angle */ int absoluteZeroSearch(); + /** Current sense and motor phase alignment */ + int alignCurrentSense(); // Open loop motion control /** diff --git a/src/common/base_classes/BLDCDriver.h b/src/common/base_classes/BLDCDriver.h index f405d785..6ae8186f 100644 --- a/src/common/base_classes/BLDCDriver.h +++ b/src/common/base_classes/BLDCDriver.h @@ -2,40 +2,17 @@ #define BLDCDRIVER_H #include "Arduino.h" +#include "FOCDriver.h" - -enum PhaseState : uint8_t { - PHASE_OFF = 0, // both sides of the phase are off - PHASE_ON = 1, // both sides of the phase are driven with PWM, dead time is applied in 6-PWM mode - PHASE_HI = 2, // only the high side of the phase is driven with PWM (6-PWM mode only) - PHASE_LO = 3, // only the low side of the phase is driven with PWM (6-PWM mode only) -}; - - -class BLDCDriver{ +class BLDCDriver: public FOCDriver{ public: - /** Initialise hardware */ - virtual int init() = 0; - /** Enable hardware */ - virtual void enable() = 0; - /** Disable hardware */ - virtual void disable() = 0; - - long pwm_frequency; //!< pwm frequency value in hertz - float voltage_power_supply; //!< power supply voltage - float voltage_limit; //!< limiting voltage set to the motor - - float dc_a; //!< currently set duty cycle on phaseA float dc_b; //!< currently set duty cycle on phaseB float dc_c; //!< currently set duty cycle on phaseC - bool initialized = false; // true if driver was successfully initialized - void* params = 0; // pointer to hardware specific parameters of driver - /** - * Set phase voltages to the harware + * Set phase voltages to the hardware * * @param Ua - phase A voltage * @param Ub - phase B voltage @@ -51,6 +28,9 @@ class BLDCDriver{ * @param sa - phase C state : active / disabled ( high impedance ) */ virtual void setPhaseState(PhaseState sa, PhaseState sb, PhaseState sc) = 0; + + /** driver type getter function */ + virtual DriverType type() override { return DriverType::BLDC; }; }; #endif diff --git a/src/common/base_classes/CurrentSense.cpp b/src/common/base_classes/CurrentSense.cpp index 03ea19ea..85ebd2c5 100644 --- a/src/common/base_classes/CurrentSense.cpp +++ b/src/common/base_classes/CurrentSense.cpp @@ -1,4 +1,5 @@ #include "CurrentSense.h" +#include "../../communication/SimpleFOCDebug.h" // get current magnitude @@ -27,7 +28,7 @@ float CurrentSense::getDCCurrent(float motor_electrical_angle){ return sign*_sqrt(ABcurrent.alpha*ABcurrent.alpha + ABcurrent.beta*ABcurrent.beta); } -// function used with the foc algorihtm +// function used with the foc algorithm // calculating DQ currents from phase currents // - function calculating park and clarke transform of the phase currents // - using getPhaseCurrents and getABCurrents internally @@ -44,11 +45,37 @@ DQCurrent_s CurrentSense::getFOCCurrents(float angle_el){ return return_current; } -// function used with the foc algorihtm +// function used with the foc algorithm // calculating Alpha Beta currents from phase currents // - function calculating Clarke transform of the phase currents ABCurrent_s CurrentSense::getABCurrents(PhaseCurrent_s current){ + // check if driver is an instance of StepperDriver + // if so there is no need to Clarke transform + if (driver_type == DriverType::Stepper){ + ABCurrent_s return_ABcurrent; + return_ABcurrent.alpha = current.a; + return_ABcurrent.beta = current.b; + return return_ABcurrent; + } + + if (driver_type == DriverType::Hybrid){ + ABCurrent_s return_ABcurrent; + //ia + ib + ic = 0 + if(current.a == 0){ + return_ABcurrent.alpha = -current.c - current.b; + return_ABcurrent.beta = current.b; + }else if(current.b == 0){ + return_ABcurrent.alpha = current.a; + return_ABcurrent.beta = -current.c - current.a; + }else{ + return_ABcurrent.alpha = current.a; + return_ABcurrent.beta = current.b; + } + return return_ABcurrent; + } + + // otherwise it's a BLDC motor and // calculate clarke transform float i_alpha, i_beta; if(!current.c){ @@ -80,7 +107,7 @@ ABCurrent_s CurrentSense::getABCurrents(PhaseCurrent_s current){ return return_ABcurrent; } -// function used with the foc algorihtm +// function used with the foc algorithm // 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){ @@ -97,8 +124,10 @@ DQCurrent_s CurrentSense::getDQCurrents(ABCurrent_s current, float angle_el){ /** Driver linking to the current sense */ -void CurrentSense::linkDriver(BLDCDriver* _driver) { - driver = _driver; +void CurrentSense::linkDriver(FOCDriver* _driver) { + driver = _driver; + // save the driver type for easier access + driver_type = driver->type(); } @@ -108,4 +137,599 @@ void CurrentSense::enable(){ void CurrentSense::disable(){ // nothing is done here, but you can override this function -}; \ No newline at end of file +}; + + +// Function aligning the current sense with motor driver +// if all pins are connected well none of this is really necessary! - can be avoided +// returns flag +// 0 - fail +// 1 - success and nothing changed +// 2 - success but pins reconfigured +// 3 - success but gains inverted +// 4 - success but pins reconfigured and gains inverted +// IMPORTANT, this function can be overriden in the child class +int CurrentSense::driverAlign(float voltage, bool modulation_centered){ + + int exit_flag = 1; + if(skip_align) return exit_flag; + + if (!initialized) return 0; + + // check if stepper or BLDC + switch(driver_type){ + case DriverType::BLDC: + return alignBLDCDriver(voltage, (BLDCDriver*)driver, modulation_centered); + case DriverType::Stepper: + return alignStepperDriver(voltage, (StepperDriver*)driver, modulation_centered); + case DriverType::Hybrid: + return alignHybridDriver(voltage, (BLDCDriver*)driver, modulation_centered); + default: + // driver type not supported + SIMPLEFOC_DEBUG("CS: Cannot align driver type!"); + return 0; + } +} + + + +// Helper function to read and average phase currents +PhaseCurrent_s CurrentSense::readAverageCurrents(int N) { + PhaseCurrent_s c = getPhaseCurrents(); + for (int i = 0; i < N; i++) { + PhaseCurrent_s c1 = getPhaseCurrents(); + 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); + } + return c; +}; + + + +// Function aligning the current sense with motor driver +// if all pins are connected well none of this is really necessary! - can be avoided +// returns flag +// 0 - fail +// 1 - success and nothing changed +// 2 - success but pins reconfigured +// 3 - success but gains inverted +// 4 - success but pins reconfigured and gains inverted +int CurrentSense::alignBLDCDriver(float voltage, BLDCDriver* bldc_driver, bool modulation_centered){ + + bool phases_switched = 0; + bool phases_inverted = 0; + + float zero = 0; + if(modulation_centered) zero = driver->voltage_limit/2.0; + + // set phase A active and phases B and C down + // 300 ms of ramping + for(int i=0; i < 100; i++){ + bldc_driver->setPwm(voltage/100.0*((float)i)+zero , zero, zero); + _delay(3); + } + _delay(500); + PhaseCurrent_s c_a = readAverageCurrents(); + bldc_driver->setPwm(zero, zero, zero); + // check if currents are to low (lower than 100mA) + // TODO calculate the 100mA threshold from the ADC resolution + // if yes throw an error and return 0 + // either the current sense is not connected or the current is + // too low for calibration purposes (one should raise the motor.voltage_sensor_align) + if((fabs(c_a.a) < 0.1f) && (fabs(c_a.b) < 0.1f) && (fabs(c_a.c) < 0.1f)){ + SIMPLEFOC_DEBUG("CS: Err too low current, rise voltage!"); + return 0; // measurement current too low + } + + + // now we have to determine + // 1) which pin correspond to which phase of the bldc driver + // 2) if the currents measured have good polarity + // + // > when we apply a voltage to a phase A of the driver what we expect to measure is the current I on the phase A + // and -I/2 on the phase B and I/2 on the phase C + + // find the highest magnitude in c_a + // and make sure it's around 2 (1.5 at least) times higher than the other two + float ca[3] = {fabs(c_a.a), fabs(c_a.b), fabs(c_a.c)}; + uint8_t max_i = -1; // max index + float max_c = 0; // max current + float max_c_ratio = 0; // max current ratio + for(int i = 0; i < 3; i++){ + if(!ca[i]) continue; // current not measured + if(ca[i] > max_c){ + max_c = ca[i]; + max_i = i; + for(int j = 0; j < 3; j++){ + if(i == j) continue; + if(!ca[j]) continue; // current not measured + float ratio = max_c / ca[j]; + if(ratio > max_c_ratio) max_c_ratio = ratio; + } + } + } + + // check the current magnitude ratios + // 1) if there is one current that is approximately 2 times higher than the other two + // this is the A current + // 2) if the max current is not at least 1.5 times higher than the other two + // we have two cases: + // - either we only measure two currents and the third one is not measured - then phase A is not measured + // - or the current sense is not connected properly + + if(max_c_ratio >=1.5f){ + switch (max_i){ + case 1: // phase B is the max current + SIMPLEFOC_DEBUG("CS: Switch A-B"); + // switch phase A and B + _swap(pinA, pinB); + _swap(offset_ia, offset_ib); + _swap(gain_a, gain_b); + _swap(c_a.b, c_a.b); + phases_switched = true; // signal that pins have been switched + break; + case 2: // phase C is the max current + SIMPLEFOC_DEBUG("CS: Switch A-C"); + // switch phase A and C + _swap(pinA, pinC); + _swap(offset_ia, offset_ic); + _swap(gain_a, gain_c); + _swap(c_a.a, c_a.c); + phases_switched = true;// signal that pins have been switched + break; + } + // check if the current is negative and invert the gain if so + if( _sign(c_a.a) < 0 ){ + SIMPLEFOC_DEBUG("CS: Inv A"); + gain_a *= -1; + phases_inverted = true; // signal that pins have been inverted + } + }else if(_isset(pinA) && _isset(pinB) && _isset(pinC)){ + // if all three currents are measured and none of them is significantly higher + // we have a problem with the current sense + SIMPLEFOC_DEBUG("CS: Err A - all currents same magnitude!"); + return 0; + }else{ //phase A is not measured so put the _NC to the phase A + if(_isset(pinA) && !_isset(pinB)){ + SIMPLEFOC_DEBUG("CS: Switch A-(B)NC"); + _swap(pinA, pinB); + _swap(offset_ia, offset_ib); + _swap(gain_a, gain_b); + _swap(c_a.b, c_a.b); + phases_switched = true; // signal that pins have been switched + }else if(_isset(pinA) && !_isset(pinC)){ + SIMPLEFOC_DEBUG("CS: Switch A-(C)NC"); + _swap(pinA, pinC); + _swap(offset_ia, offset_ic); + _swap(gain_a, gain_c); + _swap(c_a.b, c_a.c); + phases_switched = true; // signal that pins have been switched + } + } + // at this point the current sensing on phase A can be either: + // - aligned with the driver phase A + // - or the phase A is not measured and the _NC is connected to the phase A + // + // In either case A is done, now we have to check the phase B and C + + + // set phase B active and phases A and C down + // 300 ms of ramping + for(int i=0; i < 100; i++){ + bldc_driver->setPwm(zero, voltage/100.0*((float)i)+zero, zero); + _delay(3); + } + _delay(500); + PhaseCurrent_s c_b = readAverageCurrents(); + bldc_driver->setPwm(zero, zero, zero); + + // check the phase B + // find the highest magnitude in c_b + // and make sure it's around 2 (1.5 at least) times higher than the other two + float cb[3] = {fabs(c_b.a), fabs(c_b.b), fabs(c_b.c)}; + max_i = -1; // max index + max_c = 0; // max current + max_c_ratio = 0; // max current ratio + for(int i = 0; i < 3; i++){ + if(!cb[i]) continue; // current not measured + if(cb[i] > max_c){ + max_c = cb[i]; + max_i = i; + for(int j = 0; j < 3; j++){ + if(i == j) continue; + if(!cb[j]) continue; // current not measured + float ratio = max_c / cb[j]; + if(ratio > max_c_ratio) max_c_ratio = ratio; + } + } + } + if(max_c_ratio >= 1.5f){ + switch (max_i){ + case 0: // phase A is the max current + // this is an error as phase A is already aligned + SIMPLEFOC_DEBUG("CS: Err align B"); + return 0; + case 2: // phase C is the max current + SIMPLEFOC_DEBUG("CS: Switch B-C"); + _swap(pinB, pinC); + _swap(offset_ib, offset_ic); + _swap(gain_b, gain_c); + _swap(c_b.b, c_b.c); + phases_switched = true; // signal that pins have been switched + break; + } + // check if the current is negative and invert the gain if so + if( _sign(c_b.b) < 0 ){ + SIMPLEFOC_DEBUG("CS: Inv B"); + gain_b *= -1; + phases_inverted = true; // signal that pins have been inverted + } + }else if(_isset(pinB) && _isset(pinC)){ + // if all three currents are measured and none of them is significantly higher + // we have a problem with the current sense + SIMPLEFOC_DEBUG("CS: Err B - all currents same magnitude!"); + return 0; + }else{ //phase B is not measured so put the _NC to the phase B + if(_isset(pinB) && !_isset(pinC)){ + SIMPLEFOC_DEBUG("CS: Switch B-(C)NC"); + _swap(pinB, pinC); + _swap(offset_ib, offset_ic); + _swap(gain_b, gain_c); + _swap(c_b.b, c_b.c); + phases_switched = true; // signal that pins have been switched + } + } + // at this point the current sensing on phase A and B can be either: + // - aligned with the driver phase A and B + // - or the phase A and B are not measured and the _NC is connected to the phase A and B + // + // In either case A and B is done, now we have to check the phase C + // phase C is also aligned if it is measured (not _NC) + // we have to check if the current is negative and invert the gain if so + if(_isset(pinC)){ + if( _sign(c_b.c) > 0 ){ // the expected current is -I/2 (if the phase A and B are aligned and C has correct polarity) + SIMPLEFOC_DEBUG("CS: Inv C"); + gain_c *= -1; + phases_inverted = true; // signal that pins have been inverted + } + } + + // construct the return flag + // if the phases have been switched return 2 + // if the gains have been inverted return 3 + // if both return 4 + uint8_t exit_flag = 1; + if(phases_switched) exit_flag += 1; + if(phases_inverted) exit_flag += 2; + return exit_flag; +} + + +// Function aligning the current sense with motor driver +// if all pins are connected well none of this is really necessary! - can be avoided +// returns flag +// 0 - fail +// 1 - success and nothing changed +// 2 - success but pins reconfigured +// 3 - success but gains inverted +// 4 - success but pins reconfigured and gains inverted +int CurrentSense::alignStepperDriver(float voltage, StepperDriver* stepper_driver, bool modulation_centered){ + + _UNUSED(modulation_centered); + + bool phases_switched = 0; + bool phases_inverted = 0; + + if(!_isset(pinA) || !_isset(pinB)){ + SIMPLEFOC_DEBUG("CS: Pins A & B not specified!"); + return 0; + } + + // set phase A active and phases B down + // ramp 300ms + for(int i=0; i < 100; i++){ + stepper_driver->setPwm(voltage/100.0*((float)i), 0); + _delay(3); + } + _delay(500); + PhaseCurrent_s c = readAverageCurrents(); + // disable the phases + stepper_driver->setPwm(0, 0); + if (fabs(c.a) < 0.1f && fabs(c.b) < 0.1f ){ + SIMPLEFOC_DEBUG("CS: Err too low current!"); + return 0; // measurement current too low + } + // align phase A + // 1) only one phase can be measured so we first measure which ADC pin corresponds + // to the phase A by comparing the magnitude + if (fabs(c.a) < fabs(c.b)){ + SIMPLEFOC_DEBUG("CS: Switch A-B"); + // switch phase A and B + _swap(pinA, pinB); + _swap(offset_ia, offset_ib); + _swap(gain_a, gain_b); + phases_switched = true; // signal that pins have been switched + } + // 2) check if measured current a is positive and invert if not + if (c.a < 0){ + SIMPLEFOC_DEBUG("CS: Inv A"); + gain_a *= -1; + phases_inverted = true; // signal that pins have been inverted + } + + // at this point the driver's phase A is aligned with the ADC pinA + // and the pin B should be the phase B + + // set phase B active and phases A down + // ramp 300ms + for(int i=0; i < 100; i++){ + stepper_driver->setPwm(0, voltage/100.0*((float)i)); + _delay(3); + } + _delay(500); + c = readAverageCurrents(); + stepper_driver->setPwm(0, 0); + + // phase B should be aligned + // 1) we just need to verify that it has been measured + if (fabs(c.b) < 0.1f ){ + SIMPLEFOC_DEBUG("CS: Err too low current on B!"); + return 0; // measurement current too low + } + // 2) check if measured current a is positive and invert if not + if (c.b < 0){ + SIMPLEFOC_DEBUG("CS: Inv B"); + gain_b *= -1; + phases_inverted = true; // signal that pins have been inverted + } + + // construct the return flag + // if success and nothing changed return 1 + // if the phases have been switched return 2 + // if the gains have been inverted return 3 + // if both return 4 + uint8_t exit_flag = 1; + if(phases_switched) exit_flag += 1; + if(phases_inverted) exit_flag += 2; + return exit_flag; +} + + + + + +int CurrentSense::alignHybridDriver(float voltage, BLDCDriver* bldc_driver, bool modulation_centered){ + + _UNUSED(modulation_centered); + + bool phases_switched = 0; + bool phases_inverted = 0; + + // first find the middle phase, which will be set to the C phase + // set phase A active and phases B active, and C down + // ramp 300ms + for(int i=0; i < 100; i++){ + bldc_driver->setPwm(voltage/100.0*((float)i), voltage/100.0*((float)i), 0); + _delay(3); + } + _delay(500); + PhaseCurrent_s c = readAverageCurrents(); + // disable the phases + bldc_driver->setPwm(0, 0, 0); + if (fabs(c.a) < 0.1f && fabs(c.b) < 0.1f && fabs(c.c) < 0.1f ){ + SIMPLEFOC_DEBUG("CS: Err too low current!"); + return 0; // measurement current too low + } + // find the highest magnitude in c + // and make sure it's around 2 (1.5 at least) times higher than the other two + float cc[3] = {fabs(c.a), fabs(c.b), fabs(c.c)}; + uint8_t max_i = -1; // max index + float max_c = 0; // max current + float max_c_ratio = 0; // max current ratio + for(int i = 0; i < 3; i++){ + if(!cc[i]) continue; // current not measured + if(cc[i] > max_c){ + max_c = cc[i]; + max_i = i; + for(int j = 0; j < 3; j++){ + if(i == j) continue; + if(!cc[j]) continue; // current not measured + float ratio = max_c / cc[j]; + if(ratio > max_c_ratio) max_c_ratio = ratio; + } + } + } + if(max_c_ratio >=1.5f){ + switch (max_i){ + case 0: // phase A is the max current + SIMPLEFOC_DEBUG("CS: Switch A-C"); + // switch phase A and C + _swap(pinA, pinC); + _swap(offset_ia, offset_ic); + _swap(gain_a, gain_c); + _swap(c.a, c.c); + phases_switched = true; // signal that pins have been switched + break; + case 1: // phase B is the max current + SIMPLEFOC_DEBUG("CS: Switch B-C"); + // switch phase B and C + _swap(pinB, pinC); + _swap(offset_ib, offset_ic); + _swap(gain_b, gain_c); + _swap(c.b, c.c); + phases_switched = true; // signal that pins have been switched + break; + } + // check if the current is positive and invert the gain if so + // current c should be negative + if( _sign(c.c) > 0 ){ + SIMPLEFOC_DEBUG("CS: Inv C"); + gain_c *= -1; + phases_inverted = true; // signal that pins have been inverted + } + }else{ + // c - middle phase is not measured + SIMPLEFOC_DEBUG("CS: Middle phase not measured!"); + if(_isset(pinC)){ + // switch the missing phase with the phase C + if(!_isset(pinA)){ + SIMPLEFOC_DEBUG("CS: Switch (A)NC-C"); + _swap(pinA, pinC); + _swap(offset_ia, offset_ic); + _swap(gain_a, gain_c); + _swap(c.a, c.c); + phases_switched = true; // signal that pins have been switched + }else if(!_isset(pinB)){ + SIMPLEFOC_DEBUG("CS: Switch (B)NC-C"); + _swap(pinB, pinC); + _swap(offset_ib, offset_ic); + _swap(gain_b, gain_c); + _swap(c.b, c.c); + phases_switched = true; // signal that pins have been switched + } + } + } + + // at this point the current sensing on phase A and B can be either: + // - aligned with the driver phase A and B + // - or the phase A and B are not measured and the _NC is connected to the phase A and B + + // Find the phase A + + // set phase A active and phases B down + // ramp 300ms + for(int i=0; i < 100; i++){ + bldc_driver->setPwm(voltage/100.0*((float)i), 0, 0); + _delay(3); + } + _delay(500); + c = readAverageCurrents(); + // disable the phases + bldc_driver->setPwm(0, 0, 0); + + // check if currents are to low (lower than 100mA) + if((fabs(c.a) < 0.1f) && (fabs(c.b) < 0.1f) && (fabs(c.c) < 0.1f)){ + SIMPLEFOC_DEBUG("CS: Err too low current, rise voltage!"); + return 0; // measurement current too low + } + + // now we have to determine + // 1) which pin correspond to which phase of the bldc driver + // 2) if the currents measured have good polarity + // + // > when we apply a voltage to a phase A of the driver what we expect to measure is the current I on the phase A + // and -I on the phase C + + // find the highest magnitude in A + // and make sure it's around the same as the C current (if the phase C is measured) + + float ca[3] = {fabs(c.a), fabs(c.b), fabs(c.c)}; + if(c.a && c.c){ + // if a and mid-phase c measured + // verify that they have almost the same magnitude + if((fabs(c.a) - fabs(c.c)) > 0.1f){ + SIMPLEFOC_DEBUG("CS: Err A-C currents not equal!"); + return 0; + } + }else if(c.b && c.c){ + // if a and mid-phase c measured + // verify that they have almost the same magnitude + if((fabs(c.a) - fabs(c.c)) > 0.1f){ + SIMPLEFOC_DEBUG("CS: Err B-C currents not equal!"); + return 0; + }else{ + // if the current are equal + // switch phase A and B + SIMPLEFOC_DEBUG("CS: Switch A-B"); + _swap(pinA, pinB); + _swap(offset_ia, offset_ib); + _swap(gain_a, gain_b); + _swap(c.a, c.b); + phases_switched = true; // signal that pins have been switched + } + }else if(c.a && c.b){ + // check if one is significantly higher than the other + if(fabs(fabs(c.a) - fabs(c.b)) < 0.1f){ + SIMPLEFOC_DEBUG("CS: Err A-B currents zero!"); + return 0; + }else{ + // if they are not equal take the highest as A + if (fabs(c.a) < fabs(c.b)){ + // switch phase A and B + SIMPLEFOC_DEBUG("CS: Switch A-B"); + _swap(pinA, pinB); + _swap(offset_ia, offset_ib); + _swap(gain_a, gain_b); + _swap(c.a, c.b); + phases_switched = true; // signal that pins have been switched + + } + } + } + // if we get here, phase A is aligned + // check if the current is negative and invert the gain if so + if( _sign(c.a) < 0 ){ + SIMPLEFOC_DEBUG("CS: Inv A"); + gain_a *= -1; + phases_inverted = true; // signal that pins have been inverted + } + + // at this point the driver's phase A is aligned with the ADC pinA + // and the pin B should be the phase B + + // set phase B active and phases A down + // ramp 300ms + for(int i=0; i < 100; i++){ + bldc_driver->setPwm(0, voltage/100.0*((float)i), 0); + _delay(3); + } + _delay(500); + c = readAverageCurrents(); + bldc_driver->setPwm(0, 0, 0); + + // check if currents are to low (lower than 100mA) + if((fabs(c.a) < 0.1f) && (fabs(c.b) < 0.1f) && (fabs(c.c) < 0.1f)){ + SIMPLEFOC_DEBUG("CS: Err too low current, rise voltage!"); + return 0; // measurement current too low + } + + // check the phase B + // find the highest magnitude in c + // and make sure it's around the same as the C current (if the phase C is measured) + float cb[3] = {fabs(c.a), fabs(c.b), fabs(c.c)}; + if(c.b && c.c){ + // if b and mid-phase c measured + // verify that they have almost the same magnitude + if((fabs(c.b) - fabs(c.c)) > 0.1f){ + SIMPLEFOC_DEBUG("CS: Err B-C currents not equal!"); + return 0; + } + }else if(c.a && c.b){ + // check if one is significantly higher than the other + if(fabs(fabs(c.a) - fabs(c.b)) < 0.1f){ + SIMPLEFOC_DEBUG("CS: Err A-B currents zero!"); + return 0; + } + } + + // check if b has good polarity + if( _sign(c.b) < 0 ){ + SIMPLEFOC_DEBUG("CS: Inv B"); + gain_b *= -1; + phases_inverted = true; // signal that pins have been inverted + } + + // construct the return flag + // if success and nothing changed return 1 + // if the phases have been switched return 2 + // if the gains have been inverted return 3 + // if both return 4 + uint8_t exit_flag = 1; + if(phases_switched) exit_flag += 1; + if(phases_inverted) exit_flag += 2; + return exit_flag; +} + + diff --git a/src/common/base_classes/CurrentSense.h b/src/common/base_classes/CurrentSense.h index 1c839053..3d9ec1b5 100644 --- a/src/common/base_classes/CurrentSense.h +++ b/src/common/base_classes/CurrentSense.h @@ -1,12 +1,15 @@ #ifndef CURRENTSENSE_H #define CURRENTSENSE_H -#include "BLDCDriver.h" +#include "FOCDriver.h" #include "../foc_utils.h" +#include "../time_utils.h" +#include "StepperDriver.h" +#include "BLDCDriver.h" /** * Current sensing abstract class defintion - * Each current sensoring implementation needs to extend this interface + * Each current sensing implementation needs to extend this interface */ class CurrentSense{ public: @@ -23,24 +26,49 @@ class CurrentSense{ * Linking the current sense with the motor driver * Only necessary if synchronisation in between the two is required */ - void linkDriver(BLDCDriver *driver); + void linkDriver(FOCDriver *driver); // variables bool skip_align = false; //!< variable signaling that the phase current direction should be verified during initFOC() - BLDCDriver* driver = nullptr; //!< driver link + FOCDriver* driver = nullptr; //!< driver link bool initialized = false; // true if current sense was successfully initialized void* params = 0; //!< pointer to hardware specific parameters of current sensing + DriverType driver_type = DriverType::UnknownDriver; //!< driver type (BLDC or Stepper) + + // ADC measurement gain for each phase + // support for different gains for different phases of more commonly - inverted phase currents + // this should be automated later + float gain_a; //!< phase A gain + float gain_b; //!< phase B gain + float gain_c; //!< phase C gain + + float offset_ia; //!< zero current A voltage value (center of the adc reading) + float offset_ib; //!< zero current B voltage value (center of the adc reading) + float offset_ic; //!< zero current C voltage value (center of the adc reading) + + // hardware variables + int pinA; //!< pin A analog pin for current measurement + int pinB; //!< pin B analog pin for current measurement + int pinC; //!< pin C analog pin for current measurement + /** * Function intended to verify if: * - phase current are oriented properly * - if their order is the same as driver phases * * This function corrects the alignment errors if possible ans if no such thing is needed it can be left empty (return 1) - * @returns - 0 - for failure & positive number (with status) - for success + * @returns - + 0 - failure + 1 - success and nothing changed + 2 - success but pins reconfigured + 3 - success but gains inverted + 4 - success but pins reconfigured and gains inverted + * + * IMPORTANT: Default implementation provided in the CurrentSense class, but can be overriden in the child classes */ - virtual int driverAlign(float align_voltage) = 0; + virtual int driverAlign(float align_voltage, bool modulation_centered = false); /** * Function rading the phase currents a, b and c @@ -80,7 +108,7 @@ class CurrentSense{ /** * Function used for Park transform in FOC control - * It reads the Alpha Beta currents and electircal angle of the motor + * It reads the Alpha Beta currents and electrical angle of the motor * It returns the D and Q currents * * @param current - phase current @@ -98,6 +126,25 @@ class CurrentSense{ * override it to do something useful. */ virtual void disable(); + + /** + * Function used to align the current sense with the BLDC motor driver + */ + int alignBLDCDriver(float align_voltage, BLDCDriver* driver, bool modulation_centered); + /** + * Function used to align the current sense with the Stepper motor driver + */ + int alignStepperDriver(float align_voltage, StepperDriver* driver, bool modulation_centered); + /** + * Function used to align the current sense with the Hybrid motor driver + */ + int alignHybridDriver(float align_voltage, BLDCDriver* driver, bool modulation_centered); + + /** + * Function used to read the average current values over N samples + */ + PhaseCurrent_s readAverageCurrents(int N=100); + }; #endif \ No newline at end of file diff --git a/src/common/base_classes/FOCDriver.h b/src/common/base_classes/FOCDriver.h new file mode 100644 index 00000000..263460b3 --- /dev/null +++ b/src/common/base_classes/FOCDriver.h @@ -0,0 +1,48 @@ +#ifndef FOCDRIVER_H +#define FOCDRIVER_H + +#include "Arduino.h" + + +enum PhaseState : uint8_t { + PHASE_OFF = 0, // both sides of the phase are off + PHASE_ON = 1, // both sides of the phase are driven with PWM, dead time is applied in 6-PWM mode + PHASE_HI = 2, // only the high side of the phase is driven with PWM (6-PWM mode only) + PHASE_LO = 3, // only the low side of the phase is driven with PWM (6-PWM mode only) +}; + + +enum DriverType{ + UnknownDriver=0, + BLDC=1, + Stepper=2, + Hybrid=3 +}; + +/** + * FOC driver class + */ +class FOCDriver{ + public: + + /** Initialise hardware */ + virtual int init() = 0; + /** Enable hardware */ + virtual void enable() = 0; + /** Disable hardware */ + virtual void disable() = 0; + + long pwm_frequency; //!< pwm frequency value in hertz + float voltage_power_supply; //!< power supply voltage + float voltage_limit; //!< limiting voltage set to the motor + + bool initialized = false; //!< true if driver was successfully initialized + void* params = 0; //!< pointer to hardware specific parameters of driver + + bool enable_active_high = true; //!< enable pin should be set to high to enable the driver (default is HIGH) + + /** get the driver type*/ + virtual DriverType type() = 0; +}; + +#endif diff --git a/src/common/base_classes/FOCMotor.cpp b/src/common/base_classes/FOCMotor.cpp index d1427bcf..090034ce 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; @@ -88,10 +92,231 @@ void FOCMotor::useMonitoring(Print &print){ #endif } +// Measure resistance and inductance of a motor +int FOCMotor::characteriseMotor(float voltage, float correction_factor=1.0f){ + if (!this->current_sense || !this->current_sense->initialized) + { + SIMPLEFOC_DEBUG("ERR: MOT: Cannot characterise motor: CS unconfigured or not initialized"); + return 1; + } + + if (voltage <= 0.0f){ + SIMPLEFOC_DEBUG("ERR: MOT: Cannot characterise motor: Voltage is negative or less than zero"); + return 2; + } + voltage = _constrain(voltage, 0.0f, voltage_limit); + + SIMPLEFOC_DEBUG("MOT: Measuring phase to phase resistance, keep motor still..."); + + float current_electric_angle = electricalAngle(); + + // Apply zero volts and measure a zero reference + setPhaseVoltage(0, 0, current_electric_angle); + _delay(500); + + PhaseCurrent_s zerocurrent_raw = current_sense->readAverageCurrents(); + DQCurrent_s zerocurrent = current_sense->getDQCurrents(current_sense->getABCurrents(zerocurrent_raw), current_electric_angle); + + + // Ramp and hold the voltage to measure resistance + // 300 ms of ramping + current_electric_angle = electricalAngle(); + for(int i=0; i < 100; i++){ + setPhaseVoltage(0, voltage/100.0*((float)i), current_electric_angle); + _delay(3); + } + _delay(10); + PhaseCurrent_s r_currents_raw = current_sense->readAverageCurrents(); + DQCurrent_s r_currents = current_sense->getDQCurrents(current_sense->getABCurrents(r_currents_raw), current_electric_angle); + + // Zero again + setPhaseVoltage(0, 0, current_electric_angle); + + + if (fabsf(r_currents.d - zerocurrent.d) < 0.2f) + { + SIMPLEFOC_DEBUG("ERR: MOT: Motor characterisation failed: measured current too low"); + return 3; + } + + float resistance = voltage / (correction_factor * (r_currents.d - zerocurrent.d)); + if (resistance <= 0.0f) + { + SIMPLEFOC_DEBUG("ERR: MOT: Motor characterisation failed: Calculated resistance <= 0"); + return 4; + } + + SIMPLEFOC_DEBUG("MOT: Estimated phase to phase resistance: ", 2.0f * resistance); + _delay(100); + + + // Start inductance measurement + SIMPLEFOC_DEBUG("MOT: Measuring inductance, keep motor still..."); + + unsigned long t0 = 0; + unsigned long t1 = 0; + float Ltemp = 0; + float Ld = 0; + float Lq = 0; + float d_electrical_angle = 0; + + unsigned int iterations = 40; // how often the algorithm gets repeated. + unsigned int cycles = 3; // averaged measurements for each iteration + unsigned int risetime_us = 200; // initially short for worst case scenario with low inductance + unsigned int settle_us = 100000; // initially long for worst case scenario with high inductance + + // Pre-rotate the angle to the q-axis (only useful with sensor, else no harm in doing it) + current_electric_angle += 0.5f * _PI; + current_electric_angle = _normalizeAngle(current_electric_angle); + + for (size_t axis = 0; axis < 2; axis++) + { + for (size_t i = 0; i < iterations; i++) + { + // current_electric_angle = i * _2PI / iterations; // <-- Do a sweep of the inductance. Use eg. for graphing + float inductanced = 0.0f; + + float qcurrent = 0.0f; + float dcurrent = 0.0f; + for (size_t j = 0; j < cycles; j++) + { + // read zero current + zerocurrent_raw = current_sense->readAverageCurrents(20); + zerocurrent = current_sense->getDQCurrents(current_sense->getABCurrents(zerocurrent_raw), current_electric_angle); + + // step the voltage + setPhaseVoltage(0, voltage, current_electric_angle); + t0 = micros(); + delayMicroseconds(risetime_us); // wait a little bit + + PhaseCurrent_s l_currents_raw = current_sense->getPhaseCurrents(); + t1 = micros(); + setPhaseVoltage(0, 0, current_electric_angle); + + DQCurrent_s l_currents = current_sense->getDQCurrents(current_sense->getABCurrents(l_currents_raw), current_electric_angle); + delayMicroseconds(settle_us); // wait a bit for the currents to go to 0 again + + if (t0 > t1) continue; // safety first + + // calculate the inductance + float dt = (t1 - t0)/1000000.0f; + if (l_currents.d - zerocurrent.d <= 0 || (voltage - resistance * (l_currents.d - zerocurrent.d)) <= 0) + { + continue; + } + + inductanced += fabsf(- (resistance * dt) / log((voltage - resistance * (l_currents.d - zerocurrent.d)) / voltage))/correction_factor; + + qcurrent+= l_currents.q - zerocurrent.q; // average the measured currents + dcurrent+= l_currents.d - zerocurrent.d; + } + qcurrent /= cycles; + dcurrent /= cycles; + + float delta = qcurrent / (fabsf(dcurrent) + fabsf(qcurrent)); + + + inductanced /= cycles; + Ltemp = i < 2 ? inductanced : Ltemp * 0.6 + inductanced * 0.4; + + float timeconstant = fabsf(Ltemp / resistance); // Timeconstant of an RL circuit (L/R) + // SIMPLEFOC_DEBUG("MOT: Estimated time constant in us: ", 1000000.0f * timeconstant); + + // Wait as long as possible (due to limited timing accuracy & sample rate), but as short as needed (while the current still changes) + risetime_us = _constrain(risetime_us * 0.6f + 0.4f * 1000000 * 0.6f * timeconstant, 100, 10000); + settle_us = 10 * risetime_us; + + + // Serial.printf(">inductance:%f:%f|xy\n", current_electric_angle, Ltemp * 1000.0f); // <-- Plot an angle sweep + + + /** + * How this code works: + * If we apply a current spike in the d´-axis, there will be cross coupling to the q´-axis current, if we didn´t use the actual d-axis (ie. d´ != d). + * This has to do with saliency (Ld != Lq). + * The amount of cross coupled current is somewhat proportional to the angle error, which means that if we iteratively change the angle to min/maximise this current, we get the correct d-axis (and q-axis). + */ + if (axis) + { + qcurrent *= -1.0f; // to d or q axis + } + + if (qcurrent < 0) + { + current_electric_angle+=fabsf(delta); + } else + { + current_electric_angle-=fabsf(delta); + } + current_electric_angle = _normalizeAngle(current_electric_angle); + + + // Average the d-axis angle further for calculating the electrical zero later + if (axis) + { + d_electrical_angle = i < 2 ? current_electric_angle : d_electrical_angle * 0.9 + current_electric_angle * 0.1; + } + + } + + // We know the minimum is 0.5*PI radians away, so less iterations are needed. + current_electric_angle += 0.5f * _PI; + current_electric_angle = _normalizeAngle(current_electric_angle); + iterations /= 2; + + if (axis == 0) + { + Lq = Ltemp; + }else + { + Ld = Ltemp; + } + + } + + if (sensor) + { + /** + * The d_electrical_angle should now be aligned to the d axis or the -d axis. We can therefore calculate two possible electrical zero angles. + * We then report the one closest to the actual value. This could be useful if the zero search method is not reliable enough (eg. high pole count). + */ + + float estimated_zero_electric_angle_A = _normalizeAngle( (float)(sensor_direction * pole_pairs) * sensor->getMechanicalAngle() - d_electrical_angle); + float estimated_zero_electric_angle_B = _normalizeAngle( (float)(sensor_direction * pole_pairs) * sensor->getMechanicalAngle() - d_electrical_angle + _PI); + float estimated_zero_electric_angle = 0.0f; + if (fabsf(estimated_zero_electric_angle_A - zero_electric_angle) < fabsf(estimated_zero_electric_angle_B - zero_electric_angle)) + { + estimated_zero_electric_angle = estimated_zero_electric_angle_A; + } else + { + estimated_zero_electric_angle = estimated_zero_electric_angle_B; + } + + SIMPLEFOC_DEBUG("MOT: Newly estimated electrical zero: ", estimated_zero_electric_angle); + SIMPLEFOC_DEBUG("MOT: Current electrical zero: ", zero_electric_angle); + } + + + SIMPLEFOC_DEBUG("MOT: Inductance measurement complete!"); + SIMPLEFOC_DEBUG("MOT: Measured D-inductance in mH: ", Ld * 1000.0f); + SIMPLEFOC_DEBUG("MOT: Measured Q-inductance in mH: ", Lq * 1000.0f); + if (Ld > Lq) + { + SIMPLEFOC_DEBUG("WARN: MOT: Measured inductance is larger in D than in Q axis. This is normally a sign of a measurement error."); + } + if (Ld * 2.0f < Lq) + { + SIMPLEFOC_DEBUG("WARN: MOT: Measured Q inductance is more than twice the D inductance. This is probably wrong. From experience, the lower value is probably close to reality."); + } + + return 0; + +} + // 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 b5b0a557..d21e068e 100644 --- a/src/common/base_classes/FOCMotor.h +++ b/src/common/base_classes/FOCMotor.h @@ -22,7 +22,7 @@ #define _MON_ANGLE 0b0000001 // monitor angle value /** - * Motiron control type + * Motion control type */ enum MotionControlType : uint8_t { torque = 0x00, //!< Torque control @@ -78,7 +78,7 @@ class FOCMotor FOCMotor(); /** Motor hardware init function */ - virtual void init()=0; + virtual int init()=0; /** Motor disable function */ virtual void disable()=0; /** Motor enable function */ @@ -149,6 +149,15 @@ class FOCMotor */ float electricalAngle(); + /** + * Measure resistance and inductance of a motor and print results to debug. + * If a sensor is available, an estimate of zero electric angle will be reported too. + * @param voltage The voltage applied to the motor + * @param correction_factor Is 1.5 for 3 phase motors, because we measure over a series-parallel connection. TODO: what about 2 phase motors? + * @returns 0 for success, >0 for failure + */ + int characteriseMotor(float voltage, float correction_factor); + // state variables float target; //!< current target value - depends of the controller float feed_forward_velocity = 0.0f; //!< current feed forward velocity @@ -161,6 +170,10 @@ 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 + + DQCurrent_s feed_forward_current;//!< current d and q current measured + DQVoltage_s feed_forward_voltage;//!< current d and q voltage set to the motor // motor configuration parameters float voltage_sensor_align;//!< sensor and motor align voltage parameter diff --git a/src/common/base_classes/Sensor.cpp b/src/common/base_classes/Sensor.cpp index db17e92e..cd7bb170 100644 --- a/src/common/base_classes/Sensor.cpp +++ b/src/common/base_classes/Sensor.cpp @@ -19,6 +19,7 @@ void Sensor::update() { /** get current angular velocity (rad/s) */ float Sensor::getVelocity() { // calculate sample time + // if timestamps were unsigned, we could get rid of this section, unsigned overflow handles it correctly 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; @@ -28,10 +29,28 @@ float Sensor::getVelocity() { } if (Ts < min_elapsed_time) return velocity; // don't update velocity if deltaT is too small - velocity = ( (float)(full_rotations - vel_full_rotations)*_2PI + (angle_prev - vel_angle_prev) ) / Ts; - vel_angle_prev = angle_prev; - vel_full_rotations = full_rotations; - vel_angle_prev_ts = angle_prev_ts; + float current_angle = 0.0f; + float prev_angle = 0.0f; + // Avoid floating point precision loss for large full_rotations + // this is likely optional + if (full_rotations == vel_full_rotations) { + current_angle = angle_prev; + prev_angle = vel_angle_prev; + } else { + current_angle = (float) full_rotations * _2PI + angle_prev; + prev_angle = (float) vel_full_rotations * _2PI + vel_angle_prev; + } + const float delta_angle = current_angle - prev_angle; + + // floating point equality checks are bad, so instead we check that the angle change is very small + if (fabsf(delta_angle) > 1e-8f) { + velocity = delta_angle / Ts; + + vel_angle_prev = angle_prev; + vel_full_rotations = full_rotations; + vel_angle_prev_ts = angle_prev_ts; + } + return velocity; } diff --git a/src/common/base_classes/StepperDriver.h b/src/common/base_classes/StepperDriver.h index 2006fc8c..9864b235 100644 --- a/src/common/base_classes/StepperDriver.h +++ b/src/common/base_classes/StepperDriver.h @@ -1,32 +1,30 @@ #ifndef STEPPERDRIVER_H #define STEPPERDRIVER_H -#include "drivers/hardware_api.h" +#include "Arduino.h" +#include "FOCDriver.h" -class StepperDriver{ +class StepperDriver: public FOCDriver{ public: - /** Initialise hardware */ - virtual int init() = 0; - /** Enable hardware */ - virtual void enable() = 0; - /** Disable hardware */ - virtual void disable() = 0; - - long pwm_frequency; //!< pwm frequency value in hertz - float voltage_power_supply; //!< power supply voltage - float voltage_limit; //!< limiting voltage set to the motor - - bool initialized = false; // true if driver was successfully initialized - void* params = 0; // pointer to hardware specific parameters of driver - /** - * Set phase voltages to the harware + * Set phase voltages to the hardware * * @param Ua phase A voltage * @param Ub phase B voltage */ virtual void setPwm(float Ua, float Ub) = 0; + + /** + * Set phase state, enable/disable + * + * @param sc - phase A state : active / disabled ( high impedance ) + * @param sb - phase B state : active / disabled ( high impedance ) + */ + virtual void setPhaseState(PhaseState sa, PhaseState sb) = 0; + + /** driver type getter function */ + virtual DriverType type() override { return DriverType::Stepper; } ; }; #endif \ No newline at end of file diff --git a/src/common/foc_utils.cpp b/src/common/foc_utils.cpp index c938d363..7ae372f7 100644 --- a/src/common/foc_utils.cpp +++ b/src/common/foc_utils.cpp @@ -9,20 +9,21 @@ __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}; + int32_t t1, t2; unsigned int i = (unsigned int)(a * (64*4*256.0f/_2PI)); - int t1, t2, frac = i & 0xff; + int frac = i & 0xff; i = (i >> 8) & 0xff; if (i < 64) { - t1 = sine_array[i]; t2 = sine_array[i+1]; + t1 = (int32_t)sine_array[i]; t2 = (int32_t)sine_array[i+1]; } else if(i < 128) { - t1 = sine_array[128 - i]; t2 = sine_array[127 - i]; + t1 = (int32_t)sine_array[128 - i]; t2 = (int32_t)sine_array[127 - i]; } else if(i < 192) { - t1 = -sine_array[-128 + i]; t2 = -sine_array[-127 + i]; + t1 = -(int32_t)sine_array[-128 + i]; t2 = -(int32_t)sine_array[-127 + i]; } else { - t1 = -sine_array[256 - i]; t2 = -sine_array[255 - i]; + t1 = -(int32_t)sine_array[256 - i]; t2 = -(int32_t)sine_array[255 - i]; } return (1.0f/32768.0f) * (t1 + (((t2 - t1) * frac) >> 8)); } diff --git a/src/common/foc_utils.h b/src/common/foc_utils.h index f2bc9ef6..2094ab26 100644 --- a/src/common/foc_utils.h +++ b/src/common/foc_utils.h @@ -14,6 +14,8 @@ #define _UNUSED(v) (void) (v) #define _powtwo(x) (1 << (x)) +#define _swap(a, b) { auto temp = a; a = b; b = temp; } + // utility defines #define _2_SQRT3 1.15470053838f #define _SQRT3 1.73205080757f @@ -33,7 +35,7 @@ #define _HIGH_IMPEDANCE 0 #define _HIGH_Z _HIGH_IMPEDANCE #define _ACTIVE 1 -#define _NC (NOT_SET) +#define _NC ((int) NOT_SET) #define MIN_ANGLE_DETECT_MOVEMENT (_2PI/101.0f) diff --git a/src/communication/Commander.h b/src/communication/Commander.h index 4ec2b281..24bd3fbe 100644 --- a/src/communication/Commander.h +++ b/src/communication/Commander.h @@ -15,7 +15,7 @@ enum VerboseMode : uint8_t { nothing = 0x00, // display nothing - good for monitoring on_request = 0x01, // display only on user request - user_friendly = 0x02, // display textual messages to the user + user_friendly = 0x02, // display textual messages to the user machine_readable = 0x03 // display machine readable commands, matching commands to set each settings }; @@ -86,7 +86,7 @@ class Commander void run(char* user_input); /** - * Function adding a callback to the coomander withe the command id + * Function adding a callback to the commander with the command id * @param id - char command letter * @param onCommand - function pointer void function(char*) * @param label - string label to be displayed when scan command sent diff --git a/src/communication/SimpleFOCDebug.cpp b/src/communication/SimpleFOCDebug.cpp index e969d8a2..4d50b87c 100644 --- a/src/communication/SimpleFOCDebug.cpp +++ b/src/communication/SimpleFOCDebug.cpp @@ -38,6 +38,7 @@ void SimpleFOCDebug::println(const __FlashStringHelper* str) { } } + void SimpleFOCDebug::println(const char* str, float val) { if (_debugPrint != NULL) { _debugPrint->print(str); @@ -58,6 +59,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) { @@ -80,6 +87,20 @@ void SimpleFOCDebug::print(const __FlashStringHelper* str) { } } +void SimpleFOCDebug::print(const StringSumHelper str) { + if (_debugPrint != NULL) { + _debugPrint->print(str.c_str()); + } +} + + +void SimpleFOCDebug::println(const StringSumHelper str) { + if (_debugPrint != NULL) { + _debugPrint->println(str.c_str()); + } +} + + void SimpleFOCDebug::print(int val) { if (_debugPrint != NULL) { diff --git a/src/communication/SimpleFOCDebug.h b/src/communication/SimpleFOCDebug.h index 614e6371..d0ff611f 100644 --- a/src/communication/SimpleFOCDebug.h +++ b/src/communication/SimpleFOCDebug.h @@ -32,25 +32,29 @@ * **/ +// #define SIMPLEFOC_DISABLE_DEBUG -#ifndef SIMPLEFOC_DISABLE_DEBUG +#ifndef SIMPLEFOC_DISABLE_DEBUG class SimpleFOCDebug { public: static void enable(Print* debugPrint = &Serial); static void println(const __FlashStringHelper* msg); + static void println(const StringSumHelper msg); static void println(const char* msg); static void println(const __FlashStringHelper* msg, float val); 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); static void print(const char* msg); static void print(const __FlashStringHelper* msg); + static void print(const StringSumHelper msg); static void print(int val); static void print(float val); @@ -62,10 +66,6 @@ class SimpleFOCDebug { #define SIMPLEFOC_DEBUG(msg, ...) \ SimpleFOCDebug::println(F(msg), ##__VA_ARGS__) - - - - #else //ifndef SIMPLEFOC_DISABLE_DEBUG diff --git a/src/current_sense/GenericCurrentSense.cpp b/src/current_sense/GenericCurrentSense.cpp index 635535fa..54c4f12e 100644 --- a/src/current_sense/GenericCurrentSense.cpp +++ b/src/current_sense/GenericCurrentSense.cpp @@ -54,110 +54,10 @@ PhaseCurrent_s GenericCurrentSense::getPhaseCurrents(){ // returns flag // 0 - fail // 1 - success and nothing changed -// 2 - success but pins reconfigured -// 3 - success but gains inverted -// 4 - success but pins reconfigured and gains inverted -int GenericCurrentSense::driverAlign(float voltage){ +int GenericCurrentSense::driverAlign(float voltage, bool modulation_centered){ _UNUSED(voltage) ; // remove unused parameter warning int exit_flag = 1; if(skip_align) return exit_flag; - if (!initialized) return 0; - - // // set phase A active and phases B and C down - // driver->setPwm(voltage, 0, 0); - // _delay(200); - // PhaseCurrent_s c = getPhaseCurrents(); - // // read the current 100 times ( arbitrary number ) - // for (int i = 0; i < 100; i++) { - // PhaseCurrent_s c1 = getPhaseCurrents(); - // 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); - // // align phase A - // float ab_ratio = fabs(c.a / c.b); - // float ac_ratio = c.c ? fabs(c.a / c.c) : 0; - // if( ab_ratio > 1.5f ){ // should be ~2 - // gain_a *= _sign(c.a); - // }else if( ab_ratio < 0.7f ){ // should be ~0.5 - // // switch phase A and B - // int tmp_pinA = pinA; - // pinA = pinB; - // pinB = tmp_pinA; - // gain_a *= _sign(c.b); - // exit_flag = 2; // signal that pins have been switched - // }else if(_isset(pinC) && ac_ratio < 0.7f ){ // should be ~0.5 - // // switch phase A and C - // int tmp_pinA = pinA; - // pinA = pinC; - // pinC= tmp_pinA; - // gain_a *= _sign(c.c); - // exit_flag = 2;// signal that pins have been switched - // }else{ - // // error in current sense - phase either not measured or bad connection - // return 0; - // } - - // // set phase B active and phases A and C down - // driver->setPwm(0, voltage, 0); - // _delay(200); - // c = getPhaseCurrents(); - // // read the current 50 times - // for (int i = 0; i < 100; i++) { - // PhaseCurrent_s c1 = getPhaseCurrents(); - // 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); - // float ba_ratio = fabs(c.b/c.a); - // float bc_ratio = c.c ? fabs(c.b / c.c) : 0; - // if( ba_ratio > 1.5f ){ // should be ~2 - // gain_b *= _sign(c.b); - // }else if( ba_ratio < 0.7f ){ // it should be ~0.5 - // // switch phase A and B - // int tmp_pinB = pinB; - // pinB = pinA; - // pinA = tmp_pinB; - // gain_b *= _sign(c.a); - // exit_flag = 2; // signal that pins have been switched - // }else if(_isset(pinC) && bc_ratio < 0.7f ){ // should be ~0.5 - // // switch phase A and C - // int tmp_pinB = pinB; - // pinB = pinC; - // pinC = tmp_pinB; - // gain_b *= _sign(c.c); - // exit_flag = 2; // signal that pins have been switched - // }else{ - // // error in current sense - phase either not measured or bad connection - // return 0; - // } - - // // if phase C measured - // if(_isset(pinC)){ - // // set phase B active and phases A and C down - // driver->setPwm(0, 0, voltage); - // _delay(200); - // c = getPhaseCurrents(); - // // read the adc voltage 500 times ( arbitrary number ) - // for (int i = 0; i < 50; i++) { - // PhaseCurrent_s c1 = getPhaseCurrents(); - // c.c = (c.c+c1.c)/50.0f; - // } - // driver->setPwm(0, 0, 0); - // gain_c *= _sign(c.c); - // } - - // if(gain_a < 0 || gain_b < 0 || gain_c < 0) exit_flag +=2; - // exit flag is either - // 0 - fail - // 1 - success and nothing changed - // 2 - success but pins reconfigured - // 3 - success but gains inverted - // 4 - success but pins reconfigured and gains inverted return exit_flag; } diff --git a/src/current_sense/GenericCurrentSense.h b/src/current_sense/GenericCurrentSense.h index a63df49d..02bf8fa9 100644 --- a/src/current_sense/GenericCurrentSense.h +++ b/src/current_sense/GenericCurrentSense.h @@ -20,7 +20,7 @@ class GenericCurrentSense: public CurrentSense{ // CurrentSense interface implementing functions int init() override; PhaseCurrent_s getPhaseCurrents() override; - int driverAlign(float align_voltage) override; + int driverAlign(float align_voltage, bool modulation_centered) override; PhaseCurrent_s (*readCallback)() = nullptr; //!< function pointer to sensor reading diff --git a/src/current_sense/InlineCurrentSense.cpp b/src/current_sense/InlineCurrentSense.cpp index c3db74ef..35c97765 100644 --- a/src/current_sense/InlineCurrentSense.cpp +++ b/src/current_sense/InlineCurrentSense.cpp @@ -44,8 +44,14 @@ int InlineCurrentSense::init(){ params = _configureADCInline(drv_params,pinA,pinB,pinC); // if init failed return fail if (params == SIMPLEFOC_CURRENT_SENSE_INIT_FAILED) return 0; + // set the center pwm (0 voltage vector) + if(driver_type==DriverType::BLDC) + static_cast(driver)->setPwm(driver->voltage_limit/2, driver->voltage_limit/2, driver->voltage_limit/2); // calibrate zero offsets calibrateOffsets(); + // set zero voltage to all phases + if(driver_type==DriverType::BLDC) + static_cast(driver)->setPwm(0,0,0); // set the initialized flag initialized = (params!=SIMPLEFOC_CURRENT_SENSE_INIT_FAILED); // return success @@ -80,174 +86,3 @@ PhaseCurrent_s InlineCurrentSense::getPhaseCurrents(){ current.c = (!_isset(pinC)) ? 0 : (_readADCVoltageInline(pinC, params) - offset_ic)*gain_c; // amps return current; } - -// Function aligning the current sense with motor driver -// if all pins are connected well none of this is really necessary! - can be avoided -// returns flag -// 0 - fail -// 1 - success and nothing changed -// 2 - success but pins reconfigured -// 3 - success but gains inverted -// 4 - success but pins reconfigured and gains inverted -int InlineCurrentSense::driverAlign(float voltage){ - - int exit_flag = 1; - if(skip_align) return exit_flag; - - if (driver==nullptr) { - SIMPLEFOC_DEBUG("CUR: No driver linked!"); - return 0; - } - - if (!initialized) return 0; - - if(_isset(pinA)){ - // set phase A active and phases B and C down - driver->setPwm(voltage, 0, 0); - _delay(2000); - PhaseCurrent_s c = getPhaseCurrents(); - // read the current 100 times ( arbitrary number ) - for (int i = 0; i < 100; i++) { - PhaseCurrent_s c1 = getPhaseCurrents(); - 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); - // align phase A - float ab_ratio = c.b ? fabs(c.a / c.b) : 0; - float ac_ratio = c.c ? fabs(c.a / c.c) : 0; - if(_isset(pinB) && ab_ratio > 1.5f ){ // should be ~2 - gain_a *= _sign(c.a); - }else if(_isset(pinC) && ac_ratio > 1.5f ){ // should be ~2 - gain_a *= _sign(c.a); - }else if(_isset(pinB) && ab_ratio < 0.7f ){ // should be ~0.5 - // switch phase A and B - int tmp_pinA = pinA; - pinA = pinB; - pinB = tmp_pinA; - float tmp_offsetA = offset_ia; - offset_ia = offset_ib; - offset_ib = tmp_offsetA; - gain_a *= _sign(c.b); - exit_flag = 2; // signal that pins have been switched - }else if(_isset(pinC) && ac_ratio < 0.7f ){ // should be ~0.5 - // switch phase A and C - int tmp_pinA = pinA; - pinA = pinC; - pinC= tmp_pinA; - float tmp_offsetA = offset_ia; - offset_ia = offset_ic; - offset_ic = tmp_offsetA; - gain_a *= _sign(c.c); - exit_flag = 2;// signal that pins have been switched - }else{ - // error in current sense - phase either not measured or bad connection - return 0; - } - } - - if(_isset(pinB)){ - // set phase B active and phases A and C down - driver->setPwm(0, voltage, 0); - _delay(200); - PhaseCurrent_s c = getPhaseCurrents(); - // read the current 50 times - for (int i = 0; i < 100; i++) { - PhaseCurrent_s c1 = getPhaseCurrents(); - 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); - float ba_ratio = c.a ? fabs(c.b / c.a) : 0; - float bc_ratio = c.c ? fabs(c.b / c.c) : 0; - if(_isset(pinA) && ba_ratio > 1.5f ){ // should be ~2 - gain_b *= _sign(c.b); - }else if(_isset(pinC) && bc_ratio > 1.5f ){ // should be ~2 - gain_b *= _sign(c.b); - }else if(_isset(pinA) && ba_ratio < 0.7f ){ // it should be ~0.5 - // switch phase A and B - int tmp_pinB = pinB; - pinB = pinA; - pinA = tmp_pinB; - float tmp_offsetB = offset_ib; - offset_ib = offset_ia; - offset_ia = tmp_offsetB; - gain_b *= _sign(c.a); - exit_flag = 2; // signal that pins have been switched - }else if(_isset(pinC) && bc_ratio < 0.7f ){ // should be ~0.5 - // switch phase A and C - int tmp_pinB = pinB; - pinB = pinC; - pinC = tmp_pinB; - float tmp_offsetB = offset_ib; - offset_ib = offset_ic; - offset_ic = tmp_offsetB; - gain_b *= _sign(c.c); - exit_flag = 2; // signal that pins have been switched - }else{ - // error in current sense - phase either not measured or bad connection - return 0; - } - } - - // if phase C measured - if(_isset(pinC)){ - // set phase C active and phases A and B down - driver->setPwm(0, 0, voltage); - _delay(200); - PhaseCurrent_s c = getPhaseCurrents(); - // read the adc voltage 500 times ( arbitrary number ) - for (int i = 0; i < 100; i++) { - PhaseCurrent_s c1 = getPhaseCurrents(); - 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); - float ca_ratio = c.a ? fabs(c.c / c.a) : 0; - float cb_ratio = c.b ? fabs(c.c / c.b) : 0; - if(_isset(pinA) && ca_ratio > 1.5f ){ // should be ~2 - gain_c *= _sign(c.c); - }else if(_isset(pinB) && cb_ratio > 1.5f ){ // should be ~2 - gain_c *= _sign(c.c); - }else if(_isset(pinA) && ca_ratio < 0.7f ){ // it should be ~0.5 - // switch phase A and C - int tmp_pinC = pinC; - pinC = pinA; - pinA = tmp_pinC; - float tmp_offsetC = offset_ic; - offset_ic = offset_ia; - offset_ia = tmp_offsetC; - gain_c *= _sign(c.a); - exit_flag = 2; // signal that pins have been switched - }else if(_isset(pinB) && cb_ratio < 0.7f ){ // should be ~0.5 - // switch phase B and C - int tmp_pinC = pinC; - pinC = pinB; - pinB = tmp_pinC; - float tmp_offsetC = offset_ic; - offset_ic = offset_ib; - offset_ib = tmp_offsetC; - gain_c *= _sign(c.b); - exit_flag = 2; // signal that pins have been switched - }else{ - // error in current sense - phase either not measured or bad connection - return 0; - } - } - - if(gain_a < 0 || gain_b < 0 || gain_c < 0) exit_flag +=2; - // exit flag is either - // 0 - fail - // 1 - success and nothing changed - // 2 - success but pins reconfigured - // 3 - success but gains inverted - // 4 - success but pins reconfigured and gains inverted - - return exit_flag; -} diff --git a/src/current_sense/InlineCurrentSense.h b/src/current_sense/InlineCurrentSense.h index 5fdca7d7..94be0f75 100644 --- a/src/current_sense/InlineCurrentSense.h +++ b/src/current_sense/InlineCurrentSense.h @@ -6,10 +6,13 @@ #include "../common/time_utils.h" #include "../common/defaults.h" #include "../common/base_classes/CurrentSense.h" +#include "../common/base_classes/StepperDriver.h" +#include "../common/base_classes/BLDCDriver.h" #include "../common/lowpass_filter.h" #include "hardware_api.h" + class InlineCurrentSense: public CurrentSense{ public: /** @@ -33,31 +36,9 @@ class InlineCurrentSense: public CurrentSense{ // CurrentSense interface implementing functions int init() override; PhaseCurrent_s getPhaseCurrents() override; - int driverAlign(float align_voltage) override; - - // ADC measuremnet gain for each phase - // support for different gains for different phases of more commonly - inverted phase currents - // this should be automated later - float gain_a; //!< phase A gain - float gain_b; //!< phase B gain - float gain_c; //!< phase C gain - - // // per phase low pass fileters - // LowPassFilter lpf_a{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current A low pass filter - // LowPassFilter lpf_b{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current B low pass filter - // LowPassFilter lpf_c{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current C low pass filter - float offset_ia; //!< zero current A voltage value (center of the adc reading) - float offset_ib; //!< zero current B voltage value (center of the adc reading) - float offset_ic; //!< zero current C voltage value (center of the adc reading) - private: - // hardware variables - int pinA; //!< pin A analog pin for current measurement - int pinB; //!< pin B analog pin for current measurement - int pinC; //!< pin C analog pin for current measurement - // gain variables float shunt_resistor; //!< Shunt resistor value float amp_gain; //!< amp gain value diff --git a/src/current_sense/LowsideCurrentSense.cpp b/src/current_sense/LowsideCurrentSense.cpp index aeb8dea0..a0026ae3 100644 --- a/src/current_sense/LowsideCurrentSense.cpp +++ b/src/current_sense/LowsideCurrentSense.cpp @@ -47,9 +47,16 @@ int LowsideCurrentSense::init(){ // if init failed return fail if (params == SIMPLEFOC_CURRENT_SENSE_INIT_FAILED) return 0; // sync the driver - _driverSyncLowSide(driver->params, params); + void* r = _driverSyncLowSide(driver->params, params); + if(r == SIMPLEFOC_CURRENT_SENSE_INIT_FAILED) return 0; + // set the center pwm (0 voltage vector) + if(driver_type==DriverType::BLDC) + static_cast(driver)->setPwm(driver->voltage_limit/2, driver->voltage_limit/2, driver->voltage_limit/2); // calibrate zero offsets calibrateOffsets(); + // set zero voltage to all phases + if(driver_type==DriverType::BLDC) + static_cast(driver)->setPwm(0,0,0); // set the initialized flag initialized = (params!=SIMPLEFOC_CURRENT_SENSE_INIT_FAILED); // return success @@ -86,169 +93,3 @@ PhaseCurrent_s LowsideCurrentSense::getPhaseCurrents(){ current.c = (!_isset(pinC)) ? 0 : (_readADCVoltageLowSide(pinC, params) - offset_ic)*gain_c; // amps return current; } - -// Function aligning the current sense with motor driver -// if all pins are connected well none of this is really necessary! - can be avoided -// returns flag -// 0 - fail -// 1 - success and nothing changed -// 2 - success but pins reconfigured -// 3 - success but gains inverted -// 4 - success but pins reconfigured and gains inverted -int LowsideCurrentSense::driverAlign(float voltage){ - - int exit_flag = 1; - if(skip_align) return exit_flag; - - if (!initialized) return 0; - - if(_isset(pinA)){ - // set phase A active and phases B and C down - driver->setPwm(voltage, 0, 0); - _delay(2000); - PhaseCurrent_s c = getPhaseCurrents(); - // read the current 100 times ( arbitrary number ) - for (int i = 0; i < 100; i++) { - PhaseCurrent_s c1 = getPhaseCurrents(); - 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); - // align phase A - float ab_ratio = c.b ? fabs(c.a / c.b) : 0; - float ac_ratio = c.c ? fabs(c.a / c.c) : 0; - if(_isset(pinB) && ab_ratio > 1.5f ){ // should be ~2 - gain_a *= _sign(c.a); - }else if(_isset(pinC) && ac_ratio > 1.5f ){ // should be ~2 - gain_a *= _sign(c.a); - }else if(_isset(pinB) && ab_ratio < 0.7f ){ // should be ~0.5 - // switch phase A and B - int tmp_pinA = pinA; - pinA = pinB; - pinB = tmp_pinA; - float tmp_offsetA = offset_ia; - offset_ia = offset_ib; - offset_ib = tmp_offsetA; - gain_a *= _sign(c.b); - exit_flag = 2; // signal that pins have been switched - }else if(_isset(pinC) && ac_ratio < 0.7f ){ // should be ~0.5 - // switch phase A and C - int tmp_pinA = pinA; - pinA = pinC; - pinC= tmp_pinA; - float tmp_offsetA = offset_ia; - offset_ia = offset_ic; - offset_ic = tmp_offsetA; - gain_a *= _sign(c.c); - exit_flag = 2;// signal that pins have been switched - }else{ - // error in current sense - phase either not measured or bad connection - return 0; - } - } - - if(_isset(pinB)){ - // set phase B active and phases A and C down - driver->setPwm(0, voltage, 0); - _delay(200); - PhaseCurrent_s c = getPhaseCurrents(); - // read the current 50 times - for (int i = 0; i < 100; i++) { - PhaseCurrent_s c1 = getPhaseCurrents(); - 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); - float ba_ratio = c.a ? fabs(c.b / c.a) : 0; - float bc_ratio = c.c ? fabs(c.b / c.c) : 0; - if(_isset(pinA) && ba_ratio > 1.5f ){ // should be ~2 - gain_b *= _sign(c.b); - }else if(_isset(pinC) && bc_ratio > 1.5f ){ // should be ~2 - gain_b *= _sign(c.b); - }else if(_isset(pinA) && ba_ratio < 0.7f ){ // it should be ~0.5 - // switch phase A and B - int tmp_pinB = pinB; - pinB = pinA; - pinA = tmp_pinB; - float tmp_offsetB = offset_ib; - offset_ib = offset_ia; - offset_ia = tmp_offsetB; - gain_b *= _sign(c.a); - exit_flag = 2; // signal that pins have been switched - }else if(_isset(pinC) && bc_ratio < 0.7f ){ // should be ~0.5 - // switch phase A and C - int tmp_pinB = pinB; - pinB = pinC; - pinC = tmp_pinB; - float tmp_offsetB = offset_ib; - offset_ib = offset_ic; - offset_ic = tmp_offsetB; - gain_b *= _sign(c.c); - exit_flag = 2; // signal that pins have been switched - }else{ - // error in current sense - phase either not measured or bad connection - return 0; - } - } - - // if phase C measured - if(_isset(pinC)){ - // set phase C active and phases A and B down - driver->setPwm(0, 0, voltage); - _delay(200); - PhaseCurrent_s c = getPhaseCurrents(); - // read the adc voltage 500 times ( arbitrary number ) - for (int i = 0; i < 100; i++) { - PhaseCurrent_s c1 = getPhaseCurrents(); - 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); - float ca_ratio = c.a ? fabs(c.c / c.a) : 0; - float cb_ratio = c.b ? fabs(c.c / c.b) : 0; - if(_isset(pinA) && ca_ratio > 1.5f ){ // should be ~2 - gain_c *= _sign(c.c); - }else if(_isset(pinB) && cb_ratio > 1.5f ){ // should be ~2 - gain_c *= _sign(c.c); - }else if(_isset(pinA) && ca_ratio < 0.7f ){ // it should be ~0.5 - // switch phase A and C - int tmp_pinC = pinC; - pinC = pinA; - pinA = tmp_pinC; - float tmp_offsetC = offset_ic; - offset_ic = offset_ia; - offset_ia = tmp_offsetC; - gain_c *= _sign(c.a); - exit_flag = 2; // signal that pins have been switched - }else if(_isset(pinB) && cb_ratio < 0.7f ){ // should be ~0.5 - // switch phase B and C - int tmp_pinC = pinC; - pinC = pinB; - pinB = tmp_pinC; - float tmp_offsetC = offset_ic; - offset_ic = offset_ib; - offset_ib = tmp_offsetC; - gain_c *= _sign(c.b); - exit_flag = 2; // signal that pins have been switched - }else{ - // error in current sense - phase either not measured or bad connection - return 0; - } - } - - if(gain_a < 0 || gain_b < 0 || gain_c < 0) exit_flag +=2; - // exit flag is either - // 0 - fail - // 1 - success and nothing changed - // 2 - success but pins reconfigured - // 3 - success but gains inverted - // 4 - success but pins reconfigured and gains inverted - - return exit_flag; -} diff --git a/src/current_sense/LowsideCurrentSense.h b/src/current_sense/LowsideCurrentSense.h index 1652b330..6651bcd2 100644 --- a/src/current_sense/LowsideCurrentSense.h +++ b/src/current_sense/LowsideCurrentSense.h @@ -7,6 +7,8 @@ #include "../common/defaults.h" #include "../common/base_classes/CurrentSense.h" #include "../common/base_classes/FOCMotor.h" +#include "../common/base_classes/StepperDriver.h" +#include "../common/base_classes/BLDCDriver.h" #include "../common/lowpass_filter.h" #include "hardware_api.h" @@ -34,30 +36,9 @@ class LowsideCurrentSense: public CurrentSense{ // CurrentSense interface implementing functions int init() override; PhaseCurrent_s getPhaseCurrents() override; - int driverAlign(float align_voltage) override; - // ADC measuremnet gain for each phase - // support for different gains for different phases of more commonly - inverted phase currents - // this should be automated later - float gain_a; //!< phase A gain - float gain_b; //!< phase B gain - float gain_c; //!< phase C gain - - // // per phase low pass fileters - // LowPassFilter lpf_a{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current A low pass filter - // LowPassFilter lpf_b{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current B low pass filter - // LowPassFilter lpf_c{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current C low pass filter - - float offset_ia; //!< zero current A voltage value (center of the adc reading) - float offset_ib; //!< zero current B voltage value (center of the adc reading) - float offset_ic; //!< zero current C voltage value (center of the adc reading) private: - // hardware variables - int pinA; //!< pin A analog pin for current measurement - int pinB; //!< pin B analog pin for current measurement - int pinC; //!< pin C analog pin for current measurement - // gain variables float shunt_resistor; //!< Shunt resistor value float amp_gain; //!< amp gain value diff --git a/src/current_sense/hardware_api.h b/src/current_sense/hardware_api.h index 7862b708..d1f5f160 100644 --- a/src/current_sense/hardware_api.h +++ b/src/current_sense/hardware_api.h @@ -59,7 +59,10 @@ float _readADCVoltageLowSide(const int pinA, const void* cs_params); * function syncing the Driver with the ADC for the LowSide Sensing * @param driver_params - driver parameter structure - hardware specific * @param cs_params - current sense parameter structure - hardware specific + * + * @return void* - returns the pointer to the current sense parameter structure (unchanged) + * - returns SIMPLEFOC_CURRENT_SENSE_INIT_FAILED if the init fails */ -void _driverSyncLowSide(void* driver_params, void* cs_params); +void* _driverSyncLowSide(void* driver_params, void* cs_params); #endif diff --git a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp index 57ffdfb5..4c0fa8dc 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp @@ -1,89 +1,27 @@ -#include "esp32_adc_driver.h" - -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(CONFIG_IDF_TARGET_ESP32S2) && !defined(CONFIG_IDF_TARGET_ESP32S3) -#include "freertos/FreeRTOS.h" -#include "freertos/task.h" -#include "rom/ets_sys.h" -#include "esp_attr.h" -//#include "esp_intr.h" deprecated -#include "esp_intr_alloc.h" -#include "soc/rtc_io_reg.h" -#include "soc/rtc_cntl_reg.h" -#include "soc/sens_reg.h" +#include "esp32_mcu.h" +#include "esp32_adc_driver.h" -static uint8_t __analogAttenuation = 3;//11db -static uint8_t __analogWidth = 3;//12 bits -static uint8_t __analogCycles = 8; -static uint8_t __analogSamples = 0;//1 sample -static uint8_t __analogClockDiv = 1; +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) +#define SIMPLEFOC_ADC_ATTEN ADC_11db +#define SIMPLEFOC_ADC_RES 12 -// Width of returned answer () -static uint8_t __analogReturnedWidth = 12; -void __analogSetWidth(uint8_t bits){ - if(bits < 9){ - bits = 9; - } else if(bits > 12){ - bits = 12; - } - __analogReturnedWidth = bits; - __analogWidth = bits - 9; - SET_PERI_REG_BITS(SENS_SAR_START_FORCE_REG, SENS_SAR1_BIT_WIDTH, __analogWidth, SENS_SAR1_BIT_WIDTH_S); - SET_PERI_REG_BITS(SENS_SAR_READ_CTRL_REG, SENS_SAR1_SAMPLE_BIT, __analogWidth, SENS_SAR1_SAMPLE_BIT_S); +#if CONFIG_IDF_TARGET_ESP32 // if esp32 variant - SET_PERI_REG_BITS(SENS_SAR_START_FORCE_REG, SENS_SAR2_BIT_WIDTH, __analogWidth, SENS_SAR2_BIT_WIDTH_S); - SET_PERI_REG_BITS(SENS_SAR_READ_CTRL2_REG, SENS_SAR2_SAMPLE_BIT, __analogWidth, SENS_SAR2_SAMPLE_BIT_S); -} - -void __analogSetCycles(uint8_t cycles){ - __analogCycles = cycles; - SET_PERI_REG_BITS(SENS_SAR_READ_CTRL_REG, SENS_SAR1_SAMPLE_CYCLE, __analogCycles, SENS_SAR1_SAMPLE_CYCLE_S); - SET_PERI_REG_BITS(SENS_SAR_READ_CTRL2_REG, SENS_SAR2_SAMPLE_CYCLE, __analogCycles, SENS_SAR2_SAMPLE_CYCLE_S); -} - -void __analogSetSamples(uint8_t samples){ - if(!samples){ - return; - } - __analogSamples = samples - 1; - SET_PERI_REG_BITS(SENS_SAR_READ_CTRL_REG, SENS_SAR1_SAMPLE_NUM, __analogSamples, SENS_SAR1_SAMPLE_NUM_S); - SET_PERI_REG_BITS(SENS_SAR_READ_CTRL2_REG, SENS_SAR2_SAMPLE_NUM, __analogSamples, SENS_SAR2_SAMPLE_NUM_S); -} - -void __analogSetClockDiv(uint8_t clockDiv){ - if(!clockDiv){ - return; - } - __analogClockDiv = clockDiv; - SET_PERI_REG_BITS(SENS_SAR_READ_CTRL_REG, SENS_SAR1_CLK_DIV, __analogClockDiv, SENS_SAR1_CLK_DIV_S); - SET_PERI_REG_BITS(SENS_SAR_READ_CTRL2_REG, SENS_SAR2_CLK_DIV, __analogClockDiv, SENS_SAR2_CLK_DIV_S); -} - -void __analogSetAttenuation(uint8_t attenuation) -{ - __analogAttenuation = attenuation & 3; - uint32_t att_data = 0; - int i = 10; - while(i--){ - att_data |= __analogAttenuation << (i * 2); - } - WRITE_PERI_REG(SENS_SAR_ATTEN1_REG, att_data & 0xFFFF);//ADC1 has 8 channels - WRITE_PERI_REG(SENS_SAR_ATTEN2_REG, att_data); -} +#include "soc/sens_reg.h" -void IRAM_ATTR __analogInit(){ - static bool initialized = false; - if(initialized){ - return; - } +// configure the ADCs in RTC mode +// saves about 3us per call +// going from 12us to 9us +// +// TODO: See if we need to configure both ADCs or we can just configure the one we'll use +// For the moment we will configure both +void IRAM_ATTR __configFastADCs(){ - __analogSetAttenuation(__analogAttenuation); - __analogSetCycles(__analogCycles); - __analogSetSamples(__analogSamples + 1);//in samples - __analogSetClockDiv(__analogClockDiv); - __analogSetWidth(__analogWidth + 9);//in bits + SIMPLEFOC_ESP32_CS_DEBUG("Configuring fast ADCs"); + // configure both ADCs in RTC mode SET_PERI_REG_MASK(SENS_SAR_READ_CTRL_REG, SENS_SAR1_DATA_INV); SET_PERI_REG_MASK(SENS_SAR_READ_CTRL2_REG, SENS_SAR2_DATA_INV); @@ -100,159 +38,165 @@ void IRAM_ATTR __analogInit(){ SET_PERI_REG_BITS(SENS_SAR_MEAS_WAIT1_REG, SENS_SAR_AMP_WAIT2, 0x1, SENS_SAR_AMP_WAIT2_S); SET_PERI_REG_BITS(SENS_SAR_MEAS_WAIT2_REG, SENS_SAR_AMP_WAIT3, 0x1, SENS_SAR_AMP_WAIT3_S); while (GET_PERI_REG_BITS2(SENS_SAR_SLAVE_ADDR1_REG, 0x7, SENS_MEAS_STATUS_S) != 0); //wait det_fsm== - - initialized = true; } -void __analogSetPinAttenuation(uint8_t pin, uint8_t attenuation) + +uint16_t IRAM_ATTR adcRead(uint8_t pin) { int8_t channel = digitalPinToAnalogChannel(pin); - if(channel < 0 || attenuation > 3){ - return ; - } - __analogInit(); - if(channel > 7){ - SET_PERI_REG_BITS(SENS_SAR_ATTEN2_REG, 3, attenuation, ((channel - 10) * 2)); - } else { - SET_PERI_REG_BITS(SENS_SAR_ATTEN1_REG, 3, attenuation, (channel * 2)); + if(channel < 0){ + SIMPLEFOC_ESP32_CS_DEBUG("ERROR: Not ADC pin: "+String(pin)); + return false; //not adc pin } -} -bool IRAM_ATTR __adcAttachPin(uint8_t pin){ + // channels <= MAX_CHANNEL_NUM belong to ADC1 + // channels > MAX_CHANNEL_NUM belong to ADC2 (where the channel number is number-SOC_ADC_MAX_CHANNEL_NUM) + uint8_t adc_num = (channel >= SOC_ADC_MAX_CHANNEL_NUM) ? 2 : 1; + uint8_t adc_channel = (adc_num == 2) ? (channel - SOC_ADC_MAX_CHANNEL_NUM) : channel; - int8_t channel = digitalPinToAnalogChannel(pin); - if(channel < 0){ - return false;//not adc pin - } + // variable to hold the ADC value + uint16_t value = 0; + // do the ADC conversion + switch(adc_num){ + case 1: + // start the ADC1 conversion + CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_START_SAR_M); + SET_PERI_REG_BITS(SENS_SAR_MEAS_START1_REG, SENS_SAR1_EN_PAD, (1 << channel), SENS_SAR1_EN_PAD_S); + SET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_START_SAR_M); + + // wait for conversion + while (GET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_DONE_SAR) == 0); + // read the value + value = GET_PERI_REG_BITS2(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_DATA_SAR, SENS_MEAS1_DATA_SAR_S); + break; + case 2: + // start the ADC2 conversion + CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_START_SAR_M); + SET_PERI_REG_BITS(SENS_SAR_MEAS_START2_REG, SENS_SAR2_EN_PAD, (1 << (adc_channel)), SENS_SAR2_EN_PAD_S); + SET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_START_SAR_M); + + // wait for conversion + while (GET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_DONE_SAR) == 0); + // read the value + value = GET_PERI_REG_BITS2(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_DATA_SAR, SENS_MEAS2_DATA_SAR_S); + break; + } + + // return value + return value; +} - int8_t pad = digitalPinToTouchChannel(pin); - if(pad >= 0){ - uint32_t touch = READ_PERI_REG(SENS_SAR_TOUCH_ENABLE_REG); - if(touch & (1 << pad)){ - touch &= ~((1 << (pad + SENS_TOUCH_PAD_OUTEN2_S)) - | (1 << (pad + SENS_TOUCH_PAD_OUTEN1_S)) - | (1 << (pad + SENS_TOUCH_PAD_WORKEN_S))); - WRITE_PERI_REG(SENS_SAR_TOUCH_ENABLE_REG, touch); - } - } else if(pin == 25){ - CLEAR_PERI_REG_MASK(RTC_IO_PAD_DAC1_REG, RTC_IO_PDAC1_XPD_DAC | RTC_IO_PDAC1_DAC_XPD_FORCE); //stop dac1 - } else if(pin == 26){ - CLEAR_PERI_REG_MASK(RTC_IO_PAD_DAC2_REG, RTC_IO_PDAC2_XPD_DAC | RTC_IO_PDAC2_DAC_XPD_FORCE); //stop dac2 - } +#elif CONFIG_IDF_TARGET_ESP32S2 || CONFIG_IDF_TARGET_ESP32S3 // if esp32 s2 or s3 variants - pinMode(pin, ANALOG); +#include "soc/sens_reg.h" - __analogInit(); - return true; -} -bool IRAM_ATTR __adcStart(uint8_t pin){ +// configure the ADCs in RTC mode +// no real gain - see if we do something with it later +// void __configFastADCs(){ - int8_t channel = digitalPinToAnalogChannel(pin); - if(channel < 0){ - return false;//not adc pin - } +// SET_PERI_REG_MASK(SENS_SAR_READER1_CTRL_REG, SENS_SAR1_DATA_INV); +// SET_PERI_REG_MASK(SENS_SAR_READER2_CTRL_REG, SENS_SAR2_DATA_INV); - if(channel > 9){ - CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_START_SAR_M); - SET_PERI_REG_BITS(SENS_SAR_MEAS_START2_REG, SENS_SAR2_EN_PAD, (1 << (channel - 10)), SENS_SAR2_EN_PAD_S); - SET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_START_SAR_M); - } else { - CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_START_SAR_M); - SET_PERI_REG_BITS(SENS_SAR_MEAS_START1_REG, SENS_SAR1_EN_PAD, (1 << channel), SENS_SAR1_EN_PAD_S); - SET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_START_SAR_M); - } - return true; -} +// SET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_START_FORCE_M); //SAR ADC1 controller (in RTC) is started by SW +// SET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_SAR1_EN_PAD_FORCE_M); //SAR ADC1 pad enable bitmap is controlled by SW +// SET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_START_FORCE_M); //SAR ADC2 controller (in RTC) is started by SW +// SET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_SAR2_EN_PAD_FORCE_M); //SAR ADC2 pad enable bitmap is controlled by SW -bool IRAM_ATTR __adcBusy(uint8_t pin){ +// CLEAR_PERI_REG_MASK(SENS_SAR_POWER_XPD_SAR_REG, SENS_FORCE_XPD_SAR_M); //force XPD_SAR=0, use XPD_FSM +// SET_PERI_REG_BITS(SENS_SAR_POWER_XPD_SAR_REG, SENS_FORCE_XPD_AMP, 0x2, SENS_FORCE_XPD_AMP_S); //force XPD_AMP=0 +// CLEAR_PERI_REG_MASK(SENS_SAR_AMP_CTRL3_REG, 0xfff << SENS_AMP_RST_FB_FSM_S); //clear FSM +// SET_PERI_REG_BITS(SENS_SAR_AMP_CTRL1_REG, SENS_SAR_AMP_WAIT1, 0x1, SENS_SAR_AMP_WAIT1_S); +// SET_PERI_REG_BITS(SENS_SAR_AMP_CTRL1_REG, SENS_SAR_AMP_WAIT2, 0x1, SENS_SAR_AMP_WAIT2_S); +// SET_PERI_REG_BITS(SENS_SAR_POWER_XPD_SAR_REG, SENS_SAR_AMP_WAIT3, 0x1, SENS_SAR_AMP_WAIT3_S); +// while (GET_PERI_REG_BITS2(SENS_SAR_SLAVE_ADDR1_REG, 0x7, SENS_SARADC_MEAS_STATUS_S) != 0); //wait det_fsm== +// } + +uint16_t IRAM_ATTR adcRead(uint8_t pin) +{ int8_t channel = digitalPinToAnalogChannel(pin); if(channel < 0){ - return false;//not adc pin + SIMPLEFOC_ESP32_CS_DEBUG("ERROR: Not ADC pin: "+String(pin)); + return false; //not adc pin } - if(channel > 7){ - return (GET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_DONE_SAR) == 0); - } - return (GET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_DONE_SAR) == 0); + // channels <= MAX_CHANNEL_NUM belong to ADC1 + // channels > MAX_CHANNEL_NUM belong to ADC2 (where the channel number is number-SOC_ADC_MAX_CHANNEL_NUM) + uint8_t adc_num = (channel >= SOC_ADC_MAX_CHANNEL_NUM) ? 2 : 1; + uint8_t adc_channel = (adc_num == 2) ? (channel - SOC_ADC_MAX_CHANNEL_NUM) : channel; + + // variable to hold the ADC value + uint16_t value = 0; + // do the ADC conversion + switch(adc_num){ + case 1: + // start the ADC1 conversion + CLEAR_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_START_SAR_M); + SET_PERI_REG_BITS(SENS_SAR_MEAS1_CTRL2_REG, SENS_SAR1_EN_PAD, (1 << (adc_channel)), SENS_SAR1_EN_PAD_S); + SET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_START_SAR_M); + + // wait for conversion + while (GET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_DONE_SAR) == 0); + // read the value + value = GET_PERI_REG_BITS2(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_DATA_SAR, SENS_MEAS1_DATA_SAR_S); + break; + case 2: + // start the ADC2 conversion + CLEAR_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_START_SAR_M); + SET_PERI_REG_BITS(SENS_SAR_MEAS2_CTRL2_REG, SENS_SAR2_EN_PAD, (1 << (adc_channel)), SENS_SAR2_EN_PAD_S); + SET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_START_SAR_M); + + // wait for conversion + while (GET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_DONE_SAR) == 0); + // read the value + value = GET_PERI_REG_BITS2(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_DATA_SAR, SENS_MEAS2_DATA_SAR_S); + break; + } + + return value; } -uint16_t IRAM_ATTR __adcEnd(uint8_t pin) -{ +#else // if others just use analogRead - uint16_t value = 0; - int8_t channel = digitalPinToAnalogChannel(pin); - if(channel < 0){ - return 0;//not adc pin - } - if(channel > 7){ - while (GET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_DONE_SAR) == 0); //wait for conversion - value = GET_PERI_REG_BITS2(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_DATA_SAR, SENS_MEAS2_DATA_SAR_S); - } else { - while (GET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_DONE_SAR) == 0); //wait for conversion - value = GET_PERI_REG_BITS2(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_DATA_SAR, SENS_MEAS1_DATA_SAR_S); - } +#pragma message("SimpleFOC: Using analogRead for ADC reading, no fast ADC configuration available!") - // Shift result if necessary - uint8_t from = __analogWidth + 9; - if (from == __analogReturnedWidth) { - return value; - } - if (from > __analogReturnedWidth) { - return value >> (from - __analogReturnedWidth); - } - return value << (__analogReturnedWidth - from); +uint16_t IRAM_ATTR adcRead(uint8_t pin){ + return analogRead(pin); } -void __analogReadResolution(uint8_t bits) -{ - if(!bits || bits > 16){ - return; - } - __analogSetWidth(bits); // hadware from 9 to 12 - __analogReturnedWidth = bits; // software from 1 to 16 -} +#endif + + +// configure the ADC for the pin +bool IRAM_ATTR adcInit(uint8_t pin){ + static bool initialized = false; -uint16_t IRAM_ATTR adcRead(uint8_t pin) -{ int8_t channel = digitalPinToAnalogChannel(pin); if(channel < 0){ + SIMPLEFOC_ESP32_CS_DEBUG("ERROR: Not ADC pin: "+String(pin)); return false;//not adc pin } - __analogInit(); - - if(channel > 9){ - CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_START_SAR_M); - SET_PERI_REG_BITS(SENS_SAR_MEAS_START2_REG, SENS_SAR2_EN_PAD, (1 << (channel - 10)), SENS_SAR2_EN_PAD_S); - SET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_START_SAR_M); - } else { - CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_START_SAR_M); - SET_PERI_REG_BITS(SENS_SAR_MEAS_START1_REG, SENS_SAR1_EN_PAD, (1 << channel), SENS_SAR1_EN_PAD_S); - SET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_START_SAR_M); - } + uint8_t adc_num = (channel >= SOC_ADC_MAX_CHANNEL_NUM) ? 2 : 1; + uint8_t adc_channel = (adc_num == 2) ? (channel - SOC_ADC_MAX_CHANNEL_NUM) : channel; - uint16_t value = 0; + SIMPLEFOC_ESP32_CS_DEBUG("Configuring ADC"+String(adc_num)+" channel "+String(adc_channel)); - if(channel > 7){ - while (GET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_DONE_SAR) == 0); //wait for conversion - value = GET_PERI_REG_BITS2(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_DATA_SAR, SENS_MEAS2_DATA_SAR_S); - } else { - while (GET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_DONE_SAR) == 0); //wait for conversion - value = GET_PERI_REG_BITS2(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_DATA_SAR, SENS_MEAS1_DATA_SAR_S); + if(! initialized){ + analogSetAttenuation(SIMPLEFOC_ADC_ATTEN); + analogReadResolution(SIMPLEFOC_ADC_RES); } + pinMode(pin, ANALOG); + analogRead(pin); + analogSetPinAttenuation(pin, SIMPLEFOC_ADC_ATTEN); - // Shift result if necessary - uint8_t from = __analogWidth + 9; - if (from == __analogReturnedWidth) { - return value; - } - if (from > __analogReturnedWidth) { - return value >> (from - __analogReturnedWidth); - } - return value << (__analogReturnedWidth - from); -} +#if CONFIG_IDF_TARGET_ESP32 // if esp32 variant + __configFastADCs(); +#endif + initialized = true; + return true; +} #endif diff --git a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h index 869c4dde..cad441fc 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h +++ b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h @@ -1,88 +1,24 @@ - - #ifndef SIMPLEFOC_ESP32_HAL_ADC_DRIVER_H_ #define SIMPLEFOC_ESP32_HAL_ADC_DRIVER_H_ -#include "Arduino.h" +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) -/* +/** * Get ADC value for pin + * @param pin - pin number + * @return ADC value (0 - 4095) * */ uint16_t adcRead(uint8_t pin); -/* - * Set the resolution of analogRead return values. Default is 12 bits (range from 0 to 4096). - * If between 9 and 12, it will equal the set hardware resolution, else value will be shifted. - * Range is 1 - 16 - * - * Note: compatibility with Arduino SAM +/** + * Initialize ADC pin + * @param pin - pin number + * + * @return true if success + * false if pin is not an ADC pin */ -void __analogReadResolution(uint8_t bits); - -/* - * Sets the sample bits and read resolution - * Default is 12bit (0 - 4095) - * Range is 9 - 12 - * */ -void __analogSetWidth(uint8_t bits); - -/* - * Set number of cycles per sample - * Default is 8 and seems to do well - * Range is 1 - 255 - * */ -void __analogSetCycles(uint8_t cycles); +bool adcInit(uint8_t pin); -/* - * Set number of samples in the range. - * Default is 1 - * Range is 1 - 255 - * This setting splits the range into - * "samples" pieces, which could look - * like the sensitivity has been multiplied - * that many times - * */ -void __analogSetSamples(uint8_t samples); - -/* - * Set the divider for the ADC clock. - * Default is 1 - * Range is 1 - 255 - * */ -void __analogSetClockDiv(uint8_t clockDiv); - -/* - * Set the attenuation for all channels - * Default is 11db - * */ -void __analogSetAttenuation(uint8_t attenuation); - -/* - * Set the attenuation for particular pin - * Default is 11db - * */ -void __analogSetPinAttenuation(uint8_t pin, uint8_t attenuation); - -/* - * Attach pin to ADC (will also clear any other analog mode that could be on) - * */ -bool __adcAttachPin(uint8_t pin); - -/* - * Start ADC conversion on attached pin's bus - * */ -bool __adcStart(uint8_t pin); - -/* - * Check if conversion on the pin's ADC bus is currently running - * */ -bool __adcBusy(uint8_t pin); - -/* - * Get the result of the conversion (will wait if it have not finished) - * */ -uint16_t __adcEnd(uint8_t pin); #endif /* SIMPLEFOC_ESP32_HAL_ADC_DRIVER_H_ */ #endif /* ESP32 */ \ No newline at end of file diff --git a/src/current_sense/hardware_specific/esp32/esp32_ledc_mcu.cpp b/src/current_sense/hardware_specific/esp32/esp32_ledc_mcu.cpp deleted file mode 100644 index f2d65f8e..00000000 --- a/src/current_sense/hardware_specific/esp32/esp32_ledc_mcu.cpp +++ /dev/null @@ -1,27 +0,0 @@ -#include "../../hardware_api.h" -#include "../../../drivers/hardware_api.h" - -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && !defined(SOC_MCPWM_SUPPORTED) - -#include "esp32_adc_driver.h" - -#define _ADC_VOLTAGE 3.3f -#define _ADC_RESOLUTION 4095.0f - -// function reading an ADC value and returning the read voltage -void* _configureADCInline(const void* driver_params, const int pinA,const int pinB,const int pinC){ - _UNUSED(driver_params); - - pinMode(pinA, INPUT); - pinMode(pinB, INPUT); - if( _isset(pinC) ) pinMode(pinC, INPUT); - - GenericCurrentSenseParams* params = new GenericCurrentSenseParams { - .pins = { pinA, pinB, pinC }, - .adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION) - }; - - return params; -} - -#endif diff --git a/src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp b/src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp new file mode 100644 index 00000000..51b200ff --- /dev/null +++ b/src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp @@ -0,0 +1,162 @@ +#include "esp32_mcu.h" + +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) + +// check the version of the ESP-IDF +#include "esp_idf_version.h" + +#if ESP_IDF_VERSION < ESP_IDF_VERSION_VAL(5, 0, 0) +#error SimpleFOC: ESP-IDF version 4 or lower detected. Please update to ESP-IDF 5.x and Arduino-esp32 3.0 (or higher) +#endif + +#include "../../../drivers/hardware_specific/esp32/esp32_driver_mcpwm.h" +#include "../../../drivers/hardware_specific/esp32/mcpwm_private.h" + +#include "driver/mcpwm_prelude.h" +#include "soc/mcpwm_reg.h" +#include "soc/mcpwm_struct.h" + + + +// adding a debug toggle pin to measure the time of the interrupt with oscilloscope + +// #define SIMPLEFOC_ESP32_INTERRUPT_DEBUG + +#ifdef SIMPLEFOC_ESP32_INTERRUPT_DEBUG +#include "driver/gpio.h" + +#ifdef CONFIG_IDF_TARGET_ESP32S3 +#define DEBUGPIN 16 +#define GPIO_NUM GPIO_NUM_16 +#else +#define DEBUGPIN 19 +#define GPIO_NUM GPIO_NUM_19 +#endif + +#endif + + + +/** + * Low side adc reading implementation +*/ + + +// function reading an ADC value and returning the read voltage +float IRAM_ATTR _readADCVoltageLowSide(const int pin, const void* cs_params){ + ESP32CurrentSenseParams* p = (ESP32CurrentSenseParams*)cs_params; + int no_channel = 0; + for(int i=0; i < 3; i++){ + if(!_isset(p->pins[i])) continue; + if(pin == p->pins[i]) // found in the buffer + return p->adc_buffer[no_channel] * p->adc_voltage_conv; + else no_channel++; + } + SIMPLEFOC_DEBUG("ERROR: ADC pin not found in the buffer!"); + // not found + return 0; +} + + +// function configuring low-side current sensing +void* IRAM_ATTR _configureADCLowSide(const void* driver_params, const int pinA,const int pinB,const int pinC){ + // check if driver timer is already running + // fail if it is + // the easiest way that I've found to check if timer is running + // is to start it and stop it + ESP32MCPWMDriverParams *p = (ESP32MCPWMDriverParams*)driver_params; + mcpwm_timer_t* t = (mcpwm_timer_t*) p->timers[0]; + + // check if low side callback is already set + // if it is, return error + if(t->on_full != nullptr){ + SIMPLEFOC_ESP32_CS_DEBUG("ERROR: Low side callback is already set. Cannot set it again for timer: "+String(t->timer_id)+", group: "+String(t->group->group_id)); + return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; + } + + + ESP32CurrentSenseParams* params = new ESP32CurrentSenseParams{}; + int no_adc_channels = 0; + + // initialize the ADC pins + // fail if the pin is not an ADC pin + int adc_pins[3] = {pinA, pinB, pinC}; + for (int i = 0; i < 3; i++){ + if(_isset(adc_pins[i])){ + if(!adcInit(adc_pins[i])){ + SIMPLEFOC_ESP32_CS_DEBUG("ERROR: Failed to initialise ADC pin: "+String(adc_pins[i]) + String(", maybe not an ADC pin?")); + return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; + } + params->pins[no_adc_channels++] = adc_pins[i]; + } + } + + t->user_data = params; + params->adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION); + params->no_adc_channels = no_adc_channels; + return params; +} + +static bool IRAM_ATTR _mcpwmTriggerADCCallback(mcpwm_timer_handle_t tim, const mcpwm_timer_event_data_t* edata, void* user_data){ + ESP32CurrentSenseParams *p = (ESP32CurrentSenseParams*)user_data; +#ifdef SIMPLEFOC_ESP32_INTERRUPT_DEBUG // debugging toggle pin to measure the time of the interrupt with oscilloscope + gpio_set_level(GPIO_NUM,1); //cca 250ns for on+off +#endif + + // sample the phase currents one at a time + // ESP's adc read takes around 10us which is very long + // so we are sampling one phase per call + p->adc_buffer[p->buffer_index] = adcRead(p->pins[p->buffer_index]); + + // increment buffer index + p->buffer_index++; + if(p->buffer_index >= p->no_adc_channels){ + p->buffer_index = 0; + } + +#ifdef SIMPLEFOC_ESP32_INTERRUPT_DEBUG // debugging toggle pin to measure the time of the interrupt with oscilloscope + gpio_set_level(GPIO_NUM,0); //cca 250ns for on+off +#endif + return true; +} + +void* IRAM_ATTR _driverSyncLowSide(void* driver_params, void* cs_params){ +#ifdef SIMPLEFOC_ESP32_INTERRUPT_DEBUG + pinMode(DEBUGPIN, OUTPUT); +#endif + ESP32MCPWMDriverParams *p = (ESP32MCPWMDriverParams*)driver_params; + mcpwm_timer_t* t = (mcpwm_timer_t*) p->timers[0]; + + // check if low side callback is already set + // if it is, return error + if(t->on_full != nullptr){ + SIMPLEFOC_ESP32_CS_DEBUG("ERROR: Low side callback is already set. Cannot set it again for timer: "+String(t->timer_id)+", group: "+String(t->group->group_id)); + return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; + } + + // set the callback for the low side current sensing + // mcpwm_timer_event_callbacks_t can be used to set the callback + // for three timer events + // - on_full - low-side + // - on_empty - high-side + // - on_sync - sync event (not used with simplefoc) + auto cbs = mcpwm_timer_event_callbacks_t{ + .on_full = _mcpwmTriggerADCCallback, + }; + SIMPLEFOC_ESP32_CS_DEBUG("Timer "+String(t->timer_id)+" enable interrupt callback."); + // set the timer state to init (so that we can call the `mcpwm_timer_register_event_callbacks` ) + // this is a hack, as this function is not supposed to be called when the timer is running + // the timer does not really go to the init state! + t->fsm = MCPWM_TIMER_FSM_INIT; + // set the callback + CHECK_CS_ERR(mcpwm_timer_register_event_callbacks(t, &cbs, cs_params), "Failed to set low side callback"); + // set the timer state to enabled again + t->fsm = MCPWM_TIMER_FSM_ENABLE; + SIMPLEFOC_ESP32_CS_DEBUG("Timer "+String(t->timer_id)+" enable interrupts."); + CHECK_CS_ERR(esp_intr_enable(t->intr), "Failed to enable low-side interrupts!"); + + return cs_params; +} + + +#endif diff --git a/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp b/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp index 2057463c..95667069 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp @@ -1,165 +1,38 @@ -#include "../../hardware_api.h" -#include "../../../drivers/hardware_api.h" -#include "../../../drivers/hardware_specific/esp32/esp32_driver_mcpwm.h" - -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) - -#include "esp32_adc_driver.h" - -#include "driver/mcpwm.h" -#include "soc/mcpwm_reg.h" -#include "soc/mcpwm_struct.h" - -#include -#include - -#define _ADC_VOLTAGE 3.3f -#define _ADC_RESOLUTION 4095.0f - - -typedef struct ESP32MCPWMCurrentSenseParams { - int pins[3]; - float adc_voltage_conv; - mcpwm_unit_t mcpwm_unit; - int buffer_index; -} ESP32MCPWMCurrentSenseParams; +#include "esp32_mcu.h" +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) /** - * Inline adc reading implementation + * Inline adc reading implementation */ // function reading an ADC value and returning the read voltage -float _readADCVoltageInline(const int pinA, const void* cs_params){ +float IRAM_ATTR _readADCVoltageInline(const int pinA, const void* cs_params){ uint32_t raw_adc = adcRead(pinA); - return raw_adc * ((ESP32MCPWMCurrentSenseParams*)cs_params)->adc_voltage_conv; + return raw_adc * ((ESP32CurrentSenseParams*)cs_params)->adc_voltage_conv; } // function reading an ADC value and returning the read voltage -void* _configureADCInline(const void* driver_params, const int pinA, const int pinB, const int pinC){ - _UNUSED(driver_params); - - if( _isset(pinA) ) pinMode(pinA, INPUT); - if( _isset(pinB) ) pinMode(pinB, INPUT); - if( _isset(pinC) ) pinMode(pinC, INPUT); +void* IRAM_ATTR _configureADCInline(const void* driver_params, const int pinA, const int pinB, const int pinC){ - ESP32MCPWMCurrentSenseParams* params = new ESP32MCPWMCurrentSenseParams { + ESP32CurrentSenseParams* params = new ESP32CurrentSenseParams { .pins = { pinA, pinB, pinC }, .adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION) }; - return params; -} - - - -/** - * Low side adc reading implementation -*/ - -static void IRAM_ATTR mcpwm0_isr_handler(void*); -static void IRAM_ATTR mcpwm1_isr_handler(void*); -byte currentState = 1; -// two mcpwm units -// - max 2 motors per mcpwm unit (6 adc channels) -int adc_pins[2][6]={0}; -int adc_pin_count[2]={0}; -uint32_t adc_buffer[2][6]={0}; -int adc_read_index[2]={0}; - -// function reading an ADC value and returning the read voltage -float _readADCVoltageLowSide(const int pin, const void* cs_params){ - mcpwm_unit_t unit = ((ESP32MCPWMCurrentSenseParams*)cs_params)->mcpwm_unit; - int buffer_index = ((ESP32MCPWMCurrentSenseParams*)cs_params)->buffer_index; - float adc_voltage_conv = ((ESP32MCPWMCurrentSenseParams*)cs_params)->adc_voltage_conv; - - for(int i=0; i < adc_pin_count[unit]; i++){ - if( pin == ((ESP32MCPWMCurrentSenseParams*)cs_params)->pins[i]) // found in the buffer - return adc_buffer[unit][buffer_index + i] * adc_voltage_conv; + // initialize the ADC pins + // fail if the pin is not an ADC pin + for (int i = 0; i < 3; i++){ + if(_isset(params->pins[i])){ + pinMode(params->pins[i], ANALOG); + if(!adcInit(params->pins[i])) { + SIMPLEFOC_ESP32_CS_DEBUG("ERROR: Failed to initialise ADC pin: "+String(params->pins[i]) + String(", maybe not an ADC pin?")); + return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; + } + } } - // not found - return 0; -} - -// function configuring low-side current sensing -void* _configureADCLowSide(const void* driver_params, const int pinA,const int pinB,const int pinC){ - - mcpwm_unit_t unit = ((ESP32MCPWMDriverParams*)driver_params)->mcpwm_unit; - int index_start = adc_pin_count[unit]; - if( _isset(pinA) ) adc_pins[unit][adc_pin_count[unit]++] = pinA; - if( _isset(pinB) ) adc_pins[unit][adc_pin_count[unit]++] = pinB; - if( _isset(pinC) ) adc_pins[unit][adc_pin_count[unit]++] = pinC; - - if( _isset(pinA) ) pinMode(pinA, INPUT); - if( _isset(pinB) ) pinMode(pinB, INPUT); - if( _isset(pinC) ) pinMode(pinC, INPUT); - - ESP32MCPWMCurrentSenseParams* params = new ESP32MCPWMCurrentSenseParams { - .pins = { pinA, pinB, pinC }, - .adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION), - .mcpwm_unit = unit, - .buffer_index = index_start - }; return params; } -void _driverSyncLowSide(void* driver_params, void* cs_params){ - - mcpwm_dev_t* mcpwm_dev = ((ESP32MCPWMDriverParams*)driver_params)->mcpwm_dev; - mcpwm_unit_t mcpwm_unit = ((ESP32MCPWMDriverParams*)driver_params)->mcpwm_unit; - - // low-side register enable interrupt - mcpwm_dev->int_ena.timer0_tep_int_ena = true;//A PWM timer 0 TEP event will trigger this interrupt - // high side registers enable interrupt - //mcpwm_dev->int_ena.timer0_tep_int_ena = true;//A PWM timer 0 TEZ event will trigger this interrupt - - // register interrupts (mcpwm number, interrupt handler, handler argument = NULL, interrupt signal/flag, return handler = NULL) - if(mcpwm_unit == MCPWM_UNIT_0) - mcpwm_isr_register(mcpwm_unit, mcpwm0_isr_handler, NULL, ESP_INTR_FLAG_IRAM, NULL); //Set ISR Handler - else - mcpwm_isr_register(mcpwm_unit, mcpwm1_isr_handler, NULL, ESP_INTR_FLAG_IRAM, NULL); //Set ISR Handler -} - -static void IRAM_ATTR mcpwm0_isr_handler(void*) __attribute__ ((unused)); - -// Read currents when interrupt is triggered -static void IRAM_ATTR mcpwm0_isr_handler(void*){ - // // high side - // uint32_t mcpwm_intr_status = MCPWM0.int_st.timer0_tez_int_st; - - // low side - uint32_t mcpwm_intr_status = MCPWM0.int_st.timer0_tep_int_st; - if(mcpwm_intr_status){ - adc_buffer[0][adc_read_index[0]] = adcRead(adc_pins[0][adc_read_index[0]]); - adc_read_index[0]++; - if(adc_read_index[0] == adc_pin_count[0]) adc_read_index[0] = 0; - } - // low side - MCPWM0.int_clr.timer0_tep_int_clr = mcpwm_intr_status; - // high side - // MCPWM0.int_clr.timer0_tez_int_clr = mcpwm_intr_status_0; -} - -static void IRAM_ATTR mcpwm1_isr_handler(void*) __attribute__ ((unused)); - -// Read currents when interrupt is triggered -static void IRAM_ATTR mcpwm1_isr_handler(void*){ - // // high side - // uint32_t mcpwm_intr_status = MCPWM1.int_st.timer0_tez_int_st; - - // low side - uint32_t mcpwm_intr_status = MCPWM1.int_st.timer0_tep_int_st; - if(mcpwm_intr_status){ - adc_buffer[1][adc_read_index[1]] = adcRead(adc_pins[1][adc_read_index[1]]); - adc_read_index[1]++; - if(adc_read_index[1] == adc_pin_count[1]) adc_read_index[1] = 0; - } - // low side - MCPWM1.int_clr.timer0_tep_int_clr = mcpwm_intr_status; - // high side - // MCPWM1.int_clr.timer0_tez_int_clr = mcpwm_intr_status_0; -} - - #endif diff --git a/src/current_sense/hardware_specific/esp32/esp32_mcu.h b/src/current_sense/hardware_specific/esp32/esp32_mcu.h new file mode 100644 index 00000000..9207fb6a --- /dev/null +++ b/src/current_sense/hardware_specific/esp32/esp32_mcu.h @@ -0,0 +1,37 @@ +#ifndef ESP32_MCU_CURRENT_SENSING_H +#define ESP32_MCU_CURRENT_SENSING_H + +#include "../../hardware_api.h" + +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) + + +#include "../../../drivers/hardware_api.h" +#include "esp32_adc_driver.h" + + +// esp32 current sense parameters +typedef struct ESP32CurrentSenseParams { + int pins[3]; + float adc_voltage_conv; + int adc_buffer[3] = {}; + int buffer_index = 0; + int no_adc_channels = 0; +} ESP32CurrentSenseParams; + +// macros for debugging wuing the simplefoc debug system +#define SIMPLEFOC_ESP32_CS_DEBUG(str)\ + SimpleFOCDebug::println( "ESP32-CS: "+ String(str));\ + +#define CHECK_CS_ERR(func_call, message) \ + if ((func_call) != ESP_OK) { \ + SIMPLEFOC_ESP32_CS_DEBUG("ERROR - " + String(message)); \ + return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; \ + } + + +#define _ADC_VOLTAGE 3.3f +#define _ADC_RESOLUTION 4095.0f + +#endif // ESP_H && ARDUINO_ARCH_ESP32 +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp b/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp deleted file mode 100644 index a2f58ac3..00000000 --- a/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp +++ /dev/null @@ -1,258 +0,0 @@ -#include "esp32_adc_driver.h" - -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && (defined(CONFIG_IDF_TARGET_ESP32S2) || defined(CONFIG_IDF_TARGET_ESP32S3)) - -#include "freertos/FreeRTOS.h" -#include "freertos/task.h" -#include "rom/ets_sys.h" -#include "esp_attr.h" -//#include "esp_intr.h" // deprecated -#include "esp_intr_alloc.h" -#include "soc/rtc_io_reg.h" -#include "soc/rtc_cntl_reg.h" -#include "soc/sens_reg.h" - -static uint8_t __analogAttenuation = 3;//11db -static uint8_t __analogWidth = 3;//12 bits -static uint8_t __analogCycles = 8; -static uint8_t __analogSamples = 0;//1 sample -static uint8_t __analogClockDiv = 1; - -// Width of returned answer () -static uint8_t __analogReturnedWidth = 12; - -void __analogSetWidth(uint8_t bits){ - if(bits < 9){ - bits = 9; - } else if(bits > 12){ - bits = 12; - } - __analogReturnedWidth = bits; - __analogWidth = bits - 9; - // SET_PERI_REG_BITS(SENS_SAR_START_FORCE_REG, SENS_SAR1_BIT_WIDTH, __analogWidth, SENS_SAR1_BIT_WIDTH_S); - // SET_PERI_REG_BITS(SENS_SAR_READER1_CTRL_REG, SENS_SAR1_SAMPLE_BIT, __analogWidth, SENS_SAR1_SAMPLE_BIT_S); - - // SET_PERI_REG_BITS(SENS_SAR_START_FORCE_REG, SENS_SAR2_BIT_WIDTH, __analogWidth, SENS_SAR2_BIT_WIDTH_S); - // SET_PERI_REG_BITS(SENS_SAR_READER2_CTRL_REG, SENS_SAR2_SAMPLE_BIT, __analogWidth, SENS_SAR2_SAMPLE_BIT_S); -} - -void __analogSetCycles(uint8_t cycles){ - __analogCycles = cycles; - // SET_PERI_REG_BITS(SENS_SAR_READER1_CTRL_REG, SENS_SAR1_SAMPLE_CYCLE, __analogCycles, SENS_SAR1_SAMPLE_CYCLE_S); - // SET_PERI_REG_BITS(SENS_SAR_READER2_CTRL_REG, SENS_SAR2_SAMPLE_CYCLE, __analogCycles, SENS_SAR2_SAMPLE_CYCLE_S); -} - -void __analogSetSamples(uint8_t samples){ - if(!samples){ - return; - } - __analogSamples = samples - 1; - SET_PERI_REG_BITS(SENS_SAR_READER1_CTRL_REG, SENS_SAR1_SAMPLE_NUM, __analogSamples, SENS_SAR1_SAMPLE_NUM_S); - SET_PERI_REG_BITS(SENS_SAR_READER2_CTRL_REG, SENS_SAR2_SAMPLE_NUM, __analogSamples, SENS_SAR2_SAMPLE_NUM_S); -} - -void __analogSetClockDiv(uint8_t clockDiv){ - if(!clockDiv){ - return; - } - __analogClockDiv = clockDiv; - SET_PERI_REG_BITS(SENS_SAR_READER1_CTRL_REG, SENS_SAR1_CLK_DIV, __analogClockDiv, SENS_SAR1_CLK_DIV_S); - SET_PERI_REG_BITS(SENS_SAR_READER2_CTRL_REG, SENS_SAR2_CLK_DIV, __analogClockDiv, SENS_SAR2_CLK_DIV_S); -} - -void __analogSetAttenuation(uint8_t attenuation) -{ - __analogAttenuation = attenuation & 3; - uint32_t att_data = 0; - int i = 10; - while(i--){ - att_data |= __analogAttenuation << (i * 2); - } - WRITE_PERI_REG(SENS_SAR_ATTEN1_REG, att_data & 0xFFFF);//ADC1 has 8 channels - WRITE_PERI_REG(SENS_SAR_ATTEN2_REG, att_data); -} - -void IRAM_ATTR __analogInit(){ - static bool initialized = false; - if(initialized){ - return; - } - - __analogSetAttenuation(__analogAttenuation); - __analogSetCycles(__analogCycles); - __analogSetSamples(__analogSamples + 1);//in samples - __analogSetClockDiv(__analogClockDiv); - __analogSetWidth(__analogWidth + 9);//in bits - - SET_PERI_REG_MASK(SENS_SAR_READER1_CTRL_REG, SENS_SAR1_DATA_INV); - SET_PERI_REG_MASK(SENS_SAR_READER2_CTRL_REG, SENS_SAR2_DATA_INV); - - SET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_START_FORCE_M); //SAR ADC1 controller (in RTC) is started by SW - SET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_SAR1_EN_PAD_FORCE_M); //SAR ADC1 pad enable bitmap is controlled by SW - SET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_START_FORCE_M); //SAR ADC2 controller (in RTC) is started by SW - SET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_SAR2_EN_PAD_FORCE_M); //SAR ADC2 pad enable bitmap is controlled by SW - - CLEAR_PERI_REG_MASK(SENS_SAR_POWER_XPD_SAR_REG, SENS_FORCE_XPD_SAR_M); //force XPD_SAR=0, use XPD_FSM - SET_PERI_REG_BITS(SENS_SAR_POWER_XPD_SAR_REG, SENS_FORCE_XPD_AMP, 0x2, SENS_FORCE_XPD_AMP_S); //force XPD_AMP=0 - - CLEAR_PERI_REG_MASK(SENS_SAR_AMP_CTRL3_REG, 0xfff << SENS_AMP_RST_FB_FSM_S); //clear FSM - SET_PERI_REG_BITS(SENS_SAR_AMP_CTRL1_REG, SENS_SAR_AMP_WAIT1, 0x1, SENS_SAR_AMP_WAIT1_S); - SET_PERI_REG_BITS(SENS_SAR_AMP_CTRL1_REG, SENS_SAR_AMP_WAIT2, 0x1, SENS_SAR_AMP_WAIT2_S); - SET_PERI_REG_BITS(SENS_SAR_POWER_XPD_SAR_REG, SENS_SAR_AMP_WAIT3, 0x1, SENS_SAR_AMP_WAIT3_S); - while (GET_PERI_REG_BITS2(SENS_SAR_SLAVE_ADDR1_REG, 0x7, SENS_SARADC_MEAS_STATUS_S) != 0); //wait det_fsm== - - initialized = true; -} - -void __analogSetPinAttenuation(uint8_t pin, uint8_t attenuation) -{ - int8_t channel = digitalPinToAnalogChannel(pin); - if(channel < 0 || attenuation > 3){ - return ; - } - __analogInit(); - if(channel > 7){ - SET_PERI_REG_BITS(SENS_SAR_ATTEN2_REG, 3, attenuation, ((channel - 10) * 2)); - } else { - SET_PERI_REG_BITS(SENS_SAR_ATTEN1_REG, 3, attenuation, (channel * 2)); - } -} - -bool IRAM_ATTR __adcAttachPin(uint8_t pin){ - - int8_t channel = digitalPinToAnalogChannel(pin); - if(channel < 0){ - return false;//not adc pin - } - - int8_t pad = digitalPinToTouchChannel(pin); - if(pad >= 0){ - uint32_t touch = READ_PERI_REG(SENS_SAR_TOUCH_CONF_REG); - if(touch & (1 << pad)){ - touch &= ~((1 << (pad + SENS_TOUCH_OUTEN_S))); - WRITE_PERI_REG(SENS_SAR_TOUCH_CONF_REG, touch); - } - } else if(pin == 25){ - CLEAR_PERI_REG_MASK(RTC_IO_PAD_DAC1_REG, RTC_IO_PDAC1_XPD_DAC | RTC_IO_PDAC1_DAC_XPD_FORCE); //stop dac1 - } else if(pin == 26){ - CLEAR_PERI_REG_MASK(RTC_IO_PAD_DAC2_REG, RTC_IO_PDAC2_XPD_DAC | RTC_IO_PDAC2_DAC_XPD_FORCE); //stop dac2 - } - - pinMode(pin, ANALOG); - - __analogInit(); - return true; -} - -bool IRAM_ATTR __adcStart(uint8_t pin){ - - int8_t channel = digitalPinToAnalogChannel(pin); - if(channel < 0){ - return false;//not adc pin - } - - if(channel > 9){ - channel -= 10; - CLEAR_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_START_SAR_M); - SET_PERI_REG_BITS(SENS_SAR_MEAS2_CTRL2_REG, SENS_SAR2_EN_PAD, (1 << channel), SENS_SAR2_EN_PAD_S); - SET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_START_SAR_M); - } else { - CLEAR_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_START_SAR_M); - SET_PERI_REG_BITS(SENS_SAR_MEAS1_CTRL2_REG, SENS_SAR1_EN_PAD, (1 << channel), SENS_SAR1_EN_PAD_S); - SET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_START_SAR_M); - } - return true; -} - -bool IRAM_ATTR __adcBusy(uint8_t pin){ - - int8_t channel = digitalPinToAnalogChannel(pin); - if(channel < 0){ - return false;//not adc pin - } - - if(channel > 7){ - return (GET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_DONE_SAR) == 0); - } - return (GET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_DONE_SAR) == 0); -} - -uint16_t IRAM_ATTR __adcEnd(uint8_t pin) -{ - - uint16_t value = 0; - int8_t channel = digitalPinToAnalogChannel(pin); - if(channel < 0){ - return 0;//not adc pin - } - if(channel > 7){ - while (GET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_DONE_SAR) == 0); //wait for conversion - value = GET_PERI_REG_BITS2(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_DATA_SAR, SENS_MEAS2_DATA_SAR_S); - } else { - while (GET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_DONE_SAR) == 0); //wait for conversion - value = GET_PERI_REG_BITS2(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_DATA_SAR, SENS_MEAS1_DATA_SAR_S); - } - - // Shift result if necessary - uint8_t from = __analogWidth + 9; - if (from == __analogReturnedWidth) { - return value; - } - if (from > __analogReturnedWidth) { - return value >> (from - __analogReturnedWidth); - } - return value << (__analogReturnedWidth - from); -} - -void __analogReadResolution(uint8_t bits) -{ - if(!bits || bits > 16){ - return; - } - __analogSetWidth(bits); // hadware from 9 to 12 - __analogReturnedWidth = bits; // software from 1 to 16 -} - -uint16_t IRAM_ATTR adcRead(uint8_t pin) -{ - int8_t channel = digitalPinToAnalogChannel(pin); - if(channel < 0){ - return false;//not adc pin - } - - __analogInit(); - - if(channel > 9){ - channel -= 10; - CLEAR_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_START_SAR_M); - SET_PERI_REG_BITS(SENS_SAR_MEAS2_CTRL2_REG, SENS_SAR2_EN_PAD, (1 << channel), SENS_SAR2_EN_PAD_S); - SET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_START_SAR_M); - } else { - CLEAR_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_START_SAR_M); - SET_PERI_REG_BITS(SENS_SAR_MEAS1_CTRL2_REG, SENS_SAR1_EN_PAD, (1 << channel), SENS_SAR1_EN_PAD_S); - SET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_START_SAR_M); - } - - uint16_t value = 0; - - if(channel > 7){ - while (GET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_DONE_SAR) == 0); //wait for conversion - value = GET_PERI_REG_BITS2(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_DATA_SAR, SENS_MEAS2_DATA_SAR_S); - } else { - while (GET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_DONE_SAR) == 0); //wait for conversion - value = GET_PERI_REG_BITS2(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_DATA_SAR, SENS_MEAS1_DATA_SAR_S); - } - - // Shift result if necessary - uint8_t from = __analogWidth + 9; - if (from == __analogReturnedWidth) { - return value; - } - if (from > __analogReturnedWidth) { - return value >> (from - __analogReturnedWidth); - } - return value << (__analogReturnedWidth - from); -} - - -#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/generic_mcu.cpp b/src/current_sense/hardware_specific/generic_mcu.cpp index dec7205d..ff8c467c 100644 --- a/src/current_sense/hardware_specific/generic_mcu.cpp +++ b/src/current_sense/hardware_specific/generic_mcu.cpp @@ -1,4 +1,5 @@ #include "../hardware_api.h" +#include "../../communication/SimpleFOCDebug.h" // function reading an ADC value and returning the read voltage __attribute__((weak)) float _readADCVoltageInline(const int pinA, const void* cs_params){ @@ -24,18 +25,23 @@ __attribute__((weak)) void* _configureADCInline(const void* driver_params, cons // function reading an ADC value and returning the read voltage __attribute__((weak)) float _readADCVoltageLowSide(const int pinA, const void* cs_params){ - return _readADCVoltageInline(pinA, cs_params); + SIMPLEFOC_DEBUG("ERR: Low-side cs not supported!"); + return 0.0; } // Configure low side for generic mcu // cannot do much but __attribute__((weak)) void* _configureADCLowSide(const void* driver_params, const int pinA,const int pinB,const int pinC){ - return _configureADCInline(driver_params, pinA, pinB, pinC); + SIMPLEFOC_DEBUG("ERR: Low-side cs not supported!"); + return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; } // sync driver and the adc -__attribute__((weak)) void _driverSyncLowSide(void* driver_params, void* cs_params){ +__attribute__((weak)) void* _driverSyncLowSide(void* driver_params, void* cs_params){ _UNUSED(driver_params); - _UNUSED(cs_params); + return cs_params; } + +// function starting the ADC conversion for the high side current sensing +// only necessary for certain types of MCUs __attribute__((weak)) void _startADC3PinConversionLowSide(){ } diff --git a/src/current_sense/hardware_specific/rp2040/rp2040_mcu.cpp b/src/current_sense/hardware_specific/rp2040/rp2040_mcu.cpp index d84c2fd5..302e3815 100644 --- a/src/current_sense/hardware_specific/rp2040/rp2040_mcu.cpp +++ b/src/current_sense/hardware_specific/rp2040/rp2040_mcu.cpp @@ -1,5 +1,5 @@ -#if defined(TARGET_RP2040) +#if defined(TARGET_RP2040) || defined(TARGET_RP2350) #include "../../hardware_api.h" @@ -86,7 +86,7 @@ void* _configureADCInline(const void *driver_params, const int pinA, const int p // }; -// void _driverSyncLowSide(void* driver_params, void* cs_params) { +// void* _driverSyncLowSide(void* driver_params, void* cs_params) { // // nothing to do // }; diff --git a/src/current_sense/hardware_specific/samd/samd21_mcu.cpp b/src/current_sense/hardware_specific/samd/samd21_mcu.cpp index da5ba85b..3a045446 100644 --- a/src/current_sense/hardware_specific/samd/samd21_mcu.cpp +++ b/src/current_sense/hardware_specific/samd/samd21_mcu.cpp @@ -1,318 +1,173 @@ -#ifdef _SAMD21_ #include "samd21_mcu.h" -#include "../../hardware_api.h" - -static bool freeRunning = false; -static int _pinA, _pinB, _pinC; -static uint16_t a = 0xFFFF, b = 0xFFFF, c = 0xFFFF; // updated by adcStopWithDMA when configured in freerunning mode -static SAMDCurrentSenseADCDMA instance; +#if defined(_SAMD21_) -// function configuring low-side current sensing -void* _configureADCLowSide(const void* driver_params, const int pinA,const int pinB,const int pinC) -{ - _UNUSED(driver_params); +#include "../../hardware_api.h" +#include "../../../drivers/hardware_specific/samd/samd_mcu.h" - _pinA = pinA; - _pinB = pinB; - _pinC = pinC; - freeRunning = true; - instance.init(pinA, pinB, pinC); +static int _pinA = NOT_SET, _pinB = NOT_SET, _pinC = NOT_SET; +static uint16_t adc_raw[3] = {0, 0, 0}; +static uint8_t current_phase = 0; // which phase we're sampling +static bool is_high_side = true; // low-side current sense - GenericCurrentSenseParams* params = new GenericCurrentSenseParams { - .pins = { pinA, pinB, pinC } - }; +#define _SAMD21_ADC_VOLTAGE 3.3f +#define _SAMD21_ADC_RESOLUTION 4095.0f - return params; -} -void _startADC3PinConversionLowSide() -{ - instance.startADCScan(); -} -/** - * function reading an ADC value and returning the read voltage - * - * @param pinA - the arduino pin to be read (it has to be ADC pin) - */ -float _readADCVoltageLowSide(const int pinA, const void* cs_params) -{ - _UNUSED(cs_params); - - instance.readResults(a, b, c); +static void setupADCEventTriggerFromDriver(const SAMDHardwareDriverParams *par, const GenericCurrentSenseParams *cs_params) { - if(pinA == _pinA) - return instance.toVolts(a); - if(pinA == _pinB) - return instance.toVolts(b); - if(pinA == _pinC) - return instance.toVolts(c); - - return NAN; -} - -/** - * function syncing the Driver with the ADC for the LowSide Sensing - */ -void _driverSyncLowSide(void* driver_params, void* cs_params) -{ - _UNUSED(driver_params); - _UNUSED(cs_params); - - SIMPLEFOC_SAMD_DEBUG_SERIAL.println(F("TODO! _driverSyncLowSide() is not implemented")); - instance.startADCScan(); - //TODO: hook with PWM interrupts -} - - - - - - - - - - - // Credit: significant portions of this code were pulled from Paul Gould's git https://github.com/gouldpa/FOC-Arduino-Brushless - -static void adcStopWithDMA(void); -static void adcStartWithDMA(void); - -/** - * @brief ADC sync wait - * @retval void - */ -static __inline__ void ADCsync() __attribute__((always_inline, unused)); -static void ADCsync() { - while (ADC->STATUS.bit.SYNCBUSY == 1); //Just wait till the ADC is free -} + // --- Configure ADC module --- + ADC->CTRLA.bit.ENABLE = 0; + while (ADC->STATUS.bit.SYNCBUSY); + + ADC->REFCTRL.reg = ADC_REFCTRL_REFSEL_INTVCC1; + ADC->CTRLB.reg = ADC_CTRLB_PRESCALER_DIV16 | ADC_CTRLB_RESSEL_12BIT; + ADC->CTRLB.bit.FREERUN = 0; + while (ADC->STATUS.bit.SYNCBUSY); + + ADC->INPUTCTRL.bit.MUXPOS = g_APinDescription[_pinA].ulADCChannelNumber; + // there must be a way to trigger more than one ADC conversion at a time. + // SO far I was not able to do it. + // ADC->INPUTCTRL.bit.INPUTSCAN = 1; // N = number_of_channels - 1 + // ADC->INPUTCTRL.bit.INPUTOFFSET = 0; + while (ADC->STATUS.bit.SYNCBUSY); + + // Enable event start + ADC->EVCTRL.bit.STARTEI = 1; + ADC->INTENSET.bit.RESRDY = 1; + NVIC_EnableIRQ(ADC_IRQn); + + // --- Configure Event System --- + uint8_t tcc_num = par->tccPinConfigurations[1]->tcc.tccn; + + // --- Enable event output on the PWM timer (important!) --- + Tcc* tcc = nullptr; + switch (tcc_num) { + case 0: tcc = TCC0; break; + case 1: tcc = TCC1; break; + case 2: tcc = TCC2; break; + default: tcc = TCC0; break; + } -// ADC DMA sequential free running (6) with Interrupts ///////////////// + // We are enabling the overflow/underflow event output + // This is not ideal as it triggers twice per PWM cycle + // So we need to keep track if we are in high-side or low-side current sense + if (tcc) { + tcc->CTRLA.bit.ENABLE = 0; + while (tcc->SYNCBUSY.bit.ENABLE); + tcc->EVCTRL.reg |= TCC_EVCTRL_OVFEO; + tcc->CTRLA.bit.ENABLE = 1; + while (tcc->SYNCBUSY.bit.ENABLE); + } -SAMDCurrentSenseADCDMA * SAMDCurrentSenseADCDMA::getHardwareAPIInstance() -{ - - return &instance; -} + // Configure the event channel to trigger on the TCC overflow + // and connect it to the ADC start event + uint8_t evgen = 0; + switch (tcc_num) { + case 0: evgen = EVSYS_ID_GEN_TCC0_OVF; break; + case 1: evgen = EVSYS_ID_GEN_TCC1_OVF; break; + case 2: evgen = EVSYS_ID_GEN_TCC2_OVF; break; + default: evgen = EVSYS_ID_GEN_TCC0_OVF; break; + } -SAMDCurrentSenseADCDMA::SAMDCurrentSenseADCDMA() -{ -} + PM->APBCMASK.reg |= PM_APBCMASK_EVSYS; + EVSYS->CHANNEL.reg = EVSYS_CHANNEL_CHANNEL(0) + | EVSYS_CHANNEL_EVGEN(evgen) + | EVSYS_CHANNEL_PATH_ASYNCHRONOUS; + EVSYS->USER.reg = EVSYS_USER_USER(EVSYS_ID_USER_ADC_START) + | EVSYS_USER_CHANNEL(1); -void SAMDCurrentSenseADCDMA::init(int pinA, int pinB, int pinC, int pinAREF, float voltageAREF, uint32_t adcBits, uint32_t channelDMA) -{ - this->pinA = pinA; - this->pinB = pinB; - this->pinC = pinC; - this->pinAREF = pinAREF; - this->channelDMA = channelDMA; - this->voltageAREF = voltageAREF; - this->maxCountsADC = 1 << adcBits; - countsToVolts = ( voltageAREF / maxCountsADC ); - - initPins(); - initADC(); - initDMA(); - startADCScan(); //so we have something to read next time we call readResults() + // Enable ADC + ADC->CTRLA.bit.ENABLE = 1; + while (ADC->STATUS.bit.SYNCBUSY); } -void SAMDCurrentSenseADCDMA::startADCScan(){ - adcToDMATransfer(adcBuffer + oneBeforeFirstAIN, BufferSize); - adcStartWithDMA(); -} - -bool SAMDCurrentSenseADCDMA::readResults(uint16_t & a, uint16_t & b, uint16_t & c){ - if(ADC->CTRLA.bit.ENABLE) - return false; - uint32_t ainA = g_APinDescription[pinA].ulADCChannelNumber; - uint32_t ainB = g_APinDescription[pinB].ulADCChannelNumber; - a = adcBuffer[ainA]; - b = adcBuffer[ainB]; - if(_isset(pinC)) - { - uint32_t ainC = g_APinDescription[pinC].ulADCChannelNumber; - c = adcBuffer[ainC]; +// ADC interrupt (switch between A, B, C) +void ADC_Handler() { + //digitalWrite(13,HIGH); + // check if we are in high-side or low-side current sense + is_high_side = !is_high_side; + if(is_high_side){ + ADC->INTFLAG.reg = ADC_INTFLAG_RESRDY; + //digitalWrite(13,LOW); + return; } - return true; -} - -float SAMDCurrentSenseADCDMA::toVolts(uint16_t counts) { - return counts * countsToVolts; -} - -void SAMDCurrentSenseADCDMA::initPins(){ + // read the result and switch to the next channel + if (ADC->INTFLAG.bit.RESRDY) { + uint16_t result = ADC->RESULT.reg; - if (pinAREF>=0) - pinMode(pinAREF, INPUT); - pinMode(pinA, INPUT); - pinMode(pinB, INPUT); - - uint32_t ainA = g_APinDescription[pinA].ulADCChannelNumber; - uint32_t ainB = g_APinDescription[pinB].ulADCChannelNumber; - firstAIN = min(ainA, ainB); - lastAIN = max(ainA, ainB); - if( _isset(pinC) ) - { - uint32_t ainC = g_APinDescription[pinC].ulADCChannelNumber; - pinMode(pinC, INPUT); - firstAIN = min(firstAIN, ainC); - lastAIN = max(lastAIN, ainC); + adc_raw[current_phase] = result; + if (current_phase == 0) { + ADC->INPUTCTRL.bit.MUXPOS = g_APinDescription[_pinB].ulADCChannelNumber; + current_phase = 1; + } else if (current_phase == 1) { + if (_pinC >= 0) { + ADC->INPUTCTRL.bit.MUXPOS = g_APinDescription[_pinC].ulADCChannelNumber; + current_phase = 2; + } else { + ADC->INPUTCTRL.bit.MUXPOS = g_APinDescription[_pinA].ulADCChannelNumber; + current_phase = 0; + } + } else { + ADC->INPUTCTRL.bit.MUXPOS = g_APinDescription[_pinA].ulADCChannelNumber; + current_phase = 0; + } + ADC->INTFLAG.reg = ADC_INTFLAG_RESRDY; } - - oneBeforeFirstAIN = firstAIN - 1; //hack to discard noisy first readout - BufferSize = lastAIN - oneBeforeFirstAIN + 1; - + //digitalWrite(13,LOW); } -void SAMDCurrentSenseADCDMA::initADC(){ - - analogRead(pinA); // do some pin init pinPeripheral() - analogRead(pinB); // do some pin init pinPeripheral() - analogRead(pinC); // do some pin init pinPeripheral() - - ADC->CTRLA.bit.ENABLE = 0x00; // Disable ADC - ADCsync(); - //ADC->REFCTRL.bit.REFSEL = ADC_REFCTRL_REFSEL_INTVCC0_Val; // 2.2297 V Supply VDDANA - ADC->INPUTCTRL.bit.GAIN = ADC_INPUTCTRL_GAIN_1X_Val; // Gain select as 1X - // ADC->INPUTCTRL.bit.GAIN = ADC_INPUTCTRL_GAIN_DIV2_Val; // default - if (pinAREF>=0) - ADC->REFCTRL.bit.REFSEL = ADC_REFCTRL_REFSEL_AREFA; - else - ADC->REFCTRL.bit.REFSEL = ADC_REFCTRL_REFSEL_INTVCC0; - ADCsync(); // ref 31.6.16 - - /* - Bits 19:16 – INPUTSCAN[3:0]: Number of Input Channels Included in Scan - This register gives the number of input sources included in the pin scan. The number of input sources included is - INPUTSCAN + 1. The input channels included are in the range from MUXPOS + INPUTOFFSET to MUXPOS + - INPUTOFFSET + INPUTSCAN. - The range of the scan mode must not exceed the number of input channels available on the device. - Bits 4:0 – MUXPOS[4:0]: Positive Mux Input Selection - These bits define the Mux selection for the positive ADC input. Table 32-14 shows the possible input selections. If - the internal bandgap voltage or temperature sensor input channel is selected, then the Sampling Time Length bit - group in the SamplingControl register must be written. - Table 32-14. Positive Mux Input Selection - MUXPOS[4:0] Group configuration Description - 0x00 PIN0 ADC AIN0 pin - 0x01 PIN1 ADC AIN1 pin - 0x02 PIN2 ADC AIN2 pin - 0x03 PIN3 ADC AIN3 pin - 0x04 PIN4 ADC AIN4 pin - 0x05 PIN5 ADC AIN5 pin - 0x06 PIN6 ADC AIN6 pin - 0x07 PIN7 ADC AIN7 pin - 0x08 PIN8 ADC AIN8 pin - 0x09 PIN9 ADC AIN9 pin - 0x0A PIN10 ADC AIN10 pin - 0x0B PIN11 ADC AIN11 pin - 0x0C PIN12 ADC AIN12 pin - 0x0D PIN13 ADC AIN13 pin - 0x0E PIN14 ADC AIN14 pin - 0x0F PIN15 ADC AIN15 pin - 0x10 PIN16 ADC AIN16 pin - 0x11 PIN17 ADC AIN17 pin - 0x12 PIN18 ADC AIN18 pin - 0x13 PIN19 ADC AIN19 pin - 0x14-0x17 Reserved - 0x18 TEMP Temperature reference - 0x19 BANDGAP Bandgap voltage - 0x1A SCALEDCOREVCC 1/4 scaled core supply - 0x1B SCALEDIOVCC 1/4 scaled I/O supply - 0x1C DAC DAC output - 0x1D-0x1F Reserved - */ - ADC->INPUTCTRL.bit.MUXPOS = oneBeforeFirstAIN; - ADCsync(); - ADC->INPUTCTRL.bit.INPUTSCAN = lastAIN; // so the adc will scan from oneBeforeFirstAIN to lastAIN (inclusive) - ADCsync(); - ADC->INPUTCTRL.bit.INPUTOFFSET = 0; //input scan cursor - ADCsync(); - ADC->AVGCTRL.reg = 0x00 ; //no averaging - ADC->SAMPCTRL.reg = 0x05; ; //sample length in 1/2 CLK_ADC cycles, see GCLK_ADC and ADC_CTRLB_PRESCALER_DIV16 - // according to the specsheet: f_GCLK_ADC ADC input clock frequency 48 MHz, so same as fCPU - ADCsync(); - ADC->CTRLB.reg = ADC_CTRLB_PRESCALER_DIV16 | ADC_CTRLB_FREERUN | ADC_CTRLB_RESSEL_12BIT; - ADCsync(); -} - -volatile dmacdescriptor wrb[12] __attribute__ ((aligned (16))); -void SAMDCurrentSenseADCDMA::initDMA() { - // probably on by default - PM->AHBMASK.reg |= PM_AHBMASK_DMAC ; - PM->APBBMASK.reg |= PM_APBBMASK_DMAC ; - NVIC_EnableIRQ( DMAC_IRQn ) ; - DMAC->BASEADDR.reg = (uint32_t)descriptor_section; - DMAC->WRBADDR.reg = (uint32_t)wrb; - DMAC->CTRL.reg = DMAC_CTRL_DMAENABLE | DMAC_CTRL_LVLEN(0xf); -} +// ------- API functions ------- +void* _configureADCLowSide(const void* driver_params, const int pinA, const int pinB, const int pinC) { -void SAMDCurrentSenseADCDMA::adcToDMATransfer(void *rxdata, uint32_t hwords) { + if(_isset(_pinA) || _isset(_pinB) || _isset(_pinC)) { + SIMPLEFOC_DEBUG("SAMD-CUR: ERR: Pins already in use for current sensing!"); + return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED;; + } - DMAC->CHID.reg = DMAC_CHID_ID(channelDMA); - DMAC->CHCTRLA.reg &= ~DMAC_CHCTRLA_ENABLE; - DMAC->CHCTRLA.reg = DMAC_CHCTRLA_SWRST; - DMAC->SWTRIGCTRL.reg &= (uint32_t)(~(1 << channelDMA)); + // --- Configure ADC pins --- + if (_isset(pinA)) pinMode(pinA, INPUT); + if (_isset(pinB)) pinMode(pinB, INPUT); + if (_isset(pinC)) pinMode(pinC, INPUT); - DMAC->CHCTRLB.reg = DMAC_CHCTRLB_LVL(0) - | DMAC_CHCTRLB_TRIGSRC(ADC_DMAC_ID_RESRDY) - | DMAC_CHCTRLB_TRIGACT_BEAT; - DMAC->CHINTENSET.reg = DMAC_CHINTENSET_MASK ; // enable all 3 interrupts - descriptor.descaddr = 0; - descriptor.srcaddr = (uint32_t) &ADC->RESULT.reg; - descriptor.btcnt = hwords; - descriptor.dstaddr = (uint32_t)rxdata + hwords*2; // end address - descriptor.btctrl = DMAC_BTCTRL_BEATSIZE_HWORD | DMAC_BTCTRL_DSTINC | DMAC_BTCTRL_VALID; - memcpy(&descriptor_section[channelDMA],&descriptor, sizeof(dmacdescriptor)); - - // start channel - DMAC->CHID.reg = DMAC_CHID_ID(channelDMA); - DMAC->CHCTRLA.reg |= DMAC_CHCTRLA_ENABLE; -} - + // save the pins for later use + // only one motor possible for now + _pinA = pinA; + _pinB = pinB; + _pinC = pinC; -int iii = 0; + GenericCurrentSenseParams* params = new GenericCurrentSenseParams { + .pins = { pinA, pinB, pinC }, + .adc_voltage_conv = (_SAMD21_ADC_VOLTAGE) / (_SAMD21_ADC_RESOLUTION), -void adcStopWithDMA(void){ - ADCsync(); - ADC->CTRLA.bit.ENABLE = 0x00; - // ADCsync(); - // if(iii++ % 1000 == 0) - // { - // SIMPLEFOC_SAMD_DEBUG_SERIAL.print(a); - // SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" :: "); - // SIMPLEFOC_SAMD_DEBUG_SERIAL.print(b); - // SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" :: "); - // SIMPLEFOC_SAMD_DEBUG_SERIAL.print(c); - // SIMPLEFOC_SAMD_DEBUG_SERIAL.println("yo!"); - // } + }; + return params; +} -} +float _readADCVoltageLowSide(const int pin, const void* cs_params) { -void adcStartWithDMA(void){ - ADCsync(); - ADC->INPUTCTRL.bit.INPUTOFFSET = 0; - ADCsync(); - ADC->SWTRIG.bit.FLUSH = 1; - ADCsync(); - ADC->CTRLA.bit.ENABLE = 0x01; - ADCsync(); -} + // extract the ADC raw value for the given pin + float countsToVolts = ((GenericCurrentSenseParams*)cs_params)->adc_voltage_conv; -void DMAC_Handler() { - uint8_t active_channel; - active_channel = DMAC->INTPEND.reg & DMAC_INTPEND_ID_Msk; // get channel number - DMAC->CHID.reg = DMAC_CHID_ID(active_channel); - adcStopWithDMA(); - DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_TCMPL; // clear - DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_TERR; - DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_SUSP; + // read the value form the buffer + int i = 0; + for(auto p: ((GenericCurrentSenseParams*)cs_params)->pins) { + if (p == pin) return adc_raw[i] * countsToVolts; + i++; + } + return 0.0; // pin not available } +void* _driverSyncLowSide(void* driver_params, void* cs_params) { + + setupADCEventTriggerFromDriver((const SAMDHardwareDriverParams*)driver_params, (const GenericCurrentSenseParams*)cs_params); + return cs_params; +} -#endif \ No newline at end of file +#endif diff --git a/src/current_sense/hardware_specific/samd/samd21_mcu.h b/src/current_sense/hardware_specific/samd/samd21_mcu.h index e7d74426..1d323681 100644 --- a/src/current_sense/hardware_specific/samd/samd21_mcu.h +++ b/src/current_sense/hardware_specific/samd/samd21_mcu.h @@ -1,4 +1,6 @@ -#ifdef _SAMD21_ +#include "Arduino.h" + +#if defined(_SAMD21_) #ifndef CURRENT_SENSE_SAMD21_H #define CURRENT_SENSE_SAMD21_H @@ -8,59 +10,7 @@ #define SIMPLEFOC_SAMD_DEBUG_SERIAL Serial #endif -#include - typedef struct { - uint16_t btctrl; - uint16_t btcnt; - uint32_t srcaddr; - uint32_t dstaddr; - uint32_t descaddr; - } dmacdescriptor ; - - -// AREF pin is 42 - -class SAMDCurrentSenseADCDMA -{ - -public: - static SAMDCurrentSenseADCDMA * getHardwareAPIInstance(); - SAMDCurrentSenseADCDMA(); - void init(int pinA, int pinB, int pinC, int pinAREF = 42, float voltageAREF = 3.3, uint32_t adcBits = 12, uint32_t channelDMA = 3); - void startADCScan(); - bool readResults(uint16_t & a, uint16_t & b, uint16_t & c); - float toVolts(uint16_t counts); -private: - - void adcToDMATransfer(void *rxdata, uint32_t hwords); - - void initPins(); - void initADC(); - void initDMA(); - - uint32_t oneBeforeFirstAIN; // hack to discard first noisy readout - uint32_t firstAIN; - uint32_t lastAIN; - uint32_t BufferSize = 0; - - uint16_t adcBuffer[20]; - - - uint32_t pinA; - uint32_t pinB; - uint32_t pinC; - uint32_t pinAREF; - uint32_t channelDMA; // DMA channel - bool freeRunning; - - float voltageAREF; - float maxCountsADC; - float countsToVolts; - - dmacdescriptor descriptor_section[12] __attribute__ ((aligned (16))); - dmacdescriptor descriptor __attribute__ ((aligned (16))); - -}; +#include "../../hardware_api.h" #endif diff --git a/src/current_sense/hardware_specific/silabs/efr32_mcu.cpp b/src/current_sense/hardware_specific/silabs/efr32_mcu.cpp new file mode 100644 index 00000000..6ebbbf2e --- /dev/null +++ b/src/current_sense/hardware_specific/silabs/efr32_mcu.cpp @@ -0,0 +1,426 @@ +#if defined(ARDUINO_ARCH_SILABS) +#include +#include + +#include +#include +#include "efr32_mcu.h" +#include "../../../drivers/hardware_specific/silabs/efr32_mcu.h" + +#ifndef _ADC_VOLTAGE +#define _ADC_VOLTAGE 3.3f +#endif + +#ifndef _ADC_RESOLUTION +#define _ADC_RESOLUTION 4095.0f +#endif + +#ifndef _CLK_SRC_ADC_FREQ +#define _CLK_SRC_ADC_FREQ 20000000 +#endif + +#ifndef _CLK_ADC_FREQ +#define _CLK_ADC_FREQ 10000000 +#endif + +extern void _getPrsSourceAndUnderflowSignal( + TIMER_TypeDef *timer, + uint32_t *source, + uint32_t *signal); + +static void _adcBusAllocate( + uint8_t port, + uint8_t pin +) { + switch (port) { +#if (GPIO_PA_COUNT > 0) + case gpioPortA: + if (0 == pin % 2) { + if ((GPIO->ABUSALLOC & _GPIO_ABUSALLOC_AEVEN0_MASK) == GPIO_ABUSALLOC_AEVEN0_TRISTATE) { + GPIO->ABUSALLOC |= GPIO_ABUSALLOC_AEVEN0_ADC0; + } else if ((GPIO->ABUSALLOC & _GPIO_ABUSALLOC_AEVEN1_MASK) == GPIO_ABUSALLOC_AEVEN1_TRISTATE) { + GPIO->ABUSALLOC |= GPIO_ABUSALLOC_AEVEN1_ADC0; + } else {} + } else { + if ((GPIO->ABUSALLOC & _GPIO_ABUSALLOC_AODD0_MASK) == GPIO_ABUSALLOC_AODD0_TRISTATE) { + GPIO->ABUSALLOC |= GPIO_ABUSALLOC_AODD0_ADC0; + } else if ((GPIO->ABUSALLOC & _GPIO_ABUSALLOC_AODD1_MASK) == GPIO_ABUSALLOC_AODD1_TRISTATE) { + GPIO->ABUSALLOC |= GPIO_ABUSALLOC_AODD1_ADC0; + } else { + // MISRA + } + } + break; +#endif + +#if (GPIO_PB_COUNT > 0) + case gpioPortB: + if (0 == pin % 2) { + if ((GPIO->BBUSALLOC & _GPIO_BBUSALLOC_BEVEN0_MASK) == GPIO_BBUSALLOC_BEVEN0_TRISTATE) { + GPIO->BBUSALLOC |= GPIO_BBUSALLOC_BEVEN0_ADC0; + } else if ((GPIO->BBUSALLOC & _GPIO_BBUSALLOC_BEVEN1_MASK) == GPIO_BBUSALLOC_BEVEN1_TRISTATE) { + GPIO->BBUSALLOC |= GPIO_BBUSALLOC_BEVEN1_ADC0; + } else { + // MISRA + } + } else { + if ((GPIO->BBUSALLOC & _GPIO_BBUSALLOC_BODD0_MASK) == GPIO_BBUSALLOC_BODD0_TRISTATE) { + GPIO->BBUSALLOC |= GPIO_BBUSALLOC_BODD0_ADC0; + } else if ((GPIO->BBUSALLOC & _GPIO_BBUSALLOC_BODD1_MASK) == GPIO_BBUSALLOC_BODD1_TRISTATE) { + GPIO->BBUSALLOC |= GPIO_BBUSALLOC_BODD1_ADC0; + } else { + // MISRA + } + } + break; +#endif + +#if (GPIO_PC_COUNT > 0 || GPIO_PD_COUNT > 0) + case gpioPortC: + case gpioPortD: + if (0 == pin % 2) { + if ((GPIO->CDBUSALLOC & _GPIO_CDBUSALLOC_CDEVEN0_MASK) == GPIO_CDBUSALLOC_CDEVEN0_TRISTATE) { + GPIO->CDBUSALLOC |= GPIO_CDBUSALLOC_CDEVEN0_ADC0; + } else if ((GPIO->CDBUSALLOC & _GPIO_CDBUSALLOC_CDEVEN1_MASK) == GPIO_CDBUSALLOC_CDEVEN1_TRISTATE) { + GPIO->CDBUSALLOC |= GPIO_CDBUSALLOC_CDEVEN1_ADC0; + } else { + // MISRA + } + } else { + if ((GPIO->CDBUSALLOC & _GPIO_CDBUSALLOC_CDODD0_MASK) == GPIO_CDBUSALLOC_CDODD0_TRISTATE) { + GPIO->CDBUSALLOC |= GPIO_CDBUSALLOC_CDODD0_ADC0; + } else if ((GPIO->CDBUSALLOC & _GPIO_CDBUSALLOC_CDODD1_MASK) == GPIO_CDBUSALLOC_CDODD1_TRISTATE) { + GPIO->CDBUSALLOC |= GPIO_CDBUSALLOC_CDODD1_ADC0; + } else { + // MISRA + } + } + break; +#endif + } +} + +static void _adcConfig( + EFR32AdcInstance *inst, + const int pin +) { + if (!inst) return; + inst->port = getSilabsPortFromArduinoPin(pinToPinName(pin)); + inst->pin = getSilabsPinFromArduinoPin(pinToPinName(pin)); +} + +static float _readAdc( + EFR32CurrentSenseParams *params, + const int pin +) { + if (!params) return 0.0f; + + for (uint8_t i = 0; i < SILABS_MAX_ANALOG; ++i) { + if (!_isset(params->pins[i])) continue; + if (pin == params->pins[i]) { + return params->buffer[i] * params->adc_voltage_conv; + } + } + return 0.0f; +} + +static bool _dmaTransferFinishedCb( + unsigned int channel, + unsigned int sequenceNo, + void *data +) { + _UNUSED(sequenceNo); + + EFR32CurrentSenseParams *params = (EFR32CurrentSenseParams *) data; + if (!params || !params->adc || (params->dmaChannel != channel)) + return false; + + CORE_DECLARE_IRQ_STATE; + CORE_ENTER_ATOMIC(); + params->dataReady = true; + CORE_EXIT_ATOMIC(); + + return false; +} + +static void _currentSenseInitDMA( + EFR32CurrentSenseParams *params, + DMADRV_Callback_t fn, + void *data +) { + if (!params) return; + + // Initialize DMA with default parameters + DMADRV_Init(); + + DMADRV_AllocateChannel(¶ms->dmaChannel, NULL); + + // Trigger LDMA transfer on IADC scan completion + LDMA_TransferCfg_t transferCfg = + LDMA_TRANSFER_CFG_PERIPHERAL(ldmaPeripheralSignal_IADC0_IADC_SCAN); + + params->descriptor = + (LDMA_Descriptor_t)LDMA_DESCRIPTOR_LINKREL_P2M_WORD(&(params->adc->SCANFIFODATA), + params->buffer, + params->noAdcChannels, + 0); + + DMADRV_LdmaStartTransfer(params->dmaChannel, + &transferCfg, + ¶ms->descriptor, + fn, + params); +} + +static void _currentSenseInitScan( + EFR32CurrentSenseParams *params +) { + if (!params || !params->adc) return; + + // Enable Clock + CMU_ClockEnable(cmuClock_IADC0, true); + CMU_ClockEnable(cmuClock_GPIO, true); + + // Use the FSRC0 as the IADC clock so it can run in EM2 + CMU_ClockSelectSet(cmuClock_IADCCLK, cmuSelect_FSRCO); + + for (uint8_t i = 0; i < params->noAdcChannels; ++i) { + GPIO_PinModeSet(params->inst[i].port, params->inst[i].pin, gpioModeDisabled, 0); + } + + IADC_Init_t init = IADC_INIT_DEFAULT; + init.warmup = iadcWarmupKeepWarm; + init.srcClkPrescale = IADC_calcSrcClkPrescale(params->adc, _CLK_SRC_ADC_FREQ, 0); + + IADC_CfgReference_t adcRef; + switch (params->vRef) { + case 1200: adcRef = iadcCfgReferenceInt1V2; break; + case 1250: adcRef = iadcCfgReferenceExt1V25; break; + case 3300: adcRef = iadcCfgReferenceVddx; break; + case 2640: adcRef = iadcCfgReferenceVddX0P8Buf; break; + default: return; + } + + IADC_AllConfigs_t allConfigs = IADC_ALLCONFIGS_DEFAULT; + allConfigs.configs[0].reference = adcRef; + allConfigs.configs[0].vRef = params->vRef; + allConfigs.configs[0].adcClkPrescale = IADC_calcAdcClkPrescale(params->adc, + _CLK_ADC_FREQ, + 0, + iadcCfgModeNormal, + init.srcClkPrescale); + + // Reset the ADC + IADC_reset(params->adc); + + // Only configure the ADC if it is not already running + if (params->adc->CTRL == _IADC_CTRL_RESETVALUE) { + IADC_init(params->adc, &init, &allConfigs); + } + + IADC_InitScan_t initScan = IADC_INITSCAN_DEFAULT; + if ((params->mode == CS_LO_SIDE) || (params->mode == CS_HI_SIDE)) { + // Note: CS_HI_SIDE not implemented + initScan.triggerSelect = iadcTriggerSelPrs0PosEdge; + } + initScan.fifoDmaWakeup = true; + + IADC_ScanTable_t scanTable = IADC_SCANTABLE_DEFAULT; + for (uint8_t i = 0; i < params->noAdcChannels; ++i) { + scanTable.entries[i].posInput = IADC_portPinToPosInput(params->inst[i].port, params->inst[i].pin); + scanTable.entries[i].negInput = iadcNegInputGnd; + scanTable.entries[i].includeInScan = true; + } + + // Initialize IADC + IADC_init(params->adc, &init, &allConfigs); + + // Initialize Scan + IADC_initScan(params->adc, &initScan, &scanTable); + + // Allocate + for (uint8_t i = 0; i < params->noAdcChannels; ++i) { + _adcBusAllocate(params->inst[i].port, params->inst[i].pin); + } +} + +static void _currentSenseConfig( + EFR32CurrentSenseParams *params, + int adcPins[SILABS_MAX_ANALOG] +) { + if (!params) return; + + uint8_t noAdcChannels = 0; + for (int i = 0; i < SILABS_MAX_ANALOG; ++i) { + if (!_isset(adcPins[i])) continue; + if (params->firstIndex == 0xFF) params->firstIndex = i; + _adcConfig(¶ms->inst[noAdcChannels], adcPins[i]); + params->pins[noAdcChannels] = adcPins[i]; + ++noAdcChannels; + } + + params->noAdcChannels = noAdcChannels; +} + +static void _currentSenseDeinit( + EFR32CurrentSenseParams *params +) { + if (!params) return; + + DMADRV_StopTransfer(params->dmaChannel); + DMADRV_FreeChannel(params->dmaChannel); + + IADC_reset(params->adc); +} + +static void _currentSenseStartScan( + EFR32CurrentSenseParams *params +) { + if (!params || !params->adc) return; + + IADC_command(params->adc, iadcCmdStartScan); +} + +static void _currentSenseStopScan( + EFR32CurrentSenseParams *params +) { + if (!params) return; + + IADC_command(params->adc, iadcCmdStopScan); +} + +static void _currentSenseStopTranfer( + EFR32CurrentSenseParams *params +) { + if (!params || !params->adc) return; + + DMADRV_PauseTransfer(params->dmaChannel); +} + +static void _currentSenseStartTranfer( + EFR32CurrentSenseParams *params +) { + if (!params || !params->adc) return; + + DMADRV_ResumeTransfer(params->dmaChannel); +} + +//////////////////////////////////////////////////////////////////////////////// +// Low Side Mode +//////////////////////////////////////////////////////////////////////////////// + +float _readADCVoltageLowSide( + const int pin, + const void *cs_params +) { + EFR32CurrentSenseParams *params = (EFR32CurrentSenseParams *) cs_params; + if (!params) return 0.0f; + + return _readAdc(params, pin); +} + +void* _configureADCLowSide( + const void* driver_params, + const int pinA, + const int pinB, + const int pinC +) { + _UNUSED(driver_params); + + EFR32CurrentSenseParams *params = new EFR32CurrentSenseParams { + .adc_voltage_conv = (_ADC_VOLTAGE) / (_ADC_RESOLUTION), + .firstIndex = 0xFF, + .noAdcChannels = 0, + .vRef = SILABS_ADC_VREF, + .prsChannel = SILABS_ADC_PRS_CHANNEL, + .mode = CS_LO_SIDE, + .adc = SILABS_DEFAULT_ADC_PERPHERAL, + }; + + if (!params) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; + + int adcPins[3] = { pinA, pinB, pinC }; + + _currentSenseConfig(params, adcPins); + _currentSenseInitScan(params); + _currentSenseInitDMA(params, NULL, params); + _currentSenseStartScan(params); + + return params; +} + +void* _driverSyncLowSide( + void *driver_params, + void *cs_params +) { + EFR32DriverParams *driver = (EFR32DriverParams *) driver_params; + EFR32CurrentSenseParams *params = (EFR32CurrentSenseParams *) cs_params; + + if (!driver || !params) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; + + uint32_t prsSource, prsSignal; + + CMU_ClockEnable(cmuClock_PRS, true); + + _getPrsSourceAndUnderflowSignal(driver->inst[0].h.timer, &prsSource, &prsSignal); + PRS_SourceAsyncSignalSet(params->prsChannel, prsSource, prsSignal); + PRS_ConnectConsumer(params->prsChannel, prsTypeAsync, prsConsumerIADC0_SCANTRIGGER); + + return cs_params; +} + +//////////////////////////////////////////////////////////////////////////////// +// Inline Mode +//////////////////////////////////////////////////////////////////////////////// + +float _readADCVoltageInline( + const int pin, + const void *cs_params +) { + EFR32CurrentSenseParams *params = (EFR32CurrentSenseParams *) cs_params; + if (!params || !_isset(pin) || (params->firstIndex == 0xFF)) return 0.0f; + + if (params->pins[params->firstIndex] == pin) { + CORE_DECLARE_IRQ_STATE; + CORE_ENTER_ATOMIC(); + params->dataReady = false; + CORE_EXIT_ATOMIC(); + + _currentSenseStartScan(params); + + while (!params->dataReady) {} + } + + return _readAdc(params, pin); +} + +void* _configureADCInline( + const void* driver_params, + const int pinA, + const int pinB, + const int pinC +) { + _UNUSED(driver_params); + + EFR32CurrentSenseParams *params = new EFR32CurrentSenseParams { + .adc_voltage_conv = (_ADC_VOLTAGE) / (_ADC_RESOLUTION), + .firstIndex = 0xFF, + .noAdcChannels = 0, + .vRef = SILABS_ADC_VREF, + .prsChannel = SILABS_ADC_PRS_CHANNEL, + .mode = CS_INLINE, + .adc = SILABS_DEFAULT_ADC_PERPHERAL, + }; + + if (!params) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; + + int adcPins[3] = { pinA, pinB, pinC }; + + _currentSenseConfig(params, adcPins); + _currentSenseInitScan(params); + _currentSenseInitDMA(params, _dmaTransferFinishedCb, params); + + return params; +} + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/silabs/efr32_mcu.h b/src/current_sense/hardware_specific/silabs/efr32_mcu.h new file mode 100644 index 00000000..40657ec4 --- /dev/null +++ b/src/current_sense/hardware_specific/silabs/efr32_mcu.h @@ -0,0 +1,58 @@ +#ifndef EFR32_CURRENTSENSE_MCU_H +#define EFR32_CURRENTSENSE_MCU_H + +#include "../../hardware_api.h" + +#if defined(ARDUINO_ARCH_SILABS) +#include +#include +#include +#include + +#ifndef SILABS_DEFAULT_ADC_PERPHERAL +#define SILABS_DEFAULT_ADC_PERPHERAL IADC0 +#endif + +#ifndef SILABS_ADC_VREF +#define SILABS_ADC_VREF 3300 +#endif + +#ifndef SILABS_ADC_PRS_CHANNEL +#define SILABS_ADC_PRS_CHANNEL 1 +#endif + +#ifndef SILABS_MAX_ANALOG +#define SILABS_MAX_ANALOG 3 +#endif + +typedef enum { + CS_INLINE, + CS_LO_SIDE, + CS_HI_SIDE, +} EFR32CurrentSenseMode; + +typedef struct { + uint8_t port; + uint8_t pin; +} EFR32AdcInstance; + +typedef struct { + int pins[SILABS_MAX_ANALOG]; + float adc_voltage_conv; + EFR32AdcInstance inst[SILABS_MAX_ANALOG]; + uint8_t firstIndex; + uint8_t noAdcChannels; + volatile bool dataReady; + uint32_t id; + uint32_t buffer[SILABS_MAX_ANALOG]; + uint32_t vRef; + unsigned int dmaChannel; + unsigned int prsChannel; + EFR32CurrentSenseMode mode; + IADC_TypeDef *adc; + LDMA_Descriptor_t descriptor; +} EFR32CurrentSenseParams; + +#endif + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/b_g431/b_g431_hal.cpp b/src/current_sense/hardware_specific/stm32/b_g431/b_g431_hal.cpp index dc505d6f..30e0ed95 100644 --- a/src/current_sense/hardware_specific/stm32/b_g431/b_g431_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/b_g431/b_g431_hal.cpp @@ -95,7 +95,11 @@ void MX_ADC1_Init(ADC_HandleTypeDef* hadc1) hadc1->Init.EOCSelection = ADC_EOC_SINGLE_CONV; hadc1->Init.LowPowerAutoWait = DISABLE; hadc1->Init.ContinuousConvMode = DISABLE; + #ifdef OPAMP_USE_INTERNAL_CHANNEL + hadc1->Init.NbrOfConversion = 4; + #else hadc1->Init.NbrOfConversion = 5; + #endif hadc1->Init.DiscontinuousConvMode = DISABLE; hadc1->Init.ExternalTrigConv = ADC_EXTERNALTRIG_T1_TRGO; hadc1->Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_RISING; @@ -114,9 +118,10 @@ void MX_ADC1_Init(ADC_HandleTypeDef* hadc1) { SIMPLEFOC_DEBUG("HAL_ADCEx_MultiModeConfigChannel failed!"); } + #ifndef OPAMP_USE_INTERNAL_CHANNEL /** Configure Regular Channel */ - sConfig.Channel = ADC_CHANNEL_12; // ADC1_IN12 = PB1 = OP3_OUT + sConfig.Channel = ADC_CHANNEL_12; // ADC1_IN12 = PB1 = OP3_OUT or ADC2_IN18 for internal channel sConfig.Rank = ADC_REGULAR_RANK_1; sConfig.SamplingTime = ADC_SAMPLETIME_2CYCLES_5; sConfig.SingleDiff = ADC_SINGLE_ENDED; @@ -126,10 +131,20 @@ void MX_ADC1_Init(ADC_HandleTypeDef* hadc1) { SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!"); } + #endif /** Configure Regular Channel */ - sConfig.Channel = ADC_CHANNEL_3; // ADC1_IN3 = PA2 = OP1_OUT + #ifdef OPAMP_USE_INTERNAL_CHANNEL + sConfig.Channel = ADC_CHANNEL_13; // ADC1_IN3 = PA2 = OP1_OUT or ADC1_IN13 for internal channel + sConfig.Rank = ADC_REGULAR_RANK_1; + sConfig.SamplingTime = ADC_SAMPLETIME_2CYCLES_5; + sConfig.SingleDiff = ADC_SINGLE_ENDED; + sConfig.OffsetNumber = ADC_OFFSET_NONE; + sConfig.Offset = 0; + #else + sConfig.Channel = ADC_CHANNEL_3; // ADC1_IN3 = PA2 = OP1_OUT or ADC1_IN13 for internal channel sConfig.Rank = ADC_REGULAR_RANK_2; + #endif if (HAL_ADC_ConfigChannel(hadc1, &sConfig) != HAL_OK) { SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!"); @@ -140,7 +155,11 @@ void MX_ADC1_Init(ADC_HandleTypeDef* hadc1) /* Configure Regular Channel (PB12, Potentiometer) */ sConfig.Channel = ADC_CHANNEL_11; + #ifdef OPAMP_USE_INTERNAL_CHANNEL + sConfig.Rank = ADC_REGULAR_RANK_2; + #else sConfig.Rank = ADC_REGULAR_RANK_3; + #endif sConfig.SamplingTime = ADC_SAMPLETIME_47CYCLES_5; sConfig.SingleDiff = ADC_SINGLE_ENDED; sConfig.OffsetNumber = ADC_OFFSET_NONE; @@ -153,7 +172,11 @@ void MX_ADC1_Init(ADC_HandleTypeDef* hadc1) /** Configure Regular Channel (PB14, Temperature) */ sConfig.Channel = ADC_CHANNEL_5; + #ifdef OPAMP_USE_INTERNAL_CHANNEL + sConfig.Rank = ADC_REGULAR_RANK_3; + #else sConfig.Rank = ADC_REGULAR_RANK_4; + #endif sConfig.SamplingTime = ADC_SAMPLETIME_47CYCLES_5; sConfig.SingleDiff = ADC_SINGLE_ENDED; sConfig.OffsetNumber = ADC_OFFSET_NONE; @@ -163,10 +186,14 @@ void MX_ADC1_Init(ADC_HandleTypeDef* hadc1) SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!"); } - /** Configure Regular Channel (PB14, Temperature) + /** Configure Regular Channel (PA0, VBUS) */ sConfig.Channel = ADC_CHANNEL_1; + #ifdef OPAMP_USE_INTERNAL_CHANNEL + sConfig.Rank = ADC_REGULAR_RANK_4; + #else sConfig.Rank = ADC_REGULAR_RANK_5; + #endif sConfig.SamplingTime = ADC_SAMPLETIME_47CYCLES_5; sConfig.SingleDiff = ADC_SINGLE_ENDED; sConfig.OffsetNumber = ADC_OFFSET_NONE; @@ -208,7 +235,11 @@ void MX_ADC2_Init(ADC_HandleTypeDef* hadc2) hadc2->Init.EOCSelection = ADC_EOC_SINGLE_CONV; hadc2->Init.LowPowerAutoWait = DISABLE; hadc2->Init.ContinuousConvMode = DISABLE; + #ifdef OPAMP_USE_INTERNAL_CHANNEL + hadc2->Init.NbrOfConversion = 2; + #else hadc2->Init.NbrOfConversion = 1; + #endif hadc2->Init.DiscontinuousConvMode = DISABLE; hadc2->Init.ExternalTrigConv = ADC_EXTERNALTRIG_T1_TRGO; hadc2->Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_RISING; @@ -221,7 +252,11 @@ void MX_ADC2_Init(ADC_HandleTypeDef* hadc2) } /** Configure Regular Channel */ - sConfig.Channel = ADC_CHANNEL_3; // ADC2_IN3 = PA6 + #ifdef OPAMP_USE_INTERNAL_CHANNEL + sConfig.Channel = ADC_CHANNEL_16; // ADC2_IN3 = PA6 = OP2_OUT or ADC2_IN16 for internal channel + #else + sConfig.Channel = ADC_CHANNEL_3; // ADC2_IN3 = PA6 = OP2_OUT or ADC2_IN16 for internal channel + #endif sConfig.Rank = ADC_REGULAR_RANK_1; sConfig.SamplingTime = ADC_SAMPLETIME_2CYCLES_5; sConfig.SingleDiff = ADC_SINGLE_ENDED; @@ -231,6 +266,20 @@ void MX_ADC2_Init(ADC_HandleTypeDef* hadc2) { SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!"); } + #ifdef OPAMP_USE_INTERNAL_CHANNEL + /** Configure Regular Channel + */ + sConfig.Channel = ADC_CHANNEL_18; // ADC1_IN12 = PB1 = OP3_OUT or ADC2_IN18 for internal channel + sConfig.Rank = ADC_REGULAR_RANK_2; + sConfig.SamplingTime = ADC_SAMPLETIME_2CYCLES_5; + sConfig.SingleDiff = ADC_SINGLE_ENDED; + sConfig.OffsetNumber = ADC_OFFSET_NONE; + sConfig.Offset = 0; + if (HAL_ADC_ConfigChannel(hadc2, &sConfig) != HAL_OK) + { + SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!"); + } + #endif /* USER CODE BEGIN ADC2_Init 2 */ /* USER CODE END ADC2_Init 2 */ @@ -331,7 +380,7 @@ void HAL_OPAMP_MspDeInit(OPAMP_HandleTypeDef* hopamp) PA2 ------> OPAMP1_VOUT PA3 ------> OPAMP1_VINM */ - HAL_GPIO_DeInit(GPIOA, GPIO_PIN_1|GPIO_PIN_2|GPIO_PIN_3); + HAL_GPIO_DeInit(GPIOA, GPIO_PIN_1|(GPIO_PIN_2 * OPAMP_USE_INTERNAL_CHANNEL)|GPIO_PIN_3); /* USER CODE BEGIN OPAMP1_MspDeInit 1 */ @@ -348,7 +397,7 @@ void HAL_OPAMP_MspDeInit(OPAMP_HandleTypeDef* hopamp) PA6 ------> OPAMP2_VOUT PA7 ------> OPAMP2_VINP */ - HAL_GPIO_DeInit(GPIOA, GPIO_PIN_5|GPIO_PIN_6|GPIO_PIN_7); + HAL_GPIO_DeInit(GPIOA, GPIO_PIN_5|(GPIO_PIN_6 * OPAMP_USE_INTERNAL_CHANNEL)|GPIO_PIN_7); /* USER CODE BEGIN OPAMP2_MspDeInit 1 */ @@ -365,7 +414,7 @@ void HAL_OPAMP_MspDeInit(OPAMP_HandleTypeDef* hopamp) PB1 ------> OPAMP3_VOUT PB2 ------> OPAMP3_VINM */ - HAL_GPIO_DeInit(GPIOB, GPIO_PIN_0|GPIO_PIN_1|GPIO_PIN_2); + HAL_GPIO_DeInit(GPIOB, GPIO_PIN_0|(GPIO_PIN_1 * OPAMP_USE_INTERNAL_CHANNEL)|GPIO_PIN_2); /* USER CODE BEGIN OPAMP3_MspDeInit 1 */ diff --git a/src/current_sense/hardware_specific/stm32/b_g431/b_g431_hal.h b/src/current_sense/hardware_specific/stm32/b_g431/b_g431_hal.h index 2d6a1f0a..bdc21765 100644 --- a/src/current_sense/hardware_specific/stm32/b_g431/b_g431_hal.h +++ b/src/current_sense/hardware_specific/stm32/b_g431/b_g431_hal.h @@ -13,6 +13,10 @@ void MX_ADC2_Init(ADC_HandleTypeDef* hadc2); void MX_OPAMP1_Init(OPAMP_HandleTypeDef* hopamp); void MX_OPAMP2_Init(OPAMP_HandleTypeDef* hopamp); void MX_OPAMP3_Init(OPAMP_HandleTypeDef* hopamp); + +#ifndef OPAMP_USE_INTERNAL_CHANNEL + #define OPAMP_USE_INTERNAL_CHANNEL 1 +#endif #endif #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/b_g431/b_g431_mcu.cpp b/src/current_sense/hardware_specific/stm32/b_g431/b_g431_mcu.cpp index 8456759c..060f43c7 100644 --- a/src/current_sense/hardware_specific/stm32/b_g431/b_g431_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/b_g431/b_g431_mcu.cpp @@ -7,11 +7,17 @@ #include "../stm32_mcu.h" #include "../../../../drivers/hardware_specific/stm32/stm32_mcu.h" #include "communication/SimpleFOCDebug.h" +#include "../stm32_adc_utils.h" #define _ADC_VOLTAGE 3.3f #define _ADC_RESOLUTION 4096.0f +#ifdef OPAMP_USE_INTERNAL_CHANNEL +#define ADC_BUF_LEN_1 4 +#define ADC_BUF_LEN_2 2 +#else #define ADC_BUF_LEN_1 5 #define ADC_BUF_LEN_2 1 +#endif static ADC_HandleTypeDef hadc1; static ADC_HandleTypeDef hadc2; @@ -25,26 +31,59 @@ static DMA_HandleTypeDef hdma_adc2; volatile uint16_t adcBuffer1[ADC_BUF_LEN_1] = {0}; // Buffer for store the results of the ADC conversion volatile uint16_t adcBuffer2[ADC_BUF_LEN_2] = {0}; // Buffer for store the results of the ADC conversion + +// structure containing the configuration of the adc interrupt +Stm32AdcInterruptConfig adc_interrupt_config[5] = { + {0, 0, 0}, // ADC1 + {0, 0, 0}, // ADC2 + {0, 0, 0}, // ADC3 + {0, 0, 0}, // ADC4 + {0, 0, 0} // ADC5 +}; + // function reading an ADC value and returning the read voltage // As DMA is being used just return the DMA result -float _readADCVoltageInline(const int pin, const void* cs_params){ - uint32_t raw_adc = 0; - if(pin == PA2) // = ADC1_IN3 = phase U (OP1_OUT) on B-G431B-ESC1 - raw_adc = adcBuffer1[1]; - else if(pin == PA6) // = ADC2_IN3 = phase V (OP2_OUT) on B-G431B-ESC1 +float _readADCVoltageLowSide(const int pin, const void* cs_params){ + uint32_t raw_adc; + #ifdef OPAMP_USE_INTERNAL_CHANNEL + #define ADC1_OFFSET 0 + #else + #define ADC1_OFFSET 1 + #endif + + switch (pin) + { + case A_OP1_OUT: + case -1: + raw_adc = adcBuffer1[0+ADC1_OFFSET]; + break; + case A_OP2_OUT: + case -2: raw_adc = adcBuffer2[0]; -#ifdef PB1 - else if(pin == PB1) // = ADC1_IN12 = phase W (OP3_OUT) on B-G431B-ESC1 + break; + #ifdef A_OP3_OUT + case A_OP3_OUT: + #endif + case -3: + #ifdef OPAMP_USE_INTERNAL_CHANNEL + raw_adc = adcBuffer2[1]; + #else raw_adc = adcBuffer1[0]; -#endif - - else if (pin == A_POTENTIOMETER) - raw_adc = adcBuffer1[2]; - else if (pin == A_TEMPERATURE) - raw_adc = adcBuffer1[3]; - else if (pin == A_VBUS) - raw_adc = adcBuffer1[4]; - + #endif + break; + case A_POTENTIOMETER: + raw_adc = adcBuffer1[1+ADC1_OFFSET]; + break; + case A_TEMPERATURE: + raw_adc = adcBuffer1[2+ADC1_OFFSET]; + break; + case A_VBUS: + raw_adc = adcBuffer1[3+ADC1_OFFSET]; + break; + default: + raw_adc = 0; + break; + } return raw_adc * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; } @@ -54,7 +93,7 @@ void _configureOPAMP(OPAMP_HandleTypeDef *hopamp, OPAMP_TypeDef *OPAMPx_Def){ hopamp->Init.PowerMode = OPAMP_POWERMODE_HIGHSPEED; hopamp->Init.Mode = OPAMP_PGA_MODE; hopamp->Init.NonInvertingInput = OPAMP_NONINVERTINGINPUT_IO0; - hopamp->Init.InternalOutput = DISABLE; + hopamp->Init.InternalOutput = OPAMP_USE_INTERNAL_CHANNEL ? ENABLE : DISABLE; hopamp->Init.TimerControlledMuxmode = OPAMP_TIMERCONTROLLEDMUXMODE_DISABLE; hopamp->Init.PgaConnect = OPAMP_PGA_CONNECT_INVERTINGINPUT_IO0_BIAS; hopamp->Init.PgaGain = OPAMP_PGA_GAIN_16_OR_MINUS_15; @@ -132,7 +171,7 @@ void* _configureADCLowSide(const void* driver_params, const int pinA,const int p Stm32CurrentSenseParams* params = new Stm32CurrentSenseParams { .pins = { pinA, pinB, pinC }, .adc_voltage_conv = (_ADC_VOLTAGE) / (_ADC_RESOLUTION), - .timer_handle = (HardwareTimer *)(HardwareTimer_Handle[get_timer_index(TIM1)]->__this) + .timer_handle = ((STM32DriverParams*)driver_params)->timers_handle[0], }; return params; @@ -148,27 +187,33 @@ void DMA1_Channel2_IRQHandler(void) { } } -void _driverSyncLowSide(void* _driver_params, void* _cs_params){ +void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ STM32DriverParams* driver_params = (STM32DriverParams*)_driver_params; Stm32CurrentSenseParams* cs_params = (Stm32CurrentSenseParams*)_cs_params; // 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. - // 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; + stm32_pause(driver_params); + + // get the index of the adc + int adc_index = _adcToIndex(cs_params->adc_handle); + + bool tim_interrupt = _initTimerInterruptDownsampling(cs_params, driver_params, adc_interrupt_config[adc_index]); + if(tim_interrupt) { + // error in the timer interrupt initialization + SIMPLEFOC_DEBUG("STM32-CS: timer has no repetition counter, ADC interrupt not supported for B-G431"); + return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; } + // set the trigger output event - LL_TIM_SetTriggerOutput(cs_params->timer_handle->getHandle()->Instance, LL_TIM_TRGO_UPDATE); + LL_TIM_SetTriggerOutput(cs_params->timer_handle->Instance, LL_TIM_TRGO_UPDATE); // restart all the timers of the driver - _startTimers(driver_params->timers, 6); + stm32_resume(driver_params); + // return the cs parameters + // successfully initialized + // TODO verify if success in future + return _cs_params; } #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32_adc_utils.cpp b/src/current_sense/hardware_specific/stm32/stm32_adc_utils.cpp new file mode 100644 index 00000000..4e6427be --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32_adc_utils.cpp @@ -0,0 +1,495 @@ +#include "stm32_adc_utils.h" +#include "stm32_mcu.h" + +#if defined(_STM32_DEF_) + + + +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); +} + + +ADC_TypeDef* _indexToADC(uint8_t index){ + switch (index) { + case 0: + return ADC1; + break; +#ifdef ADC2 // if ADC2 exists + case 1: + return ADC2; + break; +#endif +#ifdef ADC3 // if ADC3 exists + case 2: + return ADC3; + break; +#endif +#ifdef ADC4 // if ADC4 exists + case 3: + return ADC4; + break; +#endif +#ifdef ADC5 // if ADC5 exists + case 4: + return ADC5; + break; +#endif + } + return nullptr; +} + +int _findIndexOfEntry(PinName pin) { + // remove the ALT if it is there + PinName pinName = (PinName)(pinName & ~ALTX_MASK); + int i = 0; + SIMPLEFOC_DEBUG("STM32-CS: Looking for pin "); + while (PinMap_ADC[i].pin !=NC) { + if (pinName == PinMap_ADC[i].pin ) + return i; + i++; + SIMPLEFOC_DEBUG("STM32-CS: Looking for pin ", i); + } + return -1; +} + +int _findIndexOfLastEntry(PinName pin) { + // remove the ALT if it is there + PinName pinName = (PinName)(pin & ~ALTX_MASK); + + int i = 0; + while (PinMap_ADC[i].pin!=NC) { + if ( pinName == (PinMap_ADC[i].pin & ~ALTX_MASK) + && pinName != (PinMap_ADC[i+1].pin & ~ALTX_MASK)) + return i; + i++; + } + return -1; +} +int _findIndexOfFirstEntry(PinName pin) { + // remove the ALT if it is there + PinName pinName = (PinName)(pin & ~ALTX_MASK); + int i = 0; + while (PinMap_ADC[i].pin !=NC) { + if (pinName == PinMap_ADC[i].pin ) + return i; + i++; + } + return -1; +} + +// functions finding the index of the first pin entry in the PinMap_ADC +// returns -1 if not found +int _findIndexOfFirstPinMapADCEntry(int pin) { + PinName pinName = digitalPinToPinName(pin); + // remove the ALT if it is there + return _findIndexOfFirstEntry(pinName); +} + +// functions finding the index of the last pin entry in the PinMap_ADC +// returns -1 if not found +int _findIndexOfLastPinMapADCEntry(int pin) { + PinName pinName = digitalPinToPinName(pin); + // remove the ALT if it is there + return _findIndexOfLastEntry(pinName); +} + +// find the best ADC combination for the given pins +// returns the index of the best ADC +// each pin can be connected to multiple ADCs +// the function will try to find a single ADC that can be used for all pins +// if not possible it will return nullptr +ADC_TypeDef* _findBestADCForPins(int numPins, int pins[]) { + + // assuning that there is less than 8 ADCs + uint8_t pins_at_adc[ADC_COUNT] = {0}; + + // check how many pins are there and are not set + int no_pins = 0; + for (int i = 0; i < numPins; i++) { + if(_isset(pins[i])) no_pins++; + } + + // loop over all elements and count the pins connected to each ADC + for (int i = 0; i < numPins; i++) { + int pin = pins[i]; + if(!_isset(pin)) continue; + + int index = _findIndexOfFirstPinMapADCEntry(pin); + int last_index = _findIndexOfLastPinMapADCEntry(pin); + if (index == -1) { + return nullptr; + } + for (int j = index; j <= last_index; j++) { + if (PinMap_ADC[j].pin == NC) { + break; + } + int adcIndex = _adcToIndex((ADC_TypeDef*)PinMap_ADC[j].peripheral); + pins_at_adc[adcIndex]++; + } + } + + for (int i = 0; i < ADC_COUNT; i++) { + if(!pins_at_adc[i]) continue; + SimpleFOCDebug::print("STM32-CS: ADC"); + SimpleFOCDebug::print(i+1); + SimpleFOCDebug::print(" pins: "); + SimpleFOCDebug::println(pins_at_adc[i]); + } + + // now take the first ADC that has all pins connected + for (int i = 0; i < ADC_COUNT; i++) { + if (pins_at_adc[i] == no_pins) { + return _indexToADC(i); + } + } + return nullptr; +} + + + +/** + * @brief Return ADC HAL channel linked to a PinName + * @param pin: PinName + * @retval Valid HAL channel + */ +uint32_t _getADCChannelFromPinMap(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 +#ifdef ADC_CHANNEL_1 + case 1: + channel = ADC_CHANNEL_1; + break; +#endif +#ifdef ADC_CHANNEL_2 + case 2: + channel = ADC_CHANNEL_2; + break; +#endif +#ifdef ADC_CHANNEL_3 + case 3: + channel = ADC_CHANNEL_3; + break; +#endif +#ifdef ADC_CHANNEL_4 + case 4: + channel = ADC_CHANNEL_4; + break; +#endif +#ifdef ADC_CHANNEL_5 + case 5: + channel = ADC_CHANNEL_5; + break; +#endif +#ifdef ADC_CHANNEL_6 + case 6: + channel = ADC_CHANNEL_6; + break; +#endif +#ifdef ADC_CHANNEL_7 + case 7: + channel = ADC_CHANNEL_7; + break; +#endif +#ifdef ADC_CHANNEL_8 + case 8: + channel = ADC_CHANNEL_8; + break; +#endif +#ifdef ADC_CHANNEL_9 + case 9: + channel = ADC_CHANNEL_9; + break; +#endif +#ifdef ADC_CHANNEL_10 + case 10: + channel = ADC_CHANNEL_10; + break; +#endif +#ifdef ADC_CHANNEL_11 + case 11: + channel = ADC_CHANNEL_11; + break; +#endif +#ifdef ADC_CHANNEL_12 + case 12: + channel = ADC_CHANNEL_12; + break; +#endif +#ifdef ADC_CHANNEL_13 + case 13: + channel = ADC_CHANNEL_13; + break; +#endif +#ifdef ADC_CHANNEL_14 + case 14: + channel = ADC_CHANNEL_14; + break; +#endif +#ifdef ADC_CHANNEL_15 + 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; +#endif +#ifdef ADC_CHANNEL_21 + case 21: + channel = ADC_CHANNEL_21; + break; +#endif +#ifdef ADC_CHANNEL_22 + case 22: + channel = ADC_CHANNEL_22; + break; +#endif +#ifdef ADC_CHANNEL_23 + case 23: + channel = ADC_CHANNEL_23; + break; +#ifdef ADC_CHANNEL_24 + case 24: + channel = ADC_CHANNEL_24; + break; +#endif +#ifdef ADC_CHANNEL_25 + case 25: + channel = ADC_CHANNEL_25; + break; +#endif +#ifdef ADC_CHANNEL_26 + case 26: + channel = ADC_CHANNEL_26; + break; +#ifdef ADC_CHANNEL_27 + case 27: + channel = ADC_CHANNEL_27; + break; +#endif +#ifdef ADC_CHANNEL_28 + case 28: + channel = ADC_CHANNEL_28; + break; +#endif +#ifdef ADC_CHANNEL_29 + case 29: + channel = ADC_CHANNEL_29; + break; +#endif +#ifdef ADC_CHANNEL_30 + case 30: + channel = ADC_CHANNEL_30; + break; +#endif +#ifdef ADC_CHANNEL_31 + case 31: + channel = ADC_CHANNEL_31; + break; +#endif +#endif +#endif +#endif + default: + _Error_Handler("ADC: Unknown adc channel", (int)(STM_PIN_CHANNEL(function))); + break; + } + return channel; +} + +/** + * @brief Return ADC HAL channel linked to a PinName and the ADC handle + * @param pin: PinName + * @param AdcHandle: ADC_HandleTypeDef a pointer to the ADC handle + * @retval Valid HAL channel + */ +uint32_t _getADCChannel(PinName pin, ADC_TypeDef *AdcHandle ) +{ + if (AdcHandle == NP) { + return _getADCChannelFromPinMap(pin); + } + // find the PinName that corresponds to the ADC + int first_ind = _findIndexOfFirstEntry(pin); + int last_ind = _findIndexOfLastEntry(pin); + if (first_ind == -1 || last_ind == -1) { + _Error_Handler("ADC: Pin not found in PinMap_ADC", (int)pin); + } + // find the channel + uint32_t channel = 0; + for (int i = first_ind; i <= last_ind; i++) { + if (PinMap_ADC[i].peripheral == AdcHandle) { + channel =_getADCChannelFromPinMap(PinMap_ADC[i].pin); + SIMPLEFOC_DEBUG("STM32-CS: ADC channel: ", (int)STM_PIN_CHANNEL(pinmap_function(PinMap_ADC[i].pin, PinMap_ADC))); + break; + } + } + return channel; +} + +uint32_t _getADCInjectedRank(uint8_t ind){ + switch (ind) { + #ifdef ADC_INJECTED_RANK_1 + case 0: + return ADC_INJECTED_RANK_1; + break; +#endif +#ifdef ADC_INJECTED_RANK_2 + case 1: + return ADC_INJECTED_RANK_2; + break; +#endif +#ifdef ADC_INJECTED_RANK_3 + case 2: + return ADC_INJECTED_RANK_3; + break; +#endif +#ifdef ADC_INJECTED_RANK_4 + case 3: + return ADC_INJECTED_RANK_4; + break; +#endif + default: + return 0; + break; + } +} + +// returns 0 if no interrupt is needed, 1 if interrupt is needed +uint32_t _initTimerInterruptDownsampling(Stm32CurrentSenseParams* cs_params, STM32DriverParams* driver_params, Stm32AdcInterruptConfig& adc_interrupt_config){ + + // If DIR is 0 (upcounting), the next event is high-side active (PWM rising edge) + // If DIR is 1 (downcounting), the next event is low-side active (PWM falling edge) + bool next_event_high_side = (cs_params->timer_handle->Instance->CR1 & TIM_CR1_DIR) == 0; + + // 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->Instance) ){ + // adjust the initial timer state such that the trigger + // - only necessary for the timers that have repetition counters + // - basically make sure that the next trigger event is the one that is expected (high-side first then low-side) + + // set the direction and the + for(int i=0; i< 6; i++){ + if(driver_params->timers_handle[i] == NP) continue; // skip if not set + if(next_event_high_side){ + // Set DIR bit to 0 (downcounting) + driver_params->timers_handle[i]->Instance->CR1 |= TIM_CR1_DIR; + // Set CNT to ARR so it starts upcounting from the top + driver_params->timers_handle[i]->Instance->CNT = driver_params->timers_handle[i]->Instance->ARR; + }else{ + // Set DIR bit to 0 (upcounting) + driver_params->timers_handle[i]->Instance->CR1 &= ~TIM_CR1_DIR; + // Set CNT to ARR so it starts upcounting from zero + driver_params->timers_handle[i]->Instance->CNT = 0;// driver_params->timers_handle[i]->Instance->ARR; + } + } + return 0; // no interrupt is needed, the timer will handle the downsampling + }else{ + if(!adc_interrupt_config.use_adc_interrupt){ + // If the timer has no repetition counter, it needs to use the interrupt to downsample for low side sensing + adc_interrupt_config.use_adc_interrupt = 1; + // remember that this timer does not have the repetition counter - need to downasmple + adc_interrupt_config.needs_downsample = 1; + + if(next_event_high_side) // Next event is high-side active + adc_interrupt_config.tim_downsample = 0; // skip the next interrupt (and every second one) + else // Next event is low-side active + adc_interrupt_config.tim_downsample = 1; // read the next one (and every second one after) + + return 1; // interrupt is needed + } + } + return 1; // interrupt is needed +} + +// returns 0 if no downsampling is needed, 1 if downsampling is needed, 2 if error +uint8_t _handleInjectedConvCpltCallback(ADC_HandleTypeDef *AdcHandle, Stm32AdcInterruptConfig& adc_interrupt_config, uint32_t adc_val[4]) { + #ifndef HAL_ADCEx_InjectedGetValue + return 0; // error: function not available + #else + + // if the timer han't repetition counter - downsample two times + if( adc_interrupt_config.needs_downsample && adc_interrupt_config.tim_downsample++ > 0) { + adc_interrupt_config.tim_downsample = 0; + return 1; + } + + adc_val[0]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_1); + adc_val[1]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_2); + adc_val[2]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_3); + adc_val[3]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_4); + + return 0; // no downsampling needed + #endif +} + +// reads the ADC injected voltage for the given pin +// returns the voltage +// if the pin is not found in the current sense parameters, returns 0 +float _readADCInjectedChannelVoltage(int pin, void* cs_params, Stm32AdcInterruptConfig& adc_interrupt_config, uint32_t adc_val[4]) { + #ifndef HAL_ADCEx_InjectedGetValue + return 0; // error: function not available + #else + + Stm32CurrentSenseParams* cs_p = (Stm32CurrentSenseParams*)cs_params; + uint8_t channel_no = 0; + uint8_t adc_index = (uint8_t)_adcToIndex(cs_p->adc_handle); + for(int i=0; i < 3; i++){ + if( pin == cs_p->pins[i]){ // found in the buffer + if (adc_interrupt_config.use_adc_interrupt){ + return adc_val[channel_no] * cs_p->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 = _getADCInjectedRank(channel_no); + return HAL_ADCEx_InjectedGetValue(cs_p->adc_handle, channel) * cs_p->adc_voltage_conv; + } + } + if(_isset(cs_p->pins[i])) channel_no++; + } + return 0; // pin not found + #endif +} + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32_adc_utils.h b/src/current_sense/hardware_specific/stm32/stm32_adc_utils.h new file mode 100644 index 00000000..1294e8f1 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32_adc_utils.h @@ -0,0 +1,65 @@ +#ifndef STM32_ADC_UTILS_HAL +#define STM32_ADC_UTILS_HAL + +#include "Arduino.h" + +#if defined(_STM32_DEF_) + +#define _TRGO_NOT_AVAILABLE 12345 + +// for searching the best ADCs, we need to know the number of ADCs +// it might be better to use some HAL variable for example ADC_COUNT +// here I've assumed the maximum number of ADCs is 5 +#define ADC_COUNT 5 + + +#include "../../../common/foc_utils.h" +#include "../../../communication/SimpleFOCDebug.h" +#include "../../../drivers/hardware_specific/stm32/stm32_mcu.h" +#include "stm32_mcu.h" + + + +/* Exported Functions */ +/** + * @brief Return ADC HAL channel linked to a PinName + * @param pin: PinName + * @param adc: ADC_TypeDef a pointer to the ADC handle + * @retval Valid HAL channel + */ +uint32_t _getADCChannel(PinName pin, ADC_TypeDef* adc = NP); +uint32_t _getADCInjectedRank(uint8_t ind); + +// timer to injected TRGO - architecure specific +uint32_t _timerToInjectedTRGO(TIM_HandleTypeDef* timer); + +// timer to regular TRGO - architecure specific +uint32_t _timerToRegularTRGO(TIM_HandleTypeDef* timer); + +// function returning index of the ADC instance +int _adcToIndex(ADC_HandleTypeDef *AdcHandle); +int _adcToIndex(ADC_TypeDef *AdcHandle); + +// functions helping to find the best ADC channel +int _findIndexOfFirstPinMapADCEntry(int pin); +int _findIndexOfLastPinMapADCEntry(int pin); +ADC_TypeDef* _findBestADCForPins(int num_pins, int pins[]); + + +// Structure to hold ADC interrupt configuration per ADC instance +struct Stm32AdcInterruptConfig { + bool needs_downsample = 0; + uint8_t tim_downsample = 0; + bool use_adc_interrupt = 0; +}; + +// returns 0 if no interrupt is needed, 1 if interrupt is needed +uint32_t _initTimerInterruptDownsampling(Stm32CurrentSenseParams* cs_params, STM32DriverParams* driver_params, Stm32AdcInterruptConfig& adc_interrupt_config); +// returns 0 if no downsampling is needed, 1 if downsampling is needed, 2 if error +uint8_t _handleInjectedConvCpltCallback(ADC_HandleTypeDef *AdcHandle, Stm32AdcInterruptConfig& adc_interrupt_config, uint32_t adc_val[4]); +// reads the ADC injected voltage for the given pin +// returns the voltage +// if the pin is not found in the current sense parameters, returns 0 +float _readADCInjectedChannelVoltage(int pin, void* cs_params, Stm32AdcInterruptConfig& adc_interrupt_config, uint32_t adc_val[4]); +#endif +#endif diff --git a/src/current_sense/hardware_specific/stm32/stm32_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32_mcu.cpp index 94253d74..efc55733 100644 --- a/src/current_sense/hardware_specific/stm32/stm32_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32_mcu.cpp @@ -30,4 +30,5 @@ __attribute__((weak)) float _readADCVoltageInline(const int pinA, const void* c return raw_adc * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; } + #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32_mcu.h b/src/current_sense/hardware_specific/stm32/stm32_mcu.h index 6e238170..564598d3 100644 --- a/src/current_sense/hardware_specific/stm32/stm32_mcu.h +++ b/src/current_sense/hardware_specific/stm32/stm32_mcu.h @@ -3,6 +3,8 @@ #define STM32_CURRENTSENSE_MCU_DEF #include "../../hardware_api.h" #include "../../../common/foc_utils.h" +#include "../../../drivers/hardware_specific/stm32/stm32_mcu.h" +#include "../../../drivers/hardware_specific/stm32/stm32_timerutils.h" #if defined(_STM32_DEF_) @@ -14,8 +16,9 @@ typedef struct Stm32CurrentSenseParams { int pins[3] = {(int)NOT_SET}; float adc_voltage_conv; ADC_HandleTypeDef* adc_handle = NP; - HardwareTimer* timer_handle = NP; + TIM_HandleTypeDef* timer_handle = NP; } Stm32CurrentSenseParams; + #endif #endif \ No newline at end of file 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 ec75ef4f..5378fb1a 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp @@ -3,51 +3,27 @@ #if defined(STM32F1xx) #include "../../../../communication/SimpleFOCDebug.h" +#include "../stm32_adc_utils.h" #define _TRGO_NOT_AVAILABLE 12345 -// timer to injected TRGO -// https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32F1xx_HAL_Driver/Inc/stm32f1xx_hal_adc_ex.h#L215 -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 - else - return _TRGO_NOT_AVAILABLE; -} - -// timer to regular TRGO -// https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32F1xx_HAL_Driver/Inc/stm32f1xx_hal_adc_ex.h#L215 -uint32_t _timerToRegularTRGO(HardwareTimer* timer){ - if(timer->getHandle()->Instance == TIM3) - return ADC_EXTERNALTRIGCONV_T3_TRGO; -#ifdef TIM8 // if defined timer 8 - else if(timer->getHandle()->Instance == TIM8) - return ADC_EXTERNALTRIGCONV_T8_TRGO; -#endif - else - return _TRGO_NOT_AVAILABLE; -} - ADC_HandleTypeDef hadc; +/** + * Function initializing the ADC and the injected channels for the low-side current sensing + * + * @param cs_params - current sense parameters + * @param driver_params - driver parameters + * + * @return int - 0 if success + */ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params) { ADC_InjectionConfTypeDef sConfigInjected; /**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); + hadc.Instance = _findBestADCForPins(3, cs_params->pins); + if(hadc.Instance == ADC1) __HAL_RCC_ADC1_CLK_ENABLE(); #ifdef ADC2 // if defined ADC2 else if(hadc.Instance == ADC2) __HAL_RCC_ADC2_CLK_ENABLE(); @@ -67,14 +43,19 @@ 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. */ /**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.InjectedNbrOfConversion = 0; + for(int pin_no=0; pin_no<3; pin_no++){ + if(_isset(cs_params->pins[pin_no])){ + sConfigInjected.InjectedNbrOfConversion++; + } + } sConfigInjected.InjectedSamplingTime = ADC_SAMPLETIME_1CYCLE_5; sConfigInjected.AutoInjectedConv = DISABLE; sConfigInjected.InjectedDiscontinuousConvMode = DISABLE; @@ -82,16 +63,23 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive // automating TRGO flag finding - hardware specific uint8_t tim_num = 0; - while(driver_params->timers[tim_num] != NP && tim_num < 6){ - uint32_t trigger_flag = _timerToInjectedTRGO(driver_params->timers[tim_num++]); + while(driver_params->timers_handle[tim_num] != NP && tim_num < 6){ + uint32_t trigger_flag = _timerToInjectedTRGO(driver_params->timers_handle[tim_num++]); if(trigger_flag == _TRGO_NOT_AVAILABLE) continue; // timer does not have valid trgo for injected channels + // check if TRGO used already - if yes use the next timer + if((driver_params->timers_handle[tim_num-1]->Instance->CR2 & LL_TIM_TRGO_ENABLE) || // if used for timer sync + (driver_params->timers_handle[tim_num-1]->Instance->CR2 & LL_TIM_TRGO_UPDATE)) // if used for ADC sync + { + continue; + } + // 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 = driver_params->timers[tim_num-1]; + cs_params->timer_handle = driver_params->timers_handle[tim_num-1]; // done break; } @@ -101,66 +89,68 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot sync any timer to injected channels!"); #endif return -1; - } - // display which timer is being used + }else{ #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); + SIMPLEFOC_DEBUG("STM32-CS: Using timer: ", stm32_getTimerNumber(cs_params->timer_handle->Instance)); #endif + } - // first channel - sConfigInjected.InjectedRank = ADC_REGULAR_RANK_1; - sConfigInjected.InjectedChannel = STM_PIN_CHANNEL(pinmap_function(analogInputToPinName(cs_params->pins[0]), PinMap_ADC)); - HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected); - // second channel - sConfigInjected.InjectedRank = ADC_REGULAR_RANK_2; - sConfigInjected.InjectedChannel = STM_PIN_CHANNEL(pinmap_function(analogInputToPinName(cs_params->pins[1]), PinMap_ADC)); - HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected); - // third channel - if exists - if(_isset(cs_params->pins[2])){ - sConfigInjected.InjectedRank = ADC_REGULAR_RANK_3; - sConfigInjected.InjectedChannel = STM_PIN_CHANNEL(pinmap_function(analogInputToPinName(cs_params->pins[2]), PinMap_ADC)); - HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected); - } - - #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT - // enable interrupt - HAL_NVIC_SetPriority(ADC1_2_IRQn, 0, 0); - HAL_NVIC_EnableIRQ(ADC1_2_IRQn); + uint8_t channel_no = 0; + for(int i=0; i<3; i++){ + // skip if not set + if (!_isset(cs_params->pins[i])) continue; + + sConfigInjected.InjectedRank = _getADCInjectedRank(channel_no++); + sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[i]), hadc.Instance); + 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[i]) , hadc.Instance)); #endif - + return -1; + } + } cs_params->adc_handle = &hadc; - return 0; } -void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC) +/** + * Function to initialize the ADC GPIO pins + * + * @param cs_params current sense parameters + * @param pinA pin number for phase A + * @param pinB pin number for phase B + * @param pinC pin number for phase C + * @return int 0 if success, -1 if error + */ +int _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; + int pins[3] = {pinA, pinB, pinC}; + const char* port_names[3] = {"A", "B", "C"}; + for(int i=0; i<3; i++){ + if(_isset(pins[i])){ + // check if pin is an analog pin + if(pinmap_peripheral(analogInputToPinName(pins[i]), PinMap_ADC) == NP){ +#ifdef SIMPLEFOC_STM32_DEBUG + SimpleFOCDebug::print("STM32-CS: ERR: Pin "); + SimpleFOCDebug::print(port_names[i]); + SimpleFOCDebug::println(" does not belong to any ADC!"); +#endif + return -1; + } + pinmap_pinout(analogInputToPinName(pins[i]), PinMap_ADC); + cs_params->pins[i] = pins[i]; + } } - + return 0; } -#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT + extern "C" { void ADC1_2_IRQHandler(void) { HAL_ADC_IRQHandler(&hadc); } - } -#endif #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.h b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.h index b0f4f83b..fbcf4e99 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.h +++ b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.h @@ -5,12 +5,10 @@ #if defined(STM32F1xx) #include "stm32f1xx_hal.h" -#include "../../../../common/foc_utils.h" -#include "../../../../drivers/hardware_specific/stm32/stm32_mcu.h" #include "../stm32_mcu.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); +int _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/stm32f1/stm32f1_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp index 69fea017..7b0e9024 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp @@ -5,6 +5,7 @@ #include "../../../../drivers/hardware_api.h" #include "../../../../drivers/hardware_specific/stm32/stm32_mcu.h" #include "../../../hardware_api.h" +#include "../stm32_adc_utils.h" #include "../stm32_mcu.h" #include "stm32f1_hal.h" #include "Arduino.h" @@ -12,23 +13,24 @@ #define _ADC_VOLTAGE_F1 3.3f #define _ADC_RESOLUTION_F1 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] = {0}; - -int _adcToIndex(ADC_HandleTypeDef *AdcHandle){ - if(AdcHandle->Instance == ADC1) return 0; -#ifdef ADC2 // if ADC2 exists - else if(AdcHandle->Instance == ADC2) return 1; -#endif -#ifdef ADC3 // if ADC3 exists - else if(AdcHandle->Instance == ADC3) return 2; +// array of values of 4 injected channels per adc instance (5) +uint32_t adc_val[5][4]={0}; + +#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT +#define USE_ADC_INTERRUPT 1 +#else +#define USE_ADC_INTERRUPT 0 #endif - return 0; -} + +// structure containing the configuration of the adc interrupt +Stm32AdcInterruptConfig adc_interrupt_config[5] = { + {0, 0, USE_ADC_INTERRUPT}, // ADC1 + {0, 0, USE_ADC_INTERRUPT}, // ADC2 + {0, 0, USE_ADC_INTERRUPT}, // ADC3 + {0, 0, USE_ADC_INTERRUPT}, // ADC4 + {0, 0, USE_ADC_INTERRUPT} // ADC5 +}; + void* _configureADCLowSide(const void* driver_params, const int pinA, const int pinB, const int pinC){ @@ -36,85 +38,70 @@ void* _configureADCLowSide(const void* driver_params, const int pinA, const int .pins={(int)NOT_SET,(int)NOT_SET,(int)NOT_SET}, .adc_voltage_conv = (_ADC_VOLTAGE_F1) / (_ADC_RESOLUTION_F1) }; - _adc_gpio_init(cs_params, pinA,pinB,pinC); + if(_adc_gpio_init(cs_params, pinA,pinB,pinC) != 0) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; 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){ +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; + if (cs_params->timer_handle == NULL) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; // 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; + stm32_pause(driver_params); + + // get the index of the adc + int adc_index = _adcToIndex(cs_params->adc_handle); + + bool tim_interrupt = _initTimerInterruptDownsampling(cs_params, driver_params, adc_interrupt_config[adc_index]); + if(tim_interrupt) { + // error in the timer interrupt initialization + SIMPLEFOC_DEBUG("STM32-CS: timer has no repetition counter, ADC interrupt has to be used"); } + // set the trigger output event - LL_TIM_SetTriggerOutput(cs_params->timer_handle->getHandle()->Instance, LL_TIM_TRGO_UPDATE); + LL_TIM_SetTriggerOutput(cs_params->timer_handle->Instance, LL_TIM_TRGO_UPDATE); // Start the adc calibration HAL_ADCEx_Calibration_Start(cs_params->adc_handle); - + // 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 + if(adc_interrupt_config[adc_index].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); + stm32_resume(driver_params); + + // return the cs parameters + // successfully initialized + // TODO verify if success in future + return _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 - #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; + uint8_t adc_index = (uint8_t)_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle); + return _readADCInjectedChannelVoltage(pin, (void*)cs_params, adc_interrupt_config[adc_index], adc_val[adc_index]); } -#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); + uint8_t adc_index = (uint8_t)_adcToIndex(AdcHandle); + _handleInjectedConvCpltCallback(AdcHandle, adc_interrupt_config[adc_index], adc_val[adc_index]); } } -#endif + #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_utils.cpp b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_utils.cpp new file mode 100644 index 00000000..8cd6eb96 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_utils.cpp @@ -0,0 +1,40 @@ +#include "../stm32_adc_utils.h" + +#if defined(STM32F1xx) + + +// timer to injected TRGO +// https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32F1xx_HAL_Driver/Inc/stm32f1xx_hal_adc_ex.h#L215 +uint32_t _timerToInjectedTRGO(TIM_HandleTypeDef* timer){ + if(timer->Instance == TIM1) + return ADC_EXTERNALTRIGINJECCONV_T1_TRGO; +#ifdef TIM2 // if defined timer 2 + else if(timer->Instance == TIM2) + return ADC_EXTERNALTRIGINJECCONV_T2_TRGO; +#endif +#ifdef TIM4 // if defined timer 4 + else if(timer->Instance == TIM4) + return ADC_EXTERNALTRIGINJECCONV_T4_TRGO; +#endif +#ifdef TIM5 // if defined timer 5 + else if(timer->Instance == TIM5) + return ADC_EXTERNALTRIGINJECCONV_T5_TRGO; +#endif + else + return _TRGO_NOT_AVAILABLE; +} + +// timer to regular TRGO +// https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32F1xx_HAL_Driver/Inc/stm32f1xx_hal_adc_ex.h#L215 +uint32_t _timerToRegularTRGO(TIM_HandleTypeDef* timer){ + if(timer->Instance == TIM3) + return ADC_EXTERNALTRIGCONV_T3_TRGO; +#ifdef TIM8 // if defined timer 8 + else if(timer->Instance == TIM8) + return ADC_EXTERNALTRIGCONV_T8_TRGO; +#endif + else + return _TRGO_NOT_AVAILABLE; +} + +#endif \ No newline at end of file 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 dcf32137..e6ee1cbc 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp @@ -9,25 +9,21 @@ ADC_HandleTypeDef hadc; +/** + * Function initializing the ADC and the injected channels for the low-side current sensing + * + * @param cs_params - current sense parameters + * @param driver_params - driver parameters + * + * @return int - 0 if success + */ 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); + hadc.Instance = _findBestADCForPins(3, cs_params->pins); if(hadc.Instance == ADC1) __HAL_RCC_ADC1_CLK_ENABLE(); #ifdef ADC2 // if defined ADC2 @@ -67,7 +63,12 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive /**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.InjectedNbrOfConversion = 0; + for(int pin_no=0; pin_no<3; pin_no++){ + if(_isset(cs_params->pins[pin_no])){ + sConfigInjected.InjectedNbrOfConversion++; + } + } sConfigInjected.InjectedSamplingTime = ADC_SAMPLETIME_3CYCLES; sConfigInjected.ExternalTrigInjecConvEdge = ADC_EXTERNALTRIGINJECCONVEDGE_RISING; sConfigInjected.AutoInjectedConv = DISABLE; @@ -76,16 +77,23 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive // automating TRGO flag finding - hardware specific uint8_t tim_num = 0; - while(driver_params->timers[tim_num] != NP && tim_num < 6){ - uint32_t trigger_flag = _timerToInjectedTRGO(driver_params->timers[tim_num++]); + while(driver_params->timers_handle[tim_num] != NP && tim_num < 6){ + uint32_t trigger_flag = _timerToInjectedTRGO(driver_params->timers_handle[tim_num++]); if(trigger_flag == _TRGO_NOT_AVAILABLE) continue; // timer does not have valid trgo for injected channels + // check if TRGO used already - if yes use the next timer + if((driver_params->timers_handle[tim_num-1]->Instance->CR2 & LL_TIM_TRGO_ENABLE) || // if used for timer sync + (driver_params->timers_handle[tim_num-1]->Instance->CR2 & LL_TIM_TRGO_UPDATE)) // if used for ADC sync + { + continue; + } + // 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 = driver_params->timers[tim_num-1]; + cs_params->timer_handle = driver_params->timers_handle[tim_num-1]; // done break; } @@ -95,80 +103,65 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive 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){ + }else{ #ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[1]))) ; + SIMPLEFOC_DEBUG("STM32-CS: Using timer: ", stm32_getTimerNumber(cs_params->timer_handle->Instance)); #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])); + + uint8_t channel_no = 0; + for(int i=0; i<3; i++){ + // skip if not set + if (!_isset(cs_params->pins[i])) continue; + + sConfigInjected.InjectedRank = _getADCInjectedRank(channel_no++); + sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[i]), hadc.Instance); 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 + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[i]) , hadc.Instance)); + #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) +/** + * Function to initialize the ADC GPIO pins + * + * @param cs_params current sense parameters + * @param pinA pin number for phase A + * @param pinB pin number for phase B + * @param pinC pin number for phase C + * @return int 0 if success, -1 if error + */ +int _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; + int pins[3] = {pinA, pinB, pinC}; + for(int i=0; i<3; i++){ + if(_isset(pins[i])){ + // check if pin is an analog pin + if(pinmap_peripheral(analogInputToPinName(pins[i]), PinMap_ADC) == NP){ +#ifdef SIMPLEFOC_STM32_DEBUG + SimpleFOCDebug::print("STM32-CS: ERR: Pin "); + SimpleFOCDebug::print(i == 0 ? "A" : i == 1 ? "B" : "C"); + SimpleFOCDebug::println(" does not belong to any ADC!"); +#endif + return -1; + } + pinmap_pinout(analogInputToPinName(pins[i]), PinMap_ADC); + cs_params->pins[i] = pins[i]; + } } + return 0; } -#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/stm32f4/stm32f4_hal.h b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.h index 71071a56..f0f9a03d 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.h +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.h @@ -5,13 +5,11 @@ #if defined(STM32F4xx) #include "stm32f4xx_hal.h" -#include "../../../../common/foc_utils.h" -#include "../../../../drivers/hardware_specific/stm32/stm32_mcu.h" #include "../stm32_mcu.h" -#include "stm32f4_utils.h" +#include "../stm32_adc_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); +int _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/stm32f4/stm32f4_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp index a3ee5407..7bace501 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp @@ -6,21 +6,31 @@ #include "../../../../drivers/hardware_specific/stm32/stm32_mcu.h" #include "../../../hardware_api.h" #include "../stm32_mcu.h" +#include "../stm32_adc_utils.h" #include "stm32f4_hal.h" -#include "stm32f4_utils.h" #include "Arduino.h" #define _ADC_VOLTAGE_F4 3.3f #define _ADC_RESOLUTION_F4 4096.0f +#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT +#define USE_ADC_INTERRUPT 1 +#else +#define USE_ADC_INTERRUPT 0 +#endif + +// array of values of 4 injected channels per adc instance (5) +uint32_t adc_val[5][4]={0}; -// 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] = {0}; +// structure containing the configuration of the adc interrupt +Stm32AdcInterruptConfig adc_interrupt_config[5] = { + {0, 0, USE_ADC_INTERRUPT}, // ADC1 + {0, 0, USE_ADC_INTERRUPT}, // ADC2 + {0, 0, USE_ADC_INTERRUPT}, // ADC3 + {0, 0, USE_ADC_INTERRUPT}, // ADC4 + {0, 0, USE_ADC_INTERRUPT} // ADC5 +}; void* _configureADCLowSide(const void* driver_params, const int pinA, const int pinB, const int pinC){ @@ -28,82 +38,66 @@ void* _configureADCLowSide(const void* driver_params, const int pinA, const int .pins={(int)NOT_SET,(int)NOT_SET,(int)NOT_SET}, .adc_voltage_conv = (_ADC_VOLTAGE_F4) / (_ADC_RESOLUTION_F4) }; - _adc_gpio_init(cs_params, pinA,pinB,pinC); + if(_adc_gpio_init(cs_params, pinA,pinB,pinC) != 0) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; 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){ +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; + if (cs_params->timer_handle == NULL) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; // 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; + stm32_pause(driver_params); + + // get the index of the adc + int adc_index = _adcToIndex(cs_params->adc_handle); + + bool tim_interrupt = _initTimerInterruptDownsampling(cs_params, driver_params, adc_interrupt_config[adc_index]); + if(tim_interrupt) { + // error in the timer interrupt initialization + SIMPLEFOC_DEBUG("STM32-CS: timer has no repetition counter, ADC interrupt has to be used"); } + // set the trigger output event - LL_TIM_SetTriggerOutput(cs_params->timer_handle->getHandle()->Instance, LL_TIM_TRGO_UPDATE); + LL_TIM_SetTriggerOutput(cs_params->timer_handle->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 + if (adc_interrupt_config[adc_index].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); + stm32_resume(driver_params); + + // return the cs parameters + // successfully initialized + // TODO verify if success in future + return _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 - #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; + uint8_t adc_index = (uint8_t)_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle); + return _readADCInjectedChannelVoltage(pin, (void*)cs_params, adc_interrupt_config[adc_index], adc_val[adc_index]); } -#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); + uint8_t adc_index = (uint8_t)_adcToIndex(AdcHandle); + _handleInjectedConvCpltCallback(AdcHandle, adc_interrupt_config[adc_index], adc_val[adc_index]); } } -#endif #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_utils.cpp b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_utils.cpp index 20793d8c..22a54f85 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_utils.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_utils.cpp @@ -1,151 +1,22 @@ -#include "stm32f4_utils.h" +#include "../stm32_adc_utils.h" #if defined(STM32F4xx) -/* Exported Functions */ -/** - * @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; -} - - // 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) +uint32_t _timerToInjectedTRGO(TIM_HandleTypeDef* timer){ + if(timer->Instance == TIM1) return ADC_EXTERNALTRIGINJECCONV_T1_TRGO; #ifdef TIM2 // if defined timer 2 - else if(timer->getHandle()->Instance == TIM2) + else if(timer->Instance == TIM2) return ADC_EXTERNALTRIGINJECCONV_T2_TRGO; #endif #ifdef TIM4 // if defined timer 4 - else if(timer->getHandle()->Instance == TIM4) + else if(timer->Instance == TIM4) return ADC_EXTERNALTRIGINJECCONV_T4_TRGO; #endif #ifdef TIM5 // if defined timer 5 - else if(timer->getHandle()->Instance == TIM5) + else if(timer->Instance == TIM5) return ADC_EXTERNALTRIGINJECCONV_T5_TRGO; #endif else @@ -154,40 +25,20 @@ 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){ - if(timer->getHandle()->Instance == TIM2) +uint32_t _timerToRegularTRGO(TIM_HandleTypeDef* timer){ +#ifdef TIM2 // if defined timer 2 + if(timer->Instance == TIM2) return ADC_EXTERNALTRIGCONV_T2_TRGO; +#endif #ifdef TIM3 // if defined timer 3 - else if(timer->getHandle()->Instance == TIM3) + if(timer->Instance == TIM3) return ADC_EXTERNALTRIGCONV_T3_TRGO; #endif #ifdef TIM8 // if defined timer 8 - else if(timer->getHandle()->Instance == TIM8) + if(timer->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); + return _TRGO_NOT_AVAILABLE; } #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_utils.h b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_utils.h deleted file mode 100644 index b4549bad..00000000 --- a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_utils.h +++ /dev/null @@ -1,34 +0,0 @@ - -#ifndef STM32F4_UTILS_HAL -#define STM32F4_UTILS_HAL - -#include "Arduino.h" - -#if defined(STM32F4xx) - -#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 - -#endif \ No newline at end of file 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..595eb2a4 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.cpp @@ -0,0 +1,178 @@ +#include "stm32f7_hal.h" + +#if defined(STM32F7xx) + +//#define SIMPLEFOC_STM32_DEBUG + +#include "../../../../communication/SimpleFOCDebug.h" +#define _TRGO_NOT_AVAILABLE 12345 + +ADC_HandleTypeDef hadc; + +/** + * Function initializing the ADC and the injected channels for the low-side current sensing + * + * @param cs_params - current sense parameters + * @param driver_params - driver parameters + * + * @return int - 0 if success + */ +int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params) +{ + ADC_InjectionConfTypeDef sConfigInjected; + + /**Configure the global features of the ADC (Clock, Resolution, Data Alignment and number of conversion) + */ + hadc.Instance = _findBestADCForPins(3, cs_params->pins); + + 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 = 0; + for(int pin_no=0; pin_no<3; pin_no++){ + if(_isset(cs_params->pins[pin_no])){ + sConfigInjected.InjectedNbrOfConversion++; + } + } + 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++) { + TIM_HandleTypeDef *timer_to_check = driver_params->timers_handle[tim_num++]; + TIM_TypeDef *instance_to_check = timer_to_check->Instance; + + uint32_t trigger_flag = _timerToInjectedTRGO(timer_to_check); + if(trigger_flag == _TRGO_NOT_AVAILABLE) continue; // timer does not have valid trgo for injected channels + + // check if TRGO used already - if yes use the next timer + if((timer_to_check->Instance->CR2 & LL_TIM_TRGO_ENABLE) || // if used for timer sync + (timer_to_check->Instance->CR2 & LL_TIM_TRGO_UPDATE)) // if used for ADC sync + { + continue; + } + + // 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; + }else{ +#ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: Using timer: ", stm32_getTimerNumber(cs_params->timer_handle->Instance)); +#endif + } + + uint8_t channel_no = 0; + for(int i=0; i<3; i++){ + // skip if not set + if (!_isset(cs_params->pins[i])) continue; + + sConfigInjected.InjectedRank = _getADCInjectedRank(channel_no++); + sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[i]), hadc.Instance); + 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[i]) , hadc.Instance)); + #endif + return -1; + } + } + + cs_params->adc_handle = &hadc; + return 0; +} + + +/** + * Function to initialize the ADC GPIO pins + * + * @param cs_params current sense parameters + * @param pinA pin number for phase A + * @param pinB pin number for phase B + * @param pinC pin number for phase C + * @return int 0 if success, -1 if error + */ +int _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC) +{ + int pins[3] = {pinA, pinB, pinC}; + const char* port_names[3] = {"A", "B", "C"}; + for(int i=0; i<3; i++){ + if(_isset(pins[i])){ + // check if pin is an analog pin + if(pinmap_peripheral(analogInputToPinName(pins[i]), PinMap_ADC) == NP){ +#ifdef SIMPLEFOC_STM32_DEBUG + SimpleFOCDebug::print("STM32-CS: ERR: Pin "); + SimpleFOCDebug::print(port_names[i]); + SimpleFOCDebug::println(" does not belong to any ADC!"); +#endif + return -1; + } + pinmap_pinout(analogInputToPinName(pins[i]), PinMap_ADC); + cs_params->pins[i] = pins[i]; + } + } + return 0; +} + +extern "C" { + void ADC_IRQHandler(void) + { + HAL_ADC_IRQHandler(&hadc); + } +} + +#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..391b3fb5 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.h @@ -0,0 +1,17 @@ +#ifndef STM32F7_LOWSIDE_HAL +#define STM32F7_LOWSIDE_HAL + +#include "Arduino.h" + +#if defined(STM32F7xx) + +#include "stm32f7xx_hal.h" +#include "../stm32_mcu.h" +#include "../stm32_adc_utils.h" + +int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params); +int _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC); + +#endif + +#endif \ No newline at end of file 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..d8bac8d8 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_mcu.cpp @@ -0,0 +1,103 @@ +#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 "../stm32_adc_utils.h" +#include "stm32f7_hal.h" +#include "Arduino.h" + + +#define _ADC_VOLTAGE 3.3f +#define _ADC_RESOLUTION 4096.0f + +// array of values of 4 injected channels per adc instance (5) +uint32_t adc_val[5][4]={0}; + +#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT +#define USE_ADC_INTERRUPT 1 +#else +#define USE_ADC_INTERRUPT 0 +#endif + +// structure containing the configuration of the adc interrupt +Stm32AdcInterruptConfig adc_interrupt_config[5] = { + {0, 0, USE_ADC_INTERRUPT}, // ADC1 + {0, 0, USE_ADC_INTERRUPT}, // ADC2 + {0, 0, USE_ADC_INTERRUPT}, // ADC3 + {0, 0, USE_ADC_INTERRUPT}, // ADC4 + {0, 0, USE_ADC_INTERRUPT} // ADC5 +}; + + +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) + }; + if(_adc_gpio_init(cs_params, pinA,pinB,pinC) != 0) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; + 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 SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; + + // stop all the timers for the driver + stm32_pause(driver_params); + + // get the index of the adc + int adc_index = _adcToIndex(cs_params->adc_handle); + + bool tim_interrupt = _initTimerInterruptDownsampling(cs_params, driver_params, adc_interrupt_config[adc_index]); + if(tim_interrupt) { + // error in the timer interrupt initialization + SIMPLEFOC_DEBUG("STM32-CS: timer has no repetition counter, ADC interrupt has to be used"); + } + + // set the trigger output event + LL_TIM_SetTriggerOutput(cs_params->timer_handle->Instance, LL_TIM_TRGO_UPDATE); + + // start the adc + if (adc_interrupt_config[adc_index].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 + stm32_resume(driver_params); + + // return the cs parameters + // successfully initialized + // TODO verify if success in future + return _cs_params; +} + + +// function reading an ADC value and returning the read voltage +float _readADCVoltageLowSide(const int pin, const void* cs_params){ + uint8_t adc_index = (uint8_t)_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle); + return _readADCInjectedChannelVoltage(pin, (void*)cs_params, adc_interrupt_config[adc_index], adc_val[adc_index]); +} + +extern "C" { + void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef *AdcHandle){ + uint8_t adc_index = (uint8_t)_adcToIndex(AdcHandle); + _handleInjectedConvCpltCallback(AdcHandle, adc_interrupt_config[adc_index], adc_val[adc_index]); + } +} + +#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..fbd20d40 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_utils.cpp @@ -0,0 +1,96 @@ +#include "../stm32_adc_utils.h" + +#if defined(STM32F7xx) + +/* +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(TIM_HandleTypeDef* timer){ + + if(timer->Instance == TIM1) + return ADC_EXTERNALTRIGINJECCONV_T1_TRGO; +#ifdef TIM2 // if defined timer 2 + else if(timer->Instance == TIM2) + return ADC_EXTERNALTRIGINJECCONV_T2_TRGO; +#endif +#ifdef TIM4 // if defined timer 4 + else if(timer->Instance == TIM4) + return ADC_EXTERNALTRIGINJECCONV_T4_TRGO; +#endif +#ifdef TIM5 // if defined timer 5 + else if(timer->Instance == TIM5) + return ADC_EXTERNALTRIGINJECCONV_T5_TRGO; +#endif +#ifdef TIM6 // if defined timer 6 + else if(timer->Instance == TIM6) + return ADC_EXTERNALTRIGINJECCONV_T6_TRGO; +#endif +#ifdef TIM8 // if defined timer 8 + else if(timer->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(TIM_HandleTypeDef* timer){ + if(timer->Instance == TIM1) + return ADC_EXTERNALTRIGCONV_T1_TRGO; +#ifdef TIM2 // if defined timer 2 + else if(timer->Instance == TIM2) + return ADC_EXTERNALTRIGCONV_T2_TRGO; +#endif +#ifdef TIM4 // if defined timer 4 + else if(timer->Instance == TIM4) + return ADC_EXTERNALTRIGCONV_T4_TRGO; +#endif +#ifdef TIM5 // if defined timer 5 + else if(timer->Instance == TIM5) + return ADC_EXTERNALTRIGCONV_T5_TRGO; +#endif +#ifdef TIM6 // if defined timer 6 + else if(timer->Instance == TIM6) + return ADC_EXTERNALTRIGCONV_T6_TRGO; +#endif +#ifdef TIM8 // if defined timer 8 + else if(timer->Instance == TIM8) + return ADC_EXTERNALTRIGCONV_T8_TRGO; +#endif + else + return _TRGO_NOT_AVAILABLE; +} + +#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 bf89463b..df639932 100644 --- a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp @@ -8,25 +8,21 @@ ADC_HandleTypeDef hadc; +/** + * Function initializing the ADC and the injected channels for the low-side current sensing + * + * @param cs_params - current sense parameters + * @param driver_params - driver parameters + * + * @return int - 0 if success + */ 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); + hadc.Instance = _findBestADCForPins(3, cs_params->pins); if(hadc.Instance == ADC1) { #ifdef __HAL_RCC_ADC1_CLK_ENABLE @@ -81,7 +77,7 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive #endif else{ #ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-CS: ERR: Pin does not belong to any ADC!"); + SIMPLEFOC_DEBUG("STM32-CS: ERR: Cannot find a common ADC for the pins!"); #endif return -1; // error not a valid ADC instance } @@ -90,6 +86,7 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive SIMPLEFOC_DEBUG("STM32-CS: Using ADC: ", _adcToIndex(&hadc)+1); #endif + hadc.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV4; hadc.Init.Resolution = ADC_RESOLUTION_12B; hadc.Init.ScanConvMode = ADC_SCAN_ENABLE; @@ -100,7 +97,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; @@ -113,7 +110,12 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive /**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.InjectedNbrOfConversion = 0; + for(int pin_no=0; pin_no<3; pin_no++){ + if(_isset(cs_params->pins[pin_no])){ + sConfigInjected.InjectedNbrOfConversion++; + } + } sConfigInjected.InjectedSamplingTime = ADC_SAMPLETIME_2CYCLES_5; sConfigInjected.ExternalTrigInjecConvEdge = ADC_EXTERNALTRIGINJECCONV_EDGE_RISING; sConfigInjected.AutoInjectedConv = DISABLE; @@ -126,16 +128,23 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive // automating TRGO flag finding - hardware specific uint8_t tim_num = 0; - while(driver_params->timers[tim_num] != NP && tim_num < 6){ - uint32_t trigger_flag = _timerToInjectedTRGO(driver_params->timers[tim_num++]); + while(driver_params->timers_handle[tim_num] != NP && tim_num < 6){ + uint32_t trigger_flag = _timerToInjectedTRGO(driver_params->timers_handle[tim_num++]); if(trigger_flag == _TRGO_NOT_AVAILABLE) continue; // timer does not have valid trgo for injected channels + // check if TRGO used already - if yes use the next timer + if((driver_params->timers_handle[tim_num-1]->Instance->CR2 & LL_TIM_TRGO_ENABLE) || // if used for timer sync + (driver_params->timers_handle[tim_num-1]->Instance->CR2 & LL_TIM_TRGO_UPDATE)) // if used for ADC sync + { + continue; + } + // 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 = driver_params->timers[tim_num-1]; + cs_params->timer_handle = driver_params->timers_handle[tim_num-1]; // done break; } @@ -145,100 +154,62 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot sync any timer to injected channels!"); #endif return -1; - } - - - // 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){ + }else{ #ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[1]))) ; + SIMPLEFOC_DEBUG("STM32-CS: Using timer: ", stm32_getTimerNumber(cs_params->timer_handle->Instance)); #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])); + + uint8_t channel_no = 0; + for(int i=0; i<3; i++){ + // skip if not set + if (!_isset(cs_params->pins[i])) continue; + + sConfigInjected.InjectedRank = _getADCInjectedRank(channel_no++); + sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[i]), hadc.Instance); 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 + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[i]) , hadc.Instance)); + #endif return -1; } } - - -#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT - 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 -#endif - cs_params->adc_handle = &hadc; return 0; } -void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC) +/** + * Function to initialize the ADC GPIO pins + * + * @param cs_params current sense parameters + * @param pinA pin number for phase A + * @param pinB pin number for phase B + * @param pinC pin number for phase C + * @return int 0 if success, -1 if error + */ +int _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; + int pins[3] = {pinA, pinB, pinC}; + const char* port_names[3] = {"A", "B", "C"}; + for(int i=0; i<3; i++){ + if(_isset(pins[i])){ + // check if pin is an analog pin + if(pinmap_peripheral(analogInputToPinName(pins[i]), PinMap_ADC) == NP){ +#ifdef SIMPLEFOC_STM32_DEBUG + SimpleFOCDebug::print("STM32-CS: ERR: Pin "); + SimpleFOCDebug::print(port_names[i]); + SimpleFOCDebug::println(" does not belong to any ADC!"); +#endif + return -1; + } + pinmap_pinout(analogInputToPinName(pins[i]), PinMap_ADC); + cs_params->pins[i] = pins[i]; + } } + return 0; } -#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT + extern "C" { void ADC1_2_IRQHandler(void) { @@ -265,6 +236,5 @@ extern "C" { } #endif } -#endif #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.h b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.h index 2298b17c..81faf26b 100644 --- a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.h +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.h @@ -6,13 +6,11 @@ #if defined(STM32G4xx) && !defined(ARDUINO_B_G431B_ESC1) #include "stm32g4xx_hal.h" -#include "../../../../common/foc_utils.h" -#include "../../../../drivers/hardware_specific/stm32/stm32_mcu.h" #include "../stm32_mcu.h" -#include "stm32g4_utils.h" +#include "../stm32_adc_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); +int _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/stm32g4/stm32g4_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp index 1a954105..fd13e2b2 100644 --- a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp @@ -7,8 +7,8 @@ #include "../../../../drivers/hardware_specific/stm32/stm32_mcu.h" #include "../../../hardware_api.h" #include "../stm32_mcu.h" +#include "../stm32_adc_utils.h" #include "stm32g4_hal.h" -#include "stm32g4_utils.h" #include "Arduino.h" // #define SIMPLEFOC_STM32_ADC_INTERRUPT @@ -16,13 +16,23 @@ #define _ADC_VOLTAGE_G4 3.3f #define _ADC_RESOLUTION_G4 4096.0f - // array of values of 4 injected channels per adc instance (5) uint32_t adc_val[5][4]={0}; -// does adc interrupt need a downsample - per adc (5) -bool needs_downsample[5] = {1}; -// downsampling variable - per adc (5) -uint8_t tim_downsample[5] = {0}; + +#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT +#define USE_ADC_INTERRUPT 1 +#else +#define USE_ADC_INTERRUPT 0 +#endif + +// structure containing the configuration of the adc interrupt +Stm32AdcInterruptConfig adc_interrupt_config[5] = { + {0, 0, USE_ADC_INTERRUPT}, // ADC1 + {0, 0, USE_ADC_INTERRUPT}, // ADC2 + {0, 0, USE_ADC_INTERRUPT}, // ADC3 + {0, 0, USE_ADC_INTERRUPT}, // ADC4 + {0, 0, USE_ADC_INTERRUPT} // ADC5 +}; void* _configureADCLowSide(const void* driver_params, const int pinA, const int pinB, const int pinC){ @@ -31,86 +41,100 @@ void* _configureADCLowSide(const void* driver_params, const int pinA, const int .pins={(int)NOT_SET, (int)NOT_SET, (int)NOT_SET}, .adc_voltage_conv = (_ADC_VOLTAGE_G4) / (_ADC_RESOLUTION_G4) }; - _adc_gpio_init(cs_params, pinA,pinB,pinC); + if(_adc_gpio_init(cs_params, pinA,pinB,pinC) != 0) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; 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){ +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; + if (cs_params->timer_handle == NULL) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; // 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; + stm32_pause(driver_params); + + // get the index of the adc + int adc_index = _adcToIndex(cs_params->adc_handle); + + bool tim_interrupt = _initTimerInterruptDownsampling(cs_params, driver_params, adc_interrupt_config[adc_index]); + if(tim_interrupt) { + // error in the timer interrupt initialization + SIMPLEFOC_DEBUG("STM32-CS: timer has no repetition counter, ADC interrupt has to be used"); } // set the trigger output event - LL_TIM_SetTriggerOutput(cs_params->timer_handle->getHandle()->Instance, LL_TIM_TRGO_UPDATE); + LL_TIM_SetTriggerOutput(cs_params->timer_handle->Instance, LL_TIM_TRGO_UPDATE); // Start the adc calibration - HAL_ADCEx_Calibration_Start(cs_params->adc_handle,ADC_SINGLE_ENDED); - - // 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 + HAL_ADCEx_Calibration_Start(cs_params->adc_handle, ADC_SINGLE_ENDED); + + // start the adc + if (adc_interrupt_config[adc_index].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); + stm32_resume(driver_params); + + // return the cs parameters + // successfully initialized + // TODO verify if success in future + return _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 - #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; + uint8_t adc_index = (uint8_t)_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle); + return _readADCInjectedChannelVoltage(pin, (void*)cs_params, adc_interrupt_config[adc_index], adc_val[adc_index]); } -#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); + uint8_t adc_index = (uint8_t)_adcToIndex(AdcHandle); + _handleInjectedConvCpltCallback(AdcHandle, adc_interrupt_config[adc_index], adc_val[adc_index]); } } -#endif #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_utils.cpp b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_utils.cpp index 89a9bc34..3622c928 100644 --- a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_utils.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_utils.cpp @@ -1,171 +1,42 @@ -#include "stm32g4_utils.h" +#include "../stm32_adc_utils.h" #if defined(STM32G4xx) && !defined(ARDUINO_B_G431B_ESC1) -/* Exported Functions */ -/** - * @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; -} - - // timer to injected TRGO // https://github.com/stm32duino/Arduino_Core_STM32/blob/6588dee03382e73ed42c4a5e473900ab3b79d6e4/system/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_hal_adc_ex.h#L217 -uint32_t _timerToInjectedTRGO(HardwareTimer* timer){ - if(timer->getHandle()->Instance == TIM1) +uint32_t _timerToInjectedTRGO(TIM_HandleTypeDef* timer){ + if(timer->Instance == TIM1) return ADC_EXTERNALTRIGINJEC_T1_TRGO; #ifdef TIM2 // if defined timer 2 - else if(timer->getHandle()->Instance == TIM2) + else if(timer->Instance == TIM2) return ADC_EXTERNALTRIGINJEC_T2_TRGO; #endif #ifdef TIM3 // if defined timer 3 - else if(timer->getHandle()->Instance == TIM3) + else if(timer->Instance == TIM3) return ADC_EXTERNALTRIGINJEC_T3_TRGO; #endif #ifdef TIM4 // if defined timer 4 - else if(timer->getHandle()->Instance == TIM4) + else if(timer->Instance == TIM4) return ADC_EXTERNALTRIGINJEC_T4_TRGO; #endif #ifdef TIM6 // if defined timer 6 - else if(timer->getHandle()->Instance == TIM6) + else if(timer->Instance == TIM6) return ADC_EXTERNALTRIGINJEC_T6_TRGO; #endif #ifdef TIM7 // if defined timer 7 - else if(timer->getHandle()->Instance == TIM7) + else if(timer->Instance == TIM7) return ADC_EXTERNALTRIGINJEC_T7_TRGO; #endif #ifdef TIM8 // if defined timer 8 - else if(timer->getHandle()->Instance == TIM8) + else if(timer->Instance == TIM8) return ADC_EXTERNALTRIGINJEC_T8_TRGO; #endif #ifdef TIM15 // if defined timer 15 - else if(timer->getHandle()->Instance == TIM15) + else if(timer->Instance == TIM15) return ADC_EXTERNALTRIGINJEC_T15_TRGO; #endif #ifdef TIM20 // if defined timer 15 - else if(timer->getHandle()->Instance == TIM20) + else if(timer->Instance == TIM20) return ADC_EXTERNALTRIGINJEC_T20_TRGO; #endif else @@ -174,64 +45,43 @@ uint32_t _timerToInjectedTRGO(HardwareTimer* timer){ // timer to regular TRGO // https://github.com/stm32duino/Arduino_Core_STM32/blob/6588dee03382e73ed42c4a5e473900ab3b79d6e4/system/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_hal_adc.h#L519 -uint32_t _timerToRegularTRGO(HardwareTimer* timer){ - if(timer->getHandle()->Instance == TIM1) +uint32_t _timerToRegularTRGO(TIM_HandleTypeDef* timer){ + if(timer->Instance == TIM1) return ADC_EXTERNALTRIG_T1_TRGO; #ifdef TIM2 // if defined timer 2 - else if(timer->getHandle()->Instance == TIM2) + else if(timer->Instance == TIM2) return ADC_EXTERNALTRIG_T2_TRGO; #endif #ifdef TIM3 // if defined timer 3 - else if(timer->getHandle()->Instance == TIM3) + else if(timer->Instance == TIM3) return ADC_EXTERNALTRIG_T3_TRGO; #endif #ifdef TIM4 // if defined timer 4 - else if(timer->getHandle()->Instance == TIM4) + else if(timer->Instance == TIM4) return ADC_EXTERNALTRIG_T4_TRGO; #endif #ifdef TIM6 // if defined timer 6 - else if(timer->getHandle()->Instance == TIM6) + else if(timer->Instance == TIM6) return ADC_EXTERNALTRIG_T6_TRGO; #endif #ifdef TIM7 // if defined timer 7 - else if(timer->getHandle()->Instance == TIM7) + else if(timer->Instance == TIM7) return ADC_EXTERNALTRIG_T7_TRGO; #endif #ifdef TIM8 // if defined timer 8 - else if(timer->getHandle()->Instance == TIM8) + else if(timer->Instance == TIM8) return ADC_EXTERNALTRIG_T7_TRGO; #endif #ifdef TIM15 // if defined timer 15 - else if(timer->getHandle()->Instance == TIM15) + else if(timer->Instance == TIM15) return ADC_EXTERNALTRIG_T15_TRGO; #endif #ifdef TIM20 // if defined timer 15 - else if(timer->getHandle()->Instance == TIM20) + else if(timer->Instance == TIM20) return ADC_EXTERNALTRIG_T20_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/stm32g4/stm32g4_utils.h b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_utils.h deleted file mode 100644 index fa857bd0..00000000 --- a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_utils.h +++ /dev/null @@ -1,34 +0,0 @@ - -#ifndef STM32G4_UTILS_HAL -#define STM32G4_UTILS_HAL - -#include "Arduino.h" - -#if defined(STM32G4xx) && !defined(ARDUINO_B_G431B_ESC1) - -#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/6588dee03382e73ed42c4a5e473900ab3b79d6e4/system/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_hal_adc_ex.h#L217 -uint32_t _timerToInjectedTRGO(HardwareTimer* timer); - -// timer to regular TRGO -// https://github.com/stm32duino/Arduino_Core_STM32/blob/6588dee03382e73ed42c4a5e473900ab3b79d6e4/system/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_hal_adc.h#L519 -uint32_t _timerToRegularTRGO(HardwareTimer* timer); - -// function returning index of the ADC instance -int _adcToIndex(ADC_HandleTypeDef *AdcHandle); -int _adcToIndex(ADC_TypeDef *AdcHandle); - -#endif - -#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_hal.cpp b/src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_hal.cpp new file mode 100644 index 00000000..0ad49cb9 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_hal.cpp @@ -0,0 +1,209 @@ +#include "stm32h7_hal.h" + +#if defined(STM32H7xx) + +//#define SIMPLEFOC_STM32_DEBUG + +#include "../../../../communication/SimpleFOCDebug.h" + +ADC_HandleTypeDef hadc; + +/** + * Function initializing the ADC and the injected channels for the low-side current sensing + * + * @param cs_params - current sense parameters + * @param driver_params - driver parameters + * + * @return int - 0 if success + */ +int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params) +{ + ADC_InjectionConfTypeDef sConfigInjected; + + + /**Configure the global features of the ADC (Clock, Resolution, Data Alignment and number of conversion) + */ + hadc.Instance = _findBestADCForPins(3, cs_params->pins); + + if(hadc.Instance == ADC1) __HAL_RCC_ADC12_CLK_ENABLE(); +#ifdef ADC2 // if defined ADC2 + else if(hadc.Instance == ADC2) __HAL_RCC_ADC12_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: Cannot find a common ADC for the pins!"); +#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 +#if defined(ADC_VER_V5_V90) + // only for ADC3 + if(hadc.Instance == ADC3){ + hadc.Init.DataAlign = ADC3_DATAALIGN_RIGHT; + } + // more info here + // https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32H7xx_HAL_Driver/Inc/stm32h7xx_hal_adc.h#L170C13-L170C27 + hadc.Init.DMAContinuousRequests = DISABLE; + // not sure about this one!!! maybe use: ADC_SAMPLING_MODE_NORMAL + hadc.Init.SamplingMode = ADC_SAMPLING_MODE_NORMAL; +#endif + hadc.Init.NbrOfConversion = 1; + hadc.Init.NbrOfDiscConversion = 0; + hadc.Init.EOCSelection = ADC_EOC_SINGLE_CONV; + + hadc.Init.LowPowerAutoWait = DISABLE; + + + 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 = 0; + for(int pin_no=0; pin_no<3; pin_no++){ + if(_isset(cs_params->pins[pin_no])){ + sConfigInjected.InjectedNbrOfConversion++; + } + } + // if ADC1 or ADC2 + if(hadc.Instance == ADC1 || hadc.Instance == ADC2){ + // more info here: https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32H7xx_HAL_Driver/Inc/stm32h7xx_hal_adc.h#L658 + sConfigInjected.InjectedSamplingTime = ADC_SAMPLETIME_2CYCLES_5; + }else { + // adc3 + // https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32H7xx_HAL_Driver/Inc/stm32h7xx_hal_adc.h#L673 + sConfigInjected.InjectedSamplingTime = ADC3_SAMPLETIME_2CYCLES_5; + } + sConfigInjected.ExternalTrigInjecConvEdge = ADC_EXTERNALTRIGINJECCONV_EDGE_RISINGFALLING; + sConfigInjected.AutoInjectedConv = DISABLE; + sConfigInjected.InjectedDiscontinuousConvMode = DISABLE; + sConfigInjected.InjectedOffset = 0; + sConfigInjected.InjectedSingleDiff = ADC_SINGLE_ENDED; + sConfigInjected.InjectedOffsetNumber = ADC_OFFSET_NONE; + sConfigInjected.QueueInjectedContext = DISABLE; + sConfigInjected.InjecOversamplingMode = DISABLE; + + // automating TRGO flag finding - hardware specific + uint8_t tim_num = 0; + for (size_t i=0; i<6; i++) { + TIM_HandleTypeDef *timer_to_check = driver_params->timers_handle[tim_num++]; + TIM_TypeDef *instance_to_check = timer_to_check->Instance; + + uint32_t trigger_flag = _timerToInjectedTRGO(timer_to_check); + if(trigger_flag == _TRGO_NOT_AVAILABLE) continue; // timer does not have valid trgo for injected channels + + // check if TRGO used already - if yes use the next timer + if((timer_to_check->Instance->CR2 & LL_TIM_TRGO_ENABLE) || // if used for timer sync + (timer_to_check->Instance->CR2 & LL_TIM_TRGO_UPDATE)) // if used for ADC sync + { + continue; + } + + // 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; + // 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; + }else{ +#ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: Using timer: ", stm32_getTimerNumber(cs_params->timer_handle->Instance)); +#endif + } + + uint8_t channel_no = 0; + for(int i=0; i<3; i++){ + // skip if not set + if (!_isset(cs_params->pins[i])) continue; + + sConfigInjected.InjectedRank = _getADCInjectedRank(channel_no++); + sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[i]), hadc.Instance); + 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[i]) , hadc.Instance)); + #endif + return -1; + } + } + + cs_params->adc_handle = &hadc; + return 0; +} + + +/** + * Function to initialize the ADC GPIO pins + * + * @param cs_params current sense parameters + * @param pinA pin number for phase A + * @param pinB pin number for phase B + * @param pinC pin number for phase C + * @return int 0 if success, -1 if error + */ +int _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC) +{ + int pins[3] = {pinA, pinB, pinC}; + const char* port_names[3] = {"A", "B", "C"}; + for(int i=0; i<3; i++){ + if(_isset(pins[i])){ + // check if pin is an analog pin + if(pinmap_peripheral(analogInputToPinName(pins[i]), PinMap_ADC) == NP){ +#ifdef SIMPLEFOC_STM32_DEBUG + SimpleFOCDebug::print("STM32-CS: ERR: Pin "); + SimpleFOCDebug::print(port_names[i]); + SimpleFOCDebug::println(" does not belong to any ADC!"); +#endif + return -1; + } + pinmap_pinout(analogInputToPinName(pins[i]), PinMap_ADC); + cs_params->pins[i] = pins[i]; + } + } + return 0; +} + + +extern "C" { + void ADC_IRQHandler(void) + { + HAL_ADC_IRQHandler(&hadc); + } +} + +#ifdef ADC3 +extern "C" { + void ADC3_IRQHandler(void) + { + HAL_ADC_IRQHandler(&hadc); + } +} +#endif + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_hal.h b/src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_hal.h new file mode 100644 index 00000000..70a4b762 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_hal.h @@ -0,0 +1,13 @@ +#pragma once + +#include "Arduino.h" + +#if defined(STM32H7xx) +#include "stm32h7xx_hal.h" +#include "../stm32_adc_utils.h" +#include "../stm32_mcu.h" + +int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params); +int _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/stm32h7/stm32h7_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_mcu.cpp new file mode 100644 index 00000000..31f2c16b --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_mcu.cpp @@ -0,0 +1,140 @@ +#include "../../../hardware_api.h" + +#if defined(STM32H7xx) +#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 "../stm32_adc_utils.h" +#include "stm32h7_hal.h" +#include "Arduino.h" + + +#define _ADC_VOLTAGE 3.3f +#define _ADC_RESOLUTION 4096.0f + + +// array of values of 4 injected channels per adc instance (5) +uint32_t adc_val[5][4]={0}; + +#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT +#define USE_ADC_INTERRUPT 1 +#else +#define USE_ADC_INTERRUPT 0 +#endif + +// structure containing the configuration of the adc interrupt +Stm32AdcInterruptConfig adc_interrupt_config[5] = { + {0, 0, USE_ADC_INTERRUPT}, // ADC1 + {0, 0, USE_ADC_INTERRUPT}, // ADC2 + {0, 0, USE_ADC_INTERRUPT}, // ADC3 + {0, 0, USE_ADC_INTERRUPT}, // ADC4 + {0, 0, USE_ADC_INTERRUPT} // ADC5 +}; + + +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) + }; + if(_adc_gpio_init(cs_params, pinA,pinB,pinC) != 0) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; + 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 SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; + + // stop all the timers for the driver + stm32_pause(driver_params); + + // get the index of the adc + int adc_index = _adcToIndex(cs_params->adc_handle); + bool tim_interrupt = _initTimerInterruptDownsampling(cs_params, driver_params, adc_interrupt_config[adc_index]); + if(tim_interrupt) { + // error in the timer interrupt initialization + SIMPLEFOC_DEBUG("STM32-CS: timer has no repetition counter, ADC interrupt has to be used"); + } + + + // set the trigger output event + LL_TIM_SetTriggerOutput(cs_params->timer_handle->Instance, LL_TIM_TRGO_UPDATE); + + // Start the adc calibration + if(HAL_ADCEx_Calibration_Start(cs_params->adc_handle, ADC_CALIB_OFFSET_LINEARITY, ADC_SINGLE_ENDED) != HAL_OK){ + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot calibrate ADC!"); + #endif + return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; + } + + // start the adc + if(adc_interrupt_config[adc_index].use_adc_interrupt){ + + if(cs_params->adc_handle->Instance == ADC1){ + // enable interrupt + HAL_NVIC_SetPriority(ADC_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC_IRQn); + } + #ifdef ADC2 // if defined ADC2 + else if(cs_params->adc_handle->Instance == ADC2) { + // enable interrupt + HAL_NVIC_SetPriority(ADC3_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC3_IRQn); + } + #endif + #ifdef ADC3 // if defined ADC3 + else if(cs_params->adc_handle->Instance == ADC3) { + // enable interrupt + HAL_NVIC_SetPriority(ADC3_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC3_IRQn); + } + #endif + if(HAL_ADCEx_InjectedStart_IT(cs_params->adc_handle) != HAL_OK){ + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot start injected channels in interrupt mode!"); + #endif + return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; + } + }else{ + if(HAL_ADCEx_InjectedStart(cs_params->adc_handle) != HAL_OK){ + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot start injected channels!"); + #endif + return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; + } + } + + + // restart all the timers of the driver + stm32_resume(driver_params); + + // return the cs parameters + // successfully initialized + // TODO verify if success in future + return _cs_params; +} + + +// function reading an ADC value and returning the read voltage +float _readADCVoltageLowSide(const int pin, const void* cs_params){ + uint8_t adc_index = (uint8_t)_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle); + return _readADCInjectedChannelVoltage(pin, (void*)cs_params, adc_interrupt_config[adc_index], adc_val[adc_index]); +} + +extern "C" { + void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef *AdcHandle){ + uint8_t adc_index = (uint8_t)_adcToIndex(AdcHandle); + _handleInjectedConvCpltCallback(AdcHandle, adc_interrupt_config[adc_index], adc_val[adc_index]); + } +} + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_utils.cpp b/src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_utils.cpp new file mode 100644 index 00000000..9b16263f --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_utils.cpp @@ -0,0 +1,83 @@ +#include "../stm32_adc_utils.h" + +#if defined(STM32H7xx) + +/* Exported Functions */ + + +// timer to injected TRGO +// https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32H7xx_HAL_Driver/Inc/stm32h7xx_hal_adc_ex.h#L235 +uint32_t _timerToInjectedTRGO(TIM_HandleTypeDef* timer){ + + if(timer->Instance == TIM1) + return ADC_EXTERNALTRIGINJEC_T1_TRGO; +#ifdef TIM2 // if defined timer 2 + else if(timer->Instance == TIM2) + return ADC_EXTERNALTRIGINJEC_T2_TRGO; +#endif +#ifdef TIM4 // if defined timer 4 + else if(timer->Instance == TIM4) + return ADC_EXTERNALTRIGINJEC_T4_TRGO; +#endif +#ifdef TIM3 // if defined timer 3 + else if(timer->Instance == TIM3) + return ADC_EXTERNALTRIGINJEC_T3_TRGO; +#endif +#ifdef TIM6 // if defined timer 6 + else if(timer->Instance == TIM6) + return ADC_EXTERNALTRIGINJEC_T6_TRGO; +#endif +#ifdef TIM8 // if defined timer 8 + else if(timer->Instance == TIM8) + return ADC_EXTERNALTRIGINJEC_T8_TRGO; +#endif +#ifdef TIM15 // if defined timer 15 + else if(timer->Instance == TIM15) + return ADC_EXTERNALTRIGINJEC_T15_TRGO; +#endif + else + return _TRGO_NOT_AVAILABLE; +} + +// timer to regular TRGO +// https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32H7xx_HAL_Driver/Inc/stm32h7xx_hal_adc.h#L554 +uint32_t _timerToRegularTRGO(TIM_HandleTypeDef* timer){ + if(timer->Instance == TIM1) + return ADC_EXTERNALTRIG_T1_TRGO; +#ifdef TIM2 // if defined timer 2 + else if(timer->Instance == TIM2) + return ADC_EXTERNALTRIG_T2_TRGO; +#endif +#ifdef TIM3 // if defined timer 3 + else if(timer->Instance == TIM3) + return ADC_EXTERNALTRIG_T3_TRGO; +#endif +#ifdef TIM4 // if defined timer 4 + else if(timer->Instance == TIM4) + return ADC_EXTERNALTRIG_T4_TRGO; +#endif +#ifdef TIM6 // if defined timer 6 + else if(timer->Instance == TIM6) + return ADC_EXTERNALTRIG_T6_TRGO; +#endif +#ifdef TIM8 // if defined timer 8 + else if(timer->Instance == TIM8) + return ADC_EXTERNALTRIG_T8_TRGO; +#endif +#ifdef TIM15 // if defined timer 15 + else if(timer->Instance == TIM15) + return ADC_EXTERNALTRIG_T15_TRGO; +#endif +#ifdef TIM23 // if defined timer 23 + else if(timer->Instance == TIM23) + return ADC_EXTERNALTRIG_T23_TRGO; +#endif +#ifdef TIM24 // if defined timer 24 + else if(timer->Instance == TIM24) + return ADC_EXTERNALTRIG_T24_TRGO; +#endif + else + return _TRGO_NOT_AVAILABLE; +} + +#endif \ No newline at end of file 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 688ba796..3d1352b9 100644 --- a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp @@ -8,25 +8,21 @@ ADC_HandleTypeDef hadc; +/** + * Function initializing the ADC and the injected channels for the low-side current sensing + * + * @param cs_params - current sense parameters + * @param driver_params - driver parameters + * + * @return int - 0 if success + */ 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); + hadc.Instance = _findBestADCForPins(3, cs_params->pins); if(hadc.Instance == ADC1) { #ifdef __HAL_RCC_ADC1_CLK_ENABLE @@ -81,7 +77,7 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive #endif else{ #ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-CS: ERR: Pin does not belong to any ADC!"); + SIMPLEFOC_DEBUG("STM32-CS: ERR: Cannot find a common ADC for the pins!"); #endif return -1; // error not a valid ADC instance } @@ -99,7 +95,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; @@ -125,16 +121,23 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive // automating TRGO flag finding - hardware specific uint8_t tim_num = 0; - while(driver_params->timers[tim_num] != NP && tim_num < 6){ - uint32_t trigger_flag = _timerToInjectedTRGO(driver_params->timers[tim_num++]); + while(driver_params->timers_handle[tim_num] != NP && tim_num < 6){ + uint32_t trigger_flag = _timerToInjectedTRGO(driver_params->timers_handle[tim_num++]); if(trigger_flag == _TRGO_NOT_AVAILABLE) continue; // timer does not have valid trgo for injected channels + + // check if TRGO used already - if yes use the next timer + if((driver_params->timers_handle[tim_num-1]->Instance->CR2 & LL_TIM_TRGO_ENABLE) || // if used for timer sync + (driver_params->timers_handle[tim_num-1]->Instance->CR2 & LL_TIM_TRGO_UPDATE)) // if used for ADC sync + { + continue; + } // 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 = driver_params->timers[tim_num-1]; + cs_params->timer_handle = driver_params->timers_handle[tim_num-1]; // done break; } @@ -144,100 +147,62 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot sync any timer to injected channels!"); #endif return -1; - } - - - // first channel - sConfigInjected.InjectedRank = ADC_INJECTED_RANK_1; - sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[0])); - if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ + }else{ #ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[0])) ); + SIMPLEFOC_DEBUG("STM32-CS: Using timer: ", stm32_getTimerNumber(cs_params->timer_handle->Instance)); #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])); + uint8_t channel_no = 0; + for(int i=0; i<3; i++){ + // skip if not set + if (!_isset(cs_params->pins[i])) continue; + + sConfigInjected.InjectedRank = _getADCInjectedRank(channel_no++); + sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[i]), hadc.Instance); 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 + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[i]), hadc.Instance)); + #endif return -1; } } - - -#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT - 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 -#endif - cs_params->adc_handle = &hadc; return 0; } -void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC) +/** + * Function to initialize the ADC GPIO pins + * + * @param cs_params current sense parameters + * @param pinA pin number for phase A + * @param pinB pin number for phase B + * @param pinC pin number for phase C + * @return int 0 if success, -1 if error + */ +int _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; + int pins[3] = {pinA, pinB, pinC}; + const char* port_names[3] = {"A", "B", "C"}; + for(int i=0; i<3; i++){ + if(_isset(pins[i])){ + // check if pin is an analog pin + if(pinmap_peripheral(analogInputToPinName(pins[i]), PinMap_ADC) == NP){ +#ifdef SIMPLEFOC_STM32_DEBUG + SimpleFOCDebug::print("STM32-CS: ERR: Pin "); + SimpleFOCDebug::print(port_names[i]); + SimpleFOCDebug::println(" does not belong to any ADC!"); +#endif + return -1; + } + pinmap_pinout(analogInputToPinName(pins[i]), PinMap_ADC); + cs_params->pins[i] = pins[i]; + } } + return 0; } -#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT extern "C" { void ADC1_2_IRQHandler(void) { @@ -264,6 +229,5 @@ extern "C" { } #endif } -#endif #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.h b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.h index 0317b74b..fa49d593 100644 --- a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.h +++ b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.h @@ -6,13 +6,11 @@ #if defined(STM32L4xx) #include "stm32l4xx_hal.h" -#include "../../../../common/foc_utils.h" -#include "../../../../drivers/hardware_specific/stm32/stm32_mcu.h" #include "../stm32_mcu.h" -#include "stm32l4_utils.h" +#include "../stm32_adc_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); +int _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/stm32l4/stm32l4_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp index 9dea0933..9bd4a33f 100644 --- a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp @@ -7,8 +7,8 @@ #include "../../../../drivers/hardware_specific/stm32/stm32_mcu.h" #include "../../../hardware_api.h" #include "../stm32_mcu.h" +#include "../stm32_adc_utils.h" #include "stm32l4_hal.h" -#include "stm32l4_utils.h" #include "Arduino.h" @@ -18,11 +18,21 @@ // array of values of 4 injected channels per adc instance (5) uint32_t adc_val[5][4]={0}; -// does adc interrupt need a downsample - per adc (5) -bool needs_downsample[5] = {1}; -// downsampling variable - per adc (5) -uint8_t tim_downsample[5] = {0}; +#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT +#define USE_ADC_INTERRUPT 1 +#else +#define USE_ADC_INTERRUPT 0 +#endif + +// structure containing the configuration of the adc interrupt +Stm32AdcInterruptConfig adc_interrupt_config[5] = { + {0, 0, USE_ADC_INTERRUPT}, // ADC1 + {0, 0, USE_ADC_INTERRUPT}, // ADC2 + {0, 0, USE_ADC_INTERRUPT}, // ADC3 + {0, 0, USE_ADC_INTERRUPT}, // ADC4 + {0, 0, USE_ADC_INTERRUPT} // ADC5 +}; void* _configureADCLowSide(const void* driver_params, const int pinA, const int pinB, const int pinC){ @@ -30,86 +40,97 @@ void* _configureADCLowSide(const void* driver_params, const int pinA, const int .pins={(int)NOT_SET, (int)NOT_SET, (int)NOT_SET}, .adc_voltage_conv = (_ADC_VOLTAGE_L4) / (_ADC_RESOLUTION_L4) }; - _adc_gpio_init(cs_params, pinA,pinB,pinC); + if(_adc_gpio_init(cs_params, pinA,pinB,pinC) != 0) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; 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){ +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; + if (cs_params->timer_handle == NULL) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; // 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; + stm32_pause(driver_params); + + // get the index of the adc + int adc_index = _adcToIndex(cs_params->adc_handle); + + bool tim_interrupt = _initTimerInterruptDownsampling(cs_params, driver_params, adc_interrupt_config[adc_index]); + if(tim_interrupt) { + // error in the timer interrupt initialization + SIMPLEFOC_DEBUG("STM32-CS: timer has no repetition counter, ADC interrupt has to be used"); } + // set the trigger output event - LL_TIM_SetTriggerOutput(cs_params->timer_handle->getHandle()->Instance, LL_TIM_TRGO_UPDATE); + LL_TIM_SetTriggerOutput(cs_params->timer_handle->Instance, LL_TIM_TRGO_UPDATE); // Start the adc calibration HAL_ADCEx_Calibration_Start(cs_params->adc_handle,ADC_SINGLE_ENDED); // start the adc - #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT + if (adc_interrupt_config[adc_index].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 + }else{ HAL_ADCEx_InjectedStart(cs_params->adc_handle); - #endif + } // restart all the timers of the driver - _startTimers(driver_params->timers, 6); + stm32_resume(driver_params); + // return the cs parameters + // successfully initialized + // TODO verify if success in future + return _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 - #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; + uint8_t adc_index = (uint8_t)_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle); + return _readADCInjectedChannelVoltage(pin, (void*)cs_params, adc_interrupt_config[adc_index], adc_val[adc_index]); } -#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); + uint8_t adc_index = (uint8_t)_adcToIndex(AdcHandle); + _handleInjectedConvCpltCallback(AdcHandle, adc_interrupt_config[adc_index], adc_val[adc_index]); } } -#endif #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_utils.cpp b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_utils.cpp index 376d9d68..64229b8a 100644 --- a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_utils.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_utils.cpp @@ -1,163 +1,35 @@ -#include "stm32l4_utils.h" +#include "../stm32_adc_utils.h" #if defined(STM32L4xx) -/* Exported Functions */ -/** - * @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; -} - // timer to injected TRGO // https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32L4xx_HAL_Driver/Inc/stm32l4xx_hal_adc_ex.h#L210 -uint32_t _timerToInjectedTRGO(HardwareTimer* timer){ - if(timer->getHandle()->Instance == TIM1) +uint32_t _timerToInjectedTRGO(TIM_HandleTypeDef* timer){ + if(timer->Instance == TIM1) return ADC_EXTERNALTRIGINJEC_T1_TRGO; #ifdef TIM2 // if defined timer 2 - else if(timer->getHandle()->Instance == TIM2) + else if(timer->Instance == TIM2) return ADC_EXTERNALTRIGINJEC_T2_TRGO; #endif #ifdef TIM3 // if defined timer 3 - else if(timer->getHandle()->Instance == TIM3) + else if(timer->Instance == TIM3) return ADC_EXTERNALTRIGINJEC_T3_TRGO; #endif #ifdef TIM4 // if defined timer 4 - else if(timer->getHandle()->Instance == TIM4) + else if(timer->Instance == TIM4) return ADC_EXTERNALTRIGINJEC_T4_TRGO; #endif #ifdef TIM6 // if defined timer 6 - else if(timer->getHandle()->Instance == TIM6) + else if(timer->Instance == TIM6) return ADC_EXTERNALTRIGINJEC_T6_TRGO; #endif #ifdef TIM8 // if defined timer 8 - else if(timer->getHandle()->Instance == TIM8) + else if(timer->Instance == TIM8) return ADC_EXTERNALTRIGINJEC_T8_TRGO; #endif #ifdef TIM15 // if defined timer 15 - else if(timer->getHandle()->Instance == TIM15) + else if(timer->Instance == TIM15) return ADC_EXTERNALTRIGINJEC_T15_TRGO; #endif else @@ -166,56 +38,35 @@ uint32_t _timerToInjectedTRGO(HardwareTimer* timer){ // timer to regular TRGO // https://github.com/stm32duino/Arduino_Core_STM32/blob/6588dee03382e73ed42c4a5e473900ab3b79d6e4/system/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_hal_adc.h#L519 -uint32_t _timerToRegularTRGO(HardwareTimer* timer){ - if(timer->getHandle()->Instance == TIM1) +uint32_t _timerToRegularTRGO(TIM_HandleTypeDef* timer){ + if(timer->Instance == TIM1) return ADC_EXTERNALTRIG_T1_TRGO; #ifdef TIM2 // if defined timer 2 - else if(timer->getHandle()->Instance == TIM2) + else if(timer->Instance == TIM2) return ADC_EXTERNALTRIG_T2_TRGO; #endif #ifdef TIM3 // if defined timer 3 - else if(timer->getHandle()->Instance == TIM3) + else if(timer->Instance == TIM3) return ADC_EXTERNALTRIG_T3_TRGO; #endif #ifdef TIM4 // if defined timer 4 - else if(timer->getHandle()->Instance == TIM4) + else if(timer->Instance == TIM4) return ADC_EXTERNALTRIG_T4_TRGO; #endif #ifdef TIM6 // if defined timer 6 - else if(timer->getHandle()->Instance == TIM6) + else if(timer->Instance == TIM6) return ADC_EXTERNALTRIG_T6_TRGO; #endif #ifdef TIM8 // if defined timer 8 - else if(timer->getHandle()->Instance == TIM8) + else if(timer->Instance == TIM8) return ADC_EXTERNALTRIG_T8_TRGO; #endif #ifdef TIM15 // if defined timer 15 - else if(timer->getHandle()->Instance == TIM15) + else if(timer->Instance == TIM15) return ADC_EXTERNALTRIG_T15_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/stm32l4/stm32l4_utils.h b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_utils.h deleted file mode 100644 index ceef9be7..00000000 --- a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_utils.h +++ /dev/null @@ -1,34 +0,0 @@ - -#ifndef STM32L4_UTILS_HAL -#define STM32L4_UTILS_HAL - -#include "Arduino.h" - -#if defined(STM32L4xx) - -#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/6588dee03382e73ed42c4a5e473900ab3b79d6e4/system/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_hal_adc_ex.h#L217 -uint32_t _timerToInjectedTRGO(HardwareTimer* timer); - -// timer to regular TRGO -// https://github.com/stm32duino/Arduino_Core_STM32/blob/6588dee03382e73ed42c4a5e473900ab3b79d6e4/system/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_hal_adc.h#L519 -uint32_t _timerToRegularTRGO(HardwareTimer* timer); - -// function returning index of the ADC instance -int _adcToIndex(ADC_HandleTypeDef *AdcHandle); -int _adcToIndex(ADC_TypeDef *AdcHandle); - -#endif - -#endif \ No newline at end of file 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..3a542b53 --- /dev/null +++ b/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp @@ -0,0 +1,249 @@ +#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 + + + // return the cs parameters + // successfully initialized + // TODO verify if success in future + return cs_params; +} + + +#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/BLDCDriver3PWM.h b/src/drivers/BLDCDriver3PWM.h index 1942f60b..75ee478c 100644 --- a/src/drivers/BLDCDriver3PWM.h +++ b/src/drivers/BLDCDriver3PWM.h @@ -38,10 +38,9 @@ class BLDCDriver3PWM: public BLDCDriver int enableA_pin; //!< enable pin number int enableB_pin; //!< enable pin number int enableC_pin; //!< enable pin number - bool enable_active_high = true; /** - * Set phase voltages to the harware + * Set phase voltages to the hardware * * @param Ua - phase A voltage * @param Ub - phase B voltage @@ -50,7 +49,8 @@ class BLDCDriver3PWM: public BLDCDriver void setPwm(float Ua, float Ub, float Uc) override; /** - * Set phase voltages to the harware + * Set phase voltages to the hardware + * > Only possible is the driver has separate enable pins for all phases! * * @param sc - phase A state : active / disabled ( high impedance ) * @param sb - phase B state : active / disabled ( high impedance ) diff --git a/src/drivers/BLDCDriver6PWM.h b/src/drivers/BLDCDriver6PWM.h index e8643cc5..7ba7631c 100644 --- a/src/drivers/BLDCDriver6PWM.h +++ b/src/drivers/BLDCDriver6PWM.h @@ -37,7 +37,6 @@ class BLDCDriver6PWM: public BLDCDriver int pwmB_h,pwmB_l; //!< phase B pwm pin number int pwmC_h,pwmC_l; //!< phase C pwm pin number int enable_pin; //!< enable pin number - bool enable_active_high = true; float dead_zone; //!< a percentage of dead-time(zone) (both high and low side in low) for each pwm cycle [0,1] diff --git a/src/drivers/StepperDriver2PWM.cpp b/src/drivers/StepperDriver2PWM.cpp index dbbf5b8f..e8ccc6c6 100644 --- a/src/drivers/StepperDriver2PWM.cpp +++ b/src/drivers/StepperDriver2PWM.cpp @@ -44,8 +44,8 @@ StepperDriver2PWM::StepperDriver2PWM(int _pwm1, int _dir1, int _pwm2, int _dir2, // enable motor driver void StepperDriver2PWM::enable(){ // enable_pin the driver - if enable_pin pin available - if ( _isset(enable_pin1) ) digitalWrite(enable_pin1, HIGH); - if ( _isset(enable_pin2) ) digitalWrite(enable_pin2, HIGH); + if ( _isset(enable_pin1) ) digitalWrite(enable_pin1, enable_active_high); + if ( _isset(enable_pin2) ) digitalWrite(enable_pin2, enable_active_high); // set zero to PWM setPwm(0,0); } @@ -56,8 +56,8 @@ void StepperDriver2PWM::disable() // set zero to PWM setPwm(0, 0); // disable the driver - if enable_pin pin available - if ( _isset(enable_pin1) ) digitalWrite(enable_pin1, LOW); - if ( _isset(enable_pin2) ) digitalWrite(enable_pin2, LOW); + if ( _isset(enable_pin1) ) digitalWrite(enable_pin1, !enable_active_high); + if ( _isset(enable_pin2) ) digitalWrite(enable_pin2, !enable_active_high); } @@ -84,6 +84,14 @@ int StepperDriver2PWM::init() { return params!=SIMPLEFOC_DRIVER_INIT_FAILED; } +// Set voltage to the pwm pin +void StepperDriver2PWM::setPhaseState(PhaseState sa, PhaseState sb) { + // disable if needed + if( _isset(enable_pin1) && _isset(enable_pin2)){ + digitalWrite(enable_pin1, sa == PhaseState::PHASE_ON ? enable_active_high:!enable_active_high); + digitalWrite(enable_pin2, sb == PhaseState::PHASE_ON ? enable_active_high:!enable_active_high); + } +} // Set voltage to the pwm pin void StepperDriver2PWM::setPwm(float Ua, float Ub) { diff --git a/src/drivers/StepperDriver2PWM.h b/src/drivers/StepperDriver2PWM.h index b349af06..1bd00db9 100644 --- a/src/drivers/StepperDriver2PWM.h +++ b/src/drivers/StepperDriver2PWM.h @@ -60,6 +60,15 @@ class StepperDriver2PWM: public StepperDriver */ void setPwm(float Ua, float Ub) override; + /** + * Set phase voltages to the hardware + * > Only possible is the driver has separate enable pins for both phases! + * + * @param sa phase A state : active / disabled ( high impedance ) + * @param sb phase B state : active / disabled ( high impedance ) + */ + virtual void setPhaseState(PhaseState sa, PhaseState sb) override; + private: }; diff --git a/src/drivers/StepperDriver4PWM.cpp b/src/drivers/StepperDriver4PWM.cpp index 836f5472..52f1c1d1 100644 --- a/src/drivers/StepperDriver4PWM.cpp +++ b/src/drivers/StepperDriver4PWM.cpp @@ -21,8 +21,8 @@ StepperDriver4PWM::StepperDriver4PWM(int ph1A,int ph1B,int ph2A,int ph2B,int en1 // enable motor driver void StepperDriver4PWM::enable(){ // enable_pin the driver - if enable_pin pin available - if ( _isset(enable_pin1) ) digitalWrite(enable_pin1, HIGH); - if ( _isset(enable_pin2) ) digitalWrite(enable_pin2, HIGH); + if ( _isset(enable_pin1) ) digitalWrite(enable_pin1, enable_active_high); + if ( _isset(enable_pin2) ) digitalWrite(enable_pin2, enable_active_high); // set zero to PWM setPwm(0,0); } @@ -33,8 +33,8 @@ void StepperDriver4PWM::disable() // set zero to PWM setPwm(0, 0); // disable the driver - if enable_pin pin available - if ( _isset(enable_pin1) ) digitalWrite(enable_pin1, LOW); - if ( _isset(enable_pin2) ) digitalWrite(enable_pin2, LOW); + if ( _isset(enable_pin1) ) digitalWrite(enable_pin1, !enable_active_high); + if ( _isset(enable_pin2) ) digitalWrite(enable_pin2, !enable_active_high); } @@ -59,6 +59,15 @@ int StepperDriver4PWM::init() { return params!=SIMPLEFOC_DRIVER_INIT_FAILED; } +// Set voltage to the pwm pin +void StepperDriver4PWM::setPhaseState(PhaseState sa, PhaseState sb) { + // disable if needed + if( _isset(enable_pin1) && _isset(enable_pin2)){ + digitalWrite(enable_pin1, sa == PhaseState::PHASE_ON ? enable_active_high:!enable_active_high); + digitalWrite(enable_pin2, sb == PhaseState::PHASE_ON ? enable_active_high:!enable_active_high); + } +} + // Set voltage to the pwm pin void StepperDriver4PWM::setPwm(float Ualpha, float Ubeta) { diff --git a/src/drivers/StepperDriver4PWM.h b/src/drivers/StepperDriver4PWM.h index e4b2ee42..33e7d0cf 100644 --- a/src/drivers/StepperDriver4PWM.h +++ b/src/drivers/StepperDriver4PWM.h @@ -47,6 +47,16 @@ class StepperDriver4PWM: public StepperDriver */ void setPwm(float Ua, float Ub) override; + + /** + * Set phase voltages to the hardware. + * > Only possible is the driver has separate enable pins for both phases! + * + * @param sa phase A state : active / disabled ( high impedance ) + * @param sb phase B state : active / disabled ( high impedance ) + */ + virtual void setPhaseState(PhaseState sa, PhaseState sb) override; + private: }; diff --git a/src/drivers/hardware_api.h b/src/drivers/hardware_api.h index 8b477458..7809233d 100644 --- a/src/drivers/hardware_api.h +++ b/src/drivers/hardware_api.h @@ -25,9 +25,6 @@ #define SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH true #endif - - - // flag returned if driver init fails #define SIMPLEFOC_DRIVER_INIT_FAILED ((void*)-1) diff --git a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp new file mode 100644 index 00000000..da6661ca --- /dev/null +++ b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp @@ -0,0 +1,524 @@ + +/* +* MCPWM in espressif v5.x has +* - 2x groups (units) +* each one has +* - 3 timers +* - 3 operators (that can be associated with any timer) +* which control a 2xPWM signals +* - 1x comparator + 1x generator per PWM signal + + +* Independent mode: +* ------------------ +* 6 PWM independent signals per unit +* unit(0/1) > timer(0-2) > operator(0-2) > comparator(0-1) > generator(0-1) > pwm(A/B) +* +* -------------------------------------- Table View ----------------------------- +* +* group | timer | operator | comparator | generator | pwm +* -------------------------------------------------------------------------------- +* 0-1 | 0-2 | 0 | 0 | 0 | A +* 0-1 | 0-2 | 0 | 1 | 1 | B +* 0-1 | 0-2 | 1 | 0 | 0 | A +* 0-1 | 0-2 | 1 | 1 | 1 | B +* 0-1 | 0-2 | 2 | 0 | 0 | A +* 0-1 | 0-2 | 2 | 1 | 1 | B +* +* ------------------------------------- Example 3PWM ------------------------------ +* ┌─ comparator 0 - generator 0 -> pwm A +* ┌─ operator 0 -| +* | └─ comparator 1 - generator 1 -> pmw B +* unit - timer 0-2 -| +* 0-1 └─ operator 1 - comparator 0 - generator 0 - pwm C +* +* ------------------------------------- Example 2PWM ------------------------------ +* ┌─ comparator 0 - generator 0 -> pwm A +* unit - timer 0-2 - operator 0 -| +* 0-1 └─ comparator 1 - generator 1 -> pmw B +* +* -------------------------------------- Example 4PWM ----------------------------- +* ┌─ comparator 0 - generator 0 -> pwm A +* ┌─ operator 0 -| +* | └─ comparator 1 - generator 1 -> pmw B +* unit - timer 0-2 -| +* 0-1 | ┌─ comparator 0 - generator 0 -> pwm C +* └─ operator 1 -| +* └─ comparator 0 - generator 0 -> pwm D + + +* Complementary mode +* ------------------ +* - : 3 pairs of complementary PWM signals per unit +* unit(0/1) > timer(0) > operator(0-2) > comparator(0-1) > generator(0-1) > pwm(high/low pair) +* +* -------------------------------------- Table View ----------------------------- +* +* group | timer | operator | comparator | generator | pwm +* ------------------------------------------------------------------------ +* 0-1 | 0 | 0 | 0 | 0 | A +* 0-1 | 0 | 0 | 1 | 1 | B +* 0-1 | 0 | 1 | 0 | 0 | A +* 0-1 | 0 | 1 | 1 | 1 | B +* 0-1 | 0 | 2 | 0 | 0 | A +* 0-1 | 0 | 2 | 1 | 1 | B +* +* -------------------------------------- Example 6PWM ----------------------------- +* +* ┌─ comparator 0 - generator 0 -> pwm A_h +* ┌─ operator 0 -| +* | └─ comparator 1 - generator 1 -> pmw A_l +* | +* unit | ┌─ comparator 0 - generator 0 -> pwm B_h +* (group) - timer 0 -|- operator 1 -| +* 0-1 | └─ comparator 1 - generator 1 -> pmw B_l +* | +* | ┌─ comparator 0 - generator 0 -> pwm C_h +* └─ operator 2 -| +* └─ comparator 1 - generator 1 -> pmw C_l +* + + +* More info +* ---------- +* - timers can be associated with any operator, and multiple operators can be associated with the same timer +* - comparators can be associated with any operator +* - two comparators per operator for independent mode +* - one comparator per operator for complementary mode +* - generators can be associated with any comparator +* - one generator per PWM signal for independent mode +* - two generators per pair of PWM signals for complementary mode (not used in simplefoc) +* - dead-time can be set for each generator pair in complementary mode +* +* Docs +* ------- +* More info here: https:*www.espressif.com/sites/default/files/documentation/esp32_technical_reference_manual_en.pdf#mcpwm +* and here: // https://docs.espressif.com/projects/esp-idf/en/v5.1.4/esp32/migration-guides/release-5.x/5.0/peripherals.html +*/ + +#include "../../hardware_api.h" + +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) + +#if ESP_IDF_VERSION < ESP_IDF_VERSION_VAL(5, 4, 0) +#define ESP_IDF_VERSION_UNDER_5_4_0 +#pragma message("") +#pragma message("SimpleFOC: ESP IDF version under 5.4.0, consider upgrading!") +#pragma message("") +#else ESP_IDF_VERSION >= ESP_IDF_VERSION_VAL(5, 4, 0) +#define ESP_IDF_VERSION_ABOVE_5_4_0 +#endif + + +#include "esp32_driver_mcpwm.h" + +// MCPWM driver hardware timer pointers +mcpwm_timer_handle_t timers[2][3] = {NULL}; +// MCPWM timer periods configured (directly related to the pwm frequency) +uint32_t pwm_periods[2][3]; +// how many pins from the groups 6 pins is used +uint8_t group_pins_used[2] = {0}; +// last operator in the group +mcpwm_oper_handle_t last_operator[2]; + + + +// checking if group has pins available +bool _hasAvailablePins(int group, int no_pins){ + if(group_pins_used[group] + no_pins > 6){ + return false; + } + return true; +} + +// returns the index of the last timer in the group +// -1 if no timer instantiated yet +uint8_t _findLastTimer(int group){ + int i = 0; + for(; i<3; i++){ + if(timers[group][i] == NULL){ + return i-1; + } + } + // return the last index + return i; +} +// returns the index of the next timer to instantiate +// -1 if no timers available +uint8_t _findNextTimer(int group){ + int i = 0; + for(; i<3; i++){ + if(timers[group][i] == NULL){ + return i; + } + } + return -1; +} + +/* + * find the best group for the pins + * if 6pwm + * - Only option is an an empty group + * if 3pwm + * - Best is an empty group (we can set a pwm frequency) + * - Second best is a group with 4pwms (2 operators) available (we can set the pwm frequency -> new timer+new operator) + * - Third best option is any group which has 3pwms available (but uses previously defined pwm_frequency) + * if 1pwm + * - Best option is an empty group (we can set the pwm frequency) + * - Second best is a group with 2pwms (one operator) available (we can set the pwm frequency -> new timer+new operator) + * - Third best is a group with 1pwm available (but uses previously defined pwm_frequency ) + * if 2pwm + * - Best option is an empty group (we can set the pwm frequency) + * - Second best is a group with 2pwms available (we can set the pwm frequency -> new timer+new operator) + * - Third best is one pin per group (but uses previously defined pwm_frequency ) + * if 4pwm + * - best option is an empty group (we can set the pwm frequency) + * - second best is a group with 4pwms available (we can set the pwm frequency -> new timer + new operators) + * - third best is 2pwms per group (we can set the pwm frequency -> new timers + new operators) + * + * PROBLEM: Skipping/loosing channels happens in some cases when the group has already used some odd number of pwm channels (for example 3pwm or 1pwm) + * For example if the group has already used 3pwms, there is one generator that has one pwm channel left. + * If we use this channel we have to use the same timer it has been used with before, so we cannot change the pwm frequency. + * Current implementation does use the remaining channel only if there isn't other options that would allow changing the pwm frequency. + * In this example where we have 3pwms already configured, if we try to configure 2pws after, we will skip the remaining channel + * and use a new timer and operator to allow changing the pwm frequency. In such cases we loose (cannot be used) the remaining channel. + * TODO: use the pwm_frequency to avoid skipping pwm channels ! + * + * returns + * - 1 if solution found in one group + * - 2 if solution requires using both groups + * - 0 if no solution possible +*/ +int _findBestGroup(int no_pins, long pwm_freq, int* group, int* timer){ + // an empty group is always the best option + for(int i=0; itimers[0] = timers[mcpwm_group][timer_no]; + params->mcpwm_period = pwm_periods[mcpwm_group][timer_no]; + + uint8_t no_operators = 3; // use 3 comparators one per pair of pwms + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring " + String(no_operators) + " operators."); + mcpwm_operator_config_t operator_config = { .group_id = mcpwm_group }; + operator_config.intr_priority = 0; + operator_config.flags.update_gen_action_on_tep = true; + operator_config.flags.update_gen_action_on_tez = true; + for (int i = 0; i < no_operators; i++) { + CHECK_ERR(mcpwm_new_operator(&operator_config, ¶ms->oper[i]),"Could not create operator "+String(i)); + CHECK_ERR(mcpwm_operator_connect_timer(params->oper[i], params->timers[0]),"Could not connect timer to operator: " + String(i)); + } + +#if SIMPLEFOC_ESP32_HW_DEADTIME == true // hardware dead-time (hardware 6pwm) + + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 6PWM with hardware dead-time"); + + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring " + String(no_operators) + " comparators."); + // Create and configure comparators + mcpwm_comparator_config_t comparator_config = {0}; + comparator_config.flags.update_cmp_on_tez = true; + for (int i = 0; i < no_operators; i++) { + CHECK_ERR(mcpwm_new_comparator(params->oper[i], &comparator_config, ¶ms->comparator[i]),"Could not create comparator: " + String(i)); + CHECK_ERR(mcpwm_comparator_set_compare_value(params->comparator[i], (0)), "Could not set duty on comparator: " + String(i)); + } + +#else // software dead-time (software 6pwm) +// software dead-time (software 6pwm) + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 6PWM with software dead-time"); + + int no_pins = 6; + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring " + String(no_pins) + " comparators."); + // Create and configure comparators + mcpwm_comparator_config_t comparator_config = {0}; + comparator_config.flags.update_cmp_on_tez = true; + for (int i = 0; i < no_pins; i++) { + int oper_index = (int)floor(i / 2); + CHECK_ERR(mcpwm_new_comparator(params->oper[oper_index], &comparator_config, ¶ms->comparator[i]),"Could not create comparator: " + String(i)); + CHECK_ERR(mcpwm_comparator_set_compare_value(params->comparator[i], (0)), "Could not set duty on comparator: " + String(i)); + } +#endif + + int no_generators = 6; // one per pwm + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring " + String(no_generators) + " generators."); + // Create and configure generators + mcpwm_generator_config_t generator_config = {}; + for (int i = 0; i < no_generators; i++) { + generator_config.gen_gpio_num = pins[i]; + int oper_index = (int)floor(i / 2); + CHECK_ERR(mcpwm_new_generator(params->oper[oper_index], &generator_config, ¶ms->generator[i]),"Could not create generator " + String(i) +String(" on pin: ")+String(pins[i])); + } + + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring Center-Aligned 6 pwm."); + +#if SIMPLEFOC_ESP32_HW_DEADTIME == true // hardware dead-time (hardware 6pwm) + for (int i = 0; i < no_operators; i++) { + CHECK_ERR(_configureCenterAlign(params->generator[2*i],params->comparator[i]), "Failed to configure high-side center align pwm: " + String(2*i)); + CHECK_ERR(_configureCenterAlign(params->generator[2*i+1],params->comparator[i]), "Failed to configure low-side center align pwm: " + String(2*i+1)); + + } + // only available for 6pwm + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring dead-time."); + uint32_t dead_time = (int)pwm_periods[mcpwm_group][timer_no] * dead_zone; + mcpwm_dead_time_config_t dt_config_high; + dt_config_high.posedge_delay_ticks = dead_time; + dt_config_high.negedge_delay_ticks = 0; + dt_config_high.flags.invert_output = !SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH; + mcpwm_dead_time_config_t dt_config_low; + dt_config_low.posedge_delay_ticks = 0; + dt_config_low.negedge_delay_ticks = dead_time; + dt_config_low.flags.invert_output = SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH; + for (int i = 0; i < no_operators; i++) { + CHECK_ERR(mcpwm_generator_set_dead_time(params->generator[2*i], params->generator[2*i], &dt_config_high),"Could not set dead time for generator: " + String(i)); + CHECK_ERR(mcpwm_generator_set_dead_time(params->generator[2*i+1], params->generator[2*i+1], &dt_config_low),"Could not set dead time for generator: " + String(i+1)); + } +#else // software dead-time (software 6pwm) + for (int i = 0; i < 3; i++) { + CHECK_ERR(_configureCenterAlign(params->generator[2*i],params->comparator[2*i], !SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH), "Failed to configure high-side center align pwm: " + String(2*i)); + CHECK_ERR(_configureCenterAlign(params->generator[2*i+1],params->comparator[2*i+1], SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH) , "Failed to configure low-side center align pwm: " + String(2*i+1)); + } +#endif + + SIMPLEFOC_ESP32_DRV_DEBUG("Enabling timer: "+String(timer_no)); + // Enable and start timer + CHECK_ERR(mcpwm_timer_enable(params->timers[0]), "Failed to enable timer!"); + CHECK_ERR(mcpwm_timer_start_stop(params->timers[0], MCPWM_TIMER_START_NO_STOP), "Failed to start the timer!"); + + _delay(1); + SIMPLEFOC_ESP32_DRV_DEBUG("MCPWM configured!"); + params->dead_zone = dead_zone; + // save the configuration variables for later + group_pins_used[mcpwm_group] = 6; + return params; +} + + +/* + function configuring the pins for the mcpwm + - pwm_frequency - pwm frequency + - mcpwm_group - mcpwm group + - timer_no - timer number + - no_pins - number of pins + - pins - array of pins + - dead_zone - dead zone + + returns the driver parameters +*/ +void* _configurePinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, int no_pins, int* pins){ + + ESP32MCPWMDriverParams* params = new ESP32MCPWMDriverParams{ + .pwm_frequency = pwm_frequency, + .group_id = mcpwm_group + }; + + bool shared_timer = false; + // check if timer is configured + if (timers[mcpwm_group][timer_no] == NULL){ + mcpwm_timer_config_t pwm_config; + pwm_config.group_id = mcpwm_group; + pwm_config.clk_src = MCPWM_TIMER_CLK_SRC_DEFAULT; + pwm_config.resolution_hz = _PWM_TIMEBASE_RESOLUTION_HZ; + pwm_config.count_mode = MCPWM_TIMER_COUNT_MODE_UP_DOWN; + pwm_config.intr_priority = 0; + pwm_config.period_ticks = _calcPWMPeriod(pwm_frequency); +#ifdef ESP_IDF_VERSION_ABOVE_5_4_0 + pwm_config.flags.allow_pd = 0; +#endif + // initialise the timer + CHECK_ERR(mcpwm_new_timer(&pwm_config, &timers[mcpwm_group][timer_no]), "Could not initialize the timer in group: " + String(mcpwm_group)); + // save variables for later + pwm_periods[mcpwm_group][timer_no] = pwm_config.period_ticks / 2; + params->timers[0] = timers[mcpwm_group][timer_no]; + // if the numer of used channels it not pair skip one channel + // the skipped channel cannot be used with the new timer + // TODO avoid loosing channels like this + if(group_pins_used[mcpwm_group] %2) group_pins_used[mcpwm_group]++; + }else{ + // we will use an already instantiated timer + params->timers[0] = timers[mcpwm_group][timer_no]; + SIMPLEFOC_ESP32_DRV_DEBUG("Using previously configured timer: " + String(timer_no)); + // but we cannot change its configuration without affecting the other drivers + // so let's first verify that the configuration is the same + if(_calcPWMPeriod(pwm_frequency)/2 != pwm_periods[mcpwm_group][timer_no]){ + SIMPLEFOC_ESP32_DRV_DEBUG("ERR: Timer: "+String(timer_no)+" is confgured for freq: "+String(_calcPWMFreq(pwm_periods[mcpwm_group][timer_no]))+", not for freq:" +String(pwm_frequency)); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + CHECK_ERR(mcpwm_timer_start_stop( params->timers[0], MCPWM_TIMER_STOP_EMPTY), "Failed to stop the timer!"); + + shared_timer = true; + } + + uint8_t no_operators = ceil(no_pins / 2.0); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring " + String(no_operators) + " operators."); + mcpwm_operator_config_t operator_config = { .group_id = mcpwm_group }; + operator_config.intr_priority = 0; + operator_config.flags.update_gen_action_on_tep = true; + operator_config.flags.update_gen_action_on_tez = true; + for (int i = 0; i < no_operators; i++) { + if (shared_timer && i == 0) { // first operator already configured + params->oper[0] = last_operator[mcpwm_group]; + continue; + } + CHECK_ERR(mcpwm_new_operator(&operator_config, ¶ms->oper[i]),"Could not create operator "+String(i)); + CHECK_ERR(mcpwm_operator_connect_timer(params->oper[i], params->timers[0]),"Could not connect timer to operator: " + String(i)); + } + // save the last operator in this group + last_operator[mcpwm_group] = params->oper[no_operators - 1]; + + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring " + String(no_pins) + " comparators."); + // Create and configure comparators + mcpwm_comparator_config_t comparator_config = {0}; + comparator_config.flags.update_cmp_on_tez = true; + for (int i = 0; i < no_pins; i++) { + int oper_index = shared_timer ? (int)floor((i + 1) / 2) : (int)floor(i / 2); + CHECK_ERR(mcpwm_new_comparator(params->oper[oper_index], &comparator_config, ¶ms->comparator[i]),"Could not create comparator: " + String(i)); + CHECK_ERR(mcpwm_comparator_set_compare_value(params->comparator[i], (0)), "Could not set duty on comparator: " + String(i)); + } + + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring " + String(no_pins) + " generators."); + // Create and configure generators; + mcpwm_generator_config_t generator_config = {}; + for (int i = 0; i < no_pins; i++) { + generator_config.gen_gpio_num = pins[i]; + int oper_index = shared_timer ? (int)floor((i + 1) / 2) : (int)floor(i / 2); + CHECK_ERR(mcpwm_new_generator(params->oper[oper_index], &generator_config, ¶ms->generator[i]), "Could not create generator " + String(i) +String(" on pin: ")+String(pins[i])); + } + + + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring center-aligned pwm."); + for (int i = 0; i < no_pins; i++) { + CHECK_ERR(_configureCenterAlign(params->generator[i],params->comparator[i], !SIMPLEFOC_PWM_ACTIVE_HIGH), "Failed to configure center align pwm: " + String(i)); + } + + SIMPLEFOC_ESP32_DRV_DEBUG("Enabling timer: "+String(timer_no)); + // Enable and start timer if not shared + if (!shared_timer) CHECK_ERR(mcpwm_timer_enable(params->timers[0]), "Failed to enable timer!"); + // start the timer + CHECK_ERR(mcpwm_timer_start_stop(params->timers[0], MCPWM_TIMER_START_NO_STOP), "Failed to start the timer!"); + + _delay(1); + SIMPLEFOC_ESP32_DRV_DEBUG("MCPWM configured!"); + // save the configuration variables for later + params->mcpwm_period = pwm_periods[mcpwm_group][timer_no]; + group_pins_used[mcpwm_group] += no_pins; + return params; +} + +// function setting the duty cycle to the MCPWM pin +void IRAM_ATTR _setDutyCycle(mcpwm_cmpr_handle_t cmpr, uint32_t mcpwm_period, float duty_cycle){ + float duty = _constrain(duty_cycle, 0.0, 1.0); + mcpwm_comparator_set_compare_value(cmpr, (uint32_t)(mcpwm_period*duty)); +} + +// function setting the duty cycle to the MCPWM pin +void _forcePhaseState(mcpwm_gen_handle_t generator_high, mcpwm_gen_handle_t generator_low, PhaseState phase_state){ + // phase state is forced in hardware pwm mode + // esp-idf docs: https://docs.espressif.com/projects/esp-idf/en/v5.1.4/esp32/api-reference/peripherals/mcpwm.html#generator-force-actions + // github issue: https://github.com/espressif/esp-idf/issues/12237 + mcpwm_generator_set_force_level(generator_high, (phase_state == PHASE_ON || phase_state == PHASE_HI) ? -1 : 0, true); + mcpwm_generator_set_force_level(generator_low, (phase_state == PHASE_ON || phase_state == PHASE_LO) ? -1 : 1, true); +} + +#endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h index 10497876..5726e906 100644 --- a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h +++ b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h @@ -5,92 +5,154 @@ #if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) +#include "driver/mcpwm_prelude.h" +#include "soc/mcpwm_reg.h" +#include "soc/mcpwm_struct.h" +#include "esp_idf_version.h" +// version check - this mcpwm driver is specific for ESP-IDF 5.x and arduino-esp32 3.x +#if ESP_IDF_VERSION < ESP_IDF_VERSION_VAL(5, 0, 0) +#error SimpleFOC: ESP-IDF version 4 or lower detected. Please update to ESP-IDF 5.x and Arduino-esp32 3.0 (or higher) +#endif -#pragma message("") -#pragma message("SimpleFOC: compiling for ESP32 MCPWM driver") -#pragma message("") +#ifndef SIMPLEFOC_ESP32_HW_DEADTIME + #define SIMPLEFOC_ESP32_HW_DEADTIME true // TODO: Change to false when sw-deadtime & phase_state is approved ready for general use. +#endif +//!< ESP32 MCPWM driver parameters +typedef struct ESP32MCPWMDriverParams { + long pwm_frequency; //!< frequency of the pwm signal + int group_id; //!< group of the mcpwm + mcpwm_timer_handle_t timers[2]; //!< timers of the mcpwm + mcpwm_oper_handle_t oper[3]; //!< operators of the mcpwm + mcpwm_cmpr_handle_t comparator[6]; //!< comparators of the mcpwm + mcpwm_gen_handle_t generator[6]; //!< generators of the mcpwm + uint32_t mcpwm_period; //!< period of the pwm signal + float dead_zone; //!< dead zone of the pwm signal +} ESP32MCPWMDriverParams; -#include "driver/mcpwm.h" -#include "soc/mcpwm_reg.h" -#include "soc/mcpwm_struct.h" -// empty motor slot -#define _EMPTY_SLOT -20 -#define _TAKEN_SLOT -21 +#define SIMPLEFOC_ESP32_DEBUG(tag, str)\ + SimpleFOCDebug::println( "ESP32-"+String(tag)+ ": "+ String(str)); -// ABI bus frequency - would be better to take it from somewhere -// but I did nto find a good exposed variable -#define _MCPWM_FREQ 160e6f +#define SIMPLEFOC_ESP32_DRV_DEBUG(str)\ + SIMPLEFOC_ESP32_DEBUG("DRV", str);\ -// preferred pwm resolution default -#define _PWM_RES_DEF 4096 -// min resolution -#define _PWM_RES_MIN 3000 -// max resolution -#define _PWM_RES_MAX 8000 -// pwm frequency -#define _PWM_FREQUENCY 25000 // default -#define _PWM_FREQUENCY_MAX 50000 // mqx - -// structure containing motor slot configuration -// this library supports up to 4 motors -typedef struct { - int pinA; - mcpwm_dev_t* mcpwm_num; - mcpwm_unit_t mcpwm_unit; - mcpwm_operator_t mcpwm_operator; - mcpwm_io_signals_t mcpwm_a; - mcpwm_io_signals_t mcpwm_b; - mcpwm_io_signals_t mcpwm_c; -} bldc_3pwm_motor_slots_t; - -typedef struct { - int pin1A; - mcpwm_dev_t* mcpwm_num; - mcpwm_unit_t mcpwm_unit; - mcpwm_operator_t mcpwm_operator1; - mcpwm_operator_t mcpwm_operator2; - mcpwm_io_signals_t mcpwm_1a; - mcpwm_io_signals_t mcpwm_1b; - mcpwm_io_signals_t mcpwm_2a; - mcpwm_io_signals_t mcpwm_2b; -} stepper_4pwm_motor_slots_t; - -typedef struct { - int pin1pwm; - mcpwm_dev_t* mcpwm_num; - mcpwm_unit_t mcpwm_unit; - mcpwm_operator_t mcpwm_operator; - mcpwm_io_signals_t mcpwm_a; - mcpwm_io_signals_t mcpwm_b; -} stepper_2pwm_motor_slots_t; - -typedef struct { - int pinAH; - mcpwm_dev_t* mcpwm_num; - mcpwm_unit_t mcpwm_unit; - mcpwm_operator_t mcpwm_operator1; - mcpwm_operator_t mcpwm_operator2; - mcpwm_io_signals_t mcpwm_ah; - mcpwm_io_signals_t mcpwm_bh; - mcpwm_io_signals_t mcpwm_ch; - mcpwm_io_signals_t mcpwm_al; - mcpwm_io_signals_t mcpwm_bl; - mcpwm_io_signals_t mcpwm_cl; -} bldc_6pwm_motor_slots_t; +// macro for checking the error of the mcpwm functions +// if the function returns an error the function will return SIMPLEFOC_DRIVER_INIT_FAILED +#define CHECK_ERR(func_call, message) \ + if ((func_call) != ESP_OK) { \ + SIMPLEFOC_ESP32_DRV_DEBUG("ERROR - " + String(message)); \ + return SIMPLEFOC_DRIVER_INIT_FAILED; \ + } -typedef struct ESP32MCPWMDriverParams { - long pwm_frequency; - mcpwm_dev_t* mcpwm_dev; - mcpwm_unit_t mcpwm_unit; - mcpwm_operator_t mcpwm_operator1; - mcpwm_operator_t mcpwm_operator2; - float deadtime; -} ESP32MCPWMDriverParams; - +// ABI bus frequency - would be better to take it from somewhere +// but I did nto find a good exposed variable +#define _MCPWM_FREQ 160e6f +#define _PWM_TIMEBASE_RESOLUTION_HZ (_MCPWM_FREQ) /*!< Resolution of MCPWM */ +// pwm frequency settings +#define _PWM_FREQUENCY 25000 // 25khz +#define _PWM_FREQUENCY_MAX 50000 // 50kHz + + +// low-level configuration API + +/** + * checking if group has pins available + * @param group - group of the mcpwm + * @param no_pins - number of pins + * @returns true if pins are available, false otherwise + */ +bool _hasAvailablePins(int group, int no_pins); +/** + * function finding the last timer in the group + * @param group - group of the mcpwm + * @returns index of the last timer in the group + * -1 if no timer instantiated yet + */ +uint8_t _findLastTimer(int group); + +/** + * function finding the next timer in the group + * @param group - group of the mcpwm + * @returns index of the next timer in the group + * -1 if all timers are used + */ +uint8_t _findNextTimer(int group); + + +/** + * function finding the best group and timer for the pwm signals + * + * @param no_pins - number of pins + * @param pwm_freq - frequency of the pwm signal + * @param group - pointer to the group + * @param timer - pointer to the timer + * @returns + * 1 if solution found in one group + * 2 if solution requires using both groups + * 0 if no solution possible + */ +int _findBestGroup(int no_pins, long pwm_freq, int* group, int* timer); + + +/** + * function configuring the center alignement and inversion of a pwm signal + * @param gena - mcpwm generator handle + * @param cmpa - mcpwm comparator handle + * @param inverted - true if the signal is inverted, false otherwise + */ +int _configureCenterAlign(mcpwm_gen_handle_t gena, mcpwm_cmpr_handle_t cmpa, bool inverted); + +/** + * function calculating the pwm period + * @param pwm_frequency - frequency of the pwm signal + * @return uint32_t - period of the pwm signal + */ +uint32_t _calcPWMPeriod(long pwm_frequency); +/** + * function calculating the pwm frequency + * @param pwm_period - period of the pwm signal + * @return long - frequency of the pwm signal + */ +long _calcPWMFreq(long pwm_period); + +/** + * function configuring the MCPWM for 6pwm + * @param pwm_frequency - frequency of the pwm signal + * @param mcpwm_group - group of the mcpwm + * @param timer_no - timer number + * @param dead_zone - dead zone of the pwm signal + * @param pins - array of pins + * @return ESP32MCPWMDriverParams* - pointer to the driver parameters if successful, -1 if failed + */ +void* _configure6PWMPinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, float dead_zone, int* pins); +/** + * function configuring the MCPWM for pwm generation + * @param pwm_frequency - frequency of the pwm signal + * @param mcpwm_group - group of the mcpwm + * @param timer_no - timer number + * @param no_pins - number of pins + * @param pins - array of pins + * @return ESP32MCPWMDriverParams* - pointer to the driver parameters if successful, -1 if failed + */ +void* _configurePinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, int no_pins, int* pins); +/** + * function setting the duty cycle to the MCPWM channel + * @param cmpr - mcpwm channel handle + * @param mcpwm_period - period of the pwm signal + * @param duty_cycle - duty cycle of the pwm signal + */ +void _setDutyCycle(mcpwm_cmpr_handle_t cmpr, uint32_t mcpwm_period, float duty_cycle); + +/** + * function setting the phase state + * @param generator_high - mcpwm generator handle for the high side + * @param generator_low - mcpwm generator handle for the low side + * @param phase_state - phase state + */ +void _forcePhaseState(mcpwm_gen_handle_t generator_high, mcpwm_gen_handle_t generator_low, PhaseState phase_state); #endif #endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp b/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp index a454c052..df7bd3fc 100644 --- a/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp +++ b/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp @@ -7,18 +7,21 @@ #pragma message("") #include "driver/ledc.h" +#include "esp_idf_version.h" + + +// version check - this ledc driver is specific for ESP-IDF 5.x and arduino-esp32 3.x +#if ESP_IDF_VERSION < ESP_IDF_VERSION_VAL(5, 0, 0) +#error SimpleFOC: ESP-IDF version 4 or lower detected. Please update to ESP-IDF 5.x and Arduino-esp32 3.0 (or higher) +#endif #define _PWM_FREQUENCY 25000 // 25khz #define _PWM_FREQUENCY_MAX 38000 // 38khz max to be able to have 10 bit pwm resolution #define _PWM_RES_BIT 10 // 10 bir resolution -#define _PWM_RES 1023 // 2^10-1 = 1023-1 +#define _PWM_RES 1023 // 2^10-1 = 1024-1 -// empty motor slot -#define _EMPTY_SLOT -20 -#define _TAKEN_SLOT -21 - -// figure out how many ledc channels are avaible +// figure out how many ledc channels are available // esp32 - 2x8=16 // esp32s2 - 8 // esp32c3 - 6 @@ -29,51 +32,141 @@ #define LEDC_CHANNELS (SOC_LEDC_CHANNEL_NUM) #endif +#define LEDC_CHANNELS_GROUP0 (LEDC_CHANNELS < 8 ? LEDC_CHANNELS : 8) +#define LEDC_CHANNELS_GROUP1 (LEDC_CHANNELS < 8 ? 0 : LEDC_CHANNELS - 8) + -// current channel stack index +// currently used ledc channels // support for multiple motors // esp32 has 16 channels // esp32s2 has 8 channels // esp32c3 has 6 channels -int channel_index = 0; - - +// channels from 0-7 are in group 0 and 8-15 in group 1 +// - only esp32 as of mid 2024 has the second group, all the s versions don't +int group_channels_used[2] = {0}; typedef struct ESP32LEDCDriverParams { - int channels[6]; + ledc_channel_t channels[6]; + ledc_mode_t groups[6]; long pwm_frequency; + float dead_zone; } ESP32LEDCDriverParams; +int esp32_gpio_nr(int pin) { + #if defined(BOARD_HAS_PIN_REMAP) && !defined(BOARD_USES_HW_GPIO_NUMBERS) + return digitalPinToGPIONumber(pin); + #else + return pin; + #endif +} -// configure High PWM frequency -void _setHighFrequency(const long freq, const int pin, const int channel){ - ledcSetup(channel, freq, _PWM_RES_BIT ); - ledcAttachPin(pin, channel); +/* + Function to attach a channel to a pin with advanced settings + - freq - pwm frequency + - resolution - pwm resolution + - channel - ledc channel + - inverted - output inverted + - group - ledc group + + This function is a workaround for the ledcAttachPin function that is not available in the ESP32 Arduino core, in which the + PWM signals are synchronized in pairs, while the simplefoc requires a bit more flexible configuration. + This function sets also allows configuring a channel as inverted, which is not possible with the ledcAttachPin function. + + Function returns true if the channel was successfully attached, false otherwise. +*/ +bool _ledcAttachChannelAdvanced(uint8_t pin, int _channel, int _group, uint32_t freq, uint8_t resolution, bool inverted) { + + + ledc_channel_t channel = static_cast(_channel); + ledc_mode_t group = static_cast(_group); + + ledc_timer_bit_t res = static_cast(resolution); + ledc_timer_config_t ledc_timer; + memset(&ledc_timer, 0, sizeof(ledc_timer)); + ledc_timer.speed_mode = group; + ledc_timer.timer_num = LEDC_TIMER_0; + ledc_timer.duty_resolution = res; + ledc_timer.freq_hz = freq; + ledc_timer.clk_cfg = LEDC_AUTO_CLK; + if (ledc_timer_config(&ledc_timer) != ESP_OK) { + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Failed to configure the timer:", LEDC_TIMER_0); + return false; + } + + // if active high is false invert + int pin_high_level = SIMPLEFOC_PWM_ACTIVE_HIGH ? 0 : 1; + if (inverted) pin_high_level = !pin_high_level; + + uint32_t duty = ledc_get_duty(group, channel); + ledc_channel_config_t ledc_channel; + ledc_channel.speed_mode = group; + ledc_channel.sleep_mode = LEDC_SLEEP_MODE_KEEP_ALIVE; + ledc_channel.channel = channel; + ledc_channel.timer_sel = LEDC_TIMER_0; + ledc_channel.intr_type = LEDC_INTR_DISABLE; + ledc_channel.gpio_num = esp32_gpio_nr(pin); + ledc_channel.duty = duty; + ledc_channel.hpoint = 0; + ledc_channel.flags.output_invert = pin_high_level; // 0 is active high, 1 is active low + if (ledc_channel_config(&ledc_channel)!= ESP_OK) { + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Failed to attach channel:", _channel); + return false; + } + + return true; } +// returns the number of available channels in the group +int _availableGroupChannels(int group){ + if(group == 0) return LEDC_CHANNELS_GROUP0 - group_channels_used[0]; + else if(group == 1) return LEDC_CHANNELS_GROUP1 - group_channels_used[1]; + return 0; +} - +// returns the number of the group that has enough channels available +// returns -1 if no group has enough channels +// +// NOT IMPLEMENTED BUT COULD BE USEFUL +// returns 2 if no group has enough channels but combined they do +int _findGroupWithChannelsAvailable(int no_channels){ + if(no_channels <= _availableGroupChannels(0)) return 0; + if(no_channels <= _availableGroupChannels(1)) return 1; + return -1; +} 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 + SIMPLEFOC_DEBUG("EP32-DRV: Configuring 1PWM"); // check if enough channels available - if ( channel_index + 1 >= LEDC_CHANNELS ) return SIMPLEFOC_DRIVER_INIT_FAILED; + int group = _findGroupWithChannelsAvailable(1); + if (group < 0){ + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Not enough channels available!"); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + SIMPLEFOC_DEBUG("EP32-DRV: 1PWM setup in group: ", (group)); + + // configure the channel + group_channels_used[group] += 1; + if(!_ledcAttachChannelAdvanced(pinA, group_channels_used[group], group, pwm_frequency, _PWM_RES_BIT, false)){ + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Failed to configure pin:", pinA); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } - int ch1 = channel_index++; - _setHighFrequency(pwm_frequency, pinA, ch1); ESP32LEDCDriverParams* params = new ESP32LEDCDriverParams { - .channels = { ch1 }, + .channels = { static_cast(group_channels_used[group]) }, + .groups = { (ledc_mode_t)group }, .pwm_frequency = pwm_frequency }; + SIMPLEFOC_DEBUG("EP32-DRV: 1PWM setup successful in group: ", (group)); return params; } @@ -88,18 +181,34 @@ 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 - // check if enough channels available - if ( channel_index + 2 >= LEDC_CHANNELS ) return SIMPLEFOC_DRIVER_INIT_FAILED; + SIMPLEFOC_DEBUG("EP32-DRV: Configuring 2PWM"); - int ch1 = channel_index++; - int ch2 = channel_index++; - _setHighFrequency(pwm_frequency, pinA, ch1); - _setHighFrequency(pwm_frequency, pinB, ch2); + // check if enough channels available + int group = _findGroupWithChannelsAvailable(2); + if (group < 0) { + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Not enough channels available!"); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + SIMPLEFOC_DEBUG("EP32-DRV: 2PWM setup in group: ", (group)); ESP32LEDCDriverParams* params = new ESP32LEDCDriverParams { - .channels = { ch1, ch2 }, + .channels = { static_cast(0)}, + .groups = { (ledc_mode_t)0 }, .pwm_frequency = pwm_frequency }; + + int pins[2] = {pinA, pinB}; + for(int i = 0; i < 2; i++){ + group_channels_used[group]++; + if(!_ledcAttachChannelAdvanced(pins[i], group_channels_used[group], group, pwm_frequency, _PWM_RES_BIT, false)){ + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Failed to configure pin:", pins[i]); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + + params->channels[i] = static_cast(group_channels_used[group]); + params->groups[i] = (ledc_mode_t)group; + } + SIMPLEFOC_DEBUG("EP32-DRV: 2PWM setup successful in group: ", (group)); return params; } @@ -109,20 +218,34 @@ void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const in 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 - // check if enough channels available - if ( channel_index + 3 >= LEDC_CHANNELS ) return SIMPLEFOC_DRIVER_INIT_FAILED; + SIMPLEFOC_DEBUG("EP32-DRV: Configuring 3PWM"); - int ch1 = channel_index++; - int ch2 = channel_index++; - int ch3 = channel_index++; - _setHighFrequency(pwm_frequency, pinA, ch1); - _setHighFrequency(pwm_frequency, pinB, ch2); - _setHighFrequency(pwm_frequency, pinC, ch3); + // check if enough channels available + int group = _findGroupWithChannelsAvailable(3); + if (group < 0) { + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Not enough channels available!"); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + SIMPLEFOC_DEBUG("EP32-DRV: 3PWM setup in group: ", (group)); ESP32LEDCDriverParams* params = new ESP32LEDCDriverParams { - .channels = { ch1, ch2, ch3 }, + .channels = { static_cast(0)}, + .groups = { (ledc_mode_t)0 }, .pwm_frequency = pwm_frequency }; + + int pins[3] = {pinA, pinB, pinC}; + for(int i = 0; i < 3; i++){ + if(!_ledcAttachChannelAdvanced(pins[i], group_channels_used[group], group, pwm_frequency, _PWM_RES_BIT, false)){ + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Failed to configure pin:", pins[i]); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + + params->channels[i] = static_cast(group_channels_used[group]); + params->groups[i] = (ledc_mode_t)group; + group_channels_used[group]++; + } + SIMPLEFOC_DEBUG("EP32-DRV: 3PWM setup successful in group: ", (group)); return params; } @@ -132,54 +255,162 @@ void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const in 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 - // check if enough channels available - if ( channel_index + 4 >= LEDC_CHANNELS ) return SIMPLEFOC_DRIVER_INIT_FAILED; - - int ch1 = channel_index++; - int ch2 = channel_index++; - int ch3 = channel_index++; - int ch4 = channel_index++; - _setHighFrequency(pwm_frequency, pinA, ch1); - _setHighFrequency(pwm_frequency, pinB, ch2); - _setHighFrequency(pwm_frequency, pinC, ch3); - _setHighFrequency(pwm_frequency, pinD, ch4); ESP32LEDCDriverParams* params = new ESP32LEDCDriverParams { - .channels = { ch1, ch2, ch3, ch4 }, + .channels = { static_cast(0)}, + .groups = { (ledc_mode_t)0 }, .pwm_frequency = pwm_frequency }; + + SIMPLEFOC_DEBUG("EP32-DRV: Configuring 4PWM"); + // check if enough channels available + int group = _findGroupWithChannelsAvailable(4); + if (group < 0){ + // not enough channels available on any individual group + // check if their combined number is enough (two channels per group) + if(_availableGroupChannels(0) >=2 && _availableGroupChannels(1) >=2){ + group = 2; + SIMPLEFOC_DEBUG("EP32-DRV: WARNING: Not enough available ledc channels for 4pwm in a single group! Using two groups!"); + SIMPLEFOC_DEBUG("EP32-DRV: 4PWM setup in groups: 0 and 1!"); + params->groups[2] = (ledc_mode_t)1; + params->groups[3] = (ledc_mode_t)1; + }else{ + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Not enough available ledc channels for 4pwm!"); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + }else{ + SIMPLEFOC_DEBUG("EP32-DRV: 4PWM setup in group: ", (group)); + params->groups[0] = (ledc_mode_t)group; + params->groups[1] = (ledc_mode_t)group; + params->groups[2] = (ledc_mode_t)group; + params->groups[3] = (ledc_mode_t)group; + } + + + + int pins[4] = {pinA, pinB, pinC, pinD}; + for(int i = 0; i < 4; i++){ + group_channels_used[params->groups[i]]++; + if(!_ledcAttachChannelAdvanced(pins[i], group_channels_used[params->groups[i]], params->groups[i], pwm_frequency, _PWM_RES_BIT, false)){ + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Failed to configure pin:", pins[i]); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + params->channels[i] = static_cast(group_channels_used[params->groups[i]]); + } + SIMPLEFOC_DEBUG("EP32-DRV: 4PWM setup successful!"); return params; } +void IRAM_ATTR _writeDutyCycle(float dc, void* params, int index){ + ledc_set_duty_with_hpoint(((ESP32LEDCDriverParams*)params)->groups[index],((ESP32LEDCDriverParams*)params)->channels[index], _PWM_RES*dc, _PWM_RES/2.0*(1.0-dc)); + ledc_update_duty(((ESP32LEDCDriverParams*)params)->groups[index],((ESP32LEDCDriverParams*)params)->channels[index]); +} + +void IRAM_ATTR _writeDutyCycle1PWM(float dc_a, void* params){ + _writeDutyCycle(dc_a, params, 0); +} + -void _writeDutyCycle1PWM(float dc_a, void* params){ - ledcWrite(((ESP32LEDCDriverParams*)params)->channels[0], _constrain(_PWM_RES*dc_a, 0, _PWM_RES)); +void IRAM_ATTR _writeDutyCycle2PWM(float dc_a, float dc_b, void* params){ + _writeDutyCycle(dc_a, params, 0); + _writeDutyCycle(dc_b, params, 1); } -void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params){ - ledcWrite(((ESP32LEDCDriverParams*)params)->channels[0], _constrain(_PWM_RES*dc_a, 0, _PWM_RES)); - ledcWrite(((ESP32LEDCDriverParams*)params)->channels[1], _constrain(_PWM_RES*dc_b, 0, _PWM_RES)); +void IRAM_ATTR _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params){ + _writeDutyCycle(dc_a, params, 0); + _writeDutyCycle(dc_b, params, 1); + _writeDutyCycle(dc_c, params, 2); } -void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params){ - ledcWrite(((ESP32LEDCDriverParams*)params)->channels[0], _constrain(_PWM_RES*dc_a, 0, _PWM_RES)); - ledcWrite(((ESP32LEDCDriverParams*)params)->channels[1], _constrain(_PWM_RES*dc_b, 0, _PWM_RES)); - ledcWrite(((ESP32LEDCDriverParams*)params)->channels[2], _constrain(_PWM_RES*dc_c, 0, _PWM_RES)); +void IRAM_ATTR _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, void* params){ + _writeDutyCycle(dc_1a, params, 0); + _writeDutyCycle(dc_1b, params, 1); + _writeDutyCycle(dc_2a, params, 2); + _writeDutyCycle(dc_2b, params, 3); } +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 + + SIMPLEFOC_DEBUG("EP32-DRV: Configuring 6PWM"); + SIMPLEFOC_DEBUG("EP32-DRV: WARNING - 6PWM on LEDC is poorly supported and not tested, consider using MCPWM driver instead!"); + // check if enough channels available + int group = _findGroupWithChannelsAvailable(6); + if (group < 0){ + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Not enough channels available!"); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + SIMPLEFOC_DEBUG("EP32-DRV: 6PWM setup in group: ", (group)); + ESP32LEDCDriverParams* params = new ESP32LEDCDriverParams { + .channels = { static_cast(0)}, + .groups = { (ledc_mode_t)group }, + .pwm_frequency = pwm_frequency, + .dead_zone = dead_zone + }; + + int high_side_invert = SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH ? false : true; + int low_side_invert = SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH ? true : false; + + int pin_pairs[6][2] = { + {pinA_h, pinA_l}, + {pinB_h, pinB_l}, + {pinC_h, pinC_l} + }; + + for(int i = 0; i < 3; i++){ + group_channels_used[group]++; + if(!_ledcAttachChannelAdvanced(pin_pairs[i][0], group_channels_used[group], group, pwm_frequency, _PWM_RES_BIT, high_side_invert)){ + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Failed to configure pin:", pin_pairs[i][0]); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + + params->channels[2*i] = static_cast(group_channels_used[group]); + params->groups[2*i] = (ledc_mode_t)group; -void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, void* params){ - ledcWrite(((ESP32LEDCDriverParams*)params)->channels[0], _constrain(_PWM_RES*dc_1a, 0, _PWM_RES)); - ledcWrite(((ESP32LEDCDriverParams*)params)->channels[1], _constrain(_PWM_RES*dc_1b, 0, _PWM_RES)); - ledcWrite(((ESP32LEDCDriverParams*)params)->channels[2], _constrain(_PWM_RES*dc_2a, 0, _PWM_RES)); - ledcWrite(((ESP32LEDCDriverParams*)params)->channels[3], _constrain(_PWM_RES*dc_2b, 0, _PWM_RES)); + group_channels_used[group]++; + if(!_ledcAttachChannelAdvanced(pin_pairs[i][1], group_channels_used[group], group, pwm_frequency, _PWM_RES_BIT, low_side_invert)){ + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Failed to configure pin:", pin_pairs[i][0]); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + + params->channels[2*i+1] = static_cast(group_channels_used[group]); + params->groups[2*i+1] = (ledc_mode_t)group; + } + + SIMPLEFOC_DEBUG("EP32-DRV: 6PWM setup successful in group: ", (group)); + return params; } -#endif \ No newline at end of file +void IRAM_ATTR _setPwmPairDutyCycle( void* params, int ind_h, int ind_l, float val, float dead_time, PhaseState ps){ + float pwm_h = _constrain(val - (dead_time * 0.5), 0, 1.0); + float pwm_l = _constrain(val + (dead_time * 0.5), 0, 1.0); + + // determine the phase state and set the pwm accordingly + // deactivate phases if needed + if((ps == PhaseState::PHASE_OFF) || (ps == PhaseState::PHASE_LO)){ + _writeDutyCycle(0, params, ind_h); + }else{ + _writeDutyCycle(pwm_h, params, ind_h); + } + if((ps == PhaseState::PHASE_OFF) || (ps == PhaseState::PHASE_HI)){ + _writeDutyCycle(0, params, ind_l); + }else{ + _writeDutyCycle(pwm_l, params, ind_l); + } +} + +void IRAM_ATTR _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState *phase_state, void* params){ + _setPwmPairDutyCycle(params, 0, 1, dc_a, ((ESP32LEDCDriverParams*)params)->dead_zone, phase_state[0]); + _setPwmPairDutyCycle(params, 2, 3, dc_b, ((ESP32LEDCDriverParams*)params)->dead_zone, phase_state[1]); + _setPwmPairDutyCycle(params, 4, 5, dc_c, ((ESP32LEDCDriverParams*)params)->dead_zone, phase_state[2]); +} + +#endif diff --git a/src/drivers/hardware_specific/esp32/esp32_mcpwm_mcu.cpp b/src/drivers/hardware_specific/esp32/esp32_mcpwm_mcu.cpp new file mode 100644 index 00000000..e251ab84 --- /dev/null +++ b/src/drivers/hardware_specific/esp32/esp32_mcpwm_mcu.cpp @@ -0,0 +1,236 @@ +#include "esp32_driver_mcpwm.h" + +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) + +#pragma message("") +#pragma message("SimpleFOC: compiling for ESP32 MCPWM driver") +#pragma message("") + + +int esp32_gpio_nr(int pin) { + #if defined(BOARD_HAS_PIN_REMAP) && !defined(BOARD_USES_HW_GPIO_NUMBERS) + return digitalPinToGPIONumber(pin); + #else + return pin; + #endif +} + + +// function setting the high pwm frequency to the supplied pins +// - DC motor - 1PWM setting +// - hardware specific +void* _configure1PWM(long pwm_frequency, const int pinA) { + if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25hz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max + + int group, timer; + if(!_findBestGroup(1, pwm_frequency, &group, &timer)) { + SIMPLEFOC_ESP32_DRV_DEBUG("Not enough pins available for 1PWM!"); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 1PWM in group: "+String(group)+" on timer: "+String(timer)); + // configure the timer + int pins[1] = { esp32_gpio_nr(pinA) }; + return _configurePinsMCPWM(pwm_frequency, group, timer, 1, pins); +} + + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 2PWM setting +// - 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 25hz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max + + int group, timer; + int ret = _findBestGroup(2, pwm_frequency, &group, &timer); + if(!ret) { + SIMPLEFOC_ESP32_DRV_DEBUG("Not enough pins available for 2PWM!"); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + if(ret == 1){ + // configure the 2pwm on only one group + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 2PWM in group: "+String(group)+" on timer: "+String(timer)); + // configure the timer + int pins[2] = { esp32_gpio_nr(pinA), esp32_gpio_nr(pinB) }; + return _configurePinsMCPWM(pwm_frequency, group, timer, 2, pins); + }else{ + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 2PWM as two 1PWM drivers"); + ESP32MCPWMDriverParams* params[2]; + + // the code is a bit huge for what it does + // it just instantiates two 2PMW drivers and combines the returned params + int pins[2][1] = {{pinA}, {pinB}}; + for(int i =0; i<2; i++){ + int timer = _findLastTimer(i); //find last created timer in group i + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 1PWM in group: "+String(i)+" on timer: "+String(timer)); + void* p = _configurePinsMCPWM(pwm_frequency, i, timer, 1, pins[i]); + if(p == SIMPLEFOC_DRIVER_INIT_FAILED){ + SIMPLEFOC_ESP32_DRV_DEBUG("Error configuring 1PWM"); + return SIMPLEFOC_DRIVER_INIT_FAILED; + }else{ + params[i] = (ESP32MCPWMDriverParams*)p; + } + } + // combine the driver parameters + ESP32MCPWMDriverParams* ret_params = new ESP32MCPWMDriverParams{ + .pwm_frequency = params[0]->pwm_frequency, + .group_id = 2, // both groups + }; + for(int i =0; i<2; i++){ + ret_params->timers[i] = params[i]->timers[0]; + ret_params->oper[i] = params[i]->oper[0]; + ret_params->comparator[i] = params[i]->comparator[0]; + ret_params->generator[i] = params[i]->generator[0]; + } + ret_params->mcpwm_period = params[0]->mcpwm_period; + return ret_params; + } +} + +// function setting the high pwm frequency to the supplied pins +// - BLDC motor - 3PWM setting +// - 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 25hz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max + + int group, timer; + if(!_findBestGroup(3, pwm_frequency, &group, &timer)) { + SIMPLEFOC_ESP32_DRV_DEBUG("Not enough pins available for 3PWM!"); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 3PWM in group: "+String(group)+" on timer: "+String(timer)); + // configure the timer + int pins[3] = { esp32_gpio_nr(pinA), esp32_gpio_nr(pinB), esp32_gpio_nr(pinC) }; + return _configurePinsMCPWM(pwm_frequency, group, timer, 3, pins); +} + + + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 4PWM setting +// - 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 25hz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max + + int group, timer; + int ret = _findBestGroup(4, pwm_frequency, &group, &timer); + if(!ret) { + SIMPLEFOC_ESP32_DRV_DEBUG("Not enough pins available for 4PWM!"); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + if(ret == 1){ + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 4PWM in group: "+String(group)+" on timer: "+String(timer)); + // configure the timer + int pins[4] = {pinA, pinB, pinC, pinD}; + return _configurePinsMCPWM(pwm_frequency, group, timer, 4, pins); + }else{ + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 4PWM as two 2PWM drivers"); + ESP32MCPWMDriverParams* params[2]; + + // the code is a bit huge for what it does + // it just instantiates two 2PMW drivers and combines the returned params + int pins[2][2] = {{ esp32_gpio_nr(pinA), esp32_gpio_nr(pinB) },{ esp32_gpio_nr(pinC), esp32_gpio_nr(pinD) }}; + for(int i =0; i<2; i++){ + int timer = _findNextTimer(i); //find next available timer in group i + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 2PWM in group: "+String(i)+" on timer: "+String(timer)); + void* p = _configurePinsMCPWM(pwm_frequency, i, timer, 2, pins[i]); + if(p == SIMPLEFOC_DRIVER_INIT_FAILED){ + SIMPLEFOC_ESP32_DRV_DEBUG("Error configuring 2PWM"); + return SIMPLEFOC_DRIVER_INIT_FAILED; + }else{ + params[i] = (ESP32MCPWMDriverParams*)p; + } + } + // combine the driver parameters + ESP32MCPWMDriverParams* ret_params = new ESP32MCPWMDriverParams{ + .pwm_frequency = params[0]->pwm_frequency, + .group_id = 2, // both groups + .timers = {params[0]->timers[0], params[1]->timers[0]}, + .oper = {params[0]->oper[0], params[1]->oper[0]} + }; + for(int i =0; i<4; i++){ + ret_params->comparator[i] = params[(int)floor(i/2)]->comparator[i%2]; + ret_params->generator[i] = params[(int)floor(i/2)]->generator[i%2]; + } + ret_params->mcpwm_period = params[0]->mcpwm_period; + return ret_params; + } +} + + +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 25hz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max + + int group, timer; + if(!_findBestGroup(6, pwm_frequency, &group, &timer)) { + SIMPLEFOC_ESP32_DRV_DEBUG("Not enough pins available for 6PWM!"); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 6PWM in group: "+String(group)+" on timer: "+String(timer)); + // configure the timer + int pins[6] = { esp32_gpio_nr(pinA_h), esp32_gpio_nr(pinA_l), esp32_gpio_nr(pinB_h), esp32_gpio_nr(pinB_l), esp32_gpio_nr(pinC_h), esp32_gpio_nr(pinC_l) }; + return _configure6PWMPinsMCPWM(pwm_frequency, group, timer, dead_zone, pins); +} + +// function setting the pwm duty cycle to the hardware +// - BLDC motor - 3PWM setting +void IRAM_ATTR _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params){ + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[0], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_a); + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[1], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_b); + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[2], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_c); +} + +// function setting the pwm duty cycle to the hardware +// - DCMotor -1PWM setting +// - hardware specific +void IRAM_ATTR _writeDutyCycle1PWM(float dc_a, void* params){ + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[0], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_a); +} + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 2PWM setting +// - hardware specific +void IRAM_ATTR _writeDutyCycle2PWM(float dc_a, float dc_b, void* params){ + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[0], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_a); + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[1], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_b); +} + + + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 4PWM setting +// - hardware specific +void IRAM_ATTR _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, void* params){ + // se the PWM on the slot timers + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[0], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_1a); + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[1], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_1b); + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[2], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_2a); + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[3], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_2b); +} + +void IRAM_ATTR _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState *phase_state, void* params){ +#if SIMPLEFOC_ESP32_HW_DEADTIME == true + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[0], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_a); + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[1], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_b); + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[2], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_c); + + // set the phase state + _forcePhaseState(((ESP32MCPWMDriverParams*)params)->generator[0], ((ESP32MCPWMDriverParams*)params)->generator[1], phase_state[0]); + _forcePhaseState(((ESP32MCPWMDriverParams*)params)->generator[2], ((ESP32MCPWMDriverParams*)params)->generator[3], phase_state[1]); + _forcePhaseState(((ESP32MCPWMDriverParams*)params)->generator[4], ((ESP32MCPWMDriverParams*)params)->generator[5], phase_state[2]); +#else + uint32_t period = ((ESP32MCPWMDriverParams*)params)->mcpwm_period; + const float dead_zone = (float)((ESP32MCPWMDriverParams*)params)->dead_zone * 0.5f; + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[0], period, (phase_state[0] == PHASE_ON || phase_state[0] == PHASE_HI) ? dc_a-dead_zone : 0.0f); + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[1], period, (phase_state[0] == PHASE_ON || phase_state[0] == PHASE_LO) ? dc_a+dead_zone : 1.0f); + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[2], period, (phase_state[1] == PHASE_ON || phase_state[1] == PHASE_HI) ? dc_b-dead_zone : 0.0f); + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[3], period, (phase_state[1] == PHASE_ON || phase_state[1] == PHASE_LO) ? dc_b+dead_zone : 1.0f); + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[4], period, (phase_state[2] == PHASE_ON || phase_state[2] == PHASE_HI) ? dc_c-dead_zone : 0.0f); + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[5], period, (phase_state[2] == PHASE_ON || phase_state[2] == PHASE_LO) ? dc_c+dead_zone : 1.0f); +#endif +} +#endif diff --git a/src/drivers/hardware_specific/esp32/esp32_mcu.cpp b/src/drivers/hardware_specific/esp32/esp32_mcu.cpp deleted file mode 100644 index 0dc23c10..00000000 --- a/src/drivers/hardware_specific/esp32/esp32_mcu.cpp +++ /dev/null @@ -1,431 +0,0 @@ -#include "esp32_driver_mcpwm.h" - -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) - -#ifndef SIMPLEFOC_ESP32_HW_DEADTIME - #define SIMPLEFOC_ESP32_HW_DEADTIME true // TODO: Change to false when sw-deadtime & phase_state is approved ready for general use. -#endif - -// define bldc motor slots array -bldc_3pwm_motor_slots_t esp32_bldc_3pwm_motor_slots[4] = { - {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM0A, MCPWM1A, MCPWM2A}, // 1st motor will be MCPWM0 channel A - {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_B, MCPWM0B, MCPWM1B, MCPWM2B}, // 2nd motor will be MCPWM0 channel B - {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM0A, MCPWM1A, MCPWM2A}, // 3rd motor will be MCPWM1 channel A - {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_B, MCPWM0B, MCPWM1B, MCPWM2B} // 4th motor will be MCPWM1 channel B - }; - -// define stepper motor slots array -bldc_6pwm_motor_slots_t esp32_bldc_6pwm_motor_slots[2] = { - {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM2A, MCPWM0B, MCPWM1B, MCPWM2B}, // 1st motor will be on MCPWM0 - {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM2A, MCPWM0B, MCPWM1B, MCPWM2B}, // 1st motor will be on MCPWM0 - }; - -// define 4pwm stepper motor slots array -stepper_4pwm_motor_slots_t esp32_stepper_4pwm_motor_slots[2] = { - {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM0B, MCPWM1B}, // 1st motor will be on MCPWM0 - {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM0B, MCPWM1B}, // 1st motor will be on MCPWM1 - }; - -// define 2pwm stepper motor slots array -stepper_2pwm_motor_slots_t esp32_stepper_2pwm_motor_slots[4] = { - {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM0A, MCPWM1A}, // 1st motor will be MCPWM0 channel A - {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_B, MCPWM0B, MCPWM1B}, // 2nd motor will be MCPWM0 channel B - {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM0A, MCPWM1A}, // 3rd motor will be MCPWM1 channel A - {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_B, MCPWM0B, MCPWM1B} // 4th motor will be MCPWM1 channel B - }; - - -// configuring high frequency pwm timer -// a lot of help from this post from Paul Gauld -// https://hackaday.io/project/169905-esp-32-bldc-robot-actuator-controller -// a short tutorial for v2.0.1+ -// https://kzhead.info/sun/q6uFktWgkYeMeZ8/esp32-arduino.html -// -void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm_unit_t mcpwm_unit, float dead_zone = NOT_SET){ - - mcpwm_config_t pwm_config; - pwm_config.counter_mode = MCPWM_UP_DOWN_COUNTER; // Up-down counter (triangle wave) - pwm_config.duty_mode = (_isset(dead_zone) || SIMPLEFOC_PWM_ACTIVE_HIGH == true) ? MCPWM_DUTY_MODE_0 : MCPWM_DUTY_MODE_1; // Normally Active HIGH (MCPWM_DUTY_MODE_0) - pwm_config.frequency = 2*pwm_frequency; // set the desired freq - just a placeholder for now https://github.com/simplefoc/Arduino-FOC/issues/76 - mcpwm_init(mcpwm_unit, MCPWM_TIMER_0, &pwm_config); //Configure PWM0A & PWM0B with above settings - mcpwm_init(mcpwm_unit, MCPWM_TIMER_1, &pwm_config); //Configure PWM1A & PWM1B with above settings - mcpwm_init(mcpwm_unit, MCPWM_TIMER_2, &pwm_config); //Configure PWM2A & PWM2B with above settings - - if (_isset(dead_zone)){ - // dead zone is configured - - // When using hardware deadtime, setting the phase_state parameter is not supported. - #if SIMPLEFOC_ESP32_HW_DEADTIME == true - float dead_time = (float)(_MCPWM_FREQ / (pwm_frequency)) * dead_zone; - mcpwm_deadtime_type_t pwm_mode; - if ((SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH == true) && (SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH == true)) {pwm_mode = MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE;} // Normal, noninverting driver - else if ((SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH == true) && (SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH == false)){pwm_mode = MCPWM_ACTIVE_HIGH_MODE;} // Inverted lowside driver - else if ((SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH == false) && (SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH == true)) {pwm_mode = MCPWM_ACTIVE_LOW_MODE;} // Inverted highside driver - else if ((SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH == false) && (SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH == false)){pwm_mode = MCPWM_ACTIVE_LOW_COMPLIMENT_MODE;} // Inverted low- & highside driver. Caution: This may short the FETs on reset of the ESP32, as both pins get pulled low! - mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_0, pwm_mode, dead_time/2.0, dead_time/2.0); - mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_1, pwm_mode, dead_time/2.0, dead_time/2.0); - mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_2, pwm_mode, dead_time/2.0, dead_time/2.0); - #else // Software deadtime - for (int i = 0; i < 3; i++){ - if (SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH == true) {mcpwm_set_duty_type(mcpwm_unit, (mcpwm_timer_t) i, MCPWM_GEN_A, MCPWM_DUTY_MODE_0);} // Normal, noninverted highside - else if (SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH == false) {mcpwm_set_duty_type(mcpwm_unit, (mcpwm_timer_t) i, MCPWM_GEN_A, MCPWM_DUTY_MODE_1);} // Inverted highside driver - - if (SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH == true) {mcpwm_set_duty_type(mcpwm_unit, (mcpwm_timer_t) i, MCPWM_GEN_B, MCPWM_DUTY_MODE_1);} // Normal, complementary lowside - else if (SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH == false) {mcpwm_set_duty_type(mcpwm_unit, (mcpwm_timer_t) i, MCPWM_GEN_B, MCPWM_DUTY_MODE_0);} // Inverted lowside driver - } - #endif - - } - _delay(100); - - mcpwm_stop(mcpwm_unit, MCPWM_TIMER_0); - mcpwm_stop(mcpwm_unit, MCPWM_TIMER_1); - mcpwm_stop(mcpwm_unit, MCPWM_TIMER_2); - - // manual configuration due to the lack of config flexibility in mcpwm_init() - mcpwm_num->clk_cfg.clk_prescale = 0; - // calculate prescaler and period - // step 1: calculate the prescaler using the default pwm resolution - // prescaler = bus_freq / (pwm_freq * default_pwm_res) - 1 - int prescaler = ceil((double)_MCPWM_FREQ / (double)_PWM_RES_DEF / 2.0f / (double)pwm_frequency) - 1; - // constrain prescaler - prescaler = _constrain(prescaler, 0, 128); - // now calculate the real resolution timer period necessary (pwm resolution) - // pwm_res = bus_freq / (pwm_freq * (prescaler + 1)) - int resolution_corrected = (double)_MCPWM_FREQ / 2.0f / (double)pwm_frequency / (double)(prescaler + 1); - // if pwm resolution too low lower the prescaler - if(resolution_corrected < _PWM_RES_MIN && prescaler > 0 ) - resolution_corrected = (double)_MCPWM_FREQ / 2.0f / (double)pwm_frequency / (double)(--prescaler + 1); - resolution_corrected = _constrain(resolution_corrected, _PWM_RES_MIN, _PWM_RES_MAX); - - // set prescaler - mcpwm_num->timer[0].timer_cfg0.timer_prescale = prescaler; - mcpwm_num->timer[1].timer_cfg0.timer_prescale = prescaler; - mcpwm_num->timer[2].timer_cfg0.timer_prescale = prescaler; - _delay(1); - //set period - mcpwm_num->timer[0].timer_cfg0.timer_period = resolution_corrected; - mcpwm_num->timer[1].timer_cfg0.timer_period = resolution_corrected; - mcpwm_num->timer[2].timer_cfg0.timer_period = resolution_corrected; - _delay(1); - mcpwm_num->timer[0].timer_cfg0.timer_period_upmethod = 0; - mcpwm_num->timer[1].timer_cfg0.timer_period_upmethod = 0; - mcpwm_num->timer[2].timer_cfg0.timer_period_upmethod = 0; - _delay(1); - // _delay(1); - //restart the timers - mcpwm_start(mcpwm_unit, MCPWM_TIMER_0); - mcpwm_start(mcpwm_unit, MCPWM_TIMER_1); - mcpwm_start(mcpwm_unit, MCPWM_TIMER_2); - _delay(1); - - mcpwm_sync_config_t sync_conf = { - .sync_sig = MCPWM_SELECT_TIMER0_SYNC, - .timer_val = 0, - .count_direction = MCPWM_TIMER_DIRECTION_UP - }; - mcpwm_sync_configure(mcpwm_unit, MCPWM_TIMER_0, &sync_conf); - mcpwm_sync_configure(mcpwm_unit, MCPWM_TIMER_1, &sync_conf); - mcpwm_sync_configure(mcpwm_unit, MCPWM_TIMER_2, &sync_conf); - - // Enable sync event for all timers to be the TEZ of timer0 - mcpwm_set_timer_sync_output(mcpwm_unit, MCPWM_TIMER_0, MCPWM_SWSYNC_SOURCE_TEZ); -} - -// function setting the high pwm frequency to the supplied pins -// - Stepper motor - 2PWM setting -// - hardware speciffic -// supports Arudino/ATmega328, STM32 and ESP32 -void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { - if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25hz - else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max - - stepper_2pwm_motor_slots_t m_slot = {}; - - // determine which motor are we connecting - // and set the appropriate configuration parameters - int slot_num; - for(slot_num = 0; slot_num < 4; slot_num++){ - if(esp32_stepper_2pwm_motor_slots[slot_num].pin1pwm == _EMPTY_SLOT){ // put the new motor in the first empty slot - esp32_stepper_2pwm_motor_slots[slot_num].pin1pwm = pinA; - m_slot = esp32_stepper_2pwm_motor_slots[slot_num]; - break; - } - } - - // disable all the slots with the same MCPWM - // disable 3pwm bldc motor which would go in the same slot - esp32_bldc_3pwm_motor_slots[slot_num].pinA = _TAKEN_SLOT; - if( slot_num < 2 ){ - // slot 0 of the 4pwm stepper - esp32_stepper_4pwm_motor_slots[0].pin1A = _TAKEN_SLOT; - // slot 0 of the 6pwm bldc - esp32_bldc_6pwm_motor_slots[0].pinAH = _TAKEN_SLOT; - }else{ - // slot 1 of the stepper - esp32_stepper_4pwm_motor_slots[1].pin1A = _TAKEN_SLOT; - // slot 1 of the 6pwm bldc - esp32_bldc_6pwm_motor_slots[1].pinAH = _TAKEN_SLOT; - } - // configure pins - mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_a, pinA); - mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_b, pinB); - - // configure the timer - _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit); - - ESP32MCPWMDriverParams* params = new ESP32MCPWMDriverParams { - .pwm_frequency = pwm_frequency, - .mcpwm_dev = m_slot.mcpwm_num, - .mcpwm_unit = m_slot.mcpwm_unit, - .mcpwm_operator1 = m_slot.mcpwm_operator - }; - return params; -} - - -// function setting the high pwm frequency to the supplied pins -// - BLDC motor - 3PWM setting -// - hardware speciffic -// supports Arudino/ATmega328, STM32 and ESP32 -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 25hz - else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max - - bldc_3pwm_motor_slots_t m_slot = {}; - - // determine which motor are we connecting - // and set the appropriate configuration parameters - int slot_num; - for(slot_num = 0; slot_num < 4; slot_num++){ - if(esp32_bldc_3pwm_motor_slots[slot_num].pinA == _EMPTY_SLOT){ // put the new motor in the first empty slot - esp32_bldc_3pwm_motor_slots[slot_num].pinA = pinA; - m_slot = esp32_bldc_3pwm_motor_slots[slot_num]; - break; - } - } - // disable all the slots with the same MCPWM - // disable 2pwm steppr motor which would go in the same slot - esp32_stepper_2pwm_motor_slots[slot_num].pin1pwm = _TAKEN_SLOT; - if( slot_num < 2 ){ - // slot 0 of the 4pwm stepper - esp32_stepper_4pwm_motor_slots[0].pin1A = _TAKEN_SLOT; - // slot 0 of the 6pwm bldc - esp32_bldc_6pwm_motor_slots[0].pinAH = _TAKEN_SLOT; - }else{ - // slot 1 of the stepper - esp32_stepper_4pwm_motor_slots[1].pin1A = _TAKEN_SLOT; - // slot 1 of the 6pwm bldc - esp32_bldc_6pwm_motor_slots[1].pinAH = _TAKEN_SLOT; - } - // configure pins - mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_a, pinA); - mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_b, pinB); - mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_c, pinC); - - // configure the timer - _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit); - - ESP32MCPWMDriverParams* params = new ESP32MCPWMDriverParams { - .pwm_frequency = pwm_frequency, - .mcpwm_dev = m_slot.mcpwm_num, - .mcpwm_unit = m_slot.mcpwm_unit, - .mcpwm_operator1 = m_slot.mcpwm_operator - }; - return params; -} - - -// function setting the high pwm frequency to the supplied pins -// - Stepper motor - 4PWM setting -// - hardware speciffic -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 25hz - else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max - stepper_4pwm_motor_slots_t m_slot = {}; - // determine which motor are we connecting - // and set the appropriate configuration parameters - int slot_num; - for(slot_num = 0; slot_num < 2; slot_num++){ - if(esp32_stepper_4pwm_motor_slots[slot_num].pin1A == _EMPTY_SLOT){ // put the new motor in the first empty slot - esp32_stepper_4pwm_motor_slots[slot_num].pin1A = pinA; - m_slot = esp32_stepper_4pwm_motor_slots[slot_num]; - break; - } - } - // disable all the slots with the same MCPWM - if( slot_num == 0 ){ - // slots 0 and 1 of the 3pwm bldc - esp32_bldc_3pwm_motor_slots[0].pinA = _TAKEN_SLOT; - esp32_bldc_3pwm_motor_slots[1].pinA = _TAKEN_SLOT; - // slots 0 and 1 of the 2pwm stepper taken - esp32_stepper_2pwm_motor_slots[0].pin1pwm = _TAKEN_SLOT; - esp32_stepper_2pwm_motor_slots[1].pin1pwm = _TAKEN_SLOT; - // slot 0 of the 6pwm bldc - esp32_bldc_6pwm_motor_slots[0].pinAH = _TAKEN_SLOT; - }else{ - // slots 2 and 3 of the 3pwm bldc - esp32_bldc_3pwm_motor_slots[2].pinA = _TAKEN_SLOT; - esp32_bldc_3pwm_motor_slots[3].pinA = _TAKEN_SLOT; - // slots 2 and 3 of the 2pwm stepper taken - esp32_stepper_2pwm_motor_slots[2].pin1pwm = _TAKEN_SLOT; - esp32_stepper_2pwm_motor_slots[3].pin1pwm = _TAKEN_SLOT; - // slot 1 of the 6pwm bldc - esp32_bldc_6pwm_motor_slots[1].pinAH = _TAKEN_SLOT; - } - - // configure pins - mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_1a, pinA); - mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_1b, pinB); - mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_2a, pinC); - mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_2b, pinD); - - // configure the timer - _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit); - - ESP32MCPWMDriverParams* params = new ESP32MCPWMDriverParams { - .pwm_frequency = pwm_frequency, - .mcpwm_dev = m_slot.mcpwm_num, - .mcpwm_unit = m_slot.mcpwm_unit, - .mcpwm_operator1 = m_slot.mcpwm_operator1, - .mcpwm_operator2 = m_slot.mcpwm_operator2 - }; - return params; -} - - - - -// function setting the pwm duty cycle to the hardware -// - Stepper motor - 2PWM setting -// - hardware speciffic -// ESP32 uses MCPWM -void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params){ - // se the PWM on the slot timers - // transform duty cycle from [0,1] to [0,100] - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_0, ((ESP32MCPWMDriverParams*)params)->mcpwm_operator1, dc_a*100.0); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_1, ((ESP32MCPWMDriverParams*)params)->mcpwm_operator1, dc_b*100.0); -} - - - - -// function setting the pwm duty cycle to the hardware -// - BLDC motor - 3PWM setting -// - hardware speciffic -// ESP32 uses MCPWM -void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params){ - // se the PWM on the slot timers - // transform duty cycle from [0,1] to [0,100] - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_0, ((ESP32MCPWMDriverParams*)params)->mcpwm_operator1, dc_a*100.0); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_1, ((ESP32MCPWMDriverParams*)params)->mcpwm_operator1, dc_b*100.0); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_2, ((ESP32MCPWMDriverParams*)params)->mcpwm_operator1, dc_c*100.0); -} - - - - -// function setting the pwm duty cycle to the hardware -// - Stepper motor - 4PWM setting -// - hardware speciffic -// ESP32 uses MCPWM -void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, void* params){ - // se the PWM on the slot timers - // transform duty cycle from [0,1] to [0,100] - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_0, ((ESP32MCPWMDriverParams*)params)->mcpwm_operator1, dc_1a*100.0); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_1, ((ESP32MCPWMDriverParams*)params)->mcpwm_operator1, dc_1b*100.0); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_0, ((ESP32MCPWMDriverParams*)params)->mcpwm_operator2, dc_2a*100.0); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_1, ((ESP32MCPWMDriverParams*)params)->mcpwm_operator2, dc_2b*100.0); -} - - - - -// Configuring PWM frequency, resolution and alignment -// - BLDC driver - 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 = 20000; // default frequency 20khz - centered pwm has twice lower frequency - else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max - centered pwm has twice lower frequency - bldc_6pwm_motor_slots_t m_slot = {}; - // determine which motor are we connecting - // and set the appropriate configuration parameters - int slot_num; - for(slot_num = 0; slot_num < 2; slot_num++){ - if(esp32_bldc_6pwm_motor_slots[slot_num].pinAH == _EMPTY_SLOT){ // put the new motor in the first empty slot - esp32_bldc_6pwm_motor_slots[slot_num].pinAH = pinA_h; - m_slot = esp32_bldc_6pwm_motor_slots[slot_num]; - break; - } - } - // if no slots available - if(slot_num >= 2) return SIMPLEFOC_DRIVER_INIT_FAILED; - - // disable all the slots with the same MCPWM - if( slot_num == 0 ){ - // slots 0 and 1 of the 3pwm bldc - esp32_bldc_3pwm_motor_slots[0].pinA = _TAKEN_SLOT; - esp32_bldc_3pwm_motor_slots[1].pinA = _TAKEN_SLOT; - // slot 0 of the 6pwm bldc - esp32_stepper_4pwm_motor_slots[0].pin1A = _TAKEN_SLOT; - }else{ - // slots 2 and 3 of the 3pwm bldc - esp32_bldc_3pwm_motor_slots[2].pinA = _TAKEN_SLOT; - esp32_bldc_3pwm_motor_slots[3].pinA = _TAKEN_SLOT; - // slot 1 of the 6pwm bldc - esp32_stepper_4pwm_motor_slots[1].pin1A = _TAKEN_SLOT; - } - - // configure pins - mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_ah, pinA_h); - mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_al, pinA_l); - mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_bh, pinB_h); - mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_bl, pinB_l); - mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_ch, pinC_h); - mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_cl, pinC_l); - - // configure the timer - _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit, dead_zone); - // return - ESP32MCPWMDriverParams* params = new ESP32MCPWMDriverParams { - .pwm_frequency = pwm_frequency, - .mcpwm_dev = m_slot.mcpwm_num, - .mcpwm_unit = m_slot.mcpwm_unit, - .mcpwm_operator1 = m_slot.mcpwm_operator1, - .mcpwm_operator2 = m_slot.mcpwm_operator2, - .deadtime = _isset(dead_zone) ? dead_zone : 0 - }; - return params; -} - - - - -// Function setting the duty cycle to the pwm pin (ex. analogWrite()) -// - BLDC driver - 6PWM setting -// - hardware specific -void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState *phase_state, void* params){ - // set the PWM on the slot timers - // transform duty cycle from [0,1] to [0,100.0] - #if SIMPLEFOC_ESP32_HW_DEADTIME == true - // Hardware deadtime does deadtime insertion internally - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_A, dc_a*100.0f); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_B, dc_a*100.0f); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_A, dc_b*100.0f); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_B, dc_b*100.0f); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_A, dc_c*100.0f); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_B, dc_c*100.0f); - _UNUSED(phase_state); - #else - float deadtime = 0.5f*((ESP32MCPWMDriverParams*)params)->deadtime; - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_A, (phase_state[0] == PHASE_ON || phase_state[0] == PHASE_HI) ? _constrain(dc_a-deadtime, 0.0f, 1.0f) * 100.0f : 0); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_B, (phase_state[0] == PHASE_ON || phase_state[0] == PHASE_LO) ? _constrain(dc_a+deadtime, 0.0f, 1.0f) * 100.0f : 100.0f); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_A, (phase_state[1] == PHASE_ON || phase_state[1] == PHASE_HI) ? _constrain(dc_b-deadtime, 0.0f, 1.0f) * 100.0f : 0); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_B, (phase_state[1] == PHASE_ON || phase_state[1] == PHASE_LO) ? _constrain(dc_b+deadtime, 0.0f, 1.0f) * 100.0f : 100.0f); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_A, (phase_state[2] == PHASE_ON || phase_state[2] == PHASE_HI) ? _constrain(dc_c-deadtime, 0.0f, 1.0f) * 100.0f : 0); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_B, (phase_state[2] == PHASE_ON || phase_state[2] == PHASE_LO) ? _constrain(dc_c+deadtime, 0.0f, 1.0f) * 100.0f : 100.0f); - #endif -} - -#endif diff --git a/src/drivers/hardware_specific/esp32/mcpwm_private.h b/src/drivers/hardware_specific/esp32/mcpwm_private.h new file mode 100644 index 00000000..dbf48970 --- /dev/null +++ b/src/drivers/hardware_specific/esp32/mcpwm_private.h @@ -0,0 +1,82 @@ +/* + * This is a private declaration of different MCPWM structures and types. + * It has been copied from: https://github.com/espressif/esp-idf/blob/v5.1.4/components/driver/mcpwm/mcpwm_private.h + * + * extracted by askuric 16.06.2024 + * + * SPDX-FileCopyrightText: 2022-2023 Espressif Systems (Shanghai) CO LTD + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#ifndef MCPWM_PRIVATE_H +#define MCPWM_PRIVATE_H + + +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) + +#include "freertos/FreeRTOS.h" +#include "esp_intr_alloc.h" +#include "esp_heap_caps.h" +#include "esp_pm.h" +#include "soc/soc_caps.h" +#include "hal/mcpwm_hal.h" +#include "hal/mcpwm_types.h" +#include "driver/mcpwm_types.h" + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct mcpwm_group_t mcpwm_group_t; +typedef struct mcpwm_timer_t mcpwm_timer_t; +typedef struct mcpwm_cap_timer_t mcpwm_cap_timer_t; +typedef struct mcpwm_oper_t mcpwm_oper_t; +typedef struct mcpwm_gpio_fault_t mcpwm_gpio_fault_t; +typedef struct mcpwm_gpio_sync_src_t mcpwm_gpio_sync_src_t; +typedef struct mcpwm_timer_sync_src_t mcpwm_timer_sync_src_t; + +struct mcpwm_group_t { + int group_id; // group ID, index from 0 + int intr_priority; // MCPWM interrupt priority + mcpwm_hal_context_t hal; // HAL instance is at group level + portMUX_TYPE spinlock; // group level spinlock + uint32_t prescale; // group prescale + uint32_t resolution_hz; // MCPWM group clock resolution: clock_src_hz / clock_prescale = resolution_hz + esp_pm_lock_handle_t pm_lock; // power management lock + soc_module_clk_t clk_src; // peripheral source clock + mcpwm_cap_timer_t *cap_timer; // mcpwm capture timers + mcpwm_timer_t *timers[SOC_MCPWM_TIMERS_PER_GROUP]; // mcpwm timer array + mcpwm_oper_t *operators[SOC_MCPWM_OPERATORS_PER_GROUP]; // mcpwm operator array + mcpwm_gpio_fault_t *gpio_faults[SOC_MCPWM_GPIO_FAULTS_PER_GROUP]; // mcpwm fault detectors array + mcpwm_gpio_sync_src_t *gpio_sync_srcs[SOC_MCPWM_GPIO_SYNCHROS_PER_GROUP]; // mcpwm gpio sync array +}; + +typedef enum { + MCPWM_TIMER_FSM_INIT, + MCPWM_TIMER_FSM_ENABLE, +} mcpwm_timer_fsm_t; + +struct mcpwm_timer_t { + int timer_id; // timer ID, index from 0 + mcpwm_group_t *group; // which group the timer belongs to + mcpwm_timer_fsm_t fsm; // driver FSM + portMUX_TYPE spinlock; // spin lock + intr_handle_t intr; // interrupt handle + uint32_t resolution_hz; // resolution of the timer + uint32_t peak_ticks; // peak ticks that the timer could reach to + mcpwm_timer_sync_src_t *sync_src; // timer sync_src + mcpwm_timer_count_mode_t count_mode; // count mode + mcpwm_timer_event_cb_t on_full; // callback function when MCPWM timer counts to peak value + mcpwm_timer_event_cb_t on_empty; // callback function when MCPWM timer counts to zero + mcpwm_timer_event_cb_t on_stop; // callback function when MCPWM timer stops + void *user_data; // user data which would be passed to the timer callbacks +}; + +#ifdef __cplusplus +} +#endif + +#endif + +#endif /* MCPWM_PRIVATE_H */ \ No newline at end of file diff --git a/src/drivers/hardware_specific/portenta_h7_mcu.cpp b/src/drivers/hardware_specific/portenta_h7_mcu.cpp index ad16646a..e9807609 100644 --- a/src/drivers/hardware_specific/portenta_h7_mcu.cpp +++ b/src/drivers/hardware_specific/portenta_h7_mcu.cpp @@ -1,7 +1,7 @@ #include "../hardware_api.h" -#if defined(TARGET_PORTENTA_H7) +#if defined(TARGET_PORTENTA_H7) && false #pragma message("") diff --git a/src/drivers/hardware_specific/rp2040/rp2040_mcu.cpp b/src/drivers/hardware_specific/rp2040/rp2040_mcu.cpp index f187fbef..03b4a894 100644 --- a/src/drivers/hardware_specific/rp2040/rp2040_mcu.cpp +++ b/src/drivers/hardware_specific/rp2040/rp2040_mcu.cpp @@ -6,20 +6,23 @@ #include "./rp2040_mcu.h" -#if defined(TARGET_RP2040) +#if defined(TARGET_RP2040) || defined(TARGET_RP2350) #pragma message("") -#pragma message("SimpleFOC: compiling for RP2040") +#pragma message("SimpleFOC: compiling for RP2040/RP2350") #pragma message("") - +#if !defined(SIMPLEFOC_DEBUG_RP2040) #define SIMPLEFOC_DEBUG_RP2040 +#endif #include "../../hardware_api.h" #include "hardware/pwm.h" #include "hardware/clocks.h" +#if defined(USE_ARDUINO_PINOUT) #include +#endif #define _PWM_FREQUENCY 24000 #define _PWM_FREQUENCY_MAX 66000 @@ -35,7 +38,11 @@ uint16_t wrapvalues[NUM_PWM_SLICES]; // TODO add checks which channels are already used... void setupPWM(int pin_nr, long pwm_frequency, bool invert, RP2040DriverParams* params, uint8_t index) { + #if defined(USE_ARDUINO_PINOUT) uint pin = (uint)digitalPinToPinName(pin_nr); // we could check for -DBOARD_HAS_PIN_REMAP ? + #else + uint pin = (uint)pin_nr; + #endif gpio_set_function(pin, GPIO_FUNC_PWM); uint slice = pwm_gpio_to_slice_num(pin); uint chan = pwm_gpio_to_channel(pin); @@ -86,7 +93,7 @@ void syncSlices() { pwm_set_counter(i, 0); } // enable all slices - pwm_set_mask_enabled(0xFF); + pwm_set_mask_enabled(0xFFF); } diff --git a/src/drivers/hardware_specific/rp2040/rp2040_mcu.h b/src/drivers/hardware_specific/rp2040/rp2040_mcu.h index bbfb3873..3dab10bb 100644 --- a/src/drivers/hardware_specific/rp2040/rp2040_mcu.h +++ b/src/drivers/hardware_specific/rp2040/rp2040_mcu.h @@ -4,7 +4,7 @@ #include "Arduino.h" -#if defined(TARGET_RP2040) +#if defined(TARGET_RP2040) || defined(TARGET_RP2350) diff --git a/src/drivers/hardware_specific/silabs/efr32_mcu.cpp b/src/drivers/hardware_specific/silabs/efr32_mcu.cpp new file mode 100644 index 00000000..2965a5d6 --- /dev/null +++ b/src/drivers/hardware_specific/silabs/efr32_mcu.cpp @@ -0,0 +1,368 @@ +// File: efr32_mcu.cpp + +#if defined(ARDUINO_ARCH_SILABS) + +#include +#include +#include "efr32_pwm.h" +#include "efr32_mcu.h" + +void _getPrsSourceAndUnderflowSignal( + TIMER_TypeDef *timer, + uint32_t *source, + uint32_t *signal +) { + if (!timer || !source || !signal) return; + + switch ((uint32_t) timer) { + case TIMER0_BASE: + *source = PRS_ASYNC_CH_CTRL_SOURCESEL_TIMER0; + *signal = PRS_ASYNC_CH_CTRL_SIGSEL_TIMER0UF; + break; + + case TIMER1_BASE: + *source = PRS_ASYNC_CH_CTRL_SOURCESEL_TIMER1; + *signal = PRS_ASYNC_CH_CTRL_SIGSEL_TIMER1UF; + break; + + case TIMER2_BASE: + *source = PRS_ASYNC_CH_CTRL_SOURCESEL_TIMER2; + *signal = PRS_ASYNC_CH_CTRL_SIGSEL_TIMER2UF; + break; + + default: + break; + } +} + +// Callbacks +static void _alignPWMTimers( + TIMER_InitCC_TypeDef *initCC, + void *params +) { + EFR32PwmInstance *p = (EFR32PwmInstance*)params; + if (!p || !initCC) return; + + CMU_ClockEnable(cmuClock_PRS, true); + + uint32_t prsSource, prsSignal; + + initCC->prsInputType = timerPrsInputAsyncPulse; + initCC->prsInput = true; + initCC->prsSel = SILABS_PWM_PRS_CHANNEL; + + _getPrsSourceAndUnderflowSignal(p->h.timer, &prsSource, &prsSignal); + PRS_SourceAsyncSignalSet(SILABS_PWM_PRS_CHANNEL, prsSource, prsSignal); +} + +static void _configPWMMode( + TIMER_Init_TypeDef *init, + void *params +) { + if (!init) return; + _UNUSED(params); + init->mode = timerModeUpDown; +} + +static void _alignPWMStart( + TIMER_Init_TypeDef *init, + void *params +) { + if (!init) return; + _UNUSED(params); + init->mode = timerModeUpDown; + init->riseAction = timerInputActionReloadStart; +} + +static void _setSinglePhaseState(EFR32PwmInstance *inst, PhaseState state) { + if (!inst) return; + + switch (state) { + case PhaseState::PHASE_OFF: + pwmOff(inst); + break; + + default: + pwmOn(inst); + break; + } +} + +void *_configure1PWM(long pwm_frequency, const int pinA) { + EFR32DriverParams* params = new EFR32DriverParams; + if (!params) return SIMPLEFOC_DRIVER_INIT_FAILED; + + if (!pwm_frequency || !_isset(pwm_frequency)) pwm_frequency = SILABS_DEFAULT_PWM_FREQUENCY; + else pwm_frequency = _constrain(pwm_frequency, 0, SILABS_DEFAULT_PWM_FREQUENCY); + + params->pwm_frequency = pwm_frequency; + params->noPwmChannel = 1; + + // Ensure all PWMs use the same TIMER instance + pwmHiConfig(¶ms->inst[0], SILABS_DEFAULT_PWM_PERPHERAL, pinA, 0); + + // Initialize PWM + EFR32PwmConfig pwmConfig; + pwmConfig.frequency = pwm_frequency; + pwmConfig.polarity = PWM_P_ACTIVE_LOW; + pwmConfig.outInvert = true; + + pwmHiInit(¶ms->inst[0], &pwmConfig, NULL, NULL); + + // PWM On + pwmHiOn(¶ms->inst[0]); + + // Start PWM + pwmStart(¶ms->inst[0], _configPWMMode, NULL); + + return params; +} + +void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { + EFR32DriverParams *params = new EFR32DriverParams; + if (!params) return SIMPLEFOC_DRIVER_INIT_FAILED; + + if (!pwm_frequency || !_isset(pwm_frequency)) pwm_frequency = SILABS_DEFAULT_PWM_FREQUENCY; + else pwm_frequency = _constrain(pwm_frequency, 0, SILABS_DEFAULT_PWM_FREQUENCY); + + params->pwm_frequency = pwm_frequency; + params->noPwmChannel = 2; + + // Ensure all PWMs use the same TIMER instance + pwmHiConfig(¶ms->inst[0], SILABS_DEFAULT_PWM_PERPHERAL, pinA, 0); + pwmHiConfig(¶ms->inst[1], SILABS_DEFAULT_PWM_PERPHERAL, pinB, 1); + + // Initialize PWM + EFR32PwmConfig pwmConfig; + pwmConfig.frequency = pwm_frequency << 1; + pwmConfig.polarity = PWM_P_ACTIVE_LOW; + pwmConfig.outInvert = true; + + pwmHiInit(¶ms->inst[0], &pwmConfig, NULL, NULL); + pwmHiInit(¶ms->inst[1], &pwmConfig, NULL, NULL); + + // PWM On + pwmHiOn(¶ms->inst[0]); + pwmHiOn(¶ms->inst[1]); + + // Start PWM + pwmStart(¶ms->inst[0], _configPWMMode, NULL); + + return params; +} + +void* _configure3PWM( + long pwm_frequency, + const int pinA, + const int pinB, + const int pinC +) { + EFR32DriverParams *params = new EFR32DriverParams; + if (!params) return SIMPLEFOC_DRIVER_INIT_FAILED; + + if (!pwm_frequency || !_isset(pwm_frequency)) pwm_frequency = SILABS_DEFAULT_PWM_FREQUENCY; + else pwm_frequency = _constrain(pwm_frequency, 0, SILABS_DEFAULT_PWM_FREQUENCY); + + params->pwm_frequency = pwm_frequency; + params->noPwmChannel = 3; + + // Ensure all PWMs use the same TIMER instance + pwmHiConfig(¶ms->inst[0], SILABS_DEFAULT_PWM_PERPHERAL, pinA, 0); + pwmHiConfig(¶ms->inst[1], SILABS_DEFAULT_PWM_PERPHERAL, pinB, 1); + pwmHiConfig(¶ms->inst[2], SILABS_DEFAULT_PWM_PERPHERAL, pinC, 2); + + // Initialize PWM + EFR32PwmConfig pwmConfig; + pwmConfig.frequency = pwm_frequency << 1; + pwmConfig.polarity = PWM_P_ACTIVE_LOW; + pwmConfig.outInvert = true; + + pwmHiInit(¶ms->inst[0], &pwmConfig, NULL, NULL); + pwmHiInit(¶ms->inst[1], &pwmConfig, NULL, NULL); + pwmHiInit(¶ms->inst[2], &pwmConfig, NULL, NULL); + + // PWM On + pwmHiOn(¶ms->inst[0]); + pwmHiOn(¶ms->inst[1]); + pwmHiOn(¶ms->inst[2]); + + // Start PWM + pwmStart(¶ms->inst[0], _configPWMMode, NULL); + + return params; +} + +void* _configure4PWM( + long pwm_frequency, + const int pin1A, + const int pin1B, + const int pin2A, + const int pin2B +) { + EFR32DriverParams *params = new EFR32DriverParams; + if (!params) return SIMPLEFOC_DRIVER_INIT_FAILED; + + if (!pwm_frequency || !_isset(pwm_frequency)) pwm_frequency = SILABS_DEFAULT_PWM_FREQUENCY; + else pwm_frequency = _constrain(pwm_frequency, 0, SILABS_DEFAULT_PWM_FREQUENCY); + + params->pwm_frequency = pwm_frequency; + params->noPwmChannel = 4; + + // Ensure all PWMs use the same TIMER instance + pwmHiConfig(¶ms->inst[0], SILABS_DEFAULT_PWM_PERPHERAL, pin1A, 0); + pwmHiConfig(¶ms->inst[1], SILABS_DEFAULT_PWM_PERPHERAL, pin1B, 1); + pwmHiConfig(¶ms->inst[2], SILABS_DEFAULT_PWM_PERPHERAL, pin2A, 2); + pwmHiConfig(¶ms->inst[3], SILABS_SECOND_PWM_PERPHERAL , pin2B, 0); + + // Initialize PWM + EFR32PwmConfig pwmConfig; + pwmConfig.frequency = pwm_frequency << 1; + pwmConfig.polarity = PWM_P_ACTIVE_LOW; + pwmConfig.outInvert = true; + + pwmHiInit(¶ms->inst[0], &pwmConfig, NULL, NULL); + pwmHiInit(¶ms->inst[1], &pwmConfig, NULL, NULL); + pwmHiInit(¶ms->inst[2], &pwmConfig, NULL, NULL); + pwmHiInit(¶ms->inst[3], &pwmConfig, _alignPWMTimers, ¶ms->inst[0]); + + // PWM On + pwmHiOn(¶ms->inst[0]); + pwmHiOn(¶ms->inst[1]); + pwmHiOn(¶ms->inst[2]); + pwmHiOn(¶ms->inst[3]); + + // Start PWM + pwmStart(¶ms->inst[0], _configPWMMode, NULL); + pwmStart(¶ms->inst[3], _alignPWMStart, NULL); + + return params; +} + +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 +) { + EFR32DriverParams *params = new EFR32DriverParams; + if (!params) return SIMPLEFOC_DRIVER_INIT_FAILED; + + if (!pwm_frequency || !_isset(pwm_frequency)) pwm_frequency = SILABS_DEFAULT_PWM_FREQUENCY; + else pwm_frequency = _constrain(pwm_frequency, 0, SILABS_DEFAULT_PWM_FREQUENCY); + + params->pwm_frequency = pwm_frequency; + params->dead_zone = (dead_zone == NOT_SET) ? SILABS_DEFAULT_DEAD_ZONE : dead_zone; + params->lowside = true; + params->noPwmChannel = 3; + + // Ensure all PWMs use the same TIMER instance + pwmHiConfig(¶ms->inst[0], SILABS_DEFAULT_PWM_PERPHERAL, pinA_h, 0); + pwmLoConfig(¶ms->inst[0], pinA_l); + + pwmHiConfig(¶ms->inst[1], SILABS_DEFAULT_PWM_PERPHERAL, pinB_h, 1); + pwmLoConfig(¶ms->inst[1], pinB_l); + + pwmHiConfig(¶ms->inst[2], SILABS_DEFAULT_PWM_PERPHERAL, pinC_h, 2); + pwmLoConfig(¶ms->inst[2], pinC_l); + + // Initialize PWM + EFR32PwmConfig pwmConfig; + pwmConfig.frequency = pwm_frequency << 1; + pwmConfig.polarity = PWM_P_ACTIVE_LOW; + pwmConfig.outInvert = true; + + pwmInit(¶ms->inst[0], &pwmConfig, NULL, NULL); + pwmInit(¶ms->inst[1], &pwmConfig, NULL, NULL); + pwmInit(¶ms->inst[2], &pwmConfig, NULL, NULL); + + // Dead Time PWM + uint32_t deadTimeNs = (float) (1e9f / pwm_frequency) * dead_zone; + EFR32PwmDeadTimeConfig deadTimeConfig; + deadTimeConfig.deadTimeNs = deadTimeNs >> 1; + deadTimeConfig.outputMask = TIMER_DTOGEN_DTOGCC0EN + | TIMER_DTOGEN_DTOGCC1EN + | TIMER_DTOGEN_DTOGCC2EN + | TIMER_DTOGEN_DTOGCDTI0EN + | TIMER_DTOGEN_DTOGCDTI1EN + | TIMER_DTOGEN_DTOGCDTI2EN; + pwmDeadTimeInit(¶ms->inst[0], &deadTimeConfig); + + // PWM On + pwmOn(¶ms->inst[0]); + pwmOn(¶ms->inst[1]); + pwmOn(¶ms->inst[2]); + + // Start PWM + pwmStart(¶ms->inst[0], _configPWMMode, NULL); + + return params; +} + +void _writeDutyCycle1PWM(float dc_a, void* params) { + EFR32DriverParams *p = (EFR32DriverParams*) params; + if (!p) return; + + pwmHiSetDutyCycle(&p->inst[0], dc_a * 100.0f); +} + +void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params) { + EFR32DriverParams *p = (EFR32DriverParams*) params; + if (!p) return; + + pwmHiSetDutyCycle(&p->inst[0], dc_a * 100.0f); + pwmHiSetDutyCycle(&p->inst[1], dc_b * 100.0f); +} + +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params) { + EFR32DriverParams *p = (EFR32DriverParams*) params; + if (!p) return; + + pwmHiSetDutyCycle(&p->inst[0], dc_a * 100.0f); + pwmHiSetDutyCycle(&p->inst[1], dc_b * 100.0f); + pwmHiSetDutyCycle(&p->inst[2], dc_c * 100.0f); +} + +void _writeDutyCycle4PWM( + float dc_1a, + float dc_1b, + float dc_2a, + float dc_2b, + void* params +) { + EFR32DriverParams *p = (EFR32DriverParams*) params; + if (!p) return; + + pwmHiSetDutyCycle(&p->inst[0], dc_1a * 100.0f); + pwmHiSetDutyCycle(&p->inst[1], dc_1b * 100.0f); + pwmHiSetDutyCycle(&p->inst[2], dc_2a * 100.0f); + pwmHiSetDutyCycle(&p->inst[3], dc_2b * 100.0f); +} + +void _writeDutyCycle6PWM( + float dc_a, + float dc_b, + float dc_c, + PhaseState *phase_state, + void* params +) { + EFR32DriverParams *p = (EFR32DriverParams*) params; + if (!p || !phase_state) return; + + _setSinglePhaseState(&p->inst[0], phase_state[0]); + if (phase_state[0] == PhaseState::PHASE_OFF) dc_a = 0.0f; + pwmHiSetDutyCycle(&p->inst[0], dc_a * 100); + + _setSinglePhaseState(&p->inst[1], phase_state[1]); + if (phase_state[1] == PhaseState::PHASE_OFF) dc_b = 0.0f; + pwmHiSetDutyCycle(&p->inst[1], dc_b * 100.0f); + + _setSinglePhaseState(&p->inst[2], phase_state[2]); + if (phase_state[2] == PhaseState::PHASE_OFF) dc_c = 0.0f; + pwmHiSetDutyCycle(&p->inst[2], dc_c * 100.0f); +} + +#endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/silabs/efr32_mcu.h b/src/drivers/hardware_specific/silabs/efr32_mcu.h new file mode 100644 index 00000000..4b00422a --- /dev/null +++ b/src/drivers/hardware_specific/silabs/efr32_mcu.h @@ -0,0 +1,39 @@ +#ifndef EFR32_DRIVER_MCU_H +#define EFR32_DRIVER_MCU_H + +#include "../../hardware_api.h" + +#if defined(ARDUINO_ARCH_SILABS) +#include "efr32_pwm.h" + +#ifndef SILABS_DEFAULT_PWM_PERPHERAL +#define SILABS_DEFAULT_PWM_PERPHERAL TIMER0 +#endif + +#ifndef SILABS_SECOND_PWM_PERPHERAL +#define SILABS_SECOND_PWM_PERPHERAL TIMER1 +#endif + +#ifndef SILABS_PWM_PRS_CHANNEL +#define SILABS_PWM_PRS_CHANNEL 0 +#endif + +#ifndef SILABS_DEFAULT_PWM_FREQUENCY +#define SILABS_DEFAULT_PWM_FREQUENCY 50000 +#endif + +#ifndef SILABS_DEFAULT_DEAD_ZONE +#define SILABS_DEFAULT_DEAD_ZONE 0.02f +#endif + +typedef struct EFR32DriverParams { + EFR32PwmInstance inst[4]; + uint8_t noPwmChannel; + bool lowside; + long pwm_frequency; + float dead_zone; +} EFR32DriverParams; + +#endif + +#endif diff --git a/src/drivers/hardware_specific/silabs/efr32_pwm.cpp b/src/drivers/hardware_specific/silabs/efr32_pwm.cpp new file mode 100644 index 00000000..3884220f --- /dev/null +++ b/src/drivers/hardware_specific/silabs/efr32_pwm.cpp @@ -0,0 +1,310 @@ +#if defined(ARDUINO_ARCH_SILABS) + +#include +#include +#include +#include +#include +#include "efr32_pwm.h" + +static CMU_Clock_TypeDef _getTimerClock(TIMER_TypeDef *timer) { +#if defined(_CMU_HFCLKSEL_MASK) || defined(_CMU_CMD_HFCLKSEL_MASK) + CMU_Clock_TypeDef timerClock = cmuClock_HF; +#elif defined(_CMU_SYSCLKCTRL_MASK) + CMU_Clock_TypeDef timerClock = cmuClock_SYSCLK; +#else +#error "Unknown root of clock tree" +#endif + + switch ((uint32_t)timer) { +#if defined(TIMER0_BASE) + case TIMER0_BASE: + timerClock = cmuClock_TIMER0; + break; +#endif +#if defined(TIMER1_BASE) + case TIMER1_BASE: + timerClock = cmuClock_TIMER1; + break; +#endif +#if defined(TIMER2_BASE) + case TIMER2_BASE: + timerClock = cmuClock_TIMER2; + break; +#endif +#if defined(TIMER3_BASE) + case TIMER3_BASE: + timerClock = cmuClock_TIMER3; + break; +#endif +#if defined(TIMER4_BASE) + case TIMER4_BASE: + timerClock = cmuClock_TIMER4; + break; +#endif +#if defined(TIMER5_BASE) + case TIMER5_BASE: + timerClock = cmuClock_TIMER5; + break; +#endif +#if defined(TIMER6_BASE) + case TIMER6_BASE: + timerClock = cmuClock_TIMER6; + break; +#endif +#if defined(WTIMER0_BASE) + case WTIMER0_BASE: + timerClock = cmuClock_WTIMER0; + break; +#endif +#if defined(WTIMER1_BASE) + case WTIMER1_BASE: + timerClock = cmuClock_WTIMER1; + break; +#endif +#if defined(WTIMER2_BASE) + case WTIMER2_BASE: + timerClock = cmuClock_WTIMER2; + break; +#endif +#if defined(WTIMER3_BASE) + case WTIMER3_BASE: + timerClock = cmuClock_WTIMER3; + break; +#endif + default: + break; + } + return timerClock; +} + +void pwmHiConfig( + EFR32PwmInstance *inst, + TIMER_TypeDef *timer, + const int pin, + const uint8_t channel +) { + if (!inst) return; + + inst->h.timer = timer; + inst->h.port = getSilabsPortFromArduinoPin(pinToPinName(pin)); + inst->h.pin = getSilabsPinFromArduinoPin(pinToPinName(pin)); + inst->h.channel = channel; +} + +void pwmHiInit( + EFR32PwmInstance *inst, + EFR32PwmConfig *config, + prevTimerInitCCFn fn, + void *params +) { + if (!inst || !config) return; + + // Enable Timer Clock + CMU_Clock_TypeDef timerClock = _getTimerClock(inst->h.timer); + CMU_ClockEnable(timerClock, true); + + // Set PWM pin as output + CMU_ClockEnable(cmuClock_GPIO, true); + GPIO_PinModeSet((GPIO_Port_TypeDef)inst->h.port, + inst->h.pin, + gpioModePushPull, + config->polarity); + + // Set CC channel parameters + TIMER_InitCC_TypeDef initCC = TIMER_INITCC_DEFAULT; + initCC.mode = timerCCModePWM; + if (config->outInvert) initCC.outInvert = true; + if (fn) fn(&initCC, params); + + TIMER_InitCC(inst->h.timer, inst->h.channel, &initCC); + + volatile uint32_t *routeRegister = &GPIO->TIMERROUTE[TIMER_NUM(inst->h.timer)].CC0ROUTE; + routeRegister += inst->h.channel; + *routeRegister = (inst->h.port << _GPIO_TIMER_CC0ROUTE_PORT_SHIFT) + | (inst->h.pin << _GPIO_TIMER_CC0ROUTE_PIN_SHIFT); + + // Configure TIMER frequency + uint32_t top = (CMU_ClockFreqGet(timerClock) / (config->frequency)) - 1U; + TIMER_TopSet(inst->h.timer, top); + + // Set initial duty cycle to 0% + TIMER_CompareSet(inst->h.timer, inst->h.channel, 0U); +} + +void pwmHiDeinit( + EFR32PwmInstance *inst +) { + if (!inst) return; + + pwmHiOff(inst); + + volatile uint32_t *routeRegister = &GPIO->TIMERROUTE[TIMER_NUM(inst->h.timer)].CC0ROUTE; + routeRegister += inst->h.channel; + *routeRegister = 0; + + TIMER_Reset(inst->h.timer); + GPIO_PinModeSet((GPIO_Port_TypeDef)inst->h.port, + inst->h.pin, + gpioModeDisabled, + 0); + CMU_Clock_TypeDef timerClock = _getTimerClock(inst->h.timer); + CMU_ClockEnable(timerClock, false); +} + +void pwmHiOn( + EFR32PwmInstance *inst +) { + if (!inst) return; + GPIO->TIMERROUTE_SET[TIMER_NUM(inst->h.timer)].ROUTEEN = 1 << (inst->h.channel + _GPIO_TIMER_ROUTEEN_CC0PEN_SHIFT); +} + +void pwmHiOff( + EFR32PwmInstance *inst +) { + if (!inst) return; + GPIO->TIMERROUTE_CLR[TIMER_NUM(inst->h.timer)].ROUTEEN = 1 << (inst->h.channel + _GPIO_TIMER_ROUTEEN_CC0PEN_SHIFT); +} + +void pwmHiSetDutyCycle( + EFR32PwmInstance *inst, + float percent +) { + if (!inst || (percent > 100.0f)) return; + uint32_t top = TIMER_TopGet(inst->h.timer); + volatile bool outInvert = inst->h.timer->CC[inst->h.channel].CTRL & TIMER_CC_CTRL_OUTINV; + if (outInvert) percent = 100 - percent; + TIMER_CompareBufSet(inst->h.timer, inst->h.channel, (uint32_t) (top * percent) / 100); +} + +float pwmHiGetDutyCycle( + EFR32PwmInstance *inst +) { + if (!inst) return 0; + uint32_t top = TIMER_TopGet(inst->h.timer); + uint32_t compare = TIMER_CaptureGet(inst->h.timer, inst->h.channel); + volatile bool outInvert = inst->h.timer->CC[inst->h.channel].CTRL & TIMER_CC_CTRL_OUTINV; + float percent = (float)((compare * 100) / top); + return outInvert ? (100 - percent) : percent; +} + +void pwmLoConfig( + EFR32PwmInstance *inst, + const int pin +) { + if (!inst) return; + inst->l.port = getSilabsPortFromArduinoPin(pinToPinName(pin)); + inst->l.pin = getSilabsPinFromArduinoPin(pinToPinName(pin)); +} + +void pwmLoInit( + EFR32PwmInstance *inst +) { + if (!inst) return; + + // Low side PWM + CMU_ClockEnable(cmuClock_GPIO, true); + GPIO_PinModeSet((GPIO_Port_TypeDef)inst->l.port, + inst->l.pin, + gpioModePushPull, + 0); + + volatile uint32_t *routeRegister = &GPIO->TIMERROUTE[TIMER_NUM(inst->h.timer)].CDTI0ROUTE; + routeRegister += inst->h.channel; + *routeRegister = (inst->l.port << _GPIO_TIMER_CDTI0ROUTE_PORT_SHIFT) + | (inst->l.pin << _GPIO_TIMER_CDTI0ROUTE_PIN_SHIFT); +} + +void pwmLoDeinit( + EFR32PwmInstance *inst +) { + if (!inst) return; + + pwmLoOff(inst); + + volatile uint32_t *routeRegister = &GPIO->TIMERROUTE[TIMER_NUM(inst->h.timer)].CDTI0ROUTE; + routeRegister += inst->h.channel; + *routeRegister = 0; + + GPIO_PinModeSet((GPIO_Port_TypeDef)inst->l.port, + inst->l.pin, + gpioModeDisabled, + 0); +} + +void pwmLoOn( + EFR32PwmInstance *inst +) { + if (!inst) return; + GPIO->TIMERROUTE_SET[TIMER_NUM(inst->h.timer)].ROUTEEN |= 1 << (inst->h.channel + _GPIO_TIMER_ROUTEEN_CCC0PEN_SHIFT); +} + +void pwmLoOff( + EFR32PwmInstance *inst +) { + if (!inst) return; + GPIO->TIMERROUTE_CLR[TIMER_NUM(inst->h.timer)].ROUTEEN |= 1 << (inst->h.channel + _GPIO_TIMER_ROUTEEN_CCC0PEN_SHIFT); +} + +void pwmInit( + EFR32PwmInstance *inst, + EFR32PwmConfig *config, + prevTimerInitCCFn fn, + void *params +) { + if (!inst || !config) return; + pwmHiInit(inst, config, fn, params); + pwmLoInit(inst); +} + +void pwmDeinit( + EFR32PwmInstance *inst +) { + if (!inst) return; + pwmHiDeinit(inst); + pwmLoDeinit(inst); +} + +void pwmOff(EFR32PwmInstance *inst) { + if (!inst) return; + pwmHiOff(inst); + pwmLoOff(inst); +} + +void pwmOn(EFR32PwmInstance *inst) { + if (!inst) return; + pwmHiOn(inst); + pwmLoOn(inst); +} + +void pwmStart(EFR32PwmInstance *inst, prevTimerInitFn fn, void *params) { + if (!inst) return; + + // Initialize TIMER + TIMER_Init_TypeDef timerInit = TIMER_INIT_DEFAULT; + if (fn) fn(&timerInit, params); + TIMER_Init(inst->h.timer, &timerInit); +} + +void pwmDeadTimeInit( + EFR32PwmInstance *inst, + EFR32PwmDeadTimeConfig *config +) { + if (!inst || !config) return; + + // Enable Timer Clock + CMU_Clock_TypeDef timerClock = _getTimerClock(inst->h.timer); + CMU_ClockEnable(timerClock, true); + + unsigned int dtiTime = (CMU_ClockFreqGet(timerClock) / 1e3f) * config->deadTimeNs / 1e6f; + if (dtiTime > 64) dtiTime = SILABBS_DEFAULT_DEAD_TIME; + + TIMER_InitDTI_TypeDef initDTI = TIMER_INITDTI_DEFAULT; + initDTI.riseTime = dtiTime; + initDTI.fallTime = dtiTime; + initDTI.outputsEnableMask = config->outputMask; + + TIMER_InitDTI(inst->h.timer, &initDTI); +} + +#endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/silabs/efr32_pwm.h b/src/drivers/hardware_specific/silabs/efr32_pwm.h new file mode 100644 index 00000000..a4e1fee7 --- /dev/null +++ b/src/drivers/hardware_specific/silabs/efr32_pwm.h @@ -0,0 +1,121 @@ +#ifndef EFR32_DRIVER_PWM_MCU_H +#define EFR32_DRIVER_PWM_MCU_H + +#if defined(ARDUINO_ARCH_SILABS) + +#include + +#ifndef SILABBS_DEFAULT_DEAD_TIME +#define SILABBS_DEFAULT_DEAD_TIME 3 +#endif + +typedef void (*prevTimerInitCCFn)(TIMER_InitCC_TypeDef*, void *params); +typedef void (*prevTimerInitFn)(TIMER_Init_TypeDef*, void *params); + +typedef enum { + PWM_P_ACTIVE_HIGH = 0, + PWM_P_ACTIVE_LOW = 1 +} EFR32PwmPolarity; + +typedef struct { + int frequency; /**< PWM frequency */ + bool outInvert; /**< Invert output */ + EFR32PwmPolarity polarity; /**< PWM polarity */ +} EFR32PwmConfig; + +typedef struct { + uint32_t deadTimeNs; + uint32_t outputMask; +} EFR32PwmDeadTimeConfig; + +typedef struct { + TIMER_TypeDef *timer; /**< TIMER instance */ + uint8_t channel; /**< TIMER channel */ + uint8_t port; /**< GPIO port */ + uint8_t pin; /**< GPIO pin */ +} EFR32PwmHiInstance; + +typedef struct { + uint8_t port; + uint8_t pin; +} EFR32PwmLoInstance; + +typedef struct { + EFR32PwmHiInstance h; + EFR32PwmLoInstance l; +} EFR32PwmInstance; + +// High Side +void pwmHiConfig( + EFR32PwmInstance *inst, + TIMER_TypeDef *timer, + const int pin, + const uint8_t channel); + +void pwmHiInit( + EFR32PwmInstance *inst, + EFR32PwmConfig *config, + prevTimerInitCCFn fn, + void *params); + +void pwmHiDeinit( + EFR32PwmInstance *inst); + +void pwmHiOn( + EFR32PwmInstance *inst); + +void pwmHiOff( + EFR32PwmInstance *inst); + +void pwmHiSetDutyCycle( + EFR32PwmInstance *inst, + float percent); + +float pwmHiGetDutyCycle( + EFR32PwmInstance *inst); + +// Low Side +void pwmLoConfig( + EFR32PwmInstance *inst, + const int pin); + +void pwmLoInit( + EFR32PwmInstance *inst); + +void pwmLoDeinit( + EFR32PwmInstance *inst); + +void pwmLoOn( + EFR32PwmInstance *inst); + +void pwmLoOff( + EFR32PwmInstance *inst); + +// Pwm +void pwmInit( + EFR32PwmInstance *inst, + EFR32PwmConfig *cfg, + prevTimerInitCCFn fn, + void *params); + +void pwmDeinit( + EFR32PwmInstance *inst); + +void pwmOn( + EFR32PwmInstance *inst); + +void pwmOff( + EFR32PwmInstance *inst); + +void pwmStart( + EFR32PwmInstance *inst, + prevTimerInitFn fn, + void *params); + +void pwmDeadTimeInit( + EFR32PwmInstance *inst, + EFR32PwmDeadTimeConfig *config); + +#endif + +#endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp index 65dad9f4..1a9c833e 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp @@ -1,108 +1,221 @@ #include "../../hardware_api.h" -#include "stm32_mcu.h" +#include "./stm32_mcu.h" +#include "./stm32_timerutils.h" +#include "./stm32_searchtimers.h" -#if defined(_STM32_DEF_) +#if defined(_STM32_DEF_) || defined(TARGET_STM32H7) // if stm32duino or portenta #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); -#endif - +/* + * Timer management + * SimpleFOC manages the timers using only STM32 HAL and LL APIs, and does not use the HardwareTimer API. + * This is because the HardwareTimer API is not available on all STM32 boards, and does not provide all + * the functionality that SimpleFOC requires anyway. + * By using the HAL and LL APIs directly, we can ensure that SimpleFOC works on all STM32 boards, specifically + * also those that use MBED with Arduino (Portenta H7, Giga, Nicla). + * + * When using stm32duino, the HardwareTimer API is available, and can be used in parallel with SimpleFOC, + * provided you don't use the same timers for both. + */ + +// track timers initialized via SimpleFOC +int numTimersUsed = 0; +TIM_HandleTypeDef* timersUsed[SIMPLEFOC_STM32_MAX_TIMERSUSED]; + +// reserve timers for other uses, so SimpleFOC doesn't use them for motors +int numTimersReserved = 0; +TIM_TypeDef* reservedTimers[SIMPLEFOC_STM32_MAX_TIMERSRESERVED]; + +// track drivers initialized via SimpleFOC - used to know which timer channels are used +int numMotorsUsed = 0; +STM32DriverParams* motorsUsed[SIMPLEFOC_STM32_MAX_MOTORSUSED]; + +// query functions to check which timers are used +int stm32_getNumTimersUsed() { + return numTimersUsed; +} +int stm32_getNumMotorsUsed() { + return numMotorsUsed; +} +int stm32_getNumTimersReserved() { + return numTimersReserved; +} +bool stm32_isTimerReserved(TIM_TypeDef* timer) { + for (int i=0; iperipheral)) { + return true; + } + for (int i=0; itimers_handle[j] == NULL) break; + if (motorsUsed[i]->channels[j] == STM_PIN_CHANNEL(pin->function) && ((TIM_TypeDef*)pin->peripheral) == motorsUsed[i]->timers_handle[j]->Instance) + return true; + } + } + return false; +} +TIM_HandleTypeDef* stm32_getTimer(PinMap* timer) { + for (int i=0; iInstance == (TIM_TypeDef*)timer->peripheral) + return timersUsed[i]; + } + return NULL; +} +bool stm32_reserveTimer(TIM_TypeDef* timer) { + if (numTimersReserved >= SIMPLEFOC_STM32_MAX_TIMERSRESERVED) { + SIMPLEFOC_DEBUG("STM32-DRV: ERR: too many timers reserved"); + return false; + } + reservedTimers[numTimersReserved++] = timer; + return true; +} +// function to get a timer handle given the pinmap entry of the pin you want to use +// after calling this function, the timer is marked as used by SimpleFOC +TIM_HandleTypeDef* stm32_useTimer(PinMap* timer) { + TIM_HandleTypeDef* handle = stm32_getTimer(timer); + if (handle != NULL) return handle; + if (numTimersUsed >= SIMPLEFOC_STM32_MAX_TIMERSUSED) { + SIMPLEFOC_DEBUG("STM32-DRV: ERR: too many timers used"); + return NULL; + } + if (stm32_isTimerReserved((TIM_TypeDef*)timer->peripheral)) { + SIMPLEFOC_DEBUG("STM32-DRV: ERR: timer reserved"); + return NULL; + } + handle = new TIM_HandleTypeDef(); + handle->Instance = (TIM_TypeDef*)timer->peripheral; + handle->Channel = HAL_TIM_ACTIVE_CHANNEL_CLEARED; + handle->Lock = HAL_UNLOCKED; + handle->State = HAL_TIM_STATE_RESET; + handle->hdma[0] = NULL; + handle->hdma[1] = NULL; + handle->hdma[2] = NULL; + handle->hdma[3] = NULL; + handle->hdma[4] = NULL; + handle->hdma[5] = NULL; + handle->hdma[6] = NULL; + handle->Init.Prescaler = 0; + handle->Init.Period = ((1 << 16) - 1); + handle->Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED2; + handle->Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; + handle->Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE; + #if defined(TIM_RCR_REP) + handle->Init.RepetitionCounter = 1; + #endif + enableTimerClock(handle); + HAL_TIM_Base_Init(handle); + stm32_pauseTimer(handle); + timersUsed[numTimersUsed++] = handle; + return handle; +} -#ifndef SIMPLEFOC_STM32_MAX_PINTIMERSUSED -#define SIMPLEFOC_STM32_MAX_PINTIMERSUSED 12 -#endif -int numTimerPinsUsed; -PinMap* timerPinsUsed[SIMPLEFOC_STM32_MAX_PINTIMERSUSED]; - +bool _getPwmState(void* params) { + // assume timers are synchronized and that there's at least one timer + bool dir = __HAL_TIM_IS_TIM_COUNTING_DOWN(((STM32DriverParams*)params)->timers_handle[0]); + return dir; +} -// setting pwm to hardware pin - instead analogWrite() -void _setPwm(HardwareTimer *HT, uint32_t channel, uint32_t value, int resolution) -{ - // TODO - remove commented code - // PinName pin = digitalPinToPinName(ulPin); - // TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM); - // uint32_t index = get_timer_index(Instance); - // HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this); - - HT->setCaptureCompare(channel, value, (TimerCompareFormat_t)resolution); +void stm32_pause(STM32DriverParams* params) { + if (params->master_timer != NULL) { + stm32_pauseTimer(params->master_timer); + } + else { + for (int i=0; inum_timers; i++) { + stm32_pauseTimer(params->timers_handle[i]); + } + } } - -int getLLChannel(PinMap* timer) { -#if defined(TIM_CCER_CC1NE) - if (STM_PIN_INVERTED(timer->function)) { - switch (STM_PIN_CHANNEL(timer->function)) { - case 1: return LL_TIM_CHANNEL_CH1N; - case 2: return LL_TIM_CHANNEL_CH2N; - case 3: return LL_TIM_CHANNEL_CH3N; -#if defined(LL_TIM_CHANNEL_CH4N) - case 4: return LL_TIM_CHANNEL_CH4N; -#endif - default: return -1; - } - } else -#endif - { - switch (STM_PIN_CHANNEL(timer->function)) { - case 1: return LL_TIM_CHANNEL_CH1; - case 2: return LL_TIM_CHANNEL_CH2; - case 3: return LL_TIM_CHANNEL_CH3; - case 4: return LL_TIM_CHANNEL_CH4; - default: return -1; +void stm32_resume(STM32DriverParams* params) { + if (params->master_timer != NULL) { + stm32_resumeTimer(params->master_timer); + } + else { + for (int i=0; inum_timers; i++) { + stm32_resumeTimer(params->timers_handle[i]); } } - return -1; } - - // init pin pwm -HardwareTimer* _initPinPWM(uint32_t PWM_freq, PinMap* timer) { +TIM_HandleTypeDef* stm32_initPinPWM(uint32_t PWM_freq, PinMap* timer, uint32_t mode = TIM_OCMODE_PWM1, uint32_t polarity = TIM_OCPOLARITY_HIGH, uint32_t Npolarity = TIM_OCNPOLARITY_HIGH) { // sanity check - if (timer==NP) - return NP; - uint32_t index = get_timer_index((TIM_TypeDef*)timer->peripheral); - bool init = false; - if (HardwareTimer_Handle[index] == NULL) { - HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef*)timer->peripheral); - HardwareTimer_Handle[index]->handle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3; - HardwareTimer_Handle[index]->handle.Init.RepetitionCounter = 1; - HAL_TIM_Base_Init(&(HardwareTimer_Handle[index]->handle)); - init = true; - } - HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this); + if (timer==NULL) + return NULL; + TIM_HandleTypeDef* handle = stm32_getTimer(timer); uint32_t channel = STM_PIN_CHANNEL(timer->function); - HT->pause(); - if (init) - HT->setOverflow(PWM_freq, HERTZ_FORMAT); - HT->setMode(channel, TIMER_OUTPUT_COMPARE_PWM1, timer->pin); - #if SIMPLEFOC_PWM_ACTIVE_HIGH==false - LL_TIM_OC_SetPolarity(HT->getHandle()->Instance, getLLChannel(timer), LL_TIM_OCPOLARITY_LOW); - #endif -#ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-DRV: Configuring high timer ", (int)getTimerNumber(get_timer_index(HardwareTimer_Handle[index]->handle.Instance))); - SIMPLEFOC_DEBUG("STM32-DRV: Configuring high channel ", (int)channel); + if (handle==NULL) { + handle = stm32_useTimer(timer); + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-DRV: Initializing TIM", (int)stm32_getTimerNumber(handle->Instance)); + #endif + uint32_t arr = stm32_setClockAndARR(handle, PWM_freq); + if (arrpin, PinMap_TIM); + LL_TIM_CC_EnableChannel(handle->Instance, stm32_getLLChannel(timer)); + if (IS_TIM_BREAK_INSTANCE(handle->Instance)) { + __HAL_TIM_MOE_ENABLE(handle); + } + #ifdef SIMPLEFOC_STM32_DEBUG + SimpleFOCDebug::print("STM32-DRV: Configured TIM"); + SimpleFOCDebug::print((int)stm32_getTimerNumber(handle->Instance)); + SIMPLEFOC_DEBUG("_CH", (int)channel); + #endif + return handle; } @@ -110,158 +223,100 @@ HardwareTimer* _initPinPWM(uint32_t PWM_freq, PinMap* timer) { - +/** +0110: PWM mode 1 - In upcounting, channel 1 is active as long as TIMx_CNTTIMx_CCR1 else active (OC1REF=’1’). +0111: PWM mode 2 - In upcounting, channel 1 is inactive as long as +TIMx_CNTTIMx_CCR1 else inactive + */ // init high side pin -HardwareTimer* _initPinPWMHigh(uint32_t PWM_freq, PinMap* timer) { - HardwareTimer* HT = _initPinPWM(PWM_freq, timer); - #if SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH==false && SIMPLEFOC_PWM_ACTIVE_HIGH==true - LL_TIM_OC_SetPolarity(HT->getHandle()->Instance, getLLChannel(timer), LL_TIM_OCPOLARITY_LOW); - #endif - return HT; +TIM_HandleTypeDef* _stm32_initPinPWMHigh(uint32_t PWM_freq, PinMap* timer) { + uint32_t polarity = SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH ? TIM_OCPOLARITY_HIGH : TIM_OCPOLARITY_LOW ; + TIM_HandleTypeDef* handle = stm32_initPinPWM(PWM_freq, timer, TIM_OCMODE_PWM1, polarity); + LL_TIM_OC_EnablePreload(handle->Instance, stm32_getLLChannel(timer)); + return handle; } // init low side pin -HardwareTimer* _initPinPWMLow(uint32_t PWM_freq, PinMap* timer) -{ - uint32_t index = get_timer_index((TIM_TypeDef*)timer->peripheral); - - bool init = false; - if (HardwareTimer_Handle[index] == NULL) { - HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef*)timer->peripheral); - HardwareTimer_Handle[index]->handle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3; - HardwareTimer_Handle[index]->handle.Init.RepetitionCounter = 1; - HAL_TIM_Base_Init(&(HardwareTimer_Handle[index]->handle)); - init = true; - } - HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this); - uint32_t channel = STM_PIN_CHANNEL(timer->function); +TIM_HandleTypeDef* _stm32_initPinPWMLow(uint32_t PWM_freq, PinMap* timer) { + uint32_t polarity = SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH ? TIM_OCPOLARITY_HIGH : TIM_OCPOLARITY_LOW; + TIM_HandleTypeDef* handle = stm32_initPinPWM(PWM_freq, timer, TIM_OCMODE_PWM2, polarity); + LL_TIM_OC_EnablePreload(handle->Instance, stm32_getLLChannel(timer)); + return handle; +} -#ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-DRV: Configuring low timer ", (int)getTimerNumber(get_timer_index(HardwareTimer_Handle[index]->handle.Instance))); - SIMPLEFOC_DEBUG("STM32-DRV: Configuring low channel ", (int)channel); -#endif - HT->pause(); - if (init) - HT->setOverflow(PWM_freq, HERTZ_FORMAT); - // sets internal fields of HT, but we can't set polarity here - HT->setMode(channel, TIMER_OUTPUT_COMPARE_PWM2, timer->pin); - #if SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH==false - LL_TIM_OC_SetPolarity(HT->getHandle()->Instance, getLLChannel(timer), LL_TIM_OCPOLARITY_LOW); - #endif - return HT; -} -// align the timers to end the init -void _alignPWMTimers(HardwareTimer *HT1, HardwareTimer *HT2, HardwareTimer *HT3) -{ - HT1->pause(); - HT1->refresh(); - HT2->pause(); - HT2->refresh(); - HT3->pause(); - HT3->refresh(); - HT1->resume(); - HT2->resume(); - HT3->resume(); -} +// // align the timers to end the init +// void _startTimers(TIM_HandleTypeDef *timers_to_start[], int timer_num) { +// stm32_alignTimers(timers_to_start, timer_num); +// } +// void _stopTimers(TIM_HandleTypeDef **timers_to_stop, int timer_num) { +// TIM_HandleTypeDef* timers_distinct[6]; +// timer_num = stm32_distinctTimers(timers_to_stop, timer_num, timers_distinct); +// for (int i=0; i < timer_num; i++) { +// if(timers_distinct[i] == NULL) return; +// stm32_pauseTimer(timers_distinct[i]); +// stm32_refreshTimer(timers_distinct[i]); +// #ifdef SIMPLEFOC_STM32_DEBUG +// SIMPLEFOC_DEBUG("STM32-DRV: Stopping timer ", stm32_getTimerNumber(timers_distinct[i]->Instance)); +// #endif +// } +// } -// align the timers to end the init -void _alignPWMTimers(HardwareTimer *HT1, HardwareTimer *HT2, HardwareTimer *HT3, HardwareTimer *HT4) -{ - HT1->pause(); - HT1->refresh(); - HT2->pause(); - HT2->refresh(); - HT3->pause(); - HT3->refresh(); - HT4->pause(); - HT4->refresh(); - HT1->resume(); - HT2->resume(); - HT3->resume(); - HT4->resume(); -} -// align the timers to end the init -void _stopTimers(HardwareTimer **timers_to_stop, int timer_num) -{ - // TODO - stop each timer only once - // stop timers - for (int i=0; i < timer_num; i++) { - if(timers_to_stop[i] == NP) return; - timers_to_stop[i]->pause(); - timers_to_stop[i]->refresh(); - #ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-DRV: Stopping timer ", getTimerNumber(get_timer_index(timers_to_stop[i]->getHandle()->Instance))); - #endif - } -} -// 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))); - #endif - } -} -void _alignTimersNew() { - int numTimers = 0; - HardwareTimer *timers[numTimerPinsUsed]; - - // reset timer counters - for (int i=0; iperipheral); - HardwareTimer *timer = (HardwareTimer *)(HardwareTimer_Handle[index]->__this); - bool found = false; - for (int j=0; jInstance->ARR; + uint32_t prescaler = timers_distinct[i]->Instance->PSC; + float pwm_freq = (float)freq/(prescaler+1.0f)/(arr+1.0f)/2.0f; + if (i==0) { + common_pwm_freq = pwm_freq; + } else { + if (pwm_freq != common_pwm_freq) { + #ifdef SIMPLEFOC_STM32_DEBUG + SimpleFOCDebug::print("STM32-DRV: ERR: Timer frequency different: TIM"); + SimpleFOCDebug::print(stm32_getTimerNumber(timers_distinct[0]->Instance)); + SimpleFOCDebug::print(" freq: "); + SimpleFOCDebug::print(common_pwm_freq); + SimpleFOCDebug::print(" != TIM"); + SimpleFOCDebug::print(stm32_getTimerNumber(timers_distinct[i]->Instance)); + SimpleFOCDebug::println(" freq: ", pwm_freq); + #endif + return -1; } } - if (!found) - timers[numTimers++] = timer; - } - - // enable timer clock - for (int i=0; ipause(); - timers[i]->refresh(); - #ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-DRV: Restarting timer ", getTimerNumber(get_timer_index(timers[i]->getHandle()->Instance))); - #endif } - - for (int i=0; iresume(); + if ( (common_pwm_freq - (float)pwm_frequency) > 1.0f) { + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-DRV: ERR: Common timer frequency unexpected: ", common_pwm_freq); + #endif + return -1; } - + return 0; } - - - // configure hardware 6pwm for a complementary pair of channels -STM32DriverParams* _initHardware6PWMPair(long PWM_freq, float dead_zone, PinMap* pinH, PinMap* pinL, STM32DriverParams* params, int paramsPos) { +STM32DriverParams* _stm32_initHardware6PWMPair(long PWM_freq, float dead_zone, PinMap* pinH, PinMap* pinL, STM32DriverParams* params, int paramsPos) { // sanity check - if (pinH==NP || pinL==NP) + if (pinH==NULL || pinL==NULL) return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; - #if defined(STM32L0xx) // L0 boards dont have hardware 6pwm interface return SIMPLEFOC_DRIVER_INIT_FAILED; // return nothing #endif @@ -273,68 +328,56 @@ STM32DriverParams* _initHardware6PWMPair(long PWM_freq, float dead_zone, PinMap* if (channel1!=channel2 || pinH->peripheral!=pinL->peripheral) return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; - uint32_t index = get_timer_index((TIM_TypeDef*)pinH->peripheral); - - if (HardwareTimer_Handle[index] == NULL) { - HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef*)pinH->peripheral); - HardwareTimer_Handle[index]->handle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3; - HardwareTimer_Handle[index]->handle.Init.RepetitionCounter = 1; - // HardwareTimer_Handle[index]->handle.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE; - HAL_TIM_Base_Init(&(HardwareTimer_Handle[index]->handle)); - ((HardwareTimer *)HardwareTimer_Handle[index]->__this)->setOverflow((uint32_t)PWM_freq, HERTZ_FORMAT); - } - HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this); - - HT->setMode(channel1, TIMER_OUTPUT_COMPARE_PWM1, pinH->pin); - HT->setMode(channel2, TIMER_OUTPUT_COMPARE_PWM1, pinL->pin); + uint32_t polarity = SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH ? TIM_OCPOLARITY_HIGH : TIM_OCPOLARITY_LOW; + uint32_t Npolarity = SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH ? TIM_OCNPOLARITY_HIGH : TIM_OCNPOLARITY_LOW; + TIM_HandleTypeDef* handle = stm32_initPinPWM(PWM_freq, pinH, TIM_OCMODE_PWM1, polarity, Npolarity); + pinmap_pinout(pinL->pin, PinMap_TIM); // also init the low side pin // dead time is set in nanoseconds uint32_t dead_time_ns = (float)(1e9f/PWM_freq)*dead_zone; - uint32_t dead_time = __LL_TIM_CALC_DEADTIME(SystemCoreClock, LL_TIM_GetClockDivision(HT->getHandle()->Instance), dead_time_ns); + uint32_t dead_time = __LL_TIM_CALC_DEADTIME(stm32_getTimerClockFreq(handle), LL_TIM_GetClockDivision(handle->Instance), dead_time_ns); if (dead_time>255) dead_time = 255; if (dead_time==0 && dead_zone>0) { dead_time = 255; // LL_TIM_CALC_DEADTIME returns 0 if dead_time_ns is too large SIMPLEFOC_DEBUG("STM32-DRV: WARN: dead time too large, setting to max"); } - LL_TIM_OC_SetDeadTime(HT->getHandle()->Instance, dead_time); // deadtime is non linear! - #if SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH==false - LL_TIM_OC_SetPolarity(HT->getHandle()->Instance, getLLChannel(pinH), LL_TIM_OCPOLARITY_LOW); - #endif - #if SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH==false - LL_TIM_OC_SetPolarity(HT->getHandle()->Instance, getLLChannel(pinL), LL_TIM_OCPOLARITY_LOW); - #endif - LL_TIM_CC_EnableChannel(HT->getHandle()->Instance, getLLChannel(pinH) | getLLChannel(pinL)); - HT->pause(); - // make sure timer output goes to LOW when timer channels are temporarily disabled - LL_TIM_SetOffStates(HT->getHandle()->Instance, LL_TIM_OSSI_DISABLE, LL_TIM_OSSR_ENABLE); - - params->timers[paramsPos] = HT; - params->timers[paramsPos+1] = HT; + // TODO why init this here, and not generally in the initPWM or init timer functions? + // or, since its a rather specialized and hardware-speific setting, why not move it to + // another function? + LL_TIM_SetOffStates(handle->Instance, LL_TIM_OSSI_DISABLE, LL_TIM_OSSR_ENABLE); + LL_TIM_OC_SetDeadTime(handle->Instance, dead_time); // deadtime is non linear! + LL_TIM_CC_EnableChannel(handle->Instance, stm32_getLLChannel(pinH) | stm32_getLLChannel(pinL)); + params->timers_handle[paramsPos] = handle; + params->timers_handle[paramsPos+1] = handle; params->channels[paramsPos] = channel1; params->channels[paramsPos+1] = channel2; + params->llchannels[paramsPos] = stm32_getLLChannel(pinH); + params->llchannels[paramsPos+1] = stm32_getLLChannel(pinL); return params; } -STM32DriverParams* _initHardware6PWMInterface(long PWM_freq, float dead_zone, PinMap* pinA_h, PinMap* pinA_l, PinMap* pinB_h, PinMap* pinB_l, PinMap* pinC_h, PinMap* pinC_l) { +STM32DriverParams* _stm32_initHardware6PWMInterface(long PWM_freq, float dead_zone, PinMap* pinA_h, PinMap* pinA_l, PinMap* pinB_h, PinMap* pinB_l, PinMap* pinC_h, PinMap* pinC_l) { STM32DriverParams* params = new STM32DriverParams { - .timers = { NP, NP, NP, NP, NP, NP }, + .timers_handle = { NULL, NULL, NULL, NULL, NULL, NULL }, .channels = { 0, 0, 0, 0, 0, 0 }, + .llchannels = { 0, 0, 0, 0, 0, 0 }, .pwm_frequency = PWM_freq, + .num_timers = 0, + .master_timer = NULL, .dead_zone = dead_zone, .interface_type = _HARDWARE_6PWM }; - - if (_initHardware6PWMPair(PWM_freq, dead_zone, pinA_h, pinA_l, params, 0) == SIMPLEFOC_DRIVER_INIT_FAILED) + if (_stm32_initHardware6PWMPair(PWM_freq, dead_zone, pinA_h, pinA_l, params, 0) == SIMPLEFOC_DRIVER_INIT_FAILED) return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; - if(_initHardware6PWMPair(PWM_freq, dead_zone, pinB_h, pinB_l, params, 2) == SIMPLEFOC_DRIVER_INIT_FAILED) + if(_stm32_initHardware6PWMPair(PWM_freq, dead_zone, pinB_h, pinB_l, params, 2) == SIMPLEFOC_DRIVER_INIT_FAILED) return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; - if (_initHardware6PWMPair(PWM_freq, dead_zone, pinC_h, pinC_l, params, 4) == SIMPLEFOC_DRIVER_INIT_FAILED) + if (_stm32_initHardware6PWMPair(PWM_freq, dead_zone, pinC_h, pinC_l, params, 4) == SIMPLEFOC_DRIVER_INIT_FAILED) return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; - + params->num_timers = stm32_countTimers(params->timers_handle, 6); return params; } @@ -343,214 +386,33 @@ STM32DriverParams* _initHardware6PWMInterface(long PWM_freq, float dead_zone, Pi -/* - timer combination scoring function - assigns a score, and also checks the combination is valid - returns <0 if combination is invalid, >=0 if combination is valid. lower (but positive) score is better - for 6 pwm, hardware 6-pwm is preferred over software 6-pwm - hardware 6-pwm is possible if each low channel is the inverted counterpart of its high channel - inverted channels are not allowed except when using hardware 6-pwm (in theory they could be but lets not complicate things) -*/ -int scoreCombination(int numPins, PinMap* pinTimers[]) { - // check already used - TODO move this to outer loop also... - for (int i=0; iperipheral == timerPinsUsed[i]->peripheral - && STM_PIN_CHANNEL(pinTimers[i]->function) == STM_PIN_CHANNEL(timerPinsUsed[i]->function)) - return -2; // bad combination - timer channel already used - } - - // TODO LPTIM and HRTIM should be ignored for now - - // check for inverted channels - if (numPins < 6) { - for (int i=0; ifunction)) - return -3; // bad combination - inverted channel used in non-hardware 6pwm - } - } - // check for duplicated channels - for (int i=0; iperipheral == pinTimers[j]->peripheral - && STM_PIN_CHANNEL(pinTimers[i]->function) == STM_PIN_CHANNEL(pinTimers[j]->function) - && STM_PIN_INVERTED(pinTimers[i]->function) == STM_PIN_INVERTED(pinTimers[j]->function)) - return -4; // bad combination - duplicated channel - } - } - int score = 0; - for (int i=0; iperipheral == pinTimers[j]->peripheral) - found = true; - } - if (!found) score++; - } - if (numPins==6) { - // check for inverted channels - best: 1 timer, 3 channels, 3 matching inverted channels - // >1 timer, 3 channels, 3 matching inverted channels - // 1 timer, 6 channels (no inverted channels) - // >1 timer, 6 channels (no inverted channels) - // check for inverted high-side channels - TODO is this a configuration we should allow? what if all 3 high side channels are inverted and the low-side non-inverted? - if (STM_PIN_INVERTED(pinTimers[0]->function) || STM_PIN_INVERTED(pinTimers[2]->function) || STM_PIN_INVERTED(pinTimers[4]->function)) - return -5; // bad combination - inverted channel used on high-side channel - if (pinTimers[0]->peripheral == pinTimers[1]->peripheral - && pinTimers[2]->peripheral == pinTimers[3]->peripheral - && pinTimers[4]->peripheral == pinTimers[5]->peripheral - && STM_PIN_CHANNEL(pinTimers[0]->function) == STM_PIN_CHANNEL(pinTimers[1]->function) - && STM_PIN_CHANNEL(pinTimers[2]->function) == STM_PIN_CHANNEL(pinTimers[3]->function) - && STM_PIN_CHANNEL(pinTimers[4]->function) == STM_PIN_CHANNEL(pinTimers[5]->function) - && STM_PIN_INVERTED(pinTimers[1]->function) && STM_PIN_INVERTED(pinTimers[3]->function) && STM_PIN_INVERTED(pinTimers[5]->function)) { - // hardware 6pwm, score <10 - - // TODO F37xxx doesn't support dead-time insertion, it has no TIM1/TIM8 - // F301, F302 --> 6 channels, but only 1-3 have dead-time insertion - // TIM2/TIM3/TIM4/TIM5 don't do dead-time insertion - // TIM15/TIM16/TIM17 do dead-time insertion only on channel 1 - - // TODO check these defines - #if defined(STM32F4xx_HAL_TIM_H) || defined(STM32F3xx_HAL_TIM_H) || defined(STM32F2xx_HAL_TIM_H) || defined(STM32F1xx_HAL_TIM_H) || defined(STM32F100_HAL_TIM_H) || defined(STM32FG0x1_HAL_TIM_H) || defined(STM32G0x0_HAL_TIM_H) - if (STM_PIN_CHANNEL(pinTimers[0]->function)>3 || STM_PIN_CHANNEL(pinTimers[2]->function)>3 || STM_PIN_CHANNEL(pinTimers[4]->function)>3 ) - return -8; // channel 4 does not have dead-time insertion - #endif - #ifdef STM32G4xx_HAL_TIM_H - if (STM_PIN_CHANNEL(pinTimers[0]->function)>4 || STM_PIN_CHANNEL(pinTimers[2]->function)>4 || STM_PIN_CHANNEL(pinTimers[4]->function)>4 ) - return -8; // channels 5 & 6 do not have dead-time insertion - #endif - } - else { - // check for inverted low-side channels - if (STM_PIN_INVERTED(pinTimers[1]->function) || STM_PIN_INVERTED(pinTimers[3]->function) || STM_PIN_INVERTED(pinTimers[5]->function)) - return -6; // bad combination - inverted channel used on low-side channel in software 6-pwm - if (pinTimers[0]->peripheral != pinTimers[1]->peripheral - || pinTimers[2]->peripheral != pinTimers[3]->peripheral - || pinTimers[4]->peripheral != pinTimers[5]->peripheral) - return -7; // bad combination - non-matching timers for H/L side in software 6-pwm - score += 10; // software 6pwm, score >10 - } - } - return score; -} - - - - -int findIndexOfFirstPinMapEntry(int pin) { - PinName pinName = digitalPinToPinName(pin); - int i = 0; - while (PinMap_TIM[i].pin!=NC) { - if (pinName == PinMap_TIM[i].pin) - return i; - i++; - } - return -1; -} - - -int findIndexOfLastPinMapEntry(int pin) { - PinName pinName = digitalPinToPinName(pin); - int i = 0; - while (PinMap_TIM[i].pin!=NC) { - if ( pinName == (PinMap_TIM[i].pin & ~ALTX_MASK) - && pinName != (PinMap_TIM[i+1].pin & ~ALTX_MASK)) - return i; - i++; - } - return -1; -} - - - - - - -#define NOT_FOUND 1000 - -int findBestTimerCombination(int numPins, int index, int pins[], PinMap* pinTimers[]) { - PinMap* searchArray[numPins]; - for (int j=0;j=0 && score= 0) { - #ifdef SIMPLEFOC_STM32_DEBUG - SimpleFOCDebug::print("STM32-DRV: best: "); - printTimerCombination(numPins, pinTimers, bestScore); - #endif - } - return bestScore; -}; - - - void* _configure1PWM(long pwm_frequency, const int pinA) { - if (numTimerPinsUsed+1 > SIMPLEFOC_STM32_MAX_PINTIMERSUSED) { - SIMPLEFOC_DEBUG("STM32-DRV: ERR: too many pins used"); + if (numMotorsUsed+1 > SIMPLEFOC_STM32_MAX_MOTORSUSED) { + SIMPLEFOC_DEBUG("STM32-DRV: ERR: too many drivers used"); return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; } - 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 - // center-aligned frequency is uses two periods - pwm_frequency *=2; + if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = SIMPLEFOC_STM32_PWM_FREQUENCY; // default frequency 25khz int pins[1] = { pinA }; - PinMap* pinTimers[1] = { NP }; - if (findBestTimerCombination(1, pins, pinTimers)<0) + PinMap* pinTimers[1] = { NULL }; + if (stm32_findBestTimerCombination(1, pins, pinTimers)<0) return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; - HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinTimers[0]);\ - // allign the timers - _alignTimersNew(); - + TIM_HandleTypeDef* HT1 = stm32_initPinPWM(pwm_frequency, pinTimers[0], TIM_OCMODE_PWM1, (SIMPLEFOC_PWM_ACTIVE_HIGH)?TIM_OCPOLARITY_HIGH:TIM_OCPOLARITY_LOW); uint32_t channel1 = STM_PIN_CHANNEL(pinTimers[0]->function); STM32DriverParams* params = new STM32DriverParams { - .timers = { HT1 }, + .timers_handle = { HT1 }, .channels = { channel1 }, - .pwm_frequency = pwm_frequency + .llchannels = { stm32_getLLChannel(pinTimers[0]) }, + .pwm_frequency = pwm_frequency, + .num_timers = 1, + .master_timer = NULL }; - timerPinsUsed[numTimerPinsUsed++] = pinTimers[0]; + // align the timers (in this case, just start them) + params->master_timer = stm32_alignTimers(params->timers_handle, 1); + motorsUsed[numMotorsUsed++] = params; return params; } @@ -562,80 +424,79 @@ void* _configure1PWM(long pwm_frequency, const int pinA) { // - Stepper motor - 2PWM setting // - hardware speciffic void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { - if (numTimerPinsUsed+2 > SIMPLEFOC_STM32_MAX_PINTIMERSUSED) { - SIMPLEFOC_DEBUG("STM32-DRV: ERR: too many pins used"); + if (numMotorsUsed+1 > SIMPLEFOC_STM32_MAX_MOTORSUSED) { + SIMPLEFOC_DEBUG("STM32-DRV: ERR: too many drivers used"); return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; } - 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 - // center-aligned frequency is uses two periods - pwm_frequency *=2; + if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = SIMPLEFOC_STM32_PWM_FREQUENCY; // default frequency 25khz int pins[2] = { pinA, pinB }; - PinMap* pinTimers[2] = { NP, NP }; - if (findBestTimerCombination(2, pins, pinTimers)<0) + PinMap* pinTimers[2] = { NULL, NULL }; + if (stm32_findBestTimerCombination(2, pins, pinTimers)<0) return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; - HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinTimers[0]); - HardwareTimer* HT2 = _initPinPWM(pwm_frequency, pinTimers[1]); - // allign the timers - _alignPWMTimers(HT1, HT2, HT2); + TIM_HandleTypeDef* HT1 = stm32_initPinPWM(pwm_frequency, pinTimers[0], TIM_OCMODE_PWM1, (SIMPLEFOC_PWM_ACTIVE_HIGH)?TIM_OCPOLARITY_HIGH:TIM_OCPOLARITY_LOW); + TIM_HandleTypeDef* HT2 = stm32_initPinPWM(pwm_frequency, pinTimers[1], TIM_OCMODE_PWM1, (SIMPLEFOC_PWM_ACTIVE_HIGH)?TIM_OCPOLARITY_HIGH:TIM_OCPOLARITY_LOW); + TIM_HandleTypeDef *timers[2] = {HT1, HT2}; + stm32_checkTimerFrequency(pwm_frequency, timers, 2); uint32_t channel1 = STM_PIN_CHANNEL(pinTimers[0]->function); uint32_t channel2 = STM_PIN_CHANNEL(pinTimers[1]->function); STM32DriverParams* params = new STM32DriverParams { - .timers = { HT1, HT2 }, + .timers_handle = { HT1, HT2 }, .channels = { channel1, channel2 }, - .pwm_frequency = pwm_frequency + .llchannels = { stm32_getLLChannel(pinTimers[0]), stm32_getLLChannel(pinTimers[1]) }, + .pwm_frequency = pwm_frequency, // TODO set to actual frequency + .num_timers = stm32_countTimers(timers, 2), + .master_timer = NULL }; - timerPinsUsed[numTimerPinsUsed++] = pinTimers[0]; - timerPinsUsed[numTimerPinsUsed++] = pinTimers[1]; + // align the timers + params->master_timer = stm32_alignTimers(timers, 2); + motorsUsed[numMotorsUsed++] = params; return params; } - // function setting the high pwm frequency to the supplied pins // - BLDC motor - 3PWM setting // - hardware speciffic void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { - if (numTimerPinsUsed+3 > SIMPLEFOC_STM32_MAX_PINTIMERSUSED) { - SIMPLEFOC_DEBUG("STM32-DRV: ERR: too many pins used"); + if (numMotorsUsed+1 > SIMPLEFOC_STM32_MAX_MOTORSUSED) { + SIMPLEFOC_DEBUG("STM32-DRV: ERR: too many drivers used"); return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; } - 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 - // center-aligned frequency is uses two periods - pwm_frequency *=2; + if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = SIMPLEFOC_STM32_PWM_FREQUENCY; // default frequency 25khz int pins[3] = { pinA, pinB, pinC }; - PinMap* pinTimers[3] = { NP, NP, NP }; - if (findBestTimerCombination(3, pins, pinTimers)<0) + PinMap* pinTimers[3] = { NULL, NULL, NULL }; + if (stm32_findBestTimerCombination(3, pins, pinTimers)<0) return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; - HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinTimers[0]); - HardwareTimer* HT2 = _initPinPWM(pwm_frequency, pinTimers[1]); - HardwareTimer* HT3 = _initPinPWM(pwm_frequency, pinTimers[2]); - + TIM_HandleTypeDef* HT1 = stm32_initPinPWM(pwm_frequency, pinTimers[0], TIM_OCMODE_PWM1, (SIMPLEFOC_PWM_ACTIVE_HIGH)?TIM_OCPOLARITY_HIGH:TIM_OCPOLARITY_LOW); + TIM_HandleTypeDef* HT2 = stm32_initPinPWM(pwm_frequency, pinTimers[1], TIM_OCMODE_PWM1, (SIMPLEFOC_PWM_ACTIVE_HIGH)?TIM_OCPOLARITY_HIGH:TIM_OCPOLARITY_LOW); + TIM_HandleTypeDef* HT3 = stm32_initPinPWM(pwm_frequency, pinTimers[2], TIM_OCMODE_PWM1, (SIMPLEFOC_PWM_ACTIVE_HIGH)?TIM_OCPOLARITY_HIGH:TIM_OCPOLARITY_LOW); + + TIM_HandleTypeDef *timers[3] = {HT1, HT2, HT3}; + stm32_checkTimerFrequency(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); STM32DriverParams* params = new STM32DriverParams { - .timers = { HT1, HT2, HT3 }, + .timers_handle = { HT1, HT2, HT3 }, .channels = { channel1, channel2, channel3 }, - .pwm_frequency = pwm_frequency + .llchannels = { stm32_getLLChannel(pinTimers[0]), stm32_getLLChannel(pinTimers[1]), stm32_getLLChannel(pinTimers[2]) }, + .pwm_frequency = pwm_frequency, + .num_timers = stm32_countTimers(timers, 3), + .master_timer = NULL }; - timerPinsUsed[numTimerPinsUsed++] = pinTimers[0]; - timerPinsUsed[numTimerPinsUsed++] = pinTimers[1]; - timerPinsUsed[numTimerPinsUsed++] = pinTimers[2]; - - _alignTimersNew(); - + params->master_timer = stm32_alignTimers(timers, 3); + motorsUsed[numMotorsUsed++] = params; return params; } @@ -645,26 +506,23 @@ void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const in // - Stepper motor - 4PWM setting // - hardware speciffic void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { - if (numTimerPinsUsed+4 > SIMPLEFOC_STM32_MAX_PINTIMERSUSED) { - SIMPLEFOC_DEBUG("STM32-DRV: ERR: too many pins used"); + if (numMotorsUsed+1 > SIMPLEFOC_STM32_MAX_MOTORSUSED) { + SIMPLEFOC_DEBUG("STM32-DRV: ERR: too many drivers used"); return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; } - 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 - // center-aligned frequency is uses two periods - pwm_frequency *=2; + if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = SIMPLEFOC_STM32_PWM_FREQUENCY; // default frequency 25khz int pins[4] = { pinA, pinB, pinC, pinD }; - PinMap* pinTimers[4] = { NP, NP, NP, NP }; - if (findBestTimerCombination(4, pins, pinTimers)<0) + PinMap* pinTimers[4] = { NULL, NULL, NULL, NULL }; + if (stm32_findBestTimerCombination(4, pins, pinTimers)<0) return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; - HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinTimers[0]); - HardwareTimer* HT2 = _initPinPWM(pwm_frequency, pinTimers[1]); - HardwareTimer* HT3 = _initPinPWM(pwm_frequency, pinTimers[2]); - HardwareTimer* HT4 = _initPinPWM(pwm_frequency, pinTimers[3]); - // allign the timers - _alignPWMTimers(HT1, HT2, HT3, HT4); + TIM_HandleTypeDef* HT1 = stm32_initPinPWM(pwm_frequency, pinTimers[0], TIM_OCMODE_PWM1, (SIMPLEFOC_PWM_ACTIVE_HIGH)?TIM_OCPOLARITY_HIGH:TIM_OCPOLARITY_LOW); + TIM_HandleTypeDef* HT2 = stm32_initPinPWM(pwm_frequency, pinTimers[1], TIM_OCMODE_PWM1, (SIMPLEFOC_PWM_ACTIVE_HIGH)?TIM_OCPOLARITY_HIGH:TIM_OCPOLARITY_LOW); + TIM_HandleTypeDef* HT3 = stm32_initPinPWM(pwm_frequency, pinTimers[2], TIM_OCMODE_PWM1, (SIMPLEFOC_PWM_ACTIVE_HIGH)?TIM_OCPOLARITY_HIGH:TIM_OCPOLARITY_LOW); + TIM_HandleTypeDef* HT4 = stm32_initPinPWM(pwm_frequency, pinTimers[3], TIM_OCMODE_PWM1, (SIMPLEFOC_PWM_ACTIVE_HIGH)?TIM_OCPOLARITY_HIGH:TIM_OCPOLARITY_LOW); + TIM_HandleTypeDef *timers[4] = {HT1, HT2, HT3, HT4}; + stm32_checkTimerFrequency(pwm_frequency, timers, 4); uint32_t channel1 = STM_PIN_CHANNEL(pinTimers[0]->function); uint32_t channel2 = STM_PIN_CHANNEL(pinTimers[1]->function); @@ -672,14 +530,15 @@ void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const in uint32_t channel4 = STM_PIN_CHANNEL(pinTimers[3]->function); STM32DriverParams* params = new STM32DriverParams { - .timers = { HT1, HT2, HT3, HT4 }, + .timers_handle = { HT1, HT2, HT3, HT4 }, .channels = { channel1, channel2, channel3, channel4 }, - .pwm_frequency = pwm_frequency + .llchannels = { stm32_getLLChannel(pinTimers[0]), stm32_getLLChannel(pinTimers[1]), stm32_getLLChannel(pinTimers[2]), stm32_getLLChannel(pinTimers[3]) }, + .pwm_frequency = pwm_frequency, + .num_timers = stm32_countTimers(timers, 4), + .master_timer = NULL }; - timerPinsUsed[numTimerPinsUsed++] = pinTimers[0]; - timerPinsUsed[numTimerPinsUsed++] = pinTimers[1]; - timerPinsUsed[numTimerPinsUsed++] = pinTimers[2]; - timerPinsUsed[numTimerPinsUsed++] = pinTimers[3]; + params->master_timer = stm32_alignTimers(timers, 4); + motorsUsed[numMotorsUsed++] = params; return params; } @@ -687,43 +546,55 @@ void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const in // function setting the pwm duty cycle to the hardware // - DC motor - 1PWM setting -// - hardware speciffic +// - hardware specific void _writeDutyCycle1PWM(float dc_a, void* params){ - // transform duty cycle from [0,1] to [0,255] - _setPwm(((STM32DriverParams*)params)->timers[0], ((STM32DriverParams*)params)->channels[0], _PWM_RANGE*dc_a, _PWM_RESOLUTION); + uint32_t duty = dc_a*(((STM32DriverParams*)params)->timers_handle[0]->Instance->ARR+1); + stm32_setPwm(((STM32DriverParams*)params)->timers_handle[0], ((STM32DriverParams*)params)->channels[0], duty); } // 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,4095] - _setPwm(((STM32DriverParams*)params)->timers[0], ((STM32DriverParams*)params)->channels[0], _PWM_RANGE*dc_a, _PWM_RESOLUTION); - _setPwm(((STM32DriverParams*)params)->timers[1], ((STM32DriverParams*)params)->channels[1], _PWM_RANGE*dc_b, _PWM_RESOLUTION); + uint32_t duty1 = dc_a*(((STM32DriverParams*)params)->timers_handle[0]->Instance->ARR+1); + uint32_t duty2 = dc_b*(((STM32DriverParams*)params)->timers_handle[1]->Instance->ARR+1); + if (((STM32DriverParams*)params)->num_timers == 1) LL_TIM_DisableUpdateEvent(((STM32DriverParams*)params)->timers_handle[0]->Instance); + stm32_setPwm(((STM32DriverParams*)params)->timers_handle[0], ((STM32DriverParams*)params)->channels[0], duty1); + stm32_setPwm(((STM32DriverParams*)params)->timers_handle[1], ((STM32DriverParams*)params)->channels[1], duty2); + if (((STM32DriverParams*)params)->num_timers == 1) LL_TIM_EnableUpdateEvent(((STM32DriverParams*)params)->timers_handle[0]->Instance); } // 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,4095] - _setPwm(((STM32DriverParams*)params)->timers[0], ((STM32DriverParams*)params)->channels[0], _PWM_RANGE*dc_a, _PWM_RESOLUTION); - _setPwm(((STM32DriverParams*)params)->timers[1], ((STM32DriverParams*)params)->channels[1], _PWM_RANGE*dc_b, _PWM_RESOLUTION); - _setPwm(((STM32DriverParams*)params)->timers[2], ((STM32DriverParams*)params)->channels[2], _PWM_RANGE*dc_c, _PWM_RESOLUTION); + uint32_t duty1 = dc_a*(((STM32DriverParams*)params)->timers_handle[0]->Instance->ARR+1); + uint32_t duty2 = dc_b*(((STM32DriverParams*)params)->timers_handle[1]->Instance->ARR+1); + uint32_t duty3 = dc_c*(((STM32DriverParams*)params)->timers_handle[2]->Instance->ARR+1); + if (((STM32DriverParams*)params)->num_timers == 1) LL_TIM_DisableUpdateEvent(((STM32DriverParams*)params)->timers_handle[0]->Instance); + stm32_setPwm(((STM32DriverParams*)params)->timers_handle[0], ((STM32DriverParams*)params)->channels[0], duty1); + stm32_setPwm(((STM32DriverParams*)params)->timers_handle[1], ((STM32DriverParams*)params)->channels[1], duty2); + stm32_setPwm(((STM32DriverParams*)params)->timers_handle[2], ((STM32DriverParams*)params)->channels[2], duty3); + if (((STM32DriverParams*)params)->num_timers == 1) LL_TIM_EnableUpdateEvent(((STM32DriverParams*)params)->timers_handle[0]->Instance); } // 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,4095] - _setPwm(((STM32DriverParams*)params)->timers[0], ((STM32DriverParams*)params)->channels[0], _PWM_RANGE*dc_1a, _PWM_RESOLUTION); - _setPwm(((STM32DriverParams*)params)->timers[1], ((STM32DriverParams*)params)->channels[1], _PWM_RANGE*dc_1b, _PWM_RESOLUTION); - _setPwm(((STM32DriverParams*)params)->timers[2], ((STM32DriverParams*)params)->channels[2], _PWM_RANGE*dc_2a, _PWM_RESOLUTION); - _setPwm(((STM32DriverParams*)params)->timers[3], ((STM32DriverParams*)params)->channels[3], _PWM_RANGE*dc_2b, _PWM_RESOLUTION); + uint32_t duty1 = dc_1a*(((STM32DriverParams*)params)->timers_handle[0]->Instance->ARR+1); + uint32_t duty2 = dc_1b*(((STM32DriverParams*)params)->timers_handle[1]->Instance->ARR+1); + uint32_t duty3 = dc_2a*(((STM32DriverParams*)params)->timers_handle[2]->Instance->ARR+1); + uint32_t duty4 = dc_2b*(((STM32DriverParams*)params)->timers_handle[3]->Instance->ARR+1); + if (((STM32DriverParams*)params)->num_timers == 1) LL_TIM_DisableUpdateEvent(((STM32DriverParams*)params)->timers_handle[0]->Instance); + stm32_setPwm(((STM32DriverParams*)params)->timers_handle[0], ((STM32DriverParams*)params)->channels[0], duty1); + stm32_setPwm(((STM32DriverParams*)params)->timers_handle[1], ((STM32DriverParams*)params)->channels[1], duty2); + stm32_setPwm(((STM32DriverParams*)params)->timers_handle[2], ((STM32DriverParams*)params)->channels[2], duty3); + stm32_setPwm(((STM32DriverParams*)params)->timers_handle[3], ((STM32DriverParams*)params)->channels[3], duty4); + if (((STM32DriverParams*)params)->num_timers == 1) LL_TIM_EnableUpdateEvent(((STM32DriverParams*)params)->timers_handle[0]->Instance); } @@ -733,33 +604,32 @@ void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, vo // - BLDC driver - 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 (numTimerPinsUsed+6 > SIMPLEFOC_STM32_MAX_PINTIMERSUSED) { - SIMPLEFOC_DEBUG("STM32-DRV: ERR: too many pins used"); + if (numMotorsUsed+1 > SIMPLEFOC_STM32_MAX_MOTORSUSED) { + SIMPLEFOC_DEBUG("STM32-DRV: ERR: too many drivers used"); return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; } - 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 |%0kHz max - // center-aligned frequency is uses two periods - pwm_frequency *=2; + if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = SIMPLEFOC_STM32_PWM_FREQUENCY; // default frequency 25khz // find configuration int pins[6] = { pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l }; - PinMap* pinTimers[6] = { NP, NP, NP, NP, NP, NP }; - int score = findBestTimerCombination(6, pins, pinTimers); + PinMap* pinTimers[6] = { NULL, NULL, NULL, NULL, NULL, NULL }; + int score = stm32_findBestTimerCombination(6, pins, pinTimers); STM32DriverParams* params; // configure accordingly if (score<0) params = (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; else if (score<10) // hardware pwm - params = _initHardware6PWMInterface(pwm_frequency, dead_zone, pinTimers[0], pinTimers[1], pinTimers[2], pinTimers[3], pinTimers[4], pinTimers[5]); + params = _stm32_initHardware6PWMInterface(pwm_frequency, dead_zone, pinTimers[0], pinTimers[1], pinTimers[2], pinTimers[3], pinTimers[4], pinTimers[5]); else { // software pwm - HardwareTimer* HT1 = _initPinPWMHigh(pwm_frequency, pinTimers[0]); - HardwareTimer* HT2 = _initPinPWMLow(pwm_frequency, pinTimers[1]); - HardwareTimer* HT3 = _initPinPWMHigh(pwm_frequency, pinTimers[2]); - HardwareTimer* HT4 = _initPinPWMLow(pwm_frequency, pinTimers[3]); - HardwareTimer* HT5 = _initPinPWMHigh(pwm_frequency, pinTimers[4]); - HardwareTimer* HT6 = _initPinPWMLow(pwm_frequency, pinTimers[5]); + TIM_HandleTypeDef* HT1 = _stm32_initPinPWMHigh(pwm_frequency, pinTimers[0]); + TIM_HandleTypeDef* HT2 = _stm32_initPinPWMLow(pwm_frequency, pinTimers[1]); + TIM_HandleTypeDef* HT3 = _stm32_initPinPWMHigh(pwm_frequency, pinTimers[2]); + TIM_HandleTypeDef* HT4 = _stm32_initPinPWMLow(pwm_frequency, pinTimers[3]); + TIM_HandleTypeDef* HT5 = _stm32_initPinPWMHigh(pwm_frequency, pinTimers[4]); + TIM_HandleTypeDef* HT6 = _stm32_initPinPWMLow(pwm_frequency, pinTimers[5]); + TIM_HandleTypeDef *timers[6] = {HT1, HT2, HT3, HT4, HT5, HT6}; + stm32_checkTimerFrequency(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); @@ -767,33 +637,41 @@ void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, cons uint32_t channel5 = STM_PIN_CHANNEL(pinTimers[4]->function); uint32_t channel6 = STM_PIN_CHANNEL(pinTimers[5]->function); params = new STM32DriverParams { - .timers = { HT1, HT2, HT3, HT4, HT5, HT6 }, + .timers_handle = { HT1, HT2, HT3, HT4, HT5, HT6 }, .channels = { channel1, channel2, channel3, channel4, channel5, channel6 }, + .llchannels = { stm32_getLLChannel(pinTimers[0]), stm32_getLLChannel(pinTimers[1]), stm32_getLLChannel(pinTimers[2]), stm32_getLLChannel(pinTimers[3]), stm32_getLLChannel(pinTimers[4]), stm32_getLLChannel(pinTimers[5]) }, .pwm_frequency = pwm_frequency, + .num_timers = stm32_countTimers(timers, 6), + .master_timer = NULL, .dead_zone = dead_zone, .interface_type = _SOFTWARE_6PWM }; } if (score>=0) { - for (int i=0; i<6; i++) - timerPinsUsed[numTimerPinsUsed++] = pinTimers[i]; - _alignTimersNew(); + params->master_timer = stm32_alignTimers(params->timers_handle, 6); + motorsUsed[numMotorsUsed++] = params; } return params; // success } -void _setSinglePhaseState(PhaseState state, HardwareTimer *HT, int channel1,int channel2) { - _UNUSED(channel2); +void _setSinglePhaseState(PhaseState state, TIM_HandleTypeDef *HT, int llchannel_hi, int llchannel_lo) { switch (state) { case PhaseState::PHASE_OFF: - // Due to a weird quirk in the arduino timer API, pauseChannel only disables the complementary channel (e.g. CC1NE). - // To actually make the phase floating, we must also set pwm to 0. - HT->pauseChannel(channel1); + stm32_pauseChannel(HT, llchannel_hi | llchannel_lo); break; + case PhaseState::PHASE_HI: + stm32_pauseChannel(HT, llchannel_lo); + stm32_resumeChannel(HT, llchannel_hi); + break; + case PhaseState::PHASE_LO: + stm32_pauseChannel(HT, llchannel_hi); + stm32_resumeChannel(HT, llchannel_lo); + break; + case PhaseState::PHASE_ON: default: - HT->resumeChannel(channel1); + stm32_resumeChannel(HT, llchannel_hi | llchannel_lo); break; } } @@ -803,150 +681,72 @@ void _setSinglePhaseState(PhaseState state, HardwareTimer *HT, int channel1,int // - BLDC driver - 6PWM setting // - hardware specific void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState* phase_state, void* params){ + uint32_t duty1; + uint32_t duty2; + uint32_t duty3; switch(((STM32DriverParams*)params)->interface_type){ case _HARDWARE_6PWM: - // phase a - _setSinglePhaseState(phase_state[0], ((STM32DriverParams*)params)->timers[0], ((STM32DriverParams*)params)->channels[0], ((STM32DriverParams*)params)->channels[1]); - if(phase_state[0] == PhaseState::PHASE_OFF) dc_a = 0.0f; - _setPwm(((STM32DriverParams*)params)->timers[0], ((STM32DriverParams*)params)->channels[0], _PWM_RANGE*dc_a, _PWM_RESOLUTION); - // phase b - _setSinglePhaseState(phase_state[1], ((STM32DriverParams*)params)->timers[2], ((STM32DriverParams*)params)->channels[2], ((STM32DriverParams*)params)->channels[3]); - if(phase_state[1] == PhaseState::PHASE_OFF) dc_b = 0.0f; - _setPwm(((STM32DriverParams*)params)->timers[2], ((STM32DriverParams*)params)->channels[2], _PWM_RANGE*dc_b, _PWM_RESOLUTION); - // phase c - _setSinglePhaseState(phase_state[2], ((STM32DriverParams*)params)->timers[4], ((STM32DriverParams*)params)->channels[4], ((STM32DriverParams*)params)->channels[5]); - if(phase_state[2] == PhaseState::PHASE_OFF) dc_c = 0.0f; - _setPwm(((STM32DriverParams*)params)->timers[4], ((STM32DriverParams*)params)->channels[4], _PWM_RANGE*dc_c, _PWM_RESOLUTION); + duty1 = dc_a*(((STM32DriverParams*)params)->timers_handle[0]->Instance->ARR+1); + duty2 = dc_b*(((STM32DriverParams*)params)->timers_handle[2]->Instance->ARR+1); + duty3 = dc_c*(((STM32DriverParams*)params)->timers_handle[4]->Instance->ARR+1); + if(phase_state[0] == PhaseState::PHASE_OFF) duty1 = 0; + if(phase_state[1] == PhaseState::PHASE_OFF) duty2 = 0; + if(phase_state[2] == PhaseState::PHASE_OFF) duty3 = 0; + if (((STM32DriverParams*)params)->num_timers == 1) LL_TIM_DisableUpdateEvent(((STM32DriverParams*)params)->timers_handle[0]->Instance); + _setSinglePhaseState(phase_state[0], ((STM32DriverParams*)params)->timers_handle[0], ((STM32DriverParams*)params)->llchannels[0], ((STM32DriverParams*)params)->llchannels[1]); + stm32_setPwm(((STM32DriverParams*)params)->timers_handle[0], ((STM32DriverParams*)params)->channels[0], duty1); + _setSinglePhaseState(phase_state[1], ((STM32DriverParams*)params)->timers_handle[2], ((STM32DriverParams*)params)->llchannels[2], ((STM32DriverParams*)params)->llchannels[3]); + stm32_setPwm(((STM32DriverParams*)params)->timers_handle[2], ((STM32DriverParams*)params)->channels[2], duty2); + _setSinglePhaseState(phase_state[2], ((STM32DriverParams*)params)->timers_handle[4], ((STM32DriverParams*)params)->llchannels[4], ((STM32DriverParams*)params)->llchannels[5]); + stm32_setPwm(((STM32DriverParams*)params)->timers_handle[4], ((STM32DriverParams*)params)->channels[4], duty3); + if (((STM32DriverParams*)params)->num_timers == 1) LL_TIM_EnableUpdateEvent(((STM32DriverParams*)params)->timers_handle[0]->Instance); break; case _SOFTWARE_6PWM: float dead_zone = ((STM32DriverParams*)params)->dead_zone / 2.0f; + duty1 = _constrain(dc_a - dead_zone, 0.0f, 1.0f)*(((STM32DriverParams*)params)->timers_handle[0]->Instance->ARR+1); + duty2 = _constrain(dc_b - dead_zone, 0.0f, 1.0f)*(((STM32DriverParams*)params)->timers_handle[2]->Instance->ARR+1); + duty3 = _constrain(dc_c - dead_zone, 0.0f, 1.0f)*(((STM32DriverParams*)params)->timers_handle[4]->Instance->ARR+1); + uint32_t duty1N = _constrain(dc_a + dead_zone, 0.0f, 1.0f)*(((STM32DriverParams*)params)->timers_handle[1]->Instance->ARR+1); + uint32_t duty2N = _constrain(dc_b + dead_zone, 0.0f, 1.0f)*(((STM32DriverParams*)params)->timers_handle[3]->Instance->ARR+1); + uint32_t duty3N = _constrain(dc_c + dead_zone, 0.0f, 1.0f)*(((STM32DriverParams*)params)->timers_handle[5]->Instance->ARR+1); + + LL_TIM_DisableUpdateEvent(((STM32DriverParams*)params)->timers_handle[0]->Instance); // timers for high and low side assumed to be the same timer if (phase_state[0] == PhaseState::PHASE_ON || phase_state[0] == PhaseState::PHASE_HI) - _setPwm(((STM32DriverParams*)params)->timers[0], ((STM32DriverParams*)params)->channels[0], _constrain(dc_a - dead_zone, 0.0f, 1.0f)*_PWM_RANGE, _PWM_RESOLUTION); + stm32_setPwm(((STM32DriverParams*)params)->timers_handle[0], ((STM32DriverParams*)params)->channels[0], duty1); else - _setPwm(((STM32DriverParams*)params)->timers[0], ((STM32DriverParams*)params)->channels[0], 0.0f, _PWM_RESOLUTION); + stm32_setPwm(((STM32DriverParams*)params)->timers_handle[0], ((STM32DriverParams*)params)->channels[0], 0); if (phase_state[0] == PhaseState::PHASE_ON || phase_state[0] == PhaseState::PHASE_LO) - _setPwm(((STM32DriverParams*)params)->timers[1], ((STM32DriverParams*)params)->channels[1], _constrain(dc_a + dead_zone, 0.0f, 1.0f)*_PWM_RANGE, _PWM_RESOLUTION); + stm32_setPwm(((STM32DriverParams*)params)->timers_handle[1], ((STM32DriverParams*)params)->channels[1], duty1N); else - _setPwm(((STM32DriverParams*)params)->timers[1], ((STM32DriverParams*)params)->channels[1], 0.0f, _PWM_RESOLUTION); + stm32_setPwm(((STM32DriverParams*)params)->timers_handle[1], ((STM32DriverParams*)params)->channels[1], 0); + LL_TIM_DisableUpdateEvent(((STM32DriverParams*)params)->timers_handle[2]->Instance); // timers for high and low side assumed to be the same timer if (phase_state[1] == PhaseState::PHASE_ON || phase_state[1] == PhaseState::PHASE_HI) - _setPwm(((STM32DriverParams*)params)->timers[2], ((STM32DriverParams*)params)->channels[2], _constrain(dc_b - dead_zone, 0.0f, 1.0f)*_PWM_RANGE, _PWM_RESOLUTION); + stm32_setPwm(((STM32DriverParams*)params)->timers_handle[2], ((STM32DriverParams*)params)->channels[2], duty2); else - _setPwm(((STM32DriverParams*)params)->timers[2], ((STM32DriverParams*)params)->channels[2], 0.0f, _PWM_RESOLUTION); + stm32_setPwm(((STM32DriverParams*)params)->timers_handle[2], ((STM32DriverParams*)params)->channels[2], 0); if (phase_state[1] == PhaseState::PHASE_ON || phase_state[1] == PhaseState::PHASE_LO) - _setPwm(((STM32DriverParams*)params)->timers[3], ((STM32DriverParams*)params)->channels[3], _constrain(dc_b + dead_zone, 0.0f, 1.0f)*_PWM_RANGE, _PWM_RESOLUTION); + stm32_setPwm(((STM32DriverParams*)params)->timers_handle[3], ((STM32DriverParams*)params)->channels[3], duty2N); else - _setPwm(((STM32DriverParams*)params)->timers[3], ((STM32DriverParams*)params)->channels[3], 0.0f, _PWM_RESOLUTION); + stm32_setPwm(((STM32DriverParams*)params)->timers_handle[3], ((STM32DriverParams*)params)->channels[3], 0); + LL_TIM_DisableUpdateEvent(((STM32DriverParams*)params)->timers_handle[4]->Instance); // timers for high and low side assumed to be the same timer if (phase_state[2] == PhaseState::PHASE_ON || phase_state[2] == PhaseState::PHASE_HI) - _setPwm(((STM32DriverParams*)params)->timers[4], ((STM32DriverParams*)params)->channels[4], _constrain(dc_c - dead_zone, 0.0f, 1.0f)*_PWM_RANGE, _PWM_RESOLUTION); + stm32_setPwm(((STM32DriverParams*)params)->timers_handle[4], ((STM32DriverParams*)params)->channels[4], duty3); else - _setPwm(((STM32DriverParams*)params)->timers[4], ((STM32DriverParams*)params)->channels[4], 0.0f, _PWM_RESOLUTION); + stm32_setPwm(((STM32DriverParams*)params)->timers_handle[4], ((STM32DriverParams*)params)->channels[4], 0); if (phase_state[2] == PhaseState::PHASE_ON || phase_state[2] == PhaseState::PHASE_LO) - _setPwm(((STM32DriverParams*)params)->timers[5], ((STM32DriverParams*)params)->channels[5], _constrain(dc_c + dead_zone, 0.0f, 1.0f)*_PWM_RANGE, _PWM_RESOLUTION); + stm32_setPwm(((STM32DriverParams*)params)->timers_handle[5], ((STM32DriverParams*)params)->channels[5], duty3N); else - _setPwm(((STM32DriverParams*)params)->timers[5], ((STM32DriverParams*)params)->channels[5], 0.0f, _PWM_RESOLUTION); + stm32_setPwm(((STM32DriverParams*)params)->timers_handle[5], ((STM32DriverParams*)params)->channels[5], 0); + + LL_TIM_EnableUpdateEvent(((STM32DriverParams*)params)->timers_handle[0]->Instance); + LL_TIM_EnableUpdateEvent(((STM32DriverParams*)params)->timers_handle[2]->Instance); + LL_TIM_EnableUpdateEvent(((STM32DriverParams*)params)->timers_handle[4]->Instance); break; } - _UNUSED(phase_state); -} - - - -#ifdef SIMPLEFOC_STM32_DEBUG - -int getTimerNumber(int timerIndex) { - #if defined(TIM1_BASE) - if (timerIndex==TIMER1_INDEX) return 1; - #endif - #if defined(TIM2_BASE) - if (timerIndex==TIMER2_INDEX) return 2; - #endif - #if defined(TIM3_BASE) - if (timerIndex==TIMER3_INDEX) return 3; - #endif - #if defined(TIM4_BASE) - if (timerIndex==TIMER4_INDEX) return 4; - #endif - #if defined(TIM5_BASE) - if (timerIndex==TIMER5_INDEX) return 5; - #endif - #if defined(TIM6_BASE) - if (timerIndex==TIMER6_INDEX) return 6; - #endif - #if defined(TIM7_BASE) - if (timerIndex==TIMER7_INDEX) return 7; - #endif - #if defined(TIM8_BASE) - if (timerIndex==TIMER8_INDEX) return 8; - #endif - #if defined(TIM9_BASE) - if (timerIndex==TIMER9_INDEX) return 9; - #endif - #if defined(TIM10_BASE) - if (timerIndex==TIMER10_INDEX) return 10; - #endif - #if defined(TIM11_BASE) - if (timerIndex==TIMER11_INDEX) return 11; - #endif - #if defined(TIM12_BASE) - if (timerIndex==TIMER12_INDEX) return 12; - #endif - #if defined(TIM13_BASE) - if (timerIndex==TIMER13_INDEX) return 13; - #endif - #if defined(TIM14_BASE) - if (timerIndex==TIMER14_INDEX) return 14; - #endif - #if defined(TIM15_BASE) - if (timerIndex==TIMER15_INDEX) return 15; - #endif - #if defined(TIM16_BASE) - if (timerIndex==TIMER16_INDEX) return 16; - #endif - #if defined(TIM17_BASE) - if (timerIndex==TIMER17_INDEX) return 17; - #endif - #if defined(TIM18_BASE) - if (timerIndex==TIMER18_INDEX) return 18; - #endif - #if defined(TIM19_BASE) - if (timerIndex==TIMER19_INDEX) return 19; - #endif - #if defined(TIM20_BASE) - if (timerIndex==TIMER20_INDEX) return 20; - #endif - #if defined(TIM21_BASE) - if (timerIndex==TIMER21_INDEX) return 21; - #endif - #if defined(TIM22_BASE) - if (timerIndex==TIMER22_INDEX) return 22; - #endif - return -1; -} - - -void printTimerCombination(int numPins, PinMap* timers[], int score) { - for (int i=0; iperipheral))); - SimpleFOCDebug::print("-CH"); - SimpleFOCDebug::print(STM_PIN_CHANNEL(timers[i]->function)); - if (STM_PIN_INVERTED(timers[i]->function)) - SimpleFOCDebug::print("N"); - } - SimpleFOCDebug::print(" "); - } - SimpleFOCDebug::println("score: ", 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..feee6ba2 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.h +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.h @@ -1,14 +1,39 @@ -#ifndef STM32_DRIVER_MCU_DEF -#define STM32_DRIVER_MCU_DEF +#pragma once + #include "../../hardware_api.h" -#if defined(_STM32_DEF_) +#if defined(_STM32_DEF_) || defined(TARGET_STM32H7) +// TARGET_M4 / TARGET_M7 + +#ifndef SIMPLEFOC_STM32_MAX_TIMERSUSED +#define SIMPLEFOC_STM32_MAX_TIMERSUSED 6 +#endif +#ifndef SIMPLEFOC_STM32_MAX_TIMERSRESERVED +#define SIMPLEFOC_STM32_MAX_TIMERSRESERVED 4 +#endif +#ifndef SIMPLEFOC_STM32_MAX_MOTORSUSED +#define SIMPLEFOC_STM32_MAX_MOTORSUSED 4 +#endif + + +#ifndef SIMPLEFOC_STM32_DEBUG +// comment me out to disable debug output +#define SIMPLEFOC_STM32_DEBUG +#endif + +#if defined(__MBED__) +#define PinMap_TIM PinMap_PWM +#define ALTX_MASK 0 +#endif + -// default pwm parameters -#define _PWM_RESOLUTION 12 // 12bit -#define _PWM_RANGE 4095.0f // 2^12 -1 = 4095 -#define _PWM_FREQUENCY 25000 // 25khz -#define _PWM_FREQUENCY_MAX 50000 // 50khz +/** + * No limits are placed on PWM frequency, so very fast or very slow frequencies can be set. + * A warning is displayed to debug if you get less than 8bit resolution for the PWM duty cycle. + * If no pwm_frequency is set, the default value is 25kHz. + */ +#define SIMPLEFOC_STM32_PWM_FREQUENCY 25000 // 25khz +#define SIMPLEFOC_STM32_MIN_RESOLUTION 255 // 6pwm parameters #define _HARDWARE_6PWM 1 @@ -17,16 +42,37 @@ typedef struct STM32DriverParams { - HardwareTimer* timers[6] = {NULL}; + TIM_HandleTypeDef* timers_handle[6] = {NULL}; uint32_t channels[6]; + uint32_t llchannels[6]; long pwm_frequency; + uint8_t num_timers; + TIM_HandleTypeDef* master_timer = NULL; float dead_zone; uint8_t interface_type; } STM32DriverParams; -// timer synchornisation functions -void _stopTimers(HardwareTimer **timers_to_stop, int timer_num=6); -void _startTimers(HardwareTimer **timers_to_start, int timer_num=6); -#endif +// timer allocation functions +int stm32_getNumTimersUsed(); +int stm32_getNumMotorsUsed(); +int stm32_getNumTimersReserved(); +STM32DriverParams* stm32_getMotorUsed(int index); +bool stm32_isTimerUsed(TIM_HandleTypeDef* timer); +bool stm32_isChannelUsed(PinMap* pin); +bool stm32_isTimerReserved(TIM_TypeDef* timer); +TIM_HandleTypeDef* stm32_getTimer(PinMap* timer); +TIM_HandleTypeDef* stm32_useTimer(PinMap* timer); +bool stm32_reserveTimer(TIM_TypeDef* timer); + +void stm32_pause(STM32DriverParams* params); +void stm32_resume(STM32DriverParams* params); + +// // timer synchornisation functions +// void _stopTimers(TIM_HandleTypeDef **timers_to_stop, int timer_num=6); +// void _startTimers(TIM_HandleTypeDef **timers_to_start, int timer_num=6); + +// // timer query functions +// bool _getPwmState(void* params); + #endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/stm32/stm32_searchtimers.cpp b/src/drivers/hardware_specific/stm32/stm32_searchtimers.cpp new file mode 100644 index 00000000..e04fc719 --- /dev/null +++ b/src/drivers/hardware_specific/stm32/stm32_searchtimers.cpp @@ -0,0 +1,193 @@ + +#include "./stm32_searchtimers.h" +#include "./stm32_timerutils.h" + +#if defined(_STM32_DEF_) || defined(TARGET_STM32H7) + + + +/* + timer combination scoring function + assigns a score, and also checks the combination is valid + returns <0 if combination is invalid, >=0 if combination is valid. lower (but positive) score is better + for 6 pwm, hardware 6-pwm is preferred over software 6-pwm + hardware 6-pwm is possible if each low channel is the inverted counterpart of its high channel + inverted channels are not allowed except when using hardware 6-pwm (in theory they could be but lets not complicate things) +*/ +int _stm32_scoreCombination(int numPins, PinMap* pinTimers[]) { + // check already used - TODO move this to outer loop also... + for (int i=0; ifunction)) + return -3; // bad combination - inverted channel used in non-hardware 6pwm + } + } + // check for duplicated channels + for (int i=0; iperipheral == pinTimers[j]->peripheral + && STM_PIN_CHANNEL(pinTimers[i]->function) == STM_PIN_CHANNEL(pinTimers[j]->function) + && STM_PIN_INVERTED(pinTimers[i]->function) == STM_PIN_INVERTED(pinTimers[j]->function)) + return -4; // bad combination - duplicated channel + } + } + int score = 0; + for (int i=0; iperipheral == pinTimers[j]->peripheral) + found = true; + } + if (!found) score++; + } + if (numPins==6) { + // check for inverted channels - best: 1 timer, 3 channels, 3 matching inverted channels + // >1 timer, 3 channels, 3 matching inverted channels + // 1 timer, 6 channels (no inverted channels) + // >1 timer, 6 channels (no inverted channels) + // check for inverted high-side channels + if (STM_PIN_INVERTED(pinTimers[0]->function) || STM_PIN_INVERTED(pinTimers[2]->function) || STM_PIN_INVERTED(pinTimers[4]->function)) + return -5; // bad combination - inverted channel used on high-side channel + if (pinTimers[0]->peripheral == pinTimers[1]->peripheral + && pinTimers[2]->peripheral == pinTimers[3]->peripheral + && pinTimers[4]->peripheral == pinTimers[5]->peripheral + && STM_PIN_CHANNEL(pinTimers[0]->function) == STM_PIN_CHANNEL(pinTimers[1]->function) + && STM_PIN_CHANNEL(pinTimers[2]->function) == STM_PIN_CHANNEL(pinTimers[3]->function) + && STM_PIN_CHANNEL(pinTimers[4]->function) == STM_PIN_CHANNEL(pinTimers[5]->function) + && STM_PIN_INVERTED(pinTimers[1]->function) && STM_PIN_INVERTED(pinTimers[3]->function) && STM_PIN_INVERTED(pinTimers[5]->function)) { + // hardware 6pwm, score <10 + + // TODO F37xxx doesn't support dead-time insertion, it has no TIM1/TIM8 + // F301, F302 --> 6 channels, but only 1-3 have dead-time insertion + // TIM2/TIM3/TIM4/TIM5 don't do dead-time insertion + // TIM15/TIM16/TIM17 do dead-time insertion only on channel 1 + + // TODO check these defines + #if defined(STM32F4xx_HAL_TIM_H) || defined(STM32F3xx_HAL_TIM_H) || defined(STM32F2xx_HAL_TIM_H) || defined(STM32F1xx_HAL_TIM_H) || defined(STM32F100_HAL_TIM_H) || defined(STM32FG0x1_HAL_TIM_H) || defined(STM32G0x0_HAL_TIM_H) + if (STM_PIN_CHANNEL(pinTimers[0]->function)>3 || STM_PIN_CHANNEL(pinTimers[2]->function)>3 || STM_PIN_CHANNEL(pinTimers[4]->function)>3 ) + return -8; // channel 4 does not have dead-time insertion + #endif + #ifdef STM32G4xx_HAL_TIM_H + if (STM_PIN_CHANNEL(pinTimers[0]->function)>4 || STM_PIN_CHANNEL(pinTimers[2]->function)>4 || STM_PIN_CHANNEL(pinTimers[4]->function)>4 ) + return -8; // channels 5 & 6 do not have dead-time insertion + #endif + } + else { + // check for inverted low-side channels + if (STM_PIN_INVERTED(pinTimers[1]->function) || STM_PIN_INVERTED(pinTimers[3]->function) || STM_PIN_INVERTED(pinTimers[5]->function)) + return -6; // bad combination - inverted channel used on low-side channel in software 6-pwm + if (pinTimers[0]->peripheral != pinTimers[1]->peripheral + || pinTimers[2]->peripheral != pinTimers[3]->peripheral + || pinTimers[4]->peripheral != pinTimers[5]->peripheral) + return -7; // bad combination - non-matching timers for H/L side in software 6-pwm + score += 10; // software 6pwm, score >10 + } + } + return score; +} + + + + +int _stm32_findIndexOfFirstPinMapEntry(int pin) { + PinName pinName = digitalPinToPinName(pin); + int i = 0; + while (PinMap_TIM[i].pin!=NC) { + if (pinName == PinMap_TIM[i].pin) + return i; + i++; + } + return -1; +} + + +int _stm32_findIndexOfLastPinMapEntry(int pin) { + PinName pinName = digitalPinToPinName(pin); + int i = 0; + while (PinMap_TIM[i].pin!=NC) { + if ( pinName == (PinMap_TIM[i].pin & ~ALTX_MASK) + && pinName != (PinMap_TIM[i+1].pin & ~ALTX_MASK)) + return i; + i++; + } + return -1; +} + + + + + + +#define NOT_FOUND 1000 + +int _stm32_findBestTimerCombination(int numPins, int index, int pins[], PinMap* pinTimers[]) { + PinMap* searchArray[6] = { NULL, NULL, NULL, NULL, NULL, NULL }; + for (int j=0;j=0 && score= 0) { + #ifdef SIMPLEFOC_STM32_DEBUG + SimpleFOCDebug::print("STM32-DRV: best: "); + stm32_printTimerCombination(numPins, pinTimers, bestScore); + #endif + } + return bestScore; +}; + + + + + +#endif + diff --git a/src/drivers/hardware_specific/stm32/stm32_searchtimers.h b/src/drivers/hardware_specific/stm32/stm32_searchtimers.h new file mode 100644 index 00000000..6001b637 --- /dev/null +++ b/src/drivers/hardware_specific/stm32/stm32_searchtimers.h @@ -0,0 +1,11 @@ +#pragma once + +#include "./stm32_mcu.h" + +#if defined(_STM32_DEF_) || defined(TARGET_STM32H7) + + +int stm32_findBestTimerCombination(int numPins, int pins[], PinMap* pinTimers[]); + + +#endif diff --git a/src/drivers/hardware_specific/stm32/stm32_timerutils.cpp b/src/drivers/hardware_specific/stm32/stm32_timerutils.cpp new file mode 100644 index 00000000..b4a4fbf2 --- /dev/null +++ b/src/drivers/hardware_specific/stm32/stm32_timerutils.cpp @@ -0,0 +1,1046 @@ + +#include "./stm32_timerutils.h" +#include + +#if defined(_STM32_DEF_) || defined(TARGET_STM32H7) // if stm32duino or portenta + + +void stm32_pauseTimer(TIM_HandleTypeDef* handle){ + /* Disable timer unconditionally. Required to guarantee timer is stopped, + * even if some channels are still running */ + LL_TIM_DisableCounter(handle->Instance); +// handle->State = HAL_TIM_STATE_READY; + // on advanced control timers there is also the option of using the brake or the MOE? + // TIM1->EGR |= TIM_EGR_BG; // break generation +} + + +void stm32_resumeTimer(TIM_HandleTypeDef* handle){ + LL_TIM_EnableCounter(handle->Instance); +} + + +void stm32_refreshTimer(TIM_HandleTypeDef* handle){ + handle->Instance->EGR = TIM_EVENTSOURCE_UPDATE; +} + + +void stm32_pauseChannel(TIM_HandleTypeDef* handle, uint32_t llchannels){ + LL_TIM_CC_DisableChannel(handle->Instance, llchannels); +} + + +void stm32_resumeChannel(TIM_HandleTypeDef* handle, uint32_t llchannels){ + LL_TIM_CC_EnableChannel(handle->Instance, llchannels); +} + + + + + + + + +uint32_t stm32_setClockAndARR(TIM_HandleTypeDef* handle, uint32_t PWM_freq) { + // set the clock + uint32_t arr_value = 0; + uint32_t cycles = stm32_getTimerClockFreq(handle) / PWM_freq / 2; + uint32_t prescaler = (cycles / 0x10000) + 1; // TODO consider 32 bit timers + LL_TIM_SetPrescaler(handle->Instance, prescaler - 1); + uint32_t ticks = cycles / prescaler; + if (ticks > 0) { + arr_value = ticks - 1; + } + __HAL_TIM_SET_AUTORELOAD(handle, arr_value); + stm32_refreshTimer(handle); + // #ifdef SIMPLEFOC_STM32_DEBUG + // SIMPLEFOC_DEBUG("STM32-DRV: Timer clock: ", (int)stm32_getTimerClockFreq(handle)); + // SIMPLEFOC_DEBUG("STM32-DRV: Timer prescaler: ", (int)prescaler); + // SIMPLEFOC_DEBUG("STM32-DRV: Timer ARR: ", (int)arr_value); + // #endif + return arr_value; +} + + + + + +// setting pwm to hardware pin - instead analogWrite() +void stm32_setPwm(TIM_HandleTypeDef *timer, uint32_t channel, uint32_t value) { + // value assumed to be in correct range + switch (channel) { + case 1: timer->Instance->CCR1 = value; break; + case 2: timer->Instance->CCR2 = value; break; + case 3: timer->Instance->CCR3 = value; break; + case 4: timer->Instance->CCR4 = value; break; + #ifdef LL_TIM_CHANNEL_CH5 + case 5: timer->Instance->CCR5 = value; break; + #endif + #ifdef LL_TIM_CHANNEL_CH6 + case 6: timer->Instance->CCR6 = value; break; + #endif + default: break; + } +} + + +uint32_t stm32_getHALChannel(uint32_t channel) { + uint32_t return_value; + switch (channel) { + case 1: + return_value = TIM_CHANNEL_1; + break; + case 2: + return_value = TIM_CHANNEL_2; + break; + case 3: + return_value = TIM_CHANNEL_3; + break; + case 4: + return_value = TIM_CHANNEL_4; + break; + #ifdef TIM_CHANNEL_5 + case 5: + return_value = TIM_CHANNEL_5; + break; + #endif + #ifdef TIM_CHANNEL_6 + case 6: + return_value = TIM_CHANNEL_6; + break; + #endif + default: + return_value = -1; + } + return return_value; +} + + + +uint32_t stm32_getLLChannel(PinMap* timer) { +#if defined(TIM_CCER_CC1NE) + if (STM_PIN_INVERTED(timer->function)) { + switch (STM_PIN_CHANNEL(timer->function)) { + case 1: return LL_TIM_CHANNEL_CH1N; + case 2: return LL_TIM_CHANNEL_CH2N; + case 3: return LL_TIM_CHANNEL_CH3N; +#if defined(LL_TIM_CHANNEL_CH4N) + case 4: return LL_TIM_CHANNEL_CH4N; +#endif + default: return -1; + } + } else +#endif + { + switch (STM_PIN_CHANNEL(timer->function)) { + case 1: return LL_TIM_CHANNEL_CH1; + case 2: return LL_TIM_CHANNEL_CH2; + case 3: return LL_TIM_CHANNEL_CH3; + case 4: return LL_TIM_CHANNEL_CH4; + #ifdef LL_TIM_CHANNEL_CH5 + case 5: return LL_TIM_CHANNEL_CH5; + #endif + #ifdef LL_TIM_CHANNEL_CH6 + case 6: return LL_TIM_CHANNEL_CH6; + #endif + default: return -1; + } + } + return -1; +} + + + +uint8_t stm32_countTimers(TIM_HandleTypeDef *timers[], uint8_t num_timers) { + uint8_t count = 1; + for (int i=1; iInstance; + #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) || defined(STM32H7xx) || defined(TARGET_STM32H7) + +// 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 +// +// fammilies | timers +// --------------| -------------------------------- +// f1,f4,f7 | TIM1 to TIM4 and TIM8 +// l4 | TIM1 to TIM4, TIM8 and TIM15 +// h7 | TIM1 to TIM5, TIM8, TIM15, TIM23 and TIM24 +int stm32_getInternalSourceTrigger(TIM_HandleTypeDef* master, TIM_HandleTypeDef* slave) { + // put master and slave in temp variables to avoid arrows + TIM_TypeDef *TIM_master = master->Instance; + TIM_TypeDef *TIM_slave = slave->Instance; + #if defined(TIM1) && defined(LL_TIM_TS_ITR0) + if (TIM_master == TIM1){ + #if defined(TIM2) + if(TIM_slave == TIM2) return LL_TIM_TS_ITR0; + #endif + #if defined(TIM3) + else if(TIM_slave == TIM3) return LL_TIM_TS_ITR0; + #endif + #if defined(TIM4) + else if(TIM_slave == TIM4) return LL_TIM_TS_ITR0; + #endif + #if defined(TIM8) + else if(TIM_slave == TIM8) return LL_TIM_TS_ITR0; + #endif + #if defined(STM32H7xx) || defined(TARGET_STM32H7) + #if defined(TIM23) + else if(TIM_slave == TIM23) return LL_TIM_TS_ITR0; + #endif + #if defined(TIM24) + else if(TIM_slave == TIM24) return LL_TIM_TS_ITR0; + #endif + #endif + } + #endif + #if defined(TIM2) && defined(LL_TIM_TS_ITR1) + else if (TIM_master == TIM2){ + #if defined(TIM1) + if(TIM_slave == TIM1) return LL_TIM_TS_ITR1; + #endif + #if defined(TIM3) + else if(TIM_slave == TIM3) return LL_TIM_TS_ITR1; + #endif + #if defined(TIM4) + else if(TIM_slave == TIM4) return LL_TIM_TS_ITR1; + #endif + #if defined(TIM8) + else if(TIM_slave == TIM8) return LL_TIM_TS_ITR1; + #endif + #if defined(STM32H7xx) || defined(TARGET_STM32H7) + #if defined(TIM23) + else if(TIM_slave == TIM23) return LL_TIM_TS_ITR1; + #endif + #if defined(TIM24) + else if(TIM_slave == TIM24) return LL_TIM_TS_ITR1; + #endif + #else + #if defined(TIM5) + else if(TIM_slave == TIM5) return LL_TIM_TS_ITR0; + #endif + #endif + } + #endif + #if defined(TIM3) && defined(LL_TIM_TS_ITR2) + else if (TIM_master == TIM3){ + #if defined(TIM1) + if(TIM_slave == TIM1) return LL_TIM_TS_ITR2; + #endif + #if defined(TIM2) + else if(TIM_slave == TIM2) return LL_TIM_TS_ITR2; + #endif + #if defined(TIM4) + else if(TIM_slave == TIM4) return LL_TIM_TS_ITR2; + #endif + #if defined(STM32H7xx) || defined(TARGET_STM32H7) + #if defined(TIM5) + else if(TIM_slave == TIM5) return LL_TIM_TS_ITR2; + #endif + #if defined(TIM23) + else if(TIM_slave == TIM23) return LL_TIM_TS_ITR2; + #endif + #if defined(TIM24) + else if(TIM_slave == TIM24) return LL_TIM_TS_ITR2; + #endif + #else + #if defined(TIM5) + else if(TIM_slave == TIM5) return LL_TIM_TS_ITR1; + #endif + #endif + } + #endif + #if defined(TIM4) && defined(LL_TIM_TS_ITR3) + else if (TIM_master == TIM4){ + #if defined(TIM1) + if(TIM_slave == TIM1) return LL_TIM_TS_ITR3; + #endif + #if defined(TIM2) + else if(TIM_slave == TIM2) return LL_TIM_TS_ITR3; + #endif + #if defined(TIM3) + else if(TIM_slave == TIM3) return LL_TIM_TS_ITR3; + #endif + #if defined(TIM8) + else if(TIM_slave == TIM8) return LL_TIM_TS_ITR2; + #endif + + #if defined(STM32H7xx) || defined(TARGET_STM32H7) + #if defined(TIM5) + else if(TIM_slave == TIM5) return LL_TIM_TS_ITR3; + #endif + #if defined(TIM23) + else if(TIM_slave == TIM23) return LL_TIM_TS_ITR3; + #endif + #if defined(TIM24) + else if(TIM_slave == TIM24) return LL_TIM_TS_ITR3; + #endif + #else + #if defined(TIM5) + else if(TIM_slave == TIM5) return LL_TIM_TS_ITR2; + #endif + #endif + } + #endif + #if defined(TIM5) + else if (TIM_master == TIM5){ + #if defined(STM32F4xx) || defined(STM32F1xx) || defined(STM32F7xx) // f1, f4 adn f7 have tim5 sycned with tim1 and tim3 while others (l4, h7) have tim15 + #if defined(TIM1) + if(TIM_slave == TIM1) return LL_TIM_TS_ITR0; + #endif + #if defined(TIM3) + else if(TIM_slave == TIM3) return LL_TIM_TS_ITR2; + #endif + #endif + #if defined(TIM8) + if(TIM_slave == TIM8) return LL_TIM_TS_ITR3; + #endif + + #if defined(STM32H7xx) || defined(TARGET_STM32H7) + #if defined(TIM23) + else if(TIM_slave == TIM23) return LL_TIM_TS_ITR4; + #endif + #if defined(TIM24) + else if(TIM_slave == TIM24) return LL_TIM_TS_ITR4; + #endif + #endif + } + #endif + #if defined(TIM8) + else if (TIM_master == TIM8){ + #if defined(TIM2) + if(TIM_slave==TIM2) return LL_TIM_TS_ITR1; + #endif + #if defined(TIM4) + else if(TIM_slave == TIM4) return LL_TIM_TS_ITR3; + #endif + + #if defined(STM32H7xx) || defined(TARGET_STM32H7) + #if defined(TIM5) + else if(TIM_slave == TIM5) return LL_TIM_TS_ITR1; + #endif + #if defined(TIM23) + else if(TIM_slave == TIM23) return LL_TIM_TS_ITR5; + #endif + #if defined(TIM24) + else if(TIM_slave == TIM24) return LL_TIM_TS_ITR5; + #endif + #else + #if defined(TIM5) + else if(TIM_slave == TIM5) return LL_TIM_TS_ITR3; + #endif + #endif + } + #endif + #if defined(TIM15) && (defined(STM32L4xx) || defined(STM32H7xx) || defined(TARGET_STM32H7) ) + else if (TIM_master == TIM15){ + #if defined(TIM1) + if(TIM_slave == TIM1) return LL_TIM_TS_ITR0; + #endif + #if defined(TIM3) + if(TIM_slave == TIM3) return LL_TIM_TS_ITR2; + #endif + } + #endif + #if defined(TIM23) && (defined(STM32H7xx) || defined(TARGET_STM32H7)) + else if (TIM_master == TIM23){ + #if defined(TIM1) + if(TIM_slave == TIM1) return LL_TIM_TS_ITR12; + #endif + #if defined(TIM2) + if(TIM_slave == TIM2) return LL_TIM_TS_ITR12; + #endif + #if defined(TIM3) + if(TIM_slave == TIM3) return LL_TIM_TS_ITR12; + #endif + #if defined(TIM4) + if(TIM_slave == TIM4) return LL_TIM_TS_ITR12; + #endif + #if defined(TIM5) + if(TIM_slave == TIM5) return LL_TIM_TS_ITR12; + #endif + #if defined(TIM8) + if(TIM_slave == TIM8) return LL_TIM_TS_ITR12; + #endif + #if defined(TIM24) + if(TIM_slave == TIM24) return LL_TIM_TS_ITR12; + #endif + } + #endif + #if defined(TIM24) && (defined(STM32H7xx) || defined(TARGET_STM32H7)) + else if (TIM_master == TIM24){ + #if defined(TIM1) + if(TIM_slave == TIM1) return LL_TIM_TS_ITR13; + #endif + #if defined(TIM2) + if(TIM_slave == TIM2) return LL_TIM_TS_ITR13; + #endif + #if defined(TIM3) + if(TIM_slave == TIM3) return LL_TIM_TS_ITR13; + #endif + #if defined(TIM4) + if(TIM_slave == TIM4) return LL_TIM_TS_ITR13; + #endif + #if defined(TIM5) + if(TIM_slave == TIM5) return LL_TIM_TS_ITR13; + #endif + #if defined(TIM8) + if(TIM_slave == TIM8) return LL_TIM_TS_ITR13; + #endif + #if defined(TIM23) + if(TIM_slave == TIM23) return LL_TIM_TS_ITR13; + #endif + } + #endif + return -1; // combination not supported +} +#else +// Alignment not supported for this architecture +int stm32_getInternalSourceTrigger(TIM_HandleTypeDef* master, TIM_HandleTypeDef* slave) { + return -1; +} +#endif + + + + + +// call with the timers used for a motor. The function will align the timers and +// start them at the same time (if possible). Results of this function can be: +// - success, no changes needed +// - success, different timers aligned and started concurrently +// - failure, cannot align timers +// in each case, the timers are started. +// +// TODO: this topic is quite complex if we allow multiple motors and multiple timers per motor. +// that's because the motors could potentially share the same timer. In this case aligning +// the timers is problematic. +// Even more problematic is stopping and resetting the timers. Even with a single motor, +// this would cause problems and mis-align the PWM signals. +// We can handle three cases: +// - only one timer needed, no need to align +// - master timer can be found, and timers used by only this motor: alignment possible +// - TODO all timers for all motors can be aligned to one master: alignment possible +// - otherwise, alignment not possible +// frequency alignment is based on checking their pwm frequencies match. +// pwm alignment is based on linking timers to start them at the same time, and making sure they are reset in sync. +// On newer chips supporting it (STM32G4) we use gated + reset mode to start/stop only the master timer. Slaves +// are started/stopped automatically with the master. TODO probably just using gated mode is better +// On older chips (STM32F1) we used gated mode to start/stop the slave timers with the master, but have to take +// care of the reset manually. TODO is it really needed to reset the timers? +TIM_HandleTypeDef* stm32_alignTimers(TIM_HandleTypeDef *timers_in[], uint8_t num_timers_in) { + // find the timers used + TIM_HandleTypeDef *timers[6]; + int numTimers = stm32_distinctTimers(timers_in, num_timers_in, timers); + + // compare with the existing timers used for other motors... + for (int i=0; itimers_handle[k] == NULL) break; + if (params->timers_handle[k] == timers[i]) { + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-DRV: ERR: Timer already used by another motor: TIM", stm32_getTimerNumber(timers[i]->Instance)); + #endif + // TODO handle this case, and make it a warning + // two sub-cases we could handle at the moment: + // - the other motor does not have a master timer, so we can initialize this motor without a master also + } + } + } + } + + + #ifdef SIMPLEFOC_STM32_DEBUG + SimpleFOCDebug::print("STM32-DRV: Synchronising "); + SimpleFOCDebug::print(numTimers); + SimpleFOCDebug::println(" timers"); + #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; iInstance)) { + // 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]->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_iInstance)); + #endif + // make the master timer generate ITRGx event + // if it was already configured in slave mode, disable the slave mode on the master + LL_TIM_SetSlaveMode(timers[master_index]->Instance, LL_TIM_SLAVEMODE_DISABLED ); + // Configure the master timer to send a trigger signal on enable + LL_TIM_SetTriggerOutput(timers[master_index]->Instance, LL_TIM_TRGO_ENABLE); + //LL_TIM_EnableMasterSlaveMode(timers[master_index]->Instance); + + // 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; + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-DRV: slave timer: TIM", stm32_getTimerNumber(timers[slave_index]->Instance)); + #endif + // Configure the slave timer to be triggered by the master enable signal + uint32_t trigger = stm32_getInternalSourceTrigger(timers[master_index], timers[slave_index]); + // #ifdef SIMPLEFOC_STM32_DEBUG + // SIMPLEFOC_DEBUG("STM32-DRV: slave trigger ITR ", (int)trigger); + // #endif + LL_TIM_SetTriggerInput(timers[slave_index]->Instance, trigger); + // #if defined(STM32G4xx) + // LL_TIM_SetSlaveMode(timers[slave_index]->Instance, LL_TIM_SLAVEMODE_COMBINED_GATEDRESET); + // #else + LL_TIM_SetSlaveMode(timers[slave_index]->Instance, LL_TIM_SLAVEMODE_GATED); + // #endif + } + for (int i=0; iInstance->CNT); + } + stm32_resumeTimer(timers[master_index]); + return timers[master_index]; + } + } + + // if we had only one timer, or there was no master timer found + for (int i=0; iInstance); + uint64_t clkSelection = timerClkSrc == 1 ? RCC_PERIPHCLK_TIMG1 : RCC_PERIPHCLK_TIMG2; + return HAL_RCCEx_GetPeriphCLKFreq(clkSelection); +#else + RCC_ClkInitTypeDef clkconfig = {}; + uint32_t pFLatency = 0U; + uint32_t uwTimclock = 0U, uwAPBxPrescaler = 0U; + + /* Get clock configuration */ + HAL_RCC_GetClockConfig(&clkconfig, &pFLatency); + switch (getTimerClkSrc(handle->Instance)) { + case 1: + uwAPBxPrescaler = clkconfig.APB1CLKDivider; + uwTimclock = HAL_RCC_GetPCLK1Freq(); + break; +#if !defined(STM32C0xx) && !defined(STM32F0xx) && !defined(STM32G0xx) + case 2: + uwAPBxPrescaler = clkconfig.APB2CLKDivider; + uwTimclock = HAL_RCC_GetPCLK2Freq(); + break; +#endif + default: + case 0: // Unknown timer clock source + return 0; + } + +#if defined(STM32H7xx) || defined(TARGET_STM32H7) + /* When TIMPRE bit of the RCC_CFGR register is reset, + * if APBx prescaler is 1 or 2 then TIMxCLK = HCLK, + * otherwise TIMxCLK = 2x PCLKx. + * When TIMPRE bit in the RCC_CFGR register is set, + * if APBx prescaler is 1,2 or 4, then TIMxCLK = HCLK, + * otherwise TIMxCLK = 4x PCLKx + */ + RCC_PeriphCLKInitTypeDef PeriphClkConfig = {}; + HAL_RCCEx_GetPeriphCLKConfig(&PeriphClkConfig); + + if (PeriphClkConfig.TIMPresSelection == RCC_TIMPRES_ACTIVATED) { + switch (uwAPBxPrescaler) { + default: + case RCC_APB1_DIV1: + case RCC_APB1_DIV2: + case RCC_APB1_DIV4: + /* case RCC_APB2_DIV1: */ + case RCC_APB2_DIV2: + case RCC_APB2_DIV4: + /* Note: in such cases, HCLK = (APBCLK * DIVx) */ + uwTimclock = HAL_RCC_GetHCLKFreq(); + break; + case RCC_APB1_DIV8: + case RCC_APB1_DIV16: + case RCC_APB2_DIV8: + case RCC_APB2_DIV16: + uwTimclock *= 4; + break; + } + } else { + switch (uwAPBxPrescaler) { + default: + case RCC_APB1_DIV1: + case RCC_APB1_DIV2: + /* case RCC_APB2_DIV1: */ + case RCC_APB2_DIV2: + /* Note: in such cases, HCLK = (APBCLK * DIVx) */ + uwTimclock = HAL_RCC_GetHCLKFreq(); + break; + case RCC_APB1_DIV4: + case RCC_APB1_DIV8: + case RCC_APB1_DIV16: + case RCC_APB2_DIV4: + case RCC_APB2_DIV8: + case RCC_APB2_DIV16: + uwTimclock *= 2; + break; + } + } +#else + /* When TIMPRE bit of the RCC_DCKCFGR register is reset, + * if APBx prescaler is 1, then TIMxCLK = PCLKx, + * otherwise TIMxCLK = 2x PCLKx. + * When TIMPRE bit in the RCC_DCKCFGR register is set, + * if APBx prescaler is 1,2 or 4, then TIMxCLK = HCLK, + * otherwise TIMxCLK = 4x PCLKx + */ +#if defined(STM32F4xx) || defined(STM32F7xx) +#if !defined(STM32F405xx) && !defined(STM32F415xx) &&\ + !defined(STM32F407xx) && !defined(STM32F417xx) + RCC_PeriphCLKInitTypeDef PeriphClkConfig = {}; + HAL_RCCEx_GetPeriphCLKConfig(&PeriphClkConfig); + + if (PeriphClkConfig.TIMPresSelection == RCC_TIMPRES_ACTIVATED) + switch (uwAPBxPrescaler) { + default: + case RCC_HCLK_DIV1: + case RCC_HCLK_DIV2: + case RCC_HCLK_DIV4: + /* Note: in such cases, HCLK = (APBCLK * DIVx) */ + uwTimclock = HAL_RCC_GetHCLKFreq(); + break; + case RCC_HCLK_DIV8: + case RCC_HCLK_DIV16: + uwTimclock *= 4; + break; + } else +#endif +#endif + switch (uwAPBxPrescaler) { + default: + case RCC_HCLK_DIV1: + // uwTimclock*=1; + break; + case RCC_HCLK_DIV2: + case RCC_HCLK_DIV4: + case RCC_HCLK_DIV8: + case RCC_HCLK_DIV16: + uwTimclock *= 2; + break; + } +#endif /* STM32H7xx */ + return uwTimclock; +#endif /* STM32MP1xx */ +} + + + + + + + +#if defined(__MBED__) + +void enableTimerClock(TIM_HandleTypeDef *htim) { + // Enable TIM clock +#if defined(TIM1_BASE) + if (htim->Instance == TIM1) { + __HAL_RCC_TIM1_CLK_ENABLE(); + } +#endif +#if defined(TIM2_BASE) + if (htim->Instance == TIM2) { + __HAL_RCC_TIM2_CLK_ENABLE(); + } +#endif +#if defined(TIM3_BASE) + if (htim->Instance == TIM3) { + __HAL_RCC_TIM3_CLK_ENABLE(); + } +#endif +#if defined(TIM4_BASE) + if (htim->Instance == TIM4) { + __HAL_RCC_TIM4_CLK_ENABLE(); + } +#endif +#if defined(TIM5_BASE) + if (htim->Instance == TIM5) { + __HAL_RCC_TIM5_CLK_ENABLE(); + } +#endif +#if defined(TIM6_BASE) + if (htim->Instance == TIM6) { + __HAL_RCC_TIM6_CLK_ENABLE(); + } +#endif +#if defined(TIM7_BASE) + if (htim->Instance == TIM7) { + __HAL_RCC_TIM7_CLK_ENABLE(); + } +#endif +#if defined(TIM8_BASE) + if (htim->Instance == TIM8) { + __HAL_RCC_TIM8_CLK_ENABLE(); + } +#endif +#if defined(TIM9_BASE) + if (htim->Instance == TIM9) { + __HAL_RCC_TIM9_CLK_ENABLE(); + } +#endif +#if defined(TIM10_BASE) + if (htim->Instance == TIM10) { + __HAL_RCC_TIM10_CLK_ENABLE(); + } +#endif +#if defined(TIM11_BASE) + if (htim->Instance == TIM11) { + __HAL_RCC_TIM11_CLK_ENABLE(); + } +#endif +#if defined(TIM12_BASE) + if (htim->Instance == TIM12) { + __HAL_RCC_TIM12_CLK_ENABLE(); + } +#endif +#if defined(TIM13_BASE) + if (htim->Instance == TIM13) { + __HAL_RCC_TIM13_CLK_ENABLE(); + } +#endif +#if defined(TIM14_BASE) + if (htim->Instance == TIM14) { + __HAL_RCC_TIM14_CLK_ENABLE(); + } +#endif +#if defined(TIM15_BASE) + if (htim->Instance == TIM15) { + __HAL_RCC_TIM15_CLK_ENABLE(); + } +#endif +#if defined(TIM16_BASE) + if (htim->Instance == TIM16) { + __HAL_RCC_TIM16_CLK_ENABLE(); + } +#endif +#if defined(TIM17_BASE) + if (htim->Instance == TIM17) { + __HAL_RCC_TIM17_CLK_ENABLE(); + } +#endif +#if defined(TIM18_BASE) + if (htim->Instance == TIM18) { + __HAL_RCC_TIM18_CLK_ENABLE(); + } +#endif +#if defined(TIM19_BASE) + if (htim->Instance == TIM19) { + __HAL_RCC_TIM19_CLK_ENABLE(); + } +#endif +#if defined(TIM20_BASE) + if (htim->Instance == TIM20) { + __HAL_RCC_TIM20_CLK_ENABLE(); + } +#endif +#if defined(TIM21_BASE) + if (htim->Instance == TIM21) { + __HAL_RCC_TIM21_CLK_ENABLE(); + } +#endif +#if defined(TIM22_BASE) + if (htim->Instance == TIM22) { + __HAL_RCC_TIM22_CLK_ENABLE(); + } +#endif +} + + +uint8_t getTimerClkSrc(TIM_TypeDef *tim) { + uint8_t clkSrc = 0; + + if (tim != (TIM_TypeDef *)NC) +#if defined(STM32C0xx) || defined(STM32F0xx) || defined(STM32G0xx) + /* TIMx source CLK is PCKL1 */ + clkSrc = 1; +#else + { + if ( + #if defined(TIM2_BASE) + tim == TIM2 || + #endif + #if defined(TIM3_BASE) + tim == TIM3 || + #endif + #if defined(TIM4_BASE) + tim == TIM4 || + #endif + #if defined(TIM5_BASE) + tim == TIM5 || + #endif + #if defined(TIM6_BASE) + tim == TIM6 || + #endif + #if defined(TIM7_BASE) + tim == TIM7 || + #endif + #if defined(TIM12_BASE) + tim == TIM12 || + #endif + #if defined(TIM13_BASE) + tim == TIM13 || + #endif + #if defined(TIM14_BASE) + tim == TIM14 || + #endif + #if defined(TIM18_BASE) + tim == TIM18 || + #endif + false) + clkSrc = 1; + else + if ( + #if defined(TIM1_BASE) + tim == TIM1 || + #endif + #if defined(TIM8_BASE) + tim == TIM8 || + #endif + #if defined(TIM9_BASE) + tim == TIM9 || + #endif + #if defined(TIM10_BASE) + tim == TIM10 || + #endif + #if defined(TIM11_BASE) + tim == TIM11 || + #endif + #if defined(TIM15_BASE) + tim == TIM15 || + #endif + #if defined(TIM16_BASE) + tim == TIM16 || + #endif + #if defined(TIM17_BASE) + tim == TIM17 || + #endif + #if defined(TIM19_BASE) + tim == TIM19 || + #endif + #if defined(TIM20_BASE) + tim == TIM20 || + #endif + #if defined(TIM21_BASE) + tim == TIM21 || + #endif + #if defined(TIM22_BASE) + tim == TIM22 || + #endif + false ) + clkSrc = 2; + else + return 0; + } +#endif + return clkSrc; +} + +#endif + + + + + + + + +#ifdef SIMPLEFOC_STM32_DEBUG + +/* + some utility functions to print out the timer combinations +*/ + +int stm32_getTimerNumber(TIM_TypeDef *instance) { + #if defined(TIM1_BASE) + if (instance==TIM1) return 1; + #endif + #if defined(TIM2_BASE) + if (instance==TIM2) return 2; + #endif + #if defined(TIM3_BASE) + if (instance==TIM3) return 3; + #endif + #if defined(TIM4_BASE) + if (instance==TIM4) return 4; + #endif + #if defined(TIM5_BASE) + if (instance==TIM5) return 5; + #endif + #if defined(TIM6_BASE) + if (instance==TIM6) return 6; + #endif + #if defined(TIM7_BASE) + if (instance==TIM7) return 7; + #endif + #if defined(TIM8_BASE) + if (instance==TIM8) return 8; + #endif + #if defined(TIM9_BASE) + if (instance==TIM9) return 9; + #endif + #if defined(TIM10_BASE) + if (instance==TIM10) return 10; + #endif + #if defined(TIM11_BASE) + if (instance==TIM11) return 11; + #endif + #if defined(TIM12_BASE) + if (instance==TIM12) return 12; + #endif + #if defined(TIM13_BASE) + if (instance==TIM13) return 13; + #endif + #if defined(TIM14_BASE) + if (instance==TIM14) return 14; + #endif + #if defined(TIM15_BASE) + if (instance==TIM15) return 15; + #endif + #if defined(TIM16_BASE) + if (instance==TIM16) return 16; + #endif + #if defined(TIM17_BASE) + if (instance==TIM17) return 17; + #endif + #if defined(TIM18_BASE) + if (instance==TIM18) return 18; + #endif + #if defined(TIM19_BASE) + if (instance==TIM19) return 19; + #endif + #if defined(TIM20_BASE) + if (instance==TIM20) return 20; + #endif + #if defined(TIM21_BASE) + if (instance==TIM21) return 21; + #endif + #if defined(TIM22_BASE) + if (instance==TIM22) return 22; + #endif + return -1; +} + + +void stm32_printTimerCombination(int numPins, PinMap* timers[], int score) { + for (int i=0; iperipheral)); + SimpleFOCDebug::print("-CH"); + SimpleFOCDebug::print(STM_PIN_CHANNEL(timers[i]->function)); + if (STM_PIN_INVERTED(timers[i]->function)) + SimpleFOCDebug::print("N"); + } + SimpleFOCDebug::print(" "); + } + SimpleFOCDebug::println("score: ", score); +} + +#endif + + +#endif diff --git a/src/drivers/hardware_specific/stm32/stm32_timerutils.h b/src/drivers/hardware_specific/stm32/stm32_timerutils.h new file mode 100644 index 00000000..3ba1c558 --- /dev/null +++ b/src/drivers/hardware_specific/stm32/stm32_timerutils.h @@ -0,0 +1,33 @@ + +#pragma once + +#include "./stm32_mcu.h" + +#if defined(_STM32_DEF_) || defined(TARGET_STM32H7) + +void stm32_pauseTimer(TIM_HandleTypeDef* handle); +void stm32_resumeTimer(TIM_HandleTypeDef* handle); +void stm32_refreshTimer(TIM_HandleTypeDef* handle); +void stm32_pauseChannel(TIM_HandleTypeDef* handle, uint32_t llchannels); +void stm32_resumeChannel(TIM_HandleTypeDef* handle, uint32_t llchannels); +uint32_t stm32_setClockAndARR(TIM_HandleTypeDef* handle, uint32_t PWM_freq); +uint8_t stm32_countTimers(TIM_HandleTypeDef *timers[], uint8_t num_timers); +uint8_t stm32_distinctTimers(TIM_HandleTypeDef* timers_in[], uint8_t num_timers, TIM_HandleTypeDef* timers_out[]); +uint32_t stm32_getHALChannel(uint32_t channel); +uint32_t stm32_getLLChannel(PinMap* timer); +int stm32_getInternalSourceTrigger(TIM_HandleTypeDef* master, TIM_HandleTypeDef* slave); +TIM_HandleTypeDef* stm32_alignTimers(TIM_HandleTypeDef *timers_in[], uint8_t num_timers_in); +void stm32_setPwm(TIM_HandleTypeDef *timer, uint32_t channel, uint32_t value); +uint32_t stm32_getTimerClockFreq(TIM_HandleTypeDef* handle); + +#if defined(__MBED__) +void enableTimerClock(TIM_HandleTypeDef *htim); +uint8_t getTimerClkSrc(TIM_TypeDef *tim); +#endif + +#if defined(SIMPLEFOC_STM32_DEBUG) +void stm32_printTimerCombination(int numPins, PinMap* timers[], int score); +int stm32_getTimerNumber(TIM_TypeDef *instance); +#endif + +#endif 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/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/Encoder.h b/src/sensors/Encoder.h index af6a5ab6..ab494d13 100644 --- a/src/sensors/Encoder.h +++ b/src/sensors/Encoder.h @@ -68,7 +68,7 @@ class Encoder: public Sensor{ /** * returns 0 if it does need search for absolute zero * 0 - encoder without index - * 1 - ecoder with index + * 1 - encoder with index */ int needsSearch() override; diff --git a/src/sensors/HallSensor.cpp b/src/sensors/HallSensor.cpp index d9e5ec83..64c5e48c 100644 --- a/src/sensors/HallSensor.cpp +++ b/src/sensors/HallSensor.cpp @@ -42,22 +42,21 @@ void HallSensor::handleC() { * Updates the state and sector following an interrupt */ void HallSensor::updateState() { - long new_pulse_timestamp = _micros(); - int8_t new_hall_state = C_active + (B_active << 1) + (A_active << 2); // glitch avoidance #1 - sometimes we get an interrupt but pins haven't changed - if (new_hall_state == hall_state) { - return; - } + if (new_hall_state == hall_state) return; + + long new_pulse_timestamp = _micros(); hall_state = new_hall_state; int8_t new_electric_sector = ELECTRIC_SECTORS[hall_state]; - if (new_electric_sector - electric_sector > 3) { + int8_t electric_sector_dif = new_electric_sector - electric_sector; + if (electric_sector_dif > 3) { //underflow direction = Direction::CCW; electric_rotations += direction; - } else if (new_electric_sector - electric_sector < (-3)) { + } else if (electric_sector_dif < (-3)) { //overflow direction = Direction::CW; electric_rotations += direction; @@ -96,11 +95,19 @@ void HallSensor::attachSectorCallback(void (*_onSectorChange)(int sector)) { // Sensor update function. Safely copy volatile interrupt variables into Sensor base class state variables. void HallSensor::update() { // Copy volatile variables in minimal-duration blocking section to ensure no interrupts are missed - noInterrupts(); + if (use_interrupt){ + noInterrupts(); + }else{ + A_active = digitalRead(pinA); + B_active = digitalRead(pinB); + C_active = digitalRead(pinC); + updateState(); + } + angle_prev_ts = pulse_timestamp; long last_electric_rotations = electric_rotations; int8_t last_electric_sector = electric_sector; - interrupts(); + if (use_interrupt) interrupts(); angle_prev = ((float)((last_electric_rotations * 6 + last_electric_sector) % cpr) / (float)cpr) * _2PI ; full_rotations = (int32_t)((last_electric_rotations * 6 + last_electric_sector) / cpr); } @@ -150,7 +157,7 @@ void HallSensor::init(){ } // init hall_state - A_active= digitalRead(pinA); + A_active = digitalRead(pinA); B_active = digitalRead(pinB); C_active = digitalRead(pinC); updateState(); @@ -169,4 +176,6 @@ void HallSensor::enableInterrupts(void (*doA)(), void(*doB)(), void(*doC)()){ if(doA != nullptr) attachInterrupt(digitalPinToInterrupt(pinA), doA, CHANGE); if(doB != nullptr) attachInterrupt(digitalPinToInterrupt(pinB), doB, CHANGE); if(doC != nullptr) attachInterrupt(digitalPinToInterrupt(pinC), doC, CHANGE); + + use_interrupt = true; } diff --git a/src/sensors/HallSensor.h b/src/sensors/HallSensor.h index 78e1b416..94053e0f 100644 --- a/src/sensors/HallSensor.h +++ b/src/sensors/HallSensor.h @@ -47,6 +47,7 @@ class HallSensor: public Sensor{ int pinA; //!< HallSensor hardware pin A int pinB; //!< HallSensor hardware pin B int pinC; //!< HallSensor hardware pin C + int use_interrupt; //!< True if interrupts have been attached // HallSensor configuration Pullup pullup; //!< Configuration parameter internal or external pullups diff --git a/src/sensors/MagneticSensorI2C.cpp b/src/sensors/MagneticSensorI2C.cpp index ac2b273b..9298413a 100644 --- a/src/sensors/MagneticSensorI2C.cpp +++ b/src/sensors/MagneticSensorI2C.cpp @@ -5,7 +5,10 @@ MagneticSensorI2CConfig_s AS5600_I2C = { .chip_address = 0x36, .bit_resolution = 12, .angle_register = 0x0C, - .data_start_bit = 11 + .msb_mask = 0x0F, + .msb_shift = 8, + .lsb_mask = 0xFF, + .lsb_shift = 0 }; /** Typical configuration for the 12bit AMS AS5048 magnetic sensor over I2C interface */ @@ -13,7 +16,21 @@ MagneticSensorI2CConfig_s AS5048_I2C = { .chip_address = 0x40, // highly configurable. if A1 and A2 are held low, this is probable value .bit_resolution = 14, .angle_register = 0xFE, - .data_start_bit = 15 + .msb_mask = 0xFF, + .msb_shift = 6, + .lsb_mask = 0x3F, + .lsb_shift = 0 +}; + +/** Typical configuration for the 12bit MT6701 magnetic sensor over I2C interface */ +MagneticSensorI2CConfig_s MT6701_I2C = { + .chip_address = 0x06, + .bit_resolution = 14, + .angle_register = 0x03, + .msb_mask = 0xFF, + .msb_shift = 6, + .lsb_mask = 0xFC, + .lsb_shift = 2 }; @@ -22,52 +39,49 @@ MagneticSensorI2CConfig_s AS5048_I2C = { // @param _bit_resolution bit resolution of the sensor // @param _angle_register_msb angle read register // @param _bits_used_msb number of used bits in msb -MagneticSensorI2C::MagneticSensorI2C(uint8_t _chip_address, int _bit_resolution, uint8_t _angle_register_msb, int _bits_used_msb){ - // chip I2C address - chip_address = _chip_address; - // angle read register of the magnetic sensor - angle_register_msb = _angle_register_msb; - // register maximum value (counts per revolution) +MagneticSensorI2C::MagneticSensorI2C(uint8_t _chip_address, int _bit_resolution, uint8_t _angle_register_msb, int _bits_used_msb, bool lsb_right_aligned){ + _conf.chip_address = _chip_address; + _conf.bit_resolution = _bit_resolution; + _conf.angle_register = _angle_register_msb; + _conf.msb_mask = (uint8_t)( (1 << _bits_used_msb) - 1 ); + + uint8_t lsb_used = _bit_resolution - _bits_used_msb; // used bits in LSB + _conf.lsb_mask = (uint8_t)( (1 << (lsb_used)) - 1 ); + if (!lsb_right_aligned) + _conf.lsb_shift = 8-lsb_used; + else + _conf.lsb_shift = 0; + _conf.msb_shift = lsb_used; + cpr = _powtwo(_bit_resolution); - // depending on the sensor architecture there are different combinations of - // LSB and MSB register used bits - // AS5600 uses 0..7 LSB and 8..11 MSB - // AS5048 uses 0..5 LSB and 6..13 MSB - // used bits in LSB - lsb_used = _bit_resolution - _bits_used_msb; - // extraction masks - lsb_mask = (uint8_t)( (2 << lsb_used) - 1 ); - msb_mask = (uint8_t)( (2 << _bits_used_msb) - 1 ); wire = &Wire; } -MagneticSensorI2C::MagneticSensorI2C(MagneticSensorI2CConfig_s config){ - chip_address = config.chip_address; - // angle read register of the magnetic sensor - angle_register_msb = config.angle_register; - // register maximum value (counts per revolution) - cpr = _powtwo(config.bit_resolution); - int bits_used_msb = config.data_start_bit - 7; - lsb_used = config.bit_resolution - bits_used_msb; - // extraction masks - lsb_mask = (uint8_t)( (2 << lsb_used) - 1 ); - msb_mask = (uint8_t)( (2 << bits_used_msb) - 1 ); +MagneticSensorI2C::MagneticSensorI2C(MagneticSensorI2CConfig_s config){ + _conf = config; + cpr = _powtwo(config.bit_resolution); wire = &Wire; } -void MagneticSensorI2C::init(TwoWire* _wire){ - wire = _wire; - // I2C communication begin - wire->begin(); +MagneticSensorI2C MagneticSensorI2C::AS5600() { + return {AS5600_I2C}; +} + + +void MagneticSensorI2C::init(TwoWire* _wire){ + wire = _wire; + wire->begin(); // I2C communication begin this->Sensor::init(); // call base class init } + + // Shaft angle calculation // angle is in radians [rad] float MagneticSensorI2C::getSensorAngle(){ @@ -77,38 +91,25 @@ float MagneticSensorI2C::getSensorAngle(){ -// function reading the raw counter of the magnetic sensor -int MagneticSensorI2C::getRawCount(){ - return (int)MagneticSensorI2C::read(angle_register_msb); -} - // I2C functions /* -* Read a register from the sensor -* Takes the address of the register as a uint8_t -* Returns the value of the register +* Read an angle from the angle register of the sensor */ -int MagneticSensorI2C::read(uint8_t angle_reg_msb) { +int MagneticSensorI2C::getRawCount() { // read the angle register first MSB then LSB byte readArray[2]; uint16_t readValue = 0; // notify the device that is aboout to be read - wire->beginTransmission(chip_address); - wire->write(angle_reg_msb); + wire->beginTransmission(_conf.chip_address); + wire->write(_conf.angle_register); currWireError = wire->endTransmission(false); - // read the data msb and lsb - wire->requestFrom(chip_address, (uint8_t)2); + wire->requestFrom(_conf.chip_address, (uint8_t)2); for (byte i=0; i < 2; i++) { readArray[i] = wire->read(); } - - // depending on the sensor architecture there are different combinations of - // LSB and MSB register used bits - // AS5600 uses 0..7 LSB and 8..11 MSB - // AS5048 uses 0..5 LSB and 6..13 MSB - readValue = ( readArray[1] & lsb_mask ); - readValue += ( ( readArray[0] & msb_mask ) << lsb_used ); + readValue = (readArray[0] & _conf.msb_mask) << _conf.msb_shift; + readValue |= (readArray[1] & _conf.lsb_mask) >> _conf.lsb_shift; return readValue; } diff --git a/src/sensors/MagneticSensorI2C.h b/src/sensors/MagneticSensorI2C.h index f8b189fa..381a950a 100644 --- a/src/sensors/MagneticSensorI2C.h +++ b/src/sensors/MagneticSensorI2C.h @@ -8,13 +8,17 @@ #include "../common/time_utils.h" struct MagneticSensorI2CConfig_s { - int chip_address; - int bit_resolution; - int angle_register; - int data_start_bit; + uint8_t chip_address; + uint8_t bit_resolution; + uint8_t angle_register; + uint8_t msb_mask; + uint8_t msb_shift; + uint8_t lsb_mask; + uint8_t lsb_shift; }; + // some predefined structures -extern MagneticSensorI2CConfig_s AS5600_I2C,AS5048_I2C; +extern MagneticSensorI2CConfig_s AS5600_I2C, AS5048_I2C, MT6701_I2C; #if defined(TARGET_RP2040) #define SDA I2C_SDA @@ -31,7 +35,7 @@ class MagneticSensorI2C: public Sensor{ * @param angle_register_msb angle read register msb * @param _bits_used_msb number of used bits in msb */ - MagneticSensorI2C(uint8_t _chip_address, int _bit_resolution, uint8_t _angle_register_msb, int _msb_bits_used); + MagneticSensorI2C(uint8_t _chip_address, int _bit_resolution, uint8_t _angle_register_msb, int _msb_bits_used, bool lsb_right_aligned = true); /** * MagneticSensorI2C class constructor @@ -56,13 +60,7 @@ class MagneticSensorI2C: public Sensor{ private: float cpr; //!< Maximum range of the magnetic sensor - uint16_t lsb_used; //!< Number of bits used in LSB register - uint8_t lsb_mask; - uint8_t msb_mask; - - // I2C variables - uint8_t angle_register_msb; //!< I2C angle register to read - uint8_t chip_address; //!< I2C chip select pins + MagneticSensorI2CConfig_s _conf; // I2C functions /** Read one I2C register value */ @@ -76,8 +74,6 @@ class MagneticSensorI2C: public Sensor{ /* the two wire instance for this sensor */ TwoWire* wire; - - }; diff --git a/src/sensors/MagneticSensorSPI.cpp b/src/sensors/MagneticSensorSPI.cpp index 52febc38..3bcbecf4 100644 --- a/src/sensors/MagneticSensorSPI.cpp +++ b/src/sensors/MagneticSensorSPI.cpp @@ -11,9 +11,28 @@ MagneticSensorSPIConfig_s AS5147_SPI = { .command_rw_bit = 14, .command_parity_bit = 15 }; -// AS5048 and AS5047 are the same as AS5147 -MagneticSensorSPIConfig_s AS5048_SPI = AS5147_SPI; -MagneticSensorSPIConfig_s AS5047_SPI = AS5147_SPI; + +// AS5048 and AS5047 share the same configuration as AS5147 +// we have to explicilty assign them anyway due to compiler issues +// Ex. https://community.simplefoc.com/t/esp32s3-qtpy-platformio-spi-problem/7444 +MagneticSensorSPIConfig_s AS5048_SPI = { + .spi_mode = SPI_MODE1, + .clock_speed = 1000000, + .bit_resolution = 14, + .angle_register = 0x3FFF, + .data_start_bit = 13, + .command_rw_bit = 14, + .command_parity_bit = 15 +}; +MagneticSensorSPIConfig_s AS5047_SPI = { + .spi_mode = SPI_MODE1, + .clock_speed = 1000000, + .bit_resolution = 14, + .angle_register = 0x3FFF, + .data_start_bit = 13, + .command_rw_bit = 14, + .command_parity_bit = 15 +}; /** Typical configuration for the 14bit MonolithicPower MA730 magnetic sensor over SPI interface */ MagneticSensorSPIConfig_s MA730_SPI = {