Ejemplo n.º 1
0
static int export_delay(int num, bit_delay_t * addr)
{
    int retval;
    char buf[HAL_NAME_LEN + 2];

    /* export pin for input bit */
    rtapi_snprintf(buf, HAL_NAME_LEN, "delay.%d.in", num);
    retval = hal_pin_bit_new(buf, HAL_IN, &(addr->in), comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "TIMEDELAY: ERROR: '%s' pin export failed\n", buf);
	return retval;
    }
    /* export pin for output bit */
    rtapi_snprintf(buf, HAL_NAME_LEN, "delay.%d.out", num);
    retval = hal_pin_bit_new(buf, HAL_OUT, &(addr->out), comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "TIMEDELAY: ERROR: '%s' pin export failed\n", buf);
	return retval;
    }
    
    /* export off delay parameter */
    rtapi_snprintf(buf, HAL_NAME_LEN, "delay.%d.off_delay", num);
    retval = hal_param_float_new(buf, HAL_RW, &(addr->off_delay), comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "TIMEDELAY: ERROR: '%s' parameter export failed\n", buf);
	return retval;
    }
    /* export on delay parameter */
    rtapi_snprintf(buf, HAL_NAME_LEN, "delay.%d.on_delay", num);
    retval = hal_param_float_new(buf, HAL_RW, &(addr->on_delay), comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "TIMEDELAY: ERROR: '%s' parameter export failed\n", buf);
	return retval;
    }
    /* export elapsed time parameter */
    rtapi_snprintf(buf, HAL_NAME_LEN, "delay.%d.elapsed", num);
    retval = hal_param_float_new(buf, HAL_RO, &(addr->elapsed), comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "TIMEDELAY: ERROR: '%s' parameter export failed\n", buf);
	return retval;
    }

    /* set initial parameter and pin values */
    *(addr->in) = 0;
    *(addr->out) = 0;
    addr->timer = 0.0;
    addr->off_delay = DEFAULT_DELAY;
    addr->on_delay = DEFAULT_DELAY;
    addr->elapsed = 0.0;
    return 0;
}
Ejemplo n.º 2
0
static int export_input_pin(int pinnum, io_pin * pin)
{
    char buf[HAL_NAME_LEN + 2];
    int retval;
    /* export read only HAL pin for input data */
    rtapi_snprintf(buf, HAL_NAME_LEN, "vti.in-%02d", pinnum);
    retval = hal_pin_bit_new(buf, HAL_OUT, &(pin->data), comp_id);
    if (retval != 0)
	return retval;
    /* export additional pin for inverted input data */
    rtapi_snprintf(buf, HAL_NAME_LEN, "vti.in-%02d-not", pinnum);
    retval = hal_pin_bit_new(buf, HAL_OUT, &(pin->io.not), comp_id);
    /* initialize HAL pins */
    *(pin->data) = 0;
    *(pin->io.not) = 1;
    return retval;
}
Ejemplo n.º 3
0
HalBitPin::HalBitPin(const char *name, int dir, int comp_id, int defaultVal)
{
    mDir = setDirection(dir);
    mPin = (hal_bit_t *) hal_malloc(sizeof(hal_bit_t));
    mSuccess = hal_pin_bit_new(name, mDir, &mPin, comp_id);
    if(mSuccess == 0)
        *mPin = defaultVal;
}
Ejemplo n.º 4
0
static int export_output_pin(int pinnum, io_pin * pin)
{
    char buf[HAL_NAME_LEN + 2];
    int retval;
    /* export read only HAL pin for output data */
    rtapi_snprintf(buf, HAL_NAME_LEN, "vti.out-%02d", pinnum);
    retval = hal_pin_bit_new(buf, HAL_IN, &(pin->data), comp_id);
    if (retval != 0)
	return retval;
    /* export parameter for polarity */
    rtapi_snprintf(buf, HAL_NAME_LEN, "vti.out-%02d-invert", pinnum);
    retval = hal_param_bit_new(buf, HAL_RW, &(pin->io.invert), comp_id);
    /* initialize HAL pin and param */
    *(pin->data) = 0;
    pin->io.invert = 0;
    return retval;
}
Ejemplo n.º 5
0
int export_pwmgen(hal_pru_generic_t *hpg, int i)
{
    char name[HAL_NAME_LEN + 1];
    int r, j;

    // HAL values common to all outputs in this instance
    rtapi_snprintf(name, sizeof(name), "%s.pwmgen.%02d.pwm_period", hpg->config.halname, i);
    r = hal_param_u32_new(name, HAL_RW, &(hpg->pwmgen.instance[i].hal.param.pwm_period), hpg->config.comp_id);
    if (r != 0) { return r; }

    hpg->pwmgen.instance[i].hal.param.pwm_period = 10000000;    // Default to 10 mS period, or 100 Hz

    for (j=0; j < hpg->pwmgen.instance[i].num_outputs; j++) {
        // Export HAL Pins
        rtapi_snprintf(name, sizeof(name), "%s.pwmgen.%02d.out.%02d.enable", hpg->config.halname, i, j);
        r = hal_pin_bit_new(name, HAL_IN, &(hpg->pwmgen.instance[i].out[j].hal.pin.enable), hpg->config.comp_id);
        if (r != 0) { return r; }

        rtapi_snprintf(name, sizeof(name), "%s.pwmgen.%02d.out.%02d.value", hpg->config.halname, i, j);
        r = hal_pin_float_new(name, HAL_IN, &(hpg->pwmgen.instance[i].out[j].hal.pin.value), hpg->config.comp_id);
        if (r != 0) { return r; }

        // Export HAL Parameters
        rtapi_snprintf(name, sizeof(name), "%s.pwmgen.%02d.out.%02d.scale", hpg->config.halname, i, j);
        r = hal_param_float_new(name, HAL_RW, &(hpg->pwmgen.instance[i].out[j].hal.param.scale), hpg->config.comp_id);
        if (r != 0) { return r; }

        rtapi_snprintf(name, sizeof(name), "%s.pwmgen.%02d.out.%02d.pin", hpg->config.halname, i, j);
        r = hal_param_u32_new(name, HAL_RW, &(hpg->pwmgen.instance[i].out[j].hal.param.pin), hpg->config.comp_id);
        if (r != 0) { return r; }

        // Initialize HAL Pins
        *(hpg->pwmgen.instance[i].out[j].hal.pin.enable) = 0;
        *(hpg->pwmgen.instance[i].out[j].hal.pin.value)  = 0.0;

        // Initialize HAL Parameters
        hpg->pwmgen.instance[i].out[j].hal.param.pin   = PRU_DEFAULT_PIN;
        hpg->pwmgen.instance[i].out[j].hal.param.scale = 1.0;
    }

    return 0;
}
Ejemplo n.º 6
0
int export_encoder(hal_pru_generic_t *hpg, int i)
{
    char name[HAL_NAME_LEN + 1];
    int r, j;

    // HAL values common to all channels in this instance
    // ...nothing to do here...

    // HAL values for individual channels
    for (j=0; j < hpg->encoder.instance[i].num_channels; j++) {
        // Export HAL Pins
        rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.rawcounts", hpg->config.name, i, j);
        r = hal_pin_s32_new(name, HAL_OUT, &(hpg->encoder.instance[i].chan[j].hal.pin.rawcounts), hpg->config.comp_id);
        if (r < 0) {
            HPG_ERR("error adding pin '%s', aborting\n", name);
            return r;
        }

        rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.rawlatch", hpg->config.name, i, j);
        r = hal_pin_s32_new(name, HAL_OUT, &(hpg->encoder.instance[i].chan[j].hal.pin.rawlatch), hpg->config.comp_id);
        if (r < 0) {
            HPG_ERR("error adding pin '%s', aborting\n", name);
            return r;
        }

        rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.count", hpg->config.name, i, j);
        r = hal_pin_s32_new(name, HAL_OUT, &(hpg->encoder.instance[i].chan[j].hal.pin.count), hpg->config.comp_id);
        if (r < 0) {
            HPG_ERR("error adding pin '%s', aborting\n", name);
            return r;
        }

        rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.count-latched", hpg->config.name, i, j);
        r = hal_pin_s32_new(name, HAL_OUT, &(hpg->encoder.instance[i].chan[j].hal.pin.count_latch), hpg->config.comp_id);
        if (r < 0) {
            HPG_ERR("error adding pin '%s', aborting\n", name);
            return r;
        }

        rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.position", hpg->config.name, i, j);
        r = hal_pin_float_new(name, HAL_OUT, &(hpg->encoder.instance[i].chan[j].hal.pin.position), hpg->config.comp_id);
        if (r < 0) {
            HPG_ERR("error adding pin '%s', aborting\n", name);
            return r;
        }

        rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.position-latched", hpg->config.name, i, j);
        r = hal_pin_float_new(name, HAL_OUT, &(hpg->encoder.instance[i].chan[j].hal.pin.position_latch), hpg->config.comp_id);
        if (r < 0) {
            HPG_ERR("error adding pin '%s', aborting\n", name);
            return r;
        }

        rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.velocity", hpg->config.name, i, j);
        r = hal_pin_float_new(name, HAL_OUT, &(hpg->encoder.instance[i].chan[j].hal.pin.velocity), hpg->config.comp_id);
        if (r < 0) {
            HPG_ERR("error adding pin '%s', aborting\n", name);
            return r;
        }

        rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.reset", hpg->config.name, i, j);
        r = hal_pin_bit_new(name, HAL_IN, &(hpg->encoder.instance[i].chan[j].hal.pin.reset), hpg->config.comp_id);
        if (r < 0) {
            HPG_ERR("error adding pin '%s', aborting\n", name);
            return r;
        }

        rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.index-enable", hpg->config.name, i, j);
        r = hal_pin_bit_new(name, HAL_IO, &(hpg->encoder.instance[i].chan[j].hal.pin.index_enable), hpg->config.comp_id);
        if (r < 0) {
            HPG_ERR("error adding pin '%s', aborting\n", name);
            return r;
        }

        rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.latch-enable", hpg->config.name, i, j);
        r = hal_pin_bit_new(name, HAL_IN, &(hpg->encoder.instance[i].chan[j].hal.pin.latch_enable), hpg->config.comp_id);
        if (r < 0) {
            HPG_ERR("error adding pin '%s', aborting\n", name);
            return r;
        }

        rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.latch-polarity", hpg->config.name, i, j);
        r = hal_pin_bit_new(name, HAL_IN, &(hpg->encoder.instance[i].chan[j].hal.pin.latch_polarity), hpg->config.comp_id);
        if (r < 0) {
            HPG_ERR("error adding pin '%s', aborting\n", name);
            return r;
        }

        rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.quadrature-error", hpg->config.name, i, j);
        r = hal_pin_bit_new(name, HAL_OUT, &(hpg->encoder.instance[i].chan[j].hal.pin.quadrature_error), hpg->config.comp_id);
        if (r < 0) {
            HPG_ERR("error adding pin '%s', aborting\n", name);
            return r;
        }

        // Export HAL Parameters
        rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.scale", hpg->config.name, i, j);
        r = hal_param_float_new(name, HAL_RW, &(hpg->encoder.instance[i].chan[j].hal.param.scale), hpg->config.comp_id);
        if (r < 0) {
            HPG_ERR("error adding param '%s', aborting\n", name);
            return r;
        }

        rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.A-pin", hpg->config.name, i, j);
        r = hal_param_u32_new(name, HAL_RW, &(hpg->encoder.instance[i].chan[j].hal.param.A_pin), hpg->config.comp_id);
        if (r < 0) {
            HPG_ERR("error adding param '%s', aborting\n", name);
            return r;
        }

        rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.A-invert", hpg->config.name, i, j);
        r = hal_param_bit_new(name, HAL_RW, &(hpg->encoder.instance[i].chan[j].hal.param.A_invert), hpg->config.comp_id);
        if (r < 0) {
            HPG_ERR("error adding param '%s', aborting\n", name);
            return r;
        }

        rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.B-pin", hpg->config.name, i, j);
        r = hal_param_u32_new(name, HAL_RW, &(hpg->encoder.instance[i].chan[j].hal.param.B_pin), hpg->config.comp_id);
        if (r < 0) {
            HPG_ERR("error adding param '%s', aborting\n", name);
            return r;
        }

        rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.B-invert", hpg->config.name, i, j);
        r = hal_param_bit_new(name, HAL_RW, &(hpg->encoder.instance[i].chan[j].hal.param.B_invert), hpg->config.comp_id);
        if (r < 0) {
            HPG_ERR("error adding param '%s', aborting\n", name);
            return r;
        }

        rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.index-pin", hpg->config.name, i, j);
        r = hal_param_u32_new(name, HAL_RW, &(hpg->encoder.instance[i].chan[j].hal.param.index_pin), hpg->config.comp_id);
        if (r < 0) {
            HPG_ERR("error adding param '%s', aborting\n", name);
            return r;
        }

        rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.index-invert", hpg->config.name, i, j);
        r = hal_param_bit_new(name, HAL_RW, &(hpg->encoder.instance[i].chan[j].hal.param.index_invert), hpg->config.comp_id);
        if (r < 0) {
            HPG_ERR("error adding param '%s', aborting\n", name);
            return r;
        }

        rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.index-mask", hpg->config.name, i, j);
        r = hal_param_bit_new(name, HAL_RW, &(hpg->encoder.instance[i].chan[j].hal.param.index_mask), hpg->config.comp_id);
        if (r < 0) {
            HPG_ERR("error adding param '%s', aborting\n", name);
            return r;
        }

        rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.index-mask-invert", hpg->config.name, i, j);
        r = hal_param_bit_new(name, HAL_RW, &(hpg->encoder.instance[i].chan[j].hal.param.index_mask_invert), hpg->config.comp_id);
        if (r < 0) {
            HPG_ERR("error adding param '%s', aborting\n", name);
            return r;
        }

        rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.counter-mode", hpg->config.name, i, j);
        r = hal_param_u32_new(name, HAL_RW, &(hpg->encoder.instance[i].chan[j].hal.param.counter_mode), hpg->config.comp_id);
        if (r < 0) {
            HPG_ERR("error adding param '%s', aborting\n", name);
            return r;
        }

        rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.filter", hpg->config.name, i, j);
        r = hal_param_bit_new(name, HAL_RW, &(hpg->encoder.instance[i].chan[j].hal.param.filter), hpg->config.comp_id);
        if (r < 0) {
            HPG_ERR("error adding param '%s', aborting\n", name);
            return r;
        }

        rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.vel-timeout", hpg->config.name, i, j);
        r = hal_param_float_new(name, HAL_RW, &(hpg->encoder.instance[i].chan[j].hal.param.vel_timeout), hpg->config.comp_id);
        if (r < 0) {
            HPG_ERR("error adding param '%s', aborting\n", name);
            return r;
        }

        //
        // init the hal objects that need it
        //

        hpg->encoder.instance[i].chan[j].hal.param.scale = 1.0;
        hpg->encoder.instance[i].chan[j].hal.param.index_invert = 0;
        hpg->encoder.instance[i].chan[j].hal.param.index_mask = 0;
        hpg->encoder.instance[i].chan[j].hal.param.index_mask_invert = 0;
        hpg->encoder.instance[i].chan[j].hal.param.counter_mode = 0;
//      hpg->encoder.instance[i].chan[j].hal.param.filter = 1;
        hpg->encoder.instance[i].chan[j].hal.param.vel_timeout = 0.5;

        *hpg->encoder.instance[i].chan[j].hal.pin.rawcounts = 0;
        *hpg->encoder.instance[i].chan[j].hal.pin.rawlatch = 0;

        *hpg->encoder.instance[i].chan[j].hal.pin.count = 0;
        *hpg->encoder.instance[i].chan[j].hal.pin.count_latch = 0;
        *hpg->encoder.instance[i].chan[j].hal.pin.position = 0.0;
        *hpg->encoder.instance[i].chan[j].hal.pin.position_latch = 0.0;
        *hpg->encoder.instance[i].chan[j].hal.pin.velocity = 0.0;
        *hpg->encoder.instance[i].chan[j].hal.pin.quadrature_error = 0;

        hpg->encoder.instance[i].chan[j].zero_offset = 0;

        hpg->encoder.instance[i].chan[j].prev_reg_count = 0;

        hpg->encoder.instance[i].chan[j].state = HM2_ENCODER_STOPPED;
    }

    return 0;
}
Ejemplo n.º 7
0
int hm2_led_parse_md(hostmot2_t *hm2, int md_index) {

    hm2_module_descriptor_t *md = &hm2->md[md_index];
    int r;

    //
    // some standard sanity checks
    //

    if (!hm2_md_is_consistent_or_complain(hm2, md_index, 0, 1, 4, 0x0000)) {
        HM2_ERR("inconsistent Module Descriptor!\n");
        return -EINVAL;
    }


    // LEDs were enumerated during llio setup
    
    if (hm2->llio->num_leds == 0 || hm2->config.num_leds == 0) return 0;

    if (hm2->config.num_leds > hm2->llio->num_leds) {
        hm2->config.num_leds = hm2->llio->num_leds;
        HM2_ERR( "There are only %d LEDs on this board type, defaulting to %d\n",
                hm2->llio->num_leds, hm2->config.num_leds );
    }
    else if (hm2->config.num_leds == -1) {
        hm2->config.num_leds = hm2->llio->num_leds;
    }

    //
    // looks good, start initializing
    //


    // allocate the module-global HAL shared memory
    hm2->led.instance = (hm2_led_instance_t *)hal_malloc(hm2->config.num_leds * sizeof(hm2_led_instance_t));
    if (hm2->led.instance == NULL) {
        HM2_ERR("out of memory!\n");
        r = -ENOMEM;
        goto fail0;
    }
    hm2->led.led_reg = (u32 *)kmalloc( sizeof(u32), GFP_KERNEL);
    if (hm2->led.led_reg == NULL) {
        HM2_ERR("out of memory!\n");
        r = -ENOMEM;
        goto fail0;
    }

    hm2->led.led_addr = md->base_address;

    // export to HAL
    {
        int i;
        char name[HAL_NAME_LEN+1];
        for (i = 0 ; i < hm2->config.num_leds ; i++) {
            rtapi_snprintf(name, sizeof(name), "%s.led.CR%02d", hm2->llio->name, i + 1 );
            r = hal_pin_bit_new(name, HAL_IN, &(hm2->led.instance[i].led), hm2->llio->comp_id);
            if (r < 0) {
                HM2_ERR("error adding pin '%s', aborting\n", name);
                goto fail1;
            }
        }
        return 1;

    fail1:

        kfree(hm2->led.led_reg);

    fail0:
        return r;

    }
}
Ejemplo n.º 8
0
MarkHal::MarkHal()
{
    int retval;

    /* STEP 1: initialise the hal component */
    comp_id= hal_init("mark");
    if (comp_id< 0) {
        rtapi_print_msg(RTAPI_MSG_ERR, "HALUI: ERROR: hal_init() failed\n");;
        return ;
    }

      /* STEP 2: allocate shared memory for hal pins data */
    halpins= (MarkHalPins *) hal_malloc(sizeof(MarkHalPins));
    if ( halpins== 0) {
        rtapi_print_msg(RTAPI_MSG_ERR, "HALUI: ERROR: hal_malloc() failed\n");
        hal_exit(comp_id);
        return;
    }

    /* STEP 3: export  pins*/
    retval = hal_pin_s32_new("mark.cvCmd", HAL_IO, &halpins->cvCmd, comp_id);
    if (retval != 0)
        return;
    *halpins->cvCmd=0;

    retval = hal_pin_u32_new("mark.ready-index", HAL_IN, &halpins->readyIndex, comp_id);
    if (retval != 0)
        return ;
    *halpins->readyIndex=0;

    retval = hal_pin_u32_new("mark.show", HAL_IO, &halpins->showWin, comp_id);
    if (retval != 0)
        return ;
    *halpins->showWin=0;

    retval = hal_pin_bit_new("mark.clear-result", HAL_IN, &halpins->clearResult, comp_id);
    if (retval != 0)
        return ;
    *halpins->clearResult=0;

    retval = hal_pin_u32_new("mark.posCmd", HAL_IO, &halpins->posCmd, comp_id);
    if (retval != 0)
        return;
    *halpins->posCmd=0;

    retval = hal_pin_float_new("mark.pos-x", HAL_OUT, &halpins->posAxis[0], comp_id);
    if (retval != 0)
        return ;

    retval = hal_pin_float_new("mark.pos-y", HAL_OUT, &halpins->posAxis[1], comp_id);
    if (retval != 0)
        return ;

    retval = hal_pin_float_new("mark.pos-z", HAL_OUT, &halpins->posAxis[2], comp_id);
    if (retval != 0)
        return ;

    retval = hal_pin_float_new("mark.pos-a", HAL_OUT, &halpins->posAxis[3], comp_id);
    if (retval != 0)
        return ;

    retval = hal_pin_float_new("mark.pos-b", HAL_OUT, &halpins->posAxis[4], comp_id);
    if (retval != 0)
        return ;

    retval = hal_pin_float_new("mark.pos-c", HAL_OUT, &halpins->posAxis[5], comp_id);
    if (retval != 0)
        return ;

    retval = hal_pin_u32_new("mark.reachCmd", HAL_IN, &halpins->reachCmd, comp_id);
    if (retval != 0)
        return;
    *halpins->reachCmd=0;

    retval = hal_pin_bit_new("mark.posValid", HAL_OUT, &halpins->posValid, comp_id);
    if (retval != 0)
        return ;
    *halpins->posValid=1;

    retval = hal_pin_bit_new("mark.watchPosValid", HAL_OUT, &halpins->watchPosValid, comp_id);
    if (retval != 0)
        return ;
    *halpins->watchPosValid=1;

    retval = hal_pin_bit_new("mark.watchHoleValid", HAL_OUT, &halpins->watchHoleValid, comp_id);
    if (retval != 0)
        return ;
    *halpins->watchHoleValid=1;

    retval = hal_pin_bit_new("mark.glueHoleValid", HAL_OUT, &halpins->glueHoleValid, comp_id);
    if (retval != 0)
        return ;
    *halpins->glueHoleValid=1;

    retval = hal_pin_bit_new("mark.start", HAL_IN, &halpins->start, comp_id);
    if (retval != 0)
        return ;
    *halpins->start=0;

    retval = hal_pin_bit_new("mark.paused", HAL_IN, &halpins->paused, comp_id);
    if (retval != 0)
        return ;
    *halpins->paused=0;

    retval = hal_pin_bit_new("mark.stop", HAL_IN, &halpins->stop, comp_id);
    if (retval != 0)
        return ;
    *halpins->stop=0;



    retval = hal_pin_s32_new("mark.state", HAL_OUT, &halpins->state, comp_id);
    if (retval != 0)
        return ;
    *halpins->state=MARK_WAITING;

    retval = hal_pin_u32_new("mark.fps", HAL_OUT, &halpins->fps, comp_id);
    if (retval != 0)
        return ;

    const char* ioName[io_num] = {"mark.setGlue","mark.pickupDiamond","mark.dropDiamond",
                                  "mark.lightControl","mark.glueUpDown"};

    for(int i=0; i<5; i++)
    {
        retval = hal_pin_bit_new(ioName[i], HAL_IN, &halpins->ioPin[i], comp_id);
        if (retval != 0)
            return ;
        *halpins->ioPin[i]=0;
    }

    hal_ready(comp_id);

    //run config file
    const char* halcmd="halcmd -i /home/u/cnc/configs/ppmc/ppmc.ini -f /home/u/cnc/configs/ppmc/watchDiamond.hal";
    retval=system(halcmd);
    if (retval != 0) {
        printf("ERROR: %s fail\n", halcmd);
        exit(-1);
    }
}
Ejemplo n.º 9
0
/* init_hal_io() exports HAL pins and parameters making data from
   the realtime control module visible and usable by the world
*/
static int init_hal_io(void)
{
    int n, retval;
    joint_hal_t *joint_data;

    rtapi_print_msg(RTAPI_MSG_INFO, "MOTION: init_hal_io() starting...\n");

    /* allocate shared memory for machine data */
    emcmot_hal_data = hal_malloc(sizeof(emcmot_hal_data_t));
    if (emcmot_hal_data == 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    _("MOTION: emcmot_hal_data malloc failed\n"));
	return -1;
    }

    /* export machine wide hal pins */
    if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->probe_input), mot_comp_id, "motion.probe-input")) < 0) goto error;
    if ((retval = hal_pin_bit_newf(HAL_IO, &(emcmot_hal_data->spindle_index_enable), mot_comp_id, "motion.spindle-index-enable")) < 0) goto error;

    if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->spindle_on), mot_comp_id, "motion.spindle-on")) < 0) goto error;
    if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->spindle_forward), mot_comp_id, "motion.spindle-forward")) < 0) goto error;
    if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->spindle_reverse), mot_comp_id, "motion.spindle-reverse")) < 0) goto error;
    if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->spindle_brake), mot_comp_id, "motion.spindle-brake")) < 0) goto error;
    if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->spindle_speed_out), mot_comp_id, "motion.spindle-speed-out")) < 0) goto error;
    if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->spindle_speed_out_abs), mot_comp_id, "motion.spindle-speed-out-abs")) < 0) goto error;
    if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->spindle_speed_out_rps), mot_comp_id, "motion.spindle-speed-out-rps")) < 0) goto error;
    if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->spindle_speed_out_rps_abs), mot_comp_id, "motion.spindle-speed-out-rps-abs")) < 0) goto error;
    if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->spindle_speed_cmd_rps), mot_comp_id, "motion.spindle-speed-cmd-rps")) < 0) goto error;
    if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->spindle_inhibit), mot_comp_id, "motion.spindle-inhibit")) < 0) goto error;
    *(emcmot_hal_data->spindle_inhibit) = 0;

    // spindle orient pins
    if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->spindle_orient_angle), mot_comp_id, "motion.spindle-orient-angle")) < 0) goto error;
    if ((retval = hal_pin_s32_newf(HAL_OUT, &(emcmot_hal_data->spindle_orient_mode), mot_comp_id, "motion.spindle-orient-mode")) < 0) goto error;
    if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->spindle_orient), mot_comp_id, "motion.spindle-orient")) < 0) goto error;
    if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->spindle_locked), mot_comp_id, "motion.spindle-locked")) < 0) goto error;
    if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->spindle_is_oriented), mot_comp_id, "motion.spindle-is-oriented")) < 0) goto error;
    if ((retval = hal_pin_s32_newf(HAL_IN, &(emcmot_hal_data->spindle_orient_fault), mot_comp_id, "motion.spindle-orient-fault")) < 0) goto error;
    *(emcmot_hal_data->spindle_orient_angle) = 0.0;
    *(emcmot_hal_data->spindle_orient_mode) = 0;
    *(emcmot_hal_data->spindle_orient) = 0;


