LocCorr - pre-alpha version of INASAN corrector

LocCorr/CANSERVER_pusirobo

@@ -0,0 +1,9 @@
+MULTIPLY steps & speed by microsteps!!!
+
+register U 0x581 stepper  -> OK\nU maxspeed=OK
+mesg U maxspeed 12800     -> OK\nU maxspeed=OK
+register V 0x582 stepper  -> OK\nV maxspeed=OK
+mesg V maxspeed 12800     -> OK\nV maxspeed=OK
+mesg U relmove 1600       -> OK\nU rotdir=OK\nU relsteps=OK
+mesg U setzero            -> OK\nU curpos=OK
+mesg V setzero            -> OK\nV curpos=OK

LocCorr/cmdlnopts.c

@@ -26,6 +26,8 @@
 #include <usefull_macros.h>
 
 #include "cmdlnopts.h"
+#include "config.h"
+#include "improc.h"
 
 /*
  * here are global parameters initialisation
@@ -33,23 +35,26 @@
 static int help;
 glob_pars *GP = NULL;
 
-// default PID filename:
-#define DEFAULT_PIDFILE "/tmp/loccorr.pid"
-
 //            DEFAULTS
 // default global parameters
 static glob_pars G = {
     .pidfile = DEFAULT_PIDFILE,
-    .throwpart = 0.5,
+    .configname = DEFAULT_CONFFILE,
-    .medradius = 1.,
+    .pusiservport = DEFAULT_PUSIPORT,
-    .ndilations = 2,
+    .throwpart = DEFAULT_THROWPART,
-    .nerosions = 2,
+//    .medradius = 1.,
-    .minarea = 5,
+    .ndilations = DEFAULT_DILATIONS,
-    .intensthres = 0.01,
+    .nerosions = DEFAULT_EROSIONS,
-    .maxexp = 500.,
+    .minarea = DEFAULT_MINAREA,
-    .minexp = 0.001,
+    .maxarea = DEFAULT_MAXAREA,
+    .intensthres = DEFAULT_INTENSTHRES,
+    .maxexp = EXPOS_MAX,
+    .minexp = EXPOS_MIN,
     .xtarget = -1.,
-    .ytarget = -1.
+    .ytarget = -1.,
+    .Naveraging = DEFAULT_NAVERAGE,
+    .ioport = DEFAULT_IOPORT,
+    .outputjpg = DEFAULT_OUTPJPEG,
 };
 
 /*
@@ -58,27 +63,34 @@ static glob_pars G = {
 */
 static myoption cmdlnopts[] = {
 // common options
-    {"help",    NO_ARGS,    NULL,     0,    arg_int,    APTR(&help),        _("show this help")},
+    {"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")},
     {"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)")},
     {"input",   NEED_ARG,   NULL,   'i',    arg_string, APTR(&G.inputname), _("file or directory name for monitoring (or grasshopper for capturing)")},
     {"blackp",  NEED_ARG,   NULL,   'b',    arg_double, APTR(&G.throwpart), _("fraction of black pixels to throw away when make histogram eq")},
-    {"radius",  NEED_ARG,   NULL,   'r',    arg_int,    APTR(&G.medradius), _("radius of median filter (r=1 -> 3x3, r=2 -> 5x5 etc.)")},
+//    {"radius",  NEED_ARG,   NULL,   'r',    arg_int,    APTR(&G.medradius), _("radius of median filter (r=1 -> 3x3, r=2 -> 5x5 etc.)")},
     {"equalize", NO_ARGS,   NULL,   'e',    arg_int,    APTR(&G.equalize),  _("make historam equalization of saved jpeg")},
-    {"ndilat",  NEED_ARG,   NULL,   'D',    arg_int,    APTR(&G.ndilations),_("amount of erosions after thresholding (default: 2)")},
+    {"ndilat",  NEED_ARG,   NULL,   'D',    arg_int,    APTR(&G.ndilations),_("amount of dilations after thresholding (default: 2)")},
-    {"neros",   NEED_ARG,   NULL,   'E',    arg_int,    APTR(&G.nerosions), _("amount of dilations after erosions (default: 2)")},
+    {"neros",   NEED_ARG,   NULL,   'E',    arg_int,    APTR(&G.nerosions), _("amount of erosions after dilations (default: 2)")},
-    {"minarea", NEED_ARG,   NULL,   'A',    arg_int,    APTR(&G.minarea),   _("minimal object pixels amount (default: 5)")},
+    {"minarea", NEED_ARG,   NULL,   'I',    arg_int,    APTR(&G.minarea),   _("minimal object pixels amount (default: 400)")},
+    {"maxarea", NEED_ARG,   NULL,   'A',    arg_int,    APTR(&G.maxarea),   _("maximal object pixels amount (default: 150000)")},
     {"intthres",NEED_ARG,   NULL,   'T',    arg_double, APTR(&G.intensthres),_("threshold by total object intensity when sorting = |I1-I2|/(I1+I2), default: 0.01")},
     {"xoff",    NEED_ARG,   NULL,   'x',    arg_int,    APTR(&G.xoff),      _("X offset at grabbed image")},
     {"yoff",    NEED_ARG,   NULL,   'y',    arg_int,    APTR(&G.yoff),      _("Y offset at grabbed image")},
-    {"width",   NEED_ARG,   NULL,   'w',    arg_int,    APTR(&G.width),     _("grabbed subimage width")},
+    {"width",   NEED_ARG,   NULL,   'W',    arg_int,    APTR(&G.width),     _("grabbed subimage width")},
-    {"height",  NEED_ARG,   NULL,   'h',    arg_int,    APTR(&G.height),    _("grabbed subimage height")},
+    {"height",  NEED_ARG,   NULL,   'H',    arg_int,    APTR(&G.height),    _("grabbed subimage height")},
-    {"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")},
     {"xtarget", NEED_ARG,   NULL,   'X',    arg_double, APTR(&G.xtarget),   _("target point X coordinate")},
     {"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)")},
+    {"proc",    NEED_ARG,   NULL,   'p',    arg_string, APTR(&G.processing),_("==\"" PUSIROBO_POSTPROC "\" to fix corrections with pusirobot drives")},
+    {"canport", NEED_ARG,   NULL,   'C',    arg_string, APTR(&G.pusiservport),_("port of local pusirobot CAN 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 "')")},
    end_option
 };
 

LocCorr/cmdlnopts.h

@@ -23,6 +23,24 @@
 #ifndef CMDLNOPTS_H__
 #define CMDLNOPTS_H__
 
+// default values
+#define DEFAULT_PIDFILE     "/tmp/loccorr.pid"
+#define DEFAULT_CONFFILE    "./loccorr.conf"
+#define DEFAULT_OUTPJPEG    "./outpWcrosses.jpg"
+#define DEFAULT_PUSIPORT    (4444)
+#define DEFAULT_IOPORT      (12345)
+#define DEFAULT_MAXAREA     (150000)
+#define DEFAULT_MINAREA     (400)
+#define DEFAULT_EROSIONS    (2)
+#define DEFAULT_DILATIONS   (2)
+#define DEFAULT_THROWPART   (0.5)
+#define DEFAULT_INTENSTHRES (0.01)
+#define DEFAULT_NAVERAGE    (5)
+#define DEFAULT_MAXUSTEPS   (16000)
+#define DEFAULT_MAXVSTEPS   (16000)
+#define DEFAULT_NEROSIONS   (3)
+#define DEFAULT_NDILATIONS  (3)
+
 /*
  * here are some typedef's for global data
  */
@@ -31,18 +49,25 @@ typedef struct{
     char *logfile;          // logging to this file
     char *inputname;        // input for monitor file or directory name
     char *logXYname;        // file to log XY coordinates of first point
-    double throwpart;       // fraction of black pixels to throw away when make histogram eq
+    char *configname;       // name of configuration file (default: ./loccorr.conf)
+    char *processing;       // =="pusirobo" to fix corrections with pusirobot drives
+    char *outputjpg;        // output jpeg name
+    int pusiservport;       // port of local pusirobot CAN server
     int equalize;           // make historam equalization of saved jpeg
-    int medradius;          // radius of median filter (r=1 -> 3x3, r=2 -> 5x5 etc.)
+//    int medradius;          // radius of median filter (r=1 -> 3x3, r=2 -> 5x5 etc.)
     int verb;               // logfile verbosity level
     int ndilations;         // amount of erosions (default: 2)
     int nerosions;          // amount of dilations (default: 2)
-    int minarea;            // minimal object pixels amount (default: 5)
+    int minarea;            // minimal object pixels amount (default: 400)
+    int maxarea;            // maximal object pixels amount (default: 150000)
+    int Naveraging;         // amount of images to average processing (min 2, max 25, default 5)
+    int xoff; int yoff;     // offset by X and Y axes
+    int width; int height;  // target width and height of image
+    int ioport;             // port for IO commands
+    double throwpart;       // fraction of black pixels to throw away when make histogram eq
     double intensthres;     // threshold by total object intensity when sorting = |I1-I2|/(I1+I2), default: 0.01
     double maxexp;          // max exposition time (ms)
     double minexp;          // min exposition time (ms)
-    int xoff; int yoff;     // offset by X and Y axes
-    int width; int height;  // target width and height of image
     double xtarget; double ytarget;// target point coordinates
 } glob_pars;
 

LocCorr/config.c

@@ -0,0 +1,389 @@
+/*
+ * This file is part of the loccorr project.
+ * Copyright 2021 Edward V. Emelianov <edward.emelianoff@gmail.com>.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <limits.h>
+#include <stdio.h>
+#include <string.h>
+#include <usefull_macros.h>
+
+#include "cmdlnopts.h"
+#include "config.h"
+
+static char *conffile = NULL; // configuration file name
+
+configuration theconf = {
+    .maxUsteps=DEFAULT_MAXUSTEPS,
+    .maxVsteps=DEFAULT_MAXVSTEPS,
+    .minarea=DEFAULT_MINAREA,
+    .maxarea=DEFAULT_MAXAREA,
+    .Nerosions=DEFAULT_NEROSIONS,
+    .Ndilations=DEFAULT_NDILATIONS,
+    .xoff=0,
+    .yoff=0,
+    .width=0,
+    .height=0,
+    .equalize=1,
+    .naverage=DEFAULT_NAVERAGE,
+    .stpserverport=DEFAULT_PUSIPORT,
+    .starssort=0,
+    .Kxu=0,
+    .Kyu=0,
+    .Kxv=0,
+    .Kyv=0,
+    .xtarget=-1,
+    .ytarget=-1,
+    .throwpart=DEFAULT_THROWPART,
+    .maxexp=EXPOS_MAX - DBL_EPSILON,
+    .minexp=EXPOS_MIN + DBL_EPSILON,
+    .intensthres=DEFAULT_INTENSTHRES
+};
+
+// {"", PAR_DOUBLE, (void*)&theconf., 0},
+static confparam parvals[] = {
+    {"maxarea", PAR_INT, (void*)&theconf.maxarea, 0, MINAREA-DBL_EPSILON, MAXAREA+DBL_EPSILON,
+     "maximal area (in square pixels) of recognized star image"},
+    {"minarea", PAR_INT, (void*)&theconf.minarea, 0, MINAREA-DBL_EPSILON, MAXAREA+DBL_EPSILON,
+     "minimal area (in square pixels) of recognized star image"},
+    {"ndilat", PAR_INT, (void*)&theconf.Ndilations, 0, 1.-DBL_EPSILON, MAX_NDILAT+DBL_EPSILON,
+     "amount of dilations on binarized image"},
+    {"neros", PAR_INT, (void*)&theconf.Nerosions, 0, 1.-DBL_EPSILON, MAX_NEROS+DBL_EPSILON,
+     "amount of erosions after dilations"},
+    {"xoffset", PAR_INT, (void*)&theconf.xoff, 0, -DBL_EPSILON, MAX_OFFSET+DBL_EPSILON,
+     "X offset of subimage"},
+    {"yoffset", PAR_INT, (void*)&theconf.yoff, 0, -DBL_EPSILON, MAX_OFFSET+DBL_EPSILON,
+     "Y offset of subimage"},
+    {"width", PAR_INT, (void*)&theconf.width, 0, -DBL_EPSILON, MAX_OFFSET+DBL_EPSILON,
+     "subimage width"},
+    {"height", PAR_INT, (void*)&theconf.height, 0, -DBL_EPSILON, MAX_OFFSET+DBL_EPSILON,
+     "subimage height"},
+    {"equalize", PAR_INT, (void*)&theconf.equalize, 0, -DBL_EPSILON, 1.+DBL_EPSILON,
+     "make histogram equalization"},
+    {"naverage", PAR_INT, (void*)&theconf.naverage, 0, 1-DBL_EPSILON, NAVER_MAX+DBL_EPSILON,
+     "calculate mean position by N images"},
+    {"umax", PAR_INT, (void*)&theconf.maxUsteps, 0, MINSTEPS-DBL_EPSILON, MAXSTEPS+DBL_EPSILON,
+     "maximal value of steps on U semi-axe"},
+    {"vmax", PAR_INT, (void*)&theconf.maxVsteps, 0, MINSTEPS-DBL_EPSILON, MAXSTEPS+DBL_EPSILON,
+     "maximal value of steps on V semi-axe"},
+    {"stpservport", PAR_INT, (void*)&theconf.stpserverport, 0, -DBL_EPSILON, 65536.,
+     "port number of steppers' server"},
+    {"Kxu", PAR_DOUBLE, (void*)&theconf.Kxu, 0, KUVMIN-DBL_EPSILON, KUVMAX+DBL_EPSILON,
+     "dU = Kxu*dX + Kyu*dY"},
+    {"Kyu", PAR_DOUBLE, (void*)&theconf.Kyu, 0, KUVMIN-DBL_EPSILON, KUVMAX+DBL_EPSILON,
+     "dU = Kxu*dX + Kyu*dY"},
+    {"Kxv", PAR_DOUBLE, (void*)&theconf.Kxv, 0, KUVMIN-DBL_EPSILON, KUVMAX+DBL_EPSILON,
+     "dV = Kxv*dX + Kyv*dY"},
+    {"Kyv", PAR_DOUBLE, (void*)&theconf.Kyv, 0, KUVMIN-DBL_EPSILON, KUVMAX+DBL_EPSILON,
+     "dV = Kxv*dX + Kyv*dY"},
+    {"xtarget", PAR_DOUBLE, (void*)&theconf.xtarget, 0, 1.-DBL_EPSILON, MAX_OFFSET+DBL_EPSILON,
+     "X coordinate of target position"},
+    {"ytarget", PAR_DOUBLE, (void*)&theconf.ytarget, 0, 1.-DBL_EPSILON, MAX_OFFSET+DBL_EPSILON,
+     "Y coordinate of target position"},
+    {"eqthrowpart", PAR_DOUBLE, (void*)&theconf.throwpart, 0, -DBL_EPSILON, MAX_THROWPART+DBL_EPSILON,
+     "a part of low intensity pixels to throw away when histogram equalized"},
+    {"minexp", PAR_DOUBLE, (void*)&theconf.minexp, 0, -DBL_EPSILON, EXPOS_MAX+DBL_EPSILON,
+     "minimal exposition time"},
+    {"maxexp", PAR_DOUBLE, (void*)&theconf.maxexp, 0, -DBL_EPSILON, EXPOS_MAX+DBL_EPSILON,
+     "maximal exposition time"},
+    {"intensthres", PAR_DOUBLE, (void*)&theconf.intensthres, 0, DBL_EPSILON, 1.+DBL_EPSILON,
+     "threshold by total object intensity when sorting = |I1-I2|/(I1+I2)"},
+    {"starssort", PAR_INT, (void*)&theconf.starssort, 0, -DBL_EPSILON, 1.+DBL_EPSILON,
+     "stars sorting algorithm: by distance from target (0) or by intensity (1)"},
+    {NULL,  0,  NULL, 0, 0., 0., NULL}
+};
+
+// return pointer to buff with size l filled with list of all commands (+help messages + low/high values)
+char *get_cmd_list(char *buff, int l){
+    if(!buff || l < 1) return NULL;
+    int L = l;
+    char *ptr = buff;
+    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+DBL_EPSILON, par->maxval-DBL_EPSILON);
+        if(s < 1) break;
+        L -= s; ptr += s;
+        ++par;
+    }
+    return buff;
+}
+
+static char *omitspaces(char *v){
+    if(!v) return NULL;
+    while(*v && (*v == ' ' || *v == '\t')) ++v;
+    char *ptr = strchr(v, ' ');
+    if(ptr) *ptr = 0;
+    ptr = strchr(v, '\t');
+    if(ptr) *ptr = 0;
+    return v;
+}
+
+// Read key/value from `pair` (key = value)
+// RETURNed value should be FREEd
+char *get_keyval(const char *pair, char value[128]){
+    char key[128];
+    char val[128];
+    if(!pair || strlen(pair) < 3) return strdup("#"); // 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 == 2){ // param = value
+        keyptr = omitspaces(key);
+        valptr = omitspaces(val);
+        sprintf(value, "%s", valptr);
+        return strdup(keyptr);
+    }
+    keyptr = omitspaces(key);
+    if(*keyptr == '#' || *keyptr == '%'){ // comment
+        *value = 0;
+        return strdup("#");
+    }
+    return NULL;
+}
+
+// Read key/value from file
+static char *read_key(FILE *file, char value[128]){
+    char *line = NULL;
+    size_t n = 0;
+    int got = getline(&line, &n, file);
+    if(got < 0){
+        FREE(line);
+        return NULL;
+    }
+    char *kv = get_keyval(line, value);
+    FREE(line);
+    return kv;
+}
+
+static int str2int(int *num, const char *str){
+    long res;
+    char *endptr;
+    if(!str) return 0;
+    res = strtol(str, &endptr, 0);
+    if(endptr == str || *str == '\0' || *endptr != '\0'){
+        return FALSE;
+    }
+    if(res > INT_MAX || res < INT_MIN) return FALSE;
+    if(num) *num = (int)res;
+    return TRUE;
+}
+
+/**
+ * @brief chk_keyval - check key for presence in theconf and calculate its value
+ * @param key (i) - keyword
+ * @param val (i) - value
+ * @param result - result calculated from `val`
+ * @return pointer to confparam if found & checked
+ */
+confparam *chk_keyval(const char *key, const char *val, key_value *result){
+    if(!key || !val || !result) return NULL;
+    confparam *par = parvals;
+    while(par->name){
+        if(strcmp(key, par->name) == 0){
+            DBG("key='%s', par->name='%s'", key, par->name);
+            result->type = par->type;
+            switch(par->type){
+                case PAR_INT:
+                    DBG("INTEGER");
+                    if(!str2int(&result->val.intval, val)){
+                        WARNX("Wrong integer value '%s' of parameter '%s'", val, key);
+                        return NULL;
+                    }
+                    if(result->val.intval > par->minval && result->val.intval < par->maxval)
+                        return par;
+                    else WARNX("Value (%d) of parameter %s out of range %g..%g",
+                               result->val.intval, par->name, par->minval, par->maxval);
+                break;
+                case PAR_DOUBLE:
+                    DBG("DOUBLE");
+                    if(!str2double(&result->val.dblval, val)){
+                        WARNX("Wrong double value '%s' of parameter '%s'", val, key);
+                        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)
+                        return par;
+                    else WARNX("Value (%g) of parameter %s out of range %g..%g",
+                               result->val.dblval, par->name, par->minval, par->maxval);
+                break;
+            }
+            return NULL;
+        }
+        ++par;
+    }
+    return NULL;
+}
+
+/**
+ * @brief chkconfig - check configuration file and init variables
+ * @param confname - name of file
+ * @return FALSE if configuration file wrong or absent
+ */
+int chkconfig(const char *confname){
+    DBG("Config name: %s", confname);
+    FREE(conffile);
+    conffile = strdup(confname);
+    FILE *f = fopen(confname, "r");
+    int ret = TRUE;
+    if(!f){
+        WARN("Can't open %s", confname);
+        return FALSE;
+    }
+    char *key, val[128];
+    confparam *par = parvals;
+    while(par->name){
+        par->got = 0;
+        ++par;
+    }
+    while((key = read_key(f, val))){
+        if(*key == '#'){
+            FREE(key);
+            continue; // comment
+        }
+        //DBG("key: %s", key);
+        key_value kv;
+        par = chk_keyval(key, val, &kv);
+        if(!par){
+            WARNX("Parameter '%s' is wrong or out of range", key);
+            FREE(key);
+            continue;
+        }
+        switch(par->type){
+            case PAR_INT:
+                *((int*)par->ptr) = kv.val.intval;
+            break;
+            case PAR_DOUBLE:
+                *((double*)par->ptr) = kv.val.dblval;
+            break;
+        }
+        ++par->got;
+        FREE(key);
+    }
+    fclose(f);
+    int found = 0;
+    par = parvals;
+    while(par->name){
+        DBG("parvals[]={%s, %d, %d(%g), %d}", par->name, par->type, *((int*)par->ptr), *((double*)par->ptr), par->got);
+        int k = par->got;
+        if(!k){
+            ++par;
+            continue;
+        }
+        if(k > 1){
+            WARNX("parameter '%s' meets %d times", par->name, k);
+            ret = FALSE;
+        }
+        ++found; ++par;
+    }
+#if 0
+        int Nchecked = 0;
+        if(theconf.maxUsteps >= MINSTEPS && theconf.maxUsteps <= MAXSTEPS) ++Nchecked;
+        if(theconf.maxVsteps >= MINSTEPS && theconf.maxVsteps <= MAXSTEPS) ++Nchecked;
+        if(theconf.cosXU >= -1. && theconf.cosXU <= 1.) ++Nchecked;
+        if(theconf.sinXU >= -1. && theconf.sinXU <= 1.) ++Nchecked;
+        if(theconf.cosXV >= -1. && theconf.cosXV <= 1.) ++Nchecked;
+        if(theconf.sinXV >= -1. && theconf.sinXV <= 1.) ++Nchecked;
+        if(theconf.KU >= COEFMIN && theconf.KU <= COEFMAX) ++Nchecked;
+        if(theconf.KV >= COEFMIN && theconf.KV <= COEFMAX) ++Nchecked;
+        if(theconf.maxarea <= MAXAREA && theconf.maxarea >= MINAREA) ++Nchecked;
+        if(theconf.minarea <= MAXAREA && theconf.minarea >= MINAREA) ++Nchecked;
+        if(theconf.Nerosions > 0 && theconf.Nerosions <= MAX_NEROS) ++Nchecked;
+        if(theconf.Ndilations > 0 && theconf.Ndilations <= MAX_NDILAT) ++Nchecked;
+        if(theconf.xtarget > 1.) ++Nchecked;
+        if(theconf.ytarget > 1.) ++Nchecked;
+        if(theconf.throwpart > -DBL_EPSILON && theconf.throwpart < MAX_THROWPART+DBL_EPSILON) ++Nchecked;
+        if(theconf.xoff > 0 && theconf.xoff < MAX_OFFSET) ++Nchecked;
+        if(theconf.yoff > 0 && theconf.yoff < MAX_OFFSET) ++Nchecked;
+        if(theconf.width > 0 && theconf.width < MAX_OFFSET) ++Nchecked;
+        if(theconf.height > 0 && theconf.height < MAX_OFFSET) ++Nchecked;
+        if(theconf.minexp > 0. && theconf.minexp < EXPOS_MAX) ++Nchecked;
+        if(theconf.maxexp > theconf.minexp) ++Nchecked;
+        if(theconf.equalize > -1) ++Nchecked;
+        if(theconf.intensthres > DBL_EPSILON && theconf.intensthres < 1.) ++Nchecked;
+        if(theconf.naverage > 1 && theconf.naverage <= NAVER_MAX) ++Nchecked;
+        if(theconf.stpserverport > 0 && theconf.stpserverport < 65536) ++Nchecked;
+#endif
+    DBG("chkconfig(): found %d", found);
+    return ret;
+}
+
+/**
+ * @brief saveconf    -   try to save configuration into file
+ * @param confname - config file name
+ * @return FALSE if failed
+ */
+int saveconf(const char *confname){
+    if(!confname){
+        if(!conffile){
+            WARNX("no conffile given");
+            return FALSE;
+        }
+        confname = conffile;
+    }
+    FILE *f = fopen(confname, "w");
+    if(!f){
+        WARN("Can't open %s", confname);
+        LOGERR("Can't open %s to store configuration", confname);
+        return FALSE;
+    }
+    confparam *par = parvals;
+    while(par->name){
+        par->got = 1;
+        switch(par->type){
+            case PAR_INT:
+                fprintf(f, "%s = %d\n", par->name, *((int*)par->ptr));
+                DBG("%s = %d", par->name, *((int*)par->ptr));
+            break;
+            case PAR_DOUBLE:
+                fprintf(f, "%s = %.3f\n", par->name, *((double*)par->ptr));
+                DBG("%s = %.3f", par->name, *((double*)par->ptr));
+            break;
+        }
+        ++par;
+    }
+    DBG("%s saved", confname);
+    LOGDBG("Configuration file '%s' saved", confname);
+    fclose(f);
+    return TRUE;
+}
+
+// return buffer filled with current configuration
+char *listconf(char *buf, int buflen){
+    int L;
+    char *ptr = buf;
+    confparam *par = parvals;
+    while(par->name && buflen > 0){
+        switch(par->type){
+            case PAR_INT:
+                L = snprintf(ptr, buflen, "%s=%d\n", par->name, *((int*)par->ptr));
+            break;
+            case PAR_DOUBLE:
+                L = snprintf(ptr, buflen, "%s=%.3f\n", par->name, *((double*)par->ptr));
+            break;
+            default:
+                L = 0;
+        }
+        ++par;
+        if(L > -1){
+            buflen -= L; ptr += L;
+        }else{
+            buf[buflen-1] = 0; break;
+        }
+    }
+    return buf;
+}

LocCorr/config.h

@@ -0,0 +1,104 @@
+/*
+ * This file is part of the loccorr project.
+ * Copyright 2021 Edward V. Emelianov <edward.emelianoff@gmail.com>.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#pragma once
+#ifndef CONFIG_H__
+#define CONFIG_H__
+
+// default configuration borders
+// min/max total steps range
+#define MINSTEPS        (100)
+#define MAXSTEPS        (50000)
+// steps per pixel
+#define COEFMIN         (0.1)
+#define COEFMAX         (10000)
+// area
+#define MINAREA         (4)
+#define MAXAREA         (250000)
+#define MAX_NDILAT      (100)
+#define MAX_NEROS       (100)
+#define MAX_THROWPART   (0.9)
+#define MAX_OFFSET      (10000)
+#define EXPOS_MIN       (0.001)
+#define EXPOS_MAX       (500.)
+#define NAVER_MAX       (50)
+// coefficients to convert dx,dy to du,dv
+#define KUVMIN           (-5000.)
+#define KUVMAX           (5000.)
+// default coefficient for corrections (move to Kdu, Kdv instead of du, dv)
+#define KCORR           (0.9)
+
+typedef struct{
+    int maxUsteps;      // max amount of steps by both axes
+    int maxVsteps;
+    int minarea;        // min/max area of star image
+    int maxarea;
+    int Nerosions;      // amount of erosions/dilations
+    int Ndilations;
+    int xoff;           // subimage offset
+    int yoff;
+    int width;          // subimage size
+    int height;
+    int equalize;       // !=0 to equalize output image histogram
+    int naverage;       // amount of images for average calculation (>1)
+    int stpserverport;  // steppers' server port
+    int starssort;      // stars sorting algorithm: by distance from target (0) or by intensity (1)
+    // dU = Kxu*dX + Kyu*dY; dV = Kxv*dX + Kyv*dY
+    double Kxu; double Kyu;
+    double Kxv; double Kyv;
+    double xtarget;     // target (center) values
+    double ytarget;
+    double throwpart;   // part of values to throw avay @ histogram equalisation
+    double maxexp;      // minimal and maximal exposition (in ms)
+    double minexp;
+    double intensthres; // threshold for stars intensity comparison: fabs(Ia-Ib)/(Ia+Ib) > thres -> stars differs
+} configuration;
+
+typedef enum{
+    PAR_INT,
+    PAR_DOUBLE
+} partype;
+
+typedef struct{
+    const char *name;   // parameter name
+    partype type;       // type of parameter's value
+    void *ptr;          // pointer to value in `theconf`
+    int got;            // counter of parameter in config file
+    double minval;      // min/max values
+    double maxval;
+    const char *help;   // help message
+} confparam;
+
+typedef union{
+    double dblval;
+    int intval;
+} dblint;
+
+typedef struct{
+    dblint val;
+    partype type;
+} key_value;
+
+extern configuration theconf;
+char *get_cmd_list(char *buff, int l);
+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);
+char *listconf(char *buf, int buflen);
+
+#endif // CONFIG_H__

LocCorr/grasshopper.c

@@ -24,9 +24,11 @@
 #include <usefull_macros.h>
 
 #include "cmdlnopts.h"
+#include "config.h"
 #include "fits.h"
 #include "grasshopper.h"
 #include "imagefile.h"
+#include "improc.h"
 
 static fc2Context context;
 static fc2PGRGuid guid;
@@ -121,7 +123,8 @@ int propOnOff(fc2PropertyType t, BOOL onOff){
 #define setgain(g)          setfloat(FC2_GAIN, g)
 
 static int connected = 0;
-static void disconnect(){
+void disconnectGrasshopper(){
+    if(!connected) return;
     connected = 0;
     fc2DestroyContext(context);
 }
@@ -138,10 +141,10 @@ static int changeformat(){
     DBG("packsz=%u, perc=%f, off=%d/%d, HW=%d/%d", packSz, perc, f7.offsetX, f7.offsetY, f7.height, f7.width);
     */
     f7.mode = FC2_MODE_0;
-    f7.offsetX = (GP->xoff < (int)f7i.maxWidth && GP->xoff > -1) ? GP->xoff : 0;
+    f7.offsetX = (theconf.xoff < (int)f7i.maxWidth && theconf.xoff > -1) ? theconf.xoff : 0;
-    f7.offsetY = (GP->yoff < (int)f7i.maxHeight && GP->yoff > -1) ? GP->yoff : 0;
+    f7.offsetY = (theconf.yoff < (int)f7i.maxHeight && theconf.yoff > -1) ? theconf.yoff : 0;
-    f7.width = (f7.offsetX+GP->width <= f7i.maxWidth && GP->width > 1) ? (unsigned int)GP->width : f7i.maxWidth - f7.offsetX;
+    f7.width = (f7.offsetX+theconf.width <= f7i.maxWidth && theconf.width > 1) ? (unsigned int)theconf.width : f7i.maxWidth - f7.offsetX;
-    f7.height = (f7.offsetY+GP->height <= f7i.maxHeight && GP->height > 1) ? (unsigned int)GP->height : f7i.maxHeight - f7.offsetY;
+    f7.height = (f7.offsetY+theconf.height <= f7i.maxHeight && theconf.height > 1) ? (unsigned int)theconf.height : f7i.maxHeight - f7.offsetY;
     DBG("offx=%d, offy=%d, w=%d, h=%d ", f7.offsetX, f7.offsetY, f7.width, f7.height);
     f7.pixelFormat = FC2_PIXEL_FORMAT_MONO8;
     fc2Format7PacketInfo f7p;
@@ -162,7 +165,7 @@ static int connect(){
     FC2FN(fc2GetNumOfCameras, &numCameras);
     if(numCameras == 0){
         WARNX("No cameras detected!");
-        disconnect();
+        disconnectGrasshopper();
         return 0;
     }
     DBG("Found %d camera[s]", numCameras);
@@ -213,7 +216,7 @@ static int GrabImage(fc2Image *convertedImage){
 static void calcexpgain(float newexp){
     DBG("recalculate exposition: oldexp=%g, oldgain=%g, newexp=%g", exptime, gain, newexp);
     if(newexp < exptime){ // first we should make gain lower
-        if(10.*newexp < GP->maxexp){
+        if(10.*newexp < theconf.maxexp){
             if(gain > 10.){
                 gain = 10.;
                 newexp *= 10.;
@@ -223,15 +226,15 @@ static void calcexpgain(float newexp){
             }
         }
     }else{ // if new exposition too large, increase gain
-        if(newexp > GP->maxexp){
+        if(newexp > theconf.maxexp){
             if(gain < 19.){
                 gain += 10.;
                 newexp /= 10.;
             }
         }
     }
-    if(newexp < GP->minexp) newexp = GP->minexp;
+    if(newexp < theconf.minexp) newexp = theconf.minexp;
-    else if(newexp > GP->maxexp) newexp = GP->maxexp;
+    else if(newexp > theconf.maxexp) newexp = theconf.maxexp;
     exptime = newexp;
     DBG("New values: exp=%g, gain=%g", exptime, gain);
 }
@@ -276,6 +279,7 @@ int capture_grasshopper(void (*process)(Image*)){
     }
     Image *oIma = NULL;
     while(1){
+        if(stopwork) return 1;
         if(!connect()){ // wait until camera be powered on
             DBG("Disconnected");
             sleep(1);
@@ -287,7 +291,7 @@ int capture_grasshopper(void (*process)(Image*)){
                 oldexptime = exptime;
             }else{
                 WARNX("Can't change exposition time to %gms", exptime);
-                disconnect();
+                disconnectGrasshopper();
                 continue;
             }
         }
@@ -297,13 +301,13 @@ int capture_grasshopper(void (*process)(Image*)){
                 oldgain = gain;
             }else{
                 WARNX("Can't change gain to %g", gain);
-                disconnect();
+                disconnectGrasshopper();
                 continue;
             }
         }
         if(!GrabImage(&convertedImage)){
             WARNX("Can't grab image");
-            disconnect();
+            disconnectGrasshopper();
             continue;
         }
         if(!process) continue;

LocCorr/grasshopper.h

@@ -23,6 +23,7 @@
 
 #define GRASSHOPPER_CAPT_NAME   "grasshopper"
 
+void disconnectGrasshopper();
 int capture_grasshopper(void (*process)(Image *));
 
 #endif // GRASSHOPPER_H__

LocCorr/imagefile.c

@@ -28,6 +28,7 @@
 #include <stb/stb_image_write.h>
 
 #include "cmdlnopts.h"
+#include "config.h"
 #include "draw.h"
 #include "grasshopper.h"
 #include "imagefile.h"
@@ -333,11 +334,21 @@ int Image_write_jpg(const Image *I, const char *name, int eq){
     if(!I || !I->data) return 0;
     uint8_t *outp = NULL;
     if(eq)
-        outp = equalize(I, 1, GP->throwpart);
+        outp = equalize(I, 1, theconf.throwpart);
     else
         outp = linear(I, 1);
     DBG("Try to write %s", name);
-    int r = stbi_write_jpg(name, I->width, I->height, 1, outp, 95);
+    char *tmpnm = MALLOC(char, strlen(name) + 5);
+    sprintf(tmpnm, "%s-tmp", name);
+//    char *tmpnm = tmpnam_r(buf);
+    int r = stbi_write_jpg(tmpnm, I->width, I->height, 1, outp, 95);
+    if(r){
+        if(rename(tmpnm, name)){
+            WARN("rename()");
+            r = 0;
+        }
+    }
+    FREE(tmpnm);
     FREE(outp);
     return r;
 }

LocCorr/improc.c

@@ -25,22 +25,28 @@
 
 #include "binmorph.h"
 #include "cmdlnopts.h"
+#include "config.h"
 #include "draw.h"
 #include "grasshopper.h"
 #include "fits.h"
 #include "improc.h"
 #include "inotify.h"
 #include "median.h"
-
+#include "pusirobo.h"
-// how many frames will be averaged to count image deviation
-#define AVERAGING_ARRAY_SIZE        (5)
-// tolerance of deviations by X and Y axis
-#define XY_TOLERANCE                (1.)
 
 static FILE *fXYlog = NULL;
 
-static double Xtarget = -1., Ytarget = -1.; // target coordinates
 static double tstart = 0.; // time of logging start
+int stopwork = 0;
+
+// function to process calculated corrections
+static void (*proc_corr)(double, double, int) = NULL;
+// function to get stepper server status
+char *(*stepstatus)(char *buf, int buflen) = NULL;
+// set new status
+char *(*setstepstatus)(const char *newstatus, char *buf, int buflen) = NULL;
+// move focus
+char *(*movefocus)(const char *newstatus, char *buf, int buflen) = NULL;
 
 typedef struct{
     uint32_t area;      // object area in pixels
@@ -52,19 +58,20 @@ typedef struct{
     double ysigma;
 } object;
 
+typedef enum{
+    PROCESS_NONE,
+    PROCESS_PUSIROBO
+} postproc_type;
+
+static postproc_type postprocess = PROCESS_NONE;
+
 static bool save_fits(Image *I, const char *name){
-    uint8_t *outp = NULL;
-    if(GP->equalize)
-        outp = equalize(I, 1, GP->throwpart);
-    else
-        outp = linear(I, 1);
     char fname[PATH_MAX];
-    char *flname = strdup(name);
+    snprintf(fname, PATH_MAX, name);
-    char *pt = strrchr(flname, '.');
+    char *pt = strrchr(fname, '.');
     if(pt) *pt = 0;
-    snprintf(fname, PATH_MAX, "%s.jpg", flname);
+    strncat(fname, ".jpg", PATH_MAX);
-    stbi_write_jpg(fname, I->width, I->height, 1, outp, 95);
+    Image_write_jpg(I, fname, theconf.equalize);
-    FREE(outp);
     unlink(name);
     return FITS_write(name, I);
 }
@@ -77,22 +84,37 @@ static void savebin(uint8_t *b, int W, int H, const char *name){
     }
 }
 
-// function for Qsort
+// functions for Qsort
-static int compObjs(const void *a, const void *b){
+static int compIntens(const void *a, const void *b){ // compare by intensity
     const object *oa = (const object*)a;
     const object *ob = (const object*)b;
     double idiff = (oa->Isum - ob->Isum)/(oa->Isum + ob->Isum);
-    if(fabs(idiff) > GP->intensthres) return (idiff > 0) ? -1:1;
+    if(fabs(idiff) > theconf.intensthres) return (idiff > 0) ? -1:1;
     double r2a = oa->xc * oa->xc + oa->yc * oa->yc;
     double r2b = ob->xc * ob->xc + ob->yc * ob->yc;
     return (r2a < r2b) ? -1 : 1;
 }
+static int compDist(const void *a, const void *b){ // compare by distanse from target
+    const object *oa = (const object*)a;
+    const object *ob = (const object*)b;
+    double  xa = oa->xc - theconf.xtarget, xb = ob->xc - theconf.xtarget,
+            ya = oa->yc - theconf.ytarget, yb = ob->yc - theconf.ytarget;
+    double r2a = xa*xa + ya*ya;
+    double r2b = xb*xb + yb*yb;
+    return (r2a < r2b) ? -1 : 1;
+}
+
+static void XYnewline(){
+    if(!fXYlog) return;
+    fprintf(fXYlog, "\n");
+    fflush(fXYlog);
+}
 
 static void getDeviation(object *curobj){
-    if(Xtarget < 0. || Ytarget < 0.){ // init from user parameters
+    int averflag = 0;
-        Xtarget = GP->xtarget; Ytarget = GP->ytarget;
+    static double Xc[MAX_AVERAGING_ARRAY_SIZE], Yc[MAX_AVERAGING_ARRAY_SIZE];
-    }
+    double xx = curobj->xc, yy = curobj->yc, xsum2 = 0., ysum2 = 0.;
-    static double Xc[AVERAGING_ARRAY_SIZE], Yc[AVERAGING_ARRAY_SIZE];
+    double Sx = 0., Sy = 0.;
     static int counter = 0;
     Xc[counter] = curobj->xc; Yc[counter] = curobj->yc;
     if(fXYlog){ // make log record
@@ -100,31 +122,32 @@ static void getDeviation(object *curobj){
                 dtime() - tstart, curobj->xc, curobj->yc,
                 curobj->xsigma, curobj->ysigma, curobj->WdivH);
     }
-    if(++counter != AVERAGING_ARRAY_SIZE){
+    DBG("counter = %d", counter);
-        if(fXYlog) fprintf(fXYlog, "\n");
+    if(++counter != theconf.naverage){
-        return;
+        goto process_corrections;
     }
     // it's time to calculate average deviations
-    counter = 0;
+    counter = 0; xx = 0.; yy = 0.;
-    double xdev = 0., ydev = 0., xsum2 = 0., ysum2 = 0.;
+    for(int i = 0; i < theconf.naverage; ++i){
-    for(int i = 0; i < AVERAGING_ARRAY_SIZE; ++i){
+        double x = Xc[i], y = Yc[i];
-        double dx = Xc[i] - Xtarget, dy = Yc[i] - Ytarget;
+        xx += x; yy += y;
-        xdev += dx; ydev += dy;
+        xsum2 += x*x; ysum2 += y*y;
-        xsum2 += dx*dx; ysum2 += dy*dy;
     }
-    xdev /= AVERAGING_ARRAY_SIZE; ydev /= AVERAGING_ARRAY_SIZE;
+    xx /= theconf.naverage; yy /= theconf.naverage;
-    double Sx = sqrt(xsum2/AVERAGING_ARRAY_SIZE - xdev*xdev);
+    Sx = sqrt(xsum2/theconf.naverage - xx*xx);
-    double Sy = sqrt(ysum2/AVERAGING_ARRAY_SIZE - ydev*ydev);
+    Sy = sqrt(ysum2/theconf.naverage - yy*yy);
-    DBG("xtag=%g, ytag=%g", Xtarget, Ytarget);
+    green("\n\n\n Average centroid: X=%g (+-%g), Y=%g (+-%g)\n", xx, Sx, yy, Sy);
-    green("\n\n\n Deviations: dX=%g (+-%g), dY=%g (+-%g)\n", xdev, Sx, ydev, Sy);
+    LOGDBG("getDeviation(): Average centroid: X=%g (+-%g), Y=%g (+-%g)", xx, Sx, yy, Sy);
-    if(Xtarget < 0. || Ytarget < 0. || fabs(xdev) < XY_TOLERANCE || fabs(ydev) < XY_TOLERANCE){
+    averflag = 1;
-        DBG("Target coordinates not defined or correction too small");
+    if(fXYlog) fprintf(fXYlog, "%.1f\t%.1f\t%.1f\t%.1f", xx, yy, Sx, Sy);
-        if(fXYlog) fprintf(fXYlog, "\n");
+process_corrections:
-        return;
+    if(proc_corr){
-    }
+        if(Sx > 1. || Sy > 1.){
-    if(fXYlog){
+            LOGDBG("Bad value - not process"); // don't run processing for bad data
-        fprintf(fXYlog, "%.1f\t%.1f\t%.1f\t%.1f\n", xdev, ydev, Sx, Sy);
+        }else
+            proc_corr(xx, yy, averflag);
     }
+    XYnewline();
 }
 
 void process_file(Image *I){
@@ -135,7 +158,6 @@ void process_file(Image *I){
 #define DELTA(x)
 #endif
     // I - original image
-    // M - median filtering
     // mean - local mean
     // std  - local STD
     /**** read original image ****/
@@ -148,15 +170,6 @@ void process_file(Image *I){
     if(!I->dtype) I->dtype = FLOAT_IMG;
     save_fits(I, "fitsout.fits");
     DELTA("Save original");
-/*
-    uint8_t *outp = NULL;
-    if(GP->equalize)
-        outp = equalize(I, 1, GP->throwpart);
-    else
-        outp = linear(I, 1);
-    stbi_write_jpg("jpegout.jpg", I->width, I->height, 3, outp, 95);
-    FREE(outp);
-*/
     Imtype bk;
     if(calc_background(I, &bk)){
         DBG("backgr = %g", bk);
@@ -166,12 +179,12 @@ void process_file(Image *I){
         if(ibin){
             savebin(ibin, W, H, "binary.fits");
             DELTA("save binary.fits");
-            uint8_t *er = erosionN(ibin, W, H, GP->nerosions);
+            uint8_t *er = erosionN(ibin, W, H, theconf.Nerosions);
             FREE(ibin);
             DELTA("Erosion");
             savebin(er, W, H, "erosion.fits");
             DELTA("Save erosion");
-            uint8_t *opn = dilationN(er, W, H, GP->ndilations);
+            uint8_t *opn = dilationN(er, W, H, theconf.Ndilations);
             FREE(er);
             DELTA("Opening");
             savebin(opn, W, H, "opening.fits");
@@ -187,8 +200,8 @@ void process_file(Image *I){
                     double wh = ((double)b->xmax - b->xmin)/(b->ymax - b->ymin);
                     //DBG("Obj# %zd: wh=%g, area=%d", i, wh, b->area);
                     // TODO: change magick numbers to parameters
-                    if(wh < 0.2 || wh > 5.) continue;
+                    if(wh < MINWH || wh > MAXWH) continue;
-                    if((int)b->area < GP->minarea) continue;
+                    if((int)b->area < theconf.minarea || (int)b->area > theconf.maxarea) continue;
                     double xc = 0., yc = 0.;
                     double x2c = 0., y2c = 0., Isum = 0.;
                     for(size_t y = b->ymin; y <= b->ymax; ++y){
@@ -218,7 +231,12 @@ void process_file(Image *I){
                 }
                 DELTA("Labeling");
                 printf("T%zd, N=%d\n", time(NULL), objctr);
-                qsort(Objects, objctr, sizeof(object), compObjs);
+                if(objctr > 1){
+                    if(theconf.starssort)
+                        qsort(Objects, objctr, sizeof(object), compIntens);
+                    else
+                        qsort(Objects, objctr, sizeof(object), compDist);
+                }
                 object *o = Objects;
                 green("%6s\t%6s\t%6s\t%6s\t%6s\t%6s\t%6s\t%6s\n",
                       "N", "Area", "Mv", "W/H", "Xc", "Yc", "Sx", "Sy");
@@ -228,20 +246,23 @@ void process_file(Image *I){
                            i, o->area, 20.-1.0857*log(o->Isum), o->WdivH, o->xc, o->yc, o->xsigma, o->ysigma);
                 }
                 getDeviation(Objects);
-                if(objctr){ // draw crosses @ objects' centers
+                { // prepare image
                     uint8_t *outp = NULL;
-                    if(GP->equalize)
+                    if(theconf.equalize)
-                        outp = equalize(I, 3, GP->throwpart);
+                        outp = equalize(I, 3, theconf.throwpart);
                     else
                         outp = linear(I, 3);
-                    Pattern *cross = Pattern_cross(33, 33);
+                    if(objctr){ // draw crosses @ objects' centers
+                        static Pattern *cross = NULL;
+                        if(!cross) cross = Pattern_cross(33, 33);
                         int H = I->height;
                         Img3 i3 = {.data = outp, .w = I->width, .h = H};
                         Pattern_draw3(&i3, cross, Objects[0].xc, H-Objects[0].yc, C_G);
                         for(int i = 1; i < objctr; ++i)
                             Pattern_draw3(&i3, cross, Objects[i].xc, H-Objects[i].yc, C_R);
-                    Pattern_free(&cross);
+                        // Pattern_free(&cross); don't free - static variable!
-                    stbi_write_jpg("outpWcrosses.jpg", I->width, I->height, 3, outp, 95);
+                    }
+                    stbi_write_jpg(GP->outputjpg, I->width, I->height, 3, outp, 95);
                     FREE(outp);
                 }
                 FREE(cc);
@@ -273,6 +294,7 @@ void process_file(Image *I){
 }
 
 int process_input(InputType tp, char *name){
+    DBG("process_input(%d, %s)", tp, name);
     if(tp == T_DIRECTORY) return watch_directory(name, process_file);
     else if(tp == T_CAPT_GRASSHOPPER) return capture_grasshopper(process_file);
     return watch_file(name, process_file);
@@ -293,7 +315,8 @@ void openXYlog(const char *name){
     }
     time_t t = time(NULL);
     fprintf(fXYlog, "# Start at: %s", ctime(&t));
-    fprintf(fXYlog, "# time,s\tXc\tYc\t\tSx\tSy\tW/H\tcorrX\tcorrY\tSXcorr\tSYcorr\n");
+    fprintf(fXYlog, "# time Xc\tYc\t\tSx\tSy\tW/H\taverX\taverY\tSX\tSY\n");
+    fflush(fXYlog);
     tstart = dtime();
 }
 void closeXYlog(){
@@ -301,3 +324,27 @@ void closeXYlog(){
     fclose(fXYlog);
     fXYlog = NULL;
 }
+
+/**
+ * @brief setpostprocess - set postprocessing name (what to do with deviations data)
+ * @param name - "pusirobo" for pusirobot drives
+ */
+void setpostprocess(const char *name){
+    if(!name) return;
+    if(strncasecmp(name, PUSIROBO_POSTPROC, sizeof(PUSIROBO_POSTPROC) - 1) == 0){
+        postprocess = PROCESS_PUSIROBO;
+        DBG("Postprocess: pusirobot drives");
+        LOGMSG("Postprocess: pusirobot drives");
+        if(!pusi_connect()){
+            WARNX("Pusiserver unavailable, will check later");
+            LOGWARN("Pusiserver unavailable, will check later");
+        }
+        proc_corr = pusi_process_corrections;
+        stepstatus = pusi_status;
+        setstepstatus = set_pusistatus;
+        movefocus = set_pfocus;
+    }else{
+        WARNX("Unknown postprocess \"%s\"", name);
+        LOGERR("Unknown postprocess \"%s\"", name);
+    }
+}

LocCorr/improc.h

@@ -21,9 +21,26 @@
 
 #include "imagefile.h"
 
+// tolerance of deviations by X and Y axis
+#define XY_TOLERANCE                (1.)
+// roundness parameter
+#define MINWH                       (0.2)
+#define MAXWH                       (5.)
+
+#define PUSIROBO_POSTPROC   "pusirobo"
+// how many frames will be averaged to count image deviation
+#define MAX_AVERAGING_ARRAY_SIZE        (25)
+
+extern int stopwork;
+extern double Xtarget, Ytarget;
+
 void process_file(Image *I);
 int  process_input(InputType tp, char *name);
 void openXYlog(const char *name);
 void closeXYlog();
+void setpostprocess(const char *name);
+extern char *(*stepstatus)(char *buf, int buflen);
+extern char *(*setstepstatus)(const char *newstatus, char *buf, int buflen);
+extern char *(*movefocus)(const char *newstatus, char *buf, int buflen);
 
 #endif // IMPROC_H__

LocCorr/main.c

@@ -16,26 +16,41 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+#include <math.h>
 #include <signal.h>         // signal
 #include <string.h>         // strdup
 #include <usefull_macros.h>
 
 #include "cmdlnopts.h"
+#include "config.h"
+#include "grasshopper.h"
 #include "improc.h"
+#include "pusirobo.h"
+#include "socket.h"
 
 /**
  * We REDEFINE the default WEAK function of signal processing
  */
 void signals(int sig){
-    DBG("exit %d", sig);
     if(sig){
         signal(sig, SIG_IGN);
         DBG("Get signal %d, quit.\n", sig);
     }
-    closeXYlog();
+    DBG("exit %d", sig);
-    if(GP && GP->pidfile) // remove unnesessary PID file
-        unlink(GP->pidfile);
     LOGERR("Exit with status %d", sig);
+    stopwork = 1;
+    saveconf(NULL);
+    usleep(10000);
+    DBG("disconnectGrasshopper()");
+    disconnectGrasshopper();
+    DBG("pusi_disconnect()");
+    pusi_disconnect();
+    DBG("closeXYlog()");
+    closeXYlog();
+    if(GP && GP->pidfile){ // remove unnesessary PID file
+        DBG("unlink(GP->pidfile)");
+        unlink(GP->pidfile);
+    }
     exit(sig);
 }
 
@@ -88,16 +103,10 @@ int main(int argc, char *argv[]){
     if(GP->throwpart < 0. || GP->throwpart > 0.99){
         ERRX("Fraction of black pixels should be in [0., 0.99]");
     }
+    if(GP->Naveraging < 2 || GP->Naveraging > MAX_AVERAGING_ARRAY_SIZE)
+        ERRX("Averaging amount should be from 2 to 25");
     InputType tp = chk_inp(GP->inputname);
     if(tp == T_WRONG) ERRX("Enter correct image file or directory name");
-    check4running(self, GP->pidfile);
-    DBG("%s started, snippets library version is %s\n", self, sl_libversion());
-    free(self);
-    signal(SIGTERM, signals); // kill (-15) - quit
-    signal(SIGHUP, SIG_IGN);  // hup - ignore
-    signal(SIGINT, signals);  // ctrl+C - quit
-    signal(SIGQUIT, signals); // ctrl+\ - quit
-    signal(SIGTSTP, SIG_IGN); // ignore ctrl+Z
     if(GP->logfile){
         sl_loglevel lvl = LOGLEVEL_ERR; // default log level - errors
         int v = GP->verb;
@@ -107,10 +116,70 @@ 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;
+        if(GP->minarea != DEFAULT_MINAREA || theconf.minarea == 0) theconf.minarea = GP->minarea;
+        if(GP->xtarget > 0.) theconf.xtarget = GP->xtarget;
+        if(GP->ytarget > 0.) theconf.ytarget = GP->ytarget;
+        if(GP->nerosions != DEFAULT_EROSIONS || theconf.Nerosions == 0){
+            if(GP->nerosions < 1 || GP->nerosions > MAX_NEROS) ERRX("Amount of erosions should be from 1 to %d", MAX_NEROS);
+            theconf.Nerosions = GP->nerosions;
+        }
+        if(GP->ndilations != DEFAULT_DILATIONS || theconf.Ndilations == 0){
+            if(GP->ndilations < 1 || GP->ndilations > MAX_NDILAT) ERRX("Amount of erosions should be from 1 to %d", MAX_NDILAT);
+            theconf.Ndilations = GP->ndilations;
+        }
+        if(fabs(GP->throwpart - DEFAULT_THROWPART) > DBL_EPSILON || theconf.throwpart < DBL_EPSILON){
+            if(GP->throwpart < 0. || GP->throwpart > MAX_THROWPART) ERRX("'thworpart' should be from 0 to %g", MAX_THROWPART);
+            theconf.throwpart = GP->throwpart;
+        }
+        if(GP->xoff && GP->xoff < MAX_OFFSET) theconf.xoff = GP->xoff;
+        if(GP->yoff && GP->yoff < MAX_OFFSET) theconf.yoff = GP->yoff;
+        if(GP->width && GP->width < MAX_OFFSET) theconf.width = GP->width;
+        if(GP->height && GP->height < MAX_OFFSET) theconf.height = GP->height;
+        if(fabs(GP->minexp - EXPOS_MIN) > DBL_EPSILON || theconf.minexp < DBL_EPSILON){
+            if(GP->minexp < DBL_EPSILON || GP->minexp > EXPOS_MAX) ERRX("Minimal exposition should be > 0 and < %g", EXPOS_MAX);
+            theconf.minexp = GP->minexp;
+        }
+        if(fabs(GP->maxexp - EXPOS_MAX) > DBL_EPSILON || theconf.maxexp < theconf.minexp){
+            if(GP->maxexp < theconf.minexp) ERRX("Maximal exposition should be greater than minimal");
+            theconf.maxexp = GP->maxexp;
+        }
+        if(GP->equalize && theconf.equalize == 0) theconf.equalize = 1;
+        if(fabs(GP->intensthres - DEFAULT_INTENSTHRES) > DBL_EPSILON){
+            if(GP->intensthres < DBL_EPSILON || GP->intensthres > 1.-DBL_EPSILON) ERRX("'intensthres' should be from 0 to 1");
+            theconf.intensthres = GP->intensthres;
+        }
+        if(GP->Naveraging != DEFAULT_NAVERAGE || theconf.naverage < 1){
+            if(GP->Naveraging < 1 || GP->Naveraging > NAVER_MAX) ERRX("N images for averaging should be from 1 to %d", NAVER_MAX);
+            theconf.naverage = GP->Naveraging;
+        }
+        if(GP->pusiservport != DEFAULT_PUSIPORT || theconf.stpserverport == 0){
+            if(GP->pusiservport < 1 || GP->pusiservport > 65535) ERRX("Wrong steppers' server port: %d", GP->pusiservport);
+            theconf.stpserverport = GP->pusiservport;
+        }
+    }
+    setpostprocess(GP->processing);
+    check4running(self, GP->pidfile);
+    DBG("%s started, snippets library version is %s\n", self, sl_libversion());
+    free(self);
+    signal(SIGTERM, signals); // kill (-15) - quit
+    signal(SIGHUP, SIG_IGN);  // hup - ignore
+    signal(SIGINT, signals);  // ctrl+C - quit
+    signal(SIGQUIT, signals); // ctrl+\ - quit
+    signal(SIGTSTP, SIG_IGN); // ignore ctrl+Z
     if(GP->logXYname) openXYlog(GP->logXYname);
     LOGMSG("Start application...");
-    LOGDBG("xtag=%g, ytag=%g", GP->xtarget, GP->ytarget);
+    LOGDBG("xtag=%g, ytag=%g", theconf.xtarget, theconf.ytarget);
+    openIOport(GP->ioport);
     int p = process_input(tp, GP->inputname);
+    DBG("process_input=%d", p);
     // never reached
     signals(p); // clean everything
     return p;

LocCorr/pusirobo.c

@@ -0,0 +1,748 @@
+/*
+ * This file is part of the loccorr project.
+ * Copyright 2021 Edward V. Emelianov <edward.emelianoff@gmail.com>.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#define _GNU_SOURCE
+#include <arpa/inet.h>
+#include <math.h>
+#include <netdb.h>
+#include <pthread.h>
+#include <stdio.h>
+#include <string.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <usefull_macros.h>
+
+#include "config.h"
+#include "pusirobo.h"
+#include "socket.h"
+
+// max time to wait answer "OK" from server
+#define WAITANSTIME         (1.0)
+#define ANSOK               "OK\n"
+
+// amount of consequent center coordinates coincidence in `process_targetstate`
+#define NCONSEQ             (2)
+// tolerance of coordinates coincidence (pix)
+#define COORDTOLERANCE      (0.1)
+
+// messages for CAN server
+#define registerUaxe        "register U 0x581 stepper"
+#define registerVaxe        "register V 0x582 stepper"
+#define registerFocus       "register F 0x583 stepper"
+#define setUspeed           "mesg U maxspeed 12800"
+#define setVspeed           "mesg V maxspeed 12800"
+#define setFspeed           "mesg F maxspeed 1600"
+#define Urelsteps           "mesg U relmove "
+#define Vrelsteps           "mesg V relmove "
+#define Fabssteps           "mesg F absmove "
+#define Frelsteps           "mesg F relmove "
+#define Ustatus             "mesg U status"
+#define Vstatus             "mesg V status"
+#define Fstatus             "mesg F status"
+#define Usetzero            "mesg U setzero"
+#define Vsetzero            "mesg V setzero"
+#define Fsetzero            "mesg F setzero"
+// parameter's names
+#define PARstatus           "devstatus"
+#define STEPSstatus         "steps"
+#define ERRstatus           "errstatus"
+#define CURPOSstatus        "curpos"
+// max range of U and V motors (all in microsteps!)
+#define UVmaxsteps          (35200)
+#define Fmaxsteps           (64000)
+// steps to move from the edge
+#define UVedgesteps         (960)
+
+
+#define moveU(s)                move_motor(Urelsteps, s)
+#define moveV(s)                move_motor(Vrelsteps, s)
+#define moveF(s)                move_motor(Frelsteps, s)
+#define setF(s)                 move_motor(Fabssteps, s)
+#define UVmoving_finished()     (moving_finished(Ustatus, NULL) && moving_finished(Vstatus, NULL))
+
+typedef enum{
+    SETUP_NONE,             // no setup
+    SETUP_INIT,             // the starting - move U&V to 0
+    SETUP_WAITUV0,          // wait & move U&V to middle
+    SETUP_WAITUVMID,        // wait
+    SETUP_WAITU0,           // move U->0
+    SETUP_WAITUMAX,         // move U->max
+    SETUP_WAITV0,           // V->0
+    SETUP_WAITVMAX,         // V->max
+    SETUP_FINISH
+} setupstatus;
+static setupstatus sstatus = SETUP_NONE; // setup state
+
+static pusistate state = PUSI_DISCONN;   // server state
+
+static int sockfd = -1; // server file descriptor
+
+// current steps counters (zero at the middle)
+static int Uposition = 0, Vposition = 0, Fposition = 0;
+
+void pusi_disconnect(){
+    if(sockfd > -1) close(sockfd);
+    sockfd = -1;
+    state = PUSI_DISCONN;
+}
+
+static char *findval(const char *par, const char *statusmesg){
+    if(!statusmesg || !par) return NULL;
+    char *pair = strcasestr(statusmesg, par);
+    if(!pair) return NULL;
+    pair += strlen(par);
+    while(*pair && *pair != '\n' && *pair != '=') ++pair;
+    if(*pair != '=') return NULL; // no equal sign
+    ++pair; while(*pair == ' ' || *pair == '\t') ++pair;
+    //DBG("val fof '%s' is '%s'", par, pair);
+    return pair;
+}
+
+/**
+ * @brief getparval - return value of parameter
+ * @param par (i)        - parameter value
+ * @param statusmesg (i) - message of 'status' command
+ * @param val (o)        - value of parameter
+ * @return TRUE if parameter found and set `val` to its value
+ */
+static int getparval(const char *par, const char *statusmesg, double *val){
+    char *parval = findval(par, statusmesg);
+    if(!parval) return FALSE;
+    if(!val) return TRUE;
+    *val = atof(parval);
+    return TRUE;
+}
+// the same as getparval, but check for "=OK"
+static int getOKval(const char *par, const char *statusmesg){
+    //DBG("getOKval('%s', '%s')", par, statusmesg);
+    char *parval = findval(par, statusmesg);
+    if(!parval) return FALSE;
+    if(strncmp(parval, "OK", 2) == 0) return TRUE;
+    return FALSE;
+}
+
+/**
+ * wait for answer from socket
+ * @return FALSE in case of error or timeout, TRUE if socket is ready
+ */
+static int canread(){
+    if(sockfd < 0) return FALSE;
+    fd_set fds;
+    struct timeval timeout;
+    int rc;
+    // wait not more than 10ms
+    timeout.tv_sec = 0;
+    timeout.tv_usec = 10000;
+    FD_ZERO(&fds);
+    FD_SET(sockfd, &fds);
+    do{
+        rc = select(sockfd+1, &fds, NULL, NULL, &timeout);
+        if(rc < 0){
+            if(errno != EINTR){
+                WARN("select()");
+                return FALSE;
+            }
+            continue;
+        }
+        break;
+    }while(1);
+    if(FD_ISSET(sockfd, &fds)) return TRUE;
+    return FALSE;
+}
+
+// clear incoming buffer
+static void clearbuf(){
+    if(sockfd < 0) return;
+    char buf[256] = {0};
+    while(canread() && 0 < read(sockfd, buf, 256)) DBG("clearbuf: %s", buf);
+}
+
+/**
+ * read answer "OK" from socket
+ * @param retval - if !NULL there's will be an answer copy (after "OK\n") here
+ * @return FALSE if timeout or no "OK"
+ */
+static int waitOK(char **retval){
+    if(sockfd < 0) return FALSE;
+#define BUFFERSZ    (2047)
+    char buf[BUFFERSZ+1];
+    int Nread = 0, ctr = 0;
+    double t0 = dtime();
+    while(dtime() - t0 < WAITANSTIME && Nread < BUFFERSZ){
+        if(!canread()){
+            //DBG("No answer @ %d try", ctr);
+            if(++ctr > 3) break;
+            continue;
+        }
+        ctr = 0;
+        int n = read(sockfd, buf+Nread, BUFFERSZ-Nread);
+        //DBG("n=%d", n);
+        if(n == 0) break;
+        if(n < 0) return FALSE; // disconnect or error
+        Nread += n;
+        buf[Nread] = 0;
+    }
+    //DBG("All buffer: '%s'", buf);
+    int ret = FALSE;
+    char *ok = strstr(buf, ANSOK);
+    if(ok){
+        //DBG("ans: '%s'", OK + sizeof(ANSOK)-1);
+        ret = TRUE;
+        if(retval){
+            *retval = strdup(ok + sizeof(ANSOK)-1);
+            //DBG("RETVAL: '%s'", *retval);
+        }
+    }else LOGWARN("didn't get OK answer");
+#undef BUFFERSZ
+    return ret;
+}
+
+/**
+ * @brief send_message - send character string `msg` to pusiserver
+ * @param msg - message
+ * @param ans - answer (if !NULL)
+ * @return FALSE if failed (should reconnect)
+ */
+static int send_message(const char *msg, char **ans){
+    if(!msg || sockfd < 0) return FALSE;
+    size_t L = strlen(msg);
+    clearbuf();
+    if(send(sockfd, msg, L, 0) != (ssize_t)L){
+        LOGWARN("send_message(): send() failed");
+        return FALSE;
+    }
+    DBG("Message '%s' sent", msg);
+    return waitOK(ans);
+}
+
+static void send_message_nocheck(const char *msg){
+    if(!msg || sockfd < 0) return;
+    size_t L = strlen(msg);
+    clearbuf();
+    if(send(sockfd, msg, L, 0) != (ssize_t)L){
+        WARN("send");
+    }
+    DBG("Unchecked message '%s' sent", msg);
+}
+
+// try to set default speeds
+static int setSpeed(const char *mesg, const char *name){
+    char *ans = NULL;
+    int retval = TRUE;
+    if(!send_message(mesg, &ans)){
+        LOGERR("Can't set %s motor speed", name);
+        retval = FALSE;
+    }
+    if(ans && *ans){
+        DBG("ans: %s\n", ans);
+    }else{
+        LOGERR("no %s motor", name);
+        retval = FALSE;
+    }
+    FREE(ans);
+    return retval;
+}
+
+/**
+ * @brief pusi_connect - connect to a local steppers CAN server
+ * @return FALSE if failed
+ */
+int pusi_connect(){
+    DBG("pusi_connect(%d)", theconf.stpserverport);
+    char port[10];
+    snprintf(port, 10, "%d", theconf.stpserverport);
+    pusi_disconnect();
+    struct addrinfo hints = {0}, *res, *p;
+    hints.ai_family = AF_INET;
+    hints.ai_socktype = SOCK_STREAM;
+    hints.ai_flags = AI_PASSIVE;
+    if(getaddrinfo(NULL, port, &hints, &res) != 0){
+        WARN("getaddrinfo()");
+        return FALSE;
+    }
+    // loop through all the results and connect to the first we can
+    for(p = res; p; p = p->ai_next){
+        if((sockfd = socket(p->ai_family, p->ai_socktype, p->ai_protocol)) == -1){
+            WARN("socket");
+            continue;
+        }
+        if(connect(sockfd, p->ai_addr, p->ai_addrlen) == -1){
+            WARN("connect()");
+            close(sockfd);
+            continue;
+        }
+        break; // if we get here, we have a successfull connection
+    }
+    if(!p){
+        WARNX("Can't connect to socket");
+        sockfd = -1;
+        return FALSE;
+    }
+    freeaddrinfo(res);
+    // register and set max speed; don't check `register` answer as they could be registered already
+    send_message_nocheck(registerUaxe);
+    send_message_nocheck(registerVaxe);
+    send_message_nocheck(registerFocus);
+    int retval = TRUE;
+    if(!setSpeed(setUspeed, "U")) retval = FALSE;
+    if(!setSpeed(setVspeed, "V")) retval = FALSE;
+    if(!setSpeed(setFspeed, "F")) retval = FALSE;
+    if(!retval) pusi_disconnect();
+    else{
+        state = PUSI_RELAX;
+        sstatus = SETUP_NONE;
+    }
+    return retval;
+}
+
+// return TRUE if motor is stopped
+static int moving_finished(const char *mesgstatus, int *position){
+    double val;
+    char *ans = NULL;
+    int ret = TRUE;
+    if(send_message(mesgstatus, &ans) && getparval(PARstatus, ans, &val)){
+        DBG("send(%s) true: %s %g\n", mesgstatus, ans, val);
+    }else{
+        LOGDBG("send(%s) false: %s %g\n", mesgstatus, ans, val);
+        return FALSE;
+    }
+    int ival = (int)val;
+    if(ival) ret = FALSE;
+    if(position){
+        if(getparval(CURPOSstatus, ans, &val)){
+            *position = (int) val;
+        }else LOGDBG("%s not found in '%s'", CURPOSstatus, ans);
+    }
+    FREE(ans);
+    return ret;
+}
+
+// move motor to s steps, @return FALSE if failed
+static int move_motor(const char *movecmd, int s/*, int *counter*/){
+    DBG("move %s -> %d", movecmd, s);
+    LOGDBG("move %s -> %d", movecmd, s);
+    char buf[256], *ans;
+    snprintf(buf, 255, "%s %d", movecmd, s);
+    if(!send_message(buf, &ans)){
+        LOGDBG("can't send message");
+        return FALSE;
+    }
+    int ret = TRUE;
+    if(!getOKval(STEPSstatus, ans)){
+        LOGDBG("NO OK in %s", ans);
+        ret = FALSE;
+    }
+    FREE(ans);
+    return ret;
+}
+
+static void process_movetomiddle_stage(){
+    switch(sstatus){
+        case SETUP_INIT: // initial moving
+            if(moveU(-UVmaxsteps) && moveV(-UVmaxsteps) && moveF(-Fmaxsteps*2))
+                sstatus = SETUP_WAITUV0;
+        break;
+        case SETUP_WAITUV0: // wait for both coordinates moving to zero
+            DBG("Moving to left border");
+            if(!(UVmoving_finished() && moving_finished(Fstatus, NULL))) return;
+            DBG("Reached!");
+            if(!send_message(Fsetzero, NULL)) return;
+            Fposition = 0;
+            if(moveU(theconf.maxUsteps+UVedgesteps) && moveV(theconf.maxUsteps+UVedgesteps) && moveF(Fmaxsteps/2))
+                sstatus = SETUP_WAITUVMID;
+        break;
+        case SETUP_WAITUVMID: // wait for the middle
+            DBG("Moving to the middle");
+            if(!(UVmoving_finished() && moving_finished(Fstatus, NULL))) return;
+            DBG("Reached!");
+            Uposition = 0; Vposition = 0; Fposition = Fmaxsteps/2;
+            if(!send_message(Usetzero, NULL) || !send_message(Vsetzero, NULL)) return;
+            sstatus = SETUP_NONE;
+            state = PUSI_RELAX;
+        break;
+        default:
+            sstatus = SETUP_NONE;
+            state = PUSI_RELAX;
+    }
+}
+
+/**
+ * @brief process_setup_stage - process all stages of axes setup
+ */
+static void process_setup_stage(double x, double y, int aver){
+    DBG("PROCESS: %d\n", sstatus);
+    static int ctr; // iterations counter
+     // coordinates for corrections calculation
+    static double X0U, Y0U, XmU, YmU;
+    static double X0V, Y0V, XmV, YmV;
+    switch(sstatus){
+        case SETUP_INIT: // initial moving
+            if(moveU(-UVmaxsteps) && moveV(-UVmaxsteps))
+                sstatus = SETUP_WAITUV0;
+        break;
+        case SETUP_WAITUV0: // wait for both coordinates moving to zero
+            DBG("Moving to left border");
+            if(!UVmoving_finished()) return;
+            DBG("Reached!");
+            if(moveU(theconf.maxUsteps+UVedgesteps) && moveV(theconf.maxUsteps+UVedgesteps))
+                sstatus = SETUP_WAITUVMID;
+        break;
+        case SETUP_WAITUVMID: // wait for the middle
+            DBG("Moving to the middle");
+            if(!UVmoving_finished()) return;
+            DBG("Reached!");
+            Uposition = 0; Vposition = 0;
+            if(moveU(-theconf.maxUsteps)) sstatus = SETUP_WAITU0;
+            ctr = 0;
+        break;
+        case SETUP_WAITU0: // wait while U moves to zero
+            if(!aver) return;
+            if(!moving_finished(Ustatus, NULL)) return;
+            if(++ctr < 2) return; // wait for next average coordinates
+            X0U = x; Y0U = y;
+            LOGDBG("got X0U=%.1f, Y0U=%.1f", x, y);
+            if(moveU(2*theconf.maxUsteps)) sstatus = SETUP_WAITUMAX;
+            ctr = 0;
+        break;
+        case SETUP_WAITUMAX: // wait while U moves to UVworkrange
+            if(!aver) return;
+            if(!moving_finished(Ustatus, NULL)) return;
+            if(++ctr < 2) return; // wait for next average coordinates
+            XmU = x; YmU = y;
+            LOGDBG("got XmU=%.1f, YmU=%.1f", x, y);
+            if(moveU(-theconf.maxUsteps) && moveV(-theconf.maxVsteps)) sstatus = SETUP_WAITV0;
+            ctr = 0;
+        break;
+        case SETUP_WAITV0: // wait while V moves to 0
+            if(!aver) return;
+            if(!moving_finished(Vstatus, NULL)) return;
+            if(++ctr < 2) return; // wait for next average coordinates
+            X0V = x; Y0V = y;
+            LOGDBG("got X0V=%.1f, Y0V=%.1f", x, y);
+            if(moveV(2*theconf.maxVsteps)) sstatus = SETUP_WAITVMAX;
+            ctr = 0;
+        break;
+        case SETUP_WAITVMAX: // wait while V moves to UVworkrange
+            if(!aver) return;
+            if(!moving_finished(Vstatus, NULL)) return;
+            if(++ctr < 2) return; // wait for next average coordinates
+            ctr = 0;
+            XmV = x; YmV = y;
+            LOGDBG("got XmV=%.1f, YmV=%.1f", x, y);
+            // calculate
+            double dxU = XmU - X0U, dyU = YmU - Y0U, dxV = XmV - X0V, dyV = YmV - Y0V;
+            LOGDBG("dxU=%.1f, dyU=%.1f, dxV=%.1f, dyV=%.1f", dxU, dyU, dxV, dyV);
+            double sqU = sqrt(dxU*dxU + dyU*dyU), sqV = sqrt(dxV*dxV + dyV*dyV);
+            LOGDBG("sqU=%g, sqV=%g", sqU, sqV);
+            if(sqU < DBL_EPSILON || sqV < DBL_EPSILON) goto endmoving;
+            // TODO: check configuration !!111111
+            // proportion coefficients for axes
+            double KU = 2 * theconf.maxUsteps / sqU;
+            double KV = 2 * theconf.maxVsteps / sqV;
+            double sa = dyU/sqU, ca = dxU/sqU, sb = dyV/sqV, cb = dxV/sqV; // sin(alpha) etc
+            // ctg(beta-alpha)=cos(b-a)/sin(b-a)=[cos(b)cos(a)+sin(b)sin(a)]/[sin(b)cos(a)-cos(b)sin(a)]
+            double sba = sb*ca - cb*sa; // sin(beta-alpha)
+            double ctba = (cb*ca + sb*sa) / sba;
+            if(fabs(ctba) < DBL_EPSILON || fabs(sba) < DBL_EPSILON) goto endmoving;
+            LOGDBG("KU=%.4f, KV=%.4f, sa=%.4f, ca=%.4f, sb=%.4f, cb=%.4f, ctba=%.5f, 1/sba=%.5f",
+                   KU, KV, sa, ca, sb, cb, ctba, 1./sba);
+            /*
+             * U = x*(cos(alpha) - sin(alpha)*ctg(beta-alpha)) + y*(-sin(alpha)-cos(alpha)*ctg(beta-alpha))
+             * V = x*sin(alpha)/sin(beta-alpha) + y*cos(alpha)/sin(beta-alpha)
+             */
+            theconf.Kxu = KU*(ca - sa*ctba);
+            theconf.Kyu = KU*(-sa - ca*ctba);
+            theconf.Kxv = KV*sa/sba;
+            theconf.Kyv = KV*ca/sba;
+            LOGDBG("Kxu=%g, Kyu=%g; Kxv=%g, Kyv=%g", theconf.Kxu, theconf.Kyu, theconf.Kxv, theconf.Kyv);
+            DBG("Now save new configuration");
+            saveconf(NULL); // try to store configuration
+        endmoving:
+            moveV(-theconf.maxVsteps);
+            sstatus = SETUP_FINISH;
+        break;
+        case SETUP_FINISH: // reset current coordinates
+            if(!UVmoving_finished()) return;
+            if(!send_message(Usetzero, NULL) || !send_message(Vsetzero, NULL)) return;
+            // now inner steppers' counters are in zero position -> set to zero local
+            Uposition = Vposition = 0;
+            sstatus = SETUP_NONE;
+            state = PUSI_RELAX;
+        break;
+        default: // SETUP_NONE - do nothing
+            return;
+    }
+}
+
+// return TRUE if finished
+static int process_targetstage(double X, double Y){
+    static double xprev = 0., yprev = 0.;
+    static int nhit = 0;
+    if(fabs(X - xprev) > COORDTOLERANCE || fabs(Y - yprev) > COORDTOLERANCE){
+        DBG("tolerance too bad: dx=%g, dy=%g", X-xprev, Y-yprev);
+        nhit = 0;
+        xprev = X; yprev = Y;
+        return FALSE;
+    }else if(++nhit < NCONSEQ){
+        DBG("nhit = %d", nhit);
+        return FALSE;
+    }
+    theconf.xtarget = X;
+    theconf.ytarget = Y;
+    DBG("Got target coordinates: (%.1f, %.1f)", X, Y);
+    saveconf(FALSE);
+    nhit = 0; xprev = 0.; yprev = 0.;
+    return TRUE;
+}
+
+/**
+ * @brief try2correct - try to correct position
+ * @param dX - delta of X-coordinate in image space
+ * @param dY - delta of Y-coordinate in image space
+ * @return FALSE if failed or correction out of limits
+ */
+static int try2correct(double dX, double dY){
+    double dU, dV;
+    // dU = KU*(dX*cosXU + dY*sinXU); dV = KV*(dX*cosXV + dY*sinXV)
+    dU = KCORR*(theconf.Kxu * dX + theconf.Kyu * dY);
+    dV = KCORR*(theconf.Kxv * dX + theconf.Kyv * dY);
+    int Unew = Uposition + (int)dU, Vnew = Vposition + (int)dV;
+    if(Unew > theconf.maxUsteps || Unew < -theconf.maxUsteps ||
+       Vnew > theconf.maxVsteps || Vnew < -theconf.maxVsteps){
+        // TODO: here we should signal the interface that limit reaced
+        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)",
+           Uposition, Vposition, Unew, Vnew, Uposition + (int)(dU/KCORR),
+           Vposition + (int)(dV/KCORR), dU, dV);
+    return (moveU((int)dU) && moveV((int)dV));
+}
+
+#if 0
+mesg U relmove -35200
+mesg V relmove -35200
+mesg U relmove 16960
+mesg V relmove 16960
+mesg U relmove -500000
+mesg U relmove 100000
+mesg F relmove 32000
+#endif
+/**
+ * @brief pusi_process_corrections - get XY corrections (in pixels) and move motors to fix them
+ * @param X, Y - centroid (x,y) in screen coordinate system
+ * @param aver ==1 if X and Y are averaged
+ * This function called from improc.c each time the corrections calculated (ONLY IF Xtarget/Ytarget > -1)
+ */
+void pusi_process_corrections(double X, double Y, int aver){
+    DBG("got centroid data: %g, %g", X, Y);
+    double xdev = X - theconf.xtarget, ydev = Y - theconf.ytarget;
+    switch(state){
+        case PUSI_DISCONN:
+            if(!pusi_connect()){
+                WARN("Can't reconnect");
+                LOGWARN("Can't reconnect");
+            }
+        break;
+        case PUSI_SETUP: // setup axes (before this state set Xtarget/Ytarget in improc.c)
+            process_setup_stage(X, Y, aver);
+        break;
+        case PUSI_GOTOTHEMIDDLE:
+            process_movetomiddle_stage();
+        break;
+        case PUSI_FINDTARGET: // calculate target coordinates
+            if(aver && process_targetstage(X, Y))
+                state = PUSI_RELAX;
+        break;
+        case PUSI_FIX:   // process corrections
+            if(aver){
+                red("GET AVERAGE -> correct\n");
+                if(theconf.xtarget < 1. || theconf.ytarget < 1. || fabs(xdev) < COORDTOLERANCE || fabs(ydev) < COORDTOLERANCE){
+                    DBG("Target coordinates not defined or correction too small");
+                    return;
+                }
+                if(!moving_finished(Ustatus, &Uposition) || !moving_finished(Vstatus, &Vposition)) return;
+                LOGDBG("Current position: U=%d, V=%d, deviations: dX=%.1f, dy=%.1f",
+                       Uposition, Vposition, xdev, ydev);
+                if(!try2correct(xdev, ydev)){
+                    LOGWARN("failed to correct");
+                    // TODO: do something here
+                    DBG("FAILED");
+                }
+            }
+        break;
+        default: // PUSI_RELAX
+            return;
+    }
+}
+
+// try to change state; @return TRUE if OK
+int pusi_setstate(pusistate newstate){
+    if(newstate == state) return TRUE;
+    if(newstate == PUSI_DISCONN){
+        pusi_disconnect();
+        return TRUE;
+    }
+    if(state == PUSI_DISCONN){
+        if(!pusi_connect()) return FALSE;
+    }
+    if(newstate == PUSI_SETUP || newstate == PUSI_GOTOTHEMIDDLE){
+        sstatus = SETUP_INIT;
+    }else sstatus = SETUP_NONE;
+    state = newstate;
+    return TRUE;
+}
+
+pusistate pusi_getstate(){
+    return state;
+}
+
+// get current status
+// return JSON string with different parameters
+char *pusi_status(char *buf, int buflen){
+    int l;
+    char *bptr = buf;
+    const char *s = NULL, *stage = NULL;
+    l = snprintf(bptr, buflen, "{ \"status\": ");
+    buflen -= l; bptr += l;
+    switch(state){
+        case PUSI_DISCONN:
+            l = snprintf(bptr, buflen, "\"disconnected\"");
+        break;
+        case PUSI_RELAX:
+            l = snprintf(bptr, buflen, "\"ready\"");
+        break;
+        case PUSI_SETUP:
+        case PUSI_GOTOTHEMIDDLE:
+            s = (state == PUSI_SETUP) ? "setup" : "gotomiddle";
+            switch(sstatus){
+                case SETUP_INIT:
+                    stage = "init";
+                break;
+                case SETUP_WAITUV0:
+                     stage = "waituv0";
+                break;
+                case SETUP_WAITUVMID:
+                     stage = "waituvmid";
+                break;
+                case SETUP_WAITU0:
+                     stage = "waitu0";
+                break;
+                case SETUP_WAITUMAX:
+                     stage = "waitumax";
+                break;
+                case SETUP_WAITV0:
+                     stage = "waitv0";
+                break;
+                case SETUP_WAITVMAX:
+                     stage = "waitvmax";
+                break;
+                case SETUP_FINISH:
+                     stage = "finishing";
+                break;
+                default:
+                     stage = "unknown";
+            }
+            l = snprintf(bptr, buflen, "{ \"%s\": \"%s\" }", s, stage);
+        break;
+        case PUSI_FINDTARGET:
+            l = snprintf(bptr, buflen, "\"findtarget\"");
+        break;
+        case PUSI_FIX:
+            l = snprintf(bptr, buflen, "\"fixing\"");
+        break;
+        default:
+            l = snprintf(bptr, buflen, "\"unknown\"");
+    }
+    buflen -= l; bptr += l;
+    if(state != PUSI_DISCONN){
+        l = snprintf(bptr, buflen, ", ");
+        buflen -= l; bptr += l;
+        const char *motors[] = {"Umotor", "Vmotor", "Fmotor"};
+        const char *statuses[] = {Ustatus, Vstatus, Fstatus};
+        int *pos[] = {&Uposition, &Vposition, &Fposition};
+        for(int i = 0; i < 3; ++i){
+            const char *stat = "moving";
+            if(moving_finished(statuses[i], pos[i])) stat = "stopping";
+            l = snprintf(bptr, buflen, "\"%s\": { \"status\": \"%s\", \"position\": %d }%s",
+                         motors[i], stat, *pos[i], (i==2)?"":", ");
+            buflen -= l; bptr += l;
+        }
+    }
+    snprintf(bptr, buflen, " }\n");
+    return buf;
+}
+
+typedef struct{
+    const char *str;
+    pusistate state;
+} strstate;
+
+strstate stringstatuses[] = {
+    {"disconnect", PUSI_DISCONN},
+    {"relax", PUSI_RELAX},
+    {"setup", PUSI_SETUP},
+    {"middle", PUSI_GOTOTHEMIDDLE},
+    {"findtarget", PUSI_FINDTARGET},
+    {"fix", PUSI_FIX},
+    {NULL, 0}
+};
+// try to set new status
+char *set_pusistatus(const char *newstatus, char *buf, int buflen){
+    strstate *s = stringstatuses;
+    pusistate newstate = PUSI_UNDEFINED;
+    while(s->str){
+        if(strcasecmp(s->str, newstatus) == 0){
+            newstate = s->state;
+            break;
+        }
+        ++s;
+    }
+    if(newstate != PUSI_UNDEFINED){
+        if(pusi_setstate(newstate)){
+            snprintf(buf, buflen, OK);
+            return buf;
+        }else return pusi_status(buf, buflen);
+    }
+    int L = snprintf(buf, buflen, "status '%s' undefined, allow: ", newstatus);
+    char *ptr = buf;
+    s = stringstatuses;
+    while(L > 0){
+        buflen -= L;
+        ptr += L;
+        L = snprintf(ptr, buflen, "'%s' ", s->str);
+        if((++s)->str == NULL) break;
+    }
+    ptr[L-1] = '\n';
+    return buf;
+}
+// change focus
+char *set_pfocus(const char *newstatus, char *buf, int buflen){
+    if(!moving_finished(Fstatus, &Fposition)){
+        snprintf(buf, buflen, "moving\n");
+        return buf;
+    }
+    int newval = atoi(newstatus);
+    if(newval < 0 || newval > Fmaxsteps){
+        snprintf(buf, buflen, "Bad value: %d", newval);
+    }else{
+        if(!setF(newval)) snprintf(buf, buflen, FAIL);
+        else snprintf(buf, buflen, OK);
+    }
+    return buf;
+}

LocCorr/pusirobo.h

@@ -0,0 +1,44 @@
+/*
+ * This file is part of the loccorr project.
+ * Copyright 2021 Edward V. Emelianov <edward.emelianoff@gmail.com>.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#pragma once
+#ifndef PUSIROBO_H__
+#define PUSIROBO_H__
+
+typedef enum{
+    PUSI_DISCONN,
+    PUSI_RELAX,
+    PUSI_SETUP,
+    PUSI_GOTOTHEMIDDLE,
+    PUSI_FINDTARGET,
+    PUSI_FIX,
+    PUSI_UNDEFINED
+} pusistate;
+
+// try to connect to local pusirobo server
+int pusi_connect();
+int pusi_setstate(pusistate newstate);
+pusistate pusi_getstate();
+void pusi_disconnect();
+void pusi_process_corrections(double X, double Y, int corrflag);
+char *pusi_status(char *buf, int buflen);
+char *set_pusistatus(const char *newstatus, char *buf, int buflen);
+char *set_pfocus(const char *newstatus, char *buf, int buflen);
+char *get_JSON_status(char *buf, int buflen);
+// ADD global SEND
+
+#endif // PUSIROBO_H__

LocCorr/socket.c

@@ -0,0 +1,408 @@
+/*
+ * This file is part of the loccorr project.
+ * Copyright 2021 Edward V. Emelianov <edward.emelianoff@gmail.com>.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <arpa/inet.h>  // inet_ntop
+#include <sys/ioctl.h>
+#include <limits.h>     // INT_xxx
+#include <netdb.h>      // addrinfo
+#include <poll.h>
+#include <pthread.h>
+#include <signal.h>     // pthread_kill
+#include <stdio.h>
+#include <string.h>
+#include <sys/syscall.h> // syscall
+#include <unistd.h>     // daemon
+#include <usefull_macros.h>
+
+#include "config.h"
+#include "improc.h"
+#include "socket.h"
+
+// buffer size for received data
+#define BUFLEN      (1024)
+// buffer size for answer
+#define ANSBUFLEN   (32768)
+// Max amount of connections
+#define BACKLOG     (10)
+
+// additional commands list - getters
+typedef struct{
+    const char *command;
+    char *(*handler)(char *buf, int buflen);
+    const char *help;
+} getter;
+// setters
+typedef struct{
+    const char *command;
+    char *(*handler)(const char *val, char *buf, int buflen);
+    const char *help;
+} setter;
+
+static char *helpmsg(char *buf, int buflen);
+static char *stepperstatus(char *buf, int buflen);
+static getter getterHandlers[] = {
+    {"help", helpmsg, "List avaiable commands"},
+    {"settings", listconf, "List current configuration"},
+    {"steppers", stepperstatus, "Get status of steppers' server"},
+    {NULL, NULL, NULL}
+};
+
+static char *setstepperstate(const char *state, char *buf, int buflen);
+static char *setfocusstate(const char *state, char *buf, int buflen);
+static setter setterHandlers[] = {
+    {"stpstate", setstepperstate, "Set given steppers' server state"},
+    {"focus", setfocusstate, "Move focus to given value"},
+    {NULL, NULL, NULL}
+};
+
+/**************** functions to process commands ****************/
+// getters
+static char *helpmsg(char *buf, int buflen){
+    if(get_cmd_list(buf, buflen)){
+        int l = strlen(buf), L = buflen - l;
+        char *ptr = buf + l;
+        getter *g = getterHandlers;
+        while(L > 0 && g->command){
+            int s = snprintf(ptr, L, "%s - %s\n", g->command, g->help);
+            if(s < 1) break;
+            L -= s; ptr += s;
+            ++g;
+        }
+        setter *sh = setterHandlers;
+        while(L > 0 && sh->command){
+            int s = snprintf(ptr, L, "%s=newval - %s\n", sh->command, sh->help);
+            if(s < 1) break;
+            L -= s; ptr += s;
+            ++sh;
+        }
+        return buf;
+    }
+    return NULL;
+}
+static char *stepperstatus(char *buf, int buflen){
+    if(stepstatus) return stepstatus(buf, buflen);
+    snprintf(buf, buflen, "not defined");
+    return buf;
+}
+
+// setters
+static char *setstepperstate(const char *state, char *buf, int buflen){
+    DBG("set steppersstate to %s", state);
+    if(setstepstatus) return setstepstatus(state, buf, buflen);
+    snprintf(buf, buflen, "not defined");
+    return buf;
+}
+static char *setfocusstate(const char *state, char *buf, int buflen){
+    DBG("move focus to %s", state);
+    if(movefocus) return movefocus(state, buf, buflen);
+    snprintf(buf, buflen, "not defined");
+    return buf;
+}
+
+
+static char *rmnl(const char *msg, char *buf, int buflen){
+    strncpy(buf, msg, buflen);
+    char *nl = strchr(buf, '\n');
+    if(nl) *nl = 0;
+    return buf;
+}
+/**
+ * @brief processCommand - command parser
+ * @param msg - incoming message
+ * @param ans - buffer for answer
+ * @param anslen - length of `ans`
+ * @return NULL if no answer or pointer to ans
+ */
+static char *processCommand(const char msg[BUFLEN], char *ans, int anslen){
+    char value[BUFLEN];
+    char *kv = get_keyval(msg, value);
+    confparam *par;
+    if(kv){
+        DBG("got KEY '%s' with value '%s'", kv, value);
+        key_value result;
+        par = chk_keyval(kv, value, &result);
+        if(par){
+            switch(par->type){
+                case PAR_INT:
+                    DBG("FOUND! Integer, old=%d, new=%d", *((int*)par->ptr), result.val.intval);
+                    *((int*)par->ptr) = result.val.intval;
+                break;
+                case PAR_DOUBLE:
+                    DBG("FOUND! Double, old=%g, new=%g", *((double*)par->ptr), result.val.dblval);
+                break;
+                default:
+                    snprintf(ans, anslen, "undefined type");
+                    return ans;
+            }
+            snprintf(ans, anslen, "success");
+            return ans;
+        }else{
+            setter *s = setterHandlers;
+            while(s->command){
+                int l = strlen(s->command);
+                if(strncasecmp(msg, s->command, l) == 0)
+                    return s->handler(value, ans, anslen);
+                ++s;
+            }
+        }
+        FREE(kv);
+    }else{
+        getter *g = getterHandlers;
+        while(g->command){
+            int l = strlen(g->command);
+            if(strncasecmp(msg, g->command, l) == 0)
+                return g->handler(ans, anslen);
+            ++g;
+        }
+    }
+    snprintf(ans, anslen, "Message '%s' is wrong", rmnl(msg, value, BUFLEN));
+    return ans;
+}
+
+/**************** SERVER FUNCTIONS ****************/
+/**
+ * Send data over socket (and add trailing '\n' if absent)
+ * @param sock      - socket fd
+ * @param textbuf   - zero-trailing buffer with data to send
+ * @return amount of sent bytes
+ */
+static size_t send_data(int sock, const char *textbuf){
+    ssize_t Len = strlen(textbuf);
+    if(Len != write(sock, textbuf, Len)){
+        WARN("write()");
+        LOGERR("send_data(): write() failed");
+        return 0;
+    }else{
+        LOGDBG("send_data(): sent '%s'", textbuf);
+    }
+    if(textbuf[Len-1] != '\n') Len += write(sock, "\n", 1);
+    return (size_t)Len;
+}
+
+/**
+ * @brief handle_socket - read and process data from socket
+ * @param sock - socket fd
+ * @return 0 if all OK, 1 if socket closed
+ */
+static int handle_socket(int sock){
+    FNAME();
+    char buff[BUFLEN];
+    char ansbuff[ANSBUFLEN];
+    ssize_t rd = read(sock, buff, BUFLEN-1);
+    if(rd < 1){
+        DBG("read() == %zd", rd);
+        return 1;
+    }
+    // add trailing zero to be on the safe side
+    buff[rd] = 0;
+    // 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);
+    char *ans = processCommand(buff, ansbuff, ANSBUFLEN-1); // run command parser
+    if(ans){
+        send_data(sock, ans);   // send answer
+    }
+    //pthread_mutex_unlock(&mutex);
+    return 0;
+}
+
+// main socket server
+static void *server(void *asock){
+    DBG("server(): getpid: %d, tid: %lu",getpid(), syscall(SYS_gettid));
+    int sock = *((int*)asock);
+    if(listen(sock, BACKLOG) == -1){
+        LOGERR("server(): listen() failed");
+        WARN("listen");
+        return NULL;
+    }
+    int nfd = 1;
+    struct pollfd poll_set[BACKLOG+1];
+    memset(poll_set, 0, sizeof(poll_set));
+    poll_set[0].fd = sock;
+    poll_set[0].events = POLLIN;
+    while(1){
+        poll(poll_set, nfd, 1); // poll for 1ms
+        for(int fdidx = 0; fdidx < nfd; ++fdidx){ // poll opened FDs
+            if((poll_set[fdidx].revents & POLLIN) == 0) continue;
+            poll_set[fdidx].revents = 0;
+            if(fdidx){ // client
+                int fd = poll_set[fdidx].fd;
+                //int nread = 0;
+                //ioctl(fd, FIONREAD, &nread);
+                if(handle_socket(fd)){ // socket closed - remove it from list
+                    close(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];
+                    //for(int i = fdidx; i < nfd-1; ++i)
+                    //    poll_set[i] = poll_set[i + 1];
+                    --nfd;
+                }
+            }else{ // server
+                socklen_t size = sizeof(struct sockaddr_in);
+                struct sockaddr_in their_addr;
+                int newsock = accept(sock, (struct sockaddr*)&their_addr, &size);
+                if(newsock <= 0){
+                    LOGERR("server(): accept() failed");
+                    WARN("accept()");
+                    continue;
+                }
+                struct in_addr ipAddr = their_addr.sin_addr;
+                char str[INET_ADDRSTRLEN];
+                inet_ntop(AF_INET, &ipAddr, str, INET_ADDRSTRLEN);
+                DBG("Connection from %s, give fd=%d", str, newsock);
+                LOGMSG("Got connection from %s, fd=%d", str, newsock);
+                if(nfd == BACKLOG + 1){
+                    LOGWARN("Max amount of connections: disconnect %s (%d)", str, newsock);
+                    send_data(newsock, "Max amount of connections reached!");
+                    WARNX("Limit of connections reached");
+                    close(newsock);
+                }else{
+                    memset(&poll_set[nfd], 0, sizeof(struct pollfd));
+                    poll_set[nfd].fd = newsock;
+                    poll_set[nfd].events = POLLIN;
+                    ++nfd;
+                }
+            }
+        } // endfor
+        /*
+        char *srvmesg = mesgGetText(&ServerMessages); // broadcast messages to all clients
+        if(srvmesg){ // send broadcast message to all clients or throw them to /dev/null
+            for(int fdidx = 1; fdidx < nfd; ++fdidx){
+                send_data(poll_set[fdidx].fd, srvmesg);
+            }
+            FREE(srvmesg);
+        }
+        */
+    }
+    LOGERR("server(): UNREACHABLE CODE REACHED!");
+}
+
+// data gathering & socket management
+static void daemon_(int sock){
+    if(sock < 0) return;
+    pthread_t sock_thread;//, canserver_thread;
+    if(pthread_create(&sock_thread, NULL, server, (void*) &sock)
+       //|| pthread_create(&canserver_thread, NULL, CANserver, NULL)
+       ){
+        LOGERR("daemon_(): pthread_create() failed");
+        ERR("pthread_create()");
+    }
+    do{
+        if(pthread_kill(sock_thread, 0) == ESRCH){ // died
+            WARNX("Sockets thread died");
+            LOGERR("Sockets thread died");
+            pthread_join(sock_thread, NULL);
+            if(pthread_create(&sock_thread, NULL, server, (void*) &sock)){
+                LOGERR("daemon_(): new pthread_create(sock_thread) failed");
+                ERR("pthread_create(sock_thread)");
+            }
+        }
+        /*if(pthread_kill(canserver_thread, 0) == ESRCH){
+            WARNX("CANserver thread died");
+            LOGERR("CANserver thread died");
+            pthread_join(canserver_thread, NULL);
+            if(pthread_create(&canserver_thread, NULL, CANserver, NULL)){
+                LOGERR("daemon_(): new pthread_create(canserver_thread) failed");
+                ERR("pthread_create(canserver_thread)");
+            }
+        }*/
+        usleep(1000); // sleep a little or thread's won't be able to lock mutex
+        // copy temporary buffers to main
+        //pthread_mutex_lock(&mutex);
+        /*
+         * INSERT CODE HERE
+         * fill global data buffers
+         */
+        //pthread_mutex_unlock(&mutex);
+    }while(1);
+    LOGERR("daemon_(): UNREACHABLE CODE REACHED!");
+}
+
+/**
+ * open sockets
+ * // should be called only once!!!
+ */
+static void *connect2sock(void *data){
+    FNAME();
+    char port[10];
+    int portN = *((int*)data);
+    snprintf(port, 10, "%d", portN);
+    DBG("get port: %s", port);
+    int sock = -1;
+    struct addrinfo hints, *res, *p;
+    memset(&hints, 0, sizeof(hints));
+    hints.ai_family = AF_INET;
+    hints.ai_socktype = SOCK_STREAM;
+    hints.ai_flags = AI_PASSIVE;
+    if(getaddrinfo("127.0.0.1", port, &hints, &res) != 0){ // accept only local connections
+        LOGERR("daemonize(): getaddrinfo() failed");
+        ERR("getaddrinfo");
+    }
+    struct sockaddr_in *ia = (struct sockaddr_in*)res->ai_addr;
+    char str[INET_ADDRSTRLEN];
+    inet_ntop(AF_INET, &(ia->sin_addr), str, INET_ADDRSTRLEN);
+    // loop through all the results and bind to the first we can
+    for(p = res; p != NULL; p = p->ai_next){
+        if((sock = socket(p->ai_family, p->ai_socktype, p->ai_protocol)) == -1){
+            LOGWARN("openIOport(): socket() failed");
+            WARN("socket");
+            continue;
+        }
+        int reuseaddr = 1;
+        if(setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, &reuseaddr, sizeof(int)) == -1){
+            LOGERR("openIOport(): setsockopt() failed");
+            ERR("setsockopt");
+        }
+        if(bind(sock, p->ai_addr, p->ai_addrlen) == -1){
+            close(sock);
+            LOGERR("openIOport(): bind() failed");
+            WARN("bind");
+            continue;
+        }
+        break; // if we get here, we have a successfull connection
+    }
+    if(p == NULL){
+        LOGERR("openIOport(): failed to bind socket, exit");
+        // looped off the end of the list with no successful bind
+        ERRX("failed to bind socket");
+    }
+    freeaddrinfo(res);
+    daemon_(sock);
+    close(sock);
+    LOGERR("openIOport(): UNREACHABLE CODE REACHED!");
+    signals(22);
+    return NULL;
+}
+
+
+// run socket thread
+void openIOport(int portN){
+    static int portnum = 0;
+    if(portnum) return;
+    portnum = portN;
+    pthread_t connthread;
+    DBG("open port: %d", portN);
+    if(pthread_create(&connthread, NULL, connect2sock, (void*) &portnum)){
+        LOGERR("openIOport(): pthread_create() failed");
+        ERR("pthread_create()");
+    }
+}

LocCorr/socket.h

@@ -0,0 +1,29 @@
+/*
+ * This file is part of the loccorr project.
+ * Copyright 2021 Edward V. Emelianov <edward.emelianoff@gmail.com>.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+#ifndef __SOCKET_H__
+#define __SOCKET_H__
+
+// standard answers
+#define OK               "OK\n"
+#define FAIL             "FAILED\n"
+
+void openIOport(int portN);
+
+#endif // __SOCKET_H__