0

LibSidServo/serial.c

@@ -0,0 +1,140 @@
+/*
+ * 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 <pthread.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <string.h>
+#include <usefull_macros.h>
+
+#include "serial.h"
+#include "sidservo.h"
+
+// serial devices
+static sl_tty_t *EncDev = NULL, *MntDev = NULL;
+// time of last EncData started
+static double tgot = 0.;
+// last Enc values
+static uint32_t encX = 0, encY = 0;
+
+static pthread_mutex_t mntmutex = PTHREAD_MUTEX_INITIALIZER;
+static pthread_t encthread;
+
+typedef struct __attribute__((packed)){
+    uint8_t magick;
+    uint32_t encX;
+    uint32_t encY;
+    uint8_t CRC[4];
+} enc_t;
+
+
+
+static void parce_encbuf(uint8_t databuf[ENC_DATALEN], double nexttime){
+    enc_t *edata = (enc_t*) databuf;
+    if(edata->magick != ENC_MAGICK){
+        DBG("No magick");
+        return;
+    }
+    if(edata->CRC[3]){
+        DBG("No 0 @ end: 0x%02x", edata->CRC[3]);
+        return;
+    }
+    uint32_t POS_SUM = 0;
+    for(int i = 1; i < 9; ++i) POS_SUM += databuf[i];
+    uint8_t x = POS_SUM >> 8;
+    if(edata->CRC[0] != x){
+        DBG("CRC[0] = 0x%02x, need 0x%02x", edata->CRC[0], x);
+        return;
+    }
+    uint8_t y = ((0xFFFF - POS_SUM) & 0xFF) - x;
+    if(edata->CRC[1] != y){
+        DBG("CRC[1] = 0x%02x, need 0x%02x", edata->CRC[1], y);
+        return;
+    }
+    y = (0xFFFF - POS_SUM) >> 8;
+    if(edata->CRC[2] != y){
+        DBG("CRC[2] = 0x%02x, need 0x%02x", edata->CRC[2], y);
+        return;
+    }
+    encX = edata->encX;
+    encY = edata->encY;
+    tgot = nexttime;
+}
+
+static void *encoderthread(void _U_ *u){
+    uint8_t databuf[ENC_DATALEN];
+    int wridx = 0;
+    double nexttime = 0.;
+    void add2buf(){
+        size_t len = ENC_DATALEN - wridx;
+        if(EncDev->buflen < len) len = EncDev->buflen;
+        memcpy(databuf+wridx, EncDev->buf, len);
+        wridx += len;
+    }
+    while(EncDev){
+        if(sl_tty_read(EncDev)){
+            DBG("Got %zd bytes from Encoder", EncDev->buflen);
+            if(EncDev->buflen <= ENC_DATALEN){
+                if(wridx == 0){
+                    if((uint8_t)EncDev->buf[0] == ENC_MAGICK){
+                        add2buf();
+                        nexttime = sl_dtime();
+                    }
+                }else add2buf();
+                if(wridx == ENC_DATALEN){
+                    parce_encbuf(databuf, nexttime);
+                    wridx = 0;
+                }
+            }
+        }
+    }
+    return NULL;
+}
+
+int openEncoder(const char *path, int speed){
+    EncDev = sl_tty_new((char*)path, speed, 256);
+    if(EncDev) EncDev = sl_tty_open(EncDev, 1);
+    if(!EncDev) return FALSE;
+    sl_tty_tmout(5.); // 5us timeout
+    if(pthread_create(&encthread, NULL, encoderthread, NULL)) return FALSE;
+    DBG("Encoder opened");
+    return TRUE;
+}
+
+int openMount(const char *path, int speed){
+    MntDev = sl_tty_new((char*)path, speed, 256);
+    if(MntDev) MntDev = sl_tty_open(MntDev, 1);
+    if(!MntDev) return FALSE;
+    DBG("Mount opened");
+    return TRUE;
+}
+
+void closeSerial(){
+    if(MntDev){
+        DBG("Close mount");
+        pthread_mutex_lock(&mntmutex);
+        sl_tty_close(&MntDev);
+        pthread_mutex_unlock(&mntmutex);
+    }
+    if(EncDev){
+        DBG("Close encoder");
+        pthread_cancel(encthread);
+        pthread_join(encthread, NULL);
+        sl_tty_close(&EncDev);
+    }
+}