/home/jgoppert/Projects/uvsim/uvsim/src/uvsim/utilities/RealtimeThread.cc

Go to the documentation of this file.

/*
 * RealtimeThread.cpp
 *
 *  Created on: Apr 26, 2009
 *      Author: jgoppert
 */

#include "RealtimeThread.h"
#include <iostream>
#include <sys/mman.h>
#include <cstring>

#define MAX_SAFE_STACK (8*1024) /* The maximum stack size which is
guranteed safe to access without
faulting */

void stack_prefault(void)
{
    unsigned char dummy[MAX_SAFE_STACK];
    memset(&dummy, 0, MAX_SAFE_STACK);
    return;
}

namespace uvsim
{

RealtimeThread::RealtimeThread(int priority, float freq)
{
                m_stopRequested = false;
    m_priority = priority;
    m_waitNSec = 1.0/freq*m_nSecPerSec;

    // Schedule as realtime if requested
    if (m_priority > 0)
    {
        m_sched.sched_priority = m_priority;
        if (sched_setscheduler(0, SCHED_FIFO, &m_sched)==-1)
        {
            perror("sched_setscheduler failed");
            exit(-1);
        }
        else
        {
            std::cout << "Running as a realtime thread with priority: "<< m_priority << std::endl;
        }
    }
    else
    {
        std::cout << "Running as a non-realtime thread" << std::endl;
    }

    /* Pre fault Stack */
    stack_prefault();

    clock_gettime(CLOCK_MONOTONIC, &m_time);
    /* start after one second */
    m_time.tv_sec++;
}

RealtimeThread::~RealtimeThread()
{
    pthread_cancel(m_thread);
}

void * RealtimeThread::thread_func(void *d)
{
    ((RealtimeThread *) d)->run();
}

void RealtimeThread::run()
{
    while (!m_stopRequested)
    {
        // wait until next shot
        clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &m_time, NULL);

        //do the stuff
        loop();

        //calculate next shot
        m_time.tv_nsec += m_waitNSec;

#ifdef DEBUG
        std::cout << "Realtime thread timestamp: " << m_time.tv_sec << " sec  " << m_time.tv_nsec << " nsec " << std::endl;
#endif

        while (m_time.tv_nsec >= m_nSecPerSec)
        {
            m_time.tv_nsec -= m_nSecPerSec;
            m_time.tv_sec++;
        }
    }
}

int RealtimeThread::start()
{
                m_stopRequested = false;
    return pthread_create(&m_thread, NULL, RealtimeThread::thread_func,
                          (void*) this);
}
void RealtimeThread::stop()
{
                m_stopRequested = true;
}

int RealtimeThread::join()
{
    return pthread_join(m_thread, NULL);

}

}

---