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: */
etisserant@0: 
etisserant@0: #include "states.h"
etisserant@0: #include "def.h"
etisserant@0: 
etisserant@0: #include "nmtSlave.h"
etisserant@0: 
etisserant@0: #ifdef LED_ENABLE
etisserant@0: #include "led.h"
etisserant@0: #else
etisserant@0: #define led_set_state(a,b)
etisserant@0: #endif
etisserant@0: 
etisserant@0: 
etisserant@0: e_nodeState getState(CO_Data* d)
etisserant@0: {
etisserant@0:   return d->nodeState;
etisserant@0: }
etisserant@0: 
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;
etisserant@0: 		//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:
etisserant@0: 			if (d->iam_a_slave)
etisserant@0: 			{
etisserant@0: 				proceedNMTstateChange(d,m);
etisserant@0: 			}
etisserant@0: #ifdef CANOPEN_LSS_ENABLE
etisserant@0: 		default:
etisserant@0: 			if (m->cob_id.w == 0x7E4 || m->cob_id.w == 0x705)
etisserant@0: 			{
etisserant@0: 				proceedLSS(d,m);
etisserant@0: 			}
etisserant@0: #endif
etisserant@0: 	}
etisserant@0: }
etisserant@0: 
etisserant@0: #define StartOrStop(CommType, FuncStart, FuncStop) \
etisserant@0: 	if(newCommunicationState->CommType && !d->CurrentCommunicationState.CommType){\
etisserant@0: 		MSG_ERR(0x9999,#FuncStart, 9999);\
etisserant@0: 		d->CurrentCommunicationState.CommType = 1;\
etisserant@0: 		FuncStart;\
etisserant@0: 	}else if(!newCommunicationState->CommType && d->CurrentCommunicationState.CommType){\
etisserant@0: 		MSG_ERR(0x9999,#FuncStop, 9999);\
etisserant@0: 		d->CurrentCommunicationState.CommType = 0;\
etisserant@0: 		FuncStop;\
etisserant@0: 	}
etisserant@0: #define None
etisserant@0: 
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))
etisserant@0: //	StartOrStop(Emergency,,)
etisserant@0: 	StartOrStop(csPDO,	None,	None)
dejoigny@14: 	StartOrStop(csBoot_Up,	None,	slaveSendBootUp(d))
etisserant@0: }
etisserant@0: 
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@0: 				s_state_communication newCommunicationState = {
etisserant@0: 					csBoot_Up: 1,
etisserant@0: 					csSDO: 0,
etisserant@0: 					csEmergency: 0,
etisserant@0: 					csSYNC: 0,
etisserant@0: 					csHeartbeat: 0,
etisserant@0: 					csPDO: 0};
etisserant@0: 				// 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);
etisserant@0: 				// call user app related state func.
etisserant@0: 				(*d->initialisation)();
etisserant@0: 			}
etisserant@0: 			break;
etisserant@0: 								
etisserant@0: 			case Pre_operational:
etisserant@0: 			{
etisserant@0: 				s_state_communication newCommunicationState = {
etisserant@0: 					csBoot_Up: 0,
etisserant@0: 					csSDO: 1,
etisserant@0: 					csEmergency: 1,
etisserant@0: 					csSYNC: 1,
etisserant@0: 					csHeartbeat: 1,
etisserant@0: 					csPDO: 0};
etisserant@0: 				d->nodeState = Pre_operational;
etisserant@0: 				newState = Pre_operational;
etisserant@0: 				switchCommunicationState(d, &newCommunicationState);
etisserant@0: 				(*d->preOperational)();
etisserant@0: 			}
etisserant@0: 			break;
etisserant@0: 								
etisserant@0: 			case Operational:
etisserant@0: 			if(d->nodeState == Initialisation) return 0xFF;
etisserant@0: 			{
etisserant@0: 				s_state_communication newCommunicationState = {
etisserant@0: 					csBoot_Up: 0,
etisserant@0: 					csSDO: 1,
etisserant@0: 					csEmergency: 1,
etisserant@0: 					csSYNC: 1,
etisserant@0: 					csHeartbeat: 1,
etisserant@0: 					csPDO: 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: 			{
etisserant@0: 				s_state_communication newCommunicationState = {
etisserant@0: 					csBoot_Up: 0,
etisserant@0: 					csSDO: 0,
etisserant@0: 					csEmergency: 0,
etisserant@0: 					csSYNC: 0,
etisserant@0: 					csHeartbeat: 1,
etisserant@0: 					csPDO: 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;
etisserant@0: 		}//end switch case
etisserant@0: 
etisserant@0: 		led_set_state(d, newState);		
etisserant@0: 	}
etisserant@0: 	return 0;
etisserant@0: }
etisserant@0: 
etisserant@0: UNS8 getNodeId(CO_Data* d)
etisserant@0: {
etisserant@0:   return *d->bDeviceNodeId;
etisserant@0: }
etisserant@0: 
etisserant@0: void setNodeId(CO_Data* d, UNS8 nodeId)
etisserant@0: {
etisserant@0:   UNS16 offset = d->firstIndex->SDO_SVR;
etisserant@0:   if(offset){
etisserant@0:       //cob_id_client = 0x600 + nodeId;
etisserant@0:       *(UNS32*)d->objdict[offset].pSubindex[1].pObject = 0x600 + nodeId;
etisserant@0:       //cob_id_server = 0x580 + nodeId;
etisserant@0:       *(UNS32*)d->objdict[offset].pSubindex[2].pObject = 0x580 + nodeId;
etisserant@0:       // node Id client. As we do not know the value, we put the node Id Server
etisserant@0:       //*(UNS8*)d->objdict[offset].pSubindex[3].pObject = nodeId;
etisserant@0:   }
etisserant@0: 
etisserant@0:   // ** Initialize the server(s) SDO parameters
etisserant@0:   // Remember that only one SDO server is allowed, defined at index 0x1200
etisserant@0:  
etisserant@0:   // ** Initialize the client(s) SDO parameters  
etisserant@0:   // Nothing to initialize (no default values required by the DS 401)
etisserant@0:   // ** Initialize the receive PDO communication parameters. Only for 0x1400 to 0x1403
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@0:       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:   }
etisserant@0:   // ** 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@0:       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:   }
etisserant@0:   // bDeviceNodeId is defined in the object dictionary.
etisserant@0:   *d->bDeviceNodeId = nodeId;
etisserant@0: }