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

fixed broken PID

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

90
.gitignore vendored Normal file
View File

@@ -0,0 +1,90 @@
# ---> C
# Prerequisites
*.d
# Object files
*.o
*.ko
*.obj
*.elf
# Linker output
*.ilk
*.map
*.exp
# Precompiled Headers
*.gch
*.pch
# Libraries
*.lib
*.a
*.la
*.lo
# Shared objects (inc. Windows DLLs)
*.dll
*.so
*.so.*
*.dylib
# Executables
*.exe
*.out
*.app
*.i*86
*.x86_64
*.hex
# Debug files
*.dSYM/
*.su
*.idb
*.pdb
# Kernel Module Compile Results
*.mod*
*.cmd
.tmp_versions/
modules.order
Module.symvers
Mkfile.old
dkms.conf
# ---> C++
# Prerequisites
*.d
# Compiled Object files
*.slo
*.lo
*.o
*.obj
# Precompiled Headers
*.gch
*.pch
# Compiled Dynamic libraries
*.so
*.dylib
*.dll
# Fortran module files
*.mod
*.smod
# Compiled Static libraries
*.lai
*.la
*.a
*.lib
# Executables
*.exe
*.out
*.app
build
CMakeLists.txt.user*

116
PID.c
View File

@@ -16,6 +16,7 @@
* along with this program. If not, see <http://www.gnu.org/licenses/>. * along with this program. If not, see <http://www.gnu.org/licenses/>.
*/ */
#include <float.h>
#include <math.h> #include <math.h>
#include <stdio.h> #include <stdio.h>
#include <stdlib.h> #include <stdlib.h>
@@ -25,34 +26,70 @@
#include "PID.h" #include "PID.h"
#include "serial.h" #include "serial.h"
PIDController_t *pid_create(const PIDpar_t *gain, size_t Iarrsz){ typedef struct {
PIDpar_t gain; // PID gains
double prev_error; // Previous error
double prev_tagpos; // previous target position
double integral; // Integral term
double *pidIarray; // array for Integral
struct timespec prevT; // time of previous correction
size_t pidIarrSize; // it's size
size_t curIidx; // and index of current element
} PIDController_t;
typedef struct{
axis_status_t state;
coordval_t position;
coordval_t speed;
} axisdata_t;
static PIDController_t *pid_create(const PIDpar_t *gain, size_t Iarrsz){
if(!gain || Iarrsz < 3) return NULL; if(!gain || Iarrsz < 3) return NULL;
PIDController_t *pid = (PIDController_t*)calloc(1, sizeof(PIDController_t)); PIDController_t *pid = (PIDController_t*)calloc(1, sizeof(PIDController_t));
pid->gain = *gain; pid->gain = *gain;
DBG("Created PID with P=%g, I=%g, D=%g\n", gain->P, gain->I, gain->D); DBG("Created PID with P=%g, I=%g, D=%g\n", gain->P, gain->I, gain->D);
pid->pidIarrSize = Iarrsz; pid->pidIarrSize = Iarrsz;
pid->pidIarray = (double*)calloc(Iarrsz, sizeof(double)); pid->pidIarray = (double*)calloc(Iarrsz, sizeof(double));
curtime(&pid->prevT);
return pid; return pid;
} }
// don't clear lastT! // don't clear lastT!
void pid_clear(PIDController_t *pid){ static void pid_clear(PIDController_t *pid){
if(!pid) return; if(!pid) return;
DBG("CLEAR PID PARAMETERS"); DBG("CLEAR PID PARAMETERS");
bzero(pid->pidIarray, sizeof(double) * pid->pidIarrSize); bzero(pid->pidIarray, sizeof(double) * pid->pidIarrSize);
pid->integral = 0.; pid->integral = 0.;
pid->prev_error = 0.; pid->prev_error = 0.;
pid->curIidx = 0; pid->curIidx = 0;
curtime(&pid->prevT);
} }
/*
void pid_delete(PIDController_t **pid){ static void pid_delete(PIDController_t **pid){
if(!pid || !*pid) return; if(!pid || !*pid) return;
if((*pid)->pidIarray) free((*pid)->pidIarray); if((*pid)->pidIarray) free((*pid)->pidIarray);
free(*pid); free(*pid);
*pid = NULL; *pid = NULL;
} }*/
double pid_calculate(PIDController_t *pid, double error, double dt){ // calculate new motor speed
static double pid_calculate(PIDController_t *pid, double axispos, const coordval_t *target){
double dtpid = timediff(&target->t, &pid->prevT);
if(dtpid < 0 || dtpid > Conf.PIDMaxDt){
DBG("time diff too big: clear PID");
pid_clear(pid);
pid->prev_tagpos = target->val;
return 0.;
}
double dt = timediff(&target->t, &pid->prevT);
if(dt < FLT_EPSILON){
DBG("Target time in past");
return 0.;
}
pid->prevT = target->t;
double error = target->val - axispos;
double tagspeed = (target->val - pid->prev_tagpos) / dt;
pid->prev_tagpos = target->val;
// calculate flowing integral // calculate flowing integral
double oldi = pid->pidIarray[pid->curIidx], newi = error * dt; double oldi = pid->pidIarray[pid->curIidx], newi = error * dt;
//DBG("oldi/new: %g, %g", oldi, newi); //DBG("oldi/new: %g, %g", oldi, newi);
@@ -61,43 +98,33 @@ double pid_calculate(PIDController_t *pid, double error, double dt){
pid->integral += newi - oldi; pid->integral += newi - oldi;
double derivative = (error - pid->prev_error) / dt; double derivative = (error - pid->prev_error) / dt;
pid->prev_error = error; pid->prev_error = error;
double sum = pid->gain.P * error + pid->gain.I * pid->integral + pid->gain.D * derivative; DBG("pid pars: P=%g, I=%g, D=%f", pid->gain.P, pid->gain.I, pid->gain.D);
DBG("P=%g, I=%g, D=%g; sum=%g", pid->gain.P * error, pid->gain.I * pid->integral, pid->gain.D * derivative, sum); double sum = pid->gain.P * error + pid->gain.I * pid->integral + pid->gain.D * derivative + tagspeed;
DBG("tagspeed=%g, P=%g, I=%g, D=%g; sum=%g", tagspeed, pid->gain.P * error,
pid->gain.I * pid->integral, pid->gain.D * derivative, sum);
return sum; return sum;
} }
typedef struct{
PIDController_t *PIDC;
PIDController_t *PIDV;
} PIDpair_t;
typedef struct{
axis_status_t state;
coordval_t position;
coordval_t speed;
} axisdata_t;
/** /**
* @brief process - Process PID for given axis * @brief process - Process PID for given axis
* @param tagpos - given coordinate of target position * @param tagpos - given coordinate of target position
* @param endpoint - endpoint for this coordinate * @param endpoint - endpoint for this coordinate
* @param pid - pid itself * @param pid - pid itself
* @return calculated new speed or -1 for max speed * @return calculated NEW SPEED or NAN for max speed
*/ */
static double getspeed(const coordval_t *tagpos, PIDpair_t *pidpair, axisdata_t *axis){ static double getspeed(const coordval_t *tagpos, PIDController_t *pid, axisdata_t *axis){
double dt = timediff(&tagpos->t, &axis->position.t); double dt = timediff(&tagpos->t, &axis->position.t);
if(dt < 0 || dt > Conf.PIDMaxDt){ if(dt < 0 || dt > Conf.PIDMaxDt){
DBG("target time: %ld, axis time: %ld - too big! (tag-ax=%g)", tagpos->t.tv_sec, axis->position.t.tv_sec, dt); DBG("target time: %ld, axis time: %ld - too big! (tag-ax=%g)", tagpos->t.tv_sec, axis->position.t.tv_sec, dt);
return axis->speed.val; // data is too old or wrong return axis->speed.val; // data is too old or wrong
} }
double error = tagpos->val - axis->position.val, fe = fabs(error); double error = tagpos->val - axis->position.val, fe = fabs(error);
DBG("error: %g", error); DBG("error: %g'', cur speed: %g (deg/s)", error * 180. * 3600. / M_PI, axis->speed.val*180./M_PI);
PIDController_t *pid = NULL;
switch(axis->state){ switch(axis->state){
case AXIS_SLEWING: case AXIS_SLEWING:
if(fe < Conf.MaxPointingErr){ if(fe < Conf.MaxPointingErr){
axis->state = AXIS_POINTING; axis->state = AXIS_POINTING;
DBG("--> Pointing"); DBG("--> Pointing");
pid = pidpair->PIDC;
}else{ }else{
DBG("Slewing..."); DBG("Slewing...");
return NAN; // max speed for given axis return NAN; // max speed for given axis
@@ -107,28 +134,25 @@ static double getspeed(const coordval_t *tagpos, PIDpair_t *pidpair, axisdata_t
if(fe < Conf.MaxFinePointingErr){ if(fe < Conf.MaxFinePointingErr){
axis->state = AXIS_GUIDING; axis->state = AXIS_GUIDING;
DBG("--> Guiding"); DBG("--> Guiding");
pid = pidpair->PIDV;
}else if(fe > Conf.MaxPointingErr){ }else if(fe > Conf.MaxPointingErr){
DBG("--> Slewing"); DBG("--> Slewing");
axis->state = AXIS_SLEWING; axis->state = AXIS_SLEWING;
return NAN; return NAN;
} else pid = pidpair->PIDC; }
break; break;
case AXIS_GUIDING: case AXIS_GUIDING:
pid = pidpair->PIDV;
if(fe > Conf.MaxFinePointingErr){ if(fe > Conf.MaxFinePointingErr){
DBG("--> Pointing"); DBG("--> Pointing");
axis->state = AXIS_POINTING; axis->state = AXIS_POINTING;
pid = pidpair->PIDC;
}else if(fe < Conf.MaxGuidingErr){ }else if(fe < Conf.MaxGuidingErr){
DBG("At target"); DBG("At target");
// TODO: we can point somehow that we are at target or introduce new axis state // TODO: we can point somehow that we are at target or introduce new axis state
}else DBG("Current error: %g", fe); }else DBG("Current abs error: %g", fe);
break; break;
case AXIS_STOPPED: // start pointing to target; will change speed next time case AXIS_STOPPED: // start pointing to target; will change speed next time
DBG("AXIS STOPPED!!!! --> Slewing"); DBG("AXIS STOPPED!!!! --> Slewing");
axis->state = AXIS_SLEWING; axis->state = AXIS_SLEWING;
return getspeed(tagpos, pidpair, axis); return getspeed(tagpos, pid, axis);
case AXIS_ERROR: case AXIS_ERROR:
DBG("Can't move from erroneous state"); DBG("Can't move from erroneous state");
return 0.; return 0.;
@@ -137,17 +161,7 @@ static double getspeed(const coordval_t *tagpos, PIDpair_t *pidpair, axisdata_t
DBG("WTF? Where is a PID?"); DBG("WTF? Where is a PID?");
return axis->speed.val; return axis->speed.val;
} }
double dtpid = timediff(&tagpos->t, &pid->prevT); return pid_calculate(pid, axis->position.val, tagpos);
if(dtpid < 0 || dtpid > Conf.PIDMaxDt){
DBG("time diff too big: clear PID");
pid_clear(pid);
}
if(dtpid > Conf.PIDMaxDt) dtpid = Conf.PIDCycleDt;
pid->prevT = tagpos->t;
DBG("CALC PID (er=%g, dt=%g), state=%d", error, dtpid, axis->state);
double tagspeed = pid_calculate(pid, error, dtpid);
if(axis->state == AXIS_GUIDING) return axis->speed.val + tagspeed / dtpid; // velocity-based
return tagspeed; // coordinate-based
} }
/** /**
@@ -157,18 +171,14 @@ static double getspeed(const coordval_t *tagpos, PIDpair_t *pidpair, axisdata_t
* @return error code * @return error code
*/ */
mcc_errcodes_t correct2(const coordval_pair_t *target){ mcc_errcodes_t correct2(const coordval_pair_t *target){
static PIDpair_t pidX = {0}, pidY = {0}; static PIDController_t *pidX = NULL, *pidY = NULL;
if(!pidX.PIDC){ if(!pidX){
pidX.PIDC = pid_create(&Conf.XPIDC, Conf.PIDCycleDt / Conf.PIDRefreshDt); pidX = pid_create(&Conf.XPIDV, Conf.PIDCycleDt / Conf.PIDRefreshDt);
if(!pidX.PIDC) return MCC_E_FATAL; if(!pidX) return MCC_E_FATAL;
pidX.PIDV = pid_create(&Conf.XPIDV, Conf.PIDCycleDt / Conf.PIDRefreshDt);
if(!pidX.PIDV) return MCC_E_FATAL;
} }
if(!pidY.PIDC){ if(!pidY){
pidY.PIDC = pid_create(&Conf.YPIDC, Conf.PIDCycleDt / Conf.PIDRefreshDt); pidY = pid_create(&Conf.YPIDV, Conf.PIDCycleDt / Conf.PIDRefreshDt);
if(!pidY.PIDC) return MCC_E_FATAL; if(!pidY) return MCC_E_FATAL;
pidY.PIDV = pid_create(&Conf.YPIDV, Conf.PIDCycleDt / Conf.PIDRefreshDt);
if(!pidY.PIDV) return MCC_E_FATAL;
} }
mountdata_t m; mountdata_t m;
coordpair_t tagspeed; // absolute value of speed coordpair_t tagspeed; // absolute value of speed
@@ -179,7 +189,7 @@ mcc_errcodes_t correct2(const coordval_pair_t *target){
axis.state = m.Xstate; axis.state = m.Xstate;
axis.position = m.encXposition; axis.position = m.encXposition;
axis.speed = m.encXspeed; axis.speed = m.encXspeed;
tagspeed.X = getspeed(&target->X, &pidX, &axis); tagspeed.X = getspeed(&target->X, pidX, &axis);
if(isnan(tagspeed.X)){ // max speed if(isnan(tagspeed.X)){ // max speed
if(target->X.val < axis.position.val) Xsign = -1.; if(target->X.val < axis.position.val) Xsign = -1.;
tagspeed.X = Xlimits.max.speed; tagspeed.X = Xlimits.max.speed;
@@ -191,7 +201,7 @@ mcc_errcodes_t correct2(const coordval_pair_t *target){
axis.state = m.Ystate; axis.state = m.Ystate;
axis.position = m.encYposition; axis.position = m.encYposition;
axis.speed = m.encYspeed; axis.speed = m.encYspeed;
tagspeed.Y = getspeed(&target->Y, &pidY, &axis); tagspeed.Y = getspeed(&target->Y, pidY, &axis);
if(isnan(tagspeed.Y)){ // max speed if(isnan(tagspeed.Y)){ // max speed
if(target->Y.val < axis.position.val) Ysign = -1.; if(target->Y.val < axis.position.val) Ysign = -1.;
tagspeed.Y = Ylimits.max.speed; tagspeed.Y = Ylimits.max.speed;

13
PID.h
View File

@@ -22,19 +22,10 @@
#include "sidservo.h" #include "sidservo.h"
typedef struct { /*
PIDpar_t gain; // PID gains
double prev_error; // Previous error
double integral; // Integral term
double *pidIarray; // array for Integral
struct timespec prevT; // time of previous correction
size_t pidIarrSize; // it's size
size_t curIidx; // and index of current element
} PIDController_t;
PIDController_t *pid_create(const PIDpar_t *gain, size_t Iarrsz); PIDController_t *pid_create(const PIDpar_t *gain, size_t Iarrsz);
void pid_clear(PIDController_t *pid); void pid_clear(PIDController_t *pid);
void pid_delete(PIDController_t **pid); void pid_delete(PIDController_t **pid);
double pid_calculate(PIDController_t *pid, double error, double dt); double pid_calculate(PIDController_t *pid, double error, double dt);
*/
mcc_errcodes_t correct2(const coordval_pair_t *target); mcc_errcodes_t correct2(const coordval_pair_t *target);

27
examples/scmd_traectory.c
View File

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

22
examples/traectories.c
View File

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

12
serial.c
View File

@@ -382,7 +382,10 @@ static int readstrings(buf_t *buf, int fd){
// return TRUE if got, FALSE if no data found // return TRUE if got, FALSE if no data found
static int getdata(buf_t *buf, long *out){ static int getdata(buf_t *buf, long *out){
if(!buf || buf->len < 1 || buf->len > (XYBUFSZ+1)) return FALSE; if(!buf || buf->len < 1 || buf->len > (XYBUFSZ+1)){
return FALSE;
}
DBG("got data");
// read record between last '\n' and previous (or start of string) // read record between last '\n' and previous (or start of string)
char *last = &buf->buf[buf->len - 1]; char *last = &buf->buf[buf->len - 1];
//DBG("buf: _%s_", buf->buf); //DBG("buf: _%s_", buf->buf);
@@ -439,6 +442,7 @@ static void *encoderthread2(void _U_ *u){
for(int i = 0; i < 2; ++i){ for(int i = 0; i < 2; ++i){
if(pfds[i].revents && POLLIN){ if(pfds[i].revents && POLLIN){
if(!readstrings(&strbuf[i], encfd[i])){ if(!readstrings(&strbuf[i], encfd[i])){
DBG("ERR");
++errctr; ++errctr;
break; break;
} }
@@ -446,7 +450,9 @@ static void *encoderthread2(void _U_ *u){
double curt = timefromstart(); double curt = timefromstart();
if(getdata(&strbuf[i], &msrlast[i])) mtlast[i] = curt; if(getdata(&strbuf[i], &msrlast[i])) mtlast[i] = curt;
if(curt - t0[i] >= Conf.EncoderReqInterval){ // get last records if(curt - t0[i] >= Conf.EncoderReqInterval){ // get last records
DBG("last rec %d, curt=%g, t0=%g, mtlast=%g", i, curt, t0[i], mtlast[i]);
if(curt - mtlast[i] < 1.5*Conf.EncoderReqInterval){ if(curt - mtlast[i] < 1.5*Conf.EncoderReqInterval){
DBG("time OK");
pthread_mutex_lock(&datamutex); pthread_mutex_lock(&datamutex);
double pos = (double)msrlast[i]; double pos = (double)msrlast[i];
//DBG("pos[%d]=%g", i, pos); //DBG("pos[%d]=%g", i, pos);
@@ -626,8 +632,8 @@ static int ttyopen(const char *path, speed_t speed){
tty.c_cflag = BOTHER | CS8 | CREAD | CLOCAL; // other speed, 8bit, RW, ignore line ctrl tty.c_cflag = BOTHER | CS8 | CREAD | CLOCAL; // other speed, 8bit, RW, ignore line ctrl
tty.c_ispeed = speed; tty.c_ispeed = speed;
tty.c_ospeed = speed; tty.c_ospeed = speed;
//tty.c_cc[VMIN] = 0; // non-canonical mode tty.c_cc[VMIN] = 0; // non-canonical mode
//tty.c_cc[VTIME] = 5; tty.c_cc[VTIME] = 5;
if(ioctl(fd, TCSETS2, &tty)){ if(ioctl(fd, TCSETS2, &tty)){
DBG("Can't set TTY settings"); DBG("Can't set TTY settings");
close(fd); close(fd);

8
sidservo.h
View File

@@ -136,10 +136,10 @@ typedef struct{
} extradata_t; } extradata_t;
typedef enum{ typedef enum{
AXIS_STOPPED, AXIS_STOPPED, // stop
AXIS_SLEWING, AXIS_SLEWING, // go to target with maximal speed
AXIS_POINTING, AXIS_POINTING, // axis is in pointing zone, use PID
AXIS_GUIDING, AXIS_GUIDING, // near target
AXIS_ERROR, AXIS_ERROR,
} axis_status_t; } axis_status_t;