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

 * ConfigureSlaveNode is responsible to

    35

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

    36

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

    37

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

    38

void TestMaster_initialisation()

    39

{

    40

	UNS32 PDO1_COBID = 0x0182; 

    41

	UNS32 PDO2_COBID = 0x0282;

    42

	UNS8 size = sizeof(UNS32); 

    43

    44

	eprintf("TestMaster_initialisation\n");

    45

    46

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

    47

	 * Define RPDOs to match slave ID=2 TPDOs*

    48

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

    50

			0x1400, /*UNS16 index*/

    51

			0x01, /*UNS8 subind*/ 

    52

			&PDO1_COBID, /*void * pSourceData,*/ 

    53

			&size, /* UNS8 * pExpectedSize*/

    54

			RW);  /* UNS8 checkAccess */

    55

    57

			0x1401, /*UNS16 index*/

    58

			0x01, /*UNS8 subind*/ 

    59

			&PDO2_COBID, /*void * pSourceData,*/ 

    60

			&size, /* UNS8 * pExpectedSize*/

    61

			RW);  /* UNS8 checkAccess */

    62

}

    63

    64

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

    65

 * ConfigureSlaveNode is responsible to

    66

 *  - setup slave TPDO 1 transmit time

    67

 *  - setup slave TPDO 2 transmit time

    68

 *  - switch to operational mode

    69

 *  - send NMT to slave

    70

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

    71

 * This an example of :

    72

 * Network Dictionary Access (SDO) with Callback 

    73

 * Slave node state change request (NMT) 

    74

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

    75

 * This is called first by TestMaster_preOperational

    76

 * then it called again each time a SDO exchange is

    77

 * finished.

    78

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

    79

static void ConfigureSlaveNode(CO_Data* d, UNS8 nodeId)

    80

{

    81

	// Step counts number of times ConfigureSlaveNode is called

    82

	static step = 1;

    83

    84

	UNS8 Transmission_Type = 0x01; 

    85

	UNS32 abortCode;

    86

	UNS8 res;

    87

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

    88

    89

	switch(step++){

    90

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

    91

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

    92

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

    93

					*TestSlave_Data.bDeviceNodeId, /*UNS8 nodeId*/

    94

					0x1800, /*UNS16 index*/

    95

					0x02, /*UNS8 subindex*/

    96

					1, /*UNS8 count*/

    97

					0, /*UNS8 dataType*/

    98

					&Transmission_Type,/*void *data*/

    99

					ConfigureSlaveNode); /*SDOCallback_t Callback*/			break;

   100

		case 2:	/*Second step*/

   101

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

   102

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

   103

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

   104

   105

			/* Finalise last SDO transfer with this node */

   106

			closeSDOtransfer(&TestMaster_Data,

   107

					*TestSlave_Data.bDeviceNodeId,

   108

					SDO_CLIENT);

   109

   110

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

   111

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

   112

			writeNetworkDictCallBack (d, /*CO_Data* d*/

   113

					*TestSlave_Data.bDeviceNodeId, /*UNS8 nodeId*/

   114

					0x1801, /*UNS16 index*/

   115

					0x02, /*UNS16 index*/

   116

					1, /*UNS8 count*/

   117

					0, /*UNS8 dataType*/

   118

					&Transmission_Type,/*void *data*/

   119

					ConfigureSlaveNode); /*SDOCallback_t Callback*/

   120

			break;

   121

		case 3: /*Last step*/

   122

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

   123

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

   124

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

   125

   126

			/* Finalise last SDO transfer with this node */

   127

			closeSDOtransfer(&TestMaster_Data,

   128

					*TestSlave_Data.bDeviceNodeId,

   129

					SDO_CLIENT);

   130

   131

			/* Put the master in operational mode */

   132

			setState(d, Operational);

   133

   134

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

   135

			masterSendNMTstateChange (d, nodeId, NMT_Start_Node);

   136

	}

   137

   138

}

   139

   140

void TestMaster_preOperational()

   141

{

   142

   143

	eprintf("TestMaster_preOperational\n");

   144

	ConfigureSlaveNode(&TestMaster_Data, 2);

   145

   146

}

   147

   148

void TestMaster_operational()

   149

{

   150

	eprintf("TestMaster_operational\n");

   151

}

   152

   153

void TestMaster_stopped()

   154

{

   155

	eprintf("TestMaster_stopped\n");

   156

}

   157

   158

void TestMaster_post_sync()

   159

{

   160

	eprintf("TestMaster_post_sync\n");

   161

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

   162

}

   163

   164

char query_result = 0;

   165

char waiting_answer = 0;

   166

   167

void TestMaster_post_TPDO()

   168

{

   169

	eprintf("TestMaster_post_TPDO\n");

   170

   171

//	{

   172

//		char zero = 0;

   173

//		if(MasterMap4 > 0x80){

   174

//			writeNetworkDict (

   175

//				&TestMaster_Data,

   176

//				TestSlave_Data->bDeviceNodeId,

   177

//				0x2002,

   178

//				0x00,

   179

//				1,

   180

//				0,

   181

//				&zero); 

   182

//		}

   183

//	}

   184

   185

	if(waiting_answer){

   186

		UNS32 abortCode;			

   187

		UNS8 size;			

   188

		switch(getReadResultNetworkDict (

   189

			&TestMaster_Data, 

   190

			*TestSlave_Data.bDeviceNodeId,

   191

			&query_result,

   192

			&size,

   193

			&abortCode))

   194

		{

   195

			case SDO_FINISHED:

   196

				/* Do something with result here !!*/

   197

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

   198

			case SDO_ABORTED_RCV:

   199

			case SDO_ABORTED_INTERNAL:

   200

			case SDO_RESET:

   201

				waiting_answer = 0;

   202

				closeSDOtransfer(

   203

					&TestMaster_Data,

   204

					*TestSlave_Data.	bDeviceNodeId,

   205

					SDO_CLIENT);

   206

			break;

   207

			case SDO_DOWNLOAD_IN_PROGRESS:

   208

			case SDO_UPLOAD_IN_PROGRESS:

   209

			break;

   210

		}

   211

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

   212

		readNetworkDict (

   213

			&TestMaster_Data,

   214

			*TestSlave_Data.bDeviceNodeId,

   215

			0x2002,

   216

			0x00,

   217

			0);

   218

		waiting_answer = 1;

   219

	}

   220

   221

}