Гироскоп MPU-6050 нестабильно работает

Доброго дня!
проект- стабилизация площадки по одной оси.

В принципе 2 вопроса,

  1. почему в терминале пишет, что гира не инициализирована, хотя работает?
    22:56:55.977 → Калибровка гироскопа… Не двигайте датчик!
    22:57:04.043 → Смещения: -473.00, 245.00, 8.00
    22:57:04.075 → MPU6050 ошибка
  2. плата лежит без движений но показания расчетные уезжают…

22:57:04.299 → -----Roll: -0.38 rollOutput: 36.33

22:57:30.819 → -----Roll: -2.82 rollOutput: 40.40

#include <Wire.h>
#include <Servo.h>
#include <MPU6050.h>

MPU6050 mpu;
Servo servoRoll;

// Пин для сервопривода крена
#define SERVO_ROLL_PIN 9

// Параметры PID для крена
typedef struct {
  double Kp, Ki, Kd;
  double integral, prevError;
} PID;

PID rollPID = {1.3, // определяет, насколько сильно сервопривод будет реагировать на текущую ошибку. Уменьшение Kp сделает реакцию менее агрессивной.
              0.05, //отвечает за устранение статической ошибки, но слишком большой Ki может привести к колебаниям. Уменьшение Ki сделает реакцию на медленные изменения менее агрессивной.
              0.3, //пытается предсказать будущее поведение ошибки и замедлить движение сервопривода при приближении к целевой точке. Увеличение Kd поможет сгладить движение и уменьшить колебания.
              0, 0};  // Настройте эти значения! {1.5, 0.05, 0.3, 0, 0};

// Фильтрация для крена
float compFilterCoeff = 0.98;  // Коэффициент комплементарного фильтра
float dt = 0.02;              // Время между измерениями (20 мс)
float rollAngle = 0;

/*
Фильтрация данных: Комплементарный фильтр уже используется, но можно попробовать еще больше его настроить, чтобы отфильтровывать шум.
*/
// Калибровка
#define CALIBRATION_SAMPLES 500
float gyroXoffset = 0, gyroYoffset = 0, gyroZoffset = 0;

void setup() {
  Serial.begin(19200);

  // Инициализация сервопривода крена
  servoRoll.attach(SERVO_ROLL_PIN);
  centerServos();

  // Инициализация MPU6050
  Wire.begin();
  mpu.initialize();
  delay(500);

  // Калибровка гироскопа
  calibrateGyro();

  // Проверка соединения
  Serial.println(mpu.testConnection() ? "MPU6050 подключен" : "MPU6050 ошибка");
}

void loop() {
  static uint32_t prevTime = millis();

  // Чтение данных с датчика
  int16_t ax, ay, az, gx, gy, gz;
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  // Учет калибровочных смещений
  gx -= gyroXoffset;
  gy -= gyroYoffset;
  gz -= gyroZoffset;
  // Расчет угла крена по акселерометру
  float accelRoll = atan2(ay, az) * 180 / PI;

  // Расчет угла крена по гироскопу (интегрирование)
  float gyroRollRate = gx / 131;  

  rollAngle = compFilterCoeff * (rollAngle + gyroRollRate * dt) + (1 - compFilterCoeff) * accelRoll;

  // ПИД-регуляция для крена
  float rollOutput = computePID(&rollPID, 0 - rollAngle);  // Цель - 0 градусов

  rollOutput = 35 + constrain(rollOutput, -35, 35);
  // Управление сервоприводом крена
  servoRoll.write(rollOutput);  // Ограничение ±60 градусов

  // Отладка  
  Serial.print("-----Roll: ");
  Serial.print(rollAngle);
  Serial.print(" rollOutput: ");
  Serial.println(rollOutput);
  

  // Стабилизация частоты цикла
  while (millis() - prevTime < dt * 1000) {}
  prevTime = millis();
}

// ПИД-регулятор
float computePID(PID* pid, float error) {
  pid->integral += error * dt;
  pid->integral = constrain(pid->integral, -100, 100);  // Антивиндруп

  float derivative = (error - pid->prevError) / dt;
  pid->prevError = error;

  return pid->Kp * error + pid->Ki * pid->integral + pid->Kd * derivative;
}

// Калибровка гироскопа
void calibrateGyro() {
  Serial.println("Калибровка гироскопа... Не двигайте датчик!");
  delay(1000);

  int16_t gx, gy, gz;
  long gxSum = 0, gySum = 0, gzSum = 0;

  for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
    mpu.getRotation(&gx, &gy, &gz);
    gxSum += gx;
    gySum += gy;
    gzSum += gz;
    delay(5);
  }

  gyroXoffset = gxSum / CALIBRATION_SAMPLES;
  gyroYoffset = gySum / CALIBRATION_SAMPLES;
  gyroZoffset = gzSum / CALIBRATION_SAMPLES;

  Serial.print("Смещения: ");
  Serial.print(gyroXoffset);
  Serial.print(", ");
  Serial.print(gyroYoffset);
  Serial.print(", ");
  Serial.println(gyroZoffset);
}

// Установка серв в центральное положение
void centerServos() {
  Serial.println(" Установка сервы в среднее положение!!");
  servoRoll.write(35);
  delay(1000);
}

Потому что функция mpu.testConnection() в 50 строке вернула ноль.

Вроде должно вернуться True если подключен, но возвращает False, а при этом датчик продолжает далее работать… значения то показывает. вот что не понятно.

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

Не должен. Он его вернёт если :

  1. Есть контакт.
  2. Датчик рабочий.
  3. Питание в норме.
  4. Нет геомагнитной бури, вспышек на солнце и парада планет
1 лайк