Fix IMU not work in some compiled environments

Author: Tinyu-Zhao

.github/workflows/clang-format-check.yml

@@ -8,7 +8,7 @@ jobs:
       matrix:
         path:
           - check: './'    # path to include
-            exclude: '(Fonts|utility|RFID)'  # path to exclude
+            exclude: '(Fonts|utility|RFID|THERMAL_MLX90640|HEART_MAX30100|Display|AC-SOCKET|BALA2)'  # path to exclude
           # - check: 'src'
           #   exclude: '(Fonts)' # Exclude file paths containing "Fonts"
           # - check: 'examples'

README.md

@@ -10,7 +10,9 @@ English | [中文](docs/getting_started_cn.md) | [日本語](docs/getting_starte
 
 * **For the Detailed documentation of Basic, please [Click here](https://docs.m5stack.com/en/core/basic_v2.6)**
 
-* **In order to buy Basic, please [Click here](https://shop.m5stack.com/collections/m5-controllers/products/esp32-basic-core-iot-development-kit-v2-6)**
+* **In order to buy Gray, please [Click here](https://shop.m5stack.com/products/grey-development-core)**
+
+* **In order to buy Basic, please [Click here](https://shop.m5stack.com/products/esp32-basic-core-iot-development-kit-v2-6)**
 
 *We have several master M5Cores with different configurations, this is the difference between them [Compared](https://docs.m5stack.com/en/products_selector).*
 

examples/Basics/IMU/IMU.ino

@@ -7,10 +7,10 @@
 * 获取更多资料请访问: https://docs.m5stack.com/zh_CN/core/gray
 *
 * Describe: MPU6886 example.  惯性传感器
-* Date: 2021/7/21
+* Date: 2022/9/8
 *******************************************************************************
 */
-#define M5STACK_MPU6886
+
 #include <M5Stack.h>
 
 float accX = 0.0F;  // Define variables for storing inertial sensor data

library.json

@@ -10,7 +10,7 @@
     "type": "git",
     "url": "https://github.com/m5stack/m5stack.git"
   },
-  "version": "0.4.2",
+  "version": "0.4.3",
   "frameworks": "arduino",
   "platforms": "espressif32",
   "headers": "M5Stack.h"

library.properties

@@ -1,5 +1,5 @@
 name=M5Stack
-version=0.4.2
+version=0.4.3
 author=M5Stack
 maintainer=M5Stack
 sentence=Library for M5Stack Core development kit

src/IMU.cpp

@@ -0,0 +1,109 @@
+#include "IMU.h"
+
+#include <Arduino.h>
+#include <math.h>
+
+#include "M5Stack.h"
+#undef IMU
+
+IMU::IMU() {
+}
+
+int IMU::Init(void) {
+    int imu_flag = M5.Sh200Q.Init();
+    Serial.printf("imu_flag:%d", imu_flag);
+    if (imu_flag != 0) {
+        imu_flag = M5.Mpu6886.Init();
+        if (imu_flag == 0) {
+            imuType = IMU_MPU6886;
+            Serial.printf("IMU_MPU6886");
+        } else {
+            imuType = IMU_UNKNOWN;
+            Serial.printf("IMU_UNKNOWN");
+            return -1;
+        }
+    } else {
+        imuType = IMU_SH200Q;
+    }
+    return 0;
+}
+
+void IMU::getGres() {
+    if (imuType == IMU_SH200Q) {
+        gRes = M5.Sh200Q.gRes;
+    } else if (imuType == IMU_MPU6886) {
+        gRes = M5.Mpu6886.gRes;
+    }
+}
+
+void IMU::getAres() {
+    if (imuType == IMU_SH200Q) {
+        aRes = M5.Sh200Q.aRes;
+    } else if (imuType == IMU_MPU6886) {
+        aRes = M5.Mpu6886.aRes;
+    }
+}
+
+void IMU::getAccelAdc(int16_t *ax, int16_t *ay, int16_t *az) {
+    if (imuType == IMU_SH200Q) {
+        M5.Sh200Q.getAccelAdc(ax, ay, az);
+    } else if (imuType == IMU_MPU6886) {
+        M5.Mpu6886.getAccelAdc(ax, ay, az);
+    }
+}
+
+void IMU::getAccelData(float *ax, float *ay, float *az) {
+    if (imuType == IMU_SH200Q) {
+        M5.Sh200Q.getAccelData(ax, ay, az);
+    } else if (imuType == IMU_MPU6886) {
+        M5.Mpu6886.getAccelData(ax, ay, az);
+    }
+}
+
+void IMU::getGyroAdc(int16_t *gx, int16_t *gy, int16_t *gz) {
+    if (imuType == IMU_SH200Q) {
+        M5.Sh200Q.getGyroAdc(gx, gy, gz);
+    } else if (imuType == IMU_MPU6886) {
+        M5.Mpu6886.getGyroAdc(gx, gy, gz);
+    }
+}
+
+void IMU::getGyroData(float *gx, float *gy, float *gz) {
+    if (imuType == IMU_SH200Q) {
+        M5.Sh200Q.getGyroData(gx, gy, gz);
+    } else if (imuType == IMU_MPU6886) {
+        M5.Mpu6886.getGyroData(gx, gy, gz);
+    }
+}
+
+void IMU::getTempAdc(int16_t *t) {
+    if (imuType == IMU_SH200Q) {
+        M5.Sh200Q.getTempAdc(t);
+    } else if (imuType == IMU_MPU6886) {
+        M5.Mpu6886.getTempAdc(t);
+    }
+}
+
+void IMU::getTempData(float *t) {
+    if (imuType == IMU_SH200Q) {
+        M5.Sh200Q.getTempData(t);
+    } else if (imuType == IMU_MPU6886) {
+        M5.Mpu6886.getTempData(t);
+    }
+}
+
+void IMU::getAhrsData(float *pitch, float *roll, float *yaw) {
+    float accX = 0;
+    float accY = 0;
+    float accZ = 0;
+
+    float gyroX = 0;
+    float gyroY = 0;
+    float gyroZ = 0;
+
+    getGyroData(&gyroX, &gyroY, &gyroZ);
+    getAccelData(&accX, &accY, &accZ);
+
+    MahonyAHRSupdateIMU(gyroX * DEG_TO_RAD, gyroY * DEG_TO_RAD,
+                        gyroZ * DEG_TO_RAD, accX, accY, accZ, pitch, roll, yaw);
+}

src/IMU.h

@@ -0,0 +1,34 @@
+#ifndef __IMU_H__
+#define __IMU_H__
+
+#include <Arduino.h>
+#include <Wire.h>
+
+#include "utility/MahonyAHRS.h"
+
+class IMU {
+   public:
+    enum ImuType { IMU_UNKNOWN = 0, IMU_SH200Q, IMU_MPU6886 };
+
+    IMU();
+
+    int Init(void);
+
+    void getGres();
+    void getAres();
+
+    void getAccelAdc(int16_t *ax, int16_t *ay, int16_t *az);
+    void getGyroAdc(int16_t *gx, int16_t *gy, int16_t *gz);
+    void getTempAdc(int16_t *t);
+
+    void getAccelData(float *ax, float *ay, float *az);
+    void getGyroData(float *gx, float *gy, float *gz);
+    void getTempData(float *t);
+
+    void getAhrsData(float *pitch, float *roll, float *yaw);
+
+    ImuType imuType;
+    float aRes, gRes;
+};
+
+#endif

src/M5Stack.h

@@ -111,18 +111,14 @@
 #include "M5Display.h"
 #include "SD.h"
 #include "gitTagVersion.h"
+#include "IMU.h"
 #include "utility/Button.h"
 #include "utility/CommUtil.h"
 #include "utility/Config.h"
 #include "utility/Power.h"
 #include "utility/Speaker.h"
-
-#if defined(M5STACK_MPU6886) || defined(M5STACK_MPU9250) || \
-    defined(M5STACK_MPU6050)
 #include "utility/MPU6886.h"
-#elif defined M5STACK_200Q
 #include "utility/SH200Q.h"
-#endif
 
 class M5Stack {
    public:
@@ -131,6 +127,12 @@ class M5Stack {
                bool SerialEnable = true, bool I2CEnable = false);
     void update();
 
+    // LCD
+    M5Display Lcd = M5Display();
+
+    // Power
+    POWER Power;
+
 // Button API
 #define DEBOUNCE_MS 10
     Button BtnA = Button(BUTTON_A_PIN, true, DEBOUNCE_MS);
@@ -140,26 +142,17 @@ class M5Stack {
     // SPEAKER
     SPEAKER Speaker;
 
-    // LCD
-    M5Display Lcd = M5Display();
-
-    // Power
-    POWER Power;
-
     // UART
     // HardwareSerial Serial0 = HardwareSerial(0);
     // HardwareSerial Serial2 = HardwareSerial(2);
 
-#if defined(M5STACK_MPU6886) || defined(M5STACK_MPU9250) || \
-    defined(M5STACK_MPU6050)
-    MPU6886 IMU = MPU6886();
-#elif defined M5STACK_200Q
-    SH200Q IMU = SH200Q();
-#endif
-
     // I2C
+    IMU Imu;
     CommUtil I2C = CommUtil();
 
+    MPU6886 Mpu6886;
+    SH200Q Sh200Q;
+
     /**
      * Function has been move to Power class.(for compatibility)
      * This name will be removed in a future release.
@@ -173,8 +166,14 @@ class M5Stack {
 };
 
 extern M5Stack M5;
-#define m5  M5
-#define lcd Lcd
+#define m5      M5
+#define lcd     Lcd
+#define imu     Imu
+#define IMU     Imu
+#define MPU6886 Mpu6886
+#define mpu6886 Mpu6886
+#define SH200Q  Sh200Q
+#define sh200q  Sh200Q
 #else
 #error "This library only supports boards with ESP32 processor."
 #endif

src/utility/MPU6886.cpp

@@ -3,20 +3,29 @@
 #include <Arduino.h>
 #include <math.h>
 
-#include "../M5Stack.h"
-#include "MahonyAHRS.h"
-
 MPU6886::MPU6886() {
 }
 
 void MPU6886::I2C_Read_NBytes(uint8_t driver_Addr, uint8_t start_Addr,
                               uint8_t number_Bytes, uint8_t* read_Buffer) {
-    M5.I2C.readBytes(driver_Addr, start_Addr, number_Bytes, read_Buffer);
+    Wire.beginTransmission(driver_Addr);
+    Wire.write(start_Addr);
+    Wire.endTransmission(false);
+    uint8_t i = 0;
+    Wire.requestFrom(driver_Addr, number_Bytes);
+
+    //! Put read results in the Rx buffer
+    while (Wire.available()) {
+        read_Buffer[i++] = Wire.read();
+    }
 }
 
 void MPU6886::I2C_Write_NBytes(uint8_t driver_Addr, uint8_t start_Addr,
                                uint8_t number_Bytes, uint8_t* write_Buffer) {
-    M5.I2C.writeBytes(driver_Addr, start_Addr, write_Buffer, number_Bytes);
+    Wire.beginTransmission(driver_Addr);
+    Wire.write(start_Addr);
+    Wire.write(*write_Buffer);
+    Wire.endTransmission();
 }
 
 int MPU6886::Init(void) {
@@ -114,9 +123,9 @@ void MPU6886::getGyroAdc(int16_t* gx, int16_t* gy, int16_t* gz) {
 
 void MPU6886::getTempAdc(int16_t* t) {
     uint8_t buf[2];
-    I2C_Read_NBytes(MPU6886_ADDRESS, MPU6886_TEMP_OUT_H, 2, buf);
+    I2C_Read_NBytes(MPU6886_ADDRESS, MPU6886_TEMP_OUT_H, 14, buf);
 
-    *t = ((uint16_t)buf[0] << 8) | buf[1];
+    *t = ((uint16_t)buf[6] << 8) | buf[7];
 }
 
 //!俯仰，航向，横滚: pitch，yaw，roll，指三维空间中飞行器的旋转状态。

src/utility/MPU6886.h

@@ -11,6 +11,8 @@
 #include <Arduino.h>
 #include <Wire.h>
 
+#include "MahonyAHRS.h"
+
 #define MPU6886_ADDRESS          0x68
 #define MPU6886_WHOAMI           0x75
 #define MPU6886_ACCEL_INTEL_CTRL 0x69

src/utility/SH200Q.cpp

@@ -3,20 +3,32 @@
 #include <Arduino.h>
 #include <math.h>
 
-#include "../M5Stack.h"
-#include "MahonyAHRS.h"
-
 SH200Q::SH200Q() {
 }
 
 void SH200Q::I2C_Read_NBytes(uint8_t driver_Addr, uint8_t start_Addr,
                              uint8_t number_Bytes, uint8_t* read_Buffer) {
-    M5.I2C.readBytes(driver_Addr, start_Addr, number_Bytes, read_Buffer);
+    Wire.beginTransmission(driver_Addr);
+    Wire.write(start_Addr);
+    Wire.endTransmission();
+    uint8_t i = 0;
+    Wire.requestFrom(driver_Addr, number_Bytes);
+    // byte buf = Wire1.read();
+    //*read_Buffer = buf;
+    //! Put read results in the Rx buffer
+    while (Wire.available()) {
+        read_Buffer[i++] = Wire1.read();
+    }
 }
 
 void SH200Q::I2C_Write_NBytes(uint8_t driver_Addr, uint8_t start_Addr,
                               uint8_t number_Bytes, uint8_t* write_Buffer) {
-    M5.I2C.writeBytes(driver_Addr, start_Addr, write_Buffer, number_Bytes);
+    Wire.beginTransmission(driver_Addr);
+    Wire.write(start_Addr);
+    Wire.write(*write_Buffer);
+    Wire.endTransmission();
+    // Serial.printf("I2C Write OP, ADDR: 0x%02x, ADS: 0x%02x, NumBytes: %u,
+    // Data: 0x%02x\n\r", driver_Addr, start_Addr, number_Bytes, *write_Buffer);
 }
 
 void SH200Q::sh200i_ADCReset(void) {

src/utility/SH200Q.h

@@ -4,6 +4,8 @@
 #include <Arduino.h>
 #include <Wire.h>
 
+#include "MahonyAHRS.h"
+
 #define SH200I_ADDRESS     0x6C  // 7bit i2c address
 #define SH200I_WHOAMI      0x30
 #define SH200I_ACC_CONFIG  0x0E