eddy/astrovideoguide_v3
1
0
Fork 0
mirror of https://github.com/eddyem/astrovideoguide_v3.git synced 2025-12-06 02:35:11 +03:00
Code Issues Packages Projects Releases Wiki Activity

LocCorr - pre-alpha version of INASAN corrector

Browse Source
This commit is contained in:
Edward Emelianov 2021-06-09 12:29:39 +03:00
parent cff48d3583
commit c55b407cf8
15 changed files with 2042 additions and 125 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
LocCorr/CANSERVER_pusirobo Normal file
View File

@ -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

54
LocCorr/cmdlnopts.c
View File

@ -26,6 +26,8 @@
#include <usefull_macros.h> #include <usefull_macros.h>
#include "cmdlnopts.h" #include "cmdlnopts.h"
#include "config.h"
#include "improc.h"
/* /*
* here are global parameters initialisation * here are global parameters initialisation
@ -33,23 +35,26 @@
static int help; static int help;
glob_pars *GP = NULL; glob_pars *GP = NULL;
// default PID filename:
#define DEFAULT_PIDFILE "/tmp/loccorr.pid"
// DEFAULTS // DEFAULTS
// default global parameters // default global parameters
static glob_pars G = { static glob_pars G = {
.pidfile = DEFAULT_PIDFILE, .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., .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[] = { static myoption cmdlnopts[] = {
// common options // 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)")}, {"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 ")")}, {"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)")}, {"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)")}, {"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")}, {"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")}, {"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")}, {"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")}, {"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")}, {"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")}, {"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")}, {"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")}, {"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 end_option
}; };

35
LocCorr/cmdlnopts.h
View File

@ -23,6 +23,24 @@
#ifndef CMDLNOPTS_H__ #ifndef CMDLNOPTS_H__
#define 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 * here are some typedef's for global data
*/ */
@ -31,18 +49,25 @@ typedef struct{
char *logfile; // logging to this file char *logfile; // logging to this file
char *inputname; // input for monitor file or directory name char *inputname; // input for monitor file or directory name
char *logXYname; // file to log XY coordinates of first point 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 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 verb; // logfile verbosity level
int ndilations; // amount of erosions (default: 2) int ndilations; // amount of erosions (default: 2)
int nerosions; // amount of dilations (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 intensthres; // threshold by total object intensity when sorting = |I1-I2|/(I1+I2), default: 0.01
double maxexp; // max exposition time (ms) double maxexp; // max exposition time (ms)
double minexp; // min 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 double xtarget; double ytarget;// target point coordinates
} glob_pars; } glob_pars;

389
LocCorr/config.c Normal file
View File

@ -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;
}

104
LocCorr/config.h Normal file
View File

@ -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__

30
LocCorr/grasshopper.c
View File

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

1
LocCorr/grasshopper.h
View File

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

15
LocCorr/imagefile.c
View File

@ -28,6 +28,7 @@
#include <stb/stb_image_write.h> #include <stb/stb_image_write.h>
#include "cmdlnopts.h" #include "cmdlnopts.h"
#include "config.h"
#include "draw.h" #include "draw.h"
#include "grasshopper.h" #include "grasshopper.h"
#include "imagefile.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; if(!I || !I->data) return 0;
uint8_t *outp = NULL; uint8_t *outp = NULL;
if(eq) if(eq)
outp = equalize(I, 1, GP->throwpart); outp = equalize(I, 1, theconf.throwpart);
else else
outp = linear(I, 1); outp = linear(I, 1);
DBG("Try to write %s", name); 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); FREE(outp);
return r; return r;
} }

189
LocCorr/improc.c
View File

