SDO-Zugriff aufgeteilt in Expedited und Normal. Noch nicht fertig...
authorFlorian Pose <fp@igh-essen.com>
Tue, 28 Mar 2006 17:40:57 +0000
changeset 137 3fb18812871a
parent 136 a03a684cac89
child 138 7e743a61a991
SDO-Zugriff aufgeteilt in Expedited und Normal. Noch nicht fertig...
include/ecrt.h
master/canopen.c
rt/msr_module.c
--- a/include/ecrt.h	Tue Mar 28 15:45:19 2006 +0000
+++ b/include/ecrt.h	Tue Mar 28 17:40:57 2006 +0000
@@ -59,17 +59,23 @@
 void ecrt_master_prepare_async_io(ec_master_t *master);
 void ecrt_master_debug(ec_master_t *master, int level);
 void ecrt_master_print(const ec_master_t *master);
-int ecrt_master_sdo_write(ec_master_t *master,
-                          const char *slave_addr,
-                          uint16_t sdo_index,
-                          uint8_t sdo_subindex,
-                          uint32_t value,
-                          size_t size);
+int ecrt_master_sdo_exp_write(ec_master_t *master,
+                              const char *slave_addr,
+                              uint16_t sdo_index,
+                              uint8_t sdo_subindex,
+                              uint32_t value,
+                              size_t size);
+int ecrt_master_sdo_exp_read(ec_master_t *master,
+                             const char *slave_addr,
+                             uint16_t sdo_index,
+                             uint8_t sdo_subindex,
+                             uint32_t *value);
 int ecrt_master_sdo_read(ec_master_t *master,
                          const char *slave_addr,
                          uint16_t sdo_index,
                          uint8_t sdo_subindex,
-                         uint32_t *value);
+                         uint8_t *data,
+                         size_t *size);
 int ecrt_master_write_slave_alias(ec_master_t *master,
                                   const char *slave_address, uint16_t alias);
 
@@ -93,15 +99,20 @@
 /*****************************************************************************/
 // Slave Methods
 
-int ecrt_slave_sdo_write(ec_slave_t *slave,
-                         uint16_t sdo_index,
-                         uint8_t sdo_subindex,
-                         uint32_t value,
-                         size_t size);
+int ecrt_slave_sdo_exp_write(ec_slave_t *slave,
+                             uint16_t sdo_index,
+                             uint8_t sdo_subindex,
+                             uint32_t value,
+                             size_t size);
+int ecrt_slave_sdo_exp_read(ec_slave_t *slave,
+                            uint16_t sdo_index,
+                            uint8_t sdo_subindex,
+                            uint32_t *value);
 int ecrt_slave_sdo_read(ec_slave_t *slave,
                         uint16_t sdo_index,
                         uint8_t sdo_subindex,
-                        uint32_t *value);
+                        uint8_t *data,
+                        size_t *size);
 
 /*****************************************************************************/
 // Bitwise read/write macros
--- a/master/canopen.c	Tue Mar 28 15:45:19 2006 +0000
+++ b/master/canopen.c	Tue Mar 28 17:40:57 2006 +0000
@@ -35,15 +35,15 @@
 /*****************************************************************************/
 
 /**
-   Schreibt ein CANopen-SDO (service data object).
+   Schreibt ein CANopen-SDO (service data object), expedited.
  */
 
-int ecrt_slave_sdo_write(ec_slave_t *slave, /**< EtherCAT-Slave */
-                         uint16_t sdo_index, /**< SDO-Index */
-                         uint8_t sdo_subindex, /**< SDO-Subindex */
-                         uint32_t value, /**< Neuer Wert */
-                         size_t size /**< Größe des Datenfeldes */
-                         )
+int ecrt_slave_sdo_exp_write(ec_slave_t *slave, /**< EtherCAT-Slave */
+                             uint16_t sdo_index, /**< SDO-Index */
+                             uint8_t sdo_subindex, /**< SDO-Subindex */
+                             uint32_t value, /**< Neuer Wert */
+                             size_t size /**< Größe des Datenfeldes */
+                             )
 {
     uint8_t data[0x0A];
     unsigned int i;
@@ -95,14 +95,14 @@
 /*****************************************************************************/
 
 /**
-   Liest ein CANopen-SDO (service data object).
+   Liest ein CANopen-SDO (service data object), expedited.
  */
 
-int ecrt_slave_sdo_read(ec_slave_t *slave, /**< EtherCAT-Slave */
-                        uint16_t sdo_index, /**< SDO-Index */
-                        uint8_t sdo_subindex, /**< SDO-Subindex */
-                        uint32_t *value /**< Speicher für gel. Wert */
-                        )
+int ecrt_slave_sdo_exp_read(ec_slave_t *slave, /**< EtherCAT-Slave */
+                            uint16_t sdo_index, /**< SDO-Index */
+                            uint8_t sdo_subindex, /**< SDO-Subindex */
+                            uint32_t *value /**< Speicher für gel. Wert */
+                            )
 {
     uint8_t data[0x20];
     size_t rec_size;
@@ -141,57 +141,149 @@
 /*****************************************************************************/
 
 /**
-   Schweibt ein CANopen-SDO (Variante mit Angabe des Masters und der Adresse).
-
-   Siehe ecrt_slave_sdo_write()
+   Liest ein CANopen-SDO (service data object).
+ */
+
+int ecrt_slave_sdo_read(ec_slave_t *slave, /**< EtherCAT-Slave */
+                        uint16_t sdo_index, /**< SDO-Index */
+                        uint8_t sdo_subindex, /**< SDO-Subindex */
+                        uint8_t *target, /**< Speicher für gel. Wert */
+                        size_t *size /**< Größe des Speichers */
+                        )
+{
+    uint8_t data[0x20];
+    size_t rec_size, data_size;
+    uint32_t complete_size;
+
+    EC_WRITE_U16(data,     0x2000); // Number (0), Service = SDO request
+    EC_WRITE_U8 (data + 2, 0x2 << 5); // Initiate 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 = 0x20;
+    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 transfer request
+        EC_ERR("SDO upload of 0x%04X:%X aborted on slave %i.\n",
+               sdo_index, sdo_subindex, slave->ring_position);
+        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 || // Initiate 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;
+    }
+
+    if (rec_size < 10) {
+        EC_ERR("Received currupted SDO upload response!\n");
+        return -1;
+    }
+
+    if ((complete_size = EC_READ_U32(data + 6)) > *size) {
+        EC_ERR("SDO data does not fit into buffer (%i / %i)!\n",
+               complete_size, *size);
+        return -1;
+    }
+
+    data_size = rec_size - 10;
+
+    if (data_size == complete_size) {
+        memcpy(target, data + 10, data_size);
+    }
+    else {
+        EC_ERR("SDO data incomplete.\n");
+        return -1;
+    }
+
+    return 0;
+}
+
+/*****************************************************************************/
+
+/**
+   Schreibt ein CANopen-SDO (Angabe des Masters und der Adresse), expedited.
+
+   Siehe ecrt_slave_sdo_exp_write()
 
    \return 0 wenn alles ok, < 0 bei Fehler
  */
 
-int ecrt_master_sdo_write(ec_master_t *master,
-                          /**< EtherCAT-Master */
-                          const char *addr,
-                          /**< Addresse, siehe ec_master_slave_address() */
-                          uint16_t index,
-                          /**< SDO-Index */
-                          uint8_t subindex,
-                          /**< SDO-Subindex */
-                          uint32_t value,
-                          /**< Neuer Wert */
-                          size_t size
-                          /**< Größe des Datenfeldes */
-                          )
+int ecrt_master_sdo_exp_write(ec_master_t *master,
+                              /**< EtherCAT-Master */
+                              const char *addr,
+                              /**< Addresse, siehe ec_master_slave_address() */
+                              uint16_t index,
+                              /**< SDO-Index */
+                              uint8_t subindex,
+                              /**< SDO-Subindex */
+                              uint32_t value,
+                              /**< Neuer Wert */
+                              size_t size
+                              /**< Größe des Datenfeldes */
+                              )
 {
     ec_slave_t *slave;
     if (!(slave = ec_master_slave_address(master, addr))) return -1;
-    return ecrt_slave_sdo_write(slave, index, subindex, value, size);
-}
-
-/*****************************************************************************/
-
-/**
-   Liest ein CANopen-SDO (Variante mit Angabe des Masters und der Adresse).
-
-   Siehe ecrt_slave_sdo_read()
+    return ecrt_slave_sdo_exp_write(slave, index, subindex, value, size);
+}
+
+/*****************************************************************************/
+
+/**
+   Liest ein CANopen-SDO (Angabe des Masters und der Adresse), expedited.
+
+   Siehe ecrt_slave_sdo_exp_read()
 
    \return 0 wenn alles ok, < 0 bei Fehler
  */
 
-int ecrt_master_sdo_read(ec_master_t *master,
-                         /**< EtherCAT-Slave */
-                         const char *addr,
-                         /**< Addresse, siehe ec_master_slave_address() */
-                         uint16_t index,
-                         /**< SDO-Index */
-                         uint8_t subindex,
-                         /**< SDO-Subindex */
-                         uint32_t *value
-                         /**< Speicher für gel. Wert */
-                         )
+int ecrt_master_sdo_exp_read(ec_master_t *master,
+                             /**< EtherCAT-Slave */
+                             const char *addr,
+                             /**< Addresse, siehe ec_master_slave_address() */
+                             uint16_t index,
+                             /**< SDO-Index */
+                             uint8_t subindex,
+                             /**< SDO-Subindex */
+                             uint32_t *value
+                             /**< Speicher für gel. Wert */
+                             )
 {
     ec_slave_t *slave;
     if (!(slave = ec_master_slave_address(master, addr))) return -1;
-    return ecrt_slave_sdo_read(slave, index, subindex, value);
+    return ecrt_slave_sdo_exp_read(slave, index, subindex, value);
+}
+
+/*****************************************************************************/
+
+/**
+   Liest ein CANopen-SDO (Angabe des Masters und der Adresse), expedited.
+
+   Siehe ecrt_slave_sdo_exp_read()
+
+   \return 0 wenn alles ok, < 0 bei Fehler
+ */
+
+int ecrt_master_sdo_read(ec_master_t *master, /**< EtherCAT-Master */
+                         const char *addr, /**< Addresse, siehe
+                                              ec_master_slave_address() */
+                         uint16_t sdo_index, /**< SDO-Index */
+                         uint8_t sdo_subindex, /**< SDO-Subindex */
+                         uint8_t *target, /**< Speicher für gel. Wert */
+                         size_t *size /**< Größe des Speichers */
+                         )
+{
+    ec_slave_t *slave;
+    if (!(slave = ec_master_slave_address(master, addr))) return -1;
+    return ecrt_slave_sdo_read(slave, sdo_index, sdo_subindex, target, size);
 }
 
 /*****************************************************************************/
@@ -398,9 +490,11 @@
 
 /*****************************************************************************/
 
-EXPORT_SYMBOL(ecrt_slave_sdo_write);
+EXPORT_SYMBOL(ecrt_slave_sdo_exp_write);
+EXPORT_SYMBOL(ecrt_slave_sdo_exp_read);
 EXPORT_SYMBOL(ecrt_slave_sdo_read);
-EXPORT_SYMBOL(ecrt_master_sdo_write);
+EXPORT_SYMBOL(ecrt_master_sdo_exp_write);
+EXPORT_SYMBOL(ecrt_master_sdo_exp_read);
 EXPORT_SYMBOL(ecrt_master_sdo_read);
 
 /*****************************************************************************/
--- a/rt/msr_module.c	Tue Mar 28 15:45:19 2006 +0000
+++ b/rt/msr_module.c	Tue Mar 28 17:40:57 2006 +0000
@@ -38,7 +38,7 @@
 #include "../include/ecrt.h"
 
 #define ASYNC
-//#define BLOCK1
+#define BLOCK1
 
 // Defines/Makros
 #define HZREDUCTION (MSR_ABTASTFREQUENZ / HZ)
@@ -70,8 +70,8 @@
 
 #ifdef BLOCK1
 ec_field_init_t domain1_fields[] = {
-    {&r_ssi,    "1", "Beckhoff", "EL5001", "InputValue", 0},
-    {&r_ssi_st, "1", "Beckhoff", "EL5001", "Status",     0},
+    {&r_ssi,    "5", "Beckhoff", "EL5001", "InputValue", 0},
+    {&r_ssi_st, "5", "Beckhoff", "EL5001", "Status",     0},
     {}
 };
 #else
@@ -184,9 +184,10 @@
 int __init init_rt_module(void)
 {
     struct ipipe_domain_attr attr; //ipipe
-    uint32_t version;
-
-    // Als allererstes die RT-lib initialisieren
+    uint8_t string[10];
+    size_t size;
+
+    // Als allererstes die RT-Lib initialisieren
     if (msr_rtlib_init(1, MSR_ABTASTFREQUENZ, 10, &msr_globals_register) < 0) {
         msr_print_warn("msr_modul: can't initialize rtlib!");
         goto out_return;
@@ -230,21 +231,31 @@
     ecrt_master_print(master);
 
 #ifdef BLOCK1
-    if (ecrt_master_sdo_read(master, "1", 0x100A, 1, &version)) {
+    size = 10;
+    if (ecrt_master_sdo_read(master, "1", 0x100A, 0, string, &size)) {
         printk(KERN_ERR "Could not read SSI version!\n");
         goto out_deactivate;
     }
-    printk(KERN_INFO "Software-version: %u\n", version);
-
-    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)) {
+    string[size] = 0;
+    printk(KERN_INFO "Software-version 1: %s\n", string);
+
+    size = 10;
+    if (ecrt_master_sdo_read(master, "5", 0x100A, 0, string, &size)) {
+        printk(KERN_ERR "Could not read SSI version!\n");
+        goto out_deactivate;
+    }
+    string[size] = 0;
+    printk(KERN_INFO "Software-version 5: %s\n", string);
+
+    if (ecrt_master_sdo_exp_write(master, "5", 0x4061, 1,  0, 1) ||
+        ecrt_master_sdo_exp_write(master, "5", 0x4061, 2,  1, 1) ||
+        ecrt_master_sdo_exp_write(master, "5", 0x4061, 3,  1, 1) ||
+        ecrt_master_sdo_exp_write(master, "5", 0x4066, 0,  0, 1) ||
+        ecrt_master_sdo_exp_write(master, "5", 0x4067, 0,  4, 1) ||
+        ecrt_master_sdo_exp_write(master, "5", 0x4068, 0,  0, 1) ||
+        ecrt_master_sdo_exp_write(master, "5", 0x4069, 0, 25, 1) ||
+        ecrt_master_sdo_exp_write(master, "5", 0x406A, 0, 25, 1) ||
+        ecrt_master_sdo_exp_write(master, "5", 0x406B, 0, 50, 1)) {
         printk(KERN_ERR "Failed to configure SSI slave!\n");
         goto out_deactivate;
     }