xenomai rt_sem 信号量

#include <stdio.h>
#include <signal.h>
#include <unistd.h>
#include <sys/mman.h>
 
#include <native/task.h>
#include <native/timer.h>
#include <native/sem.h>
  
#include  <rtdk.h>
#include "program.h"
RT_TASK task;
RT_SEM readDone;
RT_SEM executionDone;
RT_SEM writeDone;
short stopped = 0;
void* sensors;
void* actuators;
void sense(void *args);
void execute(void *args);
void act(void *agrs);
void sense(void *args){
rt_task_set_periodic(NULL,TM_NOW,PLCperiod);
while(!stopped){
/*
 *Read inputs status
*/
sensors = readInputs();
rt_sem_v(&readDone);
rt_task_wait_period(NULL);
}
}
void act(void *args){
rt_task_set_periodic(NULL,TM_NOW,PLCperiod);
while(!stopped){
rt_sem_p(&executionDone,0);
/*
 *Write outputs status
 */
writeOutputs(actuators);
rt_sem_v(&writeDone);
rt_task_wait_period(NULL);
}
}
void executor(void *args){
rt_task_set_periodic(NULL,TM_NOW,PLCperiod);
short step = (short)*args;
actuators = 0;// how to initialize the temporary value of actuators?
while(!stopped){
rt_sem_p(&readDone,0);
int c;
for(c=0;c<nStep;c++){
if(stepStatus[c])
step[c](sensors);// or equivalent
}
rt_sem_v(&executionDone);
rt_sem_p(&writeDone,0);
for(c=0;c<nStep;c++){
if(stepStatus[c])
condition[c](sensor,actuators);
}
rt_task_wait_period(NULL);
}
return;
}
void init_xenomai() {
// Perform auto-init of rt_print buffers if the task doesn't do so
rt_print_auto_init(1);
 // Avoids memory swapping for this program
mlockall(MCL_CURRENT|MCL_FUTURE);
}
void startup(){
stepInitializzator();
rt_sem_create(&readDone, "readSem", 0, S_PRIO);
rt_sem_create(&executionDone, "executionSem", 0, S_PRIO);
rt_sem_create(&writeDone, "writeSem", 0, S_PRIO);
rt_task_create(&task, "readerTask", 0, 99, 0);
rt_task_start(&task, &reader, NULL);
rt_task_create(&task, "executorTask", 0, 98, 0);
rt_task_start(&task, &executor, NULL);
rt_task_create(&task, "writerTask", 0, 97, 0);
rt_task_start(&task, &writer, NULL);
}
void catch_signal(int sig) {
stopped=1;
rt_printf("--should I exit?--\n");
//exit(0);
}
void wait_for_ctrl_c() {
signal(SIGTERM, catch_signal);
signal(SIGINT, catch_signal);
// wait for SIGINT (CTRL-C) or SIGTERM signal
pause();
}
void cleanup(){
rt_printf("cleaned\n");
}
int main(int argn, char** argv){
init_xenomai();
//rt_printf("initialized.\n");
//rt_printf("starting up...\n");
startup();
rt_printf("waiting Ctrl+C\n");
wait_for_ctrl_c();
rt_printf("closing...\n");
cleanup();
return 0;
}