From 757a29d0137b974fff6ddcc945d76e69ae120ecb Mon Sep 17 00:00:00 2001 From: HampusM Date: Mon, 21 Mar 2022 13:00:36 +0100 Subject: refactor: use MPU6050_light & clean up bloat --- src/sensor/calibration.cpp | 96 ---------------------------------------------- 1 file changed, 96 deletions(-) delete mode 100644 src/sensor/calibration.cpp (limited to 'src/sensor/calibration.cpp') diff --git a/src/sensor/calibration.cpp b/src/sensor/calibration.cpp deleted file mode 100644 index 5626388..0000000 --- a/src/sensor/calibration.cpp +++ /dev/null @@ -1,96 +0,0 @@ -#include "calibration.hpp" - -#include "common/time.hpp" -#include "utils.hpp" - -SensorCalibrator::SensorCalibrator(common::SharedPtr sensor, - common::SharedPtr sout) - : _sensor(sensor), _sout(sout) -{ -} - -bool SensorCalibrator::calibrate(unsigned int throttle_time) -{ - bool done = false; - auto start_time = common::time_now(); - - while (!done) - { - if (common::time_now().diff(start_time).millisecs() >= CALIBRATION_TIMEOUT) - { - return false; - } - - delay(throttle_time); - - auto values = SensorCalibratorValues(); - - for (unsigned int i = 0U; i < SENSOR_READ_CNT; i++) - { - _updateValues(values); - } - - _adjustValues(values); - - *_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(values)) - { - done = true; - } - - _adjustCalibrationWithValues(values); - } - - return true; -} - -void SensorCalibrator::_updateValues(SensorCalibratorValues &values) -{ - _sensor->read(); - - values.accel_x -= _sensor->getAccelX(); - values.accel_y -= _sensor->getAccelY(); - values.accel_z -= _sensor->getAccelZ(); - - values.gyro_x -= _sensor->getGyroX(); - values.gyro_y -= _sensor->getGyroY(); - values.gyro_z -= _sensor->getGyroZ(); -} - -void SensorCalibrator::_adjustCalibrationWithValues(const SensorCalibratorValues &values) -{ - _sensor->accel_cal_x += values.accel_x; - _sensor->accel_cal_y += values.accel_y; - _sensor->accel_cal_z += values.accel_z; - - _sensor->gyro_cal_x += values.gyro_x; - _sensor->gyro_cal_y += values.gyro_y; - _sensor->gyro_cal_z += values.gyro_z; -} - -void SensorCalibrator::_adjustValues(SensorCalibratorValues &values) -{ - 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; -} - -bool SensorCalibrator::_isValuesInRange(const SensorCalibratorValues &values) -{ - 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); -} -- cgit v1.2.3-18-g5258