From 6f9c74f1662ca87b0e604076a28cc64c2192d1dd Mon Sep 17 00:00:00 2001 From: Edward Emelianov Date: Thu, 12 Nov 2020 17:42:54 +0300 Subject: [PATCH] processed --- canserver/aux.c | 96 ++++++++++ canserver/aux.h | 34 +++- canserver/canbus.c | 278 ++++++++++++++++++++++++++++ canserver/canbus.h | 48 +++++ canserver/canopen.c | 378 ++++++++++++++++++++++++++++++++++++++ canserver/canopen.h | 90 +++++++++ canserver/cmdlnopts.c | 81 -------- canserver/cmdlnopts.h | 31 +--- canserver/dicentries.in | 143 ++++++++++++++ canserver/main.c | 6 +- canserver/processmotors.c | 26 +++ canserver/processmotors.h | 25 +++ canserver/proto.c | 106 +++++++++++ canserver/proto.h | 25 +++ canserver/pusirobot.c | 82 +++++++++ canserver/pusirobot.h | 55 ++++++ canserver/socket.c | 63 +++---- canserver/threadlist.c | 324 ++++++++++++++++++++++++++++++++ canserver/threadlist.h | 68 +++++++ 19 files changed, 1807 insertions(+), 152 deletions(-) create mode 100644 canserver/canbus.c create mode 100644 canserver/canbus.h create mode 100644 canserver/canopen.c create mode 100644 canserver/canopen.h create mode 100644 canserver/dicentries.in create mode 100644 canserver/processmotors.c create mode 100644 canserver/processmotors.h create mode 100644 canserver/proto.c create mode 100644 canserver/proto.h create mode 100644 canserver/pusirobot.c create mode 100644 canserver/pusirobot.h create mode 100644 canserver/threadlist.c create mode 100644 canserver/threadlist.h diff --git a/canserver/aux.c b/canserver/aux.c index 76c6822..86c1e0e 100644 --- a/canserver/aux.c +++ b/canserver/aux.c @@ -102,3 +102,99 @@ char *find_device(){ udev_enumerate_unref(enumerate); return path; } + +Cl_log *globlog = NULL; + +/** + * @brief Cl_createlog - create log file: init mutex, test file open ability + * @param logpath - path to log file + * @param level - lowest message level (e.g. LOGLEVEL_ERR won't allow to write warn/msg/dbg) + * @return allocated structure (should be free'd later by Cl_deletelog) or NULL + */ +Cl_log *Cl_createlog(const char *logpath, Cl_loglevel level){ + if(level < LOGLEVEL_NONE || level > LOGLEVEL_ANY) return NULL; + if(!logpath) return NULL; + FILE *logfd = fopen(logpath, "a"); + if(!logfd){ + WARN("Can't open log file"); + return NULL; + } + fclose(logfd); + Cl_log *log = MALLOC(Cl_log, 1); + if(pthread_mutex_init(&log->mutex, NULL)){ + WARN("Can't init log mutex"); + FREE(log); + return NULL; + } + log->logpath = strdup(logpath); + if(!log->logpath){ + WARN("strdup()"); + FREE(log); + return NULL; + } + log->loglevel = level; + return log; +} + +void Cl_deletelog(Cl_log **log){ + if(!log || !*log) return; + FREE((*log)->logpath); + FREE(*log); +} + +/** + * @brief Cl_putlog - put message to log file with/without timestamp + * @param timest - ==1 to put timestamp + * @param log - pointer to log structure + * @param lvl - message loglevel (if lvl > loglevel, message won't be printed) + * @param fmt - format and the rest part of message + * @return amount of symbols saved in file + */ +int Cl_putlogt(int timest, Cl_log *log, Cl_loglevel lvl, const char *fmt, ...){ + if(!log || !log->logpath) return 0; + if(lvl > log->loglevel) return 0; + if(pthread_mutex_lock(&log->mutex)){ + WARN("Can't lock log mutex"); + return 0; + } + int i = 0; + FILE *logfd = fopen(log->logpath, "a+"); + if(!logfd) goto rtn; + if(timest){ + char strtm[128]; + time_t t = time(NULL); + struct tm *curtm = localtime(&t); + strftime(strtm, 128, "%Y/%m/%d-%H:%M:%S", curtm); + i = fprintf(logfd, "%s", strtm); + } + i += fprintf(logfd, "\t"); + va_list ar; + va_start(ar, fmt); + i += vfprintf(logfd, fmt, ar); + va_end(ar); + fseek(logfd, -1, SEEK_CUR); + char c; + ssize_t r = fread(&c, 1, 1, logfd); + if(1 == r){ // add '\n' if there was no newline + if(c != '\n') i += fprintf(logfd, "\n"); + } + fclose(logfd); +rtn: + pthread_mutex_unlock(&log->mutex); + return i; +} + +/** + * @brief str2long - convert full string to long + * @param str - string + * @param l (o) - converted number + * @return 0 if OK, 1 if failed + */ +int str2long(char *str, long* l){ + if(!str) return 1; + char *eptr = NULL; + long n = strtol(str, &eptr, 0); + if(*eptr) return 2; // wrong symbols in number + if(l) *l = n; + return 0; +} diff --git a/canserver/aux.h b/canserver/aux.h index 1b01999..36037c6 100644 --- a/canserver/aux.h +++ b/canserver/aux.h @@ -22,7 +22,39 @@ #include "cmdlnopts.h" -extern glob_pars *GP; +typedef enum{ + LOGLEVEL_NONE, // no logs + LOGLEVEL_ERR, // only errors + LOGLEVEL_WARN, // only warnings and errors + LOGLEVEL_MSG, // all without debug + LOGLEVEL_DBG, // all messages + LOGLEVEL_ANY // all shit +} Cl_loglevel; + +typedef struct{ + char *logpath; // full path to logfile + Cl_loglevel loglevel; // loglevel + pthread_mutex_t mutex; // log mutex +} Cl_log; + +extern Cl_log *globlog; // global log file + +Cl_log *Cl_createlog(const char *logpath, Cl_loglevel level); +#define OPENLOG(nm, lvl) (globlog = Cl_createlog(nm, lvl)) +void Cl_deletelog(Cl_log **log); +int Cl_putlogt(int timest, Cl_log *log, Cl_loglevel lvl, const char *fmt, ...); +// shortcuts for different log levels; ..ADD - add message without timestamp +#define LOGERR(...) do{Cl_putlogt(1, globlog, LOGLEVEL_ERR, __VA_ARGS__);}while(0) +#define LOGERRADD(...) do{Cl_putlogt(1, globlog, LOGLEVEL_ERR, __VA_ARGS__);}while(0) +#define LOGWARN(...) do{Cl_putlogt(1, globlog, LOGLEVEL_WARN, __VA_ARGS__);}while(0) +#define LOGWARNADD(...) do{Cl_putlogt(0, globlog, LOGLEVEL_WARN, __VA_ARGS__);}while(0) +#define LOGMSG(...) do{Cl_putlogt(1, globlog, LOGLEVEL_MSG, __VA_ARGS__);}while(0) +#define LOGMSGADD(...) do{Cl_putlogt(0, globlog, LOGLEVEL_MSG, __VA_ARGS__);}while(0) +#define LOGDBG(...) do{Cl_putlogt(1, globlog, LOGLEVEL_DBG, __VA_ARGS__);}while(0) +#define LOGDBGADD(...) do{Cl_putlogt(0, globlog, LOGLEVEL_DBG, __VA_ARGS__);}while(0) + char *find_device(); +int str2long(char *str, long* l); + #endif // AUX_H__ diff --git a/canserver/canbus.c b/canserver/canbus.c new file mode 100644 index 0000000..1974640 --- /dev/null +++ b/canserver/canbus.c @@ -0,0 +1,278 @@ +/* + * This file is part of the stepper project. + * Copyright 2020 Edward V. Emelianov . + * + * 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 . + */ + +#include +#include +#include +#include +#include + +#include "canbus.h" + +#ifndef BUFLEN +#define BUFLEN 80 +#endif + +#ifndef WAIT_TMOUT +#define WAIT_TMOUT 0.01 +#endif + +/* +This file should provide next functions: + int canbus_open(const char *devname) - calls @the beginning, return 0 if all OK + int canbus_setspeed(int speed) - set given speed (in Kbaud) @ CAN bus (return 0 if all OK) + void canbus_close() - calls @the end + int canbus_write(CANmesg *mesg) - write `data` with length `len` to ID `ID`, return 0 if all OK + int canbus_read(CANmesg *mesg) - blocking read (broadcast if ID==0 or only from given ID) from can bus, return 0 if all OK +*/ + +static TTY_descr *dev = NULL; // shoul be global to restore if die +static int serialspeed = 115200; // speed to open serial device +static pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER; + +static char *read_string(); + +/** + * @brief read_ttyX- read data from TTY with 10ms timeout WITH disconnect detection + * @param buff (o) - buffer for data read + * @param length - buffer len + * @return amount of bytes read + */ +static int read_ttyX(TTY_descr *d){ + if(!d || d->comfd < 0) return -1; + size_t L = 0; + ssize_t l; + size_t length = d->bufsz; + char *ptr = d->buf; + fd_set rfds; + struct timeval tv; + int retval; + do{ + l = 0; + FD_ZERO(&rfds); + FD_SET(d->comfd, &rfds); + tv.tv_sec = 0; tv.tv_usec = 500; + retval = select(d->comfd + 1, &rfds, NULL, NULL, &tv); + if (!retval) break; + if(FD_ISSET(d->comfd, &rfds)){ + if((l = read(d->comfd, ptr, length)) < 1){ + return -1; // disconnect or other error - close TTY & die + } + ptr += l; L += l; + length -= l; + } + }while(l && length); + d->buflen = L; + d->buf[L] = 0; + return (size_t)L; +} + + +// thread-safe writing, add trailing '\n' +static int ttyWR(const char *buff, int len){ + FNAME(); + pthread_mutex_lock(&mutex); + //canbus_clear(); + read_string(); // clear RX buffer + DBG("Write 2tty %d bytes: ", len); +#ifdef EBUG + int _U_ n = write(STDERR_FILENO, buff, len); + fprintf(stderr, "\n"); + double t0 = dtime(); +#endif + int w = write_tty(dev->comfd, buff, (size_t)len); + if(!w) w = write_tty(dev->comfd, "\n", 1); + DBG("Written, dt=%g", dtime() - t0); + int errctr = 0; + while(1){ + char *s = read_string(); // clear echo & check + if(!s || strncmp(s, buff, strlen(buff)) != 0){ + if(++errctr > 3){ + WARNX("wrong answer! Got '%s' instead of '%s'", s, buff); + w = 1; + break; + } + }else break; + } + pthread_mutex_unlock(&mutex); + DBG("Success, dt=%g", dtime() - t0); + return w; +} + +void canbus_close(){ + if(dev) close_tty(&dev); +} + +void setserialspeed(int speed){ + serialspeed = speed; +} + +void canbus_clear(){ + while(read_ttyX(dev)); +} + +int canbus_open(const char *devname){ + if(!devname){ + WARNX("canbus_open(): need device name"); + return 1; + } + if(dev) close_tty(&dev); + dev = new_tty((char*)devname, serialspeed, BUFLEN); + if(dev){ + if(!tty_open(dev, 1)) // blocking open + close_tty(&dev); + } + if(!dev){ + return 1; + } + return 0; +} + +int canbus_setspeed(int speed){ + if(speed == 0) return 0; // default - not change + char buff[BUFLEN]; + if(speed < 10 || speed > 3000){ + WARNX("Wrong CAN bus speed value: %d", speed); + return 1; + } + int len = snprintf(buff, BUFLEN, "b %d", speed); + if(len < 1) return 2; + int r = ttyWR(buff, len); + read_string(); // clear RX buf ('Reinit CAN bus with speed XXXXkbps') + return r; +} + +int canbus_write(CANmesg *mesg){ + FNAME(); + char buf[BUFLEN]; + if(!mesg || mesg->len > 8) return 1; + int rem = BUFLEN, len = 0; + int l = snprintf(buf, rem, "s %d", mesg->ID); + rem -= l; len += l; + for(uint8_t i = 0; i < mesg->len; ++i){ + l = snprintf(&buf[len], rem, " %d", mesg->data[i]); + rem -= l; len += l; + if(rem < 0) return 2; + } + canbus_clear(); + return ttyWR(buf, len); +} + +/** + * read strings from terminal (ending with '\n') with timeout + * @return NULL if nothing was read or pointer to static buffer + */ +static char *read_string(){ + static char buf[1024]; + int LL = 1023, r = 0, l; + char *ptr = NULL; + static char *optr = NULL; + if(optr && *optr){ + ptr = optr; + optr = strchr(optr, '\n'); + if(optr){ + *optr = 0; + ++optr; + } + return ptr; + } + ptr = buf; + double d0 = dtime(); + do{ + if((l = read_ttyX(dev))){ + if(l < 0){ + ERR("tty disconnected"); + } + if(l > LL){ // buffer overflow + WARNX("read_string(): buffer overflow"); + optr = NULL; + return NULL; + } + memcpy(ptr, dev->buf, dev->buflen); + r += l; LL -= l; ptr += l; + if(ptr[-1] == '\n'){ + //DBG("Newline detected"); + break; + } + d0 = dtime(); + } + }while(dtime() - d0 < WAIT_TMOUT && LL); + if(r){ + buf[r] = 0; + optr = strchr(buf, '\n'); + if(optr){ + *optr = 0; + ++optr; + }else{ + WARNX("read_string(): no newline found"); + DBG("buf: %s", buf); + optr = NULL; + return NULL; + } + DBG("buf: %s, time: %g", buf, dtime() - d0); + return buf; + } + return NULL; +} + +CANmesg *parseCANmesg(const char *str){ + static CANmesg m; + int l = sscanf(str, "%d #0x%hx 0x%hhx 0x%hhx 0x%hhx 0x%hhx 0x%hhx 0x%hhx 0x%hhx 0x%hhx", &m.timemark, &m.ID, + &m.data[0], &m.data[1], &m.data[2], &m.data[3], &m.data[4], &m.data[5], &m.data[6], &m.data[7]); + if(l < 2) return NULL; + m.len = l - 2; + return &m; +} + +#ifdef EBUG +void showM(CANmesg *m){ + printf("TS=%d, ID=0x%X", m->timemark, m->ID); + int l = m->len; + if(l) printf(", data="); + for(int i = 0; i < l; ++i) printf(" 0x%02X", m->data[i]); + printf("\n"); +} +#endif + +int canbus_read(CANmesg *mesg){ + if(!mesg) return 1; + pthread_mutex_lock(&mutex); + double t0 = dtime(); + int ID = mesg->ID; + char *ans; + CANmesg *m; + while(dtime() - t0 < T_POLLING_TMOUT){ // read answer + if((ans = read_string())){ // parse new data + if((m = parseCANmesg(ans))){ + DBG("Got canbus message (dT=%g):", dtime() - t0); +#ifdef EBUG + showM(m); +#endif + if(ID && m->ID == ID){ + memcpy(mesg, m, sizeof(CANmesg)); + DBG("All OK"); + pthread_mutex_unlock(&mutex); + return 0; + } + } + } + } + pthread_mutex_unlock(&mutex); + return 1; +} + diff --git a/canserver/canbus.h b/canserver/canbus.h new file mode 100644 index 0000000..f2c2bc8 --- /dev/null +++ b/canserver/canbus.h @@ -0,0 +1,48 @@ +/* + * This file is part of the stepper project. + * Copyright 2020 Edward V. Emelianov . + * + * 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 . + */ +#pragma once +#ifndef CANBUS_H__ +#define CANBUS_H__ + +#include + +#ifndef T_POLLING_TMOUT +#define T_POLLING_TMOUT (0.5) +#endif + +typedef struct{ + uint32_t timemark; // time since MCU run (ms) + uint16_t ID; // 11-bit identifier + uint8_t data[8]; // up to 8 bit data, data[0] is lowest + uint8_t len; // data length +} CANmesg; + +// main (necessary) functions of canbus.c: +void canbus_close(); +int canbus_open(const char *devname); +int canbus_write(CANmesg *mesg); +int canbus_read(CANmesg *mesg); +int canbus_setspeed(int speed); +void canbus_clear(); + +// auxiliary (not necessary) functions +void setserialspeed(int speed); +void showM(CANmesg *m); +CANmesg *parseCANmesg(const char *str); + +#endif // CANBUS_H__ diff --git a/canserver/canopen.c b/canserver/canopen.c new file mode 100644 index 0000000..17800d9 --- /dev/null +++ b/canserver/canopen.c @@ -0,0 +1,378 @@ +/* + * This file is part of the stepper project. + * Copyright 2020 Edward V. Emelianov . + * + * 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 . + */ + +#include +#include +#include +#include +#include + +#include "canopen.h" + +typedef struct{ + uint32_t code; + const char *errmsg; +} abortcodes; + + +static const abortcodes AC[] = { + //while read l; do N=$(echo $l|awk '{print $1 $2}'); R=$(echo $l|awk '{$1=$2=""; print substr($0,3)}'|sed 's/\.//'); echo -e "{0x$N, \"$R\"},"; done < codes.b + {0x05030000, "Toggle bit not alternated"}, + {0x05040000, "SDO protocol timed out"}, + {0x05040001, "Client/server command specifier not valid or unknown"}, + {0x05040002, "Invalid block size (block mode only)"}, + {0x05040003, "Invalid sequence number (block mode only)"}, + {0x05040004, "CRC error (block mode only)"}, + {0x05040005, "Out of memory"}, + {0x06010000, "Unsupported access to an object"}, + {0x06010001, "Attempt to read a write only object"}, + {0x06010002, "Attempt to write a read only object"}, + {0x06020000, "Object does not exist in the object dictionary"}, + {0x06040041, "Object cannot be mapped to the PDO"}, + {0x06040042, "The number and length of the objects to be mapped would exceed PDO length"}, + {0x06040043, "General parameter incompatibility reason"}, + {0x06040047, "General internal incompatibility in the device"}, + {0x06060000, "Access failed due to a hardware error"}, + {0x06070010, "Data type does not match; length of service parameter does not match"}, + {0x06070012, "Data type does not match; length of service parameter too high"}, + {0x06070013, "Data type does not match; length of service parameter too low"}, + {0x06090011, "Sub-index does not exist"}, + {0x06090030, "Value range of parameter exceeded (only for write access)"}, + {0x06090031, "Value of parameter written too high"}, + {0x06090032, "Value of parameter written too low"}, + {0x06090036, "Maximum value is less than minimum value"}, + {0x08000000, "General error"}, + {0x08000020, "Data cannot be transferred or stored to the application"}, + {0x08000021, "Data cannot be transferred or stored to the application because of local control"}, + {0x08000022, "Data cannot be transferred or stored to the application because of the present device state"}, + {0x08000023, "Object dictionary dynamic generation fails or no object dictionary is present"}, +}; + +static const int ACmax = sizeof(AC)/sizeof(abortcodes) - 1; + +/** + * @brief abortcode_text - explanation of abort code + * @param abortcode - code + * @return text for error or NULL + */ +const char *abortcode_text(uint32_t abortcode){ //, int *n){ + int idx = ACmax/2, min_ = 0, max_ = ACmax, newidx = 0, iter=0; + do{ + ++iter; + uint32_t c = AC[idx].code; + //printf("idx=%d, min=%d, max=%d\n", idx, min_, max_); + if(c == abortcode){ + //if(n) *n = iter; + //DBG("got : %s", AC[idx].errmsg); + return AC[idx].errmsg; + }else if(c > abortcode){ + newidx = (idx + min_)/2; + max_ = idx; + + }else{ + newidx = (idx + max_ + 1)/2; + min_ = idx; + } + if(newidx == idx || min_ < 0 || max_ > ACmax){ + //if(n) *n = 0; + return NULL; + } + idx = newidx; + }while(1); +} + +// make CAN message from sdo object; don't support more then one block/packet +static CANmesg *mkMesg(SDO *sdo){ + static CANmesg mesg; + mesg.ID = RSDO_COBID + sdo->NID; + mesg.len = 8; + memset(mesg.data, 0, 8); + mesg.data[0] = SDO_CCS(sdo->ccs); + if(sdo->datalen){ // send N bytes of data + mesg.data[0] |= SDO_N(sdo->datalen) | SDO_E | SDO_S; + for(uint8_t i = 0; i < sdo->datalen; ++i) mesg.data[4+i] = sdo->data[i]; + } + mesg.data[1] = sdo->index & 0xff; // l + mesg.data[2] = (sdo->index >> 8) & 0xff; // h + mesg.data[3] = sdo->subindex; +#if 0 + FNAME(); + green("Make message to 0x%X: ", mesg.ID); + for(uint8_t i = 0; i < 8; ++i) printf("0x%02X ", mesg.data[i]); + printf("\n"); +#endif + return &mesg; +} + +// transform CAN-message to SDO +SDO *parseSDO(CANmesg *mesg){ + static SDO sdo; + if(mesg->len != 8){ + WARNX("Wrong SDO data length"); + return NULL; + } + uint16_t cobid = mesg->ID & COBID_MASK; + if(cobid != TSDO_COBID){ + DBG("cobid=0x%X, not a TSDO!", cobid); + return NULL; // not a transmit SDO + } + sdo.NID = mesg->ID & NODEID_MASK; + uint8_t spec = mesg->data[0]; + sdo.ccs = GET_CCS(spec); + sdo.index = (uint16_t)mesg->data[1] | ((uint16_t)mesg->data[2] << 8); + sdo.subindex = mesg->data[3]; + if((spec & SDO_E) && (spec & SDO_S)) sdo.datalen = SDO_datalen(spec); + else if(sdo.ccs == CCS_ABORT_TRANSFER) sdo.datalen = 4; // error code + else sdo.datalen = 0; // no data in message + for(uint8_t i = 0; i < sdo.datalen; ++i) sdo.data[i] = mesg->data[4+i]; + DBG("Got TSDO from NID=%d, ccs=%u, index=0x%X, subindex=0x%X, datalen=%d", sdo.NID, sdo.ccs, sdo.index, sdo.subindex, sdo.datalen); + return &sdo; +} + +// send request to read SDO +static int ask2read(uint16_t idx, uint8_t subidx, uint8_t NID){ + SDO sdo; + sdo.NID = NID; + sdo.ccs = CCS_INIT_UPLOAD; + sdo.datalen = 0; + sdo.index = idx; + sdo.subindex = subidx; + CANmesg *mesg = mkMesg(&sdo); + return canbus_write(mesg); +} + +static SDO *getSDOans(uint16_t idx, uint8_t subidx, uint8_t NID){ + FNAME(); + CANmesg mesg; + SDO *sdo = NULL; + double t0 = dtime(); + while(dtime() - t0 < SDO_ANS_TIMEOUT){ + mesg.ID = TSDO_COBID | NID; // read only from given ID + if(canbus_read(&mesg)){ + continue; + } + sdo = parseSDO(&mesg); + if(!sdo) continue; + if(sdo->index == idx && sdo->subindex == subidx) break; + } + if(!sdo || sdo->index != idx || sdo->subindex != subidx){ + WARNX("No answer from SDO 0x%X/0x%X", idx, subidx); + return NULL; + } + return sdo; +} + +/** + * @brief readSDOvalue - send request to SDO read + * @param idx - SDO index + * @param subidx - SDO subindex + * @param NID - target node ID + * @return SDO received or NULL if error + */ +SDO *readSDOvalue(uint16_t idx, uint8_t subidx, uint8_t NID){ + FNAME(); + if(ask2read(idx, subidx, NID)){ + WARNX("readSDOvalue(): Can't initiate upload"); + return NULL; + } + return getSDOans(idx, subidx, NID); +} + +static inline uint32_t mku32(uint8_t data[4]){ + return (uint32_t)(data[0] | (data[1]<<8) | (data[2]<<16) | (data[3]<<24)); +} + +static inline uint16_t mku16(uint8_t data[4]){ + return (uint16_t)(data[0] | (data[1]<<8)); +} + +static inline uint8_t mku8(uint8_t data[4]){ + return data[0]; +} + +static inline int32_t mki32(uint8_t data[4]){ + return (int32_t)(data[0] | (data[1]<<8) | (data[2]<<16) | (data[3]<<24)); +} + +static inline int16_t mki16(uint8_t data[4]){ + return (int16_t)(data[0] | (data[1]<<8)); +} + +static inline int8_t mki8(uint8_t data[4]){ + return (int8_t)data[0]; +} + +// read SDO value, if error - return INT64_MIN +int64_t SDO_read(const SDO_dic_entry *e, uint8_t NID){ + FNAME(); + SDO *sdo = readSDOvalue(e->index, e->subindex, NID); + if(!sdo){ + return INT64_MIN; + } + if(sdo->ccs == CCS_ABORT_TRANSFER){ // error + WARNX("Got error for SDO 0x%X", e->index); + uint32_t ac = mku32(sdo->data); + const char *etxt = abortcode_text(ac); + if(etxt) WARNX("Abort code 0x%X: %s", ac, etxt); + return INT64_MIN; + } + if(sdo->datalen != e->datasize){ + WARNX("Got SDO with length %d instead of %d (as in dictionary)", sdo->datalen, e->datasize); + } + int64_t ans = 0; + if(e->issigned){ + switch(sdo->datalen){ + case 1: + ans = mki8(sdo->data); + break; + case 4: + ans = mki32(sdo->data); + break; + default: // can't be 3! 3->2 + ans = mki16(sdo->data); + } + }else{ + switch(sdo->datalen){ + case 1: + ans = mku8(sdo->data); + break; + case 4: + ans = mku32(sdo->data); + break; + default: // can't be 3! 3->2 + ans = mku16(sdo->data); + } + } + return ans; +} + +// write SDO data, return 0 if all OK +int SDO_writeArr(const SDO_dic_entry *e, uint8_t NID, const uint8_t *data){ + FNAME(); + if(!e || !data || e->datasize < 1 || e->datasize > 4){ + WARNX("SDO_write(): bad datalen"); + return 1; + } + SDO sdo; + sdo.NID = NID; + sdo.ccs = CCS_INIT_DOWNLOAD; + sdo.datalen = e->datasize; + for(uint8_t i = 0; i < e->datasize; ++i) sdo.data[i] = data[i]; + sdo.index = e->index; + sdo.subindex = e->subindex; + CANmesg *mesgp = mkMesg(&sdo); + DBG("Canbus write.."); + if(canbus_write(mesgp)){ + WARNX("SDO_write(): Can't initiate download"); + return 2; + } + DBG("get answer"); + SDO *sdop = getSDOans(e->index, e->subindex, NID); + if(!sdop){ + WARNX("SDO_write(): SDO read error"); + return 3; + } + if(sdop->ccs == CCS_ABORT_TRANSFER){ // error + WARNX("SDO_write(): Got error for SDO 0x%X", e->index); + uint32_t ac = mku32(sdop->data); + const char *etxt = abortcode_text(ac); + if(etxt) WARNX("Abort code 0x%X: %s", ac, etxt); + return 4; + } + if(sdop->datalen != 0){ + WARNX("SDO_write(): got answer with non-zero length"); + return 5; + } + if(sdop->ccs != CCS_SEG_UPLOAD){ + WARNX("SDO_write(): got wrong answer"); + return 6; + } + return 0; +} + +int SDO_write(const SDO_dic_entry *e, uint8_t NID, int64_t data){ + if(!e) return 1; + uint8_t arr[4] = {0}; + uint32_t U; + int32_t I; + uint16_t U16; + int16_t I16; + if(e->issigned){ + switch(e->datasize){ + case 1: + arr[0] = (uint8_t) data; + break; + case 4: + I = (int32_t) data; + arr[0] = I&0xff; + arr[1] = (I>>8)&0xff; + arr[2] = (I>>16)&0xff; + arr[3] = (I>>24)&0xff; + break; + default: // can't be 3! 3->2 + I16 = (int16_t) data; + arr[0] = I16&0xff; + arr[1] = (I16>>8)&0xff; + } + }else{ + switch(e->datasize){ + case 1: + arr[0] = (uint8_t) data; + break; + case 4: + U = (uint32_t) data; + arr[0] = U&0xff; + arr[1] = (U>>8)&0xff; + arr[2] = (U>>16)&0xff; + arr[3] = (U>>24)&0xff; + break; + default: // can't be 3! 3->2 + U16 = (uint16_t) data; + arr[0] = U16&0xff; + arr[1] = (U16>>8)&0xff; + } + } + /* + DBG("DATA:"); + for(int i = 0; i < e->datasize; ++i) printf("0x%X ", arr[i]); + printf("\n"); + return 0;*/ + return SDO_writeArr(e, NID, arr); +} + + + +// read one byte of data +/*int SDO_readByte(uint16_t idx, uint8_t subidx, uint8_t *data, uint8_t NID){ + SDO *sdo = readSDOvalue(idx, subidx, NID); + if(!sdo || sdo->datalen != 1){ + WARNX("Got SDO with wrong data length: %d instead of 1", sdo->datalen); + return 1; + } + if(data) *data = sdo->data[0]; + return 0; +}*/ + +#if 0 +// write uint8_t to SDO with index idx & subindex subidx to NID +void SDO_writeU8(uint16_t idx, uint8_t subidx, uint8_t data, uint8_t NID){ + SDO sdo; + sdo.NID = NID; + sdo.ccs = CCS_INIT_DOWNLOAD; +} +#endif diff --git a/canserver/canopen.h b/canserver/canopen.h new file mode 100644 index 0000000..2c01c78 --- /dev/null +++ b/canserver/canopen.h @@ -0,0 +1,90 @@ +/* + * This file is part of the stepper project. + * Copyright 2020 Edward V. Emelianov . + * + * 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 . + */ + +#pragma once +#ifndef CANOPEN_H__ +#define CANOPEN_H__ + +#include "canbus.h" +#include "pusirobot.h" + +// timeout for answer from the SDO, seconds +#define SDO_ANS_TIMEOUT (1.) + +// COB-ID base: +#define NMT_COBID 0 +#define EMERG_COBID 0x80 +#define TIMESTAMP_COBID 0x100 +#define TPDO1_COBID 0x180 +#define RPDO1_COBID 0x200 +#define TPDO2_COBID 0x280 +#define RPDO2_COBID 0x300 +#define TPDO3_COBID 0x380 +#define RPDO3_COBID 0x400 +#define TPDO4_COBID 0x480 +#define RPDO4_COBID 0x500 +#define TSDO_COBID 0x580 +#define RSDO_COBID 0x600 +#define HEARTB_COBID 0x700 +// mask to select COB-ID base from ID +#define COBID_MASK 0x780 +// mask to select node ID from ID +#define NODEID_MASK 0x7F + +// SDO client command specifier field +typedef enum{ + CCS_SEG_DOWNLOAD = 0, + CCS_INIT_DOWNLOAD, + CCS_INIT_UPLOAD, + CCS_SEG_UPLOAD, + CCS_ABORT_TRANSFER, + CCS_BLOCK_UPLOAD, + CCS_BLOCK_DOWNLOAD +} SDO_CCS_fields; +// make number from CSS field +#define SDO_CCS(f) (f<<5) +// make CCS from number +#define GET_CCS(f) (f>>5) +// make number from data amount +#define SDO_N(n) ((4-n)<<2) +// make data amount from number +#define SDO_datalen(n) (4-((n&0xC)>>2)) +// SDO e & s fields: +#define SDO_E (1<<1) +#define SDO_S (1<<0) + +typedef struct{ + uint8_t NID; // node ID in CANopen + uint16_t index; // SDO index + uint8_t subindex; // SDO subindex + uint8_t data[4]; // 1..4 bytes of data (data[0] is lowest) + uint8_t datalen; // length of data + uint8_t ccs; // Client command specifier +} SDO; + +const char *abortcode_text(uint32_t abortcode); +SDO *parseSDO(CANmesg *mesg); +SDO *readSDOvalue(uint16_t idx, uint8_t subidx, uint8_t NID); + +int64_t SDO_read(const SDO_dic_entry *e, uint8_t NID); + +int SDO_writeArr(const SDO_dic_entry *e, uint8_t NID, const uint8_t *data); +int SDO_write(const SDO_dic_entry *e, uint8_t NID, int64_t data); + +//int SDO_readByte(uint16_t idx, uint8_t subidx, uint8_t *data, uint8_t NID); +#endif // CANOPEN_H__ diff --git a/canserver/cmdlnopts.c b/canserver/cmdlnopts.c index e97c3e3..5424021 100644 --- a/canserver/cmdlnopts.c +++ b/canserver/cmdlnopts.c @@ -89,84 +89,3 @@ glob_pars *parse_args(int argc, char **argv){ } return &G; } - -Cl_log *globlog = NULL; - -/** - * @brief Cl_createlog - create log file: init mutex, test file open ability - * @param logpath - path to log file - * @param level - lowest message level (e.g. LOGLEVEL_ERR won't allow to write warn/msg/dbg) - * @return allocated structure (should be free'd later by Cl_deletelog) or NULL - */ -Cl_log *Cl_createlog(const char *logpath, Cl_loglevel level){ - if(level < LOGLEVEL_NONE || level > LOGLEVEL_ANY) return NULL; - if(!logpath) return NULL; - FILE *logfd = fopen(logpath, "a"); - if(!logfd){ - WARN("Can't open log file"); - return NULL; - } - fclose(logfd); - Cl_log *log = MALLOC(Cl_log, 1); - if(pthread_mutex_init(&log->mutex, NULL)){ - WARN("Can't init log mutex"); - FREE(log); - return NULL; - } - log->logpath = strdup(logpath); - if(!log->logpath){ - WARN("strdup()"); - FREE(log); - return NULL; - } - log->loglevel = level; - return log; -} - -void Cl_deletelog(Cl_log **log){ - if(!log || !*log) return; - FREE((*log)->logpath); - FREE(*log); -} - -/** - * @brief Cl_putlog - put message to log file with/without timestamp - * @param timest - ==1 to put timestamp - * @param log - pointer to log structure - * @param lvl - message loglevel (if lvl > loglevel, message won't be printed) - * @param fmt - format and the rest part of message - * @return amount of symbols saved in file - */ -int Cl_putlogt(int timest, Cl_log *log, Cl_loglevel lvl, const char *fmt, ...){ - if(!log || !log->logpath) return 0; - if(lvl > log->loglevel) return 0; - if(pthread_mutex_lock(&log->mutex)){ - WARN("Can't lock log mutex"); - return 0; - } - int i = 0; - FILE *logfd = fopen(log->logpath, "a+"); - if(!logfd) goto rtn; - if(timest){ - char strtm[128]; - time_t t = time(NULL); - struct tm *curtm = localtime(&t); - strftime(strtm, 128, "%Y/%m/%d-%H:%M:%S", curtm); - i = fprintf(logfd, "%s", strtm); - } - i += fprintf(logfd, "\t"); - va_list ar; - va_start(ar, fmt); - i += vfprintf(logfd, fmt, ar); - va_end(ar); - fseek(logfd, -1, SEEK_CUR); - char c; - ssize_t r = fread(&c, 1, 1, logfd); - if(1 == r){ // add '\n' if there was no newline - if(c != '\n') i += fprintf(logfd, "\n"); - } - fclose(logfd); -rtn: - pthread_mutex_unlock(&log->mutex); - return i; -} diff --git a/canserver/cmdlnopts.h b/canserver/cmdlnopts.h index aba32e7..7c5a733 100644 --- a/canserver/cmdlnopts.h +++ b/canserver/cmdlnopts.h @@ -44,35 +44,6 @@ typedef struct{ glob_pars *parse_args(int argc, char **argv); -typedef enum{ - LOGLEVEL_NONE, // no logs - LOGLEVEL_ERR, // only errors - LOGLEVEL_WARN, // only warnings and errors - LOGLEVEL_MSG, // all without debug - LOGLEVEL_DBG, // all messages - LOGLEVEL_ANY // all shit -} Cl_loglevel; - -typedef struct{ - char *logpath; // full path to logfile - Cl_loglevel loglevel; // loglevel - pthread_mutex_t mutex; // log mutex -} Cl_log; - -// logging -extern Cl_log *globlog; // global log file -Cl_log *Cl_createlog(const char *logpath, Cl_loglevel level); -#define OPENLOG(nm, lvl) (globlog = Cl_createlog(nm, lvl)) -void Cl_deletelog(Cl_log **log); -int Cl_putlogt(int timest, Cl_log *log, Cl_loglevel lvl, const char *fmt, ...); -// shortcuts for different log levels; ..ADD - add message without timestamp -#define LOGERR(...) do{Cl_putlogt(1, globlog, LOGLEVEL_ERR, __VA_ARGS__);}while(0) -#define LOGERRADD(...) do{Cl_putlogt(1, globlog, LOGLEVEL_ERR, __VA_ARGS__);}while(0) -#define LOGWARN(...) do{Cl_putlogt(1, globlog, LOGLEVEL_WARN, __VA_ARGS__);}while(0) -#define LOGWARNADD(...) do{Cl_putlogt(0, globlog, LOGLEVEL_WARN, __VA_ARGS__);}while(0) -#define LOGMSG(...) do{Cl_putlogt(1, globlog, LOGLEVEL_MSG, __VA_ARGS__);}while(0) -#define LOGMSGADD(...) do{Cl_putlogt(0, globlog, LOGLEVEL_MSG, __VA_ARGS__);}while(0) -#define LOGDBG(...) do{Cl_putlogt(1, globlog, LOGLEVEL_DBG, __VA_ARGS__);}while(0) -#define LOGDBGADD(...) do{Cl_putlogt(0, globlog, LOGLEVEL_DBG, __VA_ARGS__);}while(0) +extern glob_pars *GP; #endif // __CMDLNOPTS_H__ diff --git a/canserver/dicentries.in b/canserver/dicentries.in new file mode 100644 index 0000000..50f57e7 --- /dev/null +++ b/canserver/dicentries.in @@ -0,0 +1,143 @@ +/* + * This file is part of the stepper project. + * Copyright 2020 Edward V. Emelianov . + * + * 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 . + */ + +// this file can be included more than once! + +// variable name / index / subindex / datasize / issigned / name + +// heartbeat time +DICENTRY(HEARTBTTIME, 0x1017, 0, 2, 0, "heartbeat time") + +// receive PDO parameter 0 +// largest subindex supported +DICENTRY(RPDOP0LS, 0x1400, 0, 1, 0, "receive PDO parameter 0, largest subindex supported") +// COB-ID used by PDO +DICENTRY(RPDOP0CI, 0x1400, 1, 4, 0, "receive PDO parameter 0, COB-ID used by PDO") +// transmission type +DICENTRY(RPDOP0TT, 0x1400, 2, 1, 0, "receive PDO parameter 0, transmission type") +// inhibit time +DICENTRY(RPDOP0IT, 0x1400, 3, 2, 0, "receive PDO parameter 0, inhibit time") +// compatibility entry +DICENTRY(RPDOP0CE, 0x1400, 4, 1, 0, "receive PDO parameter 0, compatibility entry") +// event timer +DICENTRY(RPDOP0ET, 0x1400, 5, 2, 0, "receive PDO parameter 0, event timer") + +// receive PDO mapping 0 +// number of mapped application objects +DICENTRY(RPDOM0N, 0x1600, 0, 1, 0, "receive PDO mapping 0, number of objects") +// first map +DICENTRY(RPDOM0O1, 0x1600, 1, 4, 0, "receive PDO mapping 0, mapping for 1st object") + +// transmit PDO parameter 0 +// largest subindex supported +DICENTRY(TPDOP0LS, 0x1800, 0, 1, 0, "transmit PDO parameter 0, largest subindex supported") +// COB-ID used by PDO +DICENTRY(TPDOP0CI, 0x1800, 1, 4, 0, "transmit PDO parameter 0, COB-ID used by PDO") +// transmission type +DICENTRY(TPDOP0TT, 0x1800, 2, 1, 0, "transmit PDO parameter 0, transmission type") +// inhibit time +DICENTRY(TPDOP0IT, 0x1800, 3, 2, 0, "transmit PDO parameter 0, inhibit time") +// reserved +DICENTRY(TPDOP0R, 0x1800, 4, 1, 0, "transmit PDO parameter 0, reserved") +// event timer +DICENTRY(TPDOP0ET, 0x1800, 5, 2, 0, "transmit PDO parameter 0, event timer") + +// transmit PDO mapping 0 +// number of mapped application objects +DICENTRY(TPDOM0N, 0x1A00, 0, 1, 0, "transmit PDO mapping 0, number of objects") +// first map +DICENTRY(TPDOM0O1, 0x1A00, 1, 4, 0, "transmit PDO mapping 0, mapping for 1st object") +DICENTRY(TPDOM0O2, 0x1A00, 2, 4, 0, "transmit PDO mapping 0, mapping for 2nd object") +DICENTRY(TPDOM0O3, 0x1A00, 3, 4, 0, "transmit PDO mapping 0, mapping for 3rd object") +DICENTRY(TPDOM0O4, 0x1A00, 4, 4, 0, "transmit PDO mapping 0, mapping for 4th object") +DICENTRY(TPDOM0O5, 0x1A00, 5, 4, 0, "transmit PDO mapping 0, mapping for 5th object") + +// node ID +DICENTRY(NODEID, 0x2002, 0, 1, 0, "node ID") +// baudrate +DICENTRY(BAUDRATE, 0x2003, 0, 1, 0, "baudrate") +// system control: 1- bootloader, 2 - save parameters, 3 - reset factory settings +DICENTRY(SYSCONTROL, 0x2007, 0, 1, 0, "system control: 1- bootloader, 2 - save parameters, 3 - reset factory settings") +// error status +DICENTRY(ERRSTATE, 0x6000, 0, 1, 0, "error status") +// controller status +DICENTRY(DEVSTATUS, 0x6001, 0, 1, 0, "controller status") +// rotation direction +DICENTRY(ROTDIR, 0x6002, 0, 1, 0, "rotation direction") +// maximal speed +DICENTRY(MAXSPEED, 0x6003, 0, 4, 1, "maximal speed") +// relative displacement +DICENTRY(RELSTEPS, 0x6004, 0, 4, 0, "relative displacement") +// operation mode +DICENTRY(OPMODE, 0x6005, 0, 1, 0, "operation mode") +// start speed +DICENTRY(STARTSPEED, 0x6006, 0, 2, 0, "start speed") +// stop speed +DICENTRY(STOPSPEED, 0x6007, 0, 2, 0, "stop speed") +// acceleration coefficient +DICENTRY(ACCELCOEF, 0x6008, 0, 1, 0, "acceleration coefficient") +// deceleration coefficient +DICENTRY(DECELCOEF, 0x6009, 0, 1, 0, "deceleration coefficient") +// microstepping +DICENTRY(MICROSTEPS, 0x600A, 0, 2, 0, "microstepping") +// max current +DICENTRY(MAXCURNT, 0x600B, 0, 2, 0, "maximum phase current") +// current position +DICENTRY(POSITION, 0x600C, 0, 4, 0, "current position") +// current reduction +DICENTRY(CURRREDUCT, 0x600D, 0, 1, 0, "current reduction") +// motor enable +DICENTRY(ENABLE, 0x600E, 0, 1, 0, "motor enable") +// EXT emergency stop enable +DICENTRY(EXTENABLE, 0x600F, 1, 1, 0, "EXT emergency stop enable") +// EXT emergency stop trigger mode +DICENTRY(EXTTRIGMODE, 0x600F, 2, 1, 0, "EXT emergency stop trigger mode") +// EXT emergency sensor type +DICENTRY(EXTSENSTYPE, 0x600F, 3, 1, 0, "EXT emergency sensor type") +// GPIO direction +DICENTRY(GPIODIR, 0x6011, 1, 2, 0, "GPIO direction") +// GPIO configuration +DICENTRY(GPIOCONF, 0x6011, 2, 4, 0, "GPIO configuration") +// GPIO value +DICENTRY(GPIOVAL, 0x6012, 0, 2, 0, "GPIO value") +// stall parameters +DICENTRY(STALLPARS, 0x6017, 0, 2, 0, "stall parameters (open loop)") +// offline operation +DICENTRY(OFFLNMBR, 0x6018, 1, 1, 0, "Number of offline programming command") +DICENTRY(OFFLENBL, 0x6018, 2, 1, 0, "Offline automatic operation enable") +// stall set +DICENTRY(STALLSET, 0x601B, 0, 1, 0, "stall set (open loop)") +// absolute displacement +DICENTRY(ABSSTEPS, 0x601C, 0, 4, 1, "absolute displacement") +// stop motor +DICENTRY(STOP, 0x6020, 0, 1, 0, "stop motor") +// encoder resolution +DICENTRY(ENCRESOL, 0x6021, 0, 2, 0, "encoder resolution (closed loop)") +// stall length parameter +DICENTRY(STALLLEN, 0x6028, 0, 2, 0, "stall length parameter (closed loop)") +// torque ring enable +DICENTRY(TORQRING, 0x6029, 0, 1, 0, "torque ring enable (closed loop)") +// autosave position +DICENTRY(POSAUTOSAVE, 0x602A, 0, 1, 0, "autosave position (closed loop)") +// real time speed +DICENTRY(REALTIMESPD, 0x6030, 0, 2, 1, "real time speed (closed loop)") +// calibration zero +DICENTRY(CALIBZERO, 0x6034, 0, 4, 1, "calibration zero") +// encoder position +DICENTRY(ENCPOS, 0x6035, 0, 4, 1, "encoder position") + diff --git a/canserver/main.c b/canserver/main.c index 5469068..a9ff608 100644 --- a/canserver/main.c +++ b/canserver/main.c @@ -54,7 +54,6 @@ int main(int argc, char **argv){ return parse_data_file(GP->checkfile, 0); }*/ char *dev = find_device(); - red("dev = %s\n", dev); if(!dev) ERRX("Serial device not found!"); FREE(dev); signal(SIGTERM, signals); // kill (-15) - quit @@ -69,7 +68,6 @@ int main(int argc, char **argv){ while(v--){ // increase loglevel if(++lvl == LOGLEVEL_ANY) break; } - green("Log level: %d\n", lvl); OPENLOG(GP->logfile, lvl); } #ifndef EBUG @@ -81,10 +79,10 @@ int main(int argc, char **argv){ while(1){ // guard for dead processes pid_t childpid = fork(); if(childpid){ - LOGDBG("create child with PID %d", childpid); + LOGDBG("Create child with PID %d", childpid); DBG("Created child with PID %d\n", childpid); wait(NULL); - LOGDBG("child %d died", childpid); + LOGDBG("Child %d died", childpid); WARNX("Child %d died\n", childpid); sleep(1); }else{ diff --git a/canserver/processmotors.c b/canserver/processmotors.c new file mode 100644 index 0000000..486e5cb --- /dev/null +++ b/canserver/processmotors.c @@ -0,0 +1,26 @@ +/* + * This file is part of the CANserver project. + * Copyright 2020 Edward V. Emelianov . + * + * 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 . + */ + +#include "aux.h" +#include "canbus.h" +#include "canopen.h" +#include "cmdlnopts.h" +#include "processmotors.h" +#include "pusirobot.h" + +#include diff --git a/canserver/processmotors.h b/canserver/processmotors.h new file mode 100644 index 0000000..9aafa60 --- /dev/null +++ b/canserver/processmotors.h @@ -0,0 +1,25 @@ +/* + * This file is part of the CANserver project. + * Copyright 2020 Edward V. Emelianov . + * + * 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 . + */ + +#pragma once +#ifndef PROCESSMOTORS_H__ +#define PROCESSMOTORS_H__ + + + +#endif // PROCESSMOTORS_H__ diff --git a/canserver/proto.c b/canserver/proto.c new file mode 100644 index 0000000..76ed608 --- /dev/null +++ b/canserver/proto.c @@ -0,0 +1,106 @@ +/* + * This file is part of the CANserver project. + * Copyright 2020 Edward V. Emelianov . + * + * 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 . + */ + +#include "aux.h" +#include "cmdlnopts.h" +#include "proto.h" + +#include +#include +#include + +/** + * @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); +} + +/* + * Commands format: + * function [[NAME] data] + * where: + * - function - function name (register, help, unregister, stop, absmove etc), + * - NAME - thread name, + * - 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); +} cmditem; + +// array with known functions +static cmditem functions[] = { + {"raw", sendraw}, + {NULL, NULL} +}; + +/** + * @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!!! + */ +char *processCommand(char *cmd){ + if(!cmd) return NULL; + char *saveptr = NULL, *fname = NULL, *procname = NULL, *data = NULL; + DBG("Got %s", cmd); + fname = strtok_r(cmd, " \t\r\n", &saveptr); + DBG("fname: %s", fname); + if(fname){ + procname = strtok_r(NULL, " \t\r\n", &saveptr); + DBG("procname: %s", procname); + if(procname){ + data = saveptr; + DBG("data: %s", data); + } + }else return NULL; + for(cmditem *item = functions; item->fname; ++item){ + if(0 == strcasecmp(item->fname, fname)) return item->handler(procname, data); + } + return strdup("Wrong command\n"); +} + +#if 0 +static char buf[1024]; +snprintf(buf, 1024, "FUNC=%s, PROC=%s, CMD=%s\n", fname, procname, data); +DBG("buf: %s", buf); +return buf; +#endif diff --git a/canserver/proto.h b/canserver/proto.h new file mode 100644 index 0000000..eec18d2 --- /dev/null +++ b/canserver/proto.h @@ -0,0 +1,25 @@ +/* + * This file is part of the CANserver project. + * Copyright 2020 Edward V. Emelianov . + * + * 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 . + */ + +#pragma once +#ifndef PROTO_H__ +#define PROTO_H__ + +char *processCommand(char *cmd); + +#endif // PROTO_H__ diff --git a/canserver/pusirobot.c b/canserver/pusirobot.c new file mode 100644 index 0000000..9e06e22 --- /dev/null +++ b/canserver/pusirobot.c @@ -0,0 +1,82 @@ +/* + * This file is part of the stepper project. + * Copyright 2020 Edward V. Emelianov . + * + * 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 . + */ + +#include // for NULL + +#include "pusirobot.h" + +// we should init constants here! +#undef DICENTRY +#define DICENTRY(name, idx, sidx, sz, s, n) const SDO_dic_entry name = {idx, sidx, sz, s, n}; +#include "dicentries.in" + +// now init array with all dictionary +#undef DICENTRY +#define nnn(nm) nm +#define lnk(nm) & ## nnn(nm) +#define DICENTRY(name, idx, sidx, sz, s, n) &name, +const SDO_dic_entry* allrecords[] = { +#include "dicentries.in" +}; +const int DEsz = sizeof(allrecords) / sizeof(SDO_dic_entry*); + +// controller status for bits +static const char *DevStatus[] = { + "External stop 1", + "External stop 2", + "Stall state", + "Busy state", + "External stop 3", + "The FIFO of PVT Mode 3 is empty", + "FIFO Lower bound of PVT Mode 3", + "FIFO upper limit of PVT mode 3" +}; + +// controller error statuses +static const char *DevError[] = { + "TSD, over temperature shutdown", + "AERR, coil A error", + "BERR, coil B error", + "AOC, A over current", + "BOC, B over current", + "UVLO, low voltage fault" +}; + +// return status message for given bit in status +const char *devstatus(uint8_t status, uint8_t bit){ + if(bit > 7) return NULL; + if(status & (1< 5) return NULL; + if(error & (1<index == index && entry->subindex == subindex) return (SDO_dic_entry*)entry; + } + return NULL; +} diff --git a/canserver/pusirobot.h b/canserver/pusirobot.h new file mode 100644 index 0000000..af22e22 --- /dev/null +++ b/canserver/pusirobot.h @@ -0,0 +1,55 @@ +/* + * This file is part of the stepper project. + * Copyright 2020 Edward V. Emelianov . + * + * 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 . + */ +#pragma once +#ifndef PUSIROBOT_H__ +#define PUSIROBOT_H__ + +#include + +// entry of SDO dictionary +typedef struct{ + uint16_t index; // SDO index + uint8_t subindex; // SDO subindex + uint8_t datasize; // data size: 1,2,3 or 4 bytes + uint8_t issigned; // signess: if issigned==1, then signed, else unsigned + const char *name; // dictionary entry name +} SDO_dic_entry; + +#ifndef DICENTRY +#define DICENTRY(name, idx, sidx, sz, s, n) extern const SDO_dic_entry name; +#endif + +#include "dicentries.in" + +extern const int DEsz; +extern const SDO_dic_entry* allrecords[]; + +#define MAX_SPEED_MIN -200000 +#define MAX_SPEED_MAX 200000 + +// limit switches mask in GPIO status register (x=1,2,3) +#define EXTMASK(x) (1<<(6+x)) + +#define EXTACTIVE(x, reg) ((reg&EXTMASK(x)) ? 1:0) + +// unclearable status +#define BUSY_STATE (1<<3) +const char *devstatus(uint8_t status, uint8_t bit); +const char *errname(uint8_t error, uint8_t bit); +SDO_dic_entry *dictentry_search(uint16_t index, uint8_t subindex); +#endif // PUSIROBOT_H__ diff --git a/canserver/socket.c b/canserver/socket.c index aacc497..9d6d0d3 100644 --- a/canserver/socket.c +++ b/canserver/socket.c @@ -18,6 +18,7 @@ #include "aux.h" #include "cmdlnopts.h" // glob_pars +#include "proto.h" #include "socket.h" #include "term.h" @@ -34,7 +35,8 @@ #include // daemon #include -#define BUFLEN (10240) +// buffer size for received data +#define BUFLEN (1024) // Max amount of connections #define BACKLOG (30) @@ -75,21 +77,21 @@ static int waittoread(int sock){ } /**************** SERVER FUNCTIONS ****************/ -pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER; +//pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER; /** * Send data over socket * @param sock - socket fd * @param textbuf - zero-trailing buffer with data to send - * @return 1 if all OK + * @return amount of sent bytes */ -static int send_data(int sock, char *textbuf){ +static size_t send_data(int sock, char *textbuf){ ssize_t Len = strlen(textbuf); if(Len != write(sock, textbuf, Len)){ WARN("write()"); LOGERR("send_data(): write() failed"); return 0; }else LOGDBG("send_data(): sent '%s'", textbuf); - return 1; + return (size_t)Len; } #if 0 @@ -109,17 +111,11 @@ static char* stringscan(char *str, char *needle){ #endif static void *handle_socket(void *asock){ - //putlog("handle_socket(): getpid: %d, pthread_self: %lu, tid: %lu",getpid(), pthread_self(), syscall(SYS_gettid)); FNAME(); int sock = *((int*)asock); char buff[BUFLEN]; ssize_t rd; - double t0 = dtime(); - /* - * INSERT CODE HERE - * change to while(1) if socket shouldn't be closed after data transmission - */ - while(dtime() - t0 < SOCKET_TIMEOUT){ + while(1){ if(!waittoread(sock)){ // no data incoming continue; } @@ -138,28 +134,23 @@ static void *handle_socket(void *asock){ 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); + DBG("user send '%s'", buff); + LOGDBG("user send '%s'", buff); if(GP->echo){ if(!send_data(sock, buff)){ - LOGWARN("Can't send data to user, some error occured"); + WARN("Can't send data to user, some error occured"); } } - pthread_mutex_lock(&mutex); - /* - * INSERT CODE HERE - * Process user commands here & send him an answer - */ - pthread_mutex_unlock(&mutex); - t0 = dtime(); - } - if(dtime() - t0 > SOCKET_TIMEOUT){ - LOGDBG("Socket %d closed on timeout", sock); - DBG("Closed on timeout"); - }else{ - LOGDBG("Socket %d closed", sock); - DBG("Socket closed"); + //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); } + LOGDBG("Socket %d closed", sock); + DBG("Socket closed"); close(sock); pthread_exit(NULL); return NULL; @@ -201,17 +192,17 @@ static void *server(void *asock){ // data gathering & socket management static void daemon_(int sock){ if(sock < 0) return; - pthread_t sock_thread; - if(pthread_create(&sock_thread, NULL, server, (void*) &sock)){ - LOGERR("daemon_(): pthread_create() failed"); - ERR("pthread_create()"); - } 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)){ + LOGERR("daemon_(): pthread_create() failed"); + ERR("pthread_create()"); + } do{ if(pthread_kill(sock_thread, 0) == ESRCH){ // died WARNX("Sockets thread died"); @@ -230,7 +221,7 @@ static void daemon_(int sock){ * Gather data (poll_device) */ // copy temporary buffers to main - pthread_mutex_lock(&mutex); + //pthread_mutex_lock(&mutex); int fd = open(devname, O_RDONLY); if(fd == -1){ WARN("open()"); @@ -250,7 +241,7 @@ static void daemon_(int sock){ * INSERT CODE HERE * fill global data buffers */ - pthread_mutex_unlock(&mutex); + //pthread_mutex_unlock(&mutex); }while(1); LOGERR("daemon_(): UNREACHABLE CODE REACHED!"); } diff --git a/canserver/threadlist.c b/canserver/threadlist.c new file mode 100644 index 0000000..22fa240 --- /dev/null +++ b/canserver/threadlist.c @@ -0,0 +1,324 @@ +/* + * This file is part of the CANserver project. + * Copyright 2020 Edward V. Emelianov . + * + * 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 . + */ + +#include "threadlist.h" + +#include +#include +#include +#include + +// global thread list +static threadlist *thelist = NULL; + +/** + * push data into the tail of a list (FIFO) + * @param lst (io) - list + * @param v (i) - data to push + * @return pointer to just pushed node + */ +static msglist *pushmessage(msglist **lst, char *v){ + if(!lst || !v) return NULL; + msglist *node; + if((node = MALLOC(msglist, 1)) == 0) + return NULL; // allocation error + node->data = strdup(v); + if(!node->data){ + FREE(node); + return NULL; + } + if(!*lst){ + *lst = node; + (*lst)->last = node; + return node; + } + (*lst)->last->next = node; + (*lst)->last = node; + return node; +} + +/** + * @brief popmessage - get data from head of list + * @param lst (io) - list + * @return data from first node or NULL if absent (SHOULD BE FREEd AFER USAGE!) + */ +static char *popmessage(msglist **lst){ + if(!lst || !*lst) return NULL; + char *ret; + msglist *node = *lst; + ret = (*lst)->data; + *lst = (*lst)->next; + FREE(node); + return ret; +} + +/** + * @brief getlast - find last element in thread list + * @return pointer to the last element or NULL if list is empty + */ +static threadlist *getlast(){ + if(!thelist) return NULL; + threadlist *t = thelist; + while(t->next) t = t->next; + return t; +} + +/** + * @brief findThreadByName - find thread by name + * @param name - thread name + * @return thread found or NULL if absent + */ +threadinfo *findThreadByName(char *name){ + if(!name) return NULL; + if(!thelist) return NULL; // thread list is empty + size_t l = strlen(name); + if(l < 1 || l > THREADNAMEMAXLEN) return NULL; + DBG("Try to find thread '%s'", name); + threadlist *lptr = thelist; + while(lptr){ + DBG("Check '%s'", lptr->ti.name); + if(strcmp(lptr->ti.name, name) == 0) return &lptr->ti; + lptr = lptr->next; + } + return NULL; +} + +/** + * @brief findThreadByID - find thread by numeric ID + * @param ID - thread ID + * @return thread found or NULL if absent + */ +threadinfo *findThreadByID(int ID){ + if(!thelist) return NULL; // thread list is empty + DBG("Try to find thread with ID=%d", ID); + threadlist *lptr = thelist; + while(lptr){ + DBG("Check %d", lptr->ti.ID); + if(ID == lptr->ti.ID) return &lptr->ti; + lptr = lptr->next; + } + return NULL; +} + +/** + * @brief addmesg - add message to thread's queue + * @param idx - index (MOSI/MISO) + * @param msg - message itself + * @param txt - data to add + * @return data added or NULL if failed + */ +char *addmesg(msgidx idx, message *msg, char *txt){ + if(idx < 0 || idx >= idxNUM){ + WARNX("Wrong message index"); + return NULL; + } + if(!msg) return NULL; + size_t L = strlen(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; + pthread_mutex_unlock(&msg->mutex[idx]); + return msg->text[idx]->data; +} + +/** + * @brief getmesg - get first message from queue (allocates data, should be free'd after usage!) + * @param idx - index (MOSI/MISO) + * @param msg - message itself + * @return data or NULL if empty + */ +char *getmesg(msgidx idx, message *msg){ + if(idx < 0 || idx >= idxNUM){ + WARNX("Wrong message index"); + return NULL; + } + if(!msg) return NULL; + char *text = NULL; + if(pthread_mutex_lock(&msg->mutex[idx])) return NULL; + text = popmessage(&msg->text[idx]); + pthread_mutex_unlock(&msg->mutex[idx]); + return text; +} + +/** + * @brief registerThread - register new thread + * @param name - thread name + * @param ID - thread numeric ID + * @param handler - thread handler + * @return pointer to new threadinfo struct or NULL if failed + */ +threadinfo *registerThread(char *name, int ID, void *(*handler)(void *)){ + if(!name || strlen(name) < 1 || !handler) return NULL; + threadinfo *ti = findThreadByName(name); + DBG("Register new thread with name '%s' and ID=%d", name, ID); + if(ti){ + WARNX("Thread named '%s' exists!", name); + return NULL; + } + ti = findThreadByID(ID); + if(ti){ + WARNX("Thread with ID=%d exists!", ID); + return NULL; + } + if(!thelist){ // the first element + thelist = MALLOC(threadlist, 1); + ti = &thelist->ti; + }else{ + threadlist *last = getlast(); + last->next = MALLOC(threadlist, 1); + ti = &last->next->ti; + } + ti->handler = handler; + snprintf(ti->name, THREADNAMEMAXLEN+1, "%s", name); + ti->ID = ID; + memset(&ti->mesg, 0, sizeof(ti->mesg)); + for(int i = 0; i < 2; ++i) + pthread_mutex_init(&ti->mesg.mutex[i], NULL); + if(pthread_create(&ti->thread, NULL, handler, (void*)ti)){ + WARN("pthread_create()"); + return NULL; + } + return ti; +} + +/** + * @brief killThread - kill and unregister thread with given name + * @param name - thread's name + * @return 0 if all OK + */ +int killThread(const char *name){ + if(!name || !thelist) return 1; + threadlist *lptr = thelist, *prev = NULL; + for(; lptr; lptr = lptr->next){ + if(strcmp(lptr->ti.name, 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 2; // not found +} + +#if 0 +static void *handler(void *data){ + threadinfo *ti = (threadinfo*)data; + while(1){ + char *got = getmesg(idxMOSI, &ti->mesg); + if(got){ + green("%s got: %s\n", ti->name, got); + FREE(got); + addmesg(idxMISO, &ti->mesg, "received"); + addmesg(idxMISO, &ti->mesg, "need more"); + } + usleep(100); + } + return NULL; +} + +static void dividemessages(message *msg, char *longtext){ + char *copy = strdup(longtext), *saveptr = NULL; + for(char *s = copy; ; s = NULL){ + char *nxt = strtok_r(s, " ", &saveptr); + if(!nxt) break; + addmesg(idxMOSI, msg, nxt); + } + FREE(copy); +} + +static void procmesg(char *text){ + if(!text) return; + char *nxt = strchr(text, ' '); + if(!nxt){ + WARNX("Usage: cmd data, where cmd:\n" + "\tnew threadname - create thread\n" + "\tdel threadname - delete thread\n" + "\tsend threadname data - send data to thread\n" + "\tsend all data - send data to all\n"); + return; + } + *nxt++ = 0; + if(strcasecmp(text, "new") == 0){ + registerThread(nxt, handler); + }else if(strcasecmp(text, "del") == 0){ + if(killThread(nxt)) WARNX("Can't delete '%s'", nxt); + }else if(strcasecmp(text, "send") == 0){ + text = strchr(nxt, ' '); + if(!text){ + WARNX("send all/threadname data"); + return; + } + *text++ = 0; + if(strcasecmp(nxt, "all") == 0){ // bcast + threadlist *lptr = thelist; + while(lptr){ + threadinfo *ti = &lptr->ti; + lptr = lptr->next; + green("Bcast send '%s' to thread '%s'\n", text, ti->name); + dividemessages(&ti->mesg, text); + } + }else{ // single + threadinfo *ti = findthread(nxt); + if(!ti){ + WARNX("Thread '%s' not found", nxt); + return; + } + green("Send '%s' to thread '%s'\n", text, nxt); + dividemessages(&ti->mesg, text); + } + } +} + +int main(){ + using_history(); + while(1){ + threadlist *lptr = thelist; + while(lptr){ + threadinfo *ti = &lptr->ti; + lptr = lptr->next; + char *got; + while((got = getmesg(idxMISO, &ti->mesg))){ + red("got from '%s': %s\n", ti->name, got); + fflush(stdout); + FREE(got); + } + } + char *text = readline("mesg > "); + if(!text) break; // ^D + if(strlen(text) < 1) continue; + add_history(text); + procmesg(text); + FREE(text); + } + return 0; +} + +#endif + diff --git a/canserver/threadlist.h b/canserver/threadlist.h new file mode 100644 index 0000000..b40aedc --- /dev/null +++ b/canserver/threadlist.h @@ -0,0 +1,68 @@ +/* + * This file is part of the CANserver project. + * Copyright 2020 Edward V. Emelianov . + * + * 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 . + */ + +#pragma once +#ifndef THREADLIST_H__ +#define THREADLIST_H__ + +#include + +// max length (in symbols) of thread name (any zero-terminated string) +#define THREADNAMEMAXLEN (31) + +// messages FIFO +typedef struct msglist_{ + char *data; // message itself + struct msglist_ *next, *last; // other elements of list +} msglist; + +typedef enum{ + idxMOSI = 0, // master out, slave in + idxMISO = 1, // master in, slave out + idxNUM = 2 // amount of indexes +} msgidx; + +// interthread messages; index 0 - MOSI, index 1 - MISO +typedef struct{ + msglist *text[idxNUM]; // stringified text messages + pthread_mutex_t mutex[idxNUM]; // text changing mutex +} message; + +// thread information +typedef struct{ + char name[THREADNAMEMAXLEN+1]; // thread name + int ID; // numeric ID (canopen ID) + message mesg; // inter-thread messages + pthread_t thread; // thread descriptor + void *(*handler)(void *); // handler function +} threadinfo; + +// list of threads member +typedef struct thread_list_{ + threadinfo ti; // base thread information + struct thread_list_ *next; // next element +} threadlist; + +threadinfo *findThreadByName(char *name); +threadinfo *findThreadByID(int ID); +threadinfo *registerThread(char *name, int ID, void *(*handler)(void *)); +int killThread(const char *name); +char *getmesg(msgidx idx, message *msg); +char *addmesg(msgidx idx, message *msg, char *txt); + +#endif // THREADLIST_H__