start configurator

Auxiliary_utils/LibSidServo/examples/CMakeLists.txt

@@ -5,8 +5,9 @@ include_directories(../)
 link_libraries(sidservo usefull_macros -lm)
 
 # exe list
-add_executable(goto goto.c dump.c)
+add_executable(goto goto.c dump.c conf.c)
-add_executable(dump dumpmoving.c dump.c)
+add_executable(dump dumpmoving.c dump.c conf.c)
-add_executable(dump_s dumpmoving_scmd.c dump.c)
+add_executable(dump_s dumpmoving_scmd.c dump.c conf.c)
-add_executable(dumpswing dumpswing.c dump.c)
+add_executable(dumpswing dumpswing.c dump.c conf.c)
-add_executable(traectory_s scmd_traectory.c dump.c traectories.c)
+add_executable(traectory_s scmd_traectory.c dump.c traectories.c conf.c)
+add_executable(SSIIconf SSIIconf.c conf.c)

Auxiliary_utils/LibSidServo/examples/Readme.md

@@ -0,0 +1,28 @@
+Some examples of usage of libsidservo
+=====================================
+
+## Auxiliary files
+
+*conf.c*, *conf.h* - base configuration - read from file (default: servo.conf) - to simplify examples running when config changes
+
+*dump.c*, *dump.h* - base logging and dumping functions, also some useful functions like get current position and move to zero if current position isn't at zero.
+
+*traectories.c*, *traectories.h* - modeling simple moving object traectories; also some functions like get current position in encoders' angles setting to zero at motors' zero.
+
+*simpleconv.h*
+
+
+## Examples
+
+*dumpmoving.c* (`dump`) - dump moving relative starting point by simplest text commands "X" and "Y".
+
+*dumpmoving_scmd.c* (`dump_s`) - moving relative starting point using "short" binary command.
+
+*dumpswing.c* (`dumpswing`) - shake telescope around starting point by one of axis.
+
+*goto.c* (`goto`) - get current coordinates or go to given (by simplest "X/Y" commands).
+
+*scmd_traectory.c* (`traectory_s`) - try to move around given traectory using "short" binary commands.
+
+*SSIIconf.c* (`SSIIconf`) - read/write hardware configuration of controller
+

Auxiliary_utils/LibSidServo/examples/SSIIconf.c

@@ -0,0 +1,76 @@
+/*
+ * 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/>.
+ */
+
+#include <stdio.h>
+#include <usefull_macros.h>
+
+#include "conf.h"
+#include "sidservo.h"
+#include "simpleconv.h"
+
+typedef struct{
+    int help;
+    int helpargs;
+    int writeconf;
+    char *conffile;
+    char *hwconffile;
+} parameters;
+
+static hardware_configuration_t HW = {0};
+
+static parameters G = {0};
+
+static sl_option_t cmdlnopts[] = {
+    {"help",        NO_ARGS,    NULL,   'h',    arg_int,    APTR(&G.help),      "show this help"},
+    {"help-opts",   NO_ARGS,    NULL,   'H',    arg_int,    APTR(&G.helpargs),  "configuration help"},
+    {"serconf",     NEED_ARG,   NULL,   'C',    arg_string, APTR(&G.conffile),  "serial configuration file name"},
+    {"hwconf",      NEED_ARG,   NULL,   'i',    arg_string, APTR(&G.hwconffile),"SSII configuration file name"},
+    {"writeconf",   NO_ARGS,    NULL,   0,      arg_int,    APTR(&G.writeconf), "write configuration (BE CAREFUL!)"},
+    end_option
+};
+
+static sl_option_t confopts[] = {
+    {"Xaccel",      NEED_ARG,   NULL,   0,      arg_double, APTR(&HW.Xconf.accel),  "X Default Acceleration, rad/s^2"},
+    {"Yaccel",      NEED_ARG,   NULL,   0,      arg_double, APTR(&HW.Yconf.accel),  "Y Default Acceleration, rad/s^2"},
+    end_option
+};
+
+int main(int argc, char** argv){
+    sl_init();
+    sl_parseargs(&argc, &argv, cmdlnopts);
+    if(G.help)
+        sl_showhelp(-1, cmdlnopts);
+    if(G.helpargs)
+        sl_showhelp(-1, confopts);
+    conf_t *sconf = readServoConf(G.conffile);
+    if(!sconf){
+        dumpConf();
+        return 1;
+    }
+    if(MCC_E_OK != Mount.init(sconf)) ERRX("Can't init mount");
+    if(MCC_E_OK != Mount.getHWconfig(&HW)) ERRX("Can't read configuration");
+    char *c = sl_print_opts(confopts, TRUE);
+    green("Got configuration:\n");
+    printf("%s\n", c);
+    FREE(c);
+    if(G.hwconffile && G.writeconf){
+        ;
+    }
+    Mount.quit();
+    return 0;
+}

Auxiliary_utils/LibSidServo/examples/conf.c

@@ -0,0 +1,65 @@
+/*
+ * 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/>.
+ */
+
+#include <stdio.h>
+#include <usefull_macros.h>
+
+#include "conf.h"
+
+static conf_t Config = {
+    .MountDevPath = "/dev/ttyUSB0",
+    .MountDevSpeed = 19200,
+    .EncoderDevPath = "/dev/ttyUSB1",
+    .EncoderDevSpeed = 153000,
+    .MountReqInterval = 0.1,
+    .SepEncoder = 1
+};
+
+static sl_option_t opts[] = {
+    {"MountDevPath",    NEED_ARG,   NULL,   0,  arg_string, APTR(&Config.MountDevPath),     "path to mount device"},
+    {"MountDevSpeed",   NEED_ARG,   NULL,   0,  arg_int,    APTR(&Config.MountDevSpeed),    "serial speed of mount device"},
+    {"EncoderDevPath",  NEED_ARG,   NULL,   0,  arg_string, APTR(&Config.EncoderDevPath),   "path to encoder device"},
+    {"EncoderDevSpeed", NEED_ARG,   NULL,   0,  arg_int,    APTR(&Config.EncoderDevSpeed),  "serial speed of encoder device"},
+    {"MountReqInterval",NEED_ARG,   NULL,   0,  arg_double, APTR(&Config.MountReqInterval), "interval of mount requests (not less than 0.05s)"},
+    {"SepEncoder",      NO_ARGS,    NULL,   0,  arg_int,    APTR(&Config.SepEncoder),       "encoder is separate device"},
+    end_option
+};
+
+conf_t *readServoConf(const char *filename){
+    if(!filename) filename = DEFCONFFILE;
+    int n = sl_conf_readopts(filename, opts);
+    if(n < 0){
+        WARNX("Can't read file %s", filename);
+        return NULL;
+    }
+    if(n == 0){
+        WARNX("Got ZERO parameters from %s", filename);
+        return NULL;
+    }
+    return &Config;
+}
+
+void dumpConf(){
+    char *c = sl_print_opts(opts, TRUE);
+    printf("Current configuration:\n%s\n", c);
+    FREE(c);
+}
+
+void confHelp(){
+    sl_showhelp(-1, opts);
+}

Auxiliary_utils/LibSidServo/examples/conf.h

@@ -0,0 +1,27 @@
+/*
+ * 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/>.
+ */
+
+#pragma once
+
+#include "sidservo.h"
+
+#define DEFCONFFILE "servo.conf"
+
+void confHelp();
+conf_t *readServoConf(const char *filename);
+void dumpConf();

Auxiliary_utils/LibSidServo/examples/dump.c

