void demo(void *arg) { //Round Robin rt_task_set_mode(0,T_RRB,NULL); rt_task_slice(NULL,15); //time slice in jiffies RTIME starttime, runtime; int num=*(int *)arg; RT_TASK *curtask; RT_TASK_INFO curtaskinfo; rt_printf("Task : %d\n",num); rt_sem_p(&mysync,TM_INFINITE); // let the task run RUNTIME(=200) jiffies in steps of SPINTIME(=20) jiffies runtime = 0; while(runtime < EXECTIME) { rt_timer_spin(SPINTIME*BASEPERIOD); // spin cpu doing nothing // note: rt_timer_spin function does not accept jiffies only nanoseconds // deviates from timing conventions throughout the Xenomai API runtime = runtime + SPINTIME; rt_printf("Running Task : %d at time : %d\n",num,runtime); } rt_printf("End Task : %d\n",num); }
void verifier_batterie(void *arg) { DBattery *batterie = d_new_battery(); int status; int vbat; rt_printf("tbattery : Attente du sémaphore semVerifierBatterie\n"); rt_sem_p(&semVerifierBatterie, TM_INFINITE); rt_printf("tbattery : Debut de l'execution periodique à 1s\n"); rt_task_set_periodic(NULL, TM_NOW, 1000000000); while (1) { while (cptCommErr < MAX_ECHECS) { /* Attente de l'activation périodique */ rt_task_wait_period(NULL); rt_printf("tbattery : Activation périodique\n"); status = robot->get_vbat(robot, &vbat); if (status == STATUS_OK) { cptCommErr = 0; if (vbat == BATTERY_OFF || vbat == BATTERY_LOW || vbat == BATTERY_OK) batterie->set_level(batterie, vbat); } else { cptCommErr++; rt_printf("tbattery : Erreur de communication avec le robot (%d)\n", cptCommErr); } } comm_err_handler(status); rt_sem_p(&semVerifierBatterie, TM_INFINITE); } }
void recharger_watchdog(void *arg) { int status; rt_printf("twatchdog : Attente du sémaphore semRechargerWatchdog\n"); rt_sem_p(&semRechargerWatchdog, TM_INFINITE); rt_printf("twatchdog : Debut de l'execution periodique à 1s\n"); rt_task_set_periodic(NULL, TM_NOW, 1000000000); while (1) { while (cptCommErr < MAX_ECHECS) { /* Attente de l'activation périodique */ rt_task_wait_period(NULL); rt_printf("twatchdog : Activation périodique\n"); status = robot->reload_wdt(robot); if (status == STATUS_OK) { cptCommErr = 0; } else { cptCommErr++; rt_printf("twatchdog : Erreur de communication avec le robot (%d)\n", cptCommErr); } } comm_err_handler(status); rt_sem_p(&semRechargerWatchdog, TM_INFINITE); } }
void computationTask(long arg) { usleep(1); while(1){ int i = 0; rt_mutex_acquire(&mutex_donnee,TM_INFINITE); while(tabDonnePris==1){ rt_mutex_release(&mutex_donnee); rt_sem_p(&sem_donnee,TM_INFINITE); rt_mutex_acquire(&mutex_donnee,TM_INFINITE); } tabDonnePris = 1; rt_mutex_acquire(&mutex_obs,TM_INFINITE); while(tabObstaclePris==1){ rt_mutex_release(&mutex_obs); rt_sem_p(&sem_obs,TM_INFINITE); rt_mutex_acquire(&mutex_obs,TM_INFINITE); } tabObstaclePris = 1; printf("***************** COMPUTATION TASK ******************\n"); for(i = 0; i<SENSOR_SIZE; i++){ //If the obstacle is farther than 7meters if(sensorArray[i] > 7){ obstacle* newObstacle = malloc(sizeof(obstacle)); newObstacle->distance = sensorArray[i]; //Add the new obstacle at the end of the list newObstacle->nxt = NULL; if(obstacleList == NULL) obstacleList = newObstacle; else{ obstacle* temp = obstacleList; if(temp->distance==0)temp->distance=newObstacle->distance; else{ while(temp->nxt != NULL)temp = temp->nxt; temp->nxt = newObstacle; } } } } tabDonnePris = 0; tabObstaclePris = 0; rt_mutex_release(&mutex_obs); rt_mutex_release(&mutex_donnee); rt_sem_v(&sem_obs); usleep(200000); printf("fin compute\n"); } }
void task_H(void* data) { rt_sem_p(&syncsem,TM_INFINITE); rt_task_sleep_ms(1); rt_sem_p(&semB,TM_INFINITE); rt_printf("H : I got B\n"); busy_wait_ms(1); rt_sem_p(&semA,TM_INFINITE); busy_wait_ms(2); print_pri(NULL,(char) data); rt_sem_v(&semB); rt_sem_v(&semA); }
void high(){ rt_sem_p(&synca, TM_INFINITE); rt_task_sleep_ms(200); rt_printf("HIGH RUNNING\n"); rt_sem_p(&semaphore, TM_INFINITE); //rt_mutex_acquire(&mutex, TM_INFINITE); rt_printf("HIGH AQUIRED LCOK\n"); busy_wait_ms(100); busy_wait_ms(100); rt_printf("HIGH FINISHED\n"); rt_sem_v(&semaphore); //rt_mutex_release(&mutex); }
void deplacer(void *arg) { int status; rt_printf("tmove : Attente du sémaphore semDeplacer\n"); rt_sem_p(&semDeplacer, TM_INFINITE); rt_printf("tmove : Debut de l'éxecution periodique à 200ms\n"); rt_task_set_periodic(NULL, TM_NOW, 200000000); while (1) { while (cptCommErr < MAX_ECHECS) { /* Attente de l'activation périodique */ rt_task_wait_period(NULL); //rt_printf("tmove : Activation périodique\n"); rt_mutex_acquire(&mutexEtat, TM_INFINITE); status = etatCommRobot; rt_mutex_release(&mutexEtat); if (status == STATUS_OK) { rt_mutex_acquire(&mutexMove, TM_INFINITE); switch (move->get_direction(move)) { case DIRECTION_FORWARD: status = robot->set_motors(robot, MOTEUR_ARRIERE_LENT, MOTEUR_ARRIERE_LENT); break; case DIRECTION_LEFT: status = robot->set_motors(robot, MOTEUR_ARRIERE_LENT, MOTEUR_AVANT_LENT); break; case DIRECTION_RIGHT: status = robot->set_motors(robot, MOTEUR_AVANT_LENT, MOTEUR_ARRIERE_LENT); break; case DIRECTION_STOP: status = robot->set_motors(robot, MOTEUR_STOP, MOTEUR_STOP); break; case DIRECTION_STRAIGHT: status = robot->set_motors(robot, MOTEUR_AVANT_LENT, MOTEUR_AVANT_LENT); break; } rt_mutex_release(&mutexMove); if (status == STATUS_OK) { cptCommErr = 0; } else { cptCommErr++; rt_printf("tmove : Erreur de communication avec le robot (%d)\n", cptCommErr); } } } comm_err_handler(status); rt_sem_p(&semDeplacer, TM_INFINITE); } }
void task_L(void* data) { rt_sem_p(&syncsem,TM_INFINITE); rt_sem_p(&semA,TM_INFINITE); int prio = rt_task_set_priority(NULL,99); rt_printf("L : I got A\n"); busy_wait_ms(3); rt_sem_p(&semB,TM_INFINITE); busy_wait_ms(3); print_pri(NULL,(char) data); rt_sem_v(&semB); rt_sem_v(&semA); }
void low(){ rt_sem_p(&synca, TM_INFINITE); rt_printf("LOW RUNNING\n"); rt_sem_p(&semaphore, TM_INFINITE); //rt_mutex_acquire(&mutex, TM_INFINITE); rt_printf("LOW AQUIRED LOCK\n"); busy_wait_ms(100); busy_wait_ms(100); busy_wait_ms(100); rt_printf("LOW FINISHED\n"); rt_sem_v(&semaphore); //rt_mutex_release(&mutex); }
static void sync_threads(int id){ if(rt_sem_p(&sync_sem, TM_INFINITE) == SUCCESS){ rt_printf("Task %i got semaphore\n", id); }else{ rt_printf("Task %i failed to get semaphore \n", id); } }
void demo(void *arg) { RTIME starttime, runtime; int num=*(int *)arg; RT_TASK *curtask; RT_TASK_INFO curtaskinfo; curtask=rt_task_self(); rt_printf("Task : %d\n",num); rt_sem_p(&mysync,TM_INFINITE); runtime = 0; while(runtime < EXECTIME) { rt_timer_spin(SPINTIME); // spin cpu doing nothing runtime = runtime + SPINTIME; rt_printf("Running Task : %d at ms : %d\n",num,runtime/1000000); if(runtime == (EXECTIME/2)){ if(num == 2){ rt_task_set_priority(&demo_task[1],MID+10); rt_task_set_priority(&demo_task[0],LOW+10); } } } rt_printf("End Task : %d\n",num); }
void connecter(void *arg) { DMessage *message = d_new_message(); int status; rt_printf("tconnect : Debut de l'exécution de tconnect\n"); while (1) { rt_printf("tconnect : Attente du sémaphore semConnecterRobot\n"); rt_sem_p(&semConnecterRobot, TM_INFINITE); rt_printf("tconnect : Ouverture de la communication avec le robot\n"); status = robot->open_device(robot); rt_mutex_acquire(&mutexEtat, TM_INFINITE); etatCommRobot = status; if (status == STATUS_OK) { cptCommErr = 0; //robot->start_insecurely(robot); status = robot->start(robot); if (status == STATUS_OK) { /* Demarrage du robot */ rt_printf("tconnect : Robot démarrer\n"); rt_sem_v(&semDeplacer); rt_sem_v(&semRechargerWatchdog); rt_sem_v(&semVerifierBatterie); } else { /* Impossible de demarrer le robot, tentative de reinitialisation */ robot->stop(robot); robot->close_com(robot); } } rt_mutex_release(&mutexEtat); message->put_state(message, status); serveur->send(serveur, message); } }
void prioHigh(void *arg){ int err = 0; rt_task_sleep(WAIT); int i = 0; while(i<3) { rt_printf("High priority task tries to lock semaphore\n"); err = rt_sem_p(&mysync,TM_INFINITE); if(err < 0) rt_printf("Failed pending semaphore; error: %d: %s", err, strerror(-err)); err = 0; rt_printf("High priority task locks semaphore\n"); rt_timer_spin(SPINTIME); // spin cpu doing nothing i++; rt_printf("High priority task unlocks semaphore\n"); err = rt_sem_v(&mysync); if(err < 0) rt_printf("Failed signaling semaphore; error: %d: %s", err, strerror(-err)); err = 0; } rt_printf("..........................................High priority task ends\n"); }
void taskOne(void *arg){ int i; for(i=0;i<ITER;i++){ rt_sem_p(&semGlobal1,0); rt_printf("I'm taskOne and global = %d...\n",++global); rt_sem_v(&semGlobal2); } }
void taskTwo(void *arg){ int i; for(i=0;i<ITER;i++){ rt_sem_p(&semGlobal2,0); rt_printf("I'm taskTwo and global = %d---\n",--global); rt_sem_v(&semGlobal1); } }
void task_M(void* data) { rt_sem_p(&syncsem,TM_INFINITE); rt_task_sleep_ms(1); //rt_sem_p(sem,TM_INFINITE); busy_wait_ms(5); print_pri(NULL,(char) data); //rt_sem_v(sem,TM_INFINITE); }
void taskM ( void ) { rt_sem_p(&xenomai_semaphore, TM_INFINITE); // wait for synch print_pri(&taskMed, "TaskM started\n"); rt_task_sleep(TIME_UNIT); print_pri(&taskMed, "TaskM starting work\n"); busy_wait_ms(5*TIME_UNIT_MS); print_pri(&taskMed, "TaskM complete\n"); }
void lowFunc() { rt_sem_p(&sem, TM_INFINITE); rt_mutex_acquire(&resourceMutex, TM_INFINITE); rt_printf("low locks resource\n"); busy_wait_ms(100); busy_wait_ms(100); busy_wait_ms(100); rt_printf("low unlocks resource\n"); rt_mutex_release(&resourceMutex); }
void highFunc() { rt_sem_p(&sem, TM_INFINITE); rt_task_sleep_ms(200); rt_printf("High wants to lock resource\n"); rt_mutex_acquire(&resourceMutex, TM_INFINITE); rt_printf("High locks resource\n"); busy_wait_ms(100); busy_wait_ms(100); rt_printf("High unlocks resource\n"); rt_mutex_release(&resourceMutex); }
void medFunc() { rt_sem_p(&sem, TM_INFINITE); rt_task_sleep_ms(100); rt_printf("med starts running\n"); busy_wait_ms(100); busy_wait_ms(100); busy_wait_ms(100); busy_wait_ms(100); busy_wait_ms(100); rt_printf("med finished\n"); }
void medium(){ rt_sem_p(&synca, TM_INFINITE); rt_task_sleep_ms(100); rt_printf("MEDIUM RUNNING\n"); busy_wait_ms(100); busy_wait_ms(100); busy_wait_ms(100); busy_wait_ms(100); busy_wait_ms(100); rt_printf("MEDIUM FINISHED\n"); }
void demo(void *arg) { int num = * (int *)arg; RT_TASK *curtask; RT_TASK_INFO curtaskinfo; curtask=rt_task_self(); rt_task_inquire(curtask, &curtaskinfo); rt_sem_p(&sem, TM_INFINITE); rt_printf("Task name: %s - Argument %d\n", curtaskinfo.name, num); rt_sem_v(&sem); }
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 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 printTask(long arg){ usleep(2); char buffer[50]; char* string; while(1){ rt_sem_p(&sem_obs,TM_INFINITE); rt_mutex_acquire(&mutex_obs,TM_INFINITE); printf("******************** PRINT TASK ********************\n"); tabObstaclePris=1; obstacle* temp = obstacleList; while(temp->nxt != NULL){ printf("Obstacle found ! Distance : %d\n",temp->distance); temp = temp->nxt; } tabObstaclePris = 0; temp= obstacleList; temp->nxt=NULL; temp->distance=0; rt_mutex_release(&mutex_obs); //Fin de la simulation, calcul du temps d'exécution gettimeofday(&tim, NULL); stop = tim.tv_sec+(tim.tv_usec/1000000.0); elapsed = stop-start; /********** Ecriture dans le fichier **********/ file = fopen("output", "a"); string = "Lap time : "; sprintf(buffer, "%lf", elapsed); fputs(string, file); fputs(buffer, file); fputs("\n", file); fclose(file); /********** Fin ecriture dans le fichier **********/ printf("Time : %lf\n", elapsed); gettimeofday(&tim, NULL); start = tim.tv_sec+(tim.tv_usec/1000000.0); } }
void taskH ( void ) { rt_sem_p(&xenomai_semaphore, TM_INFINITE); // wait for synch print_pri(&taskHigh, "TaskH started\n"); rt_task_sleep(2*TIME_UNIT); rt_mutex_acquire(&resMut,NULL); // lock resource print_pri(&taskHigh, "TaskH locked resource\n"); rt_task_sleep(2*TIME_UNIT); rt_mutex_release(&resMut); // unlock the resource print_pri(&taskHigh, "TaskH released resource\n"); }
void taskL ( void ) { rt_sem_p(&xenomai_semaphore, TM_INFINITE); // wait for synch print_pri(&taskLow, "TaskL started\n"); rt_mutex_acquire(&resMut,NULL); // lock resource print_pri(&taskLow, "TaskL locked resource\n"); busy_wait_ms(3*TIME_UNIT_MS); print_pri(&taskLow, "TaskL completed work\n"); print_pri(&taskLow, "TaskL released resource\n"); rt_mutex_release(&resMut); // release resource }
void task(void *arg) { rt_sem_p(&semGlobal, TM_INFINITE); int a = * (int *) arg; RT_TASK *curtask; RT_TASK_INFO curtaskinfo; curtask=rt_task_self(); rt_task_inquire(curtask,&curtaskinfo); rt_printf("Task name: %s arg: %d \n", curtaskinfo.name, a); }
void batterie(void *arg) { rt_printf("tconnect : Attente du sémarphore semConnectedRobotbatterie\n"); rt_sem_p(&semConnectedRobot, TM_INFINITE); int status=1; int niveau_batterie=0; DBattery *bat= d_new_battery(); DMessage *message; rt_printf("tbatterie : Debut de l'éxecution de periodique à 250ms\n"); rt_task_set_periodic(NULL, TM_NOW, 250000000); while (1) { rt_task_wait_period(NULL); rt_printf("tbatterie : Activation périodique\n"); rt_mutex_acquire(&mutexEtat, TM_INFINITE); status = etatCommMoniteur; rt_mutex_release(&mutexEtat); if (status == STATUS_OK) { rt_mutex_acquire(&mutexRobot, TM_INFINITE); status=d_robot_get_vbat(robot, &niveau_batterie); rt_mutex_release(&mutexRobot); rt_mutex_acquire(&mutexEtat, TM_INFINITE); etatCommRobot=status; rt_mutex_release(&mutexEtat); rt_printf("Niveau de la batterie : %d\n", niveau_batterie); rt_printf("Status : %d\n", status); if(status == STATUS_OK) { message=d_new_message(); d_battery_set_level(bat,niveau_batterie); d_message_put_battery_level(message, bat); rt_mutex_acquire(&mutexCom, TM_INFINITE); if (write_in_queue(&queueMsgGUI, message, sizeof (DMessage)) < 0) { message->free(message); } rt_mutex_release(&mutexCom); } } } }
static void test_task(void *arg) { int ret; traceobj_enter(&trobj); traceobj_mark(&trobj, 6); ret = rt_sem_p(&sem, TM_INFINITE); traceobj_check(&trobj, ret, 0); traceobj_mark(&trobj, 7); traceobj_exit(&trobj); }