From 43fe7e0aa072747e876044b9f88b50993523cf20 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Tue, 14 Sep 2021 13:55:56 +0200
Subject: [PATCH] using enum for sensitivity now

---
 mission/devices/MGMHandlerLIS3MDL.cpp         | 46 ++++++++++++-------
 mission/devices/MGMHandlerLIS3MDL.h           | 12 ++---
 .../MGMHandlerLIS3Definitions.h               | 16 +++++--
 3 files changed, 48 insertions(+), 26 deletions(-)

diff --git a/mission/devices/MGMHandlerLIS3MDL.cpp b/mission/devices/MGMHandlerLIS3MDL.cpp
index 54cf55c9..0291d8b0 100644
--- a/mission/devices/MGMHandlerLIS3MDL.cpp
+++ b/mission/devices/MGMHandlerLIS3MDL.cpp
@@ -269,9 +269,7 @@ ReturnValue_t MGMHandlerLIS3MDL::interpretDeviceReply(DeviceCommandId_t id,
     }
     case MGMLIS3MDL::READ_CONFIG_AND_DATA: {
         // TODO: Store configuration in new local datasets.
-
-        uint8_t scale = getFullScale(registers[2]);
-        float sensitivityFactor = getSensitivityFactor(scale);
+        float sensitivityFactor = getSensitivityFactor(getSensitivity(registers[2]));
 
         int16_t mgmMeasurementRawX = packet[MGMLIS3MDL::X_HIGHBYTE_IDX] << 8
                 | packet[MGMLIS3MDL::X_LOWBYTE_IDX] ;
@@ -345,23 +343,39 @@ ReturnValue_t MGMHandlerLIS3MDL::interpretDeviceReply(DeviceCommandId_t id,
     return RETURN_OK;
 }
 
-uint8_t MGMHandlerLIS3MDL::getFullScale(uint8_t ctrlRegister2) {
- uint8_t getFullScale(uint8_t ctrlRegister2) {
-    bool FS0_set = ctrlRegister2 & (1 << MGMLIS3MDL::FSO);  // Checks if FS0 bit is set
-    bool FS1_set = ctrlRegister2 & (1 << MGMLIS3MDL::FS1);  // Checks if FS1 bit is set
+MGMLIS3MDL::Sensitivies MGMHandlerLIS3MDL::getSensitivity(uint8_t ctrlRegister2) {
+    bool fs0Set = ctrlRegister2 & (1 << MGMLIS3MDL::FSO);  // Checks if FS0 bit is set
+    bool fs1Set = ctrlRegister2 & (1 << MGMLIS3MDL::FS1);  // Checks if FS1 bit is set
 
-    if (FS0_set && FS1_set)
-        return 16;
-    else if (!FS0_set && FS1_set)
-        return 12;
-    else if (FS0_set && !FS1_set)
-        return 8;
+    if (fs0Set && fs1Set)
+        return MGMLIS3MDL::Sensitivies::GAUSS_16;
+    else if (!fs0Set && fs1Set)
+        return MGMLIS3MDL::Sensitivies::GAUSS_12;
+    else if (fs0Set && !fs1Set)
+        return MGMLIS3MDL::Sensitivies::GAUSS_8;
     else
-        return 4;
+        return MGMLIS3MDL::Sensitivies::GAUSS_4;
 }
 
-float MGMHandlerLIS3MDL::getSensitivityFactor(uint8_t scale) {
-    return (float)scale / (float)MGMLIS3MDL::FIELD_LSB_PER_GAUSS;
+float MGMHandlerLIS3MDL::getSensitivityFactor(MGMLIS3MDL::Sensitivies sens) {
+    switch(sens) {
+    case(MGMLIS3MDL::GAUSS_4): {
+        return MGMLIS3MDL::FIELD_LSB_PER_GAUSS_4_SENS;
+    }
+    case(MGMLIS3MDL::GAUSS_8): {
+        return MGMLIS3MDL::FIELD_LSB_PER_GAUSS_8_SENS;
+    }
+    case(MGMLIS3MDL::GAUSS_12): {
+        return MGMLIS3MDL::FIELD_LSB_PER_GAUSS_12_SENS;
+    }
+    case(MGMLIS3MDL::GAUSS_16): {
+        return MGMLIS3MDL::FIELD_LSB_PER_GAUSS_16_SENS;
+    }
+    default: {
+        // Should never happen
+        return MGMLIS3MDL::FIELD_LSB_PER_GAUSS_4_SENS;
+    }
+    }
 }
 
 
diff --git a/mission/devices/MGMHandlerLIS3MDL.h b/mission/devices/MGMHandlerLIS3MDL.h
index 5d8364ee..1b7b9c9a 100644
--- a/mission/devices/MGMHandlerLIS3MDL.h
+++ b/mission/devices/MGMHandlerLIS3MDL.h
@@ -82,18 +82,16 @@ private:
      * e.g.: +- 4 gauss. See p.25 datasheet.
      *  @return The ReturnValue does not contain the sign of the value
      */
-    uint8_t getFullScale(uint8_t ctrlReg2);
+    MGMLIS3MDL::Sensitivies getSensitivity(uint8_t ctrlReg2);
 
     /**
-     * The 16 bit value needs to be divided by the full range of a 16bit value
-     * and then multiplied with the current scale of the MGM.
-     * This factor returns the factor required to achieve this with
-     * one multiplication.
+     * The 16 bit value needs to be multiplied with a sensitivity factor
+     * which depends on the sensitivity configuration
      *
-     * @param scale is the return value of the getFulscale Method
+     * @param sens Configured sensitivity of the LIS3 device
      * @return Multiplication factor to get the sensor value from raw data.
      */
-    float getSensitivityFactor(uint8_t scale);
+    float getSensitivityFactor(MGMLIS3MDL::Sensitivies sens);
 
     /**
      * This Command detects the device ID
diff --git a/mission/devices/devicedefinitions/MGMHandlerLIS3Definitions.h b/mission/devices/devicedefinitions/MGMHandlerLIS3Definitions.h
index a0ebb33a..98d881cf 100644
--- a/mission/devices/devicedefinitions/MGMHandlerLIS3Definitions.h
+++ b/mission/devices/devicedefinitions/MGMHandlerLIS3Definitions.h
@@ -8,19 +8,29 @@
 
 namespace MGMLIS3MDL {
 
-enum set {
+enum Set {
     ON, OFF
 };
-enum opMode {
+enum OpMode {
     LOW, MEDIUM, HIGH, ULTRA
 };
 
+enum Sensitivies: uint8_t {
+    GAUSS_4 = 4,
+    GAUSS_8 = 8,
+    GAUSS_12 = 12,
+    GAUSS_16 = 16
+};
+
 /* Actually 15, we just round up a bit */
 static constexpr size_t MAX_BUFFER_SIZE = 16;
 
 /* Field data register scaling */
 static constexpr uint8_t GAUSS_TO_MICROTESLA_FACTOR = 100;
-static constexpr uint16_t FIELD_LSB_PER_GAUSS = 27368;
+static constexpr float FIELD_LSB_PER_GAUSS_4_SENS = 1.0 / 6842.0;
+static constexpr float FIELD_LSB_PER_GAUSS_8_SENS = 1.0 / 3421.0;
+static constexpr float FIELD_LSB_PER_GAUSS_12_SENS = 1.0 / 2281.0;
+static constexpr float FIELD_LSB_PER_GAUSS_16_SENS = 1.0 / 1711.0;
 
 static const DeviceCommandId_t READ_CONFIG_AND_DATA = 0x00;
 static const DeviceCommandId_t SETUP_MGM = 0x01;