From ce9f7c8a96464133b041b97a5c430e58117881ab Mon Sep 17 00:00:00 2001 From: Michal Vokac Date: Fri, 30 Dec 2011 16:18:11 +0100 Subject: [PATCH] robofsm: Yellow APP signalization on display when system not ready. When any component not ready for start, put COMPONENT_ERR into est_pos_best which is used in dispalayd for APP status show. --- src/robofsm/robot.h | 2 ++ src/robofsm/robot_orte.c | 10 +++++++++- 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/src/robofsm/robot.h b/src/robofsm/robot.h index 4ff6ffee..8a5e1b5f 100644 --- a/src/robofsm/robot.h +++ b/src/robofsm/robot.h @@ -124,6 +124,8 @@ enum robot_component { ROBOT_COMPONENT_NUMBER }; +#define COMPONENT_ERR 0xffff + /* robot's main data structure */ struct robot { pthread_mutex_t lock; diff --git a/src/robofsm/robot_orte.c b/src/robofsm/robot_orte.c index 90198809..beac2ad0 100644 --- a/src/robofsm/robot_orte.c +++ b/src/robofsm/robot_orte.c @@ -102,7 +102,15 @@ static void send_est_pos_best_cb(const ORTESendInfo *info, void *vinstance, { struct robot_pos_type *instance = (struct robot_pos_type *)vinstance; - robot_get_est_pos(&instance->x, &instance->y, &instance->phi); + if (check_prestart_status()) { + /* if any component not ready, send component error in best position + for display to show APP yellow */ + instance->x = COMPONENT_ERR; + instance->y = COMPONENT_ERR; + instance->phi = COMPONENT_ERR; + } else { + robot_get_est_pos(&instance->x, &instance->y, &instance->phi); + } } void send_dummy_cb(const ORTESendInfo *info, void *vinstance, -- 2.39.2