refactor: implement & use shared ptr

7 files changed, 282 insertions, 125 deletions

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 <stddef.h>

+

+namespace common

+{

+template <class Target>

+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 <typename Target, typename... Args>

+SharedPtr<Target> 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 <stdlib.h>

+

+namespace common

+{

+

+template <class Target>

+SharedPtr<Target>::SharedPtr() noexcept

+	: _reference_cnt(malloc_s<unsigned int>(sizeof(unsigned int)))

+{

+	(*_reference_cnt) = 0;

+}

+

+template <class Target>

+SharedPtr<Target>::SharedPtr(nullptr_t) noexcept

+	: _reference_cnt(malloc_s<unsigned int>(sizeof(unsigned int)))

+{

+	(*_reference_cnt) = 0;

+}

+

+template <class Target>

+SharedPtr<Target>::SharedPtr(Target *target) noexcept

+	: _target(target), _reference_cnt(malloc_s<unsigned int>(sizeof(unsigned int)))

+{

+	(*_reference_cnt) = 0;

+}

+

+/**

+ * Copy constructor

+ */

+template <class Target>

+SharedPtr<Target>::SharedPtr(const SharedPtr &shared_ptr) noexcept

+	: _target(shared_ptr._target), _reference_cnt(shared_ptr._reference_cnt)

+{

+	(*_reference_cnt)++;

+}

+

+/**

+ * Move constructor

+ */

+template <class Target>

+SharedPtr<Target>::SharedPtr(SharedPtr &&shared_ptr) noexcept

+	: _target(shared_ptr._target), _reference_cnt(shared_ptr._reference_cnt)

+{

+	shared_ptr._target = nullptr;

+}

+

+template <class Target>

+SharedPtr<Target>::~SharedPtr() noexcept

+{

+	if ((*_reference_cnt) != 0U)

+	{

+		(*_reference_cnt)--;

+	}

+

+	if ((*_reference_cnt) == 0U)

+	{

+		delete _target;

+		free(_reference_cnt);

+		_reference_cnt = nullptr;

+	}

+}

+

+template <class Target>

+unsigned int SharedPtr<Target>::reference_cnt() const noexcept

+{

+	if (_reference_cnt == nullptr)

+	{

+		return 0;

+	}

+

+	return *_reference_cnt;

+}

+

+template <class Target>

+bool SharedPtr<Target>::is_disposable() const noexcept

+{

+	return _reference_cnt == nullptr || (*_reference_cnt) == 0U;

+}

+

+/**

+ * Copy assignment operator

+ */

+template <class Target>

+SharedPtr<Target> &SharedPtr<Target>::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 <class Target>

+SharedPtr<Target> &SharedPtr<Target>::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 <class Target>

+Target &SharedPtr<Target>::operator*() const noexcept

+{

+	return *(_target);

+}

+

+template <class Target>

+Target *SharedPtr<Target>::operator->() const noexcept

+{

+	return _target;

+}

+

+template <class Target, typename... Args>

+SharedPtr<Target> make_shared(Args &&...args) noexcept

+{

+	return SharedPtr<Target>(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 <Arduino.h>
 #include <Wire.h>
 #include <XInput.h>
 
 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<SerialStream>(Serial, BAUD_RATE);
+
+	auto sensor = common::make_shared<Sensor>(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<int>(sensor.getStatus()) << endl;
+		*sout << "Error: Failed to set the sensor's accelerometer sensitivity. Status: "
+			  << static_cast<int>(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<int>(sensor.getStatus()) << endl;
+		*sout << "Error: Failed to set the sensor's gyro sensitivity. Status: "
+			  << static_cast<int>(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<int>(status) << endl;
+			stop();
 		}
 
-		sout << "Error: Failed to read sensor. Status: " << static_cast<int>(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<int32_t>(sensor.getRoll()),
-					   static_cast<int32_t>(sensor.getPitch()));
-	XInput.printDebug(Serial);
+		XInput.setJoystick(JOY_LEFT, static_cast<int32_t>(sensor->getRoll()),
+						   static_cast<int32_t>(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> sensor,
+								   common::SharedPtr<SerialStream> 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> sensor,
+					 common::SharedPtr<SerialStream> 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> _sensor;
+	common::SharedPtr<SerialStream> _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<float>(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<float>(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<int16_t>(_wire.read());
-	const int16_t low = static_cast<int16_t>(_wire.read());
+	const auto high = static_cast<int16_t>(_wire.read());
+	const auto low = static_cast<int16_t>(_wire.read());
 
 	const int8_t bits_in_byte = 8;
 
-	return high << bits_in_byte | low;
+	return static_cast<int16_t>(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 <Arduino.h>
 #include <Wire.h>
@@ -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;