From aeddc92cf3d10dfa1c87585dbc63c1760364b72c Mon Sep 17 00:00:00 2001
From: Marius Eggert <eggertm@irs.uni-stuttgart.de>
Date: Fri, 20 Jan 2023 10:33:36 +0100
Subject: [PATCH] changed ang vel calculation to being sensitivity based
 instead of range based

---
 mission/devices/GyroADIS1650XHandler.cpp          | 15 ++++++---------
 mission/devices/GyroADIS1650XHandler.h            |  2 +-
 .../devicedefinitions/GyroADIS1650XDefinitions.h  |  5 +++++
 3 files changed, 12 insertions(+), 10 deletions(-)

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<ADIS1650X::RangMdlBitfield>((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<float>(angVelocXRaw) / static_cast<float>(INT16_MAX) * rangeMultiplicator;
+        primaryDataset.angVelocX.value = static_cast<float>(angVelocXRaw) * sensitivity;
         int16_t angVelocYRaw = packet[6] << 8 | packet[7];
-        primaryDataset.angVelocY.value =
-            static_cast<float>(angVelocYRaw) / static_cast<float>(INT16_MAX) * rangeMultiplicator;
+        primaryDataset.angVelocY.value = static_cast<float>(angVelocYRaw) * sensitivity;
         int16_t angVelocZRaw = packet[8] << 8 | packet[9];
-        primaryDataset.angVelocZ.value =
-            static_cast<float>(angVelocZRaw) / static_cast<float>(INT16_MAX) * rangeMultiplicator;
+        primaryDataset.angVelocZ.value = static_cast<float>(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,