eddy/BTA_utils
1
0
Fork 0
mirror of https://github.com/eddyem/BTA_utils.git synced 2026-08-13 19:49:24 +03:00
Code Issues Packages Projects Releases Wiki Activity

add motors emulation

Browse Source
This commit is contained in:
Edward Emelianov
2026-08-11 16:53:11 +03:00
parent 25d35e2294
commit a996bae853
12 changed files with 165 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
BTA_dome_modbus/Makefile
View File

@@ -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"

5
BTA_dome_modbus/client.c
View File

@@ -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);
}

7
BTA_dome_modbus/cmdlnopts.c
View File

@@ -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

10
BTA_dome_modbus/cmdlnopts.h
View File

@@ -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;

5
BTA_dome_modbus/daemon.c
View File

@@ -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
printf("cert: %s, key: %s\n", G.cert, G.key);
#endif
DBG("cert: %s, key: %s\n", G.cert, G.key);
#ifdef CLIENT
//DBG("server: %s", G.serverhost);
if(!G.serverhost) ERRX("Point server name");

3
BTA_dome_modbus/handlers_list.h
View File

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

110
BTA_dome_modbus/motors.c
View File

@@ -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
void motors_stop(){
;
// open modbus @ given speed; return FALSE if failed
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;
}

32
BTA_dome_modbus/motors.h
View File

@@ -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);
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);
int motors_set_curntsetpoint(double);
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();

2
BTA_dome_modbus/run_client
View File

@@ -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 $@

2
BTA_dome_modbus/run_server
View File

@@ -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 $@

6
BTA_dome_modbus/server.c
View File

@@ -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;

11
BTA_dome_modbus/sslsock.c
View File

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