From 767bc887de0f415f2b7c9222aafcdb69442c3fdd Mon Sep 17 00:00:00 2001
From: Michael Zillgith <michael.zillgith@mz-automation.de>
Date: Thu, 29 Oct 2020 23:17:37 +0100
Subject: [PATCH] - IED server: fixed problems in control service tracking

---
 src/iec61850/server/mms_mapping/control.c | 34 +++++++++++++++++++----
 1 file changed, 28 insertions(+), 6 deletions(-)

diff --git a/src/iec61850/server/mms_mapping/control.c b/src/iec61850/server/mms_mapping/control.c
index 61e6f103..55f7e1bd 100644
--- a/src/iec61850/server/mms_mapping/control.c
+++ b/src/iec61850/server/mms_mapping/control.c
@@ -579,8 +579,6 @@ isSboClassOperateOnce(ControlObject* self)
         return true; /* default is operate-once ! */
 }
 
-
-
 static MmsValue*
 getOperParameterOperTime(MmsValue* operParameters)
 {
@@ -650,6 +648,14 @@ operateControl(ControlObject* self, MmsValue* value, uint64_t currentTime, bool
     return CONTROL_RESULT_OK;
 }
 
+static void
+resetAddCause(ControlObject* self)
+{
+    self->addCauseValue = ADD_CAUSE_UNKNOWN;
+
+    MmsValue_setInt32(self->addCause, self->addCauseValue);
+}
+
 static void
 executeControlTask(MmsMapping* self, ControlObject* controlObject, uint64_t currentTimeInMs)
 {
@@ -732,6 +738,8 @@ executeStateMachine:
                             convertCheckHandlerResultToServiceError(checkHandlerResult));
 #endif /* (CONFIG_IEC61850_SERVICE_TRACKING == 1) */
 
+                    resetAddCause(controlObject);
+
                     if (DEBUG_IED_SERVER)
                         printf("IED_SERVER: SBOw - select rejected by application!\n");
                 }
@@ -785,6 +793,8 @@ executeStateMachine:
 #endif /* (CONFIG_IEC61850_SERVICE_TRACKING == 1) */
             }
 
+            resetAddCause(controlObject);
+
             abortControlOperation(controlObject);
             exitControlTask(controlObject);
         }
@@ -854,6 +864,8 @@ executeStateMachine:
             exitControlTask(controlObject);
 
             setOpOk(controlObject, false, currentTimeInMs);
+
+            resetAddCause(controlObject);
         }
     }
     break;
@@ -1365,12 +1377,17 @@ Control_processControlActions(MmsMapping* self, uint64_t currentTimeInMs)
                             controlObject->errorValue, controlObject->addCauseValue,
                                 controlObject->ctlNum, controlObject->origin, false);
 
-                    //TODO add service tracking code (time activated control)
+#if (CONFIG_IEC61850_SERVICE_TRACKING == 1)
+                    updateGenericTrackingObjectValues(self, controlObject, IEC61850_SERVICE_TYPE_TIME_ACTIVATED_OPERATE,
+                            convertCheckHandlerResultToServiceError(checkResult));
+#endif /* (CONFIG_IEC61850_SERVICE_TRACKING == 1) */
 
                     /* leave state Perform Test */
                     setOpRcvd(controlObject, false);
 
                     abortControlOperation(controlObject);
+
+                    resetAddCause(controlObject);
                 }
             }
 
@@ -2249,13 +2266,18 @@ Control_writeAccessControlObject(MmsMapping* self, MmsDomain* domain, char* vari
 free_and_return:
 
 #if (CONFIG_IEC61850_SERVICE_TRACKING == 1)
-    if (serviceError == IEC61850_SERVICE_ERROR_NO_ERROR)
-        updateGenericTrackingObjectValues(self, controlObject, serviceType,
-                private_IedServer_convertMmsDataAccessErrorToServiceError(indication));
+    if (serviceError == IEC61850_SERVICE_ERROR_NO_ERROR) {
+        if (indication != DATA_ACCESS_ERROR_NO_RESPONSE) {
+            updateGenericTrackingObjectValues(self, controlObject, serviceType,
+                    private_IedServer_convertMmsDataAccessErrorToServiceError(indication));
+        }
+    }
     else
         updateGenericTrackingObjectValues(self, controlObject, serviceType, serviceError);
 #endif /* (CONFIG_IEC61850_SERVICE_TRACKING == 1) */
 
+    resetAddCause(controlObject);
+
     return indication;
 }