add speed/current logging

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");

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)")},

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

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") \*/

BTA_dome_modbus/motors.c

@@ -18,7 +18,10 @@
 
 #include <math.h>
 #include <modbus/modbus.h>
+#include <stdio.h>
+#include <string.h>
 
+#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, &regs[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);
+}

BTA_dome_modbus/motors.h

@@ -21,7 +21,7 @@
 #include <usefull_macros.h>
 
 // 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();

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;