From bcdce9633dc351d3bc7f347a165348b8fab87cd9 Mon Sep 17 00:00:00 2001 From: HampusM Date: Tue, 15 Feb 2022 12:33:52 +0100 Subject: refactor: reorganize files & improve classes --- src/sensor/calibration.cpp | 107 +++++++++++++++++++++++++++++++++++++++++++++ src/sensor/calibration.hpp | 73 +++++++++++++++++++++++++++++++ src/sensor/sensor.cpp | 28 ++++++------ src/sensor/sensor.hpp | 27 ++++++------ 4 files changed, 206 insertions(+), 29 deletions(-) create mode 100644 src/sensor/calibration.cpp create mode 100644 src/sensor/calibration.hpp (limited to 'src/sensor') diff --git a/src/sensor/calibration.cpp b/src/sensor/calibration.cpp new file mode 100644 index 0000000..40bb52b --- /dev/null +++ b/src/sensor/calibration.cpp @@ -0,0 +1,107 @@ +#include "calibration.hpp" + +#include "std/time.hpp" +#include "utils.hpp" + +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(); + + while (!done) + { + if (time_now().diff(start_time).millisecs() >= CALIBRATION_TIMEOUT) + { + return false; + } + + delay(throttle_time); + + _resetValues(); + + for (unsigned int i = 0U; i < SENSOR_READ_CNT; i++) + { + _updateValues(); + } + + _adjustValues(); + + _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; + + if (_isValuesInRange()) + { + done = true; + } + + _adjustCalibration(); + } + + 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() +{ + _sensor->read(); + + _accel_x -= _sensor->getAccelX(); + _accel_y -= _sensor->getAccelY(); + _accel_z -= _sensor->getAccelZ(); + + _gyro_x -= _sensor->getGyroX(); + _gyro_y -= _sensor->getGyroY(); + _gyro_z -= _sensor->getGyroZ(); +} + +void SensorCalibrator::_adjustValues() +{ + _accel_x *= SENSOR_VAL_ADJUST; + _accel_y *= SENSOR_VAL_ADJUST; + _accel_z *= SENSOR_VAL_ADJUST; + + _gyro_x *= SENSOR_VAL_ADJUST; + _gyro_y *= SENSOR_VAL_ADJUST; + _gyro_z *= SENSOR_VAL_ADJUST; +} + +bool SensorCalibrator::_isValuesInRange() const +{ + 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); +} + +void SensorCalibrator::_adjustCalibration() +{ + _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; +} diff --git a/src/sensor/calibration.hpp b/src/sensor/calibration.hpp new file mode 100644 index 0000000..03e2e9c --- /dev/null +++ b/src/sensor/calibration.hpp @@ -0,0 +1,73 @@ +#pragma once + +#include "sensor/sensor.hpp" +#include "serial.hpp" +#include "std/memory.hpp" + +// Calibration precision +constexpr float ACCEL_CAL_X_MAX = 0.006; +constexpr float ACCEL_CAL_X_MIN = -0.006; + +constexpr float ACCEL_CAL_Y_MAX = 0.006; +constexpr float ACCEL_CAL_Y_MIN = -0.006; + +constexpr float ACCEL_CAL_Z_MAX = 0.006; +constexpr float ACCEL_CAL_Z_MIN = -0.006; + +constexpr float GYRO_CAL_X_MAX = 0.06; +constexpr float GYRO_CAL_X_MIN = -0.06; + +constexpr float GYRO_CAL_Y_MAX = 0.06; +constexpr float GYRO_CAL_Y_MIN = -0.06; + +constexpr float GYRO_CAL_Z_MAX = 0.06; +constexpr float GYRO_CAL_Z_MIN = -0.06; + +constexpr uint32_t CALIBRATION_TIMEOUT = 120000; // Milliseconds + +constexpr unsigned int SENSOR_READ_CNT = 20; +constexpr float SENSOR_VAL_ADJUST = 0.05; + +/** + * Sensor calibrator. + */ +class SensorCalibrator +{ +public: + /** + * Sensor calibrator. + * + * @param sensor A sensor to calibrate + * @param sout A Serial output stream + */ + SensorCalibrator(UniquePtr &sensor, SerialStream sout); + + /** + * Calibrates the sensor. + * + * @param throttle_time The sensor's throttle time + * @returns Whether or not the calibration succeeded. Will return false on + * timeout. + */ + bool calibrate(unsigned int throttle_time); + +private: + void _resetValues(); + void _updateValues(); + void _adjustValues(); + bool _isValuesInRange() const; + + void _adjustCalibration(); + + 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; +}; diff --git a/src/sensor/sensor.cpp b/src/sensor/sensor.cpp index a3b9a5c..5ca5dc3 100644 --- a/src/sensor/sensor.cpp +++ b/src/sensor/sensor.cpp @@ -1,20 +1,18 @@ #include "sensor.hpp" -#include "registers.hpp" -#include "utils/serial.hpp" -#include "utils/time.hpp" +#include "sensor/registers.hpp" Sensor::Sensor(uint8_t address, TwoWire wire, SerialStream sout, unsigned int throttle_time) : _wire(wire), + _sout(sout), _address(address), _throttle_enabled(true), _throttle_time(throttle_time), _last_time(0), _status(SensorStatus::OK), _accel_to_g_force(RAW_TO_G_FACTOR), - _ang_rate_to_dps(RAW_TO_DPS_FACTOR), - _sout(sout) + _ang_rate_to_dps(RAW_TO_DPS_FACTOR) { } @@ -241,52 +239,52 @@ bool Sensor::setGyroSensitivity(uint8_t sensitivity) return true; } -double Sensor::getAccelX() +double Sensor::getAccelX() const { return _accel_raw_x; } -double Sensor::getAccelY() +double Sensor::getAccelY() const { return _accel_raw_y; } -double Sensor::getAccelZ() +double Sensor::getAccelZ() const { return _accel_raw_z; } -double Sensor::getAngleX() +double Sensor::getAngleX() const { return _accel_x; } -double Sensor::getAngleY() +double Sensor::getAngleY() const { return _accel_y; } -double Sensor::getGyroX() +double Sensor::getGyroX() const { return _gyro_raw_x; } -double Sensor::getGyroY() +double Sensor::getGyroY() const { return _gyro_raw_y; } -double Sensor::getGyroZ() +double Sensor::getGyroZ() const { return _gyro_raw_z; } -double Sensor::getPitch() +double Sensor::getPitch() const { return _pitch; } -double Sensor::getRoll() +double Sensor::getRoll() const { return _roll; } diff --git a/src/sensor/sensor.hpp b/src/sensor/sensor.hpp index 3e56627..7b76ae2 100644 --- a/src/sensor/sensor.hpp +++ b/src/sensor/sensor.hpp @@ -1,7 +1,7 @@ #pragma once -#include "utils/serial.hpp" -#include "utils/time.hpp" +#include "serial.hpp" +#include "std/time.hpp" #include #include @@ -77,52 +77,52 @@ public: /** * Returns the current X axis acceleration in g:s (g-force). */ - double getAccelX(); + double getAccelX() const; /** * Returns the current Y axis acceleration in g:s (g-force). */ - double getAccelY(); + double getAccelY() const; /** * Returns the current Z axis acceleration in g:s (g-force). */ - double getAccelZ(); + double getAccelZ() const; /** * Returns the current X angle. */ - double getAngleX(); + double getAngleX() const; /** * Returns the current Y angle. */ - double getAngleY(); + double getAngleY() const; /** * Returns the current X axis in degrees/s. */ - double getGyroX(); + double getGyroX() const; /** * Returns the current Y axis in degrees/s. */ - double getGyroY(); + double getGyroY() const; /** * Returns the current Z axis in degrees/s. */ - double getGyroZ(); + double getGyroZ() const; /** * Returns the current pitch angle. */ - double getPitch(); + double getPitch() const; /** * Returns the current roll angle. */ - double getRoll(); + double getRoll() const; /** * Sets the value of a key in the sensor's register. @@ -157,6 +157,7 @@ public: private: TwoWire _wire; + SerialStream _sout; uint8_t _address; @@ -193,6 +194,4 @@ private: double _yaw = 0; int16_t _readHighLow(); - - SerialStream _sout; }; -- cgit v1.2.3-18-g5258