From ddb76ff61b4d528ed7f642b7e9ce6d7cc90d16d8 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Wed, 21 Oct 2020 20:13:27 +0200
Subject: [PATCH] lis3 bugfixes

---
 config/OBSWConfig.h                   |  2 +-
 mission/devices/MGMHandlerLIS3MDL.cpp | 22 ++++++++++------------
 2 files changed, 11 insertions(+), 13 deletions(-)

diff --git a/config/OBSWConfig.h b/config/OBSWConfig.h
index 2b8b62c4..7a569b87 100644
--- a/config/OBSWConfig.h
+++ b/config/OBSWConfig.h
@@ -6,7 +6,7 @@
 #ifndef CONFIG_OBSWCONFIG_H_
 #define CONFIG_OBSWCONFIG_H_
 
-#define ADD_TEST_CODE   	1
+#define OBSW_ADD_TEST_CODE   	1
 
 // Define not used yet, PUS stack and TMTC tasks are always started
 #define ADD_PUS_STACK 		1
diff --git a/mission/devices/MGMHandlerLIS3MDL.cpp b/mission/devices/MGMHandlerLIS3MDL.cpp
index 6761b127..de27ddbe 100644
--- a/mission/devices/MGMHandlerLIS3MDL.cpp
+++ b/mission/devices/MGMHandlerLIS3MDL.cpp
@@ -112,17 +112,15 @@ ReturnValue_t MGMHandlerLIS3MDL::buildNormalDeviceCommand(
 ReturnValue_t MGMHandlerLIS3MDL::buildCommandFromCommand(
         DeviceCommandId_t deviceCommand, const uint8_t *commandData,
 		size_t commandDataLen) {
-	lastSentCommand = deviceCommand;
-	switch(deviceCommand) {
-	case(MGMLIS3MDL::READALL_MGM): {
-	    if (commandDataLen == 0) {
-	        std::memset(commandBuffer, 0, sizeof(commandBuffer));
-	        commandBuffer[0] = readCommand(0, true);
+    lastSentCommand = deviceCommand;
+    switch(deviceCommand) {
+    case(MGMLIS3MDL::READALL_MGM): {
+        std::memset(commandBuffer, 0, sizeof(commandBuffer));
+        commandBuffer[0] = readCommand(0, true);
 
-	        rawPacket = commandBuffer;
-	        rawPacketLen = sizeof(commandBuffer);
-	        return RETURN_OK;
-	    }
+        rawPacket = commandBuffer;
+        rawPacketLen = sizeof(commandBuffer);
+        return RETURN_OK;
 	case(MGMLIS3MDL::IDENTIFY_DEVICE): {
 	    return identifyDevice();
 	}
@@ -346,8 +344,8 @@ ReturnValue_t MGMHandlerLIS3MDL::prepareRegisterWrite() {
 
 	commandBuffer[0] = writeCommand(MGMLIS3MDL::CTRL_REG1, true);
 
-	for (size_t i = 1; i <= MGMLIS3MDL::NR_OF_CTRL_REGISTERS; i++) {
-		commandBuffer[i] = registers[i];
+	for (size_t i = 0; i < MGMLIS3MDL::NR_OF_CTRL_REGISTERS; i++) {
+		commandBuffer[i + 1] = registers[i];
 	}
 	rawPacket = commandBuffer;
 	rawPacketLen = MGMLIS3MDL::NR_OF_CTRL_REGISTERS;