void startup(){ rt_queue_create(&rqueue, "rQueue", QUEUE_SIZE, 40, Q_FIFO); rt_queue_create(&lqueue, "lQueue", QUEUE_SIZE, 40, Q_FIFO); //rt_sem_create(&rsem, "rsem", 0, S_FIFO); //&task, name, stack size (0 - default), priority, mode rt_task_create(&rEnc_task, "rEnc Task", 0, 50, 0); //&task, task function, function argument rt_task_start(&rEnc_task, &rEnc, 0); //&task, name, stack size (0 - default), priority, mode rt_task_create(&lEnc_task, "lEnc Task", 0, 50, 0); //&task, task function, function argument rt_task_start(&lEnc_task, &lEnc, 0); //&task, name, stack size (0 - default), priority, mode rt_task_create(&Odo_task, "Odo Task", 0, 60, 0); //&task, task function, function argument rt_task_start(&Odo_task, &Odo, 0); //rt_sem_broadcast(&rsem); }
/* public functions ========================================================= */ int speed_init(void) { if (running) { goto err_running; } if ((ostream = fopen("speed", "w")) == NULL) { goto err_ostream; } rt_intr_create(&intr, NULL, 81, 0); rt_intr_enable(&intr); rt_mutex_create(&mutex_instant, NULL); rt_mutex_create(&mutex_average, NULL); rt_queue_create(&queue, "irq81", QUEUE_SIZE, Q_UNLIMITED, Q_FIFO); rt_task_spawn(&task_soft, NULL, 0, 80, 0, task_soft_routine, NULL); rt_task_spawn(&task_hard, NULL, 0, 90, 0, task_hard_routine, NULL); instant = 0; average = 0; running = 1; return 0; err_ostream: err_running: return -1; }
//startup code void startup() { int i; char str[10] ; void (*task_func[NTASKS]) (void *arg); task_func[0]=taskOne; task_func[1]=taskTwo; rt_queue_create(&myqueue,"myqueue",QUEUE_SIZE,10,Q_FIFO); rt_timer_set_mode(0);// set timer to tick in nanoseconds and not in jiffies for(i=0; i < NTASKS; i++) { rt_printf("start task : %d\n",i); sprintf(str,"task%d",i); rt_task_create(&task_struct[i], str, 0, 50, 0); rt_task_start(&task_struct[i], task_func[i], &i); } }
void latency (void *cookie) { int err, count, nsamples, warmup = 1; RTIME expected_tsc, period_tsc, start_ticks; RT_TIMER_INFO timer_info; RT_QUEUE q; rt_queue_create(&q, "queue", 0, 100, 0); if (!(hard_timer_running = rt_is_hard_timer_running())) { err = rt_timer_start(TM_ONESHOT); if (err) { fprintf(stderr,"latency: cannot start timer, code %d\n",err); return; } } err = rt_timer_inquire(&timer_info); if (err) { fprintf(stderr,"latency: rt_timer_inquire, code %d\n",err); return; } nsamples = ONE_BILLION / period_ns / 1; period_tsc = rt_timer_ns2tsc(period_ns); /* start time: one millisecond from now. */ start_ticks = timer_info.date + rt_timer_ns2ticks(1000000); expected_tsc = timer_info.tsc + rt_timer_ns2tsc(1000000); err = rt_task_set_periodic(NULL,start_ticks,period_ns); if (err) { fprintf(stderr,"latency: failed to set periodic, code %d\n",err); return; } for (;;) { long minj = TEN_MILLION, maxj = -TEN_MILLION, dt, sumj; long overrun = 0; test_loops++; for (count = sumj = 0; count < nsamples; count++) { expected_tsc += period_tsc; err = rt_task_wait_period(NULL); if (err) { if (err != -ETIMEDOUT) { rt_queue_delete(&q); rt_task_delete(NULL); /* Timer stopped. */ } overrun++; } dt = (long)(rt_timer_tsc() - expected_tsc); if (dt > maxj) maxj = dt; if (dt < minj) minj = dt; sumj += dt; if (!(finished || warmup) && (do_histogram || do_stats)) add_histogram(histogram_avg, dt); } if(!warmup) { if (!finished && (do_histogram || do_stats)) { add_histogram(histogram_max, maxj); add_histogram(histogram_min, minj); } minjitter = minj; if(minj < gminjitter) gminjitter = minj; maxjitter = maxj; if(maxj > gmaxjitter) gmaxjitter = maxj; avgjitter = sumj / nsamples; gavgjitter += avgjitter; goverrun += overrun; rt_sem_v(&display_sem); struct smpl_t { long minjitter, avgjitter, maxjitter, overrun; } *smpl; smpl = rt_queue_alloc(&q, sizeof(struct smpl_t)); #if 1 smpl->minjitter = rt_timer_tsc2ns(minj); smpl->maxjitter = rt_timer_tsc2ns(maxj); smpl->avgjitter = rt_timer_tsc2ns(sumj / nsamples); smpl->overrun = goverrun; rt_queue_send(&q, smpl, sizeof(struct smpl_t), TM_NONBLOCK); #endif } if(warmup && test_loops == WARMUP_TIME) { test_loops = 0; warmup = 0; } } }
int main(void) { unsigned long long before; RT_ALARM nalrm; RT_BUFFER nbuf; RT_COND ncond; RT_EVENT nevt; RT_HEAP nheap; RT_MUTEX nmtx; RT_PIPE npipe; RT_QUEUE nq; RT_SEM nsem; RT_TASK ntsk; int failed = 0; mlockall(MCL_CURRENT|MCL_FUTURE); rt_print_auto_init(1); rt_fprintf(stderr, "Checking for leaks in native skin services\n"); before = get_used(); check_native(rt_alarm_create(&nalrm, NULL)); check_native(rt_alarm_delete(&nalrm)); check_used("alarm", before, failed); before = get_used(); check_native(rt_buffer_create(&nbuf, NULL, 16384, B_PRIO)); check_native(rt_buffer_delete(&nbuf)); check_used("buffer", before, failed); before = get_used(); check_native(rt_cond_create(&ncond, NULL)); check_native(rt_cond_delete(&ncond)); check_used("cond", before, failed); before = get_used(); check_native(rt_event_create(&nevt, NULL, 0, EV_PRIO)); check_native(rt_event_delete(&nevt)); check_used("event", before, failed); before = get_used(); check_native(rt_heap_create(&nheap, "heap", 16384, H_PRIO | H_SHARED)); check_native(rt_heap_delete(&nheap)); check_used("heap", before, failed); before = get_used(); check_native(rt_mutex_create(&nmtx, NULL)); check_native(rt_mutex_delete(&nmtx)); check_used("mutex", before, failed); before = get_used(); check_native(rt_pipe_create(&npipe, NULL, P_MINOR_AUTO, 0)); check_native(rt_pipe_delete(&npipe)); check_used("pipe", before, failed); before = get_used(); check_native(rt_queue_create(&nq, "queue", 16384, Q_UNLIMITED, Q_PRIO)); check_native(rt_queue_delete(&nq)); check_used("queue", before, failed); before = get_used(); check_native(rt_sem_create(&nsem, NULL, 0, S_PRIO)); check_native(rt_sem_delete(&nsem)); check_used("sem", before, failed); before = get_used(); check_native(rt_task_spawn(&ntsk, NULL, 0, 1, T_JOINABLE, empty, NULL)); check_native(rt_task_join(&ntsk)); sleep(1); /* Leave some time for xnheap * deferred free */ check_used("task", before, failed); return failed ? EXIT_FAILURE : EXIT_SUCCESS; }
void initStruct(void) { int err; /* Creation des mutex */ if (err = rt_mutex_create(&mutexEtat, NULL)) { rt_printf("Error mutex create: %s\n", strerror(-err)); exit(EXIT_FAILURE); } if (err = rt_mutex_create(&mutexMove, NULL)) { rt_printf("Error mutex create: %s\n", strerror(-err)); exit(EXIT_FAILURE); } if (err = rt_mutex_create(&mutexRobot, NULL)) { rt_printf("Error mutex create: %s\n", strerror(-err)); exit(EXIT_FAILURE); } if (err = rt_mutex_create(&mutexServer, NULL)) { rt_printf("Error mutex create: %s\n", strerror(-err)); exit(EXIT_FAILURE); } if (err = rt_mutex_create(&mutexArene, NULL)) { rt_printf("Error mutex create: %s\n", strerror(-err)); exit(EXIT_FAILURE); } if (err = rt_mutex_create(&mutexImage, NULL)) { rt_printf("Error mutex create: %s\n", strerror(-err)); exit(EXIT_FAILURE); } if (err = rt_mutex_create(&mutexPositionRobot, NULL)) { rt_printf("Error mutex create: %s\n", strerror(-err)); exit(EXIT_FAILURE); } if (err = rt_mutex_create(&mutexPositionVoulue, NULL)) { rt_printf("Error mutex create: %s\n", strerror(-err)); exit(EXIT_FAILURE); } if (err = rt_mutex_create(&mutexValidArene, NULL)) { rt_printf("Error mutex create: %s\n", strerror(-err)); exit(EXIT_FAILURE); } /* Creation des semaphores */ if (err = rt_sem_create(&semConnecterRobot, NULL, 0, S_FIFO)) { rt_printf("Error semaphore create: %s\n", strerror(-err)); exit(EXIT_FAILURE); } if (err = rt_sem_create(&semWatchdog, NULL, 0, S_FIFO)) { rt_printf("Error semaphore create: %s\n", strerror(-err)); exit(EXIT_FAILURE); } if (err = rt_sem_create(&semPosition, NULL, 0, S_FIFO)) { rt_printf("Error semaphore create: %s\n", strerror(-err)); exit(EXIT_FAILURE); } if (err = rt_sem_create(&semAcquArene, NULL, 0, S_FIFO)) { rt_printf("Error semaphore create: %s\n", strerror(-err)); exit(EXIT_FAILURE); } if (err = rt_sem_create(&semValidArene, NULL, 0, S_FIFO)) { rt_printf("Error semaphore create: %s\n", strerror(-err)); exit(EXIT_FAILURE); } if (err = rt_sem_create(&semBattery, NULL, 0, S_FIFO)) { rt_printf("Error semaphore create: %s\n", strerror(-err)); exit(EXIT_FAILURE); } if (err = rt_sem_create(&semWebcam, NULL, 0, S_FIFO)) { rt_printf("Error semaphore create: %s\n", strerror(-err)); exit(EXIT_FAILURE); } if (err = rt_sem_create(&semMission, NULL, 0, S_FIFO)) { rt_printf("Error semaphore create: %s\n", strerror(-err)); exit(EXIT_FAILURE); } /* Creation des taches */ if (err = rt_task_create(&tcommunicate, NULL, 0, PRIORITY_TCOMMUNICATE, 0)) { rt_printf("Error task create: %s\n", strerror(-err)); exit(EXIT_FAILURE); } if (err = rt_task_create(&tconnect, NULL, 0, PRIORITY_TCONNECT, 0)) { rt_printf("Error task create: %s\n", strerror(-err)); exit(EXIT_FAILURE); } if (err = rt_task_create(&tmove, NULL, 0, PRIORITY_TMOVE, 0)) { rt_printf("Error task create: %s\n", strerror(-err)); exit(EXIT_FAILURE); } if (err = rt_task_create(&tsend, NULL, 0, PRIORITY_TSEND, 0)) { rt_printf("Error task create: %s\n", strerror(-err)); exit(EXIT_FAILURE); } if (err = rt_task_create(&twatchdog, NULL, 0, PRIORITY_TWATCHDOG, 0)) { rt_printf("Error task create: %s\n", strerror(-err)); exit(EXIT_FAILURE); } if (err = rt_task_create(&tbattery, NULL, 0, PRIORITY_TBATTERY, 0)) { rt_printf("Error task create: %s\n", strerror(-err)); exit(EXIT_FAILURE); } if (err = rt_task_create(&tcam, NULL, 0, PRIORITY_TCAM, 0)) { rt_printf("Error task create: %s\n", strerror(-err)); exit(EXIT_FAILURE); } if (err = rt_task_create(&tposition, NULL, 0, PRIORITY_TPOSITION, 0)) { rt_printf("Error task create: %s\n", strerror(-err)); exit(EXIT_FAILURE); } if (err = rt_task_create(&tarena, NULL, 0, PRIORITY_TARENA, 0)) { rt_printf("Error task create: %s\n", strerror(-err)); exit(EXIT_FAILURE); } if (err = rt_task_create(&tmission, NULL, 0, PRIORITY_TMISSION, 0)) { rt_printf("Error task create: %s\n", strerror(-err)); exit(EXIT_FAILURE); } /* Definition des periodes */ rt_task_set_periodic(&tmove, TM_NOW, 200000000); // 200ms rt_task_set_periodic(&tsend, TM_NOW, 200000000); // 200ms rt_task_set_periodic(&twatchdog, TM_NOW, 1000000000); // 1s rt_task_set_periodic(&tbattery, TM_NOW, 250000000); // 250ms rt_task_set_periodic(&tcam, TM_NOW, 600000000); // 600ms rt_task_set_periodic(&tposition, TM_NOW, 600000000); // 600ms rt_task_set_periodic(&tmission, TM_NOW, 600000000); // 600ms /* Creation des files de messages */ if (err = rt_queue_create(&queueMsgGUI, "toto", MSG_QUEUE_SIZE*sizeof(DMessage), MSG_QUEUE_SIZE, Q_FIFO)) { rt_printf("Error msg queue create: %s\n", strerror(-err)); exit(EXIT_FAILURE); } /* Creation des structures globales du projet */ areneValidee = 1; arena = 0; robot = d_new_robot(); mvt = d_new_movement(); server = d_new_server(); image = d_new_image(); positionRobot = d_new_position(); positionVoulue = d_new_position(); etat_communication = malloc(sizeof(Etat_communication_t)); etat_communication->robot = 1; etat_communication->moniteur = 1; }
void initStruct(void) { int err; /* Creation des mutex */ if (err = rt_mutex_create(&mutexEtat, NULL)) { rt_printf("Error mutex create: %s\n", strerror(-err)); exit(EXIT_FAILURE); } if (err = rt_mutex_create(&mutexMove, NULL)) { rt_printf("Error mutex create: %s\n", strerror(-err)); exit(EXIT_FAILURE); } if (err = rt_mutex_create(&mutexRobot, NULL)) { rt_printf("Error mutex create: %s\n", strerror(-err)); exit(EXIT_FAILURE); } if (err = rt_mutex_create(&mutexArena, NULL)) { rt_printf("Error mutex create: %s\n", strerror(-err)); exit(EXIT_FAILURE); } if (err = rt_mutex_create(&mutexPosition, NULL)) { rt_printf("Error mutex create: %s\n", strerror(-err)); exit(EXIT_FAILURE); } if (err = rt_mutex_create(&mutexImage, NULL)) { rt_printf("Error mutex create: %s\n", strerror(-err)); exit(EXIT_FAILURE); } if (err = rt_mutex_create(&mutexMission, NULL)) { rt_printf("Error mutex create: %s\n", strerror(-err)); exit(EXIT_FAILURE); } /* Creation du semaphore */ if (err = rt_sem_create(&semConnecterRobot, NULL, 0, S_FIFO)) { rt_printf("Error semaphore create: %s\n", strerror(-err)); exit(EXIT_FAILURE); } if (err = rt_sem_create(&semGetImage, NULL, 0, S_FIFO)) { rt_printf("Error semaphore create: %s\n", strerror(-err)); exit(EXIT_FAILURE); } if (err = rt_sem_create(&semDetectArena, NULL, 0, S_FIFO)) { rt_printf("Error semaphore create: %s\n", strerror(-err)); } if (err = rt_sem_create(&semComputePosition, NULL, 0, S_FIFO)) { rt_printf("Error semaphore create: %s\n", strerror(-err)); } if (err = rt_sem_create(&semwatchDog, NULL, 0, S_FIFO)) { rt_printf("Error semaphore create: %s\n", strerror(-err)); } if (err = rt_sem_create(&semCommunicate, NULL, 0, S_FIFO)) { rt_printf("Error semaphore create: %s\n", strerror(-err)); } if (err = rt_sem_create(&semConnect, NULL, 0, S_FIFO)) { rt_printf("Error semaphore create: %s\n", strerror(-err)); } /* Creation des taches */ if (err = rt_task_create(&tServeur, NULL, 0, PRIORITY_TSERVEUR, 0)) { rt_printf("Error task create: %s\n", strerror(-err)); exit(EXIT_FAILURE); } if (err = rt_task_create(&tconnect, NULL, 0, PRIORITY_TCONNECT, 0)) { rt_printf("Error task create: %s\n", strerror(-err)); exit(EXIT_FAILURE); } if (err = rt_task_create(&tmove, NULL, 0, PRIORITY_TMOVE, 0)) { rt_printf("Error task create: %s\n", strerror(-err)); exit(EXIT_FAILURE); } if (err = rt_task_create(&tenvoyer, NULL, 0, PRIORITY_TENVOYER, 0)) { rt_printf("Error task create: %s\n", strerror(-err)); exit(EXIT_FAILURE); } if (err = rt_task_create(&tbattery, NULL, 0, PRIORITY_TBATTERY, 0)) { rt_printf("Error task create: %s\n", strerror(-err)); exit(EXIT_FAILURE); } if (err = rt_task_create(&tcamera, NULL, 0, PRIORITY_TCAMERA, 0)) { rt_printf("Error task create: %s\n", strerror(-err)); exit(EXIT_FAILURE); } if (err = rt_task_create(&tverify, NULL, 0, PRIORITY_TVERIFY, 0)) { rt_printf("Error task create: %s\n", strerror(-err)); exit(EXIT_FAILURE); } if (err = rt_task_create(&tcomputepos, NULL, 0, PRIORITY_TCOMPUTEPOS, 0)) { rt_printf("Error task create: %s\n", strerror(-err)); exit(EXIT_FAILURE); } if (err = rt_task_create(&tarena, NULL, 0, PRIORITY_TARENA, 0)) { rt_printf("Error task create: %s\n", strerror(-err)); exit(EXIT_FAILURE); } /* Creation des files de messages */ if (err = rt_queue_create(&queueMsgGUI, "toto", MSG_QUEUE_SIZE * sizeof (DMessage), MSG_QUEUE_SIZE, Q_FIFO)) { rt_printf("Error msg queue create: %s\n", strerror(-err)); exit(EXIT_FAILURE); } /* Creation des structures globales du projet */ robot = d_new_robot(); move = d_new_movement(); serveur = d_new_server(); camera = d_new_camera(); battery = d_new_battery(); mission = NULL; position = NULL; arena = NULL; image = NULL; }