timur/LibSidServo
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fixed broken PID

Browse Source
This commit is contained in:
Edward V. Emelianov
2026-03-18 18:10:05 +03:00
parent b0d0a323ee
commit db1058ddf8
7 changed files with 204 additions and 84 deletions
Download Patch File Download Diff File Expand all files Collapse all files

27
examples/scmd_traectory.c
View File

@@ -109,17 +109,27 @@ static void runtraectory(traectory_fn tfn){
}
if(!Mount.currentT(&tcur)) continue;
if(telXY.X.t.tv_nsec == tlastXnsec && telXY.Y.t.tv_nsec == tlastYnsec) continue; // last measure - don't mind
DBG("\n\nTELPOS: %g'/%g' (%.6f/%.6f)", RAD2AMIN(telXY.X.val), RAD2AMIN(telXY.Y.val), RAD2DEG(telXY.X.val), RAD2DEG(telXY.Y.val));
tlastXnsec = telXY.X.t.tv_nsec; tlastYnsec = telXY.Y.t.tv_nsec;
double t = Mount.timeFromStart();
if(fabs(telXY.X.val) > G.Xmax || fabs(telXY.Y.val) > G.Ymax || t - tstart > G.tmax) break;
if(!traectory_point(&traectXY, t)) break;
if(fabs(telXY.X.val) > G.Xmax || fabs(telXY.Y.val) > G.Ymax || t - tstart > G.tmax){
if(fabs(telXY.X.val) > G.Xmax) DBG("X over maximal limit!");
if(fabs(telXY.Y.val) > G.Ymax) DBG("Y over maximal limit!");
if(t - tstart > G.tmax) DBG("Time over...");
break;
}
if(!traectory_point(&traectXY, t)){
DBG("Error in 'traectory_point', time from start=%g", t);
break;
}
DBG("\n\nTELPOS: %g'/%g' (%.6f/%.6f); traectory: %g'/%g' (%.6f/%.6f)",
RAD2AMIN(telXY.X.val), RAD2AMIN(telXY.Y.val), RAD2DEG(telXY.X.val), RAD2DEG(telXY.Y.val),
RAD2AMIN(traectXY.X), RAD2AMIN(traectXY.Y), RAD2DEG(traectXY.X), RAD2DEG(traectXY.Y));
target.X.val = traectXY.X; target.Y.val = traectXY.Y;
target.X.t = target.Y.t = tcur;
if(t0.tv_nsec == 0 && t0.tv_sec == 0) dumpt0(&t0);
else{
//DBG("target: %g'/%g'", RAD2AMIN(traectXY.X), RAD2AMIN(traectXY.Y));
DBG("%g: dX=%.4f'', dY=%.4f''", t-tstart, RAD2ASEC(traectXY.X-telXY.X.val), RAD2ASEC(traectXY.Y-telXY.Y.val));
DBG("%g, target-telescope: dX=%.4f'', dY=%.4f''", t-tstart, 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", Mount.timeDiff(&telXY.X.t, &t0), RAD2ASEC(traectXY.X-telXY.X.val), RAD2ASEC(traectXY.Y-telXY.Y.val));
@@ -166,15 +176,16 @@ int main(int argc, char **argv){
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;
}
// run this function only after mount inited!
if(!init_traectory(tfn, &c)){
ERRX("Can't init traectory");
return 1;
}
signal(SIGTERM, signals); // kill (-15) - quit
signal(SIGHUP, SIG_IGN); // hup - ignore
signal(SIGINT, signals); // ctrl+C - quit

22
examples/traectories.c
View File

@@ -42,6 +42,7 @@ int init_traectory(traectory_fn f, coordpair_t *XY0){
cur_traectory = f;
XYstart = *XY0;
tstart = Mount.timeFromStart();
DBG("inited @ %gs from start", tstart);
mountdata_t mdata;
int ntries = 0;
for(; ntries < 10; ++ntries){
@@ -58,11 +59,21 @@ int init_traectory(traectory_fn f, coordpair_t *XY0){
* @return FALSE if something wrong (e.g. X not in -90..90 or Y not in -180..180)
*/
int traectory_point(coordpair_t *nextpt, double t){
if(t < 0. || !cur_traectory) return FALSE;
if(t < 0. || !cur_traectory){
if(t < 0.) DBG("time in past!");
else DBG("no current traectory selected!");
return FALSE;
}
coordpair_t pt;
if(!cur_traectory(&pt, t)) return FALSE;
if(!cur_traectory(&pt, t)){
DBG("error in cur_traectory");
return FALSE;
}
if(nextpt) *nextpt = pt;
if(pt.X < -M_PI_2 || pt.X > M_PI_2 || pt.Y < -M_PI || pt.Y > M_PI) return FALSE;
if(pt.X < -M_PI_2 || pt.X > M_PI_2 || pt.Y < -M_PI || pt.Y > M_PI){
DBG("some points is over limits, pt.x=%g, pt.y=%g degrees", RAD2DEG(pt.X), RAD2DEG(pt.Y));
return FALSE;
}
return TRUE;
}
@@ -88,6 +99,7 @@ int telpos(coordval_pair_t *curpos){
// X=X0+1'/s, Y=Y0+15''/s
int Linear(coordpair_t *nextpt, double t){
coordpair_t pt;
DBG("t=%g, tfromstart=%g, dt=%g", t, tstart, t-tstart);
pt.X = XYstart.X + ASEC2RAD(0.1) * (t - tstart);
pt.Y = XYstart.Y + ASEC2RAD(15.)* (t - tstart);
if(nextpt) *nextpt = pt;
@@ -98,7 +110,7 @@ int Linear(coordpair_t *nextpt, double t){
int SinCos(coordpair_t *nextpt, double t){
coordpair_t pt;
pt.X = XYstart.X + ASEC2RAD(5.) * sin((t-tstart)/30.*2*M_PI);
pt.Y = XYstart.Y + AMIN2RAD(1.)* cos((t-tstart)/200.*2*M_PI);
pt.Y = XYstart.Y + AMIN2RAD(1.)* cos((t-tstart)/10.*2*M_PI);
if(nextpt) *nextpt = pt;
return TRUE;
}
@@ -111,7 +123,7 @@ typedef struct{
static tr_names names[] = {
{Linear, "linear", "X=X0+0.1''/s, Y=Y0+15''/s"},
{SinCos, "sincos", "X=X0+5''*sin(t/30*2pi), Y=Y0+10'*cos(t/200*2pi)"},
{SinCos, "sincos", "X=X0+5''*sin(t/30*2pi), Y=Y0+1'*cos(t/10*2pi)"},
{NULL, NULL, NULL}
};