refactor: clean sewage

1 files changed, 80 insertions, 94 deletions

diff --git a/src/sensor/sensor.cpp b/src/sensor/sensor.cpp
index e11a74b..a3b9a5c 100644
--- a/src/sensor/sensor.cpp
+++ b/src/sensor/sensor.cpp
@@ -1,50 +1,34 @@
 #include "sensor.hpp"
-#include "../utils/serial.hpp"
-#include "../utils/time.hpp"
+
 #include "registers.hpp"
+#include "utils/serial.hpp"
+#include "utils/time.hpp"
 
-Sensor::Sensor(uint8_t address, TwoWire *wire, SerialStream sout,
+Sensor::Sensor(uint8_t address, TwoWire wire, SerialStream sout,
 			   unsigned int throttle_time)
-	: _last_time(0), _sout(sout)
+	: _wire(wire),
+	  _address(address),
+	  _throttle_enabled(true),
+	  _throttle_time(throttle_time),
+	  _last_time(0),
+	  _status(SensorStatus::OK),
+	  _accel_to_g_force(RAW_TO_G_FACTOR),
+	  _ang_rate_to_dps(RAW_TO_DPS_FACTOR),
+	  _sout(sout)
 {
-	_address = address;
-	_wire = wire;
-
-	// Set default values
-	accel_cal_x = 0;
-	accel_cal_y = 0;
-	accel_cal_z = 0;
-
-	gyro_cal_x = 0;
-	gyro_cal_y = 0;
-	gyro_cal_z = 0;
-
-	_throttle_enabled = true;
-	_throttle_time = throttle_time;
-
-	_status = SensorStatus::OK;
-
-	_accel_sensitivity = 0;
-	_accel_to_g_force = 1.0 / 16384.0;
-
-	_gyro_sensitivity = 0;
-	_ang_rate_to_dps = 1.0 / 131.0;
-
-	// Reset the sensor
-	reset();
 }
 
 bool Sensor::begin()
 {
-	_wire->begin();
+	_wire.begin();
 
 	if (isConnected())
 	{
-		_wire->beginTransmission(_address);
-		_wire->write(SensorRegisters::PWR_MGMT_1);
-		_wire->write(SENSOR_WAKEUP);
+		_wire.beginTransmission(_address);
+		_wire.write(SensorRegisters::PWR_MGMT_1);
+		_wire.write(SENSOR_WAKEUP);
 
-		return (_wire->endTransmission() == 0);
+		return (_wire.endTransmission() == 0);
 	}
 
 	return false;
@@ -52,31 +36,13 @@ bool Sensor::begin()
 
 bool Sensor::isConnected()
 {
-	_wire->beginTransmission(_address);
-	return (_wire->endTransmission() == 0);
-}
-
-void Sensor::reset()
-{
-	_accel_raw_x = 0;
-	_accel_raw_y = 0;
-	_accel_raw_z = 0;
-
-	_gyro_raw_x = 0;
-	_gyro_raw_y = 0;
-	_gyro_raw_z = 0;
-
-	_accel_x = 0;
-	_accel_y = 0;
-
-	_pitch = 0;
-	_roll = 0;
-	_yaw = 0;
+	_wire.beginTransmission(_address);
+	return (_wire.endTransmission() == 0);
 }
 
 bool Sensor::read()
 {
-	Time now = time_now();
+	auto now = time_now();
 
 	if (_throttle_enabled && now.diff(_last_time).millisecs() < _throttle_time)
 	{
@@ -84,18 +50,20 @@ bool Sensor::read()
 		return false;
 	}
 
-	_wire->beginTransmission(_address);
-	_wire->write(SensorRegisters::ACCEL_XOUT_H);
+	_wire.beginTransmission(_address);
+	_wire.write(SensorRegisters::ACCEL_XOUT_H);
 
-	if (_wire->endTransmission() != 0)
+	if (_wire.endTransmission() != 0U)
 	{
 		_status = SensorStatus::ERR_WRITE;
 		return false;
 	}
 
-	int8_t response_length = _wire->requestFrom(_address, (uint8_t)14);
+	uint8_t response_length = _wire.requestFrom(_address, SensorRegisters::SELF_TEST_Y);
+
+	const uint8_t self_test_success = 14U;
 
-	if (response_length != 14)
+	if (response_length != self_test_success)
 	{
 		_status = SensorStatus::ERR_READ;
 		return false;
@@ -123,7 +91,7 @@ bool Sensor::read()
 
 	// Duration interval
 	now.update();
-	float duration = now.diff(_last_time).secs();
+	auto duration = now.diff(_last_time).secs();
 	_last_time = now;
 
 	// Convert raw acceleration to g:s (g-force)
@@ -147,12 +115,13 @@ bool Sensor::read()
 		  << endl;
 
 	// Prepare for Pitch Roll Yaw
-	float accel_y_pow_two = pow(_accel_raw_y, 2);
-	float accel_z_pow_two = pow(_accel_raw_z, 2);
+	auto accel_y_pow_two = pow(_accel_raw_y, 2);
+	auto accel_z_pow_two = pow(_accel_raw_z, 2);
 
-	_accel_x = atan2(_accel_raw_y, _accel_raw_z) * 180 / PI;
+	_accel_x = atan2(_accel_raw_y, _accel_raw_z) * ONE_EIGHTY / PI;
 
-	_accel_y = atan2(-_accel_raw_x, sqrt(accel_y_pow_two + accel_z_pow_two)) * 180 / PI;
+	_accel_y =
+		atan2(-_accel_raw_x, sqrt(accel_y_pow_two + accel_z_pow_two)) * ONE_EIGHTY / PI;
 
 	/*
 	_accel_x =
@@ -191,8 +160,11 @@ bool Sensor::read()
 		  << "Gyro raw z w/o time: " << _gyro_raw_z << "\n"
 		  << endl;
 
-	_pitch = 0.96 * _gyro_y + 0.04 * _accel_y;
-	_roll = 0.96 * _gyro_x + 0.04 * _accel_x;
+	const float gyro_balance = 0.96F;
+	const float accel_balance = 0.04F;
+
+	_pitch = gyro_balance * _gyro_y + accel_balance * _accel_y;
+	_roll = gyro_balance * _gyro_x + accel_balance * _accel_x;
 	_yaw = _gyro_z;
 
 	return true;
@@ -201,23 +173,31 @@ bool Sensor::read()
 bool Sensor::setAccelSensitivity(uint8_t sensitivity)
 {
 	if (sensitivity > 3)
+	{
 		sensitivity = 3;
+	}
 
 	_accel_sensitivity = sensitivity;
 
 	uint8_t accel_config = getRegister(SensorRegisters::ACCEL_CONFIG);
 
 	if (_status != SensorStatus::OK)
+	{
 		return false;
+	}
 
 	// No need to write same value
 	if (((accel_config >> 3) & 3) != _accel_sensitivity)
 	{
-		accel_config &= 0xE7;
+		const uint8_t magic = 0xE7U;
+
+		accel_config &= magic;
 		accel_config |= (_accel_sensitivity << 3);
 
 		if (!setRegister(SensorRegisters::ACCEL_CONFIG, accel_config))
+		{
 			return false;
+		}
 	}
 
 	// Calculate conversion factor.
@@ -229,7 +209,9 @@ bool Sensor::setAccelSensitivity(uint8_t sensitivity)
 bool Sensor::setGyroSensitivity(uint8_t sensitivity)
 {
 	if (sensitivity > 3)
+	{
 		sensitivity = 3;
+	}
 
 	_gyro_sensitivity = sensitivity;
 
@@ -243,7 +225,9 @@ bool Sensor::setGyroSensitivity(uint8_t sensitivity)
 	// No need to write same value
 	if (((gyro_config >> 3) & 3) != _gyro_sensitivity)
 	{
-		gyro_config &= 0xE7;
+		const uint8_t magic = 0xE7U;
+
+		gyro_config &= magic;
 		gyro_config |= (_gyro_sensitivity << 3);
 
 		if (!setRegister(SensorRegisters::GYRO_CONFIG, gyro_config))
@@ -257,63 +241,63 @@ bool Sensor::setGyroSensitivity(uint8_t sensitivity)
 	return true;
 }
 
-float Sensor::getAccelX()
+double Sensor::getAccelX()
 {
 	return _accel_raw_x;
 }
 
-float Sensor::getAccelY()
+double Sensor::getAccelY()
 {
 	return _accel_raw_y;
 }
 
-float Sensor::getAccelZ()
+double Sensor::getAccelZ()
 {
 	return _accel_raw_z;
 }
 
-float Sensor::getAngleX()
+double Sensor::getAngleX()
 {
 	return _accel_x;
 }
 
-float Sensor::getAngleY()
+double Sensor::getAngleY()
 {
 	return _accel_y;
 }
 
-float Sensor::getGyroX()
+double Sensor::getGyroX()
 {
 	return _gyro_raw_x;
 }
 
-float Sensor::getGyroY()
+double Sensor::getGyroY()
 {
 	return _gyro_raw_y;
 }
 
-float Sensor::getGyroZ()
+double Sensor::getGyroZ()
 {
 	return _gyro_raw_z;
 }
 
-float Sensor::getPitch()
+double Sensor::getPitch()
 {
 	return _pitch;
 }
 
-float Sensor::getRoll()
+double Sensor::getRoll()
 {
 	return _roll;
 }
 
 bool Sensor::setRegister(uint8_t reg, uint8_t value)
 {
-	_wire->beginTransmission(_address);
-	_wire->write(reg);
-	_wire->write(value);
+	_wire.beginTransmission(_address);
+	_wire.write(reg);
+	_wire.write(value);
 
-	if (_wire->endTransmission() != 0)
+	if (_wire.endTransmission() != 0)
 	{
 		_status = SensorStatus::ERR_WRITE;
 		return false;
@@ -324,23 +308,23 @@ bool Sensor::setRegister(uint8_t reg, uint8_t value)
 
 uint8_t Sensor::getRegister(uint8_t reg)
 {
-	_wire->beginTransmission(_address);
-	_wire->write(reg);
+	_wire.beginTransmission(_address);
+	_wire.write(reg);
 
-	if (_wire->endTransmission() != 0)
+	if (_wire.endTransmission() != 0U)
 	{
 		_status = SensorStatus::ERR_WRITE;
-		return 0;
+		return 0U;
 	}
 
-	uint8_t response_length = _wire->requestFrom(_address, (uint8_t)1);
-	if (response_length != 1)
+	uint8_t response_length = _wire.requestFrom(_address, 1U);
+	if (response_length != 1U)
 	{
 		_status = SensorStatus::ERR_READ;
-		return 0;
+		return 0U;
 	}
 
-	return _wire->read();
+	return _wire.read();
 }
 
 SensorStatus Sensor::getStatus()
@@ -353,8 +337,10 @@ SensorStatus Sensor::getStatus()
 
 int16_t Sensor::_readHighLow()
 {
-	int16_t high = _wire->read();
-	int16_t low = _wire->read();
+	const int16_t high = _wire.read();
+	const int16_t low = _wire.read();
+
+	const int8_t bits_in_byte = 8;
 
-	return high << 8 | low;
+	return high << bits_in_byte | low;
 }