diff --git a/software/raspberry/superviseur-robot/tasks.cpp b/software/raspberry/superviseur-robot/tasks.cpp index 347d7e0..9811532 100644 --- a/software/raspberry/superviseur-robot/tasks.cpp +++ b/software/raspberry/superviseur-robot/tasks.cpp @@ -54,6 +54,7 @@ * semaphore, etc.) */ void Tasks::robotLossCounter(){ + int status; // wait for connexion establishement cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; @@ -152,7 +153,7 @@ void Tasks::Init() { cout << "Mutexes created successfully" << endl << flush; /**************************************************************************************/ - /* Semaphors creation */ + /* Semaphores creation */ /**************************************************************************************/ if (err = rt_sem_create(&sem_barrier, NULL, 0, S_FIFO)) { cerr << "Error semaphore create: " << strerror(-err) << endl << flush; @@ -174,6 +175,10 @@ void Tasks::Init() { cerr << "Error semaphore create: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } + if (err = rt_sem_create(&sem_startRobotWithWatchdog, NULL, 0, S_FIFO)) { + cerr << "Error semaphore create: " << strerror(-err) << endl << flush; + exit(EXIT_FAILURE); + } if (err = rt_sem_create(&sem_startCamera, NULL, 0, S_FIFO)) { cerr << "Error semaphore create: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); @@ -382,6 +387,8 @@ void Tasks::ReceiveFromMonTask(void *arg) { 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_START_WITH_WD)) { + rt_sem_v(&sem_startRobotWithWatchdog); } else if (msgRcv->CompareID(MESSAGE_ROBOT_GO_FORWARD) || msgRcv->CompareID(MESSAGE_ROBOT_GO_BACKWARD) || msgRcv->CompareID(MESSAGE_ROBOT_GO_LEFT) || @@ -451,7 +458,7 @@ void Tasks::StartRobotTask(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 */ /**************************************************************************************/ @@ -477,6 +484,69 @@ void Tasks::StartRobotTask(void *arg) { } } +/** + * @brief Thread starting the communication with the robot. + */ + +// TODO : refactor with the previous : if (watchdog) {call with; start the watchdog messages tasks} else {call without} + 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); + + /**************************************************************************************/ + /* The task startRobot starts here */ + /**************************************************************************************/ + while (1) { + + Message * msgSend; + rt_sem_p(&sem_startRobot, TM_INFINITE); + cout << "Start robot without watchdog ("; + rt_mutex_acquire(&mutex_robot, TM_INFINITE); + msgSend = call(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_sem_v(&sem_reloadMessages, TM_INFINITE); + } +} + +/** + * @brief Thread sending a message every 500 ms to the robot to prevent it from turning off. + */ + +void Tasks::RobotReloadMessage(void *arg) { + int status; + + 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); + cout << "Starting to monitor packet losses " << endl << flush; + rt_task_set_periodic(NULL, TM_NOW, 500*1000*1000); + while(1) { + rt_task_wait_period(NULL); + status = (robot.Write(robot.reloadWD())); + // send info to monitor + if (state < 0) { + WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_NACK)); + } else { + WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_ACK)); + } + } + +} + /** * @brief Thread starting the communication with the robot. */ diff --git a/software/raspberry/superviseur-robot/tasks.h b/software/raspberry/superviseur-robot/tasks.h index bd38296..1dbd069 100644 --- a/software/raspberry/superviseur-robot/tasks.h +++ b/software/raspberry/superviseur-robot/tasks.h @@ -105,6 +105,7 @@ private: RT_SEM sem_openComRobot; RT_SEM sem_serverOk; RT_SEM sem_startRobot; + RT_SEM sem_startRobotWithWatchdog; RT_SEM sem_startCamera; RT_SEM sem_connexionEstablished; @@ -141,9 +142,14 @@ private: * @brief Thread starting the communication with the robot. */ void StartRobotTask(void *arg); + + /** + * @brief Thread starting the communication with the robot with a watchdog. + */ + void StartRobotWithWatchDogTask(void *arg); /** - * @brief Thread starting the communication with the robot. + * @brief Thread starting the communication with the Camera. */ void StartCameraTask(void *arg);