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

** @file   sync.c

    25

** @author Edouard TISSERANT and Francis DUPIN

    26

** @date   Tue Jun  5 09:32:32 2007

    27

**

    28

** @brief

    29

**

    30

**

    31

*/

    32

    33

#include "data.h"

    34

#include "sync.h"

    35

#include "canfestival.h"

    36

    37

/* Prototypes for internals functions */

    38

    39

/*!                                                                                                

    40

**                                                                                                 

    41

**                                                                                                 

    42

** @param d                                                                                        

    43

** @param id                                                                                       

    44

**/  

    45

void SyncAlarm(CO_Data* d, UNS32 id);

    46

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

    47

	UNS8 unsused_bSubindex);

    48

    49

/*!                                                                                                

    50

**                                                                                                 

    51

**                                                                                                 

    52

** @param d                                                                                        

    53

** @param id                                                                                       

    54

**/   

    55

void SyncAlarm(CO_Data* d, UNS32 id)

    56

{

    57

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

    58

}

    59

    60

/*!                                                                                                

    61

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

    62

**                                                                                                 

    63

** @param d                                                                                        

    64

** @param unsused_indextable                                                                       

    65

** @param unsused_bSubindex                                                                        

    66

**                                                                                                 

    67

** @return                                                                                         

    68

**/  

    69

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

    70

{

    71

	startSYNC(d);

    72

	return 0;

    73

}

    74

    75

/*!                                                                                                

    76

**                                                                                                 

    77

**                                                                                                 

    78

** @param d                                                                                        

    79

**/ 

    80

void startSYNC(CO_Data* d)

    81

{

    82

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

    83

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

    84

    85

	if(d->syncTimer != TIMER_NONE){

    86

		stopSYNC(d);

    87

	}

    88

    89

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

    90

	{

    91

		d->syncTimer = SetAlarm(

    92

				d,

    93

				0 /*No id needed*/,

    94

				&SyncAlarm,

    95

				US_TO_TIMEVAL(*d->Sync_Cycle_Period), 

    96

				US_TO_TIMEVAL(*d->Sync_Cycle_Period));

    97

	}

    98

}

    99

   100

/*!                                                                                                

   101

**                                                                                                 

   102

**                                                                                                 

   103

** @param d                                                                                        

   104

**/   

   105

void stopSYNC(CO_Data* d)

   106

{

   107

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

   108

}

   109

   110

/*!                                                                                                

   111

**                                                                                                 

   112

**                                                                                                 

   113

** @param d                                                                                        

   114

** @param cob_id                                                                                   

   115

**                                                                                                 

   116

** @return                                                                                         

   117

**/  

   118

UNS8 sendSYNC(CO_Data* d, UNS32 cob_id)

   119

{

   120

  Message m;

   121

  UNS8 resultat ;

   122

   123

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

   124

   125

  m.cob_id.w = cob_id ;

   126

  m.rtr = NOT_A_REQUEST;

   127

  m.len = 0;

   128

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

   129

  proceedSYNC(d, &m) ; 

   130

  return resultat ;

   131

}

   132

   133

/*!                                                                                                

   134

**                                                                                                 

   135

**                                                                                                 

   136

** @param d                                                                                        

   137

** @param m                                                                                        

   138

**                                                                                                 

   139

** @return                                                                                         

   140

**/ 

   141

UNS8 proceedSYNC(CO_Data* d, Message *m)

   142

