etherlab/plc_etherlab.c
author Laurent Bessard
Thu, 25 Oct 2012 19:55:02 +0200
changeset 2086 8e4992e0f147
parent 2081 b1801bf4365c
child 2106 4ec3b833f2d5
permissions -rw-r--r--
Adding block library for SDO download and SDO upload
2022
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
     1
/*
2032
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents: 2028
diff changeset
     2
 * Etherlab execution code
2022
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
     3
 *
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
     4
 * */
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
     5
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
     6
#include <rtdm/rtdm.h>
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
     7
#include <native/task.h>
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
     8
#include <native/timer.h>
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
     9
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    10
#include "ecrt.h"
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    11
#include "ec_rtdm.h"
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    12
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    13
#ifdef _WINDOWS_H
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    14
  #include "iec_types.h"
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    15
#else
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    16
  #include "iec_std_lib.h"
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    17
#endif
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    18
2036
7c31fab22c74 Adding support for setting TargetPosition only if PositionSetPoint value from MCL is valid
laurent
parents: 2032
diff changeset
    19
extern unsigned long long common_ticktime__;
7c31fab22c74 Adding support for setting TargetPosition only if PositionSetPoint value from MCL is valid
laurent
parents: 2032
diff changeset
    20
2022
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    21
// declaration of interface variables
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    22
%(located_variables_declaration)s
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    23
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    24
// process data
2032
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents: 2028
diff changeset
    25
uint8_t *domain1_pd = NULL;
2022
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    26
%(used_pdo_entry_offset_variables_declaration)s
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    27
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    28
const static ec_pdo_entry_reg_t domain1_regs[] = {
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    29
%(used_pdo_entry_configuration)s
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    30
    {}
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    31
};
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    32
/*****************************************************************************/
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    33
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    34
%(pdos_configuration_declaration)s
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    35
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    36
int rt_fd = -1;
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    37
CstructMstrAttach MstrAttach;
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    38
char rt_dev_file[64];
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    39
long long wait_period_ns = 100000LL;
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    40
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    41
// EtherCAT
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    42
static ec_master_t *master = NULL;
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    43
static ec_domain_t *domain1 = NULL;
2075
21eb91dc7071 Different etherlab sequencing within retrieve and publish. Now, timer is polled after PLC exec until frame have to be sent. This slightly reduce jitter, compared to previous sequencing
Edouard Tisserant
parents: 2039
diff changeset
    44
static int first_sent=0;
2022
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    45
%(slaves_declaration)s
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    46
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    47
/* Beremiz plugin functions */
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    48
int __init_%(location)s(int argc,char **argv)
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    49
{
2032
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents: 2028
diff changeset
    50
    uint32_t abort_code;
2039
3a218f6bd805 Adding support for uploading default value for unused entries in output pdos from slaves before starting master
laurent
parents: 2036
diff changeset
    51
    size_t result_size;
2081
b1801bf4365c Fix bug in endianization in value transmit and received by SDO
Laurent Bessard
parents: 2079
diff changeset
    52
    int rtstatus;
2032
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents: 2028
diff changeset
    53
    
2022
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    54
	MstrAttach.masterindex = %(master_number)d;
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    55
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    56
	master = ecrt_request_master(MstrAttach.masterindex);
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    57
	if (!master) return -1;
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    58
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    59
	domain1 = ecrt_master_create_domain(master);
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    60
	if (!domain1) return -1;
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    61
2079
49242019a9ca Fix C code Generator to use CoE section contained in the slave ESI file for defining Ethercat network configuration
Laurent Bessard
parents: 2076
diff changeset
    62
    // slaves PDO configuration
49242019a9ca Fix C code Generator to use CoE section contained in the slave ESI file for defining Ethercat network configuration
Laurent Bessard
parents: 2076
diff changeset
    63
%(slaves_configuration)s
2022
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    64
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    65
    if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)) {
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    66
        fprintf(stderr, "PDO entry registration failed!\n");
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    67
        return -1;
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    68
    }
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    69
2036
7c31fab22c74 Adding support for setting TargetPosition only if PositionSetPoint value from MCL is valid
laurent
parents: 2032
diff changeset
    70
	ecrt_master_set_send_interval(master, common_ticktime__);
7c31fab22c74 Adding support for setting TargetPosition only if PositionSetPoint value from MCL is valid
laurent
parents: 2032
diff changeset
    71
2079
49242019a9ca Fix C code Generator to use CoE section contained in the slave ESI file for defining Ethercat network configuration
Laurent Bessard
parents: 2076
diff changeset
    72
    // slaves initialization
2032
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents: 2028
diff changeset
    73
