add modbus support, need to test

BTA_dome_modbus/Makefile

@@ -47,6 +47,7 @@ $(CLIENT) : $(COBJDIR) $(COBJS)
 
 
 $(SERVER) : DEFINES += -DSERVER
+$(SERVER) : LDFLAGS += -lmodbus
 $(SERVER) : $(SOBJDIR) $(SOBJS)
 	@echo -e "\tLD $(SERVER)"
 	$(CC) $(LDFLAGS) $(SOBJS) -o $(SERVER)

BTA_dome_modbus/esq770.h

@@ -0,0 +1,57 @@
+/*
+ * This file is part of the sslsosk 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
+
+// Registers and their fields for ESQ-770 frequency converter
+
+// command register (rw)
+#define REG_CMD                 0x2000  // 8192
+// set frequency (rw)
+#define REG_FREQ_SET            0x3000  // 12288
+// torque limit (rw)
+#define REG_TORQUE_LIMIT        0x3006  // 12294
+#define REG_BRAKE_TORQUE_LIMIT  0x3007  // 12295
+// status (r)
+#define REG_STATUS_MAIN         0x6000  // 24576
+#define REG_STATUS_EXT          0x6001  // 24577
+// monitoring (r)
+#define REG_OUTPUT_FREQ         0xA201  // 41473 (freq * 100)
+#define REG_OUTPUT_CURRENT      0xA204  // 41476 (current * 10)
+#define REG_MOTOR_SPEED         0xA205  // 41477 (speed, depends on settings)
+
+// REG_CMD bits
+#define CMD_FORWARD             1
+#define CMD_REVERSE             2
+#define CMD_STOP                5
+#define CMD_FAST_STOP           6
+#define CMD_RESET_FAULT         7
+
+// REG_STATUS_MAIN bits
+#define STATUS_CW               1
+#define STATUS_CCW              2
+#define STATUS_STOP             3
+#define STATUS_ERR              4
+#define STATUS_LOW_V            5
+
+// REG_STATUS_EXT bit
+#define STATUS_READY_MASK       0x01
+
+#define FREQ_SCALE              100.
+#define CURRENT_SCALE           10.
+

BTA_dome_modbus/motors.c

@@ -17,9 +17,11 @@
  */
 
 #include <math.h>
+#include <modbus/modbus.h>
 #include <usefull_macros.h>
 
 #include "motors.h"
+#include "esq770.h"
 
 #if 0
 ðÒÏÔÏËÏÌ (s - ÓÅÔÔÅÒ, g - getteer):
@@ -34,6 +36,7 @@ speed=xx - (sg)
 current=xx - (sg) ÕÓÔÁ×ËÁ ÔÏËÁ
 #endif
 
+static modbus_t *modbus_ctx = NULL;
 static motor_state_t motstates[MOTORS_AMOUNT] = {0};
 
 // set points
@@ -57,20 +60,42 @@ static union{
 // current motor for status etc. getters (0..MOTORS_AMOUNT-1)
 static int motindex = 0;
 
-// open modbus @ given speed; return FALSE if failed
-static int modbus_open_m(const char _U_ *path, int _U_ speed){
-    return FALSE;
+// close modbus connection
+static void motors_close_m(){
+    if(modbus_ctx){
+        modbus_close(modbus_ctx);
+        modbus_free(modbus_ctx);
+        modbus_ctx = NULL;
+    }
 }
-static int modbus_open_e(const char _U_ *path, int _U_ speed){
-    return TRUE;
+static void motors_close_e(){ // stub for emulation mode
+    ;
 }
 
-// close modbus connection
-static void modbus_close_m(){
-    ;
+// open modbus @ given speed; return FALSE if failed
+static int motors_open_m(const char *path, int speed){
+    if(speed < 1200 || !path){
+        WARNX("Point path and right speed");
+        return FALSE;
+    }
+    if(modbus_ctx) motors_close_m();
+    modbus_ctx = modbus_new_rtu(path, speed, 'N', 8, 1);
+    if(!modbus_ctx){
+        WARNX("Can't open device %s @ %d", path, speed);
+        LOGERR("Can't open device %s @ %d", path, speed);
+        return FALSE;
+    }
+    modbus_set_response_timeout(modbus_ctx, 0, 50000); // 50ms response timeout
+    if(modbus_connect(modbus_ctx) < 0){
+        WARNX("Can't connect to device %s", path);
+        LOGERR("Can't connect to device %s", path);
+        motors_close_m();
+        return FALSE;
+    }
+    return TRUE;
 }
-static void modbus_close_e(){
-    ;
+static int motors_open_e(const char _U_ *path, int _U_ speed){ // stub for emulation mode
+    return TRUE;
 }
 
 
@@ -136,7 +161,66 @@ int motors_get_actstatus(int *val){
 
 // main motors processing routine
 static void motors_process_m(){
-    ;
+    static int curN = 0, errctr = 0;
+    static int error_cnt[MOTORS_AMOUNT] = {0};
+    if(!modbus_ctx || errctr > 100){
+        LOGERR("Modbus not inited or other error!");
+        ERRX("Modbus not inited or other error!");
+    }
+    if(flags.all){
+        if(-1 == modbus_set_slave(modbus_ctx, 0)) goto reg_error;
+        if(flags.stop){
+            speedSet = 0.;
+            // send command stop
+            if(-1 == modbus_write_register(modbus_ctx, REG_CMD, CMD_STOP)) goto reg_error;
+        }
+        if(flags.change_current){
+            // TODO: send command "max current"?
+        }
+        if(flags.change_speed){
+            // send command "set speed"
+            uint16_t dir = (speedSet > 0.) ? CMD_FORWARD : CMD_REVERSE;
+            uint16_t freq = (uint16_t)(fabs(speedSet) * FREQ_SCALE);
+            if(-1 == modbus_write_register(modbus_ctx, REG_FREQ_SET, freq)) goto reg_error;
+            if(-1 == modbus_write_register(modbus_ctx, REG_CMD, dir)) goto reg_error;
+        }
+        flags.all = 0;
+    }
+    // set slave N
+    if(-1 == modbus_set_slave(modbus_ctx, curN)) goto reg_error;
+    // ask for speed/status/current
+    uint16_t regs[2];
+    if(-1 == modbus_read_registers(modbus_ctx, REG_STATUS_MAIN, 2, regs)){
+        if((++error_cnt[curN]) > MAX_ERRORS){ // not answer - set MOT_OFF status
+            motstates[curN].status = MOT_OFF;
+            LOGDBG("Motor %d not responce", curN);
+        }
+        if(++curN >= MOTORS_AMOUNT) curN = 0;
+        return;
+    }
+    error_cnt[curN] = 0;
+    switch(regs[0]){
+        case STATUS_CW:
+        case STATUS_CCW:
+            motstates[curN].status = MOT_RUN;
+            break;
+        case STATUS_STOP:
+            motstates[curN].status = MOT_SLEEP;
+            break;
+        default:
+            motstates[curN].status = MOT_ERROR;
+    }
+    if(regs[1] & STATUS_READY_MASK) motstates[curN].status = MOT_ERROR;
+    if(motstates[curN].status == MOT_ERROR) modbus_write_register(modbus_ctx, REG_CMD, CMD_RESET_FAULT);
+    if(-1 == modbus_read_registers(modbus_ctx, REG_OUTPUT_FREQ, 1, &regs[0])) regs[0] = 0;
+    if(-1 == modbus_read_registers(modbus_ctx, REG_OUTPUT_CURRENT, 1, &regs[1])) regs[1] = 0;
+    motstates[curN].speed = ((double)regs[0]) / FREQ_SCALE;
+    motstates[curN].current = ((double)regs[1]) / CURRENT_SCALE;
+    if(++curN >= MOTORS_AMOUNT) curN = 0;
+    errctr = 0;
+    return;
+reg_error:
+    ++errctr;
 }
 static void motors_process_e(){
     static double t0 = -1., curspeed = 0.;
@@ -179,13 +263,13 @@ static void motors_process_e(){
     }
 }
 
-int (*modbus_open)(const char *, int) = modbus_open_m;
-void (*modbus_close)() = modbus_close_m;
+int (*motors_open)(const char *, int) = motors_open_m;
+void (*motors_close)() = motors_close_m;
 void (*motors_process)() = motors_process_m;
 
 void set_emulation_mode(){
     LOGMSG("Set emulation mode");
-    modbus_open =  modbus_open_e;
-    modbus_close = modbus_close_e;
+    motors_open =  motors_open_e;
+    motors_close = motors_close_e;
     motors_process = motors_process_e;
 }

BTA_dome_modbus/motors.h

@@ -18,18 +18,22 @@
 
 #pragma once
 
-#define MAX_SPEED       (700.)
-#define SPEED_TOLERANCE (0.01)
+// max errors per motor to mean it OFF
+#define MAX_ERRORS      5
+
+#define MAX_SPEED       700.
+#define SPEED_TOLERANCE 0.01
 // emulation acceleration, min^-2
-#define EMUL_ACCEL      (20000.)
-#define MAX_CURRENT     (15.)
-#define DEFAULT_CURRENT (10.)
-#define MOTORS_AMOUNT   (10)
+#define EMUL_ACCEL      20000.
+#define MAX_CURRENT     15.
+#define DEFAULT_CURRENT 10.
+#define MOTORS_AMOUNT   10
 // low, medium and high speeds
 #define LSpeed          71
 #define MSpeed          350
 #define HSpeed          610
 
+
 // ID of first motor minus 1
 #define START_ID        (0)
 // ID of motor (n=1..MOTORS_AMOUNT)
@@ -65,7 +69,7 @@ int motors_get_actstatus(int*);
 
 extern void (*motors_process)();
 
-extern int (*modbus_open)(const char *, int);
-extern void (*modbus_close)();
+extern int (*motors_open)(const char *, int);
+extern void (*motors_close)();
 
 void set_emulation_mode();

BTA_dome_modbus/server.c

@@ -208,7 +208,7 @@ void serverproc(SSL_CTX *ctx, int fd){
     }
     for(int i = 0; i < nfd; ++i) SSL_free(ssls[i]);
     motors_stop();
-    modbus_close();
+    motors_close();
 }
 
 /****************** Protocol handlers (return 0 in case of success or error code >0 if failed) ******************/

BTA_dome_modbus/sslsock.c

@@ -139,7 +139,7 @@ int open_socket(){
     int fd = OpenConn(atoi(G.port));
 #ifdef SERVER
     if(G.emulmode) set_emulation_mode();
-    if(!modbus_open(G.serialpath, G.serialspeed)){
+    if(!motors_open(G.serialpath, G.serialspeed)){
         LOGERR("Can't open %s @%d", G.serialpath, G.serialspeed);
         WARNX("Can't open %s @%d", G.serialpath, G.serialspeed);
         return 1;