telescope is moving; need to add some more commands & write FITS-header

Daemons/10micron_stellarium/mount.c

@@ -22,6 +22,7 @@
 #include <string.h>
 #include <usefull_macros.h>
 
+#include "10micron_commands.h"
 #include "emulation.h"
 #include "mount.h"
 
@@ -30,12 +31,13 @@
 
 static bool isemulated = false; // ==true for emulation mode
 
+// default serial timeout, seconds
+static double sertmout = 1.;
+
 static char mount_name[MNAME_LEN+3] = "'noname'";
 static sl_tty_t *mount_dev = NULL;
 // device mutex, blocking only in non-local functions
 static pthread_mutex_t mntdev_mutex = PTHREAD_MUTEX_INITIALIZER;
-// ring buffer for data incoming from serial port
-static sl_ringbuffer_t *RBin = NULL;
 // status
 static atomic_int mountstatus = MNT_S_ERROR;
 
@@ -148,20 +150,25 @@ bool mount_set_name(const char *name){
  * @brief mount_set_dev - open mount device without checking that mount is alive
  * @param dev - path to device
  * @param speed - baudrate
+ * @param timeout - timeout (seconds) for answer's waiting
  * @return false if failed to open device `dev`
  */
-bool mount_set_dev(char *dev, int speed, int timeout){
+bool mount_set_dev(char *dev, int speed, double timeout){
     pthread_mutex_lock(&mntdev_mutex);
     if(mount_dev) sl_tty_close(&mount_dev);
     mount_dev = sl_tty_new(dev, speed, 4096);
     if(mount_dev) mount_dev = sl_tty_open(mount_dev, 1);
     if(!mount_dev){
         pthread_mutex_unlock(&mntdev_mutex);
+        DBG("Can't init serial device");
         return false;
     }
-    sl_tty_tmout(timeout);
-    if(!RBin) RBin = sl_RB_new(BUFSIZ);
-    else sl_RB_clearbuf(RBin);
+    DBG("Mount device inited and opened");
+    // set terminal timeout to `timeout / 10` or 100ms
+    int usecs = (timeout < 1.) ? (int)(timeout * 1e5) : 100000;
+    sl_tty_tmout(usecs);
+    sertmout = timeout;
+    DBG("set timeout of answer waiting to %gs", sertmout);
     pthread_mutex_unlock(&mntdev_mutex);
     return true;
 }
@@ -179,7 +186,8 @@ static const char *statuses[MNT_S_STATAMOUNT] = {
     [MNT_S_OUTLIMIT] = "'Outside tracking limit'",
     [MNT_S_FOLSAT]= "'Following satellite'",
     [MNT_S_DATINCOSIST]= "'Data inconsistency'",
-    [MNT_S_ERROR] = "'Error (disconnected?)'"
+    [MNT_S_ERROR] = "'Error'",
+    [MNT_S_OFF] = "'Off'"
 };
 
 /**
@@ -187,53 +195,138 @@ static const char *statuses[MNT_S_STATAMOUNT] = {
  * @param status - integer status code
  * @return statically allocated string with explanation
  */
-const char* mount_status_str(){
-    int curst = (isemulated) ? emulation_status() : atomic_load(&mountstatus);
-    if(curst > -1 && curst < MNT_S_STATAMOUNT) return statuses[curst];
+const char* mount_status_str(mount_status_t st){
+    if(st < MNT_S_STATAMOUNT) return statuses[st];
     return "'Unknown status'";
 }
-// return current mount status
-mount_status_t mount_status(){
-    return (mount_status_t)atomic_load(&mountstatus);
-}
 
 /**
- * @brief write_cmd - try to write command to mount
- * @param cmd - string with command or NULL just to clear all incoming data
- * @return false on write/read error; all read information is in RBin (even if `false` returned, you SHOULD read all available strings from it)
+ * @brief send_cmd_resp - send command and wait for '#'-terminated response
+ * @param cmd     - command (e.g. ":Sr12.345#")
+ * @param resp    - buffer for response (may be NULL)
+ * @param resplen - its size
+ * @return true if '#'-terminated response received before timeout
  */
-static bool write_cmd(const char *cmd){
-    bool ret = true;
-    if(cmd){
-        size_t l = strlen(cmd);
-        if(sl_tty_write(mount_dev->comfd, cmd, l)) ret = false;
+static bool send_cmd_resp(const char *cmd, char *resp, size_t resplen){
+    if(!mount_dev) return false;
+    while(sl_tty_read(mount_dev) > 0); // clear tty buffer
+    if(!cmd) return false; // just clear buffer when cmd==NULL
+    DBG("Command '%s', fd: %d", cmd, mount_dev->comfd);
+    if(sl_tty_write(mount_dev->comfd, cmd, strlen(cmd))){
+        WARN("sl_tty_write()");
+        return false;
     }
-    int got = 0;
-    while((got = sl_tty_read(mount_dev)) > 0){
-        if(sl_RB_write(RBin, (uint8_t*) mount_dev->buf, got)){
-            got = -1;
-            break;
+    if(resp && resplen) resp[0] = 0;
+    size_t pos = 0;
+    double t0 = sl_dtime();
+    bool gotEOM = false;
+    while(sl_dtime() - t0 < sertmout){
+        int got = sl_tty_read(mount_dev);
+        if(got < 0){
+            WARN("sl_tty_read()");
+            if(resp && resplen) resp[pos < resplen ? pos : resplen-1] = 0;
+            DBG("pos: %zd, resp: '%s'", pos, resp);
+            return false;
+        } else if(pos && got == 0) break; // break if we already receive the message
+        for(int i = 0; i < got; ++i){
+            char c = mount_dev->buf[i];
+            if(c == '\r' || c == '\n') continue; // WTF?
+            if(c == '#'){ // end of message
+                gotEOM = true;
+                //if(resp && resplen) resp[pos < resplen ? pos : resplen-1] = 0;
+                //DBG("Got end of message: '%s' (waited for %gs)", resp, );
+                //return true;
+                break;
+            }
+            if(resp && pos + 1 < resplen) resp[pos++] = c;
         }
     }
-    if(got < 0) return false;
-    return ret;
+    DBG("Waited for %gs", sl_dtime() - t0);
+    if(resp && resplen){
+        resp[pos < resplen ? pos : resplen-1] = 0;
+        DBG("pos: %zd, resp: '%s'", pos, resp);
+        // 10Micron/LX200-like: '0' or 'E'/'e' -> error; '1' - OK
+        if(gotEOM || (pos == 1 && resp[0] == '1')) return true;
+    }
+    return false;
+}
+
+/**
+ * @brief write_cmd - try to write command to mount and check answer for not false/error
+ * @param cmd - string with command or NULL just to clear all incoming data
+ * @param haveanswer - true if mount have to answer to `cmd`
+ * @return false on write/read error or error in answer
+ */
+static bool write_cmd(const char *cmd, bool haveanswer){
+    char buf[128];
+    if(!haveanswer){
+        DBG("Command '%s'", cmd);
+        if(!mount_dev || !cmd){
+            DBG("no mount device?");
+            return false;
+        }
+        if(sl_tty_write(mount_dev->comfd, cmd, strlen(cmd))){
+            DBG("Error writing request '%s'", cmd);
+            return false;
+        }
+        while(sl_tty_read(mount_dev) > 0);
+    }else if(!send_cmd_resp(cmd, buf, 128)){
+        DBG("Error writing command '%s'", cmd);
+        return false;
+    }
+    return true;
+}
+
+// return current mount status
+mount_status_t mount_status(){
+    mount_status_t curst = (mount_status_t)atomic_load(&mountstatus);
+    if(curst == MNT_S_OFF) return curst; // do nothing when mount if off
+    if(!mount_dev){
+        atomic_store(&mountstatus, MNT_S_OFF);
+        return MNT_S_OFF;
+    }
+    char resp[64];
+    pthread_mutex_lock(&mntdev_mutex);
+    int ntry = 0;
+    for(; ntry < 3; ++ntry)
+        if(send_cmd_resp(CMD_GETSTAT, resp, 64)) break;
+    if(ntry == 3){ // mount not responded -> set status OFF
+        curst = MNT_S_OFF;
+    }else{ // parsing of status
+        char *nxt = NULL;
+        long statN = strtol(resp, &nxt, 10);
+        //DBG("resp=%s, nxt=%s, stat=%ld", resp, nxt, statN);
+        if(!nxt || *nxt || statN >= MNT_S_ERROR || statN < 0) curst = MNT_S_ERROR; // wrong answer
+        else curst = (mount_status_t) statN;
+        DBG("STATUS: %s (%d)", statuses[curst], curst);
+    }
+    pthread_mutex_unlock(&mntdev_mutex);
+    atomic_store(&mountstatus, curst);
+    return curst;
 }
 
 // check if mount connected
 static bool chkconn(){
+    FNAME();
     int r = 0;
     do{ // clear incoming buffer @ start
         r = sl_tty_read(mount_dev);
     }while(r > 0);
-    if(r < 0) return false;
-    write_cmd("#"); // clear cmd buffer
-    sl_RB_clearbuf(RBin);
-    bool w = write_cmd(":SB0#");
-    sl_RB_clearbuf(RBin);
-    return w;
+    if(r < 0){
+        WARN("Error reading");
+        DBG("error reading: got %d", r);
+        return false;
+    }
+    write_cmd("#", false); // clear cmd buffer
+    bool ret = false;
+    for(int i = 0; i < 5; ++i){
+        DBG("Try %d", i+1);
+        ret = write_cmd(CMD_BAUDRATE, true);
+        if(ret) break;
+    }
+    return ret;
 }
 
-
 // try to guess serial speed & set 115200
 static bool guess_speed(){
     if(!mount_dev) return false;
@@ -242,6 +335,7 @@ static bool guess_speed(){
     const int speeds[SPDBUFSZ] = {57600, 38400, 19200, 9600, 4800, 2400, 1200};
     int idx = 0;
     for(; idx < SPDBUFSZ; ++idx){
+        DBG("try speed %d", speeds[idx]);
         mount_dev->speed = speeds[idx];
         sl_tty_t *trydev = sl_tty_open(mount_dev, 1);
         if(!trydev) continue;
@@ -252,6 +346,7 @@ static bool guess_speed(){
     close(mount_dev->comfd);
     mount_dev->speed = 115200;
     if(!sl_tty_open(mount_dev, 1)) return false;
+    DBG("OK, opened @ 115200");
 #undef SPDBUFSZ
     return true;
 }
@@ -261,14 +356,16 @@ bool mount_connect(){
     if(isemulated) return true;
     if(!mount_dev) return false;
     pthread_mutex_lock(&mntdev_mutex);
-    if(!chkconn() && !guess_speed()) return false;
+    if(!chkconn() && !guess_speed()){
+        pthread_mutex_unlock(&mntdev_mutex);
+        return false;
+    }
     bool ret = true;
-    if(!write_cmd(":STOP#")) ret = false; // stop tracking after poweron
-    sl_RB_clearbuf(RBin);
-    if(!write_cmd(":U2#")) ret = false; // set high precision
-    sl_RB_clearbuf(RBin);
-    if(!write_cmd(":So10#")) ret = false; // set minimum altitude to 10 degrees
-    sl_RB_clearbuf(RBin);
+    if(!write_cmd(CMD_STOP, false)) ret = false; // stop tracking after poweron
+    if(!write_cmd(CMD_HIGHPREC, false)) ret = false; // set high precision
+    char buf[64];
+    snprintf(buf, 63, CMD_SETMINALT, 10);
+    if(!write_cmd(buf, true)) ret = false; // set minimum altitude to 10 degrees
     pthread_mutex_unlock(&mntdev_mutex);
     if(ret) LOGMSG("Connected to %s@115200", mount_dev->portname);
     else LOGERR("Can't write commands to mount");
@@ -292,23 +389,71 @@ void mount_disconnect(){
  * @return true if all OK
  */
 bool mount_point(double ra, double dec){
-    char buf[RADEC_STR_MAXLEN];
-    radec2str(ra, dec, buf);
-    DBG("Set RA/Decl to %s", buf);
-    LOGMSG("Try to set RA/Decl to %s", buf);
+    char rastr[RADEC_STR_MAXLEN], decstr[RADEC_STR_MAXLEN];
     norm_RADEC(&ra, &dec);
+    ra2str(ra, rastr);
+    dec2str(dec, decstr);
+    DBG("Set RA/Decl to %s/%s", rastr, decstr);
+    LOGMSG("Try to set RA/Decl to %s/%s", rastr, decstr);
     if(isemulated) return point_emulation(ra, dec);
-    // run real pointing to ra(hrs) & dec (deg)
-    return false;
+    if(!mount_dev) return false;
+    pthread_mutex_lock(&mntdev_mutex);
+    char cmd[128];
+    bool ret = false;
+    snprintf(cmd, 127, CMD_SETRA, rastr); // send RA
+    if(!write_cmd(cmd, true)) goto retn;
+    snprintf(cmd, 127, CMD_SETDEC, decstr); // send Dec
+    if(!write_cmd(cmd, true)) goto retn;
+    if(send_cmd_resp(CMD_SLEWRADEC, cmd, 127)){ // check error
+        if(*cmd != '0'){
+            WARNX("Slew error, answer: %s", cmd);
+            LOGWARN("Slew error, answer: %s", cmd);
+            goto retn;
+        }
+    }
+    ret = true;
+retn:
+    pthread_mutex_unlock(&mntdev_mutex);
+    return ret;
 }
 
+/**
+ * @brief mount_pointAZ - point to stationary coordinates
+ * @param A - azumuth (degrees): CLOCKWISE FROM NORTH!!!
+ * @param Z - zenith (degrees)
+ * @return false if failed
+ */
 bool mount_pointAZ(double A, double Z){
-    DBG("Point to stationary object, A=%g, Z=%g", A, Z);
+    char altstr[RADEC_STR_MAXLEN], azstr[RADEC_STR_MAXLEN];
+    if(!normAZ(&A, &Z)){
+        DBG("Wrong Z?");
+        return false;
+    }
+    d2sDMS(A, azstr, RADEC_STR_MAXLEN);
+    d2sDMS(90. - Z, altstr, RADEC_STR_MAXLEN);
+    DBG("Point to stationary object, Az=%s, Alt=%s", azstr, altstr);
     if(isemulated){
         return pointAZ_emulation(A, Z);
     }
-    ; // run real pointing
-    return false;
+    if(!mount_dev) return false;
+    pthread_mutex_lock(&mntdev_mutex);
+    bool ret = false;
+    char cmd[128];
+    snprintf(cmd, 127, CMD_SETALT, altstr);
+    if(!write_cmd(cmd, true)) goto retn;
+    snprintf(cmd, 127, CMD_SETZD, azstr);
+    if(!write_cmd(cmd, true)) goto retn;
+    if(send_cmd_resp(CMD_GOTOAZ, cmd, 127)){ // returned not '0'
+        if(*cmd != '0'){
+            WARNX("Goto error, answer: %s", cmd);
+            LOGWARN("Goto error, answer: %s", cmd);
+            goto retn;
+        }
+    }
+    ret = true;
+retn:
+    pthread_mutex_unlock(&mntdev_mutex);
+    return ret;
 }
 
 void set_emulation_mode(){
@@ -322,8 +467,49 @@ mount_status_t mount_getcoords(double *ra, double *dec){
         DBG("Emulated coordinates: %gh, %gdeg", RAD2HRS(*ra), RAD2DEG(*dec));
         return emulation_status();
     }
-    ; // get real coordinates
-    return mount_status();
+    if(!mount_dev) return MNT_S_ERROR;
+    char resp[64];
+    mount_status_t st = MNT_S_ERROR;
+    pthread_mutex_lock(&mntdev_mutex);
+    if(!send_cmd_resp(CMD_GETRA, resp, 64)) goto retn;
+    if(!str2coord(resp, ra)) goto retn;
+    if(!send_cmd_resp(CMD_GETDEC, resp, 64)) goto retn;
+    if(!str2coord(resp, dec)) goto retn;
+    st = MNT_S_STATAMOUNT;
+retn:
+    pthread_mutex_unlock(&mntdev_mutex);
+    if(st == MNT_S_STATAMOUNT) st = mount_status();
+    return st;
+}
+
+mount_status_t mount_getaz(double *a, double *z){
+    if(!a || !z) return MNT_S_ERROR;
+    if(isemulated){
+        double ra, dec;
+        get_emul_coords(&ra, &dec);
+        polarCrds_t P = {.dec = dec, .ra = ra};
+        horizCrds_t H;
+        double LST;
+        get_LST(NULL, &LST);
+        eq2hor(&P, &H, LST);
+        *a = H.az; *z = H.zd;
+        return emulation_status();
+    }
+    if(!mount_dev) return MNT_S_ERROR;
+    char resp[64];
+    mount_status_t st = MNT_S_ERROR;
+    pthread_mutex_lock(&mntdev_mutex);
+    if(!send_cmd_resp(CMD_GETAZIM, resp, 64)) goto retn;
+    if(!str2coord(resp, a)) goto retn;
+    if(!send_cmd_resp(CMD_GETALT, resp, 64)) goto retn;
+    double alt;
+    if(!str2coord(resp, &alt)) goto retn;
+    *z = 90. - alt;
+    st = MNT_S_STATAMOUNT;
+retn:
+    pthread_mutex_unlock(&mntdev_mutex);
+    if(st == MNT_S_STATAMOUNT) st = mount_status();
+    return st;
 }
 
 /**
@@ -333,10 +519,27 @@ mount_status_t mount_getcoords(double *ra, double *dec){
 bool mount_stop(){
     if(isemulated){
         emulation_stop();
-    }else{
-        return mount_stop();
+        return true;
     }
-    return true;
+    if(!mount_dev) return false;
+    bool ret = false;
+    pthread_mutex_lock(&mntdev_mutex);
+    if(write_cmd(CMD_STOP, false)) ret = true;
+    pthread_mutex_unlock(&mntdev_mutex);
+    return ret;
+}
+
+bool mount_tracking_stop(){
+    if(isemulated){
+        emulation_stop();
+        return true;
+    }
+    if(!mount_dev) return false;
+    bool ret = false;
+    pthread_mutex_lock(&mntdev_mutex);
+    if(write_cmd(CMD_TRKSTOP, false)) ret = true;
+    pthread_mutex_unlock(&mntdev_mutex);
+    return ret;
 }
 
 /**
@@ -346,10 +549,15 @@ bool mount_stop(){
 bool mount_tracking_start(){
     if(isemulated){
         emul_start_tracking();
-    }else{
-        return false;
+        return true;
     }
-    return true;
+    if(!mount_dev) return false;
+    bool ret = false;
+    pthread_mutex_lock(&mntdev_mutex);
+    write_cmd(CMD_TRKSTOP, false); // workaround from command protocol if mount was stopped with :STOP#
+    if(write_cmd(CMD_TRKSTART, false)) ret = true;
+    pthread_mutex_unlock(&mntdev_mutex);
+    return ret;
 }
 
 /**
@@ -358,7 +566,8 @@ bool mount_tracking_start(){
  */
 bool mount_park(){
     if(isemulated) return pointAZ_emulation(ParkCoords.az, ParkCoords.zd);
-    return mount_pointAZ(ParkCoords.az, ParkCoords.zd);
+    // TODO: set lower limit to 0deg
+    return mount_pointAZ(RAD2DEG(ParkCoords.az), RAD2DEG(ParkCoords.zd));
 }
 
 /**