freqgen.c

Source code for an Numeric Cmputer

		    } 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 {
		    if (++freqgen->wd.st2.state > freqgen->wd.st2.cycle_max) {
			freqgen->wd.st2.state = 0;
		    }
		    /* count the step for feedback */
		    freqgen->rawcount++;
		}
		/* look up correct output pattern */
		state = (freqgen->wd.st2.lut)[freqgen->wd.st2.state];
		/* now output the phase bits */
		for (p = 0; p < freqgen->wd.st2.num_phases; p++) {
		    /* output one phase */
		    *(freqgen->phase[p]) = state & 1;
		    /* move to the next phase */
		    state >>= 1;
		}
	    }
	}
	/* move on to next step generator */
	freqgen++;
    }
    /* done */
}

static void update_freq(void *arg, long period)
{
    freqgen_t *freqgen;
    int n;
    float tmpf, limf;

    /* 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. */
    if (periodns != old_periodns) {
	/* recompute various constants that depend on periodns */
	periodfp = periodns * 0.000000001;
	maxf = 1.0 / periodfp;
	freqscale = ((1L << 30) * 2.0) / maxf;
	accelscale = freqscale * periodfp;
	old_periodns = periodns;
    }
    /* update the step generators */
    freqgen = arg;
    for (n = 0; n < num_chan; n++) {
	/* calculate frequency limit */
	if (freqgen->wd.st0.step_type == 0) {
	    /* stepping type 0 limit depends on timing params that may change 
	     */
	    limf =
		maxf / (freqgen->wd.st0.step_len +
		freqgen->wd.st0.step_space);
	} else {
	    limf = maxf;
	}
	/* check for user specified frequency limit parameter */
	if (freqgen->maxfreq <= 0.0) {
	    /* set to zero if negative */
	    freqgen->maxfreq = 0.0;
	} else {
	    /* parameter is non-zero, compare to limf */
	    if (freqgen->maxfreq > limf) {
		/* parameter is too high, lower it */
		freqgen->maxfreq = limf;
	    } else {
		/* lower limit to match parameter */
		limf = freqgen->maxfreq;
	    }
	}
	/* convert velocity command to Hz */
	tmpf = *(freqgen->vel) * freqgen->vel_scale;
	/* limit the commanded frequency */
	if (tmpf > limf) {
	    tmpf = limf;
	} else if (tmpf < -limf) {
	    tmpf = -limf;
	}
	/* save limited frequency */
	freqgen->freq = tmpf;
	/* calculate new addval */
	freqgen->newaddval = tmpf * freqscale;
	/* check for illegal (negative) maxaccel parameter */
	if (freqgen->maxaccel <= 0.0) {
	    /* set to zero if negative */
	    freqgen->maxaccel = 0.0;
	}
	/* calculate new deltalim */
	freqgen->deltalim = freqgen->maxaccel * accelscale;
	/* move on to next channel */
	freqgen++;
    }
    /* done */
}

static void update_pos(void *arg, long period)
{
    freqgen_t *freqgen;
    int n;

    freqgen = arg;
    for (n = 0; n < num_chan; n++) {
	/* capture raw counts to latches */
	*(freqgen->count) = freqgen->rawcount;
	/* check for change in scale value */
	if ( freqgen->pos_scale != freqgen->old_scale ) {
	    /* get ready to detect future scale changes */
	    freqgen->old_scale = freqgen->pos_scale;
	    /* validate the new scale value */
	    if ((freqgen->pos_scale < 1e-20) && (freqgen->pos_scale > -1e-20)) {
		/* value too small, divide by zero is a bad thing */
		freqgen->pos_scale = 1.0;
	    }
	    /* we will need the reciprocal */
	    freqgen->scale_recip = 1.0 / freqgen->pos_scale;
	}
	/* scale count to make floating point position */
	*(freqgen->pos) = *(freqgen->count) * freqgen->scale_recip;
	/* move on to next channel */
	freqgen++;
    }
    /* done */
}

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

