M5StickC/src/IMU.cpp at ca327af2b8d8172c1c4894769fc9e731175cfe83 · m5stack/M5StickC · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
#include "IMU.h"
#include <math.h>
#include <Arduino.h>
#include "M5StickC.h"
#undef IMU

IMU::IMU() {
}

int IMU::Init(void) {
    int imu_flag = M5.Sh200Q.Init();
    if (imu_flag != 0) {
        imu_flag = M5.Mpu6886.Init();
        if (imu_flag == 0) {
            imuType = IMU_MPU6886;
        } else {
            imuType = 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,0.0f); // 0 frequency internal defined value

}

void IMU::getAhrsData(float *pitch,float *roll,float *yaw, float samplefrequency){

  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,samplefrequency);

}