3 files changed, 13 insertions, 4 deletions
diff --git a/os/hal/include/icu.h b/os/hal/include/icu.h
index 0b85aa97b..007226616 100644
--- a/os/hal/include/icu.h
+++ b/os/hal/include/icu.h
@@ -93,7 +93,7 @@ typedef void (*icucallback_t)(ICUDriver *icup);
 } while (0)
 
 /**
- * @brief   Waits for the first cycle activation edge.
+ * @brief   Waits for the next cycle activation edge.
  * @details The function waits for the next PWM input activation front then
  *          brings the driver in the @p ICU_ACTIVE state.
  * @note    If notifications are enabled then the transition to the
diff --git a/os/hal/ports/STM32/LLD/TIMv1/icu_lld.c b/os/hal/ports/STM32/LLD/TIMv1/icu_lld.c
index 6ee7ff293..d8bc2e84c 100644
--- a/os/hal/ports/STM32/LLD/TIMv1/icu_lld.c
+++ b/os/hal/ports/STM32/LLD/TIMv1/icu_lld.c
@@ -615,7 +615,7 @@ void icu_lld_start_capture(ICUDriver *icup) {
 }
 
 /**
- * @brief   Waits for the first cycle activation edge.
+ * @brief   Waits for the next cycle activation edge.
  * @details The function waits for the next PWM input activation front then
  *          brings the driver in the @p ICU_ACTIVE state.
  * @note    If notifications are enabled then the transition to the
@@ -629,10 +629,18 @@ void icu_lld_start_capture(ICUDriver *icup) {
 void icu_lld_wait_capture(ICUDriver *icup) {
 
   if (icup->config->channel == ICU_CHANNEL_1) {
+    /* Resetting capture flag.*/
+    icup->tim->SR &= ~STM32_TIM_SR_CC1IF;
+
+    /* Waiting for an edge.*/
     while ((icup->tim->SR & STM32_TIM_SR_CC1IF) == 0)
       ;
   }
   else {
+    /* Resetting capture flag.*/
+    icup->tim->SR &= ~STM32_TIM_SR_CC2IF;
+
+    /* Waiting for an edge.*/
     while ((icup->tim->SR & STM32_TIM_SR_CC2IF) == 0)
       ;
   }
diff --git a/os/hal/src/icu.c b/os/hal/src/icu.c
index fcccab727..298e1cb28 100644
--- a/os/hal/src/icu.c
+++ b/os/hal/src/icu.c
@@ -133,7 +133,7 @@ void icuStartCapture(ICUDriver *icup) {
 }
 
 /**
- * @brief   Waits for the first cycle activation edge.
+ * @brief   Waits for the next cycle activation edge.
  * @details The function waits for the next PWM input activation front then
  *          brings the driver in the @p ICU_ACTIVE state.
  * @note    If notifications are enabled then the transition to the
@@ -148,7 +148,8 @@ void icuWaitCapture(ICUDriver *icup) {
   osalDbgCheck(icup != NULL);
 
   osalSysLock();
-  osalDbgAssert(icup->state == ICU_WAITING, "invalid state");
+  osalDbgAssert((icup->state == ICU_WAITING) || (icup->state == ICU_ACTIVE),
+                "invalid state");
   icuWaitCaptureI(icup);
   osalSysUnlock();
 }