freqgen.c

CNC 的开放码,EMC2 V2.2.8版

    hal_u32_t dir_hold;		/* parameter: direction hold time */
    hal_u32_t step_len;		/* parameter: step pulse length */
    hal_u32_t step_space;	/* parameter: min step pulse spacing */
} st0_t;

typedef struct {
    unsigned char step_type;	/* stepping type - see list above */
    unsigned char state;	/* current position in state table */
    unsigned char cycle_max;	/* cycle length for step types 2 and up */
    unsigned char num_phases;	/* number of phases for types 2 and up */
    const unsigned char *lut;		/* pointer to lookup table */
} st2_t;

typedef struct {
    signed long deltalim;	/* max allowed change per period */
    signed long newaddval;	/* desired freq generator add value */
    signed long addval;		/* actual frequency generator add value */
    unsigned long accum;	/* frequency generator accumulator */
    union {
	st0_t st0;		/* working data for step type 0 */
	st2_t st2;		/* working data for step types 2 and up */
    } wd;
    hal_bit_t *phase[5];	/* pins for output signals */
    hal_s32_t rawcount;		/* param: current position (feedback) */
    hal_s32_t *count;		/* captured binary count value */
    hal_float_t pos_scale;	/* parameter: scaling factor for pos */
    float old_scale;		/* stored scale value */
    double scale_recip;		/* reciprocal value used for scaling */
    hal_float_t *pos;		/* scaled position (floating point) */
    hal_float_t *vel;		/* velocity command */
    hal_float_t vel_scale;	/* parameter: scaling for vel to Hz */
    hal_float_t maxfreq;	/* parameter: max frequency in Hz */
    hal_float_t freq;		/* parameter: velocity cmd scaled to Hz */
    hal_float_t maxaccel;	/* parameter: max accel rate in Hz/sec */
    int printed_error;          /* Has the error message been logged? */
} freqgen_t;

/* ptr to array of freqgen_t structs in shared memory, 1 per channel */
static freqgen_t *freqgen_array;

/* lookup tables for stepping types 2 and higher - phase A is the LSB */

static const unsigned char master_lut[][10] = {
    {1, 3, 2, 0, 0, 0, 0, 0, 0, 0},	/* Quadrature */
    {1, 2, 4, 0, 0, 0, 0, 0, 0, 0},	/* Three Wire */
    {1, 3, 2, 6, 4, 5, 0, 0, 0, 0},	/* Three Wire Half Step */
    {1, 2, 4, 8, 0, 0, 0, 0, 0, 0},	/* Unipolar Full Step 1 */
    {3, 6, 12, 9, 0, 0, 0, 0, 0, 0},	/* Unipoler Full Step 2 */
    {1, 7, 14, 8, 0, 0, 0, 0, 0, 0},	/* Bipolar Full Step 1 */
    {5, 6, 10, 9, 0, 0, 0, 0, 0, 0},	/* Bipoler Full Step 2 */
    {1, 3, 2, 6, 4, 12, 8, 9, 0, 0},	/* Unipolar Half Step */
    {1, 5, 7, 6, 14, 10, 8, 9, 0, 0},	/* Bipolar Half Step */
    {1, 2, 4, 8, 16, 0, 0, 0, 0, 0},	/* Five Wire Unipolar */
    {3, 6, 12, 24, 17, 0, 0, 0, 0, 0},	/* Five Wire Wave */
    {1, 3, 2, 6, 4, 12, 8, 24, 16, 17},	/* Five Wire Uni Half */
    {3, 7, 6, 14, 12, 28, 24, 25, 17, 19}	/* 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 12

#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 */
#define COUNT_PIN	2	/* output phase used for COUNT signal */

/* other globals */
static int comp_id;		/* component ID */
static int num_chan;		/* 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 */

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

static int export_freqgen(int num, freqgen_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,
			    "FREQGEN: ERROR: bad stepping type '%i', axis %i\n",
			    step_type[n], n);
	    return -1;
	} else {
	    num_chan++;
	}
    }
    if (num_chan == 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "FREQGEN: ERROR: no channels configured\n");
	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. */
    periodns = 50000;
    /* precompute some constants */
    periodfp = periodns * 0.000000001;
    maxf = 1.0 / periodfp;
    freqscale = ((1L << 30) * 2.0) / maxf;
    accelscale = freqscale * periodfp;
    old_periodns = periodns;
    /* have good config info, connect to the HAL */
    comp_id = hal_init("freqgen");
    if (comp_id < 0) {
	rtapi_print_msg(RTAPI_MSG_ERR, "FREQGEN: ERROR: hal_init() failed\n");
	return -1;
    }
    /* allocate shared memory for counter data */
    freqgen_array = hal_malloc(num_chan * sizeof(freqgen_t));
    if (freqgen_array == 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "FREQGEN: 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_freqgen(n, &(freqgen_array[n]), step_type[n]);
	if (retval != 0) {
	    rtapi_print_msg(RTAPI_MSG_ERR,
		"FREQGEN: ERROR: freqgen %d var export failed\n", n);
	    hal_exit(comp_id);
	    return -1;
	}
    }
    /* export functions */
    retval = hal_export_funct("freqgen.make-pulses", make_pulses,
	freqgen_array, 0, 0, comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "FREQGEN: ERROR: makepulses funct export failed\n");
	hal_exit(comp_id);
	return -1;
    }
    retval = hal_export_funct("freqgen.update-freq", update_freq,
	freqgen_array, 1, 0, comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "FREQGEN: ERROR: freq update funct export failed\n");
	hal_exit(comp_id);
	return -1;
    }
    retval = hal_export_funct("freqgen.capture-position", update_pos,
	freqgen_array, 1, 0, comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "FREQGEN: ERROR: pos update funct export failed\n");
	hal_exit(comp_id);
	return -1;
    }
    rtapi_print_msg(RTAPI_MSG_INFO,
	"FREQGEN: installed %d step pulse generators\n", num_chan);
    hal_ready(comp_id);
    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 its max
    (or min) value, and 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)
{
    freqgen_t *freqgen;
    int n, p;
    unsigned char tmp_step, tmp_dir, state;

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

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