user/ec_master.c
author Florian Pose <fp@igh-essen.com>
Fri, 02 Dec 2005 12:59:21 +0000
branchkernel2.6
changeset 26 60435f959e5c
parent 0 05c992bf5847
permissions -rw-r--r--
Serial-Debugger entfernt.
0
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
     1
//---------------------------------------------------------------
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
     2
//
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
     3
//  e c _ m a s t e r . c
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
     4
//
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
     5
//  $LastChangedDate$
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
     6
//  $Author$
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
     7
//
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
     8
//---------------------------------------------------------------
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
     9
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    10
#include "ec_globals.h"
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    11
#include "ec_master.h"
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    12
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    13
#define DEBUG_SEND_RECEIVE
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    14
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    15
//---------------------------------------------------------------
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    16
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    17
int EtherCAT_master_init(EtherCAT_master_t *master,
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    18
                         char *eth_dev)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    19
{
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    20
  char errbuf_libpcap[PCAP_ERRBUF_SIZE];
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    21
  char errbuf_libnet[LIBNET_ERRBUF_SIZE];
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    22
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    23
  master->slaves = NULL;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    24
  master->slave_count = 0;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    25
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    26
  master->first_command = NULL;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    27
  master->command_index = 0x00;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    28
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    29
  master->process_data = NULL;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    30
  master->pre_cb = NULL;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    31
  master->post_cb = NULL;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    32
  master->thread_continue = 0;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    33
  master->cycle_time = 0;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    34
  
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    35
  // Init Libpcap
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    36
  master->pcap_handle = pcap_open_live(eth_dev, BUFSIZ, 1, 0, errbuf_libpcap);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    37
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    38
  if (master->pcap_handle == NULL)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    39
  {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    40
    fprintf(stderr, "Couldn't open device %s: %s\n", eth_dev, errbuf_libpcap);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    41
    return 1;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    42
  }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    43
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    44
  // Init Libnet
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    45
  if (!(master->net_handle = libnet_init(LIBNET_LINK, eth_dev, errbuf_libnet)))
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    46
  {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    47
    fprintf(stderr, "Could not init %s: %s!\n", eth_dev, errbuf_libnet);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    48
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    49
    pcap_close(master->pcap_handle);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    50
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    51
    return 1;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    52
  }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    53
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    54
  return 0;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    55
}
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    56
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    57
//---------------------------------------------------------------
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    58
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    59
void EtherCAT_master_clear(EtherCAT_master_t *master)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    60
{
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    61
  libnet_destroy(master->net_handle);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    62
  pcap_close(master->pcap_handle);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    63
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    64
  // Remove all pending commands
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    65
  while (master->first_command)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    66
  {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    67
    EtherCAT_remove_command(master, master->first_command);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    68
  }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    69
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    70
  // Remove all slaves
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    71
  EtherCAT_clear_slaves(master);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    72
}
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    73
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    74
//---------------------------------------------------------------
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    75
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    76
int EtherCAT_check_slaves(EtherCAT_master_t *master,
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    77
                          EtherCAT_slave_t *slaves,
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    78
                          unsigned int slave_count)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    79
{
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    80
  EtherCAT_command_t *cmd;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    81
  EtherCAT_slave_t *cur;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    82
  unsigned int i, j, found;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    83
  unsigned char data[2];
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    84
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    85
  // EtherCAT_clear_slaves() must be called before!
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    86
  if (master->slave_count) return -1;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    87
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    88
  // Determine number of slaves
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    89
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    90
  if ((cmd = EtherCAT_broadcast_read(master, 0x0000, 4)) == NULL)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    91
  {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    92
    return -1;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    93
  }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    94
  
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    95
  if (EtherCAT_send_receive(master) != 0)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    96
  {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    97
    return -1;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    98
  }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    99
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   100
  master->slave_count = cmd->working_counter;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   101
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   102
  EtherCAT_remove_command(master, cmd);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   103
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   104
  if (master->slave_count < slave_count)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   105
  {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   106
    fprintf(stderr, "ERROR: Too few slaves on EtherCAT bus!\n");
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   107
    return -1;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   108
  }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   109
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   110
  // No slaves. Stop further processing...
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   111
  if (master->slave_count == 0) return 0;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   112
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   113
   // For every slave in the list
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   114
  for (i = 0; i < master->slave_count; i++)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   115
  {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   116
    cur = &slaves[i];
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   117
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   118
    if (!cur->desc)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   119
    {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   120
      fprintf(stderr, "ERROR: Slave has no description (list position %i)!\n", i);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   121
      return -1;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   122
    }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   123
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   124
    // Set ring position
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   125
    cur->ring_position = -i;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   126
    cur->station_address = i + 1;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   127
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   128
    // Write station address
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   129
    
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   130
    data[0] = cur->station_address & 0x00FF;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   131
    data[1] = (cur->station_address & 0xFF00) >> 8;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   132
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   133
    if ((cmd = EtherCAT_position_write(master, cur->ring_position, 0x0010, 2, data)) == NULL)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   134
    {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   135
      master->slave_count = 0;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   136
      fprintf(stderr, "ERROR: Could not create command!\n");
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   137
      return -1;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   138
    }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   139
    
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   140
    if (EtherCAT_send_receive(master) != 0)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   141
    {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   142
      master->slave_count = 0;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   143
      fprintf(stderr, "ERROR: Could not send command!\n");
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   144
      return -1;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   145
    }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   146
    
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   147
    if (cmd->working_counter != 1)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   148
    {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   149
      master->slave_count = 0;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   150
      fprintf(stderr, "ERROR: Slave %i did not repond while writing station address!\n", i);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   151
      return -1;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   152
    }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   153
    
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   154
    EtherCAT_remove_command(master, cmd);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   155
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   156
    // Read base data
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   157
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   158
    if ((cmd = EtherCAT_read(master, cur->station_address, 0x0000, 4)) == NULL)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   159
    {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   160
      master->slave_count = 0;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   161
      fprintf(stderr, "ERROR: Could not create command!\n");
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   162
      return -1;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   163
    }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   164
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   165
    if (EtherCAT_send_receive(master) != 0)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   166
    {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   167
      EtherCAT_remove_command(master, cmd);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   168
      master->slave_count = 0;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   169
      fprintf(stderr, "ERROR: Could not send command!\n");
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   170
      return -4;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   171
    }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   172
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   173
    if (cmd->working_counter != 1)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   174
    {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   175
      EtherCAT_remove_command(master, cmd);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   176
      master->slave_count = 0;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   177
      fprintf(stderr, "ERROR: Slave %i did not respond while reading base data!\n", i);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   178
      return -5;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   179
    }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   180
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   181
    // Get base data
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   182
    cur->type = cmd->data[0];
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   183
    cur->revision = cmd->data[1];
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   184
    cur->build = cmd->data[2] | (cmd->data[3] << 8);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   185
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   186
    EtherCAT_remove_command(master, cmd);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   187
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   188
    // Read identification from "Slave Information Interface" (SII)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   189
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   190
    if (EtherCAT_read_slave_information(master, cur->station_address, 0x0008, &cur->vendor_id) != 0)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   191
    {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   192
      master->slave_count = 0;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   193
      fprintf(stderr, "ERROR: Could not read SII!\n", i);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   194
      return -1;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   195
    }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   196
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   197
    if (EtherCAT_read_slave_information(master, cur->station_address, 0x000A, &cur->product_code) != 0)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   198
    {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   199
      master->slave_count = 0;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   200
      fprintf(stderr, "ERROR: Could not read SII!\n", i);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   201
      return -1;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   202
    }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   203
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   204
    if (EtherCAT_read_slave_information(master, cur->station_address, 0x000E, &cur->revision_number) != 0)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   205
    {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   206
      master->slave_count = 0;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   207
      fprintf(stderr, "ERROR: Could not read SII!\n", i);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   208
      return -1;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   209
    }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   210
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   211
    // Search for identification in "database"
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   212
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   213
    found = 0;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   214
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   215
    for (j = 0; j < slave_idents_count; j++)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   216
    {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   217
      if (slave_idents[j].vendor_id == cur->vendor_id
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   218
          && slave_idents[j].product_code == cur->product_code)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   219
      {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   220
        found = 1;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   221
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   222
        if (cur->desc != slave_idents[j].desc)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   223
        {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   224
          fprintf(stderr, "ERROR: Unexpected slave device at position %i:"
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   225
                  "%s %s. Expected: %s %s\n",
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   226
                  i, slave_idents[j].desc->vendor_name, slave_idents[j].desc->product_name,
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   227
                  cur->desc->vendor_name, cur->desc->product_name);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   228
          return -1;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   229
        }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   230
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   231
        break;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   232
      }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   233
    }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   234
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   235
    if (!found)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   236
    {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   237
      fprintf(stderr, "ERROR: Unknown slave device at position %i: Vendor %X, Code %X",
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   238
              i, cur->vendor_id, cur->product_code);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   239
      return -1;      
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   240
    }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   241
  }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   242
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   243
  return 0;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   244
}
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   245
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   246
//---------------------------------------------------------------
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   247
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   248
void EtherCAT_clear_slaves(EtherCAT_master_t *master)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   249
{
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   250
  unsigned int i;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   251
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   252
  if (master->slave_count == 0) return;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   253
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   254
  for (i = 0; i < master->slave_count; i++)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   255
  {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   256
    EtherCAT_slave_clear(&master->slaves[i]);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   257
  }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   258
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   259
  free(master->slaves);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   260
  master->slaves = NULL;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   261
}
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   262
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   263
//---------------------------------------------------------------
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   264
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   265
int EtherCAT_read_slave_information(EtherCAT_master_t *master,
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   266
                                    unsigned short int node_address,
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   267
                                    unsigned short int offset,
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   268
                                    unsigned int *target)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   269
{
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   270
  EtherCAT_command_t *cmd;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   271
  unsigned char data[10];
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   272
  unsigned int tries;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   273
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   274
  // Initiate read operation
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   275
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   276
  data[0] = 0x00;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   277
  data[1] = 0x01;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   278
  data[2] = offset & 0xFF;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   279
  data[3] = (offset & 0xFF00) >> 8;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   280
  data[4] = 0x00;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   281
  data[5] = 0x00;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   282
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   283
  if ((cmd = EtherCAT_write(master, node_address, 0x502, 6, data)) == NULL)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   284
  {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   285
    fprintf(stderr, "ERROR: Could not allocate command!\n");
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   286
    return -2;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   287
  }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   288
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   289
  if (EtherCAT_send_receive(master) != 0)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   290
  {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   291
    EtherCAT_remove_command(master, cmd);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   292
    fprintf(stderr, "ERROR: Could not write to slave!\n");
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   293
    return -3;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   294
  }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   295
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   296
  if (cmd->working_counter != 1)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   297
  {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   298
    EtherCAT_remove_command(master, cmd);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   299
    fprintf(stderr, "ERROR: Command not processed by slave!\n");
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   300
    return -4;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   301
  }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   302
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   303
  EtherCAT_remove_command(master, cmd);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   304
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   305
  // Get status of read operation
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   306
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   307
  tries = 0;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   308
  while (tries < 10)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   309
  {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   310
    if ((cmd = EtherCAT_read(master, node_address, 0x502, 10)) == NULL)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   311
    {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   312
      fprintf(stderr, "ERROR: Could not allocate command!\n");
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   313
      return -2;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   314
    }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   315
    
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   316
    if (EtherCAT_send_receive(master) != 0)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   317
    {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   318
      EtherCAT_remove_command(master, cmd);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   319
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   320
      fprintf(stderr, "ERROR: Could not read from slave!\n");
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   321
      return -3;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   322
    }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   323
    
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   324
    if (cmd->working_counter != 1)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   325
    {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   326
      EtherCAT_remove_command(master, cmd);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   327
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   328
      fprintf(stderr, "ERROR: Command not processed by slave!\n");
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   329
      return -4;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   330
    }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   331
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   332
    if ((cmd->data[1] & 0x81) == 0)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   333
    {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   334
#if 0
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   335
      printf("SLI status data: %02X %02X Address: %02X %02X\n", cmd->data[0], cmd->data[1], cmd->data[2], cmd->data[3]);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   336
      printf("Data: %02X %02X %02X %02X\n", cmd->data[6], cmd->data[7], cmd->data[8], cmd->data[9]);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   337
#endif
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   338
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   339
      memcpy(target, cmd->data + 6, 4);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   340
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   341
      EtherCAT_remove_command(master, cmd);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   342
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   343
      break;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   344
    }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   345
    
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   346
    EtherCAT_remove_command(master, cmd);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   347
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   348
    tries++;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   349
  }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   350
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   351
  if (tries == 10) fprintf(stderr, "ERROR: Timeout while reading SII!\n");
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   352
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   353
  return 0;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   354
}
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   355
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   356
//---------------------------------------------------------------
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   357
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   358
int EtherCAT_send_receive(EtherCAT_master_t *master)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   359
{
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   360
  libnet_ptag_t ptag;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   361
  struct pcap_pkthdr header;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   362
  const unsigned char *packet;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   363
  unsigned char dst[6], src[6];
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   364
  unsigned int i, length, framelength, pos, command_type, command_index;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   365
  EtherCAT_command_t *cmd;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   366
  unsigned char *data;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   367
  int bytes, command_follows, found;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   368
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   369
#ifdef DEBUG_SEND_RECEIVE
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   370
  found = 0;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   371
#endif
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   372
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   373
  length = 0;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   374
  for (cmd = master->first_command; cmd != NULL; cmd = cmd->next)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   375
  {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   376
    //if (cmd->status != Waiting) continue;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   377
    length += cmd->data_length + 12;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   378
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   379
#ifdef DEBUG_SEND_RECEIVE
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   380
    found++;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   381
#endif
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   382
  }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   383
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   384
#ifdef DEBUG_SEND_RECEIVE
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   385
  printf("Sending %i commands with length %i...\n", found, length);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   386
#endif
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   387
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   388
  if (length == 0) return 0;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   389
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   390
  framelength = length + 2;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   391
  if (framelength < 46) framelength = 46;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   392
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   393
  data = (unsigned char *) malloc(sizeof(unsigned char) * framelength);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   394
  if (!data) return -1;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   395
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   396
  data[0] = length & 0xFF;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   397
  data[1] = ((length & 0x700) >> 8) | 0x10;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   398
  pos = 2;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   399
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   400
  for (cmd = master->first_command; cmd != NULL; cmd = cmd->next)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   401
  {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   402
    if (cmd->status != Waiting)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   403
    {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   404
      printf("Old command: %02X\n", cmd->command_type);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   405
      continue;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   406
    }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   407
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   408
    cmd->command_index = master->command_index;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   409
    master->command_index = (master->command_index + 1) % 0x0100;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   410
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   411
    cmd->status = Sent;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   412
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   413
    data[pos + 0] = cmd->command_type;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   414
    data[pos + 1] = cmd->command_index;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   415
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   416
    switch (cmd->command_type)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   417
    {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   418
      case EC_CMD_APRD:
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   419
      case EC_CMD_APWR:
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   420
        data[pos + 2] = cmd->ring_position & 0xFF;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   421
        data[pos + 3] = (cmd->ring_position & 0xFF00) >> 8;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   422
        data[pos + 4] = cmd->mem_address & 0xFF;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   423
        data[pos + 5] = (cmd->mem_address & 0xFF00) >> 8;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   424
        break;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   425
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   426
      case EC_CMD_NPRD:
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   427
      case EC_CMD_NPWR:
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   428
        data[pos + 2] = cmd->node_address & 0xFF;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   429
        data[pos + 3] = (cmd->node_address & 0xFF00) >> 8;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   430
        data[pos + 4] = cmd->mem_address & 0xFF;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   431
        data[pos + 5] = (cmd->mem_address & 0xFF00) >> 8;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   432
        break;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   433
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   434
      case EC_CMD_BRD:
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   435
      case EC_CMD_BWR:
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   436
        data[pos + 2] = 0x00;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   437
        data[pos + 3] = 0x00;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   438
        data[pos + 4] = cmd->mem_address & 0xFF;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   439
        data[pos + 5] = (cmd->mem_address & 0xFF00) >> 8;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   440
        break;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   441
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   442
      case EC_CMD_LRW:
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   443
        data[pos + 2] = cmd->logical_address & 0x000000FF;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   444
        data[pos + 3] = (cmd->logical_address & 0x0000FF00) >> 8;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   445
        data[pos + 4] = (cmd->logical_address & 0x00FF0000) >> 16;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   446
        data[pos + 5] = (cmd->logical_address & 0xFF000000) >> 24;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   447
        break;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   448
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   449
      default:
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   450
        data[pos + 2] = 0x00;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   451
        data[pos + 3] = 0x00;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   452
        data[pos + 4] = 0x00;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   453
        data[pos + 5] = 0x00;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   454
        fprintf(stderr, "WARNING: Default adress while frame construction...\n");
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   455
        break;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   456
    }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   457
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   458
    data[pos + 6] = cmd->data_length & 0xFF;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   459
    data[pos + 7] = (cmd->data_length & 0x700) >> 8;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   460
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   461
    if (cmd->next) data[pos + 7] |= 0x80;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   462
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   463
    data[pos + 8] = 0x00;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   464
    data[pos + 9] = 0x00;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   465
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   466
    if (cmd->command_type == EC_CMD_APWR
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   467
        || cmd->command_type == EC_CMD_NPWR
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   468
        || cmd->command_type == EC_CMD_BWR
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   469
        || cmd->command_type == EC_CMD_LRW) // Write
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   470
    {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   471
      for (i = 0; i < cmd->data_length; i++) data[pos + 10 + i] = cmd->data[i];
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   472
    }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   473
    else // Read
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   474
    {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   475
      for (i = 0; i < cmd->data_length; i++) data[pos + 10 + i] = 0x00;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   476
    }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   477
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   478
    data[pos + 10 + cmd->data_length] = 0x00;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   479
    data[pos + 11 + cmd->data_length] = 0x00;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   480
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   481
    pos += 12 + cmd->data_length;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   482
  }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   483
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   484
  // Pad with zeros
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   485
  while (pos < 46) data[pos++] = 0x00;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   486
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   487
#ifdef DEBUG_SEND_RECEIVE
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   488
  printf("\n>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n");
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   489
  for (i = 0; i < framelength; i++)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   490
  {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   491
    printf("%02X ", data[i]);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   492
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   493
    if ((i + 1) % 16 == 0) printf("\n");
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   494
  }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   495
  printf("\n-----------------------------------------------\n");
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   496
#endif
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   497
 
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   498
  dst[0] = 0xFF;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   499
  dst[1] = 0xFF;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   500
  dst[2] = 0xFF;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   501
  dst[3] = 0xFF;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   502
  dst[4] = 0xFF;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   503
  dst[5] = 0xFF;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   504
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   505
  src[0] = 0x00;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   506
  src[1] = 0x00;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   507
  src[2] = 0x00;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   508
  src[3] = 0x00;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   509
  src[4] = 0x00;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   510
  src[5] = 0x00;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   511
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   512
  // Send frame
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   513
  ptag = libnet_build_ethernet(dst, src, 0x88A4, data, framelength, master->net_handle, 0);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   514
  bytes = libnet_write(master->net_handle);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   515
  libnet_clear_packet(master->net_handle);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   516
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   517
  if (bytes == -1)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   518
  {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   519
    free(data);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   520
    fprintf(stderr, "Could not write!\n");
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   521
    return -1;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   522
  }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   523
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   524
  packet = pcap_next(master->pcap_handle, &header); // LibPCap receives sent frame first
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   525
  packet = pcap_next(master->pcap_handle, &header);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   526
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   527
#ifdef DEBUG_SEND_RECEIVE
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   528
  for (i = 0; i < header.len - 14; i++)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   529
  {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   530
    if (packet[i + 14] == data[i]) printf("   ");
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   531
    else printf("%02X ", packet[i + 14]);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   532
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   533
    if ((i + 1) % 16 == 0) printf("\n");
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   534
  }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   535
  printf("\n<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<\n\n");
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   536
#endif
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   537
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   538
  // Free sent data
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   539
  free(data);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   540
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   541
  pos = 16;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   542
  command_follows = 1;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   543
  while (command_follows)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   544
  {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   545
    command_type = packet[pos];
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   546
    command_index = packet[pos + 1];
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   547
    length = (packet[pos + 6] & 0xFF) | ((packet[pos + 7] & 0x07) << 8);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   548
    command_follows = packet[pos + 7] & 0x80;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   549
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   550
#if 0
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   551
    printf("Command %02X received!\n", command_index);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   552
#endif
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   553
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   554
    found = 0;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   555
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   556
    for (cmd = master->first_command; cmd != NULL; cmd = cmd->next)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   557
    {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   558
      if (cmd->status == Sent
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   559
          && cmd->command_type == command_type
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   560
          && cmd->command_index == command_index
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   561
          && cmd->data_length == length)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   562
      {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   563
        found = 1;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   564
        cmd->status = Received;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   565
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   566
        cmd->data = (unsigned char *) malloc(sizeof(unsigned char) * length);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   567
        memcpy(cmd->data, packet + pos + 10, length);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   568
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   569
        cmd->working_counter = (packet[pos + length + 10] & 0xFF)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   570
          | ((packet[pos + length + 11] & 0xFF) << 8);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   571
      }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   572
    }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   573
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   574
    if (!found)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   575
    {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   576
      fprintf(stderr, "WARNING: Command not assigned!\n");
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   577
    }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   578
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   579
    pos += length + 12;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   580
  }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   581
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   582
  for (cmd = master->first_command; cmd != NULL; cmd = cmd->next)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   583
  {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   584
    if (cmd->status == Sent)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   585
    {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   586
      fprintf(stderr, "WARNING: Command not sent!\n");
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   587
    }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   588
  }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   589
      
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   590
  return 0;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   591
}
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   592
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   593
//---------------------------------------------------------------
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   594
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   595
EtherCAT_command_t *EtherCAT_read(EtherCAT_master_t *master,
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   596
                                  unsigned short node_address,
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   597
                                  unsigned short offset,
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   598
                                  unsigned int length)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   599
{
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   600
  EtherCAT_command_t *cmd;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   601
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   602
  if (node_address == 0x0000) fprintf(stderr, "WARNING: Using node address 0x0000!\n");
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   603
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   604
  cmd = (EtherCAT_command_t *) malloc(sizeof(EtherCAT_command_t));
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   605
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   606
  if (cmd == NULL)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   607
  {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   608
    return NULL;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   609
  }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   610
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   611
  EtherCAT_command_init(cmd);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   612
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   613
  cmd->command_type = EC_CMD_NPRD;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   614
  cmd->node_address = node_address;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   615
  cmd->mem_address = offset;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   616
  cmd->data_length = length;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   617
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   618
  // Add command to master's list
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   619
  add_command(master, cmd);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   620
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   621
  return cmd;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   622
}
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   623
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   624
//---------------------------------------------------------------
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   625
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   626
EtherCAT_command_t *EtherCAT_write(EtherCAT_master_t *master,
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   627
                                   unsigned short node_address,
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   628
                                   unsigned short offset,
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   629
                                   unsigned int length,
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   630
                                   const unsigned char *data)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   631
{
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   632
  EtherCAT_command_t *cmd;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   633
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   634
  if (node_address == 0x0000) fprintf(stderr, "WARNING: Using node address 0x0000!\n");
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   635
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   636
  cmd = (EtherCAT_command_t *) malloc(sizeof(EtherCAT_command_t));
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   637
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   638
  if (cmd == NULL)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   639
  {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   640
    return NULL;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   641
  }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   642
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   643
  EtherCAT_command_init(cmd);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   644
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   645
  cmd->command_type = EC_CMD_NPWR;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   646
  cmd->node_address = node_address;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   647
  cmd->mem_address = offset;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   648
  cmd->data_length = length;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   649
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   650
  cmd->data = (unsigned char *) malloc(sizeof(unsigned char) * length);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   651
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   652
  if (cmd->data == NULL)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   653
  {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   654
    free(cmd);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   655
    return NULL;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   656
  }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   657
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   658
  memcpy(cmd->data, data, length);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   659
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   660
  // Add command to master's list
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   661
  add_command(master, cmd);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   662
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   663
  return cmd;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   664
}
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   665
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   666
//---------------------------------------------------------------
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   667
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   668
EtherCAT_command_t *EtherCAT_position_read(EtherCAT_master_t *master,
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   669
                                           short ring_position,
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   670
                                           unsigned short offset,
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   671
                                           unsigned int length)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   672
{
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   673
  EtherCAT_command_t *cmd;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   674
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   675
  cmd = (EtherCAT_command_t *) malloc(sizeof(EtherCAT_command_t));
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   676
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   677
  if (cmd == NULL)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   678
  {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   679
    return NULL;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   680
  }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   681
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   682
  EtherCAT_command_init(cmd);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   683
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   684
  cmd->command_type = EC_CMD_APRD;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   685
  cmd->ring_position = ring_position;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   686
  cmd->mem_address = offset;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   687
  cmd->data_length = length;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   688
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   689
  // Add command to master's list
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   690
  add_command(master, cmd);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   691
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   692
  return cmd;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   693
}
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   694
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   695
//---------------------------------------------------------------
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   696
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   697
EtherCAT_command_t *EtherCAT_position_write(EtherCAT_master_t *master,
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   698
                                            short ring_position,
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   699
                                            unsigned short offset,
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   700
                                            unsigned int length,
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   701
                                            const unsigned char *data)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   702
{
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   703
  EtherCAT_command_t *cmd;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   704
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   705
  cmd = (EtherCAT_command_t *) malloc(sizeof(EtherCAT_command_t));
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   706
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   707
  if (cmd == NULL)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   708
  {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   709
    return NULL;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   710
  }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   711
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   712
  EtherCAT_command_init(cmd);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   713
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   714
  cmd->command_type = EC_CMD_APWR;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   715
  cmd->ring_position = ring_position;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   716
  cmd->mem_address = offset;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   717
  cmd->data_length = length;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   718
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   719
  cmd->data = (unsigned char *) malloc(sizeof(unsigned char) * length);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   720
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   721
  if (cmd->data == NULL)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   722
  {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   723
    free(cmd);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   724
    return NULL;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   725
  }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   726
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   727
  memcpy(cmd->data, data, length);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   728
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   729
  // Add command to master's list
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   730
  add_command(master, cmd);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   731
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   732
  return cmd;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   733
}
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   734
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   735
//---------------------------------------------------------------
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   736
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   737
EtherCAT_command_t *EtherCAT_broadcast_read(EtherCAT_master_t *master,
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   738
                                            unsigned short offset,
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   739
                                            unsigned int length)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   740
{
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   741
  EtherCAT_command_t *cmd;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   742
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   743
  cmd = (EtherCAT_command_t *) malloc(sizeof(EtherCAT_command_t));
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   744
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   745
  if (cmd == NULL)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   746
  {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   747
    return NULL;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   748
  }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   749
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   750
  EtherCAT_command_init(cmd);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   751
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   752
  cmd->command_type = EC_CMD_BRD;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   753
  cmd->mem_address = offset;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   754
  cmd->data_length = length;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   755
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   756
  // Add command to master's list
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   757
  add_command(master, cmd);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   758
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   759
  return cmd;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   760
}
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   761
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   762
//---------------------------------------------------------------
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   763
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   764
EtherCAT_command_t *EtherCAT_broadcast_write(EtherCAT_master_t *master,
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   765
                                             unsigned short offset,
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   766
                                             unsigned int length,
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   767
                                             const unsigned char *data)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   768
{
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   769
  EtherCAT_command_t *cmd;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   770
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   771
  cmd = (EtherCAT_command_t *) malloc(sizeof(EtherCAT_command_t));
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   772
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   773
  if (cmd == NULL)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   774
  {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   775
    return NULL;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   776
  }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   777
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   778
  EtherCAT_command_init(cmd);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   779
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   780
  cmd->command_type = EC_CMD_BWR;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   781
  cmd->mem_address = offset;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   782
  cmd->data_length = length;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   783
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   784
  cmd->data = (unsigned char *) malloc(sizeof(unsigned char) * length);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   785
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   786
  if (cmd->data == NULL)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   787
  {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   788
    free(cmd);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   789
    return NULL;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   790
  }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   791
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   792
  memcpy(cmd->data, data, length);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   793
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   794
  // Add command to master's list
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   795
  add_command(master, cmd);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   796
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   797
  return cmd;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   798
}
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   799
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   800
//---------------------------------------------------------------
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   801
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   802
EtherCAT_command_t *EtherCAT_logical_read_write(EtherCAT_master_t *master,
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   803
                                                unsigned int offset,
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   804
                                                unsigned int length,
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   805
                                                unsigned char *data)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   806
{
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   807
  EtherCAT_command_t *cmd;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   808
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   809
  cmd = (EtherCAT_command_t *) malloc(sizeof(EtherCAT_command_t));
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   810
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   811
  if (cmd == NULL)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   812
  {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   813
    return NULL;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   814
  }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   815
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   816
  EtherCAT_command_init(cmd);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   817
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   818
  cmd->command_type = EC_CMD_LRW;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   819
  cmd->mem_address = offset;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   820
  cmd->data_length = length;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   821
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   822
  cmd->data = (unsigned char *) malloc(sizeof(unsigned char) * length);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   823
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   824
  if (cmd->data == NULL)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   825
  {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   826
    free(cmd);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   827
    return NULL;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   828
  }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   829
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   830
  memcpy(cmd->data, data, length);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   831
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   832
  // Add command to master's list
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   833
  add_command(master, cmd);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   834
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   835
  return cmd;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   836
}
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   837
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   838
//---------------------------------------------------------------
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   839
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   840
void add_command(EtherCAT_master_t *master, EtherCAT_command_t *cmd)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   841
{
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   842
  EtherCAT_command_t **last_cmd;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   843
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   844
  // Find last position in the list
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   845
  last_cmd = &(master->first_command);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   846
  while (*last_cmd) last_cmd = &(*last_cmd)->next;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   847
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   848
  *last_cmd = cmd;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   849
}
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   850
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   851
//---------------------------------------------------------------
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   852
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   853
void EtherCAT_remove_command(EtherCAT_master_t *master,
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   854
                             EtherCAT_command_t *rem_cmd)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   855
{
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   856
  EtherCAT_command_t **last_cmd;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   857
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   858
  last_cmd = &(master->first_command);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   859
  while (*last_cmd)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   860
  {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   861
    if (*last_cmd == rem_cmd)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   862
    {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   863
      *last_cmd = rem_cmd->next;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   864
      EtherCAT_command_clear(rem_cmd);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   865
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   866
      return;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   867
    }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   868
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   869
    last_cmd = &(*last_cmd)->next;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   870
  }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   871
}
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   872
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   873
//---------------------------------------------------------------
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   874
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   875
int EtherCAT_state_change(EtherCAT_master_t *master,
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   876
                          EtherCAT_slave_t *slave,
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   877
                          unsigned char state_and_ack)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   878
{
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   879
  EtherCAT_command_t *cmd;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   880
  unsigned char data[2];
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   881
  unsigned int tries_left;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   882
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   883
  data[0] = state_and_ack;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   884
  data[1] = 0x00;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   885
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   886
  if ((cmd = EtherCAT_write(master, slave->station_address,
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   887
                            0x0120, 2, data)) == NULL)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   888
  {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   889
    return -1;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   890
  }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   891
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   892
  if (EtherCAT_send_receive(master) != 0)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   893
  {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   894
    EtherCAT_remove_command(master, cmd);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   895
    return -2;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   896
  }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   897
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   898
  if (cmd->working_counter != 1)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   899
  {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   900
    EtherCAT_remove_command(master, cmd);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   901
    return -3;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   902
  }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   903
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   904
  EtherCAT_remove_command(master, cmd);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   905
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   906
  slave->requested_state = state_and_ack & 0x0F;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   907
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   908
  tries_left = 10;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   909
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   910
  while (tries_left)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   911
  {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   912
    if ((cmd = EtherCAT_read(master, slave->station_address,
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   913
                             0x0130, 2)) == NULL)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   914
    {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   915
      return -1;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   916
    }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   917
    
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   918
    if (EtherCAT_send_receive(master) != 0)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   919
    {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   920
      EtherCAT_remove_command(master, cmd);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   921
      return -2;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   922
    }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   923
    
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   924
    if (cmd->working_counter != 1)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   925
    {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   926
      EtherCAT_remove_command(master, cmd);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   927
      return -3;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   928
    }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   929
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   930
    if (cmd->data[0] & 0x10) // State change error
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   931
    {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   932
      EtherCAT_remove_command(master, cmd);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   933
      return -4;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   934
    }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   935
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   936
    if (cmd->data[0] == state_and_ack & 0x0F) // State change successful
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   937
    {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   938
      EtherCAT_remove_command(master, cmd);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   939
      break;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   940
    }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   941
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   942
    EtherCAT_remove_command(master, cmd);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   943
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   944
    //printf("Trying again...\n");
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   945
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   946
    tries_left--;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   947
  }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   948
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   949
  if (!tries_left)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   950
  {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   951
    return -5;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   952
  }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   953
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   954
  slave->current_state = state_and_ack & 0x0F;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   955
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   956
  return 0;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   957
}
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   958
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   959
//---------------------------------------------------------------
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   960
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   961
int EtherCAT_broadcast_state_change(EtherCAT_master_t *master,
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   962
                                    unsigned char state)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   963
{
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   964
  EtherCAT_command_t *cmd;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   965
  unsigned char data[2];
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   966
  unsigned int tries_left;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   967
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   968
  data[0] = state;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   969
  data[1] = 0x00;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   970
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   971
  if ((cmd = EtherCAT_broadcast_write(master, 0x0120, 2, data)) == NULL)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   972
  {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   973
    return -1;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   974
  }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   975
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   976
  if (EtherCAT_send_receive(master) != 0)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   977
  {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   978
    EtherCAT_remove_command(master, cmd);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   979
    return -2;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   980
  }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   981
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   982
  if (cmd->working_counter != master->slave_count)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   983
  {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   984
    EtherCAT_remove_command(master, cmd);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   985
    return -3;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   986
  }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   987
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   988
  EtherCAT_remove_command(master, cmd);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   989
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   990
  tries_left = 10;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   991
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   992
  while (tries_left)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   993
  {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   994
    if ((cmd = EtherCAT_broadcast_read(master, 0x0130, 2)) == NULL)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   995
    {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   996
      return -1;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   997
    }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   998
    
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   999
    if (EtherCAT_send_receive(master) != 0)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1000
    {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1001
      EtherCAT_remove_command(master, cmd);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1002
      return -2;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1003
    }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1004
    
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1005
    if (cmd->working_counter != master->slave_count)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1006
    {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1007
      EtherCAT_remove_command(master, cmd);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1008
      return -3;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1009
    }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1010
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1011
    if (cmd->data[0] & 0x10) // State change error
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1012
    {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1013
      EtherCAT_remove_command(master, cmd);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1014
      return -4;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1015
    }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1016
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1017
    if (cmd->data[0] == state) // State change successful
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1018
    {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1019
      EtherCAT_remove_command(master, cmd);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1020
      break;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1021
    }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1022
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1023
    EtherCAT_remove_command(master, cmd);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1024
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1025
    //printf("Trying again...\n");
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1026
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1027
    tries_left--;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1028
  }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1029
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1030
  if (!tries_left)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1031
  {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1032
    return -5;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1033
  }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1034
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1035
  return 0;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1036
}
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1037
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1038
//---------------------------------------------------------------
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1039
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1040
int EtherCAT_start(EtherCAT_master_t *master,
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1041
                   unsigned int length,
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1042
                   void (*pre_cb)(unsigned char *),
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1043
                   void (*post_cb)(unsigned char *),
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1044
                   unsigned int cycle_time)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1045
{
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1046
  if (master->process_data)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1047
  {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1048
    fprintf(stderr, "ERROR: Process data already allocated!\n");
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1049
    return -1;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1050
  }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1051
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1052
  if ((master->process_data = (unsigned char *) malloc(length)) == NULL)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1053
  {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1054
    fprintf(stderr, "ERROR: Could not allocate process data block!\n");
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1055
    return -2;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1056
  }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1057
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1058
  memset(master->process_data, 0x00, length);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1059
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1060
  master->process_data_length = length;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1061
  master->pre_cb = pre_cb;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1062
  master->post_cb = post_cb;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1063
  master->cycle_time = cycle_time;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1064
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1065
  master->thread_continue = 1;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1066
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1067
  if (pthread_create(&master->thread, NULL, thread_function, (void *) master) != 0)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1068
  {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1069
    fprintf(stderr, "ERROR: Could not create thread!\n");
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1070
    return -3;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1071
  }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1072
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1073
  return 0;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1074
}
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1075
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1076
//---------------------------------------------------------------
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1077
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1078
int EtherCAT_stop(EtherCAT_master_t *master)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1079
{
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1080
  if (!master->thread_continue)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1081
  {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1082
    fprintf(stderr, "ERROR: Thread not running!\n");
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1083
    return -1;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1084
  }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1085
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1086
  master->thread_continue = 0;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1087
  pthread_join(master->thread, NULL);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1088
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1089
  if (master->process_data)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1090
  {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1091
    free(master->process_data);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1092
    master->process_data = NULL;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1093
  }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1094
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1095
  master->pre_cb = NULL;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1096
  master->post_cb = NULL;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1097
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1098
  return 0;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1099
}
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1100
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1101
//---------------------------------------------------------------
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1102
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1103
double current_timestamp()
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1104
{
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1105
  struct timeval tv;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1106
  gettimeofday(&tv, NULL);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1107
  return tv.tv_sec + (double) tv.tv_usec / 1000000.0;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1108
}
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1109
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1110
//---------------------------------------------------------------
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1111
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1112
void *thread_function(void *data)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1113
{
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1114
  EtherCAT_master_t *master;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1115
  EtherCAT_command_t *cmd;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1116
  double bus_start_time, bus_end_time;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1117
  double cycle_start_time, cycle_end_time, last_cycle_start_time;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1118
  unsigned int wait_usecs;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1119
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1120
  master = (EtherCAT_master_t *) data;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1121
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1122
  last_cycle_start_time = 0.0;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1123
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1124
  while (master->thread_continue)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1125
  {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1126
    cycle_start_time = current_timestamp();
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1127
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1128
    if (last_cycle_start_time != 0.0)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1129
    {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1130
      master->last_cycle_time = cycle_start_time - last_cycle_start_time;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1131
      master->last_jitter = (master->last_cycle_time - (master->cycle_time / 1000000.0))
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1132
        / (master->cycle_time / 1000000.0) * 100.0;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1133
    }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1134
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1135
    last_cycle_start_time = cycle_start_time;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1136
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1137
    if (master->pre_cb) master->pre_cb(master->process_data);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1138
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1139
    cmd = EtherCAT_logical_read_write(master,
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1140
                                      0x00000000,
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1141
                                      master->process_data_length,
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1142
                                      master->process_data);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1143
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1144
    bus_start_time = current_timestamp();
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1145
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1146
    EtherCAT_send_receive(master);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1147
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1148
    bus_end_time = current_timestamp();
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1149
    master->bus_time = bus_end_time - bus_start_time;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1150
    
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1151
#if 0
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1152
    printf("Working counter: %i\n", cmd->working_counter);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1153
#endif
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1154
    
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1155
    memcpy(master->process_data, cmd->data, master->process_data_length);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1156
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1157
    EtherCAT_remove_command(master, cmd);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1158
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1159
    if (master->post_cb) master->post_cb(master->process_data);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1160
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1161
    // Calculate working time
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1162
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1163
    cycle_end_time = current_timestamp();
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1164
    master->last_cycle_work_time = cycle_end_time - cycle_start_time;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1165
    master->last_cycle_busy_rate = master->last_cycle_work_time
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1166
      / ((double) master->cycle_time / 1000000.0) * 100.0;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1167
    wait_usecs = master->cycle_time - (unsigned int) (master->last_cycle_work_time * 1000000.0);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1168
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1169
    //printf("USECS to wait: %i\n", wait_usecs);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1170
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1171
    usleep(wait_usecs);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1172
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1173
    //printf("waited: %lf\n", current_timestamp() - cycle_end_time);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1174
  }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1175
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1176
  return (void *) 0;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1177
}
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1178
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1179
//---------------------------------------------------------------
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1180
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1181
int EtherCAT_activate_slave(EtherCAT_master_t *master,
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1182
                            EtherCAT_slave_t *slave)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1183
{
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1184
  EtherCAT_command_t *cmd;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1185
  const EtherCAT_slave_desc_t *desc;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1186
  unsigned char fmmu[16];
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1187
  unsigned char data[256];
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1188
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1189
  if (EtherCAT_state_change(master, slave, EC_STATE_INIT) != 0)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1190
  {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1191
    return -1;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1192
  }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1193
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1194
  // Resetting FMMU's
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1195
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1196
  memset(data, 0x00, 256);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1197
  cmd = EtherCAT_write(master, slave->station_address, 0x0600, 256, data);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1198
  EtherCAT_send_receive(master);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1199
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1200
  if (cmd->working_counter != 1)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1201
  {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1202
    fprintf(stderr, "ERROR: Slave did not respond!\n");
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1203
    return -2;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1204
  }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1205
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1206
  EtherCAT_remove_command(master, cmd);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1207
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1208
  // Resetting Sync Manager channels
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1209
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1210
  memset(data, 0x00, 256);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1211
  cmd = EtherCAT_write(master, slave->station_address, 0x0800, 256, data);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1212
  EtherCAT_send_receive(master);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1213
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1214
  if (cmd->working_counter != 1)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1215
  {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1216
    fprintf(stderr, "ERROR: Slave did not respond!\n");
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1217
    return -2;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1218
  }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1219
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1220
  EtherCAT_remove_command(master, cmd);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1221
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1222
  desc = slave->desc;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1223
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1224
  // Init Mailbox communication
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1225
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1226
  if (desc->type == MAILBOX)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1227
  {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1228
    if (desc->sm0)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1229
    {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1230
      cmd = EtherCAT_write(master, slave->station_address, 0x0800, 8, desc->sm0);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1231
      
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1232
      EtherCAT_send_receive(master);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1233
      
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1234
      if (cmd->working_counter != 1)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1235
      {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1236
        fprintf(stderr, "ERROR: Not all slaves responded!\n");
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1237
        return -3;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1238
      }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1239
      
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1240
      EtherCAT_remove_command(master, cmd);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1241
    }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1242
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1243
    if (desc->sm1)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1244
    {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1245
      cmd = EtherCAT_write(master, slave->station_address, 0x0808, 8, desc->sm1);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1246
      
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1247
      EtherCAT_send_receive(master);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1248
      
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1249
      if (cmd->working_counter != 1)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1250
      {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1251
        fprintf(stderr, "ERROR: Not all slaves responded!\n");
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1252
        return -4;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1253
      }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1254
      
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1255
      EtherCAT_remove_command(master, cmd);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1256
    }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1257
  }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1258
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1259
  // Change state to PREOP
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1260
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1261
  if (EtherCAT_state_change(master, slave, EC_STATE_PREOP) != 0)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1262
  {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1263
    fprintf(stderr, "ERROR: Could not set state!\n");
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1264
    return -5;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1265
  }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1266
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1267
  // Set FMMU's
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1268
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1269
  if (desc->fmmu0)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1270
  {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1271
    memcpy(fmmu, desc->fmmu0, 16);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1272
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1273
    fmmu[0] = slave->logical_address0 & 0x000000FF;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1274
    fmmu[1] = (slave->logical_address0 & 0x0000FF00) >> 8;    
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1275
    fmmu[2] = (slave->logical_address0 & 0x00FF0000) >> 16;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1276
    fmmu[3] = (slave->logical_address0 & 0xFF000000) >> 24;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1277
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1278
    cmd = EtherCAT_write(master, slave->station_address, 0x0600, 16, fmmu);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1279
    EtherCAT_send_receive(master);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1280
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1281
    if (cmd->working_counter != 1)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1282
    {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1283
      fprintf(stderr, "ERROR: Not all slaves responded (%i of 1)!\n",
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1284
              cmd->working_counter);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1285
      return -6;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1286
    }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1287
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1288
    EtherCAT_remove_command(master, cmd);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1289
  }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1290
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1291
  // Set Sync Managers
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1292
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1293
  if (desc->type != MAILBOX)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1294
  {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1295
    if (desc->sm0)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1296
    {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1297
      cmd = EtherCAT_write(master, slave->station_address, 0x0800, 8, desc->sm0);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1298
      
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1299
      EtherCAT_send_receive(master);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1300
      
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1301
      if (cmd->working_counter != 1)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1302
      {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1303
        fprintf(stderr, "ERROR: Not all slaves responded!\n");
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1304
        return -8;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1305
      }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1306
      
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1307
      EtherCAT_remove_command(master, cmd);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1308
    }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1309
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1310
    if (desc->sm1)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1311
    {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1312
      cmd = EtherCAT_write(master, slave->station_address, 0x0808, 8, desc->sm1);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1313
      
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1314
      EtherCAT_send_receive(master);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1315
      
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1316
      if (cmd->working_counter != 1)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1317
      {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1318
        fprintf(stderr, "ERROR: Not all slaves responded!\n");
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1319
        return -9;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1320
      }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1321
      
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1322
      EtherCAT_remove_command(master, cmd);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1323
    }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1324
  }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1325
  
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1326
  if (desc->sm2)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1327
  {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1328
    cmd = EtherCAT_write(master, slave->station_address, 0x0810, 8, desc->sm2);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1329
      
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1330
    EtherCAT_send_receive(master);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1331
      
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1332
    if (cmd->working_counter != 1)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1333
    {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1334
      fprintf(stderr, "ERROR: Not all slaves responded!\n");
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1335
      return -10;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1336
    }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1337
      
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1338
    EtherCAT_remove_command(master, cmd);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1339
  }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1340
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1341
  if (desc->sm3)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1342
  {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1343
    cmd = EtherCAT_write(master, slave->station_address, 0x0818, 8, desc->sm3);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1344
      
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1345
    EtherCAT_send_receive(master);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1346
      
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1347
    if (cmd->working_counter != 1)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1348
    {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1349
      fprintf(stderr, "ERROR: Not all slaves responded!\n");
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1350
      return -11;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1351
    }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1352
      
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1353
    EtherCAT_remove_command(master, cmd);
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1354
  }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1355
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1356
  // Change state to SAVEOP
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1357
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1358
  if (EtherCAT_state_change(master, slave, EC_STATE_SAVEOP) != 0)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1359
  {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1360
    fprintf(stderr, "ERROR: Could not set state!\n");
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1361
    return -12;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1362
  }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1363
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1364
  // Change state to OP
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1365
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1366
  if (EtherCAT_state_change(master, slave, EC_STATE_OP) != 0)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1367
  {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1368
    fprintf(stderr, "ERROR: Could not set state!\n");
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1369
    return -13;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1370
  }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1371
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1372
  return 0;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1373
}
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1374
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1375
//---------------------------------------------------------------
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1376
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1377
int EtherCAT_deactivate_slave(EtherCAT_master_t *master,
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1378
                            EtherCAT_slave_t *slave)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1379
{
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1380
  if (EtherCAT_state_change(master, slave, EC_STATE_INIT) != 0)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1381
  {
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1382
    return -1;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1383
  }
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1384
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1385
  return 0;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1386
}
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1387
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1388
//---------------------------------------------------------------
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1389
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1390
void set_byte(unsigned char *data,
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1391
              unsigned int offset,
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1392
              unsigned char value)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1393
{
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1394
  data[offset] = value;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1395
}
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1396
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1397
//---------------------------------------------------------------
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1398
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1399
void set_word(unsigned char *data,
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1400
              unsigned int offset,
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1401
              unsigned int value)
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1402
{
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1403
  data[offset] = value & 0xFF;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1404
  data[offset + 1] = (value & 0xFF00) >> 8;
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1405
}
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1406
05c992bf5847 trunk, tags und branches
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
  1407
//---------------------------------------------------------------