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" etisserant@0: 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: { etisserant@0: case SYNC: etisserant@0: if(d->CurrentCommunicationState.csSYNC) etisserant@0: proceedSYNC(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@0: } etisserant@0: } etisserant@0: etisserant@0: #define StartOrStop(CommType, FuncStart, FuncStop) \ etisserant@0: if(newCommunicationState->CommType && !d->CurrentCommunicationState.CommType){\ etisserant@195: MSG_WAR(0x9999,#FuncStart, 9999);\ etisserant@0: d->CurrentCommunicationState.CommType = 1;\ etisserant@0: FuncStart;\ etisserant@0: }else if(!newCommunicationState->CommType && d->CurrentCommunicationState.CommType){\ 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)) frdupin@71: /* StartOrStop(Emergency,,) */ etisserant@235: StartOrStop(csPDO, PDOInit(d), PDOStop(d)) dejoigny@14: StartOrStop(csBoot_Up, None, slaveSendBootUp(d)) 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: { greg@183: UNS16 wIndex = 0x1F22; greg@183: const indextable *ptrTable; greg@183: ODCallback_t *Callback; greg@183: UNS32 errorCode; etisserant@0: while(newState != d->nodeState){ etisserant@0: switch( newState ){ etisserant@0: case Initialisation: etisserant@0: { frdupin@71: s_state_communication newCommunicationState = {1, 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: frdupin@71: s_state_communication newCommunicationState = {0, 1, 1, 1, 1, 0}; 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: { greg@183: ptrTable =(*d->scanIndexOD)(wIndex, &errorCode, &Callback); greg@183: greg@183: if (errorCode != OD_SUCCESSFUL) greg@183: { greg@183: (*d->preOperational)(); greg@183: } greg@183: else greg@183: { greg@183: UNS32 res; greg@183: res = decompo_dcf(d,0x01); greg@183: } 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: { frdupin@71: s_state_communication newCommunicationState = {0, 1, 1, 1, 1, 1}; 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: { frdupin@71: s_state_communication newCommunicationState = {0, 0, 0, 0, 1, 0}; etisserant@0: d->nodeState = Stopped; etisserant@0: newState = Stopped; etisserant@0: switchCommunicationState(d, &newCommunicationState); etisserant@0: (*d->stopped)(); etisserant@0: } etisserant@0: break; 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@0: if(offset){ nico@215: /* cob_id_client = 0x600 + nodeId; */ etisserant@0: *(UNS32*)d->objdict[offset].pSubindex[1].pObject = 0x600 + nodeId; nico@215: /* cob_id_server = 0x580 + nodeId; */ etisserant@0: *(UNS32*)d->objdict[offset].pSubindex[2].pObject = 0x580 + nodeId; nico@215: /* node Id client. As we do not know the value, we put the node Id Server */ nico@215: /* *(UNS8*)d->objdict[offset].pSubindex[3].pObject = nodeId; */ 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)) { etisserant@236: if(*(UNS32*)d->objdict[offset].pSubindex[1].pObject == cobID[i] + *d->bDeviceNodeId) 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)) { etisserant@236: if(*(UNS32*)d->objdict[offset].pSubindex[1].pObject == cobID[i] + *d->bDeviceNodeId) etisserant@0: *(UNS32*)d->objdict[offset].pSubindex[1].pObject = cobID[i] + nodeId; etisserant@0: i ++; etisserant@0: offset ++; etisserant@0: } etisserant@0: } 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(){}