From cc870491f5025f5b3d6888298d97e057758ae4a4 Mon Sep 17 00:00:00 2001 From: Edward Emelianov Date: Fri, 3 Apr 2026 10:39:14 +0300 Subject: [PATCH] fixed deadlock --- Auxiliary_utils/LibSidServo/examples/goto.c | 4 ++ .../LibSidServo/libsidservo.creator.user | 2 +- Auxiliary_utils/LibSidServo/movingfilter.c- | 46 ------------------- Auxiliary_utils/LibSidServo/serial.c | 42 ++++++++++++++++- Auxiliary_utils/LibSidServo/ssii.c | 34 -------------- 5 files changed, 45 insertions(+), 83 deletions(-) delete mode 100644 Auxiliary_utils/LibSidServo/movingfilter.c- diff --git a/Auxiliary_utils/LibSidServo/examples/goto.c b/Auxiliary_utils/LibSidServo/examples/goto.c index b17deab..7749413 100644 --- a/Auxiliary_utils/LibSidServo/examples/goto.c +++ b/Auxiliary_utils/LibSidServo/examples/goto.c @@ -65,8 +65,12 @@ void signals(int sig){ signal(sig, SIG_IGN); DBG("Get signal %d, quit.\n", sig); } + DBG("Stop!"); + Mount.stop(); + usleep(10000); DBG("Quit"); Mount.quit(); + usleep(10000); DBG("close"); if(fcoords) fclose(fcoords); exit(sig); diff --git a/Auxiliary_utils/LibSidServo/libsidservo.creator.user b/Auxiliary_utils/LibSidServo/libsidservo.creator.user index ba48f10..c97d5a0 100644 --- a/Auxiliary_utils/LibSidServo/libsidservo.creator.user +++ b/Auxiliary_utils/LibSidServo/libsidservo.creator.user @@ -1,6 +1,6 @@ - + EnvironmentId diff --git a/Auxiliary_utils/LibSidServo/movingfilter.c- b/Auxiliary_utils/LibSidServo/movingfilter.c- deleted file mode 100644 index 16001b2..0000000 --- a/Auxiliary_utils/LibSidServo/movingfilter.c- +++ /dev/null @@ -1,46 +0,0 @@ -#include -#include -#include - -#define NFILT (5) - -static double filterK[NFILT]; - -static void buildFilter(){ - filterK[NFILT-1] = 1.; - double sum = 1.; - for(int i = NFILT-2; i > -1; --i){ - filterK[i] = (filterK[i+1] + 1.) * 1.1; - sum += filterK[i]; - } - for(int i = 0; i < NFILT; ++i){ - filterK[i] /= sum; - fprintf(stderr, "%d: %g\n", i, filterK[i]); - } -} - -static double filter(double val){ - static int ctr = 0; - static double lastvals[NFILT] = {0.}; - for(int i = NFILT-1; i > 0; --i) lastvals[i] = lastvals[i-1]; - lastvals[0] = val; - double r = 0.; - if(ctr < NFILT){ - ++ctr; - return val; - } - for(int i = 0; i < NFILT; ++i) r += filterK[i] * lastvals[i]; - return r; -} - -int main(int argc, char **argv){ - buildFilter(); - printf("Signal\tNoiced\tFiltered\n"); - for(int i = 0; i < 100; ++i){ - double di = (double)i; - double sig = di * di / 1e5 + sin(i * M_PI / 1500.); - double noiced = sig + 0.1 * (drand48() - 0.5); - printf("%.3f\t%.3f\t%.3f\n", sig, noiced, filter(noiced)); - } - return 0; -} \ No newline at end of file diff --git a/Auxiliary_utils/LibSidServo/serial.c b/Auxiliary_utils/LibSidServo/serial.c index d22916e..383f991 100644 --- a/Auxiliary_utils/LibSidServo/serial.c +++ b/Auxiliary_utils/LibSidServo/serial.c @@ -481,6 +481,41 @@ static void chkModStopped(double *prev, double cur, int *nstopped, axis_status_t *prev = cur; } +// Next two functions runs under locked mountdata_t mutex and shouldn't lock it again!! +static axis_status_t chkstopstat(int32_t *prev, int32_t cur, int32_t tag, int *nstopped, axis_status_t stat){ + if(*prev == INT32_MAX){ + stat = AXIS_STOPPED; + DBG("START"); + }else if(stat == AXIS_GONNASTOP || (stat != AXIS_STOPPED && cur == tag)){ // got command "stop" or motor is on target + if(*prev == cur){ + DBG("Test for stop, nstopped=%d", *nstopped); + if(++(*nstopped) > MOTOR_STOPPED_CNT){ + stat = AXIS_STOPPED; + DBG("AXIS stopped"); + } + }else *nstopped = 0; + }else if(*prev != cur){ + DBG("AXIS moving"); + *nstopped = 0; + } + *prev = cur; + return stat; +} + +// check for stopped/pointing states +static void ChkStopped(const SSstat *s, mountdata_t *m){ + static int32_t Xmot_prev = INT32_MAX, Ymot_prev = INT32_MAX; // previous coordinates + static int Xnstopped = 0, Ynstopped = 0; // counters to get STOPPED state + axis_status_t Xstat, Ystat; + Xstat = chkstopstat(&Xmot_prev, s->Xmot, m->Xtarget, &Xnstopped, m->Xstate); + Ystat = chkstopstat(&Ymot_prev, s->Ymot, m->Ytarget, &Ynstopped, m->Ystate); + if(Xstat != m->Xstate || Ystat != m->Ystate){ + DBG("Status changed"); + mountdata.Xstate = Xstat; + mountdata.Ystate = Ystat; + } +} + // main mount thread static void *mountthread(void _U_ *u){ int errctr = 0; @@ -552,6 +587,7 @@ static void *mountthread(void _U_ *u){ pthread_mutex_lock(&datamutex); // now change data SSconvstat(status, &mountdata, &tcur); + ChkStopped(status, &mountdata); pthread_mutex_unlock(&datamutex); // allow writing & getters do{ @@ -585,11 +621,12 @@ static int ttyopen(const char *path, speed_t speed){ 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 + // wihthout "HUPCL" it doesn't disconnects + tty.c_cflag = HUPCL | BOTHER | CS8 | CREAD | CLOCAL; // other speed, 8bit, RW, ignore line ctrl tty.c_ispeed = speed; tty.c_ospeed = speed; tty.c_cc[VMIN] = 0; // non-canonical mode - tty.c_cc[VTIME] = 5; + tty.c_cc[VTIME] = 0; if(ioctl(fd, TCSETS2, &tty)){ DBG("Can't set TTY settings"); close(fd); @@ -674,6 +711,7 @@ create_thread: // close all opened serial devices and quit threads void closeSerial(){ GlobExit = 1; + pthread_mutex_unlock(&datamutex); DBG("Give 100ms to proper close"); usleep(100000); DBG("Force closed all devices"); diff --git a/Auxiliary_utils/LibSidServo/ssii.c b/Auxiliary_utils/LibSidServo/ssii.c index e7c81cc..e7193cc 100644 --- a/Auxiliary_utils/LibSidServo/ssii.c +++ b/Auxiliary_utils/LibSidServo/ssii.c @@ -44,39 +44,6 @@ uint16_t SScalcChecksum(uint8_t *buf, int len){ return checksum; } -// Next three functions runs under locked mountdata_t mutex and shouldn't call locked it again!! -static axis_status_t chkstopstat(int32_t *prev, int32_t cur, int32_t tag, int *nstopped, axis_status_t stat){ - if(*prev == INT32_MAX){ - stat = AXIS_STOPPED; - DBG("START"); - }else if(stat == AXIS_GONNASTOP || (stat != AXIS_STOPPED && cur == tag)){ // got command "stop" or motor is on target - if(*prev == cur){ - DBG("Test for stop, nstopped=%d", *nstopped); - if(++(*nstopped) > MOTOR_STOPPED_CNT){ - stat = AXIS_STOPPED; - DBG("AXIS stopped"); - } - }else *nstopped = 0; - }else if(*prev != cur){ - DBG("AXIS moving"); - *nstopped = 0; - } - *prev = cur; - return stat; -} -// check for stopped/pointing states -static void ChkStopped(const SSstat *s, mountdata_t *m){ - static int32_t Xmot_prev = INT32_MAX, Ymot_prev = INT32_MAX; // previous coordinates - static int Xnstopped = 0, Ynstopped = 0; // counters to get STOPPED state - axis_status_t Xstat, Ystat; - Xstat = chkstopstat(&Xmot_prev, s->Xmot, m->Xtarget, &Xnstopped, m->Xstate); - Ystat = chkstopstat(&Ymot_prev, s->Ymot, m->Ytarget, &Ynstopped, m->Ystate); - if(Xstat != m->Xstate || Ystat != m->Ystate){ - DBG("Status changed"); - setStat(Xstat, Ystat); - } -} - /** * @brief SSconvstat - convert stat from SSII format to human * @param s (i) - just read data @@ -87,7 +54,6 @@ void SSconvstat(const SSstat *s, mountdata_t *m, struct timespec *t){ if(!s || !m || !t) return; m->motXposition.val = X_MOT2RAD(s->Xmot); m->motYposition.val = Y_MOT2RAD(s->Ymot); - ChkStopped(s, m); m->motXposition.t = m->motYposition.t = *t; // fill encoder data from here, as there's no separate enc thread if(!Conf.SepEncoder){