1

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

     2

 *

     3

 *  $Id$

     4

 *

     5

 *  usermdtest.c	Copyright (C) 2009-2010  Moehwald GmbH B.Benner

     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>

    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

    63

unsigned int sync_ref_counter = 0;

    64

    65

CstructMstrAttach MstrAttach;

    66

    67

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

    68

    69

// Application parameters

    72

    73

// Optional features

    74

#define CONFIGURE_PDOS  1

    75

//#define SDO_ACCESS      1

    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

//static ec_slave_config_t *sc_ana_in = NULL;

    87

//static ec_slave_config_state_t sc_ana_in_state = {};

    88

    89

// Timer

    90

static unsigned int sig_alarms = 0;

    91

//static unsigned int user_alarms = 0;

    92

    93

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

    94

static uint8_t *domain1_pd = NULL;

    95

    96

// process data

    97

    98

#define BusCoupler01_Pos    0, 0

    99

#define DigOutSlave01_Pos   0, 1

   100

#define DigOutSlave02_Pos   0, 2

   101

#define DigInSlave01_Pos    0, 3

   102

#define AnaOutSlave01_Pos   0, 4

   103

#define AnaInSlave01_Pos    0, 5

   104

#define BusCoupler02_Pos    0, 6

   105

#define AnaInSlave02_Pos    0, 7

   107

   108

   109

#define Beckhoff_EK1100 0x00000002, 0x044c2c52

   110

#define Beckhoff_EL1014 0x00000002, 0x03f63052

   111

#define Beckhoff_EL2004 0x00000002, 0x07d43052

   112

#define Beckhoff_EL4132 0x00000002, 0x10243052

   113

#define Beckhoff_EL3102 0x00000002, 0x0c1e3052

   114

#define Beckhoff_EL4102 0x00000002, 0x10063052

   115

#define Beckhoff_EL6731 0x00000002, 0x1a4b3052

   116

#define Beckhoff_EL6600 0x00000002, 0x19c93052

   117

#define Beckhoff_EL3602 0x00000002, 0x0e123052

   118

#define Beckhoff_EL5151 0x00000002, 0x141f3052

   119

   120

   121

// offsets for PDO entries

   122

static unsigned int off_dig_out0      = 0;

   123

static unsigned int off_dig_out1      = 0;

   124

static unsigned int off_dig_out2      = 0;

   125

static unsigned int off_dig_out3      = 0;

   126

static unsigned int off_dig_out4      = 0;

   127

static unsigned int off_dig_out5      = 0;

   128

static unsigned int off_dig_out6      = 0;

   129

static unsigned int off_dig_out7      = 0;

   130

static unsigned int off_dig_in0       = 0;

   131

static unsigned int off_dig_in1       = 0;

   132

static unsigned int off_dig_in2       = 0;

   133

static unsigned int off_dig_in3       = 0;

   134

static unsigned int off_ana_out0      = 0;

   135

static unsigned int off_ana_out1      = 0;

   136

static unsigned int off_ana_in0_status = 0;

   137

static unsigned int off_ana_in0_value  = 0;

   138

static unsigned int off_ana_in1_status = 0;

   139

static unsigned int off_ana_in1_value  = 0;

   140

static unsigned int off_ana_in2_status = 0;

   141

static unsigned int off_ana_in2_value  = 0;

   142

static unsigned int off_ana_in3_status = 0;

   143

static unsigned int off_ana_in3_value  = 0;

   144

   145

//static unsigned int off_dp_slave;

   146

   147

// process data

   148

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

   149

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

   150

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

   151

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

   152

   153

const static ec_pdo_entry_reg_t domain1_regs[] = {

   154

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

   155

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

   156

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

   157

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

   158

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

   159

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

   160

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

   161

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

   162

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

   163

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

   164

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

   165

   {}

   166

};

   167

   168

char rt_dev_file[64];

   169

static unsigned int counter = 0;

   170

static unsigned int blink = 0;

   171

   172

static ec_slave_config_t *sc_dig_out_01 = NULL;

   173

   174

static ec_slave_config_t *sc_dig_out_02 = NULL;

   175

   176

static ec_slave_config_t *sc_dig_in_01 = NULL;

   177

   178

static ec_slave_config_t *sc_ana_out_01 = NULL;

   179

   180

