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 <[email protected]>

Author: uLipe

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

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

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);

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);

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);
 }