robofsm: Update camera component status.

diff --git a/src/robofsm/robot_orte.c b/src/robofsm/robot_orte.c
index 28bd43228a6a03f342b23697e1f87ed0b3d5e409..49831f7246e4175ee9543e773b186e79d916c1a9 100644 (file)
--- a/src/robofsm/robot_orte.c
+++ b/src/robofsm/robot_orte.c
@@ -329,11 +329,16 @@ void rcv_camera_result_cb(const ORTERecvInfo *info, void *vinstance,
 
        switch (info->status) {
                case NEW_DATA: {
-                        if (instance->data_valid && instance->data_valid != last_response) {
+                        if (instance->error) {
+                                robot.status[COMPONENT_CAMERA] = STATUS_FAILED;
+                        } else if (instance->data_valid && instance->data_valid != last_response) {
                                 ROBOT_LOCK(camera_result);
                                 robot.target_valid = instance->target_valid;
                                 ROBOT_UNLOCK(camera_result);
                                 FSM_SIGNAL(MAIN, EV_CAMERA_DONE, NULL);
+                                printf("robot_orte: valid: %d\n", instance->target_valid);
+                                
+                                robot.status[COMPONENT_CAMERA] = STATUS_OK;
                         }
                         last_response = instance->data_valid;
                        break;