static ec_slave_config_t *sc_ana_in_01 = NULL;

   181

   182

static ec_slave_config_t *sc_dpslv_01 = NULL;

   183

   184

static ec_slave_config_t *sc_ana_in_02 = NULL;

   185

   186

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

   187

   188

/* Slave 1, "EL2004"

   189

 * Vendor ID:       0x00000002

   190

 * Product code:    0x07d43052

   191

 * Revision number: 0x00100000

   192

 */

   193

   194

ec_pdo_entry_info_t slave_1_pdo_entries[] = {

   195

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

   196

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

   197

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

   198

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

   199

};

   200

   201

ec_pdo_info_t slave_1_pdos[] = {

   202

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

   203

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

   204

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

   205

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

   206

};

   207

   208

ec_sync_info_t slave_1_syncs[] = {

   209

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

   210

   {0xff}

   211

};

   212

   213

/* Slave 2, "EL2004"

   214

 * Vendor ID:       0x00000002

   215

 * Product code:    0x07d43052

   216

 * Revision number: 0x00100000

   217

 */

   218

   219

ec_pdo_entry_info_t slave_2_pdo_entries[] = {

   220

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

   221

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

   222

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

   223

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

   224

};

   225

   226

ec_pdo_info_t slave_2_pdos[] = {

   227

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

   228

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

   229

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

   230

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

   231

};

   232

   233

ec_sync_info_t slave_2_syncs[] = {

   234

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

   235

   {0xff}

   236

};

   237

   238

/* Slave 3, "EL1014"

   239

 * Vendor ID:       0x00000002

   240

 * Product code:    0x03f63052

   241

 * Revision number: 0x00100000

   242

 */

   243

   244

ec_pdo_entry_info_t slave_3_pdo_entries[] = {

   245

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

   246

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

   247

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

   248

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

   249

};

   250

   251

ec_pdo_info_t slave_3_pdos[] = {

   252

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

   253

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

   254

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

   255

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

   256

};

   257

   258

ec_sync_info_t slave_3_syncs[] = {

   259

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

   260

   {0xff}

   261

};

   262

   263

/* Slave 4, "EL4132"

   264

 * Vendor ID:       0x00000002

   265

 * Product code:    0x10243052

   266

 * Revision number: 0x03f90000

   267

 */

   268

   269

ec_pdo_entry_info_t slave_4_pdo_entries[] = {

   270

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

   271

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

   272

};

   273

   274

ec_pdo_info_t slave_4_pdos[] = {

   275

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

   276

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

   277

};

   278

   279

ec_sync_info_t slave_4_syncs[] = {

   280

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

   281

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

   282

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

   283

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

   284

   {0xff}

   285

};

   286

   287

/* Slave 5, "EL3102"

   288

 * Vendor ID:       0x00000002

   289

 * Product code:    0x0c1e3052

   290

 * Revision number: 0x00000000

   291

 */

   292

   293

ec_pdo_entry_info_t slave_5_pdo_entries[] = {

   294

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

   295

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

   296

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

   297

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

   298

};

   299

   300

ec_pdo_info_t slave_5_pdos[] = {

   301

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

   302

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

   303

};

   304

   305

ec_sync_info_t slave_5_syncs[] = {

   306

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

   307

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

   308

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

   309

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

   310

   {0xff}

   311

};

   312

   313

/* Slave 6, "EL6731-0010"

   314

 * Vendor ID:       0x00000002

   315

 * Product code:    0x1a4b3052

   316

 * Revision number: 0x0011000a

   317

 */

   318

   319

ec_sync_info_t slave_6_syncs[] = {

   320

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

   321

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

   322

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

   323

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

   324

};

   325

   326

   327

/* Slave 7, "EL6601"

   328

 * Vendor ID:       0x00000002

   329

 * Product code:    0x19c93052

   330

 * Revision number: 0x00110000

   331

 */

   332

/*

   333

ec_sync_info_t slave_7_syncs[] = {

   334

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

   335

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

   336

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

   337

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

   338

   {0xff}

   339

};

   340

*/

   341

   342

/* Master 0, Slave 7, "EL3602"

   343

 * Vendor ID:       0x00000002

   344

 * Product code:    0x0e123052

   345

 * Revision number: 0x00100000

   346

 */

   347

