stepgen.c

CNC 的开放码,EMC2 V2.2.8版

    { 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 */

#define PICKOFF		28	/* bit location in DDS accum */



/* 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 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 step_type, int pos_mode);
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', axis %i\n",
			    step_type[n], n);
	    return -1;
	}
	if ((ctrl_type[n][0] == 'p' ) || (ctrl_type[n][0] == 'P')) {
	    ctrl_type[n] = "p";
	} else if ((ctrl_type[n][0] == 'v' ) || (ctrl_type[n][0] == 'V')) {
	    ctrl_type[n] = "v";
	} else {
	    rtapi_print_msg(RTAPI_MSG_ERR,
			    "STEPGEN: ERROR: bad control type '%s' for axis %i (must be 'p' or 'v')\n",
			    ctrl_type[n], n);
	    return -1;
	}
	num_chan++;
    }
    if (num_chan == 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
			"STEPGEN: 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. */
    old_periodns = periodns = 50000;
    old_dtns = 1000000;
    /* precompute some constants */
    periodfp = periodns * 0.000000001;
    freqscale = (1L << PICKOFF) * periodfp;
    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], (ctrl_type[n][0] == 'p'));
	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);
    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 bit PICKOFF of the accumulator
    toggles, a step is generated.
*/

static void make_pulses(void *arg, long period)
{
    stepgen_t *stepgen;
    long old_addval, target_addval, new_addval, step_now;
    int n, p;
    unsigned char outbits;

    /* 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++) {
	/* decrement "timing constraint" timers */
	if ( stepgen->timer1 > 0 ) {
	    if ( stepgen->timer1 > periodns ) {
		stepgen->timer1 -= periodns;
	    } else {
		stepgen->timer1 = 0;
	    }
	}
	if ( stepgen->timer2 > 0 ) {
	    if ( stepgen->timer2 > periodns ) {
		stepgen->timer2 -= periodns;
	    } else {
		stepgen->timer2 = 0;
	    }
	}
	if ( stepgen->timer3 > 0 ) {
	    if ( stepgen->timer3 > periodns ) {
		stepgen->timer3 -= periodns;
	    } else {
		stepgen->timer3 = 0;
		/* last timer timed out, cancel hold */
		stepgen->hold_dds = 0;
	    }
	}
	if ( !stepgen->hold_dds && *(stepgen->enable) ) {
	    /* update addval (ramping) */
	    old_addval = stepgen->addval;
	    target_addval = stepgen->target_addval;
	    if (stepgen->deltalim != 0) {
		/* implement accel/decel limit */
		if (target_addval > (old_addval + stepgen->deltalim)) {
		    /* new value is too high, increase addval as far as possible */
		    new_addval = old_addval + stepgen->deltalim;
		} else if (target_addval < (old_addval - stepgen->deltalim)) {
		    /* new value is too low, decrease addval as far as possible */
		    new_addval = old_addval - stepgen->deltalim;
		} else {
		    /* new value can be reached in one step - do it */
		    new_addval = target_addval;
		}
	    } else {
		/* go to new freq without any ramping */
		new_addval = target_addval;
	    }
	    /* save result */
	    stepgen->addval = new_addval;
	    /* check for direction reversal */
	    if (((new_addval >= 0) && (old_addval < 0)) ||
		((new_addval < 0) && (old_addval >= 0))) {
		/* reversal required, can we do so now? */
		if ( stepgen->timer3 != 0 ) {
		    /* no - hold everything until delays time out */
		    stepgen->hold_dds = 1;
		}
	    }
	}
	/* update DDS */
	if ( !stepgen->hold_dds && *(stepgen->enable) ) {
	    /* save current value of low half of accum */
	    step_now = stepgen->accum;
	    /* update the accumulator */
	    stepgen->accum += stepgen->addval;
	    /* test for changes in low half of accum */
	    step_now ^= stepgen->accum;
	    /* we only care about the pickoff bit */
	    step_now &= (1L << PICKOFF);
	    /* update rawcounts parameter */
	    stepgen->rawcount = stepgen->accum >> PICKOFF;
	} else {
	    /* DDS is in hold, no steps */
	    step_now = 0;
	}
	if ( stepgen->timer2 == 0 ) {
	    /* update direction - do not change if addval = 0 */
	    if ( stepgen->addval > 0 ) {
		stepgen->curr_dir = 1;
	    } else if ( stepgen->addval < 0 ) {
		stepgen->curr_dir = -1;
	    }
	}
	if ( step_now ) {
	    /* (re)start various timers */
	    /* timer 1 = time till end of step pulse */
	    stepgen->timer1 = stepgen->step_len;
	    /* timer 2 = time till allowed to change dir pin */
	    stepgen->timer2 = stepgen->timer1 + stepgen->dir_hold_dly;
	    /* timer 3 = time till allowed to step the other way */
	    stepgen->timer3 = stepgen->timer2 + stepgen->dir_setup;
	    if ( stepgen->step_type >= 2 ) {
		/* update state */
		stepgen->state += stepgen->curr_dir;
		if ( stepgen->state < 0 ) {
		    stepgen->state = stepgen->cycle_max;
		} else if ( stepgen->state > stepgen->cycle_max ) {
		    stepgen->state = 0;
		}
	    }
	}
	/* generate output, based on stepping type */
	if (stepgen->step_type == 0) {
	    /* step/dir output */
	    if ( stepgen->timer1 != 0 ) {
		 *(stepgen->phase[STEP_PIN]) = 1;
	    } else {
		 *(stepgen->phase[STEP_PIN]) = 0;
	    }
	    if ( stepgen->curr_dir < 0 ) {
		 *(stepgen->phase[DIR_PIN]) = 1;
	    } else {
		 *(stepgen->phase[DIR_PIN]) = 0;
	    }
	} else if (stepgen->step_type == 1) {
	    /* up/down */
	    if ( stepgen->timer1 != 0 ) {
		if ( stepgen->curr_dir < 0 ) {
		    *(stepgen->phase[UP_PIN]) = 0;
		    *(stepgen->phase[DOWN_PIN]) = 1;
		} else {
		    *(stepgen->phase[UP_PIN]) = 1;
		    *(stepgen->phase[DOWN_PIN]) = 0;
		}
	    } else {
		*(stepgen->phase[UP_PIN]) = 0;
		*(stepgen->phase[DOWN_PIN]) = 0;
	    }
	} else {
	    /* step type 2 or greater */
	    /* look up correct output pattern */
	    outbits = (stepgen->lut)[stepgen->state];
	    /* now output the phase bits */
	    for (p = 0; p < stepgen->num_phases; p++) {
		/* output one phase */
		*(stepgen->phase[p]) = outbits & 1;
		/* move to the next phase */
		outbits >>= 1;
	    }
	}
	/* move on to next step generator */
	stepgen++;
    }
    /* done */
}

