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/common/memory/shared_ptr.hpp | 44 +++++++++++ src/common/memory/shared_ptr.tpp | 153 +++++++++++++++++++++++++++++++++++++++ src/gyronardo.cpp | 102 +++++++++++++------------- src/sensor/calibration.cpp | 41 ++++++----- src/sensor/calibration.hpp | 9 ++- src/sensor/sensor.cpp | 53 ++------------ src/sensor/sensor.hpp | 5 +- 7 files changed, 282 insertions(+), 125 deletions(-) create mode 100644 src/common/memory/shared_ptr.hpp create mode 100644 src/common/memory/shared_ptr.tpp (limited to 'src') diff --git a/src/common/memory/shared_ptr.hpp b/src/common/memory/shared_ptr.hpp new file mode 100644 index 0000000..7e8a910 --- /dev/null +++ b/src/common/memory/shared_ptr.hpp @@ -0,0 +1,44 @@ +#pragma once + +#include + +namespace common +{ +template +class SharedPtr +{ +public: + SharedPtr() noexcept; + SharedPtr(nullptr_t) noexcept; // NOLINT(google-explicit-constructor) + + explicit SharedPtr(Target *target) noexcept; + + SharedPtr(const SharedPtr &shared_ptr) noexcept; + + SharedPtr(SharedPtr &&shared_ptr) noexcept; + + ~SharedPtr() noexcept; + + [[nodiscard]] unsigned int reference_cnt() const noexcept; + + [[nodiscard]] bool is_disposable() const noexcept; + + SharedPtr &operator=(const SharedPtr &rhs) noexcept; + + SharedPtr &operator=(SharedPtr &&rhs) noexcept; + + Target &operator*() const noexcept; + Target *operator->() const noexcept; + +private: + Target *_target = nullptr; + + unsigned int *_reference_cnt; +}; + +template +SharedPtr make_shared(Args &&...args) noexcept; + +} // namespace common + +#include "shared_ptr.tpp" diff --git a/src/common/memory/shared_ptr.tpp b/src/common/memory/shared_ptr.tpp new file mode 100644 index 0000000..d9b57c3 --- /dev/null +++ b/src/common/memory/shared_ptr.tpp @@ -0,0 +1,153 @@ +#pragma once + +#include "shared_ptr.hpp" + +#include "common/memory.hpp" + +#include + +namespace common +{ + +template +SharedPtr::SharedPtr() noexcept + : _reference_cnt(malloc_s(sizeof(unsigned int))) +{ + (*_reference_cnt) = 0; +} + +template +SharedPtr::SharedPtr(nullptr_t) noexcept + : _reference_cnt(malloc_s(sizeof(unsigned int))) +{ + (*_reference_cnt) = 0; +} + +template +SharedPtr::SharedPtr(Target *target) noexcept + : _target(target), _reference_cnt(malloc_s(sizeof(unsigned int))) +{ + (*_reference_cnt) = 0; +} + +/** + * Copy constructor + */ +template +SharedPtr::SharedPtr(const SharedPtr &shared_ptr) noexcept + : _target(shared_ptr._target), _reference_cnt(shared_ptr._reference_cnt) +{ + (*_reference_cnt)++; +} + +/** + * Move constructor + */ +template +SharedPtr::SharedPtr(SharedPtr &&shared_ptr) noexcept + : _target(shared_ptr._target), _reference_cnt(shared_ptr._reference_cnt) +{ + shared_ptr._target = nullptr; +} + +template +SharedPtr::~SharedPtr() noexcept +{ + if ((*_reference_cnt) != 0U) + { + (*_reference_cnt)--; + } + + if ((*_reference_cnt) == 0U) + { + delete _target; + free(_reference_cnt); + _reference_cnt = nullptr; + } +} + +template +unsigned int SharedPtr::reference_cnt() const noexcept +{ + if (_reference_cnt == nullptr) + { + return 0; + } + + return *_reference_cnt; +} + +template +bool SharedPtr::is_disposable() const noexcept +{ + return _reference_cnt == nullptr || (*_reference_cnt) == 0U; +} + +/** + * Copy assignment operator + */ +template +SharedPtr &SharedPtr::operator=(const SharedPtr &rhs) noexcept +{ + if (&rhs != this) + { + if (is_disposable()) + { + delete _target; + free(_reference_cnt); + } + + _target = nullptr; + _target = rhs._target; + + _reference_cnt = nullptr; + _reference_cnt = rhs._reference_cnt; + (*_reference_cnt)++; + } + + return *this; +} + +/** + * Move assignment operator + */ +template +SharedPtr &SharedPtr::operator=(SharedPtr &&rhs) noexcept +{ + if (&rhs != this) + { + if (is_disposable()) + { + delete _target; + free(_reference_cnt); + } + + _target = rhs._target; + rhs._target = nullptr; + + _reference_cnt = rhs._reference_cnt; + rhs._reference_cnt = nullptr; + } + + return *this; +} + +template +Target &SharedPtr::operator*() const noexcept +{ + return *(_target); +} + +template +Target *SharedPtr::operator->() const noexcept +{ + return _target; +} + +template +SharedPtr make_shared(Args &&...args) noexcept +{ + return SharedPtr(new Target(args...)); +} + +} // namespace common diff --git a/src/gyronardo.cpp b/src/gyronardo.cpp index d05b3ce..bee13b6 100644 --- a/src/gyronardo.cpp +++ b/src/gyronardo.cpp @@ -1,100 +1,102 @@ +#include "common/memory/shared_ptr.hpp" #include "sensor/calibration.hpp" #include "sensor/sensor.hpp" #include "serial.hpp" #include "utils.hpp" -#include #include #include constexpr uint8_t SENSOR_ADDRESS = 0x68U; -constexpr uint32_t SENSOR_THROTTLE_TIME = 50U; // milliseconds -constexpr uint32_t SENSOR_RETRY_TIME = 2000U; // milliseconds +constexpr unsigned int SENSOR_THROTTLE_TIME = 50U; // milliseconds +constexpr unsigned int SENSOR_RETRY_TIME = 2000U; // milliseconds -constexpr uint32_t BAUD_RATE = 9600U; - -auto sout = SerialStream(Serial_(), BAUD_RATE); - -auto sensor = Sensor(SENSOR_ADDRESS, Wire, sout, SENSOR_THROTTLE_TIME); +constexpr unsigned int BAUD_RATE = 9600U; void setup() { - sout.waitReady(); + auto sout = common::make_shared(Serial, BAUD_RATE); + + auto sensor = common::make_shared(SENSOR_ADDRESS, Wire, SENSOR_THROTTLE_TIME); - while (!sensor.begin()) + sout->waitReady(); + + while (!sensor->begin()) { - sout << "Error: Could not connect to the sensor. Retrying after 2000 " - "milliseconds..." - << endl; + *sout << "Error: Could not connect to the _sensor-> Retrying after 2000 " + "milliseconds..." + << endl; delay(SENSOR_RETRY_TIME); } - if (!sensor.setAccelSensitivity(2)) // 8g + if (!sensor->setAccelSensitivity(2)) // 8g { - sout << "Error: Failed to set the sensor's accelerometer sensitivity. Status: " - << static_cast(sensor.getStatus()) << endl; + *sout << "Error: Failed to set the sensor's accelerometer sensitivity. Status: " + << static_cast(sensor->getStatus()) << endl; stop(); } - if (!sensor.setGyroSensitivity(1)) // 500 degrees/s + if (!sensor->setGyroSensitivity(1)) // 500 degrees/s { - sout << "Error: Failed to set the sensor's gyro sensitivity. Status: " - << static_cast(sensor.getStatus()) << endl; + *sout << "Error: Failed to set the sensor's gyro sensitivity. Status: " + << static_cast(sensor->getStatus()) << endl; stop(); } - sout << "Calibrating sensor..." << endl; + *sout << "Calibrating _sensor->.." << endl; SensorCalibrator sensor_calibrator(sensor, sout); if (!sensor_calibrator.calibrate(SENSOR_THROTTLE_TIME)) { - sout << "Error: Sensor calibration timed out after " << CALIBRATION_TIMEOUT - << " milliseconds" << endl; + *sout << "Error: Sensor calibration timed out after " << CALIBRATION_TIMEOUT + << " milliseconds" << endl; stop(); } - sout << "Finished calibrating sensor\n"; + *sout << "Finished calibrating sensor\n"; - sout << "Calibration values:\n" - << "Accelerometer X: " << sensor.accel_cal_x << "\n" - << "Accelerometer Y: " << sensor.accel_cal_y << "\n" - << "Accelerometer Z: " << sensor.accel_cal_z << "\n" - << "Gyro X: " << sensor.gyro_cal_x << "\n" - << "Gyro Y: " << sensor.gyro_cal_y << "\n" - << "Gyro Z: " << sensor.gyro_cal_z << "\n"; + *sout << "Calibration values:\n" + << "Accelerometer X: " << sensor->accel_cal_x << "\n" + << "Accelerometer Y: " << sensor->accel_cal_y << "\n" + << "Accelerometer Z: " << sensor->accel_cal_z << "\n" + << "Gyro X: " << sensor->gyro_cal_x << "\n" + << "Gyro Y: " << sensor->gyro_cal_y << "\n" + << "Gyro Z: " << sensor->gyro_cal_z << "\n"; - sout << "Starting..." << endl; + *sout << "Starting..." << endl; XInput.begin(); -} -void loop() -{ - delay(SENSOR_THROTTLE_TIME); - - if (!sensor.read()) + while (true) { - SensorStatus status = sensor.getStatus(); + delay(SENSOR_THROTTLE_TIME); - if (status == SensorStatus::THROTTLED) + if (!sensor->read()) { - sout << "Warning: The sensor was read too frequently and throttled" << endl; - return; + SensorStatus status = sensor->getStatus(); + + if (status == SensorStatus::THROTTLED) + { + *sout << "Warning: The sensor was read too frequently and throttled" + << endl; + continue; + } + + *sout << "Error: Failed to read _sensor-> Status: " + << static_cast(status) << endl; + stop(); } - sout << "Error: Failed to read sensor. Status: " << static_cast(status) - << endl; - stop(); - } + *sout << "Pitch: " << sensor->getPitch() << " Roll: " << sensor->getRoll() + << endl; - sout << "Pitch: " << sensor.getPitch() << " Roll: " << sensor.getRoll() << endl; - - XInput.setJoystick(JOY_LEFT, static_cast(sensor.getRoll()), - static_cast(sensor.getPitch())); - XInput.printDebug(Serial); + XInput.setJoystick(JOY_LEFT, static_cast(sensor->getRoll()), + static_cast(sensor->getPitch())); + XInput.printDebug(Serial); + } } 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