diff --git a/BTA_dome_modbus/Makefile b/BTA_dome_modbus/Makefile index ea5997a..b2f3984 100644 --- a/BTA_dome_modbus/Makefile +++ b/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) diff --git a/BTA_dome_modbus/esq770.h b/BTA_dome_modbus/esq770.h new file mode 100644 index 0000000..9948b17 --- /dev/null +++ b/BTA_dome_modbus/esq770.h @@ -0,0 +1,57 @@ +/* + * This file is part of the sslsosk project. + * Copyright 2026 Edward V. Emelianov . + * + * 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 . + */ + +#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. + diff --git a/BTA_dome_modbus/motors.c b/BTA_dome_modbus/motors.c index 793338a..bcf933f 100644 --- a/BTA_dome_modbus/motors.c +++ b/BTA_dome_modbus/motors.c @@ -17,9 +17,11 @@ */ #include +#include #include #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, ®s[0])) regs[0] = 0; + if(-1 == modbus_read_registers(modbus_ctx, REG_OUTPUT_CURRENT, 1, ®s[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; } diff --git a/BTA_dome_modbus/motors.h b/BTA_dome_modbus/motors.h index c5bd93d..55210cf 100644 --- a/BTA_dome_modbus/motors.h +++ b/BTA_dome_modbus/motors.h @@ -18,17 +18,21 @@ #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 +#define LSpeed 71 +#define MSpeed 350 +#define HSpeed 610 + // ID of first motor minus 1 #define START_ID (0) @@ -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(); diff --git a/BTA_dome_modbus/server.c b/BTA_dome_modbus/server.c index a9d1c1a..8219633 100644 --- a/BTA_dome_modbus/server.c +++ b/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) ******************/ diff --git a/BTA_dome_modbus/sslsock.c b/BTA_dome_modbus/sslsock.c index e63cc28..f2e2db6 100644 --- a/BTA_dome_modbus/sslsock.c +++ b/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;