//    if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->inpos_output), mot_comp_id, "motion.motion-inpos")) < 0) goto error;
    if ((retval = hal_pin_float_newf(HAL_IN, &(emcmot_hal_data->spindle_revs), mot_comp_id, "motion.spindle-revs")) < 0) goto error;
    if ((retval = hal_pin_float_newf(HAL_IN, &(emcmot_hal_data->spindle_speed_in), mot_comp_id, "motion.spindle-speed-in")) < 0) goto error;
    if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->spindle_is_atspeed), mot_comp_id, "motion.spindle-at-speed")) < 0) goto error;
    *emcmot_hal_data->spindle_is_atspeed = 1;
    if ((retval = hal_pin_float_newf(HAL_IN, &(emcmot_hal_data->adaptive_feed), mot_comp_id, "motion.adaptive-feed")) < 0) goto error;
    *(emcmot_hal_data->adaptive_feed) = 1.0;
    if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->feed_hold), mot_comp_id, "motion.feed-hold")) < 0) goto error;
    *(emcmot_hal_data->feed_hold) = 0;
    if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->feed_inhibit), mot_comp_id, "motion.feed-inhibit")) < 0) goto error;
    *(emcmot_hal_data->feed_inhibit) = 0;

    if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->enable), mot_comp_id, "motion.enable")) < 0) goto error;

    /* export motion-synched digital output pins */
    /* export motion digital input pins */
    for (n = 0; n < num_dio; n++) {
	if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->synch_do[n]), mot_comp_id, "motion.digital-out-%02d", n)) < 0) goto error;
	if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->synch_di[n]), mot_comp_id, "motion.digital-in-%02d", n)) < 0) goto error;
    }

    /* export motion analog input pins */
    for (n = 0; n < num_aio; n++) {
	if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->analog_output[n]), mot_comp_id, "motion.analog-out-%02d", n)) < 0) goto error;
	if ((retval = hal_pin_float_newf(HAL_IN, &(emcmot_hal_data->analog_input[n]), mot_comp_id, "motion.analog-in-%02d", n)) < 0) goto error;
    }

    /* export machine wide hal parameters */
    retval =
	hal_pin_bit_new("motion.motion-enabled", HAL_OUT, &(emcmot_hal_data->motion_enabled),
	mot_comp_id);
    if (retval != 0) {
	return retval;
    }
    retval =
	hal_pin_bit_new("motion.in-position", HAL_OUT, &(emcmot_hal_data->in_position),
	mot_comp_id);
    if (retval != 0) {
	return retval;
    }
    retval =
	hal_pin_bit_new("motion.coord-mode", HAL_OUT, &(emcmot_hal_data->coord_mode),
	mot_comp_id);
    if (retval != 0) {
	return retval;
    }
    retval =
	hal_pin_bit_new("motion.teleop-mode", HAL_OUT, &(emcmot_hal_data->teleop_mode),
	mot_comp_id);
    if (retval != 0) {
	return retval;
    }
    retval =
	hal_pin_bit_new("motion.coord-error", HAL_OUT, &(emcmot_hal_data->coord_error),
	mot_comp_id);
    if (retval != 0) {
	return retval;
    }
    retval =
	hal_pin_bit_new("motion.on-soft-limit", HAL_OUT, &(emcmot_hal_data->on_soft_limit),
	mot_comp_id);
    if (retval != 0) {
	return retval;
    }
    retval =
	hal_pin_float_new("motion.current-vel", HAL_OUT, &(emcmot_hal_data->current_vel),
	mot_comp_id);
    if (retval != 0) {
	return retval;
    }
    retval =
	hal_pin_float_new("motion.requested-vel", HAL_OUT, &(emcmot_hal_data->requested_vel),
	mot_comp_id);
    if (retval != 0) {
	return retval;
    }
    retval =
	hal_pin_float_new("motion.distance-to-go", HAL_OUT, &(emcmot_hal_data->distance_to_go),
	mot_comp_id);
    if (retval != 0) {
	return retval;
    }
    retval =
	hal_pin_s32_new("motion.program-line", HAL_OUT, &(emcmot_hal_data->program_line),
	mot_comp_id);
    if (retval != 0) {
	return retval;
    }
    /* export debug parameters */
    /* these can be used to view any internal variable, simply change a line
       in control.c:output_to_hal() and recompile */
    retval =
	hal_param_bit_new("motion.debug-bit-0", HAL_RO, &(emcmot_hal_data->debug_bit_0),
	mot_comp_id);
    if (retval != 0) {
	return retval;
    }
    retval =
	hal_param_bit_new("motion.debug-bit-1", HAL_RO, &(emcmot_hal_data->debug_bit_1),
	mot_comp_id);
    if (retval != 0) {
	return retval;
    }

    retval =
	hal_param_float_new("motion.debug-float-0", HAL_RO, &(emcmot_hal_data->debug_float_0),
	mot_comp_id);
    if (retval != 0) {
	return retval;
    }
    retval =
	hal_param_float_new("motion.debug-float-1", HAL_RO, &(emcmot_hal_data->debug_float_1),
	mot_comp_id);
    if (retval != 0) {
	return retval;
    }

    retval =
	hal_param_float_new("motion.debug-float-2", HAL_RO, &(emcmot_hal_data->debug_float_2),
	mot_comp_id);
    if (retval != 0) {
	return retval;
    }

    retval =
	hal_param_float_new("motion.debug-float-3", HAL_RO, &(emcmot_hal_data->debug_float_3),
	mot_comp_id);
    if (retval != 0) {
	return retval;
    }

    retval =
	hal_param_s32_new("motion.debug-s32-0", HAL_RO, &(emcmot_hal_data->debug_s32_0),
	mot_comp_id);
    if (retval != 0) {
	return retval;
    }
    retval =
	hal_param_s32_new("motion.debug-s32-1", HAL_RO, &(emcmot_hal_data->debug_s32_1),
	mot_comp_id);
    if (retval != 0) {
	return retval;
    }

    // FIXME - debug only, remove later
    // export HAL parameters for some trajectory planner internal variables
    // so they can be scoped
    retval =
	hal_param_float_new("traj.pos_out", HAL_RO, &(emcmot_hal_data->traj_pos_out),
	mot_comp_id);
    if (retval != 0) {
	return retval;
    }
    retval =
	hal_param_float_new("traj.vel_out", HAL_RO, &(emcmot_hal_data->traj_vel_out),
	mot_comp_id);
    if (retval != 0) {
	return retval;
    }
    retval =
	hal_param_u32_new("traj.active_tc", HAL_RO, &(emcmot_hal_data->traj_active_tc),
	mot_comp_id);
    if (retval != 0) {
	return retval;
    }
    for ( n = 0 ; n < 4 ; n++ ) {
	retval = hal_param_float_newf(HAL_RO, &(emcmot_hal_data->tc_pos[n]), mot_comp_id, "tc.%d.pos", n);
	if (retval != 0) {
	    return retval;
	}
	retval = hal_param_float_newf(HAL_RO, &(emcmot_hal_data->tc_vel[n]), mot_comp_id, "tc.%d.vel", n);
	if (retval != 0) {
	    return retval;
	}
	retval = hal_param_float_newf(HAL_RO, &(emcmot_hal_data->tc_acc[n]), mot_comp_id, "tc.%d.acc", n);
	if (retval != 0) {
	    return retval;
	}
    }
    // end of exporting trajectory planner internals

    // export timing related HAL parameters so they can be scoped
    retval =
	hal_param_u32_new("motion.servo.last-period", HAL_RO, &(emcmot_hal_data->last_period), mot_comp_id);
    if (retval != 0) {
	return retval;
    }
#ifdef HAVE_CPU_KHZ
    retval =
	hal_param_float_new("motion.servo.last-period-ns", HAL_RO, &(emcmot_hal_data->last_period_ns), mot_comp_id);
    if (retval != 0) {
	return retval;
    }
#endif
    retval =
	hal_param_u32_new("motion.servo.overruns", HAL_RW, &(emcmot_hal_data->overruns), mot_comp_id);
    if (retval != 0) {
	return retval;
    }

    retval = hal_pin_float_new("motion.tooloffset.x", HAL_OUT, &(emcmot_hal_data->tooloffset_x), mot_comp_id);
    if (retval != 0) {
        return retval;
    }
    retval = hal_pin_float_new("motion.tooloffset.y", HAL_OUT, &(emcmot_hal_data->tooloffset_y), mot_comp_id);
    if (retval != 0) {
        return retval;
    }
    retval = hal_pin_float_new("motion.tooloffset.z", HAL_OUT, &(emcmot_hal_data->tooloffset_z), mot_comp_id);
    if (retval != 0) {
        return retval;
    }
    retval = hal_pin_float_new("motion.tooloffset.a", HAL_OUT, &(emcmot_hal_data->tooloffset_a), mot_comp_id);
    if (retval != 0) {
        return retval;
    }
    retval = hal_pin_float_new("motion.tooloffset.b", HAL_OUT, &(emcmot_hal_data->tooloffset_b), mot_comp_id);
    if (retval != 0) {
        return retval;
    }
    retval = hal_pin_float_new("motion.tooloffset.c", HAL_OUT, &(emcmot_hal_data->tooloffset_c), mot_comp_id);
    if (retval != 0) {
        return retval;
    }
    retval = hal_pin_float_new("motion.tooloffset.u", HAL_OUT, &(emcmot_hal_data->tooloffset_u), mot_comp_id);
    if (retval != 0) {
        return retval;
    }
    retval = hal_pin_float_new("motion.tooloffset.v", HAL_OUT, &(emcmot_hal_data->tooloffset_v), mot_comp_id);
    if (retval != 0) {
        return retval;
    }
    retval = hal_pin_float_new("motion.tooloffset.w", HAL_OUT, &(emcmot_hal_data->tooloffset_w), mot_comp_id);
    if (retval != 0) {
        return retval;
    }

    /* initialize machine wide pins and parameters */
    *(emcmot_hal_data->probe_input) = 0;
    /* default value of enable is TRUE, so simple machines
       can leave it disconnected */
    *(emcmot_hal_data->enable) = 1;
    
    /* motion synched dio, init to not enabled */
    for (n = 0; n < num_dio; n++) {
	 *(emcmot_hal_data->synch_do[n]) = 0;
	 *(emcmot_hal_data->synch_di[n]) = 0;
    }

    for (n = 0; n < num_aio; n++) {
	 *(emcmot_hal_data->analog_output[n]) = 0.0;
	 *(emcmot_hal_data->analog_input[n]) = 0.0;
    }
    
    /*! \todo FIXME - these don't really need initialized, since they are written
       with data from the emcmotStatus struct */
    *(emcmot_hal_data->motion_enabled) = 0;
    *(emcmot_hal_data->in_position) = 0;
    *(emcmot_hal_data->coord_mode) = 0;
    *(emcmot_hal_data->teleop_mode) = 0;
    *(emcmot_hal_data->coord_error) = 0;
    *(emcmot_hal_data->on_soft_limit) = 0;

    /* init debug parameters */
    emcmot_hal_data->debug_bit_0 = 0;
    emcmot_hal_data->debug_bit_1 = 0;
    emcmot_hal_data->debug_float_0 = 0.0;
    emcmot_hal_data->debug_float_1 = 0.0;
    emcmot_hal_data->debug_float_2 = 0.0;
    emcmot_hal_data->debug_float_3 = 0.0;

    emcmot_hal_data->overruns = 0;
    emcmot_hal_data->last_period = 0;

    /* export joint pins and parameters */
    for (n = 0; n < num_joints; n++) {
	/* point to axis data */
	joint_data = &(emcmot_hal_data->joint[n]);
	/* export all vars */
        retval = export_joint(n, joint_data);
	if (retval != 0) {
	    rtapi_print_msg(RTAPI_MSG_ERR,
		_("MOTION: joint %d pin/param export failed\n"), n);
	    return -1;
	}
	/* init axis pins and parameters */
	/* FIXME - struct members are in a state of flux - make sure to
	   update this - most won't need initing anyway */
	*(joint_data->amp_enable) = 0;
	*(joint_data->home_state) = 0;
	/* We'll init the index model to EXT_ENCODER_INDEX_MODEL_RAW for now,
	   because it is always supported. */
    }
    /* Done! */
    rtapi_print_msg(RTAPI_MSG_INFO,
	"MOTION: init_hal_io() complete, %d axes.\n", n);
    return 0;

    error:
	return retval;

}
Ejemplo n.º 10
0
int hm2_resolver_parse_md(hostmot2_t *hm2, int md_index) {
    hm2_module_descriptor_t *md = &hm2->md[md_index];
    int i, r = 0;
    int resolvers_per_instance;
    
    //
    // some standard sanity checks
    //
    
    if ( ! hm2_md_is_consistent_or_complain(hm2, md_index, 0, 5, 4, 0x001F)) {
        HM2_ERR("inconsistent resolver Module Descriptor!\n");
        return -EINVAL;
    }
    
    if (hm2->resolver.num_instances != 0) {
        HM2_ERR(
                "found duplicate Module Descriptor for %s (inconsistent firmware), not loading driver\n",
                hm2_get_general_function_name(md->gtag)
                );
        return -EINVAL;
    }
    
    resolvers_per_instance = hm2_resolver_get_param(2); // just returns 6 at the moment
    
    if (hm2->config.num_resolvers > (md->instances * resolvers_per_instance)) {
        HM2_ERR(
                "config.num_resolvers=%d, but only %d are available, not loading driver\n",
                hm2->config.num_resolvers,
                md->instances * resolvers_per_instance);
        return -EINVAL;
    }
    
    if (hm2->config.num_resolvers == 0) {
        return 0;
    }
    
    
    //
    // looks good, start initializing
    //
    
    /*At the moment it is not clear if there will ever be more than one resolver
     instance. If there were to be more than one then they would need to have
     a different base-address, and this code would need to be re-enterable.
     A bridge to cross when we come to it */
    
    if (hm2->config.num_resolvers == -1) {
        hm2->resolver.num_resolvers = md->instances * resolvers_per_instance;
        hm2->resolver.num_instances = md->instances;
    } else {
        hm2->resolver.num_resolvers = hm2->config.num_resolvers;
        hm2->resolver.num_instances = md->instances;
    }
    
    hm2->resolver.hal = (hm2_resolver_global_t *)hal_malloc(
                                                sizeof(hm2_resolver_global_t));
    if (hm2->resolver.hal == NULL) {
        HM2_ERR("out of memory!\n");
        r = -ENOMEM;
        goto fail0;
    }
    hm2->resolver.instance = (hm2_resolver_instance_t *)hal_malloc(
                hm2->resolver.num_resolvers * sizeof(hm2_resolver_instance_t));
    if (hm2->resolver.instance == NULL) {
        HM2_ERR("out of memory!\n");
        r = -ENOMEM;
        goto fail0;
    }
    
    for (i = 0 ; i < hm2->resolver.num_instances ; i++ ){
        hm2->resolver.stride = md->register_stride;
        hm2->resolver.clock_frequency = md->clock_freq;
        hm2->resolver.version = md->version;
        
        hm2->resolver.command_addr = md->base_address + (0 * md->register_stride);
        hm2->resolver.data_addr = md->base_address + (1 * md->register_stride);
        hm2->resolver.status_addr = md->base_address + (2 * md->register_stride);
        hm2->resolver.velocity_addr = md->base_address + (3 * md->register_stride);
        hm2->resolver.position_addr = md->base_address + (4 * md->register_stride);
        
        // If there were multiple resolver function instances, this would need
        // to be the number of resolvers for that particular instance
        r = hm2_register_tram_read_region(hm2, hm2->resolver.status_addr,
                                          sizeof(u32),
                                          &hm2->resolver.status_reg);
        r += hm2_register_tram_read_region(hm2, hm2->resolver.position_addr,
                                          (hm2->resolver.num_resolvers * sizeof(u32)),
                                          &hm2->resolver.position_reg);
        r += hm2_register_tram_read_region(hm2, hm2->resolver.velocity_addr,
                                          (hm2->resolver.num_resolvers * sizeof(u32)),
                                          (u32**)&hm2->resolver.velocity_reg);
        if (r < 0) {
            HM2_ERR("error registering tram read region for Resolver "
                    "register (%d)\n", i);
            goto fail1;
        }
        
    }
    
    // export the resolvers to HAL
    
    {
        int i;
        int ret;
        char name[HAL_NAME_LEN + 1];
        
        rtapi_snprintf(name, sizeof(name), "%s.resolver.excitation-khz", 
                       hm2->llio->name);
        ret= hal_pin_float_new(name, HAL_IO,
                                 &(hm2->resolver.hal->pin.excitation_khz),
                                 hm2->llio->comp_id);
        if (ret < 0) {
            HM2_ERR("error adding pin '%s', aborting\n", name);
            goto fail1;
        }
        
        for (i = 0; i < hm2->resolver.num_resolvers; i ++) {
            // pins
            
            rtapi_snprintf(name, sizeof(name), "%s.resolver.%02d.position", 
                           hm2->llio->name, i);
            ret= hal_pin_float_new(name, HAL_OUT, 
                                   &(hm2->resolver.instance[i].hal.pin.position),
                                   hm2->llio->comp_id);
            if (ret < 0) {
                HM2_ERR("error adding pin '%s', aborting\n", name);
                goto fail1;
            }
            
            rtapi_snprintf(name, sizeof(name), "%s.resolver.%02d.angle", 
                           hm2->llio->name, i);
            ret= hal_pin_float_new(name, HAL_OUT, 
                                   &(hm2->resolver.instance[i].hal.pin.angle), 
                                   hm2->llio->comp_id);
            if (ret < 0) {
                HM2_ERR("error adding pin '%s', aborting\n", name);
                goto fail1;
            }
            
            rtapi_snprintf(name, sizeof(name), "%s.resolver.%02d.velocity", 
                           hm2->llio->name, i);
            ret= hal_pin_float_new(name, HAL_OUT, 
                                   &(hm2->resolver.instance[i].hal.pin.velocity), 
                                   hm2->llio->comp_id);
            if (ret < 0) {
                HM2_ERR("error adding pin '%s', aborting\n", name);
                goto fail1;
            }
            
            rtapi_snprintf(name, sizeof(name), "%s.resolver.%02d.count", 
                           hm2->llio->name, i);
            ret= hal_pin_s32_new(name, HAL_OUT, 
                                 &(hm2->resolver.instance[i].hal.pin.count), 
                                 hm2->llio->comp_id);
            if (ret < 0) {
                HM2_ERR("error adding pin '%s', aborting\n", name);
                goto fail1;
            }
            
            rtapi_snprintf(name, sizeof(name), "%s.resolver.%02d.rawcounts",
                           hm2->llio->name, i);
            ret= hal_pin_s32_new(name, HAL_OUT, 
                                 &(hm2->resolver.instance[i].hal.pin.rawcounts), 
                                 hm2->llio->comp_id);
            if (ret < 0) {
                HM2_ERR("error adding pin '%s', aborting\n", name);
                goto fail1;
            }
            
            rtapi_snprintf(name, sizeof(name), "%s.resolver.%02d.reset", 
                           hm2->llio->name, i);
            ret= hal_pin_bit_new(name, HAL_IN, 
                                 &(hm2->resolver.instance[i].hal.pin.reset), 
                                 hm2->llio->comp_id);
            if (ret < 0) {
                HM2_ERR("error adding pin '%s', aborting\n", name);
                goto fail1;
            }
            
            rtapi_snprintf(name, sizeof(name), "%s.resolver.%02d.index-enable", 
                           hm2->llio->name, i);
            ret= hal_pin_bit_new(name, HAL_IO,
                                 &(hm2->resolver.instance[i].hal.pin.index_enable), 
                                 hm2->llio->comp_id);
            if (ret < 0) {
                HM2_ERR("error adding pin '%s', aborting\n", name);
                goto fail1;
            }
            
            rtapi_snprintf(name, sizeof(name), "%s.resolver.%02d.error", 
                           hm2->llio->name, i);
            ret= hal_pin_bit_new(name, HAL_OUT, 
                                 &(hm2->resolver.instance[i].hal.pin.error), 
                                 hm2->llio->comp_id);
            if (ret < 0) {
                HM2_ERR("error adding pin '%s', aborting\n", name);
                goto fail1;
            }
            
            
            // parameters
            rtapi_snprintf(name, sizeof(name), "%s.resolver.%02d.scale", 
                           hm2->llio->name, i);
            ret= hal_pin_float_new(name, HAL_IO,
                                     &(hm2->resolver.instance[i].hal.pin.scale),
                                     hm2->llio->comp_id);
            if (ret < 0) {
                HM2_ERR("error adding pin '%s', aborting\n", name);
                goto fail1;
            }
            
            rtapi_snprintf(name, sizeof(name), "%s.resolver.%02d.velocity-scale", 
                           hm2->llio->name, i);
            ret= hal_pin_float_new(name, HAL_IO,
                                     &(hm2->resolver.instance[i].hal.pin.vel_scale),
                                     hm2->llio->comp_id);
            if (ret < 0) {
                HM2_ERR("error adding pin '%s', aborting\n", name);
                goto fail1;
            }

            rtapi_snprintf(name, sizeof(name), "%s.resolver.%02d.index-divisor",
                           hm2->llio->name, i);
            ret= hal_pin_u32_new(name, HAL_RW,
                                     &(hm2->resolver.instance[i].hal.pin.index_div),
                                     hm2->llio->comp_id);
            if (ret < 0) {
                HM2_ERR("error adding pin '%s', aborting\n", name);
                goto fail1;
            }

            //
            // init the hal objects that need it
            // the things not initialized here will be set by hm2_resolver_tram_init()
            //
            
            *hm2->resolver.instance[i].hal.pin.reset = 0;
            *hm2->resolver.instance[i].hal.pin.scale = 1.0;
            *hm2->resolver.instance[i].hal.pin.vel_scale = 1.0;
            *hm2->resolver.instance[i].hal.pin.index_div = 1;
            *hm2->resolver.hal->pin.excitation_khz = -1; // don't-write
            hm2->resolver.kHz = (hm2->resolver.clock_frequency / 5000);
        }
    }
    
    
    return hm2->resolver.num_instances;
    
fail1:
    // This is where we would kfree anything kmalloced. 
    
    
fail0:
    hm2->resolver.num_instances = 0;
    return r;
}
Ejemplo n.º 11
0
int rtapi_app_main(void)
{
    int retval, i;
    int in_cnt = 0, out_cnt = 0;
    char buf[128];
    char *pc = axes_conf;

    /* Parsing axes configuration */
    while(*pc != 0)
    {
        int idx = -1;
        switch(*pc)
        {
            case 'X':
            case 'x':
                    idx = 0;
                    break;
            case 'Y':
            case 'y':
                    idx = 1;
                    break;

            case 'Z':
            case 'z':
                    idx = 2;
                    break;

            case 'A':
            case 'a':
                    idx = 3;
                    break;

            case 'B':
            case 'b':
                    idx = 4;
                    break;

            case 'C':
            case 'c':
                    idx = 5;
                    break;

            default: break;
        }
        if(idx >= 0)
            axis_map[num_axis++] = idx;
        pc++;
    }

    fprintf(stderr, "num_axis=%d, fifo_size=%d\n", num_axis, fifo_deep);

    /* test for number of channels */
    if ((num_axis <= 0) || (num_axis > MAX_AXIS)) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "miniemcdrv: ERROR: invalid num_chan: %d\n", num_axis);
	return -EINVAL;
    }
    /* have good config info, connect to the HAL */
    comp_id = hal_init("miniemcdrv");
    if (comp_id < 0)
    {
        rtapi_print_msg(RTAPI_MSG_ERR, "miniemcdrv: ERROR: hal_init() failed\n");
        rtapi_app_exit();
        return -EINVAL;
    }
    pgpio = hal_malloc(sizeof(gpio_t));
    memset(pgpio, 0, sizeof(gpio_t));

    pgpio->fd = open("/dev/miniemc", O_RDWR | O_SYNC);

    if(pgpio->fd < 0)
    {
        rtapi_print_msg(RTAPI_MSG_ERR,
            "miniemcdrv: ERROR: unble to create access to stepgen module\n");
        rtapi_app_exit();
        return -EIO;
    }

    pgpio->pfiq = mmap(0, sizeof(struct fiq_ipc_shared), PROT_READ | PROT_WRITE, MAP_FILE | MAP_SHARED, pgpio->fd, 0);
	if(pgpio->pfiq == MAP_FAILED)
	{
        rtapi_print_msg(RTAPI_MSG_ERR,
            "miniemcdrv: ERROR: unable to mmap stepgen ringbuffer\n");
        rtapi_app_exit();
        return -EIO;
	}

     /* Setup ringbuff size */
    fiq_static.rb_size = fifo_deep;

    memset(&cmd_pos_prev, 0, sizeof(cmd_pos_prev));
    memset(&cmd_pos_accum, 0, sizeof(cmd_pos_accum));

    //Configure PWM pins and create HAL inputs
    for( i = 0; i < MAX_PWM; i++)
    {
        rtapi_snprintf(buf, HAL_NAME_LEN, "miniemcdrv.%d.pwm-in", i);
        retval = hal_pin_float_new(buf, HAL_IN, &(pgpio->pwm_duty[i]), comp_id);
        if (retval != 0)
        {
            rtapi_app_exit();
            return retval;
        }

        if( pwm_pin_num[i] >= 0 )
        {
            emcConfigureDefault( pwm_pin_num[i] );
            emcReservePin( pwm_pin_num[i] );
           fiq_static.pwm_pin_addr[i] =  GPIOS[pwm_pin_num[i]].port_index;
           fiq_static.pwm_pin_mask[i] =  1L << GPIOS[pwm_pin_num[i]].offset;
           pgpio->pfiq->pwm_duty_cycle[i] = 0;
        } else
        {
            fiq_static.pwm_pin_mask[i] = 0;
            fiq_static.pwm_pin_addr[i] = 0;
        }
    }

    // Create axis step and dir pins
    for(i = 0; i < num_axis; i++)
    {
        if(step_pins[i] >=0 && dir_pins[i] >= 0)
        {
            if( emcGetPinMode( step_pins[i]) == egpIn ||  emcGetPinMode( dir_pins[i]) == egpIn )
            {
                rtapi_print_msg(RTAPI_MSG_ERR, "WARN: can't create axis[%d] stepgen, invalid pin\n", i);
                continue;
            }
            fiq_static.axis[i].configured = 0;
            fiq_static.axis[i].step_pin_addr = GPIOS[step_pins[i]].port_index;
            fiq_static.axis[i].step_pin_mask = 1L << GPIOS[step_pins[i]].offset;
            emcConfigureDefault( step_pins[i] );
            emcReservePin( step_pins[i] );
            fiq_static.axis[i].dir_pin_addr = GPIOS[dir_pins[i]].port_index;
            fiq_static.axis[i].dir_pin_mask = 1L << GPIOS[dir_pins[i]].offset;
            emcConfigureDefault( dir_pins[i] );
            emcReservePin( dir_pins[i] );
            fiq_static.axis[i].dir_pin_pol = dir_polarity[i];
            fiq_static.axis[i].configured = 1;
        } else
        {
            rtapi_print_msg(RTAPI_MSG_ERR,
                "miniemcdrv: WARNING: axis[%d] step and/or dir pin(s) not properly configured, skipping\n", i);
        }
        fiq_static.scan_pin_num = -1;
    }

    ioctl(pgpio->fd, AXIS_SET_IOCTL, &fiq_static );
    /*
     * Create IO pins
     */

    for( i=0; i < ARR_SZ(GPIOS); i++ )
    {
        if(  emcGetPinMode( i ) == egpRsv )
            continue;
        if( emcGetPinMode( i ) == egpIn )
        {
            rtapi_snprintf(buf, HAL_NAME_LEN, "miniemcdrv.%d.pin-in", in_cnt);
            hal_pin_bit_new(buf, HAL_OUT, &(pgpio->io_pin[i]), comp_id);
            rtapi_snprintf(buf, HAL_NAME_LEN, "miniemcdrv.%d.pin-in-inv", in_cnt);
            hal_pin_bit_new(buf, HAL_IN, &(pgpio->io_invert[i]), comp_id);
            in_cnt++;
        } else
        {
            rtapi_snprintf(buf, HAL_NAME_LEN, "miniemcdrv.%d.pin-out", out_cnt);
            hal_pin_bit_new(buf, HAL_IN, &(pgpio->io_pin[i]), comp_id);
            rtapi_snprintf(buf, HAL_NAME_LEN, "miniemcdrv.%d.pin-out-inv", out_cnt);
            hal_pin_bit_new(buf, HAL_IN, &(pgpio->io_invert[i]), comp_id);
            out_cnt++;
        }
        emcConfigureDefault( i );
    }

	// Trajectory wait output
	rtapi_snprintf(buf, HAL_NAME_LEN, "miniemcdrv.traj-wait-out");
	hal_pin_bit_new(buf, HAL_OUT, &(pgpio->traj_wait), comp_id);
    *(pgpio->traj_wait) = 1;

	// Scaner sync
	rtapi_snprintf(buf, HAL_NAME_LEN, "miniemcdrv.scan-sync-in");
	hal_pin_bit_new(buf, HAL_IN, &(pgpio->scan_sync), comp_id);

    for(i=0; i < num_axis; i++)
    {
      // Check if pin already added
      char contin = 0;
      int j;
      for(j=0; j < i; j++)
        if(axis_map[j] == axis_map[i])
        {
            contin = 1;
            break;
        }
      if(contin) continue;

  	  // commanded position pin
	  rtapi_snprintf(buf, HAL_NAME_LEN, "miniemcdrv.%d.cmd-pos", axis_map[i]);

  	  retval = hal_pin_float_new(buf, HAL_IN, &(pgpio->cmd_pos[axis_map[i]]), comp_id);
	  if (retval != 0)
	  {
        rtapi_app_exit();
		return retval;
	  }
	  //feedback position pin
	  rtapi_snprintf(buf, HAL_NAME_LEN, "miniemcdrv.%d.fb-pos", axis_map[i]);
  	  retval = hal_pin_float_new(buf, HAL_OUT, &(pgpio->fb_pos[axis_map[i]]), comp_id);
	  if (retval != 0)
	  {
        rtapi_app_exit();
		return retval;
	  }
    }



