From df992638cca8b75a2db693a34cc1e9ce1637a129 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Mon, 14 Dec 2020 22:50:15 +0100
Subject: [PATCH] stuff stored in local poo lnow

---
 mission/devices/MGMHandlerLIS3MDL.cpp           | 17 +++++++++++++++--
 mission/devices/MGMHandlerLIS3MDL.h             |  2 +-
 .../MGMHandlerLIS3Definitions.h                 |  8 ++++----
 3 files changed, 20 insertions(+), 7 deletions(-)

diff --git a/mission/devices/MGMHandlerLIS3MDL.cpp b/mission/devices/MGMHandlerLIS3MDL.cpp
index 3fa9e9ec..1cc83bf8 100644
--- a/mission/devices/MGMHandlerLIS3MDL.cpp
+++ b/mission/devices/MGMHandlerLIS3MDL.cpp
@@ -3,7 +3,8 @@
 
 MGMHandlerLIS3MDL::MGMHandlerLIS3MDL(object_id_t objectId,
 		object_id_t deviceCommunication, CookieIF* comCookie):
-		DeviceHandlerBase(objectId, deviceCommunication, comCookie) {
+		DeviceHandlerBase(objectId, deviceCommunication, comCookie),
+		dataset(this) {
 #if OBSW_ENHANCED_PRINTOUT == 1
 	debugDivider = new PeriodicOperationDivider(10);
 #endif
@@ -253,6 +254,13 @@ ReturnValue_t MGMHandlerLIS3MDL::interpretDeviceReply(DeviceCommandId_t id,
 			sif::info << "Z: " << mgmZ << " \xC2\xB5T" << std::endl;
 		}
 #endif
+		ReturnValue_t result = dataset.read(20);
+		if(result == HasReturnvaluesIF::RETURN_OK) {
+		    dataset.fieldStrengthX = mgmX;
+		    dataset.fieldStrengthY = mgmY;
+		    dataset.fieldStrengthZ = mgmZ;
+		    dataset.commit(20);
+		}
 		break;
 	}
 
@@ -266,6 +274,11 @@ ReturnValue_t MGMHandlerLIS3MDL::interpretDeviceReply(DeviceCommandId_t id,
 					<< std::endl;
 		}
 #endif
+        ReturnValue_t result = dataset.read(20);
+        if(result == HasReturnvaluesIF::RETURN_OK) {
+            dataset.temperature = tempValue;
+            dataset.commit(20);
+        }
 		break;
 	}
 
@@ -371,7 +384,7 @@ void MGMHandlerLIS3MDL::fillCommandAndReplyMap() {
 	 * We dont read single registers, we just expect special
 	 * reply from he Readall_MGM
 	 */
-	insertInCommandAndReplyMap(MGMLIS3MDL::READ_CONFIG_AND_DATA, 1);
+	insertInCommandAndReplyMap(MGMLIS3MDL::READ_CONFIG_AND_DATA, 1, &dataset);
 	insertInCommandAndReplyMap(MGMLIS3MDL::READ_TEMPERATURE, 1);
 	insertInCommandAndReplyMap(MGMLIS3MDL::SETUP_MGM, 1);
 	insertInCommandAndReplyMap(MGMLIS3MDL::IDENTIFY_DEVICE, 1);
diff --git a/mission/devices/MGMHandlerLIS3MDL.h b/mission/devices/MGMHandlerLIS3MDL.h
index ff95268c..b400daee 100644
--- a/mission/devices/MGMHandlerLIS3MDL.h
+++ b/mission/devices/MGMHandlerLIS3MDL.h
@@ -18,7 +18,6 @@
  */
 class MGMHandlerLIS3MDL: public DeviceHandlerBase {
 public:
-
 	enum class CommunicationStep {
 		DATA,
 		TEMPERATURE
@@ -59,6 +58,7 @@ protected:
 
 
 private:
+	MGMLIS3MDL::MgmPrimaryDataset dataset;
 
 	/*------------------------------------------------------------------------*/
 	/* Device specific commands and variables */
diff --git a/mission/devices/devicedefinitions/MGMHandlerLIS3Definitions.h b/mission/devices/devicedefinitions/MGMHandlerLIS3Definitions.h
index 2518af15..581e46e3 100644
--- a/mission/devices/devicedefinitions/MGMHandlerLIS3Definitions.h
+++ b/mission/devices/devicedefinitions/MGMHandlerLIS3Definitions.h
@@ -130,7 +130,7 @@ static const uint8_t BDU = 6;       //Block data update
 static const uint8_t FAST_READ = 7; //Fast read enabled = 1
 static const uint8_t CTRL_REG5_DEFAULT = 0;
 
-static const uint32_t MGM_DATA_SET_ID = 0;
+static const uint32_t MGM_DATA_SET_ID = READ_CONFIG_AND_DATA;
 
 enum MgmPoolIds: lp_id_t {
     FIELD_STRENGTH_X,
@@ -147,11 +147,11 @@ public:
     MgmPrimaryDataset(object_id_t mgmId):
         StaticLocalDataSet(sid_t(mgmId, MGM_DATA_SET_ID)) {}
 
-    lp_var_t<float> angVelocityX = lp_var_t<float>(sid.objectId,
+    lp_var_t<float> fieldStrengthX = lp_var_t<float>(sid.objectId,
             FIELD_STRENGTH_X, this);
-    lp_var_t<float> angVelocityY = lp_var_t<float>(sid.objectId,
+    lp_var_t<float> fieldStrengthY = lp_var_t<float>(sid.objectId,
             FIELD_STRENGTH_Y, this);
-    lp_var_t<float> angVelocityZ = lp_var_t<float>(sid.objectId,
+    lp_var_t<float> fieldStrengthZ = lp_var_t<float>(sid.objectId,
             FIELD_STRENGTH_Z, this);
     lp_var_t<float> temperature = lp_var_t<float>(sid.objectId,
             TEMPERATURE_CELCIUS, this);