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

00001 /*
+00002 This file is part of CanFestival, a library implementing CanOpen Stack. 
+00003 
+00004 Copyright (C): Edouard TISSERANT and Francis DUPIN
+00005 
+00006 See COPYING file for copyrights details.
+00007 
+00008 This library is free software; you can redistribute it and/or
+00009 modify it under the terms of the GNU Lesser General Public
+00010 License as published by the Free Software Foundation; either
+00011 version 2.1 of the License, or (at your option) any later version.
+00012 
+00013 This library is distributed in the hope that it will be useful,
+00014 but WITHOUT ANY WARRANTY; without even the implied warranty of
+00015 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+00016 Lesser General Public License for more details.
+00017 
+00018 You should have received a copy of the GNU Lesser General Public
+00019 License along with this library; if not, write to the Free Software
+00020 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+00021 */
+00022 
+00023 #include "states.h"
+00024 #include "def.h"
+00025 #include "dcf.h"
+00026 #include "nmtSlave.h"
+00027 
+00028 /* Prototypes for internals functions */
+00029 void switchCommunicationState(CO_Data* d, 
+00030         s_state_communication *newCommunicationState);
+00031         
+00032 /*****************************************************************************/
+00033 e_nodeState getState(CO_Data* d)
+00034 {
+00035   return d->nodeState;
+00036 }
+00037 
+00038 /*****************************************************************************/
+00039 void canDispatch(CO_Data* d, Message *m)
+00040 {
+00041          switch(m->cob_id.w >> 7)
+00042         {
+00043                 case SYNC:
+00044                         if(d->CurrentCommunicationState.csSYNC)
+00045                                 proceedSYNC(d,m);
+00046                         break;
+00047                 /* case TIME_STAMP: */
+00048                 case PDO1tx:
+00049                 case PDO1rx:
+00050                 case PDO2tx:
+00051                 case PDO2rx:
+00052                 case PDO3tx:
+00053                 case PDO3rx:
+00054                 case PDO4tx:
+00055                 case PDO4rx:
+00056                         if (d->CurrentCommunicationState.csPDO)
+00057                                 proceedPDO(d,m);
+00058                         break;
+00059                 case SDOtx:
+00060                 case SDOrx:
+00061                         if (d->CurrentCommunicationState.csSDO)
+00062                                 proceedSDO(d,m);
+00063                         break;
+00064                 case NODE_GUARD:
+00065                         if (d->CurrentCommunicationState.csHeartbeat)
+00066                                 proceedNODE_GUARD(d,m);
+00067                         break;
+00068                 case NMT:
+00069                         if (*(d->iam_a_slave))
+00070                         {
+00071                                 proceedNMTstateChange(d,m);
+00072                         }
+00073         }
+00074 }
+00075 
+00076 #define StartOrStop(CommType, FuncStart, FuncStop) \
+00077         if(newCommunicationState->CommType && !d->CurrentCommunicationState.CommType){\
+00078                 MSG_WAR(0x9999,#FuncStart, 9999);\
+00079                 d->CurrentCommunicationState.CommType = 1;\
+00080                 FuncStart;\
+00081         }else if(!newCommunicationState->CommType && d->CurrentCommunicationState.CommType){\
+00082                 MSG_WAR(0x9999,#FuncStop, 9999);\
+00083                 d->CurrentCommunicationState.CommType = 0;\
+00084                 FuncStop;\
+00085         }
+00086 #define None
+00087         
+00088 /*****************************************************************************/
+00089 void switchCommunicationState(CO_Data* d, s_state_communication *newCommunicationState)
+00090 {
+00091         StartOrStop(csSDO,      None,           resetSDO(d))
+00092         StartOrStop(csSYNC,     startSYNC(d),           stopSYNC(d))
+00093         StartOrStop(csHeartbeat,        heartbeatInit(d),       heartbeatStop(d))
+00094 /*      StartOrStop(Emergency,,) */
+00095         StartOrStop(csPDO,      None,   None)
+00096         StartOrStop(csBoot_Up,  None,   slaveSendBootUp(d))
+00097 }
+00098 
+00099 /*****************************************************************************/
+00100 UNS8 setState(CO_Data* d, e_nodeState newState)
+00101 {
+00102         UNS16 wIndex = 0x1F22;
+00103         const indextable *ptrTable;
+00104         ODCallback_t *Callback;
+00105         UNS32 errorCode;
+00106         while(newState != d->nodeState){
+00107                 switch( newState ){
+00108                         case Initialisation:
+00109                         {
+00110                                 s_state_communication newCommunicationState = {1, 0, 0, 0, 0, 0};
+00111                                 /* This will force a second loop for the state switch */
+00112                                 d->nodeState = Initialisation;
+00113                                 newState = Pre_operational;
+00114                                 switchCommunicationState(d, &newCommunicationState);
+00115                                 /* call user app related state func. */
+00116                                 (*d->initialisation)();
+00117                                 
+00118                         }
+00119                         break;
+00120                                                                 
+00121                         case Pre_operational:
+00122                         {
+00123                                 
+00124                                 s_state_communication newCommunicationState = {0, 1, 1, 1, 1, 0};
+00125                                 d->nodeState = Pre_operational;
+00126                                 newState = Pre_operational;
+00127                                 switchCommunicationState(d, &newCommunicationState);
+00128                                 if (!(*(d->iam_a_slave)))
+00129                                 {
+00130                                         ptrTable =(*d->scanIndexOD)(wIndex, &errorCode, &Callback);
+00131                                         
+00132                                         if (errorCode != OD_SUCCESSFUL)
+00133                                                 {
+00134                                                         (*d->preOperational)();
+00135                                                 }
+00136                                         else
+00137                                                 {
+00138                                                         UNS32 res;
+00139                                                         res = decompo_dcf(d,0x01);
+00140                                                 }                               
+00141                                 }
+00142                                 else 
+00143                                 {
+00144                                         (*d->preOperational)();
+00145                                 }
+00146                         }
+00147                         break;
+00148                                                                 
+00149                         case Operational:
+00150                         if(d->nodeState == Initialisation) return 0xFF;
+00151                         {
+00152                                 s_state_communication newCommunicationState = {0, 1, 1, 1, 1, 1};
+00153                                 d->nodeState = Operational;
+00154                                 newState = Operational;
+00155                                 switchCommunicationState(d, &newCommunicationState);
+00156                                 (*d->operational)();
+00157                         }
+00158                         break;
+00159                                                 
+00160                         case Stopped:
+00161                         if(d->nodeState == Initialisation) return 0xFF;
+00162                         {
+00163                                 s_state_communication newCommunicationState = {0, 0, 0, 0, 1, 0};
+00164                                 d->nodeState = Stopped;
+00165                                 newState = Stopped;
+00166                                 switchCommunicationState(d, &newCommunicationState);
+00167                                 (*d->stopped)();
+00168                         }
+00169                         break;
+00170                         
+00171                         default:
+00172                                 return 0xFF;
+00173                 }/* end switch case */
+00174         
+00175         }
+00176         return 0;
+00177 }
+00178 
+00179 /*****************************************************************************/
+00180 UNS8 getNodeId(CO_Data* d)
+00181 {
+00182   return *d->bDeviceNodeId;
+00183 }
+00184 
+00185 /*****************************************************************************/
+00186 void setNodeId(CO_Data* d, UNS8 nodeId)
+00187 {
+00188   UNS16 offset = d->firstIndex->SDO_SVR;
+00189   if(offset){
+00190       /* cob_id_client = 0x600 + nodeId; */
+00191       *(UNS32*)d->objdict[offset].pSubindex[1].pObject = 0x600 + nodeId;
+00192       /* cob_id_server = 0x580 + nodeId; */
+00193       *(UNS32*)d->objdict[offset].pSubindex[2].pObject = 0x580 + nodeId;
+00194       /* node Id client. As we do not know the value, we put the node Id Server */
+00195       /* *(UNS8*)d->objdict[offset].pSubindex[3].pObject = nodeId; */
+00196   }
+00197 
+00198   /* ** Initialize the server(s) SDO parameters */
+00199   /* Remember that only one SDO server is allowed, defined at index 0x1200 */
+00200  
+00201   /* ** Initialize the client(s) SDO parameters  */
+00202   /* Nothing to initialize (no default values required by the DS 401) */
+00203   /* ** Initialize the receive PDO communication parameters. Only for 0x1400 to 0x1403 */
+00204   {
+00205     UNS8 i = 0;
+00206     UNS16 offset = d->firstIndex->PDO_RCV;
+00207     UNS16 lastIndex = d->lastIndex->PDO_RCV;
+00208     UNS32 cobID[] = {0x200, 0x300, 0x400, 0x500};
+00209     if( offset ) while( (offset <= lastIndex) && (i < 4)) {
+00210       //if(*(UNS32*)d->objdict[offset].pSubindex[1].pObject == cobID[i] + *d->bDeviceNodeId)
+00211               *(UNS32*)d->objdict[offset].pSubindex[1].pObject = cobID[i] + nodeId;
+00212       i ++;
+00213       offset ++;
+00214     }
+00215   }
+00216   /* ** Initialize the transmit PDO communication parameters. Only for 0x1800 to 0x1803 */
+00217   {
+00218     UNS8 i = 0;
+00219     UNS16 offset = d->firstIndex->PDO_TRS;
+00220     UNS16 lastIndex = d->lastIndex->PDO_TRS;
+00221     UNS32 cobID[] = {0x180, 0x280, 0x380, 0x480};
+00222     i = 0;
+00223     if( offset ) while ((offset <= lastIndex) && (i < 4)) {
+00224       //if(*(UNS32*)d->objdict[offset].pSubindex[1].pObject == cobID[i] + *d->bDeviceNodeId)
+00225               *(UNS32*)d->objdict[offset].pSubindex[1].pObject = cobID[i] + nodeId;
+00226       i ++;
+00227       offset ++;
+00228     }
+00229   }
+00230   /* bDeviceNodeId is defined in the object dictionary. */
+00231   *d->bDeviceNodeId = nodeId;
+00232 }
+00233 
+00234 void _initialisation(){}
+00235 void _preOperational(){}
+00236 void _operational(){}
+00237 void _stopped(){}
+