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