states.c

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 ** @file   states.c
00024 ** @author Edouard TISSERANT and Francis DUPIN
00025 ** @date   Tue Jun  5 09:32:32 2007
00026 **
00027 ** @brief
00028 **
00029 **
00030 */
00031 
00032 #include "data.h"
00033 #include "sysdep.h"
00034 
00035 /** Prototypes for internals functions */
00036 /*!                                                                                                
00037 **                                                                                                 
00038 **                                                                                                 
00039 ** @param d                                                                                        
00040 ** @param newCommunicationState                                                                    
00041 **/     
00042 void switchCommunicationState(CO_Data* d, 
00043     s_state_communication *newCommunicationState);
00044     
00045 /*!                                                                                                
00046 **                                                                                                 
00047 **                                                                                                 
00048 ** @param d                                                                                        
00049 **                                                                                                 
00050 ** @return                                                                                         
00051 **/    
00052 e_nodeState getState(CO_Data* d)
00053 {
00054   return d->nodeState;
00055 }
00056 
00057 /*!                                                                                                
00058 **                                                                                                 
00059 **                                                                                                 
00060 ** @param d                                                                                        
00061 ** @param m                                                                                        
00062 **/  
00063 void canDispatch(CO_Data* d, Message *m)
00064 {
00065     UNS16 cob_id = UNS16_LE(m->cob_id);
00066      switch(cob_id >> 7)
00067     {
00068         case SYNC:        /* can be a SYNC or a EMCY message */
00069             if(cob_id == 0x080)    /* SYNC */
00070             {
00071                 if(d->CurrentCommunicationState.csSYNC)
00072                     proceedSYNC(d);
00073             } else         /* EMCY */
00074                 if(d->CurrentCommunicationState.csEmergency)
00075                     proceedEMCY(d,m);
00076             break;
00077         /* case TIME_STAMP: */
00078         case PDO1tx:
00079         case PDO1rx:
00080         case PDO2tx:
00081         case PDO2rx:
00082         case PDO3tx:
00083         case PDO3rx:
00084         case PDO4tx:
00085         case PDO4rx:
00086             if (d->CurrentCommunicationState.csPDO)
00087                 proceedPDO(d,m);
00088             break;
00089         case SDOtx:
00090         case SDOrx:
00091             if (d->CurrentCommunicationState.csSDO)
00092                 proceedSDO(d,m);
00093             break;
00094         case NODE_GUARD:
00095             if (d->CurrentCommunicationState.csHeartbeat)
00096                 proceedNODE_GUARD(d,m);
00097             break;
00098         case NMT:
00099             if (*(d->iam_a_slave))
00100             {
00101                 proceedNMTstateChange(d,m);
00102             }
00103 #ifdef CO_ENABLE_LSS
00104         case LSS:
00105             if (!d->CurrentCommunicationState.csLSS)break;
00106             if ((*(d->iam_a_slave)) && cob_id==MLSS_ADRESS)
00107             {
00108                 proceedLSS_Slave(d,m);
00109             }
00110             else if(!(*(d->iam_a_slave)) && cob_id==SLSS_ADRESS)
00111             {
00112                 proceedLSS_Master(d,m);
00113             }
00114             break;
00115 #endif
00116     }
00117 }
00118 
00119 #define StartOrStop(CommType, FuncStart, FuncStop) \
00120     if(newCommunicationState->CommType && d->CurrentCommunicationState.CommType == 0){\
00121         MSG_WAR(0x9999,#FuncStart, 9999);\
00122         d->CurrentCommunicationState.CommType = 1;\
00123         FuncStart;\
00124     }else if(!newCommunicationState->CommType && d->CurrentCommunicationState.CommType == 1){\
00125         MSG_WAR(0x9999,#FuncStop, 9999);\
00126         d->CurrentCommunicationState.CommType = 0;\
00127         FuncStop;\
00128     }
00129 #define None
00130 
00131 /*!                                                                                                
00132 **                                                                                                 
00133 **                                                                                                 
00134 ** @param d                                                                                        
00135 ** @param newCommunicationState                                                                    
00136 **/      
00137 void switchCommunicationState(CO_Data* d, s_state_communication *newCommunicationState)
00138 {
00139 #ifdef CO_ENABLE_LSS
00140     StartOrStop(csLSS,    startLSS (d),    stopLSS (d))
00141 #endif
00142     StartOrStop(csSDO,    None,        resetSDO(d))
00143     StartOrStop(csSYNC,    startSYNC (d),        stopSYNC (d))
00144     StartOrStop(csHeartbeat,    heartbeatInit(d),    heartbeatStop(d))
00145     StartOrStop(csEmergency,    emergencyInit(d),    emergencyStop(d)) 
00146     StartOrStop(csPDO,    PDOInit(d),    PDOStop(d))
00147     StartOrStop(csBoot_Up,    None,    slaveSendBootUp(d))
00148 }
00149 
00150 /*!                                                                                                
00151 **                                                                                                 
00152 **                                                                                                 
00153 ** @param d                                                                                        
00154 ** @param newState                                                                                 
00155 **                                                                                                 
00156 ** @return                                                                                         
00157 **/  
00158 UNS8 setState(CO_Data* d, e_nodeState newState)
00159 {
00160     if(newState != d->nodeState){
00161         switch( newState ){
00162             case Initialisation:
00163             {
00164                 s_state_communication newCommunicationState = {1, 0, 0, 0, 0, 0, 0};
00165                 d->nodeState = Initialisation;
00166                 switchCommunicationState(d, &newCommunicationState);
00167                 /* call user app init callback now. */
00168                 /* d->initialisation MUST NOT CALL SetState */
00169                 (*d->initialisation)(d);                
00170             }
00171 
00172             /* Automatic transition - No break statement ! */
00173             /* Transition from Initialisation to Pre_operational */
00174             /* is automatic as defined in DS301. */
00175             /* App don't have to call SetState(d, Pre_operational) */
00176                                 
00177             case Pre_operational:
00178             {
00179                 
00180                 s_state_communication newCommunicationState = {0, 1, 1, 1, 1, 0, 1};
00181                 d->nodeState = Pre_operational;
00182                 switchCommunicationState(d, &newCommunicationState);
00183                 if (!(*(d->iam_a_slave)))
00184                 {
00185                     masterSendNMTstateChange (d, 0, NMT_Reset_Node);
00186                 }
00187                 (*d->preOperational)(d);
00188             }
00189             break;
00190                                 
00191             case Operational:
00192             if(d->nodeState == Initialisation) return 0xFF;
00193             {
00194                 s_state_communication newCommunicationState = {0, 1, 1, 1, 1, 1, 0};
00195                 d->nodeState = Operational;
00196                 newState = Operational;
00197                 switchCommunicationState(d, &newCommunicationState);
00198                 (*d->operational)(d);
00199             }
00200             break;
00201                         
00202             case Stopped:
00203             if(d->nodeState == Initialisation) return 0xFF;
00204             {
00205                 s_state_communication newCommunicationState = {0, 0, 0, 0, 1, 0, 1};
00206                 d->nodeState = Stopped;
00207                 newState = Stopped;
00208                 switchCommunicationState(d, &newCommunicationState);
00209                 (*d->stopped)(d);
00210             }
00211             break;
00212             default:
00213                 return 0xFF;
00214 
00215         }/* end switch case */
00216     
00217     }
00218     /* d->nodeState contains the final state */
00219     /* may not be the requested state */
00220     return d->nodeState;  
00221 }
00222 
00223 /*!                                                                                                
00224 **                                                                                                 
00225 **                                                                                                 
00226 ** @param d                                                                                        
00227 **                                                                                                 
00228 ** @return                                                                                         
00229 **/ 
00230 UNS8 getNodeId(CO_Data* d)
00231 {
00232   return *d->bDeviceNodeId;
00233 }
00234 
00235 /*!                                                                                                
00236 **                                                                                                 
00237 **                                                                                                 
00238 ** @param d                                                                                        
00239 ** @param nodeId                                                                                   
00240 **/   
00241 void setNodeId(CO_Data* d, UNS8 nodeId)
00242 {
00243   UNS16 offset = d->firstIndex->SDO_SVR;
00244   
00245 #ifdef CO_ENABLE_LSS
00246   d->lss_transfer.nodeID=nodeId;
00247   if(nodeId==0xFF){
00248       *d->bDeviceNodeId = nodeId;
00249       return;
00250   }
00251   else
00252 #endif
00253   if(!(nodeId>0 && nodeId<=127)){
00254       MSG_WAR(0x2D01, "Invalid NodeID",nodeId);
00255       return;
00256   }
00257 
00258   if(offset){
00259     /* Adjust COB-ID Client->Server (rx) only id already set to default value or id not valid (id==0xFF)*/
00260     if((*(UNS32*)d->objdict[offset].pSubindex[1].pObject == 0x600 + *d->bDeviceNodeId)||(*d->bDeviceNodeId==0xFF)){
00261       /* cob_id_client = 0x600 + nodeId; */
00262       *(UNS32*)d->objdict[offset].pSubindex[1].pObject = 0x600 + nodeId;
00263     }
00264     /* Adjust COB-ID Server -> Client (tx) only id already set to default value or id not valid (id==0xFF)*/
00265     if((*(UNS32*)d->objdict[offset].pSubindex[2].pObject == 0x580 + *d->bDeviceNodeId)||(*d->bDeviceNodeId==0xFF)){
00266       /* cob_id_server = 0x580 + nodeId; */
00267       *(UNS32*)d->objdict[offset].pSubindex[2].pObject = 0x580 + nodeId;
00268     }
00269   }
00270 
00271   /* 
00272        Initialize the server(s) SDO parameters
00273       Remember that only one SDO server is allowed, defined at index 0x1200    
00274          
00275       Initialize the client(s) SDO parameters     
00276       Nothing to initialize (no default values required by the DS 401)    
00277       Initialize the receive PDO communication parameters. Only for 0x1400 to 0x1403 
00278   */
00279   {
00280     UNS8 i = 0;
00281     UNS16 offset = d->firstIndex->PDO_RCV;
00282     UNS16 lastIndex = d->lastIndex->PDO_RCV;
00283     UNS32 cobID[] = {0x200, 0x300, 0x400, 0x500};
00284     if( offset ) while( (offset <= lastIndex) && (i < 4)) {
00285       if((*(UNS32*)d->objdict[offset].pSubindex[1].pObject == cobID[i] + *d->bDeviceNodeId)||(*d->bDeviceNodeId==0xFF))
00286           *(UNS32*)d->objdict[offset].pSubindex[1].pObject = cobID[i] + nodeId;
00287       i ++;
00288       offset ++;
00289     }
00290   }
00291   /* ** Initialize the transmit PDO communication parameters. Only for 0x1800 to 0x1803 */
00292   {
00293     UNS8 i = 0;
00294     UNS16 offset = d->firstIndex->PDO_TRS;
00295     UNS16 lastIndex = d->lastIndex->PDO_TRS;
00296     UNS32 cobID[] = {0x180, 0x280, 0x380, 0x480};
00297     i = 0;
00298     if( offset ) while ((offset <= lastIndex) && (i < 4)) {
00299       if((*(UNS32*)d->objdict[offset].pSubindex[1].pObject == cobID[i] + *d->bDeviceNodeId)||(*d->bDeviceNodeId==0xFF))
00300           *(UNS32*)d->objdict[offset].pSubindex[1].pObject = cobID[i] + nodeId;
00301       i ++;
00302       offset ++;
00303     }
00304   }
00305 
00306   /* Update EMCY COB-ID if already set to default*/
00307   if((*d->error_cobid == *d->bDeviceNodeId + 0x80)||(*d->bDeviceNodeId==0xFF))
00308     *d->error_cobid = nodeId + 0x80;
00309 
00310   /* bDeviceNodeId is defined in the object dictionary. */
00311   *d->bDeviceNodeId = nodeId;
00312 }
00313 
00314 void _initialisation(CO_Data* d){}
00315 void _preOperational(CO_Data* d){}
00316 void _operational(CO_Data* d){}
00317 void _stopped(CO_Data* d){}