examples/DS401_Master/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 UNS8 slavenodeid;
  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         UNS8 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 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         printf("nodeid slave=%x\n",nodeId);
 125         switch(++init_step){
 126                 case 1:
 127                 {       /*disable Slave's TPDO 1 */
 128                         UNS32 TPDO_COBId = 0x80000180 + nodeId;
 129
 130                         eprintf("Master : disable slave %2.2x TPDO 1 \n", nodeId);
 131                         res = writeNetworkDictCallBack (d, /*CO_Data* d*/
 132                                         /**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/
 133                                         nodeId, /*UNS8 nodeId*/
 134                                         0x1800, /*UNS16 index*/
 135                                         0x01, /*UNS8 subindex*/
 136                                         4, /*UNS8 count*/
 137                                         0, /*UNS8 dataType*/
 138                                         &TPDO_COBId,/*void *data*/
 139                                         CheckSDOAndContinue); /*SDOCallback_t Callback*/
 140                 }
 141                 break;
 142
 143                 case 2:
 144                 {       /*setup Slave's TPDO 1 to be transmitted on SYNC*/
 145                         UNS8 Transmission_Type = 0x01;
 146
 147                         eprintf("Master : set slave %2.2x TPDO 1 transmit type\n", nodeId);
 148                         res = writeNetworkDictCallBack (d, /*CO_Data* d*/
 149                                         /**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/
 150                                         nodeId, /*UNS8 nodeId*/
 151                                         0x1800, /*UNS16 index*/
 152                                         0x02, /*UNS8 subindex*/
 153                                         1, /*UNS8 count*/
 154                                         0, /*UNS8 dataType*/
 155                                         &Transmission_Type,/*void *data*/
 156                                         CheckSDOAndContinue); /*SDOCallback_t Callback*/
 157                 }
 158                 break;
 159
 160                 case 3:
 161                 {       /*re-enable Slave's TPDO 1 */
 162                         UNS32 TPDO_COBId = 0x00000180 + nodeId;
 163
 164                         eprintf("Master : re-enable slave %2.2x TPDO 1\n", nodeId);
 165                         res = writeNetworkDictCallBack (d, /*CO_Data* d*/
 166                                         /**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/
 167                                         nodeId, /*UNS8 nodeId*/
 168                                         0x1800, /*UNS16 index*/
 169                                         0x01, /*UNS8 subindex*/
 170                                         4, /*UNS8 count*/
 171                                         0, /*UNS8 dataType*/
 172                                         &TPDO_COBId,/*void *data*/
 173                                         CheckSDOAndContinue); /*SDOCallback_t Callback*/
 174                 }
 175                 break;
 176
 177                 case 4:
 178                 {       /*disable Slave's TPDO 2 */
 179                         UNS32 TPDO_COBId = 0x80000200 + nodeId;
 180
 181                         eprintf("Master : disable slave %2.2x RPDO 1\n", nodeId);
 182                         res = writeNetworkDictCallBack (d, /*CO_Data* d*/
 183                                         /**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/
 184                                         nodeId, /*UNS8 nodeId*/
 185                                         0x1400, /*UNS16 index*/
 186                                         0x01, /*UNS8 subindex*/
 187                                         4, /*UNS8 count*/
 188                                         0, /*UNS8 dataType*/
 189                                         &TPDO_COBId,/*void *data*/
 190                                         CheckSDOAndContinue); /*SDOCallback_t Callback*/
 191                 }
 192                 break;
 193
 194
 195                 case 5:
 196                 {
 197                         UNS8 Transmission_Type = 0x01;
 198
 199                         eprintf("Master : set slave %2.2x RPDO 1 receive type\n", nodeId);
 200                         res = writeNetworkDictCallBack (d, /*CO_Data* d*/
 201                                         /**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/
 202                                         nodeId, /*UNS8 nodeId*/
 203                                         0x1400, /*UNS16 index*/
 204                                         0x02, /*UNS8 subindex*/
 205                                         1, /*UNS8 count*/
 206                                         0, /*UNS8 dataType*/
 207                                         &Transmission_Type,/*void *data*/
 208                                         CheckSDOAndContinue); /*SDOCallback_t Callback*/
 209                 }
 210                 break;
 211
 212                 case 6:
 213                 {       /*re-enable Slave's TPDO 1 */
 214                         UNS32 TPDO_COBId = 0x00000200 + nodeId;
 215
 216                         eprintf("Master : re-enable %2.2x RPDO 1\n", nodeId);
 217                         res = writeNetworkDictCallBack (d, /*CO_Data* d*/
 218                                         /**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/
 219                                         nodeId, /*UNS8 nodeId*/
 220                                         0x1400, /*UNS16 index*/
 221                                         0x01, /*UNS8 subindex*/
 222                                         4, /*UNS8 count*/
 223                                         0, /*UNS8 dataType*/
 224                                         &TPDO_COBId,/*void *data*/
 225                                         CheckSDOAndContinue); /*SDOCallback_t Callback*/
 226                 }
 227                 break;
 228
 229                 case 7:
 230                 {
 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 2 */
 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
 328         AO1 = AI1 / 2;
 329         AO2 = AI2 / 2;
 330         AO3 = AI3 / 2;
 331         AO4 = AI4 / 2;
 332         eprintf("MicroMod Digital Out: %2.2x\n",DO);
 333         eprintf("MicroMod Analogue Out1: %d\n",AO1);
 334         eprintf("MicroMod Analogue Out2: %d\n",AO2);
 335         eprintf("MicroMod Analogue Out3: %d\n",AO3);
 336         eprintf("MicroMod Analogue Out4: %d\n",AO4);
 337         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);
 338         eprintf("MicroMod Analogue In1: %d\n", AI1);
 339         eprintf("MicroMod Analogue In2: %d\n", AI2);
 340         eprintf("MicroMod Analogue In3: %d\n", AI3);
 341         eprintf("MicroMod Analogue In4: %d\n", AI4);
 342         eprintf("MicroMod Analogue In5: %d\n", AI5);
 343         eprintf("MicroMod Analogue In6: %d\n", AI6);
 344         eprintf("MicroMod Analogue In7: %d\n", AI7);
 345         eprintf("MicroMod Analogue In8: %d\n", AI8);
 346 }
 347
 348 void TestMaster_post_TPDO(CO_Data* d)
 349 {
 350 //      eprintf("TestMaster_post_TPDO\n");
 351 }
 352
 353 //s_BOARD SlaveBoard = {"0", "500K"};
 354 s_BOARD MasterBoard = {"32", "125K"};
 355
 356 #if !defined(WIN32) || defined(__CYGWIN__)
 357 void catch_signal(int sig)
 358 {
 359   signal(SIGTERM, catch_signal);
 360   signal(SIGINT, catch_signal);
 361
 362   eprintf("Got Signal %d\n",sig);
 363 }
 364 #endif
 365
 366 void help()
 367 {
 368   printf("**************************************************************\n");
 369   printf("*  TestMasterMicroMod                                        *\n");
 370   printf("*                                                            *\n");
 371   printf("*  A simple example for PC.                                  *\n");
 372   printf("*  A CanOpen master that control a MicroMod module:          *\n");
 373   printf("*  - setup module TPDO 1 transmit type                       *\n");
 374   printf("*  - setup module RPDO 1 transmit type                       *\n");
 375   printf("*  - setup module hearbeatbeat period                        *\n");
 376   printf("*  - disable others TPDOs                                    *\n");
 377   printf("*  - set state to operational                                *\n");
 378   printf("*  - send periodic SYNC                                      *\n");
 379   printf("*  - send periodic RPDO 1 to Micromod (digital output)       *\n");
 380   printf("*  - listen Micromod's TPDO 1 (digital input)                *\n");
 381   printf("*  - Mapping RPDO 1 bit per bit (digital input)              *\n");
 382   printf("*                                                            *\n");
 383   printf("*   Usage:                                                   *\n");
 384   printf("*   ./TestMasterMicroMod  [OPTIONS]                          *\n");
 385   printf("*                                                            *\n");
 386   printf("*   OPTIONS:                                                 *\n");
 387   printf("*     -l : Can library [\"libcanfestival_can_virtual.so\"]     *\n");
 388   printf("*                                                            *\n");
 389   printf("*    Slave:                                                  *\n");
 390   printf("*     -i : Slave Node id format [0x01 , 0x7F]                *\n");
 391   printf("*                                                            *\n");
 392   printf("*    Master:                                                 *\n");
 393   printf("*     -m : bus name [\"1\"]                                    *\n");
 394   printf("*     -M : 1M,500K,250K,125K,100K,50K,20K,10K                *\n");
 395   printf("*                                                            *\n");
 396   printf("**************************************************************\n");
 397 }
 398
 399 /***************************  INIT  *****************************************/
 400 void InitNodes(CO_Data* d, UNS32 id)
 401 {
 402         /****************************** INITIALISATION MASTER *******************************/
 403         if(MasterBoard.baudrate){
 404                 /* Defining the node Id */
 405                 //setNodeId(&TestMaster_Data, 0x01);
 406
 407                 /* init */
 408                 setState(&TestMaster_Data, Initialisation);
 409         }
 410 }
 411
 412 /****************************************************************************/
 413 /***************************  MAIN  *****************************************/
 414 /****************************************************************************/
 415 int main(int argc,char **argv)
 416 {
 417
 418   int c;
 419   extern char *optarg;
 420   char* LibraryPath="libcanfestival_can_virtual.so";
 421   char *snodeid;
 422   while ((c = getopt(argc, argv, "-m:s:M:S:l:i:")) != EOF)
 423   {
 424     switch(c)
 425     {
 426       case 'm' :
 427         if (optarg[0] == 0)
 428         {
 429           help();
 430           exit(1);
 431         }
 432         MasterBoard.busname = optarg;
 433         break;
 434       case 'M' :
 435         if (optarg[0] == 0)
 436         {
 437           help();
 438           exit(1);
 439         }
 440         MasterBoard.baudrate = optarg;
 441         break;
 442       case 'l' :
 443         if (optarg[0] == 0)
 444         {
 445           help();
 446           exit(1);
 447         }
 448         LibraryPath = optarg;
 449         break;
 450       case 'i' :
 451         if (optarg[0] == 0)
 452         {
 453           help();
 454           exit(1);
 455         }
 456         snodeid = optarg;
 457                 sscanf(snodeid,"%x",&slavenodeid);
 458         break;
 459       default:
 460         help();
 461         exit(1);
 462     }
 463   }
 464
 465 #if !defined(WIN32) || defined(__CYGWIN__)
 466   /* install signal handler for manual break */
 467         signal(SIGTERM, catch_signal);
 468         signal(SIGINT, catch_signal);
 469 #endif
 470
 471 #ifndef NOT_USE_DYNAMIC_LOADING
 472         LoadCanDriver(LibraryPath);
 473 #endif
 474
 475         TestMaster_Data.heartbeatError = TestMaster_heartbeatError;
 476         TestMaster_Data.initialisation = TestMaster_initialisation;
 477         TestMaster_Data.preOperational = TestMaster_preOperational;
 478         TestMaster_Data.operational = TestMaster_operational;
 479         TestMaster_Data.stopped = TestMaster_stopped;
 480         TestMaster_Data.post_sync = TestMaster_post_sync;
 481         TestMaster_Data.post_TPDO = TestMaster_post_TPDO;
 482
 483         if(!canOpen(&MasterBoard,&TestMaster_Data)){
 484                 eprintf("Cannot open Master Board\n");
 485                 goto fail_master;
 486         }
 487
 488         // Start timer thread
 489         StartTimerLoop(&InitNodes);
 490
 491         // wait Ctrl-C
 492         pause();
 493         eprintf("Finishing.\n");
 494
 495         // Reset the slave node for next use (will stop emitting heartbeat)
 496         masterSendNMTstateChange (&TestMaster_Data, slavenodeid, NMT_Reset_Node);
 497
 498         // Stop master
 499         setState(&TestMaster_Data, Stopped);
 500
 501         // Stop timer thread
 502         StopTimerLoop();
 503
 504 fail_master:
 505         if(MasterBoard.baudrate) canClose(&TestMaster_Data);
 506
 507   return 0;
 508 }
 509
 510