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