maybe ca marche maybe pas
This commit is contained in:
Lacroix Raphael
parent
cac70c70df
commit
95f1cacc5e
5 changed files with 130 additions and 23 deletions
|
|
@ -41,7 +41,7 @@
|
|||
</conf>
|
||||
<conf name="Debug__RPI_" type="1">
|
||||
<toolsSet>
|
||||
<developmentServer>xenomai@10.105.1.3:22</developmentServer>
|
||||
<developmentServer>xenomai@10.105.1.8:22</developmentServer>
|
||||
<platform>2</platform>
|
||||
</toolsSet>
|
||||
<dbx_gdbdebugger version="1">
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -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);
|
||||
}
|
||||
|
|
@ -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);
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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__
|
||||
|
|
|
|||
Loading…
Reference in a new issue