mountcontrol/LibSidServo/examples/scmd_traectory.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 <pthread.h>
#include <signal.h>
#include <usefull_macros.h>

#include "dump.h"
#include "sidservo.h"
#include "simpleconv.h"
#include "traectories.h"

// calculate some traectory and try to run over it

typedef struct{
    int help;
    int Ncycles;        // n cycles to wait stop
    double reqint;      // requests interval (seconds)
    double Xmax;        // maximal X to stop
    double Ymax;        // maximal Y to stop
    double tmax;        // maximal time of emulation
    double X0;          // starting point of traectory (-30..30 degr)
    double Y0;          // -//-
    char *coordsoutput; // dump file
    char *tfn;          // traectory function name
} parameters;

static FILE *fcoords = NULL;
static pthread_t dthr;
static parameters G = {
    .Ncycles = 40,
    .reqint = 0.1,
    .tfn = "sincos",
    .Xmax = 45.,
    .Ymax = 45.,
    .tmax = 300., // 5 minutes
    .X0 = 10.,
    .Y0 = 10.,
};

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 in stopped state (default: 40)"},
    {"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 second)"},
    {"traectory",   NEED_ARG,   NULL,   't',    arg_string, APTR(&G.tfn),       "used traectory function (default: sincos)"},
    {"xmax",        NEED_ARG,   NULL,   'X',    arg_double, APTR(&G.Xmax),      "maximal X coordinate for traectory (default: 45 degrees)"},
    {"ymax",        NEED_ARG,   NULL,   'Y',    arg_double, APTR(&G.Ymax),      "maximal X coordinate for traectory (default: 45 degrees)"},
    {"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)"},
    end_option
};

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

void signals(int sig){
    pthread_cancel(dthr);
    if(sig){
        signal(sig, SIG_IGN);
        DBG("Get signal %d, quit.\n", sig);
    }
    Mount.quit();
    if(fcoords) fclose(fcoords);
    exit(sig);
}

static void *dumping(void _U_ *u){
    dumpmoving(fcoords, 3600., G.Ncycles);
    return NULL;
}

// calculate
static void runtraectory(traectory_fn *tfn){
    if(!tfn) return;
    coords_t telXY, traectXY;
    double t0 = sl_dtime();
    uint32_t susec_last = 0;
    while(1){
        if(!telpos(&telXY)){
            WARNX("No next telescope position");
            return;
        }
        if(telXY.msrtime.tv_usec == susec_last) continue; // last measure - don't mind
        susec_last = telXY.msrtime.tv_usec;
        double t = sl_dtime();
        if(telXY.X > G.Xmax || telXY.Y > G.Ymax || t - t0 > G.tmax) break;
        if(!traectory_point(&traectXY, t)) break;
        DBG("%g: dX=%.1f'', dY=%.1f''", t-t0, RAD2ASEC(traectXY.X-telXY.X), RAD2ASEC(traectXY.Y-telXY.Y));
    }
    WARNX("No next traectory point");
}

int main(int argc, char **argv){
    sl_init();
    sl_parseargs(&argc, &argv, cmdlnopts);
    if(G.help) sl_showhelp(-1, cmdlnopts);
    if(G.Xmax < 1. || G.Xmax > 90.) ERRX("Xmax should be 1..90 degrees");
    if(G.Ymax < 1. || G.Ymax > 90.) ERRX("Ymax should be 1..90 degrees");
    // convert to radians
    G.Xmax = DEG2RAD(G.Xmax); G.Ymax = DEG2RAD(G.Ymax);
    if(G.X0 < -30. || G.X0 > 30. || G.Y0 < -30. || G.Y0 > 30.)
        ERRX("X0 and Y0 should be -30..30 degrees");
    if(G.coordsoutput){
        if(!(fcoords = fopen(G.coordsoutput, "w")))
            ERRX("Can't open %s", G.coordsoutput);
    }else fcoords = stdout;
    Config.MountReqInterval = G.reqint;
    traectory_fn *tfn = traectory_by_name(G.tfn);
    if(!tfn){
        WARNX("Bad traectory name %s, should be one of", G.tfn);
        print_tr_names();
        return 1;
    }
    coords_t c = {.X = DEG2RAD(G.X0), .Y = DEG2RAD(G.Y0)};
    if(!init_traectory(tfn, &c)){
        ERRX("Can't init traectory");
        return 1;
    }
    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
    chk0(G.Ncycles);
    logmnt(fcoords, NULL);
    if(pthread_create(&dthr, NULL, dumping, NULL)) ERRX("Can't run dump thread");
    ;
    runtraectory(tfn);
    signals(0);
    return 0;
}