diff --git a/.clang-format b/.clang-format
index ab7ebe8e..123d6a84 100644
--- a/.clang-format
+++ b/.clang-format
@@ -9,7 +9,7 @@ AllowAllConstructorInitializersOnNextLine: true
AllowAllParametersOfDeclarationOnNextLine: false
AllowShortBlocksOnASingleLine: true
AllowShortCaseLabelsOnASingleLine: false
-AllowShortFunctionsOnASingleLine: All
+AllowShortFunctionsOnASingleLine: Empty
AllowShortIfStatementsOnASingleLine: Never
AllowShortLambdasOnASingleLine: All
AllowShortLoopsOnASingleLine: false
@@ -37,7 +37,7 @@ BraceWrapping:
SplitEmptyNamespace: false
BreakBeforeTernaryOperators: true
BreakConstructorInitializers: BeforeColon
-ColumnLimit: 120
+ColumnLimit: 200
CompactNamespaces: true
Cpp11BracedListStyle: false
FixNamespaceComments: true
diff --git a/.gitignore b/.gitignore
index 6faf5688..7d3235e2 100644
--- a/.gitignore
+++ b/.gitignore
@@ -62,7 +62,7 @@ dkms.conf
/__garbage__/
**/.vscode/
**/.devcontainer
-/.idea/
+.idea/
cmake-build-debug/
/esp-idf-lib.code-workspace
diff --git a/components/mpu6050/mpu6050.c b/components/mpu6050/mpu6050.c
index 4504ce5e..b2c8f679 100644
--- a/components/mpu6050/mpu6050.c
+++ b/components/mpu6050/mpu6050.c
@@ -1,4 +1,3 @@
-
/*
* The MIT License (MIT)
*
@@ -31,2268 +30,1200 @@
/**
* @file mpu6050.c
- * Thanks to @see https://github.com/gabrielbvicari/esp32-mpu6050 for driver.
- * Improved by Raghav Jha https://github.com/horsemann07
+ *
+ * ESP-IDF driver for MPU6050 MEMS Sensor.
+ *
+ * 6-axis motion tracking devices designed for the low power, low cost,
+ * and high performance requirements of smartphones, tablets and wearable sensors.
+ *
+ * Copyright (c) 2012 Jeff Rowberg
+ * Copyright (c) 2019 Angelo Elias Dalzotto <150633@upf.br>
+ * Copyright (c) 2019 Gabriel Boni Vicari <133192@upf.br>
+ * Copyright (c) 2019 GEPID - Grupo de Pesquisa em Cultura Digital
+ * Copyright (c) 2023 Raghav Jha
+ * Copyright (c) 2023 Ruslan V. Uss
*/
-/* C headers */
+#include "mpu6050.h"
+#include "mpu6050_regs.h"
#include
-
-/* esp headers */
#include
-/* mpu6050 headers */
-#include "mpu6050.h"
-
-#define TAG ("mpu6050.c")
-
-// Internal Registers Definitions
-#define MPU6050_REGISTER_XG_OFFS_TC (0)
-#define MPU6050_REGISTER_YG_OFFS_TC (0x01)
-#define MPU6050_REGISTER_ZG_OFFS_TC (0x02)
-#define MPU6050_REGISTER_X_FINE_GAIN (0x03)
-#define MPU6050_REGISTER_Y_FINE_GAIN (0x04)
-#define MPU6050_REGISTER_Z_FINE_GAIN (0x05)
-#define MPU6050_REGISTER_XA_OFFS_H (0x06)
-#define MPU6050_REGISTER_XA_OFFS_L_TC (0x07)
-#define MPU6050_REGISTER_YA_OFFS_H (0x08)
-#define MPU6050_REGISTER_YA_OFFS_L_TC (0x09)
-#define MPU6050_REGISTER_ZA_OFFS_H (0x0A)
-#define MPU6050_REGISTER_ZA_OFFS_L_TC (0x0B)
-#define MPU6050_REGISTER_SELF_TEST_X (0x0D)
-#define MPU6050_REGISTER_SELF_TEST_Y (0x0E)
-#define MPU6050_REGISTER_SELF_TEST_Z (0x0F)
-#define MPU6050_REGISTER_SELF_TEST_A (0x10)
-#define MPU6050_REGISTER_XG_OFFS_USRH (0x13)
-#define MPU6050_REGISTER_XG_OFFS_USRL (0x14)
-#define MPU6050_REGISTER_YG_OFFS_USRH (0x15)
-#define MPU6050_REGISTER_YG_OFFS_USRL (0x16)
-#define MPU6050_REGISTER_ZG_OFFS_USRH (0x17)
-#define MPU6050_REGISTER_ZG_OFFS_USRL (0x18)
-#define MPU6050_REGISTER_SMPLRT_DIV (0x19)
-#define MPU6050_REGISTER_CONFIG (0x1A)
-#define MPU6050_REGISTER_GYRO_CONFIG (0x1B)
-#define MPU6050_REGISTER_ACCEL_CONFIG (0x1C)
-#define MPU6050_REGISTER_FF_THR (0x1D)
-#define MPU6050_REGISTER_FF_DUR (0x1E)
-#define MPU6050_REGISTER_MOT_THR (0x1F)
-#define MPU6050_REGISTER_MOT_DUR (0x20)
-#define MPU6050_REGISTER_ZRMOT_THR (0x21)
-#define MPU6050_REGISTER_ZRMOT_DUR (0x22)
-#define MPU6050_REGISTER_FIFO_EN (0x23)
-#define MPU6050_REGISTER_I2C_MST_CTRL (0x24)
-#define MPU6050_REGISTER_I2C_SLV0_ADDR (0x25)
-#define MPU6050_REGISTER_I2C_SLV0_REG (0x26)
-#define MPU6050_REGISTER_I2C_SLV0_CTRL (0x27)
-#define MPU6050_REGISTER_I2C_SLV1_ADDR (0x28)
-#define MPU6050_REGISTER_I2C_SLV1_REG (0x29)
-#define MPU6050_REGISTER_I2C_SLV1_CTRL (0x2A)
-#define MPU6050_REGISTER_I2C_SLV2_ADDR (0x2B)
-#define MPU6050_REGISTER_I2C_SLV2_REG (0x2C)
-#define MPU6050_REGISTER_I2C_SLV2_CTRL (0x2D)
-#define MPU6050_REGISTER_I2C_SLV3_ADDR (0x2E)
-#define MPU6050_REGISTER_I2C_SLV3_REG (0x2F)
-#define MPU6050_REGISTER_I2C_SLV3_CTRL (0x30)
-#define MPU6050_REGISTER_I2C_SLV4_ADDR (0x31)
-#define MPU6050_REGISTER_I2C_SLV4_REG (0x32)
-#define MPU6050_REGISTER_I2C_SLV4_DO (0x33)
-#define MPU6050_REGISTER_I2C_SLV4_CTRL (0x34)
-#define MPU6050_REGISTER_I2C_SLV4_DI (0x35)
-#define MPU6050_REGISTER_I2C_MST_STATUS (0x36)
-#define MPU6050_REGISTER_INT_PIN_CFG (0x37)
-#define MPU6050_REGISTER_INT_ENABLE (0x38)
-#define MPU6050_REGISTER_DMP_INT_STATUS (0x39)
-#define MPU6050_REGISTER_INT_STATUS (0x3A)
-#define MPU6050_REGISTER_ACCEL_XOUT_H (0x3B)
-#define MPU6050_REGISTER_ACCEL_XOUT_L (0x3C)
-#define MPU6050_REGISTER_ACCEL_YOUT_H (0x3D)
-#define MPU6050_REGISTER_ACCEL_YOUT_L (0x3E)
-#define MPU6050_REGISTER_ACCEL_ZOUT_H (0x3F)
-#define MPU6050_REGISTER_ACCEL_ZOUT_L (0x40)
-#define MPU6050_REGISTER_TEMP_OUT_H (0x41)
-#define MPU6050_REGISTER_TEMP_OUT_L (0x42)
-#define MPU6050_REGISTER_GYRO_XOUT_H (0x43)
-#define MPU6050_REGISTER_GYRO_XOUT_L (0x44)
-#define MPU6050_REGISTER_GYRO_YOUT_H (0x45)
-#define MPU6050_REGISTER_GYRO_YOUT_L (0x46)
-#define MPU6050_REGISTER_GYRO_ZOUT_H (0x47)
-#define MPU6050_REGISTER_GYRO_ZOUT_L (0x48)
-#define MPU6050_REGISTER_EXT_SENS_DATA_00 (0x49)
-#define MPU6050_REGISTER_EXT_SENS_DATA_01 (0x4A)
-#define MPU6050_REGISTER_EXT_SENS_DATA_02 (0x4B)
-#define MPU6050_REGISTER_EXT_SENS_DATA_03 (0x4C)
-#define MPU6050_REGISTER_EXT_SENS_DATA_04 (0x4D)
-#define MPU6050_REGISTER_EXT_SENS_DATA_05 (0x4E)
-#define MPU6050_REGISTER_EXT_SENS_DATA_06 (0x4F)
-#define MPU6050_REGISTER_EXT_SENS_DATA_07 (0x50)
-#define MPU6050_REGISTER_EXT_SENS_DATA_08 (0x51)
-#define MPU6050_REGISTER_EXT_SENS_DATA_09 (0x52)
-#define MPU6050_REGISTER_EXT_SENS_DATA_10 (0x53)
-#define MPU6050_REGISTER_EXT_SENS_DATA_11 (0x54)
-#define MPU6050_REGISTER_EXT_SENS_DATA_12 (0x55)
-#define MPU6050_REGISTER_EXT_SENS_DATA_13 (0x56)
-#define MPU6050_REGISTER_EXT_SENS_DATA_14 (0x57)
-#define MPU6050_REGISTER_EXT_SENS_DATA_15 (0x58)
-#define MPU6050_REGISTER_EXT_SENS_DATA_16 (0x59)
-#define MPU6050_REGISTER_EXT_SENS_DATA_17 (0x5A)
-#define MPU6050_REGISTER_EXT_SENS_DATA_18 (0x5B)
-#define MPU6050_REGISTER_EXT_SENS_DATA_19 (0x5C)
-#define MPU6050_REGISTER_EXT_SENS_DATA_20 (0x5D)
-#define MPU6050_REGISTER_EXT_SENS_DATA_21 (0x5E)
-#define MPU6050_REGISTER_EXT_SENS_DATA_22 (0x5F)
-#define MPU6050_REGISTER_EXT_SENS_DATA_23 (0x60)
-#define MPU6050_REGISTER_MOT_DETECT_STATUS (0x61)
-#define MPU6050_REGISTER_I2C_SLV0_DO (0x63)
-#define MPU6050_REGISTER_I2C_SLV1_DO (0x64)
-#define MPU6050_REGISTER_I2C_SLV2_DO (0x65)
-#define MPU6050_REGISTER_I2C_SLV3_DO (0x66)
-#define MPU6050_REGISTER_I2C_MST_DELAY_CTRL (0x67)
-#define MPU6050_REGISTER_SIGNAL_PATH_RESET (0x68)
-#define MPU6050_REGISTER_MOT_DETECT_CTRL (0x69)
-#define MPU6050_REGISTER_USER_CTRL (0x6A)
-#define MPU6050_REGISTER_PWR_MGMT_1 (0x6B)
-#define MPU6050_REGISTER_PWR_MGMT_2 (0x6C)
-#define MPU6050_REGISTER_BANK_SEL (0x6D)
-#define MPU6050_REGISTER_MEM_START_ADDR (0x6E)
-#define MPU6050_REGISTER_MEM_R_W (0x6F)
-#define MPU6050_REGISTER_DMP_CFG_1 (0x70)
-#define MPU6050_REGISTER_DMP_CFG_2 (0x71)
-#define MPU6050_REGISTER_FIFO_COUNTH (0x72)
-#define MPU6050_REGISTER_FIFO_COUNTL (0x73)
-#define MPU6050_REGISTER_FIFO_R_W (0x74)
-#define MPU6050_REGISTER_WHO_AM_I (0x75)
-
-// DLPF values
-#define MPU6050_DLPF_BW_256 (0x00)
-#define MPU6050_DLPF_BW_188 (0x01)
-#define MPU6050_DLPF_BW_98 (0x02)
-#define MPU6050_DLPF_BW_42 (0x03)
-#define MPU6050_DLPF_BW_20 (0x04)
-#define MPU6050_DLPF_BW_10 (0x05)
-#define MPU6050_DLPF_BW_5 (0x06)
-
-// DHPF values:
-#define MPU6050_DHPF_RESET (0x00)
-#define MPU6050_DHPF_5 (0x01)
-#define MPU6050_DHPF_2P5 (0x02)
-#define MPU6050_DHPF_1P25 (0x03)
-#define MPU6050_DHPF_0P63 (0x04)
-#define MPU6050_DHPF_HOLD (0x07)
-
-// Full scale gyroscope range:
-#define MPU6050_GYRO_FULL_SCALE_RANGE_250 (0x00)
-#define MPU6050_GYRO_FULL_SCALE_RANGE_500 (0x01)
-#define MPU6050_GYRO_FULL_SCALE_RANGE_1000 (0x02)
-#define MPU6050_GYRO_FULL_SCALE_RANGE_2000 (0x03)
-
-// Full scale accelerometer range:
-#define MPU6050_ACCEL_FULL_SCALE_RANGE_2 (0x00)
-#define MPU6050_ACCEL_FULL_SCALE_RANGE_4 (0x01)
-#define MPU6050_ACCEL_FULL_SCALE_RANGE_8 (0x02)
-#define MPU6050_ACCEL_FULL_SCALE_RANGE_16 (0x03)
-
-// Interrupt values:
-#define MPU6050_INTMODE_ACTIVEHIGH (0x00)
-#define MPU6050_INTMODE_ACTIVELOW (0x01)
-#define MPU6050_INTDRV_PUSHPULL (0x00)
-#define MPU6050_INTDRV_OPENDRAIN (0x01)
-#define MPU6050_INTLATCH_50USPULSE (0x00)
-#define MPU6050_INTLATCH_WAITCLEAR (0x01)
-#define MPU6050_INTCLEAR_STATUSREAD (0x00)
-#define MPU6050_INTCLEAR_ANYREAD (0x01)
-
-// Clock sources:
-#define MPU6050_CLOCK_INTERNAL (0x00)
-#define MPU6050_CLOCK_PLL_XGYRO (0x01)
-#define MPU6050_CLOCK_PLL_YGYRO (0x02)
-#define MPU6050_CLOCK_PLL_ZGYRO (0x03)
-#define MPU6050_CLOCK_PLL_EXTERNAL_32K (0x04)
-#define MPU6050_CLOCK_PLL_EXTERNAL_19M (0x05)
-#define MPU6050_CLOCK_KEEP_RESET (0x07)
-
-// Wake frequencies:
-#define MPU6050_WAKE_FREQ_1P25 (0x0)
-#define MPU6050_WAKE_FREQ_2P5 (0x1)
-#define MPU6050_WAKE_FREQ_5 (0x2)
-#define MPU6050_WAKE_FREQ_10 (0x3)
-
-// Decrement values:
-#define MPU6050_DETECT_DECREMENT_RESET (0x0)
-#define MPU6050_DETECT_DECREMENT_1 (0x1)
-#define MPU6050_DETECT_DECREMENT_2 (0x2)
-#define MPU6050_DETECT_DECREMENT_4 (0x3)
-
-// External sync values:
-#define MPU6050_EXT_SYNC_DISABLED (0x0)
-#define MPU6050_EXT_SYNC_TEMP_OUT_L (0x1)
-#define MPU6050_EXT_SYNC_GYRO_XOUT_L (0x2)
-#define MPU6050_EXT_SYNC_GYRO_YOUT_L (0x3)
-#define MPU6050_EXT_SYNC_GYRO_ZOUT_L (0x4)
-#define MPU6050_EXT_SYNC_ACCEL_XOUT_L (0x5)
-#define MPU6050_EXT_SYNC_ACCEL_YOUT_L (0x6)
-#define MPU6050_EXT_SYNC_ACCEL_ZOUT_L (0x7)
-
-// Clock division values:
-#define MPU6050_CLOCK_DIV_348 (0x0)
-#define MPU6050_CLOCK_DIV_333 (0x1)
-#define MPU6050_CLOCK_DIV_320 (0x2)
-#define MPU6050_CLOCK_DIV_308 (0x3)
-#define MPU6050_CLOCK_DIV_296 (0x4)
-#define MPU6050_CLOCK_DIV_286 (0x5)
-#define MPU6050_CLOCK_DIV_276 (0x6)
-#define MPU6050_CLOCK_DIV_267 (0x7)
-#define MPU6050_CLOCK_DIV_258 (0x8)
-#define MPU6050_CLOCK_DIV_500 (0x9)
-#define MPU6050_CLOCK_DIV_471 (0xA)
-#define MPU6050_CLOCK_DIV_444 (0xB)
-#define MPU6050_CLOCK_DIV_421 (0xC)
-#define MPU6050_CLOCK_DIV_400 (0xD)
-#define MPU6050_CLOCK_DIV_381 (0xE)
-#define MPU6050_CLOCK_DIV_364 (0xF)
-
-// Bit and length defines for SELF_TEST register:
-#define MPU6050_SELF_TEST_XA_1_BIT (0x07)
-#define MPU6050_SELF_TEST_XA_1_LENGTH (0x03)
-#define MPU6050_SELF_TEST_XA_2_BIT (0x05)
-#define MPU6050_SELF_TEST_XA_2_LENGTH (0x02)
-#define MPU6050_SELF_TEST_YA_1_BIT (0x07)
-#define MPU6050_SELF_TEST_YA_1_LENGTH (0x03)
-#define MPU6050_SELF_TEST_YA_2_BIT (0x03)
-#define MPU6050_SELF_TEST_YA_2_LENGTH (0x02)
-#define MPU6050_SELF_TEST_ZA_1_BIT (0x07)
-#define MPU6050_SELF_TEST_ZA_1_LENGTH (0x03)
-#define MPU6050_SELF_TEST_ZA_2_BIT (0x01)
-#define MPU6050_SELF_TEST_ZA_2_LENGTH (0x02)
-#define MPU6050_SELF_TEST_XG_1_BIT (0x04)
-#define MPU6050_SELF_TEST_XG_1_LENGTH (0x05)
-#define MPU6050_SELF_TEST_YG_1_BIT (0x04)
-#define MPU6050_SELF_TEST_YG_1_LENGTH (0x05)
-#define MPU6050_SELF_TEST_ZG_1_BIT (0x04)
-#define MPU6050_SELF_TEST_ZG_1_LENGTH (0x05)
-
-// Bit and length defines for CONFIG register:
-#define MPU6050_CFG_EXT_SYNC_SET_BIT (5)
-#define MPU6050_CFG_EXT_SYNC_SET_LENGTH (3)
-#define MPU6050_CFG_DLPF_CFG_BIT (2)
-#define MPU6050_CFG_DLPF_CFG_LENGTH (3)
-
-// Bit and length defines for GYRO_CONFIG register:
-#define MPU6050_GCONFIG_FS_SEL_BIT (4)
-#define MPU6050_GCONFIG_FS_SEL_LENGTH (2)
-
-// Bit and length defines for ACCEL_CONFIG register:
-#define MPU6050_ACONFIG_XA_ST_BIT (7)
-#define MPU6050_ACONFIG_YA_ST_BIT (6)
-#define MPU6050_ACONFIG_ZA_ST_BIT (5)
-#define MPU6050_ACONFIG_AFS_SEL_BIT (4)
-#define MPU6050_ACONFIG_AFS_SEL_LENGTH (2)
-#define MPU6050_ACONFIG_ACCEL_HPF_BIT (2)
-#define MPU6050_ACONFIG_ACCEL_HPF_LENGTH (3)
-
-// Bit and length defines for FIFO_EN register:
-#define MPU6050_TEMP_FIFO_EN_BIT (7)
-#define MPU6050_XG_FIFO_EN_BIT (6)
-#define MPU6050_YG_FIFO_EN_BIT (5)
-#define MPU6050_ZG_FIFO_EN_BIT (4)
-#define MPU6050_ACCEL_FIFO_EN_BIT (3)
-#define MPU6050_SLV2_FIFO_EN_BIT (2)
-#define MPU6050_SLV1_FIFO_EN_BIT (1)
-#define MPU6050_SLV0_FIFO_EN_BIT (0)
-
-// Bit and length defines for I2C_MST_CTRL register:
-#define MPU6050_MULT_MST_EN_BIT (7)
-#define MPU6050_WAIT_FOR_ES_BIT (6)
-#define MPU6050_SLV_3_FIFO_EN_BIT (5)
-#define MPU6050_I2C_MST_P_NSR_BIT (4)
-#define MPU6050_I2C_MST_CLK_BIT (3)
-#define MPU6050_I2C_MST_CLK_LENGTH (4)
-
-// Bit and length defines for I2C_SLV* register:
-#define MPU6050_I2C_SLV_RW_BIT (7)
-#define MPU6050_I2C_SLV_ADDR_BIT (6)
-#define MPU6050_I2C_SLV_ADDR_LENGTH (7)
-#define MPU6050_I2C_SLV_EN_BIT (7)
-#define MPU6050_I2C_SLV_BYTE_SW_BIT (6)
-#define MPU6050_I2C_SLV_REG_DIS_BIT (5)
-#define MPU6050_I2C_SLV_GRP_BIT (4)
-#define MPU6050_I2C_SLV_LEN_BIT (3)
-#define MPU6050_I2C_SLV_LEN_LENGTH (4)
-
-// Bit and length defines for I2C_SLV4 register:
-#define MPU6050_I2C_SLV4_RW_BIT (7)
-#define MPU6050_I2C_SLV4_ADDR_BIT (6)
-#define MPU6050_I2C_SLV4_ADDR_LENGTH (7)
-#define MPU6050_I2C_SLV4_EN_BIT (7)
-#define MPU6050_I2C_SLV4_INT_EN_BIT (6)
-#define MPU6050_I2C_SLV4_REG_DIS_BIT (5)
-#define MPU6050_I2C_SLV4_MST_DLY_BIT (4)
-#define MPU6050_I2C_SLV4_MST_DLY_LENGTH (5)
-
-// Bit and length defines for I2C_MST_STATUS register:
-#define MPU6050_MST_PASS_THROUGH_BIT (7)
-#define MPU6050_MST_I2C_SLV4_DONE_BIT (6)
-#define MPU6050_MST_I2C_LOST_ARB_BIT (5)
-#define MPU6050_MST_I2C_SLV4_NACK_BIT (4)
-#define MPU6050_MST_I2C_SLV3_NACK_BIT (3)
-#define MPU6050_MST_I2C_SLV2_NACK_BIT (2)
-#define MPU6050_MST_I2C_SLV1_NACK_BIT (1)
-#define MPU6050_MST_I2C_SLV0_NACK_BIT (0)
-
-// Bit and length defines for INT_PIN_CFG register:
-#define MPU6050_INTCFG_INT_LEVEL_BIT (7)
-#define MPU6050_INTCFG_INT_OPEN_BIT (6)
-#define MPU6050_INTCFG_LATCH_INT_EN_BIT (5)
-#define MPU6050_INTCFG_INT_RD_CLEAR_BIT (4)
-#define MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT (3)
-#define MPU6050_INTCFG_FSYNC_INT_EN_BIT (2)
-#define MPU6050_INTCFG_I2C_BYPASS_EN_BIT (1)
-#define MPU6050_INTCFG_CLKOUT_EN_BIT (0)
-
-// Bit and length defines for INT_ENABLE and INT_STATUS registers:
-#define MPU6050_INTERRUPT_FF_BIT (7)
-#define MPU6050_INTERRUPT_MOT_BIT (6)
-#define MPU6050_INTERRUPT_ZMOT_BIT (5)
-#define MPU6050_INTERRUPT_FIFO_OFLOW_BIT (4)
-#define MPU6050_INTERRUPT_I2C_MST_INT_BIT (3)
-#define MPU6050_INTERRUPT_PLL_RDY_INT_BIT (2)
-#define MPU6050_INTERRUPT_DMP_INT_BIT (1)
-#define MPU6050_INTERRUPT_DATA_RDY_BIT (0)
-
-// Bit and length defines for MOT_DETECT_STATUS register:
-#define MPU6050_MOTION_MOT_XNEG_BIT (7)
-#define MPU6050_MOTION_MOT_XPOS_BIT (6)
-#define MPU6050_MOTION_MOT_YNEG_BIT (5)
-#define MPU6050_MOTION_MOT_YPOS_BIT (4)
-#define MPU6050_MOTION_MOT_ZNEG_BIT (3)
-#define MPU6050_MOTION_MOT_ZPOS_BIT (2)
-#define MPU6050_MOTION_MOT_ZRMOT_BIT (0)
-
-// Bit and length defines for I2C_MST_DELAY_CTRL register:
-#define MPU6050_DLYCTRL_DELAY_ES_SHADOW_BIT (7)
-#define MPU6050_DLYCTRL_I2C_SLV4_DLY_EN_BIT (4)
-#define MPU6050_DLYCTRL_I2C_SLV3_DLY_EN_BIT (3)
-#define MPU6050_DLYCTRL_I2C_SLV2_DLY_EN_BIT (2)
-#define MPU6050_DLYCTRL_I2C_SLV1_DLY_EN_BIT (1)
-#define MPU6050_DLYCTRL_I2C_SLV0_DLY_EN_BIT (0)
-
-// Bit and length defines for SIGNAL_PATH_RESET register:
-#define MPU6050_PATHRESET_GYRO_RESET_BIT (2)
-#define MPU6050_PATHRESET_ACCEL_RESET_BIT (1)
-#define MPU6050_PATHRESET_TEMP_RESET_BIT (0)
-
-// Bit and length defines for MOT_DETECT_CTRL register:
-#define MPU6050_DETECT_ACCEL_DELAY_BIT (5)
-#define MPU6050_DETECT_ACCEL_DELAY_LENGTH (2)
-#define MPU6050_DETECT_FF_COUNT_BIT (3)
-#define MPU6050_DETECT_FF_COUNT_LENGTH (2)
-#define MPU6050_DETECT_MOT_COUNT_BIT (1)
-#define MPU6050_DETECT_MOT_COUNT_LENGTH (2)
-
-// Bit and length defines for USER_CTRL register:
-#define MPU6050_USERCTRL_DMP_EN_BIT (7)
-#define MPU6050_USERCTRL_FIFO_EN_BIT (6)
-#define MPU6050_USERCTRL_I2C_MST_EN_BIT (5)
-#define MPU6050_USERCTRL_I2C_IF_DIS_BIT (4)
-#define MPU6050_USERCTRL_DMP_RESET_BIT (3)
-#define MPU6050_USERCTRL_FIFO_RESET_BIT (2)
-#define MPU6050_USERCTRL_I2C_MST_RESET_BIT (1)
-#define MPU6050_USERCTRL_SIG_COND_RESET_BIT (0)
-
-// Bit and length defines for PWR_MGMT_1 register:
-#define MPU6050_PWR1_DEVICE_RESET_BIT (7)
-#define MPU6050_PWR1_SLEEP_BIT (6)
-#define MPU6050_PWR1_CYCLE_BIT (5)
-#define MPU6050_PWR1_TEMP_DIS_BIT (3)
-#define MPU6050_PWR1_CLKSEL_BIT (2)
-#define MPU6050_PWR1_CLKSEL_LENGTH (3)
-
-// Bit and length defines for PWR_MGMT_2 register:
-#define MPU6050_PWR2_LP_WAKE_CTRL_BIT (7)
-#define MPU6050_PWR2_LP_WAKE_CTRL_LENGTH (2)
-#define MPU6050_PWR2_STBY_XA_BIT (5)
-#define MPU6050_PWR2_STBY_YA_BIT (4)
-#define MPU6050_PWR2_STBY_ZA_BIT (3)
-#define MPU6050_PWR2_STBY_XG_BIT (2)
-#define MPU6050_PWR2_STBY_YG_BIT (1)
-#define MPU6050_PWR2_STBY_ZG_BIT (0)
-
-// Bit and length defines for WHO_AM_I register:
-#define MPU6050_WHO_AM_I_BIT (6)
-#define MPU6050_WHO_AM_I_LENGTH (6)
-
-// Undocumented bits and lengths:
-#define MPU6050_TC_PWR_MODE_BIT (7)
-#define MPU6050_TC_OFFSET_BIT (6)
-#define MPU6050_TC_OFFSET_LENGTH (6)
-#define MPU6050_TC_OTP_BNK_VLD_BIT (0)
-#define MPU6050_DMPINT_5_BIT (5)
-#define MPU6050_DMPINT_4_BIT (4)
-#define MPU6050_DMPINT_3_BIT (3)
-#define MPU6050_DMPINT_2_BIT (2)
-#define MPU6050_DMPINT_1_BIT (1)
-#define MPU6050_DMPINT_0_BIT (0)
-
-#define PI (3.14159265358979323846f)
-#define GYRO_MEAS_ERROR (PI * (60.0f / 180.0f))
-#define GYRO_MEAS_DRIFT (PI * (1.0f / 180.0f))
-#define BETA (sqrt(3.0f / 4.0f) * GYRO_MEAS_ERROR)
-#define ZETA (sqrt(3.0f / 4.0f) * GYRO_MEAS_DRIFT)
-
-#define ENABLE (ESP_OK)
-#define DISABLE (ESP_FAIL)
-
// Max 1MHz for esp-idf, but device supports up to 1.7Mhz
#define I2C_FREQ_HZ (1000000)
-/*
- * Macro which can be used to check parameters.
- * Prints the error code, error location, and the failed statement to serial output.
- */
-#define ESP_PARAM_CHECK(con) \
- do \
- { \
- if (!(con)) \
- { \
- ESP_LOGE(TAG, "[%s, %d]: !(%s)", __func__, __LINE__, #con); \
- return ESP_ERR_INVALID_ARG; \
- } \
- } \
- while (0)
+#define CHECK(x) do { esp_err_t __; if ((__ = x) != ESP_OK) return __; } while (0)
+#define CHECK_ARG(VAL) do { if (!(VAL)) return ESP_ERR_INVALID_ARG; } while (0)
-/*
- * Macro which can be used to check the error code,
- * and terminate the program in case the code is not ESP_OK.
- */
-#define ESP_ERROR_RETURN(ret) \
- do \
- { \
- esp_err_t __ = ret; \
- if (__ != ESP_OK) \
- { \
- return __; \
- } \
- } \
- while (0)
+static const char *TAG = "mpu6050";
-/**
- * Macro which can be used to check the error code,
- * and terminate the program in case the code is not ESP_OK.
- * In debug mode, it prints the error code, error location to serial output.
- */
-#define ESP_ERROR_CDEBUG(ret) \
- if (ret != ESP_OK) \
- { \
- ESP_LOGD(TAG, "[%s, %d] <%s> ", __func__, __LINE__, esp_err_to_name(ret)); \
- return ret; \
- }
+static const float accel_res[] = {
+ [MPU6050_ACCEL_RANGE_2] = 2.0f / 32768.0f,
+ [MPU6050_ACCEL_RANGE_4] = 4.0f / 32768.0f,
+ [MPU6050_ACCEL_RANGE_8] = 8.0f / 32768.0f,
+ [MPU6050_ACCEL_RANGE_16] = 16.0f / 32768.0f,
+};
-static float quart[4] = { 1.0f, 0.0f, 0.0f, 0.0f };
-static float delta_t = 0.0f;
+static const float gyro_res[] = {
+ [MPU6050_GYRO_RANGE_250] = 250.0f / 32768.0f,
+ [MPU6050_GYRO_RANGE_500] = 500.0f / 32768.0f,
+ [MPU6050_GYRO_RANGE_1000] = 1000.0f / 32768.0f,
+ [MPU6050_GYRO_RANGE_2000] = 2000.0f / 32768.0f,
+};
-/* ----------------------------------------------------- */
-static esp_err_t i2c_read_bytes(i2c_dev_t *dev, uint8_t reg_addr, size_t size, uint8_t *data)
+inline static float get_accel_value(mpu6050_dev_t *dev, int16_t raw)
{
- I2C_DEV_TAKE_MUTEX(dev);
- I2C_DEV_CHECK(dev, i2c_dev_read(dev, &(reg_addr), sizeof(uint8_t), data, size));
- I2C_DEV_GIVE_MUTEX(dev);
+ return (float)raw / accel_res[dev->ranges.accel];
+}
- return ESP_OK;
+inline static float get_gyro_value(mpu6050_dev_t *dev, int16_t raw)
+{
+ return (float)raw / gyro_res[dev->ranges.gyro];
}
-/* ----------------------------------------------------- */
-static esp_err_t i2c_read_byte(i2c_dev_t *dev, uint8_t reg_addr, uint8_t *data)
+
+inline static int16_t shuffle(uint16_t word)
{
- I2C_DEV_TAKE_MUTEX(dev);
- I2C_DEV_CHECK(dev, i2c_dev_read_reg(dev, reg_addr, (void *)data, 1));
- I2C_DEV_GIVE_MUTEX(dev);
+ return (int16_t)((word >> 8) | (word << 8));
+}
- return ESP_OK;
+inline static uint16_t ushuffle(uint16_t word)
+{
+ return ((word >> 8) | (word << 8));
}
-/* ----------------------------------------------------- */
-static esp_err_t i2c_read_bits(i2c_dev_t *dev, uint8_t reg_addr, uint8_t bit_start, uint8_t size, uint8_t *data)
+
+static esp_err_t read_reg_bits(mpu6050_dev_t *dev, uint8_t reg_addr, uint8_t offset, uint8_t mask, uint8_t *value)
{
- I2C_DEV_TAKE_MUTEX(dev);
+ CHECK_ARG(dev && value && mask);
- uint8_t bit;
- I2C_DEV_CHECK(dev, i2c_dev_read_reg(dev, reg_addr, (void *)&(bit), 1));
+ uint8_t buf;
- uint8_t mask = ((1 << size) - 1) << (bit_start - size + 1);
- bit &= mask;
- bit >>= (bit_start - size + 1);
- *data = bit;
- I2C_DEV_GIVE_MUTEX(dev);
+ I2C_DEV_TAKE_MUTEX(&dev->i2c_dev);
+ I2C_DEV_CHECK(&dev->i2c_dev, i2c_dev_read_reg(&dev->i2c_dev, reg_addr, &buf, 1));
+ I2C_DEV_GIVE_MUTEX(&dev->i2c_dev);
+
+ *value = (buf & mask) >> offset;
return ESP_OK;
}
-/* ----------------------------------------------------- */
-static esp_err_t i2c_read_bit(i2c_dev_t *dev, uint8_t reg_addr, uint8_t bit_number, uint8_t *data)
+
+static esp_err_t write_reg_bits(mpu6050_dev_t *dev, uint8_t reg_addr, uint8_t offset, uint8_t mask, uint8_t value)
{
- I2C_DEV_TAKE_MUTEX(dev);
+ CHECK_ARG(dev && mask);
+
+ uint8_t buf;
+
+ I2C_DEV_TAKE_MUTEX(&dev->i2c_dev);
+ I2C_DEV_CHECK(&dev->i2c_dev, i2c_dev_read_reg(&dev->i2c_dev, reg_addr, &buf, 1));
- uint8_t bit;
- I2C_DEV_CHECK(dev, i2c_dev_read_reg(dev, reg_addr, (void *)&(bit), 1));
- *data = bit & (1 << bit_number);
- I2C_DEV_GIVE_MUTEX(dev);
+ buf = (buf & ~mask) | ((value << offset) & mask);
+
+ I2C_DEV_CHECK(&dev->i2c_dev, i2c_dev_write_reg(&dev->i2c_dev, reg_addr, &buf, 1));
+ I2C_DEV_GIVE_MUTEX(&dev->i2c_dev);
return ESP_OK;
}
-/* ----------------------------------------------------- */
-// static esp_err_t i2c_write_bytes(i2c_dev_t *dev, uint8_t reg_addr, uint8_t *data, uint8_t size)
-// {
-// I2C_DEV_TAKE_MUTEX(dev);
-// I2C_DEV_CHECK(dev, i2c_dev_write(dev, &(reg_addr), sizeof(reg_addr), data, size));
-// I2C_DEV_GIVE_MUTEX(dev);
-// return ESP_OK;
-// }
-/* ----------------------------------------------------- */
-static esp_err_t i2c_write_byte(i2c_dev_t *dev, uint8_t reg_addr, uint8_t data)
-{
- I2C_DEV_TAKE_MUTEX(dev);
- I2C_DEV_CHECK(dev, i2c_dev_write_reg(dev, reg_addr, &(data), 1));
- I2C_DEV_GIVE_MUTEX(dev);
+
+static esp_err_t read_reg(mpu6050_dev_t *dev, uint8_t reg_addr, uint8_t *value)
+{
+ CHECK_ARG(dev && value);
+
+ I2C_DEV_TAKE_MUTEX(&dev->i2c_dev);
+ I2C_DEV_CHECK(&dev->i2c_dev, i2c_dev_read_reg(&dev->i2c_dev, reg_addr, value, 1));
+ I2C_DEV_GIVE_MUTEX(&dev->i2c_dev);
return ESP_OK;
}
-/* ----------------------------------------------------- */
-static esp_err_t i2c_write_bits(i2c_dev_t *dev, uint8_t reg_addr, uint8_t bit_start, uint8_t size, uint8_t data)
+
+static esp_err_t write_reg(mpu6050_dev_t *dev, uint8_t reg_addr, uint8_t data)
{
- I2C_DEV_TAKE_MUTEX(dev);
- uint8_t bit = 0;
- I2C_DEV_CHECK(dev, i2c_dev_read_reg(dev, reg_addr, (void *)&(bit), 1));
+ CHECK_ARG(dev);
- uint8_t mask = ((1 << size) - 1) << (bit_start - size + 1);
- data <<= (bit_start - size + 1);
- data &= mask;
- bit &= ~(mask);
- bit |= data;
- I2C_DEV_CHECK(dev, i2c_dev_write_reg(dev, reg_addr, &(bit), 1));
- I2C_DEV_GIVE_MUTEX(dev);
+ I2C_DEV_TAKE_MUTEX(&dev->i2c_dev);
+ I2C_DEV_CHECK(&dev->i2c_dev, i2c_dev_write_reg(&dev->i2c_dev, reg_addr, &data, 1));
+ I2C_DEV_GIVE_MUTEX(&dev->i2c_dev);
return ESP_OK;
}
-/* ----------------------------------------------------- */
-static esp_err_t i2c_write_bit(i2c_dev_t *dev, uint8_t reg_addr, uint8_t bit_number, uint8_t data)
+
+static esp_err_t read_reg_word(mpu6050_dev_t *dev, uint8_t reg_addr, int16_t *value)
{
- I2C_DEV_TAKE_MUTEX(dev);
- uint8_t bit;
+ CHECK_ARG(dev && value);
- I2C_DEV_CHECK(dev, i2c_dev_read_reg(dev, reg_addr, (void *)&(bit), 1));
+ uint16_t buf;
- if (data != 0)
- {
- bit = (bit | (1 << bit_number));
- }
- else
- {
- bit = (bit & ~(1 << bit_number));
- }
- I2C_DEV_CHECK(dev, i2c_dev_write_reg(dev, reg_addr, &(bit), 1));
- I2C_DEV_GIVE_MUTEX(dev);
+ I2C_DEV_TAKE_MUTEX(&dev->i2c_dev);
+ I2C_DEV_CHECK(&dev->i2c_dev, i2c_dev_read_reg(&dev->i2c_dev, reg_addr, &buf, 2));
+ I2C_DEV_GIVE_MUTEX(&dev->i2c_dev);
+
+ *value = shuffle(buf);
return ESP_OK;
}
-/* ----------------------------------------------------- */
-static esp_err_t i2c_write_word(i2c_dev_t *dev, uint8_t reg_addr, uint8_t data)
+
+static esp_err_t write_reg_word(mpu6050_dev_t *dev, uint8_t reg_addr, int16_t value)
{
- I2C_DEV_TAKE_MUTEX(dev);
- uint8_t data_1[] = { (uint8_t)(data >> 8), (uint8_t)(data & 0xFF) };
- I2C_DEV_CHECK(dev, i2c_dev_write(dev, &(reg_addr), sizeof(reg_addr), (uint8_t *)&(data_1), 2));
- I2C_DEV_GIVE_MUTEX(dev);
+ CHECK_ARG(dev);
+
+ uint16_t buf = ushuffle(value);
+
+ I2C_DEV_TAKE_MUTEX(&dev->i2c_dev);
+ I2C_DEV_CHECK(&dev->i2c_dev, i2c_dev_read_reg(&dev->i2c_dev, reg_addr, &buf, 2));
+ I2C_DEV_GIVE_MUTEX(&dev->i2c_dev);
return ESP_OK;
}
-/**********************************************************/
-
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_aux_vddio_level(mpu6050_dev_t *setting, uint8_t *level)
+static esp_err_t read_reg_bool(mpu6050_dev_t *dev, uint8_t reg_addr, uint8_t offset, bool *value)
{
- return (i2c_read_bit(setting, MPU6050_REGISTER_YG_OFFS_TC, MPU6050_TC_PWR_MODE_BIT, level));
+ CHECK_ARG(value);
+
+ uint8_t buf;
+ CHECK(read_reg_bits(dev, reg_addr, offset, BIT(offset), &buf));
+ *value = buf != 0 ? true : false;
+
+ return ESP_OK;
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_aux_vddio_level(mpu6050_dev_t *setting, uint8_t level)
+
+static inline esp_err_t write_reg_bool(mpu6050_dev_t *dev, uint8_t reg_addr, uint8_t offset, bool value)
{
- return (i2c_write_bit(setting, MPU6050_REGISTER_YG_OFFS_TC, MPU6050_TC_PWR_MODE_BIT, level));
+ return write_reg_bits(dev, reg_addr, offset, BIT(offset), value ? 1 : 0);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_rate(mpu6050_dev_t *setting, uint8_t *rate)
+
+////////////////////////////////////////////////////////////////////////////////
+
+esp_err_t mpu6050_init_desc(mpu6050_dev_t *dev, uint8_t addr, i2c_port_t port, gpio_num_t sda_gpio, gpio_num_t scl_gpio)
{
- return (i2c_read_byte(setting, MPU6050_REGISTER_SMPLRT_DIV, rate));
+ CHECK_ARG(dev);
+
+ if (addr != MPU6050_I2C_ADDRESS_LOW && addr != MPU6050_I2C_ADDRESS_HIGH)
+ {
+ ESP_LOGE(TAG, "Invalid device address: 0x%02x", addr);
+ return ESP_ERR_INVALID_ARG;
+ }
+
+ dev->i2c_dev.port = port;
+ dev->i2c_dev.addr = addr;
+ dev->i2c_dev.cfg.sda_io_num = sda_gpio;
+ dev->i2c_dev.cfg.scl_io_num = scl_gpio;
+#if HELPER_TARGET_IS_ESP32
+ dev->i2c_dev.cfg.master.clk_speed = I2C_FREQ_HZ;
+#endif
+
+ return i2c_dev_create_mutex(&dev->i2c_dev);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_rate(mpu6050_dev_t *setting, uint8_t rate)
+
+esp_err_t mpu6050_free_desc(mpu6050_dev_t *dev)
{
- return (i2c_write_byte(setting, MPU6050_REGISTER_SMPLRT_DIV, rate));
+ CHECK_ARG(dev);
+
+ return i2c_dev_delete_mutex(&dev->i2c_dev);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_external_frame_sync(mpu6050_dev_t *setting, uint8_t *sync)
+
+esp_err_t mpu6050_init(mpu6050_dev_t *dev)
{
- return (i2c_read_bits(setting, MPU6050_REGISTER_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT,
- MPU6050_CFG_EXT_SYNC_SET_LENGTH, sync));
+ CHECK_ARG(dev);
+
+ CHECK(mpu6050_set_clock_source(dev, MPU6050_CLOCK_PLL_X));
+ CHECK(mpu6050_set_full_scale_gyro_range(dev, MPU6050_GYRO_RANGE_250));
+ CHECK(mpu6050_set_full_scale_accel_range(dev, MPU6050_ACCEL_RANGE_2));
+ CHECK(mpu6050_set_sleep_enabled(dev, false));
+
+ return ESP_OK;
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_external_frame_sync(mpu6050_dev_t *setting, uint8_t sync)
+
+esp_err_t mpu6050_get_aux_vddio_level(mpu6050_dev_t *dev, mpu6050_vddio_level_t *level)
{
- return (i2c_write_bits(setting, MPU6050_REGISTER_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT,
- MPU6050_CFG_EXT_SYNC_SET_LENGTH, sync));
+ return read_reg_bits(dev, MPU6050_REGISTER_YG_OFFS_TC, MPU6050_TC_PWR_MODE_BIT, BIT(MPU6050_TC_PWR_MODE_BIT), (uint8_t *)level);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_dlpf_mode(mpu6050_dev_t *setting, uint8_t *mode)
+
+esp_err_t mpu6050_set_aux_vddio_level(mpu6050_dev_t *dev, mpu6050_vddio_level_t level)
{
- return (
- i2c_read_bits(setting, MPU6050_REGISTER_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH, mode));
+ return write_reg_bits(dev, MPU6050_REGISTER_YG_OFFS_TC, MPU6050_TC_PWR_MODE_BIT, BIT(MPU6050_TC_PWR_MODE_BIT), level);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_dlpf_mode(mpu6050_dev_t *setting, uint8_t mode)
+
+esp_err_t mpu6050_get_rate(mpu6050_dev_t *dev, uint8_t *rate)
{
- return (
- i2c_write_bits(setting, MPU6050_REGISTER_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH, mode));
+ return read_reg(dev, MPU6050_REGISTER_SMPLRT_DIV, rate);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_full_scale_gyro_range(mpu6050_dev_t *setting, uint8_t *gyro_range)
+
+esp_err_t mpu6050_set_rate(mpu6050_dev_t *dev, uint8_t rate)
{
- return (i2c_read_bits(setting, MPU6050_REGISTER_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT,
- MPU6050_GCONFIG_FS_SEL_LENGTH, gyro_range));
+ return write_reg(dev, MPU6050_REGISTER_SMPLRT_DIV, rate);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_full_scale_gyro_range(mpu6050_dev_t *setting, uint8_t range)
+
+esp_err_t mpu6050_get_external_frame_sync(mpu6050_dev_t *dev, mpu6050_ext_sync_t *sync)
{
- return (i2c_write_bits(setting, MPU6050_REGISTER_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT,
- MPU6050_GCONFIG_FS_SEL_LENGTH, range));
+ return read_reg_bits(dev, MPU6050_REGISTER_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT, MPU6050_CFG_EXT_SYNC_SET_MASK, (uint8_t *)sync);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_accel_x_self_test_factory_trim(mpu6050_dev_t *setting, uint8_t *accel_x)
+
+esp_err_t mpu6050_set_external_frame_sync(mpu6050_dev_t *dev, mpu6050_ext_sync_t sync)
{
- uint8_t a = 0;
- ESP_ERROR_RETURN(i2c_read_byte(setting, MPU6050_REGISTER_SELF_TEST_X, accel_x));
- ESP_ERROR_RETURN(i2c_read_byte(setting, MPU6050_REGISTER_SELF_TEST_A, &a));
- *accel_x = ((*accel_x) >> 3) | ((a >> 4) & 0x03);
- return ESP_OK;
+ return write_reg_bits(dev, MPU6050_REGISTER_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT, MPU6050_CFG_EXT_SYNC_SET_MASK, sync);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_accel_y_self_test_factory_trim(mpu6050_dev_t *setting, uint8_t *accel_y)
+
+esp_err_t mpu6050_get_dlpf_mode(mpu6050_dev_t *dev, mpu6050_dlpf_mode_t *mode)
{
- uint8_t a = 0;
- ESP_ERROR_RETURN(i2c_read_byte(setting, MPU6050_REGISTER_SELF_TEST_Y, accel_y));
- ESP_ERROR_RETURN(i2c_read_byte(setting, MPU6050_REGISTER_SELF_TEST_A, &a));
- *accel_y = ((*accel_y) >> 3) | ((a >> 2) & 0x03);
- return ESP_OK;
+ return read_reg_bits(dev, MPU6050_REGISTER_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_MASK, (uint8_t *)mode);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_accel_z_self_test_factory_trim(mpu6050_dev_t *setting, uint8_t *accel_z)
+
+esp_err_t mpu6050_set_dlpf_mode(mpu6050_dev_t *dev, mpu6050_dlpf_mode_t mode)
{
- uint8_t a = 0;
- ESP_ERROR_RETURN(i2c_read_byte(setting, MPU6050_REGISTER_SELF_TEST_Z, accel_z));
- ESP_ERROR_RETURN(i2c_read_byte(setting, MPU6050_REGISTER_SELF_TEST_A, &a));
- *accel_z = ((*accel_z) >> 3) | (a & 0x03);
- return ESP_OK;
+ return write_reg_bits(dev, MPU6050_REGISTER_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_MASK, mode);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_gyro_x_self_test_factory_trim(mpu6050_dev_t *setting, uint8_t *gypr_x)
+
+esp_err_t mpu6050_get_full_scale_gyro_range(mpu6050_dev_t *dev, mpu6050_gyro_range_t *range)
{
- ESP_ERROR_RETURN(i2c_read_byte(setting, MPU6050_REGISTER_SELF_TEST_X, gypr_x));
- *gypr_x = ((*gypr_x) & 0x1F);
- return ESP_OK;
+ esp_err_t res = read_reg_bits(dev, MPU6050_REGISTER_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_MASK, (uint8_t *)range);
+ if (res == ESP_OK)
+ dev->ranges.gyro = *range;
+
+ return res;
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_gyro_y_self_test_factory_trim(mpu6050_dev_t *setting, uint8_t *gypr_y)
+
+esp_err_t mpu6050_set_full_scale_gyro_range(mpu6050_dev_t *dev, mpu6050_gyro_range_t range)
{
- ESP_ERROR_RETURN(i2c_read_byte(setting, MPU6050_REGISTER_SELF_TEST_Y, gypr_y));
- *gypr_y = (*gypr_y & 0x1F);
- return ESP_OK;
+ CHECK_ARG(range <= MPU6050_GYRO_RANGE_2000);
+
+ esp_err_t res = write_reg_bits(dev, MPU6050_REGISTER_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_MASK, range);
+ if (res == ESP_OK)
+ dev->ranges.gyro = range;
+
+ return res;
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_gyro_z_self_test_factory_trim(mpu6050_dev_t *setting, uint8_t *gypr_z)
+
+esp_err_t mpu6050_get_accel_self_test_factory_trim(mpu6050_dev_t *dev, mpu6050_axis_t axis, uint8_t *trim)
{
- ESP_ERROR_RETURN(i2c_read_byte(setting, MPU6050_REGISTER_SELF_TEST_Z, gypr_z));
- *gypr_z = (*gypr_z & 0x1F);
+ CHECK_ARG(dev && trim && axis <= MPU6050_Z_AXIS);
+
+ static const struct { uint8_t r, s; } regs[] = {
+ [MPU6050_X_AXIS] = { .r = MPU6050_REGISTER_SELF_TEST_X, .s = 4 },
+ [MPU6050_Y_AXIS] = { .r = MPU6050_REGISTER_SELF_TEST_Y, .s = 2 },
+ [MPU6050_Z_AXIS] = { .r = MPU6050_REGISTER_SELF_TEST_Z, .s = 0 },
+ };
+
+ uint8_t a = 0;
+ I2C_DEV_TAKE_MUTEX(&dev->i2c_dev);
+ I2C_DEV_CHECK(&dev->i2c_dev, i2c_dev_read_reg(&dev->i2c_dev, regs[axis].r, trim, 1));
+ I2C_DEV_CHECK(&dev->i2c_dev, i2c_dev_read_reg(&dev->i2c_dev, MPU6050_REGISTER_SELF_TEST_A, &a, 1));
+ I2C_DEV_GIVE_MUTEX(&dev->i2c_dev);
+
+ *trim = ((*trim) >> 3) | ((a >> regs[axis].s) & 0x03);
+
return ESP_OK;
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_accel_x_self_test(mpu6050_dev_t *setting)
-{
- uint8_t byte = 0;
- ESP_ERROR_RETURN(i2c_read_bit(setting, MPU6050_REGISTER_ACCEL_CONFIG, MPU6050_ACONFIG_XA_ST_BIT, &(byte)));
- return ((byte == 0) ? DISABLE : ENABLE);
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_accel_x_self_test(mpu6050_dev_t *setting, bool enabled)
-{
- return (i2c_write_bit(setting, MPU6050_REGISTER_ACCEL_CONFIG, MPU6050_ACONFIG_XA_ST_BIT, enabled));
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_accel_y_self_test(mpu6050_dev_t *setting)
+
+esp_err_t mpu6050_get_gyro_self_test_factory_trim(mpu6050_dev_t *dev, mpu6050_axis_t axis, uint8_t *trim)
{
- uint8_t byte = 0;
- ESP_ERROR_RETURN(i2c_read_bit(setting, MPU6050_REGISTER_ACCEL_CONFIG, MPU6050_ACONFIG_YA_ST_BIT, &(byte)));
+ CHECK_ARG(axis <= MPU6050_Z_AXIS);
+
+ static const uint8_t regs[] = {
+ [MPU6050_X_AXIS] = MPU6050_REGISTER_SELF_TEST_X,
+ [MPU6050_Y_AXIS] = MPU6050_REGISTER_SELF_TEST_Y,
+ [MPU6050_Z_AXIS] = MPU6050_REGISTER_SELF_TEST_Z,
+ };
- return ((byte == 0) ? DISABLE : ENABLE);
+ CHECK(read_reg(dev, regs[axis], trim));
+ *trim = *trim & 0x1f;
+
+ return ESP_OK;
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_accel_y_self_test(mpu6050_dev_t *setting, bool enabled)
+
+static const uint8_t accel_sta_bits[] = {
+ [MPU6050_X_AXIS] = MPU6050_ACONFIG_XA_ST_BIT,
+ [MPU6050_Y_AXIS] = MPU6050_ACONFIG_YA_ST_BIT,
+ [MPU6050_Z_AXIS] = MPU6050_ACONFIG_ZA_ST_BIT,
+};
+
+esp_err_t mpu6050_get_accel_self_test(mpu6050_dev_t *dev, mpu6050_axis_t axis, bool *enabled)
{
- return (i2c_write_bit(setting, MPU6050_REGISTER_ACCEL_CONFIG, MPU6050_ACONFIG_YA_ST_BIT, enabled));
+ CHECK_ARG(axis <= MPU6050_Z_AXIS);
+
+ return read_reg_bool(dev, MPU6050_REGISTER_ACCEL_CONFIG, accel_sta_bits[axis], enabled);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_accel_z_self_test(mpu6050_dev_t *setting)
+
+esp_err_t mpu6050_set_accel_self_test(mpu6050_dev_t *dev, mpu6050_axis_t axis, bool enabled)
{
- uint8_t byte = 0;
- ESP_ERROR_RETURN(i2c_read_bit(setting, MPU6050_REGISTER_ACCEL_CONFIG, MPU6050_ACONFIG_ZA_ST_BIT, &(byte)));
+ CHECK_ARG(axis <= MPU6050_Z_AXIS);
- return ((byte == 0) ? DISABLE : ENABLE);
+ return write_reg_bool(dev, MPU6050_REGISTER_ACCEL_CONFIG, accel_sta_bits[axis], enabled);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_accel_z_self_test(mpu6050_dev_t *setting, bool enabled)
+
+esp_err_t mpu6050_get_full_scale_accel_range(mpu6050_dev_t *dev, mpu6050_accel_range_t *range)
{
- return (i2c_write_bit(setting, MPU6050_REGISTER_ACCEL_CONFIG, MPU6050_ACONFIG_ZA_ST_BIT, enabled));
+ esp_err_t res = read_reg_bits(dev, MPU6050_REGISTER_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_MASK, (uint8_t *)range);
+ if (res == ESP_OK)
+ dev->ranges.accel = *range;
+
+ return res;
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_full_scale_accel_range(mpu6050_dev_t *setting)
+
+esp_err_t mpu6050_set_full_scale_accel_range(mpu6050_dev_t *dev, mpu6050_accel_range_t range)
{
- uint8_t byte = 0;
- ESP_ERROR_RETURN(i2c_read_bits(setting, MPU6050_REGISTER_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT,
- MPU6050_ACONFIG_AFS_SEL_LENGTH, &(byte)));
+ CHECK_ARG(range <= MPU6050_ACCEL_RANGE_16);
+
+ esp_err_t res = write_reg_bits(dev, MPU6050_REGISTER_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_MASK, range);
+ if (res == ESP_OK)
+ dev->ranges.accel = range;
- return ((byte == 0) ? DISABLE : ENABLE);
+ return res;
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_full_scale_accel_range(mpu6050_dev_t *setting, uint8_t range)
+
+esp_err_t mpu6050_get_dhpf_mode(mpu6050_dev_t *dev, mpu6050_dhpf_mode_t *mode)
{
- return (i2c_write_bits(setting, MPU6050_REGISTER_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT,
- MPU6050_ACONFIG_AFS_SEL_LENGTH, range));
+ return read_reg_bits(dev, MPU6050_REGISTER_ACCEL_CONFIG, MPU6050_ACONFIG_ACCEL_HPF_BIT, MPU6050_ACONFIG_ACCEL_HPF_MASK, (uint8_t *)mode);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_dhpf_mode(mpu6050_dev_t *setting)
-{
- uint8_t byte = 0;
- ESP_ERROR_RETURN(i2c_read_bits(setting, MPU6050_REGISTER_ACCEL_CONFIG, MPU6050_ACONFIG_ACCEL_HPF_BIT,
- MPU6050_ACONFIG_ACCEL_HPF_LENGTH, &(byte)));
- return ((byte == 0) ? DISABLE : ENABLE);
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_dhpf_mode(mpu6050_dev_t *setting, uint8_t mode)
+esp_err_t mpu6050_set_dhpf_mode(mpu6050_dev_t *dev, mpu6050_dhpf_mode_t mode)
{
- return (i2c_write_bits(setting, MPU6050_REGISTER_ACCEL_CONFIG, MPU6050_ACONFIG_ACCEL_HPF_BIT,
- MPU6050_ACONFIG_ACCEL_HPF_LENGTH, mode));
+ return write_reg_bits(dev, MPU6050_REGISTER_ACCEL_CONFIG, MPU6050_ACONFIG_ACCEL_HPF_BIT, MPU6050_ACONFIG_ACCEL_HPF_MASK, mode);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_freefall_detection_threshold(mpu6050_dev_t *setting, uint8_t *threshold)
+
+esp_err_t mpu6050_get_freefall_detection_threshold(mpu6050_dev_t *dev, uint8_t *threshold)
{
- return (i2c_read_byte(setting, MPU6050_REGISTER_FF_THR, threshold));
+ return read_reg(dev, MPU6050_REGISTER_FF_THR, threshold);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_freefall_detection_threshold(mpu6050_dev_t *setting, uint8_t threshold)
+
+esp_err_t mpu6050_set_freefall_detection_threshold(mpu6050_dev_t *dev, uint8_t threshold)
{
- return (i2c_write_byte(setting, MPU6050_REGISTER_FF_THR, threshold));
+ return write_reg(dev, MPU6050_REGISTER_FF_THR, threshold);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_freefall_detection_duration(mpu6050_dev_t *setting, uint8_t *duration)
+
+esp_err_t mpu6050_get_freefall_detection_duration(mpu6050_dev_t *dev, uint8_t *duration_ms)
{
- return (i2c_read_byte(setting, MPU6050_REGISTER_FF_DUR, duration));
+ return read_reg(dev, MPU6050_REGISTER_FF_DUR, duration_ms);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_freefall_detection_duration(mpu6050_dev_t *setting, uint8_t duration)
+
+esp_err_t mpu6050_set_freefall_detection_duration(mpu6050_dev_t *dev, uint8_t duration_ms)
{
- return (i2c_write_byte(setting, MPU6050_REGISTER_FF_DUR, duration));
+ return write_reg(dev, MPU6050_REGISTER_FF_DUR, duration_ms);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_motion_detection_threshold(mpu6050_dev_t *setting, uint8_t *threshold)
+
+esp_err_t mpu6050_get_motion_detection_threshold(mpu6050_dev_t *dev, uint8_t *threshold)
{
- return (i2c_read_byte(setting, MPU6050_REGISTER_MOT_THR, threshold));
+ return read_reg(dev, MPU6050_REGISTER_MOT_THR, threshold);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_motion_detection_threshold(mpu6050_dev_t *setting, uint8_t threshold)
+
+esp_err_t mpu6050_set_motion_detection_threshold(mpu6050_dev_t *dev, uint8_t threshold)
{
- return (i2c_write_byte(setting, MPU6050_REGISTER_MOT_THR, threshold));
+ return write_reg(dev, MPU6050_REGISTER_MOT_THR, threshold);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_motion_detection_duration(mpu6050_dev_t *setting, uint8_t *duration)
+
+esp_err_t mpu6050_get_motion_detection_duration(mpu6050_dev_t *dev, uint8_t *duration)
{
- return (i2c_read_byte(setting, MPU6050_REGISTER_MOT_DUR, duration));
+ return read_reg(dev, MPU6050_REGISTER_MOT_DUR, duration);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_motion_detection_duration(mpu6050_dev_t *setting, uint8_t duration)
+
+esp_err_t mpu6050_set_motion_detection_duration(mpu6050_dev_t *dev, uint8_t duration)
{
- return (i2c_write_byte(setting, MPU6050_REGISTER_MOT_DUR, duration));
+ return write_reg(dev, MPU6050_REGISTER_MOT_DUR, duration);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_zero_motion_detection_threshold(mpu6050_dev_t *setting, uint8_t *threshold)
+
+esp_err_t mpu6050_get_zero_motion_detection_threshold(mpu6050_dev_t *dev, uint8_t *threshold)
{
- return (i2c_read_byte(setting, MPU6050_REGISTER_ZRMOT_THR, threshold));
+ return read_reg(dev, MPU6050_REGISTER_ZRMOT_THR, threshold);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_zero_motion_detection_threshold(mpu6050_dev_t *setting, uint8_t threshold)
+
+esp_err_t mpu6050_set_zero_motion_detection_threshold(mpu6050_dev_t *dev, uint8_t threshold)
{
- return (i2c_write_byte(setting, MPU6050_REGISTER_ZRMOT_THR, threshold));
+ return write_reg(dev, MPU6050_REGISTER_ZRMOT_THR, threshold);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_zero_motion_detection_duration(mpu6050_dev_t *setting, uint8_t *duration)
+
+esp_err_t mpu6050_get_zero_motion_detection_duration(mpu6050_dev_t *dev, uint8_t *duration)
{
- return (i2c_read_byte(setting, MPU6050_REGISTER_ZRMOT_DUR, duration));
+ return read_reg(dev, MPU6050_REGISTER_ZRMOT_DUR, duration);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_zero_motion_detection_duration(mpu6050_dev_t *setting, uint8_t duration)
+
+esp_err_t mpu6050_set_zero_motion_detection_duration(mpu6050_dev_t *dev, uint8_t duration)
{
- return i2c_write_byte(setting, MPU6050_REGISTER_ZRMOT_DUR, duration);
+ return write_reg(dev, MPU6050_REGISTER_ZRMOT_DUR, duration);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_temp_fifo_enabled(mpu6050_dev_t *setting)
-{
- uint8_t byte = 0;
- ESP_ERROR_RETURN(i2c_read_bit(setting, MPU6050_REGISTER_FIFO_EN, MPU6050_TEMP_FIFO_EN_BIT, &(byte)));
- return ((byte == 0) ? DISABLE : ENABLE);
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_temp_fifo_enabled(mpu6050_dev_t *setting, bool enabled)
+esp_err_t mpu6050_get_temp_fifo_enabled(mpu6050_dev_t *dev, bool *enabled)
{
- return i2c_write_bit(setting, MPU6050_REGISTER_FIFO_EN, MPU6050_TEMP_FIFO_EN_BIT, enabled);
+ return read_reg_bool(dev, MPU6050_REGISTER_FIFO_EN, MPU6050_TEMP_FIFO_EN_BIT, enabled);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_x_gyro_fifo_enabled(mpu6050_dev_t *setting)
-{
- uint8_t byte = 0;
- ESP_ERROR_RETURN(i2c_read_bit(setting, MPU6050_REGISTER_FIFO_EN, MPU6050_XG_FIFO_EN_BIT, &(byte)));
- return ((byte == 0) ? DISABLE : ENABLE);
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_x_gyro_fifo_enabled(mpu6050_dev_t *setting, bool enabled)
+esp_err_t mpu6050_set_temp_fifo_enabled(mpu6050_dev_t *dev, bool enabled)
{
- return i2c_write_bit(setting, MPU6050_REGISTER_FIFO_EN, MPU6050_XG_FIFO_EN_BIT, enabled);
+ return write_reg_bool(dev, MPU6050_REGISTER_FIFO_EN, MPU6050_TEMP_FIFO_EN_BIT, enabled);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_y_gyro_fifo_enabled(mpu6050_dev_t *setting)
-{
- uint8_t byte = 0;
- ESP_ERROR_RETURN(i2c_read_bit(setting, MPU6050_REGISTER_FIFO_EN, MPU6050_YG_FIFO_EN_BIT, &(byte)));
- return ((byte == 0) ? DISABLE : ENABLE);
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_y_gyro_fifo_enabled(mpu6050_dev_t *setting, bool enabled)
+static const uint8_t gyro_fifo_bits[] = {
+ [MPU6050_X_AXIS] = MPU6050_XG_FIFO_EN_BIT,
+ [MPU6050_Y_AXIS] = MPU6050_YG_FIFO_EN_BIT,
+ [MPU6050_Z_AXIS] = MPU6050_ZG_FIFO_EN_BIT,
+};
+
+esp_err_t mpu6050_get_gyro_fifo_enabled(mpu6050_dev_t *dev, mpu6050_axis_t axis, bool *enabled)
{
- return i2c_write_bit(setting, MPU6050_REGISTER_FIFO_EN, MPU6050_YG_FIFO_EN_BIT, enabled);
+ CHECK_ARG(axis <= MPU6050_Z_AXIS);
+
+ return read_reg_bool(dev, MPU6050_REGISTER_FIFO_EN, gyro_fifo_bits[axis], enabled);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_z_gyro_fifo_enabled(mpu6050_dev_t *setting)
+
+esp_err_t mpu6050_set_gyro_fifo_enabled(mpu6050_dev_t *dev, mpu6050_axis_t axis, bool enabled)
{
- uint8_t byte = 0;
- ESP_ERROR_RETURN(i2c_read_bit(setting, MPU6050_REGISTER_FIFO_EN, MPU6050_ZG_FIFO_EN_BIT, &(byte)));
+ CHECK_ARG(axis <= MPU6050_Z_AXIS);
- return ((byte == 0) ? DISABLE : ENABLE);
+ return write_reg_bool(dev, MPU6050_REGISTER_FIFO_EN, gyro_fifo_bits[axis], enabled);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_z_gyro_fifo_enabled(mpu6050_dev_t *setting, bool enabled)
+
+esp_err_t mpu6050_get_accel_fifo_enabled(mpu6050_dev_t *dev, bool *enabled)
{
- return i2c_write_bit(setting, MPU6050_REGISTER_FIFO_EN, MPU6050_ZG_FIFO_EN_BIT, enabled);
+ return read_reg_bool(dev, MPU6050_REGISTER_FIFO_EN, MPU6050_ACCEL_FIFO_EN_BIT, enabled);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_accel_fifo_enabled(mpu6050_dev_t *setting)
-{
- uint8_t byte = 0;
- ESP_ERROR_RETURN(i2c_read_bit(setting, MPU6050_REGISTER_FIFO_EN, MPU6050_ACCEL_FIFO_EN_BIT, &(byte)));
- return ((byte == 0) ? DISABLE : ENABLE);
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_accel_fifo_enabled(mpu6050_dev_t *setting, bool enabled)
+esp_err_t mpu6050_set_accel_fifo_enabled(mpu6050_dev_t *dev, bool enabled)
{
- return i2c_write_bit(setting, MPU6050_REGISTER_FIFO_EN, MPU6050_ACCEL_FIFO_EN_BIT, enabled);
+ return write_reg_bool(dev, MPU6050_REGISTER_FIFO_EN, MPU6050_ACCEL_FIFO_EN_BIT, enabled);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_slave_2_fifo_enabled(mpu6050_dev_t *setting)
-{
- uint8_t byte = 0;
- ESP_ERROR_RETURN(i2c_read_bit(setting, MPU6050_REGISTER_FIFO_EN, MPU6050_SLV2_FIFO_EN_BIT, &(byte)));
- return ((byte == 0) ? DISABLE : ENABLE);
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_slave_2_fifo_enabled(mpu6050_dev_t *setting, bool enabled)
+static const struct { uint8_t r, b; } slave_fifo_bits[] = {
+ [MPU6050_SLAVE_0] = { .r = MPU6050_REGISTER_FIFO_EN, .b = MPU6050_SLV2_FIFO_EN_BIT },
+ [MPU6050_SLAVE_1] = { .r = MPU6050_REGISTER_FIFO_EN, .b = MPU6050_SLV1_FIFO_EN_BIT },
+ [MPU6050_SLAVE_2] = { .r = MPU6050_REGISTER_FIFO_EN, .b = MPU6050_SLV0_FIFO_EN_BIT },
+ [MPU6050_SLAVE_3] = { .r = MPU6050_REGISTER_I2C_MST_CTRL, .b = MPU6050_SLV_3_FIFO_EN_BIT },
+};
+
+esp_err_t mpu6050_get_slave_fifo_enabled(mpu6050_dev_t *dev, mpu6050_slave_t num, bool *enabled)
{
- return i2c_write_bit(setting, MPU6050_REGISTER_FIFO_EN, MPU6050_SLV2_FIFO_EN_BIT, enabled);
+ CHECK_ARG(num < MPU6050_SLAVE_4);
+
+ return read_reg_bool(dev, slave_fifo_bits[num].r, slave_fifo_bits[num].b, enabled);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_slave_1_fifo_enabled(mpu6050_dev_t *setting)
+
+esp_err_t mpu6050_set_slave_fifo_enabled(mpu6050_dev_t *dev, mpu6050_slave_t num, bool enabled)
{
- uint8_t byte = 0;
- ESP_ERROR_RETURN(i2c_read_bit(setting, MPU6050_REGISTER_FIFO_EN, MPU6050_SLV1_FIFO_EN_BIT, &(byte)));
+ CHECK_ARG(num < MPU6050_SLAVE_4);
- return ((byte == 0) ? DISABLE : ENABLE);
+ return write_reg_bool(dev, slave_fifo_bits[num].r, slave_fifo_bits[num].b, enabled);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_slave_1_fifo_enabled(mpu6050_dev_t *setting, bool enabled)
+
+esp_err_t mpu6050_get_multi_master_enabled(mpu6050_dev_t *dev, bool *enabled)
{
- return i2c_write_bit(setting, MPU6050_REGISTER_FIFO_EN, MPU6050_SLV1_FIFO_EN_BIT, enabled);
+ return read_reg_bool(dev, MPU6050_REGISTER_I2C_MST_CTRL, MPU6050_MULT_MST_EN_BIT, enabled);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_slave_0_fifo_enabled(mpu6050_dev_t *setting)
-{
- uint8_t byte = 0;
- ESP_ERROR_RETURN(i2c_read_bit(setting, MPU6050_REGISTER_FIFO_EN, MPU6050_SLV0_FIFO_EN_BIT, &(byte)));
- return ((byte == 0) ? DISABLE : ENABLE);
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_slave_0_fifo_enabled(mpu6050_dev_t *setting, bool enabled)
+esp_err_t mpu6050_set_multi_master_enabled(mpu6050_dev_t *dev, bool enabled)
{
- return i2c_write_bit(setting, MPU6050_REGISTER_FIFO_EN, MPU6050_SLV0_FIFO_EN_BIT, enabled);
+ return write_reg_bool(dev, MPU6050_REGISTER_I2C_MST_CTRL, MPU6050_MULT_MST_EN_BIT, enabled);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_multi_master_enabled(mpu6050_dev_t *setting)
-{
- uint8_t byte = 0;
- ESP_ERROR_RETURN(i2c_read_bit(setting, MPU6050_REGISTER_I2C_MST_CTRL, MPU6050_MULT_MST_EN_BIT, &(byte)));
- return ((byte == 0) ? DISABLE : ENABLE);
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_multi_master_enabled(mpu6050_dev_t *setting, bool enabled)
+esp_err_t mpu6050_get_wait_for_external_sensor_enabled(mpu6050_dev_t *dev, bool *enabled)
{
- return i2c_write_bit(setting, MPU6050_REGISTER_I2C_MST_CTRL, MPU6050_MULT_MST_EN_BIT, enabled);
+ return read_reg_bool(dev, MPU6050_REGISTER_I2C_MST_CTRL, MPU6050_WAIT_FOR_ES_BIT, enabled);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_wait_for_external_sensor_enabled(mpu6050_dev_t *setting)
-{
- uint8_t byte = 0;
- ESP_ERROR_RETURN(i2c_read_bit(setting, MPU6050_REGISTER_I2C_MST_CTRL, MPU6050_WAIT_FOR_ES_BIT, &(byte)));
- return ((byte == 0) ? DISABLE : ENABLE);
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_wait_for_external_sensor_enabled(mpu6050_dev_t *setting, bool enabled)
+esp_err_t mpu6050_set_wait_for_external_sensor_enabled(mpu6050_dev_t *dev, bool enabled)
{
- return i2c_write_bit(setting, MPU6050_REGISTER_I2C_MST_CTRL, MPU6050_WAIT_FOR_ES_BIT, enabled);
+ return write_reg_bool(dev, MPU6050_REGISTER_I2C_MST_CTRL, MPU6050_WAIT_FOR_ES_BIT, enabled);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_slave_3_fifo_enabled(mpu6050_dev_t *setting)
-{
- uint8_t byte = 0;
- ESP_ERROR_RETURN(i2c_read_bit(setting, MPU6050_REGISTER_I2C_MST_CTRL, MPU6050_SLV_3_FIFO_EN_BIT, &(byte)));
- return ((byte == 0) ? DISABLE : ENABLE);
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_slave_3_fifo_enabled(mpu6050_dev_t *setting, bool enabled)
+esp_err_t mpu6050_get_slave_read_write_transition_enabled(mpu6050_dev_t *dev, bool *enabled)
{
- return i2c_write_bit(setting, MPU6050_REGISTER_I2C_MST_CTRL, MPU6050_SLV_3_FIFO_EN_BIT, enabled);
+ return read_reg_bool(dev, MPU6050_REGISTER_I2C_MST_CTRL, MPU6050_I2C_MST_P_NSR_BIT, enabled);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_slave_read_write_transition_enabled(mpu6050_dev_t *setting)
-{
- uint8_t byte = 0;
- ESP_ERROR_RETURN(i2c_read_bit(setting, MPU6050_REGISTER_I2C_MST_CTRL, MPU6050_I2C_MST_P_NSR_BIT, &(byte)));
- return ((byte == 0) ? DISABLE : ENABLE);
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_slave_read_write_transition_enabled(mpu6050_dev_t *setting, bool enabled)
+esp_err_t mpu6050_set_slave_read_write_transition_enabled(mpu6050_dev_t *dev, bool enabled)
{
- return i2c_write_bit(setting, MPU6050_REGISTER_I2C_MST_CTRL, MPU6050_I2C_MST_P_NSR_BIT, enabled);
+ return write_reg_bool(dev, MPU6050_REGISTER_I2C_MST_CTRL, MPU6050_I2C_MST_P_NSR_BIT, enabled);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_master_clock_speed(mpu6050_dev_t *setting, uint8_t *clk_spd)
+
+esp_err_t mpu6050_get_master_clock_speed(mpu6050_dev_t *dev, mpu6050_i2c_master_clock_t *clk_spd)
{
- return (i2c_read_bits(setting, MPU6050_REGISTER_I2C_MST_CTRL, MPU6050_I2C_MST_CLK_BIT, MPU6050_I2C_MST_CLK_LENGTH,
- clk_spd));
+ return read_reg_bits(dev, MPU6050_REGISTER_I2C_MST_CTRL, MPU6050_I2C_MST_CLK_BIT, MPU6050_I2C_MST_CLK_MASK, (uint8_t *)clk_spd);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_master_clock_speed(mpu6050_dev_t *setting, uint8_t clk_spd)
+
+esp_err_t mpu6050_set_master_clock_speed(mpu6050_dev_t *dev, mpu6050_i2c_master_clock_t clk_spd)
{
- return (i2c_write_bits(setting, MPU6050_REGISTER_I2C_MST_CTRL, MPU6050_I2C_MST_CLK_BIT, MPU6050_I2C_MST_CLK_LENGTH,
- clk_spd));
+ return write_reg_bits(dev, MPU6050_REGISTER_I2C_MST_CTRL, MPU6050_I2C_MST_CLK_BIT, MPU6050_I2C_MST_CLK_MASK, clk_spd);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_slave_address(mpu6050_dev_t *setting, uint8_t num, uint8_t *addr)
-{
- if (num > 3)
- return ESP_FAIL;
- return (i2c_read_byte(setting, MPU6050_REGISTER_I2C_SLV0_ADDR + num * 3, addr));
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_slave_address(mpu6050_dev_t *setting, uint8_t num, uint8_t address)
+esp_err_t mpu6050_get_slave_address(mpu6050_dev_t *dev, mpu6050_slave_t num, uint8_t *addr)
{
- if (num > 3)
- return ESP_FAIL;
+ CHECK_ARG(num <= MPU6050_SLAVE_4);
- return i2c_write_byte(setting, MPU6050_REGISTER_I2C_SLV0_ADDR + num * 3, address);
+ return read_reg(dev, MPU6050_REGISTER_I2C_SLV0_ADDR + num * 3, addr);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_slave_register(mpu6050_dev_t *setting, uint8_t num, uint8_t *reg)
-{
- if (num > 3)
- return ESP_FAIL;
- return (i2c_read_byte(setting, MPU6050_REGISTER_I2C_SLV0_REG + num * 3, reg));
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_slave_register(mpu6050_dev_t *setting, uint8_t num, uint8_t reg)
+esp_err_t mpu6050_set_slave_address(mpu6050_dev_t *dev, mpu6050_slave_t num, uint8_t address)
{
- if (num > 3)
- return ESP_FAIL;
+ CHECK_ARG(num <= MPU6050_SLAVE_4);
- return i2c_write_byte(setting, MPU6050_REGISTER_I2C_SLV0_REG + num * 3, reg);
+ return write_reg(dev, MPU6050_REGISTER_I2C_SLV0_ADDR + num * 3, address);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_slave_enabled(mpu6050_dev_t *setting, uint8_t num)
-{
- uint8_t enabled = 0;
- if (num > 3)
- return ESP_FAIL;
- ESP_ERROR_RETURN(
- i2c_read_bit(setting, MPU6050_REGISTER_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_EN_BIT, &(enabled)));
-
- return ((enabled == 0) ? DISABLE : ENABLE);
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_slave_enabled(mpu6050_dev_t *setting, uint8_t num, bool enabled)
+esp_err_t mpu6050_get_slave_register(mpu6050_dev_t *dev, mpu6050_slave_t num, uint8_t *reg)
{
- if (num > 3)
- return ESP_FAIL;
+ CHECK_ARG(num <= MPU6050_SLAVE_4);
- return i2c_write_bit(setting, MPU6050_REGISTER_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_EN_BIT, enabled);
+ return read_reg(dev, MPU6050_REGISTER_I2C_SLV0_REG + num * 3, reg);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_slave_word_byte_swap(mpu6050_dev_t *setting, uint8_t num)
-{
- uint8_t byte = 0;
- if (num > 3)
- return ESP_FAIL;
-
- ESP_ERROR_RETURN(
- i2c_read_bit(setting, MPU6050_REGISTER_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_BYTE_SW_BIT, &(byte)));
- return ((byte == 0) ? DISABLE : ENABLE);
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_slave_word_byte_swap(mpu6050_dev_t *setting, uint8_t num, bool enabled)
+esp_err_t mpu6050_set_slave_register(mpu6050_dev_t *dev, mpu6050_slave_t num, uint8_t reg)
{
- if (num > 3)
- return ESP_FAIL;
+ CHECK_ARG(num <= MPU6050_SLAVE_4);
- return i2c_write_bit(setting, MPU6050_REGISTER_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_BYTE_SW_BIT, enabled);
+ return write_reg(dev, MPU6050_REGISTER_I2C_SLV0_REG + num * 3, reg);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_slave_write_mode(mpu6050_dev_t *setting, uint8_t num, bool *mode)
-{
- if (num > 3)
- return ESP_FAIL;
- return (
- i2c_read_bit(setting, MPU6050_REGISTER_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_REG_DIS_BIT, (uint8_t *)mode));
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_slave_write_mode(mpu6050_dev_t *setting, uint8_t num, bool mode)
+esp_err_t mpu6050_get_slave_enabled(mpu6050_dev_t *dev, mpu6050_slave_t num, bool *enabled)
{
- if (num > 3)
- return ESP_FAIL;
+ CHECK_ARG(num <= MPU6050_SLAVE_4);
- return (i2c_write_bit(setting, MPU6050_REGISTER_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_REG_DIS_BIT, mode));
+ return read_reg_bool(dev, MPU6050_REGISTER_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_EN_BIT, enabled);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_slave_word_group_offset(mpu6050_dev_t *setting, uint8_t num, bool *enabled)
-{
- if (num > 3)
- return ESP_FAIL;
- return (
- i2c_read_bit(setting, MPU6050_REGISTER_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_GRP_BIT, (uint8_t *)enabled));
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_slave_word_group_offset(mpu6050_dev_t *setting, uint8_t num, bool enabled)
+esp_err_t mpu6050_set_slave_enabled(mpu6050_dev_t *dev, mpu6050_slave_t num, bool enabled)
{
- if (num > 3)
- return ESP_FAIL;
+ CHECK_ARG(num <= MPU6050_SLAVE_4);
- return (i2c_write_bit(setting, MPU6050_REGISTER_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_GRP_BIT, enabled));
+ return write_reg_bool(dev, MPU6050_REGISTER_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_EN_BIT, enabled);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_slave_data_length(mpu6050_dev_t *setting, uint8_t num, uint8_t *length)
+
+esp_err_t mpu6050_get_slave_word_byte_swap(mpu6050_dev_t *dev, mpu6050_slave_t num, bool *enabled)
{
- if (num > 3)
- return ESP_FAIL;
+ CHECK_ARG(num < MPU6050_SLAVE_4);
- return (i2c_read_bits(setting, MPU6050_REGISTER_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_LEN_BIT,
- MPU6050_I2C_SLV_LEN_LENGTH, length));
+ return read_reg_bool(dev, MPU6050_REGISTER_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_BYTE_SW_BIT, enabled);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_slave_data_length(mpu6050_dev_t *setting, uint8_t num, uint8_t length)
+
+esp_err_t mpu6050_set_slave_word_byte_swap(mpu6050_dev_t *dev, mpu6050_slave_t num, bool enabled)
{
- if (num > 3)
- return ESP_FAIL;
+ CHECK_ARG(num <= MPU6050_SLAVE_4);
- return (i2c_write_bits(setting, MPU6050_REGISTER_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_LEN_BIT,
- MPU6050_I2C_SLV_LEN_LENGTH, length));
+ return write_reg_bool(dev, MPU6050_REGISTER_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_BYTE_SW_BIT, enabled);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_slave_4_address(mpu6050_dev_t *setting, uint8_t *address)
+
+esp_err_t mpu6050_get_slave_write_mode(mpu6050_dev_t *dev, mpu6050_slave_t num, bool *mode)
{
- return (i2c_read_byte(setting, MPU6050_REGISTER_I2C_SLV4_ADDR, address));
+ CHECK_ARG(num <= MPU6050_SLAVE_4);
+
+ return read_reg_bool(dev, MPU6050_REGISTER_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_REG_DIS_BIT, mode);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_slave_4_address(mpu6050_dev_t *setting, uint8_t address)
+
+esp_err_t mpu6050_set_slave_write_mode(mpu6050_dev_t *dev, mpu6050_slave_t num, bool mode)
{
- return i2c_write_byte(setting, MPU6050_REGISTER_I2C_SLV4_ADDR, address);
+ CHECK_ARG(num <= MPU6050_SLAVE_4);
+
+ return write_reg_bool(dev, MPU6050_REGISTER_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_REG_DIS_BIT, mode);
}
-/* ------------------------------------------------------ */
-uint8_t mpu6050_get_slave_4_register(mpu6050_dev_t *setting, uint8_t *reg)
+
+esp_err_t mpu6050_get_slave_word_group_offset(mpu6050_dev_t *dev, mpu6050_slave_t num, bool *enabled)
{
- return (i2c_read_byte(setting, MPU6050_REGISTER_I2C_SLV4_REG, reg));
+ CHECK_ARG(num <= MPU6050_SLAVE_4);
+
+ return read_reg_bool(dev, MPU6050_REGISTER_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_GRP_BIT, enabled);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_slave_4_register(mpu6050_dev_t *setting, uint8_t reg)
+
+esp_err_t mpu6050_set_slave_word_group_offset(mpu6050_dev_t *dev, mpu6050_slave_t num, bool enabled)
{
- return (i2c_write_byte(setting, MPU6050_REGISTER_I2C_SLV4_REG, reg));
+ CHECK_ARG(num <= MPU6050_SLAVE_4);
+
+ return write_reg_bool(dev, MPU6050_REGISTER_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_GRP_BIT, enabled);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_slave_4_output_byte(mpu6050_dev_t *setting, uint8_t data)
+
+esp_err_t mpu6050_get_slave_data_length(mpu6050_dev_t *dev, mpu6050_slave_t num, uint8_t *length)
{
- return (i2c_write_byte(setting, MPU6050_REGISTER_I2C_SLV4_DO, data));
+ CHECK_ARG(num <= MPU6050_SLAVE_4);
+
+ return read_reg_bits(dev, MPU6050_REGISTER_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_LEN_BIT, MPU6050_I2C_SLV_LEN_MASK, length);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_slave_4_enabled(mpu6050_dev_t *setting, bool *enabled)
+
+esp_err_t mpu6050_set_slave_data_length(mpu6050_dev_t *dev, mpu6050_slave_t num, uint8_t length)
{
- return (i2c_read_bit(setting, MPU6050_REGISTER_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_EN_BIT, (uint8_t *)enabled));
+ CHECK_ARG(num <= MPU6050_SLAVE_4);
+
+ return write_reg_bits(dev, MPU6050_REGISTER_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_LEN_BIT, MPU6050_I2C_SLV_LEN_MASK, length);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_slave_4_enabled(mpu6050_dev_t *setting, bool enabled)
+
+esp_err_t mpu6050_set_slave_4_output_byte(mpu6050_dev_t *dev, uint8_t data)
{
- return (i2c_write_bit(setting, MPU6050_REGISTER_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_EN_BIT, enabled));
+ return write_reg(dev, MPU6050_REGISTER_I2C_SLV4_DO, data);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_slave_4_interrupt_enabled(mpu6050_dev_t *setting, bool *enabled)
+
+esp_err_t mpu6050_get_slave_4_interrupt_enabled(mpu6050_dev_t *dev, bool *enabled)
{
- return (i2c_read_bit(setting, MPU6050_REGISTER_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_INT_EN_BIT, (uint8_t *)enabled));
+ return read_reg_bool(dev, MPU6050_REGISTER_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_INT_EN_BIT, enabled);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_slave_4_interrupt_enabled(mpu6050_dev_t *setting, bool enabled)
+
+esp_err_t mpu6050_set_slave_4_interrupt_enabled(mpu6050_dev_t *dev, bool enabled)
{
- return (i2c_write_bit(setting, MPU6050_REGISTER_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_INT_EN_BIT, enabled));
+ return write_reg_bool(dev, MPU6050_REGISTER_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_INT_EN_BIT, enabled);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_slave_4_write_mode(mpu6050_dev_t *setting, uint8_t *mode)
+
+esp_err_t mpu6050_get_slave_4_master_delay(mpu6050_dev_t *dev, uint8_t *delay)
{
- return (i2c_read_bit(setting, MPU6050_REGISTER_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_REG_DIS_BIT, (uint8_t *)mode));
+ return read_reg_bits(dev, MPU6050_REGISTER_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_MST_DLY_BIT, MPU6050_I2C_SLV4_MST_DLY_LENGTH, delay);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_slave_4_write_mode(mpu6050_dev_t *setting, bool mode)
+
+esp_err_t mpu6050_set_slave_4_master_delay(mpu6050_dev_t *dev, uint8_t delay)
{
- return (i2c_write_bit(setting, MPU6050_REGISTER_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_REG_DIS_BIT, mode));
+ return write_reg_bits(dev, MPU6050_REGISTER_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_MST_DLY_BIT, MPU6050_I2C_SLV4_MST_DLY_LENGTH, delay);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_slave_4_master_delay(mpu6050_dev_t *setting, uint8_t *delay)
+
+esp_err_t mpu6050_get_slave_4_input_byte(mpu6050_dev_t *dev, uint8_t *byte)
{
- return (i2c_read_bits(setting, MPU6050_REGISTER_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_MST_DLY_BIT,
- MPU6050_I2C_SLV4_MST_DLY_LENGTH, delay));
+ return read_reg(dev, MPU6050_REGISTER_I2C_SLV4_DI, byte);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_slave_4_master_delay(mpu6050_dev_t *setting, uint8_t delay)
+
+esp_err_t mpu6050_get_passthrough_status(mpu6050_dev_t *dev, bool *enabled)
{
- return (i2c_write_bits(setting, MPU6050_REGISTER_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_MST_DLY_BIT,
- MPU6050_I2C_SLV4_MST_DLY_LENGTH, delay));
+ return read_reg_bool(dev, MPU6050_REGISTER_I2C_MST_STATUS, MPU6050_MST_PASS_THROUGH_BIT, enabled);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_slave_4_input_byte(mpu6050_dev_t *setting, uint8_t *byte)
+
+esp_err_t mpu6050_get_slave_4_is_done(mpu6050_dev_t *dev, bool *enabled)
{
- return (i2c_read_byte(setting, MPU6050_REGISTER_I2C_SLV4_DI, byte));
+ return read_reg_bool(dev, MPU6050_REGISTER_I2C_MST_STATUS, MPU6050_MST_I2C_SLV4_DONE_BIT, enabled);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_passthrough_status(mpu6050_dev_t *setting)
-{
- uint8_t byte = 0;
- ESP_ERROR_RETURN(i2c_read_bit(setting, MPU6050_REGISTER_I2C_MST_STATUS, MPU6050_MST_PASS_THROUGH_BIT, &(byte)));
- return ((byte == 0) ? DISABLE : ENABLE);
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_slave_4_is_done(mpu6050_dev_t *setting)
+esp_err_t mpu6050_get_lost_arbitration(mpu6050_dev_t *dev, bool *lost)
{
- uint8_t byte = 0;
- ESP_ERROR_RETURN(i2c_read_bit(setting, MPU6050_REGISTER_I2C_MST_STATUS, MPU6050_MST_I2C_SLV4_DONE_BIT, &(byte)));
-
- return ((byte == 0) ? DISABLE : ENABLE);
+ return read_reg_bool(dev, MPU6050_REGISTER_I2C_MST_STATUS, MPU6050_MST_I2C_LOST_ARB_BIT, lost);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_lost_arbitration(mpu6050_dev_t *setting)
-{
- uint8_t byte = 0;
- ESP_ERROR_RETURN(i2c_read_bit(setting, MPU6050_REGISTER_I2C_MST_STATUS, MPU6050_MST_I2C_LOST_ARB_BIT, &(byte)));
- return ((byte == 0) ? DISABLE : ENABLE);
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_slave_4_nack(mpu6050_dev_t *setting)
+esp_err_t mpu6050_get_slave_nack(mpu6050_dev_t *dev, mpu6050_slave_t num, bool *nack)
{
- uint8_t byte = 0;
- ESP_ERROR_RETURN(i2c_read_bit(setting, MPU6050_REGISTER_I2C_MST_STATUS, MPU6050_MST_I2C_SLV4_NACK_BIT, &(byte)));
+ CHECK_ARG(num <= MPU6050_SLAVE_4);
- return ((byte == 0) ? DISABLE : ENABLE);
+ static const uint8_t bits[] = {
+ [MPU6050_SLAVE_0] = MPU6050_MST_I2C_SLV0_NACK_BIT,
+ [MPU6050_SLAVE_1] = MPU6050_MST_I2C_SLV1_NACK_BIT,
+ [MPU6050_SLAVE_2] = MPU6050_MST_I2C_SLV2_NACK_BIT,
+ [MPU6050_SLAVE_3] = MPU6050_MST_I2C_SLV3_NACK_BIT,
+ [MPU6050_SLAVE_4] = MPU6050_MST_I2C_SLV4_NACK_BIT,
+ };
+ return read_reg_bool(dev, MPU6050_REGISTER_I2C_MST_STATUS, bits[num], nack);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_slave_3_nack(mpu6050_dev_t *setting)
-{
- uint8_t byte = 0;
- ESP_ERROR_RETURN(i2c_read_bit(setting, MPU6050_REGISTER_I2C_MST_STATUS, MPU6050_MST_I2C_SLV3_NACK_BIT, &(byte)));
- return ((byte == 0) ? DISABLE : ENABLE);
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_slave_2_nack(mpu6050_dev_t *setting)
+esp_err_t mpu6050_get_interrupt_mode(mpu6050_dev_t *dev, mpu6050_int_level_t *mode)
{
- uint8_t byte = 0;
- ESP_ERROR_RETURN(i2c_read_bit(setting, MPU6050_REGISTER_I2C_MST_STATUS, MPU6050_MST_I2C_SLV2_NACK_BIT, &(byte)));
-
- return ((byte == 0) ? DISABLE : ENABLE);
+ return read_reg_bits(dev, MPU6050_REGISTER_INT_PIN_CFG, MPU6050_INTCFG_INT_LEVEL_BIT, BIT(MPU6050_INTCFG_INT_LEVEL_BIT), (uint8_t *)mode);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_slave_1_nack(mpu6050_dev_t *setting)
-{
- uint8_t byte = 0;
- ESP_ERROR_RETURN(i2c_read_bit(setting, MPU6050_REGISTER_I2C_MST_STATUS, MPU6050_MST_I2C_SLV1_NACK_BIT, &(byte)));
- return ((byte == 0) ? DISABLE : ENABLE);
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_slave_0_nack(mpu6050_dev_t *setting)
+esp_err_t mpu6050_set_interrupt_mode(mpu6050_dev_t *dev, mpu6050_int_level_t mode)
{
- uint8_t byte = 0;
- ESP_ERROR_RETURN(i2c_read_bit(setting, MPU6050_REGISTER_I2C_MST_STATUS, MPU6050_MST_I2C_SLV0_NACK_BIT, &(byte)));
+ CHECK_ARG(mode <= MPU6050_INT_LEVEL_LOW);
- return ((byte == 0) ? DISABLE : ENABLE);
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_interrupt_mode(mpu6050_dev_t *setting, bool *mode)
-{
- return (i2c_read_bit(setting, MPU6050_REGISTER_INT_PIN_CFG, MPU6050_INTCFG_INT_LEVEL_BIT, (uint8_t *)mode));
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_interrupt_mode(mpu6050_dev_t *setting, bool mode)
-{
- return (i2c_write_bit(setting, MPU6050_REGISTER_INT_PIN_CFG, MPU6050_INTCFG_INT_LEVEL_BIT, mode));
+ return write_reg_bits(dev, MPU6050_REGISTER_INT_PIN_CFG, MPU6050_INTCFG_INT_LEVEL_BIT, BIT(MPU6050_INTCFG_INT_LEVEL_BIT), mode);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_interrupt_drive(mpu6050_dev_t *setting, bool *drive)
-{
- return (i2c_read_bit(setting, MPU6050_REGISTER_INT_PIN_CFG, MPU6050_INTCFG_INT_OPEN_BIT, (uint8_t *)drive));
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_interrupt_drive(mpu6050_dev_t *setting, bool drive)
-{
- return (i2c_write_bit(setting, MPU6050_REGISTER_INT_PIN_CFG, MPU6050_INTCFG_INT_OPEN_BIT, drive));
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_interrupt_latch(mpu6050_dev_t *setting, bool *latch)
+
+esp_err_t mpu6050_get_interrupt_drive(mpu6050_dev_t *dev, mpu6050_int_drive_t *drive)
{
- return (i2c_read_bit(setting, MPU6050_REGISTER_INT_PIN_CFG, MPU6050_INTCFG_LATCH_INT_EN_BIT, (uint8_t *)latch));
+ return read_reg_bits(dev, MPU6050_REGISTER_INT_PIN_CFG, MPU6050_INTCFG_INT_OPEN_BIT, BIT(MPU6050_INTCFG_INT_OPEN_BIT), (uint8_t *)drive);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_interrupt_latch(mpu6050_dev_t *setting, bool latch)
+
+esp_err_t mpu6050_set_interrupt_drive(mpu6050_dev_t *dev, mpu6050_int_drive_t drive)
{
- return (i2c_write_bit(setting, MPU6050_REGISTER_INT_PIN_CFG, MPU6050_INTCFG_LATCH_INT_EN_BIT, latch));
+ CHECK_ARG(drive <= MPU6050_INT_OPEN_DRAIN);
+
+ return write_reg_bits(dev, MPU6050_REGISTER_INT_PIN_CFG, MPU6050_INTCFG_INT_OPEN_BIT, BIT(MPU6050_INTCFG_INT_OPEN_BIT), drive);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_interrupt_latch_clear(mpu6050_dev_t *setting, bool *clear)
+
+esp_err_t mpu6050_get_interrupt_latch(mpu6050_dev_t *dev, mpu6050_int_latch_t *latch)
{
- return (i2c_read_bit(setting, MPU6050_REGISTER_INT_PIN_CFG, MPU6050_INTCFG_INT_RD_CLEAR_BIT, (uint8_t *)clear));
+ return read_reg_bits(dev, MPU6050_REGISTER_INT_PIN_CFG, MPU6050_INTCFG_LATCH_INT_EN_BIT, BIT(MPU6050_INTCFG_LATCH_INT_EN_BIT), (uint8_t *)latch);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_interrupt_latch_clear(mpu6050_dev_t *setting, bool clear)
+
+esp_err_t mpu6050_set_interrupt_latch(mpu6050_dev_t *dev, mpu6050_int_latch_t latch)
{
- return i2c_write_bit(setting, MPU6050_REGISTER_INT_PIN_CFG, MPU6050_INTCFG_INT_RD_CLEAR_BIT, clear);
+ CHECK_ARG(latch <= MPU6050_INT_LATCH_CONTINUOUS);
+
+ return write_reg_bits(dev, MPU6050_REGISTER_INT_PIN_CFG, MPU6050_INTCFG_LATCH_INT_EN_BIT, BIT(MPU6050_INTCFG_LATCH_INT_EN_BIT), latch);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_fsync_interrupt_level(mpu6050_dev_t *setting, bool *level)
+
+esp_err_t mpu6050_get_interrupt_latch_clear(mpu6050_dev_t *dev, bool *clear)
{
- return (i2c_read_bit(setting, MPU6050_REGISTER_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT, (uint8_t *)level));
+ return read_reg_bool(dev, MPU6050_REGISTER_INT_PIN_CFG, MPU6050_INTCFG_INT_RD_CLEAR_BIT, clear);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_fsync_interrupt_level(mpu6050_dev_t *setting, bool level)
+
+esp_err_t mpu6050_set_interrupt_latch_clear(mpu6050_dev_t *dev, bool clear)
{
- return i2c_write_bit(setting, MPU6050_REGISTER_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT, level);
+ return write_reg_bool(dev, MPU6050_REGISTER_INT_PIN_CFG, MPU6050_INTCFG_INT_RD_CLEAR_BIT, clear);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_fsync_interrupt_enabled(mpu6050_dev_t *setting)
-{
- uint8_t byte = 0;
- ESP_ERROR_RETURN(i2c_read_bit(setting, MPU6050_REGISTER_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_EN_BIT, &(byte)));
- return ((byte == 0) ? DISABLE : ENABLE);
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_fsync_interrupt_enabled(mpu6050_dev_t *setting, bool enabled)
+esp_err_t mpu6050_get_fsync_interrupt_level(mpu6050_dev_t *dev, mpu6050_int_level_t *level)
{
- return (i2c_write_bit(setting, MPU6050_REGISTER_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_EN_BIT, enabled));
+ return read_reg_bits(dev, MPU6050_REGISTER_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT, BIT(MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT), (uint8_t *)level);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_i2c_bypass_enabled(mpu6050_dev_t *setting)
+
+esp_err_t mpu6050_set_fsync_interrupt_level(mpu6050_dev_t *dev, mpu6050_int_level_t level)
{
- uint8_t byte = 0;
- ESP_ERROR_RETURN(i2c_read_bit(setting, MPU6050_REGISTER_INT_PIN_CFG, MPU6050_INTCFG_I2C_BYPASS_EN_BIT, &(byte)));
+ CHECK_ARG(level <= MPU6050_INT_LEVEL_LOW);
- return ((byte == 0) ? DISABLE : ENABLE);
+ return write_reg_bits(dev, MPU6050_REGISTER_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT, BIT(MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT), level);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_i2c_bypass_enabled(mpu6050_dev_t *setting, bool enabled)
+
+esp_err_t mpu6050_get_fsync_interrupt_enabled(mpu6050_dev_t *dev, bool *enabled)
{
- return i2c_write_bit(setting, MPU6050_REGISTER_INT_PIN_CFG, MPU6050_INTCFG_I2C_BYPASS_EN_BIT, enabled);
+ return read_reg_bool(dev, MPU6050_REGISTER_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_EN_BIT, enabled);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_clock_output_enabled(mpu6050_dev_t *setting)
-{
- uint8_t byte = 0;
- ESP_ERROR_RETURN(i2c_read_bit(setting, MPU6050_REGISTER_INT_PIN_CFG, MPU6050_INTCFG_CLKOUT_EN_BIT, &(byte)));
- return ((byte == 0) ? DISABLE : ENABLE);
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_clock_output_enabled(mpu6050_dev_t *setting, bool enabled)
+esp_err_t mpu6050_set_fsync_interrupt_enabled(mpu6050_dev_t *dev, bool enabled)
{
- return i2c_write_bit(setting, MPU6050_REGISTER_INT_PIN_CFG, MPU6050_INTCFG_CLKOUT_EN_BIT, enabled);
+ return write_reg_bool(dev, MPU6050_REGISTER_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_EN_BIT, enabled);
}
-/* ------------------------------------------------------ */
-uint8_t mpu6050_get_int_enabled(mpu6050_dev_t *setting)
-{
- uint8_t byte = 0;
- ESP_ERROR_RETURN(i2c_read_byte(setting, MPU6050_REGISTER_INT_ENABLE, &(byte)));
- return ((byte == 0) ? DISABLE : ENABLE);
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_int_enabled(mpu6050_dev_t *setting, uint8_t enabled)
+esp_err_t mpu6050_get_i2c_bypass_enabled(mpu6050_dev_t *dev, bool *enabled)
{
- return (i2c_write_byte(setting, MPU6050_REGISTER_INT_ENABLE, enabled));
+ return read_reg_bool(dev, MPU6050_REGISTER_INT_PIN_CFG, MPU6050_INTCFG_I2C_BYPASS_EN_BIT, enabled);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_int_freefall_enabled(mpu6050_dev_t *setting)
-{
- uint8_t byte = 0;
- ESP_ERROR_RETURN(i2c_read_bit(setting, MPU6050_REGISTER_INT_ENABLE, MPU6050_INTERRUPT_FF_BIT, &(byte)));
- return ((byte == 0) ? DISABLE : ENABLE);
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_int_freefall_enabled(mpu6050_dev_t *setting, bool enabled)
+esp_err_t mpu6050_set_i2c_bypass_enabled(mpu6050_dev_t *dev, bool enabled)
{
- return i2c_write_bit(setting, MPU6050_REGISTER_INT_ENABLE, MPU6050_INTERRUPT_FF_BIT, enabled);
+ return write_reg_bool(dev, MPU6050_REGISTER_INT_PIN_CFG, MPU6050_INTCFG_I2C_BYPASS_EN_BIT, enabled);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_int_motion_enabled(mpu6050_dev_t *setting)
-{
- uint8_t byte = 0;
- ESP_ERROR_RETURN(i2c_read_bit(setting, MPU6050_REGISTER_INT_ENABLE, MPU6050_INTERRUPT_MOT_BIT, &(byte)));
- return ((byte == 0) ? DISABLE : ENABLE);
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_int_motion_enabled(mpu6050_dev_t *setting, bool enabled)
+esp_err_t mpu6050_get_clock_output_enabled(mpu6050_dev_t *dev, bool *enabled)
{
- return i2c_write_bit(setting, MPU6050_REGISTER_INT_ENABLE, MPU6050_INTERRUPT_MOT_BIT, enabled);
+ return read_reg_bool(dev, MPU6050_REGISTER_INT_PIN_CFG, MPU6050_INTCFG_CLKOUT_EN_BIT, enabled);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_int_zero_motion_enabled(mpu6050_dev_t *setting)
-{
- uint8_t byte = 0;
- ESP_ERROR_RETURN(i2c_read_bit(setting, MPU6050_REGISTER_INT_ENABLE, MPU6050_INTERRUPT_ZMOT_BIT, &(byte)));
- return ((byte == 0) ? DISABLE : ENABLE);
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_int_zero_motion_enabled(mpu6050_dev_t *setting, bool enabled)
+esp_err_t mpu6050_set_clock_output_enabled(mpu6050_dev_t *dev, bool enabled)
{
- return i2c_write_bit(setting, MPU6050_REGISTER_INT_ENABLE, MPU6050_INTERRUPT_ZMOT_BIT, enabled);
+ return write_reg_bool(dev, MPU6050_REGISTER_INT_PIN_CFG, MPU6050_INTCFG_CLKOUT_EN_BIT, enabled);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_int_fifo_byte_overflow_enabled(mpu6050_dev_t *setting)
-{
- uint8_t byte = 0;
- ESP_ERROR_RETURN(i2c_read_bit(setting, MPU6050_REGISTER_INT_ENABLE, MPU6050_INTERRUPT_FIFO_OFLOW_BIT, &(byte)));
- return ((byte == 0) ? DISABLE : ENABLE);
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_int_fifo_byte_overflow_enabled(mpu6050_dev_t *setting, bool enabled)
+esp_err_t mpu6050_get_int_enabled(mpu6050_dev_t *dev, uint8_t *ints)
{
- return i2c_write_bit(setting, MPU6050_REGISTER_INT_ENABLE, MPU6050_INTERRUPT_FIFO_OFLOW_BIT, enabled);
+ return read_reg(dev, MPU6050_REGISTER_INT_ENABLE, ints);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_int_i2c_master_enabled(mpu6050_dev_t *setting)
-{
- uint8_t byte = 0;
- ESP_ERROR_RETURN(i2c_read_bit(setting, MPU6050_REGISTER_INT_ENABLE, MPU6050_INTERRUPT_I2C_MST_INT_BIT, &(byte)));
- return ((byte == 0) ? DISABLE : ENABLE);
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_int_i2c_master_enabled(mpu6050_dev_t *setting, bool enabled)
+esp_err_t mpu6050_set_int_enabled(mpu6050_dev_t *dev, uint8_t ints)
{
- return i2c_write_bit(setting, MPU6050_REGISTER_INT_ENABLE, MPU6050_INTERRUPT_I2C_MST_INT_BIT, enabled);
+ return write_reg(dev, MPU6050_REGISTER_INT_ENABLE, ints);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_int_data_ready_enabled(mpu6050_dev_t *setting)
-{
- uint8_t byte = 0;
- ESP_ERROR_RETURN(i2c_read_bit(setting, MPU6050_REGISTER_INT_ENABLE, MPU6050_INTERRUPT_DATA_RDY_BIT, &(byte)));
- return ((byte == 0) ? DISABLE : ENABLE);
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_int_data_ready_enabled(mpu6050_dev_t *setting, bool enabled)
+esp_err_t mpu6050_get_int_status(mpu6050_dev_t *dev, uint8_t *ints)
{
- return i2c_write_bit(setting, MPU6050_REGISTER_INT_ENABLE, MPU6050_INTERRUPT_DATA_RDY_BIT, enabled);
+ return read_reg(dev, MPU6050_REGISTER_INT_ENABLE, ints);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_int_status(mpu6050_dev_t *setting)
-{
- uint8_t byte = 0;
- ESP_ERROR_RETURN(i2c_read_byte(setting, MPU6050_REGISTER_INT_STATUS, &(byte)));
- return ((byte == 0) ? DISABLE : ENABLE);
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_int_freefall_status(mpu6050_dev_t *setting)
-{
- uint8_t byte = 0;
- ESP_ERROR_RETURN(i2c_read_bit(setting, MPU6050_REGISTER_INT_STATUS, MPU6050_INTERRUPT_FF_BIT, &(byte)));
+static const uint8_t accel_offs_regs[] = {
+ [MPU6050_X_AXIS] = MPU6050_REGISTER_XA_OFFS_H,
+ [MPU6050_Y_AXIS] = MPU6050_REGISTER_YA_OFFS_H,
+ [MPU6050_Z_AXIS] = MPU6050_REGISTER_ZA_OFFS_H,
+};
- return ((byte == 0) ? DISABLE : ENABLE);
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_int_motion_status(mpu6050_dev_t *setting)
+esp_err_t mpu6050_get_accel_offset(mpu6050_dev_t *dev, mpu6050_axis_t axis, int16_t *offset)
{
- uint8_t byte = 0;
- ESP_ERROR_RETURN(i2c_read_bit(setting, MPU6050_REGISTER_INT_STATUS, MPU6050_INTERRUPT_MOT_BIT, &(byte)));
+ CHECK_ARG(axis <= MPU6050_Z_AXIS);
- return ((byte == 0) ? DISABLE : ENABLE);
+ return read_reg_word(dev, accel_offs_regs[axis], offset);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_int_zero_motion_status(mpu6050_dev_t *setting)
-{
- uint8_t byte = 0;
- ESP_ERROR_RETURN(i2c_read_bit(setting, MPU6050_REGISTER_INT_STATUS, MPU6050_INTERRUPT_ZMOT_BIT, &(byte)));
- return ((byte == 0) ? DISABLE : ENABLE);
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_int_fifo_byte_overflow_status(mpu6050_dev_t *setting)
+esp_err_t mpu6050_set_accel_offset(mpu6050_dev_t *dev, mpu6050_axis_t axis, int16_t offset)
{
- uint8_t byte = 0;
- ESP_ERROR_RETURN(i2c_read_bit(setting, MPU6050_REGISTER_INT_STATUS, MPU6050_INTERRUPT_FIFO_OFLOW_BIT, &(byte)));
+ CHECK_ARG(axis <= MPU6050_Z_AXIS);
- return ((byte == 0) ? DISABLE : ENABLE);
+ return write_reg_word(dev, accel_offs_regs[axis], offset);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_int_i2c_master_status(mpu6050_dev_t *setting)
-{
- uint8_t byte = 0;
- ESP_ERROR_RETURN(i2c_read_bit(setting, MPU6050_REGISTER_INT_STATUS, MPU6050_INTERRUPT_I2C_MST_INT_BIT, &(byte)));
- return ((byte == 0) ? DISABLE : ENABLE);
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_int_data_ready_status(mpu6050_dev_t *setting)
-{
- uint8_t byte = 0;
- ESP_ERROR_RETURN(i2c_read_bit(setting, MPU6050_REGISTER_INT_STATUS, MPU6050_INTERRUPT_DATA_RDY_BIT, &(byte)));
+static const uint8_t gyro_offs_regs[] = {
+ [MPU6050_X_AXIS] = MPU6050_REGISTER_XG_OFFS_USRH,
+ [MPU6050_Y_AXIS] = MPU6050_REGISTER_YG_OFFS_USRH,
+ [MPU6050_Z_AXIS] = MPU6050_REGISTER_ZG_OFFS_USRH,
+};
- return ((byte == 0) ? DISABLE : ENABLE);
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_acceleration(mpu6050_dev_t *setting, mpu6050_acceleration_t *accel)
+esp_err_t mpu6050_get_gyro_offset(mpu6050_dev_t *dev, mpu6050_axis_t axis, int16_t *offset)
{
- uint8_t buffer[6];
- ESP_ERROR_RETURN(i2c_read_bytes(setting, MPU6050_REGISTER_ACCEL_XOUT_H, 6, (uint8_t *)buffer));
- accel->x = (((int16_t)buffer[0]) << 8) | buffer[1];
- accel->y = (((int16_t)buffer[2]) << 8) | buffer[3];
- accel->z = (((int16_t)buffer[4]) << 8) | buffer[5];
- return ESP_OK;
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_acceleration_x(mpu6050_dev_t *setting, int16_t *accel_x)
-{
- uint8_t buffer[2];
- ESP_ERROR_RETURN(i2c_read_bytes(setting, MPU6050_REGISTER_ACCEL_XOUT_H, 2, buffer));
+ CHECK_ARG(axis <= MPU6050_Z_AXIS);
- *accel_x = ((((int16_t)buffer[0]) << 8) | buffer[1]);
- return ESP_OK;
+ return read_reg_word(dev, gyro_offs_regs[axis], offset);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_acceleration_y(mpu6050_dev_t *setting, int16_t *accel_y)
+
+esp_err_t mpu6050_set_gyro_offset(mpu6050_dev_t *dev, mpu6050_axis_t axis, int16_t offset)
{
- uint8_t buffer[2];
- ESP_ERROR_RETURN(i2c_read_bytes(setting, MPU6050_REGISTER_ACCEL_YOUT_H, 2, buffer));
- *accel_y = ((((int16_t)buffer[0]) << 8) | buffer[1]);
- return ESP_OK;
+ CHECK_ARG(axis <= MPU6050_Z_AXIS);
+
+ return write_reg_word(dev, gyro_offs_regs[axis], offset);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_acceleration_z(mpu6050_dev_t *setting, int16_t *accel_z)
+
+esp_err_t mpu6050_get_raw_acceleration(mpu6050_dev_t *dev, mpu6050_raw_acceleration_t *raw_accel)
{
- uint8_t buffer[2];
- ESP_ERROR_RETURN(i2c_read_bytes(setting, MPU6050_REGISTER_ACCEL_ZOUT_H, 2, buffer));
- *accel_z = ((((int16_t)buffer[0]) << 8) | buffer[1]);
+ CHECK_ARG(dev && raw_accel);
+
+ uint16_t buf[3];
+
+ I2C_DEV_TAKE_MUTEX(&dev->i2c_dev);
+ I2C_DEV_CHECK(&dev->i2c_dev, i2c_dev_read_reg(&dev->i2c_dev, MPU6050_REGISTER_ACCEL_XOUT_H, buf, sizeof(buf)));
+ I2C_DEV_GIVE_MUTEX(&dev->i2c_dev);
+
+ raw_accel->x = shuffle(buf[0]);
+ raw_accel->y = shuffle(buf[1]);
+ raw_accel->z = shuffle(buf[2]);
+
return ESP_OK;
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_temperature(mpu6050_dev_t *setting, int16_t *temp)
+
+esp_err_t mpu6050_get_acceleration(mpu6050_dev_t *dev, mpu6050_acceleration_t *accel)
{
- uint8_t buffer[2];
- ESP_ERROR_RETURN(i2c_read_bytes(setting, MPU6050_REGISTER_TEMP_OUT_H, 2, buffer));
+ CHECK_ARG(accel);
+
+ mpu6050_raw_acceleration_t raw;
+ CHECK(mpu6050_get_raw_acceleration(dev, &raw));
+
+ accel->x = get_accel_value(dev, raw.x);
+ accel->y = get_accel_value(dev, raw.y);
+ accel->z = get_accel_value(dev, raw.z);
- int16_t rawtemp = ((((int16_t)buffer[0]) << 8) | buffer[1]);
- *temp = (rawtemp / 340.0) + 36.53;
return ESP_OK;
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_rotation(mpu6050_dev_t *setting, mpu6050_rotation_t *rotat)
+
+esp_err_t mpu6050_get_raw_acceleration_axis(mpu6050_dev_t *dev, mpu6050_axis_t axis, int16_t *raw_accel)
{
- uint8_t buffer[6];
- ESP_ERROR_RETURN(i2c_read_bytes(setting, MPU6050_REGISTER_GYRO_XOUT_H, 6, buffer));
- rotat->x = (((int16_t)buffer[0]) << 8) | buffer[1];
- rotat->y = (((int16_t)buffer[2]) << 8) | buffer[3];
- rotat->z = (((int16_t)buffer[4]) << 8) | buffer[5];
- return ESP_OK;
+ CHECK_ARG(axis <= MPU6050_Z_AXIS);
+
+ return read_reg_word(dev, MPU6050_REGISTER_ACCEL_XOUT_H + axis * 2, raw_accel);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_rotation_x(mpu6050_dev_t *setting, int16_t *rotation_x)
+
+esp_err_t mpu6050_get_acceleration_axis(mpu6050_dev_t *dev, mpu6050_axis_t axis, float *accel)
{
- uint8_t buffer[2];
- ESP_ERROR_RETURN(i2c_read_bytes(setting, MPU6050_REGISTER_GYRO_XOUT_H, 2, buffer));
- *rotation_x = ((((int16_t)buffer[0]) << 8) | buffer[1]);
+ CHECK_ARG(accel);
+
+ int16_t raw;
+ CHECK(mpu6050_get_raw_acceleration_axis(dev, axis, &raw));
+
+ *accel = get_accel_value(dev, raw);
+
return ESP_OK;
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_rotation_y(mpu6050_dev_t *setting, int16_t *rotation_y)
+
+esp_err_t mpu6050_get_raw_rotation(mpu6050_dev_t *dev, mpu6050_raw_rotation_t *raw_gyro)
{
- uint8_t buffer[2];
- ESP_ERROR_RETURN(i2c_read_bytes(setting, MPU6050_REGISTER_GYRO_YOUT_H, 2, buffer));
- *rotation_y = ((((int16_t)buffer[0]) << 8) | buffer[1]);
+ CHECK_ARG(dev && raw_gyro);
+
+ uint16_t buf[3];
+
+ I2C_DEV_TAKE_MUTEX(&dev->i2c_dev);
+ I2C_DEV_CHECK(&dev->i2c_dev, i2c_dev_read_reg(&dev->i2c_dev, MPU6050_REGISTER_GYRO_XOUT_H, buf, sizeof(buf)));
+ I2C_DEV_GIVE_MUTEX(&dev->i2c_dev);
+
+ raw_gyro->x = shuffle(buf[0]);
+ raw_gyro->y = shuffle(buf[1]);
+ raw_gyro->z = shuffle(buf[2]);
+
return ESP_OK;
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_rotation_z(mpu6050_dev_t *setting, int16_t *rotation_z)
+
+esp_err_t mpu6050_get_rotation(mpu6050_dev_t *dev, mpu6050_rotation_t *gyro)
{
- uint8_t buffer[2];
- ESP_ERROR_RETURN(i2c_read_bytes(setting, MPU6050_REGISTER_GYRO_ZOUT_H, 2, buffer));
- *rotation_z = ((((int16_t)buffer[0]) << 8) | buffer[1]);
+ CHECK_ARG(gyro);
+
+ mpu6050_raw_rotation_t raw;
+ CHECK(mpu6050_get_raw_rotation(dev, &raw));
+
+ gyro->x = get_gyro_value(dev, raw.x);
+ gyro->y = get_gyro_value(dev, raw.y);
+ gyro->z = get_gyro_value(dev, raw.z);
+
return ESP_OK;
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_motion(mpu6050_dev_t *setting, mpu6050_acceleration_t *data_accel, mpu6050_rotation_t *data_gyro)
+
+esp_err_t mpu6050_get_raw_rotation_axis(mpu6050_dev_t *dev, mpu6050_axis_t axis, int16_t *raw_gyro)
{
- uint8_t buffer[14];
- ESP_ERROR_RETURN(i2c_read_bytes(setting, MPU6050_REGISTER_ACCEL_XOUT_H, 14, buffer));
+ CHECK_ARG(axis <= MPU6050_Z_AXIS);
- data_accel->x = (((int16_t)buffer[0]) << 8) | buffer[1];
- data_accel->y = (((int16_t)buffer[2]) << 8) | buffer[3];
- data_accel->z = (((int16_t)buffer[4]) << 8) | buffer[5];
- data_gyro->x = (((int16_t)buffer[8]) << 8) | buffer[9];
- data_gyro->y = (((int16_t)buffer[10]) << 8) | buffer[11];
- data_gyro->z = (((int16_t)buffer[12]) << 8) | buffer[13];
- return ESP_OK;
+ return read_reg_word(dev, MPU6050_REGISTER_ACCEL_XOUT_H + axis * 2, raw_gyro);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_external_sensor_byte(mpu6050_dev_t *setting, int position, uint8_t *byte)
+
+esp_err_t mpu6050_get_rotation_axis(mpu6050_dev_t *dev, mpu6050_axis_t axis, float *gyro)
{
- ESP_ERROR_RETURN(i2c_read_byte(setting, MPU6050_REGISTER_EXT_SENS_DATA_00 + position, byte));
+ CHECK_ARG(gyro);
+
+ int16_t raw;
+ CHECK(mpu6050_get_raw_rotation_axis(dev, axis, &raw));
+
+ *gyro = get_gyro_value(dev, raw);
+
return ESP_OK;
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_external_sensor_word(mpu6050_dev_t *setting, int position, uint16_t *word)
+
+esp_err_t mpu6050_get_motion(mpu6050_dev_t *dev, mpu6050_acceleration_t *accel, mpu6050_rotation_t *gyro)
{
- uint8_t buffer[2];
- ESP_ERROR_RETURN(i2c_read_bytes(setting, MPU6050_REGISTER_EXT_SENS_DATA_00 + position, 2, buffer));
- *word = ((((uint16_t)buffer[0]) << 8) | buffer[1]);
+ CHECK(mpu6050_get_acceleration(dev, accel));
+ CHECK(mpu6050_get_rotation(dev, gyro));
return ESP_OK;
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_external_sensor_dword(mpu6050_dev_t *setting, int position, uint32_t *dword)
+
+esp_err_t mpu6050_get_temperature(mpu6050_dev_t *dev, float *temp)
{
- uint8_t buffer[4];
- ESP_ERROR_RETURN(i2c_read_bytes(setting, MPU6050_REGISTER_EXT_SENS_DATA_00 + position, 4, buffer));
- *dword = ((((uint32_t)buffer[0]) << 24) | (((uint32_t)buffer[1]) << 16) | (((uint16_t)buffer[2]) << 8) | buffer[3]);
+ CHECK_ARG(temp);
+
+ int16_t raw = 0;
+ CHECK(read_reg_word(dev, MPU6050_REGISTER_TEMP_OUT_H, &raw));
+ *temp = ((float)raw / 340.0f) + 36.53f;
+
return ESP_OK;
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_motion_status(mpu6050_dev_t *setting, uint8_t *status)
-{
- return (i2c_read_byte(setting, MPU6050_REGISTER_MOT_DETECT_STATUS, status));
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_x_negative_motion_detected(mpu6050_dev_t *setting, uint8_t *motion)
-{
- return (i2c_read_bit(setting, MPU6050_REGISTER_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_XNEG_BIT, motion));
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_x_positive_motion_detected(mpu6050_dev_t *setting, uint8_t *detected)
-{
- return (i2c_read_bit(setting, MPU6050_REGISTER_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_XPOS_BIT, detected));
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_y_negative_motion_detected(mpu6050_dev_t *setting, uint8_t *detected)
-{
- return (i2c_read_bit(setting, MPU6050_REGISTER_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_YNEG_BIT, detected));
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_y_positive_motion_detected(mpu6050_dev_t *setting, uint8_t *detected)
-{
- return (i2c_read_bit(setting, MPU6050_REGISTER_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_YPOS_BIT, detected));
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_z_negative_motion_detected(mpu6050_dev_t *setting, uint8_t *detected)
-{
- return (i2c_read_bit(setting, MPU6050_REGISTER_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_ZNEG_BIT, detected));
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_z_positive_motion_detected(mpu6050_dev_t *setting, uint8_t *detected)
-{
- return (i2c_read_bit(setting, MPU6050_REGISTER_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_ZPOS_BIT, detected));
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_zero_motion_detected(mpu6050_dev_t *setting, uint8_t *detected)
-{
- return (i2c_read_bit(setting, MPU6050_REGISTER_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_ZRMOT_BIT, detected));
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_slave_output_byte(mpu6050_dev_t *setting, uint8_t num, uint8_t data)
-{
- if (num > 3)
- return ESP_FAIL;
- return (i2c_write_byte(setting, MPU6050_REGISTER_I2C_SLV0_DO + num, data));
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_external_shadow_delay_enabled(mpu6050_dev_t *setting, bool *enabled)
+esp_err_t mpu6050_get_external_sensor_data(mpu6050_dev_t *dev, int position, void *buf, size_t length)
{
- return (i2c_read_bit(setting, MPU6050_REGISTER_I2C_MST_DELAY_CTRL, MPU6050_DLYCTRL_DELAY_ES_SHADOW_BIT,
- (uint8_t *)enabled));
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_external_shadow_delay_enabled(mpu6050_dev_t *setting, bool enabled)
-{
- return (i2c_write_bit(setting, MPU6050_REGISTER_I2C_MST_DELAY_CTRL, MPU6050_DLYCTRL_DELAY_ES_SHADOW_BIT, enabled));
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_slave_delay_enabled(mpu6050_dev_t *setting, uint8_t num)
-{
- uint8_t byte = 0;
- if (num > 4)
- return ESP_FAIL;
+ CHECK_ARG(dev && buf && length && position < 24 && position + length - 1 < 24);
- ESP_ERROR_RETURN(i2c_read_bit(setting, MPU6050_REGISTER_I2C_MST_DELAY_CTRL, num, &(byte)));
+ I2C_DEV_TAKE_MUTEX(&dev->i2c_dev);
+ I2C_DEV_CHECK(&dev->i2c_dev, i2c_dev_read_reg(&dev->i2c_dev, MPU6050_REGISTER_EXT_SENS_DATA_00 + position, buf, length));
+ I2C_DEV_GIVE_MUTEX(&dev->i2c_dev);
- return ((byte == 0) ? DISABLE : ENABLE);
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_slave_delay_enabled(mpu6050_dev_t *setting, uint8_t num, bool enabled)
-{
- return (i2c_write_bit(setting, MPU6050_REGISTER_I2C_MST_DELAY_CTRL, num, enabled));
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_reset_gyroscope_path(mpu6050_dev_t *setting)
-{
- return (i2c_write_bit(setting, MPU6050_REGISTER_SIGNAL_PATH_RESET, MPU6050_PATHRESET_GYRO_RESET_BIT, 1));
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_reset_accelerometer_path(mpu6050_dev_t *setting)
-{
- return (i2c_write_bit(setting, MPU6050_REGISTER_SIGNAL_PATH_RESET, MPU6050_PATHRESET_ACCEL_RESET_BIT, 1));
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_reset_temperature_path(mpu6050_dev_t *setting)
-{
- return (i2c_write_bit(setting, MPU6050_REGISTER_SIGNAL_PATH_RESET, MPU6050_PATHRESET_TEMP_RESET_BIT, 1));
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_accelerometer_power_on_delay(mpu6050_dev_t *setting, uint8_t *delay)
-{
- return (i2c_read_bits(setting, MPU6050_REGISTER_MOT_DETECT_CTRL, MPU6050_DETECT_ACCEL_DELAY_BIT,
- MPU6050_DETECT_ACCEL_DELAY_LENGTH, delay));
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_accelerometer_power_on_delay(mpu6050_dev_t *setting, uint8_t delay)
-{
- return (i2c_write_bits(setting, MPU6050_REGISTER_MOT_DETECT_CTRL, MPU6050_DETECT_ACCEL_DELAY_BIT,
- MPU6050_DETECT_ACCEL_DELAY_LENGTH, delay));
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_freefall_detection_counter_decrement(mpu6050_dev_t *setting, uint8_t *decrement)
-{
- return (i2c_read_bits(setting, MPU6050_REGISTER_MOT_DETECT_CTRL, MPU6050_DETECT_FF_COUNT_BIT,
- MPU6050_DETECT_FF_COUNT_LENGTH, decrement));
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_freefall_detection_counter_decrement(mpu6050_dev_t *setting, uint8_t decrement)
-{
- return (i2c_write_bits(setting, MPU6050_REGISTER_MOT_DETECT_CTRL, MPU6050_DETECT_FF_COUNT_BIT,
- MPU6050_DETECT_FF_COUNT_LENGTH, decrement));
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_motion_detection_counter_decrement(mpu6050_dev_t *setting, uint8_t *decrement)
-{
- return (i2c_read_bits(setting, MPU6050_REGISTER_MOT_DETECT_CTRL, MPU6050_DETECT_MOT_COUNT_BIT,
- MPU6050_DETECT_MOT_COUNT_LENGTH, decrement));
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_motion_detection_counter_decrement(mpu6050_dev_t *setting, uint8_t decrement)
-{
- return (i2c_write_bits(setting, MPU6050_REGISTER_MOT_DETECT_CTRL, MPU6050_DETECT_MOT_COUNT_BIT,
- MPU6050_DETECT_MOT_COUNT_LENGTH, decrement));
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_fifo_enabled(mpu6050_dev_t *setting)
-{
- uint8_t enabled = 0;
- ESP_ERROR_RETURN(i2c_read_bit(setting, MPU6050_REGISTER_USER_CTRL, MPU6050_USERCTRL_FIFO_EN_BIT, &(enabled)));
- return ((enabled == 0) ? DISABLE : ENABLE);
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_fifo_enabled(mpu6050_dev_t *setting, bool enabled)
-{
- return (i2c_write_bit(setting, MPU6050_REGISTER_USER_CTRL, MPU6050_USERCTRL_FIFO_EN_BIT, enabled));
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_i2c_master_mode_enabled(mpu6050_dev_t *setting)
-{
- uint8_t enabled = 0;
- ESP_ERROR_RETURN(i2c_read_bit(setting, MPU6050_REGISTER_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, &(enabled)));
- return ((enabled == 0) ? DISABLE : ENABLE);
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_i2c_master_mode_enabled(mpu6050_dev_t *setting, bool enabled)
-{
- return (i2c_write_bit(setting, MPU6050_REGISTER_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, enabled));
+ return ESP_OK;
}
-esp_err_t mpu6050_switch_spie_enabled(mpu6050_dev_t *setting, bool enabled)
-{
- return (i2c_write_bit(setting, MPU6050_REGISTER_USER_CTRL, MPU6050_USERCTRL_I2C_IF_DIS_BIT, enabled));
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_reset_fifo(mpu6050_dev_t *setting)
-{
- return (i2c_write_bit(setting, MPU6050_REGISTER_USER_CTRL, MPU6050_USERCTRL_FIFO_RESET_BIT, 1));
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_reset_sensors(mpu6050_dev_t *setting)
-{
- return (i2c_write_bit(setting, MPU6050_REGISTER_USER_CTRL, MPU6050_USERCTRL_SIG_COND_RESET_BIT, 1));
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_reset(mpu6050_dev_t *setting)
-{
- return (i2c_write_bit(setting, MPU6050_REGISTER_PWR_MGMT_1, MPU6050_PWR1_DEVICE_RESET_BIT, 1));
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_sleep_enabled(mpu6050_dev_t *setting)
+esp_err_t mpu6050_get_motion_status(mpu6050_dev_t *dev, uint8_t *status)
{
- uint8_t enabled = 0;
- ESP_ERROR_RETURN(i2c_read_bit(setting, MPU6050_REGISTER_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, &(enabled)));
- return ((enabled == 0) ? DISABLE : ENABLE);
+ return read_reg(dev, MPU6050_REGISTER_MOT_DETECT_STATUS, status);
}
-esp_err_t mpu6050_set_sleep_enabled(mpu6050_dev_t *setting, bool enabled)
-{
- return i2c_write_bit(setting, MPU6050_REGISTER_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, enabled);
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_wake_cycle_enabled(mpu6050_dev_t *setting)
+esp_err_t mpu6050_set_slave_output_byte(mpu6050_dev_t *dev, mpu6050_slave_t num, uint8_t data)
{
- uint8_t enabled = 0;
- ESP_ERROR_RETURN(i2c_read_bit(setting, MPU6050_REGISTER_PWR_MGMT_1, MPU6050_PWR1_CYCLE_BIT, &(enabled)));
+ CHECK_ARG(num < MPU6050_SLAVE_4);
- return ((enabled == 0) ? DISABLE : ENABLE);
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_wake_cycle_enabled(mpu6050_dev_t *setting, bool enabled)
-{
- return (i2c_write_bit(setting, MPU6050_REGISTER_PWR_MGMT_1, MPU6050_PWR1_CYCLE_BIT, enabled));
+ return write_reg(dev, MPU6050_REGISTER_I2C_SLV0_DO + num, data);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_temp_sensor_enabled(mpu6050_dev_t *setting)
-{
- uint8_t enabled = 0;
- ESP_ERROR_RETURN(i2c_read_bit(setting, MPU6050_REGISTER_PWR_MGMT_1, MPU6050_PWR1_TEMP_DIS_BIT, &(enabled)));
- return ((enabled == 0) ? DISABLE : ENABLE);
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_temp_sensor_enabled(mpu6050_dev_t *setting, bool enabled)
-{
- return i2c_write_bit(setting, MPU6050_REGISTER_PWR_MGMT_1, MPU6050_PWR1_TEMP_DIS_BIT, !enabled);
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_clock_source(mpu6050_dev_t *setting, uint8_t *clk_setting)
-{
- return (i2c_read_bits(setting, MPU6050_REGISTER_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH,
- clk_setting));
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_clock_source(mpu6050_dev_t *setting, uint8_t source)
+esp_err_t mpu6050_get_external_shadow_delay_enabled(mpu6050_dev_t *dev, bool *enabled)
{
- return (i2c_write_bits(setting, MPU6050_REGISTER_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH,
- source));
+ return read_reg_bool(dev, MPU6050_REGISTER_I2C_MST_DELAY_CTRL, MPU6050_DLYCTRL_DELAY_ES_SHADOW_BIT, enabled);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_wake_frequency(mpu6050_dev_t *setting, uint8_t *frequency)
-{
- ESP_ERROR_RETURN(i2c_read_bits(setting, MPU6050_REGISTER_PWR_MGMT_2, MPU6050_PWR2_LP_WAKE_CTRL_BIT,
- MPU6050_PWR2_LP_WAKE_CTRL_LENGTH, frequency));
- return ESP_OK;
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_wake_frequency(mpu6050_dev_t *setting, uint8_t frequency)
+esp_err_t mpu6050_set_external_shadow_delay_enabled(mpu6050_dev_t *dev, bool enabled)
{
- return (i2c_write_bits(setting, MPU6050_REGISTER_PWR_MGMT_2, MPU6050_PWR2_LP_WAKE_CTRL_BIT,
- MPU6050_PWR2_LP_WAKE_CTRL_LENGTH, frequency));
+ return write_reg_bool(dev, MPU6050_REGISTER_I2C_MST_DELAY_CTRL, MPU6050_DLYCTRL_DELAY_ES_SHADOW_BIT, enabled);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_standby_x_accel_enabled(mpu6050_dev_t *setting)
-{
- uint8_t enabled = 0;
- ESP_ERROR_RETURN(i2c_read_bit(setting, MPU6050_REGISTER_PWR_MGMT_2, MPU6050_PWR2_STBY_XA_BIT, &(enabled)));
- return ((enabled == 0) ? DISABLE : ENABLE);
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_standby_x_accel_enabled(mpu6050_dev_t *setting, bool enabled)
-{
- return i2c_write_bit(setting, MPU6050_REGISTER_PWR_MGMT_2, MPU6050_PWR2_STBY_XA_BIT, enabled);
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_standby_y_accel_enabled(mpu6050_dev_t *setting)
+esp_err_t mpu6050_get_slave_delay_enabled(mpu6050_dev_t *dev, mpu6050_slave_t num, bool *enabled)
{
- uint8_t enabled = 0;
- ESP_ERROR_RETURN(i2c_read_bit(setting, MPU6050_REGISTER_PWR_MGMT_2, MPU6050_PWR2_STBY_YA_BIT, &(enabled)));
+ CHECK_ARG(num <= MPU6050_SLAVE_4);
- return ((enabled == 0) ? DISABLE : ENABLE);
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_standby_y_accel_enabled(mpu6050_dev_t *setting, bool enabled)
-{
- return (i2c_write_bit(setting, MPU6050_REGISTER_PWR_MGMT_2, MPU6050_PWR2_STBY_YA_BIT, enabled));
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_standby_z_accel_enabled(mpu6050_dev_t *setting)
-{
- uint8_t enabled = 0;
- ESP_ERROR_RETURN(i2c_read_bit(setting, MPU6050_REGISTER_PWR_MGMT_2, MPU6050_PWR2_STBY_ZA_BIT, &(enabled)));
- return ((enabled == 0) ? DISABLE : ENABLE);
+ return read_reg_bool(dev, MPU6050_REGISTER_I2C_MST_DELAY_CTRL, num, enabled);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_standby_z_accel_enabled(mpu6050_dev_t *setting, bool enabled)
-{
- return (i2c_write_bit(setting, MPU6050_REGISTER_PWR_MGMT_2, MPU6050_PWR2_STBY_ZA_BIT, enabled));
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_standby_x_gyro_enabled(mpu6050_dev_t *setting)
-{
- uint8_t enabled = 0;
- ESP_ERROR_RETURN(i2c_read_bit(setting, MPU6050_REGISTER_PWR_MGMT_2, MPU6050_PWR2_STBY_XG_BIT, &(enabled)));
- return ((enabled == 0) ? DISABLE : ENABLE);
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_standby_x_gyro_enabled(mpu6050_dev_t *setting, bool enabled)
-{
- return (i2c_write_bit(setting, MPU6050_REGISTER_PWR_MGMT_2, MPU6050_PWR2_STBY_XG_BIT, enabled));
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_standby_y_gyro_enabled(mpu6050_dev_t *setting)
-{
- uint8_t enabled = 0;
- ESP_ERROR_RETURN(i2c_read_bit(setting, MPU6050_REGISTER_PWR_MGMT_2, MPU6050_PWR2_STBY_YG_BIT, &(enabled)));
- return ((enabled == 0) ? DISABLE : ENABLE);
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_standby_y_gyro_enabled(mpu6050_dev_t *setting, bool enabled)
-{
- return (i2c_write_bit(setting, MPU6050_REGISTER_PWR_MGMT_2, MPU6050_PWR2_STBY_YG_BIT, enabled));
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_standby_z_gyro_enabled(mpu6050_dev_t *setting)
-{
- uint8_t enabled = 0;
- ESP_ERROR_RETURN(i2c_read_bit(setting, MPU6050_REGISTER_PWR_MGMT_2, MPU6050_PWR2_STBY_ZG_BIT, &(enabled)));
- return ((enabled == 0) ? DISABLE : ENABLE);
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_standby_z_gyro_enabled(mpu6050_dev_t *setting, bool enabled)
-{
- return (i2c_write_bit(setting, MPU6050_REGISTER_PWR_MGMT_2, MPU6050_PWR2_STBY_ZG_BIT, enabled));
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_fifo_count(mpu6050_dev_t *setting, uint16_t *count)
-{
- uint8_t buffer[2];
- ESP_ERROR_RETURN(i2c_read_bytes(setting, MPU6050_REGISTER_FIFO_COUNTH, 2, buffer));
- *count = ((((uint16_t)buffer[0]) << 8) | buffer[1]);
- return ESP_OK;
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_fifo_byte(mpu6050_dev_t *setting, uint8_t *byte)
-{
- ESP_ERROR_RETURN(i2c_read_byte(setting, MPU6050_REGISTER_FIFO_R_W, byte));
- return ESP_OK;
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_fifo_bytes(mpu6050_dev_t *setting, uint8_t *data, uint8_t length)
+esp_err_t mpu6050_set_slave_delay_enabled(mpu6050_dev_t *dev, mpu6050_slave_t num, bool enabled)
{
- if (length <= 0)
- {
- *data = 0;
- return ESP_ERR_INVALID_ARG;
- }
- return (i2c_read_bytes(setting, MPU6050_REGISTER_FIFO_R_W, length, data));
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_fifo_byte(mpu6050_dev_t *setting, uint8_t data)
-{
- return (i2c_write_byte(setting, MPU6050_REGISTER_FIFO_R_W, data));
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_device_id(mpu6050_dev_t *setting, uint8_t *id)
-{
- return (i2c_read_bits(setting, MPU6050_REGISTER_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH, id));
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_device_id(mpu6050_dev_t *setting, uint8_t id)
-{
- return (i2c_write_bits(setting, MPU6050_REGISTER_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH, id));
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_otp_bank_valid(mpu6050_dev_t *setting, uint8_t *valid)
-{
- return (i2c_read_bit(setting, MPU6050_REGISTER_XG_OFFS_TC, MPU6050_TC_OTP_BNK_VLD_BIT, valid));
+ CHECK_ARG(num <= MPU6050_SLAVE_4);
+
+ return write_reg_bool(dev, MPU6050_REGISTER_I2C_MST_DELAY_CTRL, num, enabled);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_otp_bank_valid(mpu6050_dev_t *setting, int8_t enabled)
+
+esp_err_t mpu6050_reset_gyroscope_path(mpu6050_dev_t *dev)
{
- return (i2c_write_bit(setting, MPU6050_REGISTER_XG_OFFS_TC, MPU6050_TC_OTP_BNK_VLD_BIT, enabled));
+ return write_reg_bool(dev, MPU6050_REGISTER_SIGNAL_PATH_RESET, MPU6050_PATHRESET_GYRO_RESET_BIT, 1);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_x_gyro_offset_tc(mpu6050_dev_t *setting, int8_t *offset)
+
+esp_err_t mpu6050_reset_accelerometer_path(mpu6050_dev_t *dev)
{
- return (i2c_read_bits(setting, MPU6050_REGISTER_XG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH,
- (uint8_t *)offset));
+ return write_reg_bool(dev, MPU6050_REGISTER_SIGNAL_PATH_RESET, MPU6050_PATHRESET_ACCEL_RESET_BIT, 1);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_x_gyro_offset_tc(mpu6050_dev_t *setting, int8_t offset)
+
+esp_err_t mpu6050_reset_temperature_path(mpu6050_dev_t *dev)
{
- return (
- i2c_write_bits(setting, MPU6050_REGISTER_XG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, offset));
+ return write_reg_bool(dev, MPU6050_REGISTER_SIGNAL_PATH_RESET, MPU6050_PATHRESET_TEMP_RESET_BIT, 1);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_y_gyro_offset_tc(mpu6050_dev_t *setting, int8_t *offset)
+
+esp_err_t mpu6050_get_accelerometer_power_on_delay(mpu6050_dev_t *dev, uint8_t *delay)
{
- return (i2c_read_bits(setting, MPU6050_REGISTER_YG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH,
- (uint8_t *)offset));
+ return read_reg_bits(dev, MPU6050_REGISTER_MOT_DETECT_CTRL, MPU6050_DETECT_ACCEL_DELAY_BIT, MPU6050_DETECT_ACCEL_DELAY_MASK, delay);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_y_gyro_offset_tc(mpu6050_dev_t *setting, int8_t offset)
+
+esp_err_t mpu6050_set_accelerometer_power_on_delay(mpu6050_dev_t *dev, uint8_t delay)
{
- return (
- i2c_write_bits(setting, MPU6050_REGISTER_YG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, offset));
+ return write_reg_bits(dev, MPU6050_REGISTER_MOT_DETECT_CTRL, MPU6050_DETECT_ACCEL_DELAY_BIT, MPU6050_DETECT_ACCEL_DELAY_MASK, delay);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_z_gyro_offset_tc(mpu6050_dev_t *setting, int8_t *offset)
+
+esp_err_t mpu6050_get_freefall_detection_counter_decrement(mpu6050_dev_t *dev, uint8_t *decrement)
{
- return (i2c_read_bits(setting, MPU6050_REGISTER_ZG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH,
- (uint8_t *)offset));
+ return read_reg_bits(dev, MPU6050_REGISTER_MOT_DETECT_CTRL, MPU6050_DETECT_FF_COUNT_BIT, MPU6050_DETECT_FF_COUNT_MASK, decrement);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_z_gyro_offset_tc(mpu6050_dev_t *setting, int8_t offset)
+
+esp_err_t mpu6050_set_freefall_detection_counter_decrement(mpu6050_dev_t *dev, uint8_t decrement)
{
- return (
- i2c_write_bits(setting, MPU6050_REGISTER_ZG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, offset));
+ return write_reg_bits(dev, MPU6050_REGISTER_MOT_DETECT_CTRL, MPU6050_DETECT_FF_COUNT_BIT, MPU6050_DETECT_FF_COUNT_MASK, decrement);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_x_fine_gain(mpu6050_dev_t *setting, int8_t *gain)
+
+esp_err_t mpu6050_get_motion_detection_counter_decrement(mpu6050_dev_t *dev, uint8_t *decrement)
{
- return (i2c_read_byte(setting, MPU6050_REGISTER_X_FINE_GAIN, (uint8_t *)gain));
+ return read_reg_bits(dev, MPU6050_REGISTER_MOT_DETECT_CTRL, MPU6050_DETECT_MOT_COUNT_BIT, MPU6050_DETECT_MOT_COUNT_MASK, decrement);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_x_fine_gain(mpu6050_dev_t *setting, int8_t gain)
+
+esp_err_t mpu6050_set_motion_detection_counter_decrement(mpu6050_dev_t *dev, uint8_t decrement)
{
- return (i2c_write_byte(setting, MPU6050_REGISTER_X_FINE_GAIN, gain));
+ return write_reg_bits(dev, MPU6050_REGISTER_MOT_DETECT_CTRL, MPU6050_DETECT_MOT_COUNT_BIT, MPU6050_DETECT_MOT_COUNT_MASK, decrement);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_y_fine_gain(mpu6050_dev_t *setting, int8_t *gain)
+
+esp_err_t mpu6050_get_fifo_enabled(mpu6050_dev_t *dev, bool *enabled)
{
- return (i2c_read_byte(setting, MPU6050_REGISTER_Y_FINE_GAIN, (uint8_t *)gain));
+ return read_reg_bool(dev, MPU6050_REGISTER_USER_CTRL, MPU6050_USERCTRL_FIFO_EN_BIT, enabled);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_y_fine_gain(mpu6050_dev_t *setting, int8_t gain)
+
+esp_err_t mpu6050_set_fifo_enabled(mpu6050_dev_t *dev, bool enabled)
{
- return (i2c_write_byte(setting, MPU6050_REGISTER_Y_FINE_GAIN, gain));
+ return write_reg_bool(dev, MPU6050_REGISTER_USER_CTRL, MPU6050_USERCTRL_FIFO_EN_BIT, enabled);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_z_fine_gain(mpu6050_dev_t *setting, int8_t *gain)
+
+esp_err_t mpu6050_get_i2c_master_mode_enabled(mpu6050_dev_t *dev, bool *enabled)
{
- return (i2c_read_byte(setting, MPU6050_REGISTER_Z_FINE_GAIN, (uint8_t *)gain));
+ return read_reg_bool(dev, MPU6050_REGISTER_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, enabled);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_z_fine_gain(mpu6050_dev_t *setting, int8_t gain)
+
+esp_err_t mpu6050_set_i2c_master_mode_enabled(mpu6050_dev_t *dev, bool enabled)
{
- return (i2c_write_byte(setting, MPU6050_REGISTER_Z_FINE_GAIN, gain));
+ return write_reg_bool(dev, MPU6050_REGISTER_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, enabled);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_x_accel_offset(mpu6050_dev_t *setting, int16_t *offset)
+
+esp_err_t mpu6050_switch_spie_enabled(mpu6050_dev_t *dev, bool enabled)
{
- uint8_t buffer[2];
- ESP_ERROR_RETURN(i2c_read_bytes(setting, MPU6050_REGISTER_XA_OFFS_H, 2, buffer));
- *offset = ((((int16_t)buffer[0]) << 8) | buffer[1]);
- return ESP_OK;
+ return write_reg_bool(dev, MPU6050_REGISTER_USER_CTRL, MPU6050_USERCTRL_I2C_IF_DIS_BIT, enabled);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_x_accel_offset(mpu6050_dev_t *setting, int16_t offset)
+
+esp_err_t mpu6050_reset_fifo(mpu6050_dev_t *dev)
{
- return (i2c_write_word(setting, MPU6050_REGISTER_XA_OFFS_H, offset));
+ return write_reg_bool(dev, MPU6050_REGISTER_USER_CTRL, MPU6050_USERCTRL_FIFO_RESET_BIT, 1);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_y_accel_offset(mpu6050_dev_t *setting, int16_t *offset)
+
+esp_err_t mpu6050_reset_sensors(mpu6050_dev_t *dev)
{
- uint8_t buffer[2];
- ESP_ERROR_RETURN(i2c_read_bytes(setting, MPU6050_REGISTER_YA_OFFS_H, 2, buffer));
- *offset = ((((int16_t)buffer[0]) << 8) | buffer[1]);
- return ESP_OK;
+ return write_reg_bool(dev, MPU6050_REGISTER_USER_CTRL, MPU6050_USERCTRL_SIG_COND_RESET_BIT, 1);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_y_accel_offset(mpu6050_dev_t *setting, int16_t offset)
+
+esp_err_t mpu6050_reset(mpu6050_dev_t *dev)
{
- return (i2c_write_word(setting, MPU6050_REGISTER_YA_OFFS_H, offset));
+ return write_reg_bool(dev, MPU6050_REGISTER_PWR_MGMT_1, MPU6050_PWR1_DEVICE_RESET_BIT, 1);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_z_accel_offset(mpu6050_dev_t *setting, int16_t *offset)
+
+esp_err_t mpu6050_get_sleep_enabled(mpu6050_dev_t *dev, bool *enabled)
{
- uint8_t buffer[2];
- ESP_ERROR_RETURN(i2c_read_bytes(setting, MPU6050_REGISTER_ZA_OFFS_H, 2, buffer));
- *offset = ((((int16_t)buffer[0]) << 8) | buffer[1]);
- return ESP_OK;
+ return read_reg_bool(dev, MPU6050_REGISTER_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, enabled);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_z_accel_offset(mpu6050_dev_t *setting, int16_t offset)
+
+esp_err_t mpu6050_set_sleep_enabled(mpu6050_dev_t *dev, bool enabled)
{
- return (i2c_write_word(setting, MPU6050_REGISTER_ZA_OFFS_H, offset));
+ return write_reg_bool(dev, MPU6050_REGISTER_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, enabled);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_x_gyro_offset(mpu6050_dev_t *setting, int16_t *offset)
+
+esp_err_t mpu6050_get_wake_cycle_enabled(mpu6050_dev_t *dev, bool *enabled)
{
- uint8_t buffer[2];
- ESP_ERROR_RETURN(i2c_read_bytes(setting, MPU6050_REGISTER_XG_OFFS_USRH, 2, buffer));
- *offset = ((((int16_t)buffer[0]) << 8) | buffer[1]);
- return ESP_OK;
+ return read_reg_bool(dev, MPU6050_REGISTER_PWR_MGMT_1, MPU6050_PWR1_CYCLE_BIT, enabled);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_x_gyro_offset(mpu6050_dev_t *setting, int16_t offset)
+
+esp_err_t mpu6050_set_wake_cycle_enabled(mpu6050_dev_t *dev, bool enabled)
{
- return (i2c_write_word(setting, MPU6050_REGISTER_XG_OFFS_USRH, offset));
+ return write_reg_bool(dev, MPU6050_REGISTER_PWR_MGMT_1, MPU6050_PWR1_CYCLE_BIT, enabled);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_y_gyro_offset(mpu6050_dev_t *setting, int16_t *offset)
+
+esp_err_t mpu6050_get_temp_sensor_enabled(mpu6050_dev_t *dev, bool *enabled)
{
- uint8_t buffer[2];
- ESP_ERROR_RETURN(i2c_read_bytes(setting, MPU6050_REGISTER_YG_OFFS_USRH, 2, buffer));
- *offset = ((((int16_t)buffer[0]) << 8) | buffer[1]);
+ CHECK(read_reg_bool(dev, MPU6050_REGISTER_PWR_MGMT_1, MPU6050_PWR1_TEMP_DIS_BIT, enabled));
+ *enabled = !*enabled;
+
return ESP_OK;
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_y_gyro_offset(mpu6050_dev_t *setting, int16_t offset)
+
+esp_err_t mpu6050_set_temp_sensor_enabled(mpu6050_dev_t *dev, bool enabled)
{
- return (i2c_write_word(setting, MPU6050_REGISTER_YG_OFFS_USRH, offset));
+ return write_reg_bool(dev, MPU6050_REGISTER_PWR_MGMT_1, MPU6050_PWR1_TEMP_DIS_BIT, !enabled);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_z_gyro_offset(mpu6050_dev_t *setting, int16_t *offset)
+
+esp_err_t mpu6050_get_clock_source(mpu6050_dev_t *dev, mpu6050_clock_source_t *source)
{
- uint8_t buffer[2];
- ESP_ERROR_RETURN(i2c_read_bytes(setting, MPU6050_REGISTER_ZG_OFFS_USRH, 2, buffer));
- *offset = ((((int16_t)buffer[0]) << 8) | buffer[1]);
- return ESP_OK;
+ return read_reg_bits(dev, MPU6050_REGISTER_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_MASK, (uint8_t *)source);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_z_gyro_offset(mpu6050_dev_t *setting, int16_t offset)
+
+esp_err_t mpu6050_set_clock_source(mpu6050_dev_t *dev, mpu6050_clock_source_t source)
{
- return (i2c_write_word(setting, MPU6050_REGISTER_ZG_OFFS_USRH, offset));
+ return write_reg_bits(dev, MPU6050_REGISTER_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_MASK, source);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_int_pll_ready_enabled(mpu6050_dev_t *setting)
-{
- uint8_t enabled = 0;
- ESP_ERROR_RETURN(i2c_read_bit(setting, MPU6050_REGISTER_INT_ENABLE, MPU6050_INTERRUPT_PLL_RDY_INT_BIT, &(enabled)));
- return ((enabled == 0) ? DISABLE : ENABLE);
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_int_pll_ready_enabled(mpu6050_dev_t *setting, bool enabled)
+esp_err_t mpu6050_get_wake_frequency(mpu6050_dev_t *dev, mpu6050_wake_freq_t *frequency)
{
- return i2c_write_bit(setting, MPU6050_REGISTER_INT_ENABLE, MPU6050_INTERRUPT_PLL_RDY_INT_BIT, enabled);
+ return read_reg_bits(dev, MPU6050_REGISTER_PWR_MGMT_2, MPU6050_PWR2_LP_WAKE_CTRL_BIT, MPU6050_PWR2_LP_WAKE_CTRL_MASK, (uint8_t *)frequency);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_int_dmp_enabled(mpu6050_dev_t *setting)
-{
- uint8_t enabled = 0;
- ESP_ERROR_RETURN(i2c_read_bit(setting, MPU6050_REGISTER_INT_ENABLE, MPU6050_INTERRUPT_DMP_INT_BIT, &(enabled)));
- return ((enabled == 0) ? DISABLE : ENABLE);
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_int_dmp_enabled(mpu6050_dev_t *setting, bool enabled)
+esp_err_t mpu6050_set_wake_frequency(mpu6050_dev_t *dev, mpu6050_wake_freq_t frequency)
{
- return i2c_write_bit(setting, MPU6050_REGISTER_INT_ENABLE, MPU6050_INTERRUPT_DMP_INT_BIT, enabled);
+ return write_reg_bits(dev, MPU6050_REGISTER_PWR_MGMT_2, MPU6050_PWR2_LP_WAKE_CTRL_BIT, MPU6050_PWR2_LP_WAKE_CTRL_MASK, frequency);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_dmp_int_5_status(mpu6050_dev_t *setting)
-{
- uint8_t status = 0;
- ESP_ERROR_RETURN(i2c_read_bit(setting, MPU6050_REGISTER_DMP_INT_STATUS, MPU6050_DMPINT_5_BIT, &(status)));
- return ((status == 0) ? ESP_FAIL : ESP_OK);
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_dmp_int_4_status(mpu6050_dev_t *setting)
-{
- uint8_t status = 0;
- ESP_ERROR_RETURN(i2c_read_bit(setting, MPU6050_REGISTER_DMP_INT_STATUS, MPU6050_DMPINT_4_BIT, &(status)));
+static const uint8_t standby_accel_bits[] = {
+ [MPU6050_X_AXIS] = MPU6050_PWR2_STBY_XA_BIT,
+ [MPU6050_Y_AXIS] = MPU6050_PWR2_STBY_YA_BIT,
+ [MPU6050_Z_AXIS] = MPU6050_PWR2_STBY_ZA_BIT,
+};
- return ((status == 0) ? ESP_FAIL : ESP_OK);
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_dmp_int_3_status(mpu6050_dev_t *setting)
+esp_err_t mpu6050_get_standby_accel_enabled(mpu6050_dev_t *dev, mpu6050_axis_t axis, bool *enabled)
{
- uint8_t status = 0;
- ESP_ERROR_RETURN(i2c_read_bit(setting, MPU6050_REGISTER_DMP_INT_STATUS, MPU6050_DMPINT_3_BIT, &(status)));
+ CHECK_ARG(axis <= MPU6050_Z_AXIS);
- return ((status == 0) ? ESP_FAIL : ESP_OK);
+ return read_reg_bool(dev, MPU6050_REGISTER_PWR_MGMT_2, standby_accel_bits[axis], enabled);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_dmp_int_2_status(mpu6050_dev_t *setting)
-{
- uint8_t status = 0;
- ESP_ERROR_RETURN(i2c_read_bit(setting, MPU6050_REGISTER_DMP_INT_STATUS, MPU6050_DMPINT_2_BIT, &(status)));
- return ((status == 0) ? ESP_FAIL : ESP_OK);
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_dmp_int_1_status(mpu6050_dev_t *setting)
+esp_err_t mpu6050_set_standby_accel_enabled(mpu6050_dev_t *dev, mpu6050_axis_t axis, bool enabled)
{
- uint8_t status = 0;
- ESP_ERROR_RETURN(i2c_read_bit(setting, MPU6050_REGISTER_DMP_INT_STATUS, MPU6050_DMPINT_1_BIT, &(status)));
+ CHECK_ARG(axis <= MPU6050_Z_AXIS);
- return ((status == 0) ? ESP_FAIL : ESP_OK);
+ return write_reg_bool(dev, MPU6050_REGISTER_PWR_MGMT_2, standby_accel_bits[axis], enabled);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_dmp_int_0_status(mpu6050_dev_t *setting)
-{
- uint8_t status = 0;
- ESP_ERROR_RETURN(i2c_read_bit(setting, MPU6050_REGISTER_DMP_INT_STATUS, MPU6050_DMPINT_0_BIT, &(status)));
- return ((status == 0) ? ESP_FAIL : ESP_OK);
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_int_ppl_ready_status(mpu6050_dev_t *setting)
-{
- uint8_t byte = 0;
- ESP_ERROR_RETURN(i2c_read_bit(setting, MPU6050_REGISTER_INT_STATUS, MPU6050_INTERRUPT_PLL_RDY_INT_BIT, &(byte)));
+static const uint8_t standby_gyro_bits[] = {
+ [MPU6050_X_AXIS] = MPU6050_PWR2_STBY_XG_BIT,
+ [MPU6050_Y_AXIS] = MPU6050_PWR2_STBY_YG_BIT,
+ [MPU6050_Z_AXIS] = MPU6050_PWR2_STBY_ZG_BIT,
+};
- return ((byte == 0) ? DISABLE : ENABLE);
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_int_dmp_status(mpu6050_dev_t *setting)
+esp_err_t mpu6050_get_standby_gyro_enabled(mpu6050_dev_t *dev, mpu6050_axis_t axis, bool *enabled)
{
- uint8_t byte = 0;
- ESP_ERROR_RETURN(i2c_read_bit(setting, MPU6050_REGISTER_INT_STATUS, MPU6050_INTERRUPT_DMP_INT_BIT, &(byte)));
+ CHECK_ARG(axis <= MPU6050_Z_AXIS);
- return ((byte == 0) ? DISABLE : ENABLE);
+ return read_reg_bool(dev, MPU6050_REGISTER_PWR_MGMT_2, standby_gyro_bits[axis], enabled);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_dmp_enabled(mpu6050_dev_t *setting)
-{
- uint8_t byte = 0;
- ESP_ERROR_RETURN(i2c_read_bit(setting, MPU6050_REGISTER_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT, &(byte)));
- return ((byte == 0) ? DISABLE : ENABLE);
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_dmp_enabled(mpu6050_dev_t *setting, bool enabled)
+esp_err_t mpu6050_set_standby_gyro_enabled(mpu6050_dev_t *dev, mpu6050_axis_t axis, bool enabled)
{
- return i2c_write_bit(setting, MPU6050_REGISTER_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT, enabled);
+ CHECK_ARG(axis <= MPU6050_Z_AXIS);
+
+ return write_reg_bool(dev, MPU6050_REGISTER_PWR_MGMT_2, standby_gyro_bits[axis], enabled);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_reset_dmp(mpu6050_dev_t *setting)
+
+esp_err_t mpu6050_get_fifo_count(mpu6050_dev_t *dev, uint16_t *count)
{
- return i2c_write_bit(setting, MPU6050_REGISTER_USER_CTRL, MPU6050_USERCTRL_DMP_RESET_BIT, 1);
+ CHECK_ARG(dev && count);
+
+ I2C_DEV_TAKE_MUTEX(&dev->i2c_dev);
+ I2C_DEV_CHECK(&dev->i2c_dev, i2c_dev_read_reg(&dev->i2c_dev, MPU6050_REGISTER_FIFO_COUNTH, count, 2));
+ I2C_DEV_GIVE_MUTEX(&dev->i2c_dev);
+
+ *count = ushuffle(*count);
+ return ESP_OK;
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_dmp_config_1(mpu6050_dev_t *setting, uint8_t *config)
+
+esp_err_t mpu6050_get_fifo_bytes(mpu6050_dev_t *dev, uint8_t *data, size_t length)
{
- return (i2c_read_byte(setting, MPU6050_REGISTER_DMP_CFG_1, config));
+ CHECK_ARG(dev && data && length);
+
+ I2C_DEV_TAKE_MUTEX(&dev->i2c_dev);
+ I2C_DEV_CHECK(&dev->i2c_dev, i2c_dev_read_reg(&dev->i2c_dev, MPU6050_REGISTER_FIFO_COUNTH, data, length));
+ I2C_DEV_GIVE_MUTEX(&dev->i2c_dev);
+
+ return ESP_OK;
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_dmp_config_1(mpu6050_dev_t *setting, uint8_t config)
+
+esp_err_t mpu6050_get_fifo_byte(mpu6050_dev_t *dev, uint8_t *data)
{
- return (i2c_write_byte(setting, MPU6050_REGISTER_DMP_CFG_1, config));
+ return read_reg(dev, MPU6050_REGISTER_FIFO_R_W, data);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_get_dmp_config_2(mpu6050_dev_t *setting, uint8_t *config)
+
+esp_err_t mpu6050_set_fifo_byte(mpu6050_dev_t *dev, uint8_t data)
{
- return (i2c_read_byte(setting, MPU6050_REGISTER_DMP_CFG_2, config));
+ return write_reg(dev, MPU6050_REGISTER_FIFO_R_W, data);
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_set_dmp_config_2(mpu6050_dev_t *setting, uint8_t config)
+
+esp_err_t mpu6050_get_device_id(mpu6050_dev_t *dev, uint8_t *id)
{
- return (i2c_write_byte(setting, MPU6050_REGISTER_DMP_CFG_2, config));
+ return read_reg_bits(dev, MPU6050_REGISTER_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_MASK, id);
}
-/* ------------------------------------------------------ */
-float mpu6050_get_accel_res(uint8_t accel_scale)
-{
- float accel_res = 0;
- switch (accel_scale)
- {
- case 0:
- accel_res = 2.0 / 32768.0;
- break;
- case 1:
- accel_res = 4.0 / 32768.0;
- break;
- case 2:
- accel_res = 8.0 / 32768.0;
- break;
- case 3:
- accel_res = 16.0 / 32768.0;
- break;
- }
-
- return (accel_res);
-}
-/* ------------------------------------------------------ */
-float mpu6050_get_gyro_res(uint8_t gyro_scale)
+esp_err_t mpu6050_calibrate(mpu6050_dev_t *dev, float *accel_bias_res, float *gyro_bias_res)
{
- float gyro_res = 0;
+ CHECK_ARG(dev && accel_bias_res && gyro_bias_res);
- switch (gyro_scale)
- {
- case 0:
- gyro_res = 250.0 / 32768.0;
- break;
- case 1:
- gyro_res = 500.0 / 32768.0;
- break;
- case 2:
- gyro_res = 1000.0 / 32768.0;
- break;
- case 3:
- gyro_res = 2000.0 / 32768.0;
- break;
- }
-
- return (gyro_res);
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_calibrate(mpu6050_dev_t *setting, float *accel_bias_res, float *gyro_bias_res)
-{
int16_t temp_offset[3];
int32_t accel_bias[3] = { 0, 0, 0 };
int32_t gyro_bias[3] = { 0, 0, 0 };
@@ -2306,61 +1237,61 @@ esp_err_t mpu6050_calibrate(mpu6050_dev_t *setting, float *accel_bias_res, float
uint8_t tmp_data[12];
uint16_t packet_count;
- ESP_ERROR_CDEBUG(mpu6050_reset(setting));
- vTaskDelay(100 / portTICK_PERIOD_MS);
+ CHECK(mpu6050_reset(dev));
+ vTaskDelay(pdMS_TO_TICKS(100));
- ESP_ERROR_CDEBUG(mpu6050_set_clock_source(setting, MPU6050_CLOCK_PLL_XGYRO));
- vTaskDelay(200 / portTICK_PERIOD_MS);
+ CHECK(mpu6050_set_clock_source(dev, MPU6050_CLOCK_PLL_X));
+ vTaskDelay(pdMS_TO_TICKS(200));
// Configure device for bias calculation:
- ESP_ERROR_CDEBUG(mpu6050_set_int_enabled(setting, false));
- ESP_ERROR_CDEBUG(mpu6050_set_fifo_enabled(setting, false));
- ESP_ERROR_CDEBUG(mpu6050_set_accel_fifo_enabled(setting, false));
- ESP_ERROR_CDEBUG(mpu6050_set_z_gyro_fifo_enabled(setting, false));
- ESP_ERROR_CDEBUG(mpu6050_set_y_gyro_fifo_enabled(setting, false));
- ESP_ERROR_CDEBUG(mpu6050_set_x_gyro_fifo_enabled(setting, false));
- ESP_ERROR_CDEBUG(mpu6050_set_temp_fifo_enabled(setting, false));
- ESP_ERROR_CDEBUG(mpu6050_set_clock_source(setting, MPU6050_CLOCK_INTERNAL));
- ESP_ERROR_CDEBUG(mpu6050_set_multi_master_enabled(setting, false));
- ESP_ERROR_CDEBUG(mpu6050_set_fifo_enabled(setting, false));
- ESP_ERROR_CDEBUG(mpu6050_set_i2c_master_mode_enabled(setting, false));
- ESP_ERROR_CDEBUG(mpu6050_reset_sensors(setting));
- vTaskDelay(15 / portTICK_PERIOD_MS);
+ CHECK(mpu6050_set_int_enabled(dev, false));
+ CHECK(mpu6050_set_fifo_enabled(dev, false));
+ CHECK(mpu6050_set_accel_fifo_enabled(dev, false));
+ CHECK(mpu6050_set_gyro_fifo_enabled(dev, MPU6050_Z_AXIS, false));
+ CHECK(mpu6050_set_gyro_fifo_enabled(dev, MPU6050_X_AXIS, false));
+ CHECK(mpu6050_set_gyro_fifo_enabled(dev, MPU6050_Y_AXIS, false));
+ CHECK(mpu6050_set_temp_fifo_enabled(dev, false));
+ CHECK(mpu6050_set_clock_source(dev, MPU6050_CLOCK_INTERNAL));
+ CHECK(mpu6050_set_multi_master_enabled(dev, false));
+ CHECK(mpu6050_set_fifo_enabled(dev, false));
+ CHECK(mpu6050_set_i2c_master_mode_enabled(dev, false));
+ CHECK(mpu6050_reset_sensors(dev));
+ vTaskDelay(pdMS_TO_TICKS(15));
// Configure MPU6050 gyro and accelerometer for bias calculation:
- ESP_ERROR_CDEBUG(mpu6050_set_rate(setting, false)); // Set sample rate to 1 kHz.
- ESP_ERROR_CDEBUG(mpu6050_set_dlpf_mode(setting, MPU6050_DLPF_BW_188));
- ESP_ERROR_CDEBUG(mpu6050_set_full_scale_accel_range(setting, MPU6050_ACCEL_FULL_SCALE_RANGE_2));
- ESP_ERROR_CDEBUG(mpu6050_set_full_scale_gyro_range(setting, MPU6050_GYRO_FULL_SCALE_RANGE_250));
+ CHECK(mpu6050_set_rate(dev, 0)); // Set sample rate to 1 kHz.
+ CHECK(mpu6050_set_dlpf_mode(dev, MPU6050_DLPF_1));
+ CHECK(mpu6050_set_full_scale_accel_range(dev, MPU6050_ACCEL_RANGE_2));
+ CHECK(mpu6050_set_full_scale_gyro_range(dev, MPU6050_GYRO_RANGE_250));
/**
* Configure FIFO to capture data for bias calculation.
*/
// Enable gyroscope and accelerometer sensors for FIFO:
- ESP_ERROR_CDEBUG(mpu6050_set_fifo_enabled(setting, true));
- ESP_ERROR_CDEBUG(mpu6050_set_accel_fifo_enabled(setting, true));
- ESP_ERROR_CDEBUG(mpu6050_set_z_gyro_fifo_enabled(setting, true));
- ESP_ERROR_CDEBUG(mpu6050_set_y_gyro_fifo_enabled(setting, true));
- ESP_ERROR_CDEBUG(mpu6050_set_x_gyro_fifo_enabled(setting, true));
- vTaskDelay(80 / portTICK_PERIOD_MS); // Accumulate 80 samples in 80 ms.
+ CHECK(mpu6050_set_fifo_enabled(dev, true));
+ CHECK(mpu6050_set_accel_fifo_enabled(dev, true));
+ CHECK(mpu6050_set_gyro_fifo_enabled(dev, MPU6050_Z_AXIS, true));
+ CHECK(mpu6050_set_gyro_fifo_enabled(dev, MPU6050_X_AXIS, true));
+ CHECK(mpu6050_set_gyro_fifo_enabled(dev, MPU6050_Y_AXIS, true));
+ vTaskDelay(pdMS_TO_TICKS(80)); // Accumulate 80 samples in 80 ms.
// At end of sample accumulation, turn off FIFO sensor read:
- ESP_ERROR_CDEBUG(mpu6050_set_fifo_enabled(setting, false));
- ESP_ERROR_CDEBUG(mpu6050_set_accel_fifo_enabled(setting, false));
- ESP_ERROR_CDEBUG(mpu6050_set_z_gyro_fifo_enabled(setting, false));
- ESP_ERROR_CDEBUG(mpu6050_set_y_gyro_fifo_enabled(setting, false));
- ESP_ERROR_CDEBUG(mpu6050_set_x_gyro_fifo_enabled(setting, false));
- ESP_ERROR_CDEBUG(mpu6050_set_temp_fifo_enabled(setting, false));
+ CHECK(mpu6050_set_fifo_enabled(dev, false));
+ CHECK(mpu6050_set_accel_fifo_enabled(dev, false));
+ CHECK(mpu6050_set_gyro_fifo_enabled(dev, MPU6050_Z_AXIS, false));
+ CHECK(mpu6050_set_gyro_fifo_enabled(dev, MPU6050_X_AXIS, false));
+ CHECK(mpu6050_set_gyro_fifo_enabled(dev, MPU6050_Y_AXIS, false));
+ CHECK(mpu6050_set_temp_fifo_enabled(dev, false));
// Sets of full gyro and accelerometer data for averaging:
- ESP_ERROR_CDEBUG(mpu6050_get_fifo_count(setting, &(packet_count)));
+ CHECK(mpu6050_get_fifo_count(dev, &packet_count));
packet_count /= 12;
for (int i = 0; i < packet_count; i++)
{
// Read data for averaging:
- ESP_ERROR_CDEBUG(mpu6050_get_fifo_bytes(setting, &tmp_data[0], 6));
+ CHECK(mpu6050_get_fifo_bytes(dev, &tmp_data[0], 6));
accel_temp[0] = (int16_t)(((int16_t)tmp_data[0] << 8) | tmp_data[1]);
accel_temp[1] = (int16_t)(((int16_t)tmp_data[2] << 8) | tmp_data[3]);
accel_temp[2] = (int16_t)(((int16_t)tmp_data[4] << 8) | tmp_data[5]);
@@ -2406,9 +1337,9 @@ esp_err_t mpu6050_calibrate(mpu6050_dev_t *setting, float *accel_bias_res, float
tmp_data[5] = (-gyro_bias[2] / 4) & 0xFF;
// Push gyro biases to hardware registers:
- ESP_ERROR_CDEBUG(mpu6050_set_x_gyro_offset(setting, ((int16_t)tmp_data[0]) << 8 | tmp_data[1]));
- ESP_ERROR_CDEBUG(mpu6050_set_y_gyro_offset(setting, ((int16_t)tmp_data[2]) << 8 | tmp_data[3]));
- ESP_ERROR_CDEBUG(mpu6050_set_z_gyro_offset(setting, ((int16_t)tmp_data[4]) << 8 | tmp_data[5]));
+ CHECK(mpu6050_set_gyro_offset(dev, MPU6050_X_AXIS, ((int16_t)tmp_data[0]) << 8 | tmp_data[1]));
+ CHECK(mpu6050_set_gyro_offset(dev, MPU6050_Y_AXIS, ((int16_t)tmp_data[2]) << 8 | tmp_data[3]));
+ CHECK(mpu6050_set_gyro_offset(dev, MPU6050_Z_AXIS, ((int16_t)tmp_data[4]) << 8 | tmp_data[5]));
// Construct gyro bias in deg/s for later manual subtraction:
gyro_bias_res[0] = (float)gyro_bias[0] / (float)gyro_sensitivity;
@@ -2426,9 +1357,9 @@ esp_err_t mpu6050_calibrate(mpu6050_dev_t *setting, float *accel_bias_res, float
*/
// Read factory accelerometer trim values:
- ESP_ERROR_CDEBUG(mpu6050_get_x_accel_offset(setting, &(temp_offset[0])));
- ESP_ERROR_CDEBUG(mpu6050_get_y_accel_offset(setting, &(temp_offset[1])));
- ESP_ERROR_CDEBUG(mpu6050_get_z_accel_offset(setting, &(temp_offset[2])));
+ CHECK(mpu6050_get_accel_offset(dev, MPU6050_X_AXIS, &(temp_offset[0])));
+ CHECK(mpu6050_get_accel_offset(dev, MPU6050_Y_AXIS, &(temp_offset[1])));
+ CHECK(mpu6050_get_accel_offset(dev, MPU6050_Z_AXIS, &(temp_offset[2])));
for (int i = 0; i < 3; i++)
{
@@ -2458,9 +1389,9 @@ esp_err_t mpu6050_calibrate(mpu6050_dev_t *setting, float *accel_bias_res, float
tmp_data[5] = tmp_data[5] | mask_bit[2];
// Push accelerometer biases to hardware registers:
- mpu6050_set_x_accel_offset(setting, ((int16_t)tmp_data[0]) << 8 | tmp_data[1]);
- mpu6050_set_y_accel_offset(setting, ((int16_t)tmp_data[2]) << 8 | tmp_data[3]);
- mpu6050_set_z_accel_offset(setting, ((int16_t)tmp_data[4]) << 8 | tmp_data[5]);
+ CHECK(mpu6050_set_accel_offset(dev, MPU6050_X_AXIS, ((int16_t)tmp_data[0]) << 8 | tmp_data[1]));
+ CHECK(mpu6050_set_accel_offset(dev, MPU6050_X_AXIS, ((int16_t)tmp_data[2]) << 8 | tmp_data[3]));
+ CHECK(mpu6050_set_accel_offset(dev, MPU6050_X_AXIS, ((int16_t)tmp_data[4]) << 8 | tmp_data[5]));
// Output scaled accelerometer biases for subtraction in the main program:
accel_bias_res[0] = (float)accel_bias[0] / (float)accel_sensitivity;
@@ -2469,189 +1400,37 @@ esp_err_t mpu6050_calibrate(mpu6050_dev_t *setting, float *accel_bias_res, float
return ESP_OK;
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_self_test(mpu6050_dev_t *setting, float *destination)
+
+esp_err_t mpu6050_self_test(mpu6050_dev_t *dev, float *destination)
{
uint8_t self_test[6];
float factory_trim[6];
// Configure the accelerometer for self-test:
- ESP_ERROR_CDEBUG(mpu6050_set_accel_x_self_test(setting, true));
- ESP_ERROR_CDEBUG(mpu6050_set_accel_y_self_test(setting, true));
- ESP_ERROR_CDEBUG(mpu6050_set_accel_z_self_test(setting, true));
- ESP_ERROR_CDEBUG(mpu6050_set_full_scale_accel_range(setting, MPU6050_ACCEL_FULL_SCALE_RANGE_8));
- ESP_ERROR_CDEBUG(mpu6050_set_full_scale_gyro_range(setting, MPU6050_GYRO_FULL_SCALE_RANGE_250));
-
- mpu6050_get_accel_x_self_test_factory_trim(setting, &(self_test[0]));
- mpu6050_get_accel_y_self_test_factory_trim(setting, &(self_test[1]));
- mpu6050_get_accel_z_self_test_factory_trim(setting, &(self_test[2]));
- mpu6050_get_gyro_x_self_test_factory_trim(setting, &(self_test[3]));
- mpu6050_get_gyro_y_self_test_factory_trim(setting, &(self_test[4]));
- mpu6050_get_gyro_z_self_test_factory_trim(setting, &(self_test[5]));
+ CHECK(mpu6050_set_accel_self_test(dev, MPU6050_X_AXIS, true));
+ CHECK(mpu6050_set_accel_self_test(dev, MPU6050_Y_AXIS, true));
+ CHECK(mpu6050_set_accel_self_test(dev, MPU6050_Z_AXIS, true));
+ CHECK(mpu6050_set_full_scale_accel_range(dev, MPU6050_ACCEL_RANGE_8));
+ CHECK(mpu6050_set_full_scale_gyro_range(dev, MPU6050_GYRO_RANGE_250));
+
+ CHECK(mpu6050_get_accel_self_test_factory_trim(dev, MPU6050_X_AXIS, &self_test[0]));
+ CHECK(mpu6050_get_accel_self_test_factory_trim(dev, MPU6050_Y_AXIS, &self_test[1]));
+ CHECK(mpu6050_get_accel_self_test_factory_trim(dev, MPU6050_Z_AXIS, &self_test[2]));
+ CHECK(mpu6050_get_gyro_self_test_factory_trim(dev, MPU6050_X_AXIS, &self_test[3]));
+ CHECK(mpu6050_get_gyro_self_test_factory_trim(dev, MPU6050_Y_AXIS, &self_test[4]));
+ CHECK(mpu6050_get_gyro_self_test_factory_trim(dev, MPU6050_Z_AXIS, &self_test[5]));
// Process results to allow final comparison with factory set values:
- factory_trim[0] = (4096.0f * 0.34f) * (pow((0.92f / 0.34f), ((self_test[0] - 1.0f) / 30.0f)));
- factory_trim[1] = (4096.0f * 0.34f) * (pow((0.92f / 0.34f), ((self_test[1] - 1.0f) / 30.0f)));
- factory_trim[2] = (4096.0f * 0.34f) * (pow((0.92f / 0.34f), ((self_test[2] - 1.0f) / 30.0f)));
- factory_trim[3] = (25.0f * 131.0f) * (pow(1.046f, (self_test[3] - 1.0f)));
- factory_trim[4] = (-25.0f * 131.0f) * (pow(1.046f, (self_test[4] - 1.0f)));
- factory_trim[5] = (25.0f * 131.0f) * (pow(1.046f, (self_test[5] - 1.0f)));
-
- // Report results as a ratio of "(STR - FT) / FT" (The change from Factory
- // Trim of the Self-Test Response).
+ factory_trim[0] = (4096.0f * 0.34f) * (powf((0.92f / 0.34f), (((float)self_test[0] - 1.0f) / 30.0f)));
+ factory_trim[1] = (4096.0f * 0.34f) * (powf((0.92f / 0.34f), (((float)self_test[1] - 1.0f) / 30.0f)));
+ factory_trim[2] = (4096.0f * 0.34f) * (powf((0.92f / 0.34f), (((float)self_test[2] - 1.0f) / 30.0f)));
+ factory_trim[3] = (25.0f * 131.0f) * (powf(1.046f, ((float)self_test[3] - 1.0f)));
+ factory_trim[4] = (-25.0f * 131.0f) * (powf(1.046f, ((float)self_test[4] - 1.0f)));
+ factory_trim[5] = (25.0f * 131.0f) * (powf(1.046f, ((float)self_test[5] - 1.0f)));
+
+ // Report results as a ratio of "(STR - FT) / FT" (The change from Factory Trim of the Self-Test Response).
// To get to percent, must multiply by 100 and subtract result from 100.
for (int i = 0; i < 6; i++)
- {
- destination[i] = 100.0f + 100.0f * (self_test[i] - factory_trim[i]) / factory_trim[i];
- }
+ destination[i] = 100.0f + 100.0f * ((float)self_test[i] - factory_trim[i]) / factory_trim[i];
return ESP_OK;
}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_madgwick_quaternion_update(float accel_x, float accel_y, float accel_z, float gyro_x, float gyro_y,
- float gyro_z)
-{
- float func_1, func_2, func_3;
- float j_11o24, j_12o23, j_13o22, j_14o21, j_32, j_33;
- float q_dot_1, q_dot_2, q_dot_3, q_dot_4;
- float hat_dot_1, hat_dot_2, hat_dot_3, hat_dot_4;
- float gyro_x_err, gyro_y_err, gyro_z_err;
- float gyro_x_bias, gyro_y_bias, gyro_z_bias;
- float norm;
-
- float half_q1 = 0.5f * quart[0];
- float half_q2 = 0.5f * quart[1];
- float half_q3 = 0.5f * quart[2];
- float half_q4 = 0.5f * quart[3];
- float double_q1 = 2.0f * quart[0];
- float double_q2 = 2.0f * quart[1];
- float double_q3 = 2.0f * quart[2];
- float double_q4 = 2.0f * quart[3];
-
- // Normalise accelerometer measurement:
- norm = sqrt(accel_x * accel_x + accel_y * accel_y + accel_z * accel_z);
-
- // Handle NaN:
- if (norm == 0.0f)
- return ESP_FAIL;
-
- norm = 1.0f / norm;
- accel_x *= norm;
- accel_y *= norm;
- accel_z *= norm;
-
- // Compute the objective function and Jacobian:
- func_1 = double_q2 * quart[3] - double_q1 * quart[2] - accel_x;
- func_2 = double_q1 * quart[1] + double_q3 * quart[3] - accel_y;
- func_3 = 1.0f - double_q2 * quart[3] - double_q3 * quart[2] - accel_z;
- j_11o24 = double_q3;
- j_12o23 = double_q4;
- j_13o22 = double_q1;
- j_14o21 = double_q2;
- j_32 = 2.0f * j_14o21;
- j_33 = 2.0f * j_11o24;
-
- // Compute the gradient (matrix multiplication):
- hat_dot_1 = j_14o21 * func_2 - j_11o24 * func_1;
- hat_dot_2 = j_12o23 * func_1 + j_13o22 * func_2 - j_32 * func_3;
- hat_dot_3 = j_12o23 * func_2 - j_33 * func_3 - j_13o22 * func_1;
- hat_dot_4 = j_14o21 * func_1 + j_11o24 * func_2;
-
- // Normalize the gradient:
- norm = sqrt(hat_dot_1 * hat_dot_1 + hat_dot_2 * hat_dot_2 + hat_dot_3 * hat_dot_3 + hat_dot_4 * hat_dot_4);
- hat_dot_1 /= norm;
- hat_dot_2 /= norm;
- hat_dot_3 /= norm;
- hat_dot_4 /= norm;
-
- // Compute estimated gyroscope biases:
- gyro_x_err = double_q1 * hat_dot_2 - double_q2 * hat_dot_1 - double_q3 * hat_dot_4 + double_q4 * hat_dot_3;
- gyro_y_err = double_q1 * hat_dot_3 + double_q2 * hat_dot_4 - double_q3 * hat_dot_1 - double_q4 * hat_dot_2;
- gyro_z_err = double_q1 * hat_dot_4 - double_q2 * hat_dot_3 + double_q3 * hat_dot_2 - double_q4 * hat_dot_1;
-
- // Compute and remove gyroscope biases:
- gyro_x_bias += gyro_x_err * delta_t * ZETA;
- gyro_y_bias += gyro_y_err * delta_t * ZETA;
- gyro_z_bias += gyro_z_err * delta_t * ZETA;
-
- // Compute the quaternion derivative:
- q_dot_1 = -half_q2 * gyro_x - half_q3 * gyro_y - half_q4 * gyro_z;
- q_dot_2 = half_q1 * gyro_x + half_q3 * gyro_z - half_q4 * gyro_y;
- q_dot_3 = half_q1 * gyro_y - half_q2 * gyro_z + half_q4 * gyro_x;
- q_dot_4 = half_q1 * gyro_z + half_q2 * gyro_y - half_q3 * gyro_x;
-
- // Compute then integrate estimated quaternion derivative:
- quart[0] += (q_dot_1 - (BETA * hat_dot_1)) * delta_t;
- quart[1] += (q_dot_2 - (BETA * hat_dot_2)) * delta_t;
- quart[2] += (q_dot_3 - (BETA * hat_dot_3)) * delta_t;
- quart[3] += (q_dot_4 - (BETA * hat_dot_4)) * delta_t;
-
- // Normalize the quaternion:
- norm = sqrt(quart[0] * quart[0] + quart[1] * quart[1] + quart[2] * quart[2] + quart[3] * quart[3]);
- norm = 1.0f / norm;
- quart[0] *= norm;
- quart[1] *= norm;
- quart[2] *= norm;
- quart[3] *= norm;
-
- return ESP_OK;
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_test_connection(mpu6050_dev_t *setting)
-{
- uint8_t id;
- return (((mpu6050_get_device_id(setting, &(id)) == ESP_OK) && (id == 0x34)) ? ESP_OK : ESP_FAIL);
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_init_desc(mpu6050_dev_t *mpu6050_config, uint8_t addr, i2c_port_t port, gpio_num_t sda_gpio,
- gpio_num_t scl_gpio)
-{
- ESP_PARAM_CHECK(mpu6050_config);
-
- esp_err_t ret = ESP_OK;
-
- if ((addr != MPU6050_ADDRESS_LOW) && (addr != MPU6050_ADDRESS_HIGH))
- {
- ESP_LOGE(TAG, "Invalid device address: 0x%02x", addr);
- return ESP_ERR_INVALID_ARG;
- }
- // Init i2cdev library
- ESP_ERROR_RETURN(i2cdev_init());
-
- mpu6050_config->port = port;
- mpu6050_config->addr = addr;
- mpu6050_config->cfg.sda_io_num = sda_gpio;
- mpu6050_config->cfg.scl_io_num = scl_gpio;
-#if HELPER_TARGET_IS_ESP32
- mpu6050_config->cfg.master.clk_speed = I2C_FREQ_HZ;
-#endif
-
- ret = i2c_dev_probe(mpu6050_config, I2C_DEV_READ);
- if (ret != ESP_OK)
- {
- ESP_LOGE(TAG, "[%s, %d] device not available <%s>", __func__, __LINE__, esp_err_to_name(ret));
- return ret;
- }
-
- ret = i2c_dev_create_mutex(mpu6050_config);
- if (ret != ESP_OK)
- {
- ESP_LOGE(TAG, "[%s, %d] failed to create mutex for i2c dev. <%s>", __func__, __LINE__, esp_err_to_name(ret));
- return ret;
- }
-
- ESP_ERROR_CDEBUG(mpu6050_set_clock_source(mpu6050_config, MPU6050_CLOCK_PLL_XGYRO));
- ESP_ERROR_CDEBUG(mpu6050_set_full_scale_gyro_range(mpu6050_config, MPU6050_GYRO_FULL_SCALE_RANGE_250));
- ESP_ERROR_CDEBUG(mpu6050_set_full_scale_accel_range(mpu6050_config, MPU6050_ACCEL_FULL_SCALE_RANGE_2));
- ESP_ERROR_CDEBUG(mpu6050_set_sleep_enabled(mpu6050_config, false));
-
- return ret;
-}
-/* ------------------------------------------------------ */
-esp_err_t mpu6050_deinit_desc(mpu6050_dev_t *mpu6050_config)
-{
- ESP_PARAM_CHECK(mpu6050_config);
-
- ESP_ERROR_RETURN(i2cdev_done());
- return (i2c_dev_delete_mutex(mpu6050_config));
-}
-/* ------------------------------------------------------ */
\ No newline at end of file
diff --git a/components/mpu6050/mpu6050.h b/components/mpu6050/mpu6050.h
index 9409ae46..618aede3 100644
--- a/components/mpu6050/mpu6050.h
+++ b/components/mpu6050/mpu6050.h
@@ -1,4 +1,3 @@
-
/*
* The MIT License (MIT)
*
@@ -26,24 +25,27 @@
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- *
*/
/**
* @file mpu6050.h
- * @defgroup
+ * @defgroup mpu6050 mpu6050
* @{
- * ESP-IDF driver for MPU6050 MEMS Sensor.
- * 6-axis motion tracking devices designed for the low power, low cost,
- * and high performance requirements of smartphones, tablets and wearable sensors.
+ * ESP-IDF driver for MPU6050 MEMS Sensor.
+ *
+ * 6-axis motion tracking devices designed for the low power, low cost,
+ * and high performance requirements of smartphones, tablets and wearable sensors.
*
- * Thanks to @see https://github.com/gabrielbvicari/esp32-mpu6050 for driver.
- * Improved by Raghav Jha https://github.com/horsemann07
+ * Copyright (c) 2012 Jeff Rowberg
+ * Copyright (c) 2019 Angelo Elias Dalzotto <150633@upf.br>
+ * Copyright (c) 2019 Gabriel Boni Vicari <133192@upf.br>
+ * Copyright (c) 2019 GEPID - Grupo de Pesquisa em Cultura Digital
+ * Copyright (c) 2023 Raghav Jha
+ * Copyright (c) 2023 Ruslan V. Uss
*/
#ifndef __MPU6050_H__
#define __MPU6050_H__
-/* i2cdev header */
#include
#include
@@ -52,73 +54,302 @@ extern "C" {
#endif
// Address of MPU6050 (Can be 0x68 or 0x69):
-#define MPU6050_ADDRESS_LOW (0x68) // Address pin low (GND).
-#define MPU6050_ADDRESS_HIGH (0x69) // Address pin high (VCC).
+#define MPU6050_I2C_ADDRESS_LOW (0x68) // Address pin low (GND).
+#define MPU6050_I2C_ADDRESS_HIGH (0x69) // Address pin high (VCC).
/**
- * @brief store mpu6050 acceleration data.
- *
+ * Raw acceleration data
+ */
+typedef struct
+{
+ int16_t x; //!< raw acceleration axis x
+ int16_t y; //!< raw acceleration axis y
+ int16_t z; //!< raw acceleration axis z
+} mpu6050_raw_acceleration_t;
+
+/**
+ * Raw rotation data
*/
typedef struct
{
- int16_t x; /**< acceleration axis x */
- int16_t y; /**< acceleration axis y */
- int16_t z; /**< acceleration axis z */
+ int16_t x; //!< raw rotation axis x
+ int16_t y; //!< raw rotation axis y
+ int16_t z; //!< raw rotation axis z
+} mpu6050_raw_rotation_t;
+
+/**
+ * MPU6050 acceleration data, g
+ */
+typedef struct
+{
+ float x; //!< acceleration axis x
+ float y; //!< acceleration axis y
+ float z; //!< acceleration axis z
} mpu6050_acceleration_t;
/**
- * @brief store mpu6050 rotation data.
- *
+ * MPU6050 rotation data, °/s
*/
typedef struct
{
- int16_t x; /**< rotation axis x */
- int16_t y; /**< rotation axis y */
- int16_t z; /**< rotation axis z */
+ float x; //!< rotation axis x
+ float y; //!< rotation axis y
+ float z; //!< rotation axis z
} mpu6050_rotation_t;
/**
- * @brief i2cdev mpu6050 config.
+ * Auxiliary I2C supply voltage levels
+ */
+typedef enum {
+ MPU6050_VDDIO_VLOGIC = 0,
+ MPU6050_VDDIO_VDD,
+} mpu6050_vddio_level_t;
+
+/**
+ * Axes
+ */
+typedef enum {
+ MPU6050_X_AXIS = 0,
+ MPU6050_Y_AXIS,
+ MPU6050_Z_AXIS,
+} mpu6050_axis_t;
+
+/**
+ * Clock sources
+ */
+typedef enum {
+ MPU6050_CLOCK_INTERNAL = 0, //!< Internal oscillator
+ MPU6050_CLOCK_PLL_X, //!< PLL with X Gyro reference
+ MPU6050_CLOCK_PLL_Y, //!< PLL with Y Gyro reference
+ MPU6050_CLOCK_PLL_Z, //!< PLL with Z Gyro reference
+ MPU6050_CLOCK_EXT_32768HZ, //!< PLL with external 32.768kHz reference
+ MPU6050_CLOCK_EXT_19_2MHZ, //!< PLL with external 19.2MHz reference
+ MPU6050_CLOCK_RESERVED,
+ MPU6050_CLOCK_STOP //!< Stops the clock and keeps the timing generator in reset
+} mpu6050_clock_source_t;
+
+/**
+ * Interrupt sources
+ */
+typedef enum {
+ MPU6050_INT_DATA_READY = BIT(0), //!< Data Ready interrupt which occurs each time a write operation to all of the sensor registers has been completed
+ MPU6050_INT_DMP = BIT(1), //!< Undocumented
+ MPU6050_INT_PLL_READY = BIT(2), //!< Undocumented
+ MPU6050_INT_I2C_MASTER = BIT(3), //!< Any of the I2C Master interrupt sources
+ MPU6050_INT_FIFO_OFLOW = BIT(4), //!< FIFO buffer overflow interrupt
+ MPU6050_INT_ZERO_MOTION = BIT(5), //!< Zero motion detection interrupt
+ MPU6050_INT_MOTION = BIT(6), //!< Motion detection interrupt
+ MPU6050_INT_FREEFALL = BIT(7), //!< Freefall detection interrupt
+} mpu6050_int_source_t;
+
+/**
+ * INT pin modes
+ */
+typedef enum {
+ MPU6050_INT_PUSH_PULL = 0, //!< Push-pull
+ MPU6050_INT_OPEN_DRAIN, //!< Open drain
+} mpu6050_int_drive_t;
+
+/**
+ * Location of the frame synchronization sampled bit
+ */
+typedef enum {
+ MPU6050_EXT_SYNC_DISABLED = 0,
+ MPU6050_EXT_SYNC_TEMP_OUT,
+ MPU6050_EXT_SYNC_GYRO_XOUT,
+ MPU6050_EXT_SYNC_GYRO_YOUT,
+ MPU6050_EXT_SYNC_GYRO_ZOUT,
+ MPU6050_EXT_SYNC_ACCEL_XOUT,
+ MPU6050_EXT_SYNC_ACCEL_YOUT,
+ MPU6050_EXT_SYNC_ACCEL_ZOUT,
+} mpu6050_ext_sync_t;
+
+/**
+ * Gyroscope and accelerometer filter values
+ */
+typedef enum {
+ MPU6050_DLPF_0 = 0, //!< Accelerometer: BW = 260Hz, delay = 0, Gyroscope: BW = 256Hz, delay = 0.98ms, Fs = 8kHz
+ MPU6050_DLPF_1, //!< Accelerometer: BW = 184z, delay = 2ms, Gyroscope: BW = 188Hz, delay = 1.9ms, Fs = 1kHz
+ MPU6050_DLPF_2, //!< Accelerometer: BW = 94Hz, delay = 3ms, Gyroscope: BW = 98Hz, delay = 2.8ms, Fs = 1kHz
+ MPU6050_DLPF_3, //!< Accelerometer: BW = 44Hz, delay = 4.9ms, Gyroscope: BW = 42Hz, delay = 4.8ms, Fs = 1kHz
+ MPU6050_DLPF_4, //!< Accelerometer: BW = 21Hz, delay = 8.5ms, Gyroscope: BW = 20Hz, delay = 8.3ms, Fs = 1kHz
+ MPU6050_DLPF_5, //!< Accelerometer: BW = 10Hz, delay = 13.8ms, Gyroscope: BW = 10Hz, delay = 13.4ms, Fs = 1kHz
+ MPU6050_DLPF_6, //!< Accelerometer: BW = 5Hz, delay = 19.0ms, Gyroscope: BW = 5Hz, delay = 18.6ms, Fs = 1kHz
+} mpu6050_dlpf_mode_t;
+
+/**
+ * Scale ranges for gyroscope
+ */
+typedef enum {
+ MPU6050_GYRO_RANGE_250 = 0, //!< ± 250 °/s
+ MPU6050_GYRO_RANGE_500, //!< ± 500 °/s
+ MPU6050_GYRO_RANGE_1000, //!< ± 1000 °/s
+ MPU6050_GYRO_RANGE_2000, //!< ± 2000 °/s
+} mpu6050_gyro_range_t;
+
+/**
+ * Scale ranges for accelerometer
+ */
+typedef enum {
+ MPU6050_ACCEL_RANGE_2 = 0, //!< ± 2g
+ MPU6050_ACCEL_RANGE_4, //!< ± 4g
+ MPU6050_ACCEL_RANGE_8, //!< ± 8g
+ MPU6050_ACCEL_RANGE_16, //!< ± 16g
+} mpu6050_accel_range_t;
+
+/**
+ * Digital high pass filter modes
+ */
+typedef enum {
+ MPU6050_DHPF_RESET = 0, //!< Filter Mode = reset, Cut-off Frequency = None
+ MPU6050_DHPF_5, //!< Filter Mode = on, Cut-off Frequency = 5Hz
+ MPU6050_DHPF_2_5, //!< Filter Mode = on, Cut-off Frequency = 2.5Hz
+ MPU6050_DHPF_1_25, //!< Filter Mode = on, Cut-off Frequency = 1.25Hz
+ MPU6050_DHPF_0_63, //!< Filter Mode = on, Cut-off Frequency = 0.63Hz
+ MPU6050_DHPF_HOLD, //!< Filter Mode = hold, Cut-off Frequency = None
+} mpu6050_dhpf_mode_t;
+
+/**
+ * I2C slave numbers
+ */
+typedef enum {
+ MPU6050_SLAVE_0 = 0,
+ MPU6050_SLAVE_1,
+ MPU6050_SLAVE_2,
+ MPU6050_SLAVE_3,
+ MPU6050_SLAVE_4,
+} mpu6050_slave_t;
+
+/**
+ * I2C master clock
+ */
+typedef enum {
+ MPU6050_I2C_MASTER_CLOCK_348 = 0, //!< 348kHz
+ MPU6050_I2C_MASTER_CLOCK_333, //!< 333kHz
+ MPU6050_I2C_MASTER_CLOCK_320, //!< 320kHz
+ MPU6050_I2C_MASTER_CLOCK_308, //!< 308kHz
+ MPU6050_I2C_MASTER_CLOCK_296, //!< 296kHz
+ MPU6050_I2C_MASTER_CLOCK_286, //!< 286kHz
+ MPU6050_I2C_MASTER_CLOCK_276, //!< 276kHz
+ MPU6050_I2C_MASTER_CLOCK_267, //!< 267kHz
+ MPU6050_I2C_MASTER_CLOCK_258, //!< 258kHz
+ MPU6050_I2C_MASTER_CLOCK_500, //!< 500kHz
+ MPU6050_I2C_MASTER_CLOCK_471, //!< 471kHz
+ MPU6050_I2C_MASTER_CLOCK_444, //!< 444kHz
+ MPU6050_I2C_MASTER_CLOCK_421, //!< 421kHz
+ MPU6050_I2C_MASTER_CLOCK_400, //!< 400kHz
+ MPU6050_I2C_MASTER_CLOCK_381, //!< 381kHz
+ MPU6050_I2C_MASTER_CLOCK_364, //!< 364kHz
+} mpu6050_i2c_master_clock_t;
+
+/**
+ * Interrupt levels
+ */
+typedef enum {
+ MPU6050_INT_LEVEL_HIGH = 0, //!< Active high
+ MPU6050_INT_LEVEL_LOW, //!< Active low
+} mpu6050_int_level_t;
+
+/**
+ * Interrupt latch modes
+ */
+typedef enum {
+ MPU6050_INT_LATCH_PULSE = 0, //!< 50 us pulse
+ MPU6050_INT_LATCH_CONTINUOUS, //!< Latch until cleared
+} mpu6050_int_latch_t;
+
+/**
+ * The frequencies of wake-ups in Accelerometer Only Low Power Mode
+ */
+typedef enum {
+ MPU6050_WAKE_FREQ_1_25 = 0, //!< 1.25Hz
+ MPU6050_WAKE_FREQ_5, //!< 5Hz
+ MPU6050_WAKE_FREQ_20, //!< 20Hz
+ MPU6050_WAKE_FREQ_40, //!< 40Hz
+} mpu6050_wake_freq_t;
+
+/**
+ * Motion detection status flags
+ */
+typedef enum {
+ MPU6050_MOTION_ZERO = BIT(0),
+ MPU6050_MOTION_Z_POS = BIT(2),
+ MPU6050_MOTION_Z_NEG = BIT(3),
+ MPU6050_MOTION_Y_POS = BIT(4),
+ MPU6050_MOTION_Y_NEG = BIT(5),
+ MPU6050_MOTION_X_POS = BIT(6),
+ MPU6050_MOTION_X_NEG = BIT(7),
+} mpu6050_motion_det_flags_t;
+
+/**
+ * Device descriptor
+ */
+typedef struct {
+ i2c_dev_t i2c_dev;
+ struct
+ {
+ mpu6050_gyro_range_t gyro;
+ mpu6050_accel_range_t accel;
+ } ranges;
+} mpu6050_dev_t;
+
+/**
+ * @brief Initialize device descriptor.
+ *
+ * @param dev Device descriptor
+ * @return `ESP_OK` on success
+ */
+esp_err_t mpu6050_init_desc(mpu6050_dev_t *dev, uint8_t addr, i2c_port_t port, gpio_num_t sda_gpio, gpio_num_t scl_gpio);
+
+/**
+ * @brief Free device descriptor.
+ *
+ * @param dev Device descriptor
+ * @return `ESP_OK` on success
+ */
+esp_err_t mpu6050_free_desc(mpu6050_dev_t *dev);
+
+/**
+ * @brief Initialize device.
*
+ * @param dev Device descriptor
+ * @return `ESP_OK` on success
*/
-typedef i2c_dev_t mpu6050_dev_t;
+esp_err_t mpu6050_init(mpu6050_dev_t *dev);
/**
* @brief Get the auxiliary I2C supply voltage level.
+ *
* When set to 1, the auxiliary I2C bus high logic level is VDD. When cleared to
* 0, the auxiliary I2C bus high logic level is VLOGIC. This does not apply to
* the MPU-6000, which does not have a VLOGIC pin.
*
- * I2C supply voltage level
- *
- * @param[in] setting mpu6050 i2cdev setting
- * @param[out] level I2C supply voltage level (0 = VLOGIC, 1 = VDD).
+ * @param dev Device descriptor
+ * @param[out] level I2C supply voltage level
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_aux_vddio_level(mpu6050_dev_t *setting, uint8_t *level);
+esp_err_t mpu6050_get_aux_vddio_level(mpu6050_dev_t *dev, mpu6050_vddio_level_t *level);
/**
* @brief Set the auxiliary I2C supply voltage level.
+ *
* When set to 1, the auxiliary I2C bus high logic level is VDD. When cleared to
* 0, the auxiliary I2C bus high logic level is VLOGIC. This does not apply to
- * the MPU-6000, which does not have a VLOGIC pin.
+ * the MPU6000, which does not have a VLOGIC pin.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] level I2C supply voltage level (0 = VLOGIC, 1 = VDD).
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @param dev Device descriptor
+ * @param level I2C supply voltage level (0 = VLOGIC, 1 = VDD).
+ * @return `ESP_OK` on success
*
*/
-esp_err_t mpu6050_set_aux_vddio_level(mpu6050_dev_t *setting, uint8_t level);
+esp_err_t mpu6050_set_aux_vddio_level(mpu6050_dev_t *dev, mpu6050_vddio_level_t level);
/**
- * @brief Get gyroscope output rate divider. The sensor register output,
- * FIFO output, DMP sampling, Motion Detection, Zero Motion Detection,
- * and Free Fall Detection are all based on the Sample Rate.
+ * @brief Get gyroscope output rate divider.
+ *
+ * The sensor register output, FIFO output, DMP sampling, Motion Detection,
+ * Zero Motion Detection and Free Fall Detection are all based on the Sample Rate.
* The Sample Rate is generated by dividing the gyroscope output rate by
* SMPLRT_DIV:
*
@@ -130,296 +361,166 @@ esp_err_t mpu6050_set_aux_vddio_level(mpu6050_dev_t *setting, uint8_t level);
* Note: The accelerometer output rate is 1kHz. This means that for a Sample
* Rate greater than 1kHz, the same accelerometer sample may be output to the
* FIFO, DMP, and sensor registers more than once.
- * @param[in] setting mpu6050 i2cdev setting
+ *
+ * @param dev Device descriptor
* @param[out] rate: accelerometer sample rate
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_rate(mpu6050_dev_t *setting, uint8_t *rate);
+esp_err_t mpu6050_get_rate(mpu6050_dev_t *dev, uint8_t *rate);
/**
* @brief Set gyroscope output rate divider.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] rate New sample rate divider.
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @param dev Device descriptor
+ * @param rate New sample rate divider.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_set_rate(mpu6050_dev_t *setting, uint8_t rate);
+esp_err_t mpu6050_set_rate(mpu6050_dev_t *dev, uint8_t rate);
/**
- * @brief Get external FSYNC configuration. Configures the external
- * Frame Synchronization (FSYNC) pin sampling. An external signal connected to
- * the FSYNC pin can be sampled by configuring EXT_SYNC_SET. Signal changes to
- * the FSYNC pin are latched so that short strobes may be captured. The latched
- * FSYNC signal will be sampled at the Sampling Rate, as defined in register 25.
+ * @brief Get external FSYNC configuration.
+ *
+ * Configures the external Frame Synchronization (FSYNC) pin sampling.
+ * An external signal connected to the FSYNC pin can be sampled by configuring
+ * EXT_SYNC_SET. Signal changes to the FSYNC pin are latched so that short
+ * strobes may be captured. The latched FSYNC signal will be sampled at the
+ * Sampling Rate, as defined in register 25.
* After sampling, the latch will reset to the current FSYNC signal state.
* The sampled value will be reported in place of the least significant bit in
- * a sensor data register determined by the value of EXT_SYNC_SET according to
- * the following table:
- *
- * EXT_SYNC_SET | FSYNC Bit Location
- * -------------+-------------------
- * 0 | Input disabled
- * 1 | TEMP_OUT_L [0]
- * 2 | GYRO_XOUT_L [0]
- * 3 | GYRO_YOUT_L [0]
- * 4 | GYRO_ZOUT_L [0]
- * 5 | ACCEL_XOUT_L [0]
- * 6 | ACCEL_YOUT_L [0]
- * 7 | ACCEL_ZOUT_L [0]
- * -------------+-------------------
- * @param[in] setting mpu6050 i2cdev setting
- * @param[out] sync: FSYNC configuration value.
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
- */
-esp_err_t mpu6050_get_external_frame_sync(mpu6050_dev_t *setting, uint8_t *sync);
+ * a sensor data register determined by the value of EXT_SYNC_SET.
+ *
+ * @param dev Device descriptor
+ * @param[out] sync FSYNC configuration value.
+ * @return `ESP_OK` on success
+ */
+esp_err_t mpu6050_get_external_frame_sync(mpu6050_dev_t *dev, mpu6050_ext_sync_t *sync);
/**
* @brief Set external FSYNC configuration.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] sync New FSYNC configuration value.
+ * @param dev Device descriptor
+ * @param sync New FSYNC configuration value.
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_set_external_frame_sync(mpu6050_dev_t *setting, uint8_t sync);
+esp_err_t mpu6050_set_external_frame_sync(mpu6050_dev_t *dev, mpu6050_ext_sync_t sync);
/**
- * @brief: Get digital low-pass filter configuration. The DLPF_CFG parameter
- * sets the digital low pass filter configuration. It also determines the
- * internal sampling rate used by the device as shown in the table below.
+ * @brief: Get digital low-pass filter configuration.
+ *
+ * The DLPF_CFG parameter sets the digital low pass filter configuration.
+ * It also determines the internal sampling rate used by the device as shown
+ * in the table below.
+ *
* Note: The accelerometer output rate is 1kHz. This means that for a Sample
* Rate greater than 1kHz, the same accelerometer sample may be output to the
* FIFO, DMP, and sensor registers more than once.
*
- *| | ACCELEROMETER | GYROSCOPE | |
- *|DLPF_CFG | Bandwidth | Delay | Bandwidth | Delay | Sample Rate |
- *|---------|-----------|--------|-----------|--------|-------------|
- *| 0 | 260Hz | 0ms | 256Hz | 0.98ms | 8kHz |
- *| 1 | 184Hz | 2.0ms | 188Hz | 1.9ms | 1kHz |
- *| 2 | 94Hz | 3.0ms | 98Hz | 2.8ms | 1kHz |
- *| 3 | 44Hz | 4.9ms | 42Hz | 4.8ms | 1kHz |
- *| 4 | 21Hz | 8.5ms | 20Hz | 8.3ms | 1kHz |
- *| 5 | 10Hz | 13.8ms | 10Hz | 13.4ms | 1kHz |
- *| 6 | 5Hz | 19.0ms | 5Hz | 18.6ms | 1kHz |
- *| 7 | Reserved | Reserved | Reserved |
- *
- *
- * @param[in] setting mpu6050 i2cdev setting
+ * @param dev Device descriptor
* @param[out] mode: DLFP configuration.
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ *
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_dlpf_mode(mpu6050_dev_t *setting, uint8_t *mode);
+esp_err_t mpu6050_get_dlpf_mode(mpu6050_dev_t *dev, mpu6050_dlpf_mode_t *mode);
/**
* @brief Set digital low-pass filter configuration.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] mode New DLFP configuration setting.
+ * @param dev Device descriptor
+ * @param mode New DLFP configuration setting.
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_set_dlpf_mode(mpu6050_dev_t *setting, uint8_t mode);
+esp_err_t mpu6050_set_dlpf_mode(mpu6050_dev_t *dev, mpu6050_dlpf_mode_t mode);
/**
- * @brief Get full-scale gyroscope range. The FS_SEL parameter allows setting
- * the full-scale range of the gyro sensors, as described below:
+ * @brief Get full-scale gyroscope range.
*
- * - 0 = +/- 250 degrees/sec
- * - 1 = +/- 500 degrees/sec
- * - 2 = +/- 1000 degrees/sec
- * - 3 = +/- 2000 degrees/sec
+ * The FS_SEL parameter allows setting the full-scale range of the gyro
+ * sensors.
*
- * @param[in] setting mpu6050 i2cdev setting
+ * @param dev Device descriptor
* @param[out] gyro_range full-scale gyroscope range setting.
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_full_scale_gyro_range(mpu6050_dev_t *setting, uint8_t *gyro_range);
+esp_err_t mpu6050_get_full_scale_gyro_range(mpu6050_dev_t *dev, mpu6050_gyro_range_t *gyro_range);
/**
* @brief Set full-scale gyroscope range.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] range New full-scale gyroscope range value.
- *
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
- */
-esp_err_t mpu6050_set_full_scale_gyro_range(mpu6050_dev_t *setting, uint8_t range);
-
-/**
- * @brief Get self-test factory trim value for accelerometer X axis.
- *
- * @param[in] setting mpu6050 i2cdev setting
- * @param[out] accel_x Factory trim value.
- *
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
- */
-esp_err_t mpu6050_get_accel_x_self_test_factory_trim(mpu6050_dev_t *setting, uint8_t *accel_x);
-
-/**
- * @brief Get self-test factory trim value for accelerometer Y axis.
- *
- * @param[in] setting mpu6050 i2cdev setting
- * @param[out] accel_y Factory trim value.
+ * @param dev Device descriptor
+ * @param range New full-scale gyroscope range value.
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_accel_y_self_test_factory_trim(mpu6050_dev_t *setting, uint8_t *accel_y);
+esp_err_t mpu6050_set_full_scale_gyro_range(mpu6050_dev_t *dev, mpu6050_gyro_range_t range);
/**
- * @brief Get self-test factory trim value for accelerometer Z axis.
+ * @brief Get self-test factory trim value for accelerometer axis.
*
+ * @param dev Device descriptor
+ * @param axis Accelerometer axis
+ * @param[out] trim Factory trim value.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[out] accel_z Factory trim value.
- *
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_accel_z_self_test_factory_trim(mpu6050_dev_t *setting, uint8_t *accel_z);
+esp_err_t mpu6050_get_accel_self_test_factory_trim(mpu6050_dev_t *dev, mpu6050_axis_t axis, uint8_t *trim);
/**
- * @brief Get self-test factory trim value for gyroscope X axis.
+ * @brief Get self-test factory trim value for gyroscope axis.
*
+ * @param dev Device descriptor
+ * @param axis Gyroscope axis
+ * @param[out] trim Factory trim value.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[out] gypr_x Factory trim value.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_gyro_x_self_test_factory_trim(mpu6050_dev_t *setting, uint8_t *gypr_x);
+esp_err_t mpu6050_get_gyro_self_test_factory_trim(mpu6050_dev_t *dev, mpu6050_axis_t axis, uint8_t *trim);
/**
- * @brief Get self-test factory trim value for gyroscope Y axis.
+ * @brief Get self-test enabled for accelerometer axis.
*
+ * @param dev Device descriptor
+ * @param axis Accelerometer axis
+ * @param[out] enabled true if self-test enabled
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[out] gypr_y Factory trim value.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_gyro_y_self_test_factory_trim(mpu6050_dev_t *setting, uint8_t *gypr_y);
+esp_err_t mpu6050_get_accel_self_test(mpu6050_dev_t *dev, mpu6050_axis_t axis, bool *enabled);
/**
- * @brief Get self-test factory trim value for gyroscope Z axis.
+ * @brief Set self-test enabled for accelerometer axis.
*
+ * @param dev Device descriptor
+ * @param axis Accelerometer axis
+ * @param enabled Self test enabled value
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[out] gypr_z Factory trim value.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_gyro_z_self_test_factory_trim(mpu6050_dev_t *setting, uint8_t *gypr_z);
-
-/**
- * @brief Get self-test enabled setting for accelerometer X axis.
- *
- *
- * @param[in] setting mpu6050 i2cdev setting
- * @return
- * - ESP_OK: enable
- * - ESP_FAIL: disable
- */
-esp_err_t mpu6050_get_accel_x_self_test(mpu6050_dev_t *setting);
-
-/**
- * @brief Set self-test enabled setting for accelerometer X axis.
- *
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] enabled Self test enabled value.
- *
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
- */
-esp_err_t mpu6050_set_accel_x_self_test(mpu6050_dev_t *setting, bool enabled);
-
-/**
- * @brief Get self-test enabled setting for accelerometer Y axis.
- * @param[in] setting mpu6050 i2cdev setting
- *
- * @return
- * - ESP_OK: enable
- * - ESP_FAIL: disable
- */
-esp_err_t mpu6050_get_accel_y_self_test(mpu6050_dev_t *setting);
-
-/**
- * @brief Set self-test enabled setting for accelerometer Y axis.
- *
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] enabled Self test enabled value.
- *
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
- */
-esp_err_t mpu6050_set_accel_y_self_test(mpu6050_dev_t *setting, bool enabled);
-
-/**
- * @brief Get self-test enabled setting for accelerometer Z axis.
- *
- * @param[in] setting mpu6050 i2cdev setting
- * @return
- * - ESP_OK: enable
- * - ESP_FAIL: disable
- */
-esp_err_t mpu6050_get_accel_z_self_test(mpu6050_dev_t *setting);
-
-/**
- * @brief Set self-test enabled setting for accelerometer Z axis.
- *
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] enabled Self test enabled value.
- *
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
- */
-esp_err_t mpu6050_set_accel_z_self_test(mpu6050_dev_t *setting, bool enabled);
+esp_err_t mpu6050_set_accel_self_test(mpu6050_dev_t *dev, mpu6050_axis_t axis, bool enabled);
/**
* @brief Get full-scale accelerometer range.
- * The FS_SEL parameter allows setting the full-scale range of the accelerometer
- * sensors, as described below:
- *
- * - 0 = +/- 2g
- * - 1 = +/- 4g
- * - 2 = +/- 8g
- * - 3 = +/- 16g
*
+ * The FS_SEL parameter allows dev the full-scale range of the accelerometer
+ * sensors.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @return Current full-scale accelerometer range setting.
+ * @param dev Device descriptor
+ * @param[out] range Current full-scale accelerometer range setting
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_full_scale_accel_range(mpu6050_dev_t *setting);
+esp_err_t mpu6050_get_full_scale_accel_range(mpu6050_dev_t *dev, mpu6050_accel_range_t *range);
/**
* @brief Set full-scale accelerometer range.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] range New full-scale accelerometer range setting.
+ * @param dev Device descriptor
+ * @param range New full-scale accelerometer range setting
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_set_full_scale_accel_range(mpu6050_dev_t *setting, uint8_t range);
+esp_err_t mpu6050_set_full_scale_accel_range(mpu6050_dev_t *dev, mpu6050_accel_range_t range);
/**
* @brief Get the high-pass filter configuration.
@@ -439,35 +540,25 @@ esp_err_t mpu6050_set_full_scale_accel_range(mpu6050_dev_t *setting, uint8_t ran
* output will be the difference between the input sample and the held
* sample.
*
- * | ACCEL_HPF | Filter Mode | Cut-off Frequency|
- * | ----------|-------------|------------------|
- * | 0 | Reset | None |
- * | 1 | On | 5Hz |
- * | 2 | On | 2.5Hz |
- * | 3 | On | 1.25Hz |
- * | 4 | On | 0.63Hz |
- * | 7 | Hold | None |
- *
- *
- * @param[in] setting mpu6050 i2cdev setting
- * @return Current high-pass filter configuration.
+ * @param dev Device descriptor
+ * @param[out] mode Current high-pass filter configuration
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_dhpf_mode(mpu6050_dev_t *setting);
+esp_err_t mpu6050_get_dhpf_mode(mpu6050_dev_t *dev, mpu6050_dhpf_mode_t *mode);
/**
* @brief Set the high-pass filter configuration.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] mode New high-pass filter configuration.
+ * @param dev Device descriptor
+ * @param mode New high-pass filter configuration
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_set_dhpf_mode(mpu6050_dev_t *setting, uint8_t mode);
+esp_err_t mpu6050_set_dhpf_mode(mpu6050_dev_t *dev, mpu6050_dhpf_mode_t mode);
/**
* @brief Get free-fall event acceleration threshold.
+ *
* This register configures the detection threshold for Free Fall event
* detection. The unit of FF_THR is 1LSB = 2mg. Free Fall is detected when the
* absolute value of the accelerometer measurements for the three axes are each
@@ -475,70 +566,56 @@ esp_err_t mpu6050_set_dhpf_mode(mpu6050_dev_t *setting, uint8_t mode);
* duration counter (Register 30). The Free Fall interrupt is triggered when the
* Free Fall duration counter reaches the time specified in FF_DUR.
*
+ * @param dev Device descriptor
+ * @param[out] threshold Current free-fall acceleration threshold value (LSB = 2mg)
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[out] threshold Current free-fall acceleration threshold value (LSB = 2mg).
- *
- * * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_freefall_detection_threshold(mpu6050_dev_t *setting, uint8_t *threshold);
+esp_err_t mpu6050_get_freefall_detection_threshold(mpu6050_dev_t *dev, uint8_t *threshold);
/**
* @brief Get free-fall event acceleration threshold.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] threshold New free-fall acceleration threshold value (LSB = 2mg).
+ * @param dev Device descriptor
+ * @param threshold New free-fall acceleration threshold value (LSB = 2mg).
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_set_freefall_detection_threshold(mpu6050_dev_t *setting, uint8_t threshold);
+esp_err_t mpu6050_set_freefall_detection_threshold(mpu6050_dev_t *dev, uint8_t threshold);
/**
* @brief Get free-fall event duration threshold.
- * This register configures the duration counter threshold for Free Fall event
+ *
+ * This register configures the duration_ms counter threshold for Free Fall event
* detection. The duration counter ticks at 1kHz, therefore FF_DUR has a unit
* of 1 LSB = 1 ms.
*
* The Free Fall duration counter increments while the absolute value of the
* accelerometer measurements are each less than the detection threshold
* (Register 29). The Free Fall interrupt is triggered when the Free Fall
- * duration counter reaches the time specified in this register.
+ * duration_ms counter reaches the time specified in this register.
*
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[out] duration Current free-fall duration threshold value (LSB = 1ms).
+ * @param dev Device descriptor
+ * @param[out] duration_ms Current free-fall duration threshold value, ms
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_freefall_detection_duration(mpu6050_dev_t *setting, uint8_t *duration);
+esp_err_t mpu6050_get_freefall_detection_duration(mpu6050_dev_t *dev, uint8_t *duration_ms);
/**
* @brief Set free-fall event duration threshold.
- * This register configures the duration counter threshold for Free Fall event
- * detection. The duration counter ticks at 1kHz, therefore FF_DUR has a unit
- * of 1 LSB = 1 ms.
*
- * The Free Fall duration counter increments while the absolute value of the
- * accelerometer measurements are each less than the detection threshold
- * (Register 29). The Free Fall interrupt is triggered when the Free Fall
- * duration counter reaches the time specified in this register.
+ * @param dev Device descriptor
+ * @param duration Free-fall duration threshold value, ms
*
- * @param setting mpu6050 i2cdev setting
- * @param duration Current free-fall duration threshold value
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_set_freefall_detection_duration(mpu6050_dev_t *setting, uint8_t duration);
+esp_err_t mpu6050_set_freefall_detection_duration(mpu6050_dev_t *dev, uint8_t duration_ms);
/**
* @brief Get motion detection event acceleration threshold.
+ *
* This register configures the detection threshold for Motion interrupt
* generation. The unit of MOT_THR is 1LSB = 2mg. Motion is detected when the
* absolute value of any of the accelerometer measurements exceeds this Motion
@@ -550,30 +627,26 @@ esp_err_t mpu6050_set_freefall_detection_duration(mpu6050_dev_t *setting, uint8_
* The Motion interrupt will indicate the axis and polarity of detected motion
* in MOT_DETECT_STATUS (Register 97).
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[out] threshold Current motion detection acceleration threshold value (LSB = 2mg).
+ * @param dev Device descriptor
+ * @param[out] threshold Current motion detection acceleration threshold value (LSB = 2mg)
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_motion_detection_threshold(mpu6050_dev_t *setting, uint8_t *threshold);
+esp_err_t mpu6050_get_motion_detection_threshold(mpu6050_dev_t *dev, uint8_t *threshold);
/**
* @brief Set motion detection event acceleration threshold.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] threshold New motion detection acceleration threshold
- * value (LSB = 2mg).
+ * @param dev Device descriptor
+ * @param threshold New motion detection acceleration threshold value (LSB = 2mg)
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_set_motion_detection_threshold(mpu6050_dev_t *setting, uint8_t threshold);
+esp_err_t mpu6050_set_motion_detection_threshold(mpu6050_dev_t *dev, uint8_t threshold);
/**
- * @brief Get motion detection event duration threshold.
+ * @brief Get motion detection event duration threshold
+ *
* This register configures the duration counter threshold for Motion interrupt
* generation. The duration counter ticks at 1 kHz, therefore MOT_DUR has a unit
* of 1LSB = 1ms. The Motion detection duration counter increments when the
@@ -582,25 +655,26 @@ esp_err_t mpu6050_set_motion_detection_threshold(mpu6050_dev_t *setting, uint8_t
* triggered when the Motion detection counter reaches the time count specified
* in this register.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @return Current motion detection duration threshold value (LSB = 1ms).
+ * @param dev Device descriptor
+ * @param[out] duration Current motion detection duration threshold value, ms
+ *
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_motion_detection_duration(mpu6050_dev_t *setting, uint8_t *duration);
+esp_err_t mpu6050_get_motion_detection_duration(mpu6050_dev_t *dev, uint8_t *duration);
/**
* @brief Set motion detection event duration threshold.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] duration New motion detection duration threshold value (LSB = 1ms).
+ * @param dev Device descriptor
+ * @param duration New motion detection duration threshold value, ms
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_set_motion_detection_duration(mpu6050_dev_t *setting, uint8_t duration);
+esp_err_t mpu6050_set_motion_detection_duration(mpu6050_dev_t *dev, uint8_t duration);
/**
* @brief Get zero motion detection event acceleration threshold.
+ *
* This register configures the detection threshold for Zero Motion interrupt
* generation. The unit of ZRMOT_THR is 1LSB = 2mg. Zero Motion is detected when
* the absolute value of the accelerometer measurements for the 3 axes are each
@@ -618,31 +692,28 @@ esp_err_t mpu6050_set_motion_detection_duration(mpu6050_dev_t *setting, uint8_t
* condition is detected, the status bit is set to 1. When a zero-motion-to-
* motion condition is detected, the status bit is set to 0.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @return Current zero motion detection acceleration threshold
- * value (LSB = 2mg).
+ * @param dev Device descriptor
+ * @param[out] threshold Current zero motion detection acceleration threshold
+ * value (LSB = 2mg)
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_zero_motion_detection_threshold(mpu6050_dev_t *setting, uint8_t *threshold);
+esp_err_t mpu6050_get_zero_motion_detection_threshold(mpu6050_dev_t *dev, uint8_t *threshold);
/**
* @brief Set zero motion detection event acceleration threshold.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] threshold New zero motion detection acceleration threshold
- * value (LSB = 2mg).
+ * @param dev Device descriptor
+ * @param threshold New zero motion detection acceleration threshold
+ * value (LSB = 2mg)
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_set_zero_motion_detection_threshold(mpu6050_dev_t *setting, uint8_t threshold);
+esp_err_t mpu6050_set_zero_motion_detection_threshold(mpu6050_dev_t *dev, uint8_t threshold);
/**
* @brief Get zero motion detection event duration threshold.
+ *
* This register configures the duration counter threshold for Zero Motion
* interrupt generation. The duration counter ticks at 16 Hz, therefore
* ZRMOT_DUR has a unit of 1 LSB = 64 ms. The Zero Motion duration counter
@@ -651,184 +722,121 @@ esp_err_t mpu6050_set_zero_motion_detection_threshold(mpu6050_dev_t *setting, ui
* interrupt is triggered when the Zero Motion duration counter reaches the time
* count specified in this register.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @return Current zero motion detection duration threshold value (LSB = 64ms).
+ * @param dev Device descriptor
+ * @param[out] duration Current zero motion detection duration threshold value (LSB = 64ms)
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_zero_motion_detection_duration(mpu6050_dev_t *setting, uint8_t *duration);
+esp_err_t mpu6050_get_zero_motion_detection_duration(mpu6050_dev_t *dev, uint8_t *duration);
/**
* @brief Set zero motion detection event duration threshold.
*
+ * @param dev Device descriptor
+ * @param duration New zero motion detection duration threshold value (LSB = 64ms)
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] duration New zero motion detection duration threshold
- * value (LSB = 1ms).
- *
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_set_zero_motion_detection_duration(mpu6050_dev_t *setting, uint8_t duration);
+esp_err_t mpu6050_set_zero_motion_detection_duration(mpu6050_dev_t *dev, uint8_t duration);
/**
* @brief Get temperature FIFO enabled value.
+ *
* When set to 1, this bit enables TEMP_OUT_H and TEMP_OUT_L (Registers 65 and
* 66) to be written into the FIFO buffer.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @return
- * - ESP_OK: enable
- * - ESP_FAIL: disable
- */
-esp_err_t mpu6050_get_temp_fifo_enabled(mpu6050_dev_t *setting);
-
-/**
- * @brief Set temperature FIFO enabled value.
+ * @param dev Device descriptor
+ * @param[out] enabled true if temperature FIFO is enabled
*
- * @param[in] enabled New temperature FIFO enabled value.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_set_temp_fifo_enabled(mpu6050_dev_t *setting, bool enabled);
+esp_err_t mpu6050_get_temp_fifo_enabled(mpu6050_dev_t *dev, bool *enabled);
/**
- * @brief Get gyroscope X-axis FIFO enabled value.
- * When set to 1, this bit enables GYRO_XOUT_H and GYRO_XOUT_L (Registers 67 and
- * 68) to be written into the FIFO buffer.
- *
+ * @brief Set temperature FIFO enabled value.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @return
- * - ESP_OK: enable bit equals 1
- * - ESP_FAIL: disable
- */
-esp_err_t mpu6050_get_x_gyro_fifo_enabled(mpu6050_dev_t *setting);
-
-/**
- * @brief Set gyroscope X-axis FIFO enabled value.
+ * @param dev Device descriptor
+ * @param enabled New temperature FIFO enabled value.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] enabled New gyroscope X-axis FIFO enabled value.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_set_x_gyro_fifo_enabled(mpu6050_dev_t *setting, bool enabled);
+esp_err_t mpu6050_set_temp_fifo_enabled(mpu6050_dev_t *dev, bool enabled);
/**
- * @brief Get gyroscope Y-axis FIFO enabled value.
- * When set to 1, this bit enables GYRO_YOUT_H and GYRO_YOUT_L (Registers 69 and
- * 70) to be written into the FIFO buffer.
+ * @brief Get gyroscope axis FIFO enabled value.
*
- * @return
- * - ESP_OK: enable bit equals 1
- * - ESP_FAIL: disable
- */
-esp_err_t mpu6050_get_y_gyro_fifo_enabled(mpu6050_dev_t *setting);
-
-/**
- * @brief Set gyroscope Y-axis FIFO enabled value.
+ * @param dev Device descriptor
+ * @param axis Gyroscope axis
+ * @param[out] enabled Gyroscope axis FIFO enabled value.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] enabled New gyroscope Y-axis FIFO enabled value.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_set_y_gyro_fifo_enabled(mpu6050_dev_t *setting, bool enabled);
+esp_err_t mpu6050_get_gyro_fifo_enabled(mpu6050_dev_t *dev, mpu6050_axis_t axis, bool *enabled);
/**
- * @brief Get gyroscope Z-axis FIFO enabled value.
- * When set to 1, this bit enables GYRO_ZOUT_H and GYRO_ZOUT_L (Registers 71 and
- * 72) to be written into the FIFO buffer.
+ * @brief Set gyroscope axis FIFO enabled value.
*
- * @return
- * - ESP_OK: enable bit equals 1
- * - ESP_FAIL: disable
- */
-esp_err_t mpu6050_get_z_gyro_fifo_enabled(mpu6050_dev_t *setting);
-
-/**
- * @brief Set gyroscope Z-axis FIFO enabled value.
+ * @param dev Device descriptor
+ * @param axis Gyroscope axis
+ * @param enabled New gyroscope axis FIFO enabled value.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] enabled New gyroscope Z-axis FIFO enabled value.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_set_z_gyro_fifo_enabled(mpu6050_dev_t *setting, bool enabled);
+esp_err_t mpu6050_set_gyro_fifo_enabled(mpu6050_dev_t *dev, mpu6050_axis_t axis, bool enabled);
/**
* @brief Get accelerometer FIFO enabled value.
+ *
* When set to 1, this bit enables ACCEL_XOUT_H, ACCEL_XOUT_L, ACCEL_YOUT_H,
* ACCEL_YOUT_L, ACCEL_ZOUT_H, and ACCEL_ZOUT_L (Registers 59 to 64) to be
* written into the FIFO buffer.
*
- * @return
- * - ESP_OK: enable bit equals 1
- * - ESP_FAIL: disable
+ * @param dev Device descriptor
+ * @param axis Gyroscope axis
+ * @param[out] enabled Gyroscope axis FIFO enabled value.
+ *
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_accel_fifo_enabled(mpu6050_dev_t *setting);
+esp_err_t mpu6050_get_accel_fifo_enabled(mpu6050_dev_t *dev, bool *enabled);
/**
* @brief Set accelerometer FIFO enabled value.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] enabled New accelerometer FIFO enabled value.
- */
-esp_err_t mpu6050_set_accel_fifo_enabled(mpu6050_dev_t *setting, bool enabled);
-
-/**
- * @brief Get Slave 2 FIFO enabled value.
- * When set to 1, this bit enables EXT_SENS_DATA registers (Registers 73 to 96)
- * associated with Slave 2 to be written into the FIFO buffer.
+ * @param dev Device descriptor
+ * @param enabled New accelerometer FIFO enabled value.
*
- * @return
- * - ESP_OK: enable bit equals 1
- * - ESP_FAIL: disable
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_slave_2_fifo_enabled(mpu6050_dev_t *setting);
+esp_err_t mpu6050_set_accel_fifo_enabled(mpu6050_dev_t *dev, bool enabled);
/**
- * @brief Set Slave 2 FIFO enabled value.
+ * @brief Get Slave FIFO enabled value.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] enabled New Slave 2 FIFO enabled value.
- */
-esp_err_t mpu6050_set_slave_2_fifo_enabled(mpu6050_dev_t *setting, bool enabled);
-
-/**
- * @brief Get Slave 1 FIFO enabled value.
* When set to 1, this bit enables EXT_SENS_DATA registers (Registers 73 to 96)
- * associated with Slave 1 to be written into the FIFO buffer.
+ * associated with Slave to be written into the FIFO buffer.
*
- * @return
- * - ESP_OK: enable bit equals 1
- * - ESP_FAIL: disable
- */
-esp_err_t mpu6050_get_slave_1_fifo_enabled(mpu6050_dev_t *setting);
-
-/**
- * @brief Set Slave 1 FIFO enabled value.
+ * @param dev Device descriptor
+ * @param num Slave number (0-3)
+ * @param[out] enabled Slave FIFO enabled value.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] enabled New Slave 1 FIFO enabled value.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_set_slave_1_fifo_enabled(mpu6050_dev_t *setting, bool enabled);
+esp_err_t mpu6050_get_slave_fifo_enabled(mpu6050_dev_t *dev, mpu6050_slave_t num, bool *enabled);
/**
- * @brief Get Slave 0 FIFO enabled value.
- * When set to 1, this bit enables EXT_SENS_DATA registers (Registers 73 to 96)
- * associated with Slave 0 to be written into the FIFO buffer.
+ * @brief Set Slave FIFO enabled value.
*
- * @return Current Slave 0 FIFO enabled value.
- */
-esp_err_t mpu6050_get_slave_0_fifo_enabled(mpu6050_dev_t *setting);
-
-/**
- * @brief Set Slave 0 FIFO enabled value.
+ * @param dev Device descriptor
+ * @param num Slave number (0-3)
+ * @param enabled New Slave FIFO enabled value.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] enabled New Slave 0 FIFO enabled value.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_set_slave_0_fifo_enabled(mpu6050_dev_t *setting, bool enabled);
+esp_err_t mpu6050_set_slave_fifo_enabled(mpu6050_dev_t *dev, mpu6050_slave_t num, bool enabled);
/**
* @brief Get multi-master enabled value.
+ *
* Multi-master capability allows multiple I2C masters to operate on the same
* bus. In circuits where multi-master capability is required, set MULT_MST_EN
* to 1. This will increase current drawn by approximately 30uA.
@@ -840,22 +848,26 @@ esp_err_t mpu6050_set_slave_0_fifo_enabled(mpu6050_dev_t *setting, bool enabled)
* MPU-60X0's bus arbitration detection logic is turned on, enabling it to
* detect when the bus is available.
*
- * @return
- * - ESP_OK: enable bit equals 1
- * - ESP_FAIL: disable
+ * @param dev Device descriptor
+ * @param[out] enabled Multi-master enabled value.
+ *
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_multi_master_enabled(mpu6050_dev_t *setting);
+esp_err_t mpu6050_get_multi_master_enabled(mpu6050_dev_t *dev, bool *enabled);
/**
- * @brief Set multi-master enabled value.
+ * @brief Set multi-master enabled value
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] enabled New multi-master enabled value.
+ * @param dev Device descriptor
+ * @param enabled New multi-master enabled value.
+ *
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_set_multi_master_enabled(mpu6050_dev_t *setting, bool enabled);
+esp_err_t mpu6050_set_multi_master_enabled(mpu6050_dev_t *dev, bool enabled);
/**
* @brief Get wait-for-external-sensor-data enabled value.
+ *
* When the WAIT_FOR_ES bit is set to 1, the Data Ready interrupt will be
* delayed until External Sensor data from the Slave Devices are loaded into the
* EXT_SENS_DATA registers. This is used to ensure that both the internal sensor
@@ -863,108 +875,75 @@ esp_err_t mpu6050_set_multi_master_enabled(mpu6050_dev_t *setting, bool enabled)
* their respective data registers (i.e. the data is synced) when the Data Ready
* interrupt is triggered.
*
- * @return
- * - ESP_OK: enable bit equals 1
- * - ESP_FAIL: disable
- */
-esp_err_t mpu6050_get_wait_for_external_sensor_enabled(mpu6050_dev_t *setting);
-
-/**
- * @brief Set wait-for-external-sensor-data enabled value.
+ * @param dev Device descriptor
+ * @param[out] enabled Wait-for-external-sensor-data enabled value.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] enabled New wait-for-external-sensor-data enabled value.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_set_wait_for_external_sensor_enabled(mpu6050_dev_t *setting, bool enabled);
+esp_err_t mpu6050_get_wait_for_external_sensor_enabled(mpu6050_dev_t *dev, bool *enabled);
/**
- * @brief Get Slave 3 FIFO enabled value.
- * When set to 1, this bit enables EXT_SENS_DATA registers (Registers 73 to 96)
- * associated with Slave 3 to be written into the FIFO buffer.
+ * @brief Set wait-for-external-sensor-data enabled value.
*
- * @return
- * - ESP_OK: enable bit equals 1
- * - ESP_FAIL: disable
- */
-esp_err_t mpu6050_get_slave_3_fifo_enabled(mpu6050_dev_t *setting);
-
-/**
- * @brief Set Slave 3 FIFO enabled value.
+ * @param dev Device descriptor
+ * @param enabled New wait-for-external-sensor-data enabled value.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] enabled New Slave 3 FIFO enabled value.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_set_slave_3_fifo_enabled(mpu6050_dev_t *setting, bool enabled);
+esp_err_t mpu6050_set_wait_for_external_sensor_enabled(mpu6050_dev_t *dev, bool enabled);
/**
* @brief Get slave read/write transition enabled value.
+ *
* The I2C_MST_P_NSR bit configures the I2C Master's transition from one slave
* read to the next slave read. If the bit equals 0, there will be a restart
* between reads. If the bit equals 1, there will be a stop followed by a start
* of the following read. When a write transaction follows a read transaction,
* the stop followed by a start of the successive write will be always used.
*
- * @return
- * - ESP_OK: enable bit equals 1
- * - ESP_FAIL: disable
+ * @param dev Device descriptor
+ * @param[out] enabled Slave read/write transition enabled value
+ *
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_slave_read_write_transition_enabled(mpu6050_dev_t *setting);
+esp_err_t mpu6050_get_slave_read_write_transition_enabled(mpu6050_dev_t *dev, bool *enabled);
/**
* @brief Set slave read/write transition enabled value.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] enabled New slave read/write transition enabled value.
+ * @param dev Device descriptor
+ * @param enabled New slave read/write transition enabled value.
+ *
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_set_slave_read_write_transition_enabled(mpu6050_dev_t *setting, bool enabled);
+esp_err_t mpu6050_set_slave_read_write_transition_enabled(mpu6050_dev_t *dev, bool enabled);
/**
* @brief Get I2C master clock speed.
+ *
* I2C_MST_CLK is a 4 bit unsigned value which configures a divider on the
- * MPU-60X0 internal 8MHz clock. It sets the I2C master clock speed according to
- * the following table:
- *
- * |I2C_MST_CLK | I2C Master Clock Speed | 8MHz Clock Divider|
- * |------------|------------------------|-------------------|
- * |0 | 348kHz | 23 |
- * |1 | 333kHz | 24 |
- * |2 | 320kHz | 25 |
- * |3 | 308kHz | 26 |
- * |4 | 296kHz | 27 |
- * |5 | 286kHz | 28 |
- * |6 | 276kHz | 29 |
- * |7 | 267kHz | 30 |
- * |8 | 258kHz | 31 |
- * |9 | 500kHz | 16 |
- * |10 | 471kHz | 17 |
- * |11 | 444kHz | 18 |
- * |12 | 421kHz | 19 |
- * |13 | 400kHz | 20 |
- * |14 | 381kHz | 21 |
- * |15 | 364kHz | 22 |
- *
- * @return Current I2C master clock speed.
- *
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
- */
-esp_err_t mpu6050_get_master_clock_speed(mpu6050_dev_t *setting, uint8_t *clk_spd);
+ * MPU-60X0 internal 8MHz clock.
+ *
+ * @param dev Device descriptor
+ * @param[out] clk_spd Current I2C master clock speed.
+ *
+ * @return `ESP_OK` on success
+ */
+esp_err_t mpu6050_get_master_clock_speed(mpu6050_dev_t *dev, mpu6050_i2c_master_clock_t *clk_spd);
/**
* @brief Set I2C master clock speed.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] speed Current I2C master clock speed.
+ * @param dev Device descriptor
+ * @param speed Current I2C master clock speed.
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_set_master_clock_speed(mpu6050_dev_t *setting, uint8_t clk_spd);
+esp_err_t mpu6050_set_master_clock_speed(mpu6050_dev_t *dev, mpu6050_i2c_master_clock_t clk_spd);
/**
- * @brief Get the I2C address of the specified slave (0-3).
+ * @brief Get the I2C address of the specified slave.
+ *
* Note that Bit 7 (MSB) controls read/write mode. If Bit 7 is set, it's a read
* operation, and if it is cleared, then it's a write operation. The remaining
* bits (6-0) are the 7-bit device address of the slave device.
@@ -1001,346 +980,223 @@ esp_err_t mpu6050_set_master_clock_speed(mpu6050_dev_t *setting, uint8_t clk_spd
* Sample Rate or at the reduced rate is determined by the Delay Enable bits in
* Register 103.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] num Slave number (0-3).
- *
- * @return Current address for specified slave.
+ * @param dev Device descriptor
+ * @param num Slave number (0-4).
+ * @param[out] addr Current address for specified slave.
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_slave_address(mpu6050_dev_t *setting, uint8_t num, uint8_t *addr);
+esp_err_t mpu6050_get_slave_address(mpu6050_dev_t *dev, mpu6050_slave_t num, uint8_t *addr);
/**
- * @brief Set the I2C address of the specified slave (0-3).
+ * @brief Set the I2C address of the specified slave.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] num Slave number (0-3)
- * @param[in] address New address for specified slave.
+ * @param dev Device descriptor
+ * @param num Slave number (0-4)
+ * @param address New address for specified slave.
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_set_slave_address(mpu6050_dev_t *setting, uint8_t num, uint8_t address);
+esp_err_t mpu6050_set_slave_address(mpu6050_dev_t *dev, mpu6050_slave_t num, uint8_t address);
/**
- * @brief Get the active internal register for the specified slave (0-3).
+ * @brief Get the active internal register for the specified slave.
+ *
* Read/write operations for this slave will be done to whatever internal
* register address is stored in this MPU register.
*
* The MPU-6050 supports a total of five slaves, but Slave 4 has unique
* characteristics, and so it has its own functions.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] num Slave number (0-3).
+ * @param dev Device descriptor
+ * @param num Slave number (0-4).
+ * @param[out] reg Current active register for specified slave.
*
- * @return Current active register for specified slave.
- *
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_slave_register(mpu6050_dev_t *setting, uint8_t num, uint8_t *reg);
+esp_err_t mpu6050_get_slave_register(mpu6050_dev_t *dev, mpu6050_slave_t num, uint8_t *reg);
/**
- * @brief Set the active internal register for the specified slave (0-3).
+ * @brief Set the active internal register for the specified slave.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] num Slave number (0-3).
- * @param[in] reg New active register for specified slave.
+ * @param dev Device descriptor
+ * @param num Slave number (0-4).
+ * @param reg New active register for specified slave.
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_set_slave_register(mpu6050_dev_t *setting, uint8_t num, uint8_t reg);
+esp_err_t mpu6050_set_slave_register(mpu6050_dev_t *dev, mpu6050_slave_t num, uint8_t reg);
/**
- * @brief Get the enabled value for the specified slave (0-3).
- * When set to 1, this bit enables Slave 0 for data transfer operations. When
- * cleared to 0, this bit disables Slave 0 from data transfer operations.
+ * @brief Get the enabled value for the specified slave.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] num Slave number (0-3).
+ * @param dev Device descriptor
+ * @param num Slave number (0-4).
+ * @param[out] enabled Enabled value for specified slave.
*
- * @return
- * - ESP_OK: enable
- * - ESP_FAIL: disable
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_slave_enabled(mpu6050_dev_t *setting, uint8_t num);
+esp_err_t mpu6050_get_slave_enabled(mpu6050_dev_t *dev, mpu6050_slave_t num, bool *enabled);
/**
- * @brief Set the enabled value for the specified slave (0-3).
+ * @brief Set the enabled value for the specified slave.
+ *
+ * @param dev Device descriptor
+ * @param num Slave number (0-4).
+ * @param enabled New enabled value for specified slave.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] num Slave number (0-3).
- * @param[in] enabled New enabled value for specified slave.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_set_slave_enabled(mpu6050_dev_t *setting, uint8_t num, bool enabled);
+esp_err_t mpu6050_set_slave_enabled(mpu6050_dev_t *dev, mpu6050_slave_t num, bool enabled);
/**
- * @brief Get word pair byte-swapping enabled for the specified slave (0-3).
+ * @brief Get word pair byte-swapping enabled for the specified slave.
+ *
* When set to 1, this bit enables byte swapping. When byte swapping is enabled,
* the high and low bytes of a word pair are swapped. Please refer to
* I2C_SLV0_GRP for the pairing convention of the word pairs. When cleared to 0,
- * bytes transferred to and from Slave 0 will be written to EXT_SENS_DATA
+ * bytes transferred to and from Slave will be written to EXT_SENS_DATA
* registers in the order they were transferred.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] num Slave number (0-3).
+ * @param dev Device descriptor
+ * @param num Slave number (0-4).
+ * @param[out] enabled Word pair byte-swapping enabled value for specified slave.
*
- * @return
- * - ESP_OK: enable
- * - ESP_FAIL: disable
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_slave_word_byte_swap(mpu6050_dev_t *setting, uint8_t num);
+esp_err_t mpu6050_get_slave_word_byte_swap(mpu6050_dev_t *dev, mpu6050_slave_t num, bool *enabled);
/**
- * @brief Set word pair byte-swapping enabled for the specified slave (0-3).
+ * @brief Set word pair byte-swapping enabled for the specified slave.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] num Slave number (0-3).
- * @param[in] enabled New word pair byte-swapping enabled value for specified
- * slave.
+ * @param dev Device descriptor
+ * @param num Slave number (0-4).
+ * @param enabled New word pair byte-swapping enabled value for specified slave.
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_set_slave_word_byte_swap(mpu6050_dev_t *setting, uint8_t num, bool enabled);
+esp_err_t mpu6050_set_slave_word_byte_swap(mpu6050_dev_t *dev, mpu6050_slave_t num, bool enabled);
/**
- * @brief Get write mode for the specified slave (0-3).
+ * @brief Get write mode for the specified slave.
+ *
* When set to 1, the transaction will read or write data only. When cleared to
* 0, the transaction will write a register address prior to reading or writing
* data. This should equal 0 when specifying the register address within the
* Slave device to/from which the ensuing data transaction will take place.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] num Slave number (0-3).
+ * @param dev Device descriptor
+ * @param num Slave number (0-4).
+ * @param[out] mode Write mode: false - register address + data, true - data only
*
- * @return Current write mode for specified slave (0 = register address + data,
- * 1 = data only).
- *
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_slave_write_mode(mpu6050_dev_t *setting, uint8_t num, bool *mode);
+esp_err_t mpu6050_get_slave_write_mode(mpu6050_dev_t *dev, mpu6050_slave_t num, bool *mode);
/**
- * @brief Set write mode for the specified slave (0-3).
+ * @brief Set write mode for the specified slave.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] num Slave number (0-3).
- * @param[in] mode New write mode for specified slave (0 = register address + data,
- * 1 = data only).
+ * @param dev Device descriptor
+ * @param num Slave number (0-4).
+ * @param mode Write mode: false - register address + data, true - data only
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_set_slave_write_mode(mpu6050_dev_t *setting, uint8_t num, bool mode);
+esp_err_t mpu6050_set_slave_write_mode(mpu6050_dev_t *dev, mpu6050_slave_t num, bool mode);
/**
- * @brief Get word pair grouping order offset for the specified slave (0-3).
+ * @brief Get word pair grouping order offset for the specified slave.
+ *
* This sets specifies the grouping order of word pairs received from registers.
* When cleared to 0, bytes from register addresses 0 and 1, 2 and 3, etc (even,
* then odd register addresses) are paired to form a word. When set to 1, bytes
* from register addresses are paired 1 and 2, 3 and 4, etc. (odd, then even
* register addresses) are paired to form a word.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] num Slave number (0-3).
+ * @param dev Device descriptor
+ * @param num Slave number (0-4).
+ * @param[out] enabled Word pair grouping order offset for specified slave.
*
- * @return
- * - ESP_OK: enable
- * - ESP_FAIL: disable
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_slave_word_group_offset(mpu6050_dev_t *setting, uint8_t num, bool *enabled);
+esp_err_t mpu6050_get_slave_word_group_offset(mpu6050_dev_t *dev, mpu6050_slave_t num, bool *enabled);
/**
- * @brief Set word pair grouping order offset for the specified slave (0-3).
+ * @brief Set word pair grouping order offset for the specified slave.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] num Slave number (0-3).
- * @param[in] enabled New word pair grouping order offset for specified slave.
+ * @param dev Device descriptor
+ * @param num Slave number (0-4).
+ * @param enabled New word pair grouping order offset for specified slave.
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_set_slave_word_group_offset(mpu6050_dev_t *setting, uint8_t num, bool enabled);
+esp_err_t mpu6050_set_slave_word_group_offset(mpu6050_dev_t *dev, mpu6050_slave_t num, bool enabled);
/**
- * @brief Get number of bytes to read for the specified slave (0-3).
+ * @brief Get number of bytes to read for the specified slave.
+ *
* Specifies the number of bytes transferred to and from Slave 0. Clearing this
* bit to 0 is equivalent to disabling the register by writing 0 to I2C_SLV0_EN.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] num Slave number (0-3).
- *
- * @return Number of bytes to read for specified slave.
- *
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
- */
-esp_err_t mpu6050_get_slave_data_length(mpu6050_dev_t *setting, uint8_t num, uint8_t *length);
-
-/**
- * @brief Set number of bytes to read for the specified slave (0-3).
- *
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] num Slave number (0-3).
- * @param[in] length Number of bytes to read for specified slave.
- *
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
- */
-esp_err_t mpu6050_set_slave_data_length(mpu6050_dev_t *setting, uint8_t num, uint8_t length);
-
-/**
- * @brief Get the I2C address of Slave 4.
- * Note that Bit 7 (MSB) controls read/write mode. If Bit 7 is set, it's a read
- * operation, and if it is cleared, then it's a write operation. The remaining
- * bits (6-0) are the 7-bit device address of the slave device.
- *
- * @param[in] setting mpu6050 i2cdev setting
- * @return Current address for Slave 4.
- *
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
- */
-esp_err_t mpu6050_get_slave_4_address(mpu6050_dev_t *setting, uint8_t *address);
-
-/**
- * @brief Set the I2C address of Slave 4.
- *
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] address New address for Slave 4.
- *
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
- */
-esp_err_t mpu6050_set_slave_4_address(mpu6050_dev_t *setting, uint8_t address);
-
-/**
- * @brief Get the active internal register for the Slave 4.
- * Read/write operations for this slave will be done to whatever internal
- * register address is stored in this MPU register.
+ * @param dev Device descriptor
+ * @param num Slave number (0-4).
+ * @param[out] length Number of bytes to read for specified slave.
*
- * @return Current active register for Slave.
- *
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-uint8_t mpu6050_get_slave_4_register(mpu6050_dev_t *setting, uint8_t *reg);
+esp_err_t mpu6050_get_slave_data_length(mpu6050_dev_t *dev, mpu6050_slave_t num, uint8_t *length);
/**
- * @brief Set the active internal register for Slave 4.
+ * @brief Set number of bytes to read for the specified slave.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] reg New active register for Slave 4.
+ * @param dev Device descriptor
+ * @param num Slave number (0-4).
+ * @param length Number of bytes to read for specified slave.
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_set_slave_4_register(mpu6050_dev_t *setting, uint8_t reg);
+esp_err_t mpu6050_set_slave_data_length(mpu6050_dev_t *dev, mpu6050_slave_t num, uint8_t length);
/**
* @brief Set new byte to write to Slave 4.
* This register stores the data to be written into the Slave 4. If I2C_SLV4_RW
* is set 1 (set to read), this register has no effect.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] data New byte to write to Slave 4.
- *
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
- */
-esp_err_t mpu6050_set_slave_4_output_byte(mpu6050_dev_t *setting, uint8_t data);
-
-/**
- * @brief Get the enabled value for the Slave 4.
- * When set to 1, this bit enables Slave 4 for data transfer operations.
- * When cleared to 0, this bit disables Slave 4 from data transfer operations.
- *
- * @return
- * - ESP_OK: enable
- * - ESP_FAIL: disable
- */
-esp_err_t mpu6050_get_slave_4_enabled(mpu6050_dev_t *setting, bool *enabled);
-
-/**
- * @brief Set the enabled value for Slave 4.
- *
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] enabled New enabled value for Slave 4.
+ * @param dev Device descriptor
+ * @param data New byte to write to Slave 4.
*
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_set_slave_4_enabled(mpu6050_dev_t *setting, bool enabled);
+esp_err_t mpu6050_set_slave_4_output_byte(mpu6050_dev_t *dev, uint8_t data);
/**
* @brief Get the enabled value for Slave 4 transaction interrupts.
+ *
* When set to 1, this bit enables the generation of an interrupt signal upon
* completion of a Slave 4 transaction. When cleared to 0, this bit disables the
* generation of an interrupt signal upon completion of a Slave 4 transaction.
* The interrupt status can be observed in Register 54.
*
- * @return
- * - ESP_OK: enable
- * - ESP_FAIL: disable
+ * @param dev Device descriptor
+ * @param[out] enabled Enabled value for Slave 4 transaction interrupts.
+ *
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_slave_4_interrupt_enabled(mpu6050_dev_t *setting, bool *enabled);
+esp_err_t mpu6050_get_slave_4_interrupt_enabled(mpu6050_dev_t *dev, bool *enabled);
/**
* @brief Set the enabled value for Slave 4 transaction interrupts.
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] enabled New enabled value for Slave 4 transaction interrupts.
- */
-esp_err_t mpu6050_set_slave_4_interrupt_enabled(mpu6050_dev_t *setting, bool enabled);
-
-/**
- * @brief Get write mode for Slave 4.
- * When set to 1, the transaction will read or write data only. When cleared to
- * 0, the transaction will write a register address prior to reading or writing
- * data. This should equal 0 when specifying the register address within the
- * Slave device to/from which the ensuing data transaction will take place.
- *
- * @return Current write mode for Slave 4:
- * (0 = register address + data, 1 = data only).
+ * @param dev Device descriptor
+ * @param enabled New enabled value for Slave 4 transaction interrupts.
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_slave_4_write_mode(mpu6050_dev_t *setting, uint8_t *mode);
-
-/**
- * @brief Set write mode for the Slave 4.
- *
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] mode New write mode for Slave 4:
- * (0 = register address + data, 1 = data only).
- *
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
- */
-esp_err_t mpu6050_set_slave_4_write_mode(mpu6050_dev_t *setting, bool mode);
+esp_err_t mpu6050_set_slave_4_interrupt_enabled(mpu6050_dev_t *dev, bool enabled);
/**
* @brief Get Slave 4 master delay value.
+ *
* This configures the reduced access rate of I2C slaves relative to the Sample
* Rate. When a slave's access rate is decreased relative to the Sample Rate,
* the slave is accessed every:
@@ -1351,557 +1207,366 @@ esp_err_t mpu6050_set_slave_4_write_mode(mpu6050_dev_t *setting, bool mode);
* DLPF_CFG (Register 26). Whether a slave's access rate is reduced relative to
* the Sample Rate is determined by I2C_MST_DELAY_CTRL (Register 103).
*
- * @return Current Slave 4 master delay value.
+ * @param dev Device descriptor
+ * @param[out] delay Current Slave 4 master delay value.
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_slave_4_master_delay(mpu6050_dev_t *setting, uint8_t *delay);
+esp_err_t mpu6050_get_slave_4_master_delay(mpu6050_dev_t *dev, uint8_t *delay);
/**
* @brief Set Slave 4 master delay value.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] delay New Slave 4 master delay value.
+ * @param dev Device descriptor
+ * @param delay New Slave 4 master delay value.
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_set_slave_4_master_delay(mpu6050_dev_t *setting, uint8_t delay);
+esp_err_t mpu6050_set_slave_4_master_delay(mpu6050_dev_t *dev, uint8_t delay);
/**
* @brief Get last available byte read from Slave 4.
+ *
* This register stores the data read from Slave 4. This field is populated
* after a read transaction.
*
- * @return Last available byte read from to Slave 4.
+ * @param dev Device descriptor
+ * @param[out] byte Last available byte read from to Slave 4.
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_slave_4_input_byte(mpu6050_dev_t *setting, uint8_t *byte);
+esp_err_t mpu6050_get_slave_4_input_byte(mpu6050_dev_t *dev, uint8_t *byte);
/**
* @brief Get FSYNC interrupt status.
+ *
* This bit reflects the status of the FSYNC interrupt from an external device
* into the MPU-60X0. This is used as a way to pass an external interrupt
* through the MPU-60X0 to the host application processor. When set to 1, .
*
- * @return
- * - ESP_OK: It will cause an interrupt if FSYNC_INT_EN is asserted in INT_PIN_CFG (Register 55)
- * - ESP_FAIL: disable
+ * @param dev Device descriptor
+ * @param[out] enabled FSYNC interrupt status
+ *
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_passthrough_status(mpu6050_dev_t *setting);
+esp_err_t mpu6050_get_passthrough_status(mpu6050_dev_t *dev, bool *enabled);
/**
* @brief Get Slave 4 transaction done status.
+ *
* Automatically sets to 1 when a Slave 4 transaction has completed. This
* triggers an interrupt if the I2C_MST_INT_EN bit in the INT_ENABLE register
- * (Register 56) is asserted and if the SLV_4_DONE_INT bit is asserted in the
- * I2C_SLV4_CTRL register (Register 52).
- *
- * @return
- * - ESP_OK: enable
- * - ESP_FAIL: disable
- */
-esp_err_t mpu6050_get_slave_4_is_done(mpu6050_dev_t *setting);
-
-/**
- * @brief Get master arbitration lost status.
- * This bit automatically sets to 1 when the I2C Master has lost arbitration of
- * the auxiliary I2C bus (an error condition). This triggers an interrupt if the
- * I2C_MST_INT_EN bit in the INT_ENABLE register (Register 56) is asserted.
- *
- * @return
- * - ESP_OK: enable
- * - ESP_FAIL: disable
- */
-esp_err_t mpu6050_get_lost_arbitration(mpu6050_dev_t *setting);
-
-/**
- * @brief Get Slave 4 NACK status.
- * This bit automatically sets to 1 when the I2C Master receives a NACK in a
- * transaction with Slave 4. This triggers an interrupt if the I2C_MST_INT_EN
- * bit in the INT_ENABLE register (Register 56) is asserted.
- *
- * @return
- * - ESP_OK: enable
- * - ESP_FAIL: disable
- */
-esp_err_t mpu6050_get_slave_4_nack(mpu6050_dev_t *setting);
-
-/**
- * @brief Get Slave 3 NACK status.
- * This bit automatically sets to 1 when the I2C Master receives a NACK in a
- * transaction with Slave 3. This triggers an interrupt if the I2C_MST_INT_EN
- * bit in the INT_ENABLE register (Register 56) is asserted.
+ * (Register 56) is asserted and if the SLV_4_DONE_INT bit is asserted in the
+ * I2C_SLV4_CTRL register (Register 52).
*
- * @return
- * - ESP_OK: enable
- * - ESP_FAIL: disable
- */
-esp_err_t mpu6050_get_slave_3_nack(mpu6050_dev_t *setting);
-
-/**
- * @brief Get Slave 2 NACK status.
- * This bit automatically sets to 1 when the I2C Master receives a NACK in a
- * transaction with Slave 2. This triggers an interrupt if the I2C_MST_INT_EN
- * bit in the INT_ENABLE register (Register 56) is asserted.
+ * @param dev Device descriptor
+ * @param[out] enabled Slave 4 transaction done status
*
- * @return
- * - ESP_OK: enable
- * - ESP_FAIL: disable
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_slave_2_nack(mpu6050_dev_t *setting);
+esp_err_t mpu6050_get_slave_4_is_done(mpu6050_dev_t *dev, bool *enabled);
/**
- * @brief Get Slave 1 NACK status.
- * This bit automatically sets to 1 when the I2C Master receives a NACK in a
- * transaction with Slave 1. This triggers an interrupt if the I2C_MST_INT_EN
- * bit in the INT_ENABLE register (Register 56) is asserted.
+ * @brief Get master arbitration lost status.
+ *
+ * This bit automatically sets to 1 when the I2C Master has lost arbitration of
+ * the auxiliary I2C bus (an error condition). This triggers an interrupt if the
+ * I2C_MST_INT_EN bit in the INT_ENABLE register (Register 56) is asserted.
*
- * @return Slave 1 NACK interrupt status.
+ * @param dev Device descriptor
+ * @param[out] lost Master arbitration lost status
+ *
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_slave_1_nack(mpu6050_dev_t *setting);
+esp_err_t mpu6050_get_lost_arbitration(mpu6050_dev_t *dev, bool *lost);
/**
- * @brief Get Slave 0 NACK status.
+ * @brief Get Slave NACK status.
+ *
* This bit automatically sets to 1 when the I2C Master receives a NACK in a
- * transaction with Slave 0. This triggers an interrupt if the I2C_MST_INT_EN
+ * transaction with Slave 4. This triggers an interrupt if the I2C_MST_INT_EN
* bit in the INT_ENABLE register (Register 56) is asserted.
*
- * @return Slave 0 NACK interrupt status.
+ * @param dev Device descriptor
+ * @param[out] nack Slave NACK status.
+ *
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_slave_0_nack(mpu6050_dev_t *setting);
+esp_err_t mpu6050_get_slave_nack(mpu6050_dev_t *dev, mpu6050_slave_t num, bool *nack);
/**
* @brief Get interrupt logic level mode.
- * Will be set 0 for active-high, 1 for active-low.
- * Current interrupt mode (0 = active-high, 1 = active-low).
*
- * @param[in] setting mpu6050 i2cdev setting
- * @return
- * ESP_OK: active-high.
- * ESP_FAIL: active-low
+ * @param dev Device descriptor
+ * @param[out] mode Interrupt logic level mode
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_interrupt_mode(mpu6050_dev_t *setting, bool *mode);
+esp_err_t mpu6050_get_interrupt_mode(mpu6050_dev_t *dev, mpu6050_int_level_t *mode);
/**
* @brief Set interrupt logic level mode.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] mode New interrupt mode (0 = active-high, 1 = active-low).
+ * @param dev Device descriptor
+ * @param mode New interrupt mode.
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_set_interrupt_mode(mpu6050_dev_t *setting, bool mode);
+esp_err_t mpu6050_set_interrupt_mode(mpu6050_dev_t *dev, mpu6050_int_level_t mode);
/**
* @brief Get interrupt drive mode.
- * Will be set 0 for push-pull, 1 for open-drain.
*
- * @return Current interrupt drive mode (0 = push-pull, 1 = open-drain).
+ * @param dev Device descriptor
+ * @param[out] drive Current interrupt drive mode
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_interrupt_drive(mpu6050_dev_t *setting, bool *drive);
+esp_err_t mpu6050_get_interrupt_drive(mpu6050_dev_t *dev, mpu6050_int_drive_t *drive);
/**
* @brief Set interrupt drive mode.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] drive New interrupt drive mode (0 = push-pull, 1 = open-drain).
+ * @param dev Device descriptor
+ * @param drive New interrupt drive mode
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_set_interrupt_drive(mpu6050_dev_t *setting, bool drive);
+esp_err_t mpu6050_set_interrupt_drive(mpu6050_dev_t *dev, mpu6050_int_drive_t drive);
/**
* @brief Get interrupt latch mode.
- * Will be set 0 for 50us-pulse, 1 for latch-until-int-cleared.
*
- * @return Current latch mode (0 = 50us-pulse, 1 = latch-until-int-cleared).
+ * @param dev Device descriptor
+ * @param[out] latch Current latch mode
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_interrupt_latch(mpu6050_dev_t *setting, bool *latch);
+esp_err_t mpu6050_get_interrupt_latch(mpu6050_dev_t *dev, mpu6050_int_latch_t *latch);
/**
* @brief Set interrupt latch mode.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] latch New latch mode (0 = 50us-pulse, 1 = latch-until-int-cleared).
+ * @param dev Device descriptor
+ * @param latch New latch mode
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_set_interrupt_latch(mpu6050_dev_t *setting, bool latch);
+esp_err_t mpu6050_set_interrupt_latch(mpu6050_dev_t *dev, mpu6050_int_latch_t latch);
/**
* @brief Get interrupt latch clear mode.
- * Will be set 0 for status-read-only, 1 for any-register-read.
*
- * @return Current latch clear mode (0 = status-read-only,
- * 1 = any-register-read).
+ * @param dev Device descriptor
+ * @param[out] clear Current latch clear mode (false = status-read-only, true = any-register-read).
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_interrupt_latch_clear(mpu6050_dev_t *setting, bool *clear);
+esp_err_t mpu6050_get_interrupt_latch_clear(mpu6050_dev_t *dev, bool *clear);
/**
* @brief Set interrupt latch clear mode.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] clear New latch clear mode (0 = status-read-only,
- * 1 = any-register-read).
+ * @param dev Device descriptor
+ * @param clear New latch clear mode (false = status-read-only, true = any-register-read).
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_set_interrupt_latch_clear(mpu6050_dev_t *setting, bool clear);
+esp_err_t mpu6050_set_interrupt_latch_clear(mpu6050_dev_t *dev, bool clear);
/**
* @brief Get FSYNC interrupt logic level mode.
*
- * @return Current FSYNC interrupt mode (0 = active-high, 1 = active-low).
+ * @param dev Device descriptor
+ * @param[out] mode Current FSYNC interrupt mode.
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_fsync_interrupt_level(mpu6050_dev_t *setting, bool *level);
+esp_err_t mpu6050_get_fsync_interrupt_level(mpu6050_dev_t *dev, mpu6050_int_level_t *level);
/**
* @brief Set FSYNC interrupt logic level mode.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] mode New FSYNC interrupt mode (0 = active-high, 1 = active-low).
+ * @param dev Device descriptor
+ * @param mode New FSYNC interrupt mode.
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_set_fsync_interrupt_level(mpu6050_dev_t *setting, bool level);
+esp_err_t mpu6050_set_fsync_interrupt_level(mpu6050_dev_t *dev, mpu6050_int_level_t level);
/**
* @brief Get FSYNC pin interrupt enabled setting.
- * Will be set 0 for disabled, 1 for enabled.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] setting mpu6050 condiguration
- * @return
- * ESP_OK: for enabled.
- * ESP_FAIL: for disabled
+ * @param dev Device descriptor
+ * @param[out] enabled FSYNC pin interrupt enabled setting
+ *
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_fsync_interrupt_enabled(mpu6050_dev_t *setting);
+esp_err_t mpu6050_get_fsync_interrupt_enabled(mpu6050_dev_t *dev, bool *enabled);
/**
* @brief Set FSYNC pin interrupt enabled setting.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] enabled New FSYNC pin interrupt enabled setting.
+ * @param dev Device descriptor
+ * @param enabled New FSYNC pin interrupt enabled setting.
+ *
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_set_fsync_interrupt_enabled(mpu6050_dev_t *setting, bool enabled);
+esp_err_t mpu6050_set_fsync_interrupt_enabled(mpu6050_dev_t *dev, bool enabled);
/**
* @brief Get I2C bypass enabled status.
- * When this bit is equal to 1 and I2C_MST_EN (Register 106 bit [5]) is equal to
- * 0, the host application processor will be able to directly access the
- * auxiliary I2C bus of the MPU-60X0. When this bit is equal to 0, the host
- * application processor will not be able to directly access the auxiliary I2C
- * bus of the MPU-60X0 regardless of the state of I2C_MST_EN (Register 106
- * bit [5]).
*
- * @return Current I2C bypass enabled status.
+ * When this bit is equal to 1 and I2C master is disabled, the host
+ * application processor will be able to directly access the auxiliary I2C
+ * bus of the MPU-60X0. When this bit is equal to 0, the host application
+ * processor will not be able to directly access the auxiliary I2C
+ * bus of the MPU-60X0 regardless of the state of I2C master.
+ *
+ * @param dev Device descriptor
+ * @param[out] enabled Current I2C bypass enabled status.
+ *
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_i2c_bypass_enabled(mpu6050_dev_t *setting);
+esp_err_t mpu6050_get_i2c_bypass_enabled(mpu6050_dev_t *dev, bool *enabled);
/**
* @brief Set I2C bypass enabled status.
- * When this bit is equal to 1 and I2C_MST_EN (Register 106 bit [5]) is equal to
- * 0, the host application processor will be able to directly access the
- * auxiliary I2C bus of the MPU-60X0. When this bit is equal to 0, the host
- * application processor will not be able to directly access the auxiliary I2C
- * bus of the MPU-60X0 regardless of the state of I2C_MST_EN (Register 106
- * bit [5]).
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] enabled New I2C bypass enabled status.
+ * @param dev Device descriptor
+ * @param enabled New I2C bypass enabled status.
+ *
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_set_i2c_bypass_enabled(mpu6050_dev_t *setting, bool enabled);
+esp_err_t mpu6050_set_i2c_bypass_enabled(mpu6050_dev_t *dev, bool enabled);
/**
* @brief Get reference clock output enabled status.
+ *
* When this bit is equal to 1, a reference clock output is provided at the
* CLKOUT pin. When this bit is equal to 0, the clock output is disabled.
*
+ * @param dev Device descriptor
+ * @param[out] enabled Current reference clock output enabled status.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @return
- * ESP_OK: for enabled.
- * ESP_FAIL: for disabled
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_clock_output_enabled(mpu6050_dev_t *setting);
+esp_err_t mpu6050_get_clock_output_enabled(mpu6050_dev_t *dev, bool *enabled);
/**
* @brief Set reference clock output enabled status.
- * When this bit is equal to 1, a reference clock output is provided at the
- * CLKOUT pin. When this bit is equal to 0, the clock output is disabled.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] enabled New reference clock output enabled status.
+ * @param dev Device descriptor
+ * @param enabled New reference clock output enabled status.
+ *
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_set_clock_output_enabled(mpu6050_dev_t *setting, bool enabled);
+esp_err_t mpu6050_set_clock_output_enabled(mpu6050_dev_t *dev, bool enabled);
/**
* @brief Get full interrupt enabled status.
- * Full register byte for all interrupts, for quick reading. Each bit will be
- * set 0 for disabled, 1 for enabled.
*
- * @param[in] setting mpu6050 configuration
- * @return
- * ESP_OK: for enabled.
- * ESP_FAIL: for disabled
- */
-uint8_t mpu6050_get_int_enabled(mpu6050_dev_t *setting);
-
-/**
- * @brief Set full interrupt enabled status.
- * Full register byte for all interrupts, for quick reading. Each bit should be
+ * Full register byte for all interrupts, for quick reading. Each bit will be
* set 0 for disabled, 1 for enabled.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] enabled New interrupt enabled status.
- */
-esp_err_t mpu6050_set_int_enabled(mpu6050_dev_t *setting, uint8_t enabled);
-
-/**
- * @brief Get Free Fall interrupt enabled status.
- * Will be set 0 for disabled, 1 for enabled.
- *
- * @param[in] setting mpu6050 condiguration
- * @return
- * ESP_OK: for enabled.
- * ESP_FAIL: for disabled
- * ESP_FAIL: for disabled
- */
-esp_err_t mpu6050_get_int_freefall_enabled(mpu6050_dev_t *setting);
-
-/**
- * @brief Set Free Fall interrupt enabled status.
- *
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] enabled New interrupt enabled status.
- */
-esp_err_t mpu6050_set_int_freefall_enabled(mpu6050_dev_t *setting, bool enabled);
-
-/**
- * @brief Get Motion Detection interrupt enabled status.
- * Will be set 0 for disabled, 1 for enabled.
+ * @param dev Device descriptor
+ * @param[out] ints Combination of mpu6050_int_source_t flags
*
- * @param[in] setting mpu6050 i2cdev setting
- * @return
- * ESP_OK: for enabled.
- * ESP_FAIL: for disabled
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_int_motion_enabled(mpu6050_dev_t *setting);
+esp_err_t mpu6050_get_int_enabled(mpu6050_dev_t *dev, uint8_t *ints);
/**
- * @brief Set Motion Detection interrupt enabled status.
+ * @brief Set full interrupt enabled status.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] enabled New interrupt enabled status.
- */
-esp_err_t mpu6050_set_int_motion_enabled(mpu6050_dev_t *setting, bool enabled);
-
-/**
- * @brief Get Zero Motion Detection interrupt enabled status.
- * Will be set 0 for disabled, 1 for enabled.
+ * Full register byte for all interrupts, for quick writing. Each bit will be
+ * set 0 for disabled, 1 for enabled.
*
+ * @param dev Device descriptor
+ * @param ints Combination of mpu6050_int_source_t flags
*
- * @param[in] setting mpu6050 condiguration
- * @return
- * ESP_OK: for enabled.
- * ESP_FAIL: for disabled
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_int_zero_motion_enabled(mpu6050_dev_t *setting);
+esp_err_t mpu6050_set_int_enabled(mpu6050_dev_t *dev, uint8_t ints);
/**
- * @brief Set Zero Motion Detection interrupt enabled status.
+ * @brief Get full set of interrupt status bits.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] enabled New interrupt enabled status.
- */
-esp_err_t mpu6050_set_int_zero_motion_enabled(mpu6050_dev_t *setting, bool enabled);
-
-/**
- * @brief Get FIFO Buffer Overflow interrupt enabled status.
- * Will be set 0 for disabled, 1 for enabled.
+ * These bits clear to 0 after the register has been read. Very useful
+ * for getting multiple INT statuses, since each single bit read clears
+ * all of them because it has to read the whole byte.
*
- * @param[in] setting mpu6050 condiguration
- * @return
- * ESP_OK: for enabled.
- * ESP_FAIL: for disabled
- */
-esp_err_t mpu6050_get_int_fifo_buffer_overflow_enabled(mpu6050_dev_t *setting);
-
-/**
- * @brief Set FIFO Buffer Overflow interrupt enabled status.
+ * @param dev Device descriptor
+ * @param[out] ints Combination of mpu6050_int_source_t flags
*
- * @param[in] enabled New interrupt enabled status.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_set_int_fifo_buffer_overflow_enabled(mpu6050_dev_t *setting, bool enabled);
+esp_err_t mpu6050_get_int_status(mpu6050_dev_t *dev, uint8_t *ints);
/**
- * @brief Get I2C Master interrupt enabled status.
- * This enables any of the I2C Master interrupt sources to generate an
- * interrupt. Will be set 0 for disabled, 1 for enabled.
+ * @brief Get offset for accelerometer axis
*
- * @param[in] setting mpu6050 condiguration
- * @return
- * ESP_OK: for enabled.
- * ESP_FAIL: for disabled
- */
-esp_err_t mpu6050_get_int_i2c_master_enabled(mpu6050_dev_t *setting);
-
-/**
- * @brief Set I2C Master interrupt enabled status.
+ * Undocumented register/feature
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] enabled New interrupt enabled status.
- */
-esp_err_t mpu6050_set_int_i2c_master_enabled(mpu6050_dev_t *setting, bool enabled);
-
-/**
- * @brief Get Data Ready interrupt enabled setting.
- * This event occurs each time a write operation to all of the sensor registers
- * has been completed. Will be set 0 for disabled, 1 for enabled.
+ * @param dev Device descriptor
+ * @param axis Accelerometer axis
+ * @param[out] offset Offset
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] setting mpu6050 condiguration
- * @return
- * ESP_OK: for enabled.
- * ESP_FAIL: for disabled
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_int_data_ready_enabled(mpu6050_dev_t *setting);
+esp_err_t mpu6050_get_accel_offset(mpu6050_dev_t *dev, mpu6050_axis_t axis, int16_t *offset);
/**
- * @brief Set Data Ready interrupt enabled status.
+ * @brief Set offset for accelerometer axis
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] enabled New interrupt enabled status.
- */
-esp_err_t mpu6050_set_int_data_ready_enabled(mpu6050_dev_t *setting, bool enabled);
-
-/**
- * @brief Get full set of interrupt status bits.
- * These bits clear to 0 after the register has been read. Very useful
- * for getting multiple INT statuses, since each single bit read clears
- * all of them because it has to read the whole byte.
+ * Undocumented register/feature
*
- * @param[in] setting mpu6050 i2cdev setting
+ * @param dev Device descriptor
+ * @param axis Accelerometer axis
+ * @param offset Offset
*
- * @return
- * - ESP_OK: enable
- * - ESP_FAIL: disable
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_int_status(mpu6050_dev_t *setting);
+esp_err_t mpu6050_set_accel_offset(mpu6050_dev_t *dev, mpu6050_axis_t axis, int16_t offset);
/**
- * @brief Get Free Fall interrupt status.
- * This bit automatically sets to 1 when a Free Fall interrupt has been
- * generated. The bit clears to 0 after the register has been read.
+ * @brief Get offset for gyroscope axis
*
- * @return
- * - ESP_OK: enable
- * - ESP_FAIL: disable
- */
-esp_err_t mpu6050_get_int_freefall_status(mpu6050_dev_t *setting);
-
-/**
- * @brief Get Motion Detection interrupt status.
- * This bit automatically sets to 1 when a Motion Detection interrupt has been
- * generated. The bit clears to 0 after the register has been read.
+ * Undocumented register/feature
*
- * @return
- * - ESP_OK: enable
- * - ESP_FAIL: disable
- */
-esp_err_t mpu6050_get_int_motion_status(mpu6050_dev_t *setting);
-
-/**
- * @brief Get Zero Motion Detection interrupt status.
- * This bit automatically sets to 1 when a Zero Motion Detection interrupt has
- * been generated. The bit clears to 0 after the register has been read.
+ * @param dev Device descriptor
+ * @param axis Gyroscope axis
+ * @param[out] offset Offset
*
- * @return
- * - ESP_OK: enable
- * - ESP_FAIL: disable
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_int_zero_motion_status(mpu6050_dev_t *setting);
+esp_err_t mpu6050_get_gyro_offset(mpu6050_dev_t *dev, mpu6050_axis_t axis, int16_t *offset);
/**
- * @brief Get FIFO Buffer Overflow interrupt status.
- * This bit automatically sets to 1 when a Free Fall interrupt has been
- * generated. The bit clears to 0 after the register has been read.
+ * @brief Get offset for gyroscope axis
*
- * @return
- * - ESP_OK: enable
- * - ESP_FAIL: disable
- */
-esp_err_t mpu6050_get_int_fifo_buffer_overflow_status(mpu6050_dev_t *setting);
-
-/**
- * @brief Get I2C Master interrupt status.
- * This bit automatically sets to 1 when an I2C Master interrupt has been
- * generated. For a list of I2C Master interrupts, please refer to Register 54.
- * The bit clears to 0 after the register has been read.
+ * Undocumented register/feature
*
- * @return
- * - ESP_OK: enable
- * - ESP_FAIL: disable
- */
-esp_err_t mpu6050_get_int_i2c_master_status(mpu6050_dev_t *setting);
-
-/**
- * @brief Get Data Ready interrupt status.
- * This bit automatically sets to 1 when a Data Ready interrupt has been
- * generated. The bit clears to 0 after the register has been read.
+ * @param dev Device descriptor
+ * @param axis Gyroscope axis
+ * @param offset Offset
*
- * @return
- * - ESP_OK: enable
- * - ESP_FAIL: disable
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_int_data_ready_status(mpu6050_dev_t *setting);
+esp_err_t mpu6050_set_gyro_offset(mpu6050_dev_t *dev, mpu6050_axis_t axis, int16_t offset);
/**
* @brief Get 3-axis accelerometer readings.
+ *
* These registers store the most recent accelerometer measurements.
* Accelerometer measurements are written to these registers at the Sample Rate
* as defined in Register 25.
+ *
* The accelerometer measurement registers, along with the temperature
* measurement registers, gyroscope measurement registers, and external sensor
* data registers, are composed of two sets of registers: an internal register
* set and a user-facing read register set.
+ *
* The data within the accelerometer sensors' internal register set is always
* updated at the Sample Rate. Meanwhile, the user-facing read register set
* duplicates the internal register set's data values whenever the serial
@@ -1909,72 +1574,59 @@ esp_err_t mpu6050_get_int_data_ready_status(mpu6050_dev_t *setting);
* read measurements from the same sampling instant. Note that if burst reads
* are not used, the user is responsible for ensuring a set of single byte reads
* correspond to a single sampling instant by checking the Data Ready interrupt.
- * Each 16-bit accelerometer measurement has a full scale defined in ACCEL_FS
- * (Register 28). For each full scale setting, the accelerometers' sensitivity
- * per LSB in ACCEL_XOUT is shown in the table below:
*
- * AFS_SEL | Full Scale Range | LSB Sensitivity
- * --------+------------------+----------------
- * 0 | +/- 2g | 8192 LSB/mg
- * 1 | +/- 4g | 4096 LSB/mg
- * 2 | +/- 8g | 2048 LSB/mg
- * 3 | +/- 16g | 1024 LSB/mg
+ * @param dev Device descriptor
+ * @param[out] accel Three-axis acceleration data, g.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] accel acceleration struct.
- *
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_acceleration(mpu6050_dev_t *setting, mpu6050_acceleration_t *accel);
+esp_err_t mpu6050_get_acceleration(mpu6050_dev_t *dev, mpu6050_acceleration_t *accel);
/**
- * @brief Get X-axis accelerometer reading.
+ * @brief Get raw 3-axis accelerometer readings.
*
- * @return X-axis acceleration measurement in 16-bit 2's complement format.
+ * @param dev Device descriptor
+ * @param[out] raw_accel Raw acceleration data.
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_acceleration_x(mpu6050_dev_t *setting, int16_t *accel_x);
+esp_err_t mpu6050_get_raw_acceleration(mpu6050_dev_t *dev, mpu6050_raw_acceleration_t *raw_accel);
/**
- * @brief Get Y-axis accelerometer reading.
+ * @brief Get accelerometer reading on a single axis.
*
- * @return Y-axis acceleration measurement in 16-bit 2's complement format.
+ * @param dev Device descriptor
+ * @param axis Accelerometer axis
+ * @param[out] accel Axis acceleration measurement, g
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_acceleration_y(mpu6050_dev_t *setting, int16_t *accel_y);
+esp_err_t mpu6050_get_acceleration_axis(mpu6050_dev_t *dev, mpu6050_axis_t axis, float *accel);
/**
- * @brief Get Z-axis accelerometer reading.
+ * @brief Get raw accelerometer reading on a single axis.
*
- * @return Z-axis acceleration measurement in 16-bit 2's complement format.
+ * @param dev Device descriptor
+ * @param axis Accelerometer axis
+ * @param[out] raw_accel Raw axis acceleration measurement
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_acceleration_z(mpu6050_dev_t *setting, int16_t *accel_z);
+esp_err_t mpu6050_get_raw_acceleration_axis(mpu6050_dev_t *dev, mpu6050_axis_t axis, int16_t *raw_accel);
/**
* @brief Get current internal temperature.
*
- * @return Temperature reading.
+ * @param dev Device descriptor
+ * @param[out] temp Internal temperature, °C
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_temperature(mpu6050_dev_t *setting, int16_t *temp);
+esp_err_t mpu6050_get_temperature(mpu6050_dev_t *dev, float *temp);
/**
* @brief Get 3-axis gyroscope readings.
+ *
* These gyroscope measurement registers, along with the accelerometer
* measurement registers, temperature measurement registers, and external sensor
* data registers, are composed of two sets of registers: an internal register
@@ -1986,75 +1638,62 @@ esp_err_t mpu6050_get_temperature(mpu6050_dev_t *setting, int16_t *temp);
* read measurements from the same sampling instant. Note that if burst reads
* are not used, the user is responsible for ensuring a set of single byte reads
* correspond to a single sampling instant by checking the Data Ready interrupt.
- * Each 16-bit gyroscope measurement has a full scale defined in FS_SEL
- * (Register 27). For each full scale setting, the gyroscopes' sensitivity per
- * LSB in GYRO_xOUT is shown in the table below:
*
- * FS_SEL | Full Scale Range | LSB Sensitivity
- * -------|--------------------|----------------
- * 0 | +/- 250 degrees/s | 131 LSB/deg/s
- * 1 | +/- 500 degrees/s | 65.5 LSB/deg/s
- * 2 | +/- 1000 degrees/s | 32.8 LSB/deg/s
- * 3 | +/- 2000 degrees/s | 16.4 LSB/deg/s
+ * @param dev Device descriptor
+ * @param[out] gyro Rotation data, °/s
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] rotat rotation struct.
- *
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_rotation(mpu6050_dev_t *setting, mpu6050_rotation_t *rotat);
+esp_err_t mpu6050_get_rotation(mpu6050_dev_t *dev, mpu6050_rotation_t *gyro);
/**
- * @brief Get X-axis gyroscope reading.
+ * @brief Get raw 3-axis gyroscope readings.
*
- * @return X-axis rotation measurement in 16-bit 2's complement format.
+ * @param dev Device descriptor
+ * @param[out] raw_accel Raw rotation data.
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_rotation_x(mpu6050_dev_t *setting, int16_t *rotation_x);
+esp_err_t mpu6050_get_raw_rotation(mpu6050_dev_t *dev, mpu6050_raw_rotation_t *raw_gyro);
/**
- * @brief Get Y-axis gyroscope reading.
+ * @brief Get gyroscope reading on a single axis.
*
- * @return Y-axis rotation measurement in 16-bit 2's complement format.
+ * @param dev Device descriptor
+ * @param axis Gyroscope axis
+ * @param[out] accel Axis rotation measurement, °/s
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_rotation_y(mpu6050_dev_t *setting, int16_t *rotation_y);
+esp_err_t mpu6050_get_rotation_axis(mpu6050_dev_t *dev, mpu6050_axis_t axis, float *gyro);
/**
- * @brief Get Z-axis gyroscope reading.
+ * @brief Get raw gyroscope reading on a single axis.
*
- * @return Z-axis rotation measurement in 16-bit 2's complement format.
+ * @param dev Device descriptor
+ * @param axis Gyroscope axis
+ * @param[out] raw_accel Raw axis rotation measurement
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_rotation_z(mpu6050_dev_t *setting, int16_t *rotation_z);
+esp_err_t mpu6050_get_raw_rotation_axis(mpu6050_dev_t *dev, mpu6050_axis_t axis, int16_t *raw_gyro);
/**
* @brief Get raw 6-axis motion sensor readings (accel/gyro).
+ *
* Retrieves all currently available motion sensor values.
*
- * @param[in] setting mpu6050 i2cdev setting
+ * @param dev Device descriptor
* @param[out] data_accel acceleration struct.
* @param[out] data_gyro rotation struct.
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_motion(mpu6050_dev_t *setting, mpu6050_acceleration_t *data_accel, mpu6050_rotation_t *data_gyro);
+esp_err_t mpu6050_get_motion(mpu6050_dev_t *dev, mpu6050_acceleration_t *data_accel, mpu6050_rotation_t *data_gyro);
/**
- * @brief Read single byte from external sensor data register.
+ * @brief Read bytes from external sensor data register.
+ *
* These registers store data read from external sensors by the Slave 0, 1, 2,
* and 3 on the auxiliary I2C interface. Data read by Slave 4 is stored in
* I2C_SLV4_DI (Register 53).
@@ -2125,201 +1764,67 @@ esp_err_t mpu6050_get_motion(mpu6050_dev_t *setting, mpu6050_acceleration_t *dat
* This above is also true if one of the slaves gets NACKed and stops
* functioning.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] position Starting position (0-23).
- *
- * @return Byte read from register.
- *
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
- */
-esp_err_t mpu6050_get_external_sensor_byte(mpu6050_dev_t *setting, int position, uint8_t *byte);
-
-/**
- * @brief Read word (2 bytes) from external sensor data registers.
- *
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] position Starting position (0-21).
- *
- * @return Word read from register.
- *
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
- */
-esp_err_t mpu6050_get_external_sensor_word(mpu6050_dev_t *setting, int position, uint16_t *word);
-
-/**
- * @brief Read double word (4 bytes) from external sensor data registers.
- *
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] position Starting position (0-20).
- *
- * @return Double word read from registers.
+ * @param dev Device descriptor
+ * @param position Starting position (0-23).
+ * @param[out] buf Buffer to store data
+ * @param length Bytes to read
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_external_sensor_dword(mpu6050_dev_t *setting, int position, uint32_t *dword);
+esp_err_t mpu6050_get_external_sensor_data(mpu6050_dev_t *dev, int position, void *buf, size_t length);
/**
* @brief Get full motion detection status register content (all bits).
*
- * @param[in] setting mpu6050 i2cdev setting
- * @return Motion detection status byte.
- *
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
- */
-esp_err_t mpu6050_get_motion_status(mpu6050_dev_t *setting, uint8_t *status);
-
-/**
- * @brief Get X-axis negative motion detection interrupt status.
- *
- * @param[in] setting mpu6050 i2cdev setting
- * @return
- * - ESP_OK: enable
- * - ESP_FAIL: disable.
- *
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
- */
-esp_err_t mpu6050_get_x_negative_motion_detected(mpu6050_dev_t *setting, uint8_t *motion);
-
-/**
- * @brief Get X-axis positive motion detection interrupt status.
- *
- * @param[in] setting mpu6050 i2cdev setting
- * @return
- * - ESP_OK: enable
- * - ESP_FAIL: disable
- *
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
- */
-esp_err_t mpu6050_get_x_positive_motion_detected(mpu6050_dev_t *setting, uint8_t *detected);
-
-/**
- * @brief Get Y-axis negative motion detection interrupt status.
- *
- * @param[in] setting mpu6050 i2cdev setting
- * @return
- * - ESP_OK: enable
- * - ESP_FAIL: disable
- *
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
- */
-esp_err_t mpu6050_get_y_negative_motion_detected(mpu6050_dev_t *setting, uint8_t *detected);
-
-/**
- * @brief Get Y-axis positive motion detection interrupt status.
- *
- * @param[in] setting mpu6050 i2cdev setting
- * @return
- * - ESP_OK: enable
- * - ESP_FAIL: disable
- *
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
- */
-esp_err_t mpu6050_get_y_positive_motion_detected(mpu6050_dev_t *setting, uint8_t *detected);
-
-/**
- * @brief Get Z-axis negative motion detection interrupt status.
- *
- * @param[in] setting mpu6050 i2cdev setting
- * @return
- * - ESP_OK: enable
- * - ESP_FAIL: disable
- *
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
- */
-esp_err_t mpu6050_get_z_negative_motion_detected(mpu6050_dev_t *setting, uint8_t *detected);
-
-/**
- * @brief Get Z-axis positive motion detection interrupt status.
- *
- * @param[in] setting mpu6050 i2cdev setting
- * @return
- * - ESP_OK: enable
- * - ESP_FAIL: disable
+ * @param dev Device descriptor
+ * @param[out] status Motion detection status byte, combination of ::mpu6050_motion_det_flags_t items
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_z_positive_motion_detected(mpu6050_dev_t *setting, uint8_t *detected);
-
-/**
- * @brief Get zero motion detection interrupt status.
- *
- * @param[in] setting mpu6050 i2cdev setting
- * @return
- * - ESP_OK: enable
- * - ESP_FAIL: disable
- *
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
- */
-esp_err_t mpu6050_get_zero_motion_detected(mpu6050_dev_t *setting, uint8_t *detected);
+esp_err_t mpu6050_get_motion_status(mpu6050_dev_t *dev, uint8_t *status);
/**
* @brief Write byte to Data Output container for specified slave.
+ *
* This register holds the output data written into Slave when Slave is set to
* write mode. For further information regarding Slave control, please
* refer to Registers 37 to 39 and immediately following.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] num Slave number (0-3).
- * @param[in] data Byte to write.
+ * @param dev Device descriptor
+ * @param num Slave number (0-3).
+ * @param data Byte to write.
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_set_slave_output_byte(mpu6050_dev_t *setting, uint8_t num, uint8_t data);
+esp_err_t mpu6050_set_slave_output_byte(mpu6050_dev_t *dev, mpu6050_slave_t num, uint8_t data);
/**
* @brief Get external data shadow delay enabled status.
+ *
* This register is used to specify the timing of external sensor data
* shadowing. When DELAY_ES_SHADOW is set to 1, shadowing of external
* sensor data is delayed until all data has been received.
*
- * @return
- * - ESP_OK: enable
- * - ESP_FAIL: disable
+ * @param dev Device descriptor
+ * @param[out] enabled External data shadow delay enabled status.
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_external_shadow_delay_enabled(mpu6050_dev_t *setting, bool *enabled);
+esp_err_t mpu6050_get_external_shadow_delay_enabled(mpu6050_dev_t *dev, bool *enabled);
/**
* @brief Set external data shadow delay enabled status.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] enabled New external data shadow delay enabled status.
+ * @param dev Device descriptor
+ * @param enabled New external data shadow delay enabled status.
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_set_external_shadow_delay_enabled(mpu6050_dev_t *setting, bool enabled);
+esp_err_t mpu6050_set_external_shadow_delay_enabled(mpu6050_dev_t *dev, bool enabled);
/**
* @brief Get slave delay enabled status.
+ *
* When a particular slave delay is enabled, the rate of access for the that
* slave device is reduced. When a slave's access rate is decreased relative to
* the Sample Rate, the slave is accessed every:
@@ -2333,67 +1838,61 @@ esp_err_t mpu6050_set_external_shadow_delay_enabled(mpu6050_dev_t *setting, bool
* For further information regarding the Sample Rate, please refer to
* register 25.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] num Slave number (0-4).
+ * @param dev Device descriptor
+ * @param num Slave number (0-4).
+ * @param[out] enabled Slave delay enabled status.
*
- * @return
- * - ESP_OK: enable
- * - ESP_FAIL: disable
- *
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_slave_delay_enabled(mpu6050_dev_t *setting, uint8_t num);
+esp_err_t mpu6050_get_slave_delay_enabled(mpu6050_dev_t *dev, mpu6050_slave_t num, bool *enabled);
/**
* @brief Set slave delay enabled status.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] num Slave number (0-4).
- * @param[in] enabled New slave delay enabled status.
+ * @param dev Device descriptor
+ * @param num Slave number (0-4).
+ * @param enabled New slave delay enabled status.
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_set_slave_delay_enabled(mpu6050_dev_t *setting, uint8_t num, bool enabled);
+esp_err_t mpu6050_set_slave_delay_enabled(mpu6050_dev_t *dev, mpu6050_slave_t num, bool enabled);
/**
* @brief Reset gyroscope signal path.
+ *
* The reset will revert the signal path analog to digital converters and
* filters to their power up configurations.
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @param dev Device descriptor
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_reset_gyroscope_path(mpu6050_dev_t *setting);
+esp_err_t mpu6050_reset_gyroscope_path(mpu6050_dev_t *dev);
/**
* @brief Reset accelerometer signal path.
+ *
* The reset will revert the signal path analog to digital converters and
* filters to their power up configurations.
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @param dev Device descriptor
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_reset_accelerometer_path(mpu6050_dev_t *setting);
+esp_err_t mpu6050_reset_accelerometer_path(mpu6050_dev_t *dev);
/**
* @brief Reset temperature sensor signal path.
+ *
* The reset will revert the signal path analog to digital converters and
* filters to their power up configurations.
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @param dev Device descriptor
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_reset_temperature_path(mpu6050_dev_t *setting);
+esp_err_t mpu6050_reset_temperature_path(mpu6050_dev_t *dev);
/**
* @brief Get accelerometer power-on delay.
+ *
* The accelerometer data path provides samples to the sensor registers, Motion
* detection, Zero Motion detection, and Free Fall detection modules. The
* signal path contains filters which must be flushed on wake-up with new
@@ -2404,30 +1903,26 @@ esp_err_t mpu6050_reset_temperature_path(mpu6050_dev_t *setting);
* to Section 8 of the MPU-6000/MPU-6050 Product Specification document for
* further information regarding the detection modules.
*
- * @param[in] setting mpu6050 i2cdev setting
- *
- * @return Current accelerometer power-on delay.
+ * @param dev Device descriptor
+ * @param[out] delay Current accelerometer power-on delay.
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_accelerometer_power_on_delay(mpu6050_dev_t *setting, uint8_t *delay);
+esp_err_t mpu6050_get_accelerometer_power_on_delay(mpu6050_dev_t *dev, uint8_t *delay);
/**
* @brief Set accelerometer power-on delay.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] delay New accelerometer power-on delay (0-3).
+ * @param dev Device descriptor
+ * @param delay New accelerometer power-on delay (0-3).
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_set_accelerometer_power_on_delay(mpu6050_dev_t *setting, uint8_t delay);
+esp_err_t mpu6050_set_accelerometer_power_on_delay(mpu6050_dev_t *dev, uint8_t delay);
/**
* @brief Get Free Fall detection counter decrement configuration.
+ *
* Detection is registered by the Free Fall detection module after accelerometer
* measurements meet their respective threshold conditions over a specified
* number of samples. When the threshold conditions are met, the corresponding
@@ -2447,28 +1942,26 @@ esp_err_t mpu6050_set_accelerometer_power_on_delay(mpu6050_dev_t *setting, uint8
* reset the counter to 0. For further information on Free Fall detection,
* please refer to Registers 29 to 32.
*
- * @return Current decrement configuration.
+ * @param dev Device descriptor
+ * @param[out] decrement Current decrement configuration.
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_freefall_detection_counter_decrement(mpu6050_dev_t *setting, uint8_t *decrement);
+esp_err_t mpu6050_get_freefall_detection_counter_decrement(mpu6050_dev_t *dev, uint8_t *decrement);
/**
* @brief Set Free Fall detection counter decrement configuration.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] decrement New decrement configuration value.
+ * @param dev Device descriptor
+ * @param decrement New decrement configuration value.
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_set_freefall_detection_counter_decrement(mpu6050_dev_t *setting, uint8_t decrement);
+esp_err_t mpu6050_set_freefall_detection_counter_decrement(mpu6050_dev_t *dev, uint8_t decrement);
/**
* @brief Get Motion detection counter decrement configuration.
+ *
* Detection is registered by the Motion detection module after accelerometer
* measurements meet their respective threshold conditions over a specified
* number of samples. When the threshold conditions are met, the corresponding
@@ -2487,97 +1980,103 @@ esp_err_t mpu6050_set_freefall_detection_counter_decrement(mpu6050_dev_t *settin
* When MOT_COUNT is configured to 0 (reset), any non-qualifying sample will
* reset the counter to 0.
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @param dev Device descriptor
+ * @param[out] decrement New decrement configuration value.
+ *
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_motion_detection_counter_decrement(mpu6050_dev_t *setting, uint8_t *decrement);
+esp_err_t mpu6050_get_motion_detection_counter_decrement(mpu6050_dev_t *dev, uint8_t *decrement);
/**
* @brief Set Motion detection counter decrement configuration.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] decrement New decrement configuration value.
+ * @param dev Device descriptor
+ * @param decrement New decrement configuration value.
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_set_motion_detection_counter_decrement(mpu6050_dev_t *setting, uint8_t decrement);
+esp_err_t mpu6050_set_motion_detection_counter_decrement(mpu6050_dev_t *dev, uint8_t decrement);
/**
* @brief Get FIFO enabled status.
+ *
* When this bit is set to 0, the FIFO buffer is disabled. The FIFO buffer
* cannot be written to or read from while disabled. The FIFO buffer's state
* does not change unless the MPU-60X0 is power cycled.
*
- * @return
- * - ESP_OK: enable
- * - ESP_FAIL: disable
+ * @param dev Device descriptor
+ * @param[out] decrement FIFO enabled status.
+ *
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_fifo_enabled(mpu6050_dev_t *setting);
+esp_err_t mpu6050_get_fifo_enabled(mpu6050_dev_t *dev, bool *enabled);
/**
* @brief Set FIFO enabled status.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] enabled New FIFO enabled status.
+ * @param dev Device descriptor
+ * @param enabled New FIFO enabled status.
*
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_set_fifo_enabled(mpu6050_dev_t *setting, bool enabled);
+esp_err_t mpu6050_set_fifo_enabled(mpu6050_dev_t *dev, bool enabled);
/**
* @brief Get I2C Master Mode enabled status.
+ *
* When this mode is enabled, the MPU-60X0 acts as the I2C Master to the
* external sensor slave devices on the auxiliary I2C bus. When this bit is
* cleared to 0, the auxiliary I2C bus lines (AUX_DA and AUX_CL) are logically
* driven by the primary I2C bus (SDA and SCL). This is a precondition to
* enabling Bypass Mode.
*
- * @return
- * - ESP_OK: enable
- * - ESP_FAIL: disable
+ * @param dev Device descriptor
+ * @param[out] enabled I2C Master Mode enabled status.
+ *
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_i2c_master_mode_enabled(mpu6050_dev_t *setting);
+esp_err_t mpu6050_get_i2c_master_mode_enabled(mpu6050_dev_t *dev, bool *enabled);
/**
* @brief Set I2C Master Mode enabled status.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] enabled New I2C Master Mode enabled status.
+ * @param dev Device descriptor
+ * @param enabled New I2C Master Mode enabled status.
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_set_i2c_master_mode_enabled(mpu6050_dev_t *setting, bool enabled);
+esp_err_t mpu6050_set_i2c_master_mode_enabled(mpu6050_dev_t *dev, bool enabled);
/**
- * Switch from I2C to SPI mode (MPU-6000 only).
+ * @brief Switch from I2C to SPI mode (MPU-6000 only).
+ *
* If this is set, the primary SPI interface will be enabled in place of the
* disabled primary I2C interface.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] enabled New switch SPIE Mode enabled status.
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * Note: This driver does not support SPI mode!
+ *
+ * @param dev Device descriptor
+ * @param enabled New switch SPIE Mode enabled status.
+ *
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_switch_spie_enabled(mpu6050_dev_t *setting, bool enabled);
+esp_err_t mpu6050_switch_spie_enabled(mpu6050_dev_t *dev, bool enabled);
/**
* @brief Reset the FIFO.
+ *
* This bit resets the FIFO buffer when set to 1 while FIFO_EN equals 0. This
* bit automatically clears to 0 after the reset has been triggered.
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @param dev Device descriptor
+ *
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_reset_fifo(mpu6050_dev_t *setting);
+esp_err_t mpu6050_reset_fifo(mpu6050_dev_t *dev);
/**
* @brief Reset all sensor registers and signal paths.
+ *
* When set to 1, this bit resets the signal paths for all sensors (gyroscopes,
* accelerometers, and temperature sensor). This operation will also clear the
* sensor registers. This bit automatically clears to 0 after the reset has been
@@ -2586,103 +2085,107 @@ esp_err_t mpu6050_reset_fifo(mpu6050_dev_t *setting);
* When resetting only the signal path (and not the sensor registers), please
* use Register 104, SIGNAL_PATH_RESET.
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @param dev Device descriptor
+ *
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_reset_sensors(mpu6050_dev_t *setting);
+esp_err_t mpu6050_reset_sensors(mpu6050_dev_t *dev);
/**
* @brief Trigger a full device reset.
+ *
* A small delay of ~50ms may be desirable after triggering a reset.
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @param dev Device descriptor
+ *
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_reset(mpu6050_dev_t *setting);
+esp_err_t mpu6050_reset(mpu6050_dev_t *dev);
/**
* @brief Get sleep mode status.
- * Setting the SLEEP bit in the register puts the device into very low power
+ *
+ * The SLEEP bit in the register puts the device into very low power
* sleep mode. In this mode, only the serial interface and internal registers
* remain active, allowing for a very low standby current. Clearing this bit
* puts the device back into normal mode. To save power, the individual standby
* selections for each of the gyros should be used if any gyro axis is not used
* by the application.
*
- * @return
- * - ESP_OK: enable
- * - ESP_FAIL: disable
+ * @param dev Device descriptor
+ * @param[out] enabled Sleep mode enabled status.
+ *
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_sleep_enabled(mpu6050_dev_t *setting);
+esp_err_t mpu6050_get_sleep_enabled(mpu6050_dev_t *dev, bool *enabled);
/**
* @brief Set sleep mode status.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] enabled New sleep mode enabled status.
+ * @param dev Device descriptor
+ * @param enabled New sleep mode enabled status.
+ *
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_set_sleep_enabled(mpu6050_dev_t *setting, bool enabled);
+esp_err_t mpu6050_set_sleep_enabled(mpu6050_dev_t *dev, bool enabled);
/**
* @brief Get wake cycle enabled status.
+ *
* When this bit is set to 1 and SLEEP is disabled, the MPU-60X0 will cycle
* between sleep mode and waking up to take a single sample of data from active
* sensors at a rate determined by LP_WAKE_CTRL (Register 108).
*
- * @return
- * - ESP_OK: enable
- * - ESP_FAIL: disable
+ * @param dev Device descriptor
+ * @param[out] enabled Wake cycle enabled status.
+ *
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_wake_cycle_enabled(mpu6050_dev_t *setting);
+esp_err_t mpu6050_get_wake_cycle_enabled(mpu6050_dev_t *dev, bool *enabled);
/**
* @brief Set wake cycle enabled status.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] enabled New sleep mode enabled status.
+ * @param dev Device descriptor
+ * @param enabled Wake cycle enabled status.
+ *
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_set_wake_cycle_enabled(mpu6050_dev_t *setting, bool enabled);
+esp_err_t mpu6050_set_wake_cycle_enabled(mpu6050_dev_t *dev, bool enabled);
/**
* @brief Get temperature sensor enabled status.
+ *
* Control the usage of the internal temperature sensor.
*
* Note: this register stores the *disabled* value, but for consistency with the
* rest of the code, the function is named and used with standard true/false
* values to indicate whether the sensor is enabled or disabled, respectively.
*
- * @return
- * - ESP_OK: enable
- * - ESP_FAIL: disable
+ * @param dev Device descriptor
+ * @param[out] enabled Temperature sensor enabled status.
+ *
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_temp_sensor_enabled(mpu6050_dev_t *setting);
+esp_err_t mpu6050_get_temp_sensor_enabled(mpu6050_dev_t *dev, bool *enabled);
/**
* @brief Set temperature sensor enabled status.
+ *
* Note: this register stores the *disabled* value, but for consistency with the
* rest of the code, the function is named and used with standard true/false
* values to indicate whether the sensor is enabled or disabled, respectively.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] enabled New temperature sensor enabled status.
+ * @param dev Device descriptor
+ * @param enabled New temperature sensor enabled status.
+ *
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_set_temp_sensor_enabled(mpu6050_dev_t *setting, bool enabled);
+esp_err_t mpu6050_set_temp_sensor_enabled(mpu6050_dev_t *dev, bool enabled);
/**
* @brief Get clock source setting.
*
- * @return Current clock source setting.
- *
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
- */
-esp_err_t mpu6050_get_clock_source(mpu6050_dev_t *setting, uint8_t *clk_setting);
-
-/**
- * @brief Set clock source setting.
* An internal 8MHz oscillator, gyroscope based clock, or external sources can
* be selected as the MPU-60X0 clock source. When the internal 8 MHz oscillator
* or an external source is chosen as the clock source, the MPU-60X0 can operate
@@ -2691,219 +2194,122 @@ esp_err_t mpu6050_get_clock_source(mpu6050_dev_t *setting, uint8_t *clk_setting)
* Upon power up, the MPU-60X0 clock source defaults to the internal oscillator.
* However, it is highly recommended that the device be configured to use one of
* the gyroscopes (or an external clock source) as the clock reference for
- * improved stability. The clock source can be selected according to the
- * following table:
+ * improved stability.
*
- * CLK_SEL | Clock Source
- * --------|--------------------------------------
- * 0 | Internal oscillator
- * 1 | PLL with X Gyro reference
- * 2 | PLL with Y Gyro reference
- * 3 | PLL with Z Gyro reference
- * 4 | PLL with external 32.768kHz reference
- * 5 | PLL with external 19.2MHz reference
- * 6 | Reserved
- * 7 | Stops the clock and keeps the timing generator in reset
+ * @param dev Device descriptor
+ * @param[out] source Current clock source setting.
+ *
+ * @return `ESP_OK` on success
+ */
+esp_err_t mpu6050_get_clock_source(mpu6050_dev_t *dev, mpu6050_clock_source_t *source);
+
+/**
+ * @brief Set clock source setting.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] source New clock source setting.
+ * @param dev Device descriptor
+ * @param source New clock source setting.
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_set_clock_source(mpu6050_dev_t *setting, uint8_t source);
+esp_err_t mpu6050_set_clock_source(mpu6050_dev_t *dev, mpu6050_clock_source_t source);
/**
* @brief Get wake frequency in Accel-Only Low Power Mode.
- * The MPU-60X0 can be put into Accerlerometer Only Low Power Mode by setting
+ *
+ * The MPU-60X0 can be put into Accelerometer Only Low Power Mode by setting
* PWRSEL to 1 in the Power Management 1 register (Register 107). In this mode,
* the device will power off all devices except for the primary I2C interface,
* waking only the accelerometer at fixed intervals to take a single
- * measurement. The frequency of wake-ups can be configured with LP_WAKE_CTRL
- * as shown below:
- *
- * LP_WAKE_CTRL | Wake-up Frequency
- * -------------|------------------
- * 0 | 1.25 Hz
- * 1 | 2.5 Hz
- * 2 | 5 Hz
- * 3 | 10 Hz
+ * measurement.
*
* For further information regarding the MPU-60X0's power modes, please refer to
* Register 107.
*
- * @return Current wake frequency.
+ * @param dev Device descriptor
+ * @param[out] frequency Current wake frequency.
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_wake_frequency(mpu6050_dev_t *setting, uint8_t *frequency);
+esp_err_t mpu6050_get_wake_frequency(mpu6050_dev_t *dev, mpu6050_wake_freq_t *frequency);
/**
* @brief Set wake frequency in Accel-Only Low Power Mode.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] frequency New wake frequency.
- *
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
- */
-esp_err_t mpu6050_set_wake_frequency(mpu6050_dev_t *setting, uint8_t frequency);
-
-/**
- * @brief Get X-axis accelerometer standby enabled status.
- * If enabled, the X-axis will not gather or report data (or use power).
- *
- * @return
- * - ESP_OK: enable
- * - ESP_FAIL: disable
- */
-esp_err_t mpu6050_get_standby_x_accel_enabled(mpu6050_dev_t *setting);
-
-/**
- * @brief Set X-axis accelerometer standby enabled status.
- *
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] enabled New X-axis standby enabled status.
- *
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
- */
-esp_err_t mpu6050_set_standby_x_accel_enabled(mpu6050_dev_t *setting, bool enabled);
-
-/**
- * @brief Get Y-axis accelerometer standby enabled status.
- * If enabled, the Y-axis will not gather or report data (or use power).
- *
- * @param[in] setting mpu6050 i2cdev setting
- * @return
- * - ESP_OK: enable
- * - ESP_FAIL: disable
- */
-esp_err_t mpu6050_get_standby_y_accel_enabled(mpu6050_dev_t *setting);
-
-/**
- * @brief Set Y-axis accelerometer standby enabled status.
- *
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] enabled New Y-axis standby enabled status.
- *
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
- */
-esp_err_t mpu6050_set_standby_y_accel_enabled(mpu6050_dev_t *setting, bool enabled);
-
-/**
- * @brief Get Z-axis accelerometer standby enabled status.
- * If enabled, the Z-axis will not gather or report data (or use power).
+ * @param dev Device descriptor
+ * @param frequency New wake frequency.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @return
- * - ESP_OK: enable
- * - ESP_FAIL: disable
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_standby_z_accel_enabled(mpu6050_dev_t *setting);
+esp_err_t mpu6050_set_wake_frequency(mpu6050_dev_t *dev, mpu6050_wake_freq_t frequency);
/**
- * @brief Set Z-axis accelerometer standby enabled status.
+ * @brief Get accelerometer axis standby enabled status.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] enabled New Z-axis standby enabled status.
+ * If enabled, the axis will not gather or report data (or use power).
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
- */
-esp_err_t mpu6050_set_standby_z_accel_enabled(mpu6050_dev_t *setting, bool enabled);
-
-/**
- * @brief Get X-axis gyroscope standby enabled status.
- * If enabled, the X-axis will not gather or report data (or use power).
+ * @param dev Device descriptor
+ * @param axis Accelerometer axis.
+ * @param[out] enabled Accelerometer axis standby enabled status.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @return
- * - ESP_OK: enable
- * - ESP_FAIL: disable
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_standby_x_gyro_enabled(mpu6050_dev_t *setting);
+esp_err_t mpu6050_get_standby_accel_enabled(mpu6050_dev_t *dev, mpu6050_axis_t axis, bool *enabled);
/**
- * @brief Set X-axis gyroscope standby enabled status.
+ * @brief Set accelerometer axis standby enabled status.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] enabled New X-axis standby enabled status.
+ * @param dev Device descriptor
+ * @param axis Accelerometer axis.
+ * @param enabled Accelerometer axis standby enabled status.
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_set_standby_x_gyro_enabled(mpu6050_dev_t *setting, bool enabled);
+esp_err_t mpu6050_set_standby_accel_enabled(mpu6050_dev_t *dev, mpu6050_axis_t axis, bool enabled);
/**
- * @brief Get Y-axis gyroscope standby enabled status.
- * If enabled, the Y-axis will not gather or report data (or use power).
+ * @brief Get gyroscope axis standby enabled status.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @return
- * - ESP_OK: enable
- * - ESP_FAIL: disable
- */
-esp_err_t mpu6050_get_standby_y_gyro_enabled(mpu6050_dev_t *setting);
-
-/**
- * @brief Set Y-axis gyroscope standby enabled status.
+ * If enabled, the axis will not gather or report data (or use power).
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] enabled New Y-axis standby enabled status.
+ * @param dev Device descriptor
+ * @param axis Gyroscope axis.
+ * @param[out] enabled Gyroscope axis standby enabled status.
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_set_standby_y_gyro_enabled(mpu6050_dev_t *setting, bool enabled);
+esp_err_t mpu6050_get_standby_gyro_enabled(mpu6050_dev_t *dev, mpu6050_axis_t axis, bool *enabled);
/**
- * @brief Get Z-axis gyroscope standby enabled status.
- * If enabled, the Z-axis will not gather or report data (or use power).
- *
- * @param[in] setting mpu6050 i2cdev setting
+ * @brief Set gyroscope axis standby enabled status.
*
- * @return Current Z-axis standby enabled status.
- */
-esp_err_t mpu6050_get_standby_z_gyro_enabled(mpu6050_dev_t *setting);
-
-/**
- * @brief Set Z-axis gyroscop]e standby enabled status.
+ * If enabled, the axis will not gather or report data (or use power).
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] enabled New Z-axis standby enabled status.
+ * @param dev Device descriptor
+ * @param axis Gyroscope axis.
+ * @param enabled Gyroscope axis standby enabled status.
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_set_standby_z_gyro_enabled(mpu6050_dev_t *setting, bool enabled);
+esp_err_t mpu6050_set_standby_gyro_enabled(mpu6050_dev_t *dev, mpu6050_axis_t axis, bool enabled);
/**
* @brief Get current FIFO buffer size.
+ *
* This value indicates the number of bytes stored in the FIFO buffer. This
* number is in turn the number of bytes that can be read from the FIFO buffer
* and it is directly proportional to the number of samples available given the
* set of sensor data bound to be stored in the FIFO (Register 35 and 36).
*
+ * @param dev Device descriptor
+ * @param[out] count Current FIFO buffer size.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @return Current FIFO buffer size.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_fifo_count(mpu6050_dev_t *setting, uint16_t *count);
+esp_err_t mpu6050_get_fifo_count(mpu6050_dev_t *dev, uint16_t *count);
/**
* @brief Get byte from FIFO buffer.
+ *
* This register is used to read and write data from the FIFO buffer. Data is
* written to the FIFO in order of register number (from lowest to highest). If
* all the FIFO enable flags (see below) are enabled and all External Sensor
@@ -2926,211 +2332,72 @@ esp_err_t mpu6050_get_fifo_count(mpu6050_dev_t *setting, uint16_t *count);
* should check FIFO_COUNT to ensure that the FIFO buffer is not read when
* empty.
*
- * @param[in] setting mpu6050 i2cdev setting
+ * @param dev Device descriptor
+ * @param[out] data Byte from FIFO buffer.
*
- * @return Byte from FIFO buffer.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_fifo_byte(mpu6050_dev_t *setting, uint8_t *byte);
-esp_err_t mpu6050_get_fifo_bytes(mpu6050_dev_t *setting, uint8_t *data, uint8_t length);
-
-/**
- * @brief Write byte to FIFO buffer.
- *
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] data New FIFO byte of data.
- *
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
- */
-esp_err_t mpu6050_set_fifo_byte(mpu6050_dev_t *setting, uint8_t data);
+esp_err_t mpu6050_get_fifo_byte(mpu6050_dev_t *dev, uint8_t *data);
/**
- * @brief Get the identity of the device that is stored in the WHO_AM_I
- * register. The device ID is 6 bits (Should be 0x34).
+ * @brief Get bytes from FIFO buffer.
*
- * @param[in] setting mpu6050 i2cdev setting
+ * @param dev Device descriptor
+ * @param[out] data Buffer to store read bytes
+ * @param length How many bytes to read
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_device_id(mpu6050_dev_t *setting, uint8_t *id);
+esp_err_t mpu6050_get_fifo_bytes(mpu6050_dev_t *dev, uint8_t *data, size_t length);
/**
- * @brief Set a new ID into the WHO_AM_I register.
+ * @brief Write byte to FIFO buffer.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] id New identity of the device.
+ * @param dev Device descriptor
+ * @param data New FIFO byte of data.
*
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
- */
-esp_err_t mpu6050_set_device_id(mpu6050_dev_t *setting, uint8_t id);
-
-/**
- * Undocumented/DMP Registers/Functions
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_get_otp_bank_valid(mpu6050_dev_t *setting, uint8_t *valid);
-esp_err_t mpu6050_set_otp_bank_valid(mpu6050_dev_t *setting, int8_t enabled);
-esp_err_t mpu6050_get_x_gyro_offset_tc(mpu6050_dev_t *setting, int8_t *offset);
-esp_err_t mpu6050_set_x_gyro_offset_tc(mpu6050_dev_t *setting, int8_t offset);
-esp_err_t mpu6050_get_y_gyro_offset_tc(mpu6050_dev_t *setting, int8_t *offset);
-esp_err_t mpu6050_set_y_gyro_offset_tc(mpu6050_dev_t *setting, int8_t offset);
-esp_err_t mpu6050_get_z_gyro_offset_tc(mpu6050_dev_t *setting, int8_t *offset);
-esp_err_t mpu6050_set_z_gyro_offset_tc(mpu6050_dev_t *setting, int8_t offset);
-esp_err_t mpu6050_get_x_fine_gain(mpu6050_dev_t *setting, int8_t *gain);
-esp_err_t mpu6050_set_x_fine_gain(mpu6050_dev_t *setting, int8_t gain);
-esp_err_t mpu6050_get_y_fine_gain(mpu6050_dev_t *setting, int8_t *gain);
-esp_err_t mpu6050_set_y_fine_gain(mpu6050_dev_t *setting, int8_t gain);
-esp_err_t mpu6050_get_z_fine_gain(mpu6050_dev_t *setting, int8_t *gain);
-esp_err_t mpu6050_set_z_fine_gain(mpu6050_dev_t *setting, int8_t gain);
-esp_err_t mpu6050_get_x_accel_offset(mpu6050_dev_t *setting, int16_t *offset);
-esp_err_t mpu6050_set_x_accel_offset(mpu6050_dev_t *setting, int16_t offset);
-esp_err_t mpu6050_get_y_accel_offset(mpu6050_dev_t *setting, int16_t *offset);
-esp_err_t mpu6050_set_y_accel_offset(mpu6050_dev_t *setting, int16_t offset);
-esp_err_t mpu6050_get_z_accel_offset(mpu6050_dev_t *setting, int16_t *offset);
-esp_err_t mpu6050_set_z_accel_offset(mpu6050_dev_t *setting, int16_t offset);
-esp_err_t mpu6050_get_x_gyro_offset(mpu6050_dev_t *setting, int16_t *offset);
-esp_err_t mpu6050_set_x_gyro_offset(mpu6050_dev_t *setting, int16_t offset);
-esp_err_t mpu6050_get_y_gyro_offset(mpu6050_dev_t *setting, int16_t *offset);
-esp_err_t mpu6050_set_y_gyro_offset(mpu6050_dev_t *setting, int16_t offset);
-esp_err_t mpu6050_get_z_gyro_offset(mpu6050_dev_t *setting, int16_t *offset);
-esp_err_t mpu6050_set_z_gyro_offset(mpu6050_dev_t *setting, int16_t offset);
-esp_err_t mpu6050_get_int_pll_ready_enabled(mpu6050_dev_t *setting);
-esp_err_t mpu6050_set_int_pll_ready_enabled(mpu6050_dev_t *setting, bool enabled);
-esp_err_t mpu6050_get_int_dmp_enabled(mpu6050_dev_t *setting);
-esp_err_t mpu6050_set_int_dmp_enabled(mpu6050_dev_t *setting, bool enabled);
-esp_err_t mpu6050_get_dmp_int_5_status(mpu6050_dev_t *setting);
-esp_err_t mpu6050_get_dmp_int_4_status(mpu6050_dev_t *setting);
-esp_err_t mpu6050_get_dmp_int_3_status(mpu6050_dev_t *setting);
-esp_err_t mpu6050_get_dmp_int_2_status(mpu6050_dev_t *setting);
-esp_err_t mpu6050_get_dmp_int_1_status(mpu6050_dev_t *setting);
-esp_err_t mpu6050_get_dmp_int_0_status(mpu6050_dev_t *setting);
-esp_err_t mpu6050_get_int_ppl_ready_status(mpu6050_dev_t *setting);
-esp_err_t mpu6050_get_int_dmp_status(mpu6050_dev_t *setting);
-esp_err_t mpu6050_get_dmp_enabled(mpu6050_dev_t *setting);
-esp_err_t mpu6050_set_dmp_enabled(mpu6050_dev_t *setting, bool enabled);
-esp_err_t mpu6050_reset_dmp(mpu6050_dev_t *setting);
-esp_err_t mpu6050_get_dmp_config_1(mpu6050_dev_t *setting, uint8_t *config);
-esp_err_t mpu6050_set_dmp_config_1(mpu6050_dev_t *setting, uint8_t config);
-esp_err_t mpu6050_get_dmp_config_2(mpu6050_dev_t *setting, uint8_t *config);
-esp_err_t mpu6050_set_dmp_config_2(mpu6050_dev_t *setting, uint8_t config);
+esp_err_t mpu6050_set_fifo_byte(mpu6050_dev_t *dev, uint8_t data);
/**
- * @brief Calculates acceleration resolution.
+ * @brief Get the ID of the device.
*
- * @param[in] accel_scale Acceleration scale. The scale range values are described
- * below:
+ * Device identity is stored in the WHO_AM_I register.
+ * The device ID is 6 bits (Should be 0x34).
*
- * - 0 = +/- 2g
- * - 1 = +/- 4g
- * - 2 = +/- 8g
- * - 3 = +/- 16g
+ * @param dev Device descriptor
+ * @param[out] id Device ID.
*
- * @return Resolution of the acceleration.
+ * @return `ESP_OK` on success
*/
-float mpu6050_get_accel_res(uint8_t accel_scale);
+esp_err_t mpu6050_get_device_id(mpu6050_dev_t *dev, uint8_t *id);
/**
- * @brief Calculates rotation resolution.
+ * @brief Function which accumulates gyro and accelerometer data after device initialization.
*
- * @param[in] accel_scale Rotation scale. The scale range values are described
- * below:
+ * It calculates the average of the at-rest readings and then loads the
+ * resulting offsets into accelerometer and gyro bias registers.
*
- * - 0 = +/- 250 degrees/sec
- * - 1 = +/- 500 degrees/sec
- * - 2 = +/- 1000 degrees/sec
- * - 3 = +/- 2000 degrees/sec
+ * @param dev Device descriptor
+ * @param[out] accel_bias_res Acceleration bias resolution.
+ * @param[out] gyro_bias_res Rotation bias resolution.
*
- *
- * @return Resolution of the acceleration.
+ * @return `ESP_OK` on success
*/
-float mpu6050_get_gyro_res(uint8_t gyro_scale);
-
-/**
- * @brief Function which accumulates gyro and accelerometer data after device
- * initialization. It calculates the average of the at-rest readings and then
- * loads the resulting offsets into accelerometer and gyro bias registers.
- *
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] accel_bias_res Acceleration bias resolution.
- * @param[in] gyro_bias_res Rotation bias resolution.
- *
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
- */
-esp_err_t mpu6050_calibrate(mpu6050_dev_t *setting, float *accel_bias_res, float *gyro_bias_res);
+esp_err_t mpu6050_calibrate(mpu6050_dev_t *dev, float *accel_bias_res, float *gyro_bias_res);
/**
* @brief Accelerometer and gyroscope self test.
- * Check calibration WRT factory settings.
- *
- * @param[in] setting mpu6050 i2cdev setting
- * @param[in] destination Where the result of the self test will be stored.
- *
- * @return
- * - ESP_OK on success
- * - else 'i2c_master_cmd_begin' return error.
- */
-esp_err_t mpu6050_self_test(mpu6050_dev_t *setting, float *destination);
-
-/**
- * @brief Implementation of Sebastian Madgwick's "Efficient orientation filter
- * for inertial/magnetic sensor arrays" which fuses acceleration and rotation
- * rate to produce a quaternion-based estimate of relative device orientation,
- * which can be converted to yaw, pitch, and roll.
- * The performance of the orientation filter is at least as good as conventional
- * Kalman-based filtering algorithms but is much less computationally intensive.
- * See http://www.x-io.co.uk/category/open-source/ for more details.
- *
- * @param[in] accel_x Acceleration x-axis value.
- * @param[in] accel_y Acceleration y-axis value.
- * @param[in] accel_z Acceleration z-axis value.
- * @param[in] gyro_x Rotation x-axis value.
- * @param[in] gyro_y Rotation y-axis value.
- * @param[in] gyro_z Rotation z-axis value.
- *
- * @return ESP_OK
- */
-esp_err_t mpu6050_madgwick_quaternion_update(float accel_x, float accel_y, float accel_z, float gyro_x, float gyro_y,
- float gyro_z);
-
-/**
- * @brief Verify the I2C connection. Make sure the device is connected and
- * responds as expected.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @return
- * - ESP_OK: connection is valid else ESP_FAIL
- *
- */
-esp_err_t mpu6050_test_connection(mpu6050_dev_t *setting);
-
-/**
- * @brief Init mpu6050 i2c dev.
+ * Check calibration WRT factory settings.
*
- * @param[in] setting mpu6050 configuration
- * @return
- * - ESP_OK: i2c dev initialized successfully.
- * - ESP_FAIL: failed to initialized.
- * - ESP_ERR_INVALID_ARG: invalid argument
- * - ESP_ERROR_TIMEOUT: failed to read the mpu6050 sensor.
- */
-esp_err_t mpu6050_init_desc(mpu6050_dev_t *mpu6050_config, uint8_t addr, i2c_port_t port, gpio_num_t sda_gpio,
- gpio_num_t scl_gpio);
-
-/**
- * @brief Deinit mpu6050 i2c dev.
+ * @param dev Device descriptor
+ * @param[out] destination Where the result of the self test will be stored.
*
- * @param[in] setting mpu6050 i2cdev setting
- * @return
- * - ESP_OK: i2c dev initialized successfully.
- * - ESP_ERR_INVALID_ARG: invalid argument
+ * @return `ESP_OK` on success
*/
-esp_err_t mpu6050_deinit_desc(mpu6050_dev_t *mpu6050_config);
+esp_err_t mpu6050_self_test(mpu6050_dev_t *dev, float *destination);
#ifdef __cplusplus
}
diff --git a/components/mpu6050/mpu6050_regs.h b/components/mpu6050/mpu6050_regs.h
new file mode 100644
index 00000000..882420d1
--- /dev/null
+++ b/components/mpu6050/mpu6050_regs.h
@@ -0,0 +1,353 @@
+#ifndef __MPU6050_REGS_H__
+#define __MPU6050_REGS_H__
+
+#define MPU6050_REGISTER_XG_OFFS_TC (0)
+#define MPU6050_REGISTER_YG_OFFS_TC (0x01)
+#define MPU6050_REGISTER_ZG_OFFS_TC (0x02)
+#define MPU6050_REGISTER_X_FINE_GAIN (0x03)
+#define MPU6050_REGISTER_Y_FINE_GAIN (0x04)
+#define MPU6050_REGISTER_Z_FINE_GAIN (0x05)
+#define MPU6050_REGISTER_XA_OFFS_H (0x06)
+#define MPU6050_REGISTER_XA_OFFS_L_TC (0x07)
+#define MPU6050_REGISTER_YA_OFFS_H (0x08)
+#define MPU6050_REGISTER_YA_OFFS_L_TC (0x09)
+#define MPU6050_REGISTER_ZA_OFFS_H (0x0A)
+#define MPU6050_REGISTER_ZA_OFFS_L_TC (0x0B)
+#define MPU6050_REGISTER_SELF_TEST_X (0x0D)
+#define MPU6050_REGISTER_SELF_TEST_Y (0x0E)
+#define MPU6050_REGISTER_SELF_TEST_Z (0x0F)
+#define MPU6050_REGISTER_SELF_TEST_A (0x10)
+#define MPU6050_REGISTER_XG_OFFS_USRH (0x13)
+#define MPU6050_REGISTER_XG_OFFS_USRL (0x14)
+#define MPU6050_REGISTER_YG_OFFS_USRH (0x15)
+#define MPU6050_REGISTER_YG_OFFS_USRL (0x16)
+#define MPU6050_REGISTER_ZG_OFFS_USRH (0x17)
+#define MPU6050_REGISTER_ZG_OFFS_USRL (0x18)
+#define MPU6050_REGISTER_SMPLRT_DIV (0x19)
+#define MPU6050_REGISTER_CONFIG (0x1A)
+#define MPU6050_REGISTER_GYRO_CONFIG (0x1B)
+#define MPU6050_REGISTER_ACCEL_CONFIG (0x1C)
+#define MPU6050_REGISTER_FF_THR (0x1D)
+#define MPU6050_REGISTER_FF_DUR (0x1E)
+#define MPU6050_REGISTER_MOT_THR (0x1F)
+#define MPU6050_REGISTER_MOT_DUR (0x20)
+#define MPU6050_REGISTER_ZRMOT_THR (0x21)
+#define MPU6050_REGISTER_ZRMOT_DUR (0x22)
+#define MPU6050_REGISTER_FIFO_EN (0x23)
+#define MPU6050_REGISTER_I2C_MST_CTRL (0x24)
+#define MPU6050_REGISTER_I2C_SLV0_ADDR (0x25)
+#define MPU6050_REGISTER_I2C_SLV0_REG (0x26)
+#define MPU6050_REGISTER_I2C_SLV0_CTRL (0x27)
+#define MPU6050_REGISTER_I2C_SLV1_ADDR (0x28)
+#define MPU6050_REGISTER_I2C_SLV1_REG (0x29)
+#define MPU6050_REGISTER_I2C_SLV1_CTRL (0x2A)
+#define MPU6050_REGISTER_I2C_SLV2_ADDR (0x2B)
+#define MPU6050_REGISTER_I2C_SLV2_REG (0x2C)
+#define MPU6050_REGISTER_I2C_SLV2_CTRL (0x2D)
+#define MPU6050_REGISTER_I2C_SLV3_ADDR (0x2E)
+#define MPU6050_REGISTER_I2C_SLV3_REG (0x2F)
+#define MPU6050_REGISTER_I2C_SLV3_CTRL (0x30)
+#define MPU6050_REGISTER_I2C_SLV4_ADDR (0x31)
+#define MPU6050_REGISTER_I2C_SLV4_REG (0x32)
+#define MPU6050_REGISTER_I2C_SLV4_DO (0x33)
+#define MPU6050_REGISTER_I2C_SLV4_CTRL (0x34)
+#define MPU6050_REGISTER_I2C_SLV4_DI (0x35)
+#define MPU6050_REGISTER_I2C_MST_STATUS (0x36)
+#define MPU6050_REGISTER_INT_PIN_CFG (0x37)
+#define MPU6050_REGISTER_INT_ENABLE (0x38)
+#define MPU6050_REGISTER_DMP_INT_STATUS (0x39)
+#define MPU6050_REGISTER_INT_STATUS (0x3A)
+#define MPU6050_REGISTER_ACCEL_XOUT_H (0x3B)
+#define MPU6050_REGISTER_ACCEL_XOUT_L (0x3C)
+#define MPU6050_REGISTER_ACCEL_YOUT_H (0x3D)
+#define MPU6050_REGISTER_ACCEL_YOUT_L (0x3E)
+#define MPU6050_REGISTER_ACCEL_ZOUT_H (0x3F)
+#define MPU6050_REGISTER_ACCEL_ZOUT_L (0x40)
+#define MPU6050_REGISTER_TEMP_OUT_H (0x41)
+#define MPU6050_REGISTER_TEMP_OUT_L (0x42)
+#define MPU6050_REGISTER_GYRO_XOUT_H (0x43)
+#define MPU6050_REGISTER_GYRO_XOUT_L (0x44)
+#define MPU6050_REGISTER_GYRO_YOUT_H (0x45)
+#define MPU6050_REGISTER_GYRO_YOUT_L (0x46)
+#define MPU6050_REGISTER_GYRO_ZOUT_H (0x47)
+#define MPU6050_REGISTER_GYRO_ZOUT_L (0x48)
+#define MPU6050_REGISTER_EXT_SENS_DATA_00 (0x49)
+#define MPU6050_REGISTER_EXT_SENS_DATA_01 (0x4A)
+#define MPU6050_REGISTER_EXT_SENS_DATA_02 (0x4B)
+#define MPU6050_REGISTER_EXT_SENS_DATA_03 (0x4C)
+#define MPU6050_REGISTER_EXT_SENS_DATA_04 (0x4D)
+#define MPU6050_REGISTER_EXT_SENS_DATA_05 (0x4E)
+#define MPU6050_REGISTER_EXT_SENS_DATA_06 (0x4F)
+#define MPU6050_REGISTER_EXT_SENS_DATA_07 (0x50)
+#define MPU6050_REGISTER_EXT_SENS_DATA_08 (0x51)
+#define MPU6050_REGISTER_EXT_SENS_DATA_09 (0x52)
+#define MPU6050_REGISTER_EXT_SENS_DATA_10 (0x53)
+#define MPU6050_REGISTER_EXT_SENS_DATA_11 (0x54)
+#define MPU6050_REGISTER_EXT_SENS_DATA_12 (0x55)
+#define MPU6050_REGISTER_EXT_SENS_DATA_13 (0x56)
+#define MPU6050_REGISTER_EXT_SENS_DATA_14 (0x57)
+#define MPU6050_REGISTER_EXT_SENS_DATA_15 (0x58)
+#define MPU6050_REGISTER_EXT_SENS_DATA_16 (0x59)
+#define MPU6050_REGISTER_EXT_SENS_DATA_17 (0x5A)
+#define MPU6050_REGISTER_EXT_SENS_DATA_18 (0x5B)
+#define MPU6050_REGISTER_EXT_SENS_DATA_19 (0x5C)
+#define MPU6050_REGISTER_EXT_SENS_DATA_20 (0x5D)
+#define MPU6050_REGISTER_EXT_SENS_DATA_21 (0x5E)
+#define MPU6050_REGISTER_EXT_SENS_DATA_22 (0x5F)
+#define MPU6050_REGISTER_EXT_SENS_DATA_23 (0x60)
+#define MPU6050_REGISTER_MOT_DETECT_STATUS (0x61)
+#define MPU6050_REGISTER_I2C_SLV0_DO (0x63)
+#define MPU6050_REGISTER_I2C_SLV1_DO (0x64)
+#define MPU6050_REGISTER_I2C_SLV2_DO (0x65)
+#define MPU6050_REGISTER_I2C_SLV3_DO (0x66)
+#define MPU6050_REGISTER_I2C_MST_DELAY_CTRL (0x67)
+#define MPU6050_REGISTER_SIGNAL_PATH_RESET (0x68)
+#define MPU6050_REGISTER_MOT_DETECT_CTRL (0x69)
+#define MPU6050_REGISTER_USER_CTRL (0x6A)
+#define MPU6050_REGISTER_PWR_MGMT_1 (0x6B)
+#define MPU6050_REGISTER_PWR_MGMT_2 (0x6C)
+#define MPU6050_REGISTER_BANK_SEL (0x6D)
+#define MPU6050_REGISTER_MEM_START_ADDR (0x6E)
+#define MPU6050_REGISTER_MEM_R_W (0x6F)
+#define MPU6050_REGISTER_DMP_CFG_1 (0x70)
+#define MPU6050_REGISTER_DMP_CFG_2 (0x71)
+#define MPU6050_REGISTER_FIFO_COUNTH (0x72)
+#define MPU6050_REGISTER_FIFO_COUNTL (0x73)
+#define MPU6050_REGISTER_FIFO_R_W (0x74)
+#define MPU6050_REGISTER_WHO_AM_I (0x75)
+
+// DLPF values
+#define MPU6050_DLPF_BW_256 (0x00)
+#define MPU6050_DLPF_BW_188 (0x01)
+#define MPU6050_DLPF_BW_98 (0x02)
+#define MPU6050_DLPF_BW_42 (0x03)
+#define MPU6050_DLPF_BW_20 (0x04)
+#define MPU6050_DLPF_BW_10 (0x05)
+#define MPU6050_DLPF_BW_5 (0x06)
+
+// DHPF values:
+#define MPU6050_DHPF_RESET (0x00)
+#define MPU6050_DHPF_5 (0x01)
+#define MPU6050_DHPF_2P5 (0x02)
+#define MPU6050_DHPF_1P25 (0x03)
+#define MPU6050_DHPF_0P63 (0x04)
+#define MPU6050_DHPF_HOLD (0x07)
+
+
+// Decrement values:
+#define MPU6050_DETECT_DECREMENT_RESET (0x0)
+#define MPU6050_DETECT_DECREMENT_1 (0x1)
+#define MPU6050_DETECT_DECREMENT_2 (0x2)
+#define MPU6050_DETECT_DECREMENT_4 (0x3)
+
+// External sync values:
+#define MPU6050_EXT_SYNC_DISABLED (0x0)
+#define MPU6050_EXT_SYNC_TEMP_OUT_L (0x1)
+#define MPU6050_EXT_SYNC_GYRO_XOUT_L (0x2)
+#define MPU6050_EXT_SYNC_GYRO_YOUT_L (0x3)
+#define MPU6050_EXT_SYNC_GYRO_ZOUT_L (0x4)
+#define MPU6050_EXT_SYNC_ACCEL_XOUT_L (0x5)
+#define MPU6050_EXT_SYNC_ACCEL_YOUT_L (0x6)
+#define MPU6050_EXT_SYNC_ACCEL_ZOUT_L (0x7)
+
+// Clock division values:
+#define MPU6050_CLOCK_DIV_348 (0x0)
+#define MPU6050_CLOCK_DIV_333 (0x1)
+#define MPU6050_CLOCK_DIV_320 (0x2)
+#define MPU6050_CLOCK_DIV_308 (0x3)
+#define MPU6050_CLOCK_DIV_296 (0x4)
+#define MPU6050_CLOCK_DIV_286 (0x5)
+#define MPU6050_CLOCK_DIV_276 (0x6)
+#define MPU6050_CLOCK_DIV_267 (0x7)
+#define MPU6050_CLOCK_DIV_258 (0x8)
+#define MPU6050_CLOCK_DIV_500 (0x9)
+#define MPU6050_CLOCK_DIV_471 (0xA)
+#define MPU6050_CLOCK_DIV_444 (0xB)
+#define MPU6050_CLOCK_DIV_421 (0xC)
+#define MPU6050_CLOCK_DIV_400 (0xD)
+#define MPU6050_CLOCK_DIV_381 (0xE)
+#define MPU6050_CLOCK_DIV_364 (0xF)
+
+// Bit and length defines for SELF_TEST register:
+#define MPU6050_SELF_TEST_XA_1_BIT (0x07)
+#define MPU6050_SELF_TEST_XA_1_LENGTH (0x03)
+#define MPU6050_SELF_TEST_XA_2_BIT (0x05)
+#define MPU6050_SELF_TEST_XA_2_LENGTH (0x02)
+#define MPU6050_SELF_TEST_YA_1_BIT (0x07)
+#define MPU6050_SELF_TEST_YA_1_LENGTH (0x03)
+#define MPU6050_SELF_TEST_YA_2_BIT (0x03)
+#define MPU6050_SELF_TEST_YA_2_LENGTH (0x02)
+#define MPU6050_SELF_TEST_ZA_1_BIT (0x07)
+#define MPU6050_SELF_TEST_ZA_1_LENGTH (0x03)
+#define MPU6050_SELF_TEST_ZA_2_BIT (0x01)
+#define MPU6050_SELF_TEST_ZA_2_LENGTH (0x02)
+#define MPU6050_SELF_TEST_XG_1_BIT (0x04)
+#define MPU6050_SELF_TEST_XG_1_LENGTH (0x05)
+#define MPU6050_SELF_TEST_YG_1_BIT (0x04)
+#define MPU6050_SELF_TEST_YG_1_LENGTH (0x05)
+#define MPU6050_SELF_TEST_ZG_1_BIT (0x04)
+#define MPU6050_SELF_TEST_ZG_1_LENGTH (0x05)
+
+// Bit and length defines for CONFIG register:
+#define MPU6050_CFG_EXT_SYNC_SET_BIT (3)
+#define MPU6050_CFG_EXT_SYNC_SET_MASK (7 << MPU6050_CFG_EXT_SYNC_SET_BIT)
+#define MPU6050_CFG_DLPF_CFG_BIT (0)
+#define MPU6050_CFG_DLPF_CFG_MASK (7 << MPU6050_CFG_DLPF_CFG_BIT)
+
+// Bit and length defines for GYRO_CONFIG register:
+#define MPU6050_GCONFIG_FS_SEL_BIT (3)
+#define MPU6050_GCONFIG_FS_SEL_MASK (3 << MPU6050_GCONFIG_FS_SEL_BIT)
+
+// Bit and length defines for ACCEL_CONFIG register:
+#define MPU6050_ACONFIG_XA_ST_BIT (7)
+#define MPU6050_ACONFIG_YA_ST_BIT (6)
+#define MPU6050_ACONFIG_ZA_ST_BIT (5)
+#define MPU6050_ACONFIG_AFS_SEL_BIT (3)
+#define MPU6050_ACONFIG_AFS_SEL_MASK (3 << MPU6050_ACONFIG_AFS_SEL_BIT)
+#define MPU6050_ACONFIG_ACCEL_HPF_BIT (0)
+#define MPU6050_ACONFIG_ACCEL_HPF_MASK (7 << MPU6050_ACONFIG_ACCEL_HPF_BIT)
+
+// Bit and length defines for FIFO_EN register:
+#define MPU6050_TEMP_FIFO_EN_BIT (7)
+#define MPU6050_XG_FIFO_EN_BIT (6)
+#define MPU6050_YG_FIFO_EN_BIT (5)
+#define MPU6050_ZG_FIFO_EN_BIT (4)
+#define MPU6050_ACCEL_FIFO_EN_BIT (3)
+#define MPU6050_SLV2_FIFO_EN_BIT (2)
+#define MPU6050_SLV1_FIFO_EN_BIT (1)
+#define MPU6050_SLV0_FIFO_EN_BIT (0)
+
+// Bit and length defines for I2C_MST_CTRL register:
+#define MPU6050_MULT_MST_EN_BIT (7)
+#define MPU6050_WAIT_FOR_ES_BIT (6)
+#define MPU6050_SLV_3_FIFO_EN_BIT (5)
+#define MPU6050_I2C_MST_P_NSR_BIT (4)
+#define MPU6050_I2C_MST_CLK_BIT (0)
+#define MPU6050_I2C_MST_CLK_MASK (7 << MPU6050_I2C_MST_CLK_BIT)
+
+// Bit and length defines for I2C_SLV* register:
+#define MPU6050_I2C_SLV_RW_BIT (7)
+#define MPU6050_I2C_SLV_ADDR_BIT (6)
+#define MPU6050_I2C_SLV_ADDR_LENGTH (7)
+#define MPU6050_I2C_SLV_EN_BIT (7)
+#define MPU6050_I2C_SLV_BYTE_SW_BIT (6)
+#define MPU6050_I2C_SLV_REG_DIS_BIT (5)
+#define MPU6050_I2C_SLV_GRP_BIT (4)
+#define MPU6050_I2C_SLV_LEN_BIT (0)
+#define MPU6050_I2C_SLV_LEN_MASK (7 << MPU6050_I2C_SLV_LEN_BIT)
+
+// Bit and length defines for I2C_SLV4 register:
+#define MPU6050_I2C_SLV4_RW_BIT (7)
+#define MPU6050_I2C_SLV4_ADDR_BIT (6)
+#define MPU6050_I2C_SLV4_ADDR_LENGTH (7)
+#define MPU6050_I2C_SLV4_EN_BIT (7)
+#define MPU6050_I2C_SLV4_INT_EN_BIT (6)
+#define MPU6050_I2C_SLV4_REG_DIS_BIT (5)
+#define MPU6050_I2C_SLV4_MST_DLY_BIT (4)
+#define MPU6050_I2C_SLV4_MST_DLY_LENGTH (5)
+
+// Bit and length defines for I2C_MST_STATUS register:
+#define MPU6050_MST_PASS_THROUGH_BIT (7)
+#define MPU6050_MST_I2C_SLV4_DONE_BIT (6)
+#define MPU6050_MST_I2C_LOST_ARB_BIT (5)
+#define MPU6050_MST_I2C_SLV4_NACK_BIT (4)
+#define MPU6050_MST_I2C_SLV3_NACK_BIT (3)
+#define MPU6050_MST_I2C_SLV2_NACK_BIT (2)
+#define MPU6050_MST_I2C_SLV1_NACK_BIT (1)
+#define MPU6050_MST_I2C_SLV0_NACK_BIT (0)
+
+// Bit and length defines for INT_PIN_CFG register:
+#define MPU6050_INTCFG_INT_LEVEL_BIT (7)
+#define MPU6050_INTCFG_INT_OPEN_BIT (6)
+#define MPU6050_INTCFG_LATCH_INT_EN_BIT (5)
+#define MPU6050_INTCFG_INT_RD_CLEAR_BIT (4)
+#define MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT (3)
+#define MPU6050_INTCFG_FSYNC_INT_EN_BIT (2)
+#define MPU6050_INTCFG_I2C_BYPASS_EN_BIT (1)
+#define MPU6050_INTCFG_CLKOUT_EN_BIT (0)
+
+// Bit and length defines for INT_ENABLE and INT_STATUS registers:
+#define MPU6050_INTERRUPT_FF_BIT (7)
+#define MPU6050_INTERRUPT_MOT_BIT (6)
+#define MPU6050_INTERRUPT_ZMOT_BIT (5)
+#define MPU6050_INTERRUPT_FIFO_OFLOW_BIT (4)
+#define MPU6050_INTERRUPT_I2C_MST_INT_BIT (3)
+#define MPU6050_INTERRUPT_PLL_RDY_INT_BIT (2)
+#define MPU6050_INTERRUPT_DMP_INT_BIT (1)
+#define MPU6050_INTERRUPT_DATA_RDY_BIT (0)
+
+// Bit and length defines for MOT_DETECT_STATUS register:
+#define MPU6050_MOTION_MOT_XNEG_BIT (7)
+#define MPU6050_MOTION_MOT_XPOS_BIT (6)
+#define MPU6050_MOTION_MOT_YNEG_BIT (5)
+#define MPU6050_MOTION_MOT_YPOS_BIT (4)
+#define MPU6050_MOTION_MOT_ZNEG_BIT (3)
+#define MPU6050_MOTION_MOT_ZPOS_BIT (2)
+#define MPU6050_MOTION_MOT_ZRMOT_BIT (0)
+
+// Bit and length defines for I2C_MST_DELAY_CTRL register:
+#define MPU6050_DLYCTRL_DELAY_ES_SHADOW_BIT (7)
+#define MPU6050_DLYCTRL_I2C_SLV4_DLY_EN_BIT (4)
+#define MPU6050_DLYCTRL_I2C_SLV3_DLY_EN_BIT (3)
+#define MPU6050_DLYCTRL_I2C_SLV2_DLY_EN_BIT (2)
+#define MPU6050_DLYCTRL_I2C_SLV1_DLY_EN_BIT (1)
+#define MPU6050_DLYCTRL_I2C_SLV0_DLY_EN_BIT (0)
+
+// Bit and length defines for SIGNAL_PATH_RESET register:
+#define MPU6050_PATHRESET_GYRO_RESET_BIT (2)
+#define MPU6050_PATHRESET_ACCEL_RESET_BIT (1)
+#define MPU6050_PATHRESET_TEMP_RESET_BIT (0)
+
+// Bit and length defines for MOT_DETECT_CTRL register:
+#define MPU6050_DETECT_ACCEL_DELAY_BIT (4)
+#define MPU6050_DETECT_ACCEL_DELAY_MASK (3 << MPU6050_DETECT_ACCEL_DELAY_BIT)
+#define MPU6050_DETECT_FF_COUNT_BIT (2)
+#define MPU6050_DETECT_FF_COUNT_MASK (3 << MPU6050_DETECT_FF_COUNT_BIT)
+#define MPU6050_DETECT_MOT_COUNT_BIT (0)
+#define MPU6050_DETECT_MOT_COUNT_MASK (3 << MPU6050_DETECT_MOT_COUNT_BIT)
+
+// Bit and length defines for USER_CTRL register:
+#define MPU6050_USERCTRL_DMP_EN_BIT (7)
+#define MPU6050_USERCTRL_FIFO_EN_BIT (6)
+#define MPU6050_USERCTRL_I2C_MST_EN_BIT (5)
+#define MPU6050_USERCTRL_I2C_IF_DIS_BIT (4)
+#define MPU6050_USERCTRL_DMP_RESET_BIT (3)
+#define MPU6050_USERCTRL_FIFO_RESET_BIT (2)
+#define MPU6050_USERCTRL_I2C_MST_RESET_BIT (1)
+#define MPU6050_USERCTRL_SIG_COND_RESET_BIT (0)
+
+// Bit and length defines for PWR_MGMT_1 register:
+#define MPU6050_PWR1_DEVICE_RESET_BIT (7)
+#define MPU6050_PWR1_SLEEP_BIT (6)
+#define MPU6050_PWR1_CYCLE_BIT (5)
+#define MPU6050_PWR1_TEMP_DIS_BIT (3)
+#define MPU6050_PWR1_CLKSEL_BIT (0)
+#define MPU6050_PWR1_CLKSEL_MASK (7 << MPU6050_PWR1_CLKSEL_BIT)
+
+// Bit and length defines for PWR_MGMT_2 register:
+#define MPU6050_PWR2_LP_WAKE_CTRL_BIT (6)
+#define MPU6050_PWR2_LP_WAKE_CTRL_MASK (3 << MPU6050_PWR2_LP_WAKE_CTRL_BIT)
+#define MPU6050_PWR2_STBY_XA_BIT (5)
+#define MPU6050_PWR2_STBY_YA_BIT (4)
+#define MPU6050_PWR2_STBY_ZA_BIT (3)
+#define MPU6050_PWR2_STBY_XG_BIT (2)
+#define MPU6050_PWR2_STBY_YG_BIT (1)
+#define MPU6050_PWR2_STBY_ZG_BIT (0)
+
+// Bit and length defines for WHO_AM_I register:
+#define MPU6050_WHO_AM_I_BIT (1)
+#define MPU6050_WHO_AM_I_MASK (0x3f << MPU6050_WHO_AM_I_BIT)
+
+// Undocumented bits and lengths:
+#define MPU6050_TC_PWR_MODE_BIT (7)
+#define MPU6050_TC_OFFSET_BIT (6)
+#define MPU6050_TC_OFFSET_LENGTH (6)
+#define MPU6050_TC_OTP_BNK_VLD_BIT (0)
+#define MPU6050_DMPINT_5_BIT (5)
+#define MPU6050_DMPINT_4_BIT (4)
+#define MPU6050_DMPINT_3_BIT (3)
+#define MPU6050_DMPINT_2_BIT (2)
+#define MPU6050_DMPINT_1_BIT (1)
+#define MPU6050_DMPINT_0_BIT (0)
+
+#endif // __MPU6050_REGS_H__
diff --git a/examples/mpu6050/default/main/CMakeLists.txt b/examples/mpu6050/default/main/CMakeLists.txt
index 4d331d91..06502277 100644
--- a/examples/mpu6050/default/main/CMakeLists.txt
+++ b/examples/mpu6050/default/main/CMakeLists.txt
@@ -1,3 +1,3 @@
idf_component_register(SRCS "main.c"
INCLUDE_DIRS "."
- REQUIRES mpu6050 tca9548)
+ REQUIRES mpu6050)
diff --git a/examples/mpu6050/default/main/Kconfig.projbuild b/examples/mpu6050/default/main/Kconfig.projbuild
index 24b01443..1233e359 100644
--- a/examples/mpu6050/default/main/Kconfig.projbuild
+++ b/examples/mpu6050/default/main/Kconfig.projbuild
@@ -1,26 +1,35 @@
menu "MPU6050 Example Configuration"
- config EXAMPLE_MPU6050_ADDRESS_HIGH
- bool "mpu6050 i2c addr 0x69"
- default false
+ choice EXAMPLE_I2C_ADDRESS
+ prompt "Select I2C address"
+ default EXAMPLE_I2C_ADDRESS_LOW
+ help
+ Select I2C address
+
+ config EXAMPLE_I2C_ADDRESS_LOW
+ bool "MPU6050_I2C_ADDRESS_LOW"
+ help
+ Choose this when ADDR pin is connected to ground
+ config EXAMPLE_I2C_ADDRESS_HIGH
+ bool "MPU6050_I2C_ADDRESS_HIGH"
+ help
+ Choose this when ADDR pin is connected to VCC
+ endchoice
- config EXAMPLE_MPU6050_I2C_MASTER_SCL
+ config EXAMPLE_SCL_GPIO
int "MPU6050 SCL GPIO Number"
- default 22 if IDF_TARGET_ESP32
+ default 5 if IDF_TARGET_ESP8266
+ default 6 if IDF_TARGET_ESP32C3
+ default 19 if IDF_TARGET_ESP32 || IDF_TARGET_ESP32S2 || IDF_TARGET_ESP32S3
help
GPIO number for I2C Master clock line.
- config EXAMPLE_MPU6050_I2C_MASTER_SDA
+ config EXAMPLE_SDA_GPIO
int "MPU6050 SDA GPIO Number"
- default 21 if IDF_TARGET_ESP32
+ default 4 if IDF_TARGET_ESP8266
+ default 5 if IDF_TARGET_ESP32C3
+ default 18 if IDF_TARGET_ESP32 || IDF_TARGET_ESP32S2 || IDF_TARGET_ESP32S3
help
GPIO number for I2C Master data line.
- config EXAMPLE_MPU6050_I2C_CLOCK_HZ
- int "I2C clock frequency, Hz"
- default 100000
-
- config EXAMPLE_MPU6050_I2C_PORT
- int "I2C port number"
- default 1
endmenu
diff --git a/examples/mpu6050/default/main/main.c b/examples/mpu6050/default/main/main.c
index 51378890..c3e1a9ab 100644
--- a/examples/mpu6050/default/main/main.c
+++ b/examples/mpu6050/default/main/main.c
@@ -1,88 +1,60 @@
-/* standard C library */
#include
-
-/* freeRTOS library */
-#include "freertos/FreeRTOS.h"
-#include "freertos/task.h"
-
+#include
+#include
#include
#include
#include
-#ifdef CONFIG_EXAMPLE_MPU6050_ADDRESS_HIGH
-#define MPU6050_ADDR (MPU6050_ADDRESS_HIGH)
+#ifdef CONFIG_EXAMPLE_I2C_ADDRESS_LOW
+#define ADDR MPU6050_I2C_ADDRESS_LOW
#else
-#define MPU6050_ADDR (MPU6050_ADDRESS_LOW)
+#define ADDR MPU6050_I2C_ADDRESS_HIGH
#endif
-#define MPU6050_SDA_PIN (CONFIG_EXAMPLE_MPU6050_I2C_MASTER_SDA)
-#define MPU6050_SCL_PIN (CONFIG_EXAMPLE_MPU6050_I2C_MASTER_SCL)
-#define MPU6050_I2C_CLK_FREQZ (CONFIG_EXAMPLE_MPU6050_I2C_CLOCK_HZ)
-#define MPU6050_I2C_PORT (CONFIG_EXAMPLE_MPU6050_I2C_PORT)
-#define TAG ("mpu6050_test")
+
+static const char *TAG = "mpu6050_test";
void mpu6050_test(void *pvParameters)
{
- // testing
- esp_err_t ret = ESP_FAIL;
+ mpu6050_dev_t dev = { 0 };
- mpu6050_dev_t mpu6050_conf = { 0 };
- int16_t temp = 0;
+ ESP_ERROR_CHECK(mpu6050_init_desc(&dev, ADDR, 0, CONFIG_EXAMPLE_SDA_GPIO, CONFIG_EXAMPLE_SCL_GPIO));
- ESP_LOGI(TAG, "mpu6050 config: addr 0x%x, sda %d, scl %d, clk, %d port %d", MPU6050_ADDR, MPU6050_SDA_PIN,
- MPU6050_SCL_PIN, MPU6050_I2C_CLK_FREQZ, MPU6050_I2C_PORT);
-
- while (ret != ESP_OK)
+ while (1)
{
- ret = mpu6050_init_desc(&(mpu6050_conf), MPU6050_ADDR, MPU6050_I2C_PORT, MPU6050_SDA_PIN, MPU6050_SCL_PIN);
- if (ret != ESP_OK)
+ esp_err_t res = i2c_dev_probe(&dev.i2c_dev, I2C_DEV_WRITE);
+ if (res == ESP_OK)
{
- ESP_LOGE(TAG, "Failed to init mpu6050, error %s", esp_err_to_name(ret));
+ ESP_LOGI(TAG, "Found MPU60x0 device");
+ break;
}
- vTaskDelay(pdMS_TO_TICKS(2000));
- }
-
- while (mpu6050_test_connection(&(mpu6050_conf)) != ESP_OK)
- {
- ESP_LOGE(TAG, "mpu6050 connection failed.");
+ ESP_LOGE(TAG, "MPU60x0 not found");
vTaskDelay(pdMS_TO_TICKS(1000));
}
- ESP_LOGI(TAG, "mpu6050 connection successfull.");
+ ESP_ERROR_CHECK(mpu6050_init(&dev));
while (1)
{
- mpu6050_rotation_t mpu6050_rot = { 0 };
- mpu6050_acceleration_t mpu6050_accel = { 0 };
- mpu6050_get_temperature(&(mpu6050_conf), &temp);
- if (ret != ESP_OK)
- {
- ESP_LOGE(TAG, "failed to read temp error %s", esp_err_to_name(ret));
- }
- ret = mpu6050_get_motion(&(mpu6050_conf), &(mpu6050_accel), &(mpu6050_rot));
- if (ret != ESP_OK)
- {
- ESP_LOGE(TAG, "failed to read motion error %s", esp_err_to_name(ret));
- }
- else
- {
- ESP_LOGI(TAG, "**********************************************************************");
- ESP_LOGI(TAG, "Rotation: x=%d y=%d z=%d", mpu6050_rot.x, mpu6050_rot.y, mpu6050_rot.z);
- ESP_LOGI(TAG, "Acceleration: x=%d y=%d z=%d", mpu6050_accel.x, mpu6050_accel.y, mpu6050_accel.z);
- ESP_LOGI(TAG, "Temperature: %d", temp);
- }
+ float temp;
+ mpu6050_acceleration_t accel = { 0 };
+ mpu6050_rotation_t rotation = { 0 };
+
+ ESP_ERROR_CHECK(mpu6050_get_temperature(&dev, &temp));
+ ESP_ERROR_CHECK(mpu6050_get_motion(&dev, &accel, &rotation));
+
+ ESP_LOGI(TAG, "**********************************************************************");
+ ESP_LOGI(TAG, "Acceleration: x=%.3f y=%.3f z=%.3f", accel.x, accel.y, accel.z);
+ ESP_LOGI(TAG, "Rotation: x=%.3f y=%.3f z=%.3f", rotation.x, rotation.y, rotation.z);
+ ESP_LOGI(TAG, "Temperature: %.1f", temp);
vTaskDelay(pdMS_TO_TICKS(1000));
}
-
- // never reach here
- ESP_LOGE(TAG, "Deleting %s task", __func__);
- vTaskDelete(NULL);
}
void app_main()
{
// task
ESP_ERROR_CHECK(i2cdev_init());
- xTaskCreate(mpu6050_test, "mpu6050_test", configMINIMAL_STACK_SIZE * 8, NULL, 5, NULL);
- return;
+
+ xTaskCreate(mpu6050_test, "mpu6050_test", configMINIMAL_STACK_SIZE * 6, NULL, 5, NULL);
}