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