Add working simplest stepper motor; TODO: linear stage and turret

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
 

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;

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",
+    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->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 sdo = {
-    sdo.NID = NID;
+        .NID = NID,
-    sdo.ccs = CCS_INIT_UPLOAD;
+        .ccs = CCS_INIT_UPLOAD,
-    sdo.datalen = 0;
+        .datalen = 0,
-    sdo.index = idx;
+        .index = idx,
-    sdo.subindex = subidx;
+        .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 sdo ={
-    sdo.NID = NID;
+        .NID = NID,
-    sdo.ccs = CCS_INIT_DOWNLOAD;
+        .ccs = CCS_INIT_DOWNLOAD,
-    sdo.datalen = e->datasize;
+        .datalen = e->datasize,
-    sdo.index = e->index;
+        .index = e->index,
-    sdo.subindex = e->subindex;
+        .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 sdo = {
-    sdo.NID = NID;
+        .NID = NID,
-    sdo.ccs = CCS_INIT_DOWNLOAD;
+        .ccs = CCS_INIT_DOWNLOAD,
-    sdo.datalen = e->datasize;
+        .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

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

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

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

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

canserver/processmotors.c

@@ -31,6 +31,9 @@
 #include <unistd.h>     // usleep
 #include <usefull_macros.h>
 
+// 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)
+ * @param s       - user's command (default command HELPCMD shows all names/descriptions)
- * @return index of command in `cmdlist` or -1 if something wrong or not found
+ *      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",
+            snprintf(buf, 128, "%s> %-12s%-6d%s",
-                     cmdlist->name, cmdlist->nargs, cmdlist->descr);
+                     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
+        if(!canbus_read(&cm)){ // got raw message from CAN bus - parse it
-            DBG("Got CAN message from %d, len: %d", cm.ID, cm.len);
+            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);
+            int b = baseStepperCommands(mesg, ti);
-            DBG("idx = %d", idx);
+            if(b){ // not found, 'help' or 'stop'
-            if(-1 == idx){
+                switch(b){
-                snprintf(buf, 128, "%s wrong command '%s' or bad arguments number", ti->name, mesg);
+                    case CMDPAR_ERR_NOTFOUND: // process own commands
-                mesgAddText(&ServerMessages, buf);
-            }
-            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
+                    case CMDPAR_ERR_SHOWHELP: // show own help
-                    CANBUSPUSH(SDO_read(&DEVSTATUS, NID, &can));
-                    CANBUSPUSH(SDO_read(&POSITION, NID, &can));
-                    CANBUSPUSH(SDO_read(&ERRSTATE, NID, &can));
                     break;
-                case 2: // stop
+                    case CMDPAR_CLEARERR:
-                    CANBUSPUSH(SDO_write(&STOP, NID, 1, &can));
+                        clearerr = 1;
-                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){
+void setCANspeed(int speed){
-    if(canbus_setspeed(speed)) return 1;
     CANspeed = speed;
-    return 0;
 }

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__

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

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