1

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

     2

 *

     3

 *  $Id$

     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

    37

    38

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

    39

    40

#include "globals.h"

    41

#include "master.h"

    42

#include "mailbox.h"

    45

#include "fsm_mapping.h"

    46

    47

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

    48

    49

void ec_fsm_mapping_state_start(ec_fsm_mapping_t *);

    50

void ec_fsm_mapping_state_zero_count(ec_fsm_mapping_t *);

    51

void ec_fsm_mapping_state_add_pdo(ec_fsm_mapping_t *);

    52

void ec_fsm_mapping_state_pdo_count(ec_fsm_mapping_t *);

    53

void ec_fsm_mapping_state_end(ec_fsm_mapping_t *);

    54

void ec_fsm_mapping_state_error(ec_fsm_mapping_t *);

    55

    56

void ec_fsm_mapping_next_sync(ec_fsm_mapping_t *);

    57

    58

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

    59

    64

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

    65

        )

    66

{

    67

    fsm->fsm_coe = fsm_coe;

    68

    fsm->sdodata.data = (uint8_t *) &fsm->sdo_value;

    69

}

    70

    71

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

    72

    85

void ec_fsm_mapping_start(

    86

        ec_fsm_mapping_t *fsm, /**< mapping state machine */

    87

        ec_slave_t *slave /**< slave to configure */

    88

        )

    89

{

    90

    fsm->slave = slave;

    91

    fsm->state = ec_fsm_mapping_state_start;

    92

}

    93

    94

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

    95

    98

 * \return false, if state machine has terminated

    99

 */

   100

int ec_fsm_mapping_running(

   101

        const ec_fsm_mapping_t *fsm /**< mapping state machine */

   102

        )

   103

{

   104

    return fsm->state != ec_fsm_mapping_state_end

   105

        && fsm->state != ec_fsm_mapping_state_error;

   106

}

   107

   108

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

   109

   112

 * If the state machine's datagram is not sent or received yet, the execution

   113

 * of the state machine is delayed to the next cycle.

   115

 * \return false, if state machine has terminated

   116

 */

   117

int ec_fsm_mapping_exec(

   118

        ec_fsm_mapping_t *fsm /**< mapping state machine */

   119

        )

   120

{

   121

    fsm->state(fsm);

   122

    return ec_fsm_mapping_running(fsm);

   123

}

   124

   125

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

   126

   129

 * \return true, if the state machine terminated gracefully

   130

 */

   131

int ec_fsm_mapping_success(

   132

        const ec_fsm_mapping_t *fsm /**< mapping state machine */

   133

        )

   134

{

   135

    return fsm->state == ec_fsm_mapping_state_end;

   136

}

   137

   138

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

   140

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

   141

   144

void ec_fsm_mapping_state_start(

   145

        ec_fsm_mapping_t *fsm /**< mapping state machine */

   146

        )

   147

{

   153

    fsm->dir = EC_DIR_OUTPUT;

   154

    ec_fsm_mapping_next_sync(fsm);

   155

}

   156

   157

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

   158

   161

void ec_fsm_mapping_next_sync(

   162

        ec_fsm_mapping_t *fsm /**< mapping state machine */

   163

        )

   164

{

   165

    while (1) {

   166

        if (fsm->dir > EC_DIR_INPUT) {

   167

            // no more directions to configure mappings for

   168

            fsm->state = ec_fsm_mapping_state_end;

   169

            return;

   170

        }

   171

   172

        if (!(fsm->sync = ec_slave_get_pdo_sync(fsm->slave, fsm->dir))) {

   174

            fsm->dir++;

   175

            continue;

   176

        }

   177

   186

            break;

   188

    }

   189

   190

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

   192

                fsm->sync->index, fsm->slave->ring_position);

   193

    }

   194

   195

    // set mapped PDO count to zero

   196

    fsm->sdodata.index = 0x1C10 + fsm->sync->index;

   197

    fsm->sdodata.subindex = 0; // mapped PDO count

   198

    EC_WRITE_U8(&fsm->sdo_value, 0); // zero PDOs mapped

   199

    fsm->sdodata.size = 1;

   200

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

   201

        EC_DBG("Setting PDO count to zero for SM%u.\n", fsm->sync->index);

   202

   203

    fsm->state = ec_fsm_mapping_state_zero_count;

   204

    ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->sdodata);

   205

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

   206

}

   207

   208

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

   209

   212

