src/states.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 "states.h"
  24 #include "def.h"
  25
  26 #include "nmtSlave.h"
  27
  28 /* Prototypes for internals functions */
  29 void switchCommunicationState(CO_Data* d,
  30         s_state_communication *newCommunicationState);
  31
  32 /*****************************************************************************/
  33 e_nodeState getState(CO_Data* d)
  34 {
  35   return d->nodeState;
  36 }
  37
  38 /*****************************************************************************/
  39 void canDispatch(CO_Data* d, Message *m)
  40 {
  41          switch(m->cob_id.w >> 7)
  42         {
  43                 case SYNC:
  44                         if(d->CurrentCommunicationState.csSYNC)
  45                                 proceedSYNC(d,m);
  46                         break;
  47                 /* case TIME_STAMP: */
  48                 case PDO1tx:
  49                 case PDO1rx:
  50                 case PDO2tx:
  51                 case PDO2rx:
  52                 case PDO3tx:
  53                 case PDO3rx:
  54                 case PDO4tx:
  55                 case PDO4rx:
  56                         if (d->CurrentCommunicationState.csPDO)
  57                                 proceedPDO(d,m);
  58                         break;
  59                 case SDOtx:
  60                 case SDOrx:
  61                         if (d->CurrentCommunicationState.csSDO)
  62                                 proceedSDO(d,m);
  63                         break;
  64                 case NODE_GUARD:
  65                         if (d->CurrentCommunicationState.csHeartbeat)
  66                                 proceedNODE_GUARD(d,m);
  67                         break;
  68                 case NMT:
  69                         if (*(d->iam_a_slave))
  70                         {
  71                                 proceedNMTstateChange(d,m);
  72                         }
  73         }
  74 }
  75
  76 #define StartOrStop(CommType, FuncStart, FuncStop) \
  77         if(newCommunicationState->CommType && !d->CurrentCommunicationState.CommType){\
  78                 MSG_ERR(0x9999,#FuncStart, 9999);\
  79                 d->CurrentCommunicationState.CommType = 1;\
  80                 FuncStart;\
  81         }else if(!newCommunicationState->CommType && d->CurrentCommunicationState.CommType){\
  82                 MSG_ERR(0x9999,#FuncStop, 9999);\
  83                 d->CurrentCommunicationState.CommType = 0;\
  84                 FuncStop;\
  85         }
  86 #define None
  87
  88 /*****************************************************************************/
  89 void switchCommunicationState(CO_Data* d, s_state_communication *newCommunicationState)
  90 {
  91         StartOrStop(csSDO,      None,           resetSDO(d))
  92         StartOrStop(csSYNC,     startSYNC(d),           stopSYNC(d))
  93         StartOrStop(csHeartbeat,        heartbeatInit(d),       heartbeatStop(d))
  94 /*      StartOrStop(Emergency,,) */
  95         StartOrStop(csPDO,      None,   None)
  96         StartOrStop(csBoot_Up,  None,   slaveSendBootUp(d))
  97 }
  98
  99 /*****************************************************************************/
 100 UNS8 setState(CO_Data* d, e_nodeState newState)
 101 {
 102         while(newState != d->nodeState){
 103                 switch( newState ){
 104                         case Initialisation:
 105                         {
 106                                 s_state_communication newCommunicationState = {1, 0, 0, 0, 0, 0};
 107                                 /* This will force a second loop for the state switch */
 108                                 d->nodeState = Initialisation;
 109                                 newState = Pre_operational;
 110                                 switchCommunicationState(d, &newCommunicationState);
 111                                 /* call user app related state func. */
 112                                 (*d->initialisation)();
 113                         }
 114                         break;
 115
 116                         case Pre_operational:
 117                         {
 118                                 s_state_communication newCommunicationState = {0, 1, 1, 1, 1, 0};
 119                                 d->nodeState = Pre_operational;
 120                                 newState = Pre_operational;
 121                                 switchCommunicationState(d, &newCommunicationState);
 122                                 (*d->preOperational)();
 123                         }
 124                         break;
 125
 126                         case Operational:
 127                         if(d->nodeState == Initialisation) return 0xFF;
 128                         {
 129                                 s_state_communication newCommunicationState = {0, 1, 1, 1, 1, 1};
 130                                 d->nodeState = Operational;
 131                                 newState = Operational;
 132                                 switchCommunicationState(d, &newCommunicationState);
 133                                 (*d->operational)();
 134                         }
 135                         break;
 136
 137                         case Stopped:
 138                         if(d->nodeState == Initialisation) return 0xFF;
 139                         {
 140                                 s_state_communication newCommunicationState = {0, 0, 0, 0, 1, 0};
 141                                 d->nodeState = Stopped;
 142                                 newState = Stopped;
 143                                 switchCommunicationState(d, &newCommunicationState);
 144                                 (*d->stopped)();
 145                         }
 146                         break;
 147
 148                         default:
 149                                 return 0xFF;
 150                 }/* end switch case */
 151
 152         }
 153         return 0;
 154 }
 155
 156 /*****************************************************************************/
 157 UNS8 getNodeId(CO_Data* d)
 158 {
 159   return *d->bDeviceNodeId;
 160 }
 161
 162 /*****************************************************************************/
 163 void setNodeId(CO_Data* d, UNS8 nodeId)
 164 {
 165   UNS16 offset = d->firstIndex->SDO_SVR;
 166   if(offset){
 167       /* cob_id_client = 0x600 + nodeId; */
 168       *(UNS32*)d->objdict[offset].pSubindex[1].pObject = 0x600 + nodeId;
 169       /* cob_id_server = 0x580 + nodeId; */
 170       *(UNS32*)d->objdict[offset].pSubindex[2].pObject = 0x580 + nodeId;
 171       /* node Id client. As we do not know the value, we put the node Id Server */
 172       /* *(UNS8*)d->objdict[offset].pSubindex[3].pObject = nodeId; */
 173   }
 174
 175   /* ** Initialize the server(s) SDO parameters */
 176   /* Remember that only one SDO server is allowed, defined at index 0x1200 */
 177
 178   /* ** Initialize the client(s) SDO parameters  */
 179   /* Nothing to initialize (no default values required by the DS 401) */
 180   /* ** Initialize the receive PDO communication parameters. Only for 0x1400 to 0x1403 */
 181   {
 182     UNS8 i = 0;
 183     UNS16 offset = d->firstIndex->PDO_RCV;
 184     UNS16 lastIndex = d->lastIndex->PDO_RCV;
 185     UNS32 cobID[] = {0x200, 0x300, 0x400, 0x500};
 186     if( offset ) while( (offset <= lastIndex) && (i < 4)) {
 187       if(*(UNS32*)d->objdict[offset].pSubindex[1].pObject == cobID[i] + *d->bDeviceNodeId)
 188               *(UNS32*)d->objdict[offset].pSubindex[1].pObject = cobID[i] + nodeId;
 189       i ++;
 190       offset ++;
 191     }
 192   }
 193   /* ** Initialize the transmit PDO communication parameters. Only for 0x1800 to 0x1803 */
 194   {
 195     UNS8 i = 0;
 196     UNS16 offset = d->firstIndex->PDO_TRS;
 197     UNS16 lastIndex = d->lastIndex->PDO_TRS;
 198     UNS32 cobID[] = {0x180, 0x280, 0x380, 0x480};
 199     i = 0;
 200     if( offset ) while ((offset <= lastIndex) && (i < 4)) {
 201       if(*(UNS32*)d->objdict[offset].pSubindex[1].pObject == cobID[i] + *d->bDeviceNodeId)
 202               *(UNS32*)d->objdict[offset].pSubindex[1].pObject = cobID[i] + nodeId;
 203       i ++;
 204       offset ++;
 205     }
 206   }
 207   /* bDeviceNodeId is defined in the object dictionary. */
 208   *d->bDeviceNodeId = nodeId;
 209 }
 210
 211 void _initialisation(){}
 212 void _preOperational(){}
 213 void _operational(){}
 214 void _stopped(){}