robofsm: Do not send EV_VIDLE_DONE multiple times

If vidle reports timeout, EV_VIDLE_DONE was sent with every received
message. Now we send the event only when timeout changes from 0 to 1.

diff --git a/src/robofsm/robot_orte.c b/src/robofsm/robot_orte.c
index c83b6149916edec9e96b023d617a9925933ff4e8..6d180d9d7b50cc109fbb5d0f89aca6770ca71185 100644 (file)
--- a/src/robofsm/robot_orte.c
+++ b/src/robofsm/robot_orte.c
@@ -350,6 +350,7 @@ void rcv_vidle_status_cb(const ORTERecvInfo *info, void *vinstance,
                            void *recvCallBackParam)
 {
        struct vidle_status_type *instance = (struct vidle_status_type *)vinstance;
+       static bool last_timeout = false;
        switch (info->status) {
                case NEW_DATA:
                        // new data arrived and requested position equals actual position
@@ -358,10 +359,11 @@ void rcv_vidle_status_cb(const ORTERecvInfo *info, void *vinstance,
                        else
                                robot.status[COMPONENT_VIDLE]=STATUS_WARNING;
 
-                       if (instance->response == act_vidle_get_last_reqest() ||
-                           instance->flags & CAN_VIDLE_TIMEOUT) {
+                       if ((instance->response == act_vidle_get_last_reqest()) ||
+                           (instance->flags & CAN_VIDLE_TIMEOUT && ! last_timeout)) {
                                FSM_SIGNAL(MAIN, EV_VIDLE_DONE, NULL);
                        }
+                       last_timeout = instance->flags & CAN_VIDLE_TIMEOUT;
                        break;
                case DEADLINE:
                        robot.status[COMPONENT_VIDLE]=STATUS_FAILED;