refactor: reorganize & add debugging

1 files changed, 193 insertions, 0 deletions

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