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 /*****************************************************************************/
nico@215: 00028 void TestMaster_heartbeatError(UNS8 heartbeatID)
nico@215: 00029 {
nico@215: 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  ********************************************************/
nico@215: 00038 void TestMaster_initialisation()
nico@215: 00039 {
nico@215: 00040         UNS32 PDO1_COBID = 0x0182; 
nico@215: 00041         UNS32 PDO2_COBID = 0x0282;
nico@215: 00042         UNS8 size = sizeof(UNS32); 
nico@215: 00043         
nico@215: 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          *****************************************/
nico@215: 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*/
nico@215: 00054                         RW);  /* UNS8 checkAccess */
nico@215: 00055                         
nico@215: 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*/
nico@215: 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*/
nico@215: 00068 static void ConfigureSlaveNode(CO_Data* d, UNS8 nodeId);
nico@215: 00069 
nico@215: 00070 
nico@215: 00071 static void CheckSDOAndContinue(CO_Data* d, UNS8 nodeId)
nico@215: 00072 {
nico@215: 00073         UNS32 abortCode;        
nico@215: 00074         if(getWriteResultNetworkDict (d, nodeId, &abortCode) != SDO_FINISHED)
nico@215: 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 */
nico@215: 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  
nico@215: 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          
nico@215: 00104         UNS8 Transmission_Type = 0x01;
nico@215: 00105         UNS32 abortCode;
nico@215: 00106         UNS8 res;
nico@215: 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*/
nico@215: 00111                         eprintf("Master : set slave %2.2x TPDO 1 transmit type\n", nodeId);
nico@215: 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*/
nico@215: 00123                         eprintf("Master : set slave %2.2x TPDO 2 transmit type\n", nodeId);
nico@215: 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 */
nico@215: 00138                         setState(d, Operational);
nico@215: 00139                  
nico@215: 00140                         /* Ask slave node to go in operational mode */
nico@215: 00141                         masterSendNMTstateChange (d, nodeId, NMT_Start_Node);
nico@215: 00142                         
nico@215: 00143         }
nico@215: 00144 }
nico@215: 00145 
nico@215: 00146 void TestMaster_preOperational()
nico@215: 00147 {
nico@215: 00148 
nico@215: 00149         eprintf("TestMaster_preOperational\n");
nico@215: 00150         ConfigureSlaveNode(&TestMaster_Data, 0x02);
nico@215: 00151         
nico@215: 00152 }
nico@215: 00153 
nico@215: 00154 void TestMaster_operational()
nico@215: 00155 {
nico@215: 00156         eprintf("TestMaster_operational\n");
nico@215: 00157 }
nico@215: 00158 
nico@215: 00159 void TestMaster_stopped()
nico@215: 00160 {
nico@215: 00161         eprintf("TestMaster_stopped\n");
nico@215: 00162 }
nico@215: 00163 
nico@215: 00164 void TestMaster_post_sync()
nico@215: 00165 {
nico@215: 00166         eprintf("TestMaster_post_sync\n");
nico@215: 00167         eprintf("Master: %d %d %d %d %d %d %d %d %d %x %x\n",MasterMap1,MasterMap2 ,MasterMap3, MasterMap4,MasterMap5,MasterMap6,MasterMap7,MasterMap8,MasterMap9,MasterMap10,MasterMap11);
nico@215: 00168 }
nico@215: 00169 
nico@215: 00170 char query_result = 0;
nico@215: 00171 char waiting_answer = 0;
nico@215: 00172 
nico@215: 00173 void TestMaster_post_TPDO()
nico@215: 00174 {
nico@215: 00175         eprintf("TestMaster_post_TPDO\n");
nico@215: 00176 //
nico@215: 00177 //      {
nico@215: 00178 //              char zero = 0;
nico@215: 00179 //              if(MasterMap4 > 0x80){
nico@215: 00180 //                      writeNetworkDict (
nico@215: 00181 //                              &TestMaster_Data,
nico@215: 00182 //                              TestSlave_Data->bDeviceNodeId,
nico@215: 00183 //                              0x2002,
nico@215: 00184 //                              0x00,
nico@215: 00185 //                              1,
nico@215: 00186 //                              0,
nico@215: 00187 //                              &zero); 
nico@215: 00188 //              }
nico@215: 00189 //      }
nico@215: 00190 
nico@215: 00191         if(waiting_answer){
nico@215: 00192                 UNS32 abortCode;                        
nico@215: 00193                 UNS8 size;                      
nico@215: 00194                 switch(getReadResultNetworkDict (
nico@215: 00195                         &TestMaster_Data, 
nico@215: 00196                         0x02,
nico@215: 00197                         &query_result,
nico@215: 00198                         &size,
nico@215: 00199                         &abortCode))
nico@215: 00200                 {
nico@215: 00201                         case SDO_FINISHED:
nico@215: 00202                                 /* Do something with result here !!*/
nico@215: 00203                                 eprintf("Got SDO answer (0x2002, 0x00), %d %d\n",query_result,size);
nico@215: 00204                         case SDO_ABORTED_RCV:
nico@215: 00205                         case SDO_ABORTED_INTERNAL:
nico@215: 00206                         case SDO_RESET:
nico@215: 00207                                 waiting_answer = 0;
nico@215: 00208                                 closeSDOtransfer(
nico@215: 00209                                         &TestMaster_Data,
nico@215: 00210                                         0x02,
nico@215: 00211                                         SDO_CLIENT);
nico@215: 00212                         break;
nico@215: 00213                         case SDO_DOWNLOAD_IN_PROGRESS:
nico@215: 00214                         case SDO_UPLOAD_IN_PROGRESS:
nico@215: 00215                         break;
nico@215: 00216                 }
nico@215: 00217         }else if(MasterMap1 % 10 == 0){
nico@215: 00218                 readNetworkDict (
nico@215: 00219                         &TestMaster_Data,
nico@215: 00220                         0x02,
nico@215: 00221                         0x2002,
nico@215: 00222                         0x00,
nico@215: 00223                         0);
nico@215: 00224                 waiting_answer = 1;
nico@215: 00225         }
nico@215: 00226 }
nico@215: