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