nico@215: nico@215:
nico@215:00001 /* nico@215: 00002 This file is part of CanFestival, a library implementing CanOpen Stack. nico@215: 00003 nico@215: 00004 Copyright (C): Edouard TISSERANT and Francis DUPIN nico@215: 00005 nico@215: 00006 See COPYING file for copyrights details. nico@215: 00007 nico@215: 00008 This library is free software; you can redistribute it and/or nico@215: 00009 modify it under the terms of the GNU Lesser General Public nico@215: 00010 License as published by the Free Software Foundation; either nico@215: 00011 version 2.1 of the License, or (at your option) any later version. nico@215: 00012 nico@215: 00013 This library is distributed in the hope that it will be useful, nico@215: 00014 but WITHOUT ANY WARRANTY; without even the implied warranty of nico@215: 00015 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU nico@215: 00016 Lesser General Public License for more details. nico@215: 00017 nico@215: 00018 You should have received a copy of the GNU Lesser General Public nico@215: 00019 License along with this library; if not, write to the Free Software nico@215: 00020 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA nico@215: 00021 */ nico@215: 00022 nico@215: 00023 #include "Master.h" nico@215: 00024 #include "Slave.h" nico@215: 00025 #include "TestMasterSlave.h" nico@215: 00026 nico@215: 00027 /*****************************************************************************/ etisserant@240: 00028 void TestMaster_heartbeatError(UNS8 heartbeatID) nico@215: 00029 { etisserant@240: 00030 eprintf("TestMaster_heartbeatError %d\n", heartbeatID); nico@215: 00031 } nico@215: 00032 nico@215: 00033 /******************************************************** nico@215: 00034 * ConfigureSlaveNode is responsible to nico@215: 00035 * - setup master RPDO 1 to receive TPDO 1 from id 2 nico@215: 00036 * - setup master RPDO 2 to receive TPDO 2 from id 2 nico@215: 00037 ********************************************************/ etisserant@240: 00038 void TestMaster_initialisation() nico@215: 00039 { etisserant@240: 00040 UNS32 PDO1_COBID = 0x0182; etisserant@240: 00041 UNS32 PDO2_COBID = 0x0282; etisserant@240: 00042 UNS8 size = sizeof(UNS32); nico@215: 00043 etisserant@240: 00044 eprintf("TestMaster_initialisation\n"); nico@215: 00045 nico@215: 00046 /***************************************** nico@215: 00047 * Define RPDOs to match slave ID=2 TPDOs* nico@215: 00048 *****************************************/ etisserant@240: 00049 writeLocalDict( &TestMaster_Data, /*CO_Data* d*/ nico@215: 00050 0x1400, /*UNS16 index*/ nico@215: 00051 0x01, /*UNS8 subind*/ nico@215: 00052 &PDO1_COBID, /*void * pSourceData,*/ nico@215: 00053 &size, /* UNS8 * pExpectedSize*/ etisserant@240: 00054 RW); /* UNS8 checkAccess */ nico@215: 00055 etisserant@240: 00056 writeLocalDict( &TestMaster_Data, /*CO_Data* d*/ nico@215: 00057 0x1401, /*UNS16 index*/ nico@215: 00058 0x01, /*UNS8 subind*/ nico@215: 00059 &PDO2_COBID, /*void * pSourceData,*/ nico@215: 00060 &size, /* UNS8 * pExpectedSize*/ etisserant@240: 00061 RW); /* UNS8 checkAccess */ nico@215: 00062 } nico@215: 00063 nico@215: 00064 // Step counts number of times ConfigureSlaveNode is called nico@215: 00065 static init_step = 0; nico@215: 00066 nico@215: 00067 /*Froward declaration*/ etisserant@240: 00068 static void ConfigureSlaveNode(CO_Data* d, UNS8 nodeId); nico@215: 00069 nico@215: 00070 etisserant@240: 00071 static void CheckSDOAndContinue(CO_Data* d, UNS8 nodeId) nico@215: 00072 { etisserant@240: 00073 UNS32 abortCode; etisserant@240: 00074 if(getWriteResultNetworkDict (d, nodeId, &abortCode) != SDO_FINISHED) etisserant@240: 00075 eprintf("Master : Failed in initializing slave %2.2x, step %d, AbortCode :%4.4x \n", nodeId, init_step, abortCode); nico@215: 00076 nico@215: 00077 /* Finalise last SDO transfer with this node */ etisserant@240: 00078 closeSDOtransfer(&TestMaster_Data, nodeId, SDO_CLIENT); nico@215: 00079 nico@215: 00080 ConfigureSlaveNode(d, nodeId); nico@215: 00081 } nico@215: 00082 nico@215: 00083 /******************************************************** nico@215: 00084 * ConfigureSlaveNode is responsible to nico@215: 00085 * - setup slave TPDO 1 transmit time nico@215: 00086 * - setup slave TPDO 2 transmit time nico@215: 00087 * - switch to operational mode nico@215: 00088 * - send NMT to slave nico@215: 00089 ******************************************************** nico@215: 00090 * This an example of : nico@215: 00091 * Network Dictionary Access (SDO) with Callback nico@215: 00092 * Slave node state change request (NMT) nico@215: 00093 ******************************************************** nico@215: 00094 * This is called first by TestMaster_preOperational nico@215: 00095 * then it called again each time a SDO exchange is nico@215: 00096 * finished. nico@215: 00097 ********************************************************/ nico@215: 00098 etisserant@240: 00099 static void ConfigureSlaveNode(CO_Data* d, UNS8 nodeId) nico@215: 00100 { nico@215: 00101 /* Master configure heartbeat producer time at 1000 ms nico@215: 00102 * for slave node-id 0x02 by DCF concise */ nico@215: 00103 etisserant@240: 00104 UNS8 Transmission_Type = 0x01; etisserant@240: 00105 UNS32 abortCode; etisserant@240: 00106 UNS8 res; etisserant@240: 00107 eprintf("Master : ConfigureSlaveNode %2.2x\n", nodeId); nico@215: 00108 nico@215: 00109 switch(++init_step){ nico@215: 00110 case 1: /*First step : setup Slave's TPDO 1 to be transmitted on SYNC*/ etisserant@240: 00111 eprintf("Master : set slave %2.2x TPDO 1 transmit type\n", nodeId); etisserant@240: 00112 res = writeNetworkDictCallBack (d, /*CO_Data* d*/ nico@215: 00113 nodeId, /*UNS8 nodeId*/ nico@215: 00114 0x1800, /*UNS16 index*/ nico@215: 00115 0x02, /*UNS8 subindex*/ nico@215: 00116 1, /*UNS8 count*/ nico@215: 00117 0, /*UNS8 dataType*/ nico@215: 00118 &Transmission_Type,/*void *data*/ nico@215: 00119 CheckSDOAndContinue); /*SDOCallback_t Callback*/ nico@215: 00120 break; nico@215: 00121 nico@215: 00122 case 2: /*Second step*/ etisserant@240: 00123 eprintf("Master : set slave %2.2x TPDO 2 transmit type\n", nodeId); etisserant@240: 00124 writeNetworkDictCallBack (d, /*CO_Data* d*/ nico@215: 00125 nodeId, /*UNS8 nodeId*/ nico@215: 00126 0x1801, /*UNS16 index*/ nico@215: 00127 0x02, /*UNS16 index*/ nico@215: 00128 1, /*UNS8 count*/ nico@215: 00129 0, /*UNS8 dataType*/ nico@215: 00130 &Transmission_Type,/*void *data*/ nico@215: 00131 CheckSDOAndContinue); /*SDOCallback_t Callback*/ nico@215: 00132 break; nico@215: 00133 case 3: nico@215: 00134 nico@215: 00135 /****************************** START *******************************/ nico@215: 00136 nico@215: 00137 /* Put the master in operational mode */ etisserant@240: 00138 setState(d, Operational); nico@215: 00139 nico@215: 00140 /* Ask slave node to go in operational mode */ etisserant@240: 00141 masterSendNMTstateChange (d, nodeId, NMT_Start_Node); nico@215: 00142 nico@215: 00143 } nico@215: 00144 } nico@215: 00145 etisserant@240: 00146 void TestMaster_preOperational() nico@215: 00147 { nico@215: 00148 etisserant@240: 00149 eprintf("TestMaster_preOperational\n"); etisserant@240: 00150 ConfigureSlaveNode(&TestMaster_Data, 0x02); nico@215: 00151 nico@215: 00152 } nico@215: 00153 etisserant@240: 00154 void TestMaster_operational() nico@215: 00155 { etisserant@240: 00156 eprintf("TestMaster_operational\n"); nico@215: 00157 } nico@215: 00158 etisserant@240: 00159 void TestMaster_stopped() nico@215: 00160 { etisserant@240: 00161 eprintf("TestMaster_stopped\n"); nico@215: 00162 } nico@215: 00163 etisserant@240: 00164 void TestMaster_post_sync() nico@215: 00165 { etisserant@240: 00166 eprintf("TestMaster_post_sync\n"); etisserant@240: 00167 eprintf("Master: %d %d %d %d %d %d %d %d %d %x %x %d %d\n", etisserant@240: 00168 MasterMap1, etisserant@240: 00169 MasterMap2, etisserant@240: 00170 MasterMap3, etisserant@240: 00171 MasterMap4, etisserant@240: 00172 MasterMap5, etisserant@240: 00173 MasterMap6, etisserant@240: 00174 MasterMap7, etisserant@240: 00175 MasterMap8, etisserant@240: 00176 MasterMap9, etisserant@240: 00177 MasterMap10, etisserant@240: 00178 MasterMap11, etisserant@240: 00179 MasterMap12, etisserant@240: 00180 MasterMap13); etisserant@240: 00181 } etisserant@240: 00182 etisserant@240: 00183 char query_result = 0; etisserant@240: 00184 char waiting_answer = 0; etisserant@240: 00185 etisserant@240: 00186 etisserant@240: 00187 static void CheckSDO(CO_Data* d, UNS8 nodeId) etisserant@240: 00188 { etisserant@240: 00189 UNS32 abortCode; etisserant@240: 00190 if(getWriteResultNetworkDict (d, nodeId, &abortCode) != SDO_FINISHED) etisserant@240: 00191 eprintf("Master : Failed in changing Slave's transmit type AbortCode :%4.4x \n", abortCode); etisserant@240: 00192 etisserant@240: 00193 /* Finalise last SDO transfer with this node */ etisserant@240: 00194 closeSDOtransfer(&TestMaster_Data, nodeId, SDO_CLIENT); etisserant@240: 00195 } etisserant@240: 00196 etisserant@240: 00197 etisserant@240: 00198 static int MasterSyncCount = 0; etisserant@240: 00199 void TestMaster_post_TPDO() etisserant@240: 00200 { etisserant@240: 00201 eprintf("TestMaster_post_TPDO MasterSyncCount = %d \n", MasterSyncCount); etisserant@240: 00202 // etisserant@240: 00203 // { etisserant@240: 00204 // char zero = 0; etisserant@240: 00205 // if(MasterMap4 > 0x80){ etisserant@240: 00206 // writeNetworkDict ( etisserant@240: 00207 // &TestMaster_Data, etisserant@240: 00208 // TestSlave_Data->bDeviceNodeId, etisserant@240: 00209 // 0x2002, etisserant@240: 00210 // 0x00, etisserant@240: 00211 // 1, etisserant@240: 00212 // 0, etisserant@240: 00213 // &zero); etisserant@240: 00214 // } etisserant@240: 00215 // } etisserant@240: 00216 etisserant@240: 00217 #if 0 etisserant@240: 00218 if(waiting_answer){ etisserant@240: 00219 UNS32 abortCode; etisserant@240: 00220 UNS8 size; etisserant@240: 00221 switch(getReadResultNetworkDict ( etisserant@240: 00222 &TestMaster_Data, etisserant@240: 00223 0x02, etisserant@240: 00224 &query_result, etisserant@240: 00225 &size, etisserant@240: 00226 &abortCode)) etisserant@240: 00227 { etisserant@240: 00228 case SDO_FINISHED: etisserant@240: 00229 /* Do something with result here !!*/ etisserant@240: 00230 eprintf("Got SDO answer (0x2002, 0x00), %d %d\n",query_result,size); etisserant@240: 00231 case SDO_ABORTED_RCV: etisserant@240: 00232 case SDO_ABORTED_INTERNAL: etisserant@240: 00233 case SDO_RESET: etisserant@240: 00234 waiting_answer = 0; etisserant@240: 00235 closeSDOtransfer( etisserant@240: 00236 &TestMaster_Data, etisserant@240: 00237 0x02, etisserant@240: 00238 SDO_CLIENT); etisserant@240: 00239 break; etisserant@240: 00240 case SDO_DOWNLOAD_IN_PROGRESS: etisserant@240: 00241 case SDO_UPLOAD_IN_PROGRESS: etisserant@240: 00242 break; etisserant@240: 00243 } etisserant@240: 00244 }else if(MasterSyncCount % 10 == 0){ etisserant@240: 00245 readNetworkDict ( etisserant@240: 00246 &TestMaster_Data, etisserant@240: 00247 0x02, etisserant@240: 00248 0x2002, etisserant@240: 00249 0x00, etisserant@240: 00250 0); etisserant@240: 00251 waiting_answer = 1; etisserant@240: 00252 } etisserant@240: 00253 #endif etisserant@240: 00254 if(MasterSyncCount % 17 == 0){ etisserant@240: 00255 eprintf("Master : Ask RTR PDO (0x1402)\n"); etisserant@240: 00256 sendPDOrequest(&TestMaster_Data, 0x1402 ); etisserant@240: 00257 sendPDOrequest(&TestMaster_Data, 0x1403 ); etisserant@240: 00258 } etisserant@240: 00259 if(MasterSyncCount % 50 == 0){ etisserant@240: 00260 eprintf("Master : Change slave's transmit type to 0xFF\n"); etisserant@240: 00261 UNS8 transmitiontype = 0xFF; etisserant@240: 00262 writeNetworkDictCallBack (&TestMaster_Data, /*CO_Data* d*/ etisserant@240: 00263 2, /*UNS8 nodeId*/ etisserant@240: 00264 0x1802, /*UNS16 index*/ etisserant@240: 00265 0x02, /*UNS16 index*/ etisserant@240: 00266 1, /*UNS8 count*/ etisserant@240: 00267 0, /*UNS8 dataType*/ etisserant@240: 00268 &transmitiontype,/*void *data*/ etisserant@240: 00269 CheckSDO); /*SDOCallback_t Callback*/ etisserant@240: 00270 } etisserant@240: 00271 if(MasterSyncCount % 50 == 25){ etisserant@240: 00272 eprintf("Master : Change slave's transmit type to 0x00\n"); etisserant@240: 00273 UNS8 transmitiontype = 0x00; etisserant@240: 00274 writeNetworkDictCallBack (&TestMaster_Data, /*CO_Data* d*/ etisserant@240: 00275 2, /*UNS8 nodeId*/ etisserant@240: 00276 0x1802, /*UNS16 index*/ etisserant@240: 00277 0x02, /*UNS16 index*/ etisserant@240: 00278 1, /*UNS8 count*/ etisserant@240: 00279 0, /*UNS8 dataType*/ etisserant@240: 00280 &transmitiontype,/*void *data*/ etisserant@240: 00281 CheckSDO); /*SDOCallback_t Callback*/ etisserant@240: 00282 } etisserant@240: 00283 MasterSyncCount++; etisserant@240: 00284 } etisserant@240: