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

added 3 loss timeout

Browse source
This commit is contained in:
Raphaël LACROIX 2023-04-03 13:10:30 +02:00
parent 7258c52428
commit 0a74b96c5b
2 changed files with 119 additions and 38 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

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

@ -27,6 +27,7 @@
#define PRIORITY_TSTARTROBOT 20
#define PRIORITY_TCAMERA 21
#define PRIORITY_TBATTERY 25
#define PRIORITY_TLOSSCOUNTER 25
#define PRIORITY_TSTARTCAMERA 25
/*
@ -52,6 +53,58 @@
* @brief Initialisation des structures de l'application (tâches, mutex,
* semaphore, etc.)
*/
void Tasks::robotLossCounter(){
// wait for connexion establishement
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_connexionEstablished);
cout << "Starting to monitor packet losses " << endl << flush;
rt_task_set_periodic(NULL, TM_NOW, 100*1000*1000);
while(1){
rt_task_wait_period(NULL);
rt_mutex_acquire(&mutex_compteurRobotLoss, TM_INFINITE);
if(compteurRobotLoss >= 3){
// send info to monitor
WriteInQueue(&q_messageToMon, new Message(MESSAGE_MONITOR_LOST));
// close robot
rt_mutex_acquire(&mutex_robot, TM_INFINITE);
state = robot.Close();
rt_mutex_release(&mutex_robot);
// 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);
}
}
MessageID traitementMessage(MessageID msg){
if (msg == MESSAGE_ANSWER_COM_ERROR){
// increment
rt_mutex_acquire(&mutex_compteurRobotLoss, TM_INFINITE);
compteurRobotLoss ++;
rt_mutex_release(&mutex_compteurRobotLoss);
} else {
// reset
rt_mutex_acquire(&mutex_compteurRobotLoss, TM_INFINITE);
compteurRobotLoss = 0;
rt_mutex_release(&mutex_compteurRobotLoss);
}
return msg
}
void Tasks::Init() {
int status;
int err;
@ -75,6 +128,11 @@ void Tasks::Init() {
cerr << "Error mutex create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_mutex_create(&mutex_compteurRobotLoss, NULL)) {
cerr << "Error mutex create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_mutex_create(&mutex_cameraStarted, NULL)) {
cerr << "Error mutex create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
@ -104,6 +162,10 @@ void Tasks::Init() {
cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_sem_create(&sem_connexionEstablished, NULL, 0, S_FIFO)) {
cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_sem_create(&sem_serverOk, NULL, 0, S_FIFO)) {
cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
@ -153,6 +215,10 @@ 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)) {
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);
@ -213,7 +279,12 @@ void Tasks::Run() {
cerr << "Error task start: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_task_start(&th_lossCounter, (void(*)(void*)) & Tasks::RobotLossCounter, this)) {
cerr << "Error task start: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
cout << "Tasks launched" << endl << flush;
}
@ -323,17 +394,17 @@ void Tasks::ReceiveFromMonTask(void *arg) {
} else if (msgRcv->CompareID(MESSAGE_CAM_OPEN)){
rt_sem_v(&sem_startCamera);
}else if(msgRcv->CompareID(MESSAGE_CAM_ASK_ARENA)){
rt_mutex_acquire(&mutex_search_arena, TM_INFINITE);
searchArena = 1;
rt_mutex_release(&mutex_search_arena);
rt_mutex_acquire(&mutex_search_arena, TM_INFINITE);
searchArena = 1;
rt_mutex_release(&mutex_search_arena);
}else if(msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_START)){
rt_mutex_acquire(&mutex_robot_pos, TM_INFINITE);
robotPos = 1;
rt_mutex_release(&mutex_robot_pos);
rt_mutex_acquire(&mutex_robot_pos, TM_INFINITE);
robotPos = 1;
rt_mutex_release(&mutex_robot_pos);
}else if(msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_STOP)){
rt_mutex_acquire(&mutex_robot_pos, TM_INFINITE);
robotPos = 0;
rt_mutex_release(&mutex_robot_pos);
rt_mutex_acquire(&mutex_robot_pos, TM_INFINITE);
robotPos = 0;
rt_mutex_release(&mutex_robot_pos);
}
delete(msgRcv); // must be deleted manually, no consumer
}
@ -366,6 +437,7 @@ void Tasks::OpenComRobot(void *arg) {
if (status < 0) {
msgSend = new Message(MESSAGE_ANSWER_NACK);
} else {
rt_sem_v(&sem_connexionEstablished);
msgSend = new Message(MESSAGE_ANSWER_ACK);
}
WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon
@ -389,7 +461,7 @@ void Tasks::StartRobotTask(void *arg) {
rt_sem_p(&sem_startRobot, TM_INFINITE);
cout << "Start robot without watchdog (";
rt_mutex_acquire(&mutex_robot, TM_INFINITE);
msgSend = robot.Write(robot.StartWithoutWD());
msgSend = call(robot.Write(robot.StartWithoutWD()));
rt_mutex_release(&mutex_robot);
cout << msgSend->GetID();
cout << ")" << endl;
@ -412,7 +484,7 @@ void Tasks::StartCameraTask(void *arg) {
cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
// Synchronization barrier (waiting that all tasks are starting)
rt_sem_p(&sem_barrier, TM_INFINITE);
/**************************************************************************************/
/* The task startRobot starts here */
/**************************************************************************************/
@ -423,12 +495,12 @@ void Tasks::StartCameraTask(void *arg) {
rt_sem_p(&sem_startCamera, TM_INFINITE);
cout << "Start Camera ";
rt_mutex_acquire(&mutex_camera, TM_INFINITE);
bool status = camera.Open();
bool status = camera.Open();
if(!status){
msgSend = new Message(MESSAGE_ANSWER_NACK);
WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon
cout << "NACK !" << endl << flush ;
cout << msgSend->ToString() << endl << flush;
cout << msgSend->ToString() << endl << flush;
cout << "NACK1 ! " << endl << flush ;
rt_mutex_release(&mutex_camera);
cout << "NACK2 ! " << endl << flush ;
@ -443,9 +515,9 @@ void Tasks::StartCameraTask(void *arg) {
rt_mutex_acquire(&mutex_cameraStarted, TM_INFINITE);
cout << "ff" << endl << flush ;
cameraStarted = 1;
cout << "Cam success! " << endl <<flush;
cout << "Cam success! " << endl <<flush;
rt_mutex_release(&mutex_cameraStarted);
cout << "Fin de start cam" << endl << flush;
}
}
@ -518,8 +590,8 @@ void Tasks::BatteryTask(void *arg) {
* @brief task in charge of managing the camera.
*/
void Tasks::CameraTask(void *arg){
int cam;
int cam;
cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
// Synchronization barrier (waiting that all tasks are starting)
rt_sem_p(&sem_barrier, TM_INFINITE);
@ -527,37 +599,37 @@ void Tasks::CameraTask(void *arg){
while(1){
rt_task_wait_period(NULL);
rt_mutex_acquire(&mutex_cameraStarted, TM_INFINITE);
cam = cameraStarted;
cam = cameraStarted;
rt_mutex_release(&mutex_cameraStarted);
if(!cam){
continue;
}
rt_mutex_acquire(&mutex_camera, TM_INFINITE);
// Lancer le traitement périodique
Img img = camera.Grab();
Arena arena;
// Lancer le traitement périodique
Img img = camera.Grab();
Arena arena;
rt_mutex_release(&mutex_camera);
rt_mutex_acquire(&mutex_search_arena, TM_INFINITE);
rt_mutex_acquire(&mutex_search_arena, TM_INFINITE);
if(searchArena){
// chopper l'arene
// chopper l'arene
cout << "Search arena pls ! " << endl << flush;
arena = img.SearchArena();
// searchArena = 0; // toggle?
// searchArena = 0; // toggle?
}
rt_mutex_release(&mutex_search_arena);
rt_mutex_release(&mutex_search_arena);
rt_mutex_acquire(&mutex_robot_pos, TM_INFINITE);
rt_mutex_acquire(&mutex_robot_pos, TM_INFINITE);
if(robotPos){
// chopper le robot
cout << "Pos du robot pls !" ;
}
rt_mutex_release(&mutex_robot_pos);
rt_mutex_release(&mutex_robot_pos);
if(arena.IsEmpty()){
cout << "Arena empty" << endl << flush;
// envoi d'un no ack au moniteur
// envoi d'un no ack au moniteur
}else{
cout << "Draw Arena" << endl << flush;
img.DrawArena(arena);
@ -565,8 +637,8 @@ void Tasks::CameraTask(void *arg){
// envoi de l'arene pour vérif
}
cout << "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAARENE" << endl << flush;
WriteInQueue(&q_messageToMon, new MessageImg(MESSAGE_CAM_IMAGE,&img));
WriteInQueue(&q_messageToMon, new MessageImg(MESSAGE_CAM_IMAGE,&img));
}
}

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

@ -66,9 +66,10 @@ private:
ComRobot robot;
Camera camera;
int robotStarted = 0;
int compteurRobotLoss = 0;
int cameraStarted = 0;
int searchArena = 0;
int robotPos = 0;
int robotPos = 0;
int move = MESSAGE_ROBOT_STOP;
/**********************************************************************/
@ -82,6 +83,7 @@ private:
RT_TASK th_startCamera;
RT_TASK th_move;
RT_TASK th_battery;
RT_TASK th_lossCounter;
RT_TASK th_camera;
/**********************************************************************/
/* Mutex */
@ -91,9 +93,10 @@ private:
RT_MUTEX mutex_robotStarted;
RT_MUTEX mutex_cameraStarted;
RT_MUTEX mutex_move;
RT_MUTEX mutex_compteurRobotLoss;
RT_MUTEX mutex_camera;
RT_MUTEX mutex_search_arena;
RT_MUTEX mutex_robot_pos;
RT_MUTEX mutex_robot_pos;
/**********************************************************************/
/* Semaphores */
@ -103,6 +106,7 @@ private:
RT_SEM sem_serverOk;
RT_SEM sem_startRobot;
RT_SEM sem_startCamera;
RT_SEM sem_connexionEstablished;
/**********************************************************************/
/* Message queues */
@ -153,13 +157,18 @@ private:
* @brief task in charge of the battery status.
*/
void BatteryTask(void *arg);
/**
* @brief task in charge of counting packet losses.
*/
void RobotLossCounter(void *arg);
/**
* @brief task in charge of managing the camera.
*/
void CameraTask(void *arg);
/**********************************************************************/
/* Queue services */
/**********************************************************************/