Merge pull request #26 from bareboat-necessities/codex/fix-error-in-imureader-for-setaccelrange

Fix BMI270 FIFO setup to use supported accel range configuration

Author: mgrouch

atomS3R_bmi270_fifo/ImuReader.h

@@ -73,7 +73,7 @@ class ImuReader
     }
 
     imu.setAccelODR(accelOdr);
-    imu.setAccelRange(kDefaultAccelRange);
+    setAccelRange(imu, kDefaultAccelRange);
     imu.setGyroODR(gyroOdr);
     const float accelHz = imuAccelOdrToHz(accelOdr);
     const float gyroHz = imuGyroOdrToHz(gyroOdr);
@@ -209,6 +209,20 @@ class ImuReader
     }
   }
 
+  static void setAccelRange(BMI270 &imu, uint8_t range)
+  {
+    bmi2_sens_config config{};
+    config.type = BMI2_ACCEL;
+
+    if (imu.getConfig(&config) != BMI2_OK)
+    {
+      return;
+    }
+
+    config.cfg.acc.range = range;
+    imu.setConfig(config);
+  }
+
   BMI270 imu;
   BMI270_SensorData fifoFrames[kDefaultFifoWatermarkFrames];
   bool hasLastImuTimestamp = false;