eddy/mountcontrol
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

add PID (not tested yet)

Browse Source
This commit is contained in:
Edward V. Emelianov 2025-08-04 17:43:35 +03:00
parent 219cec6055
commit f661dfad44
10 changed files with 300 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

174
LibSidServo/PID.c Normal file
View File

@ -0,0 +1,174 @@
/*
* 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 axe
* @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) 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\n");
pid = pidpair->PIDC;
}else{
DBG("Slewing...\n");
return -1.; // max speed for given axis
}
break;
case AXIS_POINTING:
if(fe < MCC_MAX_GUIDING_ERR){
*axis->state = AXIS_GUIDING;
DBG("--> Guiding\n");
pid = pidpair->PIDV;
}else if(fe > MCC_MAX_POINTING_ERR){
DBG("--> Slewing\n");
*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\n");
*axis->state = AXIS_POINTING;
pid = pidpair->PIDC;
}else if(fe < MCC_MAX_ATTARGET_ERR){
DBG("At target\n");
// TODO: we can point somehow that we are at target or introduce new axis state
}else DBG("Current error: %g\n", fe);
break;
case AXIS_STOPPED:
case AXIS_ERROR:
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) pid_clear(pid);
double dt = tagpos->t - pid->prevT;
if(dt > MCC_PID_MAX_DT) dt = MCC_PID_CYCLE_TIME;
pid->prevT = tagpos->t;
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 axe;
axe.state = &m.Xstate;
axe.position = m.encXposition;
axe.speed = m.encXspeed;
tagspeed.X = getspeed(&target->X, &pidX, &axe);
if(tagspeed.X < 0. || tagspeed.X > MCC_MAX_X_SPEED) tagspeed.X = MCC_MAX_X_SPEED;
axe.state = &m.Ystate;
axe.position = m.encYposition;
axe.speed = m.encYspeed;
tagspeed.Y = getspeed(&target->Y, &pidY, &axe);
if(tagspeed.Y < 0. || tagspeed.Y > MCC_MAX_Y_SPEED) tagspeed.Y = MCC_MAX_Y_SPEED;
return Mount.moveWspeed(endpoint, &tagspeed);
}

40
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);

1
LibSidServo/TODO
View File

@ -1,3 +1,2 @@
1. PID: slew2
2. add model & config "model ON"

20
LibSidServo/examples/conf.c
View File

@ -31,6 +31,14 @@ static conf_t Config = {
.EncoderReqInterval = 0.05,
.SepEncoder = 2,
.EncoderSpeedInterval = 0.1,
.XPIDC.P = 0.5,
.XPIDC.D = 0.05,
.XPIDV.P = 0.5,
.XPIDV.D = 0.05,
.YPIDC.P = 0.5,
.YPIDC.D = 0.05,
.YPIDV.P = 0.5,
.YPIDV.D = 0.05,
};
static sl_option_t opts[] = {
@ -45,6 +53,18 @@ static sl_option_t opts[] = {
{"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
};

2
LibSidServo/libsidservo.files
View File

@ -1,4 +1,6 @@
CMakeLists.txt
PID.c
PID.h
examples/SSIIconf.c
examples/conf.c
examples/conf.h

21
LibSidServo/main.c
View File

@ -30,6 +30,7 @@
#include "movingmodel.h"
#include "serial.h"
#include "ssii.h"
#include "PID.h"
conf_t Conf = {0};
// parameters for model
@ -209,17 +210,18 @@ static int chkYs(double s){
return TRUE;
}
/*
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
@ -239,7 +241,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);
setStat(AXIS_SLEWING, AXIS_SLEWING);
return MCC_E_OK;
}
@ -269,14 +271,6 @@ static mcc_errcodes_t move2s(const coordpair_t *target, const coordpair_t *speed
if(!target || !speed) return MCC_E_BADFORMAT;
if(!chkX(target->X) || !chkY(target->Y)) return MCC_E_BADFORMAT;
if(!chkXs(speed->X) || !chkYs(speed->Y)) return MCC_E_BADFORMAT;
/* if(Conf.RunModel){
double curt = nanotime();
moveparam_t param = {0};
param.coord = target->X; param.speed = speed->X;
if(!model_move2(Xmodel, &param, curt)) return MCC_E_FAILED;
param.coord = target->Y; param.speed = speed->Y;
if(!model_move2(Ymodel, &param, curt)) return MCC_E_FAILED;
}*/
if(MCC_E_OK != updateMotorPos()) return MCC_E_FAILED;
short_command_t cmd = {0};
cmd.Xmot = target->X;
@ -285,7 +279,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);
setStat(AXIS_SLEWING, AXIS_SLEWING);
return MCC_E_OK;
}
@ -501,7 +495,7 @@ mount_t Mount = {
.init = init,
.quit = quit,
.getMountData = getMD,
.slewTo = slew2,
// .slewTo = slew2,
.moveTo = move2,
.moveWspeed = move2s,
.setSpeed = setspeed,
@ -512,5 +506,6 @@ mount_t Mount = {
.getHWconfig = get_hwconf,
.saveHWconfig = write_hwconf,
.currentT = nanotime,
.correctTo = correct2,
};

6
LibSidServo/serial.c
View File

@ -585,10 +585,10 @@ 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){
pthread_mutex_lock(&datamutex);
mountdata.Xstatus = Xstatus;
mountdata.Ystatus = Ystatus;
mountdata.Xstate = Xstate;
mountdata.Ystate = Ystate;
pthread_mutex_unlock(&datamutex);
}

