From 3f7b5081ead28039e85146e74957b7eee41d2cd9 Mon Sep 17 00:00:00 2001 From: Edward Emelianov Date: Wed, 14 May 2025 09:04:11 +0300 Subject: [PATCH] add more statistics --- LocCorr_new/cameracapture.c | 97 +++++++++++++++++++++++++++---------- LocCorr_new/cameracapture.h | 2 + LocCorr_new/config.h | 12 ++--- LocCorr_new/hikrobot.c | 10 ++-- LocCorr_new/hikrobot.h | 3 ++ LocCorr_new/imagefile.c | 3 +- LocCorr_new/imagefile.h | 6 +++ LocCorr_new/improc.c | 34 +++++++------ LocCorr_new/steppers.c | 79 ++++++++++++++++++------------ LocCorr_new/steppers.h | 3 ++ 10 files changed, 168 insertions(+), 81 deletions(-) diff --git a/LocCorr_new/cameracapture.c b/LocCorr_new/cameracapture.c index 6bff645..67929d4 100644 --- a/LocCorr_new/cameracapture.c +++ b/LocCorr_new/cameracapture.c @@ -21,6 +21,7 @@ #include #include #include +#include #include "cameracapture.h" #include "cmdlnopts.h" @@ -42,6 +43,15 @@ static frameformat curformat; static frameformat maxformat; static frameformat stepformat; +// statistics of last image +typedef struct{ + Imtype minval, maxval, bkg; + float avg, xc, yc; + ptstat_t stat; +} imdata_t; + +static imdata_t lastimdata = {0}; + static void changeformat(){ if(!theCam) return; if(maxformat.h < 1 || maxformat.w < 1){ @@ -190,7 +200,7 @@ static int needs_exposure_adjustment(const Image *I, float curr_x, float curr_y) float avg = I->avg_intensity; float dx = fabsf(curr_x - last_centroid_x); float dy = fabsf(curr_y - last_centroid_y); - LOGDBG("avg: %g, curr_x: %g, curr_y: %g", avg, curr_x, curr_y); + //LOGDBG("avg: %g, curr_x: %g, curr_y: %g", avg, curr_x, curr_y); // don't change brightness if average value in 5..50 if(avg > 5.f && avg < 50.f){ last_avg_intensity = avg; @@ -231,25 +241,13 @@ static void *procthread(void* v){ double t0 = sl_dtime(); #endif while(!stopwork){ - while(iCaptured < 0) usleep(1000); + DBG("===== iCaptured=%d", iCaptured); pthread_mutex_lock(&capt_mutex); if(Icap[iCaptured]){ - DBG("---- got image #%d @ %g", iCaptured, sl_dtime() - t0); + DBG("===== got image #%d @ %g", iCaptured, sl_dtime() - t0); Image *oIma = Icap[iCaptured]; // take image here and free buffer Icap[iCaptured] = NULL; pthread_mutex_unlock(&capt_mutex); - if(theconf.expmethod == EXPAUTO){ - float xc, yc; - getcenter(&xc, &yc); - if(needs_exposure_adjustment(oIma, xc, yc)) recalcexp(oIma); - }else{ - if(fabs(theconf.fixedexp - exptime) > FLT_EPSILON) - exptime = theconf.fixedexp; - if(fabs(theconf.gain - gain) > FLT_EPSILON) - gain = theconf.gain; - if(fabs(theconf.brightness - brightness) > FLT_EPSILON) - brightness = theconf.brightness; - } if(process){ if(theconf.medfilt){ Image *X = get_median(oIma, theconf.medseed); @@ -259,12 +257,38 @@ static void *procthread(void* v){ oIma = X; } } + DBG("===== process"); process(oIma); + DBG("===== done"); + lastimdata.avg = oIma->avg_intensity; + lastimdata.bkg = oIma->background; + lastimdata.minval = oIma->minval; + lastimdata.maxval = oIma->maxval; + lastimdata.stat = oIma->stat; + getcenter(&lastimdata.xc, &lastimdata.yc); } + if(theconf.expmethod == EXPAUTO){ + DBG("test"); + if(needs_exposure_adjustment(oIma, lastimdata.xc, lastimdata.yc)) recalcexp(oIma); + DBG("done"); + }else{ + if(fabs(theconf.fixedexp - exptime) > FLT_EPSILON) + exptime = theconf.fixedexp; + if(fabs(theconf.gain - gain) > FLT_EPSILON) + gain = theconf.gain; + if(fabs(theconf.brightness - brightness) > FLT_EPSILON) + brightness = theconf.brightness; + } + //Icap[iCaptured] = NULL; + //pthread_mutex_unlock(&capt_mutex); FREE(oIma->data); FREE(oIma); DBG("---- cleared image data @ %g", sl_dtime() - t0); - }else pthread_mutex_unlock(&capt_mutex); + }else{ + DBG("NO image data"); + pthread_mutex_unlock(&capt_mutex); + } + DBG("NEXT!"); usleep(1000); } return NULL; @@ -274,7 +298,7 @@ int camcapture(void (*process)(Image*)){ FNAME(); static float oldexptime = 0.; static float oldgain = -1.; - static float oldbrightness = -1.; + static float oldbrightness = 0.; Image *oIma = NULL; pthread_t proc_thread; if(pthread_create(&proc_thread, NULL, procthread, (void*)process)){ @@ -282,6 +306,7 @@ int camcapture(void (*process)(Image*)){ ERR("pthread_create()"); } while(1){ + double t0 = sl_dtime(); if(stopwork){ DBG("STOP"); break; @@ -290,13 +315,15 @@ int camcapture(void (*process)(Image*)){ LOGERR("camcapture(): camera not initialized"); ERRX("Not initialized"); } + DBG("T=%g", sl_dtime() - t0); if(!connected){ - DBG("Disconnected"); + DBG("Disconnected, try to connect"); connected = theCam->connect(); sleep(1); changeformat(); continue; } + DBG("T=%g", sl_dtime() - t0); if(fabsf(oldbrightness - brightness) > FLT_EPSILON){ // new brightness DBG("Change brightness to %g", brightness); if(theCam->setbrightness(brightness)){ @@ -305,6 +332,7 @@ int camcapture(void (*process)(Image*)){ WARNX("Can't change brightness to %g", brightness); } } + DBG("T=%g", sl_dtime() - t0); if(exptime > theconf.maxexp) exptime = theconf.maxexp; else if(exptime < theconf.minexp) exptime = theconf.minexp; if(fabsf(oldexptime - exptime) > FLT_EPSILON){ // new exsposition value @@ -315,28 +343,41 @@ int camcapture(void (*process)(Image*)){ WARNX("Can't change exposition time to %gms", exptime); } } + DBG("T=%g", sl_dtime() - t0); if(gain > gainmax) gain = gainmax; if(fabsf(oldgain - gain) > FLT_EPSILON){ // change gain DBG("Change gain to %g\n", gain); + LOGDBG("Change gain to %g", gain); if(theCam->setgain(gain)){ oldgain = gain; }else{ WARNX("Can't change gain to %g", gain); + LOGWARN("Can't change gain to %g", gain); + gain = oldgain; } } + DBG("T=%g", sl_dtime() - t0); // change format if(abs(curformat.h - theconf.height) || abs(curformat.w - theconf.width) || abs(curformat.xoff - theconf.xoff) || abs(curformat.yoff - theconf.yoff)){ changeformat(); } + DBG("Try to grab (T=%g)", sl_dtime() - t0); + static int errctr = 0; if(!(oIma = theCam->capture())){ WARNX("Can't grab image"); - camdisconnect(); + if(++errctr > MAX_CAPT_ERRORS){ + LOGERR("camcapture(): too much capture errors; reconnect camera"); + camdisconnect(); + errctr = 0; + } continue; - } + }else errctr = 0; + DBG("Grabbed @ %g", sl_dtime() - t0); pthread_mutex_lock(&capt_mutex); if(iCaptured < 0) iCaptured = 0; else iCaptured = !iCaptured; if(Icap[iCaptured]){ // try current value if previous is still busy + DBG("iCap=%d busy!", iCaptured); iCaptured = !iCaptured; } if(!Icap[iCaptured]){ // previous buffer is free @@ -344,11 +385,12 @@ int camcapture(void (*process)(Image*)){ Icap[iCaptured] = oIma; oIma = NULL; }else{ // clear our image - there's no empty buffers - DBG("---- no free buffers"); + DBG("---- no free buffers for iCap=%d", iCaptured); FREE(oIma->data); FREE(oIma); } pthread_mutex_unlock(&capt_mutex); + DBG("unlocked, T=%g", sl_dtime() - t0); } pthread_cancel(proc_thread); if(oIma){ @@ -388,10 +430,13 @@ char *camstatus(const char *messageid, char *buf, int buflen){ float xc, yc; getcenter(&xc, &yc); snprintf(buf, buflen, "{ \"%s\": \"%s\", \"camstatus\": \"%sconnected\", \"impath\": \"%s\", \"imctr\": %llu, " - "\"fps\": %.3f, \"expmethod\": \"%s\", \"exposition\": %g, \"gain\": %g, \"brightness\": %g, " - "\"xcenter\": %.1f, \"ycenter\": %.1f }\n", - MESSAGEID, messageid, connected ? "" : "dis", impath, ImNumber, getFramesPerS(), - (theconf.expmethod == EXPAUTO) ? "auto" : "manual", exptime, gain, brightness, - xc, yc); + "\"fps\": %.3f, \"expmethod\": \"%s\", \"exposition\": %g, \"gain\": %g, \"maxgain\": %g, \"brightness\": %g, " + "\"xcenter\": %.1f, \"ycenter\": %.1f , \"minval\": %d, \"maxval\": %d, \"background\": %d, " + "\"average\": %.1f, \"xc\": %.1f, \"yc\": %.1f, \"xsigma\": %.1f, \"ysigma\": %.1f, \"area\": %d }\n", + MESSAGEID, messageid, connected ? "" : "dis", impath, ImNumber, getFramesPerS(), + (theconf.expmethod == EXPAUTO) ? "auto" : "manual", exptime, gain, gainmax, brightness, + xc, yc, lastimdata.minval, lastimdata.maxval, lastimdata.bkg, lastimdata.avg, + lastimdata.stat.xc, lastimdata.stat.yc, lastimdata.stat.xsigma, lastimdata.stat.ysigma, + lastimdata.stat.area); return buf; } diff --git a/LocCorr_new/cameracapture.h b/LocCorr_new/cameracapture.h index ae02b46..924c52e 100644 --- a/LocCorr_new/cameracapture.h +++ b/LocCorr_new/cameracapture.h @@ -21,6 +21,8 @@ #include "imagefile.h" // Image +// max capture errors contract to make reconnection +#define MAX_CAPT_ERRORS (10) // format of single frame typedef struct{ diff --git a/LocCorr_new/config.h b/LocCorr_new/config.h index d4062fa..2a028aa 100644 --- a/LocCorr_new/config.h +++ b/LocCorr_new/config.h @@ -34,12 +34,12 @@ #define MAX_THROWPART (0.9) #define MAX_OFFSET (10000) // min/max exposition in ms -#define EXPOS_MIN (0.1) +#define EXPOS_MIN (0.01) #define EXPOS_MAX (4001.) #define GAIN_MIN (0.) -#define GAIN_MAX (100.) +#define GAIN_MAX (256.) #define BRIGHT_MIN (0.) -#define BRIGHT_MAX (10.) +#define BRIGHT_MAX (100.) // max average images counter #define NAVER_MAX (25) // coefficients to convert dx,dy to du,dv @@ -52,15 +52,15 @@ #define MAX_MEDIAN_SEED (7) // fixed background #define FIXED_BK_MIN (0) -#define FIXED_BK_MAX (250) +#define FIXED_BK_MAX (255) // exposition methods: 0 - auto, 1 - fixed #define EXPAUTO (0) #define EXPMANUAL (1) // roundness parameter -#define MINWH (0.3) -#define MAXWH (3.) +#define MINWH (0.01) +#define MAXWH (100.) // PID limits #define PID_P_MIN (0.1) diff --git a/LocCorr_new/hikrobot.c b/LocCorr_new/hikrobot.c index 7ba8f06..10d4062 100644 --- a/LocCorr_new/hikrobot.c +++ b/LocCorr_new/hikrobot.c @@ -372,7 +372,7 @@ static int setexp(float e){ static int cam_startexp(){ if(!handle || !pdata) return FALSE; - DBG("Start exposition"); + DBG("Start exposition for %gs", exptime); MV_CC_StopGrabbing(handle); TRY(StartGrabbing); ONERR() return FALSE; @@ -380,22 +380,26 @@ static int cam_startexp(){ } static Image* capture(){ + double starttime = sl_dtime(); if(!cam_startexp()) return NULL; MV_FRAME_OUT_INFO_EX stImageInfo = {0}; // last image info - double starttime = sl_dtime(); + DBG("Started capt @ %g", sl_dtime() - starttime); do{ usleep(100); double diff = exptime - (sl_dtime() - starttime); if(diff > 0.) continue; // wait until exposure ends DBG("diff = %g", diff); - if(diff < -5.0){ // wait much longer than exp lasts + if(diff < -MAX_READOUT_TM){ // wait much longer than exp lasts + DBG("OOps, time limit"); MV_CC_StopGrabbing(handle); return NULL; } TRY(GetOneFrameTimeout, pdata, pdatasz, &stImageInfo, 10); ONOK() break; }while(1); + DBG("Tcapt=%g, exptime=%g", sl_dtime() - starttime, exptime); Image *captIma = u8toImage(pdata, stImageInfo.nWidth, stImageInfo.nHeight, stImageInfo.nWidth); + DBG("return @ %g", sl_dtime() - starttime); return captIma; } diff --git a/LocCorr_new/hikrobot.h b/LocCorr_new/hikrobot.h index 2c0035e..3c88f1e 100644 --- a/LocCorr_new/hikrobot.h +++ b/LocCorr_new/hikrobot.h @@ -23,6 +23,9 @@ #define HIKROBOT_CAPT_NAME "hikrobot" +// maximal readout time, seconds +#define MAX_READOUT_TM (6.) + // tolerance of float values #define HR_FLOAT_TOLERANCE (0.005) diff --git a/LocCorr_new/imagefile.c b/LocCorr_new/imagefile.c index 3b1d3b5..d513b73 100644 --- a/LocCorr_new/imagefile.c +++ b/LocCorr_new/imagefile.c @@ -257,6 +257,7 @@ int get_histogram(const Image *I, size_t histo[HISTOSZ]){ */ int calc_background(Image *img){ if(!img || !img->data) return FALSE; + DBG("image min/max=%d/%d", img->minval, img->maxval); if(img->maxval == img->minval){ WARNX("Zero or overilluminated image!"); return FALSE; @@ -266,7 +267,7 @@ int calc_background(Image *img){ WARNX("Image values too small"); return FALSE; } - img->background = theconf.fixedbkg; + img->background = theconf.fixedbkgval; return TRUE; } size_t histogram[HISTOSZ]; diff --git a/LocCorr_new/imagefile.h b/LocCorr_new/imagefile.h index d7c37fa..31d0c0f 100644 --- a/LocCorr_new/imagefile.h +++ b/LocCorr_new/imagefile.h @@ -32,6 +32,11 @@ typedef uint8_t Imtype; // WARNING! Check code if you change Imtype: e.g. recalcexp() and other #define HISTOSZ (256) +typedef struct{ // statistics: mean, RMS, area + float xc; float yc; float xsigma; float ysigma; + int area; +} ptstat_t; + typedef struct{ int width; // width int height; // height @@ -40,6 +45,7 @@ typedef struct{ Imtype maxval; float avg_intensity; Imtype background; // background value + ptstat_t stat; // image statistics } Image; // input file/directory type diff --git a/LocCorr_new/improc.c b/LocCorr_new/improc.c index f6a4f2c..67db228 100644 --- a/LocCorr_new/improc.c +++ b/LocCorr_new/improc.c @@ -31,6 +31,7 @@ #include "draw.h" #include "grasshopper.h" #include "hikrobot.h" +#include "imagefile.h" #include "improc.h" #include "inotify.h" #include "steppers.h" @@ -132,7 +133,7 @@ static void getDeviation(object *curobj){ averflag = 1; if(fXYlog) fprintf(fXYlog, "%.1f\t%.1f\t%.1f\t%.1f", xx, yy, Sx, Sy); process_corrections: - LOGDBG("here"); + //LOGDBG("here"); if(theSteppers){ DBG("Process corrections"); if(theSteppers->proc_corr && averflag){ @@ -145,14 +146,10 @@ process_corrections: LOGERR("Lost connection with stepper server"); WARNX("Lost connection with stepper server"); } - LOGDBG("And there"); + //LOGDBG("And there"); XYnewline(); } -typedef struct{ // statistics: mean and RMS - float xc; float yc; float xsigma; float ysigma; -} ptstat_t; - /** * @brief sumAndStat - calculate statistics in region of interest * @param I - image (with background calculated) @@ -241,6 +238,7 @@ void process_file(Image *I){ DBG("Get sum and stat for simplest centroid"); double sum = sumAndStat(I, NULL, 0, &roi, &stat); if(sum > 0.){ + I->stat = stat; if( fabsf(stat.xc - prev_x) > XY_TOLERANCE || fabsf(stat.yc - prev_y) > XY_TOLERANCE){ DBG("Bad: was x=%d, y=%d; become x=%g, y=%g ==> need fine calculations", prev_x, prev_y, xc, yc); @@ -248,6 +246,7 @@ void process_file(Image *I){ double WdH = stat.xsigma/stat.ysigma; // wery approximate area inside sigmax*sigmay double area = .4 * stat.xsigma * stat.ysigma; + I->stat.area = (int)area; if(!isnan(WdH) && !isinf(WdH) && // if W/H is a number WdH > theconf.minwh && WdH < theconf.maxwh && // if W/H near circle area > theconf.minarea && area < theconf.maxarea){ // if star area is in range @@ -274,31 +273,38 @@ void process_file(Image *I){ uint8_t *ibin = Im2bin(I, I->background); DELTA("Made binary"); if(ibin){ - //savebin(ibin, W, H, "binary.fits"); - //DELTA("save binary.fits"); + Image *Itmp = bin2Im(ibin, I->width, I->height); + Image_write_jpg(Itmp, "binary.jpg", 1); + Image_free(&Itmp); + DELTA("save binary"); uint8_t *er = il_erosionN(ibin, W, H, theconf.Nerosions); FREE(ibin); DELTA("Erosion"); - //savebin(er, W, H, "erosion.fits"); - //DELTA("Save erosion"); + Itmp = bin2Im(er, I->width, I->height); + Image_write_jpg(Itmp, "erosion.jpg", 1); + Image_free(&Itmp); + DELTA("Save erosion"); uint8_t *opn = il_dilationN(er, W, H, theconf.Ndilations); FREE(er); DELTA("Opening"); - //savebin(opn, W, H, "opening.fits"); - //DELTA("Save opening"); + Itmp = bin2Im(opn, I->width, I->height); + Image_write_jpg(Itmp, "opening.jpg", 1); + Image_free(&Itmp); + DELTA("Save opening"); S = il_cclabel4(opn, W, H, &cc); FREE(opn); - DBG("Nobj=%zd", cc->Nobj-1); + if(S && cc) DBG("Nobj=%zd", cc->Nobj-1); if(S && cc && cc->Nobj > 1){ // Nobj = amount of objects + 1 DBGLOG("Nobj=%zd", cc->Nobj-1); if(Nallocated < cc->Nobj-1){ Nallocated = cc->Nobj-1; Objects = realloc(Objects, Nallocated*sizeof(object)); } + I->stat.area = cc->boxes[1].area; for(size_t i = 1; i < cc->Nobj; ++i){ il_Box *b = &cc->boxes[i]; double wh = ((double)b->xmax - b->xmin)/(b->ymax - b->ymin); - //DBG("Obj# %zd: wh=%g, area=%d", i, wh, b->area); + DBG("Obj# %zd: wh=%g, area=%d", i, wh, b->area); if(wh < theconf.minwh || wh > theconf.maxwh) continue; if((int)b->area < theconf.minarea || (int)b->area > theconf.maxarea) continue; ptstat_t stat; diff --git a/LocCorr_new/steppers.c b/LocCorr_new/steppers.c index dbdb6eb..a06189a 100644 --- a/LocCorr_new/steppers.c +++ b/LocCorr_new/steppers.c @@ -86,8 +86,8 @@ steppersproc *theSteppers = NULL; // stepper numbers typedef enum{ Ustepper = 0, - Vstepper = 2, - Fstepper = 1, + Vstepper = 1, + Fstepper = 2, } stepperno; const char *motornames[NMOTORS] = { @@ -200,6 +200,7 @@ static void stp_disc(){ static void stp_disconnect(){ DBG("Try to disconnect"); + LOGDBG("Try to disconnect"); if(serialsock){ DBG("Close socket"); sl_sock_delete(&serialsock); @@ -292,9 +293,8 @@ static void parse_msg(char *msg){ for(int idx = 0; idx < CMD_AMOUNT; ++idx){ // search index in commands if(0 == strcmp(stp_commands[idx], key)){ // found our - free(key); //LOGDBG("OK, idx=%d, cmd=%s", idx, stp_commands[idx]); - parser(idx, parno, ival); + LastErr = parser(idx, parno, ival); break; } } @@ -323,14 +323,19 @@ static errcodes send_message(steppercmd idx, char *msg){ if(!msg) msglen = snprintf(buf, 255, "%s\n", stp_commands[idx]); else msglen = snprintf(buf, 255, "%s%s\n", stp_commands[idx], msg); //DBG("Send message '%s', len %zd", buf, msglen); + LastErr = ERR_AMOUNT; if(sl_sock_sendstrmessage(serialsock, buf) != (ssize_t)msglen){ WARN("send()"); LOGWARN("send_message(): send() failed"); return ERR_WRONGLEN; } - LOGDBG("Message '%s' sent", buf); - ;;; // wait for errcode - return ERR_OK; + //LOGDBG("send_message(): message '%s' sent", buf); + double t0 = sl_dtime(); + while(sl_dtime() - t0 < STEPPERS_NOANS_TIMEOUT){ + if(LastErr != ERR_AMOUNT) return LastErr; + } + LOGWARN("send_message(): got NO answer for %s%s", stp_commands[idx], msg); + return ERR_CANTRUN; } // send command cmd to n'th motor with param p, @return FALSE if failed @@ -392,20 +397,24 @@ void *clientproc(void _U_ *par){ FNAME(); char rbuf[BUFSIZ]; if(!serialsock) return NULL; + double t0 = sl_dtime(); do{ ssize_t got = sl_sock_readline(serialsock, rbuf, BUFSIZ); //LOGDBG("got=%zd", got); - if(got < 0){ // disconnected + if(got < 0 || sl_dtime() - t0 > STEPPERS_NOANS_TIMEOUT){ // disconnected WARNX("Serial server disconnected"); + LOGERR("Serial server disconnected (timeout reached)"); if(serialsock) sl_sock_delete(&serialsock); + state = STP_DISCONN; return NULL; }else if(got == 0){ // nothing to read from serial port usleep(1000); continue; } + //LOGDBG("clientproc(): got '%s'", rbuf); // process data - DBG("GOT: %s", rbuf); parse_msg(rbuf); + t0 = sl_dtime(); } while(serialsock && serialsock->connected); WARNX("disconnected"); if(serialsock) sl_sock_delete(&serialsock); @@ -458,9 +467,9 @@ static void process_movetomiddle_stage(){ case SETUP_WAITUV0: // wait for all coordinates moving to zero if(!relaxed(Ustepper) || !relaxed(Vstepper) || !relaxed(Fstepper)) break; // didn't reach yet // now all motors are stopped -> send positions to zero - if( !nth_motor_setter(CMD_ABSPOS, Ustepper, 1) || - !nth_motor_setter(CMD_ABSPOS, Vstepper, 1) || - !nth_motor_setter(CMD_ABSPOS, Fstepper, 1)) break; + if( !nth_motor_setter(CMD_ABSPOS, Ustepper, 0) || + !nth_motor_setter(CMD_ABSPOS, Vstepper, 0) || + !nth_motor_setter(CMD_ABSPOS, Fstepper, 0)) break; DBG("Reached UVF0!"); // goto if(nth_motor_setter(CMD_GOTO, Ustepper, (theconf.maxUpos + theconf.minUpos)/2) && @@ -508,8 +517,8 @@ static void process_setup_stage(){ case SETUP_WAITUV0: // wait for both coordinates moving to zero if(!relaxed(Ustepper) || !relaxed(Vstepper)) break; // set current position to 0 - if( !nth_motor_setter(CMD_ABSPOS, Ustepper, 1) || - !nth_motor_setter(CMD_ABSPOS, Vstepper, 1)) break; + if( !nth_motor_setter(CMD_ABSPOS, Ustepper, 0) || + !nth_motor_setter(CMD_ABSPOS, Vstepper, 0)) break; DBG("ZERO border reached"); // goto middle if(nth_motor_setter(CMD_GOTO, Ustepper, (theconf.maxUpos+theconf.minUpos)/2) && @@ -901,30 +910,38 @@ static void *stp_process_states(_U_ void *arg){ stp_connect_server(); continue; } - // check request to change focus - if(chfocus){ - DBG("Try to move F to %d", newfocpos); - if(nth_motor_setter(CMD_GOTO, Fstepper, newfocpos)) chfocus = FALSE; - } - if(dUmove){ - DBG("Try to move U by %d", dUmove); - if(nth_motor_setter(CMD_RELPOS, Ustepper, dUmove)) dUmove = 0; - } - if(dVmove){ - DBG("Try to move V by %d", dVmove); - if(nth_motor_setter(CMD_RELPOS, Vstepper, dVmove)) dVmove = 0; - } static double t0 = -1.; if(t0 < 0.) t0 = sl_dtime(); if(state != STP_DISCONN){ if(sl_dtime() - t0 >= 0.1){ // each 0.1s check state if steppers aren't disconnected t0 = sl_dtime(); chkall(); - } - if(!relaxed(Ustepper) && !relaxed(Vstepper)) continue; - first = TRUE; + if(!relaxed(Ustepper) && !relaxed(Vstepper)) continue; + first = TRUE; + }else continue; } // if we are here, all U/V moving is finished + // check request to change focus + if(chfocus){ + DBG("Try to move F to %d", newfocpos); + if(nth_motor_setter(CMD_GOTO, Fstepper, newfocpos)){ + chfocus = FALSE; + } + } + if(dUmove){ + DBG("Try to move U by %d", dUmove); + if(nth_motor_setter(CMD_RELPOS, Ustepper, dUmove)){ + dUmove = 0; + continue; + } + } + if(dVmove){ + DBG("Try to move V by %d", dVmove); + if(nth_motor_setter(CMD_RELPOS, Vstepper, dVmove)){ + dVmove = 0; + continue; + } + } switch(state){ // steppers state machine case STP_DISCONN: if(!stp_connect_server()){ @@ -933,7 +950,7 @@ static void *stp_process_states(_U_ void *arg){ LOGWARN("Can't reconnect"); first = FALSE; } - sleep(5); + sleep(1); } break; case STP_SETUP: // setup axes (before this state set Xtarget/Ytarget in improc.c) diff --git a/LocCorr_new/steppers.h b/LocCorr_new/steppers.h index 1550694..b1d0335 100644 --- a/LocCorr_new/steppers.h +++ b/LocCorr_new/steppers.h @@ -18,6 +18,9 @@ #pragma once +// timeout for "no answer from motors", seconds +#define STEPPERS_NOANS_TIMEOUT (5.) + // set state to `disconnect` after this amount of errors in `moving_finished` #define MAX_ERR_CTR (15)