From 5db3312a91637aebcb31ffc11538eb265c5e2967 Mon Sep 17 00:00:00 2001 From: HampusM Date: Tue, 1 Mar 2022 09:43:10 +0100 Subject: refactor: improve calibration value handling --- src/sensor/calibration.cpp | 94 ++++++++++++++++++++-------------------------- src/sensor/calibration.hpp | 29 +++++++------- 2 files changed, 57 insertions(+), 66 deletions(-) (limited to 'src/sensor') diff --git a/src/sensor/calibration.cpp b/src/sensor/calibration.cpp index 827094c..1538baa 100644 --- a/src/sensor/calibration.cpp +++ b/src/sensor/calibration.cpp @@ -6,13 +6,12 @@ SensorCalibrator::SensorCalibrator(UniquePtr sensor, SerialStream sout) : _sensor(sensor), _sout(sout) { - _resetValues(); } bool SensorCalibrator::calibrate(unsigned int throttle_time) { bool done = false; - Time start_time = time_now(); + auto start_time = time_now(); while (!done) { @@ -23,85 +22,74 @@ bool SensorCalibrator::calibrate(unsigned int throttle_time) delay(throttle_time); - _resetValues(); + auto values = SensorCalibratorValues(); for (unsigned int i = 0U; i < SENSOR_READ_CNT; i++) { - _updateValues(); + _updateValues(values); } - _adjustValues(); + _adjustValues(values); - _sout << "Accel X: " << _accel_x << " " - << "Accel Y: " << _accel_y << " " - << "Accel Z: " << _accel_z << " " - << "Gyro X: " << _gyro_x << " " - << "Gyro Y: " << _gyro_y << " " - << "Gyro Z: " << _gyro_z << endl; + _sout << "Accel X: " << values.accel_x << " " + << "Accel Y: " << values.accel_y << " " + << "Accel Z: " << values.accel_z << " " + << "Gyro X: " << values.gyro_x << " " + << "Gyro Y: " << values.gyro_y << " " + << "Gyro Z: " << values.gyro_z << endl; - if (_isValuesInRange()) + if (_isValuesInRange(values)) { done = true; } - _adjustCalibration(); + _adjustCalibrationWithValues(values); } return true; } -void SensorCalibrator::_resetValues() -{ - _accel_x = 0; - _accel_y = 0; - _accel_z = 0; - - _gyro_x = 0; - _gyro_y = 0; - _gyro_z = 0; -} - -void SensorCalibrator::_updateValues() +void SensorCalibrator::_updateValues(SensorCalibratorValues &values) { _sensor->read(); - _accel_x -= _sensor->getAccelX(); - _accel_y -= _sensor->getAccelY(); - _accel_z -= _sensor->getAccelZ(); + values.accel_x -= _sensor->getAccelX(); + values.accel_y -= _sensor->getAccelY(); + values.accel_z -= _sensor->getAccelZ(); - _gyro_x -= _sensor->getGyroX(); - _gyro_y -= _sensor->getGyroY(); - _gyro_z -= _sensor->getGyroZ(); + values.gyro_x -= _sensor->getGyroX(); + values.gyro_y -= _sensor->getGyroY(); + values.gyro_z -= _sensor->getGyroZ(); } -void SensorCalibrator::_adjustValues() +void SensorCalibrator::_adjustCalibrationWithValues(const SensorCalibratorValues &values) { - _accel_x *= SENSOR_VAL_ADJUST; - _accel_y *= SENSOR_VAL_ADJUST; - _accel_z *= SENSOR_VAL_ADJUST; + _sensor->accel_cal_x += values.accel_x; + _sensor->accel_cal_y += values.accel_y; + _sensor->accel_cal_z += values.accel_z; - _gyro_x *= SENSOR_VAL_ADJUST; - _gyro_y *= SENSOR_VAL_ADJUST; - _gyro_z *= SENSOR_VAL_ADJUST; + _sensor->gyro_cal_x += values.gyro_x; + _sensor->gyro_cal_y += values.gyro_y; + _sensor->gyro_cal_z += values.gyro_z; } -bool SensorCalibrator::_isValuesInRange() const +void SensorCalibrator::_adjustValues(SensorCalibratorValues &values) { - return (_accel_x < ACCEL_CAL_X_MAX && _accel_x > ACCEL_CAL_X_MIN && - _accel_y < ACCEL_CAL_Y_MAX && _accel_y > ACCEL_CAL_Y_MIN && - _accel_z < ACCEL_CAL_Z_MAX && _accel_z > ACCEL_CAL_Z_MIN && - _gyro_x < GYRO_CAL_X_MAX && _gyro_x > GYRO_CAL_X_MIN && - _gyro_y < GYRO_CAL_Y_MAX && _gyro_y > GYRO_CAL_Y_MIN && - _gyro_z < GYRO_CAL_Z_MAX && _gyro_z > GYRO_CAL_Z_MIN); + values.accel_x *= SENSOR_VAL_ADJUST; + values.accel_y *= SENSOR_VAL_ADJUST; + values.accel_z *= SENSOR_VAL_ADJUST; + + values.gyro_x *= SENSOR_VAL_ADJUST; + values.gyro_y *= SENSOR_VAL_ADJUST; + values.gyro_z *= SENSOR_VAL_ADJUST; } -void SensorCalibrator::_adjustCalibration() +bool SensorCalibrator::_isValuesInRange(const SensorCalibratorValues &values) { - _sensor->accel_cal_x += _accel_x; - _sensor->accel_cal_y += _accel_y; - _sensor->accel_cal_z += _accel_z; - - _sensor->gyro_cal_x += _gyro_x; - _sensor->gyro_cal_y += _gyro_y; - _sensor->gyro_cal_z += _gyro_z; + return (values.accel_x < ACCEL_CAL_X_MAX && values.accel_x > ACCEL_CAL_X_MIN && + values.accel_y < ACCEL_CAL_Y_MAX && values.accel_y > ACCEL_CAL_Y_MIN && + values.accel_z < ACCEL_CAL_Z_MAX && values.accel_z > ACCEL_CAL_Z_MIN && + values.gyro_x < GYRO_CAL_X_MAX && values.gyro_x > GYRO_CAL_X_MIN && + values.gyro_y < GYRO_CAL_Y_MAX && values.gyro_y > GYRO_CAL_Y_MIN && + values.gyro_z < GYRO_CAL_Z_MAX && values.gyro_z > GYRO_CAL_Z_MIN); } diff --git a/src/sensor/calibration.hpp b/src/sensor/calibration.hpp index a808fe9..a2da58b 100644 --- a/src/sensor/calibration.hpp +++ b/src/sensor/calibration.hpp @@ -28,6 +28,18 @@ constexpr uint32_t CALIBRATION_TIMEOUT = 120000; // Milliseconds constexpr unsigned int SENSOR_READ_CNT = 20; constexpr float SENSOR_VAL_ADJUST = 0.05; +class SensorCalibratorValues +{ +public: + double accel_x = 0; + double accel_y = 0; + double accel_z = 0; + + double gyro_x = 0; + double gyro_y = 0; + double gyro_z = 0; +}; + /** * Sensor calibrator. */ @@ -52,22 +64,13 @@ public: bool calibrate(unsigned int throttle_time); private: - void _resetValues(); - void _updateValues(); - void _adjustValues(); - bool _isValuesInRange() const; + void _updateValues(SensorCalibratorValues &values); + void _adjustCalibrationWithValues(const SensorCalibratorValues &values); - void _adjustCalibration(); + static void _adjustValues(SensorCalibratorValues &values); + static bool _isValuesInRange(const SensorCalibratorValues &values); UniquePtr _sensor; SerialStream _sout; - - double _accel_x = 0; - double _accel_y = 0; - double _accel_z = 0; - - double _gyro_x = 0; - double _gyro_y = 0; - double _gyro_z = 0; }; -- cgit v1.2.3-18-g5258