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

start adding simple stepper

Browse Source
This commit is contained in:
Edward Emelianov 2020-11-19 18:46:18 +03:00
parent 2502c071cc
commit b0f487ce3f
12 changed files with 421 additions and 270 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
canserver/4tests.txt
View File

@ -8,3 +8,13 @@ mesg x 10 0x6001 0
#emul #emul
register new 3333 emulation register new 3333 emulation
#stepper
register x 0x58a stepper
#define getSDOe(SDO, fn, e) do{if(INT64_MIN != (i64 = SDO_read(&SDO, ID))) fn(i64); else ERRX(e);}while(0)
getSDOe(MICROSTEPS, setusteps, "Can't get microstepping");

1
canserver/aux.c
View File

@ -196,5 +196,6 @@ int str2long(char *str, long* l){
long n = strtol(str, &eptr, 0); long n = strtol(str, &eptr, 0);
if(*eptr) return 2; // wrong symbols in number if(*eptr) return 2; // wrong symbols in number
if(l) *l = n; if(l) *l = n;
//DBG("str '%s' to long %ld", str, n);
return 0; return 0;
} }

283
canserver/canopen.c
View File

@ -24,12 +24,6 @@
#include "canopen.h" #include "canopen.h"
typedef struct{
uint32_t code;
const char *errmsg;
} abortcodes;
static const abortcodes AC[] = { 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 //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"}, {0x05030000, "Toggle bit not alternated"},
@ -65,21 +59,19 @@ static const abortcodes AC[] = {
static const int ACmax = sizeof(AC)/sizeof(abortcodes) - 1; static const int ACmax = sizeof(AC)/sizeof(abortcodes) - 1;
// used
/** /**
* @brief abortcode_text - explanation of abort code * @brief abortcode_search - explanation of abort code
* @param abortcode - code * @param abortcode - code
* @return text for error or NULL * @return abortcode found or NULL
*/ */
const char *abortcode_text(uint32_t abortcode){ //, int *n){ static const abortcodes *abortcode_search(uint32_t abortcode){ //, int *n){
int idx = ACmax/2, min_ = 0, max_ = ACmax, newidx = 0, iter=0; int idx = ACmax/2, min_ = 0, max_ = ACmax, newidx = 0, iter=0;
do{ do{
++iter; ++iter;
uint32_t c = AC[idx].code; uint32_t c = AC[idx].code;
//printf("idx=%d, min=%d, max=%d\n", idx, min_, max_);
if(c == abortcode){ if(c == abortcode){
//if(n) *n = iter; return &AC[idx];
//DBG("got : %s", AC[idx].errmsg);
return AC[idx].errmsg;
}else if(c > abortcode){ }else if(c > abortcode){
newidx = (idx + min_)/2; newidx = (idx + min_)/2;
max_ = idx; max_ = idx;
@ -89,37 +81,43 @@ const char *abortcode_text(uint32_t abortcode){ //, int *n){
min_ = idx; min_ = idx;
} }
if(newidx == idx || min_ < 0 || max_ > ACmax){ if(newidx == idx || min_ < 0 || max_ > ACmax){
//if(n) *n = 0;
return NULL; return NULL;
} }
idx = newidx; idx = newidx;
}while(1); }while(1);
} }
// make CAN message from sdo object; don't support more then one block/packet // used
static CANmesg *mkMesg(SDO *sdo){ /**
static CANmesg mesg; * @brief mkMesg - make CAN message from sdo object
mesg.ID = RSDO_COBID + sdo->NID; * @param sdo (i) - sdo object to transform
mesg.len = 8; * @param mesg (o) - output CANmesg
memset(mesg.data, 0, 8); * @return pointer to mesg
mesg.data[0] = SDO_CCS(sdo->ccs); */
CANmesg *mkMesg(SDO *sdo, CANmesg *mesg){
if(!sdo || !mesg) return NULL;
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 if(sdo->datalen){ // send N bytes of data
mesg.data[0] |= SDO_N(sdo->datalen) | SDO_E | SDO_S; 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]; 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[1] = sdo->index & 0xff; // l
mesg.data[2] = (sdo->index >> 8) & 0xff; // h mesg->data[2] = (sdo->index >> 8) & 0xff; // h
mesg.data[3] = sdo->subindex; mesg->data[3] = sdo->subindex;
return &mesg; return mesg;
} }
// used
/** /**
* @brief parseSDO - transform CAN-message to SDO * @brief parseSDO - transform CAN-message to SDO
* @param mesg (i) - message * @param mesg (i) - message
* @param sdo (o) - SDO * @param sdo (o) - SDO
* @return sdo or NULL depending on result * @return sdo or NULL depending on result
*/ */
SDO *parseSDO(CANmesg *mesg, SDO *sdo){ SDO *parseSDO(const CANmesg *mesg, SDO *sdo){
if(mesg->len != 8){ if(mesg->len != 8){
WARNX("Wrong SDO data length"); WARNX("Wrong SDO data length");
return NULL; return NULL;
@ -137,7 +135,7 @@ SDO *parseSDO(CANmesg *mesg, SDO *sdo){
if((spec & SDO_E) && (spec & SDO_S)) sdo->datalen = SDO_datalen(spec); 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 if(sdo->ccs == CCS_ABORT_TRANSFER) sdo->datalen = 4; // error code
else sdo->datalen = 0; // no data in message else sdo->datalen = 0; // no data in message
for(uint8_t i = 0; i < sdo->datalen; ++i) sdo->data[i] = mesg->data[4+i]; 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",
sdo->NID, sdo->ccs, sdo->index, sdo->subindex, sdo->datalen); sdo->NID, sdo->ccs, sdo->index, sdo->subindex, sdo->datalen);
return sdo; return sdo;
@ -151,8 +149,8 @@ static int ask2read(uint16_t idx, uint8_t subidx, uint8_t NID){
sdo.datalen = 0; sdo.datalen = 0;
sdo.index = idx; sdo.index = idx;
sdo.subindex = subidx; sdo.subindex = subidx;
CANmesg *mesg = mkMesg(&sdo); CANmesg mesg;
return canbus_write(mesg); return canbus_write(mkMesg(&sdo, &mesg));
} }
static SDO *getSDOans(uint16_t idx, uint8_t subidx, uint8_t NID, SDO *sdo){ static SDO *getSDOans(uint16_t idx, uint8_t subidx, uint8_t NID, SDO *sdo){
@ -195,74 +193,162 @@ SDO *readSDOvalue(uint16_t idx, uint8_t subidx, uint8_t NID, SDO *sdo){
return getSDOans(idx, subidx, NID, sdo); return getSDOans(idx, subidx, NID, sdo);
} }
static inline uint32_t mku32(uint8_t data[4]){ // mk... are all used
static inline uint32_t mku32(const uint8_t data[4]){
return (uint32_t)(data[0] | (data[1]<<8) | (data[2]<<16) | (data[3]<<24)); return (uint32_t)(data[0] | (data[1]<<8) | (data[2]<<16) | (data[3]<<24));
} }
static inline uint16_t mku16(uint8_t data[4]){ static inline uint16_t mku16(const uint8_t data[4]){
return (uint16_t)(data[0] | (data[1]<<8)); return (uint16_t)(data[0] | (data[1]<<8));
} }
static inline uint8_t mku8(uint8_t data[4]){ static inline uint8_t mku8(const uint8_t data[4]){
return data[0]; return data[0];
} }
static inline int32_t mki32(uint8_t data[4]){ static inline int32_t mki32(const uint8_t data[4]){
return (int32_t)(data[0] | (data[1]<<8) | (data[2]<<16) | (data[3]<<24)); return (int32_t)(data[0] | (data[1]<<8) | (data[2]<<16) | (data[3]<<24));
} }
static inline int16_t mki16(uint8_t data[4]){ static inline int16_t mki16(const uint8_t data[4]){
return (int16_t)(data[0] | (data[1]<<8)); return (int16_t)(data[0] | (data[1]<<8));
} }
static inline int8_t mki8(uint8_t data[4]){ static inline int8_t mki8(const uint8_t data[4]){
return (int8_t)data[0]; return (int8_t)data[0];
} }
// read SDO value, if error - return INT64_MIN // used
int64_t SDO_read(const SDO_dic_entry *e, uint8_t NID){ /**
FNAME(); * @brief getSDOval - get value from SDO
SDO sdo; * @param sdo (i) - SDO
if(!readSDOvalue(e->index, e->subindex, NID, &sdo)){ * @param e (i) - its dictionary entry
return INT64_MIN; * @param ac (o) - pointer for abort code (in case of error) or NULL
} * @return value, INT64_MIN in case of error or INT64_MAX in case of zero-length
if(sdo.ccs == CCS_ABORT_TRANSFER){ // error */
int64_t getSDOval(const SDO *sdo, const SDO_dic_entry *e, const abortcodes **ac){
if(sdo->ccs == CCS_ABORT_TRANSFER){ // error
WARNX("Got error for SDO 0x%X", e->index); WARNX("Got error for SDO 0x%X", e->index);
uint32_t ac = mku32(sdo.data); uint32_t c = mku32(sdo->data);
const char *etxt = abortcode_text(ac); const abortcodes *abc = abortcode_search(c);
if(etxt) WARNX("Abort code 0x%X: %s", ac, etxt); if(abc) WARNX("Abort code 0x%X: %s", ac, abc->errmsg);
if(ac) *ac = abc;
return INT64_MIN; return INT64_MIN;
} }
if(sdo.datalen != e->datasize){ if(sdo->datalen == 0) return INT_MAX;
WARNX("Got SDO with length %d instead of %d (as in dictionary)", sdo.datalen, e->datasize); 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; int64_t ans = 0;
if(e->issigned){ if(e->issigned){
switch(sdo.datalen){ switch(sdo->datalen){
case 1: case 1:
ans = mki8(sdo.data); ans = mki8(sdo->data);
break; break;
case 4: case 4:
ans = mki32(sdo.data); ans = mki32(sdo->data);
break; break;
default: // can't be 3! 3->2 default: // can't be 3! 3->2
ans = mki16(sdo.data); ans = mki16(sdo->data);
} }
}else{ }else{
switch(sdo.datalen){ switch(sdo->datalen){
case 1: case 1:
ans = mku8(sdo.data); ans = mku8(sdo->data);
break; break;
case 4: case 4:
ans = mku32(sdo.data); ans = mku32(sdo->data);
break; break;
default: // can't be 3! 3->2 default: // can't be 3! 3->2
ans = mku16(sdo.data); ans = mku16(sdo->data);
} }
} }
return ans; return ans;
} }
// used
/**
* @brief SDO_read - form CANmesg to read SDO entry `e`
* @param e (i) - SDO dictionary entry to read
* @param NID - target node ID
* @param cm (o) - pointer to CANmesg which to modify
* @return `cm` or NULL if failed
*/
CANmesg *SDO_read(const SDO_dic_entry *e, uint8_t NID, CANmesg *cm){
if(!e || !cm) return NULL;
SDO sdo = {
.NID = NID,
.ccs = CCS_INIT_UPLOAD,
.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);
}
// used
/**
* @brief SDO_write - form CANmesg to write `data` to SDO entry `e`
* @param e
* @param NID
* @param cm
* @return
*/
CANmesg *SDO_write(const SDO_dic_entry *e, uint8_t NID, int64_t data, CANmesg *cm){
if(!e || !cm) return NULL;
uint32_t U;
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;
DBG("datalen=%d, signed=%d", e->datasize, e->issigned);
if(e->issigned){
switch(e->datasize){
case 1:
sdo.data[0] = (uint8_t) data;
break;
case 4:
I = (int32_t) data;
sdo.data[0] = I&0xff;
sdo.data[1] = (I>>8)&0xff;
sdo.data[2] = (I>>16)&0xff;
sdo.data[3] = (I>>24)&0xff;
break;
default: // can't be 3! 3->2
I16 = (int16_t) data;
sdo.data[0] = I16&0xff;
sdo.data[1] = (I16>>8)&0xff;
}
}else{
switch(e->datasize){
case 1:
sdo.data[0] = (uint8_t) data;
break;
case 4:
U = (uint32_t) data;
sdo.data[0] = U&0xff;
sdo.data[1] = (U>>8)&0xff;
sdo.data[2] = (U>>16)&0xff;
sdo.data[3] = (U>>24)&0xff;
break;
default: // can't be 3! 3->2
U16 = (uint16_t) data;
sdo.data[0] = U16&0xff;
sdo.data[1] = (U16>>8)&0xff;
}
}
mkMesg(&sdo, cm);
return cm;
}
// write SDO data, return 0 if all OK // write SDO data, return 0 if all OK
int SDO_writeArr(const SDO_dic_entry *e, uint8_t NID, const uint8_t *data){ int SDO_writeArr(const SDO_dic_entry *e, uint8_t NID, const uint8_t *data){
FNAME(); FNAME();
@ -277,9 +363,10 @@ int SDO_writeArr(const SDO_dic_entry *e, uint8_t NID, const uint8_t *data){
for(uint8_t i = 0; i < e->datasize; ++i) sdo.data[i] = data[i]; for(uint8_t i = 0; i < e->datasize; ++i) sdo.data[i] = data[i];
sdo.index = e->index; sdo.index = e->index;
sdo.subindex = e->subindex; sdo.subindex = e->subindex;
CANmesg *mesgp = mkMesg(&sdo); CANmesg mesgp;
mkMesg(&sdo, &mesgp);
DBG("Canbus write.."); DBG("Canbus write..");
if(canbus_write(mesgp)){ if(canbus_write(&mesgp)){
WARNX("SDO_write(): Can't initiate download"); WARNX("SDO_write(): Can't initiate download");
return 2; return 2;
} }
@ -292,8 +379,8 @@ int SDO_writeArr(const SDO_dic_entry *e, uint8_t NID, const uint8_t *data){
if(sdop.ccs == CCS_ABORT_TRANSFER){ // error if(sdop.ccs == CCS_ABORT_TRANSFER){ // error
WARNX("SDO_write(): Got error for SDO 0x%X", e->index); WARNX("SDO_write(): Got error for SDO 0x%X", e->index);
uint32_t ac = mku32(sdop.data); uint32_t ac = mku32(sdop.data);
const char *etxt = abortcode_text(ac); const abortcodes *e = abortcode_search(ac);
if(etxt) WARNX("Abort code 0x%X: %s", ac, etxt); if(e) WARNX("Abort code 0x%X: %s", ac, e->errmsg);
return 4; return 4;
} }
if(sdop.datalen != 0){ if(sdop.datalen != 0){
@ -306,75 +393,3 @@ int SDO_writeArr(const SDO_dic_entry *e, uint8_t NID, const uint8_t *data){
} }
return 0; 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

18
canserver/canopen.h
View File

@ -77,14 +77,18 @@ typedef struct{
uint8_t ccs; // Client command specifier uint8_t ccs; // Client command specifier
} SDO; } SDO;
const char *abortcode_text(uint32_t abortcode); typedef struct{
SDO *parseSDO(CANmesg *mesg, SDO *sdo); uint32_t code;
const char *errmsg;
} abortcodes;
CANmesg *mkMesg(SDO *sdo, CANmesg *mesg);
SDO *parseSDO(const CANmesg *mesg, SDO *sdo);
SDO *readSDOvalue(uint16_t idx, uint8_t subidx, uint8_t NID, SDO *sdo); SDO *readSDOvalue(uint16_t idx, uint8_t subidx, uint8_t NID, SDO *sdo);
int64_t getSDOval(const SDO *sdo, const SDO_dic_entry *e, const abortcodes **ac);
int64_t SDO_read(const SDO_dic_entry *e, uint8_t NID); CANmesg *SDO_read(const SDO_dic_entry *e, uint8_t NID, CANmesg *cm);
CANmesg *SDO_write(const SDO_dic_entry *e, uint8_t NID, int64_t data, CANmesg *cm);
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); //int SDO_readByte(uint16_t idx, uint8_t subidx, uint8_t *data, uint8_t NID);
#endif // CANOPEN_H__ #endif // CANOPEN_H__

121
canserver/dicentries.in
View File

@ -21,123 +21,78 @@
// variable name / index / subindex / datasize / issigned / name // variable name / index / subindex / datasize / issigned / name
// heartbeat time // heartbeat time
DICENTRY(HEARTBTTIME, 0x1017, 0, 2, 0, "heartbeat time") DICENTRY(HEARTBTTIME, 0x1017, 0, 2, 0, "heartbeat time", "hearbt")
// 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 // node ID
DICENTRY(NODEID, 0x2002, 0, 1, 0, "node ID") DICENTRY(NODEID, 0x2002, 0, 1, 0, "node ID", "nodeid")
// baudrate // baudrate
DICENTRY(BAUDRATE, 0x2003, 0, 1, 0, "baudrate") DICENTRY(BAUDRATE, 0x2003, 0, 1, 0, "baudrate", "baudrate")
// system control: 1- bootloader, 2 - save parameters, 3 - reset factory settings // 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") DICENTRY(SYSCONTROL, 0x2007, 0, 1, 0, "system control: 1- bootloader, 2 - save parameters, 3 - reset factory settings", "syscontrol")
// error status // error status
DICENTRY(ERRSTATE, 0x6000, 0, 1, 0, "error status") DICENTRY(ERRSTATE, 0x6000, 0, 1, 0, "error status", "errstatus")
// controller status // controller status
DICENTRY(DEVSTATUS, 0x6001, 0, 1, 0, "controller status") DICENTRY(DEVSTATUS, 0x6001, 0, 1, 0, "controller status", "devstatus")
// rotation direction // rotation direction
DICENTRY(ROTDIR, 0x6002, 0, 1, 0, "rotation direction") DICENTRY(ROTDIR, 0x6002, 0, 1, 0, "rotation direction", "rotdir")
// maximal speed // maximal speed
DICENTRY(MAXSPEED, 0x6003, 0, 4, 1, "maximal speed") DICENTRY(MAXSPEED, 0x6003, 0, 4, 1, "maximal speed", "maxspeed")
// relative displacement // relative displacement
DICENTRY(RELSTEPS, 0x6004, 0, 4, 0, "relative displacement") DICENTRY(RELSTEPS, 0x6004, 0, 4, 0, "relative displacement", "relsteps")
// operation mode // operation mode
DICENTRY(OPMODE, 0x6005, 0, 1, 0, "operation mode") DICENTRY(OPMODE, 0x6005, 0, 1, 0, "operation mode", "opmode")
// start speed // start speed
DICENTRY(STARTSPEED, 0x6006, 0, 2, 0, "start speed") DICENTRY(STARTSPEED, 0x6006, 0, 2, 0, "start speed", "startspd")
// stop speed // stop speed
DICENTRY(STOPSPEED, 0x6007, 0, 2, 0, "stop speed") DICENTRY(STOPSPEED, 0x6007, 0, 2, 0, "stop speed", "stopspd")
// acceleration coefficient // acceleration coefficient
DICENTRY(ACCELCOEF, 0x6008, 0, 1, 0, "acceleration coefficient") DICENTRY(ACCELCOEF, 0x6008, 0, 1, 0, "acceleration coefficient", "acccoef")
// deceleration coefficient // deceleration coefficient
DICENTRY(DECELCOEF, 0x6009, 0, 1, 0, "deceleration coefficient") DICENTRY(DECELCOEF, 0x6009, 0, 1, 0, "deceleration coefficient", "deccoef")
// microstepping // microstepping
DICENTRY(MICROSTEPS, 0x600A, 0, 2, 0, "microstepping") DICENTRY(MICROSTEPS, 0x600A, 0, 2, 0, "microstepping", "microsteps")
// max current // max current
DICENTRY(MAXCURNT, 0x600B, 0, 2, 0, "maximum phase current") DICENTRY(MAXCURNT, 0x600B, 0, 2, 0, "maximum phase current", "maxcurnt")
// current position // current position
DICENTRY(POSITION, 0x600C, 0, 4, 0, "current position") DICENTRY(POSITION, 0x600C, 0, 4, 0, "current position", "curpos")
// current reduction // current reduction
DICENTRY(CURRREDUCT, 0x600D, 0, 1, 0, "current reduction") DICENTRY(CURRREDUCT, 0x600D, 0, 1, 0, "current reduction", "curred")
// motor enable // motor enable
DICENTRY(ENABLE, 0x600E, 0, 1, 0, "motor enable") DICENTRY(ENABLE, 0x600E, 0, 1, 0, "motor enable", "enable")
// EXT emergency stop enable // EXT emergency stop enable
DICENTRY(EXTENABLE, 0x600F, 1, 1, 0, "EXT emergency stop enable") DICENTRY(EXTENABLE, 0x600F, 1, 1, 0, "EXT emergency stop enable", "extenable")
// EXT emergency stop trigger mode // EXT emergency stop trigger mode
DICENTRY(EXTTRIGMODE, 0x600F, 2, 1, 0, "EXT emergency stop trigger mode") DICENTRY(EXTTRIGMODE, 0x600F, 2, 1, 0, "EXT emergency stop trigger mode", "exttrigmod")
// EXT emergency sensor type // EXT emergency sensor type
DICENTRY(EXTSENSTYPE, 0x600F, 3, 1, 0, "EXT emergency sensor type") DICENTRY(EXTSENSTYPE, 0x600F, 3, 1, 0, "EXT emergency sensor type", "extsenstype")
// GPIO direction // GPIO direction
DICENTRY(GPIODIR, 0x6011, 1, 2, 0, "GPIO direction") DICENTRY(GPIODIR, 0x6011, 1, 2, 0, "GPIO direction", "gpiodir")
// GPIO configuration // GPIO configuration
DICENTRY(GPIOCONF, 0x6011, 2, 4, 0, "GPIO configuration") DICENTRY(GPIOCONF, 0x6011, 2, 4, 0, "GPIO configuration", "gpioconf")
// GPIO value // GPIO value
DICENTRY(GPIOVAL, 0x6012, 0, 2, 0, "GPIO value") DICENTRY(GPIOVAL, 0x6012, 0, 2, 0, "GPIO value", "gpioval")
// stall parameters // stall parameters
DICENTRY(STALLPARS, 0x6017, 0, 2, 0, "stall parameters (open loop)") DICENTRY(STALLPARS, 0x6017, 0, 2, 0, "stall parameters (open loop)", "stallpars")
// offline operation // offline operation
DICENTRY(OFFLNMBR, 0x6018, 1, 1, 0, "Number of offline programming command") DICENTRY(OFFLNMBR, 0x6018, 1, 1, 0, "Number of offline programming command", "offlnmbr")
DICENTRY(OFFLENBL, 0x6018, 2, 1, 0, "Offline automatic operation enable") DICENTRY(OFFLENBL, 0x6018, 2, 1, 0, "Offline automatic operation enable", "offlenbl")
// stall set // stall set
DICENTRY(STALLSET, 0x601B, 0, 1, 0, "stall set (open loop)") DICENTRY(STALLSET, 0x601B, 0, 1, 0, "stall set (open loop)", "stallset")
// absolute displacement // absolute displacement
DICENTRY(ABSSTEPS, 0x601C, 0, 4, 1, "absolute displacement") DICENTRY(ABSSTEPS, 0x601C, 0, 4, 1, "absolute displacement", "abssteps")
// stop motor // stop motor
DICENTRY(STOP, 0x6020, 0, 1, 0, "stop motor") DICENTRY(STOP, 0x6020, 0, 1, 0, "stop motor", "stop")
// encoder resolution // encoder resolution
DICENTRY(ENCRESOL, 0x6021, 0, 2, 0, "encoder resolution (closed loop)") DICENTRY(ENCRESOL, 0x6021, 0, 2, 0, "encoder resolution (closed loop)", "encresol")
// stall length parameter // stall length parameter
DICENTRY(STALLLEN, 0x6028, 0, 2, 0, "stall length parameter (closed loop)") DICENTRY(STALLLEN, 0x6028, 0, 2, 0, "stall length parameter (closed loop)", "stallen")
// torque ring enable // torque ring enable
DICENTRY(TORQRING, 0x6029, 0, 1, 0, "torque ring enable (closed loop)") DICENTRY(TORQRING, 0x6029, 0, 1, 0, "torque ring enable (closed loop)", "torqring")
// autosave position // autosave position
DICENTRY(POSAUTOSAVE, 0x602A, 0, 1, 0, "autosave position (closed loop)") DICENTRY(POSAUTOSAVE, 0x602A, 0, 1, 0, "autosave position (closed loop)", "autosave")
// real time speed // real time speed
DICENTRY(REALTIMESPD, 0x6030, 0, 2, 1, "real time speed (closed loop)") DICENTRY(REALTIMESPD, 0x6030, 0, 2, 1, "real time speed (closed loop)", "realtimespd")
// calibration zero // calibration zero
DICENTRY(CALIBZERO, 0x6034, 0, 4, 1, "calibration zero") DICENTRY(CALIBZERO, 0x6034, 0, 4, 1, "calibration zero", "calibzero")
// encoder position // encoder position
DICENTRY(ENCPOS, 0x6035, 0, 4, 1, "encoder position") DICENTRY(ENCPOS, 0x6035, 0, 4, 1, "encoder position", "encpos")

183
canserver/processmotors.c
View File

@ -24,6 +24,7 @@
#include "socket.h" #include "socket.h"
#include <fcntl.h> // open #include <fcntl.h> // open
#include <inttypes.h> // PRId64
#include <stdio.h> // printf #include <stdio.h> // printf
#include <string.h> // strcmp #include <string.h> // strcmp
#include <sys/stat.h> // open #include <sys/stat.h> // open
@ -37,18 +38,29 @@ static message CANbusMessages = {0}; // CANserver thread is master
#define CANBUSPUSH(mesg) mesgAddObj(&CANbusMessages, mesg, sizeof(CANmesg)) #define CANBUSPUSH(mesg) mesgAddObj(&CANbusMessages, mesg, sizeof(CANmesg))
#define CANBUSPOP() mesgGetObj(&CANbusMessages, NULL) #define CANBUSPOP() mesgGetObj(&CANbusMessages, NULL)
// commands sent to threads
// each threadCmd array should be terminated with NULLs; default command `help` shows all names/descriptions
typedef struct{
char *name; // command name
int nargs; // max arguments number
long *args; // pointer to arguments
char *descr; // description for help
} threadCmd;
// basic threads // basic threads
// messages: master - thread, slave - caller // messages: master - thread, slave - caller
static void *stpemulator(void *arg); static void *stpemulator(void *arg);
static void *rawcommands(void *arg); static void *rawcommands(void *arg);
static void *canopencmds(void *arg); static void *canopencmds(void *arg);
static void *simplestp(void *arg);
// handlers for standard types // handlers for standard types
thread_handler CANhandlers[] = { thread_handler CANhandlers[] = {
{"emulation", stpemulator}, {"canopen", canopencmds, "NodeID index subindex [data] - raw CANOpen commands with `index` and `subindex` to `NodeID`"},
{"raw", rawcommands}, {"emulation", stpemulator, "(list) - stepper emulation"},
{"canopen", canopencmds}, {"raw", rawcommands, "ID [DATA] - raw CANbus commands to raw `ID` with `DATA`"},
{NULL, NULL} {"stepper", simplestp, "(list) - simple stepper motor: no limit switches, only goto"},
{NULL, NULL, NULL}
}; };
thread_handler *get_handler(const char *name){ thread_handler *get_handler(const char *name){
@ -59,6 +71,51 @@ thread_handler *get_handler(const char *name){
return NULL; return NULL;
} }
/**
* @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
*/
static int cmdParser(threadCmd *cmdlist, char *s){
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
char buf[128];
while(cmdlist->name){
snprintf(buf, 128, "params> %s (%d arguments) - %s",
cmdlist->name, cmdlist->nargs, cmdlist->descr);
mesgAddText(&ServerMessages, buf);
++cmdlist;
}
return 10000;
}
while(cmdlist->name){
if(strcmp(cmdlist->name, command) == 0) break;
++idx; ++cmdlist;
}
if(!cmdlist->name) return -1; // command not found
if(cmdlist->nargs == 0) return idx; // simple command
int nargsplus1 = cmdlist->nargs + 1, N = 0;
long *args = MALLOC(long, nargsplus1);
for(; N < nargsplus1; ++N){
char *nxt = strtok_r(NULL, " \t,;\r\n", &saveptr);
if(!nxt) break;
if(str2long(nxt, &args[N])) break;
}
if(N != cmdlist->nargs){
FREE(args);
return -1; // bad arguments number
}
for(int i = 0; i < N; ++i)
cmdlist->args[i] = args[i];
FREE(args);
return idx;
}
/** /**
* @brief parsePacket - convert text into can packet data * @brief parsePacket - convert text into can packet data
* @param packet (o) - pointer to CANpacket or NULL (just to check) * @param packet (o) - pointer to CANpacket or NULL (just to check)
@ -116,29 +173,10 @@ static void reopen_device(){
static void processCANmessage(CANmesg *mesg){ static void processCANmessage(CANmesg *mesg){
threadinfo *ti = findThreadByID(0); threadinfo *ti = findThreadByID(0);
if(ti){ if(ti){
/*char buf[64], *ptr = buf;
int l = 64, x;
x = snprintf(ptr, l, "#0x%03X ", cm.ID);
l -= x; ptr += x;
for(int i = 0; i < cm.len; ++i){
x = snprintf(ptr, l, "0x%02X ", cm.data[i]);
l -= x; ptr += x;
}*/
mesgAddObj(&ti->answers, (void*)mesg, sizeof(CANmesg)); mesgAddObj(&ti->answers, (void*)mesg, sizeof(CANmesg));
} }
ti = findThreadByID(mesg->ID); ti = findThreadByID(mesg->ID);
if(ti){ if(ti){
/* DBG("Found");
char buf[64], *ptr = buf;
int l = 64, x;
x = snprintf(ptr, l, "#0x%03X ", mesg->ID);
l -= x; ptr += x;
for(int i = 0; i < mesg->len; ++i){
x = snprintf(ptr, l, "0x%02X ", mesg->data[i]);
l -= x; ptr += x;
}
mesgAddText(&ti->answers, buf);
*/
mesgAddObj(&ti->answers, (void*) mesg, sizeof(CANmesg)); mesgAddObj(&ti->answers, (void*) mesg, sizeof(CANmesg));
} }
} }
@ -161,7 +199,7 @@ void *CANserver(_U_ void *data){
FREE(msg); FREE(msg);
} }
usleep(1000); usleep(1000);
CANmesg cm; 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 - parce it
DBG("Got CAN message from %d, len: %d", cm.ID, cm.len); DBG("Got CAN message from %d, len: %d", cm.ID, cm.len);
processCANmessage(&cm); processCANmessage(&cm);
@ -254,7 +292,7 @@ static void sendSDO(char *mesg){
CANmesg comesg; CANmesg comesg;
uint8_t datalen = (uint8_t) N - 3; uint8_t datalen = (uint8_t) N - 3;
comesg.data[0] = SDO_CCS(CCS_INIT_DOWNLOAD); comesg.data[0] = (datalen) ? SDO_CCS(CCS_INIT_DOWNLOAD) : SDO_CCS(CCS_INIT_UPLOAD); // write or read
comesg.len = 8; comesg.len = 8;
if(datalen){ // there's data if(datalen){ // there's data
comesg.data[0] |= SDO_N(datalen) | SDO_E | SDO_S; comesg.data[0] |= SDO_N(datalen) | SDO_E | SDO_S;
@ -284,8 +322,8 @@ static void *canopencmds(void *arg){
if(parseSDO(ans, &sdo)){ if(parseSDO(ans, &sdo)){
char buf[128], *ptr = buf; char buf[128], *ptr = buf;
int rest = 128; int rest = 128;
int l = snprintf(ptr, rest, "SDO={nid=0x%02X, idx=0x%04X, subidx=%d, ccs=0x%02X, datalen=%d", int l = snprintf(ptr, rest, "%s nid=0x%02X, idx=0x%04X, subidx=%d, ccs=0x%02X, datalen=%d",
sdo.NID, sdo.index, sdo.subindex, sdo.ccs, sdo.datalen); ti->name, sdo.NID, sdo.index, sdo.subindex, sdo.ccs, sdo.datalen);
ptr += l; rest -= l; ptr += l; rest -= l;
if(sdo.datalen){ if(sdo.datalen){
l = snprintf(ptr, rest, ", data=["); l = snprintf(ptr, rest, ", data=[");
@ -298,7 +336,6 @@ static void *canopencmds(void *arg){
l = snprintf(ptr, rest, "]"); l = snprintf(ptr, rest, "]");
ptr += l; rest -= l; ptr += l; rest -= l;
} }
snprintf(ptr, rest, "}");
mesgAddText(&ServerMessages, buf); mesgAddText(&ServerMessages, buf);
} }
FREE(ans); FREE(ans);
@ -309,6 +346,96 @@ static void *canopencmds(void *arg){
return NULL; return NULL;
} }
// check incoming SDO and send data to all (thrname - thread name)
static void chkSDO(const SDO *sdo, const char *thrname){
char buf[128];
if(!sdo) return;
SDO_dic_entry *de = dictentry_search(sdo->index, sdo->subindex);
if(!de) return; // SDO not from dictionary
const abortcodes *ac = NULL;
int64_t val = getSDOval(sdo, de, &ac);
if(val == INT_MAX) // zero-length SDO - last command acknowledgement
snprintf(buf, 128, "%s %s=OK", thrname, de->varname);
else if(val == INT64_MIN) // error
snprintf(buf, 128, "%s abortcode='0x%X' error='%s'", thrname, ac->code, ac->errmsg);
else // got value
snprintf(buf, 128, "%s %s=%" PRId64, thrname, de->varname, val);
mesgAddText(&ServerMessages, buf);
}
/**
* @brief simplestp - simplest stepper motor
* @param arg - thread identifier
* @return not used
* Commands:
* maxspeed x: set maximal speed to x pulses per second
* microsteps x: set microstepping to x pulses per step
* move x: move for x pulses (+-)
* status: current position & state
* stop: stop motor
*/
static void *simplestp(void *arg){
threadinfo _U_ *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
// prepare all
CANBUSPUSH(SDO_write(&MAXSPEED, NID, 3200, &can));
while(1){
char *mesg = mesgGetText(&ti->commands);
if(mesg) do{
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);
}
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);
FREE(ans);
}while(0);
usleep(1000);
}
LOGERR("simplestp(): UNREACHABLE CODE REACHED!");
return NULL;
}
/** /**
* @brief setCANspeed - set new speed of CANbus * @brief setCANspeed - set new speed of CANbus

54
canserver/proto.c
View File

@ -32,7 +32,10 @@ static const char *ANS_OK = "OK";
static const char *ANS_WRONGCANID = "Wrong CANID"; static const char *ANS_WRONGCANID = "Wrong CANID";
static const char *ANS_NOTFOUND = "Thread not found"; static const char *ANS_NOTFOUND = "Thread not found";
static const char *ANS_CANTSEND = "Can't send message"; static const char *ANS_CANTSEND = "Can't send message";
static const char *ANS_WRONGMESG = "Wrong message";
static const char *shelp(_U_ char *par1, _U_ char *par2);
static const char *sthrds(_U_ char *par1, _U_ char *par2);
static const char *listthr(_U_ char *par1, _U_ char *par2); static const char *listthr(_U_ char *par1, _U_ char *par2);
static const char *regthr(char *thrname, char *data); static const char *regthr(char *thrname, char *data);
static const char *unregthr(char *thrname, char *data); static const char *unregthr(char *thrname, char *data);
@ -49,20 +52,47 @@ static const char *setspd(char *speed, _U_ char *data);
* you can get full list of functions by function `help` * you can get full list of functions by function `help`
*/ */
typedef struct{ typedef struct{
const char *fname; // function name const char *fname; // function name
const char *(*handler)(char *arg1, char *arg2); // data handler (arg1 and arg2 could be changed) const char *(*handler)(char *arg1, char *arg2); // data handler (arg1 and arg2 could be changed)
const char *helpmesg; // help message
} cmditem; } cmditem;
// array with known functions // array with known functions
static cmditem functions[] = { static cmditem functions[] = {
{"list", listthr}, // list threads {"help", shelp, "- show help"},
{"mesg", sendmsg}, // "mesg NAME ID [data]" {"list", listthr, "- list all threads"},
{"register", regthr}, // "register NAME ID", ID - RAW CAN ID (not canopen ID)!!! {"mesg", sendmsg, "NAME MESG - send message `MESG` to thread `NAME`"},
{"speed", setspd}, // set CANbus speed {"register", regthr, "NAME ID ROLE - register new thread with `NAME`, raw receiving `ID` running thread `ROLE`"},
{"unregister", unregthr}, // "unregister NAME" {"speed", setspd, "SPD - set CANbus speed to `SPD`"},
{NULL, NULL} {"threads", sthrds, "- list all possible threads with their message format"},
{"unregister", unregthr, "NAME - kill thread `NAME`"},
{NULL, NULL, NULL}
}; };
// show help
static const char *shelp(_U_ char *par1, _U_ char *par2){
char buf[128];
cmditem *ptr = functions;
while(ptr->fname){
snprintf(buf, 128, "help> %s %s", ptr->fname, ptr->helpmesg);
mesgAddText(&ServerMessages, buf);
++ptr;
}
return NULL;
}
// show all possible threads
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);
mesgAddText(&ServerMessages, buf);
++h;
}
return NULL;
}
// list all threads // list all threads
static const char *listthr(_U_ char *par1, _U_ char *par2){ static const char *listthr(_U_ char *par1, _U_ char *par2){
FNAME(); FNAME();
@ -72,10 +102,11 @@ static const char *listthr(_U_ char *par1, _U_ char *par2){
do{ do{
list = nextThread(list); list = nextThread(list);
if(!list) break; if(!list) break;
snprintf(msg, 256, "thread name='%s' role='%s' ID=0x%X", list->ti.name, list->ti.handler.name, list->ti.ID); snprintf(msg, 256, "thread> name='%s' role='%s' ID=0x%X", list->ti.name, list->ti.handler.name, list->ti.ID);
mesgAddText(&ServerMessages, msg); mesgAddText(&ServerMessages, msg);
empty = 0; empty = 0;
}while(1); }while(1);
mesgAddText(&ServerMessages, "thread> Send message 'list' to thread marked with (list) to get commands list");
if(empty) return "No threads"; if(empty) return "No threads";
return NULL; return NULL;
} }
@ -130,6 +161,13 @@ static const char *unregthr(char *thrname, _U_ char *data){
*/ */
static const char *sendmsg(char *thrname, char *data){ static const char *sendmsg(char *thrname, char *data){
FNAME(); FNAME();
if(!data || strlen(data) == 0) return ANS_WRONGMESG;
char *ptr = data, c = 0;
while(*ptr){ // check that message not empty
c = *ptr++;
if(c > ' ') break;
}
if(c <= ' ') return ANS_WRONGMESG;
threadinfo *ti = findThreadByName(thrname); threadinfo *ti = findThreadByName(thrname);
if(!ti) return ANS_NOTFOUND; if(!ti) return ANS_NOTFOUND;
if(!mesgAddText(&ti->commands, data)) return ANS_CANTSEND; if(!mesgAddText(&ti->commands, data)) return ANS_CANTSEND;

6
canserver/pusirobot.c
View File

@ -22,14 +22,12 @@
// we should init constants here! // we should init constants here!
#undef DICENTRY #undef DICENTRY
#define DICENTRY(name, idx, sidx, sz, s, n) const SDO_dic_entry name = {idx, sidx, sz, s, n}; #define DICENTRY(name, idx, sidx, sz, s, n, v) const SDO_dic_entry name = {idx, sidx, sz, s, n, v};
#include "dicentries.in" #include "dicentries.in"
// now init array with all dictionary // now init array with all dictionary
#undef DICENTRY #undef DICENTRY
#define nnn(nm) nm #define DICENTRY(name, idx, sidx, sz, s, n, v) &name,
#define lnk(nm) & ## nnn(nm)
#define DICENTRY(name, idx, sidx, sz, s, n) &name,
const SDO_dic_entry* allrecords[] = { const SDO_dic_entry* allrecords[] = {
#include "dicentries.in" #include "dicentries.in"
}; };

6
canserver/pusirobot.h
View File

@ -28,11 +28,11 @@ typedef struct{
uint8_t datasize; // data size: 1,2,3 or 4 bytes uint8_t datasize; // data size: 1,2,3 or 4 bytes
uint8_t issigned; // signess: if issigned==1, then signed, else unsigned uint8_t issigned; // signess: if issigned==1, then signed, else unsigned
const char *name; // dictionary entry name const char *name; // dictionary entry name
const char *varname;// variable name for output
} SDO_dic_entry; } SDO_dic_entry;
#ifndef DICENTRY #undef DICENTRY
#define DICENTRY(name, idx, sidx, sz, s, n) extern const SDO_dic_entry name; #define DICENTRY(name, idx, sidx, sz, s, n, v) extern const SDO_dic_entry name;
#endif
#include "dicentries.in" #include "dicentries.in"

6
canserver/socket.c
View File

@ -122,8 +122,10 @@ static void *server(void *asock){
close(fd); close(fd);
DBG("Client with fd %d closed", fd); DBG("Client with fd %d closed", fd);
LOGMSG("Client %d disconnected", fd); LOGMSG("Client %d disconnected", fd);
for(int i = fdidx; i < nfd; ++i) // move last to free space
poll_set[i] = poll_set[i + 1]; poll_set[fdidx] = poll_set[nfd - 1];
//for(int i = fdidx; i < nfd-1; ++i)
// poll_set[i] = poll_set[i + 1];
--nfd; --nfd;
} }
}else{ // server }else{ // server

2
canserver/threadlist.c
View File

@ -146,7 +146,7 @@ void *mesgAddObj(message *msg, void *data, size_t size){
*/ */
char *mesgAddText(message *msg, char *txt){ char *mesgAddText(message *msg, char *txt){
if(!txt) return NULL; if(!txt) return NULL;
DBG("mesg add text '%s'", txt); DBG("mesgAddText(%s)", txt);
size_t l = strlen(txt) + 1; size_t l = strlen(txt) + 1;
return mesgAddObj(msg, (void*)txt, l); return mesgAddObj(msg, (void*)txt, l);
} }

1
canserver/threadlist.h
View File

@ -42,6 +42,7 @@ typedef struct{
typedef struct{ typedef struct{
const char *name; // handler name const char *name; // handler name
void *(*handler)(void *); // handler function void *(*handler)(void *); // handler function
const char *helpmesg; // help message
} thread_handler; } thread_handler;
// thread information // thread information