eddy/small_tel
1
0
Fork 0
mirror of https://github.com/eddyem/small_tel.git synced 2025-12-06 02:35:14 +03:00
Code Issues Packages Projects Releases Wiki Activity

SSII

Browse Source
This commit is contained in:
Edward Emelianov 2022-07-31 21:22:11 +03:00
parent 2cfdda730b
commit d3dd334aa7
12 changed files with 946 additions and 92 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Auxiliary_utils/SiderealServoII/Makefile
View File

@ -1,7 +1,7 @@
# run `make DEF=...` to add extra defines # run `make DEF=...` to add extra defines
PROGRAM := runscope PROGRAM := runscope
LDFLAGS := -fdata-sections -ffunction-sections -Wl,--gc-sections -Wl,--discard-all LDFLAGS := -fdata-sections -ffunction-sections -Wl,--gc-sections -Wl,--discard-all
LDFLAGS += -lusefull_macros LDFLAGS += -lusefull_macros -lm
SRCS := $(wildcard *.c) SRCS := $(wildcard *.c)
DEFINES := $(DEF) -D_GNU_SOURCE -D_XOPEN_SOURCE=1111 DEFINES := $(DEF) -D_GNU_SOURCE -D_XOPEN_SOURCE=1111
OBJDIR := mk OBJDIR := mk

5
Auxiliary_utils/SiderealServoII/cmdlnopts.c
View File

