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,25 +343,39 @@ ReturnValue_t MGMHandlerLIS3MDL::interpretDeviceReply(DeviceCommandId_t id,
     return RETURN_OK;
 }
 
-uint8_t MGMHandlerLIS3MDL::getFullScale(uint8_t ctrlRegister2) {
-    bool FS0 = false;
-    bool FS1 = false;
-    if ((ctrlRegister2 >> 5) == 1)
-        FS0 = true;
-    if ((ctrlRegister2 >> 6) == 1)
-        FS1 = true;
-    if ((FS0 == true) && (FS1 == true))
-        return 16;
-    else if ((FS0 == false) && (FS1 == true))
-        return 12;
-    else if ((FS0 == true) && (FS1 == false))
-        return 8;
+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 (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 / (INT16_MAX);
+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;
+    }
+    }
 }
 
 

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

mission/devices/devicedefinitions/MGMHandlerLIS3Definitions.h

@@ -8,17 +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 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;