diff --git a/software/raspberry/superviseur-robot/nbproject/private/configurations.xml b/software/raspberry/superviseur-robot/nbproject/private/configurations.xml index a20af05..ffe1427 100644 --- a/software/raspberry/superviseur-robot/nbproject/private/configurations.xml +++ b/software/raspberry/superviseur-robot/nbproject/private/configurations.xml @@ -41,7 +41,7 @@ - xenomai@10.105.1.3:22 + xenomai@10.105.1.8:22 2 diff --git a/software/raspberry/superviseur-robot/nbproject/private/downloads-10.105.1.8-xenomai-22 b/software/raspberry/superviseur-robot/nbproject/private/downloads-10.105.1.8-xenomai-22 new file mode 100644 index 0000000..e69de29 diff --git a/software/raspberry/superviseur-robot/nbproject/private/timestamps-10.105.1.8-xenomai-22 b/software/raspberry/superviseur-robot/nbproject/private/timestamps-10.105.1.8-xenomai-22 new file mode 100644 index 0000000..c1f3153 --- /dev/null +++ b/software/raspberry/superviseur-robot/nbproject/private/timestamps-10.105.1.8-xenomai-22 @@ -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 diff --git a/software/raspberry/superviseur-robot/tasks.cpp b/software/raspberry/superviseur-robot/tasks.cpp index 5531358..8128cf4 100644 --- a/software/raspberry/superviseur-robot/tasks.cpp +++ b/software/raspberry/superviseur-robot/tasks.cpp @@ -27,8 +27,8 @@ #define PRIORITY_TSTARTROBOT 20 #define PRIORITY_TCAMERA 21 #define PRIORITY_TBATTERY 25 -#define PRIORITY_TLOSSCOUNTER 25 #define PRIORITY_TSTARTCAMERA 25 +#define PRIORITY_TROBOTRELOADMESSAGE 25 /* * Some remarks: @@ -52,7 +52,6 @@ /** * @brief Initialisation des structures de l'application (tâches, mutex, * semaphore, etc.) - */ void Tasks::RobotLossCounter(void* arg){ int state; @@ -75,6 +74,10 @@ void Tasks::RobotLossCounter(void* arg){ 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) { @@ -87,10 +90,11 @@ void Tasks::RobotLossCounter(void* arg){ } } + */ -MessageID Tasks::parseMessage(MessageID msg){ - if (msg == MESSAGE_ANSWER_COM_ERROR){ - +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 ++; @@ -102,6 +106,31 @@ MessageID Tasks::parseMessage(MessageID msg){ 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* + + // 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; } @@ -183,6 +212,10 @@ void Tasks::Init() { cerr << "Error semaphore create: " << strerror(-err) << endl << flush; 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; /**************************************************************************************/ @@ -220,14 +253,23 @@ void Tasks::Init() { cerr << "Error task create: " << strerror(-err) << endl << flush; 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; exit(EXIT_FAILURE); - } + }*/ if (err = rt_task_create(&th_camera, "th_camera", 0, PRIORITY_TCAMERA, 0)) { cerr << "Error task create: " << strerror(-err) << endl << flush; 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; /**************************************************************************************/ @@ -285,7 +327,15 @@ void Tasks::Run() { 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; exit(EXIT_FAILURE); } @@ -489,7 +539,7 @@ void Tasks::StartRobotTask(void *arg) { */ // TODO : refactor with the previous : if (watchdog) {call with; start the watchdog messages tasks} else {call without} - void Tasks::StartRobotWithWatchDogTask(void *arg) { +void Tasks::StartRobotWithWatchDogTask(void *arg) { cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; // Synchronization barrier (waiting that all tasks are starting) rt_sem_p(&sem_barrier, TM_INFINITE); @@ -500,8 +550,8 @@ void Tasks::StartRobotTask(void *arg) { while (1) { Message * msgSend; - rt_sem_p(&sem_startRobot, TM_INFINITE); - cout << "Start robot without watchdog ("; + rt_sem_p(&sem_startRobotWithWatchdog, TM_INFINITE); + cout << "Start robot with watchdog ("; rt_mutex_acquire(&mutex_robot, TM_INFINITE); msgSend = parseMessage(robot.Write(robot.StartWithWD())); // TODO : convert Message MessageID rt_mutex_release(&mutex_robot); @@ -513,11 +563,12 @@ void Tasks::StartRobotTask(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); } - 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; // Synchronization barrier (waiting that all tasks are starting) 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; - rt_task_set_periodic(NULL, TM_NOW, 500*1000*1000); + rt_task_set_periodic(NULL, TM_NOW, 1000*1000*1000); while(1) { 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 - if (state < 0) { + if (status < 0) { WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_NACK)); } else { 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) 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"; rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE); rs = robotStarted; @@ -621,6 +680,7 @@ void Tasks::MoveTask(void *arg) { rt_mutex_release(&mutex_robotStarted); //cout << endl << flush; } +} /** * @brief task in charge of the battery status. @@ -642,6 +702,7 @@ void Tasks::BatteryTask(void *arg) { rt_task_wait_period(NULL); rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE); rs = robotStarted; + rt_mutex_release(&mutex_robotStarted); if (rs == 1) { rt_mutex_acquire(&mutex_robot, TM_INFINITE); Message* batteryReply = parseMessage(robot.Write(robot.GetBattery())); // TODO : convert Message MessageID @@ -650,7 +711,7 @@ void Tasks::BatteryTask(void *arg) { WriteInQueue(&q_messageToMon, batteryReply); } - rt_mutex_release(&mutex_robotStarted); + } } diff --git a/software/raspberry/superviseur-robot/tasks.h b/software/raspberry/superviseur-robot/tasks.h index 0533cb5..d8b2812 100644 --- a/software/raspberry/superviseur-robot/tasks.h +++ b/software/raspberry/superviseur-robot/tasks.h @@ -83,8 +83,11 @@ private: RT_TASK th_startCamera; RT_TASK th_move; RT_TASK th_battery; - RT_TASK th_lossCounter; + /*RT_TASK th_lossCounter;*/ RT_TASK th_camera; + RT_TASK th_startRobotWithWatchdog; + RT_TASK th_robotReloadMessage; + /**********************************************************************/ /* Mutex */ /**********************************************************************/ @@ -108,6 +111,7 @@ private: RT_SEM sem_startRobotWithWatchdog; RT_SEM sem_startCamera; RT_SEM sem_connexionEstablished; + RT_SEM sem_reloadMessages; /**********************************************************************/ /* Message queues */ @@ -166,9 +170,8 @@ private: /** * @brief task in charge of counting packet losses. - */ void RobotLossCounter(void *arg); - + */ /** * @brief task in charge of managing the camera. @@ -197,8 +200,12 @@ private: * @param Message * @return Message */ - MessageID parseMessage(MessageID msg); + Message* parseMessage(Message* msg); + /** + * Reloads a message + */ + void RobotReloadMessage(void *arg); }; #endif // __TASKS_H__