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

some fixes

Browse Source
This commit is contained in:
Edward Emelianov
2026-09-03 16:22:24 +03:00
parent ae999fcd48
commit d2463f759b
8 changed files with 154 additions and 112 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
BTA_dome_modbus/bta_shdata.c
View File

@@ -1,6 +1,10 @@
// (C) V.S. Shergin, SAO RAS
#include <err.h>
#include <crypt.h>
#include <errno.h>
#include <stdio.h>
#include <string.h>
#include "bta_shdata.h"
#pragma pack(push, 4)
@@ -15,16 +19,6 @@ struct CMD_Queue ucmd = {{"Ucmd"}, 0200,0,-1,0};
static char msg[MSGLEN];
#define WARN(...) warn(__VA_ARGS__)
#define PERR(...) do{snprintf(msg, MSGLEN, __VA_ARGS__); perror(msg);} while(0)
#ifdef EBUG
#define FNAME() fprintf(stderr, "\n%s (%s, line %d)\n", __func__, __FILE__, __LINE__)
#define DBG(...) do{fprintf(stderr, "%s (%s, line %d): ", __func__, __FILE__, __LINE__); \
fprintf(stderr, __VA_ARGS__); \
fprintf(stderr, "\n");} while(0)
#else
#define FNAME() do{}while(0)
#define DBG(...) do{}while(0)
#endif //EBUG
#ifndef BTA_MODULE
volatile struct BTA_Data *sdt;
@@ -141,7 +135,7 @@ int get_shm_block(volatile struct SHM_Block *sb, int server) {
PERR("Can't prevents swapping of shared memory segment '%s'",sb->key.name);
return 0;
}
DBG("Create & attach shared memory segment '%s' %dbytes", sb->key.name, sb->size);
fprintf(stderr, "Create & attach shared memory segment '%s' %dbytes", sb->key.name, sb->size);
sb->side = server;
if(sb->init != NULL)
sb->init();

141
BTA_dome_modbus/bta_shdata.h
View File

