timur/LibSidServo
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Fixed some troubles

Browse Source
This commit is contained in:
Edward V. Emelianov
2026-03-16 18:00:04 +03:00
parent 04dba21ddc
commit b0d0a323ee
6 changed files with 119 additions and 66 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
.qtcreator/libsidservo.creator.user
View File

@@ -1,6 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?>
<!DOCTYPE QtCreatorProject>
<!-- Written by QtCreator 18.0.0, 2026-03-11T12:36:26. -->
<!-- Written by QtCreator 18.0.0, 2026-03-13T12:23:05. -->
<qtcreator>
<data>
<variable>EnvironmentId</variable>

4
examples/scmd_traectory.c
View File

@@ -146,10 +146,12 @@ int main(int argc, char **argv){
ERRX("Can't open error log %s", G.errlog);
else
fprintf(errlog, "# time Xerr'' Yerr'' // target - real\n");
setbuf(errlog, NULL);
}
if(G.coordsoutput){
if(!(fcoords = fopen(G.coordsoutput, "w")))
ERRX("Can't open %s", G.coordsoutput);
setbuf(fcoords, NULL);
}else fcoords = stdout;
Config = readServoConf(G.conffile);
if(!Config || G.dumpconf){
@@ -178,7 +180,7 @@ int main(int argc, char **argv){
signal(SIGINT, signals); // ctrl+C - quit
signal(SIGQUIT, signals); // ctrl+\ - quit
signal(SIGTSTP, SIG_IGN); // ignore ctrl+Z
chk0(G.Ncycles);
// chk0(G.Ncycles);
logmnt(fcoords, NULL);
if(pthread_create(&dthr, NULL, dumping, NULL)) ERRX("Can't run dump thread");
;

2
libsidservo.creator.user
View File

@@ -1,6 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?>
<!DOCTYPE QtCreatorProject>
<!-- Written by QtCreator 18.0.0, 2026-03-11T12:36:26. -->
<!-- Written by QtCreator 18.0.0, 2026-03-13T12:23:05. -->
<qtcreator>
<data>
<variable>EnvironmentId</variable>

47
main.c
View File

@@ -202,6 +202,7 @@ static mcc_errcodes_t init(conf_t *c){
if(!Xmodel || !Ymodel || !openMount()) return MCC_E_FAILED;
return MCC_E_OK;
}
DBG("Try to open mount device");
if(!Conf.MountDevPath || Conf.MountDevSpeed < MOUNT_BAUDRATE_MIN){
DBG("Define mount device path and speed");
ret = MCC_E_BADFORMAT;
@@ -209,33 +210,53 @@ static mcc_errcodes_t init(conf_t *c){
DBG("Can't open %s with speed %d", Conf.MountDevPath, Conf.MountDevSpeed);
ret = MCC_E_MOUNTDEV;
}
if(Conf.SepEncoder){
if(!Conf.EncoderDevPath && !Conf.EncoderXDevPath){
DBG("Define encoder device path");
ret = MCC_E_BADFORMAT;
}else if(!openEncoder()){
DBG("Can't open encoder device");
ret = MCC_E_ENCODERDEV;
}
}
// TODO: read hardware configuration on init
if(Conf.EncoderSpeedInterval < Conf.EncoderReqInterval * MCC_CONF_MIN_SPEEDC || Conf.EncoderSpeedInterval > MCC_CONF_MAX_SPEEDINT){
DBG("Wrong speed interval");
ret = MCC_E_BADFORMAT;
}
if(!SSrawcmd(CMD_EXITACM, NULL)) ret = MCC_E_FAILED;
if(ret != MCC_E_OK) return ret;
DBG("Exit ACM, exit manual mode");
SSrawcmd(CMD_EXITACM, NULL);
SStextcmd(CMD_AUTOX, NULL);
SStextcmd(CMD_AUTOY, NULL);
// read HW config to update constants
hardware_configuration_t HW;
if(MCC_E_OK != get_hwconf(&HW)) return MCC_E_FAILED;
DBG("Read hardware configuration");
ret = MCC_E_FAILED;
for(int i = 0; i < MAX_ERR_CTR; ++i){
DBG("TRY %d..", i);
ret = get_hwconf(&HW);
if(ret == MCC_E_OK) break;
}
if(MCC_E_OK != ret) return ret;
// make a pause for actual encoder's values
DBG("Check encoders");
if(Conf.SepEncoder){
if(!Conf.EncoderDevPath && !Conf.EncoderXDevPath){
DBG("Define encoder device path");
ret = MCC_E_BADFORMAT;
}else{
ret = MCC_E_ENCODERDEV;
for(int i = 0; i < MAX_ERR_CTR; ++i){
if(openEncoder()){
ret = MCC_E_OK;
break;
}
}
}
}
if(MCC_E_OK != ret) return ret;
double t0 = timefromstart();
while(timefromstart() - t0 < Conf.EncoderReqInterval) usleep(1000);
DBG("Wait for first encoders' measurement");
while(timefromstart() - t0 < Conf.EncoderReqInterval * 15.) usleep(1000);
DBG("Update motor position");
mcc_errcodes_t e = updateMotorPos();
// and refresh data after updating
DBG("Wait for next mount reading");
t0 = timefromstart();
while(timefromstart() - t0 < Conf.MountReqInterval * 3.) usleep(1000);
while(timefromstart() - t0 < Conf.MountReqInterval * 5.) usleep(1000);
DBG("ALL READY!");
return e;
}

129
serial.c
View File

@@ -49,7 +49,11 @@ static pthread_mutex_t mntmutex = PTHREAD_MUTEX_INITIALIZER,
// encoders thread and mount thread
static pthread_t encthread, mntthread;
// max timeout for 1.5 bytes of encoder and 2 bytes of mount - for `select`
static struct timeval encRtmout = {.tv_sec = 0, .tv_usec = 100}, mntRtmout = {.tv_sec = 0, .tv_usec = 50000};
// this values will be modified later
static struct timeval encRtmout = {.tv_sec = 0, .tv_usec = 100}, // encoder reading timeout
mnt1Rtmout = {.tv_sec = 0, .tv_usec = 200000}, // first reading
mntRtmout = {.tv_sec = 0, .tv_usec = 50000}; // next readings
// encoders raw data
typedef struct __attribute__((packed)){
uint8_t magick;
@@ -231,43 +235,60 @@ static int getencbyte(){
}while(1);
return (int)byte;
}
// read 1 byte from mount; return -1 if nothing to read, -2 if disconnected
static int getmntbyte(){
if(mntfd < 0) return -1;
uint8_t byte;
/**
* @brief readmntdata - read data
* @param buffer - input buffer
* @param maxlen - maximal buffer length
* @return amount of bytes read or -1 in case of error
*/
static int readmntdata(uint8_t *buffer, int maxlen){
if(mntfd < 0){
DBG("mntfd non opened");
return -1;
}
if(!buffer || maxlen < 1) return 0;
DBG("ask for %d bytes", maxlen);
int got = 0;
fd_set rfds;
/* ssize_t l = read(mntfd, &byte, 1);
//DBG("MNT read=%zd byte=0x%X", l, byte);
if(l == 0) return -1;
if(l != 1) return -2; // disconnected ??
return (int) byte;*/
struct timeval tv = mnt1Rtmout;
do{
FD_ZERO(&rfds);
FD_SET(mntfd, &rfds);
struct timeval tv = mntRtmout;
DBG("select");
int retval = select(mntfd + 1, &rfds, NULL, NULL, &tv);
DBG("returned %d", retval);
if(retval < 0){
if(errno == EINTR) continue;
DBG("Error in select()");
return -1;
}
//DBG("FD_ISSET = %d", FD_ISSET(mntfd, &rfds));
if(FD_ISSET(mntfd, &rfds)){
ssize_t l = read(mntfd, &byte, 1);
//DBG("MNT read=%zd byte=0x%X", l, byte);
if(l != 1){
ssize_t l = read(mntfd, buffer, maxlen);
if(l == 0){
DBG("read ZERO");
continue;
}
if(l < 0){
DBG("Mount disconnected?");
return -2; // disconnected ??
}
buffer += l;
maxlen -= l;
got += l;
}else{
DBG("no new data after %d bytes (%s)", got, buffer - got);
break;
} else return -1;
}while(1);
return (int)byte;
}
tv = mntRtmout;
}while(maxlen);
return got;
}
// clear data from input buffer
static void clrmntbuf(){
if(mntfd < 0) return;
uint8_t byte;
uint8_t bytes[256];
fd_set rfds;
do{
FD_ZERO(&rfds);
@@ -280,9 +301,10 @@ static void clrmntbuf(){
break;
}
if(FD_ISSET(mntfd, &rfds)){
ssize_t l = read(mntfd, &byte, 1);
if(l != 1) break;
} else break;
ssize_t l = read(mntfd, &bytes, 256);
if(l < 1) break;
DBG("clr got %zd bytes: %s", l, bytes);
}else break;
}while(1);
}
@@ -426,15 +448,19 @@ static void *encoderthread2(void _U_ *u){
if(curt - t0[i] >= Conf.EncoderReqInterval){ // get last records
if(curt - mtlast[i] < 1.5*Conf.EncoderReqInterval){
pthread_mutex_lock(&datamutex);
double pos = (double)msrlast[i];
//DBG("pos[%d]=%g", i, pos);
;;
//DBG("Got med: %g", pos);
if(i == 0){
mountdata.encXposition.val = Xenc2rad((double)msrlast[i]);
mountdata.encXposition.val = Xenc2rad(pos);
curtime(&mountdata.encXposition.t);
/*DBG("msrlast=%ld, Xpos.val=%g, t=%zd; XEzero=%d, SPR=%g",
msrlast[i], mountdata.encXposition.val, mountdata.encXposition.t.tv_sec,
X_ENC_ZERO, X_ENC_STEPSPERREV);*/
getXspeed();
}else{
mountdata.encYposition.val = Yenc2rad((double)msrlast[i]);
mountdata.encYposition.val = Yenc2rad(pos);
curtime(&mountdata.encYposition.t);
getYspeed();
}
@@ -496,7 +522,7 @@ static void chkModStopped(double *prev, double cur, int *nstopped, axis_status_t
// main mount thread
static void *mountthread(void _U_ *u){
int errctr = 0;
uint8_t buf[2*sizeof(SSstat)];
uint8_t buf[sizeof(SSstat)];
SSstat *status = (SSstat*) buf;
bzero(&mountdata, sizeof(mountdata));
double t0 = timefromstart(), tstart = t0, tcur = t0;
@@ -616,15 +642,18 @@ static int ttyopen(const char *path, speed_t speed){
// return FALSE if failed
int openEncoder(){
// TODO: open real devices in "model" mode too!
if(Conf.RunModel) return TRUE;
if(!Conf.SepEncoder) return FALSE; // try to open separate encoder when it's absent
/*
encRtmout.tv_sec = 0;
encRtmout.tv_usec = 100000000 / Conf.EncoderDevSpeed; // 10 bytes
*/
if(Conf.SepEncoder == 1){ // only one device
DBG("One device");
if(encfd[0] > -1) close(encfd[0]);
encfd[0] = ttyopen(Conf.EncoderDevPath, (speed_t) Conf.EncoderDevSpeed);
if(encfd[0] < 0) return FALSE;
encRtmout.tv_sec = 0;
encRtmout.tv_usec = 100000000 / Conf.EncoderDevSpeed; // 10 bytes
if(pthread_create(&encthread, NULL, encoderthread1, NULL)){
close(encfd[0]);
encfd[0] = -1;
@@ -638,8 +667,6 @@ int openEncoder(){
encfd[i] = ttyopen(paths[i], (speed_t) Conf.EncoderDevSpeed);
if(encfd[i] < 0) return FALSE;
}
encRtmout.tv_sec = 0;
encRtmout.tv_usec = 100000000 / Conf.EncoderDevSpeed;
if(pthread_create(&encthread, NULL, encoderthread2, NULL)){
for(int i = 0; i < 2; ++i){
close(encfd[i]);
@@ -654,6 +681,7 @@ int openEncoder(){
// return FALSE if failed
int openMount(){
// TODO: open real devices in "model" mode too!
if(Conf.RunModel) goto create_thread;
if(mntfd > -1) close(mntfd);
DBG("Open mount %s @ %d", Conf.MountDevPath, Conf.MountDevSpeed);
@@ -661,16 +689,13 @@ int openMount(){
if(mntfd < 0) return FALSE;
DBG("mntfd=%d", mntfd);
// clear buffer
while(getmntbyte() > -1);
/*int g = write(mntfd, "XXS\r", 4);
DBG("Written %d", g);
uint8_t buf[100];
do{
ssize_t l = read(mntfd, buf, 100);
DBG("got %zd", l);
}while(1);*/
clrmntbuf();
/*
mnt1Rtmout.tv_sec = 0;
mnt1Rtmout.tv_usec = 500000000 / Conf.MountDevSpeed; // 50 bytes * 10bits / speed
mntRtmout.tv_sec = 0;
mntRtmout.tv_usec = 500000000 / Conf.MountDevSpeed; // 50 bytes * 10bits / speed
mntRtmout.tv_usec = mnt1Rtmout.tv_usec / 50;
*/
create_thread:
if(pthread_create(&mntthread, NULL, mountthread, NULL)){
DBG("Can't create mount thread");
@@ -686,8 +711,7 @@ create_thread:
// close all opened serial devices and quit threads
void closeSerial(){
// TODO: close devices in "model" mode too!
if(Conf.RunModel) return;
DBG("CLOSE all devices");
if(mntfd > -1){
DBG("Cancel mount thread");
pthread_cancel(mntthread);
@@ -737,26 +761,29 @@ static int wr(const data_t *out, data_t *in, int needeol){
DBG("Wrong arguments or no mount fd");
return FALSE;
}
DBG("clrbuf");
clrmntbuf();
if(out){
DBG("write %zd bytes (%s)", out->len, out->buf);
if(out->len != (size_t)write(mntfd, out->buf, out->len)){
DBG("written bytes not equal to need");
return FALSE;
}
//DBG("eol, mntfd=%d", mntfd);
if(needeol){
int g = write(mntfd, "\r", 1); // add EOL
(void) g;
}
usleep(50000); // add little pause so that the idiot has time to swallow
//usleep(50000); // add little pause so that the idiot has time to swallow
}
if(!in) return TRUE;
in->len = 0;
for(size_t i = 0; i < in->maxlen; ++i){
int b = getmntbyte();
if(b < 0) break; // nothing to read -> go out
in->buf[in->len++] = (uint8_t) b;
if(!in || in->maxlen < 1) return TRUE;
int got = readmntdata(in->buf, in->maxlen);
if(got < 0){
DBG("Error reading mount data!");
in->len = 0;
return FALSE;
}
while(getmntbyte() > -1);
in->len = got;
return TRUE;
}
@@ -767,15 +794,17 @@ static int wr(const data_t *out, data_t *in, int needeol){
* @return FALSE if failed
*/
int MountWriteRead(const data_t *out, data_t *in){
if(Conf.RunModel) return -1;
if(Conf.RunModel) return FALSE;
double t0 = timefromstart();
pthread_mutex_lock(&mntmutex);
int ret = wr(out, in, 1);
pthread_mutex_unlock(&mntmutex);
DBG("Got %gus", (timefromstart()-t0)*1e6);
return ret;
}
// send binary data - without EOL
int MountWriteReadRaw(const data_t *out, data_t *in){
if(Conf.RunModel) return -1;
if(Conf.RunModel) return FALSE;
pthread_mutex_lock(&mntmutex);
int ret = wr(out, in, 0);
pthread_mutex_unlock(&mntmutex);

1
ssii.c
View File

@@ -82,6 +82,7 @@ void SSconvstat(const SSstat *s, mountdata_t *m, struct timespec *t){
m->motXposition.t = m->motYposition.t = *t;
// fill encoder data from here, as there's no separate enc thread
if(!Conf.SepEncoder){
DBG("ENCODER from SSII");
m->encXposition.val = Xenc2rad(s->Xenc);
DBG("encx: %g", m->encXposition.val);
m->encYposition.val = Yenc2rad(s->Yenc);