Merge pull request #153 from runger1101001/driver_refactor

Driver refactor

Author: askuric

src/common/base_classes/BLDCDriver.h

@@ -23,6 +23,7 @@ class BLDCDriver{
         float dc_c; //!< currently set duty cycle on phaseC
 
         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

src/common/base_classes/StepperDriver.h

@@ -1,6 +1,8 @@
 #ifndef STEPPERDRIVER_H
 #define STEPPERDRIVER_H
 
+#include "drivers/hardware_api.h"
+
 class StepperDriver{
     public:
 
@@ -16,7 +18,8 @@ class StepperDriver{
         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 
          * 

src/current_sense/hardware_specific/stm32g4_mcu.cpp

@@ -1,5 +1,6 @@
 #include "../hardware_api.h"
 #include "stm32g4_hal.h"
+#include "Arduino.h"
 
 #if defined(STM32G4xx) 
 #define _ADC_VOLTAGE 3.3
@@ -29,9 +30,10 @@ float _readADCVoltageInline(const int pin){
     raw_adc = adcBuffer1[1];
   else if(pin == PA6) // = ADC2_IN3 = phase V (OP2_OUT) on B-G431B-ESC1
     raw_adc = adcBuffer2[0];
+#ifdef PB1
   else if(pin == PB1) // = ADC1_IN12 = phase W (OP3_OUT) on B-G431B-ESC1
     raw_adc = adcBuffer1[0];
-
+#endif
   return raw_adc * _ADC_CONV;
 }
 // do the same for low side sensing

src/drivers/BLDCDriver3PWM.cpp

@@ -56,9 +56,8 @@ int BLDCDriver3PWM::init() {
 
   // Set the pwm frequency to the pins
   // hardware specific function - depending on driver and mcu
-  _configure3PWM(pwm_frequency, pwmA, pwmB, pwmC);
-  initialized = true; // TODO atm the api gives no feedback if initialization succeeded
-  return 0;
+  params = _configure3PWM(pwm_frequency, pwmA, pwmB, pwmC);
+  return params!=SIMPLEFOC_DRIVER_INIT_FAILED;
 }
 
 
@@ -88,5 +87,5 @@ void BLDCDriver3PWM::setPwm(float Ua, float Ub, float Uc) {
 
   // hardware specific writing
   // hardware specific function - depending on driver and mcu
-  _writeDutyCycle3PWM(dc_a, dc_b, dc_c, pwmA, pwmB, pwmC);
+  _writeDutyCycle3PWM(dc_a, dc_b, dc_c, params);
 }

src/drivers/BLDCDriver3PWM.h

@@ -58,7 +58,6 @@ class BLDCDriver3PWM: public BLDCDriver
     */
     virtual void setPhaseState(int sa, int sb, int sc) override;
   private:
-        
 };
 
 

src/drivers/BLDCDriver6PWM.cpp

@@ -58,11 +58,11 @@ int BLDCDriver6PWM::init() {
 
   // configure 6pwm
   // hardware specific function - depending on driver and mcu
-  int result = _configure6PWM(pwm_frequency, dead_zone, pwmA_h,pwmA_l, pwmB_h,pwmB_l, pwmC_h,pwmC_l);
-  initialized = (result==0);
-  return result;
+  params = _configure6PWM(pwm_frequency, dead_zone, pwmA_h,pwmA_l, pwmB_h,pwmB_l, pwmC_h,pwmC_l);
+  return params!=SIMPLEFOC_DRIVER_INIT_FAILED;
 }
 
+
 // Set voltage to the pwm pin
 void BLDCDriver6PWM::setPwm(float Ua, float Ub, float Uc) {
   // limit the voltage in driver
@@ -76,7 +76,7 @@ void BLDCDriver6PWM::setPwm(float Ua, float Ub, float Uc) {
   dc_c = _constrain(Uc / voltage_power_supply, 0.0f , 1.0f );
   // hardware specific writing
   // hardware specific function - depending on driver and mcu
-  _writeDutyCycle6PWM(dc_a, dc_b, dc_c, dead_zone, pwmA_h,pwmA_l, pwmB_h,pwmB_l, pwmC_h,pwmC_l);
+  _writeDutyCycle6PWM(dc_a, dc_b, dc_c, params);
 }
 
 

src/drivers/StepperDriver2PWM.cpp

@@ -79,9 +79,8 @@ int StepperDriver2PWM::init() {
 
   // Set the pwm frequency to the pins
   // hardware specific function - depending on driver and mcu
-  _configure2PWM(pwm_frequency, pwm1, pwm2);
-  initialized = true; // TODO atm the api gives no feedback if initialization succeeded
-  return 0;
+  params = _configure2PWM(pwm_frequency, pwm1, pwm2);
+  return params!=SIMPLEFOC_DRIVER_INIT_FAILED;
 }
 
 
@@ -103,5 +102,5 @@ void StepperDriver2PWM::setPwm(float Ua, float Ub) {
   if( _isset(dir2b) ) digitalWrite(dir2b, Ub >= 0 ? HIGH : LOW );
 
   // write to hardware
-  _writeDutyCycle2PWM(duty_cycle1, duty_cycle2, pwm1, pwm2);
+  _writeDutyCycle2PWM(duty_cycle1, duty_cycle2, params);
 }

src/drivers/StepperDriver4PWM.cpp

@@ -54,9 +54,8 @@ int StepperDriver4PWM::init() {
 
   // Set the pwm frequency to the pins
   // hardware specific function - depending on driver and mcu
-  _configure4PWM(pwm_frequency, pwm1A, pwm1B, pwm2A, pwm2B);
-  initialized = true; // TODO atm the api gives no feedback if initialization succeeded
-  return 0;
+  params = _configure4PWM(pwm_frequency, pwm1A, pwm1B, pwm2A, pwm2B);
+  return params!=SIMPLEFOC_DRIVER_INIT_FAILED;
 }
 
 
@@ -77,5 +76,5 @@ void StepperDriver4PWM::setPwm(float Ualpha, float Ubeta) {
   else
     duty_cycle2A = _constrain(abs(Ubeta)/voltage_power_supply,0.0f,1.0f);
   // write to hardware
-  _writeDutyCycle4PWM(duty_cycle1A, duty_cycle1B, duty_cycle2A, duty_cycle2B, pwm1A, pwm1B, pwm2A, pwm2B);
+  _writeDutyCycle4PWM(duty_cycle1A, duty_cycle1B, duty_cycle2A, duty_cycle2B, params);
 }

src/drivers/hardware_api.h

@@ -4,6 +4,15 @@
 #include "../common/foc_utils.h"
 #include "../common/time_utils.h"
 
+#define SIMPLEFOC_DRIVER_INIT_FAILED ((void*)-1)
+
+typedef struct GenericDriverParams {
+  int pins[6];
+  long pwm_frequency;
+  float dead_zone;
+} GenericDriverParams;
+
+
 /** 
  * Configuring PWM frequency, resolution and alignment
  * - Stepper driver - 2PWM setting
@@ -12,8 +21,10 @@
  * @param pwm_frequency - frequency in hertz - if applicable
  * @param pinA pinA bldc driver
  * @param pinB pinB bldc driver
+ * 
+ * @return -1 if failed, or pointer to internal driver parameters struct if successful
  */
-void _configure2PWM(long pwm_frequency, const int pinA, const int pinB);
+void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB);
 
 /** 
  * Configuring PWM frequency, resolution and alignment
@@ -24,8 +35,10 @@ void _configure2PWM(long pwm_frequency, const int pinA, const int pinB);
  * @param pinA pinA bldc driver
  * @param pinB pinB bldc driver
  * @param pinC pinC bldc driver
+ * 
+ * @return -1 if failed, or pointer to internal driver parameters struct if successful
  */
-void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const int pinC);
+void* _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const int pinC);
 
 /** 
  * Configuring PWM frequency, resolution and alignment
@@ -37,8 +50,10 @@ void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const in
  * @param pin1B pin1B stepper driver
  * @param pin2A pin2A stepper driver
  * @param pin2B pin2B stepper driver
+ * 
+ * @return -1 if failed, or pointer to internal driver parameters struct if successful
  */
