Merge pull request #11 from Candas1/dev

Candas1 · web-flow · commit a6daa972c23a · 2023-12-02T16:30:14.000+01:00
Dev
diff --git a/README.md b/README.md
@@ -27,9 +27,16 @@ Therefore this is an attempt to:
 
 > NEW RELEASE 📢 : <span class="simple">Simple<span class="foc">FOC</span>library</span> v2.3.2
 > - Improved [space vector modulation code](https://github.com/simplefoc/Arduino-FOC/pull/309) thanks to [@Candas1](https://github.com/Candas1)
+> - Bugfix for stepper motor initialization
+> - Bugfix for current sensing when only 2 phase currents available - please re-check your current sense PID tuning
 > - Bugfix for teensy3.2 - [#321](https://github.com/simplefoc/Arduino-FOC/pull/321)
 > - Added teensy3/4 compile to the github CI using platformio
-> - And more bugfixes - see the [complete list of 2.3.2 fixes here](https://github.com/simplefoc/Arduino-FOC/issues?q=is%3Aissue+milestone%3A2.3.2_Release)
+> - Fix compile issues with recent versions of ESP32 framework
+> - Add ADC calibration on STM32 MCUs
+> - Bugfix for crash when using ADC2 on ESP32s - [thanks to @mcells](https://github.com/simplefoc/Arduino-FOC/pull/346)
+> - Bugfix for renesas PWM on UNO R4 WiFi - [thanks to @facchinm](https://github.com/simplefoc/Arduino-FOC/pull/322)
+> - And more bugfixes - see the complete list of 2.3.2 [fixes and PRs](https://github.com/simplefoc/Arduino-FOC/milestone/9?closed=1)
+
 
 ## Arduino *SimpleFOClibrary* v2.3.2
 
diff --git a/examples/hardware_specific_examples/B_G431B_ESC1/B_G431B_ESC1.ino b/examples/hardware_specific_examples/B_G431B_ESC1/B_G431B_ESC1.ino
@@ -96,6 +96,7 @@ void setup() {
 
 void loop() {
   // main FOC algorithm function
+  motor.loopFOC();
 
   // Motion control function
   motor.move();
diff --git a/examples/hardware_specific_examples/DRV8302_driver/6pwm_example/encoder/full_control_serial/full_control_serial.ino b/examples/hardware_specific_examples/DRV8302_driver/6pwm_example/encoder/full_control_serial/full_control_serial.ino
@@ -29,7 +29,7 @@
 
 // Motor instance
 BLDCMotor motor = BLDCMotor(11);
-BLDCDriver6PWM driver = BLDCDriver6PWM(INH_A,INH_A, INH_B,INH_B, INH_C,INL_C, EN_GATE);
+BLDCDriver6PWM driver = BLDCDriver6PWM(INH_A,INL_A, INH_B,INL_B, INH_C,INL_C, EN_GATE);
 
 // encoder instance
 Encoder encoder = Encoder(2, 3, 8192);
diff --git a/src/common/base_classes/CurrentSense.cpp b/src/common/base_classes/CurrentSense.cpp
@@ -7,33 +7,12 @@
 float CurrentSense::getDCCurrent(float motor_electrical_angle){
     // read current phase currents
     PhaseCurrent_s current = getPhaseCurrents();
-    // currnet sign - if motor angle not provided the magnitude is always positive
-    float sign = 1;
-
+    
     // calculate clarke transform
-    float i_alpha, i_beta;
-    if(!current.c){
-        // if only two measured currents
-        i_alpha = current.a;  
-        i_beta = _1_SQRT3 * current.a + _2_SQRT3 * current.b;
-    }if(!current.a){
-        // if only two measured currents
-        float a = -current.c - current.b;
-        i_alpha = a;  
-        i_beta = _1_SQRT3 * a + _2_SQRT3 * current.b;
-    }if(!current.b){
-        // if only two measured currents
-        float b = -current.a - current.c;
-        i_alpha = current.a;  
-        i_beta = _1_SQRT3 * current.a + _2_SQRT3 * b;
-    }else{
-        // signal filtering using identity a + b + c = 0. Assumes measurement error is normally distributed.
-        float mid = (1.f/3) * (current.a + current.b + current.c);
-        float a = current.a - mid;
-        float b = current.b - mid;
-        i_alpha = a;
-        i_beta = _1_SQRT3 * a + _2_SQRT3 * b;
-    }
+    ABCurrent_s ABcurrent = getABCurrents(current);
+
+    // current sign - if motor angle not provided the magnitude is always positive
+    float sign = 1;
 
     // if motor angle provided function returns signed value of the current
     // determine the sign of the current
@@ -42,32 +21,46 @@ float CurrentSense::getDCCurrent(float motor_electrical_angle){
         float ct;
         float st;
         _sincos(motor_electrical_angle, &st, &ct);
-        sign = (i_beta*ct - i_alpha*st) > 0 ? 1 : -1;  
+        sign = (ABcurrent.beta*ct - ABcurrent.alpha*st) > 0 ? 1 : -1;  
     }
     // return current magnitude
-    return sign*_sqrt(i_alpha*i_alpha + i_beta*i_beta);
+    return sign*_sqrt(ABcurrent.alpha*ABcurrent.alpha + ABcurrent.beta*ABcurrent.beta);
 }
 
 // function used with the foc algorihtm
 //   calculating DQ currents from phase currents
 //   - function calculating park and clarke transform of the phase currents 
-//   - using getPhaseCurrents internally
+//   - using getPhaseCurrents and getABCurrents internally
 DQCurrent_s CurrentSense::getFOCCurrents(float angle_el){
     // read current phase currents
     PhaseCurrent_s current = getPhaseCurrents();
 
+    // calculate clarke transform
+    ABCurrent_s ABcurrent = getABCurrents(current);
+    
+    // calculate park transform
+    DQCurrent_s return_current = getDQCurrents(ABcurrent,angle_el);
+
+    return return_current;
+}
+
+// function used with the foc algorihtm
+//   calculating Alpha Beta currents from phase currents
+//   - function calculating Clarke transform of the phase currents
+ABCurrent_s CurrentSense::getABCurrents(PhaseCurrent_s current){
+
     // calculate clarke transform
     float i_alpha, i_beta;
     if(!current.c){
         // if only two measured currents
         i_alpha = current.a;  
         i_beta = _1_SQRT3 * current.a + _2_SQRT3 * current.b;
-    }if(!current.a){
+    }else if(!current.a){
         // if only two measured currents
         float a = -current.c - current.b;
         i_alpha = a;  
         i_beta = _1_SQRT3 * a + _2_SQRT3 * current.b;
-    }if(!current.b){
+    }else if(!current.b){
         // if only two measured currents
         float b = -current.a - current.c;
         i_alpha = current.a;  
@@ -81,13 +74,23 @@ DQCurrent_s CurrentSense::getFOCCurrents(float angle_el){
         i_beta = _1_SQRT3 * a + _2_SQRT3 * b;
     }
 
-    // calculate park transform
+    ABCurrent_s return_ABcurrent;
+    return_ABcurrent.alpha = i_alpha;
+    return_ABcurrent.beta = i_beta;
+    return return_ABcurrent;
+}
+
+// function used with the foc algorihtm
+//   calculating D and Q currents from Alpha Beta currents and electrical angle
+//   - function calculating Clarke transform of the phase currents
+DQCurrent_s CurrentSense::getDQCurrents(ABCurrent_s current, float angle_el){
+ // calculate park transform
     float ct;
     float st;
     _sincos(angle_el, &st, &ct);
     DQCurrent_s return_current;
-    return_current.d = i_alpha * ct + i_beta * st;
-    return_current.q = i_beta * ct - i_alpha * st;
+    return_current.d = current.alpha * ct + current.beta * st;
+    return_current.q = current.beta * ct - current.alpha * st;
     return return_current;
 }
 
@@ -96,4 +99,4 @@ DQCurrent_s CurrentSense::getFOCCurrents(float angle_el){
 */
 void CurrentSense::linkDriver(BLDCDriver* _driver) {
   driver = _driver;
-}
+}
diff --git a/src/common/base_classes/CurrentSense.h b/src/common/base_classes/CurrentSense.h
@@ -53,7 +53,7 @@ class CurrentSense{
     virtual PhaseCurrent_s getPhaseCurrents() = 0;
     /**
      * Function reading the magnitude of the current set to the motor
-     *  It returns the abosolute or signed magnitude if possible
+     *  It returns the absolute or signed magnitude if possible
      *  It can receive the motor electrical angle to help with calculation
      *  This function is used with the current control  (not foc)
      *  
@@ -62,13 +62,31 @@ class CurrentSense{
     virtual float getDCCurrent(float angle_el = 0);
 
     /**
-     * Function used for FOC contorl, it reads the DQ currents of the motor 
+     * Function used for FOC control, it reads the DQ currents of the motor 
      *   It uses the function getPhaseCurrents internally
      * 
      * @param angle_el - motor electrical angle
      */
     DQCurrent_s getFOCCurrents(float angle_el);
 
+    /**
+     * Function used for Clarke transform in FOC control
+     *   It reads the phase currents of the motor 
+     *   It returns the alpha and beta currents
+     * 
+     * @param current - phase current
+     */
+    ABCurrent_s getABCurrents(PhaseCurrent_s current);
+
+    /**
+     * Function used for Park transform in FOC control
+     *   It reads the Alpha Beta currents and electircal angle of the motor 
+     *   It returns the D and Q currents
+     * 
+     * @param current - phase current
+     */
+    DQCurrent_s getDQCurrents(ABCurrent_s current,float angle_el);
+
 
 };
 
diff --git a/src/common/base_classes/Sensor.cpp b/src/common/base_classes/Sensor.cpp
@@ -6,6 +6,8 @@
 
 void Sensor::update() {
     float val = getSensorAngle();
+    if (val<0) // sensor angles are strictly non-negative. Negative values are used to signal errors.
+        return; // TODO signal error, e.g. via a flag and counter
     angle_prev_ts = _micros();
     float d_angle = val - angle_prev;
     // if overflow happened track it as full rotation
diff --git a/src/common/foc_utils.h b/src/common/foc_utils.h
@@ -56,6 +56,12 @@ struct DQVoltage_s
     float d;
     float q;
 };
+// alpha beta current structure
+struct ABCurrent_s
+{
+    float alpha;
+    float beta;
+};
 
 
 /**
diff --git a/src/communication/Commander.cpp b/src/communication/Commander.cpp
@@ -11,10 +11,10 @@ Commander::Commander(char eol, bool echo){
 }
 
 
-void Commander::add(char id, CommandCallback onCommand, char* label ){
+void Commander::add(char id, CommandCallback onCommand, const char* label ){
   call_list[call_count] = onCommand;
   call_ids[call_count] = id;
-  call_label[call_count] = label;
+  call_label[call_count] = (char*)label;
   call_count++;
 }
 
diff --git a/src/communication/Commander.h b/src/communication/Commander.h
@@ -91,7 +91,7 @@ class Commander
      * @param onCommand  - function pointer void function(char*)
      * @param label      - string label to be displayed when scan command sent
      */
-    void add(char id , CommandCallback onCommand, char* label = nullptr);
+    void add(char id , CommandCallback onCommand, const char* label = nullptr);
 
     // printing variables
     VerboseMode verbose = VerboseMode::user_friendly; //!< flag signaling that the commands should output user understanable text
diff --git a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp
@@ -154,9 +154,8 @@ bool IRAM_ATTR __adcStart(uint8_t pin){
     }
 
     if(channel > 9){
-        channel -= 10;
         CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_START_SAR_M);
-        SET_PERI_REG_BITS(SENS_SAR_MEAS_START2_REG, SENS_SAR2_EN_PAD, (1 << channel), SENS_SAR2_EN_PAD_S);
+        SET_PERI_REG_BITS(SENS_SAR_MEAS_START2_REG, SENS_SAR2_EN_PAD, (1 << (channel - 10)), SENS_SAR2_EN_PAD_S);
         SET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_START_SAR_M);
     } else {
         CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_START_SAR_M);
@@ -225,9 +224,8 @@ uint16_t IRAM_ATTR adcRead(uint8_t pin)
     __analogInit();
 
     if(channel > 9){
-        channel -= 10;
         CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_START_SAR_M);
-        SET_PERI_REG_BITS(SENS_SAR_MEAS_START2_REG, SENS_SAR2_EN_PAD, (1 << channel), SENS_SAR2_EN_PAD_S);
+        SET_PERI_REG_BITS(SENS_SAR_MEAS_START2_REG, SENS_SAR2_EN_PAD, (1 << (channel - 10)), SENS_SAR2_EN_PAD_S);
         SET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_START_SAR_M);
     } else {
         CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_START_SAR_M);
@@ -257,4 +255,4 @@ uint16_t IRAM_ATTR adcRead(uint8_t pin)
 }
 
 
-#endif
+#endif
diff --git a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp
@@ -124,9 +124,11 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive
     HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected);
   }
   
+  #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT
   // enable interrupt
   HAL_NVIC_SetPriority(ADC1_2_IRQn, 0, 0);
   HAL_NVIC_EnableIRQ(ADC1_2_IRQn);
+  #endif
   
   cs_params->adc_handle = &hadc;
 
@@ -151,12 +153,14 @@ void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const in
 
 }
 
+#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT
 extern "C" {
   void ADC1_2_IRQHandler(void)
   {
       HAL_ADC_IRQHandler(&hadc);
   }
   
 }
+#endif
 
 #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
@@ -69,9 +69,13 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){
 
   // Start the adc calibration
   HAL_ADCEx_Calibration_Start(cs_params->adc_handle);
-
+  
   // start the adc 
+  #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT
   HAL_ADCEx_InjectedStart_IT(cs_params->adc_handle);
+  #else
+  HAL_ADCEx_InjectedStart(cs_params->adc_handle);
+  #endif
 
   // restart all the timers of the driver
   _startTimers(driver_params->timers, 6);
@@ -81,13 +85,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){
   for(int i=0; i < 3; i++){
-    if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]) // found in the buffer
-      return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv;
+    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;
+      #else
+        return HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle,i+1) * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv;
+      #endif
+    }
   } 
   return 0;
 }
 
