1

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

     2

 *

     3

 *  $Id: fsm_coe.c 920 2007-09-12 10:07:55Z fp $

     4

 *

     5

 *  Copyright (C) 2006  Florian Pose, Ingenieurgemeinschaft IgH

     6

 *

     7

 *  This file is part of the IgH EtherCAT Master.

     8

 *

     9

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

    10

 *  and/or modify it under the terms of the GNU General Public License

    11

 *  as published by the Free Software Foundation; either version 2 of the

    12

 *  License, or (at your option) any later version.

    13

 *

    14

 *  The IgH EtherCAT Master is distributed in the hope that it will be

    15

 *  useful, but WITHOUT ANY WARRANTY; without even the implied warranty of

    16

 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

    17

 *  GNU General Public License for more details.

    18

 *

    19

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

    20

 *  along with the IgH EtherCAT Master; if not, write to the Free Software

    21

 *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

    22

 *

    23

 *  The right to use EtherCAT Technology is granted and comes free of

    24

 *  charge under condition of compatibility of product made by

    25

 *  Licensee. People intending to distribute/sell products based on the

    26

 *  code, have to sign an agreement to guarantee that products using

    27

 *  software based on IgH EtherCAT master stay compatible with the actual

    28

 *  EtherCAT specification (which are released themselves as an open

    29

 *  standard) as the (only) precondition to have the right to use EtherCAT

    30

 *  Technology, IP and trade marks.

    31

 *

    32

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

    33

    34

/**

    35

   \file

    36

   EtherCAT CoE mapping state machines.

    37

*/

    38

    39

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

    40

    41

#include "globals.h"

    42

#include "master.h"

    43

#include "mailbox.h"

    44

#include "canopen.h"

    45

#include "fsm_coe_map.h"

    46

    47

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

    48

    49

void ec_fsm_coe_map_state_start(ec_fsm_coe_map_t *);

    50

void ec_fsm_coe_map_state_pdo_count(ec_fsm_coe_map_t *);

    51

void ec_fsm_coe_map_state_pdo(ec_fsm_coe_map_t *);

    52

void ec_fsm_coe_map_state_pdo_entry_count(ec_fsm_coe_map_t *);

    53

void ec_fsm_coe_map_state_pdo_entry(ec_fsm_coe_map_t *);

    54

    55

void ec_fsm_coe_map_state_end(ec_fsm_coe_map_t *);

    56

void ec_fsm_coe_map_state_error(ec_fsm_coe_map_t *);

    57

    58

void ec_fsm_coe_map_action_next_sync(ec_fsm_coe_map_t *);

    59

void ec_fsm_coe_map_action_next_pdo(ec_fsm_coe_map_t *);

    60

void ec_fsm_coe_map_action_next_pdo_entry(ec_fsm_coe_map_t *);

    61

    64

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

    65

    66

/**

    67

   Constructor.

    68

*/

    69

    70

void ec_fsm_coe_map_init(

    71

        ec_fsm_coe_map_t *fsm, /**< finite state machine */

    72

        ec_fsm_coe_t *fsm_coe /*< CoE state machine to use */

    73

        )

    74

{

    75

    fsm->state = NULL;

    76

    fsm->fsm_coe = fsm_coe;

    78

}

    79

    80

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

    81

    82

/**

    83

   Destructor.

    84

*/

    85

    86

void ec_fsm_coe_map_clear(ec_fsm_coe_map_t *fsm /**< finite state machine */)

    87

{

   108

}

   109

   110

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

   111

   112

/**

   113

   Starts to upload an SDO from a slave.

   114

*/

   115

   116

void ec_fsm_coe_map_start(

   117

        ec_fsm_coe_map_t *fsm, /**< finite state machine */

   118

        ec_slave_t *slave /**< EtherCAT slave */

   119

        )

   120

{

   121

    fsm->slave = slave;

   122

    fsm->state = ec_fsm_coe_map_state_start;

   123

}

   124

   125

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

   126

   127

/**

   128

   Executes the current state of the state machine.

   129

   \return false, if state machine has terminated

   130

*/

   131

   132

int ec_fsm_coe_map_exec(ec_fsm_coe_map_t *fsm /**< finite state machine */)

   133

{

   134

    fsm->state(fsm);

   135

   136

    return fsm->state != ec_fsm_coe_map_state_end

   137

        && fsm->state != ec_fsm_coe_map_state_error;

   138

}

   139

   140

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

   141

   142

/**

   143

   Returns, if the state machine terminated with success.

   144

   \return non-zero if successful.

   145

*/

   146

   147

int ec_fsm_coe_map_success(ec_fsm_coe_map_t *fsm /**< Finite state machine */)

   148

{

   149

    return fsm->state == ec_fsm_coe_map_state_end;

   150

}

   151

   152

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

   153

 *  CoE dictionary state machine

   154

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

   155

   156

