# HG changeset patch # User Florian Pose # Date 1134750139 0 # Node ID fd8b9ad48f88ce3ce1a8253fbb41dab0f545af80 # Parent d7ef8607e06f55fef63e6e11a82e0019740a61a0 user-Implementation aus 2.4er Branch entfernt. diff -r d7ef8607e06f -r fd8b9ad48f88 user/Makefile --- a/user/Makefile Fri Dec 16 15:34:04 2005 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,63 +0,0 @@ -#---------------------------------------------------------------- -# -# M a k e f i l e -# -# $Id$ -# -#---------------------------------------------------------------- - -LIBNET_DIR = ../../soft/libnet-install -LIBPCAP_DIR = ../../soft/libpcap-install -FLTK_DIR = ../../soft/fltk-2.0-install - -CC = g++ -CFLAGS = -Wall -g -I$(LIBNET_DIR)/include -I$(LIBPCAP_DIR)/include \ - `$(FLTK_DIR)/bin/fltk-config --cflags` - -TEST_EXE = ethercat-test -TEST_OBJ = main.o ec_master.o ec_command.o ec_slave.o -TEST_LDFLAGS = -L$(LIBNET_DIR)/lib -lnet -lpcap -lpthread - -GUI_EXE = ethercat-gui -GUI_OBJ = main_gui.o ec_master.o ec_command.o ec_slave.o -GUI_LDFLAGS = -L$(LIBNET_DIR)/lib -lnet -lpcap -lpthread `$(FLTK_DIR)/bin/fltk-config --ldflags` - -#---------------------------------------------------------------- - -first: $(TEST_EXE) $(GUI_EXE) - -$(TEST_EXE): $(TEST_OBJ) - $(CC) $(TEST_OBJ) $(TEST_LDFLAGS) -o $@ - -$(GUI_EXE): $(GUI_OBJ) - $(CC) $(GUI_OBJ) $(GUI_LDFLAGS) -o $@ - -.c.o: - $(CC) $(CFLAGS) -c $< -o $@ - -.cpp.o: - $(CC) $(CFLAGS) -c $< -o $@ - -#---------------------------------------------------------------- - -main.o: main.c \ - ec_globals.h ec_master.h ec_command.h ec_slave.h - -main_gui.o: main_gui.cpp \ - ec_globals.h ec_master.h ec_command.h ec_slave.h - -ec_command.o: ec_command.c ec_command.h - -ec_master.o: ec_master.c ec_master.h \ - ec_globals.h ec_command.h ec_slave.h - -ec_slave.o: ec_slave.c ec_slave.h \ - ec_globals.h - -#---------------------------------------------------------------- - -clean: - rm -f *.o $(TEST_EXE) $(GUI_EXE) *~ - -#---------------------------------------------------------------- - diff -r d7ef8607e06f -r fd8b9ad48f88 user/ec_command.c --- a/user/ec_command.c Fri Dec 16 15:34:04 2005 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,61 +0,0 @@ -//--------------------------------------------------------------- -// -// e c _ c o m m a n d . c -// -// $LastChangedDate$ -// $Author$ -// -//--------------------------------------------------------------- - -#include -#include - -#include "ec_command.h" - -//--------------------------------------------------------------- - -void EtherCAT_command_init(EtherCAT_command_t *cmd) -{ - cmd->command_type = 0x00; - cmd->node_address = 0x0000; - cmd->ring_position = 0x0000; - cmd->mem_address = 0x0000; - cmd->logical_address = 0x00000000; - cmd->data_length = 0; - cmd->status = Waiting; - cmd->next = NULL; - cmd->working_counter = 0; - cmd->data = NULL; -} - -//--------------------------------------------------------------- - -void EtherCAT_command_clear(EtherCAT_command_t *cmd) -{ - if (cmd->data) - { - free(cmd->data); - } - - EtherCAT_command_init(cmd); -} - -//--------------------------------------------------------------- - -void EtherCAT_command_print_data(EtherCAT_command_t *cmd) -{ - unsigned int i; - - printf("["); - - for (i = 0; i < cmd->data_length; i++) - { - printf("%02X", cmd->data[i]); - - if (i < cmd->data_length - 1) printf(" "); - } - - printf("]\n"); -} - -//--------------------------------------------------------------- diff -r d7ef8607e06f -r fd8b9ad48f88 user/ec_command.h --- a/user/ec_command.h Fri Dec 16 15:34:04 2005 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,42 +0,0 @@ -//--------------------------------------------------------------- -// -// e c _ c o m m a n d . h -// -// $LastChangedDate$ -// $Author$ -// -//--------------------------------------------------------------- - -typedef enum {Waiting, Sent, Received} EtherCAT_cmd_status_t; - -//--------------------------------------------------------------- - -typedef struct EtherCAT_command -{ - unsigned char command_type; - short ring_position; - unsigned short node_address; - unsigned short mem_address; - unsigned int logical_address; - unsigned int data_length; - - struct EtherCAT_command *next; - - EtherCAT_cmd_status_t status; - unsigned char command_index; - unsigned int working_counter; - - unsigned char *data; - -} -EtherCAT_command_t; - -//--------------------------------------------------------------- - -void EtherCAT_command_init(EtherCAT_command_t *); -void EtherCAT_command_clear(EtherCAT_command_t *); - -// Debug -void EtherCAT_command_print_data(EtherCAT_command_t *); - -//--------------------------------------------------------------- diff -r d7ef8607e06f -r fd8b9ad48f88 user/ec_globals.h --- a/user/ec_globals.h Fri Dec 16 15:34:04 2005 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,26 +0,0 @@ -//--------------------------------------------------------------- -// -// e c _ g l o b a l s . h -// -// $LastChangedDate$ -// $Author$ -// -//--------------------------------------------------------------- - -#define EC_CMD_APRD 0x01 // Auto-increment physical read -#define EC_CMD_APWR 0x02 // Auto-increment physical write -#define EC_CMD_NPRD 0x04 // Node-addressed physical read -#define EC_CMD_NPWR 0x05 // Node-addressed physical write -#define EC_CMD_BRD 0x07 // Broadcast read -#define EC_CMD_BWR 0x08 // Broadcast write -#define EC_CMD_LRW 0x0C // Logical read/write - -#define EC_STATE_UNKNOWN 0x00 -#define EC_STATE_INIT 0x01 -#define EC_STATE_PREOP 0x02 -#define EC_STATE_SAVEOP 0x04 -#define EC_STATE_OP 0x08 - -#define EC_ACK_STATE 0x10 - -//--------------------------------------------------------------- diff -r d7ef8607e06f -r fd8b9ad48f88 user/ec_master.c --- a/user/ec_master.c Fri Dec 16 15:34:04 2005 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1407 +0,0 @@ -//--------------------------------------------------------------- -// -// e c _ m a s t e r . c -// -// $LastChangedDate$ -// $Author$ -// -//--------------------------------------------------------------- - -#include "ec_globals.h" -#include "ec_master.h" - -#define DEBUG_SEND_RECEIVE - -//--------------------------------------------------------------- - -int EtherCAT_master_init(EtherCAT_master_t *master, - char *eth_dev) -{ - char errbuf_libpcap[PCAP_ERRBUF_SIZE]; - char errbuf_libnet[LIBNET_ERRBUF_SIZE]; - - master->slaves = NULL; - master->slave_count = 0; - - master->first_command = NULL; - master->command_index = 0x00; - - master->process_data = NULL; - master->pre_cb = NULL; - master->post_cb = NULL; - master->thread_continue = 0; - master->cycle_time = 0; - - // Init Libpcap - master->pcap_handle = pcap_open_live(eth_dev, BUFSIZ, 1, 0, errbuf_libpcap); - - if (master->pcap_handle == NULL) - { - fprintf(stderr, "Couldn't open device %s: %s\n", eth_dev, errbuf_libpcap); - return 1; - } - - // Init Libnet - if (!(master->net_handle = libnet_init(LIBNET_LINK, eth_dev, errbuf_libnet))) - { - fprintf(stderr, "Could not init %s: %s!\n", eth_dev, errbuf_libnet); - - pcap_close(master->pcap_handle); - - return 1; - } - - return 0; -} - -//--------------------------------------------------------------- - -void EtherCAT_master_clear(EtherCAT_master_t *master) -{ - libnet_destroy(master->net_handle); - pcap_close(master->pcap_handle); - - // Remove all pending commands - while (master->first_command) - { - EtherCAT_remove_command(master, master->first_command); - } - - // Remove all slaves - EtherCAT_clear_slaves(master); -} - -//--------------------------------------------------------------- - -int EtherCAT_check_slaves(EtherCAT_master_t *master, - EtherCAT_slave_t *slaves, - unsigned int slave_count) -{ - EtherCAT_command_t *cmd; - EtherCAT_slave_t *cur; - unsigned int i, j, found; - unsigned char data[2]; - - // EtherCAT_clear_slaves() must be called before! - if (master->slave_count) return -1; - - // Determine number of slaves - - if ((cmd = EtherCAT_broadcast_read(master, 0x0000, 4)) == NULL) - { - return -1; - } - - if (EtherCAT_send_receive(master) != 0) - { - return -1; - } - - master->slave_count = cmd->working_counter; - - EtherCAT_remove_command(master, cmd); - - if (master->slave_count < slave_count) - { - fprintf(stderr, "ERROR: Too few slaves on EtherCAT bus!\n"); - return -1; - } - - // No slaves. Stop further processing... - if (master->slave_count == 0) return 0; - - // For every slave in the list - for (i = 0; i < master->slave_count; i++) - { - cur = &slaves[i]; - - if (!cur->desc) - { - fprintf(stderr, "ERROR: Slave has no description (list position %i)!\n", i); - return -1; - } - - // Set ring position - cur->ring_position = -i; - cur->station_address = i + 1; - - // Write station address - - data[0] = cur->station_address & 0x00FF; - data[1] = (cur->station_address & 0xFF00) >> 8; - - if ((cmd = EtherCAT_position_write(master, cur->ring_position, 0x0010, 2, data)) == NULL) - { - master->slave_count = 0; - fprintf(stderr, "ERROR: Could not create command!\n"); - return -1; - } - - if (EtherCAT_send_receive(master) != 0) - { - master->slave_count = 0; - fprintf(stderr, "ERROR: Could not send command!\n"); - return -1; - } - - if (cmd->working_counter != 1) - { - master->slave_count = 0; - fprintf(stderr, "ERROR: Slave %i did not repond while writing station address!\n", i); - return -1; - } - - EtherCAT_remove_command(master, cmd); - - // Read base data - - if ((cmd = EtherCAT_read(master, cur->station_address, 0x0000, 4)) == NULL) - { - master->slave_count = 0; - fprintf(stderr, "ERROR: Could not create command!\n"); - return -1; - } - - if (EtherCAT_send_receive(master) != 0) - { - EtherCAT_remove_command(master, cmd); - master->slave_count = 0; - fprintf(stderr, "ERROR: Could not send command!\n"); - return -4; - } - - if (cmd->working_counter != 1) - { - EtherCAT_remove_command(master, cmd); - master->slave_count = 0; - fprintf(stderr, "ERROR: Slave %i did not respond while reading base data!\n", i); - return -5; - } - - // Get base data - cur->type = cmd->data[0]; - cur->revision = cmd->data[1]; - cur->build = cmd->data[2] | (cmd->data[3] << 8); - - EtherCAT_remove_command(master, cmd); - - // Read identification from "Slave Information Interface" (SII) - - if (EtherCAT_read_slave_information(master, cur->station_address, 0x0008, &cur->vendor_id) != 0) - { - master->slave_count = 0; - fprintf(stderr, "ERROR: Could not read SII!\n", i); - return -1; - } - - if (EtherCAT_read_slave_information(master, cur->station_address, 0x000A, &cur->product_code) != 0) - { - master->slave_count = 0; - fprintf(stderr, "ERROR: Could not read SII!\n", i); - return -1; - } - - if (EtherCAT_read_slave_information(master, cur->station_address, 0x000E, &cur->revision_number) != 0) - { - master->slave_count = 0; - fprintf(stderr, "ERROR: Could not read SII!\n", i); - return -1; - } - - // Search for identification in "database" - - found = 0; - - for (j = 0; j < slave_idents_count; j++) - { - if (slave_idents[j].vendor_id == cur->vendor_id - && slave_idents[j].product_code == cur->product_code) - { - found = 1; - - if (cur->desc != slave_idents[j].desc) - { - fprintf(stderr, "ERROR: Unexpected slave device at position %i:" - "%s %s. Expected: %s %s\n", - i, slave_idents[j].desc->vendor_name, slave_idents[j].desc->product_name, - cur->desc->vendor_name, cur->desc->product_name); - return -1; - } - - break; - } - } - - if (!found) - { - fprintf(stderr, "ERROR: Unknown slave device at position %i: Vendor %X, Code %X", - i, cur->vendor_id, cur->product_code); - return -1; - } - } - - return 0; -} - -//--------------------------------------------------------------- - -void EtherCAT_clear_slaves(EtherCAT_master_t *master) -{ - unsigned int i; - - if (master->slave_count == 0) return; - - for (i = 0; i < master->slave_count; i++) - { - EtherCAT_slave_clear(&master->slaves[i]); - } - - free(master->slaves); - master->slaves = NULL; -} - -//--------------------------------------------------------------- - -int EtherCAT_read_slave_information(EtherCAT_master_t *master, - unsigned short int node_address, - unsigned short int offset, - unsigned int *target) -{ - EtherCAT_command_t *cmd; - unsigned char data[10]; - unsigned int tries; - - // Initiate read operation - - data[0] = 0x00; - data[1] = 0x01; - data[2] = offset & 0xFF; - data[3] = (offset & 0xFF00) >> 8; - data[4] = 0x00; - data[5] = 0x00; - - if ((cmd = EtherCAT_write(master, node_address, 0x502, 6, data)) == NULL) - { - fprintf(stderr, "ERROR: Could not allocate command!\n"); - return -2; - } - - if (EtherCAT_send_receive(master) != 0) - { - EtherCAT_remove_command(master, cmd); - fprintf(stderr, "ERROR: Could not write to slave!\n"); - return -3; - } - - if (cmd->working_counter != 1) - { - EtherCAT_remove_command(master, cmd); - fprintf(stderr, "ERROR: Command not processed by slave!\n"); - return -4; - } - - EtherCAT_remove_command(master, cmd); - - // Get status of read operation - - tries = 0; - while (tries < 10) - { - if ((cmd = EtherCAT_read(master, node_address, 0x502, 10)) == NULL) - { - fprintf(stderr, "ERROR: Could not allocate command!\n"); - return -2; - } - - if (EtherCAT_send_receive(master) != 0) - { - EtherCAT_remove_command(master, cmd); - - fprintf(stderr, "ERROR: Could not read from slave!\n"); - return -3; - } - - if (cmd->working_counter != 1) - { - EtherCAT_remove_command(master, cmd); - - fprintf(stderr, "ERROR: Command not processed by slave!\n"); - return -4; - } - - if ((cmd->data[1] & 0x81) == 0) - { -#if 0 - printf("SLI status data: %02X %02X Address: %02X %02X\n", cmd->data[0], cmd->data[1], cmd->data[2], cmd->data[3]); - printf("Data: %02X %02X %02X %02X\n", cmd->data[6], cmd->data[7], cmd->data[8], cmd->data[9]); -#endif - - memcpy(target, cmd->data + 6, 4); - - EtherCAT_remove_command(master, cmd); - - break; - } - - EtherCAT_remove_command(master, cmd); - - tries++; - } - - if (tries == 10) fprintf(stderr, "ERROR: Timeout while reading SII!\n"); - - return 0; -} - -//--------------------------------------------------------------- - -int EtherCAT_send_receive(EtherCAT_master_t *master) -{ - libnet_ptag_t ptag; - struct pcap_pkthdr header; - const unsigned char *packet; - unsigned char dst[6], src[6]; - unsigned int i, length, framelength, pos, command_type, command_index; - EtherCAT_command_t *cmd; - unsigned char *data; - int bytes, command_follows, found; - -#ifdef DEBUG_SEND_RECEIVE - found = 0; -#endif - - length = 0; - for (cmd = master->first_command; cmd != NULL; cmd = cmd->next) - { - //if (cmd->status != Waiting) continue; - length += cmd->data_length + 12; - -#ifdef DEBUG_SEND_RECEIVE - found++; -#endif - } - -#ifdef DEBUG_SEND_RECEIVE - printf("Sending %i commands with length %i...\n", found, length); -#endif - - if (length == 0) return 0; - - framelength = length + 2; - if (framelength < 46) framelength = 46; - - data = (unsigned char *) malloc(sizeof(unsigned char) * framelength); - if (!data) return -1; - - data[0] = length & 0xFF; - data[1] = ((length & 0x700) >> 8) | 0x10; - pos = 2; - - for (cmd = master->first_command; cmd != NULL; cmd = cmd->next) - { - if (cmd->status != Waiting) - { - printf("Old command: %02X\n", cmd->command_type); - continue; - } - - cmd->command_index = master->command_index; - master->command_index = (master->command_index + 1) % 0x0100; - - cmd->status = Sent; - - data[pos + 0] = cmd->command_type; - data[pos + 1] = cmd->command_index; - - switch (cmd->command_type) - { - case EC_CMD_APRD: - case EC_CMD_APWR: - data[pos + 2] = cmd->ring_position & 0xFF; - data[pos + 3] = (cmd->ring_position & 0xFF00) >> 8; - data[pos + 4] = cmd->mem_address & 0xFF; - data[pos + 5] = (cmd->mem_address & 0xFF00) >> 8; - break; - - case EC_CMD_NPRD: - case EC_CMD_NPWR: - data[pos + 2] = cmd->node_address & 0xFF; - data[pos + 3] = (cmd->node_address & 0xFF00) >> 8; - data[pos + 4] = cmd->mem_address & 0xFF; - data[pos + 5] = (cmd->mem_address & 0xFF00) >> 8; - break; - - case EC_CMD_BRD: - case EC_CMD_BWR: - data[pos + 2] = 0x00; - data[pos + 3] = 0x00; - data[pos + 4] = cmd->mem_address & 0xFF; - data[pos + 5] = (cmd->mem_address & 0xFF00) >> 8; - break; - - case EC_CMD_LRW: - data[pos + 2] = cmd->logical_address & 0x000000FF; - data[pos + 3] = (cmd->logical_address & 0x0000FF00) >> 8; - data[pos + 4] = (cmd->logical_address & 0x00FF0000) >> 16; - data[pos + 5] = (cmd->logical_address & 0xFF000000) >> 24; - break; - - default: - data[pos + 2] = 0x00; - data[pos + 3] = 0x00; - data[pos + 4] = 0x00; - data[pos + 5] = 0x00; - fprintf(stderr, "WARNING: Default adress while frame construction...\n"); - break; - } - - data[pos + 6] = cmd->data_length & 0xFF; - data[pos + 7] = (cmd->data_length & 0x700) >> 8; - - if (cmd->next) data[pos + 7] |= 0x80; - - data[pos + 8] = 0x00; - data[pos + 9] = 0x00; - - if (cmd->command_type == EC_CMD_APWR - || cmd->command_type == EC_CMD_NPWR - || cmd->command_type == EC_CMD_BWR - || cmd->command_type == EC_CMD_LRW) // Write - { - for (i = 0; i < cmd->data_length; i++) data[pos + 10 + i] = cmd->data[i]; - } - else // Read - { - for (i = 0; i < cmd->data_length; i++) data[pos + 10 + i] = 0x00; - } - - data[pos + 10 + cmd->data_length] = 0x00; - data[pos + 11 + cmd->data_length] = 0x00; - - pos += 12 + cmd->data_length; - } - - // Pad with zeros - while (pos < 46) data[pos++] = 0x00; - -#ifdef DEBUG_SEND_RECEIVE - printf("\n>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n"); - for (i = 0; i < framelength; i++) - { - printf("%02X ", data[i]); - - if ((i + 1) % 16 == 0) printf("\n"); - } - printf("\n-----------------------------------------------\n"); -#endif - - dst[0] = 0xFF; - dst[1] = 0xFF; - dst[2] = 0xFF; - dst[3] = 0xFF; - dst[4] = 0xFF; - dst[5] = 0xFF; - - src[0] = 0x00; - src[1] = 0x00; - src[2] = 0x00; - src[3] = 0x00; - src[4] = 0x00; - src[5] = 0x00; - - // Send frame - ptag = libnet_build_ethernet(dst, src, 0x88A4, data, framelength, master->net_handle, 0); - bytes = libnet_write(master->net_handle); - libnet_clear_packet(master->net_handle); - - if (bytes == -1) - { - free(data); - fprintf(stderr, "Could not write!\n"); - return -1; - } - - packet = pcap_next(master->pcap_handle, &header); // LibPCap receives sent frame first - packet = pcap_next(master->pcap_handle, &header); - -#ifdef DEBUG_SEND_RECEIVE - for (i = 0; i < header.len - 14; i++) - { - if (packet[i + 14] == data[i]) printf(" "); - else printf("%02X ", packet[i + 14]); - - if ((i + 1) % 16 == 0) printf("\n"); - } - printf("\n<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<\n\n"); -#endif - - // Free sent data - free(data); - - pos = 16; - command_follows = 1; - while (command_follows) - { - command_type = packet[pos]; - command_index = packet[pos + 1]; - length = (packet[pos + 6] & 0xFF) | ((packet[pos + 7] & 0x07) << 8); - command_follows = packet[pos + 7] & 0x80; - -#if 0 - printf("Command %02X received!\n", command_index); -#endif - - found = 0; - - for (cmd = master->first_command; cmd != NULL; cmd = cmd->next) - { - if (cmd->status == Sent - && cmd->command_type == command_type - && cmd->command_index == command_index - && cmd->data_length == length) - { - found = 1; - cmd->status = Received; - - cmd->data = (unsigned char *) malloc(sizeof(unsigned char) * length); - memcpy(cmd->data, packet + pos + 10, length); - - cmd->working_counter = (packet[pos + length + 10] & 0xFF) - | ((packet[pos + length + 11] & 0xFF) << 8); - } - } - - if (!found) - { - fprintf(stderr, "WARNING: Command not assigned!\n"); - } - - pos += length + 12; - } - - for (cmd = master->first_command; cmd != NULL; cmd = cmd->next) - { - if (cmd->status == Sent) - { - fprintf(stderr, "WARNING: Command not sent!\n"); - } - } - - return 0; -} - -//--------------------------------------------------------------- - -EtherCAT_command_t *EtherCAT_read(EtherCAT_master_t *master, - unsigned short node_address, - unsigned short offset, - unsigned int length) -{ - EtherCAT_command_t *cmd; - - if (node_address == 0x0000) fprintf(stderr, "WARNING: Using node address 0x0000!\n"); - - cmd = (EtherCAT_command_t *) malloc(sizeof(EtherCAT_command_t)); - - if (cmd == NULL) - { - return NULL; - } - - EtherCAT_command_init(cmd); - - cmd->command_type = EC_CMD_NPRD; - cmd->node_address = node_address; - cmd->mem_address = offset; - cmd->data_length = length; - - // Add command to master's list - add_command(master, cmd); - - return cmd; -} - -//--------------------------------------------------------------- - -EtherCAT_command_t *EtherCAT_write(EtherCAT_master_t *master, - unsigned short node_address, - unsigned short offset, - unsigned int length, - const unsigned char *data) -{ - EtherCAT_command_t *cmd; - - if (node_address == 0x0000) fprintf(stderr, "WARNING: Using node address 0x0000!\n"); - - cmd = (EtherCAT_command_t *) malloc(sizeof(EtherCAT_command_t)); - - if (cmd == NULL) - { - return NULL; - } - - EtherCAT_command_init(cmd); - - cmd->command_type = EC_CMD_NPWR; - cmd->node_address = node_address; - cmd->mem_address = offset; - cmd->data_length = length; - - cmd->data = (unsigned char *) malloc(sizeof(unsigned char) * length); - - if (cmd->data == NULL) - { - free(cmd); - return NULL; - } - - memcpy(cmd->data, data, length); - - // Add command to master's list - add_command(master, cmd); - - return cmd; -} - -//--------------------------------------------------------------- - -EtherCAT_command_t *EtherCAT_position_read(EtherCAT_master_t *master, - short ring_position, - unsigned short offset, - unsigned int length) -{ - EtherCAT_command_t *cmd; - - cmd = (EtherCAT_command_t *) malloc(sizeof(EtherCAT_command_t)); - - if (cmd == NULL) - { - return NULL; - } - - EtherCAT_command_init(cmd); - - cmd->command_type = EC_CMD_APRD; - cmd->ring_position = ring_position; - cmd->mem_address = offset; - cmd->data_length = length; - - // Add command to master's list - add_command(master, cmd); - - return cmd; -} - -//--------------------------------------------------------------- - -EtherCAT_command_t *EtherCAT_position_write(EtherCAT_master_t *master, - short ring_position, - unsigned short offset, - unsigned int length, - const unsigned char *data) -{ - EtherCAT_command_t *cmd; - - cmd = (EtherCAT_command_t *) malloc(sizeof(EtherCAT_command_t)); - - if (cmd == NULL) - { - return NULL; - } - - EtherCAT_command_init(cmd); - - cmd->command_type = EC_CMD_APWR; - cmd->ring_position = ring_position; - cmd->mem_address = offset; - cmd->data_length = length; - - cmd->data = (unsigned char *) malloc(sizeof(unsigned char) * length); - - if (cmd->data == NULL) - { - free(cmd); - return NULL; - } - - memcpy(cmd->data, data, length); - - // Add command to master's list - add_command(master, cmd); - - return cmd; -} - -//--------------------------------------------------------------- - -EtherCAT_command_t *EtherCAT_broadcast_read(EtherCAT_master_t *master, - unsigned short offset, - unsigned int length) -{ - EtherCAT_command_t *cmd; - - cmd = (EtherCAT_command_t *) malloc(sizeof(EtherCAT_command_t)); - - if (cmd == NULL) - { - return NULL; - } - - EtherCAT_command_init(cmd); - - cmd->command_type = EC_CMD_BRD; - cmd->mem_address = offset; - cmd->data_length = length; - - // Add command to master's list - add_command(master, cmd); - - return cmd; -} - -//--------------------------------------------------------------- - -EtherCAT_command_t *EtherCAT_broadcast_write(EtherCAT_master_t *master, - unsigned short offset, - unsigned int length, - const unsigned char *data) -{ - EtherCAT_command_t *cmd; - - cmd = (EtherCAT_command_t *) malloc(sizeof(EtherCAT_command_t)); - - if (cmd == NULL) - { - return NULL; - } - - EtherCAT_command_init(cmd); - - cmd->command_type = EC_CMD_BWR; - cmd->mem_address = offset; - cmd->data_length = length; - - cmd->data = (unsigned char *) malloc(sizeof(unsigned char) * length); - - if (cmd->data == NULL) - { - free(cmd); - return NULL; - } - - memcpy(cmd->data, data, length); - - // Add command to master's list - add_command(master, cmd); - - return cmd; -} - -//--------------------------------------------------------------- - -EtherCAT_command_t *EtherCAT_logical_read_write(EtherCAT_master_t *master, - unsigned int offset, - unsigned int length, - unsigned char *data) -{ - EtherCAT_command_t *cmd; - - cmd = (EtherCAT_command_t *) malloc(sizeof(EtherCAT_command_t)); - - if (cmd == NULL) - { - return NULL; - } - - EtherCAT_command_init(cmd); - - cmd->command_type = EC_CMD_LRW; - cmd->mem_address = offset; - cmd->data_length = length; - - cmd->data = (unsigned char *) malloc(sizeof(unsigned char) * length); - - if (cmd->data == NULL) - { - free(cmd); - return NULL; - } - - memcpy(cmd->data, data, length); - - // Add command to master's list - add_command(master, cmd); - - return cmd; -} - -//--------------------------------------------------------------- - -void add_command(EtherCAT_master_t *master, EtherCAT_command_t *cmd) -{ - EtherCAT_command_t **last_cmd; - - // Find last position in the list - last_cmd = &(master->first_command); - while (*last_cmd) last_cmd = &(*last_cmd)->next; - - *last_cmd = cmd; -} - -//--------------------------------------------------------------- - -void EtherCAT_remove_command(EtherCAT_master_t *master, - EtherCAT_command_t *rem_cmd) -{ - EtherCAT_command_t **last_cmd; - - last_cmd = &(master->first_command); - while (*last_cmd) - { - if (*last_cmd == rem_cmd) - { - *last_cmd = rem_cmd->next; - EtherCAT_command_clear(rem_cmd); - - return; - } - - last_cmd = &(*last_cmd)->next; - } -} - -//--------------------------------------------------------------- - -int EtherCAT_state_change(EtherCAT_master_t *master, - EtherCAT_slave_t *slave, - unsigned char state_and_ack) -{ - EtherCAT_command_t *cmd; - unsigned char data[2]; - unsigned int tries_left; - - data[0] = state_and_ack; - data[1] = 0x00; - - if ((cmd = EtherCAT_write(master, slave->station_address, - 0x0120, 2, data)) == NULL) - { - return -1; - } - - if (EtherCAT_send_receive(master) != 0) - { - EtherCAT_remove_command(master, cmd); - return -2; - } - - if (cmd->working_counter != 1) - { - EtherCAT_remove_command(master, cmd); - return -3; - } - - EtherCAT_remove_command(master, cmd); - - slave->requested_state = state_and_ack & 0x0F; - - tries_left = 10; - - while (tries_left) - { - if ((cmd = EtherCAT_read(master, slave->station_address, - 0x0130, 2)) == NULL) - { - return -1; - } - - if (EtherCAT_send_receive(master) != 0) - { - EtherCAT_remove_command(master, cmd); - return -2; - } - - if (cmd->working_counter != 1) - { - EtherCAT_remove_command(master, cmd); - return -3; - } - - if (cmd->data[0] & 0x10) // State change error - { - EtherCAT_remove_command(master, cmd); - return -4; - } - - if (cmd->data[0] == state_and_ack & 0x0F) // State change successful - { - EtherCAT_remove_command(master, cmd); - break; - } - - EtherCAT_remove_command(master, cmd); - - //printf("Trying again...\n"); - - tries_left--; - } - - if (!tries_left) - { - return -5; - } - - slave->current_state = state_and_ack & 0x0F; - - return 0; -} - -//--------------------------------------------------------------- - -int EtherCAT_broadcast_state_change(EtherCAT_master_t *master, - unsigned char state) -{ - EtherCAT_command_t *cmd; - unsigned char data[2]; - unsigned int tries_left; - - data[0] = state; - data[1] = 0x00; - - if ((cmd = EtherCAT_broadcast_write(master, 0x0120, 2, data)) == NULL) - { - return -1; - } - - if (EtherCAT_send_receive(master) != 0) - { - EtherCAT_remove_command(master, cmd); - return -2; - } - - if (cmd->working_counter != master->slave_count) - { - EtherCAT_remove_command(master, cmd); - return -3; - } - - EtherCAT_remove_command(master, cmd); - - tries_left = 10; - - while (tries_left) - { - if ((cmd = EtherCAT_broadcast_read(master, 0x0130, 2)) == NULL) - { - return -1; - } - - if (EtherCAT_send_receive(master) != 0) - { - EtherCAT_remove_command(master, cmd); - return -2; - } - - if (cmd->working_counter != master->slave_count) - { - EtherCAT_remove_command(master, cmd); - return -3; - } - - if (cmd->data[0] & 0x10) // State change error - { - EtherCAT_remove_command(master, cmd); - return -4; - } - - if (cmd->data[0] == state) // State change successful - { - EtherCAT_remove_command(master, cmd); - break; - } - - EtherCAT_remove_command(master, cmd); - - //printf("Trying again...\n"); - - tries_left--; - } - - if (!tries_left) - { - return -5; - } - - return 0; -} - -//--------------------------------------------------------------- - -int EtherCAT_start(EtherCAT_master_t *master, - unsigned int length, - void (*pre_cb)(unsigned char *), - void (*post_cb)(unsigned char *), - unsigned int cycle_time) -{ - if (master->process_data) - { - fprintf(stderr, "ERROR: Process data already allocated!\n"); - return -1; - } - - if ((master->process_data = (unsigned char *) malloc(length)) == NULL) - { - fprintf(stderr, "ERROR: Could not allocate process data block!\n"); - return -2; - } - - memset(master->process_data, 0x00, length); - - master->process_data_length = length; - master->pre_cb = pre_cb; - master->post_cb = post_cb; - master->cycle_time = cycle_time; - - master->thread_continue = 1; - - if (pthread_create(&master->thread, NULL, thread_function, (void *) master) != 0) - { - fprintf(stderr, "ERROR: Could not create thread!\n"); - return -3; - } - - return 0; -} - -//--------------------------------------------------------------- - -int EtherCAT_stop(EtherCAT_master_t *master) -{ - if (!master->thread_continue) - { - fprintf(stderr, "ERROR: Thread not running!\n"); - return -1; - } - - master->thread_continue = 0; - pthread_join(master->thread, NULL); - - if (master->process_data) - { - free(master->process_data); - master->process_data = NULL; - } - - master->pre_cb = NULL; - master->post_cb = NULL; - - return 0; -} - -//--------------------------------------------------------------- - -double current_timestamp() -{ - struct timeval tv; - gettimeofday(&tv, NULL); - return tv.tv_sec + (double) tv.tv_usec / 1000000.0; -} - -//--------------------------------------------------------------- - -void *thread_function(void *data) -{ - EtherCAT_master_t *master; - EtherCAT_command_t *cmd; - double bus_start_time, bus_end_time; - double cycle_start_time, cycle_end_time, last_cycle_start_time; - unsigned int wait_usecs; - - master = (EtherCAT_master_t *) data; - - last_cycle_start_time = 0.0; - - while (master->thread_continue) - { - cycle_start_time = current_timestamp(); - - if (last_cycle_start_time != 0.0) - { - master->last_cycle_time = cycle_start_time - last_cycle_start_time; - master->last_jitter = (master->last_cycle_time - (master->cycle_time / 1000000.0)) - / (master->cycle_time / 1000000.0) * 100.0; - } - - last_cycle_start_time = cycle_start_time; - - if (master->pre_cb) master->pre_cb(master->process_data); - - cmd = EtherCAT_logical_read_write(master, - 0x00000000, - master->process_data_length, - master->process_data); - - bus_start_time = current_timestamp(); - - EtherCAT_send_receive(master); - - bus_end_time = current_timestamp(); - master->bus_time = bus_end_time - bus_start_time; - -#if 0 - printf("Working counter: %i\n", cmd->working_counter); -#endif - - memcpy(master->process_data, cmd->data, master->process_data_length); - - EtherCAT_remove_command(master, cmd); - - if (master->post_cb) master->post_cb(master->process_data); - - // Calculate working time - - cycle_end_time = current_timestamp(); - master->last_cycle_work_time = cycle_end_time - cycle_start_time; - master->last_cycle_busy_rate = master->last_cycle_work_time - / ((double) master->cycle_time / 1000000.0) * 100.0; - wait_usecs = master->cycle_time - (unsigned int) (master->last_cycle_work_time * 1000000.0); - - //printf("USECS to wait: %i\n", wait_usecs); - - usleep(wait_usecs); - - //printf("waited: %lf\n", current_timestamp() - cycle_end_time); - } - - return (void *) 0; -} - -//--------------------------------------------------------------- - -int EtherCAT_activate_slave(EtherCAT_master_t *master, - EtherCAT_slave_t *slave) -{ - EtherCAT_command_t *cmd; - const EtherCAT_slave_desc_t *desc; - unsigned char fmmu[16]; - unsigned char data[256]; - - if (EtherCAT_state_change(master, slave, EC_STATE_INIT) != 0) - { - return -1; - } - - // Resetting FMMU's - - memset(data, 0x00, 256); - cmd = EtherCAT_write(master, slave->station_address, 0x0600, 256, data); - EtherCAT_send_receive(master); - - if (cmd->working_counter != 1) - { - fprintf(stderr, "ERROR: Slave did not respond!\n"); - return -2; - } - - EtherCAT_remove_command(master, cmd); - - // Resetting Sync Manager channels - - memset(data, 0x00, 256); - cmd = EtherCAT_write(master, slave->station_address, 0x0800, 256, data); - EtherCAT_send_receive(master); - - if (cmd->working_counter != 1) - { - fprintf(stderr, "ERROR: Slave did not respond!\n"); - return -2; - } - - EtherCAT_remove_command(master, cmd); - - desc = slave->desc; - - // Init Mailbox communication - - if (desc->type == MAILBOX) - { - if (desc->sm0) - { - cmd = EtherCAT_write(master, slave->station_address, 0x0800, 8, desc->sm0); - - EtherCAT_send_receive(master); - - if (cmd->working_counter != 1) - { - fprintf(stderr, "ERROR: Not all slaves responded!\n"); - return -3; - } - - EtherCAT_remove_command(master, cmd); - } - - if (desc->sm1) - { - cmd = EtherCAT_write(master, slave->station_address, 0x0808, 8, desc->sm1); - - EtherCAT_send_receive(master); - - if (cmd->working_counter != 1) - { - fprintf(stderr, "ERROR: Not all slaves responded!\n"); - return -4; - } - - EtherCAT_remove_command(master, cmd); - } - } - - // Change state to PREOP - - if (EtherCAT_state_change(master, slave, EC_STATE_PREOP) != 0) - { - fprintf(stderr, "ERROR: Could not set state!\n"); - return -5; - } - - // Set FMMU's - - if (desc->fmmu0) - { - memcpy(fmmu, desc->fmmu0, 16); - - fmmu[0] = slave->logical_address0 & 0x000000FF; - fmmu[1] = (slave->logical_address0 & 0x0000FF00) >> 8; - fmmu[2] = (slave->logical_address0 & 0x00FF0000) >> 16; - fmmu[3] = (slave->logical_address0 & 0xFF000000) >> 24; - - cmd = EtherCAT_write(master, slave->station_address, 0x0600, 16, fmmu); - EtherCAT_send_receive(master); - - if (cmd->working_counter != 1) - { - fprintf(stderr, "ERROR: Not all slaves responded (%i of 1)!\n", - cmd->working_counter); - return -6; - } - - EtherCAT_remove_command(master, cmd); - } - - // Set Sync Managers - - if (desc->type != MAILBOX) - { - if (desc->sm0) - { - cmd = EtherCAT_write(master, slave->station_address, 0x0800, 8, desc->sm0); - - EtherCAT_send_receive(master); - - if (cmd->working_counter != 1) - { - fprintf(stderr, "ERROR: Not all slaves responded!\n"); - return -8; - } - - EtherCAT_remove_command(master, cmd); - } - - if (desc->sm1) - { - cmd = EtherCAT_write(master, slave->station_address, 0x0808, 8, desc->sm1); - - EtherCAT_send_receive(master); - - if (cmd->working_counter != 1) - { - fprintf(stderr, "ERROR: Not all slaves responded!\n"); - return -9; - } - - EtherCAT_remove_command(master, cmd); - } - } - - if (desc->sm2) - { - cmd = EtherCAT_write(master, slave->station_address, 0x0810, 8, desc->sm2); - - EtherCAT_send_receive(master); - - if (cmd->working_counter != 1) - { - fprintf(stderr, "ERROR: Not all slaves responded!\n"); - return -10; - } - - EtherCAT_remove_command(master, cmd); - } - - if (desc->sm3) - { - cmd = EtherCAT_write(master, slave->station_address, 0x0818, 8, desc->sm3); - - EtherCAT_send_receive(master); - - if (cmd->working_counter != 1) - { - fprintf(stderr, "ERROR: Not all slaves responded!\n"); - return -11; - } - - EtherCAT_remove_command(master, cmd); - } - - // Change state to SAVEOP - - if (EtherCAT_state_change(master, slave, EC_STATE_SAVEOP) != 0) - { - fprintf(stderr, "ERROR: Could not set state!\n"); - return -12; - } - - // Change state to OP - - if (EtherCAT_state_change(master, slave, EC_STATE_OP) != 0) - { - fprintf(stderr, "ERROR: Could not set state!\n"); - return -13; - } - - return 0; -} - -//--------------------------------------------------------------- - -int EtherCAT_deactivate_slave(EtherCAT_master_t *master, - EtherCAT_slave_t *slave) -{ - if (EtherCAT_state_change(master, slave, EC_STATE_INIT) != 0) - { - return -1; - } - - return 0; -} - -//--------------------------------------------------------------- - -void set_byte(unsigned char *data, - unsigned int offset, - unsigned char value) -{ - data[offset] = value; -} - -//--------------------------------------------------------------- - -void set_word(unsigned char *data, - unsigned int offset, - unsigned int value) -{ - data[offset] = value & 0xFF; - data[offset + 1] = (value & 0xFF00) >> 8; -} - -//--------------------------------------------------------------- diff -r d7ef8607e06f -r fd8b9ad48f88 user/ec_master.h --- a/user/ec_master.h Fri Dec 16 15:34:04 2005 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,120 +0,0 @@ -//--------------------------------------------------------------- -// -// e c _ m a s t e r . h -// -// $LastChangedDate$ -// $Author$ -// -//--------------------------------------------------------------- - -#include -#include -#include - -#include "ec_slave.h" -#include "ec_command.h" - -//--------------------------------------------------------------- - -typedef struct -{ - EtherCAT_slave_t *slaves; // Slaves array - unsigned int slave_count; // Number of slaves - - EtherCAT_command_t *first_command; // List of commands - - pcap_t *pcap_handle; // Handle for libpcap - libnet_t *net_handle; // Handle for libnet - - unsigned char command_index; // Current command index - - unsigned char *process_data; - unsigned int process_data_length; - void (*pre_cb)(unsigned char *); - void (*post_cb)(unsigned char *); - pthread_t thread; - int thread_continue; - unsigned int cycle_time; - - double bus_time; - - double last_jitter; - double last_cycle_time; - double last_cycle_work_time; - double last_cycle_busy_rate; -} -EtherCAT_master_t; - -//--------------------------------------------------------------- - -// Master creation and deletion -int EtherCAT_master_init(EtherCAT_master_t *, char *); -void EtherCAT_master_clear(EtherCAT_master_t *); - -// Checking for slaves -int EtherCAT_check_slaves(EtherCAT_master_t *, EtherCAT_slave_t *, unsigned int); -void EtherCAT_clear_slaves(EtherCAT_master_t *); - -// Slave information interface -int EtherCAT_read_slave_information(EtherCAT_master_t *, - unsigned short int, - unsigned short int, - unsigned int *); - -// EtherCAT commands -EtherCAT_command_t *EtherCAT_read(EtherCAT_master_t *, - unsigned short, - unsigned short, - unsigned int); -EtherCAT_command_t *EtherCAT_write(EtherCAT_master_t *, - unsigned short, - unsigned short, - unsigned int, - const unsigned char *); -EtherCAT_command_t *EtherCAT_position_read(EtherCAT_master_t *, - short, - unsigned short, - unsigned int); -EtherCAT_command_t *EtherCAT_position_write(EtherCAT_master_t *, - short, - unsigned short, - unsigned int, - const unsigned char *); -EtherCAT_command_t *EtherCAT_broadcast_read(EtherCAT_master_t *, - unsigned short, - unsigned int); -EtherCAT_command_t *EtherCAT_broadcast_write(EtherCAT_master_t *, - unsigned short, - unsigned int, - const unsigned char *); -EtherCAT_command_t *EtherCAT_logical_read_write(EtherCAT_master_t *, - unsigned int, - unsigned int, - unsigned char *); - -void EtherCAT_remove_command(EtherCAT_master_t *, EtherCAT_command_t *); - -// Slave states -int EtherCAT_state_change(EtherCAT_master_t *, EtherCAT_slave_t *, unsigned char); -int EtherCAT_broadcast_state_change(EtherCAT_master_t *, unsigned char); - -int EtherCAT_activate_slave(EtherCAT_master_t *, EtherCAT_slave_t *); -int EtherCAT_deactivate_slave(EtherCAT_master_t *, EtherCAT_slave_t *); - -// Sending and receiving -int EtherCAT_send_receive(EtherCAT_master_t *); - -int EtherCAT_start(EtherCAT_master_t *, - unsigned int, - void (*)(unsigned char *), - void (*)(unsigned char *), - unsigned int); -int EtherCAT_stop(EtherCAT_master_t *); - -// Private functions -void add_command(EtherCAT_master_t *, EtherCAT_command_t *); -void set_byte(unsigned char *, unsigned int, unsigned char); -void set_word(unsigned char *, unsigned int, unsigned int); -void *thread_function(void *); - -//--------------------------------------------------------------- diff -r d7ef8607e06f -r fd8b9ad48f88 user/ec_slave.c --- a/user/ec_slave.c Fri Dec 16 15:34:04 2005 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,137 +0,0 @@ -//--------------------------------------------------------------- -// -// e c _ s l a v e . c -// -// $LastChangedDate$ -// $Author$ -// -//--------------------------------------------------------------- - -#include - -#include "ec_globals.h" -#include "ec_slave.h" - -//--------------------------------------------------------------- - -void EtherCAT_slave_init(EtherCAT_slave_t *slave) -{ - slave->type = 0; - slave->revision = 0; - slave->build = 0; - - slave->ring_position = 0; - slave->station_address = 0; - - slave->vendor_id = 0; - slave->product_code = 0; - slave->revision_number = 0; - - slave->desc = NULL; - - slave->logical_address0 = 0; - - slave->current_state = EC_STATE_UNKNOWN; - slave->requested_state = EC_STATE_UNKNOWN; -} - -//--------------------------------------------------------------- - -void EtherCAT_slave_clear(EtherCAT_slave_t *slave) -{ - // Nothing yet... -} - -//--------------------------------------------------------------- - -void EtherCAT_slave_print(EtherCAT_slave_t *slave) -{ -} - -//--------------------------------------------------------------- - -unsigned char sm0_multi[] = {0x00, 0x18, 0xF6, 0x00, 0x26, 0x00, 0x01, 0x00}; -unsigned char sm1_multi[] = {0xF6, 0x18, 0xF6, 0x00, 0x22, 0x00, 0x01, 0x00}; - -unsigned char sm0_1014[] = {0x00, 0x10, 0x01, 0x00, 0x00, 0x00, 0x01, 0x00}; - -unsigned char sm0_2004[] = {0x00, 0x0F, 0x01, 0x00, 0x46, 0x00, 0x01, 0x00}; - -unsigned char sm2_31xx[] = {0x00, 0x10, 0x04, 0x00, 0x24, 0x00, 0x00, 0x00}; -unsigned char sm3_31xx[] = {0x00, 0x11, 0x06, 0x00, 0x20, 0x00, 0x01, 0x00}; - -unsigned char sm2_4102[] = {0x00, 0x10, 0x04, 0x00, 0x24, 0x00, 0x01, 0x00}; - - -unsigned char fmmu0_1014[] = {0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x07, - 0x00, 0x10, 0x00, 0x01, 0x01, 0x00, 0x00, 0x00}; - -unsigned char fmmu0_2004[] = {0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x07, - 0x00, 0x0F, 0x00, 0x02, 0x01, 0x00, 0x00, 0x00}; - -unsigned char fmmu0_31xx[] = {0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x07, - 0x00, 0x11, 0x00, 0x01, 0x01, 0x00, 0x00, 0x00}; - -unsigned char fmmu0_4102[] = {0x00, 0x00, 0x00, 0x00, 0x04, 0x00, 0x00, 0x07, - 0x00, 0x10, 0x00, 0x02, 0x01, 0x00, 0x00, 0x00}; - -//--------------------------------------------------------------- - -EtherCAT_slave_desc_t Beckhoff_EK1100[] = {"Beckhoff", "EK1100", "Bus Coupler", - SIMPLE, - NULL, NULL, NULL, NULL, // Noch nicht eingepflegt... - NULL, - 0}; - -EtherCAT_slave_desc_t Beckhoff_EL1014[] = {"Beckhoff", "EL1014", "4x Digital Input", - SIMPLE, - sm0_1014, NULL, NULL, NULL, - fmmu0_1014, - 1}; - -EtherCAT_slave_desc_t Beckhoff_EL2004[] = {"Beckhoff", "EL2004", "4x Digital Output", - SIMPLE, - sm0_2004, NULL, NULL, NULL, - fmmu0_2004, - 1}; - -EtherCAT_slave_desc_t Beckhoff_EL3102[] = {"Beckhoff", "EL3102", "2x Analog Input Diff", - MAILBOX, - sm0_multi, sm1_multi, sm2_31xx, sm3_31xx, - fmmu0_31xx, - 6}; - -EtherCAT_slave_desc_t Beckhoff_EL3162[] = {"Beckhoff", "EL3162", "2x Analog Input", - MAILBOX, - sm0_multi, sm1_multi, sm2_31xx, sm3_31xx, - fmmu0_31xx, - 6}; - -EtherCAT_slave_desc_t Beckhoff_EL4102[] = {"Beckhoff", "EL4102", "2x Analog Output", - MAILBOX, - sm0_multi, sm1_multi, sm2_4102, NULL, - fmmu0_4102, - 4}; - -EtherCAT_slave_desc_t Beckhoff_EL5001[] = {"Beckhoff", "EL5001", "SSI-Interface", - SIMPLE, - NULL, NULL, NULL, NULL, // Noch nicht eingepflegt... - NULL, - 0}; - -//--------------------------------------------------------------- - -unsigned int slave_idents_count = 7; - -struct slave_ident slave_idents[] = -{ - {0x00000002, 0x03F63052, Beckhoff_EL1014}, - {0x00000002, 0x044C2C52, Beckhoff_EK1100}, - {0x00000002, 0x07D43052, Beckhoff_EL2004}, - {0x00000002, 0x0C1E3052, Beckhoff_EL3102}, - {0x00000002, 0x0C5A3052, Beckhoff_EL3162}, - {0x00000002, 0x10063052, Beckhoff_EL4102}, - {0x00000002, 0x13893052, Beckhoff_EL5001} -}; - -//--------------------------------------------------------------- diff -r d7ef8607e06f -r fd8b9ad48f88 user/ec_slave.h --- a/user/ec_slave.h Fri Dec 16 15:34:04 2005 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,96 +0,0 @@ -//--------------------------------------------------------------- -// -// e c _ s l a v e . h -// -// $LastChangedDate$ -// $Author$ -// -//--------------------------------------------------------------- - -#define SIMPLE 0 -#define MAILBOX 1 - -//--------------------------------------------------------------- - -typedef struct slave_desc EtherCAT_slave_desc_t; - -typedef struct -{ - // Base data - unsigned char type; - unsigned char revision; - unsigned short build; - - // Addresses - short ring_position; - unsigned short station_address; - - // Slave information interface - unsigned int vendor_id; - unsigned int product_code; - unsigned int revision_number; - - const EtherCAT_slave_desc_t *desc; - - unsigned int logical_address0; - - unsigned int current_state; - unsigned int requested_state; - - unsigned char *process_data; -} -EtherCAT_slave_t; - -#define ECAT_INIT_SLAVE(TYPE) {0, 0, 0, 0, 0, 0, 0, 0, TYPE, 0, 0, 0, NULL} - -//--------------------------------------------------------------- - -// Slave construction and deletion -void EtherCAT_slave_init(EtherCAT_slave_t *); -void EtherCAT_slave_clear(EtherCAT_slave_t *); - -// Debug -void EtherCAT_slave_print(EtherCAT_slave_t *); - -//--------------------------------------------------------------- - -typedef struct slave_desc -{ - const char *vendor_name; - const char *product_name; - const char *product_desc; - - const int type; - - const unsigned char *sm0; - const unsigned char *sm1; - const unsigned char *sm2; - const unsigned char *sm3; - - const unsigned char *fmmu0; - - const unsigned int data_length; -} -EtherCAT_slave_desc_t; - -extern EtherCAT_slave_desc_t Beckhoff_EK1100[]; -extern EtherCAT_slave_desc_t Beckhoff_EL1014[]; -extern EtherCAT_slave_desc_t Beckhoff_EL2004[]; -extern EtherCAT_slave_desc_t Beckhoff_EL3102[]; -extern EtherCAT_slave_desc_t Beckhoff_EL3162[]; -extern EtherCAT_slave_desc_t Beckhoff_EL4102[]; -extern EtherCAT_slave_desc_t Beckhoff_EL5001[]; - -//--------------------------------------------------------------- - -struct slave_ident -{ - const unsigned int vendor_id; - const unsigned int product_code; - const EtherCAT_slave_desc_t *desc; -}; - -extern struct slave_ident slave_idents[]; -extern unsigned int slave_idents_count; - -//--------------------------------------------------------------- diff -r d7ef8607e06f -r fd8b9ad48f88 user/main.c --- a/user/main.c Fri Dec 16 15:34:04 2005 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,439 +0,0 @@ -//--------------------------------------------------------------- -// -// m a i n . c -// -// $LastChangedDate$ -// $Author$ -// -//--------------------------------------------------------------- - -#include -#include // memset() -#include // usleep() -#include - -#include "ec_globals.h" -#include "ec_master.h" - -//--------------------------------------------------------------- - -void signal_handler(int); -void write_data(unsigned char *); - -int continue_running; -unsigned short int word; - -//--------------------------------------------------------------- - -int main(int argc, char **argv) -{ - EtherCAT_master_t master; - EtherCAT_command_t *cmd, *cmd2; - unsigned char data[256]; - unsigned int i, number; - struct sigaction sa; - - sa.sa_handler = signal_handler; - sigaction(SIGINT, &sa, NULL); - - printf("CatEther-Testprogramm.\n"); - - EtherCAT_master_init(&master, "eth1"); - - if (EtherCAT_check_slaves(&master, NULL, 0) != 0) - { - fprintf(stderr, "ERROR while searching for slaves!\n"); - return -1; - } - - if (master.slave_count == 0) - { - fprintf(stderr, "ERROR: No slaves found!\n"); - return -1; - } - - for (i = 0; i < master.slave_count; i++) - { - printf("Slave found: Type %02X, Revision %02X, Build %04X\n", - master.slaves[i].type, master.slaves[i].revision, master.slaves[i].build); - } - - printf("Writing Station addresses.\n"); - - for (i = 0; i < master.slave_count; i++) - { - data[0] = i & 0x00FF; - data[1] = (i & 0xFF00) >> 8; - - cmd = EtherCAT_position_write(&master, 0 - i, 0x0010, 2, data); - - EtherCAT_send_receive(&master); - - if (cmd->working_counter != 1) - { - fprintf(stderr, "ERROR: Slave did'n repond!\n"); - return -1; - } - - EtherCAT_remove_command(&master, cmd); - } - - //---------- - - for (i = 0; i < master.slave_count; i++) - { - printf("\nKlemme %i:\n", i); - - EtherCAT_read_slave_information(&master, i, 0x0008, &number); - printf("Vendor ID: 0x%04X (%i)\n", number, number); - - EtherCAT_read_slave_information(&master, i, 0x000A, &number); - printf("Product Code: 0x%04X (%i)\n", number, number); - - EtherCAT_read_slave_information(&master, i, 0x000E, &number); - printf("Revision Number: 0x%04X (%i)\n", number, number); - } - - //---------- - - printf("\nResetting FMMU's.\n"); - - memset(data, 0x00, 256); - cmd = EtherCAT_broadcast_write(&master, 0x0600, 256, data); - EtherCAT_send_receive(&master); - - if (cmd->working_counter != master.slave_count) - { - fprintf(stderr, "ERROR: Not all slaves responded (%i of %i)!\n", - cmd->working_counter, master.slave_count); - return -1; - } - - EtherCAT_remove_command(&master, cmd); - - //---------- - - printf("Resetting Sync Manager channels.\n"); - - memset(data, 0x00, 256); - cmd = EtherCAT_broadcast_write(&master, 0x0800, 256, data); - EtherCAT_send_receive(&master); - - if (cmd->working_counter != master.slave_count) - { - fprintf(stderr, "ERROR: Not all slaves responded (%i of %i)!\n", - cmd->working_counter, master.slave_count); - return -1; - } - - EtherCAT_remove_command(&master, cmd); - - //---------- - - printf("Setting INIT state for devices.\n"); - - if (EtherCAT_broadcast_state_change(&master, EC_STATE_INIT) != 0) - { - fprintf(stderr, "ERROR: Could not set INIT state!\n"); - return -1; - } - - //---------- - - printf("Setting PREOP state for bus coupler.\n"); - - if (EtherCAT_state_change(&master, &master.slaves[0], EC_STATE_PREOP) != 0) - { - fprintf(stderr, "ERROR: Could not set state!\n"); - return -1; - } - - //---------- - - printf("Setting Sync managers 0 and 1 of device 1.\n"); - - data[0] = 0x00; - data[1] = 0x18; - data[2] = 0xF6; - data[3] = 0x00; - data[4] = 0x26; - data[5] = 0x00; - data[6] = 0x01; - data[7] = 0x00; - cmd = EtherCAT_write(&master, 0x0001, 0x0800, 8, data); - - data[0] = 0xF6; - data[1] = 0x18; - data[2] = 0xF6; - data[3] = 0x00; - data[4] = 0x22; - data[5] = 0x00; - data[6] = 0x01; - data[7] = 0x00; - cmd2 = EtherCAT_write(&master, 0x0001, 0x0808, 8, data); - - EtherCAT_send_receive(&master); - - if (cmd->working_counter != 1 || cmd2->working_counter != 1) - { - fprintf(stderr, "ERROR: Not all slaves responded!\n"); - - return -1; - } - - EtherCAT_remove_command(&master, cmd); - EtherCAT_remove_command(&master, cmd2); - - - //---------- - - printf("Setting PREOP state for device 1.\n"); - - if (EtherCAT_state_change(&master, &master.slaves[1], EC_STATE_PREOP)) - { - fprintf(stderr, "ERROR: Could not set state!\n"); - return -1; - } - - //---------- - - printf("Setting PREOP state for device 4.\n"); - - if (EtherCAT_state_change(&master, &master.slaves[4], EC_STATE_PREOP)) - { - fprintf(stderr, "ERROR: Could not set state!\n"); - return -1; - } - - //---------- - -#if 1 - printf("Setting FMMU 0 of device 1.\n"); - - data[0] = 0x00; // Logical start address [4] - data[1] = 0x00; - data[2] = 0x00; - data[3] = 0x00; - data[4] = 0x04; // Length [2] - data[5] = 0x00; - data[6] = 0x00; // Start bit - data[7] = 0x07; // End bit - data[8] = 0x00; // Physical start address [2] - data[9] = 0x10; - data[10] = 0x00; // Physical start bit - data[11] = 0x02; // Read/write enable - data[12] = 0x01; // channel enable [2] - data[13] = 0x00; - data[14] = 0x00; // Reserved [2] - data[15] = 0x00; - cmd = EtherCAT_write(&master, 0x0001, 0x0600, 16, data); - EtherCAT_send_receive(&master); - - if (cmd->working_counter != 1) - { - fprintf(stderr, "ERROR: Not all slaves responded (%i of 1)!\n", - cmd->working_counter); - return -1; - } - - EtherCAT_remove_command(&master, cmd); -#endif - - //---------- - -#if 1 - printf("Setting FMMU 0 of device 4.\n"); - - data[0] = 0x04; // Logical start address [4] - data[1] = 0x00; - data[2] = 0x00; - data[3] = 0x00; - data[4] = 0x01; // Length [2] - data[5] = 0x00; - data[6] = 0x00; // Start bit - data[7] = 0x07; // End bit - data[8] = 0x00; // Physical start address [2] - data[9] = 0x0F; - data[10] = 0x00; // Physical start bit - data[11] = 0x02; // Read/write enable - data[12] = 0x01; // channel enable [2] - data[13] = 0x00; - data[14] = 0x00; // Reserved [2] - data[15] = 0x00; - cmd = EtherCAT_write(&master, 0x0004, 0x0600, 16, data); - EtherCAT_send_receive(&master); - - if (cmd->working_counter != 1) - { - fprintf(stderr, "ERROR: Not all slaves responded (%i of 1)!\n", - cmd->working_counter); - return -1; - } - - EtherCAT_remove_command(&master, cmd); -#endif - - //---------- - - printf("Setting Sync manager 2 of device 1.\n"); - - data[0] = 0x00; - data[1] = 0x10; - data[2] = 0x04; - data[3] = 0x00; - data[4] = 0x24; - data[5] = 0x00; - data[6] = 0x01; - data[7] = 0x00; - cmd = EtherCAT_write(&master, 0x0001, 0x0810, 8, data); - EtherCAT_send_receive(&master); - - if (cmd->working_counter != 1) - { - fprintf(stderr, "ERROR: Not all slaves responded (%i of 1)!\n", cmd->working_counter); - return -1; - } - - EtherCAT_remove_command(&master, cmd); - - //---------- - - printf("Setting Sync manager 0 for device 4.\n"); - - data[0] = 0x00; - data[1] = 0x0F; - data[2] = 0x01; - data[3] = 0x00; - data[4] = 0x46; // 46 - data[5] = 0x00; - data[6] = 0x01; - data[7] = 0x00; - cmd = EtherCAT_write(&master, 0x0004, 0x0800, 8, data); - - EtherCAT_send_receive(&master); - - if (cmd->working_counter != 1) - { - fprintf(stderr, "ERROR: Not all slaves responded!\n"); - - return -1; - } - - EtherCAT_remove_command(&master, cmd); - - //---------- - - printf("Setting SAVEOP state for bus coupler.\n"); - - if (EtherCAT_state_change(&master, 0x0000, EC_STATE_SAVEOP) != 0) - { - fprintf(stderr, "ERROR: Could not set state!\n"); - return -1; - } - - //---------- - - printf("Setting SAVEOP state for device 1.\n"); - - if (EtherCAT_state_change(&master, &master.slaves[1], EC_STATE_SAVEOP) != 0) - { - fprintf(stderr, "ERROR: Could not set state!\n"); - return -1; - } - - //---------- - - printf("Setting SAVEOP state for device 4.\n"); - - if (EtherCAT_state_change(&master, &master.slaves[4], EC_STATE_SAVEOP) != 0) - { - fprintf(stderr, "ERROR: Could not set state!\n"); - return -1; - } - - //---------- - - printf("Setting OP state for bus coupler.\n"); - - if (EtherCAT_state_change(&master, &master.slaves[0], EC_STATE_OP) != 0) - { - fprintf(stderr, "ERROR: Could not set state!\n"); - return -1; - } - - //---------- - - printf("Setting OP state for device 1.\n"); - - if (EtherCAT_state_change(&master, &master.slaves[1], EC_STATE_OP) != 0) - { - fprintf(stderr, "ERROR: Could not set state!\n"); - return -1; - } - - //---------- - - printf("Setting OP state for device 4.\n"); - - if (EtherCAT_state_change(&master, &master.slaves[4], EC_STATE_OP) != 0) - { - fprintf(stderr, "ERROR: Could not set state!\n"); - return -1; - } - - //---------- - - word = 0; - - printf("Starting thread...\n"); - - if (EtherCAT_start(&master, 5, write_data, NULL, 10000) != 0) - { - return -1; - } - - continue_running = 1; - - while (continue_running) - { - usleep(200000); - - word += 1000; - word = word & 0x7FFF; - } - - //---------- - - printf("Stopping master thread...\n"); - EtherCAT_stop(&master); - - EtherCAT_master_clear(&master); - - printf("Finished.\n"); - - return 0; -} - -//--------------------------------------------------------------- - -void write_data(unsigned char *data) -{ - data[0] = word & 0xFF; - data[1] = (word & 0xFF00) >> 8; - data[2] = word & 0xFF; - data[3] = (word & 0xFF00) >> 8; - - data[4] = 0x01; -} - -//--------------------------------------------------------------- - -void signal_handler(int signum) -{ - if (signum == SIGINT || signum == SIGTERM) - { - continue_running = 0; - } -} - -//--------------------------------------------------------------- diff -r d7ef8607e06f -r fd8b9ad48f88 user/main_gui.cpp --- a/user/main_gui.cpp Fri Dec 16 15:34:04 2005 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,287 +0,0 @@ -//--------------------------------------------------------------- -// -// m a i n _ g u i . c p p -// -// $LastChangedDate$ -// $Author$ -// -//--------------------------------------------------------------- - -#include -#include // memset() -#include // usleep() -#include - -#include -#include -#include -#include -#include -#include -using namespace fltk; - -#include "ec_globals.h" -#include "ec_master.h" - -#define SLIDER_UPDATE_CYCLE 0.02 -#define VALUES_UPDATE_CYCLE 0.50 - -//--------------------------------------------------------------- - -unsigned short int write_value; -signed short int read_value; -unsigned char dig_value; - -void write_data(unsigned char *); -void read_data(unsigned char *); - -void slider_write_callback(Widget *, void *); -void slider_read_timeout(void *); -void values_timeout(void *); - -Window *window; -Slider *slider_read, *slider_write; -ValueOutput *output_cycle, *output_jitter, *output_work, *output_busy, *output_bus; -CheckButton *check1, *check2, *check3, *check4; -EtherCAT_master_t master; - -double max_cycle, max_jitter, max_work, max_busy, max_bus; - -//--------------------------------------------------------------- - -#define SLAVE_COUNT 7 - -EtherCAT_slave_t slaves[SLAVE_COUNT] = -{ - ECAT_INIT_SLAVE(Beckhoff_EK1100), - ECAT_INIT_SLAVE(Beckhoff_EL4102), - ECAT_INIT_SLAVE(Beckhoff_EL3162), - ECAT_INIT_SLAVE(Beckhoff_EL1014), - ECAT_INIT_SLAVE(Beckhoff_EL5001), - ECAT_INIT_SLAVE(Beckhoff_EL2004), - ECAT_INIT_SLAVE(Beckhoff_EL3102) -}; - -//--------------------------------------------------------------- - -int main(int argc, char **argv) -{ - //unsigned int i; - EtherCAT_slave_t *buskoppler, *input, *output, *dig_in, *dig_out; - struct sched_param sched; - - printf("CatEther-Testprogramm.\n\n"); - - //---------- - -#if 1 - printf("Setting highest task priority...\n"); - - sched.sched_priority = sched_get_priority_max(SCHED_RR); - if (sched_setscheduler(0, SCHED_RR, &sched) == -1) - { - fprintf(stderr, "ERROR: Could not set priority: %s\n", strerror(errno)); - return -1; - } -#endif - - //---------- - - printf("Initializing master...\n"); - EtherCAT_master_init(&master, "eth1"); - - printf("Checking slaves...\n"); - if (EtherCAT_check_slaves(&master, slaves, SLAVE_COUNT) != 0) - { - fprintf(stderr, "ERROR while searching for slaves!\n"); - return -1; - } - - //---------- - - // Check for slaves - - buskoppler = &slaves[0]; - output = &slaves[1]; - dig_in = &slaves[3]; - dig_out = &slaves[5]; - input = &slaves[6]; - - // Set Mapping addresses - - output->logical_address0 = 0x00000000; - input->logical_address0 = 0x00000004; - dig_in->logical_address0 = 0x0000000F; - dig_out->logical_address0 = 0x0000000E; - - //---------- - - printf("Init output slave...\n"); - - if (EtherCAT_activate_slave(&master, output) != 0) - { - fprintf(stderr, "ERROR: Could not init slave!\n"); - return -1; - } - - printf("Init input slave...\n"); - - if (EtherCAT_activate_slave(&master, input) != 0) - { - fprintf(stderr, "ERROR: Could not init slave!\n"); - return -1; - } - - printf("Init digital input slave...\n"); - - if (EtherCAT_activate_slave(&master, dig_in) != 0) - { - fprintf(stderr, "ERROR: Could not init slave!\n"); - return -1; - } - - printf("Init digital output slave...\n"); - - if (EtherCAT_activate_slave(&master, dig_out) != 0) - { - fprintf(stderr, "ERROR: Could not init slave!\n"); - return -1; - } - - //---------- - - printf("Starting FLTK window...\n"); - - window = new Window(300, 300); - window->begin(); - - slider_read = new FillSlider(50, 10, 40, 280); - slider_read->set_vertical(); - slider_read->buttoncolor(BLUE); - - slider_read->deactivate(); - - slider_write = new Slider(110, 10, 40, 280); - slider_write->set_vertical(); - slider_write->callback(slider_write_callback, NULL); - - output_cycle = new ValueOutput(200, 50, 90, 25, "Cycle time [µs]"); - output_cycle->align(ALIGN_LEFT | ALIGN_TOP); - - output_jitter = new ValueOutput(200, 90, 90, 25, "Jitter [%]"); - output_jitter->align(ALIGN_LEFT | ALIGN_TOP); - - output_work = new ValueOutput(200, 130, 90, 25, "Work time [µs]"); - output_work->align(ALIGN_LEFT | ALIGN_TOP); - - output_busy = new ValueOutput(200, 170, 90, 25, "Busy rate [%]"); - output_busy->align(ALIGN_LEFT | ALIGN_TOP); - - output_bus = new ValueOutput(200, 210, 90, 25, "Bus time [µs]"); - output_bus->align(ALIGN_LEFT | ALIGN_TOP); - - check1 = new CheckButton(200, 240, 30, 25, "1"); - check2 = new CheckButton(250, 240, 30, 25, "2"); - check3 = new CheckButton(200, 270, 30, 25, "3"); - check4 = new CheckButton(250, 270, 30, 25, "4"); - - // output_cycle = new Output(200, 35, 90, 25); - - window->end(); - window->show(); - - add_timeout(SLIDER_UPDATE_CYCLE, slider_read_timeout, NULL); - add_timeout(VALUES_UPDATE_CYCLE, values_timeout, NULL); - - printf("Starting thread...\n"); - - if (EtherCAT_start(&master, 20, write_data, read_data, 10000) != 0) - { - return -1; - } - - run(); // Start FLTK loop - - remove_timeout(slider_read_timeout, NULL); - remove_timeout(values_timeout, NULL); - - printf("Stopping master thread...\n"); - EtherCAT_stop(&master); - - printf("Deactivating slaves...\n"); - - EtherCAT_deactivate_slave(&master, dig_out); - EtherCAT_deactivate_slave(&master, dig_in); - EtherCAT_deactivate_slave(&master, input); - EtherCAT_deactivate_slave(&master, output); - EtherCAT_deactivate_slave(&master, buskoppler); - - EtherCAT_master_clear(&master); - - printf("Finished.\n"); - - return 0; -} - -//--------------------------------------------------------------- - -void write_data(unsigned char *data) -{ - data[0] = write_value & 0xFF; - data[1] = (write_value & 0xFF00) >> 8; - - data[14] = (write_value * 16 / 32767) & 0x0F; -} - -//--------------------------------------------------------------- - -void read_data(unsigned char *data) -{ - read_value = data[5] | data[6] << 8; - dig_value = data[15]; -} - -//--------------------------------------------------------------- - -void slider_read_timeout(void *data) -{ - slider_read->value((double) read_value / 65536 + 0.5); - slider_read->redraw(); - - check1->value(dig_value & 1); - check2->value(dig_value & 2); - check3->value(dig_value & 4); - check4->value(dig_value & 8); - - if (max_cycle < master.last_cycle_time) max_cycle = master.last_cycle_time; - if (max_jitter < master.last_jitter) max_jitter = master.last_jitter; - if (max_work < master.last_cycle_work_time) max_work = master.last_cycle_work_time; - if (max_busy < master.last_cycle_busy_rate) max_busy = master.last_cycle_busy_rate; - if (max_bus < master.bus_time) max_bus = master.bus_time; - - repeat_timeout(SLIDER_UPDATE_CYCLE, slider_read_timeout, NULL); -} - -//--------------------------------------------------------------- - -void values_timeout(void *data) -{ - output_cycle->value(max_cycle * 1000000.0); - output_jitter->value(max_jitter); - output_work->value(max_work * 1000000.0); - output_busy->value(max_busy); - output_bus->value(max_bus * 1000000.0); - - max_cycle = max_jitter = max_work = max_busy = max_bus = 0.0; - - repeat_timeout(VALUES_UPDATE_CYCLE, values_timeout, NULL); -} - -//--------------------------------------------------------------- - -void slider_write_callback(Widget *sender, void *data) -{ - write_value = (short unsigned int) (32767 * slider_write->value() + 0.5); -} - -//---------------------------------------------------------------