1

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

     2

 *

     3

 *  $Id$

     4

 *

     5

 *  main.c	        Copyright (C) 2011       IgH Andreas Stewering-Bone

     6

 *

     7

 *  This file is part of ethercatrtdm interface to IgH EtherCAT master 

     8

 *  

     9

 *  The IgH EtherCAT master is free software; you can

    10

 *  redistribute it and/or modify it under the terms of the GNU Lesser General

    11

 *  Public License as published by the Free Software Foundation; version 2.1

    12

 *  of the License.

    13

 *

    14

 *  The IgH EtherCAT Master is free software; you can redistribute it and/or

    15

 *  modify it under the terms of the GNU General Public License version 2, as

    16

 *  published by the Free Software Foundation.

    17

 *

    18

 *  The IgH EtherCAT master userspace library is distributed in the hope that

    19

 *  it will be useful, but WITHOUT ANY WARRANTY; without even the implied

    20

 *  warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

    21

 *  GNU Lesser General Public License for more details.

    22

 *

    23

 *  You should have received a copy of the GNU Lesser General Public License

    24

 *  along with the IgH EtherCAT master userspace library. If not, see

    25

 *  <http://www.gnu.org/licenses/>.

    26

 *  

    27

 *  The license mentioned above concerns the source code only. Using the

    28

 *  EtherCAT technology and brand is only permitted in compliance with the

    29

 *  industrial property and similar rights of Beckhoff Automation GmbH.

    30

 *

    31

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

    32

    33

    34

#include <errno.h>

    35

#include <mqueue.h>

    36

#include <signal.h>

    37

#include <pthread.h>

    38

#include <stdio.h>

    39

#include <stdlib.h>

    40

#include <string.h>

    41

#include <unistd.h>

    42

#include <limits.h>

    43

#include <sys/ioctl.h>

    44

#include <sys/mman.h>

    45

#include <time.h>

    46

    47

#include <rtdm/rtdm.h>

    48

#include <rtdk.h>

    49

    50

#include "../../include/ecrt.h"

    51

#include "../../include/ec_rtdm.h"

    52

    53

#define NSEC_PER_SEC 1000000000

    54

    55

static unsigned int cycle = 1000; /* 1 ms */

    56

    57

static pthread_t cyclicthread;

    58

    59

int rt_fd = -1;

    60

int run=0;

    61

    62

unsigned int sync_ref_counter = 0;

    63

    64

CstructMstrAttach MstrAttach;

    65

    66

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

    67

    68

    69

    70

    71

// Optional features

    72

#define CONFIGURE_PDOS  1

    73

    74

    75

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

    76

    77

// EtherCAT

    78

static ec_master_t *master = NULL;

    79

static ec_master_state_t master_state = {};

    80

    81

static ec_domain_t *domain1 = NULL;

    82

static ec_domain_state_t domain1_state = {};

    83

    84

    85

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

    86

static uint8_t *domain1_pd = NULL;

    87

    88

// process data

    89

    90

#define BusCoupler01_Pos    0, 0

    91

#define DigOutSlave01_Pos   0, 1

    92

#define DigOutSlave02_Pos   0, 2

    93

#define DigInSlave01_Pos    0, 3

    94

#define AnaOutSlave01_Pos   0, 4

    95

#define AnaInSlave01_Pos    0, 5

    96

#define BusCoupler02_Pos    0, 6

    97

#define AnaInSlave02_Pos    0, 7

    98

    99

   100

#define Beckhoff_EK1100 0x00000002, 0x044c2c52

   101

#define Beckhoff_EL1014 0x00000002, 0x03f63052

   102

#define Beckhoff_EL2004 0x00000002, 0x07d43052

   103

#define Beckhoff_EL4132 0x00000002, 0x10243052

   104

#define Beckhoff_EL3102 0x00000002, 0x0c1e3052

   105

#define Beckhoff_EL4102 0x00000002, 0x10063052

   106

#define Beckhoff_EL6731 0x00000002, 0x1a4b3052

   107

#define Beckhoff_EL6600 0x00000002, 0x19c93052

   108

#define Beckhoff_EL3602 0x00000002, 0x0e123052

   109

#define Beckhoff_EL5151 0x00000002, 0x141f3052

   110

   111

   112

// offsets for PDO entries

   113

static unsigned int off_dig_out0      = 0;

   114

static unsigned int off_dig_out1      = 0;

   115

static unsigned int off_dig_out2      = 0;

   116

