examples/xenomai/main.c
changeset 2068 3001f6523e63
parent 2056 a92e8f119723
child 2589 2b9c78543663
equal deleted inserted replaced
2067:19732da2cf86 2068:3001f6523e63
       
     1 /******************************************************************************
       
     2  *
       
     3  *  $Id$
       
     4  *
       
     5  *  main.c	        Copyright (C) 2009-2010  Moehwald GmbH B.Benner
       
     6  *                                2011       IgH Andreas Stewering-Bone
       
     7  *
       
     8  *  This file is part of ethercatrtdm interface to IgH EtherCAT master 
       
     9  *  
       
    10  *  The Moehwald ethercatrtdm interface is free software; you can
       
    11  *  redistribute it and/or modify it under the terms of the GNU Lesser General
       
    12  *  Public License as published by the Free Software Foundation; version 2.1
       
    13  *  of the License.
       
    14  *
       
    15  *  The IgH EtherCAT Master is free software; you can redistribute it and/or
       
    16  *  modify it under the terms of the GNU General Public License version 2, as
       
    17  *  published by the Free Software Foundation.
       
    18  *
       
    19  *  The IgH EtherCAT master userspace library is distributed in the hope that
       
    20  *  it will be useful, but WITHOUT ANY WARRANTY; without even the implied
       
    21  *  warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
       
    22  *  GNU Lesser General Public License for more details.
       
    23  *
       
    24  *  You should have received a copy of the GNU Lesser General Public License
       
    25  *  along with the IgH EtherCAT master userspace library. If not, see
       
    26  *  <http://www.gnu.org/licenses/>.
       
    27  *  
       
    28  *  The license mentioned above concerns the source code only. Using the
       
    29  *  EtherCAT technology and brand is only permitted in compliance with the
       
    30  *  industrial property and similar rights of Beckhoff Automation GmbH.
       
    31  *
       
    32  *****************************************************************************/
       
    33 
       
    34 #include <errno.h>
       
    35 #include <signal.h>
       
    36 #include <stdio.h>
       
    37 #include <string.h>
       
    38 #include <sys/resource.h>
       
    39 #include <sys/time.h>
       
    40 #include <sys/types.h>
       
    41 #include <unistd.h>
       
    42 #include <sys/mman.h>
       
    43 #include <rtdm/rtdm.h>
       
    44 #include <native/task.h>
       
    45 #include <native/sem.h>
       
    46 #include <native/mutex.h>
       
    47 #include <native/timer.h>
       
    48 #include <rtdk.h>
       
    49 #include <pthread.h>
       
    50 
       
    51 /****************************************************************************/
       
    52 
       
    53 #include "../../include/ecrt.h"
       
    54 #include "../../include/ec_rtdm.h"
       
    55 
       
    56 
       
    57 RT_TASK my_task;
       
    58 
       
    59 int rt_fd = -1;
       
    60 int run=0;
       
    61 
       
    62 unsigned int sync_ref_counter = 0;
       
    63 
       
    64 CstructMstrAttach MstrAttach;
       
    65 
       
    66 /****************************************************************************/
       
    67 
       
    68 
       
    69 
       
    70 // Optional features
       
    71 #define CONFIGURE_PDOS  1
       
    72 //#define SDO_ACCESS      1
       
    73 
       
    74 /****************************************************************************/
       
    75 
       
    76 // EtherCAT
       
    77 static ec_master_t *master = NULL;
       
    78 static ec_master_state_t master_state = {};
       
    79 
       
    80 static ec_domain_t *domain1 = NULL;
       
    81 static ec_domain_state_t domain1_state = {};
       
    82 
       
    83 //static ec_slave_config_t *sc_ana_in = NULL;
       
    84 //static ec_slave_config_state_t sc_ana_in_state = {};
       
    85 
       
    86 // Timer
       
    87 static unsigned int sig_alarms = 0;
       
    88 //static unsigned int user_alarms = 0;
       
    89 
       
    90 /****************************************************************************/
       
    91 static uint8_t *domain1_pd = NULL;
       
    92 
       
    93 // process data
       
    94 
       
    95 #define BusCoupler01_Pos    0, 0
       
    96 #define DigOutSlave01_Pos   0, 1
       
    97 #define DigOutSlave02_Pos   0, 2
       
    98 #define DigInSlave01_Pos    0, 3
       
    99 #define AnaOutSlave01_Pos   0, 4
       
   100 #define AnaInSlave01_Pos    0, 5
       
   101 #define BusCoupler02_Pos    0, 6
       
   102 #define AnaInSlave02_Pos    0, 7
       
   103 #define DPSlave01_Pos       0, 8
       
   104 
       
   105 
       
   106 #define Beckhoff_EK1100 0x00000002, 0x044c2c52
       
   107 #define Beckhoff_EL1014 0x00000002, 0x03f63052
       
   108 #define Beckhoff_EL2004 0x00000002, 0x07d43052
       
   109 #define Beckhoff_EL4132 0x00000002, 0x10243052
       
   110 #define Beckhoff_EL3102 0x00000002, 0x0c1e3052
       
   111 #define Beckhoff_EL4102 0x00000002, 0x10063052
       
   112 #define Beckhoff_EL6731 0x00000002, 0x1a4b3052
       
   113 #define Beckhoff_EL6600 0x00000002, 0x19c93052
       
   114 #define Beckhoff_EL3602 0x00000002, 0x0e123052
       
   115 #define Beckhoff_EL5151 0x00000002, 0x141f3052
       
   116 
       
   117 
       
   118 // offsets for PDO entries
       
   119 static unsigned int off_dig_out0      = 0;
       
   120 static unsigned int off_dig_out1      = 0;
       
   121 static unsigned int off_dig_out2      = 0;
       
   122 static unsigned int off_dig_out3      = 0;
       
   123 static unsigned int off_dig_out4      = 0;
       
   124 static unsigned int off_dig_out5      = 0;
       
   125 static unsigned int off_dig_out6      = 0;
       
   126 static unsigned int off_dig_out7      = 0;
       
   127 static unsigned int off_dig_in0       = 0;
       
   128 static unsigned int off_dig_in1       = 0;
       
   129 static unsigned int off_dig_in2       = 0;
       
   130 static unsigned int off_dig_in3       = 0;
       
   131 static unsigned int off_ana_out0      = 0;
       
   132 static unsigned int off_ana_out1      = 0;
       
   133 static unsigned int off_ana_in0_status = 0;
       
   134 static unsigned int off_ana_in0_value  = 0;
       
   135 static unsigned int off_ana_in1_status = 0;
       
   136 static unsigned int off_ana_in1_value  = 0;
       
   137 static unsigned int off_ana_in2_status = 0;
       
   138 static unsigned int off_ana_in2_value  = 0;
       
   139 static unsigned int off_ana_in3_status = 0;
       
   140 static unsigned int off_ana_in3_value  = 0;
       
   141 
       
   142 //static unsigned int off_dp_slave;
       
   143 
       
   144 // process data
       
   145 unsigned int bit_position0=0; /* Pointer to a variable to store a bit */
       
   146 unsigned int bit_position1=0; /* Pointer to a variable to store a bit */
       
   147 unsigned int bit_position2=0; /* Pointer to a variable to store a bit */
       
   148 unsigned int bit_position3=0; /* Pointer to a variable to store a bit */
       
   149 
       
   150 const static ec_pdo_entry_reg_t domain1_regs[] = {
       
   151    {DigOutSlave01_Pos, Beckhoff_EL2004, 0x7000, 0x01, &off_dig_out0, &bit_position0},
       
   152    {DigOutSlave01_Pos, Beckhoff_EL2004, 0x7010, 0x01, &off_dig_out1, &bit_position1},
       
   153    {DigOutSlave01_Pos, Beckhoff_EL2004, 0x7020, 0x01, &off_dig_out2, &bit_position2},
       
   154    {DigOutSlave01_Pos, Beckhoff_EL2004, 0x7030, 0x01, &off_dig_out3, &bit_position3},
       
   155    {DigInSlave01_Pos,  Beckhoff_EL1014, 0x6000, 0x01, &off_dig_in0},
       
   156    {AnaOutSlave01_Pos, Beckhoff_EL4132, 0x3001, 0x01, &off_ana_out0},
       
   157    {AnaOutSlave01_Pos, Beckhoff_EL4132, 0x3002, 0x01, &off_ana_out1},
       
   158    {AnaInSlave01_Pos,  Beckhoff_EL3102, 0x3101, 0x01, &off_ana_in0_status},
       
   159    {AnaInSlave01_Pos,  Beckhoff_EL3102, 0x3101, 0x02, &off_ana_in0_value},
       
   160    {AnaInSlave01_Pos,  Beckhoff_EL3102, 0x3102, 0x01, &off_ana_in1_status},
       
   161    {AnaInSlave01_Pos,  Beckhoff_EL3102, 0x3102, 0x02, &off_ana_in1_value},
       
   162    {}
       
   163 };
       
   164 
       
   165 char rt_dev_file[64];
       
   166 static unsigned int counter = 0;
       
   167 static unsigned int blink = 0;
       
   168 
       
   169 static ec_slave_config_t *sc_dig_out_01 = NULL;
       
   170 
       
   171 static ec_slave_config_t *sc_dig_out_02 = NULL;
       
   172 
       
   173 static ec_slave_config_t *sc_dig_in_01 = NULL;
       
   174 
       
   175 static ec_slave_config_t *sc_ana_out_01 = NULL;
       
   176 
       
   177 static ec_slave_config_t *sc_ana_in_01 = NULL;
       
   178 
       
   179 static ec_slave_config_t *sc_dpslv_01 = NULL;
       
   180 
       
   181 static ec_slave_config_t *sc_ana_in_02 = NULL;
       
   182 
       
   183 /*****************************************************************************/
       
   184 
       
   185 /* Slave 1, "EL2004"
       
   186  * Vendor ID:       0x00000002
       
   187  * Product code:    0x07d43052
       
   188  * Revision number: 0x00100000
       
   189  */
       
   190 
       
   191 ec_pdo_entry_info_t slave_1_pdo_entries[] = {
       
   192    {0x7000, 0x01, 1}, /* Output */
       
   193    {0x7010, 0x01, 1}, /* Output */
       
   194    {0x7020, 0x01, 1}, /* Output */
       
   195    {0x7030, 0x01, 1}, /* Output */
       
   196 };
       
   197 
       
   198 ec_pdo_info_t slave_1_pdos[] = {
       
   199    {0x1600, 1, slave_1_pdo_entries + 0}, /* Channel 1 */
       
   200    {0x1601, 1, slave_1_pdo_entries + 1}, /* Channel 2 */
       
   201    {0x1602, 1, slave_1_pdo_entries + 2}, /* Channel 3 */
       
   202    {0x1603, 1, slave_1_pdo_entries + 3}, /* Channel 4 */
       
   203 };
       
   204 
       
   205 ec_sync_info_t slave_1_syncs[] = {
       
   206    {0, EC_DIR_OUTPUT, 4, slave_1_pdos + 0, EC_WD_ENABLE},
       
   207    {0xff}
       
   208 };
       
   209 
       
   210 /* Slave 2, "EL2004"
       
   211  * Vendor ID:       0x00000002
       
   212  * Product code:    0x07d43052
       
   213  * Revision number: 0x00100000
       
   214  */
       
   215 
       
   216 ec_pdo_entry_info_t slave_2_pdo_entries[] = {
       
   217    {0x7000, 0x01, 1}, /* Output */
       
   218    {0x7010, 0x01, 1}, /* Output */
       
   219    {0x7020, 0x01, 1}, /* Output */
       
   220    {0x7030, 0x01, 1}, /* Output */
       
   221 };
       
   222 
       
   223 ec_pdo_info_t slave_2_pdos[] = {
       
   224    {0x1600, 1, slave_2_pdo_entries + 0}, /* Channel 1 */
       
   225    {0x1601, 1, slave_2_pdo_entries + 1}, /* Channel 2 */
       
   226    {0x1602, 1, slave_2_pdo_entries + 2}, /* Channel 3 */
       
   227    {0x1603, 1, slave_2_pdo_entries + 3}, /* Channel 4 */
       
   228 };
       
   229 
       
   230 ec_sync_info_t slave_2_syncs[] = {
       
   231    {0, EC_DIR_OUTPUT, 4, slave_2_pdos + 0, EC_WD_ENABLE},
       
   232    {0xff}
       
   233 };
       
   234 
       
   235 /* Slave 3, "EL1014"
       
   236  * Vendor ID:       0x00000002
       
   237  * Product code:    0x03f63052
       
   238  * Revision number: 0x00100000
       
   239  */
       
   240 
       
   241 ec_pdo_entry_info_t slave_3_pdo_entries[] = {
       
   242    {0x6000, 0x01, 1}, /* Input */
       
   243    {0x6010, 0x01, 1}, /* Input */
       
   244    {0x6020, 0x01, 1}, /* Input */
       
   245    {0x6030, 0x01, 1}, /* Input */
       
   246 };
       
   247 
       
   248 ec_pdo_info_t slave_3_pdos[] = {
       
   249    {0x1a00, 1, slave_3_pdo_entries + 0}, /* Channel 1 */
       
   250    {0x1a01, 1, slave_3_pdo_entries + 1}, /* Channel 2 */
       
   251    {0x1a02, 1, slave_3_pdo_entries + 2}, /* Channel 3 */
       
   252    {0x1a03, 1, slave_3_pdo_entries + 3}, /* Channel 4 */
       
   253 };
       
   254 
       
   255 ec_sync_info_t slave_3_syncs[] = {
       
   256    {0, EC_DIR_INPUT, 4, slave_3_pdos + 0, EC_WD_DISABLE},
       
   257    {0xff}
       
   258 };
       
   259 
       
   260 /* Slave 4, "EL4132"
       
   261  * Vendor ID:       0x00000002
       
   262  * Product code:    0x10243052
       
   263  * Revision number: 0x03f90000
       
   264  */
       
   265 
       
   266 ec_pdo_entry_info_t slave_4_pdo_entries[] = {
       
   267    {0x3001, 0x01, 16}, /* Output */
       
   268    {0x3002, 0x01, 16}, /* Output */
       
   269 };
       
   270 
       
   271 ec_pdo_info_t slave_4_pdos[] = {
       
   272    {0x1600, 1, slave_4_pdo_entries + 0}, /* RxPDO 01 mapping */
       
   273    {0x1601, 1, slave_4_pdo_entries + 1}, /* RxPDO 02 mapping */
       
   274 };
       
   275 
       
   276 ec_sync_info_t slave_4_syncs[] = {
       
   277    {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
       
   278    {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
       
   279    {2, EC_DIR_OUTPUT, 2, slave_4_pdos + 0, EC_WD_DISABLE},
       
   280    {3, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
       
   281    {0xff}
       
   282 };
       
   283 
       
   284 /* Slave 5, "EL3102"
       
   285  * Vendor ID:       0x00000002
       
   286  * Product code:    0x0c1e3052
       
   287  * Revision number: 0x00000000
       
   288  */
       
   289 
       
   290 ec_pdo_entry_info_t slave_5_pdo_entries[] = {
       
   291    {0x3101, 0x01, 8}, /* Status */
       
   292    {0x3101, 0x02, 16}, /* Value */
       
   293    {0x3102, 0x01, 8}, /* Status */
       
   294    {0x3102, 0x02, 16}, /* Value */
       
   295 };
       
   296 
       
   297 ec_pdo_info_t slave_5_pdos[] = {
       
   298    {0x1a00, 2, slave_5_pdo_entries + 0}, /* TxPDO 001 mapping */
       
   299    {0x1a01, 2, slave_5_pdo_entries + 2}, /* TxPDO 002 mapping */
       
   300 };
       
   301 
       
   302 ec_sync_info_t slave_5_syncs[] = {
       
   303    {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
       
   304    {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
       
   305    {2, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
       
   306    {3, EC_DIR_INPUT, 2, slave_5_pdos + 0, EC_WD_DISABLE},
       
   307    {0xff}
       
   308 };
       
   309 
       
   310 /* Slave 6, "EL6731-0010"
       
   311  * Vendor ID:       0x00000002
       
   312  * Product code:    0x1a4b3052
       
   313  * Revision number: 0x0011000a
       
   314  */
       
   315 
       
   316 ec_sync_info_t slave_6_syncs[] = {
       
   317    {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
       
   318    {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
       
   319    {2, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
       
   320    {3, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
       
   321 };
       
   322 
       
   323 
       
   324 /* Slave 7, "EL6601"
       
   325  * Vendor ID:       0x00000002
       
   326  * Product code:    0x19c93052
       
   327  * Revision number: 0x00110000
       
   328  */
       
   329 /*
       
   330 ec_sync_info_t slave_7_syncs[] = {
       
   331    {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
       
   332    {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
       
   333    {2, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
       
   334    {3, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
       
   335    {0xff}
       
   336 };
       
   337 */
       
   338 
       
   339 /* Master 0, Slave 7, "EL3602"
       
   340  * Vendor ID:       0x00000002
       
   341  * Product code:    0x0e123052
       
   342  * Revision number: 0x00100000
       
   343  */
       
   344 ec_pdo_entry_info_t slave_7_pdo_entries[] = {
       
   345    {0x6000, 0x01, 1}, /* Underrange */
       
   346    {0x6000, 0x02, 1}, /* Overrange */
       
   347    {0x6000, 0x03, 2}, /* Limit 1 */
       
   348    {0x6000, 0x05, 2}, /* Limit 2 */
       
   349    {0x6000, 0x07, 1}, /* Error */
       
   350    {0x0000, 0x00, 7}, /* Gap */
       
   351    {0x1800, 0x07, 1},
       
   352    {0x1800, 0x09, 1},
       
   353    {0x6000, 0x11, 32}, /* Value */
       
   354    {0x6010, 0x01, 1}, /* Underrange */
       
   355    {0x6010, 0x02, 1}, /* Overrange */
       
   356    {0x6010, 0x03, 2}, /* Limit 1 */
       
   357    {0x6010, 0x05, 2}, /* Limit 2 */
       
   358    {0x6010, 0x07, 1}, /* Error */
       
   359    {0x0000, 0x00, 7}, /* Gap */
       
   360    {0x1801, 0x07, 1},
       
   361    {0x1801, 0x09, 1},
       
   362    {0x6010, 0x11, 32}, /* Value */
       
   363 };
       
   364 
       
   365 ec_pdo_info_t slave_7_pdos[] = {
       
   366    {0x1a00, 9, slave_7_pdo_entries + 0}, /* AI TxPDO-Map Inputs Ch.1 */
       
   367    {0x1a01, 9, slave_7_pdo_entries + 9}, /* AI TxPDO-Map Inputs Ch.2 */
       
   368 };
       
   369 
       
   370 ec_sync_info_t slave_7_syncs[] = {
       
   371    {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
       
   372    {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
       
   373    {2, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
       
   374    {3, EC_DIR_INPUT, 2, slave_7_pdos + 0, EC_WD_DISABLE},
       
   375    {0xff}
       
   376 };
       
   377 
       
   378 /* Master 0, Slave 8, "EL5151"
       
   379  * Vendor ID:       0x00000002
       
   380  * Product code:    0x141f3052
       
   381  * Revision number: 0x00130000
       
   382  */
       
   383 
       
   384 ec_pdo_entry_info_t slave_8_pdo_entries[] = {
       
   385    {0x6000, 0x01, 1},
       
   386    {0x6000, 0x02, 1},
       
   387    {0x6000, 0x03, 1},
       
   388    {0x0000, 0x00, 4}, /* Gap */
       
   389    {0x6000, 0x08, 1},
       
   390    {0x6000, 0x09, 1},
       
   391    {0x6000, 0x0a, 1},
       
   392    {0x6000, 0x0b, 1},
       
   393    {0x0000, 0x00, 1}, /* Gap */
       
   394    {0x6000, 0x0d, 1},
       
   395    {0x1c32, 0x20, 1},
       
   396    {0x0000, 0x00, 1}, /* Gap */
       
   397    {0x1800, 0x09, 1},
       
   398    {0x6000, 0x11, 32},
       
   399    {0x6000, 0x12, 32},
       
   400    {0x6000, 0x14, 32},
       
   401 };
       
   402 
       
   403 ec_pdo_info_t slave_8_pdos[] = {
       
   404    {0x0000, 0, NULL},
       
   405    {0x1a00, 15, slave_8_pdo_entries + 0},
       
   406    {0x1a02, 1, slave_8_pdo_entries + 15},
       
   407 };
       
   408 
       
   409 ec_sync_info_t slave_8_syncs[] = {
       
   410    {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
       
   411    {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
       
   412    {2, EC_DIR_OUTPUT, 1, slave_8_pdos + 0, EC_WD_DISABLE},
       
   413    {3, EC_DIR_INPUT, 2, slave_8_pdos + 1, EC_WD_DISABLE},
       
   414    {0xff}
       
   415 };
       
   416 
       
   417 
       
   418 /*****************************************************************************/
       
   419 
       
   420 #if SDO_ACCESS
       
   421 static ec_sdo_request_t *sdo;
       
   422 uint8_t *sdo_adr = NULL;
       
   423 #endif
       
   424 
       
   425 
       
   426 
       
   427 void rt_check_domain_state(void)
       
   428 {
       
   429     ec_domain_state_t ds;
       
   430 
       
   431     if (rt_fd>=0)
       
   432       {
       
   433           ecrt_rtdm_domain_state(rt_fd,&ds);  
       
   434       }
       
   435 
       
   436     if (ds.working_counter != domain1_state.working_counter)
       
   437      {
       
   438         rt_printf("Domain1: WC %u.\n", ds.working_counter);
       
   439      }
       
   440     if (ds.wc_state != domain1_state.wc_state)
       
   441      {
       
   442     	rt_printf("Domain1: State %u.\n", ds.wc_state);
       
   443      }
       
   444 
       
   445     domain1_state = ds;
       
   446 }
       
   447 
       
   448 void rt_check_master_state(void)
       
   449 {
       
   450     ec_master_state_t ms;
       
   451 
       
   452     if (rt_fd>=0)
       
   453       {
       
   454           ecrt_rtdm_master_state(rt_fd,&ms);
       
   455       }
       
   456 
       
   457     if (ms.slaves_responding != master_state.slaves_responding)
       
   458     {
       
   459         rt_printf("%u slave(s).\n", ms.slaves_responding);
       
   460     }
       
   461     if (ms.al_states != master_state.al_states)
       
   462     {
       
   463         rt_printf("AL states: 0x%02X.\n", ms.al_states);
       
   464     }
       
   465     if (ms.link_up != master_state.link_up)
       
   466     {
       
   467         rt_printf("Link is %s.\n", ms.link_up ? "up" : "down");
       
   468     }
       
   469     master_state = ms;
       
   470 }
       
   471 
       
   472 
       
   473 
       
   474 
       
   475 void rt_sync()
       
   476 {
       
   477   RTIME now;
       
   478   now = rt_timer_read();
       
   479 
       
   480 
       
   481   if (rt_fd>=0)
       
   482   {
       
   483       ecrt_rtdm_master_application_time(rt_fd, &now);
       
   484   }
       
   485 
       
   486   if (sync_ref_counter) {
       
   487      sync_ref_counter--;
       
   488   } else {
       
   489      sync_ref_counter = 9;
       
   490      if (rt_fd>=0)
       
   491      {
       
   492          ecrt_rtdm_master_sync_reference_clock(rt_fd);
       
   493      }
       
   494   }
       
   495   if (rt_fd>=0)
       
   496   {
       
   497       ecrt_rtdm_master_sync_slave_clocks(rt_fd) ;
       
   498   }
       
   499 }
       
   500 
       
   501 /*****************************************************************************/
       
   502 
       
   503 #if SDO_ACCESS
       
   504 void read_sdo(void)
       
   505 {
       
   506     switch (ecrt_sdo_request_state(sdo))
       
   507         {
       
   508         case EC_REQUEST_UNUSED: // request was not used yet
       
   509             ecrt_sdo_request_read(sdo); // trigger first read
       
   510             break;
       
   511         case EC_REQUEST_BUSY:
       
   512             fprintf(stderr, "Still busy...\n");
       
   513             break;
       
   514         case EC_REQUEST_SUCCESS:
       
   515             fprintf(stderr, "SDO value: 0x%04X\n",
       
   516                     EC_READ_U16(ecrt_sdo_request_data(sdo)));
       
   517             ecrt_sdo_request_read(sdo); // trigger next read
       
   518             break;
       
   519         case EC_REQUEST_ERROR:
       
   520             fprintf(stderr, "Failed to read SDO!\n");
       
   521             ecrt_sdo_request_read(sdo); // retry reading
       
   522             break;
       
   523         }
       
   524 }
       
   525 
       
   526 void  PrintSDOState(void)
       
   527 {
       
   528     switch (ecrt_sdo_request_state(sdo))
       
   529         {
       
   530         case EC_REQUEST_UNUSED: // request was not used yet
       
   531             fprintf(stderr, "SDO State: EC_REQUEST_UNUSED\n"); // trigger first read
       
   532             break;
       
   533         case EC_REQUEST_BUSY:
       
   534             fprintf(stderr, "SDO State: EC_REQUEST_BUSY\n");
       
   535             break;
       
   536         case EC_REQUEST_SUCCESS:
       
   537             fprintf(stderr, "SDO State: EC_REQUEST_SUCCESS\n");
       
   538             break;
       
   539         case EC_REQUEST_ERROR:
       
   540             fprintf(stderr, "SDO State: EC_REQUEST_ERROR\n");
       
   541             break;
       
   542         default:
       
   543             fprintf(stderr, "SDO State: undefined\n");
       
   544             break;
       
   545   }
       
   546 }
       
   547 #endif
       
   548 
       
   549 
       
   550 static int cyccount=0;
       
   551 
       
   552 /****************************************************************************/
       
   553 
       
   554 void signal_handler(int signum) {
       
   555     switch (signum) {
       
   556         case SIGALRM:
       
   557             sig_alarms++;
       
   558             break;
       
   559     }
       
   560 }
       
   561 
       
   562 
       
   563 /**********************************************************/
       
   564 /*            REAL TIME TASK                              */
       
   565 /**********************************************************/
       
   566 void my_task_proc(void *arg)
       
   567 {
       
   568   int counter = 0;
       
   569   int divcounter = 0;
       
   570   int divider = 10;
       
   571   int ret;
       
   572 
       
   573   RTIME periodns;
       
   574   float period;
       
   575 
       
   576 
       
   577   period=1E-3; //1kHz
       
   578 
       
   579   
       
   580   periodns=(RTIME)(((double)period * 1E9) + 0.4999999);
       
   581   rt_task_set_periodic(NULL, TM_NOW, periodns);
       
   582 
       
   583   run=1;
       
   584 
       
   585   ret = rt_task_set_mode(0, T_PRIMARY, NULL);
       
   586   if (ret) {
       
   587       rt_printf("error while rt_task_set_mode, code %d\n",ret);
       
   588       return;
       
   589   }
       
   590   
       
   591 
       
   592   while (run) {
       
   593       rt_task_wait_period(NULL);
       
   594       
       
   595       counter++;
       
   596       if (counter>600000) {
       
   597           run=0;
       
   598           return;
       
   599       }
       
   600       
       
   601       // receive ethercat
       
   602       ecrt_rtdm_master_recieve(rt_fd);
       
   603       ecrt_rtdm_domain_process(rt_fd);
       
   604 
       
   605       rt_check_domain_state();
       
   606       
       
   607       if (divcounter ==0)
       
   608           {
       
   609               divcounter=divider;
       
   610               rt_check_master_state();
       
   611           }
       
   612       divcounter--;
       
   613       if ((counter % 200)==0)
       
   614           {
       
   615               blink = !blink;
       
   616               
       
   617           }
       
   618       
       
   619 
       
   620       EC_WRITE_U8(domain1_pd + off_dig_out0, blink ? 0x0 : 0x0F);
       
   621       EC_WRITE_U16(domain1_pd + off_ana_out0, blink ? 0x0: 0xfff);
       
   622       
       
   623       //sync DC
       
   624       rt_sync();
       
   625       
       
   626       // send process data
       
   627       ecrt_rtdm_domain_queque(rt_fd);
       
   628       ecrt_rtdm_master_send(rt_fd);
       
   629   }
       
   630   
       
   631 }
       
   632 
       
   633 
       
   634 /**********************************************************/
       
   635 /*            CLEANING UP                                 */
       
   636 /**********************************************************/
       
   637 void cleanup_all(void)
       
   638 {
       
   639     printf("delete my_task\n");
       
   640     rt_task_delete(&my_task);
       
   641     
       
   642     if (rt_fd >= 0) {
       
   643         printf("closing rt device %s\n", &rt_dev_file[0]);
       
   644         rt_dev_close(rt_fd);
       
   645         
       
   646     }
       
   647 }
       
   648 /****************************************************************************/
       
   649 
       
   650 void catch_signal(int sig) {
       
   651     cleanup_all();
       
   652     printf("exit\n");
       
   653     exit(0);
       
   654     return;
       
   655 }
       
   656 
       
   657 
       
   658 /****************************************************************************/
       
   659 
       
   660 int main(int argc, char **argv)
       
   661 {
       
   662     ec_slave_config_t *sc;
       
   663 
       
   664     int rtstatus;
       
   665 
       
   666     mlockall(MCL_CURRENT | MCL_FUTURE);
       
   667 
       
   668     /* Perform auto-init of rt_print buffers if the task doesn't do so */
       
   669     rt_print_auto_init(1);
       
   670 
       
   671     signal(SIGTERM, catch_signal);
       
   672     signal(SIGINT, catch_signal);
       
   673 
       
   674     MstrAttach.masterindex = 0;
       
   675     
       
   676     printf("request master\n");
       
   677     master = ecrt_request_master(MstrAttach.masterindex);
       
   678     if (!master)
       
   679         return -1;
       
   680     
       
   681 
       
   682     domain1 = ecrt_master_create_domain(master);
       
   683     if (!domain1)
       
   684         return -1;
       
   685     
       
   686 
       
   687 #ifdef CONFIGURE_PDOS
       
   688 
       
   689     printf("Configuring PDOs...\n");
       
   690     
       
   691     printf("Get Configuring el2004...\n");
       
   692     sc_dig_out_01 = ecrt_master_slave_config(master, DigOutSlave01_Pos, Beckhoff_EL2004);
       
   693     if (!sc_dig_out_01) {
       
   694         fprintf(stderr, "Failed to get slave configuration.\n");
       
   695         return -1;
       
   696     }
       
   697 
       
   698     printf("Configuring EL2004...\n");
       
   699     if (ecrt_slave_config_pdos(sc_dig_out_01, EC_END, slave_1_syncs))
       
   700         {
       
   701             fprintf(stderr, "Failed to configure PDOs.\n");
       
   702             return -1;
       
   703         }
       
   704     
       
   705     printf("Get Configuring el2004...\n");
       
   706     sc_dig_out_02 = ecrt_master_slave_config(master, DigOutSlave02_Pos, Beckhoff_EL2004);
       
   707     if (!sc_dig_out_02) {
       
   708         fprintf(stderr, "Failed to get slave configuration.\n");
       
   709         return -1;
       
   710     }
       
   711 
       
   712     printf("Configuring EL2004...\n");
       
   713     if (ecrt_slave_config_pdos(sc_dig_out_02, EC_END, slave_2_syncs)) {
       
   714         fprintf(stderr, "Failed to configure PDOs.\n");
       
   715         return -1;
       
   716     }
       
   717     
       
   718     printf("Get Configuring el1014...\n");
       
   719     sc_dig_in_01 = ecrt_master_slave_config(master, DigInSlave01_Pos, Beckhoff_EL1014);
       
   720     if (!sc_dig_in_01) {
       
   721         fprintf(stderr, "Failed to get slave configuration.\n");
       
   722         return -1;
       
   723     }
       
   724     
       
   725     printf("Configuring EL1014...\n");
       
   726     if (ecrt_slave_config_pdos(sc_dig_in_01, EC_END, slave_3_syncs)) {
       
   727         fprintf(stderr, "Failed to configure PDOs.\n");
       
   728         return -1;
       
   729     }
       
   730 
       
   731     printf("Get Configuring EL4132...\n");
       
   732     sc_ana_out_01 = ecrt_master_slave_config(master, AnaOutSlave01_Pos, Beckhoff_EL4132);
       
   733     if (!sc_ana_out_01) {
       
   734         fprintf(stderr, "Failed to get slave configuration.\n");
       
   735         return -1;
       
   736     }
       
   737 
       
   738     printf("Configuring EL4132...\n");
       
   739     if (ecrt_slave_config_pdos(sc_ana_out_01, EC_END, slave_4_syncs)) {
       
   740         fprintf(stderr, "Failed to configure PDOs.\n");
       
   741         return -1;
       
   742     }
       
   743 
       
   744     printf("Get Configuring EL3102...\n");
       
   745     sc_ana_in_01 = ecrt_master_slave_config(master, AnaInSlave01_Pos, Beckhoff_EL3102);
       
   746     if (!sc_ana_in_01) {
       
   747         fprintf(stderr, "Failed to get slave configuration.\n");
       
   748         return -1;
       
   749     }
       
   750 
       
   751     printf("Configuring EL3102...\n");
       
   752     if (ecrt_slave_config_pdos(sc_ana_in_01, EC_END, slave_5_syncs)) {
       
   753         fprintf(stderr, "Failed to configure PDOs.\n");
       
   754         return -1;
       
   755     }
       
   756 
       
   757     printf("Get Configuring EL3602...\n");
       
   758 	sc_ana_in_02 = ecrt_master_slave_config(master, AnaInSlave02_Pos, Beckhoff_EL3602);
       
   759 	if (!sc_ana_in_02) {
       
   760         fprintf(stderr, "Failed to get slave configuration.\n");
       
   761         return -1;
       
   762 	}
       
   763     
       
   764 	// DP Slave Parameter Set
       
   765 
       
   766 /*	printf( "Creating SDO requests...\n");
       
   767 	if (!(sdo = ecrt_slave_config_create_sdo_request(sc_ana_in_02, 0x8000, 0x06, 1))) {
       
   768         fprintf(stderr, "Failed to create SDO request.\n");
       
   769         return -1;
       
   770 	}
       
   771 	ecrt_sdo_request_timeout(sdo, 500); // ms
       
   772 
       
   773 	EC_WRITE_U8(ecrt_sdo_request_data(sdo), 00);
       
   774 	PrintSDOState();
       
   775 	ecrt_sdo_request_write(sdo);
       
   776 	PrintSDOState();
       
   777 
       
   778 */    
       
   779 	printf("Configuring EL3602...\n");
       
   780 	if (ecrt_slave_config_pdos(sc_ana_in_02, EC_END, slave_7_syncs)) {
       
   781         fprintf(stderr, "Failed to configure PDOs.\n");
       
   782         return -1;
       
   783 	}
       
   784     
       
   785 #endif
       
   786     
       
   787     // Create configuration for bus coupler
       
   788     sc = ecrt_master_slave_config(master, BusCoupler01_Pos, Beckhoff_EK1100);
       
   789     if (!sc)
       
   790         return -1;
       
   791     
       
   792 #ifdef CONFIGURE_PDOS
       
   793     if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)) {
       
   794         fprintf(stderr, "PDO entry registration failed!\n");
       
   795         return -1;
       
   796     }
       
   797 #endif
       
   798 
       
   799 
       
   800     printf("Get Configuring EL6731...\n");
       
   801     sc_dpslv_01 = ecrt_master_slave_config(master, DPSlave01_Pos, Beckhoff_EL6731);
       
   802     if (!sc_dpslv_01) {
       
   803         fprintf(stderr, "Failed to get slave configuration.\n");
       
   804         return -1;
       
   805     }
       
   806     
       
   807     printf("Configuring EL6731...\n");
       
   808     if (ecrt_slave_config_pdos(sc_dpslv_01, EC_END, slave_7_syncs))
       
   809         {
       
   810             fprintf(stderr, "Failed to configure PDOs.\n");
       
   811             return -1;
       
   812         }
       
   813     
       
   814 #if SDO_ACCESS
       
   815     
       
   816     
       
   817     // DP Slave Parameter Set
       
   818     fprintf(stderr, "Creating SDO requests...\n");
       
   819     if (!(sdo = ecrt_slave_config_create_sdo_request(sc_dpslv_01, 0x8000, 0, 1))) {
       
   820         fprintf(stderr, "Failed to create SDO request.\n");
       
   821         return -1;
       
   822     }
       
   823     ecrt_sdo_request_timeout(sdo, 500); // ms
       
   824     EC_WRITE_U8(ecrt_sdo_request_data(sdo), 0);
       
   825     PrintSDOState();
       
   826     ecrt_sdo_request_write(sdo);
       
   827     PrintSDOState();
       
   828     
       
   829     // Station Address
       
   830     if (!(sdo = ecrt_slave_config_create_sdo_request(sc_dpslv_01, 0x8000, 1, 2))) {
       
   831         fprintf(stderr, "Failed to create SDO request.\n");
       
   832         return -1;
       
   833     }
       
   834     ecrt_sdo_request_timeout(sdo, 500); // ms
       
   835     EC_WRITE_U16(ecrt_sdo_request_data(sdo), 5);
       
   836     //EC_WRITE_U8(ecrt_sdo_request_data(sdo), 00);
       
   837     //EC_WRITE_U8(ecrt_sdo_request_data(sdo)+1, 10);
       
   838     PrintSDOState();
       
   839     ecrt_sdo_request_write(sdo);
       
   840     PrintSDOState();
       
   841     
       
   842     // Device Type (DP Ident Number)
       
   843     if (!(sdo = ecrt_slave_config_create_sdo_request(sc_dpslv_01, 0x8000, 4, 4))) {
       
   844         fprintf(stderr, "Failed to create SDO request.\n");
       
   845         return -1;
       
   846     }
       
   847     ecrt_sdo_request_timeout(sdo, 500); // ms
       
   848     sdo_adr = ecrt_sdo_request_data(sdo);
       
   849     EC_WRITE_U32(sdo_adr, 0x095F);
       
   850     //EC_WRITE_U8(sdo_ad, 0x00); // Device Type
       
   851     //EC_WRITE_U8(sdo_adr+1, 0x00);
       
   852     //EC_WRITE_U8(sdo_adr+2, 0x09);
       
   853     //EC_WRITE_U8(sdo_adr+3, 0x5F);
       
   854     PrintSDOState();
       
   855     ecrt_sdo_request_write(sdo);
       
   856     PrintSDOState();
       
   857     
       
   858     // DP CfgData Slave
       
   859     if (!(sdo = ecrt_slave_config_create_sdo_request(sc_dpslv_01, 0x8002, 0, 244))) {
       
   860         fprintf(stderr, "Failed to create SDO request.\n");
       
   861         return -1;
       
   862     }
       
   863     ecrt_sdo_request_timeout(sdo, 500); // ms
       
   864     sdo_adr = ecrt_sdo_request_data(sdo);
       
   865     EC_WRITE_U8(sdo_adr, 0x10); // Device Type
       
   866     EC_WRITE_U8(sdo_adr+1, 0x20);
       
   867     PrintSDOState();
       
   868     ecrt_sdo_request_write(sdo);
       
   869     PrintSDOState();
       
   870     
       
   871     // DP Slave Parameter Set
       
   872     if (!(sdo = ecrt_slave_config_create_sdo_request(sc_dpslv_01, 0x8000, 0, 1))) {
       
   873         fprintf(stderr, "Failed to create SDO request.\n");
       
   874         return -1;
       
   875     }
       
   876     
       
   877     ecrt_sdo_request_timeout(sdo, 500); // ms
       
   878     
       
   879     EC_WRITE_U8(ecrt_sdo_request_data(sdo), 0x33); // DP Slave Parameter Set
       
   880     PrintSDOState();
       
   881     ecrt_sdo_request_write(sdo);
       
   882     PrintSDOState();
       
   883 #endif
       
   884     
       
   885 
       
   886     
       
   887     sprintf(&rt_dev_file[0],"%s%u",EC_RTDM_DEV_FILE_NAME,0);
       
   888     
       
   889     
       
   890     rt_fd = rt_dev_open( &rt_dev_file[0], 0);
       
   891     if (rt_fd < 0) {
       
   892         printf("can't open %s\n", &rt_dev_file[0]);
       
   893         return -1;
       
   894     }
       
   895 
       
   896     MstrAttach.domainindex = ecrt_domain_index(domain1);
       
   897     
       
   898     // attach the master over rtdm driver
       
   899     rtstatus=ecrt_rtdm_master_attach(rt_fd, &MstrAttach);
       
   900     if (rtstatus < 0)
       
   901       {
       
   902         printf("cannot attach to master over rtdm\n");
       
   903         return -1;
       
   904       }
       
   905 
       
   906     printf("Activating master...\n");
       
   907     if (ecrt_master_activate(master))
       
   908         return -1;
       
   909 
       
   910     if (!(domain1_pd = ecrt_domain_data(domain1))) {
       
   911         return -1;
       
   912     }
       
   913     fprintf(stderr, "domain1_pd:  0x%.6x\n", (unsigned int)domain1_pd);
       
   914 
       
   915 
       
   916 
       
   917     int ret;
       
   918     run=1;
       
   919 
       
   920     ret = rt_task_create(&my_task,"my_task",0,80,T_FPU);
       
   921 
       
   922     printf("starting my_task\n");
       
   923     ret = rt_task_start(&my_task,&my_task_proc,NULL);
       
   924 
       
   925 
       
   926     while (run)
       
   927       {
       
   928     	sched_yield();
       
   929       }
       
   930 
       
   931     rt_task_delete(&my_task);
       
   932 
       
   933 
       
   934     if (rt_fd >= 0)
       
   935      {
       
   936         printf("closing rt device %s\n", &rt_dev_file[0]);
       
   937 
       
   938         rt_dev_close(rt_fd);
       
   939 
       
   940      }
       
   941  
       
   942     printf("End of Program\n");
       
   943     ecrt_release_master(master);
       
   944 
       
   945     return 0;
       
   946 }
       
   947 
       
   948 /****************************************************************************/