etherlab/plc_etherlab.c
author Edouard Tisserant
Thu, 28 Mar 2013 18:06:03 +0900
changeset 2109 86832a0e113d
parent 2108 6bcfd50d5a47
child 2116 2b1980a038b1
permissions -rw-r--r--
Stripped useless depricated comments, changed print format of sdo abort code
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
2106
4ec3b833f2d5 Now use beremiz.h
Edouard Tisserant
parents: 2081
diff changeset
    12
#include "beremiz.h"
4ec3b833f2d5 Now use beremiz.h
Edouard Tisserant
parents: 2081
diff changeset
    13
#include "iec_types_all.h"
2036
7c31fab22c74 Adding support for setting TargetPosition only if PositionSetPoint value from MCL is valid
laurent
parents: 2032
diff changeset
    14
2022
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    15
// declaration of interface variables
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    16
%(located_variables_declaration)s
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    17
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    18
// process data
2032
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents: 2028
diff changeset
    19
uint8_t *domain1_pd = NULL;
2022
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    20
%(used_pdo_entry_offset_variables_declaration)s
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    21
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    22
const static ec_pdo_entry_reg_t domain1_regs[] = {
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    23
%(used_pdo_entry_configuration)s
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    24
    {}
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    25
};
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    26
/*****************************************************************************/
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
%(pdos_configuration_declaration)s
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    29
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    30
long long wait_period_ns = 100000LL;
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
// EtherCAT
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    33
static ec_master_t *master = NULL;
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    34
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
    35
static int first_sent=0;
2022
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    36
%(slaves_declaration)s
2107
6f9aa410a997 Added Logging
Edouard Tisserant
parents: 2106
diff changeset
    37
#define SLOGF(level, format, args...)\
6f9aa410a997 Added Logging
Edouard Tisserant
parents: 2106
diff changeset
    38
{\
6f9aa410a997 Added Logging
Edouard Tisserant
parents: 2106
diff changeset
    39
    char sbuf[256];\
6f9aa410a997 Added Logging
Edouard Tisserant
parents: 2106
diff changeset
    40
    int slen = snprintf(sbuf , sizeof(sbuf) , format , ##args);\
6f9aa410a997 Added Logging
Edouard Tisserant
parents: 2106
diff changeset
    41
    LogMessage(level, sbuf, slen);\
6f9aa410a997 Added Logging
Edouard Tisserant
parents: 2106
diff changeset
    42
}
2022
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    43
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    44
/* Beremiz plugin functions */
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    45
int __init_%(location)s(int argc,char **argv)
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    46
{
2032
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents: 2028
diff changeset
    47
    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
    48
    size_t result_size;
2032
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents: 2028
diff changeset
    49
    
2108
6bcfd50d5a47 Updated generated code to etherlab 1.5.2
Edouard Tisserant
parents: 2107
diff changeset
    50
	master = ecrt_request_master(%(master_number)d);
2022
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    51
	if (!master) return -1;
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    52
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    53
	domain1 = ecrt_master_create_domain(master);
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    54
	if (!domain1) return -1;
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    55
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
    56
    // 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
    57
%(slaves_configuration)s
2022
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
    if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)) {
2107
6f9aa410a997 Added Logging
Edouard Tisserant
parents: 2106
diff changeset
    60
        SLOGF(LOG_CRITICAL, "PDO entry registration failed!\n");
2022
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    61
        return -1;
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    62
    }
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    63
2036
7c31fab22c74 Adding support for setting TargetPosition only if PositionSetPoint value from MCL is valid
laurent
parents: 2032
diff changeset
    64
	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
    65
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
    66
    // slaves initialization
2032
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents: 2028
diff changeset
    67
%(slaves_initialization)s
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents: 2028
diff changeset
    68
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
    69
    // 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
    70
%(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
    71
2022
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    72
    if (ecrt_master_activate(master))
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    73
        return -1;
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    74
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    75
    if (!(domain1_pd = ecrt_domain_data(domain1))) {
2107
6f9aa410a997 Added Logging
Edouard Tisserant
parents: 2106
diff changeset
    76
        SLOGF(LOG_CRITICAL, "domain1_pd:  0x%%.6lx\n", (unsigned long)domain1_pd);
2022
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    77
        return -1;
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    78
    }
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    79
2107
6f9aa410a997 Added Logging
Edouard Tisserant
parents: 2106
diff changeset
    80
    SLOGF(LOG_INFO, "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
    81
    
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
    82
    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
    83
2022
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    84
    return 0;
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    85
}
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    86
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    87
void __cleanup_%(location)s(void)
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    88
{
2024
08d67a0af2c1 Master release code added
Donggu Kang <dgkang@higenmotor.com>
parents: 2022
diff changeset
    89
	//release master
2025
87f428326f9b Master relase code added
Donggu Kang <dgkang@higenmotor.com>
parents: 2022
diff changeset
    90
	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
    91
    first_sent = 0;
2022
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
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
    94
void __retrieve_%(location)s(void)
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
    // 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
    97
    if(first_sent){
2108
6bcfd50d5a47 Updated generated code to etherlab 1.5.2
Edouard Tisserant
parents: 2107
diff changeset
    98
        ecrt_master_receive(master);
6bcfd50d5a47 Updated generated code to etherlab 1.5.2
Edouard Tisserant
parents: 2107
diff changeset
    99
        ecrt_domain_process(domain1);
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
   100
%(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
   101
    }
2022
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
   102
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
   103
}
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
   104
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
   105
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
   106
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
   107
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
   108
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
   109
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
   110
2022
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
   111
void __publish_%(location)s(void)
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
   112
{
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
   113
%(publish_variables)s
2108
6bcfd50d5a47 Updated generated code to etherlab 1.5.2
Edouard Tisserant
parents: 2107
diff changeset
   114
    ecrt_domain_queue(domain1);
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
    {
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
   116
        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
   117
        // 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
   118
        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
   119
        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
   120
            _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
   121
            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
   122
        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
   123
        _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
   124
        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
   125
            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
   126
            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
   127
                _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
   128
            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
   129
                _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
   130
        }
22e9993348f6 Better jitter compensation before sending Ethercat frame. Jitter can be compensed in both directions, within some limits
Edouard Tisserant
parents: 2075
diff changeset
   131
        _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
   132
        _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
   133
        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
   134
            _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
   135
            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
   136
        }
22e9993348f6 Better jitter compensation before sending Ethercat frame. Jitter can be compensed in both directions, within some limits
Edouard Tisserant
parents: 2075
diff changeset
   137
        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
   138
            //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
   139
            _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
   140
        }
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
   141
    }
2108
6bcfd50d5a47 Updated generated code to etherlab 1.5.2
Edouard Tisserant
parents: 2107
diff changeset
   142
    ecrt_master_send(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
   143
    first_sent = 1;
2022
c2295d311402 First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff changeset
   144
}