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 <getopt.h>

    44

#include <netinet/in.h>

    45

#include <net/if.h>

    46

#include <sys/ioctl.h>

    47

#include <sys/mman.h>

    48

#include <time.h>

    49

    50

#include <rtdm/rtdm.h>

    51

#include <rtdk.h>

    52

    53

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

    54

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

    55

    56

#define NSEC_PER_SEC 1000000000

    57

    58

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

    59

    60

static pthread_t cyclicthread;

    61

    62

int rt_fd = -1;

    63

int run=0;

    64

    65

unsigned int sync_ref_counter = 0;

    66

    67

CstructMstrAttach MstrAttach;

    68

    69

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

    70

    71

    72

    73

    74

// Optional features

    75

#define CONFIGURE_PDOS  1

    76

    77

    78

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

    79

    80

// EtherCAT

    81

static ec_master_t *master = NULL;

    82

static ec_master_state_t master_state = {};

    83

    84

static ec_domain_t *domain1 = NULL;

    85

static ec_domain_state_t domain1_state = {};

    86

    87

    88

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

    89

static uint8_t *domain1_pd = NULL;

    90

    91

// process data

    92

    93

#define BusCoupler01_Pos    0, 0

    94

#define DigOutSlave01_Pos   0, 1

    95

#define DigOutSlave02_Pos   0, 2

    96

#define DigInSlave01_Pos    0, 3

    97

#define AnaOutSlave01_Pos   0, 4

    98

#define AnaInSlave01_Pos    0, 5

    99

#define BusCoupler02_Pos    0, 6

   100

#define AnaInSlave02_Pos    0, 7

   101

   102

   103

#define Beckhoff_EK1100 0x00000002, 0x044c2c52

   104

#define Beckhoff_EL1014 0x00000002, 0x03f63052

   105

#define Beckhoff_EL2004 0x00000002, 0x07d43052

   106

#define Beckhoff_EL4132 0x00000002, 0x10243052

   107

#define Beckhoff_EL3102 0x00000002, 0x0c1e3052

   108

#define Beckhoff_EL4102 0x00000002, 0x10063052

   109

#define Beckhoff_EL6731 0x00000002, 0x1a4b3052

   110

#define Beckhoff_EL6600 0x00000002, 0x19c93052

   111

#define Beckhoff_EL3602 0x00000002, 0x0e123052

   112

#define Beckhoff_EL5151 0x00000002, 0x141f3052

   113

   114

   115

// offsets for PDO entries

   116

static unsigned int off_dig_out0      = 0;

   117

static unsigned int off_dig_out1      = 0;

   118

static unsigned int off_dig_out2      = 0;

   119

static unsigned int off_dig_out3      = 0;

   120

static unsigned int off_dig_in0       = 0;

   121

static unsigned int off_ana_out0      = 0;

   122

static unsigned int off_ana_out1      = 0;

   123

static unsigned int off_ana_in0_status = 0;

   124

static unsigned int off_ana_in0_value  = 0;

   125

static unsigned int off_ana_in1_status = 0;

   126

static unsigned int off_ana_in1_value  = 0;

   127

   128

   129

// process data

   130

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

   131

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

   132

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

   133

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

   134

   135

const static ec_pdo_entry_reg_t domain1_regs[] = {

   136

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

   137

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

   138

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

   139

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

   140

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

   141

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

   142

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

   143

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

   144

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

   145

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

   146

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

   147

   {}

   148

};

   149

   150

char rt_dev_file[64];

   151

static unsigned int blink = 0;

   152

   153

static ec_slave_config_t *sc_dig_out_01 = NULL;

   154

   155

static ec_slave_config_t *sc_dig_out_02 = NULL;

   156

   157

static ec_slave_config_t *sc_dig_in_01 = NULL;

   158

   159

static ec_slave_config_t *sc_ana_out_01 = NULL;

   160

   161

static ec_slave_config_t *sc_ana_in_01 = NULL;

   162

   163

static ec_slave_config_t *sc_ana_in_02 = NULL;

   164

   165

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

   166

   167

/* Slave 1, "EL2004"

   168

 * Vendor ID:       0x00000002

   169

 * Product code:    0x07d43052

   170

 * Revision number: 0x00100000

   171

 */

   172

   173

ec_pdo_entry_info_t slave_1_pdo_entries[] = {

   174

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

   175

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

   176

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

   177

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

   178

};

   179

   180

ec_pdo_info_t slave_1_pdos[] = {

   181

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

   182

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

   183

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

   184

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

   185

};

   186

   187

ec_sync_info_t slave_1_syncs[] = {

   188

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

   189

   {0xff}

   190

};

   191

   192

