1

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

     2

 *

     3

 *  $Id$

     4

 *

     6

 *                                2011       IgH Andreas Stewering-Bone

     7

 *

     8

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

     9

 *  

    10

 *  The Moehwald ethercatrtdm interface is free software; you can

    11

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

    12

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

    13

 *  of the License.

    14

 *

    15

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

    16

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

    17

 *  published by the Free Software Foundation.

    18

 *

    19

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

    20

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

    21

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

    22

 *  GNU Lesser General Public License for more details.

    23

 *

    24

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

    25

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

    26

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

    27

 *  

    28

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

    29

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

    30

 *  industrial property and similar rights of Beckhoff Automation GmbH.

    31

 *

    32

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

    33

    34

#include <errno.h>

    35

#include <signal.h>

    36

#include <stdio.h>

    37

#include <string.h>

    38

#include <sys/resource.h>

    39

#include <sys/time.h>

    40

#include <sys/types.h>

    41

#include <unistd.h>

    42

#include <sys/mman.h>

    43

#include <rtdm/rtdm.h>

    44

#include <native/task.h>

    45

#include <native/sem.h>

    46

#include <native/mutex.h>

    47

#include <native/timer.h>

    48

#include <rtdk.h>

    49

#include <pthread.h>

    50

    51

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

    52

    53

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

    54

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

    55

    56

    57

RT_TASK my_task;

    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

    69

    70

// Optional features

    71

#define CONFIGURE_PDOS  1

    72

//#define SDO_ACCESS      1

    73

    74

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

    75

    76

// EtherCAT

    77

static ec_master_t *master = NULL;

    78

static ec_master_state_t master_state = {};

    79

    80

static ec_domain_t *domain1 = NULL;

    81

static ec_domain_state_t domain1_state = {};

    82

    83

//static ec_slave_config_t *sc_ana_in = NULL;

    84

//static ec_slave_config_state_t sc_ana_in_state = {};

    85

    86

// Timer

    87

static unsigned int sig_alarms = 0;

    88

//static unsigned int user_alarms = 0;

    89

    90

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

    91

static uint8_t *domain1_pd = NULL;

    92

    93

// process data

    94

    95

#define BusCoupler01_Pos    0, 0

    96

#define DigOutSlave01_Pos   0, 1

    97

#define DigOutSlave02_Pos   0, 2

    98

#define DigInSlave01_Pos    0, 3

    99

#define AnaOutSlave01_Pos   0, 4

   100

#define AnaInSlave01_Pos    0, 5

   101

#define BusCoupler02_Pos    0, 6

   102

#define AnaInSlave02_Pos    0, 7

   103

#define DPSlave01_Pos       0, 8

   104

   105

   106

#define Beckhoff_EK1100 0x00000002, 0x044c2c52

   107

#define Beckhoff_EL1014 0x00000002, 0x03f63052

   108

#define Beckhoff_EL2004 0x00000002, 0x07d43052

   109

#define Beckhoff_EL4132 0x00000002, 0x10243052

   110

#define Beckhoff_EL3102 0x00000002, 0x0c1e3052

   111

#define Beckhoff_EL4102 0x00000002, 0x10063052

   112

#define Beckhoff_EL6731 0x00000002, 0x1a4b3052

   113

#define Beckhoff_EL6600 0x00000002, 0x19c93052

   114

#define Beckhoff_EL3602 0x00000002, 0x0e123052

   115

#define Beckhoff_EL5151 0x00000002, 0x141f3052

   116

   117

   118

// offsets for PDO entries

   119

static unsigned int off_dig_out0      = 0;

   120

static unsigned int off_dig_out1      = 0;

   121

static unsigned int off_dig_out2      = 0;

   122

static unsigned int off_dig_out3      = 0;

   123

static unsigned int off_dig_out4      = 0;

   124

static unsigned int off_dig_out5      = 0;

   125

static unsigned int off_dig_out6      = 0;

   126

static unsigned int off_dig_out7      = 0;

   127

static unsigned int off_dig_in0       = 0;

   128

static unsigned int off_dig_in1       = 0;

   129

static unsigned int off_dig_in2       = 0;

   130

static unsigned int off_dig_in3       = 0;

   131

static unsigned int off_ana_out0      = 0;

   132

static unsigned int off_ana_out1      = 0;

   133

static unsigned int off_ana_in0_status = 0;

   134

static unsigned int off_ana_in0_value  = 0;

   135

static unsigned int off_ana_in1_status = 0;

   136

static unsigned int off_ana_in1_value  = 0;

   137

static unsigned int off_ana_in2_status = 0;

   138

static unsigned int off_ana_in2_value  = 0;

   139

static unsigned int off_ana_in3_status = 0;

   140

static unsigned int off_ana_in3_value  = 0;

   141

   142

//static unsigned int off_dp_slave;

   143

   144

// process data

   145

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

   146

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

   147

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

   148

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

   149

   150

const static ec_pdo_entry_reg_t domain1_regs[] = {

   151

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

   152

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

   153

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

   154

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

   155

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

   156

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

   157

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

   158

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

   159

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

   160

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

   161

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

   162

   {}

   163

};

   164

   165

char rt_dev_file[64];

   166

static unsigned int counter = 0;

   167

static unsigned int blink = 0;

   168

   169

static ec_slave_config_t *sc_dig_out_01 = NULL;

   170

   171

static ec_slave_config_t *sc_dig_out_02 = NULL;

   172

   173

static ec_slave_config_t *sc_dig_in_01 = NULL;

   174

   175

static ec_slave_config_t *sc_ana_out_01 = NULL;

   176

   177

static ec_slave_config_t *sc_ana_in_01 = NULL;

   178

   179

static ec_slave_config_t *sc_dpslv_01 = NULL;

   180

   181

static ec_slave_config_t *sc_ana_in_02 = NULL;

   182

   183

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

   184

   185

/* Slave 1, "EL2004"

   186

 * Vendor ID:       0x00000002

   187

 * Product code:    0x07d43052

   188

 * Revision number: 0x00100000

   189

 */

   190

   191

ec_pdo_entry_info_t slave_1_pdo_entries[] = {

   192

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

   193

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

   194

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

   195

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

   196

};

   197

   198

ec_pdo_info_t slave_1_pdos[] = {

   199

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

   200

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

   201

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

   202

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

   203

};

   204

   205

ec_sync_info_t slave_1_syncs[] = {

   206

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

   207

   {0xff}

   208

};

   209

   210

/* Slave 2, "EL2004"

   211

 * Vendor ID:       0x00000002

   212

 * Product code:    0x07d43052

   213

 * Revision number: 0x00100000

   214

 */

   215

   216

ec_pdo_entry_info_t slave_2_pdo_entries[] = {

   217

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

   218

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

   219

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

   220

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

   221

};

   222

   223

ec_pdo_info_t slave_2_pdos[] = {

   224

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

   225

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

   226

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

   227

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

   228

};

   229

   230

ec_sync_info_t slave_2_syncs[] = {

   231

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

   232

   {0xff}

   233

};

   234

   235

/* Slave 3, "EL1014"

   236

 * Vendor ID:       0x00000002

   237

 * Product code:    0x03f63052

   238

 * Revision number: 0x00100000

   239

 */

   240

   241

ec_pdo_entry_info_t slave_3_pdo_entries[] = {

   242

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

   243

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

   244

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

   245

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

   246

};

   247

   248

ec_pdo_info_t slave_3_pdos[] = {

   249

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

   250

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

   251

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

   252

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

   253

};

   254

   255

ec_sync_info_t slave_3_syncs[] = {

   256

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

   257

   {0xff}

   258

};

   259

   260

/* Slave 4, "EL4132"

   261

 * Vendor ID:       0x00000002

   262

 * Product code:    0x10243052

   263

 * Revision number: 0x03f90000

   264

 */

   265

   266

ec_pdo_entry_info_t slave_4_pdo_entries[] = {

   267

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

   268

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

   269

};

   270

   271

ec_pdo_info_t slave_4_pdos[] = {

   272

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

   273

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

   274

};

   275

   276

ec_sync_info_t slave_4_syncs[] = {

   277

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

   278

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

   279

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

   280

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

   281

   {0xff}

   282

};

   283

   284

/* Slave 5, "EL3102"

   285

 * Vendor ID:       0x00000002

   286

 * Product code:    0x0c1e3052

   287

 * Revision number: 0x00000000

   288

 */

   289

   290

ec_pdo_entry_info_t slave_5_pdo_entries[] = {

   291

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

   292

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

   293

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

   294

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

   295

};

   296

   297

ec_pdo_info_t slave_5_pdos[] = {

   298

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

   299

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

   300

};

   301

   302

ec_sync_info_t slave_5_syncs[] = {

   303

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

   304

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

   305

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

   306

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

   307

   {0xff}

   308

};

   309

   310

/* Slave 6, "EL6731-0010"

   311

 * Vendor ID:       0x00000002

   312

 * Product code:    0x1a4b3052

   313

 * Revision number: 0x0011000a

   314

 */

   315

   316

ec_sync_info_t slave_6_syncs[] = {

   317

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

   318

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

   319

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

   320

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

   321

};

   322

   323

   324

/* Slave 7, "EL6601"

   325

 * Vendor ID:       0x00000002

   326

 * Product code:    0x19c93052

   327

 * Revision number: 0x00110000

   328

 */

   329

/*

   330

ec_sync_info_t slave_7_syncs[] = {

   331

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

   332

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

   333

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

   334

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

   335

   {0xff}

   336

};

   337

*/

   338

   339

/* Master 0, Slave 7, "EL3602"

   340

 * Vendor ID:       0x00000002

   341

 * Product code:    0x0e123052

   342

 * Revision number: 0x00100000

   343

 */

   344

ec_pdo_entry_info_t slave_7_pdo_entries[] = {

   345

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

   346

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

   347

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

   348

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

   349

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

   350

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

   351

   {0x1800, 0x07, 1},

   352

   {0x1800, 0x09, 1},

   353

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

   354

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

   355

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

   356

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

   357

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

   358

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

   359

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

   360

   {0x1801, 0x07, 1},

   361

   {0x1801, 0x09, 1},

   362

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

   363

};

   364

   365