static unsigned int off_dig_out3      = 0;

   117

static unsigned int off_dig_in0       = 0;

   118

static unsigned int off_ana_out0      = 0;

   119

static unsigned int off_ana_out1      = 0;

   120

static unsigned int off_ana_in0_status = 0;

   121

static unsigned int off_ana_in0_value  = 0;

   122

static unsigned int off_ana_in1_status = 0;

   123

static unsigned int off_ana_in1_value  = 0;

   124

   125

   126

// process data

   127

unsigned int bit_position0=0; /* Pointer to a variable to store a bit */

   128

unsigned int bit_position1=0; /* Pointer to a variable to store a bit */

   129

unsigned int bit_position2=0; /* Pointer to a variable to store a bit */

   130

unsigned int bit_position3=0; /* Pointer to a variable to store a bit */

   131

   132

const static ec_pdo_entry_reg_t domain1_regs[] = {

   133

   {DigOutSlave01_Pos, Beckhoff_EL2004, 0x7000, 0x01, &off_dig_out0, &bit_position0},

   134

   {DigOutSlave01_Pos, Beckhoff_EL2004, 0x7010, 0x01, &off_dig_out1, &bit_position1},

   135

   {DigOutSlave01_Pos, Beckhoff_EL2004, 0x7020, 0x01, &off_dig_out2, &bit_position2},

   136

   {DigOutSlave01_Pos, Beckhoff_EL2004, 0x7030, 0x01, &off_dig_out3, &bit_position3},

   137

   {DigInSlave01_Pos,  Beckhoff_EL1014, 0x6000, 0x01, &off_dig_in0},

   138

   {AnaOutSlave01_Pos, Beckhoff_EL4132, 0x3001, 0x01, &off_ana_out0},

   139

   {AnaOutSlave01_Pos, Beckhoff_EL4132, 0x3002, 0x01, &off_ana_out1},

   140

   {AnaInSlave01_Pos,  Beckhoff_EL3102, 0x3101, 0x01, &off_ana_in0_status},

   141

   {AnaInSlave01_Pos,  Beckhoff_EL3102, 0x3101, 0x02, &off_ana_in0_value},

   142

   {AnaInSlave01_Pos,  Beckhoff_EL3102, 0x3102, 0x01, &off_ana_in1_status},

   143

   {AnaInSlave01_Pos,  Beckhoff_EL3102, 0x3102, 0x02, &off_ana_in1_value},

   144

   {}

   145

};

   146

   147

char rt_dev_file[64];

   148

static unsigned int blink = 0;

   149

   150

static ec_slave_config_t *sc_dig_out_01 = NULL;

   151

   152

static ec_slave_config_t *sc_dig_out_02 = NULL;

   153

   154

static ec_slave_config_t *sc_dig_in_01 = NULL;

   155

   156

static ec_slave_config_t *sc_ana_out_01 = NULL;

   157

   158

static ec_slave_config_t *sc_ana_in_01 = NULL;

   159

   160

static ec_slave_config_t *sc_ana_in_02 = NULL;

   161

   162

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

   163

   164

/* Slave 1, "EL2004"

   165

 * Vendor ID:       0x00000002

   166

 * Product code:    0x07d43052

   167

 * Revision number: 0x00100000

   168

 */

   169

   170

ec_pdo_entry_info_t slave_1_pdo_entries[] = {

   171

   {0x7000, 0x01, 1}, /* Output */

   172

   {0x7010, 0x01, 1}, /* Output */

   173

   {0x7020, 0x01, 1}, /* Output */

   174

   {0x7030, 0x01, 1}, /* Output */

   175

};

   176

   177

ec_pdo_info_t slave_1_pdos[] = {

   178

   {0x1600, 1, slave_1_pdo_entries + 0}, /* Channel 1 */

   179

   {0x1601, 1, slave_1_pdo_entries + 1}, /* Channel 2 */

   180

   {0x1602, 1, slave_1_pdo_entries + 2}, /* Channel 3 */

   181

   {0x1603, 1, slave_1_pdo_entries + 3}, /* Channel 4 */

   182

};

   183

   184

ec_sync_info_t slave_1_syncs[] = {

   185

   {0, EC_DIR_OUTPUT, 4, slave_1_pdos + 0, EC_WD_ENABLE},

   186

   {0xff}

   187

};

   188

   189

