alejeune/Temps-Reel_Lejeune_Robert_Lacroix
1
0
Förgrening 0
Kod Ärenden Pull-förfrågningar Släpp Wiki Aktiviteter

final version

Bläddra i källkod
This commit is contained in:
Gwenael Robert 2023-04-14 10:20:25 +02:00
förälder 6113bf708f
incheckning 74fd4f4793
3 ändrade filer med 69 tillägg och 77 borttagningar
Show stats Download patch file Download diff file Expand all files Collapse all files

6
software/raspberry/superviseur-robot/nbproject/private/timestamps-10.105.1.1-xenomai-22
Visa fil

@ -1,4 +1,4 @@
#Fri Apr 14 09:59:49 CEST 2023
#Fri Apr 14 10:14:29 CEST 2023
/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/base64/README.md=c1681393207260
/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/base64/.gitignore=c1681393207255
/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/commonitor.h=c1681393207285
@ -16,13 +16,13 @@
/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/nbproject/Makefile-Debug__PC_.mk=c1681393226957
/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/base64/base64.cpp=c1681393207264
/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/comrobot.cpp=c1681393226947
/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/tasks.h=c1681396618984
/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/tasks.h=c1681459878478
/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/gdbsudo.sh=c1681393207246
/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/camera.cpp=c1681393226941
/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/.dep.inc=c1681393207120
/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/nbproject/Makefile-variables.mk=c1681393207321
/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/base64/test.cpp=c1681393207272
/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/tasks.cpp=c1681459179257
/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/tasks.cpp=c1681460062608
/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/img.cpp=c1681393226950
/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/nbproject/Makefile-Debug__RPI_.mk=c1681393226960
/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/superviseur.doxygen=c1681393207355

139
software/raspberry/superviseur-robot/tasks.cpp
Visa fil

