Add lowside current sensing for STM32F7 microcontrollers

src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.cpp

@@ -0,0 +1,185 @@
+#include "stm32f7_hal.h"
+
+#if defined(STM32F7xx)
+
+//#define SIMPLEFOC_STM32_DEBUG
+
+#include "../../../../communication/SimpleFOCDebug.h"
+#define _TRGO_NOT_AVAILABLE 12345
+
+ADC_HandleTypeDef hadc;
+
+int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params)
+{
+  ADC_InjectionConfTypeDef sConfigInjected;
+
+  // check if all pins belong to the same ADC
+  ADC_TypeDef* adc_pin1 = (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[0]), PinMap_ADC);
+  ADC_TypeDef* adc_pin2 = (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[1]), PinMap_ADC);
+  ADC_TypeDef* adc_pin3 = _isset(cs_params->pins[2]) ? (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[2]), PinMap_ADC) : nullptr;
+ if ( (adc_pin1 != adc_pin2) || ( (adc_pin3) && (adc_pin1 != adc_pin3) )){
+#ifdef SIMPLEFOC_STM32_DEBUG
+    SIMPLEFOC_DEBUG("STM32-CS: ERR: Analog pins dont belong to the same ADC!");
+#endif
+  return -1;
+ }
+
+
+  /**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();
+#ifdef ADC2  // if defined ADC2
+  else if(hadc.Instance == ADC2) __HAL_RCC_ADC2_CLK_ENABLE();
+#endif
+#ifdef ADC3  // if defined ADC3
+  else if(hadc.Instance == ADC3) __HAL_RCC_ADC3_CLK_ENABLE();
+#endif
+  else{
+#ifdef SIMPLEFOC_STM32_DEBUG
+    SIMPLEFOC_DEBUG("STM32-CS: ERR: Pin does not belong to any ADC!");
+#endif
+    return -1; // error not a valid ADC instance
+  }
+
+#ifdef SIMPLEFOC_STM32_DEBUG
+    SIMPLEFOC_DEBUG("STM32-CS: Using ADC: ", _adcToIndex(&hadc)+1);
+#endif
+
+  hadc.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV2;
+  hadc.Init.Resolution = ADC_RESOLUTION_12B;
+  hadc.Init.ScanConvMode = ENABLE;
+  hadc.Init.ContinuousConvMode = DISABLE;
+  hadc.Init.DiscontinuousConvMode = DISABLE;
+  hadc.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE;
+  hadc.Init.ExternalTrigConv = ADC_SOFTWARE_START; // for now
+  hadc.Init.DataAlign = ADC_DATAALIGN_RIGHT;
+  hadc.Init.NbrOfConversion = 1;
+  hadc.Init.DMAContinuousRequests = DISABLE;
+  hadc.Init.EOCSelection = ADC_EOC_SINGLE_CONV;
+  if ( HAL_ADC_Init(&hadc) != HAL_OK){
+#ifdef SIMPLEFOC_STM32_DEBUG
+    SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init ADC!");
+#endif
+    return -1;
+  }
+
+  /**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_RISINGFALLING;  
+  sConfigInjected.AutoInjectedConv = DISABLE;
+  sConfigInjected.InjectedDiscontinuousConvMode = DISABLE;
+  sConfigInjected.InjectedOffset = 0;
+
+  // automating TRGO flag finding - hardware specific
+  uint8_t tim_num = 0;
+  for (size_t i=0; i<6; i++) {
+    HardwareTimer *timer_to_check = driver_params->timers[tim_num++];
+    TIM_TypeDef *instance_to_check = timer_to_check->getHandle()->Instance;
+
+    // bool TRGO_already_configured = instance_to_check->CR2 & LL_TIM_TRGO_UPDATE;
+    // if(TRGO_already_configured) continue;
+
+    uint32_t trigger_flag = _timerToInjectedTRGO(timer_to_check);
+    if(trigger_flag == _TRGO_NOT_AVAILABLE) continue; // timer does not have valid trgo for injected channels
+
+    // if the code comes here, it has found the timer available
+    // timer does have trgo flag for injected channels  
+    sConfigInjected.ExternalTrigInjecConv = trigger_flag;
+
+    // this will be the timer with which the ADC will sync
+    cs_params->timer_handle = timer_to_check;
+    if (!IS_TIM_REPETITION_COUNTER_INSTANCE(instance_to_check)) {
+      // workaround for errata 2.2.1 in ES0290 Rev 7
+      // https://www.st.com/resource/en/errata_sheet/es0290-stm32f74xxx-and-stm32f75xxx-device-limitations-stmicroelectronics.pdf
+      __HAL_RCC_DAC_CLK_ENABLE();
+    } 
+    // done
+    break;
+  }
+  if( cs_params->timer_handle == NP ){
+    // not possible to use these timers for low-side current sense
+  #ifdef SIMPLEFOC_STM32_DEBUG
+    SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot sync any timer to injected channels!");
+  #endif
+    return -1;
+  }
+  // display which timer is being used
+  #ifdef SIMPLEFOC_STM32_DEBUG
+    // it would be better to use the getTimerNumber from driver
+    SIMPLEFOC_DEBUG("STM32-CS: injected trigger for timer index: ", get_timer_index(cs_params->timer_handle->getHandle()->Instance) + 1);
+  #endif
+
+
+  // first channel
+  sConfigInjected.InjectedRank = ADC_INJECTED_RANK_1;
+  sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[0]));
+  if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){
+#ifdef SIMPLEFOC_STM32_DEBUG
+    SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[0])) );
+#endif
+    return -1;
+  }
+
+  // second channel
+  sConfigInjected.InjectedRank = ADC_INJECTED_RANK_2;
+  sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[1]));
+  if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){
+#ifdef SIMPLEFOC_STM32_DEBUG
+    SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[1]))) ;
+#endif
+    return -1;
+  }
+
+  // third channel - if exists
+  if(_isset(cs_params->pins[2])){
+    sConfigInjected.InjectedRank = ADC_INJECTED_RANK_3;
+    sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[2]));
+    if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){
+#ifdef SIMPLEFOC_STM32_DEBUG
+      SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[2]))) ;
+#endif
+      return -1;
+    }
+  }
+
+  #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;
+}
+
+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;
+  }
+}
+
+#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT
+extern "C" {
+  void ADC_IRQHandler(void)
+  {
+      HAL_ADC_IRQHandler(&hadc);
+  }
+}
+#endif
+
+#endif

src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.h

@@ -0,0 +1,15 @@
+#pragma once
+
+#include "Arduino.h"
+
+#if defined(STM32F7xx)
+#include "stm32f7xx_hal.h"
+#include "../../../../common/foc_utils.h"
+#include "../../../../drivers/hardware_specific/stm32/stm32_mcu.h"
+#include "../stm32_mcu.h"
+#include "stm32f7_utils.h"
+
+int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params);
+void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC);
+
+#endif

src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_mcu.cpp

@@ -0,0 +1,111 @@
+#include "../../../hardware_api.h"
+
+#if defined(STM32F7xx)
+#include "../../../../common/foc_utils.h"
+#include "../../../../drivers/hardware_api.h"
+#include "../../../../drivers/hardware_specific/stm32/stm32_mcu.h"
+#include "../../../hardware_api.h"
+#include "../stm32_mcu.h"
+#include "stm32f7_hal.h"
+#include "stm32f7_utils.h"
+#include "Arduino.h"
+
+
+#define _ADC_VOLTAGE 3.3f
+#define _ADC_RESOLUTION 4096.0f
+
+
+// array of values of 4 injected channels per adc instance (3)
+uint32_t adc_val[3][4]={0};
+// does adc interrupt need a downsample - per adc (3)
+bool needs_downsample[3] = {1};
+// downsampling variable - per adc (3)
+uint8_t tim_downsample[3] = {1};
+
+void* _configureADCLowSide(const void* driver_params, const int pinA, const int pinB, const int pinC){
+
+  Stm32CurrentSenseParams* cs_params= new Stm32CurrentSenseParams {
+    .pins={(int)NOT_SET,(int)NOT_SET,(int)NOT_SET},
+    .adc_voltage_conv = (_ADC_VOLTAGE) / (_ADC_RESOLUTION)
+  };
+  _adc_gpio_init(cs_params, pinA,pinB,pinC);
+  if(_adc_init(cs_params, (STM32DriverParams*)driver_params) != 0) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED;
+  return cs_params;
+}
+
+
+void _driverSyncLowSide(void* _driver_params, void* _cs_params){
+  STM32DriverParams* driver_params = (STM32DriverParams*)_driver_params;
+  Stm32CurrentSenseParams* cs_params = (Stm32CurrentSenseParams*)_cs_params;
+
+  // if compatible timer has not been found
+  if (cs_params->timer_handle == NULL) return;
+
+  // stop all the timers for the driver
+  _stopTimers(driver_params->timers, 6);
+
+  // if timer has repetition counter - it will downsample using it
+  // and it does not need the software downsample
+  if( IS_TIM_REPETITION_COUNTER_INSTANCE(cs_params->timer_handle->getHandle()->Instance) ){
+    // adjust the initial timer state such that the trigger 
+    //   - for DMA transfer aligns with the pwm peaks instead of throughs.
+    //   - for interrupt based ADC transfer 
+    //   - only necessary for the timers that have repetition counters
+
+    cs_params->timer_handle->getHandle()->Instance->CR1 |= TIM_CR1_DIR;
+    cs_params->timer_handle->getHandle()->Instance->CNT = cs_params->timer_handle->getHandle()->Instance->ARR;
+    // remember that this timer has repetition counter - no need to downasmple
+    needs_downsample[_adcToIndex(cs_params->adc_handle)] = 0;
+  }
+  // set the trigger output event
+  LL_TIM_SetTriggerOutput(cs_params->timer_handle->getHandle()->Instance, LL_TIM_TRGO_UPDATE);
+
+  // 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);
+}
+
+
+// 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
+      #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT
+        return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv;
+      #else
+        // an optimized way to go from i to the channel i=0 -> channel 1, i=1 -> channel 2, i=2 -> channel 3
+        uint32_t channel = (i == 0) ? ADC_INJECTED_RANK_1 : (i == 1) ? ADC_INJECTED_RANK_2 : ADC_INJECTED_RANK_3;
+        return HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle, channel) * ((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
+    int adc_index = _adcToIndex(AdcHandle);
+
+    // if the timer han't repetition counter - downsample two times
+    if( needs_downsample[adc_index] && tim_downsample[adc_index]++ > 0) {
+      tim_downsample[adc_index] = 0;
+      return;
+    }
+
+    adc_val[adc_index][0]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_1);
+    adc_val[adc_index][1]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_2);
+    adc_val[adc_index][2]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_3);    
+  }
+}
+#endif
+
+#endif