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 resetNode2(int oldnode, int newnode){
int idr,dlen,ntout=50; int dlen,ntout=50;
canid_t idr;
unsigned char rdata[8]; unsigned char rdata[8];
can_clean_recv(&rxpnt, &rxtime); can_clean_recv(&rxpnt, &rxtime);
if(!sendNMT(oldnode, 0x81)) return 0; if(!sendNMT(oldnode, 0x81)) return 0;
for(int i=0; i < ntout; ++i){ for(int i=0; i < ntout; ++i){
can_dsleep(0.01); can_dsleep(0.01);
while(can_recv_frame(&rxpnt, &rxtime, &idr, &dlen, rdata)){ 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; return 0;
@ -34,14 +35,15 @@ int resetNode(int node){return resetNode2(node,node);}
int getNodeState(int node){ int getNodeState(int node){
/* use Node Guarding Protocol */ /* use Node Guarding Protocol */
unsigned long idt = (0x700 | (node&0x7f) | CAN_RTR_FLAG); 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]; unsigned char rdata[8];
can_clean_recv(&rxpnt, &rxtime); can_clean_recv(&rxpnt, &rxtime);
if(can_send_frame(idt, dlen, rdata)<=0) return 0; if(can_send_frame(idt, dlen, rdata)<=0) return 0;
for(int i=0; i < ntout; ++i){ for(int i=0; i < ntout; ++i){
can_dsleep(0.01); can_dsleep(0.01);
while(can_recv_frame(&rxpnt, &rxtime, &idr, &dlen, rdata)) { 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; 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 recvSDOresp(int node, int t_func, int t_object, int t_subindex, unsigned char data[]){
int idt = 0x580|(node&0x7f); int idt = 0x580|(node&0x7f);
int idr = 0, dlen = 0; int dlen = 0;
canid_t idr;
unsigned char rdata[8] = {0}; unsigned char rdata[8] = {0};
int ntout = (t_object == 0x1010||t_object == 0x1011)? 50 : 15; int ntout = (t_object == 0x1010||t_object == 0x1011)? 50 : 15;
for(int i = 0; i < ntout; ++i){ for(int i = 0; i < ntout; ++i){
can_dsleep(0.01); 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; int r_func, r_object, r_subindex;
if(dlen < 4){ if(dlen < 4){
fprintf(stderr,"Too short SDO response from Node%d\n",node&0x7f); 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 // wait up to 'tout' sec. for the one next PDO
// if ok - return 1 for PDO1 or 2 for PDO2; else 0 if timeout // if ok - return 1 for PDO1 or 2 for PDO2; else 0 if timeout
double te = can_dtime()+tout; 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}; unsigned char rdata[8] = {0};
do{ do{
while(can_recv_frame(&rxpnt, &rxtime, &idr, &dlen, rdata)){ 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 // wait up to 'tout' sec. for the array of 'maxpdo' PDOs
// if ok - return number of PDO really received; else 0 if timeout // if ok - return number of PDO really received; else 0 if timeout
double te = can_dtime()+tout; 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}; unsigned char rdata[8] = {0};
do{ do{
while(can_recv_frame(&rxpnt, &rxtime, &idr, &dlen, rdata)){ 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){ void signals(int signo){
unlink_pidfile(); unlink_pidfile();
putlog("Get signal %d, exit", signo);
can_exit(signo); can_exit(signo);
} }
@ -92,11 +93,13 @@ int main (int argc, char *argv[]){
can_dev[8] = '1'; can_dev[8] = '1';
if(G->server){ // daemonize & run server if(G->server){ // daemonize & run server
/*
#ifndef EBUG #ifndef EBUG
if(daemon(1, 0)){ if(daemon(1, 0)){
ERR("daemon()"); ERR("daemon()");
} }
#endif #endif
*/
check4running(G->pidfilename); check4running(G->pidfilename);
#ifndef EBUG #ifndef EBUG
while(1){ // guard for dead processes 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); rc = select(sock+1, &fds, NULL, NULL, &timeout);
if(rc < 0){ if(rc < 0){
if(errno != EINTR){ if(errno != EINTR){
WARN("select()"); //WARN("select()");
return -1; return -1;
} }
continue; continue;
@ -72,8 +72,7 @@ static int waittoread(int sock){
/**************** SERVER FUNCTIONS ****************/ /**************** SERVER FUNCTIONS ****************/
// `canbus_mutex` used to exclude simultaneous CAN messages // `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;
static pthread_mutex_t canbus_mutex = PTHREAD_MUTEX_INITIALIZER, moving_mutex = PTHREAD_MUTEX_INITIALIZER;
bool emerg_stop = FALSE; 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); ssize_t W = write(sock, tbuf, L);
if(L != W){ if(L != W){
WARN("write header: %zd instead of %zd", W, L); //WARN("write header: %zd instead of %zd", W, L);
return 0; return 0;
} }
} }
@ -143,7 +142,6 @@ static void *move_focus(void *targpos){
if(move2pos(pos)) go_out_from_ESW(); if(move2pos(pos)) go_out_from_ESW();
ismoving = 0; ismoving = 0;
putlog("Focus value: %.03f", curPos()); putlog("Focus value: %.03f", curPos());
pthread_mutex_unlock(&moving_mutex);
pthread_mutex_unlock(&canbus_mutex); pthread_mutex_unlock(&canbus_mutex);
pthread_exit(NULL); pthread_exit(NULL);
return NULL; return NULL;
@ -151,13 +149,12 @@ static void *move_focus(void *targpos){
static const char *startmoving(double pos){ static const char *startmoving(double pos){
static double sp; static double sp;
if(pthread_mutex_trylock(&moving_mutex)) return S_ANS_MOVING;
pthread_t m_thread; pthread_t m_thread;
if(ismoving) return S_ANS_MOVING;
DBG("startmoving: %g", pos); DBG("startmoving: %g", pos);
sp = pos; sp = pos;
if(pthread_create(&m_thread, NULL, move_focus, (void*) &sp)){ if(pthread_create(&m_thread, NULL, move_focus, (void*) &sp)){
WARN("pthread_create()"); WARN("pthread_create()");
pthread_mutex_unlock(&moving_mutex);
return S_ANS_ERR; return S_ANS_ERR;
}else{ }else{
DBG("Thread created, detouch"); DBG("Thread created, detouch");
@ -174,16 +171,13 @@ static void *ego(_U_ void *unused){
pthread_mutex_lock(&canbus_mutex); pthread_mutex_lock(&canbus_mutex);
go_out_from_ESW(); go_out_from_ESW();
pthread_mutex_unlock(&canbus_mutex); pthread_mutex_unlock(&canbus_mutex);
pthread_mutex_unlock(&moving_mutex);
pthread_exit(NULL); pthread_exit(NULL);
return NULL; return NULL;
} }
static void getoutESW(){ static void getoutESW(){
pthread_mutex_lock(&moving_mutex);
pthread_t m_thread; pthread_t m_thread;
if(pthread_create(&m_thread, NULL, ego, NULL)){ if(pthread_create(&m_thread, NULL, ego, NULL)){
WARN("pthread_create()"); WARN("pthread_create()");
pthread_mutex_unlock(&moving_mutex);
}else{ }else{
DBG("Thread created, detouch"); DBG("Thread created, detouch");
pthread_detach(m_thread); // don't care about thread state pthread_detach(m_thread); // don't care about thread state
@ -193,6 +187,7 @@ static void getoutESW(){
static void *handle_socket(void *asock){ static void *handle_socket(void *asock){
#define getparam(x) (strncmp(found, x, sizeof(x)-1) == 0) #define getparam(x) (strncmp(found, x, sizeof(x)-1) == 0)
int sock = *((int*)asock); int sock = *((int*)asock);
putlog("[DBG] handle_socket: run for client %d", sock);
int webquery = 0; // whether query is web or regular int webquery = 0; // whether query is web or regular
char buff[BUFLEN]; char buff[BUFLEN];
ssize_t rd; ssize_t rd;
@ -240,21 +235,18 @@ static void *handle_socket(void *asock){
FOCMIN_MM, FOCMAX_MM, MINSPEED, MAXSPEED); FOCMIN_MM, FOCMAX_MM, MINSPEED, MAXSPEED);
}else if(getparam(S_CMD_STOP)){ }else if(getparam(S_CMD_STOP)){
DBG("Stop request"); DBG("Stop request");
emerg_stop = TRUE;
pthread_mutex_lock(&canbus_mutex); pthread_mutex_lock(&canbus_mutex);
pthread_mutex_lock(&moving_mutex); emerg_stop = TRUE;
if(stop()) sprintf(buff, S_ANS_ERR); if(stop()) sprintf(buff, S_ANS_ERR);
else sprintf(buff, S_ANS_OK); else sprintf(buff, S_ANS_OK);
emerg_stop = FALSE; emerg_stop = FALSE;
putlog("%s: request to stop @ %.03f", peerIP, curPos()); putlog("%s: request to stop @ %.03f", peerIP, curPos());
pthread_mutex_unlock(&moving_mutex);
pthread_mutex_unlock(&canbus_mutex); pthread_mutex_unlock(&canbus_mutex);
}else if(getparam(S_CMD_TARGSPEED)){ }else if(getparam(S_CMD_TARGSPEED)){
char *ch = strchr(found, '='); char *ch = strchr(found, '=');
double spd; 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{ 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); if(!ch || !str2double(&spd, ch+1) || fabs(spd) < MINSPEED || fabs(spd) > MAXSPEED || movewconstspeed(spd)) sprintf(buff, S_ANS_ERR);
else{ else{
DBG("Move with constant speed %g request", spd); DBG("Move with constant speed %g request", spd);
@ -262,7 +254,6 @@ static void *handle_socket(void *asock){
sprintf(buff, S_ANS_OK); sprintf(buff, S_ANS_OK);
} }
pthread_mutex_unlock(&canbus_mutex); pthread_mutex_unlock(&canbus_mutex);
pthread_mutex_unlock(&moving_mutex);
} }
}else if(getparam(S_CMD_GOTO)){ }else if(getparam(S_CMD_GOTO)){
char *ch = strchr(found, '='); char *ch = strchr(found, '=');
@ -303,10 +294,12 @@ static void *handle_socket(void *asock){
sprintf(buff, "%s", msg); sprintf(buff, "%s", msg);
}else sprintf(buff, S_ANS_ERR); }else sprintf(buff, S_ANS_ERR);
if(!send_data(sock, webquery, buff)){ 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); FREE(peerIP);
putlog("[DBG] socket %d closed", sock);
close(sock); close(sock);
pthread_exit(NULL); pthread_exit(NULL);
return NULL; return NULL;
@ -317,7 +310,7 @@ static void *handle_socket(void *asock){
static void *server(void *asock){ static void *server(void *asock){
int sock = *((int*)asock); int sock = *((int*)asock);
if(listen(sock, BACKLOG) == -1){ if(listen(sock, BACKLOG) == -1){
//WARN("listen() failed"); WARN("listen() failed");
return NULL; return NULL;
} }
while(1){ while(1){
@ -327,13 +320,15 @@ static void *server(void *asock){
int w = waittoread(sock); int w = waittoread(sock);
if(w == 0) continue; if(w == 0) continue;
else if(w < 0) break; else if(w < 0) break;
putlog("[DBG] server(): try to accept()");
newsock = accept(sock, (struct sockaddr*)&their_addr, &size); newsock = accept(sock, (struct sockaddr*)&their_addr, &size);
if(newsock <= 0){ if(newsock <= 0){
WARN("accept() failed"); ERR("accept() failed");
continue; break;
} }
struct sockaddr_in* pV4Addr = (struct sockaddr_in*)&their_addr; addtolog("\t\taccept() OK. fd=%d", newsock);
struct in_addr ipAddr = pV4Addr->sin_addr; struct sockaddr_in* ipV4Addr = (struct sockaddr_in*)&their_addr;
struct in_addr ipAddr = ipV4Addr->sin_addr;
char str[INET_ADDRSTRLEN]; char str[INET_ADDRSTRLEN];
inet_ntop(AF_INET, &ipAddr, str, INET_ADDRSTRLEN); inet_ntop(AF_INET, &ipAddr, str, INET_ADDRSTRLEN);
//DBG("Got connection from %s", str); //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 pthread_detach(handler_thread); // don't care about thread state
} }
} }
putlog("UNREACHABLE CODE REACHED!"); putlog("server(): UNREACHABLE CODE REACHED!");
return NULL; return NULL;
} }
// refresh file with focus value // refresh file with focus value
static void subst_file(char *name){ static void subst_file(char *name){
char aname[FILENAME_MAX];
if(!name) return; if(!name) return;
int l = strlen(name) + 7; snprintf(aname, FILENAME_MAX-1, "%sXXXXXX", name);
char *aname = MALLOC(char, l);
snprintf(aname, l, "%sXXXXXX", name);
int fd = mkstemp(aname); int fd = mkstemp(aname);
if(fd < 0) goto ret; if(fd < 0) return;
fchmod(fd, 0644); fchmod(fd, 0644);
FILE *f = fdopen(fd, "w"); FILE *f = fdopen(fd, "w");
if(!f) goto ret; if(!f){ close(fd); return; }
fprintf(f, "FOCUS = %.3f\n", curPos()); fprintf(f, "FOCUS = %.3f\n", curPos());
fclose(f); fclose(f);
rename(aname, name); rename(aname, name);
ret:
FREE(aname);
} }
// data gathering & socket management // data gathering & socket management
@ -437,7 +429,7 @@ void daemonize(const char *port){
} }
if(p == NULL){ if(p == NULL){
// looped off the end of the list with no successful bind // looped off the end of the list with no successful bind
ERRX("failed to bind socket"); ERRX("daemonize(): failed to bind socket");
} }
freeaddrinfo(res); freeaddrinfo(res);
DBG("going to run daemon_()"); DBG("going to run daemon_()");