132 lines
4.2 KiB
C
132 lines
4.2 KiB
C
/*
|
|
* This file is part of the libsidservo project.
|
|
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
|
*
|
|
* This program is free software: you can redistribute it and/or modify
|
|
* it under the terms of the GNU General Public License as published by
|
|
* the Free Software Foundation, either version 3 of the License, or
|
|
* (at your option) any later version.
|
|
*
|
|
* This program is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
* GNU General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License
|
|
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
// logging of mount position
|
|
|
|
#include <usefull_macros.h>
|
|
|
|
#include "dump.h"
|
|
#include "simpleconv.h"
|
|
|
|
/**
|
|
* @brief logmnt - log mount data into file
|
|
* @param fcoords - file to dump
|
|
* @param m - mount data
|
|
*/
|
|
void logmnt(FILE *fcoords, mountdata_t *m){
|
|
if(!fcoords) return;
|
|
//DBG("LOG %s", m ? "data" : "header");
|
|
static double t0 = -1.;
|
|
if(!m){ // write header
|
|
fprintf(fcoords, "# time Xmot(deg) Ymot(deg) Xenc(deg) Yenc(deg) millis T V\n");
|
|
return;
|
|
}
|
|
if(t0 < 0.) t0 = m->motposition.msrtime.tv_sec + (double)(m->motposition.msrtime.tv_usec) / 1e6;
|
|
double t = m->motposition.msrtime.tv_sec + (double)(m->motposition.msrtime.tv_usec) / 1e6 - t0;
|
|
// write data
|
|
fprintf(fcoords, "%12.6f %10.6f %10.6f %10.6f %10.6f %10u %6.1f %4.1f\n",
|
|
t, RAD2DEG(m->motposition.X), RAD2DEG(m->motposition.Y),
|
|
RAD2DEG(m->encposition.X), RAD2DEG(m->encposition.Y),
|
|
m->millis, m->temperature, m->voltage);
|
|
fflush(fcoords);
|
|
}
|
|
|
|
/**
|
|
* @brief dumpmoving - dump conf while moving
|
|
* @param fcoords - dump file
|
|
* @param t - max waiting time
|
|
* @param N - number of cycles to wait while motors aren't moving
|
|
*/
|
|
void dumpmoving(FILE *fcoords, double t, int N){
|
|
if(!fcoords) return;
|
|
mountdata_t mdata;
|
|
DBG("Start dump");
|
|
int ntries = 0;
|
|
for(; ntries < 10; ++ntries){
|
|
if(MCC_E_OK == Mount.getMountData(&mdata)) break;
|
|
}
|
|
if(ntries == 10){
|
|
WARNX("Can't get mount data");
|
|
LOGWARN("Can't get mount data");
|
|
}
|
|
uint32_t millis = mdata.millis;
|
|
int ctr = -1;
|
|
double xlast = mdata.motposition.X, ylast = mdata.motposition.Y;
|
|
double t0 = sl_dtime();
|
|
//DBG("millis = %u", millis);
|
|
while(sl_dtime() - t0 < t && ctr < N){
|
|
usleep(10000);
|
|
if(MCC_E_OK != Mount.getMountData(&mdata)){ WARNX("Can't get data"); continue;}
|
|
if(mdata.millis == millis) continue;
|
|
//DBG("Got new data, posX=%g, posY=%g", mdata.motposition.X, mdata.motposition.Y);
|
|
millis = mdata.millis;
|
|
if(fcoords) logmnt(fcoords, &mdata);
|
|
if(mdata.motposition.X != xlast || mdata.motposition.Y != ylast){
|
|
xlast = mdata.motposition.X;
|
|
ylast = mdata.motposition.Y;
|
|
ctr = 0;
|
|
}else ++ctr;
|
|
}
|
|
}
|
|
|
|
/**
|
|
* @brief waitmoving - wait until moving by both axes stops at least for N cycles
|
|
* @param N - amount of stopped cycles
|
|
*/
|
|
void waitmoving(int N){
|
|
mountdata_t mdata;
|
|
int ctr = -1;
|
|
uint32_t millis = 0;
|
|
double xlast = 0., ylast = 0.;
|
|
while(ctr < N){
|
|
usleep(10000);
|
|
if(MCC_E_OK != Mount.getMountData(&mdata)){ WARNX("Can't get data"); continue;}
|
|
if(mdata.millis == millis) continue;
|
|
if(mdata.motposition.X != xlast || mdata.motposition.Y != ylast){
|
|
xlast = mdata.motposition.X;
|
|
ylast = mdata.motposition.Y;
|
|
ctr = 0;
|
|
}else ++ctr;
|
|
}
|
|
}
|
|
|
|
/**
|
|
* @brief getMotPos - get current
|
|
* @param mot (o) - motor position (or NULL)
|
|
* @param Y (o) - encoder position (or NULL)
|
|
* @return FALSE if failed
|
|
*/
|
|
int getPos(coords_t *mot, coords_t *enc){
|
|
mountdata_t mdata;
|
|
int errcnt = 0;
|
|
do{
|
|
if(MCC_E_OK != Mount.getMountData(&mdata)) ++errcnt;
|
|
else{
|
|
errcnt = 0;
|
|
if(mdata.millis) break;
|
|
}
|
|
}while(errcnt < 10);
|
|
if(errcnt >= 10){
|
|
WARNX("Can't read mount status");
|
|
return FALSE;
|
|
}
|
|
if(mot) *mot = mdata.motposition;
|
|
if(enc) *enc = mdata.encposition;
|
|
return TRUE;
|
|
}
|