fsm.c

Go to the documentation of this file.

/* Copyright: (c) 2007 DCE Eurobot Dragon Team
 *            CTU FEE - Department of Control Engineering
 * Contacts: Tran Duy Khanh <trandk1@fel.cvut.cz>
 * License: GNU GPL v.2
 */

#include "fsm.h"
#include <stdio.h>

const char *fsm_common_event_str(enum fsm_common_events ev)
{
#define E(name) case EV_##name: return "EV_" #name;
        switch (ev) {
                E(NOEVENT);
                E(INIT);
                E(EXIT);
                E(STATE_ENTERED);
                E(RETURN);
                E(TIMEOUT);
                E(TIMER);
        }
#undef E
        return NULL;
};

int fsm_nop_state(struct robo_fsm *fsm)
{
        return RC_WAIT;
}

static void gettimeofday_ts(struct timespec *ts)
{
        struct timeval tv;
        gettimeofday(&tv, NULL);
        /* Convert from timeval to timespec */
        ts->tv_sec = tv.tv_sec;
        ts->tv_nsec = tv.tv_usec * 1000;
}

void __fsm_timespec_add_ms(struct timespec *ts, struct timespec *now, long ms)
{
        struct timespec __now;
        if (!now) {
                gettimeofday_ts(&__now);
                now = &__now;
        }
        ts->tv_sec  = now->tv_sec + ms/1000;
        /* usec overflow hack */
        if ((now->tv_nsec+(ms%1000)*1000000)>1000000000L)
                ts->tv_sec++;
        ts->tv_nsec = (now->tv_nsec + (ms%1000)*1000000)%1000000000L;
}

void __fsm_timespec_invalidate(struct timespec *ts)
{
        ts->tv_sec = 0;
        ts->tv_nsec = 0;
}

static inline int timespec_valid(struct timespec *ts)
{
        return (ts->tv_sec != 0 || ts->tv_nsec != 0);
}

static inline int timespec_cmp(struct timespec *ts1, struct timespec *ts2)
{
        if (ts1->tv_sec < ts2->tv_sec)
                return -1;
        else if (ts1->tv_sec > ts2->tv_sec)
                return +1;
        else if (ts1->tv_nsec < ts2->tv_nsec)
                return -1;
        else if (ts1->tv_nsec > ts2->tv_nsec)
                return +1;
        else
                return 0;
}

static fsm_event
sem_wait_checked(struct robo_fsm *fsm)
{
        int ret;
        fsm_event event;
        do {
                ret = sem_wait(&fsm->event_sent);
                if (ret != 0) {
                        if (errno == EINTR)
                                continue;
                        perror("sem_wait");
                        continue;
                }
        } while (0);
        event = fsm->sent_event;
        sem_post(&fsm->event_noticed);
        return event;
}

static fsm_event
sem_timedwait_checked(sem_t *sem, struct timespec *abs_timeout,
                      struct robo_fsm *fsm, enum fsm_common_events timeout_event)
{
        int ret;
        fsm_event event;
        do {
                ret = sem_timedwait(sem, abs_timeout);
                if (ret != 0) {
                        switch (errno) {
                                case ETIMEDOUT:
                                        fsm->now = *abs_timeout;
                                        __fsm_timespec_invalidate(abs_timeout);
                                        event = timeout_event;
                                        break;
                                case EINTR:
                                        continue;
                                default:
                                        gettimeofday_ts(&fsm->now);
                                        perror("sem_timedwait");
                                        fprintf(stderr, "TIMER ERROR %d timer=%ld+%.3f\n", ret, fsm->timer.tv_sec, (double)fsm->timer.tv_nsec/1000000000.0);
                                        fprintf(stderr, "TIMER ERROR %d   now=%ld+%.3f\n\n\n", ret, fsm->now.tv_sec, (double)fsm->now.tv_nsec/1000000000.0);
                                        __fsm_timespec_invalidate(&fsm->timer);
                                        continue;
                        }
                } else {
                        gettimeofday_ts(&fsm->now);
                        event = fsm->sent_event;
                        sem_post(&fsm->event_noticed);
                }
        } while (0);
        return event;
}

static inline fsm_event
wait_for_event(struct robo_fsm *fsm)
{
        fsm_event event;
        if (!(timespec_valid(&fsm->timeout) || timespec_valid(&fsm->timer))) {
                /* no timeout set */
                event = sem_wait_checked(fsm);
                gettimeofday_ts(&fsm->now);
        } else {
                /* wait for timeout or timer */
                if (timespec_valid(&fsm->timer) &&
                    (!timespec_valid(&fsm->timeout) ||
                     timespec_cmp(&fsm->timer, &fsm->timeout) <= 0)) {
                        /* wait for timer */
                        event = sem_timedwait_checked(&fsm->event_sent, &fsm->timer, fsm, EV_TIMER);
                } else {
                        /* wait for timeout */
                        event = sem_timedwait_checked(&fsm->event_sent, &fsm->timeout, fsm, EV_TIMEOUT);
                }
                __fsm_timespec_invalidate(&fsm->timeout);
        }
        return event;
}

static inline void
handle_event(struct robo_fsm *fsm, fsm_event event)
{
        int ret_code;

        fsm->event = event;     /* Set what to handle */

        /* This does all the magic */
        if (fsm->state)
                while ((ret_code = fsm->state(fsm)) != RC_WAIT);
        else
            fprintf(stderr, "FSM %s: fnc->act is NULL!!!\n", fsm->debug_name);
        fsm->event = EV_NOEVENT;
}

static void *
fsm_thread_routine(void *arg)
{
        struct robo_fsm *fsm = (struct robo_fsm *)arg;
        fsm_event event = EV_NOEVENT;

        while(event != EV_EXIT) {
                event = wait_for_event(fsm);
                handle_event(fsm, event);
        }

        return NULL;
}

void fsm_init(struct robo_fsm *fsm, char *debug_name)
{

        /* Execute EV_INIT immediately after start. */
        fsm->sent_event = EV_INIT;
        sem_init(&fsm->event_sent, 0, 1);

        /* Until EV_INIT is processed, FSM cannot be signaled. See fsm_signal. */
        sem_init(&fsm->event_noticed, 0, 0);

        __fsm_timespec_invalidate(&fsm->timeout);
        __fsm_timespec_invalidate(&fsm->timer);

        /* initialize function stack and put first state into stack */
        fsm->fnc_sp = &fsm->fnc_stack[0];

        /* Inicialize with NOP state */
        fsm->state = &fsm_nop_state;

        /* debug data */
        fsm->debug_name = debug_name;
}

int fsm_start(struct robo_fsm *fsm, pthread_attr_t *attr)
{
        int ret;
        ret = pthread_create(&fsm->threadid, attr,
                             fsm_thread_routine,
                             (void *)fsm);
        return ret;
}

int fsm_destroy(struct robo_fsm *fsm)
{
        sem_destroy(&fsm->event_sent);
        sem_destroy(&fsm->event_noticed);
        return 0;
}

---