%(slaves_initialization)s
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents: 2028
diff changeset
    74
2079
49242019a9ca Fix C code Generator to use CoE section contained in the slave ESI file for defining Ethercat network configuration
Laurent Bessard
parents: 2076
diff changeset
    75
    // extracting default value for not mapped entry in output PDOs
2039
3a218f6bd805 Adding support for uploading default value for unused entries in output pdos from slaves before starting master
laurent
parents: 2036
diff changeset
    76
%(slaves_output_pdos_default_values_extraction)s
3a218f6bd805 Adding support for uploading default value for unused entries in output pdos from slaves before starting master
laurent
parents: 2036
diff changeset
    77
2022
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    78
    sprintf(&rt_dev_file[0],"%%s%%u",EC_RTDM_DEV_FILE_NAME,0);
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    79
    rt_fd = rt_dev_open( &rt_dev_file[0], 0);
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    80
    if (rt_fd < 0) {
2032
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents: 2028
diff changeset
    81
        fprintf(stderr, "Can't open %%s\n", &rt_dev_file[0]);
2022
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    82
        return -1;
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    83
    }
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    84
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    85
    // attach the master over rtdm driver
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    86
    MstrAttach.domainindex = ecrt_domain_index(domain1);
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    87
    rtstatus = ecrt_rtdm_master_attach(rt_fd, &MstrAttach);
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    88
    if (rtstatus < 0) {
2032
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents: 2028
diff changeset
    89
        fprintf(stderr, "Cannot attach to master over rtdm\n");
2022
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    90
        return -1;
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    91
    }
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    92
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    93
    if (ecrt_master_activate(master))
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    94
        return -1;
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    95
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    96
    if (!(domain1_pd = ecrt_domain_data(domain1))) {
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    97
        fprintf(stderr, "domain1_pd:  0x%%.6lx\n", (unsigned long)domain1_pd);
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    98
        return -1;
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    99
    }
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
   100
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
   101
    fprintf(stdout, "Master %(master_number)d activated...\n");
2075
21eb91dc7071 Different etherlab sequencing within retrieve and publish. Now, timer is polled after PLC exec until frame have to be sent. This slightly reduce jitter, compared to previous sequencing
Edouard Tisserant
parents: 2039
diff changeset
   102
    
21eb91dc7071 Different etherlab sequencing within retrieve and publish. Now, timer is polled after PLC exec until frame have to be sent. This slightly reduce jitter, compared to previous sequencing
Edouard Tisserant
parents: 2039
diff changeset
   103
    first_sent = 0;
21eb91dc7071 Different etherlab sequencing within retrieve and publish. Now, timer is polled after PLC exec until frame have to be sent. This slightly reduce jitter, compared to previous sequencing
Edouard Tisserant
parents: 2039
diff changeset
   104
2022
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
   105
    return 0;
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
   106
}
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
   107
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
   108
void __cleanup_%(location)s(void)
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
   109
{
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
   110
	if (rt_fd >= 0) {
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
   111
		rt_dev_close(rt_fd);
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
   112
	}
2024
08d67a0af2c1 Master release code added
Donggu Kang <dgkang@higenmotor.com>
parents: 2022
diff changeset
   113
	//release master
2025
87f428326f9b Master relase code added
Donggu Kang <dgkang@higenmotor.com>
parents: 2022
diff changeset
   114
	ecrt_release_master(master);
2075
21eb91dc7071 Different etherlab sequencing within retrieve and publish. Now, timer is polled after PLC exec until frame have to be sent. This slightly reduce jitter, compared to previous sequencing
Edouard Tisserant
parents: 2039
diff changeset
   115
    first_sent = 0;
2022
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
   116
}
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
   117
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
   118
void __retrieve_%(location)s(void)
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
   119
{
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
   120
    // receive ethercat
2075
21eb91dc7071 Different etherlab sequencing within retrieve and publish. Now, timer is polled after PLC exec until frame have to be sent. This slightly reduce jitter, compared to previous sequencing
Edouard Tisserant
parents: 2039
diff changeset
   121
    if(first_sent){
21eb91dc7071 Different etherlab sequencing within retrieve and publish. Now, timer is polled after PLC exec until frame have to be sent. This slightly reduce jitter, compared to previous sequencing
Edouard Tisserant
parents: 2039
diff changeset
   122
        ecrt_rtdm_master_recieve(rt_fd);
21eb91dc7071 Different etherlab sequencing within retrieve and publish. Now, timer is polled after PLC exec until frame have to be sent. This slightly reduce jitter, compared to previous sequencing
Edouard Tisserant
parents: 2039
diff changeset
   123
        ecrt_rtdm_domain_process(rt_fd);
21eb91dc7071 Different etherlab sequencing within retrieve and publish. Now, timer is polled after PLC exec until frame have to be sent. This slightly reduce jitter, compared to previous sequencing
Edouard Tisserant
parents: 2039
diff changeset
   124
%(retrieve_variables)s
21eb91dc7071 Different etherlab sequencing within retrieve and publish. Now, timer is polled after PLC exec until frame have to be sent. This slightly reduce jitter, compared to previous sequencing
Edouard Tisserant
parents: 2039
diff changeset
   125
    }
2022
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
   126
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
   127
}
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
   128
