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

add LibSidServo - pre-pre-...-pre-alpha

Browse Source
This commit is contained in:
Edward Emelianov
2025-02-19 23:09:17 +03:00
parent 1d7c7b7d0e
commit 04c98ed557
30 changed files with 2932 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
Auxiliary_utils/LibSidServo/examples/CMakeLists.txt Normal file
View File

@@ -0,0 +1,12 @@
project(examples)
# common includes & library
include_directories(../)
link_libraries(sidservo usefull_macros -lm)
# exe list
add_executable(goto goto.c dump.c)
add_executable(dump dumpmoving.c dump.c)
add_executable(dump_s dumpmoving_scmd.c dump.c)
add_executable(dumpswing dumpswing.c dump.c)
add_executable(traectory_s scmd_traectory.c dump.c traectories.c)

144
Auxiliary_utils/LibSidServo/examples/dump.c Normal file
View File

@@ -0,0 +1,144 @@
/*
* 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/>.
*/
// logging of mount position
#include <usefull_macros.h>
#include "dump.h"
#include "simpleconv.h"
/**
* @brief logmnt - log mount data into file
* @param fcoords - file to dump
* @param m - mount data
*/
void logmnt(FILE *fcoords, mountdata_t *m){
if(!fcoords) return;
//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");
return;
}
if(t0 < 0.) t0 = m->motposition.msrtime.tv_sec + (double)(m->motposition.msrtime.tv_usec) / 1e6;
double t = m->motposition.msrtime.tv_sec + (double)(m->motposition.msrtime.tv_usec) / 1e6 - 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);
fflush(fcoords);
}
/**
* @brief dumpmoving - dump conf while moving
* @param fcoords - dump file
* @param t - max waiting time
* @param N - number of cycles to wait while motors aren't moving
*/
void dumpmoving(FILE *fcoords, double t, int N){
if(!fcoords) return;
mountdata_t mdata;
DBG("Start dump");
int ntries = 0;
for(; ntries < 10; ++ntries){
if(MCC_E_OK == Mount.getMountData(&mdata)) break;
}
if(ntries == 10){
WARNX("Can't get mount data");
LOGWARN("Can't get mount data");
}
uint32_t millis = mdata.millis;
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){
usleep(10000);
if(MCC_E_OK != Mount.getMountData(&mdata)){ WARNX("Can't get data"); continue;}
if(mdata.millis == millis) continue;
//DBG("Got new data, posX=%g, posY=%g", mdata.motposition.X, mdata.motposition.Y);
millis = mdata.millis;
if(fcoords) logmnt(fcoords, &mdata);
if(mdata.motposition.X != xlast || mdata.motposition.Y != ylast){
xlast = mdata.motposition.X;
ylast = mdata.motposition.Y;
ctr = 0;
}else ++ctr;
}
}
/**
* @brief waitmoving - wait until moving by both axes stops at least for N cycles
* @param N - amount of stopped cycles
*/
void waitmoving(int N){
mountdata_t mdata;
int ctr = -1;
uint32_t millis = 0;
double xlast = 0., ylast = 0.;
while(ctr < N){
usleep(10000);
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;
ctr = 0;
}else ++ctr;
}
}
/**
* @brief getMotPos - get current
* @param mot (o) - motor position (or NULL)
* @param Y (o) - encoder position (or NULL)
* @return FALSE if failed
*/
int getPos(coords_t *mot, coords_t *enc){
mountdata_t mdata;
int errcnt = 0;
do{
if(MCC_E_OK != Mount.getMountData(&mdata)) ++errcnt;
else{
errcnt = 0;
if(mdata.millis) break;
}
}while(errcnt < 10);
if(errcnt >= 10){
WARNX("Can't read mount status");
return FALSE;
}
if(mot) *mot = mdata.motposition;
if(enc) *enc = mdata.encposition;
return TRUE;
}
// check current position and go to 0 if non-zero
void chk0(int ncycles){
coords_t M;
if(!getPos(&M, NULL)) signals(2);
if(M.X || M.Y){
WARNX("Mount position isn't @ zero; moving");
Mount.moveTo(0., 0.);
waitmoving(ncycles);
green("Now mount @ zero\n");
}
}

29
Auxiliary_utils/LibSidServo/examples/dump.h Normal file
View File

@@ -0,0 +1,29 @@
/*
* 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/>.
*/
#pragma once
#include <stdio.h>
#include "sidservo.h"
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);
void chk0(int ncycles);

105
Auxiliary_utils/LibSidServo/examples/dumpmoving.c Normal file
View File

@@ -0,0 +1,105 @@
/*
* 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/>.
*/
// dump telescope moving using simplest goto command
#include <math.h>
#include <signal.h>
#include <stdio.h>
#include <time.h>
#include <usefull_macros.h>
#include "dump.h"
#include "sidservo.h"
#include "simpleconv.h"
typedef struct{
int help;
int verbose;
int Ncycles;
char *logfile;
char *coordsoutput;
} parameters;
static parameters G = {
.Ncycles = 40,
};
static FILE *fcoords = NULL;
static sl_option_t cmdlnopts[] = {
{"help", NO_ARGS, NULL, 'h', arg_int, APTR(&G.help), "show this help"},
{"verbose", NO_ARGS, NULL, 'v', arg_none, APTR(&G.verbose), "verbose level (each -v adds 1)"},
{"logfile", NEED_ARG, NULL, 'l', arg_string, APTR(&G.logfile), "log file name"},
{"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"},
end_option
};
void signals(int sig){
if(sig){
signal(sig, SIG_IGN);
DBG("Get signal %d, quit.\n", sig);
}
LOGERR("Exit with status %d", sig);
Mount.quit();
exit(sig);
}
static conf_t Config = {
.MountDevPath = "/dev/ttyUSB0",
.MountDevSpeed = 19200,
//.EncoderDevPath = "/dev/ttyUSB1",
//.EncoderDevSpeed = 153000,
.MountReqInterval = 0.1,
.SepEncoder = 0
};
int main(int argc, char **argv){
sl_init();
sl_parseargs(&argc, &argv, cmdlnopts);
if(G.help) sl_showhelp(-1, cmdlnopts);
sl_loglevel_e lvl = G.verbose + LOGLEVEL_ERR;
if(lvl >= LOGLEVEL_AMOUNT) lvl = LOGLEVEL_AMOUNT - 1;
if(G.logfile) OPENLOG(G.logfile, lvl, 1);
if(G.coordsoutput){
if(!(fcoords = fopen(G.coordsoutput, "w")))
ERRX("Can't open %s", G.coordsoutput);
}else fcoords = stdout;
logmnt(fcoords, NULL);
time_t curtime = time(NULL);
LOGMSG("Started @ %s", ctime(&curtime));
LOGMSG("Mount device %s @ %d", Config.MountDevPath, Config.MountDevSpeed);
LOGMSG("Encoder device %s @ %d", Config.EncoderDevPath, Config.EncoderDevSpeed);
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
if(MCC_E_OK != Mount.moveTo(DEG2RAD(45.), DEG2RAD(45.)))
ERRX("Can't move to 45, 45");
dumpmoving(fcoords, 30., G.Ncycles);
Mount.moveTo(0., 0.);
dumpmoving(fcoords, 30., G.Ncycles);
signals(0);
return 0;
}

174
Auxiliary_utils/LibSidServo/examples/dumpmoving_scmd.c Normal file
View File

@@ -0,0 +1,174 @@
/*
* 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/>.
*/
// dump telescope moving using short binary commands
#include <math.h>
#include <pthread.h>
#include <signal.h>
#include <stdio.h>
#include <string.h>
#include <time.h>
#include <usefull_macros.h>
#include "dump.h"
#include "sidservo.h"
#include "simpleconv.h"
typedef struct{
int help;
int Ncycles;
double reqint;
char *coordsoutput;
char *axis;
} parameters;
static parameters G = {
.Ncycles = 40,
.reqint = 0.1,
.axis = "X",
};
static FILE *fcoords = NULL;
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 or Y)"},
end_option
};
void signals(int sig){
if(sig){
signal(sig, SIG_IGN);
DBG("Get signal %d, quit.\n", sig);
}
Mount.quit();
exit(sig);
}
static conf_t Config = {
.MountDevPath = "/dev/ttyUSB0",
.MountDevSpeed = 19200,
//.EncoderDevPath = "/dev/ttyUSB1",
//.EncoderDevSpeed = 153000,
.MountReqInterval = 0.1,
.SepEncoder = 0
};
// dump thread
static void *dumping(void _U_ *u){
dumpmoving(fcoords, 3600., G.Ncycles);
return NULL;
}
// return TRUE if motor position is reached +- 0.1 degrees
#define XYcount (DEG2RAD(0.1))
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));
return TRUE;
}
// move X 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'){
cmd.Xmot = DEG2RAD(target);
cmd.Xspeed = DEG2RAD(speed);
}else{
cmd.Ymot = DEG2RAD(target);
cmd.Yspeed = DEG2RAD(speed);
}
SCMD();
if(!Wait(DEG2RAD(limit))) signals(9);
#undef SCMD
}
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")){
WARNX("\"Axis\" should be X or Y");
return 1;
}
if(G.coordsoutput){
if(!(fcoords = fopen(G.coordsoutput, "w")))
ERRX("Can't open %s", G.coordsoutput);
}else fcoords = stdout;
Config.MountReqInterval = G.reqint;
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
// move to X=40 degr with different speeds
pthread_t dthr;
chk0(G.Ncycles);
logmnt(fcoords, NULL);
if(pthread_create(&dthr, NULL, dumping, NULL)) ERRX("Can't run dump thread");
// goto 1 degr with 1'/s
move(10., 1., 1./60.);
// goto 2 degr with 2'/s
move(10., 2., 2./60.);
// goto 3 degr with 5'/s
move(10., 3., 5./60.);
// goto 4 degr with 10'/s
move(10., 4., 10./60.);
// and go back with 5deg/s
move(0., 0., 5.);
// be sure to move @ 0,0
Mount.moveTo(0., 0.);
// wait moving ends
pthread_join(dthr, NULL);
#undef SCMD
signals(0);
return 0;
}

