From 5b6427dde0bb8e3b466793243bbfc185f4739ac6 Mon Sep 17 00:00:00 2001 From: HampusM Date: Mon, 14 Mar 2022 14:13:54 +0100 Subject: refactor: implement & use shared ptr --- src/sensor/calibration.cpp | 41 ++++++++++++++++++----------------- src/sensor/calibration.hpp | 9 ++++---- src/sensor/sensor.cpp | 53 ++++++---------------------------------------- src/sensor/sensor.hpp | 5 +---- 4 files changed, 33 insertions(+), 75 deletions(-) (limited to 'src/sensor') diff --git a/src/sensor/calibration.cpp b/src/sensor/calibration.cpp index 1a2b91e..5626388 100644 --- a/src/sensor/calibration.cpp +++ b/src/sensor/calibration.cpp @@ -3,7 +3,8 @@ #include "common/time.hpp" #include "utils.hpp" -SensorCalibrator::SensorCalibrator(Sensor &sensor, SerialStream sout) +SensorCalibrator::SensorCalibrator(common::SharedPtr sensor, + common::SharedPtr sout) : _sensor(sensor), _sout(sout) { } @@ -31,12 +32,12 @@ bool SensorCalibrator::calibrate(unsigned int throttle_time) _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; + *_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)) { @@ -51,26 +52,26 @@ bool SensorCalibrator::calibrate(unsigned int throttle_time) void SensorCalibrator::_updateValues(SensorCalibratorValues &values) { - _sensor.read(); + _sensor->read(); - values.accel_x -= _sensor.getAccelX(); - values.accel_y -= _sensor.getAccelY(); - values.accel_z -= _sensor.getAccelZ(); + 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(); + 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->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; + _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) diff --git a/src/sensor/calibration.hpp b/src/sensor/calibration.hpp index 17f8fa9..c85d336 100644 --- a/src/sensor/calibration.hpp +++ b/src/sensor/calibration.hpp @@ -1,5 +1,6 @@ #pragma once +#include "common/memory/shared_ptr.hpp" #include "sensor/sensor.hpp" #include "serial.hpp" @@ -51,7 +52,8 @@ public: * @param sensor A sensor to calibrate * @param sout A Serial output stream */ - SensorCalibrator(Sensor &sensor, SerialStream sout); + SensorCalibrator(common::SharedPtr sensor, + common::SharedPtr sout); /** * Calibrates the sensor. @@ -69,7 +71,6 @@ private: static void _adjustValues(SensorCalibratorValues &values); static bool _isValuesInRange(const SensorCalibratorValues &values); - Sensor &_sensor; - - SerialStream _sout; + common::SharedPtr _sensor; + common::SharedPtr _sout; }; diff --git a/src/sensor/sensor.cpp b/src/sensor/sensor.cpp index af25a96..a801270 100644 --- a/src/sensor/sensor.cpp +++ b/src/sensor/sensor.cpp @@ -2,10 +2,8 @@ #include "sensor/registers.hpp" -Sensor::Sensor(uint8_t address, TwoWire wire, SerialStream sout, - unsigned int throttle_time) +Sensor::Sensor(uint8_t address, TwoWire wire, unsigned int throttle_time) noexcept : _wire(wire), - _sout(sout), _address(address), _throttle_enabled(true), _throttle_time(throttle_time), @@ -72,21 +70,11 @@ bool Sensor::read() noexcept _accel_raw_y = _readHighLow(); _accel_raw_z = _readHighLow(); - _sout << "\nAccel raw x: " << _accel_raw_x << "\n" - << "Accel raw y: " << _accel_raw_y << "\n" - << "Accel raw z: " << _accel_raw_z << "\n" - << endl; - // Gyroscope _gyro_raw_x = _readHighLow(); _gyro_raw_y = _readHighLow(); _gyro_raw_z = _readHighLow(); - _sout << "\nGyro raw x: " << _gyro_raw_x << "\n" - << "Gyro raw y: " << _gyro_raw_y << "\n" - << "Gyro raw z: " << _gyro_raw_z << "\n" - << endl; - // Duration interval now.update(); auto duration = now.diff(_last_time).secs(); @@ -97,21 +85,11 @@ bool Sensor::read() noexcept _accel_raw_y *= _accel_to_g_force; _accel_raw_z *= _accel_to_g_force; - _sout << "\nAccel raw x g:s: " << _accel_raw_x << "\n" - << "Accel raw y g:s: " << _accel_raw_y << "\n" - << "Accel raw z g:s: " << _accel_raw_z << "\n" - << endl; - // Error correct raw acceleration _accel_raw_x += accel_cal_x; _accel_raw_y += accel_cal_y; _accel_raw_z += accel_cal_z; - _sout << "\nAccel raw x correct: " << _accel_raw_x << "\n" - << "Accel raw y correct: " << _accel_raw_y << "\n" - << "Accel raw z correct: " << _accel_raw_z << "\n" - << endl; - // Prepare for Pitch Roll Yaw auto accel_y_pow_two = pow(_accel_raw_y, 2); auto accel_z_pow_two = pow(_accel_raw_z, 2); @@ -121,39 +99,20 @@ bool Sensor::read() noexcept _accel_y = atan2(-_accel_raw_x, sqrt(accel_y_pow_two + accel_z_pow_two)) * ONE_EIGHTY / PI; - _sout << "\nAccel x prepared: " << _accel_x << "\n" - << "Accel y prepared: " << _accel_y << "\n" - << endl; - // Convert raw Gyro to degrees/s _gyro_raw_x *= _ang_rate_to_dps; _gyro_raw_y *= _ang_rate_to_dps; _gyro_raw_z *= _ang_rate_to_dps; - _sout << "\nGyro raw x dps: " << _gyro_raw_x << "\n" - << "Gyro raw y dps: " << _gyro_raw_y << "\n" - << "Gyro raw z dps: " << _gyro_raw_z << "\n" - << endl; - // Error correct raw gyro measurements. _gyro_raw_x += gyro_cal_x; _gyro_raw_y += gyro_cal_y; _gyro_raw_z += gyro_cal_z; - _sout << "\nGyro raw x correct: " << _gyro_raw_x << "\n" - << "Gyro raw y correct: " << _gyro_raw_y << "\n" - << "Gyro raw z correct: " << _gyro_raw_z << "\n" - << endl; - _gyro_x += _gyro_raw_x * duration; _gyro_y += _gyro_raw_y * duration; _gyro_z += _gyro_raw_z * duration; - _sout << "\nGyro x w/o time: " << _gyro_x << "\n" - << "Gyro y w/o time: " << _gyro_y << "\n" - << "Gyro z w/o time: " << _gyro_z << "\n" - << endl; - _pitch = _gyro_y + _accel_y; _roll = _gyro_x + _accel_x; @@ -194,7 +153,7 @@ bool Sensor::setAccelSensitivity(uint8_t sensitivity) noexcept } // Calculate conversion factor. - _accel_to_g_force = (1 << _accel_sensitivity) * RAW_TO_G_FACTOR; + _accel_to_g_force = static_cast(1 << _accel_sensitivity) * RAW_TO_G_FACTOR; return true; } @@ -229,7 +188,7 @@ bool Sensor::setGyroSensitivity(uint8_t sensitivity) noexcept } } - _ang_rate_to_dps = (1 << _gyro_sensitivity) * RAW_TO_DPS_FACTOR; + _ang_rate_to_dps = static_cast(1 << _gyro_sensitivity) * RAW_TO_DPS_FACTOR; return true; } @@ -330,10 +289,10 @@ SensorStatus Sensor::getStatus() noexcept int16_t Sensor::_readHighLow() noexcept { - const int16_t high = static_cast(_wire.read()); - const int16_t low = static_cast(_wire.read()); + const auto high = static_cast(_wire.read()); + const auto low = static_cast(_wire.read()); const int8_t bits_in_byte = 8; - return high << bits_in_byte | low; + return static_cast(high << bits_in_byte | low); } diff --git a/src/sensor/sensor.hpp b/src/sensor/sensor.hpp index 1e7e488..1b99f6e 100644 --- a/src/sensor/sensor.hpp +++ b/src/sensor/sensor.hpp @@ -1,7 +1,6 @@ #pragma once #include "common/time.hpp" -#include "serial.hpp" #include #include @@ -33,10 +32,9 @@ public: * * @param address The address of the sensor * @param wire A Wire instance - * @param sout A serial output stream * @param throttle_time A minumum time between sensor reads for the sensor to throttle */ - Sensor(uint8_t address, TwoWire wire, SerialStream sout, unsigned int throttle_time); + Sensor(uint8_t address, TwoWire wire, unsigned int throttle_time) noexcept; /** * Initializes communication with the sensor. @@ -157,7 +155,6 @@ public: private: TwoWire _wire; - SerialStream _sout; uint8_t _address; -- cgit v1.2.3-18-g5258