"en fait la doc avait tord il fallait mettre ça"
This commit is contained in:
父節點
e665159ce7
當前提交
6da7c4091b
共有 2 個文件被更改,包括 79 次插入 和 3 次删除
|
@ -54,6 +54,7 @@
|
|||
* semaphore, etc.)
|
||||
*/
|
||||
void Tasks::robotLossCounter(){
|
||||
int status;
|
||||
|
||||
// wait for connexion establishement
|
||||
cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
|
||||
|
@ -152,7 +153,7 @@ void Tasks::Init() {
|
|||
cout << "Mutexes created successfully" << endl << flush;
|
||||
|
||||
/**************************************************************************************/
|
||||
/* Semaphors creation */
|
||||
/* Semaphores creation */
|
||||
/**************************************************************************************/
|
||||
if (err = rt_sem_create(&sem_barrier, NULL, 0, S_FIFO)) {
|
||||
cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
|
||||
|
@ -174,6 +175,10 @@ void Tasks::Init() {
|
|||
cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
|
||||
exit(EXIT_FAILURE);
|
||||
}
|
||||
if (err = rt_sem_create(&sem_startRobotWithWatchdog, NULL, 0, S_FIFO)) {
|
||||
cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
|
||||
exit(EXIT_FAILURE);
|
||||
}
|
||||
if (err = rt_sem_create(&sem_startCamera, NULL, 0, S_FIFO)) {
|
||||
cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
|
||||
exit(EXIT_FAILURE);
|
||||
|
@ -382,6 +387,8 @@ void Tasks::ReceiveFromMonTask(void *arg) {
|
|||
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_START_WITH_WD)) {
|
||||
rt_sem_v(&sem_startRobotWithWatchdog);
|
||||
} else if (msgRcv->CompareID(MESSAGE_ROBOT_GO_FORWARD) ||
|
||||
msgRcv->CompareID(MESSAGE_ROBOT_GO_BACKWARD) ||
|
||||
msgRcv->CompareID(MESSAGE_ROBOT_GO_LEFT) ||
|
||||
|
@ -451,7 +458,7 @@ void Tasks::StartRobotTask(void *arg) {
|
|||
cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
|
||||
// Synchronization barrier (waiting that all tasks are starting)
|
||||
rt_sem_p(&sem_barrier, TM_INFINITE);
|
||||
|
||||
|
||||
/**************************************************************************************/
|
||||
/* The task startRobot starts here */
|
||||
/**************************************************************************************/
|
||||
|
@ -477,6 +484,69 @@ void Tasks::StartRobotTask(void *arg) {
|
|||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Thread starting the communication with the robot.
|
||||
*/
|
||||
|
||||
// TODO : refactor with the previous : if (watchdog) {call with; start the watchdog messages tasks} else {call without}
|
||||
void Tasks::StartRobotWithWatchDogTask(void *arg) {
|
||||
cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
|
||||
// Synchronization barrier (waiting that all tasks are starting)
|
||||
rt_sem_p(&sem_barrier, TM_INFINITE);
|
||||
|
||||
/**************************************************************************************/
|
||||
/* The task startRobot starts here */
|
||||
/**************************************************************************************/
|
||||
while (1) {
|
||||
|
||||
Message * msgSend;
|
||||
rt_sem_p(&sem_startRobot, TM_INFINITE);
|
||||
cout << "Start robot without watchdog (";
|
||||
rt_mutex_acquire(&mutex_robot, TM_INFINITE);
|
||||
msgSend = call(robot.Write(robot.StartWithWD()));
|
||||
rt_mutex_release(&mutex_robot);
|
||||
cout << msgSend->GetID();
|
||||
cout << ")" << endl;
|
||||
|
||||
cout << "Movement answer: " << msgSend->ToString() << endl << flush;
|
||||
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);
|
||||
}
|
||||
|
||||
rt_sem_v(&sem_reloadMessages, TM_INFINITE);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Thread sending a message every 500 ms to the robot to prevent it from turning off.
|
||||
*/
|
||||
|
||||
void Tasks::RobotReloadMessage(void *arg) {
|
||||
int status;
|
||||
|
||||
cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
|
||||
// Synchronization barrier (waiting that all tasks are starting)
|
||||
rt_sem_p(&sem_barrier, TM_INFINITE);
|
||||
rt_sem_p(&sem_reloadMessages);
|
||||
cout << "Starting to monitor packet losses " << endl << flush;
|
||||
rt_task_set_periodic(NULL, TM_NOW, 500*1000*1000);
|
||||
while(1) {
|
||||
rt_task_wait_period(NULL);
|
||||
status = (robot.Write(robot.reloadWD()));
|
||||
// send info to monitor
|
||||
if (state < 0) {
|
||||
WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_NACK));
|
||||
} else {
|
||||
WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_ACK));
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Thread starting the communication with the robot.
|
||||
*/
|
||||
|
|
|
@ -105,6 +105,7 @@ private:
|
|||
RT_SEM sem_openComRobot;
|
||||
RT_SEM sem_serverOk;
|
||||
RT_SEM sem_startRobot;
|
||||
RT_SEM sem_startRobotWithWatchdog;
|
||||
RT_SEM sem_startCamera;
|
||||
RT_SEM sem_connexionEstablished;
|
||||
|
||||
|
@ -141,9 +142,14 @@ private:
|
|||
* @brief Thread starting the communication with the robot.
|
||||
*/
|
||||
void StartRobotTask(void *arg);
|
||||
|
||||
/**
|
||||
* @brief Thread starting the communication with the robot with a watchdog.
|
||||
*/
|
||||
void StartRobotWithWatchDogTask(void *arg);
|
||||
|
||||
/**
|
||||
* @brief Thread starting the communication with the robot.
|
||||
* @brief Thread starting the communication with the Camera.
|
||||
*/
|
||||
void StartCameraTask(void *arg);
|
||||
|
||||
|
|
載入中…
Reference in a new issue