add motors emulation

BTA_dome_modbus/Makefile

@@ -58,16 +58,17 @@ $(COBJDIR):
 	@mkdir $(COBJDIR)
 
 ifneq ($(MAKECMDGOALS),clean)
--include $(DEPS)
+-include $(SDEPS)
+-include $(CDEPS)
 endif
 
-$(COBJDIR)/%.o: %.c
+$(COBJDIR)/%.o: %.c 
 	@echo -e "\tCC $<"
-	$(CC) -MD -c $(LDFLAGS) $(CFLAGS) $(DEFINES) -o $@ $<
+	$(CC) -MP -MMD -c $(LDFLAGS) $(CFLAGS) $(DEFINES) -o $@ $<
 
-$(SOBJDIR)/%.o: %.c
+$(SOBJDIR)/%.o: %.c 
 	@echo -e "\t\tCC $<"
-	$(CC) -MD -c $(LDFLAGS) $(CFLAGS) $(DEFINES) -o $@ $<
+	$(CC) -MP -MMD -c $(LDFLAGS) $(CFLAGS) $(DEFINES) -o $@ $<
 
 clean:
 	@echo -e "\t\tCLEAN"

BTA_dome_modbus/client.c

@@ -161,9 +161,10 @@ static sl_sock_hresult_e send_motor_command(SSL *ssl, const char *cmd, value_t *
 // emergency stop motors
 static void stop_all(SSL *ssl){
     int ntries = 0;
-    value_t speed = {.type = ARG_TYPE_DOUBLE, .d = 0.};
+    //value_t speed = {.type = ARG_TYPE_DOUBLE, .d = 0.};
     while(ntries < 5){
-        if(RESULT_OK == send_motor_command(ssl, cmd_speed, &speed, NULL)) break;
+        //if(RESULT_OK == send_motor_command(ssl, cmd_speed, &speed, NULL)) break;
+        if(RESULT_OK == send_motor_command(ssl, cmd_stop, NULL, NULL)) break;
         ++ntries;
         usleep(100000);
     }

BTA_dome_modbus/cmdlnopts.c

@@ -46,8 +46,10 @@ glob_pars G = {
     .key = DEFKEY,
     .ca = DEFCA,
     .acc_timeout = 1.,
+#ifdef CLIENT
     .T_sync_lost = 5.,
     .speedchk_interval = 10.,
+#endif
 };
 
 /*
@@ -66,12 +68,15 @@ static sl_option_t cmdlnopts[] = {
     {"ca",      NEED_ARG,   NULL,   'a',    arg_string, APTR(&G.ca),        _("path to SSL ca - base cert (default:" DEFCA ")")},
     {"timeout", NEED_ARG,   NULL,   't',    arg_double, APTR(&G.acc_timeout),_("network timeout, s (default: 1)")},
 #ifdef SERVER
+    {"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")},
 #endif
 #ifdef CLIENT
     {"mottmout",NEED_ARG,   NULL,   'M',    arg_double, APTR(&G.speedchk_interval), _("interval of motor's speed checking, s (default: 10)")},
     {"server",  NEED_ARG,   NULL,   's',    arg_string, APTR(&G.serverhost), _("server node, IP:port")},
     {"lostsync",NEED_ARG,   NULL,   'L',    arg_double, APTR(&G.T_sync_lost),_("\"lost synchronization\" timeout, s (default: 5)")},
-    {"emulation",NO_ARGS,   NULL,   'e',    arg_int,    APTR(&G.emulmode),  _("run even in emulation mode")},
+    {"emulation",NO_ARGS,   NULL,   'e',    arg_int,    APTR(&G.emulmode),  _("run client even in emulation mode")},
     {"terminal",NO_ARGS,    NULL,   'T',    arg_int,    APTR(&G.terminal),  _("run client in terminal mode")},
 #endif
    end_option

BTA_dome_modbus/cmdlnopts.h

@@ -35,10 +35,7 @@
  */
 typedef struct{
     int emulmode;           // emulation mode: don't check server & run in model mode
-    int terminal;           // run client in terminal mode
     double acc_timeout;     // network timeout, s
-    double T_sync_lost;     // "lost synchronization" timeout, s
-    double speedchk_interval;// interval of motors' speed checking (resend command if don't reach yet)
     char *pidfile;          // name of PID file
     char *logfile;          // logging to this file
     char *cert;             // sertificate
@@ -46,8 +43,15 @@ typedef struct{
     char *port;             // port number
     int verbose;            // logfile verbose level
     char *ca;               // ca
+#ifdef SERVER
+    int serialspeed;        // speed of serial device
+    char *serialpath;       // path to RS-485 device
+#endif
 #ifdef CLIENT
+    int terminal;           // run client in terminal mode
+    double speedchk_interval;// interval of motors' speed checking (resend command if don't reach yet)
     char *serverhost;       // server IP address
+    double T_sync_lost;     // "lost synchronization" timeout, s
 #endif
 } glob_pars;
 

BTA_dome_modbus/daemon.c

@@ -58,6 +58,7 @@ int start_daemon(){
     int port = atoi(G.port);
     if(port < 1024 || port > 65535){
         LOGERR("Wrong port value: %d", port);
+        WARNX("Wrong port value: %d", port);
         return 1;
     }
     FILE *f = fopen(G.cert, "r");
@@ -66,9 +67,7 @@ int start_daemon(){
     f = fopen(G.key, "r");
     if(!f) ERR("Can't open certificate key file %s", G.key);
     fclose(f);
-#ifdef EBUG
+    DBG("cert: %s, key: %s\n", G.cert, G.key);
-    printf("cert: %s, key: %s\n", G.cert, G.key);
-#endif
 #ifdef CLIENT
     //DBG("server: %s", G.serverhost);
     if(!G.serverhost) ERRX("Point server name");

BTA_dome_modbus/handlers_list.h

@@ -25,11 +25,12 @@
 
 // Handlers in this list MUST be in sortered order (by name)!!!
 #define HANDLERS_LIST() \
-NEW_HANDLER(motcurrent, "motor current") \
+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(speed, "speed setter") \
+NEW_HANDLER(stop, "stop motors") \
 
 /*NEW_HANDLER(current, "current setter") \*/
 /*NEW_HANDLER(relay, "relay command") \*/

BTA_dome_modbus/motors.c

@@ -34,20 +34,50 @@ speed=xx - (sg)
 current=xx - (sg) ÕÓÔÁ×ËÁ ÔÏËÁ
 #endif
 
+static motor_state_t motstates[MOTORS_AMOUNT] = {0};
+
 // set points
-static double currentSet = 0., speedSet = 0.;
+static double currentSet = DEFAULT_CURRENT, speedSet = 0.;
+// flags for main routine
+static union{
+    struct{
+        uint32_t change_speed : 1;
+        uint32_t change_current : 1;
+        uint32_t stop : 1;
+    };
+    uint32_t all;
+} flags = {0};
+
+// emulation mode parameters
+/*static struct{
+    double tlast;
+
+} emulpar = {0};*/
+
 // current motor for status etc. getters (0..MOTORS_AMOUNT-1)
 static int motindex = 0;
 
-// stop all
+// open modbus @ given speed; return FALSE if failed
-void motors_stop(){
+static int modbus_open_m(const char _U_ *path, int _U_ speed){
-    ;
+    return FALSE;
+}
+static int modbus_open_e(const char _U_ *path, int _U_ speed){
+    return TRUE;
 }
 
 // close modbus connection
-void modbus_close(){
+static void modbus_close_m(){
     ;
 }
+static void modbus_close_e(){
+    ;
+}
+
+
+// stop all
+void motors_stop(){
+    flags.stop = 1;
+}
 
 // current setpoint getter
 double motors_get_curntsetpoint(){
@@ -56,7 +86,9 @@ double motors_get_curntsetpoint(){
 // set setpoint of current
 int motors_set_curntsetpoint(double val){
     if(val < 0. || val > MAX_CURRENT) return FALSE;
-    // do something
+    DBG("Change max current to %g", val);
+    currentSet = val;
+    flags.change_current = 1;
     return TRUE;
 }
 
@@ -68,7 +100,9 @@ double motors_get_speedsetpoint(){
 int motors_set_speedsetpoint(double val){
     double absval = fabs(val);
     if(absval > MAX_SPEED) return FALSE;
-    // do something
+    DBG("Change speed setpoint to %g", val);
+    speedSet = val;
+    flags.change_speed = 1;
     return TRUE;
 }
 
@@ -84,18 +118,74 @@ int motors_set_activenum(int N){
 
 // get current value for active motor
 int motors_get_actcurrent(double *val){
-    if(val) *val = 0.;
+    if(val) *val = motstates[motindex].current;
     return TRUE;
 }
 
 // get speed for active motor
 int motors_get_actspeed(double *val){
-    if(val) *val = 0.;
+    if(val) *val = motstates[motindex].speed;
     return TRUE;
 }
 
 // get status for active motor
 int motors_get_actstatus(int *val){
-    if(val) *val = 0;
+    if(val) *val = motstates[motindex].status;
     return TRUE;
 }
+
+// main motors processing routine
+static void motors_process_m(){
+    ;
+}
+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.;
+            if(curstatus != MOT_RUN) curstatus = MOT_SLEEP;
+        }
+        flags.all = 0;
+    }
+    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}
+        if(acceleration > 0.){
+            if(newspeed > speedSet) newspeed = speedSet;
+        }else{
+            if(newspeed < speedSet) newspeed = speedSet;
+        }
+        curspeed = newspeed;
+        curcurrent = currentSet;
+        curstatus = MOT_RUN;
+        DBG("Now speed = %g", curspeed);
+    }else{
+        if(fabs(curspeed) < SPEED_TOLERANCE) curstatus = MOT_SLEEP; // stopped
+        else curcurrent = currentSet * 0.6;
+    }
+    t0 = curt;
+    for(int i = 0; i < MOTORS_AMOUNT; ++i){
+        motstates[i].status = curstatus;
+        motstates[i].current = curcurrent;
+        motstates[i].speed = curspeed;
+    }
+}
+
+int (*modbus_open)(const char *, int) = modbus_open_m;
+void (*modbus_close)() = modbus_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_process = motors_process_e;
+}

BTA_dome_modbus/motors.h

@@ -19,7 +19,11 @@
 #pragma once
 
 #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)
 // low, medium and high speeds
 #define LSpeed    71
@@ -45,15 +49,23 @@ typedef struct{
     double current;
 } motor_state_t;
 
-void motors_stop();
 double motors_get_curntsetpoint();
-int motors_set_curntsetpoint(double val);
+int motors_set_curntsetpoint(double);
-double motors_get_speedsetpoint();
-int motors_set_speedsetpoint(double val);
-int motors_get_activenum();
-int motors_set_activenum(int N);
-int motors_get_actcurrent(double *val);
-int motors_get_actspeed(double *val);
-int motors_get_actstatus(int *val);
 
-void modbus_close();
+double motors_get_speedsetpoint();
+int motors_set_speedsetpoint(double);
+
+int motors_get_activenum();
+int motors_set_activenum(int);
+
+void motors_stop();
+int motors_get_actcurrent(double*);
+int motors_get_actspeed(double*);
+int motors_get_actstatus(int*);
+
+extern void (*motors_process)();
+
+extern int (*modbus_open)(const char *, int);
+extern void (*modbus_close)();
+
+void set_emulation_mode();

BTA_dome_modbus/run_client

@@ -1,3 +1,3 @@
 #!/bin/bash
-rm client.log 2>/dev/null
+rm client.log
 ./dome_client -a ca/ca/ca_cert.pem -c ca/client/client_cert.pem -k ca/client/private/client_key.pem -vvv -s localhost -l client.log $@

BTA_dome_modbus/run_server

@@ -1,3 +1,3 @@
 #!/bin/bash
-rm server.log 2>/dev/null
+rm server.log
 ./dome_server -a ca/ca/ca_cert.pem -c ca/server/server_cert.pem -k ca/server/private/server_key.pem -vvvl server.log $@

BTA_dome_modbus/server.c

@@ -142,6 +142,7 @@ void serverproc(SSL_CTX *ctx, int fd){
     //char buf[64];
     //int P = 0;
     while(isrunning){
+        motors_process();
         /*double tnow = sl_dtime();
         if(tnow - t0 > 5. && nfd > 1){ // broadcasting message
             //DBG("send ping");
@@ -271,6 +272,11 @@ sl_sock_hresult_e speed_handler(int _U_ index, char _U_ value[SL_VAL_LEN]){
     return RESULT_SILENCE;
 }
 
+sl_sock_hresult_e stop_handler(int _U_ index, char _U_ value[SL_VAL_LEN]){
+    motors_stop();
+    return RESULT_OK;
+}
+
 // binary search handler by name
 static int search_handler(const char *name){
     int low = 0;

BTA_dome_modbus/sslsock.c

@@ -26,6 +26,7 @@
 #include "cmdlnopts.h"
 #include "sslsock.h"
 #ifdef SERVER
+#include "motors.h"
 #include "server.h"
 #else
 #include "client.h"
@@ -132,11 +133,17 @@ static SSL_CTX* InitCTX(void){
 }
 
 int open_socket(){
-    int fd;
+    FNAME();
     SSL_library_init();
     SSL_CTX *ctx = InitCTX();
-    fd = OpenConn(atoi(G.port));
+    int fd = OpenConn(atoi(G.port));
 #ifdef SERVER
+    if(G.emulmode) set_emulation_mode();
+    if(!modbus_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;
+    }
     serverproc(ctx, fd);
 #else
     if(G.terminal) terminal(ctx, fd);