static int pins_and_params(char *argv[])
{
long port_addr[MAX_PORTS];
int data_dir[MAX_PORTS];
int use_control_in[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;
}
/* 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] == '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 = use_control_in[n] ? PARPORT_MODE_TRISTATE : 0;
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];
/* 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;
}
int _rtapi_shmem_delete_inst(int shmem_id, int instance, int module_id) {
shmem_data *shmem;
int manage_lock, retval;
#ifdef RTAPI
struct shm_status sm;
#endif
/* validate shmem ID */
if ((shmem_id < 1) || (shmem_id > RTAPI_MAX_SHMEMS)) {
return -EINVAL;
}
/* point to the shmem's data */
shmem = &(shmem_array[shmem_id]);
/* is the block valid? */
if (shmem->key == 0) {
return -EINVAL;
}
/* validate module_id */
if ((module_id < 1) || (module_id > RTAPI_MAX_MODULES)) {
return -EINVAL;
}
if (module_array[module_id].state != MODULE_STATE) {
return -EINVAL;
}
/* is this module using the block? */
if (rtapi_test_bit(module_id, shmem->bitmap) == 0) {
return -EINVAL;
}
/* check if we need to manage the mutex */
manage_lock = (shmem->magic != SHMEM_MAGIC_DEL_LOCKED);
/* if no magic delete lock held is set, get the mutex */
if (manage_lock) rtapi_mutex_get(&(rtapi_data->mutex));
/* OK, we're no longer using it */
rtapi_clear_bit(module_id, shmem->bitmap);
#ifdef ULAPI
shmem->ulusers--;
if ((shmem->ulusers == 0) && (shmem->rtusers == 0)) {
// shmdrv can detach unused shared memory from userland too
// this will munmap() the segment causing a drop in uattach refcount
// and eventual free by garbage collect in shmdrv
retval = shm_common_detach(shmem->size, shmem_addr_array[shmem_id]);
if (retval) {
rtapi_print_msg(RTAPI_MSG_ERR,
"ULAPI:%d ERROR: shm_common_detach(%02d) failed: %s\n",
rtapi_instance, shmem_id, strerror(-retval));
}
}
/* unmap the block */
shmem_addr_array[shmem_id] = NULL;
#else /* RTAPI */
shmem->rtusers--;
#endif /* RTAPI */
/* is somebody else still using the block? */
if ((shmem->ulusers > 0) || (shmem->rtusers > 0)) {
/* yes, we're done for now */
rtapi_print_msg(RTAPI_MSG_DBG,
"RTAPI: shmem %02d closed by module %02d\n", shmem_id, module_id);
if (manage_lock) rtapi_mutex_give(&(rtapi_data->mutex));
return 0;
}
#ifdef RTAPI
/* no other realtime users, free the shared memory from kernel space */
shmem_addr_array[shmem_id] = NULL;
shmem->rtusers = 0;
/* are any user processes using the block? */
if (shmem->ulusers > 0) {
/* yes, we're done for now */
rtapi_print_msg(RTAPI_MSG_DBG,
"RTAPI: shmem %02d unmapped by module %02d\n", shmem_id,
module_id);
if (manage_lock) rtapi_mutex_give(&(rtapi_data->mutex));
return 0;
}
/* no other users at all, this ID is now free */
sm.key = shmem->key;
sm.size = shmem->size;
sm.flags = 0;
if ((retval = shmdrv_detach(&sm)) < 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"RTAPI:%d ERROR: shmdrv_detach(%x,%d) fail: %d\n",
rtapi_instance, sm.key, sm.size, retval);
}
#endif /* RTAPI */
/* update the data array and usage count */
shmem->key = 0;
shmem->size = 0;
rtapi_data->shmem_count--;
/* release the lock if needed, print a debug message and return */
if (manage_lock) rtapi_mutex_give(&(rtapi_data->mutex));
rtapi_print_msg(RTAPI_MSG_DBG, "RTAPI: shmem %02d freed by module %02d\n",
shmem_id, module_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;
/* 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 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;
}
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 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_init(char *modname)
{
int n, module_id;
module_data *module;
/* say hello */
rtapi_print_msg(RTAPI_MSG_DBG, "RTAPI: initing module %s\n", modname);
/* setup revision string and code, and print opening message */
setup_revision_info();
/* get shared memory block from OS and save its address */
errno = 0;
rtapi_data = rtai_malloc(RTAPI_KEY, sizeof(rtapi_data_t));
if (rtapi_data == NULL || rtapi_data == (rtapi_data_t*)-1) {
rtapi_print_msg(RTAPI_MSG_ERR,
"RTAPI: ERROR: could not open shared memory (errno=%d)\n", errno);
return RTAPI_NOMEM;
}
/* perform a global init if needed */
init_rtapi_data(rtapi_data);
/* check revision code */
if (rtapi_data->rev_code != rev_code) {
/* mismatch - release master shared memory block */
rtapi_print_msg(RTAPI_MSG_ERR, "RTAPI: ERROR: version mismatch %d vs %d\n", rtapi_data->rev_code, rev_code);
rtai_free(RTAPI_KEY, rtapi_data);
return RTAPI_FAIL;
}
/* set up local pointers to global data */
module_array = rtapi_data->module_array;
task_array = rtapi_data->task_array;
shmem_array = rtapi_data->shmem_array;
sem_array = rtapi_data->sem_array;
fifo_array = rtapi_data->fifo_array;
irq_array = rtapi_data->irq_array;
/* perform local init */
for (n = 0; n <= RTAPI_MAX_SHMEMS; n++) {
shmem_addr_array[n] = NULL;
}
/* get the mutex */
rtapi_mutex_get(&(rtapi_data->mutex));
/* find empty spot in module array */
n = 1;
while ((n <= RTAPI_MAX_MODULES) && (module_array[n].state != NO_MODULE)) {
n++;
}
if (n > RTAPI_MAX_MODULES) {
/* no room */
rtapi_mutex_give(&(rtapi_data->mutex));
rtapi_print_msg(RTAPI_MSG_ERR, "RTAPI: ERROR: reached module limit %d\n",
n);
return RTAPI_LIMIT;
}
/* we have space for the module */
module_id = n;
module = &(module_array[n]);
/* update module data */
module->state = USERSPACE;
if (modname != NULL) {
/* use name supplied by caller, truncating if needed */
snprintf(module->name, RTAPI_NAME_LEN, "%s", modname);
} else {
/* make up a name */
snprintf(module->name, RTAPI_NAME_LEN, "ULMOD%03d", module_id);
}
rtapi_data->ul_module_count++;
rtapi_mutex_give(&(rtapi_data->mutex));
rtapi_print_msg(RTAPI_MSG_DBG, "RTAPI: module '%s' inited, ID = %02d\n",
module->name, module_id);
return module_id;
}
int rtapi_shmem_new(int key, int module_id, unsigned long int size)
{
int n;
int shmem_id;
shmem_data *shmem;
/* key must be non-zero, and also cannot match the key that RTAPI uses */
if ((key == 0) || (key == RTAPI_KEY)) {
rtapi_print_msg(RTAPI_MSG_ERR, "RTAPI: ERROR: bad shmem key: %d\n",
key);
return RTAPI_INVAL;
}
/* get the mutex */
rtapi_mutex_get(&(rtapi_data->mutex));
/* validate module_id */
if ((module_id < 1) || (module_id > RTAPI_MAX_MODULES)) {
rtapi_mutex_give(&(rtapi_data->mutex));
rtapi_print_msg(RTAPI_MSG_ERR, "RTAPI: ERROR: bad module ID: %d\n",
module_id);
return RTAPI_INVAL;
}
if (module_array[module_id].state != USERSPACE) {
rtapi_print_msg(RTAPI_MSG_ERR,
"RTAPI: ERROR: not a user space module ID: %d\n", module_id);
rtapi_mutex_give(&(rtapi_data->mutex));
return RTAPI_INVAL;
}
/* check if a block is already open for this key */
for (n = 1; n <= RTAPI_MAX_SHMEMS; n++) {
if (shmem_array[n].key == key) {
/* found a match */
shmem_id = n;
shmem = &(shmem_array[n]);
/* is it big enough? */
if (shmem->size < size) {
rtapi_mutex_give(&(rtapi_data->mutex));
rtapi_print_msg(RTAPI_MSG_ERR,
"RTAPI: ERROR: shmem size mismatch\n");
return RTAPI_INVAL;
}
/* is this module already using it? */
if (test_bit(module_id, shmem->bitmap)) {
rtapi_mutex_give(&(rtapi_data->mutex));
rtapi_print_msg(RTAPI_MSG_WARN,
"RTAPI: Warning: shmem already mapped\n");
return RTAPI_INVAL;
}
/* no, map it */
shmem_addr_array[shmem_id] = rtai_malloc(key, shmem->size);
if (shmem_addr_array[shmem_id] == NULL) {
/* map failed */
rtapi_print_msg(RTAPI_MSG_ERR,
"RTAPI: ERROR: failed to map shmem\n");
rtapi_mutex_give(&(rtapi_data->mutex));
return RTAPI_NOMEM;
}
/* update usage data */
set_bit(module_id, shmem->bitmap);
shmem->ulusers++;
/* done */
rtapi_mutex_give(&(rtapi_data->mutex));
return shmem_id;
}
}
/* find empty spot in shmem array */
n = 1;
while ((n <= RTAPI_MAX_SHMEMS) && (shmem_array[n].key != 0)) {
n++;
}
if (n > RTAPI_MAX_SHMEMS) {
/* no room */
rtapi_mutex_give(&(rtapi_data->mutex));
rtapi_print_msg(RTAPI_MSG_ERR, "RTAPI: ERROR: reached shmem limit %d\n",
n);
return RTAPI_LIMIT;
}
/* we have space for the block data */
shmem_id = n;
shmem = &(shmem_array[n]);
/* now get shared memory block from OS and save its address */
shmem_addr_array[shmem_id] = rtai_malloc(key, size);
if (shmem_addr_array[shmem_id] == NULL) {
rtapi_mutex_give(&(rtapi_data->mutex));
rtapi_print_msg(RTAPI_MSG_ERR,
"RTAPI: ERROR: could not create shmem %d\n", n);
return RTAPI_NOMEM;
}
/* the block has been created, update data */
set_bit(module_id, shmem->bitmap);
shmem->key = key;
shmem->rtusers = 0;
shmem->ulusers = 1;
shmem->size = size;
rtapi_data->shmem_count++;
/* zero the first word of the shmem area */
*((long int *) (shmem_addr_array[shmem_id])) = 0;
/* done */
rtapi_mutex_give(&(rtapi_data->mutex));
return shmem_id;
}
int tpAddLine(TP_STRUCT * tp, EmcPose end, int type, double vel, double ini_maxvel, double acc, unsigned char enables, char atspeed, int indexrotary)
{
TC_STRUCT tc;
PmLine line_xyz, line_uvw, line_abc;
PmPose start_xyz, end_xyz;
PmPose start_uvw, end_uvw;
PmPose start_abc, end_abc;
PmQuaternion identity_quat = { 1.0, 0.0, 0.0, 0.0 };
if (!tp) {
rtapi_print_msg(RTAPI_MSG_ERR, "TP is null\n");
return -1;
}
if (tp->aborting) {
rtapi_print_msg(RTAPI_MSG_ERR, "TP is aborting\n");
return -1;
}
start_xyz.tran = tp->goalPos.tran;
end_xyz.tran = end.tran;
start_uvw.tran.x = tp->goalPos.u;
start_uvw.tran.y = tp->goalPos.v;
start_uvw.tran.z = tp->goalPos.w;
end_uvw.tran.x = end.u;
end_uvw.tran.y = end.v;
end_uvw.tran.z = end.w;
start_abc.tran.x = tp->goalPos.a;
start_abc.tran.y = tp->goalPos.b;
start_abc.tran.z = tp->goalPos.c;
end_abc.tran.x = end.a;
end_abc.tran.y = end.b;
end_abc.tran.z = end.c;
start_xyz.rot = identity_quat;
end_xyz.rot = identity_quat;
start_uvw.rot = identity_quat;
end_uvw.rot = identity_quat;
start_abc.rot = identity_quat;
end_abc.rot = identity_quat;
pmLineInit(&line_xyz, start_xyz, end_xyz);
pmLineInit(&line_uvw, start_uvw, end_uvw);
pmLineInit(&line_abc, start_abc, end_abc);
tc.sync_accel = 0;
tc.cycle_time = tp->cycleTime;
if (!line_xyz.tmag_zero)
tc.target = line_xyz.tmag;
else if (!line_uvw.tmag_zero)
tc.target = line_uvw.tmag;
else
tc.target = line_abc.tmag;
tc.progress = 0.0;
tc.reqvel = vel;
tc.maxaccel = acc;
tc.feed_override = 0.0;
tc.maxvel = ini_maxvel;
tc.id = tp->nextId;
tc.active = 0;
tc.atspeed = atspeed;
tc.currentvel = 0.0;
tc.blending = 0;
tc.blend_vel = 0.0;
tc.vel_at_blend_start = 0.0;
tc.coords.line.xyz = line_xyz;
tc.coords.line.uvw = line_uvw;
tc.coords.line.abc = line_abc;
tc.motion_type = TC_LINEAR;
tc.canon_motion_type = type;
tc.blend_with_next = tp->termCond == TC_TERM_COND_BLEND;
tc.tolerance = tp->tolerance;
tc.synchronized = tp->synchronized;
tc.velocity_mode = tp->velocity_mode;
tc.uu_per_rev = tp->uu_per_rev;
tc.enables = enables;
tc.indexrotary = indexrotary;
if (syncdio.anychanged != 0) {
tc.syncdio = syncdio; //enqueue the list of DIOs that need toggling
tpClearDIOs(); // clear out the list, in order to prepare for the next time we need to use it
} else {
tc.syncdio.anychanged = 0;
}
if (tcqPut(&tp->queue, tc) == -1) {
rtapi_print_msg(RTAPI_MSG_ERR, "tcqPut failed.\n");
return -1;
}
tp->goalPos = end; // remember the end of this move, as it's
// the start of the next one.
tp->done = 0;
tp->depth = tcqLen(&tp->queue);
tp->nextId++;
return 0;
}
int tpRunCycle(TP_STRUCT * tp, long period)
{
// vel = (new position - old position) / cycle time
// (two position points required)
//
// acc = (new vel - old vel) / cycle time
// (three position points required)
TC_STRUCT *tc, *nexttc;
double primary_vel;
int on_final_decel;
EmcPose primary_before, primary_after;
EmcPose secondary_before, secondary_after;
EmcPose primary_displacement, secondary_displacement;
static double spindleoffset;
static int waiting_for_index = 0;
static int waiting_for_atspeed = 0;
double save_vel;
static double revs;
EmcPose target;
emcmotStatus->tcqlen = tcqLen(&tp->queue);
emcmotStatus->requested_vel = 0.0;
tc = tcqItem(&tp->queue, 0, period);
if(!tc) {
// this means the motion queue is empty. This can represent
// the end of the program OR QUEUE STARVATION. In either case,
// I want to stop. Some may not agree that's what it should do.
tcqInit(&tp->queue);
tp->goalPos = tp->currentPos;
tp->done = 1;
tp->depth = tp->activeDepth = 0;
tp->aborting = 0;
tp->execId = 0;
tp->motionType = 0;
tpResume(tp);
// when not executing a move, use the current enable flags
emcmotStatus->enables_queued = emcmotStatus->enables_new;
return 0;
}
if (tc->target == tc->progress && waiting_for_atspeed != tc->id) {
// if we're synced, and this move is ending, save the
// spindle position so the next synced move can be in
// the right place.
if(tc->synchronized)
spindleoffset += tc->target/tc->uu_per_rev;
else
spindleoffset = 0.0;
if(tc->indexrotary != -1) {
// this was an indexing move, so before we remove it we must
// relock the axis
tpSetRotaryUnlock(tc->indexrotary, 0);
// if it is now locked, fall through and remove the finished move.
// otherwise, just come back later and check again
if(tpGetRotaryIsUnlocked(tc->indexrotary))
return 0;
}
// done with this move
tcqRemove(&tp->queue, 1);
// so get next move
tc = tcqItem(&tp->queue, 0, period);
if(!tc) return 0;
}
// now we have the active tc. get the upcoming one, if there is one.
// it's not an error if there isn't another one - we just don't
// do blending. This happens in MDI for instance.
if(!emcmotDebug->stepping && tc->blend_with_next)
nexttc = tcqItem(&tp->queue, 1, period);
else
nexttc = NULL;
{
int this_synch_pos = tc->synchronized && !tc->velocity_mode;
int next_synch_pos = nexttc && nexttc->synchronized && !nexttc->velocity_mode;
if(!this_synch_pos && next_synch_pos) {
// we'll have to wait for spindle sync; might as well
// stop at the right place (don't blend)
tc->blend_with_next = 0;
nexttc = NULL;
}
}
if(nexttc && nexttc->atspeed) {
// we'll have to wait for the spindle to be at-speed; might as well
// stop at the right place (don't blend), like above
tc->blend_with_next = 0;
nexttc = NULL;
}
if(tp->aborting) {
// an abort message has come
if( waiting_for_index ||
waiting_for_atspeed ||
(tc->currentvel == 0.0 && !nexttc) ||
(tc->currentvel == 0.0 && nexttc && nexttc->currentvel == 0.0) ) {
tcqInit(&tp->queue);
tp->goalPos = tp->currentPos;
tp->done = 1;
tp->depth = tp->activeDepth = 0;
tp->aborting = 0;
tp->execId = 0;
tp->motionType = 0;
tp->synchronized = 0;
waiting_for_index = 0;
waiting_for_atspeed = 0;
emcmotStatus->spindleSync = 0;
tpResume(tp);
return 0;
} else {
tc->reqvel = 0.0;
if(nexttc) nexttc->reqvel = 0.0;
}
}
// this is no longer the segment we were waiting_for_index for
if(waiting_for_index && waiting_for_index != tc->id)
{
rtapi_print_msg(RTAPI_MSG_ERR,
"Was waiting for index on motion id %d, but reached id %d\n",
waiting_for_index, tc->id);
waiting_for_index = 0;
}
if(waiting_for_atspeed && waiting_for_atspeed != tc->id)
{
rtapi_print_msg(RTAPI_MSG_ERR,
"Was waiting for atspeed on motion id %d, but reached id %d\n",
waiting_for_atspeed, tc->id);
waiting_for_atspeed = 0;
}
// check for at-speed before marking the tc active
if(waiting_for_atspeed) {
if(!emcmotStatus->spindle_is_atspeed) {
/* spindle is still not at the right speed: wait */
return 0;
} else {
waiting_for_atspeed = 0;
}
}
if(tc->active == 0) {
// this means this tc is being read for the first time.
// wait for atspeed, if motion requested it. also, force
// atspeed check for the start of all spindle synchronized
// moves.
if((tc->atspeed || (tc->synchronized && !tc->velocity_mode && !emcmotStatus->spindleSync)) &&
!emcmotStatus->spindle_is_atspeed) {
waiting_for_atspeed = tc->id;
return 0;
}
if (tc->indexrotary != -1) {
// request that the axis unlock
tpSetRotaryUnlock(tc->indexrotary, 1);
// if it is unlocked, fall through and start the move.
// otherwise, just come back later and check again
if (!tpGetRotaryIsUnlocked(tc->indexrotary))
return 0;
}
tc->active = 1;
tc->currentvel = 0;
tp->depth = tp->activeDepth = 1;
tp->motionType = tc->canon_motion_type;
tc->blending = 0;
// honor accel constraint in case we happen to make an acute angle
// with the next segment.
if(tc->blend_with_next)
tc->maxaccel /= 2.0;
if(tc->synchronized) {
if(!tc->velocity_mode && !emcmotStatus->spindleSync) {
// if we aren't already synced, wait
waiting_for_index = tc->id;
// ask for an index reset
emcmotStatus->spindle_index_enable = 1;
spindleoffset = 0.0;
// don't move: wait
return 0;
}
}
}
if(waiting_for_index) {
if(emcmotStatus->spindle_index_enable) {
/* haven't passed index yet */
return 0;
} else {
/* passed index, start the move */
emcmotStatus->spindleSync = 1;
waiting_for_index=0;
tc->sync_accel=1;
revs=0;
}
}
if (tc->motion_type == TC_RIGIDTAP) {
static double old_spindlepos;
double new_spindlepos = emcmotStatus->spindleRevs;
if (emcmotStatus->spindle.direction < 0) new_spindlepos = -new_spindlepos;
switch (tc->coords.rigidtap.state) {
case TAPPING:
if (tc->progress >= tc->coords.rigidtap.reversal_target) {
// command reversal
emcmotStatus->spindle.speed *= -1;
tc->coords.rigidtap.state = REVERSING;
}
break;
case REVERSING:
if (new_spindlepos < old_spindlepos) {
PmPose start, end;
PmLine *aux = &tc->coords.rigidtap.aux_xyz;
// we've stopped, so set a new target at the original position
tc->coords.rigidtap.spindlerevs_at_reversal = new_spindlepos + spindleoffset;
pmLinePoint(&tc->coords.rigidtap.xyz, tc->progress, &start);
end = tc->coords.rigidtap.xyz.start;
pmLineInit(aux, start, end);
tc->coords.rigidtap.reversal_target = aux->tmag;
tc->target = aux->tmag + 10. * tc->uu_per_rev;
tc->progress = 0.0;
tc->coords.rigidtap.state = RETRACTION;
}
old_spindlepos = new_spindlepos;
break;
case RETRACTION:
if (tc->progress >= tc->coords.rigidtap.reversal_target) {
emcmotStatus->spindle.speed *= -1;
tc->coords.rigidtap.state = FINAL_REVERSAL;
}
break;
case FINAL_REVERSAL:
if (new_spindlepos > old_spindlepos) {
PmPose start, end;
PmLine *aux = &tc->coords.rigidtap.aux_xyz;
pmLinePoint(aux, tc->progress, &start);
end = tc->coords.rigidtap.xyz.start;
pmLineInit(aux, start, end);
tc->target = aux->tmag;
tc->progress = 0.0;
tc->synchronized = 0;
tc->reqvel = tc->maxvel;
tc->coords.rigidtap.state = FINAL_PLACEMENT;
}
old_spindlepos = new_spindlepos;
break;
case FINAL_PLACEMENT:
// this is a regular move now, it'll stop at target above.
break;
}
}
if(!tc->synchronized) emcmotStatus->spindleSync = 0;
if(nexttc && nexttc->active == 0) {
// this means this tc is being read for the first time.
nexttc->currentvel = 0;
tp->depth = tp->activeDepth = 1;
nexttc->active = 1;
nexttc->blending = 0;
// honor accel constraint if we happen to make an acute angle with the
// above segment or the following one
if(tc->blend_with_next || nexttc->blend_with_next)
nexttc->maxaccel /= 2.0;
}
if(tc->synchronized) {
double pos_error;
double oldrevs = revs;
if(tc->velocity_mode) {
pos_error = fabs(emcmotStatus->spindleSpeedIn) * tc->uu_per_rev;
if(nexttc) pos_error -= nexttc->progress; /* ?? */
if(!tp->aborting) {
tc->feed_override = emcmotStatus->net_feed_scale;
tc->reqvel = pos_error;
}
} else {
double spindle_vel, target_vel;
double new_spindlepos = emcmotStatus->spindleRevs;
if (emcmotStatus->spindle.direction < 0) new_spindlepos = -new_spindlepos;
if(tc->motion_type == TC_RIGIDTAP &&
(tc->coords.rigidtap.state == RETRACTION ||
tc->coords.rigidtap.state == FINAL_REVERSAL))
revs = tc->coords.rigidtap.spindlerevs_at_reversal -
new_spindlepos;
else
revs = new_spindlepos;
pos_error = (revs - spindleoffset) * tc->uu_per_rev - tc->progress;
if(nexttc) pos_error -= nexttc->progress;
if(tc->sync_accel) {
// detect when velocities match, and move the target accordingly.
// acceleration will abruptly stop and we will be on our new target.
spindle_vel = revs/(tc->cycle_time * tc->sync_accel++);
target_vel = spindle_vel * tc->uu_per_rev;
if(tc->currentvel >= target_vel) {
// move target so as to drive pos_error to 0 next cycle
spindleoffset = revs - tc->progress/tc->uu_per_rev;
tc->sync_accel = 0;
tc->reqvel = target_vel;
} else {
// beginning of move and we are behind: accel as fast as we can
tc->reqvel = tc->maxvel;
}
} else {
// we have synced the beginning of the move as best we can -
// track position (minimize pos_error).
double errorvel;
spindle_vel = (revs - oldrevs) / tc->cycle_time;
target_vel = spindle_vel * tc->uu_per_rev;
errorvel = pmSqrt(fabs(pos_error) * tc->maxaccel);
if(pos_error<0) errorvel = -errorvel;
tc->reqvel = target_vel + errorvel;
}
tc->feed_override = 1.0;
}
if(tc->reqvel < 0.0) tc->reqvel = 0.0;
if(nexttc) {
if (nexttc->synchronized) {
nexttc->reqvel = tc->reqvel;
nexttc->feed_override = 1.0;
if(nexttc->reqvel < 0.0) nexttc->reqvel = 0.0;
} else {
nexttc->feed_override = emcmotStatus->net_feed_scale;
}
}
} else {
tc->feed_override = emcmotStatus->net_feed_scale;
if(nexttc) {
nexttc->feed_override = emcmotStatus->net_feed_scale;
}
}
/* handle pausing */
if(tp->pausing && (!tc->synchronized || tc->velocity_mode)) {
tc->feed_override = 0.0;
if(nexttc) {
nexttc->feed_override = 0.0;
}
}
// calculate the approximate peak velocity the nexttc will hit.
// we know to start blending it in when the current tc goes below
// this velocity...
if(nexttc && nexttc->maxaccel) {
tc->blend_vel = nexttc->maxaccel *
pmSqrt(nexttc->target / nexttc->maxaccel);
if(tc->blend_vel > nexttc->reqvel * nexttc->feed_override) {
// segment has a cruise phase so let's blend over the
// whole accel period if possible
tc->blend_vel = nexttc->reqvel * nexttc->feed_override;
}
if(tc->maxaccel < nexttc->maxaccel)
tc->blend_vel *= tc->maxaccel/nexttc->maxaccel;
if(tc->tolerance) {
/* see diagram blend.fig. T (blend tolerance) is given, theta
* is calculated from dot(s1,s2)
*
* blend criteria: we are decelerating at the end of segment s1
* and we pass distance d from the end.
* find the corresponding velocity v when passing d.
*
* in the drawing note d = 2T/cos(theta)
*
* when v1 is decelerating at a to stop, v = at, t = v/a
* so required d = .5 a (v/a)^2
*
* equate the two expressions for d and solve for v
*/
double tblend_vel;
double dot;
double theta;
PmCartesian v1, v2;
v1 = tcGetEndingUnitVector(tc);
v2 = tcGetStartingUnitVector(nexttc);
pmCartCartDot(v1, v2, &dot);
theta = acos(-dot)/2.0;
if(cos(theta) > 0.001) {
tblend_vel = 2.0 * pmSqrt(tc->maxaccel * tc->tolerance / cos(theta));
if(tblend_vel < tc->blend_vel)
tc->blend_vel = tblend_vel;
}
}
}
primary_before = tcGetPos(tc);
tcRunCycle(tp, tc, &primary_vel, &on_final_decel);
primary_after = tcGetPos(tc);
pmCartCartSub(primary_after.tran, primary_before.tran,
&primary_displacement.tran);
primary_displacement.a = primary_after.a - primary_before.a;
primary_displacement.b = primary_after.b - primary_before.b;
primary_displacement.c = primary_after.c - primary_before.c;
primary_displacement.u = primary_after.u - primary_before.u;
primary_displacement.v = primary_after.v - primary_before.v;
primary_displacement.w = primary_after.w - primary_before.w;
// blend criteria
if((tc->blending && nexttc) ||
(nexttc && on_final_decel && primary_vel < tc->blend_vel)) {
// make sure we continue to blend this segment even when its
// accel reaches 0 (at the very end)
tc->blending = 1;
// hack to show blends in axis
// tp->motionType = 0;
if(tc->currentvel > nexttc->currentvel) {
target = tcGetEndpoint(tc);
tp->motionType = tc->canon_motion_type;
emcmotStatus->distance_to_go = tc->target - tc->progress;
emcmotStatus->enables_queued = tc->enables;
// report our line number to the guis
tp->execId = tc->id;
emcmotStatus->requested_vel = tc->reqvel;
} else {
tpToggleDIOs(nexttc); //check and do DIO changes
target = tcGetEndpoint(nexttc);
tp->motionType = nexttc->canon_motion_type;
emcmotStatus->distance_to_go = nexttc->target - nexttc->progress;
emcmotStatus->enables_queued = nexttc->enables;
// report our line number to the guis
tp->execId = nexttc->id;
emcmotStatus->requested_vel = nexttc->reqvel;
}
emcmotStatus->current_vel = tc->currentvel + nexttc->currentvel;
secondary_before = tcGetPos(nexttc);
save_vel = nexttc->reqvel;
nexttc->reqvel = nexttc->feed_override > 0.0 ?
((tc->vel_at_blend_start - primary_vel) / nexttc->feed_override) :
0.0;
tcRunCycle(tp, nexttc, NULL, NULL);
nexttc->reqvel = save_vel;
secondary_after = tcGetPos(nexttc);
pmCartCartSub(secondary_after.tran, secondary_before.tran,
&secondary_displacement.tran);
secondary_displacement.a = secondary_after.a - secondary_before.a;
secondary_displacement.b = secondary_after.b - secondary_before.b;
secondary_displacement.c = secondary_after.c - secondary_before.c;
secondary_displacement.u = secondary_after.u - secondary_before.u;
secondary_displacement.v = secondary_after.v - secondary_before.v;
secondary_displacement.w = secondary_after.w - secondary_before.w;
pmCartCartAdd(tp->currentPos.tran, primary_displacement.tran,
&tp->currentPos.tran);
pmCartCartAdd(tp->currentPos.tran, secondary_displacement.tran,
&tp->currentPos.tran);
tp->currentPos.a += primary_displacement.a + secondary_displacement.a;
tp->currentPos.b += primary_displacement.b + secondary_displacement.b;
tp->currentPos.c += primary_displacement.c + secondary_displacement.c;
tp->currentPos.u += primary_displacement.u + secondary_displacement.u;
tp->currentPos.v += primary_displacement.v + secondary_displacement.v;
tp->currentPos.w += primary_displacement.w + secondary_displacement.w;
} else {
tpToggleDIOs(tc); //check and do DIO changes
target = tcGetEndpoint(tc);
tp->motionType = tc->canon_motion_type;
emcmotStatus->distance_to_go = tc->target - tc->progress;
tp->currentPos = primary_after;
emcmotStatus->current_vel = tc->currentvel;
emcmotStatus->requested_vel = tc->reqvel;
emcmotStatus->enables_queued = tc->enables;
// report our line number to the guis
tp->execId = tc->id;
}
emcmotStatus->dtg.tran.x = target.tran.x - tp->currentPos.tran.x;
emcmotStatus->dtg.tran.y = target.tran.y - tp->currentPos.tran.y;
emcmotStatus->dtg.tran.z = target.tran.z - tp->currentPos.tran.z;
emcmotStatus->dtg.a = target.a - tp->currentPos.a;
emcmotStatus->dtg.b = target.b - tp->currentPos.b;
emcmotStatus->dtg.c = target.c - tp->currentPos.c;
emcmotStatus->dtg.u = target.u - tp->currentPos.u;
emcmotStatus->dtg.v = target.v - tp->currentPos.v;
emcmotStatus->dtg.w = target.w - tp->currentPos.w;
return 0;
}
int tpAddRigidTap(TP_STRUCT *tp, EmcPose end, double vel, double ini_maxvel,
double acc, unsigned char enables) {
TC_STRUCT tc;
PmLine line_xyz;
PmPose start_xyz, end_xyz;
PmCartesian abc, uvw;
PmQuaternion identity_quat = { 1.0, 0.0, 0.0, 0.0 };
if (!tp) {
rtapi_print_msg(RTAPI_MSG_ERR, "TP is null\n");
return -1;
}
if (tp->aborting) {
rtapi_print_msg(RTAPI_MSG_ERR, "TP is aborting\n");
return -1;
}
start_xyz.tran = tp->goalPos.tran;
end_xyz.tran = end.tran;
start_xyz.rot = identity_quat;
end_xyz.rot = identity_quat;
// abc cannot move
abc.x = tp->goalPos.a;
abc.y = tp->goalPos.b;
abc.z = tp->goalPos.c;
uvw.x = tp->goalPos.u;
uvw.y = tp->goalPos.v;
uvw.z = tp->goalPos.w;
pmLineInit(&line_xyz, start_xyz, end_xyz);
tc.sync_accel = 0;
tc.cycle_time = tp->cycleTime;
tc.coords.rigidtap.reversal_target = line_xyz.tmag;
// allow 10 turns of the spindle to stop - we don't want to just go on forever
tc.target = line_xyz.tmag + 10. * tp->uu_per_rev;
tc.progress = 0.0;
tc.reqvel = vel;
tc.maxaccel = acc;
tc.feed_override = 0.0;
tc.maxvel = ini_maxvel;
tc.id = tp->nextId;
tc.active = 0;
tc.atspeed = 1;
tc.currentvel = 0.0;
tc.blending = 0;
tc.blend_vel = 0.0;
tc.vel_at_blend_start = 0.0;
tc.coords.rigidtap.xyz = line_xyz;
tc.coords.rigidtap.abc = abc;
tc.coords.rigidtap.uvw = uvw;
tc.coords.rigidtap.state = TAPPING;
tc.motion_type = TC_RIGIDTAP;
tc.canon_motion_type = 0;
tc.blend_with_next = 0;
tc.tolerance = tp->tolerance;
if(!tp->synchronized) {
rtapi_print_msg(RTAPI_MSG_ERR, "Cannot add unsynchronized rigid tap move.\n");
return -1;
}
tc.synchronized = tp->synchronized;
tc.uu_per_rev = tp->uu_per_rev;
tc.velocity_mode = tp->velocity_mode;
tc.enables = enables;
tc.indexrotary = -1;
if (syncdio.anychanged != 0) {
tc.syncdio = syncdio; //enqueue the list of DIOs that need toggling
tpClearDIOs(); // clear out the list, in order to prepare for the next time we need to use it
} else {
tc.syncdio.anychanged = 0;
}
if (tcqPut(&tp->queue, tc) == -1) {
rtapi_print_msg(RTAPI_MSG_ERR, "tcqPut failed.\n");
return -1;
}
// do not change tp->goalPos here,
// since this move will end just where it started
tp->done = 0;
tp->depth = tcqLen(&tp->queue);
tp->nextId++;
return 0;
}
int rtapi_app_main(void){
int i, f, f1, k, p;
int retval;
if (!fmt_strings[0]){
rtapi_print_msg(RTAPI_MSG_ERR, "The LCD component requires at least one valid format string");
return -EINVAL;
}
comp_id = hal_init("lcd");
if (comp_id < 0) {
rtapi_print_msg(RTAPI_MSG_ERR, "LCD: ERROR: hal_init() failed\n");
return -1;
}
// allocate shared memory for data
lcd = hal_malloc(sizeof(lcd_t));
if (lcd == 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"lcd component: Out of Memory\n");
hal_exit(comp_id);
return -1;
}
// Count the instances. Very unlikely to be more than one, but...
for (lcd->num_insts = 0; fmt_strings[lcd->num_insts];lcd->num_insts++){}
lcd->insts = hal_malloc(lcd->num_insts * sizeof(lcd_inst_t));
for (i = 0; i < lcd->num_insts; i++){
lcd_inst_t *inst = &lcd->insts[i];
inst->num_pages = 1;
// count the pages demarked by | chars.
for (f = 0; fmt_strings[i][f]; f++){
if (fmt_strings[i][f] =='|') inst->num_pages++;
}
inst->pages = hal_malloc(inst->num_pages * sizeof(lcd_page_t));
//second pass
f1 = k = p = 0;
for (f = 0; fmt_strings[i][f]; f++){
if (fmt_strings[i][f] =='%') {
int type = parse_fmt(fmt_strings[i], &f, NULL, NULL, 0);
if (type > 0) {
inst->pages[p].num_args++;
}
}
if (fmt_strings[i][f + 1] =='|' || fmt_strings[i][f + 1] == 0) {
inst->pages[p].fmt = hal_malloc(f - f1 + 2);
retval = rtapi_snprintf(inst->pages[p].fmt,
f - f1 + 2, "%s",
fmt_strings[i] + f1);
inst->pages[p].length = f - f1 + 2;
inst->pages[p].args = hal_malloc(inst->pages[p].num_args
* sizeof(hal_float_t));
f1 = f + 2;
{
int a = -1, s = -1;
lcd_page_t page = inst->pages[p];
while (page.fmt[++s]){
if (page.fmt[s] == '%'){
int type = parse_fmt(page.fmt, &s, NULL, NULL, 0);
a++;
switch (type){
case 'f':
retval = hal_pin_float_newf(HAL_IN,
(hal_float_t**)&(inst->pages[p].args[a]), comp_id,
"lcd.%02i.page.%02i.arg.%02i",
i, p, a);
if (retval != 0) {
return retval;
}
break;
case 'u':
case 'c':
retval = hal_pin_u32_newf(HAL_IN,
(hal_u32_t **)&(inst->pages[p].args[a]), comp_id,
"lcd.%02i.page.%02i.arg.%02i",
i, p, a);
if (retval != 0) {
return retval;
}
break;
case 's':
retval = hal_pin_s32_newf(HAL_IN,
(hal_s32_t **)&(inst->pages[p].args[a]), comp_id,
"lcd.%02i.page.%02i.arg.%02i",
i, p, a);
if (retval != 0) {
return retval;
}
break;
case 'b':
retval = hal_pin_bit_newf(HAL_IN,
(hal_bit_t **)&(inst->pages[p].args[a]), comp_id,
"lcd.%02i.page.%02i.arg.%02i",
i, p, a);
if (retval != 0) {
return retval;
}
break;
}
}
}
}
p++; // increment the page index
}
}
}
retval = hal_export_funct("lcd", write, lcd, 1, 0, comp_id); //needs fp?
if (retval < 0) {
rtapi_print_msg(RTAPI_MSG_ERR, "LCD: ERROR: function export failed\n");
return -1;
}
for (i = 0; i < lcd->num_insts; i++){
retval = hal_pin_u32_newf(HAL_IN, &(lcd->insts[i].page_num), comp_id,
"lcd.%02i.page_num", i);
if (retval != 0) {
return retval;
}
lcd->insts[i].last_page = 0xFFFF; // force screen refresh
retval = hal_pin_u32_newf(HAL_OUT, &(lcd->insts[i].out), comp_id,
"lcd.%02i.out",i);
if (retval != 0) {
return retval;
}
retval = hal_pin_float_newf(HAL_IN, &(lcd->insts[i].contrast), comp_id,
"lcd.%02i.contrast",i);
if (retval != 0) {
return retval;
}
retval = hal_param_u32_newf(HAL_RW, &(lcd->insts[i].dp), comp_id,
"lcd.%02i.decimal-separator",i);
lcd->insts[i].dp = '.';
if (retval != 0) {
return retval;
}
lcd->insts[i].f_ptr = 0;
lcd->insts[i].buff[0] = 0x11; // turn off cursor. More init can be added
lcd->insts[i].buff[0] = 0;
lcd->insts[i].c_ptr = 0;
}
return 0;
}
int rtapi_app_main(void)
{
int n, retval = 0;
int rev, ncores, pinno;
char *endptr;
if ((rev = get_rpi_revision()) < 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"unrecognized Raspberry revision, see /proc/cpuinfo\n");
return -EINVAL;
}
ncores = number_of_cores();
rtapi_print_msg(RTAPI_MSG_INFO, "%d cores rev %d", ncores, rev);
switch (rev) {
case 3:
rtapi_print_msg(RTAPI_MSG_INFO, "Raspberry2\n");
pins = rpi2_pins;
gpios = rpi2_gpios;
npins = sizeof(rpi2_pins);
break;
case 1:
rtapi_print_msg(RTAPI_MSG_INFO, "Raspberry1 rev 1.0\n");
pins = rev1_pins;
gpios = rev1_gpios;
npins = sizeof(rev1_pins);
break;
case 2:
rtapi_print_msg(RTAPI_MSG_INFO, "Raspberry1 Rev 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 -EINVAL;
}
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(rev, ncores))
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;
}
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 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 {
howmany = 0;
for (i = 0; i < MAX_CHAN; i++) {
if (names[i] == NULL) {
break;
}
howmany = i + 1;
}
}
/* test for number of channels */
if ((howmany <= 0) || (howmany > MAX_CHAN)) {
rtapi_print_msg(RTAPI_MSG_ERR,
"SIGGEN: ERROR: invalid number of channels: %d\n", howmany);
return -1;
}
/* have good config info, connect to the HAL */
comp_id = hal_init("siggen");
if (comp_id < 0) {
rtapi_print_msg(RTAPI_MSG_ERR, "SIGGEN: ERROR: hal_init() failed\n");
return -1;
}
/* allocate shared memory for siggen data */
siggen_array = hal_malloc(howmany * sizeof(hal_siggen_t));
if (siggen_array == 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"SIGGEN: ERROR: hal_malloc() failed\n");
hal_exit(comp_id);
return -1;
}
/* export variables and functions for each siggen */
i = 0; // for names= items
for (n = 0; n < howmany; n++) {
/* export everything for this loop */
if(num_chan) {
char buf[HAL_NAME_LEN + 1];
rtapi_snprintf(buf, sizeof(buf), "siggen.%d", n);
retval = export_siggen(n, &(siggen_array[n]),buf);
} else {
retval = export_siggen(n, &(siggen_array[n]),names[i++]);
}
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"SIGGEN: ERROR: siggen %d var export failed\n", n);
hal_exit(comp_id);
return -1;
}
}
rtapi_print_msg(RTAPI_MSG_INFO,
"SIGGEN: installed %d signal generators\n", howmany);
hal_ready(comp_id);
return 0;
}
void _rtapi_wait_hook() {
unsigned long overruns = 0;
int result = rt_task_wait_period(&overruns);
if (result) {
// something went wrong:
// update stats counters in thread status
int task_id = _rtapi_task_update_stats_hook();
// paranoid, but you never know; this index off and
// things will go haywire really fast
if ((task_id < 0) || (task_id > RTAPI_MAX_TASKS)) {
rtapi_print_msg(RTAPI_MSG_ERR,
"rt_task_wait_hook: BUG - task_id out of range: %d\n",
task_id);
// maybe should call a BUG exception here
return;
}
// inquire, fill in
// exception descriptor, and call exception handler
rtapi_exception_detail_t detail = {0};
rtapi_threadstatus_t *ts = &global_data->thread_status[task_id];
rtapi_exception_t type;
// exception descriptor
detail.task_id = task_id;
detail.error_code = result;
switch (result) {
case -ETIMEDOUT:
// release point was missed
detail.flavor.xeno.overruns = overruns;
// update thread status in global_data
ts->flavor.xeno.wait_errors++;
ts->flavor.xeno.total_overruns += overruns;
type = XU_ETIMEDOUT;
break;
case -EWOULDBLOCK:
// returned if rt_task_set_periodic() has not previously
// been called for the calling task. This is clearly
// a Xenomai API usage error.
ts->api_errors++;
type = XU_EWOULDBLOCK;
break;
case -EINTR:
// returned if rt_task_unblock() has been called for
// the waiting task before the next periodic release
// point has been reached. In this case, the overrun
// counter is reset too.
// a Xenomai API usage error.
ts->api_errors++;
type = XU_EINTR;
break;
case -EPERM:
// returned if this service was called from a
// context which cannot sleep (e.g. interrupt,
// non-realtime or scheduler locked).
// a Xenomai API usage error.
ts->api_errors++;
type = XU_EPERM;
break;
default:
// the above should handle all possible returns
// as per manual, so at least leave a scent
// (or what Jeff calls a 'canary value')
ts->other_errors++;
type = XU_UNDOCUMENTED;
}
if (rt_exception_handler)
rt_exception_handler(type, &detail, ts);
} // else: ok - no overruns;
}
static int export_siggen(int num, hal_siggen_t * addr,char* prefix)
{
int retval;
char buf[HAL_NAME_LEN + 1];
/* export pins */
retval = hal_pin_float_newf(HAL_OUT, &(addr->square), comp_id,
"%s.square", prefix);
if (retval != 0) {
return retval;
}
retval = hal_pin_float_newf(HAL_OUT, &(addr->sawtooth), comp_id,
"%s.sawtooth", prefix);
if (retval != 0) {
return retval;
}
retval = hal_pin_float_newf(HAL_OUT, &(addr->triangle), comp_id,
"%s.triangle", prefix);
if (retval != 0) {
return retval;
}
retval = hal_pin_float_newf(HAL_OUT, &(addr->sine), comp_id,
"%s.sine", prefix);
if (retval != 0) {
return retval;
}
retval = hal_pin_float_newf(HAL_OUT, &(addr->cosine), comp_id,
"%s.cosine", prefix);
if (retval != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_OUT, &(addr->clock), comp_id,
"%s.clock", prefix);
if (retval != 0) {
return retval;
}
retval = hal_pin_float_newf(HAL_IN, &(addr->frequency), comp_id,
"%s.frequency", prefix);
if (retval != 0) {
return retval;
}
retval = hal_pin_float_newf(HAL_IN, &(addr->amplitude), comp_id,
"%s.amplitude", prefix);
if (retval != 0) {
return retval;
}
retval = hal_pin_float_newf(HAL_IN, &(addr->offset), comp_id,
"%s.offset", prefix);
if (retval != 0) {
return retval;
}
/* init all structure members */
*(addr->square) = 0.0;
*(addr->sawtooth) = 0.0;
*(addr->triangle) = 0.0;
*(addr->sine) = 0.0;
*(addr->cosine) = 0.0;
*(addr->clock) = 0;
*(addr->frequency) = 1.0;
*(addr->amplitude) = 1.0;
*(addr->offset) = 0.0;
addr->index = 0.0;
/* export function for this loop */
rtapi_snprintf(buf, sizeof(buf), "%s.update", prefix);
retval =
hal_export_funct(buf, calc_siggen, &(siggen_array[num]), 1, 0,
comp_id);
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"SIGGEN: ERROR: update funct export failed\n");
hal_exit(comp_id);
return -1;
}
return 0;
}
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;
}
static int init_comm_buffers(void) {
int joint_num, n;
emcmot_joint_t *joint;
int retval;
int shmem_id;
rtapi_print_msg(RTAPI_MSG_INFO,
"MOTION: init_comm_buffers() starting...\n");
emcmotStruct = 0;
emcmotDebug = 0;
emcmotStatus = 0;
c = 0;
emcmotConfig = 0;
/* allocate and initialize the shared memory structure */
shmem_id = rtapi_shmem_new(DEFAULT_SHMEM_KEY, mot_comp_id, sizeof(emcmot_struct_t));
if (shmem_id < 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"MOTION: rtapi_shmem_new failed, returned %d\n", shmem_id);
return -1;
}
retval = rtapi_shmem_getptr(shmem_id, (void **) &emcmotStruct);
if (retval < 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"MOTION: rtapi_shmem_getptr failed, returned %d\n", retval);
return -1;
}
/* zero shared memory before doing anything else. */
memset(emcmotStruct, 0, sizeof(emcmot_struct_t));
/* we'll reference emcmotStruct directly */
c = &emcmotStruct->command;
emcmotStatus = &emcmotStruct->status;
emcmotConfig = &emcmotStruct->config;
emcmotDebug = &emcmotStruct->debug;
emcmotInternal = &emcmotStruct->internal;
emcmotError = &emcmotStruct->error;
emcmotConfig->numJoints = num_joints;
emcmotStatus->vel = DEFAULT_VELOCITY;
emcmotConfig->limitVel = DEFAULT_VELOCITY;
emcmotStatus->acc = DEFAULT_ACCELERATION;
emcmotStatus->feed_scale = 1.0;
emcmotStatus->rapid_scale = 1.0;
emcmotStatus->spindle_scale = 1.0;
emcmotStatus->net_feed_scale = 1.0;
/* adaptive feed is off by default, feed override, spindle
override, and feed hold are on */
emcmotStatus->enables_new = FS_ENABLED | SS_ENABLED | FH_ENABLED;
emcmotStatus->enables_queued = emcmotStatus->enables_new;
SET_MOTION_INPOS_FLAG(1);
emcmotConfig->kinematics_type = KINEMATICS_IDENTITY;
emcmot_config_change();
/* init pointer to joint structs */
joints = joint_array;
/* init per-joint stuff */
for (joint_num = 0; joint_num < num_joints; joint_num++) {
/* point to structure for this joint */
joint = &joints[joint_num];
/* init the config fields with some "reasonable" defaults" */
joint->max_pos_limit = 1.0;
joint->min_pos_limit = -1.0;
joint->vel_limit = 1.0;
joint->acc_limit = 1.0;
joint->min_ferror = 0.01;
joint->max_ferror = 1.0;
joint->home_final_vel = -1;
joint->home_sequence = -1;
joint->comp.entry = &(joint->comp.array[0]);
/* the compensation code has -DBL_MAX at one end of the table
and +DBL_MAX at the other so _all_ commanded positions are
guaranteed to be covered by the table */
joint->comp.array[0].nominal = -DBL_MAX;
joint->comp.array[0].fwd_trim = 0.0;
joint->comp.array[0].rev_trim = 0.0;
joint->comp.array[0].fwd_slope = 0.0;
joint->comp.array[0].rev_slope = 0.0;
for ( n = 1 ; n < EMCMOT_COMP_SIZE+2 ; n++ ) {
joint->comp.array[n].nominal = DBL_MAX;
joint->comp.array[n].fwd_trim = 0.0;
joint->comp.array[n].rev_trim = 0.0;
joint->comp.array[n].fwd_slope = 0.0;
joint->comp.array[n].rev_slope = 0.0;
}
/* init status info */
joint->ferror_limit = joint->min_ferror;
/* init misc other stuff in joint structure */
joint->big_vel = 10.0 * joint->vel_limit;
SET_JOINT_INPOS_FLAG(joint, 1);
}
emcmotDebug->start_time = time(NULL);
rtapi_print_msg(RTAPI_MSG_INFO, "MOTION: init_comm_buffers() complete\n");
return 0;
}
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 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;
/* 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_task_new(void (*taskcode) (void*), void *arg,
int prio, int owner, unsigned long int stacksize,
int uses_fp, char *name, int cpu_id) {
int task_id;
int retval = 0;
task_data *task;
/* get the mutex */
rtapi_mutex_get(&(rtapi_data->mutex));
#ifdef MODULE
/* validate owner */
if ((owner < 1) || (owner > RTAPI_MAX_MODULES)) {
rtapi_mutex_give(&(rtapi_data->mutex));
return -EINVAL;
}
if (module_array[owner].state != REALTIME) {
rtapi_mutex_give(&(rtapi_data->mutex));
return -EINVAL;
}
#endif
/* find an empty entry in the task array */
task_id = 1; // tasks start at one!
// go through task_array until an empty task slot is found
while ((task_id < RTAPI_MAX_TASKS) &&
(task_array[task_id].magic == TASK_MAGIC))
task_id++;
// if task_array is full, release lock and return error
if (task_id == RTAPI_MAX_TASKS) {
rtapi_mutex_give(&(rtapi_data->mutex));
return -ENOMEM;
}
task = &(task_array[task_id]);
// if requested priority is invalid, release lock and return error
if (PRIO_LT(prio,_rtapi_prio_lowest()) ||
PRIO_GT(prio,_rtapi_prio_highest())) {
rtapi_print_msg(RTAPI_MSG_ERR,
"New task %d '%s:%d': invalid priority %d "
"(highest=%d lowest=%d)\n",
task_id, name, rtapi_instance, prio,
_rtapi_prio_highest(),
_rtapi_prio_lowest());
rtapi_mutex_give(&(rtapi_data->mutex));
return -EINVAL;
}
// task slot found; reserve it and release lock
rtapi_print_msg(RTAPI_MSG_DBG,
"Creating new task %d '%s:%d': "
"req prio %d (highest=%d lowest=%d) stack=%lu\n",
task_id, name, rtapi_instance, prio,
_rtapi_prio_highest(),
_rtapi_prio_lowest(),
stacksize);
task->magic = TASK_MAGIC;
/* fill out task structure */
if(stacksize < MIN_STACKSIZE) stacksize = MIN_STACKSIZE;
task->owner = owner;
task->arg = arg;
task->stacksize = stacksize;
task->taskcode = taskcode;
task->prio = prio;
task->uses_fp = uses_fp;
/* hopefully this works for userland thread systems too */
task->cpu = cpu_id > -1 ? cpu_id : rtapi_data->rt_cpu;
rtapi_print_msg(RTAPI_MSG_DBG,
"Task CPU: %d\n", task->cpu);
/* task->cpu = cpu_id; */
rtapi_snprintf(task->name, sizeof(task->name),
"%s:%d", name, rtapi_instance);
task->name[sizeof(task->name) - 1] = '\0';
#ifdef MODULE
/* get space for the OS's task data - this is around 900 bytes, */
/* so we don't want to statically allocate it for unused tasks. */
ostask_array[task_id] = kmalloc(sizeof(RT_TASK), GFP_USER);
if (ostask_array[task_id] == NULL) {
rtapi_mutex_give(&(rtapi_data->mutex));
return -ENOMEM;
}
#ifdef HAVE_RTAPI_TASK_NEW_HOOK
/* kernel threads: rtapi_task_new_hook() should call OS to
initialize the task - use predetermined or explicitly assigned
CPU */
retval = _rtapi_task_new_hook(task, task_id);
if (retval) {
rtapi_print_msg(RTAPI_MSG_ERR,
"rt_task_create failed, rc = %d\n", retval );
/* couldn't create task, free task data memory */
kfree(ostask_array[task_id]);
rtapi_mutex_give(&(rtapi_data->mutex));
if (retval == ENOMEM) {
/* not enough space for stack */
return -ENOMEM;
}
/* unknown error */
return -EINVAL;
}
#endif
/* the task has been created, update data */
task->state = PAUSED;
retval = task_id;
#else /* userland thread */
/* userland threads: rtapi_task_new_hook() should perform any
thread system-specific tasks, and return task_id or an error
code back to the caller (how do we know the diff between an
error and a task_id???). */
task->state = USERLAND; // userland threads don't track this
# ifdef HAVE_RTAPI_TASK_NEW_HOOK
retval = _rtapi_task_new_hook(task,task_id);
# else
retval = task_id;
# endif
#endif /* userland thread */
rtapi_data->task_count++;
rtapi_mutex_give(&(rtapi_data->mutex));
/* announce the birth of a brand new baby task */
rtapi_print_msg(RTAPI_MSG_DBG,
"RTAPI: task %02d installed by module %02d, priority %d, code: %p\n",
task_id, task->owner, task->prio, taskcode);
return task_id;
}
int _rtapi_shmem_new_inst(int userkey, int instance, int module_id, unsigned long int size) {
int n, retval;
int shmem_id;
shmem_data *shmem;
struct shm_status sm;
int key = OS_KEY(userkey, instance);
/* key must be non-zero, and also cannot match the key that RTAPI uses */
if ((key == 0) || (key == OS_KEY(RTAPI_KEY, instance))) {
rtapi_print_msg(RTAPI_MSG_ERR, "RTAPI: ERROR: bad shmem key: %d\n",
key);
return -EINVAL;
}
/* get the mutex */
rtapi_mutex_get(&(rtapi_data->mutex));
/* validate module_id */
if ((module_id < 1) || (module_id > RTAPI_MAX_MODULES)) {
rtapi_mutex_give(&(rtapi_data->mutex));
rtapi_print_msg(RTAPI_MSG_ERR, "RTAPI: ERROR: bad module ID: %d\n",
module_id);
return -EINVAL;
}
if (module_array[module_id].state != MODULE_STATE) {
rtapi_print_msg(RTAPI_MSG_ERR,
"RTAPI: ERROR: not a " OUR_API " module ID: %d\n",
module_id);
rtapi_mutex_give(&(rtapi_data->mutex));
return -EINVAL;
}
/* check if a block is already open for this key */
for (n = 1; n <= RTAPI_MAX_SHMEMS; n++) {
if (shmem_array[n].key == key) {
/* found a match */
shmem_id = n;
shmem = &(shmem_array[n]);
/* is it big enough? */
if (shmem->size < size) {
rtapi_mutex_give(&(rtapi_data->mutex));
rtapi_print_msg(RTAPI_MSG_ERR,
"RTAPI: ERROR: shmem size mismatch\n");
return -EINVAL;
}
/* is this module already using it? */
if (rtapi_test_bit(module_id, shmem->bitmap)) {
rtapi_mutex_give(&(rtapi_data->mutex));
rtapi_print_msg(RTAPI_MSG_WARN,
"RTAPI: Warning: shmem already mapped\n");
return -EEXIST;
}
/* yes, has it been mapped into kernel space? */
#ifdef RTAPI
if (shmem->rtusers == 0) {
#endif
/* no, map it and save the address */
sm.key = key;
sm.size = size;
sm.flags = 0;
#ifdef ULAPI
sm.driver_fd = shmdrv_driver_fd();
#endif
retval = shmdrv_attach(&sm, &shmem_addr_array[shmem_id]);
if (retval < 0) {
rtapi_mutex_give(&(rtapi_data->mutex));
rtapi_print_msg(RTAPI_MSG_ERR,
"shmdrv attached failed key=0x%x size=%ld\n", key, size);
return retval;
}
if (shmem_addr_array[shmem_id] == NULL) {
rtapi_print_msg(RTAPI_MSG_ERR,
"RTAPI: ERROR: failed to map shmem\n");
rtapi_mutex_give(&(rtapi_data->mutex));
#ifdef ULAPI
check_memlock_limit("failed to map shmem");
#endif
return -ENOMEM;
}
#ifdef RTAPI
}
#endif
/* update usage data */
rtapi_set_bit(module_id, shmem->bitmap);
#ifdef ULAPI
shmem->ulusers++;
#else /* RTAPI */
shmem->rtusers++;
#endif /* RTAPI */
/* announce another user for this shmem */
rtapi_print_msg(RTAPI_MSG_DBG,
"RTAPI: shmem %02d opened by module %02d\n",
shmem_id, module_id);
/* done */
rtapi_mutex_give(&(rtapi_data->mutex));
return shmem_id;
}
}
/* find empty spot in shmem array */
n = 1;
while ((n <= RTAPI_MAX_SHMEMS) && (shmem_array[n].key != 0)) {
rtapi_print_msg(RTAPI_MSG_DBG, OUR_API ": shmem %d occupuied \n",n);
n++;
}
if (n > RTAPI_MAX_SHMEMS) {
/* no room */
rtapi_mutex_give(&(rtapi_data->mutex));
rtapi_print_msg(RTAPI_MSG_ERR, "RTAPI: ERROR: reached shmem limit %d\n",
n);
return -EMFILE;
}
/* we have space for the block data */
rtapi_print_msg(RTAPI_MSG_DBG, OUR_API ": using new shmem %d \n",n);
shmem_id = n;
shmem = &(shmem_array[n]);
/* get shared memory block from OS and save its address */
sm.key = key;
sm.size = size;
sm.flags = 0;
#ifdef ULAPI
sm.driver_fd = shmdrv_driver_fd();
#endif
retval = shmdrv_create(&sm);
if (retval < 0) {
rtapi_mutex_give(&(rtapi_data->mutex));
rtapi_print_msg(RTAPI_MSG_ERR,"shmdrv create failed key=0x%x size=%ld\n", key, size);
return retval;
}
retval = shmdrv_attach(&sm, &shmem_addr_array[shmem_id]);
if (retval < 0) {
rtapi_mutex_give(&(rtapi_data->mutex));
rtapi_print_msg(RTAPI_MSG_ERR,"shmdrv attached failed key=0x%x size=%ld\n", key, size);
return retval;
}
if (shmem_addr_array[shmem_id] == NULL) {
rtapi_mutex_give(&(rtapi_data->mutex));
rtapi_print_msg(RTAPI_MSG_ERR,
"RTAPI: ERROR: could not create shmem %d\n", n);
return -ENOMEM;
}
/* the block has been created, update data */
rtapi_set_bit(module_id, shmem->bitmap);
shmem->key = key;
#ifdef RTAPI
shmem->rtusers = 1;
shmem->ulusers = 0;
#else /* ULAPI */
shmem->rtusers = 0;
shmem->ulusers = 1;
#endif /* ULAPI */
shmem->size = size;
shmem->magic = SHMEM_MAGIC;
shmem->instance = instance;
rtapi_data->shmem_count++;
/* zero the first word of the shmem area */
*((long int *) (shmem_addr_array[shmem_id])) = 0;
/* announce the birth of a brand new baby shmem */
rtapi_print_msg(RTAPI_MSG_DBG,
"RTAPI: shmem %02d created by module %02d, key: %d, size: %lu\n",
shmem_id, module_id, key, size);
/* and return the ID to the proud parent */
rtapi_mutex_give(&(rtapi_data->mutex));
return shmem_id;
}
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 _rtapi_shmem_new_inst(int userkey, int instance, int module_id, unsigned long int size) {
shmem_data *shmem;
int i, ret, actual_size;
int is_new = 0;
int key = OS_KEY(userkey, instance);
static int page_size;
if (!page_size)
page_size = sysconf(_SC_PAGESIZE);
rtapi_mutex_get(&(rtapi_data->mutex));
for (i = 1 ; i < RTAPI_MAX_SHMEMS; i++) {
if (shmem_array[i].magic == SHMEM_MAGIC && shmem_array[i].key == key) {
shmem_array[i].count ++;
rtapi_mutex_give(&(rtapi_data->mutex));
return i;
}
if (shmem_array[i].magic != SHMEM_MAGIC)
break;
}
if (i == RTAPI_MAX_SHMEMS) {
rtapi_mutex_give(&(rtapi_data->mutex));
rtapi_print_msg(RTAPI_MSG_ERR,
"rtapi_shmem_new failed due to RTAPI_MAX_SHMEMS\n");
return -ENOMEM;
}
shmem = &shmem_array[i];
// redefine size == 0 to mean 'attach only, dont create'
actual_size = size;
ret = shm_common_new(key, &actual_size, instance, &shmem->mem, size > 0);
if (ret > 0)
is_new = 1;
if (ret < 0) {
rtapi_mutex_give(&(rtapi_data->mutex));
rtapi_print_msg(RTAPI_MSG_ERR,
"shm_common_new:%d failed key=0x%x size=%ld\n",
instance, key, size);
return ret;
}
// a non-zero size was given but it didn match what we found:
if (size && (actual_size != size)) {
rtapi_print_msg(RTAPI_MSG_ERR,
"rtapi_shmem_new:%d 0x8.8%x: requested size %ld and actual size %d dont match\n",
instance, key, size, actual_size);
}
/* Touch each page by either zeroing the whole mem (if it's a new
SHM region), or by reading from it. */
if (is_new) {
memset(shmem->mem, 0, size);
} else {
unsigned int i;
for (i = 0; i < size; i += page_size) {
unsigned int x = *(volatile unsigned int *)
((unsigned char *)shmem->mem + i);
/* Use rand_r to clobber the read so GCC won't optimize it
out. */
rand_r(&x);
}
}
/* label as a valid shmem structure */
shmem->magic = SHMEM_MAGIC;
/* fill in the other fields */
shmem->size = actual_size;
shmem->key = key;
shmem->count = 1;
shmem->instance = instance;
rtapi_mutex_give(&(rtapi_data->mutex));
/* return handle to the caller */
return i;
}
static int export_wsum(int num, int num_bits, wsum_t *addr, wsum_bit_t *bitaddr)
{
int retval, i, w;
char buf[HAL_NAME_LEN+1], base[HAL_NAME_LEN+1];
rtapi_snprintf(base, sizeof(base), "wsum.%d", num);
/* export pin for offset (input) */
rtapi_snprintf(buf, sizeof(buf), "%s.offset", base);
retval = hal_pin_s32_new(buf, HAL_IO, &(addr->offset), comp_id);
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"WEIGHTED_SUM: ERROR: '%s' param export failed\n", buf);
return retval;
}
/* export pin for output sum */
rtapi_snprintf(buf, sizeof(buf), "%s.sum", base);
retval = hal_pin_s32_new(buf, HAL_OUT, &(addr->sum), comp_id);
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"WEIGHTED_SUM: ERROR: '%s' pin export failed\n", buf);
return retval;
}
/* export pin for update hold */
rtapi_snprintf(buf, sizeof(buf), "%s.hold", base);
retval = hal_pin_bit_new(buf, HAL_IN, &(addr->hold), comp_id);
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"WEIGHTED_SUM: ERROR: '%s' pin export failed\n", buf);
return retval;
}
addr->bits = bitaddr;
addr->num_bits = num_bits;
/* export the input bits and weight parameters, and set the default weights */
w = 1;
for (i=0;i<num_bits;i++) {
rtapi_snprintf(buf, sizeof(buf), "%s.bit.%d.in", base, i);
retval = hal_pin_bit_new(buf, HAL_IN, &(addr->bits[i].bit), comp_id);
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"WEIGHTED_SUM: ERROR: '%s' pin export failed\n", buf);
return retval;
}
rtapi_snprintf(buf, sizeof(buf), "%s.bit.%d.weight", base, i);
retval = hal_pin_s32_new(buf, HAL_IO, &(addr->bits[i].weight), comp_id);
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"WEIGHTED_SUM: ERROR: '%s' param export failed\n", buf);
return retval;
}
*(addr->bits[i].weight) = w;
w <<= 1;
}
/* set initial parameter and pin values */
*(addr->offset) = 0;
*(addr->sum) = 0;
return 0;
}
int rtapi_app_main(void)
{
char *cp;
char *argv[MAX_TOK];
char name[HAL_NAME_LEN + 1];
int n, retval;
#ifdef __KERNEL__
// this calculation fits in a 32-bit unsigned
// as long as CPUs are under about 6GHz
ns2tsc_factor = (cpu_khz << 6) / 15625ul;
#else
ns2tsc_factor = 1ll<<12;
#endif
/* test for config string */
if (cfg == 0) {
rtapi_print_msg(RTAPI_MSG_ERR, "PARPORT: ERROR: no config string\n");
return -1;
}
rtapi_print ( "config string '%s'\n", cfg );
/* as a RT module, we don't get a nice argc/argv command line, we only
get a single string... so we need to tokenize it ourselves */
/* in addition, it seems that insmod under kernel 2.6 will truncate
a string parameter at the first whitespace. So we allow '_' as
an alternate token separator. */
cp = cfg;
for (n = 0; n < MAX_TOK; n++) {
/* strip leading whitespace */
while ((*cp != '\0') && ( isspace(*cp) || ( *cp == '_') ))
cp++;
/* mark beginning of token */
argv[n] = cp;
/* find end of token */
while ((*cp != '\0') && !( isspace(*cp) || ( *cp == '_') ))
cp++;
/* mark end of this token, prepare to search for next one */
if (*cp != '\0') {
*cp = '\0';
cp++;
}
}
for (n = 0; n < MAX_TOK; n++) {
/* is token empty? */
if (argv[n][0] == '\0') {
/* yes - make pointer NULL */
argv[n] = NULL;
}
}
/* parse "command line", set up pins and parameters */
retval = pins_and_params(argv);
if (retval != 0) {
return retval;
}
/* export functions for each port */
for (n = 0; n < num_ports; n++) {
/* make read function name */
rtapi_snprintf(name, sizeof(name), "parport.%d.read", n);
/* export read function */
retval = hal_export_funct(name, read_port, &(port_data_array[n]),
0, 0, comp_id);
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"PARPORT: ERROR: port %d read funct export failed\n", n);
hal_exit(comp_id);
return -1;
}
/* make write function name */
rtapi_snprintf(name, sizeof(name), "parport.%d.write", n);
/* export write function */
retval = hal_export_funct(name, write_port, &(port_data_array[n]),
0, 0, comp_id);
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"PARPORT: ERROR: port %d write funct export failed\n", n);
hal_exit(comp_id);
return -1;
}
/* make reset function name */
rtapi_snprintf(name, sizeof(name), "parport.%d.reset", n);
/* export write function */
retval = hal_export_funct(name, reset_port, &(port_data_array[n]),
0, 0, comp_id);
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"PARPORT: ERROR: port %d reset funct export failed\n", n);
hal_exit(comp_id);
return -1;
}
}
/* export functions that read and write all ports */
retval = hal_export_funct("parport.read-all", read_all,
port_data_array, 0, 0, comp_id);
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"PARPORT: ERROR: read all funct export failed\n");
hal_exit(comp_id);
return -1;
}
retval = hal_export_funct("parport.write-all", write_all,
port_data_array, 0, 0, comp_id);
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"PARPORT: ERROR: write all funct export failed\n");
hal_exit(comp_id);
return -1;
}
rtapi_print_msg(RTAPI_MSG_INFO,
"PARPORT: installed driver for %d ports\n", num_ports);
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;
}
