Код (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 таким же образом?
1. Если абсолютный угол по yaw не важен, то можно и без компаса. Получим относительный угол с нулем в момент старта (куда смотрели при подаче напряжения). 2. Почему-то все множат на TimeStep и делают искусственную задержку для того, что бы алгоритм накапливал значения с точно этим интервалом. Я когда то делал наоборот - высчитывал сколько времени прошло с прошлого измерения и без задержек, то есть каждый раз получал другой TimeStep, Работало. 3. Надо учитывать, что гироскоп и акселерометр имеют большие погрешности, которые надо компенсировать алгоритмически. И только магнитометр+акселерометр+гироскоп дадут более-менее точность.
Как вы с помощью акселерометра будете вычислять угол поворота вокруг вектора g? Только интегрировать показания гироскопа, но насколько это будет точно - большой вопрос. При малых угловых ускорениях будет накапливаться ошибка.