From 79641c4a036d055eaf5ae289cf7797193bff9e13 Mon Sep 17 00:00:00 2001
From: Alberto Nidasio <alberto.nidasio@skywarder.eu>
Date: Sun, 3 Jul 2022 22:34:01 +0200
Subject: [PATCH] [BMX160WithCorrection] Updated calibration implementation

---
 src/entrypoints/bmx160-calibration-entry.cpp  |  22 ----
 .../sensors/BMX160/BMX160WithCorrection.cpp   | 108 ++++--------------
 .../sensors/BMX160/BMX160WithCorrection.h     |  20 ++--
 .../sensors/calibration/BiasCalibration.h     |  19 ++-
 .../sensors/test-bmx160-with-correction.cpp   |   8 +-
 5 files changed, 52 insertions(+), 125 deletions(-)

diff --git a/src/entrypoints/bmx160-calibration-entry.cpp b/src/entrypoints/bmx160-calibration-entry.cpp
index ba523855e..92153a606 100644
--- a/src/entrypoints/bmx160-calibration-entry.cpp
+++ b/src/entrypoints/bmx160-calibration-entry.cpp
@@ -87,8 +87,6 @@ BMX160CorrectionParameters calibrateAccelerometer(
     BMX160CorrectionParameters correctionParameters);
 BMX160CorrectionParameters calibrateMagnetometer(
     BMX160CorrectionParameters correctionParameters);
-BMX160CorrectionParameters changeMinGyroCorrectionSamples(
-    BMX160CorrectionParameters correctionParameters);
 
 int main()
 {
@@ -113,10 +111,6 @@ int main()
         case 2:
             correctionParameters = calibrateMagnetometer(correctionParameters);
             break;
-        case 3:
-            correctionParameters =
-                changeMinGyroCorrectionSamples(correctionParameters);
-            break;
 
         default:
             break;
@@ -140,7 +134,6 @@ int menu()
     printf("\nWhat do you want to do?\n");
     printf("1. Calibrate accelerometer\n");
     printf("2. Calibrate magnetometer\n");
-    printf("3. Set minimum gyroscope samples for calibration\n");
     printf("\n>> ");
     scanf("%d", &choice);
 
@@ -420,18 +413,3 @@ BMX160CorrectionParameters calibrateMagnetometer(
 
     return correctionParameters;
 }
-
-BMX160CorrectionParameters changeMinGyroCorrectionSamples(
-    BMX160CorrectionParameters correctionParameters)
-{
-    // Show the user the current parameter
-    printf(
-        "The current minimum number of gyroscope samples for calibration "
-        "is %d\n",
-        correctionParameters.minGyroSamplesForCalibration);
-
-    printf("Insert the new value: ");
-    scanf("%d", &correctionParameters.minGyroSamplesForCalibration);
-
-    return correctionParameters;
-}
diff --git a/src/shared/sensors/BMX160/BMX160WithCorrection.cpp b/src/shared/sensors/BMX160/BMX160WithCorrection.cpp
index 50d2d3751..9ea095804 100644
--- a/src/shared/sensors/BMX160/BMX160WithCorrection.cpp
+++ b/src/shared/sensors/BMX160/BMX160WithCorrection.cpp
@@ -26,6 +26,9 @@
 
 #include <fstream>
 
+using namespace miosix;
+using namespace Eigen;
+
 namespace Boardcore
 {
 
@@ -37,9 +40,8 @@ BMX160CorrectionParameters::BMX160CorrectionParameters()
 
 std::string BMX160CorrectionParameters::header()
 {
-    return "acc_A_x,accel_A_y,accel_A_z,accel_b_x,accel_b_y,accel_b_z,"
-           "mag_A_x,mag_A_y,mag_A_z,mag_b_x,mag_b_y,mag_b_z,"
-           "minGyroSamplesForCalibration";
+    return "accAx,accAy,accAz,accbx,accby,accbz,"
+           "magAx,magAy,magAz,magbx,magby,magbz";
 }
 
 void BMX160CorrectionParameters::read(std::istream& inputStream)
@@ -63,9 +65,6 @@ void BMX160CorrectionParameters::read(std::istream& inputStream)
             inputStream.ignore(1, ',');
         }
     }
-
-    // Read gyroscope correction samples
-    inputStream >> minGyroSamplesForCalibration;
 }
 
 void BMX160CorrectionParameters::print(std::ostream& outputStream) const
@@ -79,17 +78,12 @@ void BMX160CorrectionParameters::print(std::ostream& outputStream) const
     outputStream << magnetoParams(0, 0) << "," << magnetoParams(1, 0) << ","
                  << magnetoParams(2, 0) << "," << magnetoParams(0, 1) << ","
                  << magnetoParams(1, 1) << "," << magnetoParams(2, 1) << ",";
-
-    // Print gyroscope correction samples
-    outputStream << minGyroSamplesForCalibration << "\n";
 }
 
 BMX160WithCorrection::BMX160WithCorrection(
     BMX160* bmx160, BMX160CorrectionParameters correctionParameters,
     AxisOrthoOrientation rotation)
-    : bmx160(bmx160), minGyroSamplesForCalibration(
-                          correctionParameters.minGyroSamplesForCalibration),
-      rotation(rotation)
+    : bmx160(bmx160), rotation(rotation)
 {
     accelerometerCorrector << correctionParameters.accelParams;
     magnetometerCorrector << correctionParameters.magnetoParams;
@@ -97,8 +91,7 @@ BMX160WithCorrection::BMX160WithCorrection(
 
 BMX160WithCorrection::BMX160WithCorrection(
     BMX160* bmx160, BMX160CorrectionParameters correctionParameters)
-    : bmx160(bmx160), minGyroSamplesForCalibration(
-                          correctionParameters.minGyroSamplesForCalibration)
+    : bmx160(bmx160)
 {
     accelerometerCorrector << correctionParameters.accelParams;
     magnetometerCorrector << correctionParameters.magnetoParams;
@@ -110,82 +103,28 @@ bool BMX160WithCorrection::init() { return true; }
 
 bool BMX160WithCorrection::selfTest() { return true; }
 
-bool BMX160WithCorrection::calibrate()
+void BMX160WithCorrection::startCalibration()
 {
-    if (!bmx160)
-    {
-        LOG_ERR(logger, "Driver doesn't point to valid sensor");
-        return false;
-    }
-
-    int samplesCounter = 0;
-    BiasCalibration<GyroscopeData> gyroscopeCalibrator;
-    BMX160Data fifoElement;
-    uint8_t fifoSize;
-    uint64_t gyroTimestamp = 0;
-
-    // Set reference vector
-    gyroscopeCalibrator.setReferenceVector({0, 0, 0});
-
-    // Read the fifo and feed the gyroscope data to the calibrator
-    fifoSize = bmx160->getLastFifoSize();
-    for (uint8_t i = 0; i < fifoSize; i++)
-    {
-        fifoElement = bmx160->getFifoElement(i);
-
-        if (fifoElement.angularVelocityTimestamp > gyroTimestamp)
-        {
-            gyroTimestamp = fifoElement.angularVelocityTimestamp;
-            gyroscopeCalibrator.feed(fifoElement);
-
-            samplesCounter++;
-        }
-    }
-
-    // Continues until the averaged samples are at least the amount specified in
-    // the configuration
-    while (samplesCounter < minGyroSamplesForCalibration)
-    {
-        // Wait for another sample
-        miosix::Thread::sleep(100);
-
-        // Read the fifo and feed the gyroscope data to the calibrator
-        fifoSize = bmx160->getLastFifoSize();
-        for (uint8_t i = 0; i < fifoSize; i++)
-        {
-            fifoElement = bmx160->getFifoElement(i);
-
-            if (fifoElement.angularVelocityTimestamp > gyroTimestamp)
-            {
-                gyroTimestamp = fifoElement.angularVelocityTimestamp;
-                gyroscopeCalibrator.feed(fifoElement);
+    gyroscopeCalibrator.reset();
+    calibrating = true;
+}
 
-                samplesCounter++;
-            }
-        }
-    }
+void BMX160WithCorrection::stopCalibration()
+{
+    calibrating = false;
 
-    // Compute and save the calibration results
     {
-        miosix::PauseKernelLock lock;
+        PauseKernelLock lock;
         gyroscopeCorrector = gyroscopeCalibrator.computeResult();
     }
 
     // Print the calibrator data
+    Vector3f gyroscopeCorrectionParameters;
     gyroscopeCorrector >> gyroscopeCorrectionParameters;
     LOG_INFO(logger, "Gyroscope bias vector from calibration\n");
     LOG_INFO(logger, "b = [    {: >2.5f}    {: >2.5f}    {: >2.5f}    ]\n\n",
              gyroscopeCorrectionParameters(0), gyroscopeCorrectionParameters(1),
              gyroscopeCorrectionParameters(2));
-
-    return true;
-}
-
-BMX160GyroscopeCalibrationBiases BMX160WithCorrection::getGyroscopeBiases()
-{
-    return BMX160GyroscopeCalibrationBiases{gyroscopeCorrectionParameters(0),
-                                            gyroscopeCorrectionParameters(1),
-                                            gyroscopeCorrectionParameters(2)};
 }
 
 BMX160CorrectionParameters
@@ -275,22 +214,19 @@ BMX160WithCorrectionData BMX160WithCorrection::sampleImpl()
     static_cast<GyroscopeData&>(result) << avgGyro;
 
     // Correct the averaged measurements
-    AccelerometerData acc = accelerometerCorrector.correct(result);
-    result                = acc;
-    MagnetometerData mag  = magnetometerCorrector.correct(result);
-    result                = mag;
-    GyroscopeData gyro;
-    gyro   = gyroscopeCorrector.correct(result);
-    result = gyro;
+    result = accelerometerCorrector.correct(result);
+    result = magnetometerCorrector.correct(result);
+    result = gyroscopeCorrector.correct(result);
 
     // Get the timestamp of the newest value in fifo
     result.accelerationTimestamp    = fifoElement.accelerationTimestamp;
     result.magneticFieldTimestamp   = fifoElement.accelerationTimestamp;
     result.angularVelocityTimestamp = fifoElement.accelerationTimestamp;
 
-    result = rotateAxis(result);
+    if (calibrating)
+        gyroscopeCalibrator.feed(result);
 
-    return result;
+    return rotateAxis(result);
 }
 
 BMX160WithCorrectionData BMX160WithCorrection::rotateAxis(
diff --git a/src/shared/sensors/BMX160/BMX160WithCorrection.h b/src/shared/sensors/BMX160/BMX160WithCorrection.h
index 7715502d1..99f05cda1 100644
--- a/src/shared/sensors/BMX160/BMX160WithCorrection.h
+++ b/src/shared/sensors/BMX160/BMX160WithCorrection.h
@@ -27,6 +27,7 @@
 #include <sensors/calibration/AxisOrientation.h>
 #include <sensors/calibration/BiasCalibration.h>
 #include <sensors/calibration/SixParameterCalibration.h>
+#include <utils/Stats/Stats.h>
 
 #include "BMX160WithCorrectionData.h"
 
@@ -39,7 +40,6 @@ namespace Boardcore
 struct BMX160CorrectionParameters
 {
     Eigen::Matrix<float, 3, 2> accelParams, magnetoParams;
-    int minGyroSamplesForCalibration = 0;
 
     BMX160CorrectionParameters();
 
@@ -90,13 +90,20 @@ public:
     bool selfTest() override;
 
     /**
-     * @brief Performs the gyroscope calibration.
+     * @brief Starts collecting calibration data for the gyroscope.
      *
      * The gyroscope calibration consists in averaging some samples to measure
      * the bias. This function is intended to run while another thread samples
-     * the bmx at at least 10Hz.
+     * the bmx.
+     *
+     * Call stopCalibration() to end collection and finalizing the offset.
      */
-    bool calibrate();
+    void startCalibration();
+
+    /**
+     * @brief Stops the data collection and finalizes the calibration.
+     */
+    void stopCalibration();
 
     /**
      * @brief Utility function to read correction parameters from file.
@@ -119,8 +126,6 @@ private:
 
     BMX160* bmx160;
 
-    int minGyroSamplesForCalibration = 200;
-
     AxisOrthoOrientation rotation = {Direction::POSITIVE_X,
                                      Direction::POSITIVE_Y};
 
@@ -128,7 +133,8 @@ private:
     SixParameterCorrector<MagnetometerData> magnetometerCorrector;
     BiasCorrector<GyroscopeData> gyroscopeCorrector{};
 
-    Eigen::Vector3f gyroscopeCorrectionParameters;
+    bool calibrating = false;
+    BiasCalibration<GyroscopeData> gyroscopeCalibrator;
 
     PrintLogger logger = Logging::getLogger("bmx160withcorrection");
 };
diff --git a/src/shared/sensors/calibration/BiasCalibration.h b/src/shared/sensors/calibration/BiasCalibration.h
index be84f860e..389804703 100644
--- a/src/shared/sensors/calibration/BiasCalibration.h
+++ b/src/shared/sensors/calibration/BiasCalibration.h
@@ -81,6 +81,8 @@ public:
 
     Eigen::Vector3f getReferenceVector();
 
+    void reset();
+
     bool feed(const T& measured, const AxisOrientation& transform) override;
 
     bool feed(const T& measured);
@@ -88,7 +90,7 @@ public:
     BiasCorrector<T> computeResult();
 
 private:
-    Eigen::Vector3f sum, ref;
+    Eigen::Vector3f mean, ref;
     unsigned numSamples;
 };
 
@@ -135,7 +137,7 @@ T BiasCorrector<T>::correct(const T& data) const
 
 template <typename T>
 BiasCalibration<T>::BiasCalibration()
-    : sum(0, 0, 0), ref(0, 0, 0), numSamples(0)
+    : mean(0, 0, 0), ref(0, 0, 0), numSamples(0)
 {
 }
 
@@ -151,6 +153,12 @@ Eigen::Vector3f BiasCalibration<T>::getReferenceVector()
     return ref;
 }
 
+template <typename T>
+void BiasCalibration<T>::reset()
+{
+    mean = {0, 0, 0};
+}
+
 /**
  * BiasCalibration accepts an indefinite number of samples,
  * so feed(...) never returns false.
@@ -162,8 +170,9 @@ bool BiasCalibration<T>::feed(const T& measured,
     Eigen::Vector3f vec;
     measured >> vec;
 
-    sum += (transform.getMatrix().transpose() * ref) - vec;
     numSamples++;
+    mean +=
+        (((transform.getMatrix().transpose() * ref) - vec) - mean) / numSamples;
 
     return true;
 }
@@ -178,8 +187,8 @@ template <typename T>
 BiasCorrector<T> BiasCalibration<T>::computeResult()
 {
     if (numSamples == 0)
-        return {Eigen::Vector3f{0, 0, 0}};
-    return {sum / numSamples};
+        return {{0, 0, 0}};
+    return {mean};
 }
 
 }  // namespace Boardcore
diff --git a/src/tests/sensors/test-bmx160-with-correction.cpp b/src/tests/sensors/test-bmx160-with-correction.cpp
index 8afca0d0f..37be46362 100644
--- a/src/tests/sensors/test-bmx160-with-correction.cpp
+++ b/src/tests/sensors/test-bmx160-with-correction.cpp
@@ -117,11 +117,9 @@ int main()
     miosix::Thread *samplingThread =
         miosix::Thread::create(bmx160Sample, 2048, miosix::MAIN_PRIORITY,
                                nullptr, miosix::Thread::JOINABLE);
-    if (!bmx160WithCorrection.calibrate())
-    {
-        TRACE("Calibration failed!\n");
-        return -1;
-    }
+    bmx160WithCorrection.startCalibration();
+    miosix::Thread::sleep(2000);
+    bmx160WithCorrection.stopCalibration();
     stopSamplingThread = true;
     samplingThread->join();
     TRACE("Calibration completed\n");
-- 
GitLab