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;
}
