examples/TestMasterSlave/Master.c

   1 /*
   2 This file is part of CanFestival, a library implementing CanOpen Stack.
   3
   4 Copyright (C): Edouard TISSERANT and Francis DUPIN
   5
   6 See COPYING file for copyrights details.
   7
   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.
  12
  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.
  17
  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
  21 */
  22
  23 #include "Master.h"
  24 #include "Slave.h"
  25 #include "TestMasterSlave.h"
  26
  27 extern s_BOARD MasterBoard;
  28 /*****************************************************************************/
  29 void TestMaster_heartbeatError(CO_Data* d, UNS8 heartbeatID)
  30 {
  31         eprintf("TestMaster_heartbeatError %d\n", heartbeatID);
  32 }
  33
  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)
  40 {
  41         UNS32 PDO1_COBID = 0x0182;
  42         UNS32 PDO2_COBID = 0x0282;
  43         UNS8 size = sizeof(UNS32);
  44         UNS32 SINC_cicle=0;
  45         UNS8 data_type = 0;
  46
  47         eprintf("TestMaster_initialisation\n");
  48
  49         /*****************************************
  50          * Define RPDOs to match slave ID=2 TPDOs*
  51          *****************************************/
  52         writeLocalDict( &TestMaster_Data, /*CO_Data* d*/
  53                         0x1400, /*UNS16 index*/
  54                         0x01, /*UNS8 subind*/
  55                         &PDO1_COBID, /*void * pSourceData,*/
  56                         &size, /* UNS8 * pExpectedSize*/
  57                         RW);  /* UNS8 checkAccess */
  58
  59         writeLocalDict( &TestMaster_Data, /*CO_Data* d*/
  60                         0x1401, /*UNS16 index*/
  61                         0x01, /*UNS8 subind*/
  62                         &PDO2_COBID, /*void * pSourceData,*/
  63                         &size, /* UNS8 * pExpectedSize*/
  64                         RW);  /* UNS8 checkAccess */
  65
  66 }
  67
  68 // Step counts number of times ConfigureSlaveNode is called
  69 static int init_step = 0;
  70
  71 /*Froward declaration*/
  72 static void ConfigureSlaveNode(CO_Data* d, UNS8 nodeId);
  73
  74 /**/
  75 static void CheckSDOAndContinue(CO_Data* d, UNS8 nodeId)
  76 {
  77         UNS32 abortCode;
  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);
  80
  81         /* Finalise last SDO transfer with this node */
  82         closeSDOtransfer(&TestMaster_Data, nodeId, SDO_CLIENT);
  83
  84         ConfigureSlaveNode(d, nodeId);
  85 }
  86
  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
  92  *  - send NMT to slave
  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
 100  * finished.
 101  ********************************************************/
 102
 103 static void ConfigureSlaveNode(CO_Data* d, UNS8 nodeId)
 104 {
 105         /* Master configure heartbeat producer time at 1000 ms
 106          * for slave node-id 0x02 by DCF concise */
 107
 108         UNS8 Transmission_Type = 0x01;
 109         UNS32 abortCode;
 110         UNS8 res;
 111         eprintf("Master : ConfigureSlaveNode %2.2x\n", nodeId);
 112
 113         switch(++init_step){
 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*/
 120                                         1, /*UNS8 count*/
 121                                         0, /*UNS8 dataType*/
 122                                         &Transmission_Type,/*void *data*/
 123                                         CheckSDOAndContinue); /*SDOCallback_t Callback*/
 124                                         break;
 125
 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*/
 132                                         1, /*UNS8 count*/
 133                                         0, /*UNS8 dataType*/
 134                                         &Transmission_Type,/*void *data*/
 135                                         CheckSDOAndContinue); /*SDOCallback_t Callback*/
 136                                         break;
 137                 case 3:
 138
 139                 /****************************** START *******************************/
 140
 141                         /* Put the master in operational mode */
 142                         setState(d, Operational);
 143
 144                         /* Ask slave node to go in operational mode */
 145                         masterSendNMTstateChange (d, nodeId, NMT_Start_Node);
 146
 147         }
 148 }
 149
 150
 151 void TestMaster_preOperational(CO_Data* d)
 152 {
 153         eprintf("TestMaster_preOperational\n");
 154 }
 155
 156 void TestMaster_operational(CO_Data* d)
 157 {
 158         eprintf("TestMaster_operational\n");
 159 }
 160
 161 void TestMaster_stopped(CO_Data* d)
 162 {
 163         eprintf("TestMaster_stopped\n");
 164 }
 165
 166 void TestMaster_post_sync(CO_Data* d)
 167 {
 168         eprintf("TestMaster_post_sync\n");
 169         eprintf("Master: %d %d %d %d %d %d %d %d %d %x %x %d %d\n",
 170                 MasterMap1,
 171                 MasterMap2,
 172                 MasterMap3,
 173                 MasterMap4,
 174                 MasterMap5,
 175                 MasterMap6,
 176                 MasterMap7,
 177                 MasterMap8,
 178                 MasterMap9,
 179                 MasterMap10,
 180                 MasterMap11,
 181                 MasterMap12,
 182                 MasterMap13);
 183 }
 184
 185 void TestMaster_post_emcy(CO_Data* d, UNS8 nodeID, UNS16 errCode, UNS8 errReg)
 186 {
 187         eprintf("Master received EMCY message. Node: %2.2x  ErrorCode: %4.4x  ErrorRegister: %2.2x\n", nodeID, errCode, errReg);
 188 }
 189
 190 char query_result = 0;
 191 char waiting_answer = 0;
 192
 193 static void CheckSDO(CO_Data* d, UNS8 nodeId)
 194 {
 195         UNS32 abortCode;
 196         if(getWriteResultNetworkDict (d, nodeId, &abortCode) != SDO_FINISHED)
 197                 eprintf("Master : Failed in changing Slave's transmit type AbortCode :%4.4x \n", abortCode);
 198
 199         /* Finalise last SDO transfer with this node */
 200         closeSDOtransfer(&TestMaster_Data, nodeId, SDO_CLIENT);
 201 }
 202
 203
 204 static int MasterSyncCount = 0;
 205 void TestMaster_post_TPDO(CO_Data* d)
 206 {
 207         eprintf("TestMaster_post_TPDO MasterSyncCount = %d \n", MasterSyncCount);
 208 //
 209 //      {
 210 //              char zero = 0;
 211 //              if(MasterMap4 > 0x80){
 212 //                      writeNetworkDict (
 213 //                              &TestMaster_Data,
 214 //                              TestSlave_Data->bDeviceNodeId,
 215 //                              0x2002,
 216 //                              0x00,
 217 //                              1,
 218 //                              0,
 219 //                              &zero);
 220 //              }
 221 //      }
 222
 223 #if 0
 224         if(waiting_answer){
 225                 UNS32 abortCode;
 226                 UNS8 size;
 227                 switch(getReadResultNetworkDict (
 228                         &TestMaster_Data,
 229                         0x02,
 230                         &query_result,
 231                         &size,
 232                         &abortCode))
 233                 {
 234                         case SDO_FINISHED:
 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:
 239                         case SDO_RESET:
 240                                 waiting_answer = 0;
 241                                 closeSDOtransfer(
 242                                         &TestMaster_Data,
 243                                         0x02,
 244                                         SDO_CLIENT);
 245                         break;
 246                         case SDO_DOWNLOAD_IN_PROGRESS:
 247                         case SDO_UPLOAD_IN_PROGRESS:
 248                         break;
 249                 }
 250         }else if(MasterSyncCount % 10 == 0){
 251                 readNetworkDict (
 252                         &TestMaster_Data,
 253                         0x02,
 254                         0x2002,
 255                         0x00,
 256                         0);
 257                 waiting_answer = 1;
 258         }
 259 #endif
 260         if(MasterSyncCount % 17 == 0){
 261                 eprintf("Master : Ask RTR PDO (0x1402)\n");
 262                 sendPDOrequest(&TestMaster_Data, 0x1402 );
 263                 sendPDOrequest(&TestMaster_Data, 0x1403 );
 264         }
 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*/
 269                                         2, /*UNS8 nodeId*/
 270                                         0x1802, /*UNS16 index*/
 271                                         0x02, /*UNS16 index*/
 272                                         1, /*UNS8 count*/
 273                                         0, /*UNS8 dataType*/
 274                                         &transmitiontype,/*void *data*/
 275                                         CheckSDO); /*SDOCallback_t Callback*/
 276         }
 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*/
 281                                         2, /*UNS8 nodeId*/
 282                                         0x1802, /*UNS16 index*/
 283                                         0x02, /*UNS16 index*/
 284                                         1, /*UNS8 count*/
 285                                         0, /*UNS8 dataType*/
 286                                         &transmitiontype,/*void *data*/
 287                                         CheckSDO); /*SDOCallback_t Callback*/
 288         }
 289         MasterSyncCount++;
 290 }
 291
 292 void TestMaster_post_SlaveBootup(CO_Data* d, UNS8 nodeid)
 293 {
 294         eprintf("TestMaster_post_SlaveBootup %x\n", nodeid);
 295
 296         ConfigureSlaveNode(d, nodeid);
 297 }