LIS3MDL scaling bugfix #93

Merged
lteichroeb merged 2 commits from lis3mdl-scaling-bugfix into develop 2021-09-15 10:35:20 +02:00
3 changed files with 50 additions and 28 deletions

View File

@ -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;
}
}
}

View File

@ -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

View File

@ -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;