2
LibSidServo/serial.h
View File

@ -34,7 +34,7 @@ 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);

47
LibSidServo/sidservo.h
View File

@ -49,6 +49,19 @@ extern "C"
#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": 12 degrees
#define MCC_MAX_POINTING_ERR (0.20943951)
// if angle < MCC_MAX_GUIDING_ERR, chane state from "pointing" to "slewing": 15''
#define MCC_MAX_GUIDING_ERR (7.272205e-5)
// 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{
@ -60,6 +73,10 @@ typedef enum{
MCC_E_FAILED, // failed to run command - protocol error
} mcc_errcodes_t;
typedef struct{
double P, I, D;
} PIDpar_t;
typedef struct{
char* MountDevPath; // path to mount device
int MountDevSpeed; // serial speed
@ -70,8 +87,12 @@ typedef struct{
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 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 axes (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
@ -128,16 +149,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;
@ -212,19 +233,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

18
LibSidServo/ssii.c
View File

@ -37,18 +37,18 @@ uint16_t SScalcChecksum(uint8_t *buf, int len){
}
static void axestat(int32_t *prev, int32_t cur, int *nstopped, mnt_status_t *stat){
static void axestat(int32_t *prev, int32_t cur, int *nstopped, axis_status_t *stat){
if(*prev == INT32_MAX){
*stat = MNT_STOPPED;
*stat = AXIS_STOPPED;
DBG("START");
}else if(*stat != MNT_STOPPED){
}else if(*stat != AXIS_STOPPED){
if(*prev == cur && ++(*nstopped) > MOTOR_STOPPED_CNT){
*stat = MNT_STOPPED;
*stat = AXIS_STOPPED;
DBG("AXE stopped");
}
}else if(*prev != cur){
DBG("AXE moving");
//*stat = MNT_SLEWING;
//*stat = AXIS_SLEWING;
*nstopped = 0;
}
*prev = cur;
@ -57,8 +57,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);
axestat(&Xmot_prev, s->Xmot, &Xnstopped, &m->Xstate);
axestat(&Ymot_prev, s->Ymot, &Ynstopped, &m->Ystate);
}
/**
@ -190,14 +190,14 @@ mcc_errcodes_t updateMotorPos(){
}
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.Xstatus == MNT_STOPPED){
if(fabs(md.motXposition.val - md.encXposition.val) > MCC_ENCODERS_ERROR && md.Xstate == AXIS_STOPPED){
DBG("NEED to sync X: motors=%g, axes=%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.Xstatus == MNT_STOPPED){
if(fabs(md.motYposition.val - md.encYposition.val) > MCC_ENCODERS_ERROR && md.Xstate == AXIS_STOPPED){
DBG("NEED to sync Y: motors=%g, axes=%g", md.motYposition.val, md.encYposition.val);
if(!SSsetterI(CMD_MOTYSET, Y_RAD2MOT(md.encYposition.val))){
DBG("Ypos sync failed!");