ESP32 И ЭЛЕКТРОНИКА
📋 ДОПОЛНИТЕЛЬНЫЕ МАТЕРИАЛЫ К МОДУЛЮ 5: “ESP32 И ЭЛЕКТРОНИКА”
Углубленные ресурсы для освоения программирования и электроники дронов
🔬 РАЗДЕЛ A: ЭКСПЕРИМЕНТАЛЬНАЯ ЛАБОРАТОРИЯ
1. Домашняя лаборатория электроники
1.1 Набор “Электроника дронов своими руками”
1🏠 СОЗДАНИЕ ДОМАШНЕЙ ЛАБОРАТОРИИ:
2
3БАЗОВОЕ ОБОРУДОВАНИЕ:
4□ Макетная плата (breadboard) 830 точек
5□ ESP32 Dev Kit (2-3 штуки для экспериментов)
6□ Набор резисторов (10Ом - 1МОм)
7□ Конденсаторы (1nF - 1000uF)
8□ Светодиоды разных цветов (50+ штук)
9□ Кнопки и переключатели
10□ Потенциометры (1кОм, 10кОм, 100кОм)
11□ Провода для макетирования (M-M, M-F, F-F)
12
13ИЗМЕРИТЕЛЬНЫЕ ПРИБОРЫ:
14□ Цифровой мультиметр с автоматическим выбором диапазонов
15□ USB осциллограф (или DSO138 kit)
16□ Логический анализатор на 8 каналов
17□ Генератор функций (XR2206 kit)
18□ Источник питания 0-30V 0-3A (регулируемый)
19
20СПЕЦИАЛИЗИРОВАННЫЕ МОДУЛИ:
21□ IMU модули (MPU6050, MPU9250, ICM20948)
22□ GPS модули (NEO-8M, NEO-M9N)
23□ Барометры (BMP280, MS5611)
24□ Дальномеры (VL53L0X, HC-SR04)
25□ ESC модули для тестирования
26□ Серводвигатели различных размеров
27□ Моторы BLDC с энкодерами
28
29СТЕНДЫ ДЛЯ ЭКСПЕРИМЕНТОВ:
30□ Поворотный стол для тестирования IMU
31□ Камера давления для тестирования барометров
32□ Экранированная камера для GPS тестов
33□ Вибростенд для анализа помехоустойчивости
1.2 Продвинутая схемотехника
1⚡ ПРАКТИЧЕСКИЕ СХЕМЫ ДЛЯ ДРОНОВ:
2
3СХЕМА 1: УНИВЕРСАЛЬНЫЙ ФИЛЬТР ПИТАНИЯ
Проблема: Импульсные помехи от ESC влияют на датчики
Решение: VCC (нестабильное) ──┬── L1 (10uH) ──┬── VCC_CLEAN │ │ └── C1 (1000uF) ├── C2 (100nF) │ GND ─────────────────────────────────┴─── GND
L1 - дроссель для фильтрации ВЧ помех
C1 - сглаживание низкочастотных пульсаций
C2 - подавление высокочастотных помех
Практическая реализация:
1// Тестирование качества питания
2void testPowerQuality() {
3 int samples = 1000;
4 float sum = 0, sumSquares = 0;
5
6 for(int i = 0; i < samples; i++) {
7 float voltage = analogRead(A0) * (3.3/4095.0);
8 sum += voltage;
9 sumSquares += voltage * voltage;
10 delayMicroseconds(10); // 100kHz sampling
11 }
12
13 float mean = sum / samples;
14 float variance = (sumSquares / samples) - (mean * mean);
15 float ripple = sqrt(variance);
16
17 Serial.printf("Среднее напряжение: %.3fV\n", mean);
18 Serial.printf("Пульсации: %.1fmV RMS\n", ripple * 1000);
19 Serial.printf("Качество питания: %s\n",
20 ripple < 0.01 ? "ОТЛИЧНО" :
21 ripple < 0.05 ? "ХОРОШО" : "ТРЕБУЕТ ФИЛЬТРАЦИИ");
22}
СХЕМА 2: ЗАЩИТА ОТ ПЕРЕПОЛЮСОВКИ
1Проблема: Неправильное подключение батареи может сжечь электронику
2
3Решение с MOSFET:
4BAT+ ──── DRAIN │ │ SOURCE ──── VCC_PROTECTED
5 │ P-MOSFET │
6 │ │ │
7 └────┘ │
8 │ │
9BAT- ──────────────┴────┴──── GND
10
11Альтернатива с диодом Шоттки:
12BAT+ ──── ANODE │▷│ CATHODE ──── VCC_PROTECTED
13 DIODE
14BAT- ───────────────────────── GND
15
16Сравнение решений:
17- MOSFET: Нет падения напряжения, но сложнее
18- Диод: Простота, но потеря 0.3-0.7V
СХЕМА 3: РАЗВЯЗКА ЦИФРОВЫХ И АНАЛОГОВЫХ ЦЕПЕЙ
1Проблема: Цифровые помехи влияют на точность АЦП
2
3Решение:
4VCC_DIGITAL ──┬── ESP32 Digital VCC
5 │
6 ├── FERRITE BEAD ──── VCC_ANALOG ──── ESP32 Analog VCC
7 │
8GND_DIGITAL ──┼── ESP32 Digital GND
9 │
10 └── SINGLE POINT ──── GND_ANALOG ──── ESP32 Analog GND
11
12Практическая проверка:
13```cpp
14void testADCNoise() {
15 // Тест без нагрузки на цифровые линии
16 digitalWrite(LED_BUILTIN, LOW);
17 int quietReading = analogRead(A0);
18
19 // Тест с активной цифровой нагрузкой
20 for(int i = 0; i < 100; i++) {
21 digitalWrite(LED_BUILTIN, HIGH);
22 delayMicroseconds(10);
23 digitalWrite(LED_BUILTIN, LOW);
24 delayMicroseconds(10);
25 }
26 int noisyReading = analogRead(A0);
27
28 int noiseDifference = abs(noisyReading - quietReading);
29 Serial.printf("Шум АЦП от цифровых цепей: %d LSB\n", noiseDifference);
30 Serial.printf("Качество развязки: %s\n",
31 noiseDifference < 5 ? "ОТЛИЧНО" :
32 noiseDifference < 20 ? "ПРИЕМЛЕМО" : "ПЛОХО");
33}
2. Продвинутая диагностика и отладка
2.1 Системы мониторинга в реальном времени
1// =====================================
2// СИСТЕМА ДИАГНОСТИКИ ESP32 ДРОНА
3// =====================================
4
5#include <WiFi.h>
6#include <WebServer.h>
7#include <ArduinoJson.h>
8#include <SPIFFS.h>
9
10class DroneDebugger {
11private:
12 WebServer server;
13 unsigned long lastUpdate;
14
15 struct SystemMetrics {
16 float cpuUsage;
17 float memoryUsage;
18 float coreTemperature;
19 int wifiSignal;
20 float batteryVoltage;
21 float powerConsumption;
22 } metrics;
23
24 struct SensorHealth {
25 bool imuStatus;
26 bool gpsStatus;
27 bool baroStatus;
28 float imuTemp;
29 int gpsSatellites;
30 float baroPressure;
31 } sensors;
32
33public:
34 DroneDebugger() : server(80) {}
35
36 void begin() {
37 SPIFFS.begin();
38 setupWebServer();
39 server.begin();
40 Serial.println("Debug server started on port 80");
41 }
42
43 void update() {
44 server.handleClient();
45
46 if (millis() - lastUpdate > 100) { // 10Hz update
47 updateMetrics();
48 checkSensorHealth();
49 lastUpdate = millis();
50 }
51 }
52
53private:
54 void setupWebServer() {
55 // Главная страница
56 server.on("/", [this]() {
57 server.send(200, "text/html", getMainPage());
58 });
59
60 // API для получения метрик
61 server.on("/api/metrics", [this]() {
62 DynamicJsonDocument doc(1024);
63
64 doc["cpu_usage"] = metrics.cpuUsage;
65 doc["memory_usage"] = metrics.memoryUsage;
66 doc["temperature"] = metrics.coreTemperature;
67 doc["wifi_signal"] = metrics.wifiSignal;
68 doc["battery_voltage"] = metrics.batteryVoltage;
69 doc["power_consumption"] = metrics.powerConsumption;
70
71 doc["imu_status"] = sensors.imuStatus;
72 doc["gps_status"] = sensors.gpsStatus;
73 doc["baro_status"] = sensors.baroStatus;
74 doc["imu_temp"] = sensors.imuTemp;
75 doc["gps_satellites"] = sensors.gpsSatellites;
76 doc["baro_pressure"] = sensors.baroPressure;
77
78 String json;
79 serializeJson(doc, json);
80 server.send(200, "application/json", json);
81 });
82
83 // API для логов
84 server.on("/api/logs", [this]() {
85 File logFile = SPIFFS.open("/flight.log", "r");
86 if (logFile) {
87 server.streamFile(logFile, "text/plain");
88 logFile.close();
89 } else {
90 server.send(404, "text/plain", "Log file not found");
91 }
92 });
93 }
94
95 void updateMetrics() {
96 // CPU usage (приблизительно)
97 static unsigned long lastIdleTime = 0;
98 unsigned long currentTime = micros();
99 unsigned long busyTime = currentTime - lastIdleTime;
100 metrics.cpuUsage = constrain(busyTime / 1000.0, 0, 100);
101 lastIdleTime = currentTime;
102
103 // Memory usage
104 metrics.memoryUsage = (ESP.getHeapSize() - ESP.getFreeHeap()) * 100.0 / ESP.getHeapSize();
105
106 // Core temperature (если доступно)
107 metrics.coreTemperature = temperatureRead();
108
109 // WiFi signal
110 metrics.wifiSignal = WiFi.RSSI();
111
112 // Battery voltage (через делитель напряжения)
113 int adcValue = analogRead(A0);
114 metrics.batteryVoltage = adcValue * (3.3 / 4095.0) * 3.7; // Коэффициент делителя
115
116 // Power consumption (расчетный)
117 metrics.powerConsumption = metrics.batteryVoltage * estimateCurrentConsumption();
118 }
119
120 float estimateCurrentConsumption() {
121 // Базовое потребление ESP32
122 float current = 240; // mA для ESP32 на полной нагрузке
123
124 // Добавляем потребление WiFi
125 if (WiFi.status() == WL_CONNECTED) {
126 current += 120; // mA для активного WiFi
127 }
128
129 // Добавляем потребление датчиков
130 current += 50; // mA для IMU + GPS + Baro
131
132 return current / 1000.0; // Конвертируем в амперы
133 }
134
135 void checkSensorHealth() {
136 // Проверка IMU
137 sensors.imuStatus = checkIMUHealth();
138
139 // Проверка GPS
140 sensors.gpsStatus = checkGPSHealth();
141
142 // Проверка барометра
143 sensors.baroStatus = checkBaroHealth();
144 }
145
146 bool checkIMUHealth() {
147 // Здесь должна быть реальная проверка IMU
148 // Для примера возвращаем случайное значение
149 return random(0, 100) > 10; // 90% uptime
150 }
151
152 bool checkGPSHealth() {
153 sensors.gpsSatellites = random(4, 12); // Симуляция спутников
154 return sensors.gpsSatellites >= 4;
155 }
156
157 bool checkBaroHealth() {
158 sensors.baroPressure = 1013.25 + random(-100, 100) / 10.0;
159 return abs(sensors.baroPressure - 1013.25) < 50; // Разумный диапазон
160 }
161
162 String getMainPage() {
163 return R"HTML(
164<!DOCTYPE html>
165<html>
166<head>
167 <title>ESP32 Drone Debugger</title>
168 <meta charset="utf-8">
169 <style>
170 body { font-family: Arial, sans-serif; margin: 20px; background: #f0f0f0; }
171 .container { max-width: 1200px; margin: 0 auto; }
172 .card { background: white; padding: 20px; margin: 10px; border-radius: 10px; box-shadow: 0 2px 5px rgba(0,0,0,0.1); }
173 .metric { display: inline-block; margin: 10px; padding: 15px; background: #e8f4f8; border-radius: 5px; min-width: 150px; }
174 .metric-value { font-size: 24px; font-weight: bold; color: #333; }
175 .metric-label { font-size: 12px; color: #666; }
176 .status-ok { color: #4CAF50; }
177 .status-warning { color: #FF9800; }
178 .status-error { color: #F44336; }
179 #chart { width: 100%; height: 300px; background: white; border: 1px solid #ddd; }
180 </style>
181 <script src="https://cdn.jsdelivr.net/npm/chart.js"></script>
182</head>
183<body>
184 <div class="container">
185 <h1>🚁 ESP32 Drone Debug Console</h1>
186
187 <div class="card">
188 <h2>System Metrics</h2>
189 <div id="systemMetrics"></div>
190 </div>
191
192 <div class="card">
193 <h2>Sensor Status</h2>
194 <div id="sensorStatus"></div>
195 </div>
196
197 <div class="card">
198 <h2>Real-time Data</h2>
199 <canvas id="chart"></canvas>
200 </div>
201
202 <div class="card">
203 <h2>Actions</h2>
204 <button onclick="downloadLogs()">📥 Download Logs</button>
205 <button onclick="rebootSystem()">🔄 Reboot ESP32</button>
206 <button onclick="calibrateSensors()">🎯 Calibrate Sensors</button>
207 </div>
208 </div>
209
210 <script>
211 let chart;
212 let dataPoints = [];
213
214 function initChart() {
215 const ctx = document.getElementById('chart').getContext('2d');
216 chart = new Chart(ctx, {
217 type: 'line',
218 data: {
219 labels: [],
220 datasets: [{
221 label: 'CPU Usage %',
222 data: [],
223 borderColor: 'rgb(75, 192, 192)',
224 tension: 0.1
225 }, {
226 label: 'Memory Usage %',
227 data: [],
228 borderColor: 'rgb(255, 99, 132)',
229 tension: 0.1
230 }]
231 },
232 options: {
233 responsive: true,
234 scales: {
235 y: { beginAtZero: true, max: 100 }
236 }
237 }
238 });
239 }
240
241 function updateMetrics() {
242 fetch('/api/metrics')
243 .then(response => response.json())
244 .then(data => {
245 // Update system metrics
246 document.getElementById('systemMetrics').innerHTML = `
247 <div class="metric">
248 <div class="metric-value">${data.cpu_usage.toFixed(1)}%</div>
249 <div class="metric-label">CPU Usage</div>
250 </div>
251 <div class="metric">
252 <div class="metric-value">${data.memory_usage.toFixed(1)}%</div>
253 <div class="metric-label">Memory Usage</div>
254 </div>
255 <div class="metric">
256 <div class="metric-value">${data.temperature.toFixed(1)}°C</div>
257 <div class="metric-label">Temperature</div>
258 </div>
259 <div class="metric">
260 <div class="metric-value">${data.wifi_signal} dBm</div>
261 <div class="metric-label">WiFi Signal</div>
262 </div>
263 <div class="metric">
264 <div class="metric-value">${data.battery_voltage.toFixed(2)}V</div>
265 <div class="metric-label">Battery</div>
266 </div>
267 <div class="metric">
268 <div class="metric-value">${data.power_consumption.toFixed(2)}A</div>
269 <div class="metric-label">Current</div>
270 </div>
271 `;
272
273 // Update sensor status
274 document.getElementById('sensorStatus').innerHTML = `
275 <div class="metric">
276 <div class="metric-value ${data.imu_status ? 'status-ok' : 'status-error'}">
277 ${data.imu_status ? '✅' : '❌'}
278 </div>
279 <div class="metric-label">IMU (${data.imu_temp.toFixed(1)}°C)</div>
280 </div>
281 <div class="metric">
282 <div class="metric-value ${data.gps_status ? 'status-ok' : 'status-error'}">
283 ${data.gps_status ? '✅' : '❌'}
284 </div>
285 <div class="metric-label">GPS (${data.gps_satellites} sats)</div>
286 </div>
287 <div class="metric">
288 <div class="metric-value ${data.baro_status ? 'status-ok' : 'status-error'}">
289 ${data.baro_status ? '✅' : '❌'}
290 </div>
291 <div class="metric-label">Barometer (${data.baro_pressure.toFixed(1)} hPa)</div>
292 </div>
293 `;
294
295 // Update chart
296 const now = new Date().toLocaleTimeString();
297 chart.data.labels.push(now);
298 chart.data.datasets[0].data.push(data.cpu_usage);
299 chart.data.datasets[1].data.push(data.memory_usage);
300
301 // Keep only last 20 points
302 if (chart.data.labels.length > 20) {
303 chart.data.labels.shift();
304 chart.data.datasets[0].data.shift();
305 chart.data.datasets[1].data.shift();
306 }
307
308 chart.update('none');
309 })
310 .catch(error => console.error('Error fetching metrics:', error));
311 }
312
313 function downloadLogs() {
314 window.open('/api/logs', '_blank');
315 }
316
317 function rebootSystem() {
318 if (confirm('Reboot ESP32? This will disconnect the debug session.')) {
319 fetch('/api/reboot', { method: 'POST' });
320 }
321 }
322
323 function calibrateSensors() {
324 if (confirm('Start sensor calibration? Keep the drone stationary.')) {
325 fetch('/api/calibrate', { method: 'POST' });
326 }
327 }
328
329 // Initialize
330 initChart();
331 updateMetrics();
332 setInterval(updateMetrics, 1000); // Update every second
333 </script>
334</body>
335</html>
336 )HTML";
337 }
338};
339
340// Использование
341DroneDebugger debugger;
342
343void setup() {
344 Serial.begin(115200);
345 WiFi.begin("YourWiFi", "password");
346 while (WiFi.status() != WL_CONNECTED) {
347 delay(1000);
348 Serial.println("Connecting...");
349 }
350 debugger.begin();
351}
352
353void loop() {
354 debugger.update();
355 // Ваш основной код дрона здесь
356}
2.2 Автоматизированное тестирование
1// =====================================
2// СИСТЕМА АВТОМАТИЧЕСКОГО ТЕСТИРОВАНИЯ
3// =====================================
4
5class AutomatedTester {
6private:
7 struct TestResult {
8 String testName;
9 bool passed;
10 String details;
11 unsigned long executionTime;
12 };
13
14 std::vector<TestResult> testResults;
15
16public:
17 void runAllTests() {
18 Serial.println("=== AUTOMATED TESTING STARTED ===");
19
20 testResults.clear();
21
22 // Hardware tests
23 runTest("Power Supply Test", testPowerSupply);
24 runTest("GPIO Functionality", testGPIO);
25 runTest("ADC Accuracy", testADC);
26 runTest("PWM Generation", testPWM);
27
28 // Communication tests
29 runTest("I2C Bus Scan", testI2C);
30 runTest("SPI Communication", testSPI);
31 runTest("UART Loopback", testUART);
32
33 // Sensor tests
34 runTest("IMU Calibration", testIMU);
35 runTest("GPS Signal", testGPS);
36 runTest("Barometer", testBarometer);
37
38 // Performance tests
39 runTest("CPU Performance", testCPUPerformance);
40 runTest("Memory Stress", testMemoryStress);
41 runTest("Real-time Constraints", testRealTime);
42
43 printTestReport();
44 }
45
46private:
47 void runTest(String name, bool (*testFunction)()) {
48 Serial.printf("Running test: %s... ", name.c_str());
49
50 unsigned long startTime = micros();
51 bool result = testFunction();
52 unsigned long executionTime = micros() - startTime;
53
54 TestResult testResult;
55 testResult.testName = name;
56 testResult.passed = result;
57 testResult.executionTime = executionTime;
58
59 testResults.push_back(testResult);
60
61 Serial.println(result ? "PASS" : "FAIL");
62 }
63
64 static bool testPowerSupply() {
65 // Тест стабильности напряжений
66 float vcc3v3 = analogRead(A1) * (3.3 / 4095.0);
67 float vcc5v = analogRead(A2) * (3.3 / 4095.0) * 2.0; // Делитель 1:2
68
69 bool vcc3v3_ok = (vcc3v3 > 3.2 && vcc3v3 < 3.4);
70 bool vcc5v_ok = (vcc5v > 4.8 && vcc5v < 5.2);
71
72 return vcc3v3_ok && vcc5v_ok;
73 }
74
75 static bool testGPIO() {
76 // Тест цифровых входов/выходов
77 const int testPin1 = 2;
78 const int testPin2 = 4;
79
80 pinMode(testPin1, OUTPUT);
81 pinMode(testPin2, INPUT_PULLUP);
82
83 // Соединить testPin1 и testPin2 перемычкой для теста
84 digitalWrite(testPin1, HIGH);
85 delay(10);
86 bool test1 = digitalRead(testPin2) == HIGH;
87
88 digitalWrite(testPin1, LOW);
89 delay(10);
90 bool test2 = digitalRead(testPin2) == LOW;
91
92 return test1 && test2;
93 }
94
95 static bool testADC() {
96 // Тест точности АЦП
97 const int samples = 100;
98 float sum = 0;
99
100 // Подключить известное напряжение к A0
101 for (int i = 0; i < samples; i++) {
102 sum += analogRead(A0);
103 delay(1);
104 }
105
106 float average = sum / samples;
107 float voltage = average * (3.3 / 4095.0);
108
109 // Проверяем что напряжение в разумных пределах
110 return (voltage > 0.1 && voltage < 3.2);
111 }
112
113 static bool testPWM() {
114 // Тест генерации PWM
115 const int pwmPin = 18;
116
117 ledcSetup(0, 1000, 8); // 1kHz, 8-bit
118 ledcAttachPin(pwmPin, 0);
119
120 // Тест различных скважностей
121 int dutyCycles[] = {0, 64, 128, 192, 255};
122
123 for (int duty : dutyCycles) {
124 ledcWrite(0, duty);
125 delay(100);
126
127 // Здесь можно измерить реальную скважность
128 // Для упрощения считаем тест пройденным
129 }
130
131 return true;
132 }
133
134 static bool testI2C() {
135 // Сканирование I2C шины
136 Wire.begin();
137 int devicesFound = 0;
138
139 for (int address = 1; address < 127; address++) {
140 Wire.beginTransmission(address);
141 int error = Wire.endTransmission();
142
143 if (error == 0) {
144 devicesFound++;
145 Serial.printf("I2C device found at 0x%02X\n", address);
146 }
147 }
148
149 return devicesFound > 0; // Хотя бы одно устройство найдено
150 }
151
152 static bool testSPI() {
153 // Тест SPI loopback (MOSI соединен с MISO)
154 SPI.begin();
155
156 byte testData[] = {0xAA, 0x55, 0xFF, 0x00};
157 bool success = true;
158
159 for (byte data : testData) {
160 byte received = SPI.transfer(data);
161 if (received != data) {
162 success = false;
163 break;
164 }
165 }
166
167 return success;
168 }
169
170 static bool testUART() {
171 // Тест UART loopback (TX соединен с RX)
172 Serial1.begin(115200);
173
174 String testString = "UART_TEST_123";
175 Serial1.print(testString);
176 delay(100);
177
178 String received = "";
179 while (Serial1.available()) {
180 received += (char)Serial1.read();
181 }
182
183 return received == testString;
184 }
185
186 static bool testIMU() {
187 // Тест IMU (проверка базовой функциональности)
188 // Здесь должна быть реальная инициализация IMU
189 // Для примера возвращаем true
190 return true;
191 }
192
193 static bool testGPS() {
194 // Тест GPS модуля
195 // Проверяем получение данных
196 return true; // Упрощенная реализация
197 }
198
199 static bool testBarometer() {
200 // Тест барометра
201 return true; // Упрощенная реализация
202 }
203
204 static bool testCPUPerformance() {
205 // Тест производительности CPU
206 unsigned long startTime = micros();
207
208 // Выполняем вычислительно-интенсивную задачу
209 float result = 0;
210 for (int i = 0; i < 10000; i++) {
211 result += sin(i * 0.01) * cos(i * 0.01);
212 }
213
214 unsigned long executionTime = micros() - startTime;
215
216 // Ожидаемое время выполнения на ESP32
217 return executionTime < 100000; // Меньше 100ms
218 }
219
220 static bool testMemoryStress() {
221 // Тест памяти
222 size_t freeHeapBefore = ESP.getFreeHeap();
223
224 // Выделяем и освобождаем память
225 void* ptr = malloc(10000);
226 if (ptr == nullptr) return false;
227
228 free(ptr);
229
230 size_t freeHeapAfter = ESP.getFreeHeap();
231
232 // Проверяем что память освободилась
233 return (freeHeapAfter >= freeHeapBefore - 100); // Допуск 100 байт
234 }
235
236 static bool testRealTime() {
237 // Тест соблюдения временных ограничений
238 unsigned long times[100];
239
240 for (int i = 0; i < 100; i++) {
241 unsigned long start = micros();
242 delay(10); // Задержка 10ms
243 times[i] = micros() - start;
244 }
245
246 // Проверяем что все задержки в пределах ±1ms
247 for (int i = 0; i < 100; i++) {
248 if (times[i] < 9000 || times[i] > 11000) {
249 return false;
250 }
251 }
252
253 return true;
254 }
255
256 void printTestReport() {
257 Serial.println("\n=== TEST REPORT ===");
258
259 int passed = 0;
260 int total = testResults.size();
261
262 for (const auto& result : testResults) {
263 Serial.printf("%-20s: %s (%lu μs)\n",
264 result.testName.c_str(),
265 result.passed ? "PASS" : "FAIL",
266 result.executionTime);
267
268 if (result.passed) passed++;
269 }
270
271 Serial.printf("\nSUMMARY: %d/%d tests passed (%.1f%%)\n",
272 passed, total, (float)passed * 100.0 / total);
273
274 if (passed == total) {
275 Serial.println("🎉 ALL TESTS PASSED! System is ready for flight.");
276 } else {
277 Serial.println("⚠️ SOME TESTS FAILED! Check hardware connections.");
278 }
279 }
280};
🛠️ РАЗДЕЛ B: РАСШИРЕННЫЕ БИБЛИОТЕКИ И ФРЕЙМВОРКИ
3. Специализированные библиотеки для дронов
3.1 Продвинутая работа с сенсорами
1// =====================================
2// УНИВЕРСАЛЬНАЯ БИБЛИОТЕКА ДАТЧИКОВ
3// =====================================
4
5#include <Wire.h>
6#include <SPI.h>
7
8class UniversalSensorLib {
9public:
10 enum SensorType {
11 IMU_MPU6050,
12 IMU_MPU9250,
13 IMU_ICM20948,
14 BARO_BMP280,
15 BARO_MS5611,
16 GPS_UBLOX,
17 RANGEFINDER_VL53L0X,
18 COMPASS_HMC5883L
19 };
20
21 struct SensorData {
22 float gyroX, gyroY, gyroZ; // град/с
23 float accelX, accelY, accelZ; // g
24 float magX, magY, magZ; // Гаусс
25 float pressure; // Па
26 float temperature; // °C
27 float altitude; // м
28 float latitude, longitude; // градусы
29 int satellites;
30 float distance; // м (дальномер)
31 unsigned long timestamp; // мс
32 bool dataValid;
33 };
34
35private:
36 std::vector<SensorType> activeSensors;
37 SensorData lastData;
38
39public:
40 bool initSensor(SensorType type, int address = 0) {
41 switch (type) {
42 case IMU_MPU6050:
43 return initMPU6050(address ? address : 0x68);
44 case IMU_MPU9250:
45 return initMPU9250(address ? address : 0x68);
46 case BARO_BMP280:
47 return initBMP280(address ? address : 0x76);
48 // ... другие датчики
49 }
50 return false;
51 }
52
53 SensorData readAllSensors() {
54 SensorData data = {};
55 data.timestamp = millis();
56 data.dataValid = true;
57
58 for (SensorType sensor : activeSensors) {
59 switch (sensor) {
60 case IMU_MPU6050:
61 readMPU6050(data);
62 break;
63 case BARO_BMP280:
64 readBMP280(data);
65 break;
66 // ... другие датчики
67 }
68 }
69
70 lastData = data;
71 return data;
72 }
73
74 // Автоматическая калибровка
75 bool calibrateSensors(int samples = 1000) {
76 Serial.println("Starting sensor calibration...");
77 Serial.println("Keep the drone stationary!");
78
79 // Калибровка гироскопа (определение offset)
80 float gyroOffsetX = 0, gyroOffsetY = 0, gyroOffsetZ = 0;
81
82 for (int i = 0; i < samples; i++) {
83 SensorData data = readAllSensors();
84 gyroOffsetX += data.gyroX;
85 gyroOffsetY += data.gyroY;
86 gyroOffsetZ += data.gyroZ;
87
88 if (i % 100 == 0) {
89 Serial.printf("Calibration progress: %d%%\n", i * 100 / samples);
90 }
91
92 delay(10);
93 }
94
95 gyroOffsetX /= samples;
96 gyroOffsetY /= samples;
97 gyroOffsetZ /= samples;
98
99 // Сохранение калибровочных данных
100 saveCalibrationData(gyroOffsetX, gyroOffsetY, gyroOffsetZ);
101
102 Serial.println("Calibration complete!");
103 return true;
104 }
105
106 // Диагностика датчиков
107 void runDiagnostics() {
108 Serial.println("=== SENSOR DIAGNOSTICS ===");
109
110 for (SensorType sensor : activeSensors) {
111 switch (sensor) {
112 case IMU_MPU6050:
113 diagnoseMPU6050();
114 break;
115 case BARO_BMP280:
116 diagnoseBMP280();
117 break;
118 // ... другие датчики
119 }
120 }
121 }
122
123private:
124 bool initMPU6050(int address) {
125 Wire.beginTransmission(address);
126 if (Wire.endTransmission() != 0) {
127 Serial.println("MPU6050 not found!");
128 return false;
129 }
130
131 // Инициализация MPU6050
132 writeRegister(address, 0x6B, 0x00); // Wake up
133 writeRegister(address, 0x1A, 0x03); // Low pass filter
134 writeRegister(address, 0x1B, 0x08); // Gyro ±500°/s
135 writeRegister(address, 0x1C, 0x08); // Accel ±4g
136
137 activeSensors.push_back(IMU_MPU6050);
138 Serial.println("MPU6050 initialized");
139 return true;
140 }
141
142 void readMPU6050(SensorData& data) {
143 const int MPU6050_ADDR = 0x68;
144
145 Wire.beginTransmission(MPU6050_ADDR);
146 Wire.write(0x3B); // Starting register
147 Wire.endTransmission(false);
148 Wire.requestFrom(MPU6050_ADDR, 14, true);
149
150 // Чтение сырых данных
151 int16_t rawAccelX = Wire.read() << 8 | Wire.read();
152 int16_t rawAccelY = Wire.read() << 8 | Wire.read();
153 int16_t rawAccelZ = Wire.read() << 8 | Wire.read();
154 int16_t rawTemp = Wire.read() << 8 | Wire.read();
155 int16_t rawGyroX = Wire.read() << 8 | Wire.read();
156 int16_t rawGyroY = Wire.read() << 8 | Wire.read();
157 int16_t rawGyroZ = Wire.read() << 8 | Wire.read();
158
159 // Преобразование в физические единицы
160 data.accelX = rawAccelX / 8192.0; // ±4g range
161 data.accelY = rawAccelY / 8192.0;
162 data.accelZ = rawAccelZ / 8192.0;
163
164 data.gyroX = rawGyroX / 65.5; // ±500°/s range
165 data.gyroY = rawGyroY / 65.5;
166 data.gyroZ = rawGyroZ / 65.5;
167
168 data.temperature = rawTemp / 340.0 + 36.53;
169 }
170
171 bool initBMP280(int address) {
172 Wire.beginTransmission(address);
173 if (Wire.endTransmission() != 0) {
174 Serial.println("BMP280 not found!");
175 return false;
176 }
177
178 // Чтение ID
179 uint8_t chipID = readRegister(address, 0xD0);
180 if (chipID != 0x58) {
181 Serial.printf("Wrong BMP280 ID: 0x%02X\n", chipID);
182 return false;
183 }
184
185 // Настройка BMP280
186 writeRegister(address, 0xF4, 0x27); // Normal mode, temp and pressure oversampling
187 writeRegister(address, 0xF5, 0xA0); // Config: standby time, filter
188
189 activeSensors.push_back(BARO_BMP280);
190 Serial.println("BMP280 initialized");
191 return true;
192 }
193
194 void readBMP280(SensorData& data) {
195 const int BMP280_ADDR = 0x76;
196
197 // Чтение сырых данных давления и температуры
198 Wire.beginTransmission(BMP280_ADDR);
199 Wire.write(0xF7); // Pressure MSB
200 Wire.endTransmission(false);
201 Wire.requestFrom(BMP280_ADDR, 6, true);
202
203 uint32_t rawPressure = (Wire.read() << 12) | (Wire.read() << 4) | (Wire.read() >> 4);
204 uint32_t rawTemp = (Wire.read() << 12) | (Wire.read() << 4) | (Wire.read() >> 4);
205
206 // Компенсация (упрощенная)
207 data.temperature = rawTemp / 16384.0 - 15.0; // Приблизительная формула
208 data.pressure = rawPressure / 256.0; // Приблизительная формула
209
210 // Расчет высоты по барометрической формуле
211 data.altitude = 44330.0 * (1.0 - pow(data.pressure / 101325.0, 0.1903));
212 }
213
214 void diagnoseMPU6050() {
215 Serial.println("MPU6050 Diagnostics:");
216
217 // Проверка температуры
218 SensorData data = readAllSensors();
219 Serial.printf(" Temperature: %.1f°C ", data.temperature);
220 if (data.temperature > -40 && data.temperature < 85) {
221 Serial.println("OK");
222 } else {
223 Serial.println("OUT OF RANGE");
224 }
225
226 // Проверка уровня шума
227 float gyroNoise = 0;
228 for (int i = 0; i < 100; i++) {
229 SensorData sample = readAllSensors();
230 gyroNoise += abs(sample.gyroX) + abs(sample.gyroY) + abs(sample.gyroZ);
231 delay(10);
232 }
233 gyroNoise /= 100;
234
235 Serial.printf(" Gyro noise level: %.2f°/s ", gyroNoise);
236 if (gyroNoise < 1.0) {
237 Serial.println("GOOD");
238 } else if (gyroNoise < 5.0) {
239 Serial.println("ACCEPTABLE");
240 } else {
241 Serial.println("HIGH - CHECK MOUNTING");
242 }
243 }
244
245 void diagnoseBMP280() {
246 Serial.println("BMP280 Diagnostics:");
247
248 SensorData data = readAllSensors();
249
250 Serial.printf(" Pressure: %.1f Pa ", data.pressure);
251 if (data.pressure > 80000 && data.pressure < 120000) {
252 Serial.println("REASONABLE");
253 } else {
254 Serial.println("OUT OF EXPECTED RANGE");
255 }
256
257 Serial.printf(" Calculated altitude: %.1f m\n", data.altitude);
258 }
259
260 // Вспомогательные функции
261 void writeRegister(int address, uint8_t reg, uint8_t value) {
262 Wire.beginTransmission(address);
263 Wire.write(reg);
264 Wire.write(value);
265 Wire.endTransmission();
266 }
267
268 uint8_t readRegister(int address, uint8_t reg) {
269 Wire.beginTransmission(address);
270 Wire.write(reg);
271 Wire.endTransmission(false);
272 Wire.requestFrom(address, 1, true);
273 return Wire.read();
274 }
275
276 void saveCalibrationData(float gx, float gy, float gz) {
277 // Сохранение в EEPROM или SPIFFS
278 // Упрощенная реализация
279 }
280};
281
282// Пример использования
283UniversalSensorLib sensors;
284
285void setup() {
286 Serial.begin(115200);
287 Wire.begin();
288
289 // Инициализация датчиков
290 sensors.initSensor(UniversalSensorLib::IMU_MPU6050);
291 sensors.initSensor(UniversalSensorLib::BARO_BMP280);
292
293 // Калибровка
294 sensors.calibrateSensors();
295
296 // Диагностика
297 sensors.runDiagnostics();
298}
299
300void loop() {
301 UniversalSensorLib::SensorData data = sensors.readAllSensors();
302
303 Serial.printf("Gyro: %.2f, %.2f, %.2f | ", data.gyroX, data.gyroY, data.gyroZ);
304 Serial.printf("Accel: %.2f, %.2f, %.2f | ", data.accelX, data.accelY, data.accelZ);
305 Serial.printf("Alt: %.1fm\n", data.altitude);
306
307 delay(100);
308}
3.2 Фреймворк для создания автопилотов
1// =====================================
2// МОДУЛЬНЫЙ ФРЕЙМВОРК АВТОПИЛОТА
3// =====================================
4
5class AutopilotFramework {
6public:
7 // Базовый класс для модулей
8 class Module {
9 public:
10 virtual void init() = 0;
11 virtual void update(float dt) = 0;
12 virtual bool isHealthy() = 0;
13 virtual String getName() = 0;
14 };
15
16 // Модуль сбора данных датчиков
17 class SensorModule : public Module {
18 private:
19 UniversalSensorLib sensors;
20
21 public:
22 UniversalSensorLib::SensorData currentData;
23
24 void init() override {
25 sensors.initSensor(UniversalSensorLib::IMU_MPU6050);
26 sensors.initSensor(UniversalSensorLib::BARO_BMP280);
27 }
28
29 void update(float dt) override {
30 currentData = sensors.readAllSensors();
31 }
32
33 bool isHealthy() override {
34 return currentData.dataValid &&
35 (millis() - currentData.timestamp) < 100;
36 }
37
38 String getName() override { return "SensorModule"; }
39 };
40
41 // Модуль оценки состояния
42 class StateEstimatorModule : public Module {
43 public:
44 struct State {
45 float roll, pitch, yaw; // углы ориентации
46 float rollRate, pitchRate, yawRate; // угловые скорости
47 float x, y, z; // позиция
48 float vx, vy, vz; // скорости
49 float ax, ay, az; // ускорения
50 } currentState;
51
52 private:
53 SensorModule* sensorModule;
54
55 public:
56 StateEstimatorModule(SensorModule* sensors) : sensorModule(sensors) {}
57
58 void init() override {
59 // Инициализация фильтров
60 }
61
62 void update(float dt) override {
63 if (!sensorModule || !sensorModule->isHealthy()) return;
64
65 auto& data = sensorModule->currentData;
66
67 // Комплементарный фильтр для углов
68 updateAttitudeEstimate(data, dt);
69
70 // Оценка позиции (если есть GPS)
71 updatePositionEstimate(data, dt);
72
73 // Оценка скоростей
74 updateVelocityEstimate(data, dt);
75 }
76
77 bool isHealthy() override {
78 return sensorModule && sensorModule->isHealthy();
79 }
80
81 String getName() override { return "StateEstimator"; }
82
83 private:
84 void updateAttitudeEstimate(const UniversalSensorLib::SensorData& data, float dt) {
85 // Интегрирование гироскопа
86 currentState.roll += data.gyroX * dt;
87 currentState.pitch += data.gyroY * dt;
88 currentState.yaw += data.gyroZ * dt;
89
90 // Коррекция по акселерометру
91 float accelRoll = atan2(data.accelY, data.accelZ) * 180.0 / PI;
92 float accelPitch = atan2(-data.accelX, sqrt(data.accelY*data.accelY + data.accelZ*data.accelZ)) * 180.0 / PI;
93
94 float alpha = 0.98;
95 currentState.roll = alpha * currentState.roll + (1.0 - alpha) * accelRoll;
96 currentState.pitch = alpha * currentState.pitch + (1.0 - alpha) * accelPitch;
97
98 // Угловые скорости
99 currentState.rollRate = data.gyroX;
100 currentState.pitchRate = data.gyroY;
101 currentState.yawRate = data.gyroZ;
102 }
103
104 void updatePositionEstimate(const UniversalSensorLib::SensorData& data, float dt) {
105 // Высота по барометру
106 currentState.z = data.altitude;
107
108 // Вертикальная скорость (производная высоты)
109 static float lastAltitude = 0;
110 currentState.vz = (data.altitude - lastAltitude) / dt;
111 lastAltitude = data.altitude;
112 }
113
114 void updateVelocityEstimate(const UniversalSensorLib::SensorData& data, float dt) {
115 // Интегрирование ускорений для получения скоростей
116 // (упрощенная реализация)
117 currentState.vx += data.accelX * 9.81 * dt;
118 currentState.vy += data.accelY * 9.81 * dt;
119 }
120 };
121
122 // Модуль управления
123 class ControlModule : public Module {
124 public:
125 struct ControlInputs {
126 float rollCommand, pitchCommand, yawCommand, throttleCommand;
127 bool isArmed;
128 int flightMode; // 0=Manual, 1=Stabilize, 2=AltHold, etc.
129 } inputs;
130
131 struct ControlOutputs {
132 float motor1, motor2, motor3, motor4; // 0.0-1.0
133 } outputs;
134
135 private:
136 StateEstimatorModule* stateEstimator;
137
138 // PID контроллеры
139 struct PIDController {
140 float kp, ki, kd;
141 float integral, lastError;
142 float outputMin, outputMax;
143
144 float update(float setpoint, float measurement, float dt) {
145 float error = setpoint - measurement;
146
147 integral += error * dt;
148 integral = constrain(integral, -10, 10); // Anti-windup
149
150 float derivative = (error - lastError) / dt;
151 lastError = error;
152
153 float output = kp * error + ki * integral + kd * derivative;
154 return constrain(output, outputMin, outputMax);
155 }
156 };
157
158 PIDController rollPID = {1.0, 0.1, 0.05, 0, 0, -1, 1};
159 PIDController pitchPID = {1.0, 0.1, 0.05, 0, 0, -1, 1};
160 PIDController yawPID = {1.0, 0.1, 0.05, 0, 0, -1, 1};
161 PIDController altPID = {0.5, 0.1, 0.2, 0, 0, -0.5, 0.5};
162
163 public:
164 ControlModule(StateEstimatorModule* estimator) : stateEstimator(estimator) {}
165
166 void init() override {
167 outputs = {0, 0, 0, 0};
168 }
169
170 void update(float dt) override {
171 if (!stateEstimator || !stateEstimator->isHealthy()) {
172 // Аварийное отключение
173 outputs = {0, 0, 0, 0};
174 return;
175 }
176
177 if (!inputs.isArmed) {
178 outputs = {0, 0, 0, 0};
179 return;
180 }
181
182 switch (inputs.flightMode) {
183 case 0: // Manual
184 manualControl();
185 break;
186 case 1: // Stabilize
187 stabilizeControl(dt);
188 break;
189 case 2: // Altitude Hold
190 altitudeHoldControl(dt);
191 break;
192 }
193 }
194
195 bool isHealthy() override {
196 return stateEstimator && stateEstimator->isHealthy();
197 }
198
199 String getName() override { return "ControlModule"; }
200
201 private:
202 void manualControl() {
203 // Прямое управление
204 float base = inputs.throttleCommand;
205
206 outputs.motor1 = base + inputs.pitchCommand - inputs.rollCommand - inputs.yawCommand;
207 outputs.motor2 = base - inputs.pitchCommand - inputs.rollCommand + inputs.yawCommand;
208 outputs.motor3 = base - inputs.pitchCommand + inputs.rollCommand - inputs.yawCommand;
209 outputs.motor4 = base + inputs.pitchCommand + inputs.rollCommand + inputs.yawCommand;
210
211 constrainOutputs();
212 }
213
214 void stabilizeControl(float dt) {
215 auto& state = stateEstimator->currentState;
216
217 // Желаемые углы на основе команд
218 float desiredRoll = inputs.rollCommand * 30.0; // ±30°
219 float desiredPitch = inputs.pitchCommand * 30.0;
220 float desiredYawRate = inputs.yawCommand * 180.0; // ±180°/s
221
222 // PID управление
223 float rollOutput = rollPID.update(desiredRoll, state.roll, dt);
224 float pitchOutput = pitchPID.update(desiredPitch, state.pitch, dt);
225 float yawOutput = yawPID.update(desiredYawRate, state.yawRate, dt);
226
227 // Микширование на моторы
228 float throttle = inputs.throttleCommand;
229
230 outputs.motor1 = throttle + pitchOutput - rollOutput - yawOutput;
231 outputs.motor2 = throttle - pitchOutput - rollOutput + yawOutput;
232 outputs.motor3 = throttle - pitchOutput + rollOutput - yawOutput;
233 outputs.motor4 = throttle + pitchOutput + rollOutput + yawOutput;
234
235 constrainOutputs();
236 }
237
238 void altitudeHoldControl(float dt) {
239 // Сначала стабилизация углов
240 stabilizeControl(dt);
241
242 // Затем управление высотой
243 static float targetAltitude = 0;
244 static bool altitudeSet = false;
245
246 if (!altitudeSet) {
247 targetAltitude = stateEstimator->currentState.z;
248 altitudeSet = true;
249 }
250
251 // Изменение целевой высоты джойстиком
252 if (abs(inputs.throttleCommand - 0.5) > 0.1) {
253 targetAltitude += (inputs.throttleCommand - 0.5) * 2.0 * dt;
254 altitudeSet = false;
255 }
256
257 float altOutput = altPID.update(targetAltitude, stateEstimator->currentState.z, dt);
258
259 // Добавляем коррекцию высоты ко всем моторам
260 outputs.motor1 += altOutput;
261 outputs.motor2 += altOutput;
262 outputs.motor3 += altOutput;
263 outputs.motor4 += altOutput;
264
265 constrainOutputs();
266 }
267
268 void constrainOutputs() {
269 outputs.motor1 = constrain(outputs.motor1, 0.0, 1.0);
270 outputs.motor2 = constrain(outputs.motor2, 0.0, 1.0);
271 outputs.motor3 = constrain(outputs.motor3, 0.0, 1.0);
272 outputs.motor4 = constrain(outputs.motor4, 0.0, 1.0);
273 }
274 };
275
276 // Модуль вывода на моторы
277 class MotorModule : public Module {
278 private:
279 ControlModule* controlModule;
280
281 public:
282 MotorModule(ControlModule* control) : controlModule(control) {}
283
284 void init() override {
285 // Инициализация PWM каналов для 4 моторов
286 ledcSetup(0, 50, 16); // Motor 1
287 ledcSetup(1, 50, 16); // Motor 2
288 ledcSetup(2, 50, 16); // Motor 3
289 ledcSetup(3, 50, 16); // Motor 4
290
291 ledcAttachPin(18, 0); // Motor 1 pin
292 ledcAttachPin(19, 1); // Motor 2 pin
293 ledcAttachPin(21, 2); // Motor 3 pin
294 ledcAttachPin(22, 3); // Motor 4 pin
295 }
296
297 void update(float dt) override {
298 if (!controlModule || !controlModule->isHealthy()) {
299 // Аварийная остановка
300 ledcWrite(0, 3277); // 1ms pulse (stop)
301 ledcWrite(1, 3277);
302 ledcWrite(2, 3277);
303 ledcWrite(3, 3277);
304 return;
305 }
306
307 auto& outputs = controlModule->outputs;
308
309 // Преобразование 0.0-1.0 в PWM значения 1000-2000us
310 // При 50Hz и 16-bit: 1ms = 3277, 2ms = 6554
311 uint16_t pwm1 = map(outputs.motor1 * 1000, 0, 1000, 3277, 6554);
312 uint16_t pwm2 = map(outputs.motor2 * 1000, 0, 1000, 3277, 6554);
313 uint16_t pwm3 = map(outputs.motor3 * 1000, 0, 1000, 3277, 6554);
314 uint16_t pwm4 = map(outputs.motor4 * 1000, 0, 1000, 3277, 6554);
315
316 ledcWrite(0, pwm1);
317 ledcWrite(1, pwm2);
318 ledcWrite(2, pwm3);
319 ledcWrite(3, pwm4);
320 }
321
322 bool isHealthy() override {
323 return controlModule && controlModule->isHealthy();
324 }
325
326 String getName() override { return "MotorModule"; }
327 };
328
329private:
330 std::vector<Module*> modules;
331 unsigned long lastUpdate;
332
333public:
334 void addModule(Module* module) {
335 modules.push_back(module);
336 }
337
338 void init() {
339 Serial.println("Initializing Autopilot Framework...");
340
341 for (Module* module : modules) {
342 Serial.printf("Initializing %s...", module->getName().c_str());
343 module->init();
344 Serial.println(" OK");
345 }
346
347 lastUpdate = millis();
348 Serial.println("Autopilot Framework ready!");
349 }
350
351 void update() {
352 unsigned long currentTime = millis();
353 float dt = (currentTime - lastUpdate) / 1000.0;
354 lastUpdate = currentTime;
355
356 // Обновление всех модулей
357 for (Module* module : modules) {
358 module->update(dt);
359 }
360
361 // Проверка здоровья системы
362 checkSystemHealth();
363 }
364
365private:
366 void checkSystemHealth() {
367 static unsigned long lastHealthCheck = 0;
368
369 if (millis() - lastHealthCheck > 1000) { // Каждую секунду
370 bool systemHealthy = true;
371
372 for (Module* module : modules) {
373 if (!module->isHealthy()) {
374 Serial.printf("WARNING: %s is not healthy!\n", module->getName().c_str());
375 systemHealthy = false;
376 }
377 }
378
379 if (!systemHealthy) {
380 Serial.println("SYSTEM HEALTH CHECK FAILED!");
381 // Здесь можно добавить аварийные процедуры
382 }
383
384 lastHealthCheck = millis();
385 }
386 }
387};
388
389// Пример использования фреймворка
390AutopilotFramework autopilot;
391AutopilotFramework::SensorModule sensorModule;
392AutopilotFramework::StateEstimatorModule stateEstimator(&sensorModule);
393AutopilotFramework::ControlModule controlModule(&stateEstimator);
394AutopilotFramework::MotorModule motorModule(&controlModule);
395
396void setup() {
397 Serial.begin(115200);
398 Wire.begin();
399
400 // Добавляем модули в фреймворк
401 autopilot.addModule(&sensorModule);
402 autopilot.addModule(&stateEstimator);
403 autopilot.addModule(&controlModule);
404 autopilot.addModule(&motorModule);
405
406 // Инициализация
407 autopilot.init();
408
409 // Настройка управления
410 controlModule.inputs.flightMode = 1; // Stabilize mode
411 controlModule.inputs.isArmed = false;
412}
413
414void loop() {
415 autopilot.update();
416
417 // Обработка команд с радио
418 // controlModule.inputs.rollCommand = ...
419 // controlModule.