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/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..3dcc7385 100644 --- a/README.md +++ b/README.md @@ -29,27 +29,13 @@ 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) -> - 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) - - -## Arduino *SimpleFOClibrary* v2.3.4 +> NEXT RELEASE 📢 : SimpleFOClibrary v2.3.5 +> - Motor characterization code thanks to @mcells +> - Bugfix for ESP32 C6 thanks to @kondor1622 +> - 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.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..5ad8a93f 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -290,9 +290,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..2e70965f --- /dev/null +++ b/src/HybridStepperMotor.cpp @@ -0,0 +1,588 @@ +#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; +} + +// 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 + 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.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/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..33774a55 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32_adc_utils.cpp @@ -0,0 +1,347 @@ +#include "stm32_adc_utils.h" + +#if defined(_STM32_DEF_) + +// 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 + + + +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; + } +} + +#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..7e74a96f --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32_adc_utils.h @@ -0,0 +1,38 @@ +#ifndef STM32_ADC_UTILS_HAL +#define STM32_ADC_UTILS_HAL + +#include "Arduino.h" + +#if defined(_STM32_DEF_) + +#define _TRGO_NOT_AVAILABLE 12345 + +#include "../../../common/foc_utils.h" +#include "../../../communication/SimpleFOCDebug.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[]); +#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..6d69dc89 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" @@ -25,24 +26,13 @@ uint8_t use_adc_interrupt = 1; uint8_t 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; -} - 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_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,17 +46,17 @@ 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); + 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. // - 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; + cs_params->timer_handle->Instance->CR1 |= TIM_CR1_DIR; + cs_params->timer_handle->Instance->CNT = cs_params->timer_handle->Instance->ARR; // remember that this timer has repetition counter - no need to downasmple needs_downsample[_adcToIndex(cs_params->adc_handle)] = 0; }else{ @@ -79,7 +69,7 @@ void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ } } // 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); @@ -96,7 +86,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,16 +97,18 @@ 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){ + uint8_t channel_no = 0; 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; + return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][channel_no] * ((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;; + uint32_t channel = _getADCInjectedRank(channel_no); return HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle, channel) * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; } } + if(_isset(((Stm32CurrentSenseParams*)cs_params)->pins[i])) channel_no++; } return 0; } 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..be49adfe 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp @@ -6,8 +6,8 @@ #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" @@ -34,7 +34,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_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,17 +48,17 @@ 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); + 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. // - 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; + cs_params->timer_handle->Instance->CR1 |= TIM_CR1_DIR; + cs_params->timer_handle->Instance->CNT = cs_params->timer_handle->Instance->ARR; // remember that this timer has repetition counter - no need to downasmple needs_downsample[_adcToIndex(cs_params->adc_handle)] = 0; }else{ @@ -71,7 +71,7 @@ void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ } } // 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){ @@ -85,7 +85,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,16 +96,18 @@ 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){ + uint8_t channel_no = 0; 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; + return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][channel_no] * ((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; + uint32_t channel = _getADCInjectedRank(channel_no); return HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle, channel) * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; } } + if(_isset(((Stm32CurrentSenseParams*)cs_params)->pins[i])) channel_no++; } return 0; } 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..23d4b0d3 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,19 @@ 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){ + if(timer->Instance == TIM2) return ADC_EXTERNALTRIGCONV_T2_TRGO; #ifdef TIM3 // if defined timer 3 - else if(timer->getHandle()->Instance == TIM3) + else if(timer->Instance == TIM3) return ADC_EXTERNALTRIGCONV_T3_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/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..ec5ccb64 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,42 +111,23 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot sync any timer to injected channels!"); #endif return -1; - } - // display which timer is being used - #ifdef SIMPLEFOC_STM32_DEBUG - // it would be better to use the getTimerNumber from driver - SIMPLEFOC_DEBUG("STM32-CS: injected trigger for timer index: ", get_timer_index(cs_params->timer_handle->getHandle()->Instance) + 1); - #endif - - - // first channel - sConfigInjected.InjectedRank = ADC_INJECTED_RANK_1; - sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[0])); - if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ -#ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[0])) ); -#endif - return -1; - } - - // second channel - sConfigInjected.InjectedRank = ADC_INJECTED_RANK_2; - sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[1])); - if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ + }else{ #ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[1]))) ; + SIMPLEFOC_DEBUG("STM32-CS: Using timer: ", stm32_getTimerNumber(cs_params->timer_handle->Instance)); #endif - return -1; } - // third channel - if exists - if(_isset(cs_params->pins[2])){ - sConfigInjected.InjectedRank = ADC_INJECTED_RANK_3; - sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[2])); + uint8_t channel_no = 0; + for(int i=0; i<3; i++){ + // skip if not set + if (!_isset(cs_params->pins[i])) continue; + + sConfigInjected.InjectedRank = _getADCInjectedRank(channel_no++); + sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[i]), hadc.Instance); if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ -#ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[2]))) ; -#endif + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[i]) , hadc.Instance)); + #endif return -1; } } @@ -156,21 +142,36 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive 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 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..664262dc 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_mcu.cpp @@ -6,8 +6,8 @@ #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" @@ -28,7 +28,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,23 +42,23 @@ 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); + 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. // - 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; + cs_params->timer_handle->Instance->CR1 |= TIM_CR1_DIR; + cs_params->timer_handle->Instance->CNT = cs_params->timer_handle->Instance->ARR; // remember that this timer has repetition counter - no need to downasmple needs_downsample[_adcToIndex(cs_params->adc_handle)] = 0; } // set the trigger output event - LL_TIM_SetTriggerOutput(cs_params->timer_handle->getHandle()->Instance, LL_TIM_TRGO_UPDATE); + LL_TIM_SetTriggerOutput(cs_params->timer_handle->Instance, LL_TIM_TRGO_UPDATE); // start the adc #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT @@ -68,7 +68,7 @@ void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ #endif // restart all the timers of the driver - _startTimers(driver_params->timers, 6); + stm32_resume(driver_params); // return the cs parameters // successfully initialized @@ -79,16 +79,18 @@ 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){ + uint8_t channel_no = 0; 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; + return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][channel_no] * ((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; + uint32_t channel = _getADCInjectedRank(channel_no); return HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle, channel) * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; #endif } + if(_isset(((Stm32CurrentSenseParams*)cs_params)->pins[i])) channel_no++; } return 0; } 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..5db5cea7 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 @@ -36,7 +36,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_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,17 +50,17 @@ 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); + 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. // - 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; + cs_params->timer_handle->Instance->CR1 |= TIM_CR1_DIR; + cs_params->timer_handle->Instance->CNT = cs_params->timer_handle->Instance->ARR; // remember that this timer has repetition counter - no need to downasmple needs_downsample[_adcToIndex(cs_params->adc_handle)] = 0; }else{ @@ -74,10 +74,10 @@ void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ } // 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){ @@ -122,7 +122,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,16 +133,18 @@ 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){ + uint8_t channel_no = 0; 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; + return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][channel_no] * ((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; + uint32_t channel = _getADCInjectedRank(channel_no); return HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle, channel) * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; } } + if(_isset(((Stm32CurrentSenseParams*)cs_params)->pins[i])) channel_no++; } return 0; } @@ -161,6 +163,7 @@ extern "C" { 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); + adc_val[adc_index][3]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_4); } } 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/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..4a6e529c 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" @@ -35,7 +35,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_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,17 +49,17 @@ 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); + 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. // - 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; + cs_params->timer_handle->Instance->CR1 |= TIM_CR1_DIR; + cs_params->timer_handle->Instance->CNT = cs_params->timer_handle->Instance->ARR; // remember that this timer has repetition counter - no need to downasmple needs_downsample[_adcToIndex(cs_params->adc_handle)] = 0; }else{ @@ -73,7 +73,7 @@ void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ } // 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); @@ -119,7 +119,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 // TODO verify if success in future @@ -129,16 +129,18 @@ 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){ + uint8_t channel_no = 0; 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; + return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][channel_no] * ((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; + uint32_t channel = _getADCInjectedRank(channel_no); return HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle,channel) * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; } } + if(_isset(((Stm32CurrentSenseParams*)cs_params)->pins[i])) channel_no++; } return 0; } 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..80d936d1 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; 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..3eef0849 --- /dev/null +++ b/src/drivers/hardware_specific/stm32/stm32_timerutils.cpp @@ -0,0 +1,932 @@ + +#include "./stm32_timerutils.h" +#include + +#if defined(_STM32_DEF_) || defined(TARGET_STM32H7) + + +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(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 TIM1 to TIM4 and TIM8 +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 + } + #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) && !defined(TARGET_STM32H7) + 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) && !defined(TARGET_STM32H7) + else if(TIM_slave == TIM5) return LL_TIM_TS_ITR1; + #endif + #if defined(TIM5) && defined(TARGET_STM32H7) + else if(TIM_slave == TIM5) return LL_TIM_TS_ITR2; + #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) && !defined(TARGET_STM32H7) + else if(TIM_slave == TIM5) return LL_TIM_TS_ITR1; + #endif + #if defined(TIM5) && defined(TARGET_STM32H7) + else if(TIM_slave == TIM5) return LL_TIM_TS_ITR3; + #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 + #if defined(TIM15) && 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 + 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; - - };