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