From 408a529e8e54d1842766cb3b47c90c8bdc245f5d Mon Sep 17 00:00:00 2001
From: Carlo PARATA <carlo.parata@st.com>
Date: Thu, 4 Jul 2024 14:12:33 +0200
Subject: [PATCH] Fix issue on full scale

---
 library.properties       |  2 +-
 src/LSM6DSV16XSensor.cpp | 28 ++++++++++++++++++++++++----
 2 files changed, 25 insertions(+), 5 deletions(-)

diff --git a/library.properties b/library.properties
index 3376e1b..0c45524 100644
--- a/library.properties
+++ b/library.properties
@@ -1,5 +1,5 @@
 name=STM32duino LSM6DSV16X
-version=2.0.0
+version=2.0.1
 author=SRA
 maintainer=stm32duino
 sentence=Ultra Low Power inertial measurement unit.
diff --git a/src/LSM6DSV16XSensor.cpp b/src/LSM6DSV16XSensor.cpp
index fabe700..9517e64 100644
--- a/src/LSM6DSV16XSensor.cpp
+++ b/src/LSM6DSV16XSensor.cpp
@@ -55,8 +55,6 @@ LSM6DSV16XSensor::LSM6DSV16XSensor(TwoWire *i2c, uint8_t address) : dev_i2c(i2c)
   dev_spi = NULL;
   acc_is_enabled = 0L;
   gyro_is_enabled = 0L;
-  acc_fs = LSM6DSV16X_2g;
-  gyro_fs = LSM6DSV16X_125dps;
 }
 
 /** Constructor
@@ -73,8 +71,6 @@ LSM6DSV16XSensor::LSM6DSV16XSensor(SPIClass *spi, int cs_pin, uint32_t spi_speed
   dev_i2c = NULL;
   acc_is_enabled = 0L;
   gyro_is_enabled = 0L;
-  acc_fs = LSM6DSV16X_2g;
-  gyro_fs = LSM6DSV16X_125dps;
 }
 
 /**
@@ -83,6 +79,8 @@ LSM6DSV16XSensor::LSM6DSV16XSensor(SPIClass *spi, int cs_pin, uint32_t spi_speed
  */
 LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::begin()
 {
+  int32_t fs = 0;
+
   if (dev_spi) {
     // Configure CS pin
     pinMode(cs_pin, OUTPUT);
@@ -113,6 +111,16 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::begin()
     return LSM6DSV16X_ERROR;
   }
 
+  /* Set accelerometer full scale internal variable */
+  if (Get_X_FS(&fs) != LSM6DSV16X_OK) {
+    return LSM6DSV16X_ERROR;
+  }
+
+  acc_fs = (fs <= 2) ? LSM6DSV16X_2g
+           : (fs <= 4) ? LSM6DSV16X_4g
+           : (fs <= 8) ? LSM6DSV16X_8g
+           :                    LSM6DSV16X_16g;
+
   /* Full scale selection. */
   if (lsm6dsv16x_xl_full_scale_set(&reg_ctx, LSM6DSV16X_2g) != LSM6DSV16X_OK) {
     return LSM6DSV16X_ERROR;
@@ -126,6 +134,18 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::begin()
     return LSM6DSV16X_ERROR;
   }
 
+  /* Set gyroscope full scale internal variable */
+  if (Get_G_FS(&fs) != LSM6DSV16X_OK) {
+    return LSM6DSV16X_ERROR;
+  }
+
+  gyro_fs = (fs <= 125)  ? LSM6DSV16X_125dps
+            : (fs <= 250)  ? LSM6DSV16X_250dps
+            : (fs <= 500)  ? LSM6DSV16X_500dps
+            : (fs <= 1000) ? LSM6DSV16X_1000dps
+            : (fs <= 2000) ? LSM6DSV16X_2000dps
+            :                       LSM6DSV16X_4000dps;
+
   /* Full scale selection. */
   if (lsm6dsv16x_gy_full_scale_set(&reg_ctx, LSM6DSV16X_2000dps) != LSM6DSV16X_OK) {
     return LSM6DSV16X_ERROR;