CanFestival: examples/TestMasterSlave/Master.c Source File

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: