263 lines
7.7 KiB
C++
263 lines
7.7 KiB
C++
|
/*
|
||
|
|
* Copyright (C) 2018 dimercur
|
||
|
|
*
|
||
|
|
* This program is free software: you can redistribute it and/or modify
|
||
|
|
* it under the terms of the GNU General Public License as published by
|
||
|
|
* the Free Software Foundation, either version 3 of the License, or
|
||
|
|
* (at your option) any later version.
|
||
|
|
*
|
||
|
|
* This program is distributed in the hope that it will be useful,
|
||
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||
|
|
* GNU General Public License for more details.
|
||
|
|
*
|
||
|
|
* You should have received a copy of the GNU General Public License
|
||
|
|
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||
|
|
*/
|
||
|
|
|
||
|
|
/**
|
||
|
|
* \file functions.h
|
||
|
|
* \author PE.Hladik
|
||
|
|
* \version 1.0
|
||
|
|
* \date 06/06/2017
|
||
|
|
* \brief Miscellaneous functions used for destijl project.
|
||
|
|
*/
|
||
|
|
|
||
|
|
#include "tasks_pthread.h"
|
||
|
|
|
||
|
|
#ifdef __WITH_PTHREAD__
|
||
|
|
char mode_start;
|
||
|
|
|
||
|
|
void write_in_queue(RT_QUEUE *, MessageToMon);
|
||
|
|
|
||
|
|
void f_server(void *arg) {
|
||
|
|
int err;
|
||
|
|
/* INIT */
|
||
|
|
RT_TASK_INFO info;
|
||
|
|
rt_task_inquire(NULL, &info);
|
||
|
|
printf("Init %s\n", info.name);
|
||
|
|
rt_sem_p(&sem_barrier, TM_INFINITE);
|
||
|
|
|
||
|
|
err=openServer(DEFAULT_SERVER_PORT);
|
||
|
|
|
||
|
|
if (err < 0) {
|
||
|
|
printf("Failed to start server: %s\n", strerror(-err));
|
||
|
|
exit(EXIT_FAILURE);
|
||
|
|
} else {
|
||
|
|
#ifdef _WITH_TRACE_
|
||
|
|
printf("%s: server started\n", info.name);
|
||
|
|
#endif
|
||
|
|
//Waiting for a client to connect
|
||
|
|
err=acceptClient();
|
||
|
|
|
||
|
|
if (err<0) {
|
||
|
|
printf("Client accept failed: %s\n", strerror(-err));
|
||
|
|
exit(EXIT_FAILURE);
|
||
|
|
}
|
||
|
|
|
||
|
|
#ifdef _WITH_TRACE_
|
||
|
|
printf ("client connected: %d\n", err);
|
||
|
|
printf ("Rock'n'roll baby !\n");
|
||
|
|
#endif
|
||
|
|
rt_sem_broadcast(&sem_serverOk);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void f_sendToMon(void * arg) {
|
||
|
|
int err;
|
||
|
|
MessageToMon msg;
|
||
|
|
|
||
|
|
/* INIT */
|
||
|
|
RT_TASK_INFO info;
|
||
|
|
rt_task_inquire(NULL, &info);
|
||
|
|
printf("Init %s\n", info.name);
|
||
|
|
rt_sem_p(&sem_barrier, TM_INFINITE);
|
||
|
|
|
||
|
|
#ifdef _WITH_TRACE_
|
||
|
|
printf("%s : waiting for sem_serverOk\n", info.name);
|
||
|
|
#endif
|
||
|
|
rt_sem_p(&sem_serverOk, TM_INFINITE);
|
||
|
|
while (1) {
|
||
|
|
|
||
|
|
#ifdef _WITH_TRACE_
|
||
|
|
printf("%s : waiting for a message in queue\n", info.name);
|
||
|
|
#endif
|
||
|
|
if (rt_queue_read(&q_messageToMon, &msg, sizeof (MessageToRobot), TM_INFINITE) >= 0) {
|
||
|
|
#ifdef _WITH_TRACE_
|
||
|
|
printf("%s : message {%s,%s} in queue\n", info.name, msg.header, (char*)msg.data);
|
||
|
|
#endif
|
||
|
|
|
||
|
|
send_message_to_monitor(msg.header, msg.data);
|
||
|
|
free_msgToMon_data(&msg);
|
||
|
|
rt_queue_free(&q_messageToMon, &msg);
|
||
|
|
} else {
|
||
|
|
printf("Error msg queue write: %s\n", strerror(-err));
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void f_receiveFromMon(void *arg) {
|
||
|
|
MessageFromMon msg;
|
||
|
|
int err;
|
||
|
|
|
||
|
|
/* INIT */
|
||
|
|
RT_TASK_INFO info;
|
||
|
|
rt_task_inquire(NULL, &info);
|
||
|
|
printf("Init %s\n", info.name);
|
||
|
|
rt_sem_p(&sem_barrier, TM_INFINITE);
|
||
|
|
|
||
|
|
#ifdef _WITH_TRACE_
|
||
|
|
printf("%s : waiting for sem_serverOk\n", info.name);
|
||
|
|
#endif
|
||
|
|
rt_sem_p(&sem_serverOk, TM_INFINITE);
|
||
|
|
do {
|
||
|
|
#ifdef _WITH_TRACE_
|
||
|
|
printf("%s : waiting for a message from monitor\n", info.name);
|
||
|
|
#endif
|
||
|
|
err = receive_message_from_monitor(msg.header, msg.data);
|
||
|
|
#ifdef _WITH_TRACE_
|
||
|
|
printf("%s: msg {header:%s,data=%s} received from UI\n", info.name, msg.header, msg.data);
|
||
|
|
#endif
|
||
|
|
if (strcmp(msg.header, HEADER_MTS_COM_DMB) == 0) {
|
||
|
|
if (msg.data[0] == OPEN_COM_DMB) { // Open communication supervisor-robot
|
||
|
|
#ifdef _WITH_TRACE_
|
||
|
|
printf("%s: message open Xbee communication\n", info.name);
|
||
|
|
#endif
|
||
|
|
rt_sem_v(&sem_openComRobot);
|
||
|
|
}
|
||
|
|
} else if (strcmp(msg.header, HEADER_MTS_DMB_ORDER) == 0) {
|
||
|
|
if (msg.data[0] == DMB_START_WITHOUT_WD) { // Start robot
|
||
|
|
#ifdef _WITH_TRACE_
|
||
|
|
printf("%s: message start robot\n", info.name);
|
||
|
|
#endif
|
||
|
|
rt_sem_v(&sem_startRobot);
|
||
|
|
|
||
|
|
} else if ((msg.data[0] == DMB_GO_BACK)
|
||
|
|
|| (msg.data[0] == DMB_GO_FORWARD)
|
||
|
|
|| (msg.data[0] == DMB_GO_LEFT)
|
||
|
|
|| (msg.data[0] == DMB_GO_RIGHT)
|
||
|
|
|| (msg.data[0] == DMB_STOP_MOVE)) {
|
||
|
|
|
||
|
|
rt_mutex_acquire(&mutex_move, TM_INFINITE);
|
||
|
|
robotMove = msg.data[0];
|
||
|
|
rt_mutex_release(&mutex_move);
|
||
|
|
#ifdef _WITH_TRACE_
|
||
|
|
printf("%s: message update movement with %c\n", info.name, robotMove);
|
||
|
|
#endif
|
||
|
|
|
||
|
|
}
|
||
|
|
}
|
||
|
|
} while (err > 0);
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
void f_openComRobot(void * arg) {
|
||
|
|
int err;
|
||
|
|
|
||
|
|
/* INIT */
|
||
|
|
RT_TASK_INFO info;
|
||
|
|
rt_task_inquire(NULL, &info);
|
||
|
|
printf("Init %s\n", info.name);
|
||
|
|
rt_sem_p(&sem_barrier, TM_INFINITE);
|
||
|
|
|
||
|
|
while (1) {
|
||
|
|
#ifdef _WITH_TRACE_
|
||
|
|
printf("%s : Wait sem_openComRobot\n", info.name);
|
||
|
|
#endif
|
||
|
|
rt_sem_p(&sem_openComRobot, TM_INFINITE);
|
||
|
|
#ifdef _WITH_TRACE_
|
||
|
|
printf("%s : sem_openComRobot arrived => open communication robot\n", info.name);
|
||
|
|
#endif
|
||
|
|
err = open_communication_robot();
|
||
|
|
if (err == 0) {
|
||
|
|
#ifdef _WITH_TRACE_
|
||
|
|
printf("%s : the communication is opened\n", info.name);
|
||
|
|
#endif
|
||
|
|
MessageToMon msg;
|
||
|
|
set_msgToMon_header(&msg, (char*)HEADER_STM_ACK);
|
||
|
|
write_in_queue(&q_messageToMon, msg);
|
||
|
|
} else {
|
||
|
|
MessageToMon msg;
|
||
|
|
set_msgToMon_header(&msg, (char*)HEADER_STM_NO_ACK);
|
||
|
|
write_in_queue(&q_messageToMon, msg);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void f_startRobot(void * arg) {
|
||
|
|
int err;
|
||
|
|
|
||
|
|
/* INIT */
|
||
|
|
RT_TASK_INFO info;
|
||
|
|
rt_task_inquire(NULL, &info);
|
||
|
|
printf("Init %s\n", info.name);
|
||
|
|
rt_sem_p(&sem_barrier, TM_INFINITE);
|
||
|
|
|
||
|
|
while (1) {
|
||
|
|
#ifdef _WITH_TRACE_
|
||
|
|
printf("%s : Wait sem_startRobot\n", info.name);
|
||
|
|
#endif
|
||
|
|
rt_sem_p(&sem_startRobot, TM_INFINITE);
|
||
|
|
#ifdef _WITH_TRACE_
|
||
|
|
printf("%s : sem_startRobot arrived => Start robot\n", info.name);
|
||
|
|
#endif
|
||
|
|
err = send_command_to_robot(DMB_START_WITHOUT_WD);
|
||
|
|
if (err == 0) {
|
||
|
|
#ifdef _WITH_TRACE_
|
||
|
|
printf("%s : the robot is started\n", info.name);
|
||
|
|
#endif
|
||
|
|
rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
|
||
|
|
robotStarted = 1;
|
||
|
|
rt_mutex_release(&mutex_robotStarted);
|
||
|
|
MessageToMon msg;
|
||
|
|
set_msgToMon_header(&msg, (char*)HEADER_STM_ACK);
|
||
|
|
write_in_queue(&q_messageToMon, msg);
|
||
|
|
} else {
|
||
|
|
MessageToMon msg;
|
||
|
|
set_msgToMon_header(&msg, (char*)HEADER_STM_NO_ACK);
|
||
|
|
write_in_queue(&q_messageToMon, msg);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void f_move(void *arg) {
|
||
|
|
/* INIT */
|
||
|
|
RT_TASK_INFO info;
|
||
|
|
rt_task_inquire(NULL, &info);
|
||
|
|
printf("Init %s\n", info.name);
|
||
|
|
rt_sem_p(&sem_barrier, TM_INFINITE);
|
||
|
|
|
||
|
|
/* PERIODIC START */
|
||
|
|
#ifdef _WITH_PERIODIC_TRACE_
|
||
|
|
printf("%s: start period\n", info.name);
|
||
|
|
#endif
|
||
|
|
rt_task_set_periodic(NULL, TM_NOW, 100000000);
|
||
|
|
while (1) {
|
||
|
|
#ifdef _WITH_PERIODIC_TRACE_
|
||
|
|
printf("%s: Wait period \n", info.name);
|
||
|
|
#endif
|
||
|
|
rt_task_wait_period(NULL);
|
||
|
|
#ifdef _WITH_PERIODIC_TRACE_
|
||
|
|
printf("%s: Periodic activation\n", info.name);
|
||
|
|
printf("%s: move equals %c\n", info.name, robotMove);
|
||
|
|
#endif
|
||
|
|
rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
|
||
|
|
if (robotStarted) {
|
||
|
|
rt_mutex_acquire(&mutex_move, TM_INFINITE);
|
||
|
|
send_command_to_robot(robotMove);
|
||
|
|
rt_mutex_release(&mutex_move);
|
||
|
|
#ifdef _WITH_TRACE_
|
||
|
|
printf("%s: the movement %c was sent\n", info.name, robotMove);
|
||
|
|
#endif
|
||
|
|
}
|
||
|
|
rt_mutex_release(&mutex_robotStarted);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void write_in_queue(RT_QUEUE *queue, MessageToMon msg) {
|
||
|
|
void *buff;
|
||
|
|
buff = rt_queue_alloc(&q_messageToMon, sizeof (MessageToMon));
|
||
|
|
memcpy(buff, &msg, sizeof (MessageToMon));
|
||
|
|
rt_queue_send(&q_messageToMon, buff, sizeof (MessageToMon), Q_NORMAL);
|
||
|
|
}
|
||
|
|
#endif //__WITH_PTHREAD__
|