nxt
This commit is contained in:
parent
55d8d359b4
commit
d7df8e5bf1
@ -16,6 +16,7 @@
|
||||
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <math.h>
|
||||
#include <signal.h>
|
||||
#include <stdio.h>
|
||||
#include <time.h>
|
||||
@ -31,6 +32,7 @@ typedef struct{
|
||||
} parameters;
|
||||
|
||||
static parameters G = {0};
|
||||
static FILE *fcoords = NULL;
|
||||
|
||||
static sl_option_t cmdlnopts[] = {
|
||||
{"help", NO_ARGS, NULL, 'h', arg_int, APTR(&G.help), "show this help"},
|
||||
@ -51,13 +53,67 @@ void signals(int sig){
|
||||
}
|
||||
|
||||
static conf_t Config = {
|
||||
.MountDevPath = "/dev/ttyS1",
|
||||
.MountDevPath = "/dev/ttyUSB0",
|
||||
.MountDevSpeed = 19200,
|
||||
//.EncoderDevPath = "/dev/ttyUSB0",
|
||||
//.EncoderDevPath = "/dev/ttyUSB1",
|
||||
//.EncoderDevSpeed = 153000,
|
||||
.MountReqInterval = 0.1,
|
||||
.SepEncoder = 0
|
||||
};
|
||||
|
||||
#define DEG2RAD(d) (d/180.*M_PI)
|
||||
#define RAD2DEG(d) (d/M_PI*180.)
|
||||
|
||||
|
||||
static void logmnt(mountdata_t *m){
|
||||
DBG("LOG");
|
||||
static double t0 = -1.;
|
||||
if(!fcoords) return;
|
||||
if(!m){ // write header
|
||||
fprintf(fcoords, "# time Xmot(deg) Ymot(deg) Xenc(deg) Yenc(deg) millis T V\n");
|
||||
return;
|
||||
}
|
||||
if(t0 < 0.) t0 = m->motposition.msrtime.tv_sec + (double)(m->motposition.msrtime.tv_usec) / 1e6;
|
||||
double t = m->motposition.msrtime.tv_sec + (double)(m->motposition.msrtime.tv_usec) / 1e6 - t0;
|
||||
// write data
|
||||
fprintf(fcoords, "%12.6f %10.6f %10.6f %10.6f %10.6f %10u %6.1f %4.1f\n",
|
||||
t, RAD2DEG(m->motposition.X), RAD2DEG(m->motposition.Y),
|
||||
RAD2DEG(m->encposition.X), RAD2DEG(m->encposition.Y),
|
||||
m->millis, m->temperature, m->voltage);
|
||||
}
|
||||
|
||||
// dump moving but not longer than t secs; exit after 5 stopped events
|
||||
static void dumpmoving(double t){
|
||||
mountdata_t mdata;
|
||||
DBG("Start dump");
|
||||
int ntries = 0;
|
||||
for(; ntries < 10; ++ntries){
|
||||
if(MCC_E_OK == Mount.getMountData(&mdata)) break;
|
||||
}
|
||||
if(ntries == 10){
|
||||
WARNX("Can't get mount data");
|
||||
LOGWARN("Can't get mount data");
|
||||
}
|
||||
suseconds_t usec = mdata.motposition.msrtime.tv_usec;
|
||||
int ctr = 0;
|
||||
double xlast = mdata.motposition.X, ylast = mdata.motposition.Y;
|
||||
double t0 = sl_dtime();
|
||||
DBG("usec = %ld", usec);
|
||||
while(sl_dtime() - t0 < t && ctr < 5){
|
||||
usleep(10000);
|
||||
if(MCC_E_OK != Mount.getMountData(&mdata)){ WARNX("Can't get data"); continue;}
|
||||
if(mdata.motposition.msrtime.tv_usec == usec) continue;
|
||||
DBG("Got new data, posX=%g, posY=%g", mdata.motposition.X, mdata.motposition.Y);
|
||||
usec = mdata.motposition.msrtime.tv_usec;
|
||||
if(fcoords) logmnt(&mdata);
|
||||
if(mdata.motposition.X != xlast || mdata.motposition.Y != ylast){
|
||||
xlast = mdata.motposition.X;
|
||||
ylast = mdata.motposition.Y;
|
||||
ctr = 0;
|
||||
}else ++ctr;
|
||||
}
|
||||
}
|
||||
|
||||
int main(int argc, char **argv){
|
||||
sl_init();
|
||||
sl_parseargs(&argc, &argv, cmdlnopts);
|
||||
@ -65,18 +121,30 @@ int main(int argc, char **argv){
|
||||
sl_loglevel_e lvl = G.verbose + LOGLEVEL_ERR;
|
||||
if(lvl >= LOGLEVEL_AMOUNT) lvl = LOGLEVEL_AMOUNT - 1;
|
||||
if(G.logfile) OPENLOG(G.logfile, lvl, 1);
|
||||
if(G.coordsoutput){
|
||||
if(!(fcoords = fopen(G.coordsoutput, "w")))
|
||||
ERRX("Can't open %s", G.coordsoutput);
|
||||
logmnt(NULL);
|
||||
}
|
||||
time_t curtime = time(NULL);
|
||||
LOGMSG("Started @ %s", ctime(&curtime));
|
||||
DBG("Devices ready");
|
||||
LOGMSG("Mount device %s @ %d", Config.MountDevPath, Config.MountDevSpeed);
|
||||
LOGMSG("Encoder device %s @ %d", Config.EncoderDevPath, Config.EncoderDevSpeed);
|
||||
mcc_errcodes_t e = Mount.init(&Config);
|
||||
if(e != MCC_E_OK){
|
||||
WARNX("Can't init devices");
|
||||
return 1;
|
||||
}
|
||||
signal(SIGTERM, signals); // kill (-15) - quit
|
||||
signal(SIGHUP, SIG_IGN); // hup - ignore
|
||||
signal(SIGINT, signals); // ctrl+C - quit
|
||||
signal(SIGQUIT, signals); // ctrl+\ - quit
|
||||
signal(SIGTSTP, SIG_IGN); // ignore ctrl+Z
|
||||
double t0 = sl_dtime();
|
||||
while(sl_dtime() - t0 < 10.);
|
||||
Mount.quit();
|
||||
if(MCC_E_OK != Mount.moveTo(DEG2RAD(45.), DEG2RAD(45.)))
|
||||
ERRX("Can't move to 45, 45");
|
||||
dumpmoving(30.);
|
||||
Mount.moveTo(0., 0.);
|
||||
dumpmoving(30.);
|
||||
signals(0);
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<!DOCTYPE QtCreatorProject>
|
||||
<!-- Written by QtCreator 15.0.0, 2025-02-04T21:29:56. -->
|
||||
<!-- Written by QtCreator 15.0.0, 2025-02-08T19:46:09. -->
|
||||
<qtcreator>
|
||||
<data>
|
||||
<variable>EnvironmentId</variable>
|
||||
|
||||
@ -21,6 +21,7 @@
|
||||
|
||||
#include "dbg.h"
|
||||
#include "serial.h"
|
||||
#include "ssii.h"
|
||||
|
||||
conf_t Conf = {0};
|
||||
|
||||
@ -29,6 +30,7 @@ conf_t Conf = {0};
|
||||
*/
|
||||
static void quit(){
|
||||
DBG("Close serial devices");
|
||||
for(int i = 0; i < 10; ++i) if(SSemergStop()) break;
|
||||
closeSerial();
|
||||
DBG("Exit");
|
||||
}
|
||||
@ -39,6 +41,7 @@ static void quit(){
|
||||
* @return error code
|
||||
*/
|
||||
static mcc_errcodes_t init(conf_t *c){
|
||||
FNAME();
|
||||
if(!c) return MCC_E_BADFORMAT;
|
||||
Conf = *c;
|
||||
mcc_errcodes_t ret = MCC_E_OK;
|
||||
@ -58,17 +61,39 @@ static mcc_errcodes_t init(conf_t *c){
|
||||
ret = MCC_E_ENCODERDEV;
|
||||
}
|
||||
}
|
||||
if(Conf.MountReqInterval < 1 || Conf.MountReqInterval > 1000000){
|
||||
if(Conf.MountReqInterval > 1. || Conf.MountReqInterval < 0.001){
|
||||
DBG("Bad value of MountReqInterval");
|
||||
retr = MCC_E_FATAL;
|
||||
ret = MCC_E_BADFORMAT;
|
||||
}
|
||||
if(ret != MCC_E_OK) quit();
|
||||
return ret;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief move2 - simple move to given point and stop
|
||||
* @param X - new X coordinate (radians: -pi..pi)
|
||||
* @param Y - new Y coordinate (radians: -pi..pi)
|
||||
* @return error code
|
||||
*/
|
||||
static mcc_errcodes_t move2(double X, double Y){
|
||||
if(X > M_PI || X < -M_PI || Y > M_PI || Y < -M_PI){
|
||||
DBG("Wrong coords: X=%g, Y=%g", X, Y);
|
||||
return MCC_E_BADFORMAT;
|
||||
}
|
||||
if(!SSXmoveto(X) || !SSYmoveto(Y)) return MCC_E_FAILED;
|
||||
return MCC_E_OK;
|
||||
}
|
||||
|
||||
static mcc_errcodes_t emstop(){
|
||||
if(!SSemergStop()) return MCC_E_FAILED;
|
||||
return MCC_E_OK;
|
||||
}
|
||||
|
||||
// init mount class
|
||||
mount_t Mount = {
|
||||
.init = init,
|
||||
.quit = quit,
|
||||
.getMountData = getMD
|
||||
.getMountData = getMD,
|
||||
.moveTo = move2,
|
||||
.emergStop = emstop,
|
||||
};
|
||||
|
||||
@ -54,15 +54,11 @@ typedef struct __attribute__((packed)){
|
||||
uint8_t CRC[4];
|
||||
} enc_t;
|
||||
|
||||
#define Xpos2rad(pos) (2. * (double)pos / ENC_TURN_XTICKS * M_PI)
|
||||
#define Ypos2rad(pos) (2. * (double)pos / ENC_TURN_YTICKS * M_PI)
|
||||
|
||||
#if 0
|
||||
/**
|
||||
* @brief dtime - UNIX time with microsecond
|
||||
* @return value
|
||||
*/
|
||||
static double dtime(){
|
||||
double dtime(){
|
||||
double t;
|
||||
struct timeval tv;
|
||||
gettimeofday(&tv, NULL);
|
||||
@ -70,6 +66,7 @@ static double dtime(){
|
||||
return t;
|
||||
}
|
||||
|
||||
#if 0
|
||||
// init start time
|
||||
static void gttime(){
|
||||
struct timeval tv;
|
||||
@ -112,11 +109,11 @@ static void parse_encbuf(uint8_t databuf[ENC_DATALEN], struct timeval *tv){
|
||||
return;
|
||||
}
|
||||
pthread_mutex_lock(&datamutex);
|
||||
mountdata.position.X = Xpos2rad(edata->encX);
|
||||
mountdata.position.Y = Ypos2rad(edata->encY);
|
||||
mountdata.position.msrtime = *tv;
|
||||
mountdata.encposition.X = X_ENC2RAD(edata->encX);
|
||||
mountdata.encposition.Y = Y_ENC2RAD(edata->encY);
|
||||
mountdata.encposition.msrtime = *tv;
|
||||
pthread_mutex_unlock(&datamutex);
|
||||
DBG("time = %zd+%zd/1e6, X=%g deg, Y=%g deg", tv->tv_sec, tv->tv_usec, mountdata.position.X*180./M_PI, mountdata.position.Y*180./M_PI);
|
||||
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);
|
||||
}
|
||||
|
||||
// try to read 1 byte from encoder; return -1 if nothing to read or -2 if device seems to be disconnected
|
||||
@ -149,18 +146,25 @@ static int getmntbyte(){
|
||||
uint8_t byte;
|
||||
fd_set rfds;
|
||||
struct timeval tv;
|
||||
/* ssize_t l = read(mntfd, &byte, 1);
|
||||
//DBG("MNT read=%zd byte=0x%X", l, byte);
|
||||
if(l == 0) return -1;
|
||||
if(l != 1) return -2; // disconnected ??
|
||||
return (int) byte;*/
|
||||
do{
|
||||
FD_ZERO(&rfds);
|
||||
FD_SET(mntfd, &rfds);
|
||||
tv = mntRtmout;
|
||||
int retval = select(mntfd + 1, &rfds, NULL, NULL, &tv);
|
||||
if(!retval) break;
|
||||
if(retval < 0){
|
||||
if(errno == EINTR) continue;
|
||||
DBG("Error in select()");
|
||||
return -1;
|
||||
}
|
||||
//DBG("FD_ISSET = %d", FD_ISSET(mntfd, &rfds));
|
||||
if(FD_ISSET(mntfd, &rfds)){
|
||||
ssize_t l = read(mntfd, &byte, 1);
|
||||
//DBG("MNT read=%zd byte=0x%X", l, byte);
|
||||
if(l != 1) return -2; // disconnected ??
|
||||
break;
|
||||
} else return -1;
|
||||
@ -199,35 +203,58 @@ static void *encoderthread(void _U_ *u){
|
||||
return NULL;
|
||||
}
|
||||
|
||||
data_t *cmd2dat(const char *cmd){
|
||||
if(!cmd) return NULL;
|
||||
data_t *d = calloc(1, sizeof(data_t));
|
||||
if(!d) return NULL;
|
||||
d->buf = (uint8_t*)strdup(cmd);
|
||||
d->len = strlen(cmd);
|
||||
d->maxlen = d->len + 1;
|
||||
return d;
|
||||
}
|
||||
void data_free(data_t **x){
|
||||
if(!x || !*x) return;
|
||||
free((*x)->buf);
|
||||
free(*x);
|
||||
*x = NULL;
|
||||
}
|
||||
|
||||
// main mount thread
|
||||
static void *mountthread(void _U_ *u){
|
||||
int errctr = 0;
|
||||
SSstat status;
|
||||
uint8_t buf[2*sizeof(SSstat)];
|
||||
SSstat *status = (SSstat*) buf;
|
||||
// data to get
|
||||
data_t d = {.buf = (uint8_t*)&status, .maxlen = sizeof(SSstat)};
|
||||
data_t d = {.buf = buf, .maxlen = sizeof(buf)};
|
||||
// cmd to send
|
||||
const data_t cmd = {.buf = CMD_GETSTAT, .len = sizeof(CMD_GETSTAT)-1, .maxlen = sizeof(CMD_GETSTAT)-1};
|
||||
const data_t *cmd_getstat = cmd2dat(CMD_GETSTAT);
|
||||
double t0 = dtime();
|
||||
/*
|
||||
#ifdef EBUG
|
||||
double t00 = t0;
|
||||
#endif
|
||||
*/
|
||||
while(mntfd > -1 && errctr < MAX_ERR_CTR){
|
||||
// read data to status
|
||||
if(MountWriteRead(&cmd, &d) || d.len != sizeof(SSstat)){
|
||||
DBG("Can't read SSstat");
|
||||
struct timeval tgot;
|
||||
if(0 != gettimeofday(&tgot, NULL)) continue;
|
||||
if(!MountWriteRead(cmd_getstat, &d) || d.len != sizeof(SSstat)){
|
||||
DBG("Can't read SSstat, need %zd got %zd bytes", sizeof(SSstat), d.len);
|
||||
++errctr; continue;
|
||||
}
|
||||
if(SScalcChecksum((uint8_t*)status, sizeof(SSstat)-2) != status.checksum){
|
||||
DBG("BAD checksum of SSstat");
|
||||
if(SScalcChecksum(buf, sizeof(SSstat)-2) != status->checksum){
|
||||
DBG("BAD checksum of SSstat, need %d", status->checksum);
|
||||
++errctr; continue;
|
||||
}
|
||||
errctr = 0;
|
||||
pthread_mutex_lock(&datamutex);
|
||||
// now change data
|
||||
SSconvstat(&status, &mountdata);
|
||||
if(!Conf.SepEncoder){ // fill encoder data from here, as there's no separate enc thread
|
||||
;
|
||||
}
|
||||
;
|
||||
SSconvstat(status, &mountdata, &tgot);
|
||||
pthread_mutex_unlock(&datamutex);
|
||||
// allow writing & getters
|
||||
usleep(Conf.MountReqInterval);
|
||||
//DBG("t0=%g, tnow=%g", t0-t00, dtime()-t00);
|
||||
while(dtime() - t0 < Conf.MountReqInterval);
|
||||
t0 = dtime();
|
||||
}
|
||||
if(mntfd > -1){
|
||||
close(mntfd);
|
||||
@ -249,13 +276,13 @@ 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)){ close(fd); return -1; }
|
||||
DBG("Check speed");
|
||||
if(tty.c_ispeed != (speed_t) speed || tty.c_ospeed != (speed_t)speed){ close(fd); return -1; }
|
||||
// try to set exclusive
|
||||
ioctl(fd, TIOCEXCL);
|
||||
if(ioctl(fd, TIOCEXCL)){DBG("Can't make exclusive");}
|
||||
return fd;
|
||||
}
|
||||
|
||||
@ -266,7 +293,7 @@ int openEncoder(const char *path, int speed){
|
||||
encfd = ttyopen(path, (speed_t) speed);
|
||||
if(encfd < 0) return FALSE;
|
||||
encRtmout.tv_sec = 0;
|
||||
encRtmout.tv_usec = 15000000 / speed; // 1.5 bytes
|
||||
encRtmout.tv_usec = 200000000 / speed; // 20 bytes
|
||||
if(pthread_create(&encthread, NULL, encoderthread, NULL)){
|
||||
close(encfd);
|
||||
encfd = -1;
|
||||
@ -279,11 +306,23 @@ int openEncoder(const char *path, int speed){
|
||||
// return FALSE if failed
|
||||
int openMount(const char *path, int speed){
|
||||
if(mntfd > -1) close(mntfd);
|
||||
DBG("Open mount %s @ %d", path, speed);
|
||||
mntfd = ttyopen(path, (speed_t) speed);
|
||||
if(mntfd < 0) return FALSE;
|
||||
DBG("mntfd=%d", mntfd);
|
||||
// clear buffer
|
||||
while(getmntbyte() > -1);
|
||||
/*int g = write(mntfd, "XXS\r", 4);
|
||||
DBG("Written %d", g);
|
||||
uint8_t buf[100];
|
||||
do{
|
||||
ssize_t l = read(mntfd, buf, 100);
|
||||
DBG("got %zd", l);
|
||||
}while(1);*/
|
||||
mntRtmout.tv_sec = 0;
|
||||
mntRtmout.tv_usec = 20000000 / speed; // 2 bytes
|
||||
mntRtmout.tv_usec = 500000000 / speed; // 50 bytes
|
||||
if(pthread_create(&mntthread, NULL, mountthread, NULL)){
|
||||
DBG("Can't create thread");
|
||||
close(mntfd);
|
||||
mntfd = -1;
|
||||
return FALSE;
|
||||
@ -295,16 +334,18 @@ int openMount(const char *path, int speed){
|
||||
// close all opened serial devices and quit threads
|
||||
void closeSerial(){
|
||||
if(mntfd > -1){
|
||||
DBG("Close mount");
|
||||
pthread_mutex_lock(&mntmutex);
|
||||
DBG("Kill mount thread");
|
||||
if(0 == pthread_cancel(mntthread))
|
||||
pthread_join(mntthread, NULL);
|
||||
DBG("close fd");
|
||||
close(mntfd);
|
||||
mntfd = -1;
|
||||
pthread_mutex_unlock(&mntmutex);
|
||||
}
|
||||
if(encfd > -1){
|
||||
DBG("Close encoder");
|
||||
pthread_cancel(encthread);
|
||||
pthread_join(encthread, NULL);
|
||||
DBG("Kill encoder thread");
|
||||
if(0 == pthread_cancel(encthread))
|
||||
pthread_join(encthread, NULL);
|
||||
DBG("close fd");
|
||||
close(encfd);
|
||||
encfd = -1;
|
||||
}
|
||||
@ -326,12 +367,14 @@ mcc_errcodes_t getMD(mountdata_t *d){
|
||||
* @return FALSE if failed
|
||||
*/
|
||||
int MountWriteRead(const data_t *out, data_t *in){
|
||||
if(!out && !in) return FALSE;
|
||||
if((!out && !in) || mntfd < 0) return FALSE;
|
||||
int ret = FALSE;
|
||||
pthread_mutex_lock(&mntmutex);
|
||||
if(out){
|
||||
if(out->len != write(mntfd, out->buf, out->len)) goto ext;
|
||||
write(mntfd, "\r", 1); // add EOL
|
||||
if(out->len != (size_t)write(mntfd, out->buf, out->len)) goto ext;
|
||||
//DBG("Send to mount %zd bytes: %s", out->len, out->buf);
|
||||
int g = write(mntfd, "\r", 1); // add EOL
|
||||
(void) g;
|
||||
}
|
||||
if(in){
|
||||
in->len = 0;
|
||||
|
||||
@ -26,10 +26,10 @@
|
||||
#define ENC_DATALEN (13)
|
||||
// max error counter (when read() returns -1)
|
||||
#define MAX_ERR_CTR (100)
|
||||
// encoder ticks per turn
|
||||
#define ENC_TURN_XTICKS (111111.)
|
||||
#define ENC_TURN_YTICKS (111111.)
|
||||
|
||||
double dtime();
|
||||
data_t *cmd2dat(const char *cmd);
|
||||
void data_free(data_t **x);
|
||||
int openEncoder(const char *path, int speed);
|
||||
int openMount(const char *path, int speed);
|
||||
void closeSerial();
|
||||
|
||||
@ -29,6 +29,7 @@ typedef enum{
|
||||
MCC_E_BADFORMAT, // wrong arguments of function
|
||||
MCC_E_ENCODERDEV, // encoder device error or can't open
|
||||
MCC_E_MOUNTDEV, // mount device error or can't open
|
||||
MCC_E_FAILED, // failed to run command - protocol error
|
||||
} mcc_errcodes_t;
|
||||
|
||||
typedef struct{
|
||||
@ -37,7 +38,7 @@ typedef struct{
|
||||
char* EncoderDevPath; // path to encoder device
|
||||
int EncoderDevSpeed; // serial speed
|
||||
int SepEncoder; // ==1 if encoder works as separate serial device
|
||||
suseconds_t MountReqInterval;// interval between subsequent mount requests (microseconds)
|
||||
double MountReqInterval; // maximal interval between subsequent mount requests (seconds)
|
||||
;
|
||||
} conf_t;
|
||||
|
||||
@ -65,7 +66,6 @@ typedef struct{
|
||||
coords_t motposition;
|
||||
coords_t encposition;
|
||||
coords_t lastmotposition;
|
||||
coords_t motspeed;
|
||||
uint8_t keypad;
|
||||
extradata_t extradata;
|
||||
uint32_t millis;
|
||||
@ -75,9 +75,11 @@ typedef struct{
|
||||
|
||||
// mount class
|
||||
typedef struct{
|
||||
mcc_errcodes_t (*init)(conf_t *c);
|
||||
void (*quit)();
|
||||
mcc_errcodes_t (*getMountData)(mountdata_t *d);
|
||||
mcc_errcodes_t (*init)(conf_t *c); // init device
|
||||
void (*quit)(); // deinit
|
||||
mcc_errcodes_t (*getMountData)(mountdata_t *d); // get last data
|
||||
mcc_errcodes_t (*moveTo)(double X, double Y); // move to given position ans stop
|
||||
mcc_errcodes_t (*emergStop)(); // emergency stop
|
||||
} mount_t;
|
||||
|
||||
extern mount_t Mount;
|
||||
|
||||
@ -16,13 +16,20 @@
|
||||
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <ctype.h>
|
||||
#include <inttypes.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "dbg.h"
|
||||
#include "serial.h"
|
||||
#include "ssii.h"
|
||||
|
||||
uint16_t SScalcChecksum(uint8_t *buf, int len){
|
||||
uint16_t checksum = 0;
|
||||
for(int i = 0; i < len; i++)
|
||||
for(int i = 0; i < len; i++){
|
||||
//DBG("data[%d]=0x%X", i, *buf);
|
||||
checksum += *buf++;
|
||||
}
|
||||
checksum ^= 0xFF00; // invert high byte
|
||||
//DBG("Checksum of %d bytes: 0x%04x", len, checksum);
|
||||
return checksum;
|
||||
@ -30,33 +37,32 @@ uint16_t SScalcChecksum(uint8_t *buf, int len){
|
||||
|
||||
// send short/long binary command
|
||||
static int bincmd(uint8_t *cmd, int len){
|
||||
data_t d;
|
||||
static data_t *dscmd = NULL, *dlcmd = NULL;
|
||||
if(!dscmd) dscmd = cmd2dat(CMD_SHORTCMD);
|
||||
if(!dlcmd) dlcmd = cmd2dat(CMD_LONGCMD);
|
||||
if(len == sizeof(SSscmd)){
|
||||
((SSscmd*)cmd)->checksum = SScalcChecksum(cmd, len-2);
|
||||
DBG("Short command");
|
||||
d.buf = CMD_SHORTCMD;
|
||||
d.len = sizeof(CMD_SHORTCMD) - 1;
|
||||
if(!MountWriteRead(&d, NULL)) return -1;
|
||||
if(!MountWriteRead(dscmd, NULL)) return -1;
|
||||
}else if(len == sizeof(SSlcmd)){
|
||||
((SSlcmd*)cmd)->checksum = SScalcChecksum(cmd, len-2);
|
||||
DBG("Long command");
|
||||
d.buf = CMD_LONGCMD;
|
||||
d.len = sizeof(CMD_LONGCMD) - 1;
|
||||
if(!MountWriteRead(&d, NULL)) return -1;
|
||||
if(!MountWriteRead(dlcmd, NULL)) return -1;
|
||||
}else{
|
||||
return -1;
|
||||
}
|
||||
DBG("Write %d bytes and wait for ans", len);
|
||||
data_t d;
|
||||
d.buf = cmd;
|
||||
d.len = d.maxlen = len;
|
||||
return MountWriteRead(&d, &d);
|
||||
}
|
||||
// return TRUE if OK
|
||||
int SScmdS(SSscmd *cmd){
|
||||
return bincmd(cmd, sizeof(SSscmd));
|
||||
return bincmd((uint8_t *)cmd, sizeof(SSscmd));
|
||||
}
|
||||
int SScmdL(SSlcmd *cmd){
|
||||
return bincmd(cmd, sizeof(SSlcmd));
|
||||
return bincmd((uint8_t *)cmd, sizeof(SSlcmd));
|
||||
}
|
||||
|
||||
|
||||
@ -65,6 +71,106 @@ int SScmdL(SSlcmd *cmd){
|
||||
* @param status (i) - just read data
|
||||
* @param mountdata (o) - output
|
||||
*/
|
||||
void SSconvstat(const SSstat *status, mountdata_t *mountdata){
|
||||
;
|
||||
void SSconvstat(const SSstat *s, mountdata_t *m, struct timeval *tdat){
|
||||
if(!s || !m || !tdat) return;
|
||||
/*
|
||||
#ifdef EBUG
|
||||
static double t0 = -1.;
|
||||
if(t0 < 0.) t0 = dtime();
|
||||
#endif
|
||||
DBG("Convert, t=%g", dtime()-t0);
|
||||
*/
|
||||
m->motposition.X = X_MOT2RAD(s->Xmot);
|
||||
m->motposition.Y = Y_MOT2RAD(s->Ymot);
|
||||
m->motposition.msrtime = *tdat;
|
||||
// fill encoder data from here, as there's no separate enc thread
|
||||
if(!Conf.SepEncoder){
|
||||
m->encposition.X = X_ENC2RAD(s->Xenc);
|
||||
m->encposition.Y = Y_ENC2RAD(s->Yenc);
|
||||
m->encposition.msrtime = *tdat;
|
||||
}
|
||||
m->lastmotposition.X = X_MOT2RAD(s->XLast);
|
||||
m->lastmotposition.Y = Y_MOT2RAD(s->YLast);
|
||||
m->lastmotposition.msrtime = *tdat;
|
||||
m->keypad = s->keypad;
|
||||
m->extradata.ExtraBits = s->ExtraBits;
|
||||
m->extradata.ain0 = s->ain0;
|
||||
m->extradata.ain1 = s->ain1;
|
||||
m->extradata.XBits = s->XBits;
|
||||
m->extradata.YBits = s->YBits;
|
||||
m->millis = s->millis;
|
||||
m->voltage = (double)s->voltage / 10.;
|
||||
m->temperature = ((double)s->tF - 32.) * 5. / 9.;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief SStextcmd - send simple text command to mount and return answer
|
||||
* @param cmd (i) - command to send
|
||||
* @param answer (o) - answer (or NULL)
|
||||
* @return
|
||||
*/
|
||||
int SStextcmd(const char *cmd, data_t *answer){
|
||||
if(!cmd){
|
||||
DBG("try to send empty command");
|
||||
return FALSE;
|
||||
}
|
||||
data_t d;
|
||||
d.buf = (uint8_t*) cmd;
|
||||
d.len = d.maxlen = strlen(cmd);
|
||||
DBG("send %zd bytes: %s", d.len, d.buf);
|
||||
return MountWriteRead(&d, answer);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief SSgetint - send text command and return integer answer
|
||||
* @param cmd (i) - command to send
|
||||
* @param ans (o) - intval (INT64_MAX if error)
|
||||
* @return FALSE if failed
|
||||
*/
|
||||
int SSgetint(const char *cmd, int64_t *ans){
|
||||
if(!cmd || !ans) return FALSE;
|
||||
uint8_t buf[64];
|
||||
data_t d = {.buf = buf, .len = 0, .maxlen = 64};
|
||||
if(!SStextcmd(cmd, &d)) return FALSE;
|
||||
int64_t retval = INT64_MAX;
|
||||
if(d.len > 1){
|
||||
char *ptr = (char*) buf;
|
||||
size_t i = 0;
|
||||
for(; i < d.len; ++i){
|
||||
if(isdigit(*ptr)) break;
|
||||
++ptr;
|
||||
}
|
||||
if(i < d.len) retval = atol(ptr);
|
||||
}
|
||||
DBG("read int: %" PRIi64, retval);
|
||||
*ans = retval;
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
// commands to move X and Y to given motor position in radians; @return FALSE if failed
|
||||
// BE CAREFUL: after each poweron X and Y are 0
|
||||
// BE CAREFUL: angle isn't checking here
|
||||
int SSXmoveto(double pos){
|
||||
char buf[64];
|
||||
int64_t target = X_RAD2MOT(pos);
|
||||
DBG("move to angle %grad = %ld", pos, target);
|
||||
snprintf(buf, 63, "%s%" PRIi64, CMD_MOTX, target);
|
||||
return SStextcmd(buf, NULL);
|
||||
}
|
||||
int SSYmoveto(double pos){
|
||||
char buf[64];
|
||||
int64_t target = Y_RAD2MOT(pos);
|
||||
DBG("move to angle %grad = %ld", pos, target);
|
||||
snprintf(buf, 63, "%s%" PRIi64, CMD_MOTY, target);
|
||||
return SStextcmd(buf, NULL);
|
||||
}
|
||||
|
||||
int SSemergStop(){
|
||||
int i = 0;
|
||||
for(; i < 10; ++i){
|
||||
if(!SStextcmd(CMD_EMSTOPX, NULL)) continue;
|
||||
if(SStextcmd(CMD_EMSTOPY, NULL)) break;
|
||||
}
|
||||
if(i == 10) return FALSE;
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
@ -18,10 +18,12 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <math.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include "sidservo.h"
|
||||
|
||||
#if 0
|
||||
// ASCII commands
|
||||
#define U8P(x) ((uint8_t*)x)
|
||||
// get binary data of all statistics
|
||||
@ -50,26 +52,70 @@
|
||||
#define CMD_GETYAEPR U8P("XXZ")
|
||||
// exit ASCII checksum mode
|
||||
#define CMD_EXITACM U8P("YXY0\r\xb8")
|
||||
#endif
|
||||
|
||||
// get binary data of all statistics
|
||||
#define CMD_GETSTAT ("XXS")
|
||||
// send short command
|
||||
#define CMD_SHORTCMD ("XXR")
|
||||
// send long command
|
||||
#define CMD_LONGCMD ("YXR")
|
||||
// get/set X/Y in motsteps
|
||||
#define CMD_MOTX ("X")
|
||||
#define CMD_MOTY ("Y")
|
||||
// -//- in encoders' ticks
|
||||
#define CMD_ENCX ("XZ")
|
||||
#define CMD_ENCY ("YZ")
|
||||
// normal stop X/Y
|
||||
#define CMD_STOPX ("XN")
|
||||
#define CMD_STOPY ("YN")
|
||||
// emergency stop
|
||||
#define CMD_EMSTOPX ("XG")
|
||||
#define CMD_EMSTOPY ("YG")
|
||||
// getters of motor's encoders per rev
|
||||
#define CMD_GETXMEPR ("XXU")
|
||||
#define CMD_GETYMEPR ("XXV")
|
||||
// -//- axis encoders
|
||||
#define CMD_GETXAEPR ("XXT")
|
||||
#define CMD_GETYAEPR ("XXZ")
|
||||
// exit ASCII checksum mode
|
||||
#define CMD_EXITACM ("YXY0\r\xb8")
|
||||
|
||||
|
||||
// timeout (seconds) of reading answer (from last symbol read)
|
||||
#define READTIMEOUT (0.05)
|
||||
|
||||
// steps per revolution; TODO: move to Conf
|
||||
#define HA_MOT_STEPSPERREV (4394294)
|
||||
#define DEC_MOT_STEPSPERREV (3325291)
|
||||
// steps per revolution
|
||||
#define X_MOT_STEPSPERREV (4394294.)
|
||||
#define Y_MOT_STEPSPERREV (3325291.)
|
||||
|
||||
// encoder's tolerance (ticks); TODO: move to Conf
|
||||
#define HAENCTOL (25)
|
||||
#define DECENCTOL (25)
|
||||
// motor position to radians and back
|
||||
#define X_MOT2RAD(n) (2.*M_PI * (double)n / X_MOT_STEPSPERREV)
|
||||
#define Y_MOT2RAD(n) (2.*M_PI * (double)n / Y_MOT_STEPSPERREV)
|
||||
#define X_RAD2MOT(r) ((uint32_t)(r / 2./M_PI * X_MOT_STEPSPERREV))
|
||||
#define Y_RAD2MOT(r) ((uint32_t)(r / 2./M_PI * Y_MOT_STEPSPERREV))
|
||||
|
||||
// encoder per revolution
|
||||
#define X_ENC_STEPSPERREV (67108863.)
|
||||
#define Y_ENC_STEPSPERREV (67108863.)
|
||||
// encoder position to radians and back
|
||||
#define X_ENC2RAD(n) (2.*M_PI * (double)n / X_ENC_STEPSPERREV)
|
||||
#define Y_ENC2RAD(n) (2.*M_PI * (double)n / Y_ENC_STEPSPERREV)
|
||||
#define X_RAD2ENC(r) ((uint32_t)(r / 2./M_PI * X_ENC_STEPSPERREV))
|
||||
#define Y_RAD2ENC(r) ((uint32_t)(r / 2./M_PI * Y_ENC_STEPSPERREV))
|
||||
|
||||
// encoder's tolerance (ticks)
|
||||
#define YencTOL (25.)
|
||||
#define XencTOL (25.)
|
||||
|
||||
|
||||
// all need data in one
|
||||
typedef struct{ // 41 bytes
|
||||
uint8_t ctrlAddr; // 0 a8 + controller address
|
||||
int32_t DECmot; // 1 Dec/HA motor position
|
||||
int32_t HAmot; // 5
|
||||
int32_t DECenc; // 9 Dec/HA encoder position
|
||||
int32_t HAenc; // 13
|
||||
int32_t Xmot; // 1 Dec/HA motor position
|
||||
int32_t Ymot; // 5
|
||||
int32_t Xenc; // 9 Dec/HA encoder position
|
||||
int32_t Yenc; // 13
|
||||
uint8_t keypad; // 17 keypad status
|
||||
uint8_t XBits; // 18
|
||||
uint8_t YBits; // 19
|
||||
@ -79,16 +125,16 @@ typedef struct{ // 41 bytes
|
||||
uint32_t millis; // 25 milliseconds clock
|
||||
int8_t tF; // 29 temperature (degF)
|
||||
uint8_t voltage; // 30 input voltage *10 (RA worm phase?)
|
||||
uint32_t DecLast; // 31 Alt/Dec motor location at last Alt/Dec scope encoder location change
|
||||
uint32_t HALast; // 35 Az/RA motor location at last Az/RA scope encoder location change
|
||||
uint32_t XLast; // 31 Alt/Dec motor location at last Alt/Dec scope encoder location change
|
||||
uint32_t YLast; // 35 Az/RA motor location at last Az/RA scope encoder location change
|
||||
uint16_t checksum; // 39 checksum, H inverted
|
||||
}__attribute__((packed)) SSstat;
|
||||
|
||||
typedef struct{
|
||||
int32_t DECmot; // 0 DEC motor position
|
||||
int32_t DECspeed; // 4 DEC speed
|
||||
int32_t HAmot; // 8
|
||||
int32_t HAspeed; // 12
|
||||
int32_t Xmot; // 0 DEC motor position
|
||||
int32_t Xspeed; // 4 DEC speed
|
||||
int32_t Ymot; // 8
|
||||
int32_t Yspeed; // 12
|
||||
uint8_t xychange; // 16 change Xbits/Ybits value
|
||||
uint8_t XBits; // 17
|
||||
uint8_t YBits; // 18
|
||||
@ -96,18 +142,23 @@ typedef struct{
|
||||
} __attribute__((packed)) SSscmd; // short command
|
||||
|
||||
typedef struct{
|
||||
int32_t DECmot; // 0 DEC motor position
|
||||
int32_t DECspeed; // 4 DEC speed
|
||||
int32_t HAmot; // 8
|
||||
int32_t HAspeed; // 12
|
||||
int32_t DECadder; // 16 - DEC adder
|
||||
int32_t HAadder; // 20
|
||||
int32_t DECatime; // 24 DEC adder time
|
||||
int32_t HAatime; // 28
|
||||
int32_t Xmot; // 0 DEC motor position
|
||||
int32_t Xspeed; // 4 DEC speed
|
||||
int32_t Ymot; // 8
|
||||
int32_t Yspeed; // 12
|
||||
int32_t Xadder; // 16 - DEC adder
|
||||
int32_t Yadder; // 20
|
||||
int32_t Xatime; // 24 DEC adder time
|
||||
int32_t Yatime; // 28
|
||||
uint16_t checksum; // 32
|
||||
} __attribute__((packed)) SSlcmd; // long command
|
||||
|
||||
int SScmdS(SSscmd *cmd);
|
||||
int SScmdL(SSlcmd *cmd);
|
||||
uint16_t SScalcChecksum(uint8_t *buf, int len);
|
||||
void SSconvstat(const SSstat *status, mountdata_t *mountdata);
|
||||
void SSconvstat(const SSstat *status, mountdata_t *mountdata, struct timeval *tdat);
|
||||
int SStextcmd(const char *cmd, data_t *answer);
|
||||
int SSgetint(const char *cmd, int64_t *ans);
|
||||
int SSXmoveto(double pos);
|
||||
int SSYmoveto(double pos);
|
||||
int SSemergStop();
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user