From 7a0acd14f50ebc72c45397f161141f4e0940ee22 Mon Sep 17 00:00:00 2001 From: Edward Emelianov Date: Fri, 14 Mar 2025 16:42:52 +0300 Subject: [PATCH] seems like it works good; need tests on real star --- LocCorr_new/basler.c | 11 +- LocCorr_new/cameracapture.c | 140 ++++++++++++++++++++----- LocCorr_new/cmdlnopts.c | 3 +- LocCorr_new/cmdlnopts.h | 1 + LocCorr_new/config.c | 126 +++++++++++++++------- LocCorr_new/config.h | 17 ++- LocCorr_new/imagefile.c | 49 +++++---- LocCorr_new/imagefile.h | 11 +- LocCorr_new/improc.c | 204 +++++++++++++++++++++++++++--------- LocCorr_new/improc.h | 2 + LocCorr_new/loccorr.conf | 68 ++++++------ LocCorr_new/main.c | 14 ++- LocCorr_new/socket.c | 134 ++++++++++++++++++----- LocCorr_new/steppers.c | 131 +++++++++++++++++------ LocCorr_new/steppers.h | 3 + 15 files changed, 678 insertions(+), 236 deletions(-) diff --git a/LocCorr_new/basler.c b/LocCorr_new/basler.c index bd65759..0b52f12 100644 --- a/LocCorr_new/basler.c +++ b/LocCorr_new/basler.c @@ -216,14 +216,19 @@ static int connect(){ } static Image *capture(){ - FNAME(); + //FNAME(); static int toohot = FALSE; if(!isopened || !imgBuf) return NULL; float_values f; + static double t0 = 0.; if(!getFloat("DeviceTemperature", &f)) WARNX("Can't get temperature"); else{ - LOGDBG("Basler temperature: %.1f", f.val); - DBG("Temperature: %.1f", f.val); + double t = dtime(); + if(t - t0 >= 30.){ // log T each 30 seconds + LOGMSG("Basler temperature: %.1f", f.val); + t0 = t; + } + //DBG("Temperature: %.1f", f.val); if(f.val > 80.){ WARNX("Device too hot"); if(!toohot){ diff --git a/LocCorr_new/cameracapture.c b/LocCorr_new/cameracapture.c index 843b780..41edfe1 100644 --- a/LocCorr_new/cameracapture.c +++ b/LocCorr_new/cameracapture.c @@ -18,6 +18,7 @@ #include // FLT_EPSILON #include +#include #include #include @@ -122,6 +123,9 @@ static void calcexpgain(float newexp){ //convertedImage.pData, convertedImage.cols, convertedImage.rows, convertedImage.stride static void recalcexp(Image *I){ +#ifdef EBUG + green("RECALCEXP\n"); fflush(stdout); +#endif // check if user changed exposition values if(exptime < theconf.minexp){ exptime = theconf.minexp; @@ -131,36 +135,116 @@ static void recalcexp(Image *I){ exptime = theconf.maxexp; return; } - size_t *histogram = get_histogram(I); - if(!histogram){ + size_t histogram[HISTOSZ]; + if(!get_histogram(I, histogram)){ WARNX("Can't calculate histogram"); return; } int idx100; size_t sum100 = 0; - for(idx100 = 255; idx100 >= 0; --idx100){ + for(idx100 = HISTOSZ-1; idx100 >= 0; --idx100){ sum100 += histogram[idx100]; if(sum100 > 100) break; } - FREE(histogram); DBG("Sum100=%zd, idx100=%d", sum100, idx100); - if(idx100 > 230 && idx100 < 253) return; // good values + if(idx100 > 230 && idx100 < 253){ + DBG("idx100=%d - good", idx100); + return; // good values + } if(idx100 > 253){ // exposure too long + DBG("Exp too long"); calcexpgain(0.7*exptime); }else{ // exposure too short - if(idx100 > 5) + if(idx100 > 5){ + DBG("Exp too short"); calcexpgain(exptime * 230. / (float)idx100); - else + }else{ + DBG("divide exp by 2"); calcexpgain(exptime * 50.); + } } } +static int needs_exposure_adjustment(const Image *I, float curr_x, float curr_y) { + static float last_avg_intensity = -1.f; + static float last_centroid_x = -1.f, last_centroid_y = -1.f; + float avg = I->avg_intensity; + float dx = fabsf(curr_x - last_centroid_x); + float dy = fabsf(curr_y - last_centroid_y); + // Adjust if intensity changes >10% or centroid moves >20px or no x/y centroids + if(curr_x < 0.f || curr_y < 0.f) return TRUE; + if(fabsf(avg - last_avg_intensity) > 0.1f * last_avg_intensity || + dx > 20.f || dy > 20.f){ + DBG("avg_cur=%g, avg_last=%g, dx=%g, dy=%g", avg, last_avg_intensity, dx, dy); + last_avg_intensity = avg; + last_centroid_x = curr_x; + last_centroid_y = curr_y; + return TRUE; + } + return FALSE; +} + +static pthread_mutex_t capt_mutex = PTHREAD_MUTEX_INITIALIZER; +static int iCaptured = -1; // index of last captured image +static Image* Icap[2] = {0}; // buffer for last captured images +// main capture thread fills empty buffers and wait until processed thread free's one of them +static void *procthread(void* v){ + typedef void (*procfn_t)(Image*); + void (*process)(Image*) = (procfn_t)v; +#ifdef EBUG + double t0 = dtime(); +#endif + while(!stopwork){ + while(iCaptured < 0) usleep(1000); + pthread_mutex_lock(&capt_mutex); + if(Icap[iCaptured]){ + DBG("---- got image #%d @ %g", iCaptured, dtime() - t0); + Image *oIma = Icap[iCaptured]; // take image here and free buffer + Icap[iCaptured] = NULL; + pthread_mutex_unlock(&capt_mutex); + if(theconf.expmethod == EXPAUTO){ + float xc, yc; + getcenter(&xc, &yc); + if(needs_exposure_adjustment(oIma, xc, yc)) recalcexp(oIma); + }else{ + if(fabs(theconf.fixedexp - exptime) > FLT_EPSILON) + exptime = theconf.fixedexp; + if(fabs(theconf.gain - gain) > FLT_EPSILON) + gain = theconf.gain; + if(fabs(theconf.brightness - brightness) > FLT_EPSILON) + brightness = theconf.brightness; + } + if(process){ + if(theconf.medfilt){ + Image *X = get_median(oIma, theconf.medseed); + if(X){ + FREE(oIma->data); + FREE(oIma); + oIma = X; + } + } + process(oIma); + } + FREE(oIma->data); + FREE(oIma); + DBG("---- cleared image data @ %g", dtime() - t0); + }else pthread_mutex_unlock(&capt_mutex); + usleep(1000); + } + return NULL; +} + int camcapture(void (*process)(Image*)){ FNAME(); static float oldexptime = 0.; static float oldgain = -1.; static float oldbrightness = -1.; Image *oIma = NULL; + pthread_t proc_thread; + if(pthread_create(&proc_thread, NULL, procthread, (void*)process)){ + LOGERR("pthread_create() for image processing failed"); + ERR("pthread_create()"); + } while(1){ if(stopwork){ DBG("STOP"); @@ -213,35 +297,37 @@ int camcapture(void (*process)(Image*)){ camdisconnect(); continue; } - if(theconf.expmethod == EXPAUTO) recalcexp(oIma); - else{ - if(fabs(theconf.fixedexp - exptime) > FLT_EPSILON) - exptime = theconf.fixedexp; - if(fabs(theconf.gain - gain) > FLT_EPSILON) - gain = theconf.gain; - if(fabs(theconf.brightness - brightness) > FLT_EPSILON) - brightness = theconf.brightness; + pthread_mutex_lock(&capt_mutex); + if(iCaptured < 0) iCaptured = 0; + else iCaptured = !iCaptured; + if(Icap[iCaptured]){ // try current value if previous is still busy + iCaptured = !iCaptured; } - if(process){ - if(theconf.medfilt){ - Image *X = get_median(oIma, theconf.medseed); - if(X){ - FREE(oIma->data); - FREE(oIma); - oIma = X; - } - } - process(oIma); + if(!Icap[iCaptured]){ // previous buffer is free + DBG("---- take iCaptured=%d", iCaptured); + Icap[iCaptured] = oIma; + oIma = NULL; + }else{ // clear our image - there's no empty buffers + DBG("---- no free buffers"); + FREE(oIma->data); + FREE(oIma); } - FREE(oIma->data); - FREE(oIma); + pthread_mutex_unlock(&capt_mutex); } + pthread_cancel(proc_thread); if(oIma){ FREE(oIma->data); FREE(oIma); } + for(int i = 0; i < 2; ++i){ + if(Icap[i]){ + FREE(Icap[i]->data); + FREE(Icap[i]); + } + } camdisconnect(); DBG("CAMCAPTURE: out"); + pthread_join(proc_thread, NULL); return 1; } diff --git a/LocCorr_new/cmdlnopts.c b/LocCorr_new/cmdlnopts.c index 6e98116..e9414e3 100644 --- a/LocCorr_new/cmdlnopts.c +++ b/LocCorr_new/cmdlnopts.c @@ -66,6 +66,7 @@ static myoption cmdlnopts[] = { {"maxexp", NEED_ARG, NULL, 0, arg_double, APTR(&G.maxexp), _("maximal exposition time (ms), default: 500")}, {"minexp", NEED_ARG, NULL, 0, arg_double, APTR(&G.minexp), _("minimal exposition time (ms), default: 0.001")}, {"help", NO_ARGS, NULL, 'h', arg_int, APTR(&help), _("show this help")}, + {"chkconf", NO_ARGS, NULL, 'C', arg_int, APTR(&G.chkconf), _("check configuration file")}, {"logfile", NEED_ARG, NULL, 'l', arg_string, APTR(&G.logfile), _("file to save logs (default: none)")}, {"pidfile", NEED_ARG, NULL, 'P', arg_string, APTR(&G.pidfile), _("pidfile (default: " DEFAULT_PIDFILE ")")}, {"verbose", NO_ARGS, NULL, 'v', arg_none, APTR(&G.verb), _("increase verbosity level of log file (each -v increased by 1)")}, @@ -86,7 +87,7 @@ static myoption cmdlnopts[] = { {"ytarget", NEED_ARG, NULL, 'Y', arg_double, APTR(&G.ytarget), _("target point Y coordinate")}, {"logXY", NEED_ARG, NULL, 'L', arg_string, APTR(&G.logXYname), _("file to log XY coordinates of selected star")}, {"confname",NEED_ARG, NULL, 'c', arg_string, APTR(&G.configname),_("name of configuration file (default: ./loccorr.conf)")}, - {"canport", NEED_ARG, NULL, 'C', arg_string, APTR(&G.steppersport),_("port of local pusirobot CAN server (default: 4444)")}, + {"stpport", NEED_ARG, NULL, 'S', arg_string, APTR(&G.steppersport),_("port of local steppers server (default: 4444)")}, {"naverage",NEED_ARG, NULL, 'N', arg_int, APTR(&G.Naveraging),_("amount of images to average processing (min 2, max 25)")}, {"ioport", NEED_ARG, NULL, 0, arg_int, APTR(&G.ioport), _("port for IO communication")}, {"jpegout", NEED_ARG, NULL, 'j', arg_string, APTR(&G.outputjpg), _("output jpeg file location (default: '" DEFAULT_OUTPJPEG "')")}, diff --git a/LocCorr_new/cmdlnopts.h b/LocCorr_new/cmdlnopts.h index f35e733..ce4e6e0 100644 --- a/LocCorr_new/cmdlnopts.h +++ b/LocCorr_new/cmdlnopts.h @@ -54,6 +54,7 @@ typedef struct{ int steppersport; // port of local motors CAN server int equalize; // make historam equalization of saved jpeg // int medradius; // radius of median filter (r=1 -> 3x3, r=2 -> 5x5 etc.) + int chkconf; // check config file int verb; // logfile verbosity level int ndilations; // amount of erosions (default: 2) int nerosions; // amount of dilations (default: 2) diff --git a/LocCorr_new/config.c b/LocCorr_new/config.c index 44d98e5..1566900 100644 --- a/LocCorr_new/config.c +++ b/LocCorr_new/config.c @@ -51,6 +51,12 @@ configuration theconf = { .Kyu=0, .Kxv=0, .Kyv=0, + .PIDU_P = PID_P_DEFAULT, + .PIDU_I = PID_I_DEFAULT, + .PIDU_D = PID_D_DEFAULT, + .PIDV_P = PID_P_DEFAULT, + .PIDV_I = PID_I_DEFAULT, + .PIDV_D = PID_D_DEFAULT, .xtarget=-1, .ytarget=-1, .throwpart=DEFAULT_THROWPART, @@ -62,6 +68,12 @@ configuration theconf = { .medseed=MIN_MEDIAN_SEED, }; +static int isSorted = 0; // ==1 when `parvals` are sorted +static int compConfVals(const void *_1st, const void *_2nd){ + const confparam *a = (confparam*)_1st, *b = (confparam*)_2nd; + return strcmp(a->name, b->name); +} + // {"", PAR_DOUBLE, (void*)&theconf., 0}, static confparam parvals[] = { {"maxarea", PAR_INT, (void*)&theconf.maxarea, 0, MINAREA, MAXAREA, @@ -116,6 +128,18 @@ static confparam parvals[] = { "X coordinate of target position"}, {"ytarget", PAR_DOUBLE, (void*)&theconf.ytarget, 0, 1., MAX_OFFSET, "Y coordinate of target position"}, + {"pidup", PAR_DOUBLE, (void*)&theconf.PIDU_P, 0, PID_P_MIN, PID_P_MAX, + "U axis P PID parameter"}, + {"pidui", PAR_DOUBLE, (void*)&theconf.PIDU_I, 0, PID_I_MIN, PID_I_MAX, + "U axis I PID parameter"}, + {"pidud", PAR_DOUBLE, (void*)&theconf.PIDU_D, 0, PID_I_MIN, PID_I_MAX, + "U axis D PID parameter"}, + {"pidvp", PAR_DOUBLE, (void*)&theconf.PIDV_P, 0, PID_P_MIN, PID_P_MAX, + "V axis P PID parameter"}, + {"pidvi", PAR_DOUBLE, (void*)&theconf.PIDV_I, 0, PID_I_MIN, PID_I_MAX, + "V axis I PID parameter"}, + {"pidvd", PAR_DOUBLE, (void*)&theconf.PIDV_D, 0, PID_I_MIN, PID_I_MAX, + "V axis D PID parameter"}, {"eqthrowpart", PAR_DOUBLE, (void*)&theconf.throwpart, 0, 0., MAX_THROWPART, "a part of low intensity pixels to throw away when histogram equalized"}, {"minexp", PAR_DOUBLE, (void*)&theconf.minexp, 0, 0., EXPOS_MAX, @@ -148,6 +172,10 @@ char *get_cmd_list(char *buff, int l){ if(!buff || l < 1) return NULL; int L = l; char *ptr = buff; + if(!isSorted){ + qsort(parvals, sizeof(parvals)/sizeof(confparam) - 1, sizeof(confparam), compConfVals); + isSorted = 1; + } confparam *par = parvals; while(L > 0 && par->name){ int s = snprintf(ptr, L, "%s=newval - %s (from %g to %g)\n", par->name, par->help, par->minval, par->maxval); @@ -161,10 +189,8 @@ char *get_cmd_list(char *buff, int l){ static char *omitspaces(char *v){ if(!v) return NULL; while(*v && (*v == ' ' || *v == '\t')) ++v; - char *ptr = strchr(v, ' '); - if(ptr) *ptr = 0; - ptr = strchr(v, '\t'); - if(ptr) *ptr = 0; + int l = strlen(v); + while(l-- && (v[l] == ' ' || v[l] == '\t')) v[l] = 0; return v; } @@ -173,12 +199,17 @@ static char *omitspaces(char *v){ char *get_keyval(const char *pair, char value[128]){ char key[128]; char val[128]; - if(!pair || !*pair) return strdup("#"); // empty line + //if(!pair || !*pair) return strdup("#"); // empty line + if(!pair || !*pair){ + //DBG("Empty"); + return NULL; // empty line + } char *keyptr = key, *valptr = val; int x = sscanf(pair, "%127[^=]=%127[^\n]%*c", key, val); //DBG("x=%d, key='%s', val='%s'", x, key, val); if(x < 0 || x > 2) return NULL; // wrong data or EOF - if(x == 0) return strdup("#"); // empty line + //if(x == 0) return strdup("#"); // empty line + if(x == 0) return NULL; // empty line keyptr = omitspaces(key); if(x == 2){ // param = value valptr = omitspaces(val); @@ -187,7 +218,8 @@ char *get_keyval(const char *pair, char value[128]){ } if(*keyptr == '#' || *keyptr == '%'){ // comment *value = 0; - return strdup("#"); + //return strdup("#"); + return NULL; } return NULL; } @@ -219,6 +251,17 @@ static int str2int(int *num, const char *str){ return TRUE; } +// find configuration record for getter +confparam *find_key(const char *key){ + if(!key) return NULL; + confparam *par = parvals; + while(par->name){ + if(strcmp(key, par->name) == 0) return par; + ++par; + } + return NULL; +} + /** * @brief chk_keyval - check key for presence in theconf and calculate its value * @param key (i) - keyword @@ -228,39 +271,36 @@ static int str2int(int *num, const char *str){ */ confparam *chk_keyval(const char *key, const char *val, key_value *result){ if(!key || !val || !result) return NULL; - confparam *par = parvals; - 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) - WARNX("Value (%d) of parameter %s out of range %g..%g", - result->val.intval, par->name, par->minval, par->maxval); - else return par; - 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) - WARNX("Value (%g) of parameter %s out of range %g..%g", - result->val.dblval, par->name, par->minval, par->maxval); - else return par; - break; + confparam *par = find_key(key); + if(!par) return NULL; + //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; } - return NULL; - } - ++par; + if(result->val.intval < par->minval || result->val.intval > par->maxval){ + WARNX("Value (%d) of parameter %s out of range %g..%g", + result->val.intval, par->name, par->minval, par->maxval); + break; + } else return par; + 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){ + WARNX("Value (%g) of parameter %s out of range %g..%g", + result->val.dblval, par->name, par->minval, par->maxval); + break; + } else return par; + break; } return NULL; } @@ -352,6 +392,10 @@ int saveconf(const char *confname){ LOGERR("Can't open %s to store configuration", confname); return FALSE; } + if(!isSorted){ + qsort(parvals, sizeof(parvals)/sizeof(confparam) - 1, sizeof(confparam), compConfVals); + isSorted = 1; + } confparam *par = parvals; while(par->name){ par->got = 1; @@ -377,6 +421,10 @@ int saveconf(const char *confname){ char *listconf(const char *messageid, char *buf, int buflen){ int L; char *ptr = buf; + if(!isSorted){ + qsort(parvals, sizeof(parvals)/sizeof(confparam) - 1, sizeof(confparam), compConfVals); + isSorted = 1; + } confparam *par = parvals; L = snprintf(ptr, buflen, "{ \"%s\": \"%s\", ", MESSAGEID, messageid); buflen -= L; ptr += L; diff --git a/LocCorr_new/config.h b/LocCorr_new/config.h index bea5700..d4062fa 100644 --- a/LocCorr_new/config.h +++ b/LocCorr_new/config.h @@ -46,7 +46,7 @@ #define KUVMIN (-5000.) #define KUVMAX (5000.) // default coefficient for corrections (move to Kdu, Kdv instead of du, dv) -#define KCORR (0.90) +//#define KCORR (0.90) // min/max median seed #define MIN_MEDIAN_SEED (1) #define MAX_MEDIAN_SEED (7) @@ -62,6 +62,17 @@ #define MINWH (0.3) #define MAXWH (3.) +// PID limits +#define PID_P_MIN (0.1) +#define PID_P_MAX (3.) +#define PID_P_DEFAULT (0.7) +#define PID_I_MIN (0.) +#define PID_I_MAX (3.) +#define PID_I_DEFAULT (0.1) +#define PID_D_MIN (0.) +#define PID_D_MAX (5.) +#define PID_D_DEFAULT (0.05) + // messageID field name #define MESSAGEID "messageid" @@ -102,6 +113,9 @@ typedef struct{ double gain; // gain value in manual mode double brightness; // brightness @camera double intensthres; // threshold for stars intensity comparison: fabs(Ia-Ib)/(Ia+Ib) > thres -> stars differs + // PID regulator for axes U and V + double PIDU_P; double PIDU_I; double PIDU_D; + double PIDV_P; double PIDV_I; double PIDV_D; } configuration; typedef enum{ @@ -135,6 +149,7 @@ int chkconfig(const char *confname); int saveconf(const char *confname); char *get_keyval(const char *pair, char value[128]); confparam *chk_keyval(const char *key, const char *val, key_value *result); +confparam *find_key(const char *key); char *listconf(const char *messageid, char *buf, int buflen); #endif // CONFIG_H__ diff --git a/LocCorr_new/imagefile.c b/LocCorr_new/imagefile.c index 63d00d6..bd0cde6 100644 --- a/LocCorr_new/imagefile.c +++ b/LocCorr_new/imagefile.c @@ -137,7 +137,7 @@ InputType chkinput(const char *name){ * @return Image structure (fully allocated, you can FREE(data) after it) */ Image *u8toImage(const uint8_t *data, int width, int height, int stride){ - FNAME(); + //FNAME(); Image *outp = Image_new(width, height); // flip image updown for FITS coordinate system OMP_FOR() @@ -218,25 +218,26 @@ Image *Image_sim(const Image *i){ /** * @brief get_histogram - calculate image histogram * @param I - orig - * @return + * @param histo - histogram + * @return FALSE if failed */ -size_t *get_histogram(const Image *I){ - if(!I || !I->data) return NULL; - size_t *histogram = MALLOC(size_t, 256); +int get_histogram(const Image *I, size_t histo[HISTOSZ]){ + if(!I || !I->data || !histo) return FALSE; + bzero(histo, HISTOSZ*sizeof(size_t)); int wh = I->width * I->height; #pragma omp parallel { - size_t histogram_private[256] = {0}; + size_t histogram_private[HISTOSZ] = {0}; #pragma omp for nowait for(int i = 0; i < wh; ++i){ ++histogram_private[I->data[i]]; } #pragma omp critical { - for(int i = 0; i < 256; ++i) histogram[i] += histogram_private[i]; + for(int i = 0; i < HISTOSZ; ++i) histo[i] += histogram_private[i]; } } - return histogram; + return TRUE; } @@ -246,21 +247,22 @@ size_t *get_histogram(const Image *I){ * @param bk (o) - background value * @return 0 if error */ -int calc_background(const Image *img, Imtype *bk){ - if(!img || !img->data || !bk) return FALSE; +int calc_background(Image *img){ + if(!img || !img->data) return FALSE; if(img->maxval == img->minval){ WARNX("Zero or overilluminated image!"); return FALSE; } if(theconf.fixedbkg){ - if(theconf.fixedbkg > img->minval){ + if(theconf.fixedbkg < img->minval){ WARNX("Image values too small"); return FALSE; } - *bk = theconf.fixedbkg; + img->background = theconf.fixedbkg; return TRUE; } - size_t *histogram = get_histogram(img); + size_t histogram[HISTOSZ]; + if(!get_histogram(img, histogram)) return FALSE; size_t modeidx = 0, modeval = 0; for(int i = 0; i < 256; ++i) @@ -273,7 +275,6 @@ int calc_background(const Image *img, Imtype *bk){ for(int i = 2; i < 254; ++i) diff2[i] = (histogram[i+2]+histogram[i-2]-2*histogram[i])/4; //green("HISTO:\n"); //for(int i = 0; i < 256; ++i) printf("%d:\t%d\t%d\n", i, histogram[i], diff2[i]); - FREE(histogram); if(modeidx < 2) modeidx = 2; if(modeidx > 253){ WARNX("Overilluminated image"); @@ -287,7 +288,7 @@ int calc_background(const Image *img, Imtype *bk){ } //DBG("borderidx=%d -> %d", borderidx, (borderidx+modeidx)/2); //*bk = (borderidx + modeidx) / 2; - *bk = borderidx; + img->background = borderidx; return TRUE; } @@ -300,6 +301,7 @@ int calc_background(const Image *img, Imtype *bk){ */ uint8_t *linear(const Image *I, int nchannels){ // only 1 and 3 channels supported! if(!I || !I->data || (nchannels != 1 && nchannels != 3)) return NULL; + FNAME(); int width = I->width, height = I->height; size_t stride = width*nchannels, S = height*stride; uint8_t *outp = MALLOC(uint8_t, S); @@ -337,10 +339,11 @@ uint8_t *linear(const Image *I, int nchannels){ // only 1 and 3 channels support */ uint8_t *equalize(const Image *I, int nchannels, double throwpart){ if(!I || !I->data || (nchannels != 1 && nchannels != 3)) return NULL; + FNAME(); int width = I->width, height = I->height; size_t stride = width*nchannels, S = height*stride; - size_t *orig_histo = get_histogram(I); // original hystogram (linear) - if(!orig_histo) return NULL; + size_t orig_histo[HISTOSZ]; // original hystogram (linear) + if(!get_histogram(I, orig_histo)) return NULL; uint8_t *outp = MALLOC(uint8_t, S); uint8_t eq_levls[256] = {0}; // levels to convert: newpix = eq_levls[oldpix] int s = width*height; @@ -388,7 +391,6 @@ uint8_t *equalize(const Image *I, int nchannels, double throwpart){ } } } - FREE(orig_histo); return outp; } @@ -426,28 +428,33 @@ int Image_write_jpg(const Image *I, const char *name, int eq){ void Image_minmax(Image *I){ if(!I || !I->data) return; Imtype min = *(I->data), max = min; + float isum = 0.f; int wh = I->width * I->height; #ifdef EBUG - double t0 = dtime(); + //double t0 = dtime(); #endif - #pragma omp parallel shared(min, max) + #pragma omp parallel shared(min, max, isum) { int min_p = min, max_p = min; + float sum_p = 0.f; #pragma omp for nowait for(int i = 0; i < wh; ++i){ Imtype pixval = I->data[i]; if(pixval < min_p) min_p = pixval; else if(pixval > max_p) max_p = pixval; + sum_p += (float) pixval; } #pragma omp critical { if(min > min_p) min = min_p; if(max < max_p) max = max_p; + isum += sum_p; } } I->maxval = max; I->minval = min; - DBG("Image_minmax(): Min=%d, Max=%d, time: %gms", min, max, (dtime()-t0)*1e3); + I->avg_intensity = isum / (float)wh; + DBG("Image_minmax(): Min=%d, Max=%d, Isum=%g, mean=%g", min, max, isum, I->avg_intensity); } /* diff --git a/LocCorr_new/imagefile.h b/LocCorr_new/imagefile.h index 6cc9217..aaa06fb 100644 --- a/LocCorr_new/imagefile.h +++ b/LocCorr_new/imagefile.h @@ -27,7 +27,10 @@ #define OMP_FOR(x) _Pragma(Stringify(omp parallel for x)) #endif -typedef uint8_t Imtype; // maybe float or double only +typedef uint8_t Imtype; +// 65536 if Imtype is uint16_t +// WARNING! Check code if you change Imtype: e.g. recalcexp() and other +#define HISTOSZ (256) typedef struct{ int width; // width @@ -35,6 +38,8 @@ typedef struct{ Imtype *data; // picture data Imtype minval; // extremal data values Imtype maxval; + float avg_intensity; + Imtype background; // background value } Image; // input file/directory type @@ -60,8 +65,8 @@ Image *Image_new(int w, int h); Image *Image_sim(const Image *i); void Image_free(Image **I); int Image_write_jpg(const Image *I, const char *name, int equalize); -size_t *get_histogram(const Image *I); -int calc_background(const Image *img, Imtype *bk); +int get_histogram(const Image *I, size_t histo[HISTOSZ]); +int calc_background(Image *img); Image *u8toImage(const uint8_t *data, int width, int height, int stride); Image *bin2Im(const uint8_t *image, int W, int H); diff --git a/LocCorr_new/improc.c b/LocCorr_new/improc.c index 5cfd88f..ebb5887 100644 --- a/LocCorr_new/improc.c +++ b/LocCorr_new/improc.c @@ -84,6 +84,21 @@ static void XYnewline(){ fflush(fXYlog); } +// add comment string to XY log; @return FALSE if failed (file not exists) +int XYcomment(char *cmnt){ + if(!fXYlog || !cmnt) return FALSE; + if(*cmnt == '"'){ + ++cmnt; + char *e = strrchr(cmnt, '"'); + if(e) *e = 0; + } + char *n = strrchr(cmnt, '\n'); + if(n) *n = 0; + fprintf(fXYlog, "# %s\n", cmnt); + fflush(fXYlog); + return TRUE; +} + static void getDeviation(object *curobj){ int averflag = 0; static double Xc[NAVER_MAX+1], Yc[NAVER_MAX+1]; @@ -92,8 +107,8 @@ static void getDeviation(object *curobj){ static int counter = 0; Xc[counter] = curobj->xc; Yc[counter] = curobj->yc; if(fXYlog){ // make log record - fprintf(fXYlog, "%.2f\t%.1f\t%.1f\t%.1f\t%.1f\t%.1f\t", - dtime(), curobj->xc, curobj->yc, + fprintf(fXYlog, "%-14.2f\t%.1f\t%.1f\t%.1f\t%.1f\t%.1f\t", + dtime() - tstart, curobj->xc, curobj->yc, curobj->xsigma, curobj->ysigma, curobj->WdivH); } //DBG("counter = %d", counter); @@ -118,11 +133,13 @@ static void getDeviation(object *curobj){ averflag = 1; if(fXYlog) fprintf(fXYlog, "%.1f\t%.1f\t%.1f\t%.1f", xx, yy, Sx, Sy); process_corrections: - if(theSteppers && theSteppers->proc_corr && averflag){ - if(Sx > XY_TOLERANCE || Sy > XY_TOLERANCE){ - LOGDBG("Bad value - not process"); // don't run processing for bad data - }else - theSteppers->proc_corr(xx, yy); + if(theSteppers){ + if(theSteppers->proc_corr && averflag){ + if(Sx > XY_TOLERANCE || Sy > XY_TOLERANCE){ + LOGDBG("Bad value - not process"); // don't run processing for bad data + }else + theSteppers->proc_corr(xx, yy); + } }else{ LOGERR("Lost connection with stepper server"); ERRX("Lost connection with stepper server"); @@ -130,16 +147,71 @@ process_corrections: XYnewline(); } +typedef struct{ // statistics: mean and RMS + float xc; float yc; float xsigma; float ysigma; +} ptstat_t; + +/** + * @brief sumAndStat - calculate statistics in region of interest + * @param I - image (with background calculated) + * @param mask - labeled mask for objects (or NULL) + * @param idx - index on labeled mask + * @param roi - region of interest + * @param stat - (region - bacground) statistics + * @return total intensity sum + */ +static float sumAndStat(const Image *I, const size_t *mask, size_t idx, const il_Box *roi, ptstat_t *stat){ + if(!I || !roi) return -1.; + //FNAME(); + float xc = 0., yc = 0.; + float x2c = 0., y2c = 0., Isum = 0.; + int W = I->width; + //DBG("imw=%d, roi=%d:%d:%d:%d", W, roi->xmin, roi->xmax, roi->ymin, roi->ymax); + // dumb calculation as paralleling could be much slower + for(int y = roi->ymin; y <= roi->ymax; ++y){ + size_t istart = y*W + roi->xmin; + //DBG("istart=%zd", istart); + const size_t *maskptr = (mask) ? &mask[istart] : NULL; + //DBG("mask %s NULL", mask ? "!=":"=="); + Imtype *Iptr = &I->data[istart]; + for(int x = roi->xmin; x <= roi->xmax; ++x, ++Iptr){ + if(maskptr){if(*maskptr++ != idx) continue;} + if(*Iptr <= I->background) continue; + float intens = (float)(*Iptr - I->background); + float xw = x * intens, yw = y * intens; + xc += xw; + yc += yw; + x2c += xw * x; + y2c += yw * y; + Isum += intens; + } + } + if(stat && Isum > 0.){ + stat->xc = xc / Isum; + stat->yc = yc / Isum; + stat->xsigma = x2c/Isum - stat->xc*stat->xc; + stat->ysigma = y2c/Isum - stat->yc*stat->yc; + } + //DBG("xc=%g, yc=%g, xs=%g, ys=%g", stat->xc, stat->yc, stat->xsigma, stat->ysigma); + return Isum; +} + +#define MAX(a, b) ((a) > (b) ? (a) : (b)) +#define MIN(a, b) ((a) < (b) ? (a) : (b)) + void process_file(Image *I){ static double lastTproc = 0.; -/* + static int prev_x = -1, prev_y = -1; + static object *Objects = NULL; + static size_t Nallocated = 0; + il_ConnComps *cc = NULL; + size_t *S = NULL; #ifdef EBUG double t0 = dtime(), tlast = t0; #define DELTA(p) do{double t = dtime(); DBG("---> %s @ %gms (delta: %gms)", p, (t-t0)*1e3, (t-tlast)*1e3); tlast = t;}while(0) #else -*/ #define DELTA(x) -//#endif +#endif // I - original image // mean - local mean // std - local STD @@ -152,11 +224,52 @@ void process_file(Image *I){ int W = I->width, H = I->height; //save_fits(I, "fitsout.fits"); //DELTA("Save original"); - Imtype bk; - if(calc_background(I, &bk)){ - DBG("backgr = %d", bk); + if(calc_background(I)){ + DBG("backgr = %d", I->background); DELTA("Got background"); - uint8_t *ibin = Im2bin(I, bk); + int objctr = 0; + if(prev_x > 0 && prev_y > 0){ + // Define ROI bounds + il_Box roi = {.xmin = MAX(prev_x - ROI_SIZE/2, 0), + .xmax = MIN(prev_x + ROI_SIZE/2, W-1), + .ymin = MAX(prev_y - ROI_SIZE/2, 0), + .ymax = MIN(prev_y + ROI_SIZE/2, H-1)}; + ptstat_t stat; + // Calculate centroid within ROI + DBG("Get sum and stat for simplest centroid"); + double sum = sumAndStat(I, NULL, 0, &roi, &stat); + if(sum > 0.){ + if( fabsf(stat.xc - prev_x) > XY_TOLERANCE || + fabsf(stat.yc - prev_y) > XY_TOLERANCE){ + DBG("Bad: was x=%d, y=%d; become x=%g, y=%g ==> need fine calculations", prev_x, prev_y, xc, yc); + }else{ + double WdH = stat.xsigma/stat.ysigma; + // wery approximate area inside sigmax*sigmay + double area = .4 * stat.xsigma * stat.ysigma; + if(!isnan(WdH) && !isinf(WdH) && // if W/H is a number + WdH > theconf.minwh && WdH < theconf.maxwh && // if W/H near circle + area > theconf.minarea && area < theconf.maxarea){ // if star area is in range + prev_x = (int)stat.xc; + prev_y = (int)stat.yc; + DBG("Simplest centroid, Xc=%g, Yc=%g", stat.xc, stat.yc); + objctr = 1; + if(!Objects){ + Objects = (object*)malloc(sizeof(object)); + Nallocated = 1; + } + Objects[0] = (object){ + .area = area, .Isum = sum, + .WdivH = WdH, .xc = stat.xc, .yc = stat.yc, + .xsigma = stat.xsigma, .ysigma = stat.ysigma + }; + goto SKIP_FULL_PROCESS; // Skip full image processing + }else{ + DBG("BAD image: WdH=%g, area=%g, xsigma=%g, ysigma=%g", WdH, area, stat.xsigma, stat.ysigma); + } + } + } + } + uint8_t *ibin = Im2bin(I, I->background); DELTA("Made binary"); if(ibin){ //savebin(ibin, W, H, "binary.fits"); @@ -171,54 +284,46 @@ void process_file(Image *I){ DELTA("Opening"); //savebin(opn, W, H, "opening.fits"); //DELTA("Save opening"); - il_ConnComps *cc = NULL; - size_t *S = il_cclabel4(opn, W, H, &cc); + S = il_cclabel4(opn, W, H, &cc); FREE(opn); - if(cc->Nobj > 1){ // Nobj = amount of objects + 1 - DBGLOG("Nobj=%d", cc->Nobj-1); - object *Objects = MALLOC(object, cc->Nobj-1); - int objctr = 0; + DBG("Nobj=%zd", cc->Nobj-1); + if(S && cc && cc->Nobj > 1){ // Nobj = amount of objects + 1 + DBGLOG("Nobj=%zd", cc->Nobj-1); + if(Nallocated < cc->Nobj-1){ + Nallocated = cc->Nobj-1; + Objects = realloc(Objects, Nallocated*sizeof(object)); + } for(size_t i = 1; i < cc->Nobj; ++i){ il_Box *b = &cc->boxes[i]; double wh = ((double)b->xmax - b->xmin)/(b->ymax - b->ymin); //DBG("Obj# %zd: wh=%g, area=%d", i, wh, b->area); if(wh < theconf.minwh || wh > theconf.maxwh) continue; if((int)b->area < theconf.minarea || (int)b->area > theconf.maxarea) continue; - double xc = 0., yc = 0.; - double x2c = 0., y2c = 0., Isum = 0.; - for(size_t y = b->ymin; y <= b->ymax; ++y){ - size_t idx = y*W + b->xmin; - size_t *maskptr = &S[idx]; - Imtype *Iptr = &I->data[idx]; - for(size_t x = b->xmin; x <= b->xmax; ++x, ++maskptr, ++Iptr){ - if(*maskptr != i) continue; - double intens = (double) (*Iptr - bk); - if(intens < 0.) continue; - double xw = x * intens, yw = y * intens; - xc += xw; - yc += yw; - x2c += xw * x; - y2c += yw * y; - Isum += intens; + ptstat_t stat; + DBG("Get sum and stat"); + double sum = sumAndStat(I, &S[i], i, b, &stat); + if(sum > 0.){ + if(cc->Nobj == 2){ + prev_x = (int)stat.xc, prev_y = (int)stat.yc; } + Objects[objctr++] = (object){ + .area = b->area, .Isum = sum, + .WdivH = wh, .xc = stat.xc, .yc = stat.yc, + .xsigma = stat.xsigma, .ysigma = stat.ysigma + }; } - xc /= Isum; yc /= Isum; - x2c = x2c/Isum - xc*xc; - y2c = y2c/Isum - yc*yc; - Objects[objctr++] = (object){ - .area = b->area, .Isum = Isum, - .WdivH = wh, .xc = xc, .yc = yc, - .xsigma = sqrt(x2c), .ysigma = sqrt(y2c) - }; } DELTA("Labeling"); - DBGLOG("T%.2f, N=%d\n", dtime(), objctr); if(objctr > 1){ + prev_x = -1, prev_y = -1; // don't allow simple gravcenter for a lots of objects if(theconf.starssort) qsort(Objects, objctr, sizeof(object), compIntens); else qsort(Objects, objctr, sizeof(object), compDist); } +SKIP_FULL_PROCESS: + DBGLOG("T%.2f, N=%d\n", dtime(), objctr); + DELTA("Calculate deviations"); if(objctr){ #ifdef EBUG object *o = Objects; @@ -233,6 +338,7 @@ void process_file(Image *I){ #endif getDeviation(Objects); // calculate dX/dY and process corrections } + DELTA("prepare image"); { // prepare image and save jpeg uint8_t *outp = NULL; if(theconf.equalize) @@ -243,6 +349,7 @@ void process_file(Image *I){ if(!cross) cross = il_Pattern_xcross(33, 33); if(!crossL) crossL = il_Pattern_xcross(51, 51); il_Img3 i3 = {.data = outp, .w = I->width, .h = H}; + DELTA("Draw crosses"); // draw fiber center position il_Pattern_draw3(&i3, crossL, theconf.xtarget-theconf.xoff, H-(theconf.ytarget-theconf.yoff), C_R); if(objctr){ // draw crosses @ objects' centers @@ -256,7 +363,7 @@ void process_file(Image *I){ for(int i = 1; i < objctr; ++i) il_Pattern_draw3(&i3, cross, Objects[i].xc, H-Objects[i].yc, C_B); }else{xc = -1.; yc = -1.;} - char *tmpnm = MALLOC(char, strlen(GP->outputjpg) + 5); + char tmpnm[FILENAME_MAX+5]; sprintf(tmpnm, "%s-tmp", GP->outputjpg); if(stbi_write_jpg(tmpnm, I->width, I->height, 3, outp, 95)){ if(rename(tmpnm, GP->outputjpg)){ @@ -264,10 +371,9 @@ void process_file(Image *I){ LOGWARN("can't save %s", GP->outputjpg); } } - FREE(tmpnm); + DELTA("Written"); FREE(outp); } - FREE(Objects); }else{ xc = -1.; yc = -1.; Image_write_jpg(I, GP->outputjpg, theconf.equalize); @@ -333,7 +439,7 @@ void openXYlog(const char *name){ } time_t t = time(NULL); fprintf(fXYlog, "# Start at: %s", ctime(&t)); - fprintf(fXYlog, "# time Xc\tYc\t\tSx\tSy\tW/H\taverX\taverY\tSX\tSY\n"); + fprintf(fXYlog, "# time\t\tXc\tYc\tSx\tSy\tW/H\taverX\taverY\tSX\tSY\n"); fflush(fXYlog); tstart = dtime(); } diff --git a/LocCorr_new/improc.h b/LocCorr_new/improc.h index fcd7659..e191b4a 100644 --- a/LocCorr_new/improc.h +++ b/LocCorr_new/improc.h @@ -25,6 +25,7 @@ // tolerance of deviations by X and Y axis (if sigmaX or sigmaY greater, values considered to be wrong) #define XY_TOLERANCE (5.) +#define ROI_SIZE (200) extern volatile atomic_bool stopwork; extern volatile atomic_ullong ImNumber; @@ -35,6 +36,7 @@ void process_file(Image *I); int process_input(InputType tp, char *name); void openXYlog(const char *name); void closeXYlog(); +int XYcomment(char *cmnt); double getFramesPerS(); void getcenter(float *x, float *y); diff --git a/LocCorr_new/loccorr.conf b/LocCorr_new/loccorr.conf index 8c05aa5..210b532 100644 --- a/LocCorr_new/loccorr.conf +++ b/LocCorr_new/loccorr.conf @@ -1,36 +1,44 @@ -maxarea = 4000 -minarea = 100 -minwh = 0.800 -maxwh = 1.300 -ndilat = 3 -neros = 3 -xoffset = 0 -yoffset = 0 -width = 1920 -height = 1200 +Kxu = 45.155 +Kxv = 43.771 +Kyu = 68.838 +Kyv = -70.428 +brightness = 0.000 +eqthrowpart = 0.900 equalize = 0 expmethod = 0 -naverage = 3 -umax = 400 -vmax = 230 -focmax = 4500 -focmin = 20 -stpservport = 4444 -Kxu = 45.155 -Kyu = 68.838 -Kxv = 43.771 -Kyv = -70.428 -xtarget = 1039.600 -ytarget = 602.820 -eqthrowpart = 0.900 -minexp = 50.000 -maxexp = 1000.000 +fbglevel = 100 +fixedbg = 0 fixedexp = 1000.000 -intensthres = 0.010 +focmax = 4000 +focmin = 0 gain = 36.000 -brightness = 0.000 -starssort = 0 +height = 800 +intensthres = 0.010 +maxarea = 4000 +maxexp = 1000.000 +maxwh = 1.300 medfilt = 0 medseed = 1 -fixedbg = 0 -fbglevel = 100 +minarea = 100 +minexp = 50.000 +minwh = 0.800 +naverage = 3 +ndilat = 3 +neros = 3 +pidud = 0.050 +pidui = 0.150 +pidup = 0.650 +pidvd = 0.050 +pidvi = 0.150 +pidvp = 0.650 +starssort = 0 +stpservport = 4444 +umax = 750 +umin = 50 +vmax = 470 +vmin = 100 +width = 700 +xoffset = 408 +xtarget = 740.000 +yoffset = 300 +ytarget = 730.000 diff --git a/LocCorr_new/main.c b/LocCorr_new/main.c index 8381e40..2d6e73e 100644 --- a/LocCorr_new/main.c +++ b/LocCorr_new/main.c @@ -116,6 +116,15 @@ int main(int argc, char *argv[]){ initial_setup(); char *self = strdup(argv[0]); GP = parse_args(argc, argv); + if(!chkconfig(GP->configname)){ + LOGWARN("Wrong/absent configuration file"); + WARNX("Wrong/absent configuration file"); + if(GP->chkconf) return 1; + } + if(GP->chkconf){ + printf("File %s OK\n", GP->configname); + return 0; + } if(GP->throwpart < 0. || GP->throwpart > 0.99){ ERRX("Fraction of black pixels should be in [0., 0.99]"); } @@ -138,11 +147,6 @@ int main(int argc, char *argv[]){ OPENLOG(GP->logfile, lvl, 1); DBG("Opened log file @ level %d", lvl); } - int C = chkconfig(GP->configname); - if(!C){ - LOGWARN("Wrong/absent configuration file"); - WARNX("Wrong/absent configuration file"); - } // change `theconf` parameters to user values { if(GP->maxarea != DEFAULT_MAXAREA || theconf.maxarea == 0) theconf.maxarea = GP->maxarea; diff --git a/LocCorr_new/socket.c b/LocCorr_new/socket.c index dd77d1e..20f29d6 100644 --- a/LocCorr_new/socket.c +++ b/LocCorr_new/socket.c @@ -44,6 +44,8 @@ // Max amount of connections #define BACKLOG (10) +static pthread_mutex_t cmd_mutex = PTHREAD_MUTEX_INITIALIZER; + /* TODO3: add 'FAIL error text' if not OK and instead all "wrong message" */ @@ -67,9 +69,9 @@ static char *stepperstatus(const char *messageid, char *buf, int buflen); static char *getimagedata(const char *messageid, char *buf, int buflen); static getter getterHandlers[] = { {"help", helpmsg, "List avaiable commands"}, + {"imdata", getimagedata, "Get image data (status, path, FPS, counter)"}, {"settings", listconf, "List current configuration"}, {"stpserv", stepperstatus, "Get status of steppers server"}, - {"imdata", getimagedata, "Get image data (status, path, FPS, counter)"}, {NULL, NULL, NULL} }; @@ -77,12 +79,14 @@ static char *setstepperstate(const char *state, char *buf, int buflen); static char *setfocusstate(const char *state, char *buf, int buflen); static char *moveU(const char *val, char *buf, int buflen); static char *moveV(const char *val, char *buf, int buflen); +static char *addcmnt(const char *cmnt, char *buf, int buflen); static setter setterHandlers[] = { - {"stpstate", setstepperstate, "Set given steppers' server state"}, + {"comment", addcmnt, "Add comment to XY log file"}, {"focus", setfocusstate, "Move focus to given value"}, {"moveU", moveU, "Relative moving by U axe"}, {"moveV", moveV, "Relative moving by V axe"}, + {"stpstate", setstepperstate, "Set given steppers' server state"}, {NULL, NULL, NULL} }; @@ -90,6 +94,11 @@ static char *retFAIL(char *buf, int buflen){ snprintf(buf, buflen, FAIL); return buf; } +static char *retOK(char *buf, int buflen){ + snprintf(buf, buflen, OK); + return buf; +} + /**************** functions to process commands ****************/ // getters @@ -143,6 +152,13 @@ static char *moveV(const char *val, char *buf, int buflen){ if(theSteppers && theSteppers->moveByV) return theSteppers->moveByV(val, buf, buflen); return retFAIL(buf, buflen); } +static char *addcmnt(const char *cmnt, char *buf, int buflen){ + if(!cmnt) return retFAIL(buf, buflen); + char *line = strdup(cmnt); + int ret = XYcomment(line); + free(line); + return ret ? retOK(buf, buflen) : retFAIL(buf, buflen) ; +} /* static char *rmnl(const char *msg, char *buf, int buflen){ @@ -162,14 +178,15 @@ static char *rmnl(const char *msg, char *buf, int buflen){ */ static char *processCommand(const char msg[BUFLEN], char *ans, int anslen){ char value[BUFLEN]; - char *kv = get_keyval(msg, value); + char *kv = get_keyval(msg, value); // ==NULL for getters/commands without equal sign + DBG("message: %s, value: %s, key: %s", msg, value, kv); confparam *par; if(kv){ DBG("got KEY '%s' with value '%s'", kv, value); key_value result; par = chk_keyval(kv, value, &result); - free(kv); kv = NULL; - if(par){ + if(par){ // configuration parameter + DBG("found this key in conf"); switch(par->type){ case PAR_INT: DBG("FOUND! Integer, old=%d, new=%d", *((int*)par->ptr), result.val.intval); @@ -181,27 +198,67 @@ static char *processCommand(const char msg[BUFLEN], char *ans, int anslen){ break; default: snprintf(ans, anslen, FAIL); + free(kv); return ans; } snprintf(ans, anslen, OK); + free(kv); return ans; }else{ + DBG("check common setters"); setter *s = setterHandlers; while(s->command){ - int l = strlen(s->command); - if(strncasecmp(msg, s->command, l) == 0) + //int l = strlen(s->command); + //if(strncasecmp(msg, s->command, l) == 0) + DBG("check '%s' == '%s' ?", kv, s->command); + if(strcmp(kv, s->command) == 0){ + free(kv); return s->handler(value, ans, anslen); + } ++s; } } + free(kv); }else{ + DBG("getter?"); + // first check custom getters getter *g = getterHandlers; while(g->command){ - int l = strlen(g->command); - if(strncasecmp(msg, g->command, l) == 0) + //int l = strlen(g->command); + //if(strncasecmp(msg, g->command, l) == 0) + if(0 == strcmp(msg, g->command)) return g->handler(g->command, ans, anslen); ++g; } + DBG("not found in getterHandlers"); + // check custom setters + setter *s = setterHandlers; + while(s->command){ + if(strcmp(msg, s->command) == 0){ + size_t p = snprintf(ans, anslen, "%s=", msg); + s->handler(NULL, ans+p, anslen-p); + return ans; + } + ++s; + } + DBG("not found in setterHandlers"); + // and check configuration parameters + confparam *par = find_key(msg); + if(par){ + switch(par->type){ + case PAR_INT: + snprintf(ans, anslen, "%s=%d", msg, *((int*)par->ptr)); + break; + case PAR_DOUBLE: + snprintf(ans, anslen, "%s=%g", msg, *((double*)par->ptr)); + break; + default: + snprintf(ans, anslen, FAIL); + break; + } + return ans; + } + DBG("not found in parameters"); } snprintf(ans, anslen, FAIL); return ans; @@ -215,16 +272,24 @@ static char *processCommand(const char msg[BUFLEN], char *ans, int anslen){ * @return amount of sent bytes */ static size_t send_data(int sock, const char *textbuf){ - ssize_t Len = strlen(textbuf); - if(Len != write(sock, textbuf, Len)){ - WARN("write()"); - LOGERR("send_data(): write() failed"); - return 0; - }else{ - LOGDBG("send_data(): sent '%s'", textbuf); + size_t total = 0; + size_t len = strlen(textbuf); + const char *ptr = textbuf; + while(total < len){ + ssize_t sent = write(sock, ptr + total, len - total); + if(sent < 0){ + if(errno == EINTR) continue; + LOGERR("Write error: %s", strerror(errno)); + return total; + } + total += sent; } - if(textbuf[Len-1] != '\n') Len += write(sock, "\n", 1); - return (size_t)Len; + if(textbuf[len-1] != '\n'){ + if(write(sock, "\n", 1) != 1){ + LOGERR("Failed to write newline"); + } + } + return total; } /** @@ -233,7 +298,7 @@ static size_t send_data(int sock, const char *textbuf){ * @return 0 if all OK, 1 if socket closed */ static int handle_socket(int sock){ - FNAME(); + //FNAME(); char buff[BUFLEN]; char ansbuff[ANSBUFLEN]; ssize_t rd = read(sock, buff, BUFLEN-1); @@ -243,19 +308,31 @@ static int handle_socket(int sock){ } // add trailing zero to be on the safe side buff[rd] = 0; + char *eol = strchr(buff, '\n'); + if(eol) *eol = 0; // clear '\n' // now we should check what do user want // here we can process user data DBG("user %d send '%s'", sock, buff); LOGDBG("user %d send '%s'", sock, buff); - //pthread_mutex_lock(&mutex); + pthread_mutex_lock(&cmd_mutex); char *ans = processCommand(buff, ansbuff, ANSBUFLEN-1); // run command parser if(ans){ send_data(sock, ans); // send answer } - //pthread_mutex_unlock(&mutex); + pthread_mutex_unlock(&cmd_mutex); return 0; } +// shutdown client and close +static void cleanup_client(int fd) { + if(fd > 0) { + shutdown(fd, SHUT_RDWR); + close(fd); + DBG("Client with fd %d closed", fd); + LOGMSG("Client %d disconnected", fd); + } +} + // main socket server static void *server(void *asock){ DBG("server(): getpid: %d, tid: %lu",getpid(), syscall(SYS_gettid)); @@ -273,9 +350,16 @@ static void *server(void *asock){ while(1){ if(stopwork){ DBG("server() exit @ global stop"); + LOGDBG("server() exit @ global stop"); return NULL; } - poll(poll_set, nfd, 1); // poll for 1ms + int ready = poll(poll_set, nfd, 1); // 1ms timeout + if(ready < 0){ + if(errno == EINTR) continue; + LOGERR("Poll error: %s", strerror(errno)); + return NULL; + } + if(0 == ready) continue; for(int fdidx = 0; fdidx < nfd; ++fdidx){ // poll opened FDs if((poll_set[fdidx].revents & POLLIN) == 0) continue; poll_set[fdidx].revents = 0; @@ -284,9 +368,7 @@ static void *server(void *asock){ //int nread = 0; //ioctl(fd, FIONREAD, &nread); if(handle_socket(fd)){ // socket closed - remove it from list - close(fd); - DBG("Client with fd %d closed", fd); - LOGMSG("Client %d disconnected", fd); + cleanup_client(fd); // move last to free space poll_set[fdidx] = poll_set[nfd - 1]; --nfd; @@ -319,6 +401,8 @@ static void *server(void *asock){ } } // endfor } + // disconnect all + for(int i = 1; i < nfd; ++i) cleanup_client(poll_set[i].fd); LOGERR("server(): UNREACHABLE CODE REACHED!"); } diff --git a/LocCorr_new/steppers.c b/LocCorr_new/steppers.c index ffb5287..1095bc8 100644 --- a/LocCorr_new/steppers.c +++ b/LocCorr_new/steppers.c @@ -46,6 +46,14 @@ // tolerance of coordinates coincidence (pix) #define COORDTOLERANCE (0.5) +// PID +typedef struct { + double Kp, Ki, Kd; // coefficients + double integral; // intergal error accumulator + double prev_error; // previous error value for D + double prev_time; // and previous time +} PIDController; + typedef enum{ STP_DISCONN, STP_RELAX, @@ -53,7 +61,8 @@ typedef enum{ STP_GOTOTHEMIDDLE, STP_FINDTARGET, STP_FIX, - STP_UNDEFINED + STP_UNDEFINED, + STP_STATE_AMOUNT } STPstate; typedef enum{ @@ -710,6 +719,31 @@ static int process_targetstage(double X, double Y){ return TRUE; } +/** + * @brief compute_pid - calculate PID responce for error + * @param pid - U/V PID parameters + * @param error - current error + * @param current_time - and current time + * @return PID-corrected responce + */ +static double compute_pid(PIDController *pid, double error, double current_time) { + double dt = current_time - pid->prev_time; + if(dt <= 0.) dt = 0.01; // Default to 10ms if time isn't tracked + // Integral term with anti-windup + pid->integral += error * dt; + // Clamp integral to ?1000 (adjust based on system limits) + if(pid->integral > 1000.) pid->integral = 1000.; + if(pid->integral < -1000.) pid->integral = -1000.; + // Derivative term (filtered) + double derivative = (error - pid->prev_error) / dt; + pid->prev_error = error; + pid->prev_time = current_time; + double pid_out = (pid->Kp * error) + (pid->Ki * pid->integral) + (pid->Kd * derivative); + LOGDBG("PID: error=%.2f, integral=%.2f, derivative=%.2f, output=%.2f", + error, pid->integral, derivative, pid_out); + return pid_out; +} + /** * @brief try2correct - try to correct position * @param dX - delta of X-coordinate in image space @@ -718,25 +752,46 @@ static int process_targetstage(double X, double Y){ */ static int try2correct(double dX, double dY){ if(!relaxed(Ustepper) || !relaxed(Vstepper)) return FALSE; + // calculations: make Ki=0, Kd=0; increase Kp until oscillations; + // now Tu - osc period, Ku=Kp for oscillations; so: + // Kp = 0.6*Ku; Ki = 1.2*Ku/Tu; Kd = 0.075*Ku*Tu (Ziegler-Nichols) + static PIDController pidU = {0}, pidV = {0}; + // refresh parameters from configuration + pidU.Kp = theconf.PIDU_P; pidU.Ki = theconf.PIDU_I; pidU.Kd = theconf.PIDU_D; + pidV.Kp = theconf.PIDV_P; pidV.Ki = theconf.PIDV_I; pidV.Kd = theconf.PIDV_D; double dU, dV; + double current_time = dtime(); + if( current_time - pidU.prev_time > MAX_PID_TIME + || current_time - pidV.prev_time > MAX_PID_TIME){ + LOGWARN("Too old PID time: have dt=%gs", current_time - pidU.prev_time); + pidU.prev_time = pidV.prev_time = current_time; + pidU.integral = pidV.integral = 0.; + return FALSE; + } // dU = KU*(dX*cosXU + dY*sinXU); dV = KV*(dX*cosXV + dY*sinXV) - dU = KCORR*(theconf.Kxu * dX + theconf.Kyu * dY); - dV = KCORR*(theconf.Kxv * dX + theconf.Kyv * dY); - int Unew = Uposition + (int)dU, Vnew = Vposition + (int)dV; + dU = theconf.Kxu * dX + theconf.Kyu * dY; + dV = theconf.Kxv * dX + theconf.Kyv * dY; + LOGDBG("dx/dy: %g/%g; dU/dV: %g/%g", dX, dY, dU, dV); + // Compute PID outputs + double pidU_out = compute_pid(&pidU, dU, current_time); + double pidV_out = compute_pid(&pidV, dV, current_time); + int usteps = (int)pidU_out, vsteps = (int)pidV_out; + int Unew = Uposition + usteps, Vnew = Vposition + vsteps; if(Unew > theconf.maxUpos || Unew < theconf.minUpos || Vnew > theconf.maxVpos || Vnew < theconf.minVpos){ + // Reset integral to prevent windup + pidU.integral = 0; + pidV.integral = 0; // TODO: here we should signal that the limit reached and move by telescope LOGWARN("Correction failed, curpos: %d, %d, should move to %d, %d", Uposition, Vposition, Unew, Vnew); return FALSE; } - LOGDBG("try2correct(): move from (%d, %d) to (%d, %d) (abs: %d, %d), delta (%.1f, %.1f)", - Uposition, Vposition, Unew, Vnew, Uposition + (int)(dU/KCORR), - Vposition + (int)(dV/KCORR), dU, dV); + LOGDBG("try2correct(): move from (%d, %d) to (%d, %d), delta (%.1f, %.1f)", + Uposition, Vposition, Unew, Vnew, dU, dV); int ret = TRUE; - int usteps = (int)dU, vsteps = (int)dV; if(usteps) ret = nth_motor_setter(CMD_RELPOS, Ustepper, usteps); - if(ret && vsteps) ret = nth_motor_setter(CMD_RELPOS, Vstepper, vsteps); + if(vsteps) ret &= nth_motor_setter(CMD_RELPOS, Vstepper, vsteps); if(!ret) LOGWARN("Canserver: cant run corrections"); return ret; } @@ -854,32 +909,31 @@ static char *stp_status(const char *messageid, char *buf, int buflen){ return buf; } -typedef struct{ - const char *str; - STPstate state; -} strstate; // commands from client to change status -static strstate stringstatuses[] = { - {"disconnect", STP_DISCONN}, - {"relax", STP_RELAX}, - {"setup", STP_SETUP}, - {"middle", STP_GOTOTHEMIDDLE}, - {"findtarget", STP_FINDTARGET}, - {"fix", STP_FIX}, - {NULL, 0} +static const char* stringstatuses[STP_STATE_AMOUNT] = { + [STP_DISCONN] = "disconnect", + [STP_RELAX] = "relax", + [STP_SETUP] = "setup", + [STP_GOTOTHEMIDDLE] = "middle", + [STP_FINDTARGET] = "findtarget", + [STP_FIX] = "fix", + [STP_UNDEFINED] = "undefined" }; // try to set new status (global variable stepstatus) static char *set_stpstatus(const char *newstatus, char *buf, int buflen){ + if(!buf) return NULL; + if(!newstatus){ // getter + snprintf(buf, buflen, "%s", stringstatuses[state]); + return buf; + } // FNAME(); - strstate *s = stringstatuses; STPstate newstate = STP_UNDEFINED; - while(s->str){ - if(strcasecmp(s->str, newstatus) == 0){ - newstate = s->state; + for(int i = 0; i < STP_UNDEFINED; ++i){ + if(strcasecmp(stringstatuses[i], newstatus) == 0){ + newstate = (STPstate)i; break; } - ++s; } if(newstate != STP_UNDEFINED){ if(stp_setstate(newstate)){ @@ -892,12 +946,10 @@ static char *set_stpstatus(const char *newstatus, char *buf, int buflen){ } int L = snprintf(buf, buflen, "status '%s' undefined, allow: ", newstatus); char *ptr = buf; - s = stringstatuses; - while(L > 0){ + for(int i = 0; i < STP_UNDEFINED && buflen > 2; ++i){ buflen -= L; ptr += L; - L = snprintf(ptr, buflen, "'%s' ", s->str); - if((++s)->str == NULL) break; + L = snprintf(ptr, buflen-2, "'%s' ", stringstatuses[i]); } ptr[L-1] = '\n'; return buf; @@ -947,7 +999,7 @@ static void *stp_process_states(_U_ void *arg){ first = TRUE; } // if we are here, all U/V moving is finished - switch(state){ // STProbo state machine + switch(state){ // steppers state machine case STP_DISCONN: if(!stp_connect_server()){ WARNX("Can't reconnect"); @@ -974,7 +1026,7 @@ static void *stp_process_states(_U_ void *arg){ case STP_FIX: // process corrections if(coordsRdy){ coordsRdy = FALSE; - DBG("GET AVERAGE -> correct\n"); + DBG("GOT AVERAGE -> correct\n"); double xtg = theconf.xtarget - theconf.xoff, ytg = theconf.ytarget - theconf.yoff; double xdev = xtg - Xtarget, ydev = ytg - Ytarget; double corr = sqrt(xdev*xdev + ydev*ydev); @@ -1003,6 +1055,11 @@ static void *stp_process_states(_U_ void *arg){ // change focus (global variable movefocus) static char *set_pfocus(const char *newstatus, char *buf, int buflen){ + if(!buf) return NULL; + if(!newstatus){ // getter + snprintf(buf, buflen, "%d", Fposition); + return buf; + } int newval = atoi(newstatus); if(newval < theconf.minFpos || newval > theconf.maxFpos){ snprintf(buf, buflen, FAIL); @@ -1017,6 +1074,11 @@ static char *set_pfocus(const char *newstatus, char *buf, int buflen){ } // move by U and V axis static char *Umove(const char *val, char *buf, int buflen){ + if(!buf) return NULL; + if(!val){ // getter + snprintf(buf, buflen, "%d", Uposition); + return buf; + } int d = atoi(val); int Unfixed = Uposition + d; if(Unfixed > theconf.maxUpos || Unfixed < theconf.minUpos){ @@ -1030,6 +1092,11 @@ static char *Umove(const char *val, char *buf, int buflen){ return buf; } static char *Vmove(const char *val, char *buf, int buflen){ + if(!buf) return NULL; + if(!val){ // getter + snprintf(buf, buflen, "%d", Vposition); + return buf; + } int d = atoi(val); int Vnfixed = Vposition + d; if(Vnfixed > theconf.maxVpos || Vnfixed < theconf.minVpos){ diff --git a/LocCorr_new/steppers.h b/LocCorr_new/steppers.h index 42a149f..1550694 100644 --- a/LocCorr_new/steppers.h +++ b/LocCorr_new/steppers.h @@ -21,6 +21,9 @@ // set state to `disconnect` after this amount of errors in `moving_finished` #define MAX_ERR_CTR (15) +// max time interval from previous correction to clear integral/time (seconds) +#define MAX_PID_TIME (5.) + // amount of ALL motors #define NMOTORS (8)