Add toupcam support

.gitignore

@@ -3,6 +3,7 @@
 
 # Object files
 *.o
+*/mk/*
 
 # Libraries
 *.lib

LocCorr_new/CMakeLists.txt

@@ -19,14 +19,15 @@ option(DEBUG "Compile in debug mode" OFF)
 option(BASLER "Add Basler cameras support" OFF)
 option(GRASSHOPPER "Add GrassHopper cameras support" OFF)
 option(HIKROBOT "Add HikRobot cameras support" OFF)
+option(TOUPCAM "Add Toupcam CMOS support" OFF)
 
-# default flags
+# default flags (c17 because MVS code have `typedef char bool`)
-set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -W -Wextra")
+set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -W -Wextra -Werror -std=c17")
 set(CMAKE_COLOR_MAKEFILE ON)
 
 # cmake -DDEBUG=yes -> debugging
 if(DEBUG)
-    set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Og -g3 -ggdb -fno-builtin-strlen -Werror")
+    set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Og -g3 -ggdb -fno-builtin-strlen")
     add_definitions(-DEBUG)
     set(CMAKE_BUILD_TYPE DEBUG)
     set(CMAKE_VERBOSE_MAKEFILE "ON")
@@ -63,6 +64,13 @@ if(HIKROBOT)
 else()
     list(REMOVE_ITEM SOURCES "${CMAKE_CURRENT_SOURCE_DIR}/hikrobot.c")
 endif()
+if(TOUPCAM)
+    pkg_check_modules(TOUPCAM REQUIRED toupcam)
+    add_definitions("-DTOUPCAM_FOUND=1")
+else()
+    list(REMOVE_ITEM SOURCES "${CMAKE_CURRENT_SOURCE_DIR}/toupcam.c")
+endif()
+
 pkg_check_modules(MODULES REQUIRED ${MODULES})
 
 include(FindOpenMP)
@@ -89,18 +97,18 @@ message("Install dir prefix: ${CMAKE_INSTALL_PREFIX}")
 # exe file
 add_executable(${PROJ} ${SOURCES})
 # -I
-target_include_directories(${PROJ} PUBLIC ${MODULES_INCLUDE_DIRS} ${FLYCAP_INCLUDE_DIRS} ${BASLER_INCLUDE_DIRS} ${MVS_INCLUDE_DIRS})
+target_include_directories(${PROJ} PUBLIC ${MODULES_INCLUDE_DIRS} ${FLYCAP_INCLUDE_DIRS} ${BASLER_INCLUDE_DIRS} ${MVS_INCLUDE_DIRS} ${TOUPCAM_INCLUDE_DIRS})
 # -L
-target_link_directories(${PROJ} PUBLIC ${MODULES_LIBRARY_DIRS} ${FLYCAP_LIBRARY_DIRS} ${BASLER_LIBRARY_DIRS} ${MVS_LIBRARY_DIRS})
+target_link_directories(${PROJ} PUBLIC ${MODULES_LIBRARY_DIRS} ${FLYCAP_LIBRARY_DIRS} ${BASLER_LIBRARY_DIRS} ${MVS_LIBRARY_DIRS} ${TOUPCAM_LIBRARY_DIRS})
-message("MOD: ${MODULES_LIBRARY_DIRS}, FC: ${FLYCAP_LIBRARY_DIRS}, MVS: ${MVS_LIBRARY_DIRS}")
+message("MOD: ${MODULES_LIBRARY_DIRS}, FC: ${FLYCAP_LIBRARY_DIRS}, MVS: ${MVS_LIBRARY_DIRS}, TOUPCAM: ${TOUPCAM_LIBRARY_DIRS}")
 # -D
-add_definitions(${CFLAGS} -DLOCALEDIR=\"${LOCALEDIR}\"
+add_definitions(${CFLAGS} -D_XOPEN_SOURCE=666 -D_POSIX_C_SOURCE=600700 -D_DEFAULT_SOURCE -DLOCALEDIR=\"${LOCALEDIR}\"
     -DPACKAGE_VERSION=\"${VERSION}\" -DGETTEXT_PACKAGE=\"${PROJ}\"
     -DMINOR_VERSION=\"${MINOR_VERSION}\" -DMID_VERSION=\"${MID_VERSION}\"
     -DMAJOR_VERSION=\"${MAJOR_VERSION}\" -DTHREAD_NUMBER=${PROCESSOR_COUNT})
 
 # -l
-target_link_libraries(${PROJ} ${MODULES_LIBRARIES} ${FLYCAP_LIBRARIES} ${BASLER_LIBRARIES} ${MVS_LIBRARIES} -lm)
+target_link_libraries(${PROJ} ${MODULES_LIBRARIES} ${FLYCAP_LIBRARIES} ${BASLER_LIBRARIES} ${MVS_LIBRARIES} ${TOUPCAM_LIBRARIES} -lm)
 
 # Installation of the program
 INSTALL(TARGETS ${PROJ} DESTINATION "bin")

LocCorr_new/Toupcam.h

@@ -0,0 +1,28 @@
+/*
+ * This file is part of the loccorr project.
+ * Copyright 2026 Edward V. Emelianov <edward.emelianoff@gmail.com>.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#ifdef TOUPCAM_FOUND
+#include "cameracapture.h" // `camera`
+
+#define TOUPCAM_CAPT_NAME   "toupcam"
+
+extern camera Toupcam;
+
+#endif

LocCorr_new/cameracapture.c

@@ -21,6 +21,7 @@
 #include <pthread.h>
 #include <stdio.h>
 #include <string.h>
+#include <unistd.h>
 
 #include "cameracapture.h"
 #include "cmdlnopts.h"
@@ -34,7 +35,7 @@
 static camera *theCam = NULL;
 
 static float gain = 0., gainmax = 0.;
-static float exptime = 100.;
+static float exptime = -1.;
 static float brightness = 0.;
 static int connected = FALSE;
 
@@ -42,6 +43,15 @@ static frameformat curformat;
 static frameformat maxformat;
 static frameformat stepformat;
 
+// statistics of last image
+typedef struct{
+    Imtype minval, maxval, bkg;
+    float avg, xc, yc;
+    ptstat_t stat;
+} imdata_t;
+
+static imdata_t lastimdata = {0};
+
 static void changeformat(){
     if(!theCam) return;
     if(maxformat.h < 1 || maxformat.w < 1){
@@ -190,7 +200,7 @@ static int needs_exposure_adjustment(const Image *I, float curr_x, float curr_y)
     float avg = I->avg_intensity;
     float dx = fabsf(curr_x - last_centroid_x);
     float dy = fabsf(curr_y - last_centroid_y);
-    LOGDBG("avg: %g, curr_x: %g, curr_y: %g", avg, curr_x, curr_y);
+    //LOGDBG("avg: %g, curr_x: %g, curr_y: %g", avg, curr_x, curr_y);
     // don't change brightness if average value in 5..50
     if(avg > 5.f && avg < 50.f){
         last_avg_intensity = avg;
@@ -229,42 +239,50 @@ static void *procthread(void* v){
     void (*process)(Image*) = (procfn_t)v;
 #ifdef EBUG
     double t0 = sl_dtime();
+    int imno = 0;
 #endif
     while(!stopwork){
-        while(iCaptured < 0) usleep(1000);
         pthread_mutex_lock(&capt_mutex);
         if(Icap[iCaptured]){
-            DBG("---- got image #%d @ %g", iCaptured, sl_dtime() - t0);
+            DBG("===== got image #%d, iCaptured=#%d @ %g", imno++, iCaptured, sl_dtime() - t0);
             Image *oIma = Icap[iCaptured]; // take image here and free buffer
             Icap[iCaptured] = NULL;
             pthread_mutex_unlock(&capt_mutex);
+            if(process){
+                if(theconf.medfilt){
+                    Image *X = get_median(oIma, theconf.medseed);
+                    if(X){
+                        Image_free(&oIma);
+                        oIma = X;
+                    }
+                }
+                process(oIma);
+                lastimdata.avg = oIma->avg_intensity;
+                lastimdata.bkg = oIma->background;
+                lastimdata.minval = oIma->minval;
+                lastimdata.maxval = oIma->maxval;
+                lastimdata.stat = oIma->stat;
+                getcenter(&lastimdata.xc, &lastimdata.yc);
+            }
             if(theconf.expmethod == EXPAUTO){
-                float xc, yc;
+                if(needs_exposure_adjustment(oIma, lastimdata.xc, lastimdata.yc)) recalcexp(oIma);
-                getcenter(&xc, &yc);
-                if(needs_exposure_adjustment(oIma, xc, yc)) recalcexp(oIma);
             }else{
-                if(fabs(theconf.fixedexp - exptime) > FLT_EPSILON)
+                if(fabs(theconf.exptime - exptime) > FLT_EPSILON)
-                    exptime = theconf.fixedexp;
+                    exptime = theconf.exptime;
                 if(fabs(theconf.gain - gain) > FLT_EPSILON)
                     gain = theconf.gain;
                 if(fabs(theconf.brightness - brightness) > FLT_EPSILON)
                     brightness = theconf.brightness;
             }
-            if(process){
+            //Icap[iCaptured] = NULL;
-                if(theconf.medfilt){
+            //pthread_mutex_unlock(&capt_mutex);
-                    Image *X = get_median(oIma, theconf.medseed);
+            Image_free(&oIma);
-                    if(X){
+            DBG("===== cleared image data @ %g", sl_dtime() - t0);
-                        FREE(oIma->data);
+        }else{
-                        FREE(oIma);
+            //DBG("===== NO image data");
-                        oIma = X;
+            pthread_mutex_unlock(&capt_mutex);
         }
-                }
+        //DBG("===== NEXT!");
-                process(oIma);
-            }
-            FREE(oIma->data);
-            FREE(oIma);
-            DBG("---- cleared image data @ %g", sl_dtime() - t0);
-        }else pthread_mutex_unlock(&capt_mutex);
         usleep(1000);
     }
     return NULL;
@@ -274,14 +292,21 @@ int camcapture(void (*process)(Image*)){
     FNAME();
     static float oldexptime = 0.;
     static float oldgain = -1.;
-    static float oldbrightness = -1.;
+    static float oldbrightness = 0.;
     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()");
     }
+    exptime = theconf.exptime;
+#ifdef EBUG
+    static int imno = 0;
+#endif
     while(1){
+#ifdef EBUG
+        double t0 = sl_dtime();
+#endif
         if(stopwork){
             DBG("STOP");
             break;
@@ -290,13 +315,15 @@ int camcapture(void (*process)(Image*)){
             LOGERR("camcapture(): camera not initialized");
             ERRX("Not initialized");
         }
+        DBG("T=%g", sl_dtime() - t0);
         if(!connected){
-            DBG("Disconnected");
+            DBG("Disconnected, try to connect");
             connected = theCam->connect();
             sleep(1);
             changeformat();
             continue;
         }
+        DBG("T=%g", sl_dtime() - t0);
         if(fabsf(oldbrightness - brightness) > FLT_EPSILON){ // new brightness
             DBG("Change brightness to %g", brightness);
             if(theCam->setbrightness(brightness)){
@@ -305,38 +332,54 @@ int camcapture(void (*process)(Image*)){
                 WARNX("Can't change brightness to %g", brightness);
             }
         }
+        DBG("T=%g", sl_dtime() - t0);
         if(exptime > theconf.maxexp) exptime = theconf.maxexp;
         else if(exptime < theconf.minexp) exptime = theconf.minexp;
         if(fabsf(oldexptime - exptime) > FLT_EPSILON){ // new exsposition value
             DBG("Change exptime to %.2fms\n", exptime);
             if(theCam->setexp(exptime)){
                 oldexptime = exptime;
+                theconf.exptime = exptime;
             }else{
                 WARNX("Can't change exposition time to %gms", exptime);
             }
         }
+        DBG("T=%g", sl_dtime() - t0);
         if(gain > gainmax) gain = gainmax;
         if(fabsf(oldgain - gain) > FLT_EPSILON){ // change gain
             DBG("Change gain to %g\n", gain);
+            LOGDBG("Change gain to %g", gain);
             if(theCam->setgain(gain)){
                 oldgain = gain;
+                theconf.gain = gain;
             }else{
                 WARNX("Can't change gain to %g", gain);
+                LOGWARN("Can't change gain to %g", gain);
+                gain = oldgain;
             }
         }
+        DBG("T=%g", sl_dtime() - t0);
         // change format
         if(abs(curformat.h - theconf.height) || abs(curformat.w - theconf.width) || abs(curformat.xoff - theconf.xoff) || abs(curformat.yoff - theconf.yoff)){
             changeformat();
         }
+        DBG("Try to grab (T=%g)", sl_dtime() - t0);
+        static int errctr = 0;
         if(!(oIma = theCam->capture())){
-            WARNX("Can't grab image");
+            WARNX("---- Can't grab image");
+            if(++errctr > MAX_CAPT_ERRORS){
+                LOGERR("camcapture(): too much capture errors; reconnect camera");
                 camdisconnect();
-            continue;
+                errctr = 0;
             }
+            continue;
+        }else errctr = 0;
+        DBG("---- Grabbed #%d @ %g", imno++, sl_dtime() - t0);
         pthread_mutex_lock(&capt_mutex);
         if(iCaptured < 0) iCaptured = 0;
         else iCaptured = !iCaptured;
         if(Icap[iCaptured]){ // try current value if previous is still busy
+            DBG("---- iCap=%d busy!", iCaptured);
             iCaptured = !iCaptured;
         }
         if(!Icap[iCaptured]){ // previous buffer is free
@@ -344,22 +387,16 @@ int camcapture(void (*process)(Image*)){
             Icap[iCaptured] = oIma;
             oIma = NULL;
         }else{ // clear our image - there's no empty buffers
-            DBG("---- no free buffers");
+            DBG("---- no free buffers for iCap=%d", iCaptured);
-            FREE(oIma->data);
+            Image_free(&oIma);
-            FREE(oIma);
         }
         pthread_mutex_unlock(&capt_mutex);
+        DBG("T=%g", sl_dtime() - t0);
     }
     pthread_cancel(proc_thread);
-    if(oIma){
+    if(oIma) Image_free(&oIma);
-        FREE(oIma->data);
-        FREE(oIma);
-    }
     for(int i = 0; i < 2; ++i){
-        if(Icap[i]){
+        if(Icap[i]) Image_free(&Icap[i]);
-            FREE(Icap[i]->data);
-            FREE(Icap[i]);
-        }
     }
     camdisconnect();
     DBG("CAMCAPTURE: out");
@@ -388,10 +425,13 @@ char *camstatus(const char *messageid, char *buf, int buflen){
     float xc, yc;
     getcenter(&xc, &yc);
     snprintf(buf, buflen, "{ \"%s\": \"%s\", \"camstatus\": \"%sconnected\", \"impath\": \"%s\", \"imctr\": %llu, "
-             "\"fps\": %.3f, \"expmethod\": \"%s\", \"exposition\": %g, \"gain\": %g, \"brightness\": %g, "
+         "\"fps\": %.3f, \"expmethod\": \"%s\", \"exptime\": %g, \"gain\": %g, \"maxgain\": %g, \"brightness\": %g, "
-             "\"xcenter\": %.1f, \"ycenter\": %.1f }\n",
+         "\"xcenter\": %.1f, \"ycenter\": %.1f , \"minval\": %d, \"maxval\": %d, \"background\": %d, "
+         "\"average\": %.1f, \"xc\": %.1f, \"yc\": %.1f, \"xsigma\": %.1f, \"ysigma\": %.1f, \"area\": %d }\n",
          MESSAGEID, messageid, connected ? "" : "dis", impath, ImNumber, getFramesPerS(),
-             (theconf.expmethod == EXPAUTO) ? "auto" : "manual", exptime, gain, brightness,
+         (theconf.expmethod == EXPAUTO) ? "auto" : "manual", exptime, gain, gainmax, brightness,
-             xc, yc);
+         xc, yc, lastimdata.minval, lastimdata.maxval, lastimdata.bkg, lastimdata.avg,
+         lastimdata.stat.xc, lastimdata.stat.yc, lastimdata.stat.xsigma, lastimdata.stat.ysigma,
+         lastimdata.stat.area);
     return buf;
 }

LocCorr_new/cameracapture.h

@@ -21,6 +21,8 @@
 
 #include "imagefile.h" // Image
 
+// max capture errors contract to make reconnection
+#define MAX_CAPT_ERRORS     (10)
 
 // format of single frame
 typedef struct{

LocCorr_new/cmdlnopts.c

@@ -70,7 +70,7 @@ static sl_option_t cmdlnopts[] = {
     {"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)")},
-    {"input",   NEED_ARG,   NULL,   'i',    arg_string, APTR(&G.inputname), _("file or directory name for monitoring (or grasshopper/basler/hikrobot for capturing)")},
+    {"input",   NEED_ARG,   NULL,   'i',    arg_string, APTR(&G.inputname), _("file or directory name for monitoring (or grasshopper/basler/hikrobot/toupcam for capturing)")},
     {"blackp",  NEED_ARG,   NULL,   'b',    arg_double, APTR(&G.throwpart), _("fraction of black pixels to throw away when make histogram eq")},
 //    {"radius",  NEED_ARG,   NULL,   'r',    arg_int,    APTR(&G.medradius), _("radius of median filter (r=1 -> 3x3, r=2 -> 5x5 etc.)")},
     {"equalize", NO_ARGS,   NULL,   'e',    arg_int,    APTR(&G.equalize),  _("make historam equalization of saved jpeg")},

LocCorr_new/config.c

@@ -62,7 +62,7 @@ configuration theconf = {
     .throwpart=DEFAULT_THROWPART,
     .maxexp=EXPOS_MAX - 1.,
     .minexp=EXPOS_MIN,
-    .fixedexp=EXPOS_MIN*2,
+    .exptime=EXPOS_MIN*2,
     .gain=20.,
     .intensthres=DEFAULT_INTENSTHRES,
     .medseed=MIN_MEDIAN_SEED,
@@ -74,6 +74,7 @@ static int compConfVals(const void *_1st, const void *_2nd){
     return strcmp(a->name, b->name);
 }
 
+// could be in unsorted order as whould be sorted at first help call
 // {"", PAR_DOUBLE, (void*)&theconf., 0},
 static confparam parvals[] = {
     {"maxarea", PAR_INT, (void*)&theconf.maxarea, 0, MINAREA, MAXAREA,
@@ -99,7 +100,7 @@ static confparam parvals[] = {
     {"equalize", PAR_INT, (void*)&theconf.equalize, 0, 0., 1.,
      "make histogram equalization"},
     {"expmethod", PAR_INT, (void*)&theconf.expmethod, 0, 0., 1.,
-     "exposition method: 0 - auto, 1 - fixed"},
+     "0 - automatic calculation of gain and exptime, 1 - use fixed values"},
     {"naverage", PAR_INT, (void*)&theconf.naverage, 0, 1., NAVER_MAX,
      "calculate mean position by N images"},
     {"umax", PAR_INT, (void*)&theconf.maxUpos, 0, -MAXSTEPS, MAXSTEPS,
@@ -146,8 +147,8 @@ static confparam parvals[] = {
      "minimal exposition time"},
     {"maxexp", PAR_DOUBLE, (void*)&theconf.maxexp, 0, 0., EXPOS_MAX,
      "maximal exposition time"},
-    {"fixedexp", PAR_DOUBLE, (void*)&theconf.fixedexp, 0, EXPOS_MIN, EXPOS_MAX,
+    {"exptime", PAR_DOUBLE, (void*)&theconf.exptime, 0, EXPOS_MIN, EXPOS_MAX,
-     "fixed (in manual mode) exposition time"},
+     "exposition time (you can change it only when expmethod==1)"},
     {"intensthres", PAR_DOUBLE, (void*)&theconf.intensthres, 0, 0., 1.,
      "threshold by total object intensity when sorting = |I1-I2|/(I1+I2)"},
     {"gain", PAR_DOUBLE, (void*)&theconf.gain, 0, GAIN_MIN, GAIN_MAX,
@@ -162,8 +163,10 @@ static confparam parvals[] = {
      "median filter radius"},
     {"fixedbg", PAR_INT, (void*)&theconf.fixedbkg, 0, 0., 1.,
      "don't calculate background, use fixed value instead"},
-    {"fbglevel", PAR_INT, (void*)&theconf.fixedbkgval, 0, FIXED_BK_MIN, FIXED_BK_MAX,
+    {"background", PAR_INT, (void*)&theconf.background, 0, FIXED_BK_MIN, FIXED_BK_MAX,
      "fixed background level"},
+    {"writedi", PAR_INT, (void*)&theconf.writedebugimgs, 0, 0., 1.,
+     "write debug images (binary/erosion/opening)"},
     {NULL,  0,  NULL, 0, 0., 0., NULL}
 };
 

LocCorr_new/config.h

@@ -34,12 +34,12 @@
 #define MAX_THROWPART   (0.9)
 #define MAX_OFFSET      (10000)
 // min/max exposition in ms
-#define EXPOS_MIN       (0.1)
+#define EXPOS_MIN       (1e-9)
 #define EXPOS_MAX       (4001.)
 #define GAIN_MIN        (0.)
-#define GAIN_MAX        (100.)
+#define GAIN_MAX        (256.)
 #define BRIGHT_MIN      (0.)
-#define BRIGHT_MAX      (10.)
+#define BRIGHT_MAX      (100.)
 // max average images counter
 #define NAVER_MAX       (25)
 // coefficients to convert dx,dy to du,dv
@@ -52,15 +52,15 @@
 #define MAX_MEDIAN_SEED (7)
 // fixed background
 #define FIXED_BK_MIN    (0)
-#define FIXED_BK_MAX    (250)
+#define FIXED_BK_MAX    (255)
 
 // exposition methods: 0 - auto, 1 - fixed
 #define EXPAUTO         (0)
 #define EXPMANUAL       (1)
 
 // roundness parameter
-#define MINWH                       (0.3)
+#define MINWH           (0.01)
-#define MAXWH                       (3.)
+#define MAXWH           (100.)
 
 // PID limits
 #define PID_P_MIN       (0.1)
@@ -99,7 +99,8 @@ typedef struct{
     int medfilt;        // == 1 to make median filter before calculations
     int medseed;        // median seed
     int fixedbkg;       // don't calculate background, use fixed value instead
-    int fixedbkgval;    // value of bk
+    int background;     // value of background
+    int writedebugimgs; // write debugging images: binary/erosion/opening
     // dU = Kxu*dX + Kyu*dY; dV = Kxv*dX + Kyv*dY
     double Kxu; double Kyu;
     double Kxv; double Kyv;
@@ -109,7 +110,7 @@ typedef struct{
     double throwpart;   // part of values to throw avay @ histogram equalisation
     double maxexp;      // minimal and maximal exposition (in ms)
     double minexp;
-    double fixedexp;    // exptime in manual mode
+    double exptime;     // exposure time
     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

LocCorr_new/fits.c

@@ -47,11 +47,6 @@ do{ fitsstatus = 0;								\
     if(status) fits_report_error(stderr, status);\
 }while(0)
 
-void Image_free(Image **img){
-    FREE((*img)->data);
-    FREE(*img);
-}
-
 // I->data should be allocated!!!
 static inline void convflt2ima(float *f, Image *I){
     if(!I || !I->data || !f) return;

LocCorr_new/fits.h

@@ -26,7 +26,6 @@
 
 #include "imagefile.h"
 
-void Image_free(Image **ima);
 bool FITS_read(const char *filename, Image **fits);
 
 #endif // FITS_H__

LocCorr_new/hikrobot.c

@@ -279,7 +279,7 @@ static int cam_connect(){
         WARNX("Can't turn off triggered mode");
         return FALSE;
     }
-    if(!changeenum("AcquisitionMode", MV_ACQ_MODE_SINGLE)){
+    if(!changeenum("AcquisitionMode", /*MV_ACQ_MODE_SINGLE*/ MV_ACQ_MODE_CONTINUOUS)){
         WARNX("Can't set acquisition mode to single");
         return FALSE;
     }
