diff --git a/src/current_sense/hardware_specific/stm32/stm32_adc_utils.cpp b/src/current_sense/hardware_specific/stm32/stm32_adc_utils.cpp
index c8bc7d4a..8bd41966 100644
--- a/src/current_sense/hardware_specific/stm32/stm32_adc_utils.cpp
+++ b/src/current_sense/hardware_specific/stm32/stm32_adc_utils.cpp
@@ -220,229 +220,275 @@ ADC_TypeDef* _findBestADCForInjectedPins(int numPins, int pins[], ADC_HandleType
 uint32_t _getADCChannelFromPinMap(PinName pin)
 {
   uint32_t function = pinmap_function(pin, PinMap_ADC);
+  uint32_t channel = _getADCChannel(STM_PIN_CHANNEL(function));
+  if (!channel)
+  {
+    _Error_Handler("ADC: Unknown adc channel", (int)(STM_PIN_CHANNEL(function)));
+  }
+  return channel;
+}
+
+/**
+  * @brief  Return ADC HAL channel linked to a PinName and the ADC handle
+  * @param  pin: PinName
+  * @param AdcHandle: ADC_HandleTypeDef a pointer to the ADC handle
+  * @retval Valid HAL channel
+  */
+uint32_t _getADCChannel(PinName pin, ADC_TypeDef *AdcHandle )
+{  
+  if (AdcHandle == NP) {
+    return _getADCChannelFromPinMap(pin);
+  }
+  // find the PinName that corresponds to the ADC 
+  int first_ind = _findIndexOfFirstEntry(pin);
+  int last_ind = _findIndexOfLastEntry(pin);
+  if (first_ind == -1 || last_ind == -1) {
+    _Error_Handler("ADC: Pin not found in PinMap_ADC", (int)pin);
+  }
+  // find the channel
   uint32_t channel = 0;
-  switch (STM_PIN_CHANNEL(function)) {
+  for (int i = first_ind; i <= last_ind; i++) {
+    if (PinMap_ADC[i].peripheral == AdcHandle) {
+      channel =_getADCChannelFromPinMap(PinMap_ADC[i].pin);
+      break;
+    }
+  }
+  return channel;
+}
+
+uint32_t _getADCInjectedRank(uint8_t ind){
+  switch (ind) {
+#ifdef ADC_INJECTED_RANK_1
+    case 0:
+      return ADC_INJECTED_RANK_1;
+#endif
+#ifdef ADC_INJECTED_RANK_2
+    case 1:
+      return ADC_INJECTED_RANK_2;
+#endif
+#ifdef ADC_INJECTED_RANK_3
+    case 2:
+      return ADC_INJECTED_RANK_3;
+#endif
+#ifdef ADC_INJECTED_RANK_4
+    case 3:
+      return ADC_INJECTED_RANK_4;
+#endif
+    default:
+      return 0;
+  }
+}
+
+uint32_t _getADCRegularRank(uint8_t ind)
+{
+  switch (ind) {
+#ifdef ADC_REGULAR_RANK_1
+    case 0:
+      return ADC_REGULAR_RANK_1;
+#endif
+#ifdef ADC_REGULAR_RANK_2
+    case 1:
+      return ADC_REGULAR_RANK_2;
+#endif
+#ifdef ADC_REGULAR_RANK_3
+    case 2:
+      return ADC_REGULAR_RANK_3;
+#endif
+#ifdef ADC_REGULAR_RANK_4
+    case 3:
+      return ADC_REGULAR_RANK_4;
+#endif
+#ifdef ADC_REGULAR_RANK_5
+    case 4:
+      return ADC_REGULAR_RANK_5;
+#endif
+#ifdef ADC_REGULAR_RANK_6
+    case 5:
+      return ADC_REGULAR_RANK_6;
+#endif
+#ifdef ADC_REGULAR_RANK_7
+    case 6:
+      return ADC_REGULAR_RANK_7;
+#endif
+#ifdef ADC_REGULAR_RANK_8
+    case 7:
+      return ADC_REGULAR_RANK_8;
+#endif
+#ifdef ADC_REGULAR_RANK_9
+    case 8:
+      return ADC_REGULAR_RANK_9;
+#endif
+#ifdef ADC_REGULAR_RANK_10
+    case 9:
+      return ADC_REGULAR_RANK_10;
+#endif
+#ifdef ADC_REGULAR_RANK_11
+    case 10:
+      return ADC_REGULAR_RANK_11;
+#endif
+#ifdef ADC_REGULAR_RANK_12
+    case 11:
+      return ADC_REGULAR_RANK_12;
+#endif
+#ifdef ADC_REGULAR_RANK_13
+    case 12:
+      return ADC_REGULAR_RANK_13;
+#endif
+#ifdef ADC_REGULAR_RANK_14
+    case 13:
+      return ADC_REGULAR_RANK_14;
+#endif
+#ifdef ADC_REGULAR_RANK_15
+    case 14:
+      return ADC_REGULAR_RANK_15;
+#endif
+#ifdef ADC_REGULAR_RANK_16
+    case 15:
+      return ADC_REGULAR_RANK_16;
+#endif
+    default:
+      return 0;
+  }
+}
+
+uint32_t _getADCChannel(uint8_t ind)
+{
+  switch (ind)
+  {
 #ifdef ADC_CHANNEL_0
     case 0:
-      channel = ADC_CHANNEL_0;
-      break;
+      return ADC_CHANNEL_0;
 #endif
 #ifdef ADC_CHANNEL_1
     case 1:
-      channel = ADC_CHANNEL_1;
-      break;
+      return ADC_CHANNEL_1;
 #endif
 #ifdef ADC_CHANNEL_2
     case 2:
-      channel = ADC_CHANNEL_2;
-      break;
+      return ADC_CHANNEL_2;
 #endif
 #ifdef ADC_CHANNEL_3
     case 3:
-      channel = ADC_CHANNEL_3;
-      break;
+      return ADC_CHANNEL_3;
 #endif
 #ifdef ADC_CHANNEL_4
     case 4:
-      channel = ADC_CHANNEL_4;
-      break;
+      return ADC_CHANNEL_4;
 #endif
 #ifdef ADC_CHANNEL_5
     case 5:
-      channel = ADC_CHANNEL_5;
-      break;
+      return ADC_CHANNEL_5;
 #endif
 #ifdef ADC_CHANNEL_6
     case 6:
-      channel = ADC_CHANNEL_6;
-      break;
+      return ADC_CHANNEL_6;
 #endif
 #ifdef ADC_CHANNEL_7
     case 7:
-      channel = ADC_CHANNEL_7;
-      break;
+      return ADC_CHANNEL_7;
 #endif
 #ifdef ADC_CHANNEL_8
     case 8:
-      channel = ADC_CHANNEL_8;
-      break;
+      return ADC_CHANNEL_8;
 #endif
 #ifdef ADC_CHANNEL_9
     case 9:
-      channel = ADC_CHANNEL_9;
-      break;
+      return ADC_CHANNEL_9;
 #endif
 #ifdef ADC_CHANNEL_10
     case 10:
-      channel = ADC_CHANNEL_10;
-      break;
+      return ADC_CHANNEL_10;
 #endif
 #ifdef ADC_CHANNEL_11
     case 11:
-      channel = ADC_CHANNEL_11;
-      break;
+      return ADC_CHANNEL_11;
 #endif
 #ifdef ADC_CHANNEL_12
     case 12:
-      channel = ADC_CHANNEL_12;
-      break;
+      return ADC_CHANNEL_12;
 #endif
 #ifdef ADC_CHANNEL_13
     case 13:
-      channel = ADC_CHANNEL_13;
-      break;
+      return ADC_CHANNEL_13;
 #endif
 #ifdef ADC_CHANNEL_14
     case 14:
-      channel = ADC_CHANNEL_14;
-      break;
+      return ADC_CHANNEL_14;
 #endif
 #ifdef ADC_CHANNEL_15
     case 15:
-      channel = ADC_CHANNEL_15;
-      break;
+      return ADC_CHANNEL_15;
+#endif
 #ifdef ADC_CHANNEL_16
     case 16:
-      channel = ADC_CHANNEL_16;
-      break;
+      return ADC_CHANNEL_16;
 #endif
+#ifdef ADC_CHANNEL_17
     case 17:
-      channel = ADC_CHANNEL_17;
-      break;
+      return ADC_CHANNEL_17;
+#endif
 #ifdef ADC_CHANNEL_18
     case 18:
-      channel = ADC_CHANNEL_18;
-      break;
+      return ADC_CHANNEL_18;
 #endif
 #ifdef ADC_CHANNEL_19
     case 19:
-      channel = ADC_CHANNEL_19;
-      break;
+      return ADC_CHANNEL_19;
 #endif
 #ifdef ADC_CHANNEL_20
     case 20:
-      channel = ADC_CHANNEL_20;
-      break;
+      return ADC_CHANNEL_20;
 #endif
 #ifdef ADC_CHANNEL_21
     case 21:
-      channel = ADC_CHANNEL_21;
-      break;
+      return ADC_CHANNEL_21;
 #endif
 #ifdef ADC_CHANNEL_22
     case 22:
-      channel = ADC_CHANNEL_22;
-      break;
+      return ADC_CHANNEL_22;
 #endif
 #ifdef ADC_CHANNEL_23
     case 23:
-      channel = ADC_CHANNEL_23;
-      break;
+      return ADC_CHANNEL_23;
+#endif
 #ifdef ADC_CHANNEL_24
     case 24:
-      channel = ADC_CHANNEL_24;
-      break;
+      return ADC_CHANNEL_24;
 #endif
 #ifdef ADC_CHANNEL_25
     case 25:
-      channel = ADC_CHANNEL_25;
-      break;
+      return ADC_CHANNEL_25;
 #endif
 #ifdef ADC_CHANNEL_26
     case 26:
-      channel = ADC_CHANNEL_26;
-      break;
+      return ADC_CHANNEL_26;
+#endif
 #ifdef ADC_CHANNEL_27
     case 27:
-      channel = ADC_CHANNEL_27;
-      break;
+      return ADC_CHANNEL_27;
 #endif
 #ifdef ADC_CHANNEL_28
     case 28:
-      channel = ADC_CHANNEL_28;
-      break;
+      return ADC_CHANNEL_28;
 #endif
 #ifdef ADC_CHANNEL_29
     case 29:
-      channel = ADC_CHANNEL_29;
-      break;
+      return ADC_CHANNEL_29;
 #endif
 #ifdef ADC_CHANNEL_30
     case 30:
-      channel = ADC_CHANNEL_30;
-      break;
+      return ADC_CHANNEL_30;
 #endif
 #ifdef ADC_CHANNEL_31
     case 31:
-      channel = ADC_CHANNEL_31;
-      break;
-#endif
-#endif
-#endif
-#endif
-    default:
-      _Error_Handler("ADC: Unknown adc channel", (int)(STM_PIN_CHANNEL(function)));
-      break;
-  }
-  return channel;
-}
-
-/**
-  * @brief  Return ADC HAL channel linked to a PinName and the ADC handle
-  * @param  pin: PinName
-  * @param AdcHandle: ADC_HandleTypeDef a pointer to the ADC handle
-  * @retval Valid HAL channel
-  */
-uint32_t _getADCChannel(PinName pin, ADC_TypeDef *AdcHandle )
-{  
-  if (AdcHandle == NP) {
-    return _getADCChannelFromPinMap(pin);
-  }
-  // find the PinName that corresponds to the ADC 
-  int first_ind = _findIndexOfFirstEntry(pin);
-  int last_ind = _findIndexOfLastEntry(pin);
-  if (first_ind == -1 || last_ind == -1) {
-    _Error_Handler("ADC: Pin not found in PinMap_ADC", (int)pin);
-  }
-  // find the channel
-  uint32_t channel = 0;
-  for (int i = first_ind; i <= last_ind; i++) {
-    if (PinMap_ADC[i].peripheral == AdcHandle) {
-      channel =_getADCChannelFromPinMap(PinMap_ADC[i].pin);
-      break;
-    }
-  }
-  return channel;
-}
-
-uint32_t _getADCInjectedRank(uint8_t ind){
-  switch (ind) {
-  #ifdef ADC_INJECTED_RANK_1
-    case 0:
-      return ADC_INJECTED_RANK_1;
-      break;
-#endif
-#ifdef ADC_INJECTED_RANK_2
-    case 1:
-      return ADC_INJECTED_RANK_2;
-      break;
-#endif
-#ifdef ADC_INJECTED_RANK_3
-    case 2:
-      return ADC_INJECTED_RANK_3;
-      break;
-#endif
-#ifdef ADC_INJECTED_RANK_4
-    case 3:
-      return ADC_INJECTED_RANK_4;
-      break;
+      return ADC_CHANNEL_31;
 #endif
     default:
       return 0;
-      break;
   }
 }
 
+
 // returns 0 if no interrupt is needed, 1 if interrupt is needed
 uint32_t _initTimerInterruptDownsampling(Stm32CurrentSenseParams* cs_params, STM32DriverParams* driver_params, Stm32AdcInterruptConfig& adc_interrupt_config){
 
diff --git a/src/current_sense/hardware_specific/stm32/stm32_adc_utils.h b/src/current_sense/hardware_specific/stm32/stm32_adc_utils.h
index 71d836c4..c930a42a 100644
--- a/src/current_sense/hardware_specific/stm32/stm32_adc_utils.h
+++ b/src/current_sense/hardware_specific/stm32/stm32_adc_utils.h
@@ -21,7 +21,9 @@
   * @retval Valid HAL channel
   */
 uint32_t _getADCChannel(PinName pin, ADC_TypeDef* adc = NP);
+uint32_t _getADCChannel(uint8_t ind);
 uint32_t _getADCInjectedRank(uint8_t ind);
+uint32_t _getADCRegularRank(uint8_t ind);
 
 // timer to injected TRGO - architecure specific
 uint32_t _timerToInjectedTRGO(TIM_HandleTypeDef* timer);
@@ -32,6 +34,7 @@ uint32_t _timerToRegularTRGO(TIM_HandleTypeDef* timer);
 // function returning index of the ADC instance
 int _adcToIndex(ADC_HandleTypeDef *AdcHandle);
 int _adcToIndex(ADC_TypeDef *AdcHandle);
+ADC_TypeDef* _indexToADC(uint8_t index);
 
 // functions helping to find the best ADC channel
 int _findIndexOfFirstPinMapADCEntry(int pin);