GY-95T九轴传感器_Arduino_ESP32读取数据程序_附代码

这就引发了一个问题,我比较喜欢用dmp解算,因为6050自带的dmp航向角几乎很少偏移,就算一直转圈圈,偏移得也很少, 可dmp需要独占iic总线(dmp-mpu6050内部), 也就是两个只能二选一, 自己用融合算法之类的,都打不到dmp的效果,(不借助磁力计的情况下,yaw偏差都很大),所以才买了GY-95T九轴传感器。下面这个是iic连接的,这个是基于店家给的资料改的,毕竟iic的比较简单,

wcyqsmy1

452人浏览 · 2025-02-18 06:05:31

wcyqsmy1 · 2025-02-18 06:05:31 发布

废话,可以跳过

前段时间买了一个GY87_10轴陀螺仪模块,就是这个货

read-normal-img

刚开始以为这个模块也就是把三个传感器集成在了一起, 结果买回来才发现QMC5883L是连接在MPU6050的备用iic上, 并不能直接使用,要先把备用iic连接到主iic上, 就是这样

read-normal-img

正文

买模块自带的arduino程序并不能用在esp32上,表面上看起来就只有串口初始化的差异,实际上坑有点多,比如温度一直不准, 自己照着说明书写的一个程序,花了我一晚上

废话不多说,上代码 硬件连接是10,和11 这个用的是串口直接输出数据

#include <Arduino.h>

typedef struct
{
  int16_t roll;
  int16_t pitch;
  int16_t yaw;
  uint8_t leve;
  int16_t temp;
} gy;

// 定义 UART 对象，这里使用 UART2
HardwareSerial mySerial(2);
byte add = 0xa4;
byte len = 0, start_reg = 0;
unsigned char Re_buf[35], counter = 0;
unsigned char sign = 0;
gy my_95Q;

// 计算校验和
byte calculateChecksum(unsigned char *data, int length)
{
  byte sum = 0;
  for (int i = 0; i < length; i++)
  {
    sum += data[i];
  }
  return sum & 0xFF;
}

// 发送写寄存器指令
void writeRegister(byte reg, byte value)
{
  unsigned char frame[5] = {add, 0x06, reg, value, 0};
  frame[4] = calculateChecksum(frame, 4);
  for (int i = 0; i < 5; i++)
  {
    mySerial.write(frame[i]);
  }
}

// 发送读寄存器指令
void readRegisters(byte startReg, byte numRegs)
{
  unsigned char frame[5] = {add, 0x03, startReg, numRegs, 0};
  frame[4] = calculateChecksum(frame, 4);
  for (int i = 0; i < 5; i++)
  {
    mySerial.write(frame[i]);
  }
}

// 计算相对于起始地址的偏移量
byte calculateOffset(byte startReg, byte targetReg)
{
  return targetReg - startReg;
}

void setup()
{
  Serial.begin(115200);
  mySerial.begin(115200, SERIAL_8N1, 10, 11); // rx tx

  // 设置输出模式为查询模式
  writeRegister(0x03, 1);
}

