Трудности получения значений угла от MPU6050.

Я пытаюсь получить значения углов от датчика MPU6050 IMU с помощью < a href="https://docs.espressif.com/projects/esp-idf/en/stable/esp32/hw-reference/esp32/user-guide-devkitm-1.html ">ESP32-DevKitM-1 микроконтроллер.

Моя установка содержит плату ESP32, плату MPU6050 и устройство чтения SD-карт для сохранения показаний датчика:

Я использую Arduino IDE v2.3.0.

Я адаптировал следующий отрывок кода тестирования из другой публикации о датчике, используя новая библиотека Adafruit_MPU6050 и функции SD:

#include <Wire.h>
#include "SPI.h"

#include <Adafruit_Sensor.h>
#include <Adafruit_MPU6050.h>

#include "SD.h"

const int PIN_IMU_SCL = 22;
const int PIN_IMU_SDA = 21;
const int PIN_SD_CS = 23;
const int PIN_SD_MISO = 10;
const int PIN_SD_MOSI = 18;
const int PIN_SD_CLK = 5;

const int IMU_GyroScale = 131;

Adafruit_MPU6050 IMU;
double IMU_Time;
double IMU_TimePrev = -1;
double IMU_GyroRotX;
double IMU_GyroRotY;
double IMU_GyroRotZ;

File DataFile;

void IMU_GetData(double& angleX, double& angleY, double& angleZ, double& temp)
{
  bool firstTime = false;

  if (IMU_TimePrev == -1)
    firstTime = true;

  // Calculating the elapsed time...
  IMU_TimePrev = IMU_Time;
  IMU_Time = millis();
  double timeDelta = (IMU_Time - IMU_TimePrev) / 1000;

  // Acquiring the readings from the accelerometer & gyroscope...
  sensors_event_t accelData;
  sensors_event_t gyroData;
  sensors_event_t tempData;
  IMU.getEvent(&accelData, &gyroData, &tempData);

  // Calculating the accelerometer angle...
  double accelRotX = (180 / PI) * atan(accelData.acceleration.x / sqrt(pow(accelData.acceleration.y, 2) + pow(accelData.acceleration.z, 2))); 
  double accelRotY = (180 / PI) * atan(accelData.acceleration.y / sqrt(pow(accelData.acceleration.x, 2) + pow(accelData.acceleration.z, 2)));
  double accelRotZ = (180 / PI) * atan(sqrt(pow(accelData.acceleration.x, 2) + pow(accelData.acceleration.y, 2)) / accelData.acceleration.z);

  // Calculating the gyroscope angle...
  double gyroRotX = (gyroData.gyro.x / IMU_GyroScale);
  double gyroRotY = (gyroData.gyro.y / IMU_GyroScale);
  double gyroRotZ = (gyroData.gyro.z / IMU_GyroScale);

  // Combining the two (complementary filter)...
  if (firstTime)
  {
    IMU_GyroRotX = accelRotX;
    IMU_GyroRotY = accelRotY;
    IMU_GyroRotZ = accelRotZ;
  }
  else
  {
    IMU_GyroRotX += timeDelta * gyroRotX;
    IMU_GyroRotY += timeDelta * gyroRotY;
    IMU_GyroRotZ += timeDelta * gyroRotZ;
  }

  angleX = (0.96 * accelRotX) + (0.04 * IMU_GyroRotX);
  angleY = (0.96 * accelRotY) + (0.04 * IMU_GyroRotY);
  angleZ = (0.96 * accelRotZ) + (0.04 * IMU_GyroRotZ);

  temp = tempData.temperature;
}

bool IMU_Init()
{
  bool status = IMU.begin();

  if (status)
  {
    IMU.setAccelerometerRange(MPU6050_RANGE_8_G);
    IMU.setGyroRange(MPU6050_RANGE_500_DEG);
    IMU.setFilterBandwidth(MPU6050_BAND_21_HZ);
  }

  IMU_Time = millis();

  delay(100);

  return status;
}

void setup() 
{
  Serial.begin(9600);
  Serial.println("Initializing...");

  Wire.begin(PIN_IMU_SDA, PIN_IMU_SCL);
  IMU_Init();

  SPI.begin(PIN_SD_CLK, PIN_SD_MISO, PIN_SD_MOSI, PIN_SD_CS);

  while (!SD.begin(PIN_SD_CS))
  {
    Serial.println("Unable to open SD!");
    delay(500);
  }

  DataFile = SD.open("/angles.txt", FILE_WRITE);
  DataFile.close();

  delay(100);
  Serial.println("Initialized!");
}

void loop() 
{
  double angleX;
  double angleY;
  double angleZ;
  double temp;
  IMU_GetData(angleX, angleY, angleZ, temp);
  String anglesStr = String(angleX) + ":" + String(angleY) + ":" + String(angleZ);

  DataFile = SD.open("/angles.txt", FILE_APPEND);
  DataFile.println(anglesStr);
  DataFile.close();

  Serial.println(anglesStr);

  delay(200);
}

Где IMU_GetData — это функция, отвечающая за получение данных об угле от гироскопа и датчиков акселератора на плате MPU6050, а также реализацию фильтра для получения набора отфильтрованных значений угла.

Затем я просто сохраняю отфильтрованные значения углов на SD-карту. С датчиком я общаюсь с помощью библиотеки Wire I2C, а с SD-картой — с помощью библиотеки SPI.

Основная проблема, с которой я столкнулся, заключается в том, что результирующие значения углов не точно отражают вращение датчика.

В качестве теста я медленно вращал датчик на 360° вокруг каждой из трех осей и сохранял непрерывные показания угла во время каждого вращения:

Временной интервал между каждым показанием на графиках выше составляет ~0,2 с.

Первая проблема заключается в том, что вращение вокруг оси Z, видимое на последнем графике выше, не вызывает каких-либо заметных изменений ни в одном из показаний угла.

Изначально я предполагал, что это может быть вызвано значением константы IMU_GyroScale, на которую умножаются значения углов гироскопа во время вычислений, выполняемых в функции IMU_GyroScale. Я предполагал, что все чипы MPU6050 будут использовать один и тот же коэффициент масштабирования, но затем я попробовал несколько других коэффициентов масштабирования, перечисленных в таблице данных, без заметной разницы.

Есть ли в моем коде что-то явно неправильное при расчете угла оси Z? Может ли это быть признаком неисправности сенсорной платы?

Второе, что меня заинтересовало, — это формат отфильтрованных значений угла.

Изначально я предполагал, что отфильтрованные значения углов, которые я получу от датчика, будут углами Эйлера; Roll, Pitch & Ура. Однако, похоже, это не так.

На самом деле, один из основных источников, перечисленных в исходной публикации, который подробно описывает процесс расчета отфильтрованных значений угла, а также обеспечивает визуализацию значений угла, полученных от датчика:

Это «наклон»? значения, связанные с углами Эйлера? Можно ли конвертировать между ними?