@ -50,9 +50,10 @@ static glob_pars const Gdefault = {
static myoption cmdlnopts[] = { static myoption cmdlnopts[] = {
// common options // common options
{"help", NO_ARGS, NULL, 'h', arg_int, APTR(&help), _("show this help")}, {"help", NO_ARGS, NULL, 'h', arg_int, APTR(&help), _("show this help")},
{"device", NEED_ARG, NULL, 'd', arg_string, APTR(&G.device), _("serial device name (default:" DEFAULT_SERDEV ")")}, {"device", NEED_ARG, NULL, 'd', arg_string, APTR(&G.device), _("serial device name (default: " DEFAULT_SERDEV ")")},
{"speed", NEED_ARG, NULL, 's', arg_int, APTR(&G.speed), _("serial device speed (default: 115200)")}, {"speed", NEED_ARG, NULL, 's', arg_int, APTR(&G.speed), _("serial device speed (default: 19200)")},
{"logfile", NEED_ARG, NULL, 'l', arg_string, APTR(&G.logfile), _("file to save logs")}, {"logfile", NEED_ARG, NULL, 'l', arg_string, APTR(&G.logfile), _("file to save logs")},
{"motlog", NEED_ARG, NULL, 'm', arg_string, APTR(&G.motorslog), _("log motors' data into this file")},
{"pidfile", NEED_ARG, NULL, 'P', arg_string, APTR(&G.pidfile), _("pidfile (default: " DEFAULT_PIDFILE ")")}, {"pidfile", NEED_ARG, NULL, 'P', arg_string, APTR(&G.pidfile), _("pidfile (default: " DEFAULT_PIDFILE ")")},
end_option end_option
}; };

1
Auxiliary_utils/SiderealServoII/cmdlnopts.h
View File

@ -27,6 +27,7 @@ typedef struct{
char *device; // serial device name char *device; // serial device name
char *pidfile; // name of PID file char *pidfile; // name of PID file
char *logfile; // logging to this file char *logfile; // logging to this file
char *motorslog; // log of motors' data
int speed; // connection speed int speed; // connection speed
int rest_pars_num; // number of rest parameters int rest_pars_num; // number of rest parameters
char** rest_pars; // the rest parameters: array of char* char** rest_pars; // the rest parameters: array of char*

115
Auxiliary_utils/SiderealServoII/emulator.c Normal file
View File

@ -0,0 +1,115 @@
/*
* This file is part of the SSII project.
* Copyright 2022 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/>.
*/
#include <math.h>
#include <stdio.h>
#include <usefull_macros.h>
#include "emulator.h"
#include "motlog.h"
// HA/DEC of starting point, starting time
static double Eha0 = 0., Edec0 = 0., Et0 = 0.;
// monitor each 100ms motor's motion; stop when coords are the same for 3 times in a row
// inistat - returning of binary command (if len equal to sizeof SSstat)
void SSmotor_monitoring(SSstat *inistat){
DBG("Start monitoring");
//int32_t oldDec = 0, oldRA = 0; // old positions in encoder values (also for speed calculations)
SSstat prevstat = {0};
uint8_t start = 1, repeat = 0, errctr = 0;
int32_t oldRAm = 0, oldDm = 0;
double tlast = dtime();
if(inistat){ // get initial values
start = 0;
prevstat = *inistat;
oldRAm = prevstat.HAmot;
oldDm = prevstat.DECmot;
}
while(1){
if(start){ // didn't get initial values -> need to know first
if(!SSgetPartialstat(&prevstat)) continue;
start = 0;
tlast = dtime();
oldRAm = prevstat.HAmot;
oldDm = prevstat.DECmot;
}else{
if(!SSlog_motor_data(&prevstat, &tlast)){
WARNX("Hmmm... in %dth time", ++errctr);
if(++errctr > 10) break;
else continue;
}else errctr = 0;
}
if(prevstat.HAmot == oldRAm && prevstat.DECmot == oldDm){
if(++repeat > 2) break;
}else repeat = 0;
oldRAm = prevstat.HAmot;
oldDm = prevstat.DECmot;
}
DBG("End of monitoring");
}
/**
* @brief log_motor_data - get current config & put to log difference between values
* @param old (io) - old parameters value: as it was -> as it become
* @param told (io) - previous time (`dtime()` output) -> current time
* @return TRUE if all OK (if return FALSE, the `old` and `told` won't be changed!
*/
int SSlog_motor_data(SSstat *old, double *told){
SSstat s;
if(!old || !told) return FALSE;
if(!SSgetPartialstat(&s)) return FALSE;
double tnow = dtime(), tdif = tnow - *told;
mot_log(1, "%d\t%d\t%.1f\t%d\t%d\t%.1f", s.DECmot, s.DECenc, (s.DECmot - old->DECmot) / tdif,
s.HAmot, s.HAenc, (s.HAmot - old->HAmot) / tdif);
*old = s;
*told = tnow;
return TRUE;
}
// calculate next `target` position in current moment of time
// ha changed as 15''/s + 15*sin(t*2pi/600s);
// dec changed as 30*cos(t*2pi/600s)
static void emul_next_pos(double *ha, double *dec, double tnew){
if(!ha || !dec) return;
double s1, c1, t = tnew - Et0;
sincos(t*2.*M_PI/600., &s1, &c1);
*ha = Eha0 + ARCSEC2DEG(15) * (t + s1);
*dec = Edec0 + ARCSEC2DEG(30) * c1;
}
// init emulated variables and start target's searching
void SSstart_emulation(double ha_start, double dec_start){
Eha0 = ha_start;
Edec0 = dec_start;
Et0 = dtime();
DBG("Try to send short command");
SSscmd sc = {0};
// now calculate first starting position and try to catch it
double ha, dec;
emul_next_pos(&ha, &dec, Et0 + 1.);
mot_log(0, "goto ha=%d, dec=%d");
SSgoto(ha, dec);
SSwaitmoving();
// now we are near of the target: start to catch it
;
while(SScmds(&sc) != sizeof(SSstat)) WARNX("SSCMDshort bad answer!");
}

35
Auxiliary_utils/SiderealServoII/emulator.h Normal file
View File

@ -0,0 +1,35 @@
/*
* This file is part of the SSII project.
* Copyright 2022 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/>.
*/
#pragma once
#include "sidservo.h"
#define SECPERRAD (206264.802)
// convert angles
#define ARCSEC2DEG(x) (x/3600.)
#define DEG2ARCSEC(x) (x*3600.)
#define ARCSEC2RAD(x) (x/SECPERRAD)
#define RAD2ARCSEC(x) (x*SECPERRAD)
void SSmotor_monitoring(SSstat *inistat);
int SSlog_motor_data(SSstat *old, double *told);
void SSstart_emulation(double ha_start, double dec_start);

87
Auxiliary_utils/SiderealServoII/main.c
View File

@ -21,50 +21,32 @@
#include <stdlib.h> // exit, free #include <stdlib.h> // exit, free
#include <string.h> // strdup #include <string.h> // strdup
#include <unistd.h> // sleep #include <unistd.h> // sleep
#include <usefull_macros.h>
#include "cmdlnopts.h" #include "cmdlnopts.h"
#include "emulator.h"
#include "motlog.h"
#include "sidservo.h" #include "sidservo.h"
/*
#include <termios.h> // tcsetattr
#include <unistd.h> // tcsetattr, close, read, write
#include <sys/ioctl.h> // ioctl
#include <stdio.h> // printf, getchar, fopen, perror
#include <stdlib.h> // exit
#include <sys/stat.h> // read
#include <fcntl.h> // read
#include <signal.h> // signal
#include <time.h> // time
#include <string.h> // memcpy
#include <stdint.h> // int types
#include <sys/time.h> // gettimeofday
*/
/**
* This is an example of usage:
* - command line arguments,
* - log file,
* - check of another file version running,
* - signals management,
* - serial port reading/writing.
* The `cmdlnopts.[hc]` are intrinsic files of this demo.
*/
static glob_pars *GP = NULL; // for GP->pidfile need in `signals` static glob_pars *GP = NULL; // for GP->pidfile need in `signals`
/** /**
* We REDEFINE the default WEAK function of signal processing * We REDEFINE the default WEAK function of signal processing
*/ */
void signals(int sig){ void signals(int sig){
DBG("Quit");
if(sig > 0){ if(sig > 0){
signal(sig, SIG_IGN); signal(sig, SIG_IGN);
DBG("Get signal %d, quit.\n", sig); DBG("Get signal %d, quit.\n", sig);
} }
LOGERR("Exit with status %d", sig); LOGERR("Exit with status %d", sig);
DBG("unlink");
if(GP && GP->pidfile) // remove unnesessary PID file if(GP && GP->pidfile) // remove unnesessary PID file
unlink(GP->pidfile); unlink(GP->pidfile);
SSwritecmd(CMD_STOPHA);
SSwritecmd(CMD_STOPDEC);
DBG("close");
SSclose(); SSclose();
DBG("exit");
exit(sig); exit(sig);
} }
@ -76,22 +58,61 @@ int main(int argc, char *argv[]){
initial_setup(); initial_setup();
char *self = strdup(argv[0]); char *self = strdup(argv[0]);
GP = parse_args(argc, argv); GP = parse_args(argc, argv);
/* if(GP->rest_pars_num){
printf("%d extra options:\n", GP->rest_pars_num);
for(int i = 0; i < GP->rest_pars_num; ++i)
printf("%s\n", GP->rest_pars[i]);
}*/
check4running(self, GP->pidfile); check4running(self, GP->pidfile);
red("%s started, snippets library version is %s\n", self, sl_libversion()); green("%s started, snippets library version is %s\n", self, sl_libversion());
free(self); free(self);
if(GP->logfile){
if(!OPENLOG(GP->logfile, LOGLEVEL_ANY, 1)) ERRX("Can't open logfile %s", GP->logfile);
}
if(GP->motorslog){
if(!open_mot_log(GP->motorslog)) ERRX("Can't open motors' log file %s", GP->motorslog);
mot_log(0, "# Motor's data\n#time\tX\tXenc\tVx\tY\tYenc\tVy");
}
signal(SIGTERM, signals); // kill (-15) - quit signal(SIGTERM, signals); // kill (-15) - quit
signal(SIGHUP, SIG_IGN); // hup - ignore signal(SIGHUP, SIG_IGN); // hup - ignore
signal(SIGINT, signals); // ctrl+C - quit signal(SIGINT, signals); // ctrl+C - quit
signal(SIGQUIT, signals); // ctrl+\ - quit signal(SIGQUIT, signals); // ctrl+\ - quit
signal(SIGTSTP, SIG_IGN); // ignore ctrl+Z signal(SIGTSTP, SIG_IGN); // ignore ctrl+Z
if(GP->logfile) OPENLOG(GP->logfile, LOGLEVEL_ANY, 1);
LOGMSG("Start application..."); LOGMSG("Start application...");
if(!SSinit(GP->device, GP->speed)) signals(-2); if(!SSinit(GP->device, GP->speed)) signals(-2);
mot_log(0, "Starting of emulation");
SSstart_emulation(0., 0.);
mot_log(0, "Return to zero");
SSgoto(0., 0.);
SSwaitmoving();
signals(0);
// try to move for 45 degrees by both axes
if(!SSgoto(45., 45.)) signals(-3);
SSwaitmoving();
signals(0);
DBG("Try to send short command");
SSscmd sc = {
.DECmot = 500000,
.DECspeed = 1000000,
.HAmot = 600000,
.HAspeed = 2000000
};
mot_log(0, "Send short command");
while(SScmds(&sc) != sizeof(SSstat)) WARNX("SSCMDshort bad answer!");
SSmotor_monitoring((SSstat*)SSread(NULL)); // monitor
SSlcmd lc = {
.DECmot = 0,
.DECspeed = 2000000, // steps per sec * 65536 / 1953
.HAmot = 0, //-427643
.HAspeed = 2000000,
.DECadder = 100,
.HAadder = 40,
.DECatime = 1953*3,
.HAatime = 1953*4
};
DBG("Try to send long command");
mot_log(0, "Send long command");
while(SScmdl(&lc) != sizeof(SSstat)) WARNX("SSCMDlong bad answer!");
SSmotor_monitoring((SSstat*)SSread(NULL)); // monitor
mot_log(0, "Stop motors");
SSwritecmd(CMD_STOPHA);
SSwritecmd(CMD_STOPDEC);
SSmotor_monitoring(NULL); // monitor stopping
/* /*
double t0 = dtime(); double t0 = dtime();
while(1){ // read data from port and print in into terminal while(1){ // read data from port and print in into terminal

210
Auxiliary_utils/SiderealServoII/mot.log Normal file
View File

@ -0,0 +1,210 @@
# Motor's data
#time X Xenc Vx Y Yenc Vy
Send short command
13303.19 498304 54650237 5641.4 660460 47372412 -9664.9
13373.17 498816 54650237 7316.6 659711 47361345 -10703.4
13443.17 499360 54650237 7770.3 658757 47346256 -13626.6
13513.06 499739 54650237 5423.6 657616 47327322 -16328.0
13583.08 499955 54650237 3084.7 656236 47303304 -19707.5
13655.18 500000 54650237 624.1 654619 47276259 -22427.5
13726.26 500000 54650237 0.0 652752 47261344 -26265.0
13798.17 500000 54650237 0.0 650714 47227967 -28340.6
13868.11 500000 54650237 0.0 648390 47190361 -33227.5
13940.23 500000 54650237 0.0 645918 47148884 -34279.1
14012.23 500000 54650237 0.0 643167 47103676 -38213.0
14082.23 500000 54650237 0.0 640163 47054992 -42910.1
14153.09 500000 54650237 0.0 637023 47027434 -44312.0
14224.13 500000 54650237 0.0 633635 46971536 -47690.1
14295.08 500000 54650237 0.0 630023 46912563 -50906.3
14366.06 500000 54650237 0.0 626501 46853184 -49618.2
14437.97 500000 54650237 0.0 623163 46796212 -46421.7
14507.91 500000 54650237 0.0 620040 46770651 -44652.5
14578.13 500000 54650237 0.0 617192 46720784 -40557.1
14649.02 500000 54650237 0.0 614571 46675824 -36971.2
14719.00 500000 54650237 0.0 612137 46633697 -34782.4
14790.15 500000 54650237 0.0 609967 46597060 -30498.5
14862.12 500000 54650237 0.0 607957 46563570 -27933.8
14932.07 500000 54650237 0.0 606159 46548481 -25701.5
15003.06 500000 54650237 0.0 604609 46521327 -21831.6
15074.08 500000 54650237 0.0 603289 46497742 -18587.6
15144.17 500000 54650237 0.0 602170 46478563 -15965.9
15214.98 500000 54650237 0.0 601305 46463679 -12215.6
15284.90 500000 54650237 0.0 600642 46457605 -9481.3
15355.01 500000 54650237 0.0 600207 46448898 -6205.4
15426.00 500000 54650237 0.0 599985 46444326 -3126.9
15496.01 500000 54650237 0.0 599990 46444087 71.4
15565.85 500000 54650237 0.0 599990 46444035 0.0
15635.82 500000 54650237 0.0 599991 46444034 14.3
15706.88 500000 54650237 0.0 599991 46444033 0.0
15776.82 500000 54650237 0.0 599991 46444022 0.0
15846.82 500000 54650237 0.0 599991 46444022 0.0
Send long command
16131.76 499414 54650237 -6096.5 599054 46432705 -10048.0
16201.78 498878 54650237 -7655.6 598288 46420698 -10940.7
16272.79 498181 54650237 -9814.4 597315 46404769 -13700.8
16343.78 497308 54650237 -12297.2 596112 46384782 -16945.6
16414.63 496267 54650237 -14693.6 594681 46373403 -20198.4
16485.66 495078 54650237 -16740.8 593030 46346714 -23245.7
16556.65 493700 54650237 -19409.3 591170 46316243 -26198.3
16626.71 492150 54650237 -22124.5 589065 46282021 -30046.5
16698.68 490492 54650237 -23037.1 586787 46243756 -31651.7
16770.52 488593 54650237 -26433.0 584218 46201674 -35759.0
16840.52 486529 54650237 -29485.2 581499 46179979 -38842.2
16911.46 484354 54650237 -30662.0 578541 46130642 -41700.3
16982.52 482046 54650237 -32477.7 575343 46078292 -45001.5
17052.52 479510 54650237 -36229.1 571898 46021592 -49215.0
17123.55 476842 54650237 -37560.6 568330 45961068 -50231.0
17193.56 473952 54650237 -41281.0 564449 45896595 -55436.5
17263.58 470973 54650237 -42543.1 560409 45862243 -57695.3
17335.53 467810 54650237 -43966.6 556266 45791085 -57588.9
17405.49 464397 54650237 -48783.6 551976 45720984 -61319.0
17475.53 460914 54650237 -49729.4 547776 45649988 -59966.6
17545.57 457269 54650237 -52037.3 543638 45579416 -59075.5
17616.50 453463 54650237 -53660.0 539446 45543487 -59102.1
17687.50 449418 54650237 -56971.8 535222 45472384 -59492.9
17759.55 445244 54650237 -57936.9 530968 45402024 -59047.3
17831.41 440932 54650237 -60003.9 526697 45330704 -59433.4
17902.45 436674 54650237 -59937.1 522394 45260321 -60570.6
17974.42 432430 54650237 -58969.0 518180 45189210 -58552.2
18045.43 428105 54650237 -60905.2 513883 45153563 -60510.9
18117.42 423873 54650237 -58785.2 509641 45083156 -58924.1
18189.41 419603 54650237 -59310.6 505378 45011833 -59213.4
18260.41 415305 54650237 -60541.0 501074 44941371 -60625.5
18332.29 411098 54650237 -58523.4 496843 44870325 -58857.2
18404.29 406801 54650237 -59683.7 492542 44799294 -59739.3
18474.42 402518 54650237 -61069.7 488237 44764352 -61383.4
18546.29 398321 54650237 -58396.5 484095 44693048 -57631.2
18617.26 394020 54650237 -60608.9 479796 44622609 -60580.7
18688.31 389822 54650237 -59079.3 475597 44551752 -59093.3
18759.37 385581 54650237 -59682.8 471356 44480519 -59682.8
18831.32 381332 54650237 -59056.7 467106 44410023 -59070.6
18903.32 377052 54650237 -59442.7 462825 44374362 -59456.5
18974.35 372760 54650237 -60426.1 458528 44303795 -60496.5
19046.31 368506 54650237 -59117.0 454282 44232986 -59005.8
19118.29 364248 54650237 -59157.9 449997 44161769 -59533.0
19189.31 359934 54650237 -60743.3 445703 44091235 -60461.7
19261.19 355712 54650237 -58736.9 441458 44020045 -59056.9
19332.18 351422 54650237 -60427.5 437193 43984937 -60075.4
19404.29 347175 54650237 -58901.8 432950 43914065 -58846.4
19475.19 342875 54650237 -60645.6 428652 43842798 -60617.4
19547.36 338680 54650237 -58123.4 424427 43772518 -58539.0
19618.22 334365 54650237 -60900.7 420129 43701253 -60660.8
19688.31 330148 54650237 -60158.5 415897 43630749 -60372.5
19759.28 325990 54650237 -58589.9 411734 43595366 -58660.3
19830.34 321736 54650237 -59871.6 407505 43523993 -59519.7
19902.21 317482 54650237 -59187.8 403260 43453664 -59062.6
19974.27 313226 54650237 -59058.7 398970 43382500 -59530.5
20045.35 308930 54650237 -60446.7 394670 43312027 -60503.0
20116.33 304671 54650237 -60000.1 390438 43241065 -59619.8
20188.32 300465 54650237 -58421.5 386228 43205317 -58477.0
20259.33 296162 54650237 -60597.2 381910 43135048 -60808.4
20329.35 291909 54650237 -60736.3 377685 43063716 -60336.4
20400.39 287774 54650237 -58211.6 373519 42993191 -58648.0
20471.33 283528 54650237 -59847.5 369300 42922673 -59467.0
20541.34 279284 54650237 -60623.3 365056 42851557 -60623.3
20612.36 275098 54650237 -58942.8 360869 42816352 -58956.9
20684.33 270883 54650237 -58569.4 356652 42745135 -58597.2
20756.25 266580 33057240 -59827.1 352349 42666083 -59827.1
20828.38 262327 33057240 -58957.9 348072 42595149 -59290.6
20898.22 258007 33057240 -61859.3 343778 42559373 -61487.0
20970.35 253831 33057240 -57896.3 339602 42488934 -57896.3
21041.36 249523 33057240 -60668.4 335323 42417665 -60260.0
21111.23 245330 33057240 -60013.9 331075 42347376 -60801.1
21181.16 241128 33057240 -60080.2 326900 42276464 -59694.1
21252.23 236994 33057240 -58167.8 322713 42205173 -58913.6
21323.38 232744 33057240 -59734.7 318515 42170159 -59003.8
21393.36 228489 33057240 -60808.3 314272 42099029 -60636.9
21465.39 224314 33057240 -57961.9 310085 42028722 -58128.5
21536.38 220049 33057240 -60078.0 305823 41958020 -60035.7
21606.16 215804 33057240 -60833.3 301577 41886742 -60847.7
21677.40 211651 33057240 -58303.2 297398 41851757 -58668.2
21748.36 207418 33057240 -59644.9 293193 41780888 -59250.3
21820.37 203177 33057240 -58893.0 288926 41710378 -59254.1
21892.39 198885 33057240 -59592.9 284623 41639413 -59745.7
21962.35 194595 33057240 -61321.7 280359 41568240 -60950.1
22034.40 190417 33057240 -57988.2 276196 41498263 -57780.0
22106.38 186118 33057240 -59723.3 271865 41426639 -60167.9
22177.37 181833 33057240 -60368.2 267603 41391722 -60044.2
22249.38 177612 33057240 -58614.3 263362 41320779 -58892.0
22320.40 173309 33057240 -60587.8 259057 41249716 -60616.0
22391.14 169100 33057240 -59500.4 254840 41179597 -59613.5
22463.23 164870 33057240 -58675.8 250609 41108006 -58689.6
22532.89 160578 33057240 -61613.6 246325 41037724 -61498.8
22602.89 156422 33057240 -59373.1 242145 41002089 -59716.0
22674.88 152226 33057240 -58289.9 238000 40931214 -57581.4
22744.64 147930 33057240 -61572.0 233699 40860866 -61643.6
22815.62 143783 33057240 -58434.9 229552 40789182 -58434.9
22886.62 139535 33057240 -59827.7 225306 40719211 -59799.5
22958.62 135328 33057240 -58426.5 221097 40683165 -58454.2
23030.64 131033 33057240 -59642.9 216801 40612638 -59656.8
23102.47 126725 33057240 -59973.1 212499 40542177 -59889.6
23174.52 122466 33057240 -59112.6 208210 40470560 -59529.0
23244.52 118136 33057240 -61855.3 203915 40400739 -61355.3
23314.52 114010 33057240 -58936.1 199754 40329899 -59436.0
23386.56 109816 33057240 -58223.4 195589 40294238 -57820.8
23456.55 105521 33057240 -61363.3 191289 40223595 -61434.7
23527.53 101356 33057240 -58675.3 187126 40152087 -58647.1
23597.51 97130 33057240 -60394.7 182963 40082112 -59494.4
23668.53 92936 33057240 -59053.7 178758 40011123 -59208.6
23739.52 88709 33057240 -59551.9 174475 39939518 -60340.9
23809.39 84489 33057240 -60385.9 170241 39904730 -60586.3
23879.52 80315 33057240 -59516.3 166145 39833693 -58404.1
23950.37 76118 33057240 -59246.1 161890 39763522 -60064.8
24021.56 71921 33057240 -58955.0 157723 39692341 -58533.6
24092.41 67677 33057240 -59902.9 153457 39621040 -60213.4
24162.36 63423 33057240 -60809.6 149195 39585914 -60924.0
24233.40 59287 33057240 -58219.8 145108 39515260 -57530.1
24304.39 55037 33057240 -59862.7 140807 39444968 -60581.0
24375.41 50866 33057240 -58729.8 136629 39373730 -58828.4
24445.27 46821 33057240 -57901.5 132337 39302707 -61437.2
24516.41 42989 33057240 -53867.2 128237 39267360 -57634.5
24586.29 39273 33057240 -53180.7 123961 39196956 -61195.0
24656.37 35810 33057240 -49414.3 119821 39126495 -59074.5
24727.44 32464 33057240 -47080.4 115652 39054998 -58660.5
24798.30 29237 33057240 -45540.4 111353 38984040 -60668.8
24868.27 26217 33057240 -43162.0 107167 38914545 -59826.5
24937.30 23370 33057240 -41239.9 103023 38878374 -60027.5
25006.29 20697 33057240 -38744.2 98899 38807968 -59776.0
25075.42 18231 33057240 -35673.9 94773 38736609 -59688.0
25146.43 15951 33057240 -32110.9 90641 38665667 -58194.0
25215.39 13709 33057240 -32508.8 86455 38630251 -60696.7
25284.41 11683 33057240 -29352.1 82338 38559921 -59645.8
25354.33 9862 33057240 -26046.7 78215 38489376 -58973.4
25424.41 8139 33057240 -24587.6 74059 38417898 -59307.0
25493.44 6577 33057240 -22625.5 69870 38347238 -60677.4
25563.43 5198 33057240 -19701.7 65752 38311350 -58833.6
25632.47 3968 33057240 -17817.1 61579 38241650 -60447.8
25702.44 2920 33057240 -14977.8 57475 38170479 -58653.6
25772.42 2019 33057240 -12873.5 53306 38099165 -59566.5
25841.44 1282 33057240 -10679.3 49139 38028924 -60380.9
25908.49 715 33057240 -8457.5 45069 37992837 -60709.0
25978.43 317 33057240 -5689.7 41014 37922920 -57969.3
26046.42 52 33057240 -3897.5 37005 37852774 -58961.8
26114.46 11 33057240 -602.6 33219 37786211 -55645.4
26181.42 10 33057240 -14.9 29638 37755569 -53477.3
26250.45 8 33057240 -29.0 26314 37694617 -48160.6
26317.46 0 33057240 -119.4 23132 37638936 -47477.7
26383.40 0 33057240 0.0 20198 37612621 -44500.4
26450.46 0 33057240 0.0 17539 37562913 -39647.5
26517.49 0 33057240 0.0 14998 37517006 -37910.6
26585.46 0 33057240 0.0 12644 37475088 -34633.5
26651.45 0 33057240 0.0 10454 37455790 -33188.3
26718.31 0 33057240 0.0 8532 37420115 -28744.9
26784.44 0 33057240 0.0 6783 37388019 -26446.3
26853.42 0 33057240 0.0 5241 37360557 -22355.6
26921.30 0 33057240 0.0 3878 37348346 -20079.9
26987.34 0 33057240 0.0 2728 37327218 -17414.2
27054.36 0 33057240 0.0 1798 37310184 -13875.0
27121.32 0 33057240 0.0 1043 37303173 -11275.1
27187.43 0 33057240 0.0 492 37292118 -8334.6
27252.47 0 33057240 0.0 145 37285086 -5335.5
27319.49 0 33057240 0.0 9 37282145 -2029.3
27387.32 0 33057240 0.0 10 37282191 14.7
27454.45 0 33057240 0.0 8 37282196 -29.8
27520.46 0 33057240 0.0 0 37282182 -121.2
27586.48 0 33057240 0.0 0 37282177 0.0
27652.48 0 33057240 0.0 0 37282168 0.0
27718.50 0 33057240 0.0 0 37282170 0.0
Stop motors
27960.49 0 33057240 0.0 0 37282175 0.0
28026.49 0 33057240 0.0 0 37282174 0.0

69
Auxiliary_utils/SiderealServoII/motlog.c Normal file
View File

@ -0,0 +1,69 @@
/*
* This file is part of the SSII project.
* Copyright 2022 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/>.
*/
#include <usefull_macros.h>
#include <stdio.h>
#include <stdarg.h>
#include "motlog.h"
static FILE *flog = NULL;
static double time0 = 0.;
int open_mot_log(const char *path){
if(!path) return FALSE;
flog = fopen(path, "a+");
if(!flog){
WARN("Can't open %s", path);
return FALSE;
}
time0 = dtime();
return TRUE;
}
void refresh_t0(){
time0 = dtime();
}
void close_mot_log(){
if(!flog) return;
fclose(flog);
flog = NULL;
}
int mot_log(int timestamp, const char *fmt, ...){
if(!flog) return 0;
int i = 0;
// timestamp in milliseconds
#ifdef EBUG
red("Time: %10.2f\n", 1000.*(dtime() - time0));
#endif
if(timestamp) i += fprintf(flog, "%10.2f\t", 1000.*(dtime() - time0));
va_list ar;
va_start(ar, fmt);
i += vfprintf(flog, fmt, ar);
va_end(ar);
fseek(flog, -1, SEEK_CUR);
char c;
ssize_t r = fread(&c, 1, 1, flog);
if(1 == r){ // add '\n' if there was no newline
if(c != '\n') i += fprintf(flog, "\n");
}
fflush(flog);
return i;
}

25
Auxiliary_utils/SiderealServoII/motlog.h Normal file
View File

@ -0,0 +1,25 @@
/*
* This file is part of the SSII project.
* Copyright 2022 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/>.
*/
#pragma once
#include <usefull_macros.h>
int open_mot_log(const char *path);
void close_mot_log();
int mot_log(int timestamp, const char *fmt, ...);
void refresh_t0();

3
Auxiliary_utils/SiderealServoII/msg.log Normal file
View File

@ -0,0 +1,3 @@
[MSG] 2022/07/30-21:16:04 Start application...
[DBG] 2022/07/30-21:16:04 Try to open serial /dev/ttyUSB0 @19200
[ERR] 2022/07/30-21:16:32 Exit with status 0

376
Auxiliary_utils/SiderealServoII/sidservo.c
View File

@ -16,57 +16,80 @@
* along with this program. If not, see <http://www.gnu.org/licenses/>. * along with this program. If not, see <http://www.gnu.org/licenses/>.
*/ */
#include <ctype.h>
#include <stdio.h> #include <stdio.h>
#include <string.h> #include <string.h>
#include <math.h>
#include "emulator.h"
#include "motlog.h"
#include "sidservo.h" #include "sidservo.h"
static TTY_descr *dev = NULL; // shoul be global to restore if die static TTY_descr *dev = NULL; // shoul be global to restore if die
static uint8_t buff[BUFLEN]; static uint8_t buff[BUFLEN];
static int buflen = 0; static int buflen = 0;
static uint16_t calcChecksum(uint8_t *buf, int len){ static uint16_t calcChecksum(uint8_t initval, uint8_t *buf, int len){
uint16_t checksum = 0x11; // I don't know from where does this "magick" uint16_t checksum = initval; // I don't know from where does this "magick"
for(int i = 0; i < len; i++) for(int i = 0; i < len; i++)
checksum += *buf++; checksum += *buf++;
checksum ^= 0xFF00; // invert high byte checksum ^= 0xFF00; // invert high byte
DBG("Checksum: 0x%04x", checksum); DBG("Checksum of %d bytes: 0x%04x", len, checksum);
return checksum; return checksum;
} }
// encoder's settings per rev
static struct {
uint32_t hamotperrev; // motors' encoders ticks per revolution
uint32_t decmotperrev;
uint32_t haencperrev; // axis' encoders ticks per revolution
uint32_t decencperrev;
int32_t hamotzero; // motors' encoders ticks @ zero position
int32_t decmotzero;
} encsettings;
/** /**
* @brief SSinit - open serial device and get initial info * @brief SSinit - open serial device and get initial info
* @return TRUE if all OK * @return TRUE if all OK
*/ */
int SSinit(char *devpath, int speed){ int SSinit(char *devpath, int speed){
LOGDBG("Try to open serial %s @%d", devpath, speed); LOGDBG("Try to open serial %s @%d", devpath, speed);
int64_t i;
dev = new_tty(devpath, speed, BUFLEN); dev = new_tty(devpath, speed, BUFLEN);
if(dev) dev = tty_open(dev, 1); // open exclusively if(dev) dev = tty_open(dev, 1); // open exclusively
if(!dev){ if(!dev){
LOGERR("Can't open %s with speed %d. Exit.", devpath, speed); LOGERR("Can't open %s with speed %d. Exit.", devpath, speed);
signals(-1); signals(-1);
} }
for(int ntries = 0; ntries < 5; ++ntries){ // try at most 5 times for(int ntries = 0; ntries < 10; ++ntries){ // try at most 10 times
if(!SSgetstat(NULL)) continue; SSstat s;
SSstat *s = (SSstat*) buff; DBG("Try for %dth time... ", ntries);
#ifdef EBUG SSwritecmd(CMD_MOTHA);
green("\nGet data:\n"); SSwritecmd(CMD_MOTDEC);
printf("DECmot=%d (0x%08x)\n", s->DECmot, (uint32_t)s->DECmot); if(!SSgetPartialstat(&s)) continue;
printf("DECenc=%d (0x%08x)\n", s->DECenc, (uint32_t)s->DECenc); if(!SSgetInt(CMD_GETDECMEPR, &i)) continue;
printf("RAmot=%d (0x%08x)\n", s->RAmot, (uint32_t)s->RAmot); encsettings.decmotperrev = (uint32_t) i;
printf("RAenc=%d (0x%08x)\n", s->RAenc, (uint32_t)s->RAenc); LOGDBG("decmotperrev = %u", encsettings.decmotperrev);
printf("keypad=0x%02x\n", s->keypad); if(!SSgetInt(CMD_GETHAMEPR, &i)) continue;
printf("XBits=0x%02x\n", s->XBits); encsettings.hamotperrev = (uint32_t) i;
printf("YBits=0x%02x\n", s->YBits); LOGDBG("hamotperrev = %u", encsettings.hamotperrev);
printf("EBits=0x%02x\n", s->ExtraBits); if(!SSgetInt(CMD_GETDECAEPR, &i)) continue;
printf("ain0=%u\n", s->ain0); encsettings.decencperrev = (uint32_t) i;
printf("ain1=%u\n", s->ain1); LOGDBG("decencperrev = %u", encsettings.decencperrev);
printf("millis=%u\n", s->millis); if(!SSgetInt(CMD_GETHAAEPR, &i)) continue;
printf("tF=%d\n", s->tF); encsettings.haencperrev = (uint32_t) i;
printf("V=%f\n", ((float)s->voltage)/10.f); LOGDBG("haencperrev = %u", encsettings.haencperrev);
printf("checksum=0x%04x\n\n", s->checksum); double d = (double)(s.HAenc - HA_ENC_ZEROPOS);
#endif d /= encsettings.haencperrev;
if(calcChecksum(buff, sizeof(SSstat)-2) == s->checksum) return TRUE; d *= encsettings.hamotperrev;
encsettings.hamotzero = s.HAmot - (int32_t)d;
LOGDBG("hamotzero = %d", encsettings.hamotzero);
d = (double)(s.DECenc - DEC_ENC_ZEROPOS);
d /= encsettings.decencperrev;
d *= encsettings.decmotperrev;
encsettings.decmotzero = s.DECmot - (int32_t)d;
LOGDBG("decmotzero = %u", encsettings.decmotzero);
return TRUE;
} }
return FALSE; return FALSE;
} }
@ -79,18 +102,32 @@ void SSclose(){
} }
/** /**
* @brief SSwrite - write & return answer * @brief SSwrite - write command and wait for answer
* @param buf - buffer with text or binary data * @param buf - buffer with text or binary data
* @param buflen - its length * @param len - its length
* @param flags - >0: wait for binary data (until timeout), ==0: wait for '\r' or timeout, <0: don't wait at all
* @return amount of bytes read, if got answer; 0 without answer, -1 if device disconnected, -2 if can't write * @return amount of bytes read, if got answer; 0 without answer, -1 if device disconnected, -2 if can't write
*/ */
int SSwrite(const uint8_t *buf, int len){ int SSwrite(const uint8_t *buf, int len, int flags){
DBG("try to write %d bytes", len); DBG("try to write %d bytes", len);
if(write_tty(dev->comfd, (const char*)buf, len)){ #ifdef EBUG
double t00 = dtime();
for(int i = 0; i < len; ++i) if(buf[i] > 32) printf("%c", buf[i]); else printf(".");
printf("\n");
for(int i = 0; i < len; ++i) printf("0x%02x ", buf[i]);
printf("\n");
#endif
read_tty(dev); // clear incoming buffer
if(write_tty(dev->comfd, (const char*)buf, (size_t) len)){
LOGERR("Can't write data to port"); LOGERR("Can't write data to port");
DBG("Can't write, time=%g", dtime()-t00);
return -2; return -2;
} }
write_tty(dev->comfd, "\r", 1); // add EOL write_tty(dev->comfd, "\r", 1); // add EOL
if(flags < 0){
DBG("Don't wait, time=%g", dtime()-t00);
return 0;
}
buflen = 0; buflen = 0;
double t0 = dtime(); double t0 = dtime();
while(dtime() - t0 < READTIMEOUT && buflen < BUFLEN){ while(dtime() - t0 < READTIMEOUT && buflen < BUFLEN){
@ -101,11 +138,20 @@ int SSwrite(const uint8_t *buf, int len){
} }
if(r == 0) continue; if(r == 0) continue;
int rest = BUFLEN - buflen; int rest = BUFLEN - buflen;
if((int)dev->buflen > rest) dev->buflen = rest; // TODO: what to do with possible buffer overrun? int out = 0;
DBG("buf[%d]=0x%02x", r-1, dev->buf[r-1]);
if(flags == 0 && dev->buf[r-1] == '\n') out = 1; // end of message
if((int)dev->buflen > rest){
dev->buflen = (size_t) rest;
out = 1;
}
memcpy(&buff[buflen], dev->buf, dev->buflen); memcpy(&buff[buflen], dev->buf, dev->buflen);
buflen += dev->buflen; buflen += dev->buflen;
DBG("get %ld bytes, out=%d", dev->buflen, out);
if(out) break;
t0 = dtime();
} }
DBG("got buflen=%d", buflen); DBG("got buflen=%d, time=%g", buflen, dtime()-t00);
#ifdef EBUG #ifdef EBUG
for(int i = 0; i < buflen; ++i){ for(int i = 0; i < buflen; ++i){
printf("%02x (%c) ", buff[i], (buff[i] > 31) ? buff[i] : ' '); printf("%02x (%c) ", buff[i], (buff[i] > 31) ? buff[i] : ' ');
@ -115,24 +161,288 @@ int SSwrite(const uint8_t *buf, int len){
return buflen; return buflen;
} }
// write simple command with string answer
int SSwritecmd(const uint8_t *cmd){
return SSwrite(cmd, (int)strlen((const char*)cmd), 0);
}
// write command and get answer
int SSgetInt(const uint8_t *cmd, int64_t *ans){
int r = SSwritecmd(cmd);
if(r > 0 && ans){
int l = 0;
char *ptr = (char*) SSread(&l);
ptr[l] = 0;
while(l){
if(isdigit(*ptr)) break;
++ptr; ++l;
}
if(l == 0) return 0; // no integer found
*ans = atol(ptr);
DBG("from %s get answer: %ld", buff, *ans);
}
return r;
}
/** /**
* @brief SSread - return buff and buflen * @brief SSread - return buff and buflen
* @param l (o) - length of data * @param l (o) - length of data
* @return buff or NULL if buflen == 0 * @return buff or NULL if buflen == 0
*/ */
uint8_t *SSread(int *l){ uint8_t *SSread(int *l){
if(l) *l = buflen;
if(!buflen) return NULL; if(!buflen) return NULL;
if(l) *l = buflen;
return buff; return buff;
} }
// fill some fields of `s` by stupid string commands
int SSgetPartialstat(SSstat *s){
SSstat st = {0};
int64_t par;
int r = 0;
#define GET(cmd, field) do{if(SSgetInt(cmd, &par)){st.field = (int32_t)par; ++r;}}while(0)
GET(CMD_MOTDEC, DECmot);
GET(CMD_MOTHA, HAmot);
GET(CMD_ENCDEC, DECenc);
GET(CMD_ENCHA, HAenc);
#undef GET
if(s) *s = st;
#ifdef EBUG
if(s){
green("\nGet data:\n");
printf("DECmot=%d (0x%08x)\n", s->DECmot, (uint32_t)s->DECmot);
printf("DECenc=%d (0x%08x)\n", s->DECenc, (uint32_t)s->DECenc);
printf("RAmot=%d (0x%08x)\n", s->HAmot, (uint32_t)s->HAmot);
printf("RAenc=%d (0x%08x)\n", s->HAenc, (uint32_t)s->HAenc);
}
#endif
return r; // amount of fields with good data
}
static void parsestat(SSstat *s){
green("\nGet data:\n");
printf("DECmot=%d (0x%08x)\n", s->DECmot, (uint32_t)s->DECmot);
printf("DECenc=%d (0x%08x)\n", s->DECenc, (uint32_t)s->DECenc);
printf("DECLast=%d (0x%08x)\n", s->DecLast, (uint32_t)s->DecLast);
printf("RAmot=%d (0x%08x)\n", s->HAmot, (uint32_t)s->HAmot);
printf("RAenc=%d (0x%08x)\n", s->HAenc, (uint32_t)s->HAenc);
printf("RALast=%d (0x%08x)\n", s->HALast, (uint32_t)s->HALast);
printf("keypad=0x%02x\n", s->keypad);
printf("XBits=0x%02x\n", s->XBits);
printf("YBits=0x%02x\n", s->YBits);
printf("EBits=0x%02x\n", s->ExtraBits);
printf("ain0=%u\n", s->ain0);
printf("ain1=%u\n", s->ain1);
printf("millis=%u\n", s->millis);
printf("tF=%d\n", s->tF);
printf("V=%f\n", ((float)s->voltage)/10.f);
printf("checksum=0x%04x\n\n", s->checksum);
}
/** /**
* @brief SSgetstat - get struct with status & check its crc * @brief SSgetstat - get struct with status & check its crc
* @param s - pointer to allocated struct (or NULL just to check) * @param s - pointer to allocated struct (or NULL just to check)
* @return 1 if OK * @return 1 if OK
*/ */
int SSgetstat(SSstat *s){ int SSgetstat(SSstat *s){
if(SSwrite(CMD_GETSTAT, sizeof(CMD_GETSTAT)) != sizeof(SSstat)) return FALSE; int r = SSwrite(CMD_GETSTAT, 3, 1);
if(s) memcpy(s, buff, sizeof(SSstat)); SSstat *n;
switch(r){
case sizeof(SSstat):
n = (SSstat*) buff;
if(calcChecksum(0, buff, sizeof(SSstat)-2) != n->checksum) return FALSE;
if(s) memcpy(s, buff, sizeof(SSstat));
break;
default: return FALSE; // wrong answer size
}
#ifdef EBUG
if(s){
parsestat(s);
}
#endif
return TRUE; return TRUE;
} }
// send short/long binary command
static int bincmd(void *cmd, int len){
//uint8_t *buf = MALLOC(uint8_t, len + 3);
if(len == sizeof(SSscmd)){
((SSscmd*)cmd)->checksum = calcChecksum(0, cmd, len-2);
SSwrite(CMD_SHORTCMD, 3, -1);
//memcpy(buf, CMD_SHORTCMD, 3);
//memcpy(buf+3, cmd, len);
}else if(len == sizeof(SSlcmd)){
((SSlcmd*)cmd)->checksum = calcChecksum(0, cmd, len-2);
SSwrite(CMD_LONGCMD, 3, -1);
//memcpy(buf, CMD_LONGCMD, 3);
//memcpy(buf+3, cmd, len);
}else{
//free(buf);
return -1;
}
//len += 3;
DBG("Write %d bytes", len);
int r = SSwrite(cmd, len, 1);
#ifdef EBUG
if(r == sizeof(SSscmd)) parsestat((SSstat*)buff);
#endif
return r;
}
int SScmds(SSscmd *cmd){
int r = bincmd(cmd, sizeof(SSscmd));
if(r < 0) return -1;
DBG("Got %d bytes answer", r);
return r;
}
int SScmdl(SSlcmd *cmd){
int r = bincmd(cmd, sizeof(SSlcmd));
if(r < 0) return -1;
DBG("Got %d bytes answer", r);
return r;
}
// return d in 0..360 for isdec==1 and -180..180 for isdec==0
static double normangle(double d, int isdec){
if(d < -360.) d = fmod((fmod(d, 360.) + 360.), 360.);
else d = fmod(d + 360., 360.);
if(isdec) return d;
if(d < 180.) return d;
return (d - 180.);
}
/**
* @brief SSticks2deg - convert motor ticks to degrees
* @param ticks - motor ticks
* @param isdec == 1 if it is DEC axe
* @return degrees
*/
double SSticks2deg(int32_t ticks, int isdec){
double denom = (isdec) ? encsettings.decmotperrev : encsettings.hamotperrev;
int32_t zp = (isdec) ? encsettings.decmotzero : encsettings.hamotzero;
return normangle(360. * (ticks - zp) / denom, isdec);
}
/**
* @brief SSenc2deg - convert encoder ticks to degrees
* @param ticks - motor ticks
* @param isdec == 1 if it is DEC axe
* @return degrees
*/
double SSenc2deg(int32_t ticks, int isdec){
double denom = (isdec) ? encsettings.decencperrev : encsettings.haencperrev;
int32_t zp = (isdec) ? DEC_ENC_ZEROPOS : HA_ENC_ZEROPOS;
return normangle(360. * (ticks - zp) / denom, isdec);
}
/**
* @brief SSdeg2ticks - convert degrees to motor ticks
* @param d - angle
* @param isdec == 1 if it is DEC axe
* @return ticks
*/
int32_t SSdeg2ticks(double d, int isdec){
double k = (isdec) ? encsettings.decmotperrev : encsettings.hamotperrev;
int32_t zp = (isdec) ? encsettings.decmotzero : encsettings.hamotzero;
return (int32_t)(k * normangle(d, isdec) / 360.) + zp;
}
/**
* @brief SSdeg2enc - convert degrees to encoder ticks
* @param d - angle
* @param isdec == 1 if it is DEC axe
* @return ticks
*/
int32_t SSdeg2enc(double d, int isdec){
double k = (isdec) ? encsettings.decencperrev : encsettings.haencperrev;
int32_t zp = (isdec) ? DEC_ENC_ZEROPOS : HA_ENC_ZEROPOS;
return (int32_t)(k * normangle(d, isdec) / 360.) + zp;
}
// convert speed `dps` in degrees per second into ticks for `XS`/`YS` commands
int32_t SSdeg2spd(double dps, int isdec){
double k = (isdec) ? encsettings.decmotperrev : encsettings.hamotperrev;
return (int32_t)(k * dps * 65536./1953./360.);
}
// convert ticks for `XS`/`YS` into degrees per second
double SSspd2deg(int32_t ticks, int isdec){
;
}
/**
* @brief SSgoto - move telescope to given angle
* @param ha - Hour Angle (degrees)
* @param dec - Declination (degrees)
* @return TRUE if OK
*/
int SSgoto(double ha, double dec){
char buf[32];
if(ha < 0. || ha > 360.) return FALSE;
if(dec < -90. || dec > 90.) return FALSE;
int32_t raticks = SSdeg2ticks(ha, 0);
int32_t decticks = SSdeg2ticks(dec, 1);
snprintf(buf, 31, "%s%u", CMD_MOTDEC, decticks);
SSwritecmd((uint8_t*)buf);
snprintf(buf, 31, "%s%u", CMD_MOTHA, raticks);
SSwritecmd((uint8_t*)buf);
return TRUE;
}
// wait till moving ends
void SSwaitmoving(){
SSstat s = {0};
double t0 = dtime();
int32_t oldRAm = 0, oldDm = 0;
int first = 1, ctr = 0;
do{
if(!SSlog_motor_data(&s, &t0)) continue;
DBG("Moving: HA=%g, DEC=%g", SSenc2deg(s.HAenc, 0), SSenc2deg(s.DECenc, 1));
if(first) first = 0;
else if(s.HAmot == oldRAm && s.DECmot == oldDm){
if(++ctr > 2) return;
}else ctr = 0;
oldRAm = s.HAmot;
oldDm = s.DECmot;
}while(dtime() - t0 < 3.); // simplest timeout
DBG("Moving ends (or timeout ends)");
}
int SScatchtarg(double ra, double dec){
;
}
#if 0
The controller loops 1953 times per second. Fields that reference a speed/velocity/rate (e.g. the XS command) express this speed as a 32-bit number representing the number of ticks the motor encoder should advance per loop, multiplied by 2^16 (= 65,536) to express fractional values.
For example, if you want to advance the motor 1,000 counts per second, this corresponds to:
1,000 / 1953 ~= 0.512 counts per loop
which is expressed as the integer value:
round(0.512 * 65,536) = 33,555
So you would send the command ?XS33555\r? to set a max speed of 1,000 counts per second.
Note that 65,536 / 1,953 = 33.55657962109575, and 1,953 / 65,536 = 0.0298004150390625, so you can simply do:
CountsPerSecToSpeedValue(cps): return round(cps * 33.55657962109575)
SpeedValueToCountsPerSec(speed): return round(speed * 0.0298004150390625)
The SiTech Operations Manual describes the following ?C++? functions (converted to something like Python here) to do similar conversions:
def DegsPerSec2MotorSpeed(dps, ticksPerRev):
return round(ticksPerRev * dps * 0.09321272116971)
def MotorSpeed2DegsPerSec(speed, ticksPerRev):
return round(speed / ticksPerRev * 10.7281494140625)
These coefficients can be derived as follows:
10.7281494140625 = 0.0298004150390625 * 360 degs/rev
0.09321272116971 = 33.55657962109575 / 360 degs/rev
#endif

110
Auxiliary_utils/SiderealServoII/sidservo.h
View File

@ -20,42 +20,106 @@
#ifndef SIDSERVO_H__ #ifndef SIDSERVO_H__
#define SIDSERVO_H__ #define SIDSERVO_H__
#include <usefull_macros.h> #include <stdint.h>
// ASCII commands // ASCII commands
#define U8P(x) ((uint8_t*)x) #define U8P(x) ((uint8_t*)x)
// get binary data of all statistics // get binary data of all statistics
#define CMD_GETSTAT U8P("XXS") #define CMD_GETSTAT U8P("XXS")
// send short command
#define CMD_SHORTCMD U8P("XXR")
// send long command
#define CMD_LONGCMD U8P("YXR")
// get/set HA/DEC in motsteps
#define CMD_MOTDEC U8P("X")
#define CMD_MOTHA U8P("Y")
// -//- in encoders' ticks
#define CMD_ENCDEC U8P("XZ")
#define CMD_ENCHA U8P("YZ")
// normal stop dec/ra
#define CMD_STOPDEC U8P("XN")
#define CMD_STOPHA U8P("YN")
// emergency stop
#define CMD_EMSTOPDEC U8P("XG")
#define CMD_EMSTOPHA U8P("YG")
// getters of motor's encoders per rev
#define CMD_GETDECMEPR U8P("XXU")
#define CMD_GETHAMEPR U8P("XXV")
// -//- axis encoders
#define CMD_GETDECAEPR U8P("XXT")
#define CMD_GETHAAEPR U8P("XXZ")
#define BUFLEN (256) #define BUFLEN (256)
// timeout (seconds) of reading answer // timeout (seconds) of reading answer (from last symbol read)
#define READTIMEOUT (0.1) #define READTIMEOUT (0.05)
// Zero positions of RA/DEC encoderc
#define HA_ENC_ZEROPOS (43066232)
#define DEC_ENC_ZEROPOS (37282120)
// all need data in one // all need data in one
typedef struct{ typedef struct{ // 41 bytes
int32_t DECmot; // Dec/RA motor position uint8_t ctrlAddr; // 0 a8 + controller address
int32_t RAmot; int32_t DECmot; // 1 Dec/HA motor position
int32_t DECenc; // Dec/RA encoder position int32_t HAmot; // 5
int32_t RAenc; int32_t DECenc; // 9 Dec/HA encoder position
uint8_t keypad; // keypad status int32_t HAenc; // 13
uint8_t XBits; uint8_t keypad; // 17 keypad status
uint8_t YBits; uint8_t XBits; // 18
uint8_t ExtraBits; uint8_t YBits; // 19
uint16_t ain0; // analog inputs uint8_t ExtraBits; // 20
uint16_t ain1; uint16_t ain0; // 21 analog inputs
uint32_t millis; // milliseconds clock uint16_t ain1; // 23
int8_t tF; // temperature (degF) uint32_t millis; // 25 milliseconds clock
uint8_t voltage; // input voltage *10 int8_t tF; // 29 temperature (degF)
uint32_t reserved0; uint8_t voltage; // 30 input voltage *10 (RA worm phase?)
uint32_t reserved1; uint32_t DecLast; // 31 Alt/Dec motor location at last Alt/Dec scope encoder location change
uint16_t checksum; // checksum, H inverted uint32_t HALast; // 35 Az/RA motor location at last Az/RA scope encoder location change
uint16_t checksum; // 39 checksum, H inverted
}__attribute__((packed)) SSstat; }__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
uint8_t xychange; // 16 change Xbits/Ybits value
uint8_t XBits; // 17
uint8_t YBits; // 18
uint16_t checksum; // 19
} __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
uint16_t checksum; // 32
} __attribute__((packed)) SSlcmd; // long command
int SSinit(char *devpath, int speed); int SSinit(char *devpath, int speed);
void SSclose(); void SSclose();
int SSwrite(const uint8_t *buf, int len); int SSwrite(const uint8_t *buf, int len, int isbin);
int SSwritecmd(const uint8_t *cmd);
int SSgetInt(const uint8_t *cmd, int64_t *ans);
int SScmds(SSscmd *cmd);
int SScmdl(SSlcmd *cmd);
uint8_t *SSread(int *l); uint8_t *SSread(int *l);
int SSgetstat(SSstat *s); int SSgetstat(SSstat *s);
int SSgetPartialstat(SSstat *s);
double SSticks2deg(int32_t ticks, int isdec);
double SSenc2deg(int32_t ticks, int isdec);
int32_t SSdeg2ticks(double d, int isdec);
int32_t SSdeg2enc(double d, int isdec);
int SSgoto(double ha, double dec);
void SSwaitmoving();
int SScatchtarg(double ra, double dec);
#endif // SIDSERVO_H__ #endif // SIDSERVO_H__