[SVE2] Add timer compensation in level control cluster

Signed-off-by: Andrei Menzopol <[email protected]>

src/app/clusters/level-control/level-control.cpp

@@ -110,6 +110,8 @@ typedef struct
 
 static EmberAfLevelControlState stateTable[kLevelControlStateTableSize];
 
+static chip::System::Clock::Timestamp nextTimestamp;
+
 static EmberAfLevelControlState * getState(EndpointId endpoint);
 
 static EmberAfStatus moveToLevelHandler(EndpointId endpoint, CommandId commandId, uint8_t level,
@@ -139,6 +141,27 @@ static void timerCallback(System::Layer *, void * callbackContext)
     emberAfLevelControlClusterServerTickCallback(static_cast<EndpointId>(reinterpret_cast<uintptr_t>(callbackContext)));
 }
 
+static uint32_t calculateNextTimestampMs(int32_t delayMs)
+{
+	 int32_t waitTime, latency;
+	 const chip::System::Clock::Timestamp currentTime = chip::System::SystemClock().GetMonotonicTimestamp();
+
+	 latency = (int32_t)currentTime.count() - (int32_t)nextTimestamp.count();
+
+	 if(latency > delayMs)
+	 {
+		 waitTime = 0;
+	 }
+	 else
+	 {
+		 waitTime = delayMs - latency;
+	 }
+
+	 nextTimestamp += chip::System::Clock::Milliseconds32(delayMs);
+
+	 return (uint32_t)waitTime;
+}
+
 static void schedule(EndpointId endpoint, uint32_t delayMs)
 {
     DeviceLayer::SystemLayer().StartTimer(chip::System::Clock::Milliseconds32(delayMs), timerCallback,
@@ -271,7 +294,7 @@ void emberAfLevelControlClusterServerTickCallback(EndpointId endpoint)
     else
     {
         writeRemainingTime(endpoint, static_cast<uint16_t>(state->transitionTimeMs - state->elapsedTimeMs));
-        schedule(endpoint, state->eventDurationMs);
+        schedule(endpoint, calculateNextTimeMs(state->eventDurationMs));
     }
 }
 
@@ -702,7 +725,8 @@ static EmberAfStatus moveToLevelHandler(EndpointId endpoint, CommandId commandId
     state->storedLevel = storedLevel;
 
     // The setup was successful, so mark the new state as active and return.
-    schedule(endpoint, state->eventDurationMs);
+    nextTimestamp = chip::System::SystemClock().GetMonotonicTimestamp();
+    schedule(endpoint, calculateNextTimeMs(state->eventDurationMs));
     status = EMBER_ZCL_STATUS_SUCCESS;
 
     if (commandId == Commands::MoveToLevelWithOnOff::Id)
@@ -828,7 +852,8 @@ static void moveHandler(EndpointId endpoint, CommandId commandId, uint8_t moveMo
     state->storedLevel = INVALID_STORED_LEVEL;
 
     // The setup was successful, so mark the new state as active and return.
-    schedule(endpoint, state->eventDurationMs);
+    nextTimestamp = chip::System::SystemClock().GetMonotonicTimestamp();
+    schedule(endpoint, calculateNextTimeMs(state->eventDurationMs));
     status = EMBER_ZCL_STATUS_SUCCESS;
 
 send_default_response:
@@ -954,7 +979,8 @@ static void stepHandler(EndpointId endpoint, CommandId commandId, uint8_t stepMo
     state->storedLevel = INVALID_STORED_LEVEL;
 
     // The setup was successful, so mark the new state as active and return.
-    schedule(endpoint, state->eventDurationMs);
+    nextTimestamp = chip::System::SystemClock().GetMonotonicTimestamp();
+    schedule(endpoint, calculateNextTimeMs(state->eventDurationMs));
     status = EMBER_ZCL_STATUS_SUCCESS;
 
 send_default_response: