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

#include "Master.h"

    24

#include "Slave.h"

    25

#include "TestMasterSlave.h"

    26

#include "can_driver.h"

    27

    28

extern s_BOARD MasterBoard;

    29

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

    30

void TestMaster_heartbeatError(CO_Data* d, UNS8 heartbeatID)

    31

{

    32

	eprintf("TestMaster_heartbeatError %d\n", heartbeatID);

    33

}

    34

    35

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

    36

 * ConfigureSlaveNode is responsible to

    37

 *  - setup master RPDO 1 to receive TPDO 1 from id 2

    38

 *  - setup master RPDO 2 to receive TPDO 2 from id 2

    39

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

    40

void TestMaster_initialisation(CO_Data* d)

    41

{

    42

	UNS32 PDO1_COBID = 0x0182; 

    43

	UNS32 PDO2_COBID = 0x0282;

    44

	UNS8 size = sizeof(UNS32); 

    45

//	UNS32 SINC_cicle=0;

    46

//	UNS8 data_type = 0;

    47

    48

	eprintf("TestMaster_initialisation\n");

    49

    50

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

    51

	 * Define RPDOs to match slave ID=2 TPDOs*

    52

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

    53

	writeLocalDict( &TestMaster_Data, /*CO_Data* d*/

    54

			0x1400, /*UNS16 index*/

    55

			0x01, /*UNS8 subind*/ 

    56

			&PDO1_COBID, /*void * pSourceData,*/ 

    57

			&size, /* UNS8 * pExpectedSize*/

    58

			RW);  /* UNS8 checkAccess */

    59

    60

	writeLocalDict( &TestMaster_Data, /*CO_Data* d*/

    61

			0x1401, /*UNS16 index*/

    62

			0x01, /*UNS8 subind*/ 

    63

			&PDO2_COBID, /*void * pSourceData,*/ 

    64

			&size, /* UNS8 * pExpectedSize*/

    65

			RW);  /* UNS8 checkAccess */

    66

    67

}

    68

    69

// Step counts number of times ConfigureSlaveNode is called

    70

static int init_step = 0;

    71

    72

/*Froward declaration*/

    73

static void ConfigureSlaveNode(CO_Data* d, UNS8 nodeId);

    74

    75

/**/

    76

static void CheckSDOAndContinue(CO_Data* d, UNS8 nodeId)

    77

{

    78

	UNS32 abortCode;	

    79

	if(getWriteResultNetworkDict (d, nodeId, &abortCode) != SDO_FINISHED)

    80

		eprintf("Master : Failed in initializing slave %2.2x, step %d, AbortCode :%4.4x \n", nodeId, init_step, abortCode);

    81

    82

	/* Finalise last SDO transfer with this node */

    83

	closeSDOtransfer(&TestMaster_Data, nodeId, SDO_CLIENT);

    84

    85

	ConfigureSlaveNode(d, nodeId);

    86

}

    87

    88

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

    89

 * ConfigureSlaveNode is responsible to

    90

 *  - setup slave TPDO 1 transmit type

    91

 *  - setup slave TPDO 2 transmit type

    92

 *  - switch to operational mode

    93

 *  - send NMT to slave

    94

 ********************************************************

    95

 * This an example of :

    96

 * Network Dictionary Access (SDO) with Callback 

    97

 * Slave node state change request (NMT) 

    98

 ********************************************************

    99

 * This is called first by TestMaster_post_SlaveBootup

   100

 * then it called again each time a SDO exchange is

   101

 * finished.

   102

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

   103

   104

static void ConfigureSlaveNode(CO_Data* d, UNS8 nodeId)

   105

