nxt

LibSidServo/examples/dumpmoving.c

@@ -51,10 +51,11 @@ void signals(int sig){
 }
 
 static conf_t Config = {
-    .MountPath = "/dev/ttyS1",
+    .MountDevPath = "/dev/ttyS1",
-    .MountSpeed = 19200,
+    .MountDevSpeed = 19200,
-    .EncoderPath = "/dev/ttyUSB0",
+    //.EncoderDevPath = "/dev/ttyUSB0",
-    .EncoderSpeed = 153000
+    //.EncoderDevSpeed = 153000,
+    .SepEncoder = 0
 };
 
 int main(int argc, char **argv){
@@ -67,8 +68,8 @@ int main(int argc, char **argv){
     time_t curtime = time(NULL);
     LOGMSG("Started @ %s", ctime(&curtime));
     DBG("Devices ready");
-    LOGMSG("Mount device %s @ %d", Config.MountPath, Config.MountSpeed);
+    LOGMSG("Mount device %s @ %d", Config.MountDevPath, Config.MountDevSpeed);
-    LOGMSG("Encoder device %s @ %d", Config.EncoderPath, Config.EncoderSpeed);
+    LOGMSG("Encoder device %s @ %d", Config.EncoderDevPath, Config.EncoderDevSpeed);
     signal(SIGTERM, signals); // kill (-15) - quit
     signal(SIGHUP, SIG_IGN);  // hup - ignore
     signal(SIGINT, signals);  // ctrl+C - quit

LibSidServo/libsidservo.creator.user

@@ -1,6 +1,6 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <!DOCTYPE QtCreatorProject>
-<!-- Written by QtCreator 15.0.0, 2025-01-30T22:13:51. -->
+<!-- Written by QtCreator 15.0.0, 2025-02-04T21:29:56. -->
 <qtcreator>
  <data>
   <variable>EnvironmentId</variable>

LibSidServo/libsidservo.files

@@ -6,4 +6,5 @@ sidservo.h
 serial.c
 examples/CMakeLists.txt
 serial.h
+ssii.c
 ssii.h

LibSidServo/main.c

@@ -42,22 +42,26 @@ static mcc_errcodes_t init(conf_t *c){
     if(!c) return MCC_E_BADFORMAT;
     Conf = *c;
     mcc_errcodes_t ret = MCC_E_OK;
-    if(!Conf.MountPath || Conf.MountSpeed < 1200){
+    if(!Conf.MountDevPath || Conf.MountDevSpeed < 1200){
         DBG("Define mount device path and speed");
         ret = MCC_E_BADFORMAT;
-    }else if(!openMount(Conf.MountPath, Conf.MountSpeed)){
+    }else if(!openMount(Conf.MountDevPath, Conf.MountDevSpeed)){
-        DBG("Can't open %s with speed %d", Conf.MountPath, Conf.MountSpeed);
+        DBG("Can't open %s with speed %d", Conf.MountDevPath, Conf.MountDevSpeed);
         ret = MCC_E_MOUNTDEV;
     }
     if(Conf.SepEncoder){
-        if(!Conf.EncoderPath || Conf.EncoderSpeed < 1200){
+        if(!Conf.EncoderDevPath || Conf.EncoderDevSpeed < 1200){
             DBG("Define encoder device path and speed");
             ret = MCC_E_BADFORMAT;
-        }else if(!openEncoder(Conf.EncoderPath, Conf.EncoderSpeed)){
+        }else if(!openEncoder(Conf.EncoderDevPath, Conf.EncoderDevSpeed)){
-            DBG("Can't open %s with speed %d", Conf.EncoderPath, Conf.EncoderSpeed);
+            DBG("Can't open %s with speed %d", Conf.EncoderDevPath, Conf.EncoderDevSpeed);
             ret = MCC_E_ENCODERDEV;
         }
     }
+    if(Conf.MountReqInterval < 1 || Conf.MountReqInterval > 1000000){
+        DBG("Bad value of MountReqInterval");
+        retr = MCC_E_FATAL;
+    }
     if(ret != MCC_E_OK) quit();
     return ret;
 }

LibSidServo/serial.c

@@ -44,7 +44,8 @@ static pthread_mutex_t  mntmutex = PTHREAD_MUTEX_INITIALIZER,
                         datamutex = PTHREAD_MUTEX_INITIALIZER;
 // encoders thread and mount thread
 static pthread_t encthread, mntthread;
-
+// max timeout for 1.5 bytes of encoder and 2 bytes of mount
+static struct timeval encRtmout = {0}, mntRtmout = {0};
 // encoders raw data
 typedef struct __attribute__((packed)){
     uint8_t magick;
@@ -119,16 +120,15 @@ static void parse_encbuf(uint8_t databuf[ENC_DATALEN], struct timeval *tv){
 }
 
 // try to read 1 byte from encoder; return -1 if nothing to read or -2 if device seems to be disconnected
-static int getbyte(){
+static int getencbyte(){
     if(encfd < 0) return -1;
     uint8_t byte;
     fd_set rfds;
-    // default timeot = 100us, 1.5 bytes
+    struct timeval tv;
-    struct timeval tv, tvdeflt = {.tv_sec = 0, .tv_usec = 100};
     do{
         FD_ZERO(&rfds);
         FD_SET(encfd, &rfds);
-        tv = tvdeflt;
+        tv = encRtmout;
         int retval = select(encfd + 1, &rfds, NULL, NULL, &tv);
         if(!retval) break;
         if(retval < 0){
@@ -139,7 +139,31 @@ static int getbyte(){
             ssize_t l = read(encfd, &byte, 1);
             if(l != 1) return -2; // disconnected ??
             break;
+        } else return -1;
+    }while(1);
+    return (int)byte;
+}
+// read 1 byte from mount; return -1 if nothing to read, -2 if disconnected
+static int getmntbyte(){
+    if(mntfd < 0) return -1;
+    uint8_t byte;
+    fd_set rfds;
+    struct timeval tv;
+    do{
+        FD_ZERO(&rfds);
+        FD_SET(mntfd, &rfds);
+        tv = mntRtmout;
+        int retval = select(mntfd + 1, &rfds, NULL, NULL, &tv);
+        if(!retval) break;
+        if(retval < 0){
+            if(errno == EINTR) continue;
+            return -1;
         }
+        if(FD_ISSET(mntfd, &rfds)){
+            ssize_t l = read(mntfd, &byte, 1);
+            if(l != 1) return -2; // disconnected ??
+            break;
+        } else return -1;
     }while(1);
     return (int)byte;
 }
@@ -150,7 +174,7 @@ static void *encoderthread(void _U_ *u){
     int wridx = 0, errctr = 0;
     struct timeval tv;
     while(encfd > -1 && errctr < MAX_ERR_CTR){
-        int b = getbyte();
+        int b = getencbyte();
         if(b == -2) ++errctr;
         if(b < 0) continue;
         errctr = 0;
@@ -178,15 +202,32 @@ static void *encoderthread(void _U_ *u){
 // main mount thread
 static void *mountthread(void _U_ *u){
     int errctr = 0;
+    SSstat status;
+    // data to get
+    data_t d = {.buf = (uint8_t*)&status, .maxlen = sizeof(SSstat)};
+    // cmd to send
+    const data_t cmd = {.buf = CMD_GETSTAT, .len = sizeof(CMD_GETSTAT)-1, .maxlen = sizeof(CMD_GETSTAT)-1};
     while(mntfd > -1 && errctr < MAX_ERR_CTR){
-        ;
+        // read data to status
-        pthread_mutex_lock(&mntmutex);
+        if(MountWriteRead(&cmd, &d) || d.len != sizeof(SSstat)){
-        ;
+            DBG("Can't read SSstat");
+            ++errctr; continue;
+        }
+        if(SScalcChecksum((uint8_t*)status, sizeof(SSstat)-2) != status.checksum){
+            DBG("BAD checksum of SSstat");
+            ++errctr; continue;
+        }
+        errctr = 0;
+        pthread_mutex_lock(&datamutex);
+        // now change data
+        SSconvstat(&status, &mountdata);
         if(!Conf.SepEncoder){ // fill encoder data from here, as there's no separate enc thread
             ;
         }
         ;
-        pthread_mutex_unlock(&mntmutex);
+        pthread_mutex_unlock(&datamutex);
+        // allow writing & getters
+        usleep(Conf.MountReqInterval);
     }
     if(mntfd > -1){
         close(mntfd);
@@ -196,7 +237,7 @@ static void *mountthread(void _U_ *u){
 }
 
 // open device and return its FD or -1
-static int ttyopen(const char *path, int speed){
+static int ttyopen(const char *path, speed_t speed){
     int fd = -1;
     struct termios2 tty;
     DBG("Try to open %s @ %d", path, speed);
@@ -222,8 +263,10 @@ static int ttyopen(const char *path, int speed){
 int openEncoder(const char *path, int speed){
     if(!Conf.SepEncoder) return FALSE; // try to open separate encoder when it's absent
     if(encfd > -1) close(encfd);
-    encfd = ttyopen(path, speed);
+    encfd = ttyopen(path, (speed_t) speed);
     if(encfd < 0) return FALSE;
+    encRtmout.tv_sec = 0;
+    encRtmout.tv_usec = 15000000 / speed; // 1.5 bytes
     if(pthread_create(&encthread, NULL, encoderthread, NULL)){
         close(encfd);
         encfd = -1;
@@ -236,8 +279,10 @@ int openEncoder(const char *path, int speed){
 // return FALSE if failed
 int openMount(const char *path, int speed){
     if(mntfd > -1) close(mntfd);
-    mntfd = ttyopen(path, speed);
+    mntfd = ttyopen(path, (speed_t) speed);
     if(mntfd < 0) return FALSE;
+    mntRtmout.tv_sec = 0;
+    mntRtmout.tv_usec = 20000000 / speed; // 2 bytes
     if(pthread_create(&mntthread, NULL, mountthread, NULL)){
         close(mntfd);
         mntfd = -1;
@@ -273,3 +318,31 @@ mcc_errcodes_t getMD(mountdata_t  *d){
     pthread_mutex_unlock(&datamutex);
     return MCC_E_OK;
 }
+
+/**
+ * @brief MountWriteRead - write and read @ once (or only read/write)
+ * @param out (o) - data to write or NULL if not need
+ * @param in  (i) - data to read or NULL if not need
+ * @return FALSE if failed
+ */
+int MountWriteRead(const data_t *out, data_t *in){
+    if(!out && !in) return FALSE;
+    int ret = FALSE;
+    pthread_mutex_lock(&mntmutex);
+    if(out){
+        if(out->len != write(mntfd, out->buf, out->len)) goto ext;
+        write(mntfd, "\r", 1); // add EOL
+    }
+    if(in){
+        in->len = 0;
+        for(size_t i = 0; i < in->maxlen; ++i){
+            int b = getmntbyte();
+            if(b < 0) break; // nothing to read -> go out
+            in->buf[in->len++] = (uint8_t) b;
+        }
+    }
+    ret = TRUE;
+ext:
+    pthread_mutex_unlock(&mntmutex);
+    return ret;
+}

LibSidServo/serial.h

@@ -34,3 +34,4 @@ int openEncoder(const char *path, int speed);
 int openMount(const char *path, int speed);
 void closeSerial();
 mcc_errcodes_t getMD(mountdata_t  *d);
+int MountWriteRead(const data_t *out, data_t *in);

LibSidServo/sidservo.h

@@ -18,6 +18,8 @@
 
 #pragma once
 
+#include <stddef.h>
+#include <stdint.h>
 #include <sys/time.h>
 
 // error codes
@@ -30,11 +32,13 @@ typedef enum{
 } mcc_errcodes_t;
 
 typedef struct{
-    char*   MountPath;      // path to mount device
+    char*   MountDevPath;       // path to mount device
-    int     MountSpeed;     // serial speed
+    int     MountDevSpeed;      // serial speed
-    char*   EncoderPath;    // path to encoder device
+    char*   EncoderDevPath;     // path to encoder device
-    int     EncoderSpeed;   // serial speed
+    int     EncoderDevSpeed;    // serial speed
-    int     SepEncoder;     // ==1 if encoder works as separate serial device
+    int     SepEncoder;         // ==1 if encoder works as separate serial device
+    suseconds_t MountReqInterval;// interval between subsequent mount requests (microseconds)
+    ;
 } conf_t;
 
 // coordinates in degrees: X, Y and time when they were reached
@@ -42,9 +46,31 @@ typedef struct{
     double X; double Y; struct timeval msrtime;
 } coords_t;
 
+// data to read/write
 typedef struct{
-    coords_t position;
+    uint8_t *buf;   // data buffer
-    coords_t speed;
+    size_t len;     // its length
+    size_t maxlen;  // maximal buffer size
+} data_t;
+
+typedef struct{
+    uint8_t XBits;
+    uint8_t YBits;
+    uint8_t ExtraBits;
+    uint16_t ain0;
+    uint16_t ain1;
+} extradata_t;
+
+typedef struct{
+    coords_t motposition;
+    coords_t encposition;
+    coords_t lastmotposition;
+    coords_t motspeed;
+    uint8_t keypad;
+    extradata_t extradata;
+    uint32_t millis;
+    double temperature;
+    double voltage;
 } mountdata_t;
 
 // mount class

LibSidServo/ssii.c

@@ -0,0 +1,70 @@
+/*
+ * This file is part of the libsidservo project.
+ * Copyright 2025 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 "serial.h"
+#include "ssii.h"
+
+uint16_t SScalcChecksum(uint8_t *buf, int len){
+    uint16_t checksum = 0;
+    for(int i = 0; i < len; i++)
+        checksum += *buf++;
+    checksum ^= 0xFF00; // invert high byte
+    //DBG("Checksum of %d bytes: 0x%04x", len, checksum);
+    return checksum;
+}
+
+// send short/long binary command
+static int bincmd(uint8_t *cmd, int len){
+    data_t d;
+    if(len == sizeof(SSscmd)){
+        ((SSscmd*)cmd)->checksum = SScalcChecksum(cmd, len-2);
+        DBG("Short command");
+        d.buf = CMD_SHORTCMD;
+        d.len = sizeof(CMD_SHORTCMD) - 1;
+        if(!MountWriteRead(&d, NULL)) return -1;
+    }else if(len == sizeof(SSlcmd)){
+        ((SSlcmd*)cmd)->checksum = SScalcChecksum(cmd, len-2);
+        DBG("Long command");
+        d.buf = CMD_LONGCMD;
+        d.len = sizeof(CMD_LONGCMD) - 1;
+        if(!MountWriteRead(&d, NULL)) return -1;
+    }else{
+        return -1;
+    }
+    DBG("Write %d bytes and wait for ans", len);
+    d.buf = cmd;
+    d.len = d.maxlen = len;
+    return MountWriteRead(&d, &d);
+}
+// return TRUE if OK
+int SScmdS(SSscmd *cmd){
+    return bincmd(cmd, sizeof(SSscmd));
+}
+int SScmdL(SSlcmd *cmd){
+    return bincmd(cmd, sizeof(SSlcmd));
+}
+
+
+/**
+ * @brief SSconvstat - convert stat from SSII format to human
+ * @param status (i) - just read data
+ * @param mountdata (o) - output
+ */
+void SSconvstat(const SSstat *status, mountdata_t *mountdata){
+    ;
+}

LibSidServo/ssii.h

@@ -20,6 +20,8 @@
 
 #include <stdint.h>
 
+#include "sidservo.h"
+
 // ASCII commands
 #define U8P(x)   ((uint8_t*)x)
 // get binary data of all statistics
@@ -105,3 +107,7 @@ typedef struct{
     uint16_t checksum;  // 32
 } __attribute__((packed)) SSlcmd; // long command
 
+int SScmdS(SSscmd *cmd);
+int SScmdL(SSlcmd *cmd);
+uint16_t SScalcChecksum(uint8_t *buf, int len);
+void SSconvstat(const SSstat *status, mountdata_t *mountdata);