refactor: replace GY521 library

4 files changed, 717 insertions, 12 deletions

diff --git a/src/gyro.cpp b/src/gyro.cpp
index 668cef1..1a81850 100644
--- a/src/gyro.cpp
+++ b/src/gyro.cpp
@@ -1,7 +1,8 @@
 #include "Arduino.h"
-#include "GY521.h"
+#include "Wire.h"
+#include "sensor.hpp"
 
-GY521 sensor(0x68);
+Sensor sensor(0x68, &Wire);
 
 void setup()
 {
@@ -10,26 +11,23 @@ void setup()
 	// Wait for Serial because the Arduino Leonardo is weird
 	while (!Serial) {}
 
-	Wire.begin();
-
-	delay(100);
-	while (!sensor.wakeup())
+	while (!sensor.begin())
 	{
 		Serial.print(millis());
-		Serial.println("Error: Could not connect to the GY521 sensor. Retrying after 2000 milliseconds...");
+		Serial.println("Error: Could not connect to the sensor. Retrying after 2000 milliseconds...");
 		delay(2000);
 	}
 	sensor.setAccelSensitivity(2); // 8g
 	sensor.setGyroSensitivity(1);  // 500 degrees/s
 
-	sensor.setThrottle();
+	sensor.setThrottleEnabled(true);
 	Serial.println("start...");
 
 	// Calibration values
-	sensor.axe = 0.198;
-	sensor.aye = -0.018;
-	sensor.gxe = 0.780;
-	sensor.gye = -1.495;
+	sensor.accel_cal_x = 0.198;
+	sensor.accel_cal_y = -0.018;
+	sensor.gyro_cal_x = 0.780;
+	sensor.gyro_cal_y = -1.495;
 }
 
 void loop()
