blocks.c

Source code for an Numeric Cmputer

    /* check inputs */
    if (( *(estop->ok_in) != 0 ) && ( *(estop->fault_in) == 0 )) {
	/* no fault conditions, check for reset edge */
	if (( *(estop->reset) != 0 ) && ( estop->old_reset == 0 )) {
	    /* got a rising edge, indicate "OK" on outputs */
	    *(estop->ok_out) = 1;
	    *(estop->fault_out) = 0;
	}
	/* toggle watchdog */
	if ( *(estop->wd) == 0 ) {
	    *(estop->wd) = 1;
	} else {
	    *(estop->wd) = 0;
	}
    } else {
	/* fault condition exists, trip */
	*(estop->ok_out) = 0;
	*(estop->fault_out) = 1;
    }
    /* store state of reset input for next pass (for edge detect) */
    estop->old_reset = *(estop->reset);
}


static void not_funct(void *arg, long period)
{
    not_t *not;

    /* point to block data */
    not = (not_t *) arg;
    /* calculate output */
    *(not->out) = ! *(not->in);
}

static void and2_funct(void *arg, long period)
{
    and2_t *and2;

    /* point to block data */
    and2 = (and2_t *) arg;
    /* calculate output */
    *(and2->out) = *(and2->in0) && *(and2->in1);
}

static void or2_funct(void *arg, long period)
{
    or2_t *or2;

    /* point to block data */
    or2 = (or2_t *) arg;
    /* calculate output */
    *(or2->out) = *(or2->in0) || *(or2->in1);
}

static void scale_funct(void *arg, long period)
{
    scale_t *scale;

    /* point to block data */
    scale = (scale_t *) arg;
    /* calculate output */
    *(scale->out) = *(scale->in) * scale->gain + scale->offset;
}

static void lowpass_funct(void *arg, long period)
{
    lowpass_t *lowpass;

    /* point to block data */
    lowpass = (lowpass_t *) arg;
    /* calculate output */
    *(lowpass->out) += (*(lowpass->in) - *(lowpass->out)) * lowpass->gain;
}

static void match8_funct(void *arg, long period)
{
    match8_t *match8;
    int n;
    hal_bit_t a, b, tmp;

    /* point to block data */
    match8 = (match8_t *) arg;
    /* calculate output, starting with cascade/enable input */
    if ( *(match8->in) != 0 ) {
	/* input true, process a and b */
	tmp = 1;
	n = 0;
    } else {
	/* input false, ignore a and b */
	tmp = 0;
	n = 8;
    }
    /* test all bits, exit loop as soon as mismatch found */
    while ( n < 8 ) {
	a = *(match8->a[n]);
	b = *(match8->b[n]);
	if ((( a == 0 ) && ( b != 0 )) || (( a != 0 ) && ( b == 0 ))) {
	    tmp = 0;
	    n = 8;
	}
	n++;
    }
    /* write output */
    *(match8->out) = tmp;
}

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

static int export_constant(int num)
{
    int retval, msg;
    char buf[HAL_NAME_LEN + 2];
    constant_t *constant;

    /* 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);

    /* allocate shared memory for window comparator */
    constant = hal_malloc(sizeof(constant_t));
    if (constant == 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: hal_malloc() failed\n");
	return -1;
    }
    /* export pin for output */
    rtapi_snprintf(buf, HAL_NAME_LEN, "constant.%d.out", num);
    retval = hal_pin_float_new(buf, HAL_WR, &(constant->out), comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: '%s' pin export failed\n", buf);
	return retval;
    }
    /* export param for value */
    rtapi_snprintf(buf, HAL_NAME_LEN, "constant.%d.value", num);
    retval = hal_param_float_new(buf, HAL_WR, &(constant->value), comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: '%s' param export failed\n", buf);
	return retval;
    }
    /* export function */
    rtapi_snprintf(buf, HAL_NAME_LEN, "constant.%d", num);
    retval = hal_export_funct(buf, constant_funct, constant, 1, 0, comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: '%s' funct export failed\n", buf);
	return -1;
    }
    /* set default parameter values */
    constant->value = 1.0;
    /* restore saved message level */
    rtapi_set_msg_level(msg);
    return 0;
}

static int export_wcomp(int num)
{
    int retval, msg;
    char buf[HAL_NAME_LEN + 2];
    wcomp_t *wcomp;

    /* 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);

    /* allocate shared memory for window comparator */
    wcomp = hal_malloc(sizeof(wcomp_t));
    if (wcomp == 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: hal_malloc() failed\n");
	return -1;
    }
    /* export pin for input */
    rtapi_snprintf(buf, HAL_NAME_LEN, "wcomp.%d.in", num);
    retval = hal_pin_float_new(buf, HAL_RD, &(wcomp->in), comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: '%s' pin export failed\n", buf);
	return retval;
    }
    /* export pin for output */
    rtapi_snprintf(buf, HAL_NAME_LEN, "wcomp.%d.out", num);
    retval = hal_pin_bit_new(buf, HAL_WR, &(wcomp->out), comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: '%s' pin export failed\n", buf);
	return retval;
    }
    /* export params for min and max */
    rtapi_snprintf(buf, HAL_NAME_LEN, "wcomp.%d.min", num);
    retval = hal_param_float_new(buf, HAL_WR, &(wcomp->min), comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: '%s' param export failed\n", buf);
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "wcomp.%d.max", num);
    retval = hal_param_float_new(buf, HAL_WR, &(wcomp->max), comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: '%s' param export failed\n", buf);
	return retval;
    }
    /* export function */
    rtapi_snprintf(buf, HAL_NAME_LEN, "wcomp.%d", num);
    retval = hal_export_funct(buf, wcomp_funct, wcomp, 1, 0, comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: '%s' funct export failed\n", buf);
	return -1;
    }
    /* set default parameter values */
    wcomp->min = -1.0;
    wcomp->max = 1.0;
    /* restore saved message level */
    rtapi_set_msg_level(msg);
    return 0;
}

