stepgen.c

Source code for an Numeric Cmputer

    {3, 6, 12, 9, 0, 0, 0, 0, 0, 0},	/* 6: Unipoler Full Step 2 */
    {1, 7, 14, 8, 0, 0, 0, 0, 0, 0},	/* 7: Bipolar Full Step 1 */
    {5, 6, 10, 9, 0, 0, 0, 0, 0, 0},	/* 8: Bipoler Full Step 2 */
    {1, 3, 2, 6, 4, 12, 8, 9, 0, 0},	/* 9: Unipolar Half Step */
    {1, 5, 7, 6, 14, 10, 8, 9, 0, 0},	/* 10: Bipolar Half Step */
    {1, 2, 4, 8, 16, 0, 0, 0, 0, 0},	/* 11: Five Wire Unipolar */
    {3, 6, 12, 24, 17, 0, 0, 0, 0, 0},	/* 12: Five Wire Wave */
    {1, 3, 2, 6, 4, 12, 8, 24, 16, 17},	/* 13: Five Wire Uni Half */
    {3, 7, 6, 14, 12, 28, 24, 25, 17, 19}	/* 14: Five Wire Wave Half */
};

static const unsigned char cycle_len_lut[] =
    { 4, 3, 6, 4, 4, 4, 4, 8, 8, 5, 5, 10, 10 };

static const unsigned char num_phases_lut[] =
    { 2, 3, 3, 4, 4, 4, 4, 4, 4, 5, 5, 5, 5, };

#define MAX_STEP_TYPE 14

#define STEP_PIN	0	/* output phase used for STEP signal */
#define DIR_PIN		1	/* output phase used for DIR signal */
#define UP_PIN		0	/* output phase used for UP signal */
#define DOWN_PIN	1	/* output phase used for DOWN signal */

/* other globals */
static int comp_id;		/* component ID */
static int num_chan = 0;	/* number of step generators configured */
static long periodns;		/* makepulses function period in nanosec */
static long old_periodns;	/* used to detect changes in periodns */
static float periodfp;		/* makepulses function period in seconds */
static float maxf;		/* maximum frequency, step types 1 & up */
static float freqscale;		/* conv. factor from Hz to addval counts */
static float accelscale;	/* conv. Hz/sec to addval cnts/period */
static long old_dtns;		/* update_freq funct period in nsec */
static float dt;		/* update_freq period in seconds */
static float recip_dt;		/* recprocal of period, avoids divides */

/***********************************************************************
*                  LOCAL FUNCTION DECLARATIONS                         *
************************************************************************/

static int export_stepgen(int num, stepgen_t * addr, int steptype);
static void make_pulses(void *arg, long period);
static void update_freq(void *arg, long period);
static void update_pos(void *arg, long period);

/***********************************************************************
*                       INIT AND EXIT CODE                             *
************************************************************************/

int rtapi_app_main(void)
{
    int n, retval;

    for (n = 0; n < MAX_CHAN && step_type[n] != -1 ; n++) {
	if ((step_type[n] > MAX_STEP_TYPE) || (step_type[n] < 0)) {
	    rtapi_print_msg(RTAPI_MSG_ERR,
			    "STEPGEN: ERROR: bad stepping type '%i', axes %i\n",
			    step_type[n], n);
	    return -1;
	} else {
	    num_chan++;
	}
    }
    if (num_chan == 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
			"STEPGEN: ERROR: no channels configured\n");
	return -1;
    }
    /* test for 'period' */
    if (period != 0) {
	if ((period < 1000) || (period > 500000)) {
	    rtapi_print_msg(RTAPI_MSG_ERR,
			    "STEPGEN: ERROR: invalid period: %d\n",
			    period);
	    return -1;
	}
    }
    /* test for 'fp_period' */
    if (fp_period != 0) {
	if ((fp_period < (2 * period)) || (period > 50000000)) {
	    rtapi_print_msg(RTAPI_MSG_ERR,
			    "STEPGEN: ERROR: invalid fp_period: %d\n",
			    period);
	    return -1;
	}
    }
    /* periodns will be set to the proper value when 'make_pulses()' runs for 
       the first time.  We load a default value here to avoid glitches at
       startup, but all these 'constants' are recomputed inside
       'update_freq()' using the real period. */
    old_periodns = periodns = 50000;
    old_dtns = 1000000;
    /* precompute some constants */
    periodfp = periodns * 0.000000001;
    maxf = 1.0 / periodfp;
    freqscale = ((1L << 30) * 2.0) / maxf;
    accelscale = freqscale * periodfp;
    dt = old_dtns * 0.000000001;
    recip_dt = 1.0 / dt;
    /* have good config info, connect to the HAL */
    comp_id = hal_init("stepgen");
    if (comp_id < 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
			"STEPGEN: ERROR: hal_init() failed\n");
	return -1;
    }
    /* allocate shared memory for counter data */
    stepgen_array = hal_malloc(num_chan * sizeof(stepgen_t));
    if (stepgen_array == 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
			"STEPGEN: ERROR: hal_malloc() failed\n");
	hal_exit(comp_id);
	return -1;
    }
    /* export all the variables for each pulse generator */
    for (n = 0; n < num_chan; n++) {
	/* export all vars */
	retval = export_stepgen(n, &(stepgen_array[n]), step_type[n]);
	if (retval != 0) {
	    rtapi_print_msg(RTAPI_MSG_ERR,
			    "STEPGEN: ERROR: stepgen %d var export failed\n",
			    n);
	    hal_exit(comp_id);
	    return -1;
	}
    }
    /* export functions */
    retval = hal_export_funct("stepgen.make-pulses", make_pulses,
			      stepgen_array, 0, 0, comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
			"STEPGEN: ERROR: makepulses funct export failed\n");
	hal_exit(comp_id);
	return -1;
    }
    retval = hal_export_funct("stepgen.update-freq", update_freq,
			      stepgen_array, 1, 0, comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
			"STEPGEN: ERROR: freq update funct export failed\n");
	hal_exit(comp_id);
	return -1;
    }
    retval = hal_export_funct("stepgen.capture-position", update_pos,
			      stepgen_array, 1, 0, comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
			"STEPGEN: ERROR: pos update funct export failed\n");
	hal_exit(comp_id);
	return -1;
    }
    rtapi_print_msg(RTAPI_MSG_INFO,
		    "STEPGEN: installed %d step pulse generators\n",
		    num_chan);
    if (period > 0) {
	/* create a thread */
	retval = hal_create_thread("stepgen.thread", period, 0);
	if (retval < 0) {
	    rtapi_print_msg(RTAPI_MSG_ERR,
			    "STEPGEN: ERROR: could not create non-FP thread\n");
	    hal_exit(comp_id);
	    return -1;
	} else {
	    rtapi_print_msg(RTAPI_MSG_INFO,
			    "STEPGEN: created %d uS non-FP thread\n",
			    period / 1000);
	}
    }
    if (fp_period > 0) {
	/* create a floating point thread */
	retval = hal_create_thread("stepgen.threadFP", fp_period, 1);
	if (retval < 0) {
	    rtapi_print_msg(RTAPI_MSG_ERR,
			    "STEPGEN: ERROR: could not create FP thread\n");
	    hal_exit(comp_id);
	    return -1;
	} else {
	    rtapi_print_msg(RTAPI_MSG_INFO,
			    "STEPGEN: created %d uS FP thread\n",
			    fp_period / 1000);
	}
    }
    return 0;
}

void rtapi_app_exit(void)
{
    hal_exit(comp_id);
}

/***********************************************************************
*              REALTIME STEP PULSE GENERATION FUNCTIONS                *
************************************************************************/

/** The frequency generator works by adding a signed value proportional
    to frequency to an accumulator.  When the accumulator overflows (or
    underflows), it is time to generate an up (or down) output pulse.
    The add value is limited to +/-2^30, and overflows are detected
    at bit 30, not bit 31.  This means that with add_val at it's max
    (or min) value, an overflow (or underflow) occurs on every cycle.
    NOTE:  step/dir outputs cannot generate a step every cycle.  The
    maximum steprate is determined by the timing parameters.  The
    time between pulses must be at least (StepLen + StepSpace) cycles,
    and the time between direction changes must be at least (DirSetup +
    StepLen + DirHold) cycles.

*/

static void make_pulses(void *arg, long period)
{
    stepgen_t *stepgen;
    int n, p;
    unsigned char tmp_step, tmp_dir, state;

    /* store period so scaling constants can be (re)calculated */
    periodns = period;
    /* point to stepgen data structures */
    stepgen = arg;

    for (n = 0; n < num_chan; n++) {

	/* bail out if not enabled */
	if (*stepgen->enable == 0) {
	    stepgen++;
	    break;
	}

	if (stepgen->deltalim != 0) {
	    /* implement accel/decel limit */
	    if (stepgen->newaddval > (stepgen->addval + stepgen->deltalim)) {
		/* new value is too high, increase addval as far as possible */
		stepgen->addval += stepgen->deltalim;
	    } else if (stepgen->newaddval <
		       (stepgen->addval - stepgen->deltalim)) {
		/* new value is too low, decrease addval as far as possible */
		stepgen->addval -= stepgen->deltalim;
	    } else {
		/* new value can be reached in one step - do it */
		stepgen->addval = stepgen->newaddval;
	    }
	} else {
	    /* go to new freq without any ramping */
	    stepgen->addval = stepgen->newaddval;
	}
	/* get current value of bit 31 */
	tmp_step = stepgen->accum >> 31;
	/* update the accumulator */
	stepgen->accum += stepgen->addval;
	/* test for overflow/underflow (change in bit 31) */
	tmp_step ^= stepgen->accum >> 31;
	/* get direction bit, 1 if negative, 0 if positive */
	tmp_dir = stepgen->addval >> 31;

	/* generate output, based on stepping type */
	if (stepgen->wd.st0.step_type == 0) {
	    /* step/dir output, using working data in stepgen->wd.st0.xxx */
	    stepgen->wd.st0.need_step |= tmp_step;
	    /* update timers */
	    if (stepgen->wd.st0.setup_timer) {
		stepgen->wd.st0.setup_timer--;
	    }
	    if (stepgen->wd.st0.hold_timer) {
		stepgen->wd.st0.hold_timer--;
	    }
	    if (stepgen->wd.st0.space_timer) {
		stepgen->wd.st0.space_timer--;
	    }
	    if (stepgen->wd.st0.len_timer) {
		if (--stepgen->wd.st0.len_timer == 0) {
		    /* terminate step pulse */
		    *(stepgen->phase[STEP_PIN]) = 0;
		    /* begin timing pulse space */
		    stepgen->wd.st0.space_timer =
			stepgen->wd.st0.step_space;
		}
	    }
	    /* is direction OK? */
	    if (tmp_dir != *(stepgen->phase[DIR_PIN])) {
		/* need to change direction bit, can we? */
		if ((stepgen->wd.st0.hold_timer == 0) &&
		    (stepgen->wd.st0.setup_timer == 0)) {
		    /* set direction output */
		    *(stepgen->phase[DIR_PIN]) = tmp_dir;
		    /* start setup timer */
		    stepgen->wd.st0.setup_timer =
			stepgen->wd.st0.dir_setup;
		}
	    }
	    /* do we need to step? */
	    if (stepgen->wd.st0.need_step) {
		/* yes, can we? */
		if ((stepgen->wd.st0.space_timer == 0) &&
		    (stepgen->wd.st0.setup_timer == 0) &&
		    (*(stepgen->phase[DIR_PIN]) == tmp_dir)) {
		    /* yes, start step pulse */
		    *(stepgen->phase[STEP_PIN]) = 1;
		    /* start pulse length and dir hold timers */
		    stepgen->wd.st0.len_timer = stepgen->wd.st0.step_len;
		    stepgen->wd.st0.hold_timer =
			stepgen->wd.st0.step_len +
			stepgen->wd.st0.dir_hold;
		    /* don't need a step any more */
		    stepgen->wd.st0.need_step = 0;
		    /* count the step */
		    if (tmp_dir) {
			stepgen->rawcount--;
		    } else {
			stepgen->rawcount++;
		    }
		}
	    }
	} else if (stepgen->wd.st0.step_type == 1) {
	    /* pesudo-PWM */
	    *(stepgen->phase[UP_PIN]) = tmp_step & ~tmp_dir;
	    *(stepgen->phase[DOWN_PIN]) = tmp_step & tmp_dir;
	    /* count the step for feedback */
	    if (tmp_step) {
		if (tmp_dir) {
		    stepgen->rawcount--;
		} else {
		    stepgen->rawcount++;
		}
	    }
	} else {
	    /* step type 2 or greater */
	    /* update step cycle counter */
	    if (tmp_step) {
		if (tmp_dir) {
		    if (stepgen->wd.st2.state-- == 0) {
			stepgen->wd.st2.state = stepgen->wd.st2.cycle_max;
		    }
		    /* count the step for feedback */
		    stepgen->rawcount--;
		} else {
		    if (++stepgen->wd.st2.state >
			stepgen->wd.st2.cycle_max) {
			stepgen->wd.st2.state = 0;
		    }
		    /* count the step for feedback */
		    stepgen->rawcount++;
		}
		/* look up correct output pattern */
		state = (stepgen->wd.st2.lut)[stepgen->wd.st2.state];
		/* now output the phase bits */
		for (p = 0; p < stepgen->wd.st2.num_phases; p++) {
		    /* output one phase */
		    *(stepgen->phase[p]) = state & 1;
		    /* move to the next phase */
		    state >>= 1;
		}
	    }
	}
	/* move on to next step generator */
	stepgen++;
    }
    /* done */
}

static void update_freq(void *arg, long period)
{
    stepgen_t *stepgen;
    int n;
    double pos_cmd, vel_cmd, curr_pos, curr_vel, avg_v, max_freq, max_ac;
    double match_ac, match_time, est_out, est_cmd, est_err, dp, dv,
	new_vel;

    /*! \todo FIXME - while this code works just fine, there are a bunch of
       internal variables, many of which hold intermediate results that
       don't really need their own variables.  They are used either for
       clarity, or because that's how the code evolved.  This algorithm
       could use some cleanup and optimization. */
    /* this periodns stuff is a little convoluted because we need to
       calculate some constants here in this relatively slow thread but the
       constants are based on the period of the much faster 'make_pulses()'
       thread. */
    /* only recalc constants if period changes */
    if (periodns != old_periodns) {
	/* get ready to detect future period changes */
	old_periodns = periodns;
	/* recompute various constants that depend on periodns */
	periodfp = periodns * 0.000000001;
	maxf = 1.0 / periodfp;