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

update pos, statuses

Browse Source
This commit is contained in:
Edward V. Emelianov 2025-07-29 22:11:42 +03:00
parent 89b86f8b7f
commit c862d160fe
11 changed files with 229 additions and 71 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
LibSidServo/TODO
View File

@ -1,15 +1,3 @@
1. WTF after closing?
closeSerial (/home/eddy/C-files/LibSidServo/serial.c, line 484): close fd
closeSerial (/home/eddy/C-files/LibSidServo/serial.c, line 489): Kill encoder thread
closeSerial (/home/eddy/C-files/LibSidServo/serial.c, line 491): close fd
*** bit out of range 0 - FD_SETSIZE on fd_set ***: terminated
Aborted
2. Init: set "motors" positions due to "encoders" position
3. From time to time: repeat 2
1. PID: slew2
2. add model & config "model ON"

4
LibSidServo/dbg.h
View File

@ -59,6 +59,6 @@ extern conf_t Conf;
fprintf(stderr, "\n");} while(0)
#else // EBUG
#define FNAME() do{}while(0)
#define DBG(...) do{}while(0)
#define FNAME()
#define DBG(...)
#endif // EBUG

82
LibSidServo/examples/SSIIconf.c
View File

@ -16,6 +16,7 @@
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include <math.h>
#include <stdio.h>
#include <usefull_macros.h>
@ -50,6 +51,83 @@ static sl_option_t confopts[] = {
end_option
};
static void dumpaxe(char axe, axe_config_t *c){
#define STRUCTPAR(p) (c)->p
#define DUMP(par) do{printf("%c%s=%g\n", axe, #par, STRUCTPAR(par));}while(0)
#define DUMPD(par) do{printf("%c%s=%g\n", axe, #par, RAD2DEG(STRUCTPAR(par)));}while(0)
DUMPD(accel);
DUMPD(backlash);
DUMPD(errlimit);
DUMP(propgain);
DUMP(intgain);
DUMP(derivgain);
DUMP(outplimit);
DUMP(currlimit);
DUMP(intlimit);
#undef DUMP
#undef DUMPD
}
static void dumpxbits(xbits_t *c){
#define DUMPBIT(f) do{printf("X%s=%d\n", #f, STRUCTPAR(f));}while(0)
DUMPBIT(motrev);
DUMPBIT(motpolarity);
DUMPBIT(encrev);
DUMPBIT(dragtrack);
DUMPBIT(trackplat);
DUMPBIT(handpaden);
DUMPBIT(newpad);
DUMPBIT(guidemode);
#undef DUMPBIT
}
static void dumpybits(ybits_t *c){
#define DUMPBIT(f) do{printf("Y%s=%d\n", #f, STRUCTPAR(f));}while(0)
DUMPBIT(motrev);
DUMPBIT(motpolarity);
DUMPBIT(encrev);
DUMPBIT(slewtrack);
DUMPBIT(digin_sens);
printf("Ydigin=%d\n", c->digin);
#undef DUMPBIT
}
static void dumpHWconf(){
#undef STRUCTPAR
#define STRUCTPAR(p) (HW).p
#define DUMP(par) do{printf("%s=%g\n", #par, STRUCTPAR(par));}while(0)
#define DUMPD(par) do{printf("%s=%g\n", #par, RAD2DEG(STRUCTPAR(par)));}while(0)
#define DUMPU8(par) do{printf("%s=%u\n", #par, (uint8_t)STRUCTPAR(par));}while(0)
#define DUMPU32(par) do{printf("%s=%u\n", #par, (uint32_t)STRUCTPAR(par));}while(0)
green("X axe configuration:\n");
dumpaxe('X', &HW.Xconf);
green("X bits:\n");
dumpxbits(&HW.xbits);
green("Y axe configuration:\n");
dumpaxe('Y', &HW.Yconf);
green("Y bits:\n");
dumpybits(&HW.ybits);
printf("address=%d\n", HW.address);
DUMP(eqrate);
DUMP(eqadj);
DUMP(trackgoal);
DUMPD(latitude);
DUMPU32(Xsetpr);
DUMPU32(Ysetpr);
DUMPU32(Xmetpr);
DUMPU32(Ymetpr);
DUMPD(Xslewrate);
DUMPD(Yslewrate);
DUMPD(Xpanrate);
DUMPD(Ypanrate);
DUMPD(Xguiderate);
DUMPD(Yguiderate);
DUMPU32(baudrate);
DUMPD(locsdeg);
DUMPD(locsspeed);
DUMPD(backlspd);
}
int main(int argc, char** argv){
sl_init();
sl_parseargs(&argc, &argv, cmdlnopts);
@ -68,9 +146,11 @@ int main(int argc, char** argv){
green("Got configuration:\n");
printf("%s\n", c);
FREE(c);
dumpHWconf();
/*
if(G.hwconffile && G.writeconf){
;
}
}*/
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 17.0.0, 2025-07-23T22:48:05. -->
<!-- Written by QtCreator 17.0.0, 2025-07-29T22:07:00. -->
<qtcreator>
<data>
<variable>EnvironmentId</variable>

54
LibSidServo/main.c
View File

@ -77,25 +77,7 @@ static mcc_errcodes_t init(conf_t *c){
// read input data as there may be some trash on start
if(!SSrawcmd(CMD_EXITACM, &d)) ret = MCC_E_FAILED;
if(ret != MCC_E_OK) return ret;
mountdata_t md = {0};
ret = MCC_E_FATAL;
double t0 = dtime(), t = 0.;
do{
t = dtime();
if(MCC_E_OK == getMD(&md)){
if(fabs(md.encXposition.t - t) < 0.1 && fabs(md.encYposition.t - t) < 0.1){
DBG("FIX motors position to encoders");
int32_t Xpos = X_RAD2MOT(md.encXposition.val), Ypos = Y_RAD2MOT(md.encYposition.val);
if(SSsetterI(CMD_MOTXSET, Xpos) && SSsetterI(CMD_MOTYSET, Ypos)){
DBG("All OK");
ret = MCC_E_OK;
break;
}
}
}
DBG("NO DATA; dt = %g", t - t0);
}while(t - t0 < 2.);
return ret;
return updateMotorPos();
}
// check coordinates (rad) and speeds (rad/s); return FALSE if failed
@ -120,6 +102,10 @@ static int chkYs(double s){
static mcc_errcodes_t slew2(const coordpair_t *target, slewflags_t flags){
(void)target;
(void)flags;
if(MCC_E_OK != updateMotorPos()) return MCC_E_FAILED;
//...
setStat(MNT_SLEWING, MNT_SLEWING);
//...
return MCC_E_FAILED;
}
@ -133,13 +119,17 @@ static mcc_errcodes_t slew2(const coordpair_t *target, slewflags_t flags){
static mcc_errcodes_t move2(const coordpair_t *target){
if(!target) return MCC_E_BADFORMAT;
if(!chkX(target->X) || !chkY(target->Y)) return MCC_E_BADFORMAT;
if(MCC_E_OK != updateMotorPos()) return MCC_E_FAILED;
short_command_t cmd = {0};
DBG("x,y: %g, %g", target->X, target->Y);
cmd.Xmot = target->X;
cmd.Ymot = target->Y;
cmd.Xspeed = MCC_MAX_X_SPEED;
cmd.Yspeed = MCC_MAX_Y_SPEED;
return shortcmd(&cmd);
mcc_errcodes_t r = shortcmd(&cmd);
if(r != MCC_E_OK) return r;
setStat(MNT_SLEWING, MNT_SLEWING);
return MCC_E_OK;
}
/**
@ -167,12 +157,16 @@ static mcc_errcodes_t move2s(const coordpair_t *target, const coordpair_t *speed
if(!target || !speed) return MCC_E_BADFORMAT;
if(!chkX(target->X) || !chkY(target->Y)) return MCC_E_BADFORMAT;
if(!chkXs(speed->X) || !chkYs(speed->Y)) return MCC_E_BADFORMAT;
if(MCC_E_OK != updateMotorPos()) return MCC_E_FAILED;
short_command_t cmd = {0};
cmd.Xmot = target->X;
cmd.Ymot = target->Y;
cmd.Xspeed = speed->X;
cmd.Yspeed = speed->Y;
return shortcmd(&cmd);
mcc_errcodes_t r = shortcmd(&cmd);
if(r != MCC_E_OK) return r;
setStat(MNT_SLEWING, MNT_SLEWING);
return MCC_E_OK;
}
/**
@ -266,14 +260,14 @@ static mcc_errcodes_t get_hwconf(hardware_configuration_t *hwConfig){
hwConfig->address = config.address;
// TODO: What to do with eqrate, eqadj and trackgoal?
config.latitude = __bswap_16(config.latitude);
// Convert latitude (degrees * 100 to radians)
hwConfig->latitude = (double)config.latitude / 100.0 * M_PI / 180.0;
hwConfig->latitude = ((double)config.latitude) / 100.0 * M_PI / 180.0;
// Copy ticks per revolution
hwConfig->Xsetpr = config.Xsetpr;
hwConfig->Ysetpr = config.Ysetpr;
hwConfig->Xmetpr = config.Xmetpr;
hwConfig->Ymetpr = config.Ymetpr;
hwConfig->Xsetpr = __bswap_32(config.Xsetpr);
hwConfig->Ysetpr = __bswap_32(config.Ysetpr);
hwConfig->Xmetpr = __bswap_32(config.Xmetpr);
hwConfig->Ymetpr = __bswap_32(config.Ymetpr);
// Convert slew rates (ticks per loop to rad/s)
hwConfig->Xslewrate = X_MOTSPD2RS(config.Xslewrate);
hwConfig->Yslewrate = Y_MOTSPD2RS(config.Yslewrate);
@ -325,7 +319,7 @@ static mcc_errcodes_t write_hwconf(hardware_configuration_t *hwConfig){
config.xbits = hwConfig->xbits;
config.ybits = hwConfig->ybits;
// Convert latitude (radians to degrees * 100)
config.latitude = (uint16_t)(hwConfig->latitude * 180.0 / M_PI * 100.0);
config.latitude = __bswap_16((uint16_t)(hwConfig->latitude * 180.0 / M_PI * 100.0));
// Convert slew rates (rad/s to ticks per loop)
config.Xslewrate = X_RS2MOTSPD(hwConfig->Xslewrate);
config.Yslewrate = Y_RS2MOTSPD(hwConfig->Yslewrate);
@ -341,6 +335,10 @@ static mcc_errcodes_t write_hwconf(hardware_configuration_t *hwConfig){
config.locsspeed = (uint32_t)(hwConfig->locsspeed * 180.0 * 3600.0 / M_PI);
// Convert backlash speed (rad/s to ticks per loop)
config.backlspd = X_RS2MOTSPD(hwConfig->backlspd);
config.Xsetpr = __bswap_32(hwConfig->Xsetpr);
config.Ysetpr = __bswap_32(hwConfig->Ysetpr);
config.Xmetpr = __bswap_32(hwConfig->Xmetpr);
config.Ymetpr = __bswap_32(hwConfig->Ymetpr);
// TODO - next
(void) config;
return MCC_E_OK;

26
LibSidServo/serial.c
View File

@ -56,9 +56,19 @@ typedef struct __attribute__((packed)){
} enc_t;
/**
* @brief dtime - UNIX time with microsecond
* @return value
* @brief dtime - monotonic time from first run
* @return
*/
double dtime(){
struct timespec start_time = {0}, cur_time;
if(start_time.tv_sec == 0 && start_time.tv_nsec == 0){
clock_gettime(CLOCK_MONOTONIC, &start_time);
}
clock_gettime(CLOCK_MONOTONIC, &cur_time);
return ((double)(cur_time.tv_sec - start_time.tv_sec) +
(cur_time.tv_nsec - start_time.tv_nsec) * 1e-9);
}
#if 0
double dtime(){
double t;
struct timeval tv;
@ -66,6 +76,7 @@ double dtime(){
t = tv.tv_sec + ((double)tv.tv_usec)/1e6;
return t;
}
#endif
#if 0
double tv2d(struct timeval *tv){
if(!tv) return 0.;
@ -522,6 +533,13 @@ mcc_errcodes_t getMD(mountdata_t *d){
return MCC_E_OK;
}
void setStat(mnt_status_t Xstatus, mnt_status_t Ystatus){
pthread_mutex_lock(&datamutex);
mountdata.Xstatus = Xstatus;
mountdata.Ystatus = Ystatus;
pthread_mutex_unlock(&datamutex);
}
// 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;
@ -598,12 +616,16 @@ static int bincmd(uint8_t *cmd, int len){
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;
}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;
}else{
goto rtn;

1
LibSidServo/serial.h
View File

@ -35,6 +35,7 @@ int openEncoder();
int openMount();
void closeSerial();
mcc_errcodes_t getMD(mountdata_t *d);
void setStat(mnt_status_t Xstatus, mnt_status_t Ystatus);
int MountWriteRead(const data_t *out, data_t *in);
int MountWriteReadRaw(const data_t *out, data_t *in);
int cmdS(SSscmd *cmd);

19
LibSidServo/sidservo.h
View File

@ -112,12 +112,21 @@ typedef struct{
uint16_t ain1;
} extradata_t;
typedef enum{
MNT_STOPPED,
MNT_SLEWING,
MNT_POINTING,
MNT_GUIDING,
MNT_ERROR,
} mnt_status_t;
typedef struct{
mnt_status_t Xstatus;
mnt_status_t Ystatus;
coordval_t motXposition;
coordval_t motYposition;
coordval_t encXposition;
coordval_t encYposition;
// TODO: add speedX/Y
coordval_t encXspeed; // once per <config> s
coordval_t encYspeed;
uint8_t keypad;
@ -127,10 +136,6 @@ typedef struct{
double voltage;
} mountdata_t;
typedef struct{
;
} mountstat_t;
typedef struct{
double Xmot; // 0 X motor position (rad)
double Xspeed; // 4 X speed (rad/s)
@ -194,7 +199,7 @@ typedef struct{
// flags for slew function
typedef struct{
uint32_t slewNguide : 1; // ==1 to gude after slewing
uint32_t slewNguide : 1; // ==1 to guide after slewing
} slewflags_t;
// mount class
@ -203,8 +208,6 @@ typedef struct{
mcc_errcodes_t (*init)(conf_t *c); // init device
void (*quit)(); // deinit
mcc_errcodes_t (*getMountData)(mountdata_t *d); // get last data
// TODO: change (or add flags) switching slew-and-stop and slew-and-track
// add mount state: stop/slew/guide
mcc_errcodes_t (*slewTo)(const coordpair_t *target, slewflags_t flags);
mcc_errcodes_t (*correctTo)(coordval_pair_t *target);
mcc_errcodes_t (*moveTo)(const coordpair_t *target); // move to given position and stop

48
LibSidServo/ssii.c
View File

@ -35,6 +35,28 @@ uint16_t SScalcChecksum(uint8_t *buf, int len){
return checksum;
}
static void axestat(int32_t *prev, int32_t cur, int *nstopped, mnt_status_t *stat){
if(*prev == INT32_MAX){
*stat = MNT_STOPPED;
}else if(*stat != MNT_STOPPED){
if(*prev == cur){
if(++(*nstopped) > MOTOR_STOPPED_CNT) *stat = MNT_STOPPED;
}
}else if(*prev != cur){
//*stat = MNT_SLEWING;
*nstopped = 0;
}
*prev = cur;
}
// check for stopped/pointing states
static void ChkStopped(const SSstat *s, mountdata_t *m){
static int32_t Xmot_prev = INT32_MAX, Ymot_prev = INT32_MAX; // previous coordinates
static int Xnstopped = 0, Ynstopped = 0; // counters to get STOPPED state
axestat(&Xmot_prev, s->Xmot, &Xnstopped, &m->Xstatus);
axestat(&Ymot_prev, s->Ymot, &Ynstopped, &m->Ystatus);
}
/**
* @brief SSconvstat - convert stat from SSII format to human
* @param s (i) - just read data
@ -52,6 +74,7 @@ void SSconvstat(const SSstat *s, mountdata_t *m, double t){
*/
m->motXposition.val = X_MOT2RAD(s->Xmot);
m->motYposition.val = Y_MOT2RAD(s->Ymot);
ChkStopped(s, m);
m->motXposition.t = m->motYposition.t = t;
// fill encoder data from here, as there's no separate enc thread
if(!Conf.SepEncoder){
@ -154,3 +177,28 @@ int SSstop(int emerg){
return TRUE;
}
// update motors' positions due to encoders'
mcc_errcodes_t updateMotorPos(){
mountdata_t md = {0};
double t0 = dtime(), t = 0.;
DBG("start @ %g", t0);
do{
t = dtime();
if(MCC_E_OK == getMD(&md)){
DBG("got");
if(fabs(md.encXposition.t - t) < 0.1 && fabs(md.encYposition.t - t) < 0.1){
DBG("FIX motors position to encoders");
int32_t Xpos = X_RAD2MOT(md.encXposition.val), Ypos = Y_RAD2MOT(md.encYposition.val);
if(SSsetterI(CMD_MOTXSET, Xpos) && SSsetterI(CMD_MOTYSET, Ypos)){
DBG("All OK");
return MCC_E_OK;
}
}else{
DBG("on position");
return MCC_E_OK;
}
}
DBG("NO DATA; dt = %g", t - t0);
}while(t - t0 < 2.);
return MCC_E_FATAL;
}

30
LibSidServo/ssii.h
View File

@ -168,6 +168,9 @@
// Loop freq
#define SITECH_LOOP_FREQUENCY (1953.)
// amount of consequent same coordinates to detect stop
#define MOTOR_STOPPED_CNT (20)
// steps per revolution (SSI - x4 - for SSI)
// 13312000 / 4 = 3328000
#define X_MOT_STEPSPERREV_SSI (13312000.)
@ -178,9 +181,22 @@
//#define Y_MOT_STEPSPERREV (4394960.)
#define Y_MOT_STEPSPERREV (4394667.)
// convert angle in radians to +-pi
static inline double ang2half(double ang){
if(ang < -M_PI) ang += 2.*M_PI;
else if(ang > M_PI) ang -= 2.*M_PI;
return ang;
}
// convert to only positive: 0..2pi
static inline double ang2full(double ang){
if(ang < 0.) ang += 2.*M_PI;
else if(ang > 2.*M_PI) ang -= 2.*M_PI;
return ang;
}
// 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_MOT2RAD(n) ang2half(2. * M_PI * ((double)(n)) / X_MOT_STEPSPERREV)
#define Y_MOT2RAD(n) ang2half(2. * M_PI * ((double)(n)) / Y_MOT_STEPSPERREV)
#define X_RAD2MOT(r) ((int32_t)((r) / (2. * M_PI) * X_MOT_STEPSPERREV))
#define Y_RAD2MOT(r) ((int32_t)((r) / (2. * M_PI) * Y_MOT_STEPSPERREV))
// motor speed in rad/s and back
@ -202,15 +218,14 @@
#define X_ENC_STEPSPERREV (67108864.)
#define Y_ENC_STEPSPERREV (67108864.)
// encoder zero position
//#define X_ENC_ZERO (46033555)
#define X_ENC_ZERO (4603355)
#define Y_ENC_ZERO (36674010)
#define X_ENC_ZERO (61245239)
#define Y_ENC_ZERO (36999830)
// encoder reversed (no: +1)
#define X_ENC_SIGN (-1.)
#define Y_ENC_SIGN (-1.)
// encoder position to radians and back
#define X_ENC2RAD(n) (X_ENC_SIGN * 2.*M_PI * ((double)(n-X_ENC_ZERO)) / X_ENC_STEPSPERREV)
#define Y_ENC2RAD(n) (Y_ENC_SIGN * 2.*M_PI * ((double)(n-Y_ENC_ZERO)) / Y_ENC_STEPSPERREV)
#define X_ENC2RAD(n) ang2half(X_ENC_SIGN * 2.*M_PI * ((double)(n-X_ENC_ZERO)) / X_ENC_STEPSPERREV)
#define Y_ENC2RAD(n) ang2half(Y_ENC_SIGN * 2.*M_PI * ((double)(n-Y_ENC_ZERO)) / 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))
@ -320,3 +335,4 @@ int SSgetint(const char *cmd, int64_t *ans);
int SSsetterI(const char *cmd, int32_t ival);
int SSstop(int emerg);
int SSshortCmd(SSscmd *cmd);
mcc_errcodes_t updateMotorPos();

18
moving_model/main.c
View File

@ -38,12 +38,12 @@ static pars G = {
};
static limits_t limits = {
.min = {.coord = -1e6, .speed = 0.1, .accel = 0.1},
.max = {.coord = 1e6, .speed = 1e3, .accel = 50.},
.min = {.coord = -1e6, .speed = 0.01, .accel = 0.1},
.max = {.coord = 1e6, .speed = 20., .accel = 9.53523},
.jerk = 10.
};
static myoption opts[] = {
static sl_option_t opts[] = {
{"help", NO_ARGS, NULL, 'h', arg_int, APTR(&G.help), "show this help"},
{"ramp", NEED_ARG, NULL, 'r', arg_string, APTR(&G.ramptype), "ramp type: \"d\", \"t\" or \"s\" - dumb, trapezoid, s-type"},
{"deltat", NEED_ARG, NULL, 't', arg_int, APTR(&G.dT), "time interval for monitoring (microseconds, >0)"},
@ -84,9 +84,9 @@ static void monit(double tnext){
}
int main(int argc, char **argv){
initial_setup();
parseargs(&argc, &argv, opts);
if(G.help) showhelp(-1, opts);
sl_init();
sl_parseargs(&argc, &argv, opts);
if(G.help) sl_showhelp(-1, opts);
if(G.xlog){
coordslog = fopen(G.xlog, "w");
if(!coordslog) ERR("Can't open %s", G.xlog);
@ -101,8 +101,9 @@ int main(int argc, char **argv){
model = init_moving(ramp, &limits);
if(!model) ERRX("Can't init moving model: check parameters");
Tstart = nanot();
moveparam_t target = {.speed = 10., .coord = 20.};
if(move(&target)) monit(0.5);
moveparam_t target = {.speed = 8.0, .coord = 90.};
if(move(&target)) monit(60.);
/*
for(int i = 0; i < 10; ++i){
target.coord = -target.coord;
if(move(&target)) monit(1.);
@ -119,6 +120,7 @@ int main(int argc, char **argv){
target.coord = 0.; target.speed = 20.;
if(move(&target)) monit(1e6);
usleep(5000);
*/
fclose(coordslog);
return 0;
}