Fixed Save As... function in Beremiz
#include <stdlib.h>
#include <unistd.h>
#include <sys/mman.h>
#include <sys/poll.h>
#include <rtai_lxrt.h>
#include <rtai_sem.h>
#include <pthread.h>
#include <errno.h>
#include "applicfg.h"
#include "can_driver.h"
#include "timer.h"
#define TIMERLOOP_TASK_CREATED 1
TimerCallback_t exitall;
SEM *CanFestival_mutex;
SEM *condition_mutex;
SEM *control_task;
CND *timer_set;
// realtime task structures
RT_TASK *timerloop_task;
RT_TASK *Main_Task;
// linux threads id's
static pthread_t timerloop_thr;
RTIME last_time_read;
RTIME last_occured_alarm;
RTIME last_timeout_set;
int stop_timer = 0;
void TimerInit(void)
{
/* Init Main Task */
if (!(Main_Task = rt_thread_init(rt_get_name(0), 0, 0, SCHED_FIFO, 1))) {
printf("CANNOT INIT MAIN TASK\n");
exit(1);
}
/* Init Mutex */
CanFestival_mutex = rt_sem_init(rt_get_name(0), 1);
condition_mutex = rt_typed_sem_init(rt_get_name(0), 1, RES_SEM);
timer_set = rt_cond_init(rt_get_name(0));
control_task = rt_sem_init(rt_get_name(0), 0);
/* Set timer mode and start timer */
rt_set_oneshot_mode();
start_rt_timer(0);
}
/**
* Stop Timer Task
* @param exitfunction
*/
void StopTimerLoop(TimerCallback_t exitfunction)
{
exitall = exitfunction;
stop_timer = 1;
rt_cond_signal(timer_set);
}
/**
* Clean all Semaphores and main task
*/
void TimerCleanup(void)
{
/* Stop timer */
stop_rt_timer();
/* Delete all mutex and the main task */
rt_sem_delete(CanFestival_mutex);
rt_sem_delete(condition_mutex);
rt_sem_delete(timer_set);
rt_sem_delete(control_task);
rt_thread_delete(Main_Task);
}
/**
* Take a semaphore
*/
void EnterMutex(void)
{
rt_sem_wait(CanFestival_mutex);
}
/**
* Signaling a semaphore
*/
void LeaveMutex(void)
{
rt_sem_signal(CanFestival_mutex);
}
static TimerCallback_t init_callback;
/**
* Timer Task
*/
void timerloop_task_proc(void *arg)
{
int ret = 0;
// lock process in to RAM
mlockall(MCL_CURRENT | MCL_FUTURE);
timerloop_task = rt_thread_init(rt_get_name(0), 0, 0, SCHED_FIFO, 1);
rt_make_hard_real_time();
getElapsedTime();
last_timeout_set = 0;
last_occured_alarm = last_time_read;
/* trigger first alarm */
SetAlarm(NULL, 0, init_callback, 0, 0);
do{
RTIME real_alarm;
rt_sem_wait(condition_mutex);
if(last_timeout_set == TIMEVAL_MAX)
{
ret = rt_cond_wait(
timer_set,
condition_mutex); /* Then sleep until next message*/
rt_sem_signal(condition_mutex);
}else{
real_alarm = last_time_read + last_timeout_set;
ret = rt_cond_wait_until(
timer_set,
condition_mutex,
real_alarm); /* Else, sleep until next deadline */
if(ret == SEM_TIMOUT){
last_occured_alarm = real_alarm;
rt_sem_signal(condition_mutex);
EnterMutex();
TimeDispatch();
LeaveMutex();
}else{
rt_sem_signal(condition_mutex);
}
}
}while ( ret != SEM_ERR && !stop_timer);
if(exitall){
EnterMutex();
exitall(NULL,0);
LeaveMutex();
}
rt_make_soft_real_time();
rt_thread_delete(timerloop_task);
}
/**
* Create the Timer Task
* @param _init_callback
*/
void StartTimerLoop(TimerCallback_t _init_callback)
{
stop_timer = 0;
init_callback = _init_callback;
/* start timerloop_task ( do nothing and get blocked ) */
timerloop_thr = rt_thread_create(timerloop_task_proc, NULL, 0);
}
/* We assume that ReceiveLoop_task_proc is always the same */
static void (*rtai_ReceiveLoop_task_proc)(CAN_PORT) = NULL;
/**
* Enter in realtime and start the CAN receiver loop
* @param port
*/
void rtai_canReceiveLoop(CAN_PORT port)
{
RT_TASK *current_task;
mlockall(MCL_CURRENT | MCL_FUTURE);
current_task = rt_thread_init(rt_get_name(0), 0, 0, SCHED_FIFO, 1);
rt_make_hard_real_time();
rt_sem_signal(control_task);
/* Call original receive loop with port struct as a param */
rtai_ReceiveLoop_task_proc(port);
rt_make_soft_real_time();
rt_thread_delete(current_task);
}
/**
* Create the CAN Receiver Task
* @param fd0 CAN port
* @param *ReceiveLoop_thread CAN receiver thread
* @param *ReceiveLoop_task_proc CAN receiver task
*/
void CreateReceiveTask(CAN_PORT fd0, TASK_HANDLE *ReceiveLoop_thread, void* ReceiveLoop_task_proc)
{
rtai_ReceiveLoop_task_proc = ReceiveLoop_task_proc;
*ReceiveLoop_thread = rt_thread_create(rtai_canReceiveLoop, (void*)fd0, 0);
rt_sem_wait(control_task);
}
/**
* Wait for the CAN Receiver Task end
* @param *ReceiveLoop_thread CAN receiver thread
*/
void WaitReceiveTaskEnd(TASK_HANDLE *ReceiveLoop_thread)
{
rt_thread_join(*ReceiveLoop_thread);
}
/**
* Set timer for the next wakeup
* @param value
*/
void setTimer(TIMEVAL value)
{
rt_sem_wait(condition_mutex);
last_timeout_set = value;
rt_sem_signal(condition_mutex);
rt_cond_signal(timer_set);
}
/**
* Get the elapsed time since the last alarm
* @return a time in nanoseconds
*/
TIMEVAL getElapsedTime(void)
{
RTIME res;
rt_sem_wait(condition_mutex);
last_time_read = rt_get_time();
res = last_time_read - last_occured_alarm;
rt_sem_signal(condition_mutex);
return res;
}