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
;
}