CanFestival: /home/epimerde/documents/tc11/CanFestival-3/src/states.c Source File

00001 /*
nico@207: 00002 This file is part of CanFestival, a library implementing CanOpen Stack. 
nico@207: 00003 
nico@207: 00004 Copyright (C): Edouard TISSERANT and Francis DUPIN
nico@207: 00005 
nico@207: 00006 See COPYING file for copyrights details.
nico@207: 00007 
nico@207: 00008 This library is free software; you can redistribute it and/or
nico@207: 00009 modify it under the terms of the GNU Lesser General Public
nico@207: 00010 License as published by the Free Software Foundation; either
nico@207: 00011 version 2.1 of the License, or (at your option) any later version.
nico@207: 00012 
nico@207: 00013 This library is distributed in the hope that it will be useful,
nico@207: 00014 but WITHOUT ANY WARRANTY; without even the implied warranty of
nico@207: 00015 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
nico@207: 00016 Lesser General Public License for more details.
nico@207: 00017 
nico@207: 00018 You should have received a copy of the GNU Lesser General Public
nico@207: 00019 License along with this library; if not, write to the Free Software
nico@207: 00020 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
nico@207: 00021 */
nico@210: 00032 #include "states.h"
nico@210: 00033 #include "def.h"
nico@210: 00034 #include "dcf.h"
nico@210: 00035 #include "nmtSlave.h"
nico@210: 00036 
nico@210: 00044 void switchCommunicationState(CO_Data* d, 
nico@210: 00045         s_state_communication *newCommunicationState);
nico@210: 00046         
nico@210: 00054 e_nodeState getState(CO_Data* d)
nico@210: 00055 {
nico@210: 00056   return d->nodeState;
nico@210: 00057 }
nico@210: 00058 
nico@210: 00065 void canDispatch(CO_Data* d, Message *m)
nico@210: 00066 {
nico@210: 00067          switch(m->cob_id.w >> 7)
nico@210: 00068         {
nico@210: 00069                 case SYNC:
nico@210: 00070                         if(d->CurrentCommunicationState.csSYNC)
nico@210: 00071                                 proceedSYNC(d,m);
nico@210: 00072                         break;
nico@210: 00074                 case PDO1tx:
nico@210: 00075                 case PDO1rx:
nico@210: 00076                 case PDO2tx:
nico@210: 00077                 case PDO2rx:
nico@210: 00078                 case PDO3tx:
nico@210: 00079                 case PDO3rx:
nico@210: 00080                 case PDO4tx:
nico@210: 00081                 case PDO4rx:
nico@210: 00082                         if (d->CurrentCommunicationState.csPDO)
nico@210: 00083                                 proceedPDO(d,m);
nico@210: 00084                         break;
nico@210: 00085                 case SDOtx:
nico@210: 00086                 case SDOrx:
nico@210: 00087                         if (d->CurrentCommunicationState.csSDO)
nico@210: 00088                                 proceedSDO(d,m);
nico@210: 00089                         break;
nico@210: 00090                 case NODE_GUARD:
nico@210: 00091                         if (d->CurrentCommunicationState.csHeartbeat)
nico@210: 00092                                 proceedNODE_GUARD(d,m);
nico@210: 00093                         break;
nico@210: 00094                 case NMT:
nico@210: 00095                         if (*(d->iam_a_slave))
nico@210: 00096                         {
nico@210: 00097                                 proceedNMTstateChange(d,m);
nico@210: 00098                         }
nico@210: 00099         }
nico@210: 00100 }
nico@210: 00101 
nico@210: 00102 #define StartOrStop(CommType, FuncStart, FuncStop) \
nico@210: 00103         if(newCommunicationState->CommType && !d->CurrentCommunicationState.CommType){\
nico@210: 00104                 MSG_WAR(0x9999,#FuncStart, 9999);\
nico@210: 00105                 d->CurrentCommunicationState.CommType = 1;\
nico@210: 00106                 FuncStart;\
nico@210: 00107         }else if(!newCommunicationState->CommType && d->CurrentCommunicationState.CommType){\
nico@210: 00108                 MSG_WAR(0x9999,#FuncStop, 9999);\
nico@210: 00109                 d->CurrentCommunicationState.CommType = 0;\
nico@210: 00110                 FuncStop;\
nico@210: 00111         }
nico@210: 00112 #define None
nico@210: 00113 
nico@210: 00120 void switchCommunicationState(CO_Data* d, s_state_communication *newCommunicationState)
nico@210: 00121 {
nico@210: 00122         StartOrStop(csSDO,      None,           resetSDO(d))
nico@210: 00123         StartOrStop(csSYNC,     startSYNC(d),           stopSYNC(d))
nico@210: 00124         StartOrStop(csHeartbeat,        heartbeatInit(d),       heartbeatStop(d))
nico@210: 00125 /*      StartOrStop(Emergency,,) */
nico@210: 00126         StartOrStop(csPDO,      None,   None)
nico@210: 00127         StartOrStop(csBoot_Up,  None,   slaveSendBootUp(d))
nico@210: 00128 }
nico@210: 00129 
nico@210: 00138 UNS8 setState(CO_Data* d, e_nodeState newState)
nico@210: 00139 {
nico@210: 00140         UNS16 wIndex = 0x1F22;
nico@210: 00141         const indextable *ptrTable;
nico@210: 00142         ODCallback_t *Callback;
nico@210: 00143         UNS32 errorCode;
nico@210: 00144         while(newState != d->nodeState){
nico@210: 00145                 switch( newState ){
nico@210: 00146                         case Initialisation:
nico@210: 00147                         {
nico@210: 00148                                 s_state_communication newCommunicationState = {1, 0, 0, 0, 0, 0};
nico@210: 00150                                 d->nodeState = Initialisation;
nico@210: 00151                                 newState = Pre_operational;
nico@210: 00152                                 switchCommunicationState(d, &newCommunicationState);
nico@210: 00154                                 (*d->initialisation)();
nico@210: 00155                                 
nico@210: 00156                         }
nico@210: 00157                         break;
nico@210: 00158                                                                 
nico@210: 00159                         case Pre_operational:
nico@210: 00160                         {
nico@210: 00161                                 
nico@210: 00162                                 s_state_communication newCommunicationState = {0, 1, 1, 1, 1, 0};
nico@210: 00163                                 d->nodeState = Pre_operational;
nico@210: 00164                                 newState = Pre_operational;
nico@210: 00165                                 switchCommunicationState(d, &newCommunicationState);
nico@210: 00166                                 if (!(*(d->iam_a_slave)))
nico@210: 00167                                 {
nico@210: 00168                                         ptrTable =(*d->scanIndexOD)(wIndex, &errorCode, &Callback);
nico@210: 00169                                         
nico@210: 00170                                         if (errorCode != OD_SUCCESSFUL)
nico@210: 00171                                                 {
nico@210: 00172                                                         (*d->preOperational)();
nico@210: 00173                                                 }
nico@210: 00174                                         else
nico@210: 00175                                                 {
nico@210: 00176                                                         UNS32 res;
nico@210: 00177                                                         res = decompo_dcf(d,0x01);
nico@210: 00178                                                 }                               
nico@210: 00179                                 }
nico@210: 00180                                 else 
nico@210: 00181                                 {
nico@210: 00182                                         (*d->preOperational)();
nico@210: 00183                                 }
nico@210: 00184                         }
nico@210: 00185                         break;
nico@210: 00186                                                                 
nico@210: 00187                         case Operational:
nico@210: 00188                         if(d->nodeState == Initialisation) return 0xFF;
nico@210: 00189                         {
nico@210: 00190                                 s_state_communication newCommunicationState = {0, 1, 1, 1, 1, 1};
nico@210: 00191                                 d->nodeState = Operational;
nico@210: 00192                                 newState = Operational;
nico@210: 00193                                 switchCommunicationState(d, &newCommunicationState);
nico@210: 00194                                 (*d->operational)();
nico@210: 00195                         }
nico@210: 00196                         break;
nico@210: 00197                                                 
nico@210: 00198                         case Stopped:
nico@210: 00199                         if(d->nodeState == Initialisation) return 0xFF;
nico@210: 00200                         {
nico@210: 00201                                 s_state_communication newCommunicationState = {0, 0, 0, 0, 1, 0};
nico@210: 00202                                 d->nodeState = Stopped;
nico@210: 00203                                 newState = Stopped;
nico@210: 00204                                 switchCommunicationState(d, &newCommunicationState);
nico@210: 00205                                 (*d->stopped)();
nico@210: 00206                         }
nico@210: 00207                         break;
nico@210: 00208                         
nico@210: 00209                         default:
nico@210: 00210                                 return 0xFF;
nico@210: 00211                 }
nico@210: 00213         }
nico@210: 00214         return 0;
nico@210: 00215 }
nico@210: 00216 
nico@210: 00224 UNS8 getNodeId(CO_Data* d)
nico@210: 00225 {
nico@210: 00226   return *d->bDeviceNodeId;
nico@210: 00227 }
nico@210: 00228 
nico@210: 00235 void setNodeId(CO_Data* d, UNS8 nodeId)
nico@210: 00236 {
nico@210: 00237   UNS16 offset = d->firstIndex->SDO_SVR;
nico@210: 00238   if(offset){
nico@210: 00240       *(UNS32*)d->objdict[offset].pSubindex[1].pObject = 0x600 + nodeId;
nico@210: 00242       *(UNS32*)d->objdict[offset].pSubindex[2].pObject = 0x580 + nodeId;
nico@210: 00245   }
nico@210: 00246 
nico@210: 00255   {
nico@210: 00256     UNS8 i = 0;
nico@210: 00257     UNS16 offset = d->firstIndex->PDO_RCV;
nico@210: 00258     UNS16 lastIndex = d->lastIndex->PDO_RCV;
nico@210: 00259     UNS32 cobID[] = {0x200, 0x300, 0x400, 0x500};
nico@210: 00260     if( offset ) while( (offset <= lastIndex) && (i < 4)) {
nico@210: 00261       //if(*(UNS32*)d->objdict[offset].pSubindex[1].pObject == cobID[i] + *d->bDeviceNodeId)
nico@210: 00262               *(UNS32*)d->objdict[offset].pSubindex[1].pObject = cobID[i] + nodeId;
nico@210: 00263       i ++;
nico@210: 00264       offset ++;
nico@210: 00265     }
nico@210: 00266   }
nico@210: 00267   /* ** Initialize the transmit PDO communication parameters. Only for 0x1800 to 0x1803 */
nico@210: 00268   {
nico@210: 00269     UNS8 i = 0;
nico@210: 00270     UNS16 offset = d->firstIndex->PDO_TRS;
nico@210: 00271     UNS16 lastIndex = d->lastIndex->PDO_TRS;
nico@210: 00272     UNS32 cobID[] = {0x180, 0x280, 0x380, 0x480};
nico@210: 00273     i = 0;
nico@210: 00274     if( offset ) while ((offset <= lastIndex) && (i < 4)) {
nico@210: 00275       //if(*(UNS32*)d->objdict[offset].pSubindex[1].pObject == cobID[i] + *d->bDeviceNodeId)
nico@210: 00276               *(UNS32*)d->objdict[offset].pSubindex[1].pObject = cobID[i] + nodeId;
nico@210: 00277       i ++;
nico@210: 00278       offset ++;
nico@210: 00279     }
nico@210: 00280   }
nico@210: 00282   *d->bDeviceNodeId = nodeId;
nico@210: 00283 }
nico@210: 00284 
nico@210: 00285 void _initialisation(){}
nico@210: 00286 void _preOperational(){}
nico@210: 00287 void _operational(){}
nico@210: 00288 void _stopped(){}
nico@210: