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

fix deadlock (at least, I think so)

Browse Source
This commit is contained in:
Edward Emelianov 2025-07-01 23:34:54 +03:00
parent d9124740ec
commit 71c562ea7e
3 changed files with 39 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
Z1000_focus/canopen.c
View File

@ -16,14 +16,15 @@ int sendNMT(int node, int icode){
}
int resetNode2(int oldnode, int newnode){
int idr,dlen,ntout=50;
int dlen,ntout=50;
canid_t idr;
unsigned char rdata[8];
can_clean_recv(&rxpnt, &rxtime);
if(!sendNMT(oldnode, 0x81)) return 0;
for(int i=0; i < ntout; ++i){
can_dsleep(0.01);
while(can_recv_frame(&rxpnt, &rxtime, &idr, &dlen, rdata)){
if(idr == (0x700|newnode) && dlen==1 && rdata[0] == 0) return 1;
if(idr == (0x700|((canid_t)newnode)) && dlen==1 && rdata[0] == 0) return 1;
}
}
return 0;
@ -34,14 +35,15 @@ int resetNode(int node){return resetNode2(node,node);}
int getNodeState(int node){
/* use Node Guarding Protocol */
unsigned long idt = (0x700 | (node&0x7f) | CAN_RTR_FLAG);
int idr = 0, dlen = 0, ntout = 15;
int dlen = 0, ntout = 15;
canid_t idr;
unsigned char rdata[8];
can_clean_recv(&rxpnt, &rxtime);
if(can_send_frame(idt, dlen, rdata)<=0) return 0;
for(int i=0; i < ntout; ++i){
can_dsleep(0.01);
while(can_recv_frame(&rxpnt, &rxtime, &idr, &dlen, rdata)) {
if(idr == (0x700|node) && dlen == 1) return rdata[0]&0x7f;
if(idr == (0x700|((canid_t)node)) && dlen == 1) return rdata[0]&0x7f;
}
}
return 0;
@ -95,12 +97,13 @@ int sendSDOreq(int node, int object, int subindex){
int recvSDOresp(int node, int t_func, int t_object, int t_subindex, unsigned char data[]){
int idt = 0x580|(node&0x7f);
int idr = 0, dlen = 0;
int dlen = 0;
canid_t idr;
unsigned char rdata[8] = {0};
int ntout = (t_object == 0x1010||t_object == 0x1011)? 50 : 15;
for(int i = 0; i < ntout; ++i){
can_dsleep(0.01);
while(can_recv_frame(&rxpnt, &rxtime, &idr, &dlen, rdata)) if(idr==idt){
while(can_recv_frame(&rxpnt, &rxtime, &idr, &dlen, rdata)) if(idr==(canid_t)idt){
int r_func, r_object, r_subindex;
if(dlen < 4){
fprintf(stderr,"Too short SDO response from Node%d\n",node&0x7f);
@ -240,7 +243,8 @@ int recvNextPDO(double tout, int *node, unsigned long *value){
// wait up to 'tout' sec. for the one next PDO
// if ok - return 1 for PDO1 or 2 for PDO2; else 0 if timeout
double te = can_dtime()+tout;
int idr=0, dlen=0, pdon = 0;
int dlen=0, pdon = 0;
canid_t idr;
unsigned char rdata[8] = {0};
do{
while(can_recv_frame(&rxpnt, &rxtime, &idr, &dlen, rdata)){
@ -262,7 +266,8 @@ int recvPDOs(double tout, int maxpdo, int node[], int pdo_n[], unsigned long val
// wait up to 'tout' sec. for the array of 'maxpdo' PDOs
// if ok - return number of PDO really received; else 0 if timeout
double te = can_dtime()+tout;
int npdo=0, idr=0, dlen=0, pdon = 0;
int npdo=0, dlen=0, pdon = 0;
canid_t idr;
unsigned char rdata[8] = {0};
do{
while(can_recv_frame(&rxpnt, &rxtime, &idr, &dlen, rdata)){

3
Z1000_focus/main.c
View File

@ -48,6 +48,7 @@ int verbose(const char *fmt, ...){
void signals(int signo){
unlink_pidfile();
putlog("Get signal %d, exit", signo);
can_exit(signo);
}
@ -92,11 +93,13 @@ int main (int argc, char *argv[]){
can_dev[8] = '1';
if(G->server){ // daemonize & run server
/*
#ifndef EBUG
if(daemon(1, 0)){
ERR("daemon()");
}
#endif
*/
check4running(G->pidfilename);
#ifndef EBUG
while(1){ // guard for dead processes

54
Z1000_focus/socket.c
View File

@ -59,7 +59,7 @@ static int waittoread(int sock){
rc = select(sock+1, &fds, NULL, NULL, &timeout);
if(rc < 0){
if(errno != EINTR){
WARN("select()");
//WARN("select()");
return -1;
}
continue;
@ -72,8 +72,7 @@ static int waittoread(int sock){
/**************** SERVER FUNCTIONS ****************/
// `canbus_mutex` used to exclude simultaneous CAN messages
// `moving_mutex` used to block simultaneous attempts to move motor
static pthread_mutex_t canbus_mutex = PTHREAD_MUTEX_INITIALIZER, moving_mutex = PTHREAD_MUTEX_INITIALIZER;
static pthread_mutex_t canbus_mutex = PTHREAD_MUTEX_INITIALIZER;
bool emerg_stop = FALSE;
/**
@ -103,7 +102,7 @@ static int send_data(int sock, int webquery, char *buf){
}
ssize_t W = write(sock, tbuf, L);
if(L != W){
WARN("write header: %zd instead of %zd", W, L);
//WARN("write header: %zd instead of %zd", W, L);
return 0;
}
}
@ -143,7 +142,6 @@ static void *move_focus(void *targpos){
if(move2pos(pos)) go_out_from_ESW();
ismoving = 0;
putlog("Focus value: %.03f", curPos());
pthread_mutex_unlock(&moving_mutex);
pthread_mutex_unlock(&canbus_mutex);
pthread_exit(NULL);
return NULL;
@ -151,13 +149,12 @@ static void *move_focus(void *targpos){
static const char *startmoving(double pos){
static double sp;
if(pthread_mutex_trylock(&moving_mutex)) return S_ANS_MOVING;
pthread_t m_thread;
if(ismoving) return S_ANS_MOVING;
DBG("startmoving: %g", pos);
sp = pos;
if(pthread_create(&m_thread, NULL, move_focus, (void*) &sp)){
WARN("pthread_create()");
pthread_mutex_unlock(&moving_mutex);
return S_ANS_ERR;
}else{
DBG("Thread created, detouch");
@ -174,16 +171,13 @@ static void *ego(_U_ void *unused){
pthread_mutex_lock(&canbus_mutex);
go_out_from_ESW();
pthread_mutex_unlock(&canbus_mutex);
pthread_mutex_unlock(&moving_mutex);
pthread_exit(NULL);
return NULL;
}
static void getoutESW(){
pthread_mutex_lock(&moving_mutex);
pthread_t m_thread;
if(pthread_create(&m_thread, NULL, ego, NULL)){
WARN("pthread_create()");
pthread_mutex_unlock(&moving_mutex);
}else{
DBG("Thread created, detouch");
pthread_detach(m_thread); // don't care about thread state
@ -193,6 +187,7 @@ static void getoutESW(){
static void *handle_socket(void *asock){
#define getparam(x) (strncmp(found, x, sizeof(x)-1) == 0)
int sock = *((int*)asock);
putlog("[DBG] handle_socket: run for client %d", sock);
int webquery = 0; // whether query is web or regular
char buff[BUFLEN];
ssize_t rd;
@ -240,21 +235,18 @@ static void *handle_socket(void *asock){
FOCMIN_MM, FOCMAX_MM, MINSPEED, MAXSPEED);
}else if(getparam(S_CMD_STOP)){
DBG("Stop request");
emerg_stop = TRUE;
pthread_mutex_lock(&canbus_mutex);
pthread_mutex_lock(&moving_mutex);
emerg_stop = TRUE;
if(stop()) sprintf(buff, S_ANS_ERR);
else sprintf(buff, S_ANS_OK);
emerg_stop = FALSE;
putlog("%s: request to stop @ %.03f", peerIP, curPos());
pthread_mutex_unlock(&moving_mutex);
pthread_mutex_unlock(&canbus_mutex);
}else if(getparam(S_CMD_TARGSPEED)){
char *ch = strchr(found, '=');
double spd;
if(pthread_mutex_trylock(&moving_mutex)) sprintf(buff, S_ANS_MOVING);
if(pthread_mutex_trylock(&canbus_mutex)) sprintf(buff, S_ANS_MOVING);
else{
pthread_mutex_lock(&canbus_mutex);
if(!ch || !str2double(&spd, ch+1) || fabs(spd) < MINSPEED || fabs(spd) > MAXSPEED || movewconstspeed(spd)) sprintf(buff, S_ANS_ERR);
else{
DBG("Move with constant speed %g request", spd);
@ -262,7 +254,6 @@ static void *handle_socket(void *asock){
sprintf(buff, S_ANS_OK);
}
pthread_mutex_unlock(&canbus_mutex);
pthread_mutex_unlock(&moving_mutex);
}
}else if(getparam(S_CMD_GOTO)){
char *ch = strchr(found, '=');
@ -303,10 +294,12 @@ static void *handle_socket(void *asock){
sprintf(buff, "%s", msg);
}else sprintf(buff, S_ANS_ERR);
if(!send_data(sock, webquery, buff)){
WARNX("can't send data to %s, some error occured", peerIP);
break;
//WARNX("can't send data to %s, some error occured", peerIP);
}
}
FREE(peerIP);
putlog("[DBG] socket %d closed", sock);
close(sock);
pthread_exit(NULL);
return NULL;
@ -317,7 +310,7 @@ static void *handle_socket(void *asock){
static void *server(void *asock){
int sock = *((int*)asock);
if(listen(sock, BACKLOG) == -1){
//WARN("listen() failed");
WARN("listen() failed");
return NULL;
}
while(1){
@ -327,13 +320,15 @@ static void *server(void *asock){
int w = waittoread(sock);
if(w == 0) continue;
else if(w < 0) break;
putlog("[DBG] server(): try to accept()");
newsock = accept(sock, (struct sockaddr*)&their_addr, &size);
if(newsock <= 0){
WARN("accept() failed");
continue;
ERR("accept() failed");
break;
}
struct sockaddr_in* pV4Addr = (struct sockaddr_in*)&their_addr;
struct in_addr ipAddr = pV4Addr->sin_addr;
addtolog("\t\taccept() OK. fd=%d", newsock);
struct sockaddr_in* ipV4Addr = (struct sockaddr_in*)&their_addr;
struct in_addr ipAddr = ipV4Addr->sin_addr;
char str[INET_ADDRSTRLEN];
inet_ntop(AF_INET, &ipAddr, str, INET_ADDRSTRLEN);
//DBG("Got connection from %s", str);
@ -345,26 +340,23 @@ static void *server(void *asock){
pthread_detach(handler_thread); // don't care about thread state
}
}
putlog("UNREACHABLE CODE REACHED!");
putlog("server(): UNREACHABLE CODE REACHED!");
return NULL;
}
// refresh file with focus value
static void subst_file(char *name){
char aname[FILENAME_MAX];
if(!name) return;
int l = strlen(name) + 7;
char *aname = MALLOC(char, l);
snprintf(aname, l, "%sXXXXXX", name);
snprintf(aname, FILENAME_MAX-1, "%sXXXXXX", name);
int fd = mkstemp(aname);
if(fd < 0) goto ret;
if(fd < 0) return;
fchmod(fd, 0644);
FILE *f = fdopen(fd, "w");
if(!f) goto ret;
if(!f){ close(fd); return; }
fprintf(f, "FOCUS = %.3f\n", curPos());
fclose(f);
rename(aname, name);
ret:
FREE(aname);
}
// data gathering & socket management
@ -437,7 +429,7 @@ void daemonize(const char *port){
}
if(p == NULL){
// looped off the end of the list with no successful bind
ERRX("failed to bind socket");
ERRX("daemonize(): failed to bind socket");
}
freeaddrinfo(res);
DBG("going to run daemon_()");