2075
21eb91dc7071 Different etherlab sequencing within retrieve and publish. Now, timer is polled after PLC exec until frame have to be sent. This slightly reduce jitter, compared to previous sequencing
Edouard Tisserant
parents: 2039
diff changeset
   129
static RTIME _last_occur=0;
2076
22e9993348f6 Better jitter compensation before sending Ethercat frame. Jitter can be compensed in both directions, within some limits
Edouard Tisserant
parents: 2075
diff changeset
   130
static RTIME _last_publish=0;
2075
21eb91dc7071 Different etherlab sequencing within retrieve and publish. Now, timer is polled after PLC exec until frame have to be sent. This slightly reduce jitter, compared to previous sequencing
Edouard Tisserant
parents: 2039
diff changeset
   131
RTIME _current_lag=0;
2076
22e9993348f6 Better jitter compensation before sending Ethercat frame. Jitter can be compensed in both directions, within some limits
Edouard Tisserant
parents: 2075
diff changeset
   132
RTIME _max_jitter=0;
22e9993348f6 Better jitter compensation before sending Ethercat frame. Jitter can be compensed in both directions, within some limits
Edouard Tisserant
parents: 2075
diff changeset
   133
static inline RTIME max(RTIME a,RTIME b){return a>b?a:b;}
2075
21eb91dc7071 Different etherlab sequencing within retrieve and publish. Now, timer is polled after PLC exec until frame have to be sent. This slightly reduce jitter, compared to previous sequencing
Edouard Tisserant
parents: 2039
diff changeset
   134
2022
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
   135
