// Тестирование качества питания
void testPowerQuality() {
  int samples = 1000;
  float sum = 0, sumSquares = 0;
  
  for(int i = 0; i < samples; i++) {
    float voltage = analogRead(A0) * (3.3/4095.0);
    sum += voltage;
    sumSquares += voltage * voltage;
    delayMicroseconds(10);  // 100kHz sampling
  }
  
  float mean = sum / samples;
  float variance = (sumSquares / samples) - (mean * mean);
  float ripple = sqrt(variance);
  
  Serial.printf("Среднее напряжение: %.3fV\n", mean);
  Serial.printf("Пульсации: %.1fmV RMS\n", ripple * 1000);
  Serial.printf("Качество питания: %s\n", 
    ripple < 0.01 ? "ОТЛИЧНО" : 
    ripple < 0.05 ? "ХОРОШО" : "ТРЕБУЕТ ФИЛЬТРАЦИИ");
}

// =====================================
// СИСТЕМА ДИАГНОСТИКИ ESP32 ДРОНА
// =====================================

#include <WiFi.h>
#include <WebServer.h>
#include <ArduinoJson.h>
#include <SPIFFS.h>

class DroneDebugger {
private:
  WebServer server;
  unsigned long lastUpdate;
  
  struct SystemMetrics {
    float cpuUsage;
    float memoryUsage;
    float coreTemperature;
    int wifiSignal;
    float batteryVoltage;
    float powerConsumption;
  } metrics;
  
  struct SensorHealth {
    bool imuStatus;
    bool gpsStatus;
    bool baroStatus;
    float imuTemp;
    int gpsSatellites;
    float baroPressure;
  } sensors;

public:
  DroneDebugger() : server(80) {}
  
  void begin() {
    SPIFFS.begin();
    setupWebServer();
    server.begin();
    Serial.println("Debug server started on port 80");
  }
  
  void update() {
    server.handleClient();
    
    if (millis() - lastUpdate > 100) {  // 10Hz update
      updateMetrics();
      checkSensorHealth();
      lastUpdate = millis();
    }
  }
  
private:
  void setupWebServer() {
    // Главная страница
    server.on("/", [this]() {
      server.send(200, "text/html", getMainPage());
    });
    
    // API для получения метрик
    server.on("/api/metrics", [this]() {
      DynamicJsonDocument doc(1024);
      
      doc["cpu_usage"] = metrics.cpuUsage;
      doc["memory_usage"] = metrics.memoryUsage;
      doc["temperature"] = metrics.coreTemperature;
      doc["wifi_signal"] = metrics.wifiSignal;
      doc["battery_voltage"] = metrics.batteryVoltage;
      doc["power_consumption"] = metrics.powerConsumption;
      
      doc["imu_status"] = sensors.imuStatus;
      doc["gps_status"] = sensors.gpsStatus;
      doc["baro_status"] = sensors.baroStatus;
      doc["imu_temp"] = sensors.imuTemp;
      doc["gps_satellites"] = sensors.gpsSatellites;
      doc["baro_pressure"] = sensors.baroPressure;
      
      String json;
      serializeJson(doc, json);
      server.send(200, "application/json", json);
    });
    
    // API для логов
    server.on("/api/logs", [this]() {
      File logFile = SPIFFS.open("/flight.log", "r");
      if (logFile) {
        server.streamFile(logFile, "text/plain");
        logFile.close();
      } else {
        server.send(404, "text/plain", "Log file not found");
      }
    });
  }
  
  void updateMetrics() {
    // CPU usage (приблизительно)
    static unsigned long lastIdleTime = 0;
    unsigned long currentTime = micros();
    unsigned long busyTime = currentTime - lastIdleTime;
    metrics.cpuUsage = constrain(busyTime / 1000.0, 0, 100);
    lastIdleTime = currentTime;
    
    // Memory usage
    metrics.memoryUsage = (ESP.getHeapSize() - ESP.getFreeHeap()) * 100.0 / ESP.getHeapSize();
    
    // Core temperature (если доступно)
    metrics.coreTemperature = temperatureRead();
    
    // WiFi signal
    metrics.wifiSignal = WiFi.RSSI();
    
    // Battery voltage (через делитель напряжения)
    int adcValue = analogRead(A0);
    metrics.batteryVoltage = adcValue * (3.3 / 4095.0) * 3.7;  // Коэффициент делителя
    
    // Power consumption (расчетный)
    metrics.powerConsumption = metrics.batteryVoltage * estimateCurrentConsumption();
  }
  
  float estimateCurrentConsumption() {
    // Базовое потребление ESP32
    float current = 240;  // mA для ESP32 на полной нагрузке
    
    // Добавляем потребление WiFi
    if (WiFi.status() == WL_CONNECTED) {
      current += 120;  // mA для активного WiFi
    }
    
    // Добавляем потребление датчиков
    current += 50;  // mA для IMU + GPS + Baro
    
    return current / 1000.0;  // Конвертируем в амперы
  }
  
  void checkSensorHealth() {
    // Проверка IMU
    sensors.imuStatus = checkIMUHealth();
    
    // Проверка GPS
    sensors.gpsStatus = checkGPSHealth();
    
    // Проверка барометра
    sensors.baroStatus = checkBaroHealth();
  }
  
  bool checkIMUHealth() {
    // Здесь должна быть реальная проверка IMU
    // Для примера возвращаем случайное значение
    return random(0, 100) > 10;  // 90% uptime
  }
  