{

   143

   144

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

   145

        prp_j;

   146

   147

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

   148

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

   149

  /* void *     pMappedAppObject = NULL; */

   150

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

   151

  UNS32 *    pMappingParameter = NULL;  

   152

  /* pointer to the transmissiontype...*/

   153

  UNS8 *     pTransmissionType = NULL;  

   154

  UNS32 *    pwCobId = NULL;	

   155

   156

  UNS8 dataType;

   157

  UNS16 index;

   158

  UNS8 subIndex;

   159

  UNS8 offset;

   160

  UNS8 status;

   161

  UNS8 Size;

   162

  UNS32 objDict;	

   163

  UNS16 offsetObjdict;

   164

  UNS16 offsetObjdictMap;

   165

  UNS16 lastIndex;

   166

   167

  status = state3;

   168

  pdoNum = 0x00;

   169

  prp_j = 0x00;

   170

  offset = 0x00;

   171

   172

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

   173

   174

  (*d->post_sync)();

   175

   176

  /* only operational state allows PDO transmission */

   177

  if( d->nodeState != Operational ) 

   178

    return 0;

   179

   180

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

   181

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

   182

   183

  offsetObjdict = d->firstIndex->PDO_TRS;

   184

  lastIndex = d->lastIndex->PDO_TRS;

   185

  offsetObjdictMap = d->firstIndex->PDO_TRS_MAP;

   186

   187

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

   188

    switch( status ) {

   189

   190

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

   191

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

   192

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

   193

	  return 0xFF;

   194

	}

   195

      pTransmissionType = (UNS8*) d->objdict[offsetObjdict].pSubindex[2].pObject;    

   196

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

   197

      status = state4; 

   198

      break;     

   199

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

   200

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

   201

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

   202

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

   203

	d->count_sync[pdoNum] = 0;

   204

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

   205

	status = state5;

   206

	break;

   207

      }

   208

      else {

   209

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

   210

		*pTransmissionType);

   211

	pdoNum++;

   212

	offsetObjdict++;

   213

	offsetObjdictMap++;

   214

	status = state11;

   215

	break;

   216

      }      

   217

    case state5:	/* get PDO CobId */

   218

        pwCobId = (UNS32*) d->objdict[offsetObjdict].pSubindex[1].pObject;     

   219

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

   220

	status = state7;

   221

	break;     

   222

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

   223

      pMappingCount = (UNS8*) d->objdict[offsetObjdictMap].pSubindex[0].pObject;

   224

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

   225

	status = state8;

   226

    case state8:	/* get mapping parameters */

   227

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

   228

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

   229

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

   230

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

   231

	status = state9;

   232

   233

    case state9:	/* get data to transmit */ 

   234

	{

   235

	  UNS8 ByteSize;

   236

	  UNS8 tmp[]= {0,0,0,0,0,0,0,0};

   237

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

   238

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

   239

	  Size = (UNS8)(*pMappingParameter); /* Size in bits */

   240

	  ByteSize = 1 + ((Size - 1) >> 3); /*1->8 => 1 ; 9->16 => 2, ... */

   241

	  objDict = getODentry(d, index, subIndex, tmp, &ByteSize, &dataType, 0 );

   242

	  /* copy bit per bit in little endian*/

   243

	  CopyBits(Size, ((UNS8*)tmp), 0 , 0, (UNS8*)&d->process_var.data[offset>>3], offset%8, 0);

   244

	}   

   245

        if( objDict != OD_SUCCESSFUL ){

   246

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

   247

          return 0xFF;

   248

        }

   249

   250

	offset += Size ;

   251

	d->process_var.count = 1 + ((offset - 1) >> 3);

   252

	prp_j++;

   253

	status = state10;	 

   254

	break;					

   255

   256

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

   257

      if( prp_j < *pMappingCount ){

   258

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

   259

	status = state8;

   260

	break;

   261

      }

   262

      else {

   263

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

   264

	PDOmGR( d, *pwCobId );	

   265

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

   266

	pdoNum++;

   267

	offsetObjdict++;

   268

	offsetObjdictMap++;

   269

	offset = 0x00;

   270

	prp_j = 0x00;

   271

	status = state11;

   272

	break;

   273

      }

   274

   275

    case state11:     

   276

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

   277

      status = state3;

   278

      break;

   279

   280

    default:

   281

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

   282

      return 0xFF;

   283

    }/* end switch case */

   284

   285

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

   286

   287

  (*d->post_TPDO)();

   288

   289

  return 0;

   290

}

   291

   292

   293

void _post_sync(){}

   294

void _post_TPDO(){}