From 943409c53e15773b422b4121a4156a97656832f2 Mon Sep 17 00:00:00 2001 From: "Edward V. Emelianov" Date: Thu, 27 Feb 2025 22:46:51 +0300 Subject: [PATCH] start configurator --- LibSidServo/examples/CMakeLists.txt | 1 + LibSidServo/examples/Readme.md | 2 + LibSidServo/examples/SSIIconf.c | 76 ++++++++++++++++++++++++++ LibSidServo/examples/dumpmoving.c | 2 +- LibSidServo/examples/dumpmoving_scmd.c | 2 +- LibSidServo/examples/goto.c | 2 +- LibSidServo/examples/scmd_traectory.c | 2 +- LibSidServo/libsidservo.creator.user | 2 +- LibSidServo/libsidservo.files | 1 + LibSidServo/main.c | 18 ++++++ LibSidServo/serial.c | 31 +++++++++++ LibSidServo/serial.h | 1 + LibSidServo/sidservo.h | 41 ++++++++++++++ LibSidServo/ssii.h | 55 +++++++++++++++++++ 14 files changed, 231 insertions(+), 5 deletions(-) create mode 100644 LibSidServo/examples/SSIIconf.c diff --git a/LibSidServo/examples/CMakeLists.txt b/LibSidServo/examples/CMakeLists.txt index a8f89af..944628d 100644 --- a/LibSidServo/examples/CMakeLists.txt +++ b/LibSidServo/examples/CMakeLists.txt @@ -10,3 +10,4 @@ add_executable(dump dumpmoving.c dump.c conf.c) add_executable(dump_s dumpmoving_scmd.c dump.c conf.c) add_executable(dumpswing dumpswing.c dump.c conf.c) add_executable(traectory_s scmd_traectory.c dump.c traectories.c conf.c) +add_executable(SSIIconf SSIIconf.c conf.c) diff --git a/LibSidServo/examples/Readme.md b/LibSidServo/examples/Readme.md index 5204710..6a7e1dc 100644 --- a/LibSidServo/examples/Readme.md +++ b/LibSidServo/examples/Readme.md @@ -24,3 +24,5 @@ Some examples of usage of libsidservo *scmd_traectory.c* (`traectory_s`) - try to move around given traectory using "short" binary commands. +*SSIIconf.c* (`SSIIconf`) - read/write hardware configuration of controller + diff --git a/LibSidServo/examples/SSIIconf.c b/LibSidServo/examples/SSIIconf.c new file mode 100644 index 0000000..c5fb631 --- /dev/null +++ b/LibSidServo/examples/SSIIconf.c @@ -0,0 +1,76 @@ +/* + * This file is part of the libsidservo project. + * Copyright 2025 Edward V. Emelianov . + * + * 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 . + */ + +#include +#include + +#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; +} diff --git a/LibSidServo/examples/dumpmoving.c b/LibSidServo/examples/dumpmoving.c index afff7f8..323e310 100644 --- a/LibSidServo/examples/dumpmoving.c +++ b/LibSidServo/examples/dumpmoving.c @@ -49,7 +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_int, APTR(&G.conffile), "configuration file name"}, + {"conffile", NEED_ARG, NULL, 'C', arg_string, APTR(&G.conffile), "configuration file name"}, end_option }; diff --git a/LibSidServo/examples/dumpmoving_scmd.c b/LibSidServo/examples/dumpmoving_scmd.c index 36e2d09..ec9a029 100644 --- a/LibSidServo/examples/dumpmoving_scmd.c +++ b/LibSidServo/examples/dumpmoving_scmd.c @@ -57,7 +57,7 @@ static sl_option_t cmdlnopts[] = { {"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, Y or B for both)"}, - {"conffile", NEED_ARG, NULL, 'C', arg_int, APTR(&G.conffile), "configuration file name"}, + {"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 }; diff --git a/LibSidServo/examples/goto.c b/LibSidServo/examples/goto.c index 9ab7b18..3da2b9a 100644 --- a/LibSidServo/examples/goto.c +++ b/LibSidServo/examples/goto.c @@ -53,7 +53,7 @@ static sl_option_t cmdlnopts[] = { {"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_int, APTR(&G.conffile), "configuration file name"}, + {"conffile", NEED_ARG, NULL, 'C', arg_string, APTR(&G.conffile), "configuration file name"}, end_option }; diff --git a/LibSidServo/examples/scmd_traectory.c b/LibSidServo/examples/scmd_traectory.c index 924d634..00dacd2 100644 --- a/LibSidServo/examples/scmd_traectory.c +++ b/LibSidServo/examples/scmd_traectory.c @@ -66,7 +66,7 @@ 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_int, APTR(&G.conffile), "configuration file name"}, + {"conffile", NEED_ARG, NULL, 'C', arg_string, APTR(&G.conffile), "configuration file name"}, end_option }; diff --git a/LibSidServo/libsidservo.creator.user b/LibSidServo/libsidservo.creator.user index f63ec79..dff1f3d 100644 --- a/LibSidServo/libsidservo.creator.user +++ b/LibSidServo/libsidservo.creator.user @@ -1,6 +1,6 @@ - + EnvironmentId diff --git a/LibSidServo/libsidservo.files b/LibSidServo/libsidservo.files index 443fe64..18d928c 100644 --- a/LibSidServo/libsidservo.files +++ b/LibSidServo/libsidservo.files @@ -1,5 +1,6 @@ CMakeLists.txt dbg.h +examples/SSIIconf.c examples/conf.c examples/conf.h examples/dump.c diff --git a/LibSidServo/main.c b/LibSidServo/main.c index 13b8476..788ad86 100644 --- a/LibSidServo/main.c +++ b/LibSidServo/main.c @@ -230,6 +230,23 @@ 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, @@ -242,4 +259,5 @@ mount_t Mount = { .stop = stop, .shortCmd = shortcmd, .longCmd = longcmd, + .getHWconfig = get_hwconf, }; diff --git a/LibSidServo/serial.c b/LibSidServo/serial.c index 24f8fb7..3f4b823 100644 --- a/LibSidServo/serial.c +++ b/LibSidServo/serial.c @@ -478,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; +} diff --git a/LibSidServo/serial.h b/LibSidServo/serial.h index b880d91..aa2604c 100644 --- a/LibSidServo/serial.h +++ b/LibSidServo/serial.h @@ -39,3 +39,4 @@ 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); diff --git a/LibSidServo/sidservo.h b/LibSidServo/sidservo.h index 62e7d28..5f85d50 100644 --- a/LibSidServo/sidservo.h +++ b/LibSidServo/sidservo.h @@ -129,6 +129,46 @@ 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 @@ -141,6 +181,7 @@ typedef struct{ 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; diff --git a/LibSidServo/ssii.h b/LibSidServo/ssii.h index 7445885..866f78f 100644 --- a/LibSidServo/ssii.h +++ b/LibSidServo/ssii.h @@ -185,6 +185,12 @@ #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 S2ADDER(s) ((s)/1953.) @@ -247,6 +253,55 @@ 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);