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

    35

    36

#include <sched.h>

    37

#include <stdio.h>

    38

#include <stdlib.h>

    39

#include <fcntl.h>

    40

#include <signal.h>

    41

    42

#include <rtai_lxrt.h>

    43

#include <rtdm/rtdm.h>

    44

    45

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

    46

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

    47

    48

    49

    50

#define rt_printf(X,Y) 

    51

    52

#define NSEC_PER_SEC 1000000000

    53

    54

RT_TASK* task;    // Stores a handle.

    55

int priority=0;   // Highest

    56

int stack_size=0; // Use default (512)

    57

int msg_size=0;   // Use default (256)

    58

    59

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

    60

    61

int rt_fd = -1;

    62

int run=0;

    63

    64

unsigned int sync_ref_counter = 0;

    65

    66

CstructMstrAttach MstrAttach;

    67

    68

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

    69

    70

    71

    72

    73

// Optional features

    74

#define CONFIGURE_PDOS  1

    75

    76

    77

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

    78

    79

// EtherCAT

    80

static ec_master_t *master = NULL;

    81

static ec_master_state_t master_state = {};

    82

    83

static ec_domain_t *domain1 = NULL;

    84

static ec_domain_state_t domain1_state = {};

    85

    86

    87

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

    88

static uint8_t *domain1_pd = NULL;

    89

    90

// process data

    91

    92

#define BusCoupler01_Pos    0, 0

    93

#define DigOutSlave01_Pos   0, 1

    94

#define DigOutSlave02_Pos   0, 2

    95

#define DigInSlave01_Pos    0, 3

    96

#define AnaOutSlave01_Pos   0, 4

    97

#define AnaInSlave01_Pos    0, 5

    98

#define BusCoupler02_Pos    0, 6

    99

#define AnaInSlave02_Pos    0, 7

   100

   101

   102

#define Beckhoff_EK1100 0x00000002, 0x044c2c52

   103

#define Beckhoff_EL1014 0x00000002, 0x03f63052

   104

#define Beckhoff_EL2004 0x00000002, 0x07d43052

   105

#define Beckhoff_EL4132 0x00000002, 0x10243052

   106

#define Beckhoff_EL3102 0x00000002, 0x0c1e3052

   107

#define Beckhoff_EL4102 0x00000002, 0x10063052

   108

#define Beckhoff_EL6731 0x00000002, 0x1a4b3052

   109

#define Beckhoff_EL6600 0x00000002, 0x19c93052

   110

#define Beckhoff_EL3602 0x00000002, 0x0e123052

   111

#define Beckhoff_EL5151 0x00000002, 0x141f3052

   112

   113

   114

// offsets for PDO entries

   115

static unsigned int off_dig_out0      = 0;

   116

static unsigned int off_dig_out1      = 0;

   117

static unsigned int off_dig_out2      = 0;

   118

static unsigned int off_dig_out3      = 0;

   119

static unsigned int off_dig_in0       = 0;

   120

static unsigned int off_ana_out0      = 0;

   121

static unsigned int off_ana_out1      = 0;

   122

static unsigned int off_ana_in0_status = 0;

   123

static unsigned int off_ana_in0_value  = 0;

   124

static unsigned int off_ana_in1_status = 0;

   125

static unsigned int off_ana_in1_value  = 0;

   126

   127

   128

// process data

   129

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

   130

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

   131

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

   132

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

   133

   134

const static ec_pdo_entry_reg_t domain1_regs[] = {

   135

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

   136

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

   137

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

   138

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

   139

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

   140

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

   141

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

   142

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

   143

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

   144

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

   145

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

   146

   {}

   147

};

   148

   149

char rt_dev_file[64];

   150

static unsigned int blink = 0;

   151

   152

static ec_slave_config_t *sc_dig_out_01 = NULL;

   153

   154

static ec_slave_config_t *sc_dig_out_02 = NULL;

   155

   156

static ec_slave_config_t *sc_dig_in_01 = NULL;

   157

   158

static ec_slave_config_t *sc_ana_out_01 = NULL;

   159

   160

static ec_slave_config_t *sc_ana_in_01 = NULL;

   161

   162

static ec_slave_config_t *sc_ana_in_02 = NULL;

   163

   164

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

   165

   166

/* Slave 1, "EL2004"

   167

 * Vendor ID:       0x00000002

   168

 * Product code:    0x07d43052

   169

 * Revision number: 0x00100000

   170

 */

   171

   172

