From 52a9629dbd6826759e544439369733bec650f11b Mon Sep 17 00:00:00 2001 From: Felipe Neves Date: Thu, 23 Oct 2025 12:19:35 -0300 Subject: [PATCH] FOCDriver: Replace divisions in the hot PWM code paths with reciprocal multiplications To reduce time of execution of the SetPWM member function. Signed-off-by: Felipe Neves --- src/common/base_classes/FOCDriver.h | 1 + src/drivers/BLDCDriver3PWM.cpp | 10 ++++++---- src/drivers/BLDCDriver6PWM.cpp | 9 ++++++--- src/drivers/StepperDriver2PWM.cpp | 7 +++++-- src/drivers/StepperDriver4PWM.cpp | 13 ++++++++----- 5 files changed, 26 insertions(+), 14 deletions(-) diff --git a/src/common/base_classes/FOCDriver.h b/src/common/base_classes/FOCDriver.h index 263460b3..e8d3d714 100644 --- a/src/common/base_classes/FOCDriver.h +++ b/src/common/base_classes/FOCDriver.h @@ -35,6 +35,7 @@ class FOCDriver{ long pwm_frequency; //!< pwm frequency value in hertz float voltage_power_supply; //!< power supply voltage float voltage_limit; //!< limiting voltage set to the motor + float inverse_power_supply; //!< inverse of the power supply, very useful to replace divisions with reciprocal multiply which is faster bool initialized = false; //!< true if driver was successfully initialized void* params = 0; //!< pointer to hardware specific parameters of driver diff --git a/src/drivers/BLDCDriver3PWM.cpp b/src/drivers/BLDCDriver3PWM.cpp index 637c8db5..c70a2c55 100644 --- a/src/drivers/BLDCDriver3PWM.cpp +++ b/src/drivers/BLDCDriver3PWM.cpp @@ -15,7 +15,7 @@ BLDCDriver3PWM::BLDCDriver3PWM(int phA, int phB, int phC, int en1, int en2, int voltage_power_supply = DEF_POWER_SUPPLY; voltage_limit = NOT_SET; pwm_frequency = NOT_SET; - + inverse_power_supply = 1.0f / voltage_power_supply; } // enable motor driver @@ -53,6 +53,8 @@ int BLDCDriver3PWM::init() { // sanity check for the voltage limit configuration if(!_isset(voltage_limit) || voltage_limit > voltage_power_supply) voltage_limit = voltage_power_supply; + inverse_power_supply = 1.0f / voltage_power_supply; + // Set the pwm frequency to the pins // hardware specific function - depending on driver and mcu @@ -82,9 +84,9 @@ void BLDCDriver3PWM::setPwm(float Ua, float Ub, float Uc) { Uc = _constrain(Uc, 0.0f, voltage_limit); // calculate duty cycle // limited in [0,1] - dc_a = _constrain(Ua / voltage_power_supply, 0.0f , 1.0f ); - dc_b = _constrain(Ub / voltage_power_supply, 0.0f , 1.0f ); - dc_c = _constrain(Uc / voltage_power_supply, 0.0f , 1.0f ); + dc_a = _constrain(Ua * inverse_power_supply, 0.0f , 1.0f ); + dc_b = _constrain(Ub * inverse_power_supply, 0.0f , 1.0f ); + dc_c = _constrain(Uc * inverse_power_supply, 0.0f , 1.0f ); // hardware specific writing // hardware specific function - depending on driver and mcu diff --git a/src/drivers/BLDCDriver6PWM.cpp b/src/drivers/BLDCDriver6PWM.cpp index 4981858f..8bf582d5 100644 --- a/src/drivers/BLDCDriver6PWM.cpp +++ b/src/drivers/BLDCDriver6PWM.cpp @@ -16,6 +16,7 @@ BLDCDriver6PWM::BLDCDriver6PWM(int phA_h,int phA_l,int phB_h,int phB_l,int phC_h voltage_power_supply = DEF_POWER_SUPPLY; voltage_limit = NOT_SET; pwm_frequency = NOT_SET; + inverse_power_supply = 1.0f / voltage_power_supply; // dead zone initial - 2% dead_zone = 0.02f; @@ -60,6 +61,8 @@ int BLDCDriver6PWM::init() { // sanity check for the voltage limit configuration if( !_isset(voltage_limit) || voltage_limit > voltage_power_supply) voltage_limit = voltage_power_supply; + inverse_power_supply = 1.0f / voltage_power_supply; + // set phase state to disabled phase_state[0] = PhaseState::PHASE_OFF; phase_state[1] = PhaseState::PHASE_OFF; @@ -84,9 +87,9 @@ void BLDCDriver6PWM::setPwm(float Ua, float Ub, float Uc) { Uc = _constrain(Uc, 0, voltage_limit); // calculate duty cycle // limited in [0,1] - dc_a = _constrain(Ua / voltage_power_supply, 0.0f , 1.0f ); - dc_b = _constrain(Ub / voltage_power_supply, 0.0f , 1.0f ); - dc_c = _constrain(Uc / voltage_power_supply, 0.0f , 1.0f ); + dc_a = _constrain(Ua * inverse_power_supply, 0.0f , 1.0f ); + dc_b = _constrain(Ub * inverse_power_supply, 0.0f , 1.0f ); + dc_c = _constrain(Uc * inverse_power_supply, 0.0f , 1.0f ); // hardware specific writing // hardware specific function - depending on driver and mcu _writeDutyCycle6PWM(dc_a, dc_b, dc_c, phase_state, params); diff --git a/src/drivers/StepperDriver2PWM.cpp b/src/drivers/StepperDriver2PWM.cpp index e8ccc6c6..ebb3aa4d 100644 --- a/src/drivers/StepperDriver2PWM.cpp +++ b/src/drivers/StepperDriver2PWM.cpp @@ -17,6 +17,7 @@ StepperDriver2PWM::StepperDriver2PWM(int _pwm1, int* _in1, int _pwm2, int* _in2, voltage_power_supply = DEF_POWER_SUPPLY; voltage_limit = NOT_SET; pwm_frequency = NOT_SET; + inverse_power_supply = 1.0f / voltage_power_supply; } @@ -77,6 +78,8 @@ int StepperDriver2PWM::init() { // sanity check for the voltage limit configuration if( !_isset(voltage_limit) || voltage_limit > voltage_power_supply) voltage_limit = voltage_power_supply; + inverse_power_supply = 1.0f / voltage_power_supply; + // Set the pwm frequency to the pins // hardware specific function - depending on driver and mcu params = _configure2PWM(pwm_frequency, pwm1, pwm2); @@ -100,8 +103,8 @@ void StepperDriver2PWM::setPwm(float Ua, float Ub) { Ua = _constrain(Ua, -voltage_limit, voltage_limit); Ub = _constrain(Ub, -voltage_limit, voltage_limit); // hardware specific writing - duty_cycle1 = _constrain(abs(Ua)/voltage_power_supply,0.0f,1.0f); - duty_cycle2 = _constrain(abs(Ub)/voltage_power_supply,0.0f,1.0f); + duty_cycle1 = _constrain(abs(Ua)* inverse_power_supply,0.0f,1.0f); + duty_cycle2 = _constrain(abs(Ub)* inverse_power_supply,0.0f,1.0f); // phase 1 direction digitalWrite(dir1a, Ua >= 0 ? LOW : HIGH); diff --git a/src/drivers/StepperDriver4PWM.cpp b/src/drivers/StepperDriver4PWM.cpp index 52f1c1d1..e7af8f6d 100644 --- a/src/drivers/StepperDriver4PWM.cpp +++ b/src/drivers/StepperDriver4PWM.cpp @@ -15,6 +15,7 @@ StepperDriver4PWM::StepperDriver4PWM(int ph1A,int ph1B,int ph2A,int ph2B,int en1 voltage_power_supply = DEF_POWER_SUPPLY; voltage_limit = NOT_SET; pwm_frequency = NOT_SET; + inverse_power_supply = 1.0f / voltage_power_supply; } @@ -52,10 +53,12 @@ int StepperDriver4PWM::init() { // sanity check for the voltage limit configuration if( !_isset(voltage_limit) || voltage_limit > voltage_power_supply) voltage_limit = voltage_power_supply; + inverse_power_supply = 1.0f / voltage_power_supply; + // Set the pwm frequency to the pins // hardware specific function - depending on driver and mcu params = _configure4PWM(pwm_frequency, pwm1A, pwm1B, pwm2A, pwm2B); - initialized = (params!=SIMPLEFOC_DRIVER_INIT_FAILED); + initialized = (params!=SIMPLEFOC_DRIVER_INIT_FAILED); return params!=SIMPLEFOC_DRIVER_INIT_FAILED; } @@ -77,14 +80,14 @@ void StepperDriver4PWM::setPwm(float Ualpha, float Ubeta) { Ubeta = _constrain(Ubeta, -voltage_limit, voltage_limit); // hardware specific writing if( Ualpha > 0 ) - duty_cycle1B = _constrain(abs(Ualpha)/voltage_power_supply,0.0f,1.0f); + duty_cycle1B = _constrain(abs(Ualpha) * inverse_power_supply,0.0f,1.0f); else - duty_cycle1A = _constrain(abs(Ualpha)/voltage_power_supply,0.0f,1.0f); + duty_cycle1A = _constrain(abs(Ualpha) * inverse_power_supply,0.0f,1.0f); if( Ubeta > 0 ) - duty_cycle2B = _constrain(abs(Ubeta)/voltage_power_supply,0.0f,1.0f); + duty_cycle2B = _constrain(abs(Ubeta) * inverse_power_supply,0.0f,1.0f); else - duty_cycle2A = _constrain(abs(Ubeta)/voltage_power_supply,0.0f,1.0f); + duty_cycle2A = _constrain(abs(Ubeta) * inverse_power_supply,0.0f,1.0f); // write to hardware _writeDutyCycle4PWM(duty_cycle1A, duty_cycle1B, duty_cycle2A, duty_cycle2B, params); } \ No newline at end of file