From b32ca38018806a91335bc545ee0c9a024052ab17 Mon Sep 17 00:00:00 2001 From: Edward Emelianov Date: Tue, 20 Jul 2021 16:52:17 +0300 Subject: [PATCH] fixed some bugs & made more --- LocCorr/binmorph.c | 28 +++++----- LocCorr/config.c | 24 ++++++-- LocCorr/config.h | 24 ++++++-- LocCorr/grasshopper.c | 74 ++++++++++++++++++------ LocCorr/grasshopper.h | 1 + LocCorr/imagefile.c | 24 +------- LocCorr/improc.c | 119 ++++++++++++++++++++++++++++----------- LocCorr/improc.h | 21 ++++--- LocCorr/main.c | 56 +++++++++++++------ LocCorr/pusirobo.c | 43 +++++++++----- LocCorr/pusirobo.h | 2 +- LocCorr/socket.c | 127 ++++++++++++++++++++++++++---------------- 12 files changed, 358 insertions(+), 185 deletions(-) diff --git a/LocCorr/binmorph.c b/LocCorr/binmorph.c index 3541415..4719944 100644 --- a/LocCorr/binmorph.c +++ b/LocCorr/binmorph.c @@ -62,7 +62,7 @@ static void morph_init(){ * @return allocated memory area with converted input image */ uint8_t *filter4(uint8_t *image, int W, int H){ - FNAME(); + //FNAME(); if(W < MINWIDTH || H < MINHEIGHT) return NULL; uint8_t *ret = MALLOC(uint8_t, W*H); int W0 = (W + 7) / 8; // width in bytes @@ -94,7 +94,7 @@ uint8_t *filter4(uint8_t *image, int W, int H){ * @return allocated memory area with converted input image */ uint8_t *filter8(uint8_t *image, int W, int H){ - FNAME(); + //FNAME(); if(W < MINWIDTH || H < MINHEIGHT) return NULL; uint8_t *ret = MALLOC(uint8_t, W*H); int W0 = (W + 7) / 8; // width in bytes @@ -123,7 +123,7 @@ uint8_t *filter8(uint8_t *image, int W, int H){ * @return allocated memory area with dilation of input image */ uint8_t *dilation(uint8_t *image, int W, int H){ - FNAME(); + //FNAME(); if(W < MINWIDTH || H < MINHEIGHT) return NULL; int W0 = (W + 7) / 8; // width in bytes int w = W0-1, h = H-1, rest = 7 - (W - w*8); @@ -156,14 +156,14 @@ uint8_t *dilation(uint8_t *image, int W, int H){ * @return allocated memory area with erosion of input image */ uint8_t *erosion(uint8_t *image, int W, int H){ - FNAME(); + //FNAME(); if(W < MINWIDTH || H < MINHEIGHT) return NULL; if(!ER) morph_init(); int W0 = (W + 7) / 8; // width in bytes int w = W0-1, h = H-1, rest = 8 - (W - w*8); uint8_t lastmask = ~(1<name && buflen > 0){ switch(par->type){ case PAR_INT: - L = snprintf(ptr, buflen, "%s=%d\n", par->name, *((int*)par->ptr)); + L = snprintf(ptr, buflen, "\"%s\": %d", par->name, *((int*)par->ptr)); break; case PAR_DOUBLE: - L = snprintf(ptr, buflen, "%s=%.3f\n", par->name, *((double*)par->ptr)); + L = snprintf(ptr, buflen, "\"%s\": %.3f", par->name, *((double*)par->ptr)); break; default: L = 0; @@ -384,6 +393,11 @@ char *listconf(char *buf, int buflen){ }else{ buf[buflen-1] = 0; break; } + if(par->name){ // put comma + L = snprintf(ptr, buflen, ", "); + if(L > -1){buflen -= L; ptr += L;} + } } + snprintf(ptr, buflen, " }\n"); return buf; } diff --git a/LocCorr/config.h b/LocCorr/config.h index b9b8066..8038566 100644 --- a/LocCorr/config.h +++ b/LocCorr/config.h @@ -28,13 +28,17 @@ #define COEFMAX (10000) // area #define MINAREA (4) -#define MAXAREA (250000) +#define MAXAREA (2500000) #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.) +// min/max exposition in ms +#define EXPOS_MIN (0.1) +#define EXPOS_MAX (4000.) +#define GAIN_MIN (0.) +#define GAIN_MAX (20.) +// max average images counter #define NAVER_MAX (50) // coefficients to convert dx,dy to du,dv #define KUVMIN (-5000.) @@ -42,6 +46,13 @@ // default coefficient for corrections (move to Kdu, Kdv instead of du, dv) #define KCORR (0.9) +// exposition methods: 0 - auto, 1 - fixed +#define EXPAUTO (0) +#define EXPMANUAL (1) + +// messageID field name +#define MESSAGEID "messageid" + typedef struct{ int maxUsteps; // max amount of steps by both axes int maxVsteps; @@ -57,14 +68,17 @@ typedef struct{ 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) + int expmethod; // 0 - auto, 1 - fixed // dU = Kxu*dX + Kyu*dY; dV = Kxv*dX + Kyv*dY double Kxu; double Kyu; double Kxv; double Kyv; - double xtarget; // target (center) values + double xtarget; // target (center) values (in absolute coordinates! screen coords = target - offset) double ytarget; double throwpart; // part of values to throw avay @ histogram equalisation double maxexp; // minimal and maximal exposition (in ms) double minexp; + double fixedexp; // exptime in manual mode + double gain; // gain value in manual mode double intensthres; // threshold for stars intensity comparison: fabs(Ia-Ib)/(Ia+Ib) > thres -> stars differs } configuration; @@ -99,6 +113,6 @@ 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); +char *listconf(const char *messageid, char *buf, int buflen); #endif // CONFIG_H__ diff --git a/LocCorr/grasshopper.c b/LocCorr/grasshopper.c index 8e1f267..cf0b3e4 100644 --- a/LocCorr/grasshopper.c +++ b/LocCorr/grasshopper.c @@ -21,6 +21,7 @@ #include // FLT_EPSILON #include #include +#include #include #include "cmdlnopts.h" @@ -33,7 +34,6 @@ static fc2Context context; static fc2PGRGuid guid; static fc2Error err = FC2_ERROR_OK; -static float exptime = 10.; // exposition time in milliseconds static float gain = 0.; #define FC2FN(fn, ...) do{err = FC2_ERROR_OK; if(FC2_ERROR_OK != (err=fn(context __VA_OPT__(,) __VA_ARGS__))){ \ @@ -42,7 +42,6 @@ static float gain = 0.; /** * @brief setfloat - set absolute property value (float) * @param t - type of property - * @param context - initialized context * @param f - new value * @return 1 if all OK */ @@ -155,8 +154,8 @@ static int changeformat(){ } static int connect(){ - FNAME(); if(connected) return 1; + FNAME(); unsigned int numCameras = 0; if(FC2_ERROR_OK != (err = fc2CreateContext(&context))){ WARNX("fc2CreateContext(): %s", fc2ErrorToDescription(err)); @@ -188,29 +187,32 @@ static int connect(){ static int GrabImage(fc2Image *convertedImage){ FNAME(); + int ret = 0; fc2Image rawImage; // start capture FC2FN(fc2StartCapture); err = fc2CreateImage(&rawImage); if(err != FC2_ERROR_OK){ WARNX("Error in fc2CreateImage: %s", fc2ErrorToDescription(err)); - return 0; + goto rtn; } // Retrieve the image err = fc2RetrieveBuffer(context, &rawImage); if(err != FC2_ERROR_OK){ WARNX("Error in fc2RetrieveBuffer: %s", fc2ErrorToDescription(err)); - return 0; + goto rtn; } // Convert image to gray err = fc2ConvertImageTo(FC2_PIXEL_FORMAT_MONO8, &rawImage, convertedImage); if(err != FC2_ERROR_OK){ WARNX("Error in fc2ConvertImageTo: %s", fc2ErrorToDescription(err)); - return 0; + goto rtn; } + ret = 1; +rtn: fc2StopCapture(context); fc2DestroyImage(&rawImage); - return 1; + return ret; } static void calcexpgain(float newexp){ @@ -241,6 +243,15 @@ static void calcexpgain(float newexp){ //convertedImage.pData, convertedImage.cols, convertedImage.rows, convertedImage.stride static void recalcexp(fc2Image *img){ + // check if user changed exposition values + if(exptime < theconf.minexp){ + exptime = theconf.minexp; + return; + } + else if(exptime > theconf.maxexp){ + exptime = theconf.maxexp; + return; + } uint8_t *data = img->pData; int W = img->cols, H = img->rows, S = img->stride; int histogram[256] = {0}; @@ -271,15 +282,19 @@ int capture_grasshopper(void (*process)(Image*)){ FNAME(); static float oldexptime = 0.; static float oldgain = -1.; + Image *oIma = NULL; fc2Image convertedImage; err = fc2CreateImage(&convertedImage); if(err != FC2_ERROR_OK){ WARNX("capture_grasshopper(): can't create image, %s", fc2ErrorToDescription(err)); + disconnectGrasshopper(); return 0; } - Image *oIma = NULL; while(1){ - if(stopwork) return 1; + if(stopwork){ + DBG("STOP"); + break; + } if(!connect()){ // wait until camera be powered on DBG("Disconnected"); sleep(1); @@ -291,8 +306,8 @@ int capture_grasshopper(void (*process)(Image*)){ oldexptime = exptime; }else{ WARNX("Can't change exposition time to %gms", exptime); - disconnectGrasshopper(); - continue; + //disconnectGrasshopper(); + //continue; } } if(fabs(oldgain - gain) > FLT_EPSILON){ // change gain @@ -301,8 +316,8 @@ int capture_grasshopper(void (*process)(Image*)){ oldgain = gain; }else{ WARNX("Can't change gain to %g", gain); - disconnectGrasshopper(); - continue; + //disconnectGrasshopper(); + //continue; } } if(!GrabImage(&convertedImage)){ @@ -310,15 +325,42 @@ int capture_grasshopper(void (*process)(Image*)){ disconnectGrasshopper(); continue; } - if(!process) continue; - recalcexp(&convertedImage); + if(theconf.expmethod == EXPAUTO) recalcexp(&convertedImage); + else{ + if(fabs(theconf.fixedexp - exptime) > FLT_EPSILON) + exptime = theconf.fixedexp; + if(fabs(theconf.gain - gain) > FLT_EPSILON) + gain = theconf.gain; + } + if(!process){ + continue; + } oIma = u8toImage(convertedImage.pData, convertedImage.cols, convertedImage.rows, convertedImage.stride); if(oIma){ process(oIma); + FREE(oIma->data); FREE(oIma); } } fc2DestroyImage(&convertedImage); - fc2DestroyContext(context); + disconnectGrasshopper(); + DBG("GRASSHOPPER: out"); return 1; } + +// return JSON with image status +char *gsimagestatus(const char *messageid, char *buf, int buflen){ + static char *impath = NULL; + if(!impath){ + if(!(impath = realpath(GP->outputjpg, impath))){ + WARN("realpath() (%s)", impath); + impath = strdup(GP->outputjpg); + } + DBG("path: %s", impath); + } + snprintf(buf, buflen, "{ \"%s\": \"%s\", \"camstatus\": \"%sconnected\", \"impath\": \"%s\", \"imctr\": %llu, " + "\"fps\": %.3f, \"expmethod\": \"%s\", \"exposition\": %g, \"gain\": %g }\n", + MESSAGEID, messageid, connected ? "" : "dis", impath, ImNumber, getFramesPerS(), + (theconf.expmethod == EXPAUTO) ? "auto" : "manual", exptime, gain); + return buf; +} diff --git a/LocCorr/grasshopper.h b/LocCorr/grasshopper.h index a121067..5f7386b 100644 --- a/LocCorr/grasshopper.h +++ b/LocCorr/grasshopper.h @@ -25,5 +25,6 @@ void disconnectGrasshopper(); int capture_grasshopper(void (*process)(Image *)); +char *gsimagestatus(const char *messageid, char *buf, int buflen); #endif // GRASSHOPPER_H__ diff --git a/LocCorr/imagefile.c b/LocCorr/imagefile.c index c098f93..782826b 100644 --- a/LocCorr/imagefile.c +++ b/LocCorr/imagefile.c @@ -34,11 +34,6 @@ #include "imagefile.h" #include "median.h" -// weights to convert colour image into gray (sum=1!) -#define WEIGHT_RED () -#define WEIGHT_GREEN () -#define WEIGHT_BLUE () - typedef struct{ const char signature[8]; uint8_t len; @@ -224,7 +219,7 @@ uint8_t *linear(const Image *I, int nchannels){ // only 1 and 3 channels support size_t stride = width*nchannels, S = height*stride; uint8_t *outp = MALLOC(uint8_t, S); Imtype min = I->minval, max = I->maxval, W = 255./(max - min); - DBG("make linear transform %dx%d, %d channels", I->width, I->height, nchannels); + //DBG("make linear transform %dx%d, %d channels", I->width, I->height, nchannels); if(nchannels == 3){ OMP_FOR() for(int y = 0; y < height; ++y){ @@ -340,7 +335,6 @@ int Image_write_jpg(const Image *I, const char *name, int eq){ DBG("Try to write %s", name); 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)){ @@ -361,22 +355,6 @@ void Image_minmax(Image *I){ #ifdef EBUG double t0 = dtime(); #endif - /* - int N = omp_get_max_threads(); - Imtype arrmin[N], arrmax[N]; - for(int i = 0; i < N; ++i){ - arrmin[i] = min; arrmax[i] = max; - } - OMP_FOR() - for(int i = 0; i < wh; ++i){ - if(I->data[i] < arrmin[omp_get_thread_num()]) arrmin[omp_get_thread_num()] = I->data[i]; - else if(I->data[i] > arrmax[omp_get_thread_num()]) arrmax[omp_get_thread_num()] = I->data[i]; - } - - for(int i = 0; i < N; ++i){ - if(min > arrmin[i]) min = arrmin[i]; - if(max < arrmax[i]) max = arrmax[i]; - }*/ #pragma omp parallel shared(min, max) { int min_p = min, max_p = min; diff --git a/LocCorr/improc.c b/LocCorr/improc.c index 2db02a7..4f10e48 100644 --- a/LocCorr/improc.c +++ b/LocCorr/improc.c @@ -34,19 +34,27 @@ #include "median.h" #include "pusirobo.h" -static FILE *fXYlog = NULL; +float exptime = 10.; // GLOBAL: exposition time in milliseconds +volatile atomic_ullong ImNumber = 0; // GLOBAL: counter of processed images +volatile atomic_bool stopwork = FALSE; // GLOBAL: suicide +//int autoExposition = 1; // GLOBAL: ==1 if exposition calculation is auto +// GLOBAL: function to get stepper server status +char *(*stepstatus)(const char *messageid, char *buf, int buflen) = NULL; +// GLOBAL: set new status +char *(*setstepstatus)(const char *newstatus, char *buf, int buflen) = NULL; +// GLOBAL: move focus +char *(*movefocus)(const char *newstatus, char *buf, int buflen) = NULL; +// GLOBAL: get image information +char *(*imagedata)(const char *messageid, char *buf, int buflen); + +static FILE *fXYlog = NULL; static double tstart = 0.; // time of logging start -int stopwork = 0; +static double FPS = 0.; // frames per second + // function to process calculated corrections static void (*proc_corr)(double, double, int) = NULL; -// function to get stepper server status -char *(*stepstatus)(char *buf, int buflen) = NULL; -// set new status -char *(*setstepstatus)(const char *newstatus, char *buf, int buflen) = NULL; -// move focus -char *(*movefocus)(const char *newstatus, char *buf, int buflen) = NULL; typedef struct{ uint32_t area; // object area in pixels @@ -65,6 +73,7 @@ typedef enum{ static postproc_type postprocess = PROCESS_NONE; +/* static bool save_fits(Image *I, const char *name){ char fname[PATH_MAX]; snprintf(fname, PATH_MAX, name); @@ -75,14 +84,15 @@ static bool save_fits(Image *I, const char *name){ unlink(name); return FITS_write(name, I); } - +*/ +/* static void savebin(uint8_t *b, int W, int H, const char *name){ Image *I = bin2Im(b, W, H); if(I){ save_fits(I, name); Image_free(&I); } -} +}*/ // functions for Qsort static int compIntens(const void *a, const void *b){ // compare by intensity @@ -97,8 +107,9 @@ static int compIntens(const void *a, const void *b){ // compare by intensity 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 xtg = theconf.xtarget - theconf.xoff, ytg = theconf.ytarget - theconf.yoff; + double xa = oa->xc - xtg, xb = ob->xc - xtg, + ya = oa->yc - ytg, yb = ob->yc - ytg; double r2a = xa*xa + ya*ya; double r2b = xb*xb + yb*yb; return (r2a < r2b) ? -1 : 1; @@ -122,7 +133,7 @@ static void getDeviation(object *curobj){ dtime() - tstart, curobj->xc, curobj->yc, curobj->xsigma, curobj->ysigma, curobj->WdivH); } - DBG("counter = %d", counter); + //DBG("counter = %d", counter); if(++counter != theconf.naverage){ goto process_corrections; } @@ -142,7 +153,7 @@ static void getDeviation(object *curobj){ if(fXYlog) fprintf(fXYlog, "%.1f\t%.1f\t%.1f\t%.1f", xx, yy, Sx, Sy); process_corrections: if(proc_corr){ - if(Sx > 1. || Sy > 1.){ + if(Sx > XY_TOLERANCE || Sy > XY_TOLERANCE){ LOGDBG("Bad value - not process"); // don't run processing for bad data }else proc_corr(xx, yy, averflag); @@ -151,12 +162,15 @@ process_corrections: } void process_file(Image *I){ + static double lastTproc = 0.; +/* #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 @@ -168,28 +182,28 @@ void process_file(Image *I){ } int W = I->width, H = I->height; if(!I->dtype) I->dtype = FLOAT_IMG; - save_fits(I, "fitsout.fits"); - DELTA("Save original"); + //save_fits(I, "fitsout.fits"); + //DELTA("Save original"); Imtype bk; if(calc_background(I, &bk)){ - DBG("backgr = %g", bk); + //DBG("backgr = %g", bk); DELTA("Got background"); uint8_t *ibin = Im2bin(I, bk); DELTA("Made binary"); if(ibin){ - savebin(ibin, W, H, "binary.fits"); - DELTA("save binary.fits"); + //savebin(ibin, W, H, "binary.fits"); + //DELTA("save binary.fits"); uint8_t *er = erosionN(ibin, W, H, theconf.Nerosions); FREE(ibin); DELTA("Erosion"); - savebin(er, W, H, "erosion.fits"); - DELTA("Save erosion"); + //savebin(er, W, H, "erosion.fits"); + //DELTA("Save erosion"); uint8_t *opn = dilationN(er, W, H, theconf.Ndilations); FREE(er); DELTA("Opening"); - savebin(opn, W, H, "opening.fits"); - DELTA("Save opening"); - ConnComps *cc; + //savebin(opn, W, H, "opening.fits"); + //DELTA("Save opening"); + ConnComps *cc = NULL; size_t *S = cclabel4(opn, W, H, &cc); FREE(opn); if(cc->Nobj > 1){ @@ -245,8 +259,8 @@ void process_file(Image *I){ printf("%6d\t%6d\t%6.1f\t%6.1f\t%6.1f\t%6.1f\t%6.1f\t%6.1f\n", i, o->area, 20.-1.0857*log(o->Isum), o->WdivH, o->xc, o->yc, o->xsigma, o->ysigma); } - getDeviation(Objects); - { // prepare image + getDeviation(Objects); // calculate dX/dY and process corrections + { // prepare image and save jpeg uint8_t *outp = NULL; if(theconf.equalize) outp = equalize(I, 3, theconf.throwpart); @@ -262,16 +276,29 @@ void process_file(Image *I){ Pattern_draw3(&i3, cross, Objects[i].xc, H-Objects[i].yc, C_R); // Pattern_free(&cross); don't free - static variable! } - stbi_write_jpg(GP->outputjpg, I->width, I->height, 3, outp, 95); + char *tmpnm = MALLOC(char, strlen(GP->outputjpg) + 5); + sprintf(tmpnm, "%s-tmp", GP->outputjpg); + if(stbi_write_jpg(tmpnm, I->width, I->height, 3, outp, 95)){ + if(rename(tmpnm, GP->outputjpg)){ + WARN("rename()"); + LOGWARN("can't save %s", GP->outputjpg); + } + } + FREE(tmpnm); + ++ImNumber; + if(lastTproc > 1.) FPS = 1. / (dtime() - lastTproc); + lastTproc = dtime(); FREE(outp); } - FREE(cc); FREE(Objects); + /* Image *c = ST2Im(S, W, H); DELTA("conv size_t -> Ima"); save_fits(c, "size_t.fits"); Image_free(&c); DELTA("Save size_t"); + */ + /* Image *obj = Image_sim(I); OMP_FOR() for(int y = 0; y < H; ++y){ @@ -286,17 +313,41 @@ void process_file(Image *I){ Image_minmax(obj); save_fits(obj, "object.fits"); Image_free(&obj); - } + */ + }else Image_write_jpg(I, GP->outputjpg, theconf.equalize); FREE(S); + FREE(cc); } } DELTA("End"); } +static char *localimages(const char *messageid, int isdir, char *buf, int buflen){ + static char *impath = NULL; + if(!impath){ + if(!realpath(GP->outputjpg, impath)) impath = strdup(GP->outputjpg); + } + snprintf(buf, buflen, "{ \"%s\": \"%s\", \"camstatus\": \"watch %s\", \"impath\": \"%s\"}", + MESSAGEID, messageid, isdir ? "directory" : "file", impath); + return buf; +} +static char *watchdr(const char *messageid, char *buf, int buflen){ + return localimages(messageid, 1, buf, buflen); +} +static char *watchfl(const char *messageid, char *buf, int buflen){ + return localimages(messageid, 0, buf, buflen); +} + int process_input(InputType tp, char *name){ - DBG("process_input(%d, %s)", tp, name); - if(tp == T_DIRECTORY) return watch_directory(name, process_file); - else if(tp == T_CAPT_GRASSHOPPER) return capture_grasshopper(process_file); + //DBG("process_input(%d, %s)", tp, name); + if(tp == T_DIRECTORY){ + imagedata = watchdr; + return watch_directory(name, process_file); + }else if(tp == T_CAPT_GRASSHOPPER){ + imagedata = gsimagestatus; + return capture_grasshopper(process_file); + } + imagedata = watchfl; return watch_file(name, process_file); } @@ -348,3 +399,5 @@ void setpostprocess(const char *name){ LOGERR("Unknown postprocess \"%s\"", name); } } + +double getFramesPerS(){ return FPS; } diff --git a/LocCorr/improc.h b/LocCorr/improc.h index 2a8d134..73dfcac 100644 --- a/LocCorr/improc.h +++ b/LocCorr/improc.h @@ -19,28 +19,35 @@ #ifndef IMPROC_H__ #define IMPROC_H__ +#include + #include "imagefile.h" -// tolerance of deviations by X and Y axis +// tolerance of deviations by X and Y axis (if sigmaX or sigmaY greater, values considered to be wrong) #define XY_TOLERANCE (1.) // roundness parameter -#define MINWH (0.2) -#define MAXWH (5.) +#define MINWH (0.5) +#define MAXWH (2.) #define PUSIROBO_POSTPROC "pusirobo" // how many frames will be averaged to count image deviation #define MAX_AVERAGING_ARRAY_SIZE (25) -extern int stopwork; +extern volatile atomic_bool stopwork; extern double Xtarget, Ytarget; +extern volatile atomic_ullong ImNumber; +extern float exptime; +//extern int autoExposition; +extern char *(*stepstatus)(const char *messageid, char *buf, int buflen); +extern char *(*setstepstatus)(const char *newstatus, char *buf, int buflen); +extern char *(*movefocus)(const char *newstatus, char *buf, int buflen); +extern char *(*imagedata)(const char *messageid, char *buf, int buflen); void process_file(Image *I); int process_input(InputType tp, char *name); void openXYlog(const char *name); void closeXYlog(); void setpostprocess(const char *name); -extern char *(*stepstatus)(char *buf, int buflen); -extern char *(*setstepstatus)(const char *newstatus, char *buf, int buflen); -extern char *(*movefocus)(const char *newstatus, char *buf, int buflen); +double getFramesPerS(); #endif // IMPROC_H__ diff --git a/LocCorr/main.c b/LocCorr/main.c index 06f698c..86bc0e7 100644 --- a/LocCorr/main.c +++ b/LocCorr/main.c @@ -17,6 +17,7 @@ */ #include +#include #include // signal #include // strdup #include @@ -28,6 +29,8 @@ #include "pusirobo.h" #include "socket.h" +static InputType tp; + /** * We REDEFINE the default WEAK function of signal processing */ @@ -36,21 +39,17 @@ void signals(int sig){ signal(sig, SIG_IGN); DBG("Get signal %d, quit.\n", sig); } + stopwork = TRUE; DBG("exit %d", sig); LOGERR("Exit with status %d", sig); - stopwork = 1; saveconf(NULL); - usleep(10000); - DBG("disconnectGrasshopper()"); - disconnectGrasshopper(); - DBG("pusi_disconnect()"); - pusi_disconnect(); - DBG("closeXYlog()"); - closeXYlog(); if(GP && GP->pidfile){ // remove unnesessary PID file DBG("unlink(GP->pidfile)"); unlink(GP->pidfile); } + DBG("closeXYlog()"); + closeXYlog(); + DBG("EXIT %d", sig); exit(sig); } @@ -58,12 +57,18 @@ void iffound_default(pid_t pid){ ERRX("Another copy of this process found, pid=%d. Exit.", pid); } +void *procinp_thread(_U_ void* arg){ + int p = process_input(tp, GP->inputname); + LOGDBG("process_input=%d", p); + return NULL; +} + static InputType chk_inp(const char *name){ if(!name) ERRX("Point file or directory name to monitor"); - InputType tp = chkinput(GP->inputname); - if(T_WRONG == tp) return T_WRONG; + InputType itp = chkinput(GP->inputname); + if(T_WRONG == itp) return T_WRONG; green("\n%s is a ", name); - switch(tp){ + switch(itp){ case T_DIRECTORY: printf("directory"); break; @@ -93,7 +98,7 @@ static InputType chk_inp(const char *name){ return T_WRONG; } printf("\n"); - return tp; + return itp; } int main(int argc, char *argv[]){ @@ -105,8 +110,14 @@ int main(int argc, char *argv[]){ } 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); + tp = chk_inp(GP->inputname); if(tp == T_WRONG) ERRX("Enter correct image file or directory name"); + // check ability of saving file + { + FILE *f = fopen(GP->outputjpg, "w"); + if(!f) ERR("Can't create %s", GP->outputjpg); + fclose(f); + } if(GP->logfile){ sl_loglevel lvl = LOGLEVEL_ERR; // default log level - errors int v = GP->verb; @@ -178,9 +189,18 @@ int main(int argc, char *argv[]){ LOGMSG("Start application..."); LOGDBG("xtag=%g, ytag=%g", theconf.xtarget, theconf.ytarget); openIOport(GP->ioport); - int p = process_input(tp, GP->inputname); - DBG("process_input=%d", p); - // never reached - signals(p); // clean everything - return p; + pthread_t inp_thread; + if(pthread_create(&inp_thread, NULL, procinp_thread, NULL)){ + LOGERR("pthread_create() for image input failed"); + ERR("pthread_create()"); + } + while(1){ + if(stopwork || pthread_kill(inp_thread, 0) == ESRCH){ + DBG("close"); + pthread_join(inp_thread, NULL); + DBG("out"); + return 0; + } + }; + return 0; } diff --git a/LocCorr/pusirobo.c b/LocCorr/pusirobo.c index d80d34a..257dd4d 100644 --- a/LocCorr/pusirobo.c +++ b/LocCorr/pusirobo.c @@ -45,7 +45,7 @@ #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 setFspeed "mesg F maxspeed 12800" #define Urelsteps "mesg U relmove " #define Vrelsteps "mesg V relmove " #define Fabssteps "mesg F absmove " @@ -93,6 +93,7 @@ static int sockfd = -1; // server file descriptor // current steps counters (zero at the middle) static int Uposition = 0, Vposition = 0, Fposition = 0; +static uint8_t fixerr = 0; // ==1 if can't fixed void pusi_disconnect(){ if(sockfd > -1) close(sockfd); @@ -353,7 +354,7 @@ static int move_motor(const char *movecmd, int s/*, int *counter*/){ static void process_movetomiddle_stage(){ switch(sstatus){ case SETUP_INIT: // initial moving - if(moveU(-UVmaxsteps) && moveV(-UVmaxsteps) && moveF(-Fmaxsteps*2)) + if(moveU(-UVmaxsteps) && moveV(-UVmaxsteps) && moveF(-Fmaxsteps)) sstatus = SETUP_WAITUV0; break; case SETUP_WAITUV0: // wait for both coordinates moving to zero @@ -501,8 +502,8 @@ static int process_targetstage(double X, double Y){ DBG("nhit = %d", nhit); return FALSE; } - theconf.xtarget = X; - theconf.ytarget = Y; + theconf.xtarget = X + theconf.xoff; + theconf.ytarget = Y + theconf.yoff; DBG("Got target coordinates: (%.1f, %.1f)", X, Y); saveconf(FALSE); nhit = 0; xprev = 0.; yprev = 0.; @@ -550,13 +551,19 @@ mesg F relmove 32000 * 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; + //DBG("got centroid data: %g, %g", X, Y); + static int first = TRUE; + double xtg = theconf.xtarget - theconf.xoff, ytg = theconf.ytarget - theconf.yoff; + double xdev = X - xtg, ydev = Y - ytg; + if(state != PUSI_DISCONN) first = TRUE; switch(state){ case PUSI_DISCONN: if(!pusi_connect()){ WARN("Can't reconnect"); + } + if(first){ LOGWARN("Can't reconnect"); + first = FALSE; } break; case PUSI_SETUP: // setup axes (before this state set Xtarget/Ytarget in improc.c) @@ -581,9 +588,10 @@ void pusi_process_corrections(double X, double Y, int aver){ Uposition, Vposition, xdev, ydev); if(!try2correct(xdev, ydev)){ LOGWARN("failed to correct"); + fixerr = 1; // TODO: do something here DBG("FAILED"); - } + } else fixerr = 0; } break; default: // PUSI_RELAX @@ -614,11 +622,11 @@ pusistate pusi_getstate(){ // get current status // return JSON string with different parameters -char *pusi_status(char *buf, int buflen){ +char *pusi_status(const char *messageid, char *buf, int buflen){ int l; char *bptr = buf; const char *s = NULL, *stage = NULL; - l = snprintf(bptr, buflen, "{ \"status\": "); + l = snprintf(bptr, buflen, "{ \"%s\": \"%s\", \"status\": ", MESSAGEID, messageid); buflen -= l; bptr += l; switch(state){ case PUSI_DISCONN: @@ -664,7 +672,7 @@ char *pusi_status(char *buf, int buflen){ l = snprintf(bptr, buflen, "\"findtarget\""); break; case PUSI_FIX: - l = snprintf(bptr, buflen, "\"fixing\""); + l = snprintf(bptr, buflen, "\"%s\"", fixerr ? "fixoutofrange" : "fixing"); break; default: l = snprintf(bptr, buflen, "\"unknown\""); @@ -676,11 +684,13 @@ char *pusi_status(char *buf, int buflen){ const char *motors[] = {"Umotor", "Vmotor", "Fmotor"}; const char *statuses[] = {Ustatus, Vstatus, Fstatus}; int *pos[] = {&Uposition, &Vposition, &Fposition}; + const int maxpos[] = {UVmaxsteps, UVmaxsteps, Fmaxsteps}; + const int minpos[] = {-UVmaxsteps, -UVmaxsteps, 0}; 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)?"":", "); + l = snprintf(bptr, buflen, "\"%s\": { \"status\": \"%s\", \"position\": %d, \"minpos\": %d, \"maxpos\": %d }%s", + motors[i], stat, *pos[i], minpos[i], maxpos[i], (i==2)?"":", "); buflen -= l; bptr += l; } } @@ -717,7 +727,10 @@ char *set_pusistatus(const char *newstatus, char *buf, int buflen){ if(pusi_setstate(newstate)){ snprintf(buf, buflen, OK); return buf; - }else return pusi_status(buf, buflen); + }else{ + snprintf(buf, buflen, FAIL); + return buf; + } } int L = snprintf(buf, buflen, "status '%s' undefined, allow: ", newstatus); char *ptr = buf; @@ -734,12 +747,12 @@ char *set_pusistatus(const char *newstatus, char *buf, int buflen){ // change focus char *set_pfocus(const char *newstatus, char *buf, int buflen){ if(!moving_finished(Fstatus, &Fposition)){ - snprintf(buf, buflen, "moving\n"); + snprintf(buf, buflen, FAIL); return buf; } int newval = atoi(newstatus); if(newval < 0 || newval > Fmaxsteps){ - snprintf(buf, buflen, "Bad value: %d", newval); + snprintf(buf, buflen, FAIL); }else{ if(!setF(newval)) snprintf(buf, buflen, FAIL); else snprintf(buf, buflen, OK); diff --git a/LocCorr/pusirobo.h b/LocCorr/pusirobo.h index 6364ae4..436d43e 100644 --- a/LocCorr/pusirobo.h +++ b/LocCorr/pusirobo.h @@ -35,7 +35,7 @@ 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 *pusi_status(const char *messageid, 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); diff --git a/LocCorr/socket.c b/LocCorr/socket.c index bbd4212..cad2e72 100644 --- a/LocCorr/socket.c +++ b/LocCorr/socket.c @@ -16,8 +16,9 @@ * along with this program. If not, see . */ + #include // inet_ntop -#include +#include // basename #include // INT_xxx #include // addrinfo #include @@ -25,10 +26,12 @@ #include // pthread_kill #include #include +#include #include // syscall #include // daemon #include +#include "cmdlnopts.h" #include "config.h" #include "improc.h" #include "socket.h" @@ -40,10 +43,15 @@ // Max amount of connections #define BACKLOG (10) +/* +TODO3: add 'FAIL error text' if not OK and instead all "wrong message" +*/ + + // additional commands list - getters typedef struct{ const char *command; - char *(*handler)(char *buf, int buflen); + char *(*handler)(const char *messageid, char *buf, int buflen); const char *help; } getter; // setters @@ -53,26 +61,32 @@ typedef struct{ const char *help; } setter; -static char *helpmsg(char *buf, int buflen); -static char *stepperstatus(char *buf, int buflen); +static char *helpmsg(const char *messageid, char *buf, int buflen); +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"}, {"settings", listconf, "List current configuration"}, - {"steppers", stepperstatus, "Get status of steppers' server"}, + {"canbus", stepperstatus, "Get status of CAN bus server"}, + {"imdata", getimagedata, "Get image data (status, path, FPS, counter)"}, {NULL, NULL, NULL} }; static char *setstepperstate(const char *state, char *buf, int buflen); static char *setfocusstate(const char *state, char *buf, int buflen); +//static char *setexposition(const char *expos, char *buf, int buflen); +//static char *setexposmethod(const char *expos, char *buf, int buflen); static setter setterHandlers[] = { {"stpstate", setstepperstate, "Set given steppers' server state"}, {"focus", setfocusstate, "Move focus to given value"}, +// {"exptime", setexposition, "Set exposition to new value (s)"}, +// {"expmethod", setexposmethod, "Set exposition method (\"manual\"/\"auto\")"}, {NULL, NULL, NULL} }; /**************** functions to process commands ****************/ // getters -static char *helpmsg(char *buf, int buflen){ +static char *helpmsg(_U_ const char *messageid, char *buf, int buflen){ if(get_cmd_list(buf, buflen)){ int l = strlen(buf), L = buflen - l; char *ptr = buf + l; @@ -94,9 +108,14 @@ static char *helpmsg(char *buf, int buflen){ } return NULL; } -static char *stepperstatus(char *buf, int buflen){ - if(stepstatus) return stepstatus(buf, buflen); - snprintf(buf, buflen, "not defined"); +static char *stepperstatus(const char *messageid, char *buf, int buflen){ + if(stepstatus) return stepstatus(messageid, buf, buflen); + snprintf(buf, buflen, FAIL); + return buf; +} +static char *getimagedata(const char *messageid, char *buf, int buflen){ + if(imagedata) return imagedata(messageid, buf, buflen); + else snprintf(buf, buflen, FAIL); return buf; } @@ -104,23 +123,54 @@ static char *stepperstatus(char *buf, int buflen){ 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"); + snprintf(buf, buflen, FAIL); 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"); + snprintf(buf, buflen, FAIL); return buf; } +/* +static char *setexposition(const char *expos, char *buf, int buflen){ + DBG("Set exp to %s ms", expos); + float e = atof(expos); + if(e < EXPOS_MIN || e > EXPOS_MAX){ + snprintf(buf, buflen, "bad value"); + }else{ + exptime = (float) e; + LOGMSG("Set exposition time to %gms", e); + snprintf(buf, buflen, OK); + } + return buf; +} +static char *setexposmethod(const char *expos, char *buf, int buflen){ + int good = 0; + if(strncasecmp(expos, "auto", 4) == 0){ + autoExposition = TRUE; + LOGMSG("Set exposition method to \"auto\""); + good = 1; + }else if(strncasecmp(expos, "manual", 6) == 0){ + autoExposition = FALSE; + LOGMSG("Set exposition method to \"manual\""); + good = 1; + } + if(good) snprintf(buf, buflen, OK); + else snprintf(buf, buflen, "wrong method: \"%s\"", expos); + 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 @@ -144,12 +194,13 @@ static char *processCommand(const char msg[BUFLEN], char *ans, int anslen){ break; case PAR_DOUBLE: DBG("FOUND! Double, old=%g, new=%g", *((double*)par->ptr), result.val.dblval); + *((double*)par->ptr) = result.val.dblval; break; default: - snprintf(ans, anslen, "undefined type"); + snprintf(ans, anslen, FAIL); return ans; } - snprintf(ans, anslen, "success"); + snprintf(ans, anslen, OK); return ans; }else{ setter *s = setterHandlers; @@ -166,11 +217,11 @@ static char *processCommand(const char msg[BUFLEN], char *ans, int anslen){ while(g->command){ int l = strlen(g->command); if(strncasecmp(msg, g->command, l) == 0) - return g->handler(ans, anslen); + return g->handler(g->command, ans, anslen); ++g; } } - snprintf(ans, anslen, "Message '%s' is wrong", rmnl(msg, value, BUFLEN)); + snprintf(ans, anslen, FAIL); return ans; } @@ -238,6 +289,10 @@ static void *server(void *asock){ poll_set[0].fd = sock; poll_set[0].events = POLLIN; while(1){ + if(stopwork){ + DBG("server() exit @ global stop"); + return NULL; + } 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; @@ -252,8 +307,6 @@ static void *server(void *asock){ 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 @@ -283,15 +336,6 @@ static void *server(void *asock){ } } } // 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!"); } @@ -300,13 +344,16 @@ static void *server(void *asock){ 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) - ){ + if(pthread_create(&sock_thread, NULL, server, (void*) &sock)){ LOGERR("daemon_(): pthread_create() failed"); ERR("pthread_create()"); } do{ + if(stopwork){ + DBG("kill"); + pthread_join(sock_thread, NULL); + return; + } if(pthread_kill(sock_thread, 0) == ESRCH){ // died WARNX("Sockets thread died"); LOGERR("Sockets thread died"); @@ -316,23 +363,7 @@ static void daemon_(int sock){ 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!"); } @@ -388,8 +419,7 @@ static void *connect2sock(void *data){ freeaddrinfo(res); daemon_(sock); close(sock); - LOGERR("openIOport(): UNREACHABLE CODE REACHED!"); - signals(22); + LOGWARN("openIOport(): close @ global stop"); return NULL; } @@ -405,4 +435,5 @@ void openIOport(int portN){ LOGERR("openIOport(): pthread_create() failed"); ERR("pthread_create()"); } + pthread_detach(connthread); }