vergnet/very-dumb
1
1
Fork 0
Code Issues Pull requests Releases Wiki Activity

Reformat file

Browse source
This commit is contained in:
Arnaud Vergnet 2021-03-12 13:35:49 +01:00
parent c2662209c4
commit 66d4c6f94d
1 changed files with 217 additions and 216 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

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

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