diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..0c4cda4
--- /dev/null
+++ b/.gitignore
@@ -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*
diff --git a/PID.c b/PID.c
index 3f642cd..0178c3e 100644
--- a/PID.c
+++ b/PID.c
@@ -16,6 +16,7 @@
* along with this program. If not, see .
*/
+#include
#include
#include
#include
@@ -25,34 +26,70 @@
#include "PID.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;
PIDController_t *pid = (PIDController_t*)calloc(1, sizeof(PIDController_t));
pid->gain = *gain;
DBG("Created PID with P=%g, I=%g, D=%g\n", gain->P, gain->I, gain->D);
pid->pidIarrSize = Iarrsz;
pid->pidIarray = (double*)calloc(Iarrsz, sizeof(double));
+ curtime(&pid->prevT);
return pid;
}
// don't clear lastT!
-void pid_clear(PIDController_t *pid){
+static void pid_clear(PIDController_t *pid){
if(!pid) return;
DBG("CLEAR PID PARAMETERS");
bzero(pid->pidIarray, sizeof(double) * pid->pidIarrSize);
pid->integral = 0.;
pid->prev_error = 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)->pidIarray) free((*pid)->pidIarray);
free(*pid);
*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
double oldi = pid->pidIarray[pid->curIidx], newi = error * dt;
//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;
double derivative = (error - pid->prev_error) / dt;
pid->prev_error = error;
- double sum = pid->gain.P * error + pid->gain.I * pid->integral + pid->gain.D * derivative;
- DBG("P=%g, I=%g, D=%g; sum=%g", pid->gain.P * error, pid->gain.I * pid->integral, pid->gain.D * derivative, sum);
+ DBG("pid pars: P=%g, I=%g, D=%f", pid->gain.P, pid->gain.I, pid->gain.D);
+ 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;
}
-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
* @param tagpos - given coordinate of target position
* @param endpoint - endpoint for this coordinate
* @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);
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);
return axis->speed.val; // data is too old or wrong
}
double error = tagpos->val - axis->position.val, fe = fabs(error);
- DBG("error: %g", error);
- PIDController_t *pid = NULL;
+ DBG("error: %g'', cur speed: %g (deg/s)", error * 180. * 3600. / M_PI, axis->speed.val*180./M_PI);
switch(axis->state){
case AXIS_SLEWING:
if(fe < Conf.MaxPointingErr){
axis->state = AXIS_POINTING;
DBG("--> Pointing");
- pid = pidpair->PIDC;
}else{
DBG("Slewing...");
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){
axis->state = AXIS_GUIDING;
DBG("--> Guiding");
- pid = pidpair->PIDV;
}else if(fe > Conf.MaxPointingErr){
DBG("--> Slewing");
axis->state = AXIS_SLEWING;
return NAN;
- } else pid = pidpair->PIDC;
+ }
break;
case AXIS_GUIDING:
- pid = pidpair->PIDV;
if(fe > Conf.MaxFinePointingErr){
DBG("--> Pointing");
axis->state = AXIS_POINTING;
- pid = pidpair->PIDC;
}else if(fe < Conf.MaxGuidingErr){
DBG("At target");
// 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;
case AXIS_STOPPED: // start pointing to target; will change speed next time
DBG("AXIS STOPPED!!!! --> Slewing");
axis->state = AXIS_SLEWING;
- return getspeed(tagpos, pidpair, axis);
+ return getspeed(tagpos, pid, axis);
case AXIS_ERROR:
DBG("Can't move from erroneous state");
return 0.;
@@ -137,17 +161,7 @@ static double getspeed(const coordval_t *tagpos, PIDpair_t *pidpair, axisdata_t
DBG("WTF? Where is a PID?");
return axis->speed.val;
}
- double dtpid = timediff(&tagpos->t, &pid->prevT);
- 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
+ return pid_calculate(pid, axis->position.val, tagpos);
}
/**
@@ -157,18 +171,14 @@ static double getspeed(const coordval_t *tagpos, PIDpair_t *pidpair, axisdata_t
* @return error code
*/
mcc_errcodes_t correct2(const coordval_pair_t *target){
- static PIDpair_t pidX = {0}, pidY = {0};
- if(!pidX.PIDC){
- pidX.PIDC = pid_create(&Conf.XPIDC, Conf.PIDCycleDt / Conf.PIDRefreshDt);
- if(!pidX.PIDC) return MCC_E_FATAL;
- pidX.PIDV = pid_create(&Conf.XPIDV, Conf.PIDCycleDt / Conf.PIDRefreshDt);
- if(!pidX.PIDV) return MCC_E_FATAL;
+ static PIDController_t *pidX = NULL, *pidY = NULL;
+ if(!pidX){
+ pidX = pid_create(&Conf.XPIDV, Conf.PIDCycleDt / Conf.PIDRefreshDt);
+ if(!pidX) return MCC_E_FATAL;
}
- if(!pidY.PIDC){
- pidY.PIDC = pid_create(&Conf.YPIDC, Conf.PIDCycleDt / Conf.PIDRefreshDt);
- if(!pidY.PIDC) return MCC_E_FATAL;
- pidY.PIDV = pid_create(&Conf.YPIDV, Conf.PIDCycleDt / Conf.PIDRefreshDt);
- if(!pidY.PIDV) return MCC_E_FATAL;
+ if(!pidY){
+ pidY = pid_create(&Conf.YPIDV, Conf.PIDCycleDt / Conf.PIDRefreshDt);
+ if(!pidY) return MCC_E_FATAL;
}
mountdata_t m;
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.position = m.encXposition;
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(target->X.val < axis.position.val) Xsign = -1.;
tagspeed.X = Xlimits.max.speed;
@@ -191,7 +201,7 @@ mcc_errcodes_t correct2(const coordval_pair_t *target){
axis.state = m.Ystate;
axis.position = m.encYposition;
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(target->Y.val < axis.position.val) Ysign = -1.;
tagspeed.Y = Ylimits.max.speed;
diff --git a/PID.h b/PID.h
index 5ecfad0..79539f7 100644
--- a/PID.h
+++ b/PID.h
@@ -22,19 +22,10 @@
#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);
void pid_clear(PIDController_t *pid);
void pid_delete(PIDController_t **pid);
double pid_calculate(PIDController_t *pid, double error, double dt);
-
+*/
mcc_errcodes_t correct2(const coordval_pair_t *target);
diff --git a/examples/scmd_traectory.c b/examples/scmd_traectory.c
index 91af985..71981d2 100644
--- a/examples/scmd_traectory.c
+++ b/examples/scmd_traectory.c
@@ -109,17 +109,27 @@ static void runtraectory(traectory_fn tfn){
}
if(!Mount.currentT(&tcur)) continue;
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;
double t = Mount.timeFromStart();
- 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(fabs(telXY.X.val) > G.Xmax || fabs(telXY.Y.val) > G.Ymax || t - tstart > G.tmax){
+ 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.t = target.Y.t = tcur;
if(t0.tv_nsec == 0 && t0.tv_sec == 0) dumpt0(&t0);
else{
//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));
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));
@@ -166,15 +176,16 @@ int main(int argc, char **argv){
return 1;
}
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);
if(e != MCC_E_OK){
WARNX("Can't init devices");
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(SIGHUP, SIG_IGN); // hup - ignore
signal(SIGINT, signals); // ctrl+C - quit
diff --git a/examples/traectories.c b/examples/traectories.c
index d8464af..3f26a5a 100644
--- a/examples/traectories.c
+++ b/examples/traectories.c
@@ -42,6 +42,7 @@ int init_traectory(traectory_fn f, coordpair_t *XY0){
cur_traectory = f;
XYstart = *XY0;
tstart = Mount.timeFromStart();
+ DBG("inited @ %gs from start", tstart);
mountdata_t mdata;
int ntries = 0;
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)
*/
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;
- if(!cur_traectory(&pt, t)) return FALSE;
+ if(!cur_traectory(&pt, t)){
+ DBG("error in cur_traectory");
+ return FALSE;
+ }
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;
}
@@ -88,6 +99,7 @@ int telpos(coordval_pair_t *curpos){
// X=X0+1'/s, Y=Y0+15''/s
int Linear(coordpair_t *nextpt, double t){
coordpair_t pt;
+ DBG("t=%g, tfromstart=%g, dt=%g", t, tstart, t-tstart);
pt.X = XYstart.X + ASEC2RAD(0.1) * (t - tstart);
pt.Y = XYstart.Y + ASEC2RAD(15.)* (t - tstart);
if(nextpt) *nextpt = pt;
@@ -98,7 +110,7 @@ int Linear(coordpair_t *nextpt, double t){
int SinCos(coordpair_t *nextpt, double t){
coordpair_t pt;
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;
return TRUE;
}
@@ -111,7 +123,7 @@ typedef struct{
static tr_names names[] = {
{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}
};
diff --git a/serial.c b/serial.c
index 25b0ab9..67e06b7 100644
--- a/serial.c
+++ b/serial.c
@@ -382,7 +382,10 @@ static int readstrings(buf_t *buf, int fd){
// return TRUE if got, FALSE if no data found
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)
char *last = &buf->buf[buf->len - 1];
//DBG("buf: _%s_", buf->buf);
@@ -439,6 +442,7 @@ static void *encoderthread2(void _U_ *u){
for(int i = 0; i < 2; ++i){
if(pfds[i].revents && POLLIN){
if(!readstrings(&strbuf[i], encfd[i])){
+ DBG("ERR");
++errctr;
break;
}
@@ -446,7 +450,9 @@ static void *encoderthread2(void _U_ *u){
double curt = timefromstart();
if(getdata(&strbuf[i], &msrlast[i])) mtlast[i] = curt;
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){
+ DBG("time OK");
pthread_mutex_lock(&datamutex);
double pos = (double)msrlast[i];
//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_ispeed = speed;
tty.c_ospeed = speed;
- //tty.c_cc[VMIN] = 0; // non-canonical mode
- //tty.c_cc[VTIME] = 5;
+ tty.c_cc[VMIN] = 0; // non-canonical mode
+ tty.c_cc[VTIME] = 5;
if(ioctl(fd, TCSETS2, &tty)){
DBG("Can't set TTY settings");
close(fd);
diff --git a/sidservo.h b/sidservo.h
index ed4feab..e544ac1 100644
--- a/sidservo.h
+++ b/sidservo.h
@@ -136,10 +136,10 @@ typedef struct{
} extradata_t;
typedef enum{
- AXIS_STOPPED,
- AXIS_SLEWING,
- AXIS_POINTING,
- AXIS_GUIDING,
+ AXIS_STOPPED, // stop
+ AXIS_SLEWING, // go to target with maximal speed
+ AXIS_POINTING, // axis is in pointing zone, use PID
+ AXIS_GUIDING, // near target
AXIS_ERROR,
} axis_status_t;