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

    27

extern s_BOARD MasterBoard;

    28

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

    29

void TestMaster_heartbeatError(CO_Data* d, UNS8 heartbeatID)

    30

{

    31

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

    32

}

    33

    34

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

    35

 * ConfigureSlaveNode is responsible to

    36

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

    37

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

    38

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

    39

void TestMaster_initialisation(CO_Data* d)

    40

{

    41

	UNS32 PDO1_COBID = 0x0182; 

    42

	UNS32 PDO2_COBID = 0x0282;

    43

	UNS32 size = sizeof(UNS32); 

    44

	UNS32 SINC_cicle=0;

    45

	UNS8 data_type = 0;

    46

    47

	eprintf("TestMaster_initialisation\n");

    48

    49

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

    50

	 * Define RPDOs to match slave ID=2 TPDOs*

    51

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

    52

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

    53

			0x1400, /*UNS16 index*/

    54

			0x01, /*UNS8 subind*/ 

    55

			&PDO1_COBID, /*void * pSourceData,*/ 

    56

			&size, /* UNS8 * pExpectedSize*/

    57

			RW);  /* UNS8 checkAccess */

    58

    59

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

    60

			0x1401, /*UNS16 index*/

    61

			0x01, /*UNS8 subind*/ 

    62

			&PDO2_COBID, /*void * pSourceData,*/ 

    63

			&size, /* UNS8 * pExpectedSize*/

    64

			RW);  /* UNS8 checkAccess */

    65

    66

}

    67

    68

// Step counts number of times ConfigureSlaveNode is called

    69

static int init_step = 0;

    70

    71

/*Froward declaration*/

    72

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

    73

    74

/**/

    75

static void CheckSDOAndContinue(CO_Data* d, UNS8 nodeId)

    76

{

    77

	UNS32 abortCode;	

    78

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

    79

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

    80

    81

	/* Finalise last SDO transfer with this node */

    82

	closeSDOtransfer(&TestMaster_Data, nodeId, SDO_CLIENT);

    83

    84

	ConfigureSlaveNode(d, nodeId);

    85

}

    86

    87

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

    88

 * ConfigureSlaveNode is responsible to

    89

 *  - setup slave TPDO 1 transmit type

    90

 *  - setup slave TPDO 2 transmit type

    91

 *  - switch to operational mode

    92

 *  - send NMT to slave

    93

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

    94

 * This an example of :

    95

 * Network Dictionary Access (SDO) with Callback 

    96

 * Slave node state change request (NMT) 

    97

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

    98

 * This is called first by TestMaster_post_SlaveBootup

    99

 * then it called again each time a SDO exchange is

   100

 * finished.

   101

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

   102

   103

static void ConfigureSlaveNode(CO_Data* d, UNS8 nodeId)

   104

{

   105

	/* Master configure heartbeat producer time at 1000 ms 

   106

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

   107

   108

	UNS8 Transmission_Type = 0x01;

   109

	UNS8 res;

   110

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

   111

   112

	switch(++init_step){

   113

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

   114

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

   115

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

   116

					nodeId, /*UNS8 nodeId*/

   117

					0x1800, /*UNS16 index*/

   118

					0x02, /*UNS8 subindex*/

   119

					1, /*UNS8 count*/

   120

					0, /*UNS8 dataType*/

   121

					&Transmission_Type,/*void *data*/

   122

					CheckSDOAndContinue); /*SDOCallback_t Callback*/

   123

					break;

   124

   125

		case 2:	/*Second step*/

   126

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

   127

			writeNetworkDictCallBack (d, /*CO_Data* d*/

   128

					nodeId, /*UNS8 nodeId*/

   129

					0x1801, /*UNS16 index*/

   130

					0x02, /*UNS16 index*/

   131

					1, /*UNS8 count*/

   132

					0, /*UNS8 dataType*/

   133

					&Transmission_Type,/*void *data*/

   134

					CheckSDOAndContinue); /*SDOCallback_t Callback*/

   135

					break;

   136

		case 3: 

   137

   138

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

   139

   140

			/* Put the master in operational mode */

   141

			setState(d, Operational);

   142

   143

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

   144

			masterSendNMTstateChange (d, nodeId, NMT_Start_Node);

   145

   146

	}

   147

}

   148

   149

   150

void TestMaster_preOperational(CO_Data* d)

   151

{

   152

	eprintf("TestMaster_preOperational\n");

   153

}

   154

   155

void TestMaster_operational(CO_Data* d)

   156

{

   157

	eprintf("TestMaster_operational\n");

   158

}

   159

   160

void TestMaster_stopped(CO_Data* d)

   161

{

   162

	eprintf("TestMaster_stopped\n");

   163

}

   164

   165

void TestMaster_post_sync(CO_Data* d)

   166

{

   167

	eprintf("TestMaster_post_sync\n");

   168

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

   169

		MasterMap1,

   170

		MasterMap2,

   171

		MasterMap3, 

   172

		MasterMap4,

   173

		MasterMap5,

   174

		MasterMap6,

   175

		MasterMap7,

   176

		MasterMap8,

   177

		MasterMap9,

   178

		MasterMap10,

   179

		MasterMap11,

   180

		MasterMap12,

   181

		MasterMap13);

   182

}

   183

   184

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

   185

{

   186

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

   187

}

   188

   189

char query_result = 0;

   190

char waiting_answer = 0;

   191

   192

static void CheckSDO(CO_Data* d, UNS8 nodeId)

   193

{

   194

	UNS32 abortCode;	

   195

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

   196

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

   197

   198

	/* Finalise last SDO transfer with this node */

   199

	closeSDOtransfer(&TestMaster_Data, nodeId, SDO_CLIENT);

   200

}

   201

   202

   203

static int MasterSyncCount = 0;

   204

void TestMaster_post_TPDO(CO_Data* d)

   205

{

   206

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

   207

	if(MasterSyncCount % 17 == 0){

   208

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

   209

		sendPDOrequest(&TestMaster_Data, 0x1402 );

   210

		sendPDOrequest(&TestMaster_Data, 0x1403 );

   211

	}

   212

	if(MasterSyncCount % 50 == 0){

   213

		UNS8 transmitiontype;

   214

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

   215

		transmitiontype = 0xFF;

   216

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

   217

					2, /*UNS8 nodeId*/

   218

					0x1802, /*UNS16 index*/

   219

					0x02, /*UNS16 index*/

   220

					1, /*UNS8 count*/

   221

					0, /*UNS8 dataType*/

   222

					&transmitiontype,/*void *data*/

   223

					CheckSDO); /*SDOCallback_t Callback*/

   224

	}

   225

	if(MasterSyncCount % 50 == 25){

   226

		UNS8 transmitiontype = 0x00;

   227

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

   228

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

   229

					2, /*UNS8 nodeId*/

   230

					0x1802, /*UNS16 index*/

   231

					0x02, /*UNS16 index*/

   232

					1, /*UNS8 count*/

   233

					0, /*UNS8 dataType*/

   234

					&transmitiontype,/*void *data*/

   235

					CheckSDO); /*SDOCallback_t Callback*/

   236

	}

   237

	MasterSyncCount++;

   238

}

   239

   240

void TestMaster_post_SlaveBootup(CO_Data* d, UNS8 nodeid)

   241

{

   242

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

   243

   244

	ConfigureSlaveNode(d, nodeid);

   245

}