refactor: implement & use shared ptr

1 files changed, 6 insertions, 47 deletions

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);
 }