# HG changeset patch
# User mwildbolz
# Date 1349103696 -7200
# Node ID bfb63ff7106e52268599f375972866d70c7fc6bd
# Parent 5cbb8e9ad67c12afa7c6de73c768631c2ffd3953
Implementation of a basic working node-guarding
Documentation adapted
Typos corrected
diff -r 5cbb8e9ad67c -r bfb63ff7106e include/data.h
--- a/include/data.h Mon Oct 01 16:58:35 2012 +0200
+++ b/include/data.h Mon Oct 01 17:01:36 2012 +0200
@@ -89,6 +89,14 @@
heartbeatError_t heartbeatError;
e_nodeState NMTable[NMT_MAX_NODE_ID];
+ /* NMT-nodeguarding */
+ TIMER_HANDLE GuardTimeTimer;
+ TIMER_HANDLE LifeTimeTimer;
+ nodeguardError_t nodeguardError;
+ UNS16 *GuardTime;
+ UNS8 *LifeTimeFactor;
+ UNS8 nodeGuardStatus[NMT_MAX_NODE_ID];
+
/* SYNC */
TIMER_HANDLE syncTimer;
UNS32 *COB_ID_Sync;
@@ -131,10 +139,11 @@
};
#define NMTable_Initializer Unknown_state,
+#define nodeGuardStatus_Initializer 0x00,
#ifdef SDO_DYNAMIC_BUFFER_ALLOCATION
#define s_transfer_Initializer {\
- 0, /* CliServNbr */\
+ 0, /* CliServ{REPEAT_NMT_MAX_NODE_ID_TIMES(NMTable_Initializer)},Nbr */\
0, /* wohami */\
SDO_RESET, /* state */\
0, /* toggle */\
@@ -284,6 +293,14 @@
{REPEAT_NMT_MAX_NODE_ID_TIMES(NMTable_Initializer)},\
/* is well initialized at "Unknown_state". Is it ok ? (FD)*/\
\
+ /* NMT-nodeguarding */\
+ TIMER_NONE, /* GuardTimeTimer */\
+ TIMER_NONE, /* LifeTimeTimer */\
+ _nodeguardError, /* nodeguardError */\
+ & NODE_PREFIX ## _obj100C, /* GuardTime */\
+ & NODE_PREFIX ## _obj100D, /* LifeTimeFactor */\
+ {REPEAT_NMT_MAX_NODE_ID_TIMES(nodeGuardStatus_Initializer)},\
+ \
/* SYNC */\
TIMER_NONE, /* syncTimer */\
& NODE_PREFIX ## _obj1005, /* COB_ID_Sync */\
diff -r 5cbb8e9ad67c -r bfb63ff7106e include/lifegrd.h
--- a/include/lifegrd.h Mon Oct 01 16:58:35 2012 +0200
+++ b/include/lifegrd.h Mon Oct 01 17:01:36 2012 +0200
@@ -27,7 +27,28 @@
* informed about that event.
* @ingroup comobj
*/
-
+
+/** @defgroup nodeguardo Node-guarding Object
+ * The node-guarding mechanism for a device is established through cyclically polling all slaves by the NMT
+ * master. If one polled slave does not respond during a specified time (LifeTime), the local application
+ * will be informed about that event.
+ * It is also possible for the slaves to monitor the node-guarding requests coming from the master to
+ * determine, if the master operates in a right way
+ * @ingroup comobj
+ *
+ * @todo The implementation is very basic. The toggle bit of the nodes confirmation is not checked at the moment
+ */
+
+/**
+** @file lifegrd.h
+** @author Markus WILDBOLZ
+** @date Mon Oct 01 14:44:36 CEST 2012
+**
+** @brief
+**
+**
+*/
+
#ifndef __lifegrd_h__
#define __lifegrd_h__
@@ -43,11 +64,47 @@
typedef void (*post_SlaveStateChange_t)(CO_Data*, UNS8, e_nodeState);
void _post_SlaveStateChange(CO_Data* d, UNS8 nodeId, e_nodeState newNodeState);
+typedef void (*nodeguardError_t)(CO_Data*, UNS8);
+void _nodeguardError(CO_Data* d, UNS8 id);
+
#include "data.h"
/*************************************************************************
* Functions
*************************************************************************/
+/**
+ * @brief Start node guarding with respect to 0x100C and 0x100D
+ * in the object dictionary
+ *
+ * @param *d Pointer on a CAN object data structure
+ * @ingroup nodeguardo
+ */
+void nodeguardInit(CO_Data* d);
+
+/**
+ * @brief Stop producing node guarding messages
+ *
+ * @param *d Pointer on a CAN object data structure
+ * @ingroup nodeguardo
+ */
+void nodeguardStop(CO_Data* d);
+
+/**
+ * @brief Start the life guarding service (heartbeat/node guarding).
+ * This service handles NMT error control messages either by using
+ * heartbeats and/or by using node guarding messages (defined via the
+ * object dictionary)
+ *
+ * @param *d Pointer on a CAN object data structure
+ */
+void lifeGuardInit(CO_Data* d);
+
+/**
+ * @brief Stop the life guarding service (heartbeat/node guarding).
+ *
+ * @param *d Pointer on a CAN object data structure
+ */
+void lifeGuardStop(CO_Data* d);
/**
* @ingroup statemachine
@@ -65,23 +122,26 @@
* with respect to 0x1016 and 0x1017
* object dictionary entries
* @param *d Pointer on a CAN object data structure
+ * @ingroup heartbeato
*/
void heartbeatInit(CO_Data* d);
/**
* @brief Stop heartbeat consumer and producer
* @param *d Pointer on a CAN object data structure
+ * @ingroup heartbeato
*/
void heartbeatStop(CO_Data* d);
/**
* @brief This function is responsible to process a canopen-message which seams to be an NMT Error Control
- * Messages. At them moment we assume that every NMT error control message
- * is a heartbeat message.
+ * Messages.
* If a BootUp message is detected, it will return the nodeId of the Slave who booted up
* @param *d Pointer on a CAN object data structure
* @param *m Pointer on the CAN-message which has to be analysed.
+ * @ingroup nodeguardo
*/
void proceedNODE_GUARD (CO_Data* d, Message* m);
+
#endif /*__lifegrd_h__ */
diff -r 5cbb8e9ad67c -r bfb63ff7106e include/states.h
--- a/include/states.h Mon Oct 01 16:58:35 2012 +0200
+++ b/include/states.h Mon Oct 01 17:01:36 2012 +0200
@@ -55,7 +55,7 @@
INTEGER8 csSDO;
INTEGER8 csEmergency;
INTEGER8 csSYNC;
- INTEGER8 csHeartbeat;
+ INTEGER8 csLifeGuard;
INTEGER8 csPDO;
INTEGER8 csLSS;
} s_state_communication;
diff -r 5cbb8e9ad67c -r bfb63ff7106e objdictgen/Makefile.in
--- a/objdictgen/Makefile.in Mon Oct 01 16:58:35 2012 +0200
+++ b/objdictgen/Makefile.in Mon Oct 01 17:01:36 2012 +0200
@@ -28,7 +28,7 @@
gnosis: gnosis/version.py
gnosis/version.py:
- #wget http://gnosis.cx/download/Gnosis_Utils-current.tar.gz
+ wget http://gnosis.cx/download/Gnosis_Utils-current.tar.gz
mkdir -p gnosis_extract
tar xzof Gnosis_Utils-current.tar.gz -C gnosis_extract
mv gnosis_extract/Gnosis_Utils-*/gnosis .
diff -r 5cbb8e9ad67c -r bfb63ff7106e objdictgen/gen_cfile.py
--- a/objdictgen/gen_cfile.py Mon Oct 01 16:58:35 2012 +0200
+++ b/objdictgen/gen_cfile.py Mon Oct 01 17:01:36 2012 +0200
@@ -416,6 +416,20 @@
indexContents[0x1017] = """\n/* index 0x1017 : %(EntryName)s */
UNS16 %(NodeName)s_obj1017 = 0x0; /* 0 */
"""%texts
+
+ if 0x100C not in communicationlist:
+ entry_infos = Node.GetEntryInfos(0x100C)
+ texts["EntryName"] = entry_infos["name"]
+ indexContents[0x100C] = """\n/* index 0x100C : %(EntryName)s */
+ UNS8 %(NodeName)s_obj100C = 0x0; /* 0 */
+"""%texts
+
+ if 0x100D not in communicationlist:
+ entry_infos = Node.GetEntryInfos(0x100D)
+ texts["EntryName"] = entry_infos["name"]
+ indexContents[0x100D] = """\n/* index 0x100D : %(EntryName)s */
+ UNS16 %(NodeName)s_obj100D = 0x0; /* 0 */
+"""%texts
#-------------------------------------------------------------------------------
# Declaration of navigation in the Object Dictionary
diff -r 5cbb8e9ad67c -r bfb63ff7106e src/lifegrd.c
--- a/src/lifegrd.c Mon Oct 01 16:58:35 2012 +0200
+++ b/src/lifegrd.c Mon Oct 01 17:01:36 2012 +0200
@@ -39,21 +39,14 @@
#include "sysdep.h"
-void ConsumerHearbeatAlarm(CO_Data* d, UNS32 id);
-
-
-void ProducerHearbeatAlarm(CO_Data* d, UNS32 id);
-
-UNS32 OnHearbeatProducerUpdate(CO_Data* d, const indextable * unsused_indextable, UNS8 unsused_bSubindex);
-
-/*!
-**
-**
-** @param d
-** @param nodeId
-**
-** @return
-**/
+void ConsumerHeartbeatAlarm(CO_Data* d, UNS32 id);
+void ProducerHeartbeatAlarm(CO_Data* d, UNS32 id);
+UNS32 OnHearbeatProducerUpdate(CO_Data* d, const indextable * unused_indextable, UNS8 unused_bSubindex);
+
+void GuardTimeAlarm(CO_Data* d, UNS32 id);
+UNS32 OnNodeGuardUpdate(CO_Data* d, const indextable * unused_indextable, UNS8 unused_bSubindex);
+
+
e_nodeState getNodeState (CO_Data* d, UNS8 nodeId)
{
e_nodeState networkNodeState = Unknown_state;
@@ -69,8 +62,9 @@
**
** @param d
** @param id
+ * @ingroup heartbeato
**/
-void ConsumerHearbeatAlarm(CO_Data* d, UNS32 id)
+void ConsumerHeartbeatAlarm(CO_Data* d, UNS32 id)
{
UNS8 nodeId = (UNS8)(((d->ConsumerHeartbeatEntries[id]) & (UNS32)0x00FF0000) >> (UNS8)16);
/*MSG_WAR(0x00, "ConsumerHearbeatAlarm", 0x00);*/
@@ -85,12 +79,6 @@
(*d->heartbeatError)(d, nodeId);
}
-/*!
-**
-**
-** @param d
-** @param m
-**/
void proceedNODE_GUARD(CO_Data* d, Message* m )
{
UNS8 nodeId = (UNS8) GET_NODE_ID((*m));
@@ -133,6 +121,11 @@
MSG_WAR(0x3110, "Received NMT nodeId : ", nodeId);
+ /*!
+ ** Record node response for node guarding service
+ */
+ d->nodeGuardStatus[nodeId] = *d->LifeTimeFactor;
+
if (d->NMTable[nodeId] != newNodeState)
{
(*d->post_SlaveStateChange)(d, nodeId, newNodeState);
@@ -154,29 +147,30 @@
}
if( d->NMTable[nodeId] != Unknown_state ) {
- UNS8 index, ConsummerHeartBeat_nodeId ;
+ UNS8 index, ConsumerHeartBeat_nodeId ;
for( index = (UNS8)0x00; index < *d->ConsumerHeartbeatCount; index++ )
{
- ConsummerHeartBeat_nodeId = (UNS8)( ((d->ConsumerHeartbeatEntries[index]) & (UNS32)0x00FF0000) >> (UNS8)16 );
- if ( nodeId == ConsummerHeartBeat_nodeId )
+ ConsumerHeartBeat_nodeId = (UNS8)( ((d->ConsumerHeartbeatEntries[index]) & (UNS32)0x00FF0000) >> (UNS8)16 );
+ if ( nodeId == ConsumerHeartBeat_nodeId )
{
TIMEVAL time = ( (d->ConsumerHeartbeatEntries[index]) & (UNS32)0x0000FFFF ) ;
/* Renew alarm for next heartbeat. */
DelAlarm(d->ConsumerHeartBeatTimers[index]);
- d->ConsumerHeartBeatTimers[index] = SetAlarm(d, index, &ConsumerHearbeatAlarm, MS_TO_TIMEVAL(time), 0);
+ d->ConsumerHeartBeatTimers[index] = SetAlarm(d, index, &ConsumerHeartbeatAlarm, MS_TO_TIMEVAL(time), 0);
}
}
}
}
}
-/*! The Consumer Timer Callback
+/*! The Producer Timer Callback
**
**
** @param d
** @param id
+ * @ingroup heartbeato
**/
-void ProducerHearbeatAlarm(CO_Data* d, UNS32 id)
+void ProducerHeartbeatAlarm(CO_Data* d, UNS32 id)
{
if(*d->ProducerHeartBeatTime)
{
@@ -200,27 +194,97 @@
}
}
+/**
+ * @brief The guardTime - Timer Callback.
+ *
+ * This function is called every GuardTime (OD 0x100C) ms
+ * On every call, a NodeGuard-Request is sent to all nodes which have a
+ * node-state not equal to "Unknown" (according to NMTable). If the node has
+ * not responded within the lifetime, the nodeguardError function is called and
+ * the status of this node is set to "Disconnected"
+ *
+ * @param d Pointer on a CAN object data structure
+ * @param id
+ * @ingroup nodeguardo
+ */
+void GuardTimeAlarm(CO_Data* d, UNS32 id)
+{
+ if (*d->GuardTime) {
+ UNS8 i;
+
+ MSG_WAR(0x00, "Producing nodeguard-requests: ", 0);
+
+ for (i = 0; i < NMT_MAX_NODE_ID; i++) {
+ /** Send node guard request to all nodes except this node, if the
+ * node state is not "Unknown_state"
+ */
+ if (d->NMTable[i] != Unknown_state && i != *d->bDeviceNodeId) {
+
+ /** Check if the node has confirmed the guarding request within
+ * the LifeTime (GuardTime x LifeTimeFactor)
+ */
+ if (d->nodeGuardStatus[i] <= 0) {
+
+ MSG_WAR(0x00, "Node Guard alarm for nodeId : ", i);
+
+ // Call error-callback function
+ if (*d->nodeguardError) {
+ (*d->nodeguardError)(d, i);
+ }
+
+ // Mark node as disconnected
+ d->NMTable[i] = Disconnected;
+
+ }
+
+ d->nodeGuardStatus[i]--;
+
+ masterSendNMTnodeguard(d, i);
+
+ }
+ }
+ } else {
+ d->GuardTimeTimer = DelAlarm(d->GuardTimeTimer);
+ }
+
+
+
+}
+
+/**
+ * This function is called, if index 0x100C or 0x100D is updated to
+ * restart the node-guarding service with the new parameters
+ *
+ * @param d Pointer on a CAN object data structure
+ * @param unused_indextable
+ * @param unused_bSubindex
+ * @ingroup nodeguardo
+ */
+UNS32 OnNodeGuardUpdate(CO_Data* d, const indextable * unused_indextable, UNS8 unused_bSubindex)
+{
+ nodeguardStop(d);
+ nodeguardInit(d);
+ return 0;
+}
+
+
/*! This is called when Index 0x1017 is updated.
**
**
** @param d
-** @param unsused_indextable
-** @param unsused_bSubindex
+** @param unused_indextable
+** @param unused_bSubindex
**
** @return
+ * @ingroup heartbeato
**/
-UNS32 OnHeartbeatProducerUpdate(CO_Data* d, const indextable * unsused_indextable, UNS8 unsused_bSubindex)
+UNS32 OnHeartbeatProducerUpdate(CO_Data* d, const indextable * unused_indextable, UNS8 unused_bSubindex)
{
heartbeatStop(d);
heartbeatInit(d);
return 0;
}
-/*!
-**
-**
-** @param d
-**/
void heartbeatInit(CO_Data* d)
{
@@ -232,25 +296,45 @@
for( index = (UNS8)0x00; index < *d->ConsumerHeartbeatCount; index++ )
{
TIMEVAL time = (UNS16) ( (d->ConsumerHeartbeatEntries[index]) & (UNS32)0x0000FFFF ) ;
- /* MSG_WAR(0x3121, "should_time : ", should_time ) ; */
if ( time )
{
- d->ConsumerHeartBeatTimers[index] = SetAlarm(d, index, &ConsumerHearbeatAlarm, MS_TO_TIMEVAL(time), 0);
+ d->ConsumerHeartBeatTimers[index] = SetAlarm(d, index, &ConsumerHeartbeatAlarm, MS_TO_TIMEVAL(time), 0);
}
}
if ( *d->ProducerHeartBeatTime )
{
TIMEVAL time = *d->ProducerHeartBeatTime;
- d->ProducerHeartBeatTimer = SetAlarm(d, 0, &ProducerHearbeatAlarm, MS_TO_TIMEVAL(time), MS_TO_TIMEVAL(time));
- }
-}
-
-/*!
-**
-**
-** @param d
-**/
+ d->ProducerHeartBeatTimer = SetAlarm(d, 0, &ProducerHeartbeatAlarm, MS_TO_TIMEVAL(time), MS_TO_TIMEVAL(time));
+ }
+}
+
+
+void nodeguardInit(CO_Data* d)
+{
+
+ RegisterSetODentryCallBack(d, 0x100C, 0x00, &OnNodeGuardUpdate);
+ RegisterSetODentryCallBack(d, 0x100D, 0x00, &OnNodeGuardUpdate);
+
+ if (*d->GuardTime && *d->LifeTimeFactor) {
+ UNS8 i;
+
+ TIMEVAL time = *d->GuardTime;
+ d->GuardTimeTimer = SetAlarm(d, 0, &GuardTimeAlarm, MS_TO_TIMEVAL(time), MS_TO_TIMEVAL(time));
+ MSG_WAR(0x0, "GuardTime: ", time);
+
+ for (i = 0; i < NMT_MAX_NODE_ID; i++) {
+ /** Set initial value for the nodes */
+ if (d->NMTable[i] != Unknown_state && i != *d->bDeviceNodeId) {
+ d->nodeGuardStatus[i] = *d->LifeTimeFactor;
+ }
+ }
+
+ MSG_WAR(0x0, "Timer for node-guarding startet", 0);
+ }
+
+}
+
void heartbeatStop(CO_Data* d)
{
UNS8 index;
@@ -262,12 +346,28 @@
d->ProducerHeartBeatTimer = DelAlarm(d->ProducerHeartBeatTimer);
}
-/*!
-**
-**
-** @param heartbeatID
-**/
+void nodeguardStop(CO_Data* d)
+{
+ d->GuardTimeTimer = DelAlarm(d->GuardTimeTimer);
+}
+
+
+void lifeGuardInit(CO_Data* d)
+{
+ heartbeatInit(d);
+ nodeguardInit(d);
+}
+
+
+void lifeGuardStop(CO_Data* d)
+{
+ heartbeatStop(d);
+ nodeguardStop(d);
+}
+
+
void _heartbeatError(CO_Data* d, UNS8 heartbeatID){}
void _post_SlaveBootup(CO_Data* d, UNS8 SlaveID){}
void _post_SlaveStateChange(CO_Data* d, UNS8 nodeId, e_nodeState newNodeState){}
-
+void _nodeguardError(CO_Data* d, UNS8 id){}
+
diff -r 5cbb8e9ad67c -r bfb63ff7106e src/states.c
--- a/src/states.c Mon Oct 01 16:58:35 2012 +0200
+++ b/src/states.c Mon Oct 01 17:01:36 2012 +0200
@@ -92,7 +92,7 @@
proceedSDO(d,m);
break;
case NODE_GUARD:
- if (d->CurrentCommunicationState.csHeartbeat)
+ if (d->CurrentCommunicationState.csLifeGuard)
proceedNODE_GUARD(d,m);
break;
case NMT:
@@ -142,7 +142,7 @@
#endif
StartOrStop(csSDO, None, resetSDO(d))
StartOrStop(csSYNC, startSYNC(d), stopSYNC(d))
- StartOrStop(csHeartbeat, heartbeatInit(d), heartbeatStop(d))
+ StartOrStop(csLifeGuard, lifeGuardInit(d), lifeGuardStop(d))
StartOrStop(csEmergency, emergencyInit(d), emergencyStop(d))
StartOrStop(csPDO, PDOInit(d), PDOStop(d))
StartOrStop(csBoot_Up, None, slaveSendBootUp(d))