diff --git a/.github/workflows/arduino.yml b/.github/workflows/arduino.yml index cadd8ccc..9a4bc6c4 100644 --- a/.github/workflows/arduino.yml +++ b/.github/workflows/arduino.yml @@ -25,7 +25,7 @@ jobs: - arduino-boards-fqbn: arduino:avr:uno # arudino uno - compiling almost all examples sketch-names: '**.ino' required-libraries: PciManager - sketches-exclude: teensy4_current_control_low_side, full_control_serial, angle_control, bluepill_position_control, esp32_position_control, esp32_i2c_dual_bus_example, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, osc_esp32_3pwm, osc_esp32_fullcontrol, nano33IoT_velocity_control, smartstepper_control,esp32_current_control_low_side, stm32_spi_alt_example, esp32_spi_alt_example, B_G431B_ESC1, odrive_example_spi, odrive_example_encoder, single_full_control_example, double_full_control_example, stm32_current_control_low_side, open_loop_velocity_6pwm + sketches-exclude: 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 - arduino-boards-fqbn: arduino:sam:arduino_due_x # arduino due - one full example sketch-names: single_full_control_example.ino 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/esp32.yml b/.github/workflows/esp32.yml index db551b41..2f5accd5 100644 --- a/.github/workflows/esp32.yml +++ b/.github/workflows/esp32.yml @@ -33,7 +33,7 @@ jobs: - 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, esp32_i2c_dual_bus_example.ino, stepper_driver_2pwm_standalone.ino, stepper_driver_4pwm_standalone.ino + 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 diff --git a/README.md b/README.md index 4fbb7517..4657f41f 100644 --- a/README.md +++ b/README.md @@ -7,13 +7,14 @@ [![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://ardubadge.simplefoc.com?lib=Simple%20FOC)](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) @@ -29,27 +30,30 @@ Therefore this is an attempt to: - 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.4 -> - ESP32 MCUs extended support [#414](https://github.com/simplefoc/Arduino-FOC/pull/414) -> - Transition to the arduino-esp32 version v3.x (ESP-IDF v5.x) [#387](https://github.com/espressif/arduino-esp32/releases) -> - New support for MCPWM driver -> - New support for LEDC drivers - center-aligned PWM and 6PWM available -> - Rewritten and simplified the fast ADC driver code (`adcRead`) - for low-side and inline current sensing. -> - Stepper motors current sensing support [#421](https://github.com/simplefoc/Arduino-FOC/pull/421) -> - Support for current sensing (low-side and inline) - [see in docs](https://docs.simplefoc.com/current_sense) -> - Support for true FOC control - `foc_current` torque control - [see in docs](https://docs.simplefoc.com/motion_control) -> - New current sense alignment procedure [#422](https://github.com/simplefoc/Arduino-FOC/pull/422) - [see in docs](https://docs.simplefoc.com/current_sense_align) -> - Support for steppers -> - Much more robust and reliable -> - More verbose and informative -> - Support for HallSensors without interrupts [#424](https://docs.simplefoc.com/https://github.com/simplefoc/Arduino-FOC/pull/424) - [see in docs](hall_sensors) +> NEXT RELEASE 📢 : SimpleFOClibrary v2.3.5 +> +> - ESP32 bugfix +> - after the low-level API changes in the Arduino-ESP32 core [PR447](https://github.com/simplefoc/Arduino-FOC/pull/447) +> - Pin is not configured [PR458](https://github.com/simplefoc/Arduino-FOC/pull/458) +> - C6 MCPWM bugfix [PR440](https://github.com/simplefoc/Arduino-FOC/pull/440) +> - New fuctionality +> - HybridStepperMotor added to the main library [PR457](https://github.com/simplefoc/Arduino-FOC/pull/457) - [see in docs](https://docs.simplefoc.com/steppermotor) +> - Motor characterisation (phase resistance and inductance) [PR436](https://github.com/simplefoc/Arduino-FOC/pull/436) - [see in docs](https://docs.simplefoc.com/bldcmotor#how-can-i-measure-the-phase-resistance-and-inductance) +> - SAMD21 support for low-side current sensing [PR479](https://github.com/simplefoc/Arduino-FOC/pull/479) +> - RP2350 support [PR435](https://github.com/simplefoc/Arduino-FOC/pull/435) [PR468](https://github.com/simplefoc/Arduino-FOC/pull/468) +> - STM32 +> - New driver code [PR442](https://github.com/simplefoc/Arduino-FOC/pull/442) +> - Low-side current sensing support for H7 family [PR460](https://github.com/simplefoc/Arduino-FOC/pull/460) > - Docs -> - A short guide to debugging of common issues -> - A short guide to the units in the library - [see in docs](https://docs.simplefoc.com/library_units) -> - See the complete list of bugfixes and new features of v2.3.4 [fixes and PRs](https://github.com/simplefoc/Arduino-FOC/milestone/11) +> - Hybrid stepper motor example [see in docs](https://docs.simplefoc.com/stepper_control_shield) +> - Sensorless FOC example [see in docs](https://docs.simplefoc.com/sensorless_foc_nucleo_example) +> - A short guide to synchronous loop - [see in docs](https://docs.simplefoc.com/real_time_loop) +> - See the complete list of bugfixes and new features of v2.3.5 [fixes and PRs](https://github.com/simplefoc/Arduino-FOC/milestone/12) -## Arduino *SimpleFOClibrary* v2.3.4 + + +## Arduino *SimpleFOClibrary* v2.3.5

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 9143dde7..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 @@ -38,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 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 5ff53311..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 @@ -53,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 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 c1ff7bbc..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 @@ -46,7 +46,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] // limit/set the velocity of the transition in between // target angles @@ -78,4 +78,4 @@ void loop() { // user communication command.run(); -} \ No newline at end of file +} 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/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 5daa49d9..354a2118 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=Simple FOC -version=2.3.4 +version=2.3.5 author=Simplefoc maintainer=Simplefoc sentence=A library demistifying FOC for BLDC motors diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index 501a8bc9..74c3ce2f 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -93,10 +93,13 @@ int 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); @@ -290,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); diff --git a/src/BLDCMotor.h b/src/BLDCMotor.h index c261e405..386c5f19 100644 --- a/src/BLDCMotor.h +++ b/src/BLDCMotor.h @@ -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 1d8f3147..ec805914 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -57,10 +57,13 @@ int 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); diff --git a/src/StepperMotor.h b/src/StepperMotor.h index 7e7810d8..208e453e 100644 --- a/src/StepperMotor.h +++ b/src/StepperMotor.h @@ -83,6 +83,17 @@ 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 */ diff --git a/src/common/base_classes/CurrentSense.cpp b/src/common/base_classes/CurrentSense.cpp index 4813dc3b..85ebd2c5 100644 --- a/src/common/base_classes/CurrentSense.cpp +++ b/src/common/base_classes/CurrentSense.cpp @@ -59,6 +59,22 @@ ABCurrent_s CurrentSense::getABCurrents(PhaseCurrent_s current){ 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; @@ -141,10 +157,18 @@ int CurrentSense::driverAlign(float voltage, bool modulation_centered){ if (!initialized) return 0; // check if stepper or BLDC - if(driver_type == DriverType::Stepper) - return alignStepperDriver(voltage, (StepperDriver*)driver, modulation_centered); - else - return alignBLDCDriver(voltage, (BLDCDriver*)driver, modulation_centered); + 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; + } } @@ -473,3 +497,239 @@ int CurrentSense::alignStepperDriver(float voltage, StepperDriver* stepper_drive } + + + +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 d3f7f8ed..3d9ec1b5 100644 --- a/src/common/base_classes/CurrentSense.h +++ b/src/common/base_classes/CurrentSense.h @@ -34,7 +34,7 @@ class CurrentSense{ 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::Unknown; //!< driver type (BLDC or Stepper) + DriverType driver_type = DriverType::UnknownDriver; //!< driver type (BLDC or Stepper) // ADC measurement gain for each phase @@ -135,6 +135,11 @@ class CurrentSense{ * 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 */ diff --git a/src/common/base_classes/FOCDriver.h b/src/common/base_classes/FOCDriver.h index 944251a4..263460b3 100644 --- a/src/common/base_classes/FOCDriver.h +++ b/src/common/base_classes/FOCDriver.h @@ -13,9 +13,10 @@ enum PhaseState : uint8_t { enum DriverType{ - Unknown=0, + UnknownDriver=0, BLDC=1, - Stepper=2 + Stepper=2, + Hybrid=3 }; /** diff --git a/src/common/base_classes/FOCMotor.cpp b/src/common/base_classes/FOCMotor.cpp index 5d8f8127..090034ce 100644 --- a/src/common/base_classes/FOCMotor.cpp +++ b/src/common/base_classes/FOCMotor.cpp @@ -92,6 +92,227 @@ 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() { diff --git a/src/common/base_classes/FOCMotor.h b/src/common/base_classes/FOCMotor.h index 8ae0e8af..93ee1106 100644 --- a/src/common/base_classes/FOCMotor.h +++ b/src/common/base_classes/FOCMotor.h @@ -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 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 caf29c19..d3eceb83 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp @@ -162,8 +162,8 @@ bool IRAM_ATTR adcInit(uint8_t pin){ analogReadResolution(SIMPLEFOC_ADC_RES); } pinMode(pin, ANALOG); - analogSetPinAttenuation(pin, SIMPLEFOC_ADC_ATTEN); analogRead(pin); + analogSetPinAttenuation(pin, SIMPLEFOC_ADC_ATTEN); #if CONFIG_IDF_TARGET_ESP32 // if esp32 variant __configFastADCs(); diff --git a/src/current_sense/hardware_specific/rp2040/rp2040_mcu.cpp b/src/current_sense/hardware_specific/rp2040/rp2040_mcu.cpp index d2ed881b..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" diff --git a/src/current_sense/hardware_specific/samd/samd21_mcu.cpp b/src/current_sense/hardware_specific/samd/samd21_mcu.cpp index 046f3db4..3a045446 100644 --- a/src/current_sense/hardware_specific/samd/samd21_mcu.cpp +++ b/src/current_sense/hardware_specific/samd/samd21_mcu.cpp @@ -1,319 +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); - - SIMPLEFOC_SAMD_DEBUG_SERIAL.println(F("TODO! _driverSyncLowSide() is not implemented")); - instance.startADCScan(); - //TODO: hook with PWM interrupts - - return cs_params; -} - - - - - - - - - - - // 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/stm32/b_g431/b_g431_mcu.cpp b/src/current_sense/hardware_specific/stm32/b_g431/b_g431_mcu.cpp index 46cb20be..697a2f0f 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 @@ -132,7 +132,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; @@ -153,21 +153,21 @@ void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ Stm32CurrentSenseParams* cs_params = (Stm32CurrentSenseParams*)_cs_params; // stop all the timers for the driver - _stopTimers(driver_params->timers, 6); + stm32_pause(driver_params); // 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) ){ + if( IS_TIM_REPETITION_COUNTER_INSTANCE(cs_params->timer_handle->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; + cs_params->timer_handle->Instance->CR1 |= TIM_CR1_DIR; + cs_params->timer_handle->Instance->CNT = cs_params->timer_handle->Instance->ARR; } // 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 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..65f3e8cb --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32_adc_utils.cpp @@ -0,0 +1,430 @@ +#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 + 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; +} + +/** + * @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) { + case 0: + return ADC_INJECTED_RANK_1; + break; + case 1: + return ADC_INJECTED_RANK_2; + break; + case 2: + return ADC_INJECTED_RANK_3; + break; + case 3: + return ADC_INJECTED_RANK_4; + break; + 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]) { + + // 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 +} + +// 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]) { + 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 \ 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 d3bea81e..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(); @@ -74,7 +50,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_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,52 +89,63 @@ 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); + 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; } + extern "C" { void ADC1_2_IRQHandler(void) { 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 49f2f3d5..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,29 +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}; +// array of values of 4 injected channels per adc instance (5) +uint32_t adc_val[5][4]={0}; #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT -uint8_t use_adc_interrupt = 1; +#define USE_ADC_INTERRUPT 1 #else -uint8_t use_adc_interrupt = 0; +#define USE_ADC_INTERRUPT 0 #endif -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; -#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){ @@ -42,7 +38,7 @@ 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; } @@ -56,36 +52,25 @@ void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ 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; - }else{ - if(!use_adc_interrupt){ - // If the timer has no repetition counter, it needs to use the interrupt to downsample for low side sensing - use_adc_interrupt = 1; - #ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-CS: timer has no repetition counter, ADC interrupt has to be used"); - #endif - } + 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 - if(use_adc_interrupt){ + if(adc_interrupt_config[adc_index].use_adc_interrupt){ HAL_NVIC_SetPriority(ADC1_2_IRQn, 0, 0); HAL_NVIC_EnableIRQ(ADC1_2_IRQn); @@ -96,7 +81,7 @@ void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ // restart all the timers of the driver - _startTimers(driver_params->timers, 6); + stm32_resume(driver_params); // return the cs parameters // successfully initialized @@ -107,35 +92,16 @@ void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ // function reading an ADC value and returning the read voltage float _readADCVoltageLowSide(const int pin, const void* cs_params){ - for(int i=0; i < 3; i++){ - if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]){ // found in the buffer - if (use_adc_interrupt){ - return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; - }else{ - // an optimized way to go from i to the channel i=0 -> channel 1, i=1 -> channel 2, i=2 -> channel 3 - uint32_t channel = (i == 0) ? ADC_INJECTED_RANK_1 : (i == 1) ? ADC_INJECTED_RANK_2 : ADC_INJECTED_RANK_3;; - return HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle, channel) * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; - } - } - } - return 0; + 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){ - // 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 \ 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 bd0df4b6..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,65 +103,58 @@ 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){ + }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; } } - 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; } extern "C" { 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 6b597d4e..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,35 +6,39 @@ #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 - -// 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}; - #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT -uint8_t use_adc_interrupt = 1; +#define USE_ADC_INTERRUPT 1 #else -uint8_t use_adc_interrupt = 0; +#define USE_ADC_INTERRUPT 0 #endif +// array of values of 4 injected channels per adc instance (5) +uint32_t adc_val[5][4]={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){ Stm32CurrentSenseParams* cs_params= new Stm32CurrentSenseParams { .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; } @@ -48,33 +52,22 @@ void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ 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; - }else{ - if(!use_adc_interrupt){ - // If the timer has no repetition counter, it needs to use the interrupt to downsample for low side sensing - use_adc_interrupt = 1; - #ifdef SIMPLEFOC_STM32_DEBUG + 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"); - #endif - } } + // 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 - if (use_adc_interrupt){ + if (adc_interrupt_config[adc_index].use_adc_interrupt){ // enable interrupt HAL_NVIC_SetPriority(ADC_IRQn, 0, 0); HAL_NVIC_EnableIRQ(ADC_IRQn); @@ -85,7 +78,7 @@ void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ } // restart all the timers of the driver - _startTimers(driver_params->timers, 6); + stm32_resume(driver_params); // return the cs parameters // successfully initialized @@ -96,34 +89,14 @@ void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ // function reading an ADC value and returning the read voltage float _readADCVoltageLowSide(const int pin, const void* cs_params){ - for(int i=0; i < 3; i++){ - if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]){ // found in the buffer - if (use_adc_interrupt){ - return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; - }else{ - // an optimized way to go from i to the channel i=0 -> channel 1, i=1 -> channel 2, i=2 -> channel 3 - uint32_t channel = (i == 0) ? ADC_INJECTED_RANK_1 : (i == 1) ? ADC_INJECTED_RANK_2 : ADC_INJECTED_RANK_3; - return HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle, channel) * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; - } - } - } - return 0; + 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){ - // 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]); } } 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 index d4cffec6..595eb2a4 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_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_RISINGFALLING; sConfigInjected.AutoInjectedConv = DISABLE; @@ -77,15 +78,19 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive // automating TRGO flag finding - hardware specific uint8_t tim_num = 0; for (size_t i=0; i<6; i++) { - HardwareTimer *timer_to_check = driver_params->timers[tim_num++]; - TIM_TypeDef *instance_to_check = timer_to_check->getHandle()->Instance; - - // bool TRGO_already_configured = instance_to_check->CR2 & LL_TIM_TRGO_UPDATE; - // if(TRGO_already_configured) continue; + 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; @@ -106,80 +111,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 - #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){ + }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 - // 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}; + 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 ADC_IRQHandler(void) { HAL_ADC_IRQHandler(&hadc); } } -#endif #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.h b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.h index 0a3614b5..391b3fb5 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.h +++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.h @@ -1,15 +1,17 @@ -#pragma once +#ifndef STM32F7_LOWSIDE_HAL +#define STM32F7_LOWSIDE_HAL #include "Arduino.h" #if defined(STM32F7xx) + #include "stm32f7xx_hal.h" -#include "../../../../common/foc_utils.h" -#include "../../../../drivers/hardware_specific/stm32/stm32_mcu.h" #include "../stm32_mcu.h" -#include "stm32f7_utils.h" +#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 + +#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 index f5ca70f3..d8bac8d8 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_mcu.cpp @@ -6,21 +6,32 @@ #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 "stm32f7_utils.h" #include "Arduino.h" #define _ADC_VOLTAGE 3.3f #define _ADC_RESOLUTION 4096.0f +// array of values of 4 injected channels per adc instance (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 +}; -// array of values of 4 injected channels per adc instance (3) -uint32_t adc_val[3][4]={0}; -// does adc interrupt need a downsample - per adc (3) -bool needs_downsample[3] = {1}; -// downsampling variable - per adc (3) -uint8_t tim_downsample[3] = {1}; void* _configureADCLowSide(const void* driver_params, const int pinA, const int pinB, const int pinC){ @@ -28,7 +39,7 @@ 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) / (_ADC_RESOLUTION) }; - _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; } @@ -42,33 +53,32 @@ void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ 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 @@ -79,38 +89,15 @@ void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ // function reading an ADC value and returning the read voltage float _readADCVoltageLowSide(const int pin, const void* cs_params){ - for(int i=0; i < 3; i++){ - if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]){ // found in the buffer - #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/stm32f7/stm32f7_utils.cpp b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_utils.cpp index d5f8c6b2..fbd20d40 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_utils.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_utils.cpp @@ -1,145 +1,7 @@ -#include "stm32f7_utils.h" +#include "../stm32_adc_utils.h" #if defined(STM32F7xx) -/* Exported Functions */ - - -PinName analog_to_pin(uint32_t pin) { - PinName pin_name = analogInputToPinName(pin); - if (pin_name == NC) { - return (PinName) pin; - } - return pin_name; -} - - -/** - * @brief Return ADC HAL channel linked to a PinName - * @param pin: PinName - * @retval Valid HAL channel - */ -uint32_t _getADCChannel(PinName pin) -{ - uint32_t function = pinmap_function(pin, PinMap_ADC); - uint32_t channel = 0; - switch (STM_PIN_CHANNEL(function)) { -#ifdef ADC_CHANNEL_0 - case 0: - channel = ADC_CHANNEL_0; - break; -#endif - case 1: - channel = ADC_CHANNEL_1; - break; - case 2: - channel = ADC_CHANNEL_2; - break; - case 3: - channel = ADC_CHANNEL_3; - break; - case 4: - channel = ADC_CHANNEL_4; - break; - case 5: - channel = ADC_CHANNEL_5; - break; - case 6: - channel = ADC_CHANNEL_6; - break; - case 7: - channel = ADC_CHANNEL_7; - break; - case 8: - channel = ADC_CHANNEL_8; - break; - case 9: - channel = ADC_CHANNEL_9; - break; - case 10: - channel = ADC_CHANNEL_10; - break; - case 11: - channel = ADC_CHANNEL_11; - break; - case 12: - channel = ADC_CHANNEL_12; - break; - case 13: - channel = ADC_CHANNEL_13; - break; - case 14: - channel = ADC_CHANNEL_14; - break; - case 15: - channel = ADC_CHANNEL_15; - break; -#ifdef ADC_CHANNEL_16 - case 16: - channel = ADC_CHANNEL_16; - break; -#endif - case 17: - channel = ADC_CHANNEL_17; - break; -#ifdef ADC_CHANNEL_18 - case 18: - channel = ADC_CHANNEL_18; - break; -#endif -#ifdef ADC_CHANNEL_19 - case 19: - channel = ADC_CHANNEL_19; - break; -#endif -#ifdef ADC_CHANNEL_20 - case 20: - channel = ADC_CHANNEL_20; - break; - case 21: - channel = ADC_CHANNEL_21; - break; - case 22: - channel = ADC_CHANNEL_22; - break; - case 23: - channel = ADC_CHANNEL_23; - break; -#ifdef ADC_CHANNEL_24 - case 24: - channel = ADC_CHANNEL_24; - break; - case 25: - channel = ADC_CHANNEL_25; - break; - case 26: - channel = ADC_CHANNEL_26; - break; -#ifdef ADC_CHANNEL_27 - case 27: - channel = ADC_CHANNEL_27; - break; - case 28: - channel = ADC_CHANNEL_28; - break; - case 29: - channel = ADC_CHANNEL_29; - break; - case 30: - channel = ADC_CHANNEL_30; - break; - case 31: - channel = ADC_CHANNEL_31; - break; -#endif -#endif -#endif - default: - _Error_Handler("ADC: Unknown adc channel", (int)(STM_PIN_CHANNEL(function))); - break; - } - return channel; -} /* TIM1 TIM2 @@ -163,28 +25,28 @@ ADC_EXTERNALTRIGINJECCONV_T6_TRGO */ // timer to injected TRGO // https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_adc_ex.h#L179 -uint32_t _timerToInjectedTRGO(HardwareTimer* timer){ +uint32_t _timerToInjectedTRGO(TIM_HandleTypeDef* timer){ - if(timer->getHandle()->Instance == TIM1) + 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 #ifdef TIM6 // if defined timer 6 - else if(timer->getHandle()->Instance == TIM6) + else if(timer->Instance == TIM6) return ADC_EXTERNALTRIGINJECCONV_T6_TRGO; #endif #ifdef TIM8 // if defined timer 8 - else if(timer->getHandle()->Instance == TIM8) + else if(timer->Instance == TIM8) return ADC_EXTERNALTRIGINJECCONV_T8_TRGO; #endif else @@ -204,52 +66,31 @@ ADC_EXTERNALTRIGCONV_T6_TRGO // timer to regular TRGO // https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_adc.h#L331 -uint32_t _timerToRegularTRGO(HardwareTimer* timer){ - if(timer->getHandle()->Instance == TIM1) +uint32_t _timerToRegularTRGO(TIM_HandleTypeDef* timer){ + if(timer->Instance == TIM1) return ADC_EXTERNALTRIGCONV_T1_TRGO; #ifdef TIM2 // if defined timer 2 - else if(timer->getHandle()->Instance == TIM2) + else if(timer->Instance == TIM2) return ADC_EXTERNALTRIGCONV_T2_TRGO; #endif #ifdef TIM4 // if defined timer 4 - else if(timer->getHandle()->Instance == TIM4) + else if(timer->Instance == TIM4) return ADC_EXTERNALTRIGCONV_T4_TRGO; #endif #ifdef TIM5 // if defined timer 5 - else if(timer->getHandle()->Instance == TIM5) + else if(timer->Instance == TIM5) return ADC_EXTERNALTRIGCONV_T5_TRGO; #endif #ifdef TIM6 // if defined timer 6 - else if(timer->getHandle()->Instance == TIM6) + else if(timer->Instance == TIM6) return ADC_EXTERNALTRIGCONV_T6_TRGO; #endif #ifdef TIM8 // if defined timer 8 - else if(timer->getHandle()->Instance == TIM8) + else 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); -} - #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_utils.h b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_utils.h deleted file mode 100644 index 017ff464..00000000 --- a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_utils.h +++ /dev/null @@ -1,30 +0,0 @@ -#pragma once - -#include "Arduino.h" - -#if defined(STM32F7xx) - -#define _TRGO_NOT_AVAILABLE 12345 - - -/* Exported Functions */ -/** - * @brief Return ADC HAL channel linked to a PinName - * @param pin: PinName - * @retval Valid HAL channel - */ -uint32_t _getADCChannel(PinName pin); - -// timer to injected TRGO -// https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_adc_ex.h#L179 -uint32_t _timerToInjectedTRGO(HardwareTimer* timer); - -// timer to regular TRGO -// https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_adc.h#L331 -uint32_t _timerToRegularTRGO(HardwareTimer* timer); - -// function returning index of the ADC instance -int _adcToIndex(ADC_HandleTypeDef *AdcHandle); -int _adcToIndex(ADC_TypeDef *AdcHandle); - -#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp index fd1090ae..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; @@ -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,62 +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){ + }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; } } - 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; } + extern "C" { void ADC1_2_IRQHandler(void) { 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 9c73f6d7..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,27 +16,32 @@ #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 -uint8_t use_adc_interrupt = 1; +#define USE_ADC_INTERRUPT 1 #else -uint8_t use_adc_interrupt = 0; +#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_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; } @@ -50,37 +55,25 @@ void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ 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; - }else{ - if(!use_adc_interrupt){ - // If the timer has no repetition counter, it needs to use the interrupt to downsample for low side sensing - use_adc_interrupt = 1; - #ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-CS: timer has no repetition counter, ADC interrupt has to be used"); - #endif - } + 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); + HAL_ADCEx_Calibration_Start(cs_params->adc_handle, ADC_SINGLE_ENDED); // start the adc - if (use_adc_interrupt){ + if (adc_interrupt_config[adc_index].use_adc_interrupt){ // enable interrupt if(cs_params->adc_handle->Instance == ADC1) { // enable interrupt @@ -122,7 +115,7 @@ void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ } // restart all the timers of the driver - _startTimers(driver_params->timers, 6); + stm32_resume(driver_params); // return the cs parameters // successfully initialized @@ -133,34 +126,14 @@ void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ // function reading an ADC value and returning the read voltage float _readADCVoltageLowSide(const int pin, const void* cs_params){ - for(int i=0; i < 3; i++){ - if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]){ // found in the buffer - if (use_adc_interrupt){ - return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; - }else{ - // an optimized way to go from i to the channel i=0 -> channel 1, i=1 -> channel 2, i=2 -> channel 3 - uint32_t channel = (i == 0) ? ADC_INJECTED_RANK_1 : (i == 1) ? ADC_INJECTED_RANK_2 : ADC_INJECTED_RANK_3; - return HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle, channel) * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; - } - } - } - return 0; + 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){ - // 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]); } } 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 67a0473b..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 } @@ -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,60 +147,60 @@ 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; } } - 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; } extern "C" { 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 5de6432a..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,24 +18,29 @@ // 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 -uint8_t use_adc_interrupt = 1; +#define USE_ADC_INTERRUPT 1 #else -uint8_t use_adc_interrupt = 0; +#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_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; } @@ -49,37 +54,26 @@ void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ 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; - }else{ - if(!use_adc_interrupt){ - // If the timer has no repetition counter, it needs to use the interrupt to downsample for low side sensing - use_adc_interrupt = 1; - #ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-CS: timer has no repetition counter, ADC interrupt has to be used"); - #endif - } + 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 - if (use_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); @@ -119,44 +113,23 @@ void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ } // 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 - if (use_adc_interrupt){ - return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; - }else{ - // an optimized way to go from i to the channel i=0 -> channel 1, i=1 -> channel 2, i=2 -> channel 3 - uint32_t channel = (i == 0) ? ADC_INJECTED_RANK_1 : (i == 1) ? ADC_INJECTED_RANK_2 : ADC_INJECTED_RANK_3; - return HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle,channel) * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; - } - } - } - return 0; + 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){ - // 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]); } } 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/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp index a481c6ff..c67ffa36 100644 --- a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp +++ b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp @@ -181,7 +181,7 @@ uint8_t _findNextTimer(int group){ */ int _findBestGroup(int no_pins, long pwm_freq, int* group, int* timer){ // an empty group is always the best option - for(int i=0; i<2; i++){ + for(int i=0; ichannels[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; diff --git a/src/drivers/hardware_specific/esp32/esp32_mcpwm_mcu.cpp b/src/drivers/hardware_specific/esp32/esp32_mcpwm_mcu.cpp index e2c621c5..2b18417f 100644 --- a/src/drivers/hardware_specific/esp32/esp32_mcpwm_mcu.cpp +++ b/src/drivers/hardware_specific/esp32/esp32_mcpwm_mcu.cpp @@ -6,6 +6,16 @@ #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 @@ -20,7 +30,7 @@ void* _configure1PWM(long pwm_frequency, const int pinA) { } SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 1PWM in group: "+String(group)+" on timer: "+String(timer)); // configure the timer - int pins[1] = {pinA}; + int pins[1] = { esp32_gpio_nr(pinA) }; return _configurePinsMCPWM(pwm_frequency, group, timer, 1, pins); } @@ -42,7 +52,7 @@ void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { // 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] = {pinA, pinB}; + 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"); @@ -92,7 +102,7 @@ void* _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const i } SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 3PWM in group: "+String(group)+" on timer: "+String(timer)); // configure the timer - int pins[3] = {pinA, pinB, pinC}; + int pins[3] = { esp32_gpio_nr(pinA), esp32_gpio_nr(pinB), esp32_gpio_nr(pinC) }; return _configurePinsMCPWM(pwm_frequency, group, timer, 3, pins); } @@ -122,7 +132,7 @@ void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const in // 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] = {{pinA, pinB},{pinC, pinD}}; + 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)); @@ -162,7 +172,7 @@ void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, cons } SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 6PWM in group: "+String(group)+" on timer: "+String(timer)); // configure the timer - int pins[6] = {pinA_h,pinA_l, pinB_h, pinB_l, pinC_h, pinC_l}; + 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); } 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 6afbf345..03b4a894 100644 --- a/src/drivers/hardware_specific/rp2040/rp2040_mcu.cpp +++ b/src/drivers/hardware_specific/rp2040/rp2040_mcu.cpp @@ -6,11 +6,11 @@ #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) @@ -93,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/stm32/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp index 4009281e..1a9c833e 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp @@ -1,115 +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 -#define SIMPLEFOC_STM32_DEBUG #pragma message("") #pragma message("SimpleFOC: compiling for STM32") #pragma message("") -#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 - HardwareTimer* pHT = ((STM32DriverParams*)params)->timers[0]; - TIM_HandleTypeDef* htim = pHT->getHandle(); - - bool dir = __HAL_TIM_IS_TIM_COUNTING_DOWN(htim); - + 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); -} - - -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_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]); } } - 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]); + } + } +} + // 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; } @@ -117,382 +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); - -#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; +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; } -// 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); +// } -// 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(); -} +// 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 _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 - } -} -#if defined(STM32G4xx) -// function finds the appropriate timer source trigger for the master/slave timer combination -// returns -1 if no trigger source is found -// currently supports the master timers to be from TIM1 to TIM4 and TIM8 -int _getInternalSourceTrigger(HardwareTimer* master, HardwareTimer* slave) { // put master and slave in temp variables to avoid arrows - TIM_TypeDef *TIM_master = master->getHandle()->Instance; - #if defined(TIM1) && defined(LL_TIM_TS_ITR0) - if (TIM_master == TIM1) return LL_TIM_TS_ITR0;// return TIM_TS_ITR0; - #endif - #if defined(TIM2) && defined(LL_TIM_TS_ITR1) - else if (TIM_master == TIM2) return LL_TIM_TS_ITR1;//return TIM_TS_ITR1; - #endif - #if defined(TIM3) && defined(LL_TIM_TS_ITR2) - else if (TIM_master == TIM3) return LL_TIM_TS_ITR2;//return TIM_TS_ITR2; - #endif - #if defined(TIM4) && defined(LL_TIM_TS_ITR3) - else if (TIM_master == TIM4) return LL_TIM_TS_ITR3;//return TIM_TS_ITR3; - #endif - #if defined(TIM5) && defined(LL_TIM_TS_ITR4) - else if (TIM_master == TIM5) return LL_TIM_TS_ITR4;//return TIM_TS_ITR4; - #endif - #if defined(TIM8) && defined(LL_TIM_TS_ITR5) - else if (TIM_master == TIM8) return LL_TIM_TS_ITR5;//return TIM_TS_ITR5; - #endif - return -1; -} -#elif defined(STM32F4xx) || defined(STM32F1xx) || defined(STM32L4xx) || defined(STM32F7xx) - -// function finds the appropriate timer source trigger for the master/slave timer combination -// returns -1 if no trigger source is found -// currently supports the master timers to be from TIM1 to TIM4 and TIM8 -int _getInternalSourceTrigger(HardwareTimer* master, HardwareTimer* slave) { - // put master and slave in temp variables to avoid arrows - TIM_TypeDef *TIM_master = master->getHandle()->Instance; - TIM_TypeDef *TIM_slave = slave->getHandle()->Instance; - #if defined(TIM1) && defined(LL_TIM_TS_ITR0) - if (TIM_master == TIM1){ - #if 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 - } - #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(TIM5) - else if(TIM_slave == TIM5) return LL_TIM_TS_ITR0; - #endif - } - #endif - #if defined(TIM3) && defined(LL_TIM_TS_ITR2) - else if (TIM_master == TIM3){ - #if 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(TIM5) - else if(TIM_slave == TIM5) return LL_TIM_TS_ITR1; - #endif - } - #endif - #if defined(TIM4) && defined(LL_TIM_TS_ITR3) - else if (TIM_master == TIM4){ - #if 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(TIM5) - else if(TIM_slave == TIM5) return LL_TIM_TS_ITR1; - #endif - } - #endif - #if defined(TIM5) - else if (TIM_master == TIM5){ - #if !defined(STM32L4xx) // only difference between F4,F1 and L4 - #if 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 - } - #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(TIM5) - else if(TIM_slave == TIM5) return LL_TIM_TS_ITR3; - #endif - } - #endif - return -1; // combination not supported -} -#else -// Alignment not supported for this architecture -int _getInternalSourceTrigger(HardwareTimer* master, HardwareTimer* slave) { - return -1; -} -#endif -void syncTimerFrequency(long pwm_frequency, HardwareTimer *timers[], uint8_t num_timers) { - uint32_t max_frequency = 0; - uint32_t min_frequency = UINT32_MAX; - for (size_t i=0; igetTimerClkFreq(); - if (freq > max_frequency) { - max_frequency = freq; - } else if (freq < min_frequency) { - min_frequency = freq; - } - } - if (max_frequency==min_frequency) return; - uint32_t overflow_value = min_frequency/pwm_frequency; - for (size_t i=0; igetTimerClkFreq()/min_frequency; - #ifdef SIMPLEFOC_DEBUG - SIMPLEFOC_DEBUG("STM32-DRV: Setting prescale to ", (float)prescale_factor); - SIMPLEFOC_DEBUG("STM32-DRV: Setting Overflow to ", (float)overflow_value); - #endif - timers[i]->setPrescaleFactor(prescale_factor); - timers[i]->setOverflow(overflow_value,TICK_FORMAT); - timers[i]->refresh(); - } -} -void _alignTimersNew() { - int numTimers = 0; - HardwareTimer *timers[numTimerPinsUsed]; - - // find the timers used - 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; } - - #ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-DRV: Syncronising timers! Timer no. ", numTimers); - #endif - - // see if there is more then 1 timers used for the pwm - // if yes, try to align timers - if(numTimers > 1){ - // find the master timer - int16_t master_index = -1; - int triggerEvent = -1; - for (int i=0; igetHandle()->Instance)) { - // check if timer already configured in TRGO update mode (used for ADC triggering) - // in that case we should not change its TRGO configuration - if(timers[i]->getHandle()->Instance->CR2 & LL_TIM_TRGO_UPDATE) continue; - // check if the timer has the supported internal trigger for other timers - for (int slave_i=0; slave_i 1.0f) { #ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-DRV: Aligning PWM to master timer: ", getTimerNumber(get_timer_index(timers[master_index]->getHandle()->Instance))); + SIMPLEFOC_DEBUG("STM32-DRV: ERR: Common timer frequency unexpected: ", common_pwm_freq); #endif - // make the master timer generate ITRGx event - // if it was already configured in slave mode - LL_TIM_SetSlaveMode(timers[master_index]->getHandle()->Instance, LL_TIM_SLAVEMODE_DISABLED ); - // Configure the master timer to send a trigger signal on enable - LL_TIM_SetTriggerOutput(timers[master_index]->getHandle()->Instance, LL_TIM_TRGO_ENABLE); - // configure other timers to get the input trigger from the master timer - for (int slave_index=0; slave_index < numTimers; slave_index++) { - if (slave_index == master_index) - continue; - // Configure the slave timer to be triggered by the master enable signal - LL_TIM_SetTriggerInput(timers[slave_index]->getHandle()->Instance, _getInternalSourceTrigger(timers[master_index], timers[slave_index])); - LL_TIM_SetSlaveMode(timers[slave_index]->getHandle()->Instance, LL_TIM_SLAVEMODE_TRIGGER); - } - } + return -1; } - - // 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(); - } - -} - - - -// align the timers to end the init -void _startTimers(HardwareTimer **timers_to_start, int timer_num) -{ - // // TODO - start each timer only once - // // start timers - // for (int i=0; i < timer_num; i++) { - // if(timers_to_start[i] == NP) return; - // timers_to_start[i]->resume(); - // #ifdef SIMPLEFOC_STM32_DEBUG - // SIMPLEFOC_DEBUG("STM32-DRV: Starting timer ", getTimerNumber(get_timer_index(timers_to_start[i]->getHandle()->Instance))); - // #endif - // } - _alignTimersNew(); + 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 @@ -504,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; } @@ -574,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]);\ - // align 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; } @@ -793,87 +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]); - HardwareTimer *timers[2] = {HT1, HT2}; - syncTimerFrequency(pwm_frequency, timers, 2); - // 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; } -TIM_MasterConfigTypeDef sMasterConfig; -TIM_SlaveConfigTypeDef sSlaveConfig; - // 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); - HardwareTimer *timers[3] = {HT1, HT2, HT3}; - syncTimerFrequency(pwm_frequency, timers, 3); + 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; } @@ -883,28 +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]); - HardwareTimer *timers[4] = {HT1, HT2, HT3, HT4}; - syncTimerFrequency(pwm_frequency, timers, 4); - // 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); @@ -912,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; } @@ -927,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); } @@ -973,35 +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]); - HardwareTimer *timers[6] = {HT1, HT2, HT3, HT4, HT5, HT6}; - syncTimerFrequency(pwm_frequency, timers, 6); + 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); @@ -1009,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; } } @@ -1045,146 +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 411c43b2..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 + -// 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 +#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 + + +/** + * 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,19 +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); -// timer query functions -bool _getPwmState(void* params); +// 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 #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/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/MagneticSensorI2C.cpp b/src/sensors/MagneticSensorI2C.cpp index 2b3db0c2..64b7dd44 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,10 @@ 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 */ @@ -21,7 +27,10 @@ MagneticSensorI2CConfig_s MT6701_I2C = { .chip_address = 0x06, .bit_resolution = 14, .angle_register = 0x03, - .data_start_bit = 15 + .msb_mask = 0xFF, + .msb_shift = 6, + .lsb_mask = 0xFC, + .lsb_shift = 2 }; @@ -30,57 +39,49 @@ MagneticSensorI2CConfig_s MT6701_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 - // MT6701 uses 0..5 LSB and 9..15 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; } + + MagneticSensorI2C MagneticSensorI2C::AS5600() { return {AS5600_I2C}; } -void MagneticSensorI2C::init(TwoWire* _wire){ - - wire = _wire; - // I2C communication begin - wire->begin(); +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(){ @@ -90,39 +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, 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 - // MT6701 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; - - };