-
+#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT
 extern "C" {
   void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef *AdcHandle){
     // calculate the instance
@@ -104,5 +113,6 @@ extern "C" {
     adc_val[adc_index][2]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_3);
   }
 }
+#endif
 
 #endif
diff --git a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp
@@ -135,9 +135,11 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive
     }
   }
   
+  #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT
   // enable interrupt
   HAL_NVIC_SetPriority(ADC_IRQn, 0, 0);
   HAL_NVIC_EnableIRQ(ADC_IRQn);
+  #endif
   
   cs_params->adc_handle = &hadc;
   return 0;
@@ -160,11 +162,13 @@ void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const in
   }
 }
 
+#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT
 extern "C" {
   void ADC_IRQHandler(void)
   {
       HAL_ADC_IRQHandler(&hadc);
   }
 }
+#endif
 
 #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
diff --git a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp
diff --git a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp
diff --git a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp
diff --git a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp

Original file line number	Diff line number	Diff line change
`@@ -11,10 +11,10 @@ Commander::Commander(char eol, bool echo){`
`11`	`11`	`}`
`12`	`12`
`13`	`13`
`14`		`-void Commander::add(char id, CommandCallback onCommand, char* label ){`
	`14`	`+void Commander::add(char id, CommandCallback onCommand, const char* label ){`
`15`	`15`	`call_list[call_count] = onCommand;`
`16`	`16`	`call_ids[call_count] = id;`
`17`		`- call_label[call_count] = label;`
	`17`	`+ call_label[call_count] = (char*)label;`
`18`	`18`	`call_count++;`
`19`	`19`	`}`
`20`	`20`