ec_pdo_info_t slave_7_pdos[] = {

   366

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

   367

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

   368

};

   369

   370

ec_sync_info_t slave_7_syncs[] = {

   371

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

   372

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

   373

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

   374

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

   375

   {0xff}

   376

};

   377

   378

/* Master 0, Slave 8, "EL5151"

   379

 * Vendor ID:       0x00000002

   380

 * Product code:    0x141f3052

   381

 * Revision number: 0x00130000

   382

 */

   383

   384

ec_pdo_entry_info_t slave_8_pdo_entries[] = {

   385

   {0x6000, 0x01, 1},

   386

   {0x6000, 0x02, 1},

   387

   {0x6000, 0x03, 1},

   388

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

   389

   {0x6000, 0x08, 1},

   390

   {0x6000, 0x09, 1},

   391

   {0x6000, 0x0a, 1},

   392

   {0x6000, 0x0b, 1},

   393

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

   394

   {0x6000, 0x0d, 1},

   395

   {0x1c32, 0x20, 1},

   396

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

   397

   {0x1800, 0x09, 1},

   398

   {0x6000, 0x11, 32},

   399

   {0x6000, 0x12, 32},

   400

   {0x6000, 0x14, 32},

   401

};

   402

   403

ec_pdo_info_t slave_8_pdos[] = {

   404

   {0x0000, 0, NULL},

   405

   {0x1a00, 15, slave_8_pdo_entries + 0},

   406

   {0x1a02, 1, slave_8_pdo_entries + 15},

   407

};

   408

   409

ec_sync_info_t slave_8_syncs[] = {

   410

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

   411

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

   412

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

   413

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

   414

   {0xff}

   415

};

   416

   417

   418

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

   419

   420

#if SDO_ACCESS

   421

static ec_sdo_request_t *sdo;

   422

uint8_t *sdo_adr = NULL;

   423

#endif

   424

   425

   426

   427

void rt_check_domain_state(void)

   428

{

   429

    ec_domain_state_t ds;

   430

   431

    if (rt_fd>=0)

   432

      {

   433

          ecrt_rtdm_domain_state(rt_fd,&ds);  

   434

      }

   435

   436

    if (ds.working_counter != domain1_state.working_counter)

   437

     {

   438

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

   439

     }

   440

    if (ds.wc_state != domain1_state.wc_state)

   441

     {

   442

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

   443

     }

   444

   445

    domain1_state = ds;

   446

}

   447

   448

void rt_check_master_state(void)

   449

{

   450

    ec_master_state_t ms;

   451

   452

    if (rt_fd>=0)

   453

      {

   454

          ecrt_rtdm_master_state(rt_fd,&ms);

   455

      }

   456

   457

    if (ms.slaves_responding != master_state.slaves_responding)

   458

    {

   459

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

   460

    }

   461

    if (ms.al_states != master_state.al_states)

   462

    {

   463

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

   464

    }

   465

    if (ms.link_up != master_state.link_up)

   466

    {

   467

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

   468

    }

   469

    master_state = ms;

   470

}

   471

   472

   473

   474

   475

void rt_sync()

   476

{

   477

  RTIME now;

   478

  now = rt_timer_read();

   479

   480

   481

  if (rt_fd>=0)

   482

  {

   483

      ecrt_rtdm_master_application_time(rt_fd, &now);

   484

  }

   485

   486

  if (sync_ref_counter) {

   487

     sync_ref_counter--;

   488

  } else {

   489

     sync_ref_counter = 9;

   490

     if (rt_fd>=0)

   491

     {

   492

         ecrt_rtdm_master_sync_reference_clock(rt_fd);

   493

     }

   494

  }

   495

  if (rt_fd>=0)

   496

  {

   497

      ecrt_rtdm_master_sync_slave_clocks(rt_fd) ;

   498

  }

   499

}

   500

   501

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

   502

   503

#if SDO_ACCESS

   504

void read_sdo(void)

   505

{

   506

    switch (ecrt_sdo_request_state(sdo))

   507

        {

   508

        case EC_REQUEST_UNUSED: // request was not used yet

   509

            ecrt_sdo_request_read(sdo); // trigger first read

   510

            break;

   511

        case EC_REQUEST_BUSY:

   512

            fprintf(stderr, "Still busy...\n");

   513

            break;

   514

        case EC_REQUEST_SUCCESS:

   515

            fprintf(stderr, "SDO value: 0x%04X\n",

   516

                    EC_READ_U16(ecrt_sdo_request_data(sdo)));

   517

            ecrt_sdo_request_read(sdo); // trigger next read

   518

            break;

   519

        case EC_REQUEST_ERROR:

   520

            fprintf(stderr, "Failed to read SDO!\n");

   521

            ecrt_sdo_request_read(sdo); // retry reading

   522

            break;

   523

        }

   524

}

   525