diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml
index 98f1ce9d..59ebbcf2 100644
--- a/.github/workflows/ccpp.yml
+++ b/.github/workflows/ccpp.yml
@@ -20,6 +20,7 @@ jobs:
- STMicroelectronics:stm32:GenF1:pnum=BLUEPILL_F103C8 # stm32 bluepill
- STMicroelectronics:stm32:Nucleo_64:pnum=NUCLEO_F411RE # stm32 nucleo
- STMicroelectronics:stm32:GenF4:pnum=GENERIC_F405RGTX # stm32f405 - odrive
+ - STMicroelectronics:stm32:GenL4:pnum=GENERIC_L475RGTX # stm32l475
- STMicroelectronics:stm32:Disco:pnum=B_G431B_ESC1 # B-G431-ESC1
- arduino:mbed_rp2040:pico # rpi pico
@@ -75,6 +76,10 @@ jobs:
platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json
sketch-names: odrive_example_encoder.ino, odrive_example_spi.ino, stm32_current_control_low_side.ino
+ - arduino-boards-fqbn: STMicroelectronics:stm32:GenL4:pnum=GENERIC_L475RGTX # stm32l475
+ platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json
+ sketch-names: single_full_control_example.ino, stm32_spi_alt_example.ino, double_full_control_example.ino, stm32_current_control_low_side.ino
+
- arduino-boards-fqbn: STMicroelectronics:stm32:Nucleo_64:pnum=NUCLEO_F411RE # nucleo one full example
platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json
sketch-names: single_full_control_example.ino, stm32_spi_alt_example.ino, double_full_control_example.ino, stm32_current_control_low_side.ino
diff --git a/README.md b/README.md
index af50455c..424f3cd7 100644
--- a/README.md
+++ b/README.md
@@ -24,7 +24,7 @@ Therefore this is an attempt to:
- *Medium-power* BLDC driver (<30Amps): [Arduino SimpleFOCPowerShield ](https://github.com/simplefoc/Arduino-SimpleFOC-PowerShield).
- See also [@byDagor](https://github.com/byDagor)'s *fully-integrated* ESP32 based board: [Dagor Brushless Controller](https://github.com/byDagor/Dagor-Brushless-Controller)
-> NEXT RELEASE 📢 : SimpleFOClibrary v2.3.0
+> NEW RELEASE 📢 : SimpleFOClibrary v2.3.0
> - Arduino Mega 6pwm more timers supported
> - Arduino boards - frequency change support either 32kHz or 4kHz
> - Arduino Uno - synched timers in 3pwm and 6pwm mode [#71](https://github.com/simplefoc/Arduino-FOC/issues/71)
diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp
index 8865f57c..128db181 100644
--- a/src/StepperMotor.cpp
+++ b/src/StepperMotor.cpp
@@ -371,6 +371,70 @@ void StepperMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) {
driver->setPwm(Ualpha, Ubeta);
}
+// Method using FOC to set Uq and Ud to the motor at the optimal angle
+// Function implementing Sine PWM algorithms
+// - space vector not implemented yet
+//
+
+int32_t float_to_q31(float input) {
+ int32_t q31 = (int32_t) (input * 2147483648.0f);
+ q31 = (q31 > 0) ? (q31 << 1) : (-q31 << 1);
+ return q31;
+}
+
+#define pi 3.1415926535897932384626433f
+
+int32_t q31_value;
+float value_f32_sine = 0;
+float value_f32_cosine = 0;
+float cordic_cosine = 0.0f;
+float cordic_sine = 0.0f;
+
+float wrap_to_1(float x) {
+ while (x > 1.0f) {
+ x -= 2.0f;
+ }
+ while (x < -1.0f) {
+ x += 2.0f;
+ }
+ return x;
+}
+
+void StepperMotor::setPhaseVoltageCORDIC(float Uq, float Ud, float angle_el) {
+ // Sinusoidal PWM modulation
+ // Inverse Park transformation
+
+ // WHO U GONNA CALL? CORDIC ->
+
+q31_value = float_to_q31(angle_el / (2.0f * pi));
+
+CORDIC->WDATA = q31_value;
+cordic_sine = CORDIC->RDATA;
+cordic_cosine = CORDIC->RDATA;
+
+value_f32_sine = (float)cordic_sine/(float)0x80000000;
+value_f32_cosine = (float)cordic_cosine/(float)0x80000000;
+
+if (angle < 0){
+value_f32_sine = wrap_to_1(value_f32_sine);
+value_f32_sine = value_f32_sine * -1;
+}
+
+if (angle > 0){
+value_f32_sine = wrap_to_1(value_f32_sine);
+}
+
+value_f32_cosine = wrap_to_1(value_f32_cosine);
+
+ // Inverse park transform
+ Ualpha = value_f32_cosine * Ud - value_f32_sine * Uq; // -sin(angle) * Uq;
+ Ubeta = value_f32_sine * Ud + value_f32_cosine * Uq; // cos(angle) * Uq;
+
+ // set the voltages in hardware
+ driver->setPwm(Ualpha, Ubeta);
+}
+
+
// Function (iterative) generating open loop movement for target velocity
// - target_velocity - rad/s
// it uses voltage_limit variable
diff --git a/src/StepperMotor.h b/src/StepperMotor.h
index 36f4fe3f..94e002a2 100644
--- a/src/StepperMotor.h
+++ b/src/StepperMotor.h
@@ -89,6 +89,10 @@ class StepperMotor: public FOCMotor
*/
void setPhaseVoltage(float Uq, float Ud, float angle_el) override;
+ // CORDIC
+
+ void setPhaseVoltageCORDIC(float Uq, float Ud, float angle_el) override;
+
private:
/** Sensor alignment to electrical 0 angle of the motor */
diff --git a/src/common/base_classes/FOCMotor.h b/src/common/base_classes/FOCMotor.h
index c499df4a..1b620b32 100644
--- a/src/common/base_classes/FOCMotor.h
+++ b/src/common/base_classes/FOCMotor.h
@@ -136,6 +136,10 @@ class FOCMotor
* @param angle_el current electrical angle of the motor
*/
virtual void setPhaseVoltage(float Uq, float Ud, float angle_el)=0;
+
+ // CORDIC
+
+ virtual void setPhaseVoltageCORDIC(float Uq, float Ud, float angle_el)=0;
// State calculation methods
/** Shaft angle calculation in radians [rad] */
diff --git a/src/common/base_classes/StepperDriver.h b/src/common/base_classes/StepperDriver.h
index 2006fc8c..4dd7203c 100644
--- a/src/common/base_classes/StepperDriver.h
+++ b/src/common/base_classes/StepperDriver.h
@@ -5,28 +5,30 @@
class StepperDriver{
public:
-
- /** Initialise hardware */
- virtual int init() = 0;
- /** Enable hardware */
- virtual void enable() = 0;
- /** Disable hardware */
- virtual void disable() = 0;
-
- long pwm_frequency; //!< pwm frequency value in hertz
- float voltage_power_supply; //!< power supply voltage
- float voltage_limit; //!< limiting voltage set to the motor
-
- bool initialized = false; // true if driver was successfully initialized
- void* params = 0; // pointer to hardware specific parameters of driver
-
- /**
- * Set phase voltages to the harware
- *
- * @param Ua phase A voltage
- * @param Ub phase B voltage
- */
- virtual void setPwm(float Ua, float Ub) = 0;
+ /** Initialise hardware */
+virtual int init() = 0;
+/** Enable hardware */
+virtual void enable() = 0;
+/** Disable hardware */
+virtual void disable() = 0;
+
+long pwm_frequency; //!< pwm frequency value in hertz
+float voltage_power_supply; //!< power supply voltage
+float voltage_limit; //!< limiting voltage set to the motor
+
+bool initialized = false; // true if driver was successfully initialized
+void* params = 0; // pointer to hardware specific parameters of driver
+
+/**
+ * Set phase voltages to the hardware
+ *
+ * @param Ua phase A voltage
+ * @param Ub phase B voltage
+ */
+virtual void setPwm(float Ua, float Ub) = 0;
+
+
+
};
#endif
\ No newline at end of file
diff --git a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_utils.cpp b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_utils.cpp
index 7c254ce9..376d9d68 100644
--- a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_utils.cpp
+++ b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_utils.cpp
@@ -187,7 +187,7 @@ uint32_t _timerToRegularTRGO(HardwareTimer* timer){
#endif
#ifdef TIM8 // if defined timer 8
else if(timer->getHandle()->Instance == TIM8)
- return ADC_EXTERNALTRIG_T7_TRGO;
+ return ADC_EXTERNALTRIG_T8_TRGO;
#endif
#ifdef TIM15 // if defined timer 15
else if(timer->getHandle()->Instance == TIM15)
diff --git a/src/drivers/StepperDriver8PWM.cpp b/src/drivers/StepperDriver8PWM.cpp
new file mode 100644
index 00000000..5094545c
--- /dev/null
+++ b/src/drivers/StepperDriver8PWM.cpp
@@ -0,0 +1,98 @@
+#include "StepperDriver8PWM.h"
+
+StepperDriver8PWM::StepperDriver8PWM(int ph1A, int ph1B, int ph2A, int ph2B, int ph3A, int ph3B, int ph4A, int ph4B, int en1, int en2, int en3, int en4) {
+ // Pin initialization
+ pwm1A = ph1A;
+ pwm1B = ph1B;
+ pwm2A = ph2A;
+ pwm2B = ph2B;
+ pwm3A = ph3A;
+ pwm3B = ph3B;
+ pwm4A = ph4A;
+ pwm4B = ph4B;
+
+ // Enable pins
+ enable_pin1 = en1;
+ enable_pin2 = en2;
+ enable_pin3 = en3;
+ enable_pin4 = en4;
+
+ // Default values
+ voltage_power_supply = DEF_POWER_SUPPLY;
+ voltage_limit = NOT_SET;
+ pwm_frequency = NOT_SET;
+
+
+ pinMode(enable_pin1, OUTPUT);
+ pinMode(enable_pin2, OUTPUT);
+ pinMode(enable_pin3, OUTPUT);
+ pinMode(enable_pin4, OUTPUT);
+ disable();
+
+ // dead zone initial - 2%
+ dead_zone = 0.02f;
+
+}
+
+// init hardware pins for 8PWM control
+int StepperDriver8PWM::init() {
+
+ // PWM pins
+ pinMode(pwm1A, OUTPUT);
+ pinMode(pwm1B, OUTPUT);
+ pinMode(pwm2A, OUTPUT);
+ pinMode(pwm2B, OUTPUT);
+ pinMode(pwm3A, OUTPUT);
+ pinMode(pwm3B, OUTPUT);
+ pinMode(pwm4A, OUTPUT);
+ pinMode(pwm4B, OUTPUT);
+
+ if( _isset(enable_pin1) ) pinMode(enable_pin1, OUTPUT);
+ if( _isset(enable_pin2) ) pinMode(enable_pin2, OUTPUT);
+ if( _isset(enable_pin3) ) pinMode(enable_pin3, OUTPUT);
+ if( _isset(enable_pin4) ) pinMode(enable_pin4, OUTPUT);
+ // sanity check for the voltage limit configuration
+ if( !_isset(voltage_limit) || voltage_limit > voltage_power_supply) voltage_limit = voltage_power_supply;
+
+ // Set the pwm frequency to the pins
+ // hardware specific function - depending on driver and mcu
+ params = _configure8PWM(pwm_frequency, dead_zone, pwm1A, pwm1B, pwm2A, pwm2B, pwm3A, pwm3B, pwm4A, pwm4B);
+ initialized = (params!=SIMPLEFOC_DRIVER_INIT_FAILED);
+ return params!=SIMPLEFOC_DRIVER_INIT_FAILED;
+}
+
+
+// Set voltage to the pwm pin for 8PWM control
+void StepperDriver8PWM::setPwm(float Ualpha, float Ubeta) {
+ float duty_cycle1A_h1(0.0f),duty_cycle1A_h2(0.0f),duty_cycle1B_h1(0.0f),duty_cycle1B_h2(0.0f);
+ float duty_cycle2A_h1(0.0f),duty_cycle2A_h2(0.0f),duty_cycle2B_h1(0.0f),duty_cycle2B_h2(0.0f);
+
+ // limit the voltage in driver
+ Ualpha = _constrain(Ualpha, -voltage_limit, voltage_limit);
+ Ubeta = _constrain(Ubeta, -voltage_limit, voltage_limit);
+
+ // hardware specific writing
+ if( Ualpha > 0 ) {
+ duty_cycle1B_h1 = _constrain(abs(Ualpha)/voltage_power_supply,0.0f,1.0f);
+ duty_cycle1B_h2 = 0.0f; // set second half-bridge duty cycle to 0
+ }
+ else {
+ duty_cycle1A_h1 = _constrain(abs(Ualpha)/voltage_power_supply,0.0f,1.0f);
+ duty_cycle1A_h2 = 0.0f; // set second half-bridge duty cycle to 0
+ }
+
+ if( Ubeta > 0 ) {
+ duty_cycle2B_h1 = _constrain(abs(Ubeta)/voltage_power_supply,0.0f,1.0f);
+ duty_cycle2B_h2 = 0.0f; // set second half-bridge duty cycle to 0
+ }
+ else {
+ duty_cycle2A_h1 = _constrain(abs(Ubeta)/voltage_power_supply,0.0f,1.0f);
+ duty_cycle2A_h2 = 0.0f; // set second half-bridge duty cycle to 0
+ }
+
+ // write to hardware
+ _writeDutyCycle8PWM(duty_cycle1A_h1, duty_cycle1A_h2, duty_cycle1B_h1, duty_cycle1B_h2,
+ duty_cycle2A_h1, duty_cycle2A_h2, duty_cycle2B_h1, duty_cycle2B_h2, params);
+}
+
+
diff --git a/src/drivers/StepperDriver8PWM.h b/src/drivers/StepperDriver8PWM.h
new file mode 100644
index 00000000..484f4d2d
--- /dev/null
+++ b/src/drivers/StepperDriver8PWM.h
@@ -0,0 +1,68 @@
+#ifndef STEPPER_DRIVER_4PWM_h
+#define STEPPER_DRIVER_4PWM_h
+
+#include "../common/base_classes/StepperDriver.h"
+#include "../common/foc_utils.h"
+#include "../common/time_utils.h"
+#include "../common/defaults.h"
+#include "hardware_api.h"
+
+/**
+ 8 pwm stepper driver class
+*/
+class StepperDriver8PWM : public StepperDriver {
+ public:
+ /**
+ StepperMotor class constructor
+ @param ph1A 1A phase pwm pin
+ @param ph1B 1B phase pwm pin
+ @param ph2A 2A phase pwm pin
+ @param ph2B 2B phase pwm pin
+ @param ph3A 3A phase pwm pin
+ @param ph3B 3B phase pwm pin
+ @param ph4A 4A phase pwm pin
+ @param ph4B 4B phase pwm pin
+ @param en1 enable pin phase 1 (optional input)
+ @param en2 enable pin phase 2 (optional input)
+ @param en3 enable pin phase 3 (optional input)
+ @param en4 enable pin phase 4 (optional input)
+ */
+ StepperDriver8PWM(int ph1A, int ph1B, int ph2A, int ph2B, int ph3A, int ph3B, int ph4A, int ph4B, int en1 = NOT_SET, int en2 = NOT_SET, int en3 = NOT_SET, int en4 = NOT_SET);
+
+ /** Motor hardware init function */
+ int init() override;
+ /** Motor disable function */
+ void disable() override;
+ /** Motor enable function */
+ void enable() override;
+
+ float dead_zone; //!< a percentage of dead-time(zone) (both high and low side in low) for each pwm cycle [0,1]
+
+ // hardware variables
+ int pwm1A; //!< phase 1A pwm pin number
+ int pwm1B; //!< phase 1B pwm pin number
+ int pwm2A; //!< phase 2A pwm pin number
+ int pwm2B; //!< phase 2B pwm pin number
+ int pwm3A; //!< phase 3A pwm pin number
+ int pwm3B; //!< phase 3B pwm pin number
+ int pwm4A; //!< phase 4A pwm pin number
+ int pwm4B; //!< phase 4B pwm pin number
+ int enable_pin1; //!< enable pin number phase 1
+ int enable_pin2; //!< enable pin number phase 2
+ int enable_pin3; //!< enable pin number phase 3
+ int enable_pin4; //!< enable pin number phase 4
+
+ /**
+ * Set phase voltages to the harware
+ *
+ * @param Ua phase A voltage
+ * @param Ub phase B voltage
+ */
+ void setPwm(float Ua, float Ub) override;
+ private:
+
+};
+
+
+
+#endif
diff --git a/src/drivers/hardware_api.h b/src/drivers/hardware_api.h
index 8b477458..b9065293 100644
--- a/src/drivers/hardware_api.h
+++ b/src/drivers/hardware_api.h
@@ -36,7 +36,7 @@
// will be returned as a void pointer from the _configurexPWM functions
// will be provided to the _writeDutyCyclexPWM() as a void pointer
typedef struct GenericDriverParams {
- int pins[6];
+ int pins[8];
long pwm_frequency;
float dead_zone;
} GenericDriverParams;
@@ -114,6 +114,27 @@ void* _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const
*/
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);
+/**
+ * Configuring PWM frequency, resolution and alignment
+ * - Stepper driver - 8PWM setting
+ * - hardware specific
+ *
+ * @param pwm_frequency - frequency in hertz - if applicable
+ * @param pin1A pin1A stepper driver
+ * @param pin1B pin1B stepper driver
+ * @param pin2A pin2A stepper driver
+ * @param pin2B pin2B stepper driver
+ * @param pin3A pin3A stepper driver
+ * @param pin3B pin3B stepper driver
+ * @param pin4A pin4A stepper driver
+ * @param pin4B pin4B stepper driver
+ *
+ * @return -1 if failed, or pointer to internal driver parameters struct if successful
+ */
+void* _configure8PWM(long pwm_frequency, float dead_zone, const int pin1A, const int pin1B, const int pin2A, const int pin2B,
+ const int pin3A, const int pin3B, const int pin4A, const int pin4B);
+
+
/**
* Function setting the duty cycle to the pwm pin (ex. analogWrite())
* - Stepper driver - 2PWM setting
@@ -177,4 +198,23 @@ void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, vo
void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState *phase_state, void* params);
+/**
+
+*Function setting the duty cycle to the pwm pin (ex. analogWrite())
+* - Stepper driver - 8PWM setting
+* - hardware specific
+*@param dc_1a duty cycle phase 1A [0, 1]
+*@param dc_1b duty cycle phase 1B [0, 1]
+*@param dc_1c duty cycle phase 1C [0, 1]
+*@param dc_1d duty cycle phase 1D [0, 1]
+*@param dc_2a duty cycle phase 2A [0, 1]
+*@param dc_2b duty cycle phase 2B [0, 1]
+*@param dc_2c duty cycle phase 2C [0, 1]
+*@param dc_2d duty cycle phase 2D [0, 1]
+*@param params the driver parameters
+*/
+
+void _writeDutyCycle8PWM(float dc_1a, float dc_1b, float dc_1c, float dc_1d, float dc_2a, float dc_2b, float dc_2c, float dc_2d, void* params);
+
+
#endif
\ No newline at end of file
diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp
index d16808f6..dc39f3cf 100644
--- a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp
+++ b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp
@@ -184,6 +184,36 @@ void _alignPWMTimers(HardwareTimer *HT1, HardwareTimer *HT2, HardwareTimer *HT3,
HT4->resume();
}
+// align the timers to end the init
+void _alignPWMTimers(HardwareTimer *HT1, HardwareTimer *HT2, HardwareTimer *HT3, HardwareTimer *HT4, HardwareTimer *HT5, HardwareTimer *HT6, HardwareTimer *HT7, HardwareTimer *HT8)
+{
+ HT1->pause();
+ HT1->refresh();
+ HT2->pause();
+ HT2->refresh();
+ HT3->pause();
+ HT3->refresh();
+ HT4->pause();
+ HT4->refresh();
+ HT5->pause();
+ HT5->refresh();
+ HT6->pause();
+ HT6->refresh();
+ HT7->pause();
+ HT7->refresh();
+ HT8->pause();
+ HT8->refresh();
+ HT1->resume();
+ HT2->resume();
+ HT3->resume();
+ HT4->resume();
+ HT5->resume();
+ HT6->resume();
+ HT7->resume();
+ HT8->resume();
+}
+
+
// align the timers to end the init
void _stopTimers(HardwareTimer **timers_to_stop, int timer_num)
@@ -251,7 +281,6 @@ void _alignTimersNew() {
-
// 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) {
// sanity check
@@ -310,6 +339,9 @@ STM32DriverParams* _initHardware6PWMPair(long PWM_freq, float dead_zone, PinMap*
+
+
+
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* params = new STM32DriverParams {
.timers = { NP, NP, NP, NP, NP, NP },
@@ -330,6 +362,26 @@ STM32DriverParams* _initHardware6PWMInterface(long PWM_freq, float dead_zone, Pi
}
+STM32DriverParams* _initHardware8PWMInterface(long PWM_freq, float dead_zone, PinMap* pin1A, PinMap* pin1B, PinMap* pin2A, PinMap* pin2B, PinMap* pin3A, PinMap* pin3B, PinMap* pin4A, PinMap* pin4B) {
+ STM32DriverParams* params = new STM32DriverParams {
+ .timers = { NP, NP, NP, NP, NP, NP, NP, NP },
+ .channels = { 0, 0, 0, 0, 0, 0, 0, 0 },
+ .pwm_frequency = PWM_freq,
+ .dead_zone = dead_zone,
+ .interface_type = _HARDWARE_8PWM
+ };
+
+ if (_initHardware6PWMPair(PWM_freq, dead_zone, pin1A, pin1B, params, 0) == SIMPLEFOC_DRIVER_INIT_FAILED)
+ return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED;
+ if (_initHardware6PWMPair(PWM_freq, dead_zone, pin2A, pin2B, params, 2) == SIMPLEFOC_DRIVER_INIT_FAILED)
+ return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED;
+ if (_initHardware6PWMPair(PWM_freq, dead_zone, pin3A, pin3B, params, 4) == SIMPLEFOC_DRIVER_INIT_FAILED)
+ return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED;
+ if (_initHardware6PWMPair(PWM_freq, dead_zone, pin4A, pin4B, params, 6) == SIMPLEFOC_DRIVER_INIT_FAILED)
+ return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED;
+
+ return params;
+}
@@ -621,6 +673,8 @@ void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const in
.channels = { channel1, channel2, channel3 },
.pwm_frequency = pwm_frequency
};
+
+
timerPinsUsed[numTimerPinsUsed++] = pinTimers[0];
timerPinsUsed[numTimerPinsUsed++] = pinTimers[1];
timerPinsUsed[numTimerPinsUsed++] = pinTimers[2];
@@ -632,46 +686,66 @@ void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const in
+
+
// function setting the high pwm frequency to the supplied pins
-// - Stepper motor - 4PWM setting
-// - hardware speciffic
-void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) {
- if (numTimerPinsUsed+4 > SIMPLEFOC_STM32_MAX_PINTIMERSUSED) {
+// - Stepper motor - 8PWM setting
+// - hardware specific
+void* _configure8PWM(long pwm_frequency, float dead_zone, const int pin1A, const int pin1B, const int pin2A, const int pin2B,
+ const int pin3A, const int pin3B, const int pin4A, const int pin4B)
+{
+ if (numTimerPinsUsed+8 > SIMPLEFOC_STM32_MAX_PINTIMERSUSED) {
SIMPLEFOC_DEBUG("STM32-DRV: ERR: too many pins 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
+ else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to |%0kHz max
// center-aligned frequency is uses two periods
pwm_frequency *=2;
- int pins[4] = { pinA, pinB, pinC, pinD };
- PinMap* pinTimers[4] = { NP, NP, NP, NP };
- if (findBestTimerCombination(4, pins, pinTimers)<0)
- return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED;
-
- HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinTimers[0]);
- HardwareTimer* HT2 = _initPinPWM(pwm_frequency, pinTimers[1]);
- HardwareTimer* HT3 = _initPinPWM(pwm_frequency, pinTimers[2]);
- HardwareTimer* HT4 = _initPinPWM(pwm_frequency, pinTimers[3]);
- // allign the timers
- _alignPWMTimers(HT1, HT2, HT3, HT4);
-
- 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);
- uint32_t channel4 = STM_PIN_CHANNEL(pinTimers[3]->function);
+ // find configuration
+ int pins[8] = { pin1A, pin1B, pin2A, pin2B, pin3A, pin3B, pin4A, pin4B };
+ PinMap* pinTimers[8] = { NP, NP, NP, NP, NP, NP, NP, NP };
+ int score = findBestTimerCombination(8, pins, pinTimers);
- STM32DriverParams* params = new STM32DriverParams {
- .timers = { HT1, HT2, HT3, HT4 },
- .channels = { channel1, channel2, channel3, channel4 },
- .pwm_frequency = pwm_frequency
- };
- timerPinsUsed[numTimerPinsUsed++] = pinTimers[0];
- timerPinsUsed[numTimerPinsUsed++] = pinTimers[1];
- timerPinsUsed[numTimerPinsUsed++] = pinTimers[2];
- timerPinsUsed[numTimerPinsUsed++] = pinTimers[3];
- return params;
+ STM32DriverParams* params;
+ // configure accordingly
+ if (score<0)
+ params = (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED;
+ else if (score<10) // hardware pwm
+ params = _initHardware8PWMInterface(pwm_frequency, dead_zone, pinTimers[0], pinTimers[1], pinTimers[2], pinTimers[3], pinTimers[4], pinTimers[5], pinTimers[6], pinTimers[7]);
+ 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* HT7 = _initPinPWMHigh(pwm_frequency, pinTimers[6]);
+ HardwareTimer* HT8 = _initPinPWMLow(pwm_frequency, pinTimers[7]);
+ 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);
+ uint32_t channel4 = STM_PIN_CHANNEL(pinTimers[3]->function);
+ uint32_t channel5 = STM_PIN_CHANNEL(pinTimers[4]->function);
+ uint32_t channel6 = STM_PIN_CHANNEL(pinTimers[5]->function);
+ uint32_t channel7 = STM_PIN_CHANNEL(pinTimers[6]->function);
+ uint32_t channel8 = STM_PIN_CHANNEL(pinTimers[7]->function);
+ params = new STM32DriverParams {
+
+ .timers = { HT1, HT2, HT3, HT4, HT5, HT6, HT7, HT8},
+ .channels = { channel1, channel2, channel3, channel4, channel5, channel6, channel7, channel8 },
+ .pwm_frequency = pwm_frequency,
+ .dead_zone = dead_zone,
+ .interface_type = _SOFTWARE_8PWM
+ };
+ }
+ if (score>=0) {
+ for (int i=0; i<8; i++)
+ timerPinsUsed[numTimerPinsUsed++] = pinTimers[i];
+ _alignTimersNew();
+ }
+ return params; // success
}
@@ -717,7 +791,20 @@ void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, vo
_setPwm(((STM32DriverParams*)params)->timers[3], ((STM32DriverParams*)params)->channels[3], _PWM_RANGE*dc_2b, _PWM_RESOLUTION);
}
-
+// function setting the pwm duty cycle to the hardware
+// - Stepper motor - 8PWM setting
+// - hardware specific
+void _writeDutyCycle8PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, float dc_3a, float dc_3b, float dc_4a, float dc_4b, 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);
+ _setPwm(((STM32DriverParams*)params)->timers[4], ((STM32DriverParams*)params)->channels[4], _PWM_RANGE * dc_3a, _PWM_RESOLUTION);
+ _setPwm(((STM32DriverParams*)params)->timers[5], ((STM32DriverParams*)params)->channels[5], _PWM_RANGE * dc_3b, _PWM_RESOLUTION);
+ _setPwm(((STM32DriverParams*)params)->timers[6], ((STM32DriverParams*)params)->channels[6], _PWM_RANGE * dc_4a, _PWM_RESOLUTION);
+ _setPwm(((STM32DriverParams*)params)->timers[7], ((STM32DriverParams*)params)->channels[7], _PWM_RANGE * dc_4b, _PWM_RESOLUTION);
+}
// Configuring PWM frequency, resolution and alignment
diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.h b/src/drivers/hardware_specific/stm32/stm32_mcu.h
index fa6280e9..d3d9e1ff 100644
--- a/src/drivers/hardware_specific/stm32/stm32_mcu.h
+++ b/src/drivers/hardware_specific/stm32/stm32_mcu.h
@@ -15,10 +15,15 @@
#define _SOFTWARE_6PWM 0
#define _ERROR_6PWM -1
+// 8pwm parameters
+#define _HARDWARE_8PWM 1
+#define _SOFTWARE_8PWM 0
+#define _ERROR_8PWM -1
+
typedef struct STM32DriverParams {
- HardwareTimer* timers[6] = {NULL};
- uint32_t channels[6];
+ HardwareTimer* timers[8] = {NULL};
+ uint32_t channels[8];
long pwm_frequency;
float dead_zone;
uint8_t interface_type;