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>

    48

#include <pthread.h>

    49

    50

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

    51

    52

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

    53

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

    54

    55

    56

RT_TASK my_task;

    57

    58

int rt_fd = -1;

    59

int run=0;

    60

    61

struct timeval tv;

    62

unsigned int sync_ref_counter = 0;

    63

    64

CstructMstrAttach MstrAttach;

    65

    66

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

    67

    68

// Application parameters

    69

#define FREQUENCY 1000

    70

#define PRIORITY 1

    71

    72

// Optional features

    73

#define CONFIGURE_PDOS  1

    74

//#define SDO_ACCESS      1

    75

    76

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

    77

    78

// EtherCAT

    79

static ec_master_t *master = NULL;

    80

static ec_master_state_t master_state = {};

    81

    82

static ec_domain_t *domain1 = NULL;

    83

static ec_domain_state_t domain1_state = {};

    84

    85

//static ec_slave_config_t *sc_ana_in = NULL;

    86

//static ec_slave_config_state_t sc_ana_in_state = {};

    87

    88

// Timer

    89

static unsigned int sig_alarms = 0;

    90

//static unsigned int user_alarms = 0;

    91

    92

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

    93

static uint8_t *domain1_pd = NULL;

    94

    95

// process data

    96

    97

#define BusCoupler01_Pos    0, 0

    98

#define DigOutSlave01_Pos   0, 1

    99

#define DigOutSlave02_Pos   0, 2

   100

#define DigInSlave01_Pos    0, 3

   101

#define AnaOutSlave01_Pos   0, 4

   102

#define AnaInSlave01_Pos    0, 5

   103

#define BusCoupler02_Pos    0, 6

   104

#define AnaInSlave02_Pos    0, 7

   105

   106

   107

   108

#define Beckhoff_EK1100 0x00000002, 0x044c2c52

   109

#define Beckhoff_EL1014 0x00000002, 0x03f63052

   110

#define Beckhoff_EL2004 0x00000002, 0x07d43052

   111

#define Beckhoff_EL4132 0x00000002, 0x10243052

   112

#define Beckhoff_EL3102 0x00000002, 0x0c1e3052

   113

#define Beckhoff_EL4102 0x00000002, 0x10063052

   114

#define Beckhoff_EL6731 0x00000002, 0x1a4b3052

   115

#define Beckhoff_EL6600 0x00000002, 0x19c93052

   116

#define Beckhoff_EL3602 0x00000002, 0x0e123052

   117

#define Beckhoff_EL5151 0x00000002, 0x141f3052

   118

   119

   120

// offsets for PDO entries

   121

static unsigned int off_dig_out0      = 0;

   122

static unsigned int off_dig_out1      = 0;

   123

static unsigned int off_dig_out2      = 0;

   124

static unsigned int off_dig_out3      = 0;

   125

static unsigned int off_dig_out4      = 0;

   126

static unsigned int off_dig_out5      = 0;

   127

static unsigned int off_dig_out6      = 0;

   128

static unsigned int off_dig_out7      = 0;

   129

static unsigned int off_dig_in0       = 0;

   130

static unsigned int off_dig_in1       = 0;

   131

static unsigned int off_dig_in2       = 0;

   132

static unsigned int off_dig_in3       = 0;

   133

static unsigned int off_ana_out0      = 0;

   134

static unsigned int off_ana_out1      = 0;

   135

static unsigned int off_ana_in0_status = 0;

   136

static unsigned int off_ana_in0_value  = 0;

   137

static unsigned int off_ana_in1_status = 0;

   138

static unsigned int off_ana_in1_value  = 0;

   139

static unsigned int off_ana_in2_status = 0;

   140

static unsigned int off_ana_in2_value  = 0;

   141

static unsigned int off_ana_in3_status = 0;

   142

static unsigned int off_ana_in3_value  = 0;

   143

   144

//static unsigned int off_dp_slave;

   145

   146

// process data

   147

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

   148

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

   149

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

   150

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

   151

   152

const static ec_pdo_entry_reg_t domain1_regs[] = {

   153

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

   154

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

   155

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

   156

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

   157

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

   158

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

   159

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

   160

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

   161

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

   162

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

   163

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

   164

   {}

   165

};

   166

   167

char rt_dev_file[64];

   168

static unsigned int counter = 0;

   169

static unsigned int blink = 0;

   170

   171

static ec_slave_config_t *sc_dig_out_01 = NULL;

   172

   173

static ec_slave_config_t *sc_dig_out_02 = NULL;

   174

   175

static ec_slave_config_t *sc_dig_in_01 = NULL;

   176

   177

