eddy/pusirobot
1
0
Fork 0
mirror of https://github.com/eddyem/pusirobot.git synced 2025-12-06 02:25:10 +03:00
Code Issues Packages Projects Releases Wiki Activity

processed

Browse Source
This commit is contained in:
Edward Emelianov 2020-11-12 17:42:54 +03:00
parent d311e1def3
commit 6f9c74f166
19 changed files with 1807 additions and 152 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

96
canserver/aux.c
View File

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

34
canserver/aux.h
View File

@ -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__

278
canserver/canbus.c Normal file
View File

@ -0,0 +1,278 @@
/*
* This file is part of the stepper project.
* Copyright 2020 Edward V. Emelianov <edward.emelianoff@gmail.com>.
*
* 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/>.
*/
#include <pthread.h>
#include <stdio.h>
#include <string.h>
#include <sys/select.h>
#include <usefull_macros.h>
#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;
}

48
canserver/canbus.h Normal file
View File

@ -0,0 +1,48 @@
/*
* This file is part of the stepper project.
* Copyright 2020 Edward V. Emelianov <edward.emelianoff@gmail.com>.
*
* 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/>.
*/
#pragma once
#ifndef CANBUS_H__
#define CANBUS_H__
#include <stdint.h>
#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__

378
canserver/canopen.c Normal file
View File

@ -0,0 +1,378 @@
/*
* This file is part of the stepper project.
* Copyright 2020 Edward V. Emelianov <edward.emelianoff@gmail.com>.
*
* 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/>.
*/
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <usefull_macros.h>
#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

90
canserver/canopen.h Normal file
View File

@ -0,0 +1,90 @@
/*
* This file is part of the stepper project.
* Copyright 2020 Edward V. Emelianov <edward.emelianoff@gmail.com>.
*
* 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/>.
*/
#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__

81
canserver/cmdlnopts.c
View File

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

31
canserver/cmdlnopts.h
View File

@ -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__

143
canserver/dicentries.in Normal file
View File

@ -0,0 +1,143 @@
/*
* This file is part of the stepper project.
* Copyright 2020 Edward V. Emelianov <edward.emelianoff@gmail.com>.
*
* 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/>.
*/
// 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")

6
canserver/main.c
View File

@ -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{

26
canserver/processmotors.c Normal file
View File

@ -0,0 +1,26 @@
/*
* This file is part of the CANserver project.
* Copyright 2020 Edward V. Emelianov <edward.emelianoff@gmail.com>.
*
* 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/>.
*/
#include "aux.h"
#include "canbus.h"
#include "canopen.h"
#include "cmdlnopts.h"
#include "processmotors.h"
#include "pusirobot.h"
#include <usefull_macros.h>

25
canserver/processmotors.h Normal file
View File

@ -0,0 +1,25 @@
/*
* This file is part of the CANserver project.
* Copyright 2020 Edward V. Emelianov <edward.emelianoff@gmail.com>.
*
* 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/>.
*/
#pragma once
#ifndef PROCESSMOTORS_H__
#define PROCESSMOTORS_H__
#endif // PROCESSMOTORS_H__

106
canserver/proto.c Normal file
View File

@ -0,0 +1,106 @@
/*
* This file is part of the CANserver project.
* Copyright 2020 Edward V. Emelianov <edward.emelianoff@gmail.com>.
*
* 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/>.
*/
#include "aux.h"
#include "cmdlnopts.h"
#include "proto.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);
}
/*
* 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

25
canserver/proto.h Normal file
View File

@ -0,0 +1,25 @@
/*
* This file is part of the CANserver project.
* Copyright 2020 Edward V. Emelianov <edward.emelianoff@gmail.com>.
*
* 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/>.
*/
#pragma once
#ifndef PROTO_H__
#define PROTO_H__
char *processCommand(char *cmd);
#endif // PROTO_H__

82
canserver/pusirobot.c Normal file
View File

@ -0,0 +1,82 @@
/*
* This file is part of the stepper project.
* Copyright 2020 Edward V. Emelianov <edward.emelianoff@gmail.com>.
*
* 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/>.
*/
#include <stdlib.h> // 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<<bit)) return DevStatus[bit];
return NULL;
}
// error codes explanation
const char *errname(uint8_t error, uint8_t bit){
if(bit > 5) return NULL;
if(error & (1<<bit)) return DevError[bit];
return NULL;
}
// search if the object exists in dictionary - for config file parser
SDO_dic_entry *dictentry_search(uint16_t index, uint8_t subindex){
// the search is linear as dictionary can be unsorted!!!
for(int i = 0; i < DEsz; ++i){
const SDO_dic_entry *entry = allrecords[i];
if(entry->index == index && entry->subindex == subindex) return (SDO_dic_entry*)entry;
}
return NULL;
}

55
canserver/pusirobot.h Normal file
View File

@ -0,0 +1,55 @@
/*
* This file is part of the stepper project.
* Copyright 2020 Edward V. Emelianov <edward.emelianoff@gmail.com>.
*
* 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/>.
*/
#pragma once
#ifndef PUSIROBOT_H__
#define PUSIROBOT_H__
#include <stdint.h>
// 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__

59
canserver/socket.c
View File

@ -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 <unistd.h> // daemon
#include <usefull_macros.h>
#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();
//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);
}
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");
}
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!");
}

324
canserver/threadlist.c Normal file
View File

@ -0,0 +1,324 @@
/*
* This file is part of the CANserver project.
* Copyright 2020 Edward V. Emelianov <edward.emelianoff@gmail.com>.
*
* 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/>.
*/
#include "threadlist.h"
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <usefull_macros.h>
// 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

68
canserver/threadlist.h Normal file
View File

@ -0,0 +1,68 @@
/*
* This file is part of the CANserver project.
* Copyright 2020 Edward V. Emelianov <edward.emelianoff@gmail.com>.
*
* 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/>.
*/
#pragma once
#ifndef THREADLIST_H__
#define THREADLIST_H__
#include <pthread.h>
// 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__