From 7c247e1f8f8f1766e1ee60465a69e0565eaa511f Mon Sep 17 00:00:00 2001 From: eddyem Date: Tue, 24 Nov 2015 21:26:33 +0300 Subject: [PATCH] Tested, beta-stage --- angle_functions.c | 62 ++++++++++++++++++++++++++ angle_functions.h | 5 +++ bta_control.c | 111 +++++++++++++++++++++++++++++++--------------- bta_control.h | 16 +++++-- bta_print.c | 68 +--------------------------- bta_shdata.c | 8 ++-- cmdlnopts.c | 1 + 7 files changed, 162 insertions(+), 109 deletions(-) diff --git a/angle_functions.c b/angle_functions.c index 5a7dda7..c94cdac 100644 --- a/angle_functions.c +++ b/angle_functions.c @@ -32,6 +32,8 @@ #include "angle_functions.h" #include "usefull_macros.h" +static char buf[BUFSZ+1]; + extern void sla_caldj(int*, int*, int*, double*, int*); extern void sla_amp(double*, double*, double*, double*, double*, double*); extern void sla_map(double*, double*, double*, double*, double*,double*, double*, double*, double*, double*); @@ -268,3 +270,63 @@ baddate: return FALSE; } +char *time_asc(double t){ + int h, min; + double sec; + h = (int)(t/3600.); + min = (int)((t - (double)h*3600.)/60.); + sec = t - (double)h*3600. - (double)min*60.; + h %= 24; + if(sec>59.99) sec=59.99; + snprintf(buf, BUFSZ, "%02d:%02d:%05.2f", h,min,sec); + return buf; +} + +char *angle_asc(double a){ + char s; + int d, min; + double sec; + if (a >= 0.) + s = '+'; + else { + s = '-'; a = -a; + } + d = (int)(a/3600.); + min = (int)((a - (double)d*3600.)/60.); + sec = a - (double)d*3600. - (double)min*60.; + d %= 360; + if(sec>59.9) sec=59.9; + snprintf (buf, BUFSZ, "%c%02d:%02d:%04.1f", s,d,min,sec); + return buf; +} + +char *angle_fmt(double a, char *format){ + char s, *p; + int d, min, n; + double sec, msec; + char *newformat = MALLOC(char, strlen(format) + 3); + sprintf(newformat, "%s", format); + if (a >= 0.) + s = '+'; + else { + s = '-'; a = -a; + } + d = (int)(a/3600.); + min = (int)((a - (double)d*3600.)/60.); + sec = a - (double)d*3600. - (double)min*60.; + d %= 360; + if ((p = strchr(format,'.')) == NULL) + msec=59.; + else if (*(p+2) == 'f' ) { + n = *(p+1) - '0'; + msec = 60. - pow(10.,(double)(-n)); + } else + msec=60.; + if(sec>msec) sec=msec; + if (strstr(format,"%c")) + snprintf(buf, BUFSZ, newformat, s,d,min,sec); + else + snprintf(buf, BUFSZ, newformat, d,min,sec); + free(newformat); + return buf; +} diff --git a/angle_functions.h b/angle_functions.h index ca494e8..3822f0b 100644 --- a/angle_functions.h +++ b/angle_functions.h @@ -24,6 +24,8 @@ #include +#define BUFSZ 255 + #ifndef TRUE #define TRUE true #endif @@ -32,6 +34,9 @@ #define FALSE false #endif +char *time_asc(double t); +char *angle_asc(double a); +char *angle_fmt(double a, char *format); double sec_to_degr(double sec); char *get_degrees(double *ret, char *str); bool calc_AP(double r, double d, double *appRA, double *appDecl); diff --git a/bta_control.c b/bta_control.c index 0f0e1a2..1875a88 100644 --- a/bta_control.c +++ b/bta_control.c @@ -52,9 +52,9 @@ glob_pars *GP = NULL; #ifdef EMULATION #define ACS_CMD(a) do{green(#a); printf("\n");}while(0) #else -#define ACS_CMD(a) do{red(#a); printf("\n");}while(0) +//#define ACS_CMD(a) do{red(#a); printf("\n");}while(0) // Uncomment only in final release -// #define ACS_CMD(a) do{DBG(#a "\n"); a; }while(0) +#define ACS_CMD(a) do{DBG(#a "\n"); a; }while(0) #endif @@ -120,11 +120,10 @@ void set_timeout(int delay){ char indi[] = "|/-\\"; char *iptr = indi; #define WAIT_EVENT(evt, max_delay) do{int __ = 0; set_timeout(max_delay); \ - PRINT(" "); while(!tmout && !evt){\ + PRINT(" "); while(!tmout && !(evt)){ \ usleep(100000); if(!*(++iptr)) iptr = indi; if(++__%10==0) PRINT("\b. "); \ PRINT("\b%c", *iptr);}; PRINT("\n");}while(0) - #ifndef EMULATION void get_passhash(passhash *p){ int fd = -1, i, c, nlev = 0; @@ -198,21 +197,33 @@ void get_passhash(passhash *p){ * move P2 to the given angle relative to current position +- P2_ANGLE_THRES */ void cmd_P2moveto(double p2shift){ - double p2vel = 45.*60., p2dt, p2mintime = 4.5, p2secs = fabs(p2shift) * 3600.; + double p2vel = 45.*60., p2dt, p2mintime = P2_MINTIME, p2secs = fabs(p2shift) * 3600.; if(fabs(p2shift) < P2_ANGLE_THRES) return; p2dt = p2secs / p2vel; if(p2dt < p2mintime){ p2vel = p2secs / p2mintime; - if(p2vel < 1.) p2vel = 1; + if(p2vel < 1.) p2vel = 1.; p2dt = p2secs / p2vel; } + // if speed is too fast, make dt less + if(p2vel > P2_FAST_SPEED && p2dt > P2_FAST_T_CORR + P2_MINTIME){ + p2dt -= P2_FAST_T_CORR; + } if(p2shift < 0) p2vel = -p2vel; - DBG("p2vel=%g, p2dt = %g", p2vel, p2dt); + DBG("p2vel=%g, p2dt = %g, p2_val=%s", p2vel, p2dt, angle_asc(val_P)); ACS_CMD(MoveP2To(p2vel, p2dt)); #ifndef EMULATION + PRINT(_("Wait for starting")); + WAIT_EVENT(((fabs(vel_P) > 1.) && (P2_State != P2_Off)), WAITING_TMOUT); + if((fabs(vel_P) < 1.) || (P2_State == P2_Off)){ + DBG("vel: %g, state: %d", vel_P, P2_State); + WARNX(_("P2 didn't start!")); + return; + } PRINT(_("Moving P2 ")); // wait until P2 stops, set to guiding or timeout ends - WAIT_EVENT(((fabs(vel_P) < 1.) || (P2_State == P2_On)), p2dt + 1.); + WAIT_EVENT(((fabs(vel_P) < 1.) && (P2_State == P2_Off)), p2dt + 1.); + DBG("P2 state: %d, vel_P: %g, p2_val=%s", P2_State, vel_P, angle_asc(val_P)); if(tmout && P2_State != P2_Off){ WARNX(_("Timeout reached, stop P2")); ACS_CMD(MoveP2(0)); @@ -276,7 +287,7 @@ bdrg: #ifndef EMULATION if(P2_State != P2_Off){ PRINT(_("Wait for P2 stop ")); - WAIT_EVENT(P2_State == P2_Off, 5); + WAIT_EVENT(P2_State == P2_Off, WAITING_TMOUT); if(tmout && P2_State != P2_Off){ WARNX(_("Timeout reached, can't stop P2")); return 0; @@ -284,13 +295,15 @@ bdrg: } #endif } + int p2oldmode = P2_Mode; + ACS_CMD(SetPMode(P2_Off)); DBG("Move P2 to %gdegr", p2angle); if(fabs(p2angle - p2val) < P2_ANGLE_THRES){ WARNX(_("Zero moving (< %g)"), P2_ANGLE_THRES); return TRUE; } int i; - for(i = 0; i < 3; ++i){ + for(i = 0; i < 5; ++i){ if(i) PRINT(_("Try %d. "), i+1); cmd_P2moveto(p2angle - p2val); p2val = sec_to_degr(val_P); @@ -300,6 +313,8 @@ bdrg: WARNX(_("Error moving P2: have %gdegr, need %gdegr"), p2val, p2angle); return FALSE; } + PRINT(_("All OK, current P2 value: %s\n"), angle_asc(val_P)); + ACS_CMD(SetPMode(p2oldmode)); return TRUE; } @@ -317,7 +332,7 @@ bool setP2mode(char *arg){ #ifndef EMULATION if(P2_State != mode){ PRINT(_("Wait for given mode ")); - WAIT_EVENT(P2_State == mode, 5); + WAIT_EVENT(P2_State == mode, WAITING_TMOUT); if(tmout && P2_State != mode){ WARNX(_("Timeout reached, can't set P2 mode")); return FALSE; @@ -334,13 +349,13 @@ void cmd_Fmoveto(double f){ const double FOC_HVEL = 0.63, FOC_LVEL = 0.13; int _U_ fspeed; if(f < 1. || f > 199.) return; - double fshift = f - val_F, fvel, _U_ fdt; + double fshift = f - val_F, fvel, fdt; if(fabs(fshift) > 1.){ fvel = FOC_HVEL; - fspeed = (fshift > 0) ? Foc_Hplus : Foc_Hminus; - }else if(fabs(fshift) > 0.05){ + fspeed = (fshift > 0.) ? Foc_Hplus : Foc_Hminus; + }else if(fabs(fshift) > FOCUS_THRES){ fvel = FOC_LVEL; - fspeed = (fshift > 0) ? Foc_Lplus : Foc_Lminus; + fspeed = (fshift > 0.) ? Foc_Lplus : Foc_Lminus; } else{ WARNX(_("Can't move for such small distance (%gmm)"), fshift); return; @@ -348,8 +363,15 @@ void cmd_Fmoveto(double f){ fdt = fabs(fshift) / fvel; ACS_CMD(MoveFocus(fspeed, fdt)); #ifndef EMULATION + DBG("dt: %g, fvel: %g, fstate: %d, F:%g", fdt, vel_F, Foc_State, val_F); + PRINT(_("Wait for starting")); + WAIT_EVENT((Foc_State != Foc_Off || fabs(vel_F) > 0.01), WAITING_TMOUT); PRINT(_("Moving Focus ")); - WAIT_EVENT((fabs(vel_F) < 0.01 || Foc_State == Foc_Off), fdt + 1.); + WAIT_EVENT((fabs(vel_F) < 0.01 && Foc_State == Foc_Off), fdt + 1.); + DBG("fvel: %g, fstate: %d, F:%g", vel_F, Foc_State, val_F); + PRINT(_("Wait for stop")); + WAIT_EVENT((Foc_State == Foc_Off && fabs(vel_F) < 0.01), WAITING_TMOUT); + DBG("fvel: %g, fstate: %d, F:%g", vel_F, Foc_State, val_F); if(tmout && Foc_State != Foc_Off){ WARNX(_("Timeout reached, stop focus")); ACS_CMD(MoveFocus(Foc_Off, 0.)); @@ -374,7 +396,7 @@ bool moveFocus(double val){ #ifndef EMULATION if(Foc_State != Foc_Off){ PRINT(_("Wait for focus stop ")); - WAIT_EVENT(Foc_State == Foc_Off, 3); + WAIT_EVENT(Foc_State == Foc_Off, WAITING_TMOUT); if(tmout && Foc_State != Foc_Off){ WARNX(_("Timeout reached, can't stop focus motor")); return FALSE; @@ -443,12 +465,17 @@ bool setCoords(char *coords, bool isEQ){ // calculate apparent place according to other cmdline arguments if(!calc_AP(r, d, &appRA, &appDecl)) goto badcrds; DBG("Set RA/Decl to %g, %g", appRA/3600, appDecl/3600); + r = InpAlpha, d = InpDelta; // save old coordinates ACS_CMD(SetRADec(appRA, appDecl)); #ifndef EMULATION - if(InpAlpha != r || InpDelta != d){ + DBG("InpAlpha = %g, InpDelta = %g, was: A = %g, D = %g", + InpAlpha, InpDelta, r, d); + // now check whether old coordinates was changed + if(fabs(InpAlpha - r) > INPUT_COORDS_THRES || fabs(InpDelta - d) > INPUT_COORDS_THRES){ PRINT(_("Wait for command result")); - WAIT_EVENT((InpAlpha == r && InpDelta == d), 3); - if(InpAlpha != r || InpDelta != d){ + WAIT_EVENT((fabs(InpAlpha - r) < INPUT_COORDS_THRES && + fabs(InpDelta - d) < INPUT_COORDS_THRES), WAITING_TMOUT); + if(tmout){ WARNX(_("Can't send data to system!")); return FALSE; } @@ -462,10 +489,11 @@ bool setCoords(char *coords, bool isEQ){ DBG("Set A/Z to %g, %g", r/3600, d/3600); ACS_CMD(SetAzimZ(r, d)); #ifndef EMULATION - if(InpAzim != r || InpZdist != d){ + if(fabs(InpAzim - r) > INPUT_COORDS_THRES || fabs(InpZdist - d) > INPUT_COORDS_THRES){ PRINT(_("Wait for command result")); - WAIT_EVENT((InpAzim == r && InpZdist == d), 3); - if(InpAzim != r || InpZdist != d){ + WAIT_EVENT((fabs(InpAzim - r) < INPUT_COORDS_THRES && + fabs(InpZdist - d) < INPUT_COORDS_THRES), WAITING_TMOUT); + if(tmout){ WARNX(_("Can't send data to system!")); return FALSE; } @@ -487,12 +515,13 @@ badcrds: bool azreverce(){ bool ret = TRUE; int mode = Az_Mode; + DBG("mode: %d", mode); if(mode == Rev_Off) mode = Rev_On; - else mode = Rev_On; + else mode = Rev_Off; ACS_CMD(SetAzRevers(mode)); PRINT(_("Turn %s azimuth reverce "), (mode == Rev_Off) ? "off" : "on"); #ifndef EMULATION - WAIT_EVENT((Az_Mode == mode), 3); + WAIT_EVENT((Az_Mode == mode), WAITING_TMOUT); if(Az_Mode != mode) ret = FALSE; #endif return ret; @@ -500,7 +529,7 @@ bool azreverce(){ bool testauto(){ if(Tel_Mode != Automatic){ - WARNX(_("Can't stop telescope: not automatic mode!")); + WARNX(_("Not automatic mode!")); return FALSE; } return TRUE; @@ -513,7 +542,7 @@ bool stop_telescope(){ return TRUE; } ACS_CMD(StopTeleskope()); - WAIT_EVENT((Sys_Mode == SysStop), 3); + WAIT_EVENT((Sys_Mode == SysStop), WAITING_TMOUT); if(Tel_Mode != SysStop){ WARNX(_("Can't stop telescope")); return FALSE; @@ -539,18 +568,19 @@ bool gotopos(bool isradec){ ACS_CMD(SetSysTarg(TagPosition)); } DBG("start"); - usleep(500000); ACS_CMD(StartTeleskope()); + usleep(500000); #ifndef EMULATION PRINT("Go"); - WAIT_EVENT((Sys_Mode != SysStop && Sys_Mode != SysWait), 5); + WAIT_EVENT((Sys_Mode != SysStop && Sys_Mode != SysWait), WAITING_TMOUT); if(tmout){ WARNX(_("Can't move telescope")); + ACS_CMD(StopTeleskope()); return FALSE; } PRINT("Wait for tracking"); // Wait with timeout 15min - WAIT_EVENT((Sys_Mode == SysTrkOk), 900); + WAIT_EVENT((Sys_Mode == SysTrkOk), 900.); if(tmout){ WARNX(_("Eror during telescope pointing")); return FALSE; @@ -573,7 +603,7 @@ bool PCS_state(bool on){ ACS_CMD(SwitchPosCorr(PC_Off)); } #ifndef EMULATION - WAIT_EVENT((Pos_Corr == newstate), 3); + WAIT_EVENT((Pos_Corr == newstate), WAITING_TMOUT); if(tmout){ WARNX(_("Can't set new PCS state")); return FALSE; @@ -595,24 +625,33 @@ bool run_correction(char *dxdy, bool isAZ){ if(!myatod(&dy, &eptr)) goto badang; DBG("dx: %g, dy: %g", dx, dy); if(!testauto() || dx > CORR_MAX_ANGLE || dy > CORR_MAX_ANGLE) return FALSE; + int32_t oldmode = Sys_Mode; if(isAZ){ + dx /= sin(val_Z * AS2R); // transform dA to "telescope coordinates" #ifndef EMULATION - double targA = val_A+dx, targZ = val_Z+dy; + //double targA = val_A+dx, targZ = val_Z+dy; #endif - ACS_CMD(DoAZcorr(dx / sin(val_Z * AS2R), dy));// transform dA to "telescope coordinates" + ACS_CMD(DoAZcorr(dx, dy)); #ifndef EMULATION - WAIT_EVENT((fabs(val_A - targA) < CORR_THRES && fabs(val_Z - targZ) < CORR_THRES), 10); + PRINT(_("Wait for A/Z correction ends")); + //WAIT_EVENT((fabs(val_A - targA) < CORR_THRES && fabs(val_Z - targZ) < CORR_THRES), 10.); #endif }else{ #ifndef EMULATION - double targA = val_Alp+dx, targD = val_Del+dy; + //double targA = val_Alp+dx, targD = val_Del+dy; #endif ACS_CMD(DoADcorr(dx, dy)); #ifndef EMULATION - WAIT_EVENT((fabs(val_Alp - targA) < CORR_THRES && fabs(val_Del - targD) < CORR_THRES), 10); + //WAIT_EVENT((fabs(val_Alp - targA) < CORR_THRES && fabs(val_Del - targD) < CORR_THRES), 10.); #endif } #ifndef EMULATION + PRINT(_("Wait for correction starts")); + WAIT_EVENT(Sys_Mode != oldmode, 10.); + if(tmout) goto atmout; + PRINT(_("Wait for correction ends")); + WAIT_EVENT(Sys_Mode == oldmode, 150.); +atmout: if(tmout){ WARNX(_("Can't do correction (or angle is too large)")); return FALSE; diff --git a/bta_control.h b/bta_control.h index c252fa1..daefcff 100644 --- a/bta_control.h +++ b/bta_control.h @@ -22,15 +22,25 @@ #ifndef __BTA_CONTROL_H__ #define __BTA_CONTROL_H__ +// timeout for system messages process - 3 seconds +#define WAITING_TMOUT (3.) // end-switches position (in degr); prohibited angles are in range [P2_LOW_ES .. P2_HIGH_ES] #define P2_LOW_ES (21.0) #define P2_HIGH_ES (90.0) +// minimal P2 moving time +#define P2_MINTIME (4.5) +// fast speed - 10arcmin per second +#define P2_FAST_SPEED (600.) +// this value will be substituted from calculated rotation time in case of moving on large speed +#define P2_FAST_T_CORR (1.5) // angle threshold (for p2 move) in degrees #define P2_ANGLE_THRES (0.01) -#define FOCUS_THRES (0.01) +#define FOCUS_THRES (0.03) // max angles for correction of telescope (5' = 300'') #define CORR_MAX_ANGLE (300) -// correction threshold -#define CORR_THRES (0.1) +// correction threshold (arcsec) +//#define CORR_THRES (0.1) +// input coordinates threshold (arcsec) +#define INPUT_COORDS_THRES (1.) #endif // __BTA_CONTROL_H__ diff --git a/bta_print.c b/bta_print.c index 426703d..b5b7692 100644 --- a/bta_print.c +++ b/bta_print.c @@ -52,13 +52,11 @@ #include +#include "angle_functions.h" #include "bta_shdata.h" #include "bta_print.h" #include "usefull_macros.h" -#define BUFSZ 255 -static char buf[BUFSZ+1]; - typedef struct{ const char *name; const info_level lvl; @@ -220,68 +218,6 @@ const parstr const parameters_str[] = { }; - -char *time_asc(double t){ - int h, min; - double sec; - h = (int)(t/3600.); - min = (int)((t - (double)h*3600.)/60.); - sec = t - (double)h*3600. - (double)min*60.; - h %= 24; - if(sec>59.99) sec=59.99; - snprintf(buf, BUFSZ, "%02d:%02d:%05.2f", h,min,sec); - return buf; -} - -char *angle_asc(double a){ - char s; - int d, min; - double sec; - if (a >= 0.) - s = '+'; - else { - s = '-'; a = -a; - } - d = (int)(a/3600.); - min = (int)((a - (double)d*3600.)/60.); - sec = a - (double)d*3600. - (double)min*60.; - d %= 360; - if(sec>59.9) sec=59.9; - snprintf (buf, BUFSZ, "%c%02d:%02d:%04.1f", s,d,min,sec); - return buf; -} - -char *angle_fmt(double a, char *format){ - char s, *p; - int d, min, n; - double sec, msec; - char *newformat = MALLOC(char, strlen(format) + 3); - sprintf(newformat, "%s", format); - if (a >= 0.) - s = '+'; - else { - s = '-'; a = -a; - } - d = (int)(a/3600.); - min = (int)((a - (double)d*3600.)/60.); - sec = a - (double)d*3600. - (double)min*60.; - d %= 360; - if ((p = strchr(format,'.')) == NULL) - msec=59.; - else if (*(p+2) == 'f' ) { - n = *(p+1) - '0'; - msec = 60. - pow(10.,(double)(-n)); - } else - msec=60.; - if(sec>msec) sec=msec; - if (strstr(format,"%c")) - snprintf(buf, BUFSZ, newformat, s,d,min,sec); - else - snprintf(buf, BUFSZ, newformat, d,min,sec); - free(newformat); - return buf; -} - #ifndef M_PI #define M_PI (3.14159265358979323846) #endif @@ -347,7 +283,7 @@ double calc_PA(double alpha, double delta, double stime){ } void calc_AD(double az, double zd, double stime, double *alpha, double *delta){ - double sin_d, sin_a,cos_a, sin_z,cos_z; + double sin_d, sin_a, cos_a, sin_z, cos_z; double t, d, z, a, x, y; a = az * S2R; z = zd * S2R; diff --git a/bta_shdata.c b/bta_shdata.c index 19367ca..43eafa0 100644 --- a/bta_shdata.c +++ b/bta_shdata.c @@ -9,8 +9,9 @@ struct CMD_Queue ocmd = {{"Ocmd"}, 0200,0,-1,0}; // User command channel (level 2/3) struct CMD_Queue ucmd = {{"Ucmd"}, 0200,0,-1,0}; -static char msg[80]; -#define PERR(...) do{sprintf(msg, __VA_ARGS__); perror(msg);} while(0) +#define MSGLEN (80) +static char msg[MSGLEN]; +#define PERR(...) do{snprintf(msg, MSGLEN, __VA_ARGS__); perror(msg);} while(0) #ifndef BTA_MODULE volatile struct BTA_Data *sdt; @@ -127,8 +128,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 at %lx", - sb->key.name, sb->size, (uint64_t)sb->addr); + DBG("Create & attach shared memory segment '%s' %dbytes", sb->key.name, sb->size); sb->side = server; if(sb->init != NULL) sb->init(); diff --git a/cmdlnopts.c b/cmdlnopts.c index 7d4b994..011ebad 100644 --- a/cmdlnopts.c +++ b/cmdlnopts.c @@ -83,6 +83,7 @@ myoption cmdlnopts[] = { end_option }; + /** * Parce command line options and return dynamically allocated structure * to global parameters