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 "states.h"
  33 #include "def.h"
  34 #include "dcf.h"
  35 #include "nmtSlave.h"
  36
  37 /** Prototypes for internals functions */
  38 /*!
  39 **
  40 **
  41 ** @param d
  42 ** @param newCommunicationState
  43 **/
  44 void switchCommunicationState(CO_Data* d,
  45         s_state_communication *newCommunicationState);
  46
  47 /*!
  48 **
  49 **
  50 ** @param d
  51 **
  52 ** @return
  53 **/
  54 e_nodeState getState(CO_Data* d)
  55 {
  56   return d->nodeState;
  57 }
  58
  59 /*!
  60 **
  61 **
  62 ** @param d
  63 ** @param m
  64 **/
  65 void canDispatch(CO_Data* d, Message *m)
  66 {
  67          switch(m->cob_id.w >> 7)
  68         {
  69                 case SYNC:
  70                         if(d->CurrentCommunicationState.csSYNC)
  71                                 proceedSYNC(d,m);
  72                         break;
  73                 /** case TIME_STAMP: */
  74                 case PDO1tx:
  75                 case PDO1rx:
  76                 case PDO2tx:
  77                 case PDO2rx:
  78                 case PDO3tx:
  79                 case PDO3rx:
  80                 case PDO4tx:
  81                 case PDO4rx:
  82                         if (d->CurrentCommunicationState.csPDO)
  83                                 proceedPDO(d,m);
  84                         break;
  85                 case SDOtx:
  86                 case SDOrx:
  87                         if (d->CurrentCommunicationState.csSDO)
  88                                 proceedSDO(d,m);
  89                         break;
  90                 case NODE_GUARD:
  91                         if (d->CurrentCommunicationState.csHeartbeat)
  92                                 proceedNODE_GUARD(d,m);
  93                         break;
  94                 case NMT:
  95                         if (*(d->iam_a_slave))
  96                         {
  97                                 proceedNMTstateChange(d,m);
  98                         }
  99         }
 100 }
 101
 102 #define StartOrStop(CommType, FuncStart, FuncStop) \
 103         if(newCommunicationState->CommType && !d->CurrentCommunicationState.CommType){\
 104                 MSG_WAR(0x9999,#FuncStart, 9999);\
 105                 d->CurrentCommunicationState.CommType = 1;\
 106                 FuncStart;\
 107         }else if(!newCommunicationState->CommType && d->CurrentCommunicationState.CommType){\
 108                 MSG_WAR(0x9999,#FuncStop, 9999);\
 109                 d->CurrentCommunicationState.CommType = 0;\
 110                 FuncStop;\
 111         }
 112 #define None
 113
 114 /*!
 115 **
 116 **
 117 ** @param d
 118 ** @param newCommunicationState
 119 **/
 120 void switchCommunicationState(CO_Data* d, s_state_communication *newCommunicationState)
 121 {
 122         StartOrStop(csSDO,      None,           resetSDO(d))
 123         StartOrStop(csSYNC,     startSYNC(d),           stopSYNC(d))
 124         StartOrStop(csHeartbeat,        heartbeatInit(d),       heartbeatStop(d))
 125 /*      StartOrStop(Emergency,,) */
 126         StartOrStop(csPDO,      None,   None)
 127         StartOrStop(csBoot_Up,  None,   slaveSendBootUp(d))
 128 }
 129
 130 /*!
 131 **
 132 **
 133 ** @param d
 134 ** @param newState
 135 **
 136 ** @return
 137 **/
 138 UNS8 setState(CO_Data* d, e_nodeState newState)
 139 {
 140         UNS16 wIndex = 0x1F22;
 141         const indextable *ptrTable;
 142         ODCallback_t *Callback;
 143         UNS32 errorCode;
 144         while(newState != d->nodeState){
 145                 switch( newState ){
 146                         case Initialisation:
 147                         {
 148                                 s_state_communication newCommunicationState = {1, 0, 0, 0, 0, 0};
 149                                 /** This will force a second loop for the state switch */
 150                                 d->nodeState = Initialisation;
 151                                 newState = Pre_operational;
 152                                 switchCommunicationState(d, &newCommunicationState);
 153                                 /** call user app related state func. */
 154                                 (*d->initialisation)();
 155
 156                         }
 157                         break;
 158
 159                         case Pre_operational:
 160                         {
 161
 162                                 s_state_communication newCommunicationState = {0, 1, 1, 1, 1, 0};
 163                                 d->nodeState = Pre_operational;
 164                                 newState = Pre_operational;
 165                                 switchCommunicationState(d, &newCommunicationState);
 166                                 if (!(*(d->iam_a_slave)))
 167                                 {
 168                                         ptrTable =(*d->scanIndexOD)(wIndex, &errorCode, &Callback);
 169
 170                                         if (errorCode != OD_SUCCESSFUL)
 171                                                 {
 172                                                         (*d->preOperational)();
 173                                                 }
 174                                         else
 175                                                 {
 176                                                         UNS32 res;
 177                                                         res = decompo_dcf(d,0x01);
 178                                                 }
 179                                 }
 180                                 else
 181                                 {
 182                                         (*d->preOperational)();
 183                                 }
 184                         }
 185                         break;
 186
 187                         case Operational:
 188                         if(d->nodeState == Initialisation) return 0xFF;
 189                         {
 190                                 s_state_communication newCommunicationState = {0, 1, 1, 1, 1, 1};
 191                                 d->nodeState = Operational;
 192                                 newState = Operational;
 193                                 switchCommunicationState(d, &newCommunicationState);
 194                                 (*d->operational)();
 195                         }
 196                         break;
 197
 198                         case Stopped:
 199                         if(d->nodeState == Initialisation) return 0xFF;
 200                         {
 201                                 s_state_communication newCommunicationState = {0, 0, 0, 0, 1, 0};
 202                                 d->nodeState = Stopped;
 203                                 newState = Stopped;
 204                                 switchCommunicationState(d, &newCommunicationState);
 205                                 (*d->stopped)();
 206                         }
 207                         break;
 208
 209                         default:
 210                                 return 0xFF;
 211                 }/** end switch case */
 212
 213         }
 214         return 0;
 215 }
 216
 217 /*!
 218 **
 219 **
 220 ** @param d
 221 **
 222 ** @return
 223 **/
 224 UNS8 getNodeId(CO_Data* d)
 225 {
 226   return *d->bDeviceNodeId;
 227 }
 228
 229 /*!
 230 **
 231 **
 232 ** @param d
 233 ** @param nodeId
 234 **/
 235 void setNodeId(CO_Data* d, UNS8 nodeId)
 236 {
 237   UNS16 offset = d->firstIndex->SDO_SVR;
 238   if(offset){
 239       /** cob_id_client = 0x600 + nodeId; */
 240       *(UNS32*)d->objdict[offset].pSubindex[1].pObject = 0x600 + nodeId;
 241       /** cob_id_server = 0x580 + nodeId; */
 242       *(UNS32*)d->objdict[offset].pSubindex[2].pObject = 0x580 + nodeId;
 243       /** node Id client. As we do not know the value, we put the node Id Server */
 244       /** *(UNS8*)d->objdict[offset].pSubindex[3].pObject = nodeId; */
 245   }
 246
 247   /**
 248         Initialize the server(s) SDO parameters
 249         Remember that only one SDO server is allowed, defined at index 0x1200
 250
 251         Initialize the client(s) SDO parameters
 252         Nothing to initialize (no default values required by the DS 401)
 253         Initialize the receive PDO communication parameters. Only for 0x1400 to 0x1403
 254   */
 255   {
 256     UNS8 i = 0;
 257     UNS16 offset = d->firstIndex->PDO_RCV;
 258     UNS16 lastIndex = d->lastIndex->PDO_RCV;
 259     UNS32 cobID[] = {0x200, 0x300, 0x400, 0x500};
 260     if( offset ) while( (offset <= lastIndex) && (i < 4)) {
 261       //if(*(UNS32*)d->objdict[offset].pSubindex[1].pObject == cobID[i] + *d->bDeviceNodeId)
 262               *(UNS32*)d->objdict[offset].pSubindex[1].pObject = cobID[i] + nodeId;
 263       i ++;
 264       offset ++;
 265     }
 266   }
 267   /* ** Initialize the transmit PDO communication parameters. Only for 0x1800 to 0x1803 */
 268   {
 269     UNS8 i = 0;
 270     UNS16 offset = d->firstIndex->PDO_TRS;
 271     UNS16 lastIndex = d->lastIndex->PDO_TRS;
 272     UNS32 cobID[] = {0x180, 0x280, 0x380, 0x480};
 273     i = 0;
 274     if( offset ) while ((offset <= lastIndex) && (i < 4)) {
 275       //if(*(UNS32*)d->objdict[offset].pSubindex[1].pObject == cobID[i] + *d->bDeviceNodeId)
 276               *(UNS32*)d->objdict[offset].pSubindex[1].pObject = cobID[i] + nodeId;
 277       i ++;
 278       offset ++;
 279     }
 280   }
 281   /** bDeviceNodeId is defined in the object dictionary. */
 282   *d->bDeviceNodeId = nodeId;
 283 }
 284
 285 void _initialisation(){}
 286 void _preOperational(){}
 287 void _operational(){}
 288 void _stopped(){}