Add LPF and LP setting for MPU6886

m5stack · May 22, 2024 · 4492379 · 4492379
1 parent c26f1b6
commit 4492379
Show file tree

Hide file tree

Showing 2 changed files with 67 additions and 0 deletions.
diff --git a/src/utility/MPU6886.cpp b/src/utility/MPU6886.cpp
@@ -223,3 +223,41 @@ void MPU6886::getTempData(float* t) {
 
     *t = (float)temp / 326.8 + 25.0;
 }
+
+void MPU6886::sleep() {
+    unsigned char regdata;
+    I2C_Read_NBytes(MPU6886_ADDRESS, MPU6886_PWR_MGMT_1, 1, &regdata);
+    regdata |= (0x01 << 6);
+    I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_PWR_MGMT_1, 1, &regdata);
+}
+
+void MPU6886::wakeup() {
+    unsigned char regdata;
+    I2C_Read_NBytes(MPU6886_ADDRESS, MPU6886_PWR_MGMT_1, 1, &regdata);
+    regdata &= ~(0x01 << 6);
+    I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_PWR_MGMT_1, 1, &regdata);
+}
+
+void MPU6886::setAccelLPF(accel_lpf_t config)
+{
+    unsigned char regdata;
+    I2C_Read_NBytes(MPU6886_ADDRESS, MPU6886_ACCEL_CONFIG2, 1, &regdata);
+    regdata &= 0b11111000;
+    regdata |= config;
+    I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_ACCEL_CONFIG2, 1, &regdata);
+}
+
+void MPU6886::setGyroLPF(gyro_lpf_t config)
+{
+    unsigned char regdata;
+    I2C_Read_NBytes(MPU6886_ADDRESS, MPU6886_CONFIG, 1, &regdata);
+    regdata &= 0b11111000;
+    regdata |= config;
+    I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_CONFIG, 1, &regdata);
+
+
+    I2C_Read_NBytes(MPU6886_ADDRESS, MPU6886_GYRO_CONFIG, 1, &regdata);
+    regdata &= 0b11111100;
+    regdata |= (config >> 3);
+    I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_GYRO_CONFIG, 1, &regdata);
+}
diff --git a/src/utility/MPU6886.h b/src/utility/MPU6886.h
@@ -55,6 +55,30 @@ class MPU6886 {
 
     enum Gscale { GFS_250DPS = 0, GFS_500DPS, GFS_1000DPS, GFS_2000DPS };
 
+    typedef enum {
+        ACCEL_LPF_BYPASS = 0b1000,
+        ACCEL_LPF_218HZ = 0b0001,
+        ACCEL_LPF_99HZ = 0b0010,
+        ACCEL_LPF_45HZ = 0b0011,
+        ACCEL_LPF_21HZ = 0b0100,
+        ACCEL_LPF_10HZ = 0b0101,
+        ACCEL_LPF_5HZ = 0b0110,
+        ACCEL_LPF_420HZ = 0b0111
+    } accel_lpf_t;
+
+    typedef enum {
+        GYRO_LPF_BYPASS_1 = 0b11000,
+        GYRO_LPF_BYPASS_2 = 0b10000,
+        GYRO_LPF_250HZ = 0b00000,
+        GYRO_LPF_176HZ = 0b00001,
+        GYRO_LPF_92HZ = 0b00010,
+        GYRO_LPF_41HZ = 0b00011,
+        GYRO_LPF_20HZ = 0b00100,
+        GYRO_LPF_10HZ = 0b00101,
+        GYRO_LPF_5HZ = 0b00110,
+        GYRO_LPF_3281HZ = 0b00111
+    } gyro_lpf_t;
+
     Gscale Gyscale = GFS_2000DPS;
     Ascale Acscale = AFS_8G;
 
@@ -74,6 +98,11 @@ class MPU6886 {
 
     void getAhrsData(float* pitch, float* roll, float* yaw);
 
+    void sleep();
+    void wakeup();
+    void setAccelLPF(accel_lpf_t config);
+    void setGyroLPF(gyro_lpf_t config);
+
    public:
     float aRes, gRes;