displayd: Fix status handling for ODO and HOK

diff --git a/src/displayd/displayd.c b/src/displayd/displayd.c
index ca8126fe5d3c83f679f4a1b41c5695acec01f022..37a57a5ba4ac37c7b62c9eb6dd3c56e5997d2340 100644 (file)
--- a/src/displayd/displayd.c
+++ b/src/displayd/displayd.c
@@ -106,7 +106,7 @@ 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(HOK, STATUS_OK);
+               uoled_display_status(ODO, STATUS_OK);
                clock_gettime(CLOCK_MONOTONIC, &last_sent);
        }
        last_status = status;
@@ -161,15 +161,24 @@ void rcv_camera_result_cb(const ORTERecvInfo *info, void *vinstance,
 void rcv_hokuyo_scan_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 hokuyo_scan_type *instance = (struct hokuyo_scan_type *)vinstance;
        switch (info->status) {
                case NEW_DATA:
-                       uoled_display_status(HOK, STATUS_OK);
+                       status = STATUS_OK;
                        break;
                case DEADLINE:
-                       uoled_display_status(HOK, STATUS_FAILED);
+                       status = STATUS_FAILED;
                        break;
        }
+       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_ref_pos_cb (const ORTERecvInfo *info, void *vinstance,