diff --git a/src/iec61850/server/mms_mapping/control.c b/src/iec61850/server/mms_mapping/control.c
index 60b6a023..85106de4 100644
--- a/src/iec61850/server/mms_mapping/control.c
+++ b/src/iec61850/server/mms_mapping/control.c
@@ -63,29 +63,216 @@ ControlObject_sendCommandTerminationPositive(ControlObject* self);
 void
 ControlObject_sendCommandTerminationNegative(ControlObject* self);
 
+static MmsValue*
+getOperParameterCtlNum(MmsValue* operParameters)
+{
+    if (MmsValue_getType(operParameters) == MMS_STRUCTURE) {
+        if (MmsValue_getArraySize(operParameters) == 7)
+            return MmsValue_getElement(operParameters, 3);
+        else if (MmsValue_getArraySize(operParameters) == 6)
+            return MmsValue_getElement(operParameters, 2);
+    }
+
+    return NULL;
+}
+
+static MmsValue*
+getCancelParameterCtlNum(MmsValue* operParameters)
+{
+    if (MmsValue_getType(operParameters) == MMS_STRUCTURE) {
+        if (MmsValue_getArraySize(operParameters) == 6)
+            return MmsValue_getElement(operParameters, 3);
+        else if (MmsValue_getArraySize(operParameters) == 5)
+            return MmsValue_getElement(operParameters, 2);
+    }
+
+    return NULL;
+}
+
+static MmsValue*
+getCancelParameterOrigin(MmsValue* operParameters)
+{
+    if (MmsValue_getType(operParameters) == MMS_STRUCTURE) {
+        if (MmsValue_getArraySize(operParameters) == 6)
+            return MmsValue_getElement(operParameters, 2);
+        else if (MmsValue_getArraySize(operParameters) == 5)
+            return MmsValue_getElement(operParameters, 1);
+    }
+
+    return NULL;
+}
+
+static MmsValue*
+getOperParameterTest(MmsValue* operParameters)
+{
+    if (MmsValue_getType(operParameters) == MMS_STRUCTURE)
+    {
+        if (MmsValue_getArraySize(operParameters) == 7)
+            return MmsValue_getElement(operParameters, 5);
+        else if (MmsValue_getArraySize(operParameters) == 6)
+            return MmsValue_getElement(operParameters, 4);
+    }
+
+    return NULL;
+}
+
+static MmsValue*
+getOperParameterCheck(MmsValue* operParameters)
+{
+    if (MmsValue_getType(operParameters) == MMS_STRUCTURE)
+    {
+        if (MmsValue_getArraySize(operParameters) == 7)
+            return MmsValue_getElement(operParameters, 6);
+        else if (MmsValue_getArraySize(operParameters) == 6)
+            return MmsValue_getElement(operParameters, 5);
+    }
+
+    return NULL;
+}
+
+static MmsValue*
+getOperParameterOrigin(MmsValue* operParameters)
+{
+    if (MmsValue_getType(operParameters) == MMS_STRUCTURE)
+    {
+        if (MmsValue_getArraySize(operParameters) == 7)
+            return MmsValue_getElement(operParameters, 2);
+        else if (MmsValue_getArraySize(operParameters) == 6)
+            return MmsValue_getElement(operParameters, 1);
+    }
+
+    return NULL;
+}
+
+static MmsValue*
+getCancelParameterTest(MmsValue* operParameters)
+{
+    if (MmsValue_getType(operParameters) == MMS_STRUCTURE)
+    {
+        if (MmsValue_getArraySize(operParameters) == 6)
+            return MmsValue_getElement(operParameters, 5);
+        else if (MmsValue_getArraySize(operParameters) == 5)
+            return MmsValue_getElement(operParameters, 4);
+    }
+
+    return NULL;
+}
+
+static MmsValue*
+getOperParameterTime(MmsValue* operParameters)
+{
+    MmsValue* timeParameter = NULL;
+
+    if (MmsValue_getType(operParameters) == MMS_STRUCTURE)
+    {
+        if (MmsValue_getArraySize(operParameters) == 7)
+            timeParameter = MmsValue_getElement(operParameters, 4);
+        else if (MmsValue_getArraySize(operParameters) == 6)
+            timeParameter = MmsValue_getElement(operParameters, 3);
+    }
+
+    if (timeParameter != NULL)
+        if ((MmsValue_getType(timeParameter) == MMS_UTC_TIME) || (MmsValue_getType(timeParameter) == MMS_BINARY_TIME))
+            return timeParameter;
+
+    return NULL;
+}
+
+static MmsValue*
+getCancelParameterTime(MmsValue* operParameters)
+{
+    MmsValue* timeParameter = NULL;
+
+    if (MmsValue_getType(operParameters) == MMS_STRUCTURE)
+    {
+        if (MmsValue_getArraySize(operParameters) == 6)
+            timeParameter = MmsValue_getElement(operParameters, 4);
+        else if (MmsValue_getArraySize(operParameters) == 5)
+            timeParameter = MmsValue_getElement(operParameters, 3);
+    }
+
+    if (timeParameter != NULL)
+        if ((MmsValue_getType(timeParameter) == MMS_UTC_TIME) || (MmsValue_getType(timeParameter) == MMS_BINARY_TIME))
+            return timeParameter;
+
+    return NULL;
+}
+
 #if (CONFIG_IEC61850_SERVICE_TRACKING == 1)
 
 static void
