Главная » Форумы » Программирование

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

Нет ответов
Ср, 01/02/2017 - 05:18
lennen
Offline
Зарегистрирован: 10.11.2015 
#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 и сделать этот цикл с другими таймерами, кодами и тп....? Подсказка - вообще есть код, который считает питч и ролл.

 

Код:
  // 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 таким же образом?

Верх