eddy/BTA_utils
1
0
Fork 0
mirror of https://github.com/eddyem/BTA_utils.git synced 2026-09-29 21:30:39 +03:00
Code Issues Packages Projects Releases Wiki Activity

1st approach

Browse Source
This commit is contained in:
Edward Emelianov
2026-09-01 16:00:51 +03:00
parent 11f3553344
commit ae999fcd48
9 changed files with 104 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
BTA_dome_modbus/bta_shdata.c
View File

@@ -1,5 +1,6 @@
// (C) V.S. Shergin, SAO RAS // (C) V.S. Shergin, SAO RAS
#include <err.h> #include <err.h>
#include <crypt.h>
#include "bta_shdata.h" #include "bta_shdata.h"
#pragma pack(push, 4) #pragma pack(push, 4)
@@ -182,7 +183,12 @@ void get_cmd_queue(struct CMD_Queue *cq, int server){
cq->side = server; cq->side = server;
if(server){ if(server){
char buf[120]; /* выброÑ<C2BE>ить вÑ<C2B2>е команды из очереди */ char buf[120]; /* выброÑ<C2BE>ить вÑ<C2B2>е команды из очереди */
/*
÷ ÓÔÒÕËÔÕÒÁÈ ÄÌÑ ÏÞÅÒÅÄÅÊ ÓÏÏÂÝÅÎÉÊ ÉÓÐÏÌØÚÕÅÔÓÑ int32_t mtype, ÎÏ ÓÉÓÔÅÍÎÙÅ ×ÙÚÏ×Ù (msgsnd, msgrcv) ÏÖÉÄÁÀÔ long (ÎÁ 64-ÂÉÔÎÙÈ ÓÉÓÔÅÍÁÈ 8 ÂÁÊÔ). üÔÏ ÎÁÒÕÛÁÅÔ ×ÙÒÁ×ÎÉ×ÁÎÉÅ É ÐÒÉ×ÏÄÉÔ Ë ÎÅÐÒÁ×ÉÌØÎÏÍÕ ÞÔÅÎÉÀ/ÚÁÐÉÓÉ ÄÁÎÎÙÈ.
ïÓÏÂÅÎÎÏ ÏÐÁÓÎÏ: ÐÒÉ ÏÞÉÓÔËÅ ÏÞÅÒÅÄÉ ÎÁ ÓÅÒ×ÅÒÅ:
*/
while(msgrcv(cq->id, (struct msgbuf *)buf, 112, 0, IPC_NOWAIT) > 0); while(msgrcv(cq->id, (struct msgbuf *)buf, 112, 0, IPC_NOWAIT) > 0);
// òÁÚÍÅÒ ÄÁÎÎÙÈ (112) ×ÙÞÉÓÌÅÎ ÂÅÚ ÕÞ£ÔÁ ÒÁÚÍÅÒÁ long, ÞÔÏ ÍÏÖÅÔ ÐÒÉ×ÅÓÔÉ Ë ÓÂÏÑÍ.
}else }else
snd_id = cq->id; snd_id = cq->id;
cq->acckey = 0; cq->acckey = 0;

75
BTA_dome_modbus/client.c
View File

@@ -102,7 +102,7 @@ static sl_sock_hresult_e send_motor_command(SSL *ssl, const char *cmd, value_t *
} }
}else l = snprintf(buf, IOBUF_LEN-1, "%s\n", cmd); }else l = snprintf(buf, IOBUF_LEN-1, "%s\n", cmd);
buf[l] = 0; buf[l] = 0;
DBG("Send to server: %s", buf); //DBG("Send to server: %s", buf);
SSL_write(ssl, buf, l); SSL_write(ssl, buf, l);
double t0 = sl_dtime(); double t0 = sl_dtime();
while(sl_dtime() - t0 < G.acc_timeout){ while(sl_dtime() - t0 < G.acc_timeout){
@@ -113,35 +113,35 @@ static sl_sock_hresult_e send_motor_command(SSL *ssl, const char *cmd, value_t *
ERRX("Disconnected"); ERRX("Disconnected");
} }
buf[l] = 0; buf[l] = 0;
DBG("Received: \"%s\"\n", buf); //DBG("Received %d bytes: \"%s\"", l, buf);
// parser // parser
char key[SL_KEY_LEN] = {0}, val[SL_VAL_LEN] = {0}; char key[SL_KEY_LEN] = {0}, val[SL_VAL_LEN] = {0};
int got = sl_get_keyval(buf, key, val); int got = sl_get_keyval(buf, key, val);
DBG("got=%d, key=%s, val=%s", got, key, val); //DBG("got=%d, key=%s, val=%s", got, key, val);
if(got == 0){ if(got == 0){
DBG("Empty answer"); //DBG("Empty answer");
continue; continue;
} }
if((!setter && got == 1) || (setter && got == 2)){ // wrong answer if((setter && got == 2) || (getter && got == 1)){ // wrong answer
DBG("wrong answer"); DBG("wrong answer");
continue; continue;
} }
if(setter){ // check errcode in answer if(!getter){ // check errcode in answer
if(got == 2){ if(got == 2){
DBG("Getter answer"); DBG("Getter answer when setter called");
continue; continue;
} }
for(int i = 0; i < RESULT_SILENCE; ++i){ for(int i = 0; i < RESULT_SILENCE; ++i){
DBG("compare '%s' and '%s'", key, Ecodes[i]); //DBG("compare '%s' and '%s'", key, Ecodes[i]);
if(0 == strcmp(key, Ecodes[i])){ if(0 == strcmp(key, Ecodes[i])){
DBG("Found errcode for '%s': %d", key, i); //DBG("Found errcode for '%s': %d", key, i);
return i; // found return i; // found
} }
} }
}else{ // check "cmd = val" }else{ // check "cmd = val"
if(got == 1) continue; if(got == 1) continue;
if(strcmp(cmd, key)) continue; if(strcmp(cmd, key)) continue;
if(getter){ //if(getter){
if(getter->type == ARG_TYPE_INT){ if(getter->type == ARG_TYPE_INT){
long long ll; long long ll;
if(!sl_str2ll(&ll, val)) continue; if(!sl_str2ll(&ll, val)) continue;
@@ -151,10 +151,12 @@ static sl_sock_hresult_e send_motor_command(SSL *ssl, const char *cmd, value_t *
if(!sl_str2d(&d, val)) continue; if(!sl_str2d(&d, val)) continue;
getter->d = d; getter->d = d;
} }
} //}
//DBG("Getter OK");
return RESULT_OK; return RESULT_OK;
} }
} }
DBG("FAILED");
return RESULT_FAIL; return RESULT_FAIL;
} }
@@ -195,10 +197,18 @@ static int check_motor(SSL *ssl, int motno){
for(int i = 0; i < MOTORS_AMOUNT; ++i){ for(int i = 0; i < MOTORS_AMOUNT; ++i){
int s = MotorState[i].status; int s = MotorState[i].status;
if(s == MOT_OFF) continue; if(s == MOT_OFF) continue;
if(s == MOT_ERROR){
*msg = MesgWarn;
printf(msg+1, "Dome: Error in motor %d!\n", i+1);
SendMessage(msg);
MotorState[i].status = MOT_OFF;
}
if(status < s) status = s; if(status < s) status = s;
speed += MotorState[i].speed; if(status == MOT_RUN){
current += MotorState[i].current; speed += MotorState[i].speed;
++N; current += MotorState[i].current;
++N;
}
} }
if(N){ if(N){
speed /= (double)N; speed /= (double)N;
@@ -209,7 +219,7 @@ static int check_motor(SSL *ssl, int motno){
sprintf(msg+1, "Dome: All motors are Off!\n"); sprintf(msg+1, "Dome: All motors are Off!\n");
SendMessage(msg); SendMessage(msg);
}else if(status != MOT_OFF && CommonState.status == MOT_OFF){ }else if(status != MOT_OFF && CommonState.status == MOT_OFF){
*msg = MesgFault; *msg = MesgInfor;
sprintf(msg+1, "Dome: Start motors!\n"); sprintf(msg+1, "Dome: Start motors!\n");
SendMessage(msg); SendMessage(msg);
} }
@@ -222,15 +232,22 @@ static int check_motor(SSL *ssl, int motno){
// check for speed change and send given command to server // check for speed change and send given command to server
static void chk_dome_speed(SSL *ssl){ static void chk_dome_speed(SSL *ssl){
static int old_state = D_Off; static int old_state = D_Off;
static double tlast = 0.;
int new_state = Dome_Speed; int new_state = Dome_Speed;
double tlast = 0.; if(D_Locked /*|| !PEP_K_On*/){
if(old_state != D_Off) new_state = D_Off; // stop dome in locked state
else return;
}
if(new_state == old_state){ if(new_state == old_state){
if(sl_dtime() - tlast < G.speedchk_interval) return; if(sl_dtime() - tlast < G.speedchk_interval) return;
DBG("Time - tlast = %g", sl_dtime() - tlast);
if(CommonState.speed == set_speed){ if(CommonState.speed == set_speed){
tlast = sl_dtime(); tlast = sl_dtime();
return; return;
} }
DBG("Speed set command still not sent");
} }
DBG("state changed from %d to %d", old_state, new_state);
double new_speed = 0.; double new_speed = 0.;
switch(new_state){ switch(new_state){
case D_Lplus: case D_Lplus:
@@ -256,12 +273,15 @@ static void chk_dome_speed(SSL *ssl){
} }
value_t Dval = {.type = ARG_TYPE_DOUBLE, .d = new_speed}; value_t Dval = {.type = ARG_TYPE_DOUBLE, .d = new_speed};
if(RESULT_OK == send_motor_command(ssl, cmd_speed, &Dval, NULL)){ if(RESULT_OK == send_motor_command(ssl, cmd_speed, &Dval, NULL)){
DBG("New speed %g sent", new_speed);
if(RESULT_OK == send_motor_command(ssl, cmd_speed, NULL, &Dval) && fabs(Dval.d - new_speed) <= FLT_EPSILON){ if(RESULT_OK == send_motor_command(ssl, cmd_speed, NULL, &Dval) && fabs(Dval.d - new_speed) <= FLT_EPSILON){
DBG("Got answer with speed set: %g", Dval.d);
old_state = new_state; // all OK, command in work old_state = new_state; // all OK, command in work
set_speed = new_speed; set_speed = new_speed;
speedSEWD = new_speed;
tlast = sl_dtime(); tlast = sl_dtime();
} }else WARNX("error getting speed");
} }else WARNX("error sending motor command");
} }
// SHM parser; return FALSE if SHM is in erroreous state // SHM parser; return FALSE if SHM is in erroreous state
@@ -284,6 +304,14 @@ static int process_system(SSL *ssl){
if(++brokenshmctr < 10) return TRUE; if(++brokenshmctr < 10) return TRUE;
return FALSE; return FALSE;
}else brokenshmctr = 0; }else brokenshmctr = 0;
// set/reset PEP_K_On by information from server!
value_t Ival = {.type = ARG_TYPE_INT, .i = 0};
if(RESULT_OK != send_motor_command(ssl, cmd_forbidden, &Ival, NULL)) return FALSE;
#ifdef EBUG
//if(PEP_K_On != Ival.i) DBG("Set PEP_K_On to %d", !Ival.i);
#endif
if(Ival.i) PEP_K_On = 0;
else PEP_K_On = 1;
static int modelused = FALSE; static int modelused = FALSE;
if(!G.emulmode){ if(!G.emulmode){
if(UseModel == FullModel){ // model if(UseModel == FullModel){ // model
@@ -307,10 +335,15 @@ static int process_system(SSL *ssl){
} }
if(check_motor(ssl, curMotNo)){ if(check_motor(ssl, curMotNo)){
// TODO: check state for errors // TODO: check state for errors
if(++curMotNo >= MOTORS_AMOUNT) curMotNo = 0; if(++curMotNo == DomeSEW_N){ // 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(!D_Locked && Dome_State != D_Off) chk_dome_speed(ssl); chk_dome_speed(ssl);
;
return TRUE; return TRUE;
} }

1
BTA_dome_modbus/handlers_list.h
View File

@@ -25,6 +25,7 @@
// Handlers in this list MUST be in sortered order (by name)!!! // Handlers in this list MUST be in sortered order (by name)!!!
#define HANDLERS_LIST() \ #define HANDLERS_LIST() \
NEW_HANDLER(forbidden, "forbid (1) or permit (0) dome operations") \
NEW_HANDLER(motcurrent, "maximal motor current") \ NEW_HANDLER(motcurrent, "maximal motor current") \
NEW_HANDLER(motnum, "active motor number for status requests") \ NEW_HANDLER(motnum, "active motor number for status requests") \
NEW_HANDLER(motspeed, "motor speed") \ NEW_HANDLER(motspeed, "motor speed") \

16
BTA_dome_modbus/motors.c
View File

@@ -42,6 +42,7 @@ static motor_state_t motstates[MOTORS_AMOUNT] = {0};
// set points // set points
static double currentSet = DEFAULT_CURRENT, speedSet = 0.; static double currentSet = DEFAULT_CURRENT, speedSet = 0.;
// flags for main routine // flags for main routine
// TODO: add mutex or make flags atomic
static union{ static union{
struct{ struct{
uint32_t change_speed : 1; uint32_t change_speed : 1;
@@ -85,7 +86,7 @@ static int motors_open_m(const char *path, int speed){
LOGERR("Can't open device %s @ %d", path, speed); LOGERR("Can't open device %s @ %d", path, speed);
return FALSE; return FALSE;
} }
modbus_set_response_timeout(modbus_ctx, 0, 50000); // 50ms response timeout modbus_set_response_timeout(modbus_ctx, 0, MODBUS_RESPONCE_TIMEOUT); // response timeout
if(modbus_connect(modbus_ctx) < 0){ if(modbus_connect(modbus_ctx) < 0){
WARNX("Can't connect to device %s", path); WARNX("Can't connect to device %s", path);
LOGERR("Can't connect to device %s", path); LOGERR("Can't connect to device %s", path);
@@ -135,7 +136,7 @@ int motors_set_speedsetpoint(double val){
int motors_get_activenum(){ return motindex; } int motors_get_activenum(){ return motindex; }
// set number of active motor // set number of active motor
int motors_set_activenum(int N){ int motors_set_activenum(int N){
DBG("Set active motor=%d", N); //DBG("Set active motor=%d", N);
if(N < 0 || N >= MOTORS_AMOUNT) return FALSE; if(N < 0 || N >= MOTORS_AMOUNT) return FALSE;
motindex = N; motindex = N;
return TRUE; return TRUE;
@@ -170,24 +171,29 @@ static void motors_process_m(){
if(flags.all){ if(flags.all){
if(-1 == modbus_set_slave(modbus_ctx, 0)) goto reg_error; if(-1 == modbus_set_slave(modbus_ctx, 0)) goto reg_error;
if(flags.stop){ if(flags.stop){
DBG("User asks to stop");
speedSet = 0.; speedSet = 0.;
// send command stop // send command stop
if(-1 == modbus_write_register(modbus_ctx, REG_CMD, CMD_STOP)) goto reg_error; if(-1 == modbus_write_register(modbus_ctx, REG_CMD, CMD_STOP)) goto reg_error;
flags.stop = 0;
} }
if(flags.change_current){ if(flags.change_current){
DBG("User asks to change current to %g", currentSet);
// TODO: send command "max current"? // TODO: send command "max current"?
flags.change_current = 0;
} }
if(flags.change_speed){ if(flags.change_speed){
DBG("User asks to change speed to %g", speedSet);
// send command "set speed" // send command "set speed"
uint16_t dir = (speedSet > 0.) ? CMD_FORWARD : CMD_REVERSE; uint16_t dir = (speedSet > 0.) ? CMD_FORWARD : CMD_REVERSE;
uint16_t freq = (uint16_t)(fabs(speedSet) * FREQ_SCALE); uint16_t freq = (uint16_t)(fabs(speedSet) * FREQ_SCALE);
if(-1 == modbus_write_register(modbus_ctx, REG_FREQ_SET, freq)) goto reg_error;
if(-1 == modbus_write_register(modbus_ctx, REG_CMD, dir)) goto reg_error; if(-1 == modbus_write_register(modbus_ctx, REG_CMD, dir)) goto reg_error;
if(-1 == modbus_write_register(modbus_ctx, REG_FREQ_SET, freq)) goto reg_error;
flags.change_speed = 0;
} }
flags.all = 0;
} }
// set slave N // set slave N
if(-1 == modbus_set_slave(modbus_ctx, curN)) goto reg_error; if(-1 == modbus_set_slave(modbus_ctx, MOTOR_ID(curN))) goto reg_error;
// ask for speed/status/current // ask for speed/status/current
uint16_t regs[2]; uint16_t regs[2];
if(-1 == modbus_read_registers(modbus_ctx, REG_STATUS_MAIN, 2, regs)){ if(-1 == modbus_read_registers(modbus_ctx, REG_STATUS_MAIN, 2, regs)){

13
BTA_dome_modbus/motors.h
View File

@@ -21,10 +21,11 @@
// max errors per motor to mean it OFF // max errors per motor to mean it OFF
#define MAX_ERRORS 5 #define MAX_ERRORS 5
#define MAX_SPEED 700. // freq_scale == 100, so for UINT16_t we can't make speed more than 655.35
#define MAX_SPEED 655.
#define SPEED_TOLERANCE 0.01 #define SPEED_TOLERANCE 0.01
// emulation acceleration, min^-2 // emulation acceleration, min^-2
#define EMUL_ACCEL 20000. #define EMUL_ACCEL 10000.
#define MAX_CURRENT 15. #define MAX_CURRENT 15.
#define DEFAULT_CURRENT 10. #define DEFAULT_CURRENT 10.
#define MOTORS_AMOUNT 10 #define MOTORS_AMOUNT 10
@@ -33,10 +34,12 @@
#define MSpeed 350 #define MSpeed 350
#define HSpeed 610 #define HSpeed 610
// modbus responce timeout, ms
#define MODBUS_RESPONCE_TIMEOUT 100000
// ID of first motor minus 1 // ID of first motor
#define START_ID (0) #define START_ID (1)
// ID of motor (n=1..MOTORS_AMOUNT) // ID of nth motor (n=0..MOTORS_AMOUNT-1 - index)
#define MOTOR_ID(n) (n + START_ID) #define MOTOR_ID(n) (n + START_ID)
// motors' status // motors' status

2
BTA_dome_modbus/run_client
View File

@@ -1,3 +1,3 @@
#!/bin/bash #!/bin/bash
rm client.log rm -f 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 $@ ./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 #!/bin/bash
rm server.log rm -f 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 $@ ./dome_server -a ca/ca/ca_cert.pem -c ca/server/server_cert.pem -k ca/server/private/server_key.pem -vvvl server.log $@

26
BTA_dome_modbus/server.c
View File

@@ -27,6 +27,9 @@
// handlers: `index` - command index in list, `value` - setter's value or getter's answer // handlers: `index` - command index in list, `value` - setter's value or getter's answer
typedef sl_sock_hresult_e (*handler_t)(int index, char value[SL_VAL_LEN]); typedef sl_sock_hresult_e (*handler_t)(int index, char value[SL_VAL_LEN]);
// == TRUE if dome management is forbidden
static int forbidden = FALSE;
// struct for setters/getters // struct for setters/getters
typedef struct{ typedef struct{
const char *command; const char *command;
@@ -70,7 +73,7 @@ static int handle_connection(SSL *ssl){
int r = read_string(ssl, buf, IOBUF_LEN); int r = read_string(ssl, buf, IOBUF_LEN);
if(r < 0) return 0; if(r < 0) return 0;
int sd = SSL_get_fd(ssl); int sd = SSL_get_fd(ssl);
DBG("Client %d msg: \"%s\"\n", sd, buf); //DBG("Client %d msg: \"%s\"\n", sd, buf);
LOGDBG("fd=%d, message=%s", sd, buf); LOGDBG("fd=%d, message=%s", sd, buf);
int got = sl_get_keyval(buf, key, val); int got = sl_get_keyval(buf, key, val);
if(got == 0){ if(got == 0){
@@ -81,11 +84,11 @@ static int handle_connection(SSL *ssl){
sl_sock_hresult_e result = RESULT_BADKEY; sl_sock_hresult_e result = RESULT_BADKEY;
if(-1 != h_idx){ if(-1 != h_idx){
if(got == 1){ if(got == 1){
DBG("getter #%d", h_idx); //DBG("getter #%d", h_idx);
val[0] = 0; // getter val[0] = 0; // getter
}else DBG("setter #%d", h_idx); } //else DBG("setter #%d", h_idx);
result = command_list[h_idx].handler(h_idx, val); result = command_list[h_idx].handler(h_idx, val);
DBG("result: %d", result); //DBG("result: %d", result);
}else{ }else{
DBG("Command not found or help?"); DBG("Command not found or help?");
// check if user asks for help // check if user asks for help
@@ -186,6 +189,8 @@ void serverproc(SSL_CTX *ctx, int fd){
WARNX("SSL_accept()"); WARNX("SSL_accept()");
SSL_free(ssl); SSL_free(ssl);
send(client, sslerr, sizeof(sslerr)-1, MSG_NOSIGNAL); send(client, sslerr, sizeof(sslerr)-1, MSG_NOSIGNAL);
shutdown(client, SHUT_WR);
usleep(50000);
close(client); close(client);
} }
} }
@@ -212,6 +217,16 @@ void serverproc(SSL_CTX *ctx, int fd){
} }
/****************** Protocol handlers (return 0 in case of success or error code >0 if failed) ******************/ /****************** Protocol handlers (return 0 in case of success or error code >0 if failed) ******************/
sl_sock_hresult_e forbidden_handler(int _U_ index, char _U_ value[SL_VAL_LEN]){
int I;
if(ISSETTER(value)){
if(!sl_str2i(&I, value)) return RESULT_BADVAL;
forbidden = I;
return RESULT_OK;
}
snprintf(value, SL_VAL_LEN-1, "%d", forbidden);
return RESULT_SILENCE;
}
// key - keyword (command name), value - i/o buffer (value[0]==0 for getters) // key - keyword (command name), value - i/o buffer (value[0]==0 for getters)
/* /*
sl_sock_hresult_e current_handler(int _U_ index, char _U_ value[SL_VAL_LEN]){ sl_sock_hresult_e current_handler(int _U_ index, char _U_ value[SL_VAL_LEN]){
@@ -265,6 +280,7 @@ sl_sock_hresult_e relay_handler(int _U_ index, char _U_ value[SL_VAL_LEN]){
sl_sock_hresult_e speed_handler(int _U_ index, char _U_ value[SL_VAL_LEN]){ sl_sock_hresult_e speed_handler(int _U_ index, char _U_ value[SL_VAL_LEN]){
double D; double D;
if(ISSETTER(value)){ if(ISSETTER(value)){
if(forbidden) return RESULT_FAIL;
if(!sl_str2d(&D, value) || !motors_set_speedsetpoint(D)) return RESULT_BADVAL; if(!sl_str2d(&D, value) || !motors_set_speedsetpoint(D)) return RESULT_BADVAL;
return RESULT_OK; return RESULT_OK;
} }
@@ -289,7 +305,7 @@ static int search_handler(const char *name){
// Compare the target string with the struct's string field // Compare the target string with the struct's string field
int res = strcmp(name, command_list[mid].command); int res = strcmp(name, command_list[mid].command);
if(res == 0){ if(res == 0){
DBG("Found %s by %d iterations\n", name, iter); //DBG("Found %s by %d iterations\n", name, iter);
return mid; // Target found, return index return mid; // Target found, return index
}else if(res < 0){ }else if(res < 0){
high = mid - 1; // Target is smaller, search left half high = mid - 1; // Target is smaller, search left half

1
BTA_dome_modbus/sslsosk.files
View File

@@ -6,6 +6,7 @@ cmdlnopts.c
cmdlnopts.h cmdlnopts.h
daemon.c daemon.c
daemon.h daemon.h
esq770.h
handlers_list.h handlers_list.h
main.c main.c
motors.c motors.c