Unify ORTE initialization

[eurobot/public.git] / src / displayd / displayd.c

diff --git a/src/displayd/displayd.c b/src/displayd/displayd.c
index c4be3d9ad0c2a99102b5a1a08a8d8d5f3eb10085..1c3cd6b09d8dd6787b8a6d1b5ef13ab5523495a9 100644 (file)
--- a/src/displayd/displayd.c
+++ b/src/displayd/displayd.c
@@ -236,10 +236,14 @@ void rcv_camera_result_cb(const ORTERecvInfo *info, void *vinstance,
        UDE_hw_status_t status = STATUS_FAILED;
        static UDE_hw_status_t last_status;
        static struct timespec last_sent;
-       //struct camera_result_type *instance = (struct camera_result_type *)vinstance;
+       struct camera_result_type *instance = (struct camera_result_type *)vinstance;
+       char s, c;
        switch (info->status) {
                case NEW_DATA:
-                       status = STATUS_OK;
+                       if (instance->error == 0)
+                               status = STATUS_OK;
+                       else
+                               status = STATUS_WARNING;
                        break;
                case DEADLINE:
                        status = STATUS_FAILED;
@@ -251,6 +255,15 @@ void rcv_camera_result_cb(const ORTERecvInfo *info, void *vinstance,
                clock_gettime(CLOCK_MONOTONIC, &last_sent);
        }
        last_status = status;
+
+       if (status == STATUS_OK) {
+               s = (instance->side >= 0 && instance->side < 10) ?
+                       '0'+instance->side : 'E';
+               c = (instance->center >= 0 && instance->center < 10) ?
+                       '0'+instance->center : 'E';
+       } else
+               s = c = '-';
+       uoled_display_corns(s, c);
 }
 
 void rcv_hokuyo_scan_cb(const ORTERecvInfo *info, void *vinstance,
@@ -376,7 +389,7 @@ void rcv_fsm_motion_cb(const ORTERecvInfo *info, void *vinstance,
 }
 
 
-
+struct robottype_orte_data orte;
 
 int main(int argc, char *argv[])
 {
@@ -385,7 +398,6 @@ int main(int argc, char *argv[])
        int ret;
        //int size;
 
-       struct robottype_orte_data orte;
        char * tty;
 
        signal(SIGINT, sig_handler);
@@ -399,20 +411,18 @@ int main(int argc, char *argv[])
        }
 
        sercom = uoled_sercom_init(tty, NULL);
-       if (strcmp(tty, "/dev/null") != 0 && sercom) {
+       if (strcmp(tty, "/dev/null") == 0 || sercom != NULL) {
        // :wq
                printf("displayd: init serial communication OK\n");
        } else {
                fprintf(stderr, "displayd: init serial communication failed\n");
-#ifdef COMPETITION
+//#ifdef COMPETITION
                /* Sleep in order to that not be respawn too fast by init */
                sleep(3);
                return 1;
-#endif
+//#endif
        }
 
-       orte.strength = 1;
-
        /* orte initialization */
        if(robottype_roboorte_init(&orte)) {
                fprintf(stderr, "displayd: Roboorte initialization failed! Exiting...\n");
@@ -468,13 +478,17 @@ int main(int argc, char *argv[])
                if (strcmp(tty, "/dev/null") != 0) {
                        ret = poll(pfd, 1, 500/*ms*/);
                        if (ret > 0) {
-                               rcmd = uoled_recieve_cmd();
-                               orte.display_cmd.command=rcmd;
-                               if (! rcmd){
-                                       ORTEPublicationSend(orte.publication_display_cmd);
-                                       printf("displayd: publishing command recieved from display [ 0x%2x ]\n", rcmd);
+                               ret = uoled_recieve_cmd(&rcmd);
+/*                             orte.display_cmd.command=rcmd; */
+/*                             if (! rcmd){ */
+/*                                     ORTEPublicationSend(orte.publication_display_cmd); */
+/*                                     printf("displayd: publishing command recieved from display [ 0x%2x ]\n", rcmd); */
 
-                               }
+/*                             } */
+                       }
+                       if (ret < 0) {
+                               perror("Error: read from display");
+                               break;
                        }
                } else {
                        usleep(500/*ms*/*1000);
@@ -492,6 +506,11 @@ int main(int argc, char *argv[])
        printf("displayd: destroying orte instance...\n");
        robottype_roboorte_destroy(&orte);
 
+       if (!interrupt) {
+               printf("Waiting 3 seconds\n");
+               sleep(3);
+       }
+
        printf("displayd: finished!\n");
        return 0;
 }