refactor: reorganize & add debugging

1 files changed, 0 insertions, 325 deletions

diff --git a/src/sensor.cpp b/src/sensor.cpp
deleted file mode 100644
index 9d4157e..0000000
--- a/src/sensor.cpp
+++ /dev/null
@@ -1,325 +0,0 @@
-#include "sensor.hpp"
-#include "math.h"
-#include "sensor_registers.hpp"
-#include "time_utils.hpp"
-
-Sensor::Sensor(uint8_t address, TwoWire *wire, unsigned int throttle_time) : _last_time(0)
-{
-	_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();
-
-	if (isConnected())
-	{
-		_wire->beginTransmission(_address);
-		_wire->write(SensorRegisters::PWR_MGMT_1);
-		_wire->write(SENSOR_WAKEUP);
-
-		return (_wire->endTransmission() == 0);
-	}
-
-	return false;
-}
-
-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;
-}
-
-bool Sensor::read()
-{
-	Time now = 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() != 0)
-	{
-		_status = SensorStatus::ERR_WRITE;
-		return false;
-	}
-
-	int8_t response_length = _wire->requestFrom(_address, (uint8_t)14);
-
-	if (response_length != 14)
-	{
-		_status = SensorStatus::ERR_READ;
-		return false;
-	}
-
-	// Accelerometer
-	_accel_raw_x = _readTwoBytes(); // ACCEL_XOUT_H  ACCEL_XOUT_L
-	_accel_raw_y = _readTwoBytes(); // ACCEL_YOUT_H  ACCEL_YOUT_L
-	_accel_raw_z = _readTwoBytes(); // ACCEL_ZOUT_H  ACCEL_ZOUT_L
-
-	// Gyroscope
-	_gyro_raw_x = _readTwoBytes(); // GYRO_XOUT_H   GYRO_XOUT_L
-	_gyro_raw_y = _readTwoBytes(); // GYRO_YOUT_H   GYRO_YOUT_L
-	_gyro_raw_z = _readTwoBytes(); // GYRO_ZOUT_H   GYRO_ZOUT_L
-
-	// Duration interval
-	now.update();
-	float 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
-	float accel_x_pow_two = pow(_accel_raw_x, 2);
-	float accel_y_pow_two = pow(_accel_raw_y, 2);
-	float accel_z_pow_two = pow(_accel_raw_z, 2);
-
-	_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;
-
-	// 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;
-
-	_yaw = _gyro_z;
-	_pitch = 0.96 * _gyro_y + 0.04 * _accel_y;
-	_roll = 0.96 * _gyro_x + 0.04 * _accel_x;
-
-	return true;
-}
-
-bool Sensor::setAccelSensitivity(uint8_t sensitivity)
-{
-	_accel_sensitivity = sensitivity;
-
-	if (_accel_sensitivity > 3)
-		_accel_sensitivity = 3;
-
-	uint8_t val = getRegister(SensorRegisters::ACCEL_CONFIG);
-
-	if (_status != SensorStatus::OK)
-	{
-		return false;
-	}
-
-	// No need to write same value
-	if (((val >> 3) & 3) != _accel_sensitivity)
-	{
-		val &= 0xE7;
-		val |= (_accel_sensitivity << 3);
-
-		if (!setRegister(SensorRegisters::ACCEL_CONFIG, val))
-		{
-			return false;
-		}
-	}
-
-	// Calculate conversion factor.
-	_accel_to_g_force = (1 << _accel_sensitivity) * RAW_TO_G_FACTOR;
-
-	return true;
-}
-
-bool Sensor::setGyroSensitivity(uint8_t sensitivity)
-{
-	_gyro_sensitivity = sensitivity;
-
-	if (_gyro_sensitivity > 3)
-		_gyro_sensitivity = 3;
-
-	uint8_t val = getRegister(SensorRegisters::GYRO_CONFIG);
-
-	if (_status != SensorStatus::OK)
-	{
-		return false;
-	}
-
-	// No need to write same value
-	if (((val >> 3) & 3) != _gyro_sensitivity)
-	{
-		val &= 0xE7;
-		val |= (_gyro_sensitivity << 3);
-
-		if (!setRegister(SensorRegisters::GYRO_CONFIG, val))
-		{
-			return false;
-		}
-	}
-
-	_ang_rate_to_dps = (1 << _gyro_sensitivity) * RAW_TO_DPS_FACTOR;
-
-	return true;
-}
-
-float Sensor::getAccelX()
-{
-	return _accel_raw_x;
-};
-
-float Sensor::getAccelY()
-{
-	return _accel_raw_y;
-};
-
-float Sensor::getAccelZ()
-{
-	return _accel_raw_z;
-};
-
-float Sensor::getAngleX()
-{
-	return _accel_x;
-};
-
-float Sensor::getAngleY()
-{
-	return _accel_y;
-};
-
-float Sensor::getGyroX()
-{
-	return _gyro_raw_x;
-};
-
-float Sensor::getGyroY()
-{
-	return _gyro_raw_y;
-};
-
-float Sensor::getGyroZ()
-{
-	return _gyro_raw_z;
-};
-
-float Sensor::getPitch()
-{
-	return _pitch;
-};
-
-float Sensor::getRoll()
-{
-	return _roll;
-};
-
-bool Sensor::setRegister(uint8_t reg, uint8_t value)
-{
-	_wire->beginTransmission(_address);
-	_wire->write(reg);
-	_wire->write(value);
-
-	if (_wire->endTransmission() != 0)
-	{
-		_status = SensorStatus::ERR_WRITE;
-		return false;
-	}
-
-	return true;
-}
-
-uint8_t Sensor::getRegister(uint8_t reg)
-{
-	_wire->beginTransmission(_address);
-	_wire->write(reg);
-
-	if (_wire->endTransmission() != 0)
-	{
-		_status = SensorStatus::ERR_WRITE;
-		return 0;
-	}
-
-	uint8_t response_length = _wire->requestFrom(_address, (uint8_t)1);
-	if (response_length != 1)
-	{
-		_status = SensorStatus::ERR_READ;
-		return 0;
-	}
-
-	uint8_t val = _wire->read();
-
-	return val;
-}
-
-SensorStatus Sensor::getStatus()
-{
-	SensorStatus status = _status;
-	_status = SensorStatus::OK;
-
-	return status;
-};
-
-int16_t Sensor::_readTwoBytes()
-{
-	int16_t response = _wire->read();
-	response <<= 8;
-	response |= _wire->read();
-
-	return response;
-}