Add M5.IMU support

Author: EeeeBin

examples/Basics/IMU/IMU.ino

@@ -0,0 +1,66 @@
+// define must ahead #include <M5Stack.h>
+#define M5STACK_MPU6886 
+// #define M5STACK_MPU9250 
+// #define M5STACK_MPU6050
+// #define M5STACK_200Q
+
+#include <M5Stack.h>
+
+float accX = 0.0F;
+float accY = 0.0F;
+float accZ = 0.0F;
+
+float gyroX = 0.0F;
+float gyroY = 0.0F;
+float gyroZ = 0.0F;
+
+float pitch = 0.0F;
+float roll  = 0.0F;
+float yaw   = 0.0F;
+
+float temp = 0.0F;
+
+// the setup routine runs once when M5Stack starts up
+void setup(){
+
+  // Initialize the M5Stack object
+  M5.begin();
+  /*
+    Power chip connected to gpio21, gpio22, I2C device
+    Set battery charging voltage and current
+    If used battery, please call this function in your project
+  */
+  M5.Power.begin();
+    
+  M5.IMU.Init();
+
+  M5.Lcd.fillScreen(BLACK);
+  M5.Lcd.setTextColor(GREEN , BLACK);
+  M5.Lcd.setTextSize(2);
+}
+
+// the loop routine runs over and over again forever
+void loop() {
+    // put your main code here, to run repeatedly:
+  M5.IMU.getGyroData(&gyroX,&gyroY,&gyroZ);
+  M5.IMU.getAccelData(&accX,&accY,&accZ);
+  M5.IMU.getAhrsData(&pitch,&roll,&yaw);
+  M5.IMU.getTempData(&temp);
+  
+  M5.Lcd.setCursor(0, 20);
+  M5.Lcd.printf("%6.2f  %6.2f  %6.2f      ", gyroX, gyroY, gyroZ);
+  M5.Lcd.setCursor(220, 42);
+  M5.Lcd.print(" o/s");
+  M5.Lcd.setCursor(0, 65);
+  M5.Lcd.printf(" %5.2f   %5.2f   %5.2f   ", accX, accY, accZ);
+  M5.Lcd.setCursor(220, 87);
+  M5.Lcd.print(" G");
+  M5.Lcd.setCursor(0, 110);
+  M5.Lcd.printf(" %5.2f   %5.2f   %5.2f   ", pitch, roll, yaw);
+  M5.Lcd.setCursor(220, 132);
+  M5.Lcd.print(" degree");
+  M5.Lcd.setCursor(0, 155);
+  M5.Lcd.printf("Temperature : %.2f C", temp);
+
+  delay(1);
+}

src/M5Stack.h

@@ -91,13 +91,13 @@
  * </pre>
  *
  */
+// #define ESP32
 
 #ifndef _M5STACK_H_
   #define _M5STACK_H_
 
   #if defined(ESP32)
 
-    // #define MPU9250_INSDE
     #include "gitTagVersion.h"
     #include <Arduino.h>
     #include <Wire.h>
@@ -112,6 +112,12 @@
     #include "utility/Power.h"
     #include "utility/CommUtil.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:
@@ -138,11 +144,12 @@
         // HardwareSerial Serial0 = HardwareSerial(0);
         // HardwareSerial Serial2 = HardwareSerial(2);
 
-        // MPU9250
-        #ifdef MPU9250_INSDE
-          MPU9250 IMU = MPU9250();
+        #if defined(M5STACK_MPU6886) || defined(M5STACK_MPU9250) || defined(M5STACK_MPU6050)
+        MPU6886 IMU = MPU6886();
+        #elif defined M5STACK_200Q
+        SH200Q IMU = SH200Q();
         #endif
-          
+
         // I2C
         CommUtil I2C = CommUtil();
 

src/utility/MPU6886.cpp

@@ -0,0 +1,217 @@
+#include "MPU6886.h"
+#include <math.h>
+#include <Arduino.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);
+}
+
+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);
+}
+
+int MPU6886::Init(void) {
+  unsigned char tempdata[1];
+  unsigned char regdata;
+  
+  Gyscale = GFS_2000DPS;
+  Acscale = AFS_8G;
+  
+  Wire1.begin(21,22);
+  
+  I2C_Read_NBytes(MPU6886_ADDRESS, MPU6886_WHOAMI, 1, tempdata);
+  imuId = tempdata[0];
+  delay(1);
+
+  regdata = 0x00;
+  I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_PWR_MGMT_1, 1, &regdata);
+  delay(10);
+
+  regdata = (0x01<<7);
+  I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_PWR_MGMT_1, 1, &regdata);
+  delay(10);
+
+  regdata = (0x01<<0);
+  I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_PWR_MGMT_1, 1, &regdata);
+  delay(10);
+
+  regdata = 0x10;
+  I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_ACCEL_CONFIG, 1, &regdata);
+  delay(1);
+
+  regdata = 0x18;
+  I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_GYRO_CONFIG, 1, &regdata);
+  delay(1);
+
+  regdata = 0x01;
+  I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_CONFIG, 1, &regdata);
+  delay(1);
+
+  regdata = 0x05;
+  I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_SMPLRT_DIV, 1,&regdata);
+  delay(1);
+
+  regdata = 0x00;
+  I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_INT_ENABLE, 1, &regdata);
+  delay(1);
+
+  regdata = 0x00;
+  I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_ACCEL_CONFIG2, 1, &regdata);
+  delay(1);
+
+  regdata = 0x00;
+  I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_USER_CTRL, 1, &regdata);
+  delay(1);
+
+  regdata = 0x00;
+  I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_FIFO_EN, 1, &regdata);
+  delay(1);
+
+  regdata = 0x22;
+  I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_INT_PIN_CFG, 1, &regdata);
+  delay(1);
+
+  regdata = 0x01;
+  I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_INT_ENABLE, 1, &regdata);
+
+  delay(10);
+
+  setGyroFsr(Gyscale);
+  setAccelFsr(Acscale);
+  return 0;
+}
+
+void MPU6886::getAccelAdc(int16_t* ax, int16_t* ay, int16_t* az) {
+  uint8_t buf[6];  
+  I2C_Read_NBytes(MPU6886_ADDRESS, MPU6886_ACCEL_XOUT_H, 6, buf);
+
+  *ax=((int16_t)buf[0]<<8)|buf[1];
+  *ay=((int16_t)buf[2]<<8)|buf[3];
+  *az=((int16_t)buf[4]<<8)|buf[5];
+}
+
+void MPU6886::getGyroAdc(int16_t* gx, int16_t* gy, int16_t* gz) {
+  uint8_t buf[6];
+  I2C_Read_NBytes(MPU6886_ADDRESS,MPU6886_GYRO_XOUT_H, 6, buf);
+  
+  *gx=((uint16_t)buf[0]<<8)|buf[1];  
+  *gy=((uint16_t)buf[2]<<8)|buf[3];  
+  *gz=((uint16_t)buf[4]<<8)|buf[5];
+}
+
+void MPU6886::getTempAdc(int16_t *t) {
+  uint8_t buf[2];  
+  I2C_Read_NBytes(MPU6886_ADDRESS,MPU6886_TEMP_OUT_H,2,buf);
+  
+  *t=((uint16_t)buf[0]<<8)|buf[1];  
+}
+
+//!俯仰，航向，横滚：pitch，yaw，roll，指三维空间中飞行器的旋转状态。
+void MPU6886::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);
+}
+
+// Possible gyro scales (and their register bit settings)
+void MPU6886::updateGres() {
+  switch (Gyscale) {
+    case GFS_250DPS:
+      gRes = 250.0/32768.0;
+      break;
+    case GFS_500DPS:
+      gRes = 500.0/32768.0;
+      break;
+    case GFS_1000DPS:
+      gRes = 1000.0/32768.0;
+      break;
+    case GFS_2000DPS:
+      gRes = 2000.0/32768.0;
+      break;
+  }
+
+}
+
+// Possible accelerometer scales (and their register bit settings) are:
+// 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs  (11). 
+// Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value:
+void MPU6886::updateAres() {
+  switch (Acscale) {
+    case AFS_2G:
+      aRes = 2.0/32768.0;
+      break;
+    case AFS_4G:
+      aRes = 4.0/32768.0;
+      break;
+    case AFS_8G:
+      aRes = 8.0/32768.0;
+      break;
+    case AFS_16G:
+      aRes = 16.0/32768.0;
+      break;
+  }
+}
+ 
+void MPU6886::setGyroFsr(Gscale scale) {
+  unsigned char regdata;	
+  regdata = (scale<<3);
+  I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_GYRO_CONFIG, 1, &regdata);
+  delay(10);
+  Gyscale = scale;
+  updateGres();
+}
+
+void MPU6886::setAccelFsr(Ascale scale) {
+  unsigned char regdata;	
+  regdata = (scale<<3);
+  I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_ACCEL_CONFIG, 1, &regdata);
+  delay(10);
+  Acscale = scale;
+  updateAres();
+}
+
+void MPU6886::getAccelData(float* ax, float* ay, float* az) {
+  int16_t accX = 0;
+  int16_t accY = 0;
+  int16_t accZ = 0;
+  getAccelAdc(&accX,&accY,&accZ);
+
+  *ax = (float)accX * aRes;
+  *ay = (float)accY * aRes;
+  *az = (float)accZ * aRes;
+}
+      
+void MPU6886::getGyroData(float* gx, float* gy, float* gz) {
+  int16_t gyroX = 0;
+  int16_t gyroY = 0;
+  int16_t gyroZ = 0;
+  getGyroAdc(&gyroX,&gyroY,&gyroZ);
+
+  *gx = (float)gyroX * gRes;
+  *gy = (float)gyroY * gRes;
+  *gz = (float)gyroZ * gRes;
+}
+
+void MPU6886::getTempData(float *t) {
+  int16_t temp = 0;
+  getTempAdc(&temp);
+  
+  *t = (float)temp / 326.8 + 25.0;
+}