alejeune/Temps-Reel_Lejeune_Robert_Lacroix
1
0
Fork 0
Code Issues Pull requests Releases Wiki Activity

maybe ca marche maybe pas

Browse source
This commit is contained in:
Lacroix Raphael 2023-04-07 12:14:46 +02:00
parent cac70c70df
commit 95f1cacc5e
5 changed files with 130 additions and 23 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

2
software/raspberry/superviseur-robot/nbproject/private/configurations.xml
View file

@ -41,7 +41,7 @@
</conf> </conf>
<conf name="Debug__RPI_" type="1"> <conf name="Debug__RPI_" type="1">
<toolsSet> <toolsSet>
<developmentServer>xenomai@10.105.1.3:22</developmentServer> <developmentServer>xenomai@10.105.1.8:22</developmentServer>
<platform>2</platform> <platform>2</platform>
</toolsSet> </toolsSet>
<dbx_gdbdebugger version="1"> <dbx_gdbdebugger version="1">

0
software/raspberry/superviseur-robot/nbproject/private/downloads-10.105.1.8-xenomai-22 Normal file
View file

39
software/raspberry/superviseur-robot/nbproject/private/timestamps-10.105.1.8-xenomai-22 Normal file
View file

@ -0,0 +1,39 @@
#Fri Apr 07 12:13:15 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/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/tasks.cpp=c1680862388297
/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/Makefile=c1680524818973
/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/lib/base64/README.md=c1680524819097
/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/base64/compile-and-run-test=c1680524819108
/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/nbproject/Makefile-variables.mk=c1680853007538
/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/superviseur.doxygen=c1680524819223
/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/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/main.cpp=c1680524819156
/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/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/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/camera.h=c1680524985797
/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/img.h=c1680524985807
/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=c1680853007485
/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/commonitor.h=c1680524819130
/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/gdbsudo.sh=c1680524819082
/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/commonitor.cpp=c1680524819127
/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/nbproject/Package-Debug__RPI_.bash=c1680853007515
/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/base64/base64.h=c1680524819103
/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/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
VERSION=1.3
/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/nbproject/Makefile-impl.mk=c1680524819168
/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/tasks.h=c1680862388302

95
software/raspberry/superviseur-robot/tasks.cpp
View file

