encoder.c

来自「CNC 的开放码,EMC2 V2.2.8版」· C语言 代码 · 共 537 行 · 第 1/2 页

/***********************************************************************
*            REALTIME ENCODER COUNTING AND UPDATE FUNCTIONS            *
************************************************************************/

static void update(void *arg, long period)
{
    counter_t *cntr;
    atomic *buf;
    int n;
    unsigned char state;

    cntr = arg;
    for (n = 0; n < num_chan; n++) {
	buf = (atomic *) cntr->bp;
	/* get state machine current state */
	state = cntr->state;
	/* add input bits to state code */
	if (*(cntr->phaseA)) {
	    state |= SM_PHASE_A_MASK;
	}
	if (*(cntr->phaseB)) {
	    state |= SM_PHASE_B_MASK;
	}
	/* look up new state */
	if ( cntr->counter_mode ) {
	    state = lut_ctr[state & (SM_LOOKUP_MASK & ~SM_PHASE_B_MASK)];
	} else if ( cntr->x4_mode ) {
	    state = lut_x4[state & SM_LOOKUP_MASK];
	} else {
	    state = lut_x1[state & SM_LOOKUP_MASK];
	}
	/* should we count? */
	if (state & SM_CNT_UP_MASK) {
	    cntr->raw_counts++;
	    buf->raw_count = cntr->raw_counts;
	    buf->timestamp = timebase;
	    buf->count_detected = 1;
	} else if (state & SM_CNT_DN_MASK) {
	    cntr->raw_counts--;
	    buf->raw_count = cntr->raw_counts;
	    buf->timestamp = timebase;
	    buf->count_detected = 1;
	}
	/* save state machine state */
	cntr->state = state;
	/* get old phase Z state, make room for new bit value */
	state = cntr->oldZ << 1;
	/* add new value of phase Z */
	if (*(cntr->phaseZ)) {
	    state |= 1;
	}
	cntr->oldZ = state & 3;
	/* test for index enabled and rising edge on phase Z */
	if ((state & cntr->Zmask) == 1) {
	    /* capture counts, reset Zmask */
	    buf->index_count = cntr->raw_counts;
	    buf->index_detected = 1;
	    cntr->Zmask = 0;
	}
	/* move on to next channel */
	cntr++;
    }
    /* increment main timestamp counter */
    timebase += period;
    /* done */
}


/* if no edges in 100mS time, force vel to zero */
#define TIMEOUT 100000000

static void capture(void *arg, long period)
{
    counter_t *cntr;
    atomic *buf;
    int n;
    __s32 delta_counts;
    __u32 delta_time;
    double vel;

    cntr = arg;
    for (n = 0; n < num_chan; n++) {
	/* point to active buffer */
	buf = (atomic *) cntr->bp;
	/* tell update() to use the other buffer */
	if ( buf == &(cntr->buf[0]) ) {
	    cntr->bp = &(cntr->buf[1]);
	} else {
	    cntr->bp = &(cntr->buf[0]);
	}
	/* handle index */
	if ( buf->index_detected ) {
	    buf->index_detected = 0;
	    cntr->index_count = buf->index_count;
	    *(cntr->index_ena) = 0;
	}
	/* update Zmask based on index_ena */
	if (*(cntr->index_ena)) {
	    cntr->Zmask = 3;
	} else {
	    cntr->Zmask = 0;
	}
	/* done interacting with update() */
	/* check for change in scale value */
	if ( cntr->pos_scale != cntr->old_scale ) {
	    /* save new scale to detect future changes */
	    cntr->old_scale = cntr->pos_scale;
	    /* scale value has changed, test and update it */
	    if ((cntr->pos_scale < 1e-20) && (cntr->pos_scale > -1e-20)) {
		/* value too small, divide by zero is a bad thing */
		cntr->pos_scale = 1.0;
	    }
	    /* we actually want the reciprocal */
	    cntr->scale = 1.0 / cntr->pos_scale;
	}
	/* check reset input */
	if (*(cntr->reset)) {
	    /* reset is active, reset the counter */
	    /* note: we NEVER reset raw_counts, that is always a
		running count of edges seen since startup.  The
		public "count" is the difference between raw_count
		and index_count, so it will become zero. */
	    cntr->raw_count = cntr->raw_counts;
	    cntr->index_count = cntr->raw_count;
	}
	/* process data from update() */
	if ( buf->count_detected ) {
	    buf->count_detected = 0;
	    delta_counts = buf->raw_count - cntr->raw_count;
	    delta_time = buf->timestamp - cntr->timestamp;
	    cntr->raw_count = buf->raw_count;
	    cntr->timestamp = buf->timestamp;
	    if ( cntr->counts_since_timeout < 2 ) {
		cntr->counts_since_timeout++;
	    } else {
		vel = (delta_counts * cntr->scale ) / (delta_time * 1e-9);
		*(cntr->vel) = vel;
	    }
	} else {
	    /* no count */
	    if ( cntr->counts_since_timeout ) {
		/* calc time since last count */
		delta_time = timebase - cntr->timestamp;
		if ( delta_time < TIMEOUT ) {
		    /* not to long, estimate vel if a count arrived now */
		    vel = ( cntr->scale ) / (delta_time * 1e-9);
		    /* make vel positive, even if scale is negative */
		    if ( vel < 0.0 ) vel = -vel;
		    /* use lesser of estimate and previous value */
		    /* use sign of previous value, magnitude of estimate */
		    if ( vel < *(cntr->vel) ) {
			*(cntr->vel) = vel;
		    }
		    if ( -vel > *(cntr->vel) ) {
			*(cntr->vel) = -vel;
		    }
		} else {
		    /* its been a long time, stop estimating */
		    cntr->counts_since_timeout = 0;
		    *(cntr->vel) = 0;
		}
	    } else {
		/* we already stopped estimating */
		*(cntr->vel) = 0;
	    }
	}
	/* compute net counts */
	*(cntr->count) = cntr->raw_count - cntr->index_count;
	/* scale count to make floating point position */
	*(cntr->pos) = *(cntr->count) * cntr->scale;
	/* move on to next channel */
	cntr++;
    }
    /* done */
}