172
Auxiliary_utils/LibSidServo/examples/dumpswing.c Normal file
View File

@@ -0,0 +1,172 @@
/*
* 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 <stdio.h>
#include <string.h>
#include <usefull_macros.h>
#include "dump.h"
#include "sidservo.h"
#include "simpleconv.h"
// swing telescope by given axis with given period and max amplitude, reqinterval=0.05 (min)
typedef struct{
int help;
int Ncycles;
int Nswings;
double period;
double amplitude;
char *coordsoutput;
char *axis;
} parameters;
static parameters G = {
.Ncycles = 20,
.axis = "X",
.Nswings = 10,
.period = 1.,
.amplitude = 5.,
};
static FILE *fcoords = NULL;
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: 20)"},
{"coordsfile", NEED_ARG, NULL, 'o', arg_string, APTR(&G.coordsoutput),"output file with coordinates log"},
{"axis", NEED_ARG, NULL, 'a', arg_string, APTR(&G.axis), "axis to move (X or Y)"},
{"period", NEED_ARG, NULL, 'p', arg_double, APTR(&G.period), "swinging period (could be not reached if amplitude is too small) - not more than 900s (default: 1)"},
{"amplitude", NEED_ARG, NULL, 'A', arg_double, APTR(&G.amplitude), "max amplitude (could be not reaced if period is too small) - not more than 45deg (default: 5)"},
{"nswings", NEED_ARG, NULL, 'N', arg_int, APTR(&G.Nswings), "amount of swing periods (default: 10)"},
end_option
};
void signals(int sig){
if(sig){
signal(sig, SIG_IGN);
DBG("Get signal %d, quit.\n", sig);
}
Mount.quit();
exit(sig);
}
static conf_t Config = {
.MountDevPath = "/dev/ttyUSB0",
.MountDevSpeed = 19200,
//.EncoderDevPath = "/dev/ttyUSB1",
//.EncoderDevSpeed = 153000,
.MountReqInterval = 0.05,
.SepEncoder = 0
};
// dump thread
static void *dumping(void _U_ *u){
dumpmoving(fcoords, 3600., G.Ncycles);
return NULL;
}
// wait until mount is stopped within 5 cycles or until time reached t
void waithalf(double t){
mountdata_t mdata;
int ctr = -1;
uint32_t millis = 0;
double xlast = 0., ylast = 0.;
while(ctr < 5){
if(sl_dtime() >= 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;
ctr = 0;
}else{
DBG("EQ: old=%g, now=%g", RAD2DEG(ylast), RAD2DEG(mdata.motposition.Y));
++ctr;
}
}
}
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")){
WARNX("\"Axis\" should be X or Y");
return 1;
}
if(G.coordsoutput){
if(!(fcoords = fopen(G.coordsoutput, "w")))
ERRX("Can't open %s", G.coordsoutput);
}else fcoords = stdout;
if(G.Ncycles < 7) ERRX("Ncycles should be >7");
if(G.amplitude < 0.01 || G.amplitude > 45.)
ERRX("Amplitude should be from 0.01 to 45 degrees");
if(G.period < 0.1 || G.period > 900.)
ERRX("Period should be from 0.1 to 900s");
if(G.Nswings < 1) ERRX("Nswings should be more than 0");
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
pthread_t dthr;
chk0(G.Ncycles);
logmnt(fcoords, NULL);
if(pthread_create(&dthr, NULL, dumping, NULL)) ERRX("Can't run dump thread");
G.period /= 2.; // pause between commands
double tagX, tagY;
if(*G.axis == 'X'){
tagX = DEG2RAD(G.amplitude); tagY = 0.;
}else{
tagX = 0.; tagY = DEG2RAD(G.amplitude);
}
double t = sl_dtime(), t0 = t;
double divide = 2.;
for(int i = 0; i < G.Nswings; ++i){
Mount.moveTo(tagX, tagY);
DBG("CMD: %g", sl_dtime()-t0);
t += G.period / divide;
divide = 1.;
waithalf(t);
DBG("Moved to +, t=%g", t-t0);
DBG("CMD: %g", sl_dtime()-t0);
Mount.moveTo(-tagX, -tagY);
t += G.period;
waithalf(t);
DBG("Moved to -, t=%g", t-t0);
DBG("CMD: %g", sl_dtime()-t0);
}
// be sure to move @ 0,0
Mount.moveTo(0., 0.);
// wait moving ends
pthread_join(dthr, NULL);
#undef SCMD
signals(0);
return 0;
}

117
Auxiliary_utils/LibSidServo/examples/goto.c Normal file
View File

@@ -0,0 +1,117 @@
/*
* 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 given MOTOR position in degrees
#include <signal.h>
#include <stdio.h>
#include <time.h>
#include <usefull_macros.h>
#include "dump.h"
#include "sidservo.h"
#include "simpleconv.h"
typedef struct{
int help;
int Ncycles;
int wait;
char *coordsoutput;
double X;
double Y;
} parameters;
static parameters G = {
.Ncycles = 40,
.X = NAN,
.Y = NAN,
};
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 of waiting in stopped state (default: 40)"},
{"newx", NEED_ARG, NULL, 'X', arg_double, APTR(&G.X), "new X coordinate"},
{"newy", NEED_ARG, NULL, 'Y', arg_double, APTR(&G.Y), "new Y coordinate"},
{"output", NEED_ARG, NULL, 'o', arg_string, APTR(&G.coordsoutput),"file to log coordinates"},
{"wait", NO_ARGS, NULL, 'w', arg_int, APTR(&G.wait), "wait until mowing stopped"},
end_option
};
static conf_t Config = {
.MountDevPath = "/dev/ttyUSB0",
.MountDevSpeed = 19200,
//.EncoderDevPath = "/dev/ttyUSB1",
//.EncoderDevSpeed = 153000,
.MountReqInterval = 0.1,
.SepEncoder = 0
};
static FILE* fcoords = NULL;
static pthread_t dthr;
void signals(int sig){
pthread_cancel(dthr);
if(sig){
signal(sig, SIG_IGN);
DBG("Get signal %d, quit.\n", sig);
}
Mount.quit();
if(fcoords) fclose(fcoords);
exit(sig);
}
// dump thread
static void *dumping(void _U_ *u){
dumpmoving(fcoords, 3600., G.Ncycles);
return NULL;
}
int main(int _U_ argc, char _U_ **argv){
sl_init();
sl_parseargs(&argc, &argv, cmdlnopts);
if(G.help) sl_showhelp(-1, cmdlnopts);
if(MCC_E_OK != Mount.init(&Config)) ERRX("Can't init mount");
coords_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'");
G.wait = 1;
}
if(G.coordsoutput){
if(!(fcoords = fopen(G.coordsoutput, "w")))
ERRX("Can't open %s", G.coordsoutput);
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));
if(isnan(G.X) && isnan(G.Y)) goto out;
if(isnan(G.X)) G.X = RAD2DEG(M.X);
if(isnan(G.Y)) G.Y = RAD2DEG(M.Y);
printf("Moving to X=%g deg, Y=%g deg\n", G.X, G.Y);
Mount.moveTo(DEG2RAD(G.X), DEG2RAD(G.Y));
if(G.wait){
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));
}
out:
if(G.coordsoutput) pthread_join(dthr, NULL);
if(G.wait) Mount.quit();
return 0;
}

159
Auxiliary_utils/LibSidServo/examples/scmd_traectory.c Normal file
View File

@@ -0,0 +1,159 @@
/*
* 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 "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 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 *tfn; // traectory function name
} parameters;
static FILE *fcoords = 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 X coordinate for traectory (default: 45 degrees)"},
{"ymax", NEED_ARG, NULL, 'Y', arg_double, APTR(&G.Ymax), "maximal X 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)"},
end_option
};
static conf_t Config = {
.MountDevPath = "/dev/ttyUSB0",
.MountDevSpeed = 19200,
//.EncoderDevPath = "/dev/ttyUSB1",
//.EncoderDevSpeed = 153000,
.MountReqInterval = 0.05,
.SepEncoder = 0
};
void signals(int sig){
pthread_cancel(dthr);
if(sig){
signal(sig, SIG_IGN);
DBG("Get signal %d, quit.\n", sig);
}
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;
coords_t telXY, traectXY;
double t0 = sl_dtime();
uint32_t susec_last = 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(!traectory_point(&traectXY, t)) break;
DBG("%g: dX=%.1f'', dY=%.1f''", t-t0, RAD2ASEC(traectXY.X-telXY.X), RAD2ASEC(traectXY.Y-telXY.Y));
}
WARNX("No next traectory point");
}
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.coordsoutput){
if(!(fcoords = fopen(G.coordsoutput, "w")))
ERRX("Can't open %s", G.coordsoutput);
}else fcoords = stdout;
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;
}
coords_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;
}

29
Auxiliary_utils/LibSidServo/examples/simpleconv.h Normal file
View File

@@ -0,0 +1,29 @@
/*
* 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/>.
*/
// simple conversion macros
#include <math.h>
#define DEG2RAD(d) (d/180.*M_PI)
#define ASEC2RAD(d) (d/180.*M_PI/3600.)
#define AMIN2RAD(d) (d/180.*M_PI/60.)
#define RAD2DEG(r) (r/M_PI*180.)
#define RAD2ASEC(r) (r/M_PI*180.*3600.)
#define RAD2AMIN(r) (r/M_PI*180.*60.)

