less squares 4 speed + fixed some bugs (but found more)
This commit is contained in:
@@ -33,13 +33,14 @@
|
||||
#include "main.h"
|
||||
#include "movingmodel.h"
|
||||
#include "serial.h"
|
||||
#include "ssii.h"
|
||||
|
||||
// serial devices FD
|
||||
static int encfd[2] = {-1, -1}, mntfd = -1;
|
||||
// main mount data
|
||||
static mountdata_t mountdata = {0};
|
||||
// last encoders time and last encoders data - for speed measurement
|
||||
static coordval_t lastXenc = {0}, lastYenc = {0};
|
||||
//static coordval_t lastXenc = {0}, lastYenc = {0};
|
||||
|
||||
// mutexes for RW operations with mount device and data
|
||||
static pthread_mutex_t mntmutex = PTHREAD_MUTEX_INITIALIZER,
|
||||
@@ -57,17 +58,42 @@ typedef struct __attribute__((packed)){
|
||||
} enc_t;
|
||||
|
||||
// calculate current X/Y speeds
|
||||
static void getXspeed(double t){
|
||||
void getXspeed(){
|
||||
static less_square_t *ls = NULL;
|
||||
if(!ls){
|
||||
ls = LS_init(Conf.EncoderSpeedInterval / Conf.EncoderReqInterval);
|
||||
if(!ls) return;
|
||||
}
|
||||
double speed = LS_calc_slope(ls, mountdata.encXposition.val, mountdata.encXposition.t);
|
||||
if(fabs(speed) < 1.5 * MCC_MAX_X_SPEED){
|
||||
mountdata.encXspeed.val = speed;
|
||||
mountdata.encXspeed.t = mountdata.encXposition.t;
|
||||
}
|
||||
//DBG("Xspeed=%g", mountdata.encXspeed.val);
|
||||
#if 0
|
||||
mountdata.encXspeed.val = (mountdata.encXposition.val - lastXenc.val) / (t - lastXenc.t);
|
||||
mountdata.encXspeed.t = (lastXenc.t + t) / 2.;
|
||||
mountdata.encXspeed.t = (lastXenc.t + mountdata.encXposition.t) / 2.;
|
||||
lastXenc.val = mountdata.encXposition.val;
|
||||
lastXenc.t = t;
|
||||
#endif
|
||||
}
|
||||
static void getYspeed(double t){
|
||||
void getYspeed(){
|
||||
static less_square_t *ls = NULL;
|
||||
if(!ls){
|
||||
ls = LS_init(Conf.EncoderSpeedInterval / Conf.EncoderReqInterval);
|
||||
if(!ls) return;
|
||||
}
|
||||
double speed = LS_calc_slope(ls, mountdata.encYposition.val, mountdata.encYposition.t);
|
||||
if(fabs(speed) < 1.5 * MCC_MAX_Y_SPEED){
|
||||
mountdata.encYspeed.val = speed;
|
||||
mountdata.encYspeed.t = mountdata.encYposition.t;
|
||||
}
|
||||
#if 0
|
||||
mountdata.encYspeed.val = (mountdata.encYposition.val - lastYenc.val) / (t - lastYenc.t);
|
||||
mountdata.encYspeed.t = (lastYenc.t + t) / 2.;
|
||||
mountdata.encYspeed.t = (lastYenc.t + mountdata.encYposition.t) / 2.;
|
||||
lastYenc.val = mountdata.encYposition.val;
|
||||
lastYenc.t = t;
|
||||
#endif
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -115,8 +141,9 @@ static void parse_encbuf(uint8_t databuf[ENC_DATALEN], double t){
|
||||
DBG("Got positions X/Y= %.6g / %.6g", mountdata.encXposition.val, mountdata.encYposition.val);
|
||||
mountdata.encXposition.t = t;
|
||||
mountdata.encYposition.t = t;
|
||||
if(t - lastXenc.t > Conf.EncoderSpeedInterval) getXspeed(t);
|
||||
if(t - lastYenc.t > Conf.EncoderSpeedInterval) getYspeed(t);
|
||||
//if(t - lastXenc.t > Conf.EncoderSpeedInterval) getXspeed();
|
||||
//if(t - lastYenc.t > Conf.EncoderSpeedInterval) getYspeed();
|
||||
getXspeed(); getYspeed();
|
||||
pthread_mutex_unlock(&datamutex);
|
||||
//DBG("time = %zd+%zd/1e6, X=%g deg, Y=%g deg", tv->tv_sec, tv->tv_usec, mountdata.encposition.X*180./M_PI, mountdata.encposition.Y*180./M_PI);
|
||||
}
|
||||
@@ -222,6 +249,31 @@ static int getmntbyte(){
|
||||
}while(1);
|
||||
return (int)byte;
|
||||
}
|
||||
// clear data from input buffer
|
||||
static void clrmntbuf(){
|
||||
if(mntfd < 0) return;
|
||||
uint8_t byte;
|
||||
fd_set rfds;
|
||||
//double t0 = nanotime();
|
||||
//int n = 0;
|
||||
do{
|
||||
FD_ZERO(&rfds);
|
||||
FD_SET(mntfd, &rfds);
|
||||
struct timeval tv = {.tv_sec=0, .tv_usec=10};
|
||||
int retval = select(mntfd + 1, &rfds, NULL, NULL, &tv);
|
||||
if(retval < 0){
|
||||
if(errno == EINTR) continue;
|
||||
DBG("Error in select()");
|
||||
break;
|
||||
}
|
||||
if(FD_ISSET(mntfd, &rfds)){
|
||||
ssize_t l = read(mntfd, &byte, 1);
|
||||
if(l != 1) break;
|
||||
//++n;
|
||||
} else break;
|
||||
}while(1);
|
||||
//DBG("Cleared by %g (got %d bytes)", nanotime() - t0, n);
|
||||
}
|
||||
|
||||
// main encoder thread (for separate encoder): read next data and make parsing
|
||||
static void *encoderthread1(void _U_ *u){
|
||||
@@ -273,12 +325,14 @@ static void *encoderthread2(void _U_ *u){
|
||||
mountdata.encXposition.val = X_ENC2RAD(v);
|
||||
//DBG("encX(%g) = %g", t, mountdata.encXposition.val);
|
||||
mountdata.encXposition.t = t;
|
||||
if(t - lastXenc.t > Conf.EncoderSpeedInterval) getXspeed(t);
|
||||
//if(t - lastXenc.t > Conf.EncoderSpeedInterval) getXspeed();
|
||||
getXspeed();
|
||||
if(getencval(encfd[1], &v, &t)){
|
||||
mountdata.encYposition.val = Y_ENC2RAD(v);
|
||||
//DBG("encY(%g) = %g", t, mountdata.encYposition.val);
|
||||
mountdata.encYposition.t = t;
|
||||
if(t - lastYenc.t > Conf.EncoderSpeedInterval) getYspeed(t);
|
||||
//if(t - lastYenc.t > Conf.EncoderSpeedInterval) getYspeed();
|
||||
getYspeed();
|
||||
errctr = 0;
|
||||
need2ask = 0;
|
||||
} else {
|
||||
@@ -326,13 +380,14 @@ static void *mountthread(void _U_ *u){
|
||||
int errctr = 0;
|
||||
uint8_t buf[2*sizeof(SSstat)];
|
||||
SSstat *status = (SSstat*) buf;
|
||||
bzero(&mountdata, sizeof(mountdata));
|
||||
if(Conf.RunModel) while(1){
|
||||
pthread_mutex_lock(&datamutex);
|
||||
// now change data
|
||||
getModData(&mountdata);
|
||||
pthread_mutex_unlock(&datamutex);
|
||||
double t0 = nanotime();
|
||||
while(nanotime() - t0 < Conf.MountReqInterval) usleep(500);
|
||||
while(nanotime() - t0 < Conf.EncoderReqInterval) usleep(50);
|
||||
t0 = nanotime();
|
||||
}
|
||||
// data to get
|
||||
@@ -342,8 +397,32 @@ static void *mountthread(void _U_ *u){
|
||||
if(!cmd_getstat) goto failed;
|
||||
while(mntfd > -1 && errctr < MAX_ERR_CTR){
|
||||
// read data to status
|
||||
double tgot = nanotime(), t0 = tgot;
|
||||
//DBG("tgot=%g", tgot);
|
||||
double t0 = nanotime();
|
||||
#if 0
|
||||
// 127 milliseconds to get answer on X/Y commands!!!
|
||||
int64_t ans;
|
||||
int ctr = 0;
|
||||
if(SSgetint(CMD_MOTX, &ans)){
|
||||
pthread_mutex_lock(&datamutex);
|
||||
mountdata.motXposition.t = tgot;
|
||||
mountdata.motXposition.val = X_MOT2RAD(ans);
|
||||
pthread_mutex_unlock(&datamutex);
|
||||
++ctr;
|
||||
}
|
||||
tgot = nanotime();
|
||||
if(SSgetint(CMD_MOTY, &ans)){
|
||||
pthread_mutex_lock(&datamutex);
|
||||
mountdata.motXposition.t = tgot;
|
||||
mountdata.motXposition.val = X_MOT2RAD(ans);
|
||||
pthread_mutex_unlock(&datamutex);
|
||||
++ctr;
|
||||
}
|
||||
if(ctr == 2){
|
||||
mountdata.millis = (uint32_t)(1e3 * tgot);
|
||||
DBG("Got both coords; millis=%d", mountdata.millis);
|
||||
}
|
||||
#endif
|
||||
// 80 milliseconds to get answer on GETSTAT
|
||||
if(!MountWriteRead(cmd_getstat, &d) || d.len != sizeof(SSstat)){
|
||||
#ifdef EBUG
|
||||
DBG("Can't read SSstat, need %zd got %zd bytes", sizeof(SSstat), d.len);
|
||||
@@ -359,11 +438,12 @@ static void *mountthread(void _U_ *u){
|
||||
errctr = 0;
|
||||
pthread_mutex_lock(&datamutex);
|
||||
// now change data
|
||||
SSconvstat(status, &mountdata, tgot);
|
||||
SSconvstat(status, &mountdata, t0);
|
||||
pthread_mutex_unlock(&datamutex);
|
||||
//DBG("GOT FULL stat by %g", nanotime() - t0);
|
||||
// allow writing & getters
|
||||
do{
|
||||
usleep(5000);
|
||||
usleep(500);
|
||||
}while(nanotime() - t0 < Conf.MountReqInterval);
|
||||
t0 = nanotime();
|
||||
}
|
||||
@@ -439,7 +519,7 @@ int openEncoder(){
|
||||
|
||||
// return FALSE if failed
|
||||
int openMount(){
|
||||
if(Conf.RunModel) return TRUE;
|
||||
if(Conf.RunModel) goto create_thread;
|
||||
if(mntfd > -1) close(mntfd);
|
||||
DBG("Open mount %s @ %d", Conf.MountDevPath, Conf.MountDevSpeed);
|
||||
mntfd = ttyopen(Conf.MountDevPath, (speed_t) Conf.MountDevSpeed);
|
||||
@@ -456,10 +536,13 @@ int openMount(){
|
||||
}while(1);*/
|
||||
mntRtmout.tv_sec = 0;
|
||||
mntRtmout.tv_usec = 500000000 / Conf.MountDevSpeed; // 50 bytes * 10bits / speed
|
||||
create_thread:
|
||||
if(pthread_create(&mntthread, NULL, mountthread, NULL)){
|
||||
DBG("Can't create thread");
|
||||
close(mntfd);
|
||||
mntfd = -1;
|
||||
DBG("Can't create mount thread");
|
||||
if(!Conf.RunModel){
|
||||
close(mntfd);
|
||||
mntfd = -1;
|
||||
}
|
||||
return FALSE;
|
||||
}
|
||||
DBG("Mount opened, thread started");
|
||||
@@ -512,6 +595,7 @@ void setStat(mnt_status_t Xstatus, mnt_status_t Ystatus){
|
||||
// write-read without locking mutex (to be used inside other functions)
|
||||
static int wr(const data_t *out, data_t *in, int needeol){
|
||||
if((!out && !in) || mntfd < 0) return FALSE;
|
||||
clrmntbuf();
|
||||
//double t0 = nanotime();
|
||||
if(out){
|
||||
if(out->len != (size_t)write(mntfd, out->buf, out->len)){
|
||||
@@ -525,9 +609,10 @@ static int wr(const data_t *out, data_t *in, int needeol){
|
||||
}
|
||||
}
|
||||
//DBG("sent by %g", nanotime() - t0);
|
||||
uint8_t buf[256];
|
||||
data_t dumb = {.buf = buf, .maxlen = 256};
|
||||
if(!in) in = &dumb; // even if user don't ask for answer, try to read to clear trash
|
||||
//uint8_t buf[256];
|
||||
//data_t dumb = {.buf = buf, .maxlen = 256};
|
||||
if(!in) return TRUE;
|
||||
//if(!in) in = &dumb; // even if user don't ask for answer, try to read to clear trash
|
||||
in->len = 0;
|
||||
for(size_t i = 0; i < in->maxlen; ++i){
|
||||
int b = getmntbyte();
|
||||
@@ -585,26 +670,25 @@ static int bincmd(uint8_t *cmd, int len){
|
||||
int ret = FALSE;
|
||||
pthread_mutex_lock(&mntmutex);
|
||||
// dummy buffer to clear trash in input
|
||||
char ans[300];
|
||||
data_t a = {.buf = (uint8_t*)ans, .maxlen=299};
|
||||
//char ans[300];
|
||||
//data_t a = {.buf = (uint8_t*)ans, .maxlen=299};
|
||||
if(len == sizeof(SSscmd)){
|
||||
((SSscmd*)cmd)->checksum = SScalcChecksum(cmd, len-2);
|
||||
DBG("Short command");
|
||||
#ifdef EBUG
|
||||
logscmd((SSscmd*)cmd);
|
||||
#endif
|
||||
if(!wr(dscmd, &a, 1)) goto rtn;
|
||||
if(!wr(dscmd, NULL, 1)) goto rtn;
|
||||
}else if(len == sizeof(SSlcmd)){
|
||||
((SSlcmd*)cmd)->checksum = SScalcChecksum(cmd, len-2);
|
||||
DBG("Long command");
|
||||
#ifdef EBUG
|
||||
loglcmd((SSlcmd*)cmd);
|
||||
#endif
|
||||
if(!wr(dlcmd, &a, 1)) goto rtn;
|
||||
if(!wr(dlcmd, NULL, 1)) goto rtn;
|
||||
}else{
|
||||
goto rtn;
|
||||
}
|
||||
DBG("Write %d bytes and wait for ans", len);
|
||||
data_t d;
|
||||
d.buf = cmd;
|
||||
d.len = d.maxlen = len;
|
||||
|
||||
Reference in New Issue
Block a user