From 29c74283f117bb68186d1161896d82e3361680c2 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Wed, 22 Sep 2021 18:36:53 +0200
Subject: [PATCH] sanity checks

---
 .../devicehandlers/GyroL3GD20Handler.cpp      | 29 ++++++++++++++++---
 .../devicehandlers/GyroL3GD20Handler.h        |  9 +++++-
 .../devicehandlers/MgmLIS3MDLHandler.cpp      | 27 ++++++++++++++---
 .../devicehandlers/MgmLIS3MDLHandler.h        |  9 +++++-
 4 files changed, 64 insertions(+), 10 deletions(-)

diff --git a/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp b/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp
index 5b58bb468..06b4548b8 100644
--- a/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp
+++ b/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp
@@ -215,11 +215,32 @@ ReturnValue_t GyroHandlerL3GD20H::interpretDeviceReply(DeviceCommandId_t id,
 
         PoolReadGuard readSet(&dataset);
         if(readSet.getReadResult() == HasReturnvaluesIF::RETURN_OK) {
-            dataset.angVelocX = angVelocX;
-            dataset.angVelocY = angVelocY;
-            dataset.angVelocZ = angVelocZ;
+            if(std::abs(angVelocX) > 100) {
+                dataset.angVelocX = angVelocX;
+                dataset.angVelocX.setValid(true);
+            }
+            else {
+                dataset.angVelocX.setValid(false);
+            }
+
+            if(std::abs(angVelocY) > 100) {
+                dataset.angVelocY = angVelocY;
+                dataset.angVelocY.setValid(true);
+            }
+            else {
+                dataset.angVelocY.setValid(false);
+            }
+
+            if(std::abs(angVelocZ) > 100) {
+                dataset.angVelocZ = angVelocZ;
+                dataset.angVelocZ.setValid(true);
+            }
+            else {
+                dataset.angVelocZ.setValid(false);
+            }
+
             dataset.temperature = temperature;
-            dataset.setValidity(true, true);
+            dataset.temperature.setValid(true);
         }
         break;
     }
diff --git a/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.h b/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.h
index 07d9835f9..de2884e8c 100644
--- a/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.h
+++ b/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.h
@@ -40,7 +40,14 @@ protected:
             size_t commandDataLen) override;
     ReturnValue_t scanForReply(const uint8_t *start, size_t len,
             DeviceCommandId_t *foundId, size_t *foundLen) override;
-    ReturnValue_t interpretDeviceReply(DeviceCommandId_t id,
+    /**
+     * This implementation is tailored towards space systems and flags angular velocities
+     * larger than 100 as invalid
+     * @param id
+     * @param packet
+     * @return
+     */
+    virtual ReturnValue_t interpretDeviceReply(DeviceCommandId_t id,
             const uint8_t *packet) override;
 
     void fillCommandAndReplyMap() override;
diff --git a/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp b/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp
index 63dabfc9c..6bc8a19ec 100644
--- a/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp
+++ b/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp
@@ -308,10 +308,29 @@ ReturnValue_t MgmLIS3MDLHandler::interpretDeviceReply(DeviceCommandId_t id,
 #endif /* OBSW_VERBOSE_LEVEL >= 1 */
         PoolReadGuard readHelper(&dataset);
         if(readHelper.getReadResult() == HasReturnvaluesIF::RETURN_OK) {
-            dataset.fieldStrengthX = mgmX;
-            dataset.fieldStrengthY = mgmY;
-            dataset.fieldStrengthZ = mgmZ;
-            dataset.setValidity(true, true);
+            if(std::abs(mgmX) > 100.0) {
+                dataset.fieldStrengthX = mgmX;
+                dataset.fieldStrengthX.setValid(true);
+            }
+            else {
+                dataset.fieldStrengthX.setValid(false);
+            }
+
+            if(std::abs(mgmY) > 100.0) {
+                dataset.fieldStrengthY = mgmY;
+                dataset.fieldStrengthY.setValid(true);
+            }
+            else {
+                dataset.fieldStrengthY.setValid(false);
+            }
+
+            if(std::abs(mgmZ) > 150.0) {
+                dataset.fieldStrengthZ = mgmZ;
+                dataset.fieldStrengthZ.setValid(true);
+            }
+            else {
+                dataset.fieldStrengthZ.setValid(false);
+            }
         }
         break;
     }
diff --git a/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h b/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h
index 9fb411561..b0f249a99 100644
--- a/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h
+++ b/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h
@@ -52,7 +52,14 @@ protected:
             DeviceCommandId_t *id) override;
     ReturnValue_t scanForReply(const uint8_t *start, size_t len,
             DeviceCommandId_t *foundId, size_t *foundLen) override;
-    ReturnValue_t interpretDeviceReply(DeviceCommandId_t id,
+    /**
+     * This implementation is tailored towards space applications and will flag values larger
+     * than 100 microtesla on X,Y and 150 microtesla on Z as invalid
+     * @param id
+     * @param packet
+     * @return
+     */
+    virtual ReturnValue_t interpretDeviceReply(DeviceCommandId_t id,
             const uint8_t *packet) override;
     void fillCommandAndReplyMap() override;
     void modeChanged(void) override;