Собсно сам код. При компилировании постоянно возникает какая либо ошибка. Чаще всего ругань идет на mpu9250
// ФРАГМЕНТ 1: Подключение библиотек
// ================================
#include <eigen.h>
#include <Wire.h>
#include <mpu9250.h>
#include <DFRobot_BMP280.h>
//#include <Adafruit_BMP280.h>
#include <ESP32Servo.h>
#include <ESP32PWM.h>
// ================================
// ФРАГМЕНТ 2: Определение констант
// ================================
const int mpu9250_ADDRESS = 0x68; // Адрес MPU9250 на шине I2C
const int BMP280_ADDRESS = 0x76; // Адрес BMP280 на шине I2C
// Определение пинов ESC
const int ESC_PIN_1 = 2;
const int ESC_PIN_2 = 3;
const int ESC_PIN_3 = 4;
const int ESC_PIN_4 = 5;
// ================================
// ФРАГМЕНТ 3: Функция инициализации
// ================================
void setup() {
Serial.begin(9600);
// Инициализация I2C
Wire.begin();
// Инициализация MPU9250 и BMP280
mpu9250.begin();
bmp280.begin();
// Инициализация ESC
esc1.attach(ESC_PIN_1);
esc2.attach(ESC_PIN_2);
esc3.attach(ESC_PIN_3);
esc4.attach(ESC_PIN_4);
// Вызов функции калибровки
calibrateSensors();
}
// ================================
// ФРАГМЕНТ 4: Функция калибровки
// ================================
void calibrateSensors() {
// Калибровка акселерометра и гироскопа MPU9250
Serial.println("Начинаю калибровку MPU9250. Пожалуйста, не двигайте квадрокоптер.");
delay(2000); // Пауза для установки
// Собираем данные для калибровки
int calibrationSamples = 1000;
Vector3<float> accelBias(0, 0, 0);
Vector3<float> gyroBias(0, 0, 0);
for (int i = 0; i < calibrationSamples; ++i) {
mpu9250.readSensor();
accelBias += mpu9250.getAccelVector();
gyroBias += mpu9250.getGyroVector();
delay(5);
}
// Вычисляем средние значения
accelBias /= calibrationSamples;
gyroBias /= calibrationSamples;
// Сохраняем смещения
mpu9250.setAccelBias(accelBias);
mpu9250.setGyroBias(gyroBias);
Serial.println("Калибровка MPU9250 завершена.");
// Калибровка барометра BMP280
Serial.println("Начинаю калибровку BMP280. Пожалуйста, не двигайте квадрокоптер.");
delay(2000); // Пауза для установки
// Собираем данные для калибровки
double pressureSum = 0.0;
double altitudeSum = 0.0;
for (int i = 0; i < calibrationSamples; ++i) {
bmp280.update();
pressureSum += bmp280.getPressure();
altitudeSum += bmp280.getAltitude();
delay(5);
}
// Вычисляем средние значения
double pressureOffset = pressureSum / calibrationSamples;
double altitudeOffset = altitudeSum / calibrationSamples;
// Сохраняем смещения
bmp280.setPressureOffset(pressureOffset);
bmp280.setAltitudeOffset(altitudeOffset);
Serial.println("Калибровка BMP280 завершена.");
}
// ================================
// ФРАГМЕНТ 5: Основной цикл
// ================================
void loop() {
processReceiverData(); // Обработка данных от приемника
readSensors(); // Чтение данных с датчиков
// Обработка данных от приемника
float throttle = map(receiverData.throttle, 1000, 2000, 0, 100); // Маппинг значений газа
float roll = map(receiverData.roll, 1000, 2000, -50, 50); // Маппинг значений крена
float pitch = map(receiverData.pitch, 1000, 2000, -50, 50); // Маппинг значений тангажа
float yaw = map(receiverData.yaw, 1000, 2000, -50, 50); // Маппинг значений рысканья
// Простейший PID-контроллер для стабилизации
float pidRoll = pidController(roll, mpu9250.getGyroX_rads(), 1, 0, 0);
float pidPitch = pidController(pitch, mpu9250.getGyroY_rads(), 1, 0, 0);
float pidYaw = pidController(yaw, mpu9250.getGyroZ_rads(), 1, 0, 0);
// Преобразование PID-выходов в управляющие сигналы
throttle += pidRoll;
throttle += pidPitch;
yaw += pidYaw;
// Управление ESC
controlESC(throttle, roll, pitch, yaw);
// Вывод данных в Serial Monitor
printData();
delay(20); // Задержка для стабилизации цикла
//finishMainLoop(); // Вызов завершающих действий Действия и их код смотри в ФРАГМЕНТ 9:
}
// ================================
// ФРАГМЕНТ 6: Функция чтения данных с датчиков
// ================================
void readSensors() {
mpu9250.readSensor();
bmp280.update();
// Получение угловой ориентации
float roll = atan2(mpu9250.getAccelY_mss(), mpu9250.getAccelZ_mss()) * RAD_TO_DEG;
float pitch = atan2(-mpu9250.getAccelX_mss(), sqrt(pow(mpu9250.getAccelY_mss(), 2) + pow(mpu9250.getAccelZ_mss(), 2))) * RAD_TO_DEG;
// Получение данных о высоте
float altitude = bmp280.getAltitude();
// TODO: Добавить дополнительный код для обработки данных, если необходимо
// Вывод данных в Serial Monitor (для отладки)
Serial.print("Roll: "); Serial.print(roll); Serial.print(" ");
Serial.print("Pitch: "); Serial.print(pitch); Serial.print(" ");
Serial.print("Altitude: "); Serial.println(altitude);
}
// ================================
// ФРАГМЕНТ 7: Функция управления ESC
// ================================
void controlESC(float throttle, float roll, float pitch, float yaw) {
// Преобразование управляющих сигналов в скорость вращения моторов
float motor1 = throttle - pitch + roll - yaw;
float motor2 = throttle + pitch + roll + yaw;
float motor3 = throttle + pitch - roll - yaw;
float motor4 = throttle - pitch - roll + yaw;
// Использование PID-контроллера для стабилизации (простой пример)
float pidRoll = pidController(0, mpu9250.getGyroX_rads(), 1, 0, 0);
float pidPitch = pidController(0, mpu9250.getGyroY_rads(), 1, 0, 0);
float pidYaw = pidController(0, mpu9250.getGyroZ_rads(), 1, 0, 0);
motor1 += pidPitch;
motor2 += pidPitch;
motor3 -= pidPitch;
motor4 -= pidPitch;
motor1 += pidRoll;
motor2 -= pidRoll;
motor3 -= pidRoll;
motor4 += pidRoll;
motor1 += pidYaw;
motor2 -= pidYaw;
motor3 += pidYaw;
motor4 -= pidYaw;
// Преобразование скорости вращения моторов в PWM-сигналы для управления ESC
esc1.writeMicroseconds(map(constrain(motor1, 0, 100), 0, 100, 1000, 2000));
esc2.writeMicroseconds(map(constrain(motor2, 0, 100), 0, 100, 1000, 2000));
esc3.writeMicroseconds(map(constrain(motor3, 0, 100), 0, 100, 1000, 2000));
esc4.writeMicroseconds(map(constrain(motor4, 0, 100), 0, 100, 1000, 2000));
}
// ================================
// ФРАГМЕНТ 8: Функция вывода данных в Serial Monitor
// ================================
void printData() {
// Вывод значений с датчиков
Serial.print("Acc: ");
Serial.print(mpu9250.getAccelX_mss()); Serial.print(" ");
Serial.print(mpu9250.getAccelY_mss()); Serial.print(" ");
Serial.print(mpu9250.getAccelZ_mss()); Serial.print(" | ");
Serial.print("Gyro: ");
Serial.print(mpu9250.getGyroX_rads()); Serial.print(" ");
Serial.print(mpu9250.getGyroY_rads()); Serial.print(" ");
Serial.print(mpu9250.getGyroZ_rads()); Serial.print(" | ");
Serial.print("Mag: ");
Serial.print(mpu9250.getMagX_uT()); Serial.print(" ");
Serial.print(mpu9250.getMagY_uT()); Serial.print(" ");
Serial.print(mpu9250.getMagZ_uT()); Serial.print(" | ");
Serial.print("Baro: ");
Serial.print(bmp280.getPressure()); Serial.print(" ");
Serial.print(bmp280.getAltitude()); Serial.print(" ");
Serial.println(bmp280.getTemperature());
}
// ================================
// ФРАГМЕНТ 9: Завершение основного цикла Раскомментировать //finishMainLoop() ФРАГМЕНТ 5:
// ================================
//void finishMainLoop() {
// TODO: Добавить код для завершения основного цикла
// Здесь может быть код для проверки условий, отправки данных, сохранения данных и т.д.
//delay(20); // Задержка для стабилизации цикла
//}