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

seems like it works good; need tests on real star

Browse Source
This commit is contained in:
Edward Emelianov 2025-03-14 16:42:52 +03:00
parent 6ee58edc7d
commit 7a0acd14f5
15 changed files with 678 additions and 236 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
LocCorr_new/basler.c
View File

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

138
LocCorr_new/cameracapture.c
View File

@ -18,6 +18,7 @@
#include <float.h> // FLT_EPSILON
#include <math.h>
#include <pthread.h>
#include <stdio.h>
#include <string.h>
@ -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,28 +135,103 @@ 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*)){
@ -161,6 +240,11 @@ int camcapture(void (*process)(Image*)){
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;
}
if(process){
if(theconf.medfilt){
Image *X = get_median(oIma, theconf.medseed);
if(X){
FREE(oIma->data);
FREE(oIma);
oIma = X;
}
}
process(oIma);
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(!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);
}
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;
}

3
LocCorr_new/cmdlnopts.c
View File

@ -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 "')")},

1
LocCorr_new/cmdlnopts.h
View File

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

84
LocCorr_new/config.c
View File

@ -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,9 +271,8 @@ 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){
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){
@ -240,10 +282,11 @@ confparam *chk_keyval(const char *key, const char *val, key_value *result){
WARNX("Wrong integer value '%s' of parameter '%s'", val, key);
return NULL;
}
if(result->val.intval < par->minval || result->val.intval > par->maxval)
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;
} else return par;
break;
case PAR_DOUBLE:
//DBG("DOUBLE");
@ -252,15 +295,12 @@ confparam *chk_keyval(const char *key, const char *val, key_value *result){
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)
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;
}
return NULL;
}
++par;
} 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;

17
LocCorr_new/config.h
View File

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

49
LocCorr_new/imagefile.c
View File

@ -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);
}
/*

11
LocCorr_new/imagefile.h
View File

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

194
LocCorr_new/improc.c
View File

@ -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(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;
}
}
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)
.area = b->area, .Isum = sum,
.WdivH = wh, .xc = stat.xc, .yc = stat.yc,
.xsigma = stat.xsigma, .ysigma = stat.ysigma
};
}
}
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();
}

2
LocCorr_new/improc.h
View File

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

68
LocCorr_new/loccorr.conf
View File

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

14
LocCorr_new/main.c
View File

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

134
LocCorr_new/socket.c
View File

@ -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;
}
if(textbuf[Len-1] != '\n') Len += write(sock, "\n", 1);
return (size_t)Len;
total += sent;
}
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!");
}

131
LocCorr_new/steppers.c
View File

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

3
LocCorr_new/steppers.h
View File

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