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

introduce a lot of errors when trying to apply model

Browse Source
This commit is contained in:
Edward V. Emelianov
2025-07-30 16:45:42 +03:00
parent 502014bee4
commit 04ee999159
11 changed files with 627 additions and 113 deletions
Download Patch File Download Diff File Expand all files Collapse all files

108
LibSidServo/main.c
View File

@@ -16,28 +16,82 @@
* 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"
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-8, .accel = 1e-6},
.max = {.coord = 3.1241, .speed = MCC_MAX_X_SPEED, .accel = MCC_X_ACCELERATION}},
Ylimits = {
.min = {.coord = -3.1241, .speed = 1e-8, .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;
double tnow = nanotime();
moveparam_t Xp, Yp;
movestate_t Xst = Xmodel->get_state(&Xp);
if(Xst == ST_MOVE) Xst = Xmodel->proc_move(&Xp, tnow);
movestate_t Yst = Ymodel->get_state(&Yp);
if(Yst == ST_MOVE) Yst = Ymodel->proc_move(&Yp, tnow);
bzero(mountdata, sizeof(mountdata_t));
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;
mountdata->encXspeed.t = mountdata->encYspeed.t = tnow;
mountdata->encXspeed.val = Xp.speed;
mountdata->encYspeed.val = Yp.speed;
mountdata->millis = (uint32_t)(tnow * 1e3);
}
/**
* @brief init - open serial devices and do other job
* @param c - initial configuration
@@ -48,6 +102,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) 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;
@@ -102,6 +162,7 @@ static int chkYs(double s){
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);
@@ -119,6 +180,14 @@ static mcc_errcodes_t slew2(const coordpair_t *target, slewflags_t flags){
static mcc_errcodes_t move2(const coordpair_t *target){
if(!target) return MCC_E_BADFORMAT;
if(!chkX(target->X) || !chkY(target->Y)) return MCC_E_BADFORMAT;
if(Conf.RunModel){
double curt = nanotime();
moveparam_t param = {0};
param.coord = target->X; param.speed = MCC_MAX_X_SPEED;
if(!model_move2(Xmodel, &param, curt)) return MCC_E_FAILED;
param.coord = target->Y; param.speed = MCC_MAX_Y_SPEED;
if(!model_move2(Ymodel, &param, curt)) return MCC_E_FAILED;
}
if(MCC_E_OK != updateMotorPos()) return MCC_E_FAILED;
short_command_t cmd = {0};
DBG("x,y: %g, %g", target->X, target->Y);
@@ -140,6 +209,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);
@@ -157,6 +227,14 @@ 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;
@@ -174,11 +252,23 @@ static mcc_errcodes_t move2s(const coordpair_t *target, const coordpair_t *speed
* @return errcode
*/
static mcc_errcodes_t emstop(){
if(Conf.RunModel){
double curt = nanotime();
Xmodel->emergency_stop(curt);
Ymodel->emergency_stop(curt);
return MCC_E_OK;
}
if(!SSstop(TRUE)) return MCC_E_FAILED;
return MCC_E_OK;
}
// normal stop
static mcc_errcodes_t stop(){
if(Conf.RunModel){
double curt = nanotime();
Xmodel->stop(curt);
Ymodel->stop(curt);
return MCC_E_OK;
}
if(!SSstop(FALSE)) return MCC_E_FAILED;
return MCC_E_OK;
}
@@ -190,6 +280,7 @@ static mcc_errcodes_t stop(){
*/
static mcc_errcodes_t shortcmd(short_command_t *cmd){
if(!cmd) return MCC_E_BADFORMAT;
if(Conf.RunModel) return MCC_E_FAILED;
SSscmd s = {0};
DBG("tag: xmot=%g rad, ymot=%g rad", cmd->Xmot, cmd->Ymot);
s.Xmot = X_RAD2MOT(cmd->Xmot);
@@ -205,12 +296,13 @@ static mcc_errcodes_t shortcmd(short_command_t *cmd){
}
/**
* @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) return MCC_E_FAILED;
SSlcmd l = {0};
l.Xmot = X_RAD2MOT(cmd->Xmot);
l.Ymot = Y_RAD2MOT(cmd->Ymot);
@@ -226,6 +318,7 @@ static mcc_errcodes_t longcmd(long_command_t *cmd){
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 +359,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 +383,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);
@@ -359,5 +453,5 @@ mount_t Mount = {
.longCmd = longcmd,
.getHWconfig = get_hwconf,
.saveHWconfig = write_hwconf,
.currentT = dtime,
.currentT = nanotime,
};