mirror of
https://github.com/eddyem/pusirobot.git
synced 2025-12-06 10:35:11 +03:00
add more functions
This commit is contained in:
parent
6f9c74f166
commit
3dc2627c64
@ -57,6 +57,8 @@ myoption cmdlnopts[] = {
|
||||
// common options
|
||||
{"help", NO_ARGS, NULL, 'h', arg_int, APTR(&help), _("show this help")},
|
||||
{"device", NEED_ARG, NULL, 'i', arg_string, APTR(&G.device), _("serial device name (default: none)")},
|
||||
{"vid", NEED_ARG, NULL, 'V', arg_string, APTR(&G.vid), _("serial device vendor ID (default: none)")},
|
||||
{"pid", NEED_ARG, NULL, 'P', arg_string, APTR(&G.pid), _("serial device product ID (default: none)")},
|
||||
{"port", NEED_ARG, NULL, 'p', arg_string, APTR(&G.port), _("network port to connect (default: " DEFAULT_PORT ")")},
|
||||
{"logfile", NEED_ARG, NULL, 'l', arg_string, APTR(&G.logfile), _("save logs to file (default: none)")},
|
||||
{"echo", NO_ARGS, NULL, 'e', arg_int, APTR(&G.echo), _("echo users commands back")},
|
||||
|
||||
@ -63,9 +63,9 @@ int main(int argc, char **argv){
|
||||
signal(SIGTSTP, SIG_IGN); // ignore ctrl+Z
|
||||
|
||||
if(GP->logfile){
|
||||
Cl_loglevel lvl = LOGLEVEL_ERR;
|
||||
Cl_loglevel lvl = LOGLEVEL_ERR; // default log level - errors
|
||||
int v = GP->verb;
|
||||
while(v--){ // increase loglevel
|
||||
while(v--){ // increase loglevel for each "-v"
|
||||
if(++lvl == LOGLEVEL_ANY) break;
|
||||
}
|
||||
OPENLOG(GP->logfile, lvl);
|
||||
|
||||
@ -22,5 +22,93 @@
|
||||
#include "cmdlnopts.h"
|
||||
#include "processmotors.h"
|
||||
#include "pusirobot.h"
|
||||
#include "socket.h"
|
||||
|
||||
#include <fcntl.h> // open
|
||||
#include <stdio.h> // printf
|
||||
#include <string.h> // strcmp
|
||||
#include <sys/stat.h> // open
|
||||
#include <unistd.h> // usleep
|
||||
#include <usefull_macros.h>
|
||||
|
||||
message CANbusMessages = {0}; // CANserver thread is master
|
||||
|
||||
static void *stpemulator(void *arg);
|
||||
|
||||
// handlers for standard types
|
||||
thread_handler CANhandlers[] = {
|
||||
{"emulation", stpemulator},
|
||||
{NULL, NULL}
|
||||
};
|
||||
|
||||
thread_handler *get_handler(const char *name){
|
||||
for(thread_handler *ret = CANhandlers; ret->name; ++ret){
|
||||
if(strcmp(ret->name, name)) continue;
|
||||
return ret;
|
||||
}
|
||||
return NULL;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief CANserver - main CAN thread; receive/transmit raw messages by CANbusMessages
|
||||
* @param data - unused
|
||||
* @return unused
|
||||
*/
|
||||
void *CANserver(_U_ void *data){
|
||||
char *devname = find_device();
|
||||
if(!devname){
|
||||
LOGERR("Can't find serial device");
|
||||
ERRX("Can't find serial device");
|
||||
}
|
||||
while(1){
|
||||
int fd = open(devname, O_RDONLY);
|
||||
if(fd == -1){
|
||||
WARN("open()");
|
||||
LOGWARN("Device %s is absent", devname);
|
||||
FREE(devname);
|
||||
double t0 = dtime();
|
||||
while(dtime() - t0 < 5.){
|
||||
if((devname = find_device())) break;
|
||||
usleep(1000);
|
||||
}
|
||||
if(!devname){
|
||||
LOGERR("Can't open serial device, kill myself");
|
||||
ERRX("Can't open device, kill myself");
|
||||
}else LOGMSG("Change device to %s", devname);
|
||||
}else close(fd);
|
||||
|
||||
char *mesg = getmesg(idxMISO, &CANbusMessages);
|
||||
if(mesg){
|
||||
DBG("Received message: %s", mesg);
|
||||
FREE(mesg);
|
||||
// global messages to all clients:
|
||||
addmesg(idxMISO, &ServerMessages, "CANserver works\n");
|
||||
}
|
||||
}
|
||||
LOGERR("CANserver(): UNREACHABLE CODE REACHED!");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
|
||||
static void *stpemulator(void *arg){
|
||||
threadinfo *ti = (threadinfo*)arg;
|
||||
while(1){
|
||||
char *mesg = getmesg(idxMISO, &ti->mesg);
|
||||
if(mesg){
|
||||
DBG("Stepper emulator got: %s", mesg);
|
||||
addmesg(idxMISO, &ServerMessages, mesg);
|
||||
/* do something */
|
||||
FREE(mesg);
|
||||
}
|
||||
int r100 = rand() % 10000;
|
||||
if(r100 < 20){ // 20% of probability
|
||||
addmesg(idxMISO, &ServerMessages, "stpemulator works fine!\n");
|
||||
}
|
||||
if(r100 > 9998){
|
||||
addmesg(idxMISO, &ServerMessages, "O that's good!\n");
|
||||
}
|
||||
usleep(1000);
|
||||
}
|
||||
LOGERR("stpemulator(): UNREACHABLE CODE REACHED!");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
@ -20,6 +20,12 @@
|
||||
#ifndef PROCESSMOTORS_H__
|
||||
#define PROCESSMOTORS_H__
|
||||
|
||||
#include "threadlist.h"
|
||||
|
||||
extern message CANbusMessages;
|
||||
extern thread_handler CANhandlers[];
|
||||
|
||||
void *CANserver(void *data);
|
||||
thread_handler *get_handler(const char *name);
|
||||
|
||||
#endif // PROCESSMOTORS_H__
|
||||
|
||||
@ -18,39 +18,24 @@
|
||||
|
||||
#include "aux.h"
|
||||
#include "cmdlnopts.h"
|
||||
#include "processmotors.h"
|
||||
#include "proto.h"
|
||||
#include "threadlist.h"
|
||||
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <usefull_macros.h>
|
||||
|
||||
/**
|
||||
* @brief sendraw - send raw data to CANbus
|
||||
* @param id - CANid (in string format)
|
||||
* @param data - data to send (delimeters are: space, tab, comma or semicolon)
|
||||
* WARNING! parameter `data` will be broken after this function
|
||||
* id & data can be decimal, hexadecimal or octal
|
||||
* @return answer to client
|
||||
*/
|
||||
char *sendraw(char *id, char *data){
|
||||
char buf[128], *s, *saveptr;
|
||||
if(!id) return strdup("Need CAN ID\n");
|
||||
long ID, info[8]={0};
|
||||
int i;
|
||||
if(str2long(id, &ID)){
|
||||
snprintf(buf, 128, "Wrong ID: %s\n", id);
|
||||
return strdup(buf);
|
||||
}
|
||||
for(s = data, i = 0; ; s = NULL, ++i){
|
||||
char *nxt = strtok_r(s, " \t,;\r\n", &saveptr);
|
||||
if(!nxt) break;
|
||||
if(str2long(nxt, &info[i])) break;
|
||||
}
|
||||
if(i > 8) return strdup("Not more than 8 data bytes\n");
|
||||
snprintf(buf, 128, "ID=%ld, datalen=%d, data={%ld, %ld, %ld, %ld, %ld, %ld, %ld, %ld}\n",
|
||||
ID, i, info[0], info[1], info[2], info[3], info[4], info[5], info[6], info[7]);
|
||||
return strdup(buf);
|
||||
}
|
||||
// standard answers of processCommand
|
||||
static const char *ANS_OK = "OK\n";
|
||||
static const char *ANS_WRONGCANID = "Wrong CANID\n";
|
||||
static const char *ANS_NOTFOUND = "Thread not found\n";
|
||||
static const char *ANS_CANTSEND = "Can't send message\n";
|
||||
|
||||
static const char *sendraw(char *id, char *data);
|
||||
static const char *regthr(char *thrname, char *data);
|
||||
static const char *unregthr(char *thrname, char *data);
|
||||
static const char *sendmsg(char *thrname, char *data);
|
||||
|
||||
/*
|
||||
* Commands format:
|
||||
@ -61,24 +46,107 @@ char *sendraw(char *id, char *data){
|
||||
* - data - function data (e.g. relmove turret1 150)
|
||||
* you can get full list of functions by function `help`
|
||||
*/
|
||||
|
||||
typedef struct{
|
||||
char *fname;
|
||||
char *(*handler)(char *arg1, char *arg2);
|
||||
const char *fname; // function name
|
||||
const char *(*handler)(char *arg1, char *arg2); // data handler (arg1 and arg2 could be changed)
|
||||
} cmditem;
|
||||
|
||||
// array with known functions
|
||||
static cmditem functions[] = {
|
||||
{"raw", sendraw},
|
||||
{"register", regthr},
|
||||
{"unregister", unregthr},
|
||||
{"mesg", sendmsg},
|
||||
{NULL, NULL}
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief sendraw - send raw data to CANbus
|
||||
* @param id - CANid (in string format)
|
||||
* @param data - data to send (delimeters are: space, tab, comma or semicolon)
|
||||
* WARNING! parameter `data` will be broken after this function
|
||||
* id & data can be decimal, hexadecimal or octal
|
||||
* @return answer to client
|
||||
*/
|
||||
static const char *sendraw(char *id, char *data){
|
||||
char buf[128], *s, *saveptr;
|
||||
if(!id) return "Need CAN ID\n";
|
||||
long ID, info[9]={0};
|
||||
int i;
|
||||
if(str2long(id, &ID)){
|
||||
return ANS_WRONGCANID;
|
||||
}
|
||||
for(s = data, i = 0; i < 9; s = NULL, ++i){
|
||||
char *nxt = strtok_r(s, " \t,;\r\n", &saveptr);
|
||||
if(!nxt) break;
|
||||
if(str2long(nxt, &info[i])) break;
|
||||
}
|
||||
if(i > 8) return "Not more than 8 data bytes\n";
|
||||
snprintf(buf, 128, "ID=%ld, datalen=%d, data={%ld, %ld, %ld, %ld, %ld, %ld, %ld, %ld}\n",
|
||||
ID, i, info[0], info[1], info[2], info[3], info[4], info[5], info[6], info[7]);
|
||||
addmesg(idxMISO, &CANbusMessages, buf);
|
||||
return ANS_OK;
|
||||
}
|
||||
|
||||
// register new thread
|
||||
/**
|
||||
* @brief regthr - register new thread
|
||||
* @param thrname - thread name
|
||||
* @param data - CANID and thread role
|
||||
* @return answer to client
|
||||
*/
|
||||
static const char *regthr(char *thrname, char *data){
|
||||
threadinfo *ti = findThreadByName(thrname);
|
||||
if(ti) return "Thread exists\n";
|
||||
char *saveptr;
|
||||
char *id = strtok_r(data, " \t,;\r\n", &saveptr);
|
||||
if(!id) return ANS_WRONGCANID;
|
||||
char *role = strtok_r(NULL, " \t,;\r\n", &saveptr);
|
||||
if(!role) return "No thread role\n";
|
||||
DBG("Data='%s'; id='%s', role='%s'", data, id, role);
|
||||
long ID;
|
||||
if(str2long(data, &ID)){
|
||||
return ANS_WRONGCANID;
|
||||
}
|
||||
DBG("Check ID");
|
||||
ti = findThreadByID(ID);
|
||||
if(ti) return "Thread with given ID exists\n";
|
||||
thread_handler *h = get_handler(role);
|
||||
if(!h) return "Unknown role\n";
|
||||
if(!registerThread(thrname, ID, h->handler)) return "Can't register\n";
|
||||
return ANS_OK;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief unregthr - delete thread
|
||||
* @param thrname - thread's name
|
||||
* @param data - unused
|
||||
* @return answer
|
||||
*/
|
||||
static const char *unregthr(char *thrname, _U_ char *data){
|
||||
if(killThreadByName(thrname)) return ANS_NOTFOUND;
|
||||
return ANS_OK;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief sendmsg - send message to given thread
|
||||
* @param thrname - thread's naem
|
||||
* @param data - data to send
|
||||
* @return answer
|
||||
*/
|
||||
static const char *sendmsg(char *thrname, char *data){
|
||||
threadinfo *ti = findThreadByName(thrname);
|
||||
if(!ti) return ANS_NOTFOUND;
|
||||
if(!addmesg(idxMISO, &ti->mesg, data)) return ANS_CANTSEND;
|
||||
return ANS_OK;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief processCommand - parse command received by socket
|
||||
* @param cmd (io) - text command (after this function its content will be broken!)
|
||||
* @return answer to user (or NULL if none) !!!ALLOCATED HERE, should be FREEd!!!
|
||||
* @return NULL or error answer to user
|
||||
*/
|
||||
char *processCommand(char *cmd){
|
||||
const char *processCommand(char *cmd){
|
||||
if(!cmd) return NULL;
|
||||
char *saveptr = NULL, *fname = NULL, *procname = NULL, *data = NULL;
|
||||
DBG("Got %s", cmd);
|
||||
@ -95,7 +163,7 @@ char *processCommand(char *cmd){
|
||||
for(cmditem *item = functions; item->fname; ++item){
|
||||
if(0 == strcasecmp(item->fname, fname)) return item->handler(procname, data);
|
||||
}
|
||||
return strdup("Wrong command\n");
|
||||
return "Wrong command\n";
|
||||
}
|
||||
|
||||
#if 0
|
||||
|
||||
@ -20,6 +20,6 @@
|
||||
#ifndef PROTO_H__
|
||||
#define PROTO_H__
|
||||
|
||||
char *processCommand(char *cmd);
|
||||
const char *processCommand(char *cmd);
|
||||
|
||||
#endif // PROTO_H__
|
||||
|
||||
@ -18,20 +18,21 @@
|
||||
|
||||
#include "aux.h"
|
||||
#include "cmdlnopts.h" // glob_pars
|
||||
#include "processmotors.h"
|
||||
#include "proto.h"
|
||||
#include "socket.h"
|
||||
#include "term.h"
|
||||
|
||||
#include <arpa/inet.h> // inet_ntop
|
||||
#include <sys/ioctl.h>
|
||||
#include <limits.h> // INT_xxx
|
||||
#include <netdb.h> // addrinfo
|
||||
#include <poll.h>
|
||||
#include <pthread.h>
|
||||
#include <signal.h> // pthread_kill
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <sys/stat.h> // open
|
||||
#include <sys/syscall.h> // syscall
|
||||
#include <fcntl.h> // open
|
||||
#include <unistd.h> // daemon
|
||||
#include <usefull_macros.h>
|
||||
|
||||
@ -40,41 +41,7 @@
|
||||
// Max amount of connections
|
||||
#define BACKLOG (30)
|
||||
|
||||
extern glob_pars *GP;
|
||||
|
||||
/*
|
||||
* Define global data buffers here
|
||||
*/
|
||||
|
||||
/**************** COMMON FUNCTIONS ****************/
|
||||
/**
|
||||
* wait for answer from socket
|
||||
* @param sock - socket fd
|
||||
* @return 0 in case of error or timeout, 1 in case of socket ready
|
||||
*/
|
||||
static int waittoread(int sock){
|
||||
fd_set fds;
|
||||
struct timeval timeout;
|
||||
int rc;
|
||||
timeout.tv_sec = 1; // wait not more than 1 second
|
||||
timeout.tv_usec = 0;
|
||||
FD_ZERO(&fds);
|
||||
FD_SET(sock, &fds);
|
||||
do{
|
||||
rc = select(sock+1, &fds, NULL, NULL, &timeout);
|
||||
if(rc < 0){
|
||||
if(errno != EINTR){
|
||||
WARN("select()");
|
||||
LOGWARN("waittoread(): select() error");
|
||||
return 0;
|
||||
}
|
||||
continue;
|
||||
}
|
||||
break;
|
||||
}while(1);
|
||||
if(FD_ISSET(sock, &fds)) return 1;
|
||||
return 0;
|
||||
}
|
||||
message ServerMessages = {0};
|
||||
|
||||
/**************** SERVER FUNCTIONS ****************/
|
||||
//pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;
|
||||
@ -84,7 +51,7 @@ static int waittoread(int sock){
|
||||
* @param textbuf - zero-trailing buffer with data to send
|
||||
* @return amount of sent bytes
|
||||
*/
|
||||
static size_t send_data(int sock, char *textbuf){
|
||||
static size_t send_data(int sock, const char *textbuf){
|
||||
ssize_t Len = strlen(textbuf);
|
||||
if(Len != write(sock, textbuf, Len)){
|
||||
WARN("write()");
|
||||
@ -110,50 +77,35 @@ static char* stringscan(char *str, char *needle){
|
||||
}
|
||||
#endif
|
||||
|
||||
static void *handle_socket(void *asock){
|
||||
/**
|
||||
* @brief handle_socket - read and process data from socket
|
||||
* @param sock - socket fd
|
||||
* @return 0 if all OK, 1 if socket closed
|
||||
*/
|
||||
static int handle_socket(int sock){
|
||||
FNAME();
|
||||
int sock = *((int*)asock);
|
||||
char buff[BUFLEN];
|
||||
ssize_t rd;
|
||||
while(1){
|
||||
if(!waittoread(sock)){ // no data incoming
|
||||
continue;
|
||||
}
|
||||
if(!(rd = read(sock, buff, BUFLEN-1))){
|
||||
DBG("Client closed socket");
|
||||
LOGDBG("Socket %d closed", sock);
|
||||
break;
|
||||
}
|
||||
DBG("Got %zd bytes", rd);
|
||||
if(rd < 0){ // error
|
||||
LOGDBG("Close socket %d: read=%d", sock, rd);
|
||||
DBG("Nothing to read from fd %d (ret: %zd)", sock, rd);
|
||||
break;
|
||||
}
|
||||
// add trailing zero to be on the safe side
|
||||
buff[rd] = 0;
|
||||
// now we should check what do user want
|
||||
// here we can process user data
|
||||
DBG("user send '%s'", buff);
|
||||
LOGDBG("user send '%s'", buff);
|
||||
if(GP->echo){
|
||||
if(!send_data(sock, buff)){
|
||||
WARN("Can't send data to user, some error occured");
|
||||
}
|
||||
}
|
||||
//pthread_mutex_lock(&mutex);
|
||||
char *ans = processCommand(buff); // run command parser
|
||||
if(ans){
|
||||
send_data(sock, ans); // send answer
|
||||
FREE(ans);
|
||||
}
|
||||
//pthread_mutex_unlock(&mutex);
|
||||
ssize_t rd = read(sock, buff, BUFLEN-1);
|
||||
if(rd < 1){
|
||||
DBG("read() == %zd", rd);
|
||||
return 1;
|
||||
}
|
||||
LOGDBG("Socket %d closed", sock);
|
||||
DBG("Socket closed");
|
||||
close(sock);
|
||||
pthread_exit(NULL);
|
||||
return NULL;
|
||||
// add trailing zero to be on the safe side
|
||||
buff[rd] = 0;
|
||||
// now we should check what do user want
|
||||
// here we can process user data
|
||||
DBG("user %d send '%s'", sock, buff);
|
||||
LOGDBG("user %d send '%s'", sock, buff);
|
||||
if(GP->echo){
|
||||
send_data(sock, buff);
|
||||
}
|
||||
//pthread_mutex_lock(&mutex);
|
||||
const char *ans = processCommand(buff); // run command parser
|
||||
if(ans){
|
||||
send_data(sock, ans); // send answer
|
||||
}
|
||||
//pthread_mutex_unlock(&mutex);
|
||||
return 0;
|
||||
}
|
||||
|
||||
// main socket server
|
||||
@ -165,25 +117,63 @@ static void *server(void *asock){
|
||||
WARN("listen");
|
||||
return NULL;
|
||||
}
|
||||
int nfd = 1;
|
||||
// max amount of opened fd (+1 for server socket)
|
||||
#define MAX_FDS (3)
|
||||
struct pollfd poll_set[MAX_FDS];
|
||||
memset(poll_set, 0, sizeof(poll_set));
|
||||
poll_set[0].fd = sock;
|
||||
poll_set[0].events = POLLIN;
|
||||
while(1){
|
||||
socklen_t size = sizeof(struct sockaddr_in);
|
||||
struct sockaddr_in their_addr;
|
||||
int newsock;
|
||||
if(!waittoread(sock)) continue;
|
||||
newsock = accept(sock, (struct sockaddr*)&their_addr, &size);
|
||||
if(newsock <= 0){
|
||||
LOGERR("server(): accept() failed");
|
||||
WARN("accept()");
|
||||
continue;
|
||||
}
|
||||
pthread_t handler_thread;
|
||||
if(pthread_create(&handler_thread, NULL, handle_socket, (void*) &newsock)){
|
||||
LOGERR("server(): pthread_create() failed");
|
||||
WARN("pthread_create()");
|
||||
}else{
|
||||
LOGDBG("server(): listen thread created");
|
||||
DBG("Thread created, detouch");
|
||||
pthread_detach(handler_thread); // don't care about thread state
|
||||
poll(poll_set, nfd, 1); // poll for 1ms
|
||||
for(int fdidx = 0; fdidx < nfd; ++fdidx){ // poll opened FDs
|
||||
if((poll_set[fdidx].revents & POLLIN) == 0) continue;
|
||||
poll_set[fdidx].revents = 0;
|
||||
if(fdidx){ // client
|
||||
int fd = poll_set[fdidx].fd;
|
||||
//int nread = 0;
|
||||
//ioctl(fd, FIONREAD, &nread);
|
||||
if(handle_socket(fd)){ // socket closed - remove it from list
|
||||
close(fd);
|
||||
DBG("Client with fd %d closed", fd);
|
||||
LOGMSG("Client %d disconnected", fd);
|
||||
for(int i = fdidx; i < nfd; ++i)
|
||||
poll_set[i] = poll_set[i + 1];
|
||||
--nfd;
|
||||
}
|
||||
}else{ // server
|
||||
socklen_t size = sizeof(struct sockaddr_in);
|
||||
struct sockaddr_in their_addr;
|
||||
int newsock = accept(sock, (struct sockaddr*)&their_addr, &size);
|
||||
if(newsock <= 0){
|
||||
LOGERR("server(): accept() failed");
|
||||
WARN("accept()");
|
||||
continue;
|
||||
}
|
||||
struct in_addr ipAddr = their_addr.sin_addr;
|
||||
char str[INET_ADDRSTRLEN];
|
||||
inet_ntop(AF_INET, &ipAddr, str, INET_ADDRSTRLEN);
|
||||
DBG("Connection from %s, give fd=%d", str, newsock);
|
||||
LOGMSG("Got connection from %s, fd=%d", str, newsock);
|
||||
if(nfd == MAX_FDS){
|
||||
LOGWARN("Max amount of connections: disconnect %s (%d)", str, newsock);
|
||||
send_data(newsock, "Max amount of connections reached!\n");
|
||||
WARNX("Limit of connections reached");
|
||||
close(newsock);
|
||||
}else{
|
||||
memset(&poll_set[nfd], 0, sizeof(struct pollfd));
|
||||
poll_set[nfd].fd = newsock;
|
||||
poll_set[nfd].events = POLLIN;
|
||||
++nfd;
|
||||
}
|
||||
}
|
||||
} // endfor
|
||||
char *srvmesg = getmesg(idxMISO, &ServerMessages); // broadcast messages to all clients
|
||||
if(srvmesg){ // send broadcast message to all clients or throw them to /dev/null
|
||||
for(int fdidx = 1; fdidx < nfd; ++fdidx){
|
||||
send_data(poll_set[fdidx].fd, srvmesg);
|
||||
}
|
||||
FREE(srvmesg);
|
||||
}
|
||||
}
|
||||
LOGERR("server(): UNREACHABLE CODE REACHED!");
|
||||
@ -192,14 +182,9 @@ static void *server(void *asock){
|
||||
// data gathering & socket management
|
||||
static void daemon_(int sock){
|
||||
if(sock < 0) return;
|
||||
double tgot = 0.;
|
||||
char *devname = find_device();
|
||||
if(!devname){
|
||||
LOGERR("Can't find serial device");
|
||||
ERRX("Can't find serial device");
|
||||
}
|
||||
pthread_t sock_thread;
|
||||
if(pthread_create(&sock_thread, NULL, server, (void*) &sock)){
|
||||
pthread_t sock_thread, canserver_thread;
|
||||
if(pthread_create(&sock_thread, NULL, server, (void*) &sock) ||
|
||||
pthread_create(&canserver_thread, NULL, CANserver, NULL)){
|
||||
LOGERR("daemon_(): pthread_create() failed");
|
||||
ERR("pthread_create()");
|
||||
}
|
||||
@ -209,34 +194,22 @@ static void daemon_(int sock){
|
||||
LOGERR("Sockets thread died");
|
||||
pthread_join(sock_thread, NULL);
|
||||
if(pthread_create(&sock_thread, NULL, server, (void*) &sock)){
|
||||
LOGERR("daemon_(): new pthread_create() failed");
|
||||
ERR("pthread_create()");
|
||||
LOGERR("daemon_(): new pthread_create(sock_thread) failed");
|
||||
ERR("pthread_create(sock_thread)");
|
||||
}
|
||||
}
|
||||
if(pthread_kill(canserver_thread, 0) == ESRCH){
|
||||
WARNX("CANserver thread died");
|
||||
LOGERR("CANserver thread died");
|
||||
pthread_join(canserver_thread, NULL);
|
||||
if(pthread_create(&canserver_thread, NULL, CANserver, NULL)){
|
||||
LOGERR("daemon_(): new pthread_create(canserver_thread) failed");
|
||||
ERR("pthread_create(canserver_thread)");
|
||||
}
|
||||
}
|
||||
usleep(1000); // sleep a little or thread's won't be able to lock mutex
|
||||
if(dtime() - tgot < T_INTERVAL) continue;
|
||||
tgot = dtime();
|
||||
/*
|
||||
* INSERT CODE HERE
|
||||
* Gather data (poll_device)
|
||||
*/
|
||||
// copy temporary buffers to main
|
||||
//pthread_mutex_lock(&mutex);
|
||||
int fd = open(devname, O_RDONLY);
|
||||
if(fd == -1){
|
||||
WARN("open()");
|
||||
LOGWARN("Device %s is absent", devname);
|
||||
FREE(devname);
|
||||
double t0 = dtime();
|
||||
while(dtime() - t0 < 5.){
|
||||
if((devname = find_device())) break;
|
||||
usleep(1000);
|
||||
}
|
||||
if(!devname){
|
||||
LOGERR("Can't open serial device, kill myself");
|
||||
ERRX("Can't open device, kill myself");
|
||||
}else LOGMSG("Change device to %s", devname);
|
||||
}else close(fd);
|
||||
/*
|
||||
* INSERT CODE HERE
|
||||
* fill global data buffers
|
||||
|
||||
@ -20,11 +20,9 @@
|
||||
#ifndef __SOCKET_H__
|
||||
#define __SOCKET_H__
|
||||
|
||||
// timeout for socket closing
|
||||
#define SOCKET_TIMEOUT (5.0)
|
||||
// time interval for data polling (seconds)
|
||||
#define T_INTERVAL (10.)
|
||||
#include "threadlist.h"
|
||||
|
||||
extern message ServerMessages;
|
||||
void daemonize(char *port);
|
||||
|
||||
#endif // __SOCKET_H__
|
||||
|
||||
@ -61,8 +61,9 @@ static char *popmessage(msglist **lst){
|
||||
if(!lst || !*lst) return NULL;
|
||||
char *ret;
|
||||
msglist *node = *lst;
|
||||
ret = (*lst)->data;
|
||||
*lst = (*lst)->next;
|
||||
if(node->next) node->next->last = node->last; // pop not last message
|
||||
ret = node->data;
|
||||
*lst = node->next;
|
||||
FREE(node);
|
||||
return ret;
|
||||
}
|
||||
@ -132,9 +133,13 @@ char *addmesg(msgidx idx, message *msg, char *txt){
|
||||
if(L < 1) return NULL;
|
||||
DBG("Want to add mesg '%s' with length %zd", txt, L);
|
||||
if(pthread_mutex_lock(&msg->mutex[idx])) return NULL;
|
||||
if(!pushmessage(&msg->text[idx], txt)) return NULL;
|
||||
msglist *node = pushmessage(&msg->text[idx], txt);
|
||||
if(!node){
|
||||
pthread_mutex_unlock(&msg->mutex[idx]);
|
||||
return NULL;
|
||||
}
|
||||
pthread_mutex_unlock(&msg->mutex[idx]);
|
||||
return msg->text[idx]->data;
|
||||
return node->data;
|
||||
}
|
||||
|
||||
/**
|
||||
@ -197,12 +202,41 @@ threadinfo *registerThread(char *name, int ID, void *(*handler)(void *)){
|
||||
return ti;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief killThread - kill thread by its descriptor
|
||||
* @param lptr - pointer to thread descriptor
|
||||
* @param prev - pointer to previous thread in list or NULL (to found it here)
|
||||
* @return 0 if all OK
|
||||
*/
|
||||
int killThread(threadlist *lptr, threadlist *prev){
|
||||
if(!lptr) return 1;
|
||||
if(!prev){
|
||||
threadlist *t = thelist;
|
||||
for(; t; t = t->next){
|
||||
if(t == lptr) break;
|
||||
prev = lptr;
|
||||
}
|
||||
}
|
||||
DBG("Delete '%s', prev: '%s'", lptr->ti.name, prev->ti.name);
|
||||
threadlist *next = lptr->next;
|
||||
if(lptr == thelist) thelist = next;
|
||||
else if(prev) prev->next = next;
|
||||
for(int i = 0; i < 2; ++i){
|
||||
pthread_mutex_lock(&lptr->ti.mesg.mutex[i]);
|
||||
char *txt;
|
||||
while((txt = popmessage(&lptr->ti.mesg.text[i]))) FREE(txt);
|
||||
pthread_mutex_destroy(&lptr->ti.mesg.mutex[i]);
|
||||
}
|
||||
if(pthread_cancel(lptr->ti.thread)) WARN("Can't kill thread '%s'", lptr->ti.name);
|
||||
FREE(lptr);
|
||||
return 0;
|
||||
}
|
||||
/**
|
||||
* @brief killThread - kill and unregister thread with given name
|
||||
* @param name - thread's name
|
||||
* @return 0 if all OK
|
||||
*/
|
||||
int killThread(const char *name){
|
||||
int killThreadByName(const char *name){
|
||||
if(!name || !thelist) return 1;
|
||||
threadlist *lptr = thelist, *prev = NULL;
|
||||
for(; lptr; lptr = lptr->next){
|
||||
@ -210,19 +244,7 @@ int killThread(const char *name){
|
||||
prev = lptr;
|
||||
continue;
|
||||
}
|
||||
DBG("Found '%s', prev: '%s', delete", name, prev->ti.name);
|
||||
threadlist *next = lptr->next;
|
||||
if(lptr == thelist) thelist = next;
|
||||
else if(prev) prev->next = next;
|
||||
for(int i = 0; i < 2; ++i){
|
||||
pthread_mutex_lock(&lptr->ti.mesg.mutex[i]);
|
||||
char *txt;
|
||||
while((txt = popmessage(&lptr->ti.mesg.text[i]))) FREE(txt);
|
||||
pthread_mutex_destroy(&lptr->ti.mesg.mutex[i]);
|
||||
}
|
||||
if(pthread_cancel(lptr->ti.thread)) WARN("Can't kill thread '%s'", name);
|
||||
FREE(lptr);
|
||||
return 0;
|
||||
return killThread(lptr, prev);
|
||||
}
|
||||
return 2; // not found
|
||||
}
|
||||
|
||||
@ -31,6 +31,7 @@ typedef struct msglist_{
|
||||
struct msglist_ *next, *last; // other elements of list
|
||||
} msglist;
|
||||
|
||||
// for all threads MASTER is the thread itself, slaves are all others
|
||||
typedef enum{
|
||||
idxMOSI = 0, // master out, slave in
|
||||
idxMISO = 1, // master in, slave out
|
||||
@ -58,10 +59,16 @@ typedef struct thread_list_{
|
||||
struct thread_list_ *next; // next element
|
||||
} threadlist;
|
||||
|
||||
// name - handler pair for threads registering functions
|
||||
typedef struct{
|
||||
const char *name; // handler name
|
||||
void *(*handler)(void *); // handler function
|
||||
} thread_handler;
|
||||
|
||||
threadinfo *findThreadByName(char *name);
|
||||
threadinfo *findThreadByID(int ID);
|
||||
threadinfo *registerThread(char *name, int ID, void *(*handler)(void *));
|
||||
int killThread(const char *name);
|
||||
int killThreadByName(const char *name);
|
||||
char *getmesg(msgidx idx, message *msg);
|
||||
char *addmesg(msgidx idx, message *msg, char *txt);
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user