# HG changeset patch # User Florian Pose # Date 1143474728 0 # Node ID b36d187ecc0b1f24fb6c8285ca0365aec7933a1e # Parent 63a5b40eb7dae224b068047207f19c905093e0b5 Unterst?tzte Prot. auslesen, Mailbox in Slave ausgelagert, Bugfix in Anzeige des Watch-Frames und erste SDO-List-Abfrage. diff -r 63a5b40eb7da -r b36d187ecc0b master/canopen.c --- a/master/canopen.c Mon Mar 27 12:04:55 2006 +0000 +++ b/master/canopen.c Mon Mar 27 15:52:08 2006 +0000 @@ -29,6 +29,8 @@ const ec_sdo_abort_message_t sdo_abort_messages[]; +void ec_canopen_abort_msg(uint32_t); + /*****************************************************************************/ /** @@ -42,107 +44,44 @@ size_t size /**< Größe des Datenfeldes */ ) { - uint8_t data[0xF6]; - ec_command_t command; + uint8_t data[0x0A]; unsigned int i; - ec_master_t *master; - cycles_t start, end, timeout; - uint32_t abort_code; - const ec_sdo_abort_message_t *abort_msg; - - memset(data, 0x00, 0xF6); - - master = slave->master; + size_t rec_size; if (size == 0 || size > 4) { EC_ERR("Invalid SDO data size: %i!\n", size); return -1; } - EC_WRITE_U16(data, 0x000A); // Length of the Mailbox service data - EC_WRITE_U16(data + 2, slave->station_address); // Station address - EC_WRITE_U8 (data + 4, 0x00); // Channel & priority - EC_WRITE_U8 (data + 5, 0x03); // CANopen over EtherCAT - EC_WRITE_U16(data + 6, 0x02 << 12); // Number (0), Service (SDO request) - EC_WRITE_U8 (data + 8, 0x23 | ((4 - size) << 2)); // Spec., exp., init. - EC_WRITE_U16(data + 9, sdo_index); - EC_WRITE_U8 (data + 11, sdo_subindex); + EC_WRITE_U16(data, 0x02 << 12); // Number (0), Service (SDO request) + EC_WRITE_U8 (data + 2, 0x23 | ((4 - size) << 2)); // Spec., exp., init. + EC_WRITE_U16(data + 3, sdo_index); + EC_WRITE_U8 (data + 5, sdo_subindex); for (i = 0; i < size; i++) { - EC_WRITE_U8(data + 12 + i, value & 0xFF); + EC_WRITE_U8(data + 6 + i, value & 0xFF); value >>= 8; } - ec_command_init_npwr(&command, slave->station_address, 0x1800, 0xF6, data); - if (unlikely(ec_master_simple_io(master, &command))) { - EC_ERR("Mailbox sending failed on slave %i!\n", slave->ring_position); - return -1; - } - - // Read "written bit" of Sync-Manager - start = get_cycles(); - timeout = (cycles_t) 10 * cpu_khz; // 10ms - - while (1) - { - udelay(100); - - ec_command_init_nprd(&command, slave->station_address, 0x808, 8); - if (unlikely(ec_master_simple_io(master, &command))) { - EC_ERR("Mailbox checking failed on slave %i!\n", - slave->ring_position); - return -1; - } - - end = get_cycles(); - - if (EC_READ_U8(command.data + 5) & 8) break; // Written bit is high - - if ((end - start) >= timeout) { - EC_ERR("Mailbox check - Slave %i timed out.\n", - slave->ring_position); - return -1; - } - } - - if (unlikely(slave->master->debug_level) > 1) - EC_DBG("SDO download took %ius.\n", ((u32) (end - start) * 1000 - / cpu_khz)); - - ec_command_init_nprd(&command, slave->station_address, 0x18F6, 0xF6); - if (unlikely(ec_master_simple_io(master, &command))) { - EC_ERR("Mailbox receiving failed on slave %i!\n", - slave->ring_position); - return -1; - } - - if (EC_READ_U8 (command.data + 5) != 0x03) { // nicht CoE - EC_ERR("Invalid mailbox response (non-CoE) at slave %i!\n", - slave->ring_position); - return -1; - } - - if (EC_READ_U16(command.data + 6) >> 12 == 0x02 && // SDO request - EC_READ_U8 (command.data + 8) >> 5 == 0x04) { // Abort SDO transf. req. + // Mailox senden und empfangen + if (ec_slave_mailbox_send(slave, 0x03, data, 0x0A)) return -1; + + rec_size = 0x0A; + if (ec_slave_mailbox_receive(slave, 0x03, data, &rec_size)) return -1; + + if (EC_READ_U16(data) >> 12 == 0x02 && // SDO request + EC_READ_U8 (data + 2) >> 5 == 0x04) { // Abort SDO transf. req. EC_ERR("SDO download of 0x%04X:%X (value %X, size %X) aborted on slave" " %i.\n", sdo_index, sdo_subindex, value, size, slave->ring_position); - abort_code = EC_READ_U32(command.data + 12); - for (abort_msg = sdo_abort_messages; abort_msg->code; abort_msg++) { - if (abort_msg->code == abort_code) { - EC_ERR("SDO abort message 0x%08X: \"%s\".\n", - abort_msg->code, abort_msg->message); - return -1; - } - } - EC_ERR("Unknown SDO abort code 0x%08X.\n", abort_code); - return -1; - } - - if (EC_READ_U16(command.data + 6) >> 12 != 0x03 || // SDO response - EC_READ_U8 (command.data + 8) >> 5 != 0x03 || // Download response - EC_READ_U16(command.data + 9) != sdo_index || // Index - EC_READ_U8 (command.data + 11) != sdo_subindex) // Subindex + ec_canopen_abort_msg(EC_READ_U32(data + 6)); + return -1; + } + + if (EC_READ_U16(data) >> 12 != 0x03 || // SDO response + EC_READ_U8 (data + 2) >> 5 != 0x03 || // Download response + EC_READ_U16(data + 3) != sdo_index || // Index + EC_READ_U8 (data + 5) != sdo_subindex) // Subindex { EC_ERR("Invalid SDO download response at slave %i!\n", slave->ring_position); @@ -164,101 +103,37 @@ uint32_t *value /**< Speicher für gel. Wert */ ) { - uint8_t data[0xF6]; - ec_command_t command; - ec_master_t *master; - cycles_t start, end, timeout; - uint32_t abort_code; - const ec_sdo_abort_message_t *abort_msg; - - memset(data, 0x00, 0xF6); - master = slave->master; - - EC_WRITE_U16(data, 0x0006); // Length of the Mailbox service data - EC_WRITE_U16(data + 2, slave->station_address); // Station address - EC_WRITE_U8 (data + 4, 0x00); // Channel & priority - EC_WRITE_U8 (data + 5, 0x03); // CANopen over EtherCAT - EC_WRITE_U16(data + 6, 0x2000); // Number (0), Service (SDO request) - EC_WRITE_U8 (data + 8, 0x1 << 1 | 0x2 << 5); // Exp., Upload request - EC_WRITE_U16(data + 9, sdo_index); - EC_WRITE_U8 (data + 11, sdo_subindex); - - ec_command_init_npwr(&command, slave->station_address, 0x1800, 0xF6, data); - if (unlikely(ec_master_simple_io(master, &command))) { - EC_ERR("Mailbox sending failed on slave %i!\n", slave->ring_position); - return -1; - } - - // Read "written bit" of Sync-Manager - - start = get_cycles(); - timeout = cpu_khz; // 1ms - - while (1) - { - udelay(10); - - ec_command_init_nprd(&command, slave->station_address, 0x808, 8); - if (unlikely(ec_master_simple_io(master, &command))) { - EC_ERR("Mailbox checking failed on slave %i!\n", - slave->ring_position); - return -1; - } - - end = get_cycles(); - - if (EC_READ_U8(command.data + 5) & 8) { // Written bit is high - break; - } - - if (unlikely((end - start) >= timeout)) { - EC_ERR("Mailbox check on slave %i timed out.\n", - slave->ring_position); - return -1; - } - } - - ec_command_init_nprd(&command, slave->station_address, 0x18F6, 0xF6); - if (unlikely(ec_master_simple_io(master, &command))) { - EC_ERR("Mailbox receiving failed on slave %i!\n", - slave->ring_position); - return -1; - } - - if (EC_READ_U8 (command.data + 5) != 0x03) { // nicht CoE - EC_ERR("Invalid mailbox response (non-CoE) at slave %i!\n", - slave->ring_position); - return -1; - } - - if (EC_READ_U16(command.data + 6) >> 12 == 0x02 && // SDO request - EC_READ_U8 (command.data + 8) >> 5 == 0x04) { // Abort SDO transf. req. + uint8_t data[0x0A]; + size_t rec_size; + + EC_WRITE_U16(data, 0x2000); // Number (0), Service (SDO request) + EC_WRITE_U8 (data + 2, 0x1 << 1 | 0x2 << 5); // Exp., Upload request + EC_WRITE_U16(data + 3, sdo_index); + EC_WRITE_U8 (data + 5, sdo_subindex); + + if (ec_slave_mailbox_send(slave, 0x03, data, 6)) return -1; + + rec_size = 6; + if (ec_slave_mailbox_receive(slave, 0x03, data, &rec_size)) return -1; + + if (EC_READ_U16(data ) >> 12 == 0x02 && // SDO request + EC_READ_U8 (data + 2) >> 5 == 0x04) { // Abort SDO transf. req. EC_ERR("SDO upload of 0x%04X:%X aborted on slave %i.\n", sdo_index, sdo_subindex, slave->ring_position); - abort_code = EC_READ_U32(command.data + 12); - for (abort_msg = sdo_abort_messages; abort_msg->code; abort_msg++) { - if (abort_msg->code == abort_code) { - EC_ERR("SDO abort message 0x%08X: \"%s\".\n", - abort_msg->code, abort_msg->message); - return -1; - } - } - EC_ERR("Unknown SDO abort code 0x%08X.\n", abort_code); - return -1; - } - - if (EC_READ_U16(command.data + 6) >> 12 != 0x03 || // SDO response - EC_READ_U8 (command.data + 8) >> 5 != 0x02 || // Upload response - EC_READ_U16(command.data + 9) != sdo_index || // Index - EC_READ_U8 (command.data + 11) != sdo_subindex) // Subindex - { + ec_canopen_abort_msg(EC_READ_U32(data + 6)); + return -1; + } + + if (EC_READ_U16(data) >> 12 != 0x03 || // SDO response + EC_READ_U8 (data + 2) >> 5 != 0x02 || // Upload response + EC_READ_U16(data + 3) != sdo_index || // Index + EC_READ_U8 (data + 5) != sdo_subindex) { // Subindex EC_ERR("Invalid SDO upload response at slave %i!\n", slave->ring_position); return -1; } - *value = EC_READ_U32(command.data + 12); - + *value = EC_READ_U32(data + 6); return 0; } @@ -320,6 +195,92 @@ /*****************************************************************************/ +/** + Holt das Object-Dictionary aus dem Slave. + + \return 0, wenn alles ok, sonst < 0 +*/ + +int ec_slave_fetch_sdo_list(ec_slave_t *slave /**< EtherCAT-Slave */) +{ + uint8_t data[0xF0]; + size_t rec_size; + + //EC_DBG("Fetching SDO list for slave %i...\n", slave->ring_position); + + EC_WRITE_U16(data, 0x8000); // Number (0), Service (get OD request) + EC_WRITE_U8 (data + 2, 0x01); // Get OD List Request + EC_WRITE_U8 (data + 3, 0x00); // res. + EC_WRITE_U16(data + 4, 0x0000); // fragments left + EC_WRITE_U16(data + 6, 0x0001); // Deliver all SDOs! + + if (ec_slave_mailbox_send(slave, 0x03, data, 8)) return -1; + + do + { + rec_size = 0xF0; + if (ec_slave_mailbox_receive(slave, 0x03, data, &rec_size)) return -1; + + if (EC_READ_U16(data) >> 12 == 0x02 && // SDO request + EC_READ_U8 (data + 2) >> 5 == 0x04) { // Abort SDO transf. req. + EC_ERR("SDO list download aborted on slave %i.\n", + slave->ring_position); + ec_canopen_abort_msg(EC_READ_U32(data + 12)); + return -1; + } + + if (EC_READ_U16(data) >> 12 == 0x08 && // SDO information + (EC_READ_U8 (data + 2) & 0x7F) == 0x07) { // Get OD List response + EC_ERR("SDO information error response at slave %i!\n", + slave->ring_position); + ec_canopen_abort_msg(EC_READ_U32(data + 6)); + return -1; + } + + if (EC_READ_U16(data) >> 12 != 0x08 || // SDO information + (EC_READ_U8 (data + 2) & 0x7F) != 0x02) { // Get OD List response + EC_ERR("Invalid SDO list response at slave %i!\n", + slave->ring_position); + return -1; + } + + if (rec_size < 8) { + EC_ERR("Invalid data size!\n"); + return -1; + } + +#if 0 + for (i = 0; i < (rec_size - 8) / 2; i++) + EC_INFO("Object 0x%04X\n", EC_READ_U16(data + 8 + i * 2)); +#endif + } + while (EC_READ_U8(data + 2) & 0x80); + + return 0; +} + +/*****************************************************************************/ + +/** + Gibt eine SDO-Abort-Meldung aus. +*/ + +void ec_canopen_abort_msg(uint32_t abort_code) +{ + const ec_sdo_abort_message_t *abort_msg; + + for (abort_msg = sdo_abort_messages; abort_msg->code; abort_msg++) { + if (abort_msg->code == abort_code) { + EC_ERR("SDO abort message 0x%08X: \"%s\".\n", + abort_msg->code, abort_msg->message); + return; + } + } + EC_ERR("Unknown SDO abort code 0x%08X.\n", abort_code); +} + +/*****************************************************************************/ + const ec_sdo_abort_message_t sdo_abort_messages[] = { {0x05030000, "Toggle bit not changed"}, {0x05040000, "SDO protocol timeout"}, diff -r 63a5b40eb7da -r b36d187ecc0b master/master.c --- a/master/master.c Mon Mar 27 12:04:55 2006 +0000 +++ b/master/master.c Mon Mar 27 15:52:08 2006 +0000 @@ -711,24 +711,17 @@ first = 0; } if (master->slave_states & EC_SLAVE_STATE_PREOP) { - if (!first) { - printk(", "); - first = 0; - } + if (!first) printk(", "); printk("PREOP"); + first = 0; } if (master->slave_states & EC_SLAVE_STATE_SAVEOP) { - if (!first) { - printk(", "); - first = 0; - } + if (!first) printk(", "); printk("SAVEOP"); + first = 0; } if (master->slave_states & EC_SLAVE_STATE_OP) { - if (!first) { - printk(", "); - first = 0; - } + if (!first) printk(", "); printk("OP"); } printk(")\n"); @@ -860,6 +853,14 @@ if (unlikely(ec_slave_state_change(slave, EC_SLAVE_STATE_PREOP))) return -1; + // Fetch SDO list + if (slave->sii_mailbox_protocols & EC_MBOX_COE) { + if (unlikely(ec_slave_fetch_sdo_list(slave))) { + EC_ERR("Could not fetch SDO list!\n"); + return -1; + } + } + // Slaves that are not registered are only brought into PREOP // state -> nice blinking and mailbox comm. possible if (!slave->registered && !slave->type->bus_coupler) { diff -r 63a5b40eb7da -r b36d187ecc0b master/slave.c --- a/master/slave.c Mon Mar 27 12:04:55 2006 +0000 +++ b/master/slave.c Mon Mar 27 15:52:08 2006 +0000 @@ -48,6 +48,7 @@ slave->sii_product_code = 0; slave->sii_revision_number = 0; slave->sii_serial_number = 0; + slave->sii_mailbox_protocols = 0; slave->type = NULL; slave->registered = 0; slave->fmmu_count = 0; @@ -163,6 +164,12 @@ return -1; } + if (unlikely(ec_slave_sii_read(slave, 0x001C, + &slave->sii_mailbox_protocols))) { + EC_ERR("Could not read SII supported mailbox protocols!\n"); + return -1; + } + if (unlikely(ec_slave_fetch_categories(slave))) { EC_ERR("Could not fetch category data!\n"); return -1; @@ -785,6 +792,7 @@ ec_eeprom_sync_t *sync; ec_eeprom_pdo_t *pdo; ec_eeprom_pdo_entry_t *entry; + int first; EC_INFO("x-- EtherCAT slave information ---------------\n"); @@ -806,6 +814,43 @@ EC_INFO("| Supported FMMUs: %i, Sync managers: %i\n", slave->base_fmmu_count, slave->base_sync_count); + EC_INFO("| Supported mailbox protocols: "); + if (!slave->sii_mailbox_protocols) { + printk("(none)"); + } + else { + first = 1; + if (slave->sii_mailbox_protocols & EC_MBOX_AOE) { + printk("AoE"); + first = 0; + } + if (slave->sii_mailbox_protocols & EC_MBOX_EOE) { + if (!first) printk(", "); + printk("EoE"); + first = 0; + } + if (slave->sii_mailbox_protocols & EC_MBOX_COE) { + if (!first) printk(", "); + printk("CoE"); + first = 0; + } + if (slave->sii_mailbox_protocols & EC_MBOX_FOE) { + if (!first) printk(", "); + printk("FoE"); + first = 0; + } + if (slave->sii_mailbox_protocols & EC_MBOX_SOE) { + if (!first) printk(", "); + printk("SoE"); + first = 0; + } + if (slave->sii_mailbox_protocols & EC_MBOX_VOE) { + if (!first) printk(", "); + printk("VoE"); + } + } + printk("\n"); + EC_INFO("| EEPROM data:\n"); if (slave->sii_alias) @@ -908,6 +953,115 @@ /*****************************************************************************/ +/** + Sendet ein Mailbox-Kommando. + */ + +int ec_slave_mailbox_send(ec_slave_t *slave, /**< EtherCAT-Slave */ + uint8_t type, /**< Unterliegendes Protokoll */ + const uint8_t *prot_data, /**< Protokoll-Daten */ + size_t size /**< Datengröße */ + ) +{ + uint8_t data[0xF6]; + ec_command_t command; + + if (unlikely(size + 6 > 0xF6)) { + EC_ERR("Data size does not fit in mailbox!\n"); + return -1; + } + + memset(data, 0x00, 0xF6); + + EC_WRITE_U16(data, size); // Length of the Mailbox service data + EC_WRITE_U16(data + 2, slave->station_address); // Station address + EC_WRITE_U8 (data + 4, 0x00); // Channel & priority + EC_WRITE_U8 (data + 5, type); // Underlying protocol type + + memcpy(data + 6, prot_data, size); + + ec_command_init_npwr(&command, slave->station_address, 0x1800, 0xF6, data); + if (unlikely(ec_master_simple_io(slave->master, &command))) { + EC_ERR("Mailbox sending failed on slave %i!\n", slave->ring_position); + return -1; + } + + return 0; +} + +/*****************************************************************************/ + +/** + Sendet ein Mailbox-Kommando. + */ + +int ec_slave_mailbox_receive(ec_slave_t *slave, /**< EtherCAT-Slave */ + uint8_t type, /**< Unterliegendes Protokoll */ + uint8_t *prot_data, /**< Protokoll-Daten */ + size_t *size /**< Datengröße des Puffers, später + Größe der gelesenen Daten */ + ) +{ + ec_command_t command; + size_t data_size; + cycles_t start, end, timeout; + + // Read "written bit" of Sync-Manager + start = get_cycles(); + timeout = (cycles_t) 100 * cpu_khz; // 100ms + + while (1) + { + ec_command_init_nprd(&command, slave->station_address, 0x808, 8); + if (unlikely(ec_master_simple_io(slave->master, &command))) { + EC_ERR("Mailbox checking failed on slave %i!\n", + slave->ring_position); + return -1; + } + + end = get_cycles(); + + if (EC_READ_U8(command.data + 5) & 8) break; // Written bit is high + + if ((end - start) >= timeout) { + EC_ERR("Mailbox check - Slave %i timed out.\n", + slave->ring_position); + return -1; + } + + udelay(100); + } + + if (unlikely(slave->master->debug_level) > 1) + EC_DBG("SDO download took %ius.\n", ((u32) (end - start) * 1000 + / cpu_khz)); + + ec_command_init_nprd(&command, slave->station_address, 0x18F6, 0xF6); + if (unlikely(ec_master_simple_io(slave->master, &command))) { + EC_ERR("Mailbox receiving failed on slave %i!\n", + slave->ring_position); + return -1; + } + + if (EC_READ_U8(command.data + 5) != type) { // nicht CoE + EC_ERR("Invalid mailbox response (non-CoE) at slave %i!\n", + slave->ring_position); + return -1; + } + + if ((data_size = EC_READ_U16(command.data)) > *size) { + EC_ERR("CoE data does not fit in buffer (%i > %i).\n", + data_size, *size); + return -1; + } + + memcpy(prot_data, command.data + 6, data_size); + *size = data_size; + return 0; +} + +/*****************************************************************************/ + /* Emacs-Konfiguration ;;; Local Variables: *** ;;; c-basic-offset:4 *** diff -r 63a5b40eb7da -r b36d187ecc0b master/slave.h --- a/master/slave.h Mon Mar 27 12:04:55 2006 +0000 +++ b/master/slave.h Mon Mar 27 15:52:08 2006 +0000 @@ -39,6 +39,22 @@ /*****************************************************************************/ /** + Unterstützte Mailbox-Protokolle. +*/ + +enum +{ + EC_MBOX_AOE = 0x01, /**< ADS over EtherCAT */ + EC_MBOX_EOE = 0x02, /**< Ethernet over EtherCAT */ + EC_MBOX_COE = 0x04, /**< CANopen over EtherCAT */ + EC_MBOX_FOE = 0x08, /**< File Service over EtherCAT */ + EC_MBOX_SOE = 0x10, /**< Servo Profile over EtherCAT */ + EC_MBOX_VOE = 0x20 /**< Vendor specific */ +}; + +/*****************************************************************************/ + +/** FMMU-Konfiguration. */ @@ -154,6 +170,7 @@ uint32_t sii_product_code; /**< Herstellerspezifischer Produktcode */ uint32_t sii_revision_number; /**< Revisionsnummer */ uint32_t sii_serial_number; /**< Seriennummer der Klemme */ + uint32_t sii_mailbox_protocols; /**< Unterstützte Mailbox-Protokolle */ const ec_slave_type_t *type; /**< Zeiger auf die Beschreibung des Slave-Typs */ @@ -185,6 +202,13 @@ int ec_slave_state_change(ec_slave_t *, uint8_t); int ec_slave_set_fmmu(ec_slave_t *, const ec_domain_t *, const ec_sync_t *); +// Mailbox +int ec_slave_mailbox_send(ec_slave_t *, uint8_t, const uint8_t *, size_t); +int ec_slave_mailbox_receive(ec_slave_t *, uint8_t, uint8_t *, size_t *); + +// CANopen over EtherCAT +int ec_slave_fetch_sdo_list(ec_slave_t *); + // Misc void ec_slave_print(const ec_slave_t *); int ec_slave_check_crc(ec_slave_t *); diff -r 63a5b40eb7da -r b36d187ecc0b rt/msr_module.c --- a/rt/msr_module.c Mon Mar 27 12:04:55 2006 +0000 +++ b/rt/msr_module.c Mon Mar 27 15:52:08 2006 +0000 @@ -55,18 +55,20 @@ ec_domain_t *domain2 = NULL; // Prozessdaten -void *r_ssi; +void *r_ssi, *r_ssi_st; void *r_ssi2; void *r_inc; uint32_t k_angle; -uint32_t k_pos; +uint32_t k_ssi_pos; +uint32_t k_ssi_status; uint32_t k_preio; uint32_t k_postio; uint32_t k_finished; ec_field_init_t domain1_fields[] = { - {&r_ssi, "1", "Beckhoff", "EL5001", "InputValue", 0}, + {&r_ssi, "1", "Beckhoff", "EL5001", "InputValue", 0}, + {&r_ssi_st, "1", "Beckhoff", "EL5001", "Status", 0}, {} }; @@ -99,7 +101,8 @@ ecrt_domain_process(domain2); // Prozessdaten verarbeiten - k_pos = EC_READ_U32(r_ssi); + k_ssi_pos = EC_READ_U32(r_ssi); + k_ssi_status = EC_READ_U32(r_ssi_st); // Senden ecrt_domain_queue(domain1); @@ -114,7 +117,8 @@ ecrt_domain_process(domain2); // Prozessdaten verarbeiten - k_pos = EC_READ_U32(r_ssi); + k_ssi_pos = EC_READ_U32(r_ssi); + k_ssi_status = EC_READ_U32(r_ssi_st); #endif k_postio = (uint32_t) (get_cycles() - offset) * 1e6 / cpu_khz; @@ -127,8 +131,9 @@ int msr_globals_register(void) { - msr_reg_kanal("/angle0", "", &k_angle, TUINT); - msr_reg_kanal("/pos0", "", &k_pos, TUINT); + msr_reg_kanal("/angle0", "", &k_angle, TUINT); + msr_reg_kanal("/pos0", "", &k_ssi_pos, TUINT); + msr_reg_kanal("/ssi-status0", "", &k_ssi_status, TUINT); msr_reg_kanal("/Timing/Pre-IO", "ns", &k_preio, TUINT); msr_reg_kanal("/Timing/Post-IO", "ns", &k_postio, TUINT); @@ -183,17 +188,17 @@ goto out_msr_cleanup; } - //ecrt_master_print(master); + ecrt_master_print(master); printk(KERN_INFO "Registering domains...\n"); if (!(domain1 = ecrt_master_create_domain(master))) { - printk(KERN_ERR "EtherCAT: Could not register domain!\n"); + printk(KERN_ERR "Could not register domain!\n"); goto out_release_master; } if (!(domain2 = ecrt_master_create_domain(master))) { - printk(KERN_ERR "EtherCAT: Could not register domain!\n"); + printk(KERN_ERR "Could not register domain!\n"); goto out_release_master; } @@ -220,14 +225,29 @@ //ecrt_master_debug(master, 0); -#if 1 - if (ecrt_master_sdo_read(master, "6", 0x100A, 1, &version)) { +#if 0 + if (ecrt_master_sdo_read(master, "1", 0x100A, 1, &version)) { printk(KERN_ERR "Could not read SSI version!\n"); - goto out_release_master; + goto out_deactivate; } printk(KERN_INFO "Software-version: %u\n", version); #endif +#if 0 + if (ecrt_master_sdo_write(master, "1", 0x4061, 1, 0, 1) || + ecrt_master_sdo_write(master, "1", 0x4061, 2, 1, 1) || + ecrt_master_sdo_write(master, "1", 0x4061, 3, 1, 1) || + ecrt_master_sdo_write(master, "1", 0x4066, 0, 0, 1) || + ecrt_master_sdo_write(master, "1", 0x4067, 0, 4, 1) || + ecrt_master_sdo_write(master, "1", 0x4068, 0, 0, 1) || + ecrt_master_sdo_write(master, "1", 0x4069, 0, 25, 1) || + ecrt_master_sdo_write(master, "1", 0x406A, 0, 25, 1) || + ecrt_master_sdo_write(master, "1", 0x406B, 0, 50, 1)) { + printk(KERN_ERR "Failed to configure SSI slave!\n"); + goto out_deactivate; + } +#endif + #ifdef ASYNC // Einmal senden und warten... ecrt_master_prepare_async_io(master); @@ -241,12 +261,12 @@ return 0; + out_deactivate: + ecrt_master_deactivate(master); out_release_master: ecrt_release_master(master); - out_msr_cleanup: msr_rtlib_cleanup(); - out_return: return -1; }