void loop()
{
  static unsigned long lastDataTime = millis();   // 记录上次接收到数据的时间
  static unsigned long lastPromptTime = millis(); // 记录上次显示提示信息的时间

  memset(&my_95Q, 0, sizeof(my_95Q)); // 初始化结构体

  // 可动态设置起始地址和寄存器数量->>>>>>>>> 要确保读取的数据包含需要的数据,否则会下标地址溢出
  byte newStartReg = 0x08; // 这里可以修改起始地址
  byte newNumRegs = 0x1B;  // 这里可以修改寄存器数量
  readRegisters(newStartReg, newNumRegs);

  if (mySerial.available() > 0)
  {
    lastDataTime = millis(); // 更新上次接收到数据的时间
    unsigned char i = 0, sum = 0;
    while (mySerial.available())
    {
      if (counter >= sizeof(Re_buf))
      {
        counter = 0;
        continue;
      }
      Re_buf[counter] = (unsigned char)mySerial.read();

      switch (counter)
      {
      case 0:
        if (Re_buf[0] != add)
        {
          counter = 0;
          continue;
        }
        break;
      case 1:
        if (Re_buf[1] != 0x03)
        {
          counter = 0;
          continue;
        }
        break;
      case 2:
        start_reg = Re_buf[2];
        break;
      case 3:
        len = Re_buf[3];
        break;
      default:
        if (len + 5 == counter)
        {
          sign = 1;
        }
        break;
      }

      if (sign)
      {
        sign = 0;
        for (i = 0; i < counter - 1; i++)
          sum += Re_buf[i];
        if (sum == Re_buf[counter - 1])
        {
          if (start_reg == newStartReg)
          {
            memset(&my_95Q, 0, sizeof(my_95Q)); // 初始化结构体

            // 分别解析欧拉角
            byte rollOffset = calculateOffset(newStartReg, 0x14);
            my_95Q.roll = (Re_buf[4 + rollOffset + 1] << 8) | Re_buf[4 + rollOffset];

            byte pitchOffset = calculateOffset(newStartReg, 0x16);
            my_95Q.pitch = (Re_buf[4 + pitchOffset + 1] << 8) | Re_buf[4 + pitchOffset];

            byte yawOffset = calculateOffset(newStartReg, 0x18);
            my_95Q.yaw = (Re_buf[4 + yawOffset + 1] << 8) | Re_buf[4 + yawOffset];

            // 解析 leve
            byte leveOffset = calculateOffset(newStartReg, 0x1A);
            my_95Q.leve = Re_buf[4 + leveOffset];

            // 解析温度数据
            byte tempLOffset = calculateOffset(newStartReg, 0x1B);
            byte tempHOffset = calculateOffset(newStartReg, 0x1C);
            my_95Q.temp = (Re_buf[4 + tempHOffset] << 8) | Re_buf[4 + tempLOffset];
          }
          Serial.print("roll:");
          Serial.print((float)my_95Q.roll / 100);
          Serial.print(",pitch:");
          Serial.print((float)my_95Q.pitch / 100);
          Serial.print(",yaw:");
          Serial.print((float)my_95Q.yaw / 100);
          Serial.print(",leve:");
          Serial.print(my_95Q.leve);
          Serial.print(",temp:");
          Serial.print((float)my_95Q.temp / 100);
          Serial.println();
        }
        else
        {
          // Serial.println("Checksum error!");
        }
        counter = 0;
      }
      else
        counter++;
    }
  }
  else
  {
    if (millis() - lastDataTime > 1000 && millis() - lastPromptTime > 1000)
    {
      Serial.println("No data received. Waiting for data...");
      lastPromptTime = millis(); // 更新上次显示提示信息的时间
    }
  }
  delay(10);
}

下面这个是iic连接的,这个是基于店家给的资料改的,毕竟iic的比较简单,温度读取没弄好,不想折腾了,反正我一般只用串口,因为店家的调试软件也只支持串口

#include <Wire.h>

#define uint16_t unsigned int
#define iic_add 0xa4 >> 1
typedef struct
{

  int16_t roll;
  int16_t pitch;
  int16_t yaw;
  uint8_t leve;
  int16_t temp;

} gy;
unsigned char Re_buf;
unsigned char sign = 0;
gy my_95Q;
uint16_t delay_t = 0;
byte ready_Ok = 0;
void setup()
{
  byte td = 1;
  Wire.begin(12, 13, 100000); // 设置SDA为11，SCL为10，波特率为100kHz
  Serial.begin(115200);
  delay(100);
  iic_read(0x02, &td, 1);
  switch (td)
  {
  case 0:
    delay_t = 100;
    break;
  case 1:
    delay_t = 20;
    break;
  case 2:
    delay_t = 10;
    break;
  case 3:
    delay_t = 5;
    break;
  }
  delay(delay_t);
  attachInterrupt(0, Exti, RISING);
}
void loop()
{
  unsigned char data[16] = {0};

  //   if(ready_Ok)
  {
    iic_read(0x14, data, 9);
    memcpy(&my_95Q, data, 9);
    Serial.print("roll: ");
    Serial.print(my_95Q.roll / 100);
    Serial.print(",pitch: ");
    Serial.print(my_95Q.pitch / 100);
    Serial.print(",yaw:");
    Serial.print(my_95Q.yaw / 100);
    Serial.print(",leve:");
    Serial.println((float)my_95Q.leve);
    ready_Ok = 0;
  }

  delay(delay_t);
}
void Exti()
{
  if (!ready_Ok)
    ready_Ok = 1; // 数据更新标志
}
void iic_read(unsigned char add, unsigned char *data, unsigned char len)
{
  byte j = 0;
  Wire.beginTransmission(iic_add);
  Wire.write(add);
  Wire.endTransmission(false);
  Wire.requestFrom(iic_add, (int)len);
  for (j = 0; j < len; j++)
    *data++ = Wire.read();
}
void iic_write(char add, unsigned char data)
{
  Wire.beginTransmission(iic_add);
  Wire.write((uint8_t)add);
  Wire.write((uint8_t)data);
  Wire.endTransmission();
}

我已经踩了好多坑了,不希望还有小伙伴再踩同样的坑