/* export functions */
    retval = hal_export_funct("update-miniemcdrv", update,
                                pgpio, 0, 0, comp_id);
    if (retval != 0)
    {
	rtapi_print_msg(RTAPI_MSG_ERR,
            "miniemcdrv: ERROR: count funct export failed\n");
        rtapi_app_exit();
        return -EIO;
    }

    ioctl(pgpio->fd, SCAN_PIN_SETUP_IOCTL, NULL);

    //emcConfigurePin(11, egpOut );
    //emcSetPin(11, 1 );

    hal_ready(comp_id);
    return 0;
}
Ejemplo n.º 12
0
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_RO, &(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_OUT, &(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_OUT, &(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_RW, &(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_IN, &(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_RW, &(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_RW, &(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_RO, &(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_RW, &(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;
    addr->printed_error = 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_u32_new(buf, HAL_RW, &(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_u32_new(buf, HAL_RW, &(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_u32_new(buf, HAL_RW, &(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_u32_new(buf, HAL_RW, &(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_OUT, &(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_OUT, &(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_OUT, &(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_OUT, &(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_OUT, &(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 = &(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_OUT, &(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;
}
Ejemplo n.º 13
0
int hm2_pwmgen_parse_md(hostmot2_t *hm2, int md_index) {
    hm2_module_descriptor_t *md = &hm2->md[md_index];
    int r;


    // 
    // some standard sanity checks
    //

    if (!hm2_md_is_consistent_or_complain(hm2, md_index, 0, 5, 4, 0x0003)) {
        HM2_ERR("inconsistent Module Descriptor!\n");
        return -EINVAL;
    }

    if (hm2->pwmgen.num_instances != 0) {
        HM2_ERR(
            "found duplicate Module Descriptor for %s (inconsistent firmware), not loading driver\n",
            hm2_get_general_function_name(md->gtag)
        );
        return -EINVAL;
    }

    if (hm2->config.num_pwmgens > md->instances) {
        HM2_ERR(
            "config.num_pwmgens=%d, but only %d are available, not loading driver\n",
            hm2->config.num_pwmgens,
            md->instances
        );
        return -EINVAL;
    }

    if (hm2->config.num_pwmgens == 0) {
        return 0;
    }


    // 
    // looks good, start initializing
    // 


    if (hm2->config.num_pwmgens == -1) {
        hm2->pwmgen.num_instances = md->instances;
    } else {
        hm2->pwmgen.num_instances = hm2->config.num_pwmgens;
    }


    // allocate the module-global HAL shared memory
    hm2->pwmgen.hal = (hm2_pwmgen_module_global_t *)hal_malloc(sizeof(hm2_pwmgen_module_global_t));
    if (hm2->pwmgen.hal == NULL) {
        HM2_ERR("out of memory!\n");
        r = -ENOMEM;
        goto fail0;
    }


    hm2->pwmgen.instance = (hm2_pwmgen_instance_t *)hal_malloc(hm2->pwmgen.num_instances * sizeof(hm2_pwmgen_instance_t));
    if (hm2->pwmgen.instance == NULL) {
        HM2_ERR("out of memory!\n");
        r = -ENOMEM;
        goto fail0;
    }

    hm2->pwmgen.clock_frequency = md->clock_freq;
    hm2->pwmgen.version = md->version;

    hm2->pwmgen.pwm_value_addr = md->base_address + (0 * md->register_stride);
    hm2->pwmgen.pwm_mode_addr = md->base_address + (1 * md->register_stride);
    hm2->pwmgen.pwmgen_master_rate_dds_addr = md->base_address + (2 * md->register_stride);
    hm2->pwmgen.pdmgen_master_rate_dds_addr = md->base_address + (3 * md->register_stride);
    hm2->pwmgen.enable_addr = md->base_address + (4 * md->register_stride);

    r = hm2_register_tram_write_region(hm2, hm2->pwmgen.pwm_value_addr, (hm2->pwmgen.num_instances * sizeof(rtapi_u32)), &hm2->pwmgen.pwm_value_reg);
    if (r < 0) {
        HM2_ERR("error registering tram write region for PWM Value register (%d)\n", r);
        goto fail0;
    }

    hm2->pwmgen.pwm_mode_reg = (rtapi_u32 *)rtapi_kmalloc(hm2->pwmgen.num_instances * sizeof(rtapi_u32), RTAPI_GFP_KERNEL);
    if (hm2->pwmgen.pwm_mode_reg == NULL) {
        HM2_ERR("out of memory!\n");
        r = -ENOMEM;
        goto fail0;
    }

    // export to HAL
    // FIXME: r hides the r in enclosing function, and it returns the wrong thing
    {
        int i;
        int r;
        char name[HAL_NAME_LEN + 1];


        // these hal parameters affect all pwmgen instances
        r = hal_param_u32_newf(
            HAL_RW,
            &(hm2->pwmgen.hal->param.pwm_frequency),
            hm2->llio->comp_id,
            "%s.pwmgen.pwm_frequency",
            hm2->llio->name
        );
        if (r < 0) {
            HM2_ERR("error adding pwmgen.pwm_frequency param, aborting\n");
            goto fail1;
        }
        hm2->pwmgen.hal->param.pwm_frequency = 20000;
        hm2->pwmgen.written_pwm_frequency = 0;

        r = hal_param_u32_newf(
            HAL_RW,
            &(hm2->pwmgen.hal->param.pdm_frequency),
            hm2->llio->comp_id,
            "%s.pwmgen.pdm_frequency",
            hm2->llio->name
        );
        if (r < 0) {
            HM2_ERR("error adding pwmgen.pdm_frequency param, aborting\n");
            goto fail1;
        }
        hm2->pwmgen.hal->param.pdm_frequency = 20000;
        hm2->pwmgen.written_pdm_frequency = 0;


        for (i = 0; i < hm2->pwmgen.num_instances; i ++) {
            // pins
            rtapi_snprintf(name, sizeof(name), "%s.pwmgen.%02d.value", hm2->llio->name, i);
            r = hal_pin_float_new(name, HAL_IN, &(hm2->pwmgen.instance[i].hal.pin.value), hm2->llio->comp_id);
            if (r < 0) {
                HM2_ERR("error adding pin '%s', aborting\n", name);
                goto fail1;
            }

            rtapi_snprintf(name, sizeof(name), "%s.pwmgen.%02d.enable", hm2->llio->name, i);
            r = hal_pin_bit_new(name, HAL_IN, &(hm2->pwmgen.instance[i].hal.pin.enable), hm2->llio->comp_id);
            if (r < 0) {
                HM2_ERR("error adding pin '%s', aborting\n", name);
                goto fail1;
            }

            // parameters

            rtapi_snprintf(name, sizeof(name), "%s.pwmgen.%02d.scale", hm2->llio->name, i);
            r = hal_param_float_new(name, HAL_RW, &(hm2->pwmgen.instance[i].hal.param.scale), hm2->llio->comp_id);
            if (r < 0) {
                HM2_ERR("error adding param '%s', aborting\n", name);
                goto fail1;
            }

            r = hal_param_s32_newf(
                HAL_RW,
                &(hm2->pwmgen.instance[i].hal.param.output_type),
                hm2->llio->comp_id,
                "%s.pwmgen.%02d.output-type",
                hm2->llio->name,
                i
            );
            if (r < 0) {
                HM2_ERR("error adding param, aborting\n");
                goto fail1;
            }

            // init hal objects
            *(hm2->pwmgen.instance[i].hal.pin.enable) = 0;
            *(hm2->pwmgen.instance[i].hal.pin.value) = 0.0;
            hm2->pwmgen.instance[i].hal.param.scale = 1.0;
            hm2->pwmgen.instance[i].hal.param.output_type = HM2_PWMGEN_OUTPUT_TYPE_PWM;

            hm2->pwmgen.instance[i].written_output_type = -666;  // force an update at the start
            hm2->pwmgen.instance[i].written_enable = -666;       // force an update at the start
        }
    }


    return hm2->pwmgen.num_instances;


fail1:
    rtapi_kfree(hm2->pwmgen.pwm_mode_reg);

fail0:
    hm2->pwmgen.num_instances = 0;
    return r;
}
static int export_wsum(int num, int num_bits, wsum_t *addr, wsum_bit_t *bitaddr)
{
    int retval, i, w;
    char buf[HAL_NAME_LEN+1], base[HAL_NAME_LEN+1];

    rtapi_snprintf(base, sizeof(base), "wsum.%d", num);
    /* export pin for offset (input) */
    rtapi_snprintf(buf, sizeof(buf), "%s.offset", base);
    retval = hal_pin_s32_new(buf, HAL_IO, &(addr->offset), comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "WEIGHTED_SUM: ERROR: '%s' param export failed\n", buf);
	return retval;
    }

    /* export pin for output sum */
    rtapi_snprintf(buf, sizeof(buf), "%s.sum", base);
    retval = hal_pin_s32_new(buf, HAL_OUT, &(addr->sum), comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "WEIGHTED_SUM: ERROR: '%s' pin export failed\n", buf);
	return retval;
    }

    /* export pin for update hold */
    rtapi_snprintf(buf, sizeof(buf), "%s.hold", base);
    retval = hal_pin_bit_new(buf, HAL_IN, &(addr->hold), comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "WEIGHTED_SUM: ERROR: '%s' pin export failed\n", buf);
	return retval;
    }

    addr->bits = bitaddr;
    addr->num_bits = num_bits;
    /* export the input bits and weight parameters, and set the default weights */
    w = 1;
    for (i=0;i<num_bits;i++) {
	rtapi_snprintf(buf, sizeof(buf), "%s.bit.%d.in", base, i);
	retval = hal_pin_bit_new(buf, HAL_IN, &(addr->bits[i].bit), comp_id);
	if (retval != 0) {
	    rtapi_print_msg(RTAPI_MSG_ERR,
	    "WEIGHTED_SUM: ERROR: '%s' pin export failed\n", buf);
	    return retval;
	}
	rtapi_snprintf(buf, sizeof(buf), "%s.bit.%d.weight", base, i);
	retval = hal_pin_s32_new(buf, HAL_IO, &(addr->bits[i].weight), comp_id);
	if (retval != 0) {
	    rtapi_print_msg(RTAPI_MSG_ERR,
	    "WEIGHTED_SUM: ERROR: '%s' param export failed\n", buf);
	    return retval;
	}
	*(addr->bits[i].weight) = w;
	w <<= 1;
    }

    /* set initial parameter and pin values */
    *(addr->offset) = 0;
    *(addr->sum) = 0;
    return 0;
}
Ejemplo n.º 15
0
static int export_encoder_pair(int num, encoder_pair_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-ratio.%d.master-A", num);
    retval = hal_pin_bit_new(buf, HAL_IN, &(addr->master_A), comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "encoder-ratio.%d.master-B", num);
    retval = hal_pin_bit_new(buf, HAL_IN, &(addr->master_B), comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "encoder-ratio.%d.slave-A", num);
    retval = hal_pin_bit_new(buf, HAL_IN, &(addr->slave_A), comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "encoder-ratio.%d.slave-B", num);
    retval = hal_pin_bit_new(buf, HAL_IN, &(addr->slave_B), comp_id);
    if (retval != 0) {
	return retval;
    }
    /* export pin for the enable input */
    rtapi_snprintf(buf, HAL_NAME_LEN, "encoder-ratio.%d.enable", num);
    retval = hal_pin_bit_new(buf, HAL_IN, &(addr->enable), comp_id);
    if (retval != 0) {
	return retval;
    }
    /* export pin for output */
    rtapi_snprintf(buf, HAL_NAME_LEN, "encoder-ratio.%d.error", num);
    retval = hal_pin_float_new(buf, HAL_OUT, &(addr->error), comp_id);
    if (retval != 0) {
	return retval;
    }
    /* export pins for config info() */
    rtapi_snprintf(buf, HAL_NAME_LEN, "encoder-ratio.%d.master-ppr", num);
    retval = hal_pin_u32_new(buf, HAL_IO, &(addr->master_ppr), comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "encoder-ratio.%d.slave-ppr", num);
    retval = hal_pin_u32_new(buf, HAL_IO, &(addr->slave_ppr), comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "encoder-ratio.%d.master-teeth", num);
    retval = hal_pin_u32_new(buf, HAL_IO, &(addr->master_teeth), comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "encoder-ratio.%d.slave-teeth", num);
    retval = hal_pin_u32_new(buf, HAL_IO, &(addr->slave_teeth), comp_id);
    if (retval != 0) {
	return retval;
    }
    /* restore saved message level */
    rtapi_set_msg_level(msg);
    return 0;
}
Ejemplo n.º 16
0
int main(int argc, char* argv[]) {
    if (argc == 1) {
        logfile = stdout;
        logfile_name = NULL;
    } else if (argc == 2) {
        logfile = NULL;
        logfile_name = argv[1];
    } else {
        fprintf(stderr, "usage: motion-logger [LOGFILE]\n");
        exit(1);
    }

    mot_comp_id = hal_init("motion-logger");
    motion_logger_data = hal_malloc(sizeof(*motion_logger_data));
    int r = hal_pin_bit_new("motion-logger.reopen-log", HAL_IO, &motion_logger_data->reopen,
            mot_comp_id);
    if(r < 0) { errno = -r; perror("hal_pin_bit_new"); exit(1); }
    *motion_logger_data->reopen = 0;
    r = hal_ready(mot_comp_id);
    if(r < 0) { errno = -r; perror("hal_ready"); exit(1); }
    init_comm_buffers();

    while (1) {
        if (c->commandNum != c->tail) {
            // "split read"
            continue;
        }
        if (c->commandNum == emcmotStatus->commandNumEcho) {
            // nothing new
            maybe_reopen_logfile();
            usleep(10 * 1000);
            continue;
        }

        //
        // new incoming command!
        //

        emcmotStatus->head++;

        switch (c->command) {
            case EMCMOT_ABORT:
                log_print("ABORT\n");
                break;

            case EMCMOT_AXIS_ABORT:
                log_print("AXIS_ABORT joint=%d\n", c->axis);
                break;

            case EMCMOT_ENABLE:
                log_print("ENABLE\n");
                SET_MOTION_ENABLE_FLAG(1);
                update_motion_state();
                break;

            case EMCMOT_DISABLE:
                log_print("DISABLE\n");
                SET_MOTION_ENABLE_FLAG(0);
                update_motion_state();
                break;

            case EMCMOT_ENABLE_AMPLIFIER:
                log_print("ENABLE_AMPLIFIER\n");
                break;

            case EMCMOT_DISABLE_AMPLIFIER:
                log_print("DISABLE_AMPLIFIER\n");
                break;

            case EMCMOT_ENABLE_WATCHDOG:
                log_print("ENABLE_WATCHDOG\n");
                break;

            case EMCMOT_DISABLE_WATCHDOG:
                log_print("DISABLE_WATCHDOG\n");
                break;

            case EMCMOT_ACTIVATE_JOINT:
                log_print("ACTIVATE_JOINT joint=%d\n", c->axis);
                break;

            case EMCMOT_DEACTIVATE_JOINT:
                log_print("DEACTIVATE_JOINT joint=%d\n", c->axis);
                break;

            case EMCMOT_PAUSE:
                log_print("PAUSE\n");
                break;

            case EMCMOT_RESUME:
                log_print("RESUME\n");
                break;

            case EMCMOT_STEP:
                log_print("STEP\n");
                break;

            case EMCMOT_FREE:
                log_print("FREE\n");
                SET_MOTION_COORD_FLAG(0);
                SET_MOTION_TELEOP_FLAG(0);
                update_motion_state();
                break;

            case EMCMOT_COORD:
                log_print("COORD\n");
                SET_MOTION_COORD_FLAG(1);
                SET_MOTION_TELEOP_FLAG(0);
                SET_MOTION_ERROR_FLAG(0);
                update_motion_state();
                break;

            case EMCMOT_TELEOP:
                log_print("TELEOP\n");
                SET_MOTION_TELEOP_FLAG(1);
                SET_MOTION_ERROR_FLAG(0);
                update_motion_state();
                break;

            case EMCMOT_SPINDLE_SCALE:
                log_print("SPINDLE_SCALE\n");
                break;

            case EMCMOT_SS_ENABLE:
                log_print("SS_ENABLE\n");
                break;

            case EMCMOT_FEED_SCALE:
                log_print("FEED_SCALE\n");
                break;

            case EMCMOT_RAPID_SCALE:
                log_print("RAPID_SCALE\n");
                break;

            case EMCMOT_FS_ENABLE:
                log_print("FS_ENABLE\n");
                break;

            case EMCMOT_FH_ENABLE:
                log_print("FH_ENABLE\n");
                break;

            case EMCMOT_AF_ENABLE:
                log_print("AF_ENABLE\n");
                break;

            case EMCMOT_OVERRIDE_LIMITS:
                log_print("OVERRIDE_LIMITS\n");
                break;

            case EMCMOT_HOME:
                log_print("HOME joint=%d\n", c->axis);
                if (c->axis < 0) {
                    for (int j = 0; j < num_joints; j ++) {
                        mark_joint_homed(j);
                    }
                } else {
                    mark_joint_homed(c->axis);
                }
                break;

            case EMCMOT_UNHOME:
                log_print("UNHOME joint=%d\n", c->axis);
                break;

            case EMCMOT_JOG_CONT:
                log_print("JOG_CONT\n");
                break;

            case EMCMOT_JOG_INCR:
                log_print("JOG_INCR\n");
                break;

            case EMCMOT_JOG_ABS:
                log_print("JOG_ABS\n");
                break;

            case EMCMOT_SET_LINE:
                log_print(
                    "SET_LINE x=%.6f, y=%.6f, z=%.6f, a=%.6f, b=%.6f, c=%.6f, u=%.6f, v=%.6f, w=%.6f, id=%d, motion_type=%d, vel=%.6f, ini_maxvel=%.6f, acc=%.6f, turn=%d\n",
                    c->pos.tran.x, c->pos.tran.y, c->pos.tran.z,
                    c->pos.a, c->pos.b, c->pos.c,
                    c->pos.u, c->pos.v, c->pos.w,
                    c->id, c->motion_type,
                    c->vel, c->ini_maxvel,
                    c->acc, c->turn
                );
                break;

            case EMCMOT_SET_CIRCLE:
                log_print("SET_CIRCLE:\n");
                log_print(
                    "    pos: x=%.6f, y=%.6f, z=%.6f, a=%.6f, b=%.6f, c=%.6f, u=%.6f, v=%.6f, w=%.6f\n",
                    c->pos.tran.x, c->pos.tran.y, c->pos.tran.z,
                    c->pos.a, c->pos.b, c->pos.c,
                    c->pos.u, c->pos.v, c->pos.w
                );
                log_print("    center: x=%.6f, y=%.6f, z=%.6f\n", c->center.x, c->center.y, c->center.z);
                log_print("    normal: x=%.6f, y=%.6f, z=%.6f\n", c->normal.x, c->normal.y, c->normal.z);
                log_print("    id=%d, motion_type=%d, vel=%.6f, ini_maxvel=%.6f, acc=%.6f, turn=%d\n",
                    c->id, c->motion_type,
                    c->vel, c->ini_maxvel,
                    c->acc, c->turn
                );
                break;

            case EMCMOT_SET_TELEOP_VECTOR:
                log_print("SET_TELEOP_VECTOR\n");
                break;

            case EMCMOT_CLEAR_PROBE_FLAGS:
                log_print("CLEAR_PROBE_FLAGS\n");
                break;

            case EMCMOT_PROBE:
                log_print("PROBE\n");
                break;

            case EMCMOT_RIGID_TAP:
                log_print("RIGID_TAP\n");
                break;

            case EMCMOT_SET_POSITION_LIMITS:
                log_print(
                    "SET_POSITION_LIMITS joint=%d, min=%.6f, max=%.6f\n",
                    c->axis, c->minLimit, c->maxLimit
                );
                joints[c->axis].max_pos_limit = c->maxLimit;
                joints[c->axis].min_pos_limit = c->minLimit;
                break;

            case EMCMOT_SET_BACKLASH:
                log_print("SET_BACKLASH joint=%d, backlash=%.6f\n", c->axis, c->backlash);
                break;

            case EMCMOT_SET_MIN_FERROR:
                log_print("SET_MIN_FERROR joint=%d, minFerror=%.6f\n", c->axis, c->minFerror);
                break;

            case EMCMOT_SET_MAX_FERROR:
                log_print("SET_MAX_FERROR joint=%d, maxFerror=%.6f\n", c->axis, c->maxFerror);
                break;

            case EMCMOT_SET_VEL:
                log_print("SET_VEL vel=%.6f, ini_maxvel=%.6f\n", c->vel, c->ini_maxvel);
                break;

            case EMCMOT_SET_VEL_LIMIT:
                log_print("SET_VEL_LIMIT vel=%.6f\n", c->vel);
                break;

            case EMCMOT_SET_JOINT_VEL_LIMIT:
                log_print("SET_JOINT_VEL_LIMIT joint=%d, vel=%.6f\n", c->axis, c->vel);
                break;

            case EMCMOT_SET_JOINT_ACC_LIMIT:
                log_print("SET_JOINT_ACC_LIMIT joint=%d, acc=%.6f\n", c->axis, c->acc);
                break;

            case EMCMOT_SET_ACC:
                log_print("SET_ACC acc=%.6f\n", c->acc);
                break;

            case EMCMOT_SET_TERM_COND:
                log_print("SET_TERM_COND termCond=%d, tolerance=%.6f\n", c->termCond, c->tolerance);
                break;

            case EMCMOT_SET_NUM_AXES:
                log_print("SET_NUM_AXES %d\n", c->axis);
                num_joints = c->axis;
                break;

            case EMCMOT_SET_WORLD_HOME:
                log_print(
                    "SET_WORLD_HOME x=%.6f, y=%.6f, z=%.6f, a=%.6f, b=%.6f, c=%.6f, u=%.6f, v=%.6f, w=%.6f\n",
                    c->pos.tran.x, c->pos.tran.y, c->pos.tran.z,
                    c->pos.a, c->pos.b, c->pos.c,
                    c->pos.u, c->pos.v, c->pos.w
                );
                break;

            case EMCMOT_SET_HOMING_PARAMS:
                log_print(
                    "SET_HOMING_PARAMS joint=%d, offset=%.6f home=%.6f, final_vel=%.6f, search_vel=%.6f, latch_vel=%.6f, flags=0x%08x, sequence=%d, volatile=%d\n",
                    c->axis, c->offset, c->home, c->home_final_vel,
                    c->search_vel, c->latch_vel, c->flags,
                    c->home_sequence, c->volatile_home
                );
                break;

            case EMCMOT_SET_DEBUG:
                log_print("SET_DEBUG\n");
                break;

            case EMCMOT_SET_DOUT:
                log_print("SET_DOUT\n");
                break;

            case EMCMOT_SET_AOUT:
                log_print("SET_AOUT\n");
                break;

            case EMCMOT_SET_SPINDLESYNC:
                log_print("SET_SPINDLESYNC sync=%06f, flags=0x%08x\n", c->spindlesync, c->flags);
                break;

            case EMCMOT_SPINDLE_ON:
                log_print("SPINDLE_ON\n");
                break;

            case EMCMOT_SPINDLE_OFF:
                log_print("SPINDLE_OFF\n");
                break;

            case EMCMOT_SPINDLE_INCREASE:
                log_print("SPINDLE_INCREASE\n");
                break;

            case EMCMOT_SPINDLE_DECREASE:
                log_print("SPINDLE_DECREASE\n");
                break;

            case EMCMOT_SPINDLE_BRAKE_ENGAGE:
                log_print("SPINDLE_BRAKE_ENGAGE\n");
                break;

            case EMCMOT_SPINDLE_BRAKE_RELEASE:
                log_print("SPINDLE_BRAKE_RELEASE\n");
                break;

            case EMCMOT_SPINDLE_ORIENT:
                log_print("SPINDLE_ORIENT\n");
                break;

            case EMCMOT_SET_MOTOR_OFFSET:
                log_print("SET_MOTOR_OFFSET\n");
                break;

            case EMCMOT_SET_JOINT_COMP:
                log_print("SET_JOINT_COMP\n");
                break;

            case EMCMOT_SET_OFFSET:
                log_print(
                    "SET_OFFSET x=%.6f, y=%.6f, z=%.6f, a=%.6f, b=%.6f, c=%.6f u=%.6f, v=%.6f, w=%.6f\n",
                    c->tool_offset.tran.x, c->tool_offset.tran.y, c->tool_offset.tran.z,
                    c->tool_offset.a, c->tool_offset.b, c->tool_offset.c,
                    c->tool_offset.u, c->tool_offset.v, c->tool_offset.w
                );
                break;

            case EMCMOT_SET_MAX_FEED_OVERRIDE:
                log_print("SET_MAX_FEED_OVERRIDE %.6f\n", c->maxFeedScale);
                break;

            case EMCMOT_SETUP_ARC_BLENDS:
                log_print("SETUP_ARC_BLENDS\n");
                break;

            default:
                log_print("ERROR: unknown command %d\n", c->command);
                break;
        }

        update_joint_status();

        emcmotStatus->commandEcho = c->command;
        emcmotStatus->commandNumEcho = c->commandNum;
        emcmotStatus->commandStatus = EMCMOT_COMMAND_OK;
        emcmotStatus->tail = emcmotStatus->head;
    }

    return 0;
}