ASSISTANT
Чтобы учитывать смещение гироскопа и акселерометра и проводить калибровку, нам нужно получить значения смещения в состоянии покоя и учесть их при расчёте углов. Приведу обновлённую версию программы с добавлением этапа калибровки.
```cpp
#include <Wire.h>
#include <MPU6050.h>
#include <QMC5883LCompass.h>
#include <Servo.h>
// Инициализация объектов
MPU6050 mpu;
QMC5883LCompass compass;
Servo servoRoll; // Серво для компенсации крена
Servo servoYaw; // Серво для компенсации рысканья
// Переменные для хранения данных
float pitch, roll, yaw;
float gyroBiasX, gyroBiasY, gyroBiasZ;
float accelBiasX, accelBiasY, accelBiasZ;
void setup() {
Wire.begin();
Serial.begin(9600);
// Инициализация MPU6050
mpu.initialize();
if (!mpu.testConnection()) {
Serial.println("MPU6050 connection failed");
while(1);
}
// Инициализация QMC5883L (калибровка может быть необходима)
compass.init();
// Настройка сервоприводов
servoRoll.attach(9); // Пин 9 для Roll
servoYaw.attach(10); // Пин 10 для Yaw
// Калибровка гироскопа и акселерометра
calibrateSensors();
}
void loop() {
// Считываем данные с MPU6050
VectorFloat gravity;
Quaternion q;
mpu.getQuaternion(&q, nullptr);
VectorInt16 rawGyro, rawAccel;
mpu.getMotion6(&rawAccel.x, &rawAccel.y, &rawAccel.z, &rawGyro.x, &rawGyro.y, &rawGyro.z);
// Компас для корректировки угла рысканья
int x, y, z;
compass.read();
x = compass.getX();
y = compass.getY();
z = compass.getZ();
float heading = atan2(y, x);
// Применение смещения
rawGyro.x -= gyroBiasX;
rawGyro.y -= gyroBiasY;
rawGyro.z -= gyroBiasZ;
rawAccel.x -= accelBiasX;
rawAccel.y -= accelBiasY;
rawAccel.z -= accelBiasZ;
// Преобразование кватернионов в углы Эйлера
pitch = asin(2*(q.w*q.y - q.z*q.x));
roll = atan2(2*(q.w*q.x + q.y*q.z), 1 - 2*(q.x*q.x + q.y*q.y));
yaw = heading;
// Преобразование углов из радиан в градусы
pitch *= 180.0 / PI;
roll *= 180.0 / PI;
yaw *= 180.0 / PI;
// Формирование сигнала для сервоприводов
int servoRollAngle = map(roll, -90, 90, 0, 180);
int servoYawAngle = map(yaw, -180, 180, 0, 180);
servoRoll.write(servoRollAngle);
servoYaw.write(servoYawAngle);
// Задержка для стабилизации
delay(100);
}
// Функция калибровки датчиков
void calibrateSensors() {
VectorInt16 rawGyro, rawAccel;
long gyroSumX = 0, gyroSumY = 0, gyroSumZ = 0;
long accelSumX = 0, accelSumY = 0, accelSumZ = 0;
const int samples = 1000;
Serial.println("Калибровка начата...");
for (int i = 0; i < samples; i++) {
mpu.getMotion6(&rawAccel.x, &rawAccel.y, &rawAccel.z, &rawGyro.x, &rawGyro.y, &rawGyro.z);
gyroSumX += rawGyro.x;
gyroSumY += rawGyro.y;
gyroSumZ += rawGyro.z;
accelSumX += rawAccel.x;
accelSumY += rawAccel.y;
accelSumZ += rawAccel.z;
delay(5);
}
gyroBiasX = gyroSumX / samples;
gyroBiasY = gyroSumY / samples;
gyroBiasZ = gyroSumZ / samples;
accelBiasX = accelSumX / samples;
accelBiasY = accelSumY / samples;
accelBiasZ = accelSumZ / samples;
Serial.println("Калибровка завершена.");
}
```
### Объяснение изменений:
1. **Калибровка гироскопа и акселерометра:** В начале работы устройства калибруются значения смещения. Важно, чтобы сенсоры были неподвижными во время этой процедуры.
2. **Использование смещений:** Перед использованием значений гироскопа и акселерометра, мы вычитаем рассчитанные смещения из получаемых данных, чтобы уменьшить дрейф.
3. **Компаса:** Может потребоваться калибровка и правильная установка магнитометра на объекте для корректного измерения курсового угла.
Эти шаги помогают улучшить точность показаний за счет компенсации систематических ошибок и смещения нуля, возникающих в процессе измерений.