5i2x_stepgen.c

CNC 的开放码,EMC2 V2.2.8版

	s->old_enable = *(s->enable);
	/* merge new enable with previous step len and mode */
	if ( *(s->enable) ) {
	    s->len_mode_value |= 1 << ENABLE_SHIFT;
	} else {
	    s->len_mode_value &= ~(1 << ENABLE_SHIFT);
	}
	/* force write */
	write = 1;
    }
    if ( write ) {
	/* write mode, enable, and step length */
	iowrite32(s->len_mode_value, s->addr + LEN_MODE_ADDR);
	write = 0;
    }
    if ( s->stepspace != s->old_stepspace ) {
	/* convert stepspace in ns to clock periods */
	clocks = s->stepspace * (STEPGEN_MASTER_CLOCK / 1000000000.0);
	if ( clocks == 0 ) { clocks = 1; }
	/* set parameter to actual (post rounding) value */
	s->old_stepspace = clocks * (1000000000.0 / STEPGEN_MASTER_CLOCK);
	s->stepspace = s->old_stepspace;
	/* force recalc of max frequency */
	s->update_max = 1;
    }
    if ( s->scale != s->old_scale ) {
	s->old_scale = s->scale;
	/* validate the new scale value */
	if ((s->scale < 1e-20) && (s->scale > -1e-20)) {
	    /* value too small, divide by zero is a bad thing */
	    s->scale = 1.0;
	}
	/* force recalc of max frequency */
	s->update_max = 1;
    }
    if ( s->maxvel != s->old_maxvel ) {
	if ( s->maxvel < 0.0 ) {
	    s->maxvel = -s->maxvel;
	}
	s->old_maxvel = s->maxvel;
	/* force recalc of max frequency */
	s->update_max = 1;
    }
    if ( s->maxaccel != s->old_maxaccel ) {
	if ( s->maxaccel < 0.0 ) {
	    s->maxaccel = -s->maxaccel;
	}
	s->old_maxaccel = s->maxaccel;
	s->max_deltav = s->maxaccel * period * 0.000000001;
    }
    if ( s->update_max ) {
	/* either maxvel, scale, steplen, or stepspace changed */
	min_period_ns = s->steplen + s->stepspace;
	max_freq = 1000000000.0 / min_period_ns;
	s->internal_maxvel = max_freq / s->scale;
	if ( s->maxvel > 0.0 ) {
	    if ( s->maxvel < s->internal_maxvel ) {
		s->internal_maxvel = s->maxvel;
	    } else {
		s->maxvel = s->internal_maxvel;
		s->old_maxvel = s->maxvel;
	    }
	}
    }
    /* apply velocity limits */
    vel_cmd = *(s->vel_cmd);
    if ( vel_cmd > s->internal_maxvel ) {
	vel_cmd = s->internal_maxvel;
	*(s->vel_cmd) = vel_cmd;
    } else if ( vel_cmd < -s->internal_maxvel ) {
	vel_cmd = -s->internal_maxvel;
	*(s->vel_cmd) = vel_cmd;
    }
    /* apply ramping */
    if ( s->max_deltav != 0.0 ) {
	vel_diff = vel_cmd - s->current_vel;
	if ( vel_diff > s->max_deltav ) {
	    s->current_vel += s->max_deltav;
	} else if ( vel_diff < -s->max_deltav ) {
	    s->current_vel -= s->max_deltav;
	} else {
	    s->current_vel = vel_cmd;
	}
    }
    /* convert vel to freq */
    s->frequency = s->current_vel * s->scale;
    /* convert frequency to adder value and write to hardware */
    addval = s->frequency * COUNTS_PER_HZ;
    iowrite32(addval, s->addr + RATE_REG_ADDR);
}

static void write_stepgens(void *arg, long period)
{
    stepgen_t *s;

    s = arg;
    while ( s != NULL ) {
	write_stepgen(s, period);
	s = s->next;
    }
}

/******************************************************************************
                                HAL export code
 ******************************************************************************/