/* Slave 2, "EL2004"

   190

 * Vendor ID:       0x00000002

   191

 * Product code:    0x07d43052

   192

 * Revision number: 0x00100000

   193

 */

   194

   195

ec_pdo_entry_info_t slave_2_pdo_entries[] = {

   196

   {0x7000, 0x01, 1}, /* Output */

   197

   {0x7010, 0x01, 1}, /* Output */

   198

   {0x7020, 0x01, 1}, /* Output */

   199

   {0x7030, 0x01, 1}, /* Output */

   200

};

   201

   202

ec_pdo_info_t slave_2_pdos[] = {

   203

   {0x1600, 1, slave_2_pdo_entries + 0}, /* Channel 1 */

   204

   {0x1601, 1, slave_2_pdo_entries + 1}, /* Channel 2 */

   205

   {0x1602, 1, slave_2_pdo_entries + 2}, /* Channel 3 */

   206

   {0x1603, 1, slave_2_pdo_entries + 3}, /* Channel 4 */

   207

};

   208

   209

ec_sync_info_t slave_2_syncs[] = {

   210

   {0, EC_DIR_OUTPUT, 4, slave_2_pdos + 0, EC_WD_ENABLE},

   211

   {0xff}

   212

};

   213

   214

/* Slave 3, "EL1014"

   215

 * Vendor ID:       0x00000002

   216

 * Product code:    0x03f63052

   217

 * Revision number: 0x00100000

   218

 */

   219

   220

ec_pdo_entry_info_t slave_3_pdo_entries[] = {

   221

   {0x6000, 0x01, 1}, /* Input */

   222

   {0x6010, 0x01, 1}, /* Input */

   223

   {0x6020, 0x01, 1}, /* Input */

   224

   {0x6030, 0x01, 1}, /* Input */

   225

};

   226

   227

ec_pdo_info_t slave_3_pdos[] = {

   228

   {0x1a00, 1, slave_3_pdo_entries + 0}, /* Channel 1 */

   229

   {0x1a01, 1, slave_3_pdo_entries + 1}, /* Channel 2 */

   230

   {0x1a02, 1, slave_3_pdo_entries + 2}, /* Channel 3 */

   231

   {0x1a03, 1, slave_3_pdo_entries + 3}, /* Channel 4 */

   232

};

   233

   234

ec_sync_info_t slave_3_syncs[] = {

   235

   {0, EC_DIR_INPUT, 4, slave_3_pdos + 0, EC_WD_DISABLE},

   236

   {0xff}

   237

};

   238

   239

/* Slave 4, "EL4132"

   240

 * Vendor ID:       0x00000002

   241

 * Product code:    0x10243052

   242

 * Revision number: 0x03f90000

   243

 */

   244

   245

ec_pdo_entry_info_t slave_4_pdo_entries[] = {

   246

   {0x3001, 0x01, 16}, /* Output */

   247

   {0x3002, 0x01, 16}, /* Output */

   248

};

   249

   250

ec_pdo_info_t slave_4_pdos[] = {

   251

   {0x1600, 1, slave_4_pdo_entries + 0}, /* RxPDO 01 mapping */

   252

   {0x1601, 1, slave_4_pdo_entries + 1}, /* RxPDO 02 mapping */

   253

};

   254

   255

ec_sync_info_t slave_4_syncs[] = {

   256

   {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},

   257

   {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},

   258

   {2, EC_DIR_OUTPUT, 2, slave_4_pdos + 0, EC_WD_DISABLE},

   259

   {3, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},

   260

   {0xff}

   261

};

   262

   263

/* Slave 5, "EL3102"

   264

 * Vendor ID:       0x00000002

   265

 * Product code:    0x0c1e3052

   266

 * Revision number: 0x00000000

   267

 */

   268

   269

ec_pdo_entry_info_t slave_5_pdo_entries[] = {

   270

   {0x3101, 0x01, 8}, /* Status */

   271

   {0x3101, 0x02, 16}, /* Value */

   272

   {0x3102, 0x01, 8}, /* Status */

   273

   {0x3102, 0x02, 16}, /* Value */

   274

};

   275

   276

ec_pdo_info_t slave_5_pdos[] = {

   277

   {0x1a00, 2, slave_5_pdo_entries + 0}, /* TxPDO 001 mapping */

   278

   {0x1a01, 2, slave_5_pdo_entries + 2}, /* TxPDO 002 mapping */

   279

};

   280

   281