143
Auxiliary_utils/LibSidServo/examples/traectories.c Normal file
View File

@@ -0,0 +1,143 @@
/*
* 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/>.
*/
// some simplest traectories
// all traectories runs increasing X and Y from starting point
#include <stdio.h>
#include <string.h>
#include <usefull_macros.h>
#include "simpleconv.h"
#include "traectories.h"
static traectory_fn cur_traectory = NULL;
// starting point of traectory
static coords_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};
/**
* @brief init_traectory - init traectory fn, sync starting positions of motor & encoders
* @param f - function calculating next point
* @param XY0 - starting point
* @return FALSE if failed
*/
int init_traectory(traectory_fn f, coords_t *XY0){
if(!f || !XY0) return FALSE;
cur_traectory = f;
XYstart = *XY0;
tstart = sl_dtime();
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));
return TRUE;
}
/**
* @brief traectory_point - get traectory point for given time
* @param nextpt (o) - next point coordinates
* @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){
if(t < 0. || !cur_traectory) return FALSE;
coords_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;
}
// current telescope position according to starting motor coordinates
// @return FALSE if failed to get current coordinates
int telpos(coords_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;
if(curpos) *curpos = pt;
return TRUE;
}
// X=X0+1'/s, Y=Y0+15''/s
int Linear(coords_t *nextpt, double t){
coords_t pt;
pt.X = XYstart.X + ASEC2RAD(1.) * (t - tstart);
pt.Y = XYstart.Y + ASEC2RAD(15.)* (t - tstart);
if(nextpt) *nextpt = pt;
return TRUE;
}
// X=X0+5'*sin(t/30*2pi), Y=Y0+10'*cos(t/200*2pi)
int SinCos(coords_t *nextpt, double t){
coords_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;
return TRUE;
}
typedef struct{
traectory_fn f;
const char *name;
const char *help;
} tr_names;
static tr_names names[] = {
{Linear, "linear", "X=X0+1'/s, Y=Y0+15''/s"},
{SinCos, "sincos", "X=X0+5'*sin(t/30*2pi), Y=Y0+10'*cos(t/200*2pi)"},
{NULL, NULL, NULL}
};
traectory_fn traectory_by_name(const char *name){
traectory_fn f = NULL;
for(int i = 0; ; ++i){
if(!names[i].f) break;
if(strcmp(names[i].name, name) == 0){
f = names[i].f;
break;
}
}
return f;
}
// print all acceptable traectories names with help
void print_tr_names(){
for(int i = 0; ; ++i){
if(!names[i].f) break;
printf("%s: %s\n", names[i].name, names[i].help);
}
}

32
Auxiliary_utils/LibSidServo/examples/traectories.h Normal file
View File

@@ -0,0 +1,32 @@
/*
* 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/>.
*/
#pragma once
#include "sidservo.h"
// traectory
typedef int (*traectory_fn)(coords_t *, double);
int init_traectory(traectory_fn f, coords_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);