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

fixed bug in XY->UV calculations

Browse Source
This commit is contained in:
Edward Emelianov 2021-08-02 12:45:23 +03:00
parent cbf42d86e5
commit 40f437b6a0
11 changed files with 370 additions and 244 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
LocCorr/config.c
View File

@ -33,6 +33,8 @@ configuration theconf = {
.minFpos=0, .minFpos=0,
.minarea=DEFAULT_MINAREA, .minarea=DEFAULT_MINAREA,
.maxarea=DEFAULT_MAXAREA, .maxarea=DEFAULT_MAXAREA,
.maxwh = 1.1,
.minwh = 0.9,
.Nerosions=DEFAULT_NEROSIONS, .Nerosions=DEFAULT_NEROSIONS,
.Ndilations=DEFAULT_NDILATIONS, .Ndilations=DEFAULT_NDILATIONS,
.xoff=0, .xoff=0,
@ -53,6 +55,7 @@ configuration theconf = {
.maxexp=EXPOS_MAX + DBL_EPSILON, .maxexp=EXPOS_MAX + DBL_EPSILON,
.minexp=EXPOS_MIN - DBL_EPSILON, .minexp=EXPOS_MIN - DBL_EPSILON,
.fixedexp=EXPOS_MIN, .fixedexp=EXPOS_MIN,
.gain = 20.,
.intensthres=DEFAULT_INTENSTHRES .intensthres=DEFAULT_INTENSTHRES
}; };
@ -62,6 +65,10 @@ static confparam parvals[] = {
"maximal area (in square pixels) of recognized star image"}, "maximal area (in square pixels) of recognized star image"},
{"minarea", PAR_INT, (void*)&theconf.minarea, 0, MINAREA-DBL_EPSILON, MAXAREA+DBL_EPSILON, {"minarea", PAR_INT, (void*)&theconf.minarea, 0, MINAREA-DBL_EPSILON, MAXAREA+DBL_EPSILON,
"minimal area (in square pixels) of recognized star image"}, "minimal area (in square pixels) of recognized star image"},
{"minwh", PAR_DOUBLE, (void*)&theconf.minwh, 0, MINWH-DBL_EPSILON, 1.,
"minimal value of W/H roundness parameter"},
{"maxwh", PAR_DOUBLE, (void*)&theconf.maxwh, 0, 1., MAXWH+DBL_EPSILON,
"maximal value of W/H roundness parameter"},
{"ndilat", PAR_INT, (void*)&theconf.Ndilations, 0, 1.-DBL_EPSILON, MAX_NDILAT+DBL_EPSILON, {"ndilat", PAR_INT, (void*)&theconf.Ndilations, 0, 1.-DBL_EPSILON, MAX_NDILAT+DBL_EPSILON,
"amount of dilations on binarized image"}, "amount of dilations on binarized image"},
{"neros", PAR_INT, (void*)&theconf.Nerosions, 0, 1.-DBL_EPSILON, MAX_NEROS+DBL_EPSILON, {"neros", PAR_INT, (void*)&theconf.Nerosions, 0, 1.-DBL_EPSILON, MAX_NEROS+DBL_EPSILON,
@ -84,6 +91,10 @@ static confparam parvals[] = {
"maximal value of steps on U semi-axe"}, "maximal value of steps on U semi-axe"},
{"vmax", PAR_INT, (void*)&theconf.maxVsteps, 0, MINSTEPS-DBL_EPSILON, MAXSTEPS+DBL_EPSILON, {"vmax", PAR_INT, (void*)&theconf.maxVsteps, 0, MINSTEPS-DBL_EPSILON, MAXSTEPS+DBL_EPSILON,
"maximal value of steps on V semi-axe"}, "maximal value of steps on V semi-axe"},
{"focmax", PAR_INT, (void*)&theconf.maxFpos, 0, 0., Fmaxsteps,
"maximal focus position in microsteps"},
{"focmin", PAR_INT, (void*)&theconf.minFpos, 0, -Fmaxsteps, 0.,
"minimal focus position in microsteps"},
{"stpservport", PAR_INT, (void*)&theconf.stpserverport, 0, -DBL_EPSILON, 65536., {"stpservport", PAR_INT, (void*)&theconf.stpserverport, 0, -DBL_EPSILON, 65536.,
"port number of steppers' server"}, "port number of steppers' server"},
{"Kxu", PAR_DOUBLE, (void*)&theconf.Kxu, 0, KUVMIN-DBL_EPSILON, KUVMAX+DBL_EPSILON, {"Kxu", PAR_DOUBLE, (void*)&theconf.Kxu, 0, KUVMIN-DBL_EPSILON, KUVMAX+DBL_EPSILON,
@ -110,13 +121,10 @@ static confparam parvals[] = {
"threshold by total object intensity when sorting = |I1-I2|/(I1+I2)"}, "threshold by total object intensity when sorting = |I1-I2|/(I1+I2)"},
{"gain", PAR_DOUBLE, (void*)&theconf.gain, 0, GAIN_MIN-DBL_EPSILON, GAIN_MAX+DBL_EPSILON, {"gain", PAR_DOUBLE, (void*)&theconf.gain, 0, GAIN_MIN-DBL_EPSILON, GAIN_MAX+DBL_EPSILON,
"gain value in manual mode"}, "gain value in manual mode"},
{"brightness", PAR_DOUBLE, (void*)&theconf.brightness, 0, BRIGHT_MIN-DBL_EPSILON, BRIGHT_MAX-DBL_EPSILON,
"brightness value"},
{"starssort", PAR_INT, (void*)&theconf.starssort, 0, -DBL_EPSILON, 1.+DBL_EPSILON, {"starssort", PAR_INT, (void*)&theconf.starssort, 0, -DBL_EPSILON, 1.+DBL_EPSILON,
"stars sorting algorithm: by distance from target (0) or by intensity (1)"}, "stars sorting algorithm: by distance from target (0) or by intensity (1)"},
// immutable parameters (max<min -> user can't change)
{"focmax", PAR_INT, (void*)&theconf.maxFpos, 0, 1., 0.,
"maximal focus position in microsteps"},
{"focmin", PAR_INT, (void*)&theconf.minFpos, 0, 1., 0.,
"minimal focus position in microsteps"},
{NULL, 0, NULL, 0, 0., 0., NULL} {NULL, 0, NULL, 0, 0., 0., NULL}
}; };

14
LocCorr/config.h
View File

