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(CO_Data* d, 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(CO_Data* d)
  39 {
  40         UNS32 PDO1_COBID = 0x0182;
  41         UNS32 PDO2_COBID = 0x0282;
  42         UNS8 size = sizeof(UNS32);
  43         UNS32 SINC_cicle=0;
  44         UNS8 data_type = 0;
  45
  46         eprintf("TestMaster_initialisation\n");
  47
  48         /*****************************************
  49          * Define RPDOs to match slave ID=2 TPDOs*
  50          *****************************************/
  51         writeLocalDict( &TestMaster_Data, /*CO_Data* d*/
  52                         0x1400, /*UNS16 index*/
  53                         0x01, /*UNS8 subind*/
  54                         &PDO1_COBID, /*void * pSourceData,*/
  55                         &size, /* UNS8 * pExpectedSize*/
  56                         RW);  /* UNS8 checkAccess */
  57
  58         writeLocalDict( &TestMaster_Data, /*CO_Data* d*/
  59                         0x1401, /*UNS16 index*/
  60                         0x01, /*UNS8 subind*/
  61                         &PDO2_COBID, /*void * pSourceData,*/
  62                         &size, /* UNS8 * pExpectedSize*/
  63                         RW);  /* UNS8 checkAccess */
  64
  65 }
  66
  67 // Step counts number of times ConfigureSlaveNode is called
  68 static init_step = 0;
  69
  70 /*Froward declaration*/
  71 static void ConfigureSlaveNode(CO_Data* d, UNS8 nodeId);
  72
  73 /**/
  74 static void CheckSDOAndContinue(CO_Data* d, UNS8 nodeId)
  75 {
  76         UNS32 abortCode;
  77         if(getWriteResultNetworkDict (d, nodeId, &abortCode) != SDO_FINISHED)
  78                 eprintf("Master : Failed in initializing slave %2.2x, step %d, AbortCode :%4.4x \n", nodeId, init_step, abortCode);
  79
  80         /* Finalise last SDO transfer with this node */
  81         closeSDOtransfer(&TestMaster_Data, nodeId, SDO_CLIENT);
  82
  83         ConfigureSlaveNode(d, nodeId);
  84 }
  85
  86 /********************************************************
  87  * ConfigureSlaveNode is responsible to
  88  *  - setup slave TPDO 1 transmit time
  89  *  - setup slave TPDO 2 transmit time
  90  *  - switch to operational mode
  91  *  - send NMT to slave
  92  ********************************************************
  93  * This an example of :
  94  * Network Dictionary Access (SDO) with Callback
  95  * Slave node state change request (NMT)
  96  ********************************************************
  97  * This is called first by TestMaster_preOperational
  98  * then it called again each time a SDO exchange is
  99  * finished.
 100  ********************************************************/
 101
 102 static void ConfigureSlaveNode(CO_Data* d, UNS8 nodeId)
 103 {
 104         /* Master configure heartbeat producer time at 1000 ms
 105          * for slave node-id 0x02 by DCF concise */
 106
 107         UNS8 Transmission_Type = 0x01;
 108         UNS32 abortCode;
 109         UNS8 res;
 110         eprintf("Master : ConfigureSlaveNode %2.2x\n", nodeId);
 111
 112         switch(++init_step){
 113                 case 1: /*First step : setup Slave's TPDO 1 to be transmitted on SYNC*/
 114                         eprintf("Master : set slave %2.2x TPDO 1 transmit type\n", nodeId);
 115                         res = writeNetworkDictCallBack (d, /*CO_Data* d*/
 116                                         nodeId, /*UNS8 nodeId*/
 117                                         0x1800, /*UNS16 index*/
 118                                         0x02, /*UNS8 subindex*/
 119                                         1, /*UNS8 count*/
 120                                         0, /*UNS8 dataType*/
 121                                         &Transmission_Type,/*void *data*/
 122                                         CheckSDOAndContinue); /*SDOCallback_t Callback*/
 123                                         break;
 124
 125                 case 2: /*Second step*/
 126                         eprintf("Master : set slave %2.2x TPDO 2 transmit type\n", nodeId);
 127                         writeNetworkDictCallBack (d, /*CO_Data* d*/
 128                                         nodeId, /*UNS8 nodeId*/
 129                                         0x1801, /*UNS16 index*/
 130                                         0x02, /*UNS16 index*/
 131                                         1, /*UNS8 count*/
 132                                         0, /*UNS8 dataType*/
 133                                         &Transmission_Type,/*void *data*/
 134                                         CheckSDOAndContinue); /*SDOCallback_t Callback*/
 135                                         break;
 136                 case 3:
 137
 138                 /****************************** START *******************************/
 139
 140                         /* Put the master in operational mode */
 141                         setState(d, Operational);
 142
 143                         /* Ask slave node to go in operational mode */
 144                         masterSendNMTstateChange (d, nodeId, NMT_Start_Node);
 145
 146         }
 147 }
 148
 149 #ifdef CO_ENABLE_LSS
 150 static void ConfigureLSSNode(CO_Data* d);
 151 // Step counts number of times ConfigureLSSNode is called
 152 UNS8 init_step_LSS=1;
 153
 154 static void CheckLSSAndContinue(CO_Data* d, UNS8 command)
 155 {
 156         UNS32 dat1;
 157         UNS8 dat2;
 158         printf("CheckLSS->");
 159         if(getConfigResultNetworkNode (d, command, &dat1, &dat2) != LSS_FINISHED){
 160                 if(command==LSS_IDENT_REMOTE_NON_CONF){
 161                         eprintf("Master : There are not no-configured slaves in the net\n", command);
 162                         return;
 163                 }
 164                 else{
 165                         eprintf("Master : Failed in LSS comand %d.  Trying again\n", command);
 166                 }
 167         }
 168         else
 169         {
 170                 init_step_LSS++;
 171
 172                 switch(command){
 173                 case LSS_CONF_NODE_ID:
 174                         switch(dat1){
 175                                 case 0: printf("Node ID change succesful\n");break;
 176                                 case 1: printf("Node ID change error:out of range\n");break;
 177                                 case 0xFF:printf("Node ID change error:specific error\n");break;
 178                                 default:break;
 179                         }
 180                         break;
 181                 case LSS_CONF_BIT_TIMING:
 182                         switch(dat1){
 183                                 case 0: printf("Baud rate change succesful\n");break;
 184                                 case 1: printf("Baud rate change error: change baud rate not supported\n");break;
 185                                 case 0xFF:printf("Baud rate change error:specific error\n");break;
 186                                 default:break;
 187                         }
 188                         break;
 189                 case LSS_CONF_STORE:
 190                         switch(dat1){
 191                                 case 0: printf("Store configuration succesful\n");break;
 192                                 case 1: printf("Store configuration error:not supported\n");break;
 193                                 case 0xFF:printf("Store configuration error:specific error\n");break;
 194                                 default:break;
 195                         }
 196                         break;
 197                 case LSS_SM_SELECTIVE_SERIAL:
 198                         printf("Slave in CONFIGURATION mode\n");
 199                         break;
 200                 case LSS_IDENT_REMOTE_SERIAL_HIGH:
 201                         printf("node identified\n");
 202                         break;
 203                 case LSS_IDENT_REMOTE_NON_CONF:
 204                         printf("non-configured remote slave in the net\n");
 205                         break;
 206                 case LSS_INQ_VENDOR_ID:
 207                         printf("Slave VendorID %x\n", dat1);
 208                         break;
 209                 case LSS_INQ_PRODUCT_CODE:
 210                         printf("Slave Product Code %x\n", dat1);
 211                         break;
 212                 case LSS_INQ_REV_NUMBER:
 213                         printf("Slave Revision Number %x\n", dat1);
 214                         break;
 215                 case LSS_INQ_SERIAL_NUMBER:
 216                         printf("Slave Serial Number %x\n", dat1);
 217                         break;
 218                 case LSS_INQ_NODE_ID:
 219                         printf("Slave nodeid %x\n", dat1);
 220                         break;
 221 #ifdef CO_ENABLE_LSS_FS
 222                 case LSS_IDENT_FASTSCAN:
 223                         if(dat1==0)
 224                                 printf("Slave node identified with FastScan\n");
 225                         else
 226                         {
 227                                 printf("There is not unconfigured node in the net\n");
 228                                 return;
 229                         }
 230                         init_step_LSS++;
 231                         break;
 232 #endif
 233                 }
 234         }
 235
 236         printf("\n");
 237         ConfigureLSSNode(d);
 238 }
 239
 240
 241 /* First ask if there is a node with an invalid nodeID.
 242  * If FastScan is activated it is used to put the node in the state â\80\9cconfigurationâ\80?.
 243  * If FastScan is not activated, identification services are used to identify the node.
 244  * Then  switch mode service is used to put it in configuration state.
 245  * Next all the inquire and configuration services are used.
 246  * Finally, the node LSS state is restored to â\80\9cwaitingâ\80? and all the process is repeated
 247  * again until there isn't any node with a invalid nodeID.
 248  * */
 249 static void ConfigureLSSNode(CO_Data* d)
 250 {
 251         UNS32 Vendor_ID=0x12345678;
 252         UNS32 Product_Code=0x90123456;
 253         UNS32 Revision_Number=0x78901234;
 254         UNS32 Serial_Number=0x56789012;
 255         UNS32 Revision_Number_high=0x78901240;
 256         UNS32 Revision_Number_low=0x78901230;
 257         UNS32 Serial_Number_high=0x56789020;
 258         UNS32 Serial_Number_low=0x56789010;
 259         UNS8 NodeID=0x02;
 260         UNS8 Baud_Table=0;
 261         UNS8 Baud_BitTiming=3;
 262         UNS16 Switch_delay=1;
 263         UNS8 LSS_mode=LSS_WAITING_MODE;
 264         UNS8 res;
 265         eprintf("ConfigureLSSNode -> ",0);
 266
 267         switch(init_step_LSS){
 268                 case 1: /* LSS=>identify non-configured remote slave */
 269                         eprintf("LSS=>identify non-configured remote slave\n");
 270                         res=configNetworkNodeCallBack(&TestMaster_Data,LSS_IDENT_REMOTE_NON_CONF,0,0,CheckLSSAndContinue);
 271                         break;
 272 #ifdef CO_ENABLE_LSS_FS
 273                 case 2: /* LSS=>FastScan */
 274                         eprintf("LSS=>FastScan\n");
 275                         res=configNetworkNodeCallBack(&TestMaster_Data,LSS_IDENT_FASTSCAN,0,0,CheckLSSAndContinue);
 276                         break;
 277 #else
 278                 case 2: /* LSS=>identify node */
 279                         eprintf("LSS=>identify node\n");
 280                         res=configNetworkNode(&TestMaster_Data,LSS_IDENT_REMOTE_VENDOR,&Vendor_ID,0);
 281                         res=configNetworkNode(&TestMaster_Data,LSS_IDENT_REMOTE_PRODUCT,&Product_Code,0);
 282                         res=configNetworkNode(&TestMaster_Data,LSS_IDENT_REMOTE_REV_LOW,&Revision_Number_low,0);
 283                         res=configNetworkNode(&TestMaster_Data,LSS_IDENT_REMOTE_REV_HIGH,&Revision_Number_high,0);
 284                         res=configNetworkNode(&TestMaster_Data,LSS_IDENT_REMOTE_SERIAL_LOW,&Serial_Number_low,0);
 285                         res=configNetworkNodeCallBack(&TestMaster_Data,LSS_IDENT_REMOTE_SERIAL_HIGH,&Serial_Number_high,0,CheckLSSAndContinue);
 286                         break;
 287                 case 3: /*LSS=>put in configuration mode*/
 288                         eprintf("LSS=>put in configuration mode\n");
 289                         res=configNetworkNode(&TestMaster_Data,LSS_SM_SELECTIVE_VENDOR,&Vendor_ID,0);
 290                         res=configNetworkNode(&TestMaster_Data,LSS_SM_SELECTIVE_PRODUCT,&Product_Code,0);
 291                         res=configNetworkNode(&TestMaster_Data,LSS_SM_SELECTIVE_REVISION,&Revision_Number,0);
 292                         res=configNetworkNodeCallBack(&TestMaster_Data,LSS_SM_SELECTIVE_SERIAL,&Serial_Number,0,CheckLSSAndContinue);
 293                         break;
 294 #endif
 295                 case 4: /* LSS=>inquire nodeID */
 296                         eprintf("LSS=>inquire nodeID\n");
 297                         res=configNetworkNodeCallBack(&TestMaster_Data,LSS_INQ_NODE_ID,0,0,CheckLSSAndContinue);
 298                         break;
 299                 case 5: /* LSS=>inquire VendorID */
 300                         eprintf("LSS=>inquire VendorID\n");
 301                         res=configNetworkNodeCallBack(&TestMaster_Data,LSS_INQ_VENDOR_ID,0,0,CheckLSSAndContinue);
 302                         break;
 303                 case 6: /* LSS=>inquire Product code */
 304                         eprintf("LSS=>inquire Product code\n");
 305                         res=configNetworkNodeCallBack(&TestMaster_Data,LSS_INQ_PRODUCT_CODE,0,0,CheckLSSAndContinue);
 306                         break;
 307                 case 7: /* LSS=>inquire Revision Number */
 308                         eprintf("LSS=>inquire Revision Number\n");
 309                         res=configNetworkNodeCallBack(&TestMaster_Data,LSS_INQ_REV_NUMBER,0,0,CheckLSSAndContinue);
 310                         break;
 311                 case 8: /* LSS=>inquire Serial Number */
 312                         eprintf("LSS=>inquire Serial Number\n");
 313                         res=configNetworkNodeCallBack(&TestMaster_Data,LSS_INQ_SERIAL_NUMBER,0,0,CheckLSSAndContinue);
 314                         break;
 315                 case 9: /* LSS=>change the nodeID */
 316                         eprintf("LSS=>change the nodeId\n");
 317                         res=configNetworkNodeCallBack(&TestMaster_Data,LSS_CONF_NODE_ID,&NodeID,0,CheckLSSAndContinue);
 318                         break;
 319                 case 10:        /* LSS=>change the Baud rate */
 320                         eprintf("LSS=>change the Baud rate\n");
 321                         res=configNetworkNodeCallBack(&TestMaster_Data,LSS_CONF_BIT_TIMING,&Baud_Table,&Baud_BitTiming,CheckLSSAndContinue);
 322                         break;
 323                 case 11:
 324                         eprintf("LSS=>Activate Bit Timing\n");
 325                         res=configNetworkNode(&TestMaster_Data,LSS_CONF_ACT_BIT_TIMING,&Switch_delay,0);
 326                         /*no break;*/
 327                         init_step_LSS++;
 328                 case 12:
 329                         /*LSS=>store configuration*/
 330                         /* It will fail the first time (time out) due to the switch delay */
 331                         /* It will fail the second time because it is not implemented in the slave */
 332                         eprintf("LSS=>store configuration\n");
 333                         res=configNetworkNodeCallBack(&TestMaster_Data,LSS_CONF_STORE,0,0,CheckLSSAndContinue);
 334                         break;
 335                 case 13: /* LSS=>put in operation mod */
 336                         eprintf("LSS=>put in operation mode\n");
 337                         res=configNetworkNode(&TestMaster_Data,LSS_SM_GLOBAL,&LSS_mode,0);
 338                         /* Search again for not-configured slaves*/
 339                         eprintf("LSS=>identify not-configured remote slave\n");
 340                         res=configNetworkNodeCallBack(&TestMaster_Data,LSS_IDENT_REMOTE_NON_CONF,0,0,CheckLSSAndContinue);
 341                         init_step_LSS=1;
 342                         break;
 343         }
 344 }
 345 #endif
 346
 347 void TestMaster_preOperational(CO_Data* d)
 348 {
 349         eprintf("TestMaster_preOperational\n");
 350 #ifdef CO_ENABLE_LSS
 351         /* Ask slave node to go in stop mode */
 352         masterSendNMTstateChange (&TestMaster_Data, 0, NMT_Stop_Node);
 353         ConfigureLSSNode(&TestMaster_Data);
 354 #endif
 355 }
 356
 357 void TestMaster_operational(CO_Data* d)
 358 {
 359         eprintf("TestMaster_operational\n");
 360 }
 361
 362 void TestMaster_stopped(CO_Data* d)
 363 {
 364         eprintf("TestMaster_stopped\n");
 365 }
 366
 367 void TestMaster_post_sync(CO_Data* d)
 368 {
 369         eprintf("TestMaster_post_sync\n");
 370         eprintf("Master: %d %d %d %d %d %d %d %d %d %x %x %d %d\n",
 371                 MasterMap1,
 372                 MasterMap2,
 373                 MasterMap3,
 374                 MasterMap4,
 375                 MasterMap5,
 376                 MasterMap6,
 377                 MasterMap7,
 378                 MasterMap8,
 379                 MasterMap9,
 380                 MasterMap10,
 381                 MasterMap11,
 382                 MasterMap12,
 383                 MasterMap13);
 384 }
 385
 386 void TestMaster_post_emcy(CO_Data* d, UNS8 nodeID, UNS16 errCode, UNS8 errReg)
 387 {
 388         eprintf("Master received EMCY message. Node: %2.2x  ErrorCode: %4.4x  ErrorRegister: %2.2x\n", nodeID, errCode, errReg);
 389 }
 390
 391 char query_result = 0;
 392 char waiting_answer = 0;
 393
 394 static void CheckSDO(CO_Data* d, UNS8 nodeId)
 395 {
 396         UNS32 abortCode;
 397         if(getWriteResultNetworkDict (d, nodeId, &abortCode) != SDO_FINISHED)
 398                 eprintf("Master : Failed in changing Slave's transmit type AbortCode :%4.4x \n", abortCode);
 399
 400         /* Finalise last SDO transfer with this node */
 401         closeSDOtransfer(&TestMaster_Data, nodeId, SDO_CLIENT);
 402 }
 403
 404
 405 static int MasterSyncCount = 0;
 406 void TestMaster_post_TPDO(CO_Data* d)
 407 {
 408         eprintf("TestMaster_post_TPDO MasterSyncCount = %d \n", MasterSyncCount);
 409 //
 410 //      {
 411 //              char zero = 0;
 412 //              if(MasterMap4 > 0x80){
 413 //                      writeNetworkDict (
 414 //                              &TestMaster_Data,
 415 //                              TestSlave_Data->bDeviceNodeId,
 416 //                              0x2002,
 417 //                              0x00,
 418 //                              1,
 419 //                              0,
 420 //                              &zero);
 421 //              }
 422 //      }
 423
 424 #if 0
 425         if(waiting_answer){
 426                 UNS32 abortCode;
 427                 UNS8 size;
 428                 switch(getReadResultNetworkDict (
 429                         &TestMaster_Data,
 430                         0x02,
 431                         &query_result,
 432                         &size,
 433                         &abortCode))
 434                 {
 435                         case SDO_FINISHED:
 436                                 /* Do something with result here !!*/
 437                                 eprintf("Got SDO answer (0x2002, 0x00), %d %d\n",query_result,size);
 438                         case SDO_ABORTED_RCV:
 439                         case SDO_ABORTED_INTERNAL:
 440                         case SDO_RESET:
 441                                 waiting_answer = 0;
 442                                 closeSDOtransfer(
 443                                         &TestMaster_Data,
 444                                         0x02,
 445                                         SDO_CLIENT);
 446                         break;
 447                         case SDO_DOWNLOAD_IN_PROGRESS:
 448                         case SDO_UPLOAD_IN_PROGRESS:
 449                         break;
 450                 }
 451         }else if(MasterSyncCount % 10 == 0){
 452                 readNetworkDict (
 453                         &TestMaster_Data,
 454                         0x02,
 455                         0x2002,
 456                         0x00,
 457                         0);
 458                 waiting_answer = 1;
 459         }
 460 #endif
 461         if(MasterSyncCount % 17 == 0){
 462                 eprintf("Master : Ask RTR PDO (0x1402)\n");
 463                 sendPDOrequest(&TestMaster_Data, 0x1402 );
 464                 sendPDOrequest(&TestMaster_Data, 0x1403 );
 465         }
 466         if(MasterSyncCount % 50 == 0){
 467                 eprintf("Master : Change slave's transmit type to 0xFF\n");
 468                 UNS8 transmitiontype = 0xFF;
 469                 writeNetworkDictCallBack (&TestMaster_Data, /*CO_Data* d*/
 470                                         2, /*UNS8 nodeId*/
 471                                         0x1802, /*UNS16 index*/
 472                                         0x02, /*UNS16 index*/
 473                                         1, /*UNS8 count*/
 474                                         0, /*UNS8 dataType*/
 475                                         &transmitiontype,/*void *data*/
 476                                         CheckSDO); /*SDOCallback_t Callback*/
 477         }
 478         if(MasterSyncCount % 50 == 25){
 479                 eprintf("Master : Change slave's transmit type to 0x00\n");
 480                 UNS8 transmitiontype = 0x00;
 481                 writeNetworkDictCallBack (&TestMaster_Data, /*CO_Data* d*/
 482                                         2, /*UNS8 nodeId*/
 483                                         0x1802, /*UNS16 index*/
 484                                         0x02, /*UNS16 index*/
 485                                         1, /*UNS8 count*/
 486                                         0, /*UNS8 dataType*/
 487                                         &transmitiontype,/*void *data*/
 488                                         CheckSDO); /*SDOCallback_t Callback*/
 489         }
 490         MasterSyncCount++;
 491 }
 492
 493 void TestMaster_post_SlaveBootup(CO_Data* d, UNS8 nodeid)
 494 {
 495         eprintf("TestMaster_post_SlaveBootup %x\n", nodeid);
 496
 497         ConfigureSlaveNode(&TestMaster_Data, nodeid);
 498 }
 499