@ -25,22 +25,28 @@
#include "binmorph.h" #include "binmorph.h"
#include "cmdlnopts.h" #include "cmdlnopts.h"
#include "config.h"
#include "draw.h" #include "draw.h"
#include "grasshopper.h" #include "grasshopper.h"
#include "fits.h" #include "fits.h"
#include "improc.h" #include "improc.h"
#include "inotify.h" #include "inotify.h"
#include "median.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 FILE *fXYlog = NULL;
static double Xtarget = -1., Ytarget = -1.; // target coordinates
static double tstart = 0.; // time of logging start 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{ typedef struct{
uint32_t area; // object area in pixels uint32_t area; // object area in pixels
@ -52,19 +58,20 @@ typedef struct{
double ysigma; double ysigma;
} object; } object;
typedef enum{
PROCESS_NONE,
PROCESS_PUSIROBO
} postproc_type;
static postproc_type postprocess = PROCESS_NONE;
static bool save_fits(Image *I, const char *name){ 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 fname[PATH_MAX];
char *flname = strdup(name); snprintf(fname, PATH_MAX, name);
char *pt = strrchr(flname, '.'); char *pt = strrchr(fname, '.');
if(pt) *pt = 0; 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); unlink(name);
return FITS_write(name, I); 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 *oa = (const object*)a;
const object *ob = (const object*)b; const object *ob = (const object*)b;
double idiff = (oa->Isum - ob->Isum)/(oa->Isum + ob->Isum); 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 r2a = oa->xc * oa->xc + oa->yc * oa->yc;
double r2b = ob->xc * ob->xc + ob->yc * ob->yc; double r2b = ob->xc * ob->xc + ob->yc * ob->yc;
return (r2a < r2b) ? -1 : 1; 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){ 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; static int counter = 0;
Xc[counter] = curobj->xc; Yc[counter] = curobj->yc; Xc[counter] = curobj->xc; Yc[counter] = curobj->yc;
if(fXYlog){ // make log record if(fXYlog){ // make log record
@ -100,31 +122,32 @@ static void getDeviation(object *curobj){
dtime() - tstart, curobj->xc, curobj->yc, dtime() - tstart, curobj->xc, curobj->yc,
curobj->xsigma, curobj->ysigma, curobj->WdivH); 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 // 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){ void process_file(Image *I){
@ -135,7 +158,6 @@ void process_file(Image *I){
#define DELTA(x) #define DELTA(x)
#endif #endif
// I - original image // I - original image
// M - median filtering
// mean - local mean // mean - local mean
// std - local STD // std - local STD
/**** read original image ****/ /**** read original image ****/
@ -148,15 +170,6 @@ void process_file(Image *I){
if(!I->dtype) I->dtype = FLOAT_IMG; if(!I->dtype) I->dtype = FLOAT_IMG;
save_fits(I, "fitsout.fits"); save_fits(I, "fitsout.fits");
DELTA("Save original"); 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; Imtype bk;
if(calc_background(I, &bk)){ if(calc_background(I, &bk)){
DBG("backgr = %g", bk); DBG("backgr = %g", bk);
@ -166,12 +179,12 @@ void process_file(Image *I){
if(ibin){ if(ibin){
savebin(ibin, W, H, "binary.fits"); savebin(ibin, W, H, "binary.fits");
DELTA("save 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); FREE(ibin);
DELTA("Erosion"); DELTA("Erosion");
savebin(er, W, H, "erosion.fits"); savebin(er, W, H, "erosion.fits");
DELTA("Save erosion"); DELTA("Save erosion");
uint8_t *opn = dilationN(er, W, H, GP->ndilations); uint8_t *opn = dilationN(er, W, H, theconf.Ndilations);
FREE(er); FREE(er);
DELTA("Opening"); DELTA("Opening");
savebin(opn, W, H, "opening.fits"); 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); double wh = ((double)b->xmax - b->xmin)/(b->ymax - b->ymin);
//DBG("Obj# %zd: wh=%g, area=%d", i, wh, b->area); //DBG("Obj# %zd: wh=%g, area=%d", i, wh, b->area);
// TODO: change magick numbers to parameters // 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 xc = 0., yc = 0.;
double x2c = 0., y2c = 0., Isum = 0.; double x2c = 0., y2c = 0., Isum = 0.;
for(size_t y = b->ymin; y <= b->ymax; ++y){ for(size_t y = b->ymin; y <= b->ymax; ++y){
@ -218,7 +231,12 @@ void process_file(Image *I){
} }
DELTA("Labeling"); DELTA("Labeling");
printf("T%zd, N=%d\n", time(NULL), objctr); 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; object *o = Objects;
green("%6s\t%6s\t%6s\t%6s\t%6s\t%6s\t%6s\t%6s\n", 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"); "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); i, o->area, 20.-1.0857*log(o->Isum), o->WdivH, o->xc, o->yc, o->xsigma, o->ysigma);
} }
getDeviation(Objects); getDeviation(Objects);
if(objctr){ // draw crosses @ objects' centers { // prepare image
uint8_t *outp = NULL; uint8_t *outp = NULL;
if(GP->equalize) if(theconf.equalize)
outp = equalize(I, 3, GP->throwpart); outp = equalize(I, 3, theconf.throwpart);
else else
outp = linear(I, 3); outp = linear(I, 3);
Pattern *cross = Pattern_cross(33, 33); if(objctr){ // draw crosses @ objects' centers
int H = I->height; static Pattern *cross = NULL;
Img3 i3 = {.data = outp, .w = I->width, .h = H}; if(!cross) cross = Pattern_cross(33, 33);
Pattern_draw3(&i3, cross, Objects[0].xc, H-Objects[0].yc, C_G); int H = I->height;
for(int i = 1; i < objctr; ++i) Img3 i3 = {.data = outp, .w = I->width, .h = H};
Pattern_draw3(&i3, cross, Objects[i].xc, H-Objects[i].yc, C_R); Pattern_draw3(&i3, cross, Objects[0].xc, H-Objects[0].yc, C_G);
Pattern_free(&cross); for(int i = 1; i < objctr; ++i)
stbi_write_jpg("outpWcrosses.jpg", I->width, I->height, 3, outp, 95); 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(outp);
} }
FREE(cc); FREE(cc);
@ -273,6 +294,7 @@ void process_file(Image *I){
} }
int process_input(InputType tp, char *name){ int process_input(InputType tp, char *name){
DBG("process_input(%d, %s)", tp, name);
if(tp == T_DIRECTORY) return watch_directory(name, process_file); if(tp == T_DIRECTORY) return watch_directory(name, process_file);
else if(tp == T_CAPT_GRASSHOPPER) return capture_grasshopper(process_file); else if(tp == T_CAPT_GRASSHOPPER) return capture_grasshopper(process_file);
return watch_file(name, process_file); return watch_file(name, process_file);
@ -293,7 +315,8 @@ void openXYlog(const char *name){
} }
time_t t = time(NULL); time_t t = time(NULL);
fprintf(fXYlog, "# Start at: %s", ctime(&t)); 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(); tstart = dtime();
} }
void closeXYlog(){ void closeXYlog(){
@ -301,3 +324,27 @@ void closeXYlog(){
fclose(fXYlog); fclose(fXYlog);
fXYlog = NULL; 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);
}
}

17
LocCorr/improc.h
View File

@ -21,9 +21,26 @@
#include "imagefile.h" #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); void process_file(Image *I);
int process_input(InputType tp, char *name); int process_input(InputType tp, char *name);
void openXYlog(const char *name); void openXYlog(const char *name);
void closeXYlog(); 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__ #endif // IMPROC_H__

95
LocCorr/main.c
View File

@ -16,26 +16,41 @@
* along with this program. If not, see <http://www.gnu.org/licenses/>. * along with this program. If not, see <http://www.gnu.org/licenses/>.
*/ */
#include <math.h>
#include <signal.h> // signal #include <signal.h> // signal
#include <string.h> // strdup #include <string.h> // strdup
#include <usefull_macros.h> #include <usefull_macros.h>
#include "cmdlnopts.h" #include "cmdlnopts.h"
#include "config.h"
#include "grasshopper.h"
#include "improc.h" #include "improc.h"
#include "pusirobo.h"
#include "socket.h"
/** /**
* We REDEFINE the default WEAK function of signal processing * We REDEFINE the default WEAK function of signal processing
*/ */
void signals(int sig){ void signals(int sig){
DBG("exit %d", sig);
if(sig){ if(sig){
signal(sig, SIG_IGN); signal(sig, SIG_IGN);
DBG("Get signal %d, quit.\n", sig); 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); 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); exit(sig);
} }
@ -88,16 +103,10 @@ int main(int argc, char *argv[]){
if(GP->throwpart < 0. || GP->throwpart > 0.99){ if(GP->throwpart < 0. || GP->throwpart > 0.99){
ERRX("Fraction of black pixels should be in [0., 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); InputType tp = chk_inp(GP->inputname);
if(tp == T_WRONG) ERRX("Enter correct image file or directory name"); 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){ if(GP->logfile){
sl_loglevel lvl = LOGLEVEL_ERR; // default log level - errors sl_loglevel lvl = LOGLEVEL_ERR; // default log level - errors
int v = GP->verb; int v = GP->verb;
@ -107,10 +116,70 @@ int main(int argc, char *argv[]){
OPENLOG(GP->logfile, lvl, 1); OPENLOG(GP->logfile, lvl, 1);
DBG("Opened log file @ level %d", lvl); 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); if(GP->logXYname) openXYlog(GP->logXYname);
LOGMSG("Start application..."); 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); int p = process_input(tp, GP->inputname);
DBG("process_input=%d", p);
// never reached // never reached
signals(p); // clean everything signals(p); // clean everything
return p; return p;

748
LocCorr/pusirobo.c Normal file
View File

@ -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;
}

44
LocCorr/pusirobo.h Normal file
View File

@ -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__

408
LocCorr/socket.c Normal file
View File

@ -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()");
}
}

29
LocCorr/socket.h Normal file
View File

@ -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__