1

/*

     2

     3

Template C code used to produce target Ethercat C code

     4

     5

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

     6

     7

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

     8

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

     9

(at your option) any later version.

    10

    11

See COPYING file for copyrights details.

    12

    13

*/

    14

    15

#include <rtdm/rtdm.h>

    16

#include <native/task.h>

    17

#include <native/timer.h>

    18

    19

#include "ecrt.h"

    20

    21

#include "beremiz.h"

    22

#include "iec_types_all.h"

    23

    24

// declaration of interface variables

    25

%(located_variables_declaration)s

    26

    27

// process data

    28

uint8_t *domain1_pd = NULL;

    29

%(used_pdo_entry_offset_variables_declaration)s

    30

    31

const static ec_pdo_entry_reg_t domain1_regs[] = {

    32

%(used_pdo_entry_configuration)s

    33

    {}

    34

};

    35

    36

// Distributed Clock variables;

    37

%(dc_variable)s

    38

unsigned long long comp_period_ns = 500000ULL;

    39

    40

int comp_count = 1;

    41

int comp_count_max;

    42

    43

#define DC_FILTER_CNT          1024

    44

    45

// EtherCAT slave-time-based DC Synchronization variables.

    46

static uint64_t dc_start_time_ns = 0LL;

    47

static uint64_t dc_time_ns = 0;

    48

static uint8_t  dc_started = 0;

    49

static int32_t  dc_diff_ns = 0;

    50

static int32_t  prev_dc_diff_ns = 0;

    51

static int64_t  dc_diff_total_ns = 0LL;

    52

static int64_t  dc_delta_total_ns = 0LL;

    53

static int      dc_filter_idx = 0;

    54

static int64_t  dc_adjust_ns;

    55

static int64_t  system_time_base = 0LL;

    56

    57

static uint64_t dc_first_app_time = 0LL;

    58

    59

unsigned long long frame_period_ns = 0ULL;

    60

    61

int debug_count = 0;

    62

int slave_dc_used = 0;

    63

    64

void dc_init(void);

    65

uint64_t system_time_ns(void);

    66

RTIME system2count(uint64_t time);

    67

void sync_distributed_clocks(void);

    68

void update_master_clock(void);

    69

RTIME calculate_sleeptime(uint64_t wakeup_time);

    70

uint64_t calculate_first(void);

    71

    72

/*****************************************************************************/

    73

    74

%(pdos_configuration_declaration)s

    75

    76

long long wait_period_ns = 100000LL;

    77

    78

// EtherCAT

    79

static ec_master_t *master = NULL;

    80

static ec_domain_t *domain1 = NULL;

    81

static int first_sent=0;

    82

%(slaves_declaration)s

    83

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

    84

{\

    85

    char sbuf[256];\

    86

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

    87

    LogMessage(level, sbuf, slen);\

    88

}

    89

    90

/* EtherCAT plugin functions */

    91

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

    92

{

    93

    uint32_t abort_code;

    94

    size_t result_size;

    95

    96

    abort_code = 0;

    97

    result_size = 0;

    98

    99

    master = ecrt_request_master(%(master_number)d);

   100

    if (!master) {

   101

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

   102

        return -1;

   103

    }

   104

   105

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

   106

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

   107

        goto ecat_failed;

   108

    }

   109

   110

    // slaves PDO configuration

   111

%(slaves_configuration)s

   112

   113

    if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)) {

   114

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

   115

        goto ecat_failed;

   116

    }

   117

   118

    ecrt_master_set_send_interval(master, common_ticktime__);

   119

   120

    // slaves initialization

   121

/*

   122

%(slaves_initialization)s

   123

*/

   124

    // configure DC SYNC0/1 Signal

   125

%(config_dc)s

   126

   127

    // select reference clock

   128

#if DC_ENABLE

   129

    {

   130

        int ret;

   131

   132

        ret = ecrt_master_select_reference_clock(master, slave0);

   133

        if (ret <0) {

   134

            fprintf(stderr, "Failed to select reference clock : %%s\n",

   135

                strerror(-ret));

   136

            return ret;

   137

        }

   138

    }

   139

#endif

   140

   141

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

   142

