From a96682bc12526e1676757f344a6991d7d3e1462a Mon Sep 17 00:00:00 2001 From: "Edward V. Emelianov" Date: Wed, 30 Jul 2025 17:23:56 +0300 Subject: [PATCH] some problems fixed; model isn't tested yet --- LibSidServo/examples/SSIIconf.c | 1 + LibSidServo/serial.c | 20 +++++++++++--------- LibSidServo/ssii.c | 13 ++++++++----- LibSidServo/ssii.h | 2 +- 4 files changed, 21 insertions(+), 15 deletions(-) diff --git a/LibSidServo/examples/SSIIconf.c b/LibSidServo/examples/SSIIconf.c index e05b42e..ea5e4d3 100644 --- a/LibSidServo/examples/SSIIconf.c +++ b/LibSidServo/examples/SSIIconf.c @@ -107,6 +107,7 @@ static void dumpHWconf(){ dumpaxe('Y', &HW.Yconf); green("Y bits:\n"); dumpybits(&HW.ybits); + green("Other:\n"); printf("address=%d\n", HW.address); DUMP(eqrate); DUMP(eqadj); diff --git a/LibSidServo/serial.c b/LibSidServo/serial.c index 2d5e845..dc3918c 100644 --- a/LibSidServo/serial.c +++ b/LibSidServo/serial.c @@ -47,7 +47,7 @@ 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 = {0}, mntRtmout = {0}; +static struct timeval encRtmout = {.tv_sec = 0, .tv_usec = 50000}, mntRtmout = {.tv_sec = 0, .tv_usec = 50000}; // encoders raw data typedef struct __attribute__((packed)){ uint8_t magick; @@ -326,8 +326,7 @@ static void *mountthread(void _U_ *u){ int errctr = 0; uint8_t buf[2*sizeof(SSstat)]; SSstat *status = (SSstat*) buf; - if(Conf.RunModel) - while(1){ + if(Conf.RunModel) while(1){ pthread_mutex_lock(&datamutex); // now change data getModData(&mountdata); @@ -344,7 +343,7 @@ static void *mountthread(void _U_ *u){ while(mntfd > -1 && errctr < MAX_ERR_CTR){ // read data to status double tgot = nanotime(), t0 = tgot; - DBG("tgot=%g", tgot); + //DBG("tgot=%g", tgot); if(!MountWriteRead(cmd_getstat, &d) || d.len != sizeof(SSstat)){ #ifdef EBUG DBG("Can't read SSstat, need %zd got %zd bytes", sizeof(SSstat), d.len); @@ -363,7 +362,9 @@ static void *mountthread(void _U_ *u){ SSconvstat(status, &mountdata, tgot); pthread_mutex_unlock(&datamutex); // allow writing & getters - while(nanotime() - t0 < Conf.MountReqInterval) usleep(500); + do{ + usleep(5000); + }while(nanotime() - t0 < Conf.MountReqInterval); t0 = nanotime(); } data_free(&cmd_getstat); @@ -511,17 +512,19 @@ void setStat(mnt_status_t Xstatus, mnt_status_t Ystatus){ // write-read without locking mutex (to be used inside other functions) static int wr(const data_t *out, data_t *in, int needeol){ if((!out && !in) || mntfd < 0) return FALSE; + //double t0 = nanotime(); if(out){ if(out->len != (size_t)write(mntfd, out->buf, out->len)){ DBG("written bytes not equal to need"); return FALSE; } - DBG("Send to mount %zd bytes: %s", out->len, out->buf); + //DBG("Send to mount %zd bytes: %s", out->len, out->buf); if(needeol){ int g = write(mntfd, "\r", 1); // add EOL (void) g; } } + //DBG("sent by %g", nanotime() - t0); uint8_t buf[256]; data_t dumb = {.buf = buf, .maxlen = 256}; if(!in) in = &dumb; // even if user don't ask for answer, try to read to clear trash @@ -531,9 +534,8 @@ static int wr(const data_t *out, data_t *in, int needeol){ if(b < 0) break; // nothing to read -> go out in->buf[in->len++] = (uint8_t) b; } - DBG("got %zd bytes", in->len); - //DBG("Clear trashing input"); - //while(getmntbyte() > -1); + //DBG("got %zd bytes by %g", in->len, nanotime() - t0); + while(getmntbyte() > -1); return TRUE; } diff --git a/LibSidServo/ssii.c b/LibSidServo/ssii.c index 2626762..5cd6a73 100644 --- a/LibSidServo/ssii.c +++ b/LibSidServo/ssii.c @@ -40,11 +40,14 @@ uint16_t SScalcChecksum(uint8_t *buf, int len){ static void axestat(int32_t *prev, int32_t cur, int *nstopped, mnt_status_t *stat){ if(*prev == INT32_MAX){ *stat = MNT_STOPPED; + DBG("START"); }else if(*stat != MNT_STOPPED){ - if(*prev == cur){ - if(++(*nstopped) > MOTOR_STOPPED_CNT) *stat = MNT_STOPPED; + if(*prev == cur && ++(*nstopped) > MOTOR_STOPPED_CNT){ + *stat = MNT_STOPPED; + DBG("AXE stopped"); } }else if(*prev != cur){ + DBG("AXE moving"); //*stat = MNT_SLEWING; *nstopped = 0; } @@ -101,7 +104,7 @@ int SStextcmd(const char *cmd, data_t *answer){ data_t d; d.buf = (uint8_t*) cmd; d.len = d.maxlen = strlen(cmd); - DBG("send %zd bytes: %s", d.len, d.buf); + //DBG("send %zd bytes: %s", d.len, d.buf); return MountWriteRead(&d, answer); } @@ -114,7 +117,7 @@ int SSrawcmd(const char *cmd, data_t *answer){ data_t d; d.buf = (uint8_t*) cmd; d.len = d.maxlen = strlen(cmd); - DBG("send %zd bytes: %s", d.len, d.buf); + //DBG("send %zd bytes: %s", d.len, d.buf); return MountWriteReadRaw(&d, answer); } @@ -190,7 +193,7 @@ mcc_errcodes_t updateMotorPos(){ DBG("FIX motors position to encoders"); int32_t Xpos = X_RAD2MOT(md.encXposition.val), Ypos = Y_RAD2MOT(md.encYposition.val); if(SSsetterI(CMD_MOTXSET, Xpos) && SSsetterI(CMD_MOTYSET, Ypos)){ - DBG("All OK"); + DBG("All OK, Dt=%g", nanotime() - t0); return MCC_E_OK; } }else{ diff --git a/LibSidServo/ssii.h b/LibSidServo/ssii.h index f556659..8819c42 100644 --- a/LibSidServo/ssii.h +++ b/LibSidServo/ssii.h @@ -173,7 +173,7 @@ #define SITECH_LOOP_FREQUENCY (1953.) // amount of consequent same coordinates to detect stop -#define MOTOR_STOPPED_CNT (20) +#define MOTOR_STOPPED_CNT (4) // TODO: take it from settings? // steps per revolution (SSI - x4 - for SSI)