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