/* Slave 2, "EL2004"

   193

 * Vendor ID:       0x00000002

   194

 * Product code:    0x07d43052

   195

 * Revision number: 0x00100000

   196

 */

   197

   198

ec_pdo_entry_info_t slave_2_pdo_entries[] = {

   199

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

   200

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

   201

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

   202

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

   203

};

   204

   205

ec_pdo_info_t slave_2_pdos[] = {

   206

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

   207

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

   208

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

   209

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

   210

};

   211

   212

ec_sync_info_t slave_2_syncs[] = {

   213

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

   214

   {0xff}

   215

};

   216

   217

/* Slave 3, "EL1014"

   218

 * Vendor ID:       0x00000002

   219

 * Product code:    0x03f63052

   220

 * Revision number: 0x00100000

   221

 */

   222

   223

ec_pdo_entry_info_t slave_3_pdo_entries[] = {

   224

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

   225

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

   226

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

   227

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

   228

};

   229

   230

ec_pdo_info_t slave_3_pdos[] = {

   231

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

   232

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

   233

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

   234

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

   235

};

   236

   237

ec_sync_info_t slave_3_syncs[] = {

   238

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

   239

   {0xff}

   240

};

   241

   242

/* Slave 4, "EL4132"

   243

 * Vendor ID:       0x00000002

   244

 * Product code:    0x10243052

   245

 * Revision number: 0x03f90000

   246

 */

   247

   248

ec_pdo_entry_info_t slave_4_pdo_entries[] = {

   249

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

   250

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

   251

};

   252

   253

ec_pdo_info_t slave_4_pdos[] = {

   254

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

   255

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

   256

};

   257

   258

ec_sync_info_t slave_4_syncs[] = {

   259

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

   260

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

   261

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

   262

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

   263

   {0xff}

   264

};

   265

   266

/* Slave 5, "EL3102"

   267

 * Vendor ID:       0x00000002

   268

 * Product code:    0x0c1e3052

   269

 * Revision number: 0x00000000

   270

 */

   271

   272

ec_pdo_entry_info_t slave_5_pdo_entries[] = {

   273

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

   274

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

   275

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

   276

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

   277

};

   278

   279

ec_pdo_info_t slave_5_pdos[] = {

   280

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

   281

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

   282

};

   283

   284

ec_sync_info_t slave_5_syncs[] = {

   285

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

   286

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

   287

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

   288

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

   289

   {0xff}

   290

};

   291

   292

/* Slave 6, "EL6731-0010"

   293

 * Vendor ID:       0x00000002

   294

 * Product code:    0x1a4b3052

   295

 * Revision number: 0x0011000a

   296

 */

   297

   298

ec_sync_info_t slave_6_syncs[] = {

   299

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

   300

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

   301

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

   302

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

   303

};

   304

   305

   306

/* Slave 7, "EL6601"

   307

 * Vendor ID:       0x00000002

   308

 * Product code:    0x19c93052

   309

 * Revision number: 0x00110000

   310

 */

   311

/*

   312

ec_sync_info_t slave_7_syncs[] = {

   313

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

   314

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

   315

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

   316

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

   317

   {0xff}

   318

};

   319

*/

   320

   321

/* Master 0, Slave 7, "EL3602"

   322

 * Vendor ID:       0x00000002

   323

 * Product code:    0x0e123052

   324

 * Revision number: 0x00100000

   325

 */

   326

ec_pdo_entry_info_t slave_7_pdo_entries[] = {

   327

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

   328

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

   329

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

   330

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

   331

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

   332

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

   333

   {0x1800, 0x07, 1},

   334

   {0x1800, 0x09, 1},

   335

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

   336

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

   337

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

   338

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

   339

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

   340

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

   341

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

   342

   {0x1801, 0x07, 1},

   343

   {0x1801, 0x09, 1},

   344

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

   345

};

   346

   347

ec_pdo_info_t slave_7_pdos[] = {

   348

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

   349

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

   350

};

   351

   352

ec_sync_info_t slave_7_syncs[] = {

   353

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

   354

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

   355

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

   356

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

   357

   {0xff}

   358

};

   359

   360

/* Master 0, Slave 8, "EL5151"

   361

 * Vendor ID:       0x00000002

   362

 * Product code:    0x141f3052

   363

 * Revision number: 0x00130000

   364

 */

   365

   366

