int rtapi_app_main(void) { int retval = 0; comp_id = hal_init("lineardeltakins"); if(comp_id < 0) retval = comp_id; if(retval == 0) { haldata = hal_malloc(sizeof(struct haldata)); retval = !haldata; } if(retval == 0) retval = hal_pin_float_newf(HAL_IN, &haldata->r, comp_id, "lineardeltakins.R"); if(retval == 0) retval = hal_pin_float_newf(HAL_IN, &haldata->l, comp_id, "lineardeltakins.L"); if(retval == 0) { *haldata->r = DELTA_RADIUS; *haldata->l = DELTA_DIAGONAL_ROD; } if(retval == 0) { hal_ready(comp_id); } return retval; }
/** \brief Export pins for component(s) \param num: component number addr: pointer to array of the num^th FOC channel data */ static int exportFOCaxis(int num, FOC_data_t * addr) { int retval = 0; // // PINS // // make available setpoint in hal if ( (retval = hal_pin_float_newf(HAL_IN, &(addr->setpoint), comp_id, "hal_zed_can.%d.setpoint", num) ) < 0) { rtapi_print_msg(RTAPI_MSG_ERR, "HAL_ZED_CAN: ERROR: pin setpoint export failed with err=%d", retval); hal_exit(comp_id); return -1; } // make available position feedback in hal if ( (retval = hal_pin_float_newf(HAL_OUT, &(addr->feedback), comp_id, "hal_zed_can.%d.feedback", num) ) < 0) { rtapi_print_msg(RTAPI_MSG_ERR, "HAL_ZED_CAN: ERROR: pin feedback export failed with err=%d", retval); hal_exit(comp_id); return -1; } // // PARAMS // // instantiate a parameter retval = hal_param_float_newf(HAL_RW, &(addr->dbg), comp_id, "hal_zed_can.%d.debug", num); // check for failed debug space mapping if(retval != 0) { rtapi_print_msg(RTAPI_MSG_ERR, "HAL_ZED_CAN: ERROR: param debug export failed with err=%d", retval); hal_exit(comp_id); return -1; } // instantiate parameter retval = hal_pin_u32_newf(HAL_OUT, &(addr->focstatus), comp_id, "hal_zed_can.%d.status", num); // check for failed debug space mapping if(retval != 0) { rtapi_print_msg(RTAPI_MSG_ERR, "HAL_ZED_CAN: ERROR: pin status export failed with err=%d" ,retval); hal_exit(comp_id); return -1; } // instantiate parameter retval = hal_pin_u32_newf(HAL_OUT, &(addr->focerror), comp_id, "hal_zed_can.%d.error", num); // check for failed debug space mapping if(retval != 0) { rtapi_print_msg(RTAPI_MSG_ERR, "HAL_ZED_CAN: ERROR: param error export failed with err=%d",retval); hal_exit(comp_id); return -1; } // completed successfully rtapi_print_msg(RTAPI_MSG_INFO, "HAL_ZED_CAN: exportFOCaxis(%d) completed successfully.\n", num); return 0; }
int export_pwmgen(hal_pru_generic_t *hpg, int i) { int r, j; // HAL values common to all outputs in this instance r = hal_pin_u32_newf(HAL_IN, &(hpg->pwmgen.instance[i].hal.pin.pwm_period), hpg->config.comp_id, "%s.pwmgen.%02d.pwm_period", hpg->config.halname, i); if (r != 0) { return r; } *(hpg->pwmgen.instance[i].hal.pin.pwm_period) = 10000000; // Default to 10 mS period, or 100 Hz for (j=0; j < hpg->pwmgen.instance[i].num_outputs; j++) { // Export HAL Pins r = hal_pin_bit_newf(HAL_IN, &(hpg->pwmgen.instance[i].out[j].hal.pin.enable), hpg->config.comp_id, "%s.pwmgen.%02d.out.%02d.enable", hpg->config.halname, i, j); if (r != 0) { HPG_ERR("pwmgen %02d out %02d: error adding pin 'enable', aborting\n", i, j); return r; } r = hal_pin_float_newf(HAL_IN, &(hpg->pwmgen.instance[i].out[j].hal.pin.value), hpg->config.comp_id, "%s.pwmgen.%02d.out.%02d.value", hpg->config.halname, i, j); if (r != 0) { HPG_ERR("pwmgen %02d out %02d: error adding pin 'value', aborting\n", i, j); return r; } r = hal_pin_float_newf(HAL_IN, &(hpg->pwmgen.instance[i].out[j].hal.pin.scale), hpg->config.comp_id, "%s.pwmgen.%02d.out.%02d.scale", hpg->config.halname, i, j); if (r != 0) { HPG_ERR("pwmgen %02d out %02d: error adding pin 'scale', aborting\n", i, j); return r; } r = hal_pin_u32_newf(HAL_IN, &(hpg->pwmgen.instance[i].out[j].hal.pin.pin), hpg->config.comp_id, "%s.pwmgen.%02d.out.%02d.pin", hpg->config.halname, i, j); if (r != 0) { HPG_ERR("pwmgen %02d out %02d: error adding pin 'pin', aborting\n", i, j); return r; } // Initialize HAL Pins *(hpg->pwmgen.instance[i].out[j].hal.pin.enable) = 0; *(hpg->pwmgen.instance[i].out[j].hal.pin.value) = 0.0; *(hpg->pwmgen.instance[i].out[j].hal.pin.pin) = PRU_DEFAULT_PIN; *(hpg->pwmgen.instance[i].out[j].hal.pin.scale) = 1.0; } return 0; }
// init HAL objects static int export_halobjs(struct inst_data *ip, int owner_id, const char *name) { if (hal_pin_float_newf(HAL_OUT, &ip->out, owner_id, "%s.out", name)) return -1; if (hal_pin_float_newf(HAL_IN, &ip->in, owner_id, "%s.in", name)) return -1; if (hal_param_s32_newf(HAL_RO, &ip->iter,owner_id, "%s.iter", name)) return -1; // unittest observer pins, per instance if (hal_pin_s32_newf(HAL_OUT, &ip->repeat_value, owner_id, "%s.repeat_value", name)) return -1; if (hal_pin_s32_newf(HAL_OUT, &ip->prefix_len, owner_id, "%s.prefix_len", name)) return -1; // export a thread function, passing the pointer to the instance's HAL memory blob if (hal_export_functf(funct, ip, 0, 0, owner_id, "%s.funct", name)) return -1; return 0; }
int rtapi_app_main(void) { int retval = 0; comp_id = hal_init("rotarydeltakins"); if(comp_id < 0) retval = comp_id; if(retval == 0) { haldata = hal_malloc(sizeof(struct haldata)); retval = !haldata; } if(retval == 0) retval = hal_pin_float_newf(HAL_IN, &haldata->pfr, comp_id, "rotarydeltakins.platformradius"); if(retval == 0) retval = hal_pin_float_newf(HAL_IN, &haldata->tl, comp_id, "rotarydeltakins.thighlength"); if(retval == 0) retval = hal_pin_float_newf(HAL_IN, &haldata->sl, comp_id, "rotarydeltakins.shinlength"); if(retval == 0) retval = hal_pin_float_newf(HAL_IN, &haldata->fr, comp_id, "rotarydeltakins.footradius"); if(retval == 0) { *haldata->pfr = RDELTA_PFR; *haldata->tl = RDELTA_TL; *haldata->sl = RDELTA_SL; *haldata->fr = RDELTA_FR; } if(retval == 0) { hal_ready(comp_id); } return retval; }
static int export_counter(int num, counter_t * addr) { int retval, msg; /* This function exports a lot of stuff, which results in a lot of logging if msg_level is at INFO or ALL. So we save the current value of msg_level and restore it later. If you actually need to log this function's actions, change the second line below */ msg = rtapi_get_msg_level(); rtapi_set_msg_level(RTAPI_MSG_WARN); /* export pins for the quadrature inputs */ retval = hal_pin_bit_newf(HAL_IN, &(addr->phaseA), comp_id, "encoder.%d.phase-A", num); if (retval != 0) { return retval; } retval = hal_pin_bit_newf(HAL_IN, &(addr->phaseB), comp_id, "encoder.%d.phase-B", num); if (retval != 0) { return retval; } /* export pin for the index input */ retval = hal_pin_bit_newf(HAL_IN, &(addr->phaseZ), comp_id, "encoder.%d.phase-Z", num); if (retval != 0) { return retval; } /* export pin for the index enable input */ retval = hal_pin_bit_newf(HAL_IO, &(addr->index_ena), comp_id, "encoder.%d.index-enable", num); if (retval != 0) { return retval; } /* export pin for the reset input */ retval = hal_pin_bit_newf(HAL_IN, &(addr->reset), comp_id, "encoder.%d.reset", num); if (retval != 0) { return retval; } /* export pins for position latching */ retval = hal_pin_bit_newf(HAL_IN, &(addr->latch_in), comp_id, "encoder.%d.latch-input", num); if (retval != 0) { return retval; } retval = hal_pin_bit_newf(HAL_IN, &(addr->latch_rising), comp_id, "encoder.%d.latch-rising", num); if (retval != 0) { return retval; } retval = hal_pin_bit_newf(HAL_IN, &(addr->latch_falling), comp_id, "encoder.%d.latch-falling", num); if (retval != 0) { return retval; } /* export parameter for raw counts */ retval = hal_pin_s32_newf(HAL_OUT, &(addr->raw_counts), comp_id, "encoder.%d.rawcounts", num); if (retval != 0) { return retval; } /* export pin for counts captured by capture() */ retval = hal_pin_s32_newf(HAL_OUT, &(addr->count), comp_id, "encoder.%d.counts", num); if (retval != 0) { return retval; } /* export pin for counts latched by capture() */ retval = hal_pin_s32_newf(HAL_OUT, &(addr->count_latch), comp_id, "encoder.%d.counts-latched", num); if (retval != 0) { return retval; } /* export pin for minimum speed estimated by capture() */ retval = hal_pin_float_newf(HAL_IN, &(addr->min_speed), comp_id, "encoder.%d.min-speed-estimate", num); if (retval != 0) { return retval; } /* export pin for scaled position captured by capture() */ retval = hal_pin_float_newf(HAL_OUT, &(addr->pos), comp_id, "encoder.%d.position", num); if (retval != 0) { return retval; } /* export pin for scaled and interpolated position captured by capture() */ retval = hal_pin_float_newf(HAL_OUT, &(addr->pos_interp), comp_id, "encoder.%d.position-interpolated", num); if (retval != 0) { return retval; } /* export pin for latched position captured by capture() */ retval = hal_pin_float_newf(HAL_OUT, &(addr->pos_latch), comp_id, "encoder.%d.position-latched", num); if (retval != 0) { return retval; } /* export pin for scaled velocity captured by capture() */ retval = hal_pin_float_newf(HAL_OUT, &(addr->vel), comp_id, "encoder.%d.velocity", num); if (retval != 0) { return retval; } /* export pin for scaling */ retval = hal_pin_float_newf(HAL_IO, &(addr->pos_scale), comp_id, "encoder.%d.position-scale", num); if (retval != 0) { return retval; } /* export pin for x4 mode */ retval = hal_pin_bit_newf(HAL_IO, &(addr->x4_mode), comp_id, "encoder.%d.x4-mode", num); if (retval != 0) { return retval; } /* export pin for counter mode */ retval = hal_pin_bit_newf(HAL_IO, &(addr->counter_mode), comp_id, "encoder.%d.counter-mode", num); if (retval != 0) { return retval; } /* restore saved message level */ rtapi_set_msg_level(msg); 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; }
int lcec_el7342_init(int comp_id, struct lcec_slave *slave, ec_pdo_entry_reg_t *pdo_entry_regs) { lcec_master_t *master = slave->master; lcec_el7342_data_t *hal_data; int i; lcec_el7342_chan_t *chan; uint8_t info1_select, info2_select; int err; // initialize callbacks slave->proc_read = lcec_el7342_read; slave->proc_write = lcec_el7342_write; // alloc hal memory if ((hal_data = hal_malloc(sizeof(lcec_el7342_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_el7342_data_t)); slave->hal_data = hal_data; // initialize sync info slave->sync_info = lcec_el7342_syncs; // initialize global data hal_data->last_operational = 0; // initialize pins for (i=0; i<LCEC_EL7342_CHANS; i++) { chan = &hal_data->chans[i]; // read sdos // Info1 selector if ((chan->sdo_info1_select = lcec_read_sdo(slave, 0x8022 + (i << 4), 0x11, 1)) == NULL) { return -EIO; } info1_select = EC_READ_U8(ecrt_sdo_request_data(chan->sdo_info1_select)); // Info2 selector if ((chan->sdo_info2_select = lcec_read_sdo(slave, 0x8022 + (i << 4), 0x19, 1)) == NULL) { return -EIO; } info2_select = EC_READ_U8(ecrt_sdo_request_data(chan->sdo_info2_select)); // initialize POD entries LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6000 + (i << 4), 0x02, &chan->latch_ext_valid_pdo_os, &chan->latch_ext_valid_pdo_bp); 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), 0x04, &chan->count_overflow_pdo_os, &chan->count_overflow_pdo_bp); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6000 + (i << 4), 0x05, &chan->count_underflow_pdo_os, &chan->count_underflow_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, 0x6000 + (i << 4), 0x0d, &chan->inext_pdo_os, &chan->inext_pdo_bp); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x1c32 , 0x20, &chan->sync_err_pdo_os, &chan->sync_err_pdo_bp); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x1800 + (i * 3), 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), 0x12, &chan->latch_pdo_os, NULL); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x7000 + (i << 4), 0x02, &chan->ena_latch_ext_pos_pdo_os, &chan->ena_latch_ext_pos_pdo_bp); 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), 0x04, &chan->ena_latch_ext_neg_pdo_os, &chan->ena_latch_ext_neg_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); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6020 + (i << 4), 0x01, &chan->dcm_ready_to_enable_pdo_os, &chan->dcm_ready_to_enable_pdo_bp); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6020 + (i << 4), 0x02, &chan->dcm_ready_pdo_os, &chan->dcm_ready_pdo_bp); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6020 + (i << 4), 0x03, &chan->dcm_warning_pdo_os, &chan->dcm_warning_pdo_bp); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6020 + (i << 4), 0x04, &chan->dcm_error_pdo_os, &chan->dcm_error_pdo_bp); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6020 + (i << 4), 0x05, &chan->dcm_move_pos_pdo_os, &chan->dcm_move_pos_pdo_bp); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6020 + (i << 4), 0x06, &chan->dcm_move_neg_pdo_os, &chan->dcm_move_neg_pdo_bp); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6020 + (i << 4), 0x07, &chan->dcm_torque_reduced_pdo_os, &chan->dcm_torque_reduced_pdo_bp); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6020 + (i << 4), 0x0c, &chan->dcm_din1_pdo_os, &chan->dcm_din1_pdo_bp); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6020 + (i << 4), 0x0d, &chan->dcm_din2_pdo_os, &chan->dcm_din2_pdo_bp); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x1c32 , 0x20, &chan->dcm_sync_err_pdo_os, &chan->dcm_sync_err_pdo_bp); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x1806 + (i << 1), 0x09, &chan->dcm_tx_toggle_pdo_os, &chan->dcm_tx_toggle_pdo_bp); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6020 + (i << 4), 0x11, &chan->dcm_info1_pdo_os, NULL); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6020 + (i << 4), 0x12, &chan->dcm_info2_pdo_os, NULL); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x7020 + (i << 4), 0x01, &chan->dcm_ena_pdo_os, &chan->dcm_ena_pdo_bp); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x7020 + (i << 4), 0x02, &chan->dcm_reset_pdo_os, &chan->dcm_reset_pdo_bp); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x7020 + (i << 4), 0x03, &chan->dcm_reduce_torque_pdo_os, &chan->dcm_reduce_torque_pdo_bp); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x7020 + (i << 4), 0x21, &chan->dcm_velo_pdo_os, NULL); // export encoder pins 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->inext), comp_id, "%s.%s.%s.enc-%d-inext", 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-inext failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_bit_newf(HAL_OUT, &(chan->sync_err), comp_id, "%s.%s.%s.enc-%d-sync-error", 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-sync-error 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_OUT, &(chan->count_overflow), comp_id, "%s.%s.%s.enc-%d-count-overflow", 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-overflow failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_bit_newf(HAL_OUT, &(chan->count_underflow), comp_id, "%s.%s.%s.enc-%d-count-underflow", 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-underflow failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_bit_newf(HAL_OUT, &(chan->latch_ext_valid), comp_id, "%s.%s.%s.enc-%d-latch-ext-valid", 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-latch-ext-valid 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_bit_newf(HAL_IO, &(chan->ena_latch_ext_pos), comp_id, "%s.%s.%s.enc-%d-index-ext-pos-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-ndex-ext-pos-enable failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_bit_newf(HAL_IO, &(chan->ena_latch_ext_neg), comp_id, "%s.%s.%s.enc-%d-index-ext-neg-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-ndex-ext-neg-enable 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_s32_newf(HAL_OUT, &(chan->raw_latch), comp_id, "%s.%s.%s.enc-%d-raw-latch", 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-latch 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_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; } // export servo pins if ((err = hal_pin_float_newf(HAL_IO, &(chan->dcm_scale), comp_id, "%s.%s.%s.srv-%d-scale", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-scale failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_float_newf(HAL_IO, &(chan->dcm_offset), comp_id, "%s.%s.%s.srv-%d-offset", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-offset failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_float_newf(HAL_IO, &(chan->dcm_min_dc), comp_id, "%s.%s.%s.srv-%d-min-dc", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-min-dc failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_float_newf(HAL_IO, &(chan->dcm_max_dc), comp_id, "%s.%s.%s.srv-%d-max-dc", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-max-dc failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_float_newf(HAL_OUT, &(chan->dcm_curr_dc), comp_id, "%s.%s.%s.srv-%d-curr-dc", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-curr-dc failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_bit_newf(HAL_IN, &(chan->dcm_enable), comp_id, "%s.%s.%s.srv-%d-enable", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-enable failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_bit_newf(HAL_IN, &(chan->dcm_absmode), comp_id, "%s.%s.%s.srv-%d-absmode", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-absmode failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_float_newf(HAL_IN, &(chan->dcm_value), comp_id, "%s.%s.%s.srv-%d-cmd", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-value failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_s32_newf(HAL_OUT, &(chan->dcm_raw_val), comp_id, "%s.%s.%s.srv-%d-raw-cmd", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-raw-cmd failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_bit_newf(HAL_IN, &(chan->dcm_reset), comp_id, "%s.%s.%s.srv-%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.srv-%d-reset failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_bit_newf(HAL_IN, &(chan->dcm_reduce_torque), comp_id, "%s.%s.%s.srv-%d-reduce-torque", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-reduce-torque failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_bit_newf(HAL_OUT, &(chan->dcm_ready_to_enable), comp_id, "%s.%s.%s.srv-%d-ready-to-enable", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-ready-to-enable%d- failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_bit_newf(HAL_OUT, &(chan->dcm_ready), comp_id, "%s.%s.%s.srv-%d-ready", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-ready%d- failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_bit_newf(HAL_OUT, &(chan->dcm_warning), comp_id, "%s.%s.%s.srv-%d-warning", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-warning failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_bit_newf(HAL_OUT, &(chan->dcm_error), comp_id, "%s.%s.%s.srv-%d-error", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-error failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_bit_newf(HAL_OUT, &(chan->dcm_move_pos), comp_id, "%s.%s.%s.srv-%d-move-pos", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-move-pos failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_bit_newf(HAL_OUT, &(chan->dcm_move_neg), comp_id, "%s.%s.%s.srv-%d-move-neg", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-move-neg failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_bit_newf(HAL_OUT, &(chan->dcm_torque_reduced), comp_id, "%s.%s.%s.srv-%d-torque-reduced", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-torque-reduced failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_bit_newf(HAL_OUT, &(chan->dcm_din1), comp_id, "%s.%s.%s.srv-%d-din1", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-din1 failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_bit_newf(HAL_OUT, &(chan->dcm_din2), comp_id, "%s.%s.%s.srv-%d-din2", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-din2 failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_bit_newf(HAL_OUT, &(chan->dcm_sync_err), comp_id, "%s.%s.%s.srv-%d-sync-error", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-sync-error failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_bit_newf(HAL_OUT, &(chan->dcm_tx_toggle), comp_id, "%s.%s.%s.srv-%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.srv-%d-tx-toggle failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_s32_newf(HAL_OUT, &(chan->dcm_raw_info1), comp_id, "%s.%s.%s.srv-%d-raw-info1", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-raw-info1 failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_s32_newf(HAL_OUT, &(chan->dcm_raw_info2), comp_id, "%s.%s.%s.srv-%d-raw-info2", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-raw-info2 failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_u32_newf(HAL_OUT, &(chan->dcm_sel_info1), comp_id, "%s.%s.%s.srv-%d-sel-info1", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-sel-info1 failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_u32_newf(HAL_OUT, &(chan->dcm_sel_info2), comp_id, "%s.%s.%s.srv-%d-sel-info2", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-sel-info2 failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if (info1_select == INFO_SEL_MOTOR_VELO || info2_select == INFO_SEL_MOTOR_VELO) { if ((err = hal_pin_float_newf(HAL_OUT, &(chan->dcm_velo_fb), comp_id, "%s.%s.%s.srv-%d-velo-fb", 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-velo-fb failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } } if (info1_select == INFO_SEL_MOTOR_CURR || info2_select == INFO_SEL_MOTOR_CURR) { if ((err = hal_pin_float_newf(HAL_OUT, &(chan->dcm_current_fb), comp_id, "%s.%s.%s.srv-%d-current-fb", 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-current-fb failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } } // initialize pins *(chan->reset) = 0; *(chan->ina) = 0; *(chan->inb) = 0; *(chan->inext) = 0; *(chan->sync_err) = 0; *(chan->expol_stall) = 0; *(chan->tx_toggle) = 0; *(chan->count_overflow) = 0; *(chan->count_underflow) = 0; *(chan->latch_ext_valid) = 0; *(chan->ena_latch_ext_pos) = 0; *(chan->ena_latch_ext_neg) = 0; *(chan->set_raw_count) = 0; *(chan->set_raw_count_val) = 0; *(chan->raw_count) = 0; *(chan->raw_latch) = 0; *(chan->count) = 0; *(chan->pos) = 0; *(chan->pos_scale) = 1.0; *(chan->dcm_scale) = 1.0; *(chan->dcm_offset) = 0.0; *(chan->dcm_min_dc) = -1.0; *(chan->dcm_max_dc) = 1.0; *(chan->dcm_curr_dc) = 0.0; *(chan->dcm_enable) = 0; *(chan->dcm_absmode) = 0; *(chan->dcm_value) = 0.0; *(chan->dcm_raw_val) = 0; *(chan->dcm_reset) = 0; *(chan->dcm_reduce_torque) = 0; *(chan->dcm_ready_to_enable) = 0; *(chan->dcm_ready) = 0; *(chan->dcm_warning) = 0; *(chan->dcm_error) = 0; *(chan->dcm_move_pos) = 0; *(chan->dcm_move_neg) = 0; *(chan->dcm_torque_reduced) = 0; *(chan->dcm_din1) = 0; *(chan->dcm_din2) = 0; *(chan->dcm_sync_err) = 0; *(chan->dcm_tx_toggle) = 0; *(chan->dcm_raw_info1) = 0; *(chan->dcm_raw_info2) = 0; *(chan->dcm_sel_info1) = info1_select; *(chan->dcm_sel_info2) = info2_select; if (chan->dcm_velo_fb != NULL) { *(chan->dcm_velo_fb) = 0.0; } if (chan->dcm_current_fb != NULL) { *(chan->dcm_current_fb) = 0.0; } // initialize variables chan->enc_do_init = 1; chan->enc_last_count = 0; chan->enc_old_scale = *(chan->pos_scale) + 1.0; chan->enc_scale_recip = 1.0; chan->dcm_old_scale = *(chan->dcm_scale) + 1.0; chan->dcm_scale_recip = 1.0; } return 0; }
static int export_joint(int num, joint_hal_t * addr) { int retval, msg; /* This function exports a lot of stuff, which results in a lot of logging if msg_level is at INFO or ALL. So we save the current value of msg_level and restore it later. If you actually need to log this function's actions, change the second line below */ msg = rtapi_get_msg_level(); rtapi_set_msg_level(RTAPI_MSG_WARN); /* export joint pins */ //FIXME-AJ: changing these will bork configs, still we should do it retval = hal_pin_float_newf(HAL_OUT, &(addr->joint_pos_cmd), mot_comp_id, "axis.%d.joint-pos-cmd", num); if (retval != 0) { return retval; } retval = hal_pin_float_newf(HAL_OUT, &(addr->joint_pos_fb), mot_comp_id, "axis.%d.joint-pos-fb", num); if (retval != 0) { return retval; } retval = hal_pin_float_newf(HAL_OUT, &(addr->motor_pos_cmd), mot_comp_id, "axis.%d.motor-pos-cmd", num); if (retval != 0) { return retval; } retval = hal_pin_float_newf(HAL_OUT, &(addr->motor_offset), mot_comp_id, "axis.%d.motor-offset", num); if (retval != 0) { return retval; } retval = hal_pin_float_newf(HAL_IN, &(addr->motor_pos_fb), mot_comp_id, "axis.%d.motor-pos-fb", num); if (retval != 0) { return retval; } retval = hal_pin_bit_newf(HAL_IN, &(addr->pos_lim_sw), mot_comp_id, "axis.%d.pos-lim-sw-in", num); if (retval != 0) { return retval; } retval = hal_pin_bit_newf(HAL_IN, &(addr->neg_lim_sw), mot_comp_id, "axis.%d.neg-lim-sw-in", num); if (retval != 0) { return retval; } retval = hal_pin_bit_newf(HAL_IN, &(addr->home_sw), mot_comp_id, "axis.%d.home-sw-in", num); if (retval != 0) { return retval; } retval = hal_pin_bit_newf(HAL_IO, &(addr->index_enable), mot_comp_id, "axis.%d.index-enable", num); if (retval != 0) { return retval; } retval = hal_pin_bit_newf(HAL_OUT, &(addr->amp_enable), mot_comp_id, "axis.%d.amp-enable-out", num); if (retval != 0) { return retval; } retval = hal_pin_bit_newf(HAL_IN, &(addr->amp_fault), mot_comp_id, "axis.%d.amp-fault-in", num); if (retval != 0) { return retval; } retval = hal_pin_s32_newf(HAL_IN, &(addr->jog_counts), mot_comp_id, "axis.%d.jog-counts", num); if (retval != 0) { return retval; } retval = hal_pin_bit_newf(HAL_IN, &(addr->jog_enable), mot_comp_id, "axis.%d.jog-enable", num); if (retval != 0) { return retval; } retval = hal_pin_float_newf(HAL_IN, &(addr->jog_scale), mot_comp_id, "axis.%d.jog-scale", num); if (retval != 0) { return retval; } retval = hal_pin_bit_newf(HAL_IN, &(addr->jog_vel_mode), mot_comp_id, "axis.%d.jog-vel-mode", num); if (retval != 0) { return retval; } retval = hal_pin_bit_newf(HAL_OUT, &(addr->homing), mot_comp_id, "axis.%d.homing", num); if (retval != 0) { return retval; } /* export joint parameters */ //FIXME-AJ: changing these to joints will break configs. retval = hal_pin_float_newf(HAL_OUT, &(addr->coarse_pos_cmd), mot_comp_id, "axis.%d.coarse-pos-cmd", num); if (retval != 0) { return retval; } retval = hal_pin_float_newf(HAL_OUT, &(addr->joint_vel_cmd), mot_comp_id, "axis.%d.joint-vel-cmd", num); if (retval != 0) { return retval; } retval = hal_pin_float_newf(HAL_OUT, &(addr->backlash_corr), mot_comp_id, "axis.%d.backlash-corr", num); if (retval != 0) { return retval; } retval = hal_pin_float_newf(HAL_OUT, &(addr->backlash_filt), mot_comp_id, "axis.%d.backlash-filt", num); if (retval != 0) { return retval; } retval = hal_pin_float_newf(HAL_OUT, &(addr->backlash_vel), mot_comp_id, "axis.%d.backlash-vel", num); if (retval != 0) { return retval; } retval = hal_pin_float_newf(HAL_OUT, &(addr->f_error), mot_comp_id, "axis.%d.f-error", num); if (retval != 0) { return retval; } retval = hal_pin_float_newf(HAL_OUT, &(addr->f_error_lim), mot_comp_id, "axis.%d.f-error-lim", num); if (retval != 0) { return retval; } retval = hal_pin_float_newf(HAL_OUT, &(addr->free_pos_cmd), mot_comp_id, "axis.%d.free-pos-cmd", num); if (retval != 0) { return retval; } retval = hal_pin_float_newf(HAL_OUT, &(addr->free_vel_lim), mot_comp_id, "axis.%d.free-vel-lim", num); if (retval != 0) { return retval; } retval = hal_pin_bit_newf(HAL_OUT, &(addr->free_tp_enable), mot_comp_id, "axis.%d.free-tp-enable", num); if (retval != 0) { return retval; } retval = hal_pin_bit_newf(HAL_OUT, &(addr->kb_jog_active), mot_comp_id, "axis.%d.kb-jog-active", num); if (retval != 0) { return retval; } retval = hal_pin_bit_newf(HAL_OUT, &(addr->wheel_jog_active), mot_comp_id, "axis.%d.wheel-jog-active", num); if (retval != 0) { return retval; } retval = hal_pin_bit_newf(HAL_OUT, &(addr->active), mot_comp_id, "axis.%d.active", num); if (retval != 0) { return retval; } retval = hal_pin_bit_newf(HAL_OUT, &(addr->in_position), mot_comp_id, "axis.%d.in-position", num); if (retval != 0) { return retval; } retval = hal_pin_bit_newf(HAL_OUT, &(addr->error), mot_comp_id, "axis.%d.error", num); if (retval != 0) { return retval; } retval = hal_pin_bit_newf(HAL_OUT, &(addr->phl), mot_comp_id, "axis.%d.pos-hard-limit", num); if (retval != 0) { return retval; } retval = hal_pin_bit_newf(HAL_OUT, &(addr->nhl), mot_comp_id, "axis.%d.neg-hard-limit", num); if (retval != 0) { return retval; } retval = hal_pin_bit_newf(HAL_OUT, &(addr->homed), mot_comp_id, "axis.%d.homed", num); if (retval != 0) { return retval; } retval = hal_pin_bit_newf(HAL_OUT, &(addr->f_errored), mot_comp_id, "axis.%d.f-errored", num); if (retval != 0) { return retval; } retval = hal_pin_bit_newf(HAL_OUT, &(addr->faulted), mot_comp_id, "axis.%d.faulted", num); if (retval != 0) { return retval; } retval = hal_pin_s32_newf(HAL_OUT, &(addr->home_state), mot_comp_id, "axis.%d.home-state", num); if (retval != 0) { return retval; } if(num >=3 && num <= 5) { // for rotaries only... retval = hal_pin_bit_newf(HAL_OUT, &(addr->unlock), mot_comp_id, "axis.%d.unlock", num); if (retval != 0) return retval; retval = hal_pin_bit_newf(HAL_IN, &(addr->is_unlocked), mot_comp_id, "axis.%d.is-unlocked", num); if (retval != 0) return retval; } /* restore saved message level */ rtapi_set_msg_level(msg); return 0; }
int lcec_stmds5k_init(int comp_id, struct lcec_slave *slave, ec_pdo_entry_reg_t *pdo_entry_regs) { lcec_master_t *master = slave->master; lcec_stmds5k_data_t *hal_data; int err; // initialize callbacks slave->proc_read = lcec_stmds5k_read; slave->proc_write = lcec_stmds5k_write; // alloc hal memory if ((hal_data = hal_malloc(sizeof(lcec_stmds5k_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_stmds5k_data_t)); slave->hal_data = hal_data; // read sdos // B18 : torque reference if ((hal_data->sdo_torque_reference = lcec_read_sdo(slave, 0x2212, 0x00, 4)) == NULL) { return -EIO; } hal_data->torque_reference = (double)EC_READ_S32(ecrt_sdo_request_data(hal_data->sdo_torque_reference)) * STMDS5K_TORQUE_REF_DIV; if (hal_data->torque_reference > 1e-20 || hal_data->torque_reference < -1e-20) { hal_data->torque_reference_rcpt = 1.0 / hal_data->torque_reference; } else { hal_data->torque_reference_rcpt = 0.0; } // C01 : max rpm if ((hal_data->sdo_speed_max_rpm = lcec_read_sdo(slave, 0x2401, 0x00, 4)) == NULL) { return -EIO; } hal_data->speed_max_rpm = (double)EC_READ_S32(ecrt_sdo_request_data(hal_data->sdo_speed_max_rpm)); // D02 : setpoint max rpm if ((hal_data->sdo_speed_max_rpm_sp = lcec_read_sdo(slave, 0x2602, 0x00, 2)) == NULL) { return -EIO; } hal_data->speed_max_rpm_sp = (double)EC_READ_S16(ecrt_sdo_request_data(hal_data->sdo_speed_max_rpm_sp)); if (hal_data->speed_max_rpm_sp > 1e-20 || hal_data->speed_max_rpm_sp < -1e-20) { hal_data->speed_max_rpm_sp_rcpt = 1.0 / hal_data->speed_max_rpm_sp; } else { hal_data->speed_max_rpm_sp_rcpt = 0.0; } // initialize POD entries // E200 : device state byte LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x28c8, 0x00, &hal_data->dev_state_pdo_os, NULL); // E100 : speed motor (x 0.1% relative to C01) LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x2864, 0x00, &hal_data->speed_mot_pdo_os, NULL); // E02 : torque motor filterd (x 0,1 Nm) LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x2802, 0x00, &hal_data->torque_mot_pdo_os, NULL); // D200 : speed state word LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x26c8, 0x00, &hal_data->speed_state_pdo_os, NULL); // E09 : rotor position LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x2809, 0x00, &hal_data->pos_mot_pdo_os, NULL); // A180 : Device Control Byte LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x20b4, 0x00, &hal_data->dev_ctrl_pdo_os, NULL); // D230 : speed setpoint (x 0.1 % relative to D02, -200.0% .. 200.0%) LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x26e6, 0x00, &hal_data->speed_sp_rel_pdo_os, NULL); // C230 : maximum torque (x 1%, 0% .. 200%) LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x24e6, 0x00, &hal_data->torque_max_pdo_os, NULL); // export pins if ((err = hal_pin_float_newf(HAL_IN, &(hal_data->vel_cmd), comp_id, "%s.%s.%s.srv-vel-cmd", LCEC_MODULE_NAME, master->name, slave->name)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-vel-cmd failed\n", LCEC_MODULE_NAME, master->name, slave->name); return err; } if ((err = hal_pin_float_newf(HAL_OUT, &(hal_data->vel_fb), comp_id, "%s.%s.%s.srv-vel-fb", LCEC_MODULE_NAME, master->name, slave->name)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-vel-fb failed\n", LCEC_MODULE_NAME, master->name, slave->name); return err; } if ((err = hal_pin_float_newf(HAL_OUT, &(hal_data->vel_fb_rpm), comp_id, "%s.%s.%s.srv-vel-fb-rpm", LCEC_MODULE_NAME, master->name, slave->name)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-vel-fb-rpm failed\n", LCEC_MODULE_NAME, master->name, slave->name); return err; } if ((err = hal_pin_float_newf(HAL_OUT, &(hal_data->vel_rpm), comp_id, "%s.%s.%s.srv-vel-rpm", LCEC_MODULE_NAME, master->name, slave->name)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-vel-rpm failed\n", LCEC_MODULE_NAME, master->name, slave->name); return err; } if ((err = hal_pin_s32_newf(HAL_OUT, &(hal_data->enc_raw), comp_id, "%s.%s.%s.srv-enc-raw", LCEC_MODULE_NAME, master->name, slave->name)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-enc-raw failed\n", LCEC_MODULE_NAME, master->name, slave->name); return err; } if ((err = hal_pin_u32_newf(HAL_OUT, &(hal_data->pos_raw_hi), comp_id, "%s.%s.%s.srv-pos-raw-hi", LCEC_MODULE_NAME, master->name, slave->name)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-pos-raw-hi failed\n", LCEC_MODULE_NAME, master->name, slave->name); return err; } if ((err = hal_pin_u32_newf(HAL_OUT, &(hal_data->pos_raw_lo), comp_id, "%s.%s.%s.srv-pos-raw-lo", LCEC_MODULE_NAME, master->name, slave->name)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-pos-raw-lo failed\n", LCEC_MODULE_NAME, master->name, slave->name); return err; } if ((err = hal_pin_float_newf(HAL_OUT, &(hal_data->pos_fb), comp_id, "%s.%s.%s.srv-pos-fb", LCEC_MODULE_NAME, master->name, slave->name)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-pos-fb failed\n", LCEC_MODULE_NAME, master->name, slave->name); return err; } if ((err = hal_pin_float_newf(HAL_OUT, &(hal_data->torque_fb), comp_id, "%s.%s.%s.srv-torque-fb", LCEC_MODULE_NAME, master->name, slave->name)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-torque-fb failed\n", LCEC_MODULE_NAME, master->name, slave->name); return err; } if ((err = hal_pin_float_newf(HAL_OUT, &(hal_data->torque_fb_abs), comp_id, "%s.%s.%s.srv-torque-fb-abs", LCEC_MODULE_NAME, master->name, slave->name)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-torque-fb-abs failed\n", LCEC_MODULE_NAME, master->name, slave->name); return err; } if ((err = hal_pin_float_newf(HAL_OUT, &(hal_data->torque_fb_pct), comp_id, "%s.%s.%s.srv-torque-fb-pct", LCEC_MODULE_NAME, master->name, slave->name)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-torque-fb-pct failed\n", LCEC_MODULE_NAME, master->name, slave->name); return err; } if ((err = hal_pin_float_newf(HAL_IN, &(hal_data->torque_lim), comp_id, "%s.%s.%s.srv-torque-lim", LCEC_MODULE_NAME, master->name, slave->name)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-torque-lim failed\n", LCEC_MODULE_NAME, master->name, slave->name); return err; } if ((err = hal_pin_bit_newf(HAL_OUT, &(hal_data->stopped), comp_id, "%s.%s.%s.srv-stopped", LCEC_MODULE_NAME, master->name, slave->name)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-stopped failed\n", LCEC_MODULE_NAME, master->name, slave->name); return err; } if ((err = hal_pin_bit_newf(HAL_OUT, &(hal_data->at_speed), comp_id, "%s.%s.%s.srv-at-speed", LCEC_MODULE_NAME, master->name, slave->name)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-at-speed failed\n", LCEC_MODULE_NAME, master->name, slave->name); return err; } if ((err = hal_pin_bit_newf(HAL_OUT, &(hal_data->overload), comp_id, "%s.%s.%s.srv-overload", LCEC_MODULE_NAME, master->name, slave->name)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-overload failed\n", LCEC_MODULE_NAME, master->name, slave->name); return err; } if ((err = hal_pin_bit_newf(HAL_OUT, &(hal_data->ready), comp_id, "%s.%s.%s.srv-ready", LCEC_MODULE_NAME, master->name, slave->name)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-ready failed\n", LCEC_MODULE_NAME, master->name, slave->name); return err; } if ((err = hal_pin_bit_newf(HAL_OUT, &(hal_data->error), comp_id, "%s.%s.%s.srv-error", LCEC_MODULE_NAME, master->name, slave->name)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-error failed\n", LCEC_MODULE_NAME, master->name, slave->name); return err; } if ((err = hal_pin_bit_newf(HAL_OUT, &(hal_data->toggle), comp_id, "%s.%s.%s.srv-toggle", LCEC_MODULE_NAME, master->name, slave->name)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-toggle failed\n", LCEC_MODULE_NAME, master->name, slave->name); return err; } if ((err = hal_pin_bit_newf(HAL_OUT, &(hal_data->loc_ena), comp_id, "%s.%s.%s.srv-loc-ena", LCEC_MODULE_NAME, master->name, slave->name)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-loc-ena failed\n", LCEC_MODULE_NAME, master->name, slave->name); return err; } if ((err = hal_pin_bit_newf(HAL_IN, &(hal_data->enable), comp_id, "%s.%s.%s.srv-enable", LCEC_MODULE_NAME, master->name, slave->name)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-enable failed\n", LCEC_MODULE_NAME, master->name, slave->name); return err; } if ((err = hal_pin_bit_newf(HAL_IN, &(hal_data->err_reset), comp_id, "%s.%s.%s.srv-err-reset", LCEC_MODULE_NAME, master->name, slave->name)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-err-reset failed\n", LCEC_MODULE_NAME, master->name, slave->name); return err; } if ((err = hal_pin_bit_newf(HAL_IN, &(hal_data->fast_ramp), comp_id, "%s.%s.%s.srv-fast-ramp", LCEC_MODULE_NAME, master->name, slave->name)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-fast-ramp failed\n", LCEC_MODULE_NAME, master->name, slave->name); return err; } if ((err = hal_pin_bit_newf(HAL_IN, &(hal_data->brake), comp_id, "%s.%s.%s.srv-brake", LCEC_MODULE_NAME, master->name, slave->name)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-brake failed\n", LCEC_MODULE_NAME, master->name, slave->name); return err; } if ((err = hal_pin_bit_newf(HAL_IO, &(hal_data->index_ena), comp_id, "%s.%s.%s.srv-index-ena", LCEC_MODULE_NAME, master->name, slave->name)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-index-ena failed\n", LCEC_MODULE_NAME, master->name, slave->name); return err; } if ((err = hal_pin_bit_newf(HAL_IN, &(hal_data->pos_reset), comp_id, "%s.%s.%s.srv-pos-reset", LCEC_MODULE_NAME, master->name, slave->name)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-pos-reset failed\n", LCEC_MODULE_NAME, master->name, slave->name); return err; } if ((err = hal_pin_bit_newf(HAL_OUT, &(hal_data->on_home_neg), comp_id, "%s.%s.%s.srv-on-home-neg", LCEC_MODULE_NAME, master->name, slave->name)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-on-home-neg failed\n", LCEC_MODULE_NAME, master->name, slave->name); return err; } if ((err = hal_pin_bit_newf(HAL_OUT, &(hal_data->on_home_pos), comp_id, "%s.%s.%s.srv-on-home-pos", LCEC_MODULE_NAME, master->name, slave->name)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-on-home-pos failed\n", LCEC_MODULE_NAME, master->name, slave->name); return err; } // export parameters if ((err = hal_param_float_newf(HAL_RW, &(hal_data->pos_scale), comp_id, "%s.%s.%s.srv-pos-scale", LCEC_MODULE_NAME, master->name, slave->name)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-pos-scale failed\n", LCEC_MODULE_NAME, master->name, slave->name); return err; } if ((err = hal_param_float_newf(HAL_RO, &(hal_data->torque_reference), comp_id, "%s.%s.%s.srv-torque-ref", LCEC_MODULE_NAME, master->name, slave->name)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-torque-ref failed\n", LCEC_MODULE_NAME, master->name, slave->name); return err; } if ((err = hal_param_float_newf(HAL_RO, &(hal_data->speed_max_rpm), comp_id, "%s.%s.%s.srv-max-rpm", LCEC_MODULE_NAME, master->name, slave->name)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-max-rpm failed\n", LCEC_MODULE_NAME, master->name, slave->name); return err; } if ((err = hal_param_float_newf(HAL_RO, &(hal_data->speed_max_rpm_sp), comp_id, "%s.%s.%s.srv-max-rpm-sp", LCEC_MODULE_NAME, master->name, slave->name)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-max-rpm-sp failed\n", LCEC_MODULE_NAME, master->name, slave->name); return err; } if ((err = hal_param_s32_newf(HAL_RW, &(hal_data->home_raw), comp_id, "%s.%s.%s.srv-home-raw", LCEC_MODULE_NAME, master->name, slave->name)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-home-raw failed\n", LCEC_MODULE_NAME, master->name, slave->name); return err; } // set default pin values *(hal_data->vel_cmd) = 0.0; *(hal_data->vel_fb) = 0.0; *(hal_data->vel_fb_rpm) = 1.0; *(hal_data->vel_rpm) = 0.0; *(hal_data->pos_raw_hi) = 0; *(hal_data->pos_raw_lo) = 0; *(hal_data->pos_fb) = 0.0; *(hal_data->torque_fb) = 0.0; *(hal_data->torque_fb_abs) = 0.0; *(hal_data->torque_fb_pct) = 0.0; *(hal_data->torque_lim) = 1.0; *(hal_data->stopped) = 0; *(hal_data->at_speed) = 0; *(hal_data->overload) = 0; *(hal_data->ready) = 0; *(hal_data->error) = 0; *(hal_data->toggle) = 0; *(hal_data->loc_ena) = 0; *(hal_data->enable) = 0; *(hal_data->err_reset) = 0; *(hal_data->fast_ramp) = 0; *(hal_data->brake) = 0; *(hal_data->index_ena) = 0; *(hal_data->pos_reset) = 0; *(hal_data->enc_raw) = 0; *(hal_data->on_home_neg) = 0; *(hal_data->on_home_pos) = 0; // initialize variables hal_data->pos_scale = 1.0; hal_data->do_init = 1; hal_data->pos_cnt = 0; hal_data->index_cnt = 0; hal_data->last_pos_cnt = 0; hal_data->pos_scale_old = hal_data->pos_scale + 1.0; hal_data->pos_scale_rcpt = 1.0; hal_data->pos_scale_cnt = 1.0; hal_data->last_index_ena = 0; hal_data->index_ref = 0; hal_data->home_raw = 0; return 0; }
static int export_sim_enc(sim_enc_t * addr, char *prefix) { int retval, msg; /* This function exports a lot of stuff, which results in a lot of logging if msg_level is at INFO or ALL. So we save the current value of msg_level and restore it later. If you actually need to log this function's actions, change the second line below */ msg = rtapi_get_msg_level(); rtapi_set_msg_level(RTAPI_MSG_WARN); /* export param variable for pulses per rev */ retval = hal_pin_u32_newf(HAL_IO, &(addr->ppr), comp_id, "%s.ppr", prefix); if (retval != 0) { return retval; } /* export param variable for scaling */ retval = hal_pin_float_newf(HAL_IO, &(addr->scale), comp_id, "%s.scale", prefix); if (retval != 0) { return retval; } /* export pin for speed command */ retval = hal_pin_float_newf(HAL_IN, &(addr->speed), comp_id, "%s.speed", prefix); if (retval != 0) { return retval; } /* export pins for output phases */ retval = hal_pin_bit_newf(HAL_OUT, &(addr->phaseA), comp_id, "%s.phase-A", prefix); if (retval != 0) { return retval; } retval = hal_pin_bit_newf(HAL_OUT, &(addr->phaseB), comp_id, "%s.phase-B", prefix); if (retval != 0) { return retval; } retval = hal_pin_bit_newf(HAL_OUT, &(addr->phaseZ), comp_id, "%s.phase-Z", prefix); if (retval != 0) { return retval; } /* export pin for rawcounts */ retval = hal_pin_s32_newf(HAL_IN, &(addr->rawcounts), comp_id, "%s.rawcounts", prefix); if (retval != 0) { return retval; } /* init parameters */ *(addr->ppr) = 100; *(addr->scale) = 1.0; /* init internal vars */ addr->old_scale = 0.0; addr->scale_mult = 1.0; /* init the state variables */ addr->accum = 0; addr->addval = 0; addr->state = 0; addr->cycle = 0; /* init the outputs */ *(addr->phaseA) = 0; *(addr->phaseB) = 0; *(addr->phaseZ) = 0; /* restore saved message level */ rtapi_set_msg_level(msg); return 0; }
/* init_hal_io() exports HAL pins and parameters making data from the realtime control module visible and usable by the world */ static int init_hal_io(void) { int n, retval; joint_hal_t *joint_data; rtapi_print_msg(RTAPI_MSG_INFO, "MOTION: init_hal_io() starting...\n"); /* allocate shared memory for machine data */ emcmot_hal_data = hal_malloc(sizeof(emcmot_hal_data_t)); if (emcmot_hal_data == 0) { rtapi_print_msg(RTAPI_MSG_ERR, _("MOTION: emcmot_hal_data malloc failed\n")); return -1; } /* export machine wide hal pins */ if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->probe_input), mot_comp_id, "motion.probe-input")) < 0) goto error; if ((retval = hal_pin_bit_newf(HAL_IO, &(emcmot_hal_data->spindle_index_enable), mot_comp_id, "motion.spindle-index-enable")) < 0) goto error; if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->spindle_on), mot_comp_id, "motion.spindle-on")) < 0) goto error; if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->spindle_forward), mot_comp_id, "motion.spindle-forward")) < 0) goto error; if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->spindle_reverse), mot_comp_id, "motion.spindle-reverse")) < 0) goto error; if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->spindle_brake), mot_comp_id, "motion.spindle-brake")) < 0) goto error; if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->spindle_speed_out), mot_comp_id, "motion.spindle-speed-out")) < 0) goto error; if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->spindle_speed_out_abs), mot_comp_id, "motion.spindle-speed-out-abs")) < 0) goto error; if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->spindle_speed_out_rps), mot_comp_id, "motion.spindle-speed-out-rps")) < 0) goto error; if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->spindle_speed_out_rps_abs), mot_comp_id, "motion.spindle-speed-out-rps-abs")) < 0) goto error; if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->spindle_speed_cmd_rps), mot_comp_id, "motion.spindle-speed-cmd-rps")) < 0) goto error; if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->spindle_inhibit), mot_comp_id, "motion.spindle-inhibit")) < 0) goto error; *(emcmot_hal_data->spindle_inhibit) = 0; // spindle orient pins if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->spindle_orient_angle), mot_comp_id, "motion.spindle-orient-angle")) < 0) goto error; if ((retval = hal_pin_s32_newf(HAL_OUT, &(emcmot_hal_data->spindle_orient_mode), mot_comp_id, "motion.spindle-orient-mode")) < 0) goto error; if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->spindle_orient), mot_comp_id, "motion.spindle-orient")) < 0) goto error; if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->spindle_locked), mot_comp_id, "motion.spindle-locked")) < 0) goto error; if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->spindle_is_oriented), mot_comp_id, "motion.spindle-is-oriented")) < 0) goto error; if ((retval = hal_pin_s32_newf(HAL_IN, &(emcmot_hal_data->spindle_orient_fault), mot_comp_id, "motion.spindle-orient-fault")) < 0) goto error; *(emcmot_hal_data->spindle_orient_angle) = 0.0; *(emcmot_hal_data->spindle_orient_mode) = 0; *(emcmot_hal_data->spindle_orient) = 0; // if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->inpos_output), mot_comp_id, "motion.motion-inpos")) < 0) goto error; if ((retval = hal_pin_float_newf(HAL_IN, &(emcmot_hal_data->spindle_revs), mot_comp_id, "motion.spindle-revs")) < 0) goto error; if ((retval = hal_pin_float_newf(HAL_IN, &(emcmot_hal_data->spindle_speed_in), mot_comp_id, "motion.spindle-speed-in")) < 0) goto error; if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->spindle_is_atspeed), mot_comp_id, "motion.spindle-at-speed")) < 0) goto error; *emcmot_hal_data->spindle_is_atspeed = 1; if ((retval = hal_pin_float_newf(HAL_IN, &(emcmot_hal_data->adaptive_feed), mot_comp_id, "motion.adaptive-feed")) < 0) goto error; *(emcmot_hal_data->adaptive_feed) = 1.0; if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->feed_hold), mot_comp_id, "motion.feed-hold")) < 0) goto error; *(emcmot_hal_data->feed_hold) = 0; if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->feed_inhibit), mot_comp_id, "motion.feed-inhibit")) < 0) goto error; *(emcmot_hal_data->feed_inhibit) = 0; if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->enable), mot_comp_id, "motion.enable")) < 0) goto error; /* export motion-synched digital output pins */ /* export motion digital input pins */ for (n = 0; n < num_dio; n++) { if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->synch_do[n]), mot_comp_id, "motion.digital-out-%02d", n)) < 0) goto error; if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->synch_di[n]), mot_comp_id, "motion.digital-in-%02d", n)) < 0) goto error; } /* export motion analog input pins */ for (n = 0; n < num_aio; n++) { if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->analog_output[n]), mot_comp_id, "motion.analog-out-%02d", n)) < 0) goto error; if ((retval = hal_pin_float_newf(HAL_IN, &(emcmot_hal_data->analog_input[n]), mot_comp_id, "motion.analog-in-%02d", n)) < 0) goto error; } /* export machine wide hal parameters */ retval = hal_pin_bit_new("motion.motion-enabled", HAL_OUT, &(emcmot_hal_data->motion_enabled), mot_comp_id); if (retval != 0) { return retval; } retval = hal_pin_bit_new("motion.in-position", HAL_OUT, &(emcmot_hal_data->in_position), mot_comp_id); if (retval != 0) { return retval; } retval = hal_pin_bit_new("motion.coord-mode", HAL_OUT, &(emcmot_hal_data->coord_mode), mot_comp_id); if (retval != 0) { return retval; } retval = hal_pin_bit_new("motion.teleop-mode", HAL_OUT, &(emcmot_hal_data->teleop_mode), mot_comp_id); if (retval != 0) { return retval; } retval = hal_pin_bit_new("motion.coord-error", HAL_OUT, &(emcmot_hal_data->coord_error), mot_comp_id); if (retval != 0) { return retval; } retval = hal_pin_bit_new("motion.on-soft-limit", HAL_OUT, &(emcmot_hal_data->on_soft_limit), mot_comp_id); if (retval != 0) { return retval; } retval = hal_pin_float_new("motion.current-vel", HAL_OUT, &(emcmot_hal_data->current_vel), mot_comp_id); if (retval != 0) { return retval; } retval = hal_pin_float_new("motion.requested-vel", HAL_OUT, &(emcmot_hal_data->requested_vel), mot_comp_id); if (retval != 0) { return retval; } retval = hal_pin_float_new("motion.distance-to-go", HAL_OUT, &(emcmot_hal_data->distance_to_go), mot_comp_id); if (retval != 0) { return retval; } retval = hal_pin_s32_new("motion.program-line", HAL_OUT, &(emcmot_hal_data->program_line), mot_comp_id); if (retval != 0) { return retval; } /* export debug parameters */ /* these can be used to view any internal variable, simply change a line in control.c:output_to_hal() and recompile */ retval = hal_param_bit_new("motion.debug-bit-0", HAL_RO, &(emcmot_hal_data->debug_bit_0), mot_comp_id); if (retval != 0) { return retval; } retval = hal_param_bit_new("motion.debug-bit-1", HAL_RO, &(emcmot_hal_data->debug_bit_1), mot_comp_id); if (retval != 0) { return retval; } retval = hal_param_float_new("motion.debug-float-0", HAL_RO, &(emcmot_hal_data->debug_float_0), mot_comp_id); if (retval != 0) { return retval; } retval = hal_param_float_new("motion.debug-float-1", HAL_RO, &(emcmot_hal_data->debug_float_1), mot_comp_id); if (retval != 0) { return retval; } retval = hal_param_float_new("motion.debug-float-2", HAL_RO, &(emcmot_hal_data->debug_float_2), mot_comp_id); if (retval != 0) { return retval; } retval = hal_param_float_new("motion.debug-float-3", HAL_RO, &(emcmot_hal_data->debug_float_3), mot_comp_id); if (retval != 0) { return retval; } retval = hal_param_s32_new("motion.debug-s32-0", HAL_RO, &(emcmot_hal_data->debug_s32_0), mot_comp_id); if (retval != 0) { return retval; } retval = hal_param_s32_new("motion.debug-s32-1", HAL_RO, &(emcmot_hal_data->debug_s32_1), mot_comp_id); if (retval != 0) { return retval; } // FIXME - debug only, remove later // export HAL parameters for some trajectory planner internal variables // so they can be scoped retval = hal_param_float_new("traj.pos_out", HAL_RO, &(emcmot_hal_data->traj_pos_out), mot_comp_id); if (retval != 0) { return retval; } retval = hal_param_float_new("traj.vel_out", HAL_RO, &(emcmot_hal_data->traj_vel_out), mot_comp_id); if (retval != 0) { return retval; } retval = hal_param_u32_new("traj.active_tc", HAL_RO, &(emcmot_hal_data->traj_active_tc), mot_comp_id); if (retval != 0) { return retval; } for ( n = 0 ; n < 4 ; n++ ) { retval = hal_param_float_newf(HAL_RO, &(emcmot_hal_data->tc_pos[n]), mot_comp_id, "tc.%d.pos", n); if (retval != 0) { return retval; } retval = hal_param_float_newf(HAL_RO, &(emcmot_hal_data->tc_vel[n]), mot_comp_id, "tc.%d.vel", n); if (retval != 0) { return retval; } retval = hal_param_float_newf(HAL_RO, &(emcmot_hal_data->tc_acc[n]), mot_comp_id, "tc.%d.acc", n); if (retval != 0) { return retval; } } // end of exporting trajectory planner internals // export timing related HAL parameters so they can be scoped retval = hal_param_u32_new("motion.servo.last-period", HAL_RO, &(emcmot_hal_data->last_period), mot_comp_id); if (retval != 0) { return retval; } #ifdef HAVE_CPU_KHZ retval = hal_param_float_new("motion.servo.last-period-ns", HAL_RO, &(emcmot_hal_data->last_period_ns), mot_comp_id); if (retval != 0) { return retval; } #endif retval = hal_param_u32_new("motion.servo.overruns", HAL_RW, &(emcmot_hal_data->overruns), mot_comp_id); if (retval != 0) { return retval; } retval = hal_pin_float_new("motion.tooloffset.x", HAL_OUT, &(emcmot_hal_data->tooloffset_x), mot_comp_id); if (retval != 0) { return retval; } retval = hal_pin_float_new("motion.tooloffset.y", HAL_OUT, &(emcmot_hal_data->tooloffset_y), mot_comp_id); if (retval != 0) { return retval; } retval = hal_pin_float_new("motion.tooloffset.z", HAL_OUT, &(emcmot_hal_data->tooloffset_z), mot_comp_id); if (retval != 0) { return retval; } retval = hal_pin_float_new("motion.tooloffset.a", HAL_OUT, &(emcmot_hal_data->tooloffset_a), mot_comp_id); if (retval != 0) { return retval; } retval = hal_pin_float_new("motion.tooloffset.b", HAL_OUT, &(emcmot_hal_data->tooloffset_b), mot_comp_id); if (retval != 0) { return retval; } retval = hal_pin_float_new("motion.tooloffset.c", HAL_OUT, &(emcmot_hal_data->tooloffset_c), mot_comp_id); if (retval != 0) { return retval; } retval = hal_pin_float_new("motion.tooloffset.u", HAL_OUT, &(emcmot_hal_data->tooloffset_u), mot_comp_id); if (retval != 0) { return retval; } retval = hal_pin_float_new("motion.tooloffset.v", HAL_OUT, &(emcmot_hal_data->tooloffset_v), mot_comp_id); if (retval != 0) { return retval; } retval = hal_pin_float_new("motion.tooloffset.w", HAL_OUT, &(emcmot_hal_data->tooloffset_w), mot_comp_id); if (retval != 0) { return retval; } /* initialize machine wide pins and parameters */ *(emcmot_hal_data->probe_input) = 0; /* default value of enable is TRUE, so simple machines can leave it disconnected */ *(emcmot_hal_data->enable) = 1; /* motion synched dio, init to not enabled */ for (n = 0; n < num_dio; n++) { *(emcmot_hal_data->synch_do[n]) = 0; *(emcmot_hal_data->synch_di[n]) = 0; } for (n = 0; n < num_aio; n++) { *(emcmot_hal_data->analog_output[n]) = 0.0; *(emcmot_hal_data->analog_input[n]) = 0.0; } /*! \todo FIXME - these don't really need initialized, since they are written with data from the emcmotStatus struct */ *(emcmot_hal_data->motion_enabled) = 0; *(emcmot_hal_data->in_position) = 0; *(emcmot_hal_data->coord_mode) = 0; *(emcmot_hal_data->teleop_mode) = 0; *(emcmot_hal_data->coord_error) = 0; *(emcmot_hal_data->on_soft_limit) = 0; /* init debug parameters */ emcmot_hal_data->debug_bit_0 = 0; emcmot_hal_data->debug_bit_1 = 0; emcmot_hal_data->debug_float_0 = 0.0; emcmot_hal_data->debug_float_1 = 0.0; emcmot_hal_data->debug_float_2 = 0.0; emcmot_hal_data->debug_float_3 = 0.0; emcmot_hal_data->overruns = 0; emcmot_hal_data->last_period = 0; /* export joint pins and parameters */ for (n = 0; n < num_joints; n++) { /* point to axis data */ joint_data = &(emcmot_hal_data->joint[n]); /* export all vars */ retval = export_joint(n, joint_data); if (retval != 0) { rtapi_print_msg(RTAPI_MSG_ERR, _("MOTION: joint %d pin/param export failed\n"), n); return -1; } /* init axis pins and parameters */ /* FIXME - struct members are in a state of flux - make sure to update this - most won't need initing anyway */ *(joint_data->amp_enable) = 0; *(joint_data->home_state) = 0; /* We'll init the index model to EXT_ENCODER_INDEX_MODEL_RAW for now, because it is always supported. */ } /* Done! */ rtapi_print_msg(RTAPI_MSG_INFO, "MOTION: init_hal_io() complete, %d axes.\n", n); return 0; error: return retval; }
static int export_encoder(eqep_t *eqep) { if (hal_pin_bit_newf(HAL_IO, &(eqep->index_ena), comp_id, "%s.index-enable", eqep->name)) { rtapi_print_msg(RTAPI_MSG_ERR,"Error exporting index-enable\n"); return -1; } if (hal_pin_bit_newf(HAL_IO, &(eqep->reset), comp_id, "%s.reset", eqep->name)) { rtapi_print_msg(RTAPI_MSG_ERR,"Error exporting reset\n"); return -1; } if (hal_pin_s32_newf(HAL_IO, &(eqep->count), comp_id, "%s.counts", eqep->name)) { rtapi_print_msg(RTAPI_MSG_ERR,"Error exporting counts\n"); return -1; } if (hal_pin_float_newf(HAL_IO, &(eqep->pos_scale), comp_id, "%s.position-scale", eqep->name)) { rtapi_print_msg(RTAPI_MSG_ERR,"Error exporting position-scale\n"); return -1; } if (hal_pin_float_newf(HAL_IN, &(eqep->min_speed), comp_id, "%s.min-speed-estimate", eqep->name)) { rtapi_print_msg(RTAPI_MSG_ERR,"Error exporting min-speed-estimate\n"); return -1; } if (hal_pin_float_newf(HAL_OUT, &(eqep->pos_interp), comp_id, "%s.position-interpolated", eqep->name)) { rtapi_print_msg(RTAPI_MSG_ERR,"Error exporting position-interpolated\n"); return -1; } if (hal_pin_float_newf(HAL_OUT, &(eqep->vel), comp_id, "%s.velocity", eqep->name)) { rtapi_print_msg(RTAPI_MSG_ERR,"Error exporting velocity\n"); return -1; } if (hal_pin_s32_newf(HAL_OUT, &(eqep->phase_error_count), comp_id, "%s.phase-errors", eqep->name)) { rtapi_print_msg(RTAPI_MSG_ERR,"Error exporting phase_errors\n"); return -1; } if (hal_pin_float_newf(HAL_OUT, &(eqep->pos), comp_id, "%s.position", eqep->name)) { rtapi_print_msg(RTAPI_MSG_ERR,"Error exporting position\n"); return -1; } if (hal_pin_s32_newf(HAL_OUT, &(eqep->raw_counts), comp_id, "%s.rawcounts", eqep->name)) { rtapi_print_msg(RTAPI_MSG_ERR,"Error exporting rawcounts\n"); return -1; } if (hal_pin_bit_newf(HAL_IO, &(eqep->counter_mode), comp_id, "%s.counter-mode", eqep->name)) { rtapi_print_msg(RTAPI_MSG_ERR,"Error exporting counter mode\n"); return -1; } if (hal_pin_bit_newf(HAL_IO, &(eqep->x2_mode), comp_id, "%s.x2-mode", eqep->name)) { rtapi_print_msg(RTAPI_MSG_ERR,"Error exporting x2 mode\n"); return -1; } if (hal_pin_bit_newf(HAL_IO, &(eqep->invertA), comp_id, "%s.invert-A", eqep->name)) { rtapi_print_msg(RTAPI_MSG_ERR,"Error exporting invert_A mode\n"); return -1; } if (hal_pin_bit_newf(HAL_IO, &(eqep->invertB), comp_id, "%s.invert-B", eqep->name)) { rtapi_print_msg(RTAPI_MSG_ERR,"Error exporting invert_B mode\n"); return -1; } if (hal_pin_bit_newf(HAL_IO, &(eqep->invertZ), comp_id, "%s.invert-Z", eqep->name)) { rtapi_print_msg(RTAPI_MSG_ERR,"Error exporting invert_Z mode\n"); return -1; } if (hal_pin_s32_newf(HAL_OUT, &(eqep->capture_period), comp_id, "%s.capture-period", eqep->name)) { rtapi_print_msg(RTAPI_MSG_ERR,"Error exporting capture-period\n"); return -1; } if (hal_pin_s32_newf(HAL_OUT, &(eqep->capture_overflow_count), comp_id, "%s.capture-overflows", eqep->name)) { rtapi_print_msg(RTAPI_MSG_ERR,"Error exporting capture-overflows\n"); return -1; } if (hal_pin_s32_newf(HAL_OUT, &(eqep->capture_dir_change_count), comp_id, "%s.capture-dir-changes", eqep->name)) { rtapi_print_msg(RTAPI_MSG_ERR,"Error exporting capture-dir-changes\n"); return -1; } if (hal_pin_s32_newf(HAL_IN, &(eqep->capture_threshold), comp_id, "%s.capture-threshold", eqep->name)) { rtapi_print_msg(RTAPI_MSG_ERR,"Error exporting capture-threshold\n"); return -1; } if (hal_pin_u32_newf(HAL_IN, &(eqep->capture_pre), comp_id, "%s.capture-prescaler", eqep->name)) { rtapi_print_msg(RTAPI_MSG_ERR,"Error exporting capture-prescaler\n"); return -1; } return 0; }
int rtapi_app_main(void) { int res = 0, i; comp_id = hal_init("genserkins"); if (comp_id < 0) return comp_id; haldata = hal_malloc(sizeof(struct haldata)); if (!haldata) goto error; for (i = 0; i < GENSER_MAX_JOINTS; i++) { if ((res = hal_pin_float_newf(HAL_IO, &(haldata->a[i]), comp_id, "genserkins.A-%d", i)) < 0) goto error; *(haldata->a[i])=0; if ((res = hal_pin_float_newf(HAL_IO, &(haldata->alpha[i]), comp_id, "genserkins.ALPHA-%d", i)) < 0) goto error; *(haldata->alpha[i])=0; if ((res = hal_pin_float_newf(HAL_IO, &(haldata->d[i]), comp_id, "genserkins.D-%d", i)) < 0) goto error; *(haldata->d[i])=0; if ((res = hal_param_s32_newf(HAL_RW, &(haldata->unrotate[i]), comp_id, "genserkins.unrotate-%d", i)) < 0) goto error; haldata->unrotate[i]=0; } KINS_PTR = hal_malloc(sizeof(genser_struct)); haldata->pos = (go_pose *) hal_malloc(sizeof(go_pose)); if (KINS_PTR == NULL) goto error; if (haldata->pos == NULL) goto error; if ((res= hal_param_s32_newf(HAL_RO, &(KINS_PTR->iterations), comp_id, "genserkins.last-iterations")) < 0) goto error; if ((res= hal_param_s32_newf(HAL_RW, &(KINS_PTR->max_iterations), comp_id, "genserkins.max-iterations")) < 0) goto error; KINS_PTR->max_iterations = GENSER_DEFAULT_MAX_ITERATIONS; A(0) = DEFAULT_A1; A(1) = DEFAULT_A2; A(2) = DEFAULT_A3; A(3) = DEFAULT_A4; A(4) = DEFAULT_A5; A(5) = DEFAULT_A6; ALPHA(0) = DEFAULT_ALPHA1; ALPHA(1) = DEFAULT_ALPHA2; ALPHA(2) = DEFAULT_ALPHA3; ALPHA(3) = DEFAULT_ALPHA4; ALPHA(4) = DEFAULT_ALPHA5; ALPHA(5) = DEFAULT_ALPHA6; D(0) = DEFAULT_D1; D(1) = DEFAULT_D2; D(2) = DEFAULT_D3; D(3) = DEFAULT_D4; D(4) = DEFAULT_D5; D(5) = DEFAULT_D6; hal_ready(comp_id); return 0; error: hal_exit(comp_id); return res; }
static int instantiate_interpolate(const int argc, const char **argv) { const char *name = argv[1]; struct inst_data *ip; int inst_id, i; if ((inst_id = hal_inst_create(name, comp_id, sizeof(struct inst_data) + count * sizeof(struct joint), (void **)&ip)) < 0) return -1; // instance-level values ip->count = count; // attach the command ringbuffer '<instancename>.traj' if it exists // must be record mode unsigned flags; if (!hal_ring_attachf(&(ip->traj), &flags, "%s.traj", name)) { if ((flags & RINGTYPE_MASK) != RINGTYPE_RECORD) { HALERR("ring %s.traj not a record mode ring: mode=%d",name, flags & RINGTYPE_MASK); return -EINVAL; } ip->traj.header->reader = inst_id; // we're the reader - advisory } else { HALERR("ring %s.traj does not exist", name); return -EINVAL; } // per-instance objects if (hal_pin_s32_newf(HAL_OUT, &(ip->serial), inst_id, "%s.serial", name) || hal_pin_u32_newf(HAL_IN, &(ip->degree), inst_id, "%s.degree", name) || hal_pin_bit_newf(HAL_IN, &(ip->jitter_correct), inst_id, "%s.jitter-correct", name) || hal_pin_float_newf(HAL_IN, &(ip->epsilon), inst_id, "%s.epsilon", name) || hal_pin_float_newf(HAL_OUT, &(ip->duration), inst_id, "%s.duration", name) || hal_pin_float_newf(HAL_OUT, &(ip->progress), inst_id, "%s.progress", name)) return -1; *(ip->serial) = 0; *(ip->degree) = 1; *(ip->jitter_correct) = 0; *(ip->epsilon) = 0; *(ip->duration) = 0; *(ip->progress) = 0; ip->time_from_start = 0; // per-joint objects for (i = 0; i < ip->count; i++) { struct joint *jp = &ip->joints[i]; if (hal_pin_float_newf(HAL_OUT, &(jp->end_pos), inst_id, "%s.%d.end-pos", name, i) || hal_pin_float_newf(HAL_OUT, &(jp->end_vel), inst_id, "%s.%d.end-vel", name, i) || hal_pin_float_newf(HAL_OUT, &(jp->end_acc), inst_id, "%s.%d.end-acc", name, i) || hal_pin_float_newf(HAL_OUT, &(jp->curr_pos), inst_id, "%s.%d.curr-pos", name, i) || hal_pin_float_newf(HAL_OUT, &(jp->curr_vel), inst_id, "%s.%d.curr-vel", name, i) || hal_pin_float_newf(HAL_OUT, &(jp->curr_acc), inst_id, "%s.%d.curr-acc", name, i) || hal_pin_bit_newf(HAL_OUT, &(jp->traj_busy), inst_id, "%s.%d.traj-busy", name, i)) return -1; // set all pin values to zero *(jp->end_pos) = 0; *(jp->end_vel) = 0; *(jp->end_acc) = 0; *(jp->curr_pos) = 0; *(jp->curr_vel) = 0; *(jp->curr_acc) = 0; *(jp->traj_busy) = false; } hal_export_xfunct_args_t xfunct_args = { .type = FS_XTHREADFUNC, .funct.x = update, .arg = ip, .uses_fp = 0, .reentrant = 0, .owner_id = inst_id }; return hal_export_xfunctf(&xfunct_args, "%s.update", name); } static int delete_interpolate(const char *name, void *inst, const int inst_size) { struct inst_data *ip = (struct inst_data *) inst; int retval; if (ringbuffer_attached(&ip->traj)) { if ((retval = hal_ring_detach(&ip->traj)) < 0) { rtapi_print_msg(RTAPI_MSG_ERR, "%s: hal_ring_detach(%s.traj) failed: %d\n", compname, name, retval); return retval; } ip->traj.header->reader = 0; } return 0; } int rtapi_app_main(void) { comp_id = hal_xinit(TYPE_RT, 0, 0, (hal_constructor_t)instantiate_interpolate, delete_interpolate, compname); if (comp_id < 0) return comp_id; hal_ready(comp_id); return 0; }
int lcec_el5152_init(int comp_id, struct lcec_slave *slave, ec_pdo_entry_reg_t *pdo_entry_regs) { lcec_master_t *master = slave->master; lcec_el5152_data_t *hal_data; int i; lcec_el5152_chan_t *chan; int err; // initialize callbacks slave->proc_read = lcec_el5152_read; slave->proc_write = lcec_el5152_write; // alloc hal memory if ((hal_data = hal_malloc(sizeof(lcec_el5152_data_t))) == NULL) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "hal_malloc() for slave %s.%s failed\n", master->name, slave->name); return -EIO; } memset(hal_data, 0, sizeof(lcec_el5152_data_t)); slave->hal_data = hal_data; // initializer sync info slave->sync_info = lcec_el5152_syncs; // initialize global data hal_data->last_operational = 0; // initialize pins for (i=0; i<LCEC_EL5152_CHANS; i++) { chan = &hal_data->chans[i]; // initialize POD entries LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6000 + (i << 4), 0x03, &chan->set_count_done_pdo_os, &chan->set_count_done_pdo_bp); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6000 + (i << 4), 0x08, &chan->expol_stall_pdo_os, &chan->expol_stall_pdo_bp); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6000 + (i << 4), 0x09, &chan->ina_pdo_os, &chan->ina_pdo_bp); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6000 + (i << 4), 0x0a, &chan->inb_pdo_os, &chan->inb_pdo_bp); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x1800 + (i << 2), 0x09, &chan->tx_toggle_pdo_os, &chan->tx_toggle_pdo_bp); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6000 + (i << 4), 0x11, &chan->count_pdo_os, NULL); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6000 + (i << 4), 0x14, &chan->period_pdo_os, NULL); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x7000 + (i << 4), 0x03, &chan->set_count_pdo_os, &chan->set_count_pdo_bp); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x7000 + (i << 4), 0x11, &chan->set_count_val_pdo_os, NULL); // export pins if ((err = hal_pin_bit_newf(HAL_IN, &(chan->index), comp_id, "%s.%s.%s.enc-%d-index", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-index failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_bit_newf(HAL_IO, &(chan->index_ena), comp_id, "%s.%s.%s.enc-%d-index-enable", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-index-enable failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_bit_newf(HAL_IN, &(chan->reset), comp_id, "%s.%s.%s.enc-%d-reset", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-reset failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_bit_newf(HAL_OUT, &(chan->ina), comp_id, "%s.%s.%s.enc-%d-ina", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-ina failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_bit_newf(HAL_OUT, &(chan->inb), comp_id, "%s.%s.%s.enc-%d-inb", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-inb failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_bit_newf(HAL_OUT, &(chan->expol_stall), comp_id, "%s.%s.%s.enc-%d-expol-stall", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-expol-stall failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_bit_newf(HAL_OUT, &(chan->tx_toggle), comp_id, "%s.%s.%s.enc-%d-tx-toggle", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-tx-toggle failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_bit_newf(HAL_IO, &(chan->set_raw_count), comp_id, "%s.%s.%s.enc-%d-set-raw-count", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-set-raw-count failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_s32_newf(HAL_IN, &(chan->set_raw_count_val), comp_id, "%s.%s.%s.enc-%d-set-raw-count-val", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-set-raw-count-val failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_s32_newf(HAL_OUT, &(chan->raw_count), comp_id, "%s.%s.%s.enc-%d-raw-count", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-raw-count failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_s32_newf(HAL_OUT, &(chan->count), comp_id, "%s.%s.%s.enc-%d-count", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-count failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_u32_newf(HAL_OUT, &(chan->raw_period), comp_id, "%s.%s.%s.enc-%d-raw-period", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-raw-period failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_float_newf(HAL_OUT, &(chan->pos), comp_id, "%s.%s.%s.enc-%d-pos", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-pos failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_float_newf(HAL_OUT, &(chan->period), comp_id, "%s.%s.%s.enc-%d-period", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-period failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_float_newf(HAL_IO, &(chan->pos_scale), comp_id, "%s.%s.%s.enc-%d-pos-scale", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-scale failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } // initialize pins *(chan->index) = 0; *(chan->index_ena) = 0; *(chan->reset) = 0; *(chan->ina) = 0; *(chan->inb) = 0; *(chan->expol_stall) = 0; *(chan->tx_toggle) = 0; *(chan->set_raw_count) = 0; *(chan->set_raw_count_val) = 0; *(chan->raw_count) = 0; *(chan->raw_period) = 0; *(chan->count) = 0; *(chan->pos) = 0; *(chan->period) = 0; *(chan->pos_scale) = 1.0; // initialize variables chan->do_init = 1; chan->last_count = 0; chan->last_index = 0; chan->old_scale = *(chan->pos_scale) + 1.0; chan->scale = 1.0; } return 0; }
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; }
static int export_pwmgen(int num, pwmgen_t * addr, int output_type) { int retval, msg; /* This function exports a lot of stuff, which results in a lot of logging if msg_level is at INFO or ALL. So we save the current value of msg_level and restore it later. If you actually need to log this function's actions, change the second line below */ msg = rtapi_get_msg_level(); rtapi_set_msg_level(RTAPI_MSG_WARN); /* export paramameters */ retval = hal_pin_float_newf(HAL_IO, &(addr->scale), comp_id, "pwmgen.%d.scale", num); if (retval != 0) { return retval; } retval = hal_pin_float_newf(HAL_IO, &(addr->offset), comp_id, "pwmgen.%d.offset", num); if (retval != 0) { return retval; } retval = hal_pin_bit_newf(HAL_IO, &(addr->dither_pwm), comp_id, "pwmgen.%d.dither-pwm", num); if (retval != 0) { return retval; } retval = hal_pin_float_newf(HAL_IO, &(addr->pwm_freq), comp_id, "pwmgen.%d.pwm-freq", num); if (retval != 0) { return retval; } retval = hal_pin_float_newf(HAL_IO, &(addr->min_dc), comp_id, "pwmgen.%d.min-dc", num); if (retval != 0) { return retval; } retval = hal_pin_float_newf(HAL_IO, &(addr->max_dc), comp_id, "pwmgen.%d.max-dc", num); if (retval != 0) { return retval; } retval = hal_pin_float_newf(HAL_OUT, &(addr->curr_dc), comp_id, "pwmgen.%d.curr-dc", num); if (retval != 0) { return retval; } /* export pins */ retval = hal_pin_bit_newf(HAL_IN, &(addr->enable), comp_id, "pwmgen.%d.enable", num); if (retval != 0) { return retval; } *(addr->enable) = 0; retval = hal_pin_float_newf(HAL_IN, &(addr->value), comp_id, "pwmgen.%d.value", num); if (retval != 0) { return retval; } *(addr->value) = 0.0; if (output_type == 2) { /* export UP/DOWN pins */ retval = hal_pin_bit_newf(HAL_OUT, &(addr->out[UP_PIN]), comp_id, "pwmgen.%d.up", num); if (retval != 0) { return retval; } /* init the pin */ *(addr->out[UP_PIN]) = 0; retval = hal_pin_bit_newf(HAL_OUT, &(addr->out[DOWN_PIN]), comp_id, "pwmgen.%d.down", num); if (retval != 0) { return retval; } /* init the pin */ *(addr->out[DOWN_PIN]) = 0; } else { /* export PWM pin */ retval = hal_pin_bit_newf(HAL_OUT, &(addr->out[PWM_PIN]), comp_id, "pwmgen.%d.pwm", num); if (retval != 0) { return retval; } /* init the pin */ *(addr->out[PWM_PIN]) = 0; if ( output_type == 1 ) { /* export DIR pin */ retval = hal_pin_bit_newf(HAL_OUT, &(addr->out[DIR_PIN]), comp_id, "pwmgen.%d.dir", num); if (retval != 0) { return retval; } /* init the pin */ *(addr->out[DIR_PIN]) = 0; } } /* set default pin values */ *(addr->scale) = 1.0; *(addr->offset) = 0.0; *(addr->dither_pwm) = 0; *(addr->pwm_freq) = 0; *(addr->min_dc) = 0.0; *(addr->max_dc) = 1.0; *(addr->curr_dc) = 0.0; /* init other fields */ addr->period = 50000; addr->high_time = 0; addr->period_timer = 0; addr->high_timer = 0; addr->curr_output = 0; addr->output_type = output_type; addr->pwm_mode = PWM_DISABLED; addr->direction = 0; addr->old_scale = *(addr->scale) + 1.0; addr->old_pwm_freq = -1; /* restore saved message level */ rtapi_set_msg_level(msg); 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 main(int argc, char **argv) { int retval = 0; modbus_t *mb_ctx; haldata_t *haldata; slavedata_t slavedata; int slave; int hal_comp_id; struct timespec loop_timespec, remaining; int baud, bits, stopbits, verbose; char *device, *endarg; char parity; int opt; int argindex, argvalue; done = 0; // assume that nothing is specified on the command line baud = 38400; bits = 8; stopbits = 1; verbose = 0; device = "/dev/ttyS0"; parity = 'O'; /* slave / register info */ slave = 1; slavedata.read_reg_start = START_REGISTER_R; slavedata.read_reg_count = NUM_REGISTERS_R; slavedata.write_reg_start = START_REGISTER_W; slavedata.write_reg_count = NUM_REGISTERS_R; // process command line options while ((opt=getopt_long(argc, argv, option_string, long_options, NULL)) != -1) { switch(opt) { case 'b': // serial data bits, probably should be 8 (and defaults to 8) argindex=match_string(optarg, bitstrings); if (argindex<0) { printf("gs2_vfd: ERROR: invalid number of bits: %s\n", optarg); retval = -1; goto out_noclose; } bits = atoi(bitstrings[argindex]); break; case 'd': // device name, default /dev/ttyS0 // could check the device name here, but we'll leave it to the library open if (strlen(optarg) > FILENAME_MAX) { printf("gs2_vfd: ERROR: device node name is too long: %s\n", optarg); retval = -1; goto out_noclose; } device = strdup(optarg); break; case 'g': case 'v': verbose = 1; break; case 'n': // module base name if (strlen(optarg) > HAL_NAME_LEN-20) { printf("gs2_vfd: ERROR: HAL module name too long: %s\n", optarg); retval = -1; goto out_noclose; } modname = strdup(optarg); break; case 'p': // parity, should be a string like "even", "odd", or "none" argindex=match_string(optarg, paritystrings); if (argindex<0) { printf("gs2_vfd: ERROR: invalid parity: %s\n", optarg); retval = -1; goto out_noclose; } parity = paritychars[argindex]; break; case 'r': // Baud rate, 38400 default argindex=match_string(optarg, ratestrings); if (argindex<0) { printf("gs2_vfd: ERROR: invalid baud rate: %s\n", optarg); retval = -1; goto out_noclose; } baud = atoi(ratestrings[argindex]); break; case 's': // stop bits, defaults to 1 argindex=match_string(optarg, stopstrings); if (argindex<0) { printf("gs2_vfd: ERROR: invalid number of stop bits: %s\n", optarg); retval = -1; goto out_noclose; } stopbits = atoi(stopstrings[argindex]); break; case 't': // target number (MODBUS ID), default 1 argvalue = strtol(optarg, &endarg, 10); if ((*endarg != '\0') || (argvalue < 1) || (argvalue > 254)) { printf("gs2_vfd: ERROR: invalid slave number: %s\n", optarg); retval = -1; goto out_noclose; } slave = argvalue; break; case 'h': default: usage(argc, argv); exit(0); break; } } printf("%s: device='%s', baud=%d, parity='%c', bits=%d, stopbits=%d, address=%d, verbose=%d\n", modname, device, baud, parity, bits, stopbits, slave, verbose); /* point TERM and INT signals at our quit function */ /* if a signal is received between here and the main loop, it should prevent some initialization from happening */ signal(SIGINT, quit); signal(SIGTERM, quit); /* Assume 38.4k O-8-1 serial settings, device 1 */ mb_ctx = modbus_new_rtu(device, baud, parity, bits, stopbits); if (mb_ctx == NULL) { printf("%s: ERROR: couldn't open modbus serial device: %s\n", modname, modbus_strerror(errno)); goto out_noclose; } /* the open has got to work, or we're out of business */ if (((retval = modbus_connect(mb_ctx))!=0) || done) { printf("%s: ERROR: couldn't open serial device: %s\n", modname, modbus_strerror(errno)); goto out_noclose; } modbus_set_debug(mb_ctx, verbose); modbus_set_slave(mb_ctx, slave); /* create HAL component */ hal_comp_id = hal_init(modname); if ((hal_comp_id < 0) || done) { printf("%s: ERROR: hal_init failed\n", modname); retval = hal_comp_id; goto out_close; } /* grab some shmem to store the HAL data in */ haldata = (haldata_t *)hal_malloc(sizeof(haldata_t)); if ((haldata == 0) || done) { printf("%s: ERROR: unable to allocate shared memory\n", modname); retval = -1; goto out_close; } retval = hal_pin_s32_newf(HAL_OUT, &(haldata->stat1), hal_comp_id, "%s.status-1", modname); if (retval!=0) goto out_closeHAL; retval = hal_pin_s32_newf(HAL_OUT, &(haldata->stat2), hal_comp_id, "%s.status-2", modname); if (retval!=0) goto out_closeHAL; retval = hal_pin_float_newf(HAL_OUT, &(haldata->freq_cmd), hal_comp_id, "%s.frequency-command", modname); if (retval!=0) goto out_closeHAL; retval = hal_pin_float_newf(HAL_OUT, &(haldata->freq_out), hal_comp_id, "%s.frequency-out", modname); if (retval!=0) goto out_closeHAL; retval = hal_pin_float_newf(HAL_OUT, &(haldata->curr_out), hal_comp_id, "%s.output-current", modname); if (retval!=0) goto out_closeHAL; retval = hal_pin_float_newf(HAL_OUT, &(haldata->DCBusV), hal_comp_id, "%s.DC-bus-volts", modname); if (retval!=0) goto out_closeHAL; retval = hal_pin_float_newf(HAL_OUT, &(haldata->outV), hal_comp_id, "%s.output-voltage", modname); if (retval!=0) goto out_closeHAL; retval = hal_pin_float_newf(HAL_OUT, &(haldata->RPM), hal_comp_id, "%s.motor-RPM", modname); if (retval!=0) goto out_closeHAL; retval = hal_pin_float_newf(HAL_OUT, &(haldata->scale_freq), hal_comp_id, "%s.scale-frequency", modname); if (retval!=0) goto out_closeHAL; retval = hal_pin_float_newf(HAL_OUT, &(haldata->power_factor), hal_comp_id, "%s.power-factor", modname); if (retval!=0) goto out_closeHAL; retval = hal_pin_float_newf(HAL_OUT, &(haldata->load_pct), hal_comp_id, "%s.load-percentage", modname); if (retval!=0) goto out_closeHAL; retval = hal_pin_s32_newf(HAL_OUT, &(haldata->FW_Rev), hal_comp_id, "%s.firmware-revision", modname); if (retval!=0) goto out_closeHAL; retval = hal_param_s32_newf(HAL_RW, &(haldata->errorcount), hal_comp_id, "%s.error-count", modname); if (retval!=0) goto out_closeHAL; retval = hal_param_float_newf(HAL_RW, &(haldata->looptime), hal_comp_id, "%s.loop-time", modname); if (retval!=0) goto out_closeHAL; retval = hal_param_s32_newf(HAL_RW, &(haldata->retval), hal_comp_id, "%s.retval", modname); if (retval!=0) goto out_closeHAL; retval = hal_pin_bit_newf(HAL_OUT, &(haldata->at_speed), hal_comp_id, "%s.at-speed", modname); if (retval!=0) goto out_closeHAL; retval = hal_pin_bit_newf(HAL_OUT, &(haldata->is_stopped), hal_comp_id, "%s.is-stopped", modname); // JET if (retval!=0) goto out_closeHAL; retval = hal_pin_float_newf(HAL_IN, &(haldata->speed_command), hal_comp_id, "%s.speed-command", modname); if (retval!=0) goto out_closeHAL; retval = hal_pin_bit_newf(HAL_IN, &(haldata->spindle_on), hal_comp_id, "%s.spindle-on", modname); if (retval!=0) goto out_closeHAL; retval = hal_pin_bit_newf(HAL_IN, &(haldata->spindle_fwd), hal_comp_id, "%s.spindle-fwd", modname); if (retval!=0) goto out_closeHAL; retval = hal_pin_bit_newf(HAL_IN, &(haldata->spindle_rev), hal_comp_id, "%s.spindle-rev", modname); //JET if (retval!=0) goto out_closeHAL; retval = hal_pin_bit_newf(HAL_IN, &(haldata->err_reset), hal_comp_id, "%s.err-reset", modname); if (retval!=0) goto out_closeHAL; retval = hal_param_float_newf(HAL_RW, &(haldata->speed_tolerance), hal_comp_id, "%s.tolerance", modname); if (retval!=0) goto out_closeHAL; retval = hal_param_float_newf(HAL_RW, &(haldata->motor_hz), hal_comp_id, "%s.nameplate-HZ", modname); if (retval!=0) goto out_closeHAL; retval = hal_param_float_newf(HAL_RW, &(haldata->motor_RPM), hal_comp_id, "%s.nameplate-RPM", modname); if (retval!=0) goto out_closeHAL; retval = hal_param_s32_newf(HAL_RW, &(haldata->ack_delay), hal_comp_id, "%s.ack-delay", modname); if (retval!=0) goto out_closeHAL; /* make default data match what we expect to use */ *(haldata->stat1) = 0; *(haldata->stat2) = 0; *(haldata->freq_cmd) = 0; *(haldata->freq_out) = 0; *(haldata->curr_out) = 0; *(haldata->DCBusV) = 0; *(haldata->outV) = 0; *(haldata->RPM) = 0; *(haldata->scale_freq) = 0; *(haldata->power_factor) = 0; *(haldata->load_pct) = 0; *(haldata->FW_Rev) = 0; haldata->errorcount = 0; haldata->looptime = 0.1; haldata->motor_RPM = 1730; haldata->motor_hz = 60; haldata->speed_tolerance = 0.01; haldata->ack_delay = 2; *(haldata->err_reset) = 0; *(haldata->spindle_on) = 0; *(haldata->spindle_fwd) = 1; *(haldata->spindle_rev) = 0; haldata->old_run = -1; // make sure the initial value gets output haldata->old_dir = -1; haldata->old_err_reset = -1; hal_ready(hal_comp_id); /* here's the meat of the program. loop until done (which may be never) */ while (done==0) { read_data(mb_ctx, &slavedata, haldata); write_data(mb_ctx, &slavedata, haldata); /* don't want to scan too fast, and shouldn't delay more than a few seconds */ if (haldata->looptime < 0.001) haldata->looptime = 0.001; if (haldata->looptime > 2.0) haldata->looptime = 2.0; loop_timespec.tv_sec = (time_t)(haldata->looptime); loop_timespec.tv_nsec = (long)((haldata->looptime - loop_timespec.tv_sec) * 1000000000l); nanosleep(&loop_timespec, &remaining); } retval = 0; /* if we get here, then everything is fine, so just clean up and exit */ out_closeHAL: hal_exit(hal_comp_id); out_close: modbus_close(mb_ctx); modbus_free(mb_ctx); out_noclose: return retval; }
// init HAL objects static int export_halobjs(struct inst_data *ip, int owner_id, const char *name) { if (hal_pin_float_newf(HAL_OUT, &ip->out, owner_id, "%s.out", name)) return -1; if (hal_pin_float_newf(HAL_IN, &ip->in, owner_id, "%s.in", name)) return -1; if (hal_param_s32_newf(HAL_RO, &ip->iter,owner_id, "%s.iter", name)) return -1; // unittest observer pins, per instance if (hal_pin_s32_newf(HAL_OUT, &ip->repeat_value, owner_id, "%s.repeat_value", name)) return -1; if (hal_pin_s32_newf(HAL_OUT, &ip->prefix_len, owner_id, "%s.prefix_len", name)) return -1; // exporting '<instname>.funct' as an extended thread function // see lutn.c for a discussion of advantages hal_export_xfunct_args_t xfunct_args = { .type = FS_XTHREADFUNC, .funct.x = funct, .arg = ip, // the instance's HAL memory blob .uses_fp = 1, .reentrant = 0, .owner_id = owner_id }; return hal_export_xfunctf(&xfunct_args, "%s.funct", name); } // constructor - init all HAL pins, params, funct etc here static int instantiate(const int argc, const char**argv) { // argv[0]: component name const char *name = argv[1]; // instance name struct inst_data *ip; // allocate a named instance, and some HAL memory for the instance data int inst_id = hal_inst_create(name, comp_id, sizeof(struct inst_data), (void **)&ip); if (inst_id < 0) return -1; // here ip is guaranteed to point to a blob of HAL memory // of size sizeof(struct inst_data). HALINFO("inst=%s argc=%d\n", name, argc); HALINFO("instance parms: repeat=%d prefix='%s'", repeat, prefix); HALINFO("module parms: answer=%d text='%s'", answer, text); // example - parse newinst arguments getopt-style: // straight from: http://www.informit.com/articles/article.aspx?p=175771&seqNum=3 int do_all, do_help, do_verbose; /* flag variables */ char *myfile; struct option longopts[] = { { "all", no_argument, & do_all, 1 }, { "file", required_argument, NULL, 'f' }, { "help", no_argument, & do_help, 1 }, { "verbose", no_argument, & do_verbose, 1 }, { 0, 0, 0, 0 } }; int c; while ((c = getopt_long(argc, argv, ":f:", longopts, NULL)) != -1) { switch (c) { case 'f': myfile = optarg; break; case 0: // getopt_long() set a variable, just keep going break; case ':': // missing option argument HALERR("%s: option `-%c' requires an argument\n", argv[1], optopt); break; case '?': default: // invalid option HALERR("%s: option `-%c' is invalid, ignored", argv[1], optopt); break; } } HALINFO("do_all=%d do_help=%d do_verbose=%d myfile=%s", do_all, do_help, do_verbose, myfile); // these pins/params/functs will be owned by the instance, // and can be separately exited via 'halcmd delinst <instancename>' int retval = export_halobjs(ip, inst_id, name); // unittest: echo instance parameters into observer pins if (!retval) { *(ip->repeat_value) = repeat; *(ip->prefix_len) = strlen(prefix); } return retval; } // custom destructor // pins, params, and functs are automatically deallocated by hal_exit(comp_id) // regardless if a destructor is used or not (see below), so usually it is not // needed // // however, some objects like vtables, rings, threads, signals are not owned by // a component, hence cannot be automatically exited by hal_exit() even if desired // // interaction with such objects may require a custom destructor like below for // cleanup actions // NB: if a customer destructor is used, it is called // - after the instance's functs have been removed from their respective threads // (so a thread funct call cannot interact with the destructor any more) // - any pins and params of this instance are still intact when the destructor is // called, and they are automatically destroyed by the HAL library once the // destructor returns static int delete(const char *name, void *inst, const int inst_size) { HALERR("inst=%s size=%d %p\n", name, inst_size, inst); HALERR("instance parms: repeat=%d prefix='%s'", repeat, prefix); HALERR("module parms: answer=%d text='%s'", answer, text); return 0; } int rtapi_app_main(void) { HALERR("instance parms: repeat=%d prefix='%s'", repeat, prefix); HALERR("module parms: answer=%d text='%s'", answer, text); // to use default destructor, use NULL instead of delete comp_id = hal_xinit(TYPE_RT, 0, 0, instantiate, delete, compname); if (comp_id < 0) return comp_id; #if 0 // this is how an 'instance' would have been done in the legacy way struct inst_data *ip = hal_malloc(sizeof(struct inst_data)); // these pins/params/functs will be owned by the component // NB: this 'instance' cannot be exited, and no new one created on the fly if (export_halobjs(ip, comp_id, "foo")) return -1; #endif // unittest only, see nosetests/unittest_instbindings.py and // nosetests/unittest_icomp.py // purpose: echo module params into observer pins // (cant set pins to strings, so just echo the string length) if ((cd = export_observer_pins(comp_id, compname)) == NULL) return -1; *(cd->answer_value) = answer; *(cd->text_len) = strlen(text); hal_ready(comp_id); return 0; }
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_bit_newf(HAL_OUT, &(addr->enable), comp_id, "%s.enable", 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->enable) = 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 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 hm2_dpll_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, 7, 4, 0x0000)) { HM2_ERR("inconsistent Module Descriptor!\n"); return -EINVAL; } if (hm2->config.num_dplls == 0) return 0; if (hm2->config.num_dplls > md->instances) { hm2->dpll.num_instances = md->instances; HM2_ERR( "There are only %d dplls on this board type, using %d\n", md->instances, md->instances ); } else if (hm2->config.num_dplls == -1) { hm2->dpll.num_instances = md->instances; } else hm2->dpll.num_instances = hm2->config.num_dplls; // // looks good, start initializing // hm2->dpll.clock_frequency = md->clock_freq; hm2->dpll.base_rate_addr = md->base_address + 0 * md->register_stride; hm2->dpll.phase_err_addr = md->base_address + 1 * md->register_stride; hm2->dpll.control_reg0_addr = md->base_address + 2 * md->register_stride; hm2->dpll.control_reg1_addr = md->base_address + 3 * md->register_stride; hm2->dpll.timer_12_addr = md->base_address + 4 * md->register_stride; hm2->dpll.timer_34_addr = md->base_address + 5 * md->register_stride; hm2->dpll.hm2_dpll_sync_addr = md->base_address + 6 * md->register_stride; // export to HAL hm2->dpll.pins = hal_malloc(sizeof(hm2_dpll_pins_t)); r = hal_pin_float_newf(HAL_IN, &(hm2->dpll.pins->time1_us), hm2->llio->comp_id, "%s.dpll.01.timer-us", hm2->llio->name); r += hal_pin_float_newf(HAL_IN, &(hm2->dpll.pins->time2_us), hm2->llio->comp_id, "%s.dpll.02.timer-us", hm2->llio->name); r += hal_pin_float_newf(HAL_IN, &(hm2->dpll.pins->time3_us), hm2->llio->comp_id, "%s.dpll.03.timer-us", hm2->llio->name); r += hal_pin_float_newf(HAL_IN, &(hm2->dpll.pins->time4_us), hm2->llio->comp_id, "%s.dpll.04.timer-us", hm2->llio->name); r += hal_pin_float_newf(HAL_IN, &(hm2->dpll.pins->base_freq), hm2->llio->comp_id, "%s.dpll.base-freq-khz", hm2->llio->name); r += hal_pin_float_newf(HAL_OUT, &(hm2->dpll.pins->phase_error), hm2->llio->comp_id, "%s.dpll.phase-error-us", hm2->llio->name); r += hal_pin_u32_newf(HAL_IN, &(hm2->dpll.pins->time_const), hm2->llio->comp_id, "%s.dpll.time-const", hm2->llio->name); r += hal_pin_u32_newf(HAL_IN, &(hm2->dpll.pins->plimit), hm2->llio->comp_id, "%s.dpll.plimit", hm2->llio->name); r += hal_pin_u32_newf(HAL_OUT, &(hm2->dpll.pins->ddssize), hm2->llio->comp_id, "%s.dpll.ddsize", hm2->llio->name); r += hal_pin_u32_newf(HAL_OUT, &(hm2->dpll.pins->prescale), hm2->llio->comp_id, "%s.dpll.prescale", hm2->llio->name); if (r < 0) { HM2_ERR("error adding hm2_dpll timer pins, Aborting\n"); goto fail0; } *hm2->dpll.pins->time1_us = 100.0; *hm2->dpll.pins->time2_us = 100.0; *hm2->dpll.pins->time3_us = 100.0; *hm2->dpll.pins->time4_us = 100.0; *hm2->dpll.pins->prescale = 1; *hm2->dpll.pins->base_freq = -1; // An indication it needs init *hm2->dpll.pins->time_const = 0xA000; *hm2->dpll.pins->plimit = 0x400000; r = hm2_register_tram_read_region(hm2, hm2->dpll.hm2_dpll_sync_addr, sizeof(u32), &hm2->dpll.hm2_dpll_sync_reg); if (r < 0) { HM2_ERR("Error registering tram synch write. Aborting\n"); goto fail0; } r = hm2_register_tram_read_region(hm2, hm2->dpll.control_reg1_addr, sizeof(u32), &hm2->dpll.control_reg1_read); if (r < 0) { HM2_ERR("Error registering dpll control reg 1. Aborting\n"); goto fail0; } return hm2->dpll.num_instances; fail0: return r; }
struct shuttlexpress *check_for_shuttlexpress(char *dev_filename) { struct shuttlexpress *s; struct hidraw_devinfo devinfo; char name[100]; int r; printf("%s: checking %s\n", modname, dev_filename); s = (struct shuttlexpress *)calloc(1, sizeof(struct shuttlexpress)); if (s == NULL) { fprintf(stderr, "%s: out of memory!\n", modname); return NULL; } s->device_file = dev_filename; s->fd = open(s->device_file, O_RDONLY); if (s->fd < 0) { fprintf(stderr, "%s: error opening %s: %s\n", modname, s->device_file, strerror(errno)); if (errno == EACCES) { fprintf(stderr, "%s: make sure you have read permission on %s, read the shuttlexpress(1) manpage for more info\n", modname, s->device_file); } goto fail0; } r = ioctl(s->fd, HIDIOCGRAWINFO, &devinfo); if (r < 0) { fprintf(stderr, "%s: error with ioctl HIDIOCGRAWINFO on %s: %s\n", modname, s->device_file, strerror(errno)); goto fail1; } if (devinfo.vendor != VENDOR_ID) { fprintf(stderr, "%s: dev %s has unexpected Vendor ID 0x%04x (expected Contour Design, 0x%04x)\n", modname, s->device_file, devinfo.vendor, VENDOR_ID); goto fail1; } if (devinfo.product != PRODUCT_ID) { fprintf(stderr, "%s: dev %s has unexpected Product ID 0x%04x (expected ShuttleXpress, 0x%04x)\n", modname, s->device_file, devinfo.product, PRODUCT_ID); goto fail1; } r = ioctl(s->fd, HIDIOCGRAWNAME(99), name); if (r < 0) { fprintf(stderr, "%s: error with ioctl HIDIOCGRAWNAME on %s: %s\n", modname, s->device_file, strerror(errno)); goto fail1; } printf("%s: found %s on %s\n", modname, name, s->device_file); s->hal = (struct shuttlexpress_hal *)hal_malloc(sizeof(struct shuttlexpress_hal)); if (s->hal == NULL) { fprintf(stderr, "%s: ERROR: unable to allocate HAL shared memory\n", modname); goto fail1; } r = hal_pin_bit_newf(HAL_OUT, &(s->hal->button_0), hal_comp_id, "%s.%d.button-0", modname, num_devices); if (r != 0) goto fail1; r = hal_pin_bit_newf(HAL_OUT, &(s->hal->button_0_not), hal_comp_id, "%s.%d.button-0-not", modname, num_devices); if (r != 0) goto fail1; r = hal_pin_bit_newf(HAL_OUT, &(s->hal->button_1), hal_comp_id, "%s.%d.button-1", modname, num_devices); if (r != 0) goto fail1; r = hal_pin_bit_newf(HAL_OUT, &(s->hal->button_1_not), hal_comp_id, "%s.%d.button-1-not", modname, num_devices); if (r != 0) goto fail1; r = hal_pin_bit_newf(HAL_OUT, &(s->hal->button_2), hal_comp_id, "%s.%d.button-2", modname, num_devices); if (r != 0) goto fail1; r = hal_pin_bit_newf(HAL_OUT, &(s->hal->button_2_not), hal_comp_id, "%s.%d.button-2-not", modname, num_devices); if (r != 0) goto fail1; r = hal_pin_bit_newf(HAL_OUT, &(s->hal->button_3), hal_comp_id, "%s.%d.button-3", modname, num_devices); if (r != 0) goto fail1; r = hal_pin_bit_newf(HAL_OUT, &(s->hal->button_3_not), hal_comp_id, "%s.%d.button-3-not", modname, num_devices); if (r != 0) goto fail1; r = hal_pin_bit_newf(HAL_OUT, &(s->hal->button_4), hal_comp_id, "%s.%d.button-4", modname, num_devices); if (r != 0) goto fail1; r = hal_pin_bit_newf(HAL_OUT, &(s->hal->button_4_not), hal_comp_id, "%s.%d.button-4-not", modname, num_devices); if (r != 0) goto fail1; r = hal_pin_s32_newf(HAL_OUT, &(s->hal->counts), hal_comp_id, "%s.%d.counts", modname, num_devices); if (r != 0) goto fail1; r = hal_pin_float_newf(HAL_OUT, &(s->hal->spring_wheel_f), hal_comp_id, "%s.%d.spring-wheel-f", modname, num_devices); if (r != 0) goto fail1; r = hal_pin_s32_newf(HAL_OUT, &(s->hal->spring_wheel_s32), hal_comp_id, "%s.%d.spring-wheel-s32", modname, num_devices); if (r != 0) goto fail1; *s->hal->button_0 = 0; *s->hal->button_0_not = 1; *s->hal->button_1 = 0; *s->hal->button_1_not = 1; *s->hal->button_2 = 0; *s->hal->button_2_not = 1; *s->hal->button_3 = 0; *s->hal->button_3_not = 1; *s->hal->button_4 = 0; *s->hal->button_4_not = 1; *s->hal->counts = 0; *s->hal->spring_wheel_f = 0.0; *s->hal->spring_wheel_s32 = 0; return s; fail1: close(s->fd); fail0: free(s); return NULL; }