1

/*

     2

This file is part of CanFestival, a library implementing CanOpen Stack. 

     3

     4

Copyright (C): Edouard TISSERANT and Francis DUPIN

     5

     6

See COPYING file for copyrights details.

     7

     8

This library is free software; you can redistribute it and/or

     9

modify it under the terms of the GNU Lesser General Public

    10

License as published by the Free Software Foundation; either

    11

version 2.1 of the License, or (at your option) any later version.

    12

    13

This library is distributed in the hope that it will be useful,

    14

but WITHOUT ANY WARRANTY; without even the implied warranty of

    15

MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU

    16

Lesser General Public License for more details.

    17

    18

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

    19

License along with this library; if not, write to the Free Software

    20

Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

    21

*/

    22

    23

    24

#include "data.h"

    25

#include "sync.h"

    26

#include "canfestival.h"

    27

    28

/* Prototypes for internals functions */

    29

void SyncAlarm(CO_Data* d, UNS32 id);

    30

UNS32 OnCOB_ID_SyncUpdate(CO_Data* d, const indextable * unsused_indextable, 

    31

	UNS8 unsused_bSubindex);

    32

    33

    34

    35

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

    36

void SyncAlarm(CO_Data* d, UNS32 id)

    37

{

    38

	sendSYNC(d, *d->COB_ID_Sync & 0x1FFFFFFF) ;

    39

}

    40

    41

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

    42

/* This is called when Index 0x1005 is updated.*/

    43

UNS32 OnCOB_ID_SyncUpdate(CO_Data* d, const indextable * unsused_indextable, UNS8 unsused_bSubindex)

    44

{

    45

	startSYNC(d);

    46

	return 0;

    47

}

    48

    49

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

    50

void startSYNC(CO_Data* d)

    51

{

    52

	RegisterSetODentryCallBack(d, 0x1005, 0, &OnCOB_ID_SyncUpdate);

    53

	RegisterSetODentryCallBack(d, 0x1006, 0, &OnCOB_ID_SyncUpdate);

    54

    55

	if(d->syncTimer != TIMER_NONE){

    56

		stopSYNC(d);

    57

	}

    58

    59

	if(*d->COB_ID_Sync & 0x40000000 && *d->Sync_Cycle_Period)

    60

	{

    61

		d->syncTimer = SetAlarm(

    62

				d,

    63

				0 /*No id needed*/,

    64

				&SyncAlarm,

    65

				US_TO_TIMEVAL(*d->Sync_Cycle_Period), 

    66

				US_TO_TIMEVAL(*d->Sync_Cycle_Period));

    67

	}

    68

}

    69

    70

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

    71

void stopSYNC(CO_Data* d)

    72

{

    73

	d->syncTimer = DelAlarm(d->syncTimer);

    74

}

    75

    76

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

    77

UNS8 sendSYNC(CO_Data* d, UNS32 cob_id)

    78

{

    79

  Message m;

    80

  UNS8 resultat ;

    81

    82

  MSG_WAR(0x3001, "sendSYNC ", 0);

    83

    84

  m.cob_id.w = cob_id ;

    85

  m.rtr = NOT_A_REQUEST;

    86

  m.len = 0;

    87

  resultat = canSend(d->canHandle,&m) ;

    88

  proceedSYNC(d, &m) ; 

    89

  return resultat ;

    90

}

    91

    92

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

    93

UNS8 proceedSYNC(CO_Data* d, Message *m)

    94

{

    95

    96

  UNS8 	pdoNum,       /* number of the actual processed pdo-nr. */

    97

        prp_j;

    98

    99

  const UNS8 *     pMappingCount = NULL;      /* count of mapped objects...*/

   100

  /* pointer to the var which is mapped to a pdo */

   101

  /* void *     pMappedAppObject = NULL; */

   102

  /* pointer fo the var which holds the mapping parameter of an mapping entry  */

   103

  UNS32 *    pMappingParameter = NULL;  

   104

  /* pointer to the transmissiontype...*/

   105

  UNS8 *     pTransmissionType = NULL;  

   106

  UNS32 *    pwCobId = NULL;	

   107

   108

  UNS8      dataType;

   109

  UNS16 index;

   110

  UNS8 subIndex;

   111

  UNS8 offset;

   112

  UNS8 status;

   113

  UNS8 sizeData;

   114

  UNS32   objDict;	

   115

  UNS16 offsetObjdict;

   116

  UNS16 offsetObjdictMap;

   117

  UNS16 lastIndex;

   118

   119

  status = state3;

   120

  pdoNum = 0x00;

   121

  prp_j = 0x00;

   122

  offset = 0x00;

   123

   124

  MSG_WAR(0x3002, "SYNC received. Proceed. ", 0);

   125

   126

  (*d->post_sync)();

   127

   128

  /* only operational state allows PDO transmission */

   129

  if( d->nodeState != Operational ) 

   130

    return 0;

   131

   132

  /* So, the node is in operational state */

   133

  /* study all PDO stored in the objects dictionary */	

   134

   135

  offsetObjdict = d->firstIndex->PDO_TRS;

   136

  lastIndex = d->lastIndex->PDO_TRS;

   137

  offsetObjdictMap = d->firstIndex->PDO_TRS_MAP;

   138

   139

  if(offsetObjdict) while( offsetObjdict <= lastIndex) {  

   140

    switch( status ) {

   141

   142

    case state3:    /* get the PDO transmission type */

   143

      if (d->objdict[offsetObjdict].bSubCount <= 2) {

   144

	  MSG_ERR(0x1004, "Subindex 2  not found at index ", 0x1800 + pdoNum);

   145

	  return 0xFF;

   146

	}

   147

      pTransmissionType = d->objdict[offsetObjdict].pSubindex[2].pObject;    

   148

      MSG_WAR(0x3005, "Reading PDO at index : ", 0x1800 + pdoNum);

   149

      status = state4; 

   150

      break;     

   151

    case state4:	/* check if transmission type is after (this) SYNC */

   152

                        /* The message may not be transmited every SYNC but every n SYNC */      

   153

      if( (*pTransmissionType >= TRANS_SYNC_MIN) && (*pTransmissionType <= TRANS_SYNC_MAX) &&

   154

          (++d->count_sync[pdoNum] == *pTransmissionType) ) {	

   155

	d->count_sync[pdoNum] = 0;

   156

	MSG_WAR(0x3007, "  PDO is on SYNCHRO. Trans type : ", *pTransmissionType);

   157

	status = state5;

   158

	break;

   159

      }

   160

      else {

   161

	MSG_WAR(0x3008, "  Not on synchro or not at this SYNC. Trans type : ", 

   162

		*pTransmissionType);

   163

	pdoNum++;

   164

	offsetObjdict++;

   165

	offsetObjdictMap++;

   166

	status = state11;

   167

	break;

   168

      }      

   169

    case state5:	/* get PDO CobId */

   170

        pwCobId = d->objdict[offsetObjdict].pSubindex[1].pObject;     

   171

	MSG_WAR(0x3009, "  PDO CobId is : ", *pwCobId);

   172

	status = state7;

   173

	break;     

   174

    case state7:  /* get mapped objects number to transmit with this PDO */

   175

      pMappingCount = d->objdict[offsetObjdictMap].pSubindex[0].pObject;

   176

	MSG_WAR(0x300D, "  Number of objects mapped : ",*pMappingCount );

   177

	status = state8;

   178

    case state8:	/* get mapping parameters */

   179

      pMappingParameter = d->objdict[offsetObjdictMap].pSubindex[prp_j + 1].pObject;

   180

	MSG_WAR(0x300F, "  got mapping parameter : ", *pMappingParameter);

   181

	MSG_WAR(0x3050, "    at index : ", 0x1A00 + pdoNum);

   182

	MSG_WAR(0x3051, "    sub-index : ", prp_j + 1);

   183

	status = state9;

   184

   185

    case state9:	/* get data to transmit */ 

   186

      index = (UNS16)((*pMappingParameter) >> 16);

   187

      subIndex = (UNS8)(( (*pMappingParameter) >> (UNS8)8 ) & (UNS32)0x000000FF);

   188

      /* <<3 because in *pMappingParameter the size is in bits */

   189

      sizeData = (UNS8) ((*pMappingParameter & (UNS32)0x000000FF) >> 3) ;

   190

   191

        objDict = getODentry(d, index, subIndex, (void *)&d->process_var.data[offset], &sizeData, &dataType, 0 ); 

   192

   193

        if( objDict != OD_SUCCESSFUL ){

   194

          MSG_ERR(0x1013, " Couldn't find mapped variable at index-subindex-size : ", (UNS16)(*pMappingParameter));

   195

          return 0xFF;

   196

        }

   197

   198

	offset += sizeData ;

   199

	d->process_var.count = offset;

   200

	prp_j++;

   201

	status = state10;	 

   202

	break;					

   203

   204

    case state10:	/* loop to get all the data to transmit */

   205

      if( prp_j < *pMappingCount ){

   206

	MSG_WAR(0x3014, "  next variable mapped : ", prp_j);

   207

	status = state8;

   208

	break;

   209

      }

   210

      else {

   211

	MSG_WAR(0x3015, "  End scan mapped variable", 0);

   212

	PDOmGR( d, *pwCobId );	

   213

	MSG_WAR(0x3016, "  End of this pdo. Should have been sent", 0);

   214

	pdoNum++;

   215

	offsetObjdict++;

   216

	offsetObjdictMap++;

   217

	offset = 0x00;

   218

	prp_j = 0x00;

   219

	status = state11;

   220

	break;

   221

      }

   222

   223

    case state11:     

   224

      MSG_WAR(0x3017, "next pdo index : ", pdoNum);

   225

      status = state3;

   226

      break;

   227

   228

    default:

   229

      MSG_ERR(0x1019,"Unknown state has been reached : %d",status);

   230

      return 0xFF;

   231

    }/* end switch case */

   232

   233

  }/* end while( prp_i<dict_cstes.max_count_of_PDO_transmit ) */

   234

   235

  (*d->post_TPDO)();

   236

   237

  return 0;

   238

}

   239

   240

   241

void _post_sync(){}

   242

void _post_TPDO(){}