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

PID works

Browse Source
This commit is contained in:
Edward V. Emelianov
2026-03-23 17:21:33 +03:00
parent 9e7c732afd
commit 071e7fc189
6 changed files with 85 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
PID.c
View File

@@ -122,7 +122,7 @@ static double getspeed(const coordval_t *tagpos, PIDController_t *pid, axisdata_
DBG("error: %g'', cur speed: %g (deg/s)", error * 180. * 3600. / M_PI, axis->speed.val*180./M_PI); DBG("error: %g'', cur speed: %g (deg/s)", error * 180. * 3600. / M_PI, axis->speed.val*180./M_PI);
switch(axis->state){ switch(axis->state){
case AXIS_SLEWING: case AXIS_SLEWING:
if(fe < Conf.MaxPointingErr){ if(fe < Conf.MaxFinePointingErr){
axis->state = AXIS_POINTING; axis->state = AXIS_POINTING;
DBG("--> Pointing"); DBG("--> Pointing");
}else{ }else{
@@ -216,6 +216,7 @@ mcc_errcodes_t correct2(const coordval_pair_t *target){
setStat(xstate, ystate); setStat(xstate, ystate);
} }
coordpair_t endpoint; coordpair_t endpoint;
#if 0
// allow at least PIDMaxDt moving with target speed // allow at least PIDMaxDt moving with target speed
double dv = fabs(tagspeed.X - m.encXspeed.val); double dv = fabs(tagspeed.X - m.encXspeed.val);
double adder = dv/Xlimits.max.accel * (m.encXspeed.val + dv / 2.) // distanse with changing speed double adder = dv/Xlimits.max.accel * (m.encXspeed.val + dv / 2.) // distanse with changing speed
@@ -227,6 +228,16 @@ mcc_errcodes_t correct2(const coordval_pair_t *target){
+ Conf.PIDMaxDt * tagspeed.Y + Conf.PIDMaxDt * tagspeed.Y
+ tagspeed.Y * tagspeed.Y / Ylimits.max.accel / 2.; + tagspeed.Y * tagspeed.Y / Ylimits.max.accel / 2.;
endpoint.Y = m.encYposition.val + Ysign * adder; endpoint.Y = m.encYposition.val + Ysign * adder;
#endif
// allow 10s moving but not more than 10deg and not less than 1deg
double adder = fabs(tagspeed.X) * 10.;
if(adder > 0.17453) adder = 0.17453;
else if(adder < 0.017453) adder = 0.017453;
endpoint.X = m.encXposition.val + Xsign * adder;
adder = fabs(tagspeed.Y) * 10.;
if(adder > 0.17453) adder = 0.17453;
else if(adder < 0.017453) adder = 0.017453;
endpoint.Y = m.encYposition.val + Ysign * adder;
DBG("TAG speeds: %g/%g (deg/s); TAG pos: %g/%g (deg)", tagspeed.X/M_PI*180., tagspeed.Y/M_PI*180., endpoint.X/M_PI*180., endpoint.Y/M_PI*180.); DBG("TAG speeds: %g/%g (deg/s); TAG pos: %g/%g (deg)", tagspeed.X/M_PI*180., tagspeed.Y/M_PI*180., endpoint.X/M_PI*180., endpoint.Y/M_PI*180.);
return Mount.moveWspeed(&endpoint, &tagspeed); return Mount.moveWspeed(&endpoint, &tagspeed);
} }

9
main.c
View File

@@ -311,10 +311,6 @@ static mcc_errcodes_t move2(const coordpair_t *target){
cmd.Ymot = target->Y; cmd.Ymot = target->Y;
cmd.Xspeed = Xlimits.max.speed; cmd.Xspeed = Xlimits.max.speed;
cmd.Yspeed = Ylimits.max.speed; cmd.Yspeed = Ylimits.max.speed;
/*mcc_errcodes_t r = shortcmd(&cmd);
if(r != MCC_E_OK) return r;
setslewingstate();
return MCC_E_OK;*/
return shortcmd(&cmd); return shortcmd(&cmd);
} }
@@ -351,10 +347,7 @@ static mcc_errcodes_t move2s(const coordpair_t *target, const coordpair_t *speed
cmd.Ymot = target->Y; cmd.Ymot = target->Y;
cmd.Xspeed = speed->X; cmd.Xspeed = speed->X;
cmd.Yspeed = speed->Y; cmd.Yspeed = speed->Y;
mcc_errcodes_t r = shortcmd(&cmd); return shortcmd(&cmd);
if(r != MCC_E_OK) return r;
setslewingstate();
return MCC_E_OK;
} }
/** /**

74
serial.c
View File

@@ -55,6 +55,8 @@ static struct timeval encRtmout = {.tv_sec = 0, .tv_usec = 100}, // encoder read
mnt1Rtmout = {.tv_sec = 0, .tv_usec = 200000}, // first reading mnt1Rtmout = {.tv_sec = 0, .tv_usec = 200000}, // first reading
mntRtmout = {.tv_sec = 0, .tv_usec = 50000}; // next readings mntRtmout = {.tv_sec = 0, .tv_usec = 50000}; // next readings
static volatile int GlobExit = 0;
// encoders raw data // encoders raw data
typedef struct __attribute__((packed)){ typedef struct __attribute__((packed)){
uint8_t magick; uint8_t magick;
@@ -178,16 +180,16 @@ static int readmntdata(uint8_t *buffer, int maxlen){
return -1; return -1;
} }
if(!buffer || maxlen < 1) return 0; if(!buffer || maxlen < 1) return 0;
DBG("ask for %d bytes", maxlen); //DBG("ask for %d bytes", maxlen);
int got = 0; int got = 0;
fd_set rfds; fd_set rfds;
struct timeval tv = mnt1Rtmout; struct timeval tv = mnt1Rtmout;
do{ do{
FD_ZERO(&rfds); FD_ZERO(&rfds);
FD_SET(mntfd, &rfds); FD_SET(mntfd, &rfds);
DBG("select"); //DBG("select");
int retval = select(mntfd + 1, &rfds, NULL, NULL, &tv); int retval = select(mntfd + 1, &rfds, NULL, NULL, &tv);
DBG("returned %d", retval); //DBG("returned %d", retval);
if(retval < 0){ if(retval < 0){
if(errno == EINTR) continue; if(errno == EINTR) continue;
DBG("Error in select()"); DBG("Error in select()");
@@ -244,7 +246,7 @@ static void *encoderthread1(void _U_ *u){
uint8_t databuf[ENC_DATALEN]; uint8_t databuf[ENC_DATALEN];
int wridx = 0, errctr = 0; int wridx = 0, errctr = 0;
struct timespec tcur; struct timespec tcur;
while(encfd[0] > -1 && errctr < MAX_ERR_CTR){ while(encfd[0] > -1 && errctr < MAX_ERR_CTR && !GlobExit){
int b = getencbyte(); int b = getencbyte();
if(b == -2) ++errctr; if(b == -2) ++errctr;
if(b < 0) continue; if(b < 0) continue;
@@ -402,27 +404,27 @@ static void *encoderthread2(void _U_ *u){
// Kalman filtering // Kalman filtering
kalman3_predict(&kf[i]); kalman3_predict(&kf[i]);
kalman3_update(&kf[i], pos); kalman3_update(&kf[i], pos);
DBG("Got pos=%g, kalman: angle=%g, vel=%g, acc=%g", //DBG("Got pos=%g, kalman: angle=%g, vel=%g, acc=%g",
pos, kf[i].x[0], kf[i].x[1], kf[i].x[2]); // pos, kf[i].x[0], kf[i].x[1], kf[i].x[2]);
mountdata.encXposition.val = kf[i].x[0]; mountdata.encXposition.val = kf[i].x[0];
curtime(&mountdata.encXposition.t); curtime(&mountdata.encXposition.t);
/*DBG("msrlast=%ld, Xpos.val=%g, t=%zd; XEzero=%d, SPR=%g", /*DBG("msrlast=%ld, Xpos.val=%g, t=%zd; XEzero=%d, SPR=%g",
msrlast[i], mountdata.encXposition.val, mountdata.encXposition.t.tv_sec, msrlast[i], mountdata.encXposition.val, mountdata.encXposition.t.tv_sec,
X_ENC_ZERO, X_ENC_STEPSPERREV);*/ X_ENC_ZERO, X_ENC_STEPSPERREV);*/
//getXspeed(); getXspeed();
mountdata.encXspeed.val = kf[i].x[1]; //mountdata.encXspeed.val = kf[i].x[1];
mountdata.encXspeed.t = mountdata.encXposition.t; //mountdata.encXspeed.t = mountdata.encXposition.t;
}else{ }else{
pos = Yenc2rad(pos); pos = Yenc2rad(pos);
kalman3_predict(&kf[i]); kalman3_predict(&kf[i]);
kalman3_update(&kf[i], pos); kalman3_update(&kf[i], pos);
DBG("Got pos=%g, kalman: angle=%g, vel=%g, acc=%g", //DBG("Got pos=%g, kalman: angle=%g, vel=%g, acc=%g",
pos, kf[i].x[0], kf[i].x[1], kf[i].x[2]); // pos, kf[i].x[0], kf[i].x[1], kf[i].x[2]);
mountdata.encYposition.val = kf[i].x[0]; mountdata.encYposition.val = kf[i].x[0];
curtime(&mountdata.encYposition.t); curtime(&mountdata.encYposition.t);
//getYspeed(); getYspeed();
mountdata.encYspeed.val = kf[i].x[1]; //mountdata.encYspeed.val = kf[i].x[1];
mountdata.encYspeed.t = mountdata.encYposition.t; //mountdata.encYspeed.t = mountdata.encYposition.t;
} }
pthread_mutex_unlock(&datamutex); pthread_mutex_unlock(&datamutex);
} }
@@ -435,7 +437,7 @@ static void *encoderthread2(void _U_ *u){
} }
} }
if(got == 2) errctr = 0; if(got == 2) errctr = 0;
}while(encfd[0] > -1 && encfd[1] > -1 && errctr < MAX_ERR_CTR); }while(encfd[0] > -1 && encfd[1] > -1 && errctr < MAX_ERR_CTR && !GlobExit);
DBG("\n\nEXIT: ERRCTR=%d", errctr); DBG("\n\nEXIT: ERRCTR=%d", errctr);
for(int i = 0; i < 2; ++i){ for(int i = 0; i < 2; ++i){
if(encfd[i] > -1){ if(encfd[i] > -1){
@@ -491,7 +493,7 @@ static void *mountthread(void _U_ *u){
if(Conf.RunModel){ if(Conf.RunModel){
double Xprev = NAN, Yprev = NAN; // previous coordinates double Xprev = NAN, Yprev = NAN; // previous coordinates
int xcnt = 0, ycnt = 0; int xcnt = 0, ycnt = 0;
while(1){ while(!GlobExit){
coordpair_t c; coordpair_t c;
movestate_t xst, yst; movestate_t xst, yst;
// now change data // now change data
@@ -529,7 +531,7 @@ static void *mountthread(void _U_ *u){
// cmd to send // cmd to send
data_t *cmd_getstat = cmd2dat(CMD_GETSTAT); data_t *cmd_getstat = cmd2dat(CMD_GETSTAT);
if(!cmd_getstat) goto failed; if(!cmd_getstat) goto failed;
while(mntfd > -1 && errctr < MAX_ERR_CTR){ while(mntfd > -1 && errctr < MAX_ERR_CTR && !GlobExit){
// read data to status // read data to status
struct timespec tcur; struct timespec tcur;
if(!curtime(&tcur)) continue; if(!curtime(&tcur)) continue;
@@ -671,14 +673,17 @@ create_thread:
// close all opened serial devices and quit threads // close all opened serial devices and quit threads
void closeSerial(){ void closeSerial(){
DBG("CLOSE all devices"); GlobExit = 1;
DBG("Give 100ms to proper close");
usleep(100000);
DBG("Force closed all devices");
if(mntfd > -1){ if(mntfd > -1){
DBG("Cancel mount thread"); DBG("Cancel mount thread");
pthread_cancel(mntthread); pthread_cancel(mntthread);
DBG("join mount thread"); DBG("join mount thread");
pthread_join(mntthread, NULL); pthread_join(mntthread, NULL);
DBG("close mount fd"); DBG("close mount fd");
close(mntfd); if(mntfd > -1) close(mntfd);
mntfd = -1; mntfd = -1;
} }
if(encfd[0] > -1){ if(encfd[0] > -1){
@@ -687,13 +692,14 @@ void closeSerial(){
DBG("join encoder thread"); DBG("join encoder thread");
pthread_join(encthread, NULL); pthread_join(encthread, NULL);
DBG("close encoder's fd"); DBG("close encoder's fd");
close(encfd[0]); if(encfd[0] > -1) close(encfd[0]);
encfd[0] = -1; encfd[0] = -1;
if(Conf.SepEncoder == 2 && encfd[1] > -1){ if(Conf.SepEncoder == 2 && encfd[1] > -1){
close(encfd[1]); close(encfd[1]);
encfd[1] = -1; encfd[1] = -1;
} }
} }
GlobExit = 0;
} }
// get fresh encoder information // get fresh encoder information
@@ -721,10 +727,10 @@ static int wr(const data_t *out, data_t *in, int needeol){
DBG("Wrong arguments or no mount fd"); DBG("Wrong arguments or no mount fd");
return FALSE; return FALSE;
} }
DBG("clrbuf"); //DBG("clrbuf");
clrmntbuf(); clrmntbuf();
if(out){ if(out){
DBG("write %zd bytes (%s)", out->len, out->buf); //DBG("write %zd bytes (%s)", out->len, out->buf);
if(out->len != (size_t)write(mntfd, out->buf, out->len)){ if(out->len != (size_t)write(mntfd, out->buf, out->len)){
DBG("written bytes not equal to need"); DBG("written bytes not equal to need");
return FALSE; return FALSE;
@@ -755,11 +761,11 @@ static int wr(const data_t *out, data_t *in, int needeol){
*/ */
int MountWriteRead(const data_t *out, data_t *in){ int MountWriteRead(const data_t *out, data_t *in){
if(Conf.RunModel) return FALSE; if(Conf.RunModel) return FALSE;
double t0 = timefromstart(); //double t0 = timefromstart();
pthread_mutex_lock(&mntmutex); pthread_mutex_lock(&mntmutex);
int ret = wr(out, in, 1); int ret = wr(out, in, 1);
pthread_mutex_unlock(&mntmutex); pthread_mutex_unlock(&mntmutex);
DBG("Got %gus", (timefromstart()-t0)*1e6); //DBG("Got %gus", (timefromstart()-t0)*1e6);
return ret; return ret;
} }
// send binary data - without EOL // send binary data - without EOL
@@ -771,7 +777,7 @@ int MountWriteReadRaw(const data_t *out, data_t *in){
return ret; return ret;
} }
#ifdef EBUG #if 0
static void logscmd(SSscmd *c){ static void logscmd(SSscmd *c){
printf("Xmot=%d, Ymot=%d, Xspeed=%d, Yspeed=%d\n", c->Xmot, c->Ymot, c->Xspeed, c->Yspeed); printf("Xmot=%d, Ymot=%d, Xspeed=%d, Yspeed=%d\n", c->Xmot, c->Ymot, c->Xspeed, c->Yspeed);
printf("xychange=0x%02X, Xbits=0x%02X, Ybits=0x%02X\n", c->xychange, c->XBits, c->YBits); printf("xychange=0x%02X, Xbits=0x%02X, Ybits=0x%02X\n", c->xychange, c->XBits, c->YBits);
@@ -794,20 +800,17 @@ static int bincmd(uint8_t *cmd, int len){
if(!dlcmd) dlcmd = cmd2dat(CMD_LONGCMD); if(!dlcmd) dlcmd = cmd2dat(CMD_LONGCMD);
int ret = FALSE; int ret = FALSE;
pthread_mutex_lock(&mntmutex); pthread_mutex_lock(&mntmutex);
// dummy buffer to clear trash in input
//char ans[300];
//data_t a = {.buf = (uint8_t*)ans, .maxlen=299};
if(len == sizeof(SSscmd)){ if(len == sizeof(SSscmd)){
((SSscmd*)cmd)->checksum = SScalcChecksum(cmd, len-2); ((SSscmd*)cmd)->checksum = SScalcChecksum(cmd, len-2);
DBG("Short command"); //DBG("Short command");
#ifdef EBUG #if 0
logscmd((SSscmd*)cmd); logscmd((SSscmd*)cmd);
#endif #endif
if(!wr(dscmd, NULL, 1)) goto rtn; if(!wr(dscmd, NULL, 1)) goto rtn;
}else if(len == sizeof(SSlcmd)){ }else if(len == sizeof(SSlcmd)){
((SSlcmd*)cmd)->checksum = SScalcChecksum(cmd, len-2); ((SSlcmd*)cmd)->checksum = SScalcChecksum(cmd, len-2);
DBG("Long command"); // DBG("Long command");
#ifdef EBUG #if 0
loglcmd((SSlcmd*)cmd); loglcmd((SSlcmd*)cmd);
#endif #endif
if(!wr(dlcmd, NULL, 1)) goto rtn; if(!wr(dlcmd, NULL, 1)) goto rtn;
@@ -822,8 +825,11 @@ static int bincmd(uint8_t *cmd, int len){
ret = wr(&d, &in, 0); ret = wr(&d, &in, 0);
DBG("%s", ret ? "SUCCESS" : "FAIL"); DBG("%s", ret ? "SUCCESS" : "FAIL");
if(ret){ if(ret){
DBG("ANS: Xmot/Ymot: %d/%d, Ylast/Ylast: %d/%d", SSscmd *sc = (SSscmd*)cmd;
ans.Xmot, ans.Ymot, ans.XLast, ans.YLast); mountdata.Xtarget = sc->Xmot;
mountdata.Ytarget = sc->Ymot;
DBG("ANS: Xmot/Ymot: %d/%d, Ylast/Ylast: %d/%d; Xtag/Ytag: %d/%d",
ans.Xmot, ans.Ymot, ans.XLast, ans.YLast, mountdata.Xtarget, mountdata.Ytarget);
} }
rtn: rtn:
pthread_mutex_unlock(&mntmutex); pthread_mutex_unlock(&mntmutex);

23
servo_real.conf Normal file
View File

@@ -0,0 +1,23 @@
MountDevPath=/dev/ttyUSB0
MountDevSpeed=19200
EncoderDevSpeed=1000000
MountReqInterval=0.1
EncoderReqInterval=0.001
SepEncoder=2
EncoderXDevPath=/dev/encoder_X0
EncoderYDevPath=/dev/encoder_Y0
EncoderSpeedInterval=0.05
RunModel=0
# telescope is in "pointing state" when coordinate error less than MaxFinePointingErr and goes to "slewing state"
# when this error greater than MaxPointingErr
MaxPointingErr = 0.3490658504 # "pointing zone" - 20 degr
MaxFinePointingErr = 0.1745329252 # "guiding zone" - 10 degr
MaxGuidingErr = 4.8481368e-6 # "on target zone" - 1 arcsec
XPIDVP=0.9
XPIDVI=0.0005
XPIDVD=0.0
YPIDVP=0.5
YPIDVI=0.005
YPIDVD=0.
XEncZero=36627112
YEncZero=36067741

3
sidservo.h
View File

@@ -158,6 +158,9 @@ typedef struct{
uint32_t millis; uint32_t millis;
double temperature; double temperature;
double voltage; double voltage;
// target X/Y position by last `short` or `long` command
int32_t Xtarget; // in SidServo's counts
int32_t Ytarget; // -//-
} mountdata_t; } mountdata_t;
typedef struct{ typedef struct{

11
ssii.c
View File

@@ -26,7 +26,7 @@
#include "serial.h" #include "serial.h"
#include "ssii.h" #include "ssii.h"
int X_ENC_ZERO = 0, Y_ENC_ZERO = 0; int X_ENC_ZERO = 0, Y_ENC_ZERO = 0; // will be filled later from config
// defaults until read from controller // defaults until read from controller
double X_MOT_STEPSPERREV = 13312000., double X_MOT_STEPSPERREV = 13312000.,
Y_MOT_STEPSPERREV = 17578668., Y_MOT_STEPSPERREV = 17578668.,
@@ -45,12 +45,13 @@ uint16_t SScalcChecksum(uint8_t *buf, int len){
} }
// Next three functions runs under locked mountdata_t mutex and shouldn't call locked it again!! // Next three functions runs under locked mountdata_t mutex and shouldn't call locked it again!!
static axis_status_t chkstopstat(int32_t *prev, int32_t cur, int *nstopped, axis_status_t stat){ static axis_status_t chkstopstat(int32_t *prev, int32_t cur, int32_t tag, int *nstopped, axis_status_t stat){
if(*prev == INT32_MAX){ if(*prev == INT32_MAX){
stat = AXIS_STOPPED; stat = AXIS_STOPPED;
DBG("START"); DBG("START");
}else if(stat == AXIS_GONNASTOP){ // check stop }else if(stat == AXIS_GONNASTOP || (stat != AXIS_STOPPED && cur == tag)){ // got command "stop" or motor is on target
if(*prev == cur){ if(*prev == cur){
DBG("Test for stop, nstopped=%d", *nstopped);
if(++(*nstopped) > MOTOR_STOPPED_CNT){ if(++(*nstopped) > MOTOR_STOPPED_CNT){
stat = AXIS_STOPPED; stat = AXIS_STOPPED;
DBG("AXIS stopped"); DBG("AXIS stopped");
@@ -68,8 +69,8 @@ static void ChkStopped(const SSstat *s, mountdata_t *m){
static int32_t Xmot_prev = INT32_MAX, Ymot_prev = INT32_MAX; // previous coordinates static int32_t Xmot_prev = INT32_MAX, Ymot_prev = INT32_MAX; // previous coordinates
static int Xnstopped = 0, Ynstopped = 0; // counters to get STOPPED state static int Xnstopped = 0, Ynstopped = 0; // counters to get STOPPED state
axis_status_t Xstat, Ystat; axis_status_t Xstat, Ystat;
Xstat = chkstopstat(&Xmot_prev, s->Xmot, &Xnstopped, m->Xstate); Xstat = chkstopstat(&Xmot_prev, s->Xmot, m->Xtarget, &Xnstopped, m->Xstate);
Ystat = chkstopstat(&Ymot_prev, s->Ymot, &Ynstopped, m->Ystate); Ystat = chkstopstat(&Ymot_prev, s->Ymot, m->Ytarget, &Ynstopped, m->Ystate);
if(Xstat != m->Xstate || Ystat != m->Ystate){ if(Xstat != m->Xstate || Ystat != m->Ystate){
DBG("Status changed"); DBG("Status changed");
setStat(Xstat, Ystat); setStat(Xstat, Ystat);