seems like it works good; need tests on real star

LocCorr_new/basler.c

@@ -216,14 +216,19 @@ static int connect(){
 }
 
 static Image *capture(){
-    FNAME();
+    //FNAME();
     static int toohot = FALSE;
     if(!isopened || !imgBuf) return NULL;
     float_values f;
+    static double t0 = 0.;
     if(!getFloat("DeviceTemperature", &f)) WARNX("Can't get temperature");
     else{
-        LOGDBG("Basler temperature: %.1f", f.val);
+        double t = dtime();
-        DBG("Temperature: %.1f", f.val);
+        if(t - t0 >= 30.){ // log T each 30 seconds
+            LOGMSG("Basler temperature: %.1f", f.val);
+            t0 = t;
+        }
+        //DBG("Temperature: %.1f", f.val);
         if(f.val > 80.){
             WARNX("Device too hot");
             if(!toohot){

LocCorr_new/cameracapture.c

@@ -18,6 +18,7 @@
 
 #include <float.h> // FLT_EPSILON
 #include <math.h>
+#include <pthread.h>
 #include <stdio.h>
 #include <string.h>
 
@@ -122,6 +123,9 @@ static void calcexpgain(float newexp){
 
 //convertedImage.pData, convertedImage.cols, convertedImage.rows, convertedImage.stride
 static void recalcexp(Image *I){
+#ifdef EBUG
+    green("RECALCEXP\n"); fflush(stdout);
+#endif
     // check if user changed exposition values
     if(exptime < theconf.minexp){
         exptime = theconf.minexp;
@@ -131,29 +135,104 @@ static void recalcexp(Image *I){
         exptime = theconf.maxexp;
         return;
     }
-    size_t *histogram = get_histogram(I);
+    size_t histogram[HISTOSZ];
-    if(!histogram){
+    if(!get_histogram(I, histogram)){
         WARNX("Can't calculate histogram");
         return;
     }
     int idx100;
     size_t sum100 = 0;
-    for(idx100 = 255; idx100 >= 0; --idx100){
+    for(idx100 = HISTOSZ-1; idx100 >= 0; --idx100){
         sum100 += histogram[idx100];
         if(sum100 > 100) break;
     }
-    FREE(histogram);
     DBG("Sum100=%zd, idx100=%d", sum100, idx100);
-    if(idx100 > 230 && idx100 < 253) return; // good values
+    if(idx100 > 230 && idx100 < 253){
+        DBG("idx100=%d - good", idx100);
+        return; // good values
+    }
     if(idx100 > 253){ // exposure too long
+        DBG("Exp too long");
         calcexpgain(0.7*exptime);
     }else{ // exposure too short
-        if(idx100 > 5)
+        if(idx100 > 5){
+            DBG("Exp too short");
             calcexpgain(exptime * 230. / (float)idx100);
-        else
+        }else{
+            DBG("divide exp by 2");
             calcexpgain(exptime * 50.);
         }
     }
+}
+
+static int needs_exposure_adjustment(const Image *I, float curr_x, float curr_y) {
+    static float last_avg_intensity = -1.f;
+    static float last_centroid_x = -1.f, last_centroid_y = -1.f;
+    float avg = I->avg_intensity;
+    float dx = fabsf(curr_x - last_centroid_x);
+    float dy = fabsf(curr_y - last_centroid_y);
+    // Adjust if intensity changes >10% or centroid moves >20px or no x/y centroids
+    if(curr_x < 0.f || curr_y < 0.f) return TRUE;
+    if(fabsf(avg - last_avg_intensity) > 0.1f * last_avg_intensity ||
+        dx > 20.f || dy > 20.f){
+        DBG("avg_cur=%g, avg_last=%g, dx=%g, dy=%g", avg, last_avg_intensity, dx, dy);
+        last_avg_intensity = avg;
+        last_centroid_x = curr_x;
+        last_centroid_y = curr_y;
+        return TRUE;
+    }
+    return FALSE;
+}
+
+static pthread_mutex_t capt_mutex = PTHREAD_MUTEX_INITIALIZER;
+static int iCaptured = -1; // index of last captured image
+static Image* Icap[2] = {0}; // buffer for last captured images
+// main capture thread fills empty buffers and wait until processed thread free's one of them
+static void *procthread(void* v){
+    typedef void (*procfn_t)(Image*);
+    void (*process)(Image*) = (procfn_t)v;
+#ifdef EBUG
+    double t0 = dtime();
+#endif
+    while(!stopwork){
+        while(iCaptured < 0) usleep(1000);
+        pthread_mutex_lock(&capt_mutex);
+        if(Icap[iCaptured]){
+            DBG("---- got image #%d @ %g", iCaptured, 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);
+                    if(X){
+                        FREE(oIma->data);
+                        FREE(oIma);
+                        oIma = X;
+                    }
+                }
+                process(oIma);
+            }
+            FREE(oIma->data);
+            FREE(oIma);
+            DBG("---- cleared image data @ %g", dtime() - t0);
+        }else pthread_mutex_unlock(&capt_mutex);
+        usleep(1000);
+    }
+    return NULL;
+}
 
 int camcapture(void (*process)(Image*)){
     FNAME();
@@ -161,6 +240,11 @@ int camcapture(void (*process)(Image*)){
     static float oldgain = -1.;
     static float oldbrightness = -1.;
     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()");
+    }
     while(1){
         if(stopwork){
             DBG("STOP");
@@ -213,35 +297,37 @@ int camcapture(void (*process)(Image*)){
             camdisconnect();
             continue;
         }
-        if(theconf.expmethod == EXPAUTO) recalcexp(oIma);
+        pthread_mutex_lock(&capt_mutex);
-        else{
+        if(iCaptured < 0) iCaptured = 0;
-            if(fabs(theconf.fixedexp - exptime) > FLT_EPSILON)
+        else iCaptured = !iCaptured;
-                exptime = theconf.fixedexp;
+        if(Icap[iCaptured]){ // try current value if previous is still busy
-            if(fabs(theconf.gain - gain) > FLT_EPSILON)
+            iCaptured = !iCaptured;
-                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);
-                if(X){
-                    FREE(oIma->data);
-                    FREE(oIma);
-                    oIma = X;
-                }
-            }
-            process(oIma);
         }
+        if(!Icap[iCaptured]){ // previous buffer is free
+            DBG("---- take iCaptured=%d", iCaptured);
+            Icap[iCaptured] = oIma;
+            oIma = NULL;
+        }else{ // clear our image - there's no empty buffers
+            DBG("---- no free buffers");
             FREE(oIma->data);
             FREE(oIma);
         }
+        pthread_mutex_unlock(&capt_mutex);
+    }
+    pthread_cancel(proc_thread);
     if(oIma){
         FREE(oIma->data);
         FREE(oIma);
     }
+    for(int i = 0; i < 2; ++i){
+        if(Icap[i]){
+            FREE(Icap[i]->data);
+            FREE(Icap[i]);
+        }
+    }
     camdisconnect();
     DBG("CAMCAPTURE: out");
+    pthread_join(proc_thread, NULL);
     return 1;
 }
 

LocCorr_new/cmdlnopts.c

@@ -66,6 +66,7 @@ static myoption cmdlnopts[] = {
     {"maxexp",  NEED_ARG,   NULL,   0,      arg_double, APTR(&G.maxexp),    _("maximal exposition time (ms), default: 500")},
     {"minexp",  NEED_ARG,   NULL,   0,      arg_double, APTR(&G.minexp),    _("minimal exposition time (ms), default: 0.001")},
     {"help",    NO_ARGS,    NULL,   'h',    arg_int,    APTR(&help),        _("show this help")},
+    {"chkconf", NO_ARGS,    NULL,   'C',    arg_int,    APTR(&G.chkconf),   _("check configuration file")},
     {"logfile", NEED_ARG,   NULL,   'l',    arg_string, APTR(&G.logfile),   _("file to save logs (default: none)")},
     {"pidfile", NEED_ARG,   NULL,   'P',    arg_string, APTR(&G.pidfile),   _("pidfile (default: " DEFAULT_PIDFILE ")")},
     {"verbose", NO_ARGS,    NULL,   'v',    arg_none,   APTR(&G.verb),      _("increase verbosity level of log file (each -v increased by 1)")},
@@ -86,7 +87,7 @@ static myoption cmdlnopts[] = {
     {"ytarget", NEED_ARG,   NULL,   'Y',    arg_double, APTR(&G.ytarget),   _("target point Y coordinate")},
     {"logXY",   NEED_ARG,   NULL,   'L',    arg_string, APTR(&G.logXYname), _("file to log XY coordinates of selected star")},
     {"confname",NEED_ARG,   NULL,   'c',    arg_string, APTR(&G.configname),_("name of configuration file (default: ./loccorr.conf)")},
-    {"canport", NEED_ARG,   NULL,   'C',    arg_string, APTR(&G.steppersport),_("port of local pusirobot CAN server (default: 4444)")},
+    {"stpport", NEED_ARG,   NULL,   'S',    arg_string, APTR(&G.steppersport),_("port of local steppers server (default: 4444)")},
     {"naverage",NEED_ARG,   NULL,   'N',    arg_int,    APTR(&G.Naveraging),_("amount of images to average processing (min 2, max 25)")},
     {"ioport",  NEED_ARG,   NULL,   0,      arg_int,    APTR(&G.ioport),    _("port for IO communication")},
     {"jpegout", NEED_ARG,   NULL,   'j',    arg_string, APTR(&G.outputjpg), _("output jpeg file location (default: '" DEFAULT_OUTPJPEG "')")},

LocCorr_new/cmdlnopts.h

@@ -54,6 +54,7 @@ typedef struct{
     int steppersport;       // port of local motors CAN server
     int equalize;           // make historam equalization of saved jpeg
 //    int medradius;          // radius of median filter (r=1 -> 3x3, r=2 -> 5x5 etc.)
+    int chkconf;            // check config file
     int verb;               // logfile verbosity level
     int ndilations;         // amount of erosions (default: 2)
     int nerosions;          // amount of dilations (default: 2)

LocCorr_new/config.c

@@ -51,6 +51,12 @@ configuration theconf = {
     .Kyu=0,
     .Kxv=0,
     .Kyv=0,
+    .PIDU_P = PID_P_DEFAULT,
+    .PIDU_I = PID_I_DEFAULT,
+    .PIDU_D = PID_D_DEFAULT,
+    .PIDV_P = PID_P_DEFAULT,
+    .PIDV_I = PID_I_DEFAULT,
+    .PIDV_D = PID_D_DEFAULT,
     .xtarget=-1,
     .ytarget=-1,
     .throwpart=DEFAULT_THROWPART,
@@ -62,6 +68,12 @@ configuration theconf = {
     .medseed=MIN_MEDIAN_SEED,
 };
 
+static int isSorted = 0; // ==1 when `parvals` are sorted
+static int compConfVals(const void *_1st, const void *_2nd){
+    const confparam *a = (confparam*)_1st, *b = (confparam*)_2nd;
+    return strcmp(a->name, b->name);
+}
+
 // {"", PAR_DOUBLE, (void*)&theconf., 0},
 static confparam parvals[] = {
     {"maxarea", PAR_INT, (void*)&theconf.maxarea, 0, MINAREA, MAXAREA,
@@ -116,6 +128,18 @@ static confparam parvals[] = {
      "X coordinate of target position"},
     {"ytarget", PAR_DOUBLE, (void*)&theconf.ytarget, 0, 1., MAX_OFFSET,
      "Y coordinate of target position"},
+    {"pidup", PAR_DOUBLE, (void*)&theconf.PIDU_P, 0, PID_P_MIN, PID_P_MAX,
+     "U axis P PID parameter"},
+    {"pidui", PAR_DOUBLE, (void*)&theconf.PIDU_I, 0, PID_I_MIN, PID_I_MAX,
+     "U axis I PID parameter"},
+    {"pidud", PAR_DOUBLE, (void*)&theconf.PIDU_D, 0, PID_I_MIN, PID_I_MAX,
+     "U axis D PID parameter"},
+    {"pidvp", PAR_DOUBLE, (void*)&theconf.PIDV_P, 0, PID_P_MIN, PID_P_MAX,
+     "V axis P PID parameter"},
+    {"pidvi", PAR_DOUBLE, (void*)&theconf.PIDV_I, 0, PID_I_MIN, PID_I_MAX,
+     "V axis I PID parameter"},
+    {"pidvd", PAR_DOUBLE, (void*)&theconf.PIDV_D, 0, PID_I_MIN, PID_I_MAX,
+     "V axis D PID parameter"},
     {"eqthrowpart", PAR_DOUBLE, (void*)&theconf.throwpart, 0, 0., MAX_THROWPART,
      "a part of low intensity pixels to throw away when histogram equalized"},
     {"minexp", PAR_DOUBLE, (void*)&theconf.minexp, 0, 0., EXPOS_MAX,
@@ -148,6 +172,10 @@ char *get_cmd_list(char *buff, int l){
     if(!buff || l < 1) return NULL;
     int L = l;
     char *ptr = buff;
+    if(!isSorted){
+        qsort(parvals, sizeof(parvals)/sizeof(confparam) - 1, sizeof(confparam), compConfVals);
+        isSorted = 1;
+    }
     confparam *par = parvals;
     while(L > 0 && par->name){
         int s = snprintf(ptr, L, "%s=newval - %s (from %g to %g)\n", par->name, par->help, par->minval, par->maxval);
@@ -161,10 +189,8 @@ char *get_cmd_list(char *buff, int l){
 static char *omitspaces(char *v){
     if(!v) return NULL;
     while(*v && (*v == ' ' || *v == '\t')) ++v;
-    char *ptr = strchr(v, ' ');
+    int l = strlen(v);
-    if(ptr) *ptr = 0;
+    while(l-- && (v[l] == ' ' || v[l] == '\t')) v[l] = 0;
-    ptr = strchr(v, '\t');
-    if(ptr) *ptr = 0;
     return v;
 }
 
@@ -173,12 +199,17 @@ static char *omitspaces(char *v){
 char *get_keyval(const char *pair, char value[128]){
     char key[128];
     char val[128];
-    if(!pair || !*pair) return strdup("#"); // empty line
+    //if(!pair || !*pair) return strdup("#"); // empty line
+    if(!pair || !*pair){
+        //DBG("Empty");
+        return NULL; // empty line
+    }
     char *keyptr = key, *valptr = val;
     int x = sscanf(pair, "%127[^=]=%127[^\n]%*c", key, val);
     //DBG("x=%d, key='%s', val='%s'", x, key, val);
     if(x < 0 || x > 2) return NULL; // wrong data or EOF
-    if(x == 0) return strdup("#"); // empty line
+    //if(x == 0) return strdup("#"); // empty line
+    if(x == 0) return NULL; // empty line
     keyptr = omitspaces(key);
     if(x == 2){ // param = value
         valptr = omitspaces(val);
@@ -187,7 +218,8 @@ char *get_keyval(const char *pair, char value[128]){
     }
     if(*keyptr == '#' || *keyptr == '%'){ // comment
         *value = 0;
-        return strdup("#");
+        //return strdup("#");
+        return NULL;
     }
     return NULL;
 }
@@ -219,6 +251,17 @@ static int str2int(int *num, const char *str){
     return TRUE;
 }
 
+// find configuration record for getter
+confparam *find_key(const char *key){
+    if(!key) return NULL;
+    confparam *par = parvals;
+    while(par->name){
+        if(strcmp(key, par->name) == 0) return par;
+        ++par;
+    }
+    return NULL;
+}
+
 /**
  * @brief chk_keyval - check key for presence in theconf and calculate its value
  * @param key (i) - keyword
@@ -228,9 +271,8 @@ static int str2int(int *num, const char *str){
  */
 confparam *chk_keyval(const char *key, const char *val, key_value *result){
     if(!key || !val || !result) return NULL;
-    confparam *par = parvals;
+    confparam *par = find_key(key);
-    while(par->name){
+    if(!par) return NULL;
-        if(strcmp(key, par->name) == 0){
     //DBG("key='%s', par->name='%s'", key, par->name);
     result->type = par->type;
     switch(par->type){
@@ -240,10 +282,11 @@ confparam *chk_keyval(const char *key, const char *val, key_value *result){
                 WARNX("Wrong integer value '%s' of parameter '%s'", val, key);
                 return NULL;
             }
-                    if(result->val.intval < par->minval || result->val.intval > par->maxval)
+            if(result->val.intval < par->minval || result->val.intval > par->maxval){
                 WARNX("Value (%d) of parameter %s out of range %g..%g",
                        result->val.intval, par->name, par->minval, par->maxval);
-                    else return par;
+                break;
+            } else return par;
         break;
         case PAR_DOUBLE:
             //DBG("DOUBLE");
@@ -252,15 +295,12 @@ confparam *chk_keyval(const char *key, const char *val, key_value *result){
                 return NULL;
             }
             //DBG("val: %g, min: %g, max: %g", result->val.dblval, par->minval, par->maxval);
-                    if(result->val.dblval < par->minval || result->val.dblval > par->maxval)
+            if(result->val.dblval < par->minval || result->val.dblval > par->maxval){
                 WARNX("Value (%g) of parameter %s out of range %g..%g",
                        result->val.dblval, par->name, par->minval, par->maxval);
-                    else return par;
                 break;
-            }
+            } else return par;
-            return NULL;
+        break;
-        }
-        ++par;
     }
     return NULL;
 }
@@ -352,6 +392,10 @@ int saveconf(const char *confname){
         LOGERR("Can't open %s to store configuration", confname);
         return FALSE;
     }
+    if(!isSorted){
+        qsort(parvals, sizeof(parvals)/sizeof(confparam) - 1, sizeof(confparam), compConfVals);
+        isSorted = 1;
+    }
     confparam *par = parvals;
     while(par->name){
         par->got = 1;
@@ -377,6 +421,10 @@ int saveconf(const char *confname){
 char *listconf(const char *messageid, char *buf, int buflen){
     int L;
     char *ptr = buf;
+    if(!isSorted){
+        qsort(parvals, sizeof(parvals)/sizeof(confparam) - 1, sizeof(confparam), compConfVals);
+        isSorted = 1;
+    }
     confparam *par = parvals;
     L = snprintf(ptr, buflen, "{ \"%s\": \"%s\", ", MESSAGEID, messageid);
     buflen -= L; ptr += L;

LocCorr_new/config.h

@@ -46,7 +46,7 @@
 #define KUVMIN           (-5000.)
 #define KUVMAX           (5000.)
 // default coefficient for corrections (move to Kdu, Kdv instead of du, dv)
-#define KCORR           (0.90)
+//#define KCORR           (0.90)
 // min/max median seed
 #define MIN_MEDIAN_SEED (1)
 #define MAX_MEDIAN_SEED (7)
@@ -62,6 +62,17 @@
 #define MINWH                       (0.3)
 #define MAXWH                       (3.)
 
+// PID limits
+#define PID_P_MIN       (0.1)
+#define PID_P_MAX       (3.)
+#define PID_P_DEFAULT   (0.7)
+#define PID_I_MIN       (0.)
+#define PID_I_MAX       (3.)
+#define PID_I_DEFAULT   (0.1)
+#define PID_D_MIN       (0.)
+#define PID_D_MAX       (5.)
+#define PID_D_DEFAULT   (0.05)
+
 // messageID field name
 #define MESSAGEID       "messageid"
 
@@ -102,6 +113,9 @@ typedef struct{
     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
+    // PID regulator for axes U and V
+    double PIDU_P; double PIDU_I; double PIDU_D;
+    double PIDV_P; double PIDV_I; double PIDV_D;
 } configuration;
 
 typedef enum{
@@ -135,6 +149,7 @@ int chkconfig(const char *confname);
 int saveconf(const char *confname);
 char *get_keyval(const char *pair, char value[128]);
 confparam *chk_keyval(const char *key, const char *val, key_value *result);
+confparam *find_key(const char *key);
 char *listconf(const char *messageid, char *buf, int buflen);
 
 #endif // CONFIG_H__

LocCorr_new/imagefile.c

@@ -137,7 +137,7 @@ InputType chkinput(const char *name){
  * @return Image structure (fully allocated, you can FREE(data) after it)
  */
 Image *u8toImage(const uint8_t *data, int width, int height, int stride){
-    FNAME();
+    //FNAME();
     Image *outp = Image_new(width, height);
     // flip image updown for FITS coordinate system
     OMP_FOR()
@@ -218,25 +218,26 @@ Image *Image_sim(const Image *i){
 /**
  * @brief get_histogram - calculate image histogram
  * @param I - orig
- * @return
+ * @param histo - histogram
+ * @return FALSE if failed
  */
-size_t *get_histogram(const Image *I){
+int get_histogram(const Image *I, size_t histo[HISTOSZ]){
-    if(!I || !I->data) return NULL;
+    if(!I || !I->data || !histo) return FALSE;
-    size_t *histogram = MALLOC(size_t, 256);
+    bzero(histo, HISTOSZ*sizeof(size_t));
     int wh = I->width * I->height;
 #pragma omp parallel
 {
-    size_t histogram_private[256] = {0};
+    size_t histogram_private[HISTOSZ] = {0};
     #pragma omp for nowait
     for(int i = 0; i < wh; ++i){
         ++histogram_private[I->data[i]];
     }
     #pragma omp critical
     {
-        for(int i = 0; i < 256; ++i) histogram[i] += histogram_private[i];
+        for(int i = 0; i < HISTOSZ; ++i) histo[i] += histogram_private[i];
     }
 }
-    return histogram;
+    return TRUE;
 }
 
 
@@ -246,21 +247,22 @@ size_t *get_histogram(const Image *I){
  * @param bk (o)  - background value
  * @return 0 if error
  */
-int calc_background(const Image *img, Imtype *bk){
+int calc_background(Image *img){
-    if(!img || !img->data || !bk) return FALSE;
+    if(!img || !img->data) return FALSE;
     if(img->maxval == img->minval){
         WARNX("Zero or overilluminated image!");
         return FALSE;
     }
     if(theconf.fixedbkg){
-        if(theconf.fixedbkg > img->minval){
+        if(theconf.fixedbkg < img->minval){
             WARNX("Image values too small");
             return FALSE;
         }
-        *bk = theconf.fixedbkg;
+        img->background = theconf.fixedbkg;
         return TRUE;
     }
-    size_t *histogram = get_histogram(img);
+    size_t histogram[HISTOSZ];
+    if(!get_histogram(img, histogram)) return FALSE;
 
     size_t modeidx = 0, modeval = 0;
     for(int i = 0; i < 256; ++i)
@@ -273,7 +275,6 @@ int calc_background(const Image *img, Imtype *bk){
     for(int i = 2; i < 254; ++i) diff2[i] = (histogram[i+2]+histogram[i-2]-2*histogram[i])/4;
     //green("HISTO:\n");
     //for(int i = 0; i < 256; ++i) printf("%d:\t%d\t%d\n", i, histogram[i], diff2[i]);
-    FREE(histogram);
     if(modeidx < 2) modeidx = 2;
     if(modeidx > 253){
         WARNX("Overilluminated image");
@@ -287,7 +288,7 @@ int calc_background(const Image *img, Imtype *bk){
     }
     //DBG("borderidx=%d -> %d", borderidx, (borderidx+modeidx)/2);
     //*bk = (borderidx + modeidx) / 2;
-    *bk = borderidx;
+    img->background = borderidx;
     return TRUE;
 }
 
@@ -300,6 +301,7 @@ int calc_background(const Image *img, Imtype *bk){
  */
 uint8_t *linear(const Image *I, int nchannels){ // only 1 and 3 channels supported!
     if(!I || !I->data || (nchannels != 1 && nchannels != 3)) return NULL;
+    FNAME();
     int width = I->width, height = I->height;
     size_t stride = width*nchannels, S = height*stride;
     uint8_t *outp = MALLOC(uint8_t, S);
@@ -337,10 +339,11 @@ uint8_t *linear(const Image *I, int nchannels){ // only 1 and 3 channels support
  */
 uint8_t *equalize(const Image *I, int nchannels, double throwpart){
     if(!I || !I->data || (nchannels != 1 && nchannels != 3)) return NULL;
+    FNAME();
     int width = I->width, height = I->height;
     size_t stride = width*nchannels, S = height*stride;
-    size_t *orig_histo = get_histogram(I); // original hystogram (linear)
+    size_t orig_histo[HISTOSZ]; // original hystogram (linear)
-    if(!orig_histo) return NULL;
+    if(!get_histogram(I, orig_histo)) return NULL;
     uint8_t *outp = MALLOC(uint8_t, S);
     uint8_t eq_levls[256] = {0};   // levels to convert: newpix = eq_levls[oldpix]
     int s = width*height;
@@ -388,7 +391,6 @@ uint8_t *equalize(const Image *I, int nchannels, double throwpart){
             }
         }
     }
-    FREE(orig_histo);
     return outp;
 }
 
@@ -426,28 +428,33 @@ int Image_write_jpg(const Image *I, const char *name, int eq){
 void Image_minmax(Image *I){
     if(!I || !I->data) return;
     Imtype min = *(I->data), max = min;
+    float isum = 0.f;
     int wh = I->width * I->height;
 #ifdef EBUG
-    double t0 = dtime();
+    //double t0 = dtime();
 #endif
-    #pragma omp parallel shared(min, max)
+    #pragma omp parallel shared(min, max, isum)
     {
         int min_p = min, max_p = min;
+        float sum_p = 0.f;
         #pragma omp for nowait
         for(int i = 0; i < wh; ++i){
             Imtype pixval = I->data[i];
             if(pixval < min_p) min_p = pixval;
             else if(pixval > max_p) max_p = pixval;
+            sum_p += (float) pixval;
         }
         #pragma omp critical
         {
             if(min > min_p) min = min_p;
             if(max < max_p) max = max_p;
+            isum += sum_p;
         }
     }
     I->maxval = max;
     I->minval = min;
-    DBG("Image_minmax(): Min=%d, Max=%d, time: %gms", min, max, (dtime()-t0)*1e3);
+    I->avg_intensity = isum / (float)wh;
+    DBG("Image_minmax(): Min=%d, Max=%d, Isum=%g, mean=%g", min, max, isum, I->avg_intensity);
 }
 
 /*

LocCorr_new/imagefile.h

@@ -27,7 +27,10 @@
 #define OMP_FOR(x) _Pragma(Stringify(omp parallel for x))
 #endif
 
-typedef uint8_t Imtype; // maybe float or double only
+typedef uint8_t Imtype;
+// 65536 if Imtype is uint16_t
+// WARNING! Check code if you change Imtype: e.g. recalcexp() and other
+#define HISTOSZ (256)
 
 typedef struct{
     int width;			// width
@@ -35,6 +38,8 @@ typedef struct{
     Imtype *data;		// picture data
     Imtype minval;      // extremal data values
     Imtype maxval;
+    float avg_intensity;
+    Imtype background;  // background value
 } Image;
 
 // input file/directory type
@@ -60,8 +65,8 @@ Image *Image_new(int w, int h);
 Image *Image_sim(const Image *i);
 void Image_free(Image **I);
 int Image_write_jpg(const Image *I, const char *name, int equalize);
-size_t *get_histogram(const Image *I);
+int get_histogram(const Image *I, size_t histo[HISTOSZ]);
-int calc_background(const Image *img, Imtype *bk);
+int calc_background(Image *img);
 
 Image *u8toImage(const uint8_t *data, int width, int height, int stride);
 Image *bin2Im(const uint8_t *image, int W, int H);

LocCorr_new/improc.c

@@ -84,6 +84,21 @@ static void XYnewline(){
     fflush(fXYlog);
 }
 
+// add comment string to XY log; @return FALSE if failed (file not exists)
+int XYcomment(char *cmnt){
+    if(!fXYlog || !cmnt) return FALSE;
+    if(*cmnt == '"'){
+        ++cmnt;
+        char *e = strrchr(cmnt, '"');
+        if(e) *e = 0;
+    }
+    char *n = strrchr(cmnt, '\n');
+    if(n) *n = 0;
+    fprintf(fXYlog, "# %s\n", cmnt);
+    fflush(fXYlog);
+    return TRUE;
+}
+
 static void getDeviation(object *curobj){
     int averflag = 0;
     static double Xc[NAVER_MAX+1], Yc[NAVER_MAX+1];
@@ -92,8 +107,8 @@ static void getDeviation(object *curobj){
     static int counter = 0;
     Xc[counter] = curobj->xc; Yc[counter] = curobj->yc;
     if(fXYlog){ // make log record
-        fprintf(fXYlog, "%.2f\t%.1f\t%.1f\t%.1f\t%.1f\t%.1f\t",
+        fprintf(fXYlog, "%-14.2f\t%.1f\t%.1f\t%.1f\t%.1f\t%.1f\t",
-                dtime(), curobj->xc, curobj->yc,
+                dtime() - tstart, curobj->xc, curobj->yc,
                 curobj->xsigma, curobj->ysigma, curobj->WdivH);
     }
     //DBG("counter = %d", counter);
@@ -118,11 +133,13 @@ static void getDeviation(object *curobj){
     averflag = 1;
     if(fXYlog) fprintf(fXYlog, "%.1f\t%.1f\t%.1f\t%.1f", xx, yy, Sx, Sy);
 process_corrections:
-    if(theSteppers && theSteppers->proc_corr && averflag){
+    if(theSteppers){
+        if(theSteppers->proc_corr && averflag){
             if(Sx > XY_TOLERANCE || Sy > XY_TOLERANCE){
                 LOGDBG("Bad value - not process"); // don't run processing for bad data
             }else
                 theSteppers->proc_corr(xx, yy);
+        }
     }else{
         LOGERR("Lost connection with stepper server");
         ERRX("Lost connection with stepper server");
@@ -130,16 +147,71 @@ process_corrections:
     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)
+ * @param mask - labeled mask for objects (or NULL)
+ * @param idx - index on labeled mask
+ * @param roi - region of interest
+ * @param stat - (region - bacground) statistics
+ * @return total intensity sum
+ */
+static float sumAndStat(const Image *I, const size_t *mask, size_t idx, const il_Box *roi, ptstat_t *stat){
+    if(!I || !roi) return -1.;
+    //FNAME();
+    float xc = 0., yc = 0.;
+    float x2c = 0., y2c = 0., Isum = 0.;
+    int W = I->width;
+    //DBG("imw=%d, roi=%d:%d:%d:%d", W, roi->xmin, roi->xmax, roi->ymin, roi->ymax);
+    // dumb calculation as paralleling could be much slower
+    for(int y = roi->ymin; y <= roi->ymax; ++y){
+        size_t istart = y*W + roi->xmin;
+        //DBG("istart=%zd", istart);
+        const size_t *maskptr = (mask) ? &mask[istart] : NULL;
+        //DBG("mask %s NULL", mask ? "!=":"==");
+        Imtype *Iptr = &I->data[istart];
+        for(int x = roi->xmin; x <= roi->xmax; ++x, ++Iptr){
+            if(maskptr){if(*maskptr++ != idx) continue;}
+            if(*Iptr <= I->background) continue;
+            float intens = (float)(*Iptr - I->background);
+            float xw = x * intens, yw = y * intens;
+            xc += xw;
+            yc += yw;
+            x2c += xw * x;
+            y2c += yw * y;
+            Isum += intens;
+        }
+    }
+    if(stat && Isum > 0.){
+        stat->xc = xc / Isum;
+        stat->yc = yc / Isum;
+        stat->xsigma = x2c/Isum - stat->xc*stat->xc;
+        stat->ysigma = y2c/Isum - stat->yc*stat->yc;
+    }
+    //DBG("xc=%g, yc=%g, xs=%g, ys=%g", stat->xc, stat->yc, stat->xsigma, stat->ysigma);
+    return Isum;
+}
+
+#define MAX(a, b)   ((a) > (b) ? (a) : (b))
+#define MIN(a, b)   ((a) < (b) ? (a) : (b))
+
 void process_file(Image *I){
     static double lastTproc = 0.;
-/*
+    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 = dtime(), tlast = t0;
 #define DELTA(p) do{double t = dtime(); DBG("---> %s @ %gms (delta: %gms)", p, (t-t0)*1e3, (t-tlast)*1e3); tlast = t;}while(0)
 #else
-*/
 #define DELTA(x)
-//#endif
+#endif
     // I - original image
     // mean - local mean
     // std  - local STD
@@ -152,11 +224,52 @@ void process_file(Image *I){
     int W = I->width, H = I->height;
     //save_fits(I, "fitsout.fits");
     //DELTA("Save original");
-    Imtype bk;
+    if(calc_background(I)){
-    if(calc_background(I, &bk)){
+        DBG("backgr = %d", I->background);
-        DBG("backgr = %d", bk);
         DELTA("Got background");
-        uint8_t *ibin = Im2bin(I, bk);
+        int objctr = 0;
+        if(prev_x > 0 && prev_y > 0){
+            // Define ROI bounds
+            il_Box roi = {.xmin = MAX(prev_x - ROI_SIZE/2, 0),
+                          .xmax = MIN(prev_x + ROI_SIZE/2, W-1),
+                          .ymin = MAX(prev_y - ROI_SIZE/2, 0),
+                          .ymax = MIN(prev_y + ROI_SIZE/2, H-1)};
+            ptstat_t stat;
+            // Calculate centroid within ROI
+            DBG("Get sum and stat for simplest centroid");
+            double sum = sumAndStat(I, NULL, 0, &roi, &stat);
+            if(sum > 0.){
+                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);
+                }else{
+                    double WdH = stat.xsigma/stat.ysigma;
+                    // wery approximate area inside sigmax*sigmay
+                    double area = .4 * stat.xsigma * stat.ysigma;
+                    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
+                        prev_x = (int)stat.xc;
+                        prev_y = (int)stat.yc;
+                        DBG("Simplest centroid, Xc=%g, Yc=%g", stat.xc, stat.yc);
+                        objctr = 1;
+                        if(!Objects){
+                            Objects = (object*)malloc(sizeof(object));
+                            Nallocated = 1;
+                        }
+                        Objects[0] = (object){
+                            .area = area, .Isum = sum,
+                            .WdivH = WdH, .xc = stat.xc, .yc = stat.yc,
+                            .xsigma = stat.xsigma, .ysigma = stat.ysigma
+                        };
+                        goto SKIP_FULL_PROCESS; // Skip full image processing
+                    }else{
+                        DBG("BAD image: WdH=%g, area=%g, xsigma=%g, ysigma=%g", WdH, area, stat.xsigma, stat.ysigma);
+                    }
+                }
+            }
+        }
+        uint8_t *ibin = Im2bin(I, I->background);
         DELTA("Made binary");
         if(ibin){
             //savebin(ibin, W, H, "binary.fits");
@@ -171,54 +284,46 @@ void process_file(Image *I){
             DELTA("Opening");
             //savebin(opn, W, H, "opening.fits");
             //DELTA("Save opening");
-            il_ConnComps *cc = NULL;
+            S = il_cclabel4(opn, W, H, &cc);
-            size_t *S = il_cclabel4(opn, W, H, &cc);
             FREE(opn);
-            if(cc->Nobj > 1){ // Nobj = amount of objects + 1
+            DBG("Nobj=%zd", cc->Nobj-1);
-                DBGLOG("Nobj=%d", cc->Nobj-1);
+            if(S && cc && cc->Nobj > 1){ // Nobj = amount of objects + 1
-                object *Objects = MALLOC(object, cc->Nobj-1);
+                DBGLOG("Nobj=%zd", cc->Nobj-1);
-                int objctr = 0;
+                if(Nallocated < cc->Nobj-1){
+                    Nallocated = cc->Nobj-1;
+                    Objects = realloc(Objects, Nallocated*sizeof(object));
+                }
                 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);
                     if(wh < theconf.minwh || wh > theconf.maxwh) continue;
                     if((int)b->area < theconf.minarea || (int)b->area > theconf.maxarea) continue;
-                    double xc = 0., yc = 0.;
+                    ptstat_t stat;
-                    double x2c = 0., y2c = 0., Isum = 0.;
+                    DBG("Get sum and stat");
-                    for(size_t y = b->ymin; y <= b->ymax; ++y){
+                    double sum = sumAndStat(I, &S[i], i, b, &stat);
-                        size_t idx = y*W + b->xmin;
+                    if(sum > 0.){
-                        size_t *maskptr = &S[idx];
+                        if(cc->Nobj == 2){
-                        Imtype *Iptr = &I->data[idx];
+                            prev_x = (int)stat.xc, prev_y = (int)stat.yc;
-                        for(size_t x = b->xmin; x <= b->xmax; ++x, ++maskptr, ++Iptr){
-                            if(*maskptr != i) continue;
-                            double intens = (double) (*Iptr - bk);
-                            if(intens < 0.) continue;
-                            double xw = x * intens, yw = y * intens;
-                            xc += xw;
-                            yc += yw;
-                            x2c += xw * x;
-                            y2c += yw * y;
-                            Isum += intens;
                         }
-                    }
-                    xc /= Isum; yc /= Isum;
-                    x2c = x2c/Isum - xc*xc;
-                    y2c = y2c/Isum - yc*yc;
                         Objects[objctr++] = (object){
-                        .area = b->area, .Isum = Isum,
+                            .area = b->area, .Isum = sum,
-                        .WdivH = wh, .xc = xc, .yc = yc,
+                            .WdivH = wh, .xc = stat.xc, .yc = stat.yc,
-                        .xsigma = sqrt(x2c), .ysigma = sqrt(y2c)
+                            .xsigma = stat.xsigma, .ysigma = stat.ysigma
                         };
                     }
+                }
                 DELTA("Labeling");
-                DBGLOG("T%.2f, N=%d\n", dtime(), objctr);
                 if(objctr > 1){
+                    prev_x = -1, prev_y = -1; // don't allow simple gravcenter for a lots of objects
                     if(theconf.starssort)
                         qsort(Objects, objctr, sizeof(object), compIntens);
                     else
                         qsort(Objects, objctr, sizeof(object), compDist);
                 }
+SKIP_FULL_PROCESS:
+                DBGLOG("T%.2f, N=%d\n", dtime(), objctr);
+                DELTA("Calculate deviations");
                 if(objctr){
 #ifdef EBUG
                     object *o = Objects;
@@ -233,6 +338,7 @@ void process_file(Image *I){
 #endif
                     getDeviation(Objects); // calculate dX/dY and process corrections
                 }
+                DELTA("prepare image");
                 { // prepare image and save jpeg
                     uint8_t *outp = NULL;
                     if(theconf.equalize)
@@ -243,6 +349,7 @@ void process_file(Image *I){
                     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
@@ -256,7 +363,7 @@ void process_file(Image *I){
                         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 = MALLOC(char, strlen(GP->outputjpg) + 5);
+                    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)){
@@ -264,10 +371,9 @@ void process_file(Image *I){
                             LOGWARN("can't save %s", GP->outputjpg);
                         }
                     }
-                    FREE(tmpnm);
+                    DELTA("Written");
                     FREE(outp);
                 }
-                FREE(Objects);
             }else{
                 xc = -1.; yc = -1.;
                 Image_write_jpg(I, GP->outputjpg, theconf.equalize);
@@ -333,7 +439,7 @@ void openXYlog(const char *name){
     }
     time_t t = time(NULL);
     fprintf(fXYlog, "# Start at: %s", ctime(&t));
-    fprintf(fXYlog, "# time Xc\tYc\t\tSx\tSy\tW/H\taverX\taverY\tSX\tSY\n");
+    fprintf(fXYlog, "# time\t\tXc\tYc\tSx\tSy\tW/H\taverX\taverY\tSX\tSY\n");
     fflush(fXYlog);
     tstart = dtime();
 }

LocCorr_new/improc.h

@@ -25,6 +25,7 @@
 
 // tolerance of deviations by X and Y axis (if sigmaX or sigmaY greater, values considered to be wrong)
 #define XY_TOLERANCE                (5.)
+#define ROI_SIZE                    (200)
 
 extern volatile atomic_bool stopwork;
 extern volatile atomic_ullong ImNumber;
@@ -35,6 +36,7 @@ void process_file(Image *I);
 int  process_input(InputType tp, char *name);
 void openXYlog(const char *name);
 void closeXYlog();
+int XYcomment(char *cmnt);
 double getFramesPerS();
 void getcenter(float *x, float *y);
 

LocCorr_new/loccorr.conf

@@ -1,36 +1,44 @@
-maxarea = 4000
+Kxu = 45.155
-minarea = 100
+Kxv = 43.771
-minwh = 0.800
+Kyu = 68.838
-maxwh = 1.300
+Kyv = -70.428
-ndilat = 3
+brightness = 0.000
-neros = 3
+eqthrowpart = 0.900
-xoffset = 0
-yoffset = 0
-width = 1920
-height = 1200
 equalize = 0
 expmethod = 0
-naverage = 3
+fbglevel = 100
-umax = 400
+fixedbg = 0
-vmax = 230
-focmax = 4500
-focmin = 20
-stpservport = 4444
-Kxu = 45.155
-Kyu = 68.838
-Kxv = 43.771
-Kyv = -70.428
-xtarget = 1039.600
-ytarget = 602.820
-eqthrowpart = 0.900
-minexp = 50.000
-maxexp = 1000.000
 fixedexp = 1000.000
-intensthres = 0.010
+focmax = 4000
+focmin = 0
 gain = 36.000
-brightness = 0.000
+height = 800
-starssort = 0
+intensthres = 0.010
+maxarea = 4000
+maxexp = 1000.000
+maxwh = 1.300
 medfilt = 0
 medseed = 1
-fixedbg = 0
+minarea = 100
-fbglevel = 100
+minexp = 50.000
+minwh = 0.800
+naverage = 3
+ndilat = 3
+neros = 3
+pidud = 0.050
+pidui = 0.150
+pidup = 0.650
+pidvd = 0.050
+pidvi = 0.150
+pidvp = 0.650
+starssort = 0
+stpservport = 4444
+umax = 750
+umin = 50
+vmax = 470
+vmin = 100
+width = 700
+xoffset = 408
+xtarget = 740.000
+yoffset = 300
+ytarget = 730.000

LocCorr_new/main.c

@@ -116,6 +116,15 @@ int main(int argc, char *argv[]){
     initial_setup();
     char *self = strdup(argv[0]);
     GP = parse_args(argc, argv);
+    if(!chkconfig(GP->configname)){
+        LOGWARN("Wrong/absent configuration file");
+        WARNX("Wrong/absent configuration file");
+        if(GP->chkconf) return 1;
+    }
+    if(GP->chkconf){
+        printf("File %s OK\n", GP->configname);
+        return 0;
+    }
     if(GP->throwpart < 0. || GP->throwpart > 0.99){
         ERRX("Fraction of black pixels should be in [0., 0.99]");
     }
@@ -138,11 +147,6 @@ int main(int argc, char *argv[]){
         OPENLOG(GP->logfile, lvl, 1);
         DBG("Opened log file @ level %d", lvl);
     }
-    int C = chkconfig(GP->configname);
-    if(!C){
-        LOGWARN("Wrong/absent configuration file");
-        WARNX("Wrong/absent configuration file");
-    }
     // change `theconf` parameters to user values
     {
         if(GP->maxarea != DEFAULT_MAXAREA || theconf.maxarea == 0) theconf.maxarea = GP->maxarea;

LocCorr_new/socket.c

@@ -44,6 +44,8 @@
 // Max amount of connections
 #define BACKLOG     (10)
 
+static pthread_mutex_t cmd_mutex = PTHREAD_MUTEX_INITIALIZER;
+
 /*
 TODO3: add 'FAIL error text' if not OK and instead all "wrong message"
 */
@@ -67,9 +69,9 @@ static char *stepperstatus(const char *messageid, char *buf, int buflen);
 static char *getimagedata(const char *messageid, char *buf, int buflen);
 static getter getterHandlers[] = {
     {"help", helpmsg, "List avaiable commands"},
+    {"imdata", getimagedata, "Get image data (status, path, FPS, counter)"},
     {"settings", listconf, "List current configuration"},
     {"stpserv", stepperstatus, "Get status of steppers server"},
-    {"imdata", getimagedata, "Get image data (status, path, FPS, counter)"},
     {NULL, NULL, NULL}
 };
 
@@ -77,12 +79,14 @@ static char *setstepperstate(const char *state, char *buf, int buflen);
 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);
 
 static setter setterHandlers[] = {
-    {"stpstate", setstepperstate, "Set given steppers' server state"},
+    {"comment", addcmnt, "Add comment to XY log file"},
     {"focus", setfocusstate, "Move focus to given value"},
     {"moveU", moveU, "Relative moving by U axe"},
     {"moveV", moveV, "Relative moving by V axe"},
+    {"stpstate", setstepperstate, "Set given steppers' server state"},
     {NULL, NULL, NULL}
 };
 
@@ -90,6 +94,11 @@ static char *retFAIL(char *buf, int buflen){
     snprintf(buf, buflen, FAIL);
     return buf;
 }
+static char *retOK(char *buf, int buflen){
+    snprintf(buf, buflen, OK);
+    return buf;
+}
+
 
 /**************** functions to process commands ****************/
 // getters
@@ -143,6 +152,13 @@ static char *moveV(const char *val, char *buf, int buflen){
     if(theSteppers && theSteppers->moveByV) return theSteppers->moveByV(val, buf, buflen);
     return retFAIL(buf, buflen);
 }
+static char *addcmnt(const char *cmnt, char *buf, int buflen){
+    if(!cmnt) return retFAIL(buf, buflen);
+    char *line = strdup(cmnt);
+    int ret = XYcomment(line);
+    free(line);
+    return ret ? retOK(buf, buflen) : retFAIL(buf, buflen) ;
+}
 
 /*
 static char *rmnl(const char *msg, char *buf, int buflen){
@@ -162,14 +178,15 @@ static char *rmnl(const char *msg, char *buf, int buflen){
  */
 static char *processCommand(const char msg[BUFLEN], char *ans, int anslen){
     char value[BUFLEN];
-    char *kv = get_keyval(msg, value);
+    char *kv = get_keyval(msg, value); // ==NULL for getters/commands without equal sign
+    DBG("message: %s, value: %s, key: %s", msg, value, kv);
     confparam *par;
     if(kv){
         DBG("got KEY '%s' with value '%s'", kv, value);
         key_value result;
         par = chk_keyval(kv, value, &result);
-        free(kv); kv = NULL;
+        if(par){ // configuration parameter
-        if(par){
+            DBG("found this key in conf");
             switch(par->type){
                 case PAR_INT:
                     DBG("FOUND! Integer, old=%d, new=%d", *((int*)par->ptr), result.val.intval);
@@ -181,27 +198,67 @@ static char *processCommand(const char msg[BUFLEN], char *ans, int anslen){
                 break;
                 default:
                     snprintf(ans, anslen, FAIL);
+                    free(kv);
                     return ans;
             }
             snprintf(ans, anslen, OK);
+            free(kv);
             return ans;
         }else{
+            DBG("check common setters");
             setter *s = setterHandlers;
             while(s->command){
-                int l = strlen(s->command);
+                //int l = strlen(s->command);
-                if(strncasecmp(msg, s->command, l) == 0)
+                //if(strncasecmp(msg, s->command, l) == 0)
+                DBG("check '%s' == '%s' ?", kv, s->command);
+                if(strcmp(kv, s->command) == 0){
+                    free(kv);
                     return s->handler(value, ans, anslen);
+                }
                 ++s;
             }
         }
+        free(kv);
     }else{
+        DBG("getter?");
+        // first check custom getters
         getter *g = getterHandlers;
         while(g->command){
-            int l = strlen(g->command);
+            //int l = strlen(g->command);
-            if(strncasecmp(msg, g->command, l) == 0)
+            //if(strncasecmp(msg, g->command, l) == 0)
+            if(0 == strcmp(msg, g->command))
                 return g->handler(g->command, ans, anslen);
             ++g;
         }
+        DBG("not found in getterHandlers");
+        // check custom setters
+        setter *s = setterHandlers;
+        while(s->command){
+            if(strcmp(msg, s->command) == 0){
+                size_t p = snprintf(ans, anslen, "%s=", msg);
+                s->handler(NULL, ans+p, anslen-p);
+                return ans;
+            }
+            ++s;
+        }
+        DBG("not found in setterHandlers");
+        // and check configuration parameters
+        confparam *par = find_key(msg);
+        if(par){
+            switch(par->type){
+                case PAR_INT:
+                    snprintf(ans, anslen, "%s=%d", msg, *((int*)par->ptr));
+                break;
+                case PAR_DOUBLE:
+                    snprintf(ans, anslen, "%s=%g",  msg, *((double*)par->ptr));
+                break;
+                default:
+                    snprintf(ans, anslen, FAIL);
+                break;
+            }
+            return ans;
+        }
+        DBG("not found in parameters");
     }
     snprintf(ans, anslen, FAIL);
     return ans;
@@ -215,16 +272,24 @@ static char *processCommand(const char msg[BUFLEN], char *ans, int anslen){
  * @return amount of sent bytes
  */
 static size_t send_data(int sock, const char *textbuf){
-    ssize_t Len = strlen(textbuf);
+    size_t total = 0;
-    if(Len != write(sock, textbuf, Len)){
+    size_t len = strlen(textbuf);
-        WARN("write()");
+    const char *ptr = textbuf;
-        LOGERR("send_data(): write() failed");
+    while(total < len){
-        return 0;
+        ssize_t sent = write(sock, ptr + total, len - total);
-    }else{
+        if(sent < 0){
-        LOGDBG("send_data(): sent '%s'", textbuf);
+            if(errno == EINTR) continue;
+            LOGERR("Write error: %s", strerror(errno));
+            return total;
         }
-    if(textbuf[Len-1] != '\n') Len += write(sock, "\n", 1);
+        total += sent;
-    return (size_t)Len;
+    }
+    if(textbuf[len-1] != '\n'){
+        if(write(sock, "\n", 1) != 1){
+            LOGERR("Failed to write newline");
+        }
+    }
+    return total;
 }
 
 /**
@@ -233,7 +298,7 @@ static size_t send_data(int sock, const char *textbuf){
  * @return 0 if all OK, 1 if socket closed
  */
 static int handle_socket(int sock){
-    FNAME();
+    //FNAME();
     char buff[BUFLEN];
     char ansbuff[ANSBUFLEN];
     ssize_t rd = read(sock, buff, BUFLEN-1);
@@ -243,19 +308,31 @@ static int handle_socket(int sock){
     }
     // add trailing zero to be on the safe side
     buff[rd] = 0;
+    char *eol = strchr(buff, '\n');
+    if(eol) *eol = 0; // clear '\n'
     // now we should check what do user want
     // here we can process user data
     DBG("user %d send '%s'", sock, buff);
     LOGDBG("user %d send '%s'", sock, buff);
-    //pthread_mutex_lock(&mutex);
+    pthread_mutex_lock(&cmd_mutex);
     char *ans = processCommand(buff, ansbuff, ANSBUFLEN-1); // run command parser
     if(ans){
         send_data(sock, ans);   // send answer
     }
-    //pthread_mutex_unlock(&mutex);
+    pthread_mutex_unlock(&cmd_mutex);
     return 0;
 }
 
+// shutdown client and close
+static void cleanup_client(int fd) {
+    if(fd > 0) {
+        shutdown(fd, SHUT_RDWR);
+        close(fd);
+        DBG("Client with fd %d closed", fd);
+        LOGMSG("Client %d disconnected", fd);
+    }
+}
+
 // main socket server
 static void *server(void *asock){
     DBG("server(): getpid: %d, tid: %lu",getpid(), syscall(SYS_gettid));
@@ -273,9 +350,16 @@ static void *server(void *asock){
     while(1){
         if(stopwork){
             DBG("server() exit @ global stop");
+            LOGDBG("server() exit @ global stop");
             return NULL;
         }
-        poll(poll_set, nfd, 1); // poll for 1ms
+        int ready = poll(poll_set, nfd, 1); // 1ms timeout
+        if(ready < 0){
+            if(errno == EINTR) continue;
+            LOGERR("Poll error: %s", strerror(errno));
+            return NULL;
+        }
+        if(0 == ready) continue;
         for(int fdidx = 0; fdidx < nfd; ++fdidx){ // poll opened FDs
             if((poll_set[fdidx].revents & POLLIN) == 0) continue;
             poll_set[fdidx].revents = 0;
@@ -284,9 +368,7 @@ static void *server(void *asock){
                 //int nread = 0;
                 //ioctl(fd, FIONREAD, &nread);
                 if(handle_socket(fd)){ // socket closed - remove it from list
-                    close(fd);
+                    cleanup_client(fd);
-                    DBG("Client with fd %d closed", fd);
-                    LOGMSG("Client %d disconnected", fd);
                     // move last to free space
                     poll_set[fdidx] = poll_set[nfd - 1];
                     --nfd;
@@ -319,6 +401,8 @@ static void *server(void *asock){
             }
         } // endfor
     }
+    // disconnect all
+    for(int i = 1; i < nfd; ++i) cleanup_client(poll_set[i].fd);
     LOGERR("server(): UNREACHABLE CODE REACHED!");
 }
 

LocCorr_new/steppers.c

@@ -46,6 +46,14 @@
 // tolerance of coordinates coincidence (pix)
 #define COORDTOLERANCE      (0.5)
 
+// PID
+typedef struct {
+    double Kp, Ki, Kd;  // coefficients
+    double integral;    // intergal error accumulator
+    double prev_error;  // previous error value for D
+    double prev_time;   // and previous time
+} PIDController;
+
 typedef enum{
     STP_DISCONN,
     STP_RELAX,
@@ -53,7 +61,8 @@ typedef enum{
     STP_GOTOTHEMIDDLE,
     STP_FINDTARGET,
     STP_FIX,
-    STP_UNDEFINED
+    STP_UNDEFINED,
+    STP_STATE_AMOUNT
 } STPstate;
 
 typedef enum{
@@ -710,6 +719,31 @@ static int process_targetstage(double X, double Y){
     return TRUE;
 }
 
+/**
+ * @brief compute_pid - calculate PID responce for error
+ * @param pid - U/V PID parameters
+ * @param error - current error
+ * @param current_time - and current time
+ * @return PID-corrected responce
+ */
+static double compute_pid(PIDController *pid, double error, double current_time) {
+    double dt = current_time - pid->prev_time;
+    if(dt <= 0.) dt = 0.01; // Default to 10ms if time isn't tracked
+    // Integral term with anti-windup
+    pid->integral += error * dt;
+    // Clamp integral to ?1000 (adjust based on system limits)
+    if(pid->integral > 1000.) pid->integral = 1000.;
+    if(pid->integral < -1000.) pid->integral = -1000.;
+    // Derivative term (filtered)
+    double derivative = (error - pid->prev_error) / dt;
+    pid->prev_error = error;
+    pid->prev_time = current_time;
+    double pid_out = (pid->Kp * error) + (pid->Ki * pid->integral) + (pid->Kd * derivative);
+    LOGDBG("PID: error=%.2f, integral=%.2f, derivative=%.2f, output=%.2f",
+           error, pid->integral, derivative, pid_out);
+    return pid_out;
+}
+
 /**
  * @brief try2correct - try to correct position
  * @param dX - delta of X-coordinate in image space
@@ -718,25 +752,46 @@ static int process_targetstage(double X, double Y){
  */
 static int try2correct(double dX, double dY){
     if(!relaxed(Ustepper) || !relaxed(Vstepper)) return FALSE;
+    // calculations: make Ki=0, Kd=0; increase Kp until oscillations;
+    // now Tu - osc period, Ku=Kp for oscillations; so:
+    // Kp = 0.6*Ku; Ki = 1.2*Ku/Tu; Kd = 0.075*Ku*Tu (Ziegler-Nichols)
+    static PIDController pidU = {0}, pidV = {0};
+    // refresh parameters from configuration
+    pidU.Kp = theconf.PIDU_P; pidU.Ki = theconf.PIDU_I; pidU.Kd = theconf.PIDU_D;
+    pidV.Kp = theconf.PIDV_P; pidV.Ki = theconf.PIDV_I; pidV.Kd = theconf.PIDV_D;
     double dU, dV;
+    double current_time = dtime();
+    if( current_time - pidU.prev_time > MAX_PID_TIME
+        || current_time - pidV.prev_time > MAX_PID_TIME){
+        LOGWARN("Too old PID time: have dt=%gs", current_time - pidU.prev_time);
+        pidU.prev_time = pidV.prev_time = current_time;
+        pidU.integral = pidV.integral = 0.;
+        return FALSE;
+    }
     // dU = KU*(dX*cosXU + dY*sinXU); dV = KV*(dX*cosXV + dY*sinXV)
-    dU = KCORR*(theconf.Kxu * dX + theconf.Kyu * dY);
+    dU = theconf.Kxu * dX + theconf.Kyu * dY;
-    dV = KCORR*(theconf.Kxv * dX + theconf.Kyv * dY);
+    dV = theconf.Kxv * dX + theconf.Kyv * dY;
-    int Unew = Uposition + (int)dU, Vnew = Vposition + (int)dV;
+    LOGDBG("dx/dy: %g/%g; dU/dV: %g/%g", dX, dY, dU, dV);
+    // Compute PID outputs
+    double pidU_out = compute_pid(&pidU, dU, current_time);
+    double pidV_out = compute_pid(&pidV, dV, current_time);
+    int usteps = (int)pidU_out, vsteps = (int)pidV_out;
+    int Unew = Uposition + usteps, Vnew = Vposition + vsteps;
     if(Unew > theconf.maxUpos || Unew < theconf.minUpos ||
        Vnew > theconf.maxVpos || Vnew < theconf.minVpos){
+        // Reset integral to prevent windup
+        pidU.integral = 0;
+        pidV.integral = 0;
         // TODO: here we should signal that the limit reached and move by telescope
         LOGWARN("Correction failed, curpos: %d, %d, should move to %d, %d",
                 Uposition, Vposition, Unew, Vnew);
         return FALSE;
     }
-    LOGDBG("try2correct(): move from (%d, %d) to (%d, %d) (abs: %d, %d), delta (%.1f, %.1f)",
+    LOGDBG("try2correct(): move from (%d, %d) to (%d, %d), delta (%.1f, %.1f)",
-           Uposition, Vposition, Unew, Vnew, Uposition + (int)(dU/KCORR),
+           Uposition, Vposition, Unew, Vnew, dU, dV);
-           Vposition + (int)(dV/KCORR), dU, dV);
     int ret = TRUE;
-    int usteps = (int)dU, vsteps = (int)dV;
     if(usteps) ret = nth_motor_setter(CMD_RELPOS, Ustepper, usteps);
-    if(ret && vsteps) ret = nth_motor_setter(CMD_RELPOS, Vstepper, vsteps);
+    if(vsteps) ret &= nth_motor_setter(CMD_RELPOS, Vstepper, vsteps);
     if(!ret) LOGWARN("Canserver: cant run corrections");
     return ret;
 }
@@ -854,32 +909,31 @@ static char *stp_status(const char *messageid, char *buf, int buflen){
     return buf;
 }
 
-typedef struct{
-    const char *str;
-    STPstate state;
-} strstate;
 // commands from client to change status
-static strstate stringstatuses[] = {
+static const char* stringstatuses[STP_STATE_AMOUNT] = {
-    {"disconnect", STP_DISCONN},
+    [STP_DISCONN] = "disconnect",
-    {"relax", STP_RELAX},
+    [STP_RELAX] = "relax",
-    {"setup", STP_SETUP},
+    [STP_SETUP] = "setup",
-    {"middle", STP_GOTOTHEMIDDLE},
+    [STP_GOTOTHEMIDDLE] = "middle",
-    {"findtarget", STP_FINDTARGET},
+    [STP_FINDTARGET] = "findtarget",
-    {"fix", STP_FIX},
+    [STP_FIX] = "fix",
-    {NULL, 0}
+    [STP_UNDEFINED] = "undefined"
 };
 
 // try to set new status (global variable stepstatus)
 static char *set_stpstatus(const char *newstatus, char *buf, int buflen){
+    if(!buf) return NULL;
+    if(!newstatus){ // getter
+        snprintf(buf, buflen, "%s", stringstatuses[state]);
+        return buf;
+    }
     // FNAME();
-    strstate *s = stringstatuses;
     STPstate newstate = STP_UNDEFINED;
-    while(s->str){
+    for(int i = 0; i < STP_UNDEFINED; ++i){
-        if(strcasecmp(s->str, newstatus) == 0){
+        if(strcasecmp(stringstatuses[i], newstatus) == 0){
-            newstate = s->state;
+            newstate = (STPstate)i;
             break;
         }
-        ++s;
     }
     if(newstate != STP_UNDEFINED){
         if(stp_setstate(newstate)){
@@ -892,12 +946,10 @@ static char *set_stpstatus(const char *newstatus, char *buf, int buflen){
     }
     int L = snprintf(buf, buflen, "status '%s' undefined, allow: ", newstatus);
     char *ptr = buf;
-    s = stringstatuses;
+    for(int i = 0; i < STP_UNDEFINED && buflen > 2; ++i){
-    while(L > 0){
         buflen -= L;
         ptr += L;
-        L = snprintf(ptr, buflen, "'%s' ", s->str);
+        L = snprintf(ptr, buflen-2, "'%s' ", stringstatuses[i]);
-        if((++s)->str == NULL) break;
     }
     ptr[L-1] = '\n';
     return buf;
@@ -947,7 +999,7 @@ static void *stp_process_states(_U_ void *arg){
             first = TRUE;
         }
         // if we are here, all U/V moving is finished
-        switch(state){ // STProbo state machine
+        switch(state){ // steppers state machine
             case STP_DISCONN:
                 if(!stp_connect_server()){
                     WARNX("Can't reconnect");
@@ -974,7 +1026,7 @@ static void *stp_process_states(_U_ void *arg){
             case STP_FIX: // process corrections
                 if(coordsRdy){
                     coordsRdy = FALSE;
-                    DBG("GET AVERAGE -> correct\n");
+                    DBG("GOT AVERAGE -> correct\n");
                     double xtg = theconf.xtarget - theconf.xoff, ytg = theconf.ytarget - theconf.yoff;
                     double xdev = xtg - Xtarget, ydev = ytg - Ytarget;
                     double corr = sqrt(xdev*xdev + ydev*ydev);
@@ -1003,6 +1055,11 @@ static void *stp_process_states(_U_ void *arg){
 
 // change focus (global variable movefocus)
 static char *set_pfocus(const char *newstatus, char *buf, int buflen){
+    if(!buf) return NULL;
+    if(!newstatus){ // getter
+        snprintf(buf, buflen, "%d", Fposition);
+        return buf;
+    }
     int newval = atoi(newstatus);
     if(newval < theconf.minFpos || newval > theconf.maxFpos){
         snprintf(buf, buflen, FAIL);
@@ -1017,6 +1074,11 @@ static char *set_pfocus(const char *newstatus, char *buf, int buflen){
 }
 // move by U and V axis
 static char *Umove(const char *val, char *buf, int buflen){
+    if(!buf) return NULL;
+    if(!val){ // getter
+        snprintf(buf, buflen, "%d", Uposition);
+        return buf;
+    }
     int d = atoi(val);
     int Unfixed = Uposition + d;
     if(Unfixed > theconf.maxUpos || Unfixed < theconf.minUpos){
@@ -1030,6 +1092,11 @@ static char *Umove(const char *val, char *buf, int buflen){
     return buf;
 }
 static char *Vmove(const char *val, char *buf, int buflen){
+    if(!buf) return NULL;
+    if(!val){ // getter
+        snprintf(buf, buflen, "%d", Vposition);
+        return buf;
+    }
     int d = atoi(val);
     int Vnfixed = Vposition + d;
     if(Vnfixed > theconf.maxVpos || Vnfixed < theconf.minVpos){

LocCorr_new/steppers.h

@@ -21,6 +21,9 @@
 // set state to `disconnect` after this amount of errors in `moving_finished`
 #define MAX_ERR_CTR     (15)
 
+// max time interval from previous correction to clear integral/time (seconds)
+#define MAX_PID_TIME    (5.)
+
 // amount of ALL motors
 #define NMOTORS (8)