ec_sync_info_t slave_5_syncs[] = {

   282

   {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},

   283

   {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},

   284

   {2, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},

   285

   {3, EC_DIR_INPUT, 2, slave_5_pdos + 0, EC_WD_DISABLE},

   286

   {0xff}

   287

};

   288

   289

/* Slave 6, "EL6731-0010"

   290

 * Vendor ID:       0x00000002

   291

 * Product code:    0x1a4b3052

   292

 * Revision number: 0x0011000a

   293

 */

   294

   295

ec_sync_info_t slave_6_syncs[] = {

   296

   {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},

   297

   {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},

   298

   {2, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},

   299

   {3, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},

   300

};

   301

   302

   303

/* Slave 7, "EL6601"

   304

 * Vendor ID:       0x00000002

   305

 * Product code:    0x19c93052

   306

 * Revision number: 0x00110000

   307

 */

   308

/*

   309

ec_sync_info_t slave_7_syncs[] = {

   310

   {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},

   311

   {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},

   312

   {2, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},

   313

   {3, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},

   314

   {0xff}

   315

};

   316

*/

   317

   318

/* Master 0, Slave 7, "EL3602"

   319

 * Vendor ID:       0x00000002

   320

 * Product code:    0x0e123052

   321

 * Revision number: 0x00100000

   322

 */

   323

ec_pdo_entry_info_t slave_7_pdo_entries[] = {

   324

   {0x6000, 0x01, 1}, /* Underrange */

   325

   {0x6000, 0x02, 1}, /* Overrange */

   326

   {0x6000, 0x03, 2}, /* Limit 1 */

   327

   {0x6000, 0x05, 2}, /* Limit 2 */

   328

   {0x6000, 0x07, 1}, /* Error */

   329

   {0x0000, 0x00, 7}, /* Gap */

   330

   {0x1800, 0x07, 1},

   331

   {0x1800, 0x09, 1},

   332

   {0x6000, 0x11, 32}, /* Value */

   333

   {0x6010, 0x01, 1}, /* Underrange */

   334

   {0x6010, 0x02, 1}, /* Overrange */

   335

   {0x6010, 0x03, 2}, /* Limit 1 */

   336

   {0x6010, 0x05, 2}, /* Limit 2 */

   337

   {0x6010, 0x07, 1}, /* Error */

   338

   {0x0000, 0x00, 7}, /* Gap */

   339

   {0x1801, 0x07, 1},

   340

   {0x1801, 0x09, 1},

   341

   {0x6010, 0x11, 32}, /* Value */

   342

};

   343

   344

ec_pdo_info_t slave_7_pdos[] = {

   345

   {0x1a00, 9, slave_7_pdo_entries + 0}, /* AI TxPDO-Map Inputs Ch.1 */

   346

   {0x1a01, 9, slave_7_pdo_entries + 9}, /* AI TxPDO-Map Inputs Ch.2 */

   347

};

   348

   349

ec_sync_info_t slave_7_syncs[] = {

   350

   {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},

   351

   {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},

   352

   {2, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},

   353

   {3, EC_DIR_INPUT, 2, slave_7_pdos + 0, EC_WD_DISABLE},

   354

   {0xff}

   355

};

   356

   357

/* Master 0, Slave 8, "EL5151"

   358

 * Vendor ID:       0x00000002

   359

 * Product code:    0x141f3052

   360

 * Revision number: 0x00130000

   361

 */

   362

   363

ec_pdo_entry_info_t slave_8_pdo_entries[] = {

   364

   {0x6000, 0x01, 1},

   365

   {0x6000, 0x02, 1},

   366

   {0x6000, 0x03, 1},

   367

   {0x0000, 0x00, 4}, /* Gap */

   368

   {0x6000, 0x08, 1},

   369

   {0x6000, 0x09, 1},

   370

   {0x6000, 0x0a, 1},

   371

   {0x6000, 0x0b, 1},

   372

   {0x0000, 0x00, 1}, /* Gap */

   373

   {0x6000, 0x0d, 1},

   374

   {0x1c32, 0x20, 1},

   375

   {0x0000, 0x00, 1}, /* Gap */

   376

   {0x1800, 0x09, 1},

   377

   {0x6000, 0x11, 32},

   378

   {0x6000, 0x12, 32},

   379

   {0x6000, 0x14, 32},

   380

};

   381

   382

