etisserant@0: /* etisserant@0: This file is part of CanFestival, a library implementing CanOpen Stack. etisserant@0: etisserant@0: Copyright (C): Edouard TISSERANT and Francis DUPIN etisserant@0: etisserant@0: See COPYING file for copyrights details. etisserant@0: etisserant@0: This library is free software; you can redistribute it and/or etisserant@0: modify it under the terms of the GNU Lesser General Public etisserant@0: License as published by the Free Software Foundation; either etisserant@0: version 2.1 of the License, or (at your option) any later version. etisserant@0: etisserant@0: This library is distributed in the hope that it will be useful, etisserant@0: but WITHOUT ANY WARRANTY; without even the implied warranty of etisserant@0: MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU etisserant@0: Lesser General Public License for more details. etisserant@0: etisserant@0: You should have received a copy of the GNU Lesser General Public etisserant@0: License along with this library; if not, write to the Free Software etisserant@0: Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA etisserant@0: */ nico@208: /*! nico@208: ** @file states.c nico@208: ** @author Edouard TISSERANT and Francis DUPIN nico@208: ** @date Tue Jun 5 09:32:32 2007 nico@208: ** nico@208: ** @brief nico@208: ** nico@208: ** nico@208: */ etisserant@0: etisserant@0: #include "states.h" etisserant@0: #include "def.h" etisserant@204: #include "dcf.h" etisserant@0: #include "nmtSlave.h" luis@284: #include "emcy.h" etisserant@343: #ifdef CO_ENABLE_LSS etisserant@343: #include "lss.h" etisserant@343: #endif nico@208: /** Prototypes for internals functions */ nico@208: /*! nico@208: ** nico@208: ** nico@208: ** @param d nico@208: ** @param newCommunicationState nico@208: **/ frdupin@53: void switchCommunicationState(CO_Data* d, frdupin@53: s_state_communication *newCommunicationState); frdupin@53: nico@208: /*! nico@208: ** nico@208: ** nico@208: ** @param d nico@208: ** nico@208: ** @return nico@208: **/ etisserant@0: e_nodeState getState(CO_Data* d) etisserant@0: { etisserant@0: return d->nodeState; etisserant@0: } etisserant@0: nico@208: /*! nico@208: ** nico@208: ** nico@208: ** @param d nico@208: ** @param m nico@208: **/ etisserant@0: void canDispatch(CO_Data* d, Message *m) etisserant@0: { etisserant@0: switch(m->cob_id.w >> 7) etisserant@0: { luis@284: case SYNC: /* can be a SYNC or a EMCY message */ luis@284: if(m->cob_id.w == 0x080) /* SYNC */ luis@284: { luis@284: if(d->CurrentCommunicationState.csSYNC) luis@284: proceedSYNC(d); luis@284: } else /* EMCY */ luis@284: if(d->CurrentCommunicationState.csEmergency) luis@284: proceedEMCY(d,m); etisserant@0: break; nico@215: /* case TIME_STAMP: */ etisserant@0: case PDO1tx: etisserant@0: case PDO1rx: etisserant@0: case PDO2tx: etisserant@0: case PDO2rx: etisserant@0: case PDO3tx: etisserant@0: case PDO3rx: etisserant@0: case PDO4tx: etisserant@0: case PDO4rx: etisserant@0: if (d->CurrentCommunicationState.csPDO) etisserant@0: proceedPDO(d,m); etisserant@0: break; etisserant@0: case SDOtx: etisserant@0: case SDOrx: etisserant@0: if (d->CurrentCommunicationState.csSDO) etisserant@0: proceedSDO(d,m); etisserant@0: break; etisserant@0: case NODE_GUARD: etisserant@0: if (d->CurrentCommunicationState.csHeartbeat) etisserant@0: proceedNODE_GUARD(d,m); etisserant@0: break; etisserant@0: case NMT: frdupin@88: if (*(d->iam_a_slave)) etisserant@0: { etisserant@0: proceedNMTstateChange(d,m); etisserant@0: } etisserant@343: #ifdef CO_ENABLE_LSS etisserant@343: case LSS: etisserant@343: if (!d->CurrentCommunicationState.csLSS)break; etisserant@343: if ((*(d->iam_a_slave)) && m->cob_id.w==MLSS_ADRESS) etisserant@343: { etisserant@343: proceedLSS_Slave(d,m); etisserant@343: } etisserant@343: else if(!(*(d->iam_a_slave)) && m->cob_id.w==SLSS_ADRESS) etisserant@343: { etisserant@343: proceedLSS_Master(d,m); etisserant@343: } etisserant@343: break; etisserant@343: #endif etisserant@0: } etisserant@0: } etisserant@0: etisserant@0: #define StartOrStop(CommType, FuncStart, FuncStop) \ etisserant@291: if(newCommunicationState->CommType && d->CurrentCommunicationState.CommType == 0){\ etisserant@195: MSG_WAR(0x9999,#FuncStart, 9999);\ etisserant@0: d->CurrentCommunicationState.CommType = 1;\ etisserant@0: FuncStart;\ etisserant@291: }else if(!newCommunicationState->CommType && d->CurrentCommunicationState.CommType == 1){\ etisserant@195: MSG_WAR(0x9999,#FuncStop, 9999);\ etisserant@0: d->CurrentCommunicationState.CommType = 0;\ etisserant@0: FuncStop;\ etisserant@0: } etisserant@0: #define None nico@208: nico@208: /*! nico@208: ** nico@208: ** nico@208: ** @param d nico@208: ** @param newCommunicationState nico@208: **/ etisserant@0: void switchCommunicationState(CO_Data* d, s_state_communication *newCommunicationState) etisserant@0: { etisserant@0: StartOrStop(csSDO, None, resetSDO(d)) etisserant@0: StartOrStop(csSYNC, startSYNC(d), stopSYNC(d)) etisserant@0: StartOrStop(csHeartbeat, heartbeatInit(d), heartbeatStop(d)) luis@284: StartOrStop(csEmergency, emergencyInit(d), emergencyStop(d)) etisserant@235: StartOrStop(csPDO, PDOInit(d), PDOStop(d)) dejoigny@14: StartOrStop(csBoot_Up, None, slaveSendBootUp(d)) etisserant@343: #ifdef CO_ENABLE_LSS etisserant@343: StartOrStop(csLSS, startLSS(d), stopLSS(d)) etisserant@343: #endif etisserant@0: } etisserant@0: nico@208: /*! nico@208: ** nico@208: ** nico@208: ** @param d nico@208: ** @param newState nico@208: ** nico@208: ** @return nico@208: **/ etisserant@0: UNS8 setState(CO_Data* d, e_nodeState newState) etisserant@0: { etisserant@0: while(newState != d->nodeState){ etisserant@0: switch( newState ){ etisserant@0: case Initialisation: etisserant@0: { etisserant@343: s_state_communication newCommunicationState = {1, 0, 0, 0, 0, 0, 0}; nico@215: /* This will force a second loop for the state switch */ etisserant@0: d->nodeState = Initialisation; etisserant@0: newState = Pre_operational; etisserant@0: switchCommunicationState(d, &newCommunicationState); nico@215: /* call user app related state func. */ etisserant@0: (*d->initialisation)(); greg@178: etisserant@0: } etisserant@0: break; etisserant@0: etisserant@0: case Pre_operational: etisserant@0: { greg@178: etisserant@343: s_state_communication newCommunicationState = {0, 1, 1, 1, 1, 0, 1}; etisserant@0: d->nodeState = Pre_operational; etisserant@0: newState = Pre_operational; etisserant@0: switchCommunicationState(d, &newCommunicationState); greg@178: if (!(*(d->iam_a_slave))) greg@178: { groke6@349: //send_consise_dcf(d,0x01); groke6@349: masterSendNMTstateChange (d, 0, NMT_Reset_Node); groke6@349: (*d->preOperational)(); greg@178: } greg@178: else greg@178: { greg@178: (*d->preOperational)(); greg@178: } etisserant@0: } etisserant@0: break; etisserant@0: etisserant@0: case Operational: etisserant@0: if(d->nodeState == Initialisation) return 0xFF; etisserant@0: { etisserant@343: s_state_communication newCommunicationState = {0, 1, 1, 1, 1, 1, 0}; etisserant@0: d->nodeState = Operational; etisserant@0: newState = Operational; etisserant@0: switchCommunicationState(d, &newCommunicationState); etisserant@0: (*d->operational)(); etisserant@0: } etisserant@0: break; etisserant@0: etisserant@0: case Stopped: etisserant@0: if(d->nodeState == Initialisation) return 0xFF; etisserant@0: { etisserant@343: s_state_communication newCommunicationState = {0, 0, 0, 0, 1, 0, 1}; etisserant@0: d->nodeState = Stopped; etisserant@0: newState = Stopped; etisserant@0: switchCommunicationState(d, &newCommunicationState); etisserant@0: (*d->stopped)(); etisserant@0: } etisserant@0: break; etisserant@343: #ifdef CO_ENABLE_LSS etisserant@343: case LssTimingDelay: etisserant@343: { etisserant@343: s_state_communication newCommunicationState = {0, 0, 0, 0, 0, 0, 0}; etisserant@343: d->nodeState = LssTimingDelay; etisserant@343: newState = LssTimingDelay; etisserant@343: switchCommunicationState(d, &newCommunicationState); etisserant@343: } etisserant@343: break; etisserant@343: #endif etisserant@0: etisserant@0: default: etisserant@0: return 0xFF; nico@215: }/* end switch case */ etisserant@94: etisserant@0: } etisserant@0: return 0; etisserant@0: } etisserant@0: nico@208: /*! nico@208: ** nico@208: ** nico@208: ** @param d nico@208: ** nico@208: ** @return nico@208: **/ etisserant@0: UNS8 getNodeId(CO_Data* d) etisserant@0: { etisserant@0: return *d->bDeviceNodeId; etisserant@0: } etisserant@0: nico@208: /*! nico@208: ** nico@208: ** nico@208: ** @param d nico@208: ** @param nodeId nico@208: **/ etisserant@0: void setNodeId(CO_Data* d, UNS8 nodeId) etisserant@0: { etisserant@0: UNS16 offset = d->firstIndex->SDO_SVR; etisserant@343: etisserant@343: #ifdef CO_ENABLE_LSS etisserant@343: if(nodeId==0xFF) etisserant@343: { etisserant@343: *d->bDeviceNodeId = nodeId; etisserant@343: return; etisserant@343: } etisserant@343: #endif etisserant@343: etisserant@0: if(offset){ groke6@349: /* Adjust COB-ID Client->Server (rx) only id already set to default value or id not valid (id==0xFF)*/ groke6@349: if((*(UNS32*)d->objdict[offset].pSubindex[1].pObject == 0x600 + *d->bDeviceNodeId)||(*d->bDeviceNodeId==0xFF)){ nico@215: /* cob_id_client = 0x600 + nodeId; */ etisserant@0: *(UNS32*)d->objdict[offset].pSubindex[1].pObject = 0x600 + nodeId; etisserant@303: } groke6@349: /* Adjust COB-ID Server -> Client (tx) only id already set to default value or id not valid (id==0xFF)*/ groke6@349: if((*(UNS32*)d->objdict[offset].pSubindex[2].pObject == 0x580 + *d->bDeviceNodeId)||(*d->bDeviceNodeId==0xFF)){ nico@215: /* cob_id_server = 0x580 + nodeId; */ etisserant@0: *(UNS32*)d->objdict[offset].pSubindex[2].pObject = 0x580 + nodeId; etisserant@303: } etisserant@0: } etisserant@0: nico@215: /* nico@208: Initialize the server(s) SDO parameters nico@208: Remember that only one SDO server is allowed, defined at index 0x1200 nico@208: nico@208: Initialize the client(s) SDO parameters nico@208: Nothing to initialize (no default values required by the DS 401) nico@208: Initialize the receive PDO communication parameters. Only for 0x1400 to 0x1403 nico@208: */ etisserant@0: { etisserant@0: UNS8 i = 0; etisserant@0: UNS16 offset = d->firstIndex->PDO_RCV; etisserant@0: UNS16 lastIndex = d->lastIndex->PDO_RCV; etisserant@0: UNS32 cobID[] = {0x200, 0x300, 0x400, 0x500}; etisserant@0: if( offset ) while( (offset <= lastIndex) && (i < 4)) { groke6@349: if((*(UNS32*)d->objdict[offset].pSubindex[1].pObject == cobID[i] + *d->bDeviceNodeId)||(*d->bDeviceNodeId==0xFF)) etisserant@0: *(UNS32*)d->objdict[offset].pSubindex[1].pObject = cobID[i] + nodeId; etisserant@0: i ++; etisserant@0: offset ++; etisserant@0: } etisserant@0: } frdupin@71: /* ** Initialize the transmit PDO communication parameters. Only for 0x1800 to 0x1803 */ etisserant@0: { etisserant@0: UNS8 i = 0; etisserant@0: UNS16 offset = d->firstIndex->PDO_TRS; etisserant@0: UNS16 lastIndex = d->lastIndex->PDO_TRS; etisserant@0: UNS32 cobID[] = {0x180, 0x280, 0x380, 0x480}; etisserant@0: i = 0; etisserant@0: if( offset ) while ((offset <= lastIndex) && (i < 4)) { groke6@349: if((*(UNS32*)d->objdict[offset].pSubindex[1].pObject == cobID[i] + *d->bDeviceNodeId)||(*d->bDeviceNodeId==0xFF)) etisserant@0: *(UNS32*)d->objdict[offset].pSubindex[1].pObject = cobID[i] + nodeId; etisserant@0: i ++; etisserant@0: offset ++; etisserant@0: } etisserant@0: } etisserant@314: etisserant@314: /* Update EMCY COB-ID if already set to default*/ groke6@349: if((*d->error_cobid == *d->bDeviceNodeId + 0x80)||(*d->bDeviceNodeId==0xFF)) etisserant@314: *d->error_cobid = nodeId + 0x80; etisserant@314: nico@215: /* bDeviceNodeId is defined in the object dictionary. */ etisserant@0: *d->bDeviceNodeId = nodeId; etisserant@0: } etisserant@149: etisserant@149: void _initialisation(){} etisserant@149: void _preOperational(){} etisserant@149: void _operational(){} etisserant@149: void _stopped(){}