mirror of
https://github.com/m5stack/M5Stack.git
synced 2026-05-20 10:06:46 -07:00
Gitshaoxiang
78457b190e
* rename some example file * add Unit exmaple sketch comment * add module example comment
80 lines
1.5 KiB
C++
80 lines
1.5 KiB
C++
/*
|
|
* @Author: Sorzn
|
|
* @Date: 2020-01-06 11:00:49
|
|
* @LastEditTime : 2020-01-06 15:15:40
|
|
* @Description: M5Stack project
|
|
* @FilePath: /home/sakabin/Arduino/libraries/M5Stack/examples/Modules/Bala/imuAuto.cpp
|
|
*/
|
|
|
|
|
|
#include "imuCalibration.h"
|
|
|
|
static float gyroXOffset = 0;
|
|
static float gyroYOffset = 0;
|
|
static float gyroZOffset = 0;
|
|
|
|
static float accX = 0;
|
|
static float accY = 0;
|
|
static float accZ = 0;
|
|
|
|
static float gyroX = 0;
|
|
static float gyroY = 0;
|
|
static float gyroZ = 0;
|
|
|
|
static float angleAccX = 0;
|
|
static float angleGyroX = 0;
|
|
static float angleX = 0;
|
|
|
|
static uint32_t preInterval = 0;
|
|
|
|
void imu_CalcInit() {
|
|
M5.IMU.Init();
|
|
M5.IMU.setGyroFsr(M5.IMU.GFS_1000DPS);
|
|
}
|
|
|
|
void imu_setOffsetX(float x) {
|
|
gyroXOffset = x;
|
|
}
|
|
|
|
float imu_getOffsetX() {
|
|
return gyroXOffset;
|
|
}
|
|
|
|
void imu_calcGyroOffsets() {
|
|
float x = 0, y = 0, z = 0;
|
|
float x_total = 0, y_total = 0, z_total = 0;
|
|
|
|
for(int i = 0; i < 3000; i++) {
|
|
M5.IMU.getGyroData(&x, &y, &z);
|
|
x_total += x;
|
|
}
|
|
|
|
gyroXOffset = x_total / 3000;
|
|
}
|
|
|
|
void imu_update() {
|
|
float interval;
|
|
if(preInterval == 0) preInterval = millis();
|
|
|
|
M5.IMU.getGyroData(&gyroX, &gyroY, &gyroZ);
|
|
M5.IMU.getAccelData(&accX, &accY, &accZ);
|
|
|
|
angleAccX = atan2(accY, sqrt(accZ * accZ + accX * accX)) * 360 / 2.0 / PI;
|
|
|
|
gyroX -= gyroXOffset;
|
|
|
|
interval = (millis() - preInterval) * 0.001;
|
|
|
|
preInterval = millis();
|
|
|
|
angleX = (0.98 * (angleX + gyroX * interval)) + (0.02 * angleAccX);
|
|
}
|
|
|
|
float imu_getAngleX() {
|
|
#ifdef M5STACK_MPU6886 || M5STACK_200Q:
|
|
return 0 - angleX;
|
|
#else:
|
|
return angleX;
|
|
#endif
|
|
}
|