eddy/small_tel
1
0
Fork 0
mirror of https://github.com/eddyem/small_tel.git synced 2026-02-01 04:45:02 +03:00
Code Issues Packages Projects Releases Wiki Activity

is moving model really works?

Browse Source
This commit is contained in:
Edward Emelianov 2025-08-06 15:09:07 +03:00
parent 46ff11df58
commit 581cdc60f9
31 changed files with 1557 additions and 396 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

193
Auxiliary_utils/LibSidServo/PID.c Normal file
View File

@ -0,0 +1,193 @@
/*
* 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 <stdio.h>
#include <stdlib.h>
#include <strings.h>
#include "main.h"
#include "PID.h"
#include "serial.h"
PIDController_t *pid_create(const PIDpar_t *gain, size_t Iarrsz){
if(!gain || Iarrsz < 3) return NULL;
PIDController_t *pid = (PIDController_t*)calloc(1, sizeof(PIDController_t));
pid->gain = *gain;
pid->pidIarrSize = Iarrsz;
pid->pidIarray = (double*)calloc(Iarrsz, sizeof(double));
return pid;
}
// don't clear lastT!
void pid_clear(PIDController_t *pid){
if(!pid) return;
DBG("CLEAR PID PARAMETERS");
bzero(pid->pidIarray, sizeof(double) * pid->pidIarrSize);
pid->integral = 0.;
pid->prev_error = 0.;
pid->curIidx = 0;
}
void pid_delete(PIDController_t **pid){
if(!pid || !*pid) return;
if((*pid)->pidIarray) free((*pid)->pidIarray);
free(*pid);
*pid = NULL;
}
double pid_calculate(PIDController_t *pid, double error, double dt){
// calculate flowing integral
double oldi = pid->pidIarray[pid->curIidx], newi = error * dt;
//DBG("oldi/new: %g, %g", oldi, newi);
pid->pidIarray[pid->curIidx++] = newi;
if(pid->curIidx >= pid->pidIarrSize) pid->curIidx = 0;
pid->integral += newi - oldi;
double derivative = (error - pid->prev_error) / dt;
pid->prev_error = error;
double sum = pid->gain.P * error + pid->gain.I * pid->integral + pid->gain.D * derivative;
DBG("P=%g, I=%g, D=%g; sum=%g", pid->gain.P * error, pid->gain.I * pid->integral, pid->gain.D * derivative, sum);
return sum;
}
typedef struct{
PIDController_t *PIDC;
PIDController_t *PIDV;
} PIDpair_t;
typedef struct{
axis_status_t state;
coordval_t position;
coordval_t speed;
} axisdata_t;
/**
* @brief process - Process PID for given axis
* @param tagpos - given coordinate of target position
* @param endpoint - endpoint for this coordinate
* @param pid - pid itself
* @return calculated new speed or -1 for max speed
*/
static double getspeed(const coordval_t *tagpos, PIDpair_t *pidpair, axisdata_t *axis){
if(tagpos->t < axis->position.t || tagpos->t - axis->position.t > MCC_PID_MAX_DT){
DBG("target time: %g, axis time: %g - too big! (%g)", tagpos->t, axis->position.t, MCC_PID_MAX_DT);
return axis->speed.val; // data is too old or wrong
}
double error = tagpos->val - axis->position.val, fe = fabs(error);
PIDController_t *pid = NULL;
switch(axis->state){
case AXIS_SLEWING:
if(fe < MCC_MAX_POINTING_ERR){
axis->state = AXIS_POINTING;
DBG("--> Pointing");
pid = pidpair->PIDC;
}else{
DBG("Slewing...");
return -1.; // max speed for given axis
}
break;
case AXIS_POINTING:
if(fe < MCC_MAX_GUIDING_ERR){
axis->state = AXIS_GUIDING;
DBG("--> Guiding");
pid = pidpair->PIDV;
}else if(fe > MCC_MAX_POINTING_ERR){
DBG("--> Slewing");
axis->state = AXIS_SLEWING;
return -1.;
} else pid = pidpair->PIDC;
break;
case AXIS_GUIDING:
pid = pidpair->PIDV;
if(fe > MCC_MAX_GUIDING_ERR){
DBG("--> Pointing");
axis->state = AXIS_POINTING;
pid = pidpair->PIDC;
}else if(fe < MCC_MAX_ATTARGET_ERR){
DBG("At target");
// TODO: we can point somehow that we are at target or introduce new axis state
}else DBG("Current error: %g", fe);
break;
case AXIS_STOPPED: // start pointing to target; will change speed next time
DBG("AXIS STOPPED!!!!");
axis->state = AXIS_SLEWING;
return -1.;
case AXIS_ERROR:
DBG("Can't move from erroneous state");
return 0.;
}
if(!pid){
DBG("WTF? Where is a PID?");
return axis->speed.val;
}
if(tagpos->t < pid->prevT || tagpos->t - pid->prevT > MCC_PID_MAX_DT){
DBG("time diff too big: clear PID");
pid_clear(pid);
}
double dt = tagpos->t - pid->prevT;
if(dt > MCC_PID_MAX_DT) dt = MCC_PID_CYCLE_TIME;
pid->prevT = tagpos->t;
//DBG("CALC PID (er=%g, dt=%g)", error, dt);
double tagspeed = pid_calculate(pid, error, dt);
if(axis->state == AXIS_GUIDING) return axis->speed.val + tagspeed; // velocity-based
return tagspeed; // coordinate-based
}
/**
* @brief correct2 - recalculate PID and move telescope to new point with new speed
* @param target - target position (for error calculations)
* @param endpoint - stop point (some far enough point to stop in case of hang)
* @return error code
*/
mcc_errcodes_t correct2(const coordval_pair_t *target, const coordpair_t *endpoint){
static PIDpair_t pidX = {0}, pidY = {0};
if(!pidX.PIDC){
pidX.PIDC = pid_create(&Conf.XPIDC, MCC_PID_CYCLE_TIME / MCC_PID_REFRESH_DT);
if(!pidX.PIDC) return MCC_E_FATAL;
pidX.PIDV = pid_create(&Conf.XPIDV, MCC_PID_CYCLE_TIME / MCC_PID_REFRESH_DT);
if(!pidX.PIDV) return MCC_E_FATAL;
}
if(!pidY.PIDC){
pidY.PIDC = pid_create(&Conf.YPIDC, MCC_PID_CYCLE_TIME / MCC_PID_REFRESH_DT);
if(!pidY.PIDC) return MCC_E_FATAL;
pidY.PIDV = pid_create(&Conf.YPIDV, MCC_PID_CYCLE_TIME / MCC_PID_REFRESH_DT);
if(!pidY.PIDV) return MCC_E_FATAL;
}
mountdata_t m;
coordpair_t tagspeed;
if(MCC_E_OK != Mount.getMountData(&m)) return MCC_E_FAILED;
axisdata_t axis;
DBG("state: %d/%d", m.Xstate, m.Ystate);
axis.state = m.Xstate;
axis.position = m.encXposition;
axis.speed = m.encXspeed;
tagspeed.X = getspeed(&target->X, &pidX, &axis);
if(tagspeed.X < 0. || tagspeed.X > MCC_MAX_X_SPEED) tagspeed.X = MCC_MAX_X_SPEED;
axis_status_t xstate = axis.state;
axis.state = m.Ystate;
axis.position = m.encYposition;
axis.speed = m.encYspeed;
tagspeed.Y = getspeed(&target->Y, &pidY, &axis);
if(tagspeed.Y < 0. || tagspeed.Y > MCC_MAX_Y_SPEED) tagspeed.Y = MCC_MAX_Y_SPEED;
axis_status_t ystate = axis.state;
if(m.Xstate != xstate || m.Ystate != ystate){
DBG("State changed");
setStat(xstate, ystate);
}
DBG("TAG speeds: %g/%g", tagspeed.X, tagspeed.Y);
return Mount.moveWspeed(endpoint, &tagspeed);
}

40
Auxiliary_utils/LibSidServo/PID.h Normal file
View File

@ -0,0 +1,40 @@
/*
* 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/>.
*/
#pragma once
#include <stddef.h>
#include "sidservo.h"
typedef struct {
PIDpar_t gain; // PID gains
double prev_error; // Previous error
double integral; // Integral term
double *pidIarray; // array for Integral
double prevT; // time of previous correction
size_t pidIarrSize; // it's size
size_t curIidx; // and index of current element
} PIDController_t;
PIDController_t *pid_create(const PIDpar_t *gain, size_t Iarrsz);
void pid_clear(PIDController_t *pid);
void pid_delete(PIDController_t **pid);
double pid_calculate(PIDController_t *pid, double error, double dt);
mcc_errcodes_t correct2(const coordval_pair_t *target, const coordpair_t *endpoint);

64
Auxiliary_utils/LibSidServo/PID_test/PID.c Normal file
View File

@ -0,0 +1,64 @@
/*
* This file is part of the moving_model 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 <stdio.h>
#include <stdlib.h>
#include <strings.h>
#include <usefull_macros.h>
#include "PID.h"
PIDController_t *pid_create(PIDpar_t *gain, size_t Iarrsz){
if(!gain || Iarrsz < 3) return NULL;
PIDController_t *pid = (PIDController_t*)calloc(1, sizeof(PIDController_t));
pid->gain = *gain;
pid->pidIarrSize = Iarrsz;
pid->pidIarray = (double*)calloc(Iarrsz, sizeof(double));
return pid;
}
void pid_clear(PIDController_t *pid){
if(!pid) return;
DBG("CLEAR PID PARAMETERS");
bzero(pid->pidIarray, sizeof(double) * pid->pidIarrSize);
pid->integral = 0.;
pid->prev_error = 0.;
pid->curIidx = 0;
}
void pid_delete(PIDController_t **pid){
if(!pid || !*pid) return;
if((*pid)->pidIarray) free((*pid)->pidIarray);
free(*pid);
*pid = NULL;
}
double pid_calculate(PIDController_t *pid, double error, double dt){
// calculate flowing integral
double oldi = pid->pidIarray[pid->curIidx], newi = error * dt;
DBG("oldi/new: %g, %g", oldi, newi);
pid->pidIarray[pid->curIidx++] = newi;
if(pid->curIidx >= pid->pidIarrSize) pid->curIidx = 0;
pid->integral += newi - oldi;
double derivative = (error - pid->prev_error) / dt;
pid->prev_error = error;
double sum = pid->gain.P * error + pid->gain.I * pid->integral + pid->gain.D * derivative;
DBG("P=%g, I=%g, D=%g; sum=%g", pid->gain.P * error, pid->gain.I * pid->integral, pid->gain.D * derivative, sum);
return sum;
}

39
Auxiliary_utils/LibSidServo/PID_test/PID.h Normal file
View File

@ -0,0 +1,39 @@
/*
* This file is part of the moving_model 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/>.
*/
#pragma once
#include <stddef.h>
typedef struct{
double P, I, D;
} PIDpar_t;
typedef struct {
PIDpar_t gain; // PID gains
double prev_error; // Previous error
double integral; // Integral term
double *pidIarray; // array for Integral
size_t pidIarrSize; // it's size
size_t curIidx; // and index of current element
} PIDController_t;
PIDController_t *pid_create(PIDpar_t *gain, size_t Iarrsz);
void pid_clear(PIDController_t *pid);
void pid_delete(PIDController_t **pid);
double pid_calculate(PIDController_t *pid, double error, double dt);

145
Auxiliary_utils/LibSidServo/PID_test/main.c
View File

