Пример #1
0
/* release_HAL_mutex() unconditionally releases the hal_mutex
   very useful after a program segfaults while holding the mutex
*/
static int release_HAL_mutex(void)
{
    int comp_id, mem_id, retval;
    void *mem;
    hal_data_t *hal_data;

    /* do RTAPI init */
    comp_id = rtapi_init("hal_unlocker");
    if (comp_id < 0) {
        rtapi_print_msg(RTAPI_MSG_ERR, "ERROR: rtapi init failed\n");
        return -EINVAL;
    }
    /* get HAL shared memory block from RTAPI */
    mem_id = rtapi_shmem_new(HAL_KEY, comp_id, HAL_SIZE);
    if (mem_id < 0) {
        rtapi_print_msg(RTAPI_MSG_ERR,
            "ERROR: could not open shared memory\n");
        rtapi_exit(comp_id);
        return -EINVAL;
    }
    /* get address of shared memory area */
    retval = rtapi_shmem_getptr(mem_id, &mem);
    if (retval < 0) {
        rtapi_print_msg(RTAPI_MSG_ERR,
            "ERROR: could not access shared memory\n");
        rtapi_exit(comp_id);
        return -EINVAL;
    }
    /* set up internal pointers to shared mem and data structure */
    hal_data = (hal_data_t *) mem;
    /* release mutex  */
    rtapi_mutex_give(&(hal_data->mutex));
    /* release RTAPI resources */
    rtapi_shmem_delete(mem_id, comp_id);
    rtapi_exit(comp_id);
    /* done */
    return 0;

}
int main(int argc, char **argv)
{
    int n, channel, retval, size, line;
    char *cp, *cp2;
    void *shmem_ptr;
    fifo_t *fifo;
    shmem_data_t *data, *dptr;
    char buf[BUF_SIZE];
	const char *errmsg;
    int tmpin, newin;
    struct timespec delay;

    /* set return code to "fail", clear it later if all goes well */
    exitval = 1;
    channel = 0;
    for ( n = 1 ; n < argc ; n++ ) {
	cp = argv[n];
	if ( *cp != '-' ) {
	    break;
	}
	switch ( *(++cp) ) {
	case 'c':
	    if (( *(++cp) == '\0' ) && ( ++n < argc )) { 
		cp = argv[n];
	    }
	    channel = strtol(cp, &cp2, 10);
	    if (( *cp2 ) || ( channel < 0 ) || ( channel >= MAX_STREAMERS )) {
		fprintf(stderr,"ERROR: invalid channel number '%s'\n", cp );
		exit(1);
	    }
	    break;
	default:
	    fprintf(stderr,"ERROR: unknown option '%s'\n", cp );
	    exit(1);
	    break;
	}
    }
    if(n < argc) {
	int fd;
	if(argc > n+1) {
	    fprintf(stderr, "ERROR: At most one filename may be specified\n");
	    exit(1);
	}
	// make stdin be the named file
	fd = open(argv[n], O_RDONLY);
	close(0);
	dup2(fd, 0);
    }
    /* register signal handlers - if the process is killed
       we need to call hal_exit() to free the shared memory */
    signal(SIGINT, quit);
    signal(SIGTERM, quit);
    signal(SIGPIPE, SIG_IGN);
    /* connect to HAL */
    /* create a unique module name, to allow for multiple streamers */
    snprintf(comp_name, sizeof(comp_name), "halstreamer%d", getpid());
    /* connect to the HAL */
    ignore_sig = 1;
    comp_id = hal_init(comp_name);
    ignore_sig = 0;
    /* check result */
    if (comp_id < 0) {
	fprintf(stderr, "ERROR: hal_init() failed: %d\n", comp_id );
	goto out;
    }
    hal_ready(comp_id);
    /* open shmem for user/RT comms (fifo) */
    /* initial size is unknown, assume only the fifo structure */
    shmem_id = rtapi_shmem_new(STREAMER_SHMEM_KEY+channel, comp_id, sizeof(fifo_t));
    if ( shmem_id < 0 ) {
	fprintf(stderr, "ERROR: couldn't allocate user/RT shared memory\n");
	goto out;
    }
    retval = rtapi_shmem_getptr(shmem_id, &shmem_ptr);
    if ( retval < 0 ) {
	fprintf(stderr, "ERROR: couldn't map user/RT shared memory\n");
	goto out;
    }
    fifo = shmem_ptr;
    if ( fifo->magic != FIFO_MAGIC_NUM ) {
	fprintf(stderr, "ERROR: channel %d realtime part is not loaded\n", channel );
	goto out;
    }
    /* now use data in fifo structure to calculate proper shmem size */
    size = sizeof(fifo_t) + fifo->num_pins * fifo->depth * sizeof(shmem_data_t);
    /* close shmem, re-open with proper size */
    rtapi_shmem_delete(shmem_id, comp_id);
    shmem_id = rtapi_shmem_new(STREAMER_SHMEM_KEY+channel, comp_id, size);
    if ( shmem_id < 0 ) {
	fprintf(stderr, "ERROR: couldn't re-allocate user/RT shared memory\n");
	goto out;
    }
    retval = rtapi_shmem_getptr(shmem_id, &shmem_ptr);
    if ( retval < 0 ) {
	fprintf(stderr, "ERROR: couldn't re-map user/RT shared memory\n");
	goto out;
    }
    line = 1;
    fifo = shmem_ptr;
    data = fifo->data;
    while ( fgets(buf, BUF_SIZE, stdin) ) {
	/* calculate _next_ value for in */
	tmpin = fifo->in;
	newin = tmpin + 1;
	if ( newin >= fifo->depth ) {
	    newin = 0;
	}
	/* wait until there is space in the buffer */
	while ( newin == fifo->out ) {
            /* fifo full, sleep for 10mS */
	    delay.tv_sec = 0;
	    delay.tv_nsec = 10000000;
	    nanosleep(&delay,NULL);
	}
	/* make pointer fifo entry */
	dptr = &data[tmpin*fifo->num_pins];
	/* parse input line, write results to fifo */
	cp = buf;
	errmsg = NULL;
	for ( n = 0 ; n < fifo->num_pins ; n++ ) {
	    /* strip leading whitespace */
	    while ( isspace(*cp) ) {
		cp++;
	    }
	    switch ( fifo->type[n] ) {
	    case HAL_FLOAT:
		dptr->f = strtod(cp, &cp2);
		break;
	    case HAL_BIT:
		if ( *cp == '0' ) {
		    dptr->b = 0;
		    cp2 = cp + 1;
		} else if ( *cp == '1' ) {
		    dptr->b = 1;
		    cp2 = cp + 1;
		} else {
		    errmsg = "bit value not 0 or 1";
		    cp2 = cp;
		}
		break;
	    case HAL_U32:
		dptr->u = strtoul(cp, &cp2, 10);
		break;
	    case HAL_S32:
		dptr->s = strtol(cp, &cp2, 10);
		break;
	    default:
		/* better not happen */
		goto out;
	    }
	    if ( errmsg == NULL ) {
		/* no error yet, check for other possibilties */
		/* whitespace separates fields, and there is a newline
		   at the end... so if there is not space or newline at
		   the end of a field, something is wrong. */
		if ( *cp2 == '\0' ) {
		    errmsg = "premature end of line";
		} else if ( ! isspace(*cp2) ) {
		    errmsg = "bad character";
		}
	    }
	    /* test for any error */
	    if ( errmsg != NULL ) {
		/* abort loop on error */
		break;
	    }
	    /* advance pointers for next field */
	    dptr++;
	    cp = cp2;
	}
	if ( errmsg != NULL ) {
	    /* print message */
	    fprintf (stderr, "line %d, field %d: %s, skipping the line\n", line, n, errmsg );
	    /** TODO - decide whether to skip this line and continue, or 
		abort the program.  Right now it skips the line. */
	} else {
	    /* good data, keep it */
	    fifo->in = newin;
	}
	line++;
    }
    /* run was succesfull */
    exitval = 0;

out:
    ignore_sig = 1;
    if ( shmem_id >= 0 ) {
	rtapi_shmem_delete(shmem_id, comp_id);
    }
    if ( comp_id >= 0 ) {
	hal_exit(comp_id);
    }
    return exitval;
}
Пример #3
0
int hal_xinit(const int type,
	      const int userarg1,
	      const int userarg2,
	      const hal_constructor_t ctor,
	      const hal_destructor_t dtor,
	      const char *name)
{
    int comp_id, retval;

    rtapi_set_logtag("hal_lib");
    CHECK_STRLEN(name, HAL_NAME_LEN);

    // sanity: these must have been inited before by the
    // respective rtapi.so/.ko module
    CHECK_NULL(rtapi_switch);

    if ((dtor != NULL) && (ctor == NULL)) {
	HALERR("component '%s': NULL constructor doesnt make"
	       " sense with non-NULL destructor", name);
	return -EINVAL;
    }

    // RTAPI initialisation already done
    HALDBG("initializing component '%s' type=%d arg1=%d arg2=%d/0x%x",
	   name, type, userarg1, userarg2, userarg2);

    if ((lib_module_id < 0) && (type != TYPE_HALLIB)) {
	// if hal_lib not inited yet, do so now - recurse
#ifdef RTAPI
	retval = hal_xinit(TYPE_HALLIB, 0, 0, NULL, NULL, "hal_lib");
#else
	retval = hal_xinitf(TYPE_HALLIB, 0, 0, NULL, NULL, "hal_lib%ld",
			    (long) getpid());
#endif
	if (retval < 0)
	    return retval;
    }

    // tag message origin field since ulapi autoload re-tagged them temporarily
    rtapi_set_logtag("hal_lib");

    /* copy name to local vars, truncating if needed */
    char rtapi_name[RTAPI_NAME_LEN + 1];
    char hal_name[HAL_NAME_LEN + 1];

    rtapi_snprintf(hal_name, sizeof(hal_name), "%s", name);
    rtapi_snprintf(rtapi_name, RTAPI_NAME_LEN, "HAL_%s", hal_name);

    /* do RTAPI init */
    comp_id = rtapi_init(rtapi_name);
    if (comp_id < 0) {
	HALERR("rtapi init(%s) failed", rtapi_name);
	return -EINVAL;
    }

    // recursing? init HAL shm
    if ((lib_module_id < 0) && (type == TYPE_HALLIB)) {
	// recursion case, we're initing hal_lib

	// get HAL shared memory from RTAPI
	int shm_id = rtapi_shmem_new(HAL_KEY,
				     comp_id,
				     global_data->hal_size);
	if (shm_id < 0) {
	    HALERR("hal_lib:%d failed to allocate HAL shm %x, rc=%d",
		   comp_id, HAL_KEY, shm_id);
	    rtapi_exit(comp_id);
	    return -EINVAL;
	}
	// retrieve address of HAL shared memory segment
	void *mem;
	retval = rtapi_shmem_getptr(shm_id, &mem, 0);
	if (retval < 0) {
	    HALERR("hal_lib:%d failed to acquire HAL shm %x, id=%d rc=%d",
		   comp_id, HAL_KEY, shm_id, retval);
	    rtapi_exit(comp_id);
	    return -EINVAL;
	}
	// set up internal pointers to shared mem and data structure
	hal_shmem_base = (char *) mem;
	hal_data = (hal_data_t *) mem;

#ifdef RTAPI
	// only on RTAPI hal_lib initialization:
	// initialize up the HAL shm segment
	retval = init_hal_data();
	if (retval) {
	    HALERR("could not init HAL shared memory rc=%d", retval);
	    rtapi_exit(lib_module_id);
	    lib_module_id = -1;
	    return -EINVAL;
	}
	retval = hal_proc_init();
	if (retval) {
	    HALERR("could not init /proc files");
	    rtapi_exit(lib_module_id);
	    lib_module_id = -1;
	    return -EINVAL;
	}
#endif
	// record hal_lib comp_id
	lib_module_id = comp_id;
	// and the HAL shm segmed id
	lib_mem_id = shm_id;

    }
    // global_data MUST be at hand now:
    HAL_ASSERT(global_data != NULL);

    // paranoia
    HAL_ASSERT(hal_shmem_base != NULL);
    HAL_ASSERT(hal_data != NULL);
    HAL_ASSERT(lib_module_id > -1);
    HAL_ASSERT(lib_mem_id > -1);
    if (lib_module_id < 0) {
	HALERR("giving up");
	return -EINVAL;
    }

    {
	hal_comp_t *comp  __attribute__((cleanup(halpr_autorelease_mutex)));

	/* get mutex before manipulating the shared data */
	rtapi_mutex_get(&(hal_data->mutex));
	/* make sure name is unique in the system */
	if (halpr_find_comp_by_name(hal_name) != 0) {
	    /* a component with this name already exists */
	    HALERR("duplicate component name '%s'", hal_name);
	    rtapi_exit(comp_id);
	    return -EINVAL;
	}
	/* allocate a new component structure */
	comp = halpr_alloc_comp_struct();
	if (comp == 0) {
	    HALERR("insufficient memory for component '%s'", hal_name);
	    rtapi_exit(comp_id);
	    return -ENOMEM;
	}

	/* initialize the comp structure */
	comp->userarg1 = userarg1;
	comp->userarg2 = userarg2;
	comp->comp_id = comp_id;
	comp->type = type;
	comp->ctor = ctor;
	comp->dtor = dtor;
#ifdef RTAPI
	comp->pid = 0;
#else /* ULAPI */
	// a remote component starts out disowned
	comp->pid = comp->type == TYPE_REMOTE ? 0 : getpid();
#endif
	comp->state = COMP_INITIALIZING;
	comp->last_update = 0;
	comp->last_bound = 0;
	comp->last_unbound = 0;
	comp->shmem_base = hal_shmem_base;
	comp->insmod_args = 0;
	rtapi_snprintf(comp->name, sizeof(comp->name), "%s", hal_name);
	/* insert new structure at head of list */
	comp->next_ptr = hal_data->comp_list_ptr;
	hal_data->comp_list_ptr = SHMOFF(comp);

    }
    // scope exited - mutex released

    // finish hal_lib initialisation
    // in ULAPI this will happen after the recursion on hal_lib%d unwinds

    if (type == TYPE_HALLIB) {
#ifdef RTAPI
	// only on RTAPI hal_lib initialization:
	// export the instantiation support userfuncts
	hal_export_xfunct_args_t ni = {
	    .type = FS_USERLAND,
	    .funct.u = create_instance,
	    .arg = NULL,
	    .owner_id = lib_module_id
	};
	if ((retval = hal_export_xfunctf( &ni, "newinst")) < 0)
	    return retval;

	hal_export_xfunct_args_t di = {
	    .type = FS_USERLAND,
	    .funct.u = delete_instance,
	    .arg = NULL,
	    .owner_id = lib_module_id
	};
	if ((retval = hal_export_xfunctf( &di, "delinst")) < 0)
	    return retval;
#endif
	retval = hal_ready(lib_module_id);
	if (retval)
	    HALERR("hal_ready(%d) failed rc=%d", lib_module_id, retval);
	else
	    HALDBG("%s initialization complete", hal_name);
	return retval;
    }

    HALDBG("%s component '%s' id=%d initialized%s",
	   (ctor != NULL) ? "instantiable" : "legacy",
	   hal_name, comp_id,
	   (dtor != NULL) ? ", has destructor" : "");
    return comp_id;
}


