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

Improve receiveFromMon and serverTask

Browse source
This commit is contained in:
Yohan Simard 2021-03-15 10:51:03 +01:00
parent 7fcc183012
commit 3a6ae4db19
4 changed files with 78 additions and 63 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

5
software/raspberry/superviseur-robot/lib/commonitor.cpp
View file

@ -100,7 +100,10 @@ int ComMonitor::Open(int port) {
throw std::runtime_error{"Can not bind socket"}; throw std::runtime_error{"Can not bind socket"};
} }
listen(socketFD, 1); if (listen(socketFD, 1) < 0) {
cerr<<"["<<__PRETTY_FUNCTION__<<"] Can not listen on port "<<to_string(port)<<endl<<flush;
throw std::runtime_error{"Can not listen on this port"};
}
return socketFD; return socketFD;
} }

2
software/raspberry/superviseur-robot/lib/commonitor.h
View file

@ -23,7 +23,7 @@
using namespace std; using namespace std;
#define SERVER_PORT 5544 constexpr int SERVER_PORT = 5544;
/** /**
* Class used for generating a server and communicating through it with monitor * Class used for generating a server and communicating through it with monitor

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

@ -220,9 +220,7 @@ void Tasks::Join() {
/** /**
* @brief Thread handling server communication with the monitor. * @brief Thread handling server communication with the monitor.
*/ */
void Tasks::ServerTask(void *arg) { [[noreturn]] void Tasks::ServerTask(void *arg) {
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);
@ -230,19 +228,28 @@ void Tasks::ServerTask(void *arg) {
/* ************************************************************************************** /* **************************************************************************************
* The task server starts here * The task server starts here
* *************************************************************************************/ * *************************************************************************************/
rt_mutex_acquire(&mutex_monitor, TM_INFINITE);
status = monitor.Open(SERVER_PORT);
rt_mutex_release(&mutex_monitor);
cout << "Open server on port " << (SERVER_PORT) << " (" << status << ")" << endl; while (true) {
rt_mutex_acquire(&mutex_monitor, TM_INFINITE);
int status = monitor.Open(SERVER_PORT);
rt_mutex_release(&mutex_monitor);
if (status < 0) cout << "Open server on port " << SERVER_PORT << " (" << status << ")" << endl;
throw runtime_error{
"Unable to start server on port " + to_string(SERVER_PORT) if (status < 0) {
}; throw runtime_error{"Unable to start server on port " + to_string(SERVER_PORT)};
monitor.AcceptClient(); // Wait the monitor client }
cout << "Rock'n'Roll baby, client accepted!" << endl;
rt_sem_broadcast(&sem_serverOk); monitor.AcceptClient(); // Wait the monitor client
cout << "Rock'n'Roll baby, client accepted!" << endl;
rt_sem_broadcast(&sem_serverOk);
// Wait for stopServer signal
rt_sem_p(&sem_stopServer, TM_INFINITE);
rt_mutex_acquire(&mutex_monitor, TM_INFINITE);
monitor.Close();
rt_mutex_release(&mutex_monitor);
}
} }
/** /**
@ -283,57 +290,62 @@ void Tasks::ServerTask(void *arg) {
/* ************************************************************************************* /* *************************************************************************************
* The task receiveFromMon starts here * The task receiveFromMon starts here
* *************************************************************************************/ * *************************************************************************************/
rt_sem_p(&sem_serverOk, TM_INFINITE);
cout << "Received message from monitor activated" << endl;
while (true) { while (true) {
msgRcv = monitor.Read(); rt_sem_p(&sem_serverOk, TM_INFINITE);
cout << "Rcv <= " << msgRcv->ToString() << endl; cout << "Received message from monitor activated" << endl;
if (msgRcv->CompareID(MESSAGE_MONITOR_LOST)) { bool lostConnection = false;
cout << "Monitor connection lost! Stopping robot..." << endl; while (!lostConnection) {
msgRcv = monitor.Read();
cout << "Rcv <= " << msgRcv->ToString() << endl;
if (msgRcv->CompareID(MESSAGE_MONITOR_LOST)) {
cout << "Monitor connection lost! Stopping robot..." << endl;
// rt_sem_v(&sem_stopCamera); // TODO // rt_sem_v(&sem_stopCamera); // TODO
rt_sem_v(&sem_stopRobot); rt_sem_v(&sem_stopRobot);
rt_sem_v(&sem_stopComRobot); rt_sem_v(&sem_stopComRobot);
rt_sem_v(&sem_stopServer); rt_sem_v(&sem_stopServer);
} else if (msgRcv->CompareID(MESSAGE_ROBOT_COM_OPEN)) { lostConnection = true;
rt_sem_v(&sem_openComRobot); } else if (msgRcv->CompareID(MESSAGE_ROBOT_COM_OPEN)) {
} else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITHOUT_WD)) { rt_sem_v(&sem_openComRobot);
rt_mutex_acquire(&mutex_watchdogMode, TM_INFINITE); } else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITHOUT_WD)) {
watchdogMode = WITHOUT_WATCHDOG; rt_mutex_acquire(&mutex_watchdogMode, TM_INFINITE);
rt_mutex_release(&mutex_watchdogMode); watchdogMode = WITHOUT_WATCHDOG;
rt_sem_v(&sem_startRobot); rt_mutex_release(&mutex_watchdogMode);
} else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITH_WD)) { rt_sem_v(&sem_startRobot);
// TODO: gérer la watchdog } else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITH_WD)) {
rt_mutex_acquire(&mutex_watchdogMode, TM_INFINITE); // TODO: gérer la watchdog
watchdogMode = WITH_WATCHDOG; rt_mutex_acquire(&mutex_watchdogMode, TM_INFINITE);
rt_mutex_release(&mutex_watchdogMode); watchdogMode = WITH_WATCHDOG;
rt_sem_v(&sem_startRobot); rt_mutex_release(&mutex_watchdogMode);
} else if (msgRcv->CompareID(MESSAGE_ROBOT_GO_FORWARD) || rt_sem_v(&sem_startRobot);
msgRcv->CompareID(MESSAGE_ROBOT_GO_BACKWARD) || } else if (msgRcv->CompareID(MESSAGE_ROBOT_GO_FORWARD) ||
msgRcv->CompareID(MESSAGE_ROBOT_GO_LEFT) || msgRcv->CompareID(MESSAGE_ROBOT_GO_BACKWARD) ||
msgRcv->CompareID(MESSAGE_ROBOT_GO_RIGHT) || msgRcv->CompareID(MESSAGE_ROBOT_GO_LEFT) ||
msgRcv->CompareID(MESSAGE_ROBOT_STOP)) { msgRcv->CompareID(MESSAGE_ROBOT_GO_RIGHT) ||
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);
} else if (msgRcv->CompareID(MESSAGE_CAM_OPEN)) { } else if (msgRcv->CompareID(MESSAGE_CAM_OPEN)) {
// TODO // TODO
} else if (msgRcv->CompareID(MESSAGE_CAM_CLOSE)) { } else if (msgRcv->CompareID(MESSAGE_CAM_CLOSE)) {
// TODO // TODO
} else if (msgRcv->CompareID(MESSAGE_CAM_ASK_ARENA)) { } else if (msgRcv->CompareID(MESSAGE_CAM_ASK_ARENA)) {
// TODO // TODO
} else if (msgRcv->CompareID(MESSAGE_CAM_ARENA_CONFIRM)) { } else if (msgRcv->CompareID(MESSAGE_CAM_ARENA_CONFIRM)) {
// TODO // TODO
} else if (msgRcv->CompareID(MESSAGE_CAM_ARENA_INFIRM)) { } else if (msgRcv->CompareID(MESSAGE_CAM_ARENA_INFIRM)) {
// TODO // TODO
} else if (msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_START)) { } else if (msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_START)) {
// TODO // TODO
} else if (msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_STOP)) { } else if (msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_STOP)) {
// TODO // TODO
}
delete (msgRcv); // mus be deleted manually, no consumer
} }
delete (msgRcv); // mus be deleted manually, no consumer
} }
} }
@ -471,7 +483,7 @@ void Tasks::ServerTask(void *arg) {
if (msgSend->CompareID(MESSAGE_ANSWER_ACK)) { if (msgSend->CompareID(MESSAGE_ANSWER_ACK)) {
counter = 0; counter = 0;
} else { } else {
counter ++; counter++;
} }
if (counter == 3) { if (counter == 3) {
rt_sem_v(&sem_stopRobot); rt_sem_v(&sem_stopRobot);

2
software/raspberry/superviseur-robot/tasks.h
View file

@ -116,7 +116,7 @@ private:
/** /**
* @brief Thread handling server communication with the monitor. * @brief Thread handling server communication with the monitor.
*/ */
void ServerTask(void *arg); [[noreturn]] void ServerTask(void *arg);
/** /**
* @brief Thread sending data to monitor. * @brief Thread sending data to monitor.