@ -22,13 +22,18 @@
#include <usefull_macros.h>
#include "moving.h"
#include "PID.h"
// errors for states: slewing/pointing/guiding
#define MAX_POINTING_ERR (50.)
#define MAX_GUIDING_ERR (5.)
// 10-degrees zone - Coordinate-driven PID
#define MAX_POINTING_ERR (36000.)
// 1-arcminute zone - Velocity-dtiven PID
#define MAX_GUIDING_ERR (60.)
// timeout to "forget" old data from I sum array; seconds
#define PID_I_PERIOD (3.)
// PID for coordinate-driven and velocity-driven parts
static PIDController_t *pidC = NULL, *pidV = NULL;
static movemodel_t *model = NULL;
static FILE *coordslog = NULL;
@ -48,93 +53,64 @@ typedef struct{
double dTcorr;
double Tend;
double minerr;
double P, I, D;
double startcoord;
double error;
PIDpar_t gainC, gainV;
} pars;
static pars G = {
.ramptype = "t",
.dTmon = 0.01,
.dTcorr = 0.05,
.Tend = 100.,
.minerr = 0.1,
.P = 0.8,
.gainC.P = 0.1,
.gainV.P = 0.1,
.startcoord = 100.,
};
static limits_t limits = {
.min = {.coord = -1e6, .speed = 0.01, .accel = 0.1},
.max = {.coord = 1e6, .speed = 1e3, .accel = 500.},
.jerk = 10.
.max = {.coord = 6648000, .speed = 36000., .accel = 36000.}
};
typedef struct {
double kp, ki, kd; // PID gains
double prev_error; // Previous error
double integral; // Integral term
double *pidIarray; // array for Integral
size_t pidIarrSize; // it's size
size_t curIidx; // and index of current element
} PIDController;
static PIDController pid;
static sl_option_t opts[] = {
{"help", NO_ARGS, NULL, 'h', arg_int, APTR(&G.help), "show this help"},
{"ramp", NEED_ARG, NULL, 'r', arg_string, APTR(&G.ramptype), "ramp type: \"d\", \"t\" or \"s\" - dumb, trapezoid, s-type"},
{"tmon", NEED_ARG, NULL, 'T', arg_double, APTR(&G.dTmon), "time interval for monitoring (seconds, default: 0.001)"},
{"tcor", NEED_ARG, NULL, 't', arg_double, APTR(&G.dTcorr), "time interval for corrections (seconds, default: 0.05)"},
{"xlog", NEED_ARG, NULL, 'l', arg_string, APTR(&G.xlog), "log file name for coordinates logging"},
{"tend", NEED_ARG, NULL, 'e', arg_double, APTR(&G.Tend), "end time of monitoring (seconds, default: 100)"},
{"minerr", NEED_ARG, NULL, 'm', arg_double, APTR(&G.minerr), "minimal error for corrections (units, default: 0.1)"},
{"prop", NEED_ARG, NULL, 'P', arg_double, APTR(&G.P), "P-coefficient of PID"},
{"integ", NEED_ARG, NULL, 'I', arg_double, APTR(&G.I), "I-coefficient of PID"},
{"diff", NEED_ARG, NULL, 'D', arg_double, APTR(&G.D), "D-coefficient of PID"},
{"propC", NEED_ARG, NULL, 'P', arg_double, APTR(&G.gainC.P), "P-coefficient of coordinate-driven PID"},
{"integC", NEED_ARG, NULL, 'I', arg_double, APTR(&G.gainC.I), "I-coefficient of coordinate-driven PID"},
{"diffC", NEED_ARG, NULL, 'D', arg_double, APTR(&G.gainC.D), "D-coefficient of coordinate-driven PID"},
{"propV", NEED_ARG, NULL, 'p', arg_double, APTR(&G.gainV.P), "P-coefficient of velocity-driven PID"},
{"integV", NEED_ARG, NULL, 'i', arg_double, APTR(&G.gainV.I), "I-coefficient of velocity-driven PID"},
{"diffV", NEED_ARG, NULL, 'd', arg_double, APTR(&G.gainV.D), "D-coefficient of velocity-driven PID"},
{"xstart", NEED_ARG, NULL, '0', arg_double, APTR(&G.startcoord), "starting coordinate of target"},
{"error", NEED_ARG, NULL, 'E', arg_double, APTR(&G.error), "error range"},
// TODO: add parameters for limits setting
end_option
};
// calculate coordinate target for given time (starting from zero)
static double target_coord(double t){
if(t > 20. && t < 30.) return target_coord(20.);
double pos = 150. + 10. * sin(M_2_PI * t / 10.) + 0.02 * (drand48() - 0.5);
if(t > 20. && t < 30.) return 0.;
//double pos = G.startcoord + 15. * t + G.error * (drand48() - 0.5);
double pos = G.startcoord + 15. * sin(2*M_PI * t / 10.) + G.error * (drand48() - 0.5);
return pos;
}
/* P-only == oscillations
static double getNewSpeed(const moveparam_t *p, double targcoord, double dt){
double error = targcoord - p->coord;
if(fabs(error) < G.minerr) return p->speed;
return p->speed + error / dt / 500.;
}
*/
static void pid_init(PIDController *pid, double kp, double ki, double kd) {
pid->kp = fabs(kp);
pid->ki = fabs(ki);
pid->kd = fabs(kd);
pid->prev_error = 0.;
pid->integral = 0.;
pid->curIidx = 0;
pid->pidIarrSize = PID_I_PERIOD / G.dTcorr;
if(pid->pidIarrSize < 2) ERRX("I-array for PID have less than 2 elements");
pid->pidIarray = MALLOC(double, pid->pidIarrSize);
}
static void pid_clear(PIDController *pid){
if(!pid) return;
bzero(pid->pidIarray, sizeof(double) * pid->pidIarrSize);
pid->integral = 0.;
pid->prev_error = 0.;
pid->curIidx = 0;
}
static double getNewSpeed(const moveparam_t *p, double targcoord, double dt){
double error = targcoord - p->coord, fe = fabs(error);
PIDController_t *pid = NULL;
switch(state){
case Slewing:
if(fe < MAX_POINTING_ERR){
pid_clear(&pid);
pid_clear(pidC);
state = Pointing;
green("--> Pointing\n");
pid = pidC;
}else{
red("Slewing...\n");
return (error > 0.) ? limits.max.speed : -limits.max.speed;
@ -142,54 +118,55 @@ static double getNewSpeed(const moveparam_t *p, double targcoord, double dt){
break;
case Pointing:
if(fe < MAX_GUIDING_ERR){
pid_clear(&pid);
pid_clear(pidV);
state = Guiding;
green("--> Guiding\n");
pid = pidV;
}else if(fe > MAX_POINTING_ERR){
red("--> Slewing\n");
state = Slewing;
return (error > 0.) ? limits.max.speed : -limits.max.speed;
}
} else pid = pidC;
break;
case Guiding:
pid= pidV;
if(fe > MAX_GUIDING_ERR){
red("--> Pointing\n");
state = Pointing;
pid_clear(pidC);
pid = pidC;
}else if(fe < G.minerr){
green("At target\n");
//pid_clear(&pid);
//return p->speed;
}
}else printf("Current error: %g\n", fe);
break;
}
red("Calculate PID\n");
double oldi = pid.pidIarray[pid.curIidx], newi = error * dt;
pid.pidIarray[pid.curIidx++] = oldi;
if(pid.curIidx >= pid.pidIarrSize) pid.curIidx = 0;
pid.integral += newi - oldi;
double derivative = (error - pid.prev_error) / dt;
pid.prev_error = error;
DBG("P=%g, I=%g, D=%g", pid.kp * error, pid.integral, derivative);
double add = (pid.kp * error + pid.ki * pid.integral + pid.kd * derivative);
if(state == Pointing) add /= 3.;
else if(state == Guiding) add /= 7.;
DBG("ADD = %g; new speed = %g", add, p->speed + add);
if(state == Guiding) return p->speed + add / dt / 10.;
return add / dt;
if(!pid){
WARNX("where is PID?"); return p->speed;
}
double tagspeed = pid_calculate(pid, error, dt);
if(state == Guiding) return p->speed + tagspeed;
return tagspeed;
}
// ./moving -l coords -P.5 -I.05 -D1.5
// ./moving -l coords -P1.3 -D1.6
// -P0.8 -D0.1 -I0.02 -p20 -d.5 -i.02
// another: P0.8 -D0.1 -I0.02 -p5 -d0.9 -i0.1
static void start_model(double Tend){
double T = 0., Tcorr = 0.;//, Tlast = 0.;
double T = 0., Tcorr = 0.;
moveparam_t target;
uint64_t N = 0;
double errmax = 0., errsum = 0., errsum2 = 0.;
while(T <= Tend){
moveparam_t p;
movestate_t st = model->get_state(&p);
if(st == ST_MOVE) st = model->proc_move(&p, T);
double nextcoord = target_coord(T);
double error = nextcoord - p.coord;
if(state == Guiding){
double ae = fabs(error);
if(ae > errmax) errmax = ae;
errsum += error; errsum2 += error * error;
++N;
}
if(T - Tcorr >= G.dTcorr){ // check correction
double speed = getNewSpeed(&p, nextcoord, T - Tcorr);
target.coord = (speed > 0) ? p.coord + 5e5 : p.coord - 5e5;
@ -215,6 +192,9 @@ static void start_model(double Tend){
T, nextcoord, p.coord, p.speed, p.accel, error);
T += G.dTmon;
}
printf("\n\n\n"); red("Calculated errors in `guiding` mode:\n");
double mean = errsum / (double)N;
printf("max error: %g, mean error: %g, std: %g\n\n", errmax, mean, sqrt(errsum2/(double)N - mean*mean));
}
int main(int argc, char **argv){
@ -228,16 +208,15 @@ int main(int argc, char **argv){
if(G.dTmon <= 0.) ERRX("tmon should be > 0.");
if(G.dTcorr <= 0. || G.dTcorr > 1.) ERRX("tcor should be > 0. and < 1.");
if(G.Tend <= 0.) ERRX("tend should be > 0.");
pid_init(&pid, G.P, G.I, G.D);
fprintf(coordslog, "%-9s\t%-10s\t%-10s\t%-10s\t%-10s\t%-10s\n", "time", "target", "curpos", "speed", "accel", "error");
ramptype_t ramp = RAMP_AMOUNT;
if(*G.ramptype == 'd' || *G.ramptype == 'D') ramp = RAMP_DUMB;
else if(*G.ramptype == 't' || *G.ramptype == 'T') ramp = RAMP_TRAPEZIUM;
else if(*G.ramptype == 's' || *G.ramptype == 'S') ramp = RAMP_S;
else ERRX("Point \"d\" (dumb), \"s\" (s-type), or \"t\" (trapez) for ramp type");
model = init_moving(ramp, &limits);
pidC = pid_create(&G.gainC, PID_I_PERIOD / G.dTcorr);
pidV = pid_create(&G.gainV, PID_I_PERIOD / G.dTcorr);
if(!pidC || !pidV) ERRX("Can't init PID regulators");
model = init_moving(&limits);
if(!model) ERRX("Can't init moving model: check parameters");
fprintf(coordslog, "%-9s\t%-10s\t%-10s\t%-10s\t%-10s\t%-10s\n", "time", "target", "curpos", "speed", "accel", "error");
start_model(G.Tend);
pid_delete(&pidC);
pid_delete(&pidV);
fclose(coordslog);
return 0;
}

20
Auxiliary_utils/LibSidServo/PID_test/moving.c
View File

@ -24,12 +24,9 @@
#include "moving.h"
#include "moving_private.h"
#include "Dramp.h"
#include "Sramp.h"
#include "Tramp.h"
//static pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;
static movemodel_t *model = NULL;
static movemodel_t *model = &trapez;
double coord_tolerance = COORD_TOLERANCE_DEFAULT;
double time_tick = TIME_TICK_DEFAULT;
@ -56,21 +53,8 @@ static void chkminmax(double *min, double *max){
*max = t;
}
movemodel_t *init_moving(ramptype_t type, limits_t *l){
movemodel_t *init_moving(limits_t *l){
if(!l) return FALSE;
switch(type){
case RAMP_DUMB:
model = &dumb;
break;
case RAMP_TRAPEZIUM:
model = &trapez;
break;
case RAMP_S:
model = &s_shaped;
break;
default:
return FALSE;
}
if(!model->init_limits) return NULL;
moveparam_t *max = &l->max, *min = &l->min;
if(min->speed < 0.) min->speed = -min->speed;

10
Auxiliary_utils/LibSidServo/PID_test/moving.h
View File

@ -26,13 +26,6 @@
#define TIME_TICK_MIN (1e-9)
#define TIME_TICK_MAX (10.)
typedef enum{
RAMP_DUMB, // no ramp: infinite acceleration/deceleration
RAMP_TRAPEZIUM, // trapezium ramp
RAMP_S, // s-shaped ramp
RAMP_AMOUNT
} ramptype_t;
typedef enum{
ST_STOP, // stopped
ST_MOVE, // moving
@ -48,7 +41,6 @@ typedef struct{ // all values could be both as positive and negative
typedef struct{
moveparam_t min;
moveparam_t max;
double jerk;
} limits_t;
typedef struct{
@ -64,7 +56,7 @@ typedef struct{
extern double coord_tolerance;
double nanot();
movemodel_t *init_moving(ramptype_t type, limits_t *l);
movemodel_t *init_moving(limits_t *l);
int init_coordtol(double tolerance);
int init_timetick(double tick);
int move_to(moveparam_t *target, double t);

17
Auxiliary_utils/LibSidServo/examples/SSIIconf.c
View File

@ -51,10 +51,10 @@ static sl_option_t confopts[] = {
end_option
};
static void dumpaxe(char axe, axe_config_t *c){
static void dumpaxis(char axis, axis_config_t *c){
#define STRUCTPAR(p) (c)->p
#define DUMP(par) do{printf("%c%s=%g\n", axe, #par, STRUCTPAR(par));}while(0)
#define DUMPD(par) do{printf("%c%s=%g\n", axe, #par, RAD2DEG(STRUCTPAR(par)));}while(0)
#define DUMP(par) do{printf("%c%s=%g\n", axis, #par, STRUCTPAR(par));}while(0)
#define DUMPD(par) do{printf("%c%s=%g\n", axis, #par, RAD2DEG(STRUCTPAR(par)));}while(0)
DUMPD(accel);
DUMPD(backlash);
DUMPD(errlimit);
@ -99,14 +99,15 @@ static void dumpHWconf(){
#define DUMPD(par) do{printf("%s=%g\n", #par, RAD2DEG(STRUCTPAR(par)));}while(0)
#define DUMPU8(par) do{printf("%s=%u\n", #par, (uint8_t)STRUCTPAR(par));}while(0)
#define DUMPU32(par) do{printf("%s=%u\n", #par, (uint32_t)STRUCTPAR(par));}while(0)
green("X axe configuration:\n");
dumpaxe('X', &HW.Xconf);
green("X axis configuration:\n");
dumpaxis('X', &HW.Xconf);
green("X bits:\n");
dumpxbits(&HW.xbits);
green("Y axe configuration:\n");
dumpaxe('Y', &HW.Yconf);
green("Y axis configuration:\n");
dumpaxis('Y', &HW.Yconf);
green("Y bits:\n");
dumpybits(&HW.ybits);
green("Other:\n");
printf("address=%d\n", HW.address);
DUMP(eqrate);
DUMP(eqadj);
@ -142,10 +143,12 @@ int main(int argc, char** argv){
}
if(MCC_E_OK != Mount.init(sconf)) ERRX("Can't init mount");
if(MCC_E_OK != Mount.getHWconfig(&HW)) ERRX("Can't read configuration");
/*
char *c = sl_print_opts(confopts, TRUE);
green("Got configuration:\n");
printf("%s\n", c);
FREE(c);
*/
dumpHWconf();
/*
if(G.hwconffile && G.writeconf){

25
Auxiliary_utils/LibSidServo/examples/conf.c
View File

@ -31,6 +31,18 @@ static conf_t Config = {
.EncoderReqInterval = 0.05,
.SepEncoder = 2,
.EncoderSpeedInterval = 0.1,
.XPIDC.P = 0.8,
.XPIDC.I = 0.1,
.XPIDC.D = 0.3,
.XPIDV.P = 1.,
.XPIDV.I = 0.01,
.XPIDV.D = 0.2,
.YPIDC.P = 0.8,
.YPIDC.I = 0.1,
.YPIDC.D = 0.3,
.YPIDV.P = 0.5,
.YPIDV.I = 0.2,
.YPIDV.D = 0.5,
};
static sl_option_t opts[] = {
@ -44,6 +56,19 @@ static sl_option_t opts[] = {
{"EncoderXDevPath", NEED_ARG, NULL, 0, arg_string, APTR(&Config.EncoderXDevPath), "path to X encoder (/dev/encoderX0)"},
{"EncoderYDevPath", NEED_ARG, NULL, 0, arg_string, APTR(&Config.EncoderYDevPath), "path to Y encoder (/dev/encoderY0)"},
{"EncoderSpeedInterval", NEED_ARG,NULL, 0, arg_double, APTR(&Config.EncoderSpeedInterval),"interval of speed calculations, s"},
{"RunModel", NEED_ARG, NULL, 0, arg_int, APTR(&Config.RunModel), "instead of real hardware run emulation"},
{"XPIDCP", NEED_ARG, NULL, 0, arg_double, APTR(&Config.XPIDC.P), "P of X PID (coordinate driven)"},
{"XPIDCI", NEED_ARG, NULL, 0, arg_double, APTR(&Config.XPIDC.I), "I of X PID (coordinate driven)"},
{"XPIDCD", NEED_ARG, NULL, 0, arg_double, APTR(&Config.XPIDC.D), "D of X PID (coordinate driven)"},
{"YPIDCP", NEED_ARG, NULL, 0, arg_double, APTR(&Config.YPIDC.P), "P of Y PID (coordinate driven)"},
{"YPIDCI", NEED_ARG, NULL, 0, arg_double, APTR(&Config.YPIDC.I), "I of Y PID (coordinate driven)"},
{"YPIDCD", NEED_ARG, NULL, 0, arg_double, APTR(&Config.YPIDC.D), "D of Y PID (coordinate driven)"},
{"XPIDVP", NEED_ARG, NULL, 0, arg_double, APTR(&Config.XPIDV.P), "P of X PID (velocity driven)"},
{"XPIDVI", NEED_ARG, NULL, 0, arg_double, APTR(&Config.XPIDV.I), "I of X PID (velocity driven)"},
{"XPIDVD", NEED_ARG, NULL, 0, arg_double, APTR(&Config.XPIDV.D), "D of X PID (velocity driven)"},
{"YPIDVP", NEED_ARG, NULL, 0, arg_double, APTR(&Config.YPIDV.P), "P of Y PID (velocity driven)"},
{"YPIDVI", NEED_ARG, NULL, 0, arg_double, APTR(&Config.YPIDV.I), "I of Y PID (velocity driven)"},
{"YPIDVD", NEED_ARG, NULL, 0, arg_double, APTR(&Config.YPIDV.D), "D of Y PID (velocity driven)"},
end_option
};

5
Auxiliary_utils/LibSidServo/examples/dump.c
View File

@ -115,7 +115,7 @@ void dumpmoving(FILE *fcoords, double t, int N){
enct = tmsr;
if(fcoords) logmnt(fcoords, &mdata);
if(mdata.millis == mdmillis) continue;
//DBG("ctr=%d", ctr);
//DBG("ctr=%d, motpos=%g/%g", ctr, mdata.motXposition.val, mdata.motYposition.val);
mdmillis = mdata.millis;
if(mdata.motXposition.val != xlast || mdata.motYposition.val != ylast){
xlast = mdata.motXposition.val;
@ -123,10 +123,11 @@ void dumpmoving(FILE *fcoords, double t, int N){
ctr = 0;
}else ++ctr;
}
DBG("Exit dumping; tend=%g, tmon=%g", t, Mount.currentT() - t0);
}
/**
* @brief waitmoving - wait until moving by both axes stops at least for N cycles
* @brief waitmoving - wait until moving by both axiss stops at least for N cycles
* @param N - amount of stopped cycles
*/
void waitmoving(int N){

26
Auxiliary_utils/LibSidServo/examples/dumpmoving_dragNtrack.c
View File

@ -120,16 +120,13 @@ static coordpair_t lastTag = {0}, lastSpeed = {0};
// slew to given position and start tracking
// pos/speed in deg and deg/s
static mcc_errcodes_t gotos(coordpair_t *target, coordpair_t *speed){
static mcc_errcodes_t gotos(const coordpair_t *target, const coordpair_t *speed){
short_command_t cmd = {0};
DBG("Try to move to (%g, %g) with speed (%g, %g)",
target->X, target->Y, speed->X, speed->Y);
target->X = DEG2RAD(target->X);
target->Y = DEG2RAD(target->Y);
speed->X = DEG2RAD(speed->X);
speed->Y = DEG2RAD(speed->Y);
cmd.Xmot = target->X; cmd.Ymot = target->Y;
cmd.Xspeed = speed->X; cmd.Yspeed = speed->Y;
cmd.Xmot = DEG2RAD(target->X); cmd.Ymot = DEG2RAD(target->Y);
cmd.Xspeed = DEG2RAD(speed->X);
cmd.Yspeed = DEG2RAD(speed->Y);
lastTag = *target;
lastSpeed = *speed;
/*cmd.xychange = 1;
@ -142,7 +139,8 @@ static mcc_errcodes_t return2zero(){
short_command_t cmd = {0};
DBG("Try to move to zero");
cmd.Xmot = 0.; cmd.Ymot = 0.;
cmd.Xspeed = DEG2RAD(10.); cmd.Yspeed = DEG2RAD(10.);
cmd.Xspeed = MCC_MAX_X_SPEED;
cmd.Yspeed = MCC_MAX_Y_SPEED;
/*cmd.xychange = 1;
cmd.XBits = 100;
cmd.YBits = 20;*/
@ -151,11 +149,11 @@ static mcc_errcodes_t return2zero(){
static mcc_errcodes_t mkcorr(coordpair_t *adder, coordpair_t *time){
long_command_t cmd = {0};
cmd.Xspeed = lastSpeed.X;
cmd.Yspeed = lastSpeed.Y;
cmd.Xmot = lastTag.X;
cmd.Ymot = lastTag.Y;
cmd.Xadder = adder->X; cmd.Yadder = adder->Y;
cmd.Xspeed = DEG2RAD(lastSpeed.X);
cmd.Yspeed = DEG2RAD(lastSpeed.Y);
cmd.Xmot = DEG2RAD(lastTag.X);
cmd.Ymot = DEG2RAD(lastTag.Y);
cmd.Xadder = DEG2RAD(adder->X); cmd.Yadder = DEG2RAD(adder->Y);
cmd.Xatime = time->X; cmd.Yatime = time->Y;
return Mount.longCmd(&cmd);
}
@ -218,7 +216,7 @@ int main(int argc, char **argv){
sleep(5);
// return to zero and wait
green("Return 2 zero and wait\n");
return2zero();
if(!return2zero()) ERRX("Can't return");
Wait(0., 0);
Wait(0., 1);
// wait moving ends

17
Auxiliary_utils/LibSidServo/examples/dumpswing.c
View File

@ -55,7 +55,7 @@ static sl_option_t cmdlnopts[] = {
{"coordsfile", NEED_ARG, NULL, 'o', arg_string, APTR(&G.coordsoutput),"output file with coordinates log"},
{"axis", NEED_ARG, NULL, 'a', arg_string, APTR(&G.axis), "axis to move (X or Y)"},
{"period", NEED_ARG, NULL, 'p', arg_double, APTR(&G.period), "swinging period (could be not reached if amplitude is too small) - not more than 900s (default: 1)"},
{"amplitude", NEED_ARG, NULL, 'A', arg_double, APTR(&G.amplitude), "max amplitude (could be not reaced if period is too small) - not more than 45deg (default: 5)"},
{"amplitude", NEED_ARG, NULL, 'A', arg_double, APTR(&G.amplitude), "max amplitude (could be not reached if period is too small): [-45:45]deg (default: 5)"},
{"nswings", NEED_ARG, NULL, 'N', arg_int, APTR(&G.Nswings), "amount of swing periods (default: 10)"},
{"conffile", NEED_ARG, NULL, 'C', arg_int, APTR(&G.conffile), "configuration file name"},
end_option
@ -89,12 +89,12 @@ void waithalf(double t){
if(mdata.millis == millis) continue;
millis = mdata.millis;
if(mdata.motXposition.val != xlast || mdata.motYposition.val != ylast){
DBG("NEQ: old=%g, now=%g", RAD2DEG(ylast), RAD2DEG(mdata.motYposition.val));
//DBG("NEQ: old=%g, now=%g", RAD2DEG(ylast), RAD2DEG(mdata.motYposition.val));
xlast = mdata.motXposition.val;
ylast = mdata.motYposition.val;
ctr = 0;
}else{
DBG("EQ: old=%g, now=%g", RAD2DEG(ylast), RAD2DEG(mdata.motYposition.val));
//DBG("EQ: old=%g, now=%g", RAD2DEG(ylast), RAD2DEG(mdata.motYposition.val));
++ctr;
}
}
@ -114,7 +114,8 @@ int main(int argc, char **argv){
ERRX("Can't open %s", G.coordsoutput);
}else fcoords = stdout;
if(G.Ncycles < 7) ERRX("Ncycles should be >7");
if(G.amplitude < 0.01 || G.amplitude > 45.)
double absamp = fabs(G.amplitude);
if(absamp < 0.01 || absamp > 45.)
ERRX("Amplitude should be from 0.01 to 45 degrees");
if(G.period < 0.1 || G.period > 900.)
ERRX("Period should be from 0.1 to 900s");
@ -162,9 +163,13 @@ int main(int argc, char **argv){
DBG("Moved to -, t=%g", t-t0);
DBG("CMD: %g", Mount.currentT()-t0);
}
tag = (coordpair_t){.X = 0., .Y = 0.};
green("Move to zero @ %g\n", Mount.currentT());
tag = (coordpair_t){0};
// be sure to move @ 0,0
Mount.moveTo(&tag);
if(MCC_E_OK != Mount.moveTo(&tag)){
Mount.emergStop();
Mount.moveTo(&tag);
}
// wait moving ends
pthread_join(dthr, NULL);
#undef SCMD

53
Auxiliary_utils/LibSidServo/examples/scmd_traectory.c
View File

@ -30,6 +30,7 @@
typedef struct{
int help;
int dumpconf;
int Ncycles; // n cycles to wait stop
double reqint; // requests interval (seconds)
double Xmax; // maximal X to stop
@ -38,11 +39,12 @@ typedef struct{
double X0; // starting point of traectory (-30..30 degr)
double Y0; // -//-
char *coordsoutput; // dump file
char *errlog; // log with position errors
char *tfn; // traectory function name
char *conffile;
} parameters;
static FILE *fcoords = NULL;
static FILE *fcoords = NULL, *errlog = NULL;
static pthread_t dthr;
static parameters G = {
.Ncycles = 40,
@ -61,12 +63,14 @@ static sl_option_t cmdlnopts[] = {
{"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)"},
{"xmax", NEED_ARG, NULL, 'X', arg_double, APTR(&G.Xmax), "maximal abs X coordinate for traectory (default: 45 degrees)"},
{"ymax", NEED_ARG, NULL, 'Y', arg_double, APTR(&G.Ymax), "maximal abs Y 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)"},
{"conffile", NEED_ARG, NULL, 'C', arg_string, APTR(&G.conffile), "configuration file name"},
{"errlog", NEED_ARG, NULL, 'e', arg_string, APTR(&G.errlog), "file with errors log"},
{"dumpconf", NO_ARGS, NULL, 'D', arg_int, APTR(&G.dumpconf), "dump current configuration"},
end_option
};
@ -76,6 +80,8 @@ void signals(int sig){
signal(sig, SIG_IGN);
DBG("Get signal %d, quit.\n", sig);
}
Mount.stop();
sleep(1);
Mount.quit();
if(fcoords) fclose(fcoords);
exit(sig);
@ -90,22 +96,39 @@ static void *dumping(void _U_ *u){
static void runtraectory(traectory_fn tfn){
if(!tfn) return;
coordval_pair_t telXY;
coordpair_t traectXY;
double t0 = Mount.currentT();
double tlast = 0.;
coordval_pair_t target;
coordpair_t traectXY, endpoint;
endpoint.X = G.Xmax, endpoint.Y = G.Ymax;
double t0 = Mount.currentT(), tlast = 0.;
double tlastX = 0., tlastY = 0.;
while(1){
if(!telpos(&telXY)){
WARNX("No next telescope position");
return;
}
if(telXY.X.t == tlast && telXY.Y.t == tlast) continue; // last measure - don't mind
tlast = (telXY.X.t + telXY.Y.t) / 2.;
if(telXY.X.t == tlastX && telXY.Y.t == tlastY) continue; // last measure - don't mind
DBG("\n\nTELPOS: %g'/%g' measured @ %g/%g", RAD2AMIN(telXY.X.val), RAD2AMIN(telXY.Y.val), telXY.X.t, telXY.Y.t);
tlastX = telXY.X.t; tlastY = telXY.Y.t;
double t = Mount.currentT();
if(telXY.X.val > G.Xmax || telXY.Y.val > G.Ymax || t - t0 > G.tmax) break;
if(fabs(telXY.X.val) > G.Xmax || fabs(telXY.Y.val) > 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.val), RAD2ASEC(traectXY.Y-telXY.Y.val));
target.X.val = traectXY.X; target.Y.val = traectXY.Y;
target.X.t = target.Y.t = t;
// check whether we should change direction
if(telXY.X.val > traectXY.X) endpoint.X = -G.Xmax;
else if(telXY.X.val < traectXY.X) endpoint.X = G.Xmax;
if(telXY.Y.val > traectXY.Y) endpoint.Y = -G.Ymax;
else if(telXY.Y.val < traectXY.Y) endpoint.Y = G.Ymax;
DBG("target: %g'/%g'", RAD2AMIN(traectXY.X), RAD2AMIN(traectXY.Y));
DBG("%g: dX=%.4f'', dY=%.4f''", t-t0, RAD2ASEC(traectXY.X-telXY.X.val), RAD2ASEC(traectXY.Y-telXY.Y.val));
DBG("Correct to: %g/%g with EP %g/%g", RAD2DEG(target.X.val), RAD2DEG(target.Y.val), RAD2DEG(endpoint.X), RAD2DEG(endpoint.Y));
if(errlog)
fprintf(errlog, "%10.4g %10.4g %10.4g\n", t, RAD2ASEC(traectXY.X-telXY.X.val), RAD2ASEC(traectXY.Y-telXY.Y.val));
if(MCC_E_OK != Mount.correctTo(&target, &endpoint)) WARNX("Error of correction!");
while((t = Mount.currentT()) - tlast < MCC_PID_REFRESH_DT) usleep(50);
tlast = t;
}
WARNX("No next traectory point");
WARNX("No next traectory point or emulation ends");
}
int main(int argc, char **argv){
@ -118,12 +141,18 @@ int main(int argc, char **argv){
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.errlog){
if(!(errlog = fopen(G.errlog, "w")))
ERRX("Can't open error log %s", G.errlog);
else
fprintf(errlog, "# time Xerr'' Yerr'' // target - real\n");
}
if(G.coordsoutput){
if(!(fcoords = fopen(G.coordsoutput, "w")))
ERRX("Can't open %s", G.coordsoutput);
}else fcoords = stdout;
conf_t *Config = readServoConf(G.conffile);
if(!Config){
if(!Config || G.dumpconf){
dumpConf();
return 1;
}

24
Auxiliary_utils/LibSidServo/examples/servo.conf
View File

@ -1,9 +1,25 @@
Current configuration:
MountDevPath=/dev/ttyUSB0
MountDevSpeed=19200
EncoderDevPath=(null)
EncoderDevSpeed=1000000
MountReqInterval=0.1
EncoderReqInterval=0.001
SepEncoder=2
EncoderXDevPath=/dev/encoder_X0
EncoderYDevPath=/dev/encoder_Y0
MountReqInterval=0.05
SepEncoder=2
EncoderReqInterval=0.001
EncoderDevSpeed=1000000
EncoderSpeedInterval=0.05
RunModel=1
XPIDCP=0.8
XPIDCI=0.0
XPIDCD=0.0
YPIDCP=0.5
YPIDCI=0.0
YPIDCD=0.0
XPIDVP=0.2
XPIDVI=0.1
XPIDVD=0.0
YPIDVP=0.2
YPIDVI=0.1
YPIDVD=0.0

12
Auxiliary_utils/LibSidServo/examples/simpleconv.h
View File

@ -20,10 +20,10 @@
#include <math.h>
#define DEG2RAD(d) (d/180.*M_PI)
#define ASEC2RAD(d) (d/180.*M_PI/3600.)
#define AMIN2RAD(d) (d/180.*M_PI/60.)
#define RAD2DEG(r) (r/M_PI*180.)
#define RAD2ASEC(r) (r/M_PI*180.*3600.)
#define RAD2AMIN(r) (r/M_PI*180.*60.)
#define DEG2RAD(d) ((d)/180.*M_PI)
#define ASEC2RAD(d) ((d)/180.*M_PI/3600.)
#define AMIN2RAD(d) ((d)/180.*M_PI/60.)
#define RAD2DEG(r) ((r)/M_PI*180.)
#define RAD2ASEC(r) ((r)/M_PI*180.*3600.)
#define RAD2AMIN(r) ((r)/M_PI*180.*60.)

19
Auxiliary_utils/LibSidServo/examples/traectories.c
View File

@ -30,10 +30,6 @@ static traectory_fn cur_traectory = NULL;
// starting point of traectory
static coordpair_t XYstart = {0};
static double tstart = 0.;
// convert Xe/Ye to approximate motor coordinates:
// Xnew = Xcor+Xe; Ynew = Ycor+Ye; as Ye goes backwards to Ym, we have
// Xcor = Xm0 - Xe0; Ycor = Xm0 + Ye0
static coordval_pair_t XYcor = {0};
/**
* @brief init_traectory - init traectory fn, sync starting positions of motor & encoders
@ -52,9 +48,6 @@ int init_traectory(traectory_fn f, coordpair_t *XY0){
if(MCC_E_OK == Mount.getMountData(&mdata)) break;
}
if(ntries == 10) return FALSE;
XYcor.X.val = mdata.motXposition.val - mdata.encXposition.val;
XYcor.Y.val = mdata.motYposition.val - mdata.encYposition.val;
DBG("STARTING POINTS: x=%g, y=%g degrees", DEG2RAD(XYcor.X.val), DEG2RAD(XYcor.Y.val));
return TRUE;
}
@ -83,8 +76,8 @@ int telpos(coordval_pair_t *curpos){
}
if(ntries == 10) return FALSE;
coordval_pair_t pt;
pt.X.val = XYcor.X.val + mdata.encXposition.val;
pt.Y.val = XYcor.Y.val + mdata.encYposition.val;
pt.X.val = mdata.encXposition.val;
pt.Y.val = mdata.encYposition.val;
pt.X.t = mdata.encXposition.t;
pt.Y.t = mdata.encYposition.t;
if(curpos) *curpos = pt;
@ -94,7 +87,7 @@ int telpos(coordval_pair_t *curpos){
// X=X0+1'/s, Y=Y0+15''/s
int Linear(coordpair_t *nextpt, double t){
coordpair_t pt;
pt.X = XYstart.X + ASEC2RAD(1.) * (t - tstart);
pt.X = XYstart.X + ASEC2RAD(0.1) * (t - tstart);
pt.Y = XYstart.Y + ASEC2RAD(15.)* (t - tstart);
if(nextpt) *nextpt = pt;
return TRUE;
@ -103,7 +96,7 @@ int Linear(coordpair_t *nextpt, double t){
// X=X0+5'*sin(t/30*2pi), Y=Y0+10'*cos(t/200*2pi)
int SinCos(coordpair_t *nextpt, double t){
coordpair_t pt;
pt.X = XYstart.X + AMIN2RAD(5.) * sin((t-tstart)/30.*2*M_PI);
pt.X = XYstart.X + ASEC2RAD(5.) * sin((t-tstart)/30.*2*M_PI);
pt.Y = XYstart.Y + AMIN2RAD(10.)* cos((t-tstart)/200.*2*M_PI);
if(nextpt) *nextpt = pt;
return TRUE;
@ -116,8 +109,8 @@ typedef struct{
} tr_names;
static tr_names names[] = {
{Linear, "linear", "X=X0+1'/s, Y=Y0+15''/s"},
{SinCos, "sincos", "X=X0+5'*sin(t/30*2pi), Y=Y0+10'*cos(t/200*2pi)"},
{Linear, "linear", "X=X0+0.1''/s, Y=Y0+15''/s"},
{SinCos, "sincos", "X=X0+5''*sin(t/30*2pi), Y=Y0+10'*cos(t/200*2pi)"},
{NULL, NULL, NULL}
};

5
Auxiliary_utils/LibSidServo/libsidservo.config
View File

@ -1,6 +1,7 @@
// Add predefined macros for your project here. For example:
// #define THE_ANSWER 42
#define EBUG
#define _POSIX_C_SOURCE 111
#define _POSIX_C_SOURCE 11111111
#define PACKAGE_VERSION "0.0.1"
#define _XOPEN_SOURCE 666
#define _DEFAULT_SOURCE

40
Auxiliary_utils/LibSidServo/libsidservo.creator.user
View File

@ -1,10 +1,10 @@
<?xml version="1.0" encoding="UTF-8"?>
<!DOCTYPE QtCreatorProject>
<!-- Written by QtCreator 17.0.0, 2025-07-29T22:07:00. -->
<!-- Written by QtCreator 17.0.0, 2025-07-30T17:30:52. -->
<qtcreator>
<data>
<variable>EnvironmentId</variable>
<value type="QByteArray">{7bd84e39-ca37-46d3-be9d-99ebea85bc0d}</value>
<value type="QByteArray">{cf63021e-ef53-49b0-b03b-2f2570cdf3b6}</value>
</data>
<data>
<variable>ProjectExplorer.Project.ActiveTarget</variable>
@ -40,9 +40,9 @@
<value type="int" key="EditorConfiguration.PaddingMode">1</value>
<value type="int" key="EditorConfiguration.PreferAfterWhitespaceComments">0</value>
<value type="bool" key="EditorConfiguration.PreferSingleLineComments">false</value>
<value type="bool" key="EditorConfiguration.ScrollWheelZooming">true</value>
<value type="bool" key="EditorConfiguration.ScrollWheelZooming">false</value>
<value type="bool" key="EditorConfiguration.ShowMargin">false</value>
<value type="int" key="EditorConfiguration.SmartBackspaceBehavior">0</value>
<value type="int" key="EditorConfiguration.SmartBackspaceBehavior">1</value>
<value type="bool" key="EditorConfiguration.SmartSelectionChanging">true</value>
<value type="bool" key="EditorConfiguration.SpacesForTabs">true</value>
<value type="int" key="EditorConfiguration.TabKeyBehavior">0</value>
@ -51,10 +51,10 @@
<value type="bool" key="EditorConfiguration.UseIndenter">false</value>
<value type="int" key="EditorConfiguration.Utf8BomBehavior">1</value>
<value type="bool" key="EditorConfiguration.addFinalNewLine">true</value>
<value type="bool" key="EditorConfiguration.cleanIndentation">false</value>
<value type="bool" key="EditorConfiguration.cleanIndentation">true</value>
<value type="bool" key="EditorConfiguration.cleanWhitespace">true</value>
<value type="QString" key="EditorConfiguration.ignoreFileTypes">*.md, *.MD, Makefile</value>
<value type="bool" key="EditorConfiguration.inEntireDocument">false</value>
<value type="bool" key="EditorConfiguration.inEntireDocument">true</value>
<value type="bool" key="EditorConfiguration.skipTrailingWhitespace">true</value>
<value type="bool" key="EditorConfiguration.tintMarginArea">true</value>
</valuemap>
@ -75,13 +75,11 @@
<valuelist type="QVariantList" key="AutoTest.PathFilters"/>
<value type="int" key="AutoTest.RunAfterBuild">0</value>
<value type="bool" key="AutoTest.UseGlobal">true</value>
<valuelist type="QVariantList" key="ClangCodeModel.CustomCommandLineKey"/>
<value type="bool" key="ClangCodeModel.UseGlobalConfig">true</value>
<valuemap type="QVariantMap" key="ClangTools">
<value type="bool" key="ClangTools.AnalyzeOpenFiles">true</value>
<value type="bool" key="ClangTools.BuildBeforeAnalysis">true</value>
<value type="QString" key="ClangTools.DiagnosticConfig">Builtin.DefaultTidyAndClazy</value>
<value type="int" key="ClangTools.ParallelJobs">8</value>
<value type="int" key="ClangTools.ParallelJobs">4</value>
<value type="bool" key="ClangTools.PreferConfigFile">true</value>
<valuelist type="QVariantList" key="ClangTools.SelectedDirs"/>
<valuelist type="QVariantList" key="ClangTools.SelectedFiles"/>
@ -97,12 +95,12 @@
<value type="bool" key="HasPerBcDcs">true</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Desktop</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Desktop</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">{65a14f9e-e008-4c1b-89df-4eaa4774b6e3}</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">{91347f2c-5221-46a7-80b1-0a054ca02f79}</value>
<value type="qlonglong" key="ProjectExplorer.Target.ActiveBuildConfiguration">0</value>
<value type="qlonglong" key="ProjectExplorer.Target.ActiveDeployConfiguration">0</value>
<value type="qlonglong" key="ProjectExplorer.Target.ActiveRunConfiguration">0</value>
<valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.0">
<value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/Big/Data/00__Small_tel/C-sources/erfa</value>
<value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/home/eddy/Docs/SAO/10micron/C-sources/erfa_functions</value>
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
<valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
<valuelist type="QVariantList" key="GenericProjectManager.GenericMakeStep.BuildTargets">
@ -112,8 +110,8 @@
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericMakeStep</value>
</valuemap>
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Build</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Build</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Сборка</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Сборка</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Build</value>
</valuemap>
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.1">
@ -125,8 +123,8 @@
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericMakeStep</value>
</valuemap>
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Clean</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Clean</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Очистка</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Очистка</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Clean</value>
</valuemap>
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">2</value>
@ -141,8 +139,8 @@
<valuemap type="QVariantMap" key="ProjectExplorer.Target.DeployConfiguration.0">
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">0</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Deploy</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Deploy</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Развёртывание</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Развёртывание</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Deploy</value>
</valuemap>
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">1</value>
@ -154,7 +152,9 @@
<valuemap type="QVariantMap" key="ProjectExplorer.Target.RunConfiguration.0">
<value type="bool" key="Analyzer.Perf.Settings.UseGlobalSettings">true</value>
<value type="bool" key="Analyzer.QmlProfiler.Settings.UseGlobalSettings">true</value>
<value type="int" key="Analyzer.Valgrind.Callgrind.CostFormat">0</value>
<value type="bool" key="Analyzer.Valgrind.Settings.UseGlobalSettings">true</value>
<value type="QList&lt;int&gt;" key="Analyzer.Valgrind.VisibleErrorKinds"></value>
<valuelist type="QVariantList" key="CustomOutputParsers"/>
<value type="int" key="PE.EnvironmentAspect.Base">2</value>
<valuelist type="QVariantList" key="PE.EnvironmentAspect.Changes"/>
@ -173,8 +173,8 @@
<valuemap type="QVariantMap" key="ProjectExplorer.Target.DeployConfiguration.0">
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">0</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Deploy</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Deploy</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Развёртывание</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Развёртывание</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Deploy</value>
</valuemap>
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">1</value>
@ -186,7 +186,9 @@
<valuemap type="QVariantMap" key="ProjectExplorer.Target.RunConfiguration.0">
<value type="bool" key="Analyzer.Perf.Settings.UseGlobalSettings">true</value>
<value type="bool" key="Analyzer.QmlProfiler.Settings.UseGlobalSettings">true</value>
<value type="int" key="Analyzer.Valgrind.Callgrind.CostFormat">0</value>
<value type="bool" key="Analyzer.Valgrind.Settings.UseGlobalSettings">true</value>
<value type="QList&lt;int&gt;" key="Analyzer.Valgrind.VisibleErrorKinds"></value>
<valuelist type="QVariantList" key="CustomOutputParsers"/>
<value type="int" key="PE.EnvironmentAspect.Base">2</value>
<valuelist type="QVariantList" key="PE.EnvironmentAspect.Changes"/>

8
Auxiliary_utils/LibSidServo/libsidservo.files
View File

@ -1,5 +1,6 @@
CMakeLists.txt
dbg.h
PID.c
PID.h
examples/SSIIconf.c
examples/conf.c
examples/conf.h
@ -18,6 +19,11 @@ serial.c
examples/CMakeLists.txt
examples/traectories.c
examples/traectories.h
main.h
movingmodel.c
movingmodel.h
ramp.c
ramp.h
serial.h
ssii.c
ssii.h

201
Auxiliary_utils/LibSidServo/main.c
View File

@ -16,28 +16,133 @@
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
* main functions to fill struct `mount_t`
*/
#include <inttypes.h>
#include <strings.h>
#include <time.h>
#include <stdio.h>
#include <stdlib.h>
#include "dbg.h"
#include "main.h"
#include "movingmodel.h"
#include "serial.h"
#include "ssii.h"
#include "PID.h"
conf_t Conf = {0};
// parameters for model
static movemodel_t *Xmodel, *Ymodel;
// radians, rad/sec, rad/sec^2
static limits_t
Xlimits = {
.min = {.coord = -3.1241, .speed = 1e-10, .accel = 1e-6},
.max = {.coord = 3.1241, .speed = MCC_MAX_X_SPEED, .accel = MCC_X_ACCELERATION}},
Ylimits = {
.min = {.coord = -3.1241, .speed = 1e-10, .accel = 1e-6},
.max = {.coord = 3.1241, .speed = MCC_MAX_Y_SPEED, .accel = MCC_Y_ACCELERATION}}
;
static mcc_errcodes_t shortcmd(short_command_t *cmd);
/**
* @brief nanotime - monotonic time from first run
* @return time in seconds
*/
double nanotime(){
static struct timespec *start = NULL;
struct timespec now;
if(!start){
start = malloc(sizeof(struct timespec));
if(!start) return -1.;
if(clock_gettime(CLOCK_MONOTONIC, start)) return -1.;
}
if(clock_gettime(CLOCK_MONOTONIC, &now)) return -1.;
double nd = ((double)now.tv_nsec - (double)start->tv_nsec) * 1e-9;
double sd = (double)now.tv_sec - (double)start->tv_sec;
return sd + nd;
}
/**
* @brief quit - close all opened and return to default state
*/
static void quit(){
DBG("Close serial devices");
if(Conf.RunModel) return;
for(int i = 0; i < 10; ++i) if(SSstop(TRUE)) break;
DBG("Close all serial devices");
closeSerial();
DBG("Exit");
}
void getModData(mountdata_t *mountdata){
if(!mountdata || !Xmodel || !Ymodel) return;
static double oldmt = -100.; // old `millis measurement` time
static uint32_t oldmillis = 0;
double tnow = nanotime();
moveparam_t Xp, Yp;
movestate_t Xst = Xmodel->get_state(Xmodel, &Xp);
//DBG("Xstate = %d", Xst);
if(Xst == ST_MOVE) Xst = Xmodel->proc_move(Xmodel, &Xp, tnow);
movestate_t Yst = Ymodel->get_state(Ymodel, &Yp);
if(Yst == ST_MOVE) Yst = Ymodel->proc_move(Ymodel, &Yp, tnow);
mountdata->motXposition.t = mountdata->encXposition.t = mountdata->motYposition.t = mountdata->encYposition.t = tnow;
mountdata->motXposition.val = mountdata->encXposition.val = Xp.coord;
mountdata->motYposition.val = mountdata->encYposition.val = Yp.coord;
getXspeed(); getYspeed();
if(tnow - oldmt > Conf.MountReqInterval){
oldmillis = mountdata->millis = (uint32_t)(tnow * 1e3);
oldmt = tnow;
}else mountdata->millis = oldmillis;
}
/**
* less square calculations of speed
*/
less_square_t *LS_init(size_t Ndata){
if(Ndata < 5){
DBG("Ndata=%zd - TOO SMALL", Ndata);
return NULL;
}
DBG("Init less squares: %zd", Ndata);
less_square_t *l = calloc(1, sizeof(less_square_t));
l->x = calloc(Ndata, sizeof(double));
l->t2 = calloc(Ndata, sizeof(double));
l->t = calloc(Ndata, sizeof(double));
l->xt = calloc(Ndata, sizeof(double));
l->arraysz = Ndata;
return l;
}
void LS_delete(less_square_t **l){
if(!l || !*l) return;
free((*l)->x); free((*l)->t2); free((*l)->t); free((*l)->xt);
free(*l);
*l = NULL;
}
// add next data portion and calculate current slope
double LS_calc_slope(less_square_t *l, double x, double t){
if(!l) return 0.;
size_t idx = l->idx;
double oldx = l->x[idx], oldt = l->t[idx], oldt2 = l->t2[idx], oldxt = l->xt[idx];
double t2 = t * t, xt = x * t;
l->x[idx] = x; l->t2[idx] = t2;
l->t[idx] = t; l->xt[idx] = xt;
++idx;
l->idx = (idx >= l->arraysz) ? 0 : idx;
l->xsum += x - oldx;
l->t2sum += t2 - oldt2;
l->tsum += t - oldt;
l->xtsum += xt - oldxt;
double n = (double)l->arraysz;
double denominator = n * l->t2sum - l->tsum * l->tsum;
//DBG("idx=%zd, arrsz=%zd, den=%g", l->idx, l->arraysz, denominator);
if(fabs(denominator) < 1e-7) return 0.;
double numerator = n * l->xtsum - l->xsum * l->tsum;
// point: (sum_x - slope * sum_t) / n;
return (numerator / denominator);
}
/**
* @brief init - open serial devices and do other job
* @param c - initial configuration
@ -48,6 +153,12 @@ static mcc_errcodes_t init(conf_t *c){
if(!c) return MCC_E_BADFORMAT;
Conf = *c;
mcc_errcodes_t ret = MCC_E_OK;
Xmodel = model_init(&Xlimits);
Ymodel = model_init(&Ylimits);
if(Conf.RunModel){
if(!Xmodel || !Ymodel || !openMount()) return MCC_E_FAILED;
return MCC_E_OK;
}
if(!Conf.MountDevPath || Conf.MountDevSpeed < 1200){
DBG("Define mount device path and speed");
ret = MCC_E_BADFORMAT;
@ -72,10 +183,9 @@ static mcc_errcodes_t init(conf_t *c){
DBG("Wrong speed interval");
ret = MCC_E_BADFORMAT;
}
uint8_t buf[1024];
data_t d = {.buf = buf, .len = 0, .maxlen = 1024};
// read input data as there may be some trash on start
if(!SSrawcmd(CMD_EXITACM, &d)) ret = MCC_E_FAILED;
//uint8_t buf[1024];
//data_t d = {.buf = buf, .len = 0, .maxlen = 1024};
if(!SSrawcmd(CMD_EXITACM, NULL)) ret = MCC_E_FAILED;
if(ret != MCC_E_OK) return ret;
return updateMotorPos();
}
@ -99,16 +209,34 @@ static int chkYs(double s){
return TRUE;
}
// set SLEWING state if axis was stopped later
static void setslewingstate(){
//FNAME();
mountdata_t d;
if(MCC_E_OK == getMD(&d)){
axis_status_t newx = d.Xstate, newy = d.Ystate;
//DBG("old state: %d/%d", d.Xstate, d.Ystate);
if(d.Xstate == AXIS_STOPPED) newx = AXIS_SLEWING;
if(d.Ystate == AXIS_STOPPED) newy = AXIS_SLEWING;
if(newx != d.Xstate || newy != d.Ystate){
DBG("Started moving -> slew");
setStat(newx, newy);
}
}else DBG("CAN't GET MOUNT DATA!");
}
/*
static mcc_errcodes_t slew2(const coordpair_t *target, slewflags_t flags){
(void)target;
(void)flags;
//if(Conf.RunModel) return ... ;
if(MCC_E_OK != updateMotorPos()) return MCC_E_FAILED;
//...
setStat(MNT_SLEWING, MNT_SLEWING);
setStat(AXIS_SLEWING, AXIS_SLEWING);
//...
return MCC_E_FAILED;
}
*/
/**
* @brief move2 - simple move to given point and stop
@ -128,7 +256,7 @@ static mcc_errcodes_t move2(const coordpair_t *target){
cmd.Yspeed = MCC_MAX_Y_SPEED;
mcc_errcodes_t r = shortcmd(&cmd);
if(r != MCC_E_OK) return r;
setStat(MNT_SLEWING, MNT_SLEWING);
setslewingstate();
return MCC_E_OK;
}
@ -140,6 +268,7 @@ static mcc_errcodes_t move2(const coordpair_t *target){
*/
static mcc_errcodes_t setspeed(const coordpair_t *tagspeed){
if(!tagspeed || !chkXs(tagspeed->X) || !chkYs(tagspeed->Y)) return MCC_E_BADFORMAT;
if(Conf.RunModel) return MCC_E_FAILED;
int32_t spd = X_RS2MOTSPD(tagspeed->X);
if(!SSsetterI(CMD_SPEEDX, spd)) return MCC_E_FAILED;
spd = Y_RS2MOTSPD(tagspeed->Y);
@ -165,7 +294,7 @@ static mcc_errcodes_t move2s(const coordpair_t *target, const coordpair_t *speed
cmd.Yspeed = speed->Y;
mcc_errcodes_t r = shortcmd(&cmd);
if(r != MCC_E_OK) return r;
setStat(MNT_SLEWING, MNT_SLEWING);
setslewingstate();
return MCC_E_OK;
}
@ -174,11 +303,25 @@ static mcc_errcodes_t move2s(const coordpair_t *target, const coordpair_t *speed
* @return errcode
*/
static mcc_errcodes_t emstop(){
FNAME();
if(Conf.RunModel){
double curt = nanotime();
Xmodel->emergency_stop(Xmodel, curt);
Ymodel->emergency_stop(Ymodel, curt);
return MCC_E_OK;
}
if(!SSstop(TRUE)) return MCC_E_FAILED;
return MCC_E_OK;
}
// normal stop
static mcc_errcodes_t stop(){
FNAME();
if(Conf.RunModel){
double curt = nanotime();
Xmodel->stop(Xmodel, curt);
Ymodel->stop(Ymodel,curt);
return MCC_E_OK;
}
if(!SSstop(FALSE)) return MCC_E_FAILED;
return MCC_E_OK;
}
@ -190,6 +333,16 @@ static mcc_errcodes_t stop(){
*/
static mcc_errcodes_t shortcmd(short_command_t *cmd){
if(!cmd) return MCC_E_BADFORMAT;
if(Conf.RunModel){
double curt = nanotime();
moveparam_t param = {0};
param.coord = cmd->Xmot; param.speed = cmd->Xspeed;
if(!model_move2(Xmodel, &param, curt)) return MCC_E_FAILED;
param.coord = cmd->Ymot; param.speed = cmd->Yspeed;
if(!model_move2(Ymodel, &param, curt)) return MCC_E_FAILED;
setslewingstate();
return MCC_E_OK;
}
SSscmd s = {0};
DBG("tag: xmot=%g rad, ymot=%g rad", cmd->Xmot, cmd->Ymot);
s.Xmot = X_RAD2MOT(cmd->Xmot);
@ -201,16 +354,27 @@ static mcc_errcodes_t shortcmd(short_command_t *cmd){
s.YBits = cmd->YBits;
DBG("X->%d, Y->%d, Xs->%d, Ys->%d", s.Xmot, s.Ymot, s.Xspeed, s.Yspeed);
if(!cmdS(&s)) return MCC_E_FAILED;
setslewingstate();
return MCC_E_OK;
}
/**
* @brief shortcmd - send and receive long binary command
* @brief longcmd - send and receive long binary command
* @param cmd (io) - command
* @return errcode
*/
static mcc_errcodes_t longcmd(long_command_t *cmd){
if(!cmd) return MCC_E_BADFORMAT;
if(Conf.RunModel){
double curt = nanotime();
moveparam_t param = {0};
param.coord = cmd->Xmot; param.speed = cmd->Xspeed;
if(!model_move2(Xmodel, &param, curt)) return MCC_E_FAILED;
param.coord = cmd->Ymot; param.speed = cmd->Yspeed;
if(!model_move2(Ymodel, &param, curt)) return MCC_E_FAILED;
setslewingstate();
return MCC_E_OK;
}
SSlcmd l = {0};
l.Xmot = X_RAD2MOT(cmd->Xmot);
l.Ymot = Y_RAD2MOT(cmd->Ymot);
@ -221,11 +385,13 @@ static mcc_errcodes_t longcmd(long_command_t *cmd){
l.Xatime = S2ADDER(cmd->Xatime);
l.Yatime = S2ADDER(cmd->Yatime);
if(!cmdL(&l)) return MCC_E_FAILED;
setslewingstate();
return MCC_E_OK;
}
static mcc_errcodes_t get_hwconf(hardware_configuration_t *hwConfig){
if(!hwConfig) return MCC_E_BADFORMAT;
if(Conf.RunModel) return MCC_E_FAILED;
SSconfig config;
if(!cmdC(&config, FALSE)) return MCC_E_FAILED;
// Convert acceleration (ticks per loop^2 to rad/s^2)
@ -266,8 +432,8 @@ static mcc_errcodes_t get_hwconf(hardware_configuration_t *hwConfig){
// Copy ticks per revolution
hwConfig->Xsetpr = __bswap_32(config.Xsetpr);
hwConfig->Ysetpr = __bswap_32(config.Ysetpr);
hwConfig->Xmetpr = __bswap_32(config.Xmetpr);
hwConfig->Ymetpr = __bswap_32(config.Ymetpr);
hwConfig->Xmetpr = __bswap_32(config.Xmetpr) / 4; // as documentation said, real ticks are 4 times less
hwConfig->Ymetpr = __bswap_32(config.Ymetpr) / 4;
// Convert slew rates (ticks per loop to rad/s)
hwConfig->Xslewrate = X_MOTSPD2RS(config.Xslewrate);
hwConfig->Yslewrate = Y_MOTSPD2RS(config.Yslewrate);
@ -290,6 +456,7 @@ static mcc_errcodes_t get_hwconf(hardware_configuration_t *hwConfig){
static mcc_errcodes_t write_hwconf(hardware_configuration_t *hwConfig){
SSconfig config;
if(Conf.RunModel) return MCC_E_FAILED;
// Convert acceleration (rad/s^2 to ticks per loop^2)
config.Xconf.accel = X_RS2MOTACC(hwConfig->Xconf.accel);
config.Yconf.accel = Y_RS2MOTACC(hwConfig->Yconf.accel);
@ -349,7 +516,7 @@ mount_t Mount = {
.init = init,
.quit = quit,
.getMountData = getMD,
.slewTo = slew2,
// .slewTo = slew2,
.moveTo = move2,
.moveWspeed = move2s,
.setSpeed = setspeed,
@ -359,5 +526,7 @@ mount_t Mount = {
.longCmd = longcmd,
.getHWconfig = get_hwconf,
.saveHWconfig = write_hwconf,
.currentT = dtime,
.currentT = nanotime,
.correctTo = correct2,
};

80
Auxiliary_utils/LibSidServo/main.h Normal file
View File

@ -0,0 +1,80 @@
/*
* 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/>.
*/
/*
* Almost all here used for debug purposes
*/
#pragma once
#include <stdlib.h>
#include "sidservo.h"
extern conf_t Conf;
double nanotime();
void getModData(mountdata_t *mountdata);
typedef struct{
double *x, *t, *t2, *xt; // arrays of coord/time and multiply
double xsum, tsum, t2sum, xtsum; // sums of coord/time and their multiply
size_t idx; // index of current data in array
size_t arraysz; // size of arrays
} less_square_t;
less_square_t *LS_init(size_t Ndata);
void LS_delete(less_square_t **ls);
double LS_calc_slope(less_square_t *l, double x, double t);
// unused arguments of functions
#define _U_ __attribute__((__unused__))
// break absent in `case`
#define FALLTHRU __attribute__ ((fallthrough))
// and synonym for FALLTHRU
#define NOBREAKHERE __attribute__ ((fallthrough))
// weak functions
#define WEAK __attribute__ ((weak))
#ifndef DBL_EPSILON
#define DBL_EPSILON (2.2204460492503131e-16)
#endif
#ifndef FALSE
#define FALSE (0)
#endif
#ifndef TRUE
#define TRUE (1)
#endif
#ifdef EBUG
#include <stdio.h>
#define COLOR_RED "\033[1;31;40m"
#define COLOR_GREEN "\033[1;32;40m"
#define COLOR_OLD "\033[0;0;0m"
#define FNAME() do{ fprintf(stderr, COLOR_GREEN "\n%s " COLOR_OLD, __func__); \
fprintf(stderr, "(%s, line %d)\n", __FILE__, __LINE__);} while(0)
#define DBG(...) do{ fprintf(stderr, COLOR_RED "%s " COLOR_OLD, __func__); \
fprintf(stderr, "(%s, line %d): ", __FILE__, __LINE__); \
fprintf(stderr, __VA_ARGS__); \
fprintf(stderr, "\n");} while(0)
#else // EBUG
#define FNAME()
#define DBG(...)
#endif // EBUG

68
Auxiliary_utils/LibSidServo/movingmodel.c Normal file
View File

@ -0,0 +1,68 @@
/*
* This file is part of the moving_model 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 <stdio.h>
#include <string.h>
#include <pthread.h>
#include "main.h"
#include "movingmodel.h"
#include "ramp.h"
extern movemodel_t trapez;
static void chkminmax(double *min, double *max){
if(*min <= *max) return;
double t = *min;
*min = *max;
*max = t;
}
movemodel_t *model_init(limits_t *l){
if(!l) return FALSE;
movemodel_t *m = calloc(1, sizeof(movemodel_t));
// we can't use memcpy or assign as Times/Params would be common for all
*m = trapez;
m->Times = calloc(STAGE_AMOUNT, sizeof(double));
m->Params = calloc(STAGE_AMOUNT, sizeof(moveparam_t));
moveparam_t *max = &l->max, *min = &l->min;
if(min->speed < 0.) min->speed = -min->speed;
if(max->speed < 0.) max->speed = -max->speed;
if(min->accel < 0.) min->accel = -min->accel;
if(max->accel < 0.) max->accel = -max->accel;
chkminmax(&min->coord, &max->coord);
chkminmax(&min->speed, &max->speed);
chkminmax(&min->accel, &max->accel);
m->Min = l->min;
m->Max = l->max;
m->movingstage = STAGE_STOPPED;
m->state = ST_STOP;
pthread_mutex_init(&m->mutex, NULL);
DBG("model inited");
return m;
}
int model_move2(movemodel_t *model, moveparam_t *target, double t){
if(!target || !model) return FALSE;
DBG("MOVE to %g at speed %g", target->coord, target->speed);
// only positive velocity
if(target->speed < 0.) target->speed = -target->speed;
// don't mind about acceleration - user cannot set it now
return model->calculate(model, target, t);
}

76
Auxiliary_utils/LibSidServo/movingmodel.h Normal file
View File

@ -0,0 +1,76 @@
/*
* This file is part of the moving_model 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/>.
*/
#pragma once
#include <pthread.h>
#include "sidservo.h"
// tolerance, time ticks
#define COORD_TOLERANCE_DEFAULT (1e-8)
#define COORD_TOLERANCE_MIN (1e-12)
#define COORD_TOLERANCE_MAX (10.)
#define TIME_TICK_DEFAULT (0.0001)
#define TIME_TICK_MIN (1e-9)
#define TIME_TICK_MAX (10.)
typedef enum{
ST_STOP, // stopped
ST_MOVE, // moving
ST_AMOUNT
} movestate_t;
typedef struct{
double coord;
double speed;
double accel;
} moveparam_t;
typedef struct{
moveparam_t min;
moveparam_t max;
double acceleration;
} limits_t;
typedef enum{
STAGE_ACCEL, // start from last speed and accelerate/decelerate to target speed
STAGE_MAXSPEED, // go with target speed
STAGE_DECEL, // go from target speed to zero
STAGE_STOPPED, // stop
STAGE_AMOUNT
} movingstage_t;
typedef struct movemodel{
moveparam_t Min;
moveparam_t Max;
movingstage_t movingstage;
movestate_t state;
double *Times;
moveparam_t *Params;
moveparam_t curparams; // init values of limits, jerk
int (*calculate)(struct movemodel *m, moveparam_t *target, double t); // calculate stages of traectory beginning from t
movestate_t (*proc_move)(struct movemodel *m, moveparam_t *next, double t); // calculate next model point for time t
movestate_t (*get_state)(struct movemodel *m, moveparam_t *cur); // get current moving state
void (*stop)(struct movemodel *m, double t); // stop by ramp
void (*emergency_stop)(struct movemodel *m, double t); // stop with highest acceleration
double (*stoppedtime)(struct movemodel *m); // time when moving will ends
pthread_mutex_t mutex;
} movemodel_t;
movemodel_t *model_init(limits_t *l);
int model_move2(movemodel_t *model, moveparam_t *target, double t);

258
Auxiliary_utils/LibSidServo/ramp.c Normal file
View File

@ -0,0 +1,258 @@
/*
* This file is part of the moving_model 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/>.
*/
// simplest trapezioidal ramp
#include <math.h>
#include <strings.h>
#include "main.h"
#include "ramp.h"
/*
#ifdef EBUG
#undef DBG
#define DBG(...)
#endif
*/
static double coord_tolerance = COORD_TOLERANCE_DEFAULT;
static void emstop(movemodel_t *m, double _U_ t){
FNAME();
pthread_mutex_lock(&m->mutex);
m->curparams.accel = 0.;
m->curparams.speed = 0.;
bzero(m->Times, sizeof(double) * STAGE_AMOUNT);
bzero(m->Params, sizeof(moveparam_t) * STAGE_AMOUNT);
m->state = ST_STOP;
m->movingstage = STAGE_STOPPED;
pthread_mutex_unlock(&m->mutex);
}
static void stop(movemodel_t *m, double t){
FNAME();
pthread_mutex_lock(&m->mutex);
if(m->state == ST_STOP || m->movingstage == STAGE_STOPPED) goto ret;
m->movingstage = STAGE_DECEL;
m->state = ST_MOVE;
m->Times[STAGE_DECEL] = t;
m->Params[STAGE_DECEL].speed = m->curparams.speed;
if(m->curparams.speed > 0.) m->Params[STAGE_DECEL].accel = -m->Max.accel;
else m->Params[STAGE_DECEL].accel = m->Max.accel;
m->Params[STAGE_DECEL].coord = m->curparams.coord;
// speed: v=v2+a2(t-t2), v2 and a2 have different signs; t3: v3=0 -> t3=t2-v2/a2
m->Times[STAGE_STOPPED] = t - m->curparams.speed / m->Params[STAGE_DECEL].accel;
// coordinate: x=x2+v2(t-t2)+a2(t-t2)^2/2 -> x3=x2+v2(t3-t2)+a2(t3-t2)^2/2
double dt = m->Times[STAGE_STOPPED] - t;
m->Params[STAGE_STOPPED].coord = m->curparams.coord + m->curparams.speed * dt +
m->Params[STAGE_DECEL].accel * dt * dt / 2.;
ret:
pthread_mutex_unlock(&m->mutex);
}
// inner part of `calc`, could be called recoursively for hard case
static void unlockedcalc(movemodel_t *m, moveparam_t *x, double t){
// signs
double sign_a01 = 0., sign_a23 = 0., sign_vset = 0.; // accelerations on stages ACCEL and DECEL, speed on maxspeed stage
// times
double dt01 = 0., dt12 = 0., dt23 = 0.;
// absolute speed at stage 23 (or in that point); absolute max acceleration
double abs_vset = x->speed, abs_a = m->Max.accel;
// absolute target movement
double abs_Dx = fabs(x->coord - m->curparams.coord);
if(m->state == ST_STOP && abs_Dx < coord_tolerance){
DBG("Movement too small -> stay at place");
return;
}
// signs of Dx and current speed
double sign_Dx = (x->coord > m->curparams.coord) ? 1. : -1.;
double v0 = m->curparams.speed;
double sign_v0 = v0 < 0. ? -1 : 1., abs_v0 = fabs(v0);
if(v0 == 0.) sign_v0 = 0.;
// preliminary calculations (vset and dependent values could be changed)
dt01 = fabs(abs_v0 - abs_vset) / abs_a;
double abs_dx23 = abs_vset * abs_vset / 2. / abs_a;
dt23 = abs_vset / abs_a;
double abs_dx_stop = abs_v0 * abs_v0 / 2. / abs_a;
if(sign_Dx * sign_v0 >= 0. && abs_dx_stop < abs_Dx){ // we shouldn't change speed direction
if(fabs(abs_dx_stop - abs_Dx) <= coord_tolerance){ // simplest case: just stop
//DBG("Simplest case: stop");
dt01 = dt12 = 0.;
sign_a23 = -sign_v0;
dt23 = abs_v0 / abs_a;
}else if(abs_vset < abs_v0){ // move with smaller speed than now: very simple case
//DBG("Move with smaller speed");
sign_a01 = sign_a23 = -sign_v0;
sign_vset = sign_v0;
double abs_dx01 = abs_v0 * dt01 - abs_a * dt01 * dt01 / 2.;
double abs_dx12 = abs_Dx - abs_dx01 - abs_dx23;
dt12 = abs_dx12 / abs_vset;
}else{// move with larget speed
//DBG("Move with larger speed");
double abs_dx01 = abs_v0 * dt01 + abs_a * dt01 * dt01 / 2.;
if(abs_Dx < abs_dx01 + abs_dx23){ // recalculate target speed and other
abs_vset = sqrt(abs_a * abs_Dx + abs_v0 * abs_v0 / 2.);
dt01 = fabs(abs_v0 - abs_vset) / abs_a;
abs_dx01 = abs_v0 * dt01 + abs_a * dt01 * dt01 / 2.;
dt23 = abs_vset / abs_a;
abs_dx23 = abs_vset * abs_vset / 2. / abs_a;
DBG("Can't reach target speed %g, take %g instead", x->speed, abs_vset);
}
sign_a01 = sign_Dx; // sign_v0 could be ZERO!!!
sign_a23 = -sign_Dx;
sign_vset = sign_Dx;
double abs_dx12 = abs_Dx - abs_dx01 - abs_dx23;
dt12 = abs_dx12 / abs_vset;
}
}else{
// if we are here, we have the worst case: change speed direction
DBG("Hardest case: change speed direction");
// now we should calculate coordinate at which model stops and biuld new trapezium from that point
double x0 = m->curparams.coord, v0 = m->curparams.speed;
double xstop = x0 + sign_v0 * abs_dx_stop, tstop = t + abs_v0 / abs_a;
m->state = ST_STOP;
m->curparams.accel = 0.; m->curparams.coord = xstop; m->curparams.speed = 0.;
unlockedcalc(m, x, tstop); // calculate new ramp
// and change started conditions
m->curparams.coord = x0; m->curparams.speed = v0;
m->Times[STAGE_ACCEL] = t;
m->Params[STAGE_ACCEL].coord = x0;
m->Params[STAGE_ACCEL].speed = v0;
DBG("NOW t[0]=%g, X[0]=%g, V[0]=%g", t, x0, v0);
return;
}
m->state = ST_MOVE;
m->movingstage = STAGE_ACCEL;
// some knot parameters
double a01 = sign_a01 * abs_a, a23 = sign_a23 * abs_a;
double v1, v2, x0, x1, x2;
v2 = v1 = sign_vset * abs_vset;
x0 = m->curparams.coord;
x1 = x0 + v0 * dt01 + a01 * dt01 * dt01 / 2.;
x2 = x1 + v1 * dt12;
// fill knot parameters
moveparam_t *p = &m->Params[STAGE_ACCEL]; // 0-1 - change started speed
p->accel = a01;
p->speed = m->curparams.speed;
p->coord = x0;
m->Times[STAGE_ACCEL] = t;
p = &m->Params[STAGE_MAXSPEED]; // 1-2 - constant speed
p->accel = 0.;
p->speed = v1;
p->coord = x1;
m->Times[STAGE_MAXSPEED] = m->Times[STAGE_ACCEL] + dt01;
p = &m->Params[STAGE_DECEL]; // 2-3 - decrease speed
p->accel = a23;
p->speed = v2;
p->coord = x2;
m->Times[STAGE_DECEL] = m->Times[STAGE_MAXSPEED] + dt12;
p = &m->Params[STAGE_STOPPED]; // 3 - stop at target
p->accel = p->speed = 0.;
p->coord = x->coord;
m->Times[STAGE_STOPPED] = m->Times[STAGE_DECEL] + dt23;
}
/**
* @brief calc - moving calculation
* @param x - using max speed (>0!!!) and coordinate
* @param t - current time value
* @return FALSE if can't move with given parameters
*/
static int calc(movemodel_t *m, moveparam_t *x, double t) {
//DBG("target coord/speed: %g/%g; current: %g/%g", x->coord, x->speed, m->curparams.coord, m->curparams.speed);
if (!x || !m) return FALSE;
pthread_mutex_lock(&m->mutex);
int ret = FALSE;
// Validate input parameters
if(x->coord < m->Min.coord || x->coord > m->Max.coord){
DBG("Wrong coordinate [%g, %g]", m->Min.coord, m->Max.coord);
goto ret;
}
if(x->speed < m->Min.speed || x->speed > m->Max.speed){
DBG("Wrong speed [%g, %g]", m->Min.speed, m->Max.speed);
goto ret;
}
ret = TRUE; // now there's no chanses to make error
unlockedcalc(m, x, t);
// Debug output
/*for(int i = 0; i < STAGE_AMOUNT; i++){
DBG("Stage %d: t=%.6f, coord=%.6f, speed=%.6f, accel=%.6f",
i, m->Times[i], m->Params[i].coord, m->Params[i].speed, m->Params[i].accel);
}*/
ret:
pthread_mutex_unlock(&m->mutex);
return ret;
}
static movestate_t proc(movemodel_t *m, moveparam_t *next, double t){
pthread_mutex_lock(&m->mutex);
if(m->state == ST_STOP) goto ret;
for(movingstage_t s = STAGE_STOPPED; s >= 0; --s){
if(m->Times[s] <= t){ // check time for current stage
m->movingstage = s;
break;
}
}
if(m->movingstage == STAGE_STOPPED){
m->curparams.coord = m->Params[STAGE_STOPPED].coord;
pthread_mutex_unlock(&m->mutex);
DBG("REACHED STOPping stage @ t=%g", t);
for(int s = STAGE_STOPPED; s >= 0; --s){
DBG("T[%d]=%g, ", s, m->Times[s]);
}
fflush(stdout);
emstop(m, t);
goto ret;
}
// calculate current parameters
double dt = t - m->Times[m->movingstage];
double a = m->Params[m->movingstage].accel;
double v0 = m->Params[m->movingstage].speed;
double x0 = m->Params[m->movingstage].coord;
m->curparams.accel = a;
m->curparams.speed = v0 + a * dt;
m->curparams.coord = x0 + v0 * dt + a * dt * dt / 2.;
ret:
if(next) *next = m->curparams;
movestate_t st = m->state;
pthread_mutex_unlock(&m->mutex);
return st;
}
static movestate_t getst(movemodel_t *m, moveparam_t *cur){
pthread_mutex_lock(&m->mutex);
if(cur) *cur = m->curparams;
movestate_t st = m->state;
pthread_mutex_unlock(&m->mutex);
return st;
}
static double gettstop(movemodel_t *m){
pthread_mutex_lock(&m->mutex);
double r = m->Times[STAGE_STOPPED];
pthread_mutex_unlock(&m->mutex);
return r;
}
movemodel_t trapez = {
.stop = stop,
.emergency_stop = emstop,
.get_state = getst,
.calculate = calc,
.proc_move = proc,
.stoppedtime = gettstop,
};

23
Auxiliary_utils/LibSidServo/ramp.h Normal file
View File

@ -0,0 +1,23 @@
/*
* This file is part of the moving_model 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/>.
*/
#pragma once
#include "movingmodel.h"
extern movemodel_t trapez;

259
Auxiliary_utils/LibSidServo/serial.c
View File

@ -21,24 +21,26 @@
#include <fcntl.h>
#include <math.h>
#include <pthread.h>
#include <signal.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/ioctl.h>
#include <sys/stat.h>
#include <time.h>
#include <unistd.h>
#include "dbg.h"
#include "main.h"
#include "movingmodel.h"
#include "serial.h"
#include "ssii.h"
// serial devices FD
static int encfd[2] = {-1, -1}, mntfd = -1;
// main mount data
static mountdata_t mountdata = {0};
// last encoders time and last encoders data - for speed measurement
static coordval_t lastXenc = {0}, lastYenc = {0};
//static coordval_t lastXenc = {0}, lastYenc = {0};
// mutexes for RW operations with mount device and data
static pthread_mutex_t mntmutex = PTHREAD_MUTEX_INITIALIZER,
@ -46,7 +48,7 @@ static pthread_mutex_t mntmutex = PTHREAD_MUTEX_INITIALIZER,
// encoders thread and mount thread
static pthread_t encthread, mntthread;
// max timeout for 1.5 bytes of encoder and 2 bytes of mount - for `select`
static struct timeval encRtmout = {0}, mntRtmout = {0};
static struct timeval encRtmout = {.tv_sec = 0, .tv_usec = 50000}, mntRtmout = {.tv_sec = 0, .tv_usec = 50000};
// encoders raw data
typedef struct __attribute__((packed)){
uint8_t magick;
@ -55,57 +57,43 @@ typedef struct __attribute__((packed)){
uint8_t CRC[4];
} enc_t;
/**
* @brief dtime - monotonic time from first run
* @return
*/
double dtime(){
struct timespec start_time = {0}, cur_time;
if(start_time.tv_sec == 0 && start_time.tv_nsec == 0){
clock_gettime(CLOCK_MONOTONIC, &start_time);
}
clock_gettime(CLOCK_MONOTONIC, &cur_time);
return ((double)(cur_time.tv_sec - start_time.tv_sec) +
(cur_time.tv_nsec - start_time.tv_nsec) * 1e-9);
}
#if 0
double dtime(){
double t;
struct timeval tv;
gettimeofday(&tv, NULL);
t = tv.tv_sec + ((double)tv.tv_usec)/1e6;
return t;
}
#endif
#if 0
double tv2d(struct timeval *tv){
if(!tv) return 0.;
double t = tv->tv_sec + ((double)tv->tv_usec) / 1e6;
return t;
}
#endif
#if 0
// init start time
static void gttime(){
struct timeval tv;
gettimeofday(&tv, NULL);
tv_sec_got = tv.tv_sec;
tv_usec_got = tv.tv_usec;
}
#endif
// calculate current X/Y speeds
static void getXspeed(double t){
void getXspeed(){
static less_square_t *ls = NULL;
if(!ls){
ls = LS_init(Conf.EncoderSpeedInterval / Conf.EncoderReqInterval);
if(!ls) return;
}
double speed = LS_calc_slope(ls, mountdata.encXposition.val, mountdata.encXposition.t);
if(fabs(speed) < 1.5 * MCC_MAX_X_SPEED){
mountdata.encXspeed.val = speed;
mountdata.encXspeed.t = mountdata.encXposition.t;
}
//DBG("Xspeed=%g", mountdata.encXspeed.val);
#if 0
mountdata.encXspeed.val = (mountdata.encXposition.val - lastXenc.val) / (t - lastXenc.t);
mountdata.encXspeed.t = (lastXenc.t + t) / 2.;
mountdata.encXspeed.t = (lastXenc.t + mountdata.encXposition.t) / 2.;
lastXenc.val = mountdata.encXposition.val;
lastXenc.t = t;
#endif
}
static void getYspeed(double t){
void getYspeed(){
static less_square_t *ls = NULL;
if(!ls){
ls = LS_init(Conf.EncoderSpeedInterval / Conf.EncoderReqInterval);
if(!ls) return;
}
double speed = LS_calc_slope(ls, mountdata.encYposition.val, mountdata.encYposition.t);
if(fabs(speed) < 1.5 * MCC_MAX_Y_SPEED){
mountdata.encYspeed.val = speed;
mountdata.encYspeed.t = mountdata.encYposition.t;
}
#if 0
mountdata.encYspeed.val = (mountdata.encYposition.val - lastYenc.val) / (t - lastYenc.t);
mountdata.encYspeed.t = (lastYenc.t + t) / 2.;
mountdata.encYspeed.t = (lastYenc.t + mountdata.encYposition.t) / 2.;
lastYenc.val = mountdata.encYposition.val;
lastYenc.t = t;
#endif
}
/**
@ -153,8 +141,9 @@ static void parse_encbuf(uint8_t databuf[ENC_DATALEN], double t){
DBG("Got positions X/Y= %.6g / %.6g", mountdata.encXposition.val, mountdata.encYposition.val);
mountdata.encXposition.t = t;
mountdata.encYposition.t = t;
if(t - lastXenc.t > Conf.EncoderSpeedInterval) getXspeed(t);
if(t - lastYenc.t > Conf.EncoderSpeedInterval) getYspeed(t);
//if(t - lastXenc.t > Conf.EncoderSpeedInterval) getXspeed();
//if(t - lastYenc.t > Conf.EncoderSpeedInterval) getYspeed();
getXspeed(); getYspeed();
pthread_mutex_unlock(&datamutex);
//DBG("time = %zd+%zd/1e6, X=%g deg, Y=%g deg", tv->tv_sec, tv->tv_usec, mountdata.encposition.X*180./M_PI, mountdata.encposition.Y*180./M_PI);
}
@ -170,7 +159,7 @@ static int getencval(int fd, double *val, double *t){
if(fd < 0) return FALSE;
char buf[128];
int got = 0, Lmax = 127;
double t0 = dtime();
double t0 = nanotime();
do{
fd_set rfds;
FD_ZERO(&rfds);
@ -189,7 +178,7 @@ static int getencval(int fd, double *val, double *t){
buf[got] = 0;
} else continue;
if(strchr(buf, '\n')) break;
}while(Lmax && dtime() - t0 < Conf.EncoderReqInterval);
}while(Lmax && nanotime() - t0 < Conf.EncoderReqInterval);
if(got == 0) return 0; // WTF?
char *estr = strrchr(buf, '\n');
if(!estr) return 0;
@ -254,12 +243,40 @@ static int getmntbyte(){
if(FD_ISSET(mntfd, &rfds)){
ssize_t l = read(mntfd, &byte, 1);
//DBG("MNT read=%zd byte=0x%X", l, byte);
if(l != 1) return -2; // disconnected ??
if(l != 1){
DBG("Mount disconnected?");
return -2; // disconnected ??
}
break;
} else return -1;
}while(1);
return (int)byte;
}
// clear data from input buffer
static void clrmntbuf(){
if(mntfd < 0) return;
uint8_t byte;
fd_set rfds;
//double t0 = nanotime();
//int n = 0;
do{
FD_ZERO(&rfds);
FD_SET(mntfd, &rfds);
struct timeval tv = {.tv_sec=0, .tv_usec=10};
int retval = select(mntfd + 1, &rfds, NULL, NULL, &tv);
if(retval < 0){
if(errno == EINTR) continue;
DBG("Error in select()");
break;
}
if(FD_ISSET(mntfd, &rfds)){
ssize_t l = read(mntfd, &byte, 1);
if(l != 1) break;
//++n;
} else break;
}while(1);
//DBG("Cleared by %g (got %d bytes)", nanotime() - t0, n);
}
// main encoder thread (for separate encoder): read next data and make parsing
static void *encoderthread1(void _U_ *u){
@ -277,7 +294,7 @@ static void *encoderthread1(void _U_ *u){
if((uint8_t)b == ENC_MAGICK){
// DBG("Got magic -> start filling packet");
databuf[wridx++] = (uint8_t) b;
t = dtime();
t = nanotime();
}
continue;
}else databuf[wridx++] = (uint8_t) b;
@ -298,25 +315,27 @@ static void *encoderthread2(void _U_ *u){
if(Conf.SepEncoder != 2) return NULL;
DBG("Thread started");
int errctr = 0;
double t0 = dtime();
const char *req = "next\n";
double t0 = nanotime();
const char *req = "\n";
int need2ask = 0; // need or not to ask encoder for new data
while(encfd[0] > -1 && encfd[1] > -1 && errctr < MAX_ERR_CTR){
if(need2ask){
if(5 != write(encfd[0], req, 5)) { ++errctr; continue; }
else if(5 != write(encfd[1], req, 5)) { ++errctr; continue; }
if(1 != write(encfd[0], req, 1)) { ++errctr; continue; }
else if(1 != write(encfd[1], req, 1)) { ++errctr; continue; }
}
double v, t;
if(getencval(encfd[0], &v, &t)){
mountdata.encXposition.val = X_ENC2RAD(v);
//DBG("encX(%g) = %g", t, mountdata.encXposition.val);
mountdata.encXposition.t = t;
if(t - lastXenc.t > Conf.EncoderSpeedInterval) getXspeed(t);
//if(t - lastXenc.t > Conf.EncoderSpeedInterval) getXspeed();
getXspeed();
if(getencval(encfd[1], &v, &t)){
mountdata.encYposition.val = Y_ENC2RAD(v);
//DBG("encY(%g) = %g", t, mountdata.encYposition.val);
mountdata.encYposition.t = t;
if(t - lastYenc.t > Conf.EncoderSpeedInterval) getYspeed(t);
//if(t - lastYenc.t > Conf.EncoderSpeedInterval) getYspeed();
getYspeed();
errctr = 0;
need2ask = 0;
} else {
@ -329,9 +348,9 @@ static void *encoderthread2(void _U_ *u){
else need2ask = 1;
continue;
}
while(dtime() - t0 < Conf.EncoderReqInterval){ usleep(10); }
//DBG("DT=%g (RI=%g)", dtime()-t0, Conf.EncoderReqInterval);
t0 = dtime();
while(nanotime() - t0 < Conf.EncoderReqInterval){ usleep(50); }
//DBG("DT=%g (RI=%g)", nanotime()-t0, Conf.EncoderReqInterval);
t0 = nanotime();
}
DBG("ERRCTR=%d", errctr);
for(int i = 0; i < 2; ++i){
@ -364,20 +383,49 @@ static void *mountthread(void _U_ *u){
int errctr = 0;
uint8_t buf[2*sizeof(SSstat)];
SSstat *status = (SSstat*) buf;
bzero(&mountdata, sizeof(mountdata));
if(Conf.RunModel) while(1){
pthread_mutex_lock(&datamutex);
// now change data
getModData(&mountdata);
pthread_mutex_unlock(&datamutex);
double t0 = nanotime();
while(nanotime() - t0 < Conf.EncoderReqInterval) usleep(50);
t0 = nanotime();
}
// data to get
data_t d = {.buf = buf, .maxlen = sizeof(buf)};
// cmd to send
data_t *cmd_getstat = cmd2dat(CMD_GETSTAT);
if(!cmd_getstat) goto failed;
double t0 = dtime();
/*
#ifdef EBUG
double t00 = t0;
#endif
*/
while(mntfd > -1 && errctr < MAX_ERR_CTR){
// read data to status
double tgot = dtime();
double t0 = nanotime();
#if 0
// 127 milliseconds to get answer on X/Y commands!!!
int64_t ans;
int ctr = 0;
if(SSgetint(CMD_MOTX, &ans)){
pthread_mutex_lock(&datamutex);
mountdata.motXposition.t = tgot;
mountdata.motXposition.val = X_MOT2RAD(ans);
pthread_mutex_unlock(&datamutex);
++ctr;
}
tgot = nanotime();
if(SSgetint(CMD_MOTY, &ans)){
pthread_mutex_lock(&datamutex);
mountdata.motXposition.t = tgot;
mountdata.motXposition.val = X_MOT2RAD(ans);
pthread_mutex_unlock(&datamutex);
++ctr;
}
if(ctr == 2){
mountdata.millis = (uint32_t)(1e3 * tgot);
DBG("Got both coords; millis=%d", mountdata.millis);
}
#endif
// 80 milliseconds to get answer on GETSTAT
if(!MountWriteRead(cmd_getstat, &d) || d.len != sizeof(SSstat)){
#ifdef EBUG
DBG("Can't read SSstat, need %zd got %zd bytes", sizeof(SSstat), d.len);
@ -393,13 +441,14 @@ static void *mountthread(void _U_ *u){
errctr = 0;
pthread_mutex_lock(&datamutex);
// now change data
SSconvstat(status, &mountdata, tgot);
SSconvstat(status, &mountdata, t0);
pthread_mutex_unlock(&datamutex);
//DBG("GOT FULL stat by %g", nanotime() - t0);
// allow writing & getters
//DBG("t0=%g, tnow=%g", t0-t00, dtime()-t00);
if(dtime() - t0 >= Conf.MountReqInterval) usleep(50);
while(dtime() - t0 < Conf.MountReqInterval);
t0 = dtime();
do{
usleep(500);
}while(nanotime() - t0 < Conf.MountReqInterval);
t0 = nanotime();
}
data_free(&cmd_getstat);
failed:
@ -435,6 +484,7 @@ static int ttyopen(const char *path, speed_t speed){
// return FALSE if failed
int openEncoder(){
if(Conf.RunModel) return TRUE;
if(!Conf.SepEncoder) return FALSE; // try to open separate encoder when it's absent
if(Conf.SepEncoder == 1){ // only one device
DBG("One device");
@ -442,7 +492,7 @@ int openEncoder(){
encfd[0] = ttyopen(Conf.EncoderDevPath, (speed_t) Conf.EncoderDevSpeed);
if(encfd[0] < 0) return FALSE;
encRtmout.tv_sec = 0;
encRtmout.tv_usec = 200000000 / Conf.EncoderDevSpeed; // 20 bytes
encRtmout.tv_usec = 100000000 / Conf.EncoderDevSpeed; // 10 bytes
if(pthread_create(&encthread, NULL, encoderthread1, NULL)){
close(encfd[0]);
encfd[0] = -1;
@ -472,6 +522,7 @@ int openEncoder(){
// return FALSE if failed
int openMount(){
if(Conf.RunModel) goto create_thread;
if(mntfd > -1) close(mntfd);
DBG("Open mount %s @ %d", Conf.MountDevPath, Conf.MountDevSpeed);
mntfd = ttyopen(Conf.MountDevPath, (speed_t) Conf.MountDevSpeed);
@ -487,11 +538,14 @@ int openMount(){
DBG("got %zd", l);
}while(1);*/
mntRtmout.tv_sec = 0;
mntRtmout.tv_usec = 500000000 / Conf.MountDevSpeed; // 50 bytes
mntRtmout.tv_usec = 500000000 / Conf.MountDevSpeed; // 50 bytes * 10bits / speed
create_thread:
if(pthread_create(&mntthread, NULL, mountthread, NULL)){
DBG("Can't create thread");
close(mntfd);
mntfd = -1;
DBG("Can't create mount thread");
if(!Conf.RunModel){
close(mntfd);
mntfd = -1;
}
return FALSE;
}
DBG("Mount opened, thread started");
@ -500,21 +554,22 @@ int openMount(){
// close all opened serial devices and quit threads
void closeSerial(){
if(Conf.RunModel) return;
if(mntfd > -1){
DBG("Kill mount thread");
DBG("Cancel mount thread");
pthread_cancel(mntthread);
DBG("join mount thread");
pthread_join(mntthread, NULL);
DBG("close fd");
DBG("close mount fd");
close(mntfd);
mntfd = -1;
}
if(encfd[0] > -1){
DBG("Kill encoder thread");
DBG("Cancel encoder thread");
pthread_cancel(encthread);
DBG("join encoder thread");
pthread_join(encthread, NULL);
DBG("close fd");
DBG("close encoder's fd");
close(encfd[0]);
encfd[0] = -1;
if(Conf.SepEncoder == 2){
@ -533,16 +588,22 @@ mcc_errcodes_t getMD(mountdata_t *d){
return MCC_E_OK;
}
void setStat(mnt_status_t Xstatus, mnt_status_t Ystatus){
void setStat(axis_status_t Xstate, axis_status_t Ystate){
DBG("set x/y state to %d/%d", Xstate, Ystate);
pthread_mutex_lock(&datamutex);
mountdata.Xstatus = Xstatus;
mountdata.Ystatus = Ystatus;
mountdata.Xstate = Xstate;
mountdata.Ystate = Ystate;
pthread_mutex_unlock(&datamutex);
}
// write-read without locking mutex (to be used inside other functions)
static int wr(const data_t *out, data_t *in, int needeol){
if((!out && !in) || mntfd < 0) return FALSE;
if((!out && !in) || mntfd < 0){
DBG("Wrong arguments or no mount fd");
return FALSE;
}
clrmntbuf();
//double t0 = nanotime();
if(out){
if(out->len != (size_t)write(mntfd, out->buf, out->len)){
DBG("written bytes not equal to need");
@ -554,16 +615,18 @@ static int wr(const data_t *out, data_t *in, int needeol){
(void) g;
}
}
uint8_t buf[256];
data_t dumb = {.buf = buf, .maxlen = 256};
if(!in) in = &dumb; // even if user don't ask for answer, try to read to clear trash
//DBG("sent by %g", nanotime() - t0);
//uint8_t buf[256];
//data_t dumb = {.buf = buf, .maxlen = 256};
if(!in) return TRUE;
//if(!in) in = &dumb; // even if user don't ask for answer, try to read to clear trash
in->len = 0;
for(size_t i = 0; i < in->maxlen; ++i){
int b = getmntbyte();
if(b < 0) break; // nothing to read -> go out
in->buf[in->len++] = (uint8_t) b;
}
//DBG("Clear trashing input");
//DBG("got %zd bytes by %g", in->len, nanotime() - t0);
while(getmntbyte() > -1);
return TRUE;
}
@ -575,6 +638,7 @@ static int wr(const data_t *out, data_t *in, int needeol){
* @return FALSE if failed
*/
int MountWriteRead(const data_t *out, data_t *in){
if(Conf.RunModel) return -1;
pthread_mutex_lock(&mntmutex);
int ret = wr(out, in, 1);
pthread_mutex_unlock(&mntmutex);
@ -582,6 +646,7 @@ int MountWriteRead(const data_t *out, data_t *in){
}
// send binary data - without EOL
int MountWriteReadRaw(const data_t *out, data_t *in){
if(Conf.RunModel) return -1;
pthread_mutex_lock(&mntmutex);
int ret = wr(out, in, 0);
pthread_mutex_unlock(&mntmutex);
@ -605,32 +670,32 @@ static void loglcmd(SSlcmd *c){
// send short/long binary command; return FALSE if failed
static int bincmd(uint8_t *cmd, int len){
if(Conf.RunModel) return FALSE;
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};
//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");
#ifdef EBUG
logscmd((SSscmd*)cmd);
#endif
if(!wr(dscmd, &a, 1)) goto rtn;
if(!wr(dscmd, NULL, 1)) goto rtn;
}else if(len == sizeof(SSlcmd)){
((SSlcmd*)cmd)->checksum = SScalcChecksum(cmd, len-2);
DBG("Long command");
#ifdef EBUG
loglcmd((SSlcmd*)cmd);
#endif
if(!wr(dlcmd, &a, 1)) goto rtn;
if(!wr(dlcmd, NULL, 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;
@ -641,6 +706,7 @@ rtn:
return ret;
}
// short, long and config text-binary commands
// return TRUE if OK
int cmdS(SSscmd *cmd){
return bincmd((uint8_t *)cmd, sizeof(SSscmd));
@ -650,6 +716,7 @@ int cmdL(SSlcmd *cmd){
}
// rw == 1 to write, 0 to read
int cmdC(SSconfig *conf, int rw){
if(Conf.RunModel) return FALSE;
static data_t *wcmd = NULL, *rcmd = NULL;
int ret = FALSE;
// dummy buffer to clear trash in input

5
Auxiliary_utils/LibSidServo/serial.h
View File

@ -28,16 +28,17 @@
// max error counter (when read() returns -1)
#define MAX_ERR_CTR (100)
double dtime();
data_t *cmd2dat(const char *cmd);
void data_free(data_t **x);
int openEncoder();
int openMount();
void closeSerial();
mcc_errcodes_t getMD(mountdata_t *d);
void setStat(mnt_status_t Xstatus, mnt_status_t Ystatus);
void setStat(axis_status_t Xstate, axis_status_t Ystate);
int MountWriteRead(const data_t *out, data_t *in);
int MountWriteReadRaw(const data_t *out, data_t *in);
int cmdS(SSscmd *cmd);
int cmdL(SSlcmd *cmd);
int cmdC(SSconfig *conf, int rw);
void getXspeed();
void getYspeed();

88
Auxiliary_utils/LibSidServo/sidservo.h
View File

@ -16,6 +16,11 @@
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
* This file contains all need for external usage
*/
#pragma once
#ifdef __cplusplus
@ -27,14 +32,37 @@ extern "C"
#include <stdint.h>
#include <sys/time.h>
// acceptable position error - 0.1''
#define MCC_POSITION_ERROR (5e-7)
// acceptable disagreement between motor and axis encoders - 2''
#define MCC_ENCODERS_ERROR (1e-7)
// max speeds (rad/s): xs=10 deg/s, ys=8 deg/s
#define MCC_MAX_X_SPEED (0.174533)
#define MCC_MAX_Y_SPEED (0.139626)
// accelerations by both axis (for model); TODO: move speeds/accelerations into config?
// xa=12.6 deg/s^2, ya= 9.5 deg/s^2
#define MCC_X_ACCELERATION (0.219911)
#define MCC_Y_ACCELERATION (0.165806)
// max speed interval, seconds
#define MCC_CONF_MAX_SPEEDINT (2.)
// minimal speed interval in parts of EncoderReqInterval
#define MCC_CONF_MIN_SPEEDC (3.)
// PID I cycle time (analog of "RC" for PID on opamps)
#define MCC_PID_CYCLE_TIME (5.)
// maximal PID refresh time interval (if larger all old data will be cleared)
#define MCC_PID_MAX_DT (1.)
// normal PID refresh interval
#define MCC_PID_REFRESH_DT (0.1)
// boundary conditions for axis state: "slewing/pointing/guiding"
// if angle < MCC_MAX_POINTING_ERR, change state from "slewing" to "pointing": 5 degrees
//#define MCC_MAX_POINTING_ERR (0.20943951)
#define MCC_MAX_POINTING_ERR (0.08726646)
// if angle < MCC_MAX_GUIDING_ERR, chane state from "pointing" to "guiding": 1.5 deg
#define MCC_MAX_GUIDING_ERR (0.026179939)
// if error less than this value we suppose that target is captured and guiding is good: 0.1''
#define MCC_MAX_ATTARGET_ERR (4.8481368e-7)
// error codes
typedef enum{
@ -47,16 +75,25 @@ typedef enum{
} mcc_errcodes_t;
typedef struct{
char* MountDevPath; // path to mount device
int MountDevSpeed; // serial speed
char* EncoderDevPath; // path to encoder device
int EncoderDevSpeed; // serial speed
int SepEncoder; // ==1 if encoder works as separate serial device, ==2 if there's new version with two devices
char* EncoderXDevPath; // paths to new controller devices
double P, I, D;
} PIDpar_t;
typedef struct{
char* MountDevPath; // path to mount device
int MountDevSpeed; // serial speed
char* EncoderDevPath; // path to encoder device
int EncoderDevSpeed; // serial speed
int SepEncoder; // ==1 if encoder works as separate serial device, ==2 if there's new version with two devices
char* EncoderXDevPath; // paths to new controller devices
char* EncoderYDevPath;
double MountReqInterval; // interval between subsequent mount requests (seconds)
double EncoderReqInterval; // interval between subsequent encoder requests (seconds)
double EncoderSpeedInterval;// interval between speed calculations
double MountReqInterval; // interval between subsequent mount requests (seconds)
double EncoderReqInterval; // interval between subsequent encoder requests (seconds)
double EncoderSpeedInterval; // interval between speed calculations
int RunModel; // == 1 if you want to use model instead of real mount
PIDpar_t XPIDC; // gain parameters of PID for both axiss (C - coordinate driven, V - velocity driven)
PIDpar_t XPIDV;
PIDpar_t YPIDC;
PIDpar_t YPIDV;
} conf_t;
// coordinates/speeds in degrees or d/s: X, Y
@ -113,16 +150,16 @@ typedef struct{
} extradata_t;
typedef enum{
MNT_STOPPED,
MNT_SLEWING,
MNT_POINTING,
MNT_GUIDING,
MNT_ERROR,
} mnt_status_t;
AXIS_STOPPED,
AXIS_SLEWING,
AXIS_POINTING,
AXIS_GUIDING,
AXIS_ERROR,
} axis_status_t;
typedef struct{
mnt_status_t Xstatus;
mnt_status_t Ystatus;
axis_status_t Xstate;
axis_status_t Ystate;
coordval_t motXposition;
coordval_t motYposition;
coordval_t encXposition;
@ -157,7 +194,7 @@ typedef struct{
double Yatime; // 28
} long_command_t; // long command
// hardware axe configuration
// hardware axis configuration
typedef struct{
double accel; // Default Acceleration, rad/s^2
double backlash; // Backlash (???)
@ -168,13 +205,13 @@ typedef struct{
double outplimit; // Output Limit, percent (0..100)
double currlimit; // Current Limit (A)
double intlimit; // Integral Limit (???)
} __attribute__((packed)) axe_config_t;
} __attribute__((packed)) axis_config_t;
// hardware configuration
typedef struct{
axe_config_t Xconf;
axis_config_t Xconf;
xbits_t xbits;
axe_config_t Yconf;
axis_config_t Yconf;
ybits_t ybits;
uint8_t address;
double eqrate; // Equatorial Rate (???)
@ -197,19 +234,19 @@ typedef struct{
double backlspd; // Backlash speed (rad/s)
} hardware_configuration_t;
// flags for slew function
/* flags for slew function
typedef struct{
uint32_t slewNguide : 1; // ==1 to guide after slewing
} slewflags_t;
*/
// mount class
typedef struct{
// TODO: on init/quit clear all XY-bits to default`
mcc_errcodes_t (*init)(conf_t *c); // init device
void (*quit)(); // deinit
mcc_errcodes_t (*getMountData)(mountdata_t *d); // get last data
mcc_errcodes_t (*slewTo)(const coordpair_t *target, slewflags_t flags);
mcc_errcodes_t (*correctTo)(coordval_pair_t *target);
// mcc_errcodes_t (*slewTo)(const coordpair_t *target, slewflags_t flags);
mcc_errcodes_t (*correctTo)(const coordval_pair_t *target, const coordpair_t *endpoint);
mcc_errcodes_t (*moveTo)(const coordpair_t *target); // move to given position and stop
mcc_errcodes_t (*moveWspeed)(const coordpair_t *target, const coordpair_t *speed); // move with given max speed
mcc_errcodes_t (*setSpeed)(const coordpair_t *tagspeed); // set speed
@ -224,7 +261,6 @@ typedef struct{
extern mount_t Mount;
#ifdef __cplusplus
}
#endif

80
Auxiliary_utils/LibSidServo/ssii.c
View File

@ -19,8 +19,9 @@
#include <ctype.h>
#include <inttypes.h>
#include <string.h>
#include <unistd.h>
#include "dbg.h"
#include "main.h"
#include "serial.h"
#include "ssii.h"
@ -35,16 +36,18 @@ uint16_t SScalcChecksum(uint8_t *buf, int len){
return checksum;
}
static void axestat(int32_t *prev, int32_t cur, int *nstopped, mnt_status_t *stat){
// Next three functions runs under locked mountdata_t mutex and shouldn't call locked it again!!
static void chkstopstat(int32_t *prev, int32_t cur, int *nstopped, axis_status_t *stat){
if(*prev == INT32_MAX){
*stat = MNT_STOPPED;
}else if(*stat != MNT_STOPPED){
if(*prev == cur){
if(++(*nstopped) > MOTOR_STOPPED_CNT) *stat = MNT_STOPPED;
*stat = AXIS_STOPPED;
DBG("START");
}else if(*stat != AXIS_STOPPED){
if(*prev == cur && ++(*nstopped) > MOTOR_STOPPED_CNT){
*stat = AXIS_STOPPED;
DBG("AXIS stopped");
}
}else if(*prev != cur){
//*stat = MNT_SLEWING;
DBG("AXIS moving");
*nstopped = 0;
}
*prev = cur;
@ -53,8 +56,8 @@ static void axestat(int32_t *prev, int32_t cur, int *nstopped, mnt_status_t *sta
static void ChkStopped(const SSstat *s, mountdata_t *m){
static int32_t Xmot_prev = INT32_MAX, Ymot_prev = INT32_MAX; // previous coordinates
static int Xnstopped = 0, Ynstopped = 0; // counters to get STOPPED state
axestat(&Xmot_prev, s->Xmot, &Xnstopped, &m->Xstatus);
axestat(&Ymot_prev, s->Ymot, &Ynstopped, &m->Ystatus);
chkstopstat(&Xmot_prev, s->Xmot, &Xnstopped, &m->Xstate);
chkstopstat(&Ymot_prev, s->Ymot, &Ynstopped, &m->Ystate);
}
/**
@ -65,13 +68,6 @@ static void ChkStopped(const SSstat *s, mountdata_t *m){
*/
void SSconvstat(const SSstat *s, mountdata_t *m, double t){
if(!s || !m) return;
/*
#ifdef EBUG
static double t0 = -1.;
if(t0 < 0.) t0 = dtime();
#endif
DBG("Convert, t=%g", dtime()-t0);
*/
m->motXposition.val = X_MOT2RAD(s->Xmot);
m->motYposition.val = Y_MOT2RAD(s->Ymot);
ChkStopped(s, m);
@ -81,10 +77,8 @@ void SSconvstat(const SSstat *s, mountdata_t *m, double t){
m->encXposition.val = X_ENC2RAD(s->Xenc);
m->encYposition.val = Y_ENC2RAD(s->Yenc);
m->encXposition.t = m->encYposition.t = t;
getXspeed(); getYspeed();
}
//m->lastmotposition.X = X_MOT2RAD(s->XLast);
//m->lastmotposition.Y = Y_MOT2RAD(s->YLast);
//m->lastmotposition.msrtime = *tdat;
m->keypad = s->keypad;
m->extradata.ExtraBits = s->ExtraBits;
m->extradata.ain0 = s->ain0;
@ -110,7 +104,7 @@ int SStextcmd(const char *cmd, data_t *answer){
data_t d;
d.buf = (uint8_t*) cmd;
d.len = d.maxlen = strlen(cmd);
DBG("send %zd bytes: %s", d.len, d.buf);
//DBG("send %zd bytes: %s", d.len, d.buf);
return MountWriteRead(&d, answer);
}
@ -123,7 +117,7 @@ int SSrawcmd(const char *cmd, data_t *answer){
data_t d;
d.buf = (uint8_t*) cmd;
d.len = d.maxlen = strlen(cmd);
DBG("send %zd bytes: %s", d.len, d.buf);
//DBG("send %zd bytes: %s", d.len, d.buf);
return MountWriteReadRaw(&d, answer);
}
@ -180,22 +174,38 @@ int SSstop(int emerg){
// update motors' positions due to encoders'
mcc_errcodes_t updateMotorPos(){
mountdata_t md = {0};
double t0 = dtime(), t = 0.;
if(Conf.RunModel) return MCC_E_OK;
double t0 = nanotime(), t = 0.;
DBG("start @ %g", t0);
do{
t = dtime();
t = nanotime();
if(MCC_E_OK == getMD(&md)){
DBG("got");
if(fabs(md.encXposition.t - t) < 0.1 && fabs(md.encYposition.t - t) < 0.1){
DBG("FIX motors position to encoders");
int32_t Xpos = X_RAD2MOT(md.encXposition.val), Ypos = Y_RAD2MOT(md.encYposition.val);
if(SSsetterI(CMD_MOTXSET, Xpos) && SSsetterI(CMD_MOTYSET, Ypos)){
DBG("All OK");
return MCC_E_OK;
}
}else{
DBG("on position");
return MCC_E_OK;
if(md.encXposition.t == 0 || md.encYposition.t == 0){
DBG("Just started, t-t0 = %g!", t - t0);
sleep(1);
DBG("t-t0 = %g", nanotime() - t0);
//usleep(10000);
continue;
}
DBG("got; t pos x/y: %g/%g; tnow: %g", md.encXposition.t, md.encYposition.t, t);
mcc_errcodes_t OK = MCC_E_OK;
if(fabs(md.motXposition.val - md.encXposition.val) > MCC_ENCODERS_ERROR && md.Xstate == AXIS_STOPPED){
DBG("NEED to sync X: motors=%g, axiss=%g", md.motXposition.val, md.encXposition.val);
if(!SSsetterI(CMD_MOTXSET, X_RAD2MOT(md.encXposition.val))){
DBG("Xpos sync failed!");
OK = MCC_E_FAILED;
}else DBG("Xpos sync OK, Dt=%g", nanotime() - t0);
}
if(fabs(md.motYposition.val - md.encYposition.val) > MCC_ENCODERS_ERROR && md.Xstate == AXIS_STOPPED){
DBG("NEED to sync Y: motors=%g, axiss=%g", md.motYposition.val, md.encYposition.val);
if(!SSsetterI(CMD_MOTYSET, Y_RAD2MOT(md.encYposition.val))){
DBG("Ypos sync failed!");
OK = MCC_E_FAILED;
}else DBG("Ypos sync OK, Dt=%g", nanotime() - t0);
}
if(MCC_E_OK == OK){
DBG("Encoders synced");
return OK;
}
}
DBG("NO DATA; dt = %g", t - t0);

44
Auxiliary_utils/LibSidServo/ssii.h
View File

@ -16,6 +16,10 @@
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
* This file contains stuff for sidereal-servo specific protocol
*/
#pragma once
#include <math.h>
@ -169,18 +173,32 @@
#define SITECH_LOOP_FREQUENCY (1953.)
// amount of consequent same coordinates to detect stop
#define MOTOR_STOPPED_CNT (20)
#define MOTOR_STOPPED_CNT (4)
// TODO: take it from settings?
// steps per revolution (SSI - x4 - for SSI)
// 13312000 / 4 = 3328000
#define X_MOT_STEPSPERREV_SSI (13312000.)
//#define X_MOT_STEPSPERREV (3325952.)
// 13312000 / 4 = 3328000
#define X_MOT_STEPSPERREV (3328000.)
// 17578668 / 4 = 4394667
#define Y_MOT_STEPSPERREV_SSI (17578668.)
//#define Y_MOT_STEPSPERREV (4394960.)
// 17578668 / 4 = 4394667
#define Y_MOT_STEPSPERREV (4394667.)
// encoder per revolution
#define X_ENC_STEPSPERREV (67108864.)
#define Y_ENC_STEPSPERREV (67108864.)
// encoder zero position
#define X_ENC_ZERO (61245239)
#define Y_ENC_ZERO (36999830)
// encoder reversed (no: +1)
#define X_ENC_SIGN (-1.)
#define Y_ENC_SIGN (-1.)
// encoder position to radians and back
#define X_ENC2RAD(n) ang2half(X_ENC_SIGN * 2.*M_PI * ((double)((n)-X_ENC_ZERO)) / X_ENC_STEPSPERREV)
#define Y_ENC2RAD(n) ang2half(Y_ENC_SIGN * 2.*M_PI * ((double)((n)-Y_ENC_ZERO)) / Y_ENC_STEPSPERREV)
#define X_RAD2ENC(r) ((uint32_t)((r) / 2./M_PI * X_ENC_STEPSPERREV))
#define Y_RAD2ENC(r) ((uint32_t)((r) / 2./M_PI * Y_ENC_STEPSPERREV))
// convert angle in radians to +-pi
static inline double ang2half(double ang){
if(ang < -M_PI) ang += 2.*M_PI;
@ -214,21 +232,6 @@ static inline double ang2full(double ang){
#define ADDER2S(a) ((a) / SITECH_LOOP_FREQUENCY)
#define S2ADDER(s) ((s) * SITECH_LOOP_FREQUENCY)
// encoder per revolution
#define X_ENC_STEPSPERREV (67108864.)
#define Y_ENC_STEPSPERREV (67108864.)
// encoder zero position
#define X_ENC_ZERO (61245239)
#define Y_ENC_ZERO (36999830)
// encoder reversed (no: +1)
#define X_ENC_SIGN (-1.)
#define Y_ENC_SIGN (-1.)
// encoder position to radians and back
#define X_ENC2RAD(n) ang2half(X_ENC_SIGN * 2.*M_PI * ((double)(n-X_ENC_ZERO)) / X_ENC_STEPSPERREV)
#define Y_ENC2RAD(n) ang2half(Y_ENC_SIGN * 2.*M_PI * ((double)(n-Y_ENC_ZERO)) / Y_ENC_STEPSPERREV)
#define X_RAD2ENC(r) ((uint32_t)((r) / 2./M_PI * X_ENC_STEPSPERREV))
#define Y_RAD2ENC(r) ((uint32_t)((r) / 2./M_PI * Y_ENC_STEPSPERREV))
// encoder's tolerance (ticks)
#define YencTOL (25.)
#define XencTOL (25.)
@ -334,5 +337,4 @@ int SSrawcmd(const char *cmd, data_t *answer);
int SSgetint(const char *cmd, int64_t *ans);
int SSsetterI(const char *cmd, int32_t ival);
int SSstop(int emerg);
int SSshortCmd(SSscmd *cmd);
mcc_errcodes_t updateMotorPos();

9
Daemons/10micron_stellarium/main.c
View File

@ -226,6 +226,7 @@ int proc_data(uint8_t *data, ssize_t len){
// convert RA/DEC to hours/degrees
double tagRA = RA2HRS(ra), tagDec = DEC2DEG(dec);
DBG("RA: %u (%g), DEC: %d (%g)", ra, tagRA, dec, tagDec);
putlog("RA: %u (%g degr), DEC: %d (%g degr)", ra, tagRA, dec, tagDec);
// check RA/DEC
horizCrds hnow; // without refraction
polarCrds p2000, pnow;
@ -236,7 +237,7 @@ int proc_data(uint8_t *data, ssize_t len){
WARNX("Can't convert coordinates to Jnow");
return 0;
}
#ifdef EBUG
/*
int i[4], j[4]; char pm, pm1;
eraA2af(2, hnow.az, &pm, i);
eraA2af(2, hnow.zd, &pm1, j);
@ -245,10 +246,10 @@ int proc_data(uint8_t *data, ssize_t len){
pm1,j[0],j[1],j[2],j[3]);
eraA2af(2, M_PI_2 - hnow.zd, &pm, i);
DBG("h: %c%02d %02d %02d.%2.d", pm, i[0],i[1],i[2],i[3]);
#endif
*/
if(hnow.zd > 80.*ERFA_DD2R){
WARNX("Z > 80degr, stop telescope");
putlog("Z>80 - stop!");
WARNX("Z > 80degr (%g), stop telescope", hnow.zd * ERFA_DR2D);
putlog("Z=%.1f > 80 - stop!", hnow.zd * ERFA_DR2D);
stop_telescope();
return 0;
}