finished
This commit is contained in:
Lacroix Raphael
parent
95f1cacc5e
commit
c81b6379fe
5 changed files with 150 additions and 66 deletions
|
|
@ -119,7 +119,7 @@
|
|||
</conf>
|
||||
<conf name="Debug__RPI_" type="1">
|
||||
<toolsSet>
|
||||
<compilerSet>default</compilerSet>
|
||||
<compilerSet>GNU|GNU</compilerSet>
|
||||
<dependencyChecking>true</dependencyChecking>
|
||||
<rebuildPropChanged>false</rebuildPropChanged>
|
||||
</toolsSet>
|
||||
|
|
|
|||
|
|
@ -41,7 +41,7 @@
|
|||
</conf>
|
||||
<conf name="Debug__RPI_" type="1">
|
||||
<toolsSet>
|
||||
<developmentServer>xenomai@10.105.1.8:22</developmentServer>
|
||||
<developmentServer>xenomai@10.105.1.10:22</developmentServer>
|
||||
<platform>2</platform>
|
||||
</toolsSet>
|
||||
<dbx_gdbdebugger version="1">
|
||||
|
|
|
|||
|
|
@ -7,9 +7,12 @@
|
|||
<editor-bookmarks xmlns="http://www.netbeans.org/ns/editor-bookmarks/2" lastBookmarkId="0"/>
|
||||
<open-files xmlns="http://www.netbeans.org/ns/projectui-open-files/2">
|
||||
<group>
|
||||
<file>file:/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/comrobot.h</file>
|
||||
<file>file:/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/tasks.cpp</file>
|
||||
<file>file:/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/messages.h</file>
|
||||
<file>file:/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/main.cpp</file>
|
||||
<file>file:/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/tasks.h</file>
|
||||
<file>file:/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/comrobot.cpp</file>
|
||||
</group>
|
||||
</open-files>
|
||||
</project-private>
|
||||
|
|
|
|||
|
|
@ -1,39 +1,76 @@
|
|||
#Fri Mar 31 12:07:07 CEST 2023
|
||||
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/messages.cpp=c1680248859421
|
||||
#Tue Apr 11 12:36:14 CEST 2023
|
||||
/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/messages.cpp=c1680524819149
|
||||
/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/camera.cpp=c1680524985794
|
||||
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/nbproject/Package-Debug__RPI_.bash=c1679401432930
|
||||
/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/Makefile=c1680524818973
|
||||
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/commonitor.cpp=c1679401432864
|
||||
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/gdbsudo.sh=c1679401432813
|
||||
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/base64/LICENSE=c1679401432832
|
||||
/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/base64/README.md=c1680524819097
|
||||
/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/nbproject/Makefile-variables.mk=c1681203554260
|
||||
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/comrobot.cpp=c1679401457314
|
||||
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/tasks.cpp=c1680257220969
|
||||
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/.dep.inc=c1679401432701
|
||||
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/nbproject/Makefile-variables.mk=c1679401432922
|
||||
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/img.cpp=c1679401457320
|
||||
/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/nbproject/Package-Debug__PC_.bash=c1680524819176
|
||||
/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/nbproject/private/Makefile-variables.mk=c1680524819187
|
||||
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/camera.h=c1680256228274
|
||||
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/README.md=c1679401432714
|
||||
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/nbproject/Makefile-Debug__PC_.mk=c1679401457337
|
||||
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/Makefile=c1679401432709
|
||||
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/.gitignore=c1679401432820
|
||||
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/tasks.h=c1680253954166
|
||||
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/wrapper.c=c1679401457381
|
||||
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/main.cpp=c1679404095224
|
||||
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/nbproject/project.properties=c1679401432983
|
||||
/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/camera.h=c1680524985797
|
||||
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/camera.cpp=c1679401457304
|
||||
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/base64/test.cpp=c1679401432851
|
||||
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/base64/base64.h=c1679401432844
|
||||
/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/README.md=c1680524818978
|
||||
/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/img.cpp=c1680524985804
|
||||
/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/.gitignore=c1680524819087
|
||||
/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/base64/.gitignore=c1680524819092
|
||||
/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/nbproject/Makefile-Debug__RPI_.mk=c1681203554238
|
||||
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/nbproject/Makefile-Debug__RPI_.mk=c1679401457346
|
||||
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/nbproject/Package-Debug__PC_.bash=c1679401432926
|
||||
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/nbproject/project.xml=c1679401457377
|
||||
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/base64/.gitignore=c1679401432827
|
||||
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/nbproject/Makefile-impl.mk=c1679401432918
|
||||
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/nbproject/private/Makefile-variables.mk=c1679401432944
|
||||
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/superviseur.doxygen=c1679401432993
|
||||
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/messages.h=c1679401432897
|
||||
/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/base64/base64.h=c1680524819103
|
||||
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/base64/base64.cpp=c1679401432840
|
||||
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/comrobot.h=c1679401432877
|
||||
/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/base64/test.cpp=c1680524819116
|
||||
/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/comrobot.h=c1680524819138
|
||||
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/img.h=c1679401457331
|
||||
/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/nbproject/Makefile-impl.mk=c1680524819168
|
||||
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/base64/README.md=c1679401432836
|
||||
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/base64/compile-and-run-test=c1679401432848
|
||||
/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/tasks.cpp=c1681209371364
|
||||
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/messages.cpp=c1680248859421
|
||||
/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/wrapper.c=c1680524985856
|
||||
/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/.gitignore=c1680524818971
|
||||
/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/base64/compile-and-run-test=c1680524819108
|
||||
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/base64/LICENSE=c1679401432832
|
||||
/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/superviseur.doxygen=c1680524819223
|
||||
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/tasks.cpp=c1680257220969
|
||||
/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/main.cpp=c1680524819156
|
||||
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/.dep.inc=c1679401432701
|
||||
/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/base64/LICENSE=c1680524819095
|
||||
/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/base64/base64.cpp=c1680524819101
|
||||
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/nbproject/Makefile-variables.mk=c1679401432922
|
||||
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/img.cpp=c1679401457320
|
||||
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/README.md=c1679401432714
|
||||
/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/.dep.inc=c1680524818965
|
||||
/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/nbproject/Makefile-Debug__PC_.mk=c1680524985812
|
||||
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/.gitignore=c1679401432820
|
||||
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/wrapper.c=c1679401457381
|
||||
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/main.cpp=c1679404095224
|
||||
/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/comrobot.cpp=c1680524985801
|
||||
/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/nbproject/project.properties=c1680524819214
|
||||
/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/img.h=c1680524985807
|
||||
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/base64/test.cpp=c1679401432851
|
||||
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/base64/base64.h=c1679401432844
|
||||
/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/commonitor.h=c1680524819130
|
||||
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/nbproject/Package-Debug__PC_.bash=c1679401432926
|
||||
/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/gdbsudo.sh=c1680524819082
|
||||
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/nbproject/project.xml=c1679401457377
|
||||
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/nbproject/Makefile-impl.mk=c1679401432918
|
||||
/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/commonitor.cpp=c1680524819127
|
||||
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/superviseur.doxygen=c1679401432993
|
||||
/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/nbproject/Package-Debug__RPI_.bash=c1681203554249
|
||||
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/messages.h=c1679401432897
|
||||
/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/nbproject/project.xml=c1680524985847
|
||||
/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/messages.h=c1680524819153
|
||||
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/.gitignore=c1679401432706
|
||||
VERSION=1.3
|
||||
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/commonitor.h=c1679401432868
|
||||
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/base64/README.md=c1679401432836
|
||||
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/base64/compile-and-run-test=c1679401432848
|
||||
/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/tasks.h=c1680862388302
|
||||
|
|
|
|||
|
|
@ -94,43 +94,54 @@ void Tasks::RobotLossCounter(void* arg){
|
|||
|
||||
Message* Tasks::parseMessage(Message* msg){
|
||||
int state;
|
||||
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 ++;
|
||||
rt_mutex_release(&mutex_compteurRobotLoss);
|
||||
} else {
|
||||
|
||||
rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
|
||||
int tempRobotStarted = robotStarted ;
|
||||
rt_mutex_release(&mutex_robotStarted);
|
||||
|
||||
if (tempRobotStarted != 0) {
|
||||
|
||||
// reset
|
||||
rt_mutex_acquire(&mutex_compteurRobotLoss, TM_INFINITE);
|
||||
compteurRobotLoss = 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);
|
||||
|
||||
|
||||
// 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);
|
||||
}
|
||||
|
||||
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*
|
||||
|
||||
// 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);
|
||||
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
||||
|
|
@ -462,6 +473,23 @@ void Tasks::ReceiveFromMonTask(void *arg) {
|
|||
rt_mutex_acquire(&mutex_robot_pos, TM_INFINITE);
|
||||
robotPos = 0;
|
||||
rt_mutex_release(&mutex_robot_pos);
|
||||
}else if(msgRcv->CompareID(MESSAGE_MONITOR_LOST)){
|
||||
cout << "MONITOR LOST" << endl << flush;
|
||||
|
||||
rt_mutex_acquire(&mutex_robot, TM_INFINITE);
|
||||
robot.Close();
|
||||
rt_mutex_release(&mutex_robot);
|
||||
|
||||
|
||||
rt_mutex_acquire(&mutex_camera, TM_INFINITE);
|
||||
camera.Close();
|
||||
rt_mutex_release(&mutex_camera);
|
||||
|
||||
|
||||
rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
|
||||
robotStarted = 0;
|
||||
rt_mutex_release(&mutex_robotStarted);
|
||||
|
||||
}
|
||||
delete(msgRcv); // must be deleted manually, no consumer
|
||||
}
|
||||
|
|
@ -497,6 +525,8 @@ void Tasks::OpenComRobot(void *arg) {
|
|||
rt_sem_v(&sem_connexionEstablished);
|
||||
msgSend = new Message(MESSAGE_ANSWER_ACK);
|
||||
}
|
||||
|
||||
cout << "Open serial com (";
|
||||
WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon
|
||||
}
|
||||
}
|
||||
|
|
@ -580,21 +610,35 @@ void Tasks::RobotReloadMessage(void *arg) {
|
|||
int status;
|
||||
|
||||
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_robot, TM_INFINITE);
|
||||
status = parseMessage(robot.Write(robot.ReloadWD()))->GetID();
|
||||
rt_mutex_release(&mutex_robot);
|
||||
// send info to monitor
|
||||
if (status < 0) {
|
||||
WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_NACK));
|
||||
} else {
|
||||
WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_ACK));
|
||||
|
||||
|
||||
rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
|
||||
int tempRobotStarted = robotStarted ;
|
||||
rt_mutex_release(&mutex_robotStarted);
|
||||
|
||||
if (tempRobotStarted != 0) {
|
||||
rt_mutex_acquire(&mutex_robot, TM_INFINITE);
|
||||
status = parseMessage(robot.Write(robot.ReloadWD()))->GetID();
|
||||
rt_mutex_release(&mutex_robot);
|
||||
// send info to monitor
|
||||
if (status < 0) {
|
||||
WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_NACK));
|
||||
} else {
|
||||
WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_ACK));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -666,18 +710,18 @@ void Tasks::MoveTask(void *arg) {
|
|||
//cout << "Periodic movement update";
|
||||
rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
|
||||
rs = robotStarted;
|
||||
rt_mutex_release(&mutex_robotStarted);
|
||||
if (rs == 1) {
|
||||
rt_mutex_acquire(&mutex_move, TM_INFINITE);
|
||||
cpMove = move;
|
||||
rt_mutex_release(&mutex_move);
|
||||
|
||||
cout << " move: " << cpMove;
|
||||
cout << " move: " << cpMove << endl << flush;
|
||||
|
||||
rt_mutex_acquire(&mutex_robot, TM_INFINITE);
|
||||
parseMessage(robot.Write(new Message((MessageID)cpMove)));
|
||||
rt_mutex_release(&mutex_robot);
|
||||
}
|
||||
rt_mutex_release(&mutex_robotStarted);
|
||||
//cout << endl << flush;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in a new issue