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@207: 00022 
nico@207: 00023 #include "states.h"
nico@207: 00024 #include "def.h"
nico@207: 00025 #include "dcf.h"
nico@207: 00026 #include "nmtSlave.h"
nico@207: 00027 
nico@207: 00028 /* Prototypes for internals functions */
nico@207: 00029 void switchCommunicationState(CO_Data* d, 
nico@207: 00030         s_state_communication *newCommunicationState);
nico@207: 00031         
nico@207: 00032 /*****************************************************************************/
nico@207: 00033 e_nodeState getState(CO_Data* d)
nico@207: 00034 {
nico@207: 00035   return d->nodeState;
nico@207: 00036 }
nico@207: 00037 
nico@207: 00038 /*****************************************************************************/
nico@207: 00039 void canDispatch(CO_Data* d, Message *m)
nico@207: 00040 {
nico@207: 00041          switch(m->cob_id.w >> 7)
nico@207: 00042         {
nico@207: 00043                 case SYNC:
nico@207: 00044                         if(d->CurrentCommunicationState.csSYNC)
nico@207: 00045                                 proceedSYNC(d,m);
nico@207: 00046                         break;
nico@207: 00047                 /* case TIME_STAMP: */
nico@207: 00048                 case PDO1tx:
nico@207: 00049                 case PDO1rx:
nico@207: 00050                 case PDO2tx:
nico@207: 00051                 case PDO2rx:
nico@207: 00052                 case PDO3tx:
nico@207: 00053                 case PDO3rx:
nico@207: 00054                 case PDO4tx:
nico@207: 00055                 case PDO4rx:
nico@207: 00056                         if (d->CurrentCommunicationState.csPDO)
nico@207: 00057                                 proceedPDO(d,m);
nico@207: 00058                         break;
nico@207: 00059                 case SDOtx:
nico@207: 00060                 case SDOrx:
nico@207: 00061                         if (d->CurrentCommunicationState.csSDO)
nico@207: 00062                                 proceedSDO(d,m);
nico@207: 00063                         break;
nico@207: 00064                 case NODE_GUARD:
nico@207: 00065                         if (d->CurrentCommunicationState.csHeartbeat)
nico@207: 00066                                 proceedNODE_GUARD(d,m);
nico@207: 00067                         break;
nico@207: 00068                 case NMT:
nico@207: 00069                         if (*(d->iam_a_slave))
nico@207: 00070                         {
nico@207: 00071                                 proceedNMTstateChange(d,m);
nico@207: 00072                         }
nico@207: 00073         }
nico@207: 00074 }
nico@207: 00075 
nico@207: 00076 #define StartOrStop(CommType, FuncStart, FuncStop) \
nico@207: 00077         if(newCommunicationState->CommType && !d->CurrentCommunicationState.CommType){\
nico@207: 00078                 MSG_WAR(0x9999,#FuncStart, 9999);\
nico@207: 00079                 d->CurrentCommunicationState.CommType = 1;\
nico@207: 00080                 FuncStart;\
nico@207: 00081         }else if(!newCommunicationState->CommType && d->CurrentCommunicationState.CommType){\
nico@207: 00082                 MSG_WAR(0x9999,#FuncStop, 9999);\
nico@207: 00083                 d->CurrentCommunicationState.CommType = 0;\
nico@207: 00084                 FuncStop;\
nico@207: 00085         }
nico@207: 00086 #define None
nico@207: 00087         
nico@207: 00088 /*****************************************************************************/
nico@207: 00089 void switchCommunicationState(CO_Data* d, s_state_communication *newCommunicationState)
nico@207: 00090 {
nico@207: 00091         StartOrStop(csSDO,      None,           resetSDO(d))
nico@207: 00092         StartOrStop(csSYNC,     startSYNC(d),           stopSYNC(d))
nico@207: 00093         StartOrStop(csHeartbeat,        heartbeatInit(d),       heartbeatStop(d))
nico@207: 00094 /*      StartOrStop(Emergency,,) */
nico@207: 00095         StartOrStop(csPDO,      None,   None)
nico@207: 00096         StartOrStop(csBoot_Up,  None,   slaveSendBootUp(d))
nico@207: 00097 }
nico@207: 00098 
nico@207: 00099 /*****************************************************************************/
nico@207: 00100 UNS8 setState(CO_Data* d, e_nodeState newState)
nico@207: 00101 {
nico@207: 00102         UNS16 wIndex = 0x1F22;
nico@207: 00103         const indextable *ptrTable;
nico@207: 00104         ODCallback_t *Callback;
nico@207: 00105         UNS32 errorCode;
nico@207: 00106         while(newState != d->nodeState){
nico@207: 00107                 switch( newState ){
nico@207: 00108                         case Initialisation:
nico@207: 00109                         {
nico@207: 00110                                 s_state_communication newCommunicationState = {1, 0, 0, 0, 0, 0};
nico@207: 00111                                 /* This will force a second loop for the state switch */
nico@207: 00112                                 d->nodeState = Initialisation;
nico@207: 00113                                 newState = Pre_operational;
nico@207: 00114                                 switchCommunicationState(d, &newCommunicationState);
nico@207: 00115                                 /* call user app related state func. */
nico@207: 00116                                 (*d->initialisation)();
nico@207: 00117                                 
nico@207: 00118                         }
nico@207: 00119                         break;
nico@207: 00120                                                                 
nico@207: 00121                         case Pre_operational:
nico@207: 00122                         {
nico@207: 00123                                 
nico@207: 00124                                 s_state_communication newCommunicationState = {0, 1, 1, 1, 1, 0};
nico@207: 00125                                 d->nodeState = Pre_operational;
nico@207: 00126                                 newState = Pre_operational;
nico@207: 00127                                 switchCommunicationState(d, &newCommunicationState);
nico@207: 00128                                 if (!(*(d->iam_a_slave)))
nico@207: 00129                                 {
nico@207: 00130                                         ptrTable =(*d->scanIndexOD)(wIndex, &errorCode, &Callback);
nico@207: 00131                                         
nico@207: 00132                                         if (errorCode != OD_SUCCESSFUL)
nico@207: 00133                                                 {
nico@207: 00134                                                         (*d->preOperational)();
nico@207: 00135                                                 }
nico@207: 00136                                         else
nico@207: 00137                                                 {
nico@207: 00138                                                         UNS32 res;
nico@207: 00139                                                         res = decompo_dcf(d,0x01);
nico@207: 00140                                                 }                               
nico@207: 00141                                 }
nico@207: 00142                                 else 
nico@207: 00143                                 {
nico@207: 00144                                         (*d->preOperational)();
nico@207: 00145                                 }
nico@207: 00146                         }
nico@207: 00147                         break;
nico@207: 00148                                                                 
nico@207: 00149                         case Operational:
nico@207: 00150                         if(d->nodeState == Initialisation) return 0xFF;
nico@207: 00151                         {
nico@207: 00152                                 s_state_communication newCommunicationState = {0, 1, 1, 1, 1, 1};
nico@207: 00153                                 d->nodeState = Operational;
nico@207: 00154                                 newState = Operational;
nico@207: 00155                                 switchCommunicationState(d, &newCommunicationState);
nico@207: 00156                                 (*d->operational)();
nico@207: 00157                         }
nico@207: 00158                         break;
nico@207: 00159                                                 
nico@207: 00160                         case Stopped:
nico@207: 00161                         if(d->nodeState == Initialisation) return 0xFF;
nico@207: 00162                         {
nico@207: 00163                                 s_state_communication newCommunicationState = {0, 0, 0, 0, 1, 0};
nico@207: 00164                                 d->nodeState = Stopped;
nico@207: 00165                                 newState = Stopped;
nico@207: 00166                                 switchCommunicationState(d, &newCommunicationState);
nico@207: 00167                                 (*d->stopped)();
nico@207: 00168                         }
nico@207: 00169                         break;
nico@207: 00170                         
nico@207: 00171                         default:
nico@207: 00172                                 return 0xFF;
nico@207: 00173                 }/* end switch case */
nico@207: 00174         
nico@207: 00175         }
nico@207: 00176         return 0;
nico@207: 00177 }
nico@207: 00178 
nico@207: 00179 /*****************************************************************************/
nico@207: 00180 UNS8 getNodeId(CO_Data* d)
nico@207: 00181 {
nico@207: 00182   return *d->bDeviceNodeId;
nico@207: 00183 }
nico@207: 00184 
nico@207: 00185 /*****************************************************************************/
nico@207: 00186 void setNodeId(CO_Data* d, UNS8 nodeId)
nico@207: 00187 {
nico@207: 00188   UNS16 offset = d->firstIndex->SDO_SVR;
nico@207: 00189   if(offset){
nico@207: 00190       /* cob_id_client = 0x600 + nodeId; */
nico@207: 00191       *(UNS32*)d->objdict[offset].pSubindex[1].pObject = 0x600 + nodeId;
nico@207: 00192       /* cob_id_server = 0x580 + nodeId; */
nico@207: 00193       *(UNS32*)d->objdict[offset].pSubindex[2].pObject = 0x580 + nodeId;
nico@207: 00194       /* node Id client. As we do not know the value, we put the node Id Server */
nico@207: 00195       /* *(UNS8*)d->objdict[offset].pSubindex[3].pObject = nodeId; */
nico@207: 00196   }
nico@207: 00197 
nico@207: 00198   /* ** Initialize the server(s) SDO parameters */
nico@207: 00199   /* Remember that only one SDO server is allowed, defined at index 0x1200 */
nico@207: 00200  
nico@207: 00201   /* ** Initialize the client(s) SDO parameters  */
nico@207: 00202   /* Nothing to initialize (no default values required by the DS 401) */
nico@207: 00203   /* ** Initialize the receive PDO communication parameters. Only for 0x1400 to 0x1403 */
nico@207: 00204   {
nico@207: 00205     UNS8 i = 0;
nico@207: 00206     UNS16 offset = d->firstIndex->PDO_RCV;
nico@207: 00207     UNS16 lastIndex = d->lastIndex->PDO_RCV;
nico@207: 00208     UNS32 cobID[] = {0x200, 0x300, 0x400, 0x500};
nico@207: 00209     if( offset ) while( (offset <= lastIndex) && (i < 4)) {
nico@207: 00210       //if(*(UNS32*)d->objdict[offset].pSubindex[1].pObject == cobID[i] + *d->bDeviceNodeId)
nico@207: 00211               *(UNS32*)d->objdict[offset].pSubindex[1].pObject = cobID[i] + nodeId;
nico@207: 00212       i ++;
nico@207: 00213       offset ++;
nico@207: 00214     }
nico@207: 00215   }
nico@207: 00216   /* ** Initialize the transmit PDO communication parameters. Only for 0x1800 to 0x1803 */
nico@207: 00217   {
nico@207: 00218     UNS8 i = 0;
nico@207: 00219     UNS16 offset = d->firstIndex->PDO_TRS;
nico@207: 00220     UNS16 lastIndex = d->lastIndex->PDO_TRS;
nico@207: 00221     UNS32 cobID[] = {0x180, 0x280, 0x380, 0x480};
nico@207: 00222     i = 0;
nico@207: 00223     if( offset ) while ((offset <= lastIndex) && (i < 4)) {
nico@207: 00224       //if(*(UNS32*)d->objdict[offset].pSubindex[1].pObject == cobID[i] + *d->bDeviceNodeId)
nico@207: 00225               *(UNS32*)d->objdict[offset].pSubindex[1].pObject = cobID[i] + nodeId;
nico@207: 00226       i ++;
nico@207: 00227       offset ++;
nico@207: 00228     }
nico@207: 00229   }
nico@207: 00230   /* bDeviceNodeId is defined in the object dictionary. */
nico@207: 00231   *d->bDeviceNodeId = nodeId;
nico@207: 00232 }
nico@207: 00233 
nico@207: 00234 void _initialisation(){}
nico@207: 00235 void _preOperational(){}
nico@207: 00236 void _operational(){}
nico@207: 00237 void _stopped(){}
nico@207: