mirror of
https://github.com/eddyem/small_tel.git
synced 2025-12-06 10:45:16 +03:00
191 lines
7.7 KiB
C
191 lines
7.7 KiB
C
/*
|
|
* This file is part of the libsidservo project.
|
|
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
|
*
|
|
* This program is free software: you can redistribute it and/or modify
|
|
* it under the terms of the GNU General Public License as published by
|
|
* the Free Software Foundation, either version 3 of the License, or
|
|
* (at your option) any later version.
|
|
*
|
|
* This program is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
* GNU General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License
|
|
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
#include <pthread.h>
|
|
#include <signal.h>
|
|
#include <usefull_macros.h>
|
|
|
|
#include "conf.h"
|
|
#include "dump.h"
|
|
#include "sidservo.h"
|
|
#include "simpleconv.h"
|
|
#include "traectories.h"
|
|
|
|
// calculate some traectory and try to run over it
|
|
|
|
typedef struct{
|
|
int help;
|
|
int dumpconf;
|
|
int Ncycles; // n cycles to wait stop
|
|
double reqint; // requests interval (seconds)
|
|
double Xmax; // maximal X to stop
|
|
double Ymax; // maximal Y to stop
|
|
double tmax; // maximal time of emulation
|
|
double X0; // starting point of traectory (-30..30 degr)
|
|
double Y0; // -//-
|
|
char *coordsoutput; // dump file
|
|
char *errlog; // log with position errors
|
|
char *tfn; // traectory function name
|
|
char *conffile;
|
|
} parameters;
|
|
|
|
static FILE *fcoords = NULL, *errlog = NULL;
|
|
static pthread_t dthr;
|
|
static parameters G = {
|
|
.Ncycles = 40,
|
|
.reqint = 0.1,
|
|
.tfn = "sincos",
|
|
.Xmax = 45.,
|
|
.Ymax = 45.,
|
|
.tmax = 300., // 5 minutes
|
|
.X0 = 10.,
|
|
.Y0 = 10.,
|
|
};
|
|
|
|
static sl_option_t cmdlnopts[] = {
|
|
{"help", NO_ARGS, NULL, 'h', arg_int, APTR(&G.help), "show this help"},
|
|
{"ncycles", NEED_ARG, NULL, 'n', arg_int, APTR(&G.Ncycles), "N cycles in stopped state (default: 40)"},
|
|
{"coordsfile", NEED_ARG, NULL, 'o', arg_string, APTR(&G.coordsoutput),"output file with coordinates log"},
|
|
{"reqinterval", NEED_ARG, NULL, 'i', arg_double, APTR(&G.reqint), "mount requests interval (default: 0.1 second)"},
|
|
{"traectory", NEED_ARG, NULL, 't', arg_string, APTR(&G.tfn), "used traectory function (default: sincos)"},
|
|
{"xmax", NEED_ARG, NULL, 'X', arg_double, APTR(&G.Xmax), "maximal abs X coordinate for traectory (default: 45 degrees)"},
|
|
{"ymax", NEED_ARG, NULL, 'Y', arg_double, APTR(&G.Ymax), "maximal abs Y coordinate for traectory (default: 45 degrees)"},
|
|
{"tmax", NEED_ARG, NULL, 'T', arg_double, APTR(&G.tmax), "maximal duration time of emulation (default: 300 seconds)"},
|
|
{"x0", NEED_ARG, NULL, '0', arg_double, APTR(&G.X0), "starting X-coordinate of traectory (default: 10 degrees)"},
|
|
{"y0", NEED_ARG, NULL, '1', arg_double, APTR(&G.Y0), "starting Y-coordinate of traectory (default: 10 degrees)"},
|
|
{"conffile", NEED_ARG, NULL, 'C', arg_string, APTR(&G.conffile), "configuration file name"},
|
|
{"errlog", NEED_ARG, NULL, 'e', arg_string, APTR(&G.errlog), "file with errors log"},
|
|
{"dumpconf", NO_ARGS, NULL, 'D', arg_int, APTR(&G.dumpconf), "dump current configuration"},
|
|
end_option
|
|
};
|
|
|
|
void signals(int sig){
|
|
if(sig){
|
|
signal(sig, SIG_IGN);
|
|
DBG("Get signal %d, quit.\n", sig);
|
|
}
|
|
Mount.stop();
|
|
sleep(1);
|
|
Mount.quit();
|
|
if(fcoords) fclose(fcoords);
|
|
exit(sig);
|
|
}
|
|
|
|
static void *dumping(void _U_ *u){
|
|
dumpmoving(fcoords, 3600., G.Ncycles);
|
|
return NULL;
|
|
}
|
|
|
|
// calculate
|
|
static void runtraectory(traectory_fn tfn){
|
|
if(!tfn) return;
|
|
coordval_pair_t telXY;
|
|
coordval_pair_t target;
|
|
coordpair_t traectXY, endpoint;
|
|
endpoint.X = G.Xmax, endpoint.Y = G.Ymax;
|
|
double t0 = dumpt0(), tlast = 0., tstart = Mount.currentT();
|
|
double tlastX = 0., tlastY = 0.;
|
|
while(1){
|
|
if(!telpos(&telXY)){
|
|
WARNX("No next telescope position");
|
|
return;
|
|
}
|
|
if(telXY.X.t == tlastX && telXY.Y.t == tlastY) continue; // last measure - don't mind
|
|
DBG("\n\nTELPOS: %g'/%g' (%.6f/%.6f) measured @ %.6f/%.6f", RAD2AMIN(telXY.X.val), RAD2AMIN(telXY.Y.val), RAD2DEG(telXY.X.val), RAD2DEG(telXY.Y.val), telXY.X.t, telXY.Y.t);
|
|
tlastX = telXY.X.t; tlastY = telXY.Y.t;
|
|
double t = Mount.currentT();
|
|
if(fabs(telXY.X.val) > G.Xmax || fabs(telXY.Y.val) > G.Ymax || t - tstart > G.tmax) break;
|
|
if(!traectory_point(&traectXY, t)) break;
|
|
target.X.val = traectXY.X; target.Y.val = traectXY.Y;
|
|
target.X.t = target.Y.t = t;
|
|
// check whether we should change direction
|
|
if(telXY.X.val > traectXY.X) endpoint.X = -G.Xmax;
|
|
else if(telXY.X.val < traectXY.X) endpoint.X = G.Xmax;
|
|
if(telXY.Y.val > traectXY.Y) endpoint.Y = -G.Ymax;
|
|
else if(telXY.Y.val < traectXY.Y) endpoint.Y = G.Ymax;
|
|
if(t0 < 0.) t0 = dumpt0();
|
|
else{
|
|
//DBG("target: %g'/%g'", RAD2AMIN(traectXY.X), RAD2AMIN(traectXY.Y));
|
|
DBG("%g: dX=%.4f'', dY=%.4f''", t-t0, RAD2ASEC(traectXY.X-telXY.X.val), RAD2ASEC(traectXY.Y-telXY.Y.val));
|
|
//DBG("Correct to: %g/%g with EP %g/%g", RAD2DEG(target.X.val), RAD2DEG(target.Y.val), RAD2DEG(endpoint.X), RAD2DEG(endpoint.Y));
|
|
if(errlog)
|
|
fprintf(errlog, "%10.4f %10.4f %10.4f\n", telXY.X.t-t0, RAD2ASEC(traectXY.X-telXY.X.val), RAD2ASEC(traectXY.Y-telXY.Y.val));
|
|
}
|
|
if(MCC_E_OK != Mount.correctTo(&target, &endpoint)) WARNX("Error of correction!");
|
|
while((t = Mount.currentT()) - tlast < MCC_PID_REFRESH_DT) usleep(50);
|
|
tlast = t;
|
|
}
|
|
WARNX("No next traectory point or emulation ends");
|
|
}
|
|
|
|
int main(int argc, char **argv){
|
|
sl_init();
|
|
sl_parseargs(&argc, &argv, cmdlnopts);
|
|
if(G.help) sl_showhelp(-1, cmdlnopts);
|
|
if(G.Xmax < 1. || G.Xmax > 90.) ERRX("Xmax should be 1..90 degrees");
|
|
if(G.Ymax < 1. || G.Ymax > 90.) ERRX("Ymax should be 1..90 degrees");
|
|
// convert to radians
|
|
G.Xmax = DEG2RAD(G.Xmax); G.Ymax = DEG2RAD(G.Ymax);
|
|
if(G.X0 < -30. || G.X0 > 30. || G.Y0 < -30. || G.Y0 > 30.)
|
|
ERRX("X0 and Y0 should be -30..30 degrees");
|
|
if(G.errlog){
|
|
if(!(errlog = fopen(G.errlog, "w")))
|
|
ERRX("Can't open error log %s", G.errlog);
|
|
else
|
|
fprintf(errlog, "# time Xerr'' Yerr'' // target - real\n");
|
|
}
|
|
if(G.coordsoutput){
|
|
if(!(fcoords = fopen(G.coordsoutput, "w")))
|
|
ERRX("Can't open %s", G.coordsoutput);
|
|
}else fcoords = stdout;
|
|
conf_t *Config = readServoConf(G.conffile);
|
|
if(!Config || G.dumpconf){
|
|
dumpConf();
|
|
return 1;
|
|
}
|
|
Config->MountReqInterval = G.reqint;
|
|
traectory_fn tfn = traectory_by_name(G.tfn);
|
|
if(!tfn){
|
|
WARNX("Bad traectory name %s, should be one of", G.tfn);
|
|
print_tr_names();
|
|
return 1;
|
|
}
|
|
coordpair_t c = {.X = DEG2RAD(G.X0), .Y = DEG2RAD(G.Y0)};
|
|
if(!init_traectory(tfn, &c)){
|
|
ERRX("Can't init traectory");
|
|
return 1;
|
|
}
|
|
mcc_errcodes_t e = Mount.init(Config);
|
|
if(e != MCC_E_OK){
|
|
WARNX("Can't init devices");
|
|
return 1;
|
|
}
|
|
signal(SIGTERM, signals); // kill (-15) - quit
|
|
signal(SIGHUP, SIG_IGN); // hup - ignore
|
|
signal(SIGINT, signals); // ctrl+C - quit
|
|
signal(SIGQUIT, signals); // ctrl+\ - quit
|
|
signal(SIGTSTP, SIG_IGN); // ignore ctrl+Z
|
|
chk0(G.Ncycles);
|
|
logmnt(fcoords, NULL);
|
|
if(pthread_create(&dthr, NULL, dumping, NULL)) ERRX("Can't run dump thread");
|
|
;
|
|
runtraectory(tfn);
|
|
signals(0);
|
|
return 0;
|
|
}
|