@@ -356,46 +356,69 @@ static int changeformat(frameformat *f){
     return TRUE;
 }
 
-// exptime - in milliseconds!
 static int setexp(float e){
-    if(!handle) return FALSE;
+    if(!handle){
-    float eS = e / 1e3;
+        WARNX("NO HANDLE");
-    if(eS > extrvalues.maxexp || eS < extrvalues.minexp){
+        return FALSE;
-        WARNX("Wrong exposure time: %fs (should be [%fs..%fs])", eS,
+    }
+    if(e > extrvalues.maxexp || e < extrvalues.minexp){
+        WARNX("Wrong exposure time: %fs (should be [%fs..%fs])", e,
             extrvalues.minexp, extrvalues.maxexp);
         return FALSE;
     }
-    if(!changefloat("ExposureTime", e * 1e3)) return FALSE;
+    if(!changefloat("ExposureTime", e * 1e6)){
-    exptime = eS;
+        WARNX("Can't set exptime %g", e);
+        return FALSE;
+    }
+    exptime = e;
     return TRUE;
 }
 
 static int cam_startexp(){
     if(!handle || !pdata) return FALSE;
-    DBG("Start exposition");
+    DBG("+++++ Start exposition for %gs", exptime);
     MV_CC_StopGrabbing(handle);
     TRY(StartGrabbing);
-    ONERR() return FALSE;
+    ONERR(){
+        DBG("+++++ Ooops! Can't start grabbing: try another time!");
+        MV_CC_StopGrabbing(handle);
+        TRY(StartGrabbing);
+        ONERR(){
+            DBG("+++++ ERR!");
+            return FALSE;
+        }
+    }
+    DBG("+++++ OK, started");
     return TRUE;
 }
 
 static Image* capture(){
-    if(!cam_startexp()) return NULL;
-    MV_FRAME_OUT_INFO_EX stImageInfo = {0}; // last image info
     double starttime = sl_dtime();
+    static int isstarted = 0;
+    if(!isstarted){
+        if(!cam_startexp()) return NULL;
+        isstarted = 1;
+    }
+    MV_FRAME_OUT_INFO_EX stImageInfo = {0}; // last image info
+    DBG("^^^^^^^^^^^^^^^^^^^^ Started capt @ %g", sl_dtime() - starttime);
     do{
         usleep(100);
         double diff = exptime - (sl_dtime() - starttime);
         if(diff > 0.) continue; // wait until exposure ends
         DBG("diff = %g", diff);
-        if(diff < -5.0){ // wait much longer than exp lasts
+        if(diff < -MAX_READOUT_TM){ // wait much longer than exp lasts
+            DBG("^^^^^^^^^^^^^^^^^^^^ OOps, time limit");
             MV_CC_StopGrabbing(handle);
+            DBG("Restart grabbing");
+            if(!cam_startexp()) isstarted = 0;
             return NULL;
         }
-        TRY(GetOneFrameTimeout, pdata, pdatasz, &stImageInfo, 10);
+        TRY(GetOneFrameTimeout, pdata, pdatasz, &stImageInfo, 100);
         ONOK() break;
     }while(1);
+    DBG("^^^^^^^^^^^^^^^^^^^^ Tcapt=%g, exptime=%g", sl_dtime() - starttime, exptime);
     Image *captIma = u8toImage(pdata, stImageInfo.nWidth, stImageInfo.nHeight, stImageInfo.nWidth);
+    DBG("^^^^^^^^^^^^^^^^^^^^ return @ %g", sl_dtime() - starttime);
     return captIma;
 }
 

LocCorr_new/hikrobot.h

@@ -23,8 +23,11 @@
 
 #define HIKROBOT_CAPT_NAME        "hikrobot"
 
+// maximal readout time, seconds
+#define MAX_READOUT_TM      (0.3)
+
 // tolerance of float values
-#define HR_FLOAT_TOLERANCE  (0.005)
+#define HR_FLOAT_TOLERANCE  (1.1)
 
 extern camera Hikrobot;
 #endif

LocCorr_new/imagefile.c

@@ -37,9 +37,10 @@
 #include "hikrobot.h"
 #include "imagefile.h"
 #include "median.h"
+#include "Toupcam.h"
 
 typedef struct{
-    const char signature[8];
+    const uint8_t signature[8];
     uint8_t len;
     InputType it;
 } imsign;
@@ -112,6 +113,9 @@ InputType chkinput(const char *name){
 #endif
 #ifdef MVS_FOUND
     if(0 == strcmp(name, HIKROBOT_CAPT_NAME)) return T_CAPT_HIKROBOT;
+#endif
+#ifdef TOUPCAM_FOUND
+    if(0 == strcmp(name, TOUPCAM_CAPT_NAME)) return T_CAPT_TOUPCAM;
 #endif
     struct stat fd_stat;
     stat(name, &fd_stat);
@@ -204,14 +208,24 @@ Image *Image_read(const char *name){
  * @return data allocated here
  */
 Image *Image_new(int w, int h){
+    static uint64_t cnt = 0;
     if(w < 1 || h < 1) return NULL;
+    DBGLOG("Image_new(%d, #%u)", w*h, cnt);
     Image *outp = MALLOC(Image, 1);
     outp->width = w;
     outp->height = h;
+    outp->counter = cnt++;
     outp->data = MALLOC(Imtype, w*h);
     return outp;
 }
 
+void Image_free(Image **I){
+    if(!I || !*I) return;
+    DBGLOG("Image_free(%d, #%d)", (*I)->height * (*I)->width, (*I)->counter);
+    FREE((*I)->data);
+    FREE(*I);
+}
+
 /**
  * @brief Image_sim - allocate memory for new empty Image with similar size & data type
  * @param i - sample image
@@ -257,6 +271,7 @@ int get_histogram(const Image *I, size_t histo[HISTOSZ]){
  */
 int calc_background(Image *img){
     if(!img || !img->data) return FALSE;
+    DBG("image min/max=%d/%d", img->minval, img->maxval);
     if(img->maxval == img->minval){
         WARNX("Zero or overilluminated image!");
         return FALSE;
@@ -266,7 +281,7 @@ int calc_background(Image *img){
             WARNX("Image values too small");
             return FALSE;
         }
-        img->background = theconf.fixedbkg;
+        img->background = theconf.background;
         return TRUE;
     }
     size_t histogram[HISTOSZ];

LocCorr_new/imagefile.h

@@ -32,6 +32,11 @@ typedef uint8_t Imtype;
 // WARNING! Check code if you change Imtype: e.g. recalcexp() and other
 #define HISTOSZ (256)
 
+typedef struct{ // statistics: mean, RMS, area
+    float xc; float yc; float xsigma; float ysigma;
+    int area;
+} ptstat_t;
+
 typedef struct{
     int width;			// width
     int height;			// height
@@ -40,6 +45,8 @@ typedef struct{
     Imtype maxval;
     float avg_intensity;
     Imtype background;  // background value
+    ptstat_t stat;      // image statistics
+    uint64_t counter;   // image counter
 } Image;
 
 // input file/directory type
@@ -54,7 +61,8 @@ typedef enum{
     T_PNG,
     T_CAPT_GRASSHOPPER, // capture grasshopper
     T_CAPT_BASLER,
-    T_CAPT_HIKROBOT
+    T_CAPT_HIKROBOT,
+    T_CAPT_TOUPCAM,
 } InputType;
 
 void Image_minmax(Image *I);

LocCorr_new/improc.c

@@ -31,9 +31,11 @@
 #include "draw.h"
 #include "grasshopper.h"
 #include "hikrobot.h"
+#include "imagefile.h"
 #include "improc.h"
 #include "inotify.h"
 #include "steppers.h"
+#include "Toupcam.h"
 
 volatile atomic_ullong ImNumber = 0; // GLOBAL: counter of processed images
 volatile atomic_bool stopwork = FALSE; // GLOBAL: suicide
@@ -132,7 +134,7 @@ static void getDeviation(object *curobj){
     averflag = 1;
     if(fXYlog) fprintf(fXYlog, "%.1f\t%.1f\t%.1f\t%.1f", xx, yy, Sx, Sy);
 process_corrections:
-    LOGDBG("here");
+    //LOGDBG("here");
     if(theSteppers){
         DBG("Process corrections");
         if(theSteppers->proc_corr && averflag){
@@ -145,14 +147,10 @@ process_corrections:
         LOGERR("Lost connection with stepper server");
         WARNX("Lost connection with stepper server");
     }
-    LOGDBG("And there");
+    //LOGDBG("And there");
     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)
@@ -206,8 +204,6 @@ void process_file(Image *I){
     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 = sl_dtime(), tlast = t0;
 #define DELTA(p) do{double t = sl_dtime(); DBG("---> %s @ %gms (delta: %gms)", p, (t-t0)*1e3, (t-tlast)*1e3); tlast = t;}while(0)
@@ -228,6 +224,7 @@ void process_file(Image *I){
     //DELTA("Save original");
     if(calc_background(I)){
         DBG("backgr = %d", I->background);
+        theconf.background = I->background;
         DELTA("Got background");
         int objctr = 0;
         if(prev_x > 0 && prev_y > 0){
@@ -241,6 +238,7 @@ void process_file(Image *I){
             DBG("Get sum and stat for simplest centroid");
             double sum = sumAndStat(I, NULL, 0, &roi, &stat);
             if(sum > 0.){
+                I->stat = stat;
                 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);
@@ -248,6 +246,7 @@ void process_file(Image *I){
                     double WdH = stat.xsigma/stat.ysigma;
                     // wery approximate area inside sigmax*sigmay
                     double area = .4 * stat.xsigma * stat.ysigma;
+                    I->stat.area = (int)area;
                     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
@@ -274,31 +273,45 @@ void process_file(Image *I){
         uint8_t *ibin = Im2bin(I, I->background);
         DELTA("Made binary");
         if(ibin){
-            //savebin(ibin, W, H, "binary.fits");
+            if(theconf.writedebugimgs){
-            //DELTA("save binary.fits");
+                Image *Itmp = bin2Im(ibin, I->width, I->height);
+                Image_write_jpg(Itmp, "binary.jpg", 1);
+                Image_free(&Itmp);
+                DELTA("save binary");
+            }
             uint8_t *er = il_erosionN(ibin, W, H, theconf.Nerosions);
             FREE(ibin);
             DELTA("Erosion");
-            //savebin(er, W, H, "erosion.fits");
+            if(theconf.writedebugimgs){
-            //DELTA("Save erosion");
+                Image *Itmp = bin2Im(er, I->width, I->height);
+                Image_write_jpg(Itmp, "erosion.jpg", 1);
+                Image_free(&Itmp);
+                DELTA("Save erosion");
+            }
             uint8_t *opn = il_dilationN(er, W, H, theconf.Ndilations);
             FREE(er);
             DELTA("Opening");
-            //savebin(opn, W, H, "opening.fits");
+            if(theconf.writedebugimgs){
-            //DELTA("Save opening");
+                Image *Itmp = bin2Im(opn, I->width, I->height);
-            S = il_cclabel4(opn, W, H, &cc);
+                Image_write_jpg(Itmp, "opening.jpg", 1);
+                Image_free(&Itmp);
+                DELTA("Save opening");
+            }
+            il_ConnComps *cc = NULL;
+            size_t *S = il_cclabel4(opn, W, H, &cc);
             FREE(opn);
-            DBG("Nobj=%zd", cc->Nobj-1);
+            if(S && cc) 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));
                 }
+                I->stat.area = cc->boxes[1].area;
                 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);
+                    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;
                     ptstat_t stat;
@@ -323,6 +336,10 @@ void process_file(Image *I){
                     else
                         qsort(Objects, objctr, sizeof(object), compDist);
                 }
+            }
+            FREE(S);
+            FREE(cc);
+        }
 SKIP_FULL_PROCESS:
         DBGLOG("T%.2f, N=%d\n", sl_dtime(), objctr);
         DELTA("Calculate deviations");
@@ -381,10 +398,6 @@ SKIP_FULL_PROCESS:
         Image_write_jpg(I, GP->outputjpg, theconf.equalize);
     }
     DBGLOG("Image saved");
-            FREE(S);
-            FREE(cc);
-        }
-    }else Image_write_jpg(I, GP->outputjpg, theconf.equalize);
     ++ImNumber;
     if(lastTproc > 1.) FPS = 1. / (sl_dtime() - lastTproc);
     lastTproc = sl_dtime();
@@ -412,7 +425,7 @@ int process_input(InputType tp, char *name){
     if(tp == T_DIRECTORY){
         imagedata = watchdr;
         return watch_directory(name, process_file);
-    }else if(tp == T_CAPT_GRASSHOPPER || tp == T_CAPT_BASLER || tp == T_CAPT_HIKROBOT){
+    }else if(tp == T_CAPT_GRASSHOPPER || tp == T_CAPT_BASLER || tp == T_CAPT_HIKROBOT || tp == T_CAPT_TOUPCAM){
         camera *cam = NULL;
         switch(tp){
             case T_CAPT_GRASSHOPPER:
@@ -428,6 +441,11 @@ int process_input(InputType tp, char *name){
             case T_CAPT_HIKROBOT:
 #ifdef MVS_FOUND
                 cam = &Hikrobot;
+#endif
+                break;
+            case T_CAPT_TOUPCAM:
+#ifdef TOUPCAM_FOUND
+                cam = &Toupcam;
 #endif
                 break;
             default: return FALSE;

LocCorr_new/main.c

@@ -76,7 +76,8 @@ static InputType chk_inp(const char *name){
     if(!name) ERRX("Point file or directory name to monitor");
     InputType itp = chkinput(name);
     if(T_WRONG == itp) return T_WRONG;
-    green("\n%s is a ", name);
+    green("\n%s", name);
+    printf(" is a ");
     switch(itp){
         case T_DIRECTORY:
             printf("directory");
@@ -108,6 +109,9 @@ static InputType chk_inp(const char *name){
         case T_CAPT_HIKROBOT:
             printf("hikrobot camera capture");
         break;
+        case T_CAPT_TOUPCAM:
+            printf("toupcam camera capture");
+        break;
         default:
             printf("unsupported type\n");
             return T_WRONG;

LocCorr_new/socket.c

@@ -67,6 +67,7 @@ typedef struct{
 static char *helpmsg(const char *messageid, char *buf, int buflen);
 static char *stepperstatus(const char *messageid, char *buf, int buflen);
 static char *getimagedata(const char *messageid, char *buf, int buflen);
+// should be in sorted order
 static getter getterHandlers[] = {
     {"help", helpmsg, "List avaiable commands"},
     {"imdata", getimagedata, "Get image data (status, path, FPS, counter)"},
@@ -80,7 +81,7 @@ 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);
-
+// should be in sorted order
 static setter setterHandlers[] = {
     {"comment", addcmnt, "Add comment to XY log file"},
     {"focus", setfocusstate, "Move focus to given value"},

LocCorr_new/steppers.c

@@ -86,8 +86,8 @@ steppersproc *theSteppers = NULL;
 // stepper numbers
 typedef enum{
       Ustepper = 0,
-      Vstepper = 2,
+      Vstepper = 1,
-      Fstepper = 1,
+      Fstepper = 2,
     } stepperno;
 
 const char *motornames[NMOTORS] = {
@@ -200,6 +200,7 @@ static void stp_disc(){
 
 static void stp_disconnect(){
     DBG("Try to disconnect");
+    LOGDBG("Try to disconnect");
     if(serialsock){
         DBG("Close socket");
         sl_sock_delete(&serialsock);
@@ -292,9 +293,8 @@ static void parse_msg(char *msg){
         for(int idx = 0; idx < CMD_AMOUNT; ++idx){
             // search index in commands
             if(0 == strcmp(stp_commands[idx], key)){ // found our
-                free(key);
                 //LOGDBG("OK, idx=%d, cmd=%s", idx, stp_commands[idx]);
-                parser(idx, parno, ival);
+                LastErr = parser(idx, parno, ival);
                 break;
             }
         }
@@ -323,14 +323,19 @@ static errcodes send_message(steppercmd idx, char *msg){
     if(!msg) msglen = snprintf(buf, 255, "%s\n", stp_commands[idx]);
     else msglen = snprintf(buf, 255, "%s%s\n", stp_commands[idx], msg);
     //DBG("Send message '%s', len %zd", buf, msglen);
+    LastErr = ERR_AMOUNT;
     if(sl_sock_sendstrmessage(serialsock, buf) != (ssize_t)msglen){
         WARN("send()");
         LOGWARN("send_message(): send() failed");
         return ERR_WRONGLEN;
     }
-    LOGDBG("Message '%s' sent", buf);
+    //LOGDBG("send_message(): message '%s' sent", buf);
-    ;;; // wait for errcode
+    double t0 = sl_dtime();
-    return ERR_OK;
+    while(sl_dtime() - t0 < STEPPERS_NOANS_TIMEOUT){
+        if(LastErr != ERR_AMOUNT) return LastErr;
+    }
+    LOGWARN("send_message(): got NO answer for %s%s", stp_commands[idx], msg);
+    return ERR_CANTRUN;
 }
 
 // send command cmd to n'th motor with param p, @return FALSE if failed
@@ -392,20 +397,24 @@ void *clientproc(void _U_ *par){
     FNAME();
     char rbuf[BUFSIZ];
     if(!serialsock) return NULL;
+    double t0 = sl_dtime();
     do{
         ssize_t got = sl_sock_readline(serialsock, rbuf, BUFSIZ);
         //LOGDBG("got=%zd", got);
-        if(got < 0){ // disconnected
+        if(got < 0 || sl_dtime() - t0 > STEPPERS_NOANS_TIMEOUT){ // disconnected
             WARNX("Serial server disconnected");
+            LOGERR("Serial server disconnected (timeout reached)");
             if(serialsock) sl_sock_delete(&serialsock);
+            state = STP_DISCONN;
             return NULL;
         }else if(got == 0){ // nothing to read from serial port
             usleep(1000);
             continue;
         }
+        //LOGDBG("clientproc(): got '%s'", rbuf);
         // process data
-        DBG("GOT: %s", rbuf);
         parse_msg(rbuf);
+        t0 = sl_dtime();
     } while(serialsock && serialsock->connected);
     WARNX("disconnected");
     if(serialsock) sl_sock_delete(&serialsock);
@@ -458,9 +467,9 @@ static void process_movetomiddle_stage(){
         case SETUP_WAITUV0: // wait for all coordinates moving to zero
             if(!relaxed(Ustepper) || !relaxed(Vstepper) || !relaxed(Fstepper)) break; // didn't reach yet
             // now all motors are stopped -> send positions to zero
-            if( !nth_motor_setter(CMD_ABSPOS, Ustepper, 1) ||
+            if( !nth_motor_setter(CMD_ABSPOS, Ustepper, 0) ||
-                !nth_motor_setter(CMD_ABSPOS, Vstepper, 1) ||
+                !nth_motor_setter(CMD_ABSPOS, Vstepper, 0) ||
-                !nth_motor_setter(CMD_ABSPOS, Fstepper, 1)) break;
+                !nth_motor_setter(CMD_ABSPOS, Fstepper, 0)) break;
             DBG("Reached UVF0!");
             // goto
             if(nth_motor_setter(CMD_GOTO, Ustepper, (theconf.maxUpos + theconf.minUpos)/2) &&
@@ -508,8 +517,8 @@ static void process_setup_stage(){
         case SETUP_WAITUV0: // wait for both coordinates moving to zero
             if(!relaxed(Ustepper) || !relaxed(Vstepper)) break;
             // set current position to 0
-            if( !nth_motor_setter(CMD_ABSPOS, Ustepper, 1) ||
+            if( !nth_motor_setter(CMD_ABSPOS, Ustepper, 0) ||
-                !nth_motor_setter(CMD_ABSPOS, Vstepper, 1)) break;
+                !nth_motor_setter(CMD_ABSPOS, Vstepper, 0)) break;
             DBG("ZERO border reached");
             // goto middle
             if(nth_motor_setter(CMD_GOTO, Ustepper, (theconf.maxUpos+theconf.minUpos)/2) &&
@@ -901,30 +910,38 @@ static void *stp_process_states(_U_ void *arg){
             stp_connect_server();
             continue;
         }
-        // check request to change focus
-        if(chfocus){
-            DBG("Try to move F to %d", newfocpos);
-            if(nth_motor_setter(CMD_GOTO, Fstepper, newfocpos)) chfocus = FALSE;
-        }
-        if(dUmove){
-            DBG("Try to move U by %d", dUmove);
-            if(nth_motor_setter(CMD_RELPOS, Ustepper, dUmove)) dUmove = 0;
-        }
-        if(dVmove){
-            DBG("Try to move V by %d", dVmove);
-            if(nth_motor_setter(CMD_RELPOS, Vstepper, dVmove)) dVmove = 0;
-        }
         static double t0 = -1.;
         if(t0 < 0.) t0 = sl_dtime();
         if(state != STP_DISCONN){
             if(sl_dtime() - t0 >= 0.1){ // each 0.1s check state if steppers aren't disconnected
                 t0 = sl_dtime();
                 chkall();
-            }
                 if(!relaxed(Ustepper) && !relaxed(Vstepper)) continue;
                 first = TRUE;
+            }else continue;
         }
         // if we are here, all U/V moving is finished
+        // check request to change focus
+        if(chfocus){
+            DBG("Try to move F to %d", newfocpos);
+            if(nth_motor_setter(CMD_GOTO, Fstepper, newfocpos)){
+                chfocus = FALSE;
+            }
+        }
+        if(dUmove){
+            DBG("Try to move U by %d", dUmove);
+            if(nth_motor_setter(CMD_RELPOS, Ustepper, dUmove)){
+                dUmove = 0;
+                continue;
+            }
+        }
+        if(dVmove){
+            DBG("Try to move V by %d", dVmove);
+            if(nth_motor_setter(CMD_RELPOS, Vstepper, dVmove)){
+                dVmove = 0;
+                continue;
+            }
+        }
         switch(state){ // steppers state machine
             case STP_DISCONN:
                 if(!stp_connect_server()){
@@ -933,7 +950,7 @@ static void *stp_process_states(_U_ void *arg){
                         LOGWARN("Can't reconnect");
                         first = FALSE;
                     }
-                    sleep(5);
+                    sleep(1);
                 }
             break;
             case STP_SETUP: // setup axes (before this state set Xtarget/Ytarget in improc.c)

LocCorr_new/steppers.h

@@ -18,6 +18,9 @@
 
 #pragma once
 
+// timeout for "no answer from motors", seconds
+#define STEPPERS_NOANS_TIMEOUT  (5.)
+
 // set state to `disconnect` after this amount of errors in `moving_finished`
 #define MAX_ERR_CTR     (15)
 

LocCorr_new/toupcam.c

@@ -0,0 +1,318 @@
+/*
+ * This file is part of the loccorr project.
+ * Copyright 2026 Edward V. Emelianov <edward.emelianoff@gmail.com>.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <float.h>
+#include <pthread.h>
+#include <string.h>
+#include <toupcam.h>
+#include <usefull_macros.h>
+
+#include "Toupcam.h"
+
+// flags for image processing
+typedef enum{
+    IM_SLEEP,
+    IM_STARTED,
+    IM_READY,
+    IM_ERROR
+} imstate_t;
+
+// devices
+static ToupcamDeviceV2 g_dev[TOUPCAM_MAX] = {0};
+static struct{
+    ToupcamDeviceV2* dev;       // device
+    HToupcam hcam;              // hcam for all functions
+    unsigned long long flags;   // flags (read on connect)
+    pthread_mutex_t mutex;      // lock mutex for `data` writing/reading
+    void* data;                 // image data
+    size_t imsz;                // size of current image in bytes
+    imstate_t state;            // current state
+    uint64_t imseqno;           // number of image from connection
+    uint64_t lastcapno;         // last captured image number
+} toupcam = {0};
+
+// array - max ROI; geometry - current ROI
+static frameformat array = {.xoff=0, .yoff = 0, .w = 800, .h = 600}, geometry = {0};
+
+// exptime (in seconds!!!) and starting of exposition
+static double exptimeS = 0.1, starttime = 0.;
+
+#define TCHECK()    do{if(!toupcam.hcam) return FALSE;}while(0)
+
+// return constant string with error code description
+static const char *errcode(int ecode){
+    switch(ecode){
+    case 0: return "S_OK";
+    case 1: return "S_FALSE";
+    case 0x8000ffff: return "E_UNEXPECTED";
+    case 0x80004001: return "E_NOTIMPL";
+    case 0x80004002: return "E_NOINTERFACE";
+    case 0x80070005: return "E_ACCESSDENIED";
+    case 0x8007000e: return "E_OUTOFMEMORY";
+    case 0x80070057: return "E_INVALIDARG";
+    case 0x80004003: return "E_POINTER";
+    case 0x80004005: return "E_FAIL";
+    case 0x8001010e: return "E_WRONG_THREAD";
+    case 0x8007001f: return "E_GEN_FAILURE";
+    case 0x800700aa: return "E_BUSY";
+    case 0x8000000a: return "E_PENDING";
+    case 0x8001011f: return "E_TIMEOUT";
+    case 0x80072743: return "E_UNREACH";
+    default: return "Unknown error";
+    }
+}
+
+static void camcancel(){
+    FNAME();
+    if(!toupcam.hcam) return;
+    int e = Toupcam_Trigger(toupcam.hcam, 0); // stop triggering
+    if(e < 0) WARNX("Can't trigger 0: %s", errcode(e));
+    e = Toupcam_Stop(toupcam.hcam);
+    if(e < 0) WARNX("Can't stop: %s", errcode(e));
+    toupcam.state = IM_SLEEP;
+}
+
+static void Tdisconnect(){
+    FNAME();
+    camcancel();
+    if(toupcam.hcam){
+        DBG("Close camera");
+        Toupcam_Close(toupcam.hcam);
+        toupcam.hcam = NULL;
+    }
+    if(toupcam.data){
+        DBG("Free image data");
+        free(toupcam.data);
+        toupcam.data = NULL;
+    }
+}
+
+static void EventCallback(unsigned nEvent, void _U_ *pCallbackCtx){
+    FNAME();
+    DBG("CALLBACK with evt %d", nEvent);
+    if(!toupcam.hcam || !toupcam.data){ DBG("NO data!"); return; }
+    if(nEvent != TOUPCAM_EVENT_IMAGE){ DBG("Not image event"); return; }
+    ToupcamFrameInfoV4 info = {0};
+    pthread_mutex_lock(&toupcam.mutex);
+    if(Toupcam_PullImageV4(toupcam.hcam, toupcam.data, 0, 0, 0, &info) < 0){
+        DBG("Error pulling image");
+        toupcam.state = IM_ERROR;
+    }else{
+        ++toupcam.imseqno;
+        DBG("Image %lu (%dx%d) ready!", toupcam.imseqno, info.v3.width, info.v3.height);
+        toupcam.state = IM_READY;
+        toupcam.imsz = info.v3.height * info.v3.width;
+        geometry.h = info.v3.height;
+        geometry.w = info.v3.width;
+    }
+    pthread_mutex_unlock(&toupcam.mutex);
+}
+
+static int startexp(){
+    FNAME();
+    TCHECK();
+    if(toupcam.state == IM_SLEEP){
+        DBG("Sleeping -> start pull mode");
+        if(Toupcam_StartPullModeWithCallback(toupcam.hcam, EventCallback, NULL) < 0){
+            WARNX("Can't run PullMode with Callback!");
+            return FALSE;
+        }
+    }
+    // Ask to trigger for several images (maximal speed available)
+    DBG("Trigger images");
+    int e = Toupcam_Trigger(toupcam.hcam, 100);
+    if(e < 0){
+        DBG("Can't ask for images stream: %s; try 1", errcode(e));
+        e = Toupcam_Trigger(toupcam.hcam, 1);
+        if(e < 0){
+            WARNX("Can't ask for next image: %s", errcode(e));
+            return FALSE;
+        }
+    }
+    toupcam.state = IM_STARTED;
+    starttime = sl_dtime();
+    return TRUE;
+}
+
+static int Texp(float t){ // t - in milliseconds!!!
+    FNAME();
+    TCHECK();
+    if(t < FLT_EPSILON) return FALSE;
+    unsigned int microseconds = (unsigned)(t * 1e3f);
+    DBG("Set exptime to %dus", microseconds);
+    camcancel();
+    int e = Toupcam_put_ExpoTime(toupcam.hcam, microseconds);
+    if(e < 0){
+        WARNX("Can't set exp: %s", errcode(e));
+        //startexp();
+        return FALSE;
+    }
+    DBG("OK");
+    if(Toupcam_get_ExpoTime(toupcam.hcam, &microseconds) < 0) exptimeS = (double) t / 1e3;
+    else exptimeS = (float)microseconds / 1e6f;
+    DBG("Real exptime: %.4fs", exptimeS);
+    //startexp();
+    return TRUE;
+}
+
+static int Tconnect(){
+    FNAME();
+    Tdisconnect();
+    unsigned N = Toupcam_EnumV2(g_dev);
+    if(0 == N){
+        DBG("Found 0 toupcams");
+        return FALSE;
+    }
+    toupcam.dev = &g_dev[0];
+    toupcam.hcam = Toupcam_Open(g_dev[0].id);
+    if(!toupcam.hcam){
+        WARN("Can't open toupcam camera");
+        return FALSE;
+    }
+    DBG("Opened %s", toupcam.dev->displayname);
+    DBG("Clear ROI");
+    Toupcam_put_Roi(toupcam.hcam, 0, 0, 0, 0); // clear ROI
+    // now fill camera geometry
+    unsigned int xoff, yoff, h, w;
+    DBG("Get ROI");
+    Toupcam_get_Roi(toupcam.hcam, &xoff, &yoff, &w, &h);
+    DBG("off (x/y): %d/%d; wxh: %dx%d", xoff, yoff, w, h);
+    geometry.xoff = xoff; geometry.yoff = yoff; geometry.w = w; geometry.h = h;
+    toupcam.flags = Toupcam_query_Model(toupcam.hcam)->flag;
+    DBG("flags: 0x%llx", toupcam.flags);
+    DBG("Allocate data (%d bytes: 2*%d*%d)", 2 * array.w * array.h, array.w, array.h);
+    toupcam.data = calloc(array.w * array.h, 1);
+#define OPT(opt, val, comment)  do{DBG(comment); if(Toupcam_put_Option(toupcam.hcam, opt, val) < 0){ DBG("Can't put this option"); }}while(0)
+    // 12 frames/sec
+    OPT(TOUPCAM_OPTION_TRIGGER, 1, "Software/simulated trigger mode");
+    OPT(TOUPCAM_OPTION_RAW, 1, "Put to RAW mode");
+    OPT(TOUPCAM_OPTION_BINNING, 1, "Set binning to 1x1");
+#undef OPT
+    toupcam.state = IM_SLEEP;
+    toupcam.imseqno = toupcam.lastcapno = 0;
+    // 8bit
+    if(Toupcam_put_Option(toupcam.hcam, TOUPCAM_OPTION_BITDEPTH, 0) < 0) WARNX("Cant set bitdepth");
+    if(Toupcam_put_Option(toupcam.hcam, TOUPCAM_OPTION_PIXEL_FORMAT, TOUPCAM_PIXELFORMAT_RAW8) < 0){
+        WARNX("Cannot init 8bit mode!");
+        Tdisconnect();
+        return FALSE;
+    }
+    pthread_mutex_init(&toupcam.mutex, NULL);
+    if(!Texp(0.1)){ WARNX("Can't set default exptime"); }
+    return TRUE;
+}
+
+static Image *Tcapture(){
+    FNAME();
+    if(!toupcam.hcam || !toupcam.data) return NULL;
+    if(!startexp()){
+        WARNX("Can't start exposition");
+        return NULL;
+    }
+    DBG("here, exptime=%gs, dstart=%g", exptimeS, (sl_dtime() - starttime));
+    double tremain = 0.;
+    while(toupcam.state != IM_READY){
+        tremain = exptimeS - (sl_dtime() - starttime);
+        if(tremain < -2.0){
+            WARNX("Timeout - failed");
+            camcancel();
+            return NULL;
+        }
+        usleep(100);
+    }
+    if(toupcam.state != IM_READY){
+        WARNX("State=%d, not ready", toupcam.state);
+        return NULL;
+    }
+    pthread_mutex_lock(&toupcam.mutex);
+    Image *o = u8toImage(toupcam.data, geometry.w, geometry.h, geometry.w);
+    toupcam.lastcapno = toupcam.imseqno;
+    pthread_mutex_unlock(&toupcam.mutex);
+    return o;
+}
+
+static int Tbright(float b){
+    FNAME();
+    TCHECK();
+    if(b < -255.f || b > 255.f){
+        WARNX("Available brightness: -255..255");
+        return FALSE;
+    }
+    int br = (int) b;
+    DBG("Try to set brightness to %d", br);
+    camcancel();
+    int e = Toupcam_put_Brightness(toupcam.hcam, br);
+    //startexp();
+    if(e < 0){
+        WARNX("Can't set brightness: %s", errcode(e));
+        return FALSE;
+    }
+    DBG("OK");
+    return TRUE;
+}
+
+static int Tgain(float g){
+    FNAME();
+    TCHECK();
+    unsigned short G = (unsigned short)(100.f * g);
+    int ret = FALSE;
+    camcancel();
+    if(Toupcam_put_ExpoAGain(toupcam.hcam, G) < 0){
+        WARNX("Gain out of range: 1..8");
+    }else{ DBG("GAIN is %d", G); ret = TRUE;}
+    //startexp();
+    return ret;
+}
+
+static float Tmaxgain(){
+    FNAME();
+    return 8.f; // toupcam SDK returns wrong value: 16.
+}
+
+static int Tgeometry(frameformat *f){
+    FNAME();
+    TCHECK();
+    int ret = FALSE;
+    camcancel();
+    if(Toupcam_put_Roi(toupcam.hcam, (unsigned) f->xoff, (unsigned) f->yoff, (unsigned) f->w, (unsigned) f->h) >= 0){
+        geometry = *f;
+        ret = TRUE;
+    }
+    //startexp();
+    return ret;
+}
+
+static int Tglimits(frameformat *max, frameformat *step){
+    FNAME();
+    TCHECK();
+    if(max) *max = array;
+    if(step) *step = (frameformat){.w = 2, .h = 2, .xoff = 2, .yoff = 2};
+    return TRUE;
+}
+
+camera Toupcam = {
+    .disconnect = Tdisconnect,
+    .connect = Tconnect,
+    .capture = Tcapture,
+    .setbrightness = Tbright,
+    .setexp = Texp,
+    .setgain = Tgain,
+    .getmaxgain = Tmaxgain,
+    .setgeometry = Tgeometry,
+    .getgeomlimits = Tglimits,
+};