refactor: replace GY521 library

1 files changed, 219 insertions, 0 deletions

diff --git a/src/sensor.hpp b/src/sensor.hpp
new file mode 100644
index 0000000..7ba01bf
--- /dev/null
+++ b/src/sensor.hpp
@@ -0,0 +1,219 @@
+#ifndef SENSOR_HPP
+#define SENSOR_HPP
+
+#include "Arduino.h"
+#include "Wire.h"
+
+#define DEFAULT_THROTTLE_TIME 10 // milliseconds
+
+// Status codes
+#define SENSOR_OK 0
+#define SENSOR_THROTTLED 1
+#define SENSOR_ERR_READ -1
+#define SENSOR_ERR_WRITE -2
+#define SENSOR_ERR_NOT_CONNECTED -3
+
+#define SENSOR_WAKEUP 0x00
+
+#define RAD_TO_DEGREES (180.0 / PI)
+#define RAW_TO_DPS_FACTOR (1.0 / 131.0)
+#define RAW_TO_G_FACTOR (1.0 / 16384.0)
+
+class Sensor
+{
+public:
+	Sensor(uint8_t address, TwoWire *wire);
+
+	/**
+	 * 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();
+
+	/**
+	 * Returns whether or not throttling is enabled.
+	 */
+	bool getThrottleEnabled();
+
+	/**
+	 * Sets whether or not throttling is enabled.
+	 * 
+	 * @param throttle Throttling enable/disable
+	 */
+	void setThrottleEnabled(bool throttle);
+
+	/**
+	 * Returns the throttle time.
+	 */
+	uint16_t getThrottleTime();
+
+	/**
+	 * Sets the throttle time.
+	 * 
+	 * @param time_ms Time in milliseconds to throttle
+	 */
+	void setThrottleTime(uint16_t time_ms);
+
+	/**
+	 * Reads from the sensor.
+	 * 
+	 * @returns The sensor status.
+	 */
+	int16_t read();
+
+	/**
+	 * Returns the accelerometer sensitivity.
+	 * 
+	 * 0,1,2,3 ==> 2g 4g 8g 16g
+	 */
+	uint8_t getAccelSensitivity();
+
+	/**
+	 * 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);
+
+	/**
+	 * Returns the gyro sensitivity.
+	 * 
+	 * 0, 1, 2, 3 => 250, 500, 1000, 2000 degrees/s
+	 */
+	uint8_t getGyroSensitivity();
+
+	/**
+	 * 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 X angle.
+	 */
+	float getAngleX();
+
+	/**
+	 * Returns the current Y angle.
+	 */
+	float getAngleY();
+
+	/**
+	 * Returns the current temperature.
+	 */
+	float getTemperature();
+
+	/**
+	 * Returns the current X axis in degrees/s.
+	 */
+	float getGyroX();
+
+	/**
+	 * Returns the current Y axis in degrees/s.
+	 */
+	float getGyroY();
+
+	/**
+	 * Returns the current pitch angle.
+	 */
+	float getPitch();
+
+	/**
+	 * Returns the current roll angle.
+	 */
+	float getRoll();
+
+	/**
+	 * Returns the last time the sensor was read.
+	 */
+	uint32_t lastTime();
+
+	/**
+	 * 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 The sensor status
+	 */
+	uint8_t 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.
+	 */
+	int16_t 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;
+	uint16_t _throttleTime;
+
+	uint32_t _lastTime;
+	uint32_t _lastMicros;
+
+	int16_t _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, _accel_z;
+
+	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;
+
+	float _temperature;
+
+	int16_t _readTwoBytes();
+
+	TwoWire *_wire;
+};
+
+#endif