From ae999fcd48bb6fef9edb0689d05a65b7435cf9b0 Mon Sep 17 00:00:00 2001 From: Edward Emelianov Date: Tue, 1 Sep 2026 16:00:51 +0300 Subject: [PATCH] 1st approach --- BTA_dome_modbus/bta_shdata.c | 6 +++ BTA_dome_modbus/client.c | 75 ++++++++++++++++++++++++--------- BTA_dome_modbus/handlers_list.h | 1 + BTA_dome_modbus/motors.c | 16 ++++--- BTA_dome_modbus/motors.h | 13 +++--- BTA_dome_modbus/run_client | 2 +- BTA_dome_modbus/run_server | 2 +- BTA_dome_modbus/server.c | 26 +++++++++--- BTA_dome_modbus/sslsosk.files | 1 + 9 files changed, 104 insertions(+), 38 deletions(-) diff --git a/BTA_dome_modbus/bta_shdata.c b/BTA_dome_modbus/bta_shdata.c index bd6ec4f..79135a1 100644 --- a/BTA_dome_modbus/bta_shdata.c +++ b/BTA_dome_modbus/bta_shdata.c @@ -1,5 +1,6 @@ // (C) V.S. Shergin, SAO RAS #include +#include #include "bta_shdata.h" #pragma pack(push, 4) @@ -182,7 +183,12 @@ void get_cmd_queue(struct CMD_Queue *cq, int server){ cq->side = server; if(server){ char buf[120]; /* выбросить все команды из очереди */ +/* + int32_t mtype, (msgsnd, msgrcv) long ( 64- 8 ). / . + : : +*/ while(msgrcv(cq->id, (struct msgbuf *)buf, 112, 0, IPC_NOWAIT) > 0); +// (112) ޣ long, . }else snd_id = cq->id; cq->acckey = 0; diff --git a/BTA_dome_modbus/client.c b/BTA_dome_modbus/client.c index 3aee6aa..8137e7a 100644 --- a/BTA_dome_modbus/client.c +++ b/BTA_dome_modbus/client.c @@ -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); buf[l] = 0; - DBG("Send to server: %s", buf); + //DBG("Send to server: %s", buf); SSL_write(ssl, buf, l); double t0 = sl_dtime(); 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"); } buf[l] = 0; - DBG("Received: \"%s\"\n", buf); + //DBG("Received %d bytes: \"%s\"", l, buf); // parser char key[SL_KEY_LEN] = {0}, val[SL_VAL_LEN] = {0}; 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){ - DBG("Empty answer"); + //DBG("Empty answer"); continue; } - if((!setter && got == 1) || (setter && got == 2)){ // wrong answer + if((setter && got == 2) || (getter && got == 1)){ // wrong answer DBG("wrong answer"); continue; } - if(setter){ // check errcode in answer + if(!getter){ // check errcode in answer if(got == 2){ - DBG("Getter answer"); + DBG("Getter answer when setter called"); continue; } 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])){ - DBG("Found errcode for '%s': %d", key, i); + //DBG("Found errcode for '%s': %d", key, i); return i; // found } } }else{ // check "cmd = val" if(got == 1) continue; if(strcmp(cmd, key)) continue; - if(getter){ + //if(getter){ if(getter->type == ARG_TYPE_INT){ long long ll; 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; getter->d = d; } - } + //} + //DBG("Getter OK"); return RESULT_OK; } } + DBG("FAILED"); return RESULT_FAIL; } @@ -195,10 +197,18 @@ static int check_motor(SSL *ssl, int motno){ for(int i = 0; i < MOTORS_AMOUNT; ++i){ int s = MotorState[i].status; 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; - speed += MotorState[i].speed; - current += MotorState[i].current; - ++N; + if(status == MOT_RUN){ + speed += MotorState[i].speed; + current += MotorState[i].current; + ++N; + } } if(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"); SendMessage(msg); }else if(status != MOT_OFF && CommonState.status == MOT_OFF){ - *msg = MesgFault; + *msg = MesgInfor; sprintf(msg+1, "Dome: Start motors!\n"); SendMessage(msg); } @@ -222,15 +232,22 @@ static int check_motor(SSL *ssl, int motno){ // check for speed change and send given command to server static void chk_dome_speed(SSL *ssl){ static int old_state = D_Off; + static double tlast = 0.; 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(sl_dtime() - tlast < G.speedchk_interval) return; + DBG("Time - tlast = %g", sl_dtime() - tlast); if(CommonState.speed == set_speed){ tlast = sl_dtime(); return; } + DBG("Speed set command still not sent"); } + DBG("state changed from %d to %d", old_state, new_state); double new_speed = 0.; switch(new_state){ case D_Lplus: @@ -256,12 +273,15 @@ static void chk_dome_speed(SSL *ssl){ } value_t Dval = {.type = ARG_TYPE_DOUBLE, .d = new_speed}; 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){ + DBG("Got answer with speed set: %g", Dval.d); old_state = new_state; // all OK, command in work set_speed = new_speed; + speedSEWD = new_speed; tlast = sl_dtime(); - } - } + }else WARNX("error getting speed"); + }else WARNX("error sending motor command"); } // 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; return FALSE; }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; if(!G.emulmode){ if(UseModel == FullModel){ // model @@ -307,10 +335,15 @@ static int process_system(SSL *ssl){ } if(check_motor(ssl, curMotNo)){ // 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; } diff --git a/BTA_dome_modbus/handlers_list.h b/BTA_dome_modbus/handlers_list.h index 0b21300..857fd0b 100644 --- a/BTA_dome_modbus/handlers_list.h +++ b/BTA_dome_modbus/handlers_list.h @@ -25,6 +25,7 @@ // 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(motcurrent, "maximal motor current") \ NEW_HANDLER(motnum, "active motor number for status requests") \ NEW_HANDLER(motspeed, "motor speed") \ diff --git a/BTA_dome_modbus/motors.c b/BTA_dome_modbus/motors.c index bcf933f..aff04e6 100644 --- a/BTA_dome_modbus/motors.c +++ b/BTA_dome_modbus/motors.c @@ -42,6 +42,7 @@ static motor_state_t motstates[MOTORS_AMOUNT] = {0}; // set points static double currentSet = DEFAULT_CURRENT, speedSet = 0.; // flags for main routine +// TODO: add mutex or make flags atomic static union{ struct{ 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); 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){ WARNX("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; } // set number of active motor 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; motindex = N; return TRUE; @@ -170,24 +171,29 @@ static void motors_process_m(){ if(flags.all){ if(-1 == modbus_set_slave(modbus_ctx, 0)) goto reg_error; if(flags.stop){ + DBG("User asks to stop"); speedSet = 0.; // send command stop if(-1 == modbus_write_register(modbus_ctx, REG_CMD, CMD_STOP)) goto reg_error; + flags.stop = 0; } if(flags.change_current){ + DBG("User asks to change current to %g", currentSet); // TODO: send command "max current"? + flags.change_current = 0; } if(flags.change_speed){ + DBG("User asks to change speed to %g", speedSet); // send command "set speed" uint16_t dir = (speedSet > 0.) ? CMD_FORWARD : CMD_REVERSE; 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_FREQ_SET, freq)) goto reg_error; + flags.change_speed = 0; } - flags.all = 0; } // 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 uint16_t regs[2]; if(-1 == modbus_read_registers(modbus_ctx, REG_STATUS_MAIN, 2, regs)){ diff --git a/BTA_dome_modbus/motors.h b/BTA_dome_modbus/motors.h index 55210cf..485945f 100644 --- a/BTA_dome_modbus/motors.h +++ b/BTA_dome_modbus/motors.h @@ -21,10 +21,11 @@ // max errors per motor to mean it OFF #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 // emulation acceleration, min^-2 -#define EMUL_ACCEL 20000. +#define EMUL_ACCEL 10000. #define MAX_CURRENT 15. #define DEFAULT_CURRENT 10. #define MOTORS_AMOUNT 10 @@ -33,10 +34,12 @@ #define MSpeed 350 #define HSpeed 610 +// modbus responce timeout, ms +#define MODBUS_RESPONCE_TIMEOUT 100000 -// ID of first motor minus 1 -#define START_ID (0) -// ID of motor (n=1..MOTORS_AMOUNT) +// ID of first motor +#define START_ID (1) +// ID of nth motor (n=0..MOTORS_AMOUNT-1 - index) #define MOTOR_ID(n) (n + START_ID) // motors' status diff --git a/BTA_dome_modbus/run_client b/BTA_dome_modbus/run_client index e531b74..66ae3e5 100755 --- a/BTA_dome_modbus/run_client +++ b/BTA_dome_modbus/run_client @@ -1,3 +1,3 @@ #!/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 $@ diff --git a/BTA_dome_modbus/run_server b/BTA_dome_modbus/run_server index 80e0bd4..63a461a 100755 --- a/BTA_dome_modbus/run_server +++ b/BTA_dome_modbus/run_server @@ -1,3 +1,3 @@ #!/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 $@ diff --git a/BTA_dome_modbus/server.c b/BTA_dome_modbus/server.c index 8219633..095858c 100644 --- a/BTA_dome_modbus/server.c +++ b/BTA_dome_modbus/server.c @@ -27,6 +27,9 @@ // 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]); +// == TRUE if dome management is forbidden +static int forbidden = FALSE; + // struct for setters/getters typedef struct{ const char *command; @@ -70,7 +73,7 @@ static int handle_connection(SSL *ssl){ int r = read_string(ssl, buf, IOBUF_LEN); if(r < 0) return 0; 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); int got = sl_get_keyval(buf, key, val); if(got == 0){ @@ -81,11 +84,11 @@ static int handle_connection(SSL *ssl){ sl_sock_hresult_e result = RESULT_BADKEY; if(-1 != h_idx){ if(got == 1){ - DBG("getter #%d", h_idx); + //DBG("getter #%d", h_idx); 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); - DBG("result: %d", result); + //DBG("result: %d", result); }else{ DBG("Command not found or help?"); // check if user asks for help @@ -186,6 +189,8 @@ void serverproc(SSL_CTX *ctx, int fd){ WARNX("SSL_accept()"); SSL_free(ssl); send(client, sslerr, sizeof(sslerr)-1, MSG_NOSIGNAL); + shutdown(client, SHUT_WR); + usleep(50000); 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) ******************/ +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) /* 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]){ double D; if(ISSETTER(value)){ + if(forbidden) return RESULT_FAIL; if(!sl_str2d(&D, value) || !motors_set_speedsetpoint(D)) return RESULT_BADVAL; return RESULT_OK; } @@ -289,7 +305,7 @@ static int search_handler(const char *name){ // Compare the target string with the struct's string field int res = strcmp(name, command_list[mid].command); 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 }else if(res < 0){ high = mid - 1; // Target is smaller, search left half diff --git a/BTA_dome_modbus/sslsosk.files b/BTA_dome_modbus/sslsosk.files index 1e686e7..8d6aafc 100644 --- a/BTA_dome_modbus/sslsosk.files +++ b/BTA_dome_modbus/sslsosk.files @@ -6,6 +6,7 @@ cmdlnopts.c cmdlnopts.h daemon.c daemon.h +esq770.h handlers_list.h main.c motors.c