MPU6050 六轴姿态传感器 STM32F103 接线与代码

#include <Arduino.h>
#include <math.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Wire.h>

// 04_MPU6050 : raw-I2C MPU6050 (GY-521) reader, import-vs-domestic test.
// Bare Wire calls (no MPU6050 library) so it works on both genuine parts
// (WHO_AM_I=0x68) and domestic clones (WHO_AM_I=0x70). WHO_AM_I is only
// reported, never enforced, so neither part is rejected at init.

constexpr uint8_t OLED_ADDR = 0x3C;
constexpr int SCREEN_W = 128;
constexpr int SCREEN_H = 64;
constexpr uint8_t MPU_ADDR_68 = 0x68; // AD0 low / floating (GY-521 default)
constexpr uint8_t MPU_ADDR_69 = 0x69; // AD0 high

constexpr uint8_t REG_PWR_MGMT_1   = 0x6B;
constexpr uint8_t REG_SMPLRT_DIV   = 0x19;
constexpr uint8_t REG_CONFIG       = 0x1A;
constexpr uint8_t REG_GYRO_CONFIG  = 0x1B;
constexpr uint8_t REG_ACCEL_CONFIG = 0x1C;
constexpr uint8_t REG_WHO_AM_I     = 0x75;
constexpr uint8_t REG_ACCEL_XOUT_H = 0x3B; // accel(6)+temp(2)+gyro(6)=14B

constexpr float ACCEL_LSB_PER_G  = 16384.0f; // +-2g
constexpr float GYRO_LSB_PER_DPS = 131.0f;   // +-250 deg/s
constexpr float PI_F  = 3.14159265f;
constexpr float RAD2DEG = 180.0f / PI_F;
constexpr int WINDOW = 50;

Adafruit_SSD1306 display(SCREEN_W, SCREEN_H, &Wire, -1);
uint8_t g_mpuAddr = 0, g_whoAmI = 0;
bool g_found = false;
int16_t ax=0, ay=0, az=0, gx=0, gy=0, gz=0;
float pitch=0.0f, roll=0.0f;

static bool i2cProbe(uint8_t addr){ Wire.beginTransmission(addr); return Wire.endTransmission()==0; }
static bool mpuWrite(uint8_t addr, uint8_t reg, uint8_t val){ Wire.beginTransmission(addr); Wire.write(reg); Wire.write(val); return Wire.endTransmission()==0; }
static uint8_t mpuReadByte(uint8_t addr, uint8_t reg){ Wire.beginTransmission(addr); Wire.write(reg); if(Wire.endTransmission()!=0) return 0; if(Wire.requestFrom(addr,(uint8_t)1)<1) return 0; return (uint8_t)Wire.read(); }
static bool mpuReadAll(uint8_t addr, int16_t out[6]){
  Wire.beginTransmission(addr); Wire.write(REG_ACCEL_XOUT_H);
  if(Wire.endTransmission()!=0) return false;
  if(Wire.requestFrom(addr,(uint8_t)14)<14) return false;
  uint8_t b[14]; for(int i=0;i<14;i++) b[i]=(uint8_t)Wire.read();
  out[0]=(int16_t)((b[0]<<8)|b[1]); out[1]=(int16_t)((b[2]<<8)|b[3]); out[2]=(int16_t)((b[4]<<8)|b[5]);
  out[3]=(int16_t)((b[8]<<8)|b[9]); out[4]=(int16_t)((b[10]<<8)|b[11]); out[5]=(int16_t)((b[12]<<8)|b[13]);
  return true;
}

