int rtapi_app_main(void) { int n, retval; /* test for number of channels */ if ((num_chan <= 0) || (num_chan > MAX_CHAN)) { rtapi_print_msg(RTAPI_MSG_ERR, "ENCODER_RATIO: ERROR: invalid num_chan: %d\n", num_chan); return -1; } /* have good config info, connect to the HAL */ comp_id = hal_init("encoder_ratio"); if (comp_id < 0) { rtapi_print_msg(RTAPI_MSG_ERR, "ENCODER_RATIO: ERROR: hal_init() failed\n"); return -1; } /* allocate shared memory for encoder data */ encoder_pair_array = hal_malloc(num_chan * sizeof(encoder_pair_t)); if (encoder_pair_array == 0) { rtapi_print_msg(RTAPI_MSG_ERR, "ENCODER_RATIO: ERROR: hal_malloc() failed\n"); hal_exit(comp_id); return -1; } /* set up each encoder pair */ for (n = 0; n < num_chan; n++) { /* export all vars */ retval = export_encoder_pair(n, &(encoder_pair_array[n])); if (retval != 0) { rtapi_print_msg(RTAPI_MSG_ERR, "ENCODER_RATIO: ERROR: counter %d var export failed\n", n); hal_exit(comp_id); return -1; } /* init encoder pair */ encoder_pair_array[n].master_state = 0; encoder_pair_array[n].slave_state = 0; encoder_pair_array[n].master_increment = 0; encoder_pair_array[n].slave_increment = 0; encoder_pair_array[n].raw_error = 0; encoder_pair_array[n].output_scale = 1.0; *(encoder_pair_array[n].error) = 0.0; } /* export functions */ retval = hal_export_funct("encoder-ratio.sample", sample, encoder_pair_array, 0, 0, comp_id); if (retval != 0) { rtapi_print_msg(RTAPI_MSG_ERR, "ENCODER_RATIO: ERROR: sample funct export failed\n"); hal_exit(comp_id); return -1; } retval = hal_export_funct("encoder-ratio.update", update, encoder_pair_array, 1, 0, comp_id); if (retval != 0) { rtapi_print_msg(RTAPI_MSG_ERR, "ENCODER_RATIO: ERROR: update funct export failed\n"); hal_exit(comp_id); return -1; } rtapi_print_msg(RTAPI_MSG_INFO, "ENCODER_RATIO: installed %d encoder_ratio blocks\n", num_chan); hal_ready(comp_id); return 0; }
int rtapi_app_main(void) { int retval, i; int in_cnt = 0, out_cnt = 0; char buf[128]; char *pc = axes_conf; /* Parsing axes configuration */ while(*pc != 0) { int idx = -1; switch(*pc) { case 'X': case 'x': idx = 0; break; case 'Y': case 'y': idx = 1; break; case 'Z': case 'z': idx = 2; break; case 'A': case 'a': idx = 3; break; case 'B': case 'b': idx = 4; break; case 'C': case 'c': idx = 5; break; default: break; } if(idx >= 0) axis_map[num_axis++] = idx; pc++; } fprintf(stderr, "num_axis=%d, fifo_size=%d\n", num_axis, fifo_deep); /* test for number of channels */ if ((num_axis <= 0) || (num_axis > MAX_AXIS)) { rtapi_print_msg(RTAPI_MSG_ERR, "miniemcdrv: ERROR: invalid num_chan: %d\n", num_axis); return -EINVAL; } /* have good config info, connect to the HAL */ comp_id = hal_init("miniemcdrv"); if (comp_id < 0) { rtapi_print_msg(RTAPI_MSG_ERR, "miniemcdrv: ERROR: hal_init() failed\n"); rtapi_app_exit(); return -EINVAL; } pgpio = hal_malloc(sizeof(gpio_t)); memset(pgpio, 0, sizeof(gpio_t)); pgpio->fd = open("/dev/miniemc", O_RDWR | O_SYNC); if(pgpio->fd < 0) { rtapi_print_msg(RTAPI_MSG_ERR, "miniemcdrv: ERROR: unble to create access to stepgen module\n"); rtapi_app_exit(); return -EIO; } pgpio->pfiq = mmap(0, sizeof(struct fiq_ipc_shared), PROT_READ | PROT_WRITE, MAP_FILE | MAP_SHARED, pgpio->fd, 0); if(pgpio->pfiq == MAP_FAILED) { rtapi_print_msg(RTAPI_MSG_ERR, "miniemcdrv: ERROR: unable to mmap stepgen ringbuffer\n"); rtapi_app_exit(); return -EIO; } /* Setup ringbuff size */ fiq_static.rb_size = fifo_deep; memset(&cmd_pos_prev, 0, sizeof(cmd_pos_prev)); memset(&cmd_pos_accum, 0, sizeof(cmd_pos_accum)); //Configure PWM pins and create HAL inputs for( i = 0; i < MAX_PWM; i++) { rtapi_snprintf(buf, HAL_NAME_LEN, "miniemcdrv.%d.pwm-in", i); retval = hal_pin_float_new(buf, HAL_IN, &(pgpio->pwm_duty[i]), comp_id); if (retval != 0) { rtapi_app_exit(); return retval; } if( pwm_pin_num[i] >= 0 ) { emcConfigureDefault( pwm_pin_num[i] ); emcReservePin( pwm_pin_num[i] ); fiq_static.pwm_pin_addr[i] = GPIOS[pwm_pin_num[i]].port_index; fiq_static.pwm_pin_mask[i] = 1L << GPIOS[pwm_pin_num[i]].offset; pgpio->pfiq->pwm_duty_cycle[i] = 0; } else { fiq_static.pwm_pin_mask[i] = 0; fiq_static.pwm_pin_addr[i] = 0; } } // Create axis step and dir pins for(i = 0; i < num_axis; i++) { if(step_pins[i] >=0 && dir_pins[i] >= 0) { if( emcGetPinMode( step_pins[i]) == egpIn || emcGetPinMode( dir_pins[i]) == egpIn ) { rtapi_print_msg(RTAPI_MSG_ERR, "WARN: can't create axis[%d] stepgen, invalid pin\n", i); continue; } fiq_static.axis[i].configured = 0; fiq_static.axis[i].step_pin_addr = GPIOS[step_pins[i]].port_index; fiq_static.axis[i].step_pin_mask = 1L << GPIOS[step_pins[i]].offset; emcConfigureDefault( step_pins[i] ); emcReservePin( step_pins[i] ); fiq_static.axis[i].dir_pin_addr = GPIOS[dir_pins[i]].port_index; fiq_static.axis[i].dir_pin_mask = 1L << GPIOS[dir_pins[i]].offset; emcConfigureDefault( dir_pins[i] ); emcReservePin( dir_pins[i] ); fiq_static.axis[i].dir_pin_pol = dir_polarity[i]; fiq_static.axis[i].configured = 1; } else { rtapi_print_msg(RTAPI_MSG_ERR, "miniemcdrv: WARNING: axis[%d] step and/or dir pin(s) not properly configured, skipping\n", i); } fiq_static.scan_pin_num = -1; } ioctl(pgpio->fd, AXIS_SET_IOCTL, &fiq_static ); /* * Create IO pins */ for( i=0; i < ARR_SZ(GPIOS); i++ ) { if( emcGetPinMode( i ) == egpRsv ) continue; if( emcGetPinMode( i ) == egpIn ) { rtapi_snprintf(buf, HAL_NAME_LEN, "miniemcdrv.%d.pin-in", in_cnt); hal_pin_bit_new(buf, HAL_OUT, &(pgpio->io_pin[i]), comp_id); rtapi_snprintf(buf, HAL_NAME_LEN, "miniemcdrv.%d.pin-in-inv", in_cnt); hal_pin_bit_new(buf, HAL_IN, &(pgpio->io_invert[i]), comp_id); in_cnt++; } else { rtapi_snprintf(buf, HAL_NAME_LEN, "miniemcdrv.%d.pin-out", out_cnt); hal_pin_bit_new(buf, HAL_IN, &(pgpio->io_pin[i]), comp_id); rtapi_snprintf(buf, HAL_NAME_LEN, "miniemcdrv.%d.pin-out-inv", out_cnt); hal_pin_bit_new(buf, HAL_IN, &(pgpio->io_invert[i]), comp_id); out_cnt++; } emcConfigureDefault( i ); } // Trajectory wait output rtapi_snprintf(buf, HAL_NAME_LEN, "miniemcdrv.traj-wait-out"); hal_pin_bit_new(buf, HAL_OUT, &(pgpio->traj_wait), comp_id); *(pgpio->traj_wait) = 1; // Scaner sync rtapi_snprintf(buf, HAL_NAME_LEN, "miniemcdrv.scan-sync-in"); hal_pin_bit_new(buf, HAL_IN, &(pgpio->scan_sync), comp_id); for(i=0; i < num_axis; i++) { // Check if pin already added char contin = 0; int j; for(j=0; j < i; j++) if(axis_map[j] == axis_map[i]) { contin = 1; break; } if(contin) continue; // commanded position pin rtapi_snprintf(buf, HAL_NAME_LEN, "miniemcdrv.%d.cmd-pos", axis_map[i]); retval = hal_pin_float_new(buf, HAL_IN, &(pgpio->cmd_pos[axis_map[i]]), comp_id); if (retval != 0) { rtapi_app_exit(); return retval; } //feedback position pin rtapi_snprintf(buf, HAL_NAME_LEN, "miniemcdrv.%d.fb-pos", axis_map[i]); retval = hal_pin_float_new(buf, HAL_OUT, &(pgpio->fb_pos[axis_map[i]]), comp_id); if (retval != 0) { rtapi_app_exit(); return retval; } } /* export functions */ retval = hal_export_funct("update-miniemcdrv", update, pgpio, 0, 0, comp_id); if (retval != 0) { rtapi_print_msg(RTAPI_MSG_ERR, "miniemcdrv: ERROR: count funct export failed\n"); rtapi_app_exit(); return -EIO; } ioctl(pgpio->fd, SCAN_PIN_SETUP_IOCTL, NULL); //emcConfigurePin(11, egpOut ); //emcSetPin(11, 1 ); hal_ready(comp_id); return 0; }
int rtapi_app_main(void){ int retval; int i, f; char hal_name[HAL_NAME_LEN]; char *types[5] = {"invalid", "bit", "float", "s32", "u32"}; if (!config[0]) { rtapi_print_msg(RTAPI_MSG_ERR, "The mux_generic component requires at least" " one valid format string\n"); return -EINVAL; } comp_id = hal_init("mux_generic"); if (comp_id < 0) { rtapi_print_msg(RTAPI_MSG_ERR, "mux_generic: ERROR: hal_init() failed\n"); return -1; } // allocate shared memory for the base struct mux = hal_malloc(sizeof(mux_t)); if (mux == 0) { rtapi_print_msg(RTAPI_MSG_ERR, "mux_generic component: Out of Memory\n"); hal_exit(comp_id); return -1; } // Count the instances. for (mux->num_insts = 0; config[mux->num_insts];mux->num_insts++) {} mux->insts = hal_malloc(mux->num_insts * sizeof(mux_inst_t)); // Parse the config string for (i = 0; i < mux->num_insts; i++) { char c; int s, p = 0; mux_inst_t *inst = &mux->insts[i]; inst->in_type = -1; inst->out_type = -1; for (f = 0; (c = config[i][f]); f++) { int type; type = 0; switch (c) { case '0': case '1': case '2': case '3': case '4': case '5': case '6': case '7': case '8': case '9': inst->size = (inst->size * 10) + (c - '0'); if (inst->size > MAX_SIZE) inst->size = MAX_SIZE; break; case 'b': case 'B': type = HAL_BIT; break; case 'f': case 'F': type = HAL_FLOAT; break; case 's': case 'S': type = HAL_S32; break; case 'u': case 'U': type = HAL_U32; break; default: rtapi_print_msg(RTAPI_MSG_ERR, "mux_generic: invalid character in " "fmt string\n"); goto fail0; } if (type) { if (inst->in_type == -1) { inst->in_type = type; } else if (inst->out_type == -1) { inst->out_type = type; } else { rtapi_print_msg(RTAPI_MSG_ERR, "mux_generic: too many type " "specifiers in fmt string\n"); goto fail0; } } } if (inst->size < 1) { rtapi_print_msg(RTAPI_MSG_ERR, "mux_generic: No entry count given\n"); goto fail0; } else if (inst->size < 2) { rtapi_print_msg(RTAPI_MSG_ERR, "mux_generic: A one-element mux makes " "no sense\n"); goto fail0; } if (inst->in_type == -1) { rtapi_print_msg(RTAPI_MSG_ERR, "mux_generic: No type specifiers in " "fmt string\n"); goto fail0; } else if (inst->out_type == -1) { inst->out_type = inst->in_type; } retval = rtapi_snprintf(hal_name, HAL_NAME_LEN, "mux-gen.%02i", i); if (inst->in_type == HAL_FLOAT || inst->out_type == HAL_FLOAT) { retval = hal_export_funct(hal_name, write_fp, inst, 1, 0, comp_id); if (retval < 0) { rtapi_print_msg(RTAPI_MSG_ERR, "mux_generic: ERROR: function export" " failed\n"); goto fail0; } } else { retval = hal_export_funct(hal_name, write_nofp, inst, 0, 0, comp_id); if (retval < 0) { rtapi_print_msg(RTAPI_MSG_ERR, "mux_generic: ERROR: function export" " failed\n"); goto fail0; } } // Input pins // if the mux size is a power of 2 then create the bit inputs s = inst->size; for(inst->num_bits = 1; (!((s >>= 1) & 1)); inst->num_bits++); if (s == 1) { //make the bit pins inst->sel_bit = hal_malloc(inst->num_bits * sizeof(hal_bit_t*)); for (p = 0; p < inst->num_bits; p++) { retval = hal_pin_bit_newf(HAL_IN, &inst->sel_bit[p], comp_id, "mux-gen.%02i.sel-bit-%02i", i, p); if (retval != 0) { goto fail0; } } } retval = hal_pin_u32_newf(HAL_IN, &(inst->sel_int), comp_id, "mux-gen.%02i.sel-int", i); if (retval != 0) { goto fail0; } inst->inputs = hal_malloc(inst->size * sizeof(hal_data_u*)); for (p = 0; p < inst->size; p++) { retval = rtapi_snprintf(hal_name, HAL_NAME_LEN, "mux-gen.%02i.in-%s-%02i", i, types[inst->in_type], p); retval = hal_pin_new(hal_name, inst->in_type, HAL_IN, (void**)&(inst->inputs[p]), comp_id); if (retval != 0) { goto fail0; } } // Behaviour-modifiers retval = hal_pin_bit_newf(HAL_IN, &inst->suppress, comp_id, "mux-gen.%02i.suppress-no-input", i); if (retval != 0) { goto fail0; } retval = hal_pin_u32_newf(HAL_IN, &inst->debounce, comp_id, "mux-gen.%02i.debounce-us", i); if (retval != 0) { goto fail0; } retval = hal_param_u32_newf(HAL_RO, &inst->timer, comp_id, "mux-gen.%02i.elapsed", i); if (retval != 0) { goto fail0; } retval = hal_param_u32_newf(HAL_RO, &inst->selection, comp_id, "mux-gen.%02i.selected", i); if (retval != 0) { goto fail0; } //output pins retval = rtapi_snprintf(hal_name, HAL_NAME_LEN, "mux-gen.%02i.out-%s", i, types[inst->out_type]); retval = hal_pin_new(hal_name, inst->out_type, HAL_OUT, (void**)&(inst->output), comp_id); if (retval != 0) { goto fail0; } } hal_ready(comp_id); return 0; fail0: hal_exit(comp_id); return -1; }
int rtapi_app_main(void) { int n, retval = 0; int rev, pinno; char *endptr; if ((rev = rpi_revision()) < 0) return -1; switch (rev) { case 3: rtapi_print_msg(RTAPI_MSG_INFO, "Raspberry Model B Revision 1.0 + ECN0001 (no fuses, D14 removed)\n"); pins = rev1_pins; gpios = rev1_gpios; npins = sizeof(rev1_pins); break; case 2: rtapi_print_msg(RTAPI_MSG_INFO, "Raspberry Model B Revision 1.0\n"); pins = rev1_pins; gpios = rev1_gpios; npins = sizeof(rev1_pins); break; case 4: case 5: case 6: rtapi_print_msg(RTAPI_MSG_INFO, "Raspberry Model B Revision 2.0\n"); pins = rev2_pins; gpios = rev2_gpios; npins = sizeof(rev2_pins); break; default: rtapi_print_msg(RTAPI_MSG_ERR, "HAL_GPIO: ERROR: board revision %d not supported\n", rev); return -1; } port_data = hal_malloc(npins * sizeof(void *)); if (port_data == 0) { rtapi_print_msg(RTAPI_MSG_ERR, "HAL_GPIO: ERROR: hal_malloc() failed\n"); hal_exit(comp_id); return -1; } if (dir == 0) { rtapi_print_msg(RTAPI_MSG_ERR, "HAL_GPIO: ERROR: no config string\n"); return -1; } dir_map = strtoul(dir, &endptr,0); if (*endptr) { rtapi_print_msg(RTAPI_MSG_ERR, "HAL_GPIO: dir=%s - trailing garbage: '%s'\n", dir, endptr); return -1; } if (exclude == 0) { rtapi_print_msg(RTAPI_MSG_ERR, "HAL_GPIO: ERROR: no exclude string\n"); return -1; } exclude_map = strtoul(exclude, &endptr,0); if (*endptr) { rtapi_print_msg(RTAPI_MSG_ERR, "HAL_GPIO: exclude=%s - trailing garbage: '%s'\n", exclude, endptr); return -1; } if (setup_gpio_access()) return -1; comp_id = hal_init("hal_gpio"); if (comp_id < 0) { rtapi_print_msg(RTAPI_MSG_ERR, "HAL_GPIO: ERROR: hal_init() failed\n"); return -1; } for (n = 0; n < npins; n++) { if (exclude_map & RTAPI_BIT(n)) continue; pinno = pins[n]; if (dir_map & RTAPI_BIT(n)) { bcm2835_gpio_fsel(gpios[n], BCM2835_GPIO_FSEL_OUTP); if ((retval = hal_pin_bit_newf(HAL_IN, &port_data[n], comp_id, "hal_gpio.pin-%02d-out", pinno)) < 0) break; } else { bcm2835_gpio_fsel(gpios[n], BCM2835_GPIO_FSEL_INPT); if ((retval = hal_pin_bit_newf(HAL_OUT, &port_data[n], comp_id, "hal_gpio.pin-%02d-in", pinno)) < 0) break; } } if (retval < 0) { rtapi_print_msg(RTAPI_MSG_ERR, "HAL_GPIO: ERROR: pin %d export failed with err=%i\n", n,retval); hal_exit(comp_id); return -1; } retval = hal_export_funct("hal_gpio.write", write_port, 0, 0, 0, comp_id); if (retval < 0) { rtapi_print_msg(RTAPI_MSG_ERR, "HAL_GPIO: ERROR: write funct export failed\n"); hal_exit(comp_id); return -1; } retval = hal_export_funct("hal_gpio.read", read_port, 0, 0, 0, comp_id); if (retval < 0) { rtapi_print_msg(RTAPI_MSG_ERR, "HAL_GPIO: ERROR: read funct export failed\n"); hal_exit(comp_id); return -1; } rtapi_print_msg(RTAPI_MSG_INFO, "HAL_GPIO: installed driver\n"); hal_ready(comp_id); return 0; }
static int init_streamer(int num, fifo_t *tmp_fifo) { int size, retval, n, usefp; void *shmem_ptr; streamer_t *str; pin_data_t *pptr; fifo_t *fifo; char buf[HAL_NAME_LEN + 2]; /* alloc shmem for base streamer data and user specified pins */ size = sizeof(streamer_t) + tmp_fifo->num_pins*sizeof(pin_data_t); str = hal_malloc(size); if (str == 0) { rtapi_print_msg(RTAPI_MSG_ERR, "STREAMER: ERROR: couldn't allocate HAL shared memory\n"); return -ENOMEM; } /* export "standard" pins and params */ retval = hal_pin_bit_newf(HAL_OUT, &(str->empty), comp_id, "streamer.%d.empty", num); if (retval != 0 ) { rtapi_print_msg(RTAPI_MSG_ERR, "STREAMER: ERROR: 'empty' pin export failed\n"); return -EIO; } retval = hal_pin_bit_newf(HAL_IN, &(str->enable), comp_id, "streamer.%d.enable", num); if (retval != 0 ) { rtapi_print_msg(RTAPI_MSG_ERR, "STREAMER: ERROR: 'enable' pin export failed\n"); return -EIO; } retval = hal_pin_s32_newf(HAL_OUT, &(str->curr_depth), comp_id, "streamer.%d.curr-depth", num); if (retval != 0 ) { rtapi_print_msg(RTAPI_MSG_ERR, "STREAMER: ERROR: 'curr_depth' pin export failed\n"); return -EIO; } retval = hal_pin_s32_newf(HAL_IO, &(str->underruns), comp_id, "streamer.%d.underruns", num); if (retval != 0 ) { rtapi_print_msg(RTAPI_MSG_ERR, "STREAMER: ERROR: 'underruns' pin export failed\n"); return -EIO; } /* init the standard pins and params */ *(str->empty) = 1; *(str->enable) = 1; *(str->curr_depth) = 0; *(str->underruns) = 0; /* HAL pins are right after the streamer_t struct in HAL shmem */ pptr = (pin_data_t *)(str+1); usefp = 0; /* export user specified pins (the ones that stream data) */ for ( n = 0 ; n < tmp_fifo->num_pins ; n++ ) { rtapi_snprintf(buf, HAL_NAME_LEN, "streamer.%d.pin.%d", num, n); retval = hal_pin_new(buf, tmp_fifo->type[n], HAL_OUT, (void **)pptr, comp_id ); if (retval != 0 ) { rtapi_print_msg(RTAPI_MSG_ERR, "STREAMER: 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, "streamer.%d", num); retval = hal_export_funct(buf, update, str, usefp, 0, comp_id); if (retval != 0) { rtapi_print_msg(RTAPI_MSG_ERR, "STREAMER: ERROR: function export failed\n"); return retval; } /* alloc shmem for user/RT comms (fifo) */ size = sizeof(fifo_t) + tmp_fifo->num_pins * tmp_fifo->depth * sizeof(shmem_data_t); shmem_id[num] = rtapi_shmem_new(STREAMER_SHMEM_KEY+num, comp_id, size); if ( shmem_id[num] < 0 ) { rtapi_print_msg(RTAPI_MSG_ERR, "STREAMER: 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, "STREAMER: 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; /* mark it inited for user program */ fifo->magic = FIFO_MAGIC_NUM; return 0; }
static int export_delayline(int n) { int retval, nr_of_samples, i; char *str_type; // determine the required size of the ringbuffer nr_of_samples = return_instance_samples(n); size_t sample_size = sizeof(sample_t) + (nr_of_samples * sizeof(hal_data_u)); // add some headroom to be sure we dont overrun size_t rbsize = record_space(sample_size) * max_delay[n] * RB_HEADROOM; // create the delay queue if ((retval = hal_ring_newf(rbsize, sizeof(hal_delayline_t), ALLOC_HALMEM, "%s.samples", names[n])) < 0) { hal_print_msg(RTAPI_MSG_ERR, "%s: failed to create new ring '%s.samples': %d\n", cname, names[n], retval); return retval; } // use the per-using component ring access structure as the instance data, // which will also give us a handle on the scratchpad which we use for // HAL pins and other shared data if ((instance[n] = hal_malloc(sizeof(ringbuffer_t))) == NULL) return -1; if ((retval = hal_ring_attachf(instance[n], NULL, "%s.samples", names[n]))) { hal_print_msg(RTAPI_MSG_ERR, "%s: attach to ring '%s.samples' failed: %d\n", cname, names[n], retval); return -1; } // fill in instance data hal_delayline_t *hd = instance[n]->scratchpad; strncpy(hd->name, names[n], sizeof(hd->name)); hd->nsamples = nr_of_samples; hd->sample_size = sample_size; hd->max_delay = max_delay[n]; // set delay standard to value of zero hd->delay = 0; hd->output_ts = 0; hd->input_ts = hd->delay; // init pins, and at the same time fill the puntype array with // the type of pin[i] so we can later dereference proper char character; for (i = 0; i < hd->nsamples; i++) { character = samples[n][i]; rtapi_print_msg(RTAPI_MSG_DBG, "\"samples=\" string = %s" " character %d = %c \n", samples[n], i, character); // determine type hal_type_t type = HAL_TYPE_UNSPECIFIED; switch (character) { case 'b': case 'B': type = HAL_BIT; break; case 'f': case 'F': type = HAL_FLOAT; break; case 's': type = HAL_S32; break; case 'u': type = HAL_U32; break; case 'S': type = HAL_S64; break; case 'U': type = HAL_U64; break; default: hal_print_msg(RTAPI_MSG_ERR, "%s: invalid type '%c' for pin %d\n", cname, character, i); return -EINVAL; } hd->pintype[i] = type; switch (type) { case HAL_BIT: str_type = "bit"; break; case HAL_FLOAT: str_type = "flt"; break; case HAL_U32: str_type = "u32"; break; case HAL_S32: str_type = "s32"; break; case HAL_S64: str_type = "s64"; break; case HAL_U64: str_type = "u64"; break; case HAL_TYPE_MAX: case HAL_TYPE_UNSPECIFIED: // do nothing break; } // create pins of type as requested if (((retval = hal_pin_newf(type, HAL_IN, (void **) &hd->pins_in[i], comp_id, "%s.in%d-%s", hd->name, i, str_type)) < 0) || ((retval = hal_pin_newf(type, HAL_OUT, (void **) &hd->pins_out[i], comp_id, "%s.out%d-%s", hd->name, i, str_type)) < 0)) { return retval; } // post hal_pin_new(), pins are guaranteed to be set to zero } // create other pins if (((retval = hal_pin_bit_newf(HAL_IN, &(hd->enable), comp_id, "%s.enable", hd->name))) || ((retval = hal_pin_bit_newf(HAL_IN, &(hd->abort), comp_id, "%s.abort", hd->name))) || ((retval = hal_pin_u32_newf(HAL_IN, &(hd->pin_delay), comp_id, "%s.delay", hd->name))) || ((retval = hal_pin_u32_newf(HAL_OUT, &(hd->write_fail), comp_id, "%s.write-fail", hd->name))) || ((retval = hal_pin_u32_newf(HAL_OUT, &(hd->read_fail), comp_id, "%s.too-old", hd->name))) || ((retval = hal_pin_u32_newf(HAL_OUT, &(hd->too_old), comp_id, "%s.read-fail", hd->name)))) return retval; // export thread functions if (((retval = hal_export_functf(write_sample_to_ring, instance[n], 1, 0, comp_id, "%s.sample", hd->name)) < 0) || ((retval = hal_export_functf(read_sample_from_ring, instance[n], 1, 0, comp_id, "%s.output", hd->name)) < 0)) { return retval; } return 0; }
int rtapi_app_main(void) { int i, j; struct pci_dev *pDev = NULL; MotencRegMap *pCard = NULL; Device *pDevice; // Connect to the HAL. driver.componentId = hal_init("hal_motenc"); if (driver.componentId < 0) { rtapi_print_msg(RTAPI_MSG_ERR, "MOTENC: ERROR: hal_init() failed\n"); return(-EINVAL); } for(i = 0; i < MAX_DEVICES; i++) { driver.deviceTable[i] = NULL; driver.idPresent[i] = 0; } i = 0; // Find a MOTENC card. while((i < MAX_DEVICES) && ((pDev = pci_find_device(MOTENC_VENDOR_ID, MOTENC_DEVICE_ID, pDev)) != NULL)) { // Allocate memory for device object. pDevice = hal_malloc(sizeof(Device)); if (pDevice == 0) { rtapi_print_msg(RTAPI_MSG_ERR, "MOTENC: ERROR: hal_malloc() failed\n"); hal_exit(driver.componentId); return(-ENOMEM); } // Save pointer to device object. driver.deviceTable[i++] = pDevice; // Map card into memory. pCard = (MotencRegMap *)ioremap_nocache(pci_resource_start(pDev, 2), pci_resource_len(pDev, 2)); rtapi_print_msg(RTAPI_MSG_INFO, "MOTENC: Card detected in slot %2x\n", PCI_SLOT(pDev->devfn)); rtapi_print_msg(RTAPI_MSG_INFO, "MOTENC: Card address @ %p, Len = %d\n", pCard, (int)pci_resource_len(pDev, 2)); // Initialize device. Device_Init(pDevice, pCard); rtapi_print_msg(RTAPI_MSG_INFO, "MOTENC: Card is %s, ID: %d\n", pDevice->pTypeName, pDevice->boardID); if ( pDevice->boardType == 0 ) { rtapi_print_msg(RTAPI_MSG_ERR, "MOTENC: ERROR, unknown card detected\n"); hal_exit(driver.componentId); return(-ENODEV); } if ( driver.idPresent[pDevice->boardID] != 0 ) { // duplicate ID... a strict driver would bail out, but // we are nice, we try to find an unused ID j = 0; while ( driver.idPresent[j] != 0 ) { j++; if ( j >= MAX_DEVICES ) { rtapi_print_msg(RTAPI_MSG_ERR, "MOTENC: ERROR, duplicate ID, can't remap\n"); hal_exit(driver.componentId); return(-EINVAL); } } pDevice->boardID = j; rtapi_print_msg(RTAPI_MSG_WARN, "MOTENC: WARNING, duplicate ID, remapped to %d\n", j); } driver.idPresent[pDevice->boardID] = 1; // Export pins, parameters, and functions. if(Device_ExportPinsParametersFunctions(pDevice, driver.componentId)) { hal_exit(driver.componentId); return(-EINVAL); } } if(pCard == NULL) { // No card present. rtapi_print_msg(RTAPI_MSG_WARN, "MOTENC: **** No MOTENC card detected ****\n"); hal_exit(driver.componentId); return -ENODEV; } hal_ready(driver.componentId); return(0); }
int rtapi_app_main(void) { int retval,n,i; Pid *pComp; if(num_chan && names[0]) { rtapi_print_msg(RTAPI_MSG_ERR,"num_chan= and names= are mutually exclusive\n"); return -EINVAL; } if(!num_chan && !names[0]) num_chan = default_num_chan; if(num_chan) { howmany = num_chan; } else { howmany = 0; for (i = 0; i < MAX_CHAN; i++) { if ( (names[i] == NULL) || (*names[i] == 0) ){ break; } howmany = i + 1; } } // Check number of channels. if((howmany <= 0) || (howmany > MAX_CHAN)){ rtapi_print_msg(RTAPI_MSG_ERR, "AT_PID: ERROR: invalid number of channels: %d\n", howmany); return(-1); } // Connect to the HAL. component.id = hal_init("at_pid"); if(component.id < 0){ rtapi_print_msg(RTAPI_MSG_ERR, "PID: ERROR: hal_init() failed\n"); return(-1); } // Allocate memory for pid objects. component.pidTable = hal_malloc(howmany * sizeof(*pComp)); if(component.pidTable == NULL){ rtapi_print_msg(RTAPI_MSG_ERR, "PID: ERROR: hal_malloc() failed\n"); hal_exit(component.id); return(-1); } pComp = component.pidTable; i = 0; // for names= items for(n = 0; n < howmany; n++, pComp++){ // Export pins, parameters, and functions. if(num_chan) { char buf[HAL_NAME_LEN + 1]; // note name is pid not at_pid rtapi_snprintf(buf, sizeof(buf), "pid.%d", n); retval = Pid_Export(pComp, component.id, buf); } else { retval = Pid_Export(pComp, component.id, names[i++]); } if (retval != 0) { rtapi_print_msg(RTAPI_MSG_ERR, "AT_PID: ERROR: at_pid %d var export failed\n", n); hal_exit(component.id); return -1; } // Initialize pid. if(Pid_Init(pComp)){ hal_exit(component.id); return(-1); } } hal_ready(component.id); return(0); }
int rtapi_app_main(void) { int n, retval; for (n = 0; n < MAX_CHAN && output_type[n] != -1 ; n++) { if ((output_type[n] > MAX_OUTPUT_TYPE) || (output_type[n] < 0)) { rtapi_print_msg(RTAPI_MSG_ERR, "PWMGEN: ERROR: bad output type '%i', channel %i\n", output_type[n], n); return -1; } else { num_chan++; } } if (num_chan == 0) { rtapi_print_msg(RTAPI_MSG_ERR, "PWMGEN: ERROR: no channels configured\n"); return -1; } /* periodns will be set to the proper value when 'make_pulses()' runs for the first time. We load a default value here to avoid glitches at startup */ periodns = 50000; /* have good config info, connect to the HAL */ comp_id = hal_init("pwmgen"); if (comp_id < 0) { rtapi_print_msg(RTAPI_MSG_ERR, "PWMGEN: ERROR: hal_init() failed\n"); return -1; } /* allocate shared memory for generator data */ pwmgen_array = hal_malloc(num_chan * sizeof(pwmgen_t)); if (pwmgen_array == 0) { rtapi_print_msg(RTAPI_MSG_ERR, "PWMGEN: ERROR: hal_malloc() failed\n"); hal_exit(comp_id); return -1; } /* export all the variables for each PWM generator */ for (n = 0; n < num_chan; n++) { /* export all vars */ retval = export_pwmgen(n, &(pwmgen_array[n]), output_type[n]); if (retval != 0) { rtapi_print_msg(RTAPI_MSG_ERR, "PWMGEN: ERROR: pwmgen %d var export failed\n", n); hal_exit(comp_id); return -1; } } /* export functions */ retval = hal_export_funct("pwmgen.make-pulses", make_pulses, pwmgen_array, 0, 0, comp_id); if (retval != 0) { rtapi_print_msg(RTAPI_MSG_ERR, "PWMGEN: ERROR: makepulses funct export failed\n"); hal_exit(comp_id); return -1; } retval = hal_export_funct("pwmgen.update", update, pwmgen_array, 1, 0, comp_id); if (retval != 0) { rtapi_print_msg(RTAPI_MSG_ERR, "PWMGEN: ERROR: update funct export failed\n"); hal_exit(comp_id); return -1; } rtapi_print_msg(RTAPI_MSG_INFO, "PWMGEN: installed %d PWM/PDM generators\n", num_chan); hal_ready(comp_id); return 0; }
int rtapi_app_main(void) { int result, i; CopySizesInfosFromModuleParams(); compId = hal_init("classicladder_rt"); if(compId < 0) return compId; rtapi_print("creating ladder-state\n"); result = hal_export_funct("classicladder.0.refresh",hal_task,0,1, 0, compId); if(result < 0) { error: hal_exit(compId); return result; } hal_state = hal_malloc(sizeof(hal_s32_t)); result = hal_param_s32_new("classicladder.ladder-state", HAL_RO, hal_state, compId); if(result < 0) { hal_exit(compId); return result; } hal_inputs = hal_malloc(sizeof(hal_bit_t*) * numPhysInputs); if(!hal_inputs) { result = -ENOMEM; goto error; } hide_gui = hal_malloc(sizeof(hal_bit_t*)); if(!hide_gui) { result = -ENOMEM; goto error; } hal_s32_inputs = hal_malloc(sizeof(hal_s32_t*) * numS32in); if(!hal_s32_inputs) { result = -ENOMEM; goto error; } hal_float_inputs = hal_malloc(sizeof(hal_float_t*) * numFloatIn); if(!hal_float_inputs) { result = -ENOMEM; goto error; } hal_outputs = hal_malloc(sizeof(hal_bit_t*) * numPhysOutputs); if(!hal_outputs) { result = -ENOMEM; goto error; } hal_s32_outputs = hal_malloc(sizeof(hal_s32_t*) * numS32out); if(!hal_s32_outputs) { result = -ENOMEM; goto error; } hal_float_outputs = hal_malloc(sizeof(hal_float_t*) * numFloatOut); if(!hal_float_outputs) { result = -ENOMEM; goto error; } for(i=0; i<numPhysInputs; i++) { result = hal_pin_bit_newf(HAL_IN, &hal_inputs[i], compId, "classicladder.0.in-%02d", i); if(result < 0) goto error; } result = hal_pin_bit_newf(HAL_IN, &hide_gui[0], compId, "classicladder.0.hide_gui"); if(result < 0) goto error; for(i=0; i<numS32in; i++) { result = hal_pin_s32_newf(HAL_IN, &hal_s32_inputs[i], compId, "classicladder.0.s32in-%02d", i); if(result < 0) goto error; } for(i=0; i<numFloatIn; i++) { result = hal_pin_float_newf(HAL_IN, &hal_float_inputs[i], compId, "classicladder.0.floatin-%02d", i); if(result < 0) goto error; } for(i=0; i<numPhysOutputs; i++) { result = hal_pin_bit_newf(HAL_OUT, &hal_outputs[i], compId, "classicladder.0.out-%02d", i); if(result < 0) goto error; } for(i=0; i<numS32out; i++) { result = hal_pin_s32_newf(HAL_OUT, &hal_s32_outputs[i], compId, "classicladder.0.s32out-%02d", i); if(result < 0) goto error; } for(i=0; i<numFloatOut; i++) { result = hal_pin_float_newf(HAL_OUT, &hal_float_outputs[i], compId, "classicladder.0.floatout-%02d", i); if(result < 0) goto error; } hal_ready(compId); ClassicLadder_AllocAll( ); return 0; }
int hm2_uart_parse_md(hostmot2_t *hm2, int md_index) { // All this function actually does is allocate memory // and give the uart modules names. // // some standard sanity checks // int i, r = -EINVAL; hm2_module_descriptor_t *md = &hm2->md[md_index]; static int last_gtag = -1; //The UART declares a TX and RX module separately if (!hm2_md_is_consistent_or_complain(hm2, md_index, 0, 4, 0x10, 0x000F)) { HM2_ERR("inconsistent Module Descriptor!\n"); return -EINVAL; } if (hm2->uart.num_instances > 1 && last_gtag == md->gtag) { HM2_ERR( "found duplicate Module Descriptor for %s (inconsistent " "firmware), not loading driver %i %i\n", hm2_get_general_function_name(md->gtag), md->gtag, last_gtag ); return -EINVAL; } last_gtag = md->gtag; if (hm2->config.num_uarts > md->instances) { HM2_ERR( "config defines %d uarts, but only %d are available, " "not loading driver\n", hm2->config.num_uarts, md->instances ); return -EINVAL; } if (hm2->config.num_uarts == 0) { return 0; } // // looks good, start, or continue, initializing // if (hm2->uart.num_instances == 0){ if (hm2->config.num_uarts == -1) { hm2->uart.num_instances = md->instances; } else { hm2->uart.num_instances = hm2->config.num_uarts; } hm2->uart.instance = (hm2_uart_instance_t *)hal_malloc(hm2->uart.num_instances * sizeof(hm2_uart_instance_t)); if (hm2->uart.instance == NULL) { HM2_ERR("out of memory!\n"); r = -ENOMEM; goto fail0; } } for (i = 0 ; i < hm2->uart.num_instances ; i++){ hm2_uart_instance_t *inst = &hm2->uart.instance[i]; // For the time being we assume that all UARTS come on pairs if (inst->clock_freq == 0){ inst->clock_freq = md->clock_freq; r = sprintf(inst->name, "%s.uart.%01d", hm2->llio->name, i); HM2_PRINT("created UART Interface function %s.\n", inst->name); } if (md->gtag == HM2_GTAG_UART_TX){ inst->tx1_addr = md->base_address + i * md->instance_stride; inst->tx2_addr = md->base_address + i * md->instance_stride + 0x4; inst->tx3_addr = md->base_address + i * md->instance_stride + 0x8; inst->tx4_addr = md->base_address + i * md->instance_stride + 0xC; inst->tx_fifo_count_addr = (md->base_address + md->register_stride + i * md->instance_stride); inst->tx_bitrate_addr = (md->base_address + 2 * md->register_stride + i * md->instance_stride); inst->tx_mode_addr = (md->base_address + 3 * md->register_stride +i * md->instance_stride); } else if (md->gtag == HM2_GTAG_UART_RX){ inst->rx1_addr = md->base_address + i * md->instance_stride; inst->rx2_addr = md->base_address + i * md->instance_stride + 0x4; inst->rx3_addr = md->base_address + i * md->instance_stride + 0x8; inst->rx4_addr = md->base_address + i * md->instance_stride + 0xC; inst->rx_fifo_count_addr = (md->base_address + md->register_stride + i * md->instance_stride); inst->rx_bitrate_addr = (md->base_address + 2 * md->register_stride + i * md->instance_stride); inst->rx_mode_addr = (md->base_address + 3 * md->register_stride +i * md->instance_stride); } else{ HM2_ERR("Something very wierd happened"); goto fail0; } } return hm2->uart.num_instances; fail0: return r; }
int rtapi_app_main(void) { int n, retval, i; if(num_chan && names[0]) { rtapi_print_msg(RTAPI_MSG_ERR,"num_chan= and names= are mutually exclusive\n"); return -EINVAL; } if(!num_chan && !names[0]) num_chan = default_num_chan; if(num_chan) { howmany = num_chan; } else { for(i=0; names[i]; i++) {howmany = i+1;} } if ((howmany <= 0) || (howmany > MAX_CHAN)) { rtapi_print_msg(RTAPI_MSG_ERR, "SIM_ENCODER: ERROR: invalid number of channels %d\n", howmany); return -1; } /* periodns will be set to the proper value when 'make_pulses()' runs for the first time. We load a default value here to avoid glitches at startup, but all these 'constants' are recomputed inside 'update_speed()' using the real period. */ periodns = 50000; /* precompute some constants */ periodfp = periodns * 0.000000001; maxf = 1.0 / periodfp; freqscale = ((1L << 30) * 2.0) / maxf; old_periodns = periodns; /* have good config info, connect to the HAL */ comp_id = hal_init("sim_encoder"); if (comp_id < 0) { rtapi_print_msg(RTAPI_MSG_ERR, "SIM_ENCODER: ERROR: hal_init() failed\n"); return -1; } /* allocate shared memory for encoder data */ sim_enc_array = hal_malloc(howmany * sizeof(sim_enc_t)); if (sim_enc_array == 0) { rtapi_print_msg(RTAPI_MSG_ERR, "SIM_ENCODER: ERROR: hal_malloc() failed\n"); hal_exit(comp_id); return -1; } /* export all the variables for each simulated encoder */ i = 0; //for names= items for (n = 0; n < howmany; n++) { /* export all vars */ if(num_chan) { char buf[HAL_NAME_LEN + 1]; rtapi_snprintf(buf, sizeof(buf), "sim-encoder.%d", n); retval = export_sim_enc(&(sim_enc_array[n]),buf); } else { retval = export_sim_enc(&(sim_enc_array[n]),names[i++]); } if (retval != 0) { rtapi_print_msg(RTAPI_MSG_ERR, "SIM_ENCODER: ERROR: 'encoder' %d var export failed\n", n); hal_exit(comp_id); return -1; } } /* export functions */ retval = hal_export_funct("sim-encoder.make-pulses", make_pulses, sim_enc_array, 0, 0, comp_id); if (retval != 0) { rtapi_print_msg(RTAPI_MSG_ERR, "SIM_ENCODER: ERROR: makepulses funct export failed\n"); hal_exit(comp_id); return -1; } retval = hal_export_funct("sim-encoder.update-speed", update_speed, sim_enc_array, 1, 0, comp_id); if (retval != 0) { rtapi_print_msg(RTAPI_MSG_ERR, "SIM_ENCODER: ERROR: speed update funct export failed\n"); hal_exit(comp_id); return -1; } rtapi_print_msg(RTAPI_MSG_INFO, "SIM_ENCODER: installed %d simulated encoders\n", howmany); hal_ready(comp_id); return 0; }
int rtapi_app_main(void) { int n, total_bits, retval; total_bits = 0; /* check that there's at least one valid summer requested */ for (n = 0; n < MAX_SUMMERS && wsum_sizes[n] != -1 ; n++) { if ((wsum_sizes[n] > MAX_SUM_BITS) || (wsum_sizes[n]<1)) { rtapi_print_msg(RTAPI_MSG_ERR, "WEIGHTED_SUM: ERROR: Invalid number of bits '%i' for summer %i\n", wsum_sizes[n], n); return -1; } else { num_summers++; total_bits += wsum_sizes[n]; } } if (num_summers == 0) { rtapi_print_msg(RTAPI_MSG_ERR, "WEIGHTED_SUM: ERROR: no summers specified\n"); return -1; } /* have good config info, connect to the HAL */ comp_id = hal_init("weighted_sum"); if (comp_id < 0) { rtapi_print_msg(RTAPI_MSG_ERR, "WEIGHTED_SUM: ERROR: hal_init() failed\n"); return -1; } /* allocate shared memory for summer base info */ wsum_array = hal_malloc(num_summers * sizeof(wsum_t)); if (wsum_array == 0) { rtapi_print_msg(RTAPI_MSG_ERR, "WEIGHTED_SUM: ERROR: hal_malloc() for summer array failed\n"); hal_exit(comp_id); return -1; } /* allocate shared memory for summer bit arrays */ wsum_bit_array = hal_malloc(total_bits * sizeof(wsum_bit_t)); if (wsum_bit_array == 0) { rtapi_print_msg(RTAPI_MSG_ERR, "WEIGHTED_SUM: ERROR: hal_malloc() for summer bit array failed\n"); hal_exit(comp_id); return -1; } /* export pins/params for all summers */ total_bits = 0; for (n = 0; n < num_summers; n++) { /* export all vars */ retval = export_wsum(n, wsum_sizes[n], &(wsum_array[n]), &(wsum_bit_array[total_bits])); if (retval != 0) { rtapi_print_msg(RTAPI_MSG_ERR, "WEIGHTED_SUM: ERROR: group %d export failed\n", n); hal_exit(comp_id); return -1; } total_bits += wsum_array[n].num_bits; } /* export update function */ retval = hal_export_funct("process_wsums", process_wsums, wsum_array, 1, 0, comp_id); if (retval != 0) { rtapi_print_msg(RTAPI_MSG_ERR, "WEIGHTED_SUM: ERROR: process_wsums funct export failed\n"); return -1; } rtapi_print_msg(RTAPI_MSG_INFO, "WEIGHTED_SUM: installed %d weighted summers\n", num_summers); hal_ready(comp_id); return 0; }
int rtapi_app_main(void){ int i, j, n; int retval; comp_id = hal_init("matrix_kb"); if (comp_id < 0) { rtapi_print_msg(RTAPI_MSG_ERR, "matrix_kb: ERROR: hal_init() failed\n"); return -1; } // allocate shared memory for data kb = hal_malloc(sizeof(kb_t)); if (kb == 0) { rtapi_print_msg(RTAPI_MSG_ERR, "matrix_kb component: Out of Memory\n"); hal_exit(comp_id); return -1; } // Count the instances. for (kb->num_insts = 0; config[kb->num_insts];kb->num_insts++); // Count the names. for (n = 0; names[n];n++); if (n && n != kb->num_insts){ rtapi_print_msg(RTAPI_MSG_ERR, "matrix_kb: Number of sizes and number" " of names must match\n"); hal_exit(comp_id); return -1; } kb->insts = hal_malloc(kb->num_insts * sizeof(kb_inst_t)); for (i = 0; i < kb->num_insts; i++){ int a = 0; int c, r; kb_inst_t *inst = &kb->insts[i]; inst->index = i; inst->nrows = 0; inst->ncols = 0; inst->scan = 0; inst->keystroke = 0; inst->param.invert = 1; for(j = 0; config[i][j] !=0; j++){ int n = (config[i][j] | 0x20); //lower case if (n == 'x'){ inst->nrows = a; a = 0; } else if (n >= '0' && n <= '9'){ a = (a * 10) + (n - '0'); } else if (n == 's'){ inst->scan = 1; } } inst->ncols = a; if (inst->ncols == 0 || inst->nrows == 0){ rtapi_print_msg(RTAPI_MSG_ERR, "matrix_kb: Invalid size format. should be NxN\n"); hal_exit(comp_id); return -1; } if (inst->ncols > 32){ rtapi_print_msg(RTAPI_MSG_ERR, "matrix_kb: maximum number of columns is 32. Sorry\n"); hal_exit(comp_id); return -1; } for (inst->rowshift = 1; inst->ncols > (1 << inst->rowshift); inst->rowshift++); for (inst->keydown = 0xC0, inst->keyup = 0x80 ; (inst->nrows << inst->rowshift) > inst->keydown ; inst->keydown <<= 1, inst->keyup <<= 1); inst->hal.key = (hal_bit_t **)hal_malloc(inst->nrows * inst->ncols * sizeof(hal_bit_t*)); inst->now = hal_malloc(inst->nrows * sizeof(hal_u32_t)); inst->then = hal_malloc(inst->nrows * sizeof(hal_u32_t)); inst->row = 0; inst->param.rollover = 2; if (names[i]){ rtapi_snprintf(inst->name, sizeof(inst->name), "%s", names[i]); } else { rtapi_snprintf(inst->name, sizeof(inst->name), "matrix_kb.%i", i); } for (c = 0; c < inst->ncols; c++){ for (r = 0; r < inst->nrows; r++){ retval = hal_pin_bit_newf(HAL_OUT, &(inst->hal.key[r * inst->ncols + c]), comp_id, "%s.key.r%xc%x", inst->name, r, c); if (retval != 0) { rtapi_print_msg(RTAPI_MSG_ERR, "matrix_kb: Failed to create output pin\n"); hal_exit(comp_id); return -1; } } } if (inst->scan){ //internally generated scanning inst->hal.rows = (hal_bit_t **)hal_malloc(inst->nrows * sizeof(hal_bit_t*)); inst->hal.cols = (hal_bit_t **)hal_malloc(inst->ncols * sizeof(hal_bit_t*)); for (r = 0; r < inst->nrows; r++){ retval = hal_pin_bit_newf(HAL_OUT, &(inst->hal.rows[r]), comp_id, "%s.row-%02i-out",inst->name, r); if (retval != 0) { rtapi_print_msg(RTAPI_MSG_ERR, "matrix_kb: Failed to create output row pin\n"); hal_exit(comp_id); return -1; } } for (c = 0; c < inst->ncols; c++){ retval = hal_pin_bit_newf(HAL_IN, &(inst->hal.cols[c]), comp_id, "%s.col-%02i-in",inst->name, c); if (retval != 0) { rtapi_print_msg(RTAPI_MSG_ERR, "matrix_kb: Failed to create input col pin\n"); hal_exit(comp_id); return -1; } } retval = hal_pin_u32_newf(HAL_OUT, &(inst->hal.keycode), comp_id, "%s.keycode",inst->name); if (retval != 0) { rtapi_print_msg(RTAPI_MSG_ERR, "matrix_kb: Failed to create output pin\n"); hal_exit(comp_id); return -1; } retval = hal_param_bit_newf(HAL_RW, &(inst->param.invert), comp_id, "%s.negative-logic",inst->name); if (retval != 0) { rtapi_print_msg(RTAPI_MSG_ERR, "matrix_kb: Failed to create output pin\n"); hal_exit(comp_id); return -1; } retval = hal_param_u32_newf(HAL_RW, &(inst->param.rollover), comp_id, "%s.key_rollover",inst->name); if (retval != 0) { rtapi_print_msg(RTAPI_MSG_ERR, "matrix_kb: Failed to create rollover param\n"); hal_exit(comp_id); return -1; } } else // scanning by 7i73 or similar { retval = hal_pin_u32_newf(HAL_IN, &(inst->hal.keycode), comp_id, "%s.keycode",inst->name); if (retval != 0) { rtapi_print_msg(RTAPI_MSG_ERR, "matrix_kb: Failed to create input pin\n"); hal_exit(comp_id); return -1; } } retval = hal_export_funct(inst->name, loop, inst, 1, 0, comp_id); //needs fp? if (retval < 0) { rtapi_print_msg(RTAPI_MSG_ERR, "matrix_kb: ERROR: function export failed\n"); return -1; } } hal_ready(comp_id); return 0; }
int lcec_el7041_1000_init(int comp_id, struct lcec_slave *s, ec_pdo_entry_reg_t *r) { lcec_master_t *m = s->master; lcec_el7041_1000_data_t *hd; int err; int i; // initialize callbacks s->proc_read = lcec_el7041_1000_read; s->proc_write = lcec_el7041_1000_write; // alloc hal memory if ((hd = hal_malloc(sizeof(lcec_el7041_1000_data_t))) == NULL) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "hal_malloc() for slave %s.%s failed\n", m->name, s->name); return -EIO; } memset(hd, 0, sizeof(lcec_el7041_1000_data_t)); s->hal_data = hd; for (i=0; i<LCEC_CONF_ATTR_MAX;i++){ if (strcmp(s->attrs[i].attr,"maxCurrent")==0){ if (ecrt_slave_config_sdo16(s->config, 0x8010, 0x01, s->attrs[i].val) != 0) { rtapi_print_msg (RTAPI_MSG_ERR, LCEC_MSG_PFX "fail to configure slave %s.%s sdo 8010 01\n", m->name, s->name); } } else if (strcmp(s->attrs[i].attr,"nomVoltage")==0){ if (ecrt_slave_config_sdo16(s->config, 0x8010, 0x03, s->attrs[i].val) != 0) { rtapi_print_msg (RTAPI_MSG_ERR, LCEC_MSG_PFX "fail to configure slave %s.%s sdo 8010 03\n", m->name, s->name); } } } // initialize sync info s->sync_info = lcec_el7041_1000_syncs; // initialize global data hd->last_operational = 0; // initialize PDO entries #define pdo(...) LCEC_PDO_INIT(r, s->index, s->vid, s->pid, ##__VA_ARGS__) pdo(0x7000, 0x01, &hd->ena_latch_c_pdo_os, &hd->ena_latch_c_pdo_bp); pdo(0x7000, 0x02, &hd->ena_latch_ext_pos_pdo_os, &hd->ena_latch_ext_pos_pdo_bp); pdo(0x7000, 0x03, &hd->set_count_pdo_os, &hd->set_count_pdo_bp); pdo(0x7000, 0x04, &hd->ena_latch_ext_neg_pdo_os, &hd->ena_latch_ext_neg_pdo_bp); pdo(0x7000, 0x11, &hd->set_count_val_pdo_os, NULL); pdo(0x7010, 0x01, &hd->dcm_ena_pdo_os, &hd->dcm_ena_pdo_bp); pdo(0x7010, 0x02, &hd->dcm_reset_pdo_os, &hd->dcm_reset_pdo_bp); pdo(0x7010, 0x03, &hd->dcm_reduce_torque_pdo_os, &hd->dcm_reduce_torque_pdo_bp); pdo(0x7010, 0x21, &hd->dcm_velo_pdo_os, NULL); pdo(0x6000, 0x01, &hd->latch_c_valid_pdo_os, &hd->latch_c_valid_pdo_bp); pdo(0x6000, 0x02, &hd->latch_ext_valid_pdo_os, &hd->latch_ext_valid_pdo_bp); pdo(0x6000, 0x03, &hd->set_count_done_pdo_os, &hd->set_count_done_pdo_bp); pdo(0x6000, 0x04, &hd->count_underflow_pdo_os, &hd->count_overflow_pdo_bp); pdo(0x6000, 0x05, &hd->count_overflow_pdo_os, &hd->count_underflow_pdo_bp); pdo(0x6000, 0x08, &hd->expol_stall_pdo_os, &hd->expol_stall_pdo_bp); pdo(0x6000, 0x09, &hd->ina_pdo_os, &hd->ina_pdo_bp); pdo(0x6000, 0x0a, &hd->inb_pdo_os, &hd->inb_pdo_bp); pdo(0x6000, 0x0b, &hd->inc_pdo_os, &hd->inc_pdo_bp); pdo(0x6000, 0x0d, &hd->inext_pdo_os, &hd->inext_pdo_bp); pdo(0x6000, 0x0e, &hd->sync_err_pdo_os, &hd->sync_err_pdo_bp); pdo(0x6000, 0x10, &hd->tx_toggle_pdo_os, &hd->tx_toggle_pdo_bp); pdo(0x6000, 0x11, &hd->count_pdo_os, NULL); pdo(0x6000, 0x12, &hd->latch_pdo_os, NULL); pdo(0x6010, 0x01, &hd->dcm_ready_to_enable_pdo_os, &hd->dcm_ready_to_enable_pdo_bp); pdo(0x6010, 0x02, &hd->dcm_ready_pdo_os, &hd->dcm_ready_pdo_bp); pdo(0x6010, 0x03, &hd->dcm_warning_pdo_os, &hd->dcm_warning_pdo_bp); pdo(0x6010, 0x04, &hd->dcm_error_pdo_os, &hd->dcm_error_pdo_bp); pdo(0x6010, 0x05, &hd->dcm_move_pos_pdo_os, &hd->dcm_move_pos_pdo_bp); pdo(0x6010, 0x06, &hd->dcm_move_neg_pdo_os, &hd->dcm_move_neg_pdo_bp); pdo(0x6010, 0x07, &hd->dcm_torque_reduced_pdo_os, &hd->dcm_torque_reduced_pdo_bp); pdo(0x6010, 0x0c, &hd->dcm_din1_pdo_os, &hd->dcm_din1_pdo_bp); pdo(0x6010, 0x0d, &hd->dcm_din2_pdo_os, &hd->dcm_din2_pdo_bp); pdo(0x6010, 0x0e, &hd->dcm_sync_err_pdo_os, &hd->dcm_sync_err_pdo_bp); pdo(0x6010, 0x10, &hd->dcm_tx_toggle_pdo_os, &hd->dcm_tx_toggle_pdo_bp); #undef pdo #define msg(H, format) \ rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting "#H" %s.%s.%s." format " failed\n", \ LCEC_MODULE_NAME, m->name, s->name) #define hal_pin_bit(IO, V, P) hal_pin_bit_newf(IO, V, comp_id, "%s.%s.%s." P, LCEC_MODULE_NAME, m->name, s->name) #define hal_pin_s32(IO, V, P) hal_pin_s32_newf(IO, V, comp_id, "%s.%s.%s." P, LCEC_MODULE_NAME, m->name, s->name) #define hal_pin_u32(IO, V, P) hal_pin_u32_newf(IO, V, comp_id, "%s.%s.%s." P, LCEC_MODULE_NAME, m->name, s->name) #define hal_pin_flt(IO, V, P) hal_pin_float_newf(IO, V, comp_id, "%s.%s.%s." P, LCEC_MODULE_NAME, m->name, s->name) #define e(H, T, IO, V, N) if ((err = hal_##H##_##T(IO, V, N)) != 0) { msg(#H, N); return err; } // export encoder pins e(pin, bit, HAL_IN, &(hd->reset), "enc-reset"); e(pin, bit, HAL_OUT, &(hd->ina), "enc-ina"); e(pin, bit, HAL_OUT, &(hd->inb), "enc-inb"); e(pin, bit, HAL_OUT, &(hd->inc), "enc-inc"); e(pin, bit, HAL_OUT, &(hd->inext), "enc-inext"); e(pin, bit, HAL_OUT, &(hd->sync_err), "enc-sync-error"); e(pin, bit, HAL_OUT, &(hd->expol_stall), "enc-expol-stall"); e(pin, bit, HAL_OUT, &(hd->tx_toggle), "enc-tx-toggle"); e(pin, bit, HAL_OUT, &(hd->count_overflow), "enc-count-overflow"); e(pin, bit, HAL_OUT, &(hd->count_underflow), "enc-count-underflow"); e(pin, bit, HAL_OUT, &(hd->latch_c_valid), "enc-latch-c-valid"); e(pin, bit, HAL_OUT, &(hd->latch_ext_valid), "enc-latch-ext-valid"); e(pin, bit, HAL_IO, &(hd->set_raw_count), "enc-set-raw-count"); e(pin, bit, HAL_IO, &(hd->ena_latch_c), "enc-latch-c-enable"); e(pin, bit, HAL_IO, &(hd->ena_latch_ext_pos), "enc-index-ext-pos-enable"); e(pin, bit, HAL_IO, &(hd->ena_latch_ext_neg), "enc-index-ext-neg-enable"); e(pin, s32, HAL_IN, &(hd->set_raw_count_val), "enc-set-raw-count-val"); e(pin, s32, HAL_OUT, &(hd->raw_count), "enc-raw-count"); e(pin, s32, HAL_OUT, &(hd->count), "enc-count"); e(pin, s32, HAL_OUT, &(hd->raw_latch), "enc-raw-latch"); e(pin, flt, HAL_OUT, &(hd->pos), "enc-pos"); e(pin, flt, HAL_IO, &(hd->pos_scale), "enc-pos-scale"); // export servo pins e(pin, flt, HAL_IO, &(hd->dcm_scale), "srv-scale"); e(pin, flt, HAL_IO, &(hd->dcm_offset), "srv-offset"); e(pin, flt, HAL_IO, &(hd->dcm_min_dc), "srv-min-dc"); e(pin, flt, HAL_IO, &(hd->dcm_max_dc), "srv-max-dc"); e(pin, flt, HAL_OUT, &(hd->dcm_curr_dc), "srv-curr-dc"); e(pin, bit, HAL_IN, &(hd->dcm_enable), "srv-enable"); e(pin, bit, HAL_IN, &(hd->dcm_absmode), "srv-absmode"); e(pin, flt, HAL_IN, &(hd->dcm_value), "srv-cmd"); e(pin, s32, HAL_OUT, &(hd->dcm_raw_val), "srv-raw-cmd"); e(pin, bit, HAL_IN, &(hd->dcm_reset), "srv-reset"); e(pin, bit, HAL_IN, &(hd->dcm_reduce_torque), "srv-reduce-torque"); e(pin, bit, HAL_OUT, &(hd->dcm_ready_to_enable), "srv-ready-to-enable"); e(pin, bit, HAL_OUT, &(hd->dcm_ready), "srv-ready"); e(pin, bit, HAL_OUT, &(hd->dcm_warning), "srv-warning"); e(pin, bit, HAL_OUT, &(hd->dcm_error), "srv-error"); e(pin, bit, HAL_OUT, &(hd->dcm_move_pos), "srv-move-pos"); e(pin, bit, HAL_OUT, &(hd->dcm_move_neg), "srv-move-neg"); e(pin, bit, HAL_OUT, &(hd->dcm_torque_reduced), "srv-torque-reduced"); e(pin, bit, HAL_OUT, &(hd->dcm_din1), "srv-din1"); e(pin, bit, HAL_OUT, &(hd->dcm_din2), "srv-din2"); e(pin, bit, HAL_OUT, &(hd->dcm_sync_err), "srv-sync-error"); e(pin, bit, HAL_OUT, &(hd->dcm_tx_toggle), "srv-tx-toggle"); e(pin, bit, HAL_OUT, &(hd->fault), "srv-fault"); e(pin, bit, HAL_IN, &(hd->fault_reset), "srv-fault-reset"); #undef msg #undef hal_pin_bit #undef hal_pin_s32 #undef hal_pin_u32 #undef hal_pin_flt #undef e // initialize pins *(hd->reset) = 0; *(hd->ina) = 0; *(hd->inb) = 0; *(hd->inc) = 0; *(hd->inext) = 0; *(hd->sync_err) = 0; *(hd->expol_stall) = 0; *(hd->tx_toggle) = 0; *(hd->count_overflow) = 0; *(hd->count_underflow) = 0; *(hd->latch_c_valid) = 0; *(hd->latch_ext_valid) = 0; *(hd->ena_latch_c) = 0; *(hd->ena_latch_ext_pos) = 0; *(hd->ena_latch_ext_neg) = 0; *(hd->set_raw_count) = 0; *(hd->set_raw_count_val) = 0; *(hd->raw_count) = 0; *(hd->raw_latch) = 0; *(hd->count) = 0; *(hd->pos) = 0; *(hd->pos_scale) = 1.0; *(hd->dcm_scale) = 1.0; *(hd->dcm_offset) = 0.0; *(hd->dcm_min_dc) = -1.0; *(hd->dcm_max_dc) = 1.0; *(hd->dcm_curr_dc) = 0.0; *(hd->dcm_enable) = 0; *(hd->dcm_absmode) = 0; *(hd->dcm_value) = 0.0; *(hd->dcm_raw_val) = 0; *(hd->dcm_reset) = 0; *(hd->dcm_reduce_torque) = 0; *(hd->dcm_ready_to_enable) = 0; *(hd->dcm_ready) = 0; *(hd->dcm_warning) = 0; *(hd->dcm_error) = 0; *(hd->dcm_move_pos) = 0; *(hd->dcm_move_neg) = 0; *(hd->dcm_torque_reduced) = 0; *(hd->dcm_din1) = 0; *(hd->dcm_din2) = 0; *(hd->dcm_sync_err) = 0; *(hd->dcm_tx_toggle) = 0; *(hd->fault) = 0; *(hd->fault_reset) = 0; // initialize variables hd->enc_do_init = 1; hd->enc_last_count = 0; hd->enc_old_scale = *(hd->pos_scale) + 1.0; hd->enc_scale_recip = 1.0; hd->dcm_old_scale = *(hd->dcm_scale) + 1.0; hd->dcm_scale_recip = 1.0; hd->internal_fault = 0; hd->fault_reset_retry = 0; hd->fault_reset_state = 0; hd->fault_reset_cycle = 0; hd->last_dcm_enable = 0; return 0; }
int hm2_pwmgen_parse_md(hostmot2_t *hm2, int md_index) { hm2_module_descriptor_t *md = &hm2->md[md_index]; int r; // // some standard sanity checks // if (!hm2_md_is_consistent_or_complain(hm2, md_index, 0, 5, 4, 0x0003)) { HM2_ERR("inconsistent Module Descriptor!\n"); return -EINVAL; } if (hm2->pwmgen.num_instances != 0) { HM2_ERR( "found duplicate Module Descriptor for %s (inconsistent firmware), not loading driver\n", hm2_get_general_function_name(md->gtag) ); return -EINVAL; } if (hm2->config.num_pwmgens > md->instances) { HM2_ERR( "config.num_pwmgens=%d, but only %d are available, not loading driver\n", hm2->config.num_pwmgens, md->instances ); return -EINVAL; } if (hm2->config.num_pwmgens == 0) { return 0; } // // looks good, start initializing // if (hm2->config.num_pwmgens == -1) { hm2->pwmgen.num_instances = md->instances; } else { hm2->pwmgen.num_instances = hm2->config.num_pwmgens; } // allocate the module-global HAL shared memory hm2->pwmgen.hal = (hm2_pwmgen_module_global_t *)hal_malloc(sizeof(hm2_pwmgen_module_global_t)); if (hm2->pwmgen.hal == NULL) { HM2_ERR("out of memory!\n"); r = -ENOMEM; goto fail0; } hm2->pwmgen.instance = (hm2_pwmgen_instance_t *)hal_malloc(hm2->pwmgen.num_instances * sizeof(hm2_pwmgen_instance_t)); if (hm2->pwmgen.instance == NULL) { HM2_ERR("out of memory!\n"); r = -ENOMEM; goto fail0; } hm2->pwmgen.clock_frequency = md->clock_freq; hm2->pwmgen.version = md->version; hm2->pwmgen.pwm_value_addr = md->base_address + (0 * md->register_stride); hm2->pwmgen.pwm_mode_addr = md->base_address + (1 * md->register_stride); hm2->pwmgen.pwmgen_master_rate_dds_addr = md->base_address + (2 * md->register_stride); hm2->pwmgen.pdmgen_master_rate_dds_addr = md->base_address + (3 * md->register_stride); hm2->pwmgen.enable_addr = md->base_address + (4 * md->register_stride); r = hm2_register_tram_write_region(hm2, hm2->pwmgen.pwm_value_addr, (hm2->pwmgen.num_instances * sizeof(rtapi_u32)), &hm2->pwmgen.pwm_value_reg); if (r < 0) { HM2_ERR("error registering tram write region for PWM Value register (%d)\n", r); goto fail0; } hm2->pwmgen.pwm_mode_reg = (rtapi_u32 *)rtapi_kmalloc(hm2->pwmgen.num_instances * sizeof(rtapi_u32), RTAPI_GFP_KERNEL); if (hm2->pwmgen.pwm_mode_reg == NULL) { HM2_ERR("out of memory!\n"); r = -ENOMEM; goto fail0; } // export to HAL // FIXME: r hides the r in enclosing function, and it returns the wrong thing { int i; int r; char name[HAL_NAME_LEN + 1]; // these hal parameters affect all pwmgen instances r = hal_param_u32_newf( HAL_RW, &(hm2->pwmgen.hal->param.pwm_frequency), hm2->llio->comp_id, "%s.pwmgen.pwm_frequency", hm2->llio->name ); if (r < 0) { HM2_ERR("error adding pwmgen.pwm_frequency param, aborting\n"); goto fail1; } hm2->pwmgen.hal->param.pwm_frequency = 20000; hm2->pwmgen.written_pwm_frequency = 0; r = hal_param_u32_newf( HAL_RW, &(hm2->pwmgen.hal->param.pdm_frequency), hm2->llio->comp_id, "%s.pwmgen.pdm_frequency", hm2->llio->name ); if (r < 0) { HM2_ERR("error adding pwmgen.pdm_frequency param, aborting\n"); goto fail1; } hm2->pwmgen.hal->param.pdm_frequency = 20000; hm2->pwmgen.written_pdm_frequency = 0; for (i = 0; i < hm2->pwmgen.num_instances; i ++) { // pins rtapi_snprintf(name, sizeof(name), "%s.pwmgen.%02d.value", hm2->llio->name, i); r = hal_pin_float_new(name, HAL_IN, &(hm2->pwmgen.instance[i].hal.pin.value), hm2->llio->comp_id); if (r < 0) { HM2_ERR("error adding pin '%s', aborting\n", name); goto fail1; } rtapi_snprintf(name, sizeof(name), "%s.pwmgen.%02d.enable", hm2->llio->name, i); r = hal_pin_bit_new(name, HAL_IN, &(hm2->pwmgen.instance[i].hal.pin.enable), hm2->llio->comp_id); if (r < 0) { HM2_ERR("error adding pin '%s', aborting\n", name); goto fail1; } // parameters rtapi_snprintf(name, sizeof(name), "%s.pwmgen.%02d.scale", hm2->llio->name, i); r = hal_param_float_new(name, HAL_RW, &(hm2->pwmgen.instance[i].hal.param.scale), hm2->llio->comp_id); if (r < 0) { HM2_ERR("error adding param '%s', aborting\n", name); goto fail1; } r = hal_param_s32_newf( HAL_RW, &(hm2->pwmgen.instance[i].hal.param.output_type), hm2->llio->comp_id, "%s.pwmgen.%02d.output-type", hm2->llio->name, i ); if (r < 0) { HM2_ERR("error adding param, aborting\n"); goto fail1; } // init hal objects *(hm2->pwmgen.instance[i].hal.pin.enable) = 0; *(hm2->pwmgen.instance[i].hal.pin.value) = 0.0; hm2->pwmgen.instance[i].hal.param.scale = 1.0; hm2->pwmgen.instance[i].hal.param.output_type = HM2_PWMGEN_OUTPUT_TYPE_PWM; hm2->pwmgen.instance[i].written_output_type = -666; // force an update at the start hm2->pwmgen.instance[i].written_enable = -666; // force an update at the start } } return hm2->pwmgen.num_instances; fail1: rtapi_kfree(hm2->pwmgen.pwm_mode_reg); fail0: hm2->pwmgen.num_instances = 0; return r; }
int rtapi_app_main(void) { int n, retval; retval = setup_user_step_type(); if(retval < 0) { return retval; } for (n = 0; n < MAX_CHAN && step_type[n] != -1 ; n++) { if ((step_type[n] > MAX_STEP_TYPE) || (step_type[n] < 0)) { rtapi_print_msg(RTAPI_MSG_ERR, "STEPGEN: ERROR: bad stepping type '%i', axis %i\n", step_type[n], n); return -1; } if(parse_ctrl_type(ctrl_type[n]) == INVALID) { rtapi_print_msg(RTAPI_MSG_ERR, "STEPGEN: ERROR: bad control type '%s' for axis %i (must be 'p' or 'v')\n", ctrl_type[n], n); return -1; } num_chan++; } if (num_chan == 0) { rtapi_print_msg(RTAPI_MSG_ERR, "STEPGEN: ERROR: no channels configured\n"); return -1; } /* periodns will be set to the proper value when 'make_pulses()' runs for the first time. We load a default value here to avoid glitches at startup, but all these 'constants' are recomputed inside 'update_freq()' using the real period. */ old_periodns = periodns = 50000; old_dtns = 1000000; /* precompute some constants */ periodfp = periodns * 0.000000001; freqscale = (1L << PICKOFF) * periodfp; accelscale = freqscale * periodfp; dt = old_dtns * 0.000000001; recip_dt = 1.0 / dt; /* have good config info, connect to the HAL */ comp_id = hal_init("stepgen"); if (comp_id < 0) { rtapi_print_msg(RTAPI_MSG_ERR, "STEPGEN: ERROR: hal_init() failed\n"); return -1; } /* allocate shared memory for counter data */ stepgen_array = hal_malloc(num_chan * sizeof(stepgen_t)); if (stepgen_array == 0) { rtapi_print_msg(RTAPI_MSG_ERR, "STEPGEN: ERROR: hal_malloc() failed\n"); hal_exit(comp_id); return -1; } /* export all the variables for each pulse generator */ for (n = 0; n < num_chan; n++) { /* export all vars */ retval = export_stepgen(n, &(stepgen_array[n]), step_type[n], (parse_ctrl_type(ctrl_type[n]) == POSITION)); if (retval != 0) { rtapi_print_msg(RTAPI_MSG_ERR, "STEPGEN: ERROR: stepgen %d var export failed\n", n); hal_exit(comp_id); return -1; } } /* export functions */ retval = hal_export_funct("stepgen.make-pulses", make_pulses, stepgen_array, 0, 0, comp_id); if (retval != 0) { rtapi_print_msg(RTAPI_MSG_ERR, "STEPGEN: ERROR: makepulses funct export failed\n"); hal_exit(comp_id); return -1; } retval = hal_export_funct("stepgen.update-freq", update_freq, stepgen_array, 1, 0, comp_id); if (retval != 0) { rtapi_print_msg(RTAPI_MSG_ERR, "STEPGEN: ERROR: freq update funct export failed\n"); hal_exit(comp_id); return -1; } retval = hal_export_funct("stepgen.capture-position", update_pos, stepgen_array, 1, 0, comp_id); if (retval != 0) { rtapi_print_msg(RTAPI_MSG_ERR, "STEPGEN: ERROR: pos update funct export failed\n"); hal_exit(comp_id); return -1; } rtapi_print_msg(RTAPI_MSG_INFO, "STEPGEN: installed %d step pulse generators\n", num_chan); hal_ready(comp_id); return 0; }
int rtapi_app_main(void) { int n, retval; /* check that there's at least one valid input requested */ if (num_inputs<1) { rtapi_print_msg(RTAPI_MSG_ERR, "WATCHDOG: ERROR: must specify at least one input\n"); return -1; } /* but not too many */ if (num_inputs> MAX_INPUTS) { rtapi_print_msg(RTAPI_MSG_ERR, "WATCHDOG: ERROR: too many inputs requested (%d > %d)\n", num_inputs, MAX_INPUTS); return -1; } /* have good config info, connect to the HAL */ comp_id = hal_init("watchdog"); if (comp_id < 0) { rtapi_print_msg(RTAPI_MSG_ERR, "WATCHDOG: ERROR: hal_init() failed (Return code %d)\n", comp_id); return -1; } /* allocate shared memory for watchdog global and pin info */ data = hal_malloc(sizeof(watchdog_data_t)); if (data == 0) { rtapi_print_msg(RTAPI_MSG_ERR, "WATCHDOG: ERROR: hal_malloc() for common data failed\n"); hal_exit(comp_id); goto err; } inputs = hal_malloc(num_inputs * sizeof(watchdog_input_t)); if (inputs == 0) { rtapi_print_msg(RTAPI_MSG_ERR, "WATCHDOG: ERROR: hal_malloc() for input pins failed\n"); hal_exit(comp_id); goto err; } /* export pins/params for all inputs */ for (n = 0; n < num_inputs; n++) { retval=hal_pin_bit_newf(HAL_IN, &(inputs[n].input), comp_id, "watchdog.input-%d", n); if (retval != 0) { rtapi_print_msg(RTAPI_MSG_ERR, "WATCHDOG: ERROR: couldn't create input pin watchdog.input-%d\n", n); goto err; } retval=hal_pin_float_newf(HAL_IN, &(inputs[n].timeout), comp_id, "watchdog.timeout-%d", n); if (retval != 0) { rtapi_print_msg(RTAPI_MSG_ERR, "WATCHDOG: ERROR: couldn't create input parameter watchdog.timeout-%d\n", n); goto err; } (*inputs[n].timeout)=0; inputs[n].oldtimeout=-1; inputs[n].c_secs = inputs[n].t_secs = 0; inputs[n].c_nsecs = inputs[n].t_nsecs = 0; inputs[n].last = *(inputs[n].input); } /* export "global" pins */ retval=hal_pin_bit_newf(HAL_OUT, &(data->output), comp_id, "watchdog.ok-out"); if (retval != 0) { rtapi_print_msg(RTAPI_MSG_ERR, "WATCHDOG: ERROR: couldn't create output pin watchdog.ok-out\n"); goto err; } retval=hal_pin_bit_newf(HAL_IN, &(data->enable), comp_id, "watchdog.enable-in"); if (retval != 0) { rtapi_print_msg(RTAPI_MSG_ERR, "WATCHDOG: ERROR: couldn't create input pin watchdog.enable-in\n"); goto err; } /* export functions */ retval = hal_export_funct("watchdog.process", process, inputs, 0, 0, comp_id); if (retval != 0) { rtapi_print_msg(RTAPI_MSG_ERR, "WATCHDOG: ERROR: process funct export failed\n"); goto err; } retval = hal_export_funct("watchdog.set-timeouts", set_timeouts, inputs, 1, 0, comp_id); if (retval != 0) { rtapi_print_msg(RTAPI_MSG_ERR, "WATCHDOG: ERROR: set_timeouts funct export failed\n"); goto err; } rtapi_print_msg(RTAPI_MSG_INFO, "WATCHDOG: installed watchdog with %d inputs\n", num_inputs); hal_ready(comp_id); return 0; err: hal_exit(comp_id); return -1; }
int hm2_watchdog_parse_md(hostmot2_t *hm2, int md_index) { hm2_module_descriptor_t *md = &hm2->md[md_index]; int r; // // some standard sanity checks // if (!hm2_md_is_consistent_or_complain(hm2, md_index, 0, 3, 4, 0)) { HM2_ERR("inconsistent Module Descriptor!\n"); return -EINVAL; } if (hm2->watchdog.num_instances != 0) { HM2_ERR( "found duplicate Module Descriptor for %s (inconsistent firmware), not loading driver\n", hm2_get_general_function_name(md->gtag) ); return -EINVAL; } // // special sanity checks for watchdog // if (md->instances != 1) { HM2_PRINT("MD declares %d watchdogs! only using the first one...\n", md->instances); } // // looks good, start initializing // hm2->watchdog.num_instances = 1; hm2->watchdog.instance = (hm2_watchdog_instance_t *)hal_malloc(hm2->watchdog.num_instances * sizeof(hm2_watchdog_instance_t)); if (hm2->watchdog.instance == NULL) { HM2_ERR("out of memory!\n"); r = -ENOMEM; goto fail0; } hm2->watchdog.clock_frequency = md->clock_freq; hm2->watchdog.version = md->version; hm2->watchdog.timer_addr = md->base_address + (0 * md->register_stride); hm2->watchdog.status_addr = md->base_address + (1 * md->register_stride); hm2->watchdog.reset_addr = md->base_address + (2 * md->register_stride); r = hm2_register_tram_read_region(hm2, hm2->watchdog.status_addr, (hm2->watchdog.num_instances * sizeof(u32)), &hm2->watchdog.status_reg); if (r < 0) { HM2_ERR("error registering tram read region for watchdog (%d)\n", r); goto fail0; } r = hm2_register_tram_write_region(hm2, hm2->watchdog.reset_addr, sizeof(u32), &hm2->watchdog.reset_reg); if (r < 0) { HM2_ERR("error registering tram write region for watchdog (%d)!\n", r); goto fail0; } // // allocate memory for register buffers // hm2->watchdog.timer_reg = (u32 *)kmalloc(hm2->watchdog.num_instances * sizeof(u32), GFP_KERNEL); if (hm2->watchdog.timer_reg == NULL) { HM2_ERR("out of memory!\n"); r = -ENOMEM; goto fail0; } // // export to HAL // // pins r = hal_pin_bit_newf( HAL_IO, &(hm2->watchdog.instance[0].hal.pin.has_bit), hm2->llio->comp_id, "%s.watchdog.has_bit", hm2->llio->name ); if (r < 0) { HM2_ERR("error adding pin, aborting\n"); r = -EINVAL; goto fail1; } // params r = hal_param_u32_newf( HAL_RW, &(hm2->watchdog.instance[0].hal.param.timeout_ns), hm2->llio->comp_id, "%s.watchdog.timeout_ns", hm2->llio->name ); if (r < 0) { HM2_ERR("error adding param, aborting\n"); r = -EINVAL; goto fail1; } // the function { hal_export_xfunct_args_t xfunct_args = { .type = FS_XTHREADFUNC, .funct.x = hm2_pet_watchdog, .arg = hm2, .uses_fp = 0, .reentrant = 0, .owner_id = hm2->llio->comp_id }; if ((r = hal_export_xfunctf(&xfunct_args, "%s.pet_watchdog", hm2->llio->name)) != 0) { HM2_ERR("hal_export pet_watchdog failed - %d\n", r); r = -EINVAL; goto fail1; } } // // initialize the watchdog // *hm2->watchdog.instance[0].hal.pin.has_bit = 0; hm2->watchdog.instance[0].hal.param.timeout_ns = 5 * 1000 * 1000; // default timeout is 5 milliseconds hm2->watchdog.instance[0].enable = 0; // the first pet_watchdog will turn it on return hm2->watchdog.num_instances; fail1: kfree(hm2->watchdog.timer_reg); fail0: hm2->watchdog.num_instances = 0; return r; } void hm2_watchdog_print_module(hostmot2_t *hm2) { int i; HM2_PRINT("Watchdog: %d\n", hm2->watchdog.num_instances); if (hm2->watchdog.num_instances <= 0) return; HM2_PRINT(" clock_frequency: %d Hz (%s MHz)\n", hm2->watchdog.clock_frequency, hm2_hz_to_mhz(hm2->watchdog.clock_frequency)); HM2_PRINT(" version: %d\n", hm2->watchdog.version); HM2_PRINT(" timer_addr: 0x%04X\n", hm2->watchdog.timer_addr); HM2_PRINT(" status_addr: 0x%04X\n", hm2->watchdog.status_addr); HM2_PRINT(" reset_addr: 0x%04X\n", hm2->watchdog.reset_addr); for (i = 0; i < hm2->watchdog.num_instances; i ++) { HM2_PRINT(" instance %d:\n", i); HM2_PRINT(" param timeout_ns = %u\n", hm2->watchdog.instance[i].hal.param.timeout_ns); HM2_PRINT(" pin has_bit = %d\n", (*hm2->watchdog.instance[i].hal.pin.has_bit)); HM2_PRINT(" reg timer = 0x%08X\n", hm2->watchdog.timer_reg[i]); } }
int rtapi_app_main(void) { int retval; /* test for number of channels */ if ((num_chan <= 0) || (num_chan > MAX_CHAN)) { rtapi_print_msg(RTAPI_MSG_ERR, "VTI: ERROR: invalid num_chan: %d\n", num_chan); return -1; } /* test for config string */ if ((dio == 0) || (dio[0] == '\0')) { rtapi_print_msg(RTAPI_MSG_ERR, "VTI: ERROR: no dio config string\n"); return -1; } /* have good config info, connect to the HAL */ comp_id = hal_init("hal_vti"); if (comp_id < 0) { rtapi_print_msg(RTAPI_MSG_ERR, "VTI: ERROR: hal_init() failed\n"); return -1; } /* allocate shared memory for vti data */ vti_driver = hal_malloc(num_chan * sizeof(vti_struct)); if (vti_driver == 0) { rtapi_print_msg(RTAPI_MSG_ERR, "VTI: ERROR: hal_malloc() failed\n"); hal_exit(comp_id); return -1; } /* takes care of all initialisations, also autodetection and model if necessary */ if ((retval=vti_init_card()) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, "VTI: ERROR: vti_init_card() failed\n"); hal_exit(comp_id); return retval; } diocount = vti_parse_dio(); if (diocount == -1) { rtapi_print_msg(RTAPI_MSG_ERR, "VTI: ERROR: bad config info for port.\n"); return -1; } export_dio_pins(diocount); vti_dio_init(diocount / 4); /* init counter chip */ if (vti_counter_init(num_chan) == -1) { rtapi_print_msg(RTAPI_MSG_ERR, "VTI: ERROR: bad config info counter.\n"); return -1; } /* init dac chip */ vti_dac_init(num_chan); /* init adc chip */ vti_adc_init(0); // VTI controller has no ADCs /* export functions */ retval = hal_export_funct("vti.capture-position", vti_counter_capture, vti_driver, 1, 0, comp_id); if (retval != 0) { rtapi_print_msg(RTAPI_MSG_ERR, "VTI: ERROR: vti.counter-capture funct export failed\n"); hal_exit(comp_id); return -1; } rtapi_print_msg(RTAPI_MSG_INFO, "VTI: installed %d encoder counters\n", num_chan); retval = hal_export_funct("vti.write-dacs", vti_dacs_write, vti_driver, 1, 0, comp_id); if (retval != 0) { rtapi_print_msg(RTAPI_MSG_ERR, "VTI: ERROR: vti.write-dacs funct export failed\n"); hal_exit(comp_id); return -1; } rtapi_print_msg(RTAPI_MSG_INFO, "VTI: installed %d dacs\n", num_chan); retval = hal_export_funct("vti.read-adcs", vti_adcs_read, vti_driver, 1, 0, comp_id); if (retval != 0) { rtapi_print_msg(RTAPI_MSG_ERR, "VTI: ERROR: vti.read-adcs funct export failed\n"); hal_exit(comp_id); return -1; } rtapi_print_msg(RTAPI_MSG_INFO, "VTI: installed %d adcs\n", 0); retval = hal_export_funct("vti.di-read", vti_di_read, vti_driver, 0, 0, comp_id); if (retval != 0) { rtapi_print_msg(RTAPI_MSG_ERR, "VTI: ERROR: vti.di-read funct export failed\n"); hal_exit(comp_id); return -1; } rtapi_print_msg(RTAPI_MSG_INFO, "VTI: installed %d digital inputs\n", inputpinnum); retval = hal_export_funct("vti.do-write", vti_do_write, vti_driver, 0, 0, comp_id); if (retval != 0) { rtapi_print_msg(RTAPI_MSG_ERR, "VTI: ERROR: vti.do-write funct export failed\n"); hal_exit(comp_id); return -1; } rtapi_print_msg(RTAPI_MSG_INFO, "VTI: installed %d digital outputs\n", outpinnum); hal_ready(comp_id); return 0; }
int rtapi_app_main(void) { hal_pru_generic_t *hpg; int retval; comp_id = hal_init("hal_pru_generic"); if (comp_id < 0) { HPG_ERR("ERROR: hal_init() failed\n"); return -1; } // Allocate HAL shared memory for state data hpg = hal_malloc(sizeof(hal_pru_generic_t)); if (hpg == 0) { HPG_ERR("ERROR: hal_malloc() failed\n"); hal_exit(comp_id); return -1; } // Clear memory memset(hpg, 0, sizeof(hal_pru_generic_t)); // Initialize PRU and map PRU data memory if ((retval = pru_init(pru, prucode, disabled, hpg))) { HPG_ERR("ERROR: failed to initialize PRU\n"); hal_exit(comp_id); return -1; } // Setup global state hpg->config.num_pwmgens = num_pwmgens; hpg->config.num_stepgens = num_stepgens; hpg->config.num_encoders = num_encoders; hpg->config.comp_id = comp_id; hpg->config.pru_period = pru_period; hpg->config.name = modname; hpg->config.halname = halname; rtapi_print("num_pwmgens : %d\n",num_pwmgens); rtapi_print("num_stepgens: %d\n",num_stepgens); rtapi_print("num_encoders: %d\n",num_encoders); rtapi_print("Init pwm\n"); // Initialize various functions and generate PRU data ram contents if ((retval = hpg_pwmgen_init(hpg))) { HPG_ERR("ERROR: pwmgen init failed: %d\n", retval); hal_exit(comp_id); return -1; } rtapi_print("Init stepgen\n"); if ((retval = hpg_stepgen_init(hpg))) { HPG_ERR("ERROR: stepgen init failed: %d\n", retval); hal_exit(comp_id); return -1; } rtapi_print("Init encoder\n"); if ((retval = hpg_encoder_init(hpg))) { HPG_ERR("ERROR: encoder init failed: %d\n", retval); hal_exit(comp_id); return -1; } if ((retval = hpg_wait_init(hpg))) { HPG_ERR("ERROR: global task init failed: %d\n", retval); hal_exit(comp_id); return -1; } if ((retval = export_pru(hpg))) { HPG_ERR("ERROR: var export failed: %d\n", retval); hal_exit(comp_id); return -1; } hpg_stepgen_force_write(hpg); hpg_pwmgen_force_write(hpg); hpg_encoder_force_write(hpg); hpg_wait_force_write(hpg); if ((retval = setup_pru(pru, prucode, disabled, hpg))) { HPG_ERR("ERROR: failed to initialize PRU\n"); hal_exit(comp_id); return -1; } HPG_INFO("installed\n"); hal_ready(comp_id); return 0; }
int rtapi_app_main(void) { char name[HAL_NAME_LEN + 1]; int n, retval; char *data, *token; num_ports = 1; n = 0; // port number... only one for now // init driver comp_id = hal_init(modname); if(comp_id < 0) { rtapi_print_msg(RTAPI_MSG_ERR, "%s: ERROR: hal_init() failed\n", modname); return -1; } // allocate port memory port_data = hal_malloc(num_ports * sizeof(port_data_t)); if(port_data == 0) { rtapi_print_msg(RTAPI_MSG_ERR, "%s: ERROR: hal_malloc() failed\n", modname); hal_exit(comp_id); return -1; } // map control module memory configure_control_module(); // configure userleds if(user_leds != NULL) { data = user_leds; while((token = strtok(data, ",")) != NULL) { int led = strtol(token, NULL, 10); data = NULL; if(user_led_gpio_pins[led].claimed != 0) { rtapi_print_msg(RTAPI_MSG_ERR, "%s: ERROR: userled%d is not available as a GPIO.\n", modname, led); hal_exit(comp_id); return -1; } // Add HAL pin retval = hal_pin_bit_newf(HAL_IN, &(port_data->led_pins[led]), comp_id, "bb_gpio.userled%d", led); if(retval < 0) { rtapi_print_msg(RTAPI_MSG_ERR, "%s: ERROR: userled %d could not export pin, err: %d\n", modname, led, retval); hal_exit(comp_id); return -1; } // Add HAL pin retval = hal_pin_bit_newf(HAL_IN, &(port_data->led_inv[led]), comp_id, "bb_gpio.userled%d.invert", led); if(retval < 0) { rtapi_print_msg(RTAPI_MSG_ERR, "%s: ERROR: userled %d could not export pin, err: %d\n", modname, led, retval); hal_exit(comp_id); return -1; } // Initialize HAL pin *(port_data->led_inv[led]) = 0; int gpio_num = user_led_gpio_pins[led].port_num; // configure gpio port if necessary if(gpio_ports[gpio_num] == NULL) { configure_gpio_port(gpio_num); } user_led_gpio_pins[led].port = gpio_ports[gpio_num]; configure_pin(&user_led_gpio_pins[led], 'O'); } } // configure input pins if(input_pins != NULL) { data = input_pins; while((token = strtok(data, ",")) != NULL) { int pin = strtol(token, NULL, 10); int header; bb_gpio_pin *bbpin; // Fixup old pin numbering scheme: // P8/P9 was 1xx/2xx, now 8xx/9xx if (pin < 300) pin += 700; if(pin < 801 || pin > 946 || (pin > 846 && pin < 901)) { rtapi_print_msg(RTAPI_MSG_ERR, "%s: ERROR: invalid pin number '%d'. Valid pins are 801-846 for P8 pins, 901-946 for P9 pins.\n", modname, pin); hal_exit(comp_id); return -1; } if(pin < 900) { pin -= 800; bbpin = &p8_pins[pin]; header = 8; } else { pin -= 900; bbpin = &p9_pins[pin]; header = 9; } if(bbpin->claimed != 0) { rtapi_print_msg(RTAPI_MSG_ERR, "%s: ERROR: pin p%d.%02d is not available as a GPIO.\n", modname, header, pin); hal_exit(comp_id); return -1; } data = NULL; // after the first call, subsequent calls to strtok need to be on NULL // Add HAL pin retval = hal_pin_bit_newf(HAL_OUT, &(port_data->input_pins[pin + (header - 8)*PINS_PER_HEADER]), comp_id, "bb_gpio.p%d.in-%02d", header, pin); if(retval < 0) { rtapi_print_msg(RTAPI_MSG_ERR, "%s: ERROR: pin p%d.%02d could not export pin, err: %d\n", modname, header, pin, retval); hal_exit(comp_id); return -1; } // Add HAL pin retval = hal_pin_bit_newf(HAL_IN, &(port_data->input_inv[pin + (header - 8)*PINS_PER_HEADER]), comp_id, "bb_gpio.p%d.in-%02d.invert", header, pin); if(retval < 0) { rtapi_print_msg(RTAPI_MSG_ERR, "%s: ERROR: pin p%d.%02d could not export pin, err: %d\n", modname, header, pin, retval); hal_exit(comp_id); return -1; } // Initialize HAL pin *(port_data->input_inv[pin + (header - 8)*PINS_PER_HEADER]) = 0; int gpio_num = bbpin->port_num; // configure gpio port if necessary if(gpio_ports[gpio_num] == NULL) { configure_gpio_port(gpio_num); } bbpin->port = gpio_ports[gpio_num]; configure_pin(bbpin, 'U'); rtapi_print("pin %d maps to pin %d-%d, mode %d\n", pin, bbpin->port_num, bbpin->pin_num, bbpin->claimed); } } // configure output pins if(output_pins != NULL) { data = output_pins; while((token = strtok(data, ",")) != NULL) { int pin = strtol(token, NULL, 10); int header; bb_gpio_pin *bbpin; // Fixup old pin numbering scheme: // P8/P9 was 1xx/2xx, now 8xx/9xx if (pin < 300) pin += 700; if(pin < 801 || pin > 946 || (pin > 846 && pin < 901)) { rtapi_print_msg(RTAPI_MSG_ERR, "%s: ERROR: invalid pin number '%d'. Valid pins are 801-846 for P8 pins, 901-946 for P9 pins.\n", modname, pin); hal_exit(comp_id); return -1; } if(pin < 900) { pin -= 800; bbpin = &p8_pins[pin]; header = 8; } else { pin -= 900; bbpin = &p9_pins[pin]; header = 9; } if(bbpin->claimed != 0) { rtapi_print_msg(RTAPI_MSG_ERR, "%s: ERROR: pin p%d.%02d is not available as a GPIO.\n", modname, header, pin); hal_exit(comp_id); return -1; } data = NULL; // after the first call, subsequent calls to strtok need to be on NULL // Add HAL pin retval = hal_pin_bit_newf(HAL_IN, &(port_data->output_pins[pin + (header - 8)*PINS_PER_HEADER]), comp_id, "bb_gpio.p%d.out-%02d", header, pin); if(retval < 0) { rtapi_print_msg(RTAPI_MSG_ERR, "%s: ERROR: pin p%d.%02d could not export pin, err: %d\n", modname, header, pin, retval); hal_exit(comp_id); return -1; } // Add HAL pin retval = hal_pin_bit_newf(HAL_IN, &(port_data->output_inv[pin + (header - 8)*PINS_PER_HEADER]), comp_id, "bb_gpio.p%d.out-%02d.invert", header, pin); if(retval < 0) { rtapi_print_msg(RTAPI_MSG_ERR, "%s: ERROR: pin p%d.%02d could not export pin, err: %d\n", modname, header, pin, retval); hal_exit(comp_id); return -1; } // Initialize HAL pin *(port_data->output_inv[pin + (header - 8)*PINS_PER_HEADER]) = 0; int gpio_num = bbpin->port_num; // configure gpio port if necessary if(gpio_ports[gpio_num] == NULL) { configure_gpio_port(gpio_num); } bbpin->port = gpio_ports[gpio_num]; configure_pin(bbpin, 'O'); } } // export functions rtapi_snprintf(name, sizeof(name), "bb_gpio.write"); retval = hal_export_funct(name, write_port, port_data, 0, 0, comp_id); if(retval < 0) { rtapi_print_msg(RTAPI_MSG_ERR, "%s: ERROR: port %d write funct export failed\n", modname, n); hal_exit(comp_id); return -1; } rtapi_snprintf(name, sizeof(name), "bb_gpio.read"); retval = hal_export_funct(name, read_port, port_data, 0, 0, comp_id); if(retval < 0) { rtapi_print_msg(RTAPI_MSG_ERR, "%s: ERROR: port %d read funct export failed\n", modname, n); hal_exit(comp_id); return -1; } rtapi_print_msg(RTAPI_MSG_INFO, "%s: installed driver\n", modname); hal_ready(comp_id); return 0; }
int rtapi_app_main(void) { int n, retval; rtapi_print_msg(RTAPI_MSG_ERR, "FREQGEN: freqgen is deprecated and will be removed in emc2.4. " "Use stepgen with ctrl_type=v instead\n"); for (n = 0; n < MAX_CHAN && step_type[n] != -1 ; n++) { if ((step_type[n] > MAX_STEP_TYPE) || (step_type[n] < 0)) { rtapi_print_msg(RTAPI_MSG_ERR, "FREQGEN: ERROR: bad stepping type '%i', axis %i\n", step_type[n], n); return -1; } else { num_chan++; } } if (num_chan == 0) { rtapi_print_msg(RTAPI_MSG_ERR, "FREQGEN: ERROR: no channels configured\n"); return -1; } /* periodns will be set to the proper value when 'make_pulses()' runs for the first time. We load a default value here to avoid glitches at startup, but all these 'constants' are recomputed inside 'update_freq()' using the real period. */ periodns = 50000; /* precompute some constants */ periodfp = periodns * 0.000000001; maxf = 1.0 / periodfp; freqscale = ((1L << 30) * 2.0) / maxf; accelscale = freqscale * periodfp; old_periodns = periodns; /* have good config info, connect to the HAL */ comp_id = hal_init("freqgen"); if (comp_id < 0) { rtapi_print_msg(RTAPI_MSG_ERR, "FREQGEN: ERROR: hal_init() failed\n"); return -1; } /* allocate shared memory for counter data */ freqgen_array = hal_malloc(num_chan * sizeof(freqgen_t)); if (freqgen_array == 0) { rtapi_print_msg(RTAPI_MSG_ERR, "FREQGEN: ERROR: hal_malloc() failed\n"); hal_exit(comp_id); return -1; } /* export all the variables for each pulse generator */ for (n = 0; n < num_chan; n++) { /* export all vars */ retval = export_freqgen(n, &(freqgen_array[n]), step_type[n]); if (retval != 0) { rtapi_print_msg(RTAPI_MSG_ERR, "FREQGEN: ERROR: freqgen %d var export failed\n", n); hal_exit(comp_id); return -1; } } /* export functions */ retval = hal_export_funct("freqgen.make-pulses", make_pulses, freqgen_array, 0, 0, comp_id); if (retval != 0) { rtapi_print_msg(RTAPI_MSG_ERR, "FREQGEN: ERROR: makepulses funct export failed\n"); hal_exit(comp_id); return -1; } retval = hal_export_funct("freqgen.update-freq", update_freq, freqgen_array, 1, 0, comp_id); if (retval != 0) { rtapi_print_msg(RTAPI_MSG_ERR, "FREQGEN: ERROR: freq update funct export failed\n"); hal_exit(comp_id); return -1; } retval = hal_export_funct("freqgen.capture-position", update_pos, freqgen_array, 1, 0, comp_id); if (retval != 0) { rtapi_print_msg(RTAPI_MSG_ERR, "FREQGEN: ERROR: pos update funct export failed\n"); hal_exit(comp_id); return -1; } rtapi_print_msg(RTAPI_MSG_INFO, "FREQGEN: installed %d step pulse generators\n", num_chan); hal_ready(comp_id); return 0; }
static int pins_and_params(char *argv[]) { long port_addr[MAX_PORTS]; int data_dir[MAX_PORTS]; int use_control_in[MAX_PORTS]; int force_epp[MAX_PORTS]; int n, retval; /* clear port_addr and data_dir arrays */ for (n = 0; n < MAX_PORTS; n++) { port_addr[n] = 0; data_dir[n] = 0; use_control_in[n] = 0; force_epp[n] = 0; } /* parse config string, results in port_addr[] and data_dir[] arrays */ num_ports = 0; n = 0; while ((num_ports < MAX_PORTS) && (argv[n] != 0)) { port_addr[num_ports] = parse_port_addr(argv[n]); if (port_addr[num_ports] < 0) { rtapi_print_msg(RTAPI_MSG_ERR, "PARPORT: ERROR: bad port address '%s'\n", argv[n]); return -1; } n++; if (argv[n] != 0) { /* is the next token 'in' or 'out' ? */ if ((argv[n][0] == 'i') || (argv[n][0] == 'I')) { /* we aren't picky, anything starting with 'i' means 'in' ;-) */ data_dir[num_ports] = 1; use_control_in[num_ports] = 0; n++; } else if ((argv[n][0] == 'o') || (argv[n][0] == 'O')) { /* anything starting with 'o' means 'out' */ data_dir[num_ports] = 0; use_control_in[num_ports] = 0; n++; } else if ((argv[n][0] == 'e') || (argv[n][0] == 'E')) { /* anything starting with 'e' means 'epp', which is just like 'out' but with EPP mode requested, primarily for the G540 with its charge pump missing-pullup drive issue */ data_dir[num_ports] = 0; use_control_in[num_ports] = 0; force_epp[num_ports] = 1; n++; } else if ((argv[n][0] == 'x') || (argv[n][0] == 'X')) { /* experimental: some parports support a bidirectional * control port. Enable this with pins 2-9 in output mode, * which gives a very nice 8 outs and 9 ins. */ data_dir[num_ports] = 0; use_control_in[num_ports] = 1; n++; } } num_ports++; } /* OK, now we've parsed everything */ if (num_ports == 0) { rtapi_print_msg(RTAPI_MSG_ERR, "PARPORT: ERROR: no ports configured\n"); return -1; } /* have good config info, connect to the HAL */ comp_id = hal_init("hal_parport"); if (comp_id < 0) { rtapi_print_msg(RTAPI_MSG_ERR, "PARPORT: ERROR: hal_init() failed\n"); return -1; } /* allocate shared memory for parport data */ port_data_array = hal_malloc(num_ports * sizeof(parport_t)); if (port_data_array == 0) { rtapi_print_msg(RTAPI_MSG_ERR, "PARPORT: ERROR: hal_malloc() failed\n"); hal_exit(comp_id); return -1; } /* export all the pins and params for each port */ for (n = 0; n < num_ports; n++) { int modes = 0; if(use_control_in[n]) { modes = PARPORT_MODE_TRISTATE; } else if(force_epp[n]) { modes = PARPORT_MODE_EPP; } retval = hal_parport_get(comp_id, &port_data_array[n].portdata, port_addr[n], -1, modes); if(retval < 0) { // failure message already printed by hal_parport_get hal_exit(comp_id); return retval; } /* config addr and direction */ port_data_array[n].base_addr = port_data_array[n].portdata.base; port_data_array[n].data_dir = data_dir[n]; port_data_array[n].use_control_in = use_control_in[n]; if(force_epp[n] && port_data_array[n].portdata.base_hi) { /* select EPP mode in ECR */ outb(0x94, port_data_array[n].portdata.base_hi + 2); } /* set data port (pins 2-9) direction to "in" if needed */ if (data_dir[n]) { rtapi_outb(rtapi_inb(port_data_array[n].base_addr+2) | 0x20, port_data_array[n].base_addr+2); } /* export all vars */ retval = export_port(n, &(port_data_array[n])); if (retval != 0) { rtapi_print_msg(RTAPI_MSG_ERR, "PARPORT: ERROR: port %d var export failed\n", n); hal_exit(comp_id); return retval; } } return 0; }
/* init_hal_io() exports HAL pins and parameters making data from the realtime control module visible and usable by the world */ static int init_hal_io(void) { int n, retval; joint_hal_t *joint_data; rtapi_print_msg(RTAPI_MSG_INFO, "MOTION: init_hal_io() starting...\n"); /* allocate shared memory for machine data */ emcmot_hal_data = hal_malloc(sizeof(emcmot_hal_data_t)); if (emcmot_hal_data == 0) { rtapi_print_msg(RTAPI_MSG_ERR, _("MOTION: emcmot_hal_data malloc failed\n")); return -1; } /* export machine wide hal pins */ if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->probe_input), mot_comp_id, "motion.probe-input")) < 0) goto error; if ((retval = hal_pin_bit_newf(HAL_IO, &(emcmot_hal_data->spindle_index_enable), mot_comp_id, "motion.spindle-index-enable")) < 0) goto error; if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->spindle_on), mot_comp_id, "motion.spindle-on")) < 0) goto error; if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->spindle_forward), mot_comp_id, "motion.spindle-forward")) < 0) goto error; if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->spindle_reverse), mot_comp_id, "motion.spindle-reverse")) < 0) goto error; if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->spindle_brake), mot_comp_id, "motion.spindle-brake")) < 0) goto error; if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->spindle_speed_out), mot_comp_id, "motion.spindle-speed-out")) < 0) goto error; if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->spindle_speed_out_rps), mot_comp_id, "motion.spindle-speed-out-rps")) < 0) goto error; if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->spindle_speed_cmd_rps), mot_comp_id, "motion.spindle-speed-cmd-rps")) < 0) goto error; if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->spindle_inhibit), mot_comp_id, "motion.spindle-inhibit")) < 0) goto error; *(emcmot_hal_data->spindle_inhibit) = 0; // spindle orient pins if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->spindle_orient_angle), mot_comp_id, "motion.spindle-orient-angle")) < 0) goto error; if ((retval = hal_pin_s32_newf(HAL_OUT, &(emcmot_hal_data->spindle_orient_mode), mot_comp_id, "motion.spindle-orient-mode")) < 0) goto error; if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->spindle_orient), mot_comp_id, "motion.spindle-orient")) < 0) goto error; if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->spindle_locked), mot_comp_id, "motion.spindle-locked")) < 0) goto error; if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->spindle_is_oriented), mot_comp_id, "motion.spindle-is-oriented")) < 0) goto error; if ((retval = hal_pin_s32_newf(HAL_IN, &(emcmot_hal_data->spindle_orient_fault), mot_comp_id, "motion.spindle-orient-fault")) < 0) goto error; *(emcmot_hal_data->spindle_orient_angle) = 0.0; *(emcmot_hal_data->spindle_orient_mode) = 0; *(emcmot_hal_data->spindle_orient) = 0; // if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->inpos_output), mot_comp_id, "motion.motion-inpos")) < 0) goto error; if ((retval = hal_pin_float_newf(HAL_IN, &(emcmot_hal_data->spindle_revs), mot_comp_id, "motion.spindle-revs")) < 0) goto error; if ((retval = hal_pin_float_newf(HAL_IN, &(emcmot_hal_data->spindle_speed_in), mot_comp_id, "motion.spindle-speed-in")) < 0) goto error; if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->spindle_is_atspeed), mot_comp_id, "motion.spindle-at-speed")) < 0) goto error; *emcmot_hal_data->spindle_is_atspeed = 1; if ((retval = hal_pin_float_newf(HAL_IN, &(emcmot_hal_data->adaptive_feed), mot_comp_id, "motion.adaptive-feed")) < 0) goto error; *(emcmot_hal_data->adaptive_feed) = 1.0; if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->feed_hold), mot_comp_id, "motion.feed-hold")) < 0) goto error; *(emcmot_hal_data->feed_hold) = 0; if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->feed_inhibit), mot_comp_id, "motion.feed-inhibit")) < 0) goto error; *(emcmot_hal_data->feed_inhibit) = 0; if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->enable), mot_comp_id, "motion.enable")) < 0) goto error; /* export motion-synched digital output pins */ /* export motion digital input pins */ for (n = 0; n < num_dio; n++) { if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->synch_do[n]), mot_comp_id, "motion.digital-out-%02d", n)) < 0) goto error; if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->synch_di[n]), mot_comp_id, "motion.digital-in-%02d", n)) < 0) goto error; } /* export motion analog input pins */ for (n = 0; n < num_aio; n++) { if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->analog_output[n]), mot_comp_id, "motion.analog-out-%02d", n)) < 0) goto error; if ((retval = hal_pin_float_newf(HAL_IN, &(emcmot_hal_data->analog_input[n]), mot_comp_id, "motion.analog-in-%02d", n)) < 0) goto error; } /* export machine wide hal parameters */ retval = hal_pin_bit_new("motion.motion-enabled", HAL_OUT, &(emcmot_hal_data->motion_enabled), mot_comp_id); if (retval != 0) { return retval; } retval = hal_pin_bit_new("motion.in-position", HAL_OUT, &(emcmot_hal_data->in_position), mot_comp_id); if (retval != 0) { return retval; } retval = hal_pin_bit_new("motion.coord-mode", HAL_OUT, &(emcmot_hal_data->coord_mode), mot_comp_id); if (retval != 0) { return retval; } retval = hal_pin_bit_new("motion.teleop-mode", HAL_OUT, &(emcmot_hal_data->teleop_mode), mot_comp_id); if (retval != 0) { return retval; } retval = hal_pin_bit_new("motion.coord-error", HAL_OUT, &(emcmot_hal_data->coord_error), mot_comp_id); if (retval != 0) { return retval; } retval = hal_pin_bit_new("motion.on-soft-limit", HAL_OUT, &(emcmot_hal_data->on_soft_limit), mot_comp_id); if (retval != 0) { return retval; } retval = hal_pin_float_new("motion.current-vel", HAL_OUT, &(emcmot_hal_data->current_vel), mot_comp_id); if (retval != 0) { return retval; } retval = hal_pin_float_new("motion.requested-vel", HAL_OUT, &(emcmot_hal_data->requested_vel), mot_comp_id); if (retval != 0) { return retval; } retval = hal_pin_float_new("motion.distance-to-go", HAL_OUT, &(emcmot_hal_data->distance_to_go), mot_comp_id); if (retval != 0) { return retval; } retval = hal_pin_s32_new("motion.program-line", HAL_OUT, &(emcmot_hal_data->program_line), mot_comp_id); if (retval != 0) { return retval; } /* export debug parameters */ /* these can be used to view any internal variable, simply change a line in control.c:output_to_hal() and recompile */ retval = hal_param_bit_new("motion.debug-bit-0", HAL_RO, &(emcmot_hal_data->debug_bit_0), mot_comp_id); if (retval != 0) { return retval; } retval = hal_param_bit_new("motion.debug-bit-1", HAL_RO, &(emcmot_hal_data->debug_bit_1), mot_comp_id); if (retval != 0) { return retval; } retval = hal_param_float_new("motion.debug-float-0", HAL_RO, &(emcmot_hal_data->debug_float_0), mot_comp_id); if (retval != 0) { return retval; } retval = hal_param_float_new("motion.debug-float-1", HAL_RO, &(emcmot_hal_data->debug_float_1), mot_comp_id); if (retval != 0) { return retval; } retval = hal_param_float_new("motion.debug-float-2", HAL_RO, &(emcmot_hal_data->debug_float_2), mot_comp_id); if (retval != 0) { return retval; } retval = hal_param_float_new("motion.debug-float-3", HAL_RO, &(emcmot_hal_data->debug_float_3), mot_comp_id); if (retval != 0) { return retval; } retval = hal_param_s32_new("motion.debug-s32-0", HAL_RO, &(emcmot_hal_data->debug_s32_0), mot_comp_id); if (retval != 0) { return retval; } retval = hal_param_s32_new("motion.debug-s32-1", HAL_RO, &(emcmot_hal_data->debug_s32_1), mot_comp_id); if (retval != 0) { return retval; } // FIXME - debug only, remove later // export HAL parameters for some trajectory planner internal variables // so they can be scoped retval = hal_param_float_new("traj.pos_out", HAL_RO, &(emcmot_hal_data->traj_pos_out), mot_comp_id); if (retval != 0) { return retval; } retval = hal_param_float_new("traj.vel_out", HAL_RO, &(emcmot_hal_data->traj_vel_out), mot_comp_id); if (retval != 0) { return retval; } retval = hal_param_u32_new("traj.active_tc", HAL_RO, &(emcmot_hal_data->traj_active_tc), mot_comp_id); if (retval != 0) { return retval; } for ( n = 0 ; n < 4 ; n++ ) { retval = hal_param_float_newf(HAL_RO, &(emcmot_hal_data->tc_pos[n]), mot_comp_id, "tc.%d.pos", n); if (retval != 0) { return retval; } retval = hal_param_float_newf(HAL_RO, &(emcmot_hal_data->tc_vel[n]), mot_comp_id, "tc.%d.vel", n); if (retval != 0) { return retval; } retval = hal_param_float_newf(HAL_RO, &(emcmot_hal_data->tc_acc[n]), mot_comp_id, "tc.%d.acc", n); if (retval != 0) { return retval; } } // end of exporting trajectory planner internals // export timing related HAL parameters so they can be scoped retval = hal_param_u32_new("motion.servo.last-period", HAL_RO, &(emcmot_hal_data->last_period), mot_comp_id); if (retval != 0) { return retval; } #ifdef HAVE_CPU_KHZ retval = hal_param_float_new("motion.servo.last-period-ns", HAL_RO, &(emcmot_hal_data->last_period_ns), mot_comp_id); if (retval != 0) { return retval; } #endif retval = hal_param_u32_new("motion.servo.overruns", HAL_RW, &(emcmot_hal_data->overruns), mot_comp_id); if (retval != 0) { return retval; } retval = hal_pin_float_new("motion.tooloffset.x", HAL_OUT, &(emcmot_hal_data->tooloffset_x), mot_comp_id); if (retval != 0) { return retval; } retval = hal_pin_float_new("motion.tooloffset.y", HAL_OUT, &(emcmot_hal_data->tooloffset_y), mot_comp_id); if (retval != 0) { return retval; } retval = hal_pin_float_new("motion.tooloffset.z", HAL_OUT, &(emcmot_hal_data->tooloffset_z), mot_comp_id); if (retval != 0) { return retval; } retval = hal_pin_float_new("motion.tooloffset.a", HAL_OUT, &(emcmot_hal_data->tooloffset_a), mot_comp_id); if (retval != 0) { return retval; } retval = hal_pin_float_new("motion.tooloffset.b", HAL_OUT, &(emcmot_hal_data->tooloffset_b), mot_comp_id); if (retval != 0) { return retval; } retval = hal_pin_float_new("motion.tooloffset.c", HAL_OUT, &(emcmot_hal_data->tooloffset_c), mot_comp_id); if (retval != 0) { return retval; } retval = hal_pin_float_new("motion.tooloffset.u", HAL_OUT, &(emcmot_hal_data->tooloffset_u), mot_comp_id); if (retval != 0) { return retval; } retval = hal_pin_float_new("motion.tooloffset.v", HAL_OUT, &(emcmot_hal_data->tooloffset_v), mot_comp_id); if (retval != 0) { return retval; } retval = hal_pin_float_new("motion.tooloffset.w", HAL_OUT, &(emcmot_hal_data->tooloffset_w), mot_comp_id); if (retval != 0) { return retval; } /* initialize machine wide pins and parameters */ *(emcmot_hal_data->probe_input) = 0; /* default value of enable is TRUE, so simple machines can leave it disconnected */ *(emcmot_hal_data->enable) = 1; /* motion synched dio, init to not enabled */ for (n = 0; n < num_dio; n++) { *(emcmot_hal_data->synch_do[n]) = 0; *(emcmot_hal_data->synch_di[n]) = 0; } for (n = 0; n < num_aio; n++) { *(emcmot_hal_data->analog_output[n]) = 0.0; *(emcmot_hal_data->analog_input[n]) = 0.0; } /*! \todo FIXME - these don't really need initialized, since they are written with data from the emcmotStatus struct */ *(emcmot_hal_data->motion_enabled) = 0; *(emcmot_hal_data->in_position) = 0; *(emcmot_hal_data->coord_mode) = 0; *(emcmot_hal_data->teleop_mode) = 0; *(emcmot_hal_data->coord_error) = 0; *(emcmot_hal_data->on_soft_limit) = 0; /* init debug parameters */ emcmot_hal_data->debug_bit_0 = 0; emcmot_hal_data->debug_bit_1 = 0; emcmot_hal_data->debug_float_0 = 0.0; emcmot_hal_data->debug_float_1 = 0.0; emcmot_hal_data->debug_float_2 = 0.0; emcmot_hal_data->debug_float_3 = 0.0; emcmot_hal_data->overruns = 0; emcmot_hal_data->last_period = 0; /* export joint pins and parameters */ for (n = 0; n < num_joints; n++) { /* point to axis data */ joint_data = &(emcmot_hal_data->joint[n]); /* export all vars */ retval = export_joint(n, joint_data); if (retval != 0) { rtapi_print_msg(RTAPI_MSG_ERR, _("MOTION: joint %d pin/param export failed\n"), n); return -1; } /* init axis pins and parameters */ /* FIXME - struct members are in a state of flux - make sure to update this - most won't need initing anyway */ *(joint_data->amp_enable) = 0; *(joint_data->home_state) = 0; /* We'll init the index model to EXT_ENCODER_INDEX_MODEL_RAW for now, because it is always supported. */ } /* Done! */ rtapi_print_msg(RTAPI_MSG_INFO, "MOTION: init_hal_io() complete, %d axes.\n", n); return 0; error: return retval; }
/** \brief main realtime task */ int rtapi_app_main(void) { // zynq and FPGA code revision int rev, zrev, n; // save messaging level static int msg_level; int retval = 0; int fdmisc; // save message level on entering msg_level = rtapi_get_msg_level(); /* setup messaging level in: RTAPI_MSG_NONE, RTAPI_MSG_ERR, RTAPI_MSG_WARN, RTAPI_MSG_INFO, RTAPI_MSG_DBG, RTAPI_MSG_ALL */ rtapi_set_msg_level(RTAPI_MSG_ALL); // check Zynq revision if ((zrev = zynq_revision()) < 0) { // unable to determine zynq revision rtapi_print_msg(RTAPI_MSG_ERR, "HAL_ZED_CAN: ERROR: unable to determine zynq revision"); hal_exit(comp_id); return -1; } // notify zynq revision rtapi_print_msg(RTAPI_MSG_INFO, "HAL_ZED_CAN: Zynq Revision %d \n", zrev); // check Zedboard FPGA hardware revision rev = zb_revision(); // do revision specific configuration switch (rev) { case 01: rtapi_print_msg(RTAPI_MSG_INFO, "HAL_ZED_CAN: Zedboard FPGA Revision 01\n"); break; default: rtapi_print_msg(RTAPI_MSG_ERR, "HAL_ZED_CAN: ERROR: FPGA revision %d not (yet) supported\n", rev); return -1; break; } // open and map miscellaneous peripheral for PMOD IO access fdmisc = open("/dev/mem", O_RDWR); map_MiscAddress = (unsigned int) mmap(NULL, 100, PROT_READ | PROT_WRITE, MAP_SHARED, fdmisc, (off_t)MISC_ADDR ); if (-1 == map_MiscAddress ) { fprintf(stderr, "MMap failed to map Miscellaneus peripheral\n"); return 0; } printf("Map Misc peripheral: virtual 0x%x real 0x%x \n", map_MiscAddress, MISC_ADDR); // parse module parameters in order to find the number // of configured FOC channels and their CAN address/configuration for(n = 0; n < MAX_FOC_CHAN && FOC_axis[n] != -1 ; n++) { // check for a valid CAN address if( (FOC_axis[n] <= 0) || ( FOC_axis[n] > 15) ) { rtapi_print_msg(RTAPI_MSG_ERR, "HAL_ZED_CAN: ERROR: bad CAN address '%i', axis %i", FOC_axis[n], n); hal_exit(comp_id); return -1; } // check the control mode configuration if( INVALID == parse_ctrl_type(ctrl_type[n])) { rtapi_print_msg(RTAPI_MSG_ERR,"HAL_ZED_CAN: ERROR: bad control type '%s' for axis %i ('c','j','v','i','t')", ctrl_type[n], n); hal_exit(comp_id); return -1; } // found a correctly configured channel num_chan++; // notify rtapi_print_msg(RTAPI_MSG_INFO, "HAL_ZED_CAN: FOC axis %d with CAN address %d.",n, FOC_axis[n] ); rtapi_print_msg(RTAPI_MSG_INFO, "HAL_ZED_CAN: Motor gear %d, Screw gear %d, Screw ratio %d Encoder PPR %d.", Screw_gear[n], Motor_gear[n], Screw_ratio[n], PPR[n]); } if( (0 == num_chan) || (8 < num_chan) ) { rtapi_print_msg(RTAPI_MSG_ERR, "HAL_ZED_CAN: ERROR: channels configured incorrectly."); hal_exit(comp_id); return -1; } // allocate shared memory for FOC_data of each axis FOC_data_array = hal_malloc(num_chan * sizeof(FOC_data_t)); if ( 0 == FOC_data_array ) { rtapi_print_msg(RTAPI_MSG_ERR, "HAL_ZED_CAN: ERROR: hal_malloc() failed\n"); hal_exit(comp_id); return -1; } /** \note CAN8/2FOC connection is 1 axis for each CAN channel */ // configure CAN communication for(n = 0; n < num_chan ; n++) { if ( 0 != setup_CAN(n) ) { hal_exit(comp_id); return -1; } } // try to init the component comp_id = hal_init("hal_zed_can"); if( comp_id < 0 ) { rtapi_print_msg(RTAPI_MSG_ERR, "HAL_ZED_CAN: ERROR: hal_init() failed\n"); hal_exit(comp_id); return -1; } /* Export the variables/parameters for each FOC axis */ for (n = 0; n < num_chan; n++) { // export pins/params for the n^th component retval = exportFOCaxis(n, &(FOC_data_array[n]) ); if( retval < 0 ) { rtapi_print_msg(RTAPI_MSG_ERR, "HAL_ZED_CAN: ERROR: exportFOCaxis() failed"); hal_exit(comp_id); return -1; } } // export the read_feedback and send_setpoint functions as hal_zed_can.send_setpoint and hal_zed_can.read_feedback in hal retval = hal_export_funct("hal_zed_can.send_setpoint", send_setpoint, FOC_data_array, 0, 0, comp_id); if (retval < 0) { rtapi_print_msg(RTAPI_MSG_ERR, "HAL_ZED_CAN: ERROR: write funct export failed\n"); hal_exit(comp_id); return -1; } retval = hal_export_funct("hal_zed_can.read_feedback", read_feedback, FOC_data_array, 0, 0, comp_id); if (retval < 0) { rtapi_print_msg(RTAPI_MSG_ERR, "HAL_ZED_CAN: ERROR: read funct export failed\n"); hal_exit(comp_id); return -1; } // activate periodic communication on all axis setup_2FOC_periodic(); rtapi_print_msg(RTAPI_MSG_INFO, "HAL_ZED_CAN: FOC periodic data exchange active."); // all operations succeded rtapi_print_msg(RTAPI_MSG_INFO, "HAL_ZED_CAN: driver installed successfully.\n"); hal_ready(comp_id); // return to previous mesaging level rtapi_set_msg_level(msg_level); return 0; }
int rtapi_app_main(void) { int n; board_data_t *pboard; struct pci_dev *pDev; // Connect to the HAL. driver.comp_id = hal_init("opto_ac5"); if (driver.comp_id < 0) { rtapi_print_msg(RTAPI_MSG_ERR, " ERROR OPTO_AC5--- hal_init() failed\n"); return(-1); } for ( n = 0 ; n < MAX_BOARDS ; n++ ) { driver.boards[n] = NULL; } pDev = NULL; for ( n = 0 ; n < MAX_BOARDS ; n++ ) { // Find a M5I20 card. pDev = pci_get_device(opto22_VENDOR_ID, opto22_pci_AC5_DEVICE_ID, pDev); if ( pDev == NULL ) { /* no more boards */break; } /* Allocate HAL memory for the board */ pboard = (board_data_t *)(hal_malloc(sizeof(board_data_t))); if ( pboard == NULL ) { rtapi_print_msg(RTAPI_MSG_ERR, "ERROR OPTO_AC5--- hal_malloc() failed\n"); rtapi_app_exit(); return -1; } // save pointer driver.boards[n] = pboard; /* gather info about the board and save it */ pboard->pci_dev = pDev; pboard->slot = PCI_SLOT(pDev->devfn); pboard->num = n; rtapi_print("INFO OPTO_AC5--- Board %d detected in PCI Slot: %2x\n", pboard->num, pboard->slot); /* region 0 is the 32 bit memory mapped I/O region */ pboard->len = pci_resource_len(pDev, 0); pboard->base = ioremap_nocache(pci_resource_start(pDev, 0), pboard->len); if ( pboard->base == NULL ) { rtapi_print_msg(RTAPI_MSG_ERR, "ERROR OPTO_AC5--- could not find board %d PCI base address\n", pboard->num ); rtapi_app_exit(); return -1; } else { rtapi_print( "INFO OPTO_AC5--- board %d mapped to %08lx, Len = %ld\n", pboard->num, (long)pboard->base,(long)pboard->len); } // Initialize device. if(Device_Init(pboard)) { hal_exit(driver.comp_id); return(-1); } // Export pins, parameters, and functions. if(Device_ExportPinsParametersFunctions(pboard, driver.comp_id, n)) { hal_exit(driver.comp_id); return(-1); } } if(n == 0) { /* No cards detected */ rtapi_print ("ERROR OPTO_AC5--- No opto PCI-AC5 card(s) detected\n"); rtapi_app_exit(); return(-1); } hal_ready(driver.comp_id); return(0); }
int lcec_el5152_init(int comp_id, struct lcec_slave *slave, ec_pdo_entry_reg_t *pdo_entry_regs) { lcec_master_t *master = slave->master; lcec_el5152_data_t *hal_data; int i; lcec_el5152_chan_t *chan; int err; // initialize callbacks slave->proc_read = lcec_el5152_read; slave->proc_write = lcec_el5152_write; // alloc hal memory if ((hal_data = hal_malloc(sizeof(lcec_el5152_data_t))) == NULL) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "hal_malloc() for slave %s.%s failed\n", master->name, slave->name); return -EIO; } memset(hal_data, 0, sizeof(lcec_el5152_data_t)); slave->hal_data = hal_data; // initializer sync info slave->sync_info = lcec_el5152_syncs; // initialize global data hal_data->last_operational = 0; // initialize pins for (i=0; i<LCEC_EL5152_CHANS; i++) { chan = &hal_data->chans[i]; // initialize POD entries LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6000 + (i << 4), 0x03, &chan->set_count_done_pdo_os, &chan->set_count_done_pdo_bp); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6000 + (i << 4), 0x08, &chan->expol_stall_pdo_os, &chan->expol_stall_pdo_bp); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6000 + (i << 4), 0x09, &chan->ina_pdo_os, &chan->ina_pdo_bp); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6000 + (i << 4), 0x0a, &chan->inb_pdo_os, &chan->inb_pdo_bp); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x1800 + (i << 2), 0x09, &chan->tx_toggle_pdo_os, &chan->tx_toggle_pdo_bp); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6000 + (i << 4), 0x11, &chan->count_pdo_os, NULL); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6000 + (i << 4), 0x14, &chan->period_pdo_os, NULL); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x7000 + (i << 4), 0x03, &chan->set_count_pdo_os, &chan->set_count_pdo_bp); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x7000 + (i << 4), 0x11, &chan->set_count_val_pdo_os, NULL); // export pins if ((err = hal_pin_bit_newf(HAL_IN, &(chan->index), comp_id, "%s.%s.%s.enc-%d-index", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-index failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_bit_newf(HAL_IO, &(chan->index_ena), comp_id, "%s.%s.%s.enc-%d-index-enable", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-index-enable failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_bit_newf(HAL_IN, &(chan->reset), comp_id, "%s.%s.%s.enc-%d-reset", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-reset failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_bit_newf(HAL_OUT, &(chan->ina), comp_id, "%s.%s.%s.enc-%d-ina", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-ina failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_bit_newf(HAL_OUT, &(chan->inb), comp_id, "%s.%s.%s.enc-%d-inb", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-inb failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_bit_newf(HAL_OUT, &(chan->expol_stall), comp_id, "%s.%s.%s.enc-%d-expol-stall", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-expol-stall failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_bit_newf(HAL_OUT, &(chan->tx_toggle), comp_id, "%s.%s.%s.enc-%d-tx-toggle", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-tx-toggle failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_bit_newf(HAL_IO, &(chan->set_raw_count), comp_id, "%s.%s.%s.enc-%d-set-raw-count", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-set-raw-count failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_s32_newf(HAL_IN, &(chan->set_raw_count_val), comp_id, "%s.%s.%s.enc-%d-set-raw-count-val", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-set-raw-count-val failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_s32_newf(HAL_OUT, &(chan->raw_count), comp_id, "%s.%s.%s.enc-%d-raw-count", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-raw-count failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_s32_newf(HAL_OUT, &(chan->count), comp_id, "%s.%s.%s.enc-%d-count", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-count failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_u32_newf(HAL_OUT, &(chan->raw_period), comp_id, "%s.%s.%s.enc-%d-raw-period", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-raw-period failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_float_newf(HAL_OUT, &(chan->pos), comp_id, "%s.%s.%s.enc-%d-pos", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-pos failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_float_newf(HAL_OUT, &(chan->period), comp_id, "%s.%s.%s.enc-%d-period", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-period failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_float_newf(HAL_IO, &(chan->pos_scale), comp_id, "%s.%s.%s.enc-%d-pos-scale", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-scale failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } // initialize pins *(chan->index) = 0; *(chan->index_ena) = 0; *(chan->reset) = 0; *(chan->ina) = 0; *(chan->inb) = 0; *(chan->expol_stall) = 0; *(chan->tx_toggle) = 0; *(chan->set_raw_count) = 0; *(chan->set_raw_count_val) = 0; *(chan->raw_count) = 0; *(chan->raw_period) = 0; *(chan->count) = 0; *(chan->pos) = 0; *(chan->period) = 0; *(chan->pos_scale) = 1.0; // initialize variables chan->do_init = 1; chan->last_count = 0; chan->last_index = 0; chan->old_scale = *(chan->pos_scale) + 1.0; chan->scale = 1.0; } return 0; }
// init HAL objects static int export_halobjs(struct inst_data *ip, int owner_id, const char *name) { if (hal_pin_float_newf(HAL_OUT, &ip->out, owner_id, "%s.out", name)) return -1; if (hal_pin_float_newf(HAL_IN, &ip->in, owner_id, "%s.in", name)) return -1; if (hal_param_s32_newf(HAL_RO, &ip->iter,owner_id, "%s.iter", name)) return -1; // unittest observer pins, per instance if (hal_pin_s32_newf(HAL_OUT, &ip->repeat_value, owner_id, "%s.repeat_value", name)) return -1; if (hal_pin_s32_newf(HAL_OUT, &ip->prefix_len, owner_id, "%s.prefix_len", name)) return -1; // exporting '<instname>.funct' as an extended thread function // see lutn.c for a discussion of advantages hal_export_xfunct_args_t xfunct_args = { .type = FS_XTHREADFUNC, .funct.x = funct, .arg = ip, // the instance's HAL memory blob .uses_fp = 1, .reentrant = 0, .owner_id = owner_id }; return hal_export_xfunctf(&xfunct_args, "%s.funct", name); } // constructor - init all HAL pins, params, funct etc here static int instantiate(const int argc, const char**argv) { // argv[0]: component name const char *name = argv[1]; // instance name struct inst_data *ip; // allocate a named instance, and some HAL memory for the instance data int inst_id = hal_inst_create(name, comp_id, sizeof(struct inst_data), (void **)&ip); if (inst_id < 0) return -1; // here ip is guaranteed to point to a blob of HAL memory // of size sizeof(struct inst_data). HALINFO("inst=%s argc=%d\n", name, argc); HALINFO("instance parms: repeat=%d prefix='%s'", repeat, prefix); HALINFO("module parms: answer=%d text='%s'", answer, text); // example - parse newinst arguments getopt-style: // straight from: http://www.informit.com/articles/article.aspx?p=175771&seqNum=3 int do_all, do_help, do_verbose; /* flag variables */ char *myfile; struct option longopts[] = { { "all", no_argument, & do_all, 1 }, { "file", required_argument, NULL, 'f' }, { "help", no_argument, & do_help, 1 }, { "verbose", no_argument, & do_verbose, 1 }, { 0, 0, 0, 0 } }; int c; while ((c = getopt_long(argc, argv, ":f:", longopts, NULL)) != -1) { switch (c) { case 'f': myfile = optarg; break; case 0: // getopt_long() set a variable, just keep going break; case ':': // missing option argument HALERR("%s: option `-%c' requires an argument\n", argv[1], optopt); break; case '?': default: // invalid option HALERR("%s: option `-%c' is invalid, ignored", argv[1], optopt); break; } } HALINFO("do_all=%d do_help=%d do_verbose=%d myfile=%s", do_all, do_help, do_verbose, myfile); // these pins/params/functs will be owned by the instance, // and can be separately exited via 'halcmd delinst <instancename>' int retval = export_halobjs(ip, inst_id, name); // unittest: echo instance parameters into observer pins if (!retval) { *(ip->repeat_value) = repeat; *(ip->prefix_len) = strlen(prefix); } return retval; } // custom destructor // pins, params, and functs are automatically deallocated by hal_exit(comp_id) // regardless if a destructor is used or not (see below), so usually it is not // needed // // however, some objects like vtables, rings, threads, signals are not owned by // a component, hence cannot be automatically exited by hal_exit() even if desired // // interaction with such objects may require a custom destructor like below for // cleanup actions // NB: if a customer destructor is used, it is called // - after the instance's functs have been removed from their respective threads // (so a thread funct call cannot interact with the destructor any more) // - any pins and params of this instance are still intact when the destructor is // called, and they are automatically destroyed by the HAL library once the // destructor returns static int delete(const char *name, void *inst, const int inst_size) { HALERR("inst=%s size=%d %p\n", name, inst_size, inst); HALERR("instance parms: repeat=%d prefix='%s'", repeat, prefix); HALERR("module parms: answer=%d text='%s'", answer, text); return 0; } int rtapi_app_main(void) { HALERR("instance parms: repeat=%d prefix='%s'", repeat, prefix); HALERR("module parms: answer=%d text='%s'", answer, text); // to use default destructor, use NULL instead of delete comp_id = hal_xinit(TYPE_RT, 0, 0, instantiate, delete, compname); if (comp_id < 0) return comp_id; #if 0 // this is how an 'instance' would have been done in the legacy way struct inst_data *ip = hal_malloc(sizeof(struct inst_data)); // these pins/params/functs will be owned by the component // NB: this 'instance' cannot be exited, and no new one created on the fly if (export_halobjs(ip, comp_id, "foo")) return -1; #endif // unittest only, see nosetests/unittest_instbindings.py and // nosetests/unittest_icomp.py // purpose: echo module params into observer pins // (cant set pins to strings, so just echo the string length) if ((cd = export_observer_pins(comp_id, compname)) == NULL) return -1; *(cd->answer_value) = answer; *(cd->text_len) = strlen(text); hal_ready(comp_id); return 0; }
int main(int argc, char* argv[]) { if (argc == 1) { logfile = stdout; logfile_name = NULL; } else if (argc == 2) { logfile = NULL; logfile_name = argv[1]; } else { fprintf(stderr, "usage: motion-logger [LOGFILE]\n"); exit(1); } mot_comp_id = hal_init("motion-logger"); motion_logger_data = hal_malloc(sizeof(*motion_logger_data)); int r = hal_pin_bit_new("motion-logger.reopen-log", HAL_IO, &motion_logger_data->reopen, mot_comp_id); if(r < 0) { errno = -r; perror("hal_pin_bit_new"); exit(1); } *motion_logger_data->reopen = 0; r = hal_ready(mot_comp_id); if(r < 0) { errno = -r; perror("hal_ready"); exit(1); } init_comm_buffers(); while (1) { if (c->commandNum != c->tail) { // "split read" continue; } if (c->commandNum == emcmotStatus->commandNumEcho) { // nothing new maybe_reopen_logfile(); usleep(10 * 1000); continue; } // // new incoming command! // emcmotStatus->head++; switch (c->command) { case EMCMOT_ABORT: log_print("ABORT\n"); break; case EMCMOT_AXIS_ABORT: log_print("AXIS_ABORT joint=%d\n", c->axis); break; case EMCMOT_ENABLE: log_print("ENABLE\n"); SET_MOTION_ENABLE_FLAG(1); update_motion_state(); break; case EMCMOT_DISABLE: log_print("DISABLE\n"); SET_MOTION_ENABLE_FLAG(0); update_motion_state(); break; case EMCMOT_ENABLE_AMPLIFIER: log_print("ENABLE_AMPLIFIER\n"); break; case EMCMOT_DISABLE_AMPLIFIER: log_print("DISABLE_AMPLIFIER\n"); break; case EMCMOT_ENABLE_WATCHDOG: log_print("ENABLE_WATCHDOG\n"); break; case EMCMOT_DISABLE_WATCHDOG: log_print("DISABLE_WATCHDOG\n"); break; case EMCMOT_ACTIVATE_JOINT: log_print("ACTIVATE_JOINT joint=%d\n", c->axis); break; case EMCMOT_DEACTIVATE_JOINT: log_print("DEACTIVATE_JOINT joint=%d\n", c->axis); break; case EMCMOT_PAUSE: log_print("PAUSE\n"); break; case EMCMOT_RESUME: log_print("RESUME\n"); break; case EMCMOT_STEP: log_print("STEP\n"); break; case EMCMOT_FREE: log_print("FREE\n"); SET_MOTION_COORD_FLAG(0); SET_MOTION_TELEOP_FLAG(0); update_motion_state(); break; case EMCMOT_COORD: log_print("COORD\n"); SET_MOTION_COORD_FLAG(1); SET_MOTION_TELEOP_FLAG(0); SET_MOTION_ERROR_FLAG(0); update_motion_state(); break; case EMCMOT_TELEOP: log_print("TELEOP\n"); SET_MOTION_TELEOP_FLAG(1); SET_MOTION_ERROR_FLAG(0); update_motion_state(); break; case EMCMOT_SPINDLE_SCALE: log_print("SPINDLE_SCALE\n"); break; case EMCMOT_SS_ENABLE: log_print("SS_ENABLE\n"); break; case EMCMOT_FEED_SCALE: log_print("FEED_SCALE\n"); break; case EMCMOT_RAPID_SCALE: log_print("RAPID_SCALE\n"); break; case EMCMOT_FS_ENABLE: log_print("FS_ENABLE\n"); break; case EMCMOT_FH_ENABLE: log_print("FH_ENABLE\n"); break; case EMCMOT_AF_ENABLE: log_print("AF_ENABLE\n"); break; case EMCMOT_OVERRIDE_LIMITS: log_print("OVERRIDE_LIMITS\n"); break; case EMCMOT_HOME: log_print("HOME joint=%d\n", c->axis); if (c->axis < 0) { for (int j = 0; j < num_joints; j ++) { mark_joint_homed(j); } } else { mark_joint_homed(c->axis); } break; case EMCMOT_UNHOME: log_print("UNHOME joint=%d\n", c->axis); break; case EMCMOT_JOG_CONT: log_print("JOG_CONT\n"); break; case EMCMOT_JOG_INCR: log_print("JOG_INCR\n"); break; case EMCMOT_JOG_ABS: log_print("JOG_ABS\n"); break; case EMCMOT_SET_LINE: log_print( "SET_LINE x=%.6f, y=%.6f, z=%.6f, a=%.6f, b=%.6f, c=%.6f, u=%.6f, v=%.6f, w=%.6f, id=%d, motion_type=%d, vel=%.6f, ini_maxvel=%.6f, acc=%.6f, turn=%d\n", c->pos.tran.x, c->pos.tran.y, c->pos.tran.z, c->pos.a, c->pos.b, c->pos.c, c->pos.u, c->pos.v, c->pos.w, c->id, c->motion_type, c->vel, c->ini_maxvel, c->acc, c->turn ); break; case EMCMOT_SET_CIRCLE: log_print("SET_CIRCLE:\n"); log_print( " pos: x=%.6f, y=%.6f, z=%.6f, a=%.6f, b=%.6f, c=%.6f, u=%.6f, v=%.6f, w=%.6f\n", c->pos.tran.x, c->pos.tran.y, c->pos.tran.z, c->pos.a, c->pos.b, c->pos.c, c->pos.u, c->pos.v, c->pos.w ); log_print(" center: x=%.6f, y=%.6f, z=%.6f\n", c->center.x, c->center.y, c->center.z); log_print(" normal: x=%.6f, y=%.6f, z=%.6f\n", c->normal.x, c->normal.y, c->normal.z); log_print(" id=%d, motion_type=%d, vel=%.6f, ini_maxvel=%.6f, acc=%.6f, turn=%d\n", c->id, c->motion_type, c->vel, c->ini_maxvel, c->acc, c->turn ); break; case EMCMOT_SET_TELEOP_VECTOR: log_print("SET_TELEOP_VECTOR\n"); break; case EMCMOT_CLEAR_PROBE_FLAGS: log_print("CLEAR_PROBE_FLAGS\n"); break; case EMCMOT_PROBE: log_print("PROBE\n"); break; case EMCMOT_RIGID_TAP: log_print("RIGID_TAP\n"); break; case EMCMOT_SET_POSITION_LIMITS: log_print( "SET_POSITION_LIMITS joint=%d, min=%.6f, max=%.6f\n", c->axis, c->minLimit, c->maxLimit ); joints[c->axis].max_pos_limit = c->maxLimit; joints[c->axis].min_pos_limit = c->minLimit; break; case EMCMOT_SET_BACKLASH: log_print("SET_BACKLASH joint=%d, backlash=%.6f\n", c->axis, c->backlash); break; case EMCMOT_SET_MIN_FERROR: log_print("SET_MIN_FERROR joint=%d, minFerror=%.6f\n", c->axis, c->minFerror); break; case EMCMOT_SET_MAX_FERROR: log_print("SET_MAX_FERROR joint=%d, maxFerror=%.6f\n", c->axis, c->maxFerror); break; case EMCMOT_SET_VEL: log_print("SET_VEL vel=%.6f, ini_maxvel=%.6f\n", c->vel, c->ini_maxvel); break; case EMCMOT_SET_VEL_LIMIT: log_print("SET_VEL_LIMIT vel=%.6f\n", c->vel); break; case EMCMOT_SET_JOINT_VEL_LIMIT: log_print("SET_JOINT_VEL_LIMIT joint=%d, vel=%.6f\n", c->axis, c->vel); break; case EMCMOT_SET_JOINT_ACC_LIMIT: log_print("SET_JOINT_ACC_LIMIT joint=%d, acc=%.6f\n", c->axis, c->acc); break; case EMCMOT_SET_ACC: log_print("SET_ACC acc=%.6f\n", c->acc); break; case EMCMOT_SET_TERM_COND: log_print("SET_TERM_COND termCond=%d, tolerance=%.6f\n", c->termCond, c->tolerance); break; case EMCMOT_SET_NUM_AXES: log_print("SET_NUM_AXES %d\n", c->axis); num_joints = c->axis; break; case EMCMOT_SET_WORLD_HOME: log_print( "SET_WORLD_HOME x=%.6f, y=%.6f, z=%.6f, a=%.6f, b=%.6f, c=%.6f, u=%.6f, v=%.6f, w=%.6f\n", c->pos.tran.x, c->pos.tran.y, c->pos.tran.z, c->pos.a, c->pos.b, c->pos.c, c->pos.u, c->pos.v, c->pos.w ); break; case EMCMOT_SET_HOMING_PARAMS: log_print( "SET_HOMING_PARAMS joint=%d, offset=%.6f home=%.6f, final_vel=%.6f, search_vel=%.6f, latch_vel=%.6f, flags=0x%08x, sequence=%d, volatile=%d\n", c->axis, c->offset, c->home, c->home_final_vel, c->search_vel, c->latch_vel, c->flags, c->home_sequence, c->volatile_home ); break; case EMCMOT_SET_DEBUG: log_print("SET_DEBUG\n"); break; case EMCMOT_SET_DOUT: log_print("SET_DOUT\n"); break; case EMCMOT_SET_AOUT: log_print("SET_AOUT\n"); break; case EMCMOT_SET_SPINDLESYNC: log_print("SET_SPINDLESYNC sync=%06f, flags=0x%08x\n", c->spindlesync, c->flags); break; case EMCMOT_SPINDLE_ON: log_print("SPINDLE_ON\n"); break; case EMCMOT_SPINDLE_OFF: log_print("SPINDLE_OFF\n"); break; case EMCMOT_SPINDLE_INCREASE: log_print("SPINDLE_INCREASE\n"); break; case EMCMOT_SPINDLE_DECREASE: log_print("SPINDLE_DECREASE\n"); break; case EMCMOT_SPINDLE_BRAKE_ENGAGE: log_print("SPINDLE_BRAKE_ENGAGE\n"); break; case EMCMOT_SPINDLE_BRAKE_RELEASE: log_print("SPINDLE_BRAKE_RELEASE\n"); break; case EMCMOT_SPINDLE_ORIENT: log_print("SPINDLE_ORIENT\n"); break; case EMCMOT_SET_MOTOR_OFFSET: log_print("SET_MOTOR_OFFSET\n"); break; case EMCMOT_SET_JOINT_COMP: log_print("SET_JOINT_COMP\n"); break; case EMCMOT_SET_OFFSET: log_print( "SET_OFFSET x=%.6f, y=%.6f, z=%.6f, a=%.6f, b=%.6f, c=%.6f u=%.6f, v=%.6f, w=%.6f\n", c->tool_offset.tran.x, c->tool_offset.tran.y, c->tool_offset.tran.z, c->tool_offset.a, c->tool_offset.b, c->tool_offset.c, c->tool_offset.u, c->tool_offset.v, c->tool_offset.w ); break; case EMCMOT_SET_MAX_FEED_OVERRIDE: log_print("SET_MAX_FEED_OVERRIDE %.6f\n", c->maxFeedScale); break; case EMCMOT_SETUP_ARC_BLENDS: log_print("SETUP_ARC_BLENDS\n"); break; default: log_print("ERROR: unknown command %d\n", c->command); break; } update_joint_status(); emcmotStatus->commandEcho = c->command; emcmotStatus->commandNumEcho = c->commandNum; emcmotStatus->commandStatus = EMCMOT_COMMAND_OK; emcmotStatus->tail = emcmotStatus->head; } return 0; }