displayd: Show recognized corns

diff --git a/src/displayd/displayd.c b/src/displayd/displayd.c
index 157d8b73b5c61343ad599b4f26b7b74989778868..6c258d1855d63de9715765545119d153764d54c9 100644 (file)
--- a/src/displayd/displayd.c
+++ b/src/displayd/displayd.c
@@ -237,6 +237,7 @@ void rcv_camera_result_cb(const ORTERecvInfo *info, void *vinstance,
        static UDE_hw_status_t last_status;
        static struct timespec last_sent;
        struct camera_result_type *instance = (struct camera_result_type *)vinstance;
+       char s, c;
        switch (info->status) {
                case NEW_DATA:
                        if (instance->error == 0)
@@ -254,7 +255,15 @@ void rcv_camera_result_cb(const ORTERecvInfo *info, void *vinstance,
                clock_gettime(CLOCK_MONOTONIC, &last_sent);
        }
        last_status = status;
-       /* TODO: Show recognized corns  */
+
+       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,

diff --git a/src/displayd/uoled.c b/src/displayd/uoled.c
index 1419ffc90de92cc8c3fbeae4bd2f5df4911f641e..183f0c508581f4d219ed315df806f60c6f217081 100644 (file)
--- a/src/displayd/uoled.c
+++ b/src/displayd/uoled.c
@@ -239,3 +239,18 @@ int uoled_display_position(double x, double y, double phi)
        
        return 0;
 }
+
+int uoled_display_corns(int side, int center)
+{
+       int ret;
+       uint8_t msg[MGS_LENGTH_DISPLAY_COLOR];
+       msg[0]=ID_DISPLAY_CORNS;
+       msg[1]=side;
+       msg[2]=center;
+
+       ret = uoled_write_cmd(msg, MGS_LENGTH_DISPLAY_CORNS);
+       if(ret)
+               return -1;
+
+       return 0;
+}

diff --git a/src/displayd/uoled.h b/src/displayd/uoled.h
index 4fd8f20f2cd28031c88beb17d23bcf135cc02e2a..e22b03852384183d3ce6cad57096ecd03d9aefe6 100644 (file)
--- a/src/displayd/uoled.h
+++ b/src/displayd/uoled.h
@@ -332,7 +332,7 @@ int uoled_display_voltage(double voltage33, double voltage50,
 int uoled_display_position(double x, double y, double phi);
 
 int uoled_display_color(int color);
-
+int uoled_display_corns(int side, int center);
 
 #ifdef __cplusplus
 }