/**

   157

 */

   158

   159

void ec_fsm_coe_map_state_start(ec_fsm_coe_map_t *fsm /**< finite state machine */)

   160

{

   161

    // read mapping of first sync manager

   162

    fsm->sync_index = 0;

   163

    ec_fsm_coe_map_action_next_sync(fsm);

   164

}

   165

   166

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

   167

   168

/**

   169

 */

   170

   171

void ec_fsm_coe_map_action_next_sync(

   172

        ec_fsm_coe_map_t *fsm /**< finite state machine */

   173

        )

   174

{

   175

    ec_slave_t *slave = fsm->slave;

   176

    ec_sdo_entry_t *entry;

   177

   179

        if (!(fsm->sync_sdo = ec_slave_get_sdo(slave, 0x1C10 + fsm->sync_index)))

   180

            continue;

   181

   182

        if (slave->master->debug_level)

   183

            EC_DBG("Reading PDO mapping of sync manager %u of slave %u.\n",

   184

                    fsm->sync_index, slave->ring_position);

   185

   188

        if (!(entry = ec_sdo_get_entry(fsm->sync_sdo, 0))) {

   189

            EC_ERR("SDO 0x%04X has no subindex 0 on slave %u.\n",

   190

                    fsm->sync_sdo->index,

   191

                    fsm->slave->ring_position);

   192

            fsm->state = ec_fsm_coe_map_state_error;

   193

            return;

   194

        }

   195

   196

        ec_sdo_request_init_read(&fsm->request, entry);

   197

        fsm->state = ec_fsm_coe_map_state_pdo_count;

   198

        ec_fsm_coe_upload(fsm->fsm_coe, fsm->slave, &fsm->request);

   199

        ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately

   200

        return;

   201

    }

   202

   203

    fsm->state = ec_fsm_coe_map_state_end;

   204

}

   205

   206

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

   207

   208

/**

   209

 */

   210

   211

void ec_fsm_coe_map_state_pdo_count(

   212

        ec_fsm_coe_map_t *fsm /**< finite state machine */

   213

        )

   214

{

   215

    if (ec_fsm_coe_exec(fsm->fsm_coe)) return;

   216

   217

    if (!ec_fsm_coe_success(fsm->fsm_coe)) {

   218

        EC_ERR("Failed to read number of mapped PDOs from slave %u.\n",

   219

                fsm->slave->ring_position);

   220

        fsm->state = ec_fsm_coe_map_state_error;

   221

        return;

   222

    }

   223

   224

    fsm->sync_subindices = EC_READ_U8(fsm->request.data);

   225

   226

    if (fsm->slave->master->debug_level)

   227

        EC_DBG("  %u PDOs mapped.\n", fsm->sync_subindices);

   228

   229

    // read first PDO

   230

    fsm->sync_subindex = 1;

   231

    ec_fsm_coe_map_action_next_pdo(fsm);

   232

}

   233

   234

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

   235

   236

/**

   237

 */

   238

   239

void ec_fsm_coe_map_action_next_pdo(

   240

        ec_fsm_coe_map_t *fsm /**< finite state machine */

   241

        )

   242

{

   243

    ec_sdo_entry_t *entry;

   244

   245

    if (fsm->sync_subindex <= fsm->sync_subindices) {

   246

        if (!(entry = ec_sdo_get_entry(fsm->sync_sdo,

   247

                        fsm->sync_subindex))) {

   248

            EC_ERR("SDO 0x%04X has no subindex %u on slave %u.\n",

   249

                    fsm->sync_sdo->index,

   250

                    fsm->sync_subindex,

   251

                    fsm->slave->ring_position);

   252

            fsm->state = ec_fsm_coe_map_state_error;

   253

            return;

   254

        }

   255

   256

        ec_sdo_request_init_read(&fsm->request, entry);

   257

        fsm->state = ec_fsm_coe_map_state_pdo;

   258

        ec_fsm_coe_upload(fsm->fsm_coe, fsm->slave, &fsm->request);

   259

        ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately

   260

        return;

   261

    }

   262

   278

}

   279

   280

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

   281

   282

/**

   283

 */

   284

   285

void ec_fsm_coe_map_state_pdo(

   286

        ec_fsm_coe_map_t *fsm /**< finite state machine */

   287

        )

   288