{

   106

	/* Master configure heartbeat producer time at 1000 ms 

   107

	 * for slave node-id 0x02 by DCF concise */

   108

   109

	UNS8 Transmission_Type = 0x01;

   110

//	UNS32 abortCode;

   111

	UNS8 res;

   112

	eprintf("Master : ConfigureSlaveNode %2.2x\n", nodeId);

   113

   114

	switch(++init_step){

   115

		case 1: /*First step : setup Slave's TPDO 1 to be transmitted on SYNC*/

   116

			eprintf("Master : set slave %2.2x TPDO 1 transmit type\n", nodeId);

   117

			res = writeNetworkDictCallBack (d, /*CO_Data* d*/

   118

					nodeId, /*UNS8 nodeId*/

   119

					0x1800, /*UNS16 index*/

   120

					0x02, /*UNS8 subindex*/

   121

					1, /*UNS8 count*/

   122

					0, /*UNS8 dataType*/

   123

					&Transmission_Type,/*void *data*/

   124

					CheckSDOAndContinue); /*SDOCallback_t Callback*/

   125

					break;

   126

   127

		case 2:	/*Second step*/

   128

			eprintf("Master : set slave %2.2x TPDO 2 transmit type\n", nodeId);

   129

			writeNetworkDictCallBack (d, /*CO_Data* d*/

   130

					nodeId, /*UNS8 nodeId*/

   131

					0x1801, /*UNS16 index*/

   132

					0x02, /*UNS16 index*/

   133

					1, /*UNS8 count*/

   134

					0, /*UNS8 dataType*/

   135

					&Transmission_Type,/*void *data*/

   136

					CheckSDOAndContinue); /*SDOCallback_t Callback*/

   137

					break;

   138

		case 3: 

   139

   140

		/****************************** START *******************************/

   141

   142

			/* Put the master in operational mode */

   143

			setState(d, Operational);

   144

   145

			/* Ask slave node to go in operational mode */

   146

			masterSendNMTstateChange (d, nodeId, NMT_Start_Node);

   147

   148

	}

   149

}

   150

   151

   152

void TestMaster_preOperational(CO_Data* d)

   153

{

   154

	eprintf("TestMaster_preOperational\n");

   155

}

   156

   157

void TestMaster_operational(CO_Data* d)

   158

{

   159

	eprintf("TestMaster_operational\n");

   160

}

   161

   162

void TestMaster_stopped(CO_Data* d)

   163

{

   164

	eprintf("TestMaster_stopped\n");

   165

}

   166

   167

void TestMaster_post_sync(CO_Data* d)

   168

{

   169

	eprintf("TestMaster_post_sync\n");

   170

	eprintf("Master: %d %d %d %d %d %d %d %d %d %x %x %d %d\n",

   171

		MasterMap1,

   172

		MasterMap2,

   173

		MasterMap3, 

   174

		MasterMap4,

   175

		MasterMap5,

   176

		MasterMap6,

   177

		MasterMap7,

   178

		MasterMap8,

   179

		MasterMap9,

   180

		MasterMap10,

   181

		MasterMap11,

   182

		MasterMap12,

   183

		MasterMap13);

   184

}

   185

   186

void TestMaster_post_emcy(CO_Data* d, UNS8 nodeID, UNS16 errCode, UNS8 errReg)

   187

{

   188

	eprintf("Master received EMCY message. Node: %2.2x  ErrorCode: %4.4x  ErrorRegister: %2.2x\n", nodeID, errCode, errReg);

   189

}

   190

   191

char query_result = 0;

   192

char waiting_answer = 0;

   193

   194

static void CheckSDO(CO_Data* d, UNS8 nodeId)

   195

{

   196

	UNS32 abortCode;	

   197

	if(getWriteResultNetworkDict (d, nodeId, &abortCode) != SDO_FINISHED)

   198

		eprintf("Master : Failed in changing Slave's transmit type AbortCode :%4.4x \n", abortCode);

   199

   200

	/* Finalise last SDO transfer with this node */

   201

	closeSDOtransfer(&TestMaster_Data, nodeId, SDO_CLIENT);

   202

}

   203

   204

   205

static int MasterSyncCount = 0;

   206

