1 /*

     2 

     3 Template C code used to produce target Ethercat C code

     4 

     5 Copyright (C) 2011-2014: Laurent BESSARD, Edouard TISSERANT

     6 

     7 Distributed under the terms of the GNU Lesser General Public License as

     8 published by the Free Software Foundation; either version 2 of the License, or

     9 (at your option) any later version.

    10 

    11 See COPYING file for copyrights details.

    12 

    13 */

    14 

    15 #include <rtdm/rtdm.h>

    16 #include <native/task.h>

    17 #include <native/timer.h>

    18 

    19 #include "ecrt.h"

    20 

    21 #include "beremiz.h"

    22 #include "iec_types_all.h"

    23 

    24 // declaration of interface variables

    25 %(located_variables_declaration)s

    26 

    27 // process data

    28 uint8_t *domain1_pd = NULL;

    29 %(used_pdo_entry_offset_variables_declaration)s

    30 

    31 const static ec_pdo_entry_reg_t domain1_regs[] = {

    32 %(used_pdo_entry_configuration)s

    33     {}

    34 };

    35 /*****************************************************************************/

    36 

    37 %(pdos_configuration_declaration)s

    38 

    39 long long wait_period_ns = 100000LL;

    40 

    41 // EtherCAT

    42 static ec_master_t *master = NULL;

    43 static ec_domain_t *domain1 = NULL;

    44 static int first_sent=0;

    45 %(slaves_declaration)s

    46 #define SLOGF(level, format, args...)\

    47 {\

    48     char sbuf[256];\

    49     int slen = snprintf(sbuf , sizeof(sbuf) , format , ##args);\

    50     LogMessage(level, sbuf, slen);\

    51 }

    52 

    53 /* Beremiz plugin functions */

    54 int __init_%(location)s(int argc,char **argv)

    55 {

    56     uint32_t abort_code;

    57     size_t result_size;

    58     

    59     abort_code = 0;

    60     result_size = 0;

    61 

    62     master = ecrt_request_master(%(master_number)d);

    63     if (!master) {

    64         SLOGF(LOG_CRITICAL, "EtherCAT master request failed!");

    65         return -1;

    66     }

    67 

    68     if(!(domain1 = ecrt_master_create_domain(master))){

    69         SLOGF(LOG_CRITICAL, "EtherCAT Domain Creation failed!");

    70         goto ecat_failed;

    71     }

    72 

    73     // slaves PDO configuration

    74 %(slaves_configuration)s

    75 

    76     if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)) {

    77         SLOGF(LOG_CRITICAL, "EtherCAT PDO registration failed!");

    78         goto ecat_failed;

    79     }

    80 

    81     ecrt_master_set_send_interval(master, common_ticktime__);

    82 

    83     // slaves initialization

    84 %(slaves_initialization)s

    85 

    86     // extracting default value for not mapped entry in output PDOs

    87 %(slaves_output_pdos_default_values_extraction)s

    88 

    89     if (ecrt_master_activate(master)){

    90         SLOGF(LOG_CRITICAL, "EtherCAT Master activation failed");

    91         goto ecat_failed;

    92     }

    93 

    94     if (!(domain1_pd = ecrt_domain_data(domain1))) {

    95         SLOGF(LOG_CRITICAL, "Failed to map EtherCAT process data");

    96         goto ecat_failed;

    97     }

    98 

    99     SLOGF(LOG_INFO, "Master %(master_number)d activated.");

   100     

   101     first_sent = 0;

   102 

   103     return 0;

   104 

   105 ecat_failed:

   106     ecrt_release_master(master);

   107     return -1;

   108 

   109 }

   110 

   111 void __cleanup_%(location)s(void)

   112 {

   113     //release master

   114     ecrt_release_master(master);

   115     first_sent = 0;

   116 }

   117 

   118 void __retrieve_%(location)s(void)

   119 {

   120     // receive ethercat

   121     if(first_sent){

   122         ecrt_master_receive(master);

   123         ecrt_domain_process(domain1);

   124 %(retrieve_variables)s

   125     }

   126 

   127 }

   128 

   129 static RTIME _last_occur=0;

   130 static RTIME _last_publish=0;

   131 RTIME _current_lag=0;

   132 RTIME _max_jitter=0;

   133 static inline RTIME max(RTIME a,RTIME b){return a>b?a:b;}

   134 

   135 void __publish_%(location)s(void)

   136 {

   137 %(publish_variables)s

   138     ecrt_domain_queue(domain1);

   139     {

   140         RTIME current_time = rt_timer_read();

   141         // Limit spining max 1/5 of common_ticktime

   142         RTIME maxdeadline = current_time + (common_ticktime__ / 5);

   143         RTIME deadline = _last_occur ? 

   144             _last_occur + common_ticktime__ : 

   145             current_time + _max_jitter; 

   146         if(deadline > maxdeadline) deadline = maxdeadline;

   147         _current_lag = deadline - current_time;

   148         if(_last_publish != 0){

   149             RTIME period = current_time - _last_publish;

   150             if(period > common_ticktime__ )

   151                 _max_jitter = max(_max_jitter, period - common_ticktime__);

   152             else

   153                 _max_jitter = max(_max_jitter, common_ticktime__ - period);

   154         }

   155         _last_publish = current_time;

   156         _last_occur = current_time;

   157         while(current_time < deadline) {

   158             _last_occur = current_time; //Drift backward by default

   159             current_time = rt_timer_read();

   160         }

   161         if( _max_jitter * 10 < common_ticktime__ && _current_lag < _max_jitter){

   162             //Consuming security margin ?

   163             _last_occur = current_time; //Drift forward

   164         }

   165     }

   166     ecrt_master_send(master);

   167     first_sent = 1;

   168 }