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

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

    28

void TestMaster_heartbeatError(UNS8 heartbeatID)

    29

{

    30

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

    31

}

    32

    33

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

    34

void TestMaster_SDOtimeoutError (UNS8 line)

    35

{

    36

	eprintf("TestMaster_SDOtimeoutError %d\n", line);

    37

}

    38

    39

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

    40

 * ConfigureSlaveNode is responsible to

    41

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

    42

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

    43

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

    44

void TestMaster_initialisation()

    45

{

    46

	UNS32 PDO1_COBID = 0x0182; 

    47

	UNS32 PDO2_COBID = 0x0282;

    48

	UNS8 size = sizeof(UNS32); 

    49

    50

	eprintf("TestMaster_initialisation\n");

    51

    52

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

    53

	 * Define RPDOs to match slave ID=2 TPDOs*

    54

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

    55

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

    56

			0x1400, /*UNS16 index*/

    57

			0x01, /*UNS8 subind*/ 

    58

			&PDO1_COBID, /*void * pSourceData,*/ 

    59

			&size, /* UNS8 * pExpectedSize*/

    60

			RW);  /* UNS8 checkAccess */

    61

    62

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

    63

			0x1401, /*UNS16 index*/

    64

			0x01, /*UNS8 subind*/ 

    65

			&PDO2_COBID, /*void * pSourceData,*/ 

    66

			&size, /* UNS8 * pExpectedSize*/

    67

			RW);  /* UNS8 checkAccess */

    68

}

    69

    70

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

    71

 * ConfigureSlaveNode is responsible to

    72

 *  - setup slave TPDO 1 transmit time

    73

 *  - setup slave TPDO 2 transmit time

    74

 *  - switch to operational mode

    75

 *  - send NMT to slave

    76

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

    77

 * This an example of :

    78

 * Network Dictionary Access (SDO) with Callback 

    79

 * Slave node state change request (NMT) 

    80

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

    81

 * This is called first by TestMaster_preOperational

    82

 * then it called again each time a SDO exchange is

    83

 * finished.

    84

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

    85

static void ConfigureSlaveNode(CO_Data* d, UNS8 nodeId)

    86

{

    87

	// Step counts number of times ConfigureSlaveNode is called

    88

	static step = 1;

    89

    90

	UNS8 Transmission_Type = 0x01; 

    91

	UNS32 abortCode;

    92

	UNS8 res;

    93

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

    94

    95

	switch(step++){

    96

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

    97

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

    98

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

    99

					*TestSlave_Data.bDeviceNodeId, /*UNS8 nodeId*/

   100

					0x1800, /*UNS16 index*/

   101

					0x02, /*UNS8 subindex*/

   102

					1, /*UNS8 count*/

   103

					0, /*UNS8 dataType*/

   104

					&Transmission_Type,/*void *data*/

   105

					ConfigureSlaveNode); /*SDOCallback_t Callback*/			break;

   106

		case 2:	/*Second step*/

   107

			/*check and warn for previous slave OD access error*/

   108

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

   109

				eprintf("Master : Couldn't set slave %2.2x TPDO 1 transmit type. AbortCode :%4.4x \n", nodeId, abortCode);

   110

   111

			/* Finalise last SDO transfer with this node */

   112

			closeSDOtransfer(&TestMaster_Data,

   113

					*TestSlave_Data.bDeviceNodeId,

   114

					SDO_CLIENT);

   115

   116

			/*Setup Slave's TPDO 1 to be transmitted on SYNC*/

   117

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

   118

			writeNetworkDictCallBack (d, /*CO_Data* d*/

   119

					*TestSlave_Data.bDeviceNodeId, /*UNS8 nodeId*/

   120

					0x1801, /*UNS16 index*/

   121

					0x02, /*UNS16 index*/

   122

					1, /*UNS8 count*/

   123

					0, /*UNS8 dataType*/

   124

					&Transmission_Type,/*void *data*/

   125

					ConfigureSlaveNode); /*SDOCallback_t Callback*/

   126

			break;

   127

		case 3: /*Last step*/

   128

			/*check and warn for previous slave OD access error*/

   129

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

   130

				eprintf("Master : Couldn't set slave %2.2x TPDO 2 transmit type. AbortCode :%4.4x \n", nodeId, abortCode);

   131

   132

			/* Finalise last SDO transfer with this node */

   133

			closeSDOtransfer(&TestMaster_Data,

   134

					*TestSlave_Data.bDeviceNodeId,

   135

					SDO_CLIENT);

   136

   137

			/* Put the master in operational mode */

   138

			setState(d, Operational);

   139

   140

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

   141

			masterSendNMTstateChange (d, nodeId, NMT_Start_Node);

   142

	}

   143

   144

}

   145

   146

void TestMaster_preOperational()

   147

{

   148

   149

	eprintf("TestMaster_preOperational\n");

   150

	ConfigureSlaveNode(&TestMaster_Data, 2);

   151

   152

}

   153

   154

void TestMaster_operational()

   155

{

   156

	eprintf("TestMaster_operational\n");

   157

}

   158

   159

void TestMaster_stopped()

   160

{

   161

	eprintf("TestMaster_stopped\n");

   162

}

   163

   164

void TestMaster_post_sync()

   165

{

   166

	eprintf("TestMaster_post_sync\n");

   167

	eprintf("Master: %d %d %d %d\n",MasterMap1, MasterMap2, MasterMap3, MasterMap4);

   168

}

   169

   170

char query_result = 0;

   171

char waiting_answer = 0;

   172

   173

void TestMaster_post_TPDO()

   174

{

   175

	eprintf("TestMaster_post_TPDO\n");

   176

   177

//	{

   178

//		char zero = 0;

   179

//		if(MasterMap4 > 0x80){

   180

//			writeNetworkDict (

   181

//				&TestMaster_Data,

   182

//				TestSlave_Data->bDeviceNodeId,

   183

//				0x2002,

   184

//				0x00,

   185

//				1,

   186

//				0,

   187

//				&zero); 

   188

//		}

   189

//	}

   190

   191

	if(waiting_answer){

   192

		UNS32 abortCode;			

   193

		UNS8 size;			

   194

		switch(getReadResultNetworkDict (

   195

			&TestMaster_Data, 

   196

			*TestSlave_Data.bDeviceNodeId,

   197

			&query_result,

   198

			&size,

   199

			&abortCode))

   200

		{

   201

			case SDO_FINISHED:

   202

				/* Do something with result here !!*/

   203

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

   204

			case SDO_ABORTED_RCV:

   205

			case SDO_ABORTED_INTERNAL:

   206

			case SDO_RESET:

   207

				waiting_answer = 0;

   208

				closeSDOtransfer(

   209

					&TestMaster_Data,

   210

					*TestSlave_Data.	bDeviceNodeId,

   211

					SDO_CLIENT);

   212

			break;

   213

			case SDO_DOWNLOAD_IN_PROGRESS:

   214

			case SDO_UPLOAD_IN_PROGRESS:

   215

			break;

   216

		}

   217

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

   218

		readNetworkDict (

   219

			&TestMaster_Data,

   220

			*TestSlave_Data.bDeviceNodeId,

   221

			0x2002,

   222

			0x00,

   223

			0);

   224

		waiting_answer = 1;

   225

	}

   226

   227

}