/***********************************************************************
*                   LOCAL FUNCTION DEFINITIONS                         *
************************************************************************/

static int export_counter(int num, counter_t * addr)
{
    int retval, msg;
    char buf[HAL_NAME_LEN + 2];

    /* This function exports a lot of stuff, which results in a lot of
       logging if msg_level is at INFO or ALL. So we save the current value
       of msg_level and restore it later.  If you actually need to log this
       function's actions, change the second line below */
    msg = rtapi_get_msg_level();
    rtapi_set_msg_level(RTAPI_MSG_WARN);

    /* export pins for the quadrature inputs */
    rtapi_snprintf(buf, HAL_NAME_LEN, "encoder.%d.phase-A", num);
    retval = hal_pin_bit_new(buf, HAL_IN, &(addr->phaseA), comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "encoder.%d.phase-B", num);
    retval = hal_pin_bit_new(buf, HAL_IN, &(addr->phaseB), comp_id);
    if (retval != 0) {
	return retval;
    }
    /* export pin for the index input */
    rtapi_snprintf(buf, HAL_NAME_LEN, "encoder.%d.phase-Z", num);
    retval = hal_pin_bit_new(buf, HAL_IN, &(addr->phaseZ), comp_id);
    if (retval != 0) {
	return retval;
    }
    /* export pin for the index enable input */
    rtapi_snprintf(buf, HAL_NAME_LEN, "encoder.%d.index-enable", num);
    retval = hal_pin_bit_new(buf, HAL_IO, &(addr->index_ena), comp_id);
    if (retval != 0) {
	return retval;
    }
    /* export pin for the reset input */
    rtapi_snprintf(buf, HAL_NAME_LEN, "encoder.%d.reset", num);
    retval = hal_pin_bit_new(buf, HAL_IN, &(addr->reset), comp_id);
    if (retval != 0) {
	return retval;
    }
    /* export parameter for raw counts */
    rtapi_snprintf(buf, HAL_NAME_LEN, "encoder.%d.rawcounts", num);
    retval = hal_param_s32_new(buf, HAL_RO, &(addr->raw_counts), comp_id);
    if (retval != 0) {
	return retval;
    }
    /* export pin for counts captured by capture() */
    rtapi_snprintf(buf, HAL_NAME_LEN, "encoder.%d.counts", num);
    retval = hal_pin_s32_new(buf, HAL_OUT, &(addr->count), comp_id);
    if (retval != 0) {
	return retval;
    }
    /* export pin for scaled position captured by capture() */
    rtapi_snprintf(buf, HAL_NAME_LEN, "encoder.%d.position", num);
    retval = hal_pin_float_new(buf, HAL_OUT, &(addr->pos), comp_id);
    if (retval != 0) {
	return retval;
    }
    /* export pin for scaled velocity captured by capture() */
    rtapi_snprintf(buf, HAL_NAME_LEN, "encoder.%d.velocity", num);
    retval = hal_pin_float_new(buf, HAL_OUT, &(addr->vel), comp_id);
    if (retval != 0) {
	return retval;
    }
    /* export parameter for scaling */
    rtapi_snprintf(buf, HAL_NAME_LEN, "encoder.%d.position-scale", num);
    retval = hal_param_float_new(buf, HAL_RW, &(addr->pos_scale), comp_id);
    if (retval != 0) {
	return retval;
    }
    /* export parameter for x4 mode */
    rtapi_snprintf(buf, HAL_NAME_LEN, "encoder.%d.x4-mode", num);
    retval = hal_param_bit_new(buf, HAL_RW, &(addr->x4_mode), comp_id);
    if (retval != 0) {
	return retval;
    }
    /* export parameter for counter mode */
    rtapi_snprintf(buf, HAL_NAME_LEN, "encoder.%d.counter-mode", num);
    retval = hal_param_bit_new(buf, HAL_RW, &(addr->counter_mode), comp_id);
    if (retval != 0) {
	return retval;
    }
    /* restore saved message level */
    rtapi_set_msg_level(msg);
    return 0;
}