FEAT initial low side current sensing for stm32f4

Author: askuric

src/common/base_classes/CurrentSense.cpp

@@ -16,6 +16,16 @@ float CurrentSense::getDCCurrent(float motor_electrical_angle){
         // 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);
@@ -48,6 +58,16 @@ DQCurrent_s CurrentSense::getFOCCurrents(float angle_el){
         // 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);

src/common/foc_utils.h

@@ -29,6 +29,7 @@
 #define _HIGH_IMPEDANCE 0
 #define _HIGH_Z _HIGH_IMPEDANCE
 #define _ACTIVE 1
+#define _NC (NOT_SET)
 
 // dq current structure
 struct DQCurrent_s

src/current_sense/InlineCurrentSense.cpp

@@ -40,22 +40,22 @@ void InlineCurrentSense::calibrateOffsets(){
     offset_ic = 0;
     // read the adc voltage 1000 times ( arbitrary number )
     for (int i = 0; i < calibration_rounds; i++) {
-        offset_ia += _readADCVoltageInline(pinA, params);
-        offset_ib += _readADCVoltageInline(pinB, params);
+        if(_isset(pinA)) offset_ia += _readADCVoltageInline(pinA, params);
+        if(_isset(pinB)) offset_ib += _readADCVoltageInline(pinB, params);
         if(_isset(pinC)) offset_ic += _readADCVoltageInline(pinC, params);
         _delay(1);
     }
     // calculate the mean offsets
-    offset_ia = offset_ia / calibration_rounds;
-    offset_ib = offset_ib / calibration_rounds;
+    if(_isset(pinA)) offset_ia = offset_ia / calibration_rounds;
+    if(_isset(pinB)) offset_ib = offset_ib / calibration_rounds;
     if(_isset(pinC)) offset_ic = offset_ic / calibration_rounds;
 }
 
 // read all three phase currents (if possible 2 or 3)
 PhaseCurrent_s InlineCurrentSense::getPhaseCurrents(){
     PhaseCurrent_s current;
-    current.a = (_readADCVoltageInline(pinA, params) - offset_ia)*gain_a;// amps
-    current.b = (_readADCVoltageInline(pinB, params) - offset_ib)*gain_b;// amps
+    current.a = (!_isset(pinA)) ? 0 : (_readADCVoltageInline(pinA, params) - offset_ia)*gain_a;// amps
+    current.b = (!_isset(pinB)) ? 0 : (_readADCVoltageInline(pinB, params) - offset_ib)*gain_b;// amps
     current.c = (!_isset(pinC)) ? 0 : (_readADCVoltageInline(pinC, params) - offset_ic)*gain_c; // amps
     return current;
 }

src/current_sense/LowsideCurrentSense.cpp

@@ -43,23 +43,23 @@ void LowsideCurrentSense::calibrateOffsets(){
     // read the adc voltage 1000 times ( arbitrary number )
     for (int i = 0; i < calibration_rounds; i++) {
         _startADC3PinConversionLowSide();
-        offset_ia += _readADCVoltageLowSide(pinA, params);
-        offset_ib += _readADCVoltageLowSide(pinB, params);
+        if(_isset(pinA)) offset_ia += _readADCVoltageLowSide(pinA, params);
+        if(_isset(pinB)) offset_ib += _readADCVoltageLowSide(pinB, params);
         if(_isset(pinC)) offset_ic += _readADCVoltageLowSide(pinC, params);
         _delay(1);
     }
     // calculate the mean offsets
-    offset_ia = offset_ia / calibration_rounds;
-    offset_ib = offset_ib / calibration_rounds;
+    if(_isset(pinA)) offset_ia = offset_ia / calibration_rounds;
+    if(_isset(pinB)) offset_ib = offset_ib / calibration_rounds;
     if(_isset(pinC)) offset_ic = offset_ic / calibration_rounds;
 }
 
 // read all three phase currents (if possible 2 or 3)
 PhaseCurrent_s LowsideCurrentSense::getPhaseCurrents(){
     PhaseCurrent_s current;
     _startADC3PinConversionLowSide();
-    current.a = (_readADCVoltageLowSide(pinA, params) - offset_ia)*gain_a;// amps
-    current.b = (_readADCVoltageLowSide(pinB, params) - offset_ib)*gain_b;// amps
+    current.a = (!_isset(pinA)) ? 0 : (_readADCVoltageLowSide(pinA, params) - offset_ia)*gain_a;// amps
+    current.b = (!_isset(pinB)) ? 0 : (_readADCVoltageLowSide(pinB, params) - offset_ib)*gain_b;// amps
     current.c = (!_isset(pinC)) ? 0 : (_readADCVoltageLowSide(pinC, params) - offset_ic)*gain_c; // amps
     return current;
 }

src/current_sense/LowsideCurrentSense.h

@@ -21,7 +21,7 @@ class LowsideCurrentSense: public CurrentSense{
       @param phB B phase adc pin
       @param phC C phase adc pin (optional)
     */
-    LowsideCurrentSense(float shunt_resistor, float gain, int pinA, int pinB, int pinC = NOT_SET);
+    LowsideCurrentSense(float shunt_resistor, float gain, int pinA, int pinB, int pinC = _NC);
 
     // CurrentSense interface implementing functions
     int init() override;

src/current_sense/hardware_api.h

@@ -12,7 +12,7 @@
 // will be returned as a void pointer from the _configureADCx functions
 // will be provided to the _readADCVoltageX() as a void pointer
 typedef struct GenericCurrentSenseParams {
-  int pins[3];
+  int pins[3]={(int)NOT_SET};
   float adc_voltage_conv;
 } GenericCurrentSenseParams;
 

src/current_sense/hardware_specific/stm32/b_g431/b_g431_mcu.cpp

@@ -1,8 +1,11 @@
 #include "../../../hardware_api.h"
+
+#if defined(ARDUINO_B_G431B_ESC1) 
+
 #include "b_g431_hal.h"
 #include "Arduino.h"
+#include "../stm32_mcu.h"
 
-#if defined(ARDUINO_B_G431B_ESC1) 
 #define _ADC_VOLTAGE 3.3f
 #define _ADC_RESOLUTION 4096.0f
 #define ADC_BUF_LEN_1 2
@@ -32,7 +35,7 @@ float _readADCVoltageInline(const int pin, const void* cs_params){
   else if(pin == PB1) // = ADC1_IN12 = phase W (OP3_OUT) on B-G431B-ESC1
     raw_adc = adcBuffer1[0];
 #endif
-  return raw_adc * ((GenericCurrentSenseParams*)cs_params)->adc_voltage_conv;
+  return raw_adc * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv;
 }
 
 void _configureOPAMP(OPAMP_HandleTypeDef *hopamp, OPAMP_TypeDef *OPAMPx_Def){
@@ -110,7 +113,7 @@ void* _configureADCInline(const void* driver_params, const int pinA,const int pi
     return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED;
   }
 
-  GenericCurrentSenseParams* params = new GenericCurrentSenseParams {
+  Stm32CurrentSenseParams* params = new Stm32CurrentSenseParams {
     .pins = { pinA, pinB, pinC },
     .adc_voltage_conv = (_ADC_VOLTAGE) / (_ADC_RESOLUTION)
   };
@@ -128,4 +131,10 @@ void DMA1_Channel2_IRQHandler(void) {
 }
 }
 
+void _driverSyncLowSide(void* driver_params, void* cs_params){
+  _UNUSED(cs_params);
+  // Set Trigger out for DMA transfer
+  LL_TIM_SetTriggerOutput(((STM32DriverParams*)driver_params)->timers[0]->getHandle()->Instance, LL_TIM_TRGO_UPDATE);
+}
+
 #endif

src/current_sense/hardware_specific/stm32/stm32_mcu.cpp

@@ -3,6 +3,8 @@
 
 #if defined(_STM32_DEF_) and !defined(ARDUINO_B_G431B_ESC1) 
 
+#include "stm32_mcu.h"
+
 #define _ADC_VOLTAGE 3.3f
 #define _ADC_RESOLUTION 1024.0f
 
@@ -14,12 +16,18 @@ void* _configureADCInline(const void* driver_params, const int pinA,const int pi
   pinMode(pinB, INPUT);
   if( _isset(pinC) ) pinMode(pinC, INPUT);
 
-  GenericCurrentSenseParams* params = new GenericCurrentSenseParams {
+  Stm32CurrentSenseParams* params = new Stm32CurrentSenseParams {
     .pins = { pinA, pinB, pinC },
     .adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION)
   };
 
   return params;
 }
 
+// function reading an ADC value and returning the read voltage
+__attribute__((weak))  float _readADCVoltageInline(const int pinA, const void* cs_params){
+  uint32_t raw_adc = analogRead(pinA);
+  return raw_adc * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv;
+}
+
 #endif

src/current_sense/hardware_specific/stm32/stm32_mcu.h

@@ -0,0 +1,24 @@
+
+#ifndef STM32_CURRENTSENSE_MCU_DEF
+#define STM32_CURRENTSENSE_MCU_DEF
+#include "../../hardware_api.h"
+#include "../../../common/foc_utils.h"
+
+#if defined(_STM32_DEF_) and !defined(ARDUINO_B_G431B_ESC1) 
+
+#define _ADC_VOLTAGE 3.3f
+#define _ADC_RESOLUTION 1024.0f
+
+// generic implementation of the hardware specific structure
+// containing all the necessary current sense parameters
+// will be returned as a void pointer from the _configureADCx functions
+// will be provided to the _readADCVoltageX() as a void pointer
+typedef struct Stm32CurrentSenseParams {
+  int pins[3] = {(int)NOT_SET};
+  float adc_voltage_conv;
+  ADC_HandleTypeDef* adc_handle = NULL;
+  HardwareTimer* timer_handle = NULL;
+} Stm32CurrentSenseParams;
+
+#endif
+#endif

src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp

@@ -0,0 +1,138 @@
+#include "stm32f4_hal.h"
+
+#if defined(STM32F4xx)
+
+
+// timer to injected TRGO
+// https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_adc_ex.h#L179
+uint32_t _timerToInjectedTRGO(HardwareTimer* timer){
+  if(timer->getHandle()->Instance == TIM1)  
+    return ADC_EXTERNALTRIGINJECCONV_T1_TRGO;
+  else if(timer->getHandle()->Instance == TIM2) 
+    return ADC_EXTERNALTRIGINJECCONV_T2_TRGO;
+  else if(timer->getHandle()->Instance == TIM4) 
+    return ADC_EXTERNALTRIGINJECCONV_T4_TRGO;
+  else if(timer->getHandle()->Instance == TIM5) 
+    return ADC_EXTERNALTRIGINJECCONV_T5_TRGO;
+  else
+    return 0;
+}
+
+// timer to regular TRGO
+// https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_adc.h#L331
+uint32_t _timerToRegularTRGO(HardwareTimer* timer){
+  if(timer->getHandle()->Instance == TIM2)  
+    return ADC_EXTERNALTRIGCONV_T2_TRGO;
+  else if(timer->getHandle()->Instance == TIM3) 
+    return ADC_EXTERNALTRIGCONV_T3_TRGO;
+  else if(timer->getHandle()->Instance == TIM8) 
+    return ADC_EXTERNALTRIGCONV_T8_TRGO;
+  else
+    return 0;
+}
+
+ADC_HandleTypeDef hadc;
+
+void _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params)
+{
+  ADC_ChannelConfTypeDef sConfig;
+  ADC_InjectionConfTypeDef sConfigInjected;
+    
+  /**Configure the global features of the ADC (Clock, Resolution, Data Alignment and number of conversion) 
+  */
+  hadc.Instance = (ADC_TypeDef *)pinmap_peripheral(analogInputToPinName(cs_params->pins[0]), PinMap_ADC);
+
+  if(hadc.Instance == ADC1) __HAL_RCC_ADC1_CLK_ENABLE();
+  else if(hadc.Instance == ADC2) __HAL_RCC_ADC2_CLK_ENABLE();
+  else if(hadc.Instance == ADC3) __HAL_RCC_ADC3_CLK_ENABLE();
+
+  hadc.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV4;
+  hadc.Init.Resolution = ADC_RESOLUTION_12B;
+  hadc.Init.ScanConvMode = ENABLE;
+  hadc.Init.ContinuousConvMode = ENABLE;
+  hadc.Init.DiscontinuousConvMode = DISABLE;
+  hadc.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE;
+  hadc.Init.ExternalTrigConv = ADC_SOFTWARE_START;
+  hadc.Init.DataAlign = ADC_DATAALIGN_RIGHT;
+  hadc.Init.NbrOfConversion = 1;
+  hadc.Init.DMAContinuousRequests = DISABLE;
+  hadc.Init.EOCSelection = ADC_EOC_SINGLE_CONV;
+  HAL_ADC_Init(&hadc);
+  /**Configure for the selected ADC regular channel to be converted. 
+  */
+  
+  /**Configures for the selected ADC injected channel its corresponding rank in the sequencer and its sample time 
+    */
+  sConfigInjected.InjectedNbrOfConversion = _isset(cs_params->pins[2]) ? 3 : 2;
+  sConfigInjected.InjectedSamplingTime = ADC_SAMPLETIME_3CYCLES;
+  sConfigInjected.ExternalTrigInjecConvEdge = ADC_EXTERNALTRIGINJECCONVEDGE_RISING;
+
+  // automating TRGO flag finding - hardware specific
+  // sConfigInjected.ExternalTrigInjecConv = ADC_EXTERNALTRIGINJECCONV_T1_TRGO;
+  uint8_t tim_num = 0;
+  while(driver_params->timers[tim_num] && tim_num < 6){
+    uint32_t trigger_flag = _timerToInjectedTRGO(driver_params->timers[tim_num++]);
+    if(!trigger_flag) continue; // timer does not have valid trgo for injected channels
+
+    // timer does have trgo flag for injuected channels  
+    sConfigInjected.ExternalTrigInjecConv = trigger_flag;
+    
+    // this will be the timer with which the ADC will sync
+    cs_params->timer_handle = driver_params->timers[tim_num-1];
+  }
+  if(!cs_params->timer_handle){
+    // not possible to use these timers for low-side current sense
+    return;
+  };
+  sConfigInjected.AutoInjectedConv = DISABLE;
+  sConfigInjected.InjectedDiscontinuousConvMode = DISABLE;
+  sConfigInjected.InjectedOffset = 0;
+
+  // first channel
+  sConfigInjected.InjectedRank = 1;
+  sConfigInjected.InjectedChannel = STM_PIN_CHANNEL(pinmap_function(analogInputToPinName(cs_params->pins[0]), PinMap_ADC));
+  HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected);
+  // second channel
+  sConfigInjected.InjectedRank = 2;
+  sConfigInjected.InjectedChannel = STM_PIN_CHANNEL(pinmap_function(analogInputToPinName(cs_params->pins[1]), PinMap_ADC));
+  HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected);
+
+  // third channel - if exists
+  if(_isset(cs_params->pins[2])){
+    sConfigInjected.InjectedRank = 3;
+    sConfigInjected.InjectedChannel = STM_PIN_CHANNEL(pinmap_function(analogInputToPinName(cs_params->pins[2]), PinMap_ADC));
+    HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected);
+  }
+  
+  // enable interrupt
+  HAL_NVIC_SetPriority(ADC_IRQn, 0, 0);
+  HAL_NVIC_EnableIRQ(ADC_IRQn);
+  
+  cs_params->adc_handle = &hadc;
+}
+
+void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC)
+{
+  uint8_t cnt = 0;
+  if(_isset(pinA)){
+    pinmap_pinout(analogInputToPinName(pinA), PinMap_ADC);
+    cs_params->pins[cnt++] = pinA;
+  }
+  if(_isset(pinB)){
+    pinmap_pinout(analogInputToPinName(pinB), PinMap_ADC);
+    cs_params->pins[cnt++] = pinB;
+  }
+  if(_isset(pinC)){ 
+    pinmap_pinout(analogInputToPinName(pinC), PinMap_ADC);
+    cs_params->pins[cnt] = pinC;
+  }
+}
+
+extern "C" {
+  void ADC_IRQHandler(void)
+  {
+      HAL_ADC_IRQHandler(&hadc);
+  }
+}
+
+#endif