displayd: Fix some error and rework throttling

diff --git a/src/displayd/displayd.c b/src/displayd/displayd.c
index f2654111fb55f9ed9bb0b5685281d765a695eecb..4c2f920daac04ceaf337fea25ce8ae528f5a0057 100644 (file)
--- a/src/displayd/displayd.c
+++ b/src/displayd/displayd.c
@@ -21,6 +21,7 @@
 #include <unistd.h>
 #include <stdlib.h>
 #include <signal.h>
+#include <stdbool.h>
 
 struct sercom_data* sercom;
 int interrupt = 0;
@@ -61,13 +62,18 @@ int timespec_subtract (struct timespec *result,
   return x->tv_sec < y->tv_sec;
 }
 
-int miliseconds_since(struct timespec *t)
+bool miliseconds_elapsed_since(struct timespec *t, unsigned miliseconds)
 {
        struct timespec now, diff;
+       unsigned long long elapsed;
+
+       if (t->tv_sec == 0 && t->tv_nsec == 0)
+               return true;    /* Always elapsed after program start */
 
        clock_gettime(CLOCK_MONOTONIC, &now);
-       timespec_subtract(&diff, t, &now);
-       return diff.tv_sec * 1000 + diff.tv_nsec/1000000;       
+       timespec_subtract(&diff, &now, t);
+       elapsed = diff.tv_sec * 1000 + diff.tv_nsec/1000000;
+       return elapsed > miliseconds;
 }
 
 
@@ -90,8 +96,8 @@ void rcv_pwr_voltage_cb (const ORTERecvInfo *info, void *vinstance,
                        break;
        }
        if (status != last_status ||
-           miliseconds_since(&last_sent) > 1000) {
-               uoled_display_status(PWR, STATUS_OK);
+           miliseconds_elapsed_since(&last_sent, 1000)) {
+               uoled_display_status(PWR, status);
                clock_gettime(CLOCK_MONOTONIC, &last_sent);
        }
        last_status = status;
@@ -114,8 +120,8 @@ void rcv_odo_data_cb(const ORTERecvInfo *info, void *vinstance,
        }
        /* Neni potreba aktualizovat stav na displeji 25x za sekundu */
        if (status != last_status ||
-           miliseconds_since(&last_sent) > 1000) {
-               uoled_display_status(ODO, STATUS_OK);
+           miliseconds_elapsed_since(&last_sent, 1000)) {
+               uoled_display_status(ODO, status);
                clock_gettime(CLOCK_MONOTONIC, &last_sent);
        }
        last_status = status;
@@ -137,8 +143,8 @@ void rcv_motion_irc_cb(const ORTERecvInfo *info, void *vinstance,
        }
        /* Neni potreba aktualizovat stav na displeji 25x za sekundu */
        if (status != last_status ||
-           miliseconds_since(&last_sent) > 1000) {
-               uoled_display_status(MOT, STATUS_OK);
+           miliseconds_elapsed_since(&last_sent, 1000)) {
+               uoled_display_status(MOT, status);
                clock_gettime(CLOCK_MONOTONIC, &last_sent);
        }
        last_status = status;
@@ -160,8 +166,8 @@ void rcv_camera_result_cb(const ORTERecvInfo *info, void *vinstance,
                        break;
        }
        if (status != last_status ||
-           miliseconds_since(&last_sent) > 1000) {
-               uoled_display_status(CAM, STATUS_OK);
+           miliseconds_elapsed_since(&last_sent, 1000)) {
+               uoled_display_status(CAM, status);
                clock_gettime(CLOCK_MONOTONIC, &last_sent);
        }
        last_status = status;
@@ -183,8 +189,8 @@ void rcv_hokuyo_scan_cb(const ORTERecvInfo *info, void *vinstance,
                        break;
        }
        if (status != last_status ||
-           miliseconds_since(&last_sent) > 1000) {
-               uoled_display_status(HOK, STATUS_OK);
+           miliseconds_elapsed_since(&last_sent, 1000)) {
+               uoled_display_status(HOK, status);
                clock_gettime(CLOCK_MONOTONIC, &last_sent);
        }
        last_status = status;
@@ -199,7 +205,6 @@ void rcv_est_pos_best_cb (const ORTERecvInfo *info, void *vinstance,
        struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
         switch (info->status) {
                case NEW_DATA:
-                       uoled_display_position(orte_data->est_pos_best.x, orte_data->est_pos_best.y, orte_data->est_pos_best.phi);
                        status = STATUS_OK;
                        break;
                case DEADLINE:
@@ -207,8 +212,10 @@ void rcv_est_pos_best_cb (const ORTERecvInfo *info, void *vinstance,
                        break;
        }
        if (status != last_status ||
-           miliseconds_since(&last_sent) > 1000) {
-               uoled_display_status(ODO, STATUS_OK);
+           miliseconds_elapsed_since(&last_sent, 100)) {
+               uoled_display_status(APP, status);
+               if (status == STATUS_OK)
+                       uoled_display_position(orte_data->est_pos_best.x, orte_data->est_pos_best.y, orte_data->est_pos_best.phi);
                clock_gettime(CLOCK_MONOTONIC, &last_sent);
        }
        last_status = status;
@@ -233,7 +240,7 @@ void rcv_fsm_main_cb(const ORTERecvInfo *info, void *vinstance,
                         break;
         }
        if (strcmp(status,last_status) != 0 ||
-           miliseconds_since(&last_sent) > 1000) {
+           miliseconds_elapsed_since(&last_sent, 1000)) {
                 uoled_display_fsm(FSM_MAIN, status);
                clock_gettime(CLOCK_MONOTONIC, &last_sent);
        }
@@ -257,7 +264,7 @@ void rcv_fsm_act_cb(const ORTERecvInfo *info, void *vinstance,
                         break;
         }
        if (strcmp(status,last_status) != 0 ||
-           miliseconds_since(&last_sent) > 1000) {
+           miliseconds_elapsed_since(&last_sent, 1000)) {
                 uoled_display_fsm(FSM_ACT, status);
                clock_gettime(CLOCK_MONOTONIC, &last_sent);
        }
@@ -281,7 +288,7 @@ void rcv_fsm_motion_cb(const ORTERecvInfo *info, void *vinstance,
                         break;
         }
        if (strcmp(status,last_status) != 0 ||
-           miliseconds_since(&last_sent) > 1000) {
+           miliseconds_elapsed_since(&last_sent, 1000)) {
                 uoled_display_fsm(FSM_MOVE, status);
                clock_gettime(CLOCK_MONOTONIC, &last_sent);
        }