mirror of
https://github.com/eddyem/pusirobot.git
synced 2025-12-06 02:25:10 +03:00
start adding simple stepper
This commit is contained in:
parent
2502c071cc
commit
b0f487ce3f
@ -8,3 +8,13 @@ mesg x 10 0x6001 0
|
||||
|
||||
#emul
|
||||
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");
|
||||
|
||||
|
||||
@ -196,5 +196,6 @@ int str2long(char *str, long* l){
|
||||
long n = strtol(str, &eptr, 0);
|
||||
if(*eptr) return 2; // wrong symbols in number
|
||||
if(l) *l = n;
|
||||
//DBG("str '%s' to long %ld", str, n);
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -24,12 +24,6 @@
|
||||
|
||||
#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"},
|
||||
@ -65,21 +59,19 @@ static const abortcodes AC[] = {
|
||||
|
||||
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
|
||||
* @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;
|
||||
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;
|
||||
return &AC[idx];
|
||||
}else if(c > abortcode){
|
||||
newidx = (idx + min_)/2;
|
||||
max_ = idx;
|
||||
@ -89,37 +81,43 @@ const char *abortcode_text(uint32_t abortcode){ //, int *n){
|
||||
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);
|
||||
// used
|
||||
/**
|
||||
* @brief mkMesg - make CAN message from sdo object
|
||||
* @param sdo (i) - sdo object to transform
|
||||
* @param mesg (o) - output CANmesg
|
||||
* @return pointer to mesg
|
||||
*/
|
||||
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
|
||||
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[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;
|
||||
return &mesg;
|
||||
mesg->data[1] = sdo->index & 0xff; // l
|
||||
mesg->data[2] = (sdo->index >> 8) & 0xff; // h
|
||||
mesg->data[3] = sdo->subindex;
|
||||
return mesg;
|
||||
}
|
||||
|
||||
// used
|
||||
/**
|
||||
* @brief parseSDO - transform CAN-message to SDO
|
||||
* @param mesg (i) - message
|
||||
* @param sdo (o) - SDO
|
||||
* @return sdo or NULL depending on result
|
||||
*/
|
||||
SDO *parseSDO(CANmesg *mesg, SDO *sdo){
|
||||
SDO *parseSDO(const CANmesg *mesg, SDO *sdo){
|
||||
if(mesg->len != 8){
|
||||
WARNX("Wrong SDO data length");
|
||||
return NULL;
|
||||
@ -137,7 +135,7 @@ SDO *parseSDO(CANmesg *mesg, SDO *sdo){
|
||||
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];
|
||||
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);
|
||||
return sdo;
|
||||
@ -151,8 +149,8 @@ static int ask2read(uint16_t idx, uint8_t subidx, uint8_t NID){
|
||||
sdo.datalen = 0;
|
||||
sdo.index = idx;
|
||||
sdo.subindex = subidx;
|
||||
CANmesg *mesg = mkMesg(&sdo);
|
||||
return canbus_write(mesg);
|
||||
CANmesg mesg;
|
||||
return canbus_write(mkMesg(&sdo, &mesg));
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
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));
|
||||
}
|
||||
|
||||
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));
|
||||
}
|
||||
|
||||
static inline uint8_t mku8(uint8_t data[4]){
|
||||
static inline uint8_t mku8(const uint8_t data[4]){
|
||||
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));
|
||||
}
|
||||
|
||||
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));
|
||||
}
|
||||
|
||||
static inline int8_t mki8(uint8_t data[4]){
|
||||
static inline int8_t mki8(const 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;
|
||||
if(!readSDOvalue(e->index, e->subindex, NID, &sdo)){
|
||||
return INT64_MIN;
|
||||
}
|
||||
if(sdo.ccs == CCS_ABORT_TRANSFER){ // error
|
||||
// used
|
||||
/**
|
||||
* @brief getSDOval - get value from SDO
|
||||
* @param sdo (i) - SDO
|
||||
* @param e (i) - its dictionary entry
|
||||
* @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
|
||||
*/
|
||||
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);
|
||||
uint32_t ac = mku32(sdo.data);
|
||||
const char *etxt = abortcode_text(ac);
|
||||
if(etxt) WARNX("Abort code 0x%X: %s", ac, etxt);
|
||||
uint32_t c = mku32(sdo->data);
|
||||
const abortcodes *abc = abortcode_search(c);
|
||||
if(abc) WARNX("Abort code 0x%X: %s", ac, abc->errmsg);
|
||||
if(ac) *ac = abc;
|
||||
return INT64_MIN;
|
||||
}
|
||||
if(sdo.datalen != e->datasize){
|
||||
WARNX("Got SDO with length %d instead of %d (as in dictionary)", sdo.datalen, e->datasize);
|
||||
if(sdo->datalen == 0) return INT_MAX;
|
||||
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){
|
||||
switch(sdo->datalen){
|
||||
case 1:
|
||||
ans = mki8(sdo.data);
|
||||
ans = mki8(sdo->data);
|
||||
break;
|
||||
case 4:
|
||||
ans = mki32(sdo.data);
|
||||
ans = mki32(sdo->data);
|
||||
break;
|
||||
default: // can't be 3! 3->2
|
||||
ans = mki16(sdo.data);
|
||||
ans = mki16(sdo->data);
|
||||
}
|
||||
}else{
|
||||
switch(sdo.datalen){
|
||||
switch(sdo->datalen){
|
||||
case 1:
|
||||
ans = mku8(sdo.data);
|
||||
ans = mku8(sdo->data);
|
||||
break;
|
||||
case 4:
|
||||
ans = mku32(sdo.data);
|
||||
ans = mku32(sdo->data);
|
||||
break;
|
||||
default: // can't be 3! 3->2
|
||||
ans = mku16(sdo.data);
|
||||
ans = mku16(sdo->data);
|
||||
}
|
||||
}
|
||||
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
|
||||
int SDO_writeArr(const SDO_dic_entry *e, uint8_t NID, const uint8_t *data){
|
||||
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];
|
||||
sdo.index = e->index;
|
||||
sdo.subindex = e->subindex;
|
||||
CANmesg *mesgp = mkMesg(&sdo);
|
||||
CANmesg mesgp;
|
||||
mkMesg(&sdo, &mesgp);
|
||||
DBG("Canbus write..");
|
||||
if(canbus_write(mesgp)){
|
||||
if(canbus_write(&mesgp)){
|
||||
WARNX("SDO_write(): Can't initiate download");
|
||||
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
|
||||
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);
|
||||
const abortcodes *e = abortcode_search(ac);
|
||||
if(e) WARNX("Abort code 0x%X: %s", ac, e->errmsg);
|
||||
return 4;
|
||||
}
|
||||
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;
|
||||
}
|
||||
|
||||
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
|
||||
|
||||
@ -77,14 +77,18 @@ typedef struct{
|
||||
uint8_t ccs; // Client command specifier
|
||||
} SDO;
|
||||
|
||||
const char *abortcode_text(uint32_t abortcode);
|
||||
SDO *parseSDO(CANmesg *mesg, SDO *sdo);
|
||||
typedef struct{
|
||||
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);
|
||||
|
||||
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);
|
||||
int64_t getSDOval(const SDO *sdo, const SDO_dic_entry *e, const abortcodes **ac);
|
||||
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_readByte(uint16_t idx, uint8_t subidx, uint8_t *data, uint8_t NID);
|
||||
#endif // CANOPEN_H__
|
||||
|
||||
@ -21,123 +21,78 @@
|
||||
// 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")
|
||||
|
||||
DICENTRY(HEARTBTTIME, 0x1017, 0, 2, 0, "heartbeat time", "hearbt")
|
||||
// node ID
|
||||
DICENTRY(NODEID, 0x2002, 0, 1, 0, "node ID")
|
||||
DICENTRY(NODEID, 0x2002, 0, 1, 0, "node ID", "nodeid")
|
||||
// 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
|
||||
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
|
||||
DICENTRY(ERRSTATE, 0x6000, 0, 1, 0, "error status")
|
||||
DICENTRY(ERRSTATE, 0x6000, 0, 1, 0, "error status", "errstatus")
|
||||
// controller status
|
||||
DICENTRY(DEVSTATUS, 0x6001, 0, 1, 0, "controller status")
|
||||
DICENTRY(DEVSTATUS, 0x6001, 0, 1, 0, "controller status", "devstatus")
|
||||
// rotation direction
|
||||
DICENTRY(ROTDIR, 0x6002, 0, 1, 0, "rotation direction")
|
||||
DICENTRY(ROTDIR, 0x6002, 0, 1, 0, "rotation direction", "rotdir")
|
||||
// maximal speed
|
||||
DICENTRY(MAXSPEED, 0x6003, 0, 4, 1, "maximal speed")
|
||||
DICENTRY(MAXSPEED, 0x6003, 0, 4, 1, "maximal speed", "maxspeed")
|
||||
// relative displacement
|
||||
DICENTRY(RELSTEPS, 0x6004, 0, 4, 0, "relative displacement")
|
||||
DICENTRY(RELSTEPS, 0x6004, 0, 4, 0, "relative displacement", "relsteps")
|
||||
// operation mode
|
||||
DICENTRY(OPMODE, 0x6005, 0, 1, 0, "operation mode")
|
||||
DICENTRY(OPMODE, 0x6005, 0, 1, 0, "operation mode", "opmode")
|
||||
// start speed
|
||||
DICENTRY(STARTSPEED, 0x6006, 0, 2, 0, "start speed")
|
||||
DICENTRY(STARTSPEED, 0x6006, 0, 2, 0, "start speed", "startspd")
|
||||
// stop speed
|
||||
DICENTRY(STOPSPEED, 0x6007, 0, 2, 0, "stop speed")
|
||||
DICENTRY(STOPSPEED, 0x6007, 0, 2, 0, "stop speed", "stopspd")
|
||||
// acceleration coefficient
|
||||
DICENTRY(ACCELCOEF, 0x6008, 0, 1, 0, "acceleration coefficient")
|
||||
DICENTRY(ACCELCOEF, 0x6008, 0, 1, 0, "acceleration coefficient", "acccoef")
|
||||
// deceleration coefficient
|
||||
DICENTRY(DECELCOEF, 0x6009, 0, 1, 0, "deceleration coefficient")
|
||||
DICENTRY(DECELCOEF, 0x6009, 0, 1, 0, "deceleration coefficient", "deccoef")
|
||||
// microstepping
|
||||
DICENTRY(MICROSTEPS, 0x600A, 0, 2, 0, "microstepping")
|
||||
DICENTRY(MICROSTEPS, 0x600A, 0, 2, 0, "microstepping", "microsteps")
|
||||
// max current
|
||||
DICENTRY(MAXCURNT, 0x600B, 0, 2, 0, "maximum phase current")
|
||||
DICENTRY(MAXCURNT, 0x600B, 0, 2, 0, "maximum phase current", "maxcurnt")
|
||||
// current position
|
||||
DICENTRY(POSITION, 0x600C, 0, 4, 0, "current position")
|
||||
DICENTRY(POSITION, 0x600C, 0, 4, 0, "current position", "curpos")
|
||||
// current reduction
|
||||
DICENTRY(CURRREDUCT, 0x600D, 0, 1, 0, "current reduction")
|
||||
DICENTRY(CURRREDUCT, 0x600D, 0, 1, 0, "current reduction", "curred")
|
||||
// motor enable
|
||||
DICENTRY(ENABLE, 0x600E, 0, 1, 0, "motor enable")
|
||||
DICENTRY(ENABLE, 0x600E, 0, 1, 0, "motor enable", "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
|
||||
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
|
||||
DICENTRY(EXTSENSTYPE, 0x600F, 3, 1, 0, "EXT emergency sensor type")
|
||||
DICENTRY(EXTSENSTYPE, 0x600F, 3, 1, 0, "EXT emergency sensor type", "extsenstype")
|
||||
// GPIO direction
|
||||
DICENTRY(GPIODIR, 0x6011, 1, 2, 0, "GPIO direction")
|
||||
DICENTRY(GPIODIR, 0x6011, 1, 2, 0, "GPIO direction", "gpiodir")
|
||||
// GPIO configuration
|
||||
DICENTRY(GPIOCONF, 0x6011, 2, 4, 0, "GPIO configuration")
|
||||
DICENTRY(GPIOCONF, 0x6011, 2, 4, 0, "GPIO configuration", "gpioconf")
|
||||
// GPIO value
|
||||
DICENTRY(GPIOVAL, 0x6012, 0, 2, 0, "GPIO value")
|
||||
DICENTRY(GPIOVAL, 0x6012, 0, 2, 0, "GPIO value", "gpioval")
|
||||
// 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
|
||||
DICENTRY(OFFLNMBR, 0x6018, 1, 1, 0, "Number of offline programming command")
|
||||
DICENTRY(OFFLENBL, 0x6018, 2, 1, 0, "Offline automatic operation enable")
|
||||
DICENTRY(OFFLNMBR, 0x6018, 1, 1, 0, "Number of offline programming command", "offlnmbr")
|
||||
DICENTRY(OFFLENBL, 0x6018, 2, 1, 0, "Offline automatic operation enable", "offlenbl")
|
||||
// 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
|
||||
DICENTRY(ABSSTEPS, 0x601C, 0, 4, 1, "absolute displacement")
|
||||
DICENTRY(ABSSTEPS, 0x601C, 0, 4, 1, "absolute displacement", "abssteps")
|
||||
// stop motor
|
||||
DICENTRY(STOP, 0x6020, 0, 1, 0, "stop motor")
|
||||
DICENTRY(STOP, 0x6020, 0, 1, 0, "stop motor", "stop")
|
||||
// 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
|
||||
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
|
||||
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
|
||||
DICENTRY(POSAUTOSAVE, 0x602A, 0, 1, 0, "autosave position (closed loop)")
|
||||
DICENTRY(POSAUTOSAVE, 0x602A, 0, 1, 0, "autosave position (closed loop)", "autosave")
|
||||
// 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
|
||||
DICENTRY(CALIBZERO, 0x6034, 0, 4, 1, "calibration zero")
|
||||
DICENTRY(CALIBZERO, 0x6034, 0, 4, 1, "calibration zero", "calibzero")
|
||||
// encoder position
|
||||
DICENTRY(ENCPOS, 0x6035, 0, 4, 1, "encoder position")
|
||||
DICENTRY(ENCPOS, 0x6035, 0, 4, 1, "encoder position", "encpos")
|
||||
|
||||
|
||||
@ -24,6 +24,7 @@
|
||||
#include "socket.h"
|
||||
|
||||
#include <fcntl.h> // open
|
||||
#include <inttypes.h> // PRId64
|
||||
#include <stdio.h> // printf
|
||||
#include <string.h> // strcmp
|
||||
#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 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
|
||||
// messages: master - thread, slave - caller
|
||||
static void *stpemulator(void *arg);
|
||||
static void *rawcommands(void *arg);
|
||||
static void *canopencmds(void *arg);
|
||||
static void *simplestp(void *arg);
|
||||
|
||||
// handlers for standard types
|
||||
thread_handler CANhandlers[] = {
|
||||
{"emulation", stpemulator},
|
||||
{"raw", rawcommands},
|
||||
{"canopen", canopencmds},
|
||||
{NULL, NULL}
|
||||
{"canopen", canopencmds, "NodeID index subindex [data] - raw CANOpen commands with `index` and `subindex` to `NodeID`"},
|
||||
{"emulation", stpemulator, "(list) - 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"},
|
||||
{NULL, NULL, NULL}
|
||||
};
|
||||
|
||||
thread_handler *get_handler(const char *name){
|
||||
@ -59,6 +71,51 @@ thread_handler *get_handler(const char *name){
|
||||
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
|
||||
* @param packet (o) - pointer to CANpacket or NULL (just to check)
|
||||
@ -116,29 +173,10 @@ static void reopen_device(){
|
||||
static void processCANmessage(CANmesg *mesg){
|
||||
threadinfo *ti = findThreadByID(0);
|
||||
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));
|
||||
}
|
||||
ti = findThreadByID(mesg->ID);
|
||||
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));
|
||||
}
|
||||
}
|
||||
@ -161,7 +199,7 @@ void *CANserver(_U_ void *data){
|
||||
FREE(msg);
|
||||
}
|
||||
usleep(1000);
|
||||
CANmesg cm;
|
||||
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);
|
||||
processCANmessage(&cm);
|
||||
@ -254,7 +292,7 @@ static void sendSDO(char *mesg){
|
||||
|
||||
CANmesg comesg;
|
||||
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;
|
||||
if(datalen){ // there's data
|
||||
comesg.data[0] |= SDO_N(datalen) | SDO_E | SDO_S;
|
||||
@ -284,8 +322,8 @@ static void *canopencmds(void *arg){
|
||||
if(parseSDO(ans, &sdo)){
|
||||
char buf[128], *ptr = buf;
|
||||
int rest = 128;
|
||||
int l = snprintf(ptr, rest, "SDO={nid=0x%02X, idx=0x%04X, subidx=%d, ccs=0x%02X, datalen=%d",
|
||||
sdo.NID, sdo.index, sdo.subindex, sdo.ccs, sdo.datalen);
|
||||
int l = snprintf(ptr, rest, "%s nid=0x%02X, idx=0x%04X, subidx=%d, ccs=0x%02X, datalen=%d",
|
||||
ti->name, sdo.NID, sdo.index, sdo.subindex, sdo.ccs, sdo.datalen);
|
||||
ptr += l; rest -= l;
|
||||
if(sdo.datalen){
|
||||
l = snprintf(ptr, rest, ", data=[");
|
||||
@ -298,7 +336,6 @@ static void *canopencmds(void *arg){
|
||||
l = snprintf(ptr, rest, "]");
|
||||
ptr += l; rest -= l;
|
||||
}
|
||||
snprintf(ptr, rest, "}");
|
||||
mesgAddText(&ServerMessages, buf);
|
||||
}
|
||||
FREE(ans);
|
||||
@ -309,6 +346,96 @@ static void *canopencmds(void *arg){
|
||||
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
|
||||
|
||||
@ -32,7 +32,10 @@ static const char *ANS_OK = "OK";
|
||||
static const char *ANS_WRONGCANID = "Wrong CANID";
|
||||
static const char *ANS_NOTFOUND = "Thread not found";
|
||||
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 *regthr(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`
|
||||
*/
|
||||
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 *helpmesg; // help message
|
||||
} cmditem;
|
||||
|
||||
// array with known functions
|
||||
static cmditem functions[] = {
|
||||
{"list", listthr}, // list threads
|
||||
{"mesg", sendmsg}, // "mesg NAME ID [data]"
|
||||
{"register", regthr}, // "register NAME ID", ID - RAW CAN ID (not canopen ID)!!!
|
||||
{"speed", setspd}, // set CANbus speed
|
||||
{"unregister", unregthr}, // "unregister NAME"
|
||||
{NULL, NULL}
|
||||
{"help", shelp, "- show help"},
|
||||
{"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`"},
|
||||
{"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
|
||||
static const char *listthr(_U_ char *par1, _U_ char *par2){
|
||||
FNAME();
|
||||
@ -72,10 +102,11 @@ static const char *listthr(_U_ char *par1, _U_ char *par2){
|
||||
do{
|
||||
list = nextThread(list);
|
||||
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);
|
||||
empty = 0;
|
||||
}while(1);
|
||||
mesgAddText(&ServerMessages, "thread> Send message 'list' to thread marked with (list) to get commands list");
|
||||
if(empty) return "No threads";
|
||||
return NULL;
|
||||
}
|
||||
@ -130,6 +161,13 @@ static const char *unregthr(char *thrname, _U_ char *data){
|
||||
*/
|
||||
static const char *sendmsg(char *thrname, char *data){
|
||||
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);
|
||||
if(!ti) return ANS_NOTFOUND;
|
||||
if(!mesgAddText(&ti->commands, data)) return ANS_CANTSEND;
|
||||
|
||||
@ -22,14 +22,12 @@
|
||||
|
||||
// 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};
|
||||
#define DICENTRY(name, idx, sidx, sz, s, n, v) const SDO_dic_entry name = {idx, sidx, sz, s, n, v};
|
||||
#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,
|
||||
#define DICENTRY(name, idx, sidx, sz, s, n, v) &name,
|
||||
const SDO_dic_entry* allrecords[] = {
|
||||
#include "dicentries.in"
|
||||
};
|
||||
|
||||
@ -28,11 +28,11 @@ typedef struct{
|
||||
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
|
||||
const char *varname;// variable name for output
|
||||
} SDO_dic_entry;
|
||||
|
||||
#ifndef DICENTRY
|
||||
#define DICENTRY(name, idx, sidx, sz, s, n) extern const SDO_dic_entry name;
|
||||
#endif
|
||||
#undef DICENTRY
|
||||
#define DICENTRY(name, idx, sidx, sz, s, n, v) extern const SDO_dic_entry name;
|
||||
|
||||
#include "dicentries.in"
|
||||
|
||||
|
||||
@ -122,8 +122,10 @@ static void *server(void *asock){
|
||||
close(fd);
|
||||
DBG("Client with fd %d closed", fd);
|
||||
LOGMSG("Client %d disconnected", fd);
|
||||
for(int i = fdidx; i < nfd; ++i)
|
||||
poll_set[i] = poll_set[i + 1];
|
||||
// move last to free space
|
||||
poll_set[fdidx] = poll_set[nfd - 1];
|
||||
//for(int i = fdidx; i < nfd-1; ++i)
|
||||
// poll_set[i] = poll_set[i + 1];
|
||||
--nfd;
|
||||
}
|
||||
}else{ // server
|
||||
|
||||
@ -146,7 +146,7 @@ void *mesgAddObj(message *msg, void *data, size_t size){
|
||||
*/
|
||||
char *mesgAddText(message *msg, char *txt){
|
||||
if(!txt) return NULL;
|
||||
DBG("mesg add text '%s'", txt);
|
||||
DBG("mesgAddText(%s)", txt);
|
||||
size_t l = strlen(txt) + 1;
|
||||
return mesgAddObj(msg, (void*)txt, l);
|
||||
}
|
||||
|
||||
@ -42,6 +42,7 @@ typedef struct{
|
||||
typedef struct{
|
||||
const char *name; // handler name
|
||||
void *(*handler)(void *); // handler function
|
||||
const char *helpmesg; // help message
|
||||
} thread_handler;
|
||||
|
||||
// thread information
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user