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 /*****************************************************************************/
  28 void TestMaster_heartbeatError(UNS8 heartbeatID)
  29 {
  30         eprintf("TestMaster_heartbeatError %d\n", heartbeatID);
  31 }
  32
  33 /********************************************************
  34  * ConfigureSlaveNode is responsible to
  35  *  - setup master RPDO 1 to receive TPDO 1 from id 2
  36  *  - setup master RPDO 2 to receive TPDO 2 from id 2
  37  ********************************************************/
  38 void TestMaster_initialisation()
  39 {
  40         UNS32 PDO1_COBID = 0x0182;
  41         UNS32 PDO2_COBID = 0x0282;
  42         UNS8 size = sizeof(UNS32);
  43
  44         eprintf("TestMaster_initialisation\n");
  45
  46         /*****************************************
  47          * Define RPDOs to match slave ID=2 TPDOs*
  48          *****************************************/
  49         writeLocalDict( &TestMaster_Data, /*CO_Data* d*/
  50                         0x1400, /*UNS16 index*/
  51                         0x01, /*UNS8 subind*/
  52                         &PDO1_COBID, /*void * pSourceData,*/
  53                         &size, /* UNS8 * pExpectedSize*/
  54                         RW);  /* UNS8 checkAccess */
  55
  56         writeLocalDict( &TestMaster_Data, /*CO_Data* d*/
  57                         0x1401, /*UNS16 index*/
  58                         0x01, /*UNS8 subind*/
  59                         &PDO2_COBID, /*void * pSourceData,*/
  60                         &size, /* UNS8 * pExpectedSize*/
  61                         RW);  /* UNS8 checkAccess */
  62 }
  63
  64 // Step counts number of times ConfigureSlaveNode is called
  65 static init_step = 0;
  66
  67 /*Froward declaration*/
  68 static void ConfigureSlaveNode(CO_Data* d, UNS8 nodeId);
  69
  70 /**/
  71 static void CheckSDOAndContinue(CO_Data* d, UNS8 nodeId)
  72 {
  73         UNS32 abortCode;
  74         if(getWriteResultNetworkDict (d, nodeId, &abortCode) != SDO_FINISHED)
  75                 eprintf("Master : Failed in initializing slave %2.2x, step %d, AbortCode :%4.4x \n", nodeId, init_step, abortCode);
  76
  77         /* Finalise last SDO transfer with this node */
  78         closeSDOtransfer(&TestMaster_Data, nodeId, SDO_CLIENT);
  79
  80         ConfigureSlaveNode(d, nodeId);
  81 }
  82
  83 /********************************************************
  84  * ConfigureSlaveNode is responsible to
  85  *  - setup slave TPDO 1 transmit time
  86  *  - setup slave TPDO 2 transmit time
  87  *  - switch to operational mode
  88  *  - send NMT to slave
  89  ********************************************************
  90  * This an example of :
  91  * Network Dictionary Access (SDO) with Callback
  92  * Slave node state change request (NMT)
  93  ********************************************************
  94  * This is called first by TestMaster_preOperational
  95  * then it called again each time a SDO exchange is
  96  * finished.
  97  ********************************************************/
  98
  99 static void ConfigureSlaveNode(CO_Data* d, UNS8 nodeId)
 100 {
 101         /* Master configure heartbeat producer time at 1000 ms
 102          * for slave node-id 0x02 by DCF concise */
 103
 104         UNS8 Transmission_Type = 0x01;
 105         UNS32 abortCode;
 106         UNS8 res;
 107         eprintf("Master : ConfigureSlaveNode %2.2x\n", nodeId);
 108
 109         switch(++init_step){
 110                 case 1: /*First step : setup Slave's TPDO 1 to be transmitted on SYNC*/
 111                         eprintf("Master : set slave %2.2x TPDO 1 transmit type\n", nodeId);
 112                         res = writeNetworkDictCallBack (d, /*CO_Data* d*/
 113                                         nodeId, /*UNS8 nodeId*/
 114                                         0x1800, /*UNS16 index*/
 115                                         0x02, /*UNS8 subindex*/
 116                                         1, /*UNS8 count*/
 117                                         0, /*UNS8 dataType*/
 118                                         &Transmission_Type,/*void *data*/
 119                                         CheckSDOAndContinue); /*SDOCallback_t Callback*/
 120                                         break;
 121
 122                 case 2: /*Second step*/
 123                         eprintf("Master : set slave %2.2x TPDO 2 transmit type\n", nodeId);
 124                         writeNetworkDictCallBack (d, /*CO_Data* d*/
 125                                         nodeId, /*UNS8 nodeId*/
 126                                         0x1801, /*UNS16 index*/
 127                                         0x02, /*UNS16 index*/
 128                                         1, /*UNS8 count*/
 129                                         0, /*UNS8 dataType*/
 130                                         &Transmission_Type,/*void *data*/
 131                                         CheckSDOAndContinue); /*SDOCallback_t Callback*/
 132                                         break;
 133                 case 3:
 134
 135                 /****************************** START *******************************/
 136
 137                         /* Put the master in operational mode */
 138                         setState(d, Operational);
 139
 140                         /* Ask slave node to go in operational mode */
 141                         masterSendNMTstateChange (d, nodeId, NMT_Start_Node);
 142
 143         }
 144 }
 145
 146 void TestMaster_preOperational()
 147 {
 148
 149         eprintf("TestMaster_preOperational\n");
 150         ConfigureSlaveNode(&TestMaster_Data, 0x02);
 151
 152 }
 153
 154 void TestMaster_operational()
 155 {
 156         eprintf("TestMaster_operational\n");
 157 }
 158
 159 void TestMaster_stopped()
 160 {
 161         eprintf("TestMaster_stopped\n");
 162 }
 163
 164 void TestMaster_post_sync()
 165 {
 166         eprintf("TestMaster_post_sync\n");
 167         eprintf("Master: %d %d %d %d %d %d %d %d %d %x %x %d %d\n",
 168                 MasterMap1,
 169                 MasterMap2,
 170                 MasterMap3,
 171                 MasterMap4,
 172                 MasterMap5,
 173                 MasterMap6,
 174                 MasterMap7,
 175                 MasterMap8,
 176                 MasterMap9,
 177                 MasterMap10,
 178                 MasterMap11,
 179                 MasterMap12,
 180                 MasterMap13);
 181 }
 182
 183 char query_result = 0;
 184 char waiting_answer = 0;
 185
 186
 187 static void CheckSDO(CO_Data* d, UNS8 nodeId)
 188 {
 189         UNS32 abortCode;
 190         if(getWriteResultNetworkDict (d, nodeId, &abortCode) != SDO_FINISHED)
 191                 eprintf("Master : Failed in changing Slave's transmit type AbortCode :%4.4x \n", abortCode);
 192
 193         /* Finalise last SDO transfer with this node */
 194         closeSDOtransfer(&TestMaster_Data, nodeId, SDO_CLIENT);
 195 }
 196
 197
 198 static int MasterSyncCount = 0;
 199 void TestMaster_post_TPDO()
 200 {
 201         eprintf("TestMaster_post_TPDO MasterSyncCount = %d \n", MasterSyncCount);
 202 //
 203 //      {
 204 //              char zero = 0;
 205 //              if(MasterMap4 > 0x80){
 206 //                      writeNetworkDict (
 207 //                              &TestMaster_Data,
 208 //                              TestSlave_Data->bDeviceNodeId,
 209 //                              0x2002,
 210 //                              0x00,
 211 //                              1,
 212 //                              0,
 213 //                              &zero);
 214 //              }
 215 //      }
 216
 217 #if 0
 218         if(waiting_answer){
 219                 UNS32 abortCode;
 220                 UNS8 size;
 221                 switch(getReadResultNetworkDict (
 222                         &TestMaster_Data,
 223                         0x02,
 224                         &query_result,
 225                         &size,
 226                         &abortCode))
 227                 {
 228                         case SDO_FINISHED:
 229                                 /* Do something with result here !!*/
 230                                 eprintf("Got SDO answer (0x2002, 0x00), %d %d\n",query_result,size);
 231                         case SDO_ABORTED_RCV:
 232                         case SDO_ABORTED_INTERNAL:
 233                         case SDO_RESET:
 234                                 waiting_answer = 0;
 235                                 closeSDOtransfer(
 236                                         &TestMaster_Data,
 237                                         0x02,
 238                                         SDO_CLIENT);
 239                         break;
 240                         case SDO_DOWNLOAD_IN_PROGRESS:
 241                         case SDO_UPLOAD_IN_PROGRESS:
 242                         break;
 243                 }
 244         }else if(MasterSyncCount % 10 == 0){
 245                 readNetworkDict (
 246                         &TestMaster_Data,
 247                         0x02,
 248                         0x2002,
 249                         0x00,
 250                         0);
 251                 waiting_answer = 1;
 252         }
 253 #endif
 254         if(MasterSyncCount % 17 == 0){
 255                 eprintf("Master : Ask RTR PDO (0x1402)\n");
 256                 sendPDOrequest(&TestMaster_Data, 0x1402 );
 257                 sendPDOrequest(&TestMaster_Data, 0x1403 );
 258         }
 259         if(MasterSyncCount % 50 == 0){
 260                 eprintf("Master : Change slave's transmit type to 0xFF\n");
 261                 UNS8 transmitiontype = 0xFF;
 262                 writeNetworkDictCallBack (&TestMaster_Data, /*CO_Data* d*/
 263                                         2, /*UNS8 nodeId*/
 264                                         0x1802, /*UNS16 index*/
 265                                         0x02, /*UNS16 index*/
 266                                         1, /*UNS8 count*/
 267                                         0, /*UNS8 dataType*/
 268                                         &transmitiontype,/*void *data*/
 269                                         CheckSDO); /*SDOCallback_t Callback*/
 270         }
 271         if(MasterSyncCount % 50 == 25){
 272                 eprintf("Master : Change slave's transmit type to 0x00\n");
 273                 UNS8 transmitiontype = 0x00;
 274                 writeNetworkDictCallBack (&TestMaster_Data, /*CO_Data* d*/
 275                                         2, /*UNS8 nodeId*/
 276                                         0x1802, /*UNS16 index*/
 277                                         0x02, /*UNS16 index*/
 278                                         1, /*UNS8 count*/
 279                                         0, /*UNS8 dataType*/
 280                                         &transmitiontype,/*void *data*/
 281                                         CheckSDO); /*SDOCallback_t Callback*/
 282         }
 283         MasterSyncCount++;
 284 }