--- a/plugger.py Fri Jun 27 09:38:16 2008 +0200
+++ b/plugger.py Fri Jun 27 16:21:22 2008 +0200
@@ -2,7 +2,7 @@
Base definitions for beremiz plugins
"""
-import os,sys
+import os,sys,traceback
import plugins
import types
import shutil
@@ -666,6 +666,14 @@
<xsd:attribute name="CFLAGS" type="xsd:string" use="required"/>
<xsd:attribute name="Linker" type="xsd:string" use="optional" default="ld"/>
<xsd:attribute name="LDFLAGS" type="xsd:string" use="required"/>
+ <xsd:attribute name="Sync_Align_Ratio" use="optional" default="50">
+ <xsd:simpleType>
+ <xsd:restriction base="xsd:integer">
+ <xsd:minInclusive value="1"/>
+ <xsd:maxInclusive value="99"/>
+ </xsd:restriction>
+ </xsd:simpleType>
+ </xsd:attribute>
</xsd:complexType>
</xsd:element>
</xsd:schema>
@@ -955,9 +963,9 @@
buildpath,
self.PLCGeneratedLocatedVars,
logger)
- except Exception, msg:
+ except Exception, exc:
logger.write_error("Plugins code generation Failed !\n")
- logger.write_error(str(msg))
+ logger.write_error(traceback.format_exc())
return False
@@ -976,7 +984,8 @@
"retrieve_calls":"\n ".join(["__retrieve_%(s)s();"%{'s':locstr} for locstr in locstrs]),
"publish_calls":"\n ".join(["__publish_%(s)s();"%{'s':locstr} for locstr in locstrs]),
"init_calls":"\n ".join(["init_level++; if(res = __init_%(s)s(argc,argv)) return res;"%{'s':locstr} for locstr in locstrs]),
- "cleanup_calls":"\n ".join(["if(init_level-- > 0) __cleanup_%(s)s();"%{'s':locstr} for locstr in locstrs])}
+ "cleanup_calls":"\n ".join(["if(init_level-- > 0) __cleanup_%(s)s();"%{'s':locstr} for locstr in locstrs]),
+ "sync_align_ratio":self.BeremizRoot.getSync_Align_Ratio()}
target_name = self.BeremizRoot.TargetType.content["name"]
plc_main += runtime.code("plc_%s_main"%target_name)
--- a/plugins/canfestival/canfestival.py Fri Jun 27 09:38:16 2008 +0200
+++ b/plugins/canfestival/canfestival.py Fri Jun 27 16:21:22 2008 +0200
@@ -5,7 +5,7 @@
from nodelist import NodeList
from nodemanager import NodeManager
-import config_utils, gen_cfile
+import config_utils, gen_cfile, eds_utils
from networkedit import networkedit
from objdictedit import objdictedit
import canfestival_config
@@ -29,6 +29,7 @@
<xsd:attribute name="CAN_Device" type="xsd:string" use="required"/>
<xsd:attribute name="CAN_Baudrate" type="xsd:string" use="required"/>
<xsd:attribute name="NodeId" type="xsd:string" use="required"/>
+ <xsd:attribute name="Sync_Align" type="xsd:integer" use="optional" default="0"/>
</xsd:complexType>
</xsd:element>
</xsd:schema>
@@ -269,14 +270,16 @@
"candriver" : self.CanFestivalInstance.getCAN_Driver(),
"nodes_includes" : "",
"board_decls" : "",
- "nodes_declare" : "",
"nodes_init" : "",
"nodes_open" : "",
"nodes_close" : "",
"nodes_send_sync" : "",
"nodes_proceed_sync" : "",
"slavebootups" : "",
- "slavebootup_register" : ""}
+ "slavebootup_register" : "",
+ "post_sync" : "",
+ "post_sync_register" : "",
+ }
for child in self.IECSortedChilds():
childlocstr = "_".join(map(str,child.GetCurrentLocation()))
nodename = "OD_%s" % childlocstr
@@ -287,52 +290,68 @@
# Not a slave -> master
child_data = getattr(child, "CanFestivalNode")
# Apply sync setting
+ format_dict["nodes_init"] += 'NODE_MASTER_INIT(%s, %s)\n '%(
+ nodename,
+ child_data.getNodeId())
if child_data.getSync_TPDOs():
format_dict["nodes_send_sync"] += 'NODE_SEND_SYNC(%s)\n '%(nodename)
format_dict["nodes_proceed_sync"] += 'NODE_PROCEED_SYNC(%s)\n '%(nodename)
- # initialize and declare node table for post_SlaveBootup lookup
-
+
+ # initialize and declare node boot status variables for post_SlaveBootup lookup
SlaveIDs = child.GetSlaveIDs()
for id in SlaveIDs:
- format_dict["slavebootups"] += """
-int %s_slave_%d_booted = 0;
-"""%(nodename, id)
- format_dict["slavebootups"] += """
-static void %s_post_SlaveBootup(CO_Data* d, UNS8 nodeId){
- switch(nodeId){
-"""%(nodename)
+ format_dict["slavebootups"] += (
+ "int %s_slave_%d_booted = 0;\n"%(nodename, id))
+ # define post_SlaveBootup lookup functions
+ format_dict["slavebootups"] += (
+ "static void %s_post_SlaveBootup(CO_Data* d, UNS8 nodeId){\n"%(nodename)+
+ " switch(nodeId){\n")
+ # one case per declared node, mark node as booted
for id in SlaveIDs:
- format_dict["slavebootups"] += """
- case %d:
- %s_slave_%d_booted = 1;
- break;
-"""%(id, nodename, id)
- format_dict["slavebootups"] += """
- default:
- break;
- }
- if( """
+ format_dict["slavebootups"] += (
+ " case %d:\n"%(id)+
+ " %s_slave_%d_booted = 1;\n"%(nodename, id)+
+ " break;\n")
+ format_dict["slavebootups"] += (
+ " default:\n"+
+ " break;\n"+
+ " }\n"+
+ " if( ")
+ # expression to test if all declared nodes booted
format_dict["slavebootups"] += " && ".join(["%s_slave_%d_booted"%(nodename, id) for id in SlaveIDs])
-
- format_dict["slavebootups"] += """ )
- Master_post_SlaveBootup(d,nodeId);
-}
-"""
- format_dict["slavebootup_register"] += """
-%s_Data.post_SlaveBootup = %s_post_SlaveBootup;
-"""%(nodename,nodename)
-
+ format_dict["slavebootups"] += " )\n" + (
+ " Master_post_SlaveBootup(d,nodeId);\n"+
+ "}\n")
+ # register previously declared func as post_SlaveBootup callback for that node
+ format_dict["slavebootup_register"] += (
+ "%s_Data.post_SlaveBootup = %s_post_SlaveBootup;\n"%(nodename,nodename))
+ else:
+ # Slave node
+ align = child_data.getSync_Align()
+ if align > 0:
+ format_dict["post_sync"] += (
+ "static int %s_CalCount = 0;\n"%(nodename)+
+ "static void %s_post_sync(CO_Data* d){\n"%(nodename)+
+ " if(%s_CalCount < %d){\n"%(nodename, align)+
+ " %s_CalCount++;\n"%(nodename)+
+ " align_tick(1);\n"+
+ " }else{\n"+
+ " align_tick(0);\n"+
+ " }\n"+
+ "}\n")
+ format_dict["post_sync_register"] += (
+ "%s_Data.post_sync = %s_post_sync;\n"%(nodename,nodename))
+ format_dict["nodes_init"] += 'NODE_SLAVE_INIT(%s, %s)\n '%(
+ nodename,
+ child_data.getNodeId())
+
+ # Include generated OD headers
format_dict["nodes_includes"] += '#include "%s.h"\n'%(nodename)
+ # Declare CAN channels according user filled config
format_dict["board_decls"] += 'BOARD_DECL(%s, "%s", "%s")\n'%(
nodename,
child_data.getCAN_Device(),
child_data.getCAN_Baudrate())
- format_dict["nodes_declare"] += 'NODE_DECLARE(%s, %s)\n '%(
- nodename,
- child_data.getNodeId())
- format_dict["nodes_init"] += 'NODE_INIT(%s, %s)\n '%(
- nodename,
- child_data.getNodeId())
format_dict["nodes_open"] += 'NODE_OPEN(%s)\n '%(nodename)
format_dict["nodes_close"] += 'NODE_CLOSE(%s)\n '%(nodename)
--- a/plugins/canfestival/cf_runtime.c Fri Jun 27 09:38:16 2008 +0200
+++ b/plugins/canfestival/cf_runtime.c Fri Jun 27 16:21:22 2008 +0200
@@ -1,16 +1,21 @@
#include "canfestival.h"
+/* CanFestival nodes generated OD headers*/
%(nodes_includes)s
#define BOARD_DECL(nodename, busname, baudrate)\
s_BOARD nodename##Board = {busname, baudrate};
+/* CAN channels declaration */
%(board_decls)s
+/* Keep track of init level to cleanup correctly */
static int init_level=0;
+/* Retrieve PLC cycle time */
extern int common_ticktime__;
+/* Called once all NetworkEdit declares slaves have booted*/
static void Master_post_SlaveBootup(CO_Data* d, UNS8 nodeId)
{
/* Put the master in operational mode */
@@ -20,23 +25,40 @@
masterSendNMTstateChange (d, 0, NMT_Start_Node);
}
+/* Per master node slavebootup callbacks. Checks that
+ * every node have booted before calling Master_post_SlaveBootup */
%(slavebootups)s
-#define NODE_INIT(nodename, nodeid) \
+/* One slave node post_sync callback.
+ * Used to align PLC tick-time on CANopen SYNC
+ */
+%(post_sync)s
+
+#define NODE_FORCE_SYNC(nodename) \
/* Artificially force sync state to 1 so that it is not started */\
nodename##_Data.CurrentCommunicationState.csSYNC = -1;\
/* Force sync period to common_ticktime__ so that other node can read it*/\
*nodename##_Data.COB_ID_Sync = 0x40000080;\
- *nodename##_Data.Sync_Cycle_Period = common_ticktime__ * 1000;\
+ *nodename##_Data.Sync_Cycle_Period = common_ticktime__ * 1000;
+
+#define NODE_INIT(nodename, nodeid) \
/* Defining the node Id */\
setNodeId(&nodename##_Data, nodeid);\
/* init */\
setState(&nodename##_Data, Initialisation);
+#define NODE_MASTER_INIT(nodename, nodeid) \
+ NODE_FORCE_SYNC(nodename) \
+ NODE_INIT(nodename, nodeid)
+
+#define NODE_SLAVE_INIT(nodename, nodeid) \
+ NODE_INIT(nodename, nodeid)
+
void InitNodes(CO_Data* d, UNS32 id)
{
+ %(slavebootup_register)s
+ %(post_sync_register)s
%(nodes_init)s
- %(slavebootup_register)s
}
void Exit(CO_Data* d, UNS32 id)
@@ -103,7 +125,7 @@
* TODO : implement buffers to avoid such a big lock
* */
EnterMutex();
- /*Send Sync */
+ /* Send Sync */
%(nodes_send_sync)s
}
@@ -112,7 +134,7 @@
void __publish_%(locstr)s()
{
- /*Call SendPDOEvent */
+ /* Process sync event */
%(nodes_proceed_sync)s
LeaveMutex();
}
--- a/runtime/plc_Linux_main.c Fri Jun 27 09:38:16 2008 +0200
+++ b/runtime/plc_Linux_main.c Fri Jun 27 16:21:22 2008 +0200
@@ -2,14 +2,46 @@
#include <string.h>
#include <time.h>
#include <signal.h>
+#include <stdlib.h>
+void PLC_GetTime(IEC_TIME *CURRENT_TIME)
+{
+ clock_gettime(CLOCK_REALTIME, CURRENT_TIME);
+}
void PLC_timer_notify(sigval_t val)
{
- clock_gettime(CLOCK_REALTIME, &__CURRENT_TIME);
+ PLC_GetTime(&__CURRENT_TIME);
__run();
}
+timer_t PLC_timer;
+
+void PLC_SetTimer(long long next, long long period)
+{
+ struct itimerspec timerValues;
+ /*
+ printf("SetTimer(%lld,%lld)\n",next, period);
+ */
+ memset (&timerValues, 0, sizeof (struct itimerspec));
+ {
+#ifdef __lldiv_t_defined
+ lldiv_t nxt_div = lldiv(next, 1000000000);
+ lldiv_t period_div = lldiv(period, 1000000000);
+ timerValues.it_value.tv_sec = nxt_div.quot;
+ timerValues.it_value.tv_nsec = nxt_div.rem;
+ timerValues.it_interval.tv_sec = period_div.quot;
+ timerValues.it_interval.tv_nsec = period_div.rem;
+#else
+ timerValues.it_value.tv_sec = next / 1000000000;
+ timerValues.it_value.tv_nsec = next % 1000000000;
+ timerValues.it_interval.tv_sec = period / 1000000000;
+ timerValues.it_interval.tv_nsec = period % 1000000000;
+#endif
+ }
+ timer_settime (PLC_timer, 0, &timerValues, NULL);
+}
+
void catch_signal(int sig)
{
signal(SIGTERM, catch_signal);
@@ -19,36 +51,30 @@
int main(int argc,char **argv)
{
- timer_t timer;
struct sigevent sigev;
- long tv_nsec = 1000000 * (maxval(common_ticktime__,1)%1000);
- time_t tv_sec = common_ticktime__/1000;
- struct itimerspec timerValues;
+ /* Translate PLC's microseconds to Ttick nanoseconds */
+ Ttick = 1000000 * maxval(common_ticktime__,1);
memset (&sigev, 0, sizeof (struct sigevent));
- memset (&timerValues, 0, sizeof (struct itimerspec));
sigev.sigev_value.sival_int = 0;
sigev.sigev_notify = SIGEV_THREAD;
sigev.sigev_notify_attributes = NULL;
sigev.sigev_notify_function = PLC_timer_notify;
- timerValues.it_value.tv_sec = tv_sec;
- timerValues.it_value.tv_nsec = tv_nsec;
- timerValues.it_interval.tv_sec = tv_sec;
- timerValues.it_interval.tv_nsec = tv_nsec;
+ timer_create (CLOCK_REALTIME, &sigev, &PLC_timer);
if( __init(argc,argv) == 0 ){
- timer_create (CLOCK_REALTIME, &sigev, &timer);
- timer_settime (timer, 0, &timerValues, NULL);
+ PLC_SetTimer(Ttick,Ttick);
/* install signal handler for manual break */
signal(SIGTERM, catch_signal);
signal(SIGINT, catch_signal);
-
+ /* Wait some signal */
pause();
-
- timer_delete (timer);
+ /* Stop the PLC */
+ PLC_SetTimer(0,0);
}
__cleanup();
+ timer_delete (PLC_timer);
return 0;
}
--- a/runtime/plc_common_main.c Fri Jun 27 09:38:16 2008 +0200
+++ b/runtime/plc_common_main.c Fri Jun 27 16:21:22 2008 +0200
@@ -1,3 +1,9 @@
+/*
+ * Prototypes for function provided by arch-specific code (main)
+ * concatained after this template
+ ** /
+
+
/*
* Functions and variables provied by generated C softPLC
**/
@@ -39,6 +45,9 @@
{
%(retrieve_calls)s
+ /*
+ printf("run tick = %%d\n", tick + 1);
+ */
config_run__(tick++);
%(publish_calls)s
@@ -63,3 +72,88 @@
%(cleanup_calls)s
}
+
+void PLC_GetTime(IEC_TIME *CURRENT_TIME);
+void PLC_SetTimer(long long next, long long period);
+
+#define CALIBRATED -2
+#define NOT_CALIBRATED -1
+static int calibration_count = NOT_CALIBRATED;
+static IEC_TIME cal_begin;
+static long long Tsync = 0;
+static long long FreqCorr = 0;
+static int Nticks = 0;
+static int last_tick = 0;
+static long long Ttick = 0;
+#define mod %%
+/*
+ * Call this on each external sync,
+ **/
+void align_tick(int calibrate)
+{
+ /*
+ printf("align_tick(%%d)\n", calibrate);
+ */
+ if(calibrate){
+ if(calibration_count == CALIBRATED)
+ /* Re-calibration*/
+ calibration_count = NOT_CALIBRATED;
+ if(calibration_count == NOT_CALIBRATED)
+ /* Calibration start, get time*/
+ PLC_GetTime(&cal_begin);
+ calibration_count++;
+ }else{
+ if(calibration_count >= 0){
+ /* End of calibration */
+ /* Get final time */
+ IEC_TIME cal_end;
+ PLC_GetTime(&cal_end);
+ /*adjust calibration_count*/
+ calibration_count++;
+ /* compute mean of Tsync, over calibration period */
+ Tsync = ((long long)(cal_end.tv_sec - cal_begin.tv_sec) * (long long)1000000000 +
+ (cal_end.tv_nsec - cal_begin.tv_nsec)) / calibration_count;
+ if( (Nticks = (Tsync / Ttick)) > 0){
+ FreqCorr = (Tsync mod Ttick); /* to be divided by Nticks */
+ }else{
+ FreqCorr = Tsync - (Ttick mod Tsync);
+ }
+ /*
+ printf("Tsync = %%ld\n", Tsync);
+ printf("calibration_count = %%d\n", calibration_count);
+ printf("Nticks = %%d\n", Nticks);
+ */
+ calibration_count = CALIBRATED;
+ }
+ if(calibration_count == CALIBRATED){
+ /* Get Elapsed time since last PLC tick (__CURRENT_TIME) */
+ IEC_TIME now;
+ long long elapsed;
+ long long Tcorr;
+ long long PhaseCorr;
+ long long PeriodicTcorr;
+ PLC_GetTime(&now);
+ elapsed = (now.tv_sec - __CURRENT_TIME.tv_sec) * 1000000000 + now.tv_nsec - __CURRENT_TIME.tv_nsec;
+ if(Nticks > 0){
+ PhaseCorr = elapsed - (Ttick + FreqCorr/Nticks)*%(sync_align_ratio)d/100; /* to be divided by Nticks */
+ Tcorr = Ttick + (PhaseCorr + FreqCorr) / Nticks;
+ if(Nticks < 2){
+ /* When Sync source period is near Tick time */
+ /* PhaseCorr may not be applied to Periodic time given to timer */
+ PeriodicTcorr = Ttick + FreqCorr / Nticks;
+ }else{
+ PeriodicTcorr = Tcorr;
+ }
+ }else if(tick > last_tick){
+ last_tick = tick;
+ PhaseCorr = elapsed - (Tsync*%(sync_align_ratio)d/100);
+ PeriodicTcorr = Tcorr = Ttick + PhaseCorr + FreqCorr;
+ }else{
+ /*PLC did not run meanwhile. Nothing to do*/
+ return;
+ }
+ /* DO ALIGNEMENT */
+ PLC_SetTimer(Tcorr - elapsed, PeriodicTcorr);
+ }
+ }
+}
--- a/tests/linux/test_master/plc.xml Fri Jun 27 09:38:16 2008 +0200
+++ b/tests/linux/test_master/plc.xml Fri Jun 27 16:21:22 2008 +0200
@@ -8,7 +8,7 @@
productVersion="1"
creationDateTime="2008-06-24 18:44:00"/>
<contentHeader name="test_slave"
- modificationDateTime="2008-06-25 16:42:30"
+ modificationDateTime="2008-06-27 15:41:26"
language="en-US">
<coordinateInfo>
<fbd>
@@ -113,7 +113,7 @@
<configurations>
<configuration name="conf">
<resource name="res">
- <task name="tache" interval="00:00:00.100000" priority="0">
+ <task name="tache" interval="00:00:00.050000" priority="0">
<pouInstance name="toto" type="test_main"/>
</task>
</resource>
--- a/tests/linux/test_slave/canopen@canfestival/slave@CanOpenSlave/baseplugin.xml Fri Jun 27 09:38:16 2008 +0200
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<BaseParams Name="slave" IEC_Channel="0"/>
--- a/tests/linux/test_slave/canopen@canfestival/slave@CanOpenSlave/plugin.xml Fri Jun 27 09:38:16 2008 +0200
+++ b/tests/linux/test_slave/canopen@canfestival/slave@CanOpenSlave/plugin.xml Fri Jun 27 16:21:22 2008 +0200
@@ -1,2 +1,2 @@
<?xml version="1.0" encoding="UTF-8"?>
-<CanFestivalSlaveNode CAN_Device="vcan0" CAN_Baudrate="125K" NodeId="3"/>
+<CanFestivalSlaveNode CAN_Device="vcan0" CAN_Baudrate="125K" NodeId="3" Sync_Align="100"/>