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

update pos, statuses

Browse Source
This commit is contained in:
Edward V. Emelianov
2025-07-29 22:11:42 +03:00
parent 89b86f8b7f
commit c862d160fe
11 changed files with 229 additions and 71 deletions
Download Patch File Download Diff File Expand all files Collapse all files

54
LibSidServo/main.c
View File

@@ -77,25 +77,7 @@ static mcc_errcodes_t init(conf_t *c){
// read input data as there may be some trash on start
if(!SSrawcmd(CMD_EXITACM, &d)) ret = MCC_E_FAILED;
if(ret != MCC_E_OK) return ret;
mountdata_t md = {0};
ret = MCC_E_FATAL;
double t0 = dtime(), t = 0.;
do{
t = dtime();
if(MCC_E_OK == getMD(&md)){
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");
ret = MCC_E_OK;
break;
}
}
}
DBG("NO DATA; dt = %g", t - t0);
}while(t - t0 < 2.);
return ret;
return updateMotorPos();
}
// check coordinates (rad) and speeds (rad/s); return FALSE if failed
@@ -120,6 +102,10 @@ static int chkYs(double s){
static mcc_errcodes_t slew2(const coordpair_t *target, slewflags_t flags){
(void)target;
(void)flags;
if(MCC_E_OK != updateMotorPos()) return MCC_E_FAILED;
//...
setStat(MNT_SLEWING, MNT_SLEWING);
//...
return MCC_E_FAILED;
}
@@ -133,13 +119,17 @@ 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(MCC_E_OK != updateMotorPos()) return MCC_E_FAILED;
short_command_t cmd = {0};
DBG("x,y: %g, %g", target->X, target->Y);
cmd.Xmot = target->X;
cmd.Ymot = target->Y;
cmd.Xspeed = MCC_MAX_X_SPEED;
cmd.Yspeed = MCC_MAX_Y_SPEED;
return shortcmd(&cmd);
mcc_errcodes_t r = shortcmd(&cmd);
if(r != MCC_E_OK) return r;
setStat(MNT_SLEWING, MNT_SLEWING);
return MCC_E_OK;
}
/**
@@ -167,12 +157,16 @@ 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(MCC_E_OK != updateMotorPos()) return MCC_E_FAILED;
short_command_t cmd = {0};
cmd.Xmot = target->X;
cmd.Ymot = target->Y;
cmd.Xspeed = speed->X;
cmd.Yspeed = speed->Y;
return shortcmd(&cmd);
mcc_errcodes_t r = shortcmd(&cmd);
if(r != MCC_E_OK) return r;
setStat(MNT_SLEWING, MNT_SLEWING);
return MCC_E_OK;
}
/**
@@ -266,14 +260,14 @@ static mcc_errcodes_t get_hwconf(hardware_configuration_t *hwConfig){
hwConfig->address = config.address;
// TODO: What to do with eqrate, eqadj and trackgoal?
config.latitude = __bswap_16(config.latitude);
// Convert latitude (degrees * 100 to radians)
hwConfig->latitude = (double)config.latitude / 100.0 * M_PI / 180.0;
hwConfig->latitude = ((double)config.latitude) / 100.0 * M_PI / 180.0;
// Copy ticks per revolution
hwConfig->Xsetpr = config.Xsetpr;
hwConfig->Ysetpr = config.Ysetpr;
hwConfig->Xmetpr = config.Xmetpr;
hwConfig->Ymetpr = config.Ymetpr;
hwConfig->Xsetpr = __bswap_32(config.Xsetpr);
hwConfig->Ysetpr = __bswap_32(config.Ysetpr);
hwConfig->Xmetpr = __bswap_32(config.Xmetpr);
hwConfig->Ymetpr = __bswap_32(config.Ymetpr);
// Convert slew rates (ticks per loop to rad/s)
hwConfig->Xslewrate = X_MOTSPD2RS(config.Xslewrate);
hwConfig->Yslewrate = Y_MOTSPD2RS(config.Yslewrate);
@@ -325,7 +319,7 @@ static mcc_errcodes_t write_hwconf(hardware_configuration_t *hwConfig){
config.xbits = hwConfig->xbits;
config.ybits = hwConfig->ybits;
// Convert latitude (radians to degrees * 100)
config.latitude = (uint16_t)(hwConfig->latitude * 180.0 / M_PI * 100.0);
config.latitude = __bswap_16((uint16_t)(hwConfig->latitude * 180.0 / M_PI * 100.0));
// Convert slew rates (rad/s to ticks per loop)
config.Xslewrate = X_RS2MOTSPD(hwConfig->Xslewrate);
config.Yslewrate = Y_RS2MOTSPD(hwConfig->Yslewrate);
@@ -341,6 +335,10 @@ static mcc_errcodes_t write_hwconf(hardware_configuration_t *hwConfig){
config.locsspeed = (uint32_t)(hwConfig->locsspeed * 180.0 * 3600.0 / M_PI);
// Convert backlash speed (rad/s to ticks per loop)
config.backlspd = X_RS2MOTSPD(hwConfig->backlspd);
config.Xsetpr = __bswap_32(hwConfig->Xsetpr);
config.Ysetpr = __bswap_32(hwConfig->Ysetpr);
config.Xmetpr = __bswap_32(hwConfig->Xmetpr);
config.Ymetpr = __bswap_32(hwConfig->Ymetpr);
// TODO - next
(void) config;
return MCC_E_OK;