1 files changed, 0 insertions, 298 deletions
diff --git a/src/sensor/sensor.cpp b/src/sensor/sensor.cpp
deleted file mode 100644
index a801270..0000000
--- a/src/sensor/sensor.cpp
+++ /dev/null
@@ -1,298 +0,0 @@
-#include "sensor.hpp"
-
-#include "sensor/registers.hpp"
-
-Sensor::Sensor(uint8_t address, TwoWire wire, unsigned int throttle_time) noexcept
-	: _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)
-{
-}
-
-bool Sensor::begin() noexcept
-{
-	_wire.begin();
-
-	if (isConnected())
-	{
-		_wire.beginTransmission(_address);
-		_wire.write(SensorRegisters::PWR_MGMT_1);
-		_wire.write(SENSOR_WAKEUP);
-
-		return (_wire.endTransmission() == 0);
-	}
-
-	return false;
-}
-
-bool Sensor::isConnected() noexcept
-{
-	_wire.beginTransmission(_address);
-	return (_wire.endTransmission() == 0);
-}
-
-bool Sensor::read() noexcept
-{
-	auto now = common::time_now();
-
-	if (_throttle_enabled && now.diff(_last_time).millisecs() < _throttle_time)
-	{
-		_status = SensorStatus::THROTTLED;
-		return false;
-	}
-
-	_wire.beginTransmission(_address);
-	_wire.write(SensorRegisters::ACCEL_XOUT_H);
-
-	if (_wire.endTransmission() != 0U)
-	{
-		_status = SensorStatus::ERR_WRITE;
-		return false;
-	}
-
-	uint8_t response_length = _wire.requestFrom(_address, SensorRegisters::SELF_TEST_Y);
-
-	const uint8_t self_test_success = 14U;
-
-	if (response_length != self_test_success)
-	{
-		_status = SensorStatus::ERR_READ;
-		return false;
-	}
-
-	// Accelerometer
-	_accel_raw_x = _readHighLow();
-	_accel_raw_y = _readHighLow();
-	_accel_raw_z = _readHighLow();
-
-	// Gyroscope
-	_gyro_raw_x = _readHighLow();
-	_gyro_raw_y = _readHighLow();
-	_gyro_raw_z = _readHighLow();
-
-	// Duration interval
-	now.update();
-	auto duration = now.diff(_last_time).secs();
-	_last_time = now;
-
-	// Convert raw acceleration to g:s (g-force)
-	_accel_raw_x *= _accel_to_g_force;
-	_accel_raw_y *= _accel_to_g_force;
-	_accel_raw_z *= _accel_to_g_force;
-
-	// Error correct raw acceleration
-	_accel_raw_x += accel_cal_x;
-	_accel_raw_y += accel_cal_y;
-	_accel_raw_z += accel_cal_z;
-
-	// 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);
-
-	_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)) * ONE_EIGHTY / PI;
-
-	// 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;
-
-	// Error correct raw gyro measurements.
-	_gyro_raw_x += gyro_cal_x;
-	_gyro_raw_y += gyro_cal_y;
-	_gyro_raw_z += gyro_cal_z;
-
-	_gyro_x += _gyro_raw_x * duration;
-	_gyro_y += _gyro_raw_y * duration;
-	_gyro_z += _gyro_raw_z * duration;
-
-	_pitch = _gyro_y + _accel_y;
-	_roll = _gyro_x + _accel_x;
-
-	_yaw = _gyro_z;
-
-	return true;
-}
-
-bool Sensor::setAccelSensitivity(uint8_t sensitivity) noexcept
-{
-	if (sensitivity > 3)
-	{
-		sensitivity = 3;
-	}
-
-	_accel_sensitivity = sensitivity;
-
-	auto accel_config = static_cast<int>(getRegister(SensorRegisters::ACCEL_CONFIG));
-
-	if (_status != SensorStatus::OK)
-	{
-		return false;
-	}
-
-	// No need to write same value
-	if (((accel_config >> 3) & 3) != _accel_sensitivity)
-	{
-		const uint8_t magic = 0xE7U;
-
-		accel_config &= magic;
-		accel_config |= (_accel_sensitivity << 3U);
-
-		if (!setRegister(SensorRegisters::ACCEL_CONFIG,
-						 static_cast<uint8_t>(accel_config)))
-		{
-			return false;
-		}
-	}
-
-	// Calculate conversion factor.
-	_accel_to_g_force = static_cast<float>(1 << _accel_sensitivity) * RAW_TO_G_FACTOR;
-
-	return true;
-}
-
-bool Sensor::setGyroSensitivity(uint8_t sensitivity) noexcept
-{
-	if (sensitivity > 3)
-	{
-		sensitivity = 3;
-	}
-
-	_gyro_sensitivity = sensitivity;
-
-	auto gyro_config = static_cast<int>(getRegister(SensorRegisters::GYRO_CONFIG));
-
-	if (_status != SensorStatus::OK)
-	{
-		return false;
-	}
-
-	// No need to write same value
-	if (((gyro_config >> 3) & 3) != _gyro_sensitivity)
-	{
-		const uint8_t magic = 0xE7U;
-
-		gyro_config &= magic;
-		gyro_config |= (_gyro_sensitivity << 3U);
-
-		if (!setRegister(SensorRegisters::GYRO_CONFIG, static_cast<uint8_t>(gyro_config)))
-		{
-			return false;
-		}
-	}
-
-	_ang_rate_to_dps = static_cast<float>(1 << _gyro_sensitivity) * RAW_TO_DPS_FACTOR;
-
-	return true;
-}
-
-double Sensor::getAccelX() const noexcept
-{
-	return _accel_raw_x;
-}
-
-double Sensor::getAccelY() const noexcept
-{
-	return _accel_raw_y;
-}
-
-double Sensor::getAccelZ() const noexcept
-{
-	return _accel_raw_z;
-}
-
-double Sensor::getAngleX() const noexcept
-{
-	return _accel_x;
-}
-
-double Sensor::getAngleY() const noexcept
-{
-	return _accel_y;
-}
-
-double Sensor::getGyroX() const noexcept
-{
-	return _gyro_raw_x;
-}
-
-double Sensor::getGyroY() const noexcept
-{
-	return _gyro_raw_y;
-}
-
-double Sensor::getGyroZ() const noexcept
-{
-	return _gyro_raw_z;
-}
-
-double Sensor::getPitch() const noexcept
-{
-	return _pitch;
-}
-
-double Sensor::getRoll() const noexcept
-{
-	return _roll;
-}
-
-bool Sensor::setRegister(uint8_t reg, uint8_t value) noexcept
-{
-	_wire.beginTransmission(_address);
-	_wire.write(reg);
-	_wire.write(value);
-
-	if (_wire.endTransmission() != 0U)
-	{
-		_status = SensorStatus::ERR_WRITE;
-		return false;
-	}
-
-	return true;
-}
-
-uint8_t Sensor::getRegister(uint8_t reg) noexcept
-{
-	_wire.beginTransmission(_address);
-	_wire.write(reg);
-
-	if (_wire.endTransmission() != 0U)
-	{
-		_status = SensorStatus::ERR_WRITE;
-		return 0U;
-	}
-
-	uint8_t response_length = _wire.requestFrom(_address, 1U);
-	if (response_length != 1U)
-	{
-		_status = SensorStatus::ERR_READ;
-		return 0U;
-	}
-
-	return static_cast<uint8_t>(_wire.read());
-}
-
-SensorStatus Sensor::getStatus() noexcept
-{
-	SensorStatus status = _status;
-	_status = SensorStatus::OK;
-
-	return status;
-}
-
-int16_t Sensor::_readHighLow() noexcept
-{
-	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 static_cast<int16_t>(high << bits_in_byte | low);
-}