ec_pdo_entry_info_t slave_7_pdo_entries[] = {

   348

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

   349

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

   350

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

   351

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

   352

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

   353

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

   354

   {0x1800, 0x07, 1},

   355

   {0x1800, 0x09, 1},

   356

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

   357

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

   358

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

   359

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

   360

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

   361

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

   362

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

   363

   {0x1801, 0x07, 1},

   364

   {0x1801, 0x09, 1},

   365

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

   366

};

   367

   368

ec_pdo_info_t slave_7_pdos[] = {

   369

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

   370

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

   371

};

   372

   373

ec_sync_info_t slave_7_syncs[] = {

   374

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

   375

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

   376

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

   377

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

   378

   {0xff}

   379

};

   380

   381

/* Master 0, Slave 8, "EL5151"

   382

 * Vendor ID:       0x00000002

   383

 * Product code:    0x141f3052

   384

 * Revision number: 0x00130000

   385

 */

   386

   387

ec_pdo_entry_info_t slave_8_pdo_entries[] = {

   388

   {0x6000, 0x01, 1},

   389

   {0x6000, 0x02, 1},

   390

   {0x6000, 0x03, 1},

   391

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

   392

   {0x6000, 0x08, 1},

   393

   {0x6000, 0x09, 1},

   394

   {0x6000, 0x0a, 1},

   395

   {0x6000, 0x0b, 1},

   396

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

   397

   {0x6000, 0x0d, 1},

   398

   {0x1c32, 0x20, 1},

   399

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

   400

   {0x1800, 0x09, 1},

   401

   {0x6000, 0x11, 32},

   402

   {0x6000, 0x12, 32},

   403

   {0x6000, 0x14, 32},

   404

};

   405

   406

ec_pdo_info_t slave_8_pdos[] = {

   407

   {0x0000, 0, NULL},

   408

   {0x1a00, 15, slave_8_pdo_entries + 0},

   409

   {0x1a02, 1, slave_8_pdo_entries + 15},

   410

};

   411

   412

ec_sync_info_t slave_8_syncs[] = {

   413

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

   414

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

   415

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

   416

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

   417

   {0xff}

   418

};

   419

   420

   421

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

   422

   423

#if SDO_ACCESS

   424

static ec_sdo_request_t *sdo;

   425

uint8_t *sdo_adr = NULL;

   426

#endif

   427

   428

   429

   430

void rt_check_domain_state(void)

   431

{

   432

    ec_domain_state_t ds;

   433

   434

    if (rt_fd>=0)

   435

      {

   436

          ecrt_rtdm_domain_state(rt_fd,&ds);  

   437

      }

   438

   440

     {

   442

     }

   443

    if (ds.wc_state != domain1_state.wc_state)

   444

     {

   446

     }

   448

    domain1_state = ds;

   449

}

   450

   451

void rt_check_master_state(void)

   452

{

   453

    ec_master_state_t ms;

   454

   455

    if (rt_fd>=0)

   456

      {

   457

          ecrt_rtdm_master_state(rt_fd,&ms);

   458

      }

   459

   460

    if (ms.slaves_responding != master_state.slaves_responding)

   461

    {

   463

    }

   464

    if (ms.al_states != master_state.al_states)

   465

    {

   467

    }

   468

    if (ms.link_up != master_state.link_up)

   469

    {

   472

    master_state = ms;

   473

}

   474

   477

   478

void rt_sync()

   479

{

   480

  RTIME now;

   481

  now = rt_timer_read();

   483

   484

  if (rt_fd>=0)

   485

  {

   486

      ecrt_rtdm_master_application_time(rt_fd, &now);

   487

  }

   488

   489

  if (sync_ref_counter) {

   490

     sync_ref_counter--;

   491

  } else {

   492

     sync_ref_counter = 9;

   493

     if (rt_fd>=0)

   494

     {

   495

         ecrt_rtdm_master_sync_reference_clock(rt_fd);

   496

     }

   497

  }

   498

  if (rt_fd>=0)

   499

  {

   500

      ecrt_rtdm_master_sync_slave_clocks(rt_fd) ;

   501

  }

   502

}

   503

   504

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

   505

   506

#if SDO_ACCESS

   507

void read_sdo(void)

   508

{