examples/kerneltest/Master.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 #include "Master.h"
  24 #include "Slave.h"
  25 #include "TestMasterSlave.h"
  26 #include "can_driver.h"
  27
  28 extern s_BOARD MasterBoard;
  29 /*****************************************************************************/
  30 void TestMaster_heartbeatError(CO_Data* d, UNS8 heartbeatID)
  31 {
  32         eprintf("TestMaster_heartbeatError %d\n", heartbeatID);
  33 }
  34
  35 /********************************************************
  36  * ConfigureSlaveNode is responsible to
  37  *  - setup master RPDO 1 to receive TPDO 1 from id 2
  38  *  - setup master RPDO 2 to receive TPDO 2 from id 2
  39  ********************************************************/
  40 void TestMaster_initialisation(CO_Data* d)
  41 {
  42         UNS32 PDO1_COBID = 0x0182;
  43         UNS32 PDO2_COBID = 0x0282;
  44         UNS8 size = sizeof(UNS32);
  45 //      UNS32 SINC_cicle=0;
  46 //      UNS8 data_type = 0;
  47
  48         eprintf("TestMaster_initialisation\n");
  49
  50         /*****************************************
  51          * Define RPDOs to match slave ID=2 TPDOs*
  52          *****************************************/
  53         writeLocalDict( &TestMaster_Data, /*CO_Data* d*/
  54                         0x1400, /*UNS16 index*/
  55                         0x01, /*UNS8 subind*/
  56                         &PDO1_COBID, /*void * pSourceData,*/
  57                         &size, /* UNS8 * pExpectedSize*/
  58                         RW);  /* UNS8 checkAccess */
  59
  60         writeLocalDict( &TestMaster_Data, /*CO_Data* d*/
  61                         0x1401, /*UNS16 index*/
  62                         0x01, /*UNS8 subind*/
  63                         &PDO2_COBID, /*void * pSourceData,*/
  64                         &size, /* UNS8 * pExpectedSize*/
  65                         RW);  /* UNS8 checkAccess */
  66
  67 }
  68
  69 // Step counts number of times ConfigureSlaveNode is called
  70 static int init_step = 0;
  71
  72 /*Froward declaration*/
  73 static void ConfigureSlaveNode(CO_Data* d, UNS8 nodeId);
  74
  75 /**/
  76 static void CheckSDOAndContinue(CO_Data* d, UNS8 nodeId)
  77 {
  78         UNS32 abortCode;
  79         if(getWriteResultNetworkDict (d, nodeId, &abortCode) != SDO_FINISHED)
  80                 eprintf("Master : Failed in initializing slave %2.2x, step %d, AbortCode :%4.4x \n", nodeId, init_step, abortCode);
  81
  82         /* Finalise last SDO transfer with this node */
  83         closeSDOtransfer(&TestMaster_Data, nodeId, SDO_CLIENT);
  84
  85         ConfigureSlaveNode(d, nodeId);
  86 }
  87
  88 /********************************************************
  89  * ConfigureSlaveNode is responsible to
  90  *  - setup slave TPDO 1 transmit type
  91  *  - setup slave TPDO 2 transmit type
  92  *  - switch to operational mode
  93  *  - send NMT to slave
  94  ********************************************************
  95  * This an example of :
  96  * Network Dictionary Access (SDO) with Callback
  97  * Slave node state change request (NMT)
  98  ********************************************************
  99  * This is called first by TestMaster_post_SlaveBootup
 100  * then it called again each time a SDO exchange is
 101  * finished.
 102  ********************************************************/
 103
 104 static void ConfigureSlaveNode(CO_Data* d, UNS8 nodeId)
 105 {
 106         /* Master configure heartbeat producer time at 1000 ms
 107          * for slave node-id 0x02 by DCF concise */
 108
 109         UNS8 Transmission_Type = 0x01;
 110 //      UNS32 abortCode;
 111         UNS8 res;
 112         eprintf("Master : ConfigureSlaveNode %2.2x\n", nodeId);
 113
 114         switch(++init_step){
 115                 case 1: /*First step : setup Slave's TPDO 1 to be transmitted on SYNC*/
 116                         eprintf("Master : set slave %2.2x TPDO 1 transmit type\n", nodeId);
 117                         res = writeNetworkDictCallBack (d, /*CO_Data* d*/
 118                                         nodeId, /*UNS8 nodeId*/
 119                                         0x1800, /*UNS16 index*/
 120                                         0x02, /*UNS8 subindex*/
 121                                         1, /*UNS8 count*/
 122                                         0, /*UNS8 dataType*/
 123                                         &Transmission_Type,/*void *data*/
 124                                         CheckSDOAndContinue); /*SDOCallback_t Callback*/
 125                                         break;
 126
 127                 case 2: /*Second step*/
 128                         eprintf("Master : set slave %2.2x TPDO 2 transmit type\n", nodeId);
 129                         writeNetworkDictCallBack (d, /*CO_Data* d*/
 130                                         nodeId, /*UNS8 nodeId*/
 131                                         0x1801, /*UNS16 index*/
 132                                         0x02, /*UNS16 index*/
 133                                         1, /*UNS8 count*/
 134                                         0, /*UNS8 dataType*/
 135                                         &Transmission_Type,/*void *data*/
 136                                         CheckSDOAndContinue); /*SDOCallback_t Callback*/
 137                                         break;
 138                 case 3:
 139
 140                 /****************************** START *******************************/
 141
 142                         /* Put the master in operational mode */
 143                         setState(d, Operational);
 144
 145                         /* Ask slave node to go in operational mode */
 146                         masterSendNMTstateChange (d, nodeId, NMT_Start_Node);
 147
 148         }
 149 }
 150
 151
 152 void TestMaster_preOperational(CO_Data* d)
 153 {
 154         eprintf("TestMaster_preOperational\n");
 155 }
 156
 157 void TestMaster_operational(CO_Data* d)
 158 {
 159         eprintf("TestMaster_operational\n");
 160 }
 161
 162 void TestMaster_stopped(CO_Data* d)
 163 {
 164         eprintf("TestMaster_stopped\n");
 165 }
 166
 167 void TestMaster_post_sync(CO_Data* d)
 168 {
 169         eprintf("TestMaster_post_sync\n");
 170         eprintf("Master: %d %d %d %d %d %d %d %d %d %x %x %d %d\n",
 171                 MasterMap1,
 172                 MasterMap2,
 173                 MasterMap3,
 174                 MasterMap4,
 175                 MasterMap5,
 176                 MasterMap6,
 177                 MasterMap7,
 178                 MasterMap8,
 179                 MasterMap9,
 180                 MasterMap10,
 181                 MasterMap11,
 182                 MasterMap12,
 183                 MasterMap13);
 184 }
 185
 186 void TestMaster_post_emcy(CO_Data* d, UNS8 nodeID, UNS16 errCode, UNS8 errReg)
 187 {
 188         eprintf("Master received EMCY message. Node: %2.2x  ErrorCode: %4.4x  ErrorRegister: %2.2x\n", nodeID, errCode, errReg);
 189 }
 190
 191 char query_result = 0;
 192 char waiting_answer = 0;
 193
 194 static void CheckSDO(CO_Data* d, UNS8 nodeId)
 195 {
 196         UNS32 abortCode;
 197         if(getWriteResultNetworkDict (d, nodeId, &abortCode) != SDO_FINISHED)
 198                 eprintf("Master : Failed in changing Slave's transmit type AbortCode :%4.4x \n", abortCode);
 199
 200         /* Finalise last SDO transfer with this node */
 201         closeSDOtransfer(&TestMaster_Data, nodeId, SDO_CLIENT);
 202 }
 203
 204
 205 static int MasterSyncCount = 0;
 206 void TestMaster_post_TPDO(CO_Data* d)
 207 {
 208         eprintf("TestMaster_post_TPDO MasterSyncCount = %d \n", MasterSyncCount);
 209 //
 210 //      {
 211 //              char zero = 0;
 212 //              if(MasterMap4 > 0x80){
 213 //                      writeNetworkDict (
 214 //                              &TestMaster_Data,
 215 //                              TestSlave_Data->bDeviceNodeId,
 216 //                              0x2002,
 217 //                              0x00,
 218 //                              1,
 219 //                              0,
 220 //                              &zero);
 221 //              }
 222 //      }
 223
 224 #if 0
 225         if(waiting_answer){
 226                 UNS32 abortCode;
 227                 UNS8 size;
 228                 switch(getReadResultNetworkDict (
 229                         &TestMaster_Data,
 230                         0x02,
 231                         &query_result,
 232                         &size,
 233                         &abortCode))
 234                 {
 235                         case SDO_FINISHED:
 236                                 /* Do something with result here !!*/
 237                                 eprintf("Got SDO answer (0x2002, 0x00), %d %d\n",query_result,size);
 238                         case SDO_ABORTED_RCV:
 239                         case SDO_ABORTED_INTERNAL:
 240                         case SDO_RESET:
 241                                 waiting_answer = 0;
 242                                 closeSDOtransfer(
 243                                         &TestMaster_Data,
 244                                         0x02,
 245                                         SDO_CLIENT);
 246                         break;
 247                         case SDO_DOWNLOAD_IN_PROGRESS:
 248                         case SDO_UPLOAD_IN_PROGRESS:
 249                         break;
 250                 }
 251         }else if(MasterSyncCount % 10 == 0){
 252                 readNetworkDict (
 253                         &TestMaster_Data,
 254                         0x02,
 255                         0x2002,
 256                         0x00,
 257                         0);
 258                 waiting_answer = 1;
 259         }
 260 #endif
 261         if(MasterSyncCount % 17 == 0){
 262                 eprintf("Master : Ask RTR PDO (0x1402)\n");
 263                 sendPDOrequest(&TestMaster_Data, 0x1402 );
 264                 sendPDOrequest(&TestMaster_Data, 0x1403 );
 265         }
 266         if(MasterSyncCount % 50 == 0){
 267                 eprintf("Master : Change slave's transmit type to 0xFF\n");
 268                 UNS8 transmitiontype = 0xFF;
 269                 writeNetworkDictCallBack (&TestMaster_Data, /*CO_Data* d*/
 270                                         2, /*UNS8 nodeId*/
 271                                         0x1802, /*UNS16 index*/
 272                                         0x02, /*UNS16 index*/
 273                                         1, /*UNS8 count*/
 274                                         0, /*UNS8 dataType*/
 275                                         &transmitiontype,/*void *data*/
 276                                         CheckSDO); /*SDOCallback_t Callback*/
 277         }
 278         if(MasterSyncCount % 50 == 25){
 279                 eprintf("Master : Change slave's transmit type to 0x00\n");
 280                 UNS8 transmitiontype = 0x00;
 281                 writeNetworkDictCallBack (&TestMaster_Data, /*CO_Data* d*/
 282                                         2, /*UNS8 nodeId*/
 283                                         0x1802, /*UNS16 index*/
 284                                         0x02, /*UNS16 index*/
 285                                         1, /*UNS8 count*/
 286                                         0, /*UNS8 dataType*/
 287                                         &transmitiontype,/*void *data*/
 288                                         CheckSDO); /*SDOCallback_t Callback*/
 289         }
 290         MasterSyncCount++;
 291 }
 292
 293 void TestMaster_post_SlaveBootup(CO_Data* d, UNS8 nodeid)
 294 {
 295         eprintf("TestMaster_post_SlaveBootup %x\n", nodeid);
 296
 297         ConfigureSlaveNode(d, nodeid);
 298 }