mirror of
https://github.com/eddyem/zeiss_utils.git
synced 2025-12-06 02:35:15 +03:00
some fixes
This commit is contained in:
Edward Emelianov
parent
abe934338b
commit
d9124740ec
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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];
|
||||
/*
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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 <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <unistd.h>
|
||||
@ -17,278 +8,74 @@
|
||||
#include <signal.h>
|
||||
#include <string.h>
|
||||
#include <time.h>
|
||||
#include <sys/types.h>
|
||||
#include <sys/stat.h>
|
||||
#include <sys/time.h>
|
||||
#include <fcntl.h>
|
||||
#include <sys/file.h>
|
||||
#include <sys/shm.h>
|
||||
#include <sys/ipc.h>
|
||||
#include <sys/msg.h>
|
||||
#include <sys/time.h>
|
||||
#include <net/if.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <sys/socket.h>
|
||||
#include <linux/sockios.h>
|
||||
#include <linux/can.h>
|
||||
#include <linux/can/raw.h>
|
||||
#include <poll.h>
|
||||
|
||||
#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;
|
||||
/* open socket */
|
||||
if ((can_sck = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
|
||||
perror("CAN socket");
|
||||
can_exit(0);
|
||||
}
|
||||
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;}
|
||||
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);
|
||||
}
|
||||
memset(&addr, 0, sizeof(addr));
|
||||
addr.can_family = AF_CAN;
|
||||
addr.can_ifindex = ifr.ifr_ifindex;
|
||||
|
||||
unsigned long get_acckey() {return(0);}
|
||||
if(bind(can_sck, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
|
||||
perror("bind CAN socket");
|
||||
can_exit(0);
|
||||
}
|
||||
|
||||
static int shm_created=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;
|
||||
|
||||
/* 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);
|
||||
}
|
||||
}
|
||||
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);
|
||||
}
|
||||
}
|
||||
}
|
||||
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");
|
||||
}
|
||||
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; i<length; i++) rx.data[i]=data[i];
|
||||
rx.timestamp.tv_sec = sec;
|
||||
rx.timestamp.tv_usec = (int)((rtime-sec)*1000000.);
|
||||
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");
|
||||
}
|
||||
|
||||
/* ÷ÓÅ ÎÏÒÍÁÌØÎÏ Ó SHM-ÂÕÆÅÒÏÍ CAN-I/O ÐÒÏÃÅÓÓÁ */
|
||||
int can_io_shm_ok(){
|
||||
return(can_pid>0 && 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) {
|
||||
@ -296,22 +83,22 @@ int can_wait(int fd, double tout){
|
||||
if(dt>0.) can_dsleep(dt);
|
||||
return(0);
|
||||
}
|
||||
if(fd<0) fd=can_fd;
|
||||
if(fd<0) fd=can_sck;
|
||||
if(fd>0) {
|
||||
FD_ZERO(&readfds);
|
||||
FD_SET(fd, &readfds);
|
||||
width = fd+1;
|
||||
} else width = 0;
|
||||
} 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)
|
||||
|
||||
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;
|
||||
if(errno != EINTR)
|
||||
perror("Error in can_wait(){ select() }");
|
||||
return(-1);
|
||||
} else if(nfd == 0) /* timeout! */
|
||||
@ -321,148 +108,81 @@ slipping:
|
||||
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_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);
|
||||
}
|
||||
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");
|
||||
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);
|
||||
}
|
||||
*pbuf = (*pbuf + 1) % CAN_RX_SIZE;
|
||||
|
||||
if(flock(can_lk, LOCK_UN)<0) perror("unlocking CAN");
|
||||
}
|
||||
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<length;i++) tx.data[i]=data[i];
|
||||
if(flock(can_lk, LOCK_EX)<0) perror("locking CAN");
|
||||
//fprintf(stderr,"write(id=%02x,flag=%1x,len=%d)\n",tx.id,tx.flags,tx.length);fflush(stderr);
|
||||
ret = write(can_fd, &tx, sizeof(struct canmsg_t));
|
||||
if(flock(can_lk, LOCK_UN)<0) perror("unlocking CAN");
|
||||
if(server_mode)
|
||||
/* copy tx CAN-frame back to recv-buffer */
|
||||
can_put_buff_frame(can_dtime(), id, length, data);
|
||||
} else if(canout.id >= 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<length;i++) mbuf.mtext[i]=data[i];
|
||||
msgsnd( canout.id, (struct msgbuf *)&mbuf, length+12, IPC_NOWAIT);
|
||||
memset(&frame, 0, sizeof(struct can_frame)); /* init CAN frame, e.g. LEN = 0 */
|
||||
frame.can_id = id;
|
||||
frame.len = length;
|
||||
for(i=0;i<length;i++) frame.data[i]=data[i];
|
||||
if(send(can_sck, &frame, sizeof(struct can_frame),0)<0) {
|
||||
perror("send frame to CAN-socket"); fflush(stderr);
|
||||
}
|
||||
return(ret);
|
||||
}
|
||||
|
||||
static void can_abort(int sig) {
|
||||
char ss[10];
|
||||
struct shmid_ds buf;
|
||||
|
||||
if(sig) signal(sig,SIG_IGN);
|
||||
if(!server_mode) can_exit(sig);
|
||||
switch(sig){
|
||||
case 0 : strcpy(ss," "); break;
|
||||
case SIGHUP : strcpy(ss,"SIGHUP -"); break;
|
||||
case SIGINT : strcpy(ss,"SIGINT -"); break;
|
||||
case SIGQUIT: strcpy(ss,"SIGQUIT -"); break;
|
||||
case SIGFPE : strcpy(ss,"SIGFPE -"); break;
|
||||
case SIGPIPE: strcpy(ss,"SIGPIPE -"); break;
|
||||
case SIGSEGV: strcpy(ss,"SIGSEGV -"); break;
|
||||
case SIGTERM: strcpy(ss,"SIGTERM -"); break;
|
||||
default: sprintf(ss,"SIG_%d -",sig); break;
|
||||
}
|
||||
switch(sig){
|
||||
default:
|
||||
case SIGHUP :
|
||||
case SIGINT :
|
||||
can_prtime(stderr);
|
||||
fprintf(stderr,"CAN I/O: %s Ignore .....\n",ss);
|
||||
fflush(stderr);
|
||||
signal(sig, can_abort);
|
||||
return;
|
||||
case SIGPIPE:
|
||||
case SIGQUIT:
|
||||
case SIGFPE :
|
||||
case SIGSEGV:
|
||||
case SIGTERM:
|
||||
signal(SIGALRM, can_abort);
|
||||
alarm(2);
|
||||
can_prtime(stderr);
|
||||
fprintf(stderr,"CAN I/O: %s process should stop after 2sec delay...\n",ss);
|
||||
fflush(stderr);
|
||||
close(can_fd);
|
||||
can_fd = can_open = -1;
|
||||
return;
|
||||
case SIGALRM:
|
||||
can_prtime(stderr);
|
||||
fprintf(stderr,"CAN I/O: process stop!\n");
|
||||
fflush(stderr);
|
||||
close(can_lk);
|
||||
can_lk = -1;
|
||||
can_pid = 0;
|
||||
shmdt(can_shm_addr);
|
||||
shmctl(can_shm_id, IPC_STAT, &buf);
|
||||
if(buf.shm_nattch == 0){
|
||||
shmctl(can_shm_id, SHM_UNLOCK, NULL);
|
||||
shmctl(can_shm_id, IPC_RMID, NULL);
|
||||
}
|
||||
exit(sig);
|
||||
}
|
||||
}
|
||||
|
||||
void can_exit(int sig) {
|
||||
char ss[16];
|
||||
int ret;
|
||||
char ss[12];
|
||||
|
||||
struct shmid_ds buf;
|
||||
if(sig) signal(sig,SIG_IGN);
|
||||
putlog("Received signal %d\n", sig);
|
||||
if(server_mode) can_abort(sig);
|
||||
switch (sig) {
|
||||
case 0 : strcpy(ss,"Exiting -"); break;
|
||||
case SIGHUP : strcpy(ss,"SIGHUP -"); break;
|
||||
@ -489,21 +209,16 @@ void can_exit(int sig) {
|
||||
case SIGFPE :
|
||||
case SIGSEGV:
|
||||
case SIGTERM:
|
||||
if(can_fd>=0) close(can_fd);
|
||||
if(can_sck>=0) close(can_sck);
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
char *time2asc(double t){
|
||||
char *time2asc(double t)
|
||||
{
|
||||
static char stmp[10][20];
|
||||
static int itmp=0;
|
||||
char *lin = stmp[itmp];
|
||||
@ -546,3 +261,4 @@ void can_prtime(FILE *fd) {
|
||||
fprintf(fd,"%s ",time2asc(ntime));
|
||||
otime=ntime;
|
||||
}
|
||||
|
||||
|
||||
@ -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 <linux/can.h>
|
||||
|
||||
#ifndef CAN_IO_H__
|
||||
#define CAN_IO_H__
|
||||
|
||||
#include <stdio.h>
|
||||
|
||||
#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();
|
||||
|
||||
@ -16,6 +16,7 @@
|
||||
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <stdio.h>
|
||||
#include <math.h> // fabs
|
||||
#include <sys/wait.h>
|
||||
#include <sys/prctl.h>
|
||||
|
||||
@ -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){
|
||||
|
||||
@ -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);
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user