ESP32 | MPU6050 - Using MahonyAHRS (ft. dice)

2026-01-12 09:17:42 +03:00 · 2020-11-20 20:22:47 -08:00
parent a7439ac908
commit 67d81908b5
2 changed files with 1000 additions and 0 deletions
--- a/Esp32_MPU6050_MahonyARHS/MPU6050_Calibration_ESP32/MPU6050_Calibration_ESP32.ino
+++ b/Esp32_MPU6050_MahonyARHS/MPU6050_Calibration_ESP32/MPU6050_Calibration_ESP32.ino
@@ -0,0 +1,110 @@
+//Source: https://blog.naver.com/ysahn2k/221410891895
+
+#include <Wire.h>
+#define SDA 0
+#define SCL 4
+
+const int MPU_ADDR=0x68;  // I2C address of the MPU-6050
+int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;
+long Cal_AcX, Cal_AcY, Cal_AcZ, Cal_Tmp, Cal_GyX, Cal_GyY, Cal_GyZ;
+
+void setup(){
+  Wire.begin(SDA, SCL, 400000);
+  init_MPU6050();
+  Serial.begin(115200);
+}
+
+void loop(){
+
+  for(int i = 0 ; i < 2000 ; i++) {
+
+    if(i % 200 == 0) Serial.println("Calculating .....");
+    
+    Wire.beginTransmission(MPU_ADDR);
+    Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
+    Wire.endTransmission(false);
+    Wire.requestFrom(MPU_ADDR,14,true);  // request a total of 14 registers
+    AcX=Wire.read()<<8|Wire.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)     
+    AcY=Wire.read()<<8|Wire.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
+    AcZ=(Wire.read()<<8|Wire.read()) - 4096;  // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
+    Tmp=Wire.read()<<8|Wire.read();  // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
+    GyX=Wire.read()<<8|Wire.read();  // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
+    GyY=Wire.read()<<8|Wire.read();  // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
+    GyZ=Wire.read()<<8|Wire.read();  // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
+    delay(10);
+
+    // Sum data
+    Cal_AcX += AcX;
+    Cal_AcY += AcY;
+    Cal_AcZ += AcZ;
+    Cal_GyX += GyX;
+    Cal_GyY += GyY;
+    Cal_GyZ += GyZ;
+
+  }
+
+  // Average Data
+  Cal_AcX /= 2000;
+  Cal_AcY /= 2000;
+  Cal_AcZ /= 2000;
+  Cal_GyX /= 2000;
+  Cal_GyY /= 2000;
+  Cal_GyZ /= 2000;
+
+  // Print Data
+  Serial.println("End of Calculation");
+  Serial.print("AcX = "); Serial.print(Cal_AcX);
+  Serial.print(" | AcY = "); Serial.print(Cal_AcY);
+  Serial.print(" | AcZ = "); Serial.print(Cal_AcZ);
+  //Serial.print(" | Tmp = "); Serial.print(Tmp/340.00+36.53);  //equation for temperature in degrees C from datasheet
+  Serial.print(" | GyX = "); Serial.print(Cal_GyX);
+  Serial.print(" | GyY = "); Serial.print(Cal_GyY);
+  Serial.print(" | GyZ = "); Serial.println(Cal_GyZ);
+
+  while(1) {}
+
+}
+
+void init_MPU6050(){
+  //MPU6050 Initializing & Reset
+  Wire.beginTransmission(MPU_ADDR);
+  Wire.write(0x6B);  // PWR_MGMT_1 register
+  Wire.write(0);     // set to zero (wakes up the MPU-6050)
+  Wire.endTransmission(true);
+
+  //MPU6050 Clock Type
+  Wire.beginTransmission(MPU_ADDR);
+  Wire.write(0x6B);  // PWR_MGMT_1 register
+  Wire.write(0x03);     // Selection Clock 'PLL with Z axis gyroscope reference'
+  Wire.endTransmission(true);
+
+  //MPU6050 Gyroscope Configuration Setting
+  Wire.beginTransmission(MPU_ADDR);
+  Wire.write(0x1B);  // Gyroscope Configuration register
+  //Wire.write(0x00);     // FS_SEL=0, Full Scale Range = +/- 250 [degree/sec]
+  //Wire.write(0x08);     // FS_SEL=1, Full Scale Range = +/- 500 [degree/sec]
+  //Wire.write(0x10);     // FS_SEL=2, Full Scale Range = +/- 1000 [degree/sec]
+  Wire.write(0x18);     // FS_SEL=3, Full Scale Range = +/- 2000 [degree/sec]
+  Wire.endTransmission(true);
+
+  //MPU6050 Accelerometer Configuration Setting
+  Wire.beginTransmission(MPU_ADDR);
+  Wire.write(0x1C);  // Accelerometer Configuration register
+  //Wire.write(0x00);     // AFS_SEL=0, Full Scale Range = +/- 2 [g]
+  //Wire.write(0x08);     // AFS_SEL=1, Full Scale Range = +/- 4 [g]
+  Wire.write(0x10);     // AFS_SEL=2, Full Scale Range = +/- 8 [g]
+  //Wire.write(0x18);     // AFS_SEL=3, Full Scale Range = +/- 10 [g]
+  Wire.endTransmission(true);
+
+  //MPU6050 DLPF(Digital Low Pass Filter)
+  Wire.beginTransmission(MPU_ADDR);
+  Wire.write(0x1A);  // DLPF_CFG register
+  Wire.write(0x00);     // Accel BW 260Hz, Delay 0ms / Gyro BW 256Hz, Delay 0.98ms, Fs 8KHz 
+  //Wire.write(0x01);     // Accel BW 184Hz, Delay 2ms / Gyro BW 188Hz, Delay 1.9ms, Fs 1KHz 
+  //Wire.write(0x02);     // Accel BW 94Hz, Delay 3ms / Gyro BW 98Hz, Delay 2.8ms, Fs 1KHz 
+  //Wire.write(0x03);     // Accel BW 44Hz, Delay 4.9ms / Gyro BW 42Hz, Delay 4.8ms, Fs 1KHz 
+  //Wire.write(0x04);     // Accel BW 21Hz, Delay 8.5ms / Gyro BW 20Hz, Delay 8.3ms, Fs 1KHz 
+  //Wire.write(0x05);     // Accel BW 10Hz, Delay 13.8ms / Gyro BW 10Hz, Delay 13.4ms, Fs 1KHz 
+  //Wire.write(0x06);     // Accel BW 5Hz, Delay 19ms / Gyro BW 5Hz, Delay 18.6ms, Fs 1KHz 
+  Wire.endTransmission(true);
+}