refactor: reorganize & add debugging

3 files changed, 690 insertions, 0 deletions

diff --git a/src/sensor/registers.hpp b/src/sensor/registers.hpp
new file mode 100644
index 0000000..c1b19b4
--- /dev/null
+++ b/src/sensor/registers.hpp
@@ -0,0 +1,137 @@
+#ifndef SENSOR_REGISTERS_HPP
+#define SENSOR_REGISTERS_HPP
+
+namespace SensorRegisters
+{
+	const uint8_t XG_OFFS_TC = 0x00;
+	const uint8_t YG_OFFS_TC = 0x01;
+	const uint8_t ZG_OFFS_TC = 0x02;
+
+	const uint8_t X_FINE_GAIN = 0x03;
+	const uint8_t Y_FINE_GAIN = 0x04;
+	const uint8_t Z_FINE_GAIN = 0x05;
+
+	const uint8_t XA_OFFS_H = 0x06;
+	const uint8_t XA_OFFS_L_TC = 0x07;
+	const uint8_t YA_OFFS_H = 0x08;
+	const uint8_t YA_OFFS_L_TC = 0x09;
+	const uint8_t ZA_OFFS_H = 0x0A;
+	const uint8_t ZA_OFFS_L_TC = 0x0B;
+
+	const uint8_t SELF_TEST_X = 0x0D;
+	const uint8_t SELF_TEST_Y = 0x0E;
+	const uint8_t SELF_TEST_Z = 0x0F;
+	const uint8_t SELF_TEST_A = 0x10;
+
+	const uint8_t XG_OFFS_USRH = 0x13;
+	const uint8_t XG_OFFS_USRL = 0x14;
+	const uint8_t YG_OFFS_USRH = 0x15;
+	const uint8_t YG_OFFS_USRL = 0x16;
+	const uint8_t ZG_OFFS_USRH = 0x17;
+	const uint8_t ZG_OFFS_USRL = 0x18;
+
+	const uint8_t SMPLRT_DIV = 0x19;
+	const uint8_t CONFIG = 0x1A;
+	const uint8_t GYRO_CONFIG = 0x1B;
+	const uint8_t ACCEL_CONFIG = 0x1C;
+
+	const uint8_t FF_THR = 0x1D;
+	const uint8_t FF_DUR = 0x1E;
+	const uint8_t MOT_THR = 0x1F;
+	const uint8_t MOT_DUR = 0x20;
+	const uint8_t ZRMOT_THR = 0x21;
+	const uint8_t ZRMOT_DUR = 0x22;
+	const uint8_t FIFO_EN = 0x23;
+
+	const uint8_t I2C_MST_CTRL = 0x24;
+	const uint8_t I2C_SLV0_ADDR = 0x25;
+	const uint8_t I2C_SLV0_REG = 0x26;
+	const uint8_t I2C_SLV0_CTRL = 0x27;
+	const uint8_t I2C_SLV1_ADDR = 0x28;
+	const uint8_t I2C_SLV1_REG = 0x29;
+	const uint8_t I2C_SLV1_CTRL = 0x2A;
+	const uint8_t I2C_SLV2_ADDR = 0x2B;
+	const uint8_t I2C_SLV2_REG = 0x2C;
+	const uint8_t I2C_SLV2_CTRL = 0x2D;
+	const uint8_t I2C_SLV3_ADDR = 0x2E;
+	const uint8_t I2C_SLV3_REG = 0x2F;
+	const uint8_t I2C_SLV3_CTRL = 0x30;
+	const uint8_t I2C_SLV4_ADDR = 0x31;
+	const uint8_t I2C_SLV4_REG = 0x32;
+	const uint8_t I2C_SLV4_DO = 0x33;
+	const uint8_t I2C_SLV4_CTRL = 0x34;
+	const uint8_t I2C_SLV4_DI = 0x35;
+	const uint8_t I2C_MST_STATUS = 0x36;
+
+	const uint8_t INT_PIN_CFG = 0x37;
+	const uint8_t INT_ENABLE = 0x38;
+	const uint8_t DMP_INT_STATUS = 0x39;
+	const uint8_t INT_STATUS = 0x3A;
+
+	const uint8_t ACCEL_XOUT_H = 0x3B;
+	const uint8_t ACCEL_XOUT_L = 0x3C;
+	const uint8_t ACCEL_YOUT_H = 0x3D;
+	const uint8_t ACCEL_YOUT_L = 0x3E;
+	const uint8_t ACCEL_ZOUT_H = 0x3F;
+	const uint8_t ACCEL_ZOUT_L = 0x40;
+	const uint8_t TEMP_OUT_H = 0x41;
+	const uint8_t TEMP_OUT_L = 0x42;
+	const uint8_t GYRO_XOUT_H = 0x43;
+	const uint8_t GYRO_XOUT_L = 0x44;
+	const uint8_t GYRO_YOUT_H = 0x45;
+	const uint8_t GYRO_YOUT_L = 0x46;
+	const uint8_t GYRO_ZOUT_H = 0x47;
+	const uint8_t GYRO_ZOUT_L = 0x48;
+
+	const uint8_t EXT_SENS_DATA_00 = 0x49;
+	const uint8_t EXT_SENS_DATA_01 = 0x4A;
+	const uint8_t EXT_SENS_DATA_02 = 0x4B;
+	const uint8_t EXT_SENS_DATA_03 = 0x4C;
+	const uint8_t EXT_SENS_DATA_04 = 0x4D;
+	const uint8_t EXT_SENS_DATA_05 = 0x4E;
+	const uint8_t EXT_SENS_DATA_06 = 0x4F;
+	const uint8_t EXT_SENS_DATA_07 = 0x50;
+	const uint8_t EXT_SENS_DATA_08 = 0x51;
+	const uint8_t EXT_SENS_DATA_09 = 0x52;
+	const uint8_t EXT_SENS_DATA_10 = 0x53;
+	const uint8_t EXT_SENS_DATA_11 = 0x54;
+	const uint8_t EXT_SENS_DATA_12 = 0x55;
+	const uint8_t EXT_SENS_DATA_13 = 0x56;
+	const uint8_t EXT_SENS_DATA_14 = 0x57;
+	const uint8_t EXT_SENS_DATA_15 = 0x58;
+	const uint8_t EXT_SENS_DATA_16 = 0x59;
+	const uint8_t EXT_SENS_DATA_17 = 0x5A;
+	const uint8_t EXT_SENS_DATA_18 = 0x5B;
+	const uint8_t EXT_SENS_DATA_19 = 0x5C;
+	const uint8_t EXT_SENS_DATA_20 = 0x5D;
+	const uint8_t EXT_SENS_DATA_21 = 0x5E;
+	const uint8_t EXT_SENS_DATA_22 = 0x5F;
+	const uint8_t EXT_SENS_DATA_23 = 0x60;
+
+	const uint8_t MOT_DETECT_STATUS = 0x61;
+
+	const uint8_t I2C_SLV0_DO = 0x63;
+	const uint8_t I2C_SLV1_DO = 0x64;
+	const uint8_t I2C_SLV2_DO = 0x65;
+	const uint8_t I2C_SLV3_DO = 0x66;
+	const uint8_t I2C_MST_DELAY_CTRL = 0x67;
+
+	const uint8_t SIGNAL_PATH_RESET = 0x68;
+	const uint8_t MOT_DETECT_CTRL = 0x69;
+	const uint8_t USER_CTRL = 0x6A;
+
+	const uint8_t PWR_MGMT_1 = 0x6B;
+	const uint8_t PWR_MGMT_2 = 0x6C;
+	const uint8_t BANK_SEL = 0x6D;
+	const uint8_t MEM_START_ADDR = 0x6E;
+	const uint8_t MEM_R_W = 0x6F;
+
+	const uint8_t DMP_CFG_1 = 0x70;
+	const uint8_t DMP_CFG_2 = 0x71;
+	const uint8_t FIFO_COUNTH = 0x72;
+	const uint8_t FIFO_COUNTL = 0x73;
+	const uint8_t FIFO_R_W = 0x74;
+	const uint8_t WHO_AM_I = 0x75;
+}
+
+#endif
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;
+}
diff --git a/src/sensor/sensor.hpp b/src/sensor/sensor.hpp
new file mode 100644
index 0000000..c982fac
--- /dev/null
+++ b/src/sensor/sensor.hpp
@@ -0,0 +1,193 @@
+#ifndef SENSOR_HPP
+#define SENSOR_HPP
+
+#include "../utils/serial.hpp"
+#include "../utils/time.hpp"
+
+#include "Wire.h"
+#include <Arduino.h>
+
+#define SENSOR_WAKEUP 0x00
+
+#define RAW_TO_DPS_FACTOR (1.0 / 131.0)
+#define RAW_TO_G_FACTOR (1.0 / 16384.0)
+
+enum class SensorStatus
+{
+	OK = 0,
+	THROTTLED = 1,
+	ERR_READ = -1,
+	ERR_WRITE = -2,
+	ERR_NOT_CONNECTED = -3
+};
+
+/**
+ * A GY521 gyro and accelerometer sensor.
+ */
+class Sensor
+{
+public:
+	/**
+	 * A GY521 gyro and accelerometer sensor.
+	 *
+	 * @param address The address of the sensor
+	 * @param wire A Wire instance
+	 * @param sout A serial output stream
+	 * @param throttle_time A minumum time between sensor reads for the sensor to throttle
+	 */
+	Sensor(uint8_t address, TwoWire *wire, SerialStream sout, unsigned int throttle_time);
+
+	/**
+	 * Initializes communication with the sensor.
+	 *
+	 * @returns Whether or not the sensor can be communicated with.
+	 */
+	bool begin();
+
+	/**
+	 * Returns whether or not the sensor is connected.
+	 */
+	bool isConnected();
+
+	/**
+	 * Resets the sensor.
+	 */
+	void reset();
+
+	/**
+	 * Reads from the sensor.
+	 *
+	 * @returns Whether or not it succeeded.
+	 */
+	bool read();
+
+	/**
+	 * Sets the accelerometer sensitivity.
+	 *
+	 * @param sensitivity Accelerometer sensitivity. 0, 1, 2, 3 => 2g 4g 8g 16g
+	 * @returns Whether or not it succeeded.
+	 */
+	bool setAccelSensitivity(uint8_t sensitivity);
+
+	/**
+	 * Sets the gyro sensitivity.
+	 *
+	 * @param sensitivity Gyro sensitivity.
+	 * 	0, 1, 2, 3 => 250, 500, 1000, 2000 degrees/s
+	 * @returns Whether or not it succeeded.
+	 */
+	bool setGyroSensitivity(uint8_t sensitivity);
+
+	/**
+	 * Returns the current X axis acceleration in g:s (g-force).
+	 */
+	float getAccelX();
+
+	/**
+	 * Returns the current Y axis acceleration in g:s (g-force).
+	 */
+	float getAccelY();
+
+	/**
+	 * Returns the current Z axis acceleration in g:s (g-force).
+	 */
+	float getAccelZ();
+
+	/**
+	 * Returns the current X angle.
+	 */
+	float getAngleX();
+
+	/**
+	 * Returns the current Y angle.
+	 */
+	float getAngleY();
+
+	/**
+	 * Returns the current X axis in degrees/s.
+	 */
+	float getGyroX();
+
+	/**
+	 * Returns the current Y axis in degrees/s.
+	 */
+	float getGyroY();
+
+	/**
+	 * Returns the current Z axis in degrees/s.
+	 */
+	float getGyroZ();
+
+	/**
+	 * Returns the current pitch angle.
+	 */
+	float getPitch();
+
+	/**
+	 * Returns the current roll angle.
+	 */
+	float getRoll();
+
+	/**
+	 * Sets the value of a key in the sensor's register.
+	 *
+	 * @param reg The address of a registry key
+	 * @param value A new value
+	 * @returns Whether or not it succeeded.
+	 */
+	bool setRegister(uint8_t reg, uint8_t value);
+
+	/**
+	 * Returns the value of a key in the sensor's registry.
+	 *
+	 * @param reg The address of a registry key
+	 */
+	uint8_t getRegister(uint8_t reg);
+
+	/**
+	 * Returns the last sensor status.
+	 */
+	SensorStatus getStatus();
+
+	// Accelerometer calibration values
+	float accel_cal_x;
+	float accel_cal_y;
+	float accel_cal_z;
+
+	// Gyroscope calibration values
+	float gyro_cal_x;
+	float gyro_cal_y;
+	float gyro_cal_z;
+
+private:
+	uint8_t _address;
+
+	bool _throttle_enabled;
+	unsigned int _throttle_time;
+
+	Time _last_time;
+
+	SensorStatus _status;
+
+	uint8_t _accel_sensitivity;
+	float _accel_to_g_force;
+
+	float _accel_raw_x, _accel_raw_y, _accel_raw_z;
+	float _accel_x, _accel_y;
+
+	uint8_t _gyro_sensitivity;
+	float _ang_rate_to_dps;
+
+	float _gyro_raw_x, _gyro_raw_y, _gyro_raw_z;
+	float _gyro_x, _gyro_y, _gyro_z;
+
+	float _pitch, _roll, _yaw;
+
+	int16_t _readHighLow();
+
+	TwoWire *_wire;
+
+	SerialStream _sout;
+};
+
+#endif