diff --git a/Z1000_focus/DS406_canopen.h b/Z1000_focus/DS406_canopen.h index 43a84d4..2066a43 100644 --- a/Z1000_focus/DS406_canopen.h +++ b/Z1000_focus/DS406_canopen.h @@ -37,6 +37,16 @@ #define DS406_SENSOR_AMPLITUDE 0x2200 #define DS406_TARG_FREQ_DEVIAT 0x2201 +// Configuration parameters +// subindex=1 - Safety code sequence (uint16) - 0 for CW, 1 for CCW +// 2 - Safety preset value (uint32) +// 3 - Inverted safety preset value (uint32) +#define DS406_CONF_PARAMETERS 0x5000 +#define DS406_CONF_VALID 0x50FE +#define DS406_CONF_VALID_VALID 0xA5 +#define DS406_CONF_CHECKSUM 0x50FF + + #define DS406_OPER_PARAMS 0x6000 #define DS406_MEAS_UNITS_PERREV 0x6001 #define DS406_TOT_MEAS_RANGE 0x6002 diff --git a/Z1000_focus/HW_dependent.h b/Z1000_focus/HW_dependent.h index 949199a..d7f8c23 100644 --- a/Z1000_focus/HW_dependent.h +++ b/Z1000_focus/HW_dependent.h @@ -23,7 +23,7 @@ #include "motor_cancodes.h" // On lowest speeds Taccelerated = 0.22s, so wait no more than 0.25s when motor starts -#define TACCEL (0.25) +#define TACCEL (0.50) // max amount of cycles when motor stalled #define STALL_MAXCTR (50) @@ -45,7 +45,7 @@ // rev/min to raw speed value #define RAWSPEED(x) (x*5) // max/min speed (rev/min) -#define MAXSPEED (1200) +#define MAXSPEED (1550) #define MINSPEED (350) // encoder differences (if larger) for speed (MAXSPEED, MAXSPEED/2 and MAXSPEED/3) select #define ENCODER_DIFF_SPEED1 (1500) @@ -57,9 +57,12 @@ #define MOVING_TIMEOUT (300) // correction parameters: steps after stopping = CORR0 + (CORR1 + CORR2*rs)*rs) // where rs is raw speed -#define CORR0 (-46.0) -#define CORR1 (4.2857e-3) -#define CORR2 (1.5714e-5) +//#define CORR0 (-38.919) +//#define CORR1 (-4.3223e-3) +//#define CORR2 (1.6549e-5) +#define CORR0 (6.7704) +#define CORR1 (6.1857e-3) +#define CORR2 (1.1271e-5) // constants for focus conversion: foc_mm = (foc_raw - FOCRAW_0) / FOCSCALE_MM diff --git a/Z1000_focus/Makefile b/Z1000_focus/Makefile index aa388d3..ab11afa 100644 --- a/Z1000_focus/Makefile +++ b/Z1000_focus/Makefile @@ -1,18 +1,24 @@ # run `make DEF="-D... -D..."` to add extra defines -PROGRAM := can_phocus +PROGRAM := can_focus LDFLAGS := -fdata-sections -ffunction-sections -Wl,--gc-sections -Wl,--discard-all -pthread SRCS := $(wildcard *.c) DEFINES := $(DEF) -D_GNU_SOURCE -D_XOPEN_SOURCE=1111 OBJDIR := mk -CFLAGS += -O2 -Wall -Werror -Wextra -Wno-trampolines -std=gnu99 +CFLAGS += -O2 -std=gnu99 -Wall -Wextra +#ifneq (,$(findstring DEBUG, $(DEF))) +# CFLAGS += -Werror +# $(info Warnings considered as errors) +#endif OBJS := $(addprefix $(OBJDIR)/, $(SRCS:%.c=%.o)) DEPS := $(OBJS:.o=.d) CC ?= gcc -$(info GCC is: ${CC}) #CXX = g++ all : $(OBJDIR) $(PROGRAM) +debug: CFLAGS += -DEBUG -Werror +debug: all + $(PROGRAM) : $(OBJS) @echo -e "\t\tLD $(PROGRAM)" $(CC) $(LDFLAGS) $(OBJS) -o $(PROGRAM) diff --git a/Z1000_focus/can_encoder.c b/Z1000_focus/can_encoder.c index 3840319..6d987d0 100644 --- a/Z1000_focus/can_encoder.c +++ b/Z1000_focus/can_encoder.c @@ -32,6 +32,8 @@ static uint8_t encoderRDY = 0, motorRDY = 0; // printf when -v extern int verbose(const char *fmt, ...); +extern bool emerg_stop; + // CAN bus IDs: for motor's functions (PI ID [F=4] == PO ID[F=3] + 1) and parameters static unsigned long motor_id = 0, motor_p_id = 0;//, bcast_id = 1; // current motor position (RAW) @@ -422,7 +424,8 @@ static canstatus can_send_chk(unsigned char *buf, unsigned char *obuf){ SINGLEWARN(WARN_CANSEND); return CAN_CANTSEND; }else clrwarnsingle(WARN_CANSEND); - int I, rxpnt, idr, dlen; + int I, rxpnt, dlen; + canid_t idr; double rxtime; unsigned char rdata[8]; can_clean_recv(&rxpnt, &rxtime); @@ -463,7 +466,8 @@ static canstatus can_send_chk(unsigned char *buf, unsigned char *obuf){ static canstatus can_send_param(unsigned char *buf, unsigned char *obuf){ if(!motorRDY) return CAN_NOANSWER; const int l = 8; // frame length - int I, rxpnt, idr, dlen; + int I, rxpnt, dlen; + canid_t idr; double rxtime; unsigned char rdata[8]; /* diff --git a/Z1000_focus/can_encoder.h b/Z1000_focus/can_encoder.h index e945f36..8da31f9 100644 --- a/Z1000_focus/can_encoder.h +++ b/Z1000_focus/can_encoder.h @@ -50,7 +50,7 @@ typedef enum{ } sysstatus; int init_encoder(int encnode, int reset); -void returnPreOper(); +void returnPreOper(long long presetval); int getPos(double *pos); double curPos(); int init_motor_ids(int addr); diff --git a/Z1000_focus/can_io.c b/Z1000_focus/can_io.c index 19db913..46f9e85 100644 --- a/Z1000_focus/can_io.c +++ b/Z1000_focus/can_io.c @@ -1,15 +1,6 @@ -/* CAN I/O library (to use as a process) - * usage: - * first: fork() + start_can_io(NULL) - start CAN Rx-buffering process - * then: fork() + Control_1(....) - start process that uses recv/send functions - * ........................... - * then: fork() + Control_N(....) - * - * note: use init_can_io() at the begining of every Control process - * BUT DON't USE it in main() before Control process start - * ^^^^^^^^^^^^^ - * (c) vsher@sao.ru - */ +/* CAN I/O library (for compatibility with the old one, */ +/* but through the new SocketCAN interface) */ + #include #include #include @@ -17,495 +8,219 @@ #include #include #include -#include -#include -#include #include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include -#include "usefull_macros.h" -#include "canmsg.h" #include "can_io.h" -#include "checkfile.h" -char can_dev[40] = "/dev/can0"; -int can_fd=-1; -char can_lck[40] = "/tmp/dev_can0.lock"; -int can_lk=-1; -static int server_mode=0; -static int my_uid; +char can_dev[40] = "/dev/can0";/* for compatibility (only "can0" needs) */ +static int can_sck = -1; /* can raw socket */ +static struct timeval start_tv, tv; +static double start_time; -#define CAN_SHM_SIZE ((sizeof(int)*4)+CAN_CTLR_SIZE+(CAN_RX_SIZE*sizeof(canmsg_t))) +void set_sending_mode(int x) {return;} +int can_sending_mode() {return(0);} -union ShMkey { - char name[5]; - key_t code; -} can_shm_key; +void *init_can_io() { + struct sockaddr_can addr; + struct canfd_frame frame; + struct ifreq ifr; -int can_shm_id=-1; -char *can_shm_addr = NULL; - -#define can_pid (*(((int *)can_shm_addr)+0)) /* PID of CAN I/O process */ -#define can_open (*(((int *)can_shm_addr)+1)) /* file descr.of CAN-driver */ -#define rx_buff_pntr (*(((int *)can_shm_addr)+2)) /* from 0 till CAN_RX_SIZE-1 */ -#define can_mode (*(((int *)can_shm_addr)+3)) /* CAN server/client mode flags */ -#define CAN_SEND_SERVER 1 /* clients should try to send frames through CAN-server or directly to CAN-driver otherwise */ - -void *can_ctrl_addr = NULL; /* shm area reserved for control process purpose*/ -canmsg_t *rx_buff; /* rx ring buffer: CAN_RX_SIZE*sizeof(canmsg_t)*/ - -struct CMD_Queue { /* описание очереди (канала) команд */ - union{ - char name[5]; /* ключ идентефикации очереди */ - key_t code; - } key; - int mode; /* режим доступа (rwxrwxrwx) */ - int side; /* тип подсоединения: Клиент/Сервер (Sender/Receiver)*/ - int id; /* дескриптор подсоединения */ - unsigned int acckey; /* ключ доступа (для передачи Клиент->Сервер) */ -}; - -/* канал команд используем для передачи CAN-фреймов */ -static struct CMD_Queue canout = {.key.name = {'C','A','N',0,0},0200,0,-1,0}; - -/* структура сообщения */ -struct my_msgbuf { - unsigned long mtype; /* type of message */ - unsigned long acckey; /* ключ доступа клиента */ - unsigned long src_pid; /* номер процесса источника */ - unsigned long src_ip; /* IP-адр. источника, =0 - локальная команда */ - char mtext[100]; /* message text */ -}; - -static void can_abort(int sig); - -void set_server_mode(int mode) {server_mode=mode;} -int can_server() {return(server_mode);} -void set_sending_mode(int to_server) { - if(to_server) can_mode |= CAN_SEND_SERVER; - else can_mode &= ~CAN_SEND_SERVER; -} -int can_sending_mode() {return(can_mode&CAN_SEND_SERVER);} -int can_card() {return(can_fd>0);} -int can_gate() {return(0);} -double can_gate_time_offset() {return(0.0);} - -void setup_can_net(_U_ unsigned long ipaddr, _U_ int port, _U_ unsigned long acckey) {return;} - -unsigned long get_acckey() {return(0);} - -static int shm_created=0; - -/* to use _AFTER_ process forking */ -void *init_can_io() { /* returns shared area addr. for client control process*/ - int new_shm=0; - char *p, msg[256]; - - my_uid=geteuid(); - if(!can_shm_addr){ - if((p = strrchr(can_dev,'/'))){ - memcpy(&can_lck[9], p+1, 4); - memcpy(can_shm_key.name, p+1, 4); - can_shm_key.name[4]='\0'; - }else{ - ERRX("Wrong CAN device name: %s\n", can_dev); - } - can_shm_id = shmget(can_shm_key.code, CAN_SHM_SIZE, 0644); - if(can_shm_id<0 && errno==EACCES) - can_shm_id = shmget(can_shm_key.code, CAN_SHM_SIZE, 0444); - if(can_shm_id<0 && errno==ENOENT && server_mode) { - can_shm_id = shmget(can_shm_key.code, CAN_SHM_SIZE, IPC_CREAT|IPC_EXCL|0644); - new_shm = shm_created = 1; - } - if(can_shm_id<0){ - can_prtime(stderr); - if(new_shm) - ERR("Can't create shm CAN buffer '%s'",can_shm_key.name); - else if(server_mode) - ERR("CAN-I/O: Can't find shm segment for CAN buffer '%s'",can_shm_key.name); - else - ERR("Can't find shm segment for CAN buffer '%s' (maybe no CAN-I/O process?)",can_shm_key.name); - } - can_shm_addr = shmat(can_shm_id, NULL, 0); - if(can_shm_addr == (void*)-1 && errno == EACCES) - can_shm_addr = shmat(can_shm_id, NULL, SHM_RDONLY); - if(can_shm_addr == (void*)-1){ - shmctl(can_shm_id, IPC_RMID, NULL); - ERR("Can't attach shm CAN buffer '%s'",can_shm_key.name); - } + /* open socket */ + if ((can_sck = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) { + perror("CAN socket"); + can_exit(0); } - can_ctrl_addr = (canmsg_t *)(can_shm_addr+sizeof(int)*4); - rx_buff = (canmsg_t *)(can_ctrl_addr+CAN_CTLR_SIZE); - if(can_fd < 0 && canout.id < 0){ - int flags = (server_mode)? O_RDWR : O_WRONLY; - if(server_mode){ - if(( can_fd = open(can_dev, flags)) < 0 ){ - sprintf(msg,"CAN-I/O: Error opening CAN device %s", can_dev); - can_prtime(stderr); - perror(msg); - shmctl(can_shm_id, IPC_RMID, NULL); - exit(errno); - } - }else{ - can_fd = open(can_dev, flags); - canout.id = msgget(canout.key.code, canout.mode); - if(can_fd < 0 && canout.id < 0) { - WARNX("Error opening CAN device(%s) or CAN output queue '%s' (maybe no CANqueue server process?)",can_dev,canout.key.name); - } - } + strncpy(ifr.ifr_name, &can_dev[5], IFNAMSIZ - 1); + ifr.ifr_name[IFNAMSIZ - 1] = '\0'; + ifr.ifr_ifindex = if_nametoindex(ifr.ifr_name); + if (!ifr.ifr_ifindex) { + perror("if_nametoindex"); + can_exit(0); } - if(can_lk > 0) close(can_lk); - if(( can_lk = open(can_lck, O_RDWR|O_CREAT, S_IRUSR|S_IWUSR|S_IRGRP|S_IWGRP|S_IROTH|S_IWOTH )) < 0 ){ - shmctl(can_shm_id, IPC_RMID, NULL); - close(can_fd); - ERR("Error opening CAN device lock-file %s", can_lck); - } - fchmod(can_lk, S_IRUSR|S_IWUSR|S_IRGRP|S_IWGRP|S_IROTH|S_IWOTH); - if(new_shm){ - struct timeval tmv; - struct timezone tz; - gettimeofday(&tmv,&tz); - if(flock(can_lk, LOCK_EX)<0) perror("locking CAN"); - can_pid = 0; - can_open = -1; - rx_buff_pntr = 0; - for(int i = 0; i < CAN_RX_SIZE; ++i){ - rx_buff[i].id = 0; - rx_buff[i].timestamp = tmv; - } - if(flock(can_lk, LOCK_UN)<0) perror("unlocking CAN"); + memset(&addr, 0, sizeof(addr)); + addr.can_family = AF_CAN; + addr.can_ifindex = ifr.ifr_ifindex; + + if(bind(can_sck, (struct sockaddr *)&addr, sizeof(addr)) < 0) { + perror("bind CAN socket"); + can_exit(0); } + + gettimeofday(&start_tv, NULL); + start_time = (double)start_tv.tv_sec + (double)start_tv.tv_usec/1e6; + tv.tv_sec = tv.tv_usec = 0; + + signal(SIGHUP, can_exit); signal(SIGINT, can_exit); signal(SIGQUIT,can_exit); signal(SIGTERM,can_exit); - return(can_ctrl_addr); + + return(&can_dev[5]); } -/* CAN "Rx to buff" process */ -void *start_can_io(_U_ void *arg){ - set_server_mode(1); - init_can_io(); - if(can_io_ok()){ - can_prtime(stderr); - fprintf(stderr,"CAN I/O process(%d) already running!\n",can_pid); - sleep(1); - can_prtime(stderr); - fprintf(stderr,"New CAN I/O process(%d) exiting...!\n",getpid()); - exit(0); - } - if(can_fd < 0){ - can_prtime(stderr); - fprintf(stderr,"Error opening CAN device %s\n", can_dev); - shmctl(can_shm_id, IPC_RMID, NULL); - exit(0); - } - can_pid = getpid(); - can_open = can_fd; - can_mode = 0; - - signal(SIGHUP, can_abort); - signal(SIGINT, can_abort); - signal(SIGQUIT,can_abort); - signal(SIGFPE, can_abort); - signal(SIGPIPE,can_abort); - signal(SIGSEGV,can_abort); - signal(SIGALRM, SIG_IGN); - signal(SIGTERM,can_abort); - - if(shmctl(can_shm_id, SHM_LOCK, NULL) < 0) - perror("CAN I/O: can't prevents swapping of Rx-buffer area"); - - while(1){ - int n; - canmsg_t rx; - - if(!can_io_shm_ok()){can_delay(0.3); continue;} - - n = can_wait(can_fd, 0.3); - if(n < 0) sleep(1); - if(n <= 0) continue; - - do{ - //static struct timeval tm = {0,0}; - n = read(can_fd, &rx, sizeof(struct canmsg_t)); - if(n < 0){ - perror("CAN Rx error"); - } else if(n > 0) { - if(flock(can_lk, LOCK_EX)<0) perror("locking CAN"); - rx_buff[rx_buff_pntr] = rx; - rx_buff_pntr = (rx_buff_pntr + 1) % CAN_RX_SIZE; - if(flock(can_lk, LOCK_UN)<0) perror("unlocking CAN"); - } - } while(n>0); - } -} - -/* put CAN-frame to recv-buffer */ -void can_put_buff_frame(double rtime, int id, int length, unsigned char data[]) { - int i; - canmsg_t rx; - int sec = (int)rtime; - if(!server_mode) return; - if(length<0) length=0; - if(length>8) length=8; - rx.id = id&((id&CAN_EXT_FLAG)? 0x1fffffff : 0x7ff); - rx.cob=0; - rx.flags = ((id&CAN_RTR_FLAG)?MSG_RTR:0)|((id&CAN_EXT_FLAG)?MSG_EXT:0); - rx.length=length; - for(i=0; i0 && can_open>0); -} - - -/* Все нормально с CAN-I/O процессом */ -/* (но надо быть супер-юзером!) */ -int can_io_ok(){ - return(can_io_shm_ok() && (my_uid!=0||kill(can_pid, 0)==0)); -} - - -/* Возможна работа c CAN для клиента */ int can_ok() { - return(can_io_shm_ok()); + return(can_sck>0); } - /* wait for CAN-frame */ -int can_wait(int fd, double tout){ +int can_wait(int fd, double tout) +{ int nfd,width; - struct timeval tv; fd_set readfds; - if(fd==0 && tout>=0.01){ - double dt = can_dsleep(tout); - if(dt>0.) can_dsleep(dt); - return(0); + if(fd==0 && tout>=0.01) { + double dt = can_dsleep(tout); + if(dt>0.) can_dsleep(dt); + return(0); } - if(fd<0) fd=can_fd; - if(fd>0){ - FD_ZERO(&readfds); - FD_SET(fd, &readfds); - width = fd+1; - } else width = 0; + if(fd<0) fd=can_sck; + if(fd>0) { + FD_ZERO(&readfds); + FD_SET(fd, &readfds); + width = fd+1; + } else + width = 0; tv.tv_sec = (int)tout; tv.tv_usec = (int)((tout - tv.tv_sec)*1000000.+0.9); -slipping: - if(fd>0 && can_fd>0) - nfd = select(width, &readfds, (fd_set *)NULL, (fd_set *)NULL, &tv); + + if(fd>0 && can_sck>0) + nfd = select(width, &readfds, (fd_set *)NULL, (fd_set *)NULL, &tv); else - nfd = select(0, (fd_set *)NULL, (fd_set *)NULL, (fd_set *)NULL, &tv); - if(nfd < 0){ - if(errno == EINTR) - goto slipping; - perror("Error in can_wait(){ select() }"); - return(-1); + nfd = select(0, (fd_set *)NULL, (fd_set *)NULL, (fd_set *)NULL, &tv); + if(nfd < 0) { + if(errno != EINTR) + perror("Error in can_wait(){ select() }"); + return(-1); } else if(nfd == 0) /* timeout! */ - return(0); + return(0); if(fd>0 && FD_ISSET(fd, &readfds)) /* Rx frame! */ - return(1); + return(1); return(0); } -/* cleanup recv-buffer in client process */ -void can_clean_recv(int *pbuf, double *rtime) { +/* for compatibility with my old can-library */ +void can_clean_recv(int *psock, double *rtime) { struct timeval tmv; struct timezone tz; gettimeofday(&tmv,&tz); - *pbuf = rx_buff_pntr; *rtime = tmv.tv_sec + (double)tmv.tv_usec/1000000.; + *psock = can_sck; + if(can_sck>0) { + int n=0; + struct can_frame frame; + fcntl(can_sck, F_SETFL, O_NONBLOCK); + do { + n=recv(can_sck, &frame, sizeof(struct can_frame),0); + } while(n>0); + if(n<0 && errno != EAGAIN) { + perror("recv from CAN-socket"); fflush(stderr); + } + } } -/* find next rx-frame in recv-buffer for client process */ -int can_recv_frame(int *pbuf, double *rtime, - int *id, int *length, unsigned char data[]){ - return(can_get_buff_frame(pbuf, rtime, id, length, data)); -} -int can_get_buff_frame(int *pbuf, double *rtime, - int *id, int *length, unsigned char data[]) { - while(*pbuf != rx_buff_pntr){ - canmsg_t *rx = &rx_buff[*pbuf]; - struct timeval *tv = &rx->timestamp; - double t_rx; - - if(flock(can_lk, LOCK_EX)<0) perror("locking CAN"); - - t_rx = tv->tv_sec + (double)tv->tv_usec/1000000.; - if(t_rx+1. >= *rtime){ - int i; - *id = rx->id | ((rx->flags&MSG_RTR)? CAN_RTR_FLAG:0); - *length = rx->length; - for(i = 0; i < *length; i++) - data[i] = rx->data[i]; - *rtime = t_rx; - *pbuf = (*pbuf + 1) % CAN_RX_SIZE; - if(flock(can_lk, LOCK_UN)<0) perror("unlocking CAN"); - return(1); - } - *pbuf = (*pbuf + 1) % CAN_RX_SIZE; - - if(flock(can_lk, LOCK_UN)<0) perror("unlocking CAN"); +int can_recv_frame(int *psock, double *rtime, + canid_t *id, int *length, unsigned char data[]) { + int i,n=0; + struct can_frame frame; + struct pollfd pfd; + if(*psock > 0) { + pfd.fd = *psock; + pfd.events=POLLIN; + pfd.revents=0; + if((n=poll(&pfd,1,1))<0) { + perror("CAN-socket poll() error"); fflush(stderr); + return(0); + } + if(n==0) return(0); + n=recv(*psock, &frame, sizeof(struct can_frame),0); + if(n<0 && errno != EAGAIN) { + perror("recv frame from CAN-socket"); fflush(stderr); + } else if(n>0) { + if(frame.len>8) frame.len=8; // no CAN FD frames in our systems! + *id = frame.can_id; + *length = frame.len; + for(i = 0; i < frame.len; i++) + data[i] = frame.data[i]; + if(ioctl(*psock, SIOCGSTAMP, &tv)<0) { + perror("ioctl(to get frame timestamp)"); fflush(stderr); + } else + *rtime = tv.tv_sec + (double)tv.tv_usec/1000000.; + return(1); + } } return(0); } - /* send tx-frame from client process */ -/* to CAN-driver or to output queue */ -int can_send_frame(unsigned long id, int length, unsigned char data[]) { +int can_send_frame(canid_t id, int length, unsigned char data[]) { int i, ret=1; - if(can_fd<0 && canout.id<0) - return(0); + struct can_frame frame; + if(can_sck<0) + return(-1); if(length>8) length=8; if(length<0) length=0; - if(!server_mode && (can_mode&CAN_SEND_SERVER) && canout.id>=0 ) - goto send2server; - if(can_fd >= 0){ - canmsg_t tx; - tx.id = id&((id&CAN_EXT_FLAG)? 0x1fffffff : 0x7ff); - tx.cob=0; - tx.flags = ((id&CAN_RTR_FLAG)?MSG_RTR:0)|((id&CAN_EXT_FLAG)?MSG_EXT:0); - tx.length=length; - for(i=0;i= 0){ - struct my_msgbuf mbuf; -send2server: - mbuf.src_pid = getpid(); - mbuf.src_ip = 0; - mbuf.acckey = canout.acckey; - mbuf.mtype = id+1; - for(i=0;i=0) close(can_fd); - can_prtime(stderr); - fprintf(stderr,"%s process stop!\n", ss); - fflush(stderr); - close(can_lk); - shmdt(can_shm_addr); - shmctl(can_shm_id, IPC_STAT, &buf); - if(buf.shm_nattch == 0) - shmctl(can_shm_id, IPC_RMID, NULL); - unlink_pidfile(); - exit(sig); + default: + case SIGHUP : + can_prtime(stderr); + fprintf(stderr,"%s Ignore .....\n",ss); + fflush(stderr); + signal(sig, can_exit); + return; + case 0: + case SIGINT : + case SIGPIPE: + case SIGQUIT: + case SIGFPE : + case SIGSEGV: + case SIGTERM: + if(can_sck>=0) close(can_sck); + can_prtime(stderr); + fprintf(stderr,"%s process stop!\n",ss); + fflush(stderr); + exit(sig); } } -char *time2asc(double t){ - static char stmp[10][20]; - static int itmp=0; +char *time2asc(double t) +{ + static char stmp[10][20]; + static int itmp=0; char *lin = stmp[itmp]; int h, min; double sec; @@ -519,30 +234,31 @@ char *time2asc(double t){ } double can_dsleep(double dt) { - struct timespec ts,tsr; - ts.tv_sec = (time_t)dt; - ts.tv_nsec = (long)((dt-ts.tv_sec)*1e9); - nanosleep(&ts,&tsr); - return((double)ts.tv_sec + (double)ts.tv_nsec/1e9); + struct timespec ts,tsr; + ts.tv_sec = (time_t)dt; + ts.tv_nsec = (long)((dt-ts.tv_sec)*1e9); + nanosleep(&ts,&tsr); + return((double)ts.tv_sec + (double)ts.tv_nsec/1e9); } double can_dtime() { - struct timeval ct; - struct timezone tz; - gettimeofday(&ct, &tz); - return ((double)ct.tv_sec + (double)ct.tv_usec/1e6); + struct timeval ct; + struct timezone tz; + gettimeofday(&ct, &tz); + return ((double)ct.tv_sec + (double)ct.tv_usec/1e6); } char *can_atime() {return(time2asc(can_dtime()));} void can_prtime(FILE *fd) { - static double otime=0.0; - double ntime=can_dtime(); - time_t itime = (int)ntime; - if(otime==0.0) tzset(); - ntime -= (double)timezone; - if((((int)ntime)%(24*3600) < ((int)otime)%(24*3600)) || otime==0.0) - fprintf(fd,"========================\n%s",ctime(&itime)); - fprintf(fd,"%s ",time2asc(ntime)); - otime=ntime; + static double otime=0.0; + double ntime=can_dtime(); + time_t itime = (int)ntime; + if(otime==0.0) tzset(); + ntime -= (double)timezone; + if((((int)ntime)%(24*3600) < ((int)otime)%(24*3600)) || otime==0.0) + fprintf(fd,"========================\n%s",ctime(&itime)); + fprintf(fd,"%s ",time2asc(ntime)); + otime=ntime; } + diff --git a/Z1000_focus/can_io.h b/Z1000_focus/can_io.h index d9afea3..222a91a 100644 --- a/Z1000_focus/can_io.h +++ b/Z1000_focus/can_io.h @@ -1,47 +1,29 @@ -// (c) vsher@sao.ru +/* CAN I/O library (for compatibility with the old one, */ +/* but through the new SocketCAN interface) */ -#pragma once +#include -#ifndef CAN_IO_H__ -#define CAN_IO_H__ - -#include - -#define _U_ __attribute__((__unused__)) - - -#define CAN_CTLR_SIZE 1024 /* size of client process shared area */ -#define CAN_RX_SIZE 1000 /* max. # frames in Rx-buffer */ -#define CAN_RTR_FLAG 0x20000000 /* send frame as Remote Transmission Request */ -#define CAN_EXT_FLAG 0x40000000 /* send frame with extended 29-bit ID, 11-bit otherwise */ +#ifndef CAN_RTR_FLAG +#define CAN_RTR_FLAG 0x40000000 /* frame as Remote Transmission Request */ +#endif +#ifndef CAN_EFF_FLAG +#define CAN_EFF_FLAG 0x80000000 /* frame with extended 29-bit ID, 11-bit otherwise */ +#endif +#define CAN_EXT_FLAG CAN_EFF_FLAG int can_wait(int fd, double tout); #define can_delay(Tout) can_wait(0, Tout) -void set_server_mode(int mode); -int can_server(); -void set_sending_mode(int to_server); -int can_sending_mode(); -int can_card(); -int can_gate(); -double can_gate_time_offset(); -void setup_can_net(unsigned long ipaddr, int port, unsigned long acckey); -unsigned long get_acckey(); void *init_can_io(); -void *start_can_io(void *arg); -void can_put_buff_frame(double rtime, int id, int length, unsigned char data[]); -int can_io_ok(); -int can_io_shm_ok(); int can_ok(); -void can_clean_recv(int *pbuf, double *rtime); -int can_get_buff_frame(int *pbuf, double *rtime, - int *id, int *length, unsigned char data[]); -int can_recv_frame(int *pbuf, double *rtime, - int *id, int *length, unsigned char data[]); -int can_send_frame(unsigned long id, int length, unsigned char data[]); +#define can_io_ok() can_ok() +void can_clean_recv(int *psock, double *rtime); +int can_recv_frame(int *psock, double *rtime, + canid_t *id, int *length, unsigned char data[]); +int can_send_frame(canid_t id, int length, unsigned char data[]); void can_exit(int sig); char *time2asc(double t); double can_dsleep(double dt); double can_dtime(); -char *can_atime(); void can_prtime(FILE *fd); -#endif // CAN_IO_H__ +void set_sending_mode(int); +int can_sending_mode(); diff --git a/Z1000_focus/main.c b/Z1000_focus/main.c index 73317bc..bb5a14e 100644 --- a/Z1000_focus/main.c +++ b/Z1000_focus/main.c @@ -16,6 +16,7 @@ * along with this program. If not, see . */ +#include #include // fabs #include #include diff --git a/Z1000_focus/socket.c b/Z1000_focus/socket.c index 908d19f..3a4c90d 100644 --- a/Z1000_focus/socket.c +++ b/Z1000_focus/socket.c @@ -45,7 +45,7 @@ extern glob_pars *G; /** * wait for answer from socket * @param sock - socket fd - * @return 0 in case of error or timeout, 1 in case of socket ready + * @return 0 in case of timeout, -1 if error, 1 in case of socket ready */ static int waittoread(int sock){ fd_set fds; @@ -60,7 +60,7 @@ static int waittoread(int sock){ if(rc < 0){ if(errno != EINTR){ WARN("select()"); - return 0; + return -1; } continue; } @@ -98,16 +98,18 @@ static int send_data(int sock, int webquery, char *buf){ "Access-Control-Allow-Credentials: true\r\n" "Content-type: text/plain\r\nContent-Length: %zd\r\n\r\n", Len); if(L < 0){ - WARN("sprintf()"); + WARN("sprintf: L=%zd", L); return 0; } - if(L != write(sock, tbuf, L)){ - WARN("write"); + ssize_t W = write(sock, tbuf, L); + if(L != W){ + WARN("write header: %zd instead of %zd", W, L); return 0; } } - if(Len != write(sock, buf, Len)){ - WARN("write()"); + ssize_t W = write(sock, buf, Len); + if(Len != W){ + WARN("write data: %zd instead of %zd", W, Len); return 0; } return 1; @@ -204,9 +206,12 @@ static void *handle_socket(void *asock){ } double t0 = dtime(); while(dtime() - t0 < SOCKET_TIMEOUT){ - if(!waittoread(sock)){ // no data incoming + int w = waittoread(sock); + if(w == 0){ // no data incoming //DBG("no incoming data"); continue; + }else if(w < 0){ // socket closed -> go out + break; } if((rd = read(sock, buff, BUFLEN-1)) < 1){ //DBG("socket closed. Exit"); @@ -298,7 +303,7 @@ 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, some error occured"); + WARNX("can't send data to %s, some error occured", peerIP); } } FREE(peerIP); @@ -319,7 +324,9 @@ static void *server(void *asock){ socklen_t size = sizeof(struct sockaddr_in); struct sockaddr_in their_addr; int newsock; - if(!waittoread(sock)) continue; + int w = waittoread(sock); + if(w == 0) continue; + else if(w < 0) break; newsock = accept(sock, (struct sockaddr*)&their_addr, &size); if(newsock <= 0){ WARN("accept() failed"); @@ -339,6 +346,7 @@ static void *server(void *asock){ } } putlog("UNREACHABLE CODE REACHED!"); + return NULL; } // refresh file with focus value @@ -352,7 +360,7 @@ static void subst_file(char *name){ fchmod(fd, 0644); FILE *f = fdopen(fd, "w"); if(!f) goto ret; - fprintf(f, "FOCUS = %.2f\n", curPos()); + fprintf(f, "FOCUS = %.3f\n", curPos()); fclose(f); rename(aname, name); ret: @@ -364,7 +372,7 @@ static void daemon_(int sock){ if(sock < 0) return; pthread_t sock_thread; double oldpos = curPos(); - subst_file(G->focfilename); + if(G->focfilename) subst_file(G->focfilename); DBG("create server() thread"); if(pthread_create(&sock_thread, NULL, server, (void*) &sock)){ ERR("pthread_create() failed"); @@ -387,7 +395,7 @@ static void daemon_(int sock){ getoutESW(); } } - if(G->focfilename && (fabs(oldpos - curPos()) > 0.01)){ // position changed -> change it in file + if(G->focfilename && (fabs(oldpos - curPos()) > 0.001)){ // position changed -> change it in file oldpos = curPos(); subst_file(G->focfilename); } @@ -474,7 +482,9 @@ void sock_send_data(const char *host, const char *port, const char *data){ if(send(sock, data, L, 0) != (ssize_t)L){ WARN("send"); return;} double t0 = dtime(); while(dtime() - t0 < SOCKET_TIMEOUT){ - if(!waittoread(sock)) continue; + int w = waittoread(sock); + if(w == 0) continue; + else if(w < 0) break; char buff[32]; int n = read(sock, buff, 31); if(n > 0){ diff --git a/Z1000_focus/socket.h b/Z1000_focus/socket.h index 9d6ee6c..3d62768 100644 --- a/Z1000_focus/socket.h +++ b/Z1000_focus/socket.h @@ -27,7 +27,7 @@ #include "stdbool.h" // timeout for socket closing -#define SOCKET_TIMEOUT (5.0) +#define SOCKET_TIMEOUT (10.0) // default port number (strinig) #define DEFPORT "4444" @@ -53,7 +53,7 @@ #define S_STATUS_FORBIDDEN "Error: motion in forbidden position" #define S_STATUS_DAMAGE "Error: damaged state, call engineer" -bool emerg_stop; +//bool emerg_stop; void daemonize(const char *port); void sock_send_data(const char *host, const char *port, const char *data);