  bool checkGPSHealth() {
    sensors.gpsSatellites = random(4, 12);  // Симуляция спутников
    return sensors.gpsSatellites >= 4;
  }
  
  bool checkBaroHealth() {
    sensors.baroPressure = 1013.25 + random(-100, 100) / 10.0;
    return abs(sensors.baroPressure - 1013.25) < 50;  // Разумный диапазон
  }
  
  String getMainPage() {
    return R"HTML(
<!DOCTYPE html>
<html>
<head>
    <title>ESP32 Drone Debugger</title>
    <meta charset="utf-8">
    <style>
        body { font-family: Arial, sans-serif; margin: 20px; background: #f0f0f0; }
        .container { max-width: 1200px; margin: 0 auto; }
        .card { background: white; padding: 20px; margin: 10px; border-radius: 10px; box-shadow: 0 2px 5px rgba(0,0,0,0.1); }
        .metric { display: inline-block; margin: 10px; padding: 15px; background: #e8f4f8; border-radius: 5px; min-width: 150px; }
        .metric-value { font-size: 24px; font-weight: bold; color: #333; }
        .metric-label { font-size: 12px; color: #666; }
        .status-ok { color: #4CAF50; }
        .status-warning { color: #FF9800; }
        .status-error { color: #F44336; }
        #chart { width: 100%; height: 300px; background: white; border: 1px solid #ddd; }
    </style>
    <script src="https://cdn.jsdelivr.net/npm/chart.js"></script>
</head>
<body>
    <div class="container">
        <h1>🚁 ESP32 Drone Debug Console</h1>
        
        <div class="card">
            <h2>System Metrics</h2>
            <div id="systemMetrics"></div>
        </div>
        
        <div class="card">
            <h2>Sensor Status</h2>
            <div id="sensorStatus"></div>
        </div>
        
        <div class="card">
            <h2>Real-time Data</h2>
            <canvas id="chart"></canvas>
        </div>
        
        <div class="card">
            <h2>Actions</h2>
            <button onclick="downloadLogs()">📥 Download Logs</button>
            <button onclick="rebootSystem()">🔄 Reboot ESP32</button>
            <button onclick="calibrateSensors()">🎯 Calibrate Sensors</button>
        </div>
    </div>
    
    <script>
        let chart;
        let dataPoints = [];
        
        function initChart() {
            const ctx = document.getElementById('chart').getContext('2d');
            chart = new Chart(ctx, {
                type: 'line',
                data: {
                    labels: [],
                    datasets: [{
                        label: 'CPU Usage %',
                        data: [],
                        borderColor: 'rgb(75, 192, 192)',
                        tension: 0.1
                    }, {
                        label: 'Memory Usage %',
                        data: [],
                        borderColor: 'rgb(255, 99, 132)',
                        tension: 0.1
                    }]
                },
                options: {
                    responsive: true,
                    scales: {
                        y: { beginAtZero: true, max: 100 }
                    }
                }
            });
        }
        
        function updateMetrics() {
            fetch('/api/metrics')
                .then(response => response.json())
                .then(data => {
                    // Update system metrics
                    document.getElementById('systemMetrics').innerHTML = `
                        <div class="metric">
                            <div class="metric-value">${data.cpu_usage.toFixed(1)}%</div>
                            <div class="metric-label">CPU Usage</div>
                        </div>
                        <div class="metric">
                            <div class="metric-value">${data.memory_usage.toFixed(1)}%</div>
                            <div class="metric-label">Memory Usage</div>
                        </div>
                        <div class="metric">
                            <div class="metric-value">${data.temperature.toFixed(1)}°C</div>
                            <div class="metric-label">Temperature</div>
                        </div>
                        <div class="metric">
                            <div class="metric-value">${data.wifi_signal} dBm</div>
                            <div class="metric-label">WiFi Signal</div>
                        </div>
                        <div class="metric">
                            <div class="metric-value">${data.battery_voltage.toFixed(2)}V</div>
                            <div class="metric-label">Battery</div>
                        </div>
                        <div class="metric">
                            <div class="metric-value">${data.power_consumption.toFixed(2)}A</div>
                            <div class="metric-label">Current</div>
                        </div>
                    `;
                    
                    // Update sensor status
                    document.getElementById('sensorStatus').innerHTML = `
                        <div class="metric">
                            <div class="metric-value ${data.imu_status ? 'status-ok' : 'status-error'}">
                                ${data.imu_status ? '✅' : '❌'}
                            </div>
                            <div class="metric-label">IMU (${data.imu_temp.toFixed(1)}°C)</div>
                        </div>
                        <div class="metric">
                            <div class="metric-value ${data.gps_status ? 'status-ok' : 'status-error'}">
                                ${data.gps_status ? '✅' : '❌'}
                            </div>
                            <div class="metric-label">GPS (${data.gps_satellites} sats)</div>
                        </div>
                        <div class="metric">
                            <div class="metric-value ${data.baro_status ? 'status-ok' : 'status-error'}">
                                ${data.baro_status ? '✅' : '❌'}
                            </div>
                            <div class="metric-label">Barometer (${data.baro_pressure.toFixed(1)} hPa)</div>
                        </div>
                    `;
                    
                    // Update chart
                    const now = new Date().toLocaleTimeString();
                    chart.data.labels.push(now);
                    chart.data.datasets[0].data.push(data.cpu_usage);
                    chart.data.datasets[1].data.push(data.memory_usage);
                    
                    // Keep only last 20 points
                    if (chart.data.labels.length > 20) {
                        chart.data.labels.shift();
                        chart.data.datasets[0].data.shift();
                        chart.data.datasets[1].data.shift();
                    }
                    
                    chart.update('none');
                })
                .catch(error => console.error('Error fetching metrics:', error));
        }
        
        function downloadLogs() {
            window.open('/api/logs', '_blank');
        }
        
        function rebootSystem() {
            if (confirm('Reboot ESP32? This will disconnect the debug session.')) {
                fetch('/api/reboot', { method: 'POST' });
            }
        }
        
        function calibrateSensors() {
            if (confirm('Start sensor calibration? Keep the drone stationary.')) {
                fetch('/api/calibrate', { method: 'POST' });
            }
        }
        
        // Initialize
        initChart();
        updateMetrics();
        setInterval(updateMetrics, 1000);  // Update every second
    </script>
</body>
</html>
    )HTML";
  }
};

// Использование
DroneDebugger debugger;

void setup() {
  Serial.begin(115200);
  WiFi.begin("YourWiFi", "password");
  while (WiFi.status() != WL_CONNECTED) {
    delay(1000);
    Serial.println("Connecting...");
  }
  debugger.begin();
}

void loop() {
  debugger.update();
  // Ваш основной код дрона здесь
}

// =====================================
// СИСТЕМА АВТОМАТИЧЕСКОГО ТЕСТИРОВАНИЯ
// =====================================

class AutomatedTester {
private:
  struct TestResult {
    String testName;
    bool passed;
    String details;
    unsigned long executionTime;
  };
  
  std::vector<TestResult> testResults;
  
public:
  void runAllTests() {
    Serial.println("=== AUTOMATED TESTING STARTED ===");
    
    testResults.clear();
    
    // Hardware tests
    runTest("Power Supply Test", testPowerSupply);
    runTest("GPIO Functionality", testGPIO);
    runTest("ADC Accuracy", testADC);
    runTest("PWM Generation", testPWM);
    
    // Communication tests
    runTest("I2C Bus Scan", testI2C);
    runTest("SPI Communication", testSPI);
    runTest("UART Loopback", testUART);
    
    // Sensor tests
    runTest("IMU Calibration", testIMU);
    runTest("GPS Signal", testGPS);
    runTest("Barometer", testBarometer);
    
    // Performance tests
    runTest("CPU Performance", testCPUPerformance);
    runTest("Memory Stress", testMemoryStress);
    runTest("Real-time Constraints", testRealTime);
    
    printTestReport();
  }
  
private:
  void runTest(String name, bool (*testFunction)()) {
    Serial.printf("Running test: %s... ", name.c_str());
    
    unsigned long startTime = micros();
    bool result = testFunction();
    unsigned long executionTime = micros() - startTime;
    
    TestResult testResult;
    testResult.testName = name;
    testResult.passed = result;
    testResult.executionTime = executionTime;
    
    testResults.push_back(testResult);
    
    Serial.println(result ? "PASS" : "FAIL");
  }
  
  static bool testPowerSupply() {
    // Тест стабильности напряжений
    float vcc3v3 = analogRead(A1) * (3.3 / 4095.0);
    float vcc5v = analogRead(A2) * (3.3 / 4095.0) * 2.0;  // Делитель 1:2
    
    bool vcc3v3_ok = (vcc3v3 > 3.2 && vcc3v3 < 3.4);
    bool vcc5v_ok = (vcc5v > 4.8 && vcc5v < 5.2);
    
    return vcc3v3_ok && vcc5v_ok;
  }
  
  static bool testGPIO() {
    // Тест цифровых входов/выходов
    const int testPin1 = 2;
    const int testPin2 = 4;
    
    pinMode(testPin1, OUTPUT);
    pinMode(testPin2, INPUT_PULLUP);
    
    // Соединить testPin1 и testPin2 перемычкой для теста
    digitalWrite(testPin1, HIGH);
    delay(10);
    bool test1 = digitalRead(testPin2) == HIGH;
    
    digitalWrite(testPin1, LOW);
    delay(10);
    bool test2 = digitalRead(testPin2) == LOW;
    
    return test1 && test2;
  }
  
  static bool testADC() {
    // Тест точности АЦП
    const int samples = 100;
    float sum = 0;
    
    // Подключить известное напряжение к A0
    for (int i = 0; i < samples; i++) {
      sum += analogRead(A0);
      delay(1);
    }
    
    float average = sum / samples;
    float voltage = average * (3.3 / 4095.0);
    
    // Проверяем что напряжение в разумных пределах
    return (voltage > 0.1 && voltage < 3.2);
  }
  
  static bool testPWM() {
    // Тест генерации PWM
    const int pwmPin = 18;
    
    ledcSetup(0, 1000, 8);  // 1kHz, 8-bit
    ledcAttachPin(pwmPin, 0);
    
    // Тест различных скважностей
    int dutyCycles[] = {0, 64, 128, 192, 255};
    
    for (int duty : dutyCycles) {
      ledcWrite(0, duty);
      delay(100);
      
      // Здесь можно измерить реальную скважность
      // Для упрощения считаем тест пройденным
    }
    
    return true;
  }
  
  static bool testI2C() {
    // Сканирование I2C шины
    Wire.begin();
    int devicesFound = 0;
    
    for (int address = 1; address < 127; address++) {
      Wire.beginTransmission(address);
      int error = Wire.endTransmission();
      
      if (error == 0) {
        devicesFound++;
        Serial.printf("I2C device found at 0x%02X\n", address);
      }
    }
    
    return devicesFound > 0;  // Хотя бы одно устройство найдено
  }
  
  static bool testSPI() {
    // Тест SPI loopback (MOSI соединен с MISO)
    SPI.begin();
    
    byte testData[] = {0xAA, 0x55, 0xFF, 0x00};
    bool success = true;
    
    for (byte data : testData) {
      byte received = SPI.transfer(data);
      if (received != data) {
        success = false;
        break;
      }
    }
    
    return success;
  }
  
  static bool testUART() {
    // Тест UART loopback (TX соединен с RX)
    Serial1.begin(115200);
    
    String testString = "UART_TEST_123";
    Serial1.print(testString);
    delay(100);
    
    String received = "";
    while (Serial1.available()) {
      received += (char)Serial1.read();
    }
    
    return received == testString;
  }
  
  static bool testIMU() {
    // Тест IMU (проверка базовой функциональности)
    // Здесь должна быть реальная инициализация IMU
    // Для примера возвращаем true
    return true;
  }
  
  static bool testGPS() {
    // Тест GPS модуля
    // Проверяем получение данных
    return true;  // Упрощенная реализация
  }
  
  static bool testBarometer() {
    // Тест барометра
    return true;  // Упрощенная реализация
  }
  
  static bool testCPUPerformance() {
    // Тест производительности CPU
    unsigned long startTime = micros();
    
    // Выполняем вычислительно-интенсивную задачу
    float result = 0;
    for (int i = 0; i < 10000; i++) {
      result += sin(i * 0.01) * cos(i * 0.01);
    }
    
    unsigned long executionTime = micros() - startTime;
    
    // Ожидаемое время выполнения на ESP32
    return executionTime < 100000;  // Меньше 100ms
  }
  
  static bool testMemoryStress() {
    // Тест памяти
    size_t freeHeapBefore = ESP.getFreeHeap();
    
    // Выделяем и освобождаем память
    void* ptr = malloc(10000);
    if (ptr == nullptr) return false;
    
    free(ptr);
    
    size_t freeHeapAfter = ESP.getFreeHeap();
    
    // Проверяем что память освободилась
    return (freeHeapAfter >= freeHeapBefore - 100);  // Допуск 100 байт
  }
  
  static bool testRealTime() {
    // Тест соблюдения временных ограничений
    unsigned long times[100];
    
    for (int i = 0; i < 100; i++) {
      unsigned long start = micros();
      delay(10);  // Задержка 10ms
      times[i] = micros() - start;
    }
    
    // Проверяем что все задержки в пределах ±1ms
    for (int i = 0; i < 100; i++) {
      if (times[i] < 9000 || times[i] > 11000) {
        return false;
      }
    }
    
    return true;
  }
  
  void printTestReport() {
    Serial.println("\n=== TEST REPORT ===");
    
    int passed = 0;
    int total = testResults.size();
    
    for (const auto& result : testResults) {
      Serial.printf("%-20s: %s (%lu μs)\n", 
        result.testName.c_str(),
        result.passed ? "PASS" : "FAIL",
        result.executionTime);
      
      if (result.passed) passed++;
    }
    
    Serial.printf("\nSUMMARY: %d/%d tests passed (%.1f%%)\n", 
      passed, total, (float)passed * 100.0 / total);
    
    if (passed == total) {
      Serial.println("🎉 ALL TESTS PASSED! System is ready for flight.");
    } else {
      Serial.println("⚠️  SOME TESTS FAILED! Check hardware connections.");
    }
  }
};

// =====================================
// УНИВЕРСАЛЬНАЯ БИБЛИОТЕКА ДАТЧИКОВ
// =====================================

#include <Wire.h>
#include <SPI.h>

class UniversalSensorLib {
public:
  enum SensorType {
    IMU_MPU6050,
    IMU_MPU9250, 
    IMU_ICM20948,
    BARO_BMP280,
    BARO_MS5611,
    GPS_UBLOX,
    RANGEFINDER_VL53L0X,
    COMPASS_HMC5883L
  };
  
  struct SensorData {
    float gyroX, gyroY, gyroZ;      // град/с
    float accelX, accelY, accelZ;   // g
    float magX, magY, magZ;         // Гаусс
    float pressure;                 // Па
    float temperature;              // °C
    float altitude;                 // м
    float latitude, longitude;      // градусы
    int satellites;
    float distance;                 // м (дальномер)
    unsigned long timestamp;       // мс
    bool dataValid;
  };
  
private:
  std::vector<SensorType> activeSensors;
  SensorData lastData;
  
public:
  bool initSensor(SensorType type, int address = 0) {
    switch (type) {
      case IMU_MPU6050:
        return initMPU6050(address ? address : 0x68);
      case IMU_MPU9250:
        return initMPU9250(address ? address : 0x68);
      case BARO_BMP280:
        return initBMP280(address ? address : 0x76);
      // ... другие датчики
    }
    return false;
  }
  
  SensorData readAllSensors() {
    SensorData data = {};
    data.timestamp = millis();
    data.dataValid = true;
    
    for (SensorType sensor : activeSensors) {
      switch (sensor) {
        case IMU_MPU6050:
          readMPU6050(data);
          break;
        case BARO_BMP280:
          readBMP280(data);
          break;
        // ... другие датчики
      }
    }
    
    lastData = data;
    return data;
  }
  
  // Автоматическая калибровка
  bool calibrateSensors(int samples = 1000) {
    Serial.println("Starting sensor calibration...");
    Serial.println("Keep the drone stationary!");
    
    // Калибровка гироскопа (определение offset)
    float gyroOffsetX = 0, gyroOffsetY = 0, gyroOffsetZ = 0;
    
    for (int i = 0; i < samples; i++) {
      SensorData data = readAllSensors();
      gyroOffsetX += data.gyroX;
      gyroOffsetY += data.gyroY;
      gyroOffsetZ += data.gyroZ;
      
      if (i % 100 == 0) {
        Serial.printf("Calibration progress: %d%%\n", i * 100 / samples);
      }
      
      delay(10);
    }
    
    gyroOffsetX /= samples;
    gyroOffsetY /= samples;
    gyroOffsetZ /= samples;
    
    // Сохранение калибровочных данных
    saveCalibrationData(gyroOffsetX, gyroOffsetY, gyroOffsetZ);
    
    Serial.println("Calibration complete!");
    return true;
  }
  
  // Диагностика датчиков
  void runDiagnostics() {
    Serial.println("=== SENSOR DIAGNOSTICS ===");
    
    for (SensorType sensor : activeSensors) {
      switch (sensor) {
        case IMU_MPU6050:
          diagnoseMPU6050();
          break;
        case BARO_BMP280:
          diagnoseBMP280();
          break;
        // ... другие датчики
      }
    }
  }
  
private:
  bool initMPU6050(int address) {
    Wire.beginTransmission(address);
    if (Wire.endTransmission() != 0) {
      Serial.println("MPU6050 not found!");
      return false;
    }
    
    // Инициализация MPU6050
    writeRegister(address, 0x6B, 0x00);  // Wake up
    writeRegister(address, 0x1A, 0x03);  // Low pass filter
    writeRegister(address, 0x1B, 0x08);  // Gyro ±500°/s
    writeRegister(address, 0x1C, 0x08);  // Accel ±4g
    
    activeSensors.push_back(IMU_MPU6050);
    Serial.println("MPU6050 initialized");
    return true;
  }
  
  void readMPU6050(SensorData& data) {
    const int MPU6050_ADDR = 0x68;
    
    Wire.beginTransmission(MPU6050_ADDR);
    Wire.write(0x3B);  // Starting register
    Wire.endTransmission(false);
    Wire.requestFrom(MPU6050_ADDR, 14, true);
    
    // Чтение сырых данных
    int16_t rawAccelX = Wire.read() << 8 | Wire.read();
    int16_t rawAccelY = Wire.read() << 8 | Wire.read();
    int16_t rawAccelZ = Wire.read() << 8 | Wire.read();
    int16_t rawTemp   = Wire.read() << 8 | Wire.read();
    int16_t rawGyroX  = Wire.read() << 8 | Wire.read();
    int16_t rawGyroY  = Wire.read() << 8 | Wire.read();
    int16_t rawGyroZ  = Wire.read() << 8 | Wire.read();
    
    // Преобразование в физические единицы
    data.accelX = rawAccelX / 8192.0;  // ±4g range
    data.accelY = rawAccelY / 8192.0;
    data.accelZ = rawAccelZ / 8192.0;
    
    data.gyroX = rawGyroX / 65.5;      // ±500°/s range
    data.gyroY = rawGyroY / 65.5;
    data.gyroZ = rawGyroZ / 65.5;
    
    data.temperature = rawTemp / 340.0 + 36.53;
  }
  
  bool initBMP280(int address) {
    Wire.beginTransmission(address);
    if (Wire.endTransmission() != 0) {
      Serial.println("BMP280 not found!");
      return false;
    }
    
    // Чтение ID
    uint8_t chipID = readRegister(address, 0xD0);
    if (chipID != 0x58) {
      Serial.printf("Wrong BMP280 ID: 0x%02X\n", chipID);
      return false;
    }
    
    // Настройка BMP280
    writeRegister(address, 0xF4, 0x27);  // Normal mode, temp and pressure oversampling
    writeRegister(address, 0xF5, 0xA0);  // Config: standby time, filter
    
    activeSensors.push_back(BARO_BMP280);
    Serial.println("BMP280 initialized");
    return true;
  }
  
  void readBMP280(SensorData& data) {
    const int BMP280_ADDR = 0x76;
    
    // Чтение сырых данных давления и температуры
    Wire.beginTransmission(BMP280_ADDR);
    Wire.write(0xF7);  // Pressure MSB
    Wire.endTransmission(false);
    Wire.requestFrom(BMP280_ADDR, 6, true);
    
    uint32_t rawPressure = (Wire.read() << 12) | (Wire.read() << 4) | (Wire.read() >> 4);
    uint32_t rawTemp = (Wire.read() << 12) | (Wire.read() << 4) | (Wire.read() >> 4);
    
    // Компенсация (упрощенная)
    data.temperature = rawTemp / 16384.0 - 15.0;  // Приблизительная формула
    data.pressure = rawPressure / 256.0;          // Приблизительная формула
    
    // Расчет высоты по барометрической формуле
    data.altitude = 44330.0 * (1.0 - pow(data.pressure / 101325.0, 0.1903));
  }
  
  void diagnoseMPU6050() {
    Serial.println("MPU6050 Diagnostics:");
    
    // Проверка температуры
    SensorData data = readAllSensors();
    Serial.printf("  Temperature: %.1f°C ", data.temperature);
    if (data.temperature > -40 && data.temperature < 85) {
      Serial.println("OK");
    } else {
      Serial.println("OUT OF RANGE");
    }
    
    // Проверка уровня шума
    float gyroNoise = 0;
    for (int i = 0; i < 100; i++) {
      SensorData sample = readAllSensors();
      gyroNoise += abs(sample.gyroX) + abs(sample.gyroY) + abs(sample.gyroZ);
      delay(10);
    }
    gyroNoise /= 100;
    
    Serial.printf("  Gyro noise level: %.2f°/s ", gyroNoise);
    if (gyroNoise < 1.0) {
      Serial.println("GOOD");
    } else if (gyroNoise < 5.0) {
      Serial.println("ACCEPTABLE");
    } else {
      Serial.println("HIGH - CHECK MOUNTING");
    }
  }
  
  void diagnoseBMP280() {
    Serial.println("BMP280 Diagnostics:");
    
    SensorData data = readAllSensors();
    
    Serial.printf("  Pressure: %.1f Pa ", data.pressure);
    if (data.pressure > 80000 && data.pressure < 120000) {
      Serial.println("REASONABLE");
    } else {
      Serial.println("OUT OF EXPECTED RANGE");
    }
    
    Serial.printf("  Calculated altitude: %.1f m\n", data.altitude);
  }
  
  // Вспомогательные функции
  void writeRegister(int address, uint8_t reg, uint8_t value) {
    Wire.beginTransmission(address);
    Wire.write(reg);
    Wire.write(value);
    Wire.endTransmission();
  }
  
  uint8_t readRegister(int address, uint8_t reg) {
    Wire.beginTransmission(address);
    Wire.write(reg);
    Wire.endTransmission(false);
    Wire.requestFrom(address, 1, true);
    return Wire.read();
  }
  
  void saveCalibrationData(float gx, float gy, float gz) {
    // Сохранение в EEPROM или SPIFFS
    // Упрощенная реализация
  }
};

// Пример использования
UniversalSensorLib sensors;

void setup() {
  Serial.begin(115200);
  Wire.begin();
  
  // Инициализация датчиков
  sensors.initSensor(UniversalSensorLib::IMU_MPU6050);
  sensors.initSensor(UniversalSensorLib::BARO_BMP280);
  
  // Калибровка
  sensors.calibrateSensors();
  
  // Диагностика
  sensors.runDiagnostics();
}

void loop() {
  UniversalSensorLib::SensorData data = sensors.readAllSensors();
  
  Serial.printf("Gyro: %.2f, %.2f, %.2f | ", data.gyroX, data.gyroY, data.gyroZ);
  Serial.printf("Accel: %.2f, %.2f, %.2f | ", data.accelX, data.accelY, data.accelZ);
  Serial.printf("Alt: %.1fm\n", data.altitude);
  
  delay(100);
}

// =====================================
// МОДУЛЬНЫЙ ФРЕЙМВОРК АВТОПИЛОТА
// =====================================

class AutopilotFramework {
public:
  // Базовый класс для модулей
  class Module {
  public:
    virtual void init() = 0;
    virtual void update(float dt) = 0;
    virtual bool isHealthy() = 0;
    virtual String getName() = 0;
  };
  
  // Модуль сбора данных датчиков
  class SensorModule : public Module {
  private:
    UniversalSensorLib sensors;
    
  public:
    UniversalSensorLib::SensorData currentData;
    
    void init() override {
      sensors.initSensor(UniversalSensorLib::IMU_MPU6050);
      sensors.initSensor(UniversalSensorLib::BARO_BMP280);
    }
    
    void update(float dt) override {
      currentData = sensors.readAllSensors();
    }
    
    bool isHealthy() override {
      return currentData.dataValid && 
             (millis() - currentData.timestamp) < 100;
    }
    
    String getName() override { return "SensorModule"; }
  };
  
  // Модуль оценки состояния
  class StateEstimatorModule : public Module {
  public:
    struct State {
      float roll, pitch, yaw;        // углы ориентации
      float rollRate, pitchRate, yawRate;  // угловые скорости
      float x, y, z;                 // позиция
      float vx, vy, vz;              // скорости
      float ax, ay, az;              // ускорения
    } currentState;
    
  private:
    SensorModule* sensorModule;
    
  public:
    StateEstimatorModule(SensorModule* sensors) : sensorModule(sensors) {}
    
    void init() override {
      // Инициализация фильтров
    }
    
    void update(float dt) override {
      if (!sensorModule || !sensorModule->isHealthy()) return;
      
      auto& data = sensorModule->currentData;
      
      // Комплементарный фильтр для углов
      updateAttitudeEstimate(data, dt);
      
      // Оценка позиции (если есть GPS)
      updatePositionEstimate(data, dt);
      
      // Оценка скоростей
      updateVelocityEstimate(data, dt);
    }
    
    bool isHealthy() override {
      return sensorModule && sensorModule->isHealthy();
    }
    
    String getName() override { return "StateEstimator"; }
    
  private:
    void updateAttitudeEstimate(const UniversalSensorLib::SensorData& data, float dt) {
      // Интегрирование гироскопа
      currentState.roll += data.gyroX * dt;
      currentState.pitch += data.gyroY * dt;
      currentState.yaw += data.gyroZ * dt;
      
      // Коррекция по акселерометру
      float accelRoll = atan2(data.accelY, data.accelZ) * 180.0 / PI;
      float accelPitch = atan2(-data.accelX, sqrt(data.accelY*data.accelY + data.accelZ*data.accelZ)) * 180.0 / PI;
      
      float alpha = 0.98;
      currentState.roll = alpha * currentState.roll + (1.0 - alpha) * accelRoll;
      currentState.pitch = alpha * currentState.pitch + (1.0 - alpha) * accelPitch;
      
      // Угловые скорости
      currentState.rollRate = data.gyroX;
      currentState.pitchRate = data.gyroY;
      currentState.yawRate = data.gyroZ;
    }
    
    void updatePositionEstimate(const UniversalSensorLib::SensorData& data, float dt) {
      // Высота по барометру
      currentState.z = data.altitude;
      
      // Вертикальная скорость (производная высоты)
      static float lastAltitude = 0;
      currentState.vz = (data.altitude - lastAltitude) / dt;
      lastAltitude = data.altitude;
    }
    
    void updateVelocityEstimate(const UniversalSensorLib::SensorData& data, float dt) {
      // Интегрирование ускорений для получения скоростей
      // (упрощенная реализация)
      currentState.vx += data.accelX * 9.81 * dt;
      currentState.vy += data.accelY * 9.81 * dt;
    }
  };
  
  // Модуль управления
  class ControlModule : public Module {
  public:
    struct ControlInputs {
      float rollCommand, pitchCommand, yawCommand, throttleCommand;
      bool isArmed;
      int flightMode;  // 0=Manual, 1=Stabilize, 2=AltHold, etc.
    } inputs;
    
    struct ControlOutputs {
      float motor1, motor2, motor3, motor4;  // 0.0-1.0
    } outputs;
    
  private:
    StateEstimatorModule* stateEstimator;
    
    // PID контроллеры
    struct PIDController {
      float kp, ki, kd;
      float integral, lastError;
      float outputMin, outputMax;
      
      float update(float setpoint, float measurement, float dt) {
        float error = setpoint - measurement;
        
        integral += error * dt;
        integral = constrain(integral, -10, 10);  // Anti-windup
        
        float derivative = (error - lastError) / dt;
        lastError = error;
        
        float output = kp * error + ki * integral + kd * derivative;
        return constrain(output, outputMin, outputMax);
      }
    };
    
    PIDController rollPID = {1.0, 0.1, 0.05, 0, 0, -1, 1};
    PIDController pitchPID = {1.0, 0.1, 0.05, 0, 0, -1, 1};
    PIDController yawPID = {1.0, 0.1, 0.05, 0, 0, -1, 1};
    PIDController altPID = {0.5, 0.1, 0.2, 0, 0, -0.5, 0.5};
    
  public:
    ControlModule(StateEstimatorModule* estimator) : stateEstimator(estimator) {}
    
    void init() override {
      outputs = {0, 0, 0, 0};
    }
    
    void update(float dt) override {
      if (!stateEstimator || !stateEstimator->isHealthy()) {
        // Аварийное отключение
        outputs = {0, 0, 0, 0};
        return;
      }
      
      if (!inputs.isArmed) {
        outputs = {0, 0, 0, 0};
        return;
      }
      
      switch (inputs.flightMode) {
        case 0: // Manual
          manualControl();
          break;
        case 1: // Stabilize
          stabilizeControl(dt);
          break;
        case 2: // Altitude Hold
          altitudeHoldControl(dt);
          break;
      }
    }
    
    bool isHealthy() override {
      return stateEstimator && stateEstimator->isHealthy();
    }
    
    String getName() override { return "ControlModule"; }
    
  private:
    void manualControl() {
      // Прямое управление
      float base = inputs.throttleCommand;
      
      outputs.motor1 = base + inputs.pitchCommand - inputs.rollCommand - inputs.yawCommand;
      outputs.motor2 = base - inputs.pitchCommand - inputs.rollCommand + inputs.yawCommand;
      outputs.motor3 = base - inputs.pitchCommand + inputs.rollCommand - inputs.yawCommand;
      outputs.motor4 = base + inputs.pitchCommand + inputs.rollCommand + inputs.yawCommand;
      
      constrainOutputs();
    }
    
    void stabilizeControl(float dt) {
      auto& state = stateEstimator->currentState;
      
      // Желаемые углы на основе команд
      float desiredRoll = inputs.rollCommand * 30.0;    // ±30°
      float desiredPitch = inputs.pitchCommand * 30.0;
      float desiredYawRate = inputs.yawCommand * 180.0; // ±180°/s
      
      // PID управление
      float rollOutput = rollPID.update(desiredRoll, state.roll, dt);
      float pitchOutput = pitchPID.update(desiredPitch, state.pitch, dt);
      float yawOutput = yawPID.update(desiredYawRate, state.yawRate, dt);
      
      // Микширование на моторы
      float throttle = inputs.throttleCommand;
      
      outputs.motor1 = throttle + pitchOutput - rollOutput - yawOutput;
      outputs.motor2 = throttle - pitchOutput - rollOutput + yawOutput;
      outputs.motor3 = throttle - pitchOutput + rollOutput - yawOutput;
      outputs.motor4 = throttle + pitchOutput + rollOutput + yawOutput;
      
      constrainOutputs();
    }
    
    void altitudeHoldControl(float dt) {
      // Сначала стабилизация углов
      stabilizeControl(dt);
      
      // Затем управление высотой
      static float targetAltitude = 0;
      static bool altitudeSet = false;
      
      if (!altitudeSet) {
        targetAltitude = stateEstimator->currentState.z;
        altitudeSet = true;
      }
      
      // Изменение целевой высоты джойстиком
      if (abs(inputs.throttleCommand - 0.5) > 0.1) {
        targetAltitude += (inputs.throttleCommand - 0.5) * 2.0 * dt;
        altitudeSet = false;
      }
      
      float altOutput = altPID.update(targetAltitude, stateEstimator->currentState.z, dt);
      
      // Добавляем коррекцию высоты ко всем моторам
      outputs.motor1 += altOutput;
      outputs.motor2 += altOutput;
      outputs.motor3 += altOutput;
      outputs.motor4 += altOutput;
      
      constrainOutputs();
    }
    
    void constrainOutputs() {
      outputs.motor1 = constrain(outputs.motor1, 0.0, 1.0);
      outputs.motor2 = constrain(outputs.motor2, 0.0, 1.0);
      outputs.motor3 = constrain(outputs.motor3, 0.0, 1.0);
      outputs.motor4 = constrain(outputs.motor4, 0.0, 1.0);
    }
  };
  
  // Модуль вывода на моторы
  class MotorModule : public Module {
  private:
    ControlModule* controlModule;
    
  public:
    MotorModule(ControlModule* control) : controlModule(control) {}
    
    void init() override {
      // Инициализация PWM каналов для 4 моторов
      ledcSetup(0, 50, 16);  // Motor 1
      ledcSetup(1, 50, 16);  // Motor 2
      ledcSetup(2, 50, 16);  // Motor 3
      ledcSetup(3, 50, 16);  // Motor 4
      
      ledcAttachPin(18, 0);  // Motor 1 pin
      ledcAttachPin(19, 1);  // Motor 2 pin
      ledcAttachPin(21, 2);  // Motor 3 pin
      ledcAttachPin(22, 3);  // Motor 4 pin
    }
    
    void update(float dt) override {
      if (!controlModule || !controlModule->isHealthy()) {
        // Аварийная остановка
        ledcWrite(0, 3277);  // 1ms pulse (stop)
        ledcWrite(1, 3277);
        ledcWrite(2, 3277);
        ledcWrite(3, 3277);
        return;
      }
      
      auto& outputs = controlModule->outputs;
      
      // Преобразование 0.0-1.0 в PWM значения 1000-2000us
      // При 50Hz и 16-bit: 1ms = 3277, 2ms = 6554
      uint16_t pwm1 = map(outputs.motor1 * 1000, 0, 1000, 3277, 6554);
      uint16_t pwm2 = map(outputs.motor2 * 1000, 0, 1000, 3277, 6554);
      uint16_t pwm3 = map(outputs.motor3 * 1000, 0, 1000, 3277, 6554);
      uint16_t pwm4 = map(outputs.motor4 * 1000, 0, 1000, 3277, 6554);
      
      ledcWrite(0, pwm1);
      ledcWrite(1, pwm2);
      ledcWrite(2, pwm3);
      ledcWrite(3, pwm4);
    }
    
    bool isHealthy() override {
      return controlModule && controlModule->isHealthy();
    }
    
    String getName() override { return "MotorModule"; }
  };
  
private:
  std::vector<Module*> modules;
  unsigned long lastUpdate;
  
public:
  void addModule(Module* module) {
    modules.push_back(module);
  }
  
  void init() {
    Serial.println("Initializing Autopilot Framework...");
    
    for (Module* module : modules) {
      Serial.printf("Initializing %s...", module->getName().c_str());
      module->init();
      Serial.println(" OK");
    }
    
    lastUpdate = millis();
    Serial.println("Autopilot Framework ready!");
  }
  
  void update() {
    unsigned long currentTime = millis();
    float dt = (currentTime - lastUpdate) / 1000.0;
    lastUpdate = currentTime;
    
    // Обновление всех модулей
    for (Module* module : modules) {
      module->update(dt);
    }
    
    // Проверка здоровья системы
    checkSystemHealth();
  }
  
private:
  void checkSystemHealth() {
    static unsigned long lastHealthCheck = 0;
    
    if (millis() - lastHealthCheck > 1000) {  // Каждую секунду
      bool systemHealthy = true;
      
      for (Module* module : modules) {
        if (!module->isHealthy()) {
          Serial.printf("WARNING: %s is not healthy!\n", module->getName().c_str());
          systemHealthy = false;
        }
      }
      
      if (!systemHealthy) {
        Serial.println("SYSTEM HEALTH CHECK FAILED!");
        // Здесь можно добавить аварийные процедуры
      }
      
      lastHealthCheck = millis();
    }
  }
};

// Пример использования фреймворка
AutopilotFramework autopilot;
AutopilotFramework::SensorModule sensorModule;
AutopilotFramework::StateEstimatorModule stateEstimator(&sensorModule);
AutopilotFramework::ControlModule controlModule(&stateEstimator);
AutopilotFramework::MotorModule motorModule(&controlModule);

void setup() {
  Serial.begin(115200);
  Wire.begin();
  
  // Добавляем модули в фреймворк
  autopilot.addModule(&sensorModule);
  autopilot.addModule(&stateEstimator);
  autopilot.addModule(&controlModule);
  autopilot.addModule(&motorModule);
  
  // Инициализация
  autopilot.init();
  
  // Настройка управления
  controlModule.inputs.flightMode = 1;  // Stabilize mode
  controlModule.inputs.isArmed = false;
}

void loop() {
  autopilot.update();
  
  // Обработка команд с радио
  // controlModule.inputs.rollCommand = ...
  // controlModule.