void TestMaster_post_TPDO(CO_Data* d)

   207

{

   208

	eprintf("TestMaster_post_TPDO MasterSyncCount = %d \n", MasterSyncCount);

   209

//

   210

//	{

   211

//		char zero = 0;

   212

//		if(MasterMap4 > 0x80){

   213

//			writeNetworkDict (

   214

//				&TestMaster_Data,

   215

//				TestSlave_Data->bDeviceNodeId,

   216

//				0x2002,

   217

//				0x00,

   218

//				1,

   219

//				0,

   220

//				&zero); 

   221

//		}

   222

//	}

   223

   224

#if 0

   225

	if(waiting_answer){

   226

		UNS32 abortCode;			

   227

		UNS8 size;			

   228

		switch(getReadResultNetworkDict (

   229

			&TestMaster_Data, 

   230

			0x02,

   231

			&query_result,

   232

			&size,

   233

			&abortCode))

   234

		{

   235

			case SDO_FINISHED:

   236

				/* Do something with result here !!*/

   237

				eprintf("Got SDO answer (0x2002, 0x00), %d %d\n",query_result,size);

   238

			case SDO_ABORTED_RCV:

   239

			case SDO_ABORTED_INTERNAL:

   240

			case SDO_RESET:

   241

				waiting_answer = 0;

   242

				closeSDOtransfer(

   243

					&TestMaster_Data,

   244

					0x02,

   245

					SDO_CLIENT);

   246

			break;

   247

			case SDO_DOWNLOAD_IN_PROGRESS:

   248

			case SDO_UPLOAD_IN_PROGRESS:

   249

			break;

   250

		}

   251

	}else if(MasterSyncCount % 10 == 0){

   252

		readNetworkDict (

   253

			&TestMaster_Data,

   254

			0x02,

   255

			0x2002,

   256

			0x00,

   257

			0);

   258

		waiting_answer = 1;

   259

	}

   260

#endif	

   261

	if(MasterSyncCount % 17 == 0){

   262

		eprintf("Master : Ask RTR PDO (0x1402)\n");

   263

		sendPDOrequest(&TestMaster_Data, 0x1402 );

   264

		sendPDOrequest(&TestMaster_Data, 0x1403 );

   265

	}

   266

	if(MasterSyncCount % 50 == 0){

   267

		eprintf("Master : Change slave's transmit type to 0xFF\n");

   268

		UNS8 transmitiontype = 0xFF;

   269

		writeNetworkDictCallBack (&TestMaster_Data, /*CO_Data* d*/

   270

					2, /*UNS8 nodeId*/

   271

					0x1802, /*UNS16 index*/

   272

					0x02, /*UNS16 index*/

   273

					1, /*UNS8 count*/

   274

					0, /*UNS8 dataType*/

   275

					&transmitiontype,/*void *data*/

   276

					CheckSDO); /*SDOCallback_t Callback*/

   277

	}

   278

	if(MasterSyncCount % 50 == 25){

   279

		eprintf("Master : Change slave's transmit type to 0x00\n");

   280

		UNS8 transmitiontype = 0x00;

   281

		writeNetworkDictCallBack (&TestMaster_Data, /*CO_Data* d*/

   282

					2, /*UNS8 nodeId*/

   283

					0x1802, /*UNS16 index*/

   284

					0x02, /*UNS16 index*/

   285

					1, /*UNS8 count*/

   286

					0, /*UNS8 dataType*/

   287

					&transmitiontype,/*void *data*/

   288

					CheckSDO); /*SDOCallback_t Callback*/

   289

	}

   290

	MasterSyncCount++;

   291

}

   292

   293

void TestMaster_post_SlaveBootup(CO_Data* d, UNS8 nodeid)

   294

{

   295

	eprintf("TestMaster_post_SlaveBootup %x\n", nodeid);

   296

   297

	ConfigureSlaveNode(d, nodeid);

   298

}