{

   289

    if (ec_fsm_coe_exec(fsm->fsm_coe)) return;

   290

   291

    if (!ec_fsm_coe_success(fsm->fsm_coe)) {

   292

        EC_ERR("Failed to read mapped PDO index from slave %u.\n",

   293

                fsm->slave->ring_position);

   294

        fsm->state = ec_fsm_coe_map_state_error;

   295

        return;

   296

    }

   297

   298

    {

   299

        ec_sdo_entry_t *entry;

   300

   313

        if (fsm->slave->master->debug_level)

   317

            EC_ERR("Slave %u has no SDO 0x%04X.\n",

   338

        }

   339

   340

        if (!(entry = ec_sdo_get_entry(fsm->pdo_sdo, 0))) {

   341

            EC_ERR("SDO 0x%04X has no subindex 0 on slave %u.\n",

   342

                    fsm->pdo_sdo->index,

   343

                    fsm->slave->ring_position);

   351

   352

        ec_sdo_request_init_read(&fsm->request, entry);

   353

        fsm->state = ec_fsm_coe_map_state_pdo_entry_count;

   354

        ec_fsm_coe_upload(fsm->fsm_coe, fsm->slave, &fsm->request);

   355

        ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately

   356

        return;

   357

    }

   358

}

   359

   360

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

   361

   362

/**

   363

 */

   364

   365

void ec_fsm_coe_map_state_pdo_entry_count(

   366

        ec_fsm_coe_map_t *fsm /**< finite state machine */

   367

        )

   368

{

   369

    if (ec_fsm_coe_exec(fsm->fsm_coe)) return;

   370

   371

    if (!ec_fsm_coe_success(fsm->fsm_coe)) {

   372

        EC_ERR("Failed to read number of mapped PDO entries from slave %u.\n",

   373

                fsm->slave->ring_position);

   374

        fsm->state = ec_fsm_coe_map_state_error;

   375

        return;

   376

    }

   377

   378

    fsm->pdo_subindices = EC_READ_U8(fsm->request.data);

   379

   380

    if (fsm->slave->master->debug_level)

   381

        EC_DBG("    %u PDO entries mapped.\n", fsm->pdo_subindices);

   382

   383

    // read first PDO entry

   384

    fsm->pdo_subindex = 1;

   385

    ec_fsm_coe_map_action_next_pdo_entry(fsm);

   386

}

   387

   388

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

   389

   390

/**

   391

 */

   392

   393

void ec_fsm_coe_map_action_next_pdo_entry(

   394

        ec_fsm_coe_map_t *fsm /**< finite state machine */

   395

        )

   396

{

   397

    ec_sdo_entry_t *entry;

   398

   399

    if (fsm->pdo_subindex <= fsm->pdo_subindices) {

   400

        if (!(entry = ec_sdo_get_entry(fsm->pdo_sdo,

   401

                        fsm->pdo_subindex))) {

   402

            EC_ERR("SDO 0x%04X has no subindex %u on slave %u.\n",

   403

                    fsm->pdo_sdo->index, fsm->pdo_subindex,

   404

                    fsm->slave->ring_position);

   405

            fsm->state = ec_fsm_coe_map_state_error;

   406

            return;

   407

        }

   408

   409

        ec_sdo_request_init_read(&fsm->request, entry);

   410

        fsm->state = ec_fsm_coe_map_state_pdo_entry;

   411

        ec_fsm_coe_upload(fsm->fsm_coe, fsm->slave, &fsm->request);

   412

        ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately

   413

        return;

   414

    }

   415

   416

    // next PDO

   417

    fsm->sync_subindex++;

   418

    ec_fsm_coe_map_action_next_pdo(fsm);

   419

}

   420

   421

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

   422

   423

/**

   424

 */

   425

   426

void ec_fsm_coe_map_state_pdo_entry(

   427

        ec_fsm_coe_map_t *fsm /**< finite state machine */

   428

        )

   429

{

   430

    if (ec_fsm_coe_exec(fsm->fsm_coe)) return;

   431

   432

    if (!ec_fsm_coe_success(fsm->fsm_coe)) {

   433

        EC_ERR("Failed to read index of mapped PDO entry from slave %u.\n",

   434

                fsm->slave->ring_position);

   435

        fsm->state = ec_fsm_coe_map_state_error;

   436

        return;

   437

    }

   438

   439

    {

   440

        uint32_t pdo_entry_info;

   441

        ec_sdo_t *sdo;

   444

   445

        pdo_entry_info = EC_READ_U32(fsm->request.data);

   459

            EC_ERR("Slave %u has no SDO 0x%04X.\n",

   486

        }

   487

   488

        if (fsm->slave->master->debug_level) {

   489

            EC_DBG("    PDO entry 0x%04X \"%s\" (%u bit).\n",

   495

   496

        // next PDO entry

   497

        fsm->pdo_subindex++;

   498

        ec_fsm_coe_map_action_next_pdo_entry(fsm);

   499

    }

   500

}

   501

   502

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

   503

   504

/**

   505

   State: ERROR.

   506

*/

   507

   508

void ec_fsm_coe_map_state_error(

   509

        ec_fsm_coe_map_t *fsm /**< finite state machine */

   510

        )

   511

{

   512

}

   513

   514

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

   515

   516

/**

   517

   State: END.

   518

*/

   519