/*

   143

%(slaves_output_pdos_default_values_extraction)s

   144

*/

   145

   146

#if DC_ENABLE

   147

    dc_init();

   148

#endif

   149

   150

    if (ecrt_master_activate(master)){

   151

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

   152

        goto ecat_failed;

   153

    }

   154

   155

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

   156

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

   157

        goto ecat_failed;

   158

    }

   159

   160

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

   161

   162

    first_sent = 0;

   163

   164

    return 0;

   165

   166

ecat_failed:

   167

    ecrt_release_master(master);

   168

    return -1;

   169

   170

}

   171

   172

void __cleanup_%(location)s(void)

   173

{

   174

    //release master

   175

    ecrt_release_master(master);

   176

    first_sent = 0;

   177

}

   178

   179

void __retrieve_%(location)s(void)

   180

{

   181

    // receive ethercat

   182

    if(first_sent){

   183

        ecrt_master_receive(master);

   184

        ecrt_domain_process(domain1);

   185

%(retrieve_variables)s

   186

    }

   187

   188

}

   189

   190

/*

   191

static RTIME _last_occur=0;

   192

static RTIME _last_publish=0;

   193

RTIME _current_lag=0;

   194

RTIME _max_jitter=0;

   195

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

   196

*/

   197

   198

void __publish_%(location)s(void)

   199

{

   200

%(publish_variables)s

   201

    ecrt_domain_queue(domain1);

   202

    {

   203

        /*

   204

        RTIME current_time = rt_timer_read();

   205

        // Limit spining max 1/5 of common_ticktime

   206

        RTIME maxdeadline = current_time + (common_ticktime__ / 5);

   207

        RTIME deadline = _last_occur ? 

   208

            _last_occur + common_ticktime__ : 

   209

            current_time + _max_jitter; 

   210

        if(deadline > maxdeadline) deadline = maxdeadline;

   211

        _current_lag = deadline - current_time;

   212

        if(_last_publish != 0){

   213

            RTIME period = current_time - _last_publish;

   214

            if(period > common_ticktime__ )

   215

                _max_jitter = max(_max_jitter, period - common_ticktime__);

   216

            else

   217

                _max_jitter = max(_max_jitter, common_ticktime__ - period);

   218

        }

   219

        _last_publish = current_time;

   220

        _last_occur = current_time;

   221

        while(current_time < deadline) {

   222

            _last_occur = current_time; //Drift backward by default

   223

            current_time = rt_timer_read();

   224

        }

   225

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

   226

            //Consuming security margin ?

   227

            _last_occur = current_time; //Drift forward

   228

        }

   229

        */

   230

    }

   231

   232

#if DC_ENABLE

   233

    if (comp_count == 0)

   234

        sync_distributed_clocks();

   235

#endif

   236

   237

    ecrt_master_send(master);

   238

    first_sent = 1;

   239

   240

#if DC_ENABLE

   241

    if (comp_count == 0)

   242

        update_master_clock();

   243

   244

    comp_count++;

   245

   246

    if (comp_count == comp_count_max)

   247

        comp_count = 0;

   248

#endif

   249

   250

}

   251

   252

/* Test Function For Parameter (SDO) Set */

   253

   254

/*

   255

void GetSDOData(void){

   256

    uint32_t abort_code, test_value;

   257

    size_t result_size;

   258

    uint8_t value[4];

   259

   260

    abort_code = 0;

   261

    result_size = 0;

   262

    test_value = 0;

   263

   264

    if (ecrt_master_sdo_upload(master, 0, 0x1000, 0x0, (uint8_t *)value, 4, &result_size, &abort_code)) {

   265

        SLOGF(LOG_CRITICAL, "EtherCAT failed to get SDO Value");

   266

        }

   267

        test_value = EC_READ_S32((uint8_t *)value);

   268

        SLOGF(LOG_INFO, "SDO Value %%d", test_value);

   269

}

   270

*/

   271

   272

int GetMasterData(void){

   273

    master = ecrt_open_master(0);

   274

    if (!master) {

   275

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

   276

        return -1;

   277

    }

   278

    return 0;

   279

}

   280

   281

void ReleaseMasterData(void){

   282

    ecrt_release_master(master);

   283

}

   284

   285

