cont

LocCorr_new/hikrobot.c

@@ -94,7 +94,7 @@ static int changeenum(const char *key, uint32_t val){
     TRY(GetEnumValue, key, &e);
     ONERR() return FALSE;
     if(e.nCurValue == val) return TRUE;
-    WARNX("New value of '%s' changed to %d, not to %d", key, e.nCurValue, val);
+    WARNX("New ENUM value of '%s' changed to %d, not to %d", key, e.nCurValue, val);
     return FALSE;
 }
 
@@ -109,13 +109,13 @@ static int changeint(const char *key, uint32_t val){
     if(i.nCurValue == val) return TRUE;
     TRYERR(SetIntValue, key, val);
     ONERR(){
-        WARNX("Cant change %s to %u; available range is %u..%u", key, val, i.nMin, i.nMax);
+        WARNX("Can't change %s to %u; available range is %u..%u", key, val, i.nMin, i.nMax);
         return FALSE;
     }
     TRY(GetIntValue, key, &i);
     ONERR() return FALSE;
     if(i.nCurValue == val) return TRUE;
-    WARNX("New value of '%s' changed to %d, not to %d", key, i.nCurValue, val);
+    WARNX("New INT value of '%s' changed to %d, not to %d", key, i.nCurValue, val);
     return FALSE;
 }
 
@@ -135,8 +135,9 @@ static int changefloat(const char *key, float val){
     }
     TRY(GetFloatValue, key, &f);
     ONERR() return FALSE;
-    if(fabs(f.fCurValue - val) < __FLT_EPSILON__) return TRUE;
+    DBG("need: %g, have: %g (min/max: %g/%g)", val, f.fCurValue, f.fMin, f.fMax);
-    WARNX("New value of '%s' changed to %g, not to %g", key, f.fCurValue, val);
+    if(fabs(f.fCurValue - val) < HR_FLOAT_TOLERANCE) return TRUE;
+    WARNX("New FLOAT value of '%s' changed to %g, not to %g", key, f.fCurValue, val);
     return FALSE;
 }
 