diff --git a/src/sensor.cpp b/src/sensor.cpp
new file mode 100644
index 0000000..e3a2551
--- /dev/null
+++ b/src/sensor.cpp
@@ -0,0 +1,354 @@
+#include "sensor.hpp"
+#include "math.h"
+#include "sensor_registers.hpp"
+
+Sensor::Sensor(uint8_t address, TwoWire *wire)
+{
+	_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;
+	_throttleTime = DEFAULT_THROTTLE_TIME;
+
+	_lastTime = 0;
+	_lastMicros = 0;
+
+	_status = SENSOR_OK;
+
+	_accel_sensitivity = 0;
+	_accel_to_g_force = 1.0 / 16384.0;
+
+	_gyro_sensitivity = 0;
+	_ang_rate_to_dps = 1.0 / 131.0;
+
+	_temperature = 0;
+
+	// Reset the sensor
+	reset();
+}
+
+bool Sensor::begin()
+{
+	_wire->begin();
+
+	if (isConnected())
+	{
+		_wire->beginTransmission(_address);
+		_wire->write(SENSOR_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()
+{
+	setThrottleTime(DEFAULT_THROTTLE_TIME);
+
+	_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;
+	_accel_z = 0;
+
+	_pitch = 0;
+	_roll = 0;
+	_yaw = 0;
+}
+
+void Sensor::setThrottleEnabled(bool throttle = true)
+{
+	_throttle_enabled = throttle;
+};
+
+bool Sensor::getThrottleEnabled()
+{
+	return _throttle_enabled;
+};
+
+void Sensor::setThrottleTime(uint16_t time_ms)
+{
+	_throttleTime = time_ms;
+};
+
+uint16_t Sensor::getThrottleTime()
+{
+	return _throttleTime;
+};
+
+int16_t Sensor::read()
+{
+	auto time_now = millis();
+
+	if (_throttle_enabled)
+	{
+		if ((time_now - _lastTime) < _throttleTime)
+		{
+			return SENSOR_THROTTLED;
+		}
+	}
+
+	_lastTime = time_now;
+
+	_wire->beginTransmission(_address);
+	_wire->write(SENSOR_ACCEL_XOUT_H);
+
+	if (_wire->endTransmission() != 0)
+	{
+		_status = SENSOR_ERR_WRITE;
+		return _status;
+	}
+
+	int8_t response_length = _wire->requestFrom(_address, (uint8_t)14);
+
+	if (response_length != 14)
+	{
+		_status = SENSOR_ERR_READ;
+		return _status;
+	}
+
+	// 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
+
+	// TEMPERATURE
+	_temperature = _readTwoBytes(); // TEMP_OUT_H    TEMP_OUT_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
+	time_now = micros();
+	float duration = (time_now - _lastMicros) * 1e-6; // Duration in seconds.
+	_lastMicros = 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;
+	_accel_z = atan(_accel_raw_z / sqrt(accel_x_pow_two + accel_y_pow_two)) * RAD_TO_DEGREES;
+
+	// Convert temperature to celsius
+	_temperature = _temperature * 0.00294117647 + 36.53;
+
+	// 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 SENSOR_OK;
+}
+
+bool Sensor::setAccelSensitivity(uint8_t sensitivity)
+{
+	_accel_sensitivity = sensitivity;
+
+	if (_accel_sensitivity > 3)
+		_accel_sensitivity = 3;
+
+	uint8_t val = getRegister(SENSOR_ACCEL_CONFIG);
+
+	if (_status != SENSOR_OK)
+	{
+		return false;
+	}
+
+	// No need to write same value
+	if (((val >> 3) & 3) != _accel_sensitivity)
+	{
+		val &= 0xE7;
+		val |= (_accel_sensitivity << 3);
+
+		if (setRegister(SENSOR_ACCEL_CONFIG, val) != SENSOR_OK)
+		{
+			return false;
+		}
+	}
+
+	// Calculate conversion factor.
+	_accel_to_g_force = (1 << _accel_sensitivity) * RAW_TO_G_FACTOR;
+
+	return true;
+}
+
+uint8_t Sensor::getAccelSensitivity()
+{
+	uint8_t val = getRegister(SENSOR_ACCEL_CONFIG);
+
+	if (_status != SENSOR_OK)
+	{
+		return _status; // return and propagate error (best thing to do)
+	}
+
+	_accel_sensitivity = (val >> 3) & 3;
+
+	return _accel_sensitivity;
+}
+
+bool Sensor::setGyroSensitivity(uint8_t sensitivity)
+{
+	_gyro_sensitivity = sensitivity;
+
+	if (_gyro_sensitivity > 3)
+		_gyro_sensitivity = 3;
+
+	uint8_t val = getRegister(SENSOR_GYRO_CONFIG);
+
+	if (_status != 0)
+	{
+		return false;
+	}
+
+	// No need to write same value
+	if (((val >> 3) & 3) != _gyro_sensitivity)
+	{
+		val &= 0xE7;
+		val |= (_gyro_sensitivity << 3);
+
+		if (setRegister(SENSOR_GYRO_CONFIG, val) != SENSOR_OK)
+		{
+			return false;
+		}
+	}
+
+	_ang_rate_to_dps = (1 << _gyro_sensitivity) * RAW_TO_DPS_FACTOR;
+
+	return true;
+}
+
+uint8_t Sensor::getGyroSensitivity()
+{
+	uint8_t val = getRegister(SENSOR_GYRO_CONFIG);
+
+	if (_status != SENSOR_OK)
+	{
+		return _status;
+	}
+
+	_gyro_sensitivity = (val >> 3) & 3;
+
+	return _gyro_sensitivity;
+}
+
+float Sensor::getAccelX() { return _accel_raw_x; };
+float Sensor::getAccelY() { return _accel_raw_y; };
+
+float Sensor::getAngleX() { return _accel_x; };
+float Sensor::getAngleY() { return _accel_y; };
+
+float Sensor::getTemperature() { return _temperature; };
+
+float Sensor::getGyroX() { return _gyro_raw_x; };
+float Sensor::getGyroY() { return _gyro_raw_y; };
+
+float Sensor::getPitch() { return _pitch; };
+float Sensor::getRoll() { return _roll; };
+
+uint32_t Sensor::lastTime() { return _lastTime; };
+
+uint8_t Sensor::setRegister(uint8_t reg, uint8_t value)
+{
+	_wire->beginTransmission(_address);
+	_wire->write(reg);
+	_wire->write(value);
+
+	if (_wire->endTransmission() != 0)
+	{
+		_status = SENSOR_ERR_WRITE;
+		return _status;
+	}
+
+	return SENSOR_OK;
+}
+
+uint8_t Sensor::getRegister(uint8_t reg)
+{
+	_wire->beginTransmission(_address);
+	_wire->write(reg);
+
+	if (_wire->endTransmission() != 0)
+	{
+		_status = SENSOR_ERR_WRITE;
+		return _status;
+	}
+
+	uint8_t response_length = _wire->requestFrom(_address, (uint8_t)1);
+	if (response_length != 1)
+	{
+		_status = SENSOR_ERR_READ;
+		return _status;
+	}
+
+	uint8_t val = _wire->read();
+
+	return val;
+}
+
+int16_t Sensor::getStatus()
+{
+	auto error = _status;
+	_status = SENSOR_OK;
+
+	return error;
+};
+
+int16_t Sensor::_readTwoBytes()
+{
+	int16_t response = _wire->read();
+	response <<= 8;
+	response |= _wire->read();
+
+	return response;
+}
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
diff --git a/src/sensor_registers.hpp b/src/sensor_registers.hpp
new file mode 100644
index 0000000..34b47c6
--- /dev/null
+++ b/src/sensor_registers.hpp
@@ -0,0 +1,134 @@
+#ifndef SENSOR_REGISTERS_HPP
+#define SENSOR_REGISTERS_HPP
+
+#define SENSOR_XG_OFFS_TC 0x00
+#define SENSOR_YG_OFFS_TC 0x01
+#define SENSOR_ZG_OFFS_TC 0x02
+
+#define SENSOR_X_FINE_GAIN 0x03
+#define SENSOR_Y_FINE_GAIN 0x04
+#define SENSOR_Z_FINE_GAIN 0x05
+
+#define SENSOR_XA_OFFS_H 0x06
+#define SENSOR_XA_OFFS_L_TC 0x07
+#define SENSOR_YA_OFFS_H 0x08
+#define SENSOR_YA_OFFS_L_TC 0x09
+#define SENSOR_ZA_OFFS_H 0x0A
+#define SENSOR_ZA_OFFS_L_TC 0x0B
+
+#define SENSOR_SELF_TEST_X 0x0D
+#define SENSOR_SELF_TEST_Y 0x0E
+#define SENSOR_SELF_TEST_Z 0x0F
+#define SENSOR_SELF_TEST_A 0x10
+
+#define SENSOR_XG_OFFS_USRH 0x13
+#define SENSOR_XG_OFFS_USRL 0x14
+#define SENSOR_YG_OFFS_USRH 0x15
+#define SENSOR_YG_OFFS_USRL 0x16
+#define SENSOR_ZG_OFFS_USRH 0x17
+#define SENSOR_ZG_OFFS_USRL 0x18
+
+#define SENSOR_SMPLRT_DIV 0x19
+#define SENSOR_CONFIG 0x1A
+#define SENSOR_GYRO_CONFIG 0x1B
+#define SENSOR_ACCEL_CONFIG 0x1C
+
+#define SENSOR_FF_THR 0x1D
+#define SENSOR_FF_DUR 0x1E
+#define SENSOR_MOT_THR 0x1F
+#define SENSOR_MOT_DUR 0x20
+#define SENSOR_ZRMOT_THR 0x21
+#define SENSOR_ZRMOT_DUR 0x22
+#define SENSOR_FIFO_EN 0x23
+
+#define SENSOR_I2C_MST_CTRL 0x24
+#define SENSOR_I2C_SLV0_ADDR 0x25
+#define SENSOR_I2C_SLV0_REG 0x26
+#define SENSOR_I2C_SLV0_CTRL 0x27
+#define SENSOR_I2C_SLV1_ADDR 0x28
+#define SENSOR_I2C_SLV1_REG 0x29
+#define SENSOR_I2C_SLV1_CTRL 0x2A
+#define SENSOR_I2C_SLV2_ADDR 0x2B
+#define SENSOR_I2C_SLV2_REG 0x2C
+#define SENSOR_I2C_SLV2_CTRL 0x2D
+#define SENSOR_I2C_SLV3_ADDR 0x2E
+#define SENSOR_I2C_SLV3_REG 0x2F
+#define SENSOR_I2C_SLV3_CTRL 0x30
+#define SENSOR_I2C_SLV4_ADDR 0x31
+#define SENSOR_I2C_SLV4_REG 0x32
+#define SENSOR_I2C_SLV4_DO 0x33
+#define SENSOR_I2C_SLV4_CTRL 0x34
+#define SENSOR_I2C_SLV4_DI 0x35
+#define SENSOR_I2C_MST_STATUS 0x36
+
+#define SENSOR_INT_PIN_CFG 0x37
+#define SENSOR_INT_ENABLE 0x38
+#define SENSOR_DMP_INT_STATUS 0x39
+#define SENSOR_INT_STATUS 0x3A
+
+#define SENSOR_ACCEL_XOUT_H 0x3B
+#define SENSOR_ACCEL_XOUT_L 0x3C
+#define SENSOR_ACCEL_YOUT_H 0x3D
+#define SENSOR_ACCEL_YOUT_L 0x3E
+#define SENSOR_ACCEL_ZOUT_H 0x3F
+#define SENSOR_ACCEL_ZOUT_L 0x40
+#define SENSOR_TEMP_OUT_H 0x41
+#define SENSOR_TEMP_OUT_L 0x42
+#define SENSOR_GYRO_XOUT_H 0x43
+#define SENSOR_GYRO_XOUT_L 0x44
+#define SENSOR_GYRO_YOUT_H 0x45
+#define SENSOR_GYRO_YOUT_L 0x46
+#define SENSOR_GYRO_ZOUT_H 0x47
+#define SENSOR_GYRO_ZOUT_L 0x48
+
+#define SENSOR_EXT_SENS_DATA_00 0x49
+#define SENSOR_EXT_SENS_DATA_01 0x4A
+#define SENSOR_EXT_SENS_DATA_02 0x4B
+#define SENSOR_EXT_SENS_DATA_03 0x4C
+#define SENSOR_EXT_SENS_DATA_04 0x4D
+#define SENSOR_EXT_SENS_DATA_05 0x4E
+#define SENSOR_EXT_SENS_DATA_06 0x4F
+#define SENSOR_EXT_SENS_DATA_07 0x50
+#define SENSOR_EXT_SENS_DATA_08 0x51
+#define SENSOR_EXT_SENS_DATA_09 0x52
+#define SENSOR_EXT_SENS_DATA_10 0x53
+#define SENSOR_EXT_SENS_DATA_11 0x54
+#define SENSOR_EXT_SENS_DATA_12 0x55
+#define SENSOR_EXT_SENS_DATA_13 0x56
+#define SENSOR_EXT_SENS_DATA_14 0x57
+#define SENSOR_EXT_SENS_DATA_15 0x58
+#define SENSOR_EXT_SENS_DATA_16 0x59
+#define SENSOR_EXT_SENS_DATA_17 0x5A
+#define SENSOR_EXT_SENS_DATA_18 0x5B
+#define SENSOR_EXT_SENS_DATA_19 0x5C
+#define SENSOR_EXT_SENS_DATA_20 0x5D
+#define SENSOR_EXT_SENS_DATA_21 0x5E
+#define SENSOR_EXT_SENS_DATA_22 0x5F
+#define SENSOR_EXT_SENS_DATA_23 0x60
+
+#define SENSOR_MOT_DETECT_STATUS 0x61
+
+#define SENSOR_I2C_SLV0_DO 0x63
+#define SENSOR_I2C_SLV1_DO 0x64
+#define SENSOR_I2C_SLV2_DO 0x65
+#define SENSOR_I2C_SLV3_DO 0x66
+#define SENSOR_I2C_MST_DELAY_CTRL 0x67
+
+#define SENSOR_SIGNAL_PATH_RESET 0x68
+#define SENSOR_MOT_DETECT_CTRL 0x69
+#define SENSOR_USER_CTRL 0x6A
+
+#define SENSOR_PWR_MGMT_1 0x6B
+#define SENSOR_PWR_MGMT_2 0x6C
+#define SENSOR_BANK_SEL 0x6D
+#define SENSOR_MEM_START_ADDR 0x6E
+#define SENSOR_MEM_R_W 0x6F
+
+#define SENSOR_DMP_CFG_1 0x70
+#define SENSOR_DMP_CFG_2 0x71
+#define SENSOR_FIFO_COUNTH 0x72
+#define SENSOR_FIFO_COUNTL 0x73
+#define SENSOR_FIFO_R_W 0x74
+#define SENSOR_WHO_AM_I 0x75
+
+#endif