static int export_freqgen(int num, freqgen_t * addr, int step_type)
{
    int n, 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 param variable for raw counts */
    rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.rawcounts", num);
    retval = hal_param_s32_new(buf, HAL_RD, &(addr->rawcount), comp_id);
    if (retval != 0) {
	return retval;
    }
    /* export pin for counts captured by update() */
    rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.counts", num);
    retval = hal_pin_s32_new(buf, HAL_WR, &(addr->count), comp_id);
    if (retval != 0) {
	return retval;
    }
    /* export pin for scaled position captured by update() */
    rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.position-fb", num);
    retval = hal_pin_float_new(buf, HAL_WR, &(addr->pos), comp_id);
    if (retval != 0) {
	return retval;
    }
    /* export parameter for position scaling */
    rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.position-scale", num);
    retval = hal_param_float_new(buf, HAL_WR, &(addr->pos_scale), comp_id);
    if (retval != 0) {
	return retval;
    }
    /* export pin for frequency command */
    rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.velocity", num);
    retval = hal_pin_float_new(buf, HAL_RD, &(addr->vel), comp_id);
    if (retval != 0) {
	return retval;
    }
    /* export parameter for frequency scaling */
    rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.velocity-scale", num);
    retval = hal_param_float_new(buf, HAL_WR, &(addr->vel_scale), comp_id);
    if (retval != 0) {
	return retval;
    }
    /* export parameter for max frequency */
    rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.maxfreq", num);
    retval = hal_param_float_new(buf, HAL_WR, &(addr->maxfreq), comp_id);
    if (retval != 0) {
	return retval;
    }
    /* export param for scaled velocity (frequency in Hz) */
    rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.frequency", num);
    retval = hal_param_float_new(buf, HAL_RD, &(addr->freq), comp_id);
    if (retval != 0) {
	return retval;
    }
    /* export parameter for max accel/decel */
    rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.maxaccel", num);
    retval = hal_param_float_new(buf, HAL_WR, &(addr->maxaccel), comp_id);
    if (retval != 0) {
	return retval;
    }
    /* set default parameter values */
    addr->pos_scale = 1.0;
    addr->vel_scale = 1.0;
    /* set maxfreq very high - let update_freq() fix it */
    addr->maxfreq = 1e15;
    addr->maxaccel = 0.0;
    addr->wd.st0.step_type = step_type;
    /* init the step generator core to zero output */
    addr->accum = 0;
    addr->addval = 0;
    addr->newaddval = 0;
    addr->deltalim = 0;
    addr->rawcount = 0;
    if (step_type == 0) {
	/* setup for stepping type 0 - step/dir */
	addr->wd.st0.need_step = 0;
	addr->wd.st0.setup_timer = 0;
	addr->wd.st0.hold_timer = 0;
	addr->wd.st0.space_timer = 0;
	addr->wd.st0.len_timer = 0;
	/* export parameters for step/dir pulse timing */
	rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.dirsetup", num);
	retval =
	    hal_param_u8_new(buf, HAL_WR, &(addr->wd.st0.dir_setup), comp_id);
	if (retval != 0) {
	    return retval;
	}
	rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.dirhold", num);
	retval =
	    hal_param_u8_new(buf, HAL_WR, &(addr->wd.st0.dir_hold), comp_id);
	if (retval != 0) {
	    return retval;
	}
	rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.steplen", num);
	retval =
	    hal_param_u8_new(buf, HAL_WR, &(addr->wd.st0.step_len), comp_id);
	if (retval != 0) {
	    return retval;
	}
	rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.stepspace", num);
	retval =
	    hal_param_u8_new(buf, HAL_WR, &(addr->wd.st0.step_space),
	    comp_id);
	if (retval != 0) {
	    return retval;
	}
	/* init the parameters */
	addr->wd.st0.dir_setup = 1;
	addr->wd.st0.dir_hold = 1;
	addr->wd.st0.step_len = 1;
	addr->wd.st0.step_space = 1;
	/* export pins for step and direction */
	rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.step", num);
	retval =
	    hal_pin_bit_new(buf, HAL_WR, &(addr->phase[STEP_PIN]), comp_id);
	if (retval != 0) {
	    return retval;
	}
	*(addr->phase[STEP_PIN]) = 0;
	rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.dir", num);
	retval =
	    hal_pin_bit_new(buf, HAL_WR, &(addr->phase[DIR_PIN]), comp_id);
	if (retval != 0) {
	    return retval;
	}
	*(addr->phase[DIR_PIN]) = 0;
    } else if (step_type == 1) {
	/* setup for stepping type 1 - pseudo-PWM */
	/* export pins for up and down */
	rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.up", num);
	retval =
	    hal_pin_bit_new(buf, HAL_WR, &(addr->phase[UP_PIN]), comp_id);
	if (retval != 0) {
	    return retval;
	}
	*(addr->phase[UP_PIN]) = 0;
	rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.down", num);
	retval =
	    hal_pin_bit_new(buf, HAL_WR, &(addr->phase[DOWN_PIN]), comp_id);
	if (retval != 0) {
	    return retval;
	}
	*(addr->phase[DOWN_PIN]) = 0;
	rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.count", num);
	retval =
	    hal_pin_bit_new(buf, HAL_WR, &(addr->phase[COUNT_PIN]), comp_id);
	if (retval != 0) {
	    return retval;
	}
	*(addr->phase[COUNT_PIN]) = 0;
    } else {
	/* setup for stepping types 2 and higher */
	addr->wd.st2.state = 0;
	addr->wd.st2.cycle_max = cycle_len_lut[step_type - 2] - 1;
	addr->wd.st2.num_phases = num_phases_lut[step_type - 2];
	addr->wd.st2.lut = (char *) (&(master_lut[step_type - 2][0]));
	/* export pins for output phases */
	for (n = 0; n < addr->wd.st2.num_phases; n++) {
	    rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.phase-%c", num,
		n + 'A');
	    retval = hal_pin_bit_new(buf, HAL_WR, &(addr->phase[n]), comp_id);
	    if (retval != 0) {
		return retval;
	    }
	    *(addr->phase[n]) = 0;
	}
    }
    /* set initial pin values */
    *(addr->count) = 0;
    *(addr->pos) = 0.0;
    *(addr->vel) = 0.0;
    /* restore saved message level */
    rtapi_set_msg_level(msg);
    return 0;
}