CanFestival: examples/TestMasterMicroMod/TestMasterMicroMod.c Source File

00001 /*
+00002 This file is part of CanFestival, a library implementing CanOpen Stack. 
+00003 
+00004 Copyright (C): Edouard TISSERANT and Francis DUPIN
+00005 
+00006 See COPYING file for copyrights details.
+00007 
+00008 This library is free software; you can redistribute it and/or
+00009 modify it under the terms of the GNU Lesser General Public
+00010 License as published by the Free Software Foundation; either
+00011 version 2.1 of the License, or (at your option) any later version.
+00012 
+00013 This library is distributed in the hope that it will be useful,
+00014 but WITHOUT ANY WARRANTY; without even the implied warranty of
+00015 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+00016 Lesser General Public License for more details.
+00017 
+00018 You should have received a copy of the GNU Lesser General Public
+00019 License along with this library; if not, write to the Free Software
+00020 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+00021 */
+00022 
+00023 #if defined(WIN32) && !defined(__CYGWIN__)
+00024 #include <windows.h>
+00025 #include "getopt.h"
+00026 void pause(void)
+00027 {
+00028         system("PAUSE");
+00029 }
+00030 #else
+00031 #include <unistd.h>
+00032 #include <stdio.h>
+00033 #include <string.h>
+00034 #include <stdlib.h>
+00035 #include <signal.h>
+00036 #endif
+00037 
+00038 #include "canfestival.h"
+00039 #include "TestMasterMicroMod.h"
+00040 #include "TestMaster.h"
+00041 UNS8 slavenodeid;
+00042 
+00043 
+00044 /*****************************************************************************/
+00045 void TestMaster_heartbeatError(UNS8 heartbeatID)
+00046 {
+00047         eprintf("TestMaster_heartbeatError %d\n", heartbeatID);
+00048 }
+00049 
+00050 /********************************************************
+00051  * ConfigureSlaveNode is responsible to
+00052  *  - setup master RPDO 1 to receive TPDO 1 from id 0x40
+00053  *  - setup master TPDO 1 to send RPDO 1 to id 0x40
+00054  ********************************************************/
+00055 void TestMaster_initialisation()
+00056 {
+00057         UNS32 PDO1_COBID = 0x0180 + slavenodeid; 
+00058         UNS32 PDO2_COBID = 0x0200 + slavenodeid;
+00059         UNS8 size = sizeof(UNS32); 
+00060 
+00061         eprintf("TestMaster_initialisation\n");
+00062 
+00063         /*****************************************
+00064          * Define RPDOs to match slave ID=0x40 TPDOs*
+00065          *****************************************/
+00066         writeLocalDict( &TestMaster_Data, /*CO_Data* d*/
+00067                         0x1400, /*UNS16 index*/
+00068                         0x01, /*UNS8 subind*/ 
+00069                         &PDO1_COBID, /*void * pSourceData,*/ 
+00070                         &size, /* UNS8 * pExpectedSize*/
+00071                         RW);  /* UNS8 checkAccess */
+00072                         
+00073 
+00074         /*****************************************
+00075          * Define TPDOs to match slave ID=0x40 RPDOs*
+00076          *****************************************/
+00077         writeLocalDict( &TestMaster_Data, /*CO_Data* d*/
+00078                         0x1800, /*UNS16 index*/
+00079                         0x01, /*UNS8 subind*/ 
+00080                         &PDO2_COBID, /*void * pSourceData,*/ 
+00081                         &size, /* UNS8 * pExpectedSize*/
+00082                         RW);  /* UNS8 checkAccess */
+00083 }
+00084 
+00085 static init_step = 0;
+00086 
+00087 /*Froward declaration*/
+00088 static void ConfigureSlaveNode(CO_Data* d, UNS8 nodeId);
+00089 
+00090 
+00091 static void CheckSDOAndContinue(CO_Data* d, UNS8 nodeId)
+00092 {
+00093         UNS32 abortCode;
+00094 
+00095         if(getWriteResultNetworkDict (d, nodeId, &abortCode) != SDO_FINISHED)
+00096                 eprintf("Master : Failed in initializing slave %2.2x, step %d, AbortCode :%4.4x \n", nodeId, init_step, abortCode);
+00097 
+00098         /* Finalise last SDO transfer with this node */
+00099         closeSDOtransfer(&TestMaster_Data, nodeId, SDO_CLIENT);
+00100 
+00101         ConfigureSlaveNode(d, nodeId);
+00102 }
+00103 
+00104 /********************************************************
+00105  * ConfigureSlaveNode is responsible to
+00106  *  - setup slave TPDO 1 transmit time
+00107  *  - setup slave TPDO 2 transmit time
+00108  *  - setup slave Heartbeat Producer time
+00109  *  - switch to operational mode
+00110  *  - send NMT to slave
+00111  ********************************************************
+00112  * This an example of :
+00113  * Network Dictionary Access (SDO) with Callback 
+00114  * Slave node state change request (NMT) 
+00115  ********************************************************
+00116  * This is called first by TestMaster_preOperational
+00117  * then it called again each time a SDO exchange is
+00118  * finished.
+00119  ********************************************************/
+00120 static void ConfigureSlaveNode(CO_Data* d, UNS8 nodeId)
+00121 {
+00122         UNS8 res;
+00123         eprintf("Master : ConfigureSlaveNode %2.2x\n", nodeId);
+00124         switch(++init_step){
+00125                 case 1: 
+00126                 {       /*disable Slave's TPDO 1 */
+00127                         UNS32 TPDO_COBId = 0x80000180 + nodeId;
+00128                         
+00129                         eprintf("Master : disable slave %2.2x TPDO 1 \n", nodeId);
+00130                         res = writeNetworkDictCallBack (d, /*CO_Data* d*/
+00132                                         nodeId, /*UNS8 nodeId*/
+00133                                         0x1800, /*UNS16 index*/
+00134                                         0x01, /*UNS8 subindex*/
+00135                                         4, /*UNS8 count*/
+00136                                         0, /*UNS8 dataType*/
+00137                                         &TPDO_COBId,/*void *data*/
+00138                                         CheckSDOAndContinue); /*SDOCallback_t Callback*/
+00139                 }                       
+00140                 break;
+00141 
+00142                 case 2: 
+00143                 {       /*setup Slave's TPDO 1 to be transmitted on SYNC*/
+00144                         UNS8 Transmission_Type = 0x01;
+00145                         
+00146                         eprintf("Master : set slave %2.2x TPDO 1 transmit type\n", nodeId);
+00147                         res = writeNetworkDictCallBack (d, /*CO_Data* d*/
+00149                                         nodeId, /*UNS8 nodeId*/
+00150                                         0x1800, /*UNS16 index*/
+00151                                         0x02, /*UNS8 subindex*/
+00152                                         1, /*UNS8 count*/
+00153                                         0, /*UNS8 dataType*/
+00154                                         &Transmission_Type,/*void *data*/
+00155                                         CheckSDOAndContinue); /*SDOCallback_t Callback*/
+00156                 }                       
+00157                 break;
+00158 
+00159                 case 3: 
+00160                 {       /*re-enable Slave's TPDO 1 */
+00161                         UNS32 TPDO_COBId = 0x00000180 + nodeId;
+00162                         
+00163                         eprintf("Master : re-enable slave %2.2x TPDO 1\n", nodeId);
+00164                         res = writeNetworkDictCallBack (d, /*CO_Data* d*/
+00166                                         nodeId, /*UNS8 nodeId*/
+00167                                         0x1800, /*UNS16 index*/
+00168                                         0x01, /*UNS8 subindex*/
+00169                                         4, /*UNS8 count*/
+00170                                         0, /*UNS8 dataType*/
+00171                                         &TPDO_COBId,/*void *data*/
+00172                                         CheckSDOAndContinue); /*SDOCallback_t Callback*/
+00173                 }                       
+00174                 break;
+00175                                         
+00176                 case 4: 
+00177                 {       /*disable Slave's TPDO 2 */
+00178                         UNS32 TPDO_COBId = 0x80000200 + nodeId;
+00179                         
+00180                         eprintf("Master : disable slave %2.2x RPDO 1\n", nodeId);
+00181                         res = writeNetworkDictCallBack (d, /*CO_Data* d*/
+00183                                         nodeId, /*UNS8 nodeId*/
+00184                                         0x1400, /*UNS16 index*/
+00185                                         0x01, /*UNS8 subindex*/
+00186                                         4, /*UNS8 count*/
+00187                                         0, /*UNS8 dataType*/
+00188                                         &TPDO_COBId,/*void *data*/
+00189                                         CheckSDOAndContinue); /*SDOCallback_t Callback*/
+00190                 }                       
+00191                 break;
+00192 
+00193                                         
+00194                 case 5:
+00195                 {       
+00196                         UNS8 Transmission_Type = 0x01;
+00197                         
+00198                         eprintf("Master : set slave %2.2x RPDO 1 receive type\n", nodeId);
+00199                         res = writeNetworkDictCallBack (d, /*CO_Data* d*/
+00201                                         nodeId, /*UNS8 nodeId*/
+00202                                         0x1400, /*UNS16 index*/
+00203                                         0x02, /*UNS8 subindex*/
+00204                                         1, /*UNS8 count*/
+00205                                         0, /*UNS8 dataType*/
+00206                                         &Transmission_Type,/*void *data*/
+00207                                         CheckSDOAndContinue); /*SDOCallback_t Callback*/
+00208                 }       
+00209                 break;
+00210 
+00211                 case 6: 
+00212                 {       /*re-enable Slave's TPDO 1 */
+00213                         UNS32 TPDO_COBId = 0x00000200 + nodeId;
+00214                         
+00215                         eprintf("Master : re-enable %2.2x RPDO 1\n", nodeId);
+00216                         res = writeNetworkDictCallBack (d, /*CO_Data* d*/
+00218                                         nodeId, /*UNS8 nodeId*/
+00219                                         0x1400, /*UNS16 index*/
+00220                                         0x01, /*UNS8 subindex*/
+00221                                         4, /*UNS8 count*/
+00222                                         0, /*UNS8 dataType*/
+00223                                         &TPDO_COBId,/*void *data*/
+00224                                         CheckSDOAndContinue); /*SDOCallback_t Callback*/
+00225                 }                       
+00226                 break;
+00227                 
+00228                 case 7: 
+00229                 {
+00230                         UNS16 Heartbeat_Producer_Time = 0x03E8; 
+00231                         eprintf("Master : set slave %2.2x heartbeat producer time \n", nodeId);
+00232                         res = writeNetworkDictCallBack (d, /*CO_Data* d*/
+00234                                         nodeId, /*UNS8 nodeId*/
+00235                                         0x1017, /*UNS16 index*/
+00236                                         0x00, /*UNS8 subindex*/
+00237                                         2, /*UNS8 count*/
+00238                                         0, /*UNS8 dataType*/
+00239                                         &Heartbeat_Producer_Time,/*void *data*/
+00240                                         CheckSDOAndContinue); /*SDOCallback_t Callback*/
+00241                 }                       
+00242                 break;
+00243 
+00244                 case 8: 
+00245                 {       /*disable Slave's TPDO 2 */
+00246                         UNS32 TPDO_COBId = 0x80000280 + nodeId;
+00247                         
+00248                         eprintf("Master : disable slave %2.2x TPDO 2 \n", nodeId);
+00249                         res = writeNetworkDictCallBack (d, /*CO_Data* d*/
+00251                                         nodeId, /*UNS8 nodeId*/
+00252                                         0x1801, /*UNS16 index*/
+00253                                         0x01, /*UNS8 subindex*/
+00254                                         4, /*UNS8 count*/
+00255                                         0, /*UNS8 dataType*/
+00256                                         &TPDO_COBId,/*void *data*/
+00257                                         CheckSDOAndContinue); /*SDOCallback_t Callback*/
+00258                 }                       
+00259                 break;
+00260 
+00261                 case 9: 
+00262                 {       /*disable Slave's TPDO 3 */
+00263                         UNS32 TPDO_COBId = 0x80000380 + nodeId;
+00264                         
+00265                         eprintf("Master : disable slave %2.2x TPDO 3 \n", nodeId);
+00266                         res = writeNetworkDictCallBack (d, /*CO_Data* d*/
+00268                                         nodeId, /*UNS8 nodeId*/
+00269                                         0x1802, /*UNS16 index*/
+00270                                         0x01, /*UNS8 subindex*/
+00271                                         4, /*UNS8 count*/
+00272                                         0, /*UNS8 dataType*/
+00273                                         &TPDO_COBId,/*void *data*/
+00274                                         CheckSDOAndContinue); /*SDOCallback_t Callback*/
+00275                 }
+00276                 break;                  
+00277 
+00278                 case 10: 
+00279                 {       /*disable Slave's TPDO 2 */
+00280                         UNS32 TPDO_COBId = 0x80000480 + nodeId;
+00281                         
+00282                         eprintf("Master : disable slave %2.2x TPDO 4 \n", nodeId);
+00283                         res = writeNetworkDictCallBack (d, /*CO_Data* d*/
+00285                                         nodeId, /*UNS8 nodeId*/
+00286                                         0x1803, /*UNS16 index*/
+00287                                         0x01, /*UNS8 subindex*/
+00288                                         4, /*UNS8 count*/
+00289                                         0, /*UNS8 dataType*/
+00290                                         &TPDO_COBId,/*void *data*/
+00291                                         CheckSDOAndContinue); /*SDOCallback_t Callback*/
+00292                 }                       
+00293                 break;                  
+00294                 
+00295                 case 11:
+00296                         /* Put the master in operational mode */
+00297                         setState(d, Operational);
+00298                           
+00299                         /* Ask slave node to go in operational mode */
+00300                         masterSendNMTstateChange (d, nodeId, NMT_Start_Node);
+00301         }
+00302                         
+00303 }
+00304 
+00305 void TestMaster_preOperational()
+00306 {
+00307 
+00308         eprintf("TestMaster_preOperational\n");
+00309         ConfigureSlaveNode(&TestMaster_Data, slavenodeid);
+00310         
+00311 }
+00312 
+00313 void TestMaster_operational()
+00314 {
+00315         eprintf("TestMaster_operational\n");
+00316 }
+00317 
+00318 void TestMaster_stopped()
+00319 {
+00320         eprintf("TestMaster_stopped\n");
+00321 }
+00322 
+00323 void TestMaster_post_sync()
+00324 {
+00325         DO++;
+00326         eprintf("MicroMod Digital Out: %2.2x\n",DO);
+00327         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);
+00328 }
+00329 
+00330 void TestMaster_post_TPDO()
+00331 {
+00332 //      eprintf("TestMaster_post_TPDO\n");      
+00333 }
+00334 
+00335 //s_BOARD SlaveBoard = {"0", "500K"};
+00336 s_BOARD MasterBoard = {"32", "125K"};
+00337 
+00338 #if !defined(WIN32) || defined(__CYGWIN__)
+00339 void catch_signal(int sig)
+00340 {
+00341   signal(SIGTERM, catch_signal);
+00342   signal(SIGINT, catch_signal);
+00343   
+00344   eprintf("Got Signal %d\n",sig);
+00345 }
+00346 #endif
+00347 
+00348 void help()
+00349 {
+00350   printf("**************************************************************\n");
+00351   printf("*  TestMasterMicroMod                                        *\n");
+00352   printf("*                                                            *\n");
+00353   printf("*  A simple example for PC.                                  *\n");
+00354   printf("*  A CanOpen master that control a MicroMod module:          *\n");
+00355   printf("*  - setup module TPDO 1 transmit type                       *\n");
+00356   printf("*  - setup module RPDO 1 transmit type                       *\n");
+00357   printf("*  - setup module hearbeatbeat period                        *\n");
+00358   printf("*  - disable others TPDOs                                    *\n");
+00359   printf("*  - set state to operational                                *\n");
+00360   printf("*  - send periodic SYNC                                      *\n");
+00361   printf("*  - send periodic RPDO 1 to Micromod (digital output)       *\n");
+00362   printf("*  - listen Micromod's TPDO 1 (digital input)                *\n");
+00363   printf("*  - Mapping RPDO 1 bit per bit (digital input)              *\n");
+00364   printf("*                                                            *\n");
+00365   printf("*   Usage:                                                   *\n");
+00366   printf("*   ./TestMasterMicroMod  [OPTIONS]                          *\n");
+00367   printf("*                                                            *\n");
+00368   printf("*   OPTIONS:                                                 *\n");
+00369   printf("*     -l : Can library [\"libcanfestival_can_virtual.so\"]     *\n");
+00370   printf("*                                                            *\n");
+00371   printf("*    Slave:                                                  *\n");
+00372   printf("*     -i : Slave Node id format [0x01 , 0x7F]                *\n");
+00373   printf("*                                                            *\n");
+00374   printf("*    Master:                                                 *\n");
+00375   printf("*     -m : bus name [\"1\"]                                    *\n");
+00376   printf("*     -M : 1M,500K,250K,125K,100K,50K,20K,10K                *\n");
+00377   printf("*                                                            *\n");
+00378   printf("**************************************************************\n");
+00379 }
+00380 
+00381 /***************************  INIT  *****************************************/
+00382 void InitNodes(CO_Data* d, UNS32 id)
+00383 {
+00384         /****************************** INITIALISATION MASTER *******************************/
+00385         if(MasterBoard.baudrate){
+00386                 /* Defining the node Id */
+00387                 setNodeId(&TestMaster_Data, 0x01);
+00388 
+00389                 /* init */
+00390                 setState(&TestMaster_Data, Initialisation);
+00391         }
+00392 }
+00393 
+00394 /****************************************************************************/
+00395 /***************************  MAIN  *****************************************/
+00396 /****************************************************************************/
+00397 int main(int argc,char **argv)
+00398 {
+00399 
+00400   int c;
+00401   extern char *optarg;
+00402   char* LibraryPath="libcanfestival_can_virtual.so";
+00403   char *snodeid;
+00404   while ((c = getopt(argc, argv, "-m:s:M:S:l:i:")) != EOF)
+00405   {
+00406     switch(c)
+00407     {
+00408       case 'm' :
+00409         if (optarg[0] == 0)
+00410         {
+00411           help();
+00412           exit(1);
+00413         }
+00414         MasterBoard.busname = optarg;
+00415         break;
+00416       case 'M' :
+00417         if (optarg[0] == 0)
+00418         {
+00419           help();
+00420           exit(1);
+00421         }
+00422         MasterBoard.baudrate = optarg;
+00423         break;
+00424       case 'l' :
+00425         if (optarg[0] == 0)
+00426         {
+00427           help();
+00428           exit(1);
+00429         }
+00430         LibraryPath = optarg;
+00431         break;
+00432       case 'i' :
+00433         if (optarg[0] == 0)
+00434         {
+00435           help();
+00436           exit(1);
+00437         }
+00438         snodeid = optarg;
+00439                 sscanf(snodeid,"%x",&slavenodeid);
+00440         break;
+00441       default:
+00442         help();
+00443         exit(1);
+00444     }
+00445   }
+00446 
+00447 #if !defined(WIN32) || defined(__CYGWIN__)
+00448   /* install signal handler for manual break */
+00449         signal(SIGTERM, catch_signal);
+00450         signal(SIGINT, catch_signal);
+00451 #endif
+00452 
+00453 #ifndef NOT_USE_DYNAMIC_LOADING
+00454         LoadCanDriver(LibraryPath);
+00455 #endif          
+00456 
+00457         TestMaster_Data.heartbeatError = TestMaster_heartbeatError;
+00458         TestMaster_Data.initialisation = TestMaster_initialisation;
+00459         TestMaster_Data.preOperational = TestMaster_preOperational;
+00460         TestMaster_Data.operational = TestMaster_operational;
+00461         TestMaster_Data.stopped = TestMaster_stopped;
+00462         TestMaster_Data.post_sync = TestMaster_post_sync;
+00463         TestMaster_Data.post_TPDO = TestMaster_post_TPDO;
+00464         
+00465         if(!canOpen(&MasterBoard,&TestMaster_Data)){
+00466                 eprintf("Cannot open Master Board\n");
+00467                 goto fail_master;
+00468         }
+00469         
+00470         // Start timer thread
+00471         StartTimerLoop(&InitNodes);
+00472 
+00473         // wait Ctrl-C
+00474         pause();
+00475         eprintf("Finishing.\n");
+00476         
+00477         // Reset the slave node for next use (will stop emitting heartbeat)
+00478         masterSendNMTstateChange (&TestMaster_Data, slavenodeid, NMT_Reset_Node);
+00479         
+00480         // Stop master
+00481         setState(&TestMaster_Data, Stopped);
+00482         
+00483         // Stop timer thread
+00484         StopTimerLoop();
+00485         
+00486 fail_master:
+00487         if(MasterBoard.baudrate) canClose(&TestMaster_Data);    
+00488 
+00489   return 0;
+00490 }
+00491 
+00492 
+