int hal_exit(int comp_id)
{
    int *prev, next, comptype;
    char name[HAL_NAME_LEN + 1];

    CHECK_HALDATA();

    HALDBG("removing component %d", comp_id);

    {
	hal_comp_t *comp  __attribute__((cleanup(halpr_autorelease_mutex)));

	/* grab mutex before manipulating list */
	rtapi_mutex_get(&(hal_data->mutex));
	/* search component list for 'comp_id' */
	prev = &(hal_data->comp_list_ptr);
	next = *prev;
	if (next == 0) {
	    /* list is empty - should never happen, but... */
	    HALERR("no components defined");
	    return -EINVAL;
	}
	comp = SHMPTR(next);
	while (comp->comp_id != comp_id) {
	    /* not a match, try the next one */
	    prev = &(comp->next_ptr);
	    next = *prev;
	    if (next == 0) {
		/* reached end of list without finding component */
		HALERR("no such component with id %d", comp_id);
		return -EINVAL;
	    }
	    comp = SHMPTR(next);
	}

	// record type, since we're about to zap the comp in free_comp_struct()
	comptype = comp->type;

	/* save component name for later */
	rtapi_snprintf(name, sizeof(name), "%s", comp->name);
	/* get rid of the component */
	free_comp_struct(comp);

	// unlink the comp only now as free_comp_struct() must
	// determine ownership of pins/params/functs and this
	// requires access to the current comp, too
	// since this is all under lock it should not matter
	*prev = comp->next_ptr;

	// add it to free list
	comp->next_ptr = hal_data->comp_free_ptr;
	hal_data->comp_free_ptr = SHMOFF(comp);

	// scope exit - mutex released
    }

    // if unloading the hal_lib component, destroy HAL shm
    if (comptype == TYPE_HALLIB) {
	int retval;

	/* release RTAPI resources */
	retval = rtapi_shmem_delete(lib_mem_id, comp_id);
	if (retval) {
	    HALERR("rtapi_shmem_delete(%d,%d) failed: %d",
		   lib_mem_id, comp_id, retval);
	}
	// HAL shm is history, take note ASAP
	lib_mem_id = -1;
	hal_shmem_base = NULL;
	hal_data = NULL;;

	retval = rtapi_exit(comp_id);
	if (retval) {
	    HALERR("rtapi_exit(%d) failed: %d",
		   lib_module_id, retval);
	}
	// the hal_lib RTAPI module is history, too
	// in theory we'd be back to square 1
	lib_module_id = -1;

    } else {
	// the standard case
	rtapi_exit(comp_id);
    }

    //HALDBG("component '%s' id=%d removed", name, comp_id);
    return 0;
}
Пример #4
0
static int init_sampler(int num, fifo_t *tmp_fifo)
{
    int size, retval, n, usefp;
    void *shmem_ptr;
    sampler_t *str;
    pin_data_t *pptr;
    fifo_t *fifo;
    char buf[HAL_NAME_LEN + 2];

    /* alloc shmem for base sampler data and user specified pins */
    size = sizeof(sampler_t) + tmp_fifo->num_pins * sizeof(pin_data_t);
    str = hal_malloc(size);

    if (str == 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "SAMPLER: ERROR: couldn't allocate HAL shared memory\n");
	return -ENOMEM;
    }
    /* export "standard" pins and params */
    retval = hal_pin_bit_newf(HAL_OUT, &(str->full), comp_id,
	"sampler.%d.full", num);
    if (retval != 0 ) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "SAMPLER: ERROR: 'full' pin export failed\n");
	return -EIO;
    }
    retval = hal_pin_bit_newf(HAL_IN, &(str->enable), comp_id,
	"sampler.%d.enable", num);
    if (retval != 0 ) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "SAMPLER: ERROR: 'enable' pin export failed\n");
	return -EIO;
    }
    retval = hal_pin_s32_newf(HAL_OUT, &(str->curr_depth), comp_id,
	"sampler.%d.curr-depth", num);
    if (retval != 0 ) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "SAMPLEr: ERROR: 'curr_depth' pin export failed\n");
	return -EIO;
    }
    retval = hal_param_s32_newf(HAL_RW, &(str->overruns), comp_id,
	"sampler.%d.overruns", num);
    if (retval != 0 ) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "SAMPLER: ERROR: 'overruns' parameter export failed\n");
	return -EIO;
    }
    retval = hal_param_s32_newf(HAL_RW, &(str->sample_num), comp_id,
	"sampler.%d.sample-num", num);
    if (retval != 0 ) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "SAMPLER: ERROR: 'sample-num' parameter export failed\n");
	return -EIO;
    }
    /* init the standard pins and params */
    *(str->full) = 0;
    *(str->enable) = 1;
    *(str->curr_depth) = 0;
    str->overruns = 0;
    str->sample_num = 0;
    /* HAL pins are right after the sampler_t struct in HAL shmem */
    pptr = (pin_data_t *)(str+1);
    usefp = 0;
    /* export user specified pins (the ones that sample data) */
    for ( n = 0 ; n < tmp_fifo->num_pins ; n++ ) {
	rtapi_snprintf(buf, HAL_NAME_LEN, "sampler.%d.pin.%d", num, n);
	retval = hal_pin_new(buf, tmp_fifo->type[n], HAL_IN, (void **)pptr, comp_id );
	if (retval != 0 ) {
	    rtapi_print_msg(RTAPI_MSG_ERR,
		"SAMPLER: ERROR: pin '%s' export failed\n", buf);
	    return -EIO;
	}
	/* init the pin value */
	switch ( tmp_fifo->type[n] ) {
	case HAL_FLOAT:
	    *(pptr->hfloat) = 0.0;
	    usefp = 1;
	    break;
	case HAL_BIT:
	    *(pptr->hbit) = 0;
	    break;
	case HAL_U32:
	    *(pptr->hu32) = 0;
	    break;
	case HAL_S32:
	    *(pptr->hs32) = 0;
	    break;
	default:
	    break;
	}
	pptr++;
    }
    /* export update function */
    rtapi_snprintf(buf, HAL_NAME_LEN, "sampler.%d", num);
    retval = hal_export_funct(buf, sample, str, usefp, 0, comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "SAMPLER: ERROR: function export failed\n");
	return retval;
    }

    /* alloc shmem for user/RT comms (fifo) */
    size = sizeof(fifo_t) + (tmp_fifo->num_pins + 1) * tmp_fifo->depth * sizeof(shmem_data_t);
    shmem_id[num] = rtapi_shmem_new(SAMPLER_SHMEM_KEY+num, comp_id, size);
    if ( shmem_id[num] < 0 ) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "SAMPLEr: ERROR: couldn't allocate user/RT shared memory\n");
	return -ENOMEM;
    }
    retval = rtapi_shmem_getptr(shmem_id[num], &shmem_ptr);
    if ( retval < 0 ) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "SAMPLER: ERROR: couldn't map user/RT shared memory\n");
	return -ENOMEM;
    }
    fifo = shmem_ptr;
    str->fifo = fifo;
    /* copy data from temp_fifo */
    *fifo = *tmp_fifo;
    /* init fields */
    fifo->in = 0;
    fifo->out = 0;
    fifo->last_sample = 0;
    fifo->last_sample--;

    /* mark it inited for user program */
    fifo->magic = FIFO_MAGIC_NUM;
    return 0;
}
Пример #5
0
int ClassicLadder_AllocAll()
{
   	 unsigned char *pByte; 
   	 unsigned long bytes = sizeof(StrInfosGene) + sizeof(long);
   	 unsigned long *shmBase;
 	 plc_sizeinfo_s *pSizesInfos;

#ifdef RTAPI // for realtime
    int numBits, numWords, numFloats;
    pSizesInfos = &GeneralParamsMirror.SizesInfos;
    // Calculate SHMEM size.
    numBits = pSizesInfos->nbr_bits + pSizesInfos->nbr_phys_inputs + pSizesInfos->nbr_phys_outputs + pSizesInfos->nbr_error_bits;
    numWords = pSizesInfos->nbr_words+pSizesInfos->nbr_phys_words_inputs+pSizesInfos->nbr_phys_words_outputs;
    numFloats = pSizesInfos->nbr_phys_float_inputs+pSizesInfos->nbr_phys_float_outputs;
#ifdef SEQUENTIAL_SUPPORT
    numBits += NBR_STEPS;
    numWords += NBR_STEPS;
#endif
    bytes += pSizesInfos->nbr_rungs * sizeof(StrRung);
    bytes += pSizesInfos->nbr_timers * sizeof(StrTimer);
    bytes += pSizesInfos->nbr_monostables * sizeof(StrMonostable);
    bytes += pSizesInfos->nbr_counters * sizeof(StrCounter);
    bytes += pSizesInfos->nbr_timers_iec * sizeof(StrTimerIEC);
    bytes += pSizesInfos->nbr_arithm_expr * sizeof(StrArithmExpr);
    bytes += pSizesInfos->nbr_sections * sizeof(StrSection);
    bytes += pSizesInfos->nbr_symbols * sizeof(StrSymbol);
    
#ifdef SEQUENTIAL_SUPPORT
    bytes += sizeof(StrSequential);
#endif
    bytes += numWords * sizeof(int);
    bytes += numFloats * sizeof(double);
    bytes += numBits * sizeof(TYPE_FOR_BOOL_VAR);


    // Attach SHMEM with proper size.
    if ((ShmemId = rtapi_shmem_new(CL_SHMEM_KEY, compId, bytes)) < 0) 
              {
                rtapi_print_msg(RTAPI_MSG_DBG,"Failed to alloc shared memory (%x %d %lu) !\n",CL_SHMEM_KEY, compId, bytes);
               return FALSE;
              }
    rtapi_print_msg(RTAPI_MSG_INFO,"Shared memory:key- %x component id-%d # of bytes-%lu\n",CL_SHMEM_KEY, compId, bytes);
    // Map SHMEM (shared memory).
    if (rtapi_shmem_getptr(ShmemId, (void **) &shmBase, 0) < 0) 
              {
  	       rtapi_print("Failed to map shared memory !\n");
  	       return FALSE;
              }
	
     shmBase[0] = CL_SHMEM_KEY;
     shmBase[1] = bytes;
     InfosGene = (StrInfosGene*)(shmBase+1);
     InfosGene->GeneralParams.SizesInfos = *pSizesInfos;
     memcpy( &InfosGene->GeneralParams, &GeneralParamsMirror, sizeof( StrGeneralParams ) );
     rtapi_print_msg(RTAPI_MSG_INFO,"INFO----REALTIME allocations for classicladder:\n");
#endif //end of realtime code

#ifndef RTAPI// for user space

     // check if RT component was loaded:
     if (!rtapi_shmem_exists(CL_SHMEM_KEY)) {
	 rtapi_print_msg(RTAPI_MSG_ERR,
			 "classicladder: the classicladder_rt shared "
			 "memory segment (%x) does not exist",CL_SHMEM_KEY);
	 rtapi_print_msg(RTAPI_MSG_ERR, "classicladder_rt not loaded?");
	 return FALSE;
     }
     
    // Attach SHMEM with proper size.
    if ((ShmemId = rtapi_shmem_new(CL_SHMEM_KEY, compId, 0)) < 0)
              {
               rtapi_print("Failed to alloc shared memory (%x %d %lu) !\n",
               CL_SHMEM_KEY, compId, bytes);
               return FALSE;
              }
     rtapi_print_msg(RTAPI_MSG_INFO,"Shared memory:key- %x component id-%d # of bytes-%lu\n",
     CL_SHMEM_KEY, compId, bytes);
    // Map SHMEM.
     if (rtapi_shmem_getptr(ShmemId, (void **) &shmBase, 0) < 0) 
          {
           rtapi_print("Failed to map shared memory !\n");
           return FALSE;
          }

    // Check signature written by RT module to make sure we have the
    // right region and RT module is loaded.
    rtapi_print_msg(RTAPI_MSG_INFO,"INFO----USERSPACE allocations for classicladder\n");
    if (shmBase[0] != CL_SHMEM_KEY) 
          {
           rtapi_print("Shared memory conflict or RT component not loaded!\n");
           return FALSE;
          }
     bytes = shmBase[1];   
     InfosGene = (StrInfosGene*)(shmBase+1);
     pSizesInfos = &(InfosGene->GeneralParams.SizesInfos);

        // copy generalparams to gen paramsMirror so Config window displays properly
	memcpy(  &GeneralParamsMirror,&InfosGene->GeneralParams, sizeof( StrGeneralParams ) );
  	UpdateSizesOfConvVarNameTable();
#ifdef GTK_INTERFACE
	EditArithmExpr = (StrArithmExpr *)malloc(  pSizesInfos->nbr_arithm_expr * sizeof(StrArithmExpr) );
	if (!EditArithmExpr)
	{
		rtapi_print("Failed to alloc EditArithmExpr !\n");
		return FALSE;
	}
#endif
#endif

// the rest is for both realtime and userspace program

      rtapi_print_msg(RTAPI_MSG_INFO,"Sizes: rungs- %d bits- %d words- %d timers- %d mono- %d count- %d IEC timers- %d\n HAL Bin- %d HAL Bout- %d expressions- %d sections- %d symbols - %d\n  s32in - %d s32out- %d Error bits-%d\n",
        pSizesInfos->nbr_rungs,
        pSizesInfos->nbr_bits,
        pSizesInfos->nbr_words,
        pSizesInfos->nbr_timers,
        pSizesInfos->nbr_monostables,
        pSizesInfos->nbr_counters,
	pSizesInfos->nbr_timers_iec,
        pSizesInfos->nbr_phys_inputs,
        pSizesInfos->nbr_phys_outputs,
        pSizesInfos->nbr_arithm_expr,
        pSizesInfos->nbr_sections,
        pSizesInfos->nbr_symbols,
	pSizesInfos->nbr_phys_words_inputs,
	pSizesInfos->nbr_phys_words_outputs,
        pSizesInfos->nbr_error_bits);

    	// Set global SHMEM pointers for each element
    pByte = (unsigned char *) InfosGene;
	   pByte += sizeof(StrInfosGene);
    RungArray = (StrRung *) pByte;
 	   pByte += pSizesInfos->nbr_rungs * sizeof(StrRung);
    TimerArray = (StrTimer *) pByte;	
   	   pByte += pSizesInfos->nbr_timers * sizeof(StrTimer);
    MonostableArray = (StrMonostable *) pByte;
   	   pByte += pSizesInfos->nbr_monostables * sizeof(StrMonostable);
    CounterArray= (StrCounter *) pByte;
	   pByte += pSizesInfos->nbr_counters * sizeof(StrCounter);	
    NewTimerArray = (StrTimerIEC *) pByte;
	   pByte += pSizesInfos->nbr_timers_iec * sizeof(StrTimerIEC);
           ArithmExpr = (StrArithmExpr *) pByte;	
 	   pByte += pSizesInfos->nbr_arithm_expr * sizeof(StrArithmExpr);
    SectionArray = (StrSection *) pByte;	
 	   pByte += pSizesInfos->nbr_sections * sizeof(StrSection);
    SymbolArray = (StrSymbol *) pByte;	
	   pByte += pSizesInfos->nbr_symbols * sizeof(StrSymbol);
#ifdef SEQUENTIAL_SUPPORT
    Sequential = (StrSequential *) pByte;	
  	  pByte += sizeof(StrSequential);
#endif
    VarWordArray = (int *) pByte;
 	   pByte += SIZE_VAR_WORD_ARRAY * sizeof(int);
    VarFloatArray =(double *) pByte;
           pByte += SIZE_VAR_FLOAT_ARRAY * sizeof(double);
    // Allocate last for alignment reasons.
    VarArray = (TYPE_FOR_BOOL_VAR *) pByte;

	InitInfosGene( );

return TRUE;
}
Пример #6
0
/* init_comm_buffers() allocates and initializes the command,
   status, and error buffers used to communicate with the user
   space parts of emc.
*/
static int init_comm_buffers(void)
{
    int joint_num, n;
    emcmot_joint_t *joint;
    int retval;

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

    emcmotStruct = 0;
    emcmotDebug = 0;
    emcmotStatus = 0;
    emcmotCommand = 0;
    emcmotConfig = 0;

    /* record the kinematics type of the machine */
    kinType = kinematicsType();

    /* allocate and initialize the shared memory structure */
    emc_shmem_id = rtapi_shmem_new(key, mot_comp_id, sizeof(emcmot_struct_t));
    if (emc_shmem_id < 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "MOTION: rtapi_shmem_new failed, returned %d\n", emc_shmem_id);
	return -1;
    }
    retval = rtapi_shmem_getptr(emc_shmem_id, (void **) &emcmotStruct);
    if (retval < 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "MOTION: rtapi_shmem_getptr failed, returned %d\n", retval);
	return -1;
    }

    /* zero shared memory before doing anything else. */
    memset(emcmotStruct, 0, sizeof(emcmot_struct_t));

    /* we'll reference emcmotStruct directly */
    emcmotCommand = &emcmotStruct->command;
    emcmotStatus = &emcmotStruct->status;
    emcmotConfig = &emcmotStruct->config;
    emcmotDebug = &emcmotStruct->debug;
    emcmotInternal = &emcmotStruct->internal;
    emcmotError = &emcmotStruct->error;

    /* init error struct */
    emcmotErrorInit(emcmotError);

    /* init command struct */
    emcmotCommand->head = 0;
    emcmotCommand->command = 0;
    emcmotCommand->commandNum = 0;
    emcmotCommand->tail = 0;
    emcmotCommand->spindlesync = 0.0;

    /* init status struct */
    emcmotStatus->head = 0;
    emcmotStatus->commandEcho = 0;
    emcmotStatus->commandNumEcho = 0;
    emcmotStatus->commandStatus = 0;

    /* init more stuff */

    emcmotDebug->head = 0;
    emcmotConfig->head = 0;

    emcmotStatus->motionFlag = 0;
    SET_MOTION_ERROR_FLAG(0);
    SET_MOTION_COORD_FLAG(0);
    SET_MOTION_TELEOP_FLAG(0);
    emcmotDebug->split = 0;
    emcmotStatus->heartbeat = 0;
    emcmotStatus->computeTime = 0.0;
    emcmotConfig->numJoints = num_joints;

    ZERO_EMC_POSE(emcmotStatus->carte_pos_cmd);
    ZERO_EMC_POSE(emcmotStatus->carte_pos_fb);
    emcmotStatus->vel = VELOCITY;
    emcmotConfig->limitVel = VELOCITY;
    emcmotStatus->acc = ACCELERATION;
    emcmotStatus->feed_scale = 1.0;
    emcmotStatus->spindle_scale = 1.0;
    emcmotStatus->net_feed_scale = 1.0;
    /* adaptive feed is off by default, feed override, spindle 
       override, and feed hold are on */
    emcmotStatus->enables_new = FS_ENABLED | SS_ENABLED | FH_ENABLED;
    emcmotStatus->enables_queued = emcmotStatus->enables_new;
    emcmotStatus->id = 0;
    emcmotStatus->depth = 0;
    emcmotStatus->activeDepth = 0;
    emcmotStatus->paused = 0;
    emcmotStatus->overrideLimitMask = 0;
    emcmotStatus->spindle.speed = 0.0;
    SET_MOTION_INPOS_FLAG(1);
    SET_MOTION_ENABLE_FLAG(0);
    emcmotConfig->kinematics_type = kinType;

    emcmotDebug->oldPos = emcmotStatus->carte_pos_cmd;
    ZERO_EMC_POSE(emcmotDebug->oldVel);

    emcmot_config_change();

    /* init pointer to joint structs */
