Pitch, roll, yaw - Акселерометр, непонятно

Тема в разделе "Arduino & Shields", создана пользователем kras, 1 фев 2017.

  1. kras

    kras Нуб

    Код (C++):
    #include <Wire.h>
    #include <MPU6050.h>

    MPU6050 mpu;

    // Timers
    unsigned long timer = 0;
    float timeStep = 0.01;

    // Pitch, Roll and Yaw values
    float pitch = 0;
    float roll = 0;
    float yaw = 0;

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

      // Initialize MPU6050
      while(!mpu.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_2G))
      {
        Serial.println("Could not find a valid MPU6050 sensor, check wiring!");
        delay(500);
      }

      // Calibrate gyroscope. The calibration must be at rest.
      // If you don't want calibrate, comment this line.
      mpu.calibrateGyro();

      // Set threshold sensivty. Default 3.
      // If you don't want use threshold, comment this line or set 0.
      mpu.setThreshold(3);
    }

    void loop()
    {
      timer = millis();

      // Read normalized values
      Vector norm = mpu.readNormalizeGyro();

      // Calculate Pitch, Roll and Yaw
      pitch = pitch + norm.YAxis * timeStep;
      roll = roll + norm.XAxis * timeStep;
      yaw = yaw + norm.ZAxis * timeStep;

      // Output raw
      Serial.print(" Pitch = ");
      Serial.print(pitch);
      Serial.print(" Roll = ");
      Serial.print(roll);
      Serial.print(" Yaw = ");
      Serial.println(yaw);

      // Wait to full timeStep period
      delay((timeStep*1000) - (millis() - timer));
    }

    Вот так вот работает расчет углов поворота объекта с помощью акселерометра. Проблема в том, что измеряет вроде правильно. Но если у меня после объявления таймера стоит еще куча кода, а delay мне надо заменить тем же millis, то получается, что угол получается неправильным... Проблема в том, что norm.ZAxis выдает ускорение. А уже ускорение по времени связано с углом. Или не ускорение выдает, а какой-то относительный угол. Помогите пожалуйста разобраться подробнее, как работает код? Как проще заменить delay и сделать этот цикл с другими таймерами, кодами и тп....?

    Подсказка - вообще есть код, который считает питч и ролл.

    Код (C++):

      // Calculate Pitch & Roll
      int pitch = -(atan2(normAccel.XAxis, sqrt(normAccel.YAxis*normAccel.YAxis + normAccel.ZAxis*normAccel.ZAxis))*180.0)/M_PI;
      int roll = (atan2(normAccel.YAxis, normAccel.ZAxis)*180.0)/M_PI;

    Считает хорошо, впринципе. Но мне нужен Yaw, так как иначе надо акселерометр переворачивать. Как посчитать Yaw таким же образом?
     
  2. Unixon

    Unixon Оракул Модератор

    Никак, вам нужен компас.
     
  3. sslobodyan

    sslobodyan Гик

    1. Если абсолютный угол по yaw не важен, то можно и без компаса. Получим относительный угол с нулем в момент старта (куда смотрели при подаче напряжения).
    2. Почему-то все множат на TimeStep и делают искусственную задержку для того, что бы алгоритм накапливал значения с точно этим интервалом. Я когда то делал наоборот - высчитывал сколько времени прошло с прошлого измерения и без задержек, то есть каждый раз получал другой TimeStep, Работало.
    3. Надо учитывать, что гироскоп и акселерометр имеют большие погрешности, которые надо компенсировать алгоритмически. И только магнитометр+акселерометр+гироскоп дадут более-менее точность.
     
  4. Unixon

    Unixon Оракул Модератор

    Как вы с помощью акселерометра будете вычислять угол поворота вокруг вектора g? Только интегрировать показания гироскопа, но насколько это будет точно - большой вопрос. При малых угловых ускорениях будет накапливаться ошибка.
     
  5. sslobodyan

    sslobodyan Гик

    У ТС mpu6050 - там есть гироскоп. Акселерометр, естественно, здесь не поможет.