int rtapi_app_main(void)
{
int n, retval = 0;
int rev, pinno;
char *endptr;
if ((rev = rpi_revision()) < 0)
return -1;
switch (rev) {
case 3:
rtapi_print_msg(RTAPI_MSG_INFO,
"Raspberry Model B Revision 1.0 + ECN0001 (no fuses, D14 removed)\n");
pins = rev1_pins;
gpios = rev1_gpios;
npins = sizeof(rev1_pins);
break;
case 2:
rtapi_print_msg(RTAPI_MSG_INFO,
"Raspberry Model B Revision 1.0\n");
pins = rev1_pins;
gpios = rev1_gpios;
npins = sizeof(rev1_pins);
break;
case 4:
case 5:
case 6:
rtapi_print_msg(RTAPI_MSG_INFO,
"Raspberry Model B Revision 2.0\n");
pins = rev2_pins;
gpios = rev2_gpios;
npins = sizeof(rev2_pins);
break;
default:
rtapi_print_msg(RTAPI_MSG_ERR,
"HAL_GPIO: ERROR: board revision %d not supported\n", rev);
return -1;
}
port_data = hal_malloc(npins * sizeof(void *));
if (port_data == 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"HAL_GPIO: ERROR: hal_malloc() failed\n");
hal_exit(comp_id);
return -1;
}
if (dir == 0) {
rtapi_print_msg(RTAPI_MSG_ERR, "HAL_GPIO: ERROR: no config string\n");
return -1;
}
dir_map = strtoul(dir, &endptr,0);
if (*endptr) {
rtapi_print_msg(RTAPI_MSG_ERR,
"HAL_GPIO: dir=%s - trailing garbage: '%s'\n",
dir, endptr);
return -1;
}
if (exclude == 0) {
rtapi_print_msg(RTAPI_MSG_ERR, "HAL_GPIO: ERROR: no exclude string\n");
return -1;
}
exclude_map = strtoul(exclude, &endptr,0);
if (*endptr) {
rtapi_print_msg(RTAPI_MSG_ERR,
"HAL_GPIO: exclude=%s - trailing garbage: '%s'\n",
exclude, endptr);
return -1;
}
if (setup_gpio_access())
return -1;
comp_id = hal_init("hal_gpio");
if (comp_id < 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"HAL_GPIO: ERROR: hal_init() failed\n");
return -1;
}
for (n = 0; n < npins; n++) {
if (exclude_map & RTAPI_BIT(n))
continue;
pinno = pins[n];
if (dir_map & RTAPI_BIT(n)) {
bcm2835_gpio_fsel(gpios[n], BCM2835_GPIO_FSEL_OUTP);
if ((retval = hal_pin_bit_newf(HAL_IN, &port_data[n],
comp_id, "hal_gpio.pin-%02d-out", pinno)) < 0)
break;
} else {
bcm2835_gpio_fsel(gpios[n], BCM2835_GPIO_FSEL_INPT);
if ((retval = hal_pin_bit_newf(HAL_OUT, &port_data[n],
comp_id, "hal_gpio.pin-%02d-in", pinno)) < 0)
break;
}
}
if (retval < 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"HAL_GPIO: ERROR: pin %d export failed with err=%i\n",
n,retval);
hal_exit(comp_id);
return -1;
}
retval = hal_export_funct("hal_gpio.write", write_port, 0,
0, 0, comp_id);
if (retval < 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"HAL_GPIO: ERROR: write funct export failed\n");
hal_exit(comp_id);
return -1;
}
retval = hal_export_funct("hal_gpio.read", read_port, 0,
0, 0, comp_id);
if (retval < 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"HAL_GPIO: ERROR: read funct export failed\n");
hal_exit(comp_id);
return -1;
}
rtapi_print_msg(RTAPI_MSG_INFO,
"HAL_GPIO: installed driver\n");
hal_ready(comp_id);
return 0;
}
int hm2_watchdog_parse_md(hostmot2_t *hm2, int md_index) {
hm2_module_descriptor_t *md = &hm2->md[md_index];
int r;
//
// some standard sanity checks
//
if (!hm2_md_is_consistent_or_complain(hm2, md_index, 0, 3, 4, 0)) {
HM2_ERR("inconsistent Module Descriptor!\n");
return -EINVAL;
}
if (hm2->watchdog.num_instances != 0) {
HM2_ERR(
"found duplicate Module Descriptor for %s (inconsistent firmware), not loading driver\n",
hm2_get_general_function_name(md->gtag)
);
return -EINVAL;
}
//
// special sanity checks for watchdog
//
if (md->instances != 1) {
HM2_PRINT("MD declares %d watchdogs! only using the first one...\n", md->instances);
}
//
// looks good, start initializing
//
hm2->watchdog.num_instances = 1;
hm2->watchdog.instance = (hm2_watchdog_instance_t *)hal_malloc(hm2->watchdog.num_instances * sizeof(hm2_watchdog_instance_t));
if (hm2->watchdog.instance == NULL) {
HM2_ERR("out of memory!\n");
r = -ENOMEM;
goto fail0;
}
hm2->watchdog.clock_frequency = md->clock_freq;
hm2->watchdog.version = md->version;
hm2->watchdog.timer_addr = md->base_address + (0 * md->register_stride);
hm2->watchdog.status_addr = md->base_address + (1 * md->register_stride);
hm2->watchdog.reset_addr = md->base_address + (2 * md->register_stride);
//
// allocate memory for register buffers
//
hm2->watchdog.status_reg = (u32 *)kmalloc(hm2->watchdog.num_instances * sizeof(u32), GFP_KERNEL);
if (hm2->watchdog.status_reg == NULL) {
HM2_ERR("out of memory!\n");
r = -ENOMEM;
goto fail0;
}
hm2->watchdog.reset_reg = (u32 *)kmalloc(hm2->watchdog.num_instances * sizeof(u32), GFP_KERNEL);
if (hm2->watchdog.reset_reg == NULL) {
HM2_ERR("out of memory!\n");
r = -ENOMEM;
goto fail1;
}
hm2->watchdog.timer_reg = (u32 *)kmalloc(hm2->watchdog.num_instances * sizeof(u32), GFP_KERNEL);
if (hm2->watchdog.timer_reg == NULL) {
HM2_ERR("out of memory!\n");
r = -ENOMEM;
goto fail2;
}
//
// export to HAL
//
// pins
r = hal_pin_bit_newf(
HAL_IO,
&(hm2->watchdog.instance[0].hal.pin.has_bit),
hm2->llio->comp_id,
"%s.watchdog.has_bit",
hm2->llio->name
);
if (r < 0) {
HM2_ERR("error adding pin, aborting\n");
r = -EINVAL;
goto fail3;
}
// params
r = hal_param_u32_newf(
HAL_RW,
&(hm2->watchdog.instance[0].hal.param.timeout_ns),
hm2->llio->comp_id,
"%s.watchdog.timeout_ns",
hm2->llio->name
);
if (r < 0) {
HM2_ERR("error adding param, aborting\n");
r = -EINVAL;
goto fail3;
}
// the function
{
char name[HAL_NAME_LEN + 1];
rtapi_snprintf(name, sizeof(name), "%s.pet_watchdog", hm2->llio->name);
r = hal_export_funct(name, hm2_pet_watchdog, hm2, 0, 0, hm2->llio->comp_id);
if (r != 0) {
HM2_ERR("error %d exporting pet_watchdog function %s\n", r, name);
r = -EINVAL;
goto fail3;
}
}
//
// initialize the watchdog
//
*hm2->watchdog.instance[0].hal.pin.has_bit = 0;
hm2->watchdog.instance[0].hal.param.timeout_ns = 5 * 1000 * 1000; // default timeout is 5 milliseconds
hm2->watchdog.instance[0].enable = 0; // the first pet_watchdog will turn it on
hm2->watchdog.instance[0].warned_about_short_timeout = 0;
hm2->watchdog.reset_reg[0] = 0x5a000000;
hm2->watchdog.status_reg[0] = 0;
return hm2->watchdog.num_instances;
fail3:
kfree(hm2->watchdog.timer_reg);
fail2:
kfree(hm2->watchdog.reset_reg);
fail1:
kfree(hm2->watchdog.status_reg);
fail0:
hm2->watchdog.num_instances = 0;
return r;
}
int rtapi_app_main(void)
{
int n, retval;
retval = setup_user_step_type();
if(retval < 0) {
return retval;
}
for (n = 0; n < MAX_CHAN && step_type[n] != -1 ; n++) {
if ((step_type[n] > MAX_STEP_TYPE) || (step_type[n] < 0)) {
rtapi_print_msg(RTAPI_MSG_ERR,
"STEPGEN: ERROR: bad stepping type '%i', axis %i\n",
step_type[n], n);
return -1;
}
if(parse_ctrl_type(ctrl_type[n]) == INVALID) {
rtapi_print_msg(RTAPI_MSG_ERR,
"STEPGEN: ERROR: bad control type '%s' for axis %i (must be 'p' or 'v')\n",
ctrl_type[n], n);
return -1;
}
num_chan++;
}
if (num_chan == 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"STEPGEN: ERROR: no channels configured\n");
return -1;
}
/* periodns will be set to the proper value when 'make_pulses()' runs for
the first time. We load a default value here to avoid glitches at
startup, but all these 'constants' are recomputed inside
'update_freq()' using the real period. */
old_periodns = periodns = 50000;
old_dtns = 1000000;
/* precompute some constants */
periodfp = periodns * 0.000000001;
freqscale = (1L << PICKOFF) * periodfp;
accelscale = freqscale * periodfp;
dt = old_dtns * 0.000000001;
recip_dt = 1.0 / dt;
/* have good config info, connect to the HAL */
comp_id = hal_init("stepgen");
if (comp_id < 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"STEPGEN: ERROR: hal_init() failed\n");
return -1;
}
/* allocate shared memory for counter data */
stepgen_array = hal_malloc(num_chan * sizeof(stepgen_t));
if (stepgen_array == 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"STEPGEN: ERROR: hal_malloc() failed\n");
hal_exit(comp_id);
return -1;
}
/* export all the variables for each pulse generator */
for (n = 0; n < num_chan; n++) {
/* export all vars */
retval = export_stepgen(n, &(stepgen_array[n]),
step_type[n], (parse_ctrl_type(ctrl_type[n]) == POSITION));
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"STEPGEN: ERROR: stepgen %d var export failed\n", n);
hal_exit(comp_id);
return -1;
}
}
/* export functions */
retval = hal_export_funct("stepgen.make-pulses", make_pulses,
stepgen_array, 0, 0, comp_id);
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"STEPGEN: ERROR: makepulses funct export failed\n");
hal_exit(comp_id);
return -1;
}
retval = hal_export_funct("stepgen.update-freq", update_freq,
stepgen_array, 1, 0, comp_id);
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"STEPGEN: ERROR: freq update funct export failed\n");
hal_exit(comp_id);
return -1;
}
retval = hal_export_funct("stepgen.capture-position", update_pos,
stepgen_array, 1, 0, comp_id);
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"STEPGEN: ERROR: pos update funct export failed\n");
hal_exit(comp_id);
return -1;
}
rtapi_print_msg(RTAPI_MSG_INFO,
"STEPGEN: installed %d step pulse generators\n", num_chan);
hal_ready(comp_id);
return 0;
}
int rtapi_app_main(void)
{
char name[HAL_NAME_LEN + 1];
int n,i , retval, num_dac, num_enc;
unsigned int base=0x300;
/* only one port at the moment */
num_ports = 1;
n = 0;
#define ISA_BASE 0xC9000
#define ISA_MAX 0x100000 /* allgemeiner Speicherzugriff */
/* STEP 1: initialise the driver */
comp_id = hal_init("hal_evoreg");
if (comp_id < 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"EVOREG: ERROR: hal_init() failed\n");
return -1;
}
/* STEP 2: allocate shared memory for EVOREG data */
port_data_array = hal_malloc(num_ports * sizeof(evoreg_t));
if (port_data_array == 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"EVOREG: ERROR: hal_malloc() failed\n");
hal_exit(comp_id);
return -1;
}
/*! \todo FIXME: Test memory area and setup the card */
port_data_array->io_base = ioremap(ISA_BASE, ISA_MAX - ISA_BASE);
rtapi_print_msg(RTAPI_MSG_ERR,"EVOREG: io_base: %p \n", port_data_array->io_base);
outw(0x82c9,base); /* set indexregister */
/* Set all outputs to zero */
writew(0, port_data_array->io_base + 0x20); /* digital out 0-15 */
writew(0, port_data_array->io_base + 0x40); /* digital out 16-23 */
writew(0, port_data_array->io_base + 0x60); /* DAC 1 */
writew(0, port_data_array->io_base + 0x80); /* DAC 2 */
writew(0, port_data_array->io_base + 0xa0); /* DAC 3 */
/* Reset Encoder's */
writew(0, port_data_array->io_base + 0x02); /* ENCODER 1 */
writew(0, port_data_array->io_base + 0x0a); /* ENCODER 2 */
writew(0, port_data_array->io_base + 0x12); /* ENCODER 3 */
/* STEP 3: export the pin(s) */
/* Export DAC pin's */
for ( num_dac=1; num_dac<=MAX_DAC; num_dac++) {
retval = hal_pin_float_newf(HAL_IN, &(port_data_array->dac_out[num_dac-1]),
comp_id, "evoreg.%d.dac-%02d-out", 1, num_dac);
if (retval < 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"EVOREG: ERROR: port %d var export failed with err=%i\n", n + 1,
retval);
hal_exit(comp_id);
return -1;
}
}
/* Export Encoder pin's */
for ( num_enc=1; num_enc<=MAX_ENC; num_enc++) {
retval = hal_pin_float_newf(HAL_OUT, &(port_data_array->position[num_enc - 1]),
comp_id, "evoreg.%d.position-%02d-in", 1, num_enc);
if (retval < 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"EVOREG: ERROR: port %d var export failed with err=%i\n", n + 1,
retval);
hal_exit(comp_id);
return -1;
}
}
/* Export IO pin's */
/* export write only HAL pin's for the input bit */
for ( i=0; i<=45;i++) {
retval += hal_pin_bit_newf(HAL_OUT, &(port_data_array->digital_in[i]),
comp_id, "evoreg.%d.pin-%02d-in", 1, i);
/* export another write only HAL pin for the same bit inverted */
/*
retval += hal_pin_bit_newf(HAL_OUT, &(port_data_array->digital_in[(2*i)+1]),
comp_id, "evoreg.%d.pin-%02d-in-not", 1, i); */
if (retval < 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"EVOREG: ERROR: port %d var export failed with err=%i\n", n + 1,
retval);
hal_exit(comp_id);
return -1;
}
}
/* export read only HAL pin's for the output bit */
for ( i=0; i<=23;i++) {
retval += hal_pin_bit_newf(HAL_IN, &(port_data_array->digital_out[i]),
comp_id, "evoreg.%d.pin-%02d-out", 1, i);
/* export another read only HAL pin for the same bit inverted */
/*
retval += hal_pin_bit_newf(HAL_IN, &(port_data_array->digital_out[(2*i)+1]),
comp_id, "evoreg.%d.pin-%02d-out-not", 1, i)); */
if (retval < 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"EVOREG: ERROR: port %d var export failed with err=%i\n", n + 1,
retval);
hal_exit(comp_id);
return -1;
}
}
/* export parameter for scaling */
retval = hal_param_float_newf(HAL_RW, &(port_data_array->pos_scale),
comp_id, "evoreg.%d.position-scale", 1);
if (retval != 0) {
return retval;
}
/* STEP 4: export function */
rtapi_snprintf(name, sizeof(name), "evoreg.%d.update", n + 1);
retval = hal_export_funct(name, update_port, &(port_data_array[n]), 1, 0,
comp_id);
if (retval < 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"EVOREG: ERROR: port %d write funct export failed\n", n + 1);
hal_exit(comp_id);
return -1;
}
rtapi_print_msg(RTAPI_MSG_INFO,
"EVOREG: installed driver for %d card(s)\n", num_ports);
hal_ready(comp_id);
return 0;
}
static int init_sampler(int num, fifo_t *tmp_fifo)
{
int size, retval, n, usefp;
void *shmem_ptr;
sampler_t *str;
pin_data_t *pptr;
fifo_t *fifo;
char buf[HAL_NAME_LEN + 2];
/* alloc shmem for base sampler data and user specified pins */
size = sizeof(sampler_t) + tmp_fifo->num_pins * sizeof(pin_data_t);
str = hal_malloc(size);
if (str == 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"SAMPLER: ERROR: couldn't allocate HAL shared memory\n");
return -ENOMEM;
}
/* export "standard" pins and params */
retval = hal_pin_bit_newf(HAL_OUT, &(str->full), comp_id,
"sampler.%d.full", num);
if (retval != 0 ) {
rtapi_print_msg(RTAPI_MSG_ERR,
"SAMPLER: ERROR: 'full' pin export failed\n");
return -EIO;
}
retval = hal_pin_bit_newf(HAL_IN, &(str->enable), comp_id,
"sampler.%d.enable", num);
if (retval != 0 ) {
rtapi_print_msg(RTAPI_MSG_ERR,
"SAMPLER: ERROR: 'enable' pin export failed\n");
return -EIO;
}
retval = hal_pin_s32_newf(HAL_OUT, &(str->curr_depth), comp_id,
"sampler.%d.curr-depth", num);
if (retval != 0 ) {
rtapi_print_msg(RTAPI_MSG_ERR,
"SAMPLEr: ERROR: 'curr_depth' pin export failed\n");
return -EIO;
}
retval = hal_param_s32_newf(HAL_RW, &(str->overruns), comp_id,
"sampler.%d.overruns", num);
if (retval != 0 ) {
rtapi_print_msg(RTAPI_MSG_ERR,
"SAMPLER: ERROR: 'overruns' parameter export failed\n");
return -EIO;
}
retval = hal_param_s32_newf(HAL_RW, &(str->sample_num), comp_id,
"sampler.%d.sample-num", num);
if (retval != 0 ) {
rtapi_print_msg(RTAPI_MSG_ERR,
"SAMPLER: ERROR: 'sample-num' parameter export failed\n");
return -EIO;
}
/* init the standard pins and params */
*(str->full) = 0;
*(str->enable) = 1;
*(str->curr_depth) = 0;
str->overruns = 0;
str->sample_num = 0;
/* HAL pins are right after the sampler_t struct in HAL shmem */
pptr = (pin_data_t *)(str+1);
usefp = 0;
/* export user specified pins (the ones that sample data) */
for ( n = 0 ; n < tmp_fifo->num_pins ; n++ ) {
rtapi_snprintf(buf, HAL_NAME_LEN, "sampler.%d.pin.%d", num, n);
retval = hal_pin_new(buf, tmp_fifo->type[n], HAL_IN, (void **)pptr, comp_id );
if (retval != 0 ) {
rtapi_print_msg(RTAPI_MSG_ERR,
"SAMPLER: ERROR: pin '%s' export failed\n", buf);
return -EIO;
}
/* init the pin value */
switch ( tmp_fifo->type[n] ) {
case HAL_FLOAT:
*(pptr->hfloat) = 0.0;
usefp = 1;
break;
case HAL_BIT:
*(pptr->hbit) = 0;
break;
case HAL_U32:
*(pptr->hu32) = 0;
break;
case HAL_S32:
*(pptr->hs32) = 0;
break;
default:
break;
}
pptr++;
}
/* export update function */
rtapi_snprintf(buf, HAL_NAME_LEN, "sampler.%d", num);
retval = hal_export_funct(buf, sample, str, usefp, 0, comp_id);
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"SAMPLER: ERROR: function export failed\n");
return retval;
}
/* alloc shmem for user/RT comms (fifo) */
size = sizeof(fifo_t) + (tmp_fifo->num_pins + 1) * tmp_fifo->depth * sizeof(shmem_data_t);
shmem_id[num] = rtapi_shmem_new(SAMPLER_SHMEM_KEY+num, comp_id, size);
if ( shmem_id[num] < 0 ) {
rtapi_print_msg(RTAPI_MSG_ERR,
"SAMPLEr: ERROR: couldn't allocate user/RT shared memory\n");
return -ENOMEM;
}
retval = rtapi_shmem_getptr(shmem_id[num], &shmem_ptr);
if ( retval < 0 ) {
rtapi_print_msg(RTAPI_MSG_ERR,
"SAMPLER: ERROR: couldn't map user/RT shared memory\n");
return -ENOMEM;
}
fifo = shmem_ptr;
str->fifo = fifo;
/* copy data from temp_fifo */
*fifo = *tmp_fifo;
/* init fields */
fifo->in = 0;
fifo->out = 0;
fifo->last_sample = 0;
fifo->last_sample--;
/* mark it inited for user program */
fifo->magic = FIFO_MAGIC_NUM;
return 0;
}
int 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 (retval >= HAL_NAME_LEN) {
goto fail0;
}
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);
if (retval >= HAL_NAME_LEN) {
goto fail0;
}
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]);
if (retval >= HAL_NAME_LEN) {
goto fail0;
}
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;
}
/* init_threads() creates realtime threads, exports functions to
do the realtime control, and adds the functions to the threads.
*/
static int init_threads(void)
{
double base_period_sec, servo_period_sec;
int servo_base_ratio;
int retval;
rtapi_print_msg(RTAPI_MSG_INFO, "MOTION: init_threads() starting...\n");
/* if base_period not specified, assume same as servo_period */
if (base_period_nsec == 0) {
base_period_nsec = servo_period_nsec;
}
if (traj_period_nsec == 0) {
traj_period_nsec = servo_period_nsec;
}
/* servo period must be greater or equal to base period */
if (servo_period_nsec < base_period_nsec) {
rtapi_print_msg(RTAPI_MSG_ERR,
"MOTION: bad servo period %ld nsec\n", servo_period_nsec);
return -1;
}
/* convert desired periods to floating point */
base_period_sec = base_period_nsec * 0.000000001;
servo_period_sec = servo_period_nsec * 0.000000001;
/* calculate period ratios, round to nearest integer */
servo_base_ratio = (servo_period_sec / base_period_sec) + 0.5;
/* revise desired periods to be integer multiples of each other */
servo_period_nsec = base_period_nsec * servo_base_ratio;
/* create HAL threads for each period */
/* only create base thread if it is faster than servo thread */
if (servo_base_ratio > 1) {
retval = hal_create_thread("base-thread", base_period_nsec, base_thread_fp, base_cpu);
if (retval < 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"MOTION: failed to create %ld nsec base thread\n",
base_period_nsec);
return -1;
}
}
retval = hal_create_thread("servo-thread", servo_period_nsec, 1, servo_cpu);
if (retval < 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"MOTION: failed to create %ld nsec servo thread\n",
servo_period_nsec);
return -1;
}
/* export realtime functions that do the real work */
retval = hal_export_funct("motion-controller", emcmotController, 0 /* arg
*/ , 1 /* uses_fp */ , 0 /* reentrant */ , mot_comp_id);
if (retval < 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"MOTION: failed to export controller function\n");
return -1;
}
retval = hal_export_funct("motion-command-handler", emcmotCommandHandler, 0 /* arg
*/ , 1 /* uses_fp */ , 0 /* reentrant */ , mot_comp_id);
if (retval < 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"MOTION: failed to export command handler function\n");
return -1;
}
/*! \todo Another #if 0 */
#if 0
/*! \todo FIXME - currently the traj planner is called from the controller */
/* eventually it will be a separate function */
retval = hal_export_funct("motion-traj-planner", emcmotTrajPlanner, 0 /* arg
*/ , 1 /* uses_fp */ ,
0 /* reentrant */ , mot_comp_id);
if (retval < 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"MOTION: failed to export traj planner function\n");
return -1;
}
#endif
// if we don't set cycle times based on these guesses, emc doesn't
// start up right
setServoCycleTime(servo_period_nsec * 1e-9);
setTrajCycleTime(traj_period_nsec * 1e-9);
rtapi_print_msg(RTAPI_MSG_INFO, "MOTION: init_threads() 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;
/* 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 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 result, i;
CopySizesInfosFromModuleParams();
compId = hal_init("classicladder_rt");
if(compId < 0) return compId;
rtapi_print("creating ladder-state\n");
result = hal_export_funct("classicladder.0.refresh",hal_task,0,1, 0, compId);
if(result < 0) {
error:
hal_exit(compId);
return result;
}
hal_state = hal_malloc(sizeof(hal_s32_t));
result = hal_param_s32_new("classicladder.ladder-state", HAL_RO, hal_state, compId);
if(result < 0) {
hal_exit(compId);
return result;
}
hal_inputs = hal_malloc(sizeof(hal_bit_t*) * numPhysInputs);
if(!hal_inputs) { result = -ENOMEM; goto error; }
hide_gui = hal_malloc(sizeof(hal_bit_t*));
if(!hide_gui) { result = -ENOMEM; goto error; }
hal_s32_inputs = hal_malloc(sizeof(hal_s32_t*) * numS32in);
if(!hal_s32_inputs) { result = -ENOMEM; goto error; }
hal_float_inputs = hal_malloc(sizeof(hal_float_t*) * numFloatIn);
if(!hal_float_inputs) { result = -ENOMEM; goto error; }
hal_outputs = hal_malloc(sizeof(hal_bit_t*) * numPhysOutputs);
if(!hal_outputs) { result = -ENOMEM; goto error; }
hal_s32_outputs = hal_malloc(sizeof(hal_s32_t*) * numS32out);
if(!hal_s32_outputs) { result = -ENOMEM; goto error; }
hal_float_outputs = hal_malloc(sizeof(hal_float_t*) * numFloatOut);
if(!hal_float_outputs) { result = -ENOMEM; goto error; }
for(i=0; i<numPhysInputs; i++) {
result = hal_pin_bit_newf(HAL_IN, &hal_inputs[i], compId,
"classicladder.0.in-%02d", i);
if(result < 0) goto error;
}
result = hal_pin_bit_newf(HAL_IN, &hide_gui[0], compId,
"classicladder.0.hide_gui");
if(result < 0) goto error;
for(i=0; i<numS32in; i++) {
result = hal_pin_s32_newf(HAL_IN, &hal_s32_inputs[i], compId,
"classicladder.0.s32in-%02d", i);
if(result < 0) goto error;
}
for(i=0; i<numFloatIn; i++) {
result = hal_pin_float_newf(HAL_IN, &hal_float_inputs[i], compId,
"classicladder.0.floatin-%02d", i);
if(result < 0) goto error;
}
for(i=0; i<numPhysOutputs; i++) {
result = hal_pin_bit_newf(HAL_OUT, &hal_outputs[i], compId,
"classicladder.0.out-%02d", i);
if(result < 0) goto error;
}
for(i=0; i<numS32out; i++) {
result = hal_pin_s32_newf(HAL_OUT, &hal_s32_outputs[i], compId,
"classicladder.0.s32out-%02d", i);
if(result < 0) goto error;
}
for(i=0; i<numFloatOut; i++) {
result = hal_pin_float_newf(HAL_OUT, &hal_float_outputs[i], compId,
"classicladder.0.floatout-%02d", i);
if(result < 0) goto error;
}
hal_ready(compId);
ClassicLadder_AllocAll( );
return 0;
}
int rtapi_app_main(void)
{
int n, total_bits, retval;
total_bits = 0;
/* check that there's at least one valid summer requested */
for (n = 0; n < MAX_SUMMERS && wsum_sizes[n] != -1 ; n++) {
if ((wsum_sizes[n] > MAX_SUM_BITS) || (wsum_sizes[n]<1)) {
rtapi_print_msg(RTAPI_MSG_ERR,
"WEIGHTED_SUM: ERROR: Invalid number of bits '%i' for summer %i\n",
wsum_sizes[n], n);
return -1;
} else {
num_summers++;
total_bits += wsum_sizes[n];
}
}
if (num_summers == 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"WEIGHTED_SUM: ERROR: no summers specified\n");
return -1;
}
/* have good config info, connect to the HAL */
comp_id = hal_init("weighted_sum");
if (comp_id < 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"WEIGHTED_SUM: ERROR: hal_init() failed\n");
return -1;
}
/* allocate shared memory for summer base info */
wsum_array = hal_malloc(num_summers * sizeof(wsum_t));
if (wsum_array == 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"WEIGHTED_SUM: ERROR: hal_malloc() for summer array failed\n");
hal_exit(comp_id);
return -1;
}
/* allocate shared memory for summer bit arrays */
wsum_bit_array = hal_malloc(total_bits * sizeof(wsum_bit_t));
if (wsum_bit_array == 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"WEIGHTED_SUM: ERROR: hal_malloc() for summer bit array failed\n");
hal_exit(comp_id);
return -1;
}
/* export pins/params for all summers */
total_bits = 0;
for (n = 0; n < num_summers; n++) {
/* export all vars */
retval = export_wsum(n, wsum_sizes[n], &(wsum_array[n]), &(wsum_bit_array[total_bits]));
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"WEIGHTED_SUM: ERROR: group %d export failed\n", n);
hal_exit(comp_id);
return -1;
}
total_bits += wsum_array[n].num_bits;
}
/* export update function */
retval = hal_export_funct("process_wsums", process_wsums, wsum_array, 1, 0, comp_id);
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"WEIGHTED_SUM: ERROR: process_wsums funct export failed\n");
return -1;
}
rtapi_print_msg(RTAPI_MSG_INFO,
"WEIGHTED_SUM: installed %d weighted summers\n", num_summers);
hal_ready(comp_id);
return 0;
}
int rtapi_app_main(void){
int i, j, n;
int retval;
comp_id = hal_init("matrix_kb");
if (comp_id < 0) {
rtapi_print_msg(RTAPI_MSG_ERR, "matrix_kb: ERROR: hal_init() failed\n");
return -1;
}
// allocate shared memory for data
kb = hal_malloc(sizeof(kb_t));
if (kb == 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"matrix_kb component: Out of Memory\n");
hal_exit(comp_id);
return -1;
}
// Count the instances.
for (kb->num_insts = 0; config[kb->num_insts];kb->num_insts++);
// Count the names.
for (n = 0; names[n];n++);
if (n && n != kb->num_insts){
rtapi_print_msg(RTAPI_MSG_ERR, "matrix_kb: Number of sizes and number"
" of names must match\n");
hal_exit(comp_id);
return -1;
}
kb->insts = hal_malloc(kb->num_insts * sizeof(kb_inst_t));
for (i = 0; i < kb->num_insts; i++){
int a = 0;
int c, r;
kb_inst_t *inst = &kb->insts[i];
inst->index = i;
inst->nrows = 0;
inst->ncols = 0;
inst->scan = 0;
inst->keystroke = 0;
inst->param.invert = 1;
for(j = 0; config[i][j] !=0; j++){
int n = (config[i][j] | 0x20); //lower case
if (n == 'x'){
inst->nrows = a;
a = 0;
}
else if (n >= '0' && n <= '9'){
a = (a * 10) + (n - '0');
}
else if (n == 's'){
inst->scan = 1;
}
}
inst->ncols = a;
if (inst->ncols == 0 || inst->nrows == 0){
rtapi_print_msg(RTAPI_MSG_ERR,
"matrix_kb: Invalid size format. should be NxN\n");
hal_exit(comp_id);
return -1;
}
if (inst->ncols > 32){
rtapi_print_msg(RTAPI_MSG_ERR,
"matrix_kb: maximum number of columns is 32. Sorry\n");
hal_exit(comp_id);
return -1;
}
for (inst->rowshift = 1; inst->ncols > (1 << inst->rowshift); inst->rowshift++);
for (inst->keydown = 0xC0, inst->keyup = 0x80
; (inst->nrows << inst->rowshift) > inst->keydown
; inst->keydown <<= 1, inst->keyup <<= 1);
inst->hal.key = (hal_bit_t **)hal_malloc(inst->nrows * inst->ncols * sizeof(hal_bit_t*));
inst->now = hal_malloc(inst->nrows * sizeof(hal_u32_t));
inst->then = hal_malloc(inst->nrows * sizeof(hal_u32_t));
inst->row = 0;
inst->param.rollover = 2;
if (names[i]){
rtapi_snprintf(inst->name, sizeof(inst->name), "%s", names[i]);
}
else
{
rtapi_snprintf(inst->name, sizeof(inst->name), "matrix_kb.%i", i);
}
for (c = 0; c < inst->ncols; c++){
for (r = 0; r < inst->nrows; r++){
retval = hal_pin_bit_newf(HAL_OUT,
&(inst->hal.key[r * inst->ncols + c]),
comp_id,
"%s.key.r%xc%x",
inst->name, r, c);
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"matrix_kb: Failed to create output pin\n");
hal_exit(comp_id);
return -1;
}
}
}
if (inst->scan){ //internally generated scanning
inst->hal.rows = (hal_bit_t **)hal_malloc(inst->nrows * sizeof(hal_bit_t*));
inst->hal.cols = (hal_bit_t **)hal_malloc(inst->ncols * sizeof(hal_bit_t*));
for (r = 0; r < inst->nrows; r++){
retval = hal_pin_bit_newf(HAL_OUT,
&(inst->hal.rows[r]), comp_id,
"%s.row-%02i-out",inst->name, r);
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"matrix_kb: Failed to create output row pin\n");
hal_exit(comp_id);
return -1;
}
}
for (c = 0; c < inst->ncols; c++){
retval = hal_pin_bit_newf(HAL_IN,
&(inst->hal.cols[c]), comp_id,
"%s.col-%02i-in",inst->name, c);
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"matrix_kb: Failed to create input col pin\n");
hal_exit(comp_id);
return -1;
}
}
retval = hal_pin_u32_newf(HAL_OUT,
&(inst->hal.keycode), comp_id,
"%s.keycode",inst->name);
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"matrix_kb: Failed to create output pin\n");
hal_exit(comp_id);
return -1;
}
retval = hal_param_bit_newf(HAL_RW,
&(inst->param.invert), comp_id,
"%s.negative-logic",inst->name);
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"matrix_kb: Failed to create output pin\n");
hal_exit(comp_id);
return -1;
}
retval = hal_param_u32_newf(HAL_RW,
&(inst->param.rollover), comp_id,
"%s.key_rollover",inst->name);
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"matrix_kb: Failed to create rollover param\n");
hal_exit(comp_id);
return -1;
}
}
else // scanning by 7i73 or similar
{
retval = hal_pin_u32_newf(HAL_IN,
&(inst->hal.keycode), comp_id,
"%s.keycode",inst->name);
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"matrix_kb: Failed to create input pin\n");
hal_exit(comp_id);
return -1;
}
}
retval = hal_export_funct(inst->name, loop, inst, 1, 0, comp_id); //needs fp?
if (retval < 0) {
rtapi_print_msg(RTAPI_MSG_ERR, "matrix_kb: ERROR: function export failed\n");
return -1;
}
}
hal_ready(comp_id);
return 0;
}
int 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 n, retval;
rtapi_print_msg(RTAPI_MSG_ERR,
"FREQGEN: freqgen is deprecated and will be removed in emc2.4. "
"Use stepgen with ctrl_type=v instead\n");
for (n = 0; n < MAX_CHAN && step_type[n] != -1 ; n++) {
if ((step_type[n] > MAX_STEP_TYPE) || (step_type[n] < 0)) {
rtapi_print_msg(RTAPI_MSG_ERR,
"FREQGEN: ERROR: bad stepping type '%i', axis %i\n",
step_type[n], n);
return -1;
} else {
num_chan++;
}
}
if (num_chan == 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"FREQGEN: ERROR: no channels configured\n");
return -1;
}
/* periodns will be set to the proper value when 'make_pulses()' runs for
the first time. We load a default value here to avoid glitches at
startup, but all these 'constants' are recomputed inside
'update_freq()' using the real period. */
periodns = 50000;
/* precompute some constants */
periodfp = periodns * 0.000000001;
maxf = 1.0 / periodfp;
freqscale = ((1L << 30) * 2.0) / maxf;
accelscale = freqscale * periodfp;
old_periodns = periodns;
/* have good config info, connect to the HAL */
comp_id = hal_init("freqgen");
if (comp_id < 0) {
rtapi_print_msg(RTAPI_MSG_ERR, "FREQGEN: ERROR: hal_init() failed\n");
return -1;
}
/* allocate shared memory for counter data */
freqgen_array = hal_malloc(num_chan * sizeof(freqgen_t));
if (freqgen_array == 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"FREQGEN: ERROR: hal_malloc() failed\n");
hal_exit(comp_id);
return -1;
}
/* export all the variables for each pulse generator */
for (n = 0; n < num_chan; n++) {
/* export all vars */
retval = export_freqgen(n, &(freqgen_array[n]), step_type[n]);
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"FREQGEN: ERROR: freqgen %d var export failed\n", n);
hal_exit(comp_id);
return -1;
}
}
/* export functions */
retval = hal_export_funct("freqgen.make-pulses", make_pulses,
freqgen_array, 0, 0, comp_id);
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"FREQGEN: ERROR: makepulses funct export failed\n");
hal_exit(comp_id);
return -1;
}
retval = hal_export_funct("freqgen.update-freq", update_freq,
freqgen_array, 1, 0, comp_id);
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"FREQGEN: ERROR: freq update funct export failed\n");
hal_exit(comp_id);
return -1;
}
retval = hal_export_funct("freqgen.capture-position", update_pos,
freqgen_array, 1, 0, comp_id);
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"FREQGEN: ERROR: pos update funct export failed\n");
hal_exit(comp_id);
return -1;
}
rtapi_print_msg(RTAPI_MSG_INFO,
"FREQGEN: installed %d step pulse generators\n", num_chan);
hal_ready(comp_id);
return 0;
}
int rtapi_app_main(void)
{
int retval;
/* test for number of channels */
if ((num_chan <= 0) || (num_chan > MAX_CHAN)) {
rtapi_print_msg(RTAPI_MSG_ERR,
"VTI: ERROR: invalid num_chan: %d\n", num_chan);
return -1;
}
/* test for config string */
if ((dio == 0) || (dio[0] == '\0')) {
rtapi_print_msg(RTAPI_MSG_ERR, "VTI: ERROR: no dio config string\n");
return -1;
}
/* have good config info, connect to the HAL */
comp_id = hal_init("hal_vti");
if (comp_id < 0) {
rtapi_print_msg(RTAPI_MSG_ERR, "VTI: ERROR: hal_init() failed\n");
return -1;
}
/* allocate shared memory for vti data */
vti_driver = hal_malloc(num_chan * sizeof(vti_struct));
if (vti_driver == 0) {
rtapi_print_msg(RTAPI_MSG_ERR, "VTI: ERROR: hal_malloc() failed\n");
hal_exit(comp_id);
return -1;
}
/* takes care of all initialisations, also autodetection and model if necessary */
if ((retval=vti_init_card()) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"VTI: ERROR: vti_init_card() failed\n");
hal_exit(comp_id);
return retval;
}
diocount = vti_parse_dio();
if (diocount == -1) {
rtapi_print_msg(RTAPI_MSG_ERR,
"VTI: ERROR: bad config info for port.\n");
return -1;
}
export_dio_pins(diocount);
vti_dio_init(diocount / 4);
/* init counter chip */
if (vti_counter_init(num_chan) == -1) {
rtapi_print_msg(RTAPI_MSG_ERR,
"VTI: ERROR: bad config info counter.\n");
return -1;
}
/* init dac chip */
vti_dac_init(num_chan);
/* init adc chip */
vti_adc_init(0); // VTI controller has no ADCs
/* export functions */
retval = hal_export_funct("vti.capture-position", vti_counter_capture,
vti_driver, 1, 0, comp_id);
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"VTI: ERROR: vti.counter-capture funct export failed\n");
hal_exit(comp_id);
return -1;
}
rtapi_print_msg(RTAPI_MSG_INFO,
"VTI: installed %d encoder counters\n", num_chan);
retval = hal_export_funct("vti.write-dacs", vti_dacs_write,
vti_driver, 1, 0, comp_id);
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"VTI: ERROR: vti.write-dacs funct export failed\n");
hal_exit(comp_id);
return -1;
}
rtapi_print_msg(RTAPI_MSG_INFO, "VTI: installed %d dacs\n", num_chan);
retval = hal_export_funct("vti.read-adcs", vti_adcs_read,
vti_driver, 1, 0, comp_id);
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"VTI: ERROR: vti.read-adcs funct export failed\n");
hal_exit(comp_id);
return -1;
}
rtapi_print_msg(RTAPI_MSG_INFO, "VTI: installed %d adcs\n", 0);
retval = hal_export_funct("vti.di-read", vti_di_read,
vti_driver, 0, 0, comp_id);
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"VTI: ERROR: vti.di-read funct export failed\n");
hal_exit(comp_id);
return -1;
}
rtapi_print_msg(RTAPI_MSG_INFO,
"VTI: installed %d digital inputs\n", inputpinnum);
retval = hal_export_funct("vti.do-write", vti_do_write,
vti_driver, 0, 0, comp_id);
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"VTI: ERROR: vti.do-write funct export failed\n");
hal_exit(comp_id);
return -1;
}
rtapi_print_msg(RTAPI_MSG_INFO,
"VTI: installed %d digital outputs\n", outpinnum);
hal_ready(comp_id);
return 0;
}
int rtapi_app_main(void)
{
int retval, i;
int in_cnt = 0, out_cnt = 0;
char buf[128];
char *pc = axes_conf;
/* Parsing axes configuration */
while(*pc != 0)
{
int idx = -1;
switch(*pc)
{
case 'X':
case 'x':
idx = 0;
break;
case 'Y':
case 'y':
idx = 1;
break;
case 'Z':
case 'z':
idx = 2;
break;
case 'A':
case 'a':
idx = 3;
break;
case 'B':
case 'b':
idx = 4;
break;
case 'C':
case 'c':
idx = 5;
break;
default: break;
}
if(idx >= 0)
axis_map[num_axis++] = idx;
pc++;
}
fprintf(stderr, "num_axis=%d, fifo_size=%d\n", num_axis, fifo_deep);
/* test for number of channels */
if ((num_axis <= 0) || (num_axis > MAX_AXIS)) {
rtapi_print_msg(RTAPI_MSG_ERR,
"miniemcdrv: ERROR: invalid num_chan: %d\n", num_axis);
return -EINVAL;
}
/* have good config info, connect to the HAL */
comp_id = hal_init("miniemcdrv");
if (comp_id < 0)
{
rtapi_print_msg(RTAPI_MSG_ERR, "miniemcdrv: ERROR: hal_init() failed\n");
rtapi_app_exit();
return -EINVAL;
}
pgpio = hal_malloc(sizeof(gpio_t));
memset(pgpio, 0, sizeof(gpio_t));
pgpio->fd = open("/dev/miniemc", O_RDWR | O_SYNC);
if(pgpio->fd < 0)
{
rtapi_print_msg(RTAPI_MSG_ERR,
"miniemcdrv: ERROR: unble to create access to stepgen module\n");
rtapi_app_exit();
return -EIO;
}
pgpio->pfiq = mmap(0, sizeof(struct fiq_ipc_shared), PROT_READ | PROT_WRITE, MAP_FILE | MAP_SHARED, pgpio->fd, 0);
if(pgpio->pfiq == MAP_FAILED)
{
rtapi_print_msg(RTAPI_MSG_ERR,
"miniemcdrv: ERROR: unable to mmap stepgen ringbuffer\n");
rtapi_app_exit();
return -EIO;
}
/* Setup ringbuff size */
fiq_static.rb_size = fifo_deep;
memset(&cmd_pos_prev, 0, sizeof(cmd_pos_prev));
memset(&cmd_pos_accum, 0, sizeof(cmd_pos_accum));
//Configure PWM pins and create HAL inputs
for( i = 0; i < MAX_PWM; i++)
{
rtapi_snprintf(buf, HAL_NAME_LEN, "miniemcdrv.%d.pwm-in", i);
retval = hal_pin_float_new(buf, HAL_IN, &(pgpio->pwm_duty[i]), comp_id);
if (retval != 0)
{
rtapi_app_exit();
return retval;
}
if( pwm_pin_num[i] >= 0 )
{
emcConfigureDefault( pwm_pin_num[i] );
emcReservePin( pwm_pin_num[i] );
fiq_static.pwm_pin_addr[i] = GPIOS[pwm_pin_num[i]].port_index;
fiq_static.pwm_pin_mask[i] = 1L << GPIOS[pwm_pin_num[i]].offset;
pgpio->pfiq->pwm_duty_cycle[i] = 0;
} else
{
fiq_static.pwm_pin_mask[i] = 0;
fiq_static.pwm_pin_addr[i] = 0;
}
}
// Create axis step and dir pins
for(i = 0; i < num_axis; i++)
{
if(step_pins[i] >=0 && dir_pins[i] >= 0)
{
if( emcGetPinMode( step_pins[i]) == egpIn || emcGetPinMode( dir_pins[i]) == egpIn )
{
rtapi_print_msg(RTAPI_MSG_ERR, "WARN: can't create axis[%d] stepgen, invalid pin\n", i);
continue;
}
fiq_static.axis[i].configured = 0;
fiq_static.axis[i].step_pin_addr = GPIOS[step_pins[i]].port_index;
fiq_static.axis[i].step_pin_mask = 1L << GPIOS[step_pins[i]].offset;
emcConfigureDefault( step_pins[i] );
emcReservePin( step_pins[i] );
fiq_static.axis[i].dir_pin_addr = GPIOS[dir_pins[i]].port_index;
fiq_static.axis[i].dir_pin_mask = 1L << GPIOS[dir_pins[i]].offset;
emcConfigureDefault( dir_pins[i] );
emcReservePin( dir_pins[i] );
fiq_static.axis[i].dir_pin_pol = dir_polarity[i];
fiq_static.axis[i].configured = 1;
} else
{
rtapi_print_msg(RTAPI_MSG_ERR,
"miniemcdrv: WARNING: axis[%d] step and/or dir pin(s) not properly configured, skipping\n", i);
}
fiq_static.scan_pin_num = -1;
}
ioctl(pgpio->fd, AXIS_SET_IOCTL, &fiq_static );
/*
* Create IO pins
*/
for( i=0; i < ARR_SZ(GPIOS); i++ )
{
if( emcGetPinMode( i ) == egpRsv )
continue;
if( emcGetPinMode( i ) == egpIn )
{
rtapi_snprintf(buf, HAL_NAME_LEN, "miniemcdrv.%d.pin-in", in_cnt);
hal_pin_bit_new(buf, HAL_OUT, &(pgpio->io_pin[i]), comp_id);
rtapi_snprintf(buf, HAL_NAME_LEN, "miniemcdrv.%d.pin-in-inv", in_cnt);
hal_pin_bit_new(buf, HAL_IN, &(pgpio->io_invert[i]), comp_id);
in_cnt++;
} else
{
rtapi_snprintf(buf, HAL_NAME_LEN, "miniemcdrv.%d.pin-out", out_cnt);
hal_pin_bit_new(buf, HAL_IN, &(pgpio->io_pin[i]), comp_id);
rtapi_snprintf(buf, HAL_NAME_LEN, "miniemcdrv.%d.pin-out-inv", out_cnt);
hal_pin_bit_new(buf, HAL_IN, &(pgpio->io_invert[i]), comp_id);
out_cnt++;
}
emcConfigureDefault( i );
}
// Trajectory wait output
rtapi_snprintf(buf, HAL_NAME_LEN, "miniemcdrv.traj-wait-out");
hal_pin_bit_new(buf, HAL_OUT, &(pgpio->traj_wait), comp_id);
*(pgpio->traj_wait) = 1;
// Scaner sync
rtapi_snprintf(buf, HAL_NAME_LEN, "miniemcdrv.scan-sync-in");
hal_pin_bit_new(buf, HAL_IN, &(pgpio->scan_sync), comp_id);
for(i=0; i < num_axis; i++)
{
// Check if pin already added
char contin = 0;
int j;
for(j=0; j < i; j++)
if(axis_map[j] == axis_map[i])
{
contin = 1;
break;
}
if(contin) continue;
// commanded position pin
rtapi_snprintf(buf, HAL_NAME_LEN, "miniemcdrv.%d.cmd-pos", axis_map[i]);
retval = hal_pin_float_new(buf, HAL_IN, &(pgpio->cmd_pos[axis_map[i]]), comp_id);
if (retval != 0)
{
rtapi_app_exit();
return retval;
}
//feedback position pin
rtapi_snprintf(buf, HAL_NAME_LEN, "miniemcdrv.%d.fb-pos", axis_map[i]);
retval = hal_pin_float_new(buf, HAL_OUT, &(pgpio->fb_pos[axis_map[i]]), comp_id);
if (retval != 0)
{
rtapi_app_exit();
return retval;
}
}
/* export functions */
retval = hal_export_funct("update-miniemcdrv", update,
pgpio, 0, 0, comp_id);
if (retval != 0)
{
rtapi_print_msg(RTAPI_MSG_ERR,
"miniemcdrv: ERROR: count funct export failed\n");
rtapi_app_exit();
return -EIO;
}
ioctl(pgpio->fd, SCAN_PIN_SETUP_IOCTL, NULL);
//emcConfigurePin(11, egpOut );
//emcSetPin(11, 1 );
hal_ready(comp_id);
return 0;
}
int rtapi_app_main(void) {
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 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 = 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)
{
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;
}
static int export_siggen(int num, hal_siggen_t * addr)
{
int retval;
char buf[HAL_NAME_LEN + 2];
/* export pins */
rtapi_snprintf(buf, HAL_NAME_LEN, "siggen.%d.square", num);
retval = hal_pin_float_new(buf, HAL_OUT, &(addr->square), comp_id);
if (retval != 0) {
return retval;
}
rtapi_snprintf(buf, HAL_NAME_LEN, "siggen.%d.sawtooth", num);
retval = hal_pin_float_new(buf, HAL_OUT, &(addr->sawtooth), comp_id);
if (retval != 0) {
return retval;
}
rtapi_snprintf(buf, HAL_NAME_LEN, "siggen.%d.triangle", num);
retval = hal_pin_float_new(buf, HAL_OUT, &(addr->triangle), comp_id);
if (retval != 0) {
return retval;
}
rtapi_snprintf(buf, HAL_NAME_LEN, "siggen.%d.sine", num);
retval = hal_pin_float_new(buf, HAL_OUT, &(addr->sine), comp_id);
if (retval != 0) {
return retval;
}
rtapi_snprintf(buf, HAL_NAME_LEN, "siggen.%d.cosine", num);
retval = hal_pin_float_new(buf, HAL_OUT, &(addr->cosine), comp_id);
if (retval != 0) {
return retval;
}
rtapi_snprintf(buf, HAL_NAME_LEN, "siggen.%d.frequency", num);
retval = hal_pin_float_new(buf, HAL_IN, &(addr->frequency), comp_id);
if (retval != 0) {
return retval;
}
rtapi_snprintf(buf, HAL_NAME_LEN, "siggen.%d.amplitude", num);
retval = hal_pin_float_new(buf, HAL_IN, &(addr->amplitude), comp_id);
if (retval != 0) {
return retval;
}
rtapi_snprintf(buf, HAL_NAME_LEN, "siggen.%d.offset", num);
retval = hal_pin_float_new(buf, HAL_IN, &(addr->offset), comp_id);
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->frequency) = 1.0;
*(addr->amplitude) = 1.0;
*(addr->offset) = 0.0;
addr->index = 0.0;
/* export function for this loop */
rtapi_snprintf(buf, HAL_NAME_LEN, "siggen.%d.update", num);
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;
}
static int init_streamer(int num, streamer_t *str)
{
int retval, n, usefp;
pin_data_t *pptr;
char buf[HAL_NAME_LEN + 1];
/* export "standard" pins and params */
retval = hal_pin_bit_newf(HAL_OUT, &(str->empty), comp_id,
"streamer.%d.empty", num);
if (retval != 0 ) {
rtapi_print_msg(RTAPI_MSG_ERR,
"STREAMER: ERROR: 'empty' pin export failed\n");
return -EIO;
}
retval = hal_pin_bit_newf(HAL_IN, &(str->enable), comp_id,
"streamer.%d.enable", num);
if (retval != 0 ) {
rtapi_print_msg(RTAPI_MSG_ERR,
"STREAMER: ERROR: 'enable' pin export failed\n");
return -EIO;
}
retval = hal_pin_s32_newf(HAL_OUT, &(str->curr_depth), comp_id,
"streamer.%d.curr-depth", num);
if (retval != 0 ) {
rtapi_print_msg(RTAPI_MSG_ERR,
"STREAMER: ERROR: 'curr_depth' pin export failed\n");
return -EIO;
}
retval = hal_pin_s32_newf(HAL_IO, &(str->underruns), comp_id,
"streamer.%d.underruns", num);
if (retval != 0 ) {
rtapi_print_msg(RTAPI_MSG_ERR,
"STREAMER: ERROR: 'underruns' pin export failed\n");
return -EIO;
}
retval = hal_pin_bit_newf(HAL_IN, &(str->clock), comp_id,
"streamer.%d.clock", num);
if (retval != 0 ) {
rtapi_print_msg(RTAPI_MSG_ERR,
"STREAMER: ERROR: 'clock' pin export failed\n");
return -EIO;
}
retval = hal_pin_s32_newf(HAL_IN, &(str->clock_mode), comp_id,
"streamer.%d.clock-mode", num);
if (retval != 0 ) {
rtapi_print_msg(RTAPI_MSG_ERR,
"STREAMER: ERROR: 'clock_mode' pin export failed\n");
return -EIO;
}
/* init the standard pins and params */
*(str->empty) = 1;
*(str->enable) = 1;
*(str->curr_depth) = 0;
*(str->underruns) = 0;
*(str->clock_mode) = 0;
pptr = str->pins;
usefp = 0;
/* export user specified pins (the ones that stream data) */
for ( n = 0 ; n < hal_stream_element_count(&str->fifo); n++ ) {
rtapi_snprintf(buf, sizeof(buf), "streamer.%d.pin.%d", num, n);
retval = hal_pin_new(buf, hal_stream_element_type(&str->fifo, n), HAL_OUT, (void **)pptr, comp_id );
if (retval != 0 ) {
rtapi_print_msg(RTAPI_MSG_ERR,
"STREAMER: ERROR: pin '%s' export failed\n", buf);
return -EIO;
}
/* init the pin value */
switch ( hal_stream_element_type(&str->fifo, n) ) {
case HAL_FLOAT:
*(pptr->hfloat) = 0.0;
usefp = 1;
break;
case HAL_BIT:
*(pptr->hbit) = 0;
break;
case HAL_U32:
*(pptr->hu32) = 0;
break;
case HAL_S32:
*(pptr->hs32) = 0;
break;
default:
break;
}
pptr++;
}
/* export update function */
rtapi_snprintf(buf, sizeof(buf), "streamer.%d", num);
retval = hal_export_funct(buf, update, str, usefp, 0, comp_id);
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"STREAMER: ERROR: function export failed\n");
return retval;
}
return 0;
}
int rtapi_app_main(void)
{
int n, retval;
counter_t *cntr;
/* test for number of channels */
if ((num_chan <= 0) || (num_chan > MAX_CHAN)) {
rtapi_print_msg(RTAPI_MSG_ERR,
"ENCODER: ERROR: invalid num_chan: %d\n", num_chan);
return -1;
}
/* have good config info, connect to the HAL */
comp_id = hal_init("encoder");
if (comp_id < 0) {
rtapi_print_msg(RTAPI_MSG_ERR, "ENCODER: ERROR: hal_init() failed\n");
return -1;
}
/* allocate shared memory for counter data */
counter_array = hal_malloc(num_chan * sizeof(counter_t));
if (counter_array == 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"ENCODER: ERROR: hal_malloc() failed\n");
hal_exit(comp_id);
return -1;
}
/* init master timestamp counter */
timebase = 0;
/* export all the variables for each counter */
for (n = 0; n < num_chan; n++) {
/* point to struct */
cntr = &(counter_array[n]);
/* export all vars */
retval = export_counter(n, cntr);
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"ENCODER: ERROR: counter %d var export failed\n", n);
hal_exit(comp_id);
return -1;
}
/* init counter */
cntr->state = 0;
cntr->oldZ = 0;
cntr->Zmask = 0;
*(cntr->x4_mode) = 1;
*(cntr->counter_mode) = 0;
*(cntr->latch_rising) = 1;
*(cntr->latch_falling) = 1;
cntr->buf[0].count_detected = 0;
cntr->buf[1].count_detected = 0;
cntr->buf[0].index_detected = 0;
cntr->buf[1].index_detected = 0;
cntr->bp = &(cntr->buf[0]);
*(cntr->raw_counts) = 0;
cntr->raw_count = 0;
cntr->timestamp = 0;
cntr->index_count = 0;
cntr->latch_count = 0;
*(cntr->count) = 0;
*(cntr->min_speed) = 1.0;
*(cntr->pos) = 0.0;
*(cntr->pos_latch) = 0.0;
*(cntr->vel) = 0.0;
*(cntr->pos_scale) = 1.0;
cntr->old_scale = 1.0;
cntr->scale = 1.0;
cntr->counts_since_timeout = 0;
}
/* export functions */
retval = hal_export_funct("encoder.update-counters", update,
counter_array, 0, 0, comp_id);
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"ENCODER: ERROR: count funct export failed\n");
hal_exit(comp_id);
return -1;
}
retval = hal_export_funct("encoder.capture-position", capture,
counter_array, 1, 0, comp_id);
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"ENCODER: ERROR: capture funct export failed\n");
hal_exit(comp_id);
return -1;
}
rtapi_print_msg(RTAPI_MSG_INFO,
"ENCODER: installed %d encoder counters\n", num_chan);
hal_ready(comp_id);
return 0;
}
/**
\brief main realtime task */
int rtapi_app_main(void)
{
// zynq and FPGA code revision
int rev, zrev, n;
// save messaging level
static int msg_level;
int retval = 0;
int fdmisc;
// save message level on entering
msg_level = rtapi_get_msg_level();
/* setup messaging level in:
RTAPI_MSG_NONE, RTAPI_MSG_ERR, RTAPI_MSG_WARN,
RTAPI_MSG_INFO, RTAPI_MSG_DBG, RTAPI_MSG_ALL */
rtapi_set_msg_level(RTAPI_MSG_ALL);
// check Zynq revision
if ((zrev = zynq_revision()) < 0) {
// unable to determine zynq revision
rtapi_print_msg(RTAPI_MSG_ERR, "HAL_ZED_CAN: ERROR: unable to determine zynq revision");
hal_exit(comp_id);
return -1;
}
// notify zynq revision
rtapi_print_msg(RTAPI_MSG_INFO, "HAL_ZED_CAN: Zynq Revision %d \n", zrev);
// check Zedboard FPGA hardware revision
rev = zb_revision();
// do revision specific configuration
switch (rev) {
case 01:
rtapi_print_msg(RTAPI_MSG_INFO, "HAL_ZED_CAN: Zedboard FPGA Revision 01\n");
break;
default:
rtapi_print_msg(RTAPI_MSG_ERR, "HAL_ZED_CAN: ERROR: FPGA revision %d not (yet) supported\n", rev);
return -1;
break;
}
// open and map miscellaneous peripheral for PMOD IO access
fdmisc = open("/dev/mem", O_RDWR);
map_MiscAddress = (unsigned int) mmap(NULL, 100, PROT_READ | PROT_WRITE, MAP_SHARED, fdmisc, (off_t)MISC_ADDR );
if (-1 == map_MiscAddress ) {
fprintf(stderr, "MMap failed to map Miscellaneus peripheral\n");
return 0;
}
printf("Map Misc peripheral: virtual 0x%x real 0x%x \n", map_MiscAddress, MISC_ADDR);
// parse module parameters in order to find the number
// of configured FOC channels and their CAN address/configuration
for(n = 0; n < MAX_FOC_CHAN && FOC_axis[n] != -1 ; n++) {
// check for a valid CAN address
if( (FOC_axis[n] <= 0) || ( FOC_axis[n] > 15) ) {
rtapi_print_msg(RTAPI_MSG_ERR, "HAL_ZED_CAN: ERROR: bad CAN address '%i', axis %i", FOC_axis[n], n);
hal_exit(comp_id);
return -1;
}
// check the control mode configuration
if( INVALID == parse_ctrl_type(ctrl_type[n])) {
rtapi_print_msg(RTAPI_MSG_ERR,"HAL_ZED_CAN: ERROR: bad control type '%s' for axis %i ('c','j','v','i','t')",
ctrl_type[n], n);
hal_exit(comp_id);
return -1;
}
// found a correctly configured channel
num_chan++;
// notify
rtapi_print_msg(RTAPI_MSG_INFO, "HAL_ZED_CAN: FOC axis %d with CAN address %d.",n, FOC_axis[n] );
rtapi_print_msg(RTAPI_MSG_INFO, "HAL_ZED_CAN: Motor gear %d, Screw gear %d, Screw ratio %d Encoder PPR %d.", Screw_gear[n], Motor_gear[n], Screw_ratio[n], PPR[n]);
}
if( (0 == num_chan) || (8 < num_chan) ) {
rtapi_print_msg(RTAPI_MSG_ERR, "HAL_ZED_CAN: ERROR: channels configured incorrectly.");
hal_exit(comp_id);
return -1;
}
// allocate shared memory for FOC_data of each axis
FOC_data_array = hal_malloc(num_chan * sizeof(FOC_data_t));
if ( 0 == FOC_data_array ) {
rtapi_print_msg(RTAPI_MSG_ERR, "HAL_ZED_CAN: ERROR: hal_malloc() failed\n");
hal_exit(comp_id);
return -1;
}
/** \note CAN8/2FOC connection is 1 axis for each CAN channel */
// configure CAN communication
for(n = 0; n < num_chan ; n++) {
if ( 0 != setup_CAN(n) ) {
hal_exit(comp_id);
return -1;
}
}
// try to init the component
comp_id = hal_init("hal_zed_can");
if( comp_id < 0 ) {
rtapi_print_msg(RTAPI_MSG_ERR, "HAL_ZED_CAN: ERROR: hal_init() failed\n");
hal_exit(comp_id);
return -1;
}
/* Export the variables/parameters for each FOC axis */
for (n = 0; n < num_chan; n++) {
// export pins/params for the n^th component
retval = exportFOCaxis(n, &(FOC_data_array[n]) );
if( retval < 0 ) {
rtapi_print_msg(RTAPI_MSG_ERR, "HAL_ZED_CAN: ERROR: exportFOCaxis() failed");
hal_exit(comp_id);
return -1;
}
}
// export the read_feedback and send_setpoint functions as hal_zed_can.send_setpoint and hal_zed_can.read_feedback in hal
retval = hal_export_funct("hal_zed_can.send_setpoint", send_setpoint, FOC_data_array, 0, 0, comp_id);
if (retval < 0) {
rtapi_print_msg(RTAPI_MSG_ERR, "HAL_ZED_CAN: ERROR: write funct export failed\n");
hal_exit(comp_id);
return -1;
}
retval = hal_export_funct("hal_zed_can.read_feedback", read_feedback, FOC_data_array, 0, 0, comp_id);
if (retval < 0) {
rtapi_print_msg(RTAPI_MSG_ERR, "HAL_ZED_CAN: ERROR: read funct export failed\n");
hal_exit(comp_id);
return -1;
}
// activate periodic communication on all axis
setup_2FOC_periodic();
rtapi_print_msg(RTAPI_MSG_INFO, "HAL_ZED_CAN: FOC periodic data exchange active.");
// all operations succeded
rtapi_print_msg(RTAPI_MSG_INFO, "HAL_ZED_CAN: driver installed successfully.\n");
hal_ready(comp_id);
// return to previous mesaging level
rtapi_set_msg_level(msg_level);
return 0;
}
int rtapi_app_main(void)
{
int n, retval, i;
eqep_t *eqep;
/* test for number of channels */
for (howmany=0; howmany < MAX_ENC && encoders[howmany]; howmany++) ;
if(howmany <= 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"%s: ERROR: invalid number of encoders: %d\n", modname, howmany);
return -1;
}
/* have good config info, connect to the HAL */
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 shared memory for eqep data */
eqep_array = hal_malloc(howmany * sizeof(eqep_t));
if (eqep_array == 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"%s: ERROR: hal_malloc() failed\n",modname);
hal_exit(comp_id);
return -1;
}
timebase = 0;
/* setup and export all the variables for each eQEP device */
for (n = 0; n < howmany; n++) {
eqep = &(eqep_array[n]);
/* make sure it's a valid device */
for(i=0; devices[i].name; i++) {
retval = strcmp(encoders[n], devices[i].name);
if (retval == 0 ) {
eqep->name = devices[i].name;
int fd = open("/dev/mem", O_RDWR);
eqep->pwm_reg = mmap(0, IOMEMLEN, PROT_READ | PROT_WRITE, MAP_SHARED,
fd, devices[i].addr);
eqep->eqep_reg = (void*) eqep->pwm_reg + 0x180;
close(fd);
if(eqep->pwm_reg == MAP_FAILED) {
rtapi_print_msg(RTAPI_MSG_ERR,
"%s: ERROR: mmap failed %s\n", modname, eqep->name);
return -1;
}
rtapi_print("memmapped %s to %p and %p\n",eqep->name,eqep->pwm_reg,eqep->eqep_reg);
setup_eQEP(eqep);
break;
}
}
if(retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, "%s: ERROR: unknown device %s\n",
modname, encoders[n]);
return -1;
}
}
/* export functions */
retval = hal_export_funct("eqep.update", update,
eqep_array, 0, 0, comp_id);
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"%s: ERROR: function export failed\n",modname);
hal_exit(comp_id);
return -1;
}
rtapi_print_msg(RTAPI_MSG_INFO,
"%s: installed %d encoder counters\n", modname, howmany);
retval = hal_ready(comp_id);
return 0;
}
int export_gpio(__u8 **ppcfg, board_data_t *board)
{
__u8 *data;
int retval, export_out, export_oe;
int portnum, boardnum, pin;
char portchar;
gpio_t *gpio, **p;
int code, addr;
__u8 *cfg_bytes;
char name[HAL_NAME_LEN + 2];
__u32 mode0, mode1, source0, source1, source2, polarity, mask;
/* read and validate config data */
data = *ppcfg;
code = data[0];
addr = (data[1] << 8) + data[2];
cfg_bytes = &(data[3]);
/* return ptr to next block */
*ppcfg = &(cfg_bytes[24]);
/* Allocate HAL memory */
gpio = (gpio_t *)(hal_malloc(sizeof(gpio_t)));
if ( gpio == NULL ) {
rtapi_print_msg(RTAPI_MSG_ERR, "5i2x: ERROR: hal_malloc() failed\n");
return -1;
}
/* find end of linked list */
boardnum = board->num;
portnum = 0;
p = &(board->gpio);
while ( *p != NULL ) {
p = &((*p)->next);
portnum++;
}
portchar = 'A' + portnum;
/* add to end of list */
*p = gpio;
gpio->next = NULL;
/* save base address */
gpio->addr = board->base + addr;
mode0 = 0;
mode1 = 0;
source0 = 0;
source1 = 0;
source2 = 0;
polarity = 0;
mask = 1;
for ( pin = 0 ; pin < PINS_PER_PORT ; pin++ ) {
if ( cfg_bytes[pin] & HAL_INPUT_PIN_MASK ) {
/* export output HAL pins for the input bit */
retval = hal_pin_bit_newf(HAL_OUT, &(gpio->in[pin]),
comp_id, "5i20.%d.pin-%c%02d-in", boardnum, portchar, pin);
if (retval != 0) return retval;
retval = hal_pin_bit_newf(HAL_OUT, &(gpio->in_not[pin]),
comp_id, "5i20.%d.pin-%c%02d-in-not", boardnum, portchar, pin);
if (retval != 0) return retval;
/* initial values for pins */
*(gpio->in[pin]) = 0;
*(gpio->in_not[pin]) = 1;
} else {
/* input pins not used, point them at a dummy */
gpio->in[pin] = &dummy_in;
gpio->in_not[pin] = &dummy_in;
}
export_out = 0;
export_oe = 0;
if ( (cfg_bytes[pin] & SOURCE_MASK) == 0 ) {
switch ( cfg_bytes[pin] & MODE_MASK ) {
case MODE_TRI_STATE:
export_oe = 1;
case MODE_OUTPUT_ONLY:
case MODE_OPEN_COLLECTOR:
export_out = 1;
case MODE_INPUT_ONLY:
default:
break;
}
}
if ( export_out ) {
/* export input HAL pin for the output bit */
retval = hal_pin_bit_newf(HAL_IN, &(gpio->out[pin]),
comp_id, "5i20.%d.pin-%c%02d-out", boardnum, portchar, pin);
if (retval != 0) return retval;
/* export HAL param for inversion */
retval = hal_param_bit_newf(HAL_RW, &(gpio->out_invert[pin]),
comp_id, "5i20.%d.pin-%c%02d-invert", boardnum, portchar, pin);
if (retval != 0) return retval;
/* set initial value for pin and param */
*(gpio->out[pin]) = 0;
gpio->out_invert[pin] = 0;
} else {
/* output pin not used, point at a dummy */
gpio->out[pin] = &dummy_out;
}
if ( export_oe ) {
/* export input HAL pin for the output enable bit */
retval = hal_pin_bit_newf(HAL_IN, &(gpio->out_ena[pin]),
comp_id, "5i20.%d.pin-%c%02d-out-en", boardnum, portchar, pin);
if (retval != 0) return retval;
/* set initial value for pin */
*(gpio->out_ena[pin]) = 0;
} else {
/* output enable pin not used, point at a dummy */
gpio->out_ena[pin] = &dummy_oe;
}
/* set appropriate bits in config register words */
if ( cfg_bytes[pin] & SOURCE2_MASK ) source2 |= mask;
if ( cfg_bytes[pin] & SOURCE1_MASK ) source1 |= mask;
if ( cfg_bytes[pin] & SOURCE0_MASK ) source0 |= mask;
if ( cfg_bytes[pin] & MODE1_MASK ) mode1 |= mask;
if ( cfg_bytes[pin] & MODE0_MASK ) mode0 |= mask;
if ( cfg_bytes[pin] & POLARITY_MASK ) polarity |= mask;
mask <<= 1;
}
/* write config to registers */
iowrite32(source2, gpio->addr+GPIO_SOURCE1);
iowrite32(source1, gpio->addr+GPIO_SOURCE1);
iowrite32(source0, gpio->addr+GPIO_SOURCE0);
iowrite32(mode1, gpio->addr+GPIO_MODE1);
iowrite32(mode0, gpio->addr+GPIO_MODE0);
iowrite32(polarity, gpio->addr+GPIO_POLARITY);
/* export functions - one funct serves all ports */
if ( portnum > 0 ) {
/* already exported */
return 0;
}
rtapi_snprintf(name, HAL_NAME_LEN, "5i20.%d.gpio.read", boardnum);
retval = hal_export_funct(name, read_gpios, gpio, 0, 0, comp_id);
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"5i20: ERROR: board %d GPIO read funct export failed\n",
boardnum);
return -1;
}
rtapi_snprintf(name, HAL_NAME_LEN, "5i20.%d.gpio.write", boardnum);
retval = hal_export_funct(name, write_gpios, gpio, 0, 0, comp_id);
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"5i20: ERROR: board %d GPIO write funct export failed\n",
boardnum);
return -1;
}
return 0;
}
