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 main(int argc, char **argv)
{
ec_slave_config_t *sc;
if (mlockall(MCL_CURRENT | MCL_FUTURE) == -1)
{
perror("mlockall failed");
return -1;
}
master = ecrt_request_master(0);
if (!master)
return -1;
domain_r = ecrt_master_create_domain(master);
if (!domain_r)
return -1;
domain_w = ecrt_master_create_domain(master);
if (!domain_w)
return -1;
if (!(sc = ecrt_master_slave_config(master, pana, panasonic)))
{
fprintf(stderr, "Failed to get slave1 configuration.\n");
return -1;
}
#if SDO_ACCESS
if (ecrt_slave_config_sdo8(sc, 0x6060, 0, 8))
{
return -1;
}
#endif
#if CONFIGURE_PDOS
printf("Configuring PDOs...\n");
if (ecrt_slave_config_pdos(sc, EC_END, slave_0_syncs))
{
fprintf(stderr, "Failed to configure 1st PDOs.\n");
return -1;
}
#endif
if (ecrt_domain_reg_pdo_entry_list(domain_r, domain_r_regs))
{
fprintf(stderr, "1st motor RX_PDO entry registration failed!\n");
return -1;
}
if (ecrt_domain_reg_pdo_entry_list(domain_w, domain_w_regs))
{
fprintf(stderr, "1st motor TX_PDO entry registration failed!\n");
return -1;
}
ecrt_slave_config_dc(sc,0x0300,500000,0,0,0);
printf("Activating master...\n");
if (ecrt_master_activate(master))
return -1;
if (!(domain_r_pd = ecrt_domain_data(domain_r)))
{
return -1;
}
if (!(domain_w_pd = ecrt_domain_data(domain_w)))
{
return -1;
}
pid_t pid = getpid();
if (setpriority(PRIO_PROCESS, pid, -20))
fprintf(stderr, "Warning: Failed to set priority: %s\n",
strerror(errno));
signal( SIGINT , endsignal );
printf("Starting cyclic function.\n");
cyclic_task();
ecrt_release_master(master);
return 0;
}
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;
}
