fp@64: /****************************************************************************** fp@64: * fp@64: * c a n o p e n . c fp@64: * fp@64: * CANopen over EtherCAT fp@64: * fp@64: * $Id$ fp@64: * fp@64: *****************************************************************************/ fp@64: fp@64: #include fp@80: #include fp@80: #include fp@64: fp@64: #include "master.h" fp@64: fp@64: /*****************************************************************************/ fp@64: fp@73: /** fp@111: SDO Abort Code Messages fp@111: */ fp@111: fp@111: typedef struct fp@111: { fp@111: uint32_t code; fp@111: const char *message; fp@111: } fp@111: ec_sdo_abort_message_t; fp@111: fp@111: const ec_sdo_abort_message_t sdo_abort_messages[]; fp@111: fp@133: void ec_canopen_abort_msg(uint32_t); fp@133: fp@111: /*****************************************************************************/ fp@111: fp@111: /** fp@73: Schreibt ein CANopen-SDO (service data object). fp@73: */ fp@73: fp@104: int ecrt_slave_sdo_write(ec_slave_t *slave, /**< EtherCAT-Slave */ fp@101: uint16_t sdo_index, /**< SDO-Index */ fp@101: uint8_t sdo_subindex, /**< SDO-Subindex */ fp@101: uint32_t value, /**< Neuer Wert */ fp@101: size_t size /**< Größe des Datenfeldes */ fp@101: ) fp@64: { fp@133: uint8_t data[0x0A]; fp@98: unsigned int i; fp@133: size_t rec_size; fp@64: fp@64: if (size == 0 || size > 4) { fp@111: EC_ERR("Invalid SDO data size: %i!\n", size); fp@64: return -1; fp@64: } fp@64: fp@133: EC_WRITE_U16(data, 0x02 << 12); // Number (0), Service (SDO request) fp@133: EC_WRITE_U8 (data + 2, 0x23 | ((4 - size) << 2)); // Spec., exp., init. fp@133: EC_WRITE_U16(data + 3, sdo_index); fp@133: EC_WRITE_U8 (data + 5, sdo_subindex); fp@64: fp@64: for (i = 0; i < size; i++) { fp@133: EC_WRITE_U8(data + 6 + i, value & 0xFF); fp@64: value >>= 8; fp@64: } fp@64: fp@133: // Mailox senden und empfangen fp@133: if (ec_slave_mailbox_send(slave, 0x03, data, 0x0A)) return -1; fp@133: fp@133: rec_size = 0x0A; fp@133: if (ec_slave_mailbox_receive(slave, 0x03, data, &rec_size)) return -1; fp@133: fp@133: if (EC_READ_U16(data) >> 12 == 0x02 && // SDO request fp@133: EC_READ_U8 (data + 2) >> 5 == 0x04) { // Abort SDO transf. req. fp@111: EC_ERR("SDO download of 0x%04X:%X (value %X, size %X) aborted on slave" fp@111: " %i.\n", sdo_index, sdo_subindex, value, size, fp@111: slave->ring_position); fp@133: ec_canopen_abort_msg(EC_READ_U32(data + 6)); fp@133: return -1; fp@133: } fp@133: fp@133: if (EC_READ_U16(data) >> 12 != 0x03 || // SDO response fp@133: EC_READ_U8 (data + 2) >> 5 != 0x03 || // Download response fp@133: EC_READ_U16(data + 3) != sdo_index || // Index fp@133: EC_READ_U8 (data + 5) != sdo_subindex) // Subindex fp@64: { fp@111: EC_ERR("Invalid SDO download response at slave %i!\n", fp@73: slave->ring_position); fp@64: return -1; fp@64: } fp@64: fp@64: return 0; fp@64: } fp@64: fp@64: /*****************************************************************************/ fp@64: fp@80: /** fp@91: Liest ein CANopen-SDO (service data object). fp@80: */ fp@80: fp@104: int ecrt_slave_sdo_read(ec_slave_t *slave, /**< EtherCAT-Slave */ fp@101: uint16_t sdo_index, /**< SDO-Index */ fp@101: uint8_t sdo_subindex, /**< SDO-Subindex */ fp@101: uint32_t *value /**< Speicher für gel. Wert */ fp@101: ) fp@101: { fp@133: uint8_t data[0x0A]; fp@133: size_t rec_size; fp@133: fp@133: EC_WRITE_U16(data, 0x2000); // Number (0), Service (SDO request) fp@133: EC_WRITE_U8 (data + 2, 0x1 << 1 | 0x2 << 5); // Exp., Upload request fp@133: EC_WRITE_U16(data + 3, sdo_index); fp@133: EC_WRITE_U8 (data + 5, sdo_subindex); fp@133: fp@133: if (ec_slave_mailbox_send(slave, 0x03, data, 6)) return -1; fp@133: fp@133: rec_size = 6; fp@133: if (ec_slave_mailbox_receive(slave, 0x03, data, &rec_size)) return -1; fp@133: fp@133: if (EC_READ_U16(data ) >> 12 == 0x02 && // SDO request fp@133: EC_READ_U8 (data + 2) >> 5 == 0x04) { // Abort SDO transf. req. fp@111: EC_ERR("SDO upload of 0x%04X:%X aborted on slave %i.\n", fp@111: sdo_index, sdo_subindex, slave->ring_position); fp@133: ec_canopen_abort_msg(EC_READ_U32(data + 6)); fp@133: return -1; fp@133: } fp@133: fp@133: if (EC_READ_U16(data) >> 12 != 0x03 || // SDO response fp@133: EC_READ_U8 (data + 2) >> 5 != 0x02 || // Upload response fp@133: EC_READ_U16(data + 3) != sdo_index || // Index fp@133: EC_READ_U8 (data + 5) != sdo_subindex) { // Subindex fp@111: EC_ERR("Invalid SDO upload response at slave %i!\n", fp@80: slave->ring_position); fp@80: return -1; fp@80: } fp@80: fp@133: *value = EC_READ_U32(data + 6); fp@80: return 0; fp@80: } fp@80: fp@80: /*****************************************************************************/ fp@80: fp@91: /** fp@91: Schweibt ein CANopen-SDO (Variante mit Angabe des Masters und der Adresse). fp@91: fp@104: Siehe ecrt_slave_sdo_write() fp@91: fp@91: \return 0 wenn alles ok, < 0 bei Fehler fp@91: */ fp@91: fp@104: int ecrt_master_sdo_write(ec_master_t *master, fp@104: /**< EtherCAT-Master */ fp@104: const char *addr, fp@104: /**< Addresse, siehe ec_master_slave_address() */ fp@104: uint16_t index, fp@104: /**< SDO-Index */ fp@104: uint8_t subindex, fp@104: /**< SDO-Subindex */ fp@104: uint32_t value, fp@104: /**< Neuer Wert */ fp@104: size_t size fp@104: /**< Größe des Datenfeldes */ fp@104: ) fp@80: { fp@80: ec_slave_t *slave; fp@98: if (!(slave = ec_master_slave_address(master, addr))) return -1; fp@104: return ecrt_slave_sdo_write(slave, index, subindex, value, size); fp@80: } fp@80: fp@80: /*****************************************************************************/ fp@80: fp@91: /** fp@91: Liest ein CANopen-SDO (Variante mit Angabe des Masters und der Adresse). fp@91: fp@104: Siehe ecrt_slave_sdo_read() fp@91: fp@91: \return 0 wenn alles ok, < 0 bei Fehler fp@91: */ fp@91: fp@104: int ecrt_master_sdo_read(ec_master_t *master, fp@104: /**< EtherCAT-Slave */ fp@104: const char *addr, fp@104: /**< Addresse, siehe ec_master_slave_address() */ fp@104: uint16_t index, fp@104: /**< SDO-Index */ fp@104: uint8_t subindex, fp@104: /**< SDO-Subindex */ fp@104: uint32_t *value fp@104: /**< Speicher für gel. Wert */ fp@104: ) fp@80: { fp@80: ec_slave_t *slave; fp@98: if (!(slave = ec_master_slave_address(master, addr))) return -1; fp@104: return ecrt_slave_sdo_read(slave, index, subindex, value); fp@104: } fp@104: fp@104: /*****************************************************************************/ fp@104: fp@133: /** fp@133: Holt das Object-Dictionary aus dem Slave. fp@133: fp@133: \return 0, wenn alles ok, sonst < 0 fp@133: */ fp@133: fp@133: int ec_slave_fetch_sdo_list(ec_slave_t *slave /**< EtherCAT-Slave */) fp@133: { fp@133: uint8_t data[0xF0]; fp@133: size_t rec_size; fp@133: fp@133: //EC_DBG("Fetching SDO list for slave %i...\n", slave->ring_position); fp@133: fp@133: EC_WRITE_U16(data, 0x8000); // Number (0), Service (get OD request) fp@133: EC_WRITE_U8 (data + 2, 0x01); // Get OD List Request fp@133: EC_WRITE_U8 (data + 3, 0x00); // res. fp@133: EC_WRITE_U16(data + 4, 0x0000); // fragments left fp@133: EC_WRITE_U16(data + 6, 0x0001); // Deliver all SDOs! fp@133: fp@133: if (ec_slave_mailbox_send(slave, 0x03, data, 8)) return -1; fp@133: fp@133: do fp@133: { fp@133: rec_size = 0xF0; fp@133: if (ec_slave_mailbox_receive(slave, 0x03, data, &rec_size)) return -1; fp@133: fp@133: if (EC_READ_U16(data) >> 12 == 0x02 && // SDO request fp@133: EC_READ_U8 (data + 2) >> 5 == 0x04) { // Abort SDO transf. req. fp@133: EC_ERR("SDO list download aborted on slave %i.\n", fp@133: slave->ring_position); fp@133: ec_canopen_abort_msg(EC_READ_U32(data + 12)); fp@133: return -1; fp@133: } fp@133: fp@133: if (EC_READ_U16(data) >> 12 == 0x08 && // SDO information fp@133: (EC_READ_U8 (data + 2) & 0x7F) == 0x07) { // Get OD List response fp@133: EC_ERR("SDO information error response at slave %i!\n", fp@133: slave->ring_position); fp@133: ec_canopen_abort_msg(EC_READ_U32(data + 6)); fp@133: return -1; fp@133: } fp@133: fp@133: if (EC_READ_U16(data) >> 12 != 0x08 || // SDO information fp@133: (EC_READ_U8 (data + 2) & 0x7F) != 0x02) { // Get OD List response fp@133: EC_ERR("Invalid SDO list response at slave %i!\n", fp@133: slave->ring_position); fp@133: return -1; fp@133: } fp@133: fp@133: if (rec_size < 8) { fp@133: EC_ERR("Invalid data size!\n"); fp@133: return -1; fp@133: } fp@133: fp@133: #if 0 fp@133: for (i = 0; i < (rec_size - 8) / 2; i++) fp@133: EC_INFO("Object 0x%04X\n", EC_READ_U16(data + 8 + i * 2)); fp@133: #endif fp@133: } fp@133: while (EC_READ_U8(data + 2) & 0x80); fp@133: fp@133: return 0; fp@133: } fp@133: fp@133: /*****************************************************************************/ fp@133: fp@133: /** fp@133: Gibt eine SDO-Abort-Meldung aus. fp@133: */ fp@133: fp@133: void ec_canopen_abort_msg(uint32_t abort_code) fp@133: { fp@133: const ec_sdo_abort_message_t *abort_msg; fp@133: fp@133: for (abort_msg = sdo_abort_messages; abort_msg->code; abort_msg++) { fp@133: if (abort_msg->code == abort_code) { fp@133: EC_ERR("SDO abort message 0x%08X: \"%s\".\n", fp@133: abort_msg->code, abort_msg->message); fp@133: return; fp@133: } fp@133: } fp@133: EC_ERR("Unknown SDO abort code 0x%08X.\n", abort_code); fp@133: } fp@133: fp@133: /*****************************************************************************/ fp@133: fp@111: const ec_sdo_abort_message_t sdo_abort_messages[] = { fp@111: {0x05030000, "Toggle bit not changed"}, fp@111: {0x05040000, "SDO protocol timeout"}, fp@111: {0x05040001, "Client/Server command specifier not valid or unknown"}, fp@111: {0x05040005, "Out of memory"}, fp@111: {0x06010000, "Unsupported access to an object"}, fp@111: {0x06010001, "Attempt to read a write-only object"}, fp@111: {0x06010002, "Attempt to write a read-only object"}, fp@111: {0x06020000, "This object does not exist in the object directory"}, fp@111: {0x06040041, "The object cannot be mapped into the PDO"}, fp@111: {0x06040042, "The number and length of the objects to be mapped would" fp@111: " exceed the PDO length"}, fp@111: {0x06040043, "General parameter incompatibility reason"}, fp@111: {0x06040047, "Gerneral internal incompatibility in device"}, fp@111: {0x06060000, "Access failure due to a hardware error"}, fp@111: {0x06070010, "Data type does not match, length of service parameter does" fp@111: " not match"}, fp@111: {0x06070012, "Data type does not match, length of service parameter too" fp@111: " high"}, fp@111: {0x06070013, "Data type does not match, length of service parameter too" fp@111: " low"}, fp@111: {0x06090011, "Subindex does not exist"}, fp@111: {0x06090030, "Value range of parameter exceeded"}, fp@111: {0x06090031, "Value of parameter written too high"}, fp@111: {0x06090032, "Value of parameter written too low"}, fp@111: {0x06090036, "Maximum value is less than minimum value"}, fp@111: {0x08000000, "General error"}, fp@111: {0x08000020, "Data cannot be transferred or stored to the application"}, fp@111: {0x08000021, "Data cannot be transferred or stored to the application" fp@111: " because of local control"}, fp@111: {0x08000022, "Data cannot be transferred or stored to the application" fp@111: " because of the present device state"}, fp@111: {0x08000023, "Object dictionary dynamic generation fails or no object" fp@111: " dictionary is present"}, fp@111: {} fp@111: }; fp@111: fp@111: /*****************************************************************************/ fp@111: fp@104: EXPORT_SYMBOL(ecrt_slave_sdo_write); fp@104: EXPORT_SYMBOL(ecrt_slave_sdo_read); fp@104: EXPORT_SYMBOL(ecrt_master_sdo_write); fp@104: EXPORT_SYMBOL(ecrt_master_sdo_read); fp@64: fp@64: /*****************************************************************************/ fp@64: fp@64: /* Emacs-Konfiguration fp@64: ;;; Local Variables: *** fp@64: ;;; c-basic-offset:4 *** fp@64: ;;; End: *** fp@64: */