ec_pdo_t *ec_fsm_mapping_next_pdo(

   227

void ec_fsm_mapping_state_zero_count(

   228

        ec_fsm_mapping_t *fsm /**< mapping state machine */

   229

        )

   230

{

   231

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

   232

   233

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

   234

        EC_ERR("Failed to clear PDO mapping for slave %u.\n",

   235

                fsm->slave->ring_position);

   236

        fsm->state = ec_fsm_mapping_state_error;

   237

        return;

   238

    }

   239

   240

    // map all PDOs belonging to the current sync manager

   241

   242

    // find first PDO

   244

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

   245

            EC_DBG("No PDOs to map for SM%u of slave %u.\n",

   246

                    fsm->sync->index, fsm->slave->ring_position);

   247

        ec_fsm_mapping_next_sync(fsm);

   248

        return;

   249

    }

   250

   251

    // add first PDO to mapping

   252

    fsm->pdo_count = 1;

   253

    fsm->sdodata.subindex = fsm->pdo_count;

   254

    EC_WRITE_U16(&fsm->sdo_value, fsm->pdo->index);

   255

    fsm->sdodata.size = 2;

   256

   257

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

   258

        EC_DBG("Mapping PDO 0x%04X at position %u.\n",

   259

                fsm->pdo->index, fsm->sdodata.subindex);

   260

   261

    fsm->state = ec_fsm_mapping_state_add_pdo;

   262

    ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->sdodata);

   263

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

   264

}

   265

   266

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

   267

   270

void ec_fsm_mapping_state_add_pdo(

   271

        ec_fsm_mapping_t *fsm /**< mapping state machine */

   272

        )

   273

{

   274

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

   275

   276

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

   277

        EC_ERR("Failed to map PDO 0x%04X for SM%u of slave %u.\n",

   278

                fsm->pdo->index, fsm->sync->index, fsm->slave->ring_position);

   279

        fsm->state = ec_fsm_mapping_state_error;

   280

        return;

   281

    }

   282

   283

    // find next PDO

   285

        // no more PDOs to map. write PDO count

   286

        fsm->sdodata.subindex = 0;

   287

        EC_WRITE_U8(&fsm->sdo_value, fsm->pdo_count);

   288

        fsm->sdodata.size = 1;

   289

   290

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

   291

            EC_DBG("Setting number of mapped PDOs to %u.\n",

   292

                    fsm->pdo_count);

   293

   294

        fsm->state = ec_fsm_mapping_state_pdo_count;

   295

        ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->sdodata);

   296

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

   297

        return;

   298

    }

   299

   300

    // add next PDO to mapping

   301

    fsm->pdo_count++;

   302

    fsm->sdodata.subindex = fsm->pdo_count;

   303

    EC_WRITE_U16(&fsm->sdo_value, fsm->pdo->index);

   304

    fsm->sdodata.size = 2;

   305

   306

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

   307

        EC_DBG("Mapping PDO 0x%04X at position %u.\n",

   308

                fsm->pdo->index, fsm->sdodata.subindex);

   309

   310

    ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->sdodata);

   311

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

   312

}

   313

   314

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

   315

   318

void ec_fsm_mapping_state_pdo_count(

   319

        ec_fsm_mapping_t *fsm /**< mapping state machine */

   320

        )

   321

{

   322

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

   323

   324

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

   325

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

   326

                fsm->slave->ring_position);

   327

        fsm->state = ec_fsm_mapping_state_error;

   328

        return;

   329

    }

   330

   331

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

   332

        EC_DBG("Successfully set PDO mapping for SM%u of slave %u.\n",

   333

                fsm->sync->index, fsm->slave->ring_position);

   334

   335

    // mapping configuration for this sync manager complete.

   336

    ec_fsm_mapping_next_sync(fsm);

   337

}

   338

   339

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

   340

 * Common state functions

   341

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

   342

   345

void ec_fsm_mapping_state_error(

   346

        ec_fsm_mapping_t *fsm /**< mapping state machine */

   347

        )

   348

{

   349

}

   350

   351

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

   352

   355

void ec_fsm_mapping_state_end(

   356

        ec_fsm_mapping_t *fsm /**< mapping state machine */

   357

        )

   358

{

   359

}

   360

   361

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