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

                    0); /* use block mode */

   124

					break;

   125

   126

		case 2:	/*Second step*/

   127

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

   128

			writeNetworkDictCallBack (d, /*CO_Data* d*/

   129

					nodeId, /*UNS8 nodeId*/

   130

					0x1801, /*UNS16 index*/

   131

					0x02, /*UNS16 index*/

   132

					1, /*UNS8 count*/

   133

					0, /*UNS8 dataType*/

   134

					&Transmission_Type,/*void *data*/

   135

					CheckSDOAndContinue, /*SDOCallback_t Callback*/

   136

                    0); /* use block mode */

   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

	if(MasterSyncCount % 17 == 0){

   210

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

   211

		sendPDOrequest(&TestMaster_Data, 0x1402 );

   212

		sendPDOrequest(&TestMaster_Data, 0x1403 );

   213

	}

   214

	if(MasterSyncCount % 50 == 0){

   215

		UNS8 transmitiontype;

   216

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

   217

		transmitiontype = 0xFF;

   218

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

   219

					2, /*UNS8 nodeId*/

   220

					0x1802, /*UNS16 index*/

   221

					0x02, /*UNS16 index*/

   222

					1, /*UNS8 count*/

   223

					0, /*UNS8 dataType*/

   224

					&transmitiontype,/*void *data*/

   225

					CheckSDO, /*SDOCallback_t Callback*/

   226

                    0); /* use block mode */

   227

	}   

   228

	if(MasterSyncCount % 50 == 25){

   229

		UNS8 transmitiontype = 0x00;

   230

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

   231

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

   232

					2, /*UNS8 nodeId*/

   233

					0x1802, /*UNS16 index*/

   234

					0x02, /*UNS16 index*/

   235

					1, /*UNS8 count*/

   236

					0, /*UNS8 dataType*/

   237

					&transmitiontype,/*void *data*/

   238

					CheckSDO, /*SDOCallback_t Callback*/

   239

                    0); /* use block mode */

   240

	}

   241

	MasterSyncCount++;

   242

}

   243

   244

void TestMaster_post_SlaveBootup(CO_Data* d, UNS8 nodeid)

   245

{

   246

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

   247

   248

	ConfigureSlaveNode(d, nodeid);

   249

}