00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023 #include "Master.h"
00024 #include "Slave.h"
00025 #include "TestMasterSlave.h"
00026
00027
00028 void TestMaster_heartbeatError(UNS8 heartbeatID)
00029 {
00030 eprintf("TestMaster_heartbeatError %d\n", heartbeatID);
00031 }
00032
00033
00034
00035
00036
00037
00038 void TestMaster_initialisation()
00039 {
00040 UNS32 PDO1_COBID = 0x0182;
00041 UNS32 PDO2_COBID = 0x0282;
00042 UNS8 size = sizeof(UNS32);
00043
00044 eprintf("TestMaster_initialisation\n");
00045
00046
00047
00048
00049 writeLocalDict( &TestMaster_Data,
00050 0x1400,
00051 0x01,
00052 &PDO1_COBID,
00053 &size,
00054 RW);
00055
00056 writeLocalDict( &TestMaster_Data,
00057 0x1401,
00058 0x01,
00059 &PDO2_COBID,
00060 &size,
00061 RW);
00062 }
00063
00064
00065 static init_step = 0;
00066
00067
00068 static void ConfigureSlaveNode(CO_Data* d, UNS8 nodeId);
00069
00070
00071 static void CheckSDOAndContinue(CO_Data* d, UNS8 nodeId)
00072 {
00073 UNS32 abortCode;
00074 if(getWriteResultNetworkDict (d, nodeId, &abortCode) != SDO_FINISHED)
00075 eprintf("Master : Failed in initializing slave %2.2x, step %d, AbortCode :%4.4x \n", nodeId, init_step, abortCode);
00076
00077
00078 closeSDOtransfer(&TestMaster_Data, nodeId, SDO_CLIENT);
00079
00080 ConfigureSlaveNode(d, nodeId);
00081 }
00082
00083
00084
00085
00086
00087
00088
00089
00090
00091
00092
00093
00094
00095
00096
00097
00098
00099 static void ConfigureSlaveNode(CO_Data* d, UNS8 nodeId)
00100 {
00101
00102
00103
00104 UNS8 Transmission_Type = 0x01;
00105 UNS32 abortCode;
00106 UNS8 res;
00107 eprintf("Master : ConfigureSlaveNode %2.2x\n", nodeId);
00108
00109 switch(++init_step){
00110 case 1:
00111 eprintf("Master : set slave %2.2x TPDO 1 transmit type\n", nodeId);
00112 res = writeNetworkDictCallBack (d,
00113 nodeId,
00114 0x1800,
00115 0x02,
00116 1,
00117 0,
00118 &Transmission_Type,
00119 CheckSDOAndContinue);
00120 break;
00121
00122 case 2:
00123 eprintf("Master : set slave %2.2x TPDO 2 transmit type\n", nodeId);
00124 writeNetworkDictCallBack (d,
00125 nodeId,
00126 0x1801,
00127 0x02,
00128 1,
00129 0,
00130 &Transmission_Type,
00131 CheckSDOAndContinue);
00132 break;
00133 case 3:
00134
00135
00136
00137
00138 setState(d, Operational);
00139
00140
00141 masterSendNMTstateChange (d, nodeId, NMT_Start_Node);
00142
00143 }
00144 }
00145
00146 void TestMaster_preOperational()
00147 {
00148
00149 eprintf("TestMaster_preOperational\n");
00150 ConfigureSlaveNode(&TestMaster_Data, 0x02);
00151
00152 }
00153
00154 void TestMaster_operational()
00155 {
00156 eprintf("TestMaster_operational\n");
00157 }
00158
00159 void TestMaster_stopped()
00160 {
00161 eprintf("TestMaster_stopped\n");
00162 }
00163
00164 void TestMaster_post_sync()
00165 {
00166 eprintf("TestMaster_post_sync\n");
00167 eprintf("Master: %d %d %d %d %d %d %d %d %d %x %x %d %d\n",
00168 MasterMap1,
00169 MasterMap2,
00170 MasterMap3,
00171 MasterMap4,
00172 MasterMap5,
00173 MasterMap6,
00174 MasterMap7,
00175 MasterMap8,
00176 MasterMap9,
00177 MasterMap10,
00178 MasterMap11,
00179 MasterMap12,
00180 MasterMap13);
00181 }
00182
00183 char query_result = 0;
00184 char waiting_answer = 0;
00185
00186
00187 static void CheckSDO(CO_Data* d, UNS8 nodeId)
00188 {
00189 UNS32 abortCode;
00190 if(getWriteResultNetworkDict (d, nodeId, &abortCode) != SDO_FINISHED)
00191 eprintf("Master : Failed in changing Slave's transmit type AbortCode :%4.4x \n", abortCode);
00192
00193
00194 closeSDOtransfer(&TestMaster_Data, nodeId, SDO_CLIENT);
00195 }
00196
00197
00198 static int MasterSyncCount = 0;
00199 void TestMaster_post_TPDO()
00200 {
00201 eprintf("TestMaster_post_TPDO MasterSyncCount = %d \n", MasterSyncCount);
00202
00203
00204
00205
00206
00207
00208
00209
00210
00211
00212
00213
00214
00215
00216
00217 #if 0
00218 if(waiting_answer){
00219 UNS32 abortCode;
00220 UNS8 size;
00221 switch(getReadResultNetworkDict (
00222 &TestMaster_Data,
00223 0x02,
00224 &query_result,
00225 &size,
00226 &abortCode))
00227 {
00228 case SDO_FINISHED:
00229
00230 eprintf("Got SDO answer (0x2002, 0x00), %d %d\n",query_result,size);
00231 case SDO_ABORTED_RCV:
00232 case SDO_ABORTED_INTERNAL:
00233 case SDO_RESET:
00234 waiting_answer = 0;
00235 closeSDOtransfer(
00236 &TestMaster_Data,
00237 0x02,
00238 SDO_CLIENT);
00239 break;
00240 case SDO_DOWNLOAD_IN_PROGRESS:
00241 case SDO_UPLOAD_IN_PROGRESS:
00242 break;
00243 }
00244 }else if(MasterSyncCount % 10 == 0){
00245 readNetworkDict (
00246 &TestMaster_Data,
00247 0x02,
00248 0x2002,
00249 0x00,
00250 0);
00251 waiting_answer = 1;
00252 }
00253 #endif
00254 if(MasterSyncCount % 17 == 0){
00255 eprintf("Master : Ask RTR PDO (0x1402)\n");
00256 sendPDOrequest(&TestMaster_Data, 0x1402 );
00257 sendPDOrequest(&TestMaster_Data, 0x1403 );
00258 }
00259 if(MasterSyncCount % 50 == 0){
00260 eprintf("Master : Change slave's transmit type to 0xFF\n");
00261 UNS8 transmitiontype = 0xFF;
00262 writeNetworkDictCallBack (&TestMaster_Data,
00263 2,
00264 0x1802,
00265 0x02,
00266 1,
00267 0,
00268 &transmitiontype,
00269 CheckSDO);
00270 }
00271 if(MasterSyncCount % 50 == 25){
00272 eprintf("Master : Change slave's transmit type to 0x00\n");
00273 UNS8 transmitiontype = 0x00;
00274 writeNetworkDictCallBack (&TestMaster_Data,
00275 2,
00276 0x1802,
00277 0x02,
00278 1,
00279 0,
00280 &transmitiontype,
00281 CheckSDO);
00282 }
00283 MasterSyncCount++;
00284 }