static void update_pos(void *arg, long period)
{
    long long int accum_a, accum_b;
    stepgen_t *stepgen;
    int n;

    stepgen = arg;

    for (n = 0; n < num_chan; n++) {
	/* 'accum' is a long long, and its remotely possible that
	   make_pulses could change it half-way through a read.
	   So we have a crude atomic read routine */
	do {
	    accum_a = stepgen->accum;
	    accum_b = stepgen->accum;
	} while ( accum_a != accum_b );
	/* compute integer counts */
	*(stepgen->count) = accum_a >> PICKOFF;
	/* check for change in scale value */
	if (stepgen->pos_scale != stepgen->old_scale) {
	    /* get ready to detect future scale changes */
	    stepgen->old_scale = stepgen->pos_scale;
	    /* validate the new scale value */
	    if ((stepgen->pos_scale < 1e-20)
		&& (stepgen->pos_scale > -1e-20)) {
		/* value too small, divide by zero is a bad thing */
		stepgen->pos_scale = 1.0;
	    }
	    /* we will need the reciprocal, and the accum is fixed point with
	       fractional bits, so we precalc some stuff */
	    stepgen->scale_recip = (1.0 / (1L << PICKOFF)) / stepgen->pos_scale;
	}
	/* scale accumulator to make floating point position, after
	   removing the one-half count offset */
	*(stepgen->pos_fb) = (double)(accum_a-(1<< (PICKOFF-1))) * stepgen->scale_recip;
	/* move on to next channel */
	stepgen++;
    }
    /* done */
}

/* helper function - computes integeral multiple of increment that is greater
   or equal to value */
static unsigned long ulceil(unsigned long value, unsigned long increment)
{
    if ( value == 0 ) {
	return 0;
    }
    return increment*(((value-1)/increment)+1);
}

static void update_freq(void *arg, long period)
{
    stepgen_t *stepgen;
    int n, newperiod;
    long min_step_period;
    long long int accum_a, accum_b;
    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;
    double desired_freq;
    /*! \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 */
    newperiod = 0;
    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;
	freqscale = (1L << PICKOFF) * periodfp;
	accelscale = freqscale * periodfp;
	/* force re-evaluation of the timing parameters */
	newperiod = 1;
    }
    /* now recalc constants related to the period of this funct */
    /* only recalc constants if period changes */
    if (period != old_dtns) {
	/* get ready to detect future period changes */
	old_dtns = period;
	/* dT is the period of this thread, used for the position loop */
	dt = period * 0.000000001;
	/* calc the reciprocal once here, to avoid multiple divides later */
	recip_dt = 1.0 / dt;