@@ -166,8 +167,12 @@ static int cam_getbright(float *b){
     if(!handle) return FALSE;
     DBG("Get brightness");
     MVCC_INTVALUE bright;
-    TRY(GetIntValue, "Brightness", &bright);
+    //TRY(GetIntValue, "Brightness", &bright);
-    ONERR() return FALSE;
+    TRY(GetBrightness, &bright);
+    ONERR(){
+        DBG("There's no such function");
+        return FALSE;
+    }
     if(b) *b = bright.nCurValue;
     extrvalues.maxgain = bright.nMax;
     extrvalues.mingain = bright.nMin;
@@ -177,7 +182,10 @@ static int cam_getbright(float *b){
 
 static int cam_setbright(float b){
     if(!handle) return FALSE;
-    return changeint("Brightness", (uint32_t)b);
+    TRY(SetBrightness, (uint32_t)b);
+    ONERR() return FALSE;
+    return TRUE;
+    //return changeint("Brightness", (uint32_t)b);
 }
 
 static void cam_closecam(){

LocCorr_new/hikrobot.h

@@ -23,5 +23,8 @@
 
 #define HIKROBOT_CAPT_NAME        "hikrobot"
 
+// tolerance of float values
+#define HR_FLOAT_TOLERANCE  (0.005)
+
 extern camera Hikrobot;
 #endif

LocCorr_new/steppers.c

@@ -80,6 +80,7 @@ static _Atomic setupstatus sstatus = SETUP_NONE; // setup state
 
 //static int errctr = 0; // sending messages error counter (if > MAX_ERR_CTR, set state to disconnected)
 
+// common global variable for using in several sources (should be inited first)
 steppersproc *theSteppers = NULL;
 
 // stepper numbers
@@ -139,6 +140,10 @@ static const char* errtxt[ERR_AMOUNT] = {
     [ERR_CANTRUN] = "CANTRUN",
 };
 
+static atomic_int LastErr = ERR_OK;
+static sl_sock_t *serialsock = NULL;
+static pthread_t clientthread;
+
 static STPstate state = STP_DISCONN;   // server state
 // this flag set to TRUE when next Xc,Yc available
 static volatile atomic_bool coordsRdy = FALSE;
@@ -149,11 +154,9 @@ static double Xtarget = 0., Ytarget = 0.;
 static volatile atomic_bool chfocus = FALSE;
 static volatile atomic_int newfocpos = 0, dUmove = 0, dVmove = 0;
 
-static volatile atomic_int sockfd = -1; // server file descriptor
 static volatile atomic_bool motorsoff = FALSE; // flag to disconnect
 
-// mutex for message sending/receiving
+
-static pthread_mutex_t mesg_mutex = PTHREAD_MUTEX_INITIALIZER;
 
 // current steps counters (zero at the middle)
 static volatile atomic_int motposition[NMOTORS] = {0};
@@ -197,13 +200,17 @@ static void stp_disc(){
 
 static void stp_disconnect(){
     DBG("Try to disconnect");
-    if(sockfd > -1){
+    if(serialsock){
-        DBG("sockfd closed");
+        DBG("Close socket");
-        close(sockfd);
+        sl_sock_delete(&serialsock);
+        if(!pthread_cancel(clientthread)){
+            DBG("Join canceled thread");
+            pthread_join(clientthread, NULL);
+            DBG("OK");
+        }
+        LOGWARN("Stepper server disconnected");
     }
     state = STP_DISCONN;
-    sockfd = -1;
-    LOGWARN("Stepper server disconnected");
 }
 
 // check if nmot is U/V/F and return FALSE if not
@@ -212,87 +219,8 @@ static int chkNmot(int nmot){
     return FALSE;
 }
 
-/**
- * wait for answer from socket
- * @return FALSE in case of error or timeout, TRUE if socket is ready
- */
-static int canread(){
-    if(sockfd < 0) return FALSE;
-    fd_set fds;
-    struct timeval timeout;
-    int rc;
-    // wait not more than 10ms
-    timeout.tv_sec = 0;
-    timeout.tv_usec = 10000;
-    FD_ZERO(&fds);
-    FD_SET(sockfd, &fds);
-    do{
-        rc = select(sockfd+1, &fds, NULL, NULL, &timeout);
-        if(rc < 0){
-            if(errno != EINTR){
-                WARN("select()");
-                return FALSE;
-            }
-            continue;
-        }
-        break;
-    }while(1);
-    if(FD_ISSET(sockfd, &fds)){
-        //DBG("CANREAD");
-        return TRUE;
-    }
-    return FALSE;
-}
 
-// clear all data received earlier
+static errcodes parser(steppercmd idx, int nmot, int ival){
-static void clrbuf(){
-    char buf[256];
-    while(canread()){
-        ssize_t got = recv(sockfd, buf, 256, 0);
-        DBG("cleared %zd bytes of trash", got);
-        if(got <= 0){ // disconnect or error
-            LOGERR("Server disconnected");
-            ERRX("Server disconnected");
-        }
-    }
-}
-
-// read message (if exists) and return its length (or -1 if none)
-// There's could be many strings of data!!!
-static ssize_t read_message(char *msg, size_t msglen){
-    if(!msg || msglen == 0) return -1;
-    if(pthread_mutex_lock(&mesg_mutex)){
-        WARN("pthread_mutex_lock()");
-        LOGWARN("read_message(): pthread_mutex_lock() err");
-        return 0;
-    }
-    double t0 = sl_dtime();
-    size_t gotbytes = 0;
-    --msglen; // for trailing zero
-    while(sl_dtime() - t0 < WAITANSTIME && gotbytes < msglen && sockfd > 0){
-        if(!canread()) continue;
-        int n = recv(sockfd, msg+gotbytes, msglen, 0);
-        if(n <= 0){ // disconnect or error
-            LOGERR("Server disconnected");
-            ERRX("Server disconnected");
-        }
-        if(n == 0) break;
-        gotbytes += n;
-        msglen -= n;
-        if(msg[gotbytes-1] == '\n') break;
-        t0 = sl_dtime();
-    }
-    //DBG("Dt=%g, gotbytes=%zd, sockfd=%d, msg='%s'", dtime()-t0,gotbytes,sockfd,msg);
-    pthread_mutex_unlock(&mesg_mutex);
-    if(msg[gotbytes-1] != '\n'){
-        //DBG("No newline at the end");
-        return 0;
-    }
-    msg[gotbytes] = 0;
-    return gotbytes;
-}
-
-static errcodes parsing(steppercmd idx, int nmot, int ival){
     int goodidx = chkNmot(nmot);
     switch(idx){
         case CMD_ABSPOS:
@@ -337,57 +265,46 @@ static errcodes getecode(const char *msg){
 }
 
 /**
- * @brief read_parse - read answer (till got or WAITANSTIME timeout) and parse it
+ * @brief parse_msg - parse last message
- * @param idx - index of message sent (to compare answer)
+ * @param msg - message received
- * @return
  */
-static errcodes read_and_parse(steppercmd idx){
+static void parse_msg(char *msg){
-    char value[128], msg[1024];
+    char value[128];
-    double t0 = sl_dtime();
+    if(!msg) return;
-    while(sl_dtime() - t0 < WAITANSTIME*10.){
+    char *key = get_keyval(msg, value);
-        ssize_t got = read_message(msg, 1024);
+    if(key){
-        if(got < 1) continue;
+        int ival = atoi(value);
-        //LOGDBG("GOT from stepper server:\n%s\n", msg);
+        //LOGDBG("key = %s, value = %s (%d)", key, value, ival);
-        char *saveptr, *tok = msg;
+        size_t l = strlen(key);
-        for(;; tok = NULL){
+        size_t numpos = strcspn(key, "0123456789");
-            char *token = strtok_r(tok, "\n", &saveptr);
+        int parno = -1;
-            if(!token) break;
+        if(numpos < l){
-            //LOGDBG("Got line: %s", token);
+            parno = atoi(key + numpos);
-            char *key = get_keyval(token, value);
+            key[numpos] = 0;
-            if(key){
+        }
-                int ival = atoi(value);
+        //DBG("numpos=%zd, parno=%d", numpos, parno);
-                //LOGDBG("key = %s, value = %s (%d)", key, value, ival);
+        if(parno > -1){ // got motor number
-                size_t l = strlen(key);
+            if(!chkNmot(parno)){
-                size_t numpos = strcspn(key, "0123456789");
+                DBG("Not our business");
-                int parno = -1;
+                free(key); return;
-                if(numpos < l){
-                    parno = atoi(key + numpos);
-                    key[numpos] = 0;
-                }
-                //DBG("numpos=%zd, parno=%d", numpos, parno);
-                if(parno > -1){ // got motor number
-                    if(!chkNmot(parno)){
-                        DBG("Not our business");
-                        free(key); continue;
-                    }
-                }
-                // 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]);
-                    return parsing(idx, parno, ival);
-                }
-                free(key);
-            }else{
-                DBG("GOT NON-setter %s", token);
-                errcodes e = getecode(token);
-                if(e != ERR_AMOUNT) return e; // ERR_AMOUNT means some other message
             }
         }
+        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);
+                break;
+            }
+        }
+        free(key);
+        return;
+    }else{
+        DBG("GOT NON-setter %s", msg);
+        errcodes e = getecode(msg);
+        if(e != ERR_AMOUNT) LastErr = e; // for checking
     }
-    DBG("No answer detected to our command");
-    LOGDBG("read_and_parse(): no answer detected to our command");
-    return ERR_CANTRUN; // didn't get anwer need
 }
 
 /**
@@ -397,26 +314,23 @@ static errcodes read_and_parse(steppercmd idx){
  */
 static errcodes send_message(steppercmd idx, char *msg){
     // FNAME();
-    if(sockfd < 0) return ERR_CANTRUN;
+    if(!serialsock){
+        WARNX("Not connected to serial socket!");
+        return ERR_CANTRUN;
+    }
     char buf[256];
     size_t msglen;
     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);
-    if(pthread_mutex_lock(&mesg_mutex)){
+    if(sl_sock_sendstrmessage(serialsock, buf) != (ssize_t)msglen){
-        WARN("pthread_mutex_lock()");
-        LOGWARN("send_message(): pthread_mutex_lock() err");
-        return FALSE;
-    }
-    clrbuf();
-    if(send(sockfd, buf, msglen, 0) != (ssize_t)msglen){
         WARN("send()");
         LOGWARN("send_message(): send() failed");
         return ERR_WRONGLEN;
     }
-    //LOGDBG("Message '%s' sent", buf);
+    LOGDBG("Message '%s' sent", buf);
-    pthread_mutex_unlock(&mesg_mutex);
+    ;;; // wait for errcode
-    return read_and_parse(idx);
+    return ERR_OK;
 }
 
 // send command cmd to n'th motor with param p, @return FALSE if failed
@@ -470,42 +384,54 @@ static void chkall(){
 }
 
 /**
- * @brief stp_connect_server - try connect to a local steppers CAN server
+ * @brief clientproc - process data received from serial terminal
+ * @param par - socket
+ * @return NULL
+ */
+void *clientproc(void _U_ *par){
+    FNAME();
+    char rbuf[BUFSIZ];
+    if(!serialsock) return NULL;
+    do{
+        ssize_t got = sl_sock_readline(serialsock, rbuf, BUFSIZ);
+        //LOGDBG("got=%zd", got);
+        if(got < 0){ // disconnected
+            WARNX("Serial server disconnected");
+            if(serialsock) sl_sock_delete(&serialsock);
+            return NULL;
+        }else if(got == 0){ // nothing to read from serial port
+            usleep(1000);
+            continue;
+        }
+        // process data
+        DBG("GOT: %s", rbuf);
+        parse_msg(rbuf);
+    } while(serialsock && serialsock->connected);
+    WARNX("disconnected");
+    if(serialsock) sl_sock_delete(&serialsock);
+    return NULL;
+}
+
+/**
+ * @brief stp_connect_server - try connect to a local steppers serial server
  * @return FALSE if failed
  */
 static int stp_connect_server(){
-    DBG("STP_connect(%d)", theconf.stpserverport);
+    FNAME();
-    char port[10];
-    snprintf(port, 10, "%d", theconf.stpserverport);
     stp_disconnect();
-    struct addrinfo hints = {0}, *res, *p;
+    char node[32];
-    hints.ai_family = AF_INET;
+    snprintf(node, 31, "%d", theconf.stpserverport);
-    hints.ai_socktype = SOCK_STREAM;
+    DBG("Try to connect via port %s", node);
-    hints.ai_flags = AI_PASSIVE;
+    serialsock = sl_sock_run_client(SOCKT_NETLOCAL, node, 4096);
-    if(getaddrinfo(NULL, port, &hints, &res) != 0){
+    if(!serialsock){
-        WARN("getaddrinfo()");
+        DBG("Can't connect to serial server via port %s", node);
         return FALSE;
     }
-    // loop through all the results and connect to the first we can
+    if(pthread_create(&clientthread, NULL, clientproc, NULL)){
-    for(p = res; p; p = p->ai_next){
+        sl_sock_delete(&serialsock);
-        if((sockfd = socket(p->ai_family, p->ai_socktype, p->ai_protocol)) == -1){
+        LOGWARN("stp_connect_server(): can't create client thread");
-            WARN("socket");
-            continue;
-        }
-        if(connect(sockfd, p->ai_addr, p->ai_addrlen) == -1){
-            WARN("connect()");
-            close(sockfd);
-            continue;
-        }
-        break; // if we get here, we have a successfull connection
-    }
-    if(!p){
-        WARNX("Can't connect to socket");
-        LOGWARN("Can't connect to steppers server");
-        sockfd = -1;
         return FALSE;
     }
-    freeaddrinfo(res);
     // register and set max speed; don't check `register` answer as they could be registered already
     state = STP_RELAX;
     sstatus = SETUP_NONE;
@@ -1121,7 +1047,7 @@ static steppersproc steppers = {
 };
 
 /**
- * @brief STP_connect - run a thread processed steppers status
+ * @brief steppers_connect - run a thread processed steppers status
  * @return FALSE if failed to connect immediately
  */
 steppersproc* steppers_connect(){