From ca79f370b00394676797bdaca609501f4c581c64 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Leon=20Teichr=C3=B6b?= Date: Tue, 14 Sep 2021 12:30:46 +0200 Subject: [PATCH 1/2] Fixed bugs in mag. field scaling math. --- mission/devices/MGMHandlerLIS3MDL.cpp | 18 ++++++++---------- .../MGMHandlerLIS3Definitions.h | 2 ++ 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/mission/devices/MGMHandlerLIS3MDL.cpp b/mission/devices/MGMHandlerLIS3MDL.cpp index 85cb3407..54cf55c9 100644 --- a/mission/devices/MGMHandlerLIS3MDL.cpp +++ b/mission/devices/MGMHandlerLIS3MDL.cpp @@ -346,24 +346,22 @@ ReturnValue_t MGMHandlerLIS3MDL::interpretDeviceReply(DeviceCommandId_t id, } 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)) + 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 + + if (FS0_set && FS1_set) return 16; - else if ((FS0 == false) && (FS1 == true)) + else if (!FS0_set && FS1_set) return 12; - else if ((FS0 == true) && (FS1 == false)) + else if (FS0_set && !FS1_set) return 8; else return 4; } float MGMHandlerLIS3MDL::getSensitivityFactor(uint8_t scale) { - return (float) scale / (INT16_MAX); + return (float)scale / (float)MGMLIS3MDL::FIELD_LSB_PER_GAUSS; } diff --git a/mission/devices/devicedefinitions/MGMHandlerLIS3Definitions.h b/mission/devices/devicedefinitions/MGMHandlerLIS3Definitions.h index c15b60ef..a0ebb33a 100644 --- a/mission/devices/devicedefinitions/MGMHandlerLIS3Definitions.h +++ b/mission/devices/devicedefinitions/MGMHandlerLIS3Definitions.h @@ -18,7 +18,9 @@ enum opMode { /* 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 const DeviceCommandId_t READ_CONFIG_AND_DATA = 0x00; static const DeviceCommandId_t SETUP_MGM = 0x01; From 43fe7e0aa072747e876044b9f88b50993523cf20 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 14 Sep 2021 13:55:56 +0200 Subject: [PATCH 2/2] 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;