From f0b542e9cec4e0c035554c75cc011912a34c78ad Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Mon, 21 Dec 2020 19:52:00 +0100
Subject: [PATCH] rm3100 continued

---
 fsfw                                          |  2 +-
 mission/devices/MGMHandlerLIS3MDL.cpp         |  2 +-
 mission/devices/MGMHandlerRM3100.cpp          | 60 +++++++++++++++++--
 mission/devices/MGMHandlerRM3100.h            |  6 ++
 .../MGMHandlerRM3100Definitions.h             |  7 +++
 tmtc                                          |  2 +-
 6 files changed, 70 insertions(+), 9 deletions(-)

diff --git a/fsfw b/fsfw
index 086cbe1e..31b82975 160000
--- a/fsfw
+++ b/fsfw
@@ -1 +1 @@
-Subproject commit 086cbe1e3950cf6c904609352408a7fa48cc83ef
+Subproject commit 31b82975c7ef701fa7f1ba3413cfa19cc73aa2ca
diff --git a/mission/devices/MGMHandlerLIS3MDL.cpp b/mission/devices/MGMHandlerLIS3MDL.cpp
index 1cc83bf8..160f43ce 100644
--- a/mission/devices/MGMHandlerLIS3MDL.cpp
+++ b/mission/devices/MGMHandlerLIS3MDL.cpp
@@ -283,7 +283,7 @@ ReturnValue_t MGMHandlerLIS3MDL::interpretDeviceReply(DeviceCommandId_t id,
 	}
 
 	default: {
-		return DeviceHandlerIF::UNKNOW_DEVICE_REPLY;
+		return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY;
 	}
 
 	}
