1 files changed, 19 insertions, 16 deletions
diff --git a/Projects/P-NUCLEO-WB55.Nucleo/Applications/BLE/BLE_HeartRateFreeRTOS_ANCS/Core/Src/freertos_port.c b/Projects/P-NUCLEO-WB55.Nucleo/Applications/BLE/BLE_HeartRateFreeRTOS_ANCS/Core/Src/freertos_port.c
index 1cc430948..2a6bf7141 100644
--- a/Projects/P-NUCLEO-WB55.Nucleo/Applications/BLE/BLE_HeartRateFreeRTOS_ANCS/Core/Src/freertos_port.c
+++ b/Projects/P-NUCLEO-WB55.Nucleo/Applications/BLE/BLE_HeartRateFreeRTOS_ANCS/Core/Src/freertos_port.c
@@ -236,12 +236,17 @@ static void LpTimerStart( uint32_t time_to_sleep )
   uint64_t time;
 
   /* Converts the number of FreeRTOS ticks into hw timer tick */
-  
-  time = (time_to_sleep * 1000 * 1000 );
+
+  time = (time_to_sleep * 1000LL * 1000LL );
   time = time / ( CFG_TS_TICK_VAL * configTICK_RATE_HZ );
-  
+
+  if(time > 0xFFFF0000)
+  {
+    time = 0xFFFF0000; /* maximum value */
+  }
+
   HW_TS_Start(LpTimerContext.LpTimerFreeRTOS_Id, (uint32_t)time);
-  
+
   /**
    * There might be other timers already running in the timer server that may elapse
    * before this one.
@@ -280,28 +285,26 @@ static void LpEnter( void )
 static uint32_t LpGetElapsedTime( void )
 {
   uint64_t val_ticks, time_us, diff_ps;
-  uint32_t LpTimeLeftOnExit;
+  uint32_t LpTimeLeftOnExit, time2_us;
 
-  LpTimeLeftOnExit = HW_TS_RTC_ReadLeftTicksToCount();    
+  LpTimeLeftOnExit = HW_TS_RTC_ReadLeftTicksToCount();
   time_us = (CFG_TS_TICK_VAL) * (uint64_t)(LpTimerContext.LpTimeLeftOnEntry - LpTimeLeftOnExit);
-  
+
   /* Corrects the time precision lost in CFG_TS_TICK_VAL computation */
-  
+
   /* Compute the amount of pico seconds lost at each TS ticks */
   diff_ps = DIVR( ((uint64_t)CFG_RTCCLK_DIV * 1000000 * 1000000), (uint64_t)LSE_VALUE );
   diff_ps -= DIVF( (CFG_RTCCLK_DIV * 1000000), LSE_VALUE ) * 1000000;
-  /* Compute the total amount of time shift */  
+  /* Compute the total amount of time shift */
   diff_ps *= (uint64_t)(LpTimerContext.LpTimeLeftOnEntry - LpTimeLeftOnExit);
-  
+
   /* Save the time shift for next time */
   LpTimerContext.LpTimeDiffVal += diff_ps;
 
-  while(LpTimerContext.LpTimeDiffVal >= (uint64_t)(1000 * 1000))
-  {
-    /* Reports the time difference into returned time elapsed value */
-    time_us++;
-    LpTimerContext.LpTimeDiffVal -= (uint64_t)(1000 * 1000);
-  }
+  /* Reports the time difference into returned time elapsed value */
+  time2_us = LpTimerContext.LpTimeDiffVal / (1000ULL * 1000ULL);
+  LpTimerContext.LpTimeDiffVal = LpTimerContext.LpTimeDiffVal - time2_us * (1000ULL * 1000ULL);
+  time_us += time2_us;
 
   /* Convert uS time into OS ticks */
   val_ticks = time_us * configTICK_RATE_HZ;