author | Edouard Tisserant <edouard.tisserant@gmail.com> |
Fri, 12 Aug 2022 16:52:15 +0200 | |
branch | wxPython4 |
changeset 3582 | 7dcd0de97e6f |
parent 2641 | c9deff128c37 |
permissions | -rw-r--r-- |
2022
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
1 |
/* |
2165
02a2b5dee5e3
Merged GPL + LGPL v2 or later EtherCAT extension. Fixed headers and copyright notice.
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2123
diff
changeset
|
2 |
|
02a2b5dee5e3
Merged GPL + LGPL v2 or later EtherCAT extension. Fixed headers and copyright notice.
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2123
diff
changeset
|
3 |
Template C code used to produce target Ethercat C code |
02a2b5dee5e3
Merged GPL + LGPL v2 or later EtherCAT extension. Fixed headers and copyright notice.
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2123
diff
changeset
|
4 |
|
02a2b5dee5e3
Merged GPL + LGPL v2 or later EtherCAT extension. Fixed headers and copyright notice.
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2123
diff
changeset
|
5 |
Copyright (C) 2011-2014: Laurent BESSARD, Edouard TISSERANT |
02a2b5dee5e3
Merged GPL + LGPL v2 or later EtherCAT extension. Fixed headers and copyright notice.
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2123
diff
changeset
|
6 |
|
02a2b5dee5e3
Merged GPL + LGPL v2 or later EtherCAT extension. Fixed headers and copyright notice.
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2123
diff
changeset
|
7 |
Distributed under the terms of the GNU Lesser General Public License as |
02a2b5dee5e3
Merged GPL + LGPL v2 or later EtherCAT extension. Fixed headers and copyright notice.
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2123
diff
changeset
|
8 |
published by the Free Software Foundation; either version 2 of the License, or |
02a2b5dee5e3
Merged GPL + LGPL v2 or later EtherCAT extension. Fixed headers and copyright notice.
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2123
diff
changeset
|
9 |
(at your option) any later version. |
02a2b5dee5e3
Merged GPL + LGPL v2 or later EtherCAT extension. Fixed headers and copyright notice.
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2123
diff
changeset
|
10 |
|
02a2b5dee5e3
Merged GPL + LGPL v2 or later EtherCAT extension. Fixed headers and copyright notice.
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2123
diff
changeset
|
11 |
See COPYING file for copyrights details. |
02a2b5dee5e3
Merged GPL + LGPL v2 or later EtherCAT extension. Fixed headers and copyright notice.
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2123
diff
changeset
|
12 |
|
02a2b5dee5e3
Merged GPL + LGPL v2 or later EtherCAT extension. Fixed headers and copyright notice.
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2123
diff
changeset
|
13 |
*/ |
2022
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
14 |
|
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
15 |
#include <rtdm/rtdm.h> |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
16 |
#include <native/task.h> |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
17 |
#include <native/timer.h> |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
18 |
|
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
19 |
#include "ecrt.h" |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
20 |
|
2106 | 21 |
#include "beremiz.h" |
22 |
#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
|
23 |
|
2022
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
24 |
// declaration of interface variables |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
25 |
%(located_variables_declaration)s |
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 |
// process data |
2032
766078d83e22
Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
2028
diff
changeset
|
28 |
uint8_t *domain1_pd = NULL; |
2022
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
29 |
%(used_pdo_entry_offset_variables_declaration)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 |
const static ec_pdo_entry_reg_t domain1_regs[] = { |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
32 |
%(used_pdo_entry_configuration)s |
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 |
}; |
2641
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
35 |
|
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
36 |
// Distributed Clock variables; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
37 |
%(dc_variable)s |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
38 |
unsigned long long comp_period_ns = 500000ULL; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
39 |
|
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
40 |
int comp_count = 1; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
41 |
int comp_count_max; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
42 |
|
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
43 |
#define DC_FILTER_CNT 1024 |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
44 |
|
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
45 |
// EtherCAT slave-time-based DC Synchronization variables. |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
46 |
static uint64_t dc_start_time_ns = 0LL; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
47 |
static uint64_t dc_time_ns = 0; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
48 |
static uint8_t dc_started = 0; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
49 |
static int32_t dc_diff_ns = 0; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
50 |
static int32_t prev_dc_diff_ns = 0; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
51 |
static int64_t dc_diff_total_ns = 0LL; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
52 |
static int64_t dc_delta_total_ns = 0LL; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
53 |
static int dc_filter_idx = 0; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
54 |
static int64_t dc_adjust_ns; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
55 |
static int64_t system_time_base = 0LL; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
56 |
|
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
57 |
static uint64_t dc_first_app_time = 0LL; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
58 |
|
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
59 |
unsigned long long frame_period_ns = 0ULL; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
60 |
|
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
61 |
int debug_count = 0; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
62 |
int slave_dc_used = 0; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
63 |
|
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
64 |
void dc_init(void); |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
65 |
uint64_t system_time_ns(void); |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
66 |
RTIME system2count(uint64_t time); |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
67 |
void sync_distributed_clocks(void); |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
68 |
void update_master_clock(void); |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
69 |
RTIME calculate_sleeptime(uint64_t wakeup_time); |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
70 |
uint64_t calculate_first(void); |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
71 |
|
2022
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
72 |
/*****************************************************************************/ |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
73 |
|
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
74 |
%(pdos_configuration_declaration)s |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
75 |
|
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
76 |
long long wait_period_ns = 100000LL; |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
77 |
|
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
78 |
// EtherCAT |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
79 |
static ec_master_t *master = NULL; |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
80 |
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
|
81 |
static int first_sent=0; |
2022
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
82 |
%(slaves_declaration)s |
2107 | 83 |
#define SLOGF(level, format, args...)\ |
84 |
{\ |
|
85 |
char sbuf[256];\ |
|
86 |
int slen = snprintf(sbuf , sizeof(sbuf) , format , ##args);\ |
|
87 |
LogMessage(level, sbuf, slen);\ |
|
88 |
} |
|
2022
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
89 |
|
2641
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
90 |
/* EtherCAT plugin functions */ |
2022
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
91 |
int __init_%(location)s(int argc,char **argv) |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
92 |
{ |
2032
766078d83e22
Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
2028
diff
changeset
|
93 |
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
|
94 |
size_t result_size; |
2032
766078d83e22
Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
2028
diff
changeset
|
95 |
|
2123
68beaf825a20
Added initialization of abort_code and result_size to prevent warning message if not used in following code
Laurent Bessard
parents:
2117
diff
changeset
|
96 |
abort_code = 0; |
68beaf825a20
Added initialization of abort_code and result_size to prevent warning message if not used in following code
Laurent Bessard
parents:
2117
diff
changeset
|
97 |
result_size = 0; |
68beaf825a20
Added initialization of abort_code and result_size to prevent warning message if not used in following code
Laurent Bessard
parents:
2117
diff
changeset
|
98 |
|
2117
accc4cbca8d3
Now failure in template based part of C code also release the master for later use...
Edouard Tisserant
parents:
2116
diff
changeset
|
99 |
master = ecrt_request_master(%(master_number)d); |
accc4cbca8d3
Now failure in template based part of C code also release the master for later use...
Edouard Tisserant
parents:
2116
diff
changeset
|
100 |
if (!master) { |
2116
2b1980a038b1
Better logging, saffer failure at init (allows restart)
Edouard Tisserant
parents:
2108
diff
changeset
|
101 |
SLOGF(LOG_CRITICAL, "EtherCAT master request failed!"); |
2b1980a038b1
Better logging, saffer failure at init (allows restart)
Edouard Tisserant
parents:
2108
diff
changeset
|
102 |
return -1; |
2b1980a038b1
Better logging, saffer failure at init (allows restart)
Edouard Tisserant
parents:
2108
diff
changeset
|
103 |
} |
2022
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
104 |
|
2117
accc4cbca8d3
Now failure in template based part of C code also release the master for later use...
Edouard Tisserant
parents:
2116
diff
changeset
|
105 |
if(!(domain1 = ecrt_master_create_domain(master))){ |
2116
2b1980a038b1
Better logging, saffer failure at init (allows restart)
Edouard Tisserant
parents:
2108
diff
changeset
|
106 |
SLOGF(LOG_CRITICAL, "EtherCAT Domain Creation failed!"); |
2b1980a038b1
Better logging, saffer failure at init (allows restart)
Edouard Tisserant
parents:
2108
diff
changeset
|
107 |
goto ecat_failed; |
2b1980a038b1
Better logging, saffer failure at init (allows restart)
Edouard Tisserant
parents:
2108
diff
changeset
|
108 |
} |
2022
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
109 |
|
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
|
110 |
// 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
|
111 |
%(slaves_configuration)s |
2022
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 |
if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)) { |
2116
2b1980a038b1
Better logging, saffer failure at init (allows restart)
Edouard Tisserant
parents:
2108
diff
changeset
|
114 |
SLOGF(LOG_CRITICAL, "EtherCAT PDO registration failed!"); |
2b1980a038b1
Better logging, saffer failure at init (allows restart)
Edouard Tisserant
parents:
2108
diff
changeset
|
115 |
goto ecat_failed; |
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 |
|
2117
accc4cbca8d3
Now failure in template based part of C code also release the master for later use...
Edouard Tisserant
parents:
2116
diff
changeset
|
118 |
ecrt_master_set_send_interval(master, common_ticktime__); |
2036
7c31fab22c74
Adding support for setting TargetPosition only if PositionSetPoint value from MCL is valid
laurent
parents:
2032
diff
changeset
|
119 |
|
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
|
120 |
// slaves initialization |
2641
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
121 |
/* |
2032
766078d83e22
Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
2028
diff
changeset
|
122 |
%(slaves_initialization)s |
2641
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
123 |
*/ |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
124 |
// configure DC SYNC0/1 Signal |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
125 |
%(config_dc)s |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
126 |
|
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
127 |
// select reference clock |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
128 |
#if DC_ENABLE |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
129 |
{ |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
130 |
int ret; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
131 |
|
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
132 |
ret = ecrt_master_select_reference_clock(master, slave0); |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
133 |
if (ret <0) { |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
134 |
fprintf(stderr, "Failed to select reference clock : %%s\n", |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
135 |
strerror(-ret)); |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
136 |
return ret; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
137 |
} |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
138 |
} |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
139 |
#endif |
2032
766078d83e22
Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
2028
diff
changeset
|
140 |
|
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
|
141 |
// extracting default value for not mapped entry in output PDOs |
2641
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
142 |
/* |
2039
3a218f6bd805
Adding support for uploading default value for unused entries in output pdos from slaves before starting master
laurent
parents:
2036
diff
changeset
|
143 |
%(slaves_output_pdos_default_values_extraction)s |
2641
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
144 |
*/ |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
145 |
|
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
146 |
#if DC_ENABLE |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
147 |
dc_init(); |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
148 |
#endif |
2039
3a218f6bd805
Adding support for uploading default value for unused entries in output pdos from slaves before starting master
laurent
parents:
2036
diff
changeset
|
149 |
|
2116
2b1980a038b1
Better logging, saffer failure at init (allows restart)
Edouard Tisserant
parents:
2108
diff
changeset
|
150 |
if (ecrt_master_activate(master)){ |
2b1980a038b1
Better logging, saffer failure at init (allows restart)
Edouard Tisserant
parents:
2108
diff
changeset
|
151 |
SLOGF(LOG_CRITICAL, "EtherCAT Master activation failed"); |
2b1980a038b1
Better logging, saffer failure at init (allows restart)
Edouard Tisserant
parents:
2108
diff
changeset
|
152 |
goto ecat_failed; |
2b1980a038b1
Better logging, saffer failure at init (allows restart)
Edouard Tisserant
parents:
2108
diff
changeset
|
153 |
} |
2022
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
154 |
|
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
155 |
if (!(domain1_pd = ecrt_domain_data(domain1))) { |
2116
2b1980a038b1
Better logging, saffer failure at init (allows restart)
Edouard Tisserant
parents:
2108
diff
changeset
|
156 |
SLOGF(LOG_CRITICAL, "Failed to map EtherCAT process data"); |
2b1980a038b1
Better logging, saffer failure at init (allows restart)
Edouard Tisserant
parents:
2108
diff
changeset
|
157 |
goto ecat_failed; |
2022
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
158 |
} |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
159 |
|
2116
2b1980a038b1
Better logging, saffer failure at init (allows restart)
Edouard Tisserant
parents:
2108
diff
changeset
|
160 |
SLOGF(LOG_INFO, "Master %(master_number)d activated."); |
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
|
161 |
|
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
|
162 |
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
|
163 |
|
2022
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
164 |
return 0; |
2116
2b1980a038b1
Better logging, saffer failure at init (allows restart)
Edouard Tisserant
parents:
2108
diff
changeset
|
165 |
|
2b1980a038b1
Better logging, saffer failure at init (allows restart)
Edouard Tisserant
parents:
2108
diff
changeset
|
166 |
ecat_failed: |
2117
accc4cbca8d3
Now failure in template based part of C code also release the master for later use...
Edouard Tisserant
parents:
2116
diff
changeset
|
167 |
ecrt_release_master(master); |
2116
2b1980a038b1
Better logging, saffer failure at init (allows restart)
Edouard Tisserant
parents:
2108
diff
changeset
|
168 |
return -1; |
2b1980a038b1
Better logging, saffer failure at init (allows restart)
Edouard Tisserant
parents:
2108
diff
changeset
|
169 |
|
2022
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
170 |
} |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
171 |
|
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
172 |
void __cleanup_%(location)s(void) |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
173 |
{ |
2117
accc4cbca8d3
Now failure in template based part of C code also release the master for later use...
Edouard Tisserant
parents:
2116
diff
changeset
|
174 |
//release master |
accc4cbca8d3
Now failure in template based part of C code also release the master for later use...
Edouard Tisserant
parents:
2116
diff
changeset
|
175 |
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
|
176 |
first_sent = 0; |
2022
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
177 |
} |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
178 |
|
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
179 |
void __retrieve_%(location)s(void) |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
180 |
{ |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
181 |
// 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
|
182 |
if(first_sent){ |
2108
6bcfd50d5a47
Updated generated code to etherlab 1.5.2
Edouard Tisserant
parents:
2107
diff
changeset
|
183 |
ecrt_master_receive(master); |
6bcfd50d5a47
Updated generated code to etherlab 1.5.2
Edouard Tisserant
parents:
2107
diff
changeset
|
184 |
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
|
185 |
%(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
|
186 |
} |
2022
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
187 |
|
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
188 |
} |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
189 |
|
2641
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
190 |
/* |
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
|
191 |
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
|
192 |
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
|
193 |
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
|
194 |
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
|
195 |
static inline RTIME max(RTIME a,RTIME b){return a>b?a:b;} |
2641
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
196 |
*/ |
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
|
197 |
|
2022
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
198 |
void __publish_%(location)s(void) |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
199 |
{ |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
200 |
%(publish_variables)s |
2108
6bcfd50d5a47
Updated generated code to etherlab 1.5.2
Edouard Tisserant
parents:
2107
diff
changeset
|
201 |
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
|
202 |
{ |
2641
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
203 |
/* |
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
|
204 |
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
|
205 |
// 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
|
206 |
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
|
207 |
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
|
208 |
_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
|
209 |
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
|
210 |
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
|
211 |
_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
|
212 |
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
|
213 |
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
|
214 |
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
|
215 |
_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
|
216 |
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
|
217 |
_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
|
218 |
} |
22e9993348f6
Better jitter compensation before sending Ethercat frame. Jitter can be compensed in both directions, within some limits
Edouard Tisserant
parents:
2075
diff
changeset
|
219 |
_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
|
220 |
_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
|
221 |
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
|
222 |
_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
|
223 |
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
|
224 |
} |
22e9993348f6
Better jitter compensation before sending Ethercat frame. Jitter can be compensed in both directions, within some limits
Edouard Tisserant
parents:
2075
diff
changeset
|
225 |
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
|
226 |
//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
|
227 |
_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
|
228 |
} |
2641
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
229 |
*/ |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
230 |
} |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
231 |
|
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
232 |
#if DC_ENABLE |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
233 |
if (comp_count == 0) |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
234 |
sync_distributed_clocks(); |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
235 |
#endif |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
236 |
|
2108
6bcfd50d5a47
Updated generated code to etherlab 1.5.2
Edouard Tisserant
parents:
2107
diff
changeset
|
237 |
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
|
238 |
first_sent = 1; |
2641
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
239 |
|
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
240 |
#if DC_ENABLE |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
241 |
if (comp_count == 0) |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
242 |
update_master_clock(); |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
243 |
|
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
244 |
comp_count++; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
245 |
|
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
246 |
if (comp_count == comp_count_max) |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
247 |
comp_count = 0; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
248 |
#endif |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
249 |
|
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
250 |
} |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
251 |
|
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
252 |
/* Test Function For Parameter (SDO) Set */ |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
253 |
|
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
254 |
/* |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
255 |
void GetSDOData(void){ |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
256 |
uint32_t abort_code, test_value; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
257 |
size_t result_size; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
258 |
uint8_t value[4]; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
259 |
|
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
260 |
abort_code = 0; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
261 |
result_size = 0; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
262 |
test_value = 0; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
263 |
|
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
264 |
if (ecrt_master_sdo_upload(master, 0, 0x1000, 0x0, (uint8_t *)value, 4, &result_size, &abort_code)) { |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
265 |
SLOGF(LOG_CRITICAL, "EtherCAT failed to get SDO Value"); |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
266 |
} |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
267 |
test_value = EC_READ_S32((uint8_t *)value); |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
268 |
SLOGF(LOG_INFO, "SDO Value %%d", test_value); |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
269 |
} |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
270 |
*/ |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
271 |
|
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
272 |
int GetMasterData(void){ |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
273 |
master = ecrt_open_master(0); |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
274 |
if (!master) { |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
275 |
SLOGF(LOG_CRITICAL, "EtherCAT master request failed!"); |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
276 |
return -1; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
277 |
} |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
278 |
return 0; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
279 |
} |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
280 |
|
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
281 |
void ReleaseMasterData(void){ |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
282 |
ecrt_release_master(master); |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
283 |
} |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
284 |
|
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
285 |
uint32_t GetSDOData(uint16_t slave_pos, uint16_t idx, uint8_t subidx, int size){ |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
286 |
uint32_t abort_code, return_value; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
287 |
size_t result_size; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
288 |
uint8_t value[size]; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
289 |
|
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
290 |
abort_code = 0; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
291 |
result_size = 0; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
292 |
|
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
293 |
if (ecrt_master_sdo_upload(master, slave_pos, idx, subidx, (uint8_t *)value, size, &result_size, &abort_code)) { |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
294 |
SLOGF(LOG_CRITICAL, "EtherCAT failed to get SDO Value %%d %%d", idx, subidx); |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
295 |
} |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
296 |
|
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
297 |
return_value = EC_READ_S32((uint8_t *)value); |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
298 |
//SLOGF(LOG_INFO, "SDO Value %%d", return_value); |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
299 |
|
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
300 |
return return_value; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
301 |
} |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
302 |
|
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
303 |
/*****************************************************************************/ |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
304 |
|
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
305 |
void dc_init(void) |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
306 |
{ |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
307 |
slave_dc_used = 1; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
308 |
|
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
309 |
frame_period_ns = common_ticktime__; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
310 |
if (frame_period_ns <= comp_period_ns) { |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
311 |
comp_count_max = comp_period_ns / frame_period_ns; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
312 |
comp_count = 0; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
313 |
} else { |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
314 |
comp_count_max = 1; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
315 |
comp_count = 0; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
316 |
} |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
317 |
|
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
318 |
/* Set the initial master time */ |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
319 |
dc_start_time_ns = system_time_ns(); |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
320 |
dc_time_ns = dc_start_time_ns; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
321 |
|
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
322 |
/* by woonggy */ |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
323 |
dc_first_app_time = dc_start_time_ns; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
324 |
|
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
325 |
/* |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
326 |
* Attention : The initial application time is also used for phase |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
327 |
* calculation for the SYNC0/1 interrupts. Please be sure to call it at |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
328 |
* the correct phase to the realtime cycle. |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
329 |
*/ |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
330 |
ecrt_master_application_time(master, dc_start_time_ns); |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
331 |
} |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
332 |
|
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
333 |
/****************************************************************************/ |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
334 |
|
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
335 |
/* |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
336 |
* Get the time in ns for the current cpu, adjusted by system_time_base. |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
337 |
* |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
338 |
* \attention Rather than calling rt_timer_read() directly, all application |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
339 |
* time calls should use this method instead. |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
340 |
* |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
341 |
* \ret The time in ns. |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
342 |
*/ |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
343 |
uint64_t system_time_ns(void) |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
344 |
{ |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
345 |
RTIME time = rt_timer_read(); // wkk |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
346 |
|
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
347 |
if (unlikely(system_time_base > (SRTIME) time)) { |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
348 |
fprintf(stderr, "%%s() error: system_time_base greater than" |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
349 |
" system time (system_time_base: %%ld, time: %%llu\n", |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
350 |
__func__, system_time_base, time); |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
351 |
return time; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
352 |
} |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
353 |
else { |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
354 |
return time - system_time_base; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
355 |
} |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
356 |
} |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
357 |
|
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
358 |
/****************************************************************************/ |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
359 |
|
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
360 |
// Convert system time to Xenomai time in counts (via the system_time_base). |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
361 |
RTIME system2count(uint64_t time) |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
362 |
{ |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
363 |
RTIME ret; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
364 |
|
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
365 |
if ((system_time_base < 0) && |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
366 |
((uint64_t) (-system_time_base) > time)) { |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
367 |
fprintf(stderr, "%%s() error: system_time_base less than" |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
368 |
" system time (system_time_base: %%I64d, time: %%ld\n", |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
369 |
__func__, system_time_base, time); |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
370 |
ret = time; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
371 |
} |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
372 |
else { |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
373 |
ret = time + system_time_base; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
374 |
} |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
375 |
|
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
376 |
return (RTIME) rt_timer_ns2ticks(ret); // wkk |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
377 |
} |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
378 |
|
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
379 |
/*****************************************************************************/ |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
380 |
|
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
381 |
// Synchronise the distributed clocks |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
382 |
void sync_distributed_clocks(void) |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
383 |
{ |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
384 |
uint32_t ref_time = 0; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
385 |
RTIME prev_app_time = dc_time_ns; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
386 |
|
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
387 |
// get reference clock time to synchronize master cycle |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
388 |
if(!ecrt_master_reference_clock_time(master, &ref_time)) { |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
389 |
dc_diff_ns = (uint32_t) prev_app_time - ref_time; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
390 |
} |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
391 |
// call to sync slaves to ref slave |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
392 |
ecrt_master_sync_slave_clocks(master); |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
393 |
// set master time in nano-seconds |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
394 |
dc_time_ns = system_time_ns(); |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
395 |
ecrt_master_application_time(master, dc_time_ns); |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
396 |
} |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
397 |
|
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
398 |
/*****************************************************************************/ |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
399 |
|
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
400 |
/* |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
401 |
* Return the sign of a number |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
402 |
* ie -1 for -ve value, 0 for 0, +1 for +ve value |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
403 |
* \ret val the sign of the value |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
404 |
*/ |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
405 |
#define sign(val) \ |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
406 |
({ typeof (val) _val = (val); \ |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
407 |
((_val > 0) - (_val < 0)); }) |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
408 |
|
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
409 |
/*****************************************************************************/ |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
410 |
|
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
411 |
/* |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
412 |
* Update the master time based on ref slaves time diff |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
413 |
* called after the ethercat frame is sent to avoid time jitter in |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
414 |
* sync_distributed_clocks() |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
415 |
*/ |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
416 |
void update_master_clock(void) |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
417 |
{ |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
418 |
// calc drift (via un-normalised time diff) |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
419 |
int32_t delta = dc_diff_ns - prev_dc_diff_ns; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
420 |
prev_dc_diff_ns = dc_diff_ns; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
421 |
|
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
422 |
// normalise the time diff |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
423 |
dc_diff_ns = dc_diff_ns >= 0 ? |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
424 |
((dc_diff_ns + (int32_t)(frame_period_ns / 2)) %% |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
425 |
(int32_t)frame_period_ns) - (frame_period_ns / 2) : |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
426 |
((dc_diff_ns - (int32_t)(frame_period_ns / 2)) %% |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
427 |
(int32_t)frame_period_ns) - (frame_period_ns / 2) ; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
428 |
|
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
429 |
// only update if primary master |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
430 |
if (dc_started) { |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
431 |
// add to totals |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
432 |
dc_diff_total_ns += dc_diff_ns; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
433 |
dc_delta_total_ns += delta; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
434 |
dc_filter_idx++; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
435 |
|
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
436 |
if (dc_filter_idx >= DC_FILTER_CNT) { |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
437 |
dc_adjust_ns += dc_delta_total_ns >= 0 ? |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
438 |
((dc_delta_total_ns + (DC_FILTER_CNT / 2)) / DC_FILTER_CNT) : |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
439 |
((dc_delta_total_ns - (DC_FILTER_CNT / 2)) / DC_FILTER_CNT) ; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
440 |
|
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
441 |
// and add adjustment for general diff (to pull in drift) |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
442 |
dc_adjust_ns += sign(dc_diff_total_ns / DC_FILTER_CNT); |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
443 |
|
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
444 |
// limit crazy numbers (0.1%% of std cycle time) |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
445 |
if (dc_adjust_ns < -1000) { |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
446 |
dc_adjust_ns = -1000; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
447 |
} |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
448 |
if (dc_adjust_ns > 1000) { |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
449 |
dc_adjust_ns = 1000; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
450 |
} |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
451 |
// reset |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
452 |
dc_diff_total_ns = 0LL; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
453 |
dc_delta_total_ns = 0LL; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
454 |
dc_filter_idx = 0; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
455 |
} |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
456 |
// add cycles adjustment to time base (including a spot adjustment) |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
457 |
system_time_base += dc_adjust_ns + sign(dc_diff_ns); |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
458 |
} |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
459 |
else { |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
460 |
dc_started = (dc_diff_ns != 0); |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
461 |
|
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
462 |
if (dc_started) { |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
463 |
#if DC_ENABLE && DEBUG_MODE |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
464 |
// output first diff |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
465 |
fprintf(stderr, "First master diff: %%d\n", dc_diff_ns); |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
466 |
#endif |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
467 |
// record the time of this initial cycle |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
468 |
dc_start_time_ns = dc_time_ns; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
469 |
} |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
470 |
} |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
471 |
} |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
472 |
|
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
473 |
/*****************************************************************************/ |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
474 |
|
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
475 |
/* |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
476 |
* Calculate the sleeptime |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
477 |
*/ |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
478 |
RTIME calculate_sleeptime(uint64_t wakeup_time) |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
479 |
{ |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
480 |
RTIME wakeup_count = system2count (wakeup_time); |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
481 |
RTIME current_count = rt_timer_read(); |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
482 |
|
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
483 |
if ((wakeup_count < current_count) || (wakeup_count > current_count + (50 * frame_period_ns))) { |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
484 |
fprintf(stderr, "%%s(): unexpected wake time! wc = %%lld\tcc = %%lld\n", __func__, wakeup_count, current_count); |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
485 |
} |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
486 |
|
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
487 |
return wakeup_count; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
488 |
} |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
489 |
|
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
490 |
/*****************************************************************************/ |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
491 |
|
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
492 |
/* |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
493 |
* Calculate the sleeptime |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
494 |
*/ |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
495 |
uint64_t calculate_first(void) |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
496 |
{ |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
497 |
uint64_t dc_remainder = 0LL; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
498 |
uint64_t dc_phase_set_time = 0LL; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
499 |
|
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
500 |
dc_phase_set_time = system_time_ns()+ frame_period_ns * 10; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
501 |
dc_remainder = (dc_phase_set_time - dc_first_app_time) %% frame_period_ns; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
502 |
|
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
503 |
return dc_phase_set_time + frame_period_ns - dc_remainder; |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
504 |
} |
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
505 |
|
c9deff128c37
EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents:
2165
diff
changeset
|
506 |
/*****************************************************************************/ |