ec_pdo_info_t slave_8_pdos[] = {

   383

   {0x0000, 0, NULL},

   384

   {0x1a00, 15, slave_8_pdo_entries + 0},

   385

   {0x1a02, 1, slave_8_pdo_entries + 15},

   386

};

   387

   388

ec_sync_info_t slave_8_syncs[] = {

   389

   {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},

   390

   {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},

   391

   {2, EC_DIR_OUTPUT, 1, slave_8_pdos + 0, EC_WD_DISABLE},

   392

   {3, EC_DIR_INPUT, 2, slave_8_pdos + 1, EC_WD_DISABLE},

   393

   {0xff}

   394

};

   395

   396

   397

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

   398

   399

   400

void rt_check_domain_state(void)

   401

{

   402

    ec_domain_state_t ds;

   403

   404

    if (rt_fd>=0)

   405

      {

   406

          ecrt_rtdm_domain_state(rt_fd,&ds);  

   407

      }

   408

   409

    if (ds.working_counter != domain1_state.working_counter)

   410

     {

   411

        rt_printf("Domain1: WC %u.\n", ds.working_counter);

   412

     }

   413

    if (ds.wc_state != domain1_state.wc_state)

   414

     {

   415

    	rt_printf("Domain1: State %u.\n", ds.wc_state);

   416

     }

   417

   418

    domain1_state = ds;

   419

}

   420

   421

void rt_check_master_state(void)

   422

{

   423

    ec_master_state_t ms;

   424

   425

    if (rt_fd>=0)

   426

      {

   427

          ecrt_rtdm_master_state(rt_fd,&ms);

   428

      }

   429

   430

    if (ms.slaves_responding != master_state.slaves_responding)

   431

    {

   432

        rt_printf("%u slave(s).\n", ms.slaves_responding);

   433

    }

   434

    if (ms.al_states != master_state.al_states)

   435

    {

   436

        rt_printf("AL states: 0x%02X.\n", ms.al_states);

   437

    }

   438

    if (ms.link_up != master_state.link_up)

   439

    {

   440

        rt_printf("Link is %s.\n", ms.link_up ? "up" : "down");

   441

    }

   442

    master_state = ms;

   443

}

   444

   445

   446

   447

   448

void rt_sync()

   449

{

   450

  struct timespec now;

   451

  uint64_t now_ns;

   452

  clock_gettime(CLOCK_REALTIME,&now);

   453

   454

  now_ns = 1000000000LL*now.tv_sec + now.tv_nsec;

   455

   456

  if (rt_fd>=0)

   457

  {

   458

      ecrt_rtdm_master_application_time(rt_fd, &now_ns);

   459

  }

   460

   461

  if (sync_ref_counter) {

   462

     sync_ref_counter--;

   463

  } else {

   464

     sync_ref_counter = 9;

   465

     if (rt_fd>=0)

   466

     {

   467

         ecrt_rtdm_master_sync_reference_clock(rt_fd);

   468

     }

   469

  }

   470

  if (rt_fd>=0)

   471

  {

   472

      ecrt_rtdm_master_sync_slave_clocks(rt_fd) ;

   473

  }

   474

}

   475

   476

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

   477

   478

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

   479

void cleanup_all(void)

   480

{

   481

    run = 0;   

   482

}

   483

   484

void catch_signal(int sig)

   485

{

   486

    cleanup_all();    

   487

}

   488

   489

   490

   491

   492

   493

void *my_thread(void *arg)

   494

{

   495

    struct timespec next_period;

   496

   497

    int counter = 0;

   498

    int divcounter = 0;

   499

    int divider = 10;

   500

   501

   502

    clock_gettime(CLOCK_REALTIME, &next_period);

   503

    while(1) {

   504

        next_period.tv_nsec += cycle * 1000;

   505

        while (next_period.tv_nsec >= NSEC_PER_SEC) {

   506

                next_period.tv_nsec -= NSEC_PER_SEC;

   507

                next_period.tv_sec++;

   508

                }

   509

   510

        clock_nanosleep(CLOCK_REALTIME, TIMER_ABSTIME, &next_period, NULL);

   511

   512

   513

        counter++;

   514

        if (counter>60000) {

   515

            run=0;

   516

            return NULL;

   517

        }

   518

   519

        if(run ==  0) {

   520

            return NULL;

   521

        }

   522

   523

   524

        // receive ethercat

   525

        ecrt_rtdm_master_recieve(rt_fd);

   526

        ecrt_rtdm_domain_process(rt_fd);

   527

   528

        rt_check_domain_state();