[Esp-32][C++][Arduino IDE] + MPU6050. Zamiast YAW, funkcja zwraca kąt eulera ψ.

[Scislov]

Witam. Buduję samolot RC i do jego działania, potrzebne mi są jego kąty yaw pitch i roll. Wyliczam je przy użyciu biblioteki MPU6050_6Axis_MotionApps20. W przypadku pitch i roll, wszystko wylicza poprawnie. Jednak w przypadku yaw, zamiast zwracać kursu, funkcja zwraca kąt eulera ψ. Nie wiem z jakiego powodu może się tak dziać, może w przypadku esp-32 coś działa niepoprawnie. Próbowałem również obliczyć samemu yaw z kątów eulera, jednak nie udało mi się wyprowadzić wzoru. Jest to liczone chyba z kwaternionów, jednak się na nich nie znam. Tak na prawdę, jeżeli nie wiadomo z jakiego powodu jest ten problem, można by obliczyć yaw w głównej pętli programu, z ww. kątów eulera lub kwaternionów, które również można otrzymać z MPU6050, jednak nie potrafię tego zrobić.

#define __PGMSPACE_H_ 1
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"

MPU6050 mpu;



unsigned long mpu_time = 0;
uint8_t mpuIntStatus, devStatus, fifoBuffer[64], mpu_err = 0;
uint16_t packetSize, fifoCount;
float ypr[3], acc_xyz[3], acc_xyz_real[3], grav[3], euler[3];
double  vel_ypr[3];
int32_t acc_xyz_world[3], gyro[3];
volatile int mpuDataCounter = 0;
int mpuDataCounterPrev = 0;

Quaternion q;
VectorInt16 aa;
VectorInt16 aaReal;
VectorInt16 aaWorld;
VectorFloat gravity;

void mpu_init() {
  Wire.begin();
  Wire.setClock(400000);
  mpu.initialize();
  mpu.reset();
  mpu.resetI2CMaster();
  mpu.initialize();
  if (!mpu.testConnection()) {
    mpu_err = 2;
  }
  devStatus = mpu.dmpInitialize();
  if (devStatus == 0) {
    mpu.setDMPEnabled(true);
    mpuIntStatus = mpu.getIntStatus();
    packetSize = mpu.dmpGetFIFOPacketSize();
  } else {
    // ERROR!
    // 1 = initial memory load failed
    // 2 = DMP configuration updates failed
    mpu_err = 2;
  }
  mpu.setXGyroOffset(105);
  mpu.setYGyroOffset(10);
  mpu.setZGyroOffset(-21);
  mpu.setXAccelOffset(-704);
  mpu.setYAccelOffset(1441);
  mpu.setZAccelOffset(1342);
  //mpu.CalibrateAccel(6);
  //mpu.CalibrateGyro(6);
  //mpu.PrintActiveOffsets();
}

void mpu_get() {
  if (mpuDataCounter != mpuDataCounterPrev) {
    mpuDataCounterPrev = mpuDataCounter;
  }
  mpuIntStatus = mpu.getIntStatus();
  fifoCount = mpu.getFIFOCount();
  if ((mpuIntStatus & 0x10) || fifoCount >= 1024) {
    mpu.resetFIFO();
    if (mpu_err != 2) {
      mpu_err = 1;
    }
  } else {
    if (mpuIntStatus & 0x02) {
      mpu_time = millis();
      while (fifoCount < packetSize) {
        fifoCount = mpu.getFIFOCount();
        if (millis() - mpu_time > 10) {
          mpu_err = 2;
          break;
        }
      }
      mpu.getFIFOBytes(fifoBuffer, packetSize);
      fifoCount -= packetSize;
      mpu.dmpGetQuaternion(&q, fifoBuffer);
      mpu.dmpGetGyro(gyro, fifoBuffer);
      vel_ypr[0] = float(gyro[2]) / 65536;
      vel_ypr[1] = float(gyro[1]) / 65536;
      vel_ypr[2] = float(gyro[0]) / 65536;
      mpu.dmpGetEuler(euler, &q);
      euler[0] = euler[0] * 180 / M_PI;
      euler[1] = euler[1] * 180 / M_PI;
      euler[2] = euler[2] * 180 / M_PI;
      mpu.dmpGetGravity(&gravity, &q);
      grav[0] = gravity.x;
      grav[1] = gravity.y;
      grav[2] = gravity.z;
      mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
      ypr[0] = ypr[0] * 180 / M_PI;
      ypr[1] = ypr[1] * 180 / M_PI;
      ypr[2] = ypr[2] * 180 / M_PI;
      if (ypr[1] > 90) {
        ypr[1] = 90 - (ypr[1] - 90);
      }
      if (ypr[1] < -90) {
        ypr[1] = -90 + (-90 - ypr[1]);
      }
      mpu.dmpGetAccel(&aa, fifoBuffer);
      acc_xyz[0] = float(aa.x) * 0.0011975;
      acc_xyz[1] = float(aa.y) * 0.0011975;
      acc_xyz[2] = float(aa.z) * 0.0011975;
      mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
      acc_xyz_real[0] = aaReal.x;
      acc_xyz_real[1] = aaReal.y;
      acc_xyz_real[2] = aaReal.z;
      mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
      acc_xyz_world[0] = aaWorld.x;
      acc_xyz_world[1] = aaWorld.y;
      acc_xyz_world[2] = aaWorld.z;
    }
  }
}

 

Edytowano Listopad 16, 2021 przez Scislov