From 0a312fd010804b088ed50abf3716067ca379c8a8 Mon Sep 17 00:00:00 2001 From: Edward Emelianov Date: Fri, 20 Nov 2020 16:41:49 +0300 Subject: [PATCH] Add working simplest stepper motor; TODO: linear stage and turret --- canserver/4tests.txt | 90 ++++++++++++++- canserver/canbus.c | 2 - canserver/canopen.c | 61 ++++++---- canserver/cmdlnopts.c | 1 + canserver/cmdlnopts.h | 1 + canserver/dicentries.in | 2 +- canserver/main.c | 4 + canserver/processmotors.c | 229 ++++++++++++++++++++++++++++---------- canserver/processmotors.h | 2 +- canserver/proto.c | 12 +- canserver/threadlist.c | 4 +- 11 files changed, 311 insertions(+), 97 deletions(-) diff --git a/canserver/4tests.txt b/canserver/4tests.txt index f3ac269..4eee2c7 100644 --- a/canserver/4tests.txt +++ b/canserver/4tests.txt @@ -11,10 +11,96 @@ register new 3333 emulation #stepper register x 0x58a stepper +mesg x stop +mesg x status +mesg x info +mesg x absmove 3200 +mesg x relmove 3200 +mesg x relmove -3200 +mesg x enable 0 +=================== +help +help> help - show help +help> list - list all threads +help> mesg NAME MESG - send message `MESG` to thread `NAME` +help> register NAME ID ROLE - register new thread with `NAME`, raw receiving `ID` running thread `ROLE` +help> threads - list all possible threads with their message format +help> unregister NAME - kill thread `NAME` -#define getSDOe(SDO, fn, e) do{if(INT64_MIN != (i64 = SDO_read(&SDO, ID))) fn(i64); else ERRX(e);}while(0) +register x 0x58a stepper +OK +x maxspeed=OK -getSDOe(MICROSTEPS, setusteps, "Can't get microstepping"); +register y 0x58b stepper +OK +y maxspeed=OK + +list +thread> name='x' role='stepper' ID=0x58A +thread> name='y' role='stepper' ID=0x58B +thread> Send message 'help' to threads marked with (args) to get commands list + +threads +role> canopen NodeID index subindex [data] - raw CANOpen commands with `index` and `subindex` to `NodeID` +role> emulation (list) - stepper emulation +role> raw ID [DATA] - raw CANbus commands to raw `ID` with `DATA` +role> stepper (list) - simple stepper motor: no limit switches, only goto + +mesg x help +OK +x> COMMAND NARGS MEANING +x> stop 0 stop motor and clear errors +x> status 0 get current position and status +x> relmove 1 relative move +x> absmove 1 absolute move to position arg +x> enable 1 enable (!0) or disable (0) motor +x> setzero 0 set current position as zero +x> maxspeed 1 set/get maxspeed (get: arg==0) +x> info 0 get motor information + +mesg x info +OK +x errstatus=0 +x devstatus=0 +x curpos=0 +x enable=1 +x microsteps=32 +x extenable=0 +x maxspeed=3200 +x maxcurnt=600 +x gpioval=8191 +x rotdir=1 +x relsteps=0 +x abssteps=0 + +mesg x absmove 3200 +OK +x abssteps=OK +mesg x status +OK +x devstatus=0 +x curpos=3200 +x errstatus=0 + +mesg x relmove 1000 +OK +x abortcode='0x8000022' error='Data cannot be transferred or stored to the application because of the present device state' +x abortcode='0x8000022' error='Data cannot be transferred or stored to the application because of the present device state' + +второй: +x> COMMAND NARGS MEANING +x> stop 0 stop motor and clear errors +x> status 0 get current position and status +x> relmove 1 relative move +x> absmove 1 absolute move to position arg +x> enable 1 enable (!0) or disable (0) motor +x> setzero 0 set current position as zero +x> maxspeed 1 set/get maxspeed (get: arg==0) +x> info 0 get motor information + +x abssteps=OK +x rotdir=OK +x relsteps=OK diff --git a/canserver/canbus.c b/canserver/canbus.c index dea4426..e0d5aa0 100644 --- a/canserver/canbus.c +++ b/canserver/canbus.c @@ -90,7 +90,6 @@ static int read_ttyX(TTY_descr *d){ // thread-safe writing, add trailing '\n' static int ttyWR(const char *buff, int len){ - FNAME(); if(disconnected) return 1; pthread_mutex_lock(&mutex); //canbus_clear(); @@ -168,7 +167,6 @@ int canbus_setspeed(int speed){ * @return 0 if all OK */ int canbus_write(CANmesg *mesg){ - FNAME(); if(disconnected) return 1; char buf[BUFLEN]; if(!mesg || mesg->len > 8) return 1; diff --git a/canserver/canopen.c b/canserver/canopen.c index de44690..fff084b 100644 --- a/canserver/canopen.c +++ b/canserver/canopen.c @@ -96,6 +96,10 @@ static const abortcodes *abortcode_search(uint32_t abortcode){ //, int *n){ */ CANmesg *mkMesg(SDO *sdo, CANmesg *mesg){ if(!sdo || !mesg) return NULL; + /*DBG("Got SDO NID=%d, idx=0x%x, subidx=%d, len=%d, data={%x %x %x %x}", + sdo->NID, sdo->index, sdo->subindex, sdo->datalen, + sdo->data[0], sdo->data[1], sdo->data[2], sdo->data[3]);*/ + memset(mesg, 0, sizeof(CANmesg)); mesg->ID = RSDO_COBID + sdo->NID; mesg->len = 8; memset(mesg->data, 0, 8); @@ -107,6 +111,11 @@ CANmesg *mkMesg(SDO *sdo, CANmesg *mesg){ mesg->data[1] = sdo->index & 0xff; // l mesg->data[2] = (sdo->index >> 8) & 0xff; // h mesg->data[3] = sdo->subindex; +#if 0 + DBG("MESG: ID=0x%X, data=", mesg->ID); + for(int x = 0; x < 8; ++x) fprintf(stderr, "0x%02X ", mesg->data[x]); + fprintf(stderr, "\n"); +#endif return mesg; } @@ -136,19 +145,22 @@ SDO *parseSDO(const CANmesg *mesg, SDO *sdo){ 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 < 4; ++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); + DBG("Got TSDO from NID=%d, ccs=%u, index=0x%X, subindex=0x%X, datalen=%d, data=[%x %x %x %x]", + sdo->NID, sdo->ccs, sdo->index, sdo->subindex, sdo->datalen, + sdo->data[0], sdo->data[1], sdo->data[2], sdo->data[3]); return sdo; } +#if 0 // 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; + SDO sdo = { + .NID = NID, + .ccs = CCS_INIT_UPLOAD, + .datalen = 0, + .index = idx, + .subindex = subidx + }; CANmesg mesg; return canbus_write(mkMesg(&sdo, &mesg)); } @@ -192,6 +204,7 @@ SDO *readSDOvalue(uint16_t idx, uint8_t subidx, uint8_t NID, SDO *sdo){ } return getSDOans(idx, subidx, NID, sdo); } +#endif // mk... are all used static inline uint32_t mku32(const uint8_t data[4]){ @@ -282,10 +295,6 @@ CANmesg *SDO_read(const SDO_dic_entry *e, uint8_t NID, CANmesg *cm){ .index = e->index, .subindex = e->subindex }; - /*sdo.NID = NID; - sdo.ccs = CCS_INIT_UPLOAD; - sdo.index = e->index; - sdo.subindex = e->subindex;*/ return mkMesg(&sdo, cm); } @@ -303,12 +312,13 @@ CANmesg *SDO_write(const SDO_dic_entry *e, uint8_t NID, int64_t data, CANmesg *c int32_t I; uint16_t U16; int16_t I16; - SDO sdo; - sdo.NID = NID; - sdo.ccs = CCS_INIT_DOWNLOAD; - sdo.datalen = e->datasize; - sdo.index = e->index; - sdo.subindex = e->subindex; + SDO sdo ={ + .NID = NID, + .ccs = CCS_INIT_DOWNLOAD, + .datalen = e->datasize, + .index = e->index, + .subindex = e->subindex + }; DBG("datalen=%d, signed=%d", e->datasize, e->issigned); if(e->issigned){ switch(e->datasize){ @@ -349,6 +359,7 @@ CANmesg *SDO_write(const SDO_dic_entry *e, uint8_t NID, int64_t data, CANmesg *c return cm; } +#if 0 // write SDO data, return 0 if all OK int SDO_writeArr(const SDO_dic_entry *e, uint8_t NID, const uint8_t *data){ FNAME(); @@ -356,13 +367,14 @@ int SDO_writeArr(const SDO_dic_entry *e, uint8_t NID, const uint8_t *data){ WARNX("SDO_write(): bad datalen"); return 1; } - SDO sdo; - sdo.NID = NID; - sdo.ccs = CCS_INIT_DOWNLOAD; - sdo.datalen = e->datasize; + SDO sdo = { + .NID = NID, + .ccs = CCS_INIT_DOWNLOAD, + .datalen = e->datasize, + .index = e->index, + .subindex = e->subindex + }; 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, &mesgp); DBG("Canbus write.."); @@ -393,3 +405,4 @@ int SDO_writeArr(const SDO_dic_entry *e, uint8_t NID, const uint8_t *data){ } return 0; } +#endif diff --git a/canserver/cmdlnopts.c b/canserver/cmdlnopts.c index 232a366..f2f7956 100644 --- a/canserver/cmdlnopts.c +++ b/canserver/cmdlnopts.c @@ -64,6 +64,7 @@ myoption cmdlnopts[] = { {"echo", NO_ARGS, NULL, 'e', arg_int, APTR(&G.echo), _("echo users commands back")}, {"pidfile", NEED_ARG, NULL, 0, arg_string, APTR(&G.pidfile), _("name of PID file (default: " DEFAULT_PIDFILE ")")}, {"verbose", NO_ARGS, NULL, 'v', arg_none, APTR(&G.verb), _("increase verbosity level of log file (each -v increased by 1)")}, + {"speed", NEED_ARG, NULL, 's', arg_int, APTR(&G.speed), _("set CANbus speed")}, end_option }; diff --git a/canserver/cmdlnopts.h b/canserver/cmdlnopts.h index 7c5a733..f89d32a 100644 --- a/canserver/cmdlnopts.h +++ b/canserver/cmdlnopts.h @@ -34,6 +34,7 @@ typedef struct{ char *pid; // product id char *port; // port to connect char *logfile; // logfile name + int speed; // CANbus speed int verb; // increase logfile verbosity level int terminal; // run as terminal int echo; // echo user commands back diff --git a/canserver/dicentries.in b/canserver/dicentries.in index 6e8a9a4..29147e3 100644 --- a/canserver/dicentries.in +++ b/canserver/dicentries.in @@ -53,7 +53,7 @@ DICENTRY(MICROSTEPS, 0x600A, 0, 2, 0, "microstepping", "microsteps") // max current DICENTRY(MAXCURNT, 0x600B, 0, 2, 0, "maximum phase current", "maxcurnt") // current position -DICENTRY(POSITION, 0x600C, 0, 4, 0, "current position", "curpos") +DICENTRY(POSITION, 0x600C, 0, 4, 1, "current position", "curpos") // current reduction DICENTRY(CURRREDUCT, 0x600D, 0, 1, 0, "current reduction", "curred") // motor enable diff --git a/canserver/main.c b/canserver/main.c index 3bb962e..748344f 100644 --- a/canserver/main.c +++ b/canserver/main.c @@ -28,6 +28,7 @@ #include "aux.h" #include "cmdlnopts.h" #include "socket.h" +#include "processmotors.h" glob_pars *GP; // non-static: to use in outhern functions @@ -56,6 +57,9 @@ int main(int argc, char **argv){ char *dev = find_device(); if(!dev) ERRX("Serial device not found!"); FREE(dev); + if(!GP->speed) ERRX("Point CANbus speed"); + if(GP->speed < 10 || GP->speed > 3000) ERRX("Wrong CANbus speed value: %d, shold be 10..3000", GP->speed); + setCANspeed(GP->speed); signal(SIGTERM, signals); // kill (-15) - quit signal(SIGHUP, SIG_IGN); // hup - ignore signal(SIGINT, signals); // ctrl+C - quit diff --git a/canserver/processmotors.c b/canserver/processmotors.c index 2701062..7f90d4d 100644 --- a/canserver/processmotors.c +++ b/canserver/processmotors.c @@ -31,6 +31,9 @@ #include // usleep #include +// command to show help +#define HELPCMD "help" + static int CANspeed = 0; // default speed, if !=0 set it when connected // all messages are in format "ID [data]" @@ -57,9 +60,9 @@ static void *simplestp(void *arg); // handlers for standard types thread_handler CANhandlers[] = { {"canopen", canopencmds, "NodeID index subindex [data] - raw CANOpen commands with `index` and `subindex` to `NodeID`"}, - {"emulation", stpemulator, "(list) - stepper emulation"}, + {"emulation", stpemulator, "(args) - stepper emulation"}, {"raw", rawcommands, "ID [DATA] - raw CANbus commands to raw `ID` with `DATA`"}, - {"stepper", simplestp, "(list) - simple stepper motor: no limit switches, only goto"}, + {"stepper", simplestp, "(args) - simple stepper motor: no limit switches, only goto"}, {NULL, NULL, NULL} }; @@ -71,33 +74,46 @@ thread_handler *get_handler(const char *name){ return NULL; } +// bad arguments number +#define CMDPAR_ERR_BADNUMB (-1) +// command not found +#define CMDPAR_ERR_NOTFOUND (-2) +// just show help +#define CMDPAR_ERR_SHOWHELP (-3) +// clear errors +#define CMDPAR_CLEARERR (-4) + /** * @brief cmdParser - parser of user's comands * @param cmdlist - NULL-terminated array with possible commands - * @param cmd - user's command (default command `list` shows all names/descriptions) - * @return index of command in `cmdlist` or -1 if something wrong or not found + * @param s - user's command (default command HELPCMD shows all names/descriptions) + * argument `s` will be broken to tokens + * @param thrname - thread name (for help message) + * @return index of command in `cmdlist`, or negative errcode */ -static int cmdParser(threadCmd *cmdlist, char *s){ +static int cmdParser(const threadCmd *cmdlist, char *s, const char *thrname){ if(!cmdlist || !s) return -1; char *saveptr, *command; int idx = 0; command = strtok_r(s, " \t,;\r\n", &saveptr); if(!command) return -1; - if(strcmp(command, "list") == 0){ // just show help + if(strcmp(command, HELPCMD) == 0){ // just show help char buf[128]; + snprintf(buf, 128, "%s> COMMAND NARGS MEANING", thrname); + mesgAddText(&ServerMessages, buf); while(cmdlist->name){ - snprintf(buf, 128, "params> %s (%d arguments) - %s", - cmdlist->name, cmdlist->nargs, cmdlist->descr); + snprintf(buf, 128, "%s> %-12s%-6d%s", + thrname, cmdlist->name, cmdlist->nargs, cmdlist->descr); mesgAddText(&ServerMessages, buf); ++cmdlist; } - return 10000; + return CMDPAR_ERR_SHOWHELP; } while(cmdlist->name){ if(strcmp(cmdlist->name, command) == 0) break; ++idx; ++cmdlist; } - if(!cmdlist->name) return -1; // command not found + if(!cmdlist->name) return CMDPAR_ERR_NOTFOUND; // command not found if(cmdlist->nargs == 0) return idx; // simple command int nargsplus1 = cmdlist->nargs + 1, N = 0; long *args = MALLOC(long, nargsplus1); @@ -106,9 +122,13 @@ static int cmdParser(threadCmd *cmdlist, char *s){ if(!nxt) break; if(str2long(nxt, &args[N])) break; } + if(!cmdlist->args){ + WARNX("NULL instead of pointer to received data"); + return CMDPAR_ERR_BADNUMB; + } if(N != cmdlist->nargs){ FREE(args); - return -1; // bad arguments number + return CMDPAR_ERR_BADNUMB; // bad arguments number } for(int i = 0; i < N; ++i) cmdlist->args[i] = args[i]; @@ -198,10 +218,9 @@ void *CANserver(_U_ void *data){ } FREE(msg); } - usleep(1000); CANmesg cm = {0}; - if(!canbus_read(&cm)){ // got raw message from CAN bus - parce it - DBG("Got CAN message from %d, len: %d", cm.ID, cm.len); + if(!canbus_read(&cm)){ // got raw message from CAN bus - parse it + DBG("Got CAN message from 0x%03X, len: %d", cm.ID, cm.len); processCANmessage(&cm); }else if(canbus_disconnected()) reopen_device(); } @@ -363,6 +382,115 @@ static void chkSDO(const SDO *sdo, const char *thrname){ mesgAddText(&ServerMessages, buf); } +// parser of base stepper motor commands +/** + * @brief baseStepperCommands - parser of base stepper motor commands + * @param cmd - command message (don't brokes like in `cmdParser`) + * @param thrname - thread name + * @return 0 if found command (or it was erroneous), CMDPAR_ERR_NOTFOUND if not found, + * CMDPAR_ERR_SHOWHELP if got 'help', CMDPAR_CLEARERR if got 'stop' + */ +static int baseStepperCommands(const char *cmd, const threadinfo *ti){ + if(!cmd || !ti) return CMDPAR_ERR_NOTFOUND; + CANmesg can; + char buf[128]; + int i; + long par; + int NID = ti->ID & NODEID_MASK; // node ID + char *mesg = strdup(cmd); + threadCmd commands[] = { + [0] = {"stop", 0, NULL, "stop motor and clear errors"}, + [1] = {"status", 0, NULL, "get current position and status"}, + [2] = {"relmove", 1, &par, "relative move"}, + [3] = {"absmove", 1, &par, "absolute move to position arg"}, + [4] = {"enable", 1, &par, "enable (!0) or disable (0) motor"}, + [5] = {"setzero", 0, NULL, "set current position as zero"}, + [6] = {"maxspeed", 1, &par, "set/get maxspeed (get: arg==0)"}, + [7] = {"info", 0, NULL, "get motor information"}, + {NULL, 0, NULL, NULL} + }; + int idx = cmdParser(commands, mesg, ti->name); + DBG("idx = %d", idx); + if(idx < 0){ + switch(idx){ + case CMDPAR_ERR_BADNUMB: + snprintf(buf, 128, "%s bad arguments number for '%s'", ti->name, mesg); + break; + case CMDPAR_ERR_NOTFOUND: + FREE(mesg); + return CMDPAR_ERR_NOTFOUND; + break; + case CMDPAR_ERR_SHOWHELP: // do nothing + FREE(mesg); + return CMDPAR_ERR_SHOWHELP; + break; + default: + snprintf(buf, 128, "%s error in command '%s'", ti->name, mesg); + } + if(CMDPAR_ERR_SHOWHELP != idx){ + mesgAddText(&ServerMessages, buf); + FREE(mesg); + return 0; + } + } + switch(idx){ + case 0: // stop + CANBUSPUSH(SDO_write(&STOP, NID, 1, &can)); + CANBUSPUSH(SDO_read(&DEVSTATUS, NID, &can)); + CANBUSPUSH(SDO_read(&ERRSTATE, NID, &can)); + return CMDPAR_CLEARERR; + break; + case 1: // status, curpos + CANBUSPUSH(SDO_read(&DEVSTATUS, NID, &can)); + CANBUSPUSH(SDO_read(&POSITION, NID, &can)); + CANBUSPUSH(SDO_read(&ERRSTATE, NID, &can)); + break; + case 2: // relmove + i = 1; // positive direction + if(par < 0){ + i = 0; // negative direction + par = -par; + } + CANBUSPUSH(SDO_write(&ROTDIR, NID, i, &can)); + CANBUSPUSH(SDO_write(&RELSTEPS, NID, par, &can)); + break; + case 3: // absmove + CANBUSPUSH(SDO_write(&ABSSTEPS, NID, par, &can)); + break; + case 4: // enable + if(par) par = 1; + CANBUSPUSH(SDO_write(&ENABLE, NID, par, &can)); + break; + case 5: // setzero + CANBUSPUSH(SDO_write(&POSITION, NID, 0, &can)); + break; + case 6: // maxspeed + if(par) // set + CANBUSPUSH(SDO_write(&MAXSPEED, NID, par, &can)); + else + CANBUSPUSH(SDO_read(&MAXSPEED, NID, &can)); + break; + case 7: // info + CANBUSPUSH(SDO_read(&ERRSTATE, NID, &can)); + CANBUSPUSH(SDO_read(&DEVSTATUS, NID, &can)); + CANBUSPUSH(SDO_read(&POSITION, NID, &can)); + CANBUSPUSH(SDO_read(&ENABLE, NID, &can)); + CANBUSPUSH(SDO_read(&MICROSTEPS, NID, &can)); + CANBUSPUSH(SDO_read(&EXTENABLE, NID, &can)); + CANBUSPUSH(SDO_read(&MAXSPEED, NID, &can)); + CANBUSPUSH(SDO_read(&MAXCURNT, NID, &can)); + CANBUSPUSH(SDO_read(&GPIOVAL, NID, &can)); + CANBUSPUSH(SDO_read(&ROTDIR, NID, &can)); + CANBUSPUSH(SDO_read(&RELSTEPS, NID, &can)); + CANBUSPUSH(SDO_read(&ABSSTEPS, NID, &can)); + break; + default: + break; + } + FREE(mesg); + return 0; +} + /** * @brief simplestp - simplest stepper motor * @param arg - thread identifier @@ -375,60 +503,46 @@ static void chkSDO(const SDO *sdo, const char *thrname){ * stop: stop motor */ static void *simplestp(void *arg){ - threadinfo _U_ *ti = (threadinfo*)arg; + threadinfo *ti = (threadinfo*)arg; CANmesg can; - char buf[128]; - long parameters[2]; - threadCmd commands[] = { - [0] = {"test", 2, parameters, "show test values"}, - [1] = {"status", 0, NULL, "get current position and status"}, - [2] = {"stop", 0, NULL, "stop motor"}, - [3] = {"absmove", 1, parameters, "absolute move to position x"}, - {NULL, 0, NULL, NULL} - }; int NID = ti->ID & NODEID_MASK; // node ID + uint8_t clearerr = 0; // prepare all CANBUSPUSH(SDO_write(&MAXSPEED, NID, 3200, &can)); while(1){ char *mesg = mesgGetText(&ti->commands); - if(mesg) do{ + if(mesg){ DBG("Got command: %s", mesg); - int idx = cmdParser(commands, mesg); - DBG("idx = %d", idx); - if(-1 == idx){ - snprintf(buf, 128, "%s wrong command '%s' or bad arguments number", ti->name, mesg); - mesgAddText(&ServerMessages, buf); + int b = baseStepperCommands(mesg, ti); + if(b){ // not found, 'help' or 'stop' + switch(b){ + case CMDPAR_ERR_NOTFOUND: // process own commands + break; + case CMDPAR_ERR_SHOWHELP: // show own help + break; + case CMDPAR_CLEARERR: + clearerr = 1; + break; + default: + break; + } } - switch(idx){ - case 0: - snprintf(buf, 128, "%s get test params: %ld, %ld", ti->name, parameters[0], parameters[1]); - mesgAddText(&ServerMessages, buf); - break; - case 1: // status, curpos - CANBUSPUSH(SDO_read(&DEVSTATUS, NID, &can)); - CANBUSPUSH(SDO_read(&POSITION, NID, &can)); - CANBUSPUSH(SDO_read(&ERRSTATE, NID, &can)); - break; - case 2: // stop - CANBUSPUSH(SDO_write(&STOP, NID, 1, &can)); - break; - case 3: // absmove - /*if(parameters[0] < 0){ - CANBUSPUSH(SDO_write(&RELSTEPS, NID, parameters[0], &can)); - parameters[0] = -parameters[0]; - }*/ - CANBUSPUSH(SDO_write(&ABSSTEPS, NID, parameters[0], &can)); - break; - default: - break; - } - FREE(mesg); - }while(0); + } CANmesg *ans = (CANmesg*)mesgGetObj(&ti->answers, NULL); if(ans) do{ SDO sdo; if(!parseSDO(ans, &sdo)) break; chkSDO(&sdo, ti->name); + if(clearerr){ + if(sdo.index == ERRSTATE.index && sdo.subindex == ERRSTATE.subindex){ + CANBUSPUSH(SDO_write(&ERRSTATE, NID, sdo.data[0], &can)); + --clearerr; + } + if(sdo.index == DEVSTATUS.index && sdo.subindex == DEVSTATUS.subindex){ + CANBUSPUSH(SDO_write(&DEVSTATUS, NID, sdo.data[0], &can)); + --clearerr; + } + } FREE(ans); }while(0); usleep(1000); @@ -440,10 +554,7 @@ static void *simplestp(void *arg){ /** * @brief setCANspeed - set new speed of CANbus * @param speed - speed in kbaud - * @return 0 if all OK */ -int setCANspeed(int speed){ - if(canbus_setspeed(speed)) return 1; +void setCANspeed(int speed){ CANspeed = speed; - return 0; } diff --git a/canserver/processmotors.h b/canserver/processmotors.h index c2a5936..512fb86 100644 --- a/canserver/processmotors.h +++ b/canserver/processmotors.h @@ -27,6 +27,6 @@ extern thread_handler CANhandlers[]; void *CANserver(void *data); thread_handler *get_handler(const char *name); -int setCANspeed(int speed); +void setCANspeed(int speed); #endif // PROCESSMOTORS_H__ diff --git a/canserver/proto.c b/canserver/proto.c index a257386..c452249 100644 --- a/canserver/proto.c +++ b/canserver/proto.c @@ -40,7 +40,7 @@ static const char *listthr(_U_ char *par1, _U_ char *par2); static const char *regthr(char *thrname, char *data); static const char *unregthr(char *thrname, char *data); static const char *sendmsg(char *thrname, char *data); -static const char *setspd(char *speed, _U_ char *data); +//static const char *setspd(char *speed, _U_ char *data); /* * Commands format: @@ -63,7 +63,7 @@ static cmditem functions[] = { {"list", listthr, "- list all threads"}, {"mesg", sendmsg, "NAME MESG - send message `MESG` to thread `NAME`"}, {"register", regthr, "NAME ID ROLE - register new thread with `NAME`, raw receiving `ID` running thread `ROLE`"}, - {"speed", setspd, "SPD - set CANbus speed to `SPD`"}, +// {"speed", setspd, "SPD - set CANbus speed to `SPD`"}, {"threads", sthrds, "- list all possible threads with their message format"}, {"unregister", unregthr, "NAME - kill thread `NAME`"}, {NULL, NULL, NULL} @@ -86,7 +86,7 @@ static const char *sthrds(_U_ char *par1, _U_ char *par2){ char buf[128]; thread_handler *h = CANhandlers; while(h->name){ - snprintf(buf, 128, "thread> %s %s", h->name, h->helpmesg); + snprintf(buf, 128, "role> %s %s", h->name, h->helpmesg); mesgAddText(&ServerMessages, buf); ++h; } @@ -106,7 +106,7 @@ static const char *listthr(_U_ char *par1, _U_ char *par2){ mesgAddText(&ServerMessages, msg); empty = 0; }while(1); - mesgAddText(&ServerMessages, "thread> Send message 'list' to thread marked with (list) to get commands list"); + mesgAddText(&ServerMessages, "thread> Send message 'help' to threads marked with (args) to get commands list"); if(empty) return "No threads"; return NULL; } @@ -173,7 +173,7 @@ static const char *sendmsg(char *thrname, char *data){ if(!mesgAddText(&ti->commands, data)) return ANS_CANTSEND; return ANS_OK; } - +/* static const char *setspd(char *speed, _U_ char *data){ FNAME(); long spd; @@ -182,7 +182,7 @@ static const char *setspd(char *speed, _U_ char *data){ return "Wrong speed"; } return ANS_OK; -} +}*/ /** * @brief processCommand - parse command received by socket diff --git a/canserver/threadlist.c b/canserver/threadlist.c index a9a32b4..55eb718 100644 --- a/canserver/threadlist.c +++ b/canserver/threadlist.c @@ -127,7 +127,7 @@ threadinfo *findThreadByID(int ID){ */ void *mesgAddObj(message *msg, void *data, size_t size){ if(!msg || !data || size == 0) return NULL; - DBG("Want to add mesg with length %zd", size); + //DBG("Want to add mesg with length %zd", size); if(pthread_mutex_lock(&msg->mutex)) return NULL; msglist *node = pushmessage(&msg->msg, data, size); if(!node){ @@ -146,7 +146,7 @@ void *mesgAddObj(message *msg, void *data, size_t size){ */ char *mesgAddText(message *msg, char *txt){ if(!txt) return NULL; - DBG("mesgAddText(%s)", txt); + //DBG("mesgAddText(%s)", txt); size_t l = strlen(txt) + 1; return mesgAddObj(msg, (void*)txt, l); }