#ifdef STRUCTS_IN_SHMEM
    joints = &(emcmotDebug->joints[0]);
#else
    joints = &(joint_array[0]);
#endif

    /* init per-joint stuff */
    for (joint_num = 0; joint_num < num_joints; joint_num++) {
	/* point to structure for this joint */
	joint = &joints[joint_num];

	/* init the config fields with some "reasonable" defaults" */

	joint->type = 0;
	joint->max_pos_limit = 1.0;
	joint->min_pos_limit = -1.0;
	joint->vel_limit = 1.0;
	joint->acc_limit = 1.0;
	joint->min_ferror = 0.01;
	joint->max_ferror = 1.0;
	joint->home_search_vel = 0.0;
	joint->home_latch_vel = 0.0;
	joint->home_final_vel = -1;
	joint->home_offset = 0.0;
	joint->home = 0.0;
	joint->home_flags = 0;
	joint->home_sequence = -1;
	joint->backlash = 0.0;

	joint->comp.entries = 0;
	joint->comp.entry = &(joint->comp.array[0]);
	/* the compensation code has -DBL_MAX at one end of the table
	   and +DBL_MAX at the other so _all_ commanded positions are
	   guaranteed to be covered by the table */
	joint->comp.array[0].nominal = -DBL_MAX;
	joint->comp.array[0].fwd_trim = 0.0;
	joint->comp.array[0].rev_trim = 0.0;
	joint->comp.array[0].fwd_slope = 0.0;
	joint->comp.array[0].rev_slope = 0.0;
	for ( n = 1 ; n < EMCMOT_COMP_SIZE+2 ; n++ ) {
	    joint->comp.array[n].nominal = DBL_MAX;
	    joint->comp.array[n].fwd_trim = 0.0;
	    joint->comp.array[n].rev_trim = 0.0;
	    joint->comp.array[n].fwd_slope = 0.0;
	    joint->comp.array[n].rev_slope = 0.0;
	}

	/* init status info */
	joint->flag = 0;
	joint->coarse_pos = 0.0;
	joint->pos_cmd = 0.0;
	joint->vel_cmd = 0.0;
	joint->backlash_corr = 0.0;
	joint->backlash_filt = 0.0;
	joint->backlash_vel = 0.0;
	joint->motor_pos_cmd = 0.0;
	joint->motor_pos_fb = 0.0;
	joint->pos_fb = 0.0;
	joint->ferror = 0.0;
	joint->ferror_limit = joint->min_ferror;
	joint->ferror_high_mark = 0.0;

	/* init internal info */
	cubicInit(&(joint->cubic));

	/* init misc other stuff in joint structure */
	joint->big_vel = 10.0 * joint->vel_limit;
	joint->home_state = 0;

	/* init joint flags (reduntant, since flag = 0 */

	SET_JOINT_ENABLE_FLAG(joint, 0);
	SET_JOINT_ACTIVE_FLAG(joint, 0);
	SET_JOINT_NHL_FLAG(joint, 0);
	SET_JOINT_PHL_FLAG(joint, 0);
	SET_JOINT_INPOS_FLAG(joint, 1);
	SET_JOINT_HOMING_FLAG(joint, 0);
	SET_JOINT_HOMED_FLAG(joint, 0);
	SET_JOINT_FERROR_FLAG(joint, 0);
	SET_JOINT_FAULT_FLAG(joint, 0);
	SET_JOINT_ERROR_FLAG(joint, 0);

    }

    /*! \todo FIXME-- add emcmotError */

    emcmotDebug->tMin = 0.0;
    emcmotDebug->tMax = 0.0;
    emcmotDebug->tAvg = 0.0;
    emcmotDebug->sMin = 0.0;
    emcmotDebug->sMax = 0.0;
    emcmotDebug->sAvg = 0.0;
    emcmotDebug->nMin = 0.0;
    emcmotDebug->nMax = 0.0;
    emcmotDebug->nAvg = 0.0;
    emcmotDebug->yMin = 0.0;
    emcmotDebug->yMax = 0.0;
    emcmotDebug->yAvg = 0.0;
    emcmotDebug->fyMin = 0.0;
    emcmotDebug->fyMax = 0.0;
    emcmotDebug->fyAvg = 0.0;
    emcmotDebug->fMin = 0.0;
    emcmotDebug->fMax = 0.0;
    emcmotDebug->fAvg = 0.0;

    emcmotDebug->cur_time = emcmotDebug->last_time = 0.0;
    emcmotDebug->start_time = etime();
    emcmotDebug->running_time = 0.0;

    /* init motion emcmotDebug->queue */
    if (-1 == tpCreate(&emcmotDebug->queue, DEFAULT_TC_QUEUE_SIZE,
	    emcmotDebug->queueTcSpace)) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "MOTION: failed to create motion emcmotDebug->queue\n");
	return -1;
    }
