forgotten fixes

LocCorr_new/CMakeLists.txt

@@ -21,7 +21,7 @@ option(GRASSHOPPER "Add GrassHopper cameras support" OFF)
 option(HIKROBOT "Add HikRobot cameras support" OFF)
 
 # default flags
-set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -W -Wextra")
+set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -W -Wextra -std=c17")
 set(CMAKE_COLOR_MAKEFILE ON)
 
 # cmake -DDEBUG=yes -> debugging
@@ -94,7 +94,7 @@ target_include_directories(${PROJ} PUBLIC ${MODULES_INCLUDE_DIRS} ${FLYCAP_INCLU
 target_link_directories(${PROJ} PUBLIC ${MODULES_LIBRARY_DIRS} ${FLYCAP_LIBRARY_DIRS} ${BASLER_LIBRARY_DIRS} ${MVS_LIBRARY_DIRS})
 message("MOD: ${MODULES_LIBRARY_DIRS}, FC: ${FLYCAP_LIBRARY_DIRS}, MVS: ${MVS_LIBRARY_DIRS}")
 # -D
-add_definitions(${CFLAGS} -DLOCALEDIR=\"${LOCALEDIR}\"
+add_definitions(${CFLAGS} -D_XOPEN_SOURCE=666 -D_POSIX_C_SOURCE=600700 -D_DEFAULT_SOURCE -DLOCALEDIR=\"${LOCALEDIR}\"
     -DPACKAGE_VERSION=\"${VERSION}\" -DGETTEXT_PACKAGE=\"${PROJ}\"
     -DMINOR_VERSION=\"${MINOR_VERSION}\" -DMID_VERSION=\"${MID_VERSION}\"
     -DMAJOR_VERSION=\"${MAJOR_VERSION}\" -DTHREAD_NUMBER=${PROCESSOR_COUNT})

LocCorr_new/binmorph.c

@@ -436,30 +436,30 @@ size_t *il_cclabel4(uint8_t *Img, int W, int H, il_ConnComps **CC){
             l_boxes[i].ymin = H;
         }
         #pragma omp for nowait
-    for(int y = 0; y < H; ++y){
+            for(int y = 0; y < H; ++y){
-        size_t *lptr = &labels[y*W];
+                size_t *lptr = &labels[y*W];
-        for(int x = 0; x < W; ++x, ++lptr){
+                for(int x = 0; x < W; ++x, ++lptr){
-            if(!*lptr) continue;
+                    if(!*lptr) continue;
-            register size_t mark = indexes[*lptr];
+                    register size_t mark = indexes[*lptr];
-            *lptr = mark;
+                    *lptr = mark;
-            il_Box *b = &l_boxes[mark];
+                    il_Box *b = &l_boxes[mark];
-            ++b->area;
+                    ++b->area;
-            if(b->xmax < x) b->xmax = x;
+                    if(b->xmax < x) b->xmax = x;
-            if(b->xmin > x) b->xmin = x;
+                    if(b->xmin > x) b->xmin = x;
-            if(b->ymax < y) b->ymax = y;
+                    if(b->ymax < y) b->ymax = y;
-            if(b->ymin > y) b->ymin = y;
+                    if(b->ymin > y) b->ymin = y;
-        }
+                }
-    }
+            }
-    #pragma omp critical
+        #pragma omp critical
-    for(size_t i = 1; i < cidx; ++i){
+            for(size_t i = 1; i < cidx; ++i){
-        il_Box *ob = &boxes[i], *ib = &l_boxes[i];
+                il_Box *ob = &boxes[i], *ib = &l_boxes[i];
-        if(ob->xmax < ib->xmax) ob->xmax = ib->xmax;
+                if(ob->xmax < ib->xmax) ob->xmax = ib->xmax;
-        if(ob->xmin > ib->xmin) ob->xmin = ib->xmin;
+                if(ob->xmin > ib->xmin) ob->xmin = ib->xmin;
-        if(ob->ymax < ib->ymax) ob->ymax = ib->ymax;
+                if(ob->ymax < ib->ymax) ob->ymax = ib->ymax;
-        if(ob->ymin > ib->ymin) ob->ymin = ib->ymin;
+                if(ob->ymin > ib->ymin) ob->ymin = ib->ymin;
-        ob->area += ib->area;
+                ob->area += ib->area;
-    }
+            }
-    FREE(l_boxes);
+        FREE(l_boxes);
     }
     FREE(assoc);
     FREE(indexes);

LocCorr_new/cameracapture.c

@@ -21,6 +21,7 @@
 #include <pthread.h>
 #include <stdio.h>
 #include <string.h>
+#include <unistd.h>
 
 #include "cameracapture.h"
 #include "cmdlnopts.h"
@@ -34,7 +35,7 @@
 static camera *theCam = NULL;
 
 static float gain = 0., gainmax = 0.;
-static float exptime = 100.;
+static float exptime = -1.;
 static float brightness = 0.;
 static int connected = FALSE;
 
@@ -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,40 +241,48 @@ static void *procthread(void* v){
     double t0 = sl_dtime();
 #endif
     while(!stopwork){
-        while(iCaptured < 0) usleep(1000);
         pthread_mutex_lock(&capt_mutex);
+        //DBG("===== iCaptured=%d", iCaptured);
         if(Icap[iCaptured]){
-            DBG("---- got image #%d @ %g", iCaptured, sl_dtime() - t0);
+            DBG("===== got image iCaptured=#%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(process){
+                if(theconf.medfilt){
+                    Image *X = get_median(oIma, theconf.medseed);
+                    if(X){
+                        Image_free(&oIma);
+                        oIma = X;
+                    }
+                }
+                process(oIma);
+                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){
-                float xc, yc;
+                if(needs_exposure_adjustment(oIma, lastimdata.xc, lastimdata.yc)) recalcexp(oIma);
-                getcenter(&xc, &yc);
-                if(needs_exposure_adjustment(oIma, xc, yc)) recalcexp(oIma);
             }else{
-                if(fabs(theconf.fixedexp - exptime) > FLT_EPSILON)
+                if(fabs(theconf.exptime - exptime) > FLT_EPSILON)
-                    exptime = theconf.fixedexp;
+                    exptime = theconf.exptime;
                 if(fabs(theconf.gain - gain) > FLT_EPSILON)
                     gain = theconf.gain;
                 if(fabs(theconf.brightness - brightness) > FLT_EPSILON)
                     brightness = theconf.brightness;
             }
-            if(process){
+            //Icap[iCaptured] = NULL;
-                if(theconf.medfilt){
+            //pthread_mutex_unlock(&capt_mutex);
-                    Image *X = get_median(oIma, theconf.medseed);
+            Image_free(&oIma);
-                    if(X){
+            DBG("===== cleared image data @ %g", sl_dtime() - t0);
-                        FREE(oIma->data);
+        }else{
-                        FREE(oIma);
+            //DBG("===== NO image data");
-                        oIma = X;
+            pthread_mutex_unlock(&capt_mutex);
-                    }
+        }
-                }
+        //DBG("===== NEXT!");
-                process(oIma);
-            }
-            FREE(oIma->data);
-            FREE(oIma);
-            DBG("---- cleared image data @ %g", sl_dtime() - t0);
-        }else pthread_mutex_unlock(&capt_mutex);
         usleep(1000);
     }
     return NULL;
@@ -274,14 +292,16 @@ 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)){
         LOGERR("pthread_create() for image processing failed");
         ERR("pthread_create()");
     }
+    exptime = theconf.exptime;
     while(1){
+        double t0 = sl_dtime();
         if(stopwork){
             DBG("STOP");
             break;
@@ -290,13 +310,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,38 +327,54 @@ 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
             DBG("Change exptime to %.2fms\n", exptime);
             if(theCam->setexp(exptime)){
                 oldexptime = exptime;
+                theconf.exptime = exptime;
             }else{
                 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;
+                theconf.gain = 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");
+            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,22 +382,16 @@ 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);
+            Image_free(&oIma);
-            FREE(oIma);
         }
         pthread_mutex_unlock(&capt_mutex);
+        DBG("unlocked, T=%g", sl_dtime() - t0);
     }
     pthread_cancel(proc_thread);
-    if(oIma){
+    if(oIma) Image_free(&oIma);
-        FREE(oIma->data);
-        FREE(oIma);
-    }
     for(int i = 0; i < 2; ++i){
-        if(Icap[i]){
+        if(Icap[i]) Image_free(&Icap[i]);
-            FREE(Icap[i]->data);
-            FREE(Icap[i]);
-        }
     }
     camdisconnect();
     DBG("CAMCAPTURE: out");
@@ -388,10 +420,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, "
+         "\"fps\": %.3f, \"expmethod\": \"%s\", \"exptime\": %g, \"gain\": %g, \"maxgain\": %g, \"brightness\": %g, "
-             "\"xcenter\": %.1f, \"ycenter\": %.1f }\n",
+         "\"xcenter\": %.1f, \"ycenter\": %.1f , \"minval\": %d, \"maxval\": %d, \"background\": %d, "
-             MESSAGEID, messageid, connected ? "" : "dis", impath, ImNumber, getFramesPerS(),
+         "\"average\": %.1f, \"xc\": %.1f, \"yc\": %.1f, \"xsigma\": %.1f, \"ysigma\": %.1f, \"area\": %d }\n",
-             (theconf.expmethod == EXPAUTO) ? "auto" : "manual", exptime, gain, brightness,
+         MESSAGEID, messageid, connected ? "" : "dis", impath, ImNumber, getFramesPerS(),
-             xc, yc);
+         (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;
 }

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{

LocCorr_new/config.c

@@ -62,7 +62,7 @@ configuration theconf = {
     .throwpart=DEFAULT_THROWPART,
     .maxexp=EXPOS_MAX - 1.,
     .minexp=EXPOS_MIN,
-    .fixedexp=EXPOS_MIN*2,
+    .exptime=EXPOS_MIN*2,
     .gain=20.,
     .intensthres=DEFAULT_INTENSTHRES,
     .medseed=MIN_MEDIAN_SEED,
@@ -74,6 +74,7 @@ static int compConfVals(const void *_1st, const void *_2nd){
     return strcmp(a->name, b->name);
 }
 
+// could be in unsorted order as whould be sorted at first help call
 // {"", PAR_DOUBLE, (void*)&theconf., 0},
 static confparam parvals[] = {
     {"maxarea", PAR_INT, (void*)&theconf.maxarea, 0, MINAREA, MAXAREA,
@@ -99,7 +100,7 @@ static confparam parvals[] = {
     {"equalize", PAR_INT, (void*)&theconf.equalize, 0, 0., 1.,
      "make histogram equalization"},
     {"expmethod", PAR_INT, (void*)&theconf.expmethod, 0, 0., 1.,
-     "exposition method: 0 - auto, 1 - fixed"},
+     "0 - automatic calculation of gain and exptime, 1 - use fixed values"},
     {"naverage", PAR_INT, (void*)&theconf.naverage, 0, 1., NAVER_MAX,
      "calculate mean position by N images"},
     {"umax", PAR_INT, (void*)&theconf.maxUpos, 0, -MAXSTEPS, MAXSTEPS,
@@ -146,8 +147,8 @@ static confparam parvals[] = {
      "minimal exposition time"},
     {"maxexp", PAR_DOUBLE, (void*)&theconf.maxexp, 0, 0., EXPOS_MAX,
      "maximal exposition time"},
-    {"fixedexp", PAR_DOUBLE, (void*)&theconf.fixedexp, 0, EXPOS_MIN, EXPOS_MAX,
+    {"exptime", PAR_DOUBLE, (void*)&theconf.exptime, 0, EXPOS_MIN, EXPOS_MAX,
-     "fixed (in manual mode) exposition time"},
+     "exposition time (you can change it only when expmethod==1)"},
     {"intensthres", PAR_DOUBLE, (void*)&theconf.intensthres, 0, 0., 1.,
      "threshold by total object intensity when sorting = |I1-I2|/(I1+I2)"},
     {"gain", PAR_DOUBLE, (void*)&theconf.gain, 0, GAIN_MIN, GAIN_MAX,
@@ -162,8 +163,10 @@ static confparam parvals[] = {
      "median filter radius"},
     {"fixedbg", PAR_INT, (void*)&theconf.fixedbkg, 0, 0., 1.,
      "don't calculate background, use fixed value instead"},
-    {"fbglevel", PAR_INT, (void*)&theconf.fixedbkgval, 0, FIXED_BK_MIN, FIXED_BK_MAX,
+    {"background", PAR_INT, (void*)&theconf.background, 0, FIXED_BK_MIN, FIXED_BK_MAX,
      "fixed background level"},
+    {"writedi", PAR_INT, (void*)&theconf.writedebugimgs, 0, 0., 1.,
+     "write debug images (binary/erosion/opening)"},
     {NULL,  0,  NULL, 0, 0., 0., NULL}
 };
 

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       (1e-9)
 #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 MINWH           (0.01)
-#define MAXWH                       (3.)
+#define MAXWH           (100.)
 
 // PID limits
 #define PID_P_MIN       (0.1)
@@ -99,7 +99,8 @@ typedef struct{
     int medfilt;        // == 1 to make median filter before calculations
     int medseed;        // median seed
     int fixedbkg;       // don't calculate background, use fixed value instead
-    int fixedbkgval;    // value of bk
+    int background;     // value of background
+    int writedebugimgs; // write debugging images: binary/erosion/opening
     // dU = Kxu*dX + Kyu*dY; dV = Kxv*dX + Kyv*dY
     double Kxu; double Kyu;
     double Kxv; double Kyv;
@@ -109,7 +110,7 @@ typedef struct{
     double throwpart;   // part of values to throw avay @ histogram equalisation
     double maxexp;      // minimal and maximal exposition (in ms)
     double minexp;
-    double fixedexp;    // exptime in manual mode
+    double exptime;     // exposure time
     double gain;        // gain value in manual mode
     double brightness;  // brightness @camera
     double intensthres; // threshold for stars intensity comparison: fabs(Ia-Ib)/(Ia+Ib) > thres -> stars differs

LocCorr_new/fits.c

@@ -47,11 +47,6 @@ do{ fitsstatus = 0;								\
     if(status) fits_report_error(stderr, status);\
 }while(0)
 
-void Image_free(Image **img){
-    FREE((*img)->data);
-    FREE(*img);
-}
-
 // I->data should be allocated!!!
 static inline void convflt2ima(float *f, Image *I){
     if(!I || !I->data || !f) return;

LocCorr_new/fits.h

@@ -26,7 +26,6 @@
 
 #include "imagefile.h"
 
-void Image_free(Image **ima);
 bool FITS_read(const char *filename, Image **fits);
 
 #endif // FITS_H__

LocCorr_new/hikrobot.c

@@ -279,7 +279,7 @@ static int cam_connect(){
         WARNX("Can't turn off triggered mode");
         return FALSE;
     }
-    if(!changeenum("AcquisitionMode", MV_ACQ_MODE_SINGLE)){
+    if(!changeenum("AcquisitionMode", /*MV_ACQ_MODE_SINGLE*/ MV_ACQ_MODE_CONTINUOUS)){
         WARNX("Can't set acquisition mode to single");
         return FALSE;
     }
@@ -356,46 +356,69 @@ static int changeformat(frameformat *f){
     return TRUE;
 }
 
-// exptime - in milliseconds!
 static int setexp(float e){
-    if(!handle) return FALSE;
+    if(!handle){
-    float eS = e / 1e3;
+        WARNX("NO HANDLE");
-    if(eS > extrvalues.maxexp || eS < extrvalues.minexp){
+        return FALSE;
-        WARNX("Wrong exposure time: %fs (should be [%fs..%fs])", eS,
+    }
+    if(e > extrvalues.maxexp || e < extrvalues.minexp){
+        WARNX("Wrong exposure time: %fs (should be [%fs..%fs])", e,
             extrvalues.minexp, extrvalues.maxexp);
         return FALSE;
     }
-    if(!changefloat("ExposureTime", e * 1e3)) return FALSE;
+    if(!changefloat("ExposureTime", e * 1e6)){
-    exptime = eS;
+        WARNX("Can't set exptime %g", e);
+        return FALSE;
+    }
+    exptime = e;
     return TRUE;
 }
 
 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;
+    ONERR(){
+        DBG("+++++ Ooops! Can't start grabbing: try another time!");
+        MV_CC_StopGrabbing(handle);
+        TRY(StartGrabbing);
+        ONERR(){
+            DBG("+++++ ERR!");
+            return FALSE;
+        }
+    }
+    DBG("+++++ OK, started");
     return TRUE;
 }
 
 static Image* capture(){
-    if(!cam_startexp()) return NULL;
-    MV_FRAME_OUT_INFO_EX stImageInfo = {0}; // last image info
     double starttime = sl_dtime();
+    static int isstarted = 0;
+    if(!isstarted){
+        if(!cam_startexp()) return NULL;
+        isstarted = 1;
+    }
+    MV_FRAME_OUT_INFO_EX stImageInfo = {0}; // last image info
+    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);
+            DBG("Restart grabbing");
+            if(!cam_startexp()) isstarted = 0;
             return NULL;
         }
-        TRY(GetOneFrameTimeout, pdata, pdatasz, &stImageInfo, 10);
+        TRY(GetOneFrameTimeout, pdata, pdatasz, &stImageInfo, 100);
         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;
 }
 

LocCorr_new/hikrobot.h

@@ -23,8 +23,11 @@
 
 #define HIKROBOT_CAPT_NAME        "hikrobot"
 
+// maximal readout time, seconds
+#define MAX_READOUT_TM      (0.3)
+
 // tolerance of float values
-#define HR_FLOAT_TOLERANCE  (0.005)
+#define HR_FLOAT_TOLERANCE  (1.1)
 
 extern camera Hikrobot;
 #endif

LocCorr_new/imagefile.c

@@ -204,14 +204,24 @@ Image *Image_read(const char *name){
  * @return data allocated here
  */
 Image *Image_new(int w, int h){
+    static uint64_t cnt = 0;
     if(w < 1 || h < 1) return NULL;
+    DBGLOG("Image_new(%d, #%u)", w*h, cnt);
     Image *outp = MALLOC(Image, 1);
     outp->width = w;
     outp->height = h;
+    outp->counter = cnt++;
     outp->data = MALLOC(Imtype, w*h);
     return outp;
 }
 
+void Image_free(Image **I){
+    if(!I || !*I) return;
+    DBGLOG("Image_free(%d, #%d)", (*I)->height * (*I)->width, (*I)->counter);
+    FREE((*I)->data);
+    FREE(*I);
+}
+
 /**
  * @brief Image_sim - allocate memory for new empty Image with similar size & data type
  * @param i - sample image
@@ -257,6 +267,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 +277,7 @@ int calc_background(Image *img){
             WARNX("Image values too small");
             return FALSE;
         }
-        img->background = theconf.fixedbkg;
+        img->background = theconf.background;
         return TRUE;
     }
     size_t histogram[HISTOSZ];

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,8 @@ typedef struct{
     Imtype maxval;
     float avg_intensity;
     Imtype background;  // background value
+    ptstat_t stat;      // image statistics
+    uint64_t counter;   // image counter
 } Image;
 
 // input file/directory type

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)
@@ -206,8 +203,6 @@ void process_file(Image *I){
     static int prev_x = -1, prev_y = -1;
     static object *Objects = NULL;
     static size_t Nallocated = 0;
-    il_ConnComps *cc = NULL;
-    size_t *S = NULL;
 #ifdef EBUG
     double t0 = sl_dtime(), tlast = t0;
 #define DELTA(p) do{double t = sl_dtime(); DBG("---> %s @ %gms (delta: %gms)", p, (t-t0)*1e3, (t-tlast)*1e3); tlast = t;}while(0)
@@ -228,6 +223,7 @@ void process_file(Image *I){
     //DELTA("Save original");
     if(calc_background(I)){
         DBG("backgr = %d", I->background);
+        theconf.background = I->background;
         DELTA("Got background");
         int objctr = 0;
         if(prev_x > 0 && prev_y > 0){
@@ -241,6 +237,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 +245,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 +272,45 @@ void process_file(Image *I){
         uint8_t *ibin = Im2bin(I, I->background);
         DELTA("Made binary");
         if(ibin){
-            //savebin(ibin, W, H, "binary.fits");
+            if(theconf.writedebugimgs){
-            //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");
+            if(theconf.writedebugimgs){
-            //DELTA("Save erosion");
+                Image *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");
+            if(theconf.writedebugimgs){
-            //DELTA("Save opening");
+                Image *Itmp = bin2Im(opn, I->width, I->height);
-            S = il_cclabel4(opn, W, H, &cc);
+                Image_write_jpg(Itmp, "opening.jpg", 1);
+                Image_free(&Itmp);
+                DELTA("Save opening");
+            }
+            il_ConnComps *cc = NULL;
+            size_t *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;
@@ -323,68 +335,68 @@ void process_file(Image *I){
                     else
                         qsort(Objects, objctr, sizeof(object), compDist);
                 }
-SKIP_FULL_PROCESS:
-                DBGLOG("T%.2f, N=%d\n", sl_dtime(), objctr);
-                DELTA("Calculate deviations");
-                if(objctr){
-#ifdef EBUG
-                    object *o = Objects;
-                    green("%6s\t%6s\t%6s\t%6s\t%6s\t%6s\t%6s\t%6s\t%8s\n",
-                          "N", "Area", "Mv", "W/H", "Xc", "Yc", "Sx", "Sy", "Area/r^2");
-                    for(int i = 0; i < objctr; ++i, ++o){
-                        // 1.0857 = 2.5/ln(10)
-                        printf("%6d\t%6d\t%6.1f\t%6.1f\t%6.1f\t%6.1f\t%6.1f\t%6.1f\t%8.1f\n",
-                               i, o->area, 20.-1.0857*log(o->Isum), o->WdivH, o->xc, o->yc,
-                               o->xsigma, o->ysigma, o->area/o->xsigma/o->ysigma);
-                    }
-#endif
-                    getDeviation(Objects); // calculate dX/dY and process corrections
-                }
-                DELTA("prepare image");
-                { // prepare image and save jpeg
-                    uint8_t *outp = NULL;
-                    if(theconf.equalize)
-                        outp = equalize(I, 3, theconf.throwpart);
-                    else
-                        outp = linear(I, 3);
-                    static il_Pattern *cross = NULL, *crossL = NULL;
-                    if(!cross) cross = il_Pattern_xcross(33, 33);
-                    if(!crossL) crossL = il_Pattern_xcross(51, 51);
-                    il_Img3 i3 = {.data = outp, .w = I->width, .h = H};
-                    DELTA("Draw crosses");
-                    // draw fiber center position
-                    il_Pattern_draw3(&i3, crossL, theconf.xtarget-theconf.xoff, H-(theconf.ytarget-theconf.yoff), C_R);
-                    if(objctr){ // draw crosses @ objects' centers
-                        int H = I->height;
-                        // draw current star centroid
-                        il_Pattern_draw3(&i3, cross, Objects[0].xc, H-Objects[0].yc, C_G);
-                        // add offset to show in target system
-                        xc = Objects[0].xc + theconf.xoff;
-                        yc = Objects[0].yc + theconf.yoff;
-                        // draw other centroids
-                        for(int i = 1; i < objctr; ++i)
-                            il_Pattern_draw3(&i3, cross, Objects[i].xc, H-Objects[i].yc, C_B);
-                    }else{xc = -1.; yc = -1.;}
-                    char tmpnm[FILENAME_MAX+5];
-                    sprintf(tmpnm, "%s-tmp", GP->outputjpg);
-                    if(stbi_write_jpg(tmpnm, I->width, I->height, 3, outp, 95)){
-                        if(rename(tmpnm, GP->outputjpg)){
-                            WARN("rename()");
-                            LOGWARN("can't save %s", GP->outputjpg);
-                        }
-                    }
-                    DELTA("Written");
-                    FREE(outp);
-                }
-            }else{
-                xc = -1.; yc = -1.;
-                Image_write_jpg(I, GP->outputjpg, theconf.equalize);
             }
-            DBGLOG("Image saved");
             FREE(S);
             FREE(cc);
         }
-    }else Image_write_jpg(I, GP->outputjpg, theconf.equalize);
+SKIP_FULL_PROCESS:
+        DBGLOG("T%.2f, N=%d\n", sl_dtime(), objctr);
+        DELTA("Calculate deviations");
+        if(objctr){
+#ifdef EBUG
+            object *o = Objects;
+            green("%6s\t%6s\t%6s\t%6s\t%6s\t%6s\t%6s\t%6s\t%8s\n",
+                  "N", "Area", "Mv", "W/H", "Xc", "Yc", "Sx", "Sy", "Area/r^2");
+            for(int i = 0; i < objctr; ++i, ++o){
+                // 1.0857 = 2.5/ln(10)
+                printf("%6d\t%6d\t%6.1f\t%6.1f\t%6.1f\t%6.1f\t%6.1f\t%6.1f\t%8.1f\n",
+                       i, o->area, 20.-1.0857*log(o->Isum), o->WdivH, o->xc, o->yc,
+                       o->xsigma, o->ysigma, o->area/o->xsigma/o->ysigma);
+            }
+#endif
+            getDeviation(Objects); // calculate dX/dY and process corrections
+        }
+        DELTA("prepare image");
+        { // prepare image and save jpeg
+            uint8_t *outp = NULL;
+            if(theconf.equalize)
+                outp = equalize(I, 3, theconf.throwpart);
+            else
+                outp = linear(I, 3);
+            static il_Pattern *cross = NULL, *crossL = NULL;
+            if(!cross) cross = il_Pattern_xcross(33, 33);
+            if(!crossL) crossL = il_Pattern_xcross(51, 51);
+            il_Img3 i3 = {.data = outp, .w = I->width, .h = H};
+            DELTA("Draw crosses");
+            // draw fiber center position
+            il_Pattern_draw3(&i3, crossL, theconf.xtarget-theconf.xoff, H-(theconf.ytarget-theconf.yoff), C_R);
+            if(objctr){ // draw crosses @ objects' centers
+                int H = I->height;
+                // draw current star centroid
+                il_Pattern_draw3(&i3, cross, Objects[0].xc, H-Objects[0].yc, C_G);
+                // add offset to show in target system
+                xc = Objects[0].xc + theconf.xoff;
+                yc = Objects[0].yc + theconf.yoff;
+                // draw other centroids
+                for(int i = 1; i < objctr; ++i)
+                    il_Pattern_draw3(&i3, cross, Objects[i].xc, H-Objects[i].yc, C_B);
+            }else{xc = -1.; yc = -1.;}
+            char tmpnm[FILENAME_MAX+5];
+            sprintf(tmpnm, "%s-tmp", GP->outputjpg);
+            if(stbi_write_jpg(tmpnm, I->width, I->height, 3, outp, 95)){
+                if(rename(tmpnm, GP->outputjpg)){
+                    WARN("rename()");
+                    LOGWARN("can't save %s", GP->outputjpg);
+                }
+            }
+            DELTA("Written");
+            FREE(outp);
+        }
+    }else{
+        xc = -1.; yc = -1.;
+        Image_write_jpg(I, GP->outputjpg, theconf.equalize);
+    }
+    DBGLOG("Image saved");
     ++ImNumber;
     if(lastTproc > 1.) FPS = 1. / (sl_dtime() - lastTproc);
     lastTproc = sl_dtime();

LocCorr_new/socket.c

@@ -67,6 +67,7 @@ typedef struct{
 static char *helpmsg(const char *messageid, char *buf, int buflen);
 static char *stepperstatus(const char *messageid, char *buf, int buflen);
 static char *getimagedata(const char *messageid, char *buf, int buflen);
+// should be in sorted order
 static getter getterHandlers[] = {
     {"help", helpmsg, "List avaiable commands"},
     {"imdata", getimagedata, "Get image data (status, path, FPS, counter)"},
@@ -80,7 +81,7 @@ static char *setfocusstate(const char *state, char *buf, int buflen);
 static char *moveU(const char *val, char *buf, int buflen);
 static char *moveV(const char *val, char *buf, int buflen);
 static char *addcmnt(const char *cmnt, char *buf, int buflen);
-
+// should be in sorted order
 static setter setterHandlers[] = {
     {"comment", addcmnt, "Add comment to XY log file"},
     {"focus", setfocusstate, "Move focus to given value"},

LocCorr_new/steppers.c

@@ -86,8 +86,8 @@ steppersproc *theSteppers = NULL;
 // stepper numbers
 typedef enum{
       Ustepper = 0,
-      Vstepper = 2,
+      Vstepper = 1,
-      Fstepper = 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);
+    //LOGDBG("send_message(): message '%s' sent", buf);
-    ;;; // wait for errcode
+    double t0 = sl_dtime();
-    return ERR_OK;
+    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) ||
+            if( !nth_motor_setter(CMD_ABSPOS, Ustepper, 0) ||
-                !nth_motor_setter(CMD_ABSPOS, Vstepper, 1) ||
+                !nth_motor_setter(CMD_ABSPOS, Vstepper, 0) ||
-                !nth_motor_setter(CMD_ABSPOS, Fstepper, 1)) break;
+                !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) ||
+            if( !nth_motor_setter(CMD_ABSPOS, Ustepper, 0) ||
-                !nth_motor_setter(CMD_ABSPOS, Vstepper, 1)) break;
+                !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;
-            if(!relaxed(Ustepper) && !relaxed(Vstepper)) continue;
+                first = TRUE;
-            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)

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)