Created
November 23, 2023 14:34
-
-
Save doojinkang/bd4d5cc52ec78f41ee5153069cefae49 to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
// gist.github.com/doojinkang gyro.ino | |
#include "MPU6050.h" | |
MPU6050 accelgyro; | |
int16_t ax, ay, az; | |
int16_t gx, gy, gz; | |
#define GYRO_FACTOR 131.0 * M_PI / 180 | |
float roll, pitch, yaw; | |
unsigned long last_read_time = 0; | |
void setup() { | |
Serial.begin(115200); | |
// I2C initialize | |
Wire.begin(); | |
Wire.setClock(400000); | |
// MPU6050 sensor initialize | |
accelgyro.initialize(); | |
Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed"); | |
// MPU6050 sensor calibrate | |
accelgyro.CalibrateGyro(); | |
accelgyro.CalibrateAccel(); | |
// accelgyro.setXGyroOffset(220); | |
// accelgyro.setYGyroOffset(76); | |
// accelgyro.setZGyroOffset(-85); | |
while (!Serial.available()); // wait until something in | |
// last_read_time = millis(); | |
} | |
void loop() { | |
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); | |
// 1. gyro ==> angle | |
float fgx = (float)gx / GYRO_FACTOR; // 16bitVal / 131.0 * M_PI / 180 ==> rad/s | |
float fgy = (float)gy / GYRO_FACTOR; | |
float fgz = (float)gz / GYRO_FACTOR; | |
// current time (ms) | |
unsigned long t_now = millis(); | |
float dt =(t_now - last_read_time)/1000.0; | |
last_read_time = t_now; | |
roll = roll + dt * fgx; | |
pitch = pitch + dt * fgy; | |
yaw = yaw + dt * fgz; | |
// 2. accelerometer ==> angle | |
float fax = ax; | |
float fay = ay; | |
float faz = az; | |
float a_roll = atan2(fay, sqrt(fax*fax + faz*faz)); | |
float a_pitch = -atan2(fax, sqrt(fay*fay + faz*faz)); | |
float a_yaw = 0.0f; | |
// 3. gyro * 0.95 + accel * 0.05 | |
roll = 0.95 * roll + 0.05 * a_roll; | |
pitch = 0.95 * pitch + 0.05 * a_pitch; | |
// Show Result | |
Serial.print(roll); | |
Serial.print(","); | |
Serial.print(pitch); | |
Serial.print(","); | |
Serial.println(yaw); | |
delay(100); | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment