diff --git a/LocCorr/CANSERVER_pusirobo b/LocCorr/CANSERVER_pusirobo new file mode 100644 index 0000000..a7cf164 --- /dev/null +++ b/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 diff --git a/LocCorr/cmdlnopts.c b/LocCorr/cmdlnopts.c index 8af09e5..4a1c276 100644 --- a/LocCorr/cmdlnopts.c +++ b/LocCorr/cmdlnopts.c @@ -26,6 +26,8 @@ #include #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, - .medradius = 1., - .ndilations = 2, - .nerosions = 2, - .minarea = 5, - .intensthres = 0.01, - .maxexp = 500., - .minexp = 0.001, + .configname = DEFAULT_CONFFILE, + .pusiservport = DEFAULT_PUSIPORT, + .throwpart = DEFAULT_THROWPART, +// .medradius = 1., + .ndilations = DEFAULT_DILATIONS, + .nerosions = DEFAULT_EROSIONS, + .minarea = DEFAULT_MINAREA, + .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)")}, - {"neros", NEED_ARG, NULL, 'E', arg_int, APTR(&G.nerosions), _("amount of dilations after erosions (default: 2)")}, - {"minarea", NEED_ARG, NULL, 'A', arg_int, APTR(&G.minarea), _("minimal object pixels amount (default: 5)")}, + {"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 erosions after dilations (default: 2)")}, + {"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")}, - {"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")}, + {"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")}, {"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 }; diff --git a/LocCorr/cmdlnopts.h b/LocCorr/cmdlnopts.h index 5c78b8a..fef973e 100644 --- a/LocCorr/cmdlnopts.h +++ b/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; diff --git a/LocCorr/config.c b/LocCorr/config.c new file mode 100644 index 0000000..4327c3b --- /dev/null +++ b/LocCorr/config.c @@ -0,0 +1,389 @@ +/* + * This file is part of the loccorr project. + * Copyright 2021 Edward V. Emelianov . + * + * 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 . + */ + +#include +#include +#include +#include + +#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; +} diff --git a/LocCorr/config.h b/LocCorr/config.h new file mode 100644 index 0000000..b9b8066 --- /dev/null +++ b/LocCorr/config.h @@ -0,0 +1,104 @@ +/* + * This file is part of the loccorr project. + * Copyright 2021 Edward V. Emelianov . + * + * 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 . + */ +#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__ diff --git a/LocCorr/grasshopper.c b/LocCorr/grasshopper.c index c4354ac..8e1f267 100644 --- a/LocCorr/grasshopper.c +++ b/LocCorr/grasshopper.c @@ -24,9 +24,11 @@ #include #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.offsetY = (GP->yoff < (int)f7i.maxHeight && GP->yoff > -1) ? GP->yoff : 0; - f7.width = (f7.offsetX+GP->width <= f7i.maxWidth && GP->width > 1) ? (unsigned int)GP->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.offsetX = (theconf.xoff < (int)f7i.maxWidth && theconf.xoff > -1) ? theconf.xoff : 0; + f7.offsetY = (theconf.yoff < (int)f7i.maxHeight && theconf.yoff > -1) ? theconf.yoff : 0; + f7.width = (f7.offsetX+theconf.width <= f7i.maxWidth && theconf.width > 1) ? (unsigned int)theconf.width : f7i.maxWidth - f7.offsetX; + 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; - else if(newexp > GP->maxexp) newexp = GP->maxexp; + if(newexp < theconf.minexp) newexp = theconf.minexp; + 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; diff --git a/LocCorr/grasshopper.h b/LocCorr/grasshopper.h index de10feb..a121067 100644 --- a/LocCorr/grasshopper.h +++ b/LocCorr/grasshopper.h @@ -23,6 +23,7 @@ #define GRASSHOPPER_CAPT_NAME "grasshopper" +void disconnectGrasshopper(); int capture_grasshopper(void (*process)(Image *)); #endif // GRASSHOPPER_H__ diff --git a/LocCorr/imagefile.c b/LocCorr/imagefile.c index d2dde61..c098f93 100644 --- a/LocCorr/imagefile.c +++ b/LocCorr/imagefile.c @@ -28,6 +28,7 @@ #include #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; } diff --git a/LocCorr/improc.c b/LocCorr/improc.c index 0805e20..2db02a7 100644 --- a/LocCorr/improc.c +++ b/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" - -// 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.) +#include "pusirobo.h" 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); - char *pt = strrchr(flname, '.'); + snprintf(fname, PATH_MAX, name); + char *pt = strrchr(fname, '.'); if(pt) *pt = 0; - snprintf(fname, PATH_MAX, "%s.jpg", flname); - stbi_write_jpg(fname, I->width, I->height, 1, outp, 95); - FREE(outp); + strncat(fname, ".jpg", PATH_MAX); + Image_write_jpg(I, fname, theconf.equalize); 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 -static int compObjs(const void *a, const void *b){ +// functions for Qsort +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 - Xtarget = GP->xtarget; Ytarget = GP->ytarget; - } - static double Xc[AVERAGING_ARRAY_SIZE], Yc[AVERAGING_ARRAY_SIZE]; + int averflag = 0; + static double Xc[MAX_AVERAGING_ARRAY_SIZE], Yc[MAX_AVERAGING_ARRAY_SIZE]; + double xx = curobj->xc, yy = curobj->yc, xsum2 = 0., ysum2 = 0.; + 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){ - if(fXYlog) fprintf(fXYlog, "\n"); - return; + DBG("counter = %d", counter); + if(++counter != theconf.naverage){ + goto process_corrections; } // it's time to calculate average deviations - counter = 0; - double xdev = 0., ydev = 0., xsum2 = 0., ysum2 = 0.; - for(int i = 0; i < AVERAGING_ARRAY_SIZE; ++i){ - double dx = Xc[i] - Xtarget, dy = Yc[i] - Ytarget; - xdev += dx; ydev += dy; - xsum2 += dx*dx; ysum2 += dy*dy; + counter = 0; xx = 0.; yy = 0.; + for(int i = 0; i < theconf.naverage; ++i){ + double x = Xc[i], y = Yc[i]; + xx += x; yy += y; + xsum2 += x*x; ysum2 += y*y; } - xdev /= AVERAGING_ARRAY_SIZE; ydev /= AVERAGING_ARRAY_SIZE; - double Sx = sqrt(xsum2/AVERAGING_ARRAY_SIZE - xdev*xdev); - double Sy = sqrt(ysum2/AVERAGING_ARRAY_SIZE - ydev*ydev); - DBG("xtag=%g, ytag=%g", Xtarget, Ytarget); - green("\n\n\n Deviations: dX=%g (+-%g), dY=%g (+-%g)\n", xdev, Sx, ydev, Sy); - if(Xtarget < 0. || Ytarget < 0. || fabs(xdev) < XY_TOLERANCE || fabs(ydev) < XY_TOLERANCE){ - DBG("Target coordinates not defined or correction too small"); - if(fXYlog) fprintf(fXYlog, "\n"); - return; - } - if(fXYlog){ - fprintf(fXYlog, "%.1f\t%.1f\t%.1f\t%.1f\n", xdev, ydev, Sx, Sy); + xx /= theconf.naverage; yy /= theconf.naverage; + Sx = sqrt(xsum2/theconf.naverage - xx*xx); + Sy = sqrt(ysum2/theconf.naverage - yy*yy); + green("\n\n\n Average centroid: X=%g (+-%g), Y=%g (+-%g)\n", xx, Sx, yy, Sy); + LOGDBG("getDeviation(): Average centroid: X=%g (+-%g), Y=%g (+-%g)", xx, Sx, yy, Sy); + averflag = 1; + if(fXYlog) fprintf(fXYlog, "%.1f\t%.1f\t%.1f\t%.1f", xx, yy, Sx, Sy); +process_corrections: + if(proc_corr){ + if(Sx > 1. || Sy > 1.){ + LOGDBG("Bad value - not process"); // don't run processing for bad data + }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((int)b->area < GP->minarea) continue; + if(wh < MINWH || wh > MAXWH) 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) - outp = equalize(I, 3, GP->throwpart); + if(theconf.equalize) + outp = equalize(I, 3, theconf.throwpart); else outp = linear(I, 3); - Pattern *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); - stbi_write_jpg("outpWcrosses.jpg", I->width, I->height, 3, outp, 95); + 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); don't free - static variable! + } + 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); + } +} diff --git a/LocCorr/improc.h b/LocCorr/improc.h index f106056..2a8d134 100644 --- a/LocCorr/improc.h +++ b/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__ diff --git a/LocCorr/main.c b/LocCorr/main.c index 0a5d340..06f698c 100644 --- a/LocCorr/main.c +++ b/LocCorr/main.c @@ -16,26 +16,41 @@ * along with this program. If not, see . */ +#include #include // signal #include // strdup #include #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(); - if(GP && GP->pidfile) // remove unnesessary PID file - unlink(GP->pidfile); + DBG("exit %d", sig); 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; diff --git a/LocCorr/pusirobo.c b/LocCorr/pusirobo.c new file mode 100644 index 0000000..d80d34a --- /dev/null +++ b/LocCorr/pusirobo.c @@ -0,0 +1,748 @@ +/* + * This file is part of the loccorr project. + * Copyright 2021 Edward V. Emelianov . + * + * 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 . + */ +#define _GNU_SOURCE +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#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; +} diff --git a/LocCorr/pusirobo.h b/LocCorr/pusirobo.h new file mode 100644 index 0000000..6364ae4 --- /dev/null +++ b/LocCorr/pusirobo.h @@ -0,0 +1,44 @@ +/* + * This file is part of the loccorr project. + * Copyright 2021 Edward V. Emelianov . + * + * 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 . + */ +#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__ diff --git a/LocCorr/socket.c b/LocCorr/socket.c new file mode 100644 index 0000000..bbd4212 --- /dev/null +++ b/LocCorr/socket.c @@ -0,0 +1,408 @@ +/* + * This file is part of the loccorr project. + * Copyright 2021 Edward V. Emelianov . + * + * 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 . + */ + +#include // inet_ntop +#include +#include // INT_xxx +#include // addrinfo +#include +#include +#include // pthread_kill +#include +#include +#include // syscall +#include // daemon +#include + +#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()"); + } +} diff --git a/LocCorr/socket.h b/LocCorr/socket.h new file mode 100644 index 0000000..00b6756 --- /dev/null +++ b/LocCorr/socket.h @@ -0,0 +1,29 @@ +/* + * This file is part of the loccorr project. + * Copyright 2021 Edward V. Emelianov . + * + * 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 . + */ + +#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__