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 envoyer_image(void *arg) { DMessage *message = d_new_message(); DMessage *message_position = d_new_message(); DMessage *message_arena = d_new_message(); DImage *image = d_new_image(); DJpegimage *jpegimage = d_new_jpegimage(); char arena_found[] = "arena found"; rt_printf("tstreaming : Envoi de l'image à ~2.5img/s\n"); rt_task_set_periodic(NULL, TM_NOW, 400000000); while (1) { /* Attente de l'activation périodique */ rt_task_wait_period(NULL); rt_mutex_acquire(&mutexEtatCamera, TM_INFINITE); camera->get_frame(camera, image); switch (etat_camera) { case 0 : /* streaming simple */ rt_mutex_release(&mutexEtatCamera); break; case 1 : /* streaming avec arena */ rt_mutex_release(&mutexEtatCamera); /* détection de l'arène */ arena = (DArena *) image->compute_arena_position(image); /*check si l'arene a été trouvée*/ if (arena != NULL) { x_arena = arena->get_x(arena); y_arena = arena->get_y(arena); //rt_printf("x = %f; y = %f\n ", x_arena, y_arena); /*envoie du message confirmant que l'arène a été trouvée*/ message_arena->set(message_arena, ACTION_ARENA_IS_FOUND, strlen(arena_found), arena_found); serveur->send(serveur, message_arena); d_imageshop_draw_arena(image, arena); } break; case 2 : /* streaming avec position du robot */ rt_mutex_release(&mutexEtatCamera); /* détection de la position du robot */ arena = (DArena *) image->compute_arena_position(image); rt_mutex_acquire(&mutexPosition, TM_INFINITE); position_robot = (DPosition *) image->compute_robot_position(image, arena); if (position_robot != NULL) { x_robot = position_robot->get_x(position_robot); y_robot = position_robot->get_y(position_robot); orientation_robot = position_robot->get_orientation(position_robot); rt_mutex_release(&mutexPosition); /* rt_printf("\n\n\nx = %f; y = %f, angle = %f\n\n\n ", x_robot, y_robot, orientation_robot); */ /*envoie de la position par message*/ message_position->put_position(message_position, position_robot); serveur->send(serveur, message_position); d_imageshop_draw_position(image, position_robot); } break; case 3 : /* streaming avec position de l'arene et du robot */ rt_mutex_release(&mutexEtatCamera); /* détection de la position du robot et de l'arene*/ arena = (DArena *) image->compute_arena_position(image); rt_mutex_acquire(&mutexPosition, TM_INFINITE); position_robot = (DPosition *) image->compute_robot_position(image, arena); if (position_robot != NULL) { x_robot = position_robot->get_x(position_robot); y_robot = position_robot->get_y(position_robot); /* rt_printf("\n\n\nx = %f; y = %f\n\n\n ", x_robot, y_robot); */ rt_mutex_release(&mutexPosition); /*envoie de la position par message*/ message_position->put_position(message_position, position_robot); serveur->send(serveur, message_position); d_imageshop_draw_position(image, position_robot); } /*check si l'arene a été trouvée*/ if (arena != NULL) { x_arena = arena->get_x(arena); y_arena = arena->get_y(arena); /* rt_printf("x = %f; y = %f\n ", x_arena, y_arena); */ /*envoie du message confirmant que l'arène a été trouvée*/ message_arena->set(message_arena, ACTION_ARENA_IS_FOUND, strlen(arena_found), arena_found); serveur->send(serveur, message_arena); d_imageshop_draw_arena(image, arena); } break; default : rt_printf("valeur de etat_camera corrompue\n"); } /*compression et envoie de l'image*/ jpegimage->compress(jpegimage, image); //jpegimage->print(jpegimage); message->put_jpeg_image(message, jpegimage); serveur->send(serveur, message); } }