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

28
LibSidServo/examples/dumpswing.c
View File

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

17
LibSidServo/examples/scmd_traectory.c
View File

@ -98,7 +98,7 @@ static void runtraectory(traectory_fn tfn){
coordval_pair_t target; coordval_pair_t target;
coordpair_t traectXY, endpoint; coordpair_t traectXY, endpoint;
endpoint.X = G.Xmax, endpoint.Y = G.Ymax; 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.; double tlastX = 0., tlastY = 0.;
while(1){ while(1){
if(!telpos(&telXY)){ 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); 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; tlastX = telXY.X.t; tlastY = telXY.Y.t;
double t = Mount.currentT(); 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; if(!traectory_point(&traectXY, t)) break;
target.X.val = traectXY.X; target.Y.val = traectXY.Y; target.X.val = traectXY.X; target.Y.val = traectXY.Y;
target.X.t = target.Y.t = t; 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; else if(telXY.X.val < traectXY.X) endpoint.X = G.Xmax;
if(telXY.Y.val > traectXY.Y) endpoint.Y = -G.Ymax; if(telXY.Y.val > traectXY.Y) endpoint.Y = -G.Ymax;
else if(telXY.Y.val < traectXY.Y) endpoint.Y = G.Ymax; else if(telXY.Y.val < traectXY.Y) endpoint.Y = G.Ymax;
//DBG("target: %g'/%g'", RAD2AMIN(traectXY.X), RAD2AMIN(traectXY.Y)); if(t0 < 0.) t0 = dumpt0();
DBG("%g: dX=%.4f'', dY=%.4f''", t-t0, RAD2ASEC(traectXY.X-telXY.X.val), RAD2ASEC(traectXY.Y-telXY.Y.val)); else{
//DBG("Correct to: %g/%g with EP %g/%g", RAD2DEG(target.X.val), RAD2DEG(target.Y.val), RAD2DEG(endpoint.X), RAD2DEG(endpoint.Y)); //DBG("target: %g'/%g'", RAD2AMIN(traectXY.X), RAD2AMIN(traectXY.Y));
if(errlog) DBG("%g: dX=%.4f'', dY=%.4f''", t-t0, RAD2ASEC(traectXY.X-telXY.X.val), RAD2ASEC(traectXY.Y-telXY.Y.val));
fprintf(errlog, "%10.4g %10.4g %10.4g\n", t, RAD2ASEC(traectXY.X-telXY.X.val), RAD2ASEC(traectXY.Y-telXY.Y.val)); //DBG("Correct to: %g/%g with EP %g/%g", RAD2DEG(target.X.val), RAD2DEG(target.Y.val), RAD2DEG(endpoint.X), RAD2DEG(endpoint.Y));
if(errlog)
fprintf(errlog, "%10.4f %10.4f %10.4f\n", telXY.X.t-t0, RAD2ASEC(traectXY.X-telXY.X.val), RAD2ASEC(traectXY.Y-telXY.Y.val));
}
if(MCC_E_OK != Mount.correctTo(&target, &endpoint)) WARNX("Error of correction!"); if(MCC_E_OK != Mount.correctTo(&target, &endpoint)) WARNX("Error of correction!");
while((t = Mount.currentT()) - tlast < MCC_PID_REFRESH_DT) usleep(50); while((t = Mount.currentT()) - tlast < MCC_PID_REFRESH_DT) usleep(50);
tlast = t; tlast = t;

2
LibSidServo/examples/servo.conf
View File

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

18
LibSidServo/libsidservo.creator.user
View File

@ -1,6 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?> <?xml version="1.0" encoding="UTF-8"?>
<!DOCTYPE QtCreatorProject> <!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> <qtcreator>
<data> <data>
<variable>EnvironmentId</variable> <variable>EnvironmentId</variable>
@ -110,8 +110,8 @@
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericMakeStep</value> <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericMakeStep</value>
</valuemap> </valuemap>
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">1</value> <value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Сборка</value> <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Build</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Сборка</value> <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Build</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Build</value> <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Build</value>
</valuemap> </valuemap>
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.1"> <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.1">
@ -123,8 +123,8 @@
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericMakeStep</value> <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericMakeStep</value>
</valuemap> </valuemap>
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">1</value> <value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Очистка</value> <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Clean</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Очистка</value> <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Clean</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Clean</value> <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Clean</value>
</valuemap> </valuemap>
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">2</value> <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.Target.DeployConfiguration.0">
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0"> <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">0</value> <value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">0</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Развёртывание</value> <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Deploy</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Развёртывание</value> <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Deploy</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Deploy</value> <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Deploy</value>
</valuemap> </valuemap>
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">1</value> <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.Target.DeployConfiguration.0">
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0"> <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">0</value> <value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">0</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Развёртывание</value> <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Deploy</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Развёртывание</value> <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Deploy</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Deploy</value> <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Deploy</value>
</valuemap> </valuemap>
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">1</value> <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 * @return time in seconds
*/ */
double nanotime(){ double nanotime(){
static struct timespec *start = NULL; //static struct timespec *start = NULL;
struct timespec now; struct timespec now;
if(!start){ /*if(!start){
start = malloc(sizeof(struct timespec)); start = malloc(sizeof(struct timespec));
if(!start) return -1.; if(!start) return -1.;
if(clock_gettime(CLOCK_MONOTONIC, start)) return -1.; if(clock_gettime(CLOCK_MONOTONIC, start)) return -1.;
} }*/
if(clock_gettime(CLOCK_MONOTONIC, &now)) 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; 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"); 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; if(!c || !Xmodel || !Ymodel) return;
double tnow = nanotime(); double tnow = nanotime();
moveparam_t Xp, Yp; moveparam_t Xp, Yp;
@ -87,6 +88,8 @@ void getModData(coordval_pair_t *c){
c->X.t = c->Y.t = tnow; c->X.t = c->Y.t = tnow;
c->X.val = Xp.coord; c->X.val = Xp.coord;
c->Y.val = Yp.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 <stdlib.h>
#include "movingmodel.h"
#include "sidservo.h" #include "sidservo.h"
extern conf_t Conf; extern conf_t Conf;
double nanotime(); double nanotime();
void getModData(coordval_pair_t *c); void getModData(coordval_pair_t *c, movestate_t *xst, movestate_t *yst);
typedef struct{ typedef struct{
double *x, *t, *t2, *xt; // arrays of coord/time and multiply double *x, *t, *t2, *xt; // arrays of coord/time and multiply
double xsum, tsum, t2sum, xtsum; // sums of coord/time and their 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){ int model_move2(movemodel_t *model, moveparam_t *target, double t){
if(!target || !model) return FALSE; 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 // only positive velocity
if(target->speed < 0.) target->speed = -target->speed; 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 // don't mind about acceleration - user cannot set it now
return model->calculate(model, target, t); return model->calculate(model, target, t);
} }

6
LibSidServo/ramp.c
View File

@ -23,12 +23,14 @@
#include "main.h" #include "main.h"
#include "ramp.h" #include "ramp.h"
/*
#ifdef EBUG #ifdef EBUG
#undef DBG #undef DBG
#define DBG(...) #define DBG(...)
#undef FNAME
#define FNAME()
#endif #endif
*/
static double coord_tolerance = COORD_TOLERANCE_DEFAULT; static double coord_tolerance = COORD_TOLERANCE_DEFAULT;
static void emstop(movemodel_t *m, double _U_ t){ 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 // calculate current X/Y speeds
void getXspeed(){ 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; static less_square_t *ls = NULL;
if(!ls){ if(!ls){
ls = LS_init(Conf.EncoderSpeedInterval / Conf.EncoderReqInterval); ls = LS_init(Conf.EncoderSpeedInterval / Conf.EncoderReqInterval);
if(!ls) return; if(!ls) return;
} }
pthread_mutex_lock(&datamutex); 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){ if(fabs(speed) < 1.5 * MCC_MAX_X_SPEED){
mountdata.encXspeed.val = speed; mountdata.encXspeed.val = speed;
mountdata.encXspeed.t = mountdata.encXposition.t; mountdata.encXspeed.t = mountdata.encXposition.t;
@ -80,13 +82,15 @@ void getXspeed(){
#endif #endif
} }
void getYspeed(){ 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; static less_square_t *ls = NULL;
if(!ls){ if(!ls){
ls = LS_init(Conf.EncoderSpeedInterval / Conf.EncoderReqInterval); ls = LS_init(Conf.EncoderSpeedInterval / Conf.EncoderReqInterval);
if(!ls) return; if(!ls) return;
} }
pthread_mutex_lock(&datamutex); 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){ if(fabs(speed) < 1.5 * MCC_MAX_Y_SPEED){
mountdata.encYspeed.val = speed; mountdata.encYspeed.val = speed;
mountdata.encYspeed.t = mountdata.encYposition.t; mountdata.encYspeed.t = mountdata.encYposition.t;
@ -392,21 +396,31 @@ static void *mountthread(void _U_ *u){
uint8_t buf[2*sizeof(SSstat)]; uint8_t buf[2*sizeof(SSstat)];
SSstat *status = (SSstat*) buf; SSstat *status = (SSstat*) buf;
bzero(&mountdata, sizeof(mountdata)); bzero(&mountdata, sizeof(mountdata));
double t0 = nanotime(); double t0 = nanotime(), tstart = t0;
static double oldmt = -100.; // old `millis measurement` time static double oldmt = -100.; // old `millis measurement` time
static uint32_t oldmillis = 0; static uint32_t oldmillis = 0;
if(Conf.RunModel) while(1){ if(Conf.RunModel) while(1){
coordval_pair_t c; coordval_pair_t c;
movestate_t xst, yst;
// now change data // now change data
getModData(&c); getModData(&c, &xst, &yst);
pthread_mutex_lock(&datamutex); pthread_mutex_lock(&datamutex);
double tnow = c.X.t; double tnow = c.X.t;
mountdata.motXposition.t = mountdata.encXposition.t = mountdata.motYposition.t = mountdata.encYposition.t = tnow; mountdata.encXposition.t = mountdata.encYposition.t = tnow;
mountdata.motXposition.val = mountdata.encXposition.val = c.X.val; mountdata.encXposition.val = c.X.val;
mountdata.motYposition.val = mountdata.encYposition.val = c.Y.val; mountdata.encYposition.val = c.Y.val;
//DBG("t=%g, X=%g, Y=%g", tnow, c.X.val, c.Y.val); //DBG("t=%g, X=%g, Y=%g", tnow, c.X.val, c.Y.val);
if(tnow - oldmt > Conf.MountReqInterval){ 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; oldmt = tnow;
}else mountdata.millis = oldmillis; }else mountdata.millis = oldmillis;
pthread_mutex_unlock(&datamutex); pthread_mutex_unlock(&datamutex);