ec_pdo_entry_info_t slave_8_pdo_entries[] = {

   367

   {0x6000, 0x01, 1},

   368

   {0x6000, 0x02, 1},

   369

   {0x6000, 0x03, 1},

   370

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

   371

   {0x6000, 0x08, 1},

   372

   {0x6000, 0x09, 1},

   373

   {0x6000, 0x0a, 1},

   374

   {0x6000, 0x0b, 1},

   375

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

   376

   {0x6000, 0x0d, 1},

   377

   {0x1c32, 0x20, 1},

   378

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

   379

   {0x1800, 0x09, 1},

   380

   {0x6000, 0x11, 32},

   381

   {0x6000, 0x12, 32},

   382

   {0x6000, 0x14, 32},

   383

};

   384

   385

ec_pdo_info_t slave_8_pdos[] = {

   386

   {0x0000, 0, NULL},

   387

   {0x1a00, 15, slave_8_pdo_entries + 0},

   388

   {0x1a02, 1, slave_8_pdo_entries + 15},

   389

};

   390

   391

ec_sync_info_t slave_8_syncs[] = {

   392

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

   393

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

   394

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

   395

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

   396

   {0xff}

   397

};

   398

   399

   400

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

   401

   402

   403

void rt_check_domain_state(void)

   404

{

   405

    ec_domain_state_t ds;

   406

   407

    if (rt_fd>=0)

   408

      {

   409

          ecrt_rtdm_domain_state(rt_fd,&ds);  

   410

      }

   411

   412

    if (ds.working_counter != domain1_state.working_counter)

   413

     {

   414

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

   415

     }

   416

    if (ds.wc_state != domain1_state.wc_state)

   417

     {

   418

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

   419

     }

   420

   421

    domain1_state = ds;

   422

}

   423

   424

void rt_check_master_state(void)

   425

{

   426

    ec_master_state_t ms;

   427

   428

    if (rt_fd>=0)

   429

      {

   430

          ecrt_rtdm_master_state(rt_fd,&ms);

   431

      }

   432

   433

    if (ms.slaves_responding != master_state.slaves_responding)

   434

    {

   435

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

   436

    }

   437

    if (ms.al_states != master_state.al_states)

   438

    {

   439

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

   440

    }

   441

    if (ms.link_up != master_state.link_up)

   442

    {

   443

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

   444

    }

   445

    master_state = ms;

   446

}

   447

   448

   449

   450

   451

void rt_sync()

   452

{

   453

  struct timespec now;

   454

  uint64_t now_ns;

   455

  clock_gettime(CLOCK_REALTIME,&now);

   456

   457

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

   458

   459

  if (rt_fd>=0)

   460

  {

   461

      ecrt_rtdm_master_application_time(rt_fd, &now_ns);

   462

  }

   463

   464

  if (sync_ref_counter) {

   465

     sync_ref_counter--;

   466

  } else {

   467

     sync_ref_counter = 9;

   468

     if (rt_fd>=0)

   469

     {

   470

         ecrt_rtdm_master_sync_reference_clock(rt_fd);

   471

     }

   472

  }

   473

  if (rt_fd>=0)

   474

  {

   475

      ecrt_rtdm_master_sync_slave_clocks(rt_fd) ;

   476

  }

   477

}

   478

   479

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

   480

   481

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

   482

void cleanup_all(void)

   483

{

   484

    printf("delete my_task\n");

   485

   486

    pthread_kill(cyclicthread, SIGHUP);

   487

    pthread_join(cyclicthread, NULL);

   488

   489

    if (rt_fd >= 0) {

   490

        printf("closing rt device %s\n", &rt_dev_file[0]);

   491

        rt_dev_close(rt_fd);

   492

   493

    }

   494

}

   495

   496

void catch_signal(int sig)

   497

{

   498

    cleanup_all();    

   499

}

   500

   501

   502

   503

   504

   505

void *my_thread(void *arg)

   506

{

   507

    struct sched_param param = { .sched_priority = 1 };

   508

    struct timespec next_period;

   509

    int counter = 0;

   510

    int divcounter = 0;

   511

    int divider = 10;

   512

    pthread_set_name_np(pthread_self(), "ec_xenomai_posix_test");

   513

    pthread_setschedparam(pthread_self(), SCHED_FIFO, &param);

   514

   515

    clock_gettime(CLOCK_MONOTONIC, &next_period);

   516

   517

    while(1) {

   518

        next_period.tv_nsec += cycle * 1000;

   519

        while (next_period.tv_nsec >= NSEC_PER_SEC) {

   520

                next_period.tv_nsec -= NSEC_PER_SEC;

   521

                next_period.tv_sec++;

   522

        }

   523

   524

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

   525

   526

        counter++;

   527

        if (counter>600000) {

   528

            run=0;

   529

            return NULL;

   530

        }