-void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const int pin2A, const int pin2B);
+void* _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const int pin2A, const int pin2B);
 
 /** 
  * Configuring PWM frequency, resolution and alignment
@@ -54,9 +69,9 @@ void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const
  * @param pinC_h pinA high-side bldc driver 
  * @param pinC_l pinA low-side bldc driver 
  * 
- * @return 0 if config good, -1 if failed
+ * @return -1 if failed, or pointer to internal driver parameters struct if successful
  */
-int _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);
+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);
 
 /** 
  * Function setting the duty cycle to the pwm pin (ex. analogWrite())
@@ -65,10 +80,9 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const
  * 
  * @param dc_a  duty cycle phase A [0, 1]
  * @param dc_b  duty cycle phase B [0, 1]
- * @param pinA  phase A hardware pin number
- * @param pinB  phase B hardware pin number
+ * @param params  the driver parameters
  */ 
-void _writeDutyCycle2PWM(float dc_a,  float dc_b, int pinA, int pinB);
+void _writeDutyCycle2PWM(float dc_a,  float dc_b, void* params);
 
 /** 
  * Function setting the duty cycle to the pwm pin (ex. analogWrite())
@@ -78,11 +92,9 @@ void _writeDutyCycle2PWM(float dc_a,  float dc_b, int pinA, int pinB);
  * @param dc_a  duty cycle phase A [0, 1]
  * @param dc_b  duty cycle phase B [0, 1]
  * @param dc_c  duty cycle phase C [0, 1]
- * @param pinA  phase A hardware pin number
- * @param pinB  phase B hardware pin number
- * @param pinC  phase C hardware pin number
+ * @param params  the driver parameters
  */ 
-void _writeDutyCycle3PWM(float dc_a,  float dc_b, float dc_c, int pinA, int pinB, int pinC);
+void _writeDutyCycle3PWM(float dc_a,  float dc_b, float dc_c, void* params);
 
 /** 
  * Function setting the duty cycle to the pwm pin (ex. analogWrite())
@@ -93,12 +105,9 @@ void _writeDutyCycle3PWM(float dc_a,  float dc_b, float dc_c, int pinA, int pinB
  * @param dc_1b  duty cycle phase 1B [0, 1]
  * @param dc_2a  duty cycle phase 2A [0, 1]
  * @param dc_2b  duty cycle phase 2B [0, 1]
- * @param pin1A  phase 1A hardware pin number
- * @param pin1B  phase 1B hardware pin number
- * @param pin2A  phase 2A hardware pin number
- * @param pin2B  phase 2B hardware pin number
+ * @param params  the driver parameters
  */ 
-void _writeDutyCycle4PWM(float dc_1a,  float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B);
+void _writeDutyCycle4PWM(float dc_1a,  float dc_1b, float dc_2a, float dc_2b, void* params);
 
 
 /** 
@@ -109,15 +118,9 @@ void _writeDutyCycle4PWM(float dc_1a,  float dc_1b, float dc_2a, float dc_2b, in
  * @param dc_a  duty cycle phase A [0, 1]
  * @param dc_b  duty cycle phase B [0, 1]
  * @param dc_c  duty cycle phase C [0, 1]
- * @param dead_zone  duty cycle protection zone [0, 1] - both low and high side low
- * @param pinA_h  phase A high-side hardware pin number
- * @param pinA_l  phase A low-side hardware pin number
- * @param pinB_h  phase B high-side hardware pin number
- * @param pinB_l  phase B low-side hardware pin number
- * @param pinC_h  phase C high-side hardware pin number
- * @param pinC_l  phase C low-side hardware pin number
+ * @param params  the driver parameters
  * 
  */ 
-void _writeDutyCycle6PWM(float dc_a,  float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l);
+void _writeDutyCycle6PWM(float dc_a,  float dc_b, float dc_c, void* params);
 
 #endif

src/drivers/hardware_specific/atmega2560_mcu.cpp

@@ -28,64 +28,79 @@ void _pinHighFrequency(const int pin){
 // function setting the high pwm frequency to the supplied pins
 // - Stepper motor - 2PWM setting
 // - hardware speciffic
-void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) {
+void* _configure2PWM(long pwm_frequency,const int pinA, const int pinB) {
    //  High PWM frequency
    // - always max 32kHz
   _pinHighFrequency(pinA);
   _pinHighFrequency(pinB);
+  GenericDriverParams* params = new GenericDriverParams {
+    .pins = { pinA, pinB },
+    .pwm_frequency = pwm_frequency
+  };
+  return params;
 }
 
 // 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) {
+void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) {
    //  High PWM frequency
    // - always max 32kHz
   _pinHighFrequency(pinA);
   _pinHighFrequency(pinB);
   _pinHighFrequency(pinC);
+  GenericDriverParams* params = new GenericDriverParams {
+    .pins = { pinA, pinB, pinC },
+    .pwm_frequency = pwm_frequency
+  };
+  return params;
 }
 
 // function setting the pwm duty cycle to the hardware
 // - Stepper motor - 2PWM setting
 // - hardware speciffic
-void _writeDutyCycle2PWM(float dc_a,  float dc_b, int pinA, int pinB){
+void _writeDutyCycle2PWM(float dc_a,  float dc_b, void* params){
   // transform duty cycle from [0,1] to [0,255]
-  analogWrite(pinA, 255.0f*dc_a);
-  analogWrite(pinB, 255.0f*dc_b);
+  analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a);
+  analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_b);
 }
 
 // function setting the pwm duty cycle to the hardware
 // - BLDC motor - 3PWM setting
 // - hardware speciffic