static ec_slave_config_t *sc_ana_out_01 = NULL;

   178

   179

static ec_slave_config_t *sc_ana_in_01 = NULL;

   180

   181

static ec_slave_config_t *sc_dpslv_01 = NULL;

   182

   183

static ec_slave_config_t *sc_ana_in_02 = NULL;

   184

   185

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

   186

   187

/* Slave 1, "EL2004"

   188

 * Vendor ID:       0x00000002

   189

 * Product code:    0x07d43052

   190

 * Revision number: 0x00100000

   191

 */

   192

   193

ec_pdo_entry_info_t slave_1_pdo_entries[] = {

   194

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

   195

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

   196

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

   197

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

   198

};

   199

   200

ec_pdo_info_t slave_1_pdos[] = {

   201

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

   202

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

   203

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

   204

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

   205

};

   206

   207

ec_sync_info_t slave_1_syncs[] = {

   208

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

   209

   {0xff}

   210

};

   211

   212

/* Slave 2, "EL2004"

   213

 * Vendor ID:       0x00000002

   214

 * Product code:    0x07d43052

   215

 * Revision number: 0x00100000

   216

 */

   217

   218

ec_pdo_entry_info_t slave_2_pdo_entries[] = {

   219

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

   220

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

   221

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

   222

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

   223

};

   224

   225

ec_pdo_info_t slave_2_pdos[] = {

   226

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

   227

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

   228

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

   229

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

   230

};

   231

   232

ec_sync_info_t slave_2_syncs[] = {

   233

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

   234

   {0xff}

   235

};

   236

   237

/* Slave 3, "EL1014"

   238

 * Vendor ID:       0x00000002

   239

 * Product code:    0x03f63052

   240

 * Revision number: 0x00100000

   241

 */

   242

   243

ec_pdo_entry_info_t slave_3_pdo_entries[] = {

   244

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

   245

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

   246

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

   247

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

   248

};

   249

   250

ec_pdo_info_t slave_3_pdos[] = {

   251

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

   252

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

   253

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

   254

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

   255

};

   256

   257

ec_sync_info_t slave_3_syncs[] = {

   258

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

   259

   {0xff}

   260

};

   261

   262

/* Slave 4, "EL4132"

   263

 * Vendor ID:       0x00000002

   264

 * Product code:    0x10243052

   265

 * Revision number: 0x03f90000

   266

 */

   267

   268

ec_pdo_entry_info_t slave_4_pdo_entries[] = {

   269

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

   270

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

   271

};

   272

   273

ec_pdo_info_t slave_4_pdos[] = {

   274

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

   275

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

   276

};

   277

   278

ec_sync_info_t slave_4_syncs[] = {

   279

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

   280

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

   281

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

   282

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

   283

   {0xff}

   284

};

   285

   286

/* Slave 5, "EL3102"

   287

 * Vendor ID:       0x00000002

   288

 * Product code:    0x0c1e3052

   289

 * Revision number: 0x00000000

   290

 */

   291

   292

ec_pdo_entry_info_t slave_5_pdo_entries[] = {

   293

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

   294

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

   295

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

   296

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

   297

};

   298

   299

ec_pdo_info_t slave_5_pdos[] = {

   300

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

   301

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

   302

};

   303

   304

ec_sync_info_t slave_5_syncs[] = {

   305

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

   306

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

   307

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

   308

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

   309

   {0xff}

   310

};

   311

   312

/* Slave 6, "EL6731-0010"

   313

 * Vendor ID:       0x00000002

   314

 * Product code:    0x1a4b3052

   315

 * Revision number: 0x0011000a

   316

 */

   317

   318

ec_sync_info_t slave_6_syncs[] = {

   319

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

   320

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

   321

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

   322

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

   323

};

   324

   325

   326

/* Slave 7, "EL6601"

   327

 * Vendor ID:       0x00000002

   328

 * Product code:    0x19c93052

   329

 * Revision number: 0x00110000

   330

 */

   331

/*

   332

ec_sync_info_t slave_7_syncs[] = {

   333

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

   334

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

   335

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

   336

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

   337

   {0xff}

   338

};

   339

*/

   340

   341

/* Master 0, Slave 7, "EL3602"

   342

 * Vendor ID:       0x00000002

   343

 * Product code:    0x0e123052

   344

 * Revision number: 0x00100000

   345

 */

   346

ec_pdo_entry_info_t slave_7_pdo_entries[] = {

   347

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

   348

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

   349

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

   350

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

   351

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

   352

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

   353

   {0x1800, 0x07, 1},

   354

   {0x1800, 0x09, 1},

   355

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

   356

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

   357

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

   358

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

   359

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

   360

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

   361

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

   362

   {0x1801, 0x07, 1},

   363

   {0x1801, 0x09, 1},

   364

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

   365

};

   366

   367

