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