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

change system time function to UNIX time

Browse Source
This commit is contained in:
Edward Emelianov 2025-11-01 14:59:37 +03:00
parent a7fbae47f0
commit 683da9739d
15 changed files with 98 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
LibSidServo/TODO
View File

@ -1,2 +0,0 @@
1. PID: slew2

2
LibSidServo/examples/conf.c
View File

@ -52,7 +52,7 @@ static sl_option_t opts[] = {
{"EncoderDevSpeed", NEED_ARG, NULL, 0, arg_int, APTR(&Config.EncoderDevSpeed), "serial speed of encoder device"},
{"MountReqInterval",NEED_ARG, NULL, 0, arg_double, APTR(&Config.MountReqInterval), "interval of mount requests (not less than 0.05s)"},
{"EncoderReqInterval",NEED_ARG, NULL, 0, arg_double, APTR(&Config.EncoderReqInterval),"interval of encoder requests (in case of sep=2)"},
{"SepEncoder", NO_ARGS, NULL, 0, arg_int, APTR(&Config.SepEncoder), "encoder is separate device (1 - one device, 2 - two devices)"},
{"SepEncoder", NEED_ARG, 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"},

12
LibSidServo/examples/dump.c
View File

@ -23,6 +23,9 @@
#include "dump.h"
#include "simpleconv.h"
// starting dump time (to conform different logs)
static double dumpT0 = -1.;
#if 0
// amount of elements used for encoders' data filtering
#define NFILT (10)
@ -59,6 +62,10 @@ static double filter(double val, int idx){
}
#endif
// return starting time of dump
double dumpt0(){ return dumpT0; }
/**
* @brief logmnt - log mount data into file
* @param fcoords - file to dump
@ -70,10 +77,10 @@ void logmnt(FILE *fcoords, mountdata_t *m){
if(!m){ // write header
fprintf(fcoords, "# time Xmot(deg) Ymot(deg) Xenc(deg) Yenc(deg) VX(d/s) VY(d/s) millis\n");
return;
}
}else if(dumpT0 < 0.) dumpT0 = m->encXposition.t;
// write data
fprintf(fcoords, "%12.6f %10.6f %10.6f %10.6f %10.6f %10.6f %10.6f %10u\n",
m->encXposition.t, RAD2DEG(m->motXposition.val), RAD2DEG(m->motYposition.val),
m->encXposition.t - dumpT0, 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);
@ -139,6 +146,7 @@ void waitmoving(int N){
if(mdata.motXposition.val != xlast || mdata.motYposition.val != ylast){
xlast = mdata.motXposition.val;
ylast = mdata.motYposition.val;
//DBG("x/y: %g/%g", RAD2DEG(xlast), RAD2DEG(ylast));
ctr = 0;
}else ++ctr;
}

1
LibSidServo/examples/dump.h
View File

@ -27,3 +27,4 @@ void dumpmoving(FILE *fcoords, double t, int N);
void waitmoving(int N);
int getPos(coordval_pair_t *mot, coordval_pair_t *enc);
void chk0(int ncycles);
double dumpt0();

2
LibSidServo/examples/dumpmoving_dragNtrack.c
View File

@ -216,7 +216,7 @@ int main(int argc, char **argv){
sleep(5);
// return to zero and wait
green("Return 2 zero and wait\n");
if(!return2zero()) ERRX("Can't return");
if(MCC_E_OK != return2zero()) ERRX("Can't return");
Wait(0., 0);
Wait(0., 1);
// wait moving ends

28
LibSidServo/examples/dumpswing.c
View File

@ -110,16 +110,28 @@ int main(int argc, char **argv){
return 1;
}
if(G.coordsoutput){
if(!(fcoords = fopen(G.coordsoutput, "w")))
ERRX("Can't open %s", G.coordsoutput);
if(!(fcoords = fopen(G.coordsoutput, "w"))){
WARNX("Can't open %s", G.coordsoutput);
return 1;
}
}else fcoords = stdout;
if(G.Ncycles < 7) ERRX("Ncycles should be >7");
if(G.Ncycles < 2){
WARNX("Ncycles should be >2");
return 1;
}
double absamp = fabs(G.amplitude);
if(absamp < 0.01 || absamp > 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");
if(absamp < 0.01 || absamp > 45.){
WARNX("Amplitude should be from 0.01 to 45 degrees");
return 1;
}
if(G.period < 0.1 || G.period > 900.){
WARNX("Period should be from 0.1 to 900s");
return 1;
}
if(G.Nswings < 1){
WARNX("Nswings should be more than 0");
return 1;
}
conf_t *Config = readServoConf(G.conffile);
if(!Config){
dumpConf();

5
LibSidServo/examples/goto.c
View File

@ -91,11 +91,10 @@ int main(int _U_ argc, char _U_ **argv){
if(MCC_E_OK != Mount.init(Config)) ERRX("Can't init mount");
coordval_pair_t M, E;
if(!getPos(&M, &E)) ERRX("Can't get current position");
DBG("xt: %g, x: %g", M.X.t, M.X.val);
if(G.coordsoutput){
if(!G.wait) green("When logging I should wait until moving ends; added '-w'");
if(!G.wait) green("When logging I should wait until moving ends; added '-w'\n");
G.wait = 1;
}
if(G.coordsoutput){
if(!(fcoords = fopen(G.coordsoutput, "w")))
ERRX("Can't open %s", G.coordsoutput);
logmnt(fcoords, NULL);

9
LibSidServo/examples/scmd_traectory.c
View File

@ -98,7 +98,7 @@ static void runtraectory(traectory_fn tfn){
coordval_pair_t target;
coordpair_t traectXY, endpoint;
endpoint.X = G.Xmax, endpoint.Y = G.Ymax;
double t0 = Mount.currentT(), tlast = 0.;
double t0 = dumpt0(), tlast = 0., tstart = Mount.currentT();
double tlastX = 0., tlastY = 0.;
while(1){
if(!telpos(&telXY)){
@ -109,7 +109,7 @@ static void runtraectory(traectory_fn tfn){
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 - t0 > G.tmax) break;
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;
@ -118,11 +118,14 @@ static void runtraectory(traectory_fn tfn){
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.4g %10.4g %10.4g\n", t, RAD2ASEC(traectXY.X-telXY.X.val), RAD2ASEC(traectXY.Y-telXY.Y.val));
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;

2
LibSidServo/examples/servo.conf
View File

@ -1,4 +1,4 @@
Current configuration:
# Current configuration
MountDevPath=/dev/ttyUSB0
MountDevSpeed=19200
EncoderDevPath=(null)

18
LibSidServo/libsidservo.creator.user
View File

@ -1,6 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?>
<!DOCTYPE QtCreatorProject>
<!-- Written by QtCreator 17.0.0, 2025-07-30T17:30:52. -->
<!-- Written by QtCreator 17.0.2, 2025-11-01T14:58:43. -->
<qtcreator>
<data>
<variable>EnvironmentId</variable>
@ -110,8 +110,8 @@
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericMakeStep</value>
</valuemap>
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Сборка</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Сборка</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Build</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Build</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Build</value>
</valuemap>
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.1">
@ -123,8 +123,8 @@
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericMakeStep</value>
</valuemap>
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Очистка</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Очистка</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Clean</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Clean</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Clean</value>
</valuemap>
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">2</value>
@ -139,8 +139,8 @@
<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">Развёртывание</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Развёртывание</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>
@ -173,8 +173,8 @@
<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">Развёртывание</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Развёртывание</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>

15
LibSidServo/main.c
View File

@ -51,17 +51,18 @@ static mcc_errcodes_t shortcmd(short_command_t *cmd);
* @return time in seconds
*/
double nanotime(){
static struct timespec *start = NULL;
//static struct timespec *start = NULL;
struct timespec now;
if(!start){
/*if(!start){
start = malloc(sizeof(struct timespec));
if(!start) return -1.;
if(clock_gettime(CLOCK_MONOTONIC, start)) return -1.;
}
}*/
if(clock_gettime(CLOCK_MONOTONIC, &now)) return -1.;
double nd = ((double)now.tv_nsec - (double)start->tv_nsec) * 1e-9;
/*double nd = ((double)now.tv_nsec - (double)start->tv_nsec) * 1e-9;
double sd = (double)now.tv_sec - (double)start->tv_sec;
return sd + nd;
return sd + nd;*/
return (double)now.tv_sec + (double)now.tv_nsec * 1e-9;
}
/**
@ -75,7 +76,7 @@ static void quit(){
DBG("Exit");
}
void getModData(coordval_pair_t *c){
void getModData(coordval_pair_t *c, movestate_t *xst, movestate_t *yst){
if(!c || !Xmodel || !Ymodel) return;
double tnow = nanotime();
moveparam_t Xp, Yp;
@ -87,6 +88,8 @@ void getModData(coordval_pair_t *c){
c->X.t = c->Y.t = tnow;
c->X.val = Xp.coord;
c->Y.val = Yp.coord;
if(xst) *xst = Xst;
if(yst) *yst = Yst;
}
/**

3
LibSidServo/main.h
View File

@ -24,11 +24,12 @@
#include <stdlib.h>
#include "movingmodel.h"
#include "sidservo.h"
extern conf_t Conf;
double nanotime();
void getModData(coordval_pair_t *c);
void getModData(coordval_pair_t *c, movestate_t *xst, movestate_t *yst);
typedef struct{
double *x, *t, *t2, *xt; // arrays of coord/time and multiply
double xsum, tsum, t2sum, xtsum; // sums of coord/time and their multiply

7
LibSidServo/movingmodel.c
View File

@ -60,9 +60,14 @@ movemodel_t *model_init(limits_t *l){
int model_move2(movemodel_t *model, moveparam_t *target, double t){
if(!target || !model) return FALSE;
//DBG("MOVE to %g at speed %g", target->coord, target->speed);
DBG("MOVE to %g at speed %g", target->coord, target->speed);
// only positive velocity
if(target->speed < 0.) target->speed = -target->speed;
if(fabs(target->speed) < model->Min.speed){
DBG("STOP");
model->stop(model, t);
return TRUE;
}
// don't mind about acceleration - user cannot set it now
return model->calculate(model, target, t);
}

6
LibSidServo/ramp.c
View File

@ -23,12 +23,14 @@
#include "main.h"
#include "ramp.h"
/*
#ifdef EBUG
#undef DBG
#define DBG(...)
#undef FNAME
#define FNAME()
#endif
*/
static double coord_tolerance = COORD_TOLERANCE_DEFAULT;
static void emstop(movemodel_t *m, double _U_ t){

30
LibSidServo/serial.c
View File

@ -59,13 +59,15 @@ typedef struct __attribute__((packed)){
// calculate current X/Y speeds
void getXspeed(){
static double t0 = -1.; // time of start - eliminate problem of very large times in squares
if(t0 < 0.) t0 = mountdata.encXposition.t;
static less_square_t *ls = NULL;
if(!ls){
ls = LS_init(Conf.EncoderSpeedInterval / Conf.EncoderReqInterval);
if(!ls) return;
}
pthread_mutex_lock(&datamutex);
double speed = LS_calc_slope(ls, mountdata.encXposition.val, mountdata.encXposition.t);
double speed = LS_calc_slope(ls, mountdata.encXposition.val, mountdata.encXposition.t - t0);
if(fabs(speed) < 1.5 * MCC_MAX_X_SPEED){
mountdata.encXspeed.val = speed;
mountdata.encXspeed.t = mountdata.encXposition.t;
@ -80,13 +82,15 @@ void getXspeed(){
#endif
}
void getYspeed(){
static double t0 = -1.; // time of start - eliminate problem of very large times in squares
if(t0 < 0.) t0 = mountdata.encXposition.t;
static less_square_t *ls = NULL;
if(!ls){
ls = LS_init(Conf.EncoderSpeedInterval / Conf.EncoderReqInterval);
if(!ls) return;
}
pthread_mutex_lock(&datamutex);
double speed = LS_calc_slope(ls, mountdata.encYposition.val, mountdata.encYposition.t);
double speed = LS_calc_slope(ls, mountdata.encYposition.val, mountdata.encYposition.t - t0);
if(fabs(speed) < 1.5 * MCC_MAX_Y_SPEED){
mountdata.encYspeed.val = speed;
mountdata.encYspeed.t = mountdata.encYposition.t;
@ -392,21 +396,31 @@ static void *mountthread(void _U_ *u){
uint8_t buf[2*sizeof(SSstat)];
SSstat *status = (SSstat*) buf;
bzero(&mountdata, sizeof(mountdata));
double t0 = nanotime();
double t0 = nanotime(), tstart = t0;
static double oldmt = -100.; // old `millis measurement` time
static uint32_t oldmillis = 0;
if(Conf.RunModel) while(1){
coordval_pair_t c;
movestate_t xst, yst;
// now change data
getModData(&c);
getModData(&c, &xst, &yst);
pthread_mutex_lock(&datamutex);
double tnow = c.X.t;
mountdata.motXposition.t = mountdata.encXposition.t = mountdata.motYposition.t = mountdata.encYposition.t = tnow;
mountdata.motXposition.val = mountdata.encXposition.val = c.X.val;
mountdata.motYposition.val = mountdata.encYposition.val = c.Y.val;
mountdata.encXposition.t = mountdata.encYposition.t = tnow;
mountdata.encXposition.val = c.X.val;
mountdata.encYposition.val = c.Y.val;
//DBG("t=%g, X=%g, Y=%g", tnow, c.X.val, c.Y.val);
if(tnow - oldmt > Conf.MountReqInterval){
oldmillis = mountdata.millis = (uint32_t)(tnow * 1e3);
oldmillis = mountdata.millis = (uint32_t)((tnow - tstart) * 1e3);
mountdata.motYposition.t = mountdata.motXposition.t = tnow;
if(xst == ST_MOVE)
mountdata.motXposition.val = c.X.val + (c.X.val - mountdata.motXposition.val)*(drand48() - 0.5)/100.;
else
mountdata.motXposition.val = c.X.val;
if(yst == ST_MOVE)
mountdata.motYposition.val = c.Y.val + (c.Y.val - mountdata.motYposition.val)*(drand48() - 0.5)/100.;
else
mountdata.motYposition.val = c.Y.val;
oldmt = tnow;
}else mountdata.millis = oldmillis;
pthread_mutex_unlock(&datamutex);