diff --git a/CHANGELOG.md b/CHANGELOG.md index 1a407d96..2f4744ab 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -42,6 +42,10 @@ list yields a list of all related PRs for each release. previous range setting was wrong. Also fixed a small error properly set internal state on shut-down. PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/342 +- `GyroADIS1650XHandler`: Changed calculation of angular rate to be sensitivity based instead of + max. range based, as previous fix still left an margin of error between ADIS16505 sensors + and L3GD20 sensors. + PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/346 # [v1.19.0] 2023-01-10 diff --git a/mission/devices/GyroADIS1650XHandler.cpp b/mission/devices/GyroADIS1650XHandler.cpp index 5b59d7f6..765bc589 100644 --- a/mission/devices/GyroADIS1650XHandler.cpp +++ b/mission/devices/GyroADIS1650XHandler.cpp @@ -228,15 +228,15 @@ ReturnValue_t GyroADIS1650XHandler::interpretDeviceReply(DeviceCommandId_t id, static_cast((rangMdlRaw >> 2) & 0b11); switch (bitfield) { case (ADIS1650X::RangMdlBitfield::RANGE_125_1BMLZ): { - rangeMultiplicator = RANGE_1BMLZ; + sensitivity = SENSITIVITY_1BMLZ; break; } case (ADIS1650X::RangMdlBitfield::RANGE_500_2BMLZ): { - rangeMultiplicator = RANGE_2BMLZ; + sensitivity = SENSITIVITY_2BMLZ; break; } case (ADIS1650X::RangMdlBitfield::RANGE_2000_3BMLZ): { - rangeMultiplicator = RANGE_3BMLZ; + sensitivity = SENSITIVITY_3BMLZ; break; } case (RangMdlBitfield::RESERVED): { @@ -298,14 +298,11 @@ ReturnValue_t GyroADIS1650XHandler::handleSensorData(const uint8_t *packet) { { PoolReadGuard pg(&primaryDataset); int16_t angVelocXRaw = packet[4] << 8 | packet[5]; - primaryDataset.angVelocX.value = - static_cast(angVelocXRaw) / static_cast(INT16_MAX) * rangeMultiplicator; + primaryDataset.angVelocX.value = static_cast(angVelocXRaw) * sensitivity; int16_t angVelocYRaw = packet[6] << 8 | packet[7]; - primaryDataset.angVelocY.value = - static_cast(angVelocYRaw) / static_cast(INT16_MAX) * rangeMultiplicator; + primaryDataset.angVelocY.value = static_cast(angVelocYRaw) * sensitivity; int16_t angVelocZRaw = packet[8] << 8 | packet[9]; - primaryDataset.angVelocZ.value = - static_cast(angVelocZRaw) / static_cast(INT16_MAX) * rangeMultiplicator; + primaryDataset.angVelocZ.value = static_cast(angVelocZRaw) * sensitivity; float accelScaling = 0; if (adisType == ADIS1650X::Type::ADIS16507) { diff --git a/mission/devices/GyroADIS1650XHandler.h b/mission/devices/GyroADIS1650XHandler.h index f664407c..34335c51 100644 --- a/mission/devices/GyroADIS1650XHandler.h +++ b/mission/devices/GyroADIS1650XHandler.h @@ -46,7 +46,7 @@ class GyroADIS1650XHandler : public DeviceHandlerBase { ADIS1650X::Type adisType; AdisGyroPrimaryDataset primaryDataset; AdisGyroConfigDataset configDataset; - double rangeMultiplicator = ADIS1650X::RANGE_UNSET; + double sensitivity = ADIS1650X::SENSITIVITY_UNSET; bool goToNormalMode = false; bool warningSwitch = true; diff --git a/mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h b/mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h index fb9e4d44..89a0c918 100644 --- a/mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h +++ b/mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h @@ -18,6 +18,11 @@ static constexpr double RANGE_UNSET = 0.0; static constexpr double RANGE_1BMLZ = 125.0; static constexpr double RANGE_2BMLZ = 500.0; static constexpr double RANGE_3BMLZ = 2000.0; +// Sensitivities in deg/s/LSB +static constexpr double SENSITIVITY_UNSET = 0.0; +static constexpr double SENSITIVITY_1BMLZ = 0.00625; +static constexpr double SENSITIVITY_2BMLZ = 0.025; +static constexpr double SENSITIVITY_3BMLZ = 0.1; enum RangMdlBitfield { RANGE_125_1BMLZ = 0b00,