From 985a179b872fbde291fdefd517ef9c6beade76d3 Mon Sep 17 00:00:00 2001 From: Edward Emelianov Date: Tue, 8 Sep 2026 22:44:24 +0300 Subject: [PATCH] add speed/current logging --- BTA_dome_modbus/client.c | 8 +++- BTA_dome_modbus/cmdlnopts.c | 4 ++ BTA_dome_modbus/cmdlnopts.h | 1 + BTA_dome_modbus/handlers_list.h | 3 ++ BTA_dome_modbus/motors.c | 84 +++++++++++++++++++++++++++++---- BTA_dome_modbus/motors.h | 10 +++- BTA_dome_modbus/server.c | 19 ++++++++ 7 files changed, 116 insertions(+), 13 deletions(-) diff --git a/BTA_dome_modbus/client.c b/BTA_dome_modbus/client.c index bf8197c..81a204c 100644 --- a/BTA_dome_modbus/client.c +++ b/BTA_dome_modbus/client.c @@ -214,6 +214,7 @@ static int check_motor(SSL *ssl, int motno){ speed /= (double)N; current /= (double)N; } + DBG("found %d working motors, mean speed=%g, mean current=%g", N, speed, current); if(status == MOT_OFF && CommonState.status != MOT_OFF){ *msg = MesgFault; sprintf(msg+1, "Dome: All motors are Off!\n"); @@ -337,13 +338,13 @@ static int process_system(SSL *ssl){ } if(check_motor(ssl, curMotNo)){ // TODO: check state for errors - if(++curMotNo == DomeSEW_N){ // set `struct SEWdata` parameters + if(curMotNo == indexSEWD){ // set `struct SEWdata` parameters motor_state_t *st = &MotorState[curMotNo]; statusSEWD = st->status; vel_SEWD = st->speed; currentSEWD = st->current; } - if(curMotNo >= MOTORS_AMOUNT) curMotNo = 0; + if(++curMotNo >= MOTORS_AMOUNT) curMotNo = 0; } chk_dome_speed(ssl); return TRUE; @@ -393,6 +394,9 @@ void clientproc(SSL_CTX *ctx, int fd){ LOGERR("Can't get SHM block"); ERRX("Can't get SHM block"); } + // set amount of working motors + DomeSEW_N = MOTORS_AMOUNT; + DBG("set DomeSEW_N = %d", DomeSEW_N); while(isrunning){ if(!process_system(ssl)){ LOGERR("Motors error"); diff --git a/BTA_dome_modbus/cmdlnopts.c b/BTA_dome_modbus/cmdlnopts.c index 3586507..691e52a 100644 --- a/BTA_dome_modbus/cmdlnopts.c +++ b/BTA_dome_modbus/cmdlnopts.c @@ -50,6 +50,9 @@ glob_pars G = { .T_sync_lost = 5., .speedchk_interval = 10., #endif +#ifdef SERVER + .motlogdir = "/tmp", +#endif }; /* @@ -71,6 +74,7 @@ static sl_option_t cmdlnopts[] = { {"emulation",NO_ARGS, NULL, 'e', arg_int, APTR(&G.emulmode), _("run server in emulation mode")}, {"serialdev",NEED_ARG, NULL, 'd', arg_string, APTR(&G.serialpath),_("path to RS-485 device")}, {"serialspeed",NEED_ARG,NULL, 's', arg_int, APTR(&G.serialspeed),_("speed of serial device")}, + {"mldir", NEED_ARG, NULL, 'D', arg_string, APTR(&G.motlogdir), _("directory to store motor logs (default: /tmp)")}, #endif #ifdef CLIENT {"mottmout",NEED_ARG, NULL, 'M', arg_double, APTR(&G.speedchk_interval), _("interval of motor's speed checking, s (default: 10)")}, diff --git a/BTA_dome_modbus/cmdlnopts.h b/BTA_dome_modbus/cmdlnopts.h index 865afe6..043f0c9 100644 --- a/BTA_dome_modbus/cmdlnopts.h +++ b/BTA_dome_modbus/cmdlnopts.h @@ -46,6 +46,7 @@ typedef struct{ #ifdef SERVER int serialspeed; // speed of serial device char *serialpath; // path to RS-485 device + char *motlogdir; // directory to store motor logs #endif #ifdef CLIENT int terminal; // run client in terminal mode diff --git a/BTA_dome_modbus/handlers_list.h b/BTA_dome_modbus/handlers_list.h index b6a0b7e..b5700bc 100644 --- a/BTA_dome_modbus/handlers_list.h +++ b/BTA_dome_modbus/handlers_list.h @@ -26,13 +26,16 @@ // Handlers in this list MUST be in sortered order (by name)!!! #define HANDLERS_LIST() \ NEW_HANDLER(forbidden, "forbid (1) or permit (0) dome operations") \ +NEW_HANDLER(logprefix, "prefix of log file for nth motor (prefix_%d.log), max 31 chars") \ NEW_HANDLER(motcurrent, "maximal motor current") \ NEW_HANDLER(motnum, "active motor number for status requests") \ NEW_HANDLER(motspeed, "motor speed") \ NEW_HANDLER(motstatus, "motor status") \ NEW_HANDLER(nmotors, "amount of working motors") \ NEW_HANDLER(speed, "speed setter") \ +NEW_HANDLER(startlog, "start logging") \ NEW_HANDLER(stop, "stop motors") \ +NEW_HANDLER(stoplog, "stop logging and close log file") \ /*NEW_HANDLER(current, "current setter") \*/ /*NEW_HANDLER(relay, "relay command") \*/ diff --git a/BTA_dome_modbus/motors.c b/BTA_dome_modbus/motors.c index a85e07b..4b5b954 100644 --- a/BTA_dome_modbus/motors.c +++ b/BTA_dome_modbus/motors.c @@ -18,7 +18,10 @@ #include #include +#include +#include +#include "cmdlnopts.h" #include "motors.h" #include "esq770.h" @@ -35,6 +38,12 @@ speed=xx - (sg) current=xx - (sg) уставка тока #endif +// log file (speed, current) prefix +static char logprefix[MAX_LOGPREFIX_LEN + 1] = "motor"; +static FILE *logF[MOTORS_AMOUNT] = {NULL}; +static double Tstart = 0.; +static pthread_mutex_t logmutex = PTHREAD_MUTEX_INITIALIZER; + static modbus_t *modbus_ctx = NULL; static motor_state_t motstates[MOTORS_AMOUNT] = {0}; // amount of working motors @@ -69,9 +78,10 @@ static void motors_close_m(){ modbus_free(modbus_ctx); modbus_ctx = NULL; } + n_working_motors = 0; } static void motors_close_e(){ // stub for emulation mode - ; + n_working_motors = 0; } // open modbus @ given speed; return FALSE if failed @@ -97,6 +107,8 @@ static int motors_open_m(const char *path, int speed){ return TRUE; } static int motors_open_e(const char _U_ *path, int _U_ speed){ // stub for emulation mode + for(int i = 0; i < MOTORS_AMOUNT; ++i) motstates[i].status = MOT_SLEEP; + n_working_motors = MOTORS_AMOUNT; return TRUE; } @@ -126,8 +138,14 @@ double motors_get_speedsetpoint(){ // set setpoint of speed (rev/min) sl_sock_hresult_e motors_set_speedsetpoint(double val){ double absval = fabs(val); - if(absval > MAX_SPEED) return RESULT_BADVAL; - if(n_working_motors < MIN_WORKING_MOTORS) return RESULT_FAIL; + if(absval > MAX_SPEED){ + DBG("Speed to large"); + return RESULT_BADVAL; + } + if(n_working_motors < MIN_WORKING_MOTORS){ + DBG("n_working_motors = %d", n_working_motors); + return RESULT_FAIL; + } DBG("Change speed setpoint to %g", val); speedSet = val; flags.change_speed = 1; @@ -170,6 +188,14 @@ static void count_motors(){ } } +static void log_data(int motno){ + if(!logF[motno] || motstates[motno].status != MOT_RUN) return; + pthread_mutex_lock(&logmutex); + fprintf(logF[motno], "%.3f\t%.2f\t%.2f\n", + sl_dtime() - Tstart, motstates[motno].speed, motstates[motno].current); + pthread_mutex_unlock(&logmutex); +} + // main motors processing routine static void motors_process_m(){ static int curN = 0, errctr = 0; @@ -245,7 +271,7 @@ static void motors_process_m(){ DBG("status: ERROR"); motstates[curN].status = MOT_ERROR; } - if(regs[1] & STATUS_READY_MASK){ + if(!(regs[1] & STATUS_READY_MASK)){ DBG("Motor not ready"); motstates[curN].status = MOT_ERROR; } @@ -257,6 +283,8 @@ static void motors_process_m(){ if(-1 == modbus_read_registers(modbus_ctx, REG_OUTPUT_CURRENT, 1, ®s[1])) regs[1] = 0; motstates[curN].speed = FREQ2REVMIN(regs[0]); motstates[curN].current = ((double)regs[1]) / CURRENT_SCALE; + // Logging + log_data(curN); if(++curN >= MOTORS_AMOUNT){ curN = 0; count_motors(); @@ -270,12 +298,6 @@ static void motors_process_e(){ static double t0 = -1., curspeed = 0.; double curt = sl_dtime(), dt = curt - t0, curcurrent = 0.; int curstatus = motstates[0].status; - if(t0 < 0.){ - t0 = curt; - // init state ("turn motors on") - for(int i = 0; i < MOTORS_AMOUNT; ++i) motstates[i].status = MOT_SLEEP; - return; - } if(flags.all){ if(flags.stop){ speedSet = 0.; @@ -301,6 +323,7 @@ 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; @@ -319,3 +342,44 @@ void set_emulation_mode(){ } int motors_get_working_amount(){ return n_working_motors; } + +void get_logfile_prefix(char buf[MAX_LOGPREFIX_LEN+1]){ strcpy(buf, logprefix); } + +int set_logfile_prefix(char *value){ + if(strlen(value) > MAX_LOGPREFIX_LEN) return FALSE; + char *ptr = value; + while(*ptr){ + char c = *ptr; + if(c == '/' || c == '%' || c <= ' ' || c > 126) return FALSE; + ++ptr; + } + strcpy(logprefix, value); + return TRUE; +} + +int start_log(){ + char buf[PATH_MAX+1]; + pthread_mutex_lock(&logmutex); + int errctr = 0; + 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"); + if(!logF[i]) ++errctr; + } + pthread_mutex_unlock(&logmutex); + Tstart = sl_dtime(); + if(errctr == MOTORS_AMOUNT) return FALSE; + return TRUE; +} + +void stop_log(){ + pthread_mutex_lock(&logmutex); + for(int i = 0; i < MOTORS_AMOUNT; ++i){ + if(logF[i]){ + fclose(logF[i]); + logF[i] = NULL; + } + } + pthread_mutex_unlock(&logmutex); +} diff --git a/BTA_dome_modbus/motors.h b/BTA_dome_modbus/motors.h index 67e4c69..2eb38e5 100644 --- a/BTA_dome_modbus/motors.h +++ b/BTA_dome_modbus/motors.h @@ -21,7 +21,7 @@ #include // minimal amount of working motors to spin dome -#define MIN_WORKING_MOTORS 4 +#define MIN_WORKING_MOTORS 1 // max errors per motor to mean it OFF #define MAX_ERRORS 5 @@ -39,6 +39,9 @@ #define MSpeed 350 #define HSpeed 610 +// log-file prefix length +#define MAX_LOGPREFIX_LEN 31 + // modbus responce timeout, ms #define MODBUS_RESPONCE_TIMEOUT 100000 @@ -82,3 +85,8 @@ extern int (*motors_open)(const char *, int); extern void (*motors_close)(); void set_emulation_mode(); + +void get_logfile_prefix(char buf[MAX_LOGPREFIX_LEN+1]); +int set_logfile_prefix(char *value); +int start_log(); +void stop_log(); diff --git a/BTA_dome_modbus/server.c b/BTA_dome_modbus/server.c index 9a4abab..2ae12a6 100644 --- a/BTA_dome_modbus/server.c +++ b/BTA_dome_modbus/server.c @@ -298,6 +298,25 @@ sl_sock_hresult_e nmotors_handler(int _U_ index, char _U_ value[SL_VAL_LEN]){ return RESULT_SILENCE; } +sl_sock_hresult_e logprefix_handler(int _U_ index, char _U_ value[SL_VAL_LEN]){ + if(ISSETTER(value)){ + // chkeck `value`: + if(set_logfile_prefix(value)) return RESULT_OK; + return RESULT_BADVAL; + } else get_logfile_prefix(value); + return RESULT_SILENCE; +} + +sl_sock_hresult_e startlog_handler(int _U_ index, char _U_ value[SL_VAL_LEN]){ + if(start_log()) return RESULT_OK; + return RESULT_FAIL; +} + +sl_sock_hresult_e stoplog_handler(int _U_ index, char _U_ value[SL_VAL_LEN]){ + stop_log(); + return RESULT_OK; +} + // binary search handler by name static int search_handler(const char *name){ int low = 0;