etherlab/plc_etherlab.c
author Edouard Tisserant <edouard.tisserant@gmail.com>
Mon, 15 Apr 2024 09:09:59 +0200
changeset 3932 62d04933d569
parent 2641 c9deff128c37
permissions -rw-r--r--
IDE: Fix Variable grid ignoring edits. WxPython4 consequence.
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
4ec3b833f2d5 Now use beremiz.h
Edouard Tisserant
parents: 2081
diff changeset
    21
#include "beremiz.h"
4ec3b833f2d5 Now use beremiz.h
Edouard Tisserant
parents: 2081
diff changeset
    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
6f9aa410a997 Added Logging
Edouard Tisserant
parents: 2106
diff changeset
    83
#define SLOGF(level, format, args...)\
6f9aa410a997 Added Logging
Edouard Tisserant
parents: 2106
diff changeset
    84
{\
6f9aa410a997 Added Logging
Edouard Tisserant
parents: 2106
diff changeset
    85
    char sbuf[256];\
6f9aa410a997 Added Logging
Edouard Tisserant
parents: 2106
diff changeset
    86
    int slen = snprintf(sbuf , sizeof(sbuf) , format , ##args);\
6f9aa410a997 Added Logging
Edouard Tisserant
parents: 2106
diff changeset
    87
    LogMessage(level, sbuf, slen);\
6f9aa410a997 Added Logging
Edouard Tisserant
parents: 2106
diff changeset
    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
/*****************************************************************************/