From 7f993019bc938850be081e1342e580e015683076 Mon Sep 17 00:00:00 2001 From: Yohan Simard Date: Fri, 12 Mar 2021 13:25:52 +0100 Subject: [PATCH] Add sendBatteryLevel task --- .../raspberry/superviseur-robot/tasks.cpp | 156 ++++++++++-------- software/raspberry/superviseur-robot/tasks.h | 45 ++--- 2 files changed, 109 insertions(+), 92 deletions(-) diff --git a/software/raspberry/superviseur-robot/tasks.cpp b/software/raspberry/superviseur-robot/tasks.cpp index 70ea1ca..72278f5 100644 --- a/software/raspberry/superviseur-robot/tasks.cpp +++ b/software/raspberry/superviseur-robot/tasks.cpp @@ -106,34 +106,40 @@ void Tasks::Init() { } 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); + 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); - } - if ((err = rt_task_create(&th_openComRobot, "th_openComRobot", 0, PRIORITY_TOPENCOMROBOT, 0))) { - cerr << "Error task create th_openComRobot: " << strerror(-err) << endl; - exit(EXIT_FAILURE); - } - if ((err = rt_task_create(&th_startRobot, "th_startRobot", 0, PRIORITY_TSTARTROBOT, 0))) { - cerr << "Error task create th_startRobot: " << strerror(-err) << endl; - exit(EXIT_FAILURE); - } - if ((err = rt_task_create(&th_move, "th_move", 0, PRIORITY_TMOVE, 0))) { - cerr << "Error task create th_move: " << strerror(-err) << endl; - exit(EXIT_FAILURE); - } - cout << "Tasks created successfully" << endl; + 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); + } + if ((err = rt_task_create(&th_openComRobot, "th_openComRobot", 0, PRIORITY_TOPENCOMROBOT, 0))) { + cerr << "Error task create th_openComRobot: " << strerror(-err) << endl; + exit(EXIT_FAILURE); + } + if ((err = rt_task_create(&th_startRobot, "th_startRobot", 0, PRIORITY_TSTARTROBOT, 0))) { + cerr << "Error task create th_startRobot: " << strerror(-err) << endl; + exit(EXIT_FAILURE); + } + if ((err = rt_task_create(&th_move, "th_move", 0, PRIORITY_TMOVE, 0))) { + cerr << "Error task create th_move: " << strerror(-err) << endl; + exit(EXIT_FAILURE); + } - /* ************************************************************************************* - * Message queues creation - * *************************************************************************************/ - 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); - } - cout << "Queues created successfully" << endl; + if ((err = rt_task_create(&th_sendBatteryLevel, "th_sendBatteryLevel", 0, PRIORITY_TBATTERY, 0))) { + cerr << "Error task create th_sendBatteryLevel: " << strerror(-err) << endl; + exit(EXIT_FAILURE); + } + + cout << "Tasks created successfully" << endl; + + /* ************************************************************************************* + * Message queues creation + * *************************************************************************************/ + 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); + } + cout << "Queues created successfully" << endl; } @@ -150,26 +156,30 @@ void Tasks::Run() { } 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); - } - if (err = rt_task_start(&th_openComRobot, (void(*)(void*)) & Tasks::OpenComRobot, this)) { - cerr << "Error task start OpenComRobot: " << strerror(-err) << endl; - exit(EXIT_FAILURE); - } - if (err = rt_task_start(&th_startRobot, (void(*)(void*)) & Tasks::StartRobotTask, this)) { - cerr << "Error task start StartRobotTask: " << strerror(-err) << endl; - exit(EXIT_FAILURE); - } - if (err = rt_task_start(&th_move, (void(*)(void*)) & Tasks::MoveTask, this)) { - cerr << "Error task start MoveTask: " << strerror(-err) << endl; - exit(EXIT_FAILURE); + 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); + } + if (err = rt_task_start(&th_openComRobot, (void (*)(void *)) &Tasks::OpenComRobot, this)) { + cerr << "Error task start OpenComRobot: " << strerror(-err) << endl; + exit(EXIT_FAILURE); + } + if (err = rt_task_start(&th_startRobot, (void (*)(void *)) &Tasks::StartRobotTask, this)) { + cerr << "Error task start StartRobotTask: " << strerror(-err) << endl; + exit(EXIT_FAILURE); + } + if (err = rt_task_start(&th_move, (void (*)(void *)) &Tasks::MoveTask, this)) { + cerr << "Error task start MoveTask: " << strerror(-err) << endl; + exit(EXIT_FAILURE); + } + if (err = rt_task_start(&th_sendBatteryLevel, (void (*)(void *)) &Tasks::SendBatteryLevel, this)) { + cerr << "Error task start SendBatteryLevel: " << strerror(-err) << endl; + exit(EXIT_FAILURE); + } - cout << "Tasks launched" << endl; + cout << "Tasks launched" << endl; } /** @@ -326,23 +336,23 @@ void Tasks::ReceiveFromMonTask(void *arg) { * 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(robot.StartWithoutWD()); - rt_mutex_release(&mutex_robot); - cout << msgSend->GetID(); - cout << ")" << endl; + 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 << "Movement answer: " << msgSend->ToString() << endl; - WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon + 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); - } + if (msgSend->GetID() == MESSAGE_ANSWER_ACK) { + rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE); + robotStarted = 1; + rt_mutex_release(&mutex_robotStarted); + } } } @@ -350,21 +360,21 @@ void Tasks::ReceiveFromMonTask(void *arg) { * @brief Thread handling control of the robot. */ [[noreturn]] void Tasks::MoveTask(void *arg) { - int rs; - int cpMove; - - cout << "Start " << __PRETTY_FUNCTION__ << endl; - // Synchronization barrier (waiting that all tasks are starting) - rt_sem_p(&sem_barrier, TM_INFINITE); - - /* ************************************************************************************** - * The task starts here - * *************************************************************************************/ - rt_task_set_periodic(nullptr, TM_NOW, 100000000); + int rs; + int cpMove; - while (true) { - rt_task_wait_period(nullptr); - cout << "Periodic movement update"; + cout << "Start " << __PRETTY_FUNCTION__ << endl; + // Synchronization barrier (waiting that all tasks are starting) + rt_sem_p(&sem_barrier, TM_INFINITE); + + /* ************************************************************************************** + * The task starts here + * *************************************************************************************/ + rt_task_set_periodic(nullptr, TM_NOW, 100000000); + + 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); diff --git a/software/raspberry/superviseur-robot/tasks.h b/software/raspberry/superviseur-robot/tasks.h index 9959f52..1ac61d0 100644 --- a/software/raspberry/superviseur-robot/tasks.h +++ b/software/raspberry/superviseur-robot/tasks.h @@ -123,25 +123,32 @@ private: */ [[noreturn]] void OpenComRobot(void *arg); - /** - * @brief Thread starting the communication with the robot. - */ - [[noreturn]] void StartRobotTask(void *arg); - - /** - * @brief Thread handling control of the robot. - */ - [[noreturn]] void MoveTask(void *arg); - - /* ********************************************************************* - * Queue services - * ********************************************************************/ - /** - * Write a message in a given queue - * @param queue Queue identifier - * @param msg Message to be stored - */ - void WriteInQueue(RT_QUEUE *queue, Message *msg); + /** + * @brief Thread starting the communication with the robot. + */ + [[noreturn]] void StartRobotTask(void *arg); + + /** + * @brief Thread handling control of the robot. + */ + [[noreturn]] void MoveTask(void *arg); + + /** + * @brief Thread handling battery level sending. + */ + [[noreturn]] void SendBatteryLevel(void *arg); + + + + /* ********************************************************************* + * Queue services + * ********************************************************************/ + /** + * Write a message in a given queue + * @param queue Queue identifier + * @param msg Message to be stored + */ + void WriteInQueue(RT_QUEUE *queue, Message *msg); /** * Read a message from a given queue, block if empty