shortcmd checked

LibSidServo/examples/CMakeLists.txt

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

LibSidServo/examples/dump.c

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

LibSidServo/examples/dump.h

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

LibSidServo/examples/dumpmoving_scmd.c

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

LibSidServo/examples/goto.c

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

LibSidServo/libsidservo.creator.user

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

LibSidServo/libsidservo.files

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

LibSidServo/main.c

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

LibSidServo/serial.c

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

LibSidServo/serial.h

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

LibSidServo/sidservo.h

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

LibSidServo/ssii.c

@@ -35,37 +35,6 @@ uint16_t SScalcChecksum(uint8_t *buf, int len){
     return checksum;
 }
 
-// send short/long binary command
-static int bincmd(uint8_t *cmd, int len){
-    static data_t *dscmd = NULL, *dlcmd = NULL;
-    if(!dscmd) dscmd = cmd2dat(CMD_SHORTCMD);
-    if(!dlcmd) dlcmd = cmd2dat(CMD_LONGCMD);
-    if(len == sizeof(SSscmd)){
-        ((SSscmd*)cmd)->checksum = SScalcChecksum(cmd, len-2);
-        DBG("Short command");
-        if(!MountWriteRead(dscmd, NULL)) return -1;
-    }else if(len == sizeof(SSlcmd)){
-        ((SSlcmd*)cmd)->checksum = SScalcChecksum(cmd, len-2);
-        DBG("Long command");
-        if(!MountWriteRead(dlcmd, NULL)) return -1;
-    }else{
-        return -1;
-    }
-    DBG("Write %d bytes and wait for ans", len);
-    data_t d;
-    d.buf = cmd;
-    d.len = d.maxlen = len;
-    return MountWriteRead(&d, &d);
-}
-// return TRUE if OK
-int SScmdS(SSscmd *cmd){
-    return bincmd((uint8_t *)cmd, sizeof(SSscmd));
-}
-int SScmdL(SSlcmd *cmd){
-    return bincmd((uint8_t *)cmd, sizeof(SSlcmd));
-}
-
-
 /**
  * @brief SSconvstat - convert stat from SSII format to human
  * @param status (i) - just read data

LibSidServo/ssii.h

@@ -159,8 +159,6 @@ typedef struct{
     uint16_t checksum;  // 32
 } __attribute__((packed)) SSlcmd; // long command
 
-int SScmdS(SSscmd *cmd);
-int SScmdL(SSlcmd *cmd);
 uint16_t SScalcChecksum(uint8_t *buf, int len);
 void SSconvstat(const SSstat *status, mountdata_t *mountdata, struct timeval *tdat);
 int SStextcmd(const char *cmd, data_t *answer);