ec_pdo_info_t slave_7_pdos[] = {

   368

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

   369

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

   370

};

   371

   372

ec_sync_info_t slave_7_syncs[] = {

   373

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

   374

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

   375

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

   376

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

   377

   {0xff}

   378

};

   379

   380

/* Master 0, Slave 8, "EL5151"

   381

 * Vendor ID:       0x00000002

   382

 * Product code:    0x141f3052

   383

 * Revision number: 0x00130000

   384

 */

   385

   386

ec_pdo_entry_info_t slave_8_pdo_entries[] = {

   387

   {0x6000, 0x01, 1},

   388

   {0x6000, 0x02, 1},

   389

   {0x6000, 0x03, 1},

   390

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

   391

   {0x6000, 0x08, 1},

   392

   {0x6000, 0x09, 1},

   393

   {0x6000, 0x0a, 1},

   394

   {0x6000, 0x0b, 1},

   395

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

   396

   {0x6000, 0x0d, 1},

   397

   {0x1c32, 0x20, 1},

   398

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

   399

   {0x1800, 0x09, 1},

   400

   {0x6000, 0x11, 32},

   401

   {0x6000, 0x12, 32},

   402

   {0x6000, 0x14, 32},

   403

};

   404

   405

ec_pdo_info_t slave_8_pdos[] = {

   406

   {0x0000, 0, NULL},

   407

   {0x1a00, 15, slave_8_pdo_entries + 0},

   408

   {0x1a02, 1, slave_8_pdo_entries + 15},

   409

};

   410

   411

ec_sync_info_t slave_8_syncs[] = {

   412

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

   413

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

   414

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

   415

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

   416

   {0xff}

   417

};

   418

   419

   420

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

   421

   422

#if SDO_ACCESS

   423

static ec_sdo_request_t *sdo;

   424

uint8_t *sdo_adr = NULL;

   425

#endif

   426

   427

   428

   429

void rt_check_domain_state(void)

   430

{

   431

    ec_domain_state_t ds;

   432

    unsigned int printed=0;

   433

   434

    if (rt_fd>=0)

   435

      {

   436

          ecrt_rtdm_domain_state(rt_fd,&ds);  

   437

      }

   438

   439

/*    if (ds.working_counter != domain1_state.working_counter)

   440

     {

   441

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

   442

        printed=1;

   443

   444

     }

   445

    if (ds.wc_state != domain1_state.wc_state)

   446

     {

   447

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

   448

        printed=1;

   449

   450

     }

   451

    if (printed)

   452

    {

   453

    	int ret;

   454

   455

        // return to realtime mode after print

   456

   457

        ret = rt_task_set_mode(0, T_PRIMARY, NULL);

   458

        if (ret)

   459

         {

   460

            printf("error while rt_task_set_mode, code %d\n",ret);

   461

         }

   462

    }

   463

*/

   464

    domain1_state = ds;

   465

}

   466

   467

void rt_check_master_state(void)

   468

{

   469

    ec_master_state_t ms;

   470

    unsigned int printed=0;

   471

   472

    if (rt_fd>=0)

   473

      {

   474

          ecrt_rtdm_master_state(rt_fd,&ms);

   475

      }

   476

   477

/*

   478

    if (ms.slaves_responding != master_state.slaves_responding)

   479

    {

   480

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

   481

        printed=1;

   482

   483

    }

   484

    if (ms.al_states != master_state.al_states)

   485

    {

   486

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

   487

        printed=1;

   488

   489

    }

   490

    if (ms.link_up != master_state.link_up)

   491

    {

   492

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

   493

        printed=1;

   494

   495

    }

   496

    if (printed)

   497

    {

   498

    	int ret;

   499

   500

        // return to realtime mode after print

   501

   502

        ret = rt_task_set_mode(0, T_PRIMARY, NULL);

   503

        if (ret)

   504

         {

   505

            printf("error while rt_task_set_mode, code %d\n",ret);

   506

         }

   507

         }*/

   508

    master_state = ms;

   509

}

   510

   511

void rt_receive()

   512

{

   513

   514

	if (rt_fd>=0)

   515

	{

   516

		ecrt_rtdm_master_recieve(rt_fd);

   517

	}

   518

}

   519

   520

void rt_send()

   521

{

   522

   523

	if (rt_fd>=0)

   524

	{

   525

		ecrt_rtdm_master_send(rt_fd);

   526

	}

   527

}

   528

   529

void rt_sync()

   530