void __publish_%(location)s(void)
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
   136
{
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
   137
%(publish_variables)s
2075
21eb91dc7071 Different etherlab sequencing within retrieve and publish. Now, timer is polled after PLC exec until frame have to be sent. This slightly reduce jitter, compared to previous sequencing
Edouard Tisserant
parents: 2039
diff changeset
   138
    ecrt_rtdm_domain_queque(rt_fd);
21eb91dc7071 Different etherlab sequencing within retrieve and publish. Now, timer is polled after PLC exec until frame have to be sent. This slightly reduce jitter, compared to previous sequencing
Edouard Tisserant
parents: 2039
diff changeset
   139
    {
2076
22e9993348f6 Better jitter compensation before sending Ethercat frame. Jitter can be compensed in both directions, within some limits
Edouard Tisserant
parents: 2075
diff changeset
   140
        RTIME current_time = rt_timer_read();
22e9993348f6 Better jitter compensation before sending Ethercat frame. Jitter can be compensed in both directions, within some limits
Edouard Tisserant
parents: 2075
diff changeset
   141
        // Limit spining max 1/5 of common_ticktime
22e9993348f6 Better jitter compensation before sending Ethercat frame. Jitter can be compensed in both directions, within some limits
Edouard Tisserant
parents: 2075
diff changeset
   142
        RTIME maxdeadline = current_time + (common_ticktime__ / 5);
22e9993348f6 Better jitter compensation before sending Ethercat frame. Jitter can be compensed in both directions, within some limits
Edouard Tisserant
parents: 2075
diff changeset
   143
        RTIME deadline = _last_occur ? 
22e9993348f6 Better jitter compensation before sending Ethercat frame. Jitter can be compensed in both directions, within some limits
Edouard Tisserant
parents: 2075
diff changeset
   144
            _last_occur + common_ticktime__ : 
22e9993348f6 Better jitter compensation before sending Ethercat frame. Jitter can be compensed in both directions, within some limits
Edouard Tisserant
parents: 2075
diff changeset
   145
            current_time + _max_jitter; 
22e9993348f6 Better jitter compensation before sending Ethercat frame. Jitter can be compensed in both directions, within some limits
Edouard Tisserant
parents: 2075
diff changeset
   146
        if(deadline > maxdeadline) deadline = maxdeadline;
22e9993348f6 Better jitter compensation before sending Ethercat frame. Jitter can be compensed in both directions, within some limits
Edouard Tisserant
parents: 2075
diff changeset
   147
        _current_lag = deadline - current_time;
22e9993348f6 Better jitter compensation before sending Ethercat frame. Jitter can be compensed in both directions, within some limits
Edouard Tisserant
parents: 2075
diff changeset
   148
        if(_last_publish != 0){
22e9993348f6 Better jitter compensation before sending Ethercat frame. Jitter can be compensed in both directions, within some limits
Edouard Tisserant
parents: 2075
diff changeset
   149
            RTIME period = current_time - _last_publish;
22e9993348f6 Better jitter compensation before sending Ethercat frame. Jitter can be compensed in both directions, within some limits
Edouard Tisserant
parents: 2075
diff changeset
   150
            if(period > common_ticktime__ )
22e9993348f6 Better jitter compensation before sending Ethercat frame. Jitter can be compensed in both directions, within some limits
Edouard Tisserant
parents: 2075
diff changeset
   151
                _max_jitter = max(_max_jitter, period - common_ticktime__);
22e9993348f6 Better jitter compensation before sending Ethercat frame. Jitter can be compensed in both directions, within some limits
Edouard Tisserant
parents: 2075
diff changeset
   152
            else
22e9993348f6 Better jitter compensation before sending Ethercat frame. Jitter can be compensed in both directions, within some limits
Edouard Tisserant
parents: 2075
diff changeset
   153
                _max_jitter = max(_max_jitter, common_ticktime__ - period);
22e9993348f6 Better jitter compensation before sending Ethercat frame. Jitter can be compensed in both directions, within some limits
Edouard Tisserant
parents: 2075
diff changeset
   154
        }
22e9993348f6 Better jitter compensation before sending Ethercat frame. Jitter can be compensed in both directions, within some limits
Edouard Tisserant
parents: 2075
diff changeset
   155
        _last_publish = current_time;
22e9993348f6 Better jitter compensation before sending Ethercat frame. Jitter can be compensed in both directions, within some limits
Edouard Tisserant
parents: 2075
diff changeset
   156
        _last_occur = current_time;
22e9993348f6 Better jitter compensation before sending Ethercat frame. Jitter can be compensed in both directions, within some limits
Edouard Tisserant
parents: 2075
diff changeset
   157
        while(current_time < deadline) {
22e9993348f6 Better jitter compensation before sending Ethercat frame. Jitter can be compensed in both directions, within some limits
Edouard Tisserant
parents: 2075
diff changeset
   158
            _last_occur = current_time; //Drift backward by default
22e9993348f6 Better jitter compensation before sending Ethercat frame. Jitter can be compensed in both directions, within some limits
Edouard Tisserant
parents: 2075
diff changeset
   159
            current_time = rt_timer_read();
22e9993348f6 Better jitter compensation before sending Ethercat frame. Jitter can be compensed in both directions, within some limits
Edouard Tisserant
parents: 2075
diff changeset
   160
        }
22e9993348f6 Better jitter compensation before sending Ethercat frame. Jitter can be compensed in both directions, within some limits
Edouard Tisserant
parents: 2075
diff changeset
   161
        if( _max_jitter * 10 < common_ticktime__ && _current_lag < _max_jitter){
22e9993348f6 Better jitter compensation before sending Ethercat frame. Jitter can be compensed in both directions, within some limits
Edouard Tisserant
parents: 2075
diff changeset
   162
            //Consuming security margin ?
22e9993348f6 Better jitter compensation before sending Ethercat frame. Jitter can be compensed in both directions, within some limits
Edouard Tisserant
parents: 2075
diff changeset
   163
            _last_occur = current_time; //Drift forward
2075
21eb91dc7071 Different etherlab sequencing within retrieve and publish. Now, timer is polled after PLC exec until frame have to be sent. This slightly reduce jitter, compared to previous sequencing
Edouard Tisserant
parents: 2039
diff changeset
   164
        }
21eb91dc7071 Different etherlab sequencing within retrieve and publish. Now, timer is polled after PLC exec until frame have to be sent. This slightly reduce jitter, compared to previous sequencing
Edouard Tisserant
parents: 2039
diff changeset
   165
    }
21eb91dc7071 Different etherlab sequencing within retrieve and publish. Now, timer is polled after PLC exec until frame have to be sent. This slightly reduce jitter, compared to previous sequencing
Edouard Tisserant
parents: 2039
diff changeset
   166
    ecrt_rtdm_master_send(rt_fd);
21eb91dc7071 Different etherlab sequencing within retrieve and publish. Now, timer is polled after PLC exec until frame have to be sent. This slightly reduce jitter, compared to previous sequencing
Edouard Tisserant
parents: 2039
diff changeset
   167
    first_sent = 1;
2022
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
   168
}