-void _writeDutyCycle3PWM(float dc_a,  float dc_b, float dc_c, int pinA, int pinB, int pinC){
+void _writeDutyCycle3PWM(float dc_a,  float dc_b, float dc_c, void* params){
   // transform duty cycle from [0,1] to [0,255]
-  analogWrite(pinA, 255.0f*dc_a);
-  analogWrite(pinB, 255.0f*dc_b);
-  analogWrite(pinC, 255.0f*dc_c);
+  analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a);
+  analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_b);
+  analogWrite(((GenericDriverParams*)params)->pins[2], 255.0f*dc_c);
 }
 
 // function setting the high pwm frequency to the supplied pins
 // - Stepper motor - 4PWM setting
 // - hardware speciffic
-void _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const int pin2A, const int pin2B) {
+void* _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const int pin2A, const int pin2B) {
    //  High PWM frequency
    // - always max 32kHz
   _pinHighFrequency(pin1A);
   _pinHighFrequency(pin1B);
   _pinHighFrequency(pin2A);
   _pinHighFrequency(pin2B);
+  GenericDriverParams* params = new GenericDriverParams {
+    .pins = { pin1A, pin2A, pin2A, pin2B },
+    .pwm_frequency = pwm_frequency
+  };
+  return params;
 }
 
 // function setting the pwm duty cycle to the hardware
 // - Stepper motor - 4PWM setting
 // - hardware speciffic
-void _writeDutyCycle4PWM(float dc_1a,  float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){
+void _writeDutyCycle4PWM(float dc_1a,  float dc_1b, float dc_2a, float dc_2b, void* params) {
   // transform duty cycle from [0,1] to [0,255]
-  analogWrite(pin1A, 255.0f*dc_1a);
-  analogWrite(pin1B, 255.0f*dc_1b);
-  analogWrite(pin2A, 255.0f*dc_2a);
-  analogWrite(pin2B, 255.0f*dc_2b);
+  analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_1a);
+  analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_1b);
+  analogWrite(((GenericDriverParams*)params)->pins[2], 255.0f*dc_2a);
+  analogWrite(((GenericDriverParams*)params)->pins[3], 255.0f*dc_2b);
 }
 
 
@@ -122,14 +137,20 @@ int _configureComplementaryPair(int pinH, int pinL) {
 // - BLDC driver - 6PWM setting
 // - hardware specific
 // supports Arudino/ATmega328
-int _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) {
+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) {
   //  High PWM frequency
   // - always max 32kHz
   int ret_flag = 0;
   ret_flag += _configureComplementaryPair(pinA_h, pinA_l);
   ret_flag += _configureComplementaryPair(pinB_h, pinB_l);
   ret_flag += _configureComplementaryPair(pinC_h, pinC_l);
-  return ret_flag; // returns -1 if not well configured
+  if (ret_flag!=0) return SIMPLEFOC_DRIVER_INIT_FAILED;
+  GenericDriverParams* params = new GenericDriverParams {
+    .pins = { pinA_h,, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l },
+    .pwm_frequency = pwm_frequency
+    .dead_zone = dead_zone
+  };
+  return params;
 }
 
 // function setting the
@@ -149,10 +170,10 @@ void _setPwmPair(int pinH, int pinL, float val, int dead_time)
 //  - BLDC driver - 6PWM setting
 //  - hardware specific
 // supports Arudino/ATmega328
-void _writeDutyCycle6PWM(float dc_a,  float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){
-  _setPwmPair(pinA_h, pinA_l, dc_a*255.0, dead_zone*255.0);
-  _setPwmPair(pinB_h, pinB_l, dc_b*255.0, dead_zone*255.0);
-  _setPwmPair(pinC_h, pinC_l, dc_c*255.0, dead_zone*255.0);
+void _writeDutyCycle6PWM(float dc_a,  float dc_b, float dc_c, void* params){
+  _setPwmPair(((GenericDriverParams*)params)->pins[0], ((GenericDriverParams*)params)->pins[1], dc_a*255.0, ((GenericDriverParams*)params)->dead_zone*255.0);
+  _setPwmPair(((GenericDriverParams*)params)->pins[2], ((GenericDriverParams*)params)->pins[3], dc_b*255.0, ((GenericDriverParams*)params)->dead_zone*255.0);
+  _setPwmPair(((GenericDriverParams*)params)->pins[4], ((GenericDriverParams*)params)->pins[5], dc_c*255.0, ((GenericDriverParams*)params)->dead_zone*255.0);
 }
 
 #endif