static void showStatus(const char *l1, const char *l2){
  display.clearDisplay(); display.setTextColor(SSD1306_WHITE); display.setTextSize(1);
  display.setCursor(0,0); display.println("04_MPU6050"); display.println(); display.println(l1); display.println(l2); display.display();
}
static void showData(){
  char buf[40];
  display.clearDisplay(); display.setTextColor(SSD1306_WHITE); display.setTextSize(1);
  display.setCursor(0,0); display.print("04_MPU6050");
  display.setCursor(0,16); snprintf(buf,sizeof(buf),"@0x%02X WHO=0x%02X",g_mpuAddr,g_whoAmI); display.print(buf);
  display.setCursor(0,32); snprintf(buf,sizeof(buf),"P %5.1f R %5.1f",pitch,roll); display.print(buf);
  display.setCursor(0,48);
  int gxd=(int)lroundf((float)gx/GYRO_LSB_PER_DPS), gyd=(int)lroundf((float)gy/GYRO_LSB_PER_DPS), gzd=(int)lroundf((float)gz/GYRO_LSB_PER_DPS);
  snprintf(buf,sizeof(buf),"gx%4d gy%4d gz%4d",gxd,gyd,gzd); display.print(buf);
  display.display();
}
static void detectAndInit(){
  g_found=false; g_mpuAddr=0; g_whoAmI=0;
  if(i2cProbe(MPU_ADDR_68)) g_mpuAddr=MPU_ADDR_68; else if(i2cProbe(MPU_ADDR_69)) g_mpuAddr=MPU_ADDR_69;
  if(g_mpuAddr==0){ showStatus("MPU not found","Check wiring"); Serial.println("MPU6050 not found on 0x68 / 0x69"); return; }
  g_whoAmI=mpuReadByte(g_mpuAddr,REG_WHO_AM_I);
  mpuWrite(g_mpuAddr,REG_PWR_MGMT_1,0x00); delay(50);
  mpuWrite(g_mpuAddr,REG_SMPLRT_DIV,0x07); mpuWrite(g_mpuAddr,REG_CONFIG,0x03);
  mpuWrite(g_mpuAddr,REG_GYRO_CONFIG,0x00); mpuWrite(g_mpuAddr,REG_ACCEL_CONFIG,0x00);
  g_found=true;
  Serial.print("MPU addr=0x"); Serial.print(g_mpuAddr,HEX);
  Serial.print(" WHO_AM_I=0x"); Serial.println(g_whoAmI,HEX); Serial.println("init OK");
}
void setup(){
  Serial.begin(115200); delay(300);
  Wire.setSCL(PB8); Wire.setSDA(PB9); Wire.begin();
  if(!display.begin(SSD1306_SWITCHCAPVCC,OLED_ADDR)){ Serial.println("OLED init failed"); while(true) delay(1000); }
  Serial.println("=== MPU6050 import/domestic test ==="); Serial.println("OLED 0x3C: found");
  showStatus("OLED OK","MPU detect..."); detectAndInit();
}
void loop(){
  if(!g_found){ detectAndInit(); delay(500); return; }
  int16_t v[6]; int32_t sum[6]={0};
  int16_t mn[6]={32767,32767,32767,32767,32767,32767}, mx[6]={-32768,-32768,-32768,-32768,-32768,-32768};
  int n=0;
  for(; n<WINDOW; n++){
    if(!mpuReadAll(g_mpuAddr,v)) break;
    for(int i=0;i<6;i++){ sum[i]+=v[i]; if(v[i]<mn[i]) mn[i]=v[i]; if(v[i]>mx[i]) mx[i]=v[i]; }
  }
  if(n==0){ showStatus("Read failed","Check wiring"); Serial.println("read failed"); delay(500); return; }
  ax=v[0]; ay=v[1]; az=v[2]; gx=v[3]; gy=v[4]; gz=v[5];
  float axg=(float)ax/ACCEL_LSB_PER_G, ayg=(float)ay/ACCEL_LSB_PER_G, azg=(float)az/ACCEL_LSB_PER_G;
  pitch=atan2f(axg,sqrtf(ayg*ayg+azg*azg))*RAD2DEG;
  roll =atan2f(ayg,sqrtf(axg*axg+azg*azg))*RAD2DEG;
  showData();
  Serial.print("ax="); Serial.print(ax); Serial.print(" ay="); Serial.print(ay); Serial.print(" az="); Serial.print(az);
  Serial.print(" gx="); Serial.print(gx); Serial.print(" gy="); Serial.print(gy); Serial.print(" gz="); Serial.print(gz);
  Serial.print(" pitch="); Serial.print(pitch,1); Serial.print(" roll="); Serial.println(roll,1);
  const char *names[6]={"ax","ay","az","gx","gy","gz"};
  Serial.print("[stats] ");
  for(int i=0;i<6;i++){ int mean=(int)(sum[i]/n), pp=(int)(mx[i]-mn[i]); Serial.print(names[i]); Serial.print(" mean="); Serial.print(mean); Serial.print(" pp="); Serial.print(pp); if(i<5) Serial.print(" | "); }
  Serial.println();
}