static int Init_EL100x(int* off, const uint16_t position, const uint16_t index, const uint32_t vendor_id, const uint32_t product_code, const ec_sync_info_t syncs[])
{
ec_slave_config_t *sc;
if (!(sc = ecrt_master_slave_config(master, 0, position, vendor_id, product_code))) {
fprintf(stderr, "Failed to get slave configuration.\n");
return -1;
}
if (ecrt_slave_config_pdos(sc, EC_END, syncs)) {
fprintf(stderr, "Failed to configure PDOs.\n");
return -1;
}
if (0 > (*off = ecrt_slave_config_reg_pdo_entry(sc, index, 1, domain1, NULL))) {
fprintf(stderr, "Failed to configure reg PDOs.\n");
return -1;
}
printf("Slave configured @ pos %d.\n", position);
return 0;
}
bool ECDomain::configure() {
for (std::vector<ECDriver::Ptr>::iterator driver = drivers_.begin();
driver != drivers_.end(); ++driver) {
std::vector<ECPDOEntryInterface*> pdo_entries = (*driver)->getPDOConfig();
ECSlave::Ptr slave = (*driver)->getSlave();
for (std::vector<ECPDOEntryInterface*>::iterator pdo = pdo_entries.begin();
pdo != pdo_entries.end(); ++pdo) {
int offset = ecrt_slave_config_reg_pdo_entry(
slave->slave_, (*pdo)->getIndex(), (*pdo)->getSubIndex(), domain_, 0);
if (offset < 0) {
RTT::log(RTT::Error) << "Unable to map PDO entry ["<< (*pdo)->getIndex()
<< ":" << (*pdo)->getSubIndex() << "] to domain" << RTT::endlog();
return false;
} else {
(*(*pdo)->getOffsetPtr()) = offset;
}
}
}
return true;
}
/** calls ecrt_slave_config_reg_pdo_entry to get offset for reading and writing
*/
unsigned int AbstractTerminal::regPdo( uint16_t index, uint8_t subindex, ec_domain_t *domain )
{
_offset = ecrt_slave_config_reg_pdo_entry( _slave_config, index, subindex, domain, nullptr );
return _offset;
}
int __init init_mod(void)
{
int ret = -1, i;
RTIME tick_period, requested_ticks, now;
ec_slave_config_t *sc;
printk(KERN_INFO PFX "Starting...\n");
rt_sem_init(&master_sem, 1);
t_critical = cpu_khz * 1000 / FREQUENCY - cpu_khz * INHIBIT_TIME / 1000;
master = ecrt_request_master(0);
if (!master) {
ret = -EBUSY;
printk(KERN_ERR PFX "Requesting master 0 failed!\n");
goto out_return;
}
ecrt_master_callbacks(master, send_callback, receive_callback, master);
printk(KERN_INFO PFX "Registering domain...\n");
if (!(domain1 = ecrt_master_create_domain(master))) {
printk(KERN_ERR PFX "Domain creation failed!\n");
goto out_release_master;
}
printk(KERN_INFO PFX "Configuring PDOs...\n");
// create configuration for reference clock FIXME
if (!(sc = ecrt_master_slave_config(master, 0, 0, Beckhoff_EK1100))) {
printk(KERN_ERR PFX "Failed to get slave configuration.\n");
goto out_release_master;
}
for (i = 0; i < NUM_DIG_OUT; i++) {
if (!(sc = ecrt_master_slave_config(master,
DigOutSlavePos(i), Beckhoff_EL2008))) {
printk(KERN_ERR PFX "Failed to get slave configuration.\n");
goto out_release_master;
}
if (ecrt_slave_config_pdos(sc, EC_END, el2008_syncs)) {
printk(KERN_ERR PFX "Failed to configure PDOs.\n");
goto out_release_master;
}
off_dig_out[i] = ecrt_slave_config_reg_pdo_entry(sc,
0x7000, 1, domain1, NULL);
if (off_dig_out[i] < 0)
goto out_release_master;
}
if (!(sc = ecrt_master_slave_config(master,
CounterSlavePos, IDS_Counter))) {
printk(KERN_ERR PFX "Failed to get slave configuration.\n");
goto out_release_master;
}
off_counter_in = ecrt_slave_config_reg_pdo_entry(sc,
0x6020, 0x11, domain1, NULL);
if (off_counter_in < 0)
goto out_release_master;
off_counter_out = ecrt_slave_config_reg_pdo_entry(sc,
0x7020, 1, domain1, NULL);
if (off_counter_out < 0)
goto out_release_master;
// configure SYNC signals for this slave
ecrt_slave_config_dc(sc, 0x0700, 1000000, 440000, 0, 0);
printk(KERN_INFO PFX "Activating master...\n");
if (ecrt_master_activate(master)) {
printk(KERN_ERR PFX "Failed to activate master!\n");
goto out_release_master;
}
// Get internal process data for domain
domain1_pd = ecrt_domain_data(domain1);
printk(KERN_INFO PFX "Starting cyclic sample thread...\n");
requested_ticks = nano2count(TIMERTICKS);
tick_period = start_rt_timer(requested_ticks);
printk(KERN_INFO PFX "RT timer started with %i/%i ticks.\n",
(int) tick_period, (int) requested_ticks);
if (rt_task_init(&task, run, 0, 2000, 0, 1, NULL)) {
printk(KERN_ERR PFX "Failed to init RTAI task!\n");
goto out_stop_timer;
}
now = rt_get_time();
if (rt_task_make_periodic(&task, now + tick_period, tick_period)) {
printk(KERN_ERR PFX "Failed to run RTAI task!\n");
goto out_stop_task;
}
printk(KERN_INFO PFX "Initialized.\n");
return 0;
out_stop_task:
rt_task_delete(&task);
out_stop_timer:
stop_rt_timer();
out_release_master:
printk(KERN_ERR PFX "Releasing master...\n");
ecrt_release_master(master);
out_return:
rt_sem_delete(&master_sem);
printk(KERN_ERR PFX "Failed to load. Aborting.\n");
return ret;
}
int uei_ethercat_initialize (void)
{
uint8_t *data;
ec_pdo_entry_reg_t rw_pdos[] = {
// {RW_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_CURRENT_LOOP_CI, current_ci_off, NULL},
// {RW_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_CURRENT_LOOP_CP, current_cp_off, NULL},
// {RW_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_CURRENT_LOOP_OFFSET, current_offset_off, NULL},
{RW_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_CURRENT_LOOP_VAL, current_val_off, NULL},
// {RW_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_MOTOR_POSITION, motor_pos_off, NULL},
{RW_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_DRIVE_STATE, state_off, NULL},
// {RW_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_DRIVE_STATUS, status_off, NULL},
// {RW_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_DRIVE_TEMP, drive_temp_off, NULL},
// {RW_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_LATCHED_DRIVE_FAULT, latched_fault_off, NULL},
// {RW_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_LATCHED_DRIVE_STATUS, latched_status_off, NULL},
// {RW_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_MOTOR_TEMP_VOLTAGE, motor_temp_v_off, NULL},
// {RW_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_MOTOR_ENC_WRAP_POS, motor_enc_wrap_off, NULL},
{0, 0, 0x00, 0x00, 0x0, 0x0, NULL, NULL}};
// ec_pdo_entry_reg_t pv_pdos[] = {
// {PV_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, BEL_090_020_PRODCODE, ECAT_CURRENT_LOOP_CI, current_ci_off+1, NULL},
// {PV_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, BEL_090_020_PRODCODE, ECAT_CURRENT_LOOP_CP, current_cp_off+1, NULL},
// {PV_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, BEL_090_020_PRODCODE, ECAT_CURRENT_LOOP_OFFSET, current_offset_off+1, NULL},
// {PV_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, BEL_090_020_PRODCODE, ECAT_CURRENT_LOOP_VAL, current_val_off+1, NULL},
// {PV_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, BEL_090_020_PRODCODE, ECAT_DRIVE_STATUS, status_off+1, NULL},
// {PV_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, BEL_090_020_PRODCODE, ECAT_DRIVE_TEMP, drive_temp_off+1, NULL},
// {PV_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, BEL_090_020_PRODCODE, ECAT_LATCHED_DRIVE_FAULT, latched_fault_off+1, NULL},
// {PV_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, BEL_090_020_PRODCODE, ECAT_LATCHED_DRIVE_STATUS, latched_status_off+1, NULL},
// {PV_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, BEL_090_020_PRODCODE, ECAT_MOTOR_POSITION, motor_pos_off+1, NULL},
// {PV_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, BEL_090_020_PRODCODE, ECAT_MOTOR_TEMP_VOLTAGE, motor_temp_v_off+1, NULL},
// {PV_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, BEL_090_020_PRODCODE, ECAT_MOTOR_ENC_WRAP_POS, motor_enc_wrap_off+1, NULL},
// {PV_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, BEL_090_020_PRODCODE, ECAT_DRIVE_STATE, state_off+1, NULL},
// {0, 0, 0x00, 0x00, 0x0, 0x0, NULL, NULL}};
// ec_pdo_entry_reg_t el_pdos[] = {
// {EL_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_CURRENT_LOOP_CI, current_ci_off+2, NULL},
// {EL_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_CURRENT_LOOP_CP, current_cp_off+2, NULL},
// {EL_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_CURRENT_LOOP_OFFSET, current_offset_off+2, NULL},
// {EL_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_CURRENT_LOOP_VAL, current_val_off+2, NULL},
// {EL_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_DRIVE_STATUS, status_off+2, NULL},
// {EL_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_DRIVE_TEMP, drive_temp_off+2, NULL},
// {EL_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_LATCHED_DRIVE_FAULT, latched_fault_off+2, NULL},
// {EL_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_LATCHED_DRIVE_STATUS, latched_status_off+2, NULL},
// {EL_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_MOTOR_POSITION, motor_pos_off+2, NULL},
// {EL_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_MOTOR_TEMP_VOLTAGE, motor_temp_v_off+2, NULL},
// {EL_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_MOTOR_ENC_WRAP_POS, motor_enc_wrap_off+2, NULL},
// {EL_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_DRIVE_STATE, state_off+2, NULL},
// {0, 0, 0x00, 0x00, 0x0, 0x0, NULL, NULL}};
master = ecrt_request_master(0);
if (!master){
printf("Could not request master!\n");
return -1;
}
domain = ecrt_master_create_domain(master);
if (!domain) {
printf("Could not create domain!\n");
return -1;
}
printf("Created Domain\n");
if (!(rx_controller = ecrt_master_slave_config(master,RW_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE))) {
fprintf(stderr, "Failed to get slave configuration for Reaction Wheel controller!\n");
return -1;
}
// if (!(pv_controller = ecrt_master_slave_config(master,PV_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, BEL_090_020_PRODCODE))) {
// fprintf(stderr, "Failed to get slave configuration for Pivot Motor controller!\n");
// return -1;
// }
// if (!(el_controller = ecrt_master_slave_config(master,EL_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE))) {
// fprintf(stderr, "Failed to get slave configuration for Elevation Motor controller!\n");
// return -1;
// }
if (ecrt_slave_config_pdos(rx_controller, EC_END, copley_pdo_syncs)) {
perror("ecrt_slave_config_pdos() failed for RX controller.");
ecrt_release_master(master);
return 3;
}
// if (ecrt_slave_config_pdos(pv_controller, 1, copley_pdo_syncs)) {
// perror("ecrt_slave_config_pdos() failed for Pivot controller.");
// ecrt_release_master(master);
// return 3;
// }
// if (ecrt_slave_config_pdos(el_controller, 1, copley_pdo_syncs)) {
// perror("ecrt_slave_config_pdos() failed for Elevation controller.");
// ecrt_release_master(master);
// return 3;
// }
/// Register the PDO list and variable mappings
// if (ecrt_domain_reg_pdo_entry_list(domain, rw_pdos)) {
// perror("ecrt_domain_reg_pdo_entry_list() failed for reaction wheel!");
// ecrt_release_master(master);
// return -1;
// }
// if (ecrt_domain_reg_pdo_entry_list(domain, pv_pdos)) {
// perror("ecrt_domain_reg_pdo_entry_list() failed for pivot motor!");
// ecrt_release_master(master);
// return -1;
// }
// if (ecrt_domain_reg_pdo_entry_list(domain, el_pdos)) {
// perror("ecrt_domain_reg_pdo_entry_list() failed for Elevation motor!");
// ecrt_release_master(master);
// return -1;
// }
state_off[0] = ecrt_slave_config_reg_pdo_entry(rx_controller,
ECAT_DRIVE_STATE, domain, NULL);
current_val_off[0] = ecrt_slave_config_reg_pdo_entry(rx_controller,
ECAT_CURRENT_LOOP_VAL, domain, NULL);
// configure SYNC signals for this slave
ecrt_slave_config_dc(rx_controller, 0, 1000000000ll / 100, 4400000, 0, 0);
printf("Set Master/Slave Configuration\n");
if (ecrt_master_activate(master) < 0) {
printf("Could not activate master!\n");
return -1;
}
if (!(data = ecrt_domain_data(domain))) {
perror("ecrt_domain_data() failed!");
ecrt_release_master(master);
return -1;
}
ethercat_set_offsets(&rx_controller_state, data, 0);
// ethercat_set_offsets(&pv_controller_state, data, 1);
// ethercat_set_offsets(&el_controller_state, data, 2);
check_domain1_state();
check_master_state();
printf("Data: %p\t Current: %p\t State: %p\n", data, rx_controller_state.current_val, rx_controller_state.amp_state);
ecrt_master_receive(master);
ecrt_domain_process(domain);
EC_WRITE_S16(data + current_val_off[0], 0);
EC_WRITE_U16(data + state_off[0], ECAT_STATE_DISABLED);
ecrt_domain_queue(domain);
ecrt_master_send(master);
check_domain1_state();
check_master_state();
return 0;
}
int el60xx_port_init(el60xx_port_t *port, ec_slave_config_t *sc,
ec_domain_t *domain, unsigned int slot_offset, const char *name)
{
int ret = 0;
strncpy(port->name, name, EL6002_PORT_NAME_SIZE);
port->tty = ectty_create(&el60xx_tty_ops, port);
if (IS_ERR(port->tty)) {
printk(KERN_ERR PFX "Failed to create tty for %s.\n",
port->name);
ret = PTR_ERR(port->tty);
goto out_return;
}
port->max_tx_data_size = 22;
port->max_rx_data_size = 22;
port->tx_data = NULL;
port->tx_data_size = 0;
port->state = SER_REQUEST_INIT;
port->tx_request_toggle = 0;
port->rx_accepted_toggle = 0;
port->control = 0x0000;
port->off_ctrl = 0;
port->off_tx = 0;
port->off_status = 0;
port->off_rx = 0;
port->requested_rtscts = 0x00; // no hardware handshake
port->current_rtscts = 0xff;
port->requested_baud_rate = 6; // 9600
port->current_baud_rate = 0;
port->requested_data_frame = 0x03; // 8N1
port->current_data_frame = 0x00;
port->config_error = 0;
if (!(port->rtscts_sdo = ecrt_slave_config_create_sdo_request(sc,
0x8000 + slot_offset, 0x01, 1))) {
printk(KERN_ERR PFX "Failed to create SDO request for %s.\n",
port->name);
ret = -ENOMEM;
goto out_free_tty;
}
if (!(port->baud_sdo = ecrt_slave_config_create_sdo_request(sc,
0x8000 + slot_offset, 0x11, 1))) {
printk(KERN_ERR PFX "Failed to create SDO request for %s.\n",
port->name);
ret = -ENOMEM;
goto out_free_tty;
}
if (!(port->frame_sdo = ecrt_slave_config_create_sdo_request(sc,
0x8000 + slot_offset, 0x15, 1))) {
printk(KERN_ERR PFX "Failed to create SDO request for %s\n",
port->name);
ret = -ENOMEM;
goto out_free_tty;
}
ret = ecrt_slave_config_reg_pdo_entry(
sc, 0x7001 + slot_offset, 0x01, domain, NULL);
if (ret < 0) {
printk(KERN_ERR PFX "Failed to register PDO entry of %s\n",
port->name);
goto out_free_tty;
}
port->off_ctrl = ret;
ret = ecrt_slave_config_reg_pdo_entry(
sc, 0x7000 + slot_offset, 0x11, domain, NULL);
if (ret < 0) {
printk(KERN_ERR PFX "Failed to register PDO entry of %s.\n",
port->name);
goto out_free_tty;
}
port->off_tx = ret;
ret = ecrt_slave_config_reg_pdo_entry(
sc, 0x6001 + slot_offset, 0x01, domain, NULL);
if (ret < 0) {
printk(KERN_ERR PFX "Failed to register PDO entry of %s.\n",
port->name);
goto out_free_tty;
}
port->off_status = ret;
ret = ecrt_slave_config_reg_pdo_entry(
sc, 0x6000 + slot_offset, 0x11, domain, NULL);
if (ret < 0) {
printk(KERN_ERR PFX "Failed to register PDO entry of %s.\n",
port->name);
goto out_free_tty;
}
port->off_rx = ret;
if (port->max_tx_data_size > 0) {
port->tx_data = kmalloc(port->max_tx_data_size, GFP_KERNEL);
if (port->tx_data == NULL) {
printk(KERN_ERR PFX "Failed to allocate %u bytes of TX"
" memory for %s.\n", port->max_tx_data_size, port->name);
ret = -ENOMEM;
goto out_free_tty;
}
}
return 0;
out_free_tty:
ectty_free(port->tty);
out_return:
return ret;
}