@@ -1,17 +1,11 @@
// (C) V.S. Shergin, SAO RAS
#pragma once
#ifndef __BTA_SHDATA_H__
#define __BTA_SHDATA_H__
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <stdint.h>
#include <string.h>
#include <sys/ipc.h>
#include <sys/shm.h>
#include <sys/msg.h>
#include <errno.h>
#pragma pack(push, 4)
/*
@@ -41,7 +35,7 @@ extern volatile struct SHM_Block sdat;
*/
struct CMD_Queue {
union {
char name[5]; // queue key
char name[5]; // queue key
key_t code;
} key;
int32_t mode; // access mode (rwxrwxrwx)
@@ -318,7 +312,7 @@ enum{
,Balance =0x10// balancing
};
// az_mode
#define Az_Mode (sdt->az_mode) // azimuth reverce
#define Az_Mode (sdt->az_mode) // azimuth reverse
enum{
Rev_Off = 0 // move by nearest way
,Rev_On // move by longest way
@@ -425,12 +419,12 @@ enum{
#define mod_vel_D (sdt->simvelf)
// telescope & hand correction state
/*
* 0x8000 - ÁÚÉÍÕÔ ÐÏÌÏÖÉÔÅÌØÎÙÊ
* 0x8000 - Az > 0
* 0x4000 - ÏÔÒÁÂÏÔËÁ ×ËÌ.
* 0x2000 - ÒÅÖÉÍ ×ÅÄÅÎÉÑ
* 0x1000 - ÏÔÒÁÂÏÔËÁ P2 ×ËÌ.
* 0x01F0 - ÓË.ËÏÒÒ. 0.2 0.4 1.0 2.0 5.0("/ÓÅË)
* 0x000F - ÎÁÐÒ.ËÏÒÒ. +Z -Z +A -A
* 0x2000 - tracking mode
* 0x1000 - P2 auto on
* 0x01F0 - corr. speeds 0.2 0.4 1.0 2.0 5.0("/sec)
* 0x000F - corr. +Z -Z +A -A
*/
#define code_KOST (sdt->kost)
// different time (UTC, stellar, local)
@@ -698,69 +692,69 @@ struct BTA_Data {
int32_t magic; // magic value
int32_t version; // BTA_Data_Ver
int32_t size; // sizeof(struct BTA_Data)
int32_t pid; // main process PID
int32_t model; // model modes
int32_t timer; // timer selected
int32_t system; // main system mode
int32_t sys_target; // system pointing target
int32_t tel_focus; // telescope focus type
double pc_coeff[8]; // pointing correction system coefficients
int32_t tel_state; // telescope state
int32_t req_state; // new (required) state
int32_t tel_hard_state; // Power state
int32_t tel_mode; // telescope mode
int32_t az_mode; // azimuth reverce
int32_t p2_state; // P2 motor state
int32_t p2_req_mode; // P2 required state
int32_t focus_state; // focus motor state
int32_t dome_state; // dome motors state
int32_t pcor_mode; // pointing correction mode
int32_t trkok_mode; // tracking mode
double i_alpha, i_delta; // input values
double s_alpha, s_delta; // source
double v_alpha, v_delta; // intrinsic vel.
double i_azim, i_zdist; // input A/Z
double c_alpha, c_delta; // calculated values
double tag_a, tag_z, tag_p; // current values (from sensors)
double pcor_a, pcor_z, refr_z; // calculated corrections
double tcor_a, tcor_z, tref_z; // reverse calculation corr.
double diff_a, diff_z, diff_p; // coords difference
double vbasea,vbasez,vbasep; // base object velocity
double diffva,diffvz,diffvp; // correction by real speed
double speeda,speedz,speedp; // motor speed
double m_time_precip; // last precipitation time
uint8_t reserve[16]; // reserved
double rspeeda, rspeedz, rspeedp; // real motor speed (''/sec)
double simvela, simvelz, simvelp, simvelf, simveld; // model speed
uint32_t kost; // telescope & hand correction state
double m_time, s_time, l_time; // different time (UTC, stellar, local)
uint32_t ppndd_a, ppndd_z, ppndd_p, ppndd_b; // PPNDD sensor (rough) code
uint32_t dup_a, dup_z, dup_p, dup_f, dup_d; // DUP sensor (precise) code (Gray code)
uint32_t low_a, low_z, low_p, low_f, low_d; // binary 14-digit precise code
uint32_t code_a, code_z, code_p, code_b, code_f, code_d; // binary 23-digit rough code
uint32_t adc[8]; // ADC PCL818 (8-channel) codes
double val_a, val_z, val_p, val_b, val_f, val_d;
double val_t1, val_t2, val_t3, val_wnd; // calculated values
double val_alp, val_del; // RA/Decl calculated by A/Z
double vel_a, vel_z, vel_p, vel_f, vel_d; // measured speed
int32_t pid; // main process PID [ServPID]
int32_t model; // model modes [UseModel]
int32_t timer; // timer selected [ClockType]
int32_t system; // main system mode [Sys_Mode]
int32_t sys_target; // system pointing target [Sys_Target]
int32_t tel_focus; // telescope focus type [Tel_Focus]
double pc_coeff[8]; // pointing correction system coefficients [PosCor_Coeff]
int32_t tel_state; // telescope state [Tel_State]
int32_t req_state; // new (required) state [Req_State]
int32_t tel_hard_state; // Power state [Tel_Hardware]
int32_t tel_mode; // telescope mode [Tel_Mode]
int32_t az_mode; // azimuth reverse [Az_Mode]
int32_t p2_state; // P2 motor state [P2_State]
int32_t p2_req_mode; // P2 required state [P2_Mode]
int32_t focus_state; // focus motor state [Foc_State]
int32_t dome_state; // dome motors state [Dome_State]
int32_t pcor_mode; // pointing correction mode [Pos_Corr]
int32_t trkok_mode; // tracking mode [TrkOk_Mode]
double i_alpha, i_delta; // input values [InpAlpha, InpDelta]
double s_alpha, s_delta; // source [SrcAlpha, SrcDelta]
double v_alpha, v_delta; // intrinsic vel. [VelAlpha, VelDelta]
double i_azim, i_zdist; // input A/Z [InpAzim, InpZdist]
double c_alpha, c_delta; // calculated values [CurAlpha, CurDelta]
double tag_a, tag_z, tag_p; // current values (from sensors) [tag_A, tag_Z, tag_P]
double pcor_a, pcor_z, refr_z; // calculated corrections [pos_cor_A, pos_cor_Z, refract_Z]
double tcor_a, tcor_z, tref_z; // reverse calculation corr. [tel_cor_A, tel_cor_Z, tel_ref_Z]
double diff_a, diff_z, diff_p; // coords difference [Diff_A, Diff_Z, Diff_P]
double vbasea,vbasez,vbasep; // base object velocity [vel_objA, vel_objZ, vel_objP]
double diffva,diffvz,diffvp; // correction by real speed [diff_vA, diff_vZ, diff_vP]
double speeda,speedz,speedp; // motor speed [speedA, speedZ, speedP]
double m_time_precip; // last precipitation time [Precip_time]
uint8_t reserve[16]; // reserved [Reserve]
double rspeeda, rspeedz, rspeedp; // real motor speed (''/sec) [req_speedA, req_speedZ, req_speedP]
double simvela, simvelz, simvelp, simvelf, simveld; // model speed [mod_vel_A, mod_vel_Z, mod_vel_P, mod_vel_F, mod_vel_D]
uint32_t kost; // telescope & hand correction state [code_KOST]
double m_time, s_time, l_time; // different time (UTC, stellar, local) [M_time, S_time, L_time]
uint32_t ppndd_a, ppndd_z, ppndd_p, ppndd_b; // PPNDD sensor (rough) code [ppndd_A, ppndd_Z, ppndd_P, ppndd_B (pressure)]
uint32_t dup_a, dup_z, dup_p, dup_f, dup_d; // DUP sensor (precise) code (Gray code) [dup_A, dup_Z, dup_P, dup_F, dup_D]
uint32_t low_a, low_z, low_p, low_f, low_d; // binary 14-digit precise code [low_A, low_Z, low_P, low_F, low_D]
uint32_t code_a, code_z, code_p, code_b, code_f, code_d; // binary 23-digit rough code [code_A, code_Z, code_P, code_B, code_F, code_D]
uint32_t adc[8]; // ADC PCL818 (8-channel) codes [ADC(N): code_T1, code_T2, code_T3, code_Wnd]
double val_a, val_z, val_p, val_b, val_f, val_d; // calculated values [val_A, val_Z, val_P, val_B, val_F, val_D]
double val_t1, val_t2, val_t3, val_wnd; // calculated values [val_T1, val_T2, val_T3, val_Wnd]
double val_alp, val_del; // RA/Decl calculated by A/Z [val_Alp, val_Del]
double vel_a, vel_z, vel_p, vel_f, vel_d; // measured speed [vel_A, vel_Z, vel_P, vel_F, vel_D]
// system messages queue
struct SysMesg {
int32_t seq_num;
char type; // message type
char text[MesgLen]; // message itself
} sys_msg_buf[MesgNum];
} sys_msg_buf[MesgNum]; // Sys_Mesg(N)
// access levels
uint32_t code_lev[5];
uint32_t code_lev[5]; // [code_Lev(x): code_Lev1, code_Lev2, code_Lev3, code_Lev4, code_Lev5]
// network settings
uint32_t netmask, netaddr, acsmask, acsaddr;
int32_t meteo_stat; // meteo data
double inp_b, inp_t1, inp_t2, inp_t3, inp_wnd; // input meteo values
double temper, press; // values used for refraction calculation
double m_time10, m_time15; // last wind gust time
double dut1; // IERS DUT1 (src: ftp://maia.usno.navy.mil/ser7/ser7.dat), DUT1 = UT1-UTC
double a_time, z_time, p_time; // sensors reading time
double speedain, speedzin, speedpin; // input speeds
double acc_a, acc_z, acc_p, acc_f, acc_d; // acceleration (''/sec^2)
uint32_t netmask, netaddr, acsmask, acsaddr; // NetMask, NetWork, ACSMask, ACSNet]
int32_t meteo_stat; // meteo data [MeteoMode]
double inp_b, inp_t1, inp_t2, inp_t3, inp_wnd; // input meteo values [inp_B, inp_T1, inp_T2, inp_T3, inp_Wnd]
double temper, press; // values used for refraction calculation [Temper, Pressure]
double m_time10, m_time15; // last wind gust time [Wnd10_time, Wnd15_time]
double dut1; // IERS DUT1 (src: ftp://maia.usno.navy.mil/ser7/ser7.dat), DUT1 = UT1-UTC [DUT1]
double a_time, z_time, p_time; // sensors reading time [A_time, Z_time, P_time]
double speedain, speedzin, speedpin; // input speeds [speedAin, speedZin, speedPin]
double acc_a, acc_z, acc_p, acc_f, acc_d; // acceleration (''/sec^2) [acc_A, acc_Z, acc_P, acc_F, acc_D]
uint32_t code_sew; // SEW code
struct SEWdata { // sew data
int32_t status;
@@ -781,7 +775,6 @@ struct BTA_Data {
double jdate, eetime; // current Julian date, sidereal time correction by "Equation of the Equinoxes"
double val_hmd, inp_hmd; // humidity value (%%) & hand input
double worm_a, worm_z; // worm position, mkm
/* ÆÌÁÇÉ ÂÌÏËÉÒÏ×ËÉ ÕÐÒÁ×ÌÅÎÉÑ ÕÚÌÁÍÉ */
uint32_t lock_flags; // locking flags
int32_t sew_dome_speed; // SEW dome divers speed: D_Lplus, D_Hminus etc
int32_t sew_dome_num; // SEW dome drive number (for indication)
@@ -804,9 +797,9 @@ extern volatile struct BTA_Data *sdt;
// Local data structure
struct BTA_Local {
uint8_t reserve[120]; // reserved data
double pr_oil_a,pr_oil_z,pr_oil_t; // Oil pressure
double t_oil_1,t_oil_2; // Oil themperature
uint8_t reserve[120]; // reserved data
double pr_oil_a,pr_oil_z,pr_oil_t; // Oil pressure
double t_oil_1,t_oil_2; // Oil themperature
};
/**
@@ -846,6 +839,4 @@ void set_acckey(uint32_t newkey);
// restore packing
#pragma pack(pop)
//#pragma GCC diagnostic pop
#endif // __BTA_SHDATA_H__

4
BTA_dome_modbus/client.c
View File

@@ -31,7 +31,7 @@
#define NEW_HANDLER(name, unused) \
static const char* cmd_ ## name = STR(name);
static const char* _U_ cmd_ ## name = STR(name);
HANDLERS_LIST()
#undef NEW_HANDLER
@@ -310,8 +310,10 @@ static int process_system(SSL *ssl){
#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

5
BTA_dome_modbus/esq770.h
View File

@@ -52,6 +52,9 @@
// REG_STATUS_EXT bit
#define STATUS_READY_MASK 0x01
#define FREQ_SCALE 100.
// convert rev/min to frequency register value and back
// 100 * revmin / 60 * 4; 100 - scale, 60 - min2sec, 4 - pole amount
#define REVMIN2FREQ(r) (uint16_t)((100. * (r)) / 15.)
#define FREQ2REVMIN(f) ((((double)(f)) * 15.) / 100.)
#define CURRENT_SCALE 10.

1
BTA_dome_modbus/handlers_list.h
View File

@@ -30,6 +30,7 @@ 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(stop, "stop motors") \

82
BTA_dome_modbus/motors.c
View File

@@ -18,7 +18,6 @@
#include <math.h>
#include <modbus/modbus.h>
#include <usefull_macros.h>
#include "motors.h"
#include "esq770.h"
@@ -38,6 +37,8 @@ current=xx - (sg)
static modbus_t *modbus_ctx = NULL;
static motor_state_t motstates[MOTORS_AMOUNT] = {0};
// amount of working motors
static int n_working_motors = 0;
// set points
static double currentSet = DEFAULT_CURRENT, speedSet = 0.;
@@ -122,14 +123,15 @@ int motors_set_curntsetpoint(double val){
double motors_get_speedsetpoint(){
return speedSet;
}
// set setpoint of speed
int motors_set_speedsetpoint(double val){
// set setpoint of speed (rev/min)
sl_sock_hresult_e motors_set_speedsetpoint(double val){
double absval = fabs(val);
if(absval > MAX_SPEED) return FALSE;
if(absval > MAX_SPEED) return RESULT_BADVAL;
if(n_working_motors < MIN_WORKING_MOTORS) return RESULT_FAIL;
DBG("Change speed setpoint to %g", val);
speedSet = val;
flags.change_speed = 1;
return TRUE;
return RESULT_OK;
}
// get number of active motor
@@ -160,21 +162,32 @@ int motors_get_actstatus(int *val){
return TRUE;
}
// count amount of actual motors
static void count_motors(){
n_working_motors = 0;
for(int i = 0; i < MOTORS_AMOUNT; ++i){
if(motstates[i].status != MOT_OFF) ++n_working_motors;
}
}
// main motors processing routine
static void motors_process_m(){
static int curN = 0, errctr = 0;
static int error_cnt[MOTORS_AMOUNT] = {0};
if(!modbus_ctx || errctr > 100){
LOGERR("Modbus not inited or other error!");
LOGERR("Modbus not inited or other error! Error counter = %d", errctr);
ERRX("Modbus not inited or other error!");
}
if(flags.all){
if(-1 == modbus_set_slave(modbus_ctx, 0)) goto reg_error;
if(-1 == modbus_set_slave(modbus_ctx, 0)){
WARN("Can't set broadcast slave");
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;
// send command stop (don't check return code for broadcast messages!
modbus_write_register(modbus_ctx, REG_CMD, CMD_STOP);
flags.stop = 0;
}
if(flags.change_current){
@@ -186,43 +199,68 @@ static void motors_process_m(){
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_CMD, dir)) goto reg_error;
if(-1 == modbus_write_register(modbus_ctx, REG_FREQ_SET, freq)) goto reg_error;
uint16_t freq = REVMIN2FREQ(fabs(speedSet));
modbus_write_register(modbus_ctx, REG_CMD, dir);
modbus_write_register(modbus_ctx, REG_FREQ_SET, freq);
flags.change_speed = 0;
}
}
// set slave N
if(-1 == modbus_set_slave(modbus_ctx, MOTOR_ID(curN))) goto reg_error;
if(-1 == modbus_set_slave(modbus_ctx, MOTOR_ID(curN))){
WARN("Can't set slave %d", 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)){
if(-1 == modbus_read_registers(modbus_ctx, REG_STATUS_MAIN, 1, regs) ||
-1 == modbus_read_registers(modbus_ctx, REG_STATUS_EXT, 1, &regs[1])){
// WARN("err in # %d (errno=%d)", curN, errno);
if((++error_cnt[curN]) > MAX_ERRORS){ // not answer - set MOT_OFF status
motstates[curN].status = MOT_OFF;
LOGDBG("Motor %d not responce", curN);
if(motstates[curN].status != MOT_OFF){
motstates[curN].status = MOT_OFF;
LOGDBG("Motor %d not responce", curN);
DBG("Motor %d not responce", curN);
}
error_cnt[curN] = 0;
}
if(++curN >= MOTORS_AMOUNT){
curN = 0;
count_motors();
}
if(++curN >= MOTORS_AMOUNT) curN = 0;
return;
}
DBG("motor %d; status: %x, extstatus: %x", curN, regs[0], regs[1]);
error_cnt[curN] = 0;
switch(regs[0]){
case STATUS_CW:
case STATUS_CCW:
DBG("status: RUN");
motstates[curN].status = MOT_RUN;
break;
case STATUS_STOP:
DBG("status: STOP");
motstates[curN].status = MOT_SLEEP;
break;
default:
DBG("status: ERROR");
motstates[curN].status = MOT_ERROR;
}
if(regs[1] & STATUS_READY_MASK) motstates[curN].status = MOT_ERROR;
if(motstates[curN].status == MOT_ERROR) modbus_write_register(modbus_ctx, REG_CMD, CMD_RESET_FAULT);
if(regs[1] & STATUS_READY_MASK){
DBG("Motor not ready");
motstates[curN].status = MOT_ERROR;
}
if(motstates[curN].status == MOT_ERROR){
if(-1 == modbus_write_register(modbus_ctx, REG_CMD, CMD_RESET_FAULT))
WARN("Can't reset fault");
}
if(-1 == modbus_read_registers(modbus_ctx, REG_OUTPUT_FREQ, 1, &regs[0])) regs[0] = 0;
if(-1 == modbus_read_registers(modbus_ctx, REG_OUTPUT_CURRENT, 1, &regs[1])) regs[1] = 0;
motstates[curN].speed = ((double)regs[0]) / FREQ_SCALE;
motstates[curN].speed = FREQ2REVMIN(regs[0]);
motstates[curN].current = ((double)regs[1]) / CURRENT_SCALE;
if(++curN >= MOTORS_AMOUNT) curN = 0;
if(++curN >= MOTORS_AMOUNT){
curN = 0;
count_motors();
}
errctr = 0;
return;
reg_error:
@@ -279,3 +317,5 @@ void set_emulation_mode(){
motors_close = motors_close_e;
motors_process = motors_process_e;
}
int motors_get_working_amount(){ return n_working_motors; }

8
BTA_dome_modbus/motors.h
View File

@@ -18,6 +18,11 @@
#pragma once
#include <usefull_macros.h>
// minimal amount of working motors to spin dome
#define MIN_WORKING_MOTORS 4
// max errors per motor to mean it OFF
#define MAX_ERRORS 5
@@ -60,7 +65,7 @@ double motors_get_curntsetpoint();
int motors_set_curntsetpoint(double);
double motors_get_speedsetpoint();
int motors_set_speedsetpoint(double);
sl_sock_hresult_e motors_set_speedsetpoint(double);
int motors_get_activenum();
int motors_set_activenum(int);
@@ -69,6 +74,7 @@ void motors_stop();
int motors_get_actcurrent(double*);
int motors_get_actspeed(double*);
int motors_get_actstatus(int*);
int motors_get_working_amount();
extern void (*motors_process)();

9
BTA_dome_modbus/server.c
View File

@@ -281,8 +281,8 @@ 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;
if(!sl_str2d(&D, value)) return RESULT_BADVAL;
return motors_set_speedsetpoint(D);
}
snprintf(value, SL_VAL_LEN-1, "%.3f", motors_get_speedsetpoint());
return RESULT_SILENCE;
@@ -293,6 +293,11 @@ sl_sock_hresult_e stop_handler(int _U_ index, char _U_ value[SL_VAL_LEN]){
return RESULT_OK;
}
sl_sock_hresult_e nmotors_handler(int _U_ index, char _U_ value[SL_VAL_LEN]){
snprintf(value, SL_VAL_LEN-1, "%d", motors_get_working_amount());
return RESULT_SILENCE;
}
// binary search handler by name
static int search_handler(const char *name){
int low = 0;