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"};
}
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;
}

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

@ -23,7 +23,7 @@
using namespace std;
#define SERVER_PORT 5544
constexpr int SERVER_PORT = 5544;
/**
* 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.
*/
void Tasks::ServerTask(void *arg) {
int status;
[[noreturn]] void Tasks::ServerTask(void *arg) {
cout << "Start " << __PRETTY_FUNCTION__ << endl;
// Synchronization barrier (waiting that all tasks are started)
rt_sem_p(&sem_barrier, TM_INFINITE);
@ -230,19 +228,28 @@ void Tasks::ServerTask(void *arg) {
/* **************************************************************************************
* 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)
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);
cout << "Open server on port " << SERVER_PORT << " (" << status << ")" << endl;
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);
// 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
* *************************************************************************************/
rt_sem_p(&sem_serverOk, TM_INFINITE);
cout << "Received message from monitor activated" << endl;
while (true) {
msgRcv = monitor.Read();
cout << "Rcv <= " << msgRcv->ToString() << endl;
rt_sem_p(&sem_serverOk, TM_INFINITE);
cout << "Received message from monitor activated" << endl;
if (msgRcv->CompareID(MESSAGE_MONITOR_LOST)) {
cout << "Monitor connection lost! Stopping robot..." << endl;
bool lostConnection = false;
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_stopRobot);
rt_sem_v(&sem_stopComRobot);
rt_sem_v(&sem_stopServer);
} else if (msgRcv->CompareID(MESSAGE_ROBOT_COM_OPEN)) {
rt_sem_v(&sem_openComRobot);
} else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITHOUT_WD)) {
rt_mutex_acquire(&mutex_watchdogMode, TM_INFINITE);
watchdogMode = WITHOUT_WATCHDOG;
rt_mutex_release(&mutex_watchdogMode);
rt_sem_v(&sem_startRobot);
} else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITH_WD)) {
// TODO: gérer la watchdog
rt_mutex_acquire(&mutex_watchdogMode, TM_INFINITE);
watchdogMode = WITH_WATCHDOG;
rt_mutex_release(&mutex_watchdogMode);
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_sem_v(&sem_stopRobot);
rt_sem_v(&sem_stopComRobot);
rt_sem_v(&sem_stopServer);
lostConnection = true;
} else if (msgRcv->CompareID(MESSAGE_ROBOT_COM_OPEN)) {
rt_sem_v(&sem_openComRobot);
} else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITHOUT_WD)) {
rt_mutex_acquire(&mutex_watchdogMode, TM_INFINITE);
watchdogMode = WITHOUT_WATCHDOG;
rt_mutex_release(&mutex_watchdogMode);
rt_sem_v(&sem_startRobot);
} else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITH_WD)) {
// TODO: gérer la watchdog
rt_mutex_acquire(&mutex_watchdogMode, TM_INFINITE);
watchdogMode = WITH_WATCHDOG;
rt_mutex_release(&mutex_watchdogMode);
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);
} else if (msgRcv->CompareID(MESSAGE_CAM_OPEN)) {
// TODO
} else if (msgRcv->CompareID(MESSAGE_CAM_CLOSE)) {
// TODO
} else if (msgRcv->CompareID(MESSAGE_CAM_ASK_ARENA)) {
// TODO
} else if (msgRcv->CompareID(MESSAGE_CAM_ARENA_CONFIRM)) {
// TODO
} else if (msgRcv->CompareID(MESSAGE_CAM_ARENA_INFIRM)) {
// TODO
} else if (msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_START)) {
// TODO
} else if (msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_STOP)) {
// TODO
rt_mutex_acquire(&mutex_move, TM_INFINITE);
move = msgRcv->GetID();
rt_mutex_release(&mutex_move);
} else if (msgRcv->CompareID(MESSAGE_CAM_OPEN)) {
// TODO
} else if (msgRcv->CompareID(MESSAGE_CAM_CLOSE)) {
// TODO
} else if (msgRcv->CompareID(MESSAGE_CAM_ASK_ARENA)) {
// TODO
} else if (msgRcv->CompareID(MESSAGE_CAM_ARENA_CONFIRM)) {
// TODO
} else if (msgRcv->CompareID(MESSAGE_CAM_ARENA_INFIRM)) {
// TODO
} else if (msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_START)) {
// TODO
} else if (msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_STOP)) {
// 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)) {
counter = 0;
} else {
counter ++;
counter++;
}
if (counter == 3) {
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.
*/
void ServerTask(void *arg);
[[noreturn]] void ServerTask(void *arg);
/**
* @brief Thread sending data to monitor.