diff --git a/LibSidServo/libsidservo.creator.user b/LibSidServo/libsidservo.creator.user index dff1f3d..4edd319 100644 --- a/LibSidServo/libsidservo.creator.user +++ b/LibSidServo/libsidservo.creator.user @@ -1,6 +1,6 @@ - + EnvironmentId diff --git a/LibSidServo/main.c b/LibSidServo/main.c index 788ad86..3e8076c 100644 --- a/LibSidServo/main.c +++ b/LibSidServo/main.c @@ -230,20 +230,118 @@ static mcc_errcodes_t longcmd(long_command_t *cmd){ return MCC_E_OK; } -mcc_errcodes_t get_hwconf(hardware_configuration_t *c){ - if(!c) return MCC_E_BADFORMAT; - SSconfig conf; - if(!cmdC(&conf, FALSE)) return MCC_E_FAILED; - // and bored transformations - DBG("Xacc=%u", conf.Xconf.accel); - DBG("Yacc=%u", conf.Yconf.accel); - c->Xconf.accel = X_MOTACC2RS(conf.Xconf.accel); - DBG("cacc: %g", c->Xconf.accel); - c->Xconf.backlash = conf.Xconf.backlash; - // ... - c->Yconf.accel = X_MOTACC2RS(conf.Yconf.accel); - c->Xconf.backlash = conf.Xconf.backlash; - // ... +static mcc_errcodes_t get_hwconf(hardware_configuration_t *hwConfig){ + if(!hwConfig) return MCC_E_BADFORMAT; + SSconfig config; + if(!cmdC(&config, FALSE)) return MCC_E_FAILED; + // Convert acceleration (ticks per loop^2 to rad/s^2) + hwConfig->Xconf.accel = X_MOTACC2RS(config.Xconf.accel); + hwConfig->Yconf.accel = Y_MOTACC2RS(config.Yconf.accel); + // Convert backlash (ticks to radians) + hwConfig->Xconf.backlash = X_MOT2RAD(config.Xconf.backlash); + hwConfig->Yconf.backlash = Y_MOT2RAD(config.Yconf.backlash); + // Convert error limit (ticks to radians) + hwConfig->Xconf.errlimit = X_MOT2RAD(config.Xconf.errlimit); + hwConfig->Yconf.errlimit = Y_MOT2RAD(config.Yconf.errlimit); + // Proportional, integral, and derivative gains are unitless, so no conversion needed + hwConfig->Xconf.propgain = (double)config.Xconf.propgain; + hwConfig->Yconf.propgain = (double)config.Yconf.propgain; + hwConfig->Xconf.intgain = (double)config.Xconf.intgain; + hwConfig->Yconf.intgain = (double)config.Yconf.intgain; + hwConfig->Xconf.derivgain = (double)config.Xconf.derivgain; + hwConfig->Yconf.derivgain = (double)config.Yconf.derivgain; + // Output limit is a percentage (0-100) + hwConfig->Xconf.outplimit = (double)config.Xconf.outplimit / 255.0 * 100.0; + hwConfig->Yconf.outplimit = (double)config.Yconf.outplimit / 255.0 * 100.0; + // Current limit in amps + hwConfig->Xconf.currlimit = (double)config.Xconf.currlimit / 100.0; + hwConfig->Yconf.currlimit = (double)config.Yconf.currlimit / 100.0; + // Integral limit is unitless + hwConfig->Xconf.intlimit = (double)config.Xconf.intlimit; + hwConfig->Yconf.intlimit = (double)config.Yconf.intlimit; + // Copy XBits and YBits (no conversion needed) + hwConfig->xbits = config.xbits; + hwConfig->ybits = config.ybits; + // Copy address + hwConfig->address = config.address; + + // TODO: What to do with eqrate, eqadj and trackgoal? + + // Convert latitude (degrees * 100 to radians) + 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; + // Convert slew rates (ticks per loop to rad/s) + hwConfig->Xslewrate = X_MOTSPD2RS(config.Xslewrate); + hwConfig->Yslewrate = Y_MOTSPD2RS(config.Yslewrate); + // Convert pan rates (ticks per loop to rad/s) + hwConfig->Xpanrate = X_MOTSPD2RS(config.Xpanrate); + hwConfig->Ypanrate = Y_MOTSPD2RS(config.Ypanrate); + // Convert guide rates (ticks per loop to rad/s) + hwConfig->Xguiderate = X_MOTSPD2RS(config.Xguiderate); + hwConfig->Yguiderate = Y_MOTSPD2RS(config.Yguiderate); + // copy baudrate + hwConfig->baudrate = (uint32_t) config.baudrate; + // Convert local search degrees (degrees * 100 to radians) + hwConfig->locsdeg = (double)config.locsdeg / 100.0 * M_PI / 180.0; + // Convert local search speed (arcsec per second to rad/s) + hwConfig->locsspeed = (double)config.locsspeed * M_PI / (180.0 * 3600.0); + // Convert backlash speed (ticks per loop to rad/s) + hwConfig->backlspd = X_MOTSPD2RS(config.backlspd); + return MCC_E_OK; +} + +static mcc_errcodes_t write_hwconf(hardware_configuration_t *hwConfig){ + SSconfig config; + // 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); + // Convert backlash (radians to ticks) + config.Xconf.backlash = X_RAD2MOT(hwConfig->Xconf.backlash); + config.Yconf.backlash = Y_RAD2MOT(hwConfig->Yconf.backlash); + // Convert error limit (radians to ticks) + config.Xconf.errlimit = X_RAD2MOT(hwConfig->Xconf.errlimit); + config.Yconf.errlimit = Y_RAD2MOT(hwConfig->Yconf.errlimit); + // Proportional, integral, and derivative gains are unitless, so no conversion needed + config.Xconf.propgain = (uint16_t)hwConfig->Xconf.propgain; + config.Yconf.propgain = (uint16_t)hwConfig->Yconf.propgain; + config.Xconf.intgain = (uint16_t)hwConfig->Xconf.intgain; + config.Yconf.intgain = (uint16_t)hwConfig->Yconf.intgain; + config.Xconf.derivgain = (uint16_t)hwConfig->Xconf.derivgain; + config.Yconf.derivgain = (uint16_t)hwConfig->Yconf.derivgain; + // Output limit is a percentage (0-100), so convert back to 0-255 + config.Xconf.outplimit = (uint8_t)(hwConfig->Xconf.outplimit / 100.0 * 255.0); + config.Yconf.outplimit = (uint8_t)(hwConfig->Yconf.outplimit / 100.0 * 255.0); + // Current limit is in amps (convert back to *100) + config.Xconf.currlimit = (uint16_t)(hwConfig->Xconf.currlimit * 100.0); + config.Yconf.currlimit = (uint16_t)(hwConfig->Yconf.currlimit * 100.0); + // Integral limit is unitless, so no conversion needed + config.Xconf.intlimit = (uint16_t)hwConfig->Xconf.intlimit; + config.Yconf.intlimit = (uint16_t)hwConfig->Yconf.intlimit; + // Copy XBits and YBits (no conversion needed) + 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); + // Convert slew rates (rad/s to ticks per loop) + config.Xslewrate = X_RS2MOTSPD(hwConfig->Xslewrate); + config.Yslewrate = Y_RS2MOTSPD(hwConfig->Yslewrate); + // Convert pan rates (rad/s to ticks per loop) + config.Xpanrate = X_RS2MOTSPD(hwConfig->Xpanrate); + config.Ypanrate = Y_RS2MOTSPD(hwConfig->Ypanrate); + // Convert guide rates (rad/s to ticks per loop) + config.Xguiderate = X_RS2MOTSPD(hwConfig->Xguiderate); + config.Yguiderate = Y_RS2MOTSPD(hwConfig->Yguiderate); + // Convert local search degrees (radians to degrees * 100) + config.locsdeg = (uint32_t)(hwConfig->locsdeg * 180.0 / M_PI * 100.0); + // Convert local search speed (rad/s to arcsec per second) + 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); + // TODO - next return MCC_E_OK; } @@ -260,4 +358,5 @@ mount_t Mount = { .shortCmd = shortcmd, .longCmd = longcmd, .getHWconfig = get_hwconf, + .saveHWconfig = write_hwconf, }; diff --git a/LibSidServo/sidservo.h b/LibSidServo/sidservo.h index 5f85d50..53bac57 100644 --- a/LibSidServo/sidservo.h +++ b/LibSidServo/sidservo.h @@ -98,10 +98,6 @@ typedef struct{ uint32_t millis; double temperature; double voltage; - int32_t XmotRaw; - int32_t YmotRaw; - int32_t XencRaw; - int32_t YencRaw; } mountdata_t; typedef struct{ @@ -166,7 +162,7 @@ typedef struct{ uint32_t baudrate; // Baud Rate (baud) double locsdeg; // Local Search Degrees (rad) double locsspeed; // Local Search Speed (rad/s) - double backlspd; // Backlash speed (???) + double backlspd; // Backlash speed (rad/s) } hardware_configuration_t; // mount class @@ -182,6 +178,7 @@ typedef struct{ mcc_errcodes_t (*shortCmd)(short_command_t *cmd); // send/get short command mcc_errcodes_t (*longCmd)(long_command_t *cmd); // send/get long command mcc_errcodes_t (*getHWconfig)(hardware_configuration_t *c); // get hardware configuration + mcc_errcodes_t (*saveHWconfig)(hardware_configuration_t *c); // save hardware configuration } mount_t; extern mount_t Mount; diff --git a/LibSidServo/ssii.h b/LibSidServo/ssii.h index 866f78f..181113d 100644 --- a/LibSidServo/ssii.h +++ b/LibSidServo/ssii.h @@ -165,7 +165,8 @@ // X,Y - motor, XZ,YZ - encoder, XC,YC - current*100, V - voltage*10, T - temp (F), XA,YA - mode (A[uto]/M[anual]), K - handpad status bits #define CMD_GETSTATTEXT "\r" - +// Loop freq +#define SITECH_LOOP_FREQUENCY (1953.) // steps per revolution #define X_MOT_STEPSPERREV (3325952.) @@ -176,24 +177,24 @@ #define Y_SPEED_MAX (0.13963) // motor position to radians and back -#define X_MOT2RAD(n) (2.*M_PI * ((double)n) / X_MOT_STEPSPERREV) -#define Y_MOT2RAD(n) (2.*M_PI * ((double)n) / Y_MOT_STEPSPERREV) -#define X_RAD2MOT(r) ((int32_t)((r) / 2./M_PI * X_MOT_STEPSPERREV)) -#define Y_RAD2MOT(r) ((int32_t)((r) / 2./M_PI * Y_MOT_STEPSPERREV)) +#define X_MOT2RAD(n) (2. * M_PI * ((double)(n)) / X_MOT_STEPSPERREV) +#define Y_MOT2RAD(n) (2. * M_PI * ((double)(n)) / Y_MOT_STEPSPERREV) +#define X_RAD2MOT(r) ((int32_t)((r) / (2. * M_PI) * X_MOT_STEPSPERREV)) +#define Y_RAD2MOT(r) ((int32_t)((r) / (2. * M_PI) * Y_MOT_STEPSPERREV)) // motor speed in rad/s and back -#define X_MOTSPD2RS(n) (X_MOT2RAD(n)/65536.*1953.) -#define X_RS2MOTSPD(r) ((int32_t)(X_RAD2MOT(r)*65536./1953.)) -#define Y_MOTSPD2RS(n) (Y_MOT2RAD(n)/65536.*1953.) -#define Y_RS2MOTSPD(r) ((int32_t)(Y_RAD2MOT(r)*65536./1953.)) +#define X_MOTSPD2RS(n) (X_MOT2RAD(n) / 65536. * SITECH_LOOP_FREQUENCY) +#define Y_MOTSPD2RS(n) (Y_MOT2RAD(n) / 65536. * SITECH_LOOP_FREQUENCY) +#define X_RS2MOTSPD(r) ((int32_t)(X_RAD2MOT(r) * 65536. / SITECH_LOOP_FREQUENCY)) +#define Y_RS2MOTSPD(r) ((int32_t)(Y_RAD2MOT(r) * 65536. / SITECH_LOOP_FREQUENCY)) // motor acceleration -//- -#define X_MOTACC2RS(n) (X_MOT2RAD(n)/65536.*1953.*1953.) -#define X_RS2MOTACC(r) ((int32_t)(X_RAD2MOT(r)*65536./1953./1953.)) -#define Y_MOTACC2RS(n) (Y_MOT2RAD(n)/65536.*1953.*1953.) -#define Y_RS2MOTACC(r) ((int32_t)(Y_RAD2MOT(r)*65536./1953./1953.)) +#define X_MOTACC2RS(n) (X_MOT2RAD(n) / 65536. * SITECH_LOOP_FREQUENCY * SITECH_LOOP_FREQUENCY) +#define Y_MOTACC2RS(n) (Y_MOT2RAD(n) / 65536. * SITECH_LOOP_FREQUENCY * SITECH_LOOP_FREQUENCY) +#define X_RS2MOTACC(r) ((int32_t)(X_RAD2MOT(r) * 65536. / SITECH_LOOP_FREQUENCY / SITECH_LOOP_FREQUENCY)) +#define Y_RS2MOTACC(r) ((int32_t)(Y_RAD2MOT(r) * 65536. / SITECH_LOOP_FREQUENCY / SITECH_LOOP_FREQUENCY)) // adder time to seconds vice versa -#define ADDER2S(a) ((a)*1953.) -#define S2ADDER(s) ((s)/1953.) +#define ADDER2S(a) ((a) * SITECH_LOOP_FREQUENCY) +#define S2ADDER(s) ((s) / SITECH_LOOP_FREQUENCY) // encoder per revolution #define X_ENC_STEPSPERREV (67108864.)