From a100a35c08e7bd6715c853f99fe072c94014279a Mon Sep 17 00:00:00 2001 From: moukhlis Date: Mon, 15 Mar 2021 12:14:03 +0100 Subject: [PATCH] th_rcvFromMon Watchdog et RobotStart faits et a tester --- .../raspberry/superviseur-robot/tasks.cpp | 136 +++++++++++++----- software/raspberry/superviseur-robot/tasks.h | 10 +- 2 files changed, 110 insertions(+), 36 deletions(-) diff --git a/software/raspberry/superviseur-robot/tasks.cpp b/software/raspberry/superviseur-robot/tasks.cpp index 126e0b9..e1bd27b 100644 --- a/software/raspberry/superviseur-robot/tasks.cpp +++ b/software/raspberry/superviseur-robot/tasks.cpp @@ -276,31 +276,53 @@ void Tasks::ReceiveFromMonTask(void *arg) { /**************************************************************************************/ /* The task receiveFromMon starts here */ /**************************************************************************************/ + while(1){ rt_sem_p(&sem_serverOk, TM_INFINITE); cout << "Received message from monitor activated" << endl << flush; - while (1) { - msgRcv = monitor.Read(); - cout << "Rcv <= " << msgRcv->ToString() << endl << flush; + while (1) { + msgRcv = monitor.Read(); + cout << "Rcv <= " << msgRcv->ToString() << endl << flush; - if (msgRcv->CompareID(MESSAGE_MONITOR_LOST)) { - delete(msgRcv); - exit(-1); - } else if (msgRcv->CompareID(MESSAGE_ROBOT_COM_OPEN)) { - rt_sem_v(&sem_openComRobot); - } else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITHOUT_WD)) { - rt_sem_v(&sem_startRobot); - } else if (msgRcv->CompareID(MESSAGE_ROBOT_GO_FORWARD) || - msgRcv->CompareID(MESSAGE_ROBOT_GO_BACKWARD) || - msgRcv->CompareID(MESSAGE_ROBOT_GO_LEFT) || - msgRcv->CompareID(MESSAGE_ROBOT_GO_RIGHT) || - msgRcv->CompareID(MESSAGE_ROBOT_STOP)) { + if (msgRcv->CompareID(MESSAGE_MONITOR_LOST)) { + delete(msgRcv); + WriteInQueue(&q_messageComRobot, new Message(MESSAGE_ROBOT_COM_CLOSE)); + WriteInQueue(&q_messageControlRobot, new Message(MESSAGE_ROBOT_RESET)); + WriteInQueue(&q_messageControlCam, new Message(MESSAGE_CAM_CLOSE)); + rt_sem_v(&sem_restartServer, TM_INFINITE); // A VERIFIER + break; - rt_mutex_acquire(&mutex_move, TM_INFINITE); - move = msgRcv->GetID(); - rt_mutex_release(&mutex_move); + } else if (msgRcv->CompareID(MESSAGE_ROBOT_COM_OPEN) || msgRcv->CompareID(MESSAGE_ROBOT_COM_CLOSE)) { + WriteInQueue(&q_messageComRobot, msgRcv); + //rt_sem_v(&sem_openComRobot); + } else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITHOUT_WD) || msgRcv->CompareID(MESSAGE_ROBOT_START_WITH_WD)) { + WriteInQueue(&q_messageControlRobot, msgRcv); + //rt_sem_v(&sem_startRobot); + } + else if (msgRcv->CompareID(MESSAGE_ROBOT_GO_FORWARD) || + msgRcv->CompareID(MESSAGE_ROBOT_GO_BACKWARD) || + msgRcv->CompareID(MESSAGE_ROBOT_GO_LEFT) || + msgRcv->CompareID(MESSAGE_ROBOT_GO_RIGHT) || + msgRcv->CompareID(MESSAGE_ROBOT_STOP)) { + rt_mutex_acquire(&mutex_move, TM_INFINITE); + move = msgRcv->GetID(); + rt_mutex_release(&mutex_move); + } + + else if (msgRcv->CompareID(MESSAGE_CAM_OPEN) || + msgRcv->CompareID(MESSAGE_CAM_CLOSE) || + msgRcv->CompareID(MESSAGE_CAM_ASK_ARENA) || + msgRcv->CompareID(MESSAGE_CAM_ARENA_CONFIRM) || + msgRcv->CompareID(MESSAGE_CAM_ARENA_INFIRM) || + msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_START) || + msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_STOP) || + ) + { + WriteInQueue(&q_messageControlCam, msgRcv); + } + delete(msgRcv); // mus be deleted manually, no consumer } - delete(msgRcv); // mus be deleted manually, no consumer + } } @@ -350,23 +372,70 @@ void Tasks::StartRobotTask(void *arg) { /**************************************************************************************/ while (1) { + Message * tmp; Message * msgSend; - rt_sem_p(&sem_startRobot, TM_INFINITE); - cout << "Start robot without watchdog ("; - rt_mutex_acquire(&mutex_robot, TM_INFINITE); - msgSend = robot.Write(robot.StartWithoutWD()); - rt_mutex_release(&mutex_robot); - cout << msgSend->GetID(); - cout << ")" << endl; - - cout << "Movement answer: " << msgSend->ToString() << endl << flush; - WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon - - if (msgSend->GetID() == MESSAGE_ANSWER_ACK) { + tmp = ReadInQueue(&q_messageControlRobot); + if (tmp -> CompareID(MESSAGE_ROBOT_START_WITHOUT_WD)){ + cout << "Start robot without watchdog ("; + rt_mutex_acquire(&mutex_robot, TM_INFINITE); + msgSend = robot.Write(robot.StartWithoutWD()); + rt_mutex_release(&mutex_robot); + cout << msgSend->GetID(); + cout << ")" << endl; + cout << "Movement answer: " << msgSend->ToString() << endl << flush; + WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon + if (msgSend->GetID() == MESSAGE_ANSWER_ACK) { + rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE); + robotStarted = 1; + rt_mutex_release(&mutex_robotStarted); + } + } else if (tmp -> CompareID(MESSAGE_ROBOT_START_WITH_WD)){ + cout << "Start robot with watchdog ("; + rt_mutex_acquire(&mutex_robot, TM_INFINITE); + msgSend = robot.Write(robot.StartWithWD()); + rt_mutex_release(&mutex_robot); + cout << msgSend->GetID(); + cout << ")" << endl; + cout << "Movement answer: " << msgSend->ToString() << endl << flush; + WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon + if (msgSend->GetID() == MESSAGE_ANSWER_ACK) { + rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE); + robotStarted = 1; + rt_mutex_release(&mutex_robotStarted); + rt_task_set_periodic(&th_watchDog,TM_NOW,1000000000); + } + + } else if (tmp -> CompareID(MESSAGE_ROBOT_RESET)){ + rt_task_set_periodic(&th_watchDog,TM_NOW,0); + cout << "Stopping Robot ("; + rt_mutex_acquire(&mutex_robot, TM_INFINITE); + msgSend = robot.Write(new Message(MESSAGE_ROBOT_RESET)); + rt_mutex_release(&mutex_robot); + cout << msgSend->GetID(); + cout << ")" << endl; + cout << "Movement answer: " << msgSend->ToString() << endl << flush; + WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE); - robotStarted = 1; + robotStarted = 0; rt_mutex_release(&mutex_robotStarted); - } + } + + + } +} + + +void Tasks::WatchDog(void *arg) { + cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; + // Synchronization barrier (waiting that all tasks are starting) + rt_sem_p(&sem_barrier, TM_INFINITE); + rt_task_set_periodic(NULL, TM_NOW, 0); + while (1) { + rt_task_wait_period(NULL); + Message * msgSend; + rt_mutex_acquire(&mutex_robot, TM_INFINITE); + msgSend = robot.ReloadWD(); + rt_mutex_release(&mutex_robot); } } @@ -403,7 +472,6 @@ void Tasks::MoveTask(void *arg) { rt_mutex_acquire(&mutex_robot, TM_INFINITE); robot.Write(new Message((MessageID)cpMove)); rt_mutex_release(&mutex_robot); - previousMove = cpMove; } } diff --git a/software/raspberry/superviseur-robot/tasks.h b/software/raspberry/superviseur-robot/tasks.h index d042d1c..4bb8ed8 100644 --- a/software/raspberry/superviseur-robot/tasks.h +++ b/software/raspberry/superviseur-robot/tasks.h @@ -99,6 +99,8 @@ private: RT_SEM sem_openComRobot; RT_SEM sem_serverOk; RT_SEM sem_startRobot; + RT_SEM sem_watchdogStart; + RT_SEM sem_restartServer; @@ -107,7 +109,6 @@ private: /**********************************************************************/ int MSG_QUEUE_SIZE; RT_QUEUE q_messageToMon; - RT_QUEUE q_messageComRobot; RT_QUEUE q_messageControlRobot; RT_QUEUE q_messageControlCam; @@ -168,7 +169,12 @@ private: * @return Nothing */ void ReadBattery(void *arg); - + + /** + * Sends periodically a message to the robot + * @return Nothing + */ + void Watchdog(void *arg); };