//    tpInit(&emcmotDebug->queue); // tpInit called from tpCreate
    tpSetCycleTime(&emcmotDebug->queue, emcmotConfig->trajCycleTime);
    tpSetPos(&emcmotDebug->queue, emcmotStatus->carte_pos_cmd);
    tpSetVmax(&emcmotDebug->queue, emcmotStatus->vel, emcmotStatus->vel);
    tpSetAmax(&emcmotDebug->queue, emcmotStatus->acc);

    emcmotStatus->tail = 0;

    rtapi_print_msg(RTAPI_MSG_INFO, "MOTION: init_comm_buffers() complete\n");
    return 0;
}
Пример #7
0
int main(int argc, char **argv)
{
    int n, channel, retval, size, tag;
    long int samples;
    unsigned long this_sample;
    char  *cp2;
    char *name = NULL;
    void *shmem_ptr;
    fifo_t *fifo;
    shmem_data_t *data, *dptr, buf[MAX_PINS];
    int tmpout, newout;
    struct timespec delay;

    /* set return code to "fail", clear it later if all goes well */
    exitval = 1;
    channel = 0;
    tag = 0;
    samples = -1;  /* -1 means run forever */
    int  opt;

    while ((opt = getopt(argc, argv, "tn:c:N:")) != -1) {
	switch (opt) {
	case 'c':
	    channel = strtol(optarg, &cp2, 10);
	    if (( *cp2 ) || ( channel < 0 ) || ( channel >= MAX_SAMPLERS )) {
		fprintf(stderr,"ERROR: invalid channel number '%s'\n", optarg );
		exit(1);
	    }
	    break;
	case 'n':
	    samples = strtol(optarg, &cp2, 10);
	    if (( *cp2 ) || ( samples < 0 )) {
		fprintf(stderr, "ERROR: invalid sample count '%s'\n", optarg );
		exit(1);
	    }
	    break;
	case 'N':
	    name = optarg;
	    break;
	case 't':
	    tag = 1;
	    break;
	default: /* '?' */
	    fprintf(stderr,"ERROR: unknown option '%c'\n", opt);
	    fprintf(stderr,"valid options are:\n" );
	    fprintf(stderr,"\t-t\t\ttag values with sample number\n" );
	    fprintf(stderr,"\t-c <int>\t channel number\n" );
	    fprintf(stderr,"\t-n <int>\t sample count\n" );
	    fprintf(stderr,"\t-N <name>\t set HAL component name\n" );
	    exit(1);
	    exit(EXIT_FAILURE);
	}
    }
    if (optind < argc) {
	int fd;
	if(argc > optind+1) {
	    fprintf(stderr, "ERROR: At most one filename may be specified\n");
	    exit(1);
	}
	// make stdout be the named file
	fd = open(argv[optind], O_WRONLY | O_CREAT, 0666);
	close(1);
	dup2(fd, 1);
    }

    /* register signal handlers - if the process is killed
       we need to call hal_exit() to free the shared memory */
    signal(SIGINT, quit);
    signal(SIGTERM, quit);
    signal(SIGPIPE, quit);
    /* connect to HAL */
    if (name == NULL) {
	/* create a unique module name, to allow for multiple samplers */
	snprintf(comp_name, sizeof(comp_name), "halsampler%d", getpid());
	name = comp_name;
    }
    /* connect to the HAL */
    ignore_sig = 1;
    comp_id = hal_init(name);
    ignore_sig = 0;
    /* check result */
    if (comp_id < 0) {
	fprintf(stderr, "ERROR: hal_init() failed: %d\n", comp_id );
	goto out;
    }
    hal_ready(comp_id);
    /* open shmem for user/RT comms (fifo) */
    /* initial size is unknown, assume only the fifo structure */
    shmem_id = rtapi_shmem_new(SAMPLER_SHMEM_KEY+channel, comp_id, sizeof(fifo_t));
    if ( shmem_id < 0 ) {
	fprintf(stderr, "ERROR: couldn't allocate user/RT shared memory\n");
	goto out;
    }
    retval = rtapi_shmem_getptr(shmem_id, &shmem_ptr, 0);
    if ( retval < 0 ) {
	fprintf(stderr, "ERROR: couldn't map user/RT shared memory\n");
	goto out;
    }
    fifo = shmem_ptr;
    if ( fifo->magic != FIFO_MAGIC_NUM ) {
	fprintf(stderr, "ERROR: channel %d realtime part is not loaded\n", channel );
	goto out;
    }
    /* now use data in fifo structure to calculate proper shmem size */
    size = sizeof(fifo_t) + (1+fifo->num_pins) * fifo->depth * sizeof(shmem_data_t);
    /* close shmem, re-open with proper size */
    rtapi_shmem_delete(shmem_id, comp_id);
    shmem_id = rtapi_shmem_new(SAMPLER_SHMEM_KEY+channel, comp_id, size);
    if ( shmem_id < 0 ) {
	fprintf(stderr, "ERROR: couldn't re-allocate user/RT shared memory\n");
	goto out;
    }
    retval = rtapi_shmem_getptr(shmem_id, &shmem_ptr, 0);
    if ( retval < 0 ) {
	fprintf(stderr, "ERROR: couldn't re-map user/RT shared memory\n");
	goto out;
    }
    fifo = shmem_ptr;
    data = fifo->data;
    while ( samples != 0 ) {
	while ( fifo->in == fifo->out ) {
            /* fifo empty, sleep for 10mS */
	    delay.tv_sec = 0;
	    delay.tv_nsec = 10000000;
	    nanosleep(&delay,NULL);
	}
	/* make pointer to fifo entry */
	tmpout = fifo->out;
	newout = tmpout + 1;
	if ( newout >= fifo->depth ) {
	    newout = 0;
	}
	dptr = &data[tmpout * (fifo->num_pins+1)];
	/* read data from shmem into buffer */
	for ( n = 0 ; n < fifo->num_pins ; n++ ) {
	    buf[n] = *(dptr++);
	}
	/* and read sample number */
	this_sample = dptr->u;
	if ( fifo->out != tmpout ) {
	    /* the sample was overwritten while we were reading it */
	    /* so ignore it */
	    continue;
	} else {
	    /* update 'out' for next sample */
	    fifo->out = newout;
	}
	if ( this_sample != ++(fifo->last_sample) ) {
	    printf ( "overrun\n" );
	    fifo->last_sample = this_sample;
	}
	if ( tag ) {
	    printf ( "%ld ", this_sample );
	}
	for ( n = 0 ; n < fifo->num_pins ; n++ ) {
	    switch ( fifo->type[n] ) {
	    case HAL_FLOAT:
		printf ( "%f ", buf[n].f);
		break;
	    case HAL_BIT:
		if ( buf[n].b ) {
		    printf ( "1 " );
		} else {
		    printf ( "0 " );
		}
		break;
	    case HAL_U32:
		printf ( "%lu ", (unsigned long)buf[n].u);
		break;
	    case HAL_S32:
		printf ( "%ld ", (long)buf[n].s);
		break;
	    default:
		/* better not happen */
		goto out;
	    }
	}
	printf ( "\n" );
	if ( samples > 0 ) {
	    samples--;
	}
    }
    /* run was succesfull */
    exitval = 0;

out:
    ignore_sig = 1;
    if ( shmem_id >= 0 ) {
	rtapi_shmem_delete(shmem_id, comp_id);
    }
    if ( comp_id >= 0 ) {
	hal_exit(comp_id);
    }
    return exitval;
}
Пример #8
0
int rtapi_app_main(void)
{
    int retval;
    void *mem;

    rtapi_switch = rtapi_get_handle();
    hal_print_msg(RTAPI_MSG_DBG,
		    "HAL_LIB:%d loading RT support gd=%pp\n",rtapi_instance,global_data);

    /* do RTAPI init */
    lib_module_id = rtapi_init("HAL_LIB");
    if (lib_module_id < 0) {
	hal_print_msg(RTAPI_MSG_ERR,
			"HAL_LIB:%d ERROR: rtapi init failed\n",
			rtapi_instance);
	return -EINVAL;
    }

    // paranoia
    if (global_data == NULL) {
	hal_print_msg(RTAPI_MSG_ERR,
			"HAL_LIB:%d ERROR: global_data == NULL\n",
			rtapi_instance);
	return -EINVAL;
    }

    /* get HAL shared memory block from RTAPI */
    lib_mem_id = rtapi_shmem_new(HAL_KEY, lib_module_id, global_data->hal_size);

    if (lib_mem_id < 0) {
	hal_print_msg(RTAPI_MSG_ERR,
			"HAL_LIB:%d ERROR: could not open shared memory\n",
			rtapi_instance);
	rtapi_exit(lib_module_id);
	return -EINVAL;
    }
    /* get address of shared memory area */
    retval = rtapi_shmem_getptr(lib_mem_id, &mem, 0);

    if (retval < 0) {
	hal_print_msg(RTAPI_MSG_ERR,
			"HAL_LIB:%d ERROR: could not access shared memory\n",
			rtapi_instance);
	rtapi_exit(lib_module_id);
	return -EINVAL;
    }
    /* set up internal pointers to shared mem and data structure */
    hal_shmem_base = (char *) mem;
    hal_data = (hal_data_t *) mem;
    /* perform a global init if needed */
    retval = init_hal_data();

    if ( retval ) {
	hal_print_msg(RTAPI_MSG_ERR,
			"HAL_LIB:%d ERROR: could not init shared memory\n",
			rtapi_instance);
	rtapi_exit(lib_module_id);
	return -EINVAL;
    }

    retval = hal_proc_init();

    if ( retval ) {
	hal_print_msg(RTAPI_MSG_ERR,
			"HAL_LIB: ERROR:%d could not init /proc files\n",
			rtapi_instance);
	rtapi_exit(lib_module_id);
	return -EINVAL;
    }

    /* done */
    hal_print_msg(RTAPI_MSG_DBG,
		    "HAL_LIB:%d kernel lib installed successfully\n",
		    rtapi_instance);
    return 0;
}
Пример #9
0
static int init_comm_buffers(void) {
    int joint_num, n;
    emcmot_joint_t *joint;
    int retval;
    int shmem_id;

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

    emcmotStruct = 0;
    emcmotDebug = 0;
    emcmotStatus = 0;
    c = 0;
    emcmotConfig = 0;

    /* allocate and initialize the shared memory structure */
    shmem_id = rtapi_shmem_new(DEFAULT_SHMEM_KEY, mot_comp_id, sizeof(emcmot_struct_t));
    if (shmem_id < 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "MOTION: rtapi_shmem_new failed, returned %d\n", shmem_id);
	return -1;
    }
    retval = rtapi_shmem_getptr(shmem_id, (void **) &emcmotStruct);
    if (retval < 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "MOTION: rtapi_shmem_getptr failed, returned %d\n", retval);
	return -1;
    }

    /* zero shared memory before doing anything else. */
    memset(emcmotStruct, 0, sizeof(emcmot_struct_t));

    /* we'll reference emcmotStruct directly */
    c = &emcmotStruct->command;
    emcmotStatus = &emcmotStruct->status;
    emcmotConfig = &emcmotStruct->config;
    emcmotDebug = &emcmotStruct->debug;
    emcmotInternal = &emcmotStruct->internal;
    emcmotError = &emcmotStruct->error;

    emcmotConfig->numJoints = num_joints;

    emcmotStatus->vel = DEFAULT_VELOCITY;
    emcmotConfig->limitVel = DEFAULT_VELOCITY;
    emcmotStatus->acc = DEFAULT_ACCELERATION;
    emcmotStatus->feed_scale = 1.0;
    emcmotStatus->rapid_scale = 1.0;
    emcmotStatus->spindle_scale = 1.0;
    emcmotStatus->net_feed_scale = 1.0;
    /* adaptive feed is off by default, feed override, spindle 
       override, and feed hold are on */
    emcmotStatus->enables_new = FS_ENABLED | SS_ENABLED | FH_ENABLED;
    emcmotStatus->enables_queued = emcmotStatus->enables_new;
    SET_MOTION_INPOS_FLAG(1);
    emcmotConfig->kinematics_type = KINEMATICS_IDENTITY;

    emcmot_config_change();

    /* init pointer to joint structs */
    joints = joint_array;

    /* init per-joint stuff */
    for (joint_num = 0; joint_num < num_joints; joint_num++) {
	/* point to structure for this joint */
	joint = &joints[joint_num];

	/* init the config fields with some "reasonable" defaults" */

	joint->max_pos_limit = 1.0;
	joint->min_pos_limit = -1.0;
	joint->vel_limit = 1.0;
	joint->acc_limit = 1.0;
	joint->min_ferror = 0.01;
	joint->max_ferror = 1.0;
	joint->home_final_vel = -1;
	joint->home_sequence = -1;

	joint->comp.entry = &(joint->comp.array[0]);
	/* the compensation code has -DBL_MAX at one end of the table
	   and +DBL_MAX at the other so _all_ commanded positions are
	   guaranteed to be covered by the table */
	joint->comp.array[0].nominal = -DBL_MAX;
	joint->comp.array[0].fwd_trim = 0.0;
	joint->comp.array[0].rev_trim = 0.0;
	joint->comp.array[0].fwd_slope = 0.0;
	joint->comp.array[0].rev_slope = 0.0;
	for ( n = 1 ; n < EMCMOT_COMP_SIZE+2 ; n++ ) {
	    joint->comp.array[n].nominal = DBL_MAX;
	    joint->comp.array[n].fwd_trim = 0.0;
	    joint->comp.array[n].rev_trim = 0.0;
	    joint->comp.array[n].fwd_slope = 0.0;
	    joint->comp.array[n].rev_slope = 0.0;
	}

	/* init status info */
	joint->ferror_limit = joint->min_ferror;

	/* init misc other stuff in joint structure */
	joint->big_vel = 10.0 * joint->vel_limit;

	SET_JOINT_INPOS_FLAG(joint, 1);
    }

    emcmotDebug->start_time = time(NULL);

    rtapi_print_msg(RTAPI_MSG_INFO, "MOTION: init_comm_buffers() complete\n");
    return 0;
}
Пример #10
0
/* part of the Linux kernel module that kicks off the shmem task */
int rtapi_app_main(void)
{
    int retval;
    int shmem_prio;
    long period;

    module = rtapi_init("SHMEMTASK");
    if (module < 0) {
	rtapi_print("shmemtask init: rtapi_init returned %d\n", module);
	return -1;
    }
    /* allocate and initialize the shared memory structure */
    shmem_mem = rtapi_shmem_new(key, module, sizeof(SHMEM_STRUCT));
    if (shmem_mem < 0) {
	rtapi_print("shmemtask init: rtapi_shmem_new returned %d\n",
	    shmem_mem);
	rtapi_exit(module);
	return -1;
    }
    retval = rtapi_shmem_getptr(shmem_mem, (void **) &shmem_struct);
    if (retval < 0) {
	rtapi_print("shmemtask init: rtapi_shmem_getptr returned %d\n",
	    retval);
	rtapi_exit(module);
	return -1;
    }

    shmem_struct->heartbeat = 0;

    /* is timer started? if so, what period? */
    period = rtapi_clock_set_period(0);
    if (period == 0) {
	/* not running, start it */
	rtapi_print("shmemtask init: starting timer with period %ld\n",
	    TIMER_PERIOD_NSEC);
	period = rtapi_clock_set_period(TIMER_PERIOD_NSEC);
	if (period < 0) {
	    rtapi_print
		("shmemtask init: rtapi_clock_set_period failed with %ld\n",
		period);
	    rtapi_exit(module);
	    return -1;
	}
    }
    /* make sure period <= desired period (allow 1% roundoff error) */
    if (period > (TIMER_PERIOD_NSEC + (TIMER_PERIOD_NSEC / 100))) {
	/* timer period too long */
	rtapi_print("shmemtask init: clock period too long: %ld\n", period);
	rtapi_exit(module);
	return -1;
    }
    rtapi_print("shmemtask init: desired clock %ld, actual %ld\n",
	TIMER_PERIOD_NSEC, period);

    /* set the task priority to lowest, since we only have one task */
    shmem_prio = rtapi_prio_lowest();

    /* create the shmem task */
    shmem_task = rtapi_task_new(shmem_code, 0 /* arg */ , shmem_prio, module,
	SHMEM_STACKSIZE, RTAPI_NO_FP);
    if (shmem_task < 0) {
	rtapi_print("shmemtask init: rtapi_task_new returned %d\n",
	    shmem_task);
	rtapi_exit(module);
	return -1;
    }

    /* start the shmem task */
    retval = rtapi_task_start(shmem_task, SHMEM_PERIOD_NSEC);
    if (retval < 0) {
	rtapi_print("shmemtask init: rtapi_task_start returned %d\n", retval);
	rtapi_exit(module);
	return -1;
    }

    rtapi_print("shmemtask init: started shmem task\n");

    return 0;
}
Пример #11
0
int hal_ring_new(const char *name, int size, int sp_size, int mode)
{
    hal_ring_t *rbdesc;
    int *prev, next, cmp, retval;
    int ring_id;
    ringheader_t *rhptr;

    CHECK_HALDATA();
    CHECK_STRLEN(name, HAL_NAME_LEN);
    CHECK_LOCK(HAL_LOCK_LOAD);

    {
	hal_ring_t *ptr __attribute__((cleanup(halpr_autorelease_mutex)));

	rtapi_mutex_get(&(hal_data->mutex));

	// make sure no such ring name already exists
	if ((ptr = halpr_find_ring_by_name(name)) != 0) {
	    HALERR("ring '%s' already exists", name);
	    return -EEXIST;
	}
	// allocate a new ring id - needed since we dont track ring shm
	// segments in RTAPI
	if ((ring_id = next_ring_id()) < 0) {
	    HALERR("cant allocate new ring id for '%s'", name);
	    return -ENOMEM;
	}

	// allocate a new ring descriptor
	if ((rbdesc = alloc_ring_struct()) == 0)
	    NOMEM("ring '%s'", name);

	rbdesc->handle = rtapi_next_handle();
	rbdesc->flags = mode;
	rbdesc->ring_id = ring_id;

	// make total allocation fit ringheader, ringbuffer and scratchpad
	rbdesc->total_size = ring_memsize( rbdesc->flags, size, sp_size);

	if (rbdesc->flags & ALLOC_HALMEM) {
	    void *ringmem = shmalloc_up(rbdesc->total_size);
	    if (ringmem == NULL)
		NOMEM("ring '%s' size %d - insufficient HAL memory for ring",
		      name,rbdesc->total_size);

	    rbdesc->ring_offset = SHMOFF(ringmem);
	    rhptr = ringmem;
	} else {
	    // allocate shared memory segment for ring and init
	    rbdesc->ring_shmkey = OS_KEY((RTAPI_RING_SHM_KEY + ring_id), rtapi_instance);

	    int shmid;

	    // allocate an RTAPI shm segment owned by HAL_LIB_xxx
	    if ((shmid = rtapi_shmem_new(rbdesc->ring_shmkey, lib_module_id,
					 rbdesc->total_size)) < 0)
		NOMEM("rtapi_shmem_new(0x%8.8x,%d) failed: %d",
		       rbdesc->ring_shmkey, lib_module_id,
		       rbdesc->total_size);

	    // map the segment now so we can fill in the ringheader details
	    if ((retval = rtapi_shmem_getptr(shmid,
					     (void **)&rhptr, 0)) < 0)
		NOMEM("rtapi_shmem_getptr for %d failed %d",
		       shmid, retval);
	}

	HALDBG("created ring '%s' in %s, total_size=%d",
	       name, (rbdesc->flags & ALLOC_HALMEM) ? "halmem" : "shm",
	       rbdesc->total_size);

	ringheader_init(rhptr, rbdesc->flags, size, sp_size);
	rhptr->refcount = 0; // on hal_ring_attach: increase; on hal_ring_detach: decrease
	rtapi_snprintf(rbdesc->name, sizeof(rbdesc->name), "%s", name);
	rbdesc->next_ptr = 0;

	// search list for 'name' and insert new structure
	prev = &(hal_data->ring_list_ptr);
	next = *prev;
	while (1) {
	    if (next == 0) {
		/* reached end of list, insert here */
		rbdesc->next_ptr = next;
		*prev = SHMOFF(rbdesc);
		return 0;
	    }
	    ptr = SHMPTR(next);
	    cmp = strcmp(ptr->name, rbdesc->name);
	    if (cmp > 0) {
		/* found the right place for it, insert here */
		rbdesc->next_ptr = next;
		*prev = SHMOFF(rbdesc);
		return 0;
	    }
	    /* didn't find it yet, look at next one */
	    prev = &(ptr->next_ptr);
	    next = *prev;
	}
	// automatic unlock by scope exit
    }
}
Пример #12
0
int hal_ring_attach(const char *name, ringbuffer_t *rbptr,unsigned *flags)
{
    hal_ring_t *rbdesc;
    ringheader_t *rhptr;

    CHECK_HALDATA();
    CHECK_STRLEN(name, HAL_NAME_LEN);

    // no mutex(es) held up to here
    {
	int retval  __attribute__((cleanup(halpr_autorelease_mutex)));
	rtapi_mutex_get(&(hal_data->mutex));

	if ((rbdesc = halpr_find_ring_by_name(name)) == NULL) {
	    HALERR("no such ring '%s'", name);
	    return -ENOENT;
	}

	// calling hal_ring_attach(name, NULL, NULL) is a way to determine
	// if a given ring exists.
	// hal_ring_attach(name, NULL, &flags) is a way to inspect the flags
	// of an existing ring without actually attaching it.
	if (rbptr == NULL) {
	    if (flags)
		*flags = rbdesc->flags;
	    return 0;
	}

	if (rbdesc->flags & ALLOC_HALMEM) {
	    rhptr = SHMPTR(rbdesc->ring_offset);
	} else {
	    int shmid;

	    // map in the shm segment - size 0 means 'must exist'
	    if ((retval = rtapi_shmem_new_inst(rbdesc->ring_shmkey,
					       rtapi_instance, lib_module_id,
					   0 )) < 0) {
		if (retval != -EEXIST)  {
		    HALERR("ring '%s': rtapi_shmem_new_inst() failed %d",
			   name, retval);
		    return retval;
		}
		// tried to map shm again. May happen in halcmd_commands:print_ring_info().
		// harmless.
	    }
	    shmid = retval;

	    // make it accessible
	    if ((retval = rtapi_shmem_getptr(shmid, (void **)&rhptr, 0))) {
		HALERR("ring '%s': rtapi_shmem_getptr %d failed %d",
		       name, shmid, retval);
		return -ENOMEM;
	    }
	}
	// record usage in ringheader
	rhptr->refcount++;
	// fill in ringbuffer_t
	ringbuffer_init(rhptr, rbptr);

	if (flags)
	    *flags = rbdesc->flags;
	// hal mutex unlock happens automatically on scope exit
    }
    return 0;
}
Пример #13
0
int hal_ring_delete(const char *name)
{
    int retval;

    CHECK_HALDATA();
    CHECK_STRLEN(name, HAL_NAME_LEN);
    CHECK_LOCK(HAL_LOCK_LOAD);

    {
	hal_ring_t *hrptr __attribute__((cleanup(halpr_autorelease_mutex)));
	rtapi_mutex_get(&(hal_data->mutex));

	// ring must exist
	if ((hrptr = halpr_find_ring_by_name(name)) == NULL) {
	    HALERR("ring '%s' not found", name);
	    return -ENOENT;
	}

	ringheader_t *rhptr;
	int shmid = -1;

	if (hrptr->flags & ALLOC_HALMEM) {
	    // ring exists as HAL memory.
	    rhptr = SHMPTR(hrptr->ring_offset);
	} else {
	    // ring exists as shm segment. Retrieve shared memory address.
	    if ((shmid = rtapi_shmem_new_inst(hrptr->ring_shmkey,
					      rtapi_instance, lib_module_id,
					      0 )) < 0) {
		if (shmid != -EEXIST)  {
		    HALERR("ring '%s': rtapi_shmem_new_inst() failed %d",
			   name, shmid);
		    return shmid;
		}
	    }
	    if ((retval = rtapi_shmem_getptr(shmid, (void **)&rhptr, 0))) {
		HALERR("ring '%s': rtapi_shmem_getptr %d failed %d",
		       name, shmid, retval);
		return -ENOMEM;
	    }
	}
	// assure attach/detach balance is zero:
	if (rhptr->refcount) {
	    HALERR("ring '%s' still attached - refcount=%d",
		   name, rhptr->refcount);
	    return -EBUSY;
	}

	HALDBG("deleting ring '%s'", name);
	if (hrptr->flags & ALLOC_HALMEM) {
	    ; // if there were a HAL memory free function, call it here
	} else {
	    if ((retval = rtapi_shmem_delete(shmid, lib_module_id)) < 0)  {
		HALERR("ring '%s': rtapi_shmem_delete(%d,%d) failed: %d",
		       name, shmid, lib_module_id, retval);
		return retval;
	    }
	}
	// search for the ring (again..)
	int *prev = &(hal_data->ring_list_ptr);
	int next = *prev;
	while (next != 0) {
	    hrptr = SHMPTR(next);
	    if (strcmp(hrptr->name, name) == 0) {
		// this is the right ring
		// unlink from list
		*prev = hrptr->next_ptr;
		// and delete it, linking it on the free list
		free_ring_struct(hrptr);
		return 0;
	    }
	    // no match, try the next one
	    prev = &(hrptr->next_ptr);
	    next = *prev;
	}

	HALERR("BUG: deleting ring '%s'; not found in ring_list?",
	       name);
	return -ENOENT;
    }

}
Пример #14
0
int main(int argc, gchar * argv[])
{
    int retval;
    int num_samples = SCOPE_NUM_SAMPLES_DEFAULT;
    void *shm_base;

    /* process and remove any GTK specific command line args */
    gtk_init(&argc, &argv);
    /* process halscope command line args (if any) here */
    if(argc > 1) num_samples = atoi(argv[1]);
    if(num_samples <= 0)
	num_samples = SCOPE_NUM_SAMPLES_DEFAULT;

    /* connect to the HAL */
    comp_id = hal_init("halscope");
    if (comp_id < 0) {
	rtapi_print_msg(RTAPI_MSG_ERR, "SCOPE: ERROR: hal_init() failed\n");
	return -1;
    }

    if (!halpr_find_funct_by_name("scope.sample")) {
	char buf[1000];
	sprintf(buf, EMC2_BIN_DIR "/halcmd loadrt scope_rt num_samples=%d",
		num_samples);
	if(system(buf) != 0) {
	    rtapi_print_msg(RTAPI_MSG_ERR, "loadrt scope_rt failed\n");
	    hal_exit(comp_id);
	    exit(1);
	}
    }
    /* set up a shared memory region for the scope data */
    shm_id = rtapi_shmem_new(SCOPE_SHM_KEY, comp_id, sizeof(scope_shm_control_t));
    if (shm_id < 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "SCOPE: ERROR: failed to get shared memory\n");
	hal_exit(comp_id);
	return -1;
    }
    retval = rtapi_shmem_getptr(shm_id, &shm_base);
    if (retval < 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "SCOPE: ERROR: failed to map shared memory\n");
	rtapi_shmem_delete(shm_id, comp_id);
	hal_exit(comp_id);
	return -1;
    }

    hal_ready(comp_id);

    /* register an exit function to cleanup and disconnect from the HAL */
    g_atexit(exit_from_hal);

    /* init control structure */
    ctrl_usr = &ctrl_struct;
    init_usr_control_struct(shm_base);

    /* init watchdog */
    ctrl_shm->watchdog = 10;
    /* set main window */
    define_scope_windows();
    /* this makes the application exit when the window is closed */
    gtk_signal_connect(GTK_OBJECT(ctrl_usr->main_win), "destroy",
	GTK_SIGNAL_FUNC(main_window_closed), NULL);
    gtk_signal_connect(GTK_OBJECT(ctrl_usr->main_win), "focus-in-event",
	GTK_SIGNAL_FUNC(set_focus), NULL);
    /* define menu windows */
    /* do next level of init */
    init_horiz();
    init_vert();
    init_trig();
    init_display();
    init_run_mode_window();
    /* register signal handlers for ctrl-C and SIGTERM */
    signal(SIGINT, quit);
    signal(SIGTERM, quit);

    /* The interface is now completely set up */
    /* show the window */
    gtk_widget_show(ctrl_usr->main_win);
    /* read the saved config file */
    read_config_file(".scope.cfg");
    /* arrange for periodic call of heartbeat() */
    gtk_timeout_add(100, heartbeat, NULL);
    /* enter the main loop */
    gtk_main();
    write_config_file(".scope.cfg");

    return (0);
}