int main() {
mlockall(MCL_CURRENT|MCL_FUTURE);
rt_print_auto_init(1);
RT_TASK low, med, high, sync;
rt_task_create(&low, "low", 0, 10, T_CPU(1)|T_JOINABLE);
rt_task_create(&med, "med", 0, 20, T_CPU(1)|T_JOINABLE);
rt_task_create(&high, "high", 0, 30, T_CPU(1)|T_JOINABLE);
rt_task_create(&sync, "sync", 0, 99, T_CPU(1)|T_JOINABLE);
rt_sem_create(&sem, "sem", 0, S_PRIO);
rt_sem_create(&resourceSem, "resourceSem", 1, S_PRIO);
rt_mutex_create(&resourceMutex, "resourceMutex");
rt_task_start(&low, &lowFunc, NULL);
rt_task_start(&med, &medFunc, NULL);
rt_task_start(&high, &highFunc, NULL);
rt_task_start(&sync, &syncFunc, NULL);
rt_task_join(&low);
rt_task_join(&med);
rt_task_join(&high);
rt_task_join(&sync);
rt_sem_delete(&sem);
rt_sem_delete(&resourceSem);
rt_mutex_delete(&resourceMutex);
return 0;
}
int main(){
mlockall(MCL_CURRENT|MCL_FUTURE);
rt_print_auto_init(1);
rt_sem_create(&semaphore, "sem", 1, S_PRIO);
rt_sem_create(&synca, "sync", 0, S_PRIO);
rt_mutex_create(&mutex, "mutex");
RT_TASK L, M, H;
rt_task_shadow(NULL, "main", 4, T_CPU(1)|T_JOINABLE);
rt_task_create(&L, "low", 0, 1, T_CPU(1)|T_JOINABLE);
rt_task_create(&M, "medium", 0, 2, T_CPU(1)|T_JOINABLE);
rt_task_create(&H, "high", 0, 3, T_CPU(1)|T_JOINABLE);
rt_task_start(&L, &low, (void*) 0);
rt_task_start(&M, &medium, (void*) 0);
rt_task_start(&H, &high, (void*) 0);
usleep(100000);
rt_printf("RELEASING SYNC\n");
rt_sem_broadcast(&synca);
rt_task_join(&L);
rt_task_join(&M);
rt_task_join(&H);
rt_sem_delete(&synca);
rt_sem_delete(&semaphore);
rt_mutex_delete(&mutex);
return 0;
}
int main(){
rt_print_auto_init(1);
mlockall(MCL_CURRENT|MCL_FUTURE);
rt_task_shadow(NULL, "main", 5, T_CPU(0)|T_JOINABLE);
#ifdef mutex
rt_mutex_create(&a, "Mutex");
rt_mutex_create(&b, "b");
#endif
rt_task_create(&task1, "Task1", 0, 1, T_CPU(0)|T_JOINABLE);
rt_task_create(&task2, "Task2", 0, 2, T_CPU(0)|T_JOINABLE);
rt_task_start(&task1, &semWait1, NULL);
rt_task_start(&task2, &semWait2, NULL);
rt_printf("sync \n");
rt_task_join(&task1);
rt_task_join(&task2);
#ifdef mutex
rt_mutex_delete(&a);
rt_mutex_delete(&b);
#endif
}
int main ()
{
io_init();
RT_TASK test[3];
mlockall(MCL_CURRENT|MCL_FUTURE);
char name[10];
long i;
for (i = 1; i <= 3; i++) {
sprintf(name, "test%lu", i);
rt_task_create(&test[i-1], name, 0, i, T_CPU(1)|T_JOINABLE);
rt_task_start(&test[i-1], &periodicTest, (void*) i);
}
pthread_t disturbances[10];
for (i = 0; i < 10; i++) {
pthread_create(&disturbances[i], NULL, disturbance, NULL);
}
for (i = 0; i < 10; i++) {
pthread_join(disturbances[i], NULL);
}
wait_for_ctrl_c();
return 0;
}
int _rtapi_task_start_hook(task_data *task, int task_id) {
int which_cpu = 0;
int uses_fpu = 0;
int retval;
#ifdef XENOMAI_V2
// seems to work for me
// not sure T_CPU(n) is possible - see:
// http://www.xenomai.org/pipermail/xenomai-help/2010-09/msg00081.html
if (task->cpu > -1) // explicitly set by threads, motmod
which_cpu = T_CPU(task->cpu);
// http://www.xenomai.org/documentation/trunk/html/api/group__task.html#ga03387550693c21d0223f739570ccd992
// Passing T_FPU|T_CPU(1) in the mode parameter thus creates a
// task with FPU support enabled and which will be affine to CPU #1
// the task will start out dormant; execution begins with rt_task_start()
// since this is a usermode RT task, it will be FP anyway
if (task->uses_fp)
uses_fpu = T_FPU;
#endif
// optionally start as relaxed thread - meaning defacto a standard Linux thread
// without RT features
// see https://xenomai.org/pipermail/xenomai/2015-July/034745.html and
// https://github.com/machinekit/machinekit/issues/237#issuecomment-126590880
int prio = (task->flags & TF_NONRT) ? 0 :task->prio;
if ((retval = rt_task_create (&ostask_array[task_id], task->name,
task->stacksize, prio,
uses_fpu | which_cpu | T_JOINABLE)
) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"rt_task_create failed: %d %s\n",
retval, strerror(-retval));
return -ENOMEM;
}
#ifndef XENOMAI_V2
// Xenomai-3 CPU affinity
cpu_set_t cpus;
CPU_SET(task->cpu, &cpus);
rt_task_set_affinity (&ostask_array[task_id], &cpus);
#endif
if ((retval = rt_task_start( &ostask_array[task_id],
_rtapi_task_wrapper, (void *)(long)task_id))) {
rtapi_print_msg(RTAPI_MSG_INFO,
"rt_task_start failed: %d %s\n",
retval, strerror(-retval));
return -ENOMEM;
}
return 0;
}
int _rtapi_task_new_hook(task_data *task, int task_id) {
rtapi_print_msg(RTAPI_MSG_DBG,
"rt_task_create %d \"%s\" cpu=%d fpu=%d prio=%d\n",
task_id, task->name, task->cpu, task->uses_fp,
task->prio );
return rt_task_create(ostask_array[task_id], task->name, task->stacksize,
task->prio,
(task->uses_fp ? T_FPU : 0) | T_CPU(task->cpu));
}
int main(){
mlockall(MCL_CURRENT | MCL_FUTURE);
rt_print_auto_init(1);
rt_sem_create(&semA, "A", 1, S_FIFO | S_PRIO);
rt_sem_create(&semB, "B", 1, S_FIFO | S_PRIO);
rt_sem_create(&syncsem, "ss", 0, S_FIFO);
RT_TASK tasks[3];
rt_task_create(tasks, "C", 0, 99, T_CPU(0)|T_JOINABLE);
//rt_task_create(&(tasks[1]), "B", 0, 50, T_CPU(0));
rt_task_create(tasks+2, "D", 0, 33, T_CPU(0));
rt_task_start(&(tasks[0]),&task_H,(void*)'H');
//rt_task_start(&(tasks[1]),&task_M,(void*)'M');
rt_task_start(&(tasks[2]),&task_L,(void*)'L');
rt_task_shadow(NULL, "main", 0, 0);
rt_task_sleep_ms(200);
rt_sem_broadcast(&syncsem);
//rt_task_sleep_ms(2000);
rt_task_join(tasks);
rt_task_join(tasks+2);
rt_sem_delete(&semA);
rt_sem_delete(&semB);
/*
pthread_t disturbance[10];
for (i=0; i<10; i++){
pthread_create(&(disturbance[i]),NULL,&busy_wait,NULL);
}
for (i=0; i<10; i++){
pthread_join(disturbance[i],NULL);
}
*/
return 0;
};
int main() {
mlockall(MCL_CURRENT|MCL_FUTURE);
RT_TASK low, high, sync;
rt_task_create(&low, "low", 0, 0, T_CPU(1)|T_JOINABLE);
rt_task_create(&high, "high", 0, 1, T_CPU(1)|T_JOINABLE);
rt_task_create(&sync, "sync", 0, 99, T_CPU(1)|T_JOINABLE);
rt_sem_create(&sem, "sem", 0, S_PRIO);
rt_task_start(&low, &wait, NULL);
rt_task_start(&high, &wait, NULL);
rt_task_start(&sync, &syncFunc, NULL);
rt_task_join(&low);
rt_task_join(&high);
rt_task_join(&sync);
rt_sem_delete(&sem);
return 0;
}
int _rtapi_task_start_hook(task_data *task, int task_id) {
int which_cpu = 0;
int retval;
#if !defined(BROKEN_XENOMAU_CPU_AFFINITY)
// seems to work for me
// not sure T_CPU(n) is possible - see:
// http://www.xenomai.org/pipermail/xenomai-help/2010-09/msg00081.html
if (task->cpu > -1) // explicitly set by threads, motmod
which_cpu = T_CPU(task->cpu);
#endif
// http://www.xenomai.org/documentation/trunk/html/api/group__task.html#ga03387550693c21d0223f739570ccd992
// Passing T_FPU|T_CPU(1) in the mode parameter thus creates a
// task with FPU support enabled and which will be affine to CPU #1
// the task will start out dormant; execution begins with rt_task_start()
// since this is a usermode RT task, it will be FP anyway
if ((retval = rt_task_create (&ostask_array[task_id], task->name,
task->stacksize, task->prio,
(task->uses_fp ? T_FPU : 0) |
which_cpu | T_JOINABLE)
) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"rt_task_create failed, rc = %d\n", retval );
return -ENOMEM;
}
if ((retval = rt_task_start( &ostask_array[task_id],
_rtapi_task_wrapper, (void *)(long)task_id))) {
rtapi_print_msg(RTAPI_MSG_INFO,
"rt_task_start failed, rc = %d\n", retval );
return -ENOMEM;
}
return 0;
}
EinspurDynamikRealtime::EinspurDynamikRealtime()
: XenomaiTask::XenomaiTask("EinspurDynamikRealtimeTask", 0, 99, T_FPU | T_CPU(1))
{
m = coCoviseConfig::getFloat("mass", "COVER.Plugin.SteeringWheel.EinspurDynamik.inertia", 1500);
I = coCoviseConfig::getFloat("moiYaw", "COVER.Plugin.SteeringWheel.EinspurDynamik.inertia", 2700);
lv = coCoviseConfig::getFloat("lengthfront", "COVER.Plugin.SteeringWheel.EinspurDynamik.measures", 2.5);
lh = coCoviseConfig::getFloat("lengthrear", "COVER.Plugin.SteeringWheel.EinspurDynamik.measures", 2.3);
roadFrictionForce = 0.0;
yNull.x = 0;
yNull.y = 0;
yNull.kappa = 0;
yNull.v = 0;
yNull.alpha = 0;
yNull.epsilon = 0;
yNull.dEpsilon = 0;
yNull.zeta = 0;
yNull.dZeta = 0;
yNull.u = 0;
stepcount = 0;
errorcontrol = coCoviseConfig::getFloat("error", "COVER.Plugin.SteeringWheel.Dynamics", 1e-4);
num_intsteps = 1;
i[0] = coCoviseConfig::getFloat("reverse", "COVER.Plugin.SteeringWheel.Dynamics.transmission", -3.6);
i[1] = 0;
i[2] = coCoviseConfig::getFloat("first", "COVER.Plugin.SteeringWheel.Dynamics.transmission", 3.6);
i[3] = coCoviseConfig::getFloat("second", "COVER.Plugin.SteeringWheel.Dynamics.transmission", 2.19);
i[4] = coCoviseConfig::getFloat("third", "COVER.Plugin.SteeringWheel.Dynamics.transmission", 1.41);
i[5] = coCoviseConfig::getFloat("fourth", "COVER.Plugin.SteeringWheel.Dynamics.transmission", 1);
i[6] = coCoviseConfig::getFloat("fifth", "COVER.Plugin.SteeringWheel.Dynamics.transmission", 0.83);
iag = coCoviseConfig::getFloat("axle", "COVER.Plugin.SteeringWheel.Dynamics.transmission", 3.5);
b = coCoviseConfig::getFloat("width", "COVER.Plugin.SteeringWheel.Dynamics.measures", 1.8);
h = coCoviseConfig::getFloat("height", "COVER.Plugin.SteeringWheel.Dynamics.measures", 1.3);
Aw = coCoviseConfig::getFloat("Aw", "COVER.Plugin.SteeringWheel.Dynamics.aerodynamics", 2.3);
cw = coCoviseConfig::getFloat("cw", "COVER.Plugin.SteeringWheel.Dynamics.aerodynamics", 0.3);
hs = coCoviseConfig::getFloat("heightcenter", "COVER.Plugin.SteeringWheel.Dynamics.inertia", 0.5);
Iw = coCoviseConfig::getFloat("moiRoll", "COVER.Plugin.SteeringWheel.Dynamics.inertia", 600);
kw = coCoviseConfig::getFloat("kr", "COVER.Plugin.SteeringWheel.Dynamics.roll", 600000);
dw = coCoviseConfig::getFloat("dr", "COVER.Plugin.SteeringWheel.Dynamics.roll", 20000);
In = coCoviseConfig::getFloat("moiPitch", "COVER.Plugin.SteeringWheel.Dynamics.inertia", 2800);
kn = coCoviseConfig::getFloat("kp", "COVER.Plugin.SteeringWheel.Dynamics.pitch", 200000);
dn = coCoviseConfig::getFloat("dp", "COVER.Plugin.SteeringWheel.Dynamics.pitch", 50000);
Im = coCoviseConfig::getFloat("moiMotor", "COVER.Plugin.SteeringWheel.Dynamics.engine", 0.5);
km = coCoviseConfig::getFloat("km", "COVER.Plugin.SteeringWheel.Dynamics.engine", 200);
cm = coCoviseConfig::getFloat("cm", "COVER.Plugin.SteeringWheel.Dynamics.engine", 0.01);
kc = coCoviseConfig::getFloat("kc", "COVER.Plugin.SteeringWheel.Dynamics.transmission", 800);
cc = coCoviseConfig::getFloat("cc", "COVER.Plugin.SteeringWheel.Dynamics.transmission", 0.04);
wr = coCoviseConfig::getFloat("wheelradius", "COVER.Plugin.SteeringWheel.Dynamics.measures", 0.25);
chassisTrans.makeIdentity();
bodyTrans.makeIdentity();
accPedal = 0;
brakePedal = 0;
clutchPedal = 0;
gear = 0;
steerWheelAngle = 0;
steerPosition = 0;
steerSpeed = 0;
double Mm = coCoviseConfig::getFloat("maxoutputmoment", "COVER.Plugin.SteeringWheel.Dynamics.engine", 400);
double um = 2.0 * M_PI / 60.0 * coCoviseConfig::getFloat("maxoutputspeed", "COVER.Plugin.SteeringWheel.Dynamics.engine", 4600);
ul = 2.0 * M_PI / 60.0 * coCoviseConfig::getFloat("speedlimit", "COVER.Plugin.SteeringWheel.Dynamics.engine", 7300);
ui = 2.0 * M_PI / 60.0 * coCoviseConfig::getFloat("idlespeed", "COVER.Plugin.SteeringWheel.Dynamics.engine", 800);
if (ul > 2 * um)
{
ul = 2 * um;
std::cerr << "Engine speed limit to high for motor characteristic, setting to " << 60.0 / (2.0 * M_PI) * ul << std::endl;
}
ach = -km / (um * um);
bch = (2 * km) / um;
cch = Mm;
//accPedalIdle = (km*tanh(cm*ui))/(ach * ui * ui + bch * ui + cch);
accPedalIdle = 0.1;
runTask = true;
taskFinished = false;
returningToAction = false;
movingToGround = false;
pause = true;
overruns = 0;
if (coVRMSController::instance()->isMaster())
{
//motPlat = new ValidateMotionPlatform("rtcan0");
motPlat = ValidateMotionPlatform::instance();
steerCon = new CanOpenController("rtcan1");
steerWheel = new XenomaiSteeringWheel(*steerCon, 1);
start();
}
}
INTERNAL_QUAL int rtos_task_create(RTOS_TASK* task,
int priority,
unsigned cpu_affinity,
const char* name,
int sched_type,
size_t stack_size,
void * (*start_routine)(void *),
ThreadInterface* obj)
{
rtos_task_check_priority(&sched_type, &priority);
XenoCookie* xcookie = (XenoCookie*)malloc( sizeof(XenoCookie) );
xcookie->data = obj;
xcookie->wrapper = start_routine;
if ( name == 0 || strlen(name) == 0)
name = "XenoThread";
task->name = strncpy( (char*)malloc( (strlen(name)+1)*sizeof(char) ), name, strlen(name)+1 );
task->sched_type = sched_type; // User requested scheduler.
int rv;
unsigned int aff = 0;
if ( cpu_affinity != 0 ) {
// calculate affinity:
for(unsigned i = 0; i < 8*sizeof(cpu_affinity); i++) {
if(cpu_affinity & (1 << i)) {
// RTHAL_NR_CPUS is defined in the kernel, not in user space. So we just limit up to 7, until Xenomai allows us to get the maximum.
if ( i > 7 ) {
const unsigned int all_cpus = ~0;
if ( cpu_affinity != all_cpus ) // suppress this warning when ~0 is provided
log(Warning) << "rtos_task_create: ignoring cpu_affinity for "<< name << " on CPU " << i << " since it's larger than RTHAL_NR_CPUS - 1 (="<< 7 <<")"<<endlog();
} else {
aff |= T_CPU(i);
}
}
}
}
if (stack_size == 0) {
log(Debug) << "Raizing default stack size to 128kb for Xenomai threads in Orocos." <<endlog();
stack_size = 128000;
}
// task, name, stack, priority, mode, fun, arg
// UGLY, how can I check in Xenomai that a name is in use before calling rt_task_spawn ???
rv = rt_task_spawn(&(task->xenotask), name, stack_size, priority, T_JOINABLE | (aff & T_CPUMASK), rtos_xeno_thread_wrapper, xcookie);
if ( rv == -EEXIST ) {
free( task->name );
task->name = strncpy( (char*)malloc( (strlen(name)+2)*sizeof(char) ), name, strlen(name)+1 );
task->name[ strlen(name) ] = '0';
task->name[ strlen(name)+1 ] = 0;
while ( rv == -EEXIST && task->name[ strlen(name) ] != '9') {
task->name[ strlen(name) ] += 1;
rv = rt_task_spawn(&(task->xenotask), task->name, stack_size, priority, T_JOINABLE | (aff & T_CPUMASK), rtos_xeno_thread_wrapper, xcookie);
}
}
if ( rv == -EEXIST ) {
log(Warning) << name << ": an object with that name is already existing in Xenomai." << endlog();
rv = rt_task_spawn(&(task->xenotask), 0, stack_size, priority, T_JOINABLE | (aff & T_CPUMASK), rtos_xeno_thread_wrapper, xcookie);
}
if ( rv != 0) {
log(Error) << name << " : CANNOT INIT Xeno TASK " << task->name <<" error code: "<< rv << endlog();
return rv;
}
rt_task_yield();
return 0;
}
int main(int argc, char **argv)
{
int cpu = 0, c, err, sig;
struct sigaction sa;
char task_name[16];
sigset_t mask;
while ((c = getopt(argc, argv, "g:hp:l:T:qH:B:sD:t:fc:P:b")) != EOF)
switch (c) {
case 'g':
do_gnuplot = strdup(optarg);
break;
case 'h':
do_histogram = 1;
break;
case 's':
do_stats = 1;
break;
case 'H':
histogram_size = atoi(optarg);
break;
case 'B':
bucketsize = atoi(optarg);
break;
case 'p':
period_ns = atoi(optarg) * 1000LL;
break;
case 'l':
data_lines = atoi(optarg);
break;
case 'T':
test_duration = atoi(optarg);
alarm(test_duration + WARMUP_TIME);
break;
case 'q':
quiet = 1;
break;
case 'D':
benchdev_no = atoi(optarg);
break;
case 't':
test_mode = atoi(optarg);
break;
case 'f':
freeze_max = 1;
break;
case 'c':
cpu = T_CPU(atoi(optarg));
break;
case 'P':
priority = atoi(optarg);
break;
case 'b':
stop_upon_switch = 1;
break;
default:
fprintf(stderr,
"usage: latency [options]\n"
" [-h] # print histograms of min, avg, max latencies\n"
" [-g <file>] # dump histogram to <file> in gnuplot format\n"
" [-s] # print statistics of min, avg, max latencies\n"
" [-H <histogram-size>] # default = 200, increase if your last bucket is full\n"
" [-B <bucket-size>] # default = 1000ns, decrease for more resolution\n"
" [-p <period_us>] # sampling period\n"
" [-l <data-lines per header>] # default=21, 0 to supress headers\n"
" [-T <test_duration_seconds>] # default=0, so ^C to end\n"
" [-q] # supresses RTD, RTH lines if -T is used\n"
" [-D <testing_device_no>] # number of testing device, default=0\n"
" [-t <test_mode>] # 0=user task (default), 1=kernel task, 2=timer IRQ\n"
" [-f] # freeze trace for each new max latency\n"
" [-c <cpu>] # pin measuring task down to given CPU\n"
" [-P <priority>] # task priority (test mode 0 and 1 only)\n"
" [-b] # break upon mode switch\n"
);
exit(2);
}
if (!test_duration && quiet) {
fprintf(stderr,
"latency: -q only works if -T has been given.\n");
quiet = 0;
}
if ((test_mode < USER_TASK) || (test_mode > TIMER_HANDLER)) {
fprintf(stderr, "latency: invalid test mode.\n");
exit(2);
}
time(&test_start);
histogram_avg = calloc(histogram_size, sizeof(long));
histogram_max = calloc(histogram_size, sizeof(long));
histogram_min = calloc(histogram_size, sizeof(long));
if (!(histogram_avg && histogram_max && histogram_min))
cleanup();
if (period_ns == 0)
period_ns = CONFIG_XENO_DEFAULT_PERIOD; /* ns */
if (priority <= T_LOPRIO)
priority = T_LOPRIO + 1;
else if (priority > T_HIPRIO)
priority = T_HIPRIO;
sigemptyset(&mask);
sigaddset(&mask, SIGINT);
sigaddset(&mask, SIGTERM);
sigaddset(&mask, SIGHUP);
sigaddset(&mask, SIGALRM);
pthread_sigmask(SIG_BLOCK, &mask, NULL);
sigemptyset(&sa.sa_mask);
sa.sa_sigaction = sigdebug;
sa.sa_flags = SA_SIGINFO;
sigaction(SIGDEBUG, &sa, NULL);
if (freeze_max) {
/* If something goes wrong, we want to freeze the current
trace path to help debugging. */
signal(SIGSEGV, faulthand);
signal(SIGBUS, faulthand);
}
setlinebuf(stdout);
printf("== Sampling period: %Ld us\n"
"== Test mode: %s\n"
"== All results in microseconds\n",
period_ns / 1000, test_mode_names[test_mode]);
mlockall(MCL_CURRENT | MCL_FUTURE);
if (test_mode != USER_TASK) {
char devname[RTDM_MAX_DEVNAME_LEN];
snprintf(devname, RTDM_MAX_DEVNAME_LEN, "rttest-timerbench%d",
benchdev_no);
benchdev = rt_dev_open(devname, O_RDWR);
if (benchdev < 0) {
fprintf(stderr,
"latency: failed to open benchmark device, code %d\n"
"(modprobe RTAI_timerbench?)\n", benchdev);
return 0;
}
}
rt_timer_set_mode(TM_ONESHOT); /* Force aperiodic timing. */
snprintf(task_name, sizeof(task_name), "display-%d", getpid());
err = rt_task_create(&display_task, task_name, 0, 0, T_FPU);
if (err) {
fprintf(stderr,
"latency: failed to create display task, code %d\n",
err);
return 0;
}
err = rt_task_start(&display_task, &display, NULL);
if (err) {
fprintf(stderr,
"latency: failed to start display task, code %d\n",
err);
return 0;
}
if (test_mode == USER_TASK) {
snprintf(task_name, sizeof(task_name), "sampling-%d", getpid());
err =
rt_task_create(&latency_task, task_name, 0, priority,
T_FPU | cpu | T_WARNSW);
if (err) {
fprintf(stderr,
"latency: failed to create latency task, code %d\n",
err);
return 0;
}
err = rt_task_start(&latency_task, &latency, NULL);
if (err) {
fprintf(stderr,
"latency: failed to start latency task, code %d\n",
err);
return 0;
}
}
sigwait(&mask, &sig);
finished = 1;
cleanup();
return 0;
}
int main(void){
/* Perform auto-init of rt_print buffers if the task doesn't do so */
rt_print_auto_init(1);
/* Lock memory : avoid memory swapping for this program */
mlockall(MCL_CURRENT|MCL_FUTURE);
/* Make main a Real-Time thread */
if(rt_task_shadow(NULL, NULL, RT_MAIN_PRI, T_CPU(CPU_ID) ) != SUCCESS){
printf("RT Thread main failed to be created!\n");
exit(1);
}else{
/* print success */
rt_printf("RT Thread main initiated successfully \n");
}
/* Create semaphore for synchronized start */
if(rt_sem_create(&sync_sem, "sync_start", 0, S_PRIO) != SUCCESS){
rt_printf("sync semaphore failed to be created \n");
exit(1);
}
/* Create Barrier */
#ifdef semaphore
if(rt_sem_create(&sem, "Barrier_sem", 0, S_PRIO) != SUCCESS){
rt_printf("Barrier_sem failed to be created \n");
exit(1);
}
#endif
#ifdef mutex
if(rt_mutex_create(&mut, "Barrier_mut") != SUCCESS){
rt_printf("Barrier_mut failed to be created \n");
exit(1);
}
#endif
/* Initialize Real-Time threads */
int* id;
int i;
RT_TASK rt_threads[N_RT_THREADS];
for(i=0; i<N_RT_THREADS; i++){
/* Give the new thread the channel to respond to by argument */
int *method = malloc(sizeof(int));
*method = USE_METHOD;
/* Create new thread */
if(rt_task_create(&rt_threads[i], NULL, 0, RT_THREADS_PRI[i], T_CPU(CPU_ID)|T_JOINABLE) != SUCCESS){
rt_printf("RT Thread %i failed to be created!\n", i);
exit(1);
}
/* Execute task */
if(rt_task_start(&rt_threads[i], RT_FUNC[i], id) != SUCCESS){
rt_printf("RT Thread %i failed to start!\n", (void*) i);
exit(1);
}
/* print success */
rt_printf("RT Thread %i initiated successfully \n", i);
}
/* Wait untill all threads have been blocked, restart them */
rt_task_sleep(SLEEP_DELAY);
if(rt_sem_broadcast(&sync_sem)!= SUCCESS){
rt_printf("main thread failed to broadcast on BARRIER semaphore \n");
exit(1);
}
rt_task_sleep(SLEEP_DELAY);
/* Wait for all threads to terminate */
for(i=0; i<N_RT_THREADS; i++){
rt_task_join(&rt_threads[i]);
}
/* Clean up */
rt_sem_delete(&sem);
rt_sem_delete(&sync_sem);
rt_mutex_delete(&mut);
return 0;
}
