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.

diff --git a/src/robofsm/robot.h b/src/robofsm/robot.h
index 4ff6ffeef781ae0488e0a113f8eff97376c16880..8a5e1b5f6c3a43cfaa6187bebe3da254ce344ec6 100644 (file)
--- 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 90198809171b15bedf8443301600f64da0178585..beac2ad0e30b22eb8afa692ff06506dcecc71a4a 100644 (file)
--- 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,