From 01a8f232f7d6e6e0e7dc2fb4348d72cbf0f21308 Mon Sep 17 00:00:00 2001 From: Edward Emelianov Date: Thu, 19 Nov 2026 11:59:58 +0300 Subject: [PATCH] more fixes --- BTA_dome_modbus/Makefile | 2 +- BTA_dome_modbus/Readme.md | 4 ++-- BTA_dome_modbus/client.c | 28 ++++++++++++++++++++-------- BTA_dome_modbus/cmdlnopts.h | 1 - BTA_dome_modbus/esq770.h | 3 +++ BTA_dome_modbus/io.c | 2 +- BTA_dome_modbus/motors.c | 28 +++++++++++++++++----------- BTA_dome_modbus/motors.h | 10 +++++----- BTA_dome_modbus/server.c | 23 ++++++++++++++++------- BTA_dome_modbus/sslsock.c | 24 +++++++++++++++++------- 10 files changed, 82 insertions(+), 43 deletions(-) diff --git a/BTA_dome_modbus/Makefile b/BTA_dome_modbus/Makefile index 36c2e35..819de3a 100644 --- a/BTA_dome_modbus/Makefile +++ b/BTA_dome_modbus/Makefile @@ -33,7 +33,7 @@ release: LDFLAGS += -flto release: TARGET := RELEASE release: $(TARGFILE) $(CLIENT) $(SERVER) -debug: CFLAGS += -DEBUG -Werror +debug: CFLAGS += -DEBUG -Werror -g debug: TARGET := DEBUG debug: $(TARGFILE) $(CLIENT) $(SERVER) diff --git a/BTA_dome_modbus/Readme.md b/BTA_dome_modbus/Readme.md index c6f9bbf..c6ba81b 100644 --- a/BTA_dome_modbus/Readme.md +++ b/BTA_dome_modbus/Readme.md @@ -125,7 +125,7 @@ treated as “nothing yet”. ## 6. Protocol -Text, one message per line, `\n` terminator. +Text, one message per line, `\n` terminator. It is unacceptable to end a message with `\r\n`. ### 6.1 Message forms @@ -449,7 +449,7 @@ the option tables are compiled conditionally. | `SPEED_TOLERANCE` | 0.01 | rev/min | | `MAX_CURRENT` / `DEFAULT_CURRENT` | 15 / 10 | A | | `LSpeed` / `MSpeed` / `HSpeed` | 71 / 350 / 610 | rev/min presets | -| `MODBUS_RESPONCE_TIMEOUT` | 100000 | µs | +| `MODBUS_RESPONSE_TIMEOUT` | 100000 | µs | | `EMUL_ACCEL` | 10000 | min⁻² | | `BACKLOG` | 10 | max simultaneous SSL clients | | `IOBUF_LEN` / `SL_VAL_LEN` / `SL_KEY_LEN` | 256 / (external) | protocol buffer sizes | diff --git a/BTA_dome_modbus/client.c b/BTA_dome_modbus/client.c index 7df10a3..d85d155 100644 --- a/BTA_dome_modbus/client.c +++ b/BTA_dome_modbus/client.c @@ -177,6 +177,7 @@ static void stop_all(SSL *ssl){ }else{ Dome_Speed = D_Off; set_speed = 0.; + speedSEWD = 0.; } } @@ -211,15 +212,19 @@ static int check_motor(SSL *ssl, int motno){ if(motno != MOTORS_AMOUNT - 1) return ret; int status = 0, N = 0; // mean status is largest double speed = 0., current = 0.; + static int warnsent[MOTORS_AMOUNT] = {0}; // flag that error warning for this motor was sent for(int i = 0; i < MOTORS_AMOUNT; ++i){ int s = MotorState[i].status; if(s == MOT_OFF) continue; if(s == MOT_ERROR){ - *msg = MesgWarn; - sprintf(msg+1, "Dome: Error in motor %d!\n", i+1); - SendMessage(msg); + if(!warnsent[i]){ + *msg = MesgWarn; + sprintf(msg+1, "Dome: Error in motor %d!\n", i+1); + SendMessage(msg); + warnsent[i] = 1; + } s = MotorState[i].status = MOT_OFF; - } + }else warnsent[i] = 0; if(status < s) status = s; if(s == MOT_RUN){ speed += MotorState[i].speed; @@ -260,7 +265,7 @@ static void chk_dome_speed(SSL *ssl){ if(sl_dtime() - tlast < G.speedchk_interval) return; DBG("Time - tlast = %g", sl_dtime() - tlast); tlast = sl_dtime(); - if(fabs(CommonState.speed - fabs(set_speed)) < SPEED_TOLERANCE){ + if(fabs(CommonState.speed - set_speed) < SPEED_TOLERANCE){ return; } DBG("Speed set command still not sent"); @@ -305,7 +310,11 @@ static void chk_dome_speed(SSL *ssl){ // SHM parser; return FALSE if SHM is in erroreous state static int process_system(SSL *ssl){ - static int curMotNo = 0; // current motor number (we'll scan all 10 motors by one) + static int curMotNo = -1; // current motor number (we'll scan all 10 motors by one) + if(curMotNo == -1){ // pass first run, wait for M_time initialized + curMotNo = 0; + return TRUE; + } double curtime = sl_dtime(); // check time sync static int brokenshmctr = 0; @@ -429,6 +438,7 @@ void clientproc(SSL_CTX *ctx, int fd){ LOGDBG("Received: \"%s\"", buf); }else if(bytes < 0){ LOGERR("Server disconnected or other error"); + break; } usleep(100000); } @@ -454,8 +464,9 @@ void terminal(SSL_CTX *ctx, int fd){ if(sl_canread(0)){ ssize_t L = getline(&lptr, &N, stdin); if(L < 0) break; // ctrl + D - if(L > (ssize_t)(sizeof(buf)-1)) WARNX("String too long!"); - else if(L > 0){ + //if(L > (ssize_t)(sizeof(buf)-1)) WARNX("String too long!"); + //else + if(L > 0){ if(!write_string(ssl, lptr)) break; // error } } @@ -473,5 +484,6 @@ void terminal(SSL_CTX *ctx, int fd){ } } FREE(lptr); + SSL_shutdown(ssl); SSL_free(ssl); } diff --git a/BTA_dome_modbus/cmdlnopts.h b/BTA_dome_modbus/cmdlnopts.h index c4e9d54..f5a086d 100644 --- a/BTA_dome_modbus/cmdlnopts.h +++ b/BTA_dome_modbus/cmdlnopts.h @@ -43,7 +43,6 @@ typedef struct{ char *port; // port number int verbose; // logfile verbose level char *ca; // ca - char *conffile; // configuration file #ifdef SERVER int serialspeed; // speed of serial device char *serialpath; // path to RS-485 device diff --git a/BTA_dome_modbus/esq770.h b/BTA_dome_modbus/esq770.h index f8f090e..645ce38 100644 --- a/BTA_dome_modbus/esq770.h +++ b/BTA_dome_modbus/esq770.h @@ -18,6 +18,9 @@ #pragma once +#include +#include + // Registers and their fields for ESQ-770 frequency converter // command register (rw) diff --git a/BTA_dome_modbus/io.c b/BTA_dome_modbus/io.c index 7a8b8ca..0593edb 100644 --- a/BTA_dome_modbus/io.c +++ b/BTA_dome_modbus/io.c @@ -210,7 +210,7 @@ void io_process(){ } #ifdef EBUG for(int i = 0; i < NADCCHNLS; ++i){ - if(abs(adcbuf[i] - ADCBuf[i]) < ADCTHRESH) continue; + if(abs((int)adcbuf[i] - (int)ADCBuf[i]) < ADCTHRESH) continue; // GotNewADC = TRUE; break; printf("ADC input %d changed to %d\n", i, adcbuf[i]); diff --git a/BTA_dome_modbus/motors.c b/BTA_dome_modbus/motors.c index 0937281..26de3bc 100644 --- a/BTA_dome_modbus/motors.c +++ b/BTA_dome_modbus/motors.c @@ -104,7 +104,7 @@ static int motors_open_m(const char *path, int speed){ LOGERR("Can't open device %s @ %d", path, speed); return FALSE; } - modbus_set_response_timeout(modbus_ctx, 0, MODBUS_RESPONCE_TIMEOUT); // response timeout + modbus_set_response_timeout(modbus_ctx, 0, MODBUS_RESPONSE_TIMEOUT); // response timeout if(modbus_connect(modbus_ctx) < 0){ WARNX("Can't connect to device %s", path); LOGERR("Can't connect to device %s", path); @@ -147,12 +147,14 @@ int motors_set_curntsetpoint(double val){ // get setpoint of speed double motors_get_speedsetpoint(){ - return speedSet; + pthread_mutex_lock(&statemutex); + double spd = speedSet; + pthread_mutex_unlock(&statemutex); + return spd; } // set setpoint of speed (rev/min) sl_sock_hresult_e motors_set_speedsetpoint(double val){ - double absval = fabs(val); - if(absval > MAX_SPEED){ + if(fabs(val) > MAX_SPEED){ DBG("Speed to large"); return RESULT_BADVAL; } @@ -258,10 +260,10 @@ static void motors_process_m(){ if(fabs(speedSet) < SPEED_TOLERANCE){ // stop motor modbus_write_register(modbus_ctx, REG_CMD, CMD_STOP); }else{ // send command "set speed" - uint16_t dir = (speedSet > 0.) ? CMD_FORWARD : CMD_REVERSE; uint16_t freq = REVMIN2FREQ(fabs(speedSet)); - modbus_write_register(modbus_ctx, REG_CMD, dir); modbus_write_register(modbus_ctx, REG_FREQ_SET, freq); + uint16_t dir = (speedSet > 0.) ? CMD_FORWARD : CMD_REVERSE; + modbus_write_register(modbus_ctx, REG_CMD, dir); } flags.change_speed = 0; } @@ -293,9 +295,12 @@ static void motors_process_m(){ } DBG("motor %d; status: %x, extstatus: %x", curN, regs[0], regs[1]); error_cnt[curN] = 0; + double rotdir = 1.; switch(regs[0]){ - case STATUS_CW: case STATUS_CCW: + rotdir = -1.; + // fallthrough + case STATUS_CW: DBG("status: RUN"); motstates[curN].status = MOT_RUN; break; @@ -317,7 +322,7 @@ static void motors_process_m(){ } 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 = FREQ2REVMIN(regs[0]); + motstates[curN].speed = rotdir * FREQ2REVMIN(regs[0]); motstates[curN].current = ((double)regs[1]) / CURRENT_SCALE; // Logging log_data(curN); @@ -346,6 +351,7 @@ static void motors_process_e(){ } flags.all = 0; } + // both curspeed and speedSet are signed if(fabs(curspeed - speedSet) > SPEED_TOLERANCE){ // need to calculate new speed value double acceleration = (curspeed < speedSet) ? EMUL_ACCEL : -EMUL_ACCEL; double newspeed = curspeed + acceleration * dt / 60.; // acceleration in min^{-2} @@ -364,10 +370,10 @@ static void motors_process_e(){ } t0 = curt; for(int i = 0; i < MOTORS_AMOUNT; ++i){ - log_data(i); motstates[i].status = curstatus; motstates[i].current = curcurrent; motstates[i].speed = curspeed; + log_data(i); } pthread_mutex_unlock(&statemutex); } @@ -409,12 +415,12 @@ int start_log(){ for(int i = 0; i < MOTORS_AMOUNT; ++i){ if(logF[i]) fclose(logF[i]); snprintf(buf, PATH_MAX, "%s/%s_%02d.log", G.motlogdir, logprefix, i); - logF[i] = fopen(buf, "a"); + logF[i] = fopen(buf, "w"); if(!logF[i]) ++errctr; } pthread_mutex_unlock(&logmutex); - Tstart = sl_dtime(); if(errctr == MOTORS_AMOUNT) return FALSE; + Tstart = sl_dtime(); return TRUE; } diff --git a/BTA_dome_modbus/motors.h b/BTA_dome_modbus/motors.h index 6297877..89f2cde 100644 --- a/BTA_dome_modbus/motors.h +++ b/BTA_dome_modbus/motors.h @@ -43,8 +43,8 @@ // log-file prefix length #define MAX_LOGPREFIX_LEN 31 -// modbus responce timeout, ms -#define MODBUS_RESPONCE_TIMEOUT 100000 +// modbus response timeout, mks +#define MODBUS_RESPONSE_TIMEOUT 100000 // ID of first motor #define START_ID (1) @@ -60,9 +60,9 @@ enum{ }; typedef struct{ - int status; - double speed; - double current; + int status; // one of MOT_xxx statuses + double speed; // current speed with sign (RPM) + double current; // current current (A) } motor_state_t; double motors_get_curntsetpoint(); diff --git a/BTA_dome_modbus/server.c b/BTA_dome_modbus/server.c index 59ece81..0195e52 100644 --- a/BTA_dome_modbus/server.c +++ b/BTA_dome_modbus/server.c @@ -72,7 +72,7 @@ static int handle_connection(SSL *ssl){ int r = read_string(ssl, buf, IOBUF_LEN); if(r < 0) return 0; else if(r == 0) return 1; // nothing to read or buffer overflow - LOGDBG("fd=%d, message=%s", SSL_get_fd(ssl), buf); + LOGDBG("fd=%d, message: '%s'", SSL_get_fd(ssl), buf); int got = sl_get_keyval(buf, key, val); if(got == 0){ DBG("Comment or empty string"); @@ -93,7 +93,7 @@ static int handle_connection(SSL *ssl){ if(0 == strcmp(key, "help")){ for(size_t i = 0; i < HANDLERS_AMOUNT; ++i){ snprintf(buf, IOBUF_LEN-1, "%s: %s\n", command_list[i].command, command_list[i].helpstring); - write_string(ssl, buf); + if(!write_string(ssl, buf)) return 0; } return 1; } @@ -101,7 +101,7 @@ static int handle_connection(SSL *ssl){ // now `val` is an answer or error code if(result != RESULT_SILENCE) snprintf(buf, IOBUF_LEN-1, "%s\n", sl_sock_hresult2str(result)); else snprintf(buf, IOBUF_LEN-1, "%s=%s\n", key, val); - write_string(ssl, buf); + if(!write_string(ssl, buf)) return 0; return 1; } @@ -117,12 +117,14 @@ static int timeouted_sslaccept(SSL *ssl){ if(x < 0){ int sslerr = SSL_get_error(ssl, x); if(SSL_ERROR_WANT_READ == sslerr || - SSL_ERROR_WANT_WRITE == sslerr) continue; + SSL_ERROR_WANT_WRITE == sslerr){ + usleep(10000); + continue; + } DBG("SSL error %d", sslerr); return FALSE; }else if(x == 1) return TRUE; - // x == 0 -> unclosed connection - usleep(10000); + else return FALSE; // x == 0 -> unclosed connection } DBG("Timeout"); return FALSE; @@ -197,6 +199,12 @@ void serverproc(SSL_CTX *ctx, int fd){ } } } + if(nfd == 1){ // no clients -> stop dome + if(fabs(motors_get_speedsetpoint()) > SPEED_TOLERANCE){ // stop motors if speed setpoint > 0 when nobody connected + LOGERR("Stop motors: no clients connected!"); + motors_stop(); + } + } // scan connections for(int fdidx = 1; fdidx < nfd; ++fdidx){ if((poll_set[fdidx].revents & POLLIN) == 0) continue; @@ -219,7 +227,8 @@ void serverproc(SSL_CTX *ctx, int fd){ SSL_shutdown(ssls[i]); SSL_free(ssls[i]); } - motors_stop(); + motors_stop(); // set stop flag + motors_process(); // send stop command motors_close(); } diff --git a/BTA_dome_modbus/sslsock.c b/BTA_dome_modbus/sslsock.c index 9de9557..1ab2898 100644 --- a/BTA_dome_modbus/sslsock.c +++ b/BTA_dome_modbus/sslsock.c @@ -181,6 +181,8 @@ static int geterrcode(SSL *ssl, int errcode){ DBG("ERR: %d", sslerr); if(SSL_ERROR_WANT_READ == sslerr || SSL_ERROR_WANT_WRITE == sslerr) return 0; // empty call + // don't log "zero return" when client disconnected + if(sslerr == SSL_ERROR_ZERO_RETURN) return -1; int sd = SSL_get_fd(ssl); LOGERR("SSL error %d @client %d", sslerr, sd); WARNX("SSL error %d @client %d", sslerr, sd); @@ -209,13 +211,20 @@ int read_string(SSL *ssl, char *buf, int l){ return 0; } l = eol - buf + 1; - bytes = SSL_read(ssl, buf, l); - //DBG("Read: %d", bytes); - if(bytes < 1){ // error - return geterrcode(ssl, bytes); + char *ptr = buf; + int total = 0; + while(l){ + bytes = SSL_read(ssl, ptr, l); + //DBG("Read: %d", bytes); + if(bytes <= 0){ // error + return geterrcode(ssl, bytes); + } + l -= bytes; + ptr += bytes; + total += bytes; } - buf[bytes-1] = 0; // replace '\n' with 0 - return bytes; + ptr[-1] = 0; // replace '\n' with 0 + return total; } /** @@ -227,11 +236,12 @@ int read_string(SSL *ssl, char *buf, int l){ int write_string(SSL *ssl, char *buf){ double t0 = sl_dtime(); int l = strlen(buf); - while(sl_dtime() - t0 < 1. && l > 0){ + while(sl_dtime() - t0 < G.acc_timeout && l > 0){ int written = SSL_write(ssl, buf, l); if(written <= 0){ int err = SSL_get_error(ssl, written); if(err == SSL_ERROR_WANT_READ || err == SSL_ERROR_WANT_WRITE){ // try next time + usleep(10000); continue; } return FALSE; // error