@ -27,8 +27,8 @@
#define PRIORITY_TSTARTROBOT 20 #define PRIORITY_TSTARTROBOT 20
#define PRIORITY_TCAMERA 21 #define PRIORITY_TCAMERA 21
#define PRIORITY_TBATTERY 25 #define PRIORITY_TBATTERY 25
#define PRIORITY_TLOSSCOUNTER 25
#define PRIORITY_TSTARTCAMERA 25 #define PRIORITY_TSTARTCAMERA 25
#define PRIORITY_TROBOTRELOADMESSAGE 25
/* /*
* Some remarks: * Some remarks:
@ -52,7 +52,6 @@
/** /**
* @brief Initialisation des structures de l'application (tâches, mutex, * @brief Initialisation des structures de l'application (tâches, mutex,
* semaphore, etc.) * semaphore, etc.)
*/
void Tasks::RobotLossCounter(void* arg){ void Tasks::RobotLossCounter(void* arg){
int state; int state;
@ -75,6 +74,10 @@ void Tasks::RobotLossCounter(void* arg){
rt_mutex_acquire(&mutex_robot, TM_INFINITE); rt_mutex_acquire(&mutex_robot, TM_INFINITE);
state = robot.Close(); state = robot.Close();
rt_mutex_release(&mutex_robot); rt_mutex_release(&mutex_robot);
rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
robotStarted = 0;
rt_mutex_release(&mutex_robotStarted);
// send info to monitor // send info to monitor
if (state < 0) { if (state < 0) {
@ -87,10 +90,11 @@ void Tasks::RobotLossCounter(void* arg){
} }
} }
*/
MessageID Tasks::parseMessage(MessageID msg){ Message* Tasks::parseMessage(Message* msg){
if (msg == MESSAGE_ANSWER_COM_ERROR){ 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 // increment
rt_mutex_acquire(&mutex_compteurRobotLoss, TM_INFINITE); rt_mutex_acquire(&mutex_compteurRobotLoss, TM_INFINITE);
compteurRobotLoss ++; compteurRobotLoss ++;
@ -102,6 +106,31 @@ MessageID Tasks::parseMessage(MessageID msg){
compteurRobotLoss = 0; compteurRobotLoss = 0;
rt_mutex_release(&mutex_compteurRobotLoss); 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; return msg;
} }
@ -183,6 +212,10 @@ void Tasks::Init() {
cerr << "Error semaphore create: " << strerror(-err) << endl << flush; cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
} }
if (err = rt_sem_create(&sem_reloadMessages, NULL, 0, S_FIFO)) {
cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
cout << "Semaphores created successfully" << endl << flush; cout << "Semaphores created successfully" << endl << flush;
/**************************************************************************************/ /**************************************************************************************/
@ -220,14 +253,23 @@ void Tasks::Init() {
cerr << "Error task create: " << strerror(-err) << endl << flush; cerr << "Error task create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
} }
if (err = rt_task_create(&th_lossCounter, "th_lossCounter", 0, PRIORITY_TLOSSCOUNTER, 0)) { /*if (err = rt_task_create(&th_lossCounter, "th_lossCounter", 0, PRIORITY_TLOSSCOUNTER, 0)) {
cerr << "Error task create: " << strerror(-err) << endl << flush; cerr << "Error task create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
} }*/
if (err = rt_task_create(&th_camera, "th_camera", 0, PRIORITY_TCAMERA, 0)) { if (err = rt_task_create(&th_camera, "th_camera", 0, PRIORITY_TCAMERA, 0)) {
cerr << "Error task create: " << strerror(-err) << endl << flush; cerr << "Error task create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
} }
if (err = rt_task_create(&th_startRobotWithWatchdog, "th_startRobotWithWatchdog", 0, PRIORITY_TSTARTROBOT, 0)) {
cerr << "Error task create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_task_create(&th_robotReloadMessage, "th_robotReloadMessage", 0, PRIORITY_TROBOTRELOADMESSAGE, 0)) {
cerr << "Error task create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
cout << "Tasks created successfully" << endl << flush; cout << "Tasks created successfully" << endl << flush;
/**************************************************************************************/ /**************************************************************************************/
@ -285,7 +327,15 @@ void Tasks::Run() {
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
} }
if (err = rt_task_start(&th_lossCounter, (void(*)(void*)) & Tasks::RobotLossCounter, this)) { /*if (err = rt_task_start(&th_lossCounter, (void(*)(void*)) & Tasks::RobotLossCounter, this)) {
cerr << "Error task start: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}*/
if (err = rt_task_start(&th_robotReloadMessage, (void(*)(void*)) & Tasks::RobotReloadMessage, this)) {
cerr << "Error task start: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_task_start(&th_startRobotWithWatchdog, (void(*)(void*)) & Tasks::StartRobotWithWatchDogTask, this)) {
cerr << "Error task start: " << strerror(-err) << endl << flush; cerr << "Error task start: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
} }
@ -500,8 +550,8 @@ void Tasks::StartRobotTask(void *arg) {
while (1) { while (1) {
Message * msgSend; Message * msgSend;
rt_sem_p(&sem_startRobot, TM_INFINITE); rt_sem_p(&sem_startRobotWithWatchdog, TM_INFINITE);
cout << "Start robot without watchdog ("; cout << "Start robot with watchdog (";
rt_mutex_acquire(&mutex_robot, TM_INFINITE); rt_mutex_acquire(&mutex_robot, TM_INFINITE);
msgSend = parseMessage(robot.Write(robot.StartWithWD())); // TODO : convert Message MessageID msgSend = parseMessage(robot.Write(robot.StartWithWD())); // TODO : convert Message MessageID
rt_mutex_release(&mutex_robot); rt_mutex_release(&mutex_robot);
@ -513,11 +563,12 @@ void Tasks::StartRobotTask(void *arg) {
if (msgSend->GetID() == MESSAGE_ANSWER_ACK) { if (msgSend->GetID() == MESSAGE_ANSWER_ACK) {
rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE); rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
cout << "Ca a strat avec watchdog" << endl;
robotStarted = 1; robotStarted = 1;
rt_mutex_release(&mutex_robotStarted); rt_mutex_release(&mutex_robotStarted);
} }
rt_sem_v(&sem_reloadMessages, TM_INFINITE); rt_sem_v(&sem_reloadMessages);
} }
} }
@ -531,14 +582,16 @@ void Tasks::RobotReloadMessage(void *arg) {
cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
// Synchronization barrier (waiting that all tasks are starting) // Synchronization barrier (waiting that all tasks are starting)
rt_sem_p(&sem_barrier, TM_INFINITE); rt_sem_p(&sem_barrier, TM_INFINITE);
rt_sem_p(&sem_reloadMessages); rt_sem_p(&sem_reloadMessages, TM_INFINITE);
cout << "Starting to monitor packet losses " << endl << flush; cout << "Starting to monitor packet losses " << endl << flush;
rt_task_set_periodic(NULL, TM_NOW, 500*1000*1000); rt_task_set_periodic(NULL, TM_NOW, 1000*1000*1000);
while(1) { while(1) {
rt_task_wait_period(NULL); rt_task_wait_period(NULL);
status = parseMessage(robot.Write(robot.reloadWD())); // TODO : convert Message MessageID rt_mutex_acquire(&mutex_robot, TM_INFINITE);
status = parseMessage(robot.Write(robot.ReloadWD()))->GetID();
rt_mutex_release(&mutex_robot);
// send info to monitor // send info to monitor
if (state < 0) { if (status < 0) {
WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_NACK)); WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_NACK));
} else { } else {
WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_ACK)); WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_ACK));
@ -603,7 +656,13 @@ void Tasks::MoveTask(void *arg) {
// Synchronization barrier (waiting that all tasks are starting) // Synchronization barrier (waiting that all tasks are starting)
rt_sem_p(&sem_barrier, TM_INFINITE); rt_sem_p(&sem_barrier, TM_INFINITE);
/*********************************************************/ /**************************************************************************************/
/* The task starts here */
/**************************************************************************************/
rt_task_set_periodic(NULL, TM_NOW, 100000000);
while (1) {
rt_task_wait_period(NULL);
//cout << "Periodic movement update"; //cout << "Periodic movement update";
rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE); rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
rs = robotStarted; rs = robotStarted;
@ -621,6 +680,7 @@ void Tasks::MoveTask(void *arg) {
rt_mutex_release(&mutex_robotStarted); rt_mutex_release(&mutex_robotStarted);
//cout << endl << flush; //cout << endl << flush;
} }
}
/** /**
* @brief task in charge of the battery status. * @brief task in charge of the battery status.
@ -642,6 +702,7 @@ void Tasks::BatteryTask(void *arg) {
rt_task_wait_period(NULL); rt_task_wait_period(NULL);
rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE); rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
rs = robotStarted; rs = robotStarted;
rt_mutex_release(&mutex_robotStarted);
if (rs == 1) { if (rs == 1) {
rt_mutex_acquire(&mutex_robot, TM_INFINITE); rt_mutex_acquire(&mutex_robot, TM_INFINITE);
Message* batteryReply = parseMessage(robot.Write(robot.GetBattery())); // TODO : convert Message MessageID Message* batteryReply = parseMessage(robot.Write(robot.GetBattery())); // TODO : convert Message MessageID
@ -650,7 +711,7 @@ void Tasks::BatteryTask(void *arg) {
WriteInQueue(&q_messageToMon, batteryReply); WriteInQueue(&q_messageToMon, batteryReply);
} }
rt_mutex_release(&mutex_robotStarted);
} }
} }

15
software/raspberry/superviseur-robot/tasks.h
View file

@ -83,8 +83,11 @@ private:
RT_TASK th_startCamera; RT_TASK th_startCamera;
RT_TASK th_move; RT_TASK th_move;
RT_TASK th_battery; RT_TASK th_battery;
RT_TASK th_lossCounter; /*RT_TASK th_lossCounter;*/
RT_TASK th_camera; RT_TASK th_camera;
RT_TASK th_startRobotWithWatchdog;
RT_TASK th_robotReloadMessage;
/**********************************************************************/ /**********************************************************************/
/* Mutex */ /* Mutex */
/**********************************************************************/ /**********************************************************************/
@ -108,6 +111,7 @@ private:
RT_SEM sem_startRobotWithWatchdog; RT_SEM sem_startRobotWithWatchdog;
RT_SEM sem_startCamera; RT_SEM sem_startCamera;
RT_SEM sem_connexionEstablished; RT_SEM sem_connexionEstablished;
RT_SEM sem_reloadMessages;
/**********************************************************************/ /**********************************************************************/
/* Message queues */ /* Message queues */
@ -166,9 +170,8 @@ private:
/** /**
* @brief task in charge of counting packet losses. * @brief task in charge of counting packet losses.
*/
void RobotLossCounter(void *arg); void RobotLossCounter(void *arg);
*/
/** /**
* @brief task in charge of managing the camera. * @brief task in charge of managing the camera.
@ -197,8 +200,12 @@ private:
* @param Message * @param Message
* @return Message * @return Message
*/ */
MessageID parseMessage(MessageID msg); Message* parseMessage(Message* msg);
/**
* Reloads a message
*/
void RobotReloadMessage(void *arg);
}; };
#endif // __TASKS_H__ #endif // __TASKS_H__