examples/TestMasterMicroMod/TestMasterMicroMod.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 #if defined(WIN32) && !defined(__CYGWIN__)
  24 #include <windows.h>
  25 #include "getopt.h"
  26 void pause(void)
  27 {
  28         system("PAUSE");
  29 }
  30 #else
  31 #include <unistd.h>
  32 #include <stdio.h>
  33 #include <string.h>
  34 #include <stdlib.h>
  35 #include <signal.h>
  36 #endif
  37
  38 #include "canfestival.h"
  39 #include "TestMasterMicroMod.h"
  40 #include "TestMaster.h"
  41 unsigned int slavenodeid = 0x40;
  42
  43
  44 /*****************************************************************************/
  45 void TestMaster_heartbeatError(CO_Data* d, UNS8 heartbeatID)
  46 {
  47         eprintf("TestMaster_heartbeatError %d\n", heartbeatID);
  48 }
  49
  50 /********************************************************
  51  * ConfigureSlaveNode is responsible to
  52  *  - setup master RPDO 1 to receive TPDO 1 from id 0x40
  53  *  - setup master TPDO 1 to send RPDO 1 to id 0x40
  54  ********************************************************/
  55 void TestMaster_initialisation(CO_Data* d)
  56 {
  57         UNS32 PDO1_COBID = 0x0180 + slavenodeid;
  58         UNS32 PDO2_COBID = 0x0200 + slavenodeid;
  59         UNS32 size = sizeof(UNS32);
  60
  61         eprintf("TestMaster_initialisation\n");
  62
  63         /*****************************************
  64          * Define RPDOs to match slave ID=0x40 TPDOs*
  65          *****************************************/
  66         writeLocalDict( &TestMaster_Data, /*CO_Data* d*/
  67                         0x1400, /*UNS16 index*/
  68                         0x01, /*UNS8 subind*/
  69                         &PDO1_COBID, /*void * pSourceData,*/
  70                         &size, /* UNS8 * pExpectedSize*/
  71                         RW);  /* UNS8 checkAccess */
  72
  73
  74         /*****************************************
  75          * Define TPDOs to match slave ID=0x40 RPDOs*
  76          *****************************************/
  77         writeLocalDict( &TestMaster_Data, /*CO_Data* d*/
  78                         0x1800, /*UNS16 index*/
  79                         0x01, /*UNS8 subind*/
  80                         &PDO2_COBID, /*void * pSourceData,*/
  81                         &size, /* UNS8 * pExpectedSize*/
  82                         RW);  /* UNS8 checkAccess */
  83 }
  84
  85 static int init_step = 0;
  86
  87 /*Froward declaration*/
  88 static void ConfigureSlaveNode(CO_Data* d, UNS8 nodeId);
  89
  90 /**/
  91 static void CheckSDOAndContinue(CO_Data* d, UNS8 nodeId)
  92 {
  93         UNS32 abortCode;
  94
  95         if(getWriteResultNetworkDict (d, nodeId, &abortCode) != SDO_FINISHED)
  96                 eprintf("Master : Failed in initializing slave %2.2x, step %d, AbortCode :%4.4x \n", nodeId, init_step, abortCode);
  97
  98         /* Finalise last SDO transfer with this node */
  99         closeSDOtransfer(&TestMaster_Data, nodeId, SDO_CLIENT);
 100
 101         ConfigureSlaveNode(d, nodeId);
 102 }
 103
 104 /********************************************************
 105  * ConfigureSlaveNode is responsible to
 106  *  - setup slave TPDO 1 transmit time
 107  *  - setup slave TPDO 2 transmit time
 108  *  - setup slave Heartbeat Producer time
 109  *  - switch to operational mode
 110  *  - send NMT to slave
 111  ********************************************************
 112  * This an example of :
 113  * Network Dictionary Access (SDO) with Callback
 114  * Slave node state change request (NMT)
 115  ********************************************************
 116  * This is called first by TestMaster_preOperational
 117  * then it called again each time a SDO exchange is
 118  * finished.
 119  ********************************************************/
 120 static void ConfigureSlaveNode(CO_Data* d, UNS8 nodeId)
 121 {
 122         UNS8 res;
 123         eprintf("Master : ConfigureSlaveNode %2.2x\n", nodeId);
 124         switch(++init_step){
 125                 case 1:
 126                 {       /*disable Slave's TPDO 1 */
 127                         UNS32 TPDO_COBId = 0x80000180 + nodeId;
 128
 129                         eprintf("Master : disable slave %2.2x TPDO 1 \n", nodeId);
 130                         res = writeNetworkDictCallBack (d, /*CO_Data* d*/
 131                                         /**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/
 132                                         nodeId, /*UNS8 nodeId*/
 133                                         0x1800, /*UNS16 index*/
 134                                         0x01, /*UNS8 subindex*/
 135                                         4, /*UNS8 count*/
 136                                         0, /*UNS8 dataType*/
 137                                         &TPDO_COBId,/*void *data*/
 138                                         CheckSDOAndContinue); /*SDOCallback_t Callback*/
 139                 }
 140                 break;
 141
 142                 case 2:
 143                 {       /*setup Slave's TPDO 1 to be transmitted on SYNC*/
 144                         UNS8 Transmission_Type = 0x01;
 145
 146                         eprintf("Master : set slave %2.2x TPDO 1 transmit type\n", nodeId);
 147                         res = writeNetworkDictCallBack (d, /*CO_Data* d*/
 148                                         /**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/
 149                                         nodeId, /*UNS8 nodeId*/
 150                                         0x1800, /*UNS16 index*/
 151                                         0x02, /*UNS8 subindex*/
 152                                         1, /*UNS8 count*/
 153                                         0, /*UNS8 dataType*/
 154                                         &Transmission_Type,/*void *data*/
 155                                         CheckSDOAndContinue); /*SDOCallback_t Callback*/
 156                 }
 157                 break;
 158
 159                 case 3:
 160                 {       /*re-enable Slave's TPDO 1 */
 161                         UNS32 TPDO_COBId = 0x00000180 + nodeId;
 162
 163                         eprintf("Master : re-enable slave %2.2x TPDO 1\n", nodeId);
 164                         res = writeNetworkDictCallBack (d, /*CO_Data* d*/
 165                                         /**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/
 166                                         nodeId, /*UNS8 nodeId*/
 167                                         0x1800, /*UNS16 index*/
 168                                         0x01, /*UNS8 subindex*/
 169                                         4, /*UNS8 count*/
 170                                         0, /*UNS8 dataType*/
 171                                         &TPDO_COBId,/*void *data*/
 172                                         CheckSDOAndContinue); /*SDOCallback_t Callback*/
 173                 }
 174                 break;
 175
 176                 case 4:
 177                 {       /*disable Slave's RPDO 1 */
 178                         UNS32 TPDO_COBId = 0x80000200 + nodeId;
 179
 180                         eprintf("Master : disable slave %2.2x RPDO 1\n", nodeId);
 181                         res = writeNetworkDictCallBack (d, /*CO_Data* d*/
 182                                         /**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/
 183                                         nodeId, /*UNS8 nodeId*/
 184                                         0x1400, /*UNS16 index*/
 185                                         0x01, /*UNS8 subindex*/
 186                                         4, /*UNS8 count*/
 187                                         0, /*UNS8 dataType*/
 188                                         &TPDO_COBId,/*void *data*/
 189                                         CheckSDOAndContinue); /*SDOCallback_t Callback*/
 190                 }
 191                 break;
 192
 193
 194                 case 5:
 195                 {
 196                         UNS8 Transmission_Type = 0x01;
 197
 198                         eprintf("Master : set slave %2.2x RPDO 1 receive type\n", nodeId);
 199                         res = writeNetworkDictCallBack (d, /*CO_Data* d*/
 200                                         /**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/
 201                                         nodeId, /*UNS8 nodeId*/
 202                                         0x1400, /*UNS16 index*/
 203                                         0x02, /*UNS8 subindex*/
 204                                         1, /*UNS8 count*/
 205                                         0, /*UNS8 dataType*/
 206                                         &Transmission_Type,/*void *data*/
 207                                         CheckSDOAndContinue); /*SDOCallback_t Callback*/
 208                 }
 209                 break;
 210
 211                 case 6:
 212                 {       /*re-enable Slave's RPDO 1 */
 213                         UNS32 TPDO_COBId = 0x00000200 + nodeId;
 214
 215                         eprintf("Master : re-enable %2.2x RPDO 1\n", nodeId);
 216                         res = writeNetworkDictCallBack (d, /*CO_Data* d*/
 217                                         /**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/
 218                                         nodeId, /*UNS8 nodeId*/
 219                                         0x1400, /*UNS16 index*/
 220                                         0x01, /*UNS8 subindex*/
 221                                         4, /*UNS8 count*/
 222                                         0, /*UNS8 dataType*/
 223                                         &TPDO_COBId,/*void *data*/
 224                                         CheckSDOAndContinue); /*SDOCallback_t Callback*/
 225                 }
 226                 break;
 227
 228                 case 7:
 229                 {
 230                         /*set the heartbeat Producer Time*/
 231                         UNS16 Heartbeat_Producer_Time = 0x03E8;
 232                         eprintf("Master : set slave %2.2x heartbeat producer time \n", nodeId);
 233                         res = writeNetworkDictCallBack (d, /*CO_Data* d*/
 234                                         /**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/
 235                                         nodeId, /*UNS8 nodeId*/
 236                                         0x1017, /*UNS16 index*/
 237                                         0x00, /*UNS8 subindex*/
 238                                         2, /*UNS8 count*/
 239                                         0, /*UNS8 dataType*/
 240                                         &Heartbeat_Producer_Time,/*void *data*/
 241                                         CheckSDOAndContinue); /*SDOCallback_t Callback*/
 242                 }
 243                 break;
 244
 245                 case 8:
 246                 {       /*disable Slave's TPDO 2 */
 247                         UNS32 TPDO_COBId = 0x80000280 + nodeId;
 248
 249                         eprintf("Master : disable slave %2.2x TPDO 2 \n", nodeId);
 250                         res = writeNetworkDictCallBack (d, /*CO_Data* d*/
 251                                         /**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/
 252                                         nodeId, /*UNS8 nodeId*/
 253                                         0x1801, /*UNS16 index*/
 254                                         0x01, /*UNS8 subindex*/
 255                                         4, /*UNS8 count*/
 256                                         0, /*UNS8 dataType*/
 257                                         &TPDO_COBId,/*void *data*/
 258                                         CheckSDOAndContinue); /*SDOCallback_t Callback*/
 259                 }
 260                 break;
 261
 262                 case 9:
 263                 {       /*disable Slave's TPDO 3 */
 264                         UNS32 TPDO_COBId = 0x80000380 + nodeId;
 265
 266                         eprintf("Master : disable slave %2.2x TPDO 3 \n", nodeId);
 267                         res = writeNetworkDictCallBack (d, /*CO_Data* d*/
 268                                         /**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/
 269                                         nodeId, /*UNS8 nodeId*/
 270                                         0x1802, /*UNS16 index*/
 271                                         0x01, /*UNS8 subindex*/
 272                                         4, /*UNS8 count*/
 273                                         0, /*UNS8 dataType*/
 274                                         &TPDO_COBId,/*void *data*/
 275                                         CheckSDOAndContinue); /*SDOCallback_t Callback*/
 276                 }
 277                 break;
 278
 279                 case 10:
 280                 {       /*disable Slave's TPDO 4 */
 281                         UNS32 TPDO_COBId = 0x80000480 + nodeId;
 282
 283                         eprintf("Master : disable slave %2.2x TPDO 4 \n", nodeId);
 284                         res = writeNetworkDictCallBack (d, /*CO_Data* d*/
 285                                         /**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/
 286                                         nodeId, /*UNS8 nodeId*/
 287                                         0x1803, /*UNS16 index*/
 288                                         0x01, /*UNS8 subindex*/
 289                                         4, /*UNS8 count*/
 290                                         0, /*UNS8 dataType*/
 291                                         &TPDO_COBId,/*void *data*/
 292                                         CheckSDOAndContinue); /*SDOCallback_t Callback*/
 293                 }
 294                 break;
 295
 296                 case 11:
 297                         /* Put the master in operational mode */
 298                         setState(d, Operational);
 299
 300                         /* Ask slave node to go in operational mode */
 301                         masterSendNMTstateChange (d, nodeId, NMT_Start_Node);
 302         }
 303
 304 }
 305
 306 void TestMaster_preOperational(CO_Data* d)
 307 {
 308
 309         eprintf("TestMaster_preOperational\n");
 310         ConfigureSlaveNode(&TestMaster_Data, slavenodeid);
 311
 312 }
 313
 314 void TestMaster_operational(CO_Data* d)
 315 {
 316         eprintf("TestMaster_operational\n");
 317 }
 318
 319 void TestMaster_stopped(CO_Data* d)
 320 {
 321         eprintf("TestMaster_stopped\n");
 322 }
 323
 324 void TestMaster_post_sync(CO_Data* d)
 325 {
 326         DO++;
 327         eprintf("MicroMod Digital Out: %2.2x\n",DO);
 328         eprintf("MicroMod Digital In (by bit): DI1: %2.2x DI2: %2.2x DI3: %2.2x DI4: %2.2x DI5: %2.2x DI6: %2.2x DI7: %2.2x DI8: %2.2x\n",DI1,DI2,DI3,DI4,DI5,DI6,DI7,DI8);
 329 }
 330
 331 void TestMaster_post_TPDO(CO_Data* d)
 332 {
 333 //      eprintf("TestMaster_post_TPDO\n");
 334 }
 335
 336 //s_BOARD SlaveBoard = {"0", "500K"};
 337 s_BOARD MasterBoard = {"0", "125K"};
 338
 339 #if !defined(WIN32) || defined(__CYGWIN__)
 340 void catch_signal(int sig)
 341 {
 342   signal(SIGTERM, catch_signal);
 343   signal(SIGINT, catch_signal);
 344
 345   eprintf("Got Signal %d\n",sig);
 346 }
 347 #endif
 348
 349 void help(void)
 350 {
 351   printf("**************************************************************\n");
 352   printf("*  TestMasterMicroMod                                        *\n");
 353   printf("*                                                            *\n");
 354   printf("*  A simple example for PC.                                  *\n");
 355   printf("*  A CanOpen master that control a MicroMod module:          *\n");
 356   printf("*  - setup module TPDO 1 transmit type                       *\n");
 357   printf("*  - setup module RPDO 1 transmit type                       *\n");
 358   printf("*  - setup module hearbeatbeat period                        *\n");
 359   printf("*  - disable others TPDOs                                    *\n");
 360   printf("*  - set state to operational                                *\n");
 361   printf("*  - send periodic SYNC                                      *\n");
 362   printf("*  - send periodic RPDO 1 to Micromod (digital output)       *\n");
 363   printf("*  - listen Micromod's TPDO 1 (digital input)                *\n");
 364   printf("*  - Mapping RPDO 1 bit per bit (digital input)              *\n");
 365   printf("*                                                            *\n");
 366   printf("*   Usage:                                                   *\n");
 367   printf("*   ./TestMasterMicroMod  [OPTIONS]                          *\n");
 368   printf("*                                                            *\n");
 369   printf("*   OPTIONS:                                                 *\n");
 370   printf("*     -l : Can library [\"libcanfestival_can_virtual.so\"]     *\n");
 371   printf("*                                                            *\n");
 372   printf("*    Slave:                                                  *\n");
 373   printf("*     -i : Slave Node id format [0x01 , 0x7F]                *\n");
 374   printf("*                                                            *\n");
 375   printf("*    Master:                                                 *\n");
 376   printf("*     -m : bus name [\"1\"]                                    *\n");
 377   printf("*     -M : 1M,500K,250K,125K,100K,50K,20K,10K                *\n");
 378   printf("*                                                            *\n");
 379   printf("**************************************************************\n");
 380 }
 381
 382 /***************************  INIT  *****************************************/
 383 void InitNodes(CO_Data* d, UNS32 id)
 384 {
 385         /****************************** INITIALISATION MASTER *******************************/
 386         if(MasterBoard.baudrate){
 387                 /* Defining the node Id */
 388                 setNodeId(&TestMaster_Data, 0x01);
 389
 390                 /* init */
 391                 setState(&TestMaster_Data, Initialisation);
 392         }
 393 }
 394
 395 /***************************  EXIT  *****************************************/
 396 void Exit(CO_Data* d, UNS32 id)
 397 {
 398         if(strcmp(MasterBoard.baudrate, "none")){
 399                 masterSendNMTstateChange(&TestMaster_Data, 0x02, NMT_Reset_Node);
 400
 401         //Stop master
 402                 setState(&TestMaster_Data, Stopped);
 403         }
 404 }
 405 /****************************************************************************/
 406 /***************************  MAIN  *****************************************/
 407 /****************************************************************************/
 408 int main(int argc,char **argv)
 409 {
 410
 411   int c;
 412   extern char *optarg;
 413   char* LibraryPath="libcanfestival_can_virtual.so";
 414   char *snodeid;
 415   while ((c = getopt(argc, argv, "-m:s:M:S:l:i:")) != EOF)
 416   {
 417     switch(c)
 418     {
 419       case 'm' :
 420         if (optarg[0] == 0)
 421         {
 422           help();
 423           exit(1);
 424         }
 425         MasterBoard.busname = optarg;
 426         break;
 427       case 'M' :
 428         if (optarg[0] == 0)
 429         {
 430           help();
 431           exit(1);
 432         }
 433         MasterBoard.baudrate = optarg;
 434         break;
 435       case 'l' :
 436         if (optarg[0] == 0)
 437         {
 438           help();
 439           exit(1);
 440         }
 441         LibraryPath = optarg;
 442         break;
 443       case 'i' :
 444         if (optarg[0] == 0)
 445         {
 446           help();
 447           exit(1);
 448         }
 449         snodeid = optarg;
 450                 sscanf(snodeid,"%x",&slavenodeid);
 451         break;
 452       default:
 453         help();
 454         exit(1);
 455     }
 456   }
 457
 458 #if !defined(WIN32) || defined(__CYGWIN__)
 459   /* install signal handler for manual break */
 460         signal(SIGTERM, catch_signal);
 461         signal(SIGINT, catch_signal);
 462         TimerInit();
 463 #endif
 464
 465 #ifndef NOT_USE_DYNAMIC_LOADING
 466         LoadCanDriver(LibraryPath);
 467 #endif
 468
 469         TestMaster_Data.heartbeatError = TestMaster_heartbeatError;
 470         TestMaster_Data.initialisation = TestMaster_initialisation;
 471         TestMaster_Data.preOperational = TestMaster_preOperational;
 472         TestMaster_Data.operational = TestMaster_operational;
 473         TestMaster_Data.stopped = TestMaster_stopped;
 474         TestMaster_Data.post_sync = TestMaster_post_sync;
 475         TestMaster_Data.post_TPDO = TestMaster_post_TPDO;
 476
 477         if(!canOpen(&MasterBoard,&TestMaster_Data)){
 478                 eprintf("Cannot open Master Board\n");
 479                 goto fail_master;
 480         }
 481
 482         // Start timer thread
 483         StartTimerLoop(&InitNodes);
 484
 485         // wait Ctrl-C
 486         pause();
 487         eprintf("Finishing.\n");
 488
 489         // Stop timer thread
 490         StopTimerLoop(&Exit);
 491
 492 fail_master:
 493         if(MasterBoard.baudrate) canClose(&TestMaster_Data);
 494
 495   TimerCleanup();
 496   return 0;
 497 }
 498
 499