Adjust displayapp delay to compensate time spent (#482)

1 files changed, 9 insertions, 2 deletions

diff --git a/src/displayapp/DisplayApp.cpp b/src/displayapp/DisplayApp.cpp
index 04ebd2d..6d66afe 100644
--- a/src/displayapp/DisplayApp.cpp
+++ b/src/displayapp/DisplayApp.cpp
@@ -114,6 +114,7 @@ uint32_t count = 0;
 bool toggle = true;
 void DisplayApp::Refresh() {
   TickType_t queueTimeout;
+  TickType_t delta;
   switch (state) {
     case States::Idle:
       IdleState();
@@ -121,7 +122,11 @@ void DisplayApp::Refresh() {
       break;
     case States::Running:
       RunningState();
-      queueTimeout = 20;
+      delta = xTaskGetTickCount() - lastWakeTime;
+      if (delta > 20) {
+        delta = 20;
+      }
+      queueTimeout = 20 - delta;
       break;
     default:
       queueTimeout = portMAX_DELAY;
@@ -129,7 +134,9 @@ void DisplayApp::Refresh() {
   }
 
   Messages msg;
-  if (xQueueReceive(msgQueue, &msg, queueTimeout)) {
+  bool messageReceived = xQueueReceive(msgQueue, &msg, queueTimeout);
+  lastWakeTime = xTaskGetTickCount();
+  if (messageReceived) {
     switch (msg) {
       case Messages::GoToSleep:
         brightnessController.Backup();