eddy/mountcontrol
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

shortcmd checked

Browse Source
This commit is contained in:
Edward V. Emelianov 2025-02-16 21:59:08 +03:00
parent 53b18299dc
commit 5250aa185d
13 changed files with 281 additions and 94 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
LibSidServo/examples/CMakeLists.txt
View File

@ -5,5 +5,6 @@ include_directories(../)
link_libraries(sidservo usefull_macros)
# exe list
add_executable(goto goto.c dump.c)
add_executable(dump dumpmoving.c dump.c)
add_executable(dump_s dumpmoving_scmd.c dump.c)

32
LibSidServo/examples/dump.c
View File

@ -29,10 +29,9 @@
* @param m - mount data
*/
void logmnt(FILE *fcoords, mountdata_t *m){
if(!fcoords || !m) return;
DBG("LOG");
static double t0 = -1.;
if(!fcoords) return;
//DBG("LOG %s", m ? "data" : "header");
static double t0 = -1.;
if(!m){ // write header
fprintf(fcoords, "# time Xmot(deg) Ymot(deg) Xenc(deg) Yenc(deg) millis T V\n");
return;
@ -73,7 +72,7 @@ void dumpmoving(FILE *fcoords, double t, int N){
usleep(10000);
if(MCC_E_OK != Mount.getMountData(&mdata)){ WARNX("Can't get data"); continue;}
if(mdata.millis == millis) continue;
DBG("Got new data, posX=%g, posY=%g", mdata.motposition.X, mdata.motposition.Y);
//DBG("Got new data, posX=%g, posY=%g", mdata.motposition.X, mdata.motposition.Y);
millis = mdata.millis;
if(fcoords) logmnt(fcoords, &mdata);
if(mdata.motposition.X != xlast || mdata.motposition.Y != ylast){
@ -104,3 +103,28 @@ void waitmoving(int N){
}else ++ctr;
}
}
/**
* @brief getMotPos - get current
* @param mot (o) - motor position (or NULL)
* @param Y (o) - encoder position (or NULL)
* @return FALSE if failed
*/
int getPos(coords_t *mot, coords_t *enc){
mountdata_t mdata;
int errcnt = 0;
do{
if(MCC_E_OK != Mount.getMountData(&mdata)) ++errcnt;
else{
errcnt = 0;
if(mdata.millis) break;
}
}while(errcnt < 10);
if(errcnt >= 10){
WARNX("Can't read mount status");
return FALSE;
}
if(mot) *mot = mdata.motposition;
if(enc) *enc = mdata.encposition;
return TRUE;
}

1
LibSidServo/examples/dump.h
View File

@ -25,3 +25,4 @@
void logmnt(FILE *fcoords, mountdata_t *m);
void dumpmoving(FILE *fcoords, double t, int N);
void waitmoving(int N);
int getPos(coords_t *mot, coords_t *enc);

77
LibSidServo/examples/dumpmoving_scmd.c
View File

@ -28,7 +28,6 @@
#include "dump.h"
#include "sidservo.h"
#include "simpleconv.h"
#include "ssii.h"
typedef struct{
int help;
@ -72,14 +71,32 @@ static void *dumping(void _U_ *u){
return NULL;
}
// return TRUE if motor position is reached +-2 counts
#define X2count (X_RAD2MOT(2))
#define Y2count (Y_RAD2MOT(2))
static int Xreached(double xtag){
// return TRUE if motor position is reached +- 0.1 degrees
#define X2count (DEG2RAD(0.1))
#define Y2count (DEG2RAD(0.1))
static int WaitX(double xtag){
mountdata_t mdata;
if(MCC_E_OK != Mount.getMountData(&mdata)) return FALSE;
if(fabs(mdata.motposition.X - xtag) <= X2count) return TRUE;
return FALSE;
red("Wait for %g degrees\n", RAD2DEG(xtag));
int errcnt = 0;
double sign = 0.;
uint32_t millis = 0;
do{
if(MCC_E_OK != Mount.getMountData(&mdata)) ++errcnt;
else{
errcnt = 0;
if(mdata.millis == millis) continue;
millis = mdata.millis;
if(sign == 0.) sign = (mdata.motposition.X > xtag) ? 1. : -1.;
printf("X=%g deg, need %g deg; delta=%g arcmin\n", RAD2DEG(mdata.motposition.X),
RAD2DEG(xtag), RAD2DEG(sign*(mdata.motposition.X - xtag))*60.);
}
}while(sign*(mdata.motposition.X - xtag) > X2count && errcnt < 10);
if(errcnt >= 10){
WARNX("Too much errors");
return FALSE;
}
green("X reached position %g degrees\n", RAD2DEG(xtag));
return TRUE;
}
/*
static int Yreached(double ytag){
@ -90,6 +107,18 @@ static int Yreached(double ytag){
}
*/
// check current position and go to 0 if non-zero
static void chk0(){
coords_t M;
if(!getPos(&M, NULL)) signals(2);
if(M.X || M.Y){
WARNX("Mount position isn't @ zero; moving");
Mount.moveTo(0., 0.);
waitmoving(G.Ncycles);
green("Now mount @ zero\n");
}
}
int main(int argc, char **argv){
sl_init();
sl_parseargs(&argc, &argv, cmdlnopts);
@ -98,7 +127,6 @@ int main(int argc, char **argv){
if(!(fcoords = fopen(G.coordsoutput, "w")))
ERRX("Can't open %s", G.coordsoutput);
}else fcoords = stdout;
logmnt(fcoords, NULL);
mcc_errcodes_t e = Mount.init(&Config);
if(e != MCC_E_OK){
WARNX("Can't init devices");
@ -113,34 +141,37 @@ int main(int argc, char **argv){
// start with 1degr/s, increase to 15, decrease to 1
short_command_t cmd = {0};
pthread_t dthr;
if(pthread_create(&dthr, NULL, dumping, NULL)) ERRX("Can't run dump thread");
#define SCMD() do{if(MCC_E_OK != Mount.shortCmd(&cmd)) ERRX("Can't run command"); }while(0)
// goto 5 degr with 1deg/s
chk0();
logmnt(fcoords, NULL);
if(pthread_create(&dthr, NULL, dumping, NULL)) ERRX("Can't run dump thread");
// goto 10 degr with 1deg/s
cmd.Xmot = DEG2RAD(30.);
cmd.Xspeed = DEG2RAD(1.);
SCMD();
while(!Xreached(DEG2RAD(5.)));
// goto 15 degr with 5deg/s
if(!WaitX(DEG2RAD(10.))) signals(9);
// goto 20 degr with 5deg/s
cmd.Xmot = DEG2RAD(30.);
cmd.Xspeed = DEG2RAD(2.);
SCMD();
if(!WaitX(DEG2RAD(20.))) signals(9);
// goto 30 degr with 15deg/s
cmd.Xmot = DEG2RAD(40.);
cmd.Xspeed = DEG2RAD(5.);
SCMD();
while(!Xreached(DEG2RAD(10.)));
// goto 25 degr with 15deg/s
cmd.Xmot = DEG2RAD(30.);
cmd.Xspeed = DEG2RAD(15.);
SCMD();
while(!Xreached(DEG2RAD(25.)));
// goto 29.9 degr with 1deg/s
cmd.Xmot = DEG2RAD(30.);
if(!WaitX(DEG2RAD(30.))) signals(9);
// goto 40 degr with 1deg/s
cmd.Xmot = DEG2RAD(40.);
cmd.Xspeed = DEG2RAD(1.);
SCMD();
while(!Xreached(DEG2RAD(29.9)));
if(!WaitX(DEG2RAD(40.))) signals(9);
// and go back with 15deg/s
cmd.Xmot = DEG2RAD(0.);
cmd.Xspeed = DEG2RAD(15.);
SCMD();
while(!Xreached(DEG2RAD(0.1)));
if(!WaitX(DEG2RAD(0.))) signals(9);
Mount.moveTo(0., 0.);
waitmoving(10);
// wait moving ends
pthread_join(dthr, NULL);
#undef SCMD

91
LibSidServo/examples/goto.c Normal file
View File

@ -0,0 +1,91 @@
/*
* This file is part of the libsidservo project.
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
// move telescope to given MOTOR position in degrees
#include <signal.h>
#include <stdio.h>
#include <time.h>
#include <usefull_macros.h>
#include "dump.h"
#include "sidservo.h"
#include "simpleconv.h"
typedef struct{
int help;
int Ncycles;
double X;
double Y;
} parameters;
static parameters G = {
.Ncycles = 40,
.X = NAN,
.Y = NAN,
};
static sl_option_t cmdlnopts[] = {
{"help", NO_ARGS, NULL, 'h', arg_int, APTR(&G.help), "show this help"},
{"ncycles", NEED_ARG, NULL, 'n', arg_int, APTR(&G.Ncycles), "N cycles of waiting in stopped state (default: 40)"},
{"newx", NEED_ARG, NULL, 'X', arg_double, APTR(&G.X), "New X coordinate"},
{"newy", NEED_ARG, NULL, 'Y', arg_double, APTR(&G.Y), "New Y coordinate"},
end_option
};
static conf_t Config = {
.MountDevPath = "/dev/ttyUSB0",
.MountDevSpeed = 19200,
//.EncoderDevPath = "/dev/ttyUSB1",
//.EncoderDevSpeed = 153000,
.MountReqInterval = 0.1,
.SepEncoder = 0
};
void signals(int sig){
if(sig){
signal(sig, SIG_IGN);
DBG("Get signal %d, quit.\n", sig);
}
Mount.quit();
exit(sig);
}
int main(int _U_ argc, char _U_ **argv){
sl_init();
sl_parseargs(&argc, &argv, cmdlnopts);
if(G.help) sl_showhelp(-1, cmdlnopts);
if(MCC_E_OK != Mount.init(&Config)) ERRX("Can't init mount");
coords_t M;
if(!getPos(&M, NULL)) ERRX("Can't get current position");
printf("Mount position: X=%g, Y=%g\n", RAD2DEG(M.X), RAD2DEG(M.Y));
if(isnan(G.X) && isnan(G.Y)){
Mount.quit();
return 0;
}
if(isnan(G.X)) G.X = RAD2DEG(M.X);
if(isnan(G.Y)) G.Y = RAD2DEG(M.Y);
printf("Moving to X=%g deg, Y=%g deg\n", G.X, G.Y);
Mount.moveTo(DEG2RAD(G.X), DEG2RAD(G.Y));
sleep(1);
waitmoving(G.Ncycles);
if(!getPos(&M, NULL)) WARNX("Can't get current position");
else printf("New mount position: X=%g, Y=%g\n", RAD2DEG(M.X), RAD2DEG(M.Y));
Mount.quit();
return 0;
}

2
LibSidServo/libsidservo.creator.user
View File

@ -1,6 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?>
<!DOCTYPE QtCreatorProject>
<!-- Written by QtCreator 15.0.0, 2025-02-11T22:41:16. -->
<!-- Written by QtCreator 15.0.1, 2025-02-16T21:56:40. -->
<qtcreator>
<data>
<variable>EnvironmentId</variable>

1
LibSidServo/libsidservo.files
View File

@ -4,6 +4,7 @@ examples/dump.c
examples/dump.h
examples/dumpmoving.c
examples/dumpmoving_scmd.c
examples/goto.c
examples/simpleconv.h
main.c
sidservo.h

6
LibSidServo/main.c
View File

@ -101,6 +101,7 @@ static mcc_errcodes_t emstop(){
static mcc_errcodes_t shortcmd(short_command_t *cmd){
if(!cmd) return MCC_E_BADFORMAT;
SSscmd s = {0};
DBG("xmot=%g, ymot=%g", cmd->Xmot, cmd->Ymot);
s.Xmot = X_RAD2MOT(cmd->Xmot);
s.Ymot = Y_RAD2MOT(cmd->Ymot);
s.Xspeed = X_RS2MOTSPD(cmd->Xspeed);
@ -108,7 +109,8 @@ static mcc_errcodes_t shortcmd(short_command_t *cmd){
s.xychange = cmd->xychange;
s.XBits = cmd->XBits;
s.YBits = cmd->YBits;
if(SScmdS(&s)) return MCC_E_FAILED;
DBG("X->%d, Y->%d, Xs->%d, Ys->%d", s.Xmot, s.Ymot, s.Xspeed, s.Yspeed);
if(!cmdS(&s)) return MCC_E_FAILED;
cmd->Xmot = X_MOT2RAD(s.Xmot);
cmd->Ymot = Y_MOT2RAD(s.Ymot);
cmd->Xspeed = X_MOTSPD2RS(s.Xspeed);
@ -135,7 +137,7 @@ static mcc_errcodes_t longcmd(long_command_t *cmd){
l.Yadder = Y_RS2MOTSPD(cmd->Yadder);
l.Xatime = S2ADDER(cmd->Xatime);
l.Yatime = S2ADDER(cmd->Yatime);
if(SScmdL(&l)) return MCC_E_FAILED;
if(!cmdL(&l)) return MCC_E_FAILED;
cmd->Xmot = X_MOT2RAD(l.Xmot);
cmd->Ymot = Y_MOT2RAD(l.Ymot);
cmd->Xspeed = X_MOTSPD2RS(l.Xspeed);

104
LibSidServo/serial.c
View File

@ -32,7 +32,6 @@
#include "dbg.h"
#include "serial.h"
#include "ssii.h"
// serial devices FD
static int encfd = -1, mntfd = -1;
@ -338,16 +337,14 @@ int openMount(const char *path, int speed){
void closeSerial(){
if(mntfd > -1){
DBG("Kill mount thread");
if(0 == pthread_cancel(mntthread))
pthread_join(mntthread, NULL);
pthread_cancel(mntthread);
DBG("close fd");
close(mntfd);
mntfd = -1;
}
if(encfd > -1){
DBG("Kill encoder thread");
if(0 == pthread_cancel(encthread))
pthread_join(encthread, NULL);
pthread_cancel(encthread);
DBG("close fd");
close(encfd);
encfd = -1;
@ -363,21 +360,19 @@ mcc_errcodes_t getMD(mountdata_t *d){
return MCC_E_OK;
}
/**
* @brief MountWriteRead - write and read @ once (or only read/write)
* @param out (o) - data to write or NULL if not need
* @param in (i) - data to read or NULL if not need
* @return FALSE if failed
*/
int MountWriteRead(const data_t *out, data_t *in){
// 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;
int ret = FALSE;
pthread_mutex_lock(&mntmutex);
if(out){
if(out->len != (size_t)write(mntfd, out->buf, out->len)) goto ext;
if(out->len != (size_t)write(mntfd, out->buf, out->len)){
DBG("written bytes not equal to need");
return FALSE;
}
//DBG("Send to mount %zd bytes: %s", out->len, out->buf);
int g = write(mntfd, "\r", 1); // add EOL
(void) g;
if(needeol){
int g = write(mntfd, "\r", 1); // add EOL
(void) g;
}
}
if(in){
in->len = 0;
@ -387,8 +382,79 @@ int MountWriteRead(const data_t *out, data_t *in){
in->buf[in->len++] = (uint8_t) b;
}
}
ret = TRUE;
ext:
return TRUE;
}
/**
* @brief MountWriteRead - write and read @ once (or only read/write)
* @param out (o) - data to write or NULL if not need
* @param in (i) - data to read or NULL if not need
* @return FALSE if failed
*/
int MountWriteRead(const data_t *out, data_t *in){
pthread_mutex_lock(&mntmutex);
int ret = wr(out, in, 1);
pthread_mutex_unlock(&mntmutex);
return ret;
}
#ifdef EBUG
static void logscmd(SSscmd *c){
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);
if(c->checksum != SScalcChecksum((uint8_t*)c, sizeof(SSscmd)-2)) printf("Checksum failed\n");
else printf("Checksum OK\n");
}
static void loglcmd(SSlcmd *c){
printf("Xmot=%d, Ymot=%d, Xspeed=%d, Yspeed=%d\n", c->Xmot, c->Ymot, c->Xspeed, c->Yspeed);
printf("Xadder=%d, Yadder=%d, Xatime=%d, Yatime=%d\n", c->Xadder, c->Yadder, c->Xatime, c->Yatime);
if(c->checksum != SScalcChecksum((uint8_t*)c, sizeof(SSlcmd)-2)) printf("Checksum failed\n");
else printf("Checksum OK\n");
}
#endif
// send short/long binary command; return FALSE if failed
static int bincmd(uint8_t *cmd, int len){
static data_t *dscmd = NULL, *dlcmd = NULL;
if(!dscmd) dscmd = cmd2dat(CMD_SHORTCMD);
if(!dlcmd) dlcmd = cmd2dat(CMD_LONGCMD);
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};
if(len == sizeof(SSscmd)){
((SSscmd*)cmd)->checksum = SScalcChecksum(cmd, len-2);
DBG("Short command");
logscmd((SSscmd*)cmd);
if(!wr(dscmd, &a, 1)) goto rtn;
}else if(len == sizeof(SSlcmd)){
((SSlcmd*)cmd)->checksum = SScalcChecksum(cmd, len-2);
DBG("Long command");
loglcmd((SSlcmd*)cmd);
if(!wr(dlcmd, &a, 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;
ret = wr(&d, &d, 0);
#ifdef EBUG
if(len == sizeof(SSscmd)) logscmd((SSscmd*)cmd);
else loglcmd((SSlcmd*)cmd);
#endif
DBG("%s", ret ? "SUCCESS" : "FAIL");
rtn:
pthread_mutex_unlock(&mntmutex);
return ret;
}
// return TRUE if OK
int cmdS(SSscmd *cmd){
return bincmd((uint8_t *)cmd, sizeof(SSscmd));
}
int cmdL(SSlcmd *cmd){
return bincmd((uint8_t *)cmd, sizeof(SSlcmd));
}

3
LibSidServo/serial.h
View File

@ -19,6 +19,7 @@
#pragma once
#include "sidservo.h"
#include "ssii.h"
// magick starting sequence
#define ENC_MAGICK (204)
@ -35,3 +36,5 @@ int openMount(const char *path, int speed);
void closeSerial();
mcc_errcodes_t getMD(mountdata_t *d);
int MountWriteRead(const data_t *out, data_t *in);
int cmdS(SSscmd *cmd);
int cmdL(SSlcmd *cmd);

24
LibSidServo/sidservo.h
View File

@ -74,24 +74,24 @@ typedef struct{
} mountdata_t;
typedef struct{
int32_t Xmot; // 0 X motor position (rad)
int32_t Xspeed; // 4 X speed (rad/s)
int32_t Ymot; // 8
int32_t Yspeed; // 12
double Xmot; // 0 X motor position (rad)
double Xspeed; // 4 X speed (rad/s)
double Ymot; // 8
double Yspeed; // 12
uint8_t xychange; // 16 change Xbits/Ybits value
uint8_t XBits; // 17
uint8_t YBits; // 18
} short_command_t; // short command
typedef struct{
int32_t Xmot; // 0 X motor position (rad)
int32_t Xspeed; // 4 X speed (rad/s)
int32_t Ymot; // 8
int32_t Yspeed; // 12
int32_t Xadder; // 16 - X adder (rad/s)
int32_t Yadder; // 20
int32_t Xatime; // 24 X adder time, sec
int32_t Yatime; // 28
double Xmot; // 0 X motor position (rad)
double Xspeed; // 4 X speed (rad/s)
double Ymot; // 8
double Yspeed; // 12
double Xadder; // 16 - X adder (rad/s)
double Yadder; // 20
double Xatime; // 24 X adder time, sec
double Yatime; // 28
} long_command_t; // long command
// mount class

31
LibSidServo/ssii.c
View File

@ -35,37 +35,6 @@ uint16_t SScalcChecksum(uint8_t *buf, int len){
return checksum;
}
// send short/long binary command
static int bincmd(uint8_t *cmd, int len){
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");
if(!MountWriteRead(dscmd, NULL)) return -1;
}else if(len == sizeof(SSlcmd)){
((SSlcmd*)cmd)->checksum = SScalcChecksum(cmd, len-2);
DBG("Long command");
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((uint8_t *)cmd, sizeof(SSscmd));
}
int SScmdL(SSlcmd *cmd){
return bincmd((uint8_t *)cmd, sizeof(SSlcmd));
}
/**
* @brief SSconvstat - convert stat from SSII format to human
* @param status (i) - just read data

2
LibSidServo/ssii.h
View File

@ -159,8 +159,6 @@ typedef struct{
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, struct timeval *tdat);
int SStextcmd(const char *cmd, data_t *answer);