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/calibration.cpp | 107 ---------------------------------------------------- 1 file changed, 107 deletions(-) delete mode 100644 src/calibration.cpp (limited to 'src/calibration.cpp') diff --git a/src/calibration.cpp b/src/calibration.cpp deleted file mode 100644 index 66a52bf..0000000 --- a/src/calibration.cpp +++ /dev/null @@ -1,107 +0,0 @@ -#include "calibration.hpp" - -#include "utils/general.hpp" -#include "utils/time.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() -{ - 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; -} -- cgit v1.2.3-18-g5258