3 files changed, 15 insertions, 1 deletions
diff --git a/os/ex/ST/l3gd20.c b/os/ex/ST/l3gd20.c
index 1030abb97..c618a8c8d 100644
--- a/os/ex/ST/l3gd20.c
+++ b/os/ex/ST/l3gd20.c
@@ -332,6 +332,10 @@ static msg_t reset_sensivity(void *ip) {
   else if(((L3GD20Driver *)ip)->config->fullscale == L3GD20_FS_2000DPS)
 	for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++)
       ((L3GD20Driver *)ip)->sensitivity[i] = L3GD20_SENS_2000DPS;
+  else {
+    osalDbgAssert(FALSE, "reset_sensivity(), full scale issue");
+    return MSG_RESET;
+  }
   return MSG_OK;
 }
 
diff --git a/os/ex/ST/lis3mdl.c b/os/ex/ST/lis3mdl.c
index 9f95e6a55..3ed042aa7 100644
--- a/os/ex/ST/lis3mdl.c
+++ b/os/ex/ST/lis3mdl.c
@@ -313,8 +313,10 @@ static msg_t reset_sensivity(void *ip) {
   else if(((LIS3MDLDriver *)ip)->config->fullscale == LIS3MDL_FS_16GA)
     for(i = 0; i < LIS3MDL_NUMBER_OF_AXES; i++)
       ((LIS3MDLDriver *)ip)->sensitivity[i] = LIS3MDL_SENS_16GA;
-  else
+  else {
     osalDbgAssert(FALSE, "reset_sensivity(), compass full scale issue");
+    return MSG_RESET;
+  }
   return MSG_OK;
 }
 
diff --git a/os/ex/ST/lsm6ds0.c b/os/ex/ST/lsm6ds0.c
index 5afed9ef7..f657ad879 100644
--- a/os/ex/ST/lsm6ds0.c
+++ b/os/ex/ST/lsm6ds0.c
@@ -591,6 +591,10 @@ static msg_t acc_reset_sensivity(void *ip) {
   else if(((LSM6DS0Driver *)ip)->config->acccfg->fullscale == LSM6DS0_ACC_FS_16G)
 	for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)
       ((LSM6DS0Driver *)ip)->accsensitivity[i] = LSM6DS0_ACC_SENS_16G;
+  else {
+    osalDbgAssert(FALSE, "reset_sensivity(), accelerometer full scale issue");
+    return MSG_RESET;
+  }
   return MSG_OK;
 }
 
@@ -611,6 +615,10 @@ static msg_t gyro_reset_sensivity(void *ip) {
   else if(((LSM6DS0Driver *)ip)->config->gyrocfg->fullscale == LSM6DS0_GYRO_FS_2000DSP)
 	for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)
       ((LSM6DS0Driver *)ip)->gyrosensitivity[i] = LSM6DS0_GYRO_SENS_2000DPS;
+  else {
+    osalDbgAssert(FALSE, "reset_sensivity(), gyroscope full scale issue");
+    return MSG_RESET;
+  }
   return MSG_OK;
 }