ec_pdo_entry_info_t slave_1_pdo_entries[] = {

   173

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

   174

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

   175

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

   176

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

   177

};

   178

   179

ec_pdo_info_t slave_1_pdos[] = {

   180

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

   181

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

   182

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

   183

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

   184

};

   185

   186

ec_sync_info_t slave_1_syncs[] = {

   187

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

   188

   {0xff}

   189

};

   190

   191

/* Slave 2, "EL2004"

   192

 * Vendor ID:       0x00000002

   193

 * Product code:    0x07d43052

   194

 * Revision number: 0x00100000

   195

 */

   196

   197

ec_pdo_entry_info_t slave_2_pdo_entries[] = {

   198

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

   199

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

   200

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

   201

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

   202

};

   203

   204

ec_pdo_info_t slave_2_pdos[] = {

   205

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

   206

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

   207

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

   208

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

   209

};

   210

   211

ec_sync_info_t slave_2_syncs[] = {

   212

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

   213

   {0xff}

   214

};

   215

   216

/* Slave 3, "EL1014"

   217

 * Vendor ID:       0x00000002

   218

 * Product code:    0x03f63052

   219

 * Revision number: 0x00100000

   220

 */

   221

   222

ec_pdo_entry_info_t slave_3_pdo_entries[] = {

   223

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

   224

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

   225

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

   226

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

   227

};

   228

   229

ec_pdo_info_t slave_3_pdos[] = {

   230

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

   231

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

   232

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

   233

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

   234

};

   235

   236

ec_sync_info_t slave_3_syncs[] = {

   237

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

   238

   {0xff}

   239

};

   240

   241

/* Slave 4, "EL4132"

   242

 * Vendor ID:       0x00000002

   243

 * Product code:    0x10243052

   244

 * Revision number: 0x03f90000

   245

 */

   246

   247

ec_pdo_entry_info_t slave_4_pdo_entries[] = {

   248

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

   249

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

   250

};

   251

   252

ec_pdo_info_t slave_4_pdos[] = {

   253

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

   254

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

   255

};

   256

   257

ec_sync_info_t slave_4_syncs[] = {

   258

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

   259

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

   260

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

   261

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

   262

   {0xff}

   263

};

   264

   265

/* Slave 5, "EL3102"

   266

 * Vendor ID:       0x00000002

   267

 * Product code:    0x0c1e3052

   268

 * Revision number: 0x00000000

   269

 */

   270

   271

ec_pdo_entry_info_t slave_5_pdo_entries[] = {

   272

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

   273

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

   274

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

   275

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

   276

};

   277

   278

ec_pdo_info_t slave_5_pdos[] = {

   279

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

   280

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

   281

};

   282

   283

ec_sync_info_t slave_5_syncs[] = {

   284

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

   285

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

   286

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

   287

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

   288

   {0xff}

   289

};

   290

   291

/* Slave 6, "EL6731-0010"

   292

 * Vendor ID:       0x00000002

   293

 * Product code:    0x1a4b3052

   294

 * Revision number: 0x0011000a

   295

 */

   296

   297

ec_sync_info_t slave_6_syncs[] = {

   298

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

   299

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

   300

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

   301

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

   302

};

   303

   304

   305

/* Slave 7, "EL6601"

   306

 * Vendor ID:       0x00000002

   307

 * Product code:    0x19c93052

   308

 * Revision number: 0x00110000

   309

 */

   310

/*

   311

ec_sync_info_t slave_7_syncs[] = {

   312

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

   313

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

   314

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

   315

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

   316

   {0xff}

   317

};

   318

*/

   319

   320

/* Master 0, Slave 7, "EL3602"

   321

 * Vendor ID:       0x00000002

   322

 * Product code:    0x0e123052

   323

 * Revision number: 0x00100000

   324

 */

   325

ec_pdo_entry_info_t slave_7_pdo_entries[] = {

   326

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

   327

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

   328

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

   329

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

   330

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

   331

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

   332

   {0x1800, 0x07, 1},

   333

   {0x1800, 0x09, 1},

   334

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

   335

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

   336

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

   337

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

   338

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

   339

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

   340

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

   341

   {0x1801, 0x07, 1},

   342

   {0x1801, 0x09, 1},

   343

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

   344

};

   345

   346

ec_pdo_info_t slave_7_pdos[] = {

   347

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

   348

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

   349

};

   350

   351

ec_sync_info_t slave_7_syncs[] = {

   352

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

   353

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

   354

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

   355

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

   356

   {0xff}

   357

};

   358

   359

