mountcontrol/LibSidServo/examples/dumpmoving.c

/*
 * 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 <math.h>
#include <signal.h>
#include <stdio.h>
#include <time.h>
#include <usefull_macros.h>

#include "sidservo.h"

typedef struct{
    int help;
    int verbose;
    char *logfile;
    char *coordsoutput;
} parameters;

static parameters G = {0};
static FILE *fcoords = NULL;

static sl_option_t cmdlnopts[] = {
    {"help",        NO_ARGS,    NULL,   'h',    arg_int,    APTR(&G.help),      "show this help"},
    {"verbose",     NO_ARGS,    NULL,   'v',    arg_none,   APTR(&G.verbose),   "verbose level (each -v adds 1)"},
    {"logfile",     NEED_ARG,   NULL,   'l',    arg_string, APTR(&G.logfile),   "log file name"},
    {"coordsfile",  NEED_ARG,   NULL,   'o',    arg_string, APTR(&G.coordsoutput),"output file with coordinates log"},
    end_option
};

void signals(int sig){
    if(sig){
        signal(sig, SIG_IGN);
        DBG("Get signal %d, quit.\n", sig);
    }
    LOGERR("Exit with status %d", sig);
    Mount.quit();
    exit(sig);
}

static conf_t Config = {
    .MountDevPath = "/dev/ttyUSB0",
    .MountDevSpeed = 19200,
    //.EncoderDevPath = "/dev/ttyUSB1",
    //.EncoderDevSpeed = 153000,
    .MountReqInterval = 0.1,
    .SepEncoder = 0
};

#define DEG2RAD(d)  (d/180.*M_PI)
#define RAD2DEG(d)  (d/M_PI*180.)


static void logmnt(mountdata_t *m){
    DBG("LOG");
    static double t0 = -1.;
    if(!fcoords) return;
    if(!m){ // write header
        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;
    double t = m->motposition.msrtime.tv_sec + (double)(m->motposition.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),
        RAD2DEG(m->encposition.X), RAD2DEG(m->encposition.Y),
        m->millis, m->temperature, m->voltage);
}

// dump moving but not longer than t secs; exit after 5 stopped events
static void dumpmoving(double t){
    mountdata_t mdata;
    DBG("Start dump");
    int ntries = 0;
    for(; ntries < 10; ++ntries){
        if(MCC_E_OK == Mount.getMountData(&mdata)) break;
    }
    if(ntries == 10){
        WARNX("Can't get mount data");
        LOGWARN("Can't get mount data");
    }
    suseconds_t usec = mdata.motposition.msrtime.tv_usec;
    int ctr = 0;
    double xlast = mdata.motposition.X, ylast = mdata.motposition.Y;
    double t0 = sl_dtime();
    DBG("usec = %ld", usec);
    while(sl_dtime() - t0 < t && ctr < 5){
        usleep(10000);
        if(MCC_E_OK != Mount.getMountData(&mdata)){ WARNX("Can't get data"); continue;}
        if(mdata.motposition.msrtime.tv_usec == usec) continue;
        DBG("Got new data, posX=%g, posY=%g", mdata.motposition.X, mdata.motposition.Y);
        usec = mdata.motposition.msrtime.tv_usec;
        if(fcoords) logmnt(&mdata);
        if(mdata.motposition.X != xlast || mdata.motposition.Y != ylast){
            xlast = mdata.motposition.X;
            ylast = mdata.motposition.Y;
            ctr = 0;
        }else ++ctr;
    }
}

int main(int argc, char **argv){
    sl_init();
    sl_parseargs(&argc, &argv, cmdlnopts);
    if(G.help) sl_showhelp(-1, cmdlnopts);
    sl_loglevel_e lvl = G.verbose + LOGLEVEL_ERR;
    if(lvl >= LOGLEVEL_AMOUNT) lvl = LOGLEVEL_AMOUNT - 1;
    if(G.logfile) OPENLOG(G.logfile, lvl, 1);
    if(G.coordsoutput){
        if(!(fcoords = fopen(G.coordsoutput, "w")))
            ERRX("Can't open %s", G.coordsoutput);
        logmnt(NULL);
    }
    time_t curtime = time(NULL);
    LOGMSG("Started @ %s", ctime(&curtime));
    LOGMSG("Mount device %s @ %d", Config.MountDevPath, Config.MountDevSpeed);
    LOGMSG("Encoder device %s @ %d", Config.EncoderDevPath, Config.EncoderDevSpeed);
    mcc_errcodes_t e = Mount.init(&Config);
    if(e != MCC_E_OK){
        WARNX("Can't init devices");
        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.)))
        ERRX("Can't move to 45, 45");
    dumpmoving(30.);
    Mount.moveTo(0., 0.);
    dumpmoving(30.);
    signals(0);
    return 0;
}