displayd: Do not refresh the display too fast

TODO do the same change in the other callbacks.

diff --git a/src/displayd/Makefile.omk b/src/displayd/Makefile.omk
index c7c6f8225768824d88c21458481cf3f7f9edfdab..70d1a4b56c53471499766c2e8adcae7fb708396f 100644 (file)
--- a/src/displayd/Makefile.omk
+++ b/src/displayd/Makefile.omk
@@ -2,7 +2,7 @@
 
 bin_PROGRAMS += displayd
 displayd_SOURCES = displayd.c oledlib.c uoled.c
-lib_LOADLIBES = roboorte robottype orte pthread sercom
+lib_LOADLIBES = roboorte robottype orte pthread sercom rt
 
 
 test_PROGRAMS  = test_uoled

diff --git a/src/displayd/displayd.c b/src/displayd/displayd.c
index 46e0f9b0b26a1235033d057de9f0ec27ede1f116..f1fdb9cc5c60b8d4a246276457af7c5fa06db81c 100644 (file)
--- a/src/displayd/displayd.c
+++ b/src/displayd/displayd.c
@@ -22,6 +22,47 @@
 
 struct sercom_data* sercom;
 
+/* Subtract the `struct timespec' values X and Y,
+   storing the result in RESULT (result = x - y).
+   Return 1 if the difference is negative, otherwise 0.  */
+
+int
+timespec_subtract (struct timespec *result,
+                  struct timespec *x,
+                  struct timespec *y)
+{
+  /* Perform the carry for the later subtraction by updating Y. */
+  if (x->tv_nsec < y->tv_nsec) {
+    int num_sec = (y->tv_nsec - x->tv_nsec) / 1000000000 + 1;
+    y->tv_nsec -= 1000000000 * num_sec;
+    y->tv_sec += num_sec;
+  }
+  if (x->tv_nsec - y->tv_nsec > 1000000000) {
+    int num_sec = (x->tv_nsec - y->tv_nsec) / 1000000000;
+    y->tv_nsec += 1000000000 * num_sec;
+    y->tv_sec -= num_sec;
+  }
+
+  /* Compute the time remaining to wait.
+     `tv_nsec' is certainly positive. */
+  result->tv_sec = x->tv_sec - y->tv_sec;
+  result->tv_nsec = x->tv_nsec - y->tv_nsec;
+
+  /* Return 1 if result is negative. */
+  return x->tv_sec < y->tv_sec;
+}
+
+
+int miliseconds_since(struct timespec *t)
+{
+       struct timespec now, diff;
+
+       clock_gettime(CLOCK_MONOTONIC, &now);
+       timespec_subtract(&diff, t, &now);
+       return diff.tv_sec * 1000 + diff.tv_nsec/1000000;       
+}
+
+
 void rcv_pwr_voltage_cb (const ORTERecvInfo *info, void *vinstance,
                         void *recvCallBackParam)
 {
@@ -32,7 +73,6 @@ void rcv_pwr_voltage_cb (const ORTERecvInfo *info, void *vinstance,
                        uoled_display_status(PWR, STATUS_OK);
                        break;
                case DEADLINE:
-                       printf("ORTE deadline occurred - PWR_VOLTAGE\n");
                        uoled_display_status(PWR, STATUS_FAILED);
                        break;
        }
@@ -41,16 +81,27 @@ void rcv_pwr_voltage_cb (const ORTERecvInfo *info, void *vinstance,
 void rcv_odo_data_cb(const ORTERecvInfo *info, void *vinstance,
                         void *recvCallBackParam)
 {
+       UDE_hw_status_t status = STATUS_FAILED;
+       static UDE_hw_status_t last_status;
+       static struct timespec last_sent;
+       
        //struct odo_data_type *instance = (struct odo_data_type *)vinstance;
        switch (info->status) {
                case NEW_DATA:
-                       uoled_display_status(HOK, STATUS_OK);
+                       status = STATUS_OK;
                        break;
                case DEADLINE:
-                       printf("ORTE deadline occurred - ODO_DATA\n");
-                       uoled_display_status(HOK, STATUS_FAILED);
+                       status = STATUS_FAILED;
                        break;
        }
+#warning TODO predelat vsechny statusy callbacku podle teto funkce
+       /* Neni potreba aktualizovat stav na displeji 25x za sekundu */
+       if (status != last_status ||
+           miliseconds_since(&last_sent) > 1000) {
+               uoled_display_status(HOK, STATUS_OK);
+               clock_gettime(CLOCK_MONOTONIC, &last_sent);
+       }
+       last_status = status;
 }
 
 void rcv_motion_irc_cb(const ORTERecvInfo *info, void *vinstance,