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)
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,
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;
+}
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
}