@ -36,21 +36,27 @@
#define MAX_OFFSET (10000) #define MAX_OFFSET (10000)
// min/max exposition in ms // min/max exposition in ms
#define EXPOS_MIN (0.1) #define EXPOS_MIN (0.1)
#define EXPOS_MAX (4000.) #define EXPOS_MAX (4001.)
#define GAIN_MIN (0.) #define GAIN_MIN (0.)
#define GAIN_MAX (20.) #define GAIN_MAX (33.)
#define BRIGHT_MIN (0.)
#define BRIGHT_MAX (1000.)
// max average images counter // max average images counter
#define NAVER_MAX (50) #define NAVER_MAX (50)
// coefficients to convert dx,dy to du,dv // coefficients to convert dx,dy to du,dv
#define KUVMIN (-5000.) #define KUVMIN (-5000.)
#define KUVMAX (5000.) #define KUVMAX (5000.)
// default coefficient for corrections (move to Kdu, Kdv instead of du, dv) // default coefficient for corrections (move to Kdu, Kdv instead of du, dv)
#define KCORR (0.9) #define KCORR (0.97)
// exposition methods: 0 - auto, 1 - fixed // exposition methods: 0 - auto, 1 - fixed
#define EXPAUTO (0) #define EXPAUTO (0)
#define EXPMANUAL (1) #define EXPMANUAL (1)
// roundness parameter
#define MINWH (0.3)
#define MAXWH (3.)
// messageID field name // messageID field name
#define MESSAGEID "messageid" #define MESSAGEID "messageid"
@ -75,6 +81,7 @@ typedef struct{
// dU = Kxu*dX + Kyu*dY; dV = Kxv*dX + Kyv*dY // dU = Kxu*dX + Kyu*dY; dV = Kxv*dX + Kyv*dY
double Kxu; double Kyu; double Kxu; double Kyu;
double Kxv; double Kyv; double Kxv; double Kyv;
double minwh; double maxwh; // roundness parameters
double xtarget; // target (center) values (in absolute coordinates! screen coords = target - offset) double xtarget; // target (center) values (in absolute coordinates! screen coords = target - offset)
double ytarget; double ytarget;
double throwpart; // part of values to throw avay @ histogram equalisation double throwpart; // part of values to throw avay @ histogram equalisation
@ -82,6 +89,7 @@ typedef struct{
double minexp; double minexp;
double fixedexp; // exptime in manual mode double fixedexp; // exptime in manual mode
double gain; // gain value in manual mode 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 double intensthres; // threshold for stars intensity comparison: fabs(Ia-Ib)/(Ia+Ib) > thres -> stars differs
} configuration; } configuration;

31
LocCorr/grasshopper.c
View File

@ -34,7 +34,9 @@
static fc2Context context; static fc2Context context;
static fc2PGRGuid guid; static fc2PGRGuid guid;
static fc2Error err = FC2_ERROR_OK; static fc2Error err = FC2_ERROR_OK;
static float gain = 0.; static float gain = 20.;
static float exptime = 100.;
static float brightness = 0.;
#define FC2FN(fn, ...) do{err = FC2_ERROR_OK; if(FC2_ERROR_OK != (err=fn(context __VA_OPT__(,) __VA_ARGS__))){ \ #define FC2FN(fn, ...) do{err = FC2_ERROR_OK; if(FC2_ERROR_OK != (err=fn(context __VA_OPT__(,) __VA_ARGS__))){ \
WARNX(#fn "(): %s", fc2ErrorToDescription(err)); return 0;}}while(0) WARNX(#fn "(): %s", fc2ErrorToDescription(err)); return 0;}}while(0)
@ -186,7 +188,7 @@ static int connect(){
} }
static int GrabImage(fc2Image *convertedImage){ static int GrabImage(fc2Image *convertedImage){
FNAME(); //FNAME();
int ret = 0; int ret = 0;
fc2Image rawImage; fc2Image rawImage;
// start capture // start capture
@ -282,6 +284,7 @@ int capture_grasshopper(void (*process)(Image*)){
FNAME(); FNAME();
static float oldexptime = 0.; static float oldexptime = 0.;
static float oldgain = -1.; static float oldgain = -1.;
static float oldbrightness = -1.;
Image *oIma = NULL; Image *oIma = NULL;
fc2Image convertedImage; fc2Image convertedImage;
err = fc2CreateImage(&convertedImage); err = fc2CreateImage(&convertedImage);
@ -300,24 +303,28 @@ int capture_grasshopper(void (*process)(Image*)){
sleep(1); sleep(1);
continue; continue;
} }
if(fabsf(oldbrightness - brightness) > FLT_EPSILON){ // new brightness
DBG("Change brightness to %g", brightness);
if(setbrightness(brightness)){
oldbrightness = brightness;
}else{
WARNX("Can't change brightness to %g", brightness);
}
}
if(fabsf(oldexptime - exptime) > FLT_EPSILON){ // new exsposition value if(fabsf(oldexptime - exptime) > FLT_EPSILON){ // new exsposition value
red("Change exptime to %.2fms\n", exptime); DBG("Change exptime to %.2fms\n", exptime);
if(setexp(exptime)){ if(setexp(exptime)){
oldexptime = exptime; oldexptime = exptime;
}else{ }else{
WARNX("Can't change exposition time to %gms", exptime); WARNX("Can't change exposition time to %gms", exptime);
//disconnectGrasshopper();
//continue;
} }
} }
if(fabs(oldgain - gain) > FLT_EPSILON){ // change gain if(fabsf(oldgain - gain) > FLT_EPSILON){ // change gain
red("Change gain to %g\n", gain); DBG("Change gain to %g\n", gain);
if(setgain(gain)){ if(setgain(gain)){
oldgain = gain; oldgain = gain;
}else{ }else{
WARNX("Can't change gain to %g", gain); WARNX("Can't change gain to %g", gain);
//disconnectGrasshopper();
//continue;
} }
} }
if(!GrabImage(&convertedImage)){ if(!GrabImage(&convertedImage)){
@ -331,6 +338,8 @@ int capture_grasshopper(void (*process)(Image*)){
exptime = theconf.fixedexp; exptime = theconf.fixedexp;
if(fabs(theconf.gain - gain) > FLT_EPSILON) if(fabs(theconf.gain - gain) > FLT_EPSILON)
gain = theconf.gain; gain = theconf.gain;
if(fabs(theconf.brightness - brightness) > FLT_EPSILON)
brightness = theconf.brightness;
} }
if(!process){ if(!process){
continue; continue;
@ -359,8 +368,8 @@ char *gsimagestatus(const char *messageid, char *buf, int buflen){
DBG("path: %s", impath); DBG("path: %s", impath);
} }
snprintf(buf, buflen, "{ \"%s\": \"%s\", \"camstatus\": \"%sconnected\", \"impath\": \"%s\", \"imctr\": %llu, " snprintf(buf, buflen, "{ \"%s\": \"%s\", \"camstatus\": \"%sconnected\", \"impath\": \"%s\", \"imctr\": %llu, "
"\"fps\": %.3f, \"expmethod\": \"%s\", \"exposition\": %g, \"gain\": %g }\n", "\"fps\": %.3f, \"expmethod\": \"%s\", \"exposition\": %g, \"gain\": %g, \"brightness\": %g }\n",
MESSAGEID, messageid, connected ? "" : "dis", impath, ImNumber, getFramesPerS(), MESSAGEID, messageid, connected ? "" : "dis", impath, ImNumber, getFramesPerS(),
(theconf.expmethod == EXPAUTO) ? "auto" : "manual", exptime, gain); (theconf.expmethod == EXPAUTO) ? "auto" : "manual", exptime, gain, brightness);
return buf; return buf;
} }

6
LocCorr/imagefile.c
View File