int export_stepgen(__u8 **ppcfg, board_data_t *board)
{
    __u8 *data;
    int retval;
    int gennum, boardnum;
    stepgen_t *stepgen, **p;
    int code, addr, pin0, pin1;
    char name[HAL_NAME_LEN + 2];

    /* read and validate config data */
    data = *ppcfg;
    code = data[0];
    addr = (data[1] << 8) + data[2];
    pin0 = data[3];
    pin1 = data[4];
    /* return ptr to next block */
    *ppcfg = &(data[5]);
    /* Allocate HAL memory for the step generator */
    stepgen = (stepgen_t *)(hal_malloc(sizeof(stepgen_t)));
    if ( stepgen == NULL ) {
	rtapi_print_msg(RTAPI_MSG_ERR, "5i2x: ERROR: hal_malloc() failed\n");
	return -1;
    }
    /* find end of linked list */
    boardnum = board->num;
    gennum = 0;
    p = &(board->stepgen);
    while ( *p != NULL ) {
	p = &((*p)->next);
	gennum++;
    }
    /* add to end of list */
    *p = stepgen;
    stepgen->next = NULL;
    rtapi_print_msg(RTAPI_MSG_ERR,
	"board %d stepgen %d at %x, using pins %d and %d\n",
	boardnum, gennum, addr, pin0, pin1 );
    /* export output HAL pins for feedbacks */
    retval = hal_pin_s32_newf(HAL_OUT, &(stepgen->counts),
	comp_id, "5i20.%d.stepgen.%d.counts", boardnum, gennum);
    if (retval != 0) return retval;
    retval = hal_pin_float_newf(HAL_OUT, &(stepgen->pos_fb),
	comp_id, "5i20.%d.stepgen.%d.pos-fb", boardnum, gennum);
    if (retval != 0) return retval;
    /* export HAL input pins for control */
    retval = hal_pin_bit_newf(HAL_IN, &(stepgen->enable),
	comp_id, "5i20.%d.stepgen.%d.enable", boardnum, gennum);
    if (retval != 0) return retval;
    retval = hal_pin_float_newf(HAL_IN, &(stepgen->vel_cmd),
	comp_id, "5i20.%d.stepgen.%d.vel-cmd", boardnum, gennum);
    if (retval != 0) return retval;
    /* now the parameters */
    retval = hal_param_u32_newf(HAL_RW, &(stepgen->step_type),
	comp_id, "5i20.%d.stepgen.%d.step-type", boardnum, gennum);
    if (retval != 0) return retval;
    retval = hal_param_float_newf(HAL_RO, &(stepgen->frequency),
	comp_id, "5i20.%d.stepgen.%d.frequency", boardnum, gennum);
    if (retval != 0) return retval;
    retval = hal_param_float_newf(HAL_RW, &(stepgen->maxaccel),
	comp_id, "5i20.%d.stepgen.%d.maxaccel", boardnum, gennum);
    if (retval != 0) return retval;
    retval = hal_param_float_newf(HAL_RW, &(stepgen->maxvel),
	comp_id, "5i20.%d.stepgen.%d.maxvel", boardnum, gennum);
    if (retval != 0) return retval;
    retval = hal_param_float_newf(HAL_RW, &(stepgen->scale),
	comp_id, "5i20.%d.stepgen.%d.scale", boardnum, gennum);
    if (retval != 0) return retval;
    retval = hal_param_u32_newf(HAL_RW, &(stepgen->steplen),
	comp_id, "5i20.%d.stepgen.%d.steplen", boardnum, gennum);
    if (retval != 0) return retval;
    retval = hal_param_u32_newf(HAL_RW, &(stepgen->stepspace),
	comp_id, "5i20.%d.stepgen.%d.stepspace", boardnum, gennum);
    if (retval != 0) return retval;
    retval = hal_param_u32_newf(HAL_RW, &(stepgen->dirsetup),
	comp_id, "5i20.%d.stepgen.%d.dirsetup", boardnum, gennum);
    if (retval != 0) return retval;
    retval = hal_param_u32_newf(HAL_RW, &(stepgen->dirhold),
	comp_id, "5i20.%d.stepgen.%d.dirhold", boardnum, gennum);
    if (retval != 0) return retval;
    /* set initial value for pin and params */
    *(stepgen->counts) = 0;
    *(stepgen->pos_fb) = 0.0;
    *(stepgen->enable) = 0;
    *(stepgen->vel_cmd) = 0.0;
    stepgen->step_type = 0;
    stepgen->frequency = 0.0;
    stepgen->maxaccel = 0.0;
    stepgen->maxvel = 0.0;
    stepgen->scale = 200.0;
    stepgen->steplen = 100;
    stepgen->stepspace = 100;
    stepgen->dirsetup = 100;
    stepgen->dirhold = 100;
    /* init other stuff */
    stepgen->addr = board->base + addr;
    /* setting these so they don't match the actual values forces
	an immediate update of the hw once the functions run */
    stepgen->old_enable = 1;
    stepgen->old_step_type = 100;
    stepgen->old_maxaccel = -1.0;
    stepgen->old_maxvel = -1.0;
    stepgen->old_scale = 0.0;
    stepgen->counts_hires = 0;
    stepgen->old_steplen = 0;
    stepgen->old_stepspace = 0;
    stepgen->old_dirsetup = 0;
    stepgen->old_dirhold = 0;
    stepgen->internal_maxvel = 0;
    stepgen->max_deltav = 0;
    stepgen->current_vel = 0;
    stepgen->update_max = 1;
    stepgen->old_accum = 0;
    stepgen->counts_hires = 0;
    stepgen->len_mode_value = 0x00000001;
    stepgen->hold_setup_value = 0x00010001;
    /* export functions - one funct serves all generators */
    if ( gennum > 0 ) {
	/* already exported */
	return 0;
    }
    rtapi_snprintf(name, HAL_NAME_LEN, "5i20.%d.stepgen.read", boardnum);
    retval = hal_export_funct(name, read_stepgens, stepgen, 1, 0, comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "5i20: ERROR: board %d stepgen read funct export failed\n",
	    boardnum);
	return -1;
    }
    rtapi_snprintf(name, HAL_NAME_LEN, "5i20.%d.stepgen.write", boardnum);
    retval = hal_export_funct(name, write_stepgens, stepgen, 1, 0, comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "5i20: ERROR: board %d stepgen read funct export failed\n",
	    boardnum);
	return -1;
    }
    return 0;
}