From: greg <greg>
Date: Fri, 4 May 2007 10:26:46 +0000 (+0000)
Subject: Add concise DCF support
X-Git-Tag: jan-benda-base~78
X-Git-Url: https://rtime.felk.cvut.cz/gitweb/CanFestival-3.git/commitdiff_plain/20dc75597e2f6f789fc2deb39ab8b81d57c45066

Add concise DCF support
---

diff --git a/doc/manual/en/manual.odt b/doc/manual/en/manual.odt
index 226fce9..e196fd3 100644
Binary files a/doc/manual/en/manual.odt and b/doc/manual/en/manual.odt differ
diff --git a/include/data.h b/include/data.h
index cdc64f7..dd08ade 100644
--- a/include/data.h
+++ b/include/data.h
@@ -90,6 +90,11 @@ struct struct_CO_Data {
 	CAN_HANDLE canHandle;	
 	scanIndexOD_t scanIndexOD;
 	storeODSubIndex_t storeODSubIndex; 
+	
+	/* DCF concise */
+	void* dcf_cursor;
+	UNS32 dcf_count_targets;
+	
 };
 
 #define NMTable_Initializer Unknown_state,
@@ -173,7 +178,9 @@ struct struct_CO_Data {
 	0,                                         /* toggle */\
 	NULL,                   /* canSend */\
 	NODE_PREFIX ## _scanIndexOD,                /* scanIndexOD */\
-	_storeODSubIndex                /* storeODSubIndex */\
+	_storeODSubIndex,                /* storeODSubIndex */\
+	NULL,		/*dcf_cursor*/\
+	1		/*dcf_count_targets*/\
 }
 
 #endif /* __data_h__ */
diff --git a/include/dcf.h b/include/dcf.h
new file mode 100644
index 0000000..ee722cd
--- /dev/null
+++ b/include/dcf.h
@@ -0,0 +1,24 @@
+/*
+This file is part of CanFestival, a library implementing CanOpen Stack. 
+
+Copyright (C): Edouard TISSERANT and Francis DUPIN
+
+See COPYING file for copyrights details.
+
+This library is free software; you can redistribute it and/or
+modify it under the terms of the GNU Lesser General Public
+License as published by the Free Software Foundation; either
+version 2.1 of the License, or (at your option) any later version.
+
+This library is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public
+License along with this library; if not, write to the Free Software
+Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+*/
+
+
+UNS32 decompo_dcf(CO_Data* d, UNS8 nodeId);
diff --git a/src/Makefile.in b/src/Makefile.in
index 10b9d87..c93c7f1 100644
--- a/src/Makefile.in
+++ b/src/Makefile.in
@@ -34,7 +34,7 @@ TIMERS_DRIVER = SUB_TIMERS_DRIVER
 INCLUDES = -I../include -I../include/$(TARGET) -I../include/$(CAN_DRIVER) -I../include/$(TIMERS_DRIVER) -I../drivers/$(TARGET)
 
 OBJS = $(TARGET)_objacces.o $(TARGET)_lifegrd.o $(TARGET)_sdo.o\
-	    $(TARGET)_pdo.o $(TARGET)_sync.o $(TARGET)_nmtSlave.o $(TARGET)_nmtMaster.o $(TARGET)_states.o $(TARGET)_timer.o
+	    $(TARGET)_pdo.o $(TARGET)_sync.o $(TARGET)_nmtSlave.o $(TARGET)_nmtMaster.o $(TARGET)_states.o $(TARGET)_timer.o $(TARGET)_dcf.o
 
 # # # # Target specific paramters # # # #
 