-copyControlValuesToTrackingObject(MmsMapping* self, ControlObject* controlObject)
+copyControlValuesToTrackingObject(MmsMapping* self, ControlObject* controlObject, IEC61850_ServiceType serviceType)
 {
     //TODO determine type!
+
     if (controlObject->ctlVal) {
+
+        ControlTrkInstance trkInst = NULL;
+
         if (MmsValue_getType(controlObject->ctlVal) == MMS_BOOLEAN) {
+
+            //TODO handle binary controlled step position
+            //TODO handle binary controlled integer/analog
+
             if (self->spcTrk) {
-                if (self->spcTrk->ctlVal)
-                    MmsValue_update(self->spcTrk->ctlVal->mmsValue, controlObject->ctlVal);
 
-                if (self->spcTrk->origin)
-                    MmsValue_update(self->spcTrk->origin->mmsValue, controlObject->origin);
+                trkInst = self->spcTrk;
 
-                if (self->spcTrk->ctlNum)
-                    MmsValue_update(self->spcTrk->ctlNum->mmsValue, controlObject->ctlNum);
+                if (trkInst->ctlVal)
+                    MmsValue_update(trkInst->ctlVal->mmsValue, controlObject->ctlVal);
+            }
 
-                if (self->spcTrk->operTm)
-                    MmsValue_setUtcTimeMs(self->spcTrk->operTm->mmsValue, controlObject->operateTime);
+        }
+
+        if (trkInst) {
+            if (trkInst->origin)
+                MmsValue_update(trkInst->origin->mmsValue, controlObject->origin);
+
+            if (trkInst->ctlNum)
+                MmsValue_update(trkInst->ctlNum->mmsValue, controlObject->ctlNum);
+
+            if (trkInst->operTm)
+                MmsValue_setUtcTimeMs(trkInst->operTm->mmsValue, controlObject->operateTime);
 
-                // TODO implement remaining fields
+            if (trkInst->respAddCause)
+                MmsValue_update(trkInst->respAddCause->mmsValue, controlObject->addCause);
+
+            MmsValue* operVal = NULL;
+
+            if (serviceType == IEC61850_SERVICE_TYPE_SELECT_WITH_VALUES) {
+                if (controlObject->sbow)
+                    operVal = controlObject->sbow;
             }
+            else if (serviceType == IEC61850_SERVICE_TYPE_OPERATE) {
+                if (controlObject->oper)
+                    operVal = controlObject->oper;
+            }
+            else if (serviceType == IEC61850_SERVICE_TYPE_CANCEL) {
+                if (controlObject->cancel) {
+                    if (trkInst->Test) {
+                        MmsValue_update(trkInst->Test->mmsValue, getCancelParameterTest(operVal));
+                    }
+
+                    if (trkInst->T) {
+                        MmsValue_update(trkInst->T->mmsValue, getCancelParameterTime(operVal));
+                    }
+                }
+            }
+
+            if (operVal) {
+                if (trkInst->Test) {
+                    MmsValue_update(trkInst->Test->mmsValue, getOperParameterTest(operVal));
+                }
+
+                if (trkInst->Check) {
+                    MmsValue_update(trkInst->Check->mmsValue, getOperParameterCheck(operVal));
+                }
+
+                if (trkInst->T) {
+                    MmsValue_update(trkInst->T->mmsValue, getOperParameterTime(operVal));
+                }
+            }
+
         }
     }
 }
@@ -165,7 +352,7 @@ convertCheckHandlerResultToServiceError(CheckHandlerResult controlHandlerResult)
 static void
 updateGenericTrackingObjectValues(MmsMapping* self, ControlObject* controlObject, IEC61850_ServiceType serviceType, IEC61850_ServiceError errVal)
 {
-    copyControlValuesToTrackingObject(self, controlObject);
+    copyControlValuesToTrackingObject(self, controlObject, serviceType);
 
     ServiceTrkInstance trkInst = NULL;
 
@@ -1129,107 +1316,6 @@ getCtlVal(MmsValue* operParameters)
     return NULL;
 }
 