/* Master 0, Slave 8, "EL5151"

   360

 * Vendor ID:       0x00000002

   361

 * Product code:    0x141f3052

   362

 * Revision number: 0x00130000

   363

 */

   364

   365

ec_pdo_entry_info_t slave_8_pdo_entries[] = {

   366

   {0x6000, 0x01, 1},

   367

   {0x6000, 0x02, 1},

   368

   {0x6000, 0x03, 1},

   369

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

   370

   {0x6000, 0x08, 1},

   371

   {0x6000, 0x09, 1},

   372

   {0x6000, 0x0a, 1},

   373

   {0x6000, 0x0b, 1},

   374

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

   375

   {0x6000, 0x0d, 1},

   376

   {0x1c32, 0x20, 1},

   377

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

   378

   {0x1800, 0x09, 1},

   379

   {0x6000, 0x11, 32},

   380

   {0x6000, 0x12, 32},

   381

   {0x6000, 0x14, 32},

   382

};

   383

   384

ec_pdo_info_t slave_8_pdos[] = {

   385

   {0x0000, 0, NULL},

   386

   {0x1a00, 15, slave_8_pdo_entries + 0},

   387

   {0x1a02, 1, slave_8_pdo_entries + 15},

   388

};

   389

   390

ec_sync_info_t slave_8_syncs[] = {

   391

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

   392

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

   393

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

   394

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

   395

   {0xff}

   396

};

   397

   398

   399

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

   400

   401

   402

void rt_check_domain_state(void)

   403

{

   404

    ec_domain_state_t ds;

   405

   406

    if (rt_fd>=0)

   407

      {

   408

          ecrt_rtdm_domain_state(rt_fd,&ds);  

   409

      }

   410

   411

    if (ds.working_counter != domain1_state.working_counter)

   412

     {

   413

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

   414

     }

   415

    if (ds.wc_state != domain1_state.wc_state)

   416

     {

   417

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

   418

     }

   419

   420

    domain1_state = ds;

   421

}

   422

   423

void rt_check_master_state(void)

   424

{

   425

    ec_master_state_t ms;

   426

   427

    if (rt_fd>=0)

   428

      {

   429

          ecrt_rtdm_master_state(rt_fd,&ms);

   430

      }

   431

   432

    if (ms.slaves_responding != master_state.slaves_responding)

   433

    {

   434

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

   435

    }

   436

    if (ms.al_states != master_state.al_states)

   437

    {

   438

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

   439

    }

   440

    if (ms.link_up != master_state.link_up)

   441

    {

   442

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

   443

    }

   444

    master_state = ms;

   445

}

   446

   447

   448

   449

   450

void rt_sync()

   451

{

   452

   453

  uint64_t now_ns;

   454

  now_ns =  rt_get_real_time_ns();

   455

   456

   457

  if (rt_fd>=0)

   458

  {

   459

      ecrt_rtdm_master_application_time(rt_fd, &now_ns);

   460

  }

   461

   462

  if (sync_ref_counter) {

   463

     sync_ref_counter--;

   464

  } else {

   465

     sync_ref_counter = 9;

   466

     if (rt_fd>=0)

   467

     {

   468

         ecrt_rtdm_master_sync_reference_clock(rt_fd);

   469

     }

   470

  }

   471

  if (rt_fd>=0)

   472

  {

   473

      ecrt_rtdm_master_sync_slave_clocks(rt_fd) ;

   474

  }

   475

}

   476

   477

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

   478

   479

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

   480

void cleanup_all(void)

   481

{

   482

    run = 0;   

   483

}

   484

   485

void catch_signal(int sig)

   486

{

   487

    cleanup_all();    

   488

}

   489

   490

   491

   492

   493

   494

void my_cyclic(void)

   495

{

   496

   497

    int counter = 0;

   498

    int divcounter = 0;

   499

    int divider = 10;

   500

    int period;

   501

   502

    rt_set_periodic_mode();

   503

    period = (int) nano2count((RTIME)cycle*1000);

   504

    start_rt_timer(period);

   505

    rt_make_hard_real_time();

   506

   507

   508

    while(1) {

   509

   510

        rt_task_wait_period();

   511

   512

   513

        counter++;

   514

        if (counter>60000) {

   515

            run=0;

   516

        }

   517

   518

        if(run ==  0) {

   519

            rt_make_soft_real_time();

   520

            return;

   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();

   529

   530

        if (divcounter ==0)