examples/kerneltest/Master.c
author greg
Mon, 05 May 2008 11:25:14 +0200
changeset 455 26f762d4116d
parent 391 7802a7d5584f
child 496 06b489ebe751
permissions -rw-r--r--
Add support for USB-CAN devices from VScom
391
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
     1
/*
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
     2
This file is part of CanFestival, a library implementing CanOpen Stack. 
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
     3
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
     4
Copyright (C): Edouard TISSERANT and Francis DUPIN
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
     5
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
     6
See COPYING file for copyrights details.
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
     7
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
     8
This library is free software; you can redistribute it and/or
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
     9
modify it under the terms of the GNU Lesser General Public
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    10
License as published by the Free Software Foundation; either
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    11
version 2.1 of the License, or (at your option) any later version.
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    12
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    13
This library is distributed in the hope that it will be useful,
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    14
but WITHOUT ANY WARRANTY; without even the implied warranty of
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    15
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    16
Lesser General Public License for more details.
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    17
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    18
You should have received a copy of the GNU Lesser General Public
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    19
License along with this library; if not, write to the Free Software
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    20
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    21
*/
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    22
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    23
#include "Master.h"
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    24
#include "Slave.h"
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    25
#include "TestMasterSlave.h"
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    26
#include "can_driver.h"
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    27
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    28
extern s_BOARD MasterBoard;
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    29
/*****************************************************************************/
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    30
void TestMaster_heartbeatError(CO_Data* d, UNS8 heartbeatID)
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    31
{
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    32
	eprintf("TestMaster_heartbeatError %d\n", heartbeatID);
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    33
}
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    34
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    35
/********************************************************
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    36
 * ConfigureSlaveNode is responsible to
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    37
 *  - setup master RPDO 1 to receive TPDO 1 from id 2
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    38
 *  - setup master RPDO 2 to receive TPDO 2 from id 2
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    39
 ********************************************************/
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    40
void TestMaster_initialisation(CO_Data* d)
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    41
{
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    42
	UNS32 PDO1_COBID = 0x0182; 
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    43
	UNS32 PDO2_COBID = 0x0282;
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    44
	UNS8 size = sizeof(UNS32); 
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    45
//	UNS32 SINC_cicle=0;
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    46
//	UNS8 data_type = 0;
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    47
	
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    48
	eprintf("TestMaster_initialisation\n");
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    49
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    50
	/*****************************************
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    51
	 * Define RPDOs to match slave ID=2 TPDOs*
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    52
	 *****************************************/
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    53
	writeLocalDict( &TestMaster_Data, /*CO_Data* d*/
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    54
			0x1400, /*UNS16 index*/
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    55
			0x01, /*UNS8 subind*/ 
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    56
			&PDO1_COBID, /*void * pSourceData,*/ 
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    57
			&size, /* UNS8 * pExpectedSize*/
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    58
			RW);  /* UNS8 checkAccess */
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    59
			
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    60
	writeLocalDict( &TestMaster_Data, /*CO_Data* d*/
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    61
			0x1401, /*UNS16 index*/
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    62
			0x01, /*UNS8 subind*/ 
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    63
			&PDO2_COBID, /*void * pSourceData,*/ 
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    64
			&size, /* UNS8 * pExpectedSize*/
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    65
			RW);  /* UNS8 checkAccess */
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    66
					
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    67
}
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    68
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    69
// Step counts number of times ConfigureSlaveNode is called
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    70
static int init_step = 0;
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    71
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    72
/*Froward declaration*/
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    73
static void ConfigureSlaveNode(CO_Data* d, UNS8 nodeId);
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    74
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    75
/**/
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    76
static void CheckSDOAndContinue(CO_Data* d, UNS8 nodeId)
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    77
{
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    78
	UNS32 abortCode;	
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    79
	if(getWriteResultNetworkDict (d, nodeId, &abortCode) != SDO_FINISHED)
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    80
		eprintf("Master : Failed in initializing slave %2.2x, step %d, AbortCode :%4.4x \n", nodeId, init_step, abortCode);
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    81
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    82
	/* Finalise last SDO transfer with this node */
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    83
	closeSDOtransfer(&TestMaster_Data, nodeId, SDO_CLIENT);
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    84
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    85
	ConfigureSlaveNode(d, nodeId);
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    86
}
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    87
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    88
/********************************************************
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    89
 * ConfigureSlaveNode is responsible to
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    90
 *  - setup slave TPDO 1 transmit type
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    91
 *  - setup slave TPDO 2 transmit type
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    92
 *  - switch to operational mode
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    93
 *  - send NMT to slave
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    94
 ********************************************************
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    95
 * This an example of :
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    96
 * Network Dictionary Access (SDO) with Callback 
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    97
 * Slave node state change request (NMT) 
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    98
 ********************************************************
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
    99
 * This is called first by TestMaster_post_SlaveBootup
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   100
 * then it called again each time a SDO exchange is
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   101
 * finished.
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   102
 ********************************************************/
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   103
 
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   104
static void ConfigureSlaveNode(CO_Data* d, UNS8 nodeId)
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   105
{
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   106
	/* Master configure heartbeat producer time at 1000 ms 
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   107
	 * for slave node-id 0x02 by DCF concise */
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   108
	 
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   109
	UNS8 Transmission_Type = 0x01;
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   110
//	UNS32 abortCode;
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   111
	UNS8 res;
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   112
	eprintf("Master : ConfigureSlaveNode %2.2x\n", nodeId);
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   113
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   114
	switch(++init_step){
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   115
		case 1: /*First step : setup Slave's TPDO 1 to be transmitted on SYNC*/
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   116
			eprintf("Master : set slave %2.2x TPDO 1 transmit type\n", nodeId);
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   117
			res = writeNetworkDictCallBack (d, /*CO_Data* d*/
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   118
					nodeId, /*UNS8 nodeId*/
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   119
					0x1800, /*UNS16 index*/
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   120
					0x02, /*UNS8 subindex*/
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   121
					1, /*UNS8 count*/
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   122
					0, /*UNS8 dataType*/
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   123
					&Transmission_Type,/*void *data*/
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   124
					CheckSDOAndContinue); /*SDOCallback_t Callback*/
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   125
					break;
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   126
		
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   127
		case 2:	/*Second step*/
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   128
			eprintf("Master : set slave %2.2x TPDO 2 transmit type\n", nodeId);
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   129
			writeNetworkDictCallBack (d, /*CO_Data* d*/
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   130
					nodeId, /*UNS8 nodeId*/
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   131
					0x1801, /*UNS16 index*/
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   132
					0x02, /*UNS16 index*/
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   133
					1, /*UNS8 count*/
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   134
					0, /*UNS8 dataType*/
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   135
					&Transmission_Type,/*void *data*/
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   136
					CheckSDOAndContinue); /*SDOCallback_t Callback*/
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   137
					break;
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   138
		case 3: 
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   139
		
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   140
		/****************************** START *******************************/
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   141
		
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   142
			/* Put the master in operational mode */
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   143
			setState(d, Operational);
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   144
		 
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   145
			/* Ask slave node to go in operational mode */
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   146
			masterSendNMTstateChange (d, nodeId, NMT_Start_Node);
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   147
			
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   148
	}
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   149
}
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   150
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   151
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   152
void TestMaster_preOperational(CO_Data* d)
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   153
{
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   154
	eprintf("TestMaster_preOperational\n");
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   155
}
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   156
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   157
void TestMaster_operational(CO_Data* d)
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   158
{
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   159
	eprintf("TestMaster_operational\n");
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   160
}
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   161
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   162
void TestMaster_stopped(CO_Data* d)
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   163
{
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   164
	eprintf("TestMaster_stopped\n");
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   165
}
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   166
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   167
void TestMaster_post_sync(CO_Data* d)
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   168
{
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   169
	eprintf("TestMaster_post_sync\n");
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   170
	eprintf("Master: %d %d %d %d %d %d %d %d %d %x %x %d %d\n",
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   171
		MasterMap1,
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   172
		MasterMap2,
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   173
		MasterMap3, 
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   174
		MasterMap4,
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   175
		MasterMap5,
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   176
		MasterMap6,
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   177
		MasterMap7,
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   178
		MasterMap8,
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   179
		MasterMap9,
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   180
		MasterMap10,
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   181
		MasterMap11,
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   182
		MasterMap12,
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   183
		MasterMap13);
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   184
}
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   185
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   186
void TestMaster_post_emcy(CO_Data* d, UNS8 nodeID, UNS16 errCode, UNS8 errReg)
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   187
{
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   188
	eprintf("Master received EMCY message. Node: %2.2x  ErrorCode: %4.4x  ErrorRegister: %2.2x\n", nodeID, errCode, errReg);
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   189
}
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   190
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   191
char query_result = 0;
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   192
char waiting_answer = 0;
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   193
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   194
static void CheckSDO(CO_Data* d, UNS8 nodeId)
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   195
{
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   196
	UNS32 abortCode;	
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   197
	if(getWriteResultNetworkDict (d, nodeId, &abortCode) != SDO_FINISHED)
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   198
		eprintf("Master : Failed in changing Slave's transmit type AbortCode :%4.4x \n", abortCode);
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   199
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   200
	/* Finalise last SDO transfer with this node */
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   201
	closeSDOtransfer(&TestMaster_Data, nodeId, SDO_CLIENT);
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   202
}
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   203
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   204
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   205
static int MasterSyncCount = 0;
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   206
void TestMaster_post_TPDO(CO_Data* d)
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   207
{
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   208
	eprintf("TestMaster_post_TPDO MasterSyncCount = %d \n", MasterSyncCount);
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   209
//
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   210
//	{
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   211
//		char zero = 0;
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   212
//		if(MasterMap4 > 0x80){
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   213
//			writeNetworkDict (
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   214
//				&TestMaster_Data,
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   215
//				TestSlave_Data->bDeviceNodeId,
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   216
//				0x2002,
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   217
//				0x00,
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   218
//				1,
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   219
//				0,
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   220
//				&zero); 
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   221
//		}
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   222
//	}
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   223
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   224
#if 0
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   225
	if(waiting_answer){
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   226
		UNS32 abortCode;			
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   227
		UNS8 size;			
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   228
		switch(getReadResultNetworkDict (
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   229
			&TestMaster_Data, 
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   230
			0x02,
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   231
			&query_result,
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   232
			&size,
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   233
			&abortCode))
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   234
		{
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   235
			case SDO_FINISHED:
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   236
				/* Do something with result here !!*/
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   237
				eprintf("Got SDO answer (0x2002, 0x00), %d %d\n",query_result,size);
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   238
			case SDO_ABORTED_RCV:
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   239
			case SDO_ABORTED_INTERNAL:
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   240
			case SDO_RESET:
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   241
				waiting_answer = 0;
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   242
				closeSDOtransfer(
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   243
					&TestMaster_Data,
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   244
					0x02,
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   245
					SDO_CLIENT);
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   246
			break;
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   247
			case SDO_DOWNLOAD_IN_PROGRESS:
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   248
			case SDO_UPLOAD_IN_PROGRESS:
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   249
			break;
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   250
		}
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   251
	}else if(MasterSyncCount % 10 == 0){
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   252
		readNetworkDict (
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   253
			&TestMaster_Data,
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   254
			0x02,
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   255
			0x2002,
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   256
			0x00,
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   257
			0);
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   258
		waiting_answer = 1;
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   259
	}
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   260
#endif	
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   261
	if(MasterSyncCount % 17 == 0){
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   262
		eprintf("Master : Ask RTR PDO (0x1402)\n");
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   263
		sendPDOrequest(&TestMaster_Data, 0x1402 );
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   264
		sendPDOrequest(&TestMaster_Data, 0x1403 );
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   265
	}
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   266
	if(MasterSyncCount % 50 == 0){
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   267
		eprintf("Master : Change slave's transmit type to 0xFF\n");
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   268
		UNS8 transmitiontype = 0xFF;
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   269
		writeNetworkDictCallBack (&TestMaster_Data, /*CO_Data* d*/
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   270
					2, /*UNS8 nodeId*/
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   271
					0x1802, /*UNS16 index*/
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   272
					0x02, /*UNS16 index*/
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   273
					1, /*UNS8 count*/
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   274
					0, /*UNS8 dataType*/
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   275
					&transmitiontype,/*void *data*/
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   276
					CheckSDO); /*SDOCallback_t Callback*/
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   277
	}
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   278
	if(MasterSyncCount % 50 == 25){
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   279
		eprintf("Master : Change slave's transmit type to 0x00\n");
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   280
		UNS8 transmitiontype = 0x00;
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   281
		writeNetworkDictCallBack (&TestMaster_Data, /*CO_Data* d*/
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   282
					2, /*UNS8 nodeId*/
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   283
					0x1802, /*UNS16 index*/
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   284
					0x02, /*UNS16 index*/
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   285
					1, /*UNS8 count*/
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   286
					0, /*UNS8 dataType*/
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   287
					&transmitiontype,/*void *data*/
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   288
					CheckSDO); /*SDOCallback_t Callback*/
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   289
	}
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   290
	MasterSyncCount++;
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   291
}
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   292
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   293
void TestMaster_post_SlaveBootup(CO_Data* d, UNS8 nodeid)
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   294
{
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   295
	eprintf("TestMaster_post_SlaveBootup %x\n", nodeid);
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   296
	
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   297
	ConfigureSlaveNode(d, nodeid);
7802a7d5584f Accepted Vladimir Chren linux kernelspace port patch.
etisserant
parents:
diff changeset
   298
}