static int export_comp(int num)
{
    int retval, msg;
    char buf[HAL_NAME_LEN + 2];
    comp_t *comp;

    /* 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);

    /* allocate shared memory for 2-input comparator */
    comp = hal_malloc(sizeof(comp_t));
    if (comp == 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: hal_malloc() failed\n");
	return -1;
    }
    /* export pins for inputs */
    rtapi_snprintf(buf, HAL_NAME_LEN, "comp.%d.in0", num);
    retval = hal_pin_float_new(buf, HAL_RD, &(comp->in0), comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: '%s' pin export failed\n", buf);
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "comp.%d.in1", num);
    retval = hal_pin_float_new(buf, HAL_RD, &(comp->in1), comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: '%s' pin export failed\n", buf);
	return retval;
    }
    /* export pin for output */
    rtapi_snprintf(buf, HAL_NAME_LEN, "comp.%d.out", num);
    retval = hal_pin_bit_new(buf, HAL_WR, &(comp->out), comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: '%s' pin export failed\n", buf);
	return retval;
    }
    /* export params for hystersis */
    rtapi_snprintf(buf, HAL_NAME_LEN, "comp.%d.hyst", num);
    retval = hal_param_float_new(buf, HAL_WR, &(comp->hyst), comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: '%s' param export failed\n", buf);
	return retval;
    }
    /* export function */
    rtapi_snprintf(buf, HAL_NAME_LEN, "comp.%d", num);
    retval = hal_export_funct(buf, comp_funct, comp, 1, 0, comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: '%s' funct export failed\n", buf);
	return -1;
    }
    /* set default parameter values */
    comp->hyst = 0.0;
    /* restore saved message level */
    rtapi_set_msg_level(msg);
    return 0;
}

static int export_mux2(int num)
{
    int retval, msg;
    char buf[HAL_NAME_LEN + 2];
    mux2_t *mux2;

    /* 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);

    /* allocate shared memory for 2 input multiplexor */
    mux2 = hal_malloc(sizeof(mux2_t));
    if (mux2 == 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: hal_malloc() failed\n");
	return -1;
    }
    /* export pins for inputs */
    rtapi_snprintf(buf, HAL_NAME_LEN, "mux2.%d.in0", num);
    retval = hal_pin_float_new(buf, HAL_RD, &(mux2->in0), comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: '%s' pin export failed\n", buf);
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "mux2.%d.in1", num);
    retval = hal_pin_float_new(buf, HAL_RD, &(mux2->in1), comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: '%s' pin export failed\n", buf);
	return retval;
    }
    /* export pin for output */
    rtapi_snprintf(buf, HAL_NAME_LEN, "mux2.%d.out", num);
    retval = hal_pin_float_new(buf, HAL_WR, &(mux2->out), comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: '%s' pin export failed\n", buf);
	return retval;
    }
    /* export pin for select input */
    rtapi_snprintf(buf, HAL_NAME_LEN, "mux2.%d.sel", num);
    retval = hal_pin_bit_new(buf, HAL_RD, &(mux2->sel), comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: '%s' pin export failed\n", buf);
	return retval;
    }
    /* export function */
    rtapi_snprintf(buf, HAL_NAME_LEN, "mux2.%d", num);
    retval = hal_export_funct(buf, mux2_funct, mux2, 1, 0, comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: '%s' funct export failed\n", buf);
	return -1;
    }
    /* restore saved message level */
    rtapi_set_msg_level(msg);
    return 0;
}

static int export_mux4(int num)
{
    int retval, msg;
    char buf[HAL_NAME_LEN + 2];
    mux4_t *mux4;

    /* 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);

    /* allocate shared memory for 4 input multiplexor */
    mux4 = hal_malloc(sizeof(mux4_t));
    if (mux4 == 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: hal_malloc() failed\n");
	return -1;
    }
    /* export pins for inputs */
    rtapi_snprintf(buf, HAL_NAME_LEN, "mux4.%d.in0", num);
    retval = hal_pin_float_new(buf, HAL_RD, &(mux4->in0), comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: '%s' pin export failed\n", buf);
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "mux4.%d.in1", num);
    retval = hal_pin_float_new(buf, HAL_RD, &(mux4->in1), comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: '%s' pin export failed\n", buf);
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "mux4.%d.in2", num);
    retval = hal_pin_float_new(buf, HAL_RD, &(mux4->in2), comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: '%s' pin export failed\n", buf);
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "mux4.%d.in3", num);
    retval = hal_pin_float_new(buf, HAL_RD, &(mux4->in3), comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: '%s' pin export failed\n", buf);
	return retval;
    }
    /* export pin for output */
    rtapi_snprintf(buf, HAL_NAME_LEN, "mux4.%d.out", num);
    retval = hal_pin_float_new(buf, HAL_WR, &(mux4->out), comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: '%s' pin export failed\n", buf);