diff --git a/software/raspberry/superviseur-robot/tasks.cpp b/software/raspberry/superviseur-robot/tasks.cpp index 1244172..264d89f 100644 --- a/software/raspberry/superviseur-robot/tasks.cpp +++ b/software/raspberry/superviseur-robot/tasks.cpp @@ -52,63 +52,63 @@ * semaphore, etc.) */ void Tasks::Init() { - int status; - int err; + int status; + int err; - /* ************************************************************************************** - * Mutex creation - * *************************************************************************************/ - if ((err = rt_mutex_create(&mutex_monitor, nullptr))) { - cerr << "Error mutex create mutex_monitor: " << strerror(-err) << endl; - exit(EXIT_FAILURE); - } - if ((err = rt_mutex_create(&mutex_robot, nullptr))) { - cerr << "Error mutex create mutex_robot: " << strerror(-err) << endl; - exit(EXIT_FAILURE); - } - if ((err = rt_mutex_create(&mutex_robotStarted, nullptr))) { - cerr << "Error mutex create mutex_robotStarted: " << strerror(-err) << endl; - exit(EXIT_FAILURE); - } - if ((err = rt_mutex_create(&mutex_move, nullptr))) { - cerr << "Error mutex create mutex_move: " << strerror(-err) << endl; - exit(EXIT_FAILURE); - } - cout << "Mutexes created successfully" << endl; + /* ************************************************************************************** + * Mutex creation + * *************************************************************************************/ + if ((err = rt_mutex_create(&mutex_monitor, nullptr))) { + cerr << "Error mutex create mutex_monitor: " << strerror(-err) << endl; + exit(EXIT_FAILURE); + } + if ((err = rt_mutex_create(&mutex_robot, nullptr))) { + cerr << "Error mutex create mutex_robot: " << strerror(-err) << endl; + exit(EXIT_FAILURE); + } + if ((err = rt_mutex_create(&mutex_robotStarted, nullptr))) { + cerr << "Error mutex create mutex_robotStarted: " << strerror(-err) << endl; + exit(EXIT_FAILURE); + } + if ((err = rt_mutex_create(&mutex_move, nullptr))) { + cerr << "Error mutex create mutex_move: " << strerror(-err) << endl; + exit(EXIT_FAILURE); + } + cout << "Mutexes created successfully" << endl; - /* ************************************************************************************** - * Semaphores creation - * *************************************************************************************/ - if ((err = rt_sem_create(&sem_barrier, nullptr, 0, S_FIFO))) { - cerr << "Error semaphore create sem_barrier: " << strerror(-err) << endl; - exit(EXIT_FAILURE); - } - if ((err = rt_sem_create(&sem_openComRobot, nullptr, 0, S_FIFO))) { - cerr << "Error semaphore create sem_openComRobot: " << strerror(-err) << endl; - exit(EXIT_FAILURE); - } - if ((err = rt_sem_create(&sem_serverOk, nullptr, 0, S_FIFO))) { - cerr << "Error semaphore create sem_serverOk: " << strerror(-err) << endl; - exit(EXIT_FAILURE); - } - if ((err = rt_sem_create(&sem_startRobot, nullptr, 0, S_FIFO))) { - cerr << "Error semaphore create sem_startRobot: " << strerror(-err) << endl; - exit(EXIT_FAILURE); - } - cout << "Semaphores created successfully" << endl; + /* ************************************************************************************** + * Semaphores creation + * *************************************************************************************/ + if ((err = rt_sem_create(&sem_barrier, nullptr, 0, S_FIFO))) { + cerr << "Error semaphore create sem_barrier: " << strerror(-err) << endl; + exit(EXIT_FAILURE); + } + if ((err = rt_sem_create(&sem_openComRobot, nullptr, 0, S_FIFO))) { + cerr << "Error semaphore create sem_openComRobot: " << strerror(-err) << endl; + exit(EXIT_FAILURE); + } + if ((err = rt_sem_create(&sem_serverOk, nullptr, 0, S_FIFO))) { + cerr << "Error semaphore create sem_serverOk: " << strerror(-err) << endl; + exit(EXIT_FAILURE); + } + if ((err = rt_sem_create(&sem_startRobot, nullptr, 0, S_FIFO))) { + cerr << "Error semaphore create sem_startRobot: " << strerror(-err) << endl; + exit(EXIT_FAILURE); + } + cout << "Semaphores created successfully" << endl; - /* ************************************************************************************* - * Tasks creation - * *************************************************************************************/ - if ((err = rt_task_create(&th_server, "th_server", 0, PRIORITY_TSERVER, 0))) { - cerr << "Error task create th_server: " << strerror(-err) << endl; - exit(EXIT_FAILURE); - } - if ((err = rt_task_create(&th_sendToMon, "th_sendToMon", 0, PRIORITY_TSENDTOMON, 0))) { - cerr << "Error task create th_sendToMon: " << strerror(-err) << endl; - exit(EXIT_FAILURE); - } + /* ************************************************************************************* + * Tasks creation + * *************************************************************************************/ + if ((err = rt_task_create(&th_server, "th_server", 0, PRIORITY_TSERVER, 0))) { + cerr << "Error task create th_server: " << strerror(-err) << endl; + exit(EXIT_FAILURE); + } + if ((err = rt_task_create(&th_sendToMon, "th_sendToMon", 0, PRIORITY_TSENDTOMON, 0))) { + cerr << "Error task create th_sendToMon: " << strerror(-err) << endl; + exit(EXIT_FAILURE); + } if ((err = rt_task_create(&th_receiveFromMon, "th_receiveFromMon", 0, PRIORITY_TRECEIVEFROMMON, 0))) { cerr << "Error task create th_receiveFromMon: " << strerror(-err) << endl; exit(EXIT_FAILURE); @@ -136,7 +136,7 @@ void Tasks::Init() { /* ************************************************************************************* * Message queues creation * *************************************************************************************/ - if ((err = rt_queue_create(&q_messageToMon, "q_messageToMon", sizeof(Message * ) * 50, Q_UNLIMITED, Q_FIFO)) < 0) { + if ((err = rt_queue_create(&q_messageToMon, "q_messageToMon", sizeof(Message *) * 50, Q_UNLIMITED, Q_FIFO)) < 0) { cerr << "Error msg queue create q_messageToMon: " << strerror(-err) << endl; exit(EXIT_FAILURE); } @@ -148,17 +148,17 @@ void Tasks::Init() { * @brief Démarrage des tâches */ void Tasks::Run() { - rt_task_set_priority(nullptr, T_LOPRIO); - int err; + rt_task_set_priority(nullptr, T_LOPRIO); + int err; - if (err = rt_task_start(&th_server, (void(*)(void*)) & Tasks::ServerTask, this)) { - cerr << "Error task start ServerTask: " << strerror(-err) << endl; - exit(EXIT_FAILURE); - } - if (err = rt_task_start(&th_sendToMon, (void(*)(void*)) & Tasks::SendToMonTask, this)) { - cerr << "Error task start SendToMonTask: " << strerror(-err) << endl; - exit(EXIT_FAILURE); - } + if (err = rt_task_start(&th_server, (void (*)(void *)) &Tasks::ServerTask, this)) { + cerr << "Error task start ServerTask: " << strerror(-err) << endl; + exit(EXIT_FAILURE); + } + if (err = rt_task_start(&th_sendToMon, (void (*)(void *)) &Tasks::SendToMonTask, this)) { + cerr << "Error task start SendToMonTask: " << strerror(-err) << endl; + exit(EXIT_FAILURE); + } if (err = rt_task_start(&th_receiveFromMon, (void (*)(void *)) &Tasks::ReceiveFromMonTask, this)) { cerr << "Error task start ReceiveFromMonTask: " << strerror(-err) << endl; exit(EXIT_FAILURE); @@ -187,174 +187,175 @@ void Tasks::Run() { * @brief Arrêt des tâches */ void Tasks::Stop() { - monitor.Close(); - robot.Close(); + monitor.Close(); + robot.Close(); } /** */ void Tasks::Join() { - cout << "Tasks synchronized" << endl; - rt_sem_broadcast(&sem_barrier); - pause(); + cout << "Tasks synchronized" << endl; + rt_sem_broadcast(&sem_barrier); + pause(); } /** * @brief Thread handling server communication with the monitor. */ void Tasks::ServerTask(void *arg) { - int status; - - cout << "Start " << __PRETTY_FUNCTION__ << endl; - // Synchronization barrier (waiting that all tasks are started) - rt_sem_p(&sem_barrier, TM_INFINITE); + int status; - /* ************************************************************************************** - * The task server starts here - * *************************************************************************************/ - rt_mutex_acquire(&mutex_monitor, TM_INFINITE); - status = monitor.Open(SERVER_PORT); - rt_mutex_release(&mutex_monitor); + cout << "Start " << __PRETTY_FUNCTION__ << endl; + // Synchronization barrier (waiting that all tasks are started) + rt_sem_p(&sem_barrier, TM_INFINITE); - cout << "Open server on port " << (SERVER_PORT) << " (" << status << ")" << endl; + /* ************************************************************************************** + * The task server starts here + * *************************************************************************************/ + rt_mutex_acquire(&mutex_monitor, TM_INFINITE); + status = monitor.Open(SERVER_PORT); + rt_mutex_release(&mutex_monitor); - if (status < 0) throw std::runtime_error { - "Unable to start server on port " + std::to_string(SERVER_PORT) - }; - monitor.AcceptClient(); // Wait the monitor client - cout << "Rock'n'Roll baby, client accepted!" << endl; - rt_sem_broadcast(&sem_serverOk); + cout << "Open server on port " << (SERVER_PORT) << " (" << status << ")" << endl; + + if (status < 0) + throw std::runtime_error{ + "Unable to start server on port " + std::to_string(SERVER_PORT) + }; + monitor.AcceptClient(); // Wait the monitor client + cout << "Rock'n'Roll baby, client accepted!" << endl; + rt_sem_broadcast(&sem_serverOk); } /** * @brief Thread sending data to monitor. */ -[[noreturn]] void Tasks::SendToMonTask(void* arg) { - Message *msg; - - cout << "Start " << __PRETTY_FUNCTION__ << endl; - // Synchronization barrier (waiting that all tasks are starting) - rt_sem_p(&sem_barrier, TM_INFINITE); +[[noreturn]] void Tasks::SendToMonTask(void *arg) { + Message *msg; - /* ************************************************************************************* - * The task sendToMon starts here - * *************************************************************************************/ - rt_sem_p(&sem_serverOk, TM_INFINITE); + cout << "Start " << __PRETTY_FUNCTION__ << endl; + // Synchronization barrier (waiting that all tasks are starting) + rt_sem_p(&sem_barrier, TM_INFINITE); - while (true) { - cout << "wait msg to send" << endl; - msg = ReadInQueue(&q_messageToMon); - cout << "Send msg to mon: " << msg->ToString() << endl; - rt_mutex_acquire(&mutex_monitor, TM_INFINITE); - monitor.Write(msg); // The message is deleted with the Write - rt_mutex_release(&mutex_monitor); - } + /* ************************************************************************************* + * The task sendToMon starts here + * *************************************************************************************/ + rt_sem_p(&sem_serverOk, TM_INFINITE); + + while (true) { + cout << "wait msg to send" << endl; + msg = ReadInQueue(&q_messageToMon); + cout << "Send msg to mon: " << msg->ToString() << endl; + rt_mutex_acquire(&mutex_monitor, TM_INFINITE); + monitor.Write(msg); // The message is deleted with the Write + rt_mutex_release(&mutex_monitor); + } } /** * @brief Thread receiving data from monitor. */ void Tasks::ReceiveFromMonTask(void *arg) { - Message *msgRcv; - - cout << "Start " << __PRETTY_FUNCTION__ << endl; - // Synchronization barrier (waiting that all tasks are starting) - rt_sem_p(&sem_barrier, TM_INFINITE); - - /* ************************************************************************************* - * The task receiveFromMon starts here - * *************************************************************************************/ - rt_sem_p(&sem_serverOk, TM_INFINITE); - cout << "Received message from monitor activated" << endl; + Message *msgRcv; - while (true) { - msgRcv = monitor.Read(); - cout << "Rcv <= " << msgRcv->ToString() << endl; + cout << "Start " << __PRETTY_FUNCTION__ << endl; + // Synchronization barrier (waiting that all tasks are starting) + rt_sem_p(&sem_barrier, TM_INFINITE); - 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)) { + /* ************************************************************************************* + * The task receiveFromMon starts here + * *************************************************************************************/ + rt_sem_p(&sem_serverOk, TM_INFINITE); + cout << "Received message from monitor activated" << endl; - rt_mutex_acquire(&mutex_move, TM_INFINITE); - move = msgRcv->GetID(); - rt_mutex_release(&mutex_move); - } - delete(msgRcv); // mus be deleted manually, no consumer - } + while (true) { + msgRcv = monitor.Read(); + cout << "Rcv <= " << msgRcv->ToString() << endl; + + 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)) { + + rt_mutex_acquire(&mutex_move, TM_INFINITE); + move = msgRcv->GetID(); + rt_mutex_release(&mutex_move); + } + delete (msgRcv); // mus be deleted manually, no consumer + } } /** * @brief Thread opening communication with the robot. */ [[noreturn]] void Tasks::OpenComRobot(void *arg) { - int status; - int err; + int status; + int err; - cout << "Start " << __PRETTY_FUNCTION__ << endl; - // Synchronization barrier (waiting that all tasks are starting) - rt_sem_p(&sem_barrier, TM_INFINITE); - - /* ************************************************************************************* - * The task openComRobot starts here - * *************************************************************************************/ - while (true) { - rt_sem_p(&sem_openComRobot, TM_INFINITE); - cout << "Open serial com ("; - rt_mutex_acquire(&mutex_robot, TM_INFINITE); - status = robot.Open(); - rt_mutex_release(&mutex_robot); - cout << status; - cout << ")" << endl << flush; + cout << "Start " << __PRETTY_FUNCTION__ << endl; + // Synchronization barrier (waiting that all tasks are starting) + rt_sem_p(&sem_barrier, TM_INFINITE); - Message * msgSend; - if (status < 0) { - msgSend = new Message(MESSAGE_ANSWER_NACK); - } else { - msgSend = new Message(MESSAGE_ANSWER_ACK); - } - WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon - } + /* ************************************************************************************* + * The task openComRobot starts here + * *************************************************************************************/ + while (true) { + rt_sem_p(&sem_openComRobot, TM_INFINITE); + cout << "Open serial com ("; + rt_mutex_acquire(&mutex_robot, TM_INFINITE); + status = robot.Open(); + rt_mutex_release(&mutex_robot); + cout << status; + cout << ")" << endl << flush; + + Message *msgSend; + if (status < 0) { + msgSend = new Message(MESSAGE_ANSWER_NACK); + } else { + msgSend = new Message(MESSAGE_ANSWER_ACK); + } + WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon + } } /** * @brief Thread starting the communication with the robot. */ [[noreturn]] void Tasks::StartRobotTask(void *arg) { - cout << "Start " << __PRETTY_FUNCTION__ << endl; - // Synchronization barrier (waiting that all tasks are starting) - rt_sem_p(&sem_barrier, TM_INFINITE); - - /* ************************************************************************************* - * The task startRobot starts here - * *************************************************************************************/ - while (true) { - Message *msgSend; - rt_sem_p(&sem_startRobot, TM_INFINITE); - cout << "Start robot without watchdog ("; - rt_mutex_acquire(&mutex_robot, TM_INFINITE); - msgSend = robot.Write(ComRobot::StartWithoutWD()); - rt_mutex_release(&mutex_robot); - cout << msgSend->GetID(); - cout << ")" << endl; + cout << "Start " << __PRETTY_FUNCTION__ << endl; + // Synchronization barrier (waiting that all tasks are starting) + rt_sem_p(&sem_barrier, TM_INFINITE); - cout << "Movement answer: " << msgSend->ToString() << endl; - WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon + /* ************************************************************************************* + * The task startRobot starts here + * *************************************************************************************/ + while (true) { + Message *msgSend; + rt_sem_p(&sem_startRobot, TM_INFINITE); + cout << "Start robot without watchdog ("; + rt_mutex_acquire(&mutex_robot, TM_INFINITE); + msgSend = robot.Write(ComRobot::StartWithoutWD()); + rt_mutex_release(&mutex_robot); + cout << msgSend->GetID(); + cout << ")" << endl; - if (msgSend->GetID() == MESSAGE_ANSWER_ACK) { - rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE); - robotStarted = 1; - rt_mutex_release(&mutex_robotStarted); - } - } + cout << "Movement answer: " << msgSend->ToString() << endl; + 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); + } + } } @@ -368,10 +369,10 @@ void Tasks::ReceiveFromMonTask(void *arg) { /* The task sendBatteryLevel starts here */ /* *************************************************************************************/ - rt_task_set_periodic(NULL, TM_NOW, 500000000); // 500 ms //TODO + rt_task_set_periodic(nullptr, TM_NOW, 500000000); // 500 ms //TODO while (true) { - rt_task_wait_period(NULL); + rt_task_wait_period(nullptr); cout << "Periodic battery level update" << endl; rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE); @@ -411,22 +412,22 @@ void Tasks::ReceiveFromMonTask(void *arg) { while (true) { rt_task_wait_period(nullptr); cout << "Periodic movement update"; - rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE); - rs = robotStarted; - rt_mutex_release(&mutex_robotStarted); - if (rs == 1) { - rt_mutex_acquire(&mutex_move, TM_INFINITE); - cpMove = move; - rt_mutex_release(&mutex_move); - - cout << " move: " << cpMove; - - rt_mutex_acquire(&mutex_robot, TM_INFINITE); - robot.Write(new Message((MessageID)cpMove)); - rt_mutex_release(&mutex_robot); - } - cout << endl << flush; - } + rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE); + rs = robotStarted; + rt_mutex_release(&mutex_robotStarted); + if (rs == 1) { + rt_mutex_acquire(&mutex_move, TM_INFINITE); + cpMove = move; + rt_mutex_release(&mutex_move); + + cout << " move: " << cpMove; + + rt_mutex_acquire(&mutex_robot, TM_INFINITE); + robot.Write(new Message((MessageID) cpMove)); + rt_mutex_release(&mutex_robot); + } + cout << endl << flush; + } } /** @@ -435,11 +436,11 @@ void Tasks::ReceiveFromMonTask(void *arg) { * @param msg Message to be stored */ void Tasks::WriteInQueue(RT_QUEUE *queue, Message *msg) { - int err; - if ((err = rt_queue_write(queue, (const void *) &msg, sizeof ((const void *) &msg), Q_NORMAL)) < 0) { - cerr << "Write in queue failed: " << strerror(-err) << endl; - throw std::runtime_error{"Error in write in queue"}; - } + int err; + if ((err = rt_queue_write(queue, (const void *) &msg, sizeof((const void *) &msg), Q_NORMAL)) < 0) { + cerr << "Write in queue failed: " << strerror(-err) << endl; + throw std::runtime_error{"Error in write in queue"}; + } } /** @@ -448,16 +449,16 @@ void Tasks::WriteInQueue(RT_QUEUE *queue, Message *msg) { * @return Message read */ Message *Tasks::ReadInQueue(RT_QUEUE *queue) { - int err; - Message *msg; + int err; + Message *msg; - if ((err = rt_queue_read(queue, &msg, sizeof ((void*) &msg), TM_INFINITE)) < 0) { - cout << "Read in queue failed: " << strerror(-err) << endl; - throw std::runtime_error{"Error in read in queue"}; - }/** else { + if ((err = rt_queue_read(queue, &msg, sizeof((void *) &msg), TM_INFINITE)) < 0) { + cout << "Read in queue failed: " << strerror(-err) << endl; + throw std::runtime_error{"Error in read in queue"}; + }/** else { cout << "@msg :" << msg << endl << flush; } **/ - return msg; + return msg; }