1 files changed, 360 insertions, 0 deletions
diff --git a/src/sensor/sensor.cpp b/src/sensor/sensor.cpp
new file mode 100644
index 0000000..e11a74b
--- /dev/null
+++ b/src/sensor/sensor.cpp
@@ -0,0 +1,360 @@
+#include "sensor.hpp"
+#include "../utils/serial.hpp"
+#include "../utils/time.hpp"
+#include "registers.hpp"
+
+Sensor::Sensor(uint8_t address, TwoWire *wire, SerialStream sout,
+			   unsigned int throttle_time)
+	: _last_time(0), _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();
+
+	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 = _readHighLow();
+	_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();
+	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;
+
+	_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
+	float accel_y_pow_two = pow(_accel_raw_y, 2);
+	float accel_z_pow_two = pow(_accel_raw_z, 2);
+
+	_accel_x = atan2(_accel_raw_y, _accel_raw_z) * 180 / PI;
+
+	_accel_y = atan2(-_accel_raw_x, sqrt(accel_y_pow_two + accel_z_pow_two)) * 180 / 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;
+	*/
+
+	// 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 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"
+		  << endl;
+
+	_pitch = 0.96 * _gyro_y + 0.04 * _accel_y;
+	_roll = 0.96 * _gyro_x + 0.04 * _accel_x;
+	_yaw = _gyro_z;
+
+	return true;
+}
+
+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;
+		accel_config |= (_accel_sensitivity << 3);
+
+		if (!setRegister(SensorRegisters::ACCEL_CONFIG, accel_config))
+			return false;
+	}
+
+	// Calculate conversion factor.
+	_accel_to_g_force = (1 << _accel_sensitivity) * RAW_TO_G_FACTOR;
+
+	return true;
+}
+
+bool Sensor::setGyroSensitivity(uint8_t sensitivity)
+{
+	if (sensitivity > 3)
+		sensitivity = 3;
+
+	_gyro_sensitivity = sensitivity;
+
+	uint8_t gyro_config = getRegister(SensorRegisters::GYRO_CONFIG);
+
+	if (_status != SensorStatus::OK)
+	{
+		return false;
+	}
+
+	// No need to write same value
+	if (((gyro_config >> 3) & 3) != _gyro_sensitivity)
+	{
+		gyro_config &= 0xE7;
+		gyro_config |= (_gyro_sensitivity << 3);
+
+		if (!setRegister(SensorRegisters::GYRO_CONFIG, gyro_config))
+		{
+			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;
+	}
+
+	return _wire->read();
+}
+
+SensorStatus Sensor::getStatus()
+{
+	SensorStatus status = _status;
+	_status = SensorStatus::OK;
+
+	return status;
+}
+
+int16_t Sensor::_readHighLow()
+{
+	int16_t high = _wire->read();
+	int16_t low = _wire->read();
+
+	return high << 8 | low;
+}