-static MmsValue*
-getOperParameterCtlNum(MmsValue* operParameters)
-{
-    if (MmsValue_getType(operParameters) == MMS_STRUCTURE) {
-        if (MmsValue_getArraySize(operParameters) == 7)
-            return MmsValue_getElement(operParameters, 3);
-        else if (MmsValue_getArraySize(operParameters) == 6)
-            return MmsValue_getElement(operParameters, 2);
-    }
-
-    return NULL;
-}
-
-static MmsValue*
-getCancelParameterCtlNum(MmsValue* operParameters)
-{
-    if (MmsValue_getType(operParameters) == MMS_STRUCTURE) {
-        if (MmsValue_getArraySize(operParameters) == 6)
-            return MmsValue_getElement(operParameters, 3);
-        else if (MmsValue_getArraySize(operParameters) == 5)
-            return MmsValue_getElement(operParameters, 2);
-    }
-
-    return NULL;
-}
-
-static MmsValue*
-getCancelParameterOrigin(MmsValue* operParameters)
-{
-    if (MmsValue_getType(operParameters) == MMS_STRUCTURE) {
-        if (MmsValue_getArraySize(operParameters) == 6)
-            return MmsValue_getElement(operParameters, 2);
-        else if (MmsValue_getArraySize(operParameters) == 5)
-            return MmsValue_getElement(operParameters, 1);
-    }
-
-    return NULL;
-}
-
-static MmsValue*
-getOperParameterTest(MmsValue* operParameters)
-{
-    if (MmsValue_getType(operParameters) == MMS_STRUCTURE)
-    {
-        if (MmsValue_getArraySize(operParameters) == 7)
-            return MmsValue_getElement(operParameters, 5);
-        else if (MmsValue_getArraySize(operParameters) == 6)
-            return MmsValue_getElement(operParameters, 4);
-    }
-
-    return NULL;
-}
-
-static MmsValue*
-getOperParameterCheck(MmsValue* operParameters)
-{
-    if (MmsValue_getType(operParameters) == MMS_STRUCTURE)
-    {
-        if (MmsValue_getArraySize(operParameters) == 7)
-            return MmsValue_getElement(operParameters, 6);
-        else if (MmsValue_getArraySize(operParameters) == 6)
-            return MmsValue_getElement(operParameters, 5);
-    }
-
-    return NULL;
-}
-
-static MmsValue*
-getOperParameterOrigin(MmsValue* operParameters)
-{
-    if (MmsValue_getType(operParameters) == MMS_STRUCTURE)
-    {
-        if (MmsValue_getArraySize(operParameters) == 7)
-            return MmsValue_getElement(operParameters, 2);
-        else if (MmsValue_getArraySize(operParameters) == 6)
-            return MmsValue_getElement(operParameters, 1);
-    }
-
-    return NULL;
-}
-
-static MmsValue*
-getOperParameterTime(MmsValue* operParameters)
-{
-    MmsValue* timeParameter = NULL;
-
-    if (MmsValue_getType(operParameters) == MMS_STRUCTURE)
-    {
-        if (MmsValue_getArraySize(operParameters) == 7)
-            timeParameter = MmsValue_getElement(operParameters, 4);
-        else if (MmsValue_getArraySize(operParameters) == 6)
-            timeParameter = MmsValue_getElement(operParameters, 3);
-    }
-
-    if (timeParameter != NULL)
-        if ((MmsValue_getType(timeParameter) == MMS_UTC_TIME) || (MmsValue_getType(timeParameter) == MMS_BINARY_TIME))
-            return timeParameter;
-
-    return NULL;
-}
-
 void
 ControlObject_sendCommandTerminationPositive(ControlObject* self)
 {
diff --git a/src/mms/iso_mms/common/mms_value.c b/src/mms/iso_mms/common/mms_value.c
index bf38ae7b..150798e4 100644
--- a/src/mms/iso_mms/common/mms_value.c
+++ b/src/mms/iso_mms/common/mms_value.c
@@ -225,84 +225,88 @@ MmsValue_equalTypes(const MmsValue* self, const MmsValue* otherValue)
 bool
 MmsValue_update(MmsValue* self, const MmsValue* update)
 {
-    if (self->type == update->type) {
-        switch (self->type)
-        {
-        case MMS_STRUCTURE:
-            case MMS_ARRAY:
-            if (updateStructuredComponent(self, update) == false)
-                return false;
-            break;
-        case MMS_BOOLEAN:
-            self->value.boolean = update->value.boolean;
-            break;
-        case MMS_FLOAT:
-            if (self->value.floatingPoint.formatWidth == update->value.floatingPoint.formatWidth) {
-                self->value.floatingPoint.exponentWidth = update->value.floatingPoint.exponentWidth;
-                memcpy(self->value.floatingPoint.buf, update->value.floatingPoint.buf,
-                        self->value.floatingPoint.formatWidth / 8);
-            }
-            else
-                return false;
-            break;
-        case MMS_INTEGER:
-        case MMS_UNSIGNED:
-            if (BerInteger_setFromBerInteger(self->value.integer, update->value.integer))
-                return true;
-            else
-                return false;
-            break;
-        case MMS_UTC_TIME:
-            memcpy(self->value.utcTime, update->value.utcTime, 8);
-            break;
-        case MMS_BIT_STRING:
-            if (self->value.bitString.size == update->value.bitString.size)
-                memcpy(self->value.bitString.buf, update->value.bitString.buf, bitStringByteSize(self));
-            else if (update->value.bitString.size < self->value.bitString.size) {
-                int i;
-
-                for (i = 0; i < update->value.bitString.size; i++) {
-                    MmsValue_setBitStringBit(self, i, MmsValue_getBitStringBit(update, i));
-                }
-            }
-            else
-                return false;
-            break;
-        case MMS_OCTET_STRING:
+    if (self && update) {
+        if (self->type == update->type) {
+            switch (self->type)
             {
-                int size = update->value.octetString.size;
+            case MMS_STRUCTURE:
+                case MMS_ARRAY:
+                if (updateStructuredComponent(self, update) == false)
+                    return false;
+                break;
+            case MMS_BOOLEAN:
+                self->value.boolean = update->value.boolean;
+                break;
+            case MMS_FLOAT:
+                if (self->value.floatingPoint.formatWidth == update->value.floatingPoint.formatWidth) {
+                    self->value.floatingPoint.exponentWidth = update->value.floatingPoint.exponentWidth;
+                    memcpy(self->value.floatingPoint.buf, update->value.floatingPoint.buf,
+                            self->value.floatingPoint.formatWidth / 8);
+                }
+                else
+                    return false;
+                break;
+            case MMS_INTEGER:
+            case MMS_UNSIGNED:
+                if (BerInteger_setFromBerInteger(self->value.integer, update->value.integer))
+                    return true;
+                else
+                    return false;
+                break;
+            case MMS_UTC_TIME:
+                memcpy(self->value.utcTime, update->value.utcTime, 8);
+                break;
+            case MMS_BIT_STRING:
+                if (self->value.bitString.size == update->value.bitString.size)
+                    memcpy(self->value.bitString.buf, update->value.bitString.buf, bitStringByteSize(self));
+                else if (update->value.bitString.size < self->value.bitString.size) {
+                    int i;
+
+                    for (i = 0; i < update->value.bitString.size; i++) {
+                        MmsValue_setBitStringBit(self, i, MmsValue_getBitStringBit(update, i));
+                    }
+                }
+                else
+                    return false;
+                break;
+            case MMS_OCTET_STRING:
+                {
+                    int size = update->value.octetString.size;
 
-                if (size > self->value.octetString.maxSize) {
-                    GLOBAL_FREEMEM(self->value.octetString.buf);
-                    self->value.octetString.buf = (uint8_t*) GLOBAL_MALLOC(size);
+                    if (size > self->value.octetString.maxSize) {
+                        GLOBAL_FREEMEM(self->value.octetString.buf);
+                        self->value.octetString.buf = (uint8_t*) GLOBAL_MALLOC(size);
 
-                    if (self->value.octetString.buf == NULL)
-                        return false;
+                        if (self->value.octetString.buf == NULL)
+                            return false;
 
-                    self->value.octetString.maxSize = size;
-                }
+                        self->value.octetString.maxSize = size;
+                    }
 
-                memcpy(self->value.octetString.buf, update->value.octetString.buf, size);
+                    memcpy(self->value.octetString.buf, update->value.octetString.buf, size);
 
-                self->value.octetString.size = size;
+                    self->value.octetString.size = size;
+                }
+                break;
+            case MMS_VISIBLE_STRING:
+                MmsValue_setVisibleString(self, update->value.visibleString.buf);
+                break;
+            case MMS_STRING:
+                MmsValue_setMmsString(self, update->value.visibleString.buf);
+                break;
+            case MMS_BINARY_TIME:
+                self->value.binaryTime.size = update->value.binaryTime.size;
+                memcpy(self->value.binaryTime.buf, update->value.binaryTime.buf,
+                        update->value.binaryTime.size);
+                break;
+            default:
+                return false;
+                break;
             }
-            break;
-        case MMS_VISIBLE_STRING:
-            MmsValue_setVisibleString(self, update->value.visibleString.buf);
-            break;
-        case MMS_STRING:
-            MmsValue_setMmsString(self, update->value.visibleString.buf);
-            break;
-        case MMS_BINARY_TIME:
-            self->value.binaryTime.size = update->value.binaryTime.size;
-            memcpy(self->value.binaryTime.buf, update->value.binaryTime.buf,
-                    update->value.binaryTime.size);
-            break;
-        default:
-            return false;
-            break;
+            return true;
         }
-        return true;
+        else
+            return false;
     }
     else
         return false;