@ -135,7 +135,7 @@ Image *u8toImage(uint8_t *data, int width, int height, int stride){
} }
outp->minval = (Imtype) min; outp->minval = (Imtype) min;
outp->maxval = (Imtype) max; outp->maxval = (Imtype) max;
DBG("\nMAX=%g, MIN=%g\n", outp->maxval, outp->minval); //DBG("\nMAX=%g, MIN=%g\n", outp->maxval, outp->minval);
outp->data = MALLOC(Imtype, width*height); outp->data = MALLOC(Imtype, width*height);
// flip image updown for FITS coordinate system // flip image updown for FITS coordinate system
OMP_FOR() OMP_FOR()
@ -277,7 +277,7 @@ uint8_t *equalize(const Image *I, int nchannels, double throwpart){
Nwhite += orig_hysto[stopidx]; Nwhite += orig_hysto[stopidx];
if(Nwhite >= wpart) break; if(Nwhite >= wpart) break;
}*/ }*/
DBG("Throw %d (real: %d black) pixels, startidx=%d", bpart, Nblack, startidx); //DBG("Throw %d (real: %d black) pixels, startidx=%d", bpart, Nblack, startidx);
/* /*
double part = (double)(s + 1) / 256., N = 0.; double part = (double)(s + 1) / 256., N = 0.;
for(int i = 0; i < 256; ++i){ for(int i = 0; i < 256; ++i){
@ -332,7 +332,7 @@ int Image_write_jpg(const Image *I, const char *name, int eq){
outp = equalize(I, 1, theconf.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);
char *tmpnm = MALLOC(char, strlen(name) + 5); char *tmpnm = MALLOC(char, strlen(name) + 5);
sprintf(tmpnm, "%s-tmp", name); sprintf(tmpnm, "%s-tmp", name);
int r = stbi_write_jpg(tmpnm, I->width, I->height, 1, outp, 95); int r = stbi_write_jpg(tmpnm, I->width, I->height, 1, outp, 95);

22
LocCorr/improc.c
View File

@ -34,7 +34,6 @@
#include "median.h" #include "median.h"
#include "pusirobo.h" #include "pusirobo.h"
float exptime = 10.; // GLOBAL: exposition time in milliseconds
volatile atomic_ullong ImNumber = 0; // GLOBAL: counter of processed images volatile atomic_ullong ImNumber = 0; // GLOBAL: counter of processed images
volatile atomic_bool stopwork = FALSE; // GLOBAL: suicide volatile atomic_bool stopwork = FALSE; // GLOBAL: suicide
//int autoExposition = 1; // GLOBAL: ==1 if exposition calculation is auto //int autoExposition = 1; // GLOBAL: ==1 if exposition calculation is auto
@ -47,6 +46,8 @@ char *(*setstepstatus)(const char *newstatus, char *buf, int buflen) = NULL;
char *(*movefocus)(const char *newstatus, char *buf, int buflen) = NULL; char *(*movefocus)(const char *newstatus, char *buf, int buflen) = NULL;
// GLOBAL: get image information // GLOBAL: get image information
char *(*imagedata)(const char *messageid, char *buf, int buflen); char *(*imagedata)(const char *messageid, char *buf, int buflen);
// GLOBAL: disconnect stepper server
void (*stepdisconnect)() = NULL;
static FILE *fXYlog = NULL; static FILE *fXYlog = NULL;
static double tstart = 0.; // time of logging start static double tstart = 0.; // time of logging start
@ -54,7 +55,7 @@ static double FPS = 0.; // frames per second
// function to process calculated corrections // function to process calculated corrections
static void (*proc_corr)(double, double, int) = NULL; static void (*proc_corr)(double, double) = NULL;
typedef struct{ typedef struct{
uint32_t area; // object area in pixels uint32_t area; // object area in pixels
@ -152,11 +153,11 @@ static void getDeviation(object *curobj){
averflag = 1; averflag = 1;
if(fXYlog) fprintf(fXYlog, "%.1f\t%.1f\t%.1f\t%.1f", xx, yy, Sx, Sy); if(fXYlog) fprintf(fXYlog, "%.1f\t%.1f\t%.1f\t%.1f", xx, yy, Sx, Sy);
process_corrections: process_corrections:
if(proc_corr){ if(proc_corr && averflag){
if(Sx > XY_TOLERANCE || Sy > XY_TOLERANCE){ if(Sx > XY_TOLERANCE || Sy > XY_TOLERANCE){
LOGDBG("Bad value - not process"); // don't run processing for bad data LOGDBG("Bad value - not process"); // don't run processing for bad data
}else }else
proc_corr(xx, yy, averflag); proc_corr(xx, yy);
} }
XYnewline(); XYnewline();
} }
@ -214,7 +215,7 @@ 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 < MINWH || wh > MAXWH) continue; if(wh < theconf.minwh || wh > theconf.maxwh) continue;
if((int)b->area < theconf.minarea || (int)b->area > theconf.maxarea) 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.;
@ -266,12 +267,16 @@ void process_file(Image *I){
outp = equalize(I, 3, theconf.throwpart); outp = equalize(I, 3, theconf.throwpart);
else else
outp = linear(I, 3); outp = linear(I, 3);
static Pattern *cross = NULL;
if(!cross) cross = Pattern_cross(33, 33);
Img3 i3 = {.data = outp, .w = I->width, .h = H};
// draw fiber center position
Pattern_draw3(&i3, cross, theconf.xtarget-theconf.xoff, H-(theconf.ytarget-theconf.yoff), C_B);
if(objctr){ // draw crosses @ objects' centers if(objctr){ // draw crosses @ objects' centers
static Pattern *cross = NULL;
if(!cross) cross = Pattern_cross(33, 33);
int H = I->height; int H = I->height;
Img3 i3 = {.data = outp, .w = I->width, .h = H}; // draw current star centroid
Pattern_draw3(&i3, cross, Objects[0].xc, H-Objects[0].yc, C_G); Pattern_draw3(&i3, cross, Objects[0].xc, H-Objects[0].yc, C_G);
// draw other centroids
for(int i = 1; i < objctr; ++i) for(int i = 1; i < objctr; ++i)
Pattern_draw3(&i3, cross, Objects[i].xc, H-Objects[i].yc, C_R); Pattern_draw3(&i3, cross, Objects[i].xc, H-Objects[i].yc, C_R);
// Pattern_free(&cross); don't free - static variable! // Pattern_free(&cross); don't free - static variable!
@ -390,6 +395,7 @@ void setpostprocess(const char *name){
WARNX("Pusiserver unavailable, will check later"); WARNX("Pusiserver unavailable, will check later");
LOGWARN("Pusiserver unavailable, will check later"); LOGWARN("Pusiserver unavailable, will check later");
} }
stepdisconnect = pusi_stop;
proc_corr = pusi_process_corrections; proc_corr = pusi_process_corrections;
stepstatus = pusi_status; stepstatus = pusi_status;
setstepstatus = set_pusistatus; setstepstatus = set_pusistatus;

6
LocCorr/improc.h
View File

@ -25,23 +25,19 @@
// tolerance of deviations by X and Y axis (if sigmaX or sigmaY greater, values considered to be wrong) // tolerance of deviations by X and Y axis (if sigmaX or sigmaY greater, values considered to be wrong)
#define XY_TOLERANCE (1.) #define XY_TOLERANCE (1.)
// roundness parameter
#define MINWH (0.5)
#define MAXWH (2.)
#define PUSIROBO_POSTPROC "pusirobo" #define PUSIROBO_POSTPROC "pusirobo"
// how many frames will be averaged to count image deviation // how many frames will be averaged to count image deviation
#define MAX_AVERAGING_ARRAY_SIZE (25) #define MAX_AVERAGING_ARRAY_SIZE (25)
extern volatile atomic_bool stopwork; extern volatile atomic_bool stopwork;
extern double Xtarget, Ytarget;
extern volatile atomic_ullong ImNumber; extern volatile atomic_ullong ImNumber;
extern float exptime;
//extern int autoExposition; //extern int autoExposition;
extern char *(*stepstatus)(const char *messageid, char *buf, int buflen); extern char *(*stepstatus)(const char *messageid, char *buf, int buflen);
extern char *(*setstepstatus)(const char *newstatus, 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 *(*movefocus)(const char *newstatus, char *buf, int buflen);
extern char *(*imagedata)(const char *messageid, char *buf, int buflen); extern char *(*imagedata)(const char *messageid, char *buf, int buflen);
extern void (*stepdisconnect)();
void process_file(Image *I); void process_file(Image *I);
int process_input(InputType tp, char *name); int process_input(InputType tp, char *name);

32
LocCorr/loccorr.conf Normal file
View File

@ -0,0 +1,32 @@
maxarea = 10000
minarea = 100
minwh = 0.800
maxwh = 1.300
ndilat = 4
neros = 5
xoffset = 0
yoffset = 0
width = 0
height = 0
equalize = 0
expmethod = 1
naverage = 5
umax = 16000
vmax = 16000
focmax = 32000
focmin = -32000
stpservport = 4444
Kxu = 28.047
Kyu = 90.067
Kxv = 69.539
Kyv = -66.882
xtarget = 810.335
ytarget = 518.906
eqthrowpart = 0.900
minexp = 50.000
maxexp = 3999.000
fixedexp = 700.000
intensthres = 0.010
gain = 20.000
brightness = 0.000
starssort = 0

3
LocCorr/main.c
View File

@ -41,15 +41,16 @@ void signals(int sig){
} }
stopwork = TRUE; stopwork = TRUE;
DBG("exit %d", sig); DBG("exit %d", sig);
LOGERR("Exit with status %d", sig);
saveconf(NULL); saveconf(NULL);
if(GP && GP->pidfile){ // remove unnesessary PID file if(GP && GP->pidfile){ // remove unnesessary PID file
DBG("unlink(GP->pidfile)"); DBG("unlink(GP->pidfile)");
unlink(GP->pidfile); unlink(GP->pidfile);
} }
if(stepdisconnect) stepdisconnect();
DBG("closeXYlog()"); DBG("closeXYlog()");
closeXYlog(); closeXYlog();
DBG("EXIT %d", sig); DBG("EXIT %d", sig);
LOGERR("Exit with status %d", sig);
exit(sig); exit(sig);
} }

428
LocCorr/pusirobo.c
View File

@ -20,6 +20,7 @@
#include <math.h> #include <math.h>
#include <netdb.h> #include <netdb.h>
#include <pthread.h> #include <pthread.h>
#include <stdatomic.h>
#include <stdio.h> #include <stdio.h>
#include <string.h> #include <string.h>
#include <sys/types.h> #include <sys/types.h>
@ -27,6 +28,7 @@
#include <usefull_macros.h> #include <usefull_macros.h>
#include "config.h" #include "config.h"
#include "improc.h" // global variable stopwork
#include "pusirobo.h" #include "pusirobo.h"
#include "socket.h" #include "socket.h"
@ -37,14 +39,14 @@
// amount of consequent center coordinates coincidence in `process_targetstate` // amount of consequent center coordinates coincidence in `process_targetstate`
#define NCONSEQ (2) #define NCONSEQ (2)
// tolerance of coordinates coincidence (pix) // tolerance of coordinates coincidence (pix)
#define COORDTOLERANCE (0.1) #define COORDTOLERANCE (0.5)
// messages for CAN server // messages for CAN server
#define registerUaxe "register U 0x581 stepper" #define registerUaxe "register U 0x581 stepper"
#define registerVaxe "register V 0x582 stepper" #define registerVaxe "register V 0x582 stepper"
#define registerFocus "register F 0x583 stepper" #define registerFocus "register F 0x583 stepper"
#define setUspeed "mesg U maxspeed 12800" #define setUspeed "mesg U maxspeed 22400"
#define setVspeed "mesg V maxspeed 12800" #define setVspeed "mesg V maxspeed 22400"
#define setFspeed "mesg F maxspeed 12800" #define setFspeed "mesg F maxspeed 12800"
#define Urelsteps "mesg U relmove " #define Urelsteps "mesg U relmove "
#define Vrelsteps "mesg V relmove " #define Vrelsteps "mesg V relmove "
@ -62,16 +64,24 @@
#define ERRstatus "errstatus" #define ERRstatus "errstatus"
#define CURPOSstatus "curpos" #define CURPOSstatus "curpos"
// max range of U and V motors (all in microsteps!) // max range of U and V motors (all in microsteps!)
#define UVmaxsteps (35200) #define UVmaxsteps (96000)
// steps to move from the edge // steps to move from the edge
#define UVedgesteps (960) #define UVedgesteps (3200)
#define moveU(s) move_motor(Urelsteps, s) #define moveU(s) move_motor(Urelsteps, s)
#define moveV(s) move_motor(Vrelsteps, s) #define moveV(s) move_motor(Vrelsteps, s)
#define moveF(s) move_motor(Frelsteps, s) #define moveF(s) move_motor(Frelsteps, s)
#define setF(s) move_motor(Fabssteps, s) #define setF(s) move_motor(Fabssteps, s)
#define UVmoving_finished() (moving_finished(Ustatus, NULL) && moving_finished(Vstatus, NULL))
typedef enum{
PUSI_DISCONN,
PUSI_RELAX,
PUSI_SETUP,
PUSI_GOTOTHEMIDDLE,
PUSI_FINDTARGET,
PUSI_FIX,
PUSI_UNDEFINED
} pusistate;
typedef enum{ typedef enum{
SETUP_NONE, // no setup SETUP_NONE, // no setup
@ -84,9 +94,18 @@ typedef enum{
SETUP_WAITVMAX, // V->max SETUP_WAITVMAX, // V->max
SETUP_FINISH SETUP_FINISH
} setupstatus; } setupstatus;
static setupstatus sstatus = SETUP_NONE; // setup state static _Atomic setupstatus sstatus = SETUP_NONE; // setup state
static pusistate state = PUSI_DISCONN; // server state static pusistate state = PUSI_DISCONN; // server state
// the `ismoving` flag allows not to make corrections with bad images made when moving
static volatile atomic_bool ismoving = FALSE; // == TRUE if any of steppers @hanging part is moving
// this flag set to TRUE when next Xc,Yc available
static volatile atomic_bool coordsRdy = FALSE;
static double Xtarget = 0., Ytarget = 0.;
// flag & new focus value
static volatile atomic_bool chfocus = FALSE;
static volatile atomic_int newfocpos = 0;
static int sockfd = -1; // server file descriptor static int sockfd = -1; // server file descriptor
@ -94,12 +113,14 @@ static int sockfd = -1; // server file descriptor
static pthread_mutex_t sendmesg_mutex = PTHREAD_MUTEX_INITIALIZER; static pthread_mutex_t sendmesg_mutex = PTHREAD_MUTEX_INITIALIZER;
// current steps counters (zero at the middle) // current steps counters (zero at the middle)
static int Uposition = 0, Vposition = 0, Fposition = 0; static volatile atomic_int Uposition = 0, Vposition = 0, Fposition = 0;
static volatile atomic_bool Umoving = FALSE, Vmoving = FALSE, Fmoving = FALSE;
static uint8_t fixerr = 0; // ==1 if can't fixed static uint8_t fixerr = 0; // ==1 if can't fixed
void pusi_disconnect(){ static void pusi_disconnect(){
if(sockfd > -1) close(sockfd); if(sockfd > -1) close(sockfd);
sockfd = -1; sockfd = -1;
Umoving = Vmoving = Fmoving = ismoving = FALSE;
state = PUSI_DISCONN; state = PUSI_DISCONN;
} }
@ -209,7 +230,9 @@ static int waitOK(char **retval){
*retval = strdup(ok + sizeof(ANSOK)-1); *retval = strdup(ok + sizeof(ANSOK)-1);
//DBG("RETVAL: '%s'", *retval); //DBG("RETVAL: '%s'", *retval);
} }
}else LOGWARN("didn't get OK answer"); }else{
LOGWARN("didn't get OK answer");
}
#undef BUFFERSZ #undef BUFFERSZ
return ret; return ret;
} }
@ -230,7 +253,7 @@ static int send_message(const char *msg, char **ans){
LOGWARN("send_message(): send() failed"); LOGWARN("send_message(): send() failed");
return FALSE; return FALSE;
} }
DBG("Message '%s' sent", msg); //DBG("Message '%s' sent", msg);
int r = waitOK(ans); int r = waitOK(ans);
pthread_mutex_unlock(&sendmesg_mutex); pthread_mutex_unlock(&sendmesg_mutex);
return r; return r;
@ -267,10 +290,11 @@ static int setSpeed(const char *mesg, const char *name){
} }
/** /**
* @brief pusi_connect - connect to a local steppers CAN server * @brief pusi_connect_server - try connect to a local steppers CAN server
* @return FALSE if failed * @return FALSE if failed
*/ */
int pusi_connect(){ static int pusi_connect_server(){
Umoving = Fmoving = Vmoving = ismoving = FALSE;
DBG("pusi_connect(%d)", theconf.stpserverport); DBG("pusi_connect(%d)", theconf.stpserverport);
char port[10]; char port[10];
snprintf(port, 10, "%d", theconf.stpserverport); snprintf(port, 10, "%d", theconf.stpserverport);
@ -318,15 +342,38 @@ int pusi_connect(){
return retval; return retval;
} }
static void *pusi_process_states(_U_ void *arg);
static pthread_t processingthread;
/**
* @brief pusi_connect - run a thread processed steppers status
* @return FALSE if failed to connect immediately
*/
int pusi_connect(){
int c = pusi_connect_server();
if(pthread_create(&processingthread, NULL, pusi_process_states, NULL)){
LOGERR("pthread_create() for pusirobo server failed");
ERR("pthread_create()");
}
return c;
}
// stop processing & disconnect
void pusi_stop(){
pthread_join(processingthread, NULL);
pusi_disconnect();
}
// return TRUE if motor is stopped // return TRUE if motor is stopped
static int moving_finished(const char *mesgstatus, int *position){ static int moving_finished(const char *mesgstatus, volatile atomic_int *position){
double val; double val;
char *ans = NULL; char *ans = NULL;
int ret = TRUE; int ret = TRUE;
if(send_message(mesgstatus, &ans) && getparval(PARstatus, ans, &val)){ if(send_message(mesgstatus, &ans) && getparval(PARstatus, ans, &val)){
DBG("send(%s) true: %s %g\n", mesgstatus, ans, val); //DBG("send(%s) true: %s %g\n", mesgstatus, ans, val);
}else{ }else{
WARNX("send(%s) false: %s %g\n", mesgstatus, ans, val);
LOGDBG("send(%s) false: %s %g\n", mesgstatus, ans, val); LOGDBG("send(%s) false: %s %g\n", mesgstatus, ans, val);
pusi_disconnect();
return FALSE; return FALSE;
} }
int ival = (int)val; int ival = (int)val;
@ -334,25 +381,31 @@ static int moving_finished(const char *mesgstatus, int *position){
if(position){ if(position){
if(getparval(CURPOSstatus, ans, &val)){ if(getparval(CURPOSstatus, ans, &val)){
*position = (int) val; *position = (int) val;
}else LOGDBG("%s not found in '%s'", CURPOSstatus, ans); }else{
WARNX("%s not found in '%s'", CURPOSstatus, ans);
LOGDBG("%s not found in '%s'", CURPOSstatus, ans);
}
} }
FREE(ans); FREE(ans);
return ret; return ret;
} }
// move motor to s steps, @return FALSE if failed // move motor to s steps, @return FALSE if failed
static int move_motor(const char *movecmd, int s/*, int *counter*/){ static int move_motor(const char *movecmd, int s){
DBG("move %s -> %d", movecmd, s); DBG("move %s -> %d", movecmd, s);
LOGDBG("move %s -> %d", movecmd, s); LOGDBG("move %s -> %d", movecmd, s);
char buf[256], *ans; char buf[256], *ans;
snprintf(buf, 255, "%s %d", movecmd, s); snprintf(buf, 255, "%s %d", movecmd, s);
if(!send_message(buf, &ans)){ if(!send_message(buf, &ans)){
WARNX("can't send message");
LOGDBG("can't send message"); LOGDBG("can't send message");
pusi_disconnect();
return FALSE; return FALSE;
} }
int ret = TRUE; int ret = TRUE;
if(!getOKval(STEPSstatus, ans)){ if(!getOKval(STEPSstatus, ans)){
LOGDBG("NO OK in %s", ans); WARNX("NO OK in %s", ans);
LOGWARN("NO OK in %s", ans);
ret = FALSE; ret = FALSE;
} }
FREE(ans); FREE(ans);
@ -362,27 +415,27 @@ static int move_motor(const char *movecmd, int s/*, int *counter*/){
static void process_movetomiddle_stage(){ static void process_movetomiddle_stage(){
switch(sstatus){ switch(sstatus){
case SETUP_INIT: // initial moving case SETUP_INIT: // initial moving
if(moveU(-UVmaxsteps) && moveV(-UVmaxsteps) && moveF(-Fmaxsteps)) if(moveF(-Fmaxsteps) && moveU(-UVmaxsteps) && moveV(-UVmaxsteps))
sstatus = SETUP_WAITUV0; sstatus = SETUP_WAITUV0;
break; break;
case SETUP_WAITUV0: // wait for both coordinates moving to zero case SETUP_WAITUV0: // wait for both coordinates moving to zero
DBG("Moving to left border"); DBG("Reached UVF0!");
if(!(UVmoving_finished() && moving_finished(Fstatus, NULL))) return; if(moveF(Fmaxsteps/2) && moveU(theconf.maxUsteps+UVedgesteps) && moveV(theconf.maxVsteps+UVedgesteps))
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; sstatus = SETUP_WAITUVMID;
else{
LOGWARN("GOTO middle: err in move command");
sstatus = SETUP_INIT;
}
break; break;
case SETUP_WAITUVMID: // wait for the middle case SETUP_WAITUVMID: // wait for the middle
DBG("Moving to the middle"); DBG("Reached middle position");
if(!(UVmoving_finished() && moving_finished(Fstatus, NULL))) return; if(!send_message(Fsetzero, NULL) || !send_message(Usetzero, NULL) || !send_message(Vsetzero, NULL)){
DBG("Reached!"); LOGWARN("GOTO middle: err in set 0 command");
Uposition = 0; Vposition = 0; Fposition = Fmaxsteps/2; sstatus = SETUP_INIT;
if(!send_message(Usetzero, NULL) || !send_message(Vsetzero, NULL)) return; return;
sstatus = SETUP_NONE; }
state = PUSI_RELAX; Uposition = Vposition = Fposition = 0;
break; // fallthrough
default: default:
sstatus = SETUP_NONE; sstatus = SETUP_NONE;
state = PUSI_RELAX; state = PUSI_RELAX;
@ -392,9 +445,8 @@ static void process_movetomiddle_stage(){
/** /**
* @brief process_setup_stage - process all stages of axes setup * @brief process_setup_stage - process all stages of axes setup
*/ */
static void process_setup_stage(double x, double y, int aver){ static void process_setup_stage(){
DBG("PROCESS: %d\n", sstatus); DBG("PROCESS: %d\n", sstatus);
static int ctr; // iterations counter
// coordinates for corrections calculation // coordinates for corrections calculation
static double X0U, Y0U, XmU, YmU; static double X0U, Y0U, XmU, YmU;
static double X0V, Y0V, XmV, YmV; static double X0V, Y0V, XmV, YmV;
@ -404,54 +456,61 @@ static void process_setup_stage(double x, double y, int aver){
sstatus = SETUP_WAITUV0; sstatus = SETUP_WAITUV0;
break; break;
case SETUP_WAITUV0: // wait for both coordinates moving to zero case SETUP_WAITUV0: // wait for both coordinates moving to zero
DBG("Moving to left border"); DBG("Left border reached");
if(!UVmoving_finished()) return;
DBG("Reached!");
if(moveU(theconf.maxUsteps+UVedgesteps) && moveV(theconf.maxUsteps+UVedgesteps)) if(moveU(theconf.maxUsteps+UVedgesteps) && moveV(theconf.maxUsteps+UVedgesteps))
sstatus = SETUP_WAITUVMID; sstatus = SETUP_WAITUVMID;
else{
LOGWARN("Can't move U/V -> 0");
sstatus = SETUP_INIT;
}
break; break;
case SETUP_WAITUVMID: // wait for the middle case SETUP_WAITUVMID: // wait for the middle
DBG("Moving to the middle"); DBG("The middle reached");
if(!UVmoving_finished()) return;
DBG("Reached!");
Uposition = 0; Vposition = 0;
if(moveU(-theconf.maxUsteps)) sstatus = SETUP_WAITU0; if(moveU(-theconf.maxUsteps)) sstatus = SETUP_WAITU0;
ctr = 0; else{
LOGWARN("Can't move U -> middle");
sstatus = SETUP_INIT;
}
break; break;
case SETUP_WAITU0: // wait while U moves to zero case SETUP_WAITU0: // wait while U moves to zero
if(!aver) return; if(!coordsRdy) return;
if(!moving_finished(Ustatus, NULL)) return; coordsRdy = FALSE;
if(++ctr < 2) return; // wait for next average coordinates X0U = Xtarget; Y0U = Ytarget;
X0U = x; Y0U = y; DBG("got X0U=%.1f, Y0U=%.1f", X0U, Y0U);
LOGDBG("got X0U=%.1f, Y0U=%.1f", x, y); LOGDBG("got X0U=%.1f, Y0U=%.1f", X0U, Y0U);
if(moveU(2*theconf.maxUsteps)) sstatus = SETUP_WAITUMAX; if(moveU(2*theconf.maxUsteps)) sstatus = SETUP_WAITUMAX;
ctr = 0; else{
LOGWARN("Can't move U -> max");
sstatus = SETUP_INIT;
}
break; break;
case SETUP_WAITUMAX: // wait while U moves to UVworkrange case SETUP_WAITUMAX: // wait while U moves to UVworkrange
if(!aver) return; if(!coordsRdy) return;
if(!moving_finished(Ustatus, NULL)) return; coordsRdy = FALSE;
if(++ctr < 2) return; // wait for next average coordinates XmU = Xtarget; YmU = Ytarget;
XmU = x; YmU = y; LOGDBG("got XmU=%.1f, YmU=%.1f", XmU, YmU);
LOGDBG("got XmU=%.1f, YmU=%.1f", x, y);
if(moveU(-theconf.maxUsteps) && moveV(-theconf.maxVsteps)) sstatus = SETUP_WAITV0; if(moveU(-theconf.maxUsteps) && moveV(-theconf.maxVsteps)) sstatus = SETUP_WAITV0;
ctr = 0; else{
LOGWARN("Can't move U -> mid OR/AND V -> min");
sstatus = SETUP_INIT;
}
break; break;
case SETUP_WAITV0: // wait while V moves to 0 case SETUP_WAITV0: // wait while V moves to 0
if(!aver) return; if(!coordsRdy) return;
if(!moving_finished(Vstatus, NULL)) return; coordsRdy = FALSE;
if(++ctr < 2) return; // wait for next average coordinates X0V = Xtarget; Y0V = Ytarget;
X0V = x; Y0V = y; LOGDBG("got X0V=%.1f, Y0V=%.1f", X0V, Y0V);
LOGDBG("got X0V=%.1f, Y0V=%.1f", x, y);
if(moveV(2*theconf.maxVsteps)) sstatus = SETUP_WAITVMAX; if(moveV(2*theconf.maxVsteps)) sstatus = SETUP_WAITVMAX;
ctr = 0; else{
LOGWARN("Can't move V -> max");
sstatus = SETUP_INIT;
}
break; break;
case SETUP_WAITVMAX: // wait while V moves to UVworkrange case SETUP_WAITVMAX: // wait while V moves to UVworkrange
if(!aver) return; if(!coordsRdy) return;
if(!moving_finished(Vstatus, NULL)) return; coordsRdy = FALSE;
if(++ctr < 2) return; // wait for next average coordinates XmV = Xtarget; YmV = Ytarget;
ctr = 0; LOGDBG("got XmV=%.1f, YmV=%.1f", XmV, YmV);
XmV = x; YmV = y;
LOGDBG("got XmV=%.1f, YmV=%.1f", x, y);
// calculate // calculate
double dxU = XmU - X0U, dyU = YmU - Y0U, dxV = XmV - X0V, dyV = YmV - Y0V; 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); LOGDBG("dxU=%.1f, dyU=%.1f, dxV=%.1f, dyV=%.1f", dxU, dyU, dxV, dyV);
@ -463,29 +522,27 @@ static void process_setup_stage(double x, double y, int aver){
double KU = 2 * theconf.maxUsteps / sqU; double KU = 2 * theconf.maxUsteps / sqU;
double KV = 2 * theconf.maxVsteps / sqV; double KV = 2 * theconf.maxVsteps / sqV;
double sa = dyU/sqU, ca = dxU/sqU, sb = dyV/sqV, cb = dxV/sqV; // sin(alpha) etc 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)] LOGDBG("KU=%.4f, KV=%.4f, sa=%.4f, ca=%.4f, sb=%.4f, cb=%.4f",
double sba = sb*ca - cb*sa; // sin(beta-alpha) KU, KV, sa, ca, sb, cb);
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)) * [dX dY] = M*[dU dV], M = [ca/KU cb/KV; sa/KU sb/KV] ===>
* V = x*sin(alpha)/sin(beta-alpha) + y*cos(alpha)/sin(beta-alpha) * [dU dV] = inv(M)*[dX dY],
* inv(M) = 1/(ca/KU*sb/KV - sa/KU*cb/KV)*[sb/KV -cb/KV; -sa/KU ca/KU]
*/ */
theconf.Kxu = KU*(ca - sa*ctba); double mul = 1/(ca/KU*sb/KV - sa/KU*cb/KV);
theconf.Kyu = KU*(-sa - ca*ctba); theconf.Kxu = mul*sb/KV;
theconf.Kxv = KV*sa/sba; theconf.Kyu = -mul*cb/KV;
theconf.Kyv = KV*ca/sba; theconf.Kxv = -mul*sa/KU;
theconf.Kyv = mul*ca/KU;
LOGDBG("Kxu=%g, Kyu=%g; Kxv=%g, Kyv=%g", theconf.Kxu, theconf.Kyu, theconf.Kxv, theconf.Kyv); LOGDBG("Kxu=%g, Kyu=%g; Kxv=%g, Kyv=%g", theconf.Kxu, theconf.Kyu, theconf.Kxv, theconf.Kyv);
DBG("Now save new configuration"); DBG("Now save new configuration");
saveconf(NULL); // try to store configuration saveconf(NULL); // try to store configuration
// fallthrough
endmoving: endmoving:
moveV(-theconf.maxVsteps); moveV(-theconf.maxVsteps);
sstatus = SETUP_FINISH; sstatus = SETUP_FINISH;
break; break;
case SETUP_FINISH: // reset current coordinates case SETUP_FINISH: // reset current coordinates
if(!UVmoving_finished()) return;
if(!send_message(Usetzero, NULL) || !send_message(Vsetzero, NULL)) return; if(!send_message(Usetzero, NULL) || !send_message(Vsetzero, NULL)) return;
// now inner steppers' counters are in zero position -> set to zero local // now inner steppers' counters are in zero position -> set to zero local
Uposition = Vposition = 0; Uposition = Vposition = 0;
@ -513,7 +570,8 @@ static int process_targetstage(double X, double Y){
theconf.xtarget = X + theconf.xoff; theconf.xtarget = X + theconf.xoff;
theconf.ytarget = Y + theconf.yoff; theconf.ytarget = Y + theconf.yoff;
DBG("Got target coordinates: (%.1f, %.1f)", X, Y); DBG("Got target coordinates: (%.1f, %.1f)", X, Y);
saveconf(FALSE); LOGMSG("Got target coordinates: (%.1f, %.1f)", X, Y);
saveconf(NULL);
nhit = 0; xprev = 0.; yprev = 0.; nhit = 0; xprev = 0.; yprev = 0.;
return TRUE; return TRUE;
} }
@ -530,9 +588,10 @@ static int try2correct(double dX, double dY){
dU = KCORR*(theconf.Kxu * dX + theconf.Kyu * dY); dU = KCORR*(theconf.Kxu * dX + theconf.Kyu * dY);
dV = KCORR*(theconf.Kxv * dX + theconf.Kyv * dY); dV = KCORR*(theconf.Kxv * dX + theconf.Kyv * dY);
int Unew = Uposition + (int)dU, Vnew = Vposition + (int)dV; int Unew = Uposition + (int)dU, Vnew = Vposition + (int)dV;
if(Unew > theconf.maxUsteps || Unew < -theconf.maxUsteps || int Unfixed = Unew + Fposition, Vnfixed = Vnew + Fposition; // fixed by focus position
Vnew > theconf.maxVsteps || Vnew < -theconf.maxVsteps){ if(Unfixed > theconf.maxUsteps || Unfixed < -theconf.maxUsteps ||
// TODO: here we should signal the interface that limit reaced Vnfixed > theconf.maxVsteps || Vnfixed < -theconf.maxVsteps){
// TODO: here we should signal that the limit reached
LOGWARN("Correction failed, curpos: %d, %d, should move to %d, %d", LOGWARN("Correction failed, curpos: %d, %d, should move to %d, %d",
Uposition, Vposition, Unew, Vnew); Uposition, Vposition, Unew, Vnew);
return FALSE; return FALSE;
@ -543,79 +602,38 @@ static int try2correct(double dX, double dY){
return (moveU((int)dU) && moveV((int)dV)); return (moveU((int)dU) && moveV((int)dV));
} }
#if 0 // global variable proc_corr
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 * @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 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) * 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){ void pusi_process_corrections(double X, double Y){
//DBG("got centroid data: %g, %g", X, Y); static bool coordstrusted = TRUE;
static int first = TRUE; if(ismoving){ // don't process coordinates when moving
double xtg = theconf.xtarget - theconf.xoff, ytg = theconf.ytarget - theconf.yoff; coordstrusted = FALSE;
double xdev = X - xtg, ydev = Y - ytg; coordsRdy = FALSE;
if(state != PUSI_DISCONN) first = TRUE; return;
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)
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");
fixerr = 1;
// TODO: do something here
DBG("FAILED");
} else fixerr = 0;
}
break;
default: // PUSI_RELAX
return;
} }
if(!coordstrusted){ // don't trust first coordinates after moving finished
coordstrusted = TRUE;
coordsRdy = FALSE;
return;
}
//DBG("got centroid data: %g, %g", X, Y);
Xtarget = X; Ytarget = Y;
coordsRdy = TRUE;
} }
// try to change state; @return TRUE if OK // try to change state; @return TRUE if OK
int pusi_setstate(pusistate newstate){ static int pusi_setstate(pusistate newstate){
if(newstate == state) return TRUE; if(newstate == state) return TRUE;
if(newstate == PUSI_DISCONN){ if(newstate == PUSI_DISCONN){
pusi_disconnect(); pusi_disconnect();
return TRUE; return TRUE;
} }
if(state == PUSI_DISCONN){ if(state == PUSI_DISCONN){
if(!pusi_connect()) return FALSE; if(!pusi_connect_server()) return FALSE;
} }
if(newstate == PUSI_SETUP || newstate == PUSI_GOTOTHEMIDDLE){ if(newstate == PUSI_SETUP || newstate == PUSI_GOTOTHEMIDDLE){
sstatus = SETUP_INIT; sstatus = SETUP_INIT;
@ -624,11 +642,7 @@ int pusi_setstate(pusistate newstate){
return TRUE; return TRUE;
} }
pusistate pusi_getstate(){ // get current status (global variable stepstatus)
return state;
}
// get current status
// return JSON string with different parameters // return JSON string with different parameters
char *pusi_status(const char *messageid, char *buf, int buflen){ char *pusi_status(const char *messageid, char *buf, int buflen){
int l; int l;
@ -690,11 +704,11 @@ char *pusi_status(const char *messageid, char *buf, int buflen){
l = snprintf(bptr, buflen, ", "); l = snprintf(bptr, buflen, ", ");
buflen -= l; bptr += l; buflen -= l; bptr += l;
const char *motors[] = {"Umotor", "Vmotor", "Fmotor"}; const char *motors[] = {"Umotor", "Vmotor", "Fmotor"};
const char *statuses[] = {Ustatus, Vstatus, Fstatus}; volatile atomic_bool *mv[] = {&Umoving, &Vmoving, &Fmoving};
int *pos[] = {&Uposition, &Vposition, &Fposition}; volatile atomic_int *pos[] = {&Uposition, &Vposition, &Fposition};
for(int i = 0; i < 3; ++i){ for(int i = 0; i < 3; ++i){
const char *stat = "moving"; const char *stat = "stopping";
if(moving_finished(statuses[i], pos[i])) stat = "stopping"; if(*mv[i]) stat = "moving";
l = snprintf(bptr, buflen, "\"%s\": { \"status\": \"%s\", \"position\": %d }%s", l = snprintf(bptr, buflen, "\"%s\": { \"status\": \"%s\", \"position\": %d }%s",
motors[i], stat, *pos[i], (i==2)?"":", "); motors[i], stat, *pos[i], (i==2)?"":", ");
buflen -= l; bptr += l; buflen -= l; bptr += l;
@ -708,7 +722,7 @@ typedef struct{
const char *str; const char *str;
pusistate state; pusistate state;
} strstate; } strstate;
// commands from client to change status
strstate stringstatuses[] = { strstate stringstatuses[] = {
{"disconnect", PUSI_DISCONN}, {"disconnect", PUSI_DISCONN},
{"relax", PUSI_RELAX}, {"relax", PUSI_RELAX},
@ -718,7 +732,8 @@ strstate stringstatuses[] = {
{"fix", PUSI_FIX}, {"fix", PUSI_FIX},
{NULL, 0} {NULL, 0}
}; };
// try to set new status
// try to set new status (global variable stepstatus)
char *set_pusistatus(const char *newstatus, char *buf, int buflen){ char *set_pusistatus(const char *newstatus, char *buf, int buflen){
strstate *s = stringstatuses; strstate *s = stringstatuses;
pusistate newstate = PUSI_UNDEFINED; pusistate newstate = PUSI_UNDEFINED;
@ -750,18 +765,109 @@ char *set_pusistatus(const char *newstatus, char *buf, int buflen){
ptr[L-1] = '\n'; ptr[L-1] = '\n';
return buf; return buf;
} }
// change focus
// change focus (global variable movefocus)
char *set_pfocus(const char *newstatus, char *buf, int buflen){ char *set_pfocus(const char *newstatus, char *buf, int buflen){
if(!moving_finished(Fstatus, &Fposition)){
snprintf(buf, buflen, FAIL);
return buf;
}
int newval = atoi(newstatus); int newval = atoi(newstatus);
if(newval < theconf.minFpos || newval > theconf.maxFpos){ if(newval < theconf.minFpos || newval > theconf.maxFpos){
snprintf(buf, buflen, FAIL); snprintf(buf, buflen, FAIL);
}else{ }else{
if(!setF(newval)) snprintf(buf, buflen, FAIL); snprintf(buf, buflen, OK);
else snprintf(buf, buflen, OK); newfocpos = newval;
chfocus = TRUE;
} }
return buf; return buf;
} }
// MAIN THREAD
static void *pusi_process_states(_U_ void *arg){
FNAME();
static bool first = TRUE; // flag for logging when can't reconnect
while(!stopwork){
usleep(10000);
// check for moving
switch(state){
case PUSI_SETUP:
case PUSI_GOTOTHEMIDDLE:
case PUSI_FIX:
if(moving_finished(Ustatus, &Uposition)) Umoving = FALSE;
else Umoving = TRUE;
if(moving_finished(Vstatus, &Vposition)) Vmoving = FALSE;
else Vmoving = TRUE;
if(moving_finished(Fstatus, &Fposition)) Fmoving = FALSE;
else Fmoving = TRUE;
if(Umoving || Vmoving || Fmoving) ismoving = TRUE;
else ismoving = FALSE;
break;
case PUSI_DISCONN:
sleep(1);
pusi_connect_server();
break;
default:
break;
}
if(ismoving){
coordsRdy = FALSE;
continue;
}
// check request to change focus
if(chfocus){
chfocus = FALSE;
int delta = newfocpos - Fposition;
moveF(delta); moveU(delta); moveV(delta);
continue;
}
// if we are here, all U/V/F moving is finished
if(state != PUSI_DISCONN) first = TRUE;
switch(state){ // pusirobo state machine
case PUSI_DISCONN:
if(!pusi_connect_server()){
WARNX("Can't reconnect");
if(first){
LOGWARN("Can't reconnect");
first = FALSE;
}
sleep(5);
}
break;
case PUSI_SETUP: // setup axes (before this state set Xtarget/Ytarget in improc.c)
process_setup_stage();
break;
case PUSI_GOTOTHEMIDDLE:
process_movetomiddle_stage();
break;
case PUSI_FINDTARGET: // calculate target coordinates
if(coordsRdy){
coordsRdy = FALSE;
if(process_targetstage(Xtarget, Ytarget))
state = PUSI_RELAX;
}
break;
case PUSI_FIX: // process corrections
if(coordsRdy){
coordsRdy = FALSE;
red("GET 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);
if(theconf.xtarget < 1. || theconf.ytarget < 1. || corr < COORDTOLERANCE){
DBG("Target coordinates not defined or correction too small, targ: (%.1f, %.1f); corr: %.1f, %.1f (abs: %.1f)",
theconf.xtarget, theconf.ytarget, xdev, ydev, corr);
break;
}
LOGDBG("Current position: U=%d, V=%d, deviations: dX=%.1f, dy=%.1f",
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
break;
}
}
return NULL;
}

23
LocCorr/pusirobo.h
View File

@ -19,26 +19,17 @@
#ifndef PUSIROBO_H__ #ifndef PUSIROBO_H__
#define 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 // try to connect to local pusirobo server
int pusi_connect(); int pusi_connect();
int pusi_setstate(pusistate newstate); // disconnect
pusistate pusi_getstate(); void pusi_stop();
void pusi_disconnect(); // global variable proc_corr
void pusi_process_corrections(double X, double Y, int corrflag); void pusi_process_corrections(double X, double Y);
// global variable stepstatus
char *pusi_status(const char *messageid, char *buf, int buflen); char *pusi_status(const char *messageid, char *buf, int buflen);
// global variable setstepstatus
char *set_pusistatus(const char *newstatus, char *buf, int buflen); char *set_pusistatus(const char *newstatus, char *buf, int buflen);
// global variable movefocus
char *set_pfocus(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__ #endif // PUSIROBO_H__

31
LocCorr/socket.c
View File

@ -79,8 +79,6 @@ static char *setfocusstate(const char *state, char *buf, int buflen);
static setter setterHandlers[] = { static setter setterHandlers[] = {
{"stpstate", setstepperstate, "Set given steppers' server state"}, {"stpstate", setstepperstate, "Set given steppers' server state"},
{"focus", setfocusstate, "Move focus to given value"}, {"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} {NULL, NULL, NULL}
}; };
@ -132,35 +130,6 @@ static char *setfocusstate(const char *state, char *buf, int buflen){
snprintf(buf, buflen, FAIL); snprintf(buf, buflen, FAIL);
return buf; 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){ static char *rmnl(const char *msg, char *buf, int buflen){