diff --git a/src/dcf.c b/src/dcf.c
new file mode 100644
index 0000000..dc11cb3
--- /dev/null
+++ b/src/dcf.c
@@ -0,0 +1,118 @@
+/*
+This file is part of CanFestival, a library implementing CanOpen Stack. 
+
+Copyright (C): Edouard TISSERANT and Francis DUPIN
+
+See COPYING file for copyrights details.
+
+This library is free software; you can redistribute it and/or
+modify it under the terms of the GNU Lesser General Public
+License as published by the Free Software Foundation; either
+version 2.1 of the License, or (at your option) any later version.
+
+This library is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public
+License along with this library; if not, write to the Free Software
+Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+*/
+
+#include "objacces.h"
+#include "sdo.h"
+#include "dcf.h"
+
+const indextable *ptrTable;
+
+static void CheckSDOAndContinue(CO_Data* d, UNS8 nodeId)
+{
+	UNS32 res;
+	UNS8 line;
+	UNS8 err;
+	UNS16 Index;
+	UNS8 SubIndex;
+	UNS32 abortCode;
+	
+	if(getWriteResultNetworkDict (d, nodeId, &abortCode) != SDO_FINISHED)
+		printf("Master : Failed in initializing slave %2.2x, AbortCode :%4.4x \n", nodeId, abortCode);
+	
+	closeSDOtransfer(d, nodeId, SDO_CLIENT);
+	res = decompo_dcf(d,nodeId);
+}
+
+UNS32 decompo_dcf(CO_Data* d,UNS8 nodeId)
+{
+		UNS32 errorCode;
+		UNS16 target_Index;
+		UNS8 target_Subindex;
+		UNS32 target_Size;
+		void* target_data = NULL;
+		UNS32 res;
+  		ODCallback_t *Callback;
+
+		ptrTable = (*d->scanIndexOD)(0x1F22, &errorCode, &Callback);
+		if (errorCode != OD_SUCCESSFUL)
+		{
+    		return errorCode;
+		}
+
+		/*Loop on all Nodes supported in DCF subindexes*/
+		while (nodeId < ptrTable->bSubCount){
+			UNS32 nb_targets;
+	  		
+	  		UNS8 szData = ptrTable->pSubindex[nodeId].size;
+	  		void* dcfend;
+	  		
+		  	{
+			  	void* dcf = *((void**)ptrTable->pSubindex[nodeId].pObject);
+		  		dcfend = dcf + szData;
+				if (!d->dcf_cursor)	{
+					d->dcf_cursor = dcf + 4;
+					d->dcf_count_targets = 0;
+				}
+	#ifdef CANOPEN_BIG_ENDIAN
+				nb_targets = ((UNS8*)d->dcf++) | ((UNS8*)d->dcf++) << 8 | ((UNS8*)d->dcf++) << 16 | ((UNS8*)d->dcf++) << 24;
+	#else
+				nb_targets = *((UNS32*)dcf);
+	#endif
+		  	}
+			
+			// condition on consise DCF string for NodeID, if big enough
+			if(d->dcf_cursor + 7 < dcfend && d->dcf_count_targets < nb_targets)
+			{
+				// pointer to the DCF string for NodeID
+	#ifdef CANOPEN_BIG_ENDIAN
+				target_Index = ((UNS8*)d->dcf_cursor++) | ((UNS8*)d->dcf_cursor++) << 8;
+				target_Subindex = ((UNS8*)d->dcf_cursor++);
+				target_Size = ((UNS8*)d->dcf_cursor++) | ((UNS8*)d->dcf_cursor++) << 8 | ((UNS8*)d->dcf_cursor++) << 16 | ((UNS8*)d->dcf_cursor++) << 24;
+	#else
+				target_Index = *((UNS16*)(d->dcf_cursor)); d->dcf_cursor += 2;
+				target_Subindex = *((UNS8*)(d->dcf_cursor++));
+				target_Size = *((UNS32*)(d->dcf_cursor)); d->dcf_cursor += 4;
+	#endif
+				
+					printf("Master : ConfigureSlaveNode %2.2x (Concise DCF)\n",nodeId);
+					res = writeNetworkDictCallBack(d, /*CO_Data* d*/
+							nodeId, /*UNS8 nodeId*/
+							target_Index, /*UNS16 index*/
+							target_Subindex, /*UNS8 subindex*/
+							target_Size, /*UNS8 count*/
+							0, /*UNS8 dataType*/
+							d->dcf_cursor,/*void *data*/
+							CheckSDOAndContinue); /*SDOCallback_t Callback*/					
+					/*Push d->dcf_cursor to the end of data*/
+					
+					d->dcf_cursor += target_Size;
+					d->dcf_count_targets++;
+					
+					return ;
+			}			
+				nodeId++;
+				d->dcf_cursor = NULL;
+		}
+		/* Switch Master to preOperational state */
+		(*d->preOperational)();
+		
+}
diff --git a/src/states.c b/src/states.c
index dd7bb58..c2c2cbc 100644
--- a/src/states.c
+++ b/src/states.c
@@ -110,16 +110,26 @@ UNS8 setState(CO_Data* d, e_nodeState newState)
 				switchCommunicationState(d, &newCommunicationState);
 				/* call user app related state func. */
 				(*d->initialisation)();
+				
 			}
 			break;
 								
 			case Pre_operational:
 			{
+				
 				s_state_communication newCommunicationState = {0, 1, 1, 1, 1, 0};
 				d->nodeState = Pre_operational;
 				newState = Pre_operational;
 				switchCommunicationState(d, &newCommunicationState);
-				(*d->preOperational)();
+				if (!(*(d->iam_a_slave)))
+				{
+					UNS32 res;
+					res = decompo_dcf(d,0x01);
+				}
+				else 
+				{
+					(*d->preOperational)();
+				}
 			}
 			break;