✨ (MPU6050): Read measurements

src/i2cdev/mpu6050.h

@@ -0,0 +1,336 @@
+#ifndef __I2CDEVLIB_MPU6050_H__
+#define __I2CDEVLIB_MPU6050_H__
+
+#include "i2cdevbus.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif // __cplusplus
+
+#define MPU6050_I2CADDR_AD0_LOW (0x68)
+#define MPU6050_I2CADDR_AD0_HIGH (0x69)
+#define MPU6050_I2CADDR_BASE (MPU6050_I2CADDR_AD0_LOW)
+
+#define MPU6050_DEVICE_ID (0x68) // Expected MPU6050_REG_WHO_AM_I value
+
+#define MPU6050_CREATE_MASK(shift, length) (((1 << (length)) - 1) << (shift))
+
+typedef enum mpu6050_reg_t {
+    MPU6050_REG_XG_OFFS_TC = 0x00,
+    MPU6050_REG_YG_OFFS_TC = 0x01,
+    MPU6050_REG_ZG_OFFS_TC = 0x02,
+
+    MPU6050_REG_X_FINE_GAIN = 0x03,
+    MPU6050_REG_Y_FINE_GAIN = 0x04,
+    MPU6050_REG_Z_FINE_GAIN = 0x05,
+
+    MPU6050_REG_XA_OFFS_H = 0x06,
+    MPU6050_REG_XA_OFFS_L = 0x07,
+
+    MPU6050_REG_YA_OFFS_H = 0x08,
+    MPU6050_REG_YA_OFFS_L = 0x09,
+
+    MPU6050_REG_ZA_OFFS_H = 0x0A,
+    MPU6050_REG_ZA_OFFS_L = 0x0B,
+
+    MPU6050_REG_SELF_TEST_X = 0x0D,
+    MPU6050_REG_SELF_TEST_Y = 0x0E,
+    MPU6050_REG_SELF_TEST_Z = 0x0F,
+    MPU6050_REG_SELF_TEST_A = 0x10,
+
+    MPU6050_REG_XG_OFFS_USRH = 0x13,
+    MPU6050_REG_XG_OFFS_USRL = 0x14,
+    MPU6050_REG_YG_OFFS_USRH = 0x15,
+    MPU6050_REG_YG_OFFS_USRL = 0x16,
+    MPU6050_REG_ZG_OFFS_USRH = 0x17,
+    MPU6050_REG_ZG_OFFS_USRL = 0x18,
+
+    MPU6050_REG_SMPLRT_DIV = 0x19,
+
+    MPU6050_REG_CONFIG = 0x1A,
+    MPU6050_REG_GYRO_CONFIG = 0x1B,
+    MPU6050_REG_ACCEL_CONFIG = 0x1C,
+
+    MPU6050_REG_FF_THR = 0x1D,
+    MPU6050_REG_FF_DUR = 0x1E,
+
+    MPU6050_REG_MOT_THR = 0x1F,
+    MPU6050_REG_MOT_DUR = 0x20,
+
+    MPU6050_REG_ZRMOT_THR = 0x21,
+    MPU6050_REG_ZRMOT_DUR = 0x22,
+
+    MPU6050_REG_FIFO_EN = 0x23,
+
+    MPU6050_REG_I2C_MST_CTRL = 0x24,
+    MPU6050_REG_I2C_SLV0_ADDR = 0x25,
+    MPU6050_REG_I2C_SLV0_REG = 0x26,
+    MPU6050_REG_I2C_SLV0_CTRL = 0x27,
+    MPU6050_REG_I2C_SLV1_ADDR = 0x28,
+    MPU6050_REG_I2C_SLV1_REG = 0x29,
+    MPU6050_REG_I2C_SLV1_CTRL = 0x2A,
+    MPU6050_REG_I2C_SLV2_ADDR = 0x2B,
+    MPU6050_REG_I2C_SLV2_REG = 0x2C,
+    MPU6050_REG_I2C_SLV2_CTRL = 0x2D,
+    MPU6050_REG_I2C_SLV3_ADDR = 0x2E,
+    MPU6050_REG_I2C_SLV3_REG = 0x2F,
+    MPU6050_REG_I2C_SLV3_CTRL = 0x30,
+    MPU6050_REG_I2C_SLV4_ADDR = 0x31,
+    MPU6050_REG_I2C_SLV4_REG = 0x32,
+    MPU6050_REG_I2C_SLV4_DO = 0x33,
+    MPU6050_REG_I2C_SLV4_CTRL = 0x34,
+    MPU6050_REG_I2C_SLV4_DI = 0x35,
+    MPU6050_REG_I2C_MST_STATUS = 0x36,
+
+    MPU6050_REG_INT_PIN_CFG = 0x37,
+    MPU6050_REG_INT_ENABLE = 0x38,
+    MPU6050_REG_DMP_INT_STATUS = 0x39,
+    MPU6050_REG_INT_STATUS = 0x3A,
+
+    MPU6050_REG_ACCEL_XOUT_H = 0x3B,
+    MPU6050_REG_ACCEL_XOUT_L = 0x3C,
+    MPU6050_REG_ACCEL_YOUT_H = 0x3D,
+    MPU6050_REG_ACCEL_YOUT_L = 0x3E,
+    MPU6050_REG_ACCEL_ZOUT_H = 0x3F,
+    MPU6050_REG_ACCEL_ZOUT_L = 0x40,
+
+    MPU6050_REG_TEMP_OUT_H = 0x41,
+    MPU6050_REG_TEMP_OUT_L = 0x42,
+
+    MPU6050_REG_GYRO_XOUT_H = 0x43,
+    MPU6050_REG_GYRO_XOUT_L = 0x44,
+    MPU6050_REG_GYRO_YOUT_H = 0x45,
+    MPU6050_REG_GYRO_YOUT_L = 0x46,
+    MPU6050_REG_GYRO_ZOUT_H = 0x47,
+    MPU6050_REG_GYRO_ZOUT_L = 0x48,
+
+    MPU6050_REG_EXT_SENS_DATA_00 = 0x49,
+    MPU6050_REG_EXT_SENS_DATA_01 = 0x4A,
+    MPU6050_REG_EXT_SENS_DATA_02 = 0x4B,
+    MPU6050_REG_EXT_SENS_DATA_03 = 0x4C,
+    MPU6050_REG_EXT_SENS_DATA_04 = 0x4D,
+    MPU6050_REG_EXT_SENS_DATA_05 = 0x4E,
+    MPU6050_REG_EXT_SENS_DATA_06 = 0x4F,
+    MPU6050_REG_EXT_SENS_DATA_07 = 0x50,
+    MPU6050_REG_EXT_SENS_DATA_08 = 0x51,
+    MPU6050_REG_EXT_SENS_DATA_09 = 0x52,
+    MPU6050_REG_EXT_SENS_DATA_10 = 0x53,
+    MPU6050_REG_EXT_SENS_DATA_11 = 0x54,
+    MPU6050_REG_EXT_SENS_DATA_12 = 0x55,
+    MPU6050_REG_EXT_SENS_DATA_13 = 0x56,
+    MPU6050_REG_EXT_SENS_DATA_14 = 0x57,
+    MPU6050_REG_EXT_SENS_DATA_15 = 0x58,
+    MPU6050_REG_EXT_SENS_DATA_16 = 0x59,
+    MPU6050_REG_EXT_SENS_DATA_17 = 0x5A,
+    MPU6050_REG_EXT_SENS_DATA_18 = 0x5B,
+    MPU6050_REG_EXT_SENS_DATA_19 = 0x5C,
+    MPU6050_REG_EXT_SENS_DATA_20 = 0x5D,
+    MPU6050_REG_EXT_SENS_DATA_21 = 0x5E,
+    MPU6050_REG_EXT_SENS_DATA_22 = 0x5F,
+    MPU6050_REG_EXT_SENS_DATA_23 = 0x60,
+
+    MPU6050_REG_MOV_DETECT_STATUS = 0x61,
+
+    MPU6050_REG_I2C_SLV0_DO = 0x63,
+    MPU6050_REG_I2C_SLV1_DO = 0x64,
+    MPU6050_REG_I2C_SLV2_DO = 0x65,
+    MPU6050_REG_I2C_SLV3_DO = 0x66,
+
+    MPU6050_REG_I2C_MST_DELAY_CTRL = 0x67,
+
+    MPU6050_REG_SIGNAL_PATH_RESET = 0x68,
+
+    MPU6050_REG_MOT_DETECT_CTRL = 0x69,
+
+    MPU6050_REG_USER_CTRL = 0x6A,
+
+    MPU6050_REG_PWR_MGMT_1 = 0x6B,
+    MPU6050_REG_PWR_MGMT_2 = 0x6C,
+
+    MPU6050_REG_BANK_SEL = 0x6D,
+
+    MPU6050_REG_MEM_START_ADDR = 0x6E,
+    MPU6050_REG_MEM_R_W = 0x6F,
+
+    MPU6050_REG_DMP_CFG_1 = 0x70,
+    MPU6050_REG_DMP_CFG_2 = 0x71,
+
+    MPU6050_REG_FIFO_COUNT_H = 0x72,
+    MPU6050_REG_FIFO_COUNT_L = 0x73,
+
+    MPU6050_REG_FIFO_R_W = 0x74,
+
+    /// Device ID
+    /// The value of the WHO_AM_I register should be MPU6050_DEVICE_ID
+    MPU6050_REG_WHO_AM_I = 0x75,
+} mpu6050_reg_t;
+
+#define MPU6050_PWR_MGMT_1_DEVICE_RESET_SHIFT (7)
+#define MPU6050_PWR_MGMT_1_DEVICE_RESET_LENGTH (1)
+#define MPU6050_PWR_MGMT_1_DEVICE_RESET MPU6050_CREATE_MASK(MPU6050_PWR_MGMT_1_DEVICE_RESET_SHIFT, MPU6050_PWR_MGMT_1_DEVICE_RESET_LENGTH)
+
+#define MPU6050_SIGNAL_PATH_RESET_GYRO_RESET_SHIFT (2)
+#define MPU6050_SIGNAL_PATH_RESET_GYRO_RESET_LENGTH (1)
+#define MPU6050_SIGNAL_PATH_RESET_GYRO_RESET MPU6050_CREATE_MASK(MPU6050_SIGNAL_PATH_RESET_GYRO_RESET_SHIFT, MPU6050_SIGNAL_PATH_RESET_GYRO_RESET_LENGTH)
+
+#define MPU6050_SIGNAL_PATH_RESET_ACCEL_RESET_SHIFT (1)
+#define MPU6050_SIGNAL_PATH_RESET_ACCEL_RESET_LENGTH (1)
+#define MPU6050_SIGNAL_PATH_RESET_ACCEL_RESET MPU6050_CREATE_MASK(MPU6050_SIGNAL_PATH_RESET_ACCEL_RESET_SHIFT, MPU6050_SIGNAL_PATH_RESET_ACCEL_RESET_LENGTH)
+
+#define MPU6050_SIGNAL_PATH_RESET_TEMP_RESET_SHIFT (0)
+#define MPU6050_SIGNAL_PATH_RESET_TEMP_RESET_LENGTH (1)
+#define MPU6050_SIGNAL_PATH_RESET_TEMP_RESET MPU6050_CREATE_MASK(MPU6050_SIGNAL_PATH_RESET_TEMP_RESET_SHIFT, MPU6050_SIGNAL_PATH_RESET_TEMP_RESET_LENGTH)
+
+#define MPU6050_ACCEL_CONFIG_AFS_SEL_SHIFT (3)
+#define MPU6050_ACCEL_CONFIG_AFS_SEL_LENGTH (2)
+#define MPU6050_ACCEL_CONFIG_AFS_SEL MPU6050_CREATE_MASK(MPU6050_ACCEL_CONFIG_AFS_SEL_SHIFT, MPU6050_ACCEL_CONFIG_AFS_SEL_LENGTH)
+
+typedef enum mpu6050_accel_range_t {
+    MPU6050_ACCEL_RANGE_2G = 0b00,
+    MPU6050_ACCEL_RANGE_4G = 0b01,
+    MPU6050_ACCEL_RANGE_8G = 0b10,
+    MPU6050_ACCEL_RANGE_16G = 0b11,
+} mpu6050_accel_range_t;
+
+#define MPU6050_GYRO_CONFIG_FS_SEL_SHIFT (3)
+#define MPU6050_GYRO_CONFIG_FS_SEL_LENGTH (2)
+#define MPU6050_GYRO_CONFIG_FS_SEL MPU6050_CREATE_MASK(MPU6050_GYRO_CONFIG_FS_SEL_SHIFT, MPU6050_GYRO_CONFIG_FS_SEL_LENGTH)
+
+typedef enum mpu6050_gyro_range_t {
+    MPU6050_GYRO_RANGE_250_DEG = 0b00,
+    MPU6050_GYRO_RANGE_500_DEG = 0b01,
+    MPU6050_GYRO_RANGE_1000_DEG = 0b10,
+    MPU6050_GYRO_RANGE_2000_DEG = 0b11,
+} mpu6050_gyro_range_t;
+
+#define MPU6050_REG_CONFIG_FSYNC_SHIFT (3)
+#define MPU6050_REG_CONFIG_FSYNC_LENGTH (3)
+#define MPU6050_REG_CONFIG_FSYNC MPU6050_CREATE_MASK(MPU6050_REG_CONFIG_FSYNC_SHIFT, MPU6050_REG_CONFIG_FSYNC_LENGTH)
+
+typedef enum mpu6050_fsync_t {
+    MPU6050_FSYNC_DISABLED = 0,
+    MPU6050_FSYNC_TEMP_OUT_L = 1,
+    MPU6050_FSYNC_GYRO_XOUT_L = 2,
+    MPU6050_FSYNC_GYRO_YOUT_L = 3,
+    MPU6050_FSYNC_GYRO_ZOUT_L = 4,
+    MPU6050_FSYNC_ACCEL_XOUT_L = 5,
+    MPU6050_FSYNC_ACCEL_YOUT_L = 6,
+    MPU6050_FSYNC_ACCEL_ZOUT_L = 7,
+} mpu6050_fsync_t;
+
+#define MPU6050_REG_CONFIG_DLPF_CFG_SHIFT (0)
+#define MPU6050_REG_CONFIG_DLPF_CFG_LENGTH (3)
+#define MPU6050_REG_CONFIG_DLPF_CFG MPU6050_CREATE_MASK(MPU6050_REG_CONFIG_DLPF_CFG_SHIFT, MPU6050_REG_CONFIG_DLPF_CFG_LENGTH)
+
+typedef enum mpu6050_bandwidth_t {
+    /// Accel: 260 Hz, Gyro: 256 Hz
+    MPU6050_BANDWIDTH_260_HZ = 0,
+    /// Accel: 184 Hz, Gyro: 188 Hz
+    MPU6050_BANDWIDTH_184_HZ = 1,
+    /// Accel: 94 Hz, Gyro: 98 Hz
+    MPU6050_BANDWIDTH_94_HZ = 2,
+    /// Accel: 44 Hz, Gyro: 42 Hz
+    MPU6050_BANDWIDTH_44_HZ = 3,
+    /// Accel: 21 Hz, Gyro: 20 Hz
+    MPU6050_BANDWIDTH_21_HZ = 4,
+    /// Accel: 10 Hz, Gyro: 10 Hz
+    MPU6050_BANDWIDTH_10_HZ = 5,
+    /// Accel: 5 Hz, Gyro: 5 Hz
+    MPU6050_BANDWIDTH_5_HZ = 6,
+} mpu6050_bandwidth_t;
+
+typedef enum mpu6050_highpass_t {
+    MPU6050_HIGHPASS_OFF = 0b0000,
+    MPU6050_HIGHPASS_5_HZ = 0b0001,
+    MPU6050_HIGHPASS_2_5_HZ = 0b0010,
+    MPU6050_HIGHPASS_1_25_HZ = 0b0011,
+    MPU6050_HIGHPASS_0_63_HZ = 0b0100,
+    MPU6050_HIGHPASS_UNUSED = 0b0101,
+    MPU6050_HIGHPASS_HOLD = 0b0110,
+} mpu6050_highpass_t;
+
+typedef enum mpu6050_cycle_rate_t {
+    MPU6050_CYCLE_RATE_1_25_HZ = 0b000,
+    MPU6050_CYCLE_RATE_5_HZ = 0b001,
+    MPU6050_CYCLE_RATE_20_HZ = 0b010,
+    MPU6050_CYCLE_RATE_40_HZ = 0b011,
+} mpu6050_cycle_rate_t;
+
+typedef struct mpu6050_3axis_data {
+    float x;
+    float y;
+    float z;
+} mpu6050_3axis_data;
+
+typedef struct mpu6050_motion_data {
+    mpu6050_3axis_data accel;
+    mpu6050_3axis_data gyro;
+} mpu6050_motion_data;
+
+typedef struct mpu6050_all_data {
+    mpu6050_3axis_data accel;
+    mpu6050_3axis_data gyro;
+    float temperature;
+} mpu6050_all_data;
+
+mpu6050_3axis_data mpu6050_process_accel_data(int16_t x, int16_t y, int16_t z, mpu6050_accel_range_t range) {
+    mpu6050_3axis_data data;
+
+    float scale = 1.0f;
+    switch (range) {
+        case MPU6050_ACCEL_RANGE_2G:
+            scale = 16384.0f;
+            break;
+        case MPU6050_ACCEL_RANGE_4G:
+            scale = 8192.0f;
+            break;
+        case MPU6050_ACCEL_RANGE_8G:
+            scale = 4096.0f;
+            break;
+        case MPU6050_ACCEL_RANGE_16G:
+            scale = 2048.0f;
+            break;
+    }
+
+    data.x = (float)x / scale;
+    data.y = (float)y / scale;
+    data.z = (float)z / scale;
+
+    return data;
+}
+
+mpu6050_3axis_data mpu6050_process_gyro_data(int16_t x, int16_t y, int16_t z, mpu6050_gyro_range_t range) {
+    mpu6050_3axis_data data;
+
+    float scale = 1.0f;
+    switch (range) {
+        case MPU6050_GYRO_RANGE_250_DEG:
+            scale = 131.0f;
+            break;
+        case MPU6050_GYRO_RANGE_500_DEG:
+            scale = 65.5f;
+            break;
+        case MPU6050_GYRO_RANGE_1000_DEG:
+            scale = 32.8f;
+            break;
+        case MPU6050_GYRO_RANGE_2000_DEG:
+            scale = 16.4f;
+            break;
+    }
+
+    data.x = (float)x / scale;
+    data.y = (float)y / scale;
+    data.z = (float)z / scale;
+
+    return data;
+}
+
+static float mpu6050_process_temperature(int16_t temp) {
+    return (float)temp / 340.0f + 36.53f;
+}
+
+#ifdef __cplusplus
+};
+#endif // __cplusplus
+
+#endif //__I2CDEVLIB_MPU6050_H__