Move the library to the separate repository
This commit is contained in:
197
polltest/main.c
Normal file
197
polltest/main.c
Normal file
@@ -0,0 +1,197 @@
|
||||
/*
|
||||
* This file is part of the libsidservo project.
|
||||
* Copyright 2026 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* This program is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <fcntl.h>
|
||||
#include <poll.h>
|
||||
#include <signal.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <usefull_macros.h>
|
||||
|
||||
// suppose that we ONLY poll data
|
||||
#define XYBUFSZ (128)
|
||||
|
||||
struct{
|
||||
int help;
|
||||
char *Xpath;
|
||||
char *Ypath;
|
||||
double dt;
|
||||
} G = {
|
||||
.Xpath = "/dev/encoder_X0",
|
||||
.Ypath = "/dev/encoder_Y0",
|
||||
.dt = 0.001,
|
||||
};
|
||||
|
||||
sl_option_t options[] = {
|
||||
{"help", NO_ARGS, NULL, 'h', arg_int, APTR(&G.help), "show this help"},
|
||||
{"Xpath", NEED_ARG, NULL, 'X', arg_string, APTR(&G.Xpath), "path to X encoder"},
|
||||
{"Ypath", NEED_ARG, NULL, 'Y', arg_string, APTR(&G.Ypath), "path to Y encoder"},
|
||||
{"dt", NEED_ARG, NULL, 'd', arg_double, APTR(&G.dt), "request interval (1e-4..10s)"},
|
||||
};
|
||||
|
||||
typedef struct{
|
||||
char buf[XYBUFSZ+1];
|
||||
int len;
|
||||
} buf_t;
|
||||
|
||||
static int Xfd = -1, Yfd = -1;
|
||||
|
||||
void signals(int sig){
|
||||
if(sig){
|
||||
signal(sig, SIG_IGN);
|
||||
DBG("Get signal %d, quit.\n", sig);
|
||||
}
|
||||
DBG("close");
|
||||
if(Xfd > 0){ close(Xfd); Xfd = -1; }
|
||||
if(Yfd > 0){ close(Yfd); Yfd = -1; }
|
||||
exit(sig);
|
||||
}
|
||||
|
||||
static int op(const char *nm){
|
||||
int fd = open(nm, O_RDWR|O_NOCTTY|O_NONBLOCK);
|
||||
if(fd < 0) ERR("Can't open %s", nm);
|
||||
struct termios2 tty;
|
||||
if(ioctl(fd, TCGETS2, &tty)) ERR("Can't read TTY settings");
|
||||
tty.c_lflag = 0; // ~(ICANON | ECHO | ECHOE | ISIG)
|
||||
tty.c_iflag = 0; // don't do any changes in input stream
|
||||
tty.c_oflag = 0; // don't do any changes in output stream
|
||||
tty.c_cflag = BOTHER | CS8 | CREAD | CLOCAL; // other speed, 8bit, RW, ignore line ctrl
|
||||
tty.c_ispeed = 1000000;
|
||||
tty.c_ospeed = 1000000;
|
||||
if(ioctl(fd, TCSETS2, &tty)) ERR("Can't set TTY settings");
|
||||
// try to set exclusive
|
||||
if(ioctl(fd, TIOCEXCL)){DBG("Can't make exclusive");}
|
||||
return fd;
|
||||
}
|
||||
|
||||
// write to buffer next data portion; return FALSE in case of error
|
||||
static int readstrings(buf_t *buf, int fd){
|
||||
FNAME();
|
||||
if(!buf){WARNX("Empty buffer"); return FALSE;}
|
||||
int L = XYBUFSZ - buf->len;
|
||||
if(L == 0){
|
||||
DBG("buffer overfull!", buf->len);
|
||||
char *lastn = strrchr(buf->buf, '\n');
|
||||
if(lastn){
|
||||
fprintf(stderr, "BUFOVR: _%s_", buf->buf);
|
||||
++lastn;
|
||||
buf->len = XYBUFSZ - (lastn - buf->buf);
|
||||
DBG("Memmove %d", buf->len);
|
||||
memmove(lastn, buf->buf, buf->len);
|
||||
buf->buf[buf->len] = 0;
|
||||
}else buf->len = 0;
|
||||
L = XYBUFSZ - buf->len;
|
||||
}
|
||||
int got = read(fd, &buf->buf[buf->len], L);
|
||||
if(got < 0){
|
||||
WARN("read()");
|
||||
return FALSE;
|
||||
}else if(got == 0){ DBG("NO data"); return TRUE; }
|
||||
buf->len += got;
|
||||
buf->buf[buf->len] = 0;
|
||||
DBG("buf[%d]: %s", buf->len, buf->buf);
|
||||
return TRUE;
|
||||
}
|
||||
// return TRUE if got, FALSE if no data found
|
||||
static int getdata(buf_t *buf, long *out){
|
||||
if(!buf || buf->len < 1) return FALSE;
|
||||
// read record between last '\n' and previous (or start of string)
|
||||
char *last = &buf->buf[buf->len - 1];
|
||||
//DBG("buf: _%s_", buf->buf);
|
||||
if(*last != '\n') return FALSE;
|
||||
*last = 0;
|
||||
//DBG("buf: _%s_", buf->buf);
|
||||
char *prev = strrchr(buf->buf, '\n');
|
||||
if(!prev) prev = buf->buf;
|
||||
else{
|
||||
fprintf(stderr, "MORETHANONE: _%s_", buf->buf);
|
||||
++prev; // after last '\n'
|
||||
}
|
||||
if(out) *out = atol(prev);
|
||||
// clear buffer
|
||||
buf->len = 0;
|
||||
return TRUE;
|
||||
}
|
||||
// try to write '\n' asking new data portion; return FALSE if failed
|
||||
static int asknext(int fd){
|
||||
FNAME();
|
||||
if(fd < 0) return FALSE;
|
||||
int i = 0;
|
||||
for(; i < 5; ++i){
|
||||
int l = write(fd, "\n", 1);
|
||||
//DBG("l=%d", l);
|
||||
if(1 == l) return TRUE;
|
||||
usleep(100);
|
||||
}
|
||||
DBG("5 tries... failed!");
|
||||
return FALSE;
|
||||
}
|
||||
|
||||
int main(int argc, char **argv){
|
||||
buf_t xbuf, ybuf;
|
||||
long xlast, ylast;
|
||||
double xtlast, ytlast;
|
||||
sl_init();
|
||||
sl_parseargs(&argc, &argv, options);
|
||||
if(G.help) sl_showhelp(-1, options);
|
||||
if(G.dt < 1e-4) ERRX("dx too small");
|
||||
if(G.dt > 10.) ERRX("dx too big");
|
||||
Xfd = op(G.Xpath);
|
||||
Yfd = op(G.Ypath);
|
||||
struct pollfd pfds[2];
|
||||
pfds[0].fd = Xfd; pfds[0].events = POLLIN;
|
||||
pfds[1].fd = Yfd; pfds[1].events = POLLIN;
|
||||
double t0x, t0y, tstart;
|
||||
asknext(Xfd); asknext(Yfd);
|
||||
t0x = t0y = tstart = sl_dtime();
|
||||
DBG("Start");
|
||||
do{ // main cycle
|
||||
if(poll(pfds, 2, 0) < 0){
|
||||
WARN("poll()");
|
||||
break;
|
||||
}
|
||||
if(pfds[0].revents && POLLIN){
|
||||
DBG("got X");
|
||||
if(!readstrings(&xbuf, Xfd)) break;
|
||||
}
|
||||
if(pfds[1].revents && POLLIN){
|
||||
DBG("got Y");
|
||||
if(!readstrings(&ybuf, Yfd)) break;
|
||||
}
|
||||
double curt = sl_dtime();
|
||||
if(getdata(&xbuf, &xlast)) xtlast = curt;
|
||||
if(curt - t0x >= G.dt){ // get last records
|
||||
if(curt - xtlast < 1.5*G.dt)
|
||||
printf("%-14.4fX=%ld\n", xtlast-tstart, xlast);
|
||||
if(!asknext(Xfd)) break;
|
||||
t0x = (curt - t0x < 2.*G.dt) ? t0x + G.dt : curt;
|
||||
}
|
||||
curt = sl_dtime();
|
||||
if(getdata(&ybuf, &ylast)) ytlast = curt;
|
||||
if(curt - t0y >= G.dt){ // get last records
|
||||
if(curt - ytlast < 1.5*G.dt)
|
||||
printf("%-14.4fY=%ld\n", ytlast-tstart, ylast);
|
||||
if(!asknext(Yfd)) break;
|
||||
t0y = (curt - t0y < 2.*G.dt) ? t0y + G.dt : curt;
|
||||
}
|
||||
}while(Xfd > 0 && Yfd > 0);
|
||||
DBG("OOps: disconnected");
|
||||
signals(0);
|
||||
return 0;
|
||||
}
|
||||
Reference in New Issue
Block a user