uint32_t GetSDOData(uint16_t slave_pos, uint16_t idx, uint8_t subidx, int size){

   286

    uint32_t abort_code, return_value;

   287

    size_t result_size;

   288

    uint8_t value[size];

   289

   290

    abort_code = 0;

   291

    result_size = 0;

   292

   293

    if (ecrt_master_sdo_upload(master, slave_pos, idx, subidx, (uint8_t *)value, size, &result_size, &abort_code)) {

   294

        SLOGF(LOG_CRITICAL, "EtherCAT failed to get SDO Value %%d %%d", idx, subidx);

   295

    }

   296

   297

    return_value = EC_READ_S32((uint8_t *)value);

   298

    //SLOGF(LOG_INFO, "SDO Value %%d", return_value);

   299

   300

    return return_value;

   301

}

   302

   303

/*****************************************************************************/

   304

   305

void dc_init(void)

   306

{

   307

    slave_dc_used = 1;

   308

   309

    frame_period_ns = common_ticktime__;

   310

    if (frame_period_ns <= comp_period_ns) {

   311

        comp_count_max = comp_period_ns / frame_period_ns;

   312

        comp_count = 0;

   313

    } else  {

   314

        comp_count_max = 1;

   315

        comp_count = 0;

   316

    }

   317

   318

    /* Set the initial master time */

   319

    dc_start_time_ns = system_time_ns();

   320

    dc_time_ns = dc_start_time_ns;

   321

   322

    /* by woonggy */

   323

    dc_first_app_time = dc_start_time_ns;

   324

   325

    /*

   326

     * Attention : The initial application time is also used for phase

   327

     * calculation for the SYNC0/1 interrupts. Please be sure to call it at

   328

     * the correct phase to the realtime cycle.

   329

     */

   330

    ecrt_master_application_time(master, dc_start_time_ns);

   331

}

   332

   333

/****************************************************************************/

   334

   335

/*

   336

 * Get the time in ns for the current cpu, adjusted by system_time_base.

   337

 *

   338

 * \attention Rather than calling rt_timer_read() directly, all application

   339

 * time calls should use this method instead.

   340

 *

   341

 * \ret The time in ns.

   342

 */

   343

uint64_t system_time_ns(void)

   344

{

   345

    RTIME time = rt_timer_read();   // wkk

   346

   347

    if (unlikely(system_time_base > (SRTIME) time)) {

   348

        fprintf(stderr, "%%s() error: system_time_base greater than"

   349

                " system time (system_time_base: %%ld, time: %%llu\n",

   350

                __func__, system_time_base, time);

   351

        return time;

   352

    }

   353

    else {

   354

        return time - system_time_base;

   355

    }

   356

}

   357

   358

/****************************************************************************/

   359

   360

// Convert system time to Xenomai time in counts (via the system_time_base).

   361

RTIME system2count(uint64_t time)

   362

{

   363

    RTIME ret;

   364

   365

    if ((system_time_base < 0) &&

   366

            ((uint64_t) (-system_time_base) > time)) {

   367

        fprintf(stderr, "%%s() error: system_time_base less than"

   368

                " system time (system_time_base: %%I64d, time: %%ld\n",

   369

                __func__, system_time_base, time);

   370

        ret = time;

   371

    }

   372

    else {

   373

        ret = time + system_time_base;

   374

    }

   375

   376

    return (RTIME) rt_timer_ns2ticks(ret); // wkk

   377

}

   378

   379

/*****************************************************************************/

   380

   381

// Synchronise the distributed clocks

   382

void sync_distributed_clocks(void)

   383

{

   384

    uint32_t ref_time = 0;

   385

    RTIME prev_app_time = dc_time_ns;

   386

   387

    // get reference clock time to synchronize master cycle

   388

    if(!ecrt_master_reference_clock_time(master, &ref_time)) {

   389

        dc_diff_ns = (uint32_t) prev_app_time - ref_time;

   390

    }

   391

    // call to sync slaves to ref slave

   392

    ecrt_master_sync_slave_clocks(master);

   393

    // set master time in nano-seconds

   394

    dc_time_ns = system_time_ns();

   395

    ecrt_master_application_time(master, dc_time_ns);

   396

}

   397

   398

/*****************************************************************************/

   399

   400

/*

   401

 * Return the sign of a number

   402

 * ie -1 for -ve value, 0 for 0, +1 for +ve value

   403

 * \ret val the sign of the value

   404

 */

   405

#define sign(val) \

   406

        ({ typeof (val) _val = (val); \

   407

        ((_val > 0) - (_val < 0)); })

   408

   409

/*****************************************************************************/

   410

   411

/*

   412

 * Update the master time based on ref slaves time diff

   413

 * called after the ethercat frame is sent to avoid time jitter in

   414

 * sync_distributed_clocks()

   415

 */

   416

void update_master_clock(void)

   417

{

   418

    // calc drift (via un-normalised time diff)

   419

    int32_t delta = dc_diff_ns - prev_dc_diff_ns;

   420

    prev_dc_diff_ns = dc_diff_ns;

   421

   422

    // normalise the time diff

   423

    dc_diff_ns = dc_diff_ns >= 0 ?

   424

            ((dc_diff_ns + (int32_t)(frame_period_ns / 2)) %%

   425

                    (int32_t)frame_period_ns) - (frame_period_ns / 2) :

   426

                    ((dc_diff_ns - (int32_t)(frame_period_ns / 2)) %%

   427

                            (int32_t)frame_period_ns) - (frame_period_ns / 2) ;

   428

   429

    // only update if primary master

   430

    if (dc_started) {

   431

        // add to totals

   432

        dc_diff_total_ns += dc_diff_ns;

   433

        dc_delta_total_ns += delta;

   434

        dc_filter_idx++;

   435

   436

        if (dc_filter_idx >= DC_FILTER_CNT) {

   437

            dc_adjust_ns += dc_delta_total_ns >= 0 ?

   438

                    ((dc_delta_total_ns + (DC_FILTER_CNT / 2)) / DC_FILTER_CNT) :

   439

                    ((dc_delta_total_ns - (DC_FILTER_CNT / 2)) / DC_FILTER_CNT) ;

   440

   441

            // and add adjustment for general diff (to pull in drift)

   442

            dc_adjust_ns += sign(dc_diff_total_ns / DC_FILTER_CNT);

   443

   444

            // limit crazy numbers (0.1%% of std cycle time)

   445

            if (dc_adjust_ns < -1000) {

   446

                dc_adjust_ns = -1000;

   447

            }

   448

            if (dc_adjust_ns > 1000) {

   449

                dc_adjust_ns =  1000;

   450

            }

   451

            // reset

   452

            dc_diff_total_ns = 0LL;

   453

            dc_delta_total_ns = 0LL;

   454

            dc_filter_idx = 0;

   455

        }

   456

        // add cycles adjustment to time base (including a spot adjustment)

   457

        system_time_base += dc_adjust_ns + sign(dc_diff_ns);

   458

    }

   459

    else {

   460

        dc_started = (dc_diff_ns != 0);

   461

   462

        if (dc_started) {

   463

#if DC_ENABLE && DEBUG_MODE

   464

            // output first diff

   465

            fprintf(stderr, "First master diff: %%d\n", dc_diff_ns);

   466

#endif

   467

            // record the time of this initial cycle

   468

            dc_start_time_ns = dc_time_ns;

   469

        }

   470

    }

   471

}

   472

   473

/*****************************************************************************/

   474

   475

/*

   476

 * Calculate the sleeptime

   477

 */

   478

RTIME calculate_sleeptime(uint64_t wakeup_time)

   479

{

   480

    RTIME wakeup_count = system2count (wakeup_time);

   481

    RTIME current_count = rt_timer_read();

   482

   483

    if ((wakeup_count < current_count) || (wakeup_count > current_count + (50 * frame_period_ns)))  {

   484

        fprintf(stderr, "%%s(): unexpected wake time! wc = %%lld\tcc = %%lld\n", __func__, wakeup_count, current_count);

   485

    }

   486

   487

    return wakeup_count;

   488

}

   489

   490

/*****************************************************************************/

   491

   492

/*

   493

 * Calculate the sleeptime

   494

 */

   495

uint64_t calculate_first(void)