vidle: Perform a new command even if the previous one was not finished

diff --git a/src/eb_vidle/fsm_vidle.c b/src/eb_vidle/fsm_vidle.c
index f388610a034374bb5a000246e44d9bebbf4021df..c9b5cd47fdd37889801fa6a02d156d62ddd94f59 100644 (file)
--- a/src/eb_vidle/fsm_vidle.c
+++ b/src/eb_vidle/fsm_vidle.c
@@ -112,6 +112,10 @@ static void move(struct fsm *fsm, enum event event)
                        fsm->req_pos = fsm->start_pos;
                break;
        case EVENT_DO:
+               if (fsm->can_req_position != fsm->req_target) {
+                       fsm->flags |= CAN_VIDLE_TIMEOUT;
+                       fsm->current_state = wait_for_cmd;
+               }
                if(fsm->req_spd != 0 && counter++ >= 10)
                {
                        counter = 0;
@@ -122,7 +126,7 @@ static void move(struct fsm *fsm, enum event event)
                        }
                }
                if (timer_msec - fsm->time_start > (fsm->req_spd == 0 ? 1000 : 3000)) {
-                       fsm->flags = CAN_VIDLE_TIMEOUT;
+                       fsm->flags |= CAN_VIDLE_TIMEOUT;
                        fsm->can_response = fsm->req_target;
                        fsm->current_state = wait_for_cmd;
                        fsm->req_pos = fsm->act_pos;