// this is now delayed to first hal_init() in this process int hal_rtapi_attach() { int retval; void *mem; char rtapi_name[RTAPI_NAME_LEN + 1]; if (lib_mem_id < 0) { hal_print_msg(RTAPI_MSG_DBG, "HAL: initializing hal_lib\n"); rtapi_snprintf(rtapi_name, RTAPI_NAME_LEN, "HAL_LIB_%d", (int)getpid()); lib_module_id = rtapi_init(rtapi_name); if (lib_module_id < 0) { hal_print_msg(RTAPI_MSG_ERR, "HAL: ERROR: could not not initialize RTAPI - realtime not started?\n"); return -EINVAL; } if (global_data == NULL) { hal_print_msg(RTAPI_MSG_ERR, "HAL: ERROR: RTAPI shutting down - exiting\n"); exit(1); } /* 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: ERROR: could not open shared memory\n"); 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: ERROR: could not access shared memory\n"); 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: ERROR: could not init shared memory\n"); rtapi_exit(lib_module_id); return -EINVAL; } } return 0; }
int rtapi_app_main(void) { int retval; void *shm_base; long skip; /* connect to the HAL */ comp_id = hal_init("scope_rt"); if (comp_id < 0) { rtapi_print_msg(RTAPI_MSG_ERR, "SCOPE: ERROR: hal_init() failed\n"); return -1; } /* connect to scope shared memory block */ skip = (sizeof(scope_shm_control_t) + 3) & ~3; shm_size = skip + num_samples * sizeof(scope_data_t); shm_id = rtapi_shmem_new(SCOPE_SHM_KEY, comp_id, shm_size); if (shm_id < 0) { rtapi_print_msg(RTAPI_MSG_ERR, "SCOPE RT: ERROR: failed to get shared memory (key=0x%x, size=%lu)\n", SCOPE_SHM_KEY, shm_size ); 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; } /* init control structure */ ctrl_rt = &ctrl_struct; init_rt_control_struct(shm_base); /* export scope data sampling function */ retval = hal_export_funct("scope.sample", sample, NULL, 0, 0, comp_id); if (retval != 0) { rtapi_print_msg(RTAPI_MSG_ERR, "SCOPE_RT: ERROR: sample funct export failed\n"); hal_exit(comp_id); return -1; } rtapi_print_msg(RTAPI_MSG_DBG, "SCOPE_RT: installed sample function\n"); hal_ready(comp_id); return 0; }
int main() { int retval; module = rtapi_init("SHMEM_USR"); if (module < 1) { rtapi_print_msg(RTAPI_MSG_ERR, "shmemusr main: rtapi_init returned %d\n", module); return -1; } /* allocate the shared memory structure */ shmem_id = rtapi_shmem_new(key, module, sizeof(SHMEM_STRUCT)); if (shmem_id < 0) { rtapi_print_msg(RTAPI_MSG_ERR, "shmemusr main: rtapi_shmem_new returned %d\n", shmem_id); rtapi_exit(module); return -1; } retval = rtapi_shmem_getptr(shmem_id, (void **) &shmem_struct); if (retval < 0) { rtapi_print_msg(RTAPI_MSG_ERR, "shmemusr main: rtapi_shmem_getptr returned %d\n", retval); rtapi_exit(module); return -1; } signal(SIGINT, quit); while (!done) { rtapi_print("%lu\n", shmem_struct->heartbeat); sleep(1); } retval = rtapi_shmem_delete(shmem_id, module); if (retval < 0) { rtapi_print_msg(RTAPI_MSG_ERR, "shmemusr main: rtapi_free_shmem returned %d\n", retval); rtapi_exit(module); return -1; } return rtapi_exit(module); }
/* 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, global_data->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; }
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; }
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; }
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; }
/* 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; }
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; }
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; }
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; }
/* 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; }
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 } }
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); }