refactor: make misc improvements & update calibration precision

3 files changed, 33 insertions, 39 deletions

diff --git a/src/sensor/calibration.hpp b/src/sensor/calibration.hpp
index 585d655..17f8fa9 100644
--- a/src/sensor/calibration.hpp
+++ b/src/sensor/calibration.hpp
@@ -4,28 +4,28 @@
 #include "serial.hpp"
 
 // Calibration precision
-constexpr float ACCEL_CAL_X_MAX = 0.006;
-constexpr float ACCEL_CAL_X_MIN = -0.006;
+constexpr double ACCEL_CAL_X_MAX = 0.003;
+constexpr double ACCEL_CAL_X_MIN = -0.003;
 
-constexpr float ACCEL_CAL_Y_MAX = 0.006;
-constexpr float ACCEL_CAL_Y_MIN = -0.006;
+constexpr double ACCEL_CAL_Y_MAX = 0.003;
+constexpr double ACCEL_CAL_Y_MIN = -0.003;
 
-constexpr float ACCEL_CAL_Z_MAX = 0.006;
-constexpr float ACCEL_CAL_Z_MIN = -0.006;
+constexpr double ACCEL_CAL_Z_MAX = 0.003;
+constexpr double ACCEL_CAL_Z_MIN = -0.003;
 
-constexpr float GYRO_CAL_X_MAX = 0.06;
-constexpr float GYRO_CAL_X_MIN = -0.06;
+constexpr double GYRO_CAL_X_MAX = 0.003;
+constexpr double GYRO_CAL_X_MIN = -0.003;
 
-constexpr float GYRO_CAL_Y_MAX = 0.06;
-constexpr float GYRO_CAL_Y_MIN = -0.06;
+constexpr double GYRO_CAL_Y_MAX = 0.05;
+constexpr double GYRO_CAL_Y_MIN = -0.05;
 
-constexpr float GYRO_CAL_Z_MAX = 0.06;
-constexpr float GYRO_CAL_Z_MIN = -0.06;
+constexpr double GYRO_CAL_Z_MAX = 0.04;
+constexpr double GYRO_CAL_Z_MIN = -0.04;
 
-constexpr uint32_t CALIBRATION_TIMEOUT = 120000; // Milliseconds
+constexpr uint32_t CALIBRATION_TIMEOUT = 120000; // milliseconds
 
-constexpr unsigned int SENSOR_READ_CNT = 20;
-constexpr float SENSOR_VAL_ADJUST = 0.05;
+constexpr uint32_t SENSOR_READ_CNT = 20;
+constexpr double SENSOR_VAL_ADJUST = 0.05;
 
 class SensorCalibratorValues
 {
diff --git a/src/sensor/sensor.cpp b/src/sensor/sensor.cpp
index 9f48e7f..4d7f256 100644
--- a/src/sensor/sensor.cpp
+++ b/src/sensor/sensor.cpp
@@ -121,13 +121,9 @@ bool Sensor::read() noexcept
 	_accel_y =
 		atan2(-_accel_raw_x, sqrt(accel_y_pow_two + accel_z_pow_two)) * ONE_EIGHTY / PI;
 
-	/*
-	_accel_x =
-		atan(_accel_raw_y / sqrt(accel_x_pow_two + accel_z_pow_two)) * RAD_TO_DEGREES;
-
-	_accel_y = atan(-1.0 * _accel_raw_x / sqrt(accel_y_pow_two + accel_z_pow_two)) *
-			   RAD_TO_DEGREES;
-	*/
+	_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;
@@ -153,16 +149,14 @@ bool Sensor::read() noexcept
 	_gyro_y += _gyro_raw_y * duration;
 	_gyro_z += _gyro_raw_z * duration;
 
-	_sout << "\nGyro raw x w/o time: " << _gyro_raw_x << "\n"
-		  << "Gyro raw y w/o time: " << _gyro_raw_y << "\n"
-		  << "Gyro raw z w/o time: " << _gyro_raw_z << "\n"
+	_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;
 
-	const float gyro_balance = 0.96F;
-	const float accel_balance = 0.04F;
+	_pitch = _gyro_y + _accel_y;
+	_roll = _gyro_x + _accel_x;
 
-	_pitch = gyro_balance * _gyro_y + accel_balance * _accel_y;
-	_roll = gyro_balance * _gyro_x + accel_balance * _accel_x;
 	_yaw = _gyro_z;
 
 	return true;
@@ -296,7 +290,7 @@ bool Sensor::setRegister(uint8_t reg, uint8_t value) noexcept
 	_wire.write(reg);
 	_wire.write(value);
 
-	if (_wire.endTransmission() != 0)
+	if (_wire.endTransmission() != 0U)
 	{
 		_status = SensorStatus::ERR_WRITE;
 		return false;
@@ -336,8 +330,8 @@ SensorStatus Sensor::getStatus() noexcept
 
 int16_t Sensor::_readHighLow() noexcept
 {
-	const int16_t high = _wire.read();
-	const int16_t low = _wire.read();
+	const int16_t high = static_cast<int16_t>(_wire.read());
+	const int16_t low = static_cast<int16_t>(_wire.read());
 
 	const int8_t bits_in_byte = 8;
 
diff --git a/src/sensor/sensor.hpp b/src/sensor/sensor.hpp
index 5bdf6ca..b79b918 100644
--- a/src/sensor/sensor.hpp
+++ b/src/sensor/sensor.hpp
@@ -11,7 +11,7 @@ constexpr uint8_t SENSOR_WAKEUP = 0x00U;
 constexpr float RAW_TO_DPS_FACTOR = 1.0 / 131.0;
 constexpr float RAW_TO_G_FACTOR = 1.0 / 16384.0;
 
-constexpr unsigned int ONE_EIGHTY = 180;
+constexpr uint32_t ONE_EIGHTY = 180;
 
 enum class SensorStatus
 {
@@ -146,14 +146,14 @@ public:
 	SensorStatus getStatus() noexcept;
 
 	// Accelerometer calibration values
-	double accel_cal_x = 0.0F;
-	double accel_cal_y = 0.0F;
-	double accel_cal_z = 0.0F;
+	double accel_cal_x = 0;
+	double accel_cal_y = 0;
+	double accel_cal_z = 0;
 
 	// Gyroscope calibration values
-	double gyro_cal_x = 0.0F;
-	double gyro_cal_y = 0.0F;
-	double gyro_cal_z = 0.0F;
+	double gyro_cal_x = 0;
+	double gyro_cal_y = 0;
+	double gyro_cal_z = 0;
 
 private:
 	TwoWire _wire;