@@ -36,8 +36,8 @@ void logmnt(FILE *fcoords, mountdata_t *m){
         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;
+    if(t0 < 0.) t0 = m->encposition.msrtime.tv_sec + (double)(m->encposition.msrtime.tv_usec) / 1e6;
-    double t = m->motposition.msrtime.tv_sec + (double)(m->motposition.msrtime.tv_usec) / 1e6 - t0;
+    double t = m->encposition.msrtime.tv_sec + (double)(m->encposition.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),
@@ -64,17 +64,17 @@ void dumpmoving(FILE *fcoords, double t, int N){
         WARNX("Can't get mount data");
         LOGWARN("Can't get mount data");
     }
-    uint32_t millis = mdata.millis;
+    uint32_t millis = mdata.encposition.msrtime.tv_usec;
     int ctr = -1;
     double xlast = mdata.motposition.X, ylast = mdata.motposition.Y;
     double t0 = sl_dtime();
     //DBG("millis = %u", millis);
     while(sl_dtime() - t0 < t && ctr < N){
-        usleep(10000);
+        usleep(1000);
         if(MCC_E_OK != Mount.getMountData(&mdata)){ WARNX("Can't get data"); continue;}
-        if(mdata.millis == millis) continue;
+        if(mdata.encposition.msrtime.tv_usec == millis) continue;
         //DBG("Got new data, posX=%g, posY=%g", mdata.motposition.X, mdata.motposition.Y);
-        millis = mdata.millis;
+        millis = mdata.encposition.msrtime.tv_usec;
         if(fcoords) logmnt(fcoords, &mdata);
         if(mdata.motposition.X != xlast || mdata.motposition.Y != ylast){
             xlast = mdata.motposition.X;
@@ -107,13 +107,13 @@ void waitmoving(int N){
 }
 
 /**
- * @brief getMotPos - get current
+ * @brief getPos - get current position
  * @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;
+    mountdata_t mdata = {0};
     int errcnt = 0;
     do{
         if(MCC_E_OK != Mount.getMountData(&mdata)) ++errcnt;
@@ -137,7 +137,8 @@ void chk0(int ncycles){
     if(!getPos(&M, NULL)) signals(2);
     if(M.X || M.Y){
         WARNX("Mount position isn't @ zero; moving");
-        Mount.moveTo(0., 0.);
+        double zero = 0.;
+        Mount.moveTo(&zero, &zero);
         waitmoving(ncycles);
         green("Now mount @ zero\n");
     }

Auxiliary_utils/LibSidServo/examples/dumpmoving.c

@@ -24,6 +24,7 @@
 #include <time.h>
 #include <usefull_macros.h>
 
+#include "conf.h"
 #include "dump.h"
 #include "sidservo.h"
 #include "simpleconv.h"
@@ -34,6 +35,7 @@ typedef struct{
     int Ncycles;
     char *logfile;
     char *coordsoutput;
+    char *conffile;
 } parameters;
 
 static parameters G = {
@@ -47,6 +49,7 @@ static sl_option_t cmdlnopts[] = {
     {"logfile",     NEED_ARG,   NULL,   'l',    arg_string, APTR(&G.logfile),   "log file name"},
     {"ncycles",     NEED_ARG,   NULL,   'n',    arg_int,    APTR(&G.Ncycles),   "N cycles in stopped state (default: 40)"},
     {"coordsfile",  NEED_ARG,   NULL,   'o',    arg_string, APTR(&G.coordsoutput),"output file with coordinates log"},
+    {"conffile",    NEED_ARG,   NULL,   'C',    arg_string, APTR(&G.conffile),  "configuration file name"},
     end_option
 };
 
@@ -60,15 +63,6 @@ void signals(int sig){
     exit(sig);
 }
 
-static conf_t Config = {
-    .MountDevPath = "/dev/ttyUSB0",
-    .MountDevSpeed = 19200,
-    //.EncoderDevPath = "/dev/ttyUSB1",
-    //.EncoderDevSpeed = 153000,
-    .MountReqInterval = 0.1,
-    .SepEncoder = 0
-};
-
 int main(int argc, char **argv){
     sl_init();
     sl_parseargs(&argc, &argv, cmdlnopts);
@@ -76,6 +70,11 @@ 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);
+    conf_t *Config = readServoConf(G.conffile);
+    if(!Config){
+        dumpConf();
+        return 1;
+    }
     if(G.coordsoutput){
         if(!(fcoords = fopen(G.coordsoutput, "w")))
             ERRX("Can't open %s", G.coordsoutput);
@@ -83,22 +82,21 @@ int main(int argc, char **argv){
     logmnt(fcoords, NULL);
     time_t curtime = time(NULL);
     LOGMSG("Started @ %s", ctime(&curtime));
-    LOGMSG("Mount device %s @ %d", Config.MountDevPath, Config.MountDevSpeed);
+    LOGMSG("Mount device %s @ %d", Config->MountDevPath, Config->MountDevSpeed);
-    LOGMSG("Encoder device %s @ %d", Config.EncoderDevPath, Config.EncoderDevSpeed);
+    LOGMSG("Encoder device %s @ %d", Config->EncoderDevPath, Config->EncoderDevSpeed);
-    mcc_errcodes_t e = Mount.init(&Config);
+    if(MCC_E_OK != Mount.init(Config)) ERRX("Can't init devices");
-    if(e != MCC_E_OK){
+    coords_t M;
-        WARNX("Can't init devices");
+    if(!getPos(&M, NULL)) ERRX("Can't get current position");
-        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
-    if(MCC_E_OK != Mount.moveTo(DEG2RAD(45.), DEG2RAD(45.)))
+    double tagx = DEG2RAD(45.) + M.X, tagy = DEG2RAD(45.) + M.Y;
+    if(MCC_E_OK != Mount.moveTo(&tagx, &tagy))
         ERRX("Can't move to 45, 45");
     dumpmoving(fcoords, 30., G.Ncycles);
-    Mount.moveTo(0., 0.);
+    Mount.moveTo(&M.X, &M.Y);
     dumpmoving(fcoords, 30., G.Ncycles);
     signals(0);
     return 0;

Auxiliary_utils/LibSidServo/examples/dumpmoving_scmd.c

@@ -26,6 +26,7 @@
 #include <time.h>
 #include <usefull_macros.h>
 
+#include "conf.h"
 #include "dump.h"
 #include "sidservo.h"
 #include "simpleconv.h"
@@ -33,24 +34,31 @@
 typedef struct{
     int help;
     int Ncycles;
+    int relative;
     double reqint;
     char *coordsoutput;
+    char *conffile;
     char *axis;
 } parameters;
 
 static parameters G = {
     .Ncycles = 40,
-    .reqint = 0.1,
+    .reqint = -1.,
     .axis = "X",
 };
+
 static FILE *fcoords = NULL;
 
+static coords_t M;
+
 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 in stopped state (default: 40)"},
     {"coordsfile",  NEED_ARG,   NULL,   'o',    arg_string, APTR(&G.coordsoutput),"output file with coordinates log"},
     {"reqinterval", NEED_ARG,   NULL,   'i',    arg_double, APTR(&G.reqint),    "mount requests interval (default: 0.1)"},
-    {"axis",        NEED_ARG,   NULL,   'a',    arg_string, APTR(&G.axis),      "axis to move (X or Y)"},
+    {"axis",        NEED_ARG,   NULL,   'a',    arg_string, APTR(&G.axis),      "axis to move (X, Y or B for both)"},
+    {"conffile",    NEED_ARG,   NULL,   'C',    arg_string, APTR(&G.conffile),  "configuration file name"},
+    {"relative",    NO_ARGS,    NULL,   'r',    arg_int,    APTR(&G.relative),  "relative move"},
     end_option
 };
 
@@ -63,15 +71,6 @@ void signals(int sig){
     exit(sig);
 }
 
-static conf_t Config = {
-    .MountDevPath = "/dev/ttyUSB0",
-    .MountDevSpeed = 19200,
-    //.EncoderDevPath = "/dev/ttyUSB1",
-    //.EncoderDevSpeed = 153000,
-    .MountReqInterval = 0.1,
-    .SepEncoder = 0
-};
-
 // dump thread
 static void *dumping(void _U_ *u){
     dumpmoving(fcoords, 3600., G.Ncycles);
@@ -108,20 +107,23 @@ static int Wait(double tag){
     return TRUE;
 }
 
-// move X to 40 degr with given speed until given coord
+// move X/Y to 40 degr with given speed until given coord
 static void move(double target, double limit, double speed){
 #define SCMD()   do{if(MCC_E_OK != Mount.shortCmd(&cmd)) ERRX("Can't run command"); }while(0)
     green("Move %s to %g until %g with %gdeg/s\n", G.axis, target, limit, speed);
     short_command_t cmd = {0};
-    if(*G.axis == 'X'){
+    if(*G.axis == 'X' || *G.axis == 'B'){
-        cmd.Xmot = DEG2RAD(target);
+        cmd.Xmot = DEG2RAD(target) + M.X;
         cmd.Xspeed = DEG2RAD(speed);
-    }else{
+        limit = DEG2RAD(limit) + M.X;
-        cmd.Ymot = DEG2RAD(target);
+    }
+    if(*G.axis == 'Y' || *G.axis == 'B'){
+        cmd.Ymot = DEG2RAD(target) + M.Y;
         cmd.Yspeed = DEG2RAD(speed);
+        limit = DEG2RAD(limit) + M.Y;
     }
     SCMD();
-    if(!Wait(DEG2RAD(limit))) signals(9);
+    if(!Wait(limit)) signals(9);
 #undef SCMD
 }
 
@@ -130,20 +132,25 @@ int main(int argc, char **argv){
     sl_init();
     sl_parseargs(&argc, &argv, cmdlnopts);
     if(G.help) sl_showhelp(-1, cmdlnopts);
-    if(strcmp(G.axis, "X") && strcmp(G.axis, "Y")){
+    if(strcmp(G.axis, "X") && strcmp(G.axis, "Y") && strcmp(G.axis, "B")){
-        WARNX("\"Axis\" should be X or Y");
+        WARNX("\"Axis\" should be X, Y or B");
         return 1;
     }
     if(G.coordsoutput){
         if(!(fcoords = fopen(G.coordsoutput, "w")))
             ERRX("Can't open %s", G.coordsoutput);
     }else fcoords = stdout;
-    Config.MountReqInterval = G.reqint;
+    conf_t *Config = readServoConf(G.conffile);
-    mcc_errcodes_t e = Mount.init(&Config);
+    if(!Config){
-    if(e != MCC_E_OK){
+        dumpConf();
+        return 1;
+    }
+    if(G.reqint > 0.) Config->MountReqInterval = G.reqint;
+    if(MCC_E_OK != Mount.init(Config)){
         WARNX("Can't init devices");
         return 1;
     }
+    if(!getPos(&M, NULL)) ERRX("Can't get current position");
     signal(SIGTERM, signals); // kill (-15) - quit
     signal(SIGHUP, SIG_IGN);  // hup - ignore
     signal(SIGINT, signals);  // ctrl+C - quit
@@ -151,7 +158,6 @@ int main(int argc, char **argv){
     signal(SIGTSTP, SIG_IGN); // ignore ctrl+Z
     // move to X=40 degr with different speeds
     pthread_t dthr;
-    chk0(G.Ncycles);
     logmnt(fcoords, NULL);
     if(pthread_create(&dthr, NULL, dumping, NULL)) ERRX("Can't run dump thread");
     // goto 1 degr with 1'/s
@@ -165,7 +171,7 @@ int main(int argc, char **argv){
     // and go back with 5deg/s
     move(0., 0., 5.);
     // be sure to move @ 0,0
-    Mount.moveTo(0., 0.);
+    Mount.moveTo(&M.X, &M.Y);
     // wait moving ends
     pthread_join(dthr, NULL);
 #undef SCMD

Auxiliary_utils/LibSidServo/examples/dumpswing.c

@@ -22,6 +22,7 @@
 #include <string.h>
 #include <usefull_macros.h>
 
+#include "conf.h"
 #include "dump.h"
 #include "sidservo.h"
 #include "simpleconv.h"
@@ -35,6 +36,7 @@ typedef struct{
     double period;
     double amplitude;
     char *coordsoutput;
+    char *conffile;
     char *axis;
 } parameters;
 
@@ -55,6 +57,7 @@ static sl_option_t cmdlnopts[] = {
     {"period",      NEED_ARG,   NULL,   'p',    arg_double, APTR(&G.period),    "swinging period (could be not reached if amplitude is too small) - not more than 900s (default: 1)"},
     {"amplitude",   NEED_ARG,   NULL,   'A',    arg_double, APTR(&G.amplitude), "max amplitude (could be not reaced if period is too small) - not more than 45deg (default: 5)"},
     {"nswings",     NEED_ARG,   NULL,   'N',    arg_int,    APTR(&G.Nswings),   "amount of swing periods (default: 10)"},
+    {"conffile",    NEED_ARG,   NULL,   'C',    arg_int,    APTR(&G.conffile),  "configuration file name"},
     end_option
 };
 
@@ -67,15 +70,6 @@ void signals(int sig){
     exit(sig);
 }
 
-static conf_t Config = {
-    .MountDevPath = "/dev/ttyUSB0",
-    .MountDevSpeed = 19200,
-    //.EncoderDevPath = "/dev/ttyUSB1",
-    //.EncoderDevSpeed = 153000,
-    .MountReqInterval = 0.05,
-    .SepEncoder = 0
-};
-
 // dump thread
 static void *dumping(void _U_ *u){
     dumpmoving(fcoords, 3600., G.Ncycles);
@@ -125,7 +119,12 @@ int main(int argc, char **argv){
     if(G.period < 0.1 || G.period > 900.)
         ERRX("Period should be from 0.1 to 900s");
     if(G.Nswings < 1) ERRX("Nswings should be more than 0");
-    mcc_errcodes_t e = Mount.init(&Config);
+    conf_t *Config = readServoConf(G.conffile);
+    if(!Config){
+        dumpConf();
+        return 1;
+    }
+    mcc_errcodes_t e = Mount.init(Config);
     if(e != MCC_E_OK){
         WARNX("Can't init devices");
         return 1;
@@ -147,23 +146,24 @@ int main(int argc, char **argv){
         tagX = 0.; tagY = DEG2RAD(G.amplitude);
     }
     double t = sl_dtime(), t0 = t;
-    double divide = 2.;
+    double divide = 2., rtagX = -tagX, rtagY = -tagY;
     for(int i = 0; i < G.Nswings; ++i){
-        Mount.moveTo(tagX, tagY);
+        Mount.moveTo(&tagX, &tagY);
         DBG("CMD: %g", sl_dtime()-t0);
         t += G.period / divide;
         divide = 1.;
         waithalf(t);
         DBG("Moved to +, t=%g", t-t0);
         DBG("CMD: %g", sl_dtime()-t0);
-        Mount.moveTo(-tagX, -tagY);
+        Mount.moveTo(&rtagX, &rtagY);
         t += G.period;
         waithalf(t);
         DBG("Moved to -, t=%g", t-t0);
         DBG("CMD: %g", sl_dtime()-t0);
     }
+    double zero = 0.;
     // be sure to move @ 0,0
-    Mount.moveTo(0., 0.);
+    Mount.moveTo(&zero, &zero);
     // wait moving ends
     pthread_join(dthr, NULL);
 #undef SCMD

Auxiliary_utils/LibSidServo/examples/goto.c

@@ -23,6 +23,7 @@
 #include <time.h>
 #include <usefull_macros.h>
 
+#include "conf.h"
 #include "dump.h"
 #include "sidservo.h"
 #include "simpleconv.h"
@@ -31,7 +32,9 @@ typedef struct{
     int help;
     int Ncycles;
     int wait;
+    int relative;
     char *coordsoutput;
+    char *conffile;
     double X;
     double Y;
 } parameters;
@@ -49,18 +52,11 @@ static sl_option_t cmdlnopts[] = {
     {"newy",        NEED_ARG,   NULL,   'Y',    arg_double, APTR(&G.Y),         "new Y coordinate"},
     {"output",      NEED_ARG,   NULL,   'o',    arg_string, APTR(&G.coordsoutput),"file to log coordinates"},
     {"wait",        NO_ARGS,    NULL,   'w',    arg_int,    APTR(&G.wait),      "wait until mowing stopped"},
+    {"relative",    NO_ARGS,    NULL,   'r',    arg_int,    APTR(&G.relative),  "relative move"},
+    {"conffile",    NEED_ARG,   NULL,   'C',    arg_string, APTR(&G.conffile),  "configuration file name"},
     end_option
 };
 
-static conf_t Config = {
-    .MountDevPath = "/dev/ttyUSB0",
-    .MountDevSpeed = 19200,
-    //.EncoderDevPath = "/dev/ttyUSB1",
-    //.EncoderDevSpeed = 153000,
-    .MountReqInterval = 0.1,
-    .SepEncoder = 0
-};
-
 static FILE* fcoords = NULL;
 static pthread_t dthr;
 
@@ -84,8 +80,14 @@ static void *dumping(void _U_ *u){
 int main(int _U_ argc, char _U_ **argv){
     sl_init();
     sl_parseargs(&argc, &argv, cmdlnopts);
-    if(G.help) sl_showhelp(-1, cmdlnopts);
+    if(G.help)
-    if(MCC_E_OK != Mount.init(&Config)) ERRX("Can't init mount");
+        sl_showhelp(-1, cmdlnopts);
+    conf_t *Config = readServoConf(G.conffile);
+    if(!Config){
+        dumpConf();
+        return 1;
+    }
+    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");
     if(G.coordsoutput){
@@ -100,10 +102,22 @@ int main(int _U_ argc, char _U_ **argv){
     }
     printf("Mount position: X=%g, Y=%g\n", RAD2DEG(M.X), RAD2DEG(M.Y));
     if(isnan(G.X) && isnan(G.Y)) goto out;
-    if(isnan(G.X)) G.X = RAD2DEG(M.X);
+    double *xtag = NULL, *ytag = NULL, xr, yr;
-    if(isnan(G.Y)) G.Y = RAD2DEG(M.Y);
+    if(!isnan(G.X)){
-    printf("Moving to X=%g deg, Y=%g deg\n", G.X, G.Y);
+        xr = DEG2RAD(G.X);
-    Mount.moveTo(DEG2RAD(G.X), DEG2RAD(G.Y));
+        if(G.relative) xr += M.X;
+        xtag = &xr;
+    }
+    if(!isnan(G.Y)){
+        yr = DEG2RAD(G.Y);
+        if(G.relative) yr += M.Y;
+        ytag = &yr;
+    }
+    printf("Moving to ");
+    if(xtag) printf("X=%gdeg ", G.X);
+    if(ytag) printf("Y=%gdeg", G.Y);
+    printf("\n");
+    Mount.moveTo(xtag, ytag);
     if(G.wait){
         sleep(1);
         waitmoving(G.Ncycles);
@@ -112,6 +126,9 @@ int main(int _U_ argc, char _U_ **argv){
     }
 out:
     if(G.coordsoutput) pthread_join(dthr, NULL);
-    if(G.wait) Mount.quit();
+    if(G.wait){
+        if(getPos(&M, NULL)) printf("Mount position: X=%g, Y=%g\n", RAD2DEG(M.X), RAD2DEG(M.Y));
+        Mount.quit();
+    }
     return 0;
 }

Auxiliary_utils/LibSidServo/examples/scmd_traectory.c

@@ -20,6 +20,7 @@
 #include <signal.h>
 #include <usefull_macros.h>
 
+#include "conf.h"
 #include "dump.h"
 #include "sidservo.h"
 #include "simpleconv.h"
@@ -38,6 +39,7 @@ typedef struct{
     double Y0;          // -//-
     char *coordsoutput; // dump file
     char *tfn;          // traectory function name
+    char *conffile;
 } parameters;
 
 static FILE *fcoords = NULL;
@@ -64,18 +66,10 @@ static sl_option_t cmdlnopts[] = {
     {"tmax",        NEED_ARG,   NULL,   'T',    arg_double, APTR(&G.tmax),      "maximal duration time of emulation (default: 300 seconds)"},
     {"x0",          NEED_ARG,   NULL,   '0',    arg_double, APTR(&G.X0),        "starting X-coordinate of traectory (default: 10 degrees)"},
     {"y0",          NEED_ARG,   NULL,   '1',    arg_double, APTR(&G.Y0),        "starting Y-coordinate of traectory (default: 10 degrees)"},
+    {"conffile",    NEED_ARG,   NULL,   'C',    arg_string, APTR(&G.conffile),  "configuration file name"},
     end_option
 };
 
-static conf_t Config = {
-    .MountDevPath = "/dev/ttyUSB0",
-    .MountDevSpeed = 19200,
-    //.EncoderDevPath = "/dev/ttyUSB1",
-    //.EncoderDevSpeed = 153000,
-    .MountReqInterval = 0.05,
-    .SepEncoder = 0
-};
-
 void signals(int sig){
     pthread_cancel(dthr);
     if(sig){
@@ -127,7 +121,12 @@ int main(int argc, char **argv){
         if(!(fcoords = fopen(G.coordsoutput, "w")))
             ERRX("Can't open %s", G.coordsoutput);
     }else fcoords = stdout;
-    Config.MountReqInterval = G.reqint;
+    conf_t *Config = readServoConf(G.conffile);
+    if(!Config){
+        dumpConf();
+        return 1;
+    }
+    Config->MountReqInterval = G.reqint;
     traectory_fn tfn = traectory_by_name(G.tfn);
     if(!tfn){
         WARNX("Bad traectory name %s, should be one of", G.tfn);
@@ -139,7 +138,7 @@ int main(int argc, char **argv){
         ERRX("Can't init traectory");
         return 1;
     }
-    mcc_errcodes_t e = Mount.init(&Config);
+    mcc_errcodes_t e = Mount.init(Config);
     if(e != MCC_E_OK){
         WARNX("Can't init devices");
         return 1;

Auxiliary_utils/LibSidServo/libsidservo.creator.user

@@ -1,6 +1,6 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <!DOCTYPE QtCreatorProject>
-<!-- Written by QtCreator 15.0.1, 2025-02-19T23:05:54. -->
+<!-- Written by QtCreator 15.0.1, 2025-02-27T22:45:06. -->
 <qtcreator>
  <data>
   <variable>EnvironmentId</variable>

Auxiliary_utils/LibSidServo/libsidservo.files

@@ -1,5 +1,8 @@
 CMakeLists.txt
 dbg.h
+examples/SSIIconf.c
+examples/conf.c
+examples/conf.h
 examples/dump.c
 examples/dump.h
 examples/dumpmoving.c

Auxiliary_utils/LibSidServo/main.c

@@ -16,6 +16,7 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+#include <inttypes.h>
 #include <stdio.h>
 #include <stdlib.h>
 
@@ -30,7 +31,7 @@ conf_t Conf = {0};
  */
 static void quit(){
     DBG("Close serial devices");
-    for(int i = 0; i < 10; ++i) if(SSemergStop()) break;
+    for(int i = 0; i < 10; ++i) if(SSstop(TRUE)) break;
     closeSerial();
     DBG("Exit");
 }
@@ -61,26 +62,101 @@ static mcc_errcodes_t init(conf_t *c){
             ret = MCC_E_ENCODERDEV;
         }
     }
-    if(Conf.MountReqInterval > 1. || Conf.MountReqInterval < 0.001){
+    if(Conf.MountReqInterval > 1. || Conf.MountReqInterval < 0.05){
         DBG("Bad value of MountReqInterval");
         ret = MCC_E_BADFORMAT;
     }
+    uint8_t buf[1024];
+    data_t d = {.buf = buf, .len = 0, .maxlen = 1024};
+    // 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) quit();
     return ret;
 }
 
+// check coordinates and speeds; return FALSE if failed
+// TODO fix to real limits!!!
+static int chkX(double X){
+    if(X > 2.*M_PI || X < -2.*M_PI) return FALSE;
+    return TRUE;
+}
+static int chkY(double Y){
+    if(Y > 2.*M_PI || Y < -2.*M_PI) return FALSE;
+    return TRUE;
+}
+static int chkXs(double s){
+    if(s < 0. || s > X_SPEED_MAX) return FALSE;
+    return TRUE;
+}
+static int chkYs(double s){
+    if(s < 0. || s > Y_SPEED_MAX) return FALSE;
+    return TRUE;
+}
+
+
 /**
  * @brief move2 - simple move to given point and stop
- * @param X - new X coordinate (radians: -pi..pi)
+ * @param X - new X coordinate (radians: -pi..pi) or NULL
- * @param Y - new Y coordinate (radians: -pi..pi)
+ * @param Y - new Y coordinate (radians: -pi..pi) or NULL
  * @return error code
  */
-static mcc_errcodes_t move2(double X, double Y){
+static mcc_errcodes_t move2(const double *X, const double *Y){
-    if(X > M_PI || X < -M_PI || Y > M_PI || Y < -M_PI){
+    if(!X && !Y) return MCC_E_BADFORMAT;
-        DBG("Wrong coords: X=%g, Y=%g", X, Y);
+    if(X){
-        return MCC_E_BADFORMAT;
+        if(!chkX(*X)) return MCC_E_BADFORMAT;
+        int32_t tag = X_RAD2MOT(*X);
+        DBG("X: %g, tag: %d", *X, tag);
+        if(!SSsetterI(CMD_MOTX, tag)) return MCC_E_FAILED;
     }
-    if(!SSXmoveto(X) || !SSYmoveto(Y)) return MCC_E_FAILED;
+    if(Y){
+        if(!chkY(*Y)) return MCC_E_BADFORMAT;
+        int32_t tag = Y_RAD2MOT(*Y);
+        DBG("Y: %g, tag: %d", *Y, tag);
+        if(!SSsetterI(CMD_MOTY, tag)) return MCC_E_FAILED;
+    }
+    return MCC_E_OK;
+}
+
+/**
+ * @brief setspeed - set maximal speed over axis
+ * @param X (i) - max speed or NULL
+ * @param Y (i) - -//-
+ * @return errcode
+ */
+static mcc_errcodes_t setspeed(const double *X, const double *Y){
+    if(!X && !Y) return MCC_E_BADFORMAT;
+    if(X){
+        if(!chkXs(*X)) return MCC_E_BADFORMAT;
+        int32_t spd = X_RS2MOTSPD(*X);
+        if(!SSsetterI(CMD_SPEEDX, spd)) return MCC_E_FAILED;
+    }
+    if(Y){
+        if(!chkYs(*Y)) return MCC_E_BADFORMAT;
+        int32_t spd = Y_RS2MOTSPD(*Y);
+        if(!SSsetterI(CMD_SPEEDY, spd)) return MCC_E_FAILED;
+    }
+    return MCC_E_OK;
+}
+
+/**
+ * @brief move2s - move to target with given max speed
+ * @param target (i) - target or NULL
+ * @param speed (i) - speed or NULL
+ * @return
+ */
+static mcc_errcodes_t move2s(const coords_t *target, const coords_t *speed){
+    if(!target && !speed) return MCC_E_BADFORMAT;
+    if(!target) return setspeed(&speed->X, &speed->Y);
+    if(!speed) return move2(&target->X, &target->Y);
+    if(!chkX(target->X) || !chkY(target->Y) || !chkXs(speed->X) || !chkYs(speed->Y))
+        return MCC_E_BADFORMAT;
+    char buf[128];
+    int32_t spd = X_RS2MOTSPD(speed->X), tag = X_RAD2MOT(target->X);
+    snprintf(buf, 127, "%s%" PRIi64 "%s%" PRIi64, CMD_MOTX, tag, CMD_MOTXYS, spd);
+    if(!SStextcmd(buf, NULL)) return MCC_E_FAILED;
+    spd = Y_RS2MOTSPD(speed->Y); tag = Y_RAD2MOT(target->Y);
+    snprintf(buf, 127, "%s%" PRIi64 "%s%" PRIi64, CMD_MOTY, tag, CMD_MOTXYS, spd);
+    if(!SStextcmd(buf, NULL)) return MCC_E_FAILED;
     return MCC_E_OK;
 }
 
@@ -89,7 +165,12 @@ static mcc_errcodes_t move2(double X, double Y){
  * @return errcode
  */
 static mcc_errcodes_t emstop(){
-    if(!SSemergStop()) return MCC_E_FAILED;
+    if(!SSstop(TRUE)) return MCC_E_FAILED;
+    return MCC_E_OK;
+}
+// normal stop
+static mcc_errcodes_t stop(){
+    if(!SSstop(FALSE)) return MCC_E_FAILED;
     return MCC_E_OK;
 }
 
@@ -149,13 +230,34 @@ static mcc_errcodes_t longcmd(long_command_t *cmd){
     return MCC_E_OK;
 }
 
+mcc_errcodes_t get_hwconf(hardware_configuration_t *c){
+    if(!c) return MCC_E_BADFORMAT;
+    SSconfig conf;
+    if(!cmdC(&conf, FALSE)) return MCC_E_FAILED;
+    // and bored transformations
+    DBG("Xacc=%u", conf.Xconf.accel);
+    DBG("Yacc=%u", conf.Yconf.accel);
+    c->Xconf.accel = X_MOTACC2RS(conf.Xconf.accel);
+    DBG("cacc: %g", c->Xconf.accel);
+    c->Xconf.backlash = conf.Xconf.backlash;
+    // ...
+    c->Yconf.accel = X_MOTACC2RS(conf.Yconf.accel);
+    c->Xconf.backlash = conf.Xconf.backlash;
+    // ...
+    return MCC_E_OK;
+}
+
 // init mount class
 mount_t Mount = {
     .init = init,
     .quit = quit,
     .getMountData = getMD,
     .moveTo = move2,
+    .moveWspeed = move2s,
+    .setSpeed = setspeed,
     .emergStop = emstop,
+    .stop = stop,
     .shortCmd = shortcmd,
     .longCmd = longcmd,
+    .getHWconfig = get_hwconf,
 };

Auxiliary_utils/LibSidServo/serial.c

@@ -48,8 +48,8 @@ static struct timeval encRtmout = {0}, mntRtmout = {0};
 // encoders raw data
 typedef struct __attribute__((packed)){
     uint8_t magick;
-    int32_t encX;
     int32_t encY;
+    int32_t encX;
     uint8_t CRC[4];
 } enc_t;
 
@@ -82,6 +82,13 @@ static void gttime(){
  */
 static void parse_encbuf(uint8_t databuf[ENC_DATALEN], struct timeval *tv){
     enc_t *edata = (enc_t*) databuf;
+/*
+#ifdef EBUG
+    DBG("ENCBUF:");
+    for(int i = 0; i < ENC_DATALEN; ++i) printf("%02X ", databuf[i]);
+    printf("\n");
+#endif
+*/
     if(edata->magick != ENC_MAGICK){
         DBG("No magick");
         return;
@@ -112,13 +119,13 @@ static void parse_encbuf(uint8_t databuf[ENC_DATALEN], struct timeval *tv){
     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.encposition.X*180./M_PI, mountdata.encposition.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
 static int getencbyte(){
     if(encfd < 0) return -1;
-    uint8_t byte;
+    uint8_t byte = 0;
     fd_set rfds;
     struct timeval tv;
     do{
@@ -181,10 +188,10 @@ static void *encoderthread(void _U_ *u){
         if(b == -2) ++errctr;
         if(b < 0) continue;
         errctr = 0;
-        DBG("Got byte from Encoder: 0x%02X", b);
+//        DBG("Got byte from Encoder: 0x%02X", b);
         if(wridx == 0){
             if((uint8_t)b == ENC_MAGICK){
-                DBG("Got magic -> start filling packet");
+//                DBG("Got magic -> start filling packet");
                 databuf[wridx++] = (uint8_t) b;
                 gettimeofday(&tv, NULL);
             }
@@ -239,7 +246,11 @@ static void *mountthread(void _U_ *u){
         struct timeval tgot;
         if(0 != gettimeofday(&tgot, NULL)) continue;
         if(!MountWriteRead(cmd_getstat, &d) || d.len != sizeof(SSstat)){
+#ifdef EBUG
             DBG("Can't read SSstat, need %zd got %zd bytes", sizeof(SSstat), d.len);
+            for(size_t i = 0; i < d.len; ++i) printf("%02X ", d.buf[i]);
+            printf("\n");
+#endif
             ++errctr; continue;
         }
         if(SScalcChecksum(buf, sizeof(SSstat)-2) != status->checksum){
@@ -375,14 +386,15 @@ static int wr(const data_t *out, data_t *in, int needeol){
             (void) g;
         }
     }
-    if(in){
+    uint8_t buf[256];
+    data_t dumb = {.buf = buf, .maxlen = 256};
+    if(!in) in = &dumb; // even if user don't ask for answer, try to read to clear trash
     in->len = 0;
     for(size_t i = 0; i < in->maxlen; ++i){
         int b = getmntbyte();
         if(b < 0) break; // nothing to read -> go out
         in->buf[in->len++] = (uint8_t) b;
     }
-    }
     return TRUE;
 }
 
@@ -398,6 +410,13 @@ int MountWriteRead(const data_t *out, data_t *in){
     pthread_mutex_unlock(&mntmutex);
     return ret;
 }
+// send binary data - without EOL
+int MountWriteReadRaw(const data_t *out, data_t *in){
+    pthread_mutex_lock(&mntmutex);
+    int ret = wr(out, in, 0);
+    pthread_mutex_unlock(&mntmutex);
+    return ret;
+}
 
 #ifdef EBUG
 static void logscmd(SSscmd *c){
@@ -459,3 +478,34 @@ int cmdS(SSscmd *cmd){
 int cmdL(SSlcmd *cmd){
     return bincmd((uint8_t *)cmd, sizeof(SSlcmd));
 }
+// rw == 1 to write, 0 to read
+int cmdC(SSconfig *conf, int rw){
+    static data_t *wcmd = NULL, *rcmd = NULL;
+    int ret = FALSE;
+    // dummy buffer to clear trash in input
+    char ans[300];
+    data_t a = {.buf = (uint8_t*)ans, .maxlen=299};
+    if(!wcmd) wcmd = cmd2dat(CMD_PROGFLASH);
+    if(!rcmd) rcmd = cmd2dat(CMD_DUMPFLASH);
+    pthread_mutex_lock(&mntmutex);
+    if(rw){ // write
+        if(!wr(wcmd, &a, 1)) goto rtn;
+    }else{ // read
+        data_t d;
+        d.buf = (uint8_t *) conf;
+        d.len = 0; d.maxlen = sizeof(SSconfig);
+        ret = wr(rcmd, &d, 1);
+        DBG("wr returned %s; got %zd bytes of %zd", ret ? "TRUE" : "FALSE", d.len, d.maxlen);
+        if(d.len != d.maxlen) return FALSE;
+        // simplest checksum
+        uint16_t sum = 0;
+        for(uint32_t i = 0; i < sizeof(SSconfig)-2; ++i) sum += d.buf[i];
+        if(sum != conf->checksum){
+            DBG("got sum: %u, need: %u", conf->checksum, sum);
+            return FALSE;
+        }
+    }
+rtn:
+    pthread_mutex_unlock(&mntmutex);
+    return ret;
+}

Auxiliary_utils/LibSidServo/serial.h

@@ -36,5 +36,7 @@ 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 MountWriteReadRaw(const data_t *out, data_t *in);
 int cmdS(SSscmd *cmd);
 int cmdL(SSlcmd *cmd);
+int cmdC(SSconfig *conf, int rw);

Auxiliary_utils/LibSidServo/sidservo.h

@@ -18,6 +18,11 @@
 
 #pragma once
 
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
 #include <stddef.h>
 #include <stdint.h>
 #include <sys/time.h>
@@ -55,8 +60,30 @@ typedef struct{
 } data_t;
 
 typedef struct{
-    uint8_t XBits;
+    uint8_t motrev :1;      // If 1, the motor encoder is incremented in the opposite direction
-    uint8_t YBits;
+    uint8_t motpolarity :1; // If 1, the motor polarity is reversed
+    uint8_t encrev :1;      // If 1, the axis encoder is reversed
+    uint8_t dragtrack :1;   // If 1, we are in computerless Drag and Track mode
+    uint8_t trackplat :1;   // If 1, we are in the tracking platform mode
+    uint8_t handpaden :1;   // If 1, hand paddle is enabled
+    uint8_t newpad :1;      // If 1, hand paddle is compatible with New hand paddle, which allows slewing in two directions and guiding
+    uint8_t guidemode :1;   // If 1, we are in guide mode. The pan rate is added or subtracted from the current tracking rate
+} xbits_t;
+
+typedef struct{
+    uint8_t motrev :1;      // If 1, the motor encoder is incremented in the opposite direction
+    uint8_t motpolarity :1; // If 1, the motor polarity is reversed
+    uint8_t encrev :1;      // If 1, the axis encoder is reversed
+    /* If 1, we are in computerless Slew and Track mode
+       (no clutches; use handpad to slew; must be in Drag and Track mode too) */
+    uint8_t slewtrack :1;
+    uint8_t digin_sens :1; // Digital input from radio handpad receiver, or RA PEC Sensor sync
+    uint8_t digin :3; // Digital input from radio handpad receiver
+} ybits_t;
+
+typedef struct{
+    xbits_t XBits;
+    ybits_t YBits;
     uint8_t ExtraBits;
     uint16_t ain0;
     uint16_t ain1;
@@ -71,8 +98,16 @@ typedef struct{
     uint32_t millis;
     double temperature;
     double voltage;
+    int32_t XmotRaw;
+    int32_t YmotRaw;
+    int32_t XencRaw;
+    int32_t YencRaw;
 } mountdata_t;
 
+typedef struct{
+    ;
+} mountstat_t;
+
 typedef struct{
     double Xmot;        // 0  X motor position (rad)
     double Xspeed;      // 4  X speed (rad/s)
@@ -94,15 +129,64 @@ typedef struct{
     double Yatime;      // 28
 } long_command_t; // long command
 
+// hardware axe configuration
+typedef struct{
+    double accel;       // Default Acceleration, rad/s^2
+    double backlash;    // Backlash (???)
+    double errlimit;    // Error Limit, rad
+    double propgain;    // Proportional Gain (???)
+    double intgain;     // Integral Gain (???)
+    double derivgain;   // Derivative Gain (???)
+    double outplimit;   // Output Limit, percent (0..100)
+    double currlimit;   // Current Limit (A)
+    double intlimit;    // Integral Limit (???)
+} __attribute__((packed)) axe_config_t;
+
+// hardware configuration
+typedef struct{
+    axe_config_t Xconf;
+    xbits_t xbits;
+    axe_config_t Yconf;
+    ybits_t ybits;
+    uint8_t address;
+    double eqrate;      // Equatorial Rate (???)
+    double eqadj;       // Equatorial UpDown adjust (???)
+    double trackgoal;   // Tracking Platform Goal (???)
+    double latitude;    // Latitude, rad
+    uint32_t Ysetpr;    // Azm Scope Encoder Ticks Per Rev
+    uint32_t Xsetpr;    // Alt Scope Encoder Ticks Per Rev
+    uint32_t Ymetpr;    // Azm Motor Ticks Per Rev
+    uint32_t Xmetpr;    // Alt Motor Ticks Per Rev
+    double Xslewrate;   // Alt/Dec Slew Rate (rad/s)
+    double Yslewrate;   // Azm/RA Slew Rate (rad/s)
+    double Xpanrate;    // Alt/Dec Pan Rate (rad/s)
+    double Ypanrate;    // Azm/RA Pan Rate (rad/s)
+    double Xguiderate;  // Alt/Dec Guide Rate (rad/s)
+    double Yguiderate;  // Azm/RA Guide Rate (rad/s)
+    uint32_t baudrate;  // Baud Rate (baud)
+    double locsdeg;     // Local Search Degrees (rad)
+    double locsspeed;   // Local Search Speed (rad/s)
+    double backlspd;    // Backlash speed (???)
+} hardware_configuration_t;
+
 // mount class
 typedef struct{
     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  (*moveTo)(const double *X, const double *Y); // move to given position ans stop
+    mcc_errcodes_t  (*moveWspeed)(const coords_t *target, const  coords_t *speed); // move with given max speed
+    mcc_errcodes_t  (*setSpeed)(const double *X, const double *Y); // set speed
+    mcc_errcodes_t  (*stop)(); // stop
     mcc_errcodes_t  (*emergStop)(); // emergency stop
     mcc_errcodes_t  (*shortCmd)(short_command_t *cmd); // send/get short command
     mcc_errcodes_t  (*longCmd)(long_command_t *cmd); // send/get long command
+    mcc_errcodes_t  (*getHWconfig)(hardware_configuration_t *c); // get hardware configuration
 } mount_t;
 
 extern mount_t Mount;
+
+
+#ifdef __cplusplus
+}
+#endif

Auxiliary_utils/LibSidServo/ssii.c

@@ -90,6 +90,19 @@ int SStextcmd(const char *cmd, data_t *answer){
     return MountWriteRead(&d, answer);
 }
 
+// the same as SStextcmd, but not adding EOL - send raw 'cmd'
+int SSrawcmd(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 MountWriteReadRaw(&d, answer);
+}
+
 /**
  * @brief SSgetint - send text command and return integer answer
  * @param cmd (i) - command to send
@@ -116,29 +129,25 @@ int SSgetint(const char *cmd, int64_t *ans){
     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
+ * @brief SSsetterI - integer setter
-// BE CAREFUL: angle isn't checking here
+ * @param cmd - command to send
-int SSXmoveto(double pos){
+ * @param ival - value
-    char buf[64];
+ * @return false if failed
-    int64_t target = X_RAD2MOT(pos);
+ */
-    DBG("move to angle %grad = %ld", pos, target);
+int SSsetterI(const char *cmd, int32_t ival){
-    snprintf(buf, 63, "%s%" PRIi64, CMD_MOTX, target);
+    char buf[128];
-    return SStextcmd(buf, NULL);
+    snprintf(buf, 127, "%s%" PRIi32, cmd, ival);
-}
-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 SSstop(int emerg){
     int i = 0;
+    const char *cmdx = (emerg) ? CMD_EMSTOPX : CMD_STOPX;
+    const char *cmdy = (emerg) ? CMD_EMSTOPY : CMD_STOPY;
     for(; i < 10; ++i){
-        if(!SStextcmd(CMD_EMSTOPX, NULL)) continue;
+        if(!SStextcmd(cmdx, NULL)) continue;
-        if(SStextcmd(CMD_EMSTOPY, NULL)) break;
+        if(SStextcmd(cmdy, NULL)) break;
     }
     if(i == 10) return FALSE;
     return TRUE;

Auxiliary_utils/LibSidServo/ssii.h

@@ -23,92 +23,186 @@
 
 #include "sidservo.h"
 
-#if 0
+/*********** base commands ***********/
-// ASCII commands
-#define U8P(x)   ((uint8_t*)x)
-// get binary data of all statistics
-#define CMD_GETSTAT     U8P("XXS")
-// send short command
-#define CMD_SHORTCMD    U8P("XXR")
-// send long command
-#define CMD_LONGCMD     U8P("YXR")
 // get/set X/Y in motsteps
-#define CMD_MOTX        U8P("X")
+#define CMD_MOTX        "X"
-#define CMD_MOTY        U8P("Y")
+#define CMD_MOTY        "Y"
-// -//- in encoders' ticks
+// set X/Y position with speed "sprintf(buf, "%s%d%s%d", CMD_MOTx, tagx, CMD_MOTxS, tags)
-#define CMD_ENCX        U8P("XZ")
+#define CMD_MOTXYS      "S"
-#define CMD_ENCY        U8P("YZ")
+// reset current motor position to given value  (and stop, if moving)
+#define CMD_MOTXSET     "XF"
+#define CMD_MOTYSET     "YF"
+// acceleration (per each loop, max: 3900)
+#define CMD_MOTXACCEL   "XR"
+#define CMD_MOTYACCEL   "YR"
+// PID regulator:
+// P: 0..32767
+#define CMD_PIDPX       "XP"
+#define CMD_PIDPY       "YP"
+// I: 0..32767
+#define CMD_PIDIX       "XI"
+#define CMD_PIDIY       "YI"
+// limit of I (doesn't work): 0:24000 (WTF???)
+#define CMD_PIDILX      "XL"
+#define CMD_PIDILY      "YL"
+// D: 0..32767
+#define CMD_PIDDX       "XD"
+#define CMD_PIDDY       "YD"
+// current position error
+#define CMD_POSERRX     "XE"
+#define CMD_POSERRY     "YE"
+// max position error limit (X: E#, Y: e#)
+#define CMD_POSERRLIMX  "XEL"
+#define CMD_POSERRLIMY  "YEL"
+// current PWM output: 0..255 (or set max PWM out)
+#define CMD_PWMOUTX     "XO"
+#define CMD_PWMOUTY     "YO"
+// motor current *100 (or set current limit): 0..240
+#define CMD_MOTCURNTX   "XC"
+#define CMD_MOTCURNTY   "YC"
+// change axis to Manual mode and set the PWM output: -255:255
+#define CMD_MANUALPWMX  "XM"
+#define CMD_MANUALPWMY  "YM"
+// change axis to Auto mode
+#define CMD_AUTOX       "XA"
+#define CMD_AUTOY       "YA"
+// get positioin in encoders' ticks or reset it to given value
+#define CMD_ENCX        "XZ"
+#define CMD_ENCY        "YZ"
+// get/set speed (geter x: S#, getter y: s#)
+#define CMD_SPEEDX      "XS"
+#define CMD_SPEEDY      "YS"
 // normal stop X/Y
-#define CMD_STOPX       U8P("XN")
+#define CMD_STOPX       "XN"
-#define CMD_STOPY       U8P("YN")
+#define CMD_STOPY       "YN"
+// lower speed -> drag&track or slew&track
+#define CMD_STOPTRACKX  "XNT"
+#define CMD_STOPTRACKY  "YNT"
 // emergency stop
-#define CMD_EMSTOPX     U8P("XG")
+#define CMD_EMSTOPX     "XG"
-#define CMD_EMSTOPY     U8P("YG")
+#define CMD_EMSTOPY     "YG"
-// getters of motor's encoders per rev
+// get/set X/Ybits
-#define CMD_GETXMEPR    U8P("XXU")
+#define CMD_BITSX       "XB"
-#define CMD_GETYMEPR    U8P("XXV")
+#define CMD_BITSY       "YB"
+
+/*********** getters/setters without "Y" variant ***********/
+// get handpad status (decimal)
+#define CMD_HANDPAD     "XK"
+// get TCPU (deg F)
+#define CMD_TCPU        "XH"
+// get firmware version *10
+#define CMD_FIRMVER     "XV"
+// get motor voltage *10
+#define CMD_MOTVOLTAGE  "XJ"
+// get/set current CPU clock (milliseconds)
+#define CMD_MILLIS      "XY"
+// reset servo
+#define CMD_RESET       "XQ"
+// clear to factory defaults
+#define CMD_CLRDEFAULTS "XU"
+// save configuration to flash ROM
+#define CMD_WRITEFLASH  "XW"
+// read config from flash to RAM
+#define CMD_READFLASH   "XT"
+// write to flash following full config (128 bytes + 2 bytes of checksum)
+#define CMD_PROGFLASH   "FC"
+// read configuration (-//-)
+#define CMD_DUMPFLASH   "SC"
+// get serial number
+#define CMD_SERIAL      "YV"
+
+/*********** extended commands ***********/
+// get/set latitute
+#define CMD_LATITUDE    "XXL"
+// getters/setters of motor's encoders per rev
+#define CMD_MEPRX       "XXU"
+#define CMD_MEPRY       "XXV"
 // -//- axis encoders
-#define CMD_GETXAEPR    U8P("XXT")
+#define CMD_AEPRX       "XXT"
-#define CMD_GETYAEPR    U8P("XXZ")
+#define CMD_AEPRY       "XXZ"
-// exit ASCII checksum mode
+// get/set slew rate
-#define CMD_EXITACM     U8P("YXY0\r\xb8")
+#define CMD_SLEWRATEX   "XXA"
-#endif
+#define CMD_SLEWRATEY   "XXB"
+// get/set pan rate
+#define CMD_PANRATEX    "XXC"
+#define CMD_PANRATEY    "XXD"
+// get/set platform tracking rate
+#define CMD_PLATRATE    "XXE"
+// get/set platform up/down adjuster
+#define CMD_PLATADJ     "XXF"
+// get/set platform goal
+#define CMD_PLATGOAL    "XXG"
+// get/set guide rate
+#define CMD_GUIDERATEX  "XXH"
+#define CMD_GUIDERATEY  "XXI"
+// get/set picservo timeout (seconds)
+#define CMD_PICTMOUT    "XXJ"
+// get/set digital outputs of radio handpad
+#define CMD_RADIODIGOUT "XXQ"
+// get/set argo navis mode
+#define CMD_ARGONAVIS   "XXN"
+// get/set local search distance
+#define CMD_LOSCRCHDISTX    "XXM"
+#define CMD_LOSCRCHDISTY    "XXO"
+// get/set backlash
+#define CMD_BACKLASHX   "XXO"
+#define CMD_BACKLASHY   "XXP"
+
 
 // get binary data of all statistics
-#define CMD_GETSTAT     ("XXS")
+#define CMD_GETSTAT     "XXS"
 // send short command
-#define CMD_SHORTCMD    ("XXR")
+#define CMD_SHORTCMD    "XXR"
 // send long command
-#define CMD_LONGCMD     ("YXR")
+#define CMD_LONGCMD     "YXR"
-// get/set X/Y in motsteps
+
-#define CMD_MOTX        ("X")
+
-#define CMD_MOTY        ("Y")
+/*********** special ***********/
-// -//- 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")
+#define CMD_EXITACM     "YXY0\r\xb8"
+// controller status:
+// X# Y# XZ# YZ# XC# YC# V# T# X[AM] Y[AM] K#
+// X,Y - motor, XZ,YZ - encoder, XC,YC - current*100, V - voltage*10, T - temp (F), XA,YA - mode (A[uto]/M[anual]), K - handpad status bits
+#define CMD_GETSTATTEXT "\r"
+
+
 
 // steps per revolution
-//#define X_MOT_STEPSPERREV   (3325440.)
 #define X_MOT_STEPSPERREV   (3325952.)
-//#define Y_MOT_STEPSPERREV   (4394496.)
 #define Y_MOT_STEPSPERREV   (4394960.)
 
+// maximal speeds in rad/s: 10deg/s by X and 8deg/s by Y
+#define X_SPEED_MAX         (0.17453)
+#define Y_SPEED_MAX         (0.13963)
+
 // motor position to radians and back
-#define X_MOT2RAD(n)    (2.*M_PI * (double)n / X_MOT_STEPSPERREV)
+#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 Y_MOT2RAD(n)    (2.*M_PI * ((double)n) / Y_MOT_STEPSPERREV)
-#define X_RAD2MOT(r)    ((int32_t)(r / 2./M_PI * X_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))
+#define Y_RAD2MOT(r)    ((int32_t)((r) / 2./M_PI * Y_MOT_STEPSPERREV))
 // motor speed in rad/s and back
 #define X_MOTSPD2RS(n)  (X_MOT2RAD(n)/65536.*1953.)
 #define X_RS2MOTSPD(r)  ((int32_t)(X_RAD2MOT(r)*65536./1953.))
 #define Y_MOTSPD2RS(n)  (Y_MOT2RAD(n)/65536.*1953.)
 #define Y_RS2MOTSPD(r)  ((int32_t)(Y_RAD2MOT(r)*65536./1953.))
+// motor acceleration -//-
+#define X_MOTACC2RS(n)  (X_MOT2RAD(n)/65536.*1953.*1953.)
+#define X_RS2MOTACC(r)  ((int32_t)(X_RAD2MOT(r)*65536./1953./1953.))
+#define Y_MOTACC2RS(n)  (Y_MOT2RAD(n)/65536.*1953.*1953.)
+#define Y_RS2MOTACC(r)  ((int32_t)(Y_RAD2MOT(r)*65536./1953./1953.))
+
 // adder time to seconds vice versa
-#define ADDER2S(a)  (a*1953.)
+#define ADDER2S(a)  ((a)*1953.)
-#define S2ADDER(s)  (s/1953.)
+#define S2ADDER(s)  ((s)/1953.)
 
 // encoder per revolution
 #define X_ENC_STEPSPERREV   (67108864.)
 #define Y_ENC_STEPSPERREV   (67108864.)
 // encoder position to radians and back
-#define X_ENC2RAD(n)    (2.*M_PI * (double)n / X_ENC_STEPSPERREV)
+#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 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 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))
+#define Y_RAD2ENC(r)    ((uint32_t)((r) / 2./M_PI * Y_ENC_STEPSPERREV))
 
 // encoder's tolerance (ticks)
 #define YencTOL    (25.)
@@ -123,8 +217,8 @@ typedef struct{ // 41 bytes
     int32_t Xenc;       // 9  Dec/HA encoder position
     int32_t Yenc;       // 13
     uint8_t keypad;     // 17 keypad status
-    uint8_t XBits;      // 18
+    xbits_t XBits;      // 18
-    uint8_t YBits;      // 19
+    ybits_t YBits;      // 19
     uint8_t ExtraBits;  // 20
     uint16_t ain0;      // 21 analog inputs
     uint16_t ain1;      // 23
@@ -159,11 +253,60 @@ typedef struct{
     uint16_t checksum;  // 32
 } __attribute__((packed)) SSlcmd; // long command
 
+typedef struct{
+    uint32_t accel;     // Default Acceleration (0..3900)
+    uint32_t backlash;  // Backlash (???)
+    uint16_t errlimit;  // Error Limit (0..32767)
+    uint16_t propgain;  // Proportional Gain (0..32767)
+    uint16_t intgain;   // Integral Gain (0..32767)
+    uint16_t derivgain; // Derivative Gain (0..32767)
+    uint16_t outplimit; // Output Limit, 0xFF = 100.0 (0..255)
+    uint16_t currlimit; // Current Limit * 100 (0..240)
+    uint16_t intlimit;  // Integral Limit (0..24000)
+} __attribute__((packed)) AxeConfig;
+
+typedef struct{
+    AxeConfig Xconf;
+    xbits_t xbits;
+    uint8_t unused0;
+    AxeConfig Yconf;
+    ybits_t ybits;
+    uint8_t unused1;
+    uint8_t address;
+    uint8_t unused2;
+    uint32_t eqrate;    // Equatorial Rate (platform?) (???)
+    int32_t eqadj;      // Equatorial UpDown adjust (???)
+    uint32_t trackgoal; // Tracking Platform Goal (???)
+    uint16_t latitude;  // Latitude * 100, MSB FIRST!!
+    uint32_t Ysetpr;    // Azm Scope Encoder Ticks Per Rev, MSB FIRST!!
+    uint32_t Xsetpr;    // Alt Scope Encoder Ticks Per Rev, MSB FIRST!!
+    uint32_t Ymetpr;    // Azm Motor Ticks Per Rev, MSB FIRST!!
+    uint32_t Xmetpr;    // Alt Motor Ticks Per Rev, MSB FIRST!!
+    int32_t Xslewrate;  // Alt/Dec Slew Rate (rates are negative in "through the pole" mode!!!)
+    int32_t Yslewrate;  // Azm/RA Slew Rate
+    int32_t Xpanrate;   // Alt/Dec Pan Rate
+    int32_t Ypanrate;   // Azm/RA Pan Rate
+    int32_t Xguiderate; // Alt/Dec Guide Rate
+    int32_t Yguiderate; // Azm/RA Guide Rate
+    uint8_t unknown0;   // R/A PEC Auto Sync Enable (if low bit = 1), or PicServo Comm Timeout??
+    uint8_t unused3;
+    uint8_t baudrate;   // Baud Rate??
+    uint8_t unused4;
+    uint8_t specmode;   // 1 = Enable Argo Navis, 2 = Enable Sky Commander
+    uint8_t unused5;
+    uint32_t locsdeg;   // Local Search Degrees * 100
+    uint32_t locsspeed; // Local Search Speed, arcsec per sec (???)
+    uint32_t backlspd;  // Backlash speed
+    uint32_t pecticks;  // RA/Azm PEC Ticks
+    uint16_t unused6;
+    uint16_t checksum;
+} __attribute__((packed)) SSconfig;
+
 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);
+int SSrawcmd(const char *cmd, data_t *answer);
 int SSgetint(const char *cmd, int64_t *ans);
-int SSXmoveto(double pos);
+int SSsetterI(const char *cmd, int32_t ival);
-int SSYmoveto(double pos);
+int SSstop(int emerg);
-int SSemergStop();
 int SSshortCmd(SSscmd *cmd);