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 ** @file   states.c
  24 ** @author Edouard TISSERANT and Francis DUPIN
  25 ** @date   Tue Jun  5 09:32:32 2007
  26 **
  27 ** @brief
  28 **
  29 **
  30 */
  31
  32 #include "data.h"
  33 #include "sysdep.h"
  34
  35 /** Prototypes for internals functions */
  36 /*!
  37 **
  38 **
  39 ** @param d
  40 ** @param newCommunicationState
  41 **/
  42 void switchCommunicationState(CO_Data* d,
  43         s_state_communication *newCommunicationState);
  44
  45 /*!
  46 **
  47 **
  48 ** @param d
  49 **
  50 ** @return
  51 **/
  52 e_nodeState getState(CO_Data* d)
  53 {
  54   return d->nodeState;
  55 }
  56
  57 /*!
  58 **
  59 **
  60 ** @param d
  61 ** @param m
  62 **/
  63 void canDispatch(CO_Data* d, Message *m)
  64 {
  65         UNS16 cob_id = UNS16_LE(m->cob_id);
  66          switch(cob_id >> 7)
  67         {
  68                 case SYNC:              /* can be a SYNC or a EMCY message */
  69                         if(cob_id == 0x080)     /* SYNC */
  70                         {
  71                                 if(d->CurrentCommunicationState.csSYNC)
  72                                         proceedSYNC(d);
  73                         } else          /* EMCY */
  74                                 if(d->CurrentCommunicationState.csEmergency)
  75                                         proceedEMCY(d,m);
  76                         break;
  77                 /* case TIME_STAMP: */
  78                 case PDO1tx:
  79                 case PDO1rx:
  80                 case PDO2tx:
  81                 case PDO2rx:
  82                 case PDO3tx:
  83                 case PDO3rx:
  84                 case PDO4tx:
  85                 case PDO4rx:
  86                         if (d->CurrentCommunicationState.csPDO)
  87                                 proceedPDO(d,m);
  88                         break;
  89                 case SDOtx:
  90                 case SDOrx:
  91                         if (d->CurrentCommunicationState.csSDO)
  92                                 proceedSDO(d,m);
  93                         break;
  94                 case NODE_GUARD:
  95                         if (d->CurrentCommunicationState.csHeartbeat)
  96                                 proceedNODE_GUARD(d,m);
  97                         break;
  98                 case NMT:
  99                         if (*(d->iam_a_slave))
 100                         {
 101                                 proceedNMTstateChange(d,m);
 102                         }
 103 #ifdef CO_ENABLE_LSS
 104                 case LSS:
 105                         if (!d->CurrentCommunicationState.csLSS)break;
 106                         if ((*(d->iam_a_slave)) && cob_id==MLSS_ADRESS)
 107                         {
 108                                 proceedLSS_Slave(d,m);
 109                         }
 110                         else if(!(*(d->iam_a_slave)) && cob_id==SLSS_ADRESS)
 111                         {
 112                                 proceedLSS_Master(d,m);
 113                         }
 114                         break;
 115 #endif
 116         }
 117 }
 118
 119 #define StartOrStop(CommType, FuncStart, FuncStop) \
 120         if(newCommunicationState->CommType && d->CurrentCommunicationState.CommType == 0){\
 121                 MSG_WAR(0x9999,#FuncStart, 9999);\
 122                 d->CurrentCommunicationState.CommType = 1;\
 123                 FuncStart;\
 124         }else if(!newCommunicationState->CommType && d->CurrentCommunicationState.CommType == 1){\
 125                 MSG_WAR(0x9999,#FuncStop, 9999);\
 126                 d->CurrentCommunicationState.CommType = 0;\
 127                 FuncStop;\
 128         }
 129 #define None
 130
 131 /*!
 132 **
 133 **
 134 ** @param d
 135 ** @param newCommunicationState
 136 **/
 137 void switchCommunicationState(CO_Data* d, s_state_communication *newCommunicationState)
 138 {
 139 #ifdef CO_ENABLE_LSS
 140         StartOrStop(csLSS,      startLSS(d),    stopLSS(d))
 141 #endif
 142         StartOrStop(csSDO,      None,           resetSDO(d))
 143         StartOrStop(csSYNC,     startSYNC(d),           stopSYNC(d))
 144         StartOrStop(csHeartbeat,        heartbeatInit(d),       heartbeatStop(d))
 145         StartOrStop(csEmergency,        emergencyInit(d),       emergencyStop(d))
 146         StartOrStop(csPDO,      PDOInit(d),     PDOStop(d))
 147         StartOrStop(csBoot_Up,  None,   slaveSendBootUp(d))
 148 }
 149
 150 /*!
 151 **
 152 **
 153 ** @param d
 154 ** @param newState
 155 **
 156 ** @return
 157 **/
 158 UNS8 setState(CO_Data* d, e_nodeState newState)
 159 {
 160         if(newState != d->nodeState){
 161                 switch( newState ){
 162                         case Initialisation:
 163                         {
 164                                 s_state_communication newCommunicationState = {1, 0, 0, 0, 0, 0, 0};
 165                                 d->nodeState = Initialisation;
 166                                 switchCommunicationState(d, &newCommunicationState);
 167                                 /* call user app init callback now. */
 168                                 /* d->initialisation MUST NOT CALL SetState */
 169                                 (*d->initialisation)(d);
 170                         }
 171
 172                         /* Automatic transition - No break statement ! */
 173                         /* Transition from Initialisation to Pre_operational */
 174                         /* is automatic as defined in DS301. */
 175                         /* App don't have to call SetState(d, Pre_operational) */
 176
 177                         case Pre_operational:
 178                         {
 179
 180                                 s_state_communication newCommunicationState = {0, 1, 1, 1, 1, 0, 1};
 181                                 d->nodeState = Pre_operational;
 182                                 switchCommunicationState(d, &newCommunicationState);
 183                                 if (!(*(d->iam_a_slave)))
 184                                 {
 185                                         masterSendNMTstateChange (d, 0, NMT_Reset_Node);
 186                                 }
 187                 (*d->preOperational)(d);
 188                         }
 189                         break;
 190
 191                         case Operational:
 192                         if(d->nodeState == Initialisation) return 0xFF;
 193                         {
 194                                 s_state_communication newCommunicationState = {0, 1, 1, 1, 1, 1, 0};
 195                                 d->nodeState = Operational;
 196                                 newState = Operational;
 197                                 switchCommunicationState(d, &newCommunicationState);
 198                                 (*d->operational)(d);
 199                         }
 200                         break;
 201
 202                         case Stopped:
 203                         if(d->nodeState == Initialisation) return 0xFF;
 204                         {
 205                                 s_state_communication newCommunicationState = {0, 0, 0, 0, 1, 0, 1};
 206                                 d->nodeState = Stopped;
 207                                 newState = Stopped;
 208                                 switchCommunicationState(d, &newCommunicationState);
 209                                 (*d->stopped)(d);
 210                         }
 211                         break;
 212                         default:
 213                                 return 0xFF;
 214
 215                 }/* end switch case */
 216
 217         }
 218         /* d->nodeState contains the final state */
 219         /* may not be the requested state */
 220     return d->nodeState;
 221 }
 222
 223 /*!
 224 **
 225 **
 226 ** @param d
 227 **
 228 ** @return
 229 **/
 230 UNS8 getNodeId(CO_Data* d)
 231 {
 232   return *d->bDeviceNodeId;
 233 }
 234
 235 /*!
 236 **
 237 **
 238 ** @param d
 239 ** @param nodeId
 240 **/
 241 void setNodeId(CO_Data* d, UNS8 nodeId)
 242 {
 243   UNS16 offset = d->firstIndex->SDO_SVR;
 244
 245 #ifdef CO_ENABLE_LSS
 246   d->lss_transfer.nodeID=nodeId;
 247   if(nodeId==0xFF){
 248         *d->bDeviceNodeId = nodeId;
 249         return;
 250   }
 251   else
 252 #endif
 253   if(!(nodeId>0 && nodeId<=127)){
 254           MSG_WAR(0x2D01, "Invalid NodeID",nodeId);
 255           return;
 256   }
 257
 258   if(offset){
 259     /* Adjust COB-ID Client->Server (rx) only id already set to default value or id not valid (id==0xFF)*/
 260     if((*(UNS32*)d->objdict[offset].pSubindex[1].pObject == 0x600 + *d->bDeviceNodeId)||(*d->bDeviceNodeId==0xFF)){
 261       /* cob_id_client = 0x600 + nodeId; */
 262       *(UNS32*)d->objdict[offset].pSubindex[1].pObject = 0x600 + nodeId;
 263     }
 264     /* Adjust COB-ID Server -> Client (tx) only id already set to default value or id not valid (id==0xFF)*/
 265     if((*(UNS32*)d->objdict[offset].pSubindex[2].pObject == 0x580 + *d->bDeviceNodeId)||(*d->bDeviceNodeId==0xFF)){
 266       /* cob_id_server = 0x580 + nodeId; */
 267       *(UNS32*)d->objdict[offset].pSubindex[2].pObject = 0x580 + nodeId;
 268     }
 269   }
 270
 271   /*
 272         Initialize the server(s) SDO parameters
 273         Remember that only one SDO server is allowed, defined at index 0x1200
 274
 275         Initialize the client(s) SDO parameters
 276         Nothing to initialize (no default values required by the DS 401)
 277         Initialize the receive PDO communication parameters. Only for 0x1400 to 0x1403
 278   */
 279   {
 280     UNS8 i = 0;
 281     UNS16 offset = d->firstIndex->PDO_RCV;
 282     UNS16 lastIndex = d->lastIndex->PDO_RCV;
 283     UNS32 cobID[] = {0x200, 0x300, 0x400, 0x500};
 284     if( offset ) while( (offset <= lastIndex) && (i < 4)) {
 285       if((*(UNS32*)d->objdict[offset].pSubindex[1].pObject == cobID[i] + *d->bDeviceNodeId)||(*d->bDeviceNodeId==0xFF))
 286               *(UNS32*)d->objdict[offset].pSubindex[1].pObject = cobID[i] + nodeId;
 287       i ++;
 288       offset ++;
 289     }
 290   }
 291   /* ** Initialize the transmit PDO communication parameters. Only for 0x1800 to 0x1803 */
 292   {
 293     UNS8 i = 0;
 294     UNS16 offset = d->firstIndex->PDO_TRS;
 295     UNS16 lastIndex = d->lastIndex->PDO_TRS;
 296     UNS32 cobID[] = {0x180, 0x280, 0x380, 0x480};
 297     i = 0;
 298     if( offset ) while ((offset <= lastIndex) && (i < 4)) {
 299       if((*(UNS32*)d->objdict[offset].pSubindex[1].pObject == cobID[i] + *d->bDeviceNodeId)||(*d->bDeviceNodeId==0xFF))
 300               *(UNS32*)d->objdict[offset].pSubindex[1].pObject = cobID[i] + nodeId;
 301       i ++;
 302       offset ++;
 303     }
 304   }
 305
 306   /* Update EMCY COB-ID if already set to default*/
 307   if((*d->error_cobid == *d->bDeviceNodeId + 0x80)||(*d->bDeviceNodeId==0xFF))
 308     *d->error_cobid = nodeId + 0x80;
 309
 310   /* bDeviceNodeId is defined in the object dictionary. */
 311   *d->bDeviceNodeId = nodeId;
 312 }
 313
 314 void _initialisation(CO_Data* d){}
 315 void _preOperational(CO_Data* d){}
 316 void _operational(CO_Data* d){}
 317 void _stopped(CO_Data* d){}