SSII

Auxiliary_utils/SiderealServoII/emulator.c

@@ -0,0 +1,115 @@
+/*
+ * This file is part of the SSII project.
+ * Copyright 2022 Edward V. Emelianov <edward.emelianoff@gmail.com>.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <math.h>
+#include <stdio.h>
+#include <usefull_macros.h>
+
+#include "emulator.h"
+#include "motlog.h"
+
+// HA/DEC of starting point, starting time
+static double Eha0 = 0., Edec0 = 0., Et0 = 0.;
+
+// monitor each 100ms motor's motion; stop when coords are the same for 3 times in a row
+// inistat - returning of binary command (if len equal to sizeof SSstat)
+void SSmotor_monitoring(SSstat *inistat){
+    DBG("Start monitoring");
+    //int32_t oldDec = 0, oldRA = 0; // old positions in encoder values (also for speed calculations)
+    SSstat prevstat = {0};
+    uint8_t start = 1, repeat = 0, errctr = 0;
+    int32_t oldRAm = 0, oldDm = 0;
+    double tlast = dtime();
+    if(inistat){ // get initial values
+        start = 0;
+        prevstat = *inistat;
+        oldRAm = prevstat.HAmot;
+        oldDm = prevstat.DECmot;
+    }
+    while(1){
+        if(start){ // didn't get initial values -> need to know first
+            if(!SSgetPartialstat(&prevstat)) continue;
+            start = 0;
+            tlast = dtime();
+            oldRAm = prevstat.HAmot;
+            oldDm = prevstat.DECmot;
+        }else{
+            if(!SSlog_motor_data(&prevstat, &tlast)){
+                WARNX("Hmmm... in %dth time", ++errctr);
+                if(++errctr > 10) break;
+                else continue;
+            }else errctr = 0;
+        }
+        if(prevstat.HAmot == oldRAm && prevstat.DECmot == oldDm){
+            if(++repeat > 2) break;
+        }else repeat = 0;
+        oldRAm = prevstat.HAmot;
+        oldDm = prevstat.DECmot;
+    }
+    DBG("End of monitoring");
+}
+
+
+/**
+ * @brief log_motor_data - get current config & put to log difference between values
+ * @param old (io) - old parameters value: as it was -> as it become
+ * @param told (io) - previous time (`dtime()` output) -> current time
+ * @return TRUE if all OK (if return FALSE, the `old` and `told` won't be changed!
+ */
+int SSlog_motor_data(SSstat *old, double *told){
+    SSstat s;
+    if(!old || !told) return FALSE;
+    if(!SSgetPartialstat(&s)) return FALSE;
+    double tnow = dtime(), tdif = tnow - *told;
+    mot_log(1, "%d\t%d\t%.1f\t%d\t%d\t%.1f", s.DECmot, s.DECenc, (s.DECmot - old->DECmot) / tdif,
+            s.HAmot, s.HAenc, (s.HAmot - old->HAmot) / tdif);
+    *old = s;
+    *told = tnow;
+    return TRUE;
+}
+
+// calculate next `target` position in current moment of time
+// ha changed as 15''/s + 15*sin(t*2pi/600s);
+// dec changed as 30*cos(t*2pi/600s)
+static void emul_next_pos(double *ha, double *dec, double tnew){
+    if(!ha || !dec) return;
+    double s1, c1, t = tnew - Et0;
+    sincos(t*2.*M_PI/600., &s1, &c1);
+    *ha = Eha0 + ARCSEC2DEG(15) * (t + s1);
+    *dec = Edec0 + ARCSEC2DEG(30) * c1;
+}
+
+// init emulated variables and start target's searching
+void SSstart_emulation(double ha_start, double dec_start){
+    Eha0 = ha_start;
+    Edec0 = dec_start;
+    Et0 = dtime();
+    DBG("Try to send short command");
+    SSscmd sc = {0};
+    // now calculate first starting position and try to catch it
+    double ha, dec;
+    emul_next_pos(&ha, &dec, Et0 + 1.);
+    mot_log(0, "goto ha=%d, dec=%d");
+    SSgoto(ha, dec);
+    SSwaitmoving();
+    // now we are near of the target: start to catch it
+    ;
+    while(SScmds(&sc) != sizeof(SSstat)) WARNX("SSCMDshort bad answer!");
+
+}
+