eddy/mountcontrol
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

add speed + a lot of refactoring

Browse Source
This commit is contained in:
Edward V. Emelianov 2025-07-22 23:52:08 +03:00
parent fc943abe03
commit b0421972d1
22 changed files with 513 additions and 161 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
LibSidServo/TODO Normal file
View File

@ -0,0 +1,11 @@
1. WTF after closing?
closeSerial (/home/eddy/C-files/LibSidServo/serial.c, line 484): close fd
closeSerial (/home/eddy/C-files/LibSidServo/serial.c, line 489): Kill encoder thread
closeSerial (/home/eddy/C-files/LibSidServo/serial.c, line 491): close fd
*** bit out of range 0 - FD_SETSIZE on fd_set ***: terminated
Aborted

4
LibSidServo/examples/conf.c
View File

@ -29,7 +29,8 @@ static conf_t Config = {
.EncoderDevSpeed = 153000,
.MountReqInterval = 0.1,
.EncoderReqInterval = 0.05,
.SepEncoder = 2
.SepEncoder = 2,
.EncoderSpeedInterval = 0.1,
};
static sl_option_t opts[] = {
@ -42,6 +43,7 @@ static sl_option_t opts[] = {
{"SepEncoder", NO_ARGS, NULL, 0, arg_int, APTR(&Config.SepEncoder), "encoder is separate device (1 - one device, 2 - two devices)"},
{"EncoderXDevPath", NEED_ARG, NULL, 0, arg_string, APTR(&Config.EncoderXDevPath), "path to X encoder (/dev/encoderX0)"},
{"EncoderYDevPath", NEED_ARG, NULL, 0, arg_string, APTR(&Config.EncoderYDevPath), "path to Y encoder (/dev/encoderY0)"},
{"EncoderSpeedInterval", NEED_ARG,NULL, 0, arg_double, APTR(&Config.EncoderSpeedInterval),"interval of speed calculations, s"},
end_option
};

99
LibSidServo/examples/dump.c
View File

@ -23,6 +23,42 @@
#include "dump.h"
#include "simpleconv.h"
#if 0
// amount of elements used for encoders' data filtering
#define NFILT (10)
static double filterK[NFILT];
static double lastvals[2][NFILT] = {0};
static int need2buildFilter = 1;
static void buildFilter(){
filterK[NFILT-1] = 1.;
double sum = 1.;
for(int i = NFILT-2; i > -1; --i){
filterK[i] = (filterK[i+1] + 1.) * 1.1;
sum += filterK[i];
}
for(int i = 0; i < NFILT; ++i) filterK[i] /= sum;
}
static double filter(double val, int idx){
if(need2buildFilter){
buildFilter();
need2buildFilter = 0;
}
static int ctr[2] = {0};
for(int i = NFILT-1; i > 0; --i) lastvals[idx][i] = lastvals[idx][i-1];
lastvals[idx][0] = val;
double r = 0.;
if(ctr[idx] < NFILT){
++ctr[idx];
return val;
}
for(int i = 0; i < NFILT; ++i) r += filterK[i] * lastvals[idx][i];
return r;
}
#endif
/**
* @brief logmnt - log mount data into file
* @param fcoords - file to dump
@ -33,16 +69,18 @@ void logmnt(FILE *fcoords, mountdata_t *m){
//DBG("LOG %s", m ? "data" : "header");
static double t0 = -1.;
if(!m){ // write header
fprintf(fcoords, "# time Xmot(deg) Ymot(deg) Xenc(deg) Yenc(deg) millis T V\n");
fprintf(fcoords, "# time Xmot(deg) Ymot(deg) Xenc(deg) Yenc(deg) VX(d/s) VY(d/s) millis\n");
return;
}
if(t0 < 0.) t0 = m->encposition.msrtime.tv_sec + (double)(m->encposition.msrtime.tv_usec) / 1e6;
double t = m->encposition.msrtime.tv_sec + (double)(m->encposition.msrtime.tv_usec) / 1e6 - t0;
double tnow = (m->encXposition.t + m->encYposition.t) / 2.;
if(t0 < 0.) t0 = tnow;
double t = tnow - t0;
// write data
fprintf(fcoords, "%12.6f %10.6f %10.6f %10.6f %10.6f %10u %6.1f %4.1f\n",
t, RAD2DEG(m->motposition.X), RAD2DEG(m->motposition.Y),
RAD2DEG(m->encposition.X), RAD2DEG(m->encposition.Y),
m->millis, m->temperature, m->voltage);
fprintf(fcoords, "%12.6f %10.6f %10.6f %10.6f %10.6f %10.6f %10.6f %10u\n",
t, RAD2DEG(m->motXposition.val), RAD2DEG(m->motYposition.val),
RAD2DEG(m->encXposition.val), RAD2DEG(m->encYposition.val),
RAD2DEG(m->encXspeed.val), RAD2DEG(m->encYspeed.val),
m->millis);
fflush(fcoords);
}
@ -64,21 +102,24 @@ void dumpmoving(FILE *fcoords, double t, int N){
WARNX("Can't get mount data");
LOGWARN("Can't get mount data");
}
uint32_t millis = mdata.encposition.msrtime.tv_usec;
uint32_t mdmillis = mdata.millis;
double enct = (mdata.encXposition.t + mdata.encYposition.t) / 2.;
int ctr = -1;
double xlast = mdata.motposition.X, ylast = mdata.motposition.Y;
double t0 = sl_dtime();
//DBG("millis = %u", millis);
while(sl_dtime() - t0 < t && ctr < N){
double xlast = mdata.motXposition.val, ylast = mdata.motYposition.val;
double t0 = Mount.currentT();
while(Mount.currentT() - t0 < t && ctr < N){
usleep(1000);
if(MCC_E_OK != Mount.getMountData(&mdata)){ WARNX("Can't get data"); continue;}
if(mdata.encposition.msrtime.tv_usec == millis) continue;
//DBG("Got new data, posX=%g, posY=%g", mdata.motposition.X, mdata.motposition.Y);
millis = mdata.encposition.msrtime.tv_usec;
double tmsr = (mdata.encXposition.t + mdata.encYposition.t) / 2.;
if(tmsr == enct) continue;
enct = tmsr;
if(fcoords) logmnt(fcoords, &mdata);
if(mdata.motposition.X != xlast || mdata.motposition.Y != ylast){
xlast = mdata.motposition.X;
ylast = mdata.motposition.Y;
if(mdata.millis == mdmillis) continue;
DBG("ctr=%d", ctr);
mdmillis = mdata.millis;
if(mdata.motXposition.val != xlast || mdata.motYposition.val != ylast){
xlast = mdata.motXposition.val;
ylast = mdata.motYposition.val;
ctr = 0;
}else ++ctr;
}
@ -98,9 +139,9 @@ void waitmoving(int N){
if(MCC_E_OK != Mount.getMountData(&mdata)){ WARNX("Can't get data"); continue;}
if(mdata.millis == millis) continue;
millis = mdata.millis;
if(mdata.motposition.X != xlast || mdata.motposition.Y != ylast){
xlast = mdata.motposition.X;
ylast = mdata.motposition.Y;
if(mdata.motXposition.val != xlast || mdata.motYposition.val != ylast){
xlast = mdata.motXposition.val;
ylast = mdata.motYposition.val;
ctr = 0;
}else ++ctr;
}
@ -112,7 +153,7 @@ void waitmoving(int N){
* @param Y (o) - encoder position (or NULL)
* @return FALSE if failed
*/
int getPos(coords_t *mot, coords_t *enc){
int getPos(coordval_pair_t *mot, coordval_pair_t *enc){
mountdata_t mdata = {0};
int errcnt = 0;
do{
@ -126,16 +167,22 @@ int getPos(coords_t *mot, coords_t *enc){
WARNX("Can't read mount status");
return FALSE;
}
if(mot) *mot = mdata.motposition;
if(enc) *enc = mdata.encposition;
if(mot){
mot->X = mdata.motXposition;
mot->Y = mdata.motYposition;
}
if(enc){
enc->X = mdata.encXposition;
enc->Y = mdata.encYposition;
}
return TRUE;
}
// check current position and go to 0 if non-zero
void chk0(int ncycles){
coords_t M;
coordval_pair_t M;
if(!getPos(&M, NULL)) signals(2);
if(M.X || M.Y){
if(M.X.val || M.Y.val){
WARNX("Mount position isn't @ zero; moving");
double zero = 0.;
Mount.moveTo(&zero, &zero);

2
LibSidServo/examples/dump.h
View File

@ -25,5 +25,5 @@
void logmnt(FILE *fcoords, mountdata_t *m);
void dumpmoving(FILE *fcoords, double t, int N);
void waitmoving(int N);
int getPos(coords_t *mot, coords_t *enc);
int getPos(coordval_pair_t *mot, coordval_pair_t *enc);
void chk0(int ncycles);

6
LibSidServo/examples/dumpmoving.c
View File

@ -85,18 +85,18 @@ int main(int argc, char **argv){
LOGMSG("Mount device %s @ %d", Config->MountDevPath, Config->MountDevSpeed);
LOGMSG("Encoder device %s @ %d", Config->EncoderDevPath, Config->EncoderDevSpeed);
if(MCC_E_OK != Mount.init(Config)) ERRX("Can't init devices");
coords_t M;
coordval_pair_t M;
if(!getPos(&M, NULL)) ERRX("Can't get current position");
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
double tagx = DEG2RAD(45.) + M.X, tagy = DEG2RAD(45.) + M.Y;
double tagx = DEG2RAD(45.) + M.X.val, tagy = DEG2RAD(45.) + M.Y.val;
if(MCC_E_OK != Mount.moveTo(&tagx, &tagy))
ERRX("Can't move to 45, 45");
dumpmoving(fcoords, 30., G.Ncycles);
Mount.moveTo(&M.X, &M.Y);
Mount.moveTo(&M.X.val, &M.Y.val);
dumpmoving(fcoords, 30., G.Ncycles);
signals(0);
return 0;

170
LibSidServo/examples/dumpmoving_dragNtrack.c Normal file
View File

@ -0,0 +1,170 @@
/*
* 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/>.
*/
// move telescope to target using short command and force it to track mode
#include <math.h>
#include <pthread.h>
#include <signal.h>
#include <stdio.h>
#include <string.h>
#include <time.h>
#include <usefull_macros.h>
#include "conf.h"
#include "dump.h"
#include "sidservo.h"
#include "simpleconv.h"
typedef struct{
int help;
int Ncycles;
int relative;
double reqint;
char *coordsoutput;
char *conffile;
char *axis;
} parameters;
static parameters G = {
.Ncycles = 40,
.reqint = -1.,
.axis = "X",
};
static FILE *fcoords = NULL;
static coords_t M;
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)"},
{"axis", NEED_ARG, NULL, 'a', arg_string, APTR(&G.axis), "axis to move (X, Y or B for both)"},
{"conffile", NEED_ARG, NULL, 'C', arg_string, APTR(&G.conffile), "configuration file name"},
{"relative", NO_ARGS, NULL, 'r', arg_int, APTR(&G.relative), "relative move"},
end_option
};
void signals(int sig){
if(sig){
signal(sig, SIG_IGN);
DBG("Get signal %d, quit.\n", sig);
}
Mount.quit();
exit(sig);
}
// dump thread
static void *dumping(void _U_ *u){
dumpmoving(fcoords, 3600., G.Ncycles);
return NULL;
}
// return TRUE if motor position is reached +- 0.01 degrees
#define XYcount (DEG2RAD(0.01))
static int Wait(double tag){
mountdata_t mdata;
red("Wait for %g degrees\n", RAD2DEG(tag));
int errcnt = 0;
double sign = 0.;
uint32_t millis = 0;
double curpos = 0.;
do{
if(MCC_E_OK != Mount.getMountData(&mdata)) ++errcnt;
else{
errcnt = 0;
if(mdata.millis == millis) continue;
millis = mdata.millis;
if(*G.axis == 'X') curpos = mdata.motposition.X;
else curpos = mdata.motposition.Y;
if(sign == 0.) sign = (curpos > tag) ? 1. : -1.;
//printf("%s=%g deg, need %g deg; delta=%g arcmin\n", G.axis, RAD2DEG(curpos),
// RAD2DEG(tag), RAD2DEG(sign*(curpos - tag))*60.);
}
}while(sign*(curpos - tag) > XYcount && errcnt < 10);
if(errcnt >= 10){
WARNX("Too much errors");
return FALSE;
}
green("%s reached position %g degrees\n", G.axis, RAD2DEG(tag));
fflush(stdout);
return TRUE;
}
// move X/Y to 40 degr with given speed until given coord
static void move(double target, double limit, double speed){
green("Move %s to %g until %g with %gdeg/s\n", G.axis, target, limit, speed);
short_command_t cmd = {0};
if(*G.axis == 'X' || *G.axis == 'B'){
cmd.Xmot = DEG2RAD(target) + M.X;
cmd.Xspeed = DEG2RAD(speed);
limit = DEG2RAD(limit) + M.X;
}
if(*G.axis == 'Y' || *G.axis == 'B'){
cmd.Ymot = DEG2RAD(target) + M.Y;
cmd.Yspeed = DEG2RAD(speed);
if(*G.axis != 'B') limit = DEG2RAD(limit) + M.Y;
}
if(MCC_E_OK != Mount.shortCmd(&cmd)) ERRX("Can't run command");
if(!Wait(limit)) signals(9);
}
int main(int argc, char **argv){
sl_init();
sl_parseargs(&argc, &argv, cmdlnopts);
if(G.help) sl_showhelp(-1, cmdlnopts);
if(strcmp(G.axis, "X") && strcmp(G.axis, "Y") && strcmp(G.axis, "B")){
WARNX("\"Axis\" should be X, Y or B");
return 1;
}
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){
dumpConf();
return 1;
}
if(G.reqint > 0.) Config->MountReqInterval = G.reqint;
if(MCC_E_OK != Mount.init(Config)){
WARNX("Can't init devices");
return 1;
}
if(!getPos(&M, NULL)) ERRX("Can't get current position");
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
// move to X=40 degr with different speeds
pthread_t dthr;
logmnt(fcoords, NULL);
if(pthread_create(&dthr, NULL, dumping, NULL)) ERRX("Can't run dump thread");
// goto 10 degr with 2d/s and try to track for 8 seconds
move(10., 10.+2./60., 2.);
// be sure to move @ 0,0
Mount.moveTo(&M.X, &M.Y);
// wait moving ends
pthread_join(dthr, NULL);
signals(0);
return 0;
}

23
LibSidServo/examples/dumpmoving_scmd.c
View File

@ -49,7 +49,7 @@ static parameters G = {
static FILE *fcoords = NULL;
static coords_t M;
static coordval_pair_t M;
static sl_option_t cmdlnopts[] = {
{"help", NO_ARGS, NULL, 'h', arg_int, APTR(&G.help), "show this help"},
@ -92,8 +92,8 @@ static int Wait(double tag){
errcnt = 0;
if(mdata.millis == millis) continue;
millis = mdata.millis;
if(*G.axis == 'X') curpos = mdata.motposition.X;
else curpos = mdata.motposition.Y;
if(*G.axis == 'X') curpos = mdata.motXposition.val;
else curpos = mdata.motYposition.val;
if(sign == 0.) sign = (curpos > tag) ? 1. : -1.;
//printf("%s=%g deg, need %g deg; delta=%g arcmin\n", G.axis, RAD2DEG(curpos),
// RAD2DEG(tag), RAD2DEG(sign*(curpos - tag))*60.);
@ -110,22 +110,20 @@ static int Wait(double tag){
// move X/Y to 40 degr with given speed until given coord
static void move(double target, double limit, double speed){
#define SCMD() do{if(MCC_E_OK != Mount.shortCmd(&cmd)) ERRX("Can't run command"); }while(0)
green("Move %s to %g until %g with %gdeg/s\n", G.axis, target, limit, speed);
short_command_t cmd = {0};
if(*G.axis == 'X' || *G.axis == 'B'){
cmd.Xmot = DEG2RAD(target) + M.X;
cmd.Xmot = DEG2RAD(target) + M.X.val;
cmd.Xspeed = DEG2RAD(speed);
limit = DEG2RAD(limit) + M.X;
limit = DEG2RAD(limit) + M.X.val;
}
if(*G.axis == 'Y' || *G.axis == 'B'){
cmd.Ymot = DEG2RAD(target) + M.Y;
cmd.Ymot = DEG2RAD(target) + M.Y.val;
cmd.Yspeed = DEG2RAD(speed);
if(*G.axis != 'B') limit = DEG2RAD(limit) + M.Y;
if(*G.axis != 'B') limit = DEG2RAD(limit) + M.Y.val;
}
SCMD();
if(MCC_E_OK != Mount.shortCmd(&cmd)) ERRX("Can't run command");
if(!Wait(limit)) signals(9);
#undef SCMD
}
@ -169,11 +167,10 @@ int main(int argc, char **argv){
move(10., 3., 15./60.);
// and go back with 7deg/s
move(0., 0., 7.);
// be sure to move @ 0,0
Mount.moveTo(&M.X, &M.Y);
// be sure to move @ starting position
Mount.moveTo(&M.X.val, &M.Y.val);
// wait moving ends
pthread_join(dthr, NULL);
#undef SCMD
signals(0);
return 0;
}

20
LibSidServo/examples/dumpswing.c
View File

@ -83,18 +83,18 @@ void waithalf(double t){
uint32_t millis = 0;
double xlast = 0., ylast = 0.;
while(ctr < 5){
if(sl_dtime() >= t) return;
if(Mount.currentT() >= t) return;
usleep(1000);
if(MCC_E_OK != Mount.getMountData(&mdata)){ WARNX("Can't get data"); continue;}
if(mdata.millis == millis) continue;
millis = mdata.millis;
if(mdata.motposition.X != xlast || mdata.motposition.Y != ylast){
DBG("NEQ: old=%g, now=%g", RAD2DEG(ylast), RAD2DEG(mdata.motposition.Y));
xlast = mdata.motposition.X;
ylast = mdata.motposition.Y;
if(mdata.motXposition.val != xlast || mdata.motYposition.val != ylast){
DBG("NEQ: old=%g, now=%g", RAD2DEG(ylast), RAD2DEG(mdata.motYposition.val));
xlast = mdata.motXposition.val;
ylast = mdata.motYposition.val;
ctr = 0;
}else{
DBG("EQ: old=%g, now=%g", RAD2DEG(ylast), RAD2DEG(mdata.motposition.Y));
DBG("EQ: old=%g, now=%g", RAD2DEG(ylast), RAD2DEG(mdata.motYposition.val));
++ctr;
}
}
@ -145,21 +145,21 @@ int main(int argc, char **argv){
}else{
tagX = 0.; tagY = DEG2RAD(G.amplitude);
}
double t = sl_dtime(), t0 = t;
double t = Mount.currentT(), t0 = t;
double divide = 2., rtagX = -tagX, rtagY = -tagY;
for(int i = 0; i < G.Nswings; ++i){
Mount.moveTo(&tagX, &tagY);
DBG("CMD: %g", sl_dtime()-t0);
DBG("CMD: %g", Mount.currentT()-t0);
t += G.period / divide;
divide = 1.;
waithalf(t);
DBG("Moved to +, t=%g", t-t0);
DBG("CMD: %g", sl_dtime()-t0);
DBG("CMD: %g", Mount.currentT()-t0);
Mount.moveTo(&rtagX, &rtagY);
t += G.period;
waithalf(t);
DBG("Moved to -, t=%g", t-t0);
DBG("CMD: %g", sl_dtime()-t0);
DBG("CMD: %g", Mount.currentT()-t0);
}
double zero = 0.;
// be sure to move @ 0,0

16
LibSidServo/examples/goto.c
View File

@ -40,7 +40,7 @@ typedef struct{
} parameters;
static parameters G = {
.Ncycles = 40,
.Ncycles = 10,
.X = NAN,
.Y = NAN,
};
@ -88,7 +88,7 @@ int main(int _U_ argc, char _U_ **argv){
return 1;
}
if(MCC_E_OK != Mount.init(Config)) ERRX("Can't init mount");
coords_t M;
coordval_pair_t M;
if(!getPos(&M, NULL)) ERRX("Can't get current position");
if(G.coordsoutput){
if(!G.wait) green("When logging I should wait until moving ends; added '-w'");
@ -100,20 +100,20 @@ int main(int _U_ argc, char _U_ **argv){
logmnt(fcoords, NULL);
if(pthread_create(&dthr, NULL, dumping, NULL)) ERRX("Can't run dump thread");
}
printf("Mount position: X=%g, Y=%g\n", RAD2DEG(M.X), RAD2DEG(M.Y));
printf("Mount position: X=%g, Y=%g\n", RAD2DEG(M.X.val), RAD2DEG(M.Y.val));
if(isnan(G.X) && isnan(G.Y)) goto out;
double *xtag = NULL, *ytag = NULL, xr, yr;
double _7deg = RAD2DEG(7.);
if(!isnan(G.X)){
xr = DEG2RAD(G.X);
if(G.relative) xr += M.X;
if(G.relative) xr += M.X.val;
xtag = &xr;
// set max speed
Mount.setSpeed(&_7deg, NULL);
}
if(!isnan(G.Y)){
yr = DEG2RAD(G.Y);
if(G.relative) yr += M.Y;
if(G.relative) yr += M.Y.val;
ytag = &yr;
Mount.setSpeed(NULL, &_7deg);
}
@ -126,12 +126,14 @@ int main(int _U_ argc, char _U_ **argv){
sleep(1);
waitmoving(G.Ncycles);
if(!getPos(&M, NULL)) WARNX("Can't get current position");
else printf("New mount position: X=%g, Y=%g\n", RAD2DEG(M.X), RAD2DEG(M.Y));
else printf("New mount position: X=%g, Y=%g\n", RAD2DEG(M.X.val), RAD2DEG(M.Y.val));
}
out:
DBG("JOIN");
if(G.coordsoutput) pthread_join(dthr, NULL);
DBG("QUIT");
if(G.wait){
if(getPos(&M, NULL)) printf("Mount position: X=%g, Y=%g\n", RAD2DEG(M.X), RAD2DEG(M.Y));
if(getPos(&M, NULL)) printf("Mount position: X=%g, Y=%g\n", RAD2DEG(M.X.val), RAD2DEG(M.Y.val));
Mount.quit();
}
return 0;

19
LibSidServo/examples/scmd_traectory.c
View File

@ -89,20 +89,21 @@ static void *dumping(void _U_ *u){
// calculate
static void runtraectory(traectory_fn tfn){
if(!tfn) return;
coords_t telXY, traectXY;
double t0 = sl_dtime();
uint32_t susec_last = 0;
coordval_pair_t telXY;
coordpair_t traectXY;
double t0 = Mount.currentT();
double tlast = 0.;
while(1){
if(!telpos(&telXY)){
WARNX("No next telescope position");
return;
}
if(telXY.msrtime.tv_usec == susec_last) continue; // last measure - don't mind
susec_last = telXY.msrtime.tv_usec;
double t = sl_dtime();
if(telXY.X > G.Xmax || telXY.Y > G.Ymax || t - t0 > G.tmax) break;
if(telXY.X.t == tlast && telXY.Y.t == tlast) continue; // last measure - don't mind
tlast = (telXY.X.t + telXY.Y.t) / 2.;
double t = Mount.currentT();
if(telXY.X.val > G.Xmax || telXY.Y.val > G.Ymax || t - t0 > G.tmax) break;
if(!traectory_point(&traectXY, t)) break;
DBG("%g: dX=%.1f'', dY=%.1f''", t-t0, RAD2ASEC(traectXY.X-telXY.X), RAD2ASEC(traectXY.Y-telXY.Y));
DBG("%g: dX=%.1f'', dY=%.1f''", t-t0, RAD2ASEC(traectXY.X-telXY.X.val), RAD2ASEC(traectXY.Y-telXY.Y.val));
}
WARNX("No next traectory point");
}
@ -133,7 +134,7 @@ int main(int argc, char **argv){
print_tr_names();
return 1;
}
coords_t c = {.X = DEG2RAD(G.X0), .Y = DEG2RAD(G.Y0)};
coordpair_t c = {.X = DEG2RAD(G.X0), .Y = DEG2RAD(G.Y0)};
if(!init_traectory(tfn, &c)){
ERRX("Can't init traectory");
return 1;

3
LibSidServo/examples/servo.conf
View File

@ -4,5 +4,6 @@ EncoderXDevPath=/dev/encoder_X0
EncoderYDevPath=/dev/encoder_Y0
MountReqInterval=0.05
SepEncoder=2
EncoderReqInterval=0.01
EncoderReqInterval=0.001
EncoderDevSpeed=1000000
EncoderSpeedInterval=0.05

39
LibSidServo/examples/traectories.c
View File

@ -28,12 +28,12 @@
static traectory_fn cur_traectory = NULL;
// starting point of traectory
static coords_t XYstart = {0};
static coordpair_t XYstart = {0};
static double tstart = 0.;
// convert Xe/Ye to approximate motor coordinates:
// Xnew = Xcor+Xe; Ynew = Ycor+Ye; as Ye goes backwards to Ym, we have
// Xcor = Xm0 - Xe0; Ycor = Xm0 + Ye0
static coords_t XYcor = {0};
static coordval_pair_t XYcor = {0};
/**
* @brief init_traectory - init traectory fn, sync starting positions of motor & encoders
@ -41,20 +41,20 @@ static coords_t XYcor = {0};
* @param XY0 - starting point
* @return FALSE if failed
*/
int init_traectory(traectory_fn f, coords_t *XY0){
int init_traectory(traectory_fn f, coordpair_t *XY0){
if(!f || !XY0) return FALSE;
cur_traectory = f;
XYstart = *XY0;
tstart = sl_dtime();
tstart = Mount.currentT();
mountdata_t mdata;
int ntries = 0;
for(; ntries < 10; ++ntries){
if(MCC_E_OK == Mount.getMountData(&mdata)) break;
}
if(ntries == 10) return FALSE;
XYcor.X = mdata.motposition.X - mdata.encposition.X;
XYcor.Y = mdata.motposition.X + mdata.encposition.Y;
DBG("STARTING POINTS: x=%g, y=%g degrees", DEG2RAD(XYcor.X), DEG2RAD(XYcor.Y));
XYcor.X.val = mdata.motXposition.val - mdata.encXposition.val;
XYcor.Y.val = mdata.motYposition.val - mdata.encYposition.val;
DBG("STARTING POINTS: x=%g, y=%g degrees", DEG2RAD(XYcor.X.val), DEG2RAD(XYcor.Y.val));
return TRUE;
}
@ -64,12 +64,10 @@ int init_traectory(traectory_fn f, coords_t *XY0){
* @param t - UNIX-time of event
* @return FALSE if something wrong (e.g. X not in -90..90 or Y not in -180..180)
*/
int traectory_point(coords_t *nextpt, double t){
int traectory_point(coordpair_t *nextpt, double t){
if(t < 0. || !cur_traectory) return FALSE;
coords_t pt;
coordpair_t pt;
if(!cur_traectory(&pt, t)) return FALSE;
pt.msrtime.tv_sec = floor(t);
pt.msrtime.tv_usec = (uint32_t) t - pt.msrtime.tv_sec;
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;
return TRUE;
@ -77,24 +75,25 @@ int traectory_point(coords_t *nextpt, double t){
// current telescope position according to starting motor coordinates
// @return FALSE if failed to get current coordinates
int telpos(coords_t *curpos){
int telpos(coordval_pair_t *curpos){
mountdata_t mdata;
int ntries = 0;
for(; ntries < 10; ++ntries){
if(MCC_E_OK == Mount.getMountData(&mdata)) break;
}
if(ntries == 10) return FALSE;
coords_t pt;
pt.X = XYcor.X + mdata.encposition.X;
pt.Y = XYcor.Y + mdata.encposition.Y;
pt.msrtime = mdata.encposition.msrtime;
coordval_pair_t pt;
pt.X.val = XYcor.X.val + mdata.encXposition.val;
pt.Y.val = XYcor.Y.val + mdata.encYposition.val;
pt.X.t = mdata.encXposition.t;
pt.Y.t = mdata.encYposition.t;
if(curpos) *curpos = pt;
return TRUE;
}
// X=X0+1'/s, Y=Y0+15''/s
int Linear(coords_t *nextpt, double t){
coords_t pt;
int Linear(coordpair_t *nextpt, double t){
coordpair_t pt;
pt.X = XYstart.X + ASEC2RAD(1.) * (t - tstart);
pt.Y = XYstart.Y + ASEC2RAD(15.)* (t - tstart);
if(nextpt) *nextpt = pt;
@ -102,8 +101,8 @@ int Linear(coords_t *nextpt, double t){
}
// X=X0+5'*sin(t/30*2pi), Y=Y0+10'*cos(t/200*2pi)
int SinCos(coords_t *nextpt, double t){
coords_t pt;
int SinCos(coordpair_t *nextpt, double t){
coordpair_t pt;
pt.X = XYstart.X + AMIN2RAD(5.) * sin((t-tstart)/30.*2*M_PI);
pt.Y = XYstart.Y + AMIN2RAD(10.)* cos((t-tstart)/200.*2*M_PI);
if(nextpt) *nextpt = pt;

12
LibSidServo/examples/traectories.h
View File

@ -21,12 +21,12 @@
#include "sidservo.h"
// traectory
typedef int (*traectory_fn)(coords_t *, double);
typedef int (*traectory_fn)(coordpair_t *, double);
int init_traectory(traectory_fn f, coords_t *XY0);
int init_traectory(traectory_fn f, coordpair_t *XY0);
traectory_fn traectory_by_name(const char *name);
void print_tr_names();
int traectory_point(coords_t *nextpt, double t);
int telpos(coords_t *curpos);
int Linear(coords_t *nextpt, double t);
int SinCos(coords_t *nextpt, double t);
int traectory_point(coordpair_t *nextpt, double t);
int telpos(coordval_pair_t *curpos);
int Linear(coordpair_t *nextpt, double t);
int SinCos(coordpair_t *nextpt, double t);

2
LibSidServo/libsidservo.config
View File

@ -1,6 +1,6 @@
// Add predefined macros for your project here. For example:
// #define THE_ANSWER 42
#define EBUG
#define _POSIX_C_SOURCE
// #define _POSIX_C_SOURCE 111
#define PACKAGE_VERSION "0.0.1"

37
LibSidServo/libsidservo.creator.user
View File

@ -1,6 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?>
<!DOCTYPE QtCreatorProject>
<!-- Written by QtCreator 15.0.1, 2025-03-02T22:49:47. -->
<!-- Written by QtCreator 17.0.0, 2025-07-22T23:51:22. -->
<qtcreator>
<data>
<variable>EnvironmentId</variable>
@ -13,8 +13,8 @@
<data>
<variable>ProjectExplorer.Project.EditorSettings</variable>
<valuemap type="QVariantMap">
<value type="bool" key="EditorConfiguration.AutoDetect">true</value>
<value type="bool" key="EditorConfiguration.AutoIndent">true</value>
<value type="bool" key="EditorConfiguration.AutoSpacesForTabs">false</value>
<value type="bool" key="EditorConfiguration.CamelCaseNavigation">true</value>
<valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.0">
<value type="QString" key="language">Cpp</value>
@ -94,6 +94,7 @@
<variable>ProjectExplorer.Project.Target.0</variable>
<valuemap type="QVariantMap">
<value type="QString" key="DeviceType">Desktop</value>
<value type="bool" key="HasPerBcDcs">true</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Desktop</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Desktop</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">{65a14f9e-e008-4c1b-89df-4eaa4774b6e3}</value>
@ -135,6 +136,38 @@
<valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.UserEnvironmentChanges"/>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">По умолчанию</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericBuildConfiguration</value>
<value type="qlonglong" key="ProjectExplorer.Target.ActiveDeployConfiguration">0</value>
<value type="qlonglong" key="ProjectExplorer.Target.ActiveRunConfiguration">0</value>
<valuemap type="QVariantMap" key="ProjectExplorer.Target.DeployConfiguration.0">
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">0</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Deploy</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Deploy</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Deploy</value>
</valuemap>
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">1</value>
<valuemap type="QVariantMap" key="ProjectExplorer.DeployConfiguration.CustomData"/>
<value type="bool" key="ProjectExplorer.DeployConfiguration.CustomDataEnabled">false</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.DefaultDeployConfiguration</value>
</valuemap>
<value type="qlonglong" key="ProjectExplorer.Target.DeployConfigurationCount">1</value>
<valuemap type="QVariantMap" key="ProjectExplorer.Target.RunConfiguration.0">
<value type="bool" key="Analyzer.Perf.Settings.UseGlobalSettings">true</value>
<value type="bool" key="Analyzer.QmlProfiler.Settings.UseGlobalSettings">true</value>
<value type="bool" key="Analyzer.Valgrind.Settings.UseGlobalSettings">true</value>
<valuelist type="QVariantList" key="CustomOutputParsers"/>
<value type="int" key="PE.EnvironmentAspect.Base">2</value>
<valuelist type="QVariantList" key="PE.EnvironmentAspect.Changes"/>
<value type="bool" key="PE.EnvironmentAspect.PrintOnRun">false</value>
<value type="QString" key="PerfRecordArgsId">-e cpu-cycles --call-graph dwarf,4096 -F 250</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.CustomExecutableRunConfiguration</value>
<value type="QString" key="ProjectExplorer.RunConfiguration.BuildKey"></value>
<value type="bool" key="ProjectExplorer.RunConfiguration.Customized">false</value>
<value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value>
<value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value>
</valuemap>
<value type="qlonglong" key="ProjectExplorer.Target.RunConfigurationCount">1</value>
</valuemap>
<value type="qlonglong" key="ProjectExplorer.Target.BuildConfigurationCount">1</value>
<valuemap type="QVariantMap" key="ProjectExplorer.Target.DeployConfiguration.0">

1
LibSidServo/libsidservo.files
View File

@ -6,6 +6,7 @@ examples/conf.h
examples/dump.c
examples/dump.h
examples/dumpmoving.c
examples/dumpmoving_dragNtrack.c
examples/dumpmoving_scmd.c
examples/dumpswing.c
examples/goto.c

14
LibSidServo/main.c
View File

@ -66,6 +66,10 @@ static mcc_errcodes_t init(conf_t *c){
DBG("Bad value of MountReqInterval");
ret = MCC_E_BADFORMAT;
}
if(Conf.EncoderSpeedInterval < Conf.EncoderReqInterval * MCC_CONF_MIN_SPEEDC || Conf.EncoderSpeedInterval > MCC_CONF_MAX_SPEEDINT){
DBG("Wrong speed interval");
ret = MCC_E_BADFORMAT;
}
uint8_t buf[1024];
data_t d = {.buf = buf, .len = 0, .maxlen = 1024};
// read input data as there may be some trash on start
@ -93,6 +97,12 @@ static int chkYs(double s){
return TRUE;
}
static mcc_errcodes_t slew2(const coordpair_t *target, slewflags_t flags){
(void)target;
(void)flags;
return MCC_E_FAILED;
}
/**
* @brief move2 - simple move to given point and stop
@ -144,7 +154,7 @@ static mcc_errcodes_t setspeed(const double *X, const double *Y){
* @param speed (i) - speed or NULL
* @return
*/
static mcc_errcodes_t move2s(const coords_t *target, const coords_t *speed){
static mcc_errcodes_t move2s(const coordpair_t *target, const coordpair_t *speed){
if(!target && !speed) return MCC_E_BADFORMAT;
if(!target) return setspeed(&speed->X, &speed->Y);
if(!speed) return move2(&target->X, &target->Y);
@ -351,6 +361,7 @@ mount_t Mount = {
.init = init,
.quit = quit,
.getMountData = getMD,
.slewTo = slew2,
.moveTo = move2,
.moveWspeed = move2s,
.setSpeed = setspeed,
@ -360,4 +371,5 @@ mount_t Mount = {
.longCmd = longcmd,
.getHWconfig = get_hwconf,
.saveHWconfig = write_hwconf,
.currentT = dtime,
};

0
LibSidServo/movingfilter.c → LibSidServo/movingfilter.c-
View File

108
LibSidServo/serial.c
View File

@ -37,13 +37,15 @@
static int encfd[2] = {-1, -1}, mntfd = -1;
// main mount data
static mountdata_t mountdata = {0};
// last encoders time and last encoders data - for speed measurement
static coordval_t lastXenc = {0}, lastYenc = {0};
// mutexes for RW operations with mount device and data
static pthread_mutex_t mntmutex = PTHREAD_MUTEX_INITIALIZER,
datamutex = PTHREAD_MUTEX_INITIALIZER;
// encoders thread and mount thread
static pthread_t encthread, mntthread;
// max timeout for 1.5 bytes of encoder and 2 bytes of mount
// max timeout for 1.5 bytes of encoder and 2 bytes of mount - for `select`
static struct timeval encRtmout = {0}, mntRtmout = {0};
// encoders raw data
typedef struct __attribute__((packed)){
@ -64,7 +66,13 @@ double dtime(){
t = tv.tv_sec + ((double)tv.tv_usec)/1e6;
return t;
}
#if 0
double tv2d(struct timeval *tv){
if(!tv) return 0.;
double t = tv->tv_sec + ((double)tv->tv_usec) / 1e6;
return t;
}
#endif
#if 0
// init start time
static void gttime(){
@ -75,12 +83,26 @@ static void gttime(){
}
#endif
// calculate current X/Y speeds
static void getXspeed(double t){
mountdata.encXspeed.val = (mountdata.encXposition.val - lastXenc.val) / (t - lastXenc.t);
mountdata.encXspeed.t = (lastXenc.t + t) / 2.;
lastXenc.val = mountdata.encXposition.val;
lastXenc.t = t;
}
static void getYspeed(double t){
mountdata.encYspeed.val = (mountdata.encYposition.val - lastYenc.val) / (t - lastYenc.t);
mountdata.encYspeed.t = (lastYenc.t + t) / 2.;
lastYenc.val = mountdata.encYposition.val;
lastYenc.t = t;
}
/**
* @brief parse_encbuf - check encoder buffer (for separate encoder) and fill fresh data
* @brief parse_encbuf - check encoder buffer (for encoder data based on SSII proto) and fill fresh data
* @param databuf - input buffer with 13 bytes of data
* @param nexttime - time when databuf[0] got
* @param t - time when databuf[0] got
*/
static void parse_encbuf(uint8_t databuf[ENC_DATALEN], struct timeval *tv){
static void parse_encbuf(uint8_t databuf[ENC_DATALEN], double t){
enc_t *edata = (enc_t*) databuf;
/*
#ifdef EBUG
@ -115,9 +137,13 @@ static void parse_encbuf(uint8_t databuf[ENC_DATALEN], struct timeval *tv){
return;
}
pthread_mutex_lock(&datamutex);
mountdata.encposition.X = X_ENC2RAD(edata->encX);
mountdata.encposition.Y = Y_ENC2RAD(edata->encY);
mountdata.encposition.msrtime = *tv;
mountdata.encXposition.val = X_ENC2RAD(edata->encX);
mountdata.encYposition.val = Y_ENC2RAD(edata->encY);
DBG("Got positions X/Y= %.6g / %.6g", mountdata.encXposition.val, mountdata.encYposition.val);
mountdata.encXposition.t = t;
mountdata.encYposition.t = t;
if(t - lastXenc.t > Conf.EncoderSpeedInterval) getXspeed(t);
if(t - lastYenc.t > Conf.EncoderSpeedInterval) getYspeed(t);
pthread_mutex_unlock(&datamutex);
//DBG("time = %zd+%zd/1e6, X=%g deg, Y=%g deg", tv->tv_sec, tv->tv_usec, mountdata.encposition.X*180./M_PI, mountdata.encposition.Y*180./M_PI);
}
@ -126,10 +152,10 @@ static void parse_encbuf(uint8_t databuf[ENC_DATALEN], struct timeval *tv){
* @brief getencval - get uint64_t data from encoder
* @param fd - encoder fd
* @param val - value read
* @param tv - measurement time
* @param t - measurement time
* @return amount of data read or 0 if problem
*/
static int getencval(int fd, double *val, struct timeval *tv){
static int getencval(int fd, double *val, double *t){
if(fd < 0) return FALSE;
char buf[128];
int got = 0, Lmax = 127;
@ -152,7 +178,7 @@ static int getencval(int fd, double *val, struct timeval *tv){
buf[got] = 0;
} else continue;
if(strchr(buf, '\n')) break;
}while(Lmax && dtime() - t0 > Conf.EncoderReqInterval);
}while(Lmax && dtime() - t0 < Conf.EncoderReqInterval);
if(got == 0) return 0; // WTF?
char *estr = strrchr(buf, '\n');
if(!estr) return 0;
@ -167,7 +193,7 @@ static int getencval(int fd, double *val, struct timeval *tv){
return 0; // wrong number
}
if(val) *val = (double) data;
if(tv) gettimeofday(tv, NULL);
if(t) *t = t0;
return got;
}
// try to read 1 byte from encoder; return -1 if nothing to read or -2 if device seems to be disconnected
@ -175,11 +201,10 @@ static int getencbyte(){
if(encfd[0] < 0) return -1;
uint8_t byte = 0;
fd_set rfds;
struct timeval tv;
do{
FD_ZERO(&rfds);
FD_SET(encfd[0], &rfds);
tv = encRtmout;
struct timeval tv = encRtmout;
int retval = select(encfd[0] + 1, &rfds, NULL, NULL, &tv);
if(!retval) break;
if(retval < 0){
@ -199,7 +224,6 @@ static int getmntbyte(){
if(mntfd < 0) return -1;
uint8_t byte;
fd_set rfds;
struct timeval tv;
/* ssize_t l = read(mntfd, &byte, 1);
//DBG("MNT read=%zd byte=0x%X", l, byte);
if(l == 0) return -1;
@ -208,7 +232,7 @@ static int getmntbyte(){
do{
FD_ZERO(&rfds);
FD_SET(mntfd, &rfds);
tv = mntRtmout;
struct timeval tv = mntRtmout;
int retval = select(mntfd + 1, &rfds, NULL, NULL, &tv);
if(retval < 0){
if(errno == EINTR) continue;
@ -231,7 +255,7 @@ static void *encoderthread1(void _U_ *u){
if(Conf.SepEncoder != 1) return NULL;
uint8_t databuf[ENC_DATALEN];
int wridx = 0, errctr = 0;
struct timeval tv;
double t = 0.;
while(encfd[0] > -1 && errctr < MAX_ERR_CTR){
int b = getencbyte();
if(b == -2) ++errctr;
@ -242,12 +266,12 @@ static void *encoderthread1(void _U_ *u){
if((uint8_t)b == ENC_MAGICK){
// DBG("Got magic -> start filling packet");
databuf[wridx++] = (uint8_t) b;
gettimeofday(&tv, NULL);
t = dtime();
}
continue;
}else databuf[wridx++] = (uint8_t) b;
if(wridx == ENC_DATALEN){
parse_encbuf(databuf, &tv);
parse_encbuf(databuf, t);
wridx = 0;
}
}
@ -258,30 +282,47 @@ static void *encoderthread1(void _U_ *u){
return NULL;
}
// main encoder thread for separate encoders as USB devices /dev/encoder_X0 and /dev/encoder_Y0
static void *encoderthread2(void _U_ *u){
if(Conf.SepEncoder != 2) return NULL;
DBG("Thread started");
int errctr = 0;
double t0 = dtime();
const char *req = "next\n";
int need2ask = 0; // need or not to ask encoder for new data
while(encfd[0] > -1 && encfd[1] > -1 && errctr < MAX_ERR_CTR){
if(5 != write(encfd[0], req, 5)) ++errctr;
if(5 != write(encfd[1], req, 5)) ++errctr;
double v;
struct timeval tv;
if(getencval(encfd[0], &v, &tv)){
mountdata.encposition.X = X_ENC2RAD(v);
mountdata.encposition.msrtime = tv;
if(getencval(encfd[1], &v, &tv)){
mountdata.encposition.Y = Y_ENC2RAD(v);
mountdata.encposition.msrtime = tv;
if(need2ask){
if(5 != write(encfd[0], req, 5)) { ++errctr; continue; }
else if(5 != write(encfd[1], req, 5)) { ++errctr; continue; }
}
double v, t;
if(getencval(encfd[0], &v, &t)){
mountdata.encXposition.val = X_ENC2RAD(v);
//DBG("encX(%g) = %g", t, mountdata.encXposition.val);
mountdata.encXposition.t = t;
if(t - lastXenc.t > Conf.EncoderSpeedInterval) getXspeed(t);
if(getencval(encfd[1], &v, &t)){
mountdata.encYposition.val = Y_ENC2RAD(v);
//DBG("encY(%g) = %g", t, mountdata.encYposition.val);
mountdata.encYposition.t = t;
if(t - lastYenc.t > Conf.EncoderSpeedInterval) getYspeed(t);
errctr = 0;
} else ++errctr;
} else ++errctr;
need2ask = 0;
} else {
if(need2ask) ++errctr;
else need2ask = 1;
continue;
}
} else {
if(need2ask) ++errctr;
else need2ask = 1;
continue;
}
while(dtime() - t0 < Conf.EncoderReqInterval){ usleep(10); }
//DBG("DT=%g (RI=%g)", dtime()-t0, Conf.EncoderReqInterval);
t0 = dtime();
}
DBG("ERRCTR=%d", errctr);
for(int i = 0; i < 2; ++i){
if(encfd[i] > -1){
close(encfd[i]);
@ -325,8 +366,7 @@ static void *mountthread(void _U_ *u){
*/
while(mntfd > -1 && errctr < MAX_ERR_CTR){
// read data to status
struct timeval tgot;
if(0 != gettimeofday(&tgot, NULL)) continue;
double tgot = dtime();
if(!MountWriteRead(cmd_getstat, &d) || d.len != sizeof(SSstat)){
#ifdef EBUG
DBG("Can't read SSstat, need %zd got %zd bytes", sizeof(SSstat), d.len);
@ -342,7 +382,7 @@ static void *mountthread(void _U_ *u){
errctr = 0;
pthread_mutex_lock(&datamutex);
// now change data
SSconvstat(status, &mountdata, &tgot);
SSconvstat(status, &mountdata, tgot);
pthread_mutex_unlock(&datamutex);
// allow writing & getters
//DBG("t0=%g, tnow=%g", t0-t00, dtime()-t00);

50
LibSidServo/sidservo.h
View File

@ -27,6 +27,11 @@ extern "C"
#include <stdint.h>
#include <sys/time.h>
// max speed interval, seconds
#define MCC_CONF_MAX_SPEEDINT (2.)
// minimal speed interval in parts of EncoderReqInterval
#define MCC_CONF_MIN_SPEEDC (3.)
// error codes
typedef enum{
MCC_E_OK = 0, // all OK
@ -47,13 +52,24 @@ typedef struct{
char* EncoderYDevPath;
double MountReqInterval; // interval between subsequent mount requests (seconds)
double EncoderReqInterval; // interval between subsequent encoder requests (seconds)
;
double EncoderSpeedInterval;// interval between speed calculations
} conf_t;
// coordinates in degrees: X, Y and time when they were reached
// coordinates/speeds in degrees or d/s: X, Y
typedef struct{
double X; double Y; struct timeval msrtime;
} coords_t;
double X; double Y;
} coordpair_t;
// coordinate/speed and time of last measurement
typedef struct{
double val;
double t;
} coordval_t;
typedef struct{
coordval_t X;
coordval_t Y;
} coordval_pair_t;
// data to read/write
typedef struct{
@ -80,8 +96,8 @@ typedef struct{
/* If 1, we are in computerless Slew and Track mode
(no clutches; use handpad to slew; must be in Drag and Track mode too) */
uint8_t slewtrack :1;
uint8_t digin_sens :1; // Digital input from radio handpad receiver, or RA PEC Sensor sync
uint8_t digin :3; // Digital input from radio handpad receiver
uint8_t digin_sens :1; // Digital input from radio handpad receiver, or RA PEC Sensor sync
uint8_t digin :3; // Digital input from radio handpad receiver
} ybits_t;
typedef struct{
@ -93,9 +109,13 @@ typedef struct{
} extradata_t;
typedef struct{
coords_t motposition;
coords_t encposition;
coords_t lastmotposition;
coordval_t motXposition;
coordval_t motYposition;
coordval_t encXposition;
coordval_t encYposition;
// TODO: add speedX/Y
coordval_t encXspeed; // once per <config> s
coordval_t encYspeed;
uint8_t keypad;
extradata_t extradata;
uint32_t millis;
@ -168,13 +188,22 @@ typedef struct{
double backlspd; // Backlash speed (rad/s)
} hardware_configuration_t;
// flags for slew function
typedef struct{
uint32_t slewNguide : 1; // ==1 to gude after slewing
} slewflags_t;
// mount class
typedef struct{
// TODO: on init/quit clear all XY-bits to default`
mcc_errcodes_t (*init)(conf_t *c); // init device
void (*quit)(); // deinit
mcc_errcodes_t (*getMountData)(mountdata_t *d); // get last data
// TODO: change (or add flags) switching slew-and-stop and slew-and-track
// add mount state: stop/slew/guide
mcc_errcodes_t (*slewTo)(const coordpair_t *target, slewflags_t flags);
mcc_errcodes_t (*moveTo)(const double *X, const double *Y); // move to given position ans stop
mcc_errcodes_t (*moveWspeed)(const coords_t *target, const coords_t *speed); // move with given max speed
mcc_errcodes_t (*moveWspeed)(const coordpair_t *target, const coordpair_t *speed); // move with given max speed
mcc_errcodes_t (*setSpeed)(const double *X, const double *Y); // set speed
mcc_errcodes_t (*stop)(); // stop
mcc_errcodes_t (*emergStop)(); // emergency stop
@ -182,6 +211,7 @@ typedef struct{
mcc_errcodes_t (*longCmd)(long_command_t *cmd); // send/get long command
mcc_errcodes_t (*getHWconfig)(hardware_configuration_t *c); // get hardware configuration
mcc_errcodes_t (*saveHWconfig)(hardware_configuration_t *c); // save hardware configuration
double (*currentT)(); // current time
} mount_t;
extern mount_t Mount;

27
LibSidServo/ssii.c
View File

@ -37,11 +37,12 @@ uint16_t SScalcChecksum(uint8_t *buf, int len){
/**
* @brief SSconvstat - convert stat from SSII format to human
* @param status (i) - just read data
* @param mountdata (o) - output
* @param s (i) - just read data
* @param m (o) - output
* @param t - measurement time
*/
void SSconvstat(const SSstat *s, mountdata_t *m, struct timeval *tdat){
if(!s || !m || !tdat) return;
void SSconvstat(const SSstat *s, mountdata_t *m, double t){
if(!s || !m) return;
/*
#ifdef EBUG
static double t0 = -1.;
@ -49,18 +50,18 @@ void SSconvstat(const SSstat *s, mountdata_t *m, struct timeval *tdat){
#endif
DBG("Convert, t=%g", dtime()-t0);
*/
m->motposition.X = X_MOT2RAD(s->Xmot);
m->motposition.Y = Y_MOT2RAD(s->Ymot);
m->motposition.msrtime = *tdat;
m->motXposition.val = X_MOT2RAD(s->Xmot);
m->motYposition.val = Y_MOT2RAD(s->Ymot);
m->motXposition.t = m->motYposition.t = t;
// fill encoder data from here, as there's no separate enc thread
if(!Conf.SepEncoder){
m->encposition.X = X_ENC2RAD(s->Xenc);
m->encposition.Y = Y_ENC2RAD(s->Yenc);
m->encposition.msrtime = *tdat;
m->encXposition.val = X_ENC2RAD(s->Xenc);
m->encYposition.val = Y_ENC2RAD(s->Yenc);
m->encXposition.t = m->encYposition.t = t;
}
m->lastmotposition.X = X_MOT2RAD(s->XLast);
m->lastmotposition.Y = Y_MOT2RAD(s->YLast);
m->lastmotposition.msrtime = *tdat;
//m->lastmotposition.X = X_MOT2RAD(s->XLast);
//m->lastmotposition.Y = Y_MOT2RAD(s->YLast);
//m->lastmotposition.msrtime = *tdat;
m->keypad = s->keypad;
m->extradata.ExtraBits = s->ExtraBits;
m->extradata.ain0 = s->ain0;

11
LibSidServo/ssii.h
View File

@ -169,7 +169,9 @@
#define SITECH_LOOP_FREQUENCY (1953.)
// steps per revolution
// 13312000 / 4 = 3328000
#define X_MOT_STEPSPERREV (3325952.)
// 17578668 / 4 = 4394667
#define Y_MOT_STEPSPERREV (4394960.)
// maximal speeds in rad/s: 10deg/s by X and 8deg/s by Y
@ -199,9 +201,12 @@
// encoder per revolution
#define X_ENC_STEPSPERREV (67108864.)
#define Y_ENC_STEPSPERREV (67108864.)
// encoder zero position
#define X_ENC_ZERO (46033555)
#define Y_ENC_ZERO (36674010)
// encoder position to radians and back
#define X_ENC2RAD(n) (2.*M_PI * ((double)n) / X_ENC_STEPSPERREV)
#define Y_ENC2RAD(n) (2.*M_PI * ((double)n) / Y_ENC_STEPSPERREV)
#define X_ENC2RAD(n) (2.*M_PI * ((double)(n-X_ENC_ZERO)) / X_ENC_STEPSPERREV)
#define Y_ENC2RAD(n) (2.*M_PI * ((double)(n-Y_ENC_ZERO)) / Y_ENC_STEPSPERREV)
#define X_RAD2ENC(r) ((uint32_t)((r) / 2./M_PI * X_ENC_STEPSPERREV))
#define Y_RAD2ENC(r) ((uint32_t)((r) / 2./M_PI * Y_ENC_STEPSPERREV))
@ -304,7 +309,7 @@ typedef struct{
} __attribute__((packed)) SSconfig;
uint16_t SScalcChecksum(uint8_t *buf, int len);
void SSconvstat(const SSstat *status, mountdata_t *mountdata, struct timeval *tdat);
void SSconvstat(const SSstat *status, mountdata_t *mountdata, double t);
int SStextcmd(const char *cmd, data_t *answer);
int SSrawcmd(const char *cmd, data_t *answer);
int SSgetint(const char *cmd, int64_t *ans);