You've already forked M5-ProductExampleCodes
mirror of
https://github.com/m5stack/M5-ProductExampleCodes.git
synced 2026-05-20 10:24:10 -07:00
vany5921
58 lines
1.5 KiB
C++
58 lines
1.5 KiB
C++
#define M5STACK_MPU6886
|
|
#include "M5Stack.h"
|
|
#include <Preferences.h>
|
|
|
|
Preferences preferences;
|
|
|
|
void GetGyroOffset(uint16_t times, int16_t* x_offset, int16_t* y_offset, int16_t* z_offset) {
|
|
int64_t x = 0, y = 0, z = 0;
|
|
int16_t gyro_x, gyro_y, gyro_z;
|
|
for (size_t i = 0; i < times; i++) {
|
|
M5.IMU.getGyroAdc(&gyro_x, &gyro_y, &gyro_z);
|
|
x += gyro_x;
|
|
y += gyro_y;
|
|
z += gyro_z;
|
|
}
|
|
|
|
*x_offset = x / times;
|
|
*y_offset = y / times;
|
|
*z_offset = z / times;
|
|
}
|
|
|
|
void calibrationInit() {
|
|
preferences.begin("Bala2Cal", false);
|
|
}
|
|
|
|
void calibrationGet(int16_t* gyro_x_offset, int16_t* gyro_y_offset, int16_t* gyro_z_offset, float *angle_center) {
|
|
*gyro_x_offset = preferences.getInt("gryo_x", 0);
|
|
*gyro_y_offset = preferences.getInt("gryo_y", 0);
|
|
*gyro_z_offset = preferences.getInt("gryo_z", 0);
|
|
|
|
*angle_center = preferences.getFloat("angle", 0.0);
|
|
}
|
|
|
|
void calibrationGryo() {
|
|
M5.Lcd.fillScreen(BLACK);
|
|
M5.Lcd.setTextFont(2);
|
|
M5.Lcd.setTextColor(GREEN);
|
|
M5.Lcd.printf("Start gryo calibration\r\n");
|
|
delay(1000);
|
|
|
|
M5.Lcd.printf("Please keep BALA2 still for 2 seconds\r\n");
|
|
|
|
int16_t x_offset = 0;
|
|
int16_t y_offset = 0;
|
|
int16_t z_offset = 0;
|
|
GetGyroOffset(2000, &x_offset, &y_offset, &z_offset);
|
|
M5.Lcd.printf("Finish calibration !!!\r\n");
|
|
|
|
preferences.putInt("gryo_x", x_offset);
|
|
preferences.putInt("gryo_y", y_offset);
|
|
preferences.putInt("gryo_z", z_offset);
|
|
|
|
delay(1000);
|
|
}
|
|
|
|
void calibrationSaveCenterAngle(float angle) {
|
|
preferences.putFloat("angle", angle);
|
|
} |