2 This file is part of CanFestival, a library implementing CanOpen Stack.
4 Copyright (C): Edouard TISSERANT and Francis DUPIN
6 See COPYING file for copyrights details.
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.
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.
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
25 #include "TestMasterSlave.h"
27 extern s_BOARD MasterBoard;
28 /*****************************************************************************/
29 void TestMaster_heartbeatError(CO_Data* d, UNS8 heartbeatID)
31 eprintf("TestMaster_heartbeatError %d\n", heartbeatID);
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)
41 UNS32 PDO1_COBID = 0x0182;
42 UNS32 PDO2_COBID = 0x0282;
43 UNS8 size = sizeof(UNS32);
47 eprintf("TestMaster_initialisation\n");
49 /*****************************************
50 * Define RPDOs to match slave ID=2 TPDOs*
51 *****************************************/
52 writeLocalDict( &TestMaster_Data, /*CO_Data* d*/
53 0x1400, /*UNS16 index*/
55 &PDO1_COBID, /*void * pSourceData,*/
56 &size, /* UNS8 * pExpectedSize*/
57 RW); /* UNS8 checkAccess */
59 writeLocalDict( &TestMaster_Data, /*CO_Data* d*/
60 0x1401, /*UNS16 index*/
62 &PDO2_COBID, /*void * pSourceData,*/
63 &size, /* UNS8 * pExpectedSize*/
64 RW); /* UNS8 checkAccess */
68 // Step counts number of times ConfigureSlaveNode is called
69 static int init_step = 0;
71 /*Froward declaration*/
72 static void ConfigureSlaveNode(CO_Data* d, UNS8 nodeId);
75 static void CheckSDOAndContinue(CO_Data* d, UNS8 nodeId)
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);
81 /* Finalise last SDO transfer with this node */
82 closeSDOtransfer(&TestMaster_Data, nodeId, SDO_CLIENT);
84 ConfigureSlaveNode(d, nodeId);
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
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
101 ********************************************************/
103 static void ConfigureSlaveNode(CO_Data* d, UNS8 nodeId)
105 /* Master configure heartbeat producer time at 1000 ms
106 * for slave node-id 0x02 by DCF concise */
108 UNS8 Transmission_Type = 0x01;
111 eprintf("Master : ConfigureSlaveNode %2.2x\n", nodeId);
114 case 1: /*First step : setup Slave's TPDO 1 to be transmitted on SYNC*/
115 eprintf("Master : set slave %2.2x TPDO 1 transmit type\n", nodeId);
116 res = writeNetworkDictCallBack (d, /*CO_Data* d*/
117 nodeId, /*UNS8 nodeId*/
118 0x1800, /*UNS16 index*/
119 0x02, /*UNS8 subindex*/
122 &Transmission_Type,/*void *data*/
123 CheckSDOAndContinue); /*SDOCallback_t Callback*/
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*/
134 &Transmission_Type,/*void *data*/
135 CheckSDOAndContinue); /*SDOCallback_t Callback*/
139 /****************************** START *******************************/
141 /* Put the master in operational mode */
142 setState(d, Operational);
144 /* Ask slave node to go in operational mode */
145 masterSendNMTstateChange (d, nodeId, NMT_Start_Node);
151 void TestMaster_preOperational(CO_Data* d)
153 eprintf("TestMaster_preOperational\n");
156 void TestMaster_operational(CO_Data* d)
158 eprintf("TestMaster_operational\n");
161 void TestMaster_stopped(CO_Data* d)
163 eprintf("TestMaster_stopped\n");
166 void TestMaster_post_sync(CO_Data* d)
168 eprintf("TestMaster_post_sync\n");
169 eprintf("Master: %d %d %d %d %d %d %d %d %d %x %x %d %d\n",
185 void TestMaster_post_emcy(CO_Data* d, UNS8 nodeID, UNS16 errCode, UNS8 errReg)
187 eprintf("Master received EMCY message. Node: %2.2x ErrorCode: %4.4x ErrorRegister: %2.2x\n", nodeID, errCode, errReg);
190 char query_result = 0;
191 char waiting_answer = 0;
193 static void CheckSDO(CO_Data* d, UNS8 nodeId)
196 if(getWriteResultNetworkDict (d, nodeId, &abortCode) != SDO_FINISHED)
197 eprintf("Master : Failed in changing Slave's transmit type AbortCode :%4.4x \n", abortCode);
199 /* Finalise last SDO transfer with this node */
200 closeSDOtransfer(&TestMaster_Data, nodeId, SDO_CLIENT);
204 static int MasterSyncCount = 0;
205 void TestMaster_post_TPDO(CO_Data* d)
207 eprintf("TestMaster_post_TPDO MasterSyncCount = %d \n", MasterSyncCount);
211 // if(MasterMap4 > 0x80){
212 // writeNetworkDict (
214 // TestSlave_Data->bDeviceNodeId,
227 switch(getReadResultNetworkDict (
235 /* Do something with result here !!*/
236 eprintf("Got SDO answer (0x2002, 0x00), %d %d\n",query_result,size);
237 case SDO_ABORTED_RCV:
238 case SDO_ABORTED_INTERNAL:
246 case SDO_DOWNLOAD_IN_PROGRESS:
247 case SDO_UPLOAD_IN_PROGRESS:
250 }else if(MasterSyncCount % 10 == 0){
260 if(MasterSyncCount % 17 == 0){
261 eprintf("Master : Ask RTR PDO (0x1402)\n");
262 sendPDOrequest(&TestMaster_Data, 0x1402 );
263 sendPDOrequest(&TestMaster_Data, 0x1403 );
265 if(MasterSyncCount % 50 == 0){
266 eprintf("Master : Change slave's transmit type to 0xFF\n");
267 UNS8 transmitiontype = 0xFF;
268 writeNetworkDictCallBack (&TestMaster_Data, /*CO_Data* d*/
270 0x1802, /*UNS16 index*/
271 0x02, /*UNS16 index*/
274 &transmitiontype,/*void *data*/
275 CheckSDO); /*SDOCallback_t Callback*/
277 if(MasterSyncCount % 50 == 25){
278 eprintf("Master : Change slave's transmit type to 0x00\n");
279 UNS8 transmitiontype = 0x00;
280 writeNetworkDictCallBack (&TestMaster_Data, /*CO_Data* d*/
282 0x1802, /*UNS16 index*/
283 0x02, /*UNS16 index*/
286 &transmitiontype,/*void *data*/
287 CheckSDO); /*SDOCallback_t Callback*/
292 void TestMaster_post_SlaveBootup(CO_Data* d, UNS8 nodeid)
294 eprintf("TestMaster_post_SlaveBootup %x\n", nodeid);
296 ConfigureSlaveNode(d, nodeid);