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 #ifdef CO_ENABLE_LSS
 147 static void ConfigureLSSNode(CO_Data* d);
 148 UNS8 init_step_LSS=1;
 149
 150 static void CheckLSSAndContinue(CO_Data* d, UNS8 command)
 151 {
 152         UNS32 dat1;
 153         UNS8 dat2;
 154         printf("CheckLSS->");
 155         if(getConfigResultNetworkNode (d, command, &dat1, &dat2) != LSS_FINISHED){
 156                 if(command==LSS_IDENT_NON_CONF_SLAVE){
 157                         eprintf("Master : There are not no-configured slaves in the net\n", command);
 158                         return;
 159                 }
 160                 else{
 161                         eprintf("Master : Failed in LSS comand %d.  Trying again\n", command);
 162                 }
 163         }
 164         else
 165         {
 166                 init_step_LSS++;
 167
 168                 switch(command){
 169                 case LSS_CONF_NODE_ID:
 170                         switch(dat1){
 171                                 case 0: printf("Node ID change succesful\n");break;
 172                                 case 1: printf("Node ID change error:out of range\n");break;
 173                                 case 0xFF:printf("Node ID change error:specific error\n");break;
 174                                 default:break;
 175                         }
 176                         break;
 177                 case LSS_CONF_BIT_TIMING:
 178                         switch(dat1){
 179                                 case 0: printf("Baud rate change succesful\n");break;
 180                                 case 1: printf("Baud rate change error: change baud rate not supported\n");break;
 181                                 case 0xFF:printf("Baud rate change error:specific error\n");break;
 182                                 default:break;
 183                         }
 184                         break;
 185                 case LSS_CONF_STORE:
 186                         switch(dat1){
 187                                 case 0: printf("Store configuration succesful\n");break;
 188                                 case 1: printf("Store configuration error:not supported\n");break;
 189                                 case 0xFF:printf("Store configuration error:specific error\n");break;
 190                                 default:break;
 191                         }
 192                         break;
 193                 case LSS_SM_SELECTIVE_RESP:
 194                         printf("Slave in CONFIGURATION mode\n");
 195                         break;
 196                 case LSS_IDENT_SLAVE:
 197                         printf("node identified\n");
 198                         break;
 199                 case LSS_IDENT_NON_CONF_SLAVE:
 200                         printf("non-configured remote slave in the net\n");
 201                         break;
 202                 case LSS_INQ_VENDOR_ID:
 203                         printf("Slave VendorID %x\n", dat1);
 204                         break;
 205                 case LSS_INQ_PRODUCT_CODE:
 206                         printf("Slave Product Code %x\n", dat1);
 207                         break;
 208                 case LSS_INQ_REV_NUMBER:
 209                         printf("Slave Revision Number %x\n", dat1);
 210                         break;
 211                 case LSS_INQ_SERIAL_NUMBER:
 212                         printf("Slave Serial Number %x\n", dat1);
 213                         break;
 214                 case LSS_INQ_NODE_ID:
 215                         printf("Slave nodeid %x\n", dat1);
 216                         break;
 217                 }
 218         }
 219
 220         printf("\n");
 221         ConfigureLSSNode(d);
 222 }
 223
 224
 225 struct timeval master_prev_time,master_current_time;
 226 static void ConfigureLSSNode(CO_Data* d)
 227 {
 228         UNS32 Vendor_ID=0x12345678;
 229         UNS32 Product_Code=0x90123456;
 230         UNS32 Revision_Number=0x78901234;
 231         UNS32 Serial_Number=0x56789012;
 232         UNS32 Revision_Number_high=0x78901240;
 233         UNS32 Revision_Number_low=0x78901230;
 234         UNS32 Serial_Number_high=0x56789020;
 235         UNS32 Serial_Number_low=0x56789010;
 236         UNS8 NodeID=0x02;
 237         UNS8 Baud_Table=0;
 238         UNS8 Baud_BitTiming=3;
 239         UNS16 Switch_delay=1;
 240         UNS8 LSS_mode=LSS_WAITING_MODE;
 241         UNS8 res;
 242         eprintf("ConfigureLSSNode-> ");
 243
 244         switch(init_step_LSS){
 245                 case 1: /* LSS=>identify non-configured remote slave */
 246                         eprintf("LSS=>identify non-configured remote slave\n");
 247                         res=configNetworkNodeCallBack(&TestMaster_Data,LSS_IDENT_REMOTE_NON_CONF,0,0,CheckLSSAndContinue);
 248                         break;
 249                 case 2: /* LSS=>identify node */
 250                         eprintf("LSS=>identify node\n");
 251                         res=configNetworkNode(&TestMaster_Data,LSS_IDENT_REMOTE_VENDOR,&Vendor_ID,0);
 252                         res=configNetworkNode(&TestMaster_Data,LSS_IDENT_REMOTE_PRODUCT,&Product_Code,0);
 253                         res=configNetworkNode(&TestMaster_Data,LSS_IDENT_REMOTE_REV_LOW,&Revision_Number_low,0);
 254                         res=configNetworkNode(&TestMaster_Data,LSS_IDENT_REMOTE_REV_HIGH,&Revision_Number_high,0);
 255                         res=configNetworkNode(&TestMaster_Data,LSS_IDENT_REMOTE_SERIAL_LOW,&Serial_Number_low,0);
 256                         res=configNetworkNodeCallBack(&TestMaster_Data,LSS_IDENT_REMOTE_SERIAL_HIGH,&Serial_Number_high,0,CheckLSSAndContinue);
 257                         break;
 258                 case 3: /*First step : setup Slave's TPDO 1 to be transmitted on SYNC*/
 259                         eprintf("LSS=>put in configuration mode\n");
 260                         res=configNetworkNode(&TestMaster_Data,LSS_SM_SELECTIVE_VENDOR,&Vendor_ID,0);
 261                         res=configNetworkNode(&TestMaster_Data,LSS_SM_SELECTIVE_PRODUCT,&Product_Code,0);
 262                         res=configNetworkNode(&TestMaster_Data,LSS_SM_SELECTIVE_REVISION,&Revision_Number,0);
 263                         res=configNetworkNodeCallBack(&TestMaster_Data,LSS_SM_SELECTIVE_SERIAL,&Serial_Number,0,CheckLSSAndContinue);
 264                         break;
 265                 case 4: /* LSS=>inquire nodeID */
 266                         eprintf("LSS=>inquire nodeID\n");
 267                         res=configNetworkNodeCallBack(&TestMaster_Data,LSS_INQ_NODE_ID,0,0,CheckLSSAndContinue);
 268                         break;
 269                 case 5: /* LSS=>inquire VendorID */
 270                         eprintf("LSS=>inquire VendorID\n");
 271                         res=configNetworkNodeCallBack(&TestMaster_Data,LSS_INQ_VENDOR_ID,0,0,CheckLSSAndContinue);
 272                         break;
 273                 case 6: /* LSS=>inquire Product code */
 274                         eprintf("LSS=>inquire Product code\n");
 275                         res=configNetworkNodeCallBack(&TestMaster_Data,LSS_INQ_PRODUCT_CODE,0,0,CheckLSSAndContinue);
 276                         break;
 277                 case 7: /* LSS=>inquire Revision Number */
 278                         eprintf("LSS=>inquire Revision Number\n");
 279                         res=configNetworkNodeCallBack(&TestMaster_Data,LSS_INQ_REV_NUMBER,0,0,CheckLSSAndContinue);
 280                         break;
 281                 case 8: /* LSS=>inquire Serial Number */
 282                         eprintf("LSS=>inquire Serial Number\n");
 283                         res=configNetworkNodeCallBack(&TestMaster_Data,LSS_INQ_SERIAL_NUMBER,0,0,CheckLSSAndContinue);
 284                         break;
 285                 case 9: /* LSS=>change the nodeID */
 286                         eprintf("LSS=>change the nodeId\n");
 287                         res=configNetworkNodeCallBack(&TestMaster_Data,LSS_CONF_NODE_ID,&NodeID,0,CheckLSSAndContinue);
 288                         break;
 289                 case 10:        /* LSS=>change the Baud rate */
 290                         eprintf("LSS=>change the Baud rate\n");
 291                         res=configNetworkNodeCallBack(&TestMaster_Data,LSS_CONF_BIT_TIMING,&Baud_Table,&Baud_BitTiming,CheckLSSAndContinue);
 292                         break;
 293                 case 11:
 294                         eprintf("LSS=>Activate Bit Timing\n");
 295                         res=configNetworkNode(&TestMaster_Data,LSS_CONF_ACT_BIT_TIMING,&Switch_delay,0);
 296                         /*no break;*/
 297                         init_step_LSS++;
 298                 case 12:
 299                         /*LSS=>store configuration*/
 300                         /* It will fail the first time (time out) due to the switch delay */
 301                         /* It will fail the second time because it is not implemented in the slave */
 302                         eprintf("LSS=>store configuration\n");
 303                         res=configNetworkNodeCallBack(&TestMaster_Data,LSS_CONF_STORE,0,0,CheckLSSAndContinue);
 304                         break;
 305                 case 13: /* LSS=>put in operation mod */
 306                         eprintf("LSS=>put in operation mode\n");
 307                         res=configNetworkNode(&TestMaster_Data,LSS_SM_GLOBAL,&LSS_mode,0);
 308                         break;
 309         }
 310 }
 311 #endif
 312
 313 void TestMaster_preOperational()
 314 {
 315         eprintf("TestMaster_preOperational\n");
 316 #ifdef CO_ENABLE_LSS
 317         /* Ask slave node to go in stop mode */
 318         masterSendNMTstateChange (&TestMaster_Data, 0, NMT_Stop_Node);
 319         ConfigureLSSNode(&TestMaster_Data);
 320 #endif
 321 }
 322
 323 void TestMaster_operational()
 324 {
 325         eprintf("TestMaster_operational\n");
 326 }
 327
 328 void TestMaster_stopped()
 329 {
 330         eprintf("TestMaster_stopped\n");
 331 }
 332
 333 void TestMaster_post_sync()
 334 {
 335         eprintf("TestMaster_post_sync\n");
 336         eprintf("Master: %d %d %d %d %d %d %d %d %d %x %x %d %d\n",
 337                 MasterMap1,
 338                 MasterMap2,
 339                 MasterMap3,
 340                 MasterMap4,
 341                 MasterMap5,
 342                 MasterMap6,
 343                 MasterMap7,
 344                 MasterMap8,
 345                 MasterMap9,
 346                 MasterMap10,
 347                 MasterMap11,
 348                 MasterMap12,
 349                 MasterMap13);
 350 }
 351
 352 void TestMaster_post_emcy(UNS8 nodeID, UNS16 errCode, UNS8 errReg)
 353 {
 354         eprintf("Master received EMCY message. Node: %2.2x  ErrorCode: %4.4x  ErrorRegister: %2.2x\n", nodeID, errCode, errReg);
 355 }
 356
 357 char query_result = 0;
 358 char waiting_answer = 0;
 359
 360 static void CheckSDO(CO_Data* d, UNS8 nodeId)
 361 {
 362         UNS32 abortCode;
 363         if(getWriteResultNetworkDict (d, nodeId, &abortCode) != SDO_FINISHED)
 364                 eprintf("Master : Failed in changing Slave's transmit type AbortCode :%4.4x \n", abortCode);
 365
 366         /* Finalise last SDO transfer with this node */
 367         closeSDOtransfer(&TestMaster_Data, nodeId, SDO_CLIENT);
 368 }
 369
 370
 371 static int MasterSyncCount = 0;
 372 void TestMaster_post_TPDO()
 373 {
 374         eprintf("TestMaster_post_TPDO MasterSyncCount = %d \n", MasterSyncCount);
 375 //
 376 //      {
 377 //              char zero = 0;
 378 //              if(MasterMap4 > 0x80){
 379 //                      writeNetworkDict (
 380 //                              &TestMaster_Data,
 381 //                              TestSlave_Data->bDeviceNodeId,
 382 //                              0x2002,
 383 //                              0x00,
 384 //                              1,
 385 //                              0,
 386 //                              &zero);
 387 //              }
 388 //      }
 389
 390 #if 0
 391         if(waiting_answer){
 392                 UNS32 abortCode;
 393                 UNS8 size;
 394                 switch(getReadResultNetworkDict (
 395                         &TestMaster_Data,
 396                         0x02,
 397                         &query_result,
 398                         &size,
 399                         &abortCode))
 400                 {
 401                         case SDO_FINISHED:
 402                                 /* Do something with result here !!*/
 403                                 eprintf("Got SDO answer (0x2002, 0x00), %d %d\n",query_result,size);
 404                         case SDO_ABORTED_RCV:
 405                         case SDO_ABORTED_INTERNAL:
 406                         case SDO_RESET:
 407                                 waiting_answer = 0;
 408                                 closeSDOtransfer(
 409                                         &TestMaster_Data,
 410                                         0x02,
 411                                         SDO_CLIENT);
 412                         break;
 413                         case SDO_DOWNLOAD_IN_PROGRESS:
 414                         case SDO_UPLOAD_IN_PROGRESS:
 415                         break;
 416                 }
 417         }else if(MasterSyncCount % 10 == 0){
 418                 readNetworkDict (
 419                         &TestMaster_Data,
 420                         0x02,
 421                         0x2002,
 422                         0x00,
 423                         0);
 424                 waiting_answer = 1;
 425         }
 426 #endif
 427         if(MasterSyncCount % 17 == 0){
 428                 eprintf("Master : Ask RTR PDO (0x1402)\n");
 429                 sendPDOrequest(&TestMaster_Data, 0x1402 );
 430                 sendPDOrequest(&TestMaster_Data, 0x1403 );
 431         }
 432         if(MasterSyncCount % 50 == 0){
 433                 eprintf("Master : Change slave's transmit type to 0xFF\n");
 434                 UNS8 transmitiontype = 0xFF;
 435                 writeNetworkDictCallBack (&TestMaster_Data, /*CO_Data* d*/
 436                                         2, /*UNS8 nodeId*/
 437                                         0x1802, /*UNS16 index*/
 438                                         0x02, /*UNS16 index*/
 439                                         1, /*UNS8 count*/
 440                                         0, /*UNS8 dataType*/
 441                                         &transmitiontype,/*void *data*/
 442                                         CheckSDO); /*SDOCallback_t Callback*/
 443         }
 444         if(MasterSyncCount % 50 == 25){
 445                 eprintf("Master : Change slave's transmit type to 0x00\n");
 446                 UNS8 transmitiontype = 0x00;
 447                 writeNetworkDictCallBack (&TestMaster_Data, /*CO_Data* d*/
 448                                         2, /*UNS8 nodeId*/
 449                                         0x1802, /*UNS16 index*/
 450                                         0x02, /*UNS16 index*/
 451                                         1, /*UNS8 count*/
 452                                         0, /*UNS8 dataType*/
 453                                         &transmitiontype,/*void *data*/
 454                                         CheckSDO); /*SDOCallback_t Callback*/
 455         }
 456         MasterSyncCount++;
 457 }
 458
 459 void TestMaster_post_SlaveBootup(UNS8 nodeid)
 460 {
 461         eprintf("TestMaster_post_SlaveBootup %x\n", nodeid);
 462         ConfigureSlaveNode(&TestMaster_Data, nodeid);
 463 }
 464