diff --git a/src/current_sense/hardware_specific/stm32/stm32_mcu.h b/src/current_sense/hardware_specific/stm32/stm32_mcu.h
index 055f20db..5c7b1a28 100644
--- a/src/current_sense/hardware_specific/stm32/stm32_mcu.h
+++ b/src/current_sense/hardware_specific/stm32/stm32_mcu.h
@@ -3,6 +3,8 @@
 #define STM32_CURRENTSENSE_MCU_DEF
 #include "../../hardware_api.h"
 #include "../../../common/foc_utils.h"
+#include "../../../drivers/hardware_specific/stm32/stm32_mcu.h"
+#include "../../../drivers/hardware_specific/stm32/stm32_timerutils.h"
 
 #if defined(_STM32_DEF_) 
 
diff --git a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp
index 880fb3d6..f8fd6fbf 100644
--- a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp
+++ b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp
@@ -41,6 +41,14 @@ uint32_t _timerToRegularTRGO(TIM_HandleTypeDef* timer){
 
 ADC_HandleTypeDef hadc;
 
+/**
+ * Function initializing the ADC and the injected channels for the low-side current sensing
+ * 
+ * @param cs_params - current sense parameters
+ * @param driver_params - driver parameters
+ * 
+ * @return int - 0 if success 
+ */
 int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params)
 {
   ADC_InjectionConfTypeDef sConfigInjected;
@@ -86,6 +94,13 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive
     uint32_t trigger_flag = _timerToInjectedTRGO(driver_params->timers_handle[tim_num++]);
     if(trigger_flag == _TRGO_NOT_AVAILABLE) continue; // timer does not have valid trgo for injected channels
 
+    // check if TRGO used already - if yes use the next timer
+    if((driver_params->timers_handle[tim_num-1]->Instance->CR2 & LL_TIM_TRGO_ENABLE) || // if used for timer sync
+       (driver_params->timers_handle[tim_num-1]->Instance->CR2 & LL_TIM_TRGO_UPDATE)) // if used for ADC sync
+      {
+      continue;
+    }
+    
     // if the code comes here, it has found the timer available
     // timer does have trgo flag for injected channels  
     sConfigInjected.ExternalTrigInjecConv = trigger_flag;
@@ -101,12 +116,11 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive
     SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot sync any timer to injected channels!");
   #endif
     return -1;
-  }
-  // display which timer is being used
+  }else{
   #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->Instance) + 1); 
+      SIMPLEFOC_DEBUG("STM32-CS: Using timer: ", stm32_getTimerNumber(cs_params->timer_handle->Instance));
   #endif
+  }
 
   // first channel
   sConfigInjected.InjectedRank = ADC_REGULAR_RANK_1;
@@ -129,24 +143,38 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive
   return 0;
 }
 
-void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC)
+/**
+ * Function to initialize the ADC GPIO pins
+ * 
+ * @param cs_params current sense parameters
+ * @param pinA pin number for phase A
+ * @param pinB pin number for phase B
+ * @param pinC pin number for phase C
+ * @return int 0 if success, -1 if error
+ */
+int _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;
+  int pins[3] = {pinA, pinB, pinC};
+  const char* port_names[3] = {"A", "B", "C"};
+  for(int i=0; i<3; i++){
+    if(_isset(pins[i])){
+      // check if pin is an analog pin
+      if(pinmap_peripheral(analogInputToPinName(pins[i]), PinMap_ADC) == NP){
+#ifdef SIMPLEFOC_STM32_DEBUG
+        SimpleFOCDebug::print("STM32-CS: ERR: Pin ");
+        SimpleFOCDebug::print(port_names[i]);
+        SimpleFOCDebug::println(" does not belong to any ADC!");
+#endif
+        return -1;
+      }
+      pinmap_pinout(analogInputToPinName(pins[i]), PinMap_ADC);
+      cs_params->pins[i] = pins[i];
+    }
   }
-
+  return 0;
 }
 
+
 extern "C" {
   void ADC1_2_IRQHandler(void)
   {
diff --git a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.h b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.h
index b0f4f83b..fbcf4e99 100644
--- a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.h
+++ b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.h
@@ -5,12 +5,10 @@
 
 #if defined(STM32F1xx) 
 #include "stm32f1xx_hal.h"
-#include "../../../../common/foc_utils.h"
-#include "../../../../drivers/hardware_specific/stm32/stm32_mcu.h"
 #include "../stm32_mcu.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);
+int _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC);
 
 #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
index 7d31f966..b52b48bd 100644
--- a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp
+++ b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp
@@ -42,7 +42,7 @@ void* _configureADCLowSide(const void* driver_params, const int pinA, const int
     .pins={(int)NOT_SET,(int)NOT_SET,(int)NOT_SET},
     .adc_voltage_conv = (_ADC_VOLTAGE_F1) / (_ADC_RESOLUTION_F1)
   };
-  _adc_gpio_init(cs_params, pinA,pinB,pinC);
+  if(_adc_gpio_init(cs_params, pinA,pinB,pinC) != 0) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED;
   if(_adc_init(cs_params, (STM32DriverParams*)driver_params) != 0) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED;
   return cs_params;
 }
diff --git a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp
index 5320cc3e..6fc97d69 100644
--- a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp
+++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp
@@ -9,6 +9,14 @@
 
 ADC_HandleTypeDef hadc;
 
+/**
+ * Function initializing the ADC and the injected channels for the low-side current sensing
+ * 
+ * @param cs_params - current sense parameters
+ * @param driver_params - driver parameters
+ * 
+ * @return int - 0 if success 
+ */
 int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params)
 {
   ADC_InjectionConfTypeDef sConfigInjected;
@@ -80,6 +88,13 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive
     uint32_t trigger_flag = _timerToInjectedTRGO(driver_params->timers_handle[tim_num++]);
     if(trigger_flag == _TRGO_NOT_AVAILABLE) continue; // timer does not have valid trgo for injected channels
 
+    // check if TRGO used already - if yes use the next timer
+    if((driver_params->timers_handle[tim_num-1]->Instance->CR2 & LL_TIM_TRGO_ENABLE) || // if used for timer sync
+       (driver_params->timers_handle[tim_num-1]->Instance->CR2 & LL_TIM_TRGO_UPDATE)) // if used for ADC sync
+      {
+      continue;
+    }
+
     // if the code comes here, it has found the timer available
     // timer does have trgo flag for injected channels  
     sConfigInjected.ExternalTrigInjecConv = trigger_flag;
@@ -95,12 +110,11 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive
     SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot sync any timer to injected channels!");
   #endif
     return -1;
+  }else{
+#ifdef SIMPLEFOC_STM32_DEBUG
+    SIMPLEFOC_DEBUG("STM32-CS: Using timer: ", stm32_getTimerNumber(cs_params->timer_handle->Instance));
+#endif
   }
-  // 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->Instance) + 1);
-  #endif
 
 
   // first channel
@@ -139,21 +153,35 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive
   return 0;
 }
 
-void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC)
+/**
+ * Function to initialize the ADC GPIO pins
+ * 
+ * @param cs_params current sense parameters
+ * @param pinA pin number for phase A
+ * @param pinB pin number for phase B
+ * @param pinC pin number for phase C
+ * @return int 0 if success, -1 if error
+ */
+int _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;
+  int pins[3] = {pinA, pinB, pinC};
+  const char* port_names[3] = {"A", "B", "C"};
+  for(int i=0; i<3; i++){
+    if(_isset(pins[i])){
+      // check if pin is an analog pin
+      if(pinmap_peripheral(analogInputToPinName(pins[i]), PinMap_ADC) == NP){
+#ifdef SIMPLEFOC_STM32_DEBUG
+        SimpleFOCDebug::print("STM32-CS: ERR: Pin ");
+        SimpleFOCDebug::print(port_names[i]);
+        SimpleFOCDebug::println(" does not belong to any ADC!");
+#endif
+        return -1;
+      }
+      pinmap_pinout(analogInputToPinName(pins[i]), PinMap_ADC);
+      cs_params->pins[i] = pins[i];
+    }
   }
+  return 0;
 }
 
 extern "C" {
diff --git a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.h b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.h
index 71071a56..ca73f181 100644
--- a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.h
+++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.h
@@ -5,13 +5,11 @@
 
 #if defined(STM32F4xx)
 #include "stm32f4xx_hal.h"
-#include "../../../../common/foc_utils.h"
-#include "../../../../drivers/hardware_specific/stm32/stm32_mcu.h"
 #include "../stm32_mcu.h"
 #include "stm32f4_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);
+int _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC);
 
 #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
index d3c7bd7f..85d142bc 100644
--- a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp
+++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp
@@ -34,7 +34,7 @@ void* _configureADCLowSide(const void* driver_params, const int pinA, const int
     .pins={(int)NOT_SET,(int)NOT_SET,(int)NOT_SET},
     .adc_voltage_conv = (_ADC_VOLTAGE_F4) / (_ADC_RESOLUTION_F4)
   };
-  _adc_gpio_init(cs_params, pinA,pinB,pinC);
+  if(_adc_gpio_init(cs_params, pinA,pinB,pinC) != 0) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED;
   if(_adc_init(cs_params, (STM32DriverParams*)driver_params) != 0) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED;
   return cs_params;
 }
diff --git a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.cpp b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.cpp
index 04575397..5d036cab 100644
--- a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.cpp
+++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.cpp
@@ -9,6 +9,14 @@
 
 ADC_HandleTypeDef hadc;
 
+/**
+ * Function initializing the ADC and the injected channels for the low-side current sensing
+ * 
+ * @param cs_params - current sense parameters
+ * @param driver_params - driver parameters
+ * 
+ * @return int - 0 if success 
+ */
 int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params)
 {
   ADC_InjectionConfTypeDef sConfigInjected;
@@ -80,12 +88,16 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive
     TIM_HandleTypeDef *timer_to_check = driver_params->timers_handle[tim_num++];
     TIM_TypeDef *instance_to_check = timer_to_check->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
 
+    // check if TRGO used already - if yes use the next timer
+    if((timer_to_check->Instance->CR2 & LL_TIM_TRGO_ENABLE) || // if used for timer sync
+       (timer_to_check->Instance->CR2 & LL_TIM_TRGO_UPDATE)) // if used for ADC sync
+      {
+      continue;
+    }
+
     // if the code comes here, it has found the timer available
     // timer does have trgo flag for injected channels  
     sConfigInjected.ExternalTrigInjecConv = trigger_flag;
@@ -106,12 +118,11 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive
     SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot sync any timer to injected channels!");
   #endif
     return -1;
+  }else{
+#ifdef SIMPLEFOC_STM32_DEBUG
+    SIMPLEFOC_DEBUG("STM32-CS: Using timer: ", stm32_getTimerNumber(cs_params->timer_handle->Instance));
+#endif
   }
-  // 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->Instance) + 1);
-  #endif
 
 
   // first channel
@@ -156,21 +167,36 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive
   return 0;
 }
 
-void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC)
+
+/**
+ * Function to initialize the ADC GPIO pins
+ * 
+ * @param cs_params current sense parameters
+ * @param pinA pin number for phase A
+ * @param pinB pin number for phase B
+ * @param pinC pin number for phase C
+ * @return int 0 if success, -1 if error
+ */
+int _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;
+  int pins[3] = {pinA, pinB, pinC};
+  const char* port_names[3] = {"A", "B", "C"};
+  for(int i=0; i<3; i++){
+    if(_isset(pins[i])){
+      // check if pin is an analog pin
+      if(pinmap_peripheral(analogInputToPinName(pins[i]), PinMap_ADC) == NP){
+#ifdef SIMPLEFOC_STM32_DEBUG
+        SimpleFOCDebug::print("STM32-CS: ERR: Pin ");
+        SimpleFOCDebug::print(port_names[i]);
+        SimpleFOCDebug::println(" does not belong to any ADC!");
+#endif
+        return -1;
+      }
+      pinmap_pinout(analogInputToPinName(pins[i]), PinMap_ADC);
+      cs_params->pins[i] = pins[i];
+    }
   }
+  return 0;
 }
 
 #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT
diff --git a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.h b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.h
index 0a3614b5..ebd9e75e 100644
--- a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.h
+++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.h
@@ -4,12 +4,10 @@
 
 #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);
+int _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC);
 
 #endif
diff --git a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_mcu.cpp
index e4b9c850..868aafd0 100644
--- a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_mcu.cpp
+++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_mcu.cpp
@@ -28,7 +28,7 @@ void* _configureADCLowSide(const void* driver_params, const int pinA, const int
     .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_gpio_init(cs_params, pinA,pinB,pinC) != 0) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED;
   if(_adc_init(cs_params, (STM32DriverParams*)driver_params) != 0) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED;
   return cs_params;
 }
diff --git a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp
index 84a9560c..79f6fd4f 100644
--- a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp
+++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp
@@ -8,6 +8,14 @@
 
 ADC_HandleTypeDef hadc;
 
+/**
+ * Function initializing the ADC and the injected channels for the low-side current sensing
+ * 
+ * @param cs_params - current sense parameters
+ * @param driver_params - driver parameters
+ * 
+ * @return int - 0 if success 
+ */
 int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params)
 {
   ADC_InjectionConfTypeDef sConfigInjected;
@@ -130,6 +138,13 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive
     uint32_t trigger_flag = _timerToInjectedTRGO(driver_params->timers_handle[tim_num++]);
     if(trigger_flag == _TRGO_NOT_AVAILABLE) continue; // timer does not have valid trgo for injected channels
 
+    // check if TRGO used already - if yes use the next timer
+    if((driver_params->timers_handle[tim_num-1]->Instance->CR2 & LL_TIM_TRGO_ENABLE) || // if used for timer sync
+       (driver_params->timers_handle[tim_num-1]->Instance->CR2 & LL_TIM_TRGO_UPDATE)) // if used for ADC sync
+      {
+      continue;
+    }
+
     // if the code comes here, it has found the timer available
     // timer does have trgo flag for injected channels  
     sConfigInjected.ExternalTrigInjecConv = trigger_flag;
@@ -145,6 +160,10 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive
     SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot sync any timer to injected channels!");
 #endif
     return -1;
+  }else{
+#ifdef SIMPLEFOC_STM32_DEBUG
+    SIMPLEFOC_DEBUG("STM32-CS: Using timer: ", stm32_getTimerNumber(cs_params->timer_handle->Instance));
+#endif
   }
 
 
@@ -184,23 +203,38 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive
   return 0;
 }
 
-void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC)
+/**
+ * Function to initialize the ADC GPIO pins
+ * 
+ * @param cs_params current sense parameters
+ * @param pinA pin number for phase A
+ * @param pinB pin number for phase B
+ * @param pinC pin number for phase C
+ * @return int 0 if success, -1 if error
+ */
+int _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;
+  int pins[3] = {pinA, pinB, pinC};
+  const char* port_names[3] = {"A", "B", "C"};
+  for(int i=0; i<3; i++){
+    if(_isset(pins[i])){
+      // check if pin is an analog pin
+      if(pinmap_peripheral(analogInputToPinName(pins[i]), PinMap_ADC) == NP){
+#ifdef SIMPLEFOC_STM32_DEBUG
+        SimpleFOCDebug::print("STM32-CS: ERR: Pin ");
+        SimpleFOCDebug::print(port_names[i]);
+        SimpleFOCDebug::println(" does not belong to any ADC!");
+#endif
+        return -1;
+      }
+      pinmap_pinout(analogInputToPinName(pins[i]), PinMap_ADC);
+      cs_params->pins[i] = pins[i];
+    }
   }
+  return 0;
 }
 
+
 extern "C" {
   void ADC1_2_IRQHandler(void)
   {
diff --git a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.h b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.h
index 2298b17c..34fb947e 100644
--- a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.h
+++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.h
@@ -6,13 +6,11 @@
 #if defined(STM32G4xx) && !defined(ARDUINO_B_G431B_ESC1)
 
 #include "stm32g4xx_hal.h"
-#include "../../../../common/foc_utils.h"
-#include "../../../../drivers/hardware_specific/stm32/stm32_mcu.h"
 #include "../stm32_mcu.h"
 #include "stm32g4_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);
+int _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC);
 
 #endif
 
diff --git a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp
index 9be98860..a4d66b60 100644
--- a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp
+++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp
@@ -36,7 +36,7 @@ void* _configureADCLowSide(const void* driver_params, const int pinA, const int
     .pins={(int)NOT_SET, (int)NOT_SET, (int)NOT_SET},
     .adc_voltage_conv = (_ADC_VOLTAGE_G4) / (_ADC_RESOLUTION_G4)
   };
-  _adc_gpio_init(cs_params, pinA,pinB,pinC);
+  if(_adc_gpio_init(cs_params, pinA,pinB,pinC) != 0) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED;
   if(_adc_init(cs_params, (STM32DriverParams*)driver_params) != 0) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED;
   return cs_params;
 }
diff --git a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp
index 8f7889c4..2d5e5661 100644
--- a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp
+++ b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp
@@ -8,6 +8,14 @@
 
 ADC_HandleTypeDef hadc;
 
+/**
+ * Function initializing the ADC and the injected channels for the low-side current sensing
+ * 
+ * @param cs_params - current sense parameters
+ * @param driver_params - driver parameters
+ * 
+ * @return int - 0 if success 
+ */
 int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params)
 {
   ADC_InjectionConfTypeDef sConfigInjected;
@@ -128,6 +136,13 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive
   while(driver_params->timers_handle[tim_num] != NP && tim_num < 6){
     uint32_t trigger_flag = _timerToInjectedTRGO(driver_params->timers_handle[tim_num++]);
     if(trigger_flag == _TRGO_NOT_AVAILABLE) continue; // timer does not have valid trgo for injected channels
+    
+    // check if TRGO used already - if yes use the next timer
+    if((driver_params->timers_handle[tim_num-1]->Instance->CR2 & LL_TIM_TRGO_ENABLE) || // if used for timer sync
+       (driver_params->timers_handle[tim_num-1]->Instance->CR2 & LL_TIM_TRGO_UPDATE)) // if used for ADC sync
+      {
+      continue;
+    }
 
     // if the code comes here, it has found the timer available
     // timer does have trgo flag for injected channels  
@@ -144,6 +159,10 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive
     SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot sync any timer to injected channels!");
 #endif
     return -1;
+  }else{
+#ifdef SIMPLEFOC_STM32_DEBUG
+    SIMPLEFOC_DEBUG("STM32-CS: Using timer: ", stm32_getTimerNumber(cs_params->timer_handle->Instance));
+#endif
   }
 
 
@@ -183,21 +202,35 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive
   return 0;
 }
 
-void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC)
+/**
+ * Function to initialize the ADC GPIO pins
+ * 
+ * @param cs_params current sense parameters
+ * @param pinA pin number for phase A
+ * @param pinB pin number for phase B
+ * @param pinC pin number for phase C
+ * @return int 0 if success, -1 if error
+ */
+int _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;
+  int pins[3] = {pinA, pinB, pinC};
+  const char* port_names[3] = {"A", "B", "C"};
+  for(int i=0; i<3; i++){
+    if(_isset(pins[i])){
+      // check if pin is an analog pin
+      if(pinmap_peripheral(analogInputToPinName(pins[i]), PinMap_ADC) == NP){
+#ifdef SIMPLEFOC_STM32_DEBUG
+        SimpleFOCDebug::print("STM32-CS: ERR: Pin ");
+        SimpleFOCDebug::print(port_names[i]);
+        SimpleFOCDebug::println(" does not belong to any ADC!");
+#endif
+        return -1;
+      }
+      pinmap_pinout(analogInputToPinName(pins[i]), PinMap_ADC);
+      cs_params->pins[i] = pins[i];
+    }
   }
+  return 0;
 }
 
 extern "C" {
diff --git a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.h b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.h
index 0317b74b..2004ff69 100644
--- a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.h
+++ b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.h
@@ -6,13 +6,11 @@
 #if defined(STM32L4xx) 
 
 #include "stm32l4xx_hal.h"
-#include "../../../../common/foc_utils.h"
-#include "../../../../drivers/hardware_specific/stm32/stm32_mcu.h"
 #include "../stm32_mcu.h"
 #include "stm32l4_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);
+int _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC);
 
 #endif
 
diff --git a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp
index ed55b482..4ddc5484 100644
--- a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp
+++ b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp
@@ -35,7 +35,7 @@ void* _configureADCLowSide(const void* driver_params, const int pinA, const int
     .pins={(int)NOT_SET, (int)NOT_SET, (int)NOT_SET},
     .adc_voltage_conv = (_ADC_VOLTAGE_L4) / (_ADC_RESOLUTION_L4)
   };
-  _adc_gpio_init(cs_params, pinA,pinB,pinC);
+  if(_adc_gpio_init(cs_params, pinA,pinB,pinC) != 0) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED;
   if(_adc_init(cs_params, (STM32DriverParams*)driver_params) != 0) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED;
   return cs_params;
 }