beremiz

Added PLC tick alignement on external synchronization source feature.

2008-06-27, etisserant
2390b409eb93
Parents 8b3faaf3715e
Children 66a4dc7f534a
Added PLC tick alignement on external synchronization source feature.
8 files changed, 234 insertions(+), 66 deletions(-)
  • +13 -4
    plugger.py
  • +56 -37
    plugins/canfestival/canfestival.py
  • +27 -5
    plugins/canfestival/cf_runtime.c
  • +41 -15
    runtime/plc_Linux_main.c
  • +94 -0
    runtime/plc_common_main.c
  • +2 -2
    tests/linux/test_master/plc.xml
  • +0 -2
    tests/linux/test_slave/canopen@canfestival/slave@CanOpenSlave/baseplugin.xml
  • +1 -1
    tests/linux/test_slave/canopen@canfestival/slave@CanOpenSlave/plugin.xml
    • --- 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"/>