examples/TestMasterSlaveLSS/Master.c

   1 /*
   2 This file is part of CanFestival, a library implementing CanOpen Stack.
   3
   4 Copyright (C): Edouard TISSERANT , Francis DUPIN and Jorge BERZOSA
   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 "SlaveA.h"
  25 #include "SlaveB.h"
  26 #include "TestMasterSlaveLSS.h"
  27
  28 extern s_BOARD MasterBoard;
  29 /*****************************************************************************/
  30 void TestMaster_heartbeatError(CO_Data* d, UNS8 heartbeatID)
  31 {
  32         eprintf("TestMaster_heartbeatError %d\n", heartbeatID);
  33 }
  34
  35 /********************************************************
  36  * TestMaster_initialisation is responsible to
  37  *  - setup master RPDO 1 to receive TPDO 1 from id 2
  38  *  - setup master RPDO 2 to receive TPDO 1 from id 3
  39  ********************************************************/
  40 void TestMaster_initialisation(CO_Data* d)
  41 {
  42         UNS32 PDO1_COBID = 0x0182;
  43         UNS32 PDO2_COBID = 0x0183;
  44         UNS8 size = sizeof(UNS32);
  45
  46         eprintf("TestMaster_initialisation\n");
  47
  48         /*****************************************
  49          * Define RPDO to match slave ID=2 TPDO1*
  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         /*****************************************
  59          * Define RPDO to match slave ID=3 TPDO1*
  60          *****************************************/
  61         writeLocalDict( &TestMaster_Data, /*CO_Data* d*/
  62                         0x1401, /*UNS16 index*/
  63                         0x01, /*UNS8 subind*/
  64                         &PDO2_COBID, /*void * pSourceData,*/
  65                         &size, /* UNS8 * pExpectedSize*/
  66                         RW);  /* UNS8 checkAccess */
  67
  68 }
  69
  70 // Step counts number of times ConfigureSlaveNode is called
  71 // There is one per each slave
  72 static init_step[] ={0,0};
  73
  74 /*Forward declaration*/
  75 static void ConfigureSlaveNode(CO_Data* d, UNS8 nodeId);
  76
  77 static void CheckSDOAndContinue(CO_Data* d, UNS8 nodeId)
  78 {
  79         UNS32 abortCode;
  80         if(getWriteResultNetworkDict (d, nodeId, &abortCode) != SDO_FINISHED)
  81                 eprintf("Master : Failed in initializing slave %2.2x, step %d, AbortCode :%4.4x \n", nodeId, init_step, abortCode);
  82
  83         /* Finalise last SDO transfer with this node */
  84         closeSDOtransfer(&TestMaster_Data, nodeId, SDO_CLIENT);
  85
  86         ConfigureSlaveNode(d, nodeId);
  87 }
  88
  89 /********************************************************
  90  * ConfigureSlaveNode is responsible to
  91  *  - setup slave 'n' TPDO 1 transmit type
  92  *  - setup slave 'n' Producer Hertbeat Time
  93  *  - setup the Consumer Heartbeat Time for slave 'n'
  94  *  - switch to operational mode
  95  *  - send NMT to slave
  96  ********************************************************
  97  * This an example of :
  98  * Network Dictionary Access (SDO) with Callback
  99  * Slave node state change request (NMT)
 100  ********************************************************
 101  * This is called first by TestMaster_post_SlaveBootup
 102  * after the LSS configuration has been done
 103  * then it called again each time a SDO exchange is
 104  * finished.
 105  ********************************************************/
 106
 107 static void ConfigureSlaveNode(CO_Data* d, UNS8 nodeId)
 108 {
 109         /* Master configure heartbeat producer time at 0 ms
 110          * for slaves node-id 0x02 and 0x03 by DCF concise */
 111
 112         UNS8 Transmission_Type = 0x01;
 113         UNS16 Slave_Prod_Heartbeat_T=1000;//ms
 114         UNS32 Master_Cons_Heartbeat_Base=0x05DC; //1500ms
 115         UNS32 abortCode;
 116         UNS8 res;
 117         eprintf("Master : ConfigureSlaveNode %2.2x\n", nodeId);
 118
 119         switch(++init_step[nodeId-2]){
 120                 case 1: /*First step : setup Slave's TPDO 1 to be transmitted on SYNC*/
 121                         eprintf("Master : set slave %2.2x TPDO 1 transmit type\n", nodeId);
 122                         res = writeNetworkDictCallBack (d, /*CO_Data* d*/
 123                                         nodeId, /*UNS8 nodeId*/
 124                                         0x1800, /*UNS16 index*/
 125                                         0x02, /*UNS8 subindex*/
 126                                         1, /*UNS8 count*/
 127                                         0, /*UNS8 dataType*/
 128                                         &Transmission_Type,/*void *data*/
 129                                         CheckSDOAndContinue); /*SDOCallback_t Callback*/
 130                                         break;
 131                 case 2: /* Second step : Set the new heartbeat producer time in the slave */
 132                 {
 133                         UNS32 Master_Cons_Heartbeat_T=Master_Cons_Heartbeat_Base + (nodeId * 0x10000);
 134                         UNS8 size = sizeof(UNS32);
 135
 136                         eprintf("Master : set slave %2.2x Producer Heartbeat Time = %d\n", nodeId,Slave_Prod_Heartbeat_T);
 137                         res = writeNetworkDictCallBack (d, /*CO_Data* d*/
 138                                         nodeId, /*UNS8 nodeId*/
 139                                         0x1017, /*UNS16 index*/
 140                                         0x00, /*UNS8 subindex*/
 141                                         2, /*UNS8 count*/
 142                                         0, /*UNS8 dataType*/
 143                                         &Slave_Prod_Heartbeat_T,/*void *data*/
 144                                         CheckSDOAndContinue); /*SDOCallback_t Callback*/
 145                                         break;
 146
 147                         /* Set the new heartbeat consumer time in the master*/
 148                         eprintf("Master : set Consumer Heartbeat Time for slave %2.2x = %d\n", nodeId,Master_Cons_Heartbeat_T);
 149                         writeLocalDict( &TestMaster_Data, /*CO_Data* d*/
 150                                 0x1016, /*UNS16 index*/
 151                                 nodeId-1, /*UNS8 subind*/
 152                                 &Master_Cons_Heartbeat_T, /*void * pSourceData,*/
 153                                 &size, /* UNS8 * pExpectedSize*/
 154                                 RW);  /* UNS8 checkAccess */
 155                 }
 156                 break;
 157                 case 3:
 158
 159                 /****************************** START *******************************/
 160
 161                         /* Put the master in operational mode */
 162                         setState(d, Operational);
 163
 164                         /* Ask slave node to go in operational mode */
 165                         masterSendNMTstateChange (d, nodeId, NMT_Start_Node);
 166         }
 167 }
 168
 169 static void ConfigureLSSNode(CO_Data* d);
 170 // Step counts number of times ConfigureLSSNode is called
 171 UNS8 init_step_LSS=1;
 172
 173 static void CheckLSSAndContinue(CO_Data* d, UNS8 command)
 174 {
 175         UNS32 dat1;
 176         UNS8 dat2;
 177
 178         printf("CheckLSS->");
 179         if(getConfigResultNetworkNode (d, command, &dat1, &dat2) != LSS_FINISHED){
 180                         eprintf("Master : Failed in LSS comand %d.  Trying again\n", command);
 181         }
 182         else
 183         {
 184                 init_step_LSS++;
 185
 186                 switch(command){
 187                 case LSS_CONF_NODE_ID:
 188                         switch(dat1){
 189                                 case 0: printf("Node ID change succesful\n");break;
 190                                 case 1: printf("Node ID change error:out of range\n");break;
 191                                 case 0xFF:printf("Node ID change error:specific error\n");break;
 192                                 default:break;
 193                         }
 194                         break;
 195                 case LSS_CONF_BIT_TIMING:
 196                         switch(dat1){
 197                                 case 0: printf("Baud rate change succesful\n");break;
 198                                 case 1: printf("Baud rate change error: change baud rate not supported\n");break;
 199                                 case 0xFF:printf("Baud rate change error:specific error\n");break;
 200                                 default:break;
 201                         }
 202                         break;
 203                 case LSS_CONF_STORE:
 204                         switch(dat1){
 205                                 case 0: printf("Store configuration succesful\n");break;
 206                                 case 1: printf("Store configuration error:not supported\n");break;
 207                                 case 0xFF:printf("Store configuration error:specific error\n");break;
 208                                 default:break;
 209                         }
 210                         break;
 211                 case LSS_CONF_ACT_BIT_TIMING:
 212                         if(dat1==0){
 213                                 UNS8 LSS_mode=LSS_WAITING_MODE;
 214                                 UNS32 SINC_cicle=50000;// us
 215                                 UNS8 size = sizeof(UNS32);
 216
 217                                 /* The slaves are now configured (nodeId and Baudrate) via the LSS services.
 218                                 * Switch the LSS state to WAITING and restart the slaves. */
 219                                 MasterBoard.baudrate="250K";
 220
 221                                 printf("Master : Switch Delay period finished. Switching to LSS WAITING state\n");
 222                                 configNetworkNode(d,LSS_SM_GLOBAL,&LSS_mode,0,NULL);
 223
 224                                 printf("Master : Restarting all the slaves\n");
 225                                 masterSendNMTstateChange (d, 0x00, NMT_Reset_Node);
 226
 227                                 printf("Master : Starting the SYNC producer\n");
 228                                 writeLocalDict( d, /*CO_Data* d*/
 229                                         0x1006, /*UNS16 index*/
 230                                         0x00, /*UNS8 subind*/
 231                                         &SINC_cicle, /*void * pSourceData,*/
 232                                         &size, /* UNS8 * pExpectedSize*/
 233                                         RW);  /* UNS8 checkAccess */
 234
 235                                 return;
 236                         }
 237                         else{
 238                                 UNS16 Switch_delay=1;
 239                                 UNS8 LSS_mode=LSS_CONFIGURATION_MODE;
 240
 241                                 eprintf("Master : unable to activate bit timing. trying again\n");
 242                                 configNetworkNode(d,LSS_CONF_ACT_BIT_TIMING,&Switch_delay,0,CheckLSSAndContinue);
 243                                 return;
 244                         }
 245                         break;
 246                 case LSS_SM_SELECTIVE_SERIAL:
 247                         printf("Slave in LSS CONFIGURATION state\n");
 248                         break;
 249                 case LSS_IDENT_REMOTE_SERIAL_HIGH:
 250                         printf("node identified\n");
 251                         break;
 252                 case LSS_IDENT_REMOTE_NON_CONF:
 253                         if(dat1==0)
 254                                 eprintf("There are no-configured remote slave(s) in the net\n");
 255                         else
 256                         {
 257                                 UNS16 Switch_delay=1;
 258                                 UNS8 LSS_mode=LSS_CONFIGURATION_MODE;
 259
 260                                 /*The configuration of the slaves' nodeId ended.
 261                                  * Start the configuration of the baud rate. */
 262                                 eprintf("Master : There are not no-configured slaves in the net\n", command);
 263                                 eprintf("Switching all the nodes to LSS CONFIGURATION state\n");
 264                                 configNetworkNode(d,LSS_SM_GLOBAL,&LSS_mode,0,NULL);
 265                                 eprintf("LSS=>Activate Bit Timing\n");
 266                                 configNetworkNode(d,LSS_CONF_ACT_BIT_TIMING,&Switch_delay,0,CheckLSSAndContinue);
 267                                 return;
 268                         }
 269                         break;
 270                 case LSS_INQ_VENDOR_ID:
 271                         printf("Slave VendorID %x\n", dat1);
 272                         break;
 273                 case LSS_INQ_PRODUCT_CODE:
 274                         printf("Slave Product Code %x\n", dat1);
 275                         break;
 276                 case LSS_INQ_REV_NUMBER:
 277                         printf("Slave Revision Number %x\n", dat1);
 278                         break;
 279                 case LSS_INQ_SERIAL_NUMBER:
 280                         printf("Slave Serial Number %x\n", dat1);
 281                         break;
 282                 case LSS_INQ_NODE_ID:
 283                         printf("Slave nodeid %x\n", dat1);
 284                         break;
 285 #ifdef CO_ENABLE_LSS_FS
 286                 case LSS_IDENT_FASTSCAN:
 287                         if(dat1==0)
 288                                 printf("Slave node identified with FastScan\n");
 289                         else
 290                         {
 291                                 printf("There is not unconfigured node in the net\n");
 292                                 return;
 293                         }
 294                         init_step_LSS++;
 295                         break;
 296 #endif
 297
 298                 }
 299         }
 300
 301         printf("\n");
 302         ConfigureLSSNode(d);
 303 }
 304
 305 /* Initial nodeID and VendorID. They are incremented by one for each slave*/
 306 UNS8 NodeID=0x02;
 307 UNS32 Vendor_ID=0x12345678;
 308
 309 /* Configuration of the nodeID and baudrate with LSS services:
 310  * --First ask if there is a node with an invalid nodeID.
 311  * --If FastScan is activated it is used to put the slave in the state “configuration”.
 312  * --If FastScan is not activated, identification services are used to identify the slave. Then
 313  *       switch mode service is used to put it in configuration state.
 314  * --Next, all the inquire services are used (only for example) and a valid nodeId and a
 315  *       new baudrate are assigned to the slave.
 316  * --Finally, the slave's LSS state is restored to “waiting” and all the process is repeated
 317  *       again until there isn't any node with an invalid nodeID.
 318  * --After the configuration of all the slaves finished the LSS state of all of them is switched
 319  *       again to "configuration" and the Activate Bit Timing service is requested. On sucessfull, the
 320  *       LSS state is restored to "waiting" and NMT state is changed to reset (by means of the NMT services).
 321  * */
 322 static void ConfigureLSSNode(CO_Data* d)
 323 {
 324         UNS32 Product_Code=0x90123456;
 325         UNS32 Revision_Number=0x78901234;
 326         UNS32 Serial_Number=0x56789012;
 327         UNS32 Revision_Number_high=0x78901240;
 328         UNS32 Revision_Number_low=0x78901230;
 329         UNS32 Serial_Number_high=0x56789020;
 330         UNS32 Serial_Number_low=0x56789010;
 331         UNS8 LSS_mode=LSS_WAITING_MODE;
 332         UNS8 Baud_Table=0;
 333         //UNS8 Baud_BitTiming=3;
 334         char* Baud_BitTiming="250K";
 335         UNS8 res;
 336         eprintf("ConfigureLSSNode step %d -> ",init_step_LSS);
 337
 338         switch(init_step_LSS){
 339                 case 1: /* LSS=>identify non-configured remote slave */
 340                         eprintf("LSS=>identify no-configured remote slave(s)\n");
 341                         res=configNetworkNode(&TestMaster_Data,LSS_IDENT_REMOTE_NON_CONF,0,0,CheckLSSAndContinue);
 342                         break;
 343 #ifdef CO_ENABLE_LSS_FS
 344                 case 2: /* LSS=>FastScan */
 345                         eprintf("LSS=>FastScan\n");
 346                         res=configNetworkNode(&TestMaster_Data,LSS_IDENT_FASTSCAN,0,0,CheckLSSAndContinue);
 347                         break;
 348 #else
 349                 case 2: /* LSS=>identify node */
 350                         eprintf("LSS=>identify node\n");
 351                         res=configNetworkNode(&TestMaster_Data,LSS_IDENT_REMOTE_VENDOR,&Vendor_ID,0,NULL);
 352                         res=configNetworkNode(&TestMaster_Data,LSS_IDENT_REMOTE_PRODUCT,&Product_Code,0,NULL);
 353                         res=configNetworkNode(&TestMaster_Data,LSS_IDENT_REMOTE_REV_LOW,&Revision_Number_low,0,NULL);
 354                         res=configNetworkNode(&TestMaster_Data,LSS_IDENT_REMOTE_REV_HIGH,&Revision_Number_high,0,NULL);
 355                         res=configNetworkNode(&TestMaster_Data,LSS_IDENT_REMOTE_SERIAL_LOW,&Serial_Number_low,0,NULL);
 356                         res=configNetworkNode(&TestMaster_Data,LSS_IDENT_REMOTE_SERIAL_HIGH,&Serial_Number_high,0,CheckLSSAndContinue);
 357                         break;
 358                 case 3: /*LSS=>put in configuration mode*/
 359                         eprintf("LSS=>put in configuration mode\n");
 360                         res=configNetworkNode(&TestMaster_Data,LSS_SM_SELECTIVE_VENDOR,&Vendor_ID,0,NULL);
 361                         res=configNetworkNode(&TestMaster_Data,LSS_SM_SELECTIVE_PRODUCT,&Product_Code,0,NULL);
 362                         res=configNetworkNode(&TestMaster_Data,LSS_SM_SELECTIVE_REVISION,&Revision_Number,0,NULL);
 363                         res=configNetworkNode(&TestMaster_Data,LSS_SM_SELECTIVE_SERIAL,&Serial_Number,0,CheckLSSAndContinue);
 364                         Vendor_ID++;
 365                         break;
 366 #endif
 367                 case 4: /* LSS=>inquire nodeID */
 368                         eprintf("LSS=>inquire nodeID\n");
 369                         res=configNetworkNode(&TestMaster_Data,LSS_INQ_NODE_ID,0,0,CheckLSSAndContinue);
 370                         break;
 371                 case 5: /* LSS=>inquire VendorID */
 372                         eprintf("LSS=>inquire VendorID\n");
 373                         res=configNetworkNode(&TestMaster_Data,LSS_INQ_VENDOR_ID,0,0,CheckLSSAndContinue);
 374                         break;
 375                 case 6: /* LSS=>inquire Product code */
 376                         eprintf("LSS=>inquire Product code\n");
 377                         res=configNetworkNode(&TestMaster_Data,LSS_INQ_PRODUCT_CODE,0,0,CheckLSSAndContinue);
 378                         break;
 379                 case 7: /* LSS=>inquire Revision Number */
 380                         eprintf("LSS=>inquire Revision Number\n");
 381                         res=configNetworkNode(&TestMaster_Data,LSS_INQ_REV_NUMBER,0,0,CheckLSSAndContinue);
 382                         break;
 383                 case 8: /* LSS=>inquire Serial Number */
 384                         eprintf("LSS=>inquire Serial Number\n");
 385                         res=configNetworkNode(&TestMaster_Data,LSS_INQ_SERIAL_NUMBER,0,0,CheckLSSAndContinue);
 386                         break;
 387                 case 9: /* LSS=>change the nodeID */
 388                         eprintf("LSS=>change the nodeId\n");
 389                         res=configNetworkNode(&TestMaster_Data,LSS_CONF_NODE_ID,&NodeID,0,CheckLSSAndContinue);
 390                         NodeID++;
 391                         break;
 392                 case 10:        /* LSS=>change the Baud rate */
 393                         eprintf("LSS=>change the Baud rate\n");
 394                         res=configNetworkNode(&TestMaster_Data,LSS_CONF_BIT_TIMING,&Baud_Table,&Baud_BitTiming,CheckLSSAndContinue);
 395                         break;
 396                 case 11:
 397                         /*LSS=>store configuration*/
 398                         eprintf("LSS=>store configuration\n");
 399                         res=configNetworkNode(&TestMaster_Data,LSS_CONF_STORE,0,0,CheckLSSAndContinue);
 400                         break;
 401                 case 12: /* LSS=>put in waiting mode */
 402                         eprintf("LSS=>put in waiting mode\n");
 403                         res=configNetworkNode(&TestMaster_Data,LSS_SM_GLOBAL,&LSS_mode,0,NULL);
 404                         /* Search again for no-configured slaves*/
 405                         eprintf("LSS=>identify no-configured remote slave(s)\n");
 406                         res=configNetworkNode(&TestMaster_Data,LSS_IDENT_REMOTE_NON_CONF,0,0,CheckLSSAndContinue);
 407                         init_step_LSS=1;
 408                         break;
 409         }
 410 }
 411
 412 void TestMaster_preOperational(CO_Data* d)
 413 {
 414         eprintf("TestMaster_preOperational\n");
 415
 416         /* Ask slaves to go in stop mode */
 417         masterSendNMTstateChange (d, 0, NMT_Stop_Node);
 418         ConfigureLSSNode(&TestMaster_Data);
 419 }
 420
 421 void TestMaster_operational(CO_Data* d)
 422 {
 423         eprintf("TestMaster_operational\n");
 424 }
 425
 426 void TestMaster_stopped(CO_Data* d)
 427 {
 428         eprintf("TestMaster_stopped\n");
 429 }
 430
 431 void TestMaster_post_sync(CO_Data* d)
 432 {
 433         eprintf("TestMaster_post_sync\n");
 434         eprintf("Master: %d %d %d\n",
 435                 MasterMap1,
 436                 MasterMap2,
 437                 MasterMap3);
 438 }
 439
 440 void TestMaster_post_emcy(CO_Data* d, UNS8 nodeID, UNS16 errCode, UNS8 errReg)
 441 {
 442         eprintf("Master received EMCY message. Node: %2.2x  ErrorCode: %4.4x  ErrorRegister: %2.2x\n", nodeID, errCode, errReg);
 443 }
 444
 445 void TestMaster_post_TPDO(CO_Data* d)
 446 {
 447         eprintf("TestMaster_post_TPDO\n");
 448 }
 449
 450 void TestMaster_post_SlaveBootup(CO_Data* d, UNS8 nodeid)
 451 {
 452         eprintf("TestMaster_post_SlaveBootup %x\n", nodeid);
 453         /* Wait until the new baud rate is stored before configure the slaves*/
 454         if(MasterBoard.baudrate=="250K")
 455                 ConfigureSlaveNode(d, nodeid);
 456 }
 457