@ -54,63 +54,6 @@
* semaphore, etc.)
*/
Message* Tasks::parseMessage(Message* msg){
int state;
rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
int tempRobotStarted = robotStarted ;
rt_mutex_release(&mutex_robotStarted);
if (tempRobotStarted != 0) {
if (msg->CompareID(MESSAGE_ANSWER_NACK) ||msg->CompareID(MESSAGE_ANSWER_ROBOT_TIMEOUT) ||msg->CompareID(MESSAGE_ANSWER_ROBOT_UNKNOWN_COMMAND) ||msg->CompareID(MESSAGE_ANSWER_ROBOT_ERROR) ||msg->CompareID(MESSAGE_ANSWER_COM_ERROR)){
// increment
rt_mutex_acquire(&mutex_compteurRobotLoss, TM_INFINITE);
compteurRobotLoss ++;
cout << "Current loss value " << compteurRobotLoss << endl << flush;
rt_mutex_release(&mutex_compteurRobotLoss);
} else {
// reset
rt_mutex_acquire(&mutex_compteurRobotLoss, TM_INFINITE);
if (compteurRobotLoss > 0){
cout << "Loss value reset" << endl << flush;
}
compteurRobotLoss = 0;
rt_mutex_release(&mutex_compteurRobotLoss);
}
rt_mutex_acquire(&mutex_compteurRobotLoss, TM_INFINITE);
if(compteurRobotLoss >= 3){
// send info to monitor
WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_COM_ERROR)); // TODO : c'est probablement faux mais on a suivi la doc *sig*
cout << "Lost communication with robot " << endl << flush;
// close robot
rt_mutex_acquire(&mutex_robot, TM_INFINITE);
state = robot.Close();
rt_mutex_release(&mutex_robot);
rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
robotStarted = 0;
rt_mutex_release(&mutex_robotStarted);
rt_mutex_acquire(&mutex_compteurRobotLoss, TM_INFINITE);
compteurRobotLoss = 0;
rt_mutex_release(&mutex_compteurRobotLoss);
// send info to monitor
if (state < 0) {
WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_NACK));
} else {
WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_ACK));
}
}
rt_mutex_release(&mutex_compteurRobotLoss);
}
return msg;
}
void Tasks::Init() {
int status;
int err;
@ -203,9 +146,6 @@ void Tasks::Init() {
if (err = rt_sem_create(&sem_reloadMessages, NULL, 0, S_FIFO)) {
cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}if (err = rt_sem_create(&sem_answerSync, NULL, 0, S_FIFO)) {
cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
cout << "Semaphores created successfully" << endl << flush;
@ -585,7 +525,7 @@ void Tasks::StartRobotTask(void *arg) {
}
/**
* @brief Thread starting the communication with the robot.
* @brief Thread starting the communication with the robot with watchdog.
*/
// TODO : refactor with the previous : if (watchdog) {call with; start the watchdog messages tasks} else {call without}
@ -613,7 +553,6 @@ void Tasks::StartRobotWithWatchDogTask(void *arg) {
if (msgSend->GetID() == MESSAGE_ANSWER_ACK) {
rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
cout << "Ca a strat avec watchdog" << endl;
robotStarted = 1;
rt_mutex_release(&mutex_robotStarted);
}
@ -631,20 +570,16 @@ void Tasks::RobotReloadMessage(void *arg) {
cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
// Synchronization barrier (waiting that all tasks are starting)
rt_sem_p(&sem_barrier, TM_INFINITE);
rt_sem_p(&sem_reloadMessages, TM_INFINITE);
cout << "Starting to monitor packet losses " << endl << flush;
rt_task_set_periodic(NULL, TM_NOW, 1000*1000*1000);
while(1) {
rt_task_wait_period(NULL);
rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
int tempRobotStarted = robotStarted ;
rt_mutex_release(&mutex_robotStarted);
@ -729,7 +664,6 @@ void Tasks::StartCameraTask(void *arg) {
WriteInQueue(&q_messageToMon, msg);
cout << "Fin de start cam" << endl << flush;
}
}
@ -793,7 +727,6 @@ void Tasks::BatteryTask(void *arg) {
rt_mutex_release(&mutex_robotStarted);
if (rs == 1) {
rt_mutex_acquire(&mutex_robot, TM_INFINITE);
Message* batteryReply = parseMessage(robot.Write(robot.GetBattery()));
rt_mutex_release(&mutex_robot);
@ -830,7 +763,6 @@ void Tasks::CameraTask(void *arg){
rt_mutex_acquire(&mutex_image, TM_INFINITE);
int statusGetImage = getImage;
rt_mutex_release(&mutex_image);
cout << "Statut du traitement " << statusGetImage << endl << flush;
if(statusGetImage){
// Lancer le traitement périodique
//cout << "Traitement de l'image" << endl << flush ;
@ -842,8 +774,6 @@ void Tasks::CameraTask(void *arg){
rt_mutex_acquire(&mutex_search_arena, TM_INFINITE);
arena = image->SearchArena(); // Mise a jour de l'arene tout le temps pour la position du robot mais draw si search arena = true
if(searchArena){
cout << "Arret du traitment !!!!" << endl << flush;
statusGetImage = 0;
if(arena.IsEmpty()){
cout << "Arena is empty" << endl << flush;
@ -875,8 +805,6 @@ void Tasks::CameraTask(void *arg){
}
}
rt_mutex_release(&mutex_robot_pos);
// Envoi de l'image
cout << "Envoi de l'image" << endl << flush ;
imgToSend = new MessageImg(MESSAGE_CAM_IMAGE,image);
WriteInQueue(&q_messageToMon, imgToSend);
@ -912,6 +840,69 @@ void Tasks::CameraTask(void *arg){
}
}
/**
* @brief verifie si la connexion avec le robot est toujours opérationnel
*/
Message* Tasks::parseMessage(Message* msg){
int state;
rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
int tempRobotStarted = robotStarted ;
rt_mutex_release(&mutex_robotStarted);
if (tempRobotStarted != 0) {
if (msg->CompareID(MESSAGE_ANSWER_NACK) ||msg->CompareID(MESSAGE_ANSWER_ROBOT_TIMEOUT) ||msg->CompareID(MESSAGE_ANSWER_ROBOT_UNKNOWN_COMMAND) ||msg->CompareID(MESSAGE_ANSWER_ROBOT_ERROR) ||msg->CompareID(MESSAGE_ANSWER_COM_ERROR)){
// increment
rt_mutex_acquire(&mutex_compteurRobotLoss, TM_INFINITE);
compteurRobotLoss ++;
cout << "Current loss value " << compteurRobotLoss << endl << flush;
rt_mutex_release(&mutex_compteurRobotLoss);
} else {
// reset
rt_mutex_acquire(&mutex_compteurRobotLoss, TM_INFINITE);
if (compteurRobotLoss > 0){
cout << "Loss value reset" << endl << flush;
}
compteurRobotLoss = 0;
rt_mutex_release(&mutex_compteurRobotLoss);
}
rt_mutex_acquire(&mutex_compteurRobotLoss, TM_INFINITE);
if(compteurRobotLoss >= 3){
// send info to monitor
WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_COM_ERROR)); // TODO : c'est probablement faux mais on a suivi la doc *sig*
cout << "================================== " << endl << flush;
cout << "|| Lost communication with robot || " << endl << flush;
cout << "================================== " << endl << flush;
// close robot
rt_mutex_acquire(&mutex_robot, TM_INFINITE);
state = robot.Close();
rt_mutex_release(&mutex_robot);
rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
robotStarted = 0;
rt_mutex_release(&mutex_robotStarted);
rt_mutex_acquire(&mutex_compteurRobotLoss, TM_INFINITE);
compteurRobotLoss = 0;
rt_mutex_release(&mutex_compteurRobotLoss);
// send info to monitor
if (state < 0) {
WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_NACK));
} else {
WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_ACK));
}
}
rt_mutex_release(&mutex_compteurRobotLoss);
}
return msg;
}
/**
* Write a message in a given queue
* @param queue Queue identifier
@ -944,3 +935,5 @@ Message *Tasks::ReadInQueue(RT_QUEUE *queue) {
return msg;
}

1
software/raspberry/superviseur-robot/tasks.h
Visa fil

@ -118,7 +118,6 @@ private:
RT_SEM sem_startRobot;
RT_SEM sem_startRobotWithWatchdog;
RT_SEM sem_startCamera;
RT_SEM sem_answerSync;
RT_SEM sem_connexionEstablished;
RT_SEM sem_reloadMessages;