diff --git a/mission/devices/MGMHandlerRM3100.cpp b/mission/devices/MGMHandlerRM3100.cpp
index 7c546ffa..3e6fffd3 100644
--- a/mission/devices/MGMHandlerRM3100.cpp
+++ b/mission/devices/MGMHandlerRM3100.cpp
@@ -11,11 +11,6 @@ MGMHandlerRM3100::MGMHandlerRM3100(object_id_t objectId,
 
 MGMHandlerRM3100::~MGMHandlerRM3100() {}
 
-ReturnValue_t MGMHandlerRM3100::buildTransitionDeviceCommand(
-		DeviceCommandId_t *id) {
-	return RETURN_OK;
-}
-
 void MGMHandlerRM3100::doStartUp() {
 	if(internalState == STATE_NONE) {
 		internalState = STATE_CONFIGURE_CMM;
@@ -40,6 +35,27 @@ void MGMHandlerRM3100::doStartUp() {
 void MGMHandlerRM3100::doShutDown() {
 }
 
+ReturnValue_t MGMHandlerRM3100::buildTransitionDeviceCommand(
+		DeviceCommandId_t *id) {
+	if(internalState == STATE_CONFIGURE_CMM) {
+		*id = RM3100::CONFIGURE_CMM;
+		commandBuffer[0] = RM3100::CMM_REGISTER;
+		commandBuffer[1] = RM3100::CMM_VALUE;
+		this->rawPacket = commandBuffer;
+		this->rawPacketLen = 2;
+	}
+
+	if(internalState == STATE_READ_CMM) {
+		*id = RM3100::READ_CMM;
+		commandBuffer[0] = RM3100::CMM_REGISTER | RM3100::READ_MASK;
+		commandBuffer[1] = 0;
+		this->rawPacket = commandBuffer;
+		this->rawPacketLen = 2;
+	}
+	return RETURN_OK;
+}
+
+
 ReturnValue_t MGMHandlerRM3100::buildNormalDeviceCommand(
 		DeviceCommandId_t *id) {
 	return RETURN_OK;
@@ -54,10 +70,42 @@ ReturnValue_t MGMHandlerRM3100::buildCommandFromCommand(
 ReturnValue_t MGMHandlerRM3100::scanForReply(const uint8_t *start,
         size_t len, DeviceCommandId_t *foundId,
 		size_t *foundLen) {
-	return RETURN_OK;
+
+	*foundId = this->getPendingCommand();
+	*foundLen = this->rawPacketLen;
+
+	// Data with SPI Interface has always this answer
+	if (start[0] == 0b11111111) {
+		return RETURN_OK;
+	}
+	else {
+		return DeviceHandlerIF::INVALID_DATA;
+	}
 }
 
 ReturnValue_t MGMHandlerRM3100::interpretDeviceReply(
 		DeviceCommandId_t id, const uint8_t *packet) {
+	if(id == RM3100::CONFIGURE_CMM or id == RM3100::CONFIGURE_TMRC) {
+		commandExecuted = true;
+	}
+	else if(id == RM3100::READ_CMM) {
+		if(packet[1] == cmmRegValue) {
+			commandExecuted = true;
+		}
+	}
+	else if(id == RM3100::READ_TMRC) {
+		if(packet[1] == tmrcRegValue) {
+			commandExecuted = true;
+		}
+		else {
+			// Attempt reconfiguration.
+			internalState = STATE_CONFIGURE_TMRC;
+			return HasReturnvaluesIF::RETURN_FAILED;
+		}
+	}
+	else {
+		return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY;
+	}
+
 	return RETURN_OK;
 }
diff --git a/mission/devices/MGMHandlerRM3100.h b/mission/devices/MGMHandlerRM3100.h
index cf459056..ed948560 100644
--- a/mission/devices/MGMHandlerRM3100.h
+++ b/mission/devices/MGMHandlerRM3100.h
@@ -44,6 +44,12 @@ private:
 	};
 	InternalState internalState = InternalState::STATE_NONE;
 	bool commandExecuted = false;
+
+	uint8_t commandBuffer[9];
+	uint8_t commandBufferLen = 0;
+
+	uint8_t cmmRegValue = RM3100::CMM_VALUE;
+	uint8_t tmrcRegValue = RM3100::TMRC_DEFAULT_VALUE;
 };
 
 #endif /* MISSION_DEVICEHANDLING_MGMRM3100HANDLER_H_ */
diff --git a/mission/devices/devicedefinitions/MGMHandlerRM3100Definitions.h b/mission/devices/devicedefinitions/MGMHandlerRM3100Definitions.h
index dd519697..4f38a217 100644
--- a/mission/devices/devicedefinitions/MGMHandlerRM3100Definitions.h
+++ b/mission/devices/devicedefinitions/MGMHandlerRM3100Definitions.h
@@ -1,6 +1,7 @@
 #ifndef MISSION_DEVICES_DEVICEDEFINITIONS_MGMHANDLERRM3100DEFINITIONS_H_
 #define MISSION_DEVICES_DEVICEDEFINITIONS_MGMHANDLERRM3100DEFINITIONS_H_
 
+#include <fsfw/devicehandlers/DeviceHandlerIF.h>
 #include <cstdint>
 
 namespace RM3100 {
@@ -47,7 +48,13 @@ static constexpr uint8_t REVID_REGISTER = 0x36;
 // Range in Microtesla. 1 T equals 10000 Gauss (for comparison with LIS3 MGM)
 static constexpr uint16_t RANGE = 800;
 
+static constexpr DeviceCommandId_t CONFIGURE_CMM = 3;
+static constexpr DeviceCommandId_t READ_CMM = 4;
 
+//static constexpr DeviceCommandId_t CONFIGURE_CYC
+
+static constexpr DeviceCommandId_t CONFIGURE_TMRC = 5;
+static constexpr DeviceCommandId_t READ_TMRC = 6;
 
 }
 
diff --git a/tmtc b/tmtc
index 07b6a9df..e0c896e6 160000
--- a/tmtc
+++ b/tmtc
@@ -1 +1 @@
-Subproject commit 07b6a9df18baff999ca52c2